/****************************************************************************/
/*                         WE mass lock filter                              */
/*                     Project name: Complete.prj                           */
/*                                                                          */
/*                        File name: main.c                                 */
/*                           Editor: NN                                     */
/*                          Version: 1.0.0 (original 1.0.0)                 */
/*                             date: 2006/8/31                              */
/*                                                                          */
/*  (c) Copyright 2002 HIRATUKA Engineering Co.,Ltd. All rights reserved.   */
/****************************************************************************/
/*
Board setting
   DIPSW   S1,  1:OFF  2:OFF  3:ON   4:OFF  5:ON   6:ON  (10KHz)
   DIPSW   S2,  1:ON   2:OFF  3:OFF  4:ON   5:OFF  6:OFF (INT Enable & INT4)

Refer to Readme.txt for more information.
*/

#include <stdio.h>
#include "c6711dsk.h"
#include <c6x.h>
#include "c6xdskif.h"
#include <time.h>
#include "fastrts67x.h"

#define ORDER 12 

/************ Constant Value ********************************************/
double Troffset  = 0.010;  /*Transmitted light offset*/
double Trtrigger = 0.023;  /*Trigger threshold by Tr*/
double Trcounter = 0.030;  /*Lock judgement counter threshold by Tr*/
double fgbefore  = 5.4;    /*filter gain before lock*/
double fgafter   = 0.96;   /*filter gain after lock*/
/***********************************************************************/

double u[ORDER/2][2];           /* intermidiate buffer */
int    LC = 0;                  /* counter for lock signal */

const double am[ORDER/2-1][2] = 
{    { 0.8056852074,                0.},         /* a11, a21          */
     { 1.581653030,      -0.6254259562},         /* a12, a22          */
     { 1.599454328,      -0.6396193565},         /* a13, a23          */
     { 1.683871152,      -0.7277703518},         /* a14, a24          */
     { 1.683955253,       -0.727809165}};        /* a15, a25          */
const double bm[ORDER/2-1][3] =
{    { 1.,      1.095069074,               0.},  /* b01, b11, b21     */
     { 1.,      2.111187955,      1.119910158},  /* b02, b12, b22     */
     { 1.,      1.952492589,     0.9605736906},  /* b03, b13, b23     */
     { 1.,      1.841250384,      0.848876703},  /* b04, b14, b24     */
     { 1.,     -1.986683357,     0.9867276901}}; /* b05, b15, b25     */
const double cm[ORDER/2][2] = 
{    { 0.8056852074,               0.},          /* a11, a21          */
     { 1.581653030,     -0.6254259562},          /* a12, a22          */
     { 1.599454328,     -0.6396193565},          /* a13, a23          */
     { 1.683871152,     -0.7277703518},          /* a14, a24          */
     { 1.683955253,      -0.727809165},          /* a15, a25          */
     { 1.999724537,     -0.9997245557}};         /* a16, a26      LPF */
const double dm[ORDER/2][3] =
{    { 1.,      1.095069074,               0.},  /* b01, b11, b21     */
     { 1.,      2.111187955,      1.119910158},  /* b02, b12, b22     */
     { 1.,      1.952492589,     0.9605736906},  /* b03, b13, b23     */
     { 1.,      1.841250384,      0.848876703},  /* b04, b14, b24     */
     { 1.,     -1.986683357,     0.9867276901},  /* b05, b15, b25     */
     { 1.,     -1.996971989,     0.9969742812}}; /* b06, b16, b26 LPF */
       
void enableGlobalINT(void);						/* enable GIE                 */
void enableNMI(void);							/* enable NMIE                */
void enableSpecificINT(int INTnumber);			/* enable specific interrupt  */

void main(void)
{
    /*define variables*/
    int k,m;
	
	/*program*/
    /*buffer clear*/
    for (m=0; m<ORDER/2; m++) 
    for (k=0; k<2; k++)
    {
    	u[m][k] = 0.0;  
    }

    *(unsigned volatile int *)EMIF_CE2 = 0x0120C422; /* CE2 Bus timming control */

	enableSpecificINT(4);						/* enable INT4(External INT)         */
	enableNMI();								/* enable NMI(Non Maskable Interrupt)*/
	enableGlobalINT();							/* set GIE(Global Interrupt Enable)  */
    
    for (;;)
    {
												/* Keep waiting for Timer0 Interrupt */
    }
}

