/*-----------------------------------------------------------*/
/*   File:     STC32_FIR_2.C     Version 1.0                 */
/*   Function: STC32 FIR 11 order Lowpass filter Program     */
/*   Authors:  Bob Lin                                       */
/*   Date:     11-08-2001                                    */
/*-----------------------------------------------------------*/

#define  real      float
#define  true      1
#define  TRUE      1
#define  false     0
#define  FALSE     0
#define  PageSize  256
#define  pi        3.1415926

#include "stdlib.h"
#include "stdio.h"
#include "ctype.h"
#include "math.h"
#include "string.h"
#include "fft_1.h"

/*--------------------------------------------------------*/

int  ltoa( long number, char *buffer ) ;

void main( void );

void stc32_init( void );
void delay( int count );

void settimer0freq( int );
int  inittimer0isr( void );
void enabletimer0isr( void );
void disabletimer0isr( void );

void enableEINT3isr( void );
void disableEINT3isr( void );

void settimer1freq( int );
int  inittimer1isr( void );
void enabletimer1isr( void );
void disabletimer1isr( void );

void c_int01( void );
void c_int07( void );
void c_int08( void );

void rs232_flush( void );
char rs232_in( void );
int  rs232_out( char );
void string2PC( char *str );
char lowhex( char chr );
char highhex( char chr );
int  get_nth_byte( int value, int nth );

void set_dac_cmd( int n, real x );

int  get_encoder( int n );
void set_encoder( int n, int offset );
real get_position( int n );

void set_servo_on( int n );
void set_servo_off( int n );

int  get_positive_limit( int n );
int  get_negative_limit( int n );

void keybd_flush( void );
int  kbhit( void );
char getkb( void );

void LCD_flush( void );
int  LCD_out1( char );
int  LCD_out2( char );
int  LCD_out( char );
void string2LCD( char *str );
void LCD_HOME0( void );
void LCD_HOME1( void );

void get_adc(float *ainf );
void set_pwm( int n, int duty );

/*--------------------------------------------------------*/

/*#include "stc32.h"*/
volatile int *ENC0 = (int*) 0x910000;
volatile int *ENC1 = (int*) 0x918000;

volatile unsigned int   *AD0 =  (unsigned int*) 0x900000;
volatile unsigned int   *AD1 =  (unsigned int*) 0x908000;

volatile unsigned int   *PORTA = (unsigned int*) 0x940000;
volatile unsigned int   *PORTB = (unsigned int*) 0x940001;
volatile unsigned int   *PORTC = (unsigned int*) 0x940002;
volatile unsigned int   *L55_CTL = (unsigned int*) 0x940003;

volatile unsigned int   *CNT0 = (unsigned int*) 0x948000;
volatile unsigned int   *CNT1 = (unsigned int*) 0x948001;
volatile unsigned int   *CNT2 = (unsigned int*) 0x948002;
volatile unsigned int   *L54_CTL = (unsigned int*) 0x948003;

volatile unsigned int   *DA0 = (unsigned int*) 0x930000;
volatile unsigned int   *DA1 = (unsigned int*) 0x938000;

volatile unsigned int   *C_IOSTRB = (unsigned int*) 0x808060;
volatile unsigned int   *C_STRB0 = (unsigned int*) 0x808064;
volatile unsigned int   *C_STRB1 = (unsigned int*) 0x808068;

volatile unsigned int   *TMR0_CTRL = (unsigned int*) 0x808020;
volatile unsigned int   *TMR0_CNT = (unsigned int*) 0x808024;
volatile unsigned int   *TMR0_PERIOD = (unsigned int*) 0x808028;

volatile unsigned int   *TMR1_CTRL = (unsigned int*) 0x808030;
volatile unsigned int   *TMR1_CNT = (unsigned int*) 0x808034;
volatile unsigned int   *TMR1_PERIOD = (unsigned int*) 0x808038;

volatile unsigned int   *DATA_BUF  = (unsigned int*) 0x950000;
volatile unsigned int   *INT_EN    = (unsigned int*) 0x950001;
volatile unsigned int   *INT_FIFO  = (unsigned int*) 0x950002;
volatile unsigned int   *COM_CTRL  = (unsigned int*) 0x950003;
volatile unsigned int   *COM_STATE = (unsigned int*) 0x950005;

/*#include "stc32.h"*/
/*-------------------------------------------------------*/
#define  MAXQ      64
#define  MAXKBD    128
#define  MAXLCD    128

