/* WAVELET.C */

#include <math.h>

typedef enum { DECOMP, RECON } wavetype;

#include "wavelet.h"

void WaveletCoeffs(double alpha, double beta, double *wavecoeffs)
{
	double tcosa, tcosb, tsina, tsinb;
	char i;
	/* precalculate cosine of alpha and sine of beta to reduce */
	/* processing time */
	tcosa = cos(alpha);
	tcosb = cos(beta);
	tsina = sin(alpha);
	tsinb = sin(beta);

	/* calculate first two wavelet coefficients  a = a(-2) and b = a(-1) */
	wavecoeffs[0] = ((1.0 + tcosa + tsina) * (1.0 - tcosb - tsinb)
																			+ 2.0 * tsinb * tcosa) / 4.0;
	wavecoeffs[1] = ((1.0 - tcosa + tsina) * (1.0 + tcosb - tsinb)
																			- 2.0 * tsinb * tcosa) / 4.0;

	/* precalculate cosine and sine of alpha minus beta to reduce */
	/* processing time */
	tcosa = cos(alpha - beta);
	tsina = sin(alpha - beta);

	/* calculate last four wavelet coefficients  c = a(0), d = a(1), */
	/* e = a(2), and f = a(3) */
	wavecoeffs[2]  = (1.0 + tcosa + tsina) / 2.0;
	wavecoeffs[3]  = (1.0 + tcosa - tsina) / 2.0;
	wavecoeffs[4]  = 1 - wavecoeffs[0] - wavecoeffs[2];
	wavecoeffs[5]  = 1 - wavecoeffs[1] - wavecoeffs[3];

	/* zero out very small coefficient values caused by truncation error */
	for (i = 0; i < 6; i++)
	{
		if (fabs(wavecoeffs[i]) < 1.0e-15)
			wavecoeffs[i] = 0.0;
	}

}


char MakeWaveletFilters(double *wavecoeffs, double *Lfilter,
								double *Hfilter, wavetype transform)
{
	char i, j, k, filterlength;

	/* find the first non-zero wavelet coefficient */
	i = 0;
	while(wavecoeffs[i] == 0.0)
		i++;

	/* find the last non-zero wavelet coefficient */
	j = 5;
	while(wavecoeffs[j] == 0.0)
		j--;

	/* form the decomposition filters h~ and g~ or the reconstruction
		 filters h and g.  the division by 2 in the construction
		 of the decomposition filters is for normalization */
	filterlength = j - i + 1;
	for(k = 0; k < filterlength; k++)
	{
		if (transform == DECOMP)
		{
			Lfilter[k] = wavecoeffs[j--] / 2.0;
			Hfilter[k] = (double) (((i++ & 0x01) * 2) - 1) * wavecoeffs[i] / 2.0;
		}
		else
		{
			Lfilter[k] = wavecoeffs[i++];
			Hfilter[k] = (double) (((j-- & 0x01) * 2) - 1) * wavecoeffs[j];
		}
	}

	/* clear out the additional array locations, if any */
	while (k < 6)
	{
		Lfilter[k] = 0.0;
		Hfilter[k++] = 0.0;
	}

	return filterlength;
}


double DotP(double *data, double *filter, char filtlen)
{
	char i;
	double sum;

	sum = 0.0;
	for (i = 0; i < filtlen; i++)
		sum += *data-- * *filter++; /* decreasing data pointer is moving */
																/* backward in time */
	return sum;
}


void ConvolveDec2(double *input_sequence, int inp_length,
					double *filter, char filtlen, double *output_sequence)
	/* convolve the input sequence with the filter and decimate by two */
{
	int i;
	char shortlen, offset;

	for(i = 0; (i <= inp_length + 8) && ((i - filtlen) <= inp_length + 8); i += 2)
	{
		if (i < filtlen)
			*output_sequence++ = DotP(input_sequence + i, filter, i + 1);
		else if (i > (inp_length + 5))
		{
			shortlen = filtlen - (char) (i - inp_length - 4);
			offset = (char) (i - inp_length - 4);
			*output_sequence++ = DotP(input_sequence + inp_length + 4,
																				filter + offset, shortlen);
		}
		else
			*output_sequence++ = DotP(input_sequence + i, filter, filtlen);
	}

}


