#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include "IComponenteCAN.h"
#include <ESP32Servo.h>

#ifndef ServoAgroModel
#define ServoAgroModel

class ServoAgro : public ComponenteCAN {
  public:

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

    String Mod_ID;
    String _ID;
    int ID_Num;
    PinoModel _pino;

    bool Incremental = true;

    bool Iniciado = false;

    int _Angulo = 0;

    Servo _Servo;

    void Inicializar() {
      if (Iniciado) {
        MostrarLog("Servo ja inicializado");
        return;
      }
      
      if (_pino.Definido()) {
        _Servo.attach(_pino.num);
        AtualizarAngulo(Incremental ? 0 : 180);

        Iniciado = true;

        MostrarLog("Servo iniciado");
      }
      else {
        MostrarLog("Servo nao iniciado");
      }
    }

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

      // Redefinir as configurações para os valores iniciais
      if (_pino.Definido()) {
        _Servo.detach();
      }

      MostrarLog("Servo Desligado");

      Iniciado = false;
    }

    void AtualizarAngulo(int novoAngulo) {
      if (Iniciado) {
        MostrarLog("Angulo atualizado de " + String(_Angulo) + " para " + String(novoAngulo));

        _Angulo = novoAngulo;
      
        _Servo.write(_Angulo);
      }
    }

    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(_Angulo); // 0 a 180
          break;
        }
      }
      return data;
    }

    static std::vector<uint8_t> ConfigurarServo(std::vector<ServoAgro*>& lista, std::vector<uint8_t>& data, const String& Mod_ID) {
      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](ServoAgro* s) { return s->ID_Num == idNum; });
      bool jaExiste = it != lista.end();
      ServoAgro* sensor;
      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 7) return status;
          bool conectar = data[3] == 1;
          bool incremental = data[4] == 1;
          TiposBarramentos tipoBarramento = (TiposBarramentos)data[5];
          int pino = data[6];
          if (conectar) {
            if (!jaExiste) {
              sensor = new ServoAgro(Mod_ID, "sSRV_" + String(idNum));
            }
            else {
              sensor = *it;
            }
            if (!sensor->Iniciado) {
              sensor->ID_Num = idNum;
              sensor->Incremental = incremental;
              sensor->_pino = PinoModel(pino, tipoBarramento, _OUTPUT);
              sensor->Inicializar();
              if (!jaExiste) {
                lista.push_back(sensor);
                sensor->MostrarLog("Servo adicionado via CAN: sSRV_" + String(idNum));
              }
            }
            status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          } else {
            if (jaExiste) {
              sensor = *it;
              sensor->Desligar();
              status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
              sensor->MostrarLog("Servo removido via CAN: sSRV_" + String(idNum));
              delete sensor;
              lista.erase(it);
            }
          }
          break;
        }
      }
      return status;
    }

    static void ComandarServo(std::vector<ServoAgro*>& lista, std::vector<uint8_t>& data) {
      if (data.size() < 3) return;
      CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
      uint8_t idNum = data[1];
      for (auto* _servo : lista) {
        if (_servo->ID_Num == idNum) {
          switch (posicao) {
            case CanMessagePosicaoDados::Command1: {
              int novoAngulo = (int)data[2];
              _servo->AtualizarAngulo(novoAngulo);
              break;
            }
          }
          return;
        }
      }
    }


  private:
    bool DebugMode = false;

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

};






#endif