package chromaticTuner.tuner;

public class PitchDetector {
	private OnFrequencyUpdateListener m_frequencyUpdateListener = null;

	private int m_sampleRate = 0;
	private int m_bufferSize = 0;

	private OnAudioLevelListener m_audioLevelListener = null;
	private double m_rms = Double.NEGATIVE_INFINITY;

	private double m_rmsThreshold = Double.NEGATIVE_INFINITY;
	
	private int m_note=-1;
	private double m_currentNoteInitialAutocorrelation=0.0;
	private float m_currentNoteWaveLengths[] = null;
	private int m_currentNoteWaveLengthNum=0;
	private float m_noteWaveLengths[] = null;
	private double m_noteDetectionAutocorrelationValues[] = null;
	private int m_noteWaveLengthNum=0;
	
	private float m_aFrequency=440.0f;
	
	private int m_frames = 500;
	private int m_frameJump = 20;
	
	private float m_requiredAutoCorrelationMultiplier = 4f;
	private float m_lowPassFreq = 400.0f;
	private int m_lowPassAboveNote = 2;

	private float m_noteDetectionMultiplier = 2.0f;
	private float m_noteDetectionWaveMultiplier = 6.0f;
	private float m_noteDetectionWaveFirstDivisor = m_noteDetectionWaveMultiplier-m_noteDetectionMultiplier;

	private RmsMeasurer m_rmsMeasurer = null;
	private LowPassFilter m_lowPassFilter = null;
	private Normalizer m_normalizer = null;
	private AudioInterpolator m_interpolator = null;

	static final float NOTE_FREQS[] = { 32.703201293945312f, 34.647899627685547f, 36.708099365234375f, 38.890899658203125f,
		41.203498840332031f,43.653598785400391f,46.249298095703125f,48.999500274658203f,51.912998199462891f,
		55.000000000000000f,58.270500183105469f,61.735401153564453f,65.406402587890625f,69.295799255371094f,
		73.416198730468750f,77.781799316406250f,82.406997680664062f,87.307197570800781f,92.498596191406250f,
		97.999000549316406f,103.825996398925781f,110.000000000000000f,116.541000366210938f,123.470802307128906f,
		130.812805175781250f,138.591598510742188f,146.832397460937500f,155.563598632812500f,164.813995361328125f,
		174.614395141601562f,184.997192382812500f,195.998001098632812f,207.651992797851562f,220.000000000000000f,
		233.082000732421875f,246.941604614257812f,261.625610351562500f,277.183197021484375f,293.664794921875000f,
		311.127197265625000f,329.627990722656250f,349.228790283203125f,369.994384765625000f,391.996002197265625f,
		415.303985595703125f,440.000000000000000f,466.164001464843750f,493.883209228515625f,523.251220703125000f};
	
	/* after initially detecting note, use the following values for finding the actual pitch.
	 * completely different notes are used to spot detection errors while detecting the initial note
	 */
	static final float ONE_NOTE_HALF_STEPS[] = { -6.0f, -5.0f, -4.0f, -3.0f,
		-2.0f,-1.0f,-0.8f,-0.6f,-0.5f,
		-0.4f,-0.35f,-0.3f,-0.25f,-0.2f,
		-0.18f,-0.16f,-0.14f,-0.12f,-0.10f,
		-0.08f,-0.06f,-0.04f,-0.02f,-0.01f,
		0.0f,0.01f,0.02f,0.04f,0.06f,
		0.08f,0.10f,0.12f,0.14f,0.16f,
		0.18f,0.2f,0.25f,0.3f,0.35f,
		0.4f,0.5f,0.6f,0.8f,1.0f,
		2.0f,3.0f,4.0f,5.0f};

	public PitchDetector(int sampleRate) {
		m_sampleRate = sampleRate;
		initAutocorrelation();
		m_noteWaveLengthNum=PitchDetector.NOTE_FREQS.length;
		m_currentNoteWaveLengths = new float[m_noteWaveLengthNum];
		m_noteWaveLengths = new float[m_noteWaveLengthNum];
		m_noteDetectionAutocorrelationValues = new double[m_noteWaveLengthNum];

		for (int i=0;i<PitchDetector.NOTE_FREQS.length;i++)
		{
			m_noteWaveLengths[i]=m_sampleRate/PitchDetector.NOTE_FREQS[i];
			m_noteDetectionAutocorrelationValues[i]=Double.MAX_VALUE;
		}

		m_frames = (int) Math.floor(m_noteWaveLengths[0]+1);
		m_frameJump = m_sampleRate/3000;
		if (m_frameJump<=0)
		{
			m_frameJump=1;
		}

		m_rmsMeasurer = new RmsMeasurer();
		m_lowPassFilter = new LowPassFilter(m_lowPassFreq);
		m_lowPassFilter.setSampleRate(m_sampleRate);
		m_normalizer = new Normalizer();
		m_interpolator = new AudioInterpolator();
	}
	
