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

#ifndef BombaPressurizadoraModel
#define BombaPressurizadoraModel

class BombaPressurizadora : public ComponenteCAN {
  public:
  
    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 = nullptr;

    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 {
          PotenciaLeitura = RampaMax;
          //_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(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: {
          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;
    }

    static std::vector<uint8_t> 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() < 2) return status;
      CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
      uint8_t idNum = data[1];
      auto it = std::find_if(lista.begin(), lista.end(), [idNum](BombaPressurizadora* s) { return s->ID_Num == idNum; });
      bool jaExiste = it != lista.end();
      BombaPressurizadora* bomba;
      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 8) return status;
          bool conectar = data[3] == 1;
          int pressaoSp = (int)data[4];
          int sensor_id = (data[5] == 0xFF) ? -1 : (int)data[5];
          int pinoEn = data[6];
          int pinoCs = data[7];
          if (conectar) {
            if (!jaExiste) {
              bomba = new BombaPressurizadora(Mod_ID, "sBMB_" + String(idNum));
            }
            else {
              bomba = *it;
            }
            if (!bomba->Iniciado) {
              bool malha_fechada = sensor_id >= 0;
              if (malha_fechada) {
                auto prs = std::find_if(sensoresPressao.begin(), sensoresPressao.end(), [sensor_id](SensorPressao* s) { return s->ID_Num == sensor_id; });
                bool sensor_encontrado = prs != sensoresPressao.end();
                if (sensoresPressao.size() > 0) {
                  if (sensor_encontrado && (*prs)->Iniciado) {
                    bomba->sensorPressao = *prs;
                  }
                }
                if (bomba->sensorPressao == nullptr) {
                  PrintTela("Sensor de pressao " + String(sensor_id) + " nao encontrado 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;
              if (!jaExiste) {
                lista.push_back(bomba);
                PrintTela("Bomba Pressurizadora adicionado via CAN: sBMB_" + String(idNum));
              }
            }
            //status = bomba->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          } else {
            if (jaExiste) {
              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() < 6) return status;
          if (!jaExiste) return status;

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

          bomba = *it;
          if (!bomba->Iniciado) {
            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;
    }

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

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



};






#endif