// Dispositivo: Direcional
// Versão Firmware: 7
// Ultima atualização: 08/05/2024
// Atualização: Otimização de leitura de sensores

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
#define D_Code Dir
#define VERSION 7

#define _pinoLED RGB_BUILTIN

long _baudRate = 115200;

bool Conectado = false;
bool AA_Ref000 = false;
bool AA_Ref090 = false;
bool AA_Ref270 = false;

int _TaxaAmostragem;
int FrequenciaMaxima = 10000;
double _Margem = 0.5;

int dutyCycle = 512;
double _frequencia = 4000;

PinoModel _pinoGeral;

class Motor {
  public:
    // Construtor
    Motor(String desc) {
      _ID = desc;
    }

    // Definições
    String _ID;
    int _canal;
    PinoModel _pinoENA;
    PinoModel _pinoDIR;
    PinoModel _pinoPUL;
    PinoModel _pinoEncoderA;
    PinoModel _pinoEncoderB;
    PinoModel _pinoRef000;
    PinoModel _pinoRef090;
    PinoModel _pinoRef270;
    
    bool Iniciado = false;
    bool Testando = false;
    bool Referenciando = false;

    // Consumo
    double _Angulo;
    double _AgRef;
    Sentido _Sentido;
    
    // Entrada de dados
    double _Angulo_SP;
    double _Angulo_Offset = 0;
    Sentido _Sentido_SP = Parado;
    bool _MotorLigado = false;
    bool _Estabilizar = true;
    bool _Referenciar = true;

    bool E0 = false;
    bool Ag0 = false;
    bool _Ag0 = false;
    bool E90 = false;
    bool Ag90 = false;
    bool _Ag90 = false;
    bool E270 = false;
    bool Ag270 = false;
    bool _Ag270 = false;
    bool Referenciado = false;
    int RefMax = 90;
    int RefMin = -90;
    int ulEncoderA = 0;
    int ulEncoderB = 0;
    double GrausPino = 0;
    bool EA_M = false;
    bool EB_M = false;


    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(_ID + " ja inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      if (_pinoPUL.Definido()) {
        AtualizarPWM();
      }
      _pinoENA.Conectar(true);
      _pinoDIR.Conectar(false);
      _pinoEncoderA.Conectar();
      ulEncoderA = _pinoEncoderA.get();
      EA_M = false;
      _pinoEncoderB.Conectar();
      ulEncoderB = _pinoEncoderB.get();
      EB_M = false;
      _pinoRef000.Conectar();
      Ag0 = _pinoRef000.get() == 1;
      _Ag0 = Ag0;
      _pinoRef090.Conectar();
      Ag90 = _pinoRef090.get() == 1;
      _Ag90 = Ag90;
      _pinoRef270.Conectar();
      Ag270 = _pinoRef270.get() == 1;
      _Ag270 = Ag270;
      
      xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 20 - _canal, &RampaTaskHandle, tskNO_AFFINITY);


      E0 = false;
      E90 = false;
      E270 = false;

      ResetRef(_Referenciar);

      EnviarDadosSerial(_ID + " Iniciado");

      Iniciado = true;

      EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void ResetRef(bool _Ref) {
      _Angulo = 0;
      GrausPino = 0;
      _Sentido_SP = Parado;
      _Sentido = Parado;
      Referenciado = !_Ref;
      Referenciando = _Ref;
      _MotorLigado = _Ref;
    }

    void AtualizarPWM() {
      ledcSetup(_canal, _frequencia, 10);
      ledcAttachPin(_pinoPUL.num, _canal);
      ledcWrite(_canal, dutyCycle);
    }

    void Desligar() {
      if (!Iniciado) {
        EnviarDadosSerial(_ID + " nao esta inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      // Parar a execução das tarefas
      vTaskDelete(RampaTaskHandle);

      // Redefinir as configurações para os valores iniciais
      _pinoENA.Desconectar();
      _pinoDIR.Desconectar();
      _pinoPUL.Desconectar();
      _pinoEncoderA.Desconectar();
      _pinoEncoderB.Desconectar();
      _pinoRef000.Desconectar();
      _pinoRef090.Desconectar();
      _pinoRef270.Desconectar();
      
      
      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

      EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void Testar() {
      Testando = true;

      Testando = false;
    }

  

  private:

    TaskHandle_t RampaTaskHandle = NULL;
    static void RampaTaskWrapper(void *pvParameters) {
      Motor *motor = static_cast<Motor*>(pvParameters);
      motor->RampaTask();
    }

    void RampaTask() {
      unsigned long previousMillisRampa = millis();
    
      while (true) { // Loop infinito mais claro
        // Verifica a conexão antes de prosseguir
        if (!Conectado) {
          // Espera por 1 segundo para reduzir o uso da CPU se não estiver conectado
          vTaskDelay(1000 / portTICK_PERIOD_MS);
          continue; // Pula para a próxima iteração do loop
        }

        // Executa as funções de controle e monitoramento do motor
        AferirPosicao();
        if (Referenciando) {
          ReferenciarMotor();
        } else {
          VerificarAnguloSP();
        }
        Atualizar();
        AferirAnguloAbsoluto();

        // Envia dados via serial em intervalos definidos por _TaxaAmostragem
        if (millis() - previousMillisRampa >= _TaxaAmostragem) {
          previousMillisRampa = millis(); // Atualiza o tempo para o próximo envio
          EnviarDadosSerial(MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoder(_Sentido, _Angulo, Ag0, Ag90, Ag270, Referenciando, Referenciado)));
        }

        // Libera a CPU para outras tarefas por um curto período
        vTaskDelay(1 / portTICK_PERIOD_MS); // Torna o delay mais explícito e ajustado ao tick do sistema
      }
    }

    void Atualizar() {
      _pinoENA.set(!_MotorLigado);
      _pinoDIR.set(_Sentido_SP != Horario);
    }

    void AferirPosicao() {
      if (_pinoEncoderA.Definido() && _pinoEncoderB.Definido()) {
        int leituraEncoderA = _pinoEncoderA.get();
        int leituraEncoderB = _pinoEncoderB.get();

        if (!EA_M) {
          EA_M = leituraEncoderA != ulEncoderA;
          if (EA_M) {
            _Sentido = leituraEncoderA != leituraEncoderB ? Horario : Antihorario;
          }
        }
        if (!EB_M) {
          EB_M = leituraEncoderB != ulEncoderB;
        }


        if (EA_M && EB_M) {

          if (_Sentido == Horario) {
            _Angulo = _Angulo + _Margem;
            _AgRef = _AgRef + _Margem;
          } else if (_Sentido == Antihorario) {
            _Angulo = _Angulo - _Margem;
            _AgRef = _AgRef - _Margem;
          }

          EA_M = false;
          EB_M = false;
        }

        ulEncoderA = leituraEncoderA;
        ulEncoderB = leituraEncoderB;
      }
    }


    void AferirAnguloAbsoluto() {
      AtualizarEstadoSensores();
      if (GrausPino == 0) {
        VerificarReferenciaInicial();
      } else {
        AtualizarAnguloComReferencia();
      }
      AtualizarEstadosAnteriores();
    }

    void AtualizarEstadoSensores() {
      _Ag0 = _pinoRef000.get() == 1;
      _Ag90 = _pinoRef090.get() == 1;
      _Ag270 = _pinoRef270.get() == 1;
    }

    void VerificarReferenciaInicial() {
      VerificarSensor(&_pinoRef000, &_Ag0, &Ag0, &E0, 0);
      VerificarSensor(&_pinoRef090, &_Ag90, &Ag90, &E90, 90);
      VerificarSensor(&_pinoRef270, &_Ag270, &Ag270, &E270, -90);
    }

    void VerificarSensor(PinoModel* pino, bool* estadoAtual, bool* estadoAnterior, bool* evento, double anguloReferencia) {
      if (!pino->Definido()) return;

      if (*estadoAtual && !*estadoAnterior && !*evento) { // Entrou no sensor
        _AgRef = 0;
        *evento = true;
      } else if (!*estadoAtual && *estadoAnterior && *evento) { // Saiu do sensor
        GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef;
        Referenciado = true;
        *evento = false;
      }
    }

    void AtualizarAnguloComReferencia() {
      if (Referenciando || AA_Ref000) {
        AtualizarAnguloNoSensor(&_Ag0, &Ag0, 0);
      }
      if (Referenciando || AA_Ref090) {
        AtualizarAnguloNoSensor(&_Ag90, &Ag90, 90);
      }
      if (Referenciando || AA_Ref270) {
        AtualizarAnguloNoSensor(&_Ag270, &Ag270, -90);
      }
    }

    void AtualizarAnguloNoSensor(bool* estadoAtual, bool* estadoAnterior, double anguloReferencia) {
      if (*estadoAtual != *estadoAnterior) {
        _Angulo = CalculaAnguloPino(anguloReferencia, *estadoAtual);
      }
    }

    void AtualizarEstadosAnteriores() {
      Ag0 = _Ag0;
      Ag90 = _Ag90;
      Ag270 = _Ag270;
    }

    double CalculaAnguloPino(double Inicio, bool Entrando) {
      double MeioPino = (GrausPino / 2.0);
      double Multiplicador = 1.0; 
      if (Entrando) {
        Multiplicador = (_Sentido == Horario ? -1.0 : 1.0);
      }
      else {
        Multiplicador = (_Sentido == Horario ? 1.0 : -1.0);
      }
      double Angulo = Inicio + (MeioPino * Multiplicador) + _Angulo_Offset;
      return Angulo;
    }


    void VerificarAnguloSP() {
      if (_MotorLigado) {
        if (_Angulo_SP > -1) {
          if (_Sentido_SP == Horario && _Angulo >= _Angulo_SP) {
            _MotorLigado = false;
          }
          else if (_Sentido_SP == Antihorario && _Angulo <= (_Angulo_SP * -1)) {
            _MotorLigado = false;
          }
        }
        else {
          if (Referenciado && (_Sentido_SP == Horario && _Angulo >= RefMax)) {
            _MotorLigado = false;
          }
          else if (Referenciado && (_Sentido_SP == Antihorario && _Angulo <= RefMin)) {
            _MotorLigado = false;
          }
        }
      }
      else {
        if (_Estabilizar && _Sentido_SP == Parado && _Angulo != _Angulo_SP) {
          _Sentido_SP = _Angulo > 0 ? Antihorario : Horario;
          _MotorLigado = true;
        }
      }
    }

    void ReferenciarMotor() {
      int AgP1 = RefMax / 2;

      // Verifica se o motor já está na posição de referência (dentro da margem de 0º).
      if (Referenciado && (_Angulo >= -_Margem && _Angulo <= _Margem)) {
          Referenciando = false;
          _MotorLigado = false;
      } else {
        // Inicia o processo de referenciamento se o motor está parado.
        if (_Sentido_SP == Parado) {
          _Angulo_SP = AgP1;
          _Sentido_SP = Horario;
        }
        // Verifica se o motor excedeu a margem de referência enquanto se move no sentido horário.
        else if (_Sentido_SP == Horario && _Angulo > _Margem) {
          _Angulo_SP = -AgP1; // Muda para antihorário para tentar encontrar o ponto de referência.
          _Sentido_SP = Antihorario;
        }
        // Verifica se o motor excedeu a margem de referência enquanto se move no sentido antihorário.
        else if (_Sentido_SP == Antihorario && _Angulo < -_Margem) {
          _Angulo_SP = AgP1; // Muda para horário para tentar encontrar o ponto de referência.
          _Sentido_SP = Horario;
        }
        // Continua o processo de referenciamento conforme a lógica original.
        else if (_Sentido_SP == Horario && _Angulo >= AgP1) {
          _Angulo_SP = -AgP1;
          _Sentido_SP = Antihorario;
        }
        else if (_Sentido_SP == Antihorario && _Angulo <= -AgP1) {
          _Angulo_SP = AgP1 * 2;
          _Sentido_SP = Horario;
        }
        else if (_Sentido_SP == Horario && _Angulo >= AgP1 * 2) {
          _Angulo_SP = -AgP1 * 2;
          _Sentido_SP = Antihorario;
        }
        // Adiciona mais condições conforme necessário para cobrir o processo completo de referenciamento.
      }
    }

};

Motor M1("ET");
Motor M2("EF");
Motor M3("DT");
Motor M4("DF");

Motor* MotorPorID(String ID) {
  Motor* _motor = 
    ID == "ET" ? &M1 : 
    ID == "EF" ? &M2 : 
    ID == "DT" ? &M3 : 
    ID == "DF" ? &M4 : 
    nullptr;
  return _motor;
}

void setup() {
  Serial.begin(_baudRate);
  //iniciarMCP(D_Code, 8, 18, 0x20, false);
}

void loop() {
  std::vector<ProtocoloSerial> protocolos = LerBufferSerialFila();

  for (int i = 0; i < protocolos.size(); i++) {
    ProcessarProtocolo(protocolos[i].protocolo, protocolos[i].funcao, protocolos[i].idMensagem);
  }
}

void ProcessarProtocolo(String Protocolo, F_Code _funcao, int idMensagem) {
  
  if (_funcao == Chk) {
    EnviarDadosSerial(MontarProtocoloVerificacao(D_Code, VERSION));
  }
  else if (_funcao == Cfg) {
    std::vector<String> Partes = SplitString(Protocolo, (char*)",");
    String ID = Partes[0];
    bool Conectar = Partes[1] == "1";
    if (ID == "MD") {
      _TaxaAmostragem = Partes[2].toInt();
      AA_Ref000 = Partes[3] == "1";
      AA_Ref090 = Partes[4] == "1";
      AA_Ref270 = Partes[5] == "1";
      TiposBarramentos _barramento = (TiposBarramentos)Partes[6].toInt();
      int _num = Partes[7].toInt();
      _pinoGeral = PinoModel(_num, _barramento, _OUTPUT);

      if (_pinoGeral.Definido()) {
        _pinoGeral.set(true);
      }

      EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectar ? "1" : "0"));
      vTaskDelay(pdMS_TO_TICKS(100));
      Conectado = Conectar;
    }
    else {
      Motor* _motor = MotorPorID(ID);

      int canal = Partes[2].toInt();
      bool motor_ativado = Partes[3] == "1";
      bool estabilizar = Partes[4] == "1";
      bool referenciar = Partes[5] == "1";
      int _offset = Partes[6].toInt();
      TiposBarramentos pulB = (TiposBarramentos)Partes[7].toInt();
      int pul = Partes[8].toInt();
      TiposBarramentos dirB = (TiposBarramentos)Partes[9].toInt();
      int dir = Partes[10].toInt();
      TiposBarramentos enaB = (TiposBarramentos)Partes[11].toInt();
      int ena = Partes[12].toInt();
      TiposBarramentos encAB = (TiposBarramentos)Partes[13].toInt();
      int encoderA = Partes[14].toInt();
      TiposBarramentos encBB = (TiposBarramentos)Partes[15].toInt();
      int encoderB = Partes[16].toInt();
      TiposBarramentos ref0B = (TiposBarramentos)Partes[17].toInt();
      int ref0 = Partes[18].toInt();
      TiposBarramentos ref90B = (TiposBarramentos)Partes[19].toInt();
      int ref90 = Partes[20].toInt();
      TiposBarramentos ref270B = (TiposBarramentos)Partes[21].toInt();
      int ref270 = Partes[22].toInt();
        
      if (motor_ativado) {
        if (Conectar) {
          _motor->_canal = canal;
          _motor->_Estabilizar = estabilizar;
          _motor->_Referenciar = referenciar;
          _motor->_Angulo_Offset = _offset;
          _motor->_pinoPUL = PinoModel(pul, pulB, _PWM);
          _motor->_pinoDIR = PinoModel(dir, dirB, _OUTPUT);
          _motor->_pinoENA = PinoModel(ena, enaB, _OUTPUT);
          _motor->_pinoEncoderA = PinoModel(encoderA, encAB, _INPUT, DIGITAL);
          _motor->_pinoEncoderB = PinoModel(encoderB, encBB, _INPUT, DIGITAL);
          _motor->_pinoRef000 = PinoModel(ref0, ref0B, _INPUT, DIGITAL);
          _motor->_pinoRef090 = PinoModel(ref90, ref90B, _INPUT, DIGITAL);
          _motor->_pinoRef270 = PinoModel(ref270, ref270B, _INPUT, DIGITAL);
          _motor->Inicializar();
        }
        else {
          _motor->Desligar();
        }
        vTaskDelay(500);
      }
    }
  }
  else if (_funcao == Cmd) {
      std::vector<String> Partes = SplitString(Protocolo, (char*)",");
      String ID = Partes[0];

      Motor* _motor = MotorPorID(ID);

      Sentido sentido = (Sentido)Partes[1].toInt();
      int angulo = Partes[2].toInt();
      int porcentagem = Partes[3].toInt();
      double frequencia = (porcentagem / 100.0) * FrequenciaMaxima;
      bool referenciar = Partes[4] == "1";
      int _offset = Partes[5].toInt();

      _frequencia = frequencia;

      _motor->_Sentido_SP = sentido;
      _motor->_Angulo_SP = angulo;
      _motor->_Angulo_Offset = _offset;

      _motor->AtualizarPWM();

      if (referenciar) {
        _motor->ResetRef(referenciar);
      }
      else {
        _motor->_MotorLigado = sentido != Parado;
      }
  }
  else if (_funcao == Tst) {
    std::vector<String> Partes = SplitString(Protocolo, (char*)",");
    String ID = Partes[0];

    Motor* _motor = MotorPorID(ID);

    bool EmTeste = _motor->Testando;
    if (!EmTeste) {
      _motor->Testar();
    }
        
  }

  EnviarDadosSerial(MontarProtocoloMensagemRecebida(idMensagem));

}