#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "filter.h"

#ifndef M_PI
#define	M_PI		3.14159265358979323846
#endif

#define	Debug	1

/*
	differential filter
	
	\frac{2}{T} \frac{1-z^{-1}}{1+z^{-1}}
	where T = sampling time
*/

extern void differential(int num, double *inp, double *outp, double sampling)
{
	int	k;
	
	outp[0] = 0.00;
	for (k=1; k<num; k++) {
		outp[k] = (2.00/sampling)*(inp[k]-inp[k-1]) - outp[k-1];
	}
}

/*
	low pass filter
	
	G(\frac{2}{T} \frac{1-z^{-1}}{1+z^{-1}})
	where
	G(s) = \frac{\omega^{2}}{s^{2} + 2\zeta\omega s + \omega^{2}}
	\omaga = \frac{2}{T} atan2(2\pi cutofffreq, 2)
	
	cutofffreq : cut off frequency [Hz]
	zeta : damping ratio
*/

extern void lowpass(int num, double *inp, double *outp, double sampling,
		    double cutofffreq, double zeta)
{
	int	k;
	double	cutoffomega, omega, ot, c0, c1, c2;
	
	cutoffomega = 2.00 * M_PI * cutofffreq;
	omega = 2.00/sampling * atan2(cutoffomega*sampling, 2.00);
	ot = omega * sampling;
	c0 = 4 + 4*zeta*ot + ot*ot;
	c1 = -8 + 2*ot*ot;
	c2 = 4 - 4*zeta*ot + ot*ot;
#if Debug
	fprintf(stderr, "omega = %lf   zeta = %lf\n", omega, zeta);
	fprintf(stderr, "filter coefs : %lf %lf %lf %lf\n",
			c0, c1, c2, ot*ot);
#endif

	outp[0] = outp[1] = 0.00;
	for (k=2; k<num; k++) {
		outp[k] = (ot*ot*(inp[k]+2*inp[k-1]+inp[k-2])
			   - c1*outp[k-1] - c2*outp[k-2]) / c0;
	}
}