interrupt void int4(void)
{
	/*define variables*/
	double SI, G, Tr, Tg, un, cal;
    int m;
    short SI0, G0, Tr0, So, DA1, DA2, DA3; 

/*	clock_t start;
	clock_t overhead;
	clock_t elapsed;
	start = clock();
	overhead = clock() - start;
	start = clock(); 
*/	

    SI0  = *( volatile short *)DSKIF_AD0; 	/*Signal Input*/  
//    LS0 = *( volatile short *)DSKIF_AD1; 	/*Locked sign*/
    G0  = *( volatile short *)DSKIF_AD2; 	/*Gain for feedback*/ 
    Tr0 = *( volatile short *)DSKIF_AD3; 	/*Transmitted light*/

	SI   = (double)SI0;						/*modified Signal Input*/
	G   = (double)G0;						/*modified Gain for feedback*/
	Tr  = (double)Tr0*3.051757e-5 + Troffset;	/*modified Trs. light original0.015*/
	DA3 = (short)(Tr * 32678.) ;			/*Trans cal. light monitor*/

/****************Gain value modify (G -> 0<G<1)*************/    
	if (G<0)
		G = 0.;
	else
		G = G*4.8828125e-4;
  
/****************Triger setting [Tg=Triger]*****************/
	if (Tr > Trtrigger)
		Tg = 1.;
	else
		Tg = 0.;

    DA2 = ( short)Tg * 8192 + (short)LC ; 	/*Triger and LC monitor*/

/****************Lock judgement [LC=Lock Counter]***********/
	if (Tr > Trcounter)
   {
	   if (LC < 1000) /*counter > 5msec*/ 
		   LC++;
	   else
	       LC = 1000;	   
   }
   else 
		LC = 0;  

/****************Error signal calculation*******************/
    if (LC < 1000)/*****Lock Before*****/
    {
		cal = _rcpdp(Tr)*0.1;          /*Normalization function*/
	    SI = SI * cal * Tg;      
		DA1 = ( short)(SI*0.1) ;
	 	for (m=0; m<ORDER/2-1; m++)
        {
            un = am[m][0]*u[m][0] + am[m][1]*u[m][1] + SI;
            SI = bm[m][0]*un + bm[m][1]*u[m][0] + bm[m][2]*u[m][1];
            u[m][1] = u[m][0];          /* shift of data */
            u[m][0] = un;               /* shift of data */
        }
        SI = SI*5.094971566e-006*fgbefore;
    }
    
    else/*****Lock After******/
    {
    	DA1 = ( short)(SI*0.1) ;
	 	for (m=0; m<ORDER/2; m++)
        {
            un = cm[m][0]*u[m][0] + cm[m][1]*u[m][1] + SI;
            SI = dm[m][0]*un + dm[m][1]*u[m][0] + dm[m][2]*u[m][1];
            u[m][1] = u[m][0];          /* shift of data */
            u[m][0] = un;               /* shift of data */
        }
        SI = SI*4.216521791e-006*fgafter;
    }

/**************Over flow problem*********************/
   SI= SI*G;    
    if(SI>=32767) So = 32767;
	else if (SI<-32768) So=-32768;
	else So=(short)SI; 

   *( unsigned volatile short *)DSKIF_DA0 = So; /*Signal Output*/
   *( unsigned volatile short *)DSKIF_DA1 = DA1; /*Normalized and triggerd error Output*/ 
   *( unsigned volatile short *)DSKIF_DA2 = DA2; /*Triger and LC monitor Output*/ 
   *( unsigned volatile short *)DSKIF_DA3 = DA3; /*Offsetted transmitted light Output*/
    
//    elapsed = clock() - start - overhead;
//    printf("Time = %ld cycles\n", (long)elapsed); 
//    printf("Time = %ld %e cycles\n", (long)elapsed,(double)elapsed/CLOCKS_PER_SEC); 	
   
}

/****************************************************************************/
/*  enableGlobalINT() - enable GIE                                          */
/****************************************************************************/
void enableGlobalINT(void)
{
    CSR |= 0x1;
}

/****************************************************************************/
/*  enableNMI() - enable NMIE                                               */
/****************************************************************************/
void enableNMI(void)
{
    IER = _set(IER, 1, 1);
}

/****************************************************************************/
/*  enableSpecificINT() - enable specific interrupt                         */
/*                                                                          */
/*  Arguments -                                                             */
/*    INTnumber - interrupt number to be enabled                            */
/****************************************************************************/
void enableSpecificINT(INTnumber)
{
    IER = _set(IER, INTnumber, INTnumber);  /* set specific IER bits        */
}