/*****************************************************************************
 * wearable.c
 * Lab 6: Final Project
 * ECE 476: Digital Systems Design Using Microcontrollers
 * Cornell University
 * April 2008
 * Andrew Chin (hc454) and Ping-Hong Lu (pl328)
 ****************************************************************************/

/*****************************************************************************
* This program will periodically take a sample from the accelerometers and 
* then listen for a short while for a command from the base station.
*
*****************************************************************************/
#include <mega644.h>
#include <spi.h>
#include <at86rf230.h>
#include <kxp74.h>
#include <stdio.h>

#define SAMPLE_TIMEOUT 10
#define SIMPSON_THRESHOLD 11000

unsigned char sample_timer;

unsigned char led;
unsigned int led_toggle_count;

unsigned char x_step_began;
unsigned char z_step_began;
unsigned char x_step_done;
unsigned char z_step_done;
unsigned char step_count;
unsigned char step_lockout_timer;

unsigned char step_state;

int accumulated_x;
unsigned char meter_count;
unsigned char integration_samples[3];
unsigned char three_count;
int simpsons;

int pace_counter1;
int pace_counter2;
long padding;
unsigned char calibration_done;
unsigned int calibration_count;
unsigned int calibration_total;
unsigned char default_x;
unsigned char send_count;

unsigned int timer_count;
unsigned char speed;
unsigned char calculated_speed;

unsigned char every_other_step;
unsigned char half_step;



//State Machine States
#define NO_STEP         0
#define X_START         1
#define Z_START         2
#define X_DONE          3
#define Z_DONE          4
#define X_START_Z_DONE  5
#define Z_START_X_DONE  6
#define X_START_Z_START 7
#define STEP_DETECTED   8
/****************************************************************************/
// This interrupt runs when a compare-match occurs every 1 ms.  It decrements
// the sample_timer global variable which is then used to determine when the
// "sample" task runs.

interrupt [TIM0_COMPA] void ms_timer(void)  {  //Triggers once per ms
  if (sample_timer > 0) sample_timer--;

  timer_count++;
}

/****************************************************************************/
// This task runs every 15 ms and takes readings from the accelerometer and 
// makes interpretations of the user's walking/running movements.  The 
// distance calculations, which utilize the Simpson's rule, occur after every 
// three samples, with the last sample becoming the first sample point of the
// next set.  A finite state machine determines discrete steps based on the x
// and z accelerations going above and below certian thresholds.  The distance, 
// steps, and speed information is sent to the basestation every time sample runs.  

