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

#ifndef SensorPressaoModel
#define SensorPressaoModel

class SensorPressao : public ComponenteCAN {
  public:

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

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

    double _Vmin = 0.0;
    double _Vmax = 4.5;
    double _Pmax = 174.045;

    int leitura = 0;
    double Pressao = 0;

    SimpleKalmanFilter filtro = SimpleKalmanFilter(2, 2, 0.01);

    bool Iniciado = false;

    void Inicializar() {
      if (Iniciado) {
        PrintTela(_ID + " ja inicializado");
        return;
      }

      leitura = 0;

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

      Iniciado = _pino.Definido();

      if (Iniciado) {
        PrintTela(_ID + " iniciado");
      }
      else {
        PrintTela(_ID + " nao iniciado");
      }
    }

    void Desligar() {
      if (!Iniciado) {
        PrintTela(_ID + " nao esta inicializado");
        return;
      }

      _pino.Desconectar(); // Desconecta o pino

      Iniciado = false;
      
      PrintTela(_ID + " Desligado");
    }

    void RequisitarDados() {
      AferirPressao();
    }

    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 pressaoEscalada = (uint16_t)(Pressao * 100);
          data.push_back((uint8_t)(leitura >> 8)); data.push_back((uint8_t)(leitura & 0xFF));
          data.push_back(pressaoEscalada >> 8); data.push_back(pressaoEscalada & 0xFF);
          break;
        }
      }
      return data;
    }

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

  private:
    void AferirPressao() {
      if (Iniciado) {
        leitura = _pino.get();
        double leituraPressao = filtro.updateEstimate(leitura);

        double tensao = fmap(leituraPressao, 0.0, 4095.0, 0.0, 3300.0) / 1000.0;
        Pressao = fmap(tensao, _Vmin, _Vmax, 0.0, _Pmax);
      }
    }
    

};



#endif