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

#ifndef SensorCorrenteModel_v2
#define SensorCorrenteModel_v2

class SensorCorrente_v2 : public ComponenteCAN {
  public:
    String ID;
    uint8_t ID_Num = 0;
    uint8_t _Endereco = 0;
    TiposShuntCorrente _A_Shunt = TiposShuntCorrente::INA228_10A;
    int _CanalMUX = -1;
    bool isINA219 = false;

    bool Iniciado = false;

    float busVoltage = 0;
    float current = 0;
    float power = 0;
    float energy = 0;
    float charge = 0;
    uint16_t codAlarme = 0;
    float temperature = 0;

    SensorCorrente_v2() {}

    SensorCorrente_v2(
      const String& id,
      uint8_t idNum,
      uint8_t endereco,
      TiposShuntCorrente tipoShunt,
      int canalMux = -1
    ) :
      ID(id),
      ID_Num(idNum),
      _Endereco(endereco),
      _A_Shunt(tipoShunt),
      _CanalMUX(canalMux)
    {
    }

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

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

      isINA219 = (_A_Shunt == TiposShuntCorrente::INA219_3A2);

      bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
      if (!encontrado) {
        Iniciado = false;
        MostrarLog("Endereco I2C nao encontrado: " + String(_Endereco));
        I2CService::LiberarAcessoI2C(ID_Num);
        return;
      }

      if (isINA219) {
        sINA219 = Adafruit_INA219(_Endereco);
        Iniciado = sINA219.begin();

        if (Iniciado) {
          sINA219.setCalibration_32V_2A();
          MostrarLog("INA219 iniciado no canal " + String(_CanalMUX));
        } else {
          MostrarLog("Falha ao iniciar INA219");
        }
      }
      else {
        sINA228 = Adafruit_INA228();
        Iniciado = sINA228.begin(_Endereco);

        if (Iniciado) {
          ConfigurarShuntINA228();
          sINA228.setAveragingCount(INA228_COUNT_16);
          sINA228.setCurrentConversionTime(INA228_TIME_280_us);
          MostrarLog("INA228 iniciado no canal " + String(_CanalMUX));
        } else {
          MostrarLog("Falha ao iniciar INA228");
        }
      }

      I2CService::LiberarAcessoI2C(ID_Num);
    }

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

      Iniciado = false;
      MostrarLog("Sensor desligado");
    }

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

    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 volts = (uint16_t)(busVoltage * 100.0f);
          uint16_t amps  = (uint16_t)(current / 10.0f);
          uint16_t watts = (uint16_t)(power / 10.0f);

          data.push_back(volts >> 8); data.push_back(volts & 0xFF);
          data.push_back(amps  >> 8); data.push_back(amps  & 0xFF);
          data.push_back(watts >> 8); data.push_back(watts & 0xFF);
          break;
        }

        case CanMessagePosicaoDados::Dados2: {
          if (isINA219) break;

          uint16_t wh   = (uint16_t)(energy / 10.0f);
          uint16_t temp = (uint16_t)(temperature * 100.0f);

          data.push_back(wh >> 8);   data.push_back(wh & 0xFF);
          data.push_back(temp >> 8); data.push_back(temp & 0xFF);
          data.push_back(codAlarme >> 8); data.push_back(codAlarme & 0xFF);
          break;
        }
      }

      return data;
    }

  private:
    bool DebugMode = true;
    SimpleKalmanFilter filtro = SimpleKalmanFilter(10, 10, 0.05);
    Adafruit_INA219 sINA219;
    Adafruit_INA228 sINA228;

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

    void ConfigurarShuntINA228() {
      if (_A_Shunt == TiposShuntCorrente::INA228_10A) {
        sINA228.setShunt(0.015, 10.0);
      }
      else if (_A_Shunt == TiposShuntCorrente::INA228_50A) {
        sINA228.setShunt(0.0015, 50.0);
      }
      else if (_A_Shunt == TiposShuntCorrente::INA228_100A) {
        sINA228.setShunt(0.00075, 100.0);
      }
    }

    void AferirCorrente() {
      if (!Iniciado) return;
      if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) return;

      if (isINA219) {
        busVoltage = sINA219.getBusVoltage_V();
        float leituraCorrente = sINA219.getCurrent_mA();
        current = filtro.updateEstimate(leituraCorrente);
        power = sINA219.getPower_mW();
      }
      else {
        busVoltage = sINA228.getBusVoltage_V();
        float leituraCorrente = sINA228.getCurrent_mA();
        current = filtro.updateEstimate(leituraCorrente);
        power = sINA228.getPower_mW();
        temperature = sINA228.readDieTemp();
        energy = sINA228.readEnergy();
        charge = sINA228.readCharge();
        codAlarme = (uint16_t)LerRegistradorINA(0x0B, 2);
      }

      I2CService::LiberarAcessoI2C(ID_Num);
    }

    uint32_t LerRegistradorINA(uint8_t reg, uint8_t bytes) {
      uint8_t buffer[4] = {0};
      if (bytes > 4) return 0;

      if (!I2CService::LeituraSegura(_Endereco, buffer, bytes, reg)) {
        MostrarLog("Falha na leitura do registrador 0x" + String(reg, HEX));
        return 0;
      }

      uint32_t valor = 0;
      for (uint8_t i = 0; i < bytes; i++) {
        valor = (valor << 8) | buffer[i];
      }

      return valor;
    }
};

#endif