int  TICK=0, TICK0=0, TICK1=0;
int  LED=0;
int  count=0;
int  rear_i = -1, front_i = -1;
int  rear_o = -1, front_o = -1;
char queue_i[ MAXQ ];
char queue_o[ MAXQ ];

int  rear_KBD = -1, front_KBD = -1;
char queue_KBD[ MAXKBD ];
char KBD_old[] = { 0, 0, 0, 0 };
int  KBD_cnt = 0;
char KBD_T1[] = "123X456Y789Z*0#/";
char PORTC_REG = 0;

int  rear_LCD = -1, front_LCD = -1;
char queue_LCD[ MAXLCD ];
int  ain[2];
int  bias[] = { 0, 0 };
float adinf[2];
real SCALAR[] = { 1.0, 1.0 };

/*-------------------------------------------------*/
int full=0;
int count1=0;
struct InpBuf
 {
  float iData[PageSize];
  int Isfull;
  int Index;
  };

//Buf[x][y] x=input ADchannel and y=buffer
struct InpBuf Buf1[2],Buf2[2];  
float  datainput[PageSize];

/*-----------FIR Filter used variable define here-------*/

float filterout1[PageSize];
float filterout2[PageSize];
float x[11],x1[11];
float c[6]={0.2,0.1871,0.1514,0.1009,0.0468,0};
float temp=0,temp1=0,y=0;

/*-------------------------------------------------------*/
void main()
{  char cmd, keybd, str[20];
   int  axis = 0,i=0,j=0;
   float fs=10000.0;      // SampleRate is 10Khz
   float fc=500.0;        // Cut-off frequency is 500hz
   float v1=0;
//-----------FIR Filter Init here---------------//   
   v1=(2*fc)/fs;
   c[0]=v1;
   for(i=1;i<=5;i++)
   {
    c[i]=(sin(i*pi*v1))/(i*pi);
    } 
//----------------------------------------------//
   *C_STRB0 = 0xf1008;
   *C_STRB1 = 0xf10f0;

   *COM_CTRL=0x80;
   delay(10);
   *DATA_BUF=12;        /* 12:9600, 6:19200, 3:38400, 2:57600 */
   delay(10);
   *INT_EN=0;
   delay(10);
   *COM_CTRL=0x03;      /* 8 bits, 1 start, 1 stop, no parity */
   delay(10);
   *INT_FIFO=0x7;
   delay(10);

   stc32_init();
   set_encoder( 0, 0 );
   set_encoder( 1, 0 );
   set_dac_cmd( 0, 0.0 );
   set_dac_cmd( 1, 0.0 );

   settimer0freq( 1000 );
   inittimer0isr();
   settimer1freq( 10000 );
   inittimer1isr();

//   enableEINT3isr();

   
   LCD_HOME1();
   string2LCD("FIR TEST2 ");
   /*---------------------------------*/
   
   Buf1[0].Isfull=0;
   Buf1[1].Isfull=0;
   Buf1[0].Index=0;
   Buf1[1].Index=0;
  
   
   /*---------------------------------*/
   //FFT_Init();
   string2PC("\n STC32 \n");
   while ( 1 ) 
   {
     if(full==1)
     {
       count1=0;
       
       for ( i=0; i <256; i++ )
       {
        datainput[i]=(Buf1[0].iData[i]);
        
       }
         
         //FFT_Compute();
         //FFT_Powerout();
         for(j=0;j<256;j++)
         {
          temp=0;
          for(i=1;i<=10;i++){x[i-1]=x[i];}
          x[10]=datainput[j];
          for(i=1;i<=5;i++){temp=temp+c[i]*(x[5+i]+x[5-i]);}
          temp=temp+c[0]*x[5];
          filterout1[j]=temp;
          }
         full=0;
         }
      if(full==2)
      {
        count1=0;  
        
        for ( i=0; i <PageSize; i++ )
        {
         datainput[i]=Buf1[1].iData[i];
        
         }
         
         //FFT_Compute();
         //FFT_Powerout();
        for(j=0;j<256;j++)
          {
          temp=0;
          for(i=1;i<=10;i++){x[i-1]=x[i];}
          x[10]=datainput[j];
          for(i=1;i<=5;i++){temp=temp+c[i]*(x[5+i]+x[5-i]);}
          temp=temp+c[0]*x[5];
          filterout2[j]=temp;
          } 
         full=0;
         }          
      
   }  /* while */

}

