#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "../machine.h"
//The auxiliary subroutine, calculating the scaling factors
int NormCoeff(double cmax, int * aShift, double * aScale)
{
   int Stop=0;
   double Scale=1;
   int Shift=0;
   if(cmax!=0)
      while(!Stop)
      {
	 Stop=1;
	 if(cmax>=1)
	 {
	    Stop=0;
	    cmax/=2;
	    Scale/=2;
	    Shift++;
	 }
	 if(cmax<0.5)
	 {
	    Stop=0;
	    cmax*=2;
	    Scale*=2;
	    Shift--;
	 }
      }
   *aScale=Scale;
   *aShift=Shift;
}

//Function generating the assembler code for DSP56002, implementing
//the IIR filter
//Input data format:
//      Ca - a coefficients (starting from a1)
//      Na - number  of a coefficients (not including a0=1)
//      Cb - b coefficients (starting from b0)
//      Nb - number of b coefficients
//      fout - output file
int GenIIR(double * Ca, int Na, double * Cb, int Nb, FILE * fout, char * FltrName)
{
   //First we calculate the scaling factor for "a" coefficients
   int ShiftA, ShiftB;
   double ScaleA,ScaleB;
   int i,Stop;
   double cmax;
   cmax=fabs(Ca[0]);
   for(i=0;i<Na;i++)
   {
      if (fabs(Ca[i])>cmax) cmax=fabs(Ca[i]);
   }
   NormCoeff(cmax,&ShiftA,&ScaleA);
   //Then we calculate the scaling factor for "b" coefficients
   cmax=fabs(Cb[0]);
   for(i=0;i<Nb;i++)
   {
      if (fabs(Cb[i])>cmax) cmax=fabs(Cb[i]);
   }
   NormCoeff(cmax,&ShiftB,&ScaleB);
   //Now we know how to scale the coefficients of the filter
   //Start code generation
   //Assumptions:
   // "R0" register points the following samples
   // "R4" register points the following filter coefficients
   // The samples' buffers are organized as circular buffers
   //
   //We generate the filter coefficients
   fprintf(fout,"\torg\tx:\n");
   fprintf(fout,"FltBufIn_%s\tbsm\t%d\n",FltrName,Nb);
   fprintf(fout,"FltBufOut_%s\tbsm\t%d\n",FltrName,Na);
   fprintf(fout,"FltInSmpPtr_%s\n\tdc\tFltBufIn_%s\n",FltrName,FltrName);
   fprintf(fout,"FltOutSmpPtr_%s\n\tdc\tFltBufOut_%s\n",FltrName,FltrName);
   fprintf(fout,"\torg\ty:\n");
   fprintf(fout,"CoeffA_%s\n",FltrName);
   for(i=0;i<Na;i++)  fprintf(fout,"\tdc\t@FRC(%.15g)\n",-Ca[i]*ScaleA);
   fprintf(fout,"CoeffB_%s\n",FltrName);
   for(i=0;i<Nb;i++)  fprintf(fout,"\tdc\t@FRC(%.15g)\n",Cb[i]*ScaleB);
   //We generate the macro containing the filter's routine
   fprintf(fout,"\torg\tp:\n");
   fprintf(fout,"Filtr_%s\tmacro\n",FltrName);
   //We set the index registers
   fprintf(fout,"\tmove\tx:FltInSmpPtr_%s,r0\n",FltrName);
   fprintf(fout,"\tmove\t#%d,m0\n",Nb-1); //Circular buffer of input samples
   fprintf(fout,"\tmove\t#CoeffB_%s,r4\n",FltrName);
   fprintf(fout,"\tmove\t#(-1),m4\n");
   fprintf(fout,"\tmove\tx0,x:-(r0)\n");
   fprintf(fout,"\tmove\tr0,x:FltInSmpPtr_%s\n",FltrName);
   //We write the new sample into the input samples buffer
   fprintf(fout,"\tmove\tx:(r0)+,x0\ty:(r4)+,y0\n");
   //Now we are generating the routine calculating the sum of previous input
   //samples multiplied by "b" coefficients
   for(i=1;i<=Nb;i++)
   {
      if((i==1) && (i==Nb))
      {
	 fprintf(fout,"\tmpy\tX0,Y0,b\n");
      }
      else if(i==1)
      {
	 fprintf(fout,"\tmpy\tX0,Y0,b\tX:(R0)+,X0\tY:(R4)+,Y0\n");
      }
      else if(i==Nb)
      {
	 fprintf(fout,"\tmac\tX0,Y0,b\n");
      }
      else
      {
	 fprintf(fout,"\tmac\tX0,Y0,b\tX:(R0)+,X0\tY:(R4)+,Y0\n");
      }
   }
   //Scale the result
   if(ShiftB<-1)
   {
      fprintf(fout,"\trep\t#%d\n",-ShiftB);
      fprintf(fout,"\tasr\tb\n");
   }
   else if(ShiftB==-1)
   {
      fprintf(fout,"\asr\tb\n");
   }
   if(ShiftB>1)
   {
      fprintf(fout,"\trep\t#%d\n",ShiftB);
      fprintf(fout,"\tasl\tb\n");
   }
   else if(ShiftB==1)
   {
      fprintf(fout,"\tasl\tb\n");
   }
   //Now we calculate the sum of previous output samples multiplied
   //by "a" coefficients
   //We prepare the index registers
   fprintf(fout,"\tmove\t#CoeffA_%s,r4\n",FltrName);
   fprintf(fout,"\tmove\tx:FltOutSmpPtr_%s,r0\n",FltrName);
   fprintf(fout,"\tmove\t#%d,m0\n\tnop\n",Na-1); //Bufor cykliczny próbek wyjściowych
   fprintf(fout,"\tmove\tx:(r0)+,x0\ty:(r4)+,y0\n");
   //We generate the summing routine
   for(i=1;i<=Na;i++)
   {
      if((i==1) && (i==Na))
      {
	 fprintf(fout,"\tmpy\tX0,Y0,a\n");
      }
      else if(i==1)
      {
	 fprintf(fout,"\tmpy\tX0,Y0,a\tX:(R0)+,X0\tY:(R4)+,Y0\n");
      }
      else if(i==Na)
      {
	 fprintf(fout,"\tmac\tX0,Y0,a\n");
      }
      else
      {
	 fprintf(fout,"\tmac\tX0,Y0,a\tX:(R0)+,X0\tY:(R4)+,Y0\n");
      }
   }
   //Scale the result
   if(ShiftA<-1)
   {
      fprintf(fout,"\trep\t#%d\n",-ShiftA);
      fprintf(fout,"\tasr\ta\n");
   }
   else if(ShiftA==-1)
   {
      fprintf(fout,"\asr\ta\n");
   }
   if(ShiftA>1)
   {
      fprintf(fout,"\trep\t#%d\n",ShiftA);
      fprintf(fout,"\tasl\ta\n");
   }
   else if(ShiftA==1)
   {
      fprintf(fout,"\tasl\ta\n");
   }
   fprintf(fout,"\tmove\tX:FltOutSmpPtr_%s,r0\n",FltrName);
   //We add both the components
   fprintf(fout,"\tadd\tb,a\n");
   fprintf(fout,"\tmove\ta,X:-(r0)\n");
   fprintf(fout,"\tmove\tR0,X:FltOutSmpPtr_%s\n",FltrName);
   fprintf(fout,"\tendm\n");
}

/*
   void main()
   {
   //Function for testing of code generation routine
   double Ca[]={  -2.1219, 1.7872, -0.5410  };
   int Na=3;
   double Cb[]={  0.0865, -0.0244, -0.0244, 0.0865  };
   int Nb=4;
   FILE * fout=fopen("filtr.asm","wt");
   GenIIR(Ca,Na,Cb,Nb,fout,"f1");
   fclose(fout);
   }
*/

//Scilab interface function
void C2F(cgenfilt) (double * Cfa, int * Nfa, double * Cfb, int * Nfb,
   char * fname, int * len_fn, char * fltrname,
   int * len_fltr, int * result)
{
   FILE * fout;
   char fn[(*len_fn)+1];
   char fltrn[(*len_fltr)+1];
   strncpy(fn,fname,*len_fn);
   fn[*len_fn]=0;
   strncpy(fltrn,fltrname,*len_fltr);
   fltrn[*len_fltr]=0;
   fout=fopen(fn,"wt");
   GenIIR(Cfa, *Nfa, Cfb, *Nfb, fout, fltrn);
   fclose(fout);
   *result=1;
}