#ifndef SensorIMUModel
#define SensorIMUModel

#include <MPU9250_WE.h>
#include <Adafruit_BMP280.h>
#include <Wire.h>
#include "SerialService.h"
#include "Pinout.h"

class SensorIMU {
  public:
    String Mod_ID;
    String _ID;
    int ID_Num;
    bool Iniciado = false;

    // Endereços I2C
    #define MPU_ADDR 0x68
    #define BMP_ADDR 0x76

    // MPU9250 (IMU 9 eixos)
    MPU9250_WE mpu = MPU9250_WE(MPU_ADDR);

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

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

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

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

      if (!mpu.init()) {
        PrintTela("MPU9250 nao encontrado no endereco " + String(MPU_ADDR));
        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(BMP_ADDR)) {
        PrintTela("BMP280 nao encontrado no endereco " + String(BMP_ADDR));
        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);

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

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

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void RequisitarDados() {
      AferirDados();
    }

    std::vector<uint8_t> MontarMensagemCAN() {
      std::vector<uint8_t> data;
      data.push_back(ID_Num);               // Byte 0 - ID
      data.push_back(Iniciado ? 1 : 0);     // Byte 1 - Status

      // Exemplo com AccX, AccY, Temp, Pressao, Altitude
      int16_t accX_int = static_cast<int16_t>(AccX * 100);
      int16_t accY_int = static_cast<int16_t>(AccY * 100);
      int16_t temp_int = static_cast<int16_t>(Temp * 100);
      uint16_t pressao_int = static_cast<uint16_t>(Pressao / 10);
      int16_t alt_int = static_cast<int16_t>(Altitude * 10);

      data.push_back(accX_int >> 8); data.push_back(accX_int & 0xFF);
      data.push_back(accY_int >> 8); data.push_back(accY_int & 0xFF);
      data.push_back(temp_int >> 8); data.push_back(temp_int & 0xFF);
      data.push_back(pressao_int >> 8); data.push_back(pressao_int & 0xFF);
      data.push_back(alt_int >> 8); data.push_back(alt_int & 0xFF);

      return data;
    }

  private:
    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();
    }
};

#endif