#ifndef SensorIMUModel
#define SensorIMUModel

#include "SerialService.h"
#include "I2CService.h"
#include <MPU9250_asukiaaa.h>
#include <Adafruit_BMP280.h>
#include <MahonyAHRS.h>
#include <Wire.h>

class SensorIMU {
  public:

    static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorIMU*>& lista, std::vector<uint8_t>& data, const String& Mod_ID);

    String Mod_ID;
    String _ID;
    int ID_Num;
    bool Iniciado = false;
    bool MpuIniciado = false;
    bool BmpIniciado = false;

    // Endereços I2C
    byte _EnderecoMPU; // 0x68
    byte _EnderecoBMP; // 0x76

    // MPU9250 (IMU 9 eixos)
    MPU9250_asukiaaa mpu;

    // BMP280 (Temperatura, Pressão, Altitude)
    Adafruit_BMP280 bmp;

    Mahony filter;
    int filterHz = 100;
    TickType_t filterDelay = pdMS_TO_TICKS(roundf(1000.0f / filterHz));

    // Leituras principais
    float AccX = 0, AccY = 0, AccZ = 0;
    float GyroX = 0, GyroY = 0, GyroZ = 0;
    float MagX = 0, MagY = 0, MagZ = 0;
    float Temp = 0;
    float Pressao = 0;
    float Altitude = 0;
    float Roll = 0;
    float Pitch = 0;
    float Yaw = 0;

    SensorIMU(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    void Inicializar() {
      if (Iniciado) {
        PrintTela(_ID + " ja inicializado");
        return;
      }

      if (!I2CService::SolicitarAcessoI2C(ID_Num)) return;

      MpuIniciado = I2CService::VerificaEnderecoBarramento(_EnderecoMPU);
      if (MpuIniciado) {
        mpu.setWire(&Wire);
        mpu.beginAccel();
        mpu.beginGyro();
        //mpu.beginMag();
        PrintTela("MPU9250 iniciado");
      } else {
        PrintTela("MPU9250 nao encontrado no endereco " + String(_EnderecoMPU));
      }
      
      bool BmpEncontrado = I2CService::VerificaEnderecoBarramento(_EnderecoBMP);
      if (BmpEncontrado) {
        BmpIniciado = bmp.begin(_EnderecoBMP);
        if (BmpIniciado) {
          bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
            Adafruit_BMP280::SAMPLING_X2,
            Adafruit_BMP280::SAMPLING_X16,
            Adafruit_BMP280::FILTER_X16,
            Adafruit_BMP280::STANDBY_MS_500);
          PrintTela("BMP280 iniciado");
        }
        else {
          PrintTela("Erro ao iniciar BMP280");
        }
      }
      else {
        PrintTela("BMP280 nao encontrado no endereco " + String(_EnderecoBMP));
      }

      Iniciado = MpuIniciado || BmpIniciado;
      if (Iniciado) {
        filterDelay = 1000.0f / filterHz;
        xTaskCreatePinnedToCore(&SensorIMU::IMUTaskWrapper, "IMUTask", 4096, this, 10, &IMUTaskHandle, tskNO_AFFINITY);
        PrintTela(_ID + " iniciado");
      }

      I2CService::LiberarAcessoI2C(ID_Num);
    }

    void Desligar() {
      if (!Iniciado) {
        PrintTela(_ID + " nao esta inicializado");
        return;
      }

      Iniciado = false;

      if (I2CService::QuemEstaUsando() == ID_Num) {
        while (I2CService::QuemEstaUsando() == ID_Num) {
          delay(10);
        }
      }

      // Parar a execução das tarefas
      if (IMUTaskHandle != NULL) {
        vTaskDelete(IMUTaskHandle);
        IMUTaskHandle = NULL;
      }

      PrintTela(_ID + " Desligado");
    }

    void RequisitarDados() {
      AferirDadosTMP();
    }

    std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
      std::vector<uint8_t> data;
      data.push_back(static_cast<uint8_t>(posicao));
      data.push_back(ID_Num);
      
      switch (posicao) {
        case CanMessagePosicaoDados::Status: {
          data.push_back(Iniciado ? 1 : 0);
          break;
        }

        case CanMessagePosicaoDados::Dados1: { // Roll, Pitch, Yaw
          int16_t roll = Roll * 100;
          int16_t pitch = Pitch * 100;
          int16_t yaw = Yaw * 100;

          data.push_back(roll >> 8);  data.push_back(roll & 0xFF);
          data.push_back(pitch >> 8); data.push_back(pitch & 0xFF);
          data.push_back(yaw >> 8);   data.push_back(yaw & 0xFF);
          break;
        }

        case CanMessagePosicaoDados::Dados2: { // Temp, Pressão, Altitude
          int16_t temp = Temp * 100;
          uint16_t pressao = Pressao / 10;        // Ex: 100000 Pa → 10000 (precisão: 10 Pa)
          int16_t altitude = Altitude / 10;
    
          data.push_back(temp >> 8); data.push_back(temp & 0xFF);
          data.push_back(pressao >> 8); data.push_back(pressao & 0xFF);
          data.push_back(altitude >> 8); data.push_back(altitude & 0xFF);
          break;
        }
    
        case CanMessagePosicaoDados::Dados3: { // Acc
          int16_t accX = AccX * 100;
          int16_t accY = AccY * 100;
          int16_t accZ = AccZ * 100;
    
          data.push_back(accX >> 8); data.push_back(accX & 0xFF);
          data.push_back(accY >> 8); data.push_back(accY & 0xFF);
          data.push_back(accZ >> 8); data.push_back(accZ & 0xFF);
          break;
        }
    
        case CanMessagePosicaoDados::Dados4: { // Gyro
          int16_t gyroX = GyroX * 100;
          int16_t gyroY = GyroY * 100;
          int16_t gyroZ = GyroZ * 100;
    
          data.push_back(gyroX >> 8); data.push_back(gyroX & 0xFF);
          data.push_back(gyroY >> 8); data.push_back(gyroY & 0xFF);
          data.push_back(gyroZ >> 8); data.push_back(gyroZ & 0xFF);
          break;
        }
    
        case CanMessagePosicaoDados::Dados5: { // Mag
          int16_t magX = MagX * 100;
          int16_t magY = MagY * 100;
          int16_t magZ = MagZ * 100;
    
          data.push_back(magX >> 8); data.push_back(magX & 0xFF);
          data.push_back(magY >> 8); data.push_back(magY & 0xFF);
          data.push_back(magZ >> 8); data.push_back(magZ & 0xFF);
          break;
        }
    
      }
    
      return data;
    }


  private:
    TaskHandle_t IMUTaskHandle = NULL;
    static void IMUTaskWrapper(void *pvParameters) {
      SensorIMU *sensor = static_cast<SensorIMU*>(pvParameters);
      sensor->IMUTask();
    }

    void IMUTask() {
      TickType_t xLastWakeTime = xTaskGetTickCount();

      while (1) {
        if (Iniciado) {
          AferirDadosIMU();
          vTaskDelayUntil(&xLastWakeTime, filterDelay);
        }
        else {
          vTaskDelay(1000);
        }
      }
    }

    void AferirDadosIMU() {
      if (!MpuIniciado) return;

      if (!I2CService::SolicitarAcessoI2C(ID_Num, -1, 5)) return;

      mpu.accelUpdate();
      mpu.gyroUpdate();
      mpu.magUpdate();

      AccX = mpu.accelX();
      AccY = mpu.accelY();
      AccZ = mpu.accelZ();

      GyroX = mpu.gyroX();
      GyroY = mpu.gyroY();
      GyroZ = mpu.gyroZ();

      /*MagX = mpu.magX();
      MagY = mpu.magY();
      MagZ = mpu.magZ();*/

      filter.updateIMU(GyroX, GyroY, GyroZ, AccX, AccY, AccZ);

      // Recupera os quaternions
      float q0 = filter.getQ0();
      float q1 = filter.getQ1();
      float q2 = filter.getQ2();
      float q3 = filter.getQ3();

      // Vetor "up"
      float upX = 2 * (q1 * q3 - q0 * q2);
      float upY = 2 * (q2 * q3 + q0 * q1);
      float upZ = 1 - 2 * (q1 * q1 + q2 * q2);

      // Calcula os ângulos corrigidos
      Roll = atan2(upY, upZ) * RAD_TO_DEG;
      Roll = (Roll > 0) ? Roll - 180 : Roll + 180;
      Pitch = -(atan2(-upX, sqrt(upY * upY + upZ * upZ)) * RAD_TO_DEG);
      Yaw = atan2(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3) * RAD_TO_DEG;
      
      I2CService::LiberarAcessoI2C(ID_Num);
    }

    void AferirDadosTMP() {
      if (!BmpIniciado) return;

      if (!I2CService::SolicitarAcessoI2C(ID_Num)) return;

      Temp = bmp.readTemperature();
      Pressao = bmp.readPressure();
      Altitude = bmp.readAltitude();  

      I2CService::LiberarAcessoI2C(ID_Num);
    }
};


