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

#ifndef SensorPressaoModel_v2_h
#define SensorPressaoModel_v2_h

class SensorPressao_v2 : public ComponenteCAN {
  public:
    SensorPressao_v2(
      const String& id,
      uint8_t idNum,
      int pino,
      TiposBarramentos barramento = TiposBarramentos::CONTROLADOR,
      double vmin = 0.0,
      double vmax = 4.5,
      double pmax = 174.045
    )
    : _ID(id),
      ID_Num(idNum),
      _pino(pino, barramento, TiposPinos::_INPUT, TiposLeituras::ANALOGICO),
      _Vmin(vmin),
      _Vmax(vmax),
      _Pmax(pmax),
      filtro(2, 2, 0.01) {
    }

    String _ID;
    uint8_t ID_Num = 0;
    PinoModel _pino;

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

    int leitura = 0;
    double Tensao = 0.0;
    double Pressao = 0.0;

    bool Iniciado = false;

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

      leitura = 0;
      Tensao = 0.0;
      Pressao = 0.0;

      _pino.Conectar();
      Iniciado = _pino.Definido();

      if (Iniciado) {
        MostrarLog("Sensor iniciado");
      } else {
        MostrarLog("Sensor nao iniciado");
      }
    }

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

      _pino.Desconectar();

      Iniciado = false;
      leitura = 0;
      Tensao = 0.0;
      Pressao = 0.0;

      MostrarLog("Sensor desligado");
    }

    void RequisitarDados() {
      AtualizarLeitura();

      //MostrarLog("Leitura: " + String(leitura) + ", Pressao: " + String(Pressao));
    }

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

      leitura = _pino.get();
      double leituraFiltrada = filtro.updateEstimate(leitura);

      Tensao = fmap(leituraFiltrada, 0.0, 4095.0, 0.0, 3300.0) / 1000.0;
      Pressao = fmap(Tensao, _Vmin, _Vmax, 0.0, _Pmax);

      if (Pressao < 0) Pressao = 0;
      if (Pressao > _Pmax) Pressao = _Pmax;
    }

    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: {
          AtualizarLeitura();

          uint16_t pressaoEscalada = (uint16_t)(Pressao * 100.0);
          uint16_t tensaoEscalada = (uint16_t)(Tensao * 1000.0);

          data.push_back((uint8_t)(leitura >> 8));
          data.push_back((uint8_t)(leitura & 0xFF));

          data.push_back((uint8_t)(pressaoEscalada >> 8));
          data.push_back((uint8_t)(pressaoEscalada & 0xFF));

          data.push_back((uint8_t)(tensaoEscalada >> 8));
          data.push_back((uint8_t)(tensaoEscalada & 0xFF));
          break;
        }

        default:
          break;
      }

      return data;
    }

  private:
    bool DebugMode = false;
    SimpleKalmanFilter filtro;

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

#endif