/************************************************************************/
/*  WFLC0.C         Weighted-frequency Fourier Linear Combiner 0.0      */
/*  Cameron N. Riviere, Carnegie Mellon University                      */
/*  last mod.    11/5/98                                                */
/*  This is WF1H, modified to include harmonics.                        */
/*  Adaptive weighted-frequency algorithm for noise canceling in 1-D data.*/
/*  Here, frequency of reference, as well as ampl., is an adaptive weight.*/
/*  Mu is the standard adapt. rate param.  mu0 is a 2nd param. on the   */
/*      frequency weight.  A bias (highpass) weight is used.            */
/*  Ampl. weights for harmonics will be initialized to 0.               */
/*  To run this program, type at the DOS prompt:                        */
/*  WFLC0 filename ext mu mu0 mub M w0 w1 wM+1 offset                    */
/*  Note:  This code is written in Turbo C for the PC.                  */
/************************************************************************/
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <conio.h>
#include <string.h>
void calc_output(void);

FILE *fp1; /*description file*/
FILE *fp2; /*input file*/
FILE *fp3; /*output file*/
FILE *fp4; /*time history of weights*/
FILE *fp5; /*" " " harmonic weights*/
double input,datum; /* data,reference,ref.initializn.index*/
char inls[32],ext[32],desfile[32],infile[32],outfile[32];
int i,M; /*i=counter, M=filter order*/
long k=1; /*time index*/
double mu,mu0,mub;/*adapt. rates for ampl., freq., bias weight respectively */
double output;   /*filter output*/
double e;        /*filter error*/
double w[60],x[60],w0,wbias=0; /*ampl.wts., ref.vector, freq.wt., bias wt. */
double offset=0; /* constant offset to be subtracted before processing */

main(int argc, char *argv[])
{
  if(argc!=11) /*help utility*/
    {
      clrscr();
      printf("\n\n\n\n");
      printf("\t               Carnegie Mellon University\n");
      printf("\tCenter for Medical Robotics and Computer Assisted Surgery\n\n");
      printf("\t\t\t\t WFLC\n");
      printf("\t\tWeighted-frequency Fourier Linear Combiner 0.0\n\n");
      printf("\t\tVariable-frequency quasi-periodic noise canceler for 1-D data.\n");
      printf("\t\tIncludes a high pass bias weight.\n\n");
      printf("To run this program, type: \n");
      printf(" WFLC  filename  ext  mu  mu0  mub  M  w0  w1  wM+1  offset\n\n");
      printf("mu is the amplitude adaptation rate.\n");
      printf("mu0 is the frequency adaptation rate.\n");
      printf("mub is the bias (highpass) weight adaptation rate.\n");
      printf("w0 is the initial value of the fundamental frequency weight.\n");
      printf("w1 and wM+1 are initial values of the 1st sin & cos amplitudes.\n
      printf("offset is any constant you wish to subtract before processing.\n\n");
      exit(1);
    }
/* assign parameters */
  strcpy(inls,argv[1]);
  strcpy(ext,argv[2]);
  mu = atof(argv[3]);
  mu0 = atof(argv[4]);
  mub = atof(argv[5]);
  M = atoi(argv[6]);
  for(i=1; i<=2*M; i++) w[i]=0; /* initialize w's */
  w0 = atof(argv[7]);
  w[1] = atof(argv[8]);
  w[M+1] = atof(argv[9]);
  offset = atof(argv[10]);
  clrscr();
/* create description file */
  strcpy(desfile,inls);
  strcat(desfile,ext);
  strcat(desfile,".wfe");
  fp1 = fopen(desfile,"w");
  fprintf(fp1," created by:    WFLC\n");
  fprintf(fp1,"%s.%s %lf %lf %lf %d  %f %f %f %f\n",inls,ext,mu,mu0,mub,M,w0,w[1],w[M+1],offset);
  fclose(fp1);
/* input data filename */
  strcpy(infile,inls);
  strcat(infile,".");
  strcat(infile,ext);
/* create output filename */  
  strcpy(outfile,inls);
  strcat(outfile,ext);
  strcat(outfile,".wfc");
  calc_output();
  fcloseall();
  return 0; 
}

void calc_output(void)
{
  fp2 = fopen(infile,"r");
  if(fp2==NULL)
     {
      printf("cannot open file %s\n",infile);
      exit(1);
     }
  fp3=fopen(outfile,"w");
  if(fp3 ==NULL)
    {
      printf("Cannot open file %s.\n",outfile);
      exit(1);
    }        /*file-opening check*/
  fp4=fopen("weights.wfc","w");
  if(fp4 ==NULL)
    {
      printf("Cannot open file weights.wfc.\n");
      exit(1);
    }        /*file-opening check*/
  fp5=fopen("hweights.wfc","w");
  if(fp5 ==NULL)
    {
      printf("Cannot open file hweights.wfc.\n");
      exit(1);
    }        /*file-opening check*/
/*initialize*/
   sumw0 = 0;
   while(fscanf(fp2,"%lf",&datum)!=EOF)
   {
    input = datum - offset; /* remove offset */
/* locate next sine & cosine samples */
    sumw0 += w0;
    for(i=1; i<=M; i++)
    {
     x[i] = sin(i*sumw0);
     x[M+i] = cos(i*sumw0);
    }
/* output = truncated Fourier series */
    output = 0;
    for(i=1; i<=2*M; i++) output += w[i] * x[i];
/* calculate error */
    e = input - output - wbias;
/* update bias weight */
    wbias += 2*mub*e;
/* update frequency weight, 'blind' to harmonics */
    sumcross = 0;
    for(i=1; i<M; i++) sumcross += i*(w[M+i]*x[i]-w[i]*x[M+i]);
    w0 -= 2*mu0*e*sumcross;
/* update amplitude weights */
    for(i=1; i<=2*M; i++)  w[i] += 2*mu*e*x[i];
/*output*/    
    fprintf(fp3,"%lf\n",e+offset); /* output error */
    fprintf(fp4,"%f  %f  %f  %f\n",w0,w[1],w[M+1],wbias+offset);/*output weights*/
    for(i=2;i<=M; i++) fprintf(fp5,"%f ",w[i]);/*harmonic weights*/
    for(i=M+2;i<=2*M;i++) fprintf(fp5,"%f ",w[i]);/*harmonic weights*/
    fprintf(fp5,"\n");
   }
}