#include <Wire.h>
#include <LSM303.h>
#include <LiquidCrystal.h>

#define SIZE 50
#define WINDOW_SIZE 4

LSM303 compass;
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
int running_min[3], running_max[3], last_a[3], curr_a[3];

// The previous 4 readings, used to find the current reading which is their average.
// Use idx as an index.
int sumX, sumY, sumZ, idx;
int xSamples[4], ySamples[4], zSamples[4];

int samplingCounter, steps;
int pre_steps = 0;

double stride = 0;
double height = 170;
double steps_to_strides[] = {height/5.0, height/5.0, height/5.0, height/4.0, height/3.0, height/2.0, height/1.2, height, height, 1.2 * height};
double dist = 0;
double velocity = 0;
double calories = 0;
double weight = 60;

unsigned long prevMillis;

unsigned long time;

// Unused.
//int badFlag;

int dynamicPrecision[3], dynamicThreshold[3];

void reInit(){
  samplingCounter = 0;

  int p2pX = running_max[0] - running_min[0];
  int p2pY = running_max[1] - running_min[1];
  int p2pZ = running_max[2] - running_min[2];

  //badFlag = 0;

  dynamicPrecision[0] = p2pX / 10;
  dynamicPrecision[1] = p2pY / 10;
  dynamicPrecision[2] = p2pZ / 10;
  //Serial.print("Dynamic Precision : ");
  //Serial.println(dynamicPrecision);

  int avgX = (running_max[0] + running_min[0]) / 2;
  int avgY = (running_max[1] + running_min[1]) / 2;
  int avgZ = (running_max[2] + running_min[2]) / 2;

  dynamicThreshold[0] = avgX;
  dynamicThreshold[1] = avgY;
  dynamicThreshold[2] = avgZ;

  running_min[0] = running_min[1] = running_min[2] = 32767;
  running_max[0] = running_max[1] = running_max[2] = -32768;
}

bool check_valid_read() {
  return abs(sumX/4-curr_a[0]) >= dynamicPrecision[0] | abs(sumY/4-curr_a[1]) >= dynamicPrecision[1]
          | abs(sumZ/4-curr_a[2]) >= dynamicPrecision[2];
}

bool read(){
  sumX -= xSamples[idx];
  sumY -= ySamples[idx];
  sumZ -= zSamples[idx];

  compass.read();

  xSamples[idx] = compass.a.x;
  ySamples[idx] = compass.a.y;
  zSamples[idx] = compass.a.z;

  sumX += xSamples[idx];
  sumY += ySamples[idx];
  sumZ += zSamples[idx];

  if(!check_valid_read())
    return false;

  idx = (idx + 1) % 4;

  for(int i = 0; i < 3; ++i)
    last_a[i] = curr_a[i];

  curr_a[0] = sumX / 4;
  curr_a[1] = sumY / 4;
  curr_a[2] = sumZ / 4;

  for(int i = 0; i < 3; ++i){
    running_min[i] = min(running_min[i], curr_a[i]);
    running_max[i] = max(running_max[i], curr_a[i]);
  }

  /*
  curr_a[0] = compass.a.x;
  curr_a[1] = compass.a.y;
  curr_a[2] = compass.a.z;
  */

  //Serial.println("readings");
  Serial.print(curr_a[0]);
  Serial.print(" ");
  Serial.print(curr_a[1]);
  Serial.print(" ");
  Serial.println(curr_a[2]);

  return true;
}

bool checkStep(){
  if(!read())
      return false;

  unsigned long currTime = millis();
  if(currTime - time < 200) return true;
  time = currTime;

  for(int i = 0; i < 3; ++i){
    int d = abs(last_a[i] - curr_a[i]);
    if(d >= dynamicPrecision[i] && last_a[i] > dynamicThreshold[i] && curr_a[i] < dynamicThreshold[i]){
       ++steps;
       break;
    }
  }
  if (millis() - prevMillis > 2000){
    prevMillis = millis();
    stride = steps_to_strides[min(9, steps - pre_steps)];
    dist += stride;
    velocity = (steps - pre_steps) * stride;
    if (velocity == 0){
      calories += weight / 1800.0; // calories burnt while resting
    }
    else {
      calories += velocity * weight / 400.0;
    }
    pre_steps = steps;
  }
  return true;
}

void setup(){

  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  lcd.begin(16,2);
  prevMillis = millis();

  running_min[0] = running_min[1] = running_min[2] = 32767;
  running_max[0] = running_max[1] = running_max[2] = -32768;
  for(int i = 0; i < 20; ++i) read();
  reInit();
}

void loop(){
  if(samplingCounter == 20) reInit();
  samplingCounter += checkStep();
  //Serial.print("steps :");
  //Serial.println(steps);

  lcd.setCursor(0,0);
  lcd.print("Cnt:");
  lcd.print(steps);
  lcd.print(' ');
  lcd.print("D:");
  lcd.print(dist);
  lcd.setCursor(0,1);
  lcd.print("V:");
  lcd.print(velocity);
  lcd.print(' ');
  lcd.print("C:");
  lcd.print(calories);

  delay(100);
}