/*
 * filter.c
 *
 *  Created on: Dec 19, 2014
 *      Author: hutch
 */

#include "filter.h"
#include "queue.h"
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <float.h>

/********************************************************************
 * These arrays hold the filter coefficients for the filters.
 ********************************************************************/
// FIR filter size and coefficient storage.
#define FIR_FILTER_TAP_COUNT 50
double firCoefficient[FIR_FILTER_TAP_COUNT];
// IIR A Coefficients count and storage.
#define IIR_A_COEFFICIENT_COUNT 4
double iir_a_coefficient[FILTER_IIR_FILTER_COUNT][IIR_A_COEFFICIENT_COUNT];
// IIR B Coefficients count and storage.
#define IIR_B_COEFFICIENT_COUNT 5
double iir_b_coefficient[FILTER_IIR_FILTER_COUNT][IIR_B_COEFFICIENT_COUNT];

/********************************************************************
 * Static queues: x, y, z, and output.
 ********************************************************************/
#define X_QUEUE_SIZE FIR_FILTER_TAP_COUNT				// Input queue needs to be the same as FIR filter tap count.
static queue_t xQueue;													// The queue will store past input values.

#define Y_QUEUE_SIZE IIR_B_COEFFICIENT_COUNT
static queue_t yQueue;													// Residual y values will be stored here.

#define Z_QUEUE_SIZE IIR_A_COEFFICIENT_COUNT
static queue_t zQueue[FILTER_IIR_FILTER_COUNT];	// Residual z values will be stored here.

#define OUTPUT_QUEUE_SIZE FILTER_INPUT_PULSE_WIDTH		// Need to keep this many outputs so you can search for pulses.
static queue_t outputQueue[FILTER_IIR_FILTER_COUNT];	// Keep track of IIR filter outputs to compute power.

/**************************************************************************************************
 *Keep track of the last value computed by the power function.
 *************************************************************************************************/
static double currentPowerValue[FILTER_IIR_FILTER_COUNT];

/**************************************************************************************************
 * Filter Constants. These are the actual constants. These must be loaded into the coefficient arrays.
 *************************************************************************************************/
static double firCoefficientConstants[FIR_FILTER_TAP_COUNT] = {};
static double iirACoefficientConstants[FILTER_IIR_FILTER_COUNT][IIR_A_COEFFICIENT_COUNT] = {};
static double iirBCoefficientConstants[FILTER_IIR_FILTER_COUNT][IIR_B_COEFFICIENT_COUNT] = {};

/**************************************************************************************************
 * These functions load the filter coefficients.
 *************************************************************************************************/
// Loads the A coefficients into the IIR filter.
void loadIirACoefficients(double iirACoefficientConstants[FILTER_IIR_FILTER_COUNT][IIR_A_COEFFICIENT_COUNT]) {
	for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++) {
		for (int j=0; j<IIR_A_COEFFICIENT_COUNT; j++)
		iir_a_coefficient[i][j] = iirACoefficientConstants[i][j];
	}
}

// Loads the B coefficients into the IIR filter.
void loadIirBCoefficients(double iirACoefficientConstants[FILTER_IIR_FILTER_COUNT][IIR_B_COEFFICIENT_COUNT]) {
	for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++) {
		for (int j=0; j<IIR_B_COEFFICIENT_COUNT; j++)
		iir_b_coefficient[i][j] = iirBCoefficientConstants[i][j];
	}
}

// Load the coefficients into the FIR filter.
void loadFirCoefficients(double fir_coefficient_values[]) {
	for (int i=0; i<FIR_FILTER_TAP_COUNT; i++) {
		firCoefficient[i] = fir_coefficient_values[i];
	}
}

/**************************************************************************************************
 * These functions fill all of the queues with 0s.
 *************************************************************************************************/
// Fills all zQueues with 0s.
void zeroZQueues() {
	for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++)
		for (int j=0; j<Z_QUEUE_SIZE; j++)
			queue_overwritePush(&(zQueue[i]), 0.0);
}

// Fills the yQueue with 0s.
void zeroYQueue() {
	for (int i=0; i<Y_QUEUE_SIZE; i++)
		queue_overwritePush(&yQueue, 0.0);
}

// Fills the xQueue with 0s.
void zeroXQueue() {
	for (int i=0; i<X_QUEUE_SIZE; i++)
		queue_overwritePush(&xQueue, 0.0);
}

// Fills the output queue with 0s.
void zeroOutputQueues() {
	for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++)
		for (int j=0; j<OUTPUT_QUEUE_SIZE; j++)
			queue_overwritePush(&(outputQueue[i]), 0.0);
}

// Return the array containing the current computed power values.
double* getCurrentPowerValues() {
	return currentPowerValue;
}

