#include "filter.h"

#ifndef PI
    #define PI M_PI
#endif

//  ----------------------------------------------------------------------------------
//  FILTERS
//  ----------------------------------------------------------------------------------

filter::filter(void)
{
    memset(&filtP,0,sizeof(filtP));
    memset(&filtState,0,sizeof(filtState));
}

//  ------------------
//  ~filter
//  ------------------

filter::~filter(void)
{
}

//  ------------------
//  initFilters
//  ------------------

void filter::initFilters(int axis)
{
	double Fs = kFreq;

	//notch variables
	double w0;
	double alpha;
	double b0,b1,b2,a0,a1,a2;

	//lag lead variable
	double omega1, omega2;
	double k0, k1, k2;

    memset(filtState[axis], 0, sizeof(filtState[axis]));

    for (int ifilter = 0; ifilter < NiFILT; ifilter++){

        if( filtP[axis][ifilter].ftype == FILTER_NOTCH){

            w0 = 2.0*PI*filtP[axis][ifilter].f1/Fs;
            alpha = sin(w0)/(2.0*filtP[axis][ifilter].q);

			// notch:      H(s) = (s^2 + 1) / (s^2 + s/Q + 1)

			b0 =  1;
			b1 = -2*cos(w0);
			b2 =  1;
			a0 =  1 + alpha;
			a1 = -2*cos(w0);
			a2 =  1 - alpha;

			//   y[n] = (b0/a0)*x[n] + (b1/a0)*x[n-1] + (b2/a0)*x[n-2] - (a1/a0)*y[n-1] - (a2/a0)*y[n-2]
			if(a0!=0){
                filtState[axis][ifilter].x0Gain = b0/a0;
                filtState[axis][ifilter].x1Gain = b1/a0;
                filtState[axis][ifilter].x2Gain = b2/a0;
                filtState[axis][ifilter].y1Gain = a1/a0;
                filtState[axis][ifilter].y2Gain = a2/a0;
			}

        } else if( filtP[axis][ifilter].ftype == FILTER_LOWPASS){

            w0 = 2.0*PI*filtP[axis][ifilter].f1/Fs;
            alpha = sin(w0)/(2.0*filtP[axis][ifilter].q);

			//  LPF:        H(s) = 1 / (s^2 + s/Q + 1)

			b0 =  (1 - cos(w0))/2;
			b1 =   1 - cos(w0);
			b2 =  (1 - cos(w0))/2;
			a0 =   1 + alpha;
			a1 =  -2*cos(w0);
			a2 =   1 - alpha;

			if(a0!=0){
                filtState[axis][ifilter].x0Gain = (b0/a0);
                filtState[axis][ifilter].x1Gain = (b1/a0);
                filtState[axis][ifilter].x2Gain = (b2/a0);
                filtState[axis][ifilter].y1Gain = (a1/a0);
                filtState[axis][ifilter].y2Gain = (a2/a0);
			}

		}
        else if( filtP[axis][ifilter].ftype == FILTER_LAGLEAD){
			//lag lead filter - 1st order ( no [n-2] terms)*/
            if(filtP[axis][ifilter].f1>0 && filtP[axis][ifilter].q>0 ){
                omega1 = filtP[axis][ifilter].f1 * (2 * PI);
                omega2 = filtP[axis][ifilter].q * omega1;
				k1 = exp( -omega1 / Fs );
				k2 = exp( -omega2 / Fs );
				k0 = ( 1.0 - k1 )/( 1.0 - k2 );
                filtState[axis][ifilter].x0Gain = k0;
                filtState[axis][ifilter].x1Gain = -k0 * k2;
                filtState[axis][ifilter].x2Gain = 0;
                filtState[axis][ifilter].y1Gain = -k1;
                filtState[axis][ifilter].y2Gain = 0;
			}
		}
	}
}

//  ------------------
//  runfilters
//  ------------------

float filter::runfilters(float in, int axis)
{
	double input = in;
	double output = input;

    for (int ifilter = 0; ifilter < NiFILT; ifilter++){
        if(filtP[axis][ifilter].ftype != FILTER_NONE){
			input = output;
            output =  (filtState[axis][ifilter].x0Gain)*input;
            output += (filtState[axis][ifilter].x1Gain)*(filtState[axis][ifilter].x1);
            output += (filtState[axis][ifilter].x2Gain)*(filtState[axis][ifilter].x2);
            output -= (filtState[axis][ifilter].y1Gain)*(filtState[axis][ifilter].y1);
            output -= (filtState[axis][ifilter].y2Gain)*(filtState[axis][ifilter].y2);
            filtState[axis][ifilter].y2 = filtState[axis][ifilter].y1;
            filtState[axis][ifilter].y1 = output;
            filtState[axis][ifilter].x2 = filtState[axis][ifilter].x1;
            filtState[axis][ifilter].x1 = input;
		}
	}
	return (float)output;
}

//  ------------------
//  runfilter
//  ------------------

float filter::runfilter(float in, int axis, int ifilter)
{
    double input = in;
    double output = input;

    if(filtP[axis][ifilter].ftype != FILTER_NONE){
        input = output;
        output =  (filtState[axis][ifilter].x0Gain)*input;
        output += (filtState[axis][ifilter].x1Gain)*(filtState[axis][ifilter].x1);
        output += (filtState[axis][ifilter].x2Gain)*(filtState[axis][ifilter].x2);
        output -= (filtState[axis][ifilter].y1Gain)*(filtState[axis][ifilter].y1);
        output -= (filtState[axis][ifilter].y2Gain)*(filtState[axis][ifilter].y2);
        filtState[axis][ifilter].y2 = filtState[axis][ifilter].y1;
        filtState[axis][ifilter].y1 = output;
        filtState[axis][ifilter].x2 = filtState[axis][ifilter].x1;
        filtState[axis][ifilter].x1 = input;
    }
    return (float)output;
}

//  ------------------
//  addFilter
//  ------------------

void filter::addFilter(int axis,int ifilt,int ftype, float f1, float q)
{
    if( (axis<0) || (axis>(NiAXES-1)))return;
    if( (ifilt<0) || (ifilt>(NiFILT-1)))return;

    filtP[axis][ifilt].f1 = f1;
    filtP[axis][ifilt].q = q;
    filtP[axis][ifilt].ftype = ftype;
    initFilters(axis);
}

//  ------------------
//  getFilter
//  ------------------

void filter::getFilter(int axis,int ifilt,filterPar_t *fp)
{
	if(fp==NULL)return;
	if( (axis<0) || (axis>(NiAXES-1)))return;
	if( (ifilt<0) || (ifilt>(NiFILT-1)))return;

    fp->f1 = filtP[axis][ifilt].f1;
    fp->q = filtP[axis][ifilt].q;
    fp->ftype = filtP[axis][ifilt].ftype;
}

//  ------------------
//  setFilter
//  ------------------

void filter::setFilter(int axis,int ifilt,filterPar_t *fp)
{
	if(fp==NULL)return;
	if( (axis<0) || (axis>(NiAXES-1)))return;
	if( (ifilt<0) || (ifilt>(NiFILT-1)))return;

    filtP[axis][ifilt].f1 = fp->f1;
    filtP[axis][ifilt].q = fp->q;
    filtP[axis][ifilt].ftype = fp->ftype;

	initFilters(axis);
}
