/**************************************************
   Written By:
   ERIK S. WHEELER

   Echo Cancellation using LINKED LIST
   Presented:
     Echo Cancellation Group
     DSP Project Presentation Conference
     Mississippi State University
     12 / 4 / 95
     EE 4733/6733 -- Digital Signal Processing
     Instructor -- Dr. Joseph Picone
   Group Members:
     Vince J. Allen (allen@isip.MsState.Edu)
     Mark E. Henderson (henders@isip.MsState.Edu)
     Erik S. Wheeler (wheeler@isip.MsState.Edu)
     John L. Williams (SPwillia@EE.MsState.Edu)
***************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define FALSE 0
#define MAXLENGTH 32000
#define LMS 1
#define NTAPS 20           /* Number of Taps */
#define ALPHA 0.0078125     /* 2^(-7)  */
#define BETA1 0.00097656       /* << 2^(-8)  (sug.=2^(-10) */
#define INIT_LY 0.0
#define CUTOFF 2.0
#define DELAY 0
#define PCI 499
#define SIGNAL "xfile.dat"
#define IMPULSE "hfile.dat"
#define OUTFILE "yfile.dat"

typedef float datatype;

typedef struct listelement_type {
  datatype dataitem;
  struct listelement_type *next;
  struct listelement_type *prev;
} listelement;

void Usage(char *argv[]) {
  printf("\nUsage: %s SIGNAL ECHO OUTPUT ERROR FILTER SNR [flags]\n", argv[0]);
  printf("****************************************************\n");
  printf("SIGNAL = Binary file containing input signal\n");
  printf("ECHO   = Binary file containing echo response of input signal\n");
  printf("OUTPUT = Binary file containing cancelled echo\n");
  printf("ERROR  = Binary file containing percent error\n");
  printf("FILTER = Binary file containing filter coefficiencts\n");
  printf("SNR    = Ascii file containing SNR calc\n");
  printf("[-aALPHA] = (default = %f) \n",ALPHA);
  printf("[-bBETA1] = Step Size (default = %f) \n",BETA1);
  printf("[-cCUTOFF] = Minimum signal value (default = %f)\n",CUTOFF);
  printf("[-dDELAY] = delay of echo (default = %d)\n",DELAY);
  printf("[-lLY] = Initial Ly (default = %f) \n",INIT_LY);
  printf("[-mMAXLENGTH] = Maximum File Length (default = %d) \n",MAXLENGTH);
  printf("[-nNTAPS] = Number of Filter Taps (default = %d) \n",NTAPS);
  printf("[-pPCI] = Print Coefficient Interval (default = %d)\n",PCI); 
  printf("Brackets [] denote optional\n");
  printf("***ALL FILES*** are SHORT INT (16 bit) (max value = %d)\n\n",10000);
  exit(1);
}

/*  Creates a Circular List and returns a pointer to this list */
listelement_type * CreateCircularList (int length, float initval) {
  listelement_type * listptr=NULL;
  listelement_type * tempptr1=NULL;
  listelement_type * tempptr2=NULL;
  int lcv=0;
  
  if ((listptr = (struct listelement_type *) malloc (sizeof (listelement_type))) == NULL) {
    printf("\nMemory Allocation Error -- Execution Aborted\n");
    exit(1);
  }
  listptr -> next = NULL;
  listptr -> prev = NULL;
  listptr -> dataitem = initval;
  tempptr1 = listptr;

  for (lcv=1;lcv<length;lcv++) {
    if ((tempptr2 = (struct listelement_type *) malloc (sizeof (listelement_type))) == NULL) {
      printf("\nMemory Allocation Error -- Execution Aborted\n");
      exit(1);
    }
    tempptr2 -> next = NULL;
    tempptr2 -> prev = tempptr1;
    tempptr2 -> dataitem = initval;
    tempptr1 -> next = tempptr2;
    tempptr1 = tempptr2;
  }
  tempptr1 -> next = listptr;
  listptr -> prev = tempptr1;
  return listptr;
}

