// --------------------------------------------------------------------------------------
// 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; 
  PTFPE = 0xFF; 
  PTGPE = 0xFF; 
  PTHPE = 0xFF; 
  PTJPE = 0xFF; 
  
  /* Configures PTG[2:1] as accelerometer sensitivity 
      PTG2:PTG1
        0    0  = 1.5g
        0    1  = 2.0g
        1    0  = 4.0g
        1    1  = 6.0g
  */
  PTGD = 0x00;
  PTGDD = 0x06;
  
  // Configures ADC peripheral (ADC clock = 6.25MHz)
  // Bus clock as clock source, 12-bit conversion and divisor=4
  ADCCFG = 0x44;
    
  // Timer2 overflow about every 1ms
  TPM2MOD = 25000;
  // Stops timer2 and select 1 as prescaler divisor
  TPM2SC = 0x00;
    
  // Initializes SCI Peripheral
  InitSCI(fei_baud);
          
}

/////////////////////////////////////////////////////////////////////////////////////////
// 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 = 0x08;                 // Select ADC8 (PTA6) channel 
  y.reading[samp]  = (dword)(ReadAcceleration()<<4);
  ADCSC1 = 0x09;                 // 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();   break;
      case avg   : avg_data();      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");
  while (!SW1){}
  for(;;){
    while(SW4){
      ShowAcceleration();
    } //end while(SW4)
    while(SW3){_Stop;}
  } //end for(;;)
    
}

/////////////////////////////////////////////////////////////////////////////////////////
// KBI_ISR
// --------------------------------------------------------------------------------------
// Reads PTA[3:2] 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
  KBI1SC_KBACK = 1;
}