# HG changeset patch
# User Chris Cannam
# Date 1394207480 0
# Node ID ab0b04e1c56bea130b914652b3724e82dace6e35
# Parent  f559ab000b67147c0f157273ae80b2b4baebbe7e
Implement the plugin!

diff -r f559ab000b67 -r ab0b04e1c56b ConstrainedHarmonicPeak.cpp
--- a/ConstrainedHarmonicPeak.cpp	Fri Mar 07 14:34:50 2014 +0000
+++ b/ConstrainedHarmonicPeak.cpp	Fri Mar 07 15:51:20 2014 +0000
@@ -12,7 +12,8 @@
     Plugin(inputSampleRate),
     m_blockSize(0),
     m_minFreq(0),
-    m_maxFreq(inputSampleRate/2)
+    m_maxFreq(inputSampleRate/2),
+    m_harmonics(5)
 {
 }
 
@@ -113,6 +114,17 @@
     d.isQuantized = false;
     list.push_back(d);
 
+    d.identifier = "harmonics";
+    d.name = "Harmonics";
+    d.description = "Maximum number of harmonics to consider";
+    d.unit = "";
+    d.minValue = 1;
+    d.maxValue = 20;
+    d.defaultValue = 5;
+    d.isQuantized = true;
+    d.quantizeStep = 1;
+    list.push_back(d);
+
     return list;
 }
 
@@ -123,6 +135,8 @@
 	return m_minFreq;
     } else if (identifier == "maxfreq") {
 	return m_maxFreq;
+    } else if (identifier == "harmonics") {
+	return m_harmonics;
     }
     return 0;
 }
@@ -134,6 +148,8 @@
 	m_minFreq = value;
     } else if (identifier == "maxfreq") {
 	m_maxFreq = value;
+    } else if (identifier == "harmonics") {
+	m_harmonics = int(round(value));
     }
 }
 
@@ -193,11 +209,98 @@
 {
 }
 
+double
+ConstrainedHarmonicPeak::findInterpolatedPeak(const double *in, 
+					      int peakbin,
+					      int bins)
+{
+    // duplicate with SimpleCepstrum plugin
+    // after jos, 
+    // https://ccrma.stanford.edu/~jos/sasp/Quadratic_Interpolation_Spectral_Peaks.html
+
+    if (peakbin < 1 || peakbin > bins - 2) {
+        return peakbin;
+    }
+
+    double alpha = in[peakbin-1];
+    double beta  = in[peakbin];
+    double gamma = in[peakbin+1];
+
+    double denom = (alpha - 2*beta + gamma);
+
+    if (denom == 0) {
+        // flat
+        return peakbin;
+    }
+
+    double p = ((alpha - gamma) / denom) / 2.0;
+
+    return double(peakbin) + p;
+}
+
 ConstrainedHarmonicPeak::FeatureSet
 ConstrainedHarmonicPeak::process(const float *const *inputBuffers, Vamp::RealTime timestamp)
 {
     FeatureSet fs;
 
+    int hs = m_blockSize/2;
+
+    double *mags = new double[hs+1];
+    for (int i = 0; i <= hs; ++i) {
+	mags[i] = sqrtf(inputBuffers[0][i*2] * inputBuffers[0][i*2] +
+			inputBuffers[0][i*2+1] * inputBuffers[0][i*2+1]);
+    }
+
+    // bin freq is bin * samplerate / fftsize
+
+    int minbin = int(floor((m_minFreq * m_blockSize) / m_inputSampleRate));
+    int maxbin = int(ceil((m_maxFreq * m_blockSize) / m_inputSampleRate));
+    if (minbin > hs) minbin = hs;
+    if (maxbin > hs) maxbin = hs;
+    if (maxbin <= minbin) return fs;
+
+    double *hps = new double[maxbin - minbin + 1];
+
+    // HPS in dB after MzHarmonicSpectrum
+
+    for (int bin = minbin; bin <= maxbin; ++bin) {
+
+	int i = bin - minbin;
+	hps[i] = 1.0;
+
+	int contributing = 0;
+
+	for (int j = 1; j <= m_harmonics; ++j) {
+	    if (j * bin > hs) break;
+	    hps[i] *= mags[j * bin];
+	    ++contributing;
+	}
+
+	if (hps[i] <= 0.0) {
+	    hps[i] = -120.0;
+	} else {
+	    hps[i] = 20.0 / contributing * log10(hps[i]);
+	}
+    }
+
+    double maxdb = -120.0;
+    int maxidx = 0;
+    for (int i = 0; i <= maxbin - minbin; ++i) {
+	if (hps[i] > maxdb) {
+	    maxdb = hps[i];
+	    maxidx = i;
+	}
+    }
+
+    double interpolated = findInterpolatedPeak(hps, maxidx, maxbin - minbin + 1);
+    interpolated = interpolated + minbin;
+
+    double freq = interpolated * m_inputSampleRate / m_blockSize;
+
+    Feature f;
+    f.values.push_back(freq);
+    fs[0].push_back(f);
+
     return fs;
 }
 
@@ -205,7 +308,6 @@
 ConstrainedHarmonicPeak::getRemainingFeatures()
 {
     FeatureSet fs;
-
     return fs;
 }
 
diff -r f559ab000b67 -r ab0b04e1c56b ConstrainedHarmonicPeak.h
--- a/ConstrainedHarmonicPeak.h	Fri Mar 07 14:34:50 2014 +0000
+++ b/ConstrainedHarmonicPeak.h	Fri Mar 07 15:51:20 2014 +0000
@@ -46,6 +46,9 @@
     int m_blockSize;
     float m_minFreq;
     float m_maxFreq;
+    int m_harmonics;
+
+    static double findInterpolatedPeak(const double *in, int peakbin, int bins);
 };
 
 
diff -r f559ab000b67 -r ab0b04e1c56b Makefile.inc
--- a/Makefile.inc	Fri Mar 07 14:34:50 2014 +0000
+++ b/Makefile.inc	Fri Mar 07 15:51:20 2014 +0000
@@ -7,7 +7,7 @@
 CFLAGS := $(CFLAGS) 
 CXXFLAGS := -I. $(CXXFLAGS)
 
-PLUGIN := chp(PLUGIN_EXT)
+PLUGIN := chp$(PLUGIN_EXT)
 
 SOURCES := ConstrainedHarmonicPeak.cpp