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

#ifndef SensorMassaModel
#define SensorMassaModel

class SensorMassa {
  public:

    static void InicializarLista(std::vector<SensorMassa*>& lista, const String& parametros, const String& Mod_ID);

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

    String Mod_ID;
    String _ID;
    int ID_Num;
    PinoModel _pinoDT;
    PinoModel _pinoSCK;

    float _Offset = 1895800.0;
    float _MassaRef = 0.8;
    float _MassaLeituraRef = 40876.37;

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

    double leituraMassa = 0;
    float Massa = 0;

    bool Iniciado = false;

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

      if (_pinoDT.Definido() && _pinoSCK.Definido()) {
        balanca.begin(_pinoDT.num, _pinoSCK.num);
        balanca.set_scale();
        //balanca->tare();

        Iniciado = true;

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

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

      // Redefinir as configurações para os valores iniciais
      _pinoDT.Desconectar();
      _pinoSCK.Desconectar();

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void RequisitarDados() {
      AferirMassa();
    }

    std::vector<uint8_t> MontarMensagemCAN() {
      std::vector<uint8_t> data;
    
      data.push_back(ID_Num);               // Byte 0 - ID do sensor
      data.push_back(Iniciado ? 1 : 0);     // Byte 1 - Status
    
      uint16_t massaEscalada = (uint16_t)(Massa * 100);
      data.push_back(massaEscalada >> 8);
      data.push_back(massaEscalada & 0xFF);
      
      return data;
    }



  private:
    void AferirMassa() {
      if (Iniciado) {
        float leitura = balanca.get_units(5);
        leituraMassa = filtro.updateEstimate(leitura);
        Massa = CalcularMassa(leituraMassa);
      }
    }

    float CalcularMassa(float _leitura) {
      double leituraMassa = _leitura * -1;
      leituraMassa += _Offset;

      float FatorLeitura = _MassaRef / _MassaLeituraRef;
      float massa = FatorLeitura * leituraMassa;
      return massa;
    }


};


// Função para inicializar os sensores de massa com IDs e pinos específicos
void SensorMassa::InicializarLista(std::vector<SensorMassa*>& lista, const String& parametros, const String& Mod_ID) {
  // Verifica se há sensores já na lista
  if (!lista.empty()) {
    PrintTela("Limpando lista de sensores de massa existente...");

    for (auto* sensor : lista) {
      sensor->Desligar();  // Desliga cada sensor antes de remover
      PrintTela("Sensor de Massa " + sensor->_ID + " desligado.");
      delete sensor;
    }

    lista.clear();  // Limpa a lista após desligar todos os itens
  }

  if (parametros.length() > 0) {
    std::vector<String> _config = SplitString(parametros, SplitConfig);  // Divide a configuração por vírgulas

    for (const String& config : _config) {
      std::vector<String> partes = SplitString(config, SplitSubParams);  // Divide cada configuração pelo '-'

      if (partes.size() == 6) {
        String id = partes[0];
        bool conectar = partes[1] == "1";
        TiposBarramentos pinoDtB = (TiposBarramentos)partes[2].toInt();
        int pinoDt = partes[3].toInt();
        TiposBarramentos pinoSckB = (TiposBarramentos)partes[4].toInt();
        int pinoSck = partes[5].toInt();

        if (conectar) {
          SensorMassa* sensor = new SensorMassa(Mod_ID, id);
          sensor->_pinoDT = PinoModel(pinoDt, pinoDtB, _INPUT, DIGITAL);
          sensor->_pinoSCK = PinoModel(pinoSck, pinoSckB, _OUTPUT);
          sensor->Inicializar();

          lista.push_back(sensor);
          PrintTela("Sensor de massa " + id + " inicializado nos pinos " + String(pinoDt) + " e " + String(pinoSck));
        } 
      } else {
        PrintTela("Configuração inválida para sensor de massa: " + config);
      }
    }
  } else {
    PrintTela("Configuração de sensor de massa não encontrada.");
  }
}

#endif