/* lslfilter.c++	recursive Least-Squares Lattice (LSL) scalar case
			for exponentially weighted data

		See Algorithm 5.3 of Cowan & Grant, Adaptive Filters
		p 115
*/

static const char rcsid[] = "@(#)lslfilter.c++	1.1 16:19:06 3/31/94   EFC";

#include <lslfilter.hpp>

#define min(a,b)		((a) > (b) ? (b) : (a) )

void LSLFilter::free_space()
{
	if ( p != 0)
	{
		delete []delta;
		delete []gamma;
		delete []b;
		delete []sig_f;
		delete []sig_b;
		delete []b_old;
		delete []sig_b_old;
	}

}

int LSLFilter::order(const int ord)
{
	if ( ord <= 0 )			// query current size only
		return p;

	if ( p != ord )
		free_space();

	p = ord;

	delta = new float[p];
	gamma = new float[p];
	b     = new float[p];
	sig_f = new float[p];
	sig_b = new float[p];
	b_old = new float[p];
	sig_b_old = new float[p];

	if ( delta == NULL || gamma == NULL || b == NULL || sig_f == NULL ||
	     sig_b == NULL || b_old == NULL || sig_b_old == NULL )
			err = -1;
	else
			err = 0;

	invals = 0;

	return p;

}

float LSLFilter::filter(const float x)
{
	float f;

	f = b[0] = x;

	if ( invals++ < 1 )
	{
		sig_f[0] = sig_b[0] = x * x;

		if ( _output_error )
			return out_val = x - f;
		else
			return out_val = f;
	}

	const int t = 1;
	int i, ip, iend;
	float kf, kb, old_gamma, old_f;
	 	
	sig_f[0] = sig_b[0] = lambda * sig_f[0] + x * x;

	iend = min(t,p) - 1;
	for (i = 0, ip = 1; i < iend; i++, ip++)
		{
			delta[ip] = lambda * delta[ip]
					+ b_old[i] * f / (1 - gamma[i]);
			old_gamma = gamma[ip];
			gamma[ip] = gamma[i] + b[i] * b[i] / sig_b[i];
			kf = delta[ip] / sig_f[i];
			kb = delta[ip] / sig_b_old[i];
			old_f = f;
			f = old_f - kb * b_old[i];
			b[ip] = b_old[i] - kf * old_f;
			if (t < p)
			{
				sig_f[ip] = sig_f[i] - kb * delta[ip];
				sig_b[ip] = sig_b_old[i] - kf * delta[ip];
			}
			else
			{
				sig_f[ip] = lambda * sig_f[ip]
					+ f * f /(1 - old_gamma);
				sig_b[ip] = lambda * sig_b_old[ip]
					+ b[ip] * b[ip] /(1 - gamma[ip]);
			}
		}

		// swap b/sig_b with the old ones
		float *tmp;

		tmp = b_old;
		b_old = b;
		b = tmp;

		tmp = sig_b_old;
		sig_b_old = sig_b;
		sig_b = tmp;


		if ( _output_error )
			return out_val = f - x;
		else
			return out_val = f;


}