/* Least Mean Square (LMS) adaptive FIR filter

Copyright (C) 2001-2002 Andrew Rogers

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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

#include "lms.h"

LMS::LMS(int order, double mu, double *coeffs=NULL, double *init=NULL)
{
  	h=(double *)malloc(sizeof(double)*(order+1));	//There are order+1 number of coeffs
        delay=(double *)malloc(sizeof(double)*(order+1));
        n=order;
        if(coeffs!=NULL) for(int i=0; i<=n; i++) h[i]=coeffs[i];
        else for(int i=0; i<=n; i++) h[i]=0;
        if(init!=NULL) for(int i=0; i<=n; i++) delay[i]=init[i];
        mU=mu;
}

double LMS::sample(double x, double e)
{
         //Update the coeffs according to LMS algorithm
        double d=mU*e;  	//Change in coeffs
        for(int i=0; i<=n; i++) h[i]=h[i]+d*delay[i];

	// Shift new sample (x) into the delay line
        for(int i=n; i>=1; i--) delay[i]=delay[i-1];
        delay[0]=x;

        //Calculate filter output
        double y=0;
        for(int i=0; i<=n; i++) y+=h[i]*delay[i];

	return y;
}

double * LMS::getCoeffs(double *coeffs)
{
	for(int i=0; i<=n; i++) coeffs[i]=h[i];
        return coeffs;
}

LMS::~LMS()
{
	free(h); free(delay);
}