// Dispositivo: Movimentação
// Versão Firmware: 18
// Ultima atualização: 26/04/2024
// Atualização: Adicionado enfileiramento de mensageria

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\utils.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
#include <PID_v1.h>
#define D_Code Mov
#define VERSION 18

#define _pinoLED RGB_BUILTIN

long _baudRate = 115200;
bool Conectado = false;
int _TaxaAmostragem = 500;

int PPR = 22; // Pulsos por revolução
int RampaMin = 0;
int ZeroRampa = 0;
int RampaMax = 1023;
const int RpmMax = 650; // RPM Máximo aferir
float RelacaoPPR = 0;
float MenorPeriodo = 0;

int delayBalanceamento = 5000;
float RPM_Offset_Max = 25.0;
float RPM_Margem = 5.0;
float RPM_SetPoint = 0.0;
float RPM_Medio = 0.0;
bool RPM_Estavel = true;

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

    // Definições
    String _ID;
    int _canal;
    PinoModel _pinoPWM;
    PinoModel _pinoDIR;
    PinoModel _pinoBRK;
    PinoModel _pinoSTP;
    PinoModel _pinoHLA;
    PinoModel _pinoHLB;
    PinoModel _pinoHLC;

    double _Kp;
    double _Ki;
    double _Kd;
    PID* _PID = nullptr;

    bool Iniciado = false;
    bool Testando = false;
    bool Revertendo = false;
    bool SentidoContrario = false;
    StatusMotor Aceleracao = Estavel;
    StatusMotor AceleracaoA = Estavel;

    // Consumo
    const int leituras = 5;
    volatile float RPM_arr[5];
    volatile bool Hall_arr[5][3];
    double RPM;
    double PotenciaAtual;
    volatile int ultimaLeituraHallA = 0;
    volatile int ultimaLeituraHallB = 0;
    volatile int ultimaLeituraHallC = 0;
    volatile unsigned int tempoAnterior = 0;
    volatile float periodo = 0;

    // Entrada de dados
    bool _MalhaFechada = true;
    int _PotMap;
    double _RPM_SP;
    Sentido _SentidoSP = Parado;
    Sentido _Sentido = Parado;
    Sentido _SentidoU = Parado;
    int _US_Addr;
    bool _Alarme = false;
    bool _Freio = false;
    int _Margem = 2;
    double _RPM_Offset = 0;

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

      if (_pinoPWM.Definido()) {
        ledcSetup(_canal, 1000, 10);
        ledcAttachPin(_pinoPWM.num, _canal);
        PotenciaAtual = ZeroRampa;
        _PotMap = PotenciaAtual;
        _RPM_SP = 0;
        ledcWrite(_canal, _PotMap);
      }
      _pinoDIR.Conectar(false);
      _SentidoU = Parado;
      _pinoBRK.Conectar(_Freio);
      _pinoSTP.Conectar(true);
      if (_pinoHLA.Definido()) {
        _pinoHLA.Conectar();
        ultimaLeituraHallA = _pinoHLA.get();
        RPM = 0;
        periodo = 0;
        tempoAnterior = 0;
        
        xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 15000, this, 25 - _canal, &RPMTaskHandle, tskNO_AFFINITY);

        ReiniciarAceleracaoArr();
        ReiniciarHallArr();

        _PID = new PID(&RPM, &PotenciaAtual, &_RPM_SP, _Kp, _Ki, _Kd, DIRECT);
        _PID->SetOutputLimits(RampaMin, RampaMax);
        _PID->SetMode(AUTOMATIC);
      }
      _pinoHLB.Conectar();
      ultimaLeituraHallB = _pinoHLB.get();
      _pinoHLC.Conectar();
      ultimaLeituraHallC = _pinoHLC.get();


      xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 21 - _canal + 4, &RampaTaskHandle, tskNO_AFFINITY);

      EnviarDadosSerial(_ID + " Iniciado");
      
      Iniciado = true;

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

    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);
      if (_pinoHLA.Definido()) {
        vTaskDelete(RPMTaskHandle);
      }

      // Redefinir as configurações para os valores iniciais
      _pinoPWM.Desconectar();
      _pinoPWM.Desconectar();
      _pinoDIR.Desconectar();
      _pinoBRK.Desconectar();
      _pinoSTP.Desconectar();
      _pinoHLA.Desconectar();
      _pinoHLB.Desconectar();
      _pinoHLC.Desconectar();

      // Outras redefinições de variáveis de estado, se necessário
      delete _PID;

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

    void ReiniciarAceleracaoArr() {
      for (int i = 0; i < leituras; i++) {
        RPM_arr[i] = -1.0;
      }
    }

    void ReiniciarHallArr() {
      for (int i = 0; i < leituras; i++) {
        Hall_arr[i][0] = false;
        Hall_arr[i][1] = false;
        Hall_arr[i][2] = false;
      }
    }

  private:

    TaskHandle_t RPMTaskHandle = NULL;
    static void RPMTaskWrapper(void *pvParameters) {
      Motor *motor = static_cast<Motor*>(pvParameters);
      motor->RPMTask();
    }

    void RPMTask() {
      unsigned long firstMilis = millis();
      int pulsos = 0;
      while (1)
      {
        if (!Conectado) {
          // Caso não esteja conectado, aguarda 1 segundo até a próxima verificação, liberando uso de CPU
          vTaskDelay(1000);
          continue;
        }
        
        // Medir RPM apenas quando ocorrer um pulso no sensor HALL
        int LeituraHa = _pinoHLA.get();
        if (LeituraHa != ultimaLeituraHallA) {
          ultimaLeituraHallA = LeituraHa;
          pulsos++;

          // Calcular o período em microssegundos
          unsigned long tempoAtual = micros();
          unsigned long periodoUs = tempoAtual - tempoAnterior;
          periodo = periodoUs / 1000.0; // Converte de us para ms (2300 us para 2,3 ms)
          
          // Salva o valor do RPM anterior
          double RPM_A = RPM;

          // Se o período entre pulsos for menor que o tempo mínimo entre pulsos em ms, significa que o sensor está em uma posição em que existe oscilação de leitura, 
          // pois o RPM estaria acima do máximo, logo, assumir o valor de RPM aferido anteriormente
          if (periodo < MenorPeriodo) {
            RPM = RPM_A;
          }
          // Calcular RPM se houve variação de tempo entre o pulso atual e o pulso anterior
          else if (periodo != 0) {
            // RPM = 60 / (Pulsos por Revolução * Período em segundos)
            // A fórmula foi adaptada para otimizar processamento
            RPM = RelacaoPPR / periodo;
          }
          // Se não houve alteração no período, então o motor não se moveu
          else {
            RPM = 0;
          }

          CalcularSentidoGiro(RPM);

          // Reiniciar variáveis
          tempoAnterior = tempoAtual;
        }

        CorrigirPotenciaMotor();

        long intMillis = (millis() - firstMilis);

        // A cada _TaxaAmostragem, enviar os dados para o software
        if (intMillis >= _TaxaAmostragem) {
          if (pulsos == 0) {
            RPM = 0;
            periodo = 0;
            CalcularSentidoGiro(RPM);
          }
          pulsos = 0;

          double PotAtual = fmap(PotenciaAtual, RampaMin, RampaMax, 0.0, 100.0);
          EnviarDadosSerial(MontarProtocoloSensor(sRPM, _ID, MontarDadosProtocoloRPM(RPM, PotAtual, Aceleracao, _Sentido, Revertendo, _RPM_Offset)));
          firstMilis = millis();
        }

        // Aguarda metade do menor período possível entre leituras
        vTaskDelay(1);
      }
    }

    void CalcularSentidoGiro(float _RPM) {
      if (!_pinoHLA.Definido() || !_pinoHLB.Definido() || !_pinoHLC.Definido()) {
        CalculaAceleracao(_RPM);
        return;
      }

      bool LeituraHa = ultimaLeituraHallA == 1;
      bool LeituraHb = _pinoHLB.get() == 1;
      bool LeituraHc = _pinoHLC.get() == 1;

      for (int i = 1; i < leituras; i++) {
        Hall_arr[i - 1][0] = Hall_arr[i][0];
        Hall_arr[i - 1][1] = Hall_arr[i][1];
        Hall_arr[i - 1][2] = Hall_arr[i][2];
      }

      Hall_arr[leituras - 1][0] = LeituraHa;
      Hall_arr[leituras - 1][1] = LeituraHb;
      Hall_arr[leituras - 1][2] = LeituraHc;

      // Leituras dos sensores Hall nas leituras anteriores
      bool LeituraAnteriorHa = Hall_arr[leituras - 2][0];
      bool LeituraAnteriorHb = Hall_arr[leituras - 2][1];
      bool LeituraAnteriorHc = Hall_arr[leituras - 2][2];


      // Comparação das leituras atuais com as leituras anteriores
      if (LeituraHa == LeituraAnteriorHc && LeituraHc == LeituraAnteriorHa) {
        _Sentido = Antihorario;
      }
      else if (LeituraHa == LeituraAnteriorHb && LeituraHb == LeituraAnteriorHa) {
        _Sentido = Horario;
      }
      else {
        // Outros casos (não determinados)
      }

      CalculaAceleracao(_RPM);

    }

    void CalculaAceleracao(float _RPM) {
      float RPM_total = _RPM;
      int Desconsiderar = 0;
      // Salvar as ultimas leituras
      for (int i = 1; i < leituras; i++) {
        RPM_arr[i - 1] = RPM_arr[i];
        
        RPM_total += RPM_arr[i - 1] < 0 ? 0 : RPM_arr[i - 1];
        Desconsiderar += RPM_arr[i - 1] < 0 ? 1 : 0;
      }
      RPM_arr[leituras - 1] = _RPM;

      float RPM_medio = RPM_total / (leituras - Desconsiderar);

      AceleracaoA = Aceleracao;

      if (_SentidoSP == Parado && _RPM == 0) {
        Aceleracao = Estavel;
      } else if (RPM_arr[leituras - 3] < (RPM_arr[leituras - 2] - _Margem) && RPM_arr[leituras - 2] < (_RPM - _Margem)) {
        Aceleracao = Acelerando;
      } else if (RPM_arr[leituras - 3] > (RPM_arr[leituras - 2] + _Margem) && RPM_arr[leituras - 2] > (_RPM + _Margem)) {
        Aceleracao = Desacelerando;
      } else {
        Aceleracao = Estavel;
      }

    }

    void CorrigirPotenciaMotor() {
      if (_MalhaFechada && !Revertendo && _SentidoSP != Parado) {
        if (_pinoHLA.Definido() && _pinoHLB.Definido() && _pinoHLC.Definido()) {
          SentidoContrario = _Sentido != _SentidoSP;
        }

        if (SentidoContrario) {
          RPM = RPM * -1;
        }
        /*else if (Aceleracao == Desacelerando && AceleracaoA == Estavel) {
          RPM = 0;
        }*/

        float _rpmA = RPM;
        if (RPM_Estavel) {
          RPM = RPM + _RPM_Offset;
        }
        
        _PID->Compute();
        
        RPM = _rpmA;
      }
    }

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

    void RampaTask() {
      unsigned long firstMilis = millis();
      while (1)
      {
        if (!Conectado) {
          // Caso não esteja conectado, aguarda 1 segundo até a próxima verificação, liberando uso de CPU
          vTaskDelay(1000);
          continue;
        }

        Atualizar();

        vTaskDelay(1);
      }
    }

    void Atualizar() {

      bool Travado = _PotMap > ZeroRampa && RPM_arr[leituras - 1] == 0 && RPM_arr[leituras - 2] > 0 && RPM_arr[leituras - 3] > 0 && _Sentido != Parado;

      if (_pinoPWM.Definido() && Travado) {
        _PotMap = ZeroRampa;
        ledcWrite(_canal, _PotMap);
        vTaskDelay(400);
        _PotMap = PotenciaAtual;
      }

      if (_pinoDIR.Definido()) {
        bool ReleAtuado = _SentidoSP == Horario;

        if (_SentidoSP != _SentidoU && ReleAtuado) {
          _pinoDIR.set(ReleAtuado);
          vTaskDelay(1000);
        }
        
        _pinoDIR.set(ReleAtuado);

        _SentidoU = _SentidoSP;
      }
      if (_pinoBRK.Definido()) {
        _pinoBRK.set(_Freio);
      }
      if (_pinoSTP.Definido()) {
        _pinoSTP.set(HIGH);
      }
      if (_pinoPWM.Definido()) {
        if (_SentidoSP == Parado) {
          _PotMap = ZeroRampa;
        }
        else {
          _PotMap = PotenciaAtual;
        }
        ledcWrite(_canal, _PotMap);
      }
    }

};

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 BalanceamentoTask(void *pvParameters);
TaskHandle_t BalanceamentoTaskHandle = NULL;