void sample(void)  {
  unsigned char x,y,z;

  set_sensor_clock();  //Set clock phase & polarity for accelerometers
  tx_frame_length = 0;

  // Take 313 cycles to calibrate the default x sensor reading
  if (calibration_done == 0 && calibration_count != 312)
  {
    x = get_sensor(1, CONVERT_XAXIS);
    calibration_total += x;
    calibration_count++;
    
    // After taking the final calibration sample, take the average and
    // set calibration flag as done
    if (calibration_count == 312)
    {
      default_x = calibration_total / 312;
      calibration_done = 1; 
      PORTD.2 = 0;
    }
    return;
  }
  
  // Variable used to detect if data stack has overrun the alloted size
  if (padding != 0)
    printf("data corruption occurred\n\r");
  
//4/19  printf("calibrated x is: %d meter count is %d\n\r", default_x, meter_count);
//  printf("speed is %d timer count is: %d\r", speed, timer_count);


  if (step_lockout_timer > 0)
    step_lockout_timer--;

  x = get_sensor(1, CONVERT_XAXIS);
  //printf("x value is %d\n\r", x);
  
  // Update the triple set for doing simpson's rule calculation of area
  // under the curve
  integration_samples[three_count] = x;
  
  // On the third sample, calculate the simpsons
  if (three_count == 2)//x > 74 || x < 70
  {
    if ((integration_samples[0] > (default_x - 4) &&
         integration_samples[0] < (default_x + 3))&&
        (integration_samples[1] > (default_x - 4) &&
         integration_samples[1] < (default_x + 3))&&
        (integration_samples[2] > (default_x - 4) &&
         integration_samples[2] < (default_x + 3)))
    {
      // standing still with slight movements, ignore
    }
    else
    { 
      simpsons = (int)((int)integration_samples[0]-default_x + 
                      ((int)integration_samples[1]-default_x)* 4 +
                       (int)integration_samples[2]-default_x)* 16 / 3;
      // If the user is fast walking or running, scale the x readings up and
      // set the step detection to every other detection
      if (pace_counter1 < 50 && pace_counter1 > 0)
      {
        simpsons = simpsons * 1.35;
        every_other_step = 1;
      }
      else
      {
        simpsons = simpsons * 0.9;
        every_other_step = 1;
      }
      // Don't let an underflow occur from too many negative x readings            
      if ((accumulated_x + simpsons) > 0)
      {
        accumulated_x += simpsons;
      }
      // If the total x distance is enough to count as one step
      if (accumulated_x > SIMPSON_THRESHOLD)
      {
        accumulated_x = accumulated_x - SIMPSON_THRESHOLD;
        meter_count++;
        // save value
        calculated_speed = ((2237/timer_count) > 10 ? calculated_speed : ((char)(2237/timer_count)));
        // Increase speed incrementally to avoid unrealistic jumps from 
        // inaccurate speed calculations
        if (calculated_speed > speed)
          speed++;
        else if (calculated_speed < speed)
          speed--;
        else ;
        
        timer_count = 0;
        // Remove any erroneous x readings that cause the accumulated_x to go 
        // 2 x the threshold
        while (accumulated_x > SIMPSON_THRESHOLD) accumulated_x -= SIMPSON_THRESHOLD;
      }
    }
  }
  
  // Send the meters, step count, and speed over RF
  {
      send_count = 0;
      tx_frame_length = 3;
      transmit_frame[0] = meter_count;
      transmit_frame[1] = step_count;
      transmit_frame[2] = speed;
    
      set_transceiver_clock();
      if (RF_quick_listen() == 1)  {
        RF_upload_frame();
        if (receive_frame[0] == DATA_REQ)  {
          PORTD.7 = ~PORTD.7;
          RF_rx_to_tx();
          RF_download_frame();
          RF_transmit_frame();
          RF_wait_for_transmit();
        }
      }
      receive_frame[0] = 0;
      RF_tx_to_rx();
  }      
  send_count++;

  delay_us(1);

  y = get_sensor(1, CONVERT_YAXIS);

  delay_us(1);

  z = get_sensor(1, CONVERT_ZAXIS); 
  
  delay_us(1);

  //printf("x y z %d %d %d\n\r", x, y, z);
    
  // State machine for step detection
  switch (step_state)
  {
    // Look for breaking either the x or z threshold
    case NO_STEP:
      if (step_lockout_timer != 0)
      {
        break;
      }
      if (x > 90)
      { 
        step_state = X_START;
        break;
      }
      if (z < 75)
      {
        step_state = Z_START;
        break;
      }
      break;
    // Look for finishing the x step or breaking the z threshold
    case X_START:
      if (x < 80)
      {
        step_state = X_DONE;
        break;
      }
      if (z < 75)
      {
        step_state = X_START_Z_START;
        break;
      }
      break;   
    // Look for finishing the z step or breaking the x threshold
    case Z_START:
      if (z > 80)
      {
        step_state = Z_DONE;
        break;
      }
      if (x > 90)
      { 
        step_state = X_START_Z_START;
        break;
      }
      break;  
    // Look for finishing the x or z steps
    case X_START_Z_START:
      if (x < 80)
      {
        step_state = Z_START_X_DONE;
        break;
      }
      if (z > 80)
      {
        step_state = X_START_Z_DONE;
        break;
      }
      break;    
    // Look for finishing the x step
    case X_START_Z_DONE:
      if (x < 80)
      {
        step_state = STEP_DETECTED;
        break;
      }
      break;    
    // Look for finishing the z step
    case Z_START_X_DONE:
      if (z > 80)
      {
        step_state = STEP_DETECTED;
        break;
      }    
      break;  
    // Look for breaking either the z threshold
    case X_DONE:
      if (z < 75)
      {
        step_state = Z_START_X_DONE;
        break;
      }
      break;    
    // Look for breaking either the x threshold
    case Z_DONE:
      if (x > 90)
      { 
        step_state = X_START_Z_DONE;
        break;
      }      break;  
    // Both axis done, increment whole or half step depending on speed.
    case STEP_DETECTED:
      if (every_other_step == 1)
      {
        if (half_step == 1)
        {
          step_count++;
          half_step = 0;
        }
        else
          half_step = 1;
      }
      else
        step_count++;
      
      step_state = NO_STEP;
      step_lockout_timer = 9;
      pace_counter1 = pace_counter2;
      pace_counter2 = 0;
      break; 
    default: 
      break;
  }


//  printf("pace counter = %d\n\r", pace_counter1);

  pace_counter2++;
  three_count++;
  if (three_count == 3)
  {
    integration_samples[0] = integration_samples[2];
    three_count = 1;
  }

/*
  if (led_toggle_count++ == 8)
  {
    led_toggle_count = 0;
    if (led == 0)
    {
      led = 1;
      PORTD.2 = 0;
    }             
    else
    {
      led = 0;
      PORTD.2 = 1;
    }
  }
*/
}

