#include "IQmathLib.h"
#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "DSP281x_Device.h"     // DSP281x Headerfile Include File


#include "filter_v1.h"


//#include "filter.h"
//#include "myfir16.h"


/*
#define IIR16_COEFF {\
			-10304,25264,4493,8986,4493}

#define IIR16_ISF	1298
#define IIR16_NBIQ	1
#define IIR16_QFMAT	14



#define FIR16_COEFF_50_75HZ {\
			4927,-1568135,-2289592,-2814717,-2881240,-2030268,471,3407677,7601571,11730182,\
			14023990,12910061,6880988,-4194369,-19136408,-34799420,-46792491,-49938254,-39649165,-12779469,\
			30867456,88408033,153681876,218365909,273219549,309985256}




#define FIR16_COEFF_58 {\
			1165,2622601,2884733,3409000,4129869,5178410,6489088,8127441,10093469,12387172,\
			15008553,17957610,21168811,24707691,28443180,32375278,36438450,40567161,44761411,48890130,\
			53018853,56885437,60620954,64028796,67108963,69861455,72155199,73924660,75300908,76087336,\
		}




#define FIR16_COEFF_WROTOR {\
			316,1245500,1245499,1245499,1245498,1311034,1376569,1376568,1442103,1507637,\
			1573172,1638706,1769776,1835310,1966380,2031914,2162984,2294053,2425123,2556192,\
			2752797,2883866,3080471,3211539,3408144,3604749,3801353,3997957,4194562,4391166,\
			4653306,4849910,5112050,5308653,5570793,5832933,6095072,6357212,6619351,6881491,\
			7143630,7471306,7733445,7995584,8323260,8585399,8913074,9240749,9502889,9830564,\
			10158239,10420379,10748054,11075729,11337869,11665544,11993219,12320895,12583034,12910710,\
			13238386,13500525,13828201,14090341,14418017,14680157,15007833,15269973,15532113,15859790,\
			16121930,16384071,16646211,16908352,17104957,17367098,17629239,17825844,18022449,18284591,\
			18481196,18677802,18874407,19071013,19202083,19398689,19529759,19660830,19791900,19922971,\
			20054041,20185112,20250647,20381718,20447253,20512789,20578324,20578323,20643859,20643859,\
			20709395}


*/

/* Filter Symbolic Constants                        */
//#define FIR_ORDER_58   58                                       

                                

//#define FIR_ORDER_50_75HZ   50                                       



//#define FIR16_COEFF {\
//			16519,532422588}


/* Filter Symbolic Constants                        */
//#define FIR_ORDER   2    




                                        
/* Create an Instance of FIRFILT_GEN module and place the object in "firfilt" section       */ 
//#pragma DATA_SECTION(fir, "firfilt");
//FIR16  fir = FIR16_DEFAULTS;
//FIR16  fir_58 = FIR16_DEFAULTS;
//FIR16  fir_wrotor = FIR16_DEFAULTS;

//FIR16  fir_50_75hz_0 = FIR16_DEFAULTS;
/*
FIR16  fir_50_75hz_1 = FIR16_DEFAULTS;
FIR16  fir_50_75hz_2 = FIR16_DEFAULTS;
FIR16  fir_50_75hz_3 = FIR16_DEFAULTS;
FIR16  fir_50_75hz_4 = FIR16_DEFAULTS;
FIR16  fir_50_75hz_5 = FIR16_DEFAULTS;
*/


                                            
/* Define the Delay buffer for the 50th order filterfilter and place it in "firldb" section */  
//#pragma DATA_SECTION(dbuffer_fir,"firldb");
//long dbuffer_fir[(FIR_ORDER+2)/2];                                        

//long dbuffer_fir_50_75hz_0[(FIR_ORDER_50_75HZ+2)/2];
/*long dbuffer_fir_50_75hz_1[(FIR_ORDER_50_75HZ+2)/2];
long dbuffer_fir_50_75hz_2[(FIR_ORDER_50_75HZ+2)/2];
long dbuffer_fir_50_75hz_3[(FIR_ORDER_50_75HZ+2)/2];
long dbuffer_fir_50_75hz_4[(FIR_ORDER_50_75HZ+2)/2];
long dbuffer_fir_50_75hz_5[(FIR_ORDER_50_75HZ+2)/2];
  */                                      






