/* Recursive Least Square (RLS) adaptive FIR filter demo

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 <qapplication.h>
#include <qmainwindow.h>
#include "mplot.h"
#include "fir.h"
#include "rls.h"
#include <math.h>
#include <stdio.h>

#define LENGTH 50

#define NS_FILTER_ORDER 2

#define REF_FILTER_ORDER 2
#define REF_FILTER_COEFFS {0.7,0.2,0.3}

#define RLS_FILTER_ORDER 2
#define RLS_FILTER_COEFFS {0.9,0.9,0.9}


double frand();


int main(int argc, char **argv)
{
    double n[LENGTH];
    double y1[LENGTH];		//Filtered noise
    double r[LENGTH];		//Output of reference filter
    double y3[LENGTH];		//Output of adaptive filter
    double ha[RLS_FILTER_ORDER+1];
    double e[LENGTH];          //Trace of error
    double h[NS_FILTER_ORDER+1];
    double ht[RLS_FILTER_ORDER+1][LENGTH];	//Trace of coefficients

    double hr[REF_FILTER_ORDER+1]=REF_FILTER_COEFFS; 	//The coeffs for the reference filter, just some sensibly random values

    double M=3.2;

    QApplication app(argc,argv);      //Do all the GUI stuff
    QMainWindow *mainwin=new QMainWindow;
    mainwin->setCaption("RLS");
    mainwin->setGeometry(100,100,200,100);
    app.setMainWidget(mainwin);
    mainwin->show();

    double pi=acos((double)-1);

    printf("pi=%.30lf\n",pi);

    for(int i=0; i<=NS_FILTER_ORDER; i++) h[i]=0.5*(1+cos(2*pi*(i-NS_FILTER_ORDER/2)/M));

    double hi[RLS_FILTER_ORDER+1]=RLS_FILTER_COEFFS;		//Initial coeffs for RLS

    FIR ref_filter(REF_FILTER_ORDER,hr,hi); 		//Reference filter that we try to match
    FIR ns_filter(NS_FILTER_ORDER,h);            	//Noise shaping filter
    RLS ad_filter(RLS_FILTER_ORDER,0.99,hi); 	//RLS adaptive filter

    double error=0;

    for(int i=0;i<LENGTH;i++)
    {
        n[i]=i;

        y1[i]=ns_filter.sample(frand());
        r[i]=ref_filter.sample(y1[i]);
        y3[i]=ad_filter.sample(y1[i],error);
        error=r[i]-y3[i];

        ad_filter.getCoeffs(ha);
        for(int j=0;j<=RLS_FILTER_ORDER;j++)ht[j][i]=ha[j];
        e[i]=error;
    }

    MPlot *signalplot=new MPlot();
    signalplot->setCaption("Shaped noise & error signals plot");
    signalplot->setRangeFromFloat(0,-1.5,(float)LENGTH,1.5);
    signalplot->drawFromFloat(LENGTH,n,y1,0,QColor(0,255,0));  //Plot the signal after shaping filter
    signalplot->drawFromFloat(LENGTH,n,e,1,QColor(255,0,0));     //Plot the error r(n)-x(n)
    signalplot->setAxis(10,10);
    signalplot->show();

    MPlot *coeffplot=new MPlot();
    coeffplot->setCaption("Coefficient plot");
    coeffplot->setRangeFromFloat(0.0,-0.2,(float)LENGTH,1.0);
    coeffplot->drawFromFloat(LENGTH,n,ht[0],0,QColor(255,0,0));
    coeffplot->drawFromFloat(LENGTH,n,ht[1],1,QColor(0,0,255));
    coeffplot->drawFromFloat(LENGTH,n,ht[2],2,QColor(0,255,0));
    coeffplot->setAxis(10,12);
    coeffplot->show();

    int retval=app.exec();
    delete coeffplot;
    delete signalplot;
    return retval;
}

double frand()
{
    return(rand()-(1<<30))/((double)(1<<15)*(double)(1<<15));
}