/*  Generic Open File function with error checking */
FILE * OpenFile(char * filename, char * mode) {
  FILE *fp;
  if ((fp=fopen(filename,mode))==NULL) {
    printf("Cannot open file %s\n",filename);
    exit(1);
  }
  return fp;
}

/*  Binary File Element To List with error checking */
void BinFileElementToList(FILE *fp, listelement_type  * listptr) {
  short int hd_data=0;

  if (fread(&hd_data, sizeof(short int), 1, fp) != 1) {
    printf("Read Error");
    fclose(fp);
    exit(1);
  }
  listptr -> dataitem = (datatype) hd_data;
}

/* Binary File Element to return value */
datatype BinFileElementToVar (FILE *fp) {
  short int hd_data=0;
  
  if (fread(&hd_data, sizeof(short int), 1, fp) != 1) {
    printf("Read Error");
    fclose(fp);
    exit(1);
  }
  return (datatype) hd_data;
}

/*  Used only for incrementally printing coefficient data to binary file */
void PrintQueueToFile (listelement_type * listptr, FILE *fp) {
  listelement_type * startptr=listptr;
  short int hd_data;
  
  if (listptr == NULL)
    printf ("Queue is empty!\n");
  else {
    hd_data = (short int)(1000*listptr->dataitem);
    if (fwrite(&hd_data, sizeof(short int), 1, fp) != 1) {
      printf ("Read Error in Writing COEFF\n");
      exit(1);
    }
    while (listptr -> next != startptr) {
      listptr = listptr->next;
      hd_data = (short int)(1000*listptr->dataitem);
      if (fwrite(&hd_data, sizeof(short int), 1, fp) != 1) {
	printf ("Read Error in Writing COEFF\n");
	exit(1);
      }
    }
  }
}

/* Prints Entire circular list to screen (used mostly during debugging) */
void PrintQueue (listelement_type * listptr) {
  listelement_type * startptr=listptr;

  if (listptr == NULL)
    printf ("Queue is empty!\n");
  else {
    printf("%f\t", listptr->dataitem);
    while (listptr -> next != startptr) {
      listptr = listptr -> next;
      printf ("%f\t", listptr -> dataitem);
    }
  }
  printf ("\n");
}

/* Returns the absolute value of a floating point number */
/* I have no idea why c doesn't provide a function like this, */
/* or if they do I could not find it. */
float Fabs (float x) {
  return sqrt(x*x);
}

/* Performs Multiplicatioon and Summing for Convolution Routine */
datatype MultItem (listelement_type *x,listelement_type *xs,
                   listelement_type *h,listelement_type *hs) {
  datatype returndata=0;
  while (h->next!=hs) {
    returndata += x->dataitem * h->dataitem;
    x=x->prev;
    h=h->next;
  } 
  returndata += x->dataitem * h->dataitem;
  return returndata;
}

/* PARTIAL Convolution Routine -- performs convolution only on     */
/* completely overlapped lists.  Not Used in Final Version because */
/* only current time convolution output is required for compution, */
/* and MultItem function can handle that.                          */
void Conv(listelement_type *xptr, listelement_type *hptr, 
          listelement_type *yptr, listelement_type *eptr) {
  listelement_type * startptrx = xptr;
  listelement_type * startptrh = hptr;
  yptr->dataitem = MultItem (xptr, startptrx, hptr, startptrh);
  yptr=yptr->next;
  xptr=xptr->next;
  while (xptr != eptr) {
    yptr->dataitem = MultItem (xptr, startptrx, hptr, startptrh); 
    xptr=xptr->next;
    yptr=yptr->next;
  }
  yptr->dataitem = MultItem (xptr, startptrx, hptr, startptrh);
}