//#pragma DATA_SECTION(dbuffer_fir_58,"firldb");
//long dbuffer_fir_58[(FIR_ORDER_58+2)/2];      
                                  
//#pragma DATA_SECTION(dbuffer_fir_wrotor,"firldb");

         

/* Define Constant Co-efficient Array  and place the
.constant section in ROM memory				*/ 

//long const coeff_fir[(FIR_ORDER+2)/2]= FIR16_COEFF;
//long const coeff_fir_58[(FIR_ORDER_58+2)/2]= FIR16_COEFF_58;



         

//long const coeff_fir_50_75hz_0[(FIR_ORDER_50_75HZ+2)/2]= FIR16_COEFF_50_75HZ;
/*long const coeff_fir_50_75hz_1[(FIR_ORDER_50_75HZ+2)/2]= FIR16_COEFF_50_75HZ;
long const coeff_fir_50_75hz_2[(FIR_ORDER_50_75HZ+2)/2]= FIR16_COEFF_50_75HZ;
long const coeff_fir_50_75hz_3[(FIR_ORDER_50_75HZ+2)/2]= FIR16_COEFF_50_75HZ;
long const coeff_fir_50_75hz_4[(FIR_ORDER_50_75HZ+2)/2]= FIR16_COEFF_50_75HZ;
long const coeff_fir_50_75hz_5[(FIR_ORDER_50_75HZ+2)/2]= FIR16_COEFF_50_75HZ;
*/

         
/* Finter Input and Output Variables                */   
     


/* Create an Instance of IIR5BIQD16 module and place the object in "iirfilt" section    */ 
//#pragma DATA_SECTION(iir, "iirfilt");
//IIR5BIQ16  iir = IIR5BIQ16_DEFAULTS;
                                                                  
/* =============================================================================
Modify the delay buffer size to comensurate with the no of biquads in the filter
Size of the Delay buffer=2*nbiq
==============================================================================*/ 
/* Define the Delay buffer for the cascaded 6 biquad IIR filter and place it in "iirfilt" section */
//#pragma DATA_SECTION(dbuffer_iir,"iirfilt");
//int dbuffer_iir[2*IIR16_NBIQ];


/* =============================================================================
Modify the array size and symbolic constant to suit your filter requiremnt.
Size of the coefficient array=5*nbiq
==============================================================================*/
/* Define the Delay buffer for the cascaded 6 biquad IIR filter and place it in "iirfilt" section */
       
//const int coeff_iir[5*IIR16_NBIQ]=IIR16_COEFF;
         

/*
void init_my_filter_fir()
{

// FIR Generic Filter Initialisation    
       fir.order=FIR_ORDER; 
       fir.dbuffer_ptr=dbuffer_fir;
       fir.coeff_ptr=(long *)coeff_fir; 
       fir.init(&fir);
}


void init_my_filter_fir_50_75hz()
{

// FIR Generic Filter Initialisation    
       fir_50_75hz_0.order=FIR_ORDER_50_75HZ; 
       fir_50_75hz_0.dbuffer_ptr=dbuffer_fir_50_75hz_0;
       fir_50_75hz_0.coeff_ptr=(long *)coeff_fir_50_75hz_0; 
       fir_50_75hz_0.init(&fir_50_75hz_0);

}




//#pragma CODE_SECTION(calc_my_filter_fir,".fast_run");
int calc_my_filter_fir(int xn)
{
int yn;

//       xn=sgen.out;
       fir.input=xn;
       fir.calc(&fir);
       yn=fir.output;
       return yn;
}



void init_my_filter_fir_58()
{

// FIR Generic Filter Initialisation    
       fir_58.order=FIR_ORDER_58; 
       fir_58.dbuffer_ptr=dbuffer_fir_58;
       fir_58.coeff_ptr=(long *)coeff_fir_58; 
       fir_58.init(&fir_58);
}




//#pragma CODE_SECTION(calc_my_filter_fir_58,".fast_run");
int calc_my_filter_fir_58(int xn)
{
int yn;

//       xn=sgen.out;
       fir_58.input=xn;
       fir_58.calc(&fir_58);
       yn=fir_58.output;
       return yn;
}







void calc_my_filter_fir_50_75hz(_iq xn_0,_iq xn_1,_iq xn_2,_iq xn_3,_iq xn_4,_iq xn_5,
                               _iq *yn_0,_iq *yn_1,_iq *yn_2,_iq *yn_3,_iq *yn_4,_iq *yn_5)
{
       fir_50_75hz_0.input=_IQtoIQ15(xn_0);
       fir_50_75hz_0.calc(&fir_50_75hz_0);
       *yn_0=_IQ15toIQ(fir_50_75hz_0.output);
}



void init_my_filter_iir()
{

// IIR Filter Initialisation                                
        iir.dbuffer_ptr=dbuffer_iir;
        iir.coeff_ptr=(int *)coeff_iir; 
        iir.qfmat=IIR16_QFMAT;
        iir.nbiq=IIR16_NBIQ; 
        iir.isf=IIR16_ISF; 
        iir.init(&iir);
}


//#pragma CODE_SECTION(calc_my_filter_iir,".fast_run");
int calc_my_filter_iir(int xn)
{
int yn;

//       xn=sgen.out;
       iir.input=xn;
       iir.calc(&iir);
       yn=iir.output;
       return yn;
}

*/

