/////////////////////////////////////////////////////////////////////////////////////////
// PeriphInit
// --------------------------------------------------------------------------------------
// Initializes various registers and peripherals
/////////////////////////////////////////////////////////////////////////////////////////
void PeriphInit(void)
{
  // Disables COP and Enable STOP instruction and RESET and BKGD pin
  SOPT1 = 0x23;
  
  // Selects FEI mode
  // Sets trimming for fBUS about 25 MHz
  ICS_FEI();
  
  // Enable all pullups
  PTAPE = 0xFF; 
  PTBPE = 0xFF; 
  PTCPE = 0xFF; 
  PTDPE = 0xFF; 
  PTEPE = 0xFF; 
; 
  


  // Configures ADC peripheral (ADC clock = 2MHz)
  // Bus clock as clock source, 12-bit conversion and divisor=4
  ADCCFG = 0x44;
  APCTL1 = 0x07;
 
   
  // Timer2 overflow about every 1ms
  TPM2MOD = 25000;
  // Stops timer2 and select 1 as prescaler divisor
  TPM2SC = 0x00;
    
  // Initializes SCI Peripheral
  InitSCI(fei_baud);
  
  ////////////////////////////////////////////////////////////////////////////  
// Init LCD 
////////////////////////////////////////////////////////////////////////
     
   LCDPEN0 = 0xFF;
   LCDPEN1 = 0xFF;
   LCDPEN2 = 0xFF;
   LCDPEN3 = 0xFF;
    
             //Enable all LCD Pins 
    
    //Enable Back Plane Pins
    LCDBPEN0 = 0xDB; //
    LCDBPEN1 = 0x03;

     
    //Set Back Planes
    LCDWF0 = 0x01;
    LCDWF3 = 0x02;
    LCDWF6 = 0x04;
    LCDWF9 = 0x08;
    LCDWF1 = 0x10;
    LCDWF7 = 0x20;
    LCDWF4 = 0x40;
    LCDWF8 = 0x80;
      
    //Front plane data
      
      SetDisplay();
      
      
      ///LCD Control registers
      LCDRVC = 0x8b;
      LCDSUPPLY = 0x83;
      LCDC0 =    0xA7;  
      
      //Set up blink mode
      
      LCDBCTL_BRATE =4;
      //LCDBCTL_BLINK  = 1; 
     
      
    

          
}

/////////////////////////////////////////////////////////////////////////////////////////
// filter_data 
// --------------------------------------------------------------------------------------
// Filters the collected x,y,z data using simple IIR filter
/////////////////////////////////////////////////////////////////////////////////////////
void filter_data()
{
byte i;
dword X, Y, Z;

  X = x.reading[samp];
  Y = y.reading[samp];
  Z = z.reading[samp];
  
  for (i=samp;i>0;i--){
    X = (X + ((x.reading[i] + x.result[i-1])>>1))>>1;
    Y = (Y + ((y.reading[i] + y.result[i-1])>>1))>>1;
    Z = (Z + ((z.reading[i] + z.result[i-1])>>1))>>1;
  }
  
  x.result[samp] = (word)X;
  y.result[samp] = (word)Y;
  z.result[samp] = (word)Z;
  
  
}

/////////////////////////////////////////////////////////////////////////////////////////
// avg_data 
// --------------------------------------------------------------------------------------
// - averages 10 collected x,y,z values
// - puts results in elements 0 of arrays
/////////////////////////////////////////////////////////////////////////////////////////
void avg_data()
{
byte j;
long x_avg=0, y_avg=0, z_avg=0;

  for (j=1;j<=samp;j++){
    x_avg += x.reading[j];
    y_avg += y.reading[j];
    z_avg += z.reading[j];
  }
  x.result[samp] = (word)(x_avg>>4);
  y.result[samp] = (word)(y_avg>>4);
  z.result[samp] = (word)(z_avg>>4);
  
 
}

/////////////////////////////////////////////////////////////////////////////////////////
// copy_data 
// --------------------------------------------------------------------------------------
// - copies reading into result
/////////////////////////////////////////////////////////////////////////////////////////
void copy_data() {
  x.result[samp] = x.reading[samp];
  y.result[samp] = y.reading[samp];
  z.result[samp] = z.reading[samp];
}

