#include "FIR_Filter.hpp"
#include <iostream>

FIR_Filter::FIR_Filter() : isInitialized_(false), dataBuff_(), lastFilteredValue_(0.0)
{
}

void FIR_Filter::init(const double initialValue)
{
	for(size_t i = 0; i < FILTER_LENGTH; i++)
	{
		dataBuff_[i] = initialValue;
	}
	lastFilteredValue_ = initialValue;

	isInitialized_ = true;
}

double FIR_Filter::update(const double newValue, const double* coeffs, const size_t num_coeffs)
{
	if(!isInitialized_)
	{
		init(0);
	}

	if(FILTER_LENGTH >= num_coeffs)
	{
		// shift all values down by one
		size_t n = 0;
		for(n = (FILTER_LENGTH - 1); n > 0; n--)
		{
			dataBuff_[n] = dataBuff_[n - 1];
		}

		// Insert most recent value
		dataBuff_[0] = newValue;

		// Apply the filter
		double accum = 0.0;
		for(size_t i = 0; i < num_coeffs; i++)
		{
			accum += dataBuff_[i] * coeffs[i];
		}

		// Save
		lastFilteredValue_ = accum;
	}

	return lastFilteredValue_;
}

double FIR_Filter::getLastFilteredValue() const
{
	return lastFilteredValue_;
}