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

#define _pinoLED RGB_BUILTIN
int _pinoReleGeral = -1;

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;
    int _pinoPWM;
    int _pinoDIR;
    int _pinoBRK;
    int _pinoSTP;
    int _pinoHLA;
    int _pinoHLB;
    int _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 > -1) {
        ledcSetup(_canal, 1000, 10);
        ledcAttachPin(_pinoPWM, _canal);
        PotenciaAtual = ZeroRampa;
        _PotMap = PotenciaAtual;
        _RPM_SP = 0;
        ledcWrite(_canal, _PotMap);
      }
      if (_pinoDIR > -1) {
        pinMode(_pinoDIR, OUTPUT);
        digitalWrite(_pinoDIR, LOW);

        _SentidoU = Parado;
      }
      if (_pinoBRK > -1) {
        pinMode(_pinoBRK, OUTPUT);
        digitalWrite(_pinoBRK, _Freio);
      }
      if (_pinoSTP > -1) {
        pinMode(_pinoSTP, OUTPUT);
        digitalWrite(_pinoSTP, HIGH);
      }
      if (_pinoHLA > -1) {
        pinMode(_pinoHLA, INPUT);
        ultimaLeituraHallA = digitalRead(_pinoHLA);
        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);
      }
      if (_pinoHLB > -1) {
        pinMode(_pinoHLB, INPUT);
        ultimaLeituraHallB = digitalRead(_pinoHLB);
      }
      if (_pinoHLC > -1) {
        pinMode(_pinoHLC, INPUT);
        ultimaLeituraHallC = digitalRead(_pinoHLC);
      }


      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 > -1) {
        vTaskDelete(RPMTaskHandle);
      }

      // Desanexar o canal PWM
      if (_pinoPWM > -1) {
        ledcDetachPin(_pinoPWM);
      }
      
      // Redefinir as configurações para os valores iniciais
      pinMode(_pinoPWM, INPUT);
      pinMode(_pinoDIR, INPUT);
      pinMode(_pinoBRK, INPUT);
      pinMode(_pinoSTP, INPUT);
      pinMode(_pinoHLA, INPUT);
      pinMode(_pinoHLB, INPUT);
      pinMode(_pinoHLC, INPUT);

      // 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
        bool LeituraHa = digitalRead(_pinoHLA) == 1;
        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 == -1 || _pinoHLB == -1 || _pinoHLC == -1) {
        CalculaAceleracao(_RPM);
        return;
      }

      bool LeituraHa = ultimaLeituraHallA == 1;
      bool LeituraHb = digitalRead(_pinoHLB) == 1;
      bool LeituraHc = digitalRead(_pinoHLC) == 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 > -1 && _pinoHLB > -1 && _pinoHLC > -1) {
          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 > -1 && Travado) {
        _PotMap = ZeroRampa;
        ledcWrite(_canal, _PotMap);
        vTaskDelay(400);
        _PotMap = PotenciaAtual;
      }

      if (_pinoDIR > -1) {
        bool ReleAtuado = _SentidoSP == Horario;

        if (_SentidoSP != _SentidoU && ReleAtuado) {
          digitalWrite(_pinoDIR, ReleAtuado);
          vTaskDelay(1000);
        }
        
        digitalWrite(_pinoDIR, ReleAtuado);

        _SentidoU = _SentidoSP;
      }
      if (_pinoBRK > -1) {
        digitalWrite(_pinoBRK, _Freio);
      }
      if (_pinoSTP > -1) {
        digitalWrite(_pinoSTP, HIGH);
      }
      if (_pinoPWM > -1) {
        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);
  
  if (_pinoReleGeral > -1) {
    pinMode(_pinoReleGeral, OUTPUT);
    digitalWrite(_pinoReleGeral, LOW);
  }

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

  RecalcularRelacaoPPR();
}

