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

LSM303 compass;
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;

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;
    }
  }
  return true;
}

void setup(){
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();

  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);
  delay(100);
}