	public void setBufferSize(int bufferSize)
	{
		m_bufferSize = bufferSize;
		initAutocorrelation();
	}

	public void processBuffer(short[] buffer) {
		processBufferWithAutoCorrelation(buffer);
	}

	public void setOnFrequencyUpdateListener(
			OnFrequencyUpdateListener frequencyUpdateListener) {
		m_frequencyUpdateListener = frequencyUpdateListener;
	}

	public void setOnVisualizationValueListener(
			OnAudioLevelListener visualizationValueListener) {
		m_audioLevelListener = visualizationValueListener;
	}

	public void setRmsThreshold(double rmsThreshold) {
		m_rmsThreshold = rmsThreshold;
	}
	
	public float getAFrequency()
	{
		return m_aFrequency;
	}
	
	public void setAFrequency(float aFreq)
	{
		m_aFrequency=aFreq;
	}

	private float m_usedWaveLengths[];
	private int m_waveLengthNum=0;
	private double m_autocorrelation=0.0;
	private double m_bestAutocorrelation=0.0;
	private float m_bestAutocorrelationFreq=0.0f;
	private float m_currentWaveLength=0.0f;
	private int m_bestAutocorrelationIndex=-1;
	private int m_wavelengthIndex=0;
	private int m_frameIndex=0;
	private void processBufferWithAutoCorrelation(short[] buffer) {
		if (buffer.length == 0 || buffer.length != m_bufferSize) {
			return;
		}

		m_rms = m_rmsMeasurer.measureRms(buffer);
		if (m_audioLevelListener != null) {
			m_audioLevelListener.onAudioLevel((float) m_rms);
		}
		if (m_rms < m_rmsThreshold) {
			if (m_frequencyUpdateListener != null) {
				m_frequencyUpdateListener.onFrequencyUpdate(0.0f);
			}
			initAutocorrelation();
			return;
		}
		
		try {
			if (m_note>=0)
			{
				m_lowPassFilter.setFilterFrequency(NoteCalculator.calculateFrequencyFromNote(m_note+m_lowPassAboveNote, m_aFrequency));
			}
			else
			{
				m_lowPassFilter.setFilterFrequency(m_lowPassFreq);
			}
			m_lowPassFilter.filterBuffer(buffer, buffer);
		} catch (Exception e) {
			e.printStackTrace();
		}
		
		m_normalizer.normalizeBuffer(buffer);
		
		if (m_note>=0)
		{
			m_usedWaveLengths=m_currentNoteWaveLengths;
			populateWaveLengthsWithOneNote(m_usedWaveLengths, m_note);
			m_waveLengthNum = m_currentNoteWaveLengthNum;
		}
		else
		{
			m_usedWaveLengths=m_noteWaveLengths;
			m_waveLengthNum=m_noteWaveLengthNum;
		}
		
		doAutocorrelation(buffer);
		
		if (m_note<0 && m_bestAutocorrelationIndex>=0)
		{
			m_note = Math.round(NoteCalculator.calculateNoteFromFrequency(m_bestAutocorrelationFreq, m_aFrequency));
			m_currentNoteInitialAutocorrelation = m_bestAutocorrelation;
		}
		else if (m_bestAutocorrelationIndex>=0) // check if still the same note
		{
			if (m_note!=Math.round(NoteCalculator.calculateNoteFromFrequency(m_bestAutocorrelationFreq, m_aFrequency)))
			{
				initAutocorrelation();
			}
		}

		//System.out.println("Best frequency: " + m_bestAutocorrelationFreq);
		
		if (m_note>= 0 && m_bestAutocorrelation > m_currentNoteInitialAutocorrelation*m_requiredAutoCorrelationMultiplier)
		{
			//System.out.println("Autocorrelation worse than initially, initializing");
			initAutocorrelation();
			m_bestAutocorrelationFreq=0.0f;
		}

		if (m_bestAutocorrelationFreq<0.0)
		{
			m_bestAutocorrelationFreq=0.0f;
		}

		if (m_frequencyUpdateListener != null)
		{
			m_frequencyUpdateListener.onFrequencyUpdate(m_bestAutocorrelationFreq);
		}
	}

