/* Least Mean Square (LMS) 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 "lms.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 LMS_FILTER_ORDER 2
#define LMS_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[LMS_FILTER_ORDER+1];
    double e[LENGTH];       //Trace of error
    double h[NS_FILTER_ORDER+1];
    double ht[LMS_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("LMS");
    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[LMS_FILTER_ORDER+1]=LMS_FILTER_COEFFS;    //Initial coeffs for LMS

    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
    LMS ad_filter(LMS_FILTER_ORDER,0.9,hi);     //LMS 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<=LMS_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("green"));  //Plot the signal after shaping filter
    signalplot->drawFromFloat(LENGTH,n,e,2,QColor("red"));     //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("red"));
    coeffplot->drawFromFloat(LENGTH,n,ht[1],1,QColor("blue"));
    coeffplot->drawFromFloat(LENGTH,n,ht[2],2,QColor("green"));
    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));
}