#include <Wire.h>
#include <math.h>
const int MPU=0x68;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,vx,vy,vz,v,vt;

static double prev = 0;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(9600);
}
void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,12,true);
  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();
  GyX=Wire.read()<<8|Wire.read();
  GyY=Wire.read()<<8|Wire.read();
  GyZ=Wire.read()<<8|Wire.read();
  /*
  Serial.print("Accelerometer: ");
  Serial.print("X = "); Serial.print(AcX);
  Serial.print(" | Y = "); Serial.print(AcY);
  Serial.print(" | Z = "); Serial.println(AcZ); */
  vx += AcX/9.81;
  vy += AcY/9.81;
  vz += AcZ/9.81;
  v = sqrt(pow(vx, 2)+pow(vy, 2)+pow(vz, 2));
  vt = v + (0.9*(vt-v));
  Serial.print("\nVelocity: ");
  Serial.print(AcX);
  /*
  Serial.print("\nGyroscope: ");
  Serial.print("X = "); Serial.print(GyX);
  Serial.print(" | Y = "); Serial.print(GyY);
  Serial.print(" | Z = "); Serial.println(GyZ);*/

  //Serial.print("X Normalize: "); Serial.println(normalize(AcX));
  //Serial.print("Velocity: "); Serial.println(speed(AcX, AcY, AcZ));
  delay(10);
}

double speed(size_t AcX, size_t AcY, size_t AcZ){
    double speed;
    speed = sqrt(pow(AcX, 2)+pow(AcY, 2)+pow(AcZ, 2));
    if(prev!=speed){
      prev = speed - prev;
      return prev;
      prev = speed;
    }else return speed;
}