std::vector<uint8_t> SensorIMU::ConfigurarSensor(std::vector<SensorIMU*>& lista, std::vector<uint8_t>& data, const String& Mod_ID) {
  std::vector<uint8_t> status;
  if (data.size() < 2) return status;
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
  uint8_t idNum = data[1];
  auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorIMU* s) { return s->ID_Num == idNum; });
  bool jaExiste = it != lista.end();
  SensorIMU* sensor;
  switch (posicao) {
    case CanMessagePosicaoDados::Config1: {
      if (data.size() < 6) return status;
      bool conectar = data[3] == 1;
      uint8_t enderecoMpu = data[4];
      uint8_t enderecoBmp = data[5];
      if (conectar) {
        if (!jaExiste) {
          sensor = new SensorIMU(Mod_ID, "sIMU_" + String(idNum));
        }
        else {
          sensor = *it;
        }
        if (!sensor->Iniciado) {
          sensor->ID_Num = idNum;
          sensor->_EnderecoMPU = enderecoMpu;
          sensor->_EnderecoBMP = enderecoBmp;
          sensor->Inicializar();
          if (!jaExiste) {
            lista.push_back(sensor);
            PrintTela("Sensor IMU adicionado via CAN: sIMU_" + String(idNum));
          }
        }
        status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
      } else {
        if (jaExiste) {
          sensor = *it;
          sensor->Desligar();
          status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          delete sensor;
          lista.erase(it);
          PrintTela("Sensor IMU removido via CAN: sIMU_" + String(idNum));
        }
      }
      break;
    }
  }
  return status;
}

#endif