//
//  dsptools.cpp
//  wablet
//
//  Created by Robert Tubb on 21/06/2011.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#include "dsptools.h"
#include "ofMain.h"

DSPTools::DSPTools(){
    for(int i = 0; i < 3; i++){
        xv[i] = 0.0;
        yv[i] = 0.0;
    }
    getHPCoefficientsButterworth2Pole(ax, by);
}

double DSPTools::highpass1(double ax){
    static double xm1 = 0.0;
    double sample;
    sample = 2*(ax - xm1);
    xm1 = ax;
    return sample;    
    
}
double DSPTools::lowpass1(double ax){
    static double xm1 = 0.0;
    double sample;
    sample = 0.5*(ax + xm1);
    xm1 = ax;
    return sample;
    
}

void DSPTools::getLPCoefficientsButterworth2Pole(const int samplerate, const double cutoff, double* const ax, double* const by)
{
    double sqrt2 = 1.4142135623730950488;
    
    double QcRaw  = (2 * PI * cutoff) / samplerate; // Find cutoff frequency in [0..PI]
    double QcWarp = tan(QcRaw); // Warp cutoff frequency
    
    double gain = 1 / (1+sqrt2/QcWarp + 2/(QcWarp*QcWarp));
    by[2] = (1 - sqrt2/QcWarp + 2/(QcWarp*QcWarp)) * gain;
    by[1] = (2 - 2 * 2/(QcWarp*QcWarp)) * gain;
    by[0] = 1;
    ax[0] = 1 * gain;
    ax[1] = 2 * gain;
    ax[2] = 1 * gain;
}
void DSPTools::getHPCoefficientsButterworth2Pole(double* const ax, double* const by)
{
    ax[0] = 0.997987115675119;
    ax[1] = -1.995974231350238;
    ax[2] = 0.997987115675119;

    by[0] = 1.000000000000000;
    by[1] = -1.995970179642828;
    by[2] = 0.995978283057647;
}

double DSPTools::butter(double sample)
{

        xv[2] = xv[1]; 
        xv[1] = xv[0];
        xv[0] = sample;
        yv[2] = yv[1]; 
        yv[1] = yv[0];
        
        yv[0] =   (ax[0] * xv[0] + ax[1] * xv[1] + ax[2] * xv[2]
                   - by[1] * yv[1]
                   - by[2] * yv[2]);
        
        return yv[0];

}