/*-------------------------------------------------------*/
void stc32_init( void )
{
   /* Init Interrupt Vector */
   asm("\t.globl\t_c_int01");
   asm("\t.globl\t_c_int07");
   asm("\t.globl\t_c_int08");
   asm("\t.sect\t\".vector\"");
   asm("\tNOP");
   asm("\tNOP");   
   asm("\tNOP");
   asm("\tNOP");
   asm("\tbr\t_c_int01");
   asm("\t.space\t4");
   asm("\tbr\t_c_int07");
   asm("\tbr\t_c_int08");

/*************************************/
   asm("\t.sect\t\".text\"");
   /* High 16 bits of interrupt vectors */
   asm("\tLDI\t87ffH,IF");
   asm("\tLSH\t16,IF");
   /* Disable all Interrupt */
   asm("\tLDI\t0H,IE");
   /* initial 8255, A in, B out, C out */
   *L55_CTL = 0x90;
   
   /* initial 8254 CNT0, Mode 3 */
   *L54_CTL = 0x36;
   *CNT0 = (1000 & 0xff);
   *CNT0 = (1000>>8);

   /* initial 8254 CNT1, Mode 1 */
   *L54_CTL = 0x72;
   *CNT1 = 0;
   *CNT1 = 0;

   /* initial 8254 CNT2, Mode 1 */
   *L54_CTL = 0xb2;
   *CNT2 = 0;
   *CNT2 = 0;

   /* Enable D/A and turn on LED */
   asm("\tLDI\t2H,IOF");
   /* Enable GIE flag, Enable INT edge trigger */
   asm("\tOR\t6000H,ST");

   /* Initial LCD */
   LCD_out2( 0x03 );
   delay(400);
   LCD_out2( 0x03 );    
   LCD_out2( 0x03 );
   // Set 4 bit Operation
   LCD_out2( 0x02 ); 
   // Set 2 Line, 5x7 dot Character Font
   LCD_out2( 0x02 );
   LCD_out2( 0x08 );
   // Clear Display
   LCD_out2( 0x00 );
   LCD_out2( 0x01 );
   // Turn on Display and Cursor
   LCD_out2( 0x00 );
   LCD_out2( 0x0f );
   // Set Normal Operation
   LCD_out2( 0x00 );
   LCD_out2( 0x06 );
}

/*------------------------------------*/
void delay( int count )
{  int a;
   for( a = 0; a < count; a++ ) ;
}

/*---------------------------------------------------*/
inline void settimer0freq( int freq )
{
   *TMR0_PERIOD = 12500000/freq;
}

int inittimer0isr( void )
{
   /* Start Timer0 */
   *TMR0_CTRL = 0x2C1;
   asm("\tOR\t100H,IE");
}

inline void enabletimer0isr( void )
{
   /* Enable Timer Interrupt 0 */
   asm(" OR 100H,IE");
}

inline void disabletimer0isr( void )
{
   asm("\tAND\t06ffh,IE");
}

inline void enableEINT3isr( void )
{
   /* Enable External Interrupt 3 */
   asm("\tOR\t08H,IE");
}

inline void disableEINT3isr( void )
{
   asm("\tAND\t07f7h,IE");
}

/*---------------------------------------------------*/
inline void settimer1freq( int freq )
{
   *TMR1_PERIOD = 12500000/freq;
}

int inittimer1isr( void )
{
   /* Start Timer1 */
   *TMR1_CTRL = 0x2C1;
   asm("\tOR\t200H,IE");
}

inline void enabletimer1isr( void )
{
   /* Enable Timer Interrupt 1 */
   asm(" OR 200H,IE");
}

inline void disabletimer1isr( void )
{
   asm("\tAND\t05ffh,IE");
}

/*---------------------------------------------------*/

void c_int01()    // 8.3333 KHz
{
   TICK++;

    if ( (TICK & 4095) == 4095 ) {

      LED = 1 - LED;
      if ( LED == 1 )
	 asm("\tLDI\t2H,IOF");
      else
	 asm("\tLDI\t6H,IOF");
    }

   asm("\tNOP");
   asm("\tNOP");
}