#pragma CODE_SECTION(exp_regul_iq19,".fast_run");
_iq19 exp_regul_iq19(_iq19 k_exp_regul, _iq19 InpVarCurr, _iq19 InpVarInstant)
{
 _iq19 t1;

 t1 = (InpVarCurr + _IQ19mpy( (InpVarInstant-InpVarCurr),  k_exp_regul));
 return t1;
}


#pragma CODE_SECTION(exp_regul_iq,".fast_run");
//
// Tсек = Tпериод/k_exp_regul
//
_iq exp_regul_iq(_iq k_exp_regul, _iq InpVarFiltered, _iq InpVarInstant)
{
 _iq t1;
 t1 = (InpVarFiltered + _IQmpy( (InpVarInstant-InpVarFiltered),  k_exp_regul));
 return t1;
}

#pragma CODE_SECTION(exp_regul_iq,".fast_run");
void exp_regul_iq_fast(_iq k_exp_regul, _iq *InpVarCurr, _iq InpVarInstant)
{
 _iq t1;
 volatile _iq t2,t3,t4;

  
  t2 = (InpVarInstant- *InpVarCurr);
  t3 =  _IQmpy( t2,  k_exp_regul);
  t4 = *InpVarCurr + t3;
  t1 = t4;
  *InpVarCurr = t1;
// t1 = (InpVarCurr + _IQmpy( (InpVarInstant-InpVarCurr),  k_exp_regul));
// return t1;
}





/* 

_iq18 filter_1p(_iq18 predx,_iq18 predy, _iq18 inpx)
{
  _iq18 t1;

  t1 = _IQ18mpy(k1_filter_1p_fast,(predx+inpx))+_IQ18mpy(k2_filter_1p_fast,predy);
  return t1;
}


*/

/*

void init_filter_batter2()
{
  u_filter_batter2[0]=0;
  u_filter_batter2[1]=0;
  u_filter_batter2[2]=0;

  i_filter_batter2[0]=0;
  i_filter_batter2[1]=0;
  i_filter_batter2[2]=0;

  k_filter_batter2[0]=_IQ(K1_FILTER_BATTER2_3HZ);
  k_filter_batter2[1]=_IQ(K2_FILTER_BATTER2_3HZ);
  k_filter_batter2[2]=_IQ(K3_FILTER_BATTER2_3HZ  );

}




//#pragma CODE_SECTION(filter_batter2,".fast_run");
_iq filter_batter2(_iq InpVarCurr)
{
_iq y;
   
    y = _IQmpy(k_filter_batter2[0],( InpVarCurr + _IQmpyI32(i_filter_batter2[0],2) + i_filter_batter2[1]   )  ) + 
               _IQmpy(k_filter_batter2[1], u_filter_batter2[0]) + _IQmpy(k_filter_batter2[2], u_filter_batter2[1]);

    u_filter_batter2[1]=u_filter_batter2[0];
	u_filter_batter2[0]=y;

	i_filter_batter2[1]=i_filter_batter2[0];
	i_filter_batter2[0]=InpVarCurr;
    return y;

}


*/




