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

#include "mulaw.h"

#define LC        5
#define LW       10
#define MT 10000000
#define HLEN   1024

unsigned char buff[MT]; 

static double marcoef(double *xin, int len, double *cret, int m);
static double invfl(double *x, int np, double *cret, int m);
static double residual(double *xin, int len, double *poly, int m);

int main(int argc, char **argv)
{
  int kt, nt, cc, kstart, ichan, k;
  double samp1, samp2, samp2x, samp3, d1, d2;
  double vec[2*LW+1], coef[LC+1], err1, err2;
  int mask = 0377;

  /*** read the whole file of bytes ***/
  for (nt=0; nt<MT; nt++){
    cc=getc(stdin);
    if(cc==EOF) break;
    buff[nt]=cc;
  }
  if(nt==MT)
    fprintf(stderr,"WARNING, file buffer full, output may be
truncated\n"); 
  fprintf(stderr,"bytes in: %d\n",nt); 

  /*** loop on channels in the file ***/
  for (ichan=0; ichan<2; ichan++){
    fprintf(stderr,"track %d\n",ichan); 

    /*** skip over the header ***/
    kstart=HLEN+ichan;

    /** convert from mulaw to doubles **/
    samp2 = mu_2_float[(int)buff[kstart-2]];
    samp3 = mu_2_float[(int)buff[kstart]];

    /*** loop through the whole file ***/
    for (kt=kstart; kt<(nt-2); kt+=2){

      /* samp1 samp2 samp3 are three consecutive samples from channel */
      /* samp2 came from buff[kt] *************************************/
      samp1 = samp2;
      samp2 = samp3;
      samp3 = mu_2_float[(int)buff[kt+2]];

      /*** look for a large negative value between two small values **/
      if(samp3 > -.2 && samp3< .2 && 
	 samp1 > -.2 && samp1< .2 &&
	 samp2< -.3){      

	/* d1 = difference samp2 and linear interpolated samp1 & samp3 */
	d1     = samp2  - (samp1+samp3)/2;

	/* complement bits of mulaw and convert to double **/
	samp2x = mu_2_float[buff[kt]^mask];

	/* d2 = same thing after complementation */
	d2     = samp2x - (samp1+samp3)/2; 

	/** use absolute values **/
	if(d1<0) d1= -d1;
	if(d2<0) d2= -d2;
	
	if(d2 < d1){
	  /*** complementation makes a smaller distance ********/
	  /*** point passes primary tests, compute LPC error ***/

	  /*** extract 2*LW+1 samples (some maybe modified by earlier steps)*/
	  for (k= -LW; k<=LW; k++)
	    vec[k+LW] = mu_2_float[(int)buff[kt+2*k]];

	  /*** get LC long polynomial and error **/
	  (void)marcoef(vec,2*LW+1,coef,LC);
	  err1= residual(vec,2*LW+1,coef,LC); 

	  /*** now try the same thing, but with the modified value **/
	  vec[LW]=samp2x;
	  (void)marcoef(vec,2*LW+1,coef,LC);
	  err2= residual(vec,2*LW+1,coef,LC);

	  /*** if the modification is _MUCH_ better, accept it ***/
	  if(err2 < err1/10){
	    fprintf(stderr,"modified %d %g %g %g\n",
		    (kt-HLEN)/2,samp2,samp2x,err1/err2); 
	    buff[kt] ^= mask;
	  }
	}
      }
    }
  }

  /*** write out the modified data ***/
  for (kt=0; kt<nt; kt++)
    putc(buff[kt],stdout);

  return 0; 
}

/*****************************************************/
/* calculate power in the lpc residual  **************/
/* from the len samples in array x    ****************/
/*****************************************************/
static double residual(double *xin, int len, double *poly, int m)
{
  double err, tot; 
  int j, k;

  err=0.0; 
  for (k=m; k<len; k++){
    tot=xin[k]; 
    for (j=0; j<m; j++)
      tot += poly[j+1]*xin[k+1-j];
    err += tot*tot;
  }

  return err;
}

/*****************************************************/
/* returns m autocorr lpc coefficients, calculated ***/
/* from the len   samples in array x    **************/
/*****************************************************/
static double marcoef(double *xin, int len, double *cret, int m)
{
  double tpi = 2*M_PI;
  double x[2*LW+1];
  double errn; 
  int j, k; 

  /* protect the users signal */
  for (k=0; k<len; k++)
    x[k]=xin[k];

  /* calculate difference signal and window it  */
  x[len] = x[len-1];
  for (j=0; j<len; j++)
    x[j] = (x[j+1]-x[j])*(.54-.46*cos((tpi*j)/(len-1)));

  /* calc autocorr coeff, solve for inv fil coeffs */
  errn=invfl(x,len,cret,m);

  /* cret now contains 1., a_1, a_2, ... , a_m, 0, ... ,0 */

  return errn;
}

/**********************************************/
/*  calculation of m+1 length autocorrelation */
/*  sequence from the inverse filter input    */
/*  sequence                                  */
/**********************************************/
static double invfl(double *x, int np, double *cret, int m) 
{
  double f[LC], tf[LC], a[LC], r[LC+1];       
  int i, j, jj, n;
  double ss, alpha, beta, gamma, c, q, sm, errn;

  for (jj=0; jj<=m; jj++){   
    ss=0.0;    
    for (i=0; i<(np-jj); i++)
      ss += x[i]*x[i+jj];
    r[jj]=ss;
  }

  /* levinson's method for solving the autocorrelation matrix */
  f[0]=1.0;
  alpha=r[0];
  beta=r[1];
  a[0]= -r[1]/r[0];

  gamma=a[0]*r[1];  
  for (n=1; n<m; n++){

    c   = -beta/alpha;
    for (j=1; j<n; j++)
      tf[j]=f[j] + c*f[n-j];
    for (j=1; j<n; j++)
      f[j]=tf[j];
    
    f[n]=c*f[0];
    alpha += c*beta;

    beta=0.0;  
    for (j=0; j<=n; j++)
      beta += f[j]*r[n+1-j];

    q= -(r[n+1]+gamma)/alpha;
    for (j=0; j<n; j++)
      a[j] += q*f[n-j];
    a[n]=q*f[0];
   
    gamma=0.0;
    for (j=0; j<=n; j++)
      gamma += a[j]*r[n+1-j];
  }

  /* calculate normalized error errn */
  sm=0.0;    
  for (j=0; j<m; j++)
    sm += a[j]*r[j+1];
  errn=1.+sm/r[0];

  for (j=0; j<m; j++)
    cret[j+1]=a[j];      
  cret[0]=1.0;
  
  return errn;
}