#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;
#define MPU 0x68  //Dirección I2C del IRS

//Conversiones
#define A_R 16384.0 
#define G_R 131.0

#define RAD_A_DEG 180/PI //Convertir de Radián a Grados

int16_t AcX, AcY, AcZ, GyX, GyY, GyZ; //Valores sin convertir (RAW)

//Angulos
float Acc[2];
float Gy[3];
double Angle;

//Tiempos
int tiempo_prev;
float dt;

//Setup
void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(115200); //9600bps si necesario
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  Serial.println("");
  delay(100);
}

double v0 = 0;
double v;
double acc;

void loop() {
  //Leer los valores del IRS
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); //0x3B -> AcX
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); //Coger 6 registros del AcX
  AcX = Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
  AcY = Wire.read()<<8|Wire.read();
  AcZ = Wire.read()<<8|Wire.read();

  //A partir del acelerómetro se calculan los ángulos
  Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R), 2)+ pow((AcZ/A_R), 2)))*RAD_TO_DEG;
  Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R), 2)+ pow((AcZ/A_R), 2)))*RAD_TO_DEG;

  //Leer los valores del Gyro
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);

  GyX = Wire.read()<<8|Wire.read();
  GyY = Wire.read()<<8|Wire.read();
  GyZ = Wire.read()<<8|Wire.read();

  //Calculo del angulo del gyro
  Gy[0] = GyX/G_R;
  Gy[1] = GyY/G_R;
  Gy[2] = GyZ/G_R;

  dt = (millis() - tiempo_prev) / 1000.0;
  tiempo_prev = millis();

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  acc = a.acceleration.z -9.81;
  
  v = round(v0 + acc*dt + 0.0055);
  v0 = v;

 

  //Fitro
  //Angle[0] = 0.98 * (Angle[0]+Gy[0]*dt) + 0.02*Acc[0];
  //Angle[1] = 0.98 * (Angle[1]+Gy[1]*dt) + 0.02*Acc[1];
  //1.99720

  //Integral respecto tiempo para calcular HEADING
  Angle = Angle +Gy[2]*dt;
  Angle = Angle*1.99820; //COMPENSACIÓN ARREGLAR
  int hdg = (int) Angle;
  String valores = "HEADING: " + String(hdg) + "º";
  Serial.println(valores); 
  Serial.print(v);
  Serial.println(" m/s");
  delay(1000);
}