//
// Programmer:    Craig Stuart Sapp <craig@ccrma.stanford.edu>
// Creation Date: Mon Jul  2 17:50:34 PDT 2001
// Last Modified: Thu Jul  5 23:09:55 PDT 2001
// Filename:      ...sig/maint/code/Filter/Pitch/Pitch.cpp
// Web Address:   http://sig.sapp.org/src/sigSignal/Pitch.cpp
// Documentation: http://sig.sapp.org/doc/classes/Pitch
// Syntax:        C++ 
//
// Description: Filter object which determines the pitch of the
//     input signal;
//

#include "Pitch.h"
#include <stdlib.h>
#include "Block.h"
#include "ComplexD.h"
#include "transforms.h"

#include <stdio.h>
#include "clm.h"

#define PITCH_HPS 0
#define PITCH_ML  1


//////////////////////////////
//
// Pitch::Pitch --
//

Pitch::Pitch(void) {
   setName("Pitch");
   brandname = FILTER;
   clear();

   mlrefmatrix.setSize(0);
   mlinit = 0;
   algorithm = PITCH_HPS;
   framesize = 1024;
   overlap = 2;
   countdown = framesize/overlap;
   subframesize = countdown;
   harmonics = 5;
   zeroPad = 4;
   currentFrame = 0;
   subframecount = 2*2;
   subframes.setSize(subframecount);  
   for (int i=0; i<subframecount; i++) {
      subframes[i].setSize(4096);
      subframes[i].setSize(0);
   }
   framebuffer.setSize(subframecount+10);
   framebuffer.reset();
   signalbuffer.setSize(zeroPad * framesize);
   spectrumbuffer.setSize(zeroPad * framesize);
   tspectrum.setSize(zeroPad * framesize);
 
   window.setSize(1024);
   mus_make_fft_window_with_window(MUS_HANN_WINDOW, 1024, 0, window.getBase());

}



//////////////////////////////
//
// Pitch::~Pitch --
//

Pitch::~Pitch() { 

   // delete the storeage for mlrefmatrix if it used:
   for (int i=0; i<mlrefmatrix.getSize(); i++) {
      if (mlrefmatrix[i] != NULL) {
         delete [] mlrefmatrix[i];
         mlrefmatrix[i] = NULL;
      }
   }
}



//////////////////////////////
//
// Pitch::action -- If a subframe is filled then generate a new
//     pitch estimate, otherwise do nothing except count down
//     to the next analysis time.
//

void Pitch::action(void) {
   inputValue = inputs[0];

   if (subframes[currentFrame].getSize() >= subframesize) {
      Array<double> *ptr = &(subframes[currentFrame]);
      framebuffer.insert(ptr);
      currentFrame++;
      if (currentFrame >=  subframecount) {
         currentFrame = 0;
      }
      subframes[currentFrame].setSize(0);
      if (framebuffer.getCount() >= overlap) {
         buildAnalysisFrame();
         generatePitchAnalysis();
      }
      countdown = framesize/overlap;
      // throw away the old data
      if (framebuffer.getCount() > 1) {
         framebuffer.extract();
      }
   } else {
      subframes[currentFrame].append(inputValue);
      countdown--;
   }
}



//////////////////////////////
//
// Pitch::clear --
//

void Pitch::clear(void) {
   pitch = 0.0;
   amplitude = 0.0;
   countdown = 0;
}



//////////////////////////////
//
// Pitch::doHPS -- use the HPS algorithm for pitch detection.
//

void Pitch::doHPS(void) {
   algorithm = PITCH_HPS;
}



//////////////////////////////
//
// Pitch::doML -- use the ML algorithm for pitch detection.
//

void Pitch::doML(void) {
   algorithm = PITCH_ML;
   if (mlinit == 0) {
      setupML();
   }
}



//////////////////////////////
//
// Pitch::output --
//

sampleType Pitch::output(int channel) {
   if (channel == 0) {
      return pitch;
   } else if (channel == 2) {
      return countdown;
   } else if (channel == 1) {
      return amplitude;
   } else if (channel == 3) {
      return pindex;
   }
   return 0.0;
}



//////////////////////////////
//
// Pitch::printState --
//

void Pitch::printState(void) {

}


//////////////////////////////////////////////////////////////////////////
//
// private functions
//

//////////////////////////////
//
// buildAnalysisFrame -- build the zero padded, windowed signal
//    to get it ready for pitch algorithm processing.
//

void Pitch::buildAnalysisFrame(void) {
   // assume overlap of two for now:

   int i;
   for (i=0; i<512; i++) {
      signalbuffer[i] = (*framebuffer[1])[i] * window[i];
      signalbuffer[i+512] = (*framebuffer[0])[i] * window[i+512];
   }


}



//////////////////////////////
//
// Pitch::generatePitchAnalysis -- send the signalbuffer data to the 
//    pitch analyzer and update the new pitch estimate.
//

void Pitch::generatePitchAnalysis(void) {

   switch (algorithm) {
      case PITCH_HPS:
         pindex = HPS(signalbuffer.getBase(), signalbuffer.getSize(), 
            20, signalbuffer.getSize()-1, harmonics, 
            spectrumbuffer.getBase());
         break;
      case PITCH_ML:
         pindex = ML(signalbuffer.getBase(), signalbuffer.getSize(),
                  0, 511, spectrumbuffer.getBase(), tspectrum.getBase(),
                  mlrefmatrix.getBase(), mlrefmatrix.getSize());
         break;
   }

//   pitch = pindex;
   pitch = pindex / 4096 * getSrate();  // good only for HPS
   amplitude = spectrumbuffer[pindex];


}



/////////////////////////////////////////////////////////////////////////
// 
// other functions
//


//////////////////////////////
//
// Pitch::absfft --
//

void Pitch::absfft(double* spectrum, double* paddedFrame, int size) {
   double *tempr = new double[size];
   double *tempi = new double[size];
   int i;
   for (i=0; i<size; i++) {
      tempr[i] = paddedFrame[i];
      tempi[i] = 0.0;
   }

   ::mus_fft(tempr, tempi, size, -1);

   for (i=0; i<size; i++) {
      spectrum[i] = sqrt(tempr[i] * tempr[i] + tempi[i] * tempi[i]);
   }
}



// md5sum: 91a0ebbf51e194b9bc2ee30eb194dc2a Pitch.cpp [20050403]