/*-----------------------------------------------------*/
/* Timer 0 Interrupt, Frequency 1Khz (100KHz<=>10usec) */
/*-----------------------------------------------------*/
void c_int07()
{  
   char tmp, tmp1;  // KBD temporary variable */
   int a;

   asm("\tNOP");
   asm("\tNOP");
   //TICK0++;
   //count=TICK0;
 
   /* input a < 12, but output a < 11 */
   while( ( *COM_STATE & 0x1 ) == 1 )
      {
      if ( ++rear_i == MAXQ )   rear_i = 0;
      queue_i[rear_i] = *DATA_BUF & 0xff;
      }
   if ( ( *COM_STATE & 0x20 ) == 0x20 )
      {
      for ( a = 0; ( a < 11 ) && ( front_o != rear_o ); a++ )
	 {
	 if ( ++front_o == MAXQ )   front_o = 0;
	 *DATA_BUF = queue_o[ front_o ];
	 }
      }

   tmp =  *PORTA & 0xf;
   if( (KBD_old[KBD_cnt] != tmp) && (KBD_old[KBD_cnt] == 0) ) {
      if ( ++rear_KBD == MAXKBD )  rear_KBD = 0;
      if ( tmp == 0x1 )  tmp1 = 0;
      else if ( tmp == 0x2 )  tmp1 = 1;
      else if ( tmp == 0x4 )  tmp1 = 2;
      else tmp1 = 3;
      queue_KBD[ rear_KBD ] = KBD_T1[(KBD_cnt<<2)+tmp1];
   }
   KBD_old[KBD_cnt] = tmp;
   if ( ++KBD_cnt > 2 ) {
      if ( KBD_cnt == 4 ) {
	 KBD_cnt = 0;
	 *PORTC = PORTC_REG | 0x10;
      }  else
	 *PORTC = PORTC_REG | 0x80;
   } else if ( KBD_cnt == 2 )
	 *PORTC = PORTC_REG | 0x40;
   else
	 *PORTC = PORTC_REG | 0x20;

   if ( front_LCD != rear_LCD ) {
      if ( ++front_LCD == MAXLCD ) front_LCD = 0;
      *PORTB = queue_LCD[ front_LCD ];
   }

   asm("\tNOP");
   asm("\tNOP");
}


/*-----------------------------------------------*/

void c_int08()
{
    count1++;
    get_adc(adinf);
      if(Buf1[0].Isfull==0)
          {
          Buf1[1].Isfull=1;
          Buf1[0].iData[Buf1[0].Index]=adinf[0];
          Buf2[0].iData[Buf1[0].Index]=adinf[1];
          set_dac_cmd( 0,filterout1[Buf1[0].Index]);
          
          Buf1[0].Index++;
          if(Buf1[0].Index>=PageSize)
            {
             Buf1[0].Isfull=1;
             Buf1[1].Isfull=0;
             Buf1[0].Index=0;
             full=1;
             }
          }
        else if(Buf1[1].Isfull==0)
          {
            Buf1[0].Isfull=1;
            Buf1[1].iData[Buf1[1].Index]=adinf[0];
            Buf2[1].iData[Buf1[1].Index]=adinf[1];
            set_dac_cmd( 0,filterout2[Buf1[1].Index]);
            Buf1[1].Index++;
            if(Buf1[1].Index>=PageSize)
              {
               Buf1[1].Isfull=1;
               Buf1[0].Isfull=0;
               Buf1[1].Index=0;
               full=2;
               }
           } 
    
    //set_dac_cmd( 0,adinf[0]);
    //set_dac_cmd( 1,adinf[1]);

}

/*--------------------------------------------------------*/
void rs232_flush( void )
{
   front_i = rear_i = -1;
   front_o = rear_o = -1;
}

char rs232_in( void )
{
   if ( front_i == rear_i ) return(0);
   if ( ++front_i == MAXQ ) front_i = 0;
   return ( queue_i[ front_i ]);
}

int rs232_out( char chr )
{
   if ( ( (rear_o+1) & (MAXQ-1) ) == front_o )  return(0);
   if ( ++rear_o == MAXQ ) rear_o = 0;
   queue_o[ rear_o ] = chr;
   return( 1 );
}

void string2PC( char *str )
{ char *s;

   s = str; while(*s!=0x00) rs232_out(*s++);
}

char lowhex( char chr )
{
   chr = chr & 0x0f;
   if ( chr<10 ) return( '0'+chr );
   else return('A'+chr-10);
}

char highhex( char chr )
{
   chr = chr >> 4;
   chr = chr & 0x0f;
   if ( chr<10 ) return( '0'+chr );
   else return('A'+chr-10);
}

int get_nth_byte( int value, int nth )
{
    switch (nth) {
	case 0: return( value & 0x000000ff ); break;
	case 1: return( (value>>8)  & 0x000000ff ); break;
	case 2: return( (value>>16) & 0x000000ff ); break;
	case 3: return( (value>>24) & 0x000000ff ); break;
    }
}