	private int m_wavelengthPopulateIndex=0;
	private void populateWaveLengthsWithOneNote(float[] waveLengths, int note) {
		m_wavelengthPopulateIndex=0;
		
		for (m_wavelengthPopulateIndex=0;m_wavelengthPopulateIndex<ONE_NOTE_HALF_STEPS.length;m_wavelengthPopulateIndex++)
		{
			waveLengths[m_wavelengthPopulateIndex]=m_sampleRate/
					NoteCalculator.calculateFrequencyFromNote(note+ONE_NOTE_HALF_STEPS[m_wavelengthPopulateIndex],
							m_aFrequency);
		}
		m_currentNoteWaveLengthNum=m_wavelengthPopulateIndex;
	}

	private void initAutocorrelation() {
		m_note=-1;
		m_currentNoteInitialAutocorrelation=0.0;
	}

	private int m_initialNoteDetectionMultiply = -1;
	private int m_initialNoteDetectionIndex = -1;
	private void doAutocorrelation(short[] buffer)
	{
		// least square autocorrelation
		m_bestAutocorrelation=Double.POSITIVE_INFINITY;
		m_bestAutocorrelationFreq=-1.0f;

		m_bestAutocorrelationIndex=-1;

		for (m_wavelengthIndex = 0; m_wavelengthIndex < m_waveLengthNum; m_wavelengthIndex++) {
			m_autocorrelation = 0.0;
			m_currentWaveLength = m_usedWaveLengths[m_wavelengthIndex];
			for (m_frameIndex = 0; m_frameIndex < m_frames; m_frameIndex+=m_frameJump) {
				m_autocorrelation += Math.pow(buffer[m_frameIndex] - m_interpolator.cubicInterpolatedBufferValue(buffer,m_frameIndex+m_currentWaveLength),2);
			}

			if (m_note<0)
			{
				m_noteDetectionAutocorrelationValues[m_wavelengthIndex] = m_autocorrelation;
			}
			if (m_autocorrelation < m_bestAutocorrelation || m_bestAutocorrelation<0)
			{
				m_bestAutocorrelation = m_autocorrelation;
				m_bestAutocorrelationFreq = (float) (m_sampleRate / m_currentWaveLength);
				m_bestAutocorrelationIndex=m_wavelengthIndex;
			}
		}

		if (m_note<0)
		{
			// trying to correct possible wavelength multiply error
			m_initialNoteDetectionMultiply = 2;
			m_initialNoteDetectionIndex = -1;
			while (m_bestAutocorrelationFreq*m_initialNoteDetectionMultiply < NOTE_FREQS[NOTE_FREQS.length-1])
			{
				int currentIndex = getClosestNoteFreqIndex(m_bestAutocorrelationFreq*m_initialNoteDetectionMultiply,
						m_bestAutocorrelationIndex);
				if (currentIndex>0 && m_noteDetectionAutocorrelationValues[currentIndex] <
						m_bestAutocorrelation*m_noteDetectionMultiplier*
						(m_noteDetectionWaveMultiplier/(m_noteDetectionWaveFirstDivisor+m_initialNoteDetectionMultiply)))
				{
					m_initialNoteDetectionIndex = currentIndex;
				}
				m_initialNoteDetectionMultiply++;
			}

			if (m_initialNoteDetectionIndex>=0)
			{
				m_bestAutocorrelation = m_noteDetectionAutocorrelationValues[m_initialNoteDetectionIndex];
				m_bestAutocorrelationFreq = NOTE_FREQS[m_initialNoteDetectionIndex];
				m_bestAutocorrelationIndex= m_initialNoteDetectionIndex;
			}
		}
	}

	private float m_closestNoteSmallestDiff = -1.0f;
	private int m_closestNoteSmallestDiffIndex = -1;
	private int getClosestNoteFreqIndex(float freq, int startIndex)
	{
		m_closestNoteSmallestDiff = Float.MAX_VALUE;
		m_closestNoteSmallestDiffIndex = -1;
		for (int i=startIndex;i<NOTE_FREQS.length;i++)
		{
			float diff = Math.abs(NOTE_FREQS[i]-freq);
			if (diff < m_closestNoteSmallestDiff)
			{
				m_closestNoteSmallestDiff = diff;
				m_closestNoteSmallestDiffIndex = i;
			}
			else
			{
				return m_closestNoteSmallestDiffIndex;
			}
		}
		return -1;
	}
}