/////////////////////////////////////////////////////////////////////////////////////////
// ReadAcceleration
// --------------------------------------------------------------------------------------
// Reads acceleration data on a given axis and saves it to the axis structure
/////////////////////////////////////////////////////////////////////////////////////////
word ReadAcceleration(void)
{

  word adc;

  while (!(ADCSC1_COCO)){}              // Waits until ADC conversion is completed

  adc=ADCR;
  return adc;
    
}

/////////////////////////////////////////////////////////////////////////////////////////
// ShowAcceleration
// --------------------------------------------------------------------------------------
// -  Prints the accelaration data in the terminal;
/////////////////////////////////////////////////////////////////////////////////////////
void ShowAcceleration ()
{
word SampleCNT;
byte j,k;
   
  // Read acceleration data
  ADCSC1 = 0x01;                 // Select ADC1 (PTA1) channel
  x.reading[samp]  = (dword)(ReadAcceleration()<<4);
  ADCSC1 = 0x02;                 // Select ADC8 (PTA6) channel 
  y.reading[samp]  = (dword)(ReadAcceleration()<<4);
  ADCSC1 = 0x03;                 // Select ADC9 (PTA7) channel
  z.reading[samp]  = (dword)(ReadAcceleration()<<4);
  
  StartTPM(0);   //0 = TPM prescaler = /2

  if(samp>0){
    switch (mode){
      case filter: filter_data(); 
      ClearDisplay();
      Freescale(ON);
      string = "IIRFILTER";
      DisplayString(string); 
    break;
  
      case avg   : avg_data();      
        ClearDisplay();
        Freescale(ON);
        string = "AVERAGE";
        DisplayString(string);
    break;
      default    : copy_data();
    }
  } else {
    copy_data();
  }
  
  SampleCNT = StopTPM();
  if (SampleCNT<0x0100) {
    for(j=0xff;j>0;j--){
      for(k=0x10;k>0;k--){}
    }
  }

  // Display Acceleration
  SendMsg("\r\n");
  SendMsg(word2asc((word)x.result[samp],dis_base));
  SendMsg(",");
  SendMsg(word2asc((word)y.result[samp],dis_base));
  SendMsg(",");
  SendMsg(word2asc((word)z.result[samp],dis_base));
  SendMsg(",");
  SendMsg(word2asc(SampleCNT,dis_base));
  
  // Shift array of results if we hit max
  if (samp >= max-1) {
    for (j=0;j<max-1;j++){
      x.result[j]  = x.result[j+1];
      x.reading[j] = x.reading[j+1];
      y.result[j]  = y.result[j+1];
      y.reading[j] = y.reading[j+1];
      z.result[j]  = z.result[j+1];
      z.reading[j] = z.reading[j+1];
    }
    samp = max-1;
  } else {
    samp++;
  } //end if (i => max) 
  
}

/////////////////////////////////////////////////////////////////////////////////////////
// MAIN
// --------------------------------------------------------------------------------------
// Entry point
/////////////////////////////////////////////////////////////////////////////////////////
void main(void)
{
  PeriphInit();
  InitKBI();
  EnableInterrupts; 
// Selects fBUS as timer1 clock source and start timer
  TPM1SC = 0x08;
//  SendMsg("\fX, Y, Z\r\n");
  
  ClearDisplay();
  Freescale(ON);
  string = "ACCELDEMO";
  DisplayString(string);
  
  
  while (!SW1){}
  for(;;){
    while(SW4){
      ShowAcceleration();
    } //end while(SW4)
    
  } //end for(;;)
    
}

/////////////////////////////////////////////////////////////////////////////////////////
// KBI_ISR
// --------------------------------------------------------------------------------------
// Reads PTA[7:6] and shifts to LSBs
// Debounces switch
// Acknowledges KBF
/////////////////////////////////////////////////////////////////////////////////////////
interrupt VectorNumber_Vkeyboard void KBI_ISR(void){
byte d,b;
  
  //capture which pin was pushed
  mode = (byte)(KBI_VAL);
  //debounce button
  for (d=0xff;d>0;d--){
    for (b=0x80;b>0;b--){}
  }
  //clear KBF
  KBISC_KBACK = 1;
}