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

#ifndef ServoAgroModel_v2
#define ServoAgroModel_v2

class ServoAgro_v2 : public ComponenteCAN {
  public:
    String ID;
    uint8_t ID_Num = 0;
    bool Incremental = false;
    bool Iniciado = false;
    PinoModel _pino;

    ServoAgro_v2() {}

    ServoAgro_v2(
      const String& id,
      uint8_t idNum,
      bool incremental,
      int pino,
      TiposBarramentos tipoBarramento
    ) :
      ID(id),
      ID_Num(idNum),
      Incremental(incremental),
      _pino(PinoModel(pino, tipoBarramento, _OUTPUT))
    {
    }

    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;
      }

      if (_pino.Definido()) {
        _Servo.detach();
      }

      MostrarLog("Servo Desligado");

      Iniciado = false;
    }

    void RequisitarDados() {
      if (!Iniciado) return;
    }

    void ProcessarComando(std::vector<uint8_t>& data) {
      if (!Iniciado) return;
      if (data.size() < 3) return;
      CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
      uint8_t idNum = data[1];
      if (idNum != ID_Num) return;
      switch (posicao) {
        case CanMessagePosicaoDados::Command1: {
          int novoAngulo = (int)data[2];
          AtualizarAngulo(novoAngulo);
          break;
        }
      }
    }

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


  private:
    bool DebugMode = true;

    Servo _Servo;
    int _Angulo = 0;

    void AtualizarAngulo(int novoAngulo) {
      novoAngulo = constrain(novoAngulo, 0, 180);
      MostrarLog("Angulo atualizado de " + String(_Angulo) + " para " + String(novoAngulo));
      _Angulo = novoAngulo;
      _Servo.write(_Angulo);
    }

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

};






#endif