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

#ifndef SensorMagneticoModel_v2
#define SensorMagneticoModel_v2

class SensorMagnetico_v2 : public ComponenteCAN {
  public:
    String ID;
    uint8_t ID_Num = 0;
    uint8_t _Endereco = 0x36;
    int _CanalMUX = -1;

    bool Iniciado = false;
    uint16_t raw = 0;
    float angle = 0;

    SensorMagnetico_v2() {}

    SensorMagnetico_v2(
      const String& id,
      uint8_t idNum,
      uint8_t endereco = 0x36,
      int canalMux = -1
    ) :
      ID(id),
      ID_Num(idNum),
      _Endereco(endereco),
      _CanalMUX(canalMux)
    {
    }

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

      if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) {
        MostrarLog("Falha ao solicitar acesso I2C");
        return;
      }

      bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
      Iniciado = encontrado;

      if (!Iniciado) {
        MostrarLog("Sensor nao iniciado, endereco " + String(_Endereco) + " nao encontrado");
      } else {
        MostrarLog("Sensor iniciado");
      }

      I2CService::LiberarAcessoI2C(ID_Num);
    }

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

      Iniciado = false;
      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);
          break;
        }
      }

      return data;
    }

  private:
    bool DebugMode = true;
    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);

      I2CService::LiberarAcessoI2C(ID_Num);
    }

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

      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;
      return valor;
    }

    float RawToDegrees(uint16_t valorRaw) {
      return (valorRaw * 360.0f) / 4096.0f;
    }
};

#endif