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

#ifndef BicoModel
#define BicoModel

class Bico : public ComponenteCAN {
  public:

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

    String Mod_ID;
    String _ID;
    int ID_Num;
    PinoModel _pino;
    bool Iniciado = false;

    bool AtuacaoNivelAlto = true;

    Estado _Status = Desligado;

    ServoAgro* servoMov = nullptr;

    void Inicializar() {
      if (Iniciado) {
        MostrarLog("Bico ja inicializado");
        return;
      }

      if (servoMov == nullptr) {
        PrintTela("Servo nao definido, bico nao tera controle de movimento");
        //Iniciado = false;
        //return;
      }
      
      _pino.Conectar(!AtuacaoNivelAlto);

      Iniciado = _pino.Definido();

      if (Iniciado) {
        MostrarLog("Bico iniciado");
      }
      else
      {
        MostrarLog("Bico nao iniciado");
      }
      
    }

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

      servoMov = nullptr;

      // Redefinir as configurações para os valores iniciais
      _pino.Desconectar();

      MostrarLog("Bico Desligado");

      Iniciado = false;
    }

    void AtualizarStatus(Estado novoEstado, int novoAngulo) {
      if (Iniciado) {
        bool status = novoEstado == Ligado;
        if (!AtuacaoNivelAlto) {
          status = !status;
        }

        MostrarLog("Atualizando status do bico de " + String(_Status) + " para " + String(novoEstado));
        
        _pino.set(status);
        _Status = (Estado)_pino.get();

        if (servoMov != nullptr) {
          servoMov->AtualizarAngulo(novoAngulo);
        }
      }
    }

    void AtualizarLeitura() {
      if (Iniciado) {
        _Status = (Estado)_pino.get();
      }
    }

    void RequisitarDados() {
      AtualizarLeitura();
    }

    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: {
          int angulo = 0;
          if (servoMov != nullptr) {
            angulo = servoMov->_Angulo;
          }
          data.push_back(static_cast<uint8_t>(_Status));
          data.push_back(angulo);
          break;
        }
      }
      return data;
    }

    static std::vector<uint8_t> ConfigurarBico(std::vector<Bico*>& lista, std::vector<uint8_t>& data, const String& Mod_ID, std::vector<ServoAgro*>& servos) {
      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](Bico* s) { return s->ID_Num == idNum; });
      bool jaExiste = it != lista.end();
      Bico* bico;
      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 8) return status;
          bool conectar = data[3] == 1;
          bool atuacaoNivelAlto = data[4] == 1;
          int servo_id = (data[5] == 0xFF) ? -1 : (int)data[5];
          TiposBarramentos tipoBarramento = (TiposBarramentos)data[6];
          int pino = data[7];
          if (conectar) {
            if (!jaExiste) {
              bico = new Bico(Mod_ID, "sBIC_" + String(idNum));
            }
            else {
              bico = *it;
            }
            if (!bico->Iniciado) {
              bool com_movimento = servo_id >= 0;
              if (com_movimento) {
                if (servos.size() > 0) {
                  auto srv = std::find_if(servos.begin(), servos.end(), [servo_id](ServoAgro* s) { return s->ID_Num == servo_id; });
                  bool servo_encontrado = srv != servos.end();
                  if (servo_encontrado && (*srv)->Iniciado) {
                    bico->servoMov = *srv;
                  }
                }
                if (bico->servoMov == nullptr) {
                  PrintTela("O servo " + String(servo_id) + " nao foi encontrado para ser associado ao bico.");
                  delete bico;
                  return status;
                }
              }
              bico->ID_Num = idNum;
              bico->_pino = PinoModel(pino, tipoBarramento, _OUTPUT);
              bico->AtuacaoNivelAlto = atuacaoNivelAlto;
              bico->Inicializar();
              if (!jaExiste) {
                lista.push_back(bico);
                bico->MostrarLog("Bico adicionado via CAN: sBIC_" + String(idNum));
              }
            }
            status = bico->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          } else {
            if (jaExiste) {
              bico = *it;
              bico->Desligar();
              status = bico->MontarMensagemCAN(CanMessagePosicaoDados::Status);
              bico->MostrarLog("Bico removido via CAN: sBIC_" + String(idNum));
              delete bico;
              lista.erase(it);
            }
          }
          break;
        }
      }
      return status;
    }

    static void ComandarBico(std::vector<Bico*>& lista, std::vector<uint8_t>& data) {
      if (data.size() < 4) return;
      CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
      uint8_t idNum = data[1];
      for (auto* _bico : lista) {
        if (_bico->ID_Num == idNum) {
          switch (posicao) {
            case CanMessagePosicaoDados::Command1: {
              Estado novoEstado = (Estado)data[2];
              int novoAngulo = data[3];
              _bico->AtualizarStatus(novoEstado, novoAngulo);
              break;
            }
          }
          return;
        }
      }
    }


  private:
    bool DebugMode = false;

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

};



#endif