/*!
 * \file    accelerometer.c
 * \brief   Simple driver MPR083 using I2C
 * \version $Revision: 1.3 $
 * \author  Anthony Huereca
 */

 #include "accelerometer.h"

/*******************************************************************/
/*!
 * Initialize ADC and Accelerometer
 */
void Accel_init() 
{
  /* Set Senstivity to 1.5g */
  PTCDD_PTCDD4=1;
  PTCDD_PTCDD5=1; 
  PTCD_PTCD4 = 0;
  PTCD_PTCD5 = 0;
  
  /* Put Accelerometer in Sleep Mode */
  PTCDD_PTCDD3=1;
  PTCD_PTCD3=0;
}



/*******************************************************************/
/*!
 * Perform an IIR filter on X, Y, and Z axis
 */
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;
}

/*******************************************************************/
/*!
 * Get Average from ADC outputs
 * Calculates ADC value for x, y, and z axis.
 */
void avg_data()
{
  byte j;
  long x_avg=0, y_avg=0, z_avg=0;
  
  for(j=1;j<max;j++) 
  {  
    // Read acceleration data
    x_avg +=  (dword)(ReadAcceleration(0x04)<<4);  // Select ADC4 (x-axis) channel
    y_avg +=  (dword)(ReadAcceleration(0x05)<<4);  // Select ADC5 (y-axis) channel 
    z_avg +=  (dword)(ReadAcceleration(0x06)<<4);  // Select ADC6 (z-axis) channel
  }
 
  x.result[0] = (word)(x_avg>>4);
  y.result[0] = (word)(y_avg>>4);
  z.result[0] = (word)(z_avg>>4); 
}

/*******************************************************************/
/*!
 * Read Values from Accelerometer
 * @return ADC Value
 */
word ReadAcceleration(byte channel)
{
  word adc;
  ADCSC1 = channel; 
  while (!(ADCSC1_COCO)){}              // Waits until ADC conversion is completed

  adc=ADCR;
  return adc;
   
}



/*******************************************************************/
/*!
 * Demo to see how hard you can shake the board from side to side
 */
void ShakeDemo() 
{
  int i;
  byte count=0;
  dword cur=0;
  byte result;

  high=0;
  PTCD_PTCD4 = 1;  /* Set for 6G Sensitivity */
  PTCD_PTCD5 = 1; 

  calibrate();
  AccelOn(Range6g);
  
  LEDScrollTouch("1.. 2.. 3.. SHAKE IT!");

  /* Take 20000 samples */
  for(i=0;i<20000;i++) 
  {
    cur=ReadAcceleration(0x05);  /* Select ADC5 (x-axis) channel */
    
    /* See if reached new high value */
    if(cur>high)
      high=cur;
   
    /* Display bar */ 
    result=(high-0x800)/128;
    PTAD=0x1F; 
    if(result>8) 
    {
      PTED=~(0xFF);
      PTDD=~(0xFF>>(16-result));
    } 
    else
    {
      PTED=~(0xFF>>(8-result));
      PTDD=~(0x00);
    }
  }
 
  PTCD_PTCD4 = 0;  /* Set accelerometer for 1.5G (Lowest Power) */
  PTCD_PTCD5 = 0; 
  
  AccelOff();  

  LEDScroll("Results: ");

  if(high==0xfff) 
  {
    LEDScroll("SHAKE MASTER!");
  }
  else if(high>0xf00) 
  {
    LEDScroll("Everything is blurry!");
  }   
  else if(high>0xd00) 
  {
    LEDScroll("Getting dizzy");
  }   
  else if(high>0xb00) 
  {
    LEDScroll("Not too bad");
  }   
  else if(high>0x900) 
  {
    LEDScroll("Did something move?");
  }
  else 
  {
    LEDScroll("That was really weak");
  }      
}

/*******************************************************************/
/*!
 * Demonstrate the Accelerometer
 * Moves dot around on the LED matrix in response to tilting the board left or right
 * or forward and back
 * 
 * Also determines if board is flat or upright when Demo starts and adjusts
 * how tilting moves the dot based on that
 */