// Return the filter-number with the highest computed power in its filtered spectrum.
uint16_t getFilterNumberWithMaximumPower() {
	uint16_t minIndex = 0;													// Assume that the index containing the min value is 0.
	double min = currentPowerValue[minIndex];				// Make that value your current min.
	for (int i=1; i<FILTER_IIR_FILTER_COUNT; i++) {	// Now compare that value with all others to see if you were right.
		if (currentPowerValue[i] < min)	{							// If you find a smaller value, make that your new min and keep going.
			min = currentPowerValue[i];
			minIndex = i;																// Keep track of where you found the value.
		}
	}
	return minIndex;	// Just return the min that you found.
}

/**************************************************************************************************
 * Must call filter_init() before using the filters.
 *************************************************************************************************/
// Keep track of whether the filters have been initialized.
static bool filterInitFlag = false;

// This must be called before invoking any filter functions.
void filter_init() {
	// Init queues and fill them with 0s.
	queue_init(&xQueue, X_QUEUE_SIZE);					// Init the main input queue.
	zeroXQueue();
	queue_init(&yQueue, Y_QUEUE_SIZE);					// Init the y-queue (output of FIR)
	zeroYQueue();
	for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++)
		queue_init(&(zQueue[i]), Z_QUEUE_SIZE);		// Init all z-queues (output of IIR)
	zeroZQueues();
	for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++)
		queue_init(&(outputQueue[i]), OUTPUT_QUEUE_SIZE);		// Init all z-queues (output of IIR)
	zeroOutputQueues();
	for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++)	// Zero-out the current value outputs.
		currentPowerValue[i] = 0.0;
	// Load all of the coefficient constants.
	loadIirACoefficients(iirACoefficientConstants);	// Load the IIR-a coefficients.
	loadIirBCoefficients(iirBCoefficientConstants);	// Load the IIR-b coefficients.
	loadFirCoefficients(firCoefficientConstants);		// Load the FIR coefficients.
	filterInitFlag = true;													// Note that the init has been called.
}

uint16_t filter_getFirDecimationFactor() {return FILTER_FIR_DECIMATION_FACTOR;}

// Puts a new input into the FIR-filter input queue.
void filter_addNewInput(double x) {
	if (!filterInitFlag) {
		printf("ERROR! Must call filter_init() prior to calling addNewInput().\r\n");
		exit(0);
	}
	queue_overwritePush(&xQueue, x);
}

// Processes the data in the input queue and returns a single output.
// You must call this routine once for each FILTER_FIR_DECIMATION_FACTOR inputs.
double filter_firFilter() {
	if (!filterInitFlag) {
		printf("ERROR! Must call filter_init() prior to calling filter_firFilter().\r\n");
		exit(0);
	}
	double accumulator = 0.0;
	// FIR filter is just a weighted sum.
	for (int i=0; i<FIR_FILTER_TAP_COUNT; i++) {
		// Grab inputs from the input queue and mult-acc them with the FIR coefficients.
		accumulator += (queue_readElementAt(&xQueue, i) * firCoefficient[FIR_FILTER_TAP_COUNT - i - 1]);
	}
	queue_overwritePush(&yQueue, accumulator);	// Insert the computed value into the yQueue for the IIR filter.
	return accumulator;	// Return the latest value - might be useful for testing, etc.
}

// IIR Filter
double filter_iirFilter(uint16_t filterNumber) {
	if (!filterInitFlag) {
		printf("ERROR! Must call filter_init() prior to calling filter_iirFilter().\r\n");
		exit(0);
	}
	// Compute the a- and b-loops separately and then combine them later to compute the final z-value.
	// Compute the b-loop.
	double bSummation = 0.0;
	for (int i=0; i<IIR_B_COEFFICIENT_COUNT; i++)
		bSummation += iir_b_coefficient[filterNumber][i] * queue_readElementAt(&yQueue, IIR_B_COEFFICIENT_COUNT-i-1);
//	printf("b-summation: %g\r\n", bSummation);
	// Compute the a-loop.
	double aSummation = 0.0;
	for (int i=0; i<IIR_A_COEFFICIENT_COUNT; i++) {
		aSummation += iir_a_coefficient[filterNumber][i] * queue_readElementAt(&(zQueue[filterNumber]), IIR_A_COEFFICIENT_COUNT - i-1);
	}
//	printf("a-summation: %g\r\n", aSummation);
	// Compute the final z-value and push it into the zQueue.
	double zValue = bSummation - aSummation;										// Compute the final z-value using the b- and a-summations.
	queue_overwritePush(&(zQueue[filterNumber]), zValue);				// This queue is used to keep a history of past z-values for the IIR filter.
	queue_overwritePush(&(outputQueue[filterNumber]), zValue);	// This queue keeps track of a larger history of past values so you can compute power.
	return zValue;
}