/*--------------------------------------------------------*/
void set_dac_cmd( int n, real x )
{ int a;

   a = (int) (x * 204.7) + 2048;
   switch ( n ) {
       case 0: *DA0 = a;  break;
       case 1: *DA1 = a;  break;
   }
}


/*--------------------------------------------------------*/
int get_encoder( int n )
{ int a = 0 ;

   switch ( n ) {
       case 0: a = ((*ENC0<<8)>>8) + bias[0];  break;
       case 1: a = ((*ENC1<<8)>>8) + bias[1];  break;
   }
   return( a );
}

void set_encoder( int n, int offset )
{
   switch ( n ) {
       case 0: bias[0] = offset - ((*ENC0<<8)>>8);  break;
       case 1: bias[1] = offset - ((*ENC1<<8)>>8);  break;
   }
}

real get_position( int n )
{ real x = 0.0 ;

   switch ( n ) {
       case 0: x = SCALAR[0] * (real) ( ((*ENC0<<8)>>8) + bias[0] );
	       break;
       case 1: x = SCALAR[1] * (real) ( ((*ENC1<<8)>>8) + bias[1] );
	       break;
   }
   return( x );
}

/*--------------------------------------------------------*/
inline void set_servo_on( int n )
{
   PORTC_REG |= (1 << n);
   *PORTC |= PORTC_REG;
}

inline void set_servo_off( int n )
{
   PORTC_REG &= ~(1 << n);
   *PORTC &= PORTC_REG;
}

/*--------------------------------------------------------*/
int get_positive_limit( int n )
{
   switch ( n ) {
       case 0:  return( (*PORTA & 0x10) == 0 );  break;
       case 1:  return( (*PORTA & 0x20) == 0 );  break;
      default:  return( 0 );
   }
}

int get_negative_limit( int n )
{
   switch ( n ) {
       case 0:  return( (*PORTA & 0x40) == 0 );  break;
       case 1:  return( (*PORTA & 0x80) == 0 );  break;
      default:  return( 0 );
   }
}

void keybd_flush( void )
{
   front_KBD = rear_KBD = -1;
}

char getkb( void )
{
   if ( front_KBD == rear_KBD ) return( 0 );
   if ( ++front_KBD == MAXKBD ) front_KBD = 0;
   return ( queue_KBD[ front_KBD ]);
}

int kbhit( void )
{
   if ( front_KBD == rear_KBD )  return( 0 );
   else  return( 1 );
}


/*--------------------------------------------------------*/
void LCD_flush( void )
{
   front_LCD = rear_LCD = -1;
   LCD_out2( 0 );
   LCD_out2( 1 );
}

int LCD_out1( char value )
{
   if ( ( (rear_LCD+1) & (MAXLCD-1) ) == front_LCD ) return( 0 );
   if ( ++rear_LCD == MAXLCD )  rear_LCD = 0;
   queue_LCD[ rear_LCD ] = value | 0x80;
   return( 1 );
}

int LCD_out2( char value )
{  int a;

   a = LCD_out1( value | 0x40 );
   a += LCD_out1( value );
   if ( a == 2 )  return( 1 );
   else  return( 0 );
}

int LCD_out( char a )
{
   LCD_out2( 0x10 | ( a >> 4 ) );
   LCD_out2( 0x10 | ( a & 0xf ) );
}

void string2LCD( char *str )
{
   while(*str!=0x0) LCD_out(*str++);
}

void LCD_HOME0( void )
{
   // Move To (0,0)
   LCD_out2( 0x08 );
   LCD_out2( 0x00 );
}

void LCD_HOME1( void )
{
   // Move To (0,1)
   LCD_out2( 0x0c );
   LCD_out2( 0x00 );
}

/*--------------------------------------------------------*/

void get_adc(float* ainf)
{ 
  int a;  /* to be finished */
   ain[0] = *AD0 & 0xfff;
   ain[1] = *AD1 & 0xfff;
   if(ain[0]<2048)ainf[0]=(float)(ain[0])/204.8;
     else ainf[0]=(float)(ain[0]-4095)/204.8;
   if(ain[1]<2048)ainf[1]=(float)(ain[1])/204.8;
     else ainf[1]=(float)(ain[1]-4095)/204.8;
   a = *DA0;
   *AD0 = 0;
}

void set_pwm( int n, int duty ) /* n = 1,2   0 <= duty < 1000 */
{ 
   if ( duty != 0 )
      duty = 1000 - duty;
   *(CNT0+n) = (duty & 0xff);
   *(CNT0+n) = (duty>>8);
}

/*--------------------------------------------------------*/