#include <stdio.h>
#include <math.h>
#include "mex.h"

double  *NFArray(int size)
{
	double	*p;
	p = (double *)mxCalloc(sizeof(*p),size);
	return p;
}

/****************************************************************/
/* ACbank                                                       */
/*         Asymmetrical Compensation filterbank by Toshio Irino */
/* combined with Meddis's inner haircell model                  */ 
/* A Single channel implementation                              */
/*                                                              */
/*       By Aki Härmä April 26 1999     		        */
/****************************************************************/
void trans(double *y, double *x,
	   long int len,
	   double fr, double fs, double *P)
{
 
   int q, w, e, mlen, chan;
   long int o, aindex, bindex, outindex;
  double b, c, p1, p2, rkC, ERB, CentF, devC;
  double *A1,*A2,*B1,*B2;
  double temp, Lint, oldY;
  double Pii;
  double *z1,*z2,*z3,*z4,*rk,*th,*phi,*ga,*gb;
  int k;

  z1=NFArray(3);z2=NFArray(3);z3=NFArray(3);z4=NFArray(3);
  rk=NFArray(4);th=NFArray(4);phi=NFArray(4);
  A1=NFArray(4);A2=NFArray(4);
  B1=NFArray(4);B2=NFArray(4);ga=NFArray(4);gb=NFArray(4);

  Lint=exp(-1/(0.02*fs)); /* Loudness integration (t=20 ms) */
  oldY=1;
  /* Initialization */
  Pii=4*atan(1);  /*Value of pi */
  b=1.68;
 
  ERB=24.7+0.108*fr;
  rkC=2*Pii*b*ERB/fs;
  CentF=2*Pii*fr/fs;
  devC=b*ERB/fs;

  for (o=0;o<len;o++)
    {
 for (k=0;k<4;k++)
    { 
     
      p1=1.35-0.19*fabs(c);
      p2=0.29-0.004*fabs(c);
      rk[k]=exp(-(k+1)*p1*rkC);
      th[k]=CentF+devC*pow(2,k)*p2*c;
      phi[k]=CentF-devC*pow(2,k)*p2*c;  
      A1[k]=-2*rk[k]*cos(phi[k]);A2[k]=rk[k]*rk[k];
      B1[k]=-2*rk[k]*cos(th[k]);B2[k]=A2[k];
      /* gain normalization coefficients */
      /* Normalization near the peak frequency */  
/*       ga[k]=sqrt(pow(A2[k]*cos(2*th[k])+A1[k]*cos(th[k])+1,2)+ */
/* 		 pow(A2[k]*sin(2*th[k])+A1[k]*sin(th[k]),2)); */
/*       gb[k]=sqrt(pow(B2[k]*cos(2*th[k])+B1[k]*cos(th[k])+1,2)+ */
/* 		 pow(B2[k]*sin(2*th[k])+B1[k]*sin(th[k]),2)); */
      /* Normalization at the end of passbank */
      if(th[k]<phi[k]){ga[k]=A2[k]+A1[k]+1;gb[k]=B2[k]+B1[k]+1;}
      else{ga[k]=A2[k]-A1[k]+1;gb[k]=B2[k]-B1[k]+1;}
    }

      /* 1st stage */
      z1[0]=(gb[0]/ga[0])*x[o]-B1[0]*z1[1]-B2[0]*z1[2];
      temp=z1[0]+A1[0]*z1[1]+A2[0]*z1[2];
      z1[2]=z1[1];z1[1]=z1[0];
      /* 2nd stage */
      z2[0]=(gb[1]/ga[1])*temp-B1[1]*z2[1]-B2[1]*z2[2];
      temp=z2[0]+A1[1]*z2[1]+A2[1]*z2[2];
      z2[2]=z2[1];z2[1]=z2[0];
      /* 3nd stage */
      z3[0]=(gb[2]/ga[2])*temp-B1[2]*z3[1]-B2[2]*z3[2];
      temp=z3[0]+A1[2]*z3[1]+A2[2]*z3[2];
      z3[2]=z3[1];z3[1]=z3[0];
      /* 4nd stage */
      z4[0]=(gb[3]/ga[3])*temp-B1[3]*z4[1]-B2[3]*z4[2];
      y[o]=z4[0]+A1[3]*z4[1]+A2[3]*z4[2];
      z4[2]=z4[1];z4[1]=z4[0];

        /* Loudness integration */
      oldY=(1-Lint)*((y[o]<0.0)?0.0:y[o])+Lint*oldY;
      P[o]=20*log10(oldY)+20;
     /*  P[o]=oldY+20; */
      c=3.38-0.107*P[o];


     }
  
return;
}

/************************  MEXFUNCTION *****************************/
void mexFunction(
	int		nlhs,
	mxArray	*plhs[],
	int		nrhs,
	const mxArray    *prhs[]
	)
{
	double	*y, *x, *P;
	double fr,fs,A,B;
        int q, n, m, adim, bdim, mlen, N;
        long int len,mm,nn;
/* GET POINTERS */
	x = mxGetPr(prhs[0]);
	mm = (long int)mxGetM(prhs[0]);
	nn= (long int)mxGetN(prhs[0]);
	if(nn==1)len=mm; else len=nn;

        A= mxGetScalar(prhs[1]);
        B= mxGetScalar(prhs[2]);
	plhs[1] = mxCreateDoubleMatrix(len, 1, mxREAL);
	P= mxGetPr(plhs[1]);

       plhs[0] = mxCreateDoubleMatrix(len, 1, mxREAL);
	y = mxGetPr(plhs[0]);

	/* Do the actual computations in a subroutine */

	trans(y,x,len,A,B,P);
	return;
}