void setup() {
  Serial.begin(_baudRate);

  delay(10);

  xTaskCreatePinnedToCore(BalanceamentoTask, "BalanceamentoTask", 8000, NULL, 5, &BalanceamentoTaskHandle, tskNO_AFFINITY);

  RecalcularRelacaoPPR();
}

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();
      int _RampaMin = Partes[3].toInt();
      int _RampaMax = Partes[4].toInt();
      int _PPR = Partes[5].toInt();
      int _delayBalanceamento = Partes[6].toInt();
      int _margemRPM = Partes[7].toInt();
      int _maxOffsetRPM = Partes[8].toInt();

      RampaMin = _RampaMin;
      RampaMax = _RampaMax;
      PPR = _PPR;
      delayBalanceamento = _delayBalanceamento;
      RPM_Margem = _margemRPM;
      RPM_Offset_Max = _maxOffsetRPM;

      RecalcularRelacaoPPR();

      EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectar ? "1" : "0"));
      vTaskDelay(pdMS_TO_TICKS(100));
      Conectado = Conectar;
    }
    else {
      int canal = Partes[2].toInt();
      bool _alarme = Partes[3] == "1";
      bool motor_ativado = Partes[4] == "1";
      double _Kp = Partes[5].toDouble();
      double _Ki = Partes[6].toDouble();
      double _Kd = Partes[7].toDouble();
      TiposBarramentos pwmB = (TiposBarramentos)Partes[8].toInt();
      int pwm = Partes[9].toInt();
      TiposBarramentos dirB = (TiposBarramentos)Partes[10].toInt();
      int dir = Partes[11].toInt();
      TiposBarramentos brkB = (TiposBarramentos)Partes[12].toInt();
      int brk = Partes[13].toInt();
      TiposBarramentos stpB = (TiposBarramentos)Partes[14].toInt();
      int stp = Partes[15].toInt();
      TiposBarramentos hlAB = (TiposBarramentos)Partes[16].toInt();
      int hallA = Partes[17].toInt();
      TiposBarramentos hlBB = (TiposBarramentos)Partes[18].toInt();
      int hallB = Partes[19].toInt();
      TiposBarramentos hlCB = (TiposBarramentos)Partes[20].toInt();
      int hallC = Partes[21].toInt();

      Motor* _motor = MotorPorID(ID);
      if (motor_ativado) {
        if (Conectar) {
          _motor->_canal = canal;
          _motor->_pinoPWM = PinoModel(pwm, pwmB, _PWM);
          _motor->_pinoDIR = PinoModel(dir, dirB, _OUTPUT);
          _motor->_pinoBRK = PinoModel(brk, brkB, _OUTPUT);
          _motor->_pinoSTP = PinoModel(stp, stpB, _OUTPUT);
          _motor->_pinoHLA = PinoModel(hallA, hlAB, _INPUT, DIGITAL);
          _motor->_pinoHLB = PinoModel(hallB, hlBB, _INPUT, DIGITAL);
          _motor->_pinoHLC = PinoModel(hallC, hlCB, _INPUT, DIGITAL);
          _motor->_Alarme = _alarme;
          _motor->_Kp = _Kp;
          _motor->_Ki = _Ki;
          _motor->_Kd = _Kd;
                
          _motor->Inicializar();
        }
        else {
          _motor->Desligar();
        }
        vTaskDelay(pdMS_TO_TICKS(500));
      }
    }
  }
  else if (_funcao == Cmd) {
    std::vector<String> Partes = SplitString(Protocolo, (char*)",");

    String ID = Partes[0];

    Motor* _motor = MotorPorID(ID);

    _motor->_SentidoSP = (Sentido)Partes[1].toInt();
    _motor->_RPM_SP = Partes[2].toInt();
    _motor->_Alarme = Partes[3] == "1";
    _motor->ReiniciarAceleracaoArr();
  }
  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));

}

