#ifndef SensorIMUModel
#define SensorIMUModel

#include <MPU9250_WE.h>
#include <Adafruit_BMP280.h>
#include <MadgwickAHRS.h>
#include <Wire.h>
#include "SerialService.h"
#include "Pinout.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;

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

    // MPU9250 (IMU 9 eixos)
    MPU9250_WE* mpu;

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

    Madgwick filter;

    // 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;
      }

      mpu = new MPU9250_WE(_EnderecoMPU);
      if (!mpu->init()) {
        PrintTela("MPU9250 nao encontrado no endereco " + String(_EnderecoMPU));
        Iniciado = false;
        //return;
      }

      mpu->autoOffsets();
      mpu->setSampleRateDivider(5);
      mpu->setAccRange(MPU9250_ACC_RANGE_2G);
      mpu->enableAccDLPF(true);
      mpu->setAccDLPF(MPU9250_DLPF_6);

      if (!bmp.begin(_EnderecoBMP)) {
        PrintTela("BMP280 nao encontrado no endereco " + String(_EnderecoBMP));
        Iniciado = false;
        return;
      }

      bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
                      Adafruit_BMP280::SAMPLING_X2,
                      Adafruit_BMP280::SAMPLING_X16,
                      Adafruit_BMP280::FILTER_X16,
                      Adafruit_BMP280::STANDBY_MS_500);

      filter.begin(100); // 100 Hz

      xTaskCreatePinnedToCore(&SensorIMU::IMUTaskWrapper, "IMUTask", 5000, this, 20, &IMUTaskHandle, tskNO_AFFINITY);

      Iniciado = true;
      PrintTela(_ID + " iniciado");
    }

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

      delete mpu;
      mpu = nullptr;

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

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void RequisitarDados() {
      /*Serial.print("Temp: "); Serial.println(Temp);
      Serial.print("Pressao: "); Serial.println(Pressao);
      Serial.print("Altitude: "); Serial.println(Altitude);
      Serial.print("Roll: "); Serial.println(Roll);
      Serial.print("Pitch: "); Serial.println(Pitch);
      Serial.print("Yaw: "); Serial.println(Yaw);
      Serial.println();*/
    }

    std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
      std::vector<uint8_t> data;
      data.push_back(ID_Num);
      data.push_back(static_cast<uint8_t>(posicao));
    
      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() {
      while (1) {
        if (Iniciado) {
          AferirDados();
          vTaskDelay(pdMS_TO_TICKS(10)); // 10ms → 100 Hz
        }
      }
    }

    void AferirDados() {
      if (!Iniciado) return;

      xyzFloat acc = mpu->getGValues();
      xyzFloat gyr = mpu->getGyrValues();
      xyzFloat mag = mpu->getMagValues();

      AccX = acc.x;
      AccY = acc.y;
      AccZ = acc.z;

      GyroX = gyr.x;
      GyroY = gyr.y;
      GyroZ = gyr.z;

      MagX = mag.x;
      MagY = mag.y;
      MagZ = mag.z;

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

      filter.update(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ);\
      Roll = filter.getRoll();
      Pitch = filter.getPitch();
      Yaw = filter.getYaw();
    }
};


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() < 3) return status;
  uint8_t idNum = data[1];
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[2];
  switch (posicao) {
    case CanMessagePosicaoDados::Config1: {
      if (data.size() < 7) return status;
      bool conectar = data[4] == 1;
      uint8_t enderecoMpu = data[5];
      uint8_t enderecoBmp = data[6];
      auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorIMU* s) { return s->ID_Num == idNum; });
      bool existente = (it != lista.end());
      if (conectar) {
        SensorIMU* sensor;
        if (!existente) {
          sensor = new SensorIMU(Mod_ID, "sIMU_" + String(idNum));
        }
        else {
          sensor = *it;
        }
        sensor->ID_Num = idNum;
        sensor->_EnderecoMPU = enderecoMpu;
        sensor->_EnderecoBMP = enderecoBmp;
        sensor->Inicializar();
        if (!existente) {
          lista.push_back(sensor);
          PrintTela("Sensor IMU adicionado via CAN: sIMU_" + String(idNum));
        }
        status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
      } else {
        if (existente) {
          SensorIMU* 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