From 8926bbf8ea93a60639886726d446c7d45cd33cfb Mon Sep 17 00:00:00 2001
From: Louis-Joseph Fournier <louis-joseph.fournier@parrot.com>
Date: Wed, 13 Jan 2016 11:41:06 +0100
Subject: [PATCH] Player: fix low notes and smooth note change

---
 src/TunerWorker.cpp      |  1 +
 src/audio/FreqPlayer.cpp | 29 ++++++++++++++++++++++++++++-
 src/audio/FreqPlayer.hpp |  4 ++++
 src/scale/Scale.cpp      |  6 +++---
 src/scale/Scale.hpp      |  4 ++--
 5 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/src/TunerWorker.cpp b/src/TunerWorker.cpp
index 2d0ecf7..54eea46 100644
--- a/src/TunerWorker.cpp
+++ b/src/TunerWorker.cpp
@@ -273,6 +273,7 @@ void TunerWorker::Entry()
 						);
 
 				// update frequency to update from previous tuner results
+				player->Reset();
 				player->SetFreq(pitchDetection->GetNoteFreq(result.note, result.octave));
 			}
 
diff --git a/src/audio/FreqPlayer.cpp b/src/audio/FreqPlayer.cpp
index 57f6520..1c4aa70 100644
--- a/src/audio/FreqPlayer.cpp
+++ b/src/audio/FreqPlayer.cpp
@@ -17,6 +17,7 @@
 
 #include <math.h>
 #include <stdint.h>
+//#include <iostream>
 
 #include "FreqPlayer.hpp"
 
@@ -27,16 +28,28 @@ template<typename sample_t> FreqPlayer<sample_t>::FreqPlayer(int _rate):
 	n_frame(0),
 	waveform(W_SINUS)
 {
+	k = K();
+	k_update = -1; // invalid: don't update
 }
 
 template<typename sample_t> void FreqPlayer<sample_t>::Reset()
 {
 	n_frame = 0;
+	if (k_update != -1) {
+		k = k_update;
+		k_update = -1;
+	}
+}
+
+template<typename sample_t> double FreqPlayer<sample_t>::K() const
+{
+	return freq / rate * M_PI * 2;
 }
 
 template<typename sample_t> void FreqPlayer<sample_t>::SetFreq(double freq)
 {
 	this->freq = freq;
+	k_update = K();
 }
 
 template<typename sample_t> void FreqPlayer<sample_t>::SetVolume(double volume)
@@ -49,7 +62,21 @@ template<> double FreqPlayer<double>::max() { return 1; }
 
 template<typename sample_t> double FreqPlayer<sample_t>::radius()
 {
-	return (double) (n_frame++) * freq / rate * M_PI * 2;
+	double ret = (n_frame++) * k;
+
+	/* to update frequency factor, wait current radius to go to beginning
+	 * in interval [0, 2PI]
+	 */
+	if (k_update != -1) {
+		double a = fmod(ret, M_PI * 1);
+		double b = fmod(n_frame * k, M_PI * 2);
+		if (b < a) { // next frame go back to beginning of circle
+			n_frame = 0;
+			k = k_update;
+			k_update = -1;
+		}
+	}
+	return ret;
 }
 
 template<typename sample_t> sample_t FreqPlayer<sample_t>::AudioFrame()
diff --git a/src/audio/FreqPlayer.hpp b/src/audio/FreqPlayer.hpp
index 61adf69..6062287 100644
--- a/src/audio/FreqPlayer.hpp
+++ b/src/audio/FreqPlayer.hpp
@@ -40,7 +40,11 @@ template<typename sample_t> class FreqPlayer {
 	int n_frame;
 	/// wave form
 	WAVEFORM waveform;
+	/// pre computed factor
+	double k, k_update;
 
+	/// return k computed
+	double K() const;
 	/// return the max sample_t
 	sample_t max();
 	/// return the current radius
diff --git a/src/scale/Scale.cpp b/src/scale/Scale.cpp
index 5959d73..662ca79 100644
--- a/src/scale/Scale.cpp
+++ b/src/scale/Scale.cpp
@@ -120,7 +120,7 @@ void Scale::ConstructEqualTemperament()
 	updateScale();
 }
 
-double Scale::GetLa()
+double Scale::GetLa() const
 {
 	return actualLa;
 }
@@ -140,12 +140,12 @@ void Scale::SetLa(double la)
 	if (freq_setted) updateScale();
 }
 
-double Scale::GetNoteFreq(int note, int octave)
+double Scale::GetNoteFreq(int note, int octave) const
 {
 	assert(note >= 0 && note < nbNote);
 	double f = actualNoteFreq[note];
 	octave -= 4;
-	if (octave < 0) f /= 1 << octave;
+	if (octave < 0) f /= 1 << (-octave);
 	else if (octave > 0) f *= 1 << octave;
 
 	return f;
diff --git a/src/scale/Scale.hpp b/src/scale/Scale.hpp
index b704128..3f0c7ee 100644
--- a/src/scale/Scale.hpp
+++ b/src/scale/Scale.hpp
@@ -57,13 +57,13 @@ class Scale {
 	void SetNotesFrequencies(const double freq[nbNote]);
 
 	/// Get ref freq for la4
-	double GetLa();
+	double GetLa() const;
 
 	/// Set ref freq for la4
 	void SetLa(double la);
 
 	/// get note frequency
-	double GetNoteFreq(int note, int octave);
+	double GetNoteFreq(int note, int octave) const;
 
 	/**
 	 * Find nearest note, octave, and deviation