/****************************************************************************/
// This function correctly initializes the necessary control registers and 
// I/O ports as well as initializes global variables to zero.  Lastly, it
// enables all interrupts.

void initialize(void) {
  DDRD.2 = 1;
  PORTD.2 = 1;

  DDRD = 0xFF;
  DDRA = 0xFF;
  PORTA = 0x00;
  PORTD.7 = 1;
  sample_timer = 0;

  // Initialize Mega64's SPI settings
  init_spi();
  // Initialize accelerometer's SPI
  init_sensor_spi();
  set_sensor_clock();
  // Get accelerometers in proper mode
  init_sensors();
  // Initialize transceiver SPI
  RF_init_spi();
  delay_us(100);
  set_transceiver_clock();
  // Put transceiver in receiver mode
  RF_init_transmitter();

  sample_timer = SAMPLE_TIMEOUT;
  TCCR0A = 0b00000010;  //OC0A & OC0B disconnected, WGM set up for CTC mode
  TCCR0B = 0b00000011;  //Timer set up to osc/64... count to 125 for 1 ms
  OCR0A = 125;
  // Interrupt on compare match A
  TIMSK0 = 0b00000010;

  led = 0;
  led_toggle_count = 0;
  x_step_began = 0;
  z_step_began = 0;
  x_step_done = 0;
  z_step_done = 0;
  step_count = 0;
  step_lockout_timer = 0;
  integration_samples[0] = 0;
  integration_samples[1] = 0;
  integration_samples[2] = 0;
  three_count = 0;

  step_state = NO_STEP;
  
  accumulated_x = 0;
  meter_count = 0;
  
  calibration_done = 0;
  calibration_count = 0;
  calibration_total = 0;
  default_x = 0;

  padding = 0;
  pace_counter1 = 0;
  pace_counter2 = 0;
  send_count = 0;
  
  timer_count = 0;
  speed = 0;
  calculated_speed = 0;

  every_other_step = 0;  
  half_step = 0;

  //set up UART
  UCSR0B = 0x18 ;
  UBRR0L = 51 ;
  printf("starting...\n\r") ;

  //Enable interrupts
  #asm
    sei
  #endasm
}

/****************************************************************************/
// The main function starts by running a delay to allow the other components 
// time to reach a steady state.  It then calls the initialization function
// and runs whenever the interrupt has counted down the sample_timer variable
// to zero.

void main(void)  {
  delay_ms(2000);
  initialize();

  while(1)  {
    if (sample_timer == 0)  {
      sample_timer = SAMPLE_TIMEOUT;
      sample();
    }
  }

}

/****************************************************************************/