void RecalcularRelacaoPPR() {
  RelacaoPPR = (60 / PPR) * 1000; // Multiplicador do cálculo de RPM (período em ms)
  MenorPeriodo = RelacaoPPR / RpmMax; // Tempo mínimo de leitura
}

void BalanceamentoTask(void *pvParameters) {
  unsigned long firstMilis = millis();
  int _delay = delayBalanceamento > 0 ? delayBalanceamento : 5000;

  while (1)
  {
    long intMillis = (millis() - firstMilis);

    if (Conectado && intMillis >= _delay) {
      BalancearCargasMotores();
      firstMilis = millis();
    }

    vTaskDelay(1);
  }
}

void BalancearCargasMotores() {
  RPM_Medio = (M1.RPM + M2.RPM + M3.RPM + M4.RPM) / 4.0;
  RPM_SetPoint = (M1._RPM_SP + M2._RPM_SP + M3._RPM_SP + M4._RPM_SP) / 4.0;
  RPM_Estavel = (RPM_Medio >= (RPM_SetPoint - RPM_Margem)) && (RPM_Medio <= (RPM_SetPoint + RPM_Margem));

  double M1_Pot = M1.PotenciaAtual;
  double M2_Pot = M2.PotenciaAtual;
  double M3_Pot = M3.PotenciaAtual;
  double M4_Pot = M4.PotenciaAtual;

  double mediaPotencia = (M1_Pot + M2_Pot + M3_Pot + M4_Pot) / 4.0;

  // Certifique-se de verificar se a potência atual não é zero antes de dividir
  double offsetM1_RPM = (M1_Pot != 0 ? (M1.RPM * (mediaPotencia - M1_Pot)) / M1_Pot : 0) * -1;
  double offsetM2_RPM = (M2_Pot != 0 ? (M2.RPM * (mediaPotencia - M2_Pot)) / M2_Pot : 0) * -1;
  double offsetM3_RPM = (M3_Pot != 0 ? (M3.RPM * (mediaPotencia - M3_Pot)) / M3_Pot : 0) * -1;
  double offsetM4_RPM = (M4_Pot != 0 ? (M4.RPM * (mediaPotencia - M4_Pot)) / M4_Pot : 0) * -1;

  // Aplica o offset com limitação
  M1._RPM_Offset = fmin(fmax(offsetM1_RPM, -RPM_Offset_Max), RPM_Offset_Max);
  M2._RPM_Offset = fmin(fmax(offsetM2_RPM, -RPM_Offset_Max), RPM_Offset_Max);
  M3._RPM_Offset = fmin(fmax(offsetM3_RPM, -RPM_Offset_Max), RPM_Offset_Max);
  M4._RPM_Offset = fmin(fmax(offsetM4_RPM, -RPM_Offset_Max), RPM_Offset_Max);
}