#include "SerialService.h"
#include "SensorPressaoModel.h"
#include "Pinout.h"
#include "Utils.h"
#include <PID_v1.h>

#ifndef BombaPressurizadoraModel
#define BombaPressurizadoraModel

class BombaPressurizadora {
  public:

    static std::vector<uint8_t> ConfigurarBomba(std::vector<BombaPressurizadora*>& lista, std::vector<uint8_t>& data, const String& Mod_ID, std::vector<SensorPressao*>& sensoresPressao);
    static void ComandarBomba(std::vector<BombaPressurizadora*>& lista, std::vector<uint8_t>& data, uint8_t& id);

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

    String Mod_ID;
    String _ID;
    int ID_Num;
    PinoModel _pinoEN;
    PinoModel _pinoCS;
    PinoModel _pinoINA;
    PinoModel _pinoINB;
    PinoModel _pinoPWM;
    int _canal = 0;

    float _Kp = 3.0;
    float _Ki = 2.2;
    float _Kd = 0.5;
    const int histerese = 3;
    bool bombaDesligadaPorPressao = false;
    PID* _PID = nullptr;

    SensorPressao* sensorPressao;

    int ZeroRampa = 0;
    int RampaMax = 1023;
    
    bool Iniciado = false;

    // Consumo
    Estado EstadoLeitura = Desligado;
    double PotenciaLeitura = 0;
    double PercentualPotenciaLeitura = 0;

    // Entrada de dados
    Estado _EstadoControle = Desligado;
    double _PressaoSP = 0;

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

      if (sensorPressao == nullptr) {
        PrintTela("Sensor de pressao nao definido, bomba nao sera inicializada");
        Iniciado = false;
        return;
      }
      
      _EstadoControle = Desligado;
      _pinoEN.Conectar(_EstadoControle);
      _pinoCS.Conectar();
      _pinoINA.Conectar(false);
      _pinoINB.Conectar(true);
      if (_pinoPWM.Definido()) {
        ledcSetup(_canal, 1000, 10);
        ledcAttachPin(_pinoPWM.num, _canal);
        PotenciaLeitura = 0;
        _PressaoSP = 0;
        ledcWrite(_canal, _PressaoSP);
      }

      Iniciado = _pinoEN.Definido() && _pinoPWM.Definido();

      AtualizarLeitura();

      if (Iniciado) {
        _PID = new PID(&sensorPressao->Pressao, &PotenciaLeitura, &_PressaoSP, _Kp, _Ki, _Kd, DIRECT);
        _PID->SetOutputLimits(ZeroRampa, RampaMax);
        _PID->SetMode(AUTOMATIC);
  
        PrintTela(_ID + " iniciado");
      }
      else {
        PrintTela(_ID + " nao iniciado");
      }
    }

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

      delete _PID;
      _PID = nullptr;
      delete sensorPressao;
      sensorPressao = nullptr;
      
      // Redefinir as configurações para os valores iniciais
      _pinoPWM.Desconectar();
      _pinoEN.Desconectar();
      _pinoCS.Desconectar();
      _pinoINA.Desconectar();
      _pinoINB.Desconectar();
      _pinoPWM.Desconectar();

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void AlterarStatusBomba() {
      if (Iniciado) {
        bool status = _EstadoControle == Ligado;

        _pinoEN.set(status);
        
        AtualizarLeitura();
      }
    }

    void Loop() {
      if (Iniciado) {
        sensorPressao->RequisitarDados();
        double _PressaoAtual = sensorPressao->Pressao; // Obtém a leitura mais recente

        if (_PressaoAtual >= (_PressaoSP + histerese)) {
          bombaDesligadaPorPressao = true;
        } else if (_PressaoAtual <= (_PressaoSP - histerese) && bombaDesligadaPorPressao) {
          // Permite que o PID atue novamente se a pressão cair abaixo do set point - histerese
          bombaDesligadaPorPressao = false;
        }

        if (_EstadoControle == Desligado || bombaDesligadaPorPressao) {
          PotenciaLeitura = ZeroRampa;
        } else {
          _PID->Compute();
          // Ajusta PotenciaAtual com base no PID
        }

        ledcWrite(_canal, PotenciaLeitura);

        AtualizarLeitura();
      }
    }

    void RequisitarDados() {

    }

    std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
      std::vector<uint8_t> data;
      data.push_back(ID_Num);
      data.push_back(static_cast<uint8_t>(posicao));
      switch (posicao) {
        case CanMessagePosicaoDados::Status: {
          data.push_back(Iniciado ? 1 : 0);
          break;
        }
        case CanMessagePosicaoDados::Dados1: {
          data.push_back(static_cast<uint8_t>(EstadoLeitura));
          data.push_back((int)PercentualPotenciaLeitura);
          data.push_back((int)_PressaoSP);
          data.push_back((int)sensorPressao->Pressao);
          break;
        }
      }
      return data;
    }


  private:
    void AtualizarLeitura() {
      if (Iniciado) {
        EstadoLeitura = _pinoEN.get() == 1 ? Ligado : Desligado;
        PercentualPotenciaLeitura = (PotenciaLeitura / RampaMax) * 100;
      }
    }



};


