#include "FastIMU.h"
#include <Wire.h>

#define IMU_ADDRESS 0x69  // Dirección de la IMU, puede ser 0x68 o 0x69
#define I2C_SDA 21        // Pin SDA para la IMU
#define I2C_SCL 22        // Pin SCL para la IMU

#define PWMA 25
#define PWMB 26
#define MTTR_N 27 //izquierdo
#define MTTR_P 32 // izquierdo
#define STD_BY 2
#define MTTL_N 33 //derecho
#define MTTL_P 14 //derecho

// Definición de canales PWM
int estado=0;
const int pwmChannelA = 1;
const int pwmChannelB = 0;
const int pwmFreq = 5000; // Frecuencia PWM
const int pwmResolution = 8; // Resolución de 8 bits (0-255)
float b = 0;
float aux = 0;
unsigned long previousMillis = 0;
const long interval = 45; // Intervalo de tiempo en milisegundos para la actualización de PWM
bool cr = 1;
unsigned long currentMillis = 0;

unsigned long previousMillis1 = 0;
unsigned long currentMillis1 = 0;

MPU6500 IMU; // Cambiar al nombre de cualquier IMU soportado

// Variables para almacenamiento de datos
AccelData accelData;
GyroData gyroData;

// Variables para cálculos de velocidad y posición
float velocidad_resultado;
float aceleracion;

float velocidad_actual = 0.0;
unsigned long tiempo_anterior = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin(I2C_SDA, I2C_SCL); // Inicializa el bus I2C con pines específicos
  Wire.setClock(400000);        // Reloj I2C de 400 kHz
  Serial.println("Inicializando el MPU6500...");

  // Inicializa el sensor
  calData calib;
  int err = IMU.init(calib, IMU_ADDRESS);
  if (err != 0) {
    Serial.print("Error initializing IMU: "); // Reporta error de inicialización
    Serial.println(err);                      // de la IMU
    while (true) {
      // Añade un parpadeo LED o algún otro indicador para mostrar que hay un problema.
    }                             
  }

  Serial.println("MPU6500 inicializado correctamente!");

  // Configuración adicional si es necesaria
  err = IMU.setAccelRange(2); // Rango del Gyro ±500 DPS y del acelerómetro ±2g
  if (err != 0) {
    Serial.print("Error Setting range: "); // Reporta error de definición
    Serial.println(err);                   // de rango del acelerómetro
    while (true) {
      // Añade un parpadeo LED o algún otro indicador para mostrar que hay un problema.
  }                          
  }


  pinMode(MTTR_P, OUTPUT);
  pinMode(MTTR_N, OUTPUT);
  pinMode(STD_BY, OUTPUT);
  pinMode(MTTL_P, OUTPUT);
  pinMode(MTTL_N, OUTPUT);

  // Configuración de canales PWM
  ledcSetup(0, 5000, 8);
  ledcSetup(1, 5000, 8);

  // Asignación de canales PWM a los pines
  ledcAttachPin(PWMA, pwmChannelA);
  ledcAttachPin(PWMB, pwmChannelB);


  // Configuración de los pines para avanzar
  digitalWrite(STD_BY, HIGH); // se deshabilita el driver
  digitalWrite(MTTR_P, HIGH);
  digitalWrite(MTTR_N, LOW);
  digitalWrite(MTTL_P, LOW);
  digitalWrite(MTTL_N, HIGH); // Con esta combinación de bits el motor gira CW, ósea hacia atrás
  digitalWrite(STD_BY, LOW); // Después de cambiar el modo se habilita el movimiento


  

    
}

void loop() {
  currentMillis = millis();
  currentMillis1 = millis();
  // Incremento y decremento gradual de PWM
  IMU.update();
  IMU.getAccel(&accelData);
  IMU.getGyro(&gyroData);

switch (estado) {
    // Caso 0: ARRANQUE
    case 0:
     if (currentMillis - previousMillis >= 1000) {
    previousMillis = currentMillis;
 Serial.println("estado0");
  aux=0;
   estado = 1;
  }
      break;

    case 1:

    
    if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
      arranque();
    
      }   
    if (currentMillis1 - previousMillis1 >= 5000) { 
    previousMillis1 = currentMillis1;

    aceleracion = (accelData.accelX)*977.86;
    velocidad_resultado = calcularVelocidad(aceleracion);

    
    estado = 0; 
    Serial.println(velocidad_resultado);
   }

      break;

    case 2:
    

     if (currentMillis - previousMillis >= 10000) {
        previousMillis = currentMillis;
       Serial.println("estado2");
       
       estado=3;
       
    
      }

      break;

    
    // Caso 3: velocidad de parada
    case 3:
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        parada();
    
      }
  
      break;

    default:
      // Código por defecto
      
      break;
  
}

}

void arranque() {
   
      aux=5;

     
    if (cr){
    ledcWrite(pwmChannelA, 57);
    ledcWrite(pwmChannelB, 47);
    cr=0;
    }
    else{
    ledcWrite(pwmChannelA, 54);
    ledcWrite(pwmChannelB, 51);
    cr=1;
        }
    
}

void parada() {
  
  aux--;
  if ( aux < 1){estado=0;
  ledcWrite(pwmChannelA, 0);
    ledcWrite(pwmChannelB, 0);}
  if(aux<4){
    
     
    if (cr){
    ledcWrite(pwmChannelA, 47);
    ledcWrite(pwmChannelB, 37);
    cr=0;
    }
    else{
    ledcWrite(pwmChannelA, 44);
    ledcWrite(pwmChannelB, 41);
    cr=1;
        }
    }
  
  if(aux<3){
    
     
    if (cr){
    ledcWrite(pwmChannelA, 37);
    ledcWrite(pwmChannelB, 27);
    cr=0;
    }
    else{
    ledcWrite(pwmChannelA, 34);
    ledcWrite(pwmChannelB, 31);
    cr=1;
        }
    }
  
  if(aux<2){
    
     
    if (cr){
    ledcWrite(pwmChannelA, 27);
    ledcWrite(pwmChannelB, 17);
    cr=0;
    }
    else{
    ledcWrite(pwmChannelA, 24);
    ledcWrite(pwmChannelB, 21);
    cr=1;
        }
    }
  
  if(aux<1){
    
     
    if (cr){
    ledcWrite(pwmChannelA, 0);
    ledcWrite(pwmChannelB, 0);
    cr=0;
    }
    else{
    ledcWrite(pwmChannelA, 0);
    ledcWrite(pwmChannelB, 0);
    cr=1;
        }
    }
  
}





float calcularVelocidad(float aceleracion) {
  // Obtén el tiempo actual
  unsigned long tiempo_actual = millis();

  // Calcula el cambio en el tiempo en segundos
  float delta_tiempo = (tiempo_actual - tiempo_anterior) / 1000.0;  // Convierte a segundos

  // Calcula el cambio en la velocidad usando la aceleración actual
  float delta_velocidad = aceleracion * delta_tiempo;

  // Actualiza la velocidad actual
  velocidad_actual = delta_velocidad;

  // Actualiza el tiempo anterior para la próxima iteración
  tiempo_anterior = tiempo_actual;

  return velocidad_actual;
}



int alertaAceleracion(float aceleracion) {
  return (aceleracion < -15) ? 1 : 0;
}