//    tonalisa - software to look at overtone-structures
//    Copyright (C) 2016  Dominik Schmidt-Philipp
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
#include <QObject>
#include <QDebug>
#include <math.h>
//#include <lo/lo.h>
//#include <lo/lo_cpp.h>
#include "peaks.h"

Peaks::Peaks(QObject *parent)
  : QObject(parent)
{
  m_fftData.reserve(1024);
  m_fftSize = m_fftData.size() * 2;
  m_windowSize = 501;   // samples
  m_sampleRate = 48000; // Hz
  m_threshold = -40;    // dB
  //for ( int i=1; i<10; i++) {
  //  m_freqs << i * 136.1;
  //  m_amps << 1.0/i;
  //}
}

// getters --------------------------------
QList<double> Peaks::freqs() const {
  return m_freqs;
}
QList<double> Peaks::amps() const {
  return m_amps;
}
QList<double> Peaks::peaks() const {
  QList<double> peaks;
  peaks.clear(); // probably obsolete
  peaks.reserve(2*m_freqs.size());
  for ( int i=0; i<m_freqs.size(); i++) {
    peaks << m_freqs[i];
    peaks << m_amps[i];
  }
  return peaks;
}
double Peaks::freqBase() const {
  return m_freqBase;
}
QList<double> Peaks::fftData() {
  return m_fftData;
}
double Peaks::sampleRate() const {
  return m_sampleRate;
}
int Peaks::windowIndex() const {
  return m_windowIndex;
}
int Peaks::windowSize() const {
  return m_windowSize;
}
double Peaks::threshold() const {
  return m_threshold;
}
// setters --------------------------------
void Peaks::setFreqBase(const double &freqBase)
{
  if ( m_freqBase != freqBase ) {
    m_freqBase = freqBase;
    emit freqBaseChanged();
  }
}
void Peaks::setFftData(QList<double> &fftData)
{
  m_fftData = fftData;
  m_fftSize = m_fftData.size() * 2;
  new_fftData();
  emit fftDataChanged();
}
void Peaks::setSampleRate(const double &sampleRate)
{
  if ( m_sampleRate != sampleRate ) {
    m_sampleRate = sampleRate;
    emit sampleRateChanged();
  }
}
void Peaks::setWindowIndex( const int &indx )
{
  if( m_windowIndex != indx ) {
    if(indx != MUS_DPSS_WINDOW) {
      m_windowIndex = (mus_fft_window_t)indx;
      emit windowChanged();
    }
  }
}
void Peaks::setWindowSize( const int &windowSize )
{
  if (m_windowSize != windowSize){
    m_windowSize = windowSize;
  }
}
void Peaks::setFreqs(QList<double> &freqs) 
{
  bool same = true;
  if (freqs.size() == m_freqs.size()) {
    for (int i=0; i<m_freqs.size();i++)
      if (freqs[i] != m_freqs[i])
	same = false;
  } else { same = false; }
  if(!same){
    m_freqs = freqs;
    oscSendPeaks();
    emit peaksChanged();
  }
}
void Peaks::setAmps(QList<double> &amps) 
{
	bool same = true;
	if (amps.size() == m_amps.size()) {
		for (int i=0; i<m_amps.size();i++)
			if (amps[i] != m_amps[i])
				same = false;
	} else { same = false; }
	if(!same){
		m_amps = amps;
		oscSendPeaks();
		emit peaksChanged();
	}
	
}
void Peaks::setThreshold(const double &threshold) {
  if (m_threshold != threshold) {
    m_threshold = threshold;
    emit thresholdChanged();
    new_fftData();
  }
}

//----------------------------------------------------
void Peaks::oscSendPeaks()
{
//  lo::Address a("localhost", "9000");
//  lo::Message f;
//  lo::Message am;
//  for (int i=0;i<m_freqs.size();i++) {
//    f.add("f", m_freqs[i]);
//    am.add("f", m_amps[i]);
//  }
//  a.send("/current/freqs", f);
//  a.send("/current/amps", am);
}