int DecomposeBranches(double *In, int Inlen, double *Lfilter,
		double *Hfilter, char filtlen, double *OutL, double *OutH)
	/* take input data and filters and form two branches of have the
	   original length. length of branches is returned */
{
	ConvolveDec2(In, Inlen, Lfilter, filtlen, OutL);
	ConvolveDec2(In, Inlen, Hfilter, filtlen, OutH);
	return (Inlen / 2);
}


void WaveletDecomposition(double *InData, int Inlength, double *Lfilter,
			double *Hfilter, char filtlen, char levels, double **OutData)
	/* assumes that the input data has 2 ^ (levels) data points per unit
		 interval.  first InData is input signal; all others are the
		 intermediate approximation coefficients */
{
	char i;

	for (i = 0; i < levels; i++)
	{
		Inlength = DecomposeBranches(InData, Inlength, Lfilter, Hfilter,
												filtlen, OutData[2 * i], OutData[(2 * i) + 1]);
		InData = OutData[2 * i];
	}
}


double DotpEven(double *data, double *filter, char filtlen)
{
	char i;
	double sum;

	sum = 0.0;
	for (i = 0; i < filtlen; i += 2)
		sum += *data-- * filter[i]; /* decreasing data pointer is moving */
																/* backward in time */
	return sum;
}


double DotpOdd(double *data, double *filter, char filtlen)
{
	char i;
	double sum;

	sum = 0.0;
	for (i = 1; i < filtlen; i += 2)
		sum += *data-- * filter[i];	/* decreasing data pointer is moving */
																/* backward in time */
	return sum;
}


void ConvolveInt2(double *input_sequence, int inp_length, double *filter,
					char filtlen, char sum_output, double *output_sequence)
	/* insert zeros between each element of the input sequence and
	   convolve with the filter to interpolate the data */
{
	int i;

	if (sum_output) /* summation with previous convolution if true */
	{
			/* every other dot product interpolates the data */
		for(i = (filtlen / 2) - 1; i < inp_length + filtlen - 2; i++)
		{
			*output_sequence++ += DotpOdd(input_sequence + i, filter, filtlen);
			*output_sequence++ += DotpEven(input_sequence + i + 1, filter, filtlen);
		}

		*output_sequence++ += DotpOdd(input_sequence + i, filter, filtlen);
	}
	else /* first convolution of pair if false */
	{
			/* every other dot product interpolates the data */
		for(i = (filtlen / 2) - 1; i < inp_length + filtlen - 2; i++)
		{
			*output_sequence++ = DotpOdd(input_sequence + i, filter, filtlen);
			*output_sequence++ = DotpEven(input_sequence + i + 1, filter, filtlen);
		}

		*output_sequence++ = DotpOdd(input_sequence + i, filter, filtlen);
	}
}


int ReconstructBranches(double *InL, double *InH, int Inlen,
				double *Lfilter, double *Hfilter, char filtlen, double *Out)
	/* take input data and filters and form two branches of have the
	   original length. length of branches is returned */
{
	ConvolveInt2(InL, Inlen, Lfilter, filtlen, 0, Out);
	ConvolveInt2(InH, Inlen, Hfilter, filtlen, 1, Out);
	return Inlen * 2;
}


void WaveletReconstruction(double **InData, int Inlength, double *Lfilter,
								double *Hfilter, char filtlen, char levels, double *OutData)
	/* assumes that the input data has 2 ^ (levels) data points per unit
	   interval */
{
	double *Output;
	char i;

	Inlength = Inlength >> levels;
	/* destination of all but last branch reconstruction is the next
		 higher intermediate approximation */
	for (i = levels - 1; i > 0; i--)
	{
		Output = InData[2 * (i - 1)];
		Inlength = ReconstructBranches(InData[2 * i], InData[(2 * i) + 1],
													Inlength, Lfilter, Hfilter, filtlen, Output);
	}

	/* destination of the last branch reconstruction is the output data */
	ReconstructBranches(InData[0], InData[1], Inlength, Lfilter, Hfilter,
																											filtlen, OutData);
}


double CalculateMSE(double *DataSet1, double *DataSet2, int length)
{
	/* calculate mean squared error of output of reconstruction as
		 compared to the original input data */
	int i;
	double pointdiff, topsum, botsum;

	topsum = botsum = 0.0;
	for (i = 0; i < length; i++)
	{
		pointdiff = DataSet1[i] - DataSet2[i];
		topsum += pointdiff * pointdiff;
		botsum += DataSet1[i] * DataSet1[i];
	}

	return topsum / botsum;
}