void TiltDemo()
{
  calibrate();

  LEDScroll("Tilt");

  /* Continue until E8 is pressed */
  while(LastKeyPressed()!=8) 
  {
    AccelOn(Range15g); 
  
    /* Get current position */  
    avg_data();  
    
    AccelOff();   
     
    /* Badge held Flat */
    if(z.init<21000) 
    {
      position_flat();
    }
    /* Badge held straight up */ 
    else if (x.init<21000) 
    {
      position_upright();     
    }
    /* 
     * Badge held sideways or odd direction. 
     * Default to flat mode 
     */
    else 
    {
      x.init=34000;
      y.init=34000;
      position_flat();     
    }
 
    /* Output dot */
    PTAD  = (byte) y_output;
    
    if(x_output>0x0080) 
    {
      PTED = ~(0x00);  
      PTDD  = (byte) ~(x_output>>8); 
    }
    else 
    {
      PTDD = ~(0x00);  
      PTED = (byte) ~(x_output);  
    }
  
    /* Do a Delay so LED dot moves slow */
    
    /* Turn on RTC Clock */
    SCGC2_RTC=1;
  
    /* Turn on RTC Interrupt for .1 seconds */
    rtc_init(0xD);
    
    /* Go to sleep */
    asm (stop #0x2000);
    
    /* Turn off RTC Interrupt */      
    RTCSC  = 0x00;
  
    /* Turn off RTC Clock */
    SCGC2_RTC=0;
  }
}

/*******************************************************************/
/*!
 * Get default values from accelerometer
 */
void calibrate() 
{   
  AccelOn(Range15g); 
  
  avg_data(); 
  
  AccelOff(); 
  
  x.init=x.result[0];
  y.init=y.result[0];
  z.init=z.result[0];

  /* Initizalize variables for TiltDemo */
  y_output=0x0004;
  x_output=0x0080;  
}



/*******************************************************************/
/*!
 * Detect tilting in the X-direction to move mouse 
 */
byte MouseX(dword xinit)
{
  y.init=xinit;

  AccelOn(Range15g); 
  
  /* Get current position */
  avg_data();  
  
  AccelOff();   
     
  /* If tilting far right move mouse fast to right */ 
  if(y.result[0]<(y.init-(2*SENSITIVITY))) 
  { 
    x_output=8;
  }   
  /* If tilting slightly right move mouse slowly to right */   
  else if(y.result[0]<(y.init-SENSITIVITY)) 
  { 
      x_output=1;
  }  
  /* If tilting far left move mouse fast to left */    
  else if(y.result[0]>(y.init+(2*SENSITIVITY)))
  {
     x_output=-8;
  }
  /* If tilting slightly left move mouse slowly to left */         
  else if(y.result[0]>(y.init+SENSITIVITY))
  {
     x_output=-1;
  }
  /* If not tilting than don't move mouse */       
  else 
  {
    x_output=0;
  }
  return x_output;
}

/*******************************************************************/
/*!
 * Detect tilting in the Y-direction to move mouse 
 */
byte MouseY(dword yinit)
{
  x.init=yinit;

  AccelOn(Range15g); 
  
  /* Get current position */
  avg_data();  
   
  AccelOff();   
    
  /* If tilting far up, move mouse fast up */  
  if(x.result[0]<(x.init-(2*SENSITIVITY))) 
  { 
    y_output=8;
  }
  /* If tilting slightly up, move mouse slowly up */  
  else if(x.result[0]<(x.init-SENSITIVITY)) 
  { 
    y_output=1;
  }
  /* If tilting far down, move mouse fast down */     
  else if(x.result[0]>(x.init+(2*SENSITIVITY)))
  {
     y_output=-8;
  }
  /* If tilting slightly down, move mouse slowly down */        
  else if(x.result[0]>(x.init+SENSITIVITY))
  {
     y_output=-1;
  }
  /* If not tilting than don't move mouse */          
  else 
  {
    y_output=0;
  }
  return y_output;
}


/*******************************************************************/
/*!
 * Detect tilting when the board is laid flat
 */
void position_flat() 
{
  unsigned long x_min, x_max, y_min, y_max;

  y_min = y.init-SENSITIVITY;
  y_max = y.init+SENSITIVITY;  
  x_min = x.init-SENSITIVITY;
  x_max = x.init+SENSITIVITY; 
  
  /* Check to see if need to shift dot on the x-axis on LED 
   *  (y-axis output from accelerometer) 
   */
  if(y.result[0]<y_min) 
  { 
    /* If tilting to left move LED to the left */
    if(!(x_output&0x8000)) 
    {
      /* Shift over if not reached edge of LED */
      x_output=(word)(x_output<<1);
    }   
  } 
  else if(y.result[0]>y_max)
  {
    /* If tilting to right move LED to the right */
    if(!(x_output&0x0001)) 
    {
      /* Shift over if not reached edge of LED */
      x_output=(word)(x_output>>1);
    } 
  }
    
  /* Check to see if need to shift dot on the y-axis on LED 
   *  (x-axis output from accelerometer) 
   */
  if(x.result[0]<x_min) 
  { 
    /* If tilting down move LED down */    
    if(!(y_output&0x0010)) 
    {
      /* Shift over if not reached edge of LED */
      y_output=(byte)(y_output<<1);
    }   
  }
  else if(x.result[0]>x_max)
  {
    /* If tilting up move LED up */
    if(!(y_output&0x0001)) 
    {
      /* Shift over if not reached edge of LED */
      y_output=(byte)(y_output>>1);
    } 
  }    
}

/*******************************************************************/
/*!
 * Detect tilting when the board is held upright
 */
void position_upright() 
{
  unsigned long z_min, z_max, y_min, y_max;
  
  z_min = z.init-SENSITIVITY;
  z_max = z.init+SENSITIVITY;  
  y_min = y.init-SENSITIVITY;
  y_max = y.init+SENSITIVITY; 
  
  /* Check to see if need to shift dot on the x-axis on LED 
   *  (y-axis output from accelerometer) 
   */
  if(y.result[0]<y_min) 
  { 
    /* If tilting to left move LED to the left */
    if(!(x_output&0x8000)) 
    {
      /* Shift over if not reached edge of LED */
      x_output=(word)(x_output<<1);
    }   
  } 
  else if(y.result[0]>y_max)
  {
    /* If tilting to right move LED to the right */
    if(!(x_output&0x0001)) 
    {
      /* Shift over if not reached edge of LED */
      x_output=(word)(x_output>>1);
    } 
  }
    
  /* Check to see if need to shift dot on the y-axis on LED 
   *  (z-axis output from accelerometer) 
   */
  if(z.result[0]<z_min) 
  { 
    /* If tilting down move LED down */
    if(!(y_output&0x0001)) 
    {
      /* Shift over if not reached edge of LED */
      y_output=(byte)(y_output>>1);
    }   
  }
  else if(z.result[0]>z_max)
  {
      /* If tilting up move LED up */    
    if(!(y_output&0x0010)) 
    {
      /* Shift over if not reached edge of LED */
      y_output=(byte)(y_output<<1);
    } 
  }    
}


/*******************************************************************/
/*!
 * Get accelerometer out of sleep mode, turn on ADC clock,
 * and configure ADC
 */
void AccelOn(char sensitivity)
{
  int i;

  switch(sensitivity) 
  {
    case Range15g:
      PTCD_PTCD4 = 0;  
      PTCD_PTCD5 = 0; 
      break;
    case Range2g:
      PTCD_PTCD4 = 0;  
      PTCD_PTCD5 = 1; 
      break;
    case Range4g:
      PTCD_PTCD4 = 1;  
      PTCD_PTCD5 = 0; 
      break;
    case Range6g:
      PTCD_PTCD4 = 1;  
      PTCD_PTCD5 = 1; 
      break;
    default:
      PTCD_PTCD4 = 0;  
      PTCD_PTCD5 = 0; 
      break;                  
  }
  
  /* Wake up accelerometer */
  PTCD_PTCD3=1;
  
  /* Turn on ADC Clock */
  SCGC1_ADC=1;
   
  /* Configure ADC since reset when turned clock off */
  // Bus clock as clock source, 12-bit conversion and divisor=4
  ADCCFG = 0x44;   
  
  /* Delay to allow time for accel chip to wake up */
  for(i=0;i<3000;i++) 
  { 
    asm("nop");
  }  
}

/*******************************************************************/
/*!
 * Put accelerometer into sleep mode and turn off ADC clock
 */
void AccelOff()
{
  PTCD_PTCD4 = 0;  
  PTCD_PTCD5 = 0; 

  /* Put accelerometer into sleep mode */
  PTCD_PTCD3=0; 
  
  /* Turn off ADC Clock */ 
  SCGC1_ADC=0;
}