#ifndef SensorCorrenteModel
#define SensorCorrenteModel

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

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

    String Mod_ID = "";
    String _ID;
    int ID_Num;
    byte _Endereco;
    TiposShuntCorrente _A_Shunt = TiposShuntCorrente::INA228_10A;
    int _CanalMUX;
    bool isINA219 = false;

    bool Iniciado = false;
    float busVoltage = 0;
    float current = 0;
    float power = 0;
    float energy = 0;
    float temperature = 0;

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

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

      isINA219 = _A_Shunt == TiposShuntCorrente::INA219_3A2;

      bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
      if (encontrado) {
        if (isINA219) {
          sINA219 = Adafruit_INA219(_Endereco);
          Iniciado = sINA219.begin();
          if (Iniciado) {
            sINA219.setCalibration_32V_2A();
            MostrarLog("Sensor (INA219) iniciado no canal " + String(_CanalMUX));
          }
          else {
            PrintTela("Sensor nao iniciado!");
          }
        }
        else {
          Iniciado = true;
          switch (_A_Shunt) {
            case TiposShuntCorrente::INA228_10A:
              currentLSB = currentLSB_10A;
              break;
            case TiposShuntCorrente::INA228_50A:
              currentLSB = currentLSB_50A;
              break;
            case TiposShuntCorrente::INA228_100A:
              currentLSB = currentLSB_100A;
              break;
          }
          EscreverRegistradorINA(0x00, 0x8000);  // Bit 15 = 1 → RESET
          uint16_t cal = (uint16_t)(0.00512 / (currentLSB * shuntLSB));
          //EscreverRegistradorINA(0xD4, cal);
          MostrarLog("Sensor (INA228) iniciado no canal " + String(_CanalMUX));
        }        
      } 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) {
          delay(10);
        }
      }
      
      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 = busVoltage * 100;
          uint16_t amps = current * 100;
          uint16_t watts = power * 10;
          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 = energy * 10;
          uint16_t temp = temperature * 100;
          data.push_back(wh >> 8); data.push_back(wh & 0xFF);
          data.push_back(temp >> 8); data.push_back(temp & 0xFF);
          break;
        }
      }
      return data;
    }

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

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

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

  private:

    bool DebugMode = true;
    void MostrarLog(String mensagem) {
      if (DebugMode) {
        PrintTela("[COR " + _ID + "] " + mensagem);
      }
    }

    Adafruit_INA219 sINA219;
    static constexpr float currentLSB_10A  = 0.0001;
    static constexpr float currentLSB_50A  = 0.00119;
    static constexpr float currentLSB_100A = 0.00243;
    static constexpr float shuntLSB        = 1.0e-6;
    static constexpr float vBusLSB         = 1.23e-5;
    float currentLSB                       = 0.0001;

    uint32_t LerRegistradorINA(uint8_t reg, uint8_t bytes) {
      uint8_t buffer[4] = {0};
  
      if (bytes > 4) return 0;
  
      // Aumentar timeout para garantir tempo de resposta
      if (!I2CService::LeituraSegura(_Endereco, buffer, bytes, reg)) {
        Serial.printf("[sCOR] Falha na leitura do registrador 0x%02X\n", reg);
        return 0;
      }
  
      uint32_t valor = 0;
      for (uint8_t i = 0; i < bytes; i++) {
        valor = (valor << 8) | buffer[i];
      }
  
      return valor;
    }

    void EscreverRegistradorINA(uint8_t reg, uint16_t valor) {
      uint8_t dados[3];
      dados[0] = reg;
      dados[1] = (valor >> 8) & 0xFF;
      dados[2] = valor & 0xFF;
    
      if (!I2CService::EscritaSegura(_Endereco, dados, 3, 50)) {
        Serial.printf("[INA] Falha ao escrever no registrador 0x%02X\n", reg);
      }
    }

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

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

      if (isINA219) {
        busVoltage = sINA219.getBusVoltage_V();
        current = sINA219.getCurrent_mA();
        power = sINA219.getPower_mW();

        MostrarLog("BusVoltage: " + String(busVoltage) + ", Current: " + String(current) + ", Power: " + String(power));
      }
      else {
        uint32_t rawBus = LerRegistradorINA(0x05, 3);
        uint32_t rawCurrent = LerRegistradorINA(0x07, 3);
        uint32_t rawPower = LerRegistradorINA(0x08, 3);
        uint64_t rawEnergy = ((uint64_t)LerRegistradorINA(0x0A, 3) << 16) | LerRegistradorINA(0x0B, 2);
        uint16_t rawTemp = LerRegistradorINA(0x0F, 2);
  
        busVoltage = rawBus * vBusLSB;
        power = rawPower * currentLSB * 3.125;
        current = rawCurrent * currentLSB;
        energy = rawEnergy * currentLSB * 3.125 * 16 / 3600.0;
        temperature = ((int16_t)rawTemp) * 0.0078125;

        MostrarLog("BusVoltage: " + String(busVoltage) + ", Current: " + String(current) + ", Power: " + String(power) + ", Energy: " + String(energy) + ", Temperature: " + String(temperature));
      }

      I2CService::LiberarAcessoI2C(ID_Num);
    }
};

#endif