/*
    This file is part of EqualizerAPO, a system-wide equalizer.
    Copyright (C) 2014  Jonas Thedering

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#include "stdafx.h"
#include "helpers/MemoryHelper.h"
#include "IIRFilter.h"

using namespace std;

#define IS_DENORMAL(d) (abs(d) < DBL_MIN)

IIRFilter::IIRFilter(const vector<double>& coefficients)
{
	order = (unsigned)coefficients.size() / 2 - 1;
	a = (double*)MemoryHelper::alloc(order * sizeof(double));
	b = (double*)MemoryHelper::alloc(order * sizeof(double));
	x = NULL;
	y = NULL;

	double a0 = coefficients[order + 1];
	b0 = coefficients[0] / a0;
	for (unsigned i = 0; i < order; i++)
	{
		b[i] = coefficients[i + 1] / a0;
		a[i] = -coefficients[i + order + 2] / a0;
	}
}

IIRFilter::~IIRFilter()
{
	MemoryHelper::free(a);
	MemoryHelper::free(b);

	if (x != NULL)
		MemoryHelper::free(x);
	if (y != NULL)
		MemoryHelper::free(y);
}

vector<wstring> IIRFilter::initialize(float sampleRate, unsigned maxFrameCount, vector<wstring> channelNames)
{
	channelCount = (unsigned)channelNames.size();

	if (x != NULL)
		MemoryHelper::free(x);
	if (y != NULL)
		MemoryHelper::free(y);

	x = (double*)MemoryHelper::alloc(order * channelCount * sizeof(double));
	y = (double*)MemoryHelper::alloc(order * channelCount * sizeof(double));
	memset(x, 0, order * channelCount * sizeof(double));
	memset(y, 0, order * channelCount * sizeof(double));

	return channelNames;
}

#pragma AVRT_CODE_BEGIN
void IIRFilter::process(float** output, float** input, unsigned frameCount)
{
	for (unsigned i = 0; i < channelCount; i++)
	{
		float* inputChannel = input[i];
		float* outputChannel = output[i];

		unsigned channelOffset = i * order;
		double* xo = x + channelOffset;
		double* yo = y + channelOffset;
		for (unsigned j = 0; j < frameCount; j++)
		{
			double sample = inputChannel[j];
			double sum = b0 * sample;

			for (unsigned k = order - 1; k > 0; k--)
			{
				sum += b[k] * xo[k];
				xo[k] = xo[k - 1];
			}

			sum += b[0] * xo[0];

			for (unsigned k = order - 1; k > 0; k--)
			{
				sum += a[k] * yo[k];
				yo[k] = yo[k - 1];
			}

			sum += a[0] * yo[0];

			xo[0] = sample;
			yo[0] = sum;

			outputChannel[j] = (float)sum;
		}
	}

	for (unsigned i = 0; i < channelCount * order; i++)
	{
		if (IS_DENORMAL(x[i]))
			x[i] = 0.0;
		if (IS_DENORMAL(y[i]))
			y[i] = 0.0;
	}
}
#pragma AVRT_CODE_END