#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include "IComponenteCAN.h"
#include <SimpleKalmanFilter.h>

#ifndef SensorTemperaturaNtcModel
#define SensorTemperaturaNtcModel

class SensorTemperaturaNtc : public ComponenteCAN {
  public:
    String ID;
    uint8_t ID_Num = 0;
    PinoModel _pino;
    bool _ntcGnd = false;

    int leitura = 0;
    double temperatura = -1;
    double tensao = 0;

    bool Iniciado = false;

    float OffsetTemperatura = 0.0f;

    SensorTemperaturaNtc() {}

    SensorTemperaturaNtc(
      const String& id,
      uint8_t idNum,
      int pino,
      TiposBarramentos tipoBarramento,
      int canalMux = -1,
      float offsetTemperatura = 0.0f,
      bool ntcGnd = false
    ) :
      ID(id),
      ID_Num(idNum),
      _pino(PinoModel(pino, tipoBarramento, _INPUT, ANALOGICO, idNum, canalMux)),
      OffsetTemperatura(offsetTemperatura),
      _ntcGnd(ntcGnd)
    {
    }

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

      leitura = 0;
      temperatura = -1;
      tensao = 0;

      _pino.Conectar();
      _pino.ConfigurarLeituraAnalogica(12, ADC_11db);

      Iniciado = _pino.Definido();

      MostrarLog(Iniciado ? "Sensor iniciado" : "Sensor nao iniciado");
    }

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

      _pino.Desconectar();

      Iniciado = false;

      MostrarLog("Sensor desligado");
    }

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

    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: {
          int16_t temperaturaInt =
            isfinite(temperatura)
              ? static_cast<int16_t>(round(temperatura * 100.0f))
              : INT16_MIN;

          uint16_t tensaoMv =
            isfinite(tensao) && tensao >= 0.0
              ? static_cast<uint16_t>(round(tensao * 1000.0))
              : 0xFFFF;

          uint16_t leituraAdc = static_cast<uint16_t>(leitura);

          data.push_back(static_cast<uint8_t>(leituraAdc >> 8));
          data.push_back(static_cast<uint8_t>(leituraAdc & 0xFF));

          data.push_back(static_cast<uint8_t>(temperaturaInt >> 8));
          data.push_back(static_cast<uint8_t>(temperaturaInt & 0xFF));

          data.push_back(static_cast<uint8_t>(tensaoMv >> 8));
          data.push_back(static_cast<uint8_t>(tensaoMv & 0xFF));
          break;
        }
      }

      return data;
    }

  private:
    bool DebugMode = false;
    // SimpleKalmanFilter filtro = SimpleKalmanFilter(5, 15, 0.01);

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

    void AferirTemperatura() {
      constexpr int QuantidadeAmostras = 16;

      leitura = _pino.get(QuantidadeAmostras);
      const int tensaoMv = _pino.get(QuantidadeAmostras, FormatoLeituraAnalogica::Milivolts);

      if (leitura < 0 || tensaoMv < 0) {
          leitura = -1;
          tensao = NAN;
          temperatura = NAN;

          MostrarLog("Falha na leitura do sensor NTC");
          return;
      }

      tensao = tensaoMv / 1000.0f;

      // Detecta curto, pino flutuante, circuito aberto ou saturação.
      if (leitura <= 20 || leitura >= 4080) {
          temperatura = NAN;
          MostrarLog("Leitura NTC fora da faixa | Raw: " + String(leitura) + " | Tensao: " + String(tensao, 3) + " V");
          return;
      }

      temperatura = CalcularTemperaturaPorTensao(static_cast<float>(tensaoMv), _ntcGnd, OffsetTemperatura);

      MostrarLog(
          "Leitura: " + String(leitura) +
          " | Tensao: " + String(tensao, 3) + " V" +
          " | Temperatura: " + String(temperatura, 2) + " C"
      );
    }

    float CalcularTemperatura(float leituraAdc, float offset = 0.0f) {
      const float Vs = 5.0f;
      const float Beta = 3950.0f;
      const float To = 298.15f;
      const float Ro = 10000.0f;
      const float adcMax = 4095.0f;
      static constexpr float divisor = 1.0f;
      static constexpr float vOffset = (2.516f / 3.036f);

      if (leituraAdc < 200.0f) return -1.0f;

      float Vadc = leituraAdc / adcMax * Vs;
      Vadc = Vadc * divisor * vOffset;

      tensao = Vadc;

      if (Vadc <= 0.001f) return -100.0f;
      if (Vadc >= Vs) return NAN;

      float Rt = Ro * (Vadc / (Vs - Vadc));
      float T = 1.0f / (1.0f / To + log(Rt / Ro) / Beta);
      float Tc = T - 273.15f + offset;

      return roundf(Tc * 100.0f) / 100.0f;
    }

    float CalcularTemperaturaPorTensao(float tensaoAdcMv, bool ntcGnd, float offset = 0.0f) {
        constexpr float Vs = 5.0f;

        constexpr float Rfixo  = 10000.0f; // Resistor interno do motor

        // Valores reais da placa:
        constexpr float Rserie = 4700.0f;  // Retorno até o GPIO
        constexpr float Rgnd   = 6800.0f;  // GPIO até o GND
        constexpr float Rcarga = Rserie + Rgnd;

        constexpr float Beta   = 3950.0f;
        constexpr float T0     = 298.15f;  // 25 °C em kelvin
        constexpr float Rntc25 = 10000.0f;

        const float vAdc = tensaoAdcMv / 1000.0f;

        if (!isfinite(vAdc) || vAdc <= 0.01f || vAdc >= 3.05f)
          return NAN;

        // Tensão no retorno do motor, antes do resistor de 4,7 kΩ.
        const float vRetorno = vAdc * (Rserie + Rgnd) / Rgnd;

        if (!isfinite(vRetorno) || vRetorno <= 0.01f || vRetorno >= Vs - 0.01f) {
          return NAN;
        }

        float resistenciaNtc = NAN;

        if (ntcGnd) {
          /*
              5 V
              |
            Rfixo
              |
            retorno
              |
              NTC
              |
              GND
              A carga externa Rcarga fica em paralelo com o NTC.
          */

          const float rEquivalente = Rfixo * vRetorno / (Vs - vRetorno);
          const float inversoRntc = (1.0f / rEquivalente) - (1.0f / Rcarga);
          if (!isfinite(inversoRntc) || inversoRntc <= 0.0f)
            return NAN;
          resistenciaNtc = 1.0f / inversoRntc;
        }
        else {
          /*
              5 V
              |
              NTC
              |
            retorno
              |
            Rfixo
              |
              GND
              A carga externa Rcarga fica em paralelo com Rfixo.
          */

          const float rInferior = (Rfixo * Rcarga) / (Rfixo + Rcarga);
          resistenciaNtc = rInferior * (Vs - vRetorno) / vRetorno;
        }

        if (!isfinite(resistenciaNtc) || resistenciaNtc <= 0.0f)
          return NAN;

        const float temperaturaK = 1.0f / ((1.0f / T0) + logf(resistenciaNtc / Rntc25) / Beta);
        const float temperaturaC = temperaturaK - 273.15f + offset;

        if (!isfinite(temperaturaC) || temperaturaC < -40.0f || temperaturaC > 150.0f) {
          return NAN;
        }

        return roundf(temperaturaC * 100.0f) / 100.0f;
    }

  };



#endif