std::vector<uint8_t> BombaPressurizadora::ConfigurarBomba(std::vector<BombaPressurizadora*>& lista, std::vector<uint8_t>& data, const String& Mod_ID, std::vector<SensorPressao*>& sensoresPressao) {
  std::vector<uint8_t> status;
  if (data.size() < 3) return status;
  uint8_t idNum = data[1];
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[2];
  auto it = std::find_if(lista.begin(), lista.end(), [idNum](BombaPressurizadora* s) { return s->ID_Num == idNum; });
  switch (posicao) {
    case CanMessagePosicaoDados::Config1: {
      if (data.size() < 8) return status;
      bool conectar = data[4] == 1;
      int pressaoSp = (int)data[5];
      int pinoEn = data[6];
      int pinoCs = data[7];
      if (conectar) {
        if (it == lista.end()) {
          BombaPressurizadora* bomba = new BombaPressurizadora(Mod_ID, "sBMB_" + String(idNum));
          if (sensoresPressao.size() > 0) {
            if (sensoresPressao[0]->Iniciado) {
              bomba->sensorPressao = sensoresPressao[0];
            }
          }
          if (bomba->sensorPressao == nullptr) {
            PrintTela("Nenhum sensor de pressão disponível para associar à bomba.");
            delete bomba;
            return status;
          }
          bomba->ID_Num = idNum;
          bomba->_pinoEN = PinoModel(pinoEn, CONTROLADOR, _OUTPUT);
          bomba->_pinoCS = PinoModel(pinoCs, CONTROLADOR, _INPUT);
          bomba->_PressaoSP = (double)pressaoSp;
          lista.push_back(bomba);
          PrintTela("Bomba Pressurizadora adicionado via CAN: sBMB_" + String(idNum));
        }
      } else {
        if (it != lista.end()) {
          BombaPressurizadora* bomba = *it;
          bomba->Desligar();
          status = bomba->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          delete bomba;
          lista.erase(it);
          PrintTela("Bomba Pressurizadora removido via CAN: sBMB_" + String(idNum));
        }
      }
      break;
    }
    case CanMessagePosicaoDados::Config2: {
      if (data.size() < 7) return status;
      if (it == lista.end()) return status;

      int pinoIna = data[4];
      int pinoInb = data[5];
      int pinoPwm = data[6];

      BombaPressurizadora* bomba = *it;
      bomba->_pinoINA = PinoModel(pinoIna, CONTROLADOR, _OUTPUT);
      bomba->_pinoINB = PinoModel(pinoInb, CONTROLADOR, _OUTPUT);
      bomba->_pinoPWM = PinoModel(pinoPwm, CONTROLADOR, _PWM);
      bomba->Inicializar();
      PrintTela("Bomba Pressurizadora adicionado via CAN: sBMB_" + String(bomba->ID_Num));
      status = bomba->MontarMensagemCAN(CanMessagePosicaoDados::Status);
      break;
    }
  }
  return status;
}

void BombaPressurizadora::ComandarBomba(std::vector<BombaPressurizadora*>& lista, std::vector<uint8_t>& data, uint8_t& id) {
  if (data.size() < 5) return;
  Estado novoEstado = (Estado)data[3];
  int pressao = (int)data[4];
  for (auto* _bomba : lista) {
    if (_bomba->ID_Num == id) {
      _bomba->_EstadoControle = novoEstado;
      _bomba->_PressaoSP = (double)pressao;
      _bomba->AlterarStatusBomba();
    }
  }
}


#endif