void Peaks::addPeak(double freq, double amp)
{
  m_freqs << freq;
  m_amps << amp;
  
  oscSendPeaks();
  emit peaksChanged();
}

// peak finding ----------------------------------------------
void Peaks::resetPeaks()
{
  m_freqs.clear();
  m_amps.clear();
  oscSendPeaks();
  emit peaksChanged();
}

QVector<double> Peaks::interpolatePeak(int k_beta){
  double alpha,beta,gamma,p,k_star,hz,amp;
  QVector<double> ret;
  // fit parabola through highest +-1 peaks
  // algorithm from JOS PARSHL CCRMA
  alpha = 20*log10(m_fftData[k_beta-1]);
  beta = 20*log10(m_fftData[k_beta]);
  gamma = 20*log10(m_fftData[k_beta+1]);

  p = (alpha-gamma) / (alpha - 2*beta + gamma);
  p /= 2;
  k_star = k_beta + p;
  amp = beta - (alpha - gamma) * p / 4;
  hz = m_sampleRate*k_star/m_fftSize;
  ret << hz;
  ret << amp;
  return ret;
}

void Peaks::findHighestPeak()
{
  double alpha,beta,gamma,p,k_star,hz,amp,max = 0;
  int k_beta = 0;
  
  for ( int i=0; i<m_fftData.size(); i++) {
    if ( m_fftData[i] > max && i>50) {
      max = m_fftData[i];
      k_beta = i;
    }
  }

  // fit parabola through highest +-1 peaks
  // algorithm from JOS PARSHL CCRMA
  alpha = 20*log10(m_fftData[k_beta-1]);
  beta = 20*log10(m_fftData[k_beta]);
  gamma = 20*log10(m_fftData[k_beta+1]);

  p = (alpha-gamma) / (alpha - 2*beta + gamma);
  p /= 2;
  k_star = k_beta + p;
  amp = beta - (alpha - gamma) * p / 4;
  hz = m_sampleRate*k_star/m_fftSize;
  
  // save found peak to peak list
  addPeak(hz,pow(10, amp/20));
  //emit peaksChanged();

  // remove the peak we just found
  // version 1: simple zero out some environment
  /*for ( int i=-8; i<10; i++)
    if(k_beta+i >=0)
    m_fftData[k_beta + i] = -10000;*/
  // version 2: subtract fft from scaled window
  mus_float_t re[m_fftSize];
  mus_float_t im[m_fftSize];
  mus_float_t* window;
  window = mus_make_fft_window(m_windowIndex,m_windowSize,6.0);
  for ( int i=0; i<m_fftSize; i++ ) {
    if (i < m_windowSize ) {
      re[i]= sin(i * 2 * M_PI * k_star / m_fftSize) * window[i];
      im[i]= cos(i * 2 * M_PI * k_star / m_fftSize) * window[i];
    }
    else {
      re[i] = 0;
      im[i] = 0;
    }
  }
  free(window);
  mus_fft(re,im,m_fftSize,1);
  for ( int i=0; i<m_fftSize/2; i++ ) {
    m_fftData[i] -= sqrt(re[i]*re[i]+im[i]*im[i])/m_sampleRate;
  }
  // end subtract lobe from window
  emit fftDataChanged();
 
}

void Peaks::new_fftData(){
  QList<int> peaks;
  QList<int> peaks_f1;
  
  // find all maxima
  // filter treshold
  for ( int i=1; i<m_fftData.size(); i++ ) {
    if ( m_fftData[i] > m_fftData[i-1] &&
	 m_fftData[i] > m_fftData[i+1] ) {
      peaks << i;
    }
  }

  for ( int i=1; i<peaks.size(); i++ ) {
    if ( 20*log10(m_fftData[peaks[i]]) > m_threshold ) {
      peaks_f1 << peaks[i];
    }
  }

  QVector<double> peak;
  // interpolate peak values
  resetPeaks();
  for ( int i=0; i<peaks_f1.size(); i++ ) {
    peak = interpolatePeak(peaks_f1[i]);
    addPeak(peak[0],pow(10,peak[1]/20));
  }
}
