#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include "IComponenteCAN.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) {
    }

    String _ID;
    uint8_t ID_Num = 0;
    PinoModel _pino;

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

    double GanhoCalibracaoPressao = 1.1361506;
    double OffsetCalibracaoPressao = -14.887795;

    int leitura = 0;
    double Tensao = 0.0;

    /*
     * Pressão convertida e calibrada, mas sem filtro.
     * Será usada futuramente para proteção rápida.
     */
    double PressaoBruta = 0.0;

    /*
     * Pressão filtrada usada pela malha de controle.
     */
    double Pressao = 0.0;

    bool LeituraValida = false;
    bool FiltroInicializado = false;
    bool Iniciado = false;

    unsigned long UltimaLeituraMs = 0;
    unsigned long UltimaTentativaLeituraMs = 0;
    unsigned long UltimaAtualizacaoFiltroMs = 0;

    unsigned int FalhasConsecutivas = 0;

    /*
     * Evita aquisições duplicadas muito próximas,
     * caso controle e telemetria solicitem leitura.
     */
    unsigned long IntervaloMinimoLeituraMs = 5;

    /*
     * Constante de tempo do filtro.
     *
     * 0,10 s:
     * - responde rápido;
     * - reduz ruído do ADC;
     * - não adiciona atraso excessivo ao PID.
     */
    double TauFiltroPressaoSeg = 0.10;

    /*
     * Faixa elétrica plausível.
     *
     * O sensor calibrado em 0 psi deve ficar acima
     * do limite mínimo.
     */
    double TensaoMinimaValida = 0.10;
    double TensaoMaximaValida = 3.25;

    portMUX_TYPE _stateMux =
      portMUX_INITIALIZER_UNLOCKED;

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

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

      ResetarEstado();

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

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

      Iniciado = false;
      _pino.Desconectar();

      ResetarEstado();

      MostrarLog("Sensor desligado");
    }

    void RequisitarDados() {
      AtualizarLeitura();
    }

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

      unsigned long agora = millis();

      /*
       * Impede duas aquisições simultâneas e evita
       * atualizações duplicadas muito próximas.
       */
      portENTER_CRITICAL(&_stateMux);

      bool leituraMuitoRecente =
        UltimaTentativaLeituraMs > 0 &&
        (agora - UltimaTentativaLeituraMs) <
          IntervaloMinimoLeituraMs;

      if (_leituraEmAndamento || leituraMuitoRecente) {
        portEXIT_CRITICAL(&_stateMux);
        return;
      }

      _leituraEmAndamento = true;
      UltimaTentativaLeituraMs = agora;

      portEXIT_CRITICAL(&_stateMux);

      /*
       * Aquisição física fora da região crítica.
       */
      int novaLeitura = _pino.get();

      double novaTensao =
        fmap(
          (double)novaLeitura,
          0.0,
          4095.0,
          0.0,
          3300.0
        ) / 1000.0;

      bool configuracaoValida =
        isfinite(_Vmin) &&
        isfinite(_Vmax) &&
        isfinite(_Pmax) &&
        isfinite(GanhoCalibracaoPressao) &&
        isfinite(OffsetCalibracaoPressao) &&
        _Vmax > _Vmin &&
        _Pmax > 0.0;

      bool leituraEletricaValida =
        novaLeitura > 0 &&
        novaLeitura < 4095 &&
        isfinite(novaTensao) &&
        novaTensao >= TensaoMinimaValida &&
        novaTensao <= TensaoMaximaValida;

      if (
        !configuracaoValida ||
        !leituraEletricaValida
      ) {
        RegistrarLeituraInvalida(
          novaLeitura,
          novaTensao
        );

        return;
      }

      double pressaoCalculada =
        fmap(
          novaTensao,
          _Vmin,
          _Vmax,
          0.0,
          _Pmax
        );

      pressaoCalculada =
        (
          pressaoCalculada *
          GanhoCalibracaoPressao
        ) +
        OffsetCalibracaoPressao;

      if (!isfinite(pressaoCalculada)) {
        RegistrarLeituraInvalida(
          novaLeitura,
          novaTensao
        );

        return;
      }

      double novaPressaoBruta =
        constrain(
          pressaoCalculada,
          0.0,
          _Pmax
        );

      double pressaoAnterior;
      unsigned long ultimaAtualizacao;
      bool filtroInicializadoAnterior;

      portENTER_CRITICAL(&_stateMux);

      pressaoAnterior = Pressao;
      ultimaAtualizacao =
        UltimaAtualizacaoFiltroMs;

      filtroInicializadoAnterior =
        FiltroInicializado;

      portEXIT_CRITICAL(&_stateMux);

      double novaPressaoFiltrada;

      if (
        !filtroInicializadoAnterior ||
        ultimaAtualizacao == 0
      ) {
        /*
         * A primeira leitura começa no valor real.
         * O filtro não parte artificialmente de zero.
         */
        novaPressaoFiltrada =
          novaPressaoBruta;
      } else {
        double dt =
          (agora - ultimaAtualizacao) /
          1000.0;

        dt = constrain(
          dt,
          0.001,
          0.500
        );

        double tau =
          TauFiltroPressaoSeg;

        if (
          !isfinite(tau) ||
          tau < 0.001
        ) {
          tau = 0.001;
        }

        double alpha =
          dt / (tau + dt);

        alpha = constrain(
          alpha,
          0.01,
          1.0
        );

        novaPressaoFiltrada =
          (alpha * novaPressaoBruta) +
          (
            (1.0 - alpha) *
            pressaoAnterior
          );
      }

      /*
       * Publica a nova amostra de forma consistente.
       */
      portENTER_CRITICAL(&_stateMux);

      leitura = novaLeitura;
      Tensao = novaTensao;

      PressaoBruta = novaPressaoBruta;
      Pressao = novaPressaoFiltrada;

      LeituraValida = true;
      FiltroInicializado = true;

      UltimaLeituraMs = agora;
      UltimaAtualizacaoFiltroMs = agora;

      FalhasConsecutivas = 0;
      _leituraEmAndamento = false;

      portEXIT_CRITICAL(&_stateMux);
    }

    unsigned long IdadeLeituraMs() const {
      unsigned long ultimaLeitura =
        UltimaLeituraMs;

      if (ultimaLeitura == 0)
        return ULONG_MAX;

      return millis() - ultimaLeitura;
    }

    bool LeituraRecente(
      unsigned long timeoutMs = 250
    ) const {
      return
        LeituraValida &&
        UltimaLeituraMs > 0 &&
        IdadeLeituraMs() <= timeoutMs;
    }

    /*
     * Snapshot consistente para a bomba.
     */
    void ObterSnapshotControle(
      double& pressaoBruta,
      double& pressaoFiltrada,
      bool& leituraValida,
      unsigned long& idadeLeituraMs
    ) {
      unsigned long ultimaLeitura;

      portENTER_CRITICAL(&_stateMux);

      pressaoBruta = PressaoBruta;
      pressaoFiltrada = Pressao;
      leituraValida = LeituraValida;
      ultimaLeitura = UltimaLeituraMs;

      portEXIT_CRITICAL(&_stateMux);

      idadeLeituraMs =
        ultimaLeitura == 0
          ? ULONG_MAX
          : millis() - ultimaLeitura;

      leituraValida =
        leituraValida &&
        ultimaLeitura > 0;
    }

    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: {
          int leituraAtual;
          double pressaoAtual;
          double tensaoAtual;

          portENTER_CRITICAL(&_stateMux);

          leituraAtual = leitura;
          pressaoAtual = Pressao;
          tensaoAtual = Tensao;

          portEXIT_CRITICAL(&_stateMux);

          double pressaoEscaladaDouble =
            round(
              pressaoAtual * 100.0
            );

          pressaoEscaladaDouble =
            constrain(
              pressaoEscaladaDouble,
              0.0,
              65535.0
            );

          double tensaoEscaladaDouble =
            round(
              tensaoAtual * 1000.0
            );

          tensaoEscaladaDouble =
            constrain(
              tensaoEscaladaDouble,
              0.0,
              65535.0
            );

          uint16_t pressaoEscalada =
            (uint16_t)pressaoEscaladaDouble;

          uint16_t tensaoEscalada =
            (uint16_t)tensaoEscaladaDouble;

          uint16_t leituraEscalada =
            leituraAtual < 0
              ? 0
              : (
                  leituraAtual > 65535
                    ? 65535
                    : (uint16_t)leituraAtual
                );

          data.push_back(
            (uint8_t)(leituraEscalada >> 8)
          );

          data.push_back(
            (uint8_t)(leituraEscalada & 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;
    bool _leituraEmAndamento = false;

    void RegistrarLeituraInvalida(
      int novaLeitura,
      double novaTensao
    ) {
      portENTER_CRITICAL(&_stateMux);

      /*
       * Atualizamos ADC e tensão para diagnóstico,
       * mas preservamos a última pressão válida.
       */
      leitura = novaLeitura;
      Tensao =
        isfinite(novaTensao)
          ? novaTensao
          : 0.0;

      LeituraValida = false;

      if (FalhasConsecutivas < UINT_MAX) {
        FalhasConsecutivas++;
      }

      _leituraEmAndamento = false;

      portEXIT_CRITICAL(&_stateMux);
    }

    void ResetarEstado() {
      portENTER_CRITICAL(&_stateMux);

      leitura = 0;
      Tensao = 0.0;

      PressaoBruta = 0.0;
      Pressao = 0.0;

      LeituraValida = false;
      FiltroInicializado = false;

      UltimaLeituraMs = 0;
      UltimaTentativaLeituraMs = 0;
      UltimaAtualizacaoFiltroMs = 0;

      FalhasConsecutivas = 0;
      _leituraEmAndamento = false;

      portEXIT_CRITICAL(&_stateMux);
    }

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

#endif