// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>

const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ;
int16_t samplingRate = 20 ;
int16_t invalid_variance = 2000;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(2400);
}

int16_t smoothX[2]={0,0},smoothY[2]={0,0},smoothZ[2]={0,0} ;

int16_t smooth(int16_t nVal , int16_t* arr){
  int16_t res = ( arr[0] + arr[1] +  nVal ) / 3 ;
  arr[0] = arr[1] ;
  arr[1] = nVal ;
  return res ;
}

const int16_t siz = 50 ;

int16_t x[siz],y[siz],z[siz] ;
int16_t pointer = 0 , steps = 0 ;

int16_t variance(int16_t* arr){
  int16_t maxi = arr[0] , mini = arr[0] ;
  for(int i=0;i<siz ;i++){
    maxi = max (maxi , arr[i]);
    mini = min (mini , arr[i]);
  }
  return abs(maxi - mini) ;
}


int16_t threshold(int16_t* arr){
  int16_t maxi = arr[0] , mini = arr[0] ;
  for(int i=0;i<siz ;i++){
    maxi = max (maxi , arr[i]);
    mini = min (mini , arr[i]);
  }
  return (maxi + mini) / 2 ;
}


int16_t cal_steps(int16_t* arr){
  int16_t thres = threshold(arr) , counter = 0 ;
  for(int i=1 ; i<siz ; i++){
    if(arr[i-1] - arr[i] > 0 && arr[i-1] > thres && arr[i] < thres){
       counter++ ;
    }
  }
  return counter ;
}

int16_t getSteps(){
  int varx , vary , varz ;
  varx = variance(x) ;
  vary = variance(y) ;
  varz = variance(z) ;

  int step_x = cal_steps(x);
  int step_y = cal_steps(y);
  int step_z = cal_steps(z);

  if(varx < invalid_variance && vary < invalid_variance && varz < invalid_variance){
    return 0;
   }

  return min(min(step_x, step_y), step_z);



  /*if(varx >= vary && varx >= varz){

    if(abs(varx)<invalid_variance){return (int16_t)0;}
    Serial.println("direction x");
    return cal_steps(x) ;
  }else if(vary >= varz && vary >= varx){

    if(abs(vary)<invalid_variance){return (int16_t)0;}
    Serial.println("direction y");
    return cal_steps(y) ;
  }else if(varz >= varx && varz >= vary){

    if(abs(varz)<invalid_variance){return (int16_t)0;}
    Serial.println("direction z");
    return cal_steps(z) ;
  }
  return 0;*/
}



void loop(){

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

  int16_t smoothValX = smooth(AcX,smoothX) ;
  int16_t smoothValY = smooth(AcY,smoothY) ;
  int16_t smoothValZ = smooth(AcZ,smoothZ) ;

  x[pointer] = smoothValX ;
  y[pointer] = smoothValY ;
  z[pointer++] = smoothValZ ;

  if(pointer >= siz){
    pointer = 0 ;
    int16_t tmp = getSteps() ;
    //Serial.println(tmp);
    steps+=tmp;
    Serial.println(steps) ;
  }

  /*Serial.print(smoothValX);
  Serial.print("\t");
  Serial.print(smoothValY);
  Serial.print("\t");
  Serial.println(smoothValZ);*/

 delay(samplingRate);
}