/* Recursive Least Square (RLS) 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 "rls.h"

RLS::RLS(int order, double f, double *coeffs=NULL, double *init=NULL)
{
        int sz=sizeof(double)*(order+1);
        int sz2=sizeof(double)*(order+1)*(order+1);
        W=(double *)malloc(sz);	//There are order+1 number of coeffs
        X=(double *)malloc(sz);
        G=(double *)malloc(sz);
        P=(double *)malloc(sz2);
        PX=(double *)malloc(sz);
        n=order;
        if(coeffs!=NULL) for(int i=0; i<=n; i++) W[i]=coeffs[i];
        else for(int i=0; i<=n; i++) W[i]=0.0;
        if(init!=NULL) for(int i=0; i<=n; i++) X[i]=init[i];
        ff=f;
        for(int i=0; i<=n; i++)for(int j=0; j<=n; j++) P[i*(n+1)+j]=0.0;
        for(int i=0; i<=n; i++) P[i*(n+1)+i]=1.0;
}

double RLS::sample(double x, double e)
{
        //Iteratively calculate the inverse autocorrelation matrix P
        for(int row=0; row<=n; row++)
        {
        	PX[row]=0;
                for(int i=0; i<=n; i++)PX[row]+=P[row*(n+1)+i]*X[i];
        };

        double den=ff;
        for(int row=0; row<=n; row++)den+=PX[row]*X[row];

        for(int row=0; row<=n; row++)G[row]=PX[row]/den;

        for(int col=0;col<=n;col++)for(int row=0;row<=n;row++)P[row*(n+1)+col]=(P[row*(n+1)+col]-G[row]*PX[col])/ff;

        //Update filter coefficients
        for(int i=0;i<=n;i++)W[i]=W[i]+G[i]*e;

        //Do the actual filter bit
        double output=0;
        for(int i=n; i>0; i--)X[i]=X[i-1];
        X[0]=x;
        for(int i=0;i<=n;i++)output+=W[i]*X[i];

        return output;
}

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

RLS::~RLS()
{
        //free(W);
        free(X);
        free(G);
        free(P);
        free(PX);
}