#ifndef ServoAgroModel_v3
#define ServoAgroModel_v3

#include <Arduino.h>

#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include "IComponenteCAN.h"

/*
 * Controle de servo diretamente pelo periférico LEDC do ESP32-S3.
 *
 * Não depende da biblioteca ESP32Servo.
 *
 * Cada servo precisa utilizar um canal LEDC exclusivo.
 *
 * ESP32-S3:
 *   canais LEDC válidos: 0 a 7
 *   resolução máxima: 14 bits
 */
class ServoAgro_v3 : public ComponenteCAN {
  public:
    String ID;
    uint8_t ID_Num = 0;
    bool Incremental = false;
    bool Iniciado = false;
    PinoModel _pino;

    ServoAgro_v3() {}

    ServoAgro_v3(
      const String& id,
      uint8_t idNum,
      bool incremental,
      int pino,
      TiposBarramentos tipoBarramento,
      uint8_t canalPwm,
      uint16_t pulsoMinUs = 544,
      uint16_t pulsoMaxUs = 2400
    ) :
      ID(id),
      ID_Num(idNum),
      Incremental(incremental),
      _pino(PinoModel(pino, tipoBarramento, _OUTPUT)),
      _canalPwm(canalPwm),
      _pulsoMinUs(pulsoMinUs),
      _pulsoMaxUs(pulsoMaxUs)
    {
    }

    void Inicializar() {
      MostrarLog(
        "Entrou no Inicializar"
        " | pino=" + String(_pino.num) +
        " | canalPWM=" + String(_canalPwm) +
        " | incremental=" + String(Incremental ? 1 : 0)
      );

      if (Iniciado) {
        MostrarLog("Servo ja inicializado");
        return;
      }

      if (!_pino.Definido()) {
        MostrarLog("Servo nao iniciado | pino nao definido");
        return;
      }

      if (_canalPwm >= QuantidadeCanaisLedc) {
        MostrarLog(
          "Servo nao iniciado | canal LEDC invalido: " +
          String(_canalPwm)
        );
        return;
      }

      if (_pulsoMinUs >= _pulsoMaxUs) {
        MostrarLog(
          "Servo nao iniciado | intervalo de pulso invalido"
        );
        return;
      }

      if (!ReservarCanal(_canalPwm)) {
        MostrarLog(
          "Servo nao iniciado | canal LEDC ja reservado: " +
          String(_canalPwm)
        );
        return;
      }

      _canalReservado = true;

      MostrarLog(
        "Configurando LEDC"
        " | frequencia=" + String(FrequenciaHz) +
        "Hz resolucao=" + String(ResolucaoBits) +
        "bits"
      );

      double frequenciaReal = ledcSetup(
        _canalPwm,
        FrequenciaHz,
        ResolucaoBits
      );

      if (frequenciaReal <= 0.0) {
        MostrarLog(
          "Falha no ledcSetup"
          " | canal=" + String(_canalPwm)
        );

        LiberarRecursos();
        return;
      }

      _frequenciaRealHz = static_cast<uint32_t>(frequenciaReal);

      MostrarLog(
        "LEDC configurado"
        " | frequencia_real=" + String(_frequenciaRealHz) +
        "Hz"
      );

      ledcAttachPin(
        static_cast<uint8_t>(_pino.num),
        _canalPwm
        );

        _pinoAnexado = true;

        MostrarLog(
            "Pino anexado ao LEDC"
            " | pino=" + String(_pino.num) +
            " canal=" + String(_canalPwm)
        );

        /*
        * Primeiro aplica um duty válido.
        *
        * No Arduino-ESP32 2.x, ledcReadFreq() retorna zero
        * quando o duty atual do canal é zero.
        */
        int anguloInicial = Incremental ? 0 : 180;

        if (!AplicarAngulo(anguloInicial, true)) {
            MostrarLog("Falha ao aplicar angulo inicial");
            LiberarRecursos();
            return;
        }

        /*
        * Agora o canal já possui duty diferente de zero,
        * então ledcReadFreq() consegue retornar a frequência.
        */
        uint32_t frequenciaConfirmada = ledcReadFreq(_canalPwm);
        if (frequenciaConfirmada != FrequenciaHz) {
            LiberarRecursos();
            return;
        }

        MostrarLog(
            "PWM confirmado"
            " | frequencia=" + String(frequenciaConfirmada) + "Hz" +
            " duty=" + String(ledcRead(_canalPwm)) +
            " angulo=" + String(_Angulo)
        );

        if (frequenciaConfirmada == 0) {
            MostrarLog(
                "Aviso: nao foi possivel ler a frequencia, "
                "mas o ledcSetup retornou " +
                String(_frequenciaRealHz) + "Hz"
            );
        }

        Iniciado = true;

        MostrarLog(
            "Servo iniciado"
            " | angulo=" + String(_Angulo) +
            " pulso=" + String(_pulsoAtualUs) + "us" +
            " duty=" + String(_dutyAtual)
        );
    }

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

      /*
       * Duty zero interrompe os pulsos antes de desconectar o pino.
       */
      if (_canalReservado) {
        ledcWrite(_canalPwm, 0);
      }

      delay(2);

      LiberarRecursos();

      Iniciado = false;

