#ifndef SensorMagneticoModel
#define SensorMagneticoModel

#include "SerialService.h"
#include "I2CService.h"
#include "Utils.h"
#include "IComponenteCAN.h"
#include <Wire.h>
#include <vector>

class SensorMagnetico : public ComponenteCAN {
  public:
    SensorMagnetico(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    String Mod_ID = "";
    String _ID;
    int ID_Num;
    byte _Endereco = 0x36;
    int _CanalMUX;

    bool Iniciado = false;
    uint16_t raw = 0;
    float angle = 0;
    unsigned long last_send = 0;

    void Inicializar() {
      if (Iniciado) {
        MostrarLog("Sensor ja inicializado");
        return;
      }

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

      bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
      if (encontrado) {
        Iniciado = true;
      } else {
        Iniciado = false;
        MostrarLog("Sensor nao iniciado, endereco " + String(_Endereco) + " nao encontrado!");
      }
    
      I2CService::LiberarAcessoI2C(ID_Num);
    }

    void Desligar() {
      if (!Iniciado) {
        MostrarLog("Sensor nao esta inicializado");
        return;
      }

      Iniciado = false;

      if (I2CService::QuemEstaUsando() == ID_Num) {
        while (I2CService::QuemEstaUsando() == ID_Num) {
          vTaskDelay(10);
        }
      }
      
      MostrarLog("Sensor Desligado");
    }

    void RequisitarDados() {
      if (!Iniciado) return;
      AferirAngulo();
    }

    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: {
          uint16_t ang100 = (uint16_t)round(angle * 100.0f);
          data.push_back(raw >> 8); data.push_back(raw & 0xFF);
          data.push_back(ang100 >> 8); data.push_back(ang100 & 0xFF);

          MostrarLog("Enviando dados: " + String(angle));
          break;
        }
      }
      return data;
    }

    static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorMagnetico*>& 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](SensorMagnetico* s) { return s->ID_Num == idNum; });
      bool jaExiste = it != lista.end();
      SensorMagnetico* sensor;

      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 6) return status;
          bool conectar = data[3] == 1;
          uint8_t endereco = data[4];
          int canalMux = (data[5] == 255) ? -1 : data[5];

          if (conectar) {
            if (!jaExiste) {
              sensor = new SensorMagnetico(Mod_ID, "sMAG_" + String(idNum));
            } else {
              sensor = *it;
            }
            if (!sensor->Iniciado) {
              sensor->ID_Num = idNum;
              sensor->_Endereco = endereco;
              sensor->_CanalMUX = canalMux;
              sensor->Inicializar();
              if (!jaExiste) {
                lista.push_back(sensor);
                sensor->MostrarLog("Sensor magnetico adicionado via CAN: sMAG_" + String(idNum));
              }
            }
            status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          } else {
            if (jaExiste) {
              sensor = *it;
              sensor->Desligar();
              status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
              sensor->MostrarLog("Sensor magnetico removido via CAN: sMAG_" + String(idNum));
              delete sensor;
              lista.erase(it);
            }
          }
          break;
        }
      }
      return status;
    }

  private:

    bool DebugMode = false;
    static const uint8_t REG_RAW_ANGLE = 0x0C;

    void MostrarLog(String mensagem) {
      if (DebugMode) {
        PrintTela("[MAG " + _ID + "] " + mensagem);
      }
    }

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

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

      raw = lerRawAngle();
      angle = rawToDegrees(raw);

      //MostrarLog("Raw: " + String(raw) + ", Angle: " + String(angle));

      I2CService::LiberarAcessoI2C(ID_Num);
    }

    uint16_t lerRawAngle() {
        uint8_t buffer[2] = {0};

        // Usa LeituraSegura para ler 2 bytes do registrador RAW_ANGLE
        if (!I2CService::LeituraSegura(_Endereco, buffer, 2, REG_RAW_ANGLE)) {
            MostrarLog("Falha na leitura do RAW_ANGLE");
            return 0;
        }

        uint16_t valor = ((uint16_t)buffer[0] << 8) | buffer[1];
        valor &= 0x0FFF; // 12 bits válidos

        return valor;
    }

    float rawToDegrees(uint16_t raw) {
        // 0..4095 -> 0..360°
        return (raw * 360.0f) / 4096.0f;
    }

};

#endif