// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU_addr=0x68 , arraySize = 100, threshold = 3900;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ, count = 0;
int averagex[arraySize] , averagey[arraySize] , averagez[arraySize];
int sumx = 0, sumy = 0, sumz = 0;
int passedThresholdx = 0, passedThresholdy = 0, passedThresholdz = 0, steps = 0;
bool enterx = false, entery = false, enterz = false;
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(9600);
  Serial.print("fady");
}
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)
  averagex[count % arraySize] = AcX;
  averagey[count % arraySize] = AcY;
  averagez[count % arraySize] = AcZ;
  count++;

  if (AcX > threshold && passedThresholdx == 0  && !entery && !enterz) {
    enterx = true;
    passedThresholdy++;
  } else if (AcX < threshold && passedThresholdx == 1 && enterx) {
    Serial.print (++steps);
    Serial.print("\n");
    passedThresholdx = 0;
  } else
  if (AcY > threshold && passedThresholdy == 0  && !enterx && !enterz) {
    entery = true;
    passedThresholdy++;
  } else if (AcY < threshold && passedThresholdy == 1 && entery) {
    Serial.print (++steps);
    Serial.print("\n");
    passedThresholdy = 0;
  }
  else if (AcZ > threshold && passedThresholdz == 0  && !entery && !enterx) {
    enterz = true;
    passedThresholdy++;
  } else if (AcZ < threshold && passedThresholdz == 1 && enterz) {
    Serial.print (++steps);
    Serial.print("\n");
    passedThresholdz = 0;
  }

  if(count >= arraySize){
    for (int i = 0; i < arraySize; i++) {
      sumx += averagex[i];
      sumy += averagey[i];
      sumz += averagez[i];
    }
//    Serial.print((sumx / arraySize));
//    Serial.print(" ");
//    Serial.print(sumy / arraySize);
//    Serial.print(" ");
//    Serial.print((sumz / arraySize));
//    Serial.print(" ");
//    Serial.print("\n");
    sumx = sumy = sumz = 0;
  }

  delay(50);
}