      MostrarLog("Servo desligado");
    }

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

    void ProcessarComando(std::vector<uint8_t>& data) {
      if (!Iniciado) {
        return;
      }

      if (data.size() < 3) {
        return;
      }

      CanMessagePosicaoDados posicao =
        static_cast<CanMessagePosicaoDados>(data[0]);

      uint8_t idNum = data[1];

      if (idNum != ID_Num) {
        return;
      }

      switch (posicao) {
        case CanMessagePosicaoDados::Command1: {
          int novoAngulo = static_cast<int>(data[2]);
          AtualizarAngulo(novoAngulo);
          break;
        }

        default:
          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>(
              constrain(_Angulo, 0, 180)
            )
          );
          break;
        }

        default:
          break;
      }

      return data;
    }

    int ObterAngulo() const {
      return _Angulo;
    }

    uint16_t ObterPulsoUs() const {
      return _pulsoAtualUs;
    }

    uint32_t ObterDuty() const {
      return _dutyAtual;
    }

    uint8_t ObterCanalPwm() const {
      return _canalPwm;
    }

  private:
    /*
     * O ESP32-S3 possui 8 canais LEDC.
     */
    static constexpr uint8_t QuantidadeCanaisLedc = 8;

    /*
     * O ESP32-S3 suporta no máximo 14 bits no LEDC.
     */
    static constexpr uint8_t ResolucaoBits = 14;

    /*
     * Servos convencionais utilizam período de 20 ms.
     */
    static constexpr uint32_t FrequenciaHz = 50;

    bool DebugMode = false;

    uint8_t _canalPwm = 0xFF;

    uint16_t _pulsoMinUs = 544;
    uint16_t _pulsoMaxUs = 2400;

    uint32_t _frequenciaRealHz = 0;

    bool _canalReservado = false;
    bool _pinoAnexado = false;

    int _Angulo = 0;

    uint16_t _pulsoAtualUs = 0;
    uint32_t _dutyAtual = 0;
    bool _sinalAplicado = false;

    /*
     * Um único mapa é compartilhado entre todas as instâncias
     * de ServoAgro_v3.
     */
    static bool* CanaisReservados() {
      static bool canais[QuantidadeCanaisLedc] = {
        false, false, false, false,
        false, false, false, false
      };

      return canais;
    }

    static bool ReservarCanal(uint8_t canal) {
      if (canal >= QuantidadeCanaisLedc) {
        return false;
      }

      bool* canais = CanaisReservados();

      if (canais[canal]) {
        return false;
      }

      canais[canal] = true;
      return true;
    }

    static void LiberarCanal(uint8_t canal) {
      if (canal >= QuantidadeCanaisLedc) {
        return;
      }

      CanaisReservados()[canal] = false;
    }

    static uint32_t DutyMaximo() {
      return (1UL << ResolucaoBits) - 1UL;
    }

    static uint32_t PeriodoUs() {
      return 1000000UL / FrequenciaHz;
    }

    uint16_t AnguloParaPulsoUs(int angulo) const {
      angulo = constrain(angulo, 0, 180);

      uint32_t intervalo =
        static_cast<uint32_t>(_pulsoMaxUs - _pulsoMinUs);

      uint32_t pulso =
        static_cast<uint32_t>(_pulsoMinUs) +
        (
          static_cast<uint32_t>(angulo) *
          intervalo
        ) / 180UL;

      return static_cast<uint16_t>(pulso);
    }

    uint32_t PulsoUsParaDuty(uint16_t pulsoUs) const {
      /*
       * duty = pulso / período * duty máximo
       *
       * A soma de PeriodoUs()/2 realiza arredondamento
       * inteiro em vez de simplesmente truncar.
       */
      uint64_t numerador =
        static_cast<uint64_t>(pulsoUs) *
        static_cast<uint64_t>(DutyMaximo());

      numerador += PeriodoUs() / 2UL;

      return static_cast<uint32_t>(
        numerador / PeriodoUs()
      );
    }

    bool AplicarAngulo(int novoAngulo, bool confirmar = false) {
        if (!_canalReservado || !_pinoAnexado) {
            MostrarLog(
                "Nao foi possivel atualizar angulo"
                " | PWM nao inicializado"
            );

            return false;
        }

        novoAngulo = constrain(novoAngulo, 0, 180);

        /*
        * Só ignora uma posição repetida se um sinal já tiver
        * sido realmente aplicado ao canal LEDC.
        */
        if (_sinalAplicado && novoAngulo == _Angulo) {
            return true;
        }

        uint16_t pulsoUs =
            AnguloParaPulsoUs(novoAngulo);

        uint32_t duty =
            PulsoUsParaDuty(pulsoUs);

        ledcWrite(_canalPwm, duty);

        if (confirmar) {
            /*
            * Frequência de 50 Hz: cada período possui 20 ms.
            */
            vTaskDelay(pdMS_TO_TICKS(25));

            uint32_t dutyConfirmado =
                ledcRead(_canalPwm);

            if (dutyConfirmado != duty) {
                MostrarLog(
                    "Falha ao confirmar duty"
                    " | esperado=" + String(duty) +
                    " lido=" + String(dutyConfirmado)
                );

                return false;
            }
        }

        _Angulo = novoAngulo;
        _pulsoAtualUs = pulsoUs;
        _dutyAtual = duty;
        _sinalAplicado = true;

        return true;
    }

    void AtualizarAngulo(int novoAngulo) {
      if (!Iniciado) {
        MostrarLog(
          "Comando de angulo ignorado"
          " | servo nao iniciado"
        );

        return;
      }

      AplicarAngulo(novoAngulo);
    }

    void LiberarRecursos() {
      if (_pinoAnexado && _pino.Definido()) {
        ledcDetachPin(
          static_cast<uint8_t>(_pino.num)
        );

        /*
         * Depois do detach deixa o sinal explicitamente baixo.
         */
        pinMode(_pino.num, OUTPUT);
        digitalWrite(_pino.num, LOW);

        _pinoAnexado = false;
      }

      if (_canalReservado) {
        LiberarCanal(_canalPwm);
        _canalReservado = false;
      }

      _frequenciaRealHz = 0;
      _pulsoAtualUs = 0;
      _dutyAtual = 0;
      _sinalAplicado = false;
    }

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

#endif