/* Main Echo Cancellation Code */
int main (int argc, char *argv[]) {
  FILE *sigfp, *echofp, *outfp, *errfp, *filtfp, *snrfp;
  char ch1, ch2;
  short int hd_data, prcterr;
  int i, k, count1=0, count2=0, fileflag=0;
  int maxlength = MAXLENGTH;
  int pci      = PCI;
  int ntaps    = NTAPS;
  int delay    = DELAY;
  datatype ly  = INIT_LY;
  float alpha  = ALPHA;
  float beta1  = BETA1;
  float cutoff = CUTOFF;
  float twobeta=0.0, sumofsqrs=0.0, sigavgpow=0.0;
  float est=0.0, echo=0.0, error=0.0, le=0.0, snr=0.0;
  listelement_type * sigsp;    /* signal start pointer        */
  listelement_type * filtsp;   /* filter start piinter        */
  listelement_type * sigtp;    /* signal current time pointer */
  listelement_type * tsigtp;   /* temp signal time pointer    */
  listelement_type * tfilttp;  /* temp filter time pointer    */


  char *signalfile = "tempsig.dat";   /*  Used to avoid compiler warnings */
  char *echofile = "tempecho.dat";
  char *outputfile = "tempout.dat";
  char *errorfile = "temperror.dat";
  char *filtfile = "tempfilt.dat";
  char *snrfile = "tempsnr.dat";

/*  EVALUATE COMMAND LINE ARGUMENTS */
/*  NO SPACES BETWEEN FLAG AND VALUE, AND FILES MUST BE SPECIFIED */
/*  Please note:  Files must be input in order, but flags may     */
/*  be input at any time (before, during, or after file inputs).  */
/*  What do you think, pretty cool huh?                           */
  for (i=1;i<argc;i++) {
    switch (ch1 = argv[i][0]) {
    case '-':
      switch (ch2 = argv[i][1]) {
      case 'm':
	printf("max file length -- %d\n",(maxlength = atoi(argv[i] + 2)));
	break;
      case 'n':
	printf("number of filter taps -- %d\n",(ntaps = atoi(argv[i] + 2)));
	break;
      case 'a':
	printf("alpha -- %f\n",(alpha = atof(argv[i]+2)));
	break;
      case 'b':
	printf("step size (beta1) -- %f\n",(beta1 = atof(argv[i]+2)));
	break;
      case 'd':
	printf("delay -- %d\n",(delay = atoi(argv[i]+2)));
	break;
      case 'l':
	printf("initial ly -- %f\n",(ly = atof(argv[i]+2)));
	break;
      case 'c':
	printf("cutoff -- %f\n",(cutoff = atof(argv[i]+2)));
	break;
      case 'p':
	printf("print coefficient interval -- %d\n",(pci = atoi(argv[i]+2)));
	break;
      default:
	printf("ERROR in SWITCH\n");
	Usage(argv);
      }
      break;
    default:
      switch (++fileflag) {
      case 1:
	printf("SIGNAL -- %s\n",(signalfile = argv[i]));
	break;
      case 2:
	printf("ECHO   -- %s\n",(echofile = argv[i]));
	break;
      case 3:
	printf("OUTPUT -- %s\n",(outputfile = argv[i]));
	break;
      case 4:
	printf("PERCENT ERROR -- %s\n",(errorfile = argv[i]));
	break;
      case 5:
	printf("FILTER COEFF. -- %s\n",(filtfile = argv[i]));
	break;
      case 6:
	printf("SNR -- %s\n",(snrfile = argv[i]));
	break;
      default:
	printf("TOO MANY FILES !\n");
	Usage(argv);
      }
    }
  }
  if (fileflag != 6) {
    printf("ILLEGAL NUMBER OF FILES\n");
    Usage(argv);
  }

/* Create Circular Lists and initialize pointers                */
/* signal list is delay+ntaps long. Since M=1 the only          */
/* variables that must be maintained for previous values        */
/* are the signal and the filter                                */
  sigsp   = CreateCircularList(ntaps+delay,0.0);
  filtsp  = CreateCircularList(ntaps,0.0);
  sigtp   = sigsp;

  sigfp  = OpenFile(signalfile,"rb");
  echofp = OpenFile(echofile,"rb");
  outfp  = OpenFile(outputfile,"wb");
  errfp  = OpenFile(errorfile,"wb");
  filtfp = OpenFile(filtfile,"wb");
  snrfp  = OpenFile(snrfile,"w"); 

  sumofsqrs = 0;

/* Read in First Chunk of Data                     */
/* This process is done in steps in order to point */
/* sigsp to proper memory location.                */
  for (i=0;i<ntaps;i++) {
    BinFileElementToList(sigfp, sigtp);
    sumofsqrs += sigtp->dataitem * sigtp->dataitem;
    sigtp = sigtp->next;
    echo = BinFileElementToVar(echofp);
  }
  sigsp=sigtp->prev;

/* Read in Second Chunk of Data                     */
/* Pushes First ntaps values of Echo data off queue */
/* and fills signal queue.                          */
  for (i=ntaps;i<delay+ntaps-1;i++) {
    BinFileElementToList(sigfp, sigtp);
    sumofsqrs += sigtp->dataitem * sigtp->dataitem;
    sigtp = sigtp->next;
    echo = BinFileElementToVar(echofp);
  }

/* Initialize counters for file writes */
  count1=delay+ntaps-1;
  count2=delay+ntaps-1;

/* Start Echo Cancellation Algorithm */
  for (i=delay+ntaps-1;i<maxlength;i++) {
    BinFileElementToList(sigfp, sigtp);

/* Efficient Calculation of Long Term (2*ntaps-1) Average Signal Power */
    sumofsqrs += (sigtp->dataitem * sigtp->dataitem) - 
      (sigtp->next->dataitem * sigtp->next->dataitem);
    sigavgpow = sumofsqrs / (ntaps+delay);
    echo = BinFileElementToVar(echofp);

/* Calculation of Echo Estimate */
    est = MultItem(sigsp,sigsp,filtsp,filtsp);

/* Write Output (echo[i] - estimate[i]) */
    if (Fabs(sigtp->dataitem)<cutoff) hd_data=0;
    else  hd_data = (short int)(echo - est);
    if (fwrite(&hd_data, sizeof(short int), 1, outfp) != 1) {
      printf ("Read Error in %s\n",outputfile);
      exit(1);
    }

/* Calculation of Error  for file output and coeff. update */
    error = echo - est - sigtp->dataitem;
    prcterr = (short int) error;
    if (fwrite(&prcterr, sizeof(short int), 1, errfp) != 1) {
      printf ("Read Error in %s\n",errorfile);
      exit(1);
    }

/* Prints Filter Coefficients to file at interval pci */
    if (count2++>=pci) {
      PrintQueueToFile(filtsp,filtfp);
      printf ("ERROR %8.3f\ttime = %d\tsig = %8.3f",error,i,sigtp->dataitem);
      printf ("\techo = %8.3f\test = %8.3f",echo,est);
      printf ("\toutput = %d\tPOW = %f\tLy = %f\n",hd_data,sigavgpow,ly);
      count2=0;
    }

/* Approximation of sqrt of power (used only for snr purposes */
    ly = (1 - alpha)*ly + (alpha * Fabs(sigtp->dataitem)) + alpha*cutoff; 
    twobeta = (beta1 / (sigavgpow));
    le = (1 - alpha)*le + (alpha * Fabs(error)) + alpha*cutoff;
    snr = 10 * log ((ly*ly)/(le*le));
    fprintf(snrfp,"%d\t%f\n",i,snr);

/* Update Coefficient using Least Mean Square Algorithm */
    tsigtp = sigtp->next;
    tfilttp = filtsp->prev;
    for (k=0;k<ntaps;k++) {
      tfilttp->dataitem += twobeta * error * tsigtp->dataitem;
      tsigtp = tsigtp -> next;
      tfilttp = tfilttp -> prev;
    }
    if (count1++>=pci) {
      printf("\nFILTER COEFFICIENTS  time = %d\n",i);
      PrintQueue(filtsp);
      count1=0;
    }

    sigsp = sigsp->next;
    sigtp = sigtp->next;
  }

/* House Keeping */
/* Memory locations have not been deallocated because Unix */
/* does this for you.  If this is to run on a PC, changes  */
/* may need to be made. */
  PrintQueueToFile(filtsp,filtfp);
  fclose(filtfp);
  fclose(sigfp);
  fclose(echofp);
  fclose(outfp);
  fclose(errfp);
}