void loop() {
  if (Serial.available() > 3) {
    String Protocolo = "";
    F_Code _funcao = Nda;
    while (Serial.available()) {
      char Entrada = (char)Serial.read();
      if (Entrada == EndLine) {
        break;
      }
      Protocolo += Entrada;
      if (Protocolo.length() == 3) {
        if (_funcao == Nda) {
          _funcao = (F_Code)((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2]).toInt();
          Protocolo = "";
        }
      }
    }

    EnviarDadosSerial("OK");

    if (_funcao == Chk) {
      EnviarDadosSerial(MontarProtocoloVerificacao(D_Code));
    }
    else if (_funcao == Cfg) {
      String ID = ((String)Protocolo[0] + (String)Protocolo[1]);
      bool Conectar = (String)Protocolo[3] == "1";
      if (ID == "MD") {
        _TaxaAmostragem = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7] + (String)Protocolo[8] + (String)Protocolo[9]).toInt();
        int pinoReleGeral = ((String)Protocolo[11] + (String)Protocolo[12]).toInt();
        int _RampaMin = ((String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17]).toInt();
        int _RampaMax = ((String)Protocolo[19] + (String)Protocolo[20] + (String)Protocolo[21] + (String)Protocolo[22]).toInt();
        int _PPR = ((String)Protocolo[24] + (String)Protocolo[25] + (String)Protocolo[26]).toInt();
        int _delayBalanceamento = ((String)Protocolo[28] + (String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31] + (String)Protocolo[32]).toInt();
        int _margemRPM = ((String)Protocolo[34] + (String)Protocolo[35]).toInt();
        int _maxOffsetRPM = ((String)Protocolo[37] + (String)Protocolo[38]).toInt();

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

        if (pinoReleGeral > -1) {
          pinMode(_pinoReleGeral, INPUT);
          _pinoReleGeral = pinoReleGeral;
          pinMode(_pinoReleGeral, OUTPUT);
          digitalWrite(_pinoReleGeral, Conectar);
        }

        RecalcularRelacaoPPR();

        vTaskDelay(pdMS_TO_TICKS(100));
        Conectado = Conectar;
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
      }
      else {
        int canal = ((String)Protocolo[5]).toInt();
        bool _alarme = (String)Protocolo[7] == "1";
        bool motor_ativado = (String)Protocolo[9] == "1";
        double _Kp = ((String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15]).toDouble();
        double _Ki = ((String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19] + (String)Protocolo[20] + (String)Protocolo[21]).toDouble();
        double _Kd = ((String)Protocolo[23] + (String)Protocolo[24] + (String)Protocolo[25] + (String)Protocolo[26] + (String)Protocolo[27]).toDouble();
        int pwm = ((String)Protocolo[29] + (String)Protocolo[30]).toInt();
        int dir = ((String)Protocolo[32] + (String)Protocolo[33]).toInt();
        int brk = ((String)Protocolo[35] + (String)Protocolo[36]).toInt();
        int stp = ((String)Protocolo[38] + (String)Protocolo[39]).toInt();
        int hallA = ((String)Protocolo[41] + (String)Protocolo[42]).toInt();
        int hallB = ((String)Protocolo[44] + (String)Protocolo[45]).toInt();
        int hallC = ((String)Protocolo[47] + (String)Protocolo[48]).toInt();

        Motor* _motor = MotorPorID(ID);
        if (motor_ativado) {
          if (Conectar) {
            _motor->_canal = canal;
            _motor->_pinoPWM = pwm;
            _motor->_pinoDIR = dir;
            _motor->_pinoBRK = brk;
            _motor->_pinoSTP = stp;
            _motor->_pinoHLA = hallA;
            _motor->_pinoHLB = hallB;
            _motor->_pinoHLC = hallC;
            _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) {
      //canal;potencia;sentido;rampa;precisao;rpm
      //0;000;0;000;000;000
      String ID = ((String)Protocolo[0] + (String)Protocolo[1]);

      Motor* _motor = MotorPorID(ID);

      _motor->_SentidoSP = (Sentido)((String)Protocolo[3]).toInt();
      _motor->_RPM_SP = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7]).toInt();
      _motor->_Alarme = ((String)Protocolo[9]) == "1";
      _motor->ReiniciarAceleracaoArr();
    }
    else if (_funcao == Tst) {
      String ID = ((String)Protocolo[0] +(String)Protocolo[1]);
          
      Motor* _motor = MotorPorID(ID);

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

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) {
  while (1)
  {
    int _delay = delayBalanceamento > 0 ? delayBalanceamento : 5000;

    if (Conectado && delayBalanceamento > 0) {
      BalancearCargasMotores();
    }

    vTaskDelay(_delay);
  }
}

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