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

//LSM303 compass;
int running_min[3], running_max[3], last_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 badFlag;

int dynamicPrecision, dynamicThreshold;


void Init(){
  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 < 3; ++i){
    compass.read();

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

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

    running_min[0] = min(running_min[0], xSamples[i]);
    running_min[1] = min(running_min[1], ySamples[i]);
    running_min[2] = min(running_min[2], zSamples[i]);

    running_max[0] = max(running_max[0], xSamples[i]);
    running_max[1] = max(running_max[1], ySamples[i]);
    running_max[2] = max(running_max[2], zSamples[i]);
  }

  idx = 3;

  samplingCounter = 50;
}

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;

  // Not sure about the following, we may need dynamic precision for each axis separately.
  // Assuming dynamicPrecision is the maximum of 10% of each peak to peak value of the axes.
  dynamicPrecision = max(p2pX, max(p2pY, p2pZ)) / 10;

  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 = max(avgX,max(avgY,avgZ));


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

// i think we can make dynamicPrecision and dynamicThreshold is vector

bool check_valid_step(int max_change) {
  if(abs(last_a[0]-(int)compass.a.x) == max_change && last_a[0] > dynamicThreshold && compass.a.x < dynamicThreshold)
    return true;

  if(abs(last_a[0]-(int)compass.a.y) == max_change && last_a[1] > dynamicThreshold && compass.a.y < dynamicThreshold)
    return true;

  if(abs(last_a[0]-(int)compass.a.z) == max_change && last_a[2] > dynamicThreshold && compass.a.z < dynamicThreshold)
    return true;

   return false;
}
bool checkStep(){
   compass.read();


    int max_change = max( abs(last_a[0]-(int)compass.a.x)  , max( abs(last_a[1]-(int)compass.a.y) ,  abs(last_a[2]-(int)compass.a.z) ));
   if(max_change < dynamicPrecision) return false;


   steps += check_valid_step();

  last_a = compass.a;

  running_min[0] = min(running_min[0], last_a[0]);
  running_min[1] = min(running_min[1], last_a[1]);
  running_min[2] = min(running_min[2], last_a[2]);

  running_max[0] = max(running_max[0], last_a[0]);
  running_max[1] = max(running_max[1], last_a[1]);
  running_max[2] = max(running_max[2], last_a[2]);
  return true;

}

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

  Init();
}

void loop(){
  if(samplingCounter == 50)
    reInit();

  samplingCounter += checkStep();
}