#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)
    {
    }

    bool EstaIniciado() {
      const uint32_t geracaoAtual = I2CService::GeracaoBarramento();

      bool iniciado;
      uint32_t geracaoInicializacao;

      portENTER_CRITICAL(&_snapshotLock);
      iniciado = Iniciado;
      geracaoInicializacao = _geracaoI2CInicializacao;
      portEXIT_CRITICAL(&_snapshotLock);

      return iniciado &&
             geracaoAtual != 0 &&
             geracaoInicializacao == geracaoAtual;
    }

    uint8_t FalhasConsecutivas() {
      uint8_t falhas;
      portENTER_CRITICAL(&_snapshotLock);
      falhas = _falhasConsecutivas;
      portEXIT_CRITICAL(&_snapshotLock);
      return falhas;
    }

    uint32_t TotalFalhasLeitura() {
      uint32_t total;
      portENTER_CRITICAL(&_snapshotLock);
      total = _totalFalhasLeitura;
      portEXIT_CRITICAL(&_snapshotLock);
      return total;
    }

    void Inicializar() {
      if (EstaIniciado()) {
        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::VerificaEnderecoBarramentoPassivo(_Endereco);
      if (!encontrado) {
        AtualizarEstadoIniciado(false);
        MostrarLog("Endereco I2C nao encontrado: " + String(_Endereco));
        I2CService::LiberarAcessoI2C(ID_Num);
        return;
      }

      if (isINA219) {
        sINA219 = Adafruit_INA219(_Endereco);
        bool iniciou = sINA219.begin(&Wire);
        AtualizarEstadoIniciado(iniciou);

        if (iniciou) {
          sINA219.setCalibration_32V_2A();
          MostrarLog("INA219 iniciado no canal " + String(_CanalMUX));
        } else {
          MostrarLog("Falha ao iniciar INA219");
        }
      }
      else {
        sINA228 = Adafruit_INA228();
        bool iniciou = sINA228.begin(_Endereco, &Wire);
        AtualizarEstadoIniciado(iniciou);

        if (iniciou) {
          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);

      if (EstaIniciado()) {
        portENTER_CRITICAL(&_snapshotLock);
        _falhasConsecutivas = 0;
        portEXIT_CRITICAL(&_snapshotLock);
      }
    }

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

      AtualizarEstadoIniciado(false);
      MostrarLog("Sensor desligado");
    }

    void RequisitarDados() {
      if (!EstaIniciado()) 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);

      const uint32_t geracaoAtual = I2CService::GeracaoBarramento();
      bool iniciado;
      bool ina219;
      float snapBusVoltage;
      float snapCurrent;
      float snapPower;
      float snapEnergy;
      uint16_t snapCodAlarme;
      float snapTemperature;

      // Copia curta e atômica. Serialização acontece fora da critical section.
      portENTER_CRITICAL(&_snapshotLock);
      iniciado = Iniciado &&
                 geracaoAtual != 0 &&
                 _geracaoI2CInicializacao == geracaoAtual;
      ina219 = isINA219;
      snapBusVoltage = busVoltage;
      snapCurrent = current;
      snapPower = power;
      snapEnergy = energy;
      snapCodAlarme = codAlarme;
      snapTemperature = temperature;
      portEXIT_CRITICAL(&_snapshotLock);

      switch (posicao) {
        case CanMessagePosicaoDados::Status: {
          data.push_back(iniciado ? 1 : 0);
          break;
        }

        case CanMessagePosicaoDados::Dados1: {
          uint16_t volts = (uint16_t)(snapBusVoltage * 100.0f);
          uint16_t amps  = (uint16_t)(snapCurrent / 10.0f);
          uint16_t watts = (uint16_t)(snapPower / 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 (ina219) break;

          uint16_t wh   = (uint16_t)(snapEnergy / 10.0f);
          uint16_t temp = (uint16_t)(snapTemperature * 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(snapCodAlarme >> 8); data.push_back(snapCodAlarme & 0xFF);
          break;
        }
      }

      return data;
    }

  private:
    bool DebugMode = false;
    SimpleKalmanFilter filtro = SimpleKalmanFilter(10, 10, 0.05);
    Adafruit_INA219 sINA219;
    Adafruit_INA228 sINA228;
    portMUX_TYPE _snapshotLock = portMUX_INITIALIZER_UNLOCKED;
    uint8_t _falhasConsecutivas = 0;
    uint32_t _totalFalhasLeitura = 0;
    uint32_t _geracaoI2CInicializacao = 0;
    static constexpr uint8_t LimiteFalhasAntesReinit = 3;

    void AtualizarEstadoIniciado(bool iniciado) {
      const uint32_t geracao =
        iniciado ? I2CService::GeracaoBarramento() : 0;

      portENTER_CRITICAL(&_snapshotLock);
      Iniciado = iniciado;
      _geracaoI2CInicializacao = geracao;
      portEXIT_CRITICAL(&_snapshotLock);
    }

    void RegistrarFalhaAquisicao(const String& motivo) {
      uint8_t falhas;

      portENTER_CRITICAL(&_snapshotLock);
      if (_falhasConsecutivas < UINT8_MAX) {
        _falhasConsecutivas++;
      }
      _totalFalhasLeitura++;
      falhas = _falhasConsecutivas;
      portEXIT_CRITICAL(&_snapshotLock);

      MostrarLog(
        "Falha de aquisicao " + String(falhas) + "/" +
        String(LimiteFalhasAntesReinit) + " | " + motivo
      );

      if (falhas >= LimiteFalhasAntesReinit) {
        AtualizarEstadoIniciado(false);
        MostrarLog("Sensor marcado para reinicializacao controlada");
      }

      I2CService::NotificarFalhaDispositivo(ID_Num, motivo);
    }

    void RegistrarSucessoAquisicao() {
      portENTER_CRITICAL(&_snapshotLock);
      _falhasConsecutivas = 0;
      portEXIT_CRITICAL(&_snapshotLock);

      I2CService::NotificarSucessoDispositivo(ID_Num);
    }

    bool AmostraValida(
      float tensaoBarramento,
      float corrente,
      float potencia,
      float temperaturaDie
    ) {
      if (!isfinite(tensaoBarramento) || !isfinite(corrente)) return false;

      const float limiteTensao = isINA219 ? 32.5f : 85.0f;
      if (tensaoBarramento < -0.5f || tensaoBarramento > limiteTensao) return false;

      float limiteCorrenteA = 10.0f;
      if (isINA219) {
        limiteCorrenteA = 3.2f;
      }
      else if (_A_Shunt == TiposShuntCorrente::INA228_50A) {
        limiteCorrenteA = 50.0f;
      }
      else if (_A_Shunt == TiposShuntCorrente::INA228_100A) {
        limiteCorrenteA = 100.0f;
      }

      // A biblioteca Adafruit retorna corrente em mA.
      if (fabsf(corrente) > (limiteCorrenteA * 1250.0f)) return false;

      if (isINA219) {
        return isfinite(potencia) && fabsf(potencia) <= 120000.0f;
      }

      return isfinite(temperaturaDie) &&
             temperaturaDie >= -40.0f && temperaturaDie <= 150.0f;
    }

    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 (!EstaIniciado()) return;
      if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) {
        // Contencao de mutex ou recovery em andamento nao prova falha do INA.
        // Apenas pulamos esta amostra e tentamos novamente no proximo tick.
        return;
      }

      // As APIs Adafruit abaixo nao propagam de forma util os erros de Wire.
      // Este ping confirma o dispositivo no ramal antes da aquisicao.
      if (!I2CService::VerificaEnderecoBarramentoPassivo(_Endereco)) {
        I2CService::LiberarAcessoI2C(ID_Num);
        RegistrarFalhaAquisicao("dispositivo nao respondeu no endereco I2C");
        return;
      }

      float novoBusVoltage = 0.0f;
      float leituraCorrenteBruta = 0.0f;
      float novaPotencia = 0.0f;
      float novaTemperatura = 0.0f;

      if (isINA219) {
        novoBusVoltage = sINA219.getBusVoltage_V();
        leituraCorrenteBruta = sINA219.getCurrent_mA();
        novaPotencia = sINA219.getPower_mW();
      }
      else {
        novoBusVoltage = sINA228.getBusVoltage_V();
        leituraCorrenteBruta = sINA228.getCurrent_mA();
        //power = sINA228.getPower_mW();
        novaTemperatura = sINA228.readDieTemp();
        //energy = sINA228.readEnergy();
        //charge = sINA228.readCharge();
        //codAlarme = (uint16_t)LerRegistradorINA(0x0B, 2);
      }

      const bool amostraValida = AmostraValida(
        novoBusVoltage,
        leituraCorrenteBruta,
        novaPotencia,
        novaTemperatura
      );

      if (!amostraValida) {
        I2CService::LiberarAcessoI2C(ID_Num);
        RegistrarFalhaAquisicao("amostra invalida/fora da faixa");
        return;
      }

      // Somente amostras validadas entram no estado interno do filtro.
      const float novaCorrente = filtro.updateEstimate(leituraCorrenteBruta);

      I2CService::LiberarAcessoI2C(ID_Num);
      RegistrarSucessoAquisicao();

      // Publica o conjunto inteiro de uma vez. O CAN nunca observa metade da
      // amostra anterior com metade da nova.
      portENTER_CRITICAL(&_snapshotLock);
      busVoltage = novoBusVoltage;
      current = novaCorrente;
      if (isINA219) {
        power = novaPotencia;
      }
      else {
        temperature = novaTemperatura;
      }
      portEXIT_CRITICAL(&_snapshotLock);
    }

    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