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

#ifndef SensorTemperaturaModel
#define SensorTemperaturaModel

class SensorTemperatura : public ComponenteCAN {
  public:

    SensorTemperatura(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    // Definições
    String Mod_ID = "";
    String _ID;
    int ID_Num;
    PinoModel _pino;

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

    //SimpleKalmanFilter filtro = SimpleKalmanFilter(5, 15, 0.01);

    bool Iniciado = false;
    bool Checando = false;

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

      leitura = 0;

      _pino.Conectar(); // Conecta o pino antes de criar a tarefa

      Iniciado = _pino.Definido();

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

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

      _pino.Desconectar(); // Desconecta o pino

      MostrarLog("Sensor Desligado");

      Iniciado = false;
    }

    void RequisitarDados() {
      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 leituraInt = static_cast<int16_t>(round(temperatura * 100.0f));
          data.push_back((uint8_t)(leitura >> 8)); data.push_back((uint8_t)(leitura & 0xFF));
          data.push_back((uint8_t)(leituraInt >> 8)); data.push_back((uint8_t)(leituraInt & 0xFF));
          break;
        }
      }
      return data;
    }

    static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorTemperatura*>& 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](SensorTemperatura* s) { return s->ID_Num == idNum; });
      bool jaExiste = it != lista.end();
      SensorTemperatura* sensor;
      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 7) return status;
          bool conectar = data[3] == 1;
          TiposBarramentos tipoBarramento = (TiposBarramentos)data[4];
          int pino = data[5];
          int canalMux = (data[6] == 255) ? -1 : data[6];
          if (conectar) {
            if (!jaExiste) {
              sensor = new SensorTemperatura(Mod_ID, "sTMP_" + String(idNum));
            }
            else {
              sensor = *it;
            }
            if (!sensor->Iniciado) {
              sensor->ID_Num = idNum;
              sensor->_pino = PinoModel(pino, tipoBarramento, _INPUT, ANALOGICO, idNum, canalMux);
              sensor->Inicializar();
              if (!jaExiste) {
                lista.push_back(sensor);
                sensor->MostrarLog("Sensor de temperatura adicionado via CAN: sTMP_" + String(idNum));
              } 
            }
            status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          } else {
            if (jaExiste) {
              sensor = *it;
              sensor->Desligar();
              status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
              sensor->MostrarLog("Sensor de temperatura removido via CAN: sTMP_" + String(idNum));
              delete sensor;
              lista.erase(it);
            }
          }
          break;
        }
      }
      return status;
    }


    private:

    bool DegugMode = false;

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

    void AferirTemperatura() {
      if (Iniciado) {
        leitura = _pino.get(1);
        //leitura = filtro.updateEstimate(leitura);
        temperatura = CalcularTemperatura((float)leitura);

        MostrarLog("Leitura:" + String(leitura) + ", Tensao: " + String(tensao) + ", Temperatura: " + String(temperatura));
    
      }
    }

    float CalcularTemperatura(float leitura, float offset = 0.0f) {
      const float Vs = 5.0;         // Tensão de referência (divisor)
      const float Beta = 3950.0;    // Coeficiente Beta do NTC
      const float To = 298.15;      // 25°C em Kelvin
      const float Ro = 10000.0;     // NTC 10k
      const float adcMax = 4095.0;  // ADC de 12 bits
      static constexpr float divisor = 1.0f;
      static constexpr float vOffset = (2.516f / 3.036f); // v real / v ads
    
      if (leitura < 200) return -1.0; // ignora valores muito baixos (desconectado?)
    
      float Vadc = (float)leitura / adcMax * Vs;
      Vadc = Vadc * divisor * vOffset;

      tensao = Vadc;
      
      if (Vadc <= 0.001) return -100.0; // proteção para log(0)
      if (Vadc >= Vs) return NAN;
    
      float Rt = Ro * (Vadc / (Vs - Vadc)); // NTC em cima
      // float Rt = Ro * ((Vs - Vadc) / Vadc); // NTC embaixo, se for o caso
    
      float T = 1.0 / (1.0 / To + log(Rt / Ro) / Beta);
      float Tc = T - 273.15 + offset;

      return roundf(Tc * 100.0f) / 100.0f; // arredonda para 2 casas
    }

  };



#endif