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

#ifndef SensorMassaModel_v2_h
#define SensorMassaModel_v2_h

class SensorMassa_v2 : public ComponenteCAN {
  public:
    SensorMassa_v2(
      const String& id,
      uint8_t idNum,
      int pinoDT,
      int pinoSCK,
      TiposBarramentos barramento = CONTROLADOR,
      long offsetHx = 1996283,
      float escalaHx = 50857.316406,
      float offsetMassa = 0.0
    )
    : _ID(id),
      ID_Num(idNum),
      _pinoDT(pinoDT, barramento, _INPUT, DIGITAL),
      _pinoSCK(pinoSCK, barramento, _OUTPUT),
      _Offset(offsetHx),
      _Escala(escalaHx),
      _OffsetMassa(offsetMassa),
      filtro(0.05, 0.05, 0.25) {
    }

    String _ID;
    uint8_t ID_Num = 0;

    PinoModel _pinoDT;
    PinoModel _pinoSCK;

    long _Offset = 1996283;
    float _Escala = 50857.316406f;
    float _OffsetMassa = 0.0f;

    HX711 balanca;
    SimpleKalmanFilter filtro;

    float Massa = 0.0f;
    float leitura = 0.0f;
    float leituraCorrigida = 0.0f;

    bool Iniciado = false;

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

      _pinoDT.Conectar();
      _pinoSCK.Conectar(false);

      if (_pinoDT.Definido() && _pinoSCK.Definido()) {
        balanca.begin(_pinoDT.num, _pinoSCK.num);
        balanca.set_offset(_Offset);
        balanca.set_scale(_Escala);

        leitura = 0.0f;
        leituraCorrigida = 0.0f;
        Massa = 0.0f;

        Iniciado = true;
        MostrarLog("Sensor iniciado");
      } else {
        Iniciado = false;
        MostrarLog("Sensor nao iniciado");
      }
    }

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

      _pinoDT.Desconectar();
      _pinoSCK.Desconectar();

      leitura = 0.0f;
      leituraCorrigida = 0.0f;
      Massa = 0.0f;
      Iniciado = false;

      MostrarLog("Sensor desligado");
    }

    void RequisitarDados() {
      AtualizarLeitura();
    }

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

      if (!balanca.is_ready()) {
        MostrarLog("HX711 nao pronto");
        return;
      }

      leitura = balanca.get_units(5);

      float valor = -leitura;
      valor += _OffsetMassa;
      leituraCorrigida = valor;

      Massa = filtro.updateEstimate(leituraCorrigida);

      if (Massa < 0.0f) Massa = 0.0f;
    }

    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 leituraEscalada = EscalarFloat2Bytes(leituraCorrigida);
          uint16_t massaEscalada = EscalarFloat2Bytes(Massa);

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

        default:
          break;
      }

      return data;
    }

  private:
    bool DebugMode = false;

    uint16_t EscalarFloat2Bytes(float valor) {
      if (valor < 0.0f) valor = 0.0f;

      float escalado = valor * 100.0f;
      if (escalado > 65535.0f) escalado = 65535.0f;

      return (uint16_t)(escalado);
    }

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

#endif