// The purpose of this function is to compute the power contained in the spectrum produced by a given filter (filterNumber).
// When first invoked, it sets individual bool flags for each filter output, noting that they have not been called yet.
// The first time the filter is called for a given filter-number, it completely computes the power across all
// filter output values. It also keeps track of the zero-th (first) value that is squared and accumulated.
// The next time the this function is called, it only subtracts the previously-stored square of the earliest value and adds
// the square of the latest value. This dramatically improves efficiency and makes this function O(1) when computing
// power across filter outputs of any length.
double filter_computePower(uint16_t filterNumber) {
	static bool allFiltersFirstPass = true;													// Keep track of when this function is called the first time.
	static bool individualFilterFirstPass[FILTER_IIR_FILTER_COUNT];	// Keep track of when power is computed the first time for each filter.
	static double earliestSquareValue[FILTER_IIR_FILTER_COUNT];			// Keep track of the earliest value that is accumulated.
	static double accumulator[FILTER_IIR_FILTER_COUNT];							// Keep the accumulated value for each filter output.
	if (allFiltersFirstPass) {																			// First time for this function, initialize all of the first-pass flags for all filters.
		for (int i=0; i<FILTER_IIR_FILTER_COUNT; i++)									// Need to do this for each filterNumber.
			individualFilterFirstPass[i] = true;												// Now, for each filter, the first-time will be noted.
		allFiltersFirstPass = false;																	// All done, don't do it again.
	}
	// Only process the number of elements that are currently in the queue.
	queue_size_t elementCount = queue_elementCount(&(outputQueue[filterNumber]));	// Find out how many elements you have.
	if (individualFilterFirstPass[filterNumber]) {																// Is this your first pass?
		double earliestValue = queue_readElementAt(&(outputQueue[filterNumber]), 0);// Handle the earliest value specially.
		earliestSquareValue[filterNumber] = earliestValue * earliestValue;					// Square the earliest value and stash it for later.
		accumulator[filterNumber] = earliestSquareValue[filterNumber];							// Accumulate the square of the earliest value.
		for (unsigned int i=1; i<elementCount; i++) {																// Compute the sum of the square of remaining filter output values.
			queue_data_t filterOutput = queue_readElementAt(&(outputQueue[filterNumber]), i);	// Get the output from the correct output queue (filterNumber).
			accumulator[filterNumber] += filterOutput * filterOutput;									// Square the filter output and accumulate it.
		}
		individualFilterFirstPass[filterNumber] = false;	// Finished the first pass, next time, just subtract the earliest square value and add the latest.
	} else {
		// Not the first pass, just subtract square of the earliest filter-value and add the square of the latest value.
		accumulator[filterNumber] -= earliestSquareValue[filterNumber];							// Subtract the last earliest square value from the earlier-computed accumulator.
		double earliestValue = queue_readElementAt(&(outputQueue[filterNumber]), 0);// Handle the earliest value specially.
		earliestSquareValue[filterNumber] = earliestValue * earliestValue;					// Compute the square of the earliest value and stash it for later.
		double latestFilterOutput = queue_readElementAt(&(outputQueue[filterNumber]), elementCount-1);	// Get the latest filter output.
		accumulator[filterNumber] += latestFilterOutput * latestFilterOutput;				// Square the latest filter output and accumulate it.
	}
	currentPowerValue[filterNumber] = accumulator[filterNumber];	// Keep track of the latest computed power number
	return accumulator[filterNumber]; 														// Also return it just for the sake of added utility.
}

// This tests the IIR filter.
// It copies in test coefficients and then checks the computed values.
double test_a_iir_coefficients[IIR_A_COEFFICIENT_COUNT] = {4, 3, 2, 1};
double test_b_iir_coefficients[IIR_B_COEFFICIENT_COUNT] = {0, 1, 2, 3, 4};
double test_finalZQueueResults[Z_QUEUE_SIZE] = {-184, 599, -1933, 6255};
#define TEST_INPUT_COUNT 10
double test_inputValue[TEST_INPUT_COUNT] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
bool filter_runTest(bool debugPrintFlag) {
	filter_init();
	// Setup all of the test coefficients and input values (yQueue).
	loadIirACoefficients(iirACoefficientConstants);
	loadIirBCoefficients(iirBCoefficientConstants);
	zeroZQueues();	// Fill the zQueue with 0s.
	zeroYQueue();	// Fill the yQueue with 0s.
	for (int i=0; i<TEST_INPUT_COUNT; i++) {
		queue_overwritePush(&yQueue, test_inputValue[i]);		// Load a new input.
		if (debugPrintFlag) {
			printf("===================== i=%d =====================\r\n", i);
			printf("yQueue\r\n");
			queue_print(&yQueue);
			printf("zQueue before running filter\r\n");
			queue_print(&(zQueue[0]));
		}
		filter_iirFilter(0);	// Run the filter.
		if (debugPrintFlag) {
			printf("zQueue after running filter\r\n");
			queue_print(&(zQueue[0]));
		}
	}
	bool success = true;	// Be optimistic.
	for (int i=0; i<Z_QUEUE_SIZE; i++) {
		if (test_finalZQueueResults[i] != queue_readElementAt(&(zQueue[0]), i)) {
			success = false;	// When results don't match, fail.
			break;						// No need to continue, just quit.
		}
	}
	if (debugPrintFlag) {
		if (!success)
			printf("filter_runTest() failed.\r\n");
		else
			printf("filter_runTest() passed.\r\n");
	}
	return success;
}







