#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\MemoryService.h"
#define D_Code Mov

long _baudRate = 115200;

bool Conectado = false;

int _TaxaAmostragem = 500;
const int PPR = 22; // Pulsos por revolução
bool M1_Ativado = false;
bool M2_Ativado = false;
bool M3_Ativado = false;
bool M4_Ativado = false;

int RampaMin = 0;
int ZeroRampa = 0;
int RampaMax = 1023;

class Motor {
public:
  // Construtor
  Motor(String desc, int SentidoAddr) {
    _ID = desc;
    _US_Addr = SentidoAddr;
  }

  // Definições
  String _ID;
  int _canal;
  int _pinoPWM;
  int _pinoVEL;
  int _pinoDIR;
  int _pinoBRK;
  int _pinoSTP;

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

  // Consumo
  const int leituras = 5;
  volatile float RPM_arr[5];
  volatile float RPM;
  double PotenciaAtual;
  volatile int ultimaLeituraHall = 0;
  volatile unsigned int tempoAnterior = 0;
  volatile float periodo = 0;

  // Entrada de dados
  bool _FOC;
  int _Rampa;
  int _Precisao;
  int _FatorDivisor = 1;
  double _Potencia_SP;
  double _Potencia_SP_A;
  int _PotMap;
  double _RPM_SP;
  Sentido _Sentido = Parado;
  Sentido _SentidoA = Parado;
  Sentido _SentidoM = Parado;
  int _US_Addr;
  bool _Freio = false;
  bool _RampaAtivada = false;
  int _Margem = 2;

  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);
      ledcWrite(_canal, ZeroRampa);
    }
    if (_pinoVEL > -1) {
      pinMode(_pinoVEL, INPUT);
      ultimaLeituraHall = digitalRead(_pinoVEL);
      RPM = 0;
      periodo = 0;
      tempoAnterior = 0;
      
      xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 5000, this, _canal, &RPMTaskHandle, 0);

      ReiniciarAceleracaoArr();
    }
    if (_pinoDIR > -1) {
      pinMode(_pinoDIR, OUTPUT);
      digitalWrite(_pinoDIR, LOW);

      _SentidoM = (Sentido)LerMemoria(_US_Addr);
    }
    if (_pinoBRK > -1) {
      pinMode(_pinoBRK, OUTPUT);
      digitalWrite(_pinoBRK, LOW);
    }
    if (_pinoSTP > -1) {
      pinMode(_pinoSTP, OUTPUT);
      digitalWrite(_pinoSTP, LOW);
    }

    xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 18 - _canal, &RampaTaskHandle, 0);

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

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

  void Desligar() {
    if (!Iniciado) {
      EnviarDadosSerial(_ID + " nao esta inicializado");
      return;
    }

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

    // Desanexar o canal PWM
    ledcDetachPin(_pinoPWM);
    
    // Redefinir as configurações para os valores iniciais
    pinMode(_pinoPWM, INPUT);
    pinMode(_pinoVEL, INPUT);
    pinMode(_pinoDIR, INPUT);
    pinMode(_pinoBRK, INPUT);
    pinMode(_pinoSTP, INPUT);

    // Outras redefinições de variáveis de estado, se necessário
    RPM = 0;
    periodo = 0;
    tempoAnterior = 0;

    EnviarDadosSerial(_ID + " Desligado");

    Iniciado = false;

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

  void Testar() {
    Testando = true;
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, "Testando motor " + _ID + "..."));
    TestePino("DIR", _pinoDIR);
    TestePino("BRK", _pinoBRK);
    TestePino("STP", _pinoSTP);
    TestePWM();
    //TesteAcionamentoMotores();
    Testando = false;
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, "Teste finalizado"));
  }

  void TestePino(String Descricao, int Pino) {
    EnviarDadosSerial("Pino " + ((String)Pino) + ": " + Descricao);
    digitalWrite(Pino, LOW);
    vTaskDelay(100);
    digitalWrite(Pino, HIGH);
    vTaskDelay(100);
    int Estado = digitalRead(Pino);
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, Descricao, ((String)((Estado == 1) ? "SUCESSO" : "FALHA")), "1", ((String)Estado))));
    vTaskDelay(1000);
    digitalWrite(Pino, LOW);
    vTaskDelay(100);
    Estado = digitalRead(Pino);
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, Descricao, ((String)((Estado == 0) ? "SUCESSO" : "FALHA")), "0", ((String)Estado))));
    vTaskDelay(1000);
  }

  void TestePWM() {
    EnviarDadosSerial("Pino " + ((String)_pinoPWM) + ": Canal " + ((String)_canal));
    AtualizaTestePWM(0);
    AtualizaTestePWM(255);
    AtualizaTestePWM(512);
    AtualizaTestePWM(768);
    AtualizaTestePWM(1024);
  }

  void AtualizaTestePWM(int Comando) {
    ledcWrite(_canal, Comando);
    vTaskDelay(100);
    int Leitura = analogRead(_pinoPWM);
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, "PWM", (Leitura == Comando ? "SUCESSO" : "FALHA"), (String)Comando, (String)Leitura)));
    vTaskDelay(500);
  }

  void TesteAcionamentoMotores() {
    EnviarDadosSerial("Sentido Horario");
    PotenciaAtual = 20.0;
    _Sentido = Horario;
    Atualizar();
    vTaskDelay(2000);
    EnviarDadosSerial("Sentido Parado");
    _Sentido = Parado;
    PotenciaAtual = 0.0;
    Atualizar();
    vTaskDelay(2000);
    EnviarDadosSerial("Sentido Antihorario");
    _Sentido = Antihorario;
    PotenciaAtual = 20.0;
    Atualizar();
    vTaskDelay(2000);
    EnviarDadosSerial("Sentido Parado");
    _Sentido = Parado;
    PotenciaAtual = 0.0;
    Atualizar();
    vTaskDelay(2000);
  }

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

private:

  void Atualizar() {
    if (_pinoDIR > -1) {
      if (_SentidoA != Parado && _SentidoA != _SentidoM) {
        Revertendo = true;
        _SentidoM = _SentidoA;
        GravarMemoria(_US_Addr, (int)_SentidoM);

        digitalWrite(_pinoSTP, HIGH);
        digitalWrite(_pinoDIR, HIGH);
        vTaskDelay(1500);
        digitalWrite(_pinoSTP, LOW);
        digitalWrite(_pinoDIR, LOW);
        Revertendo = false;
      }
    }
    if (_pinoBRK > -1) {
      digitalWrite(_pinoBRK, _Freio);
    }
    if (_pinoSTP > -1) {
      digitalWrite(_pinoSTP, _SentidoA != Parado);
    }
    if (_pinoPWM > -1) {
      EnviarDadosSerial((String)PotenciaAtual);
      _PotMap = map(PotenciaAtual, ZeroRampa, RampaMax, RampaMin, RampaMax);
      if (PotenciaAtual == ZeroRampa && _Sentido != Parado) {
        ledcWrite(_canal, ZeroRampa);
        vTaskDelay(500);
      }
      else {
        ledcWrite(_canal, _PotMap);
      }
    }
  }

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

  void RPMTask() {
    const int RpmMax = 650; // RPM Máximo aferir
    const float RelacaoPPR = (60 / PPR) * 1000; // Multiplicador do cálculo de RPM (período em ms)
    const float MenorPeriodo = RelacaoPPR / RpmMax; // Tempo mínimo de leitura
    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 Leitura = digitalRead(_pinoVEL);
      if (Leitura != ultimaLeituraHall) {
        ultimaLeituraHall = Leitura;
        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;
        }

        CalculaAceleracao(RPM);

        // Reiniciar variáveis
        tempoAnterior = tempoAtual;
      }

      long intMillis = (millis() - firstMilis);

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

        double PotAtual = map(_PotMap, RampaMin, RampaMax, 0, 100);
        EnviarDadosSerial(MontarProtocoloSensor(sRPM, _ID, MontarDadosProtocoloRPM(RPM, PotAtual, _Potencia_SP, periodo, Aceleracao)));
        firstMilis = millis();

        //CorrigePotenciaMotor();
      }

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

  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;

    if (Desconsiderar > 0 || _RampaAtivada) {
      if (_Potencia_SP < _Potencia_SP_A) {
        Aceleracao = Desacelerando;
      }
      else {
        Aceleracao = Acelerando;
      }
      return;
    }

    //float RPM_medio = RPM_total / (leituras - Desconsiderar);
    float RPM_medio = RPM_arr[leituras - 2];
    int mg = 5;

    if (_SentidoA == Parado && _RPM == 0) {
      Aceleracao = Estavel;
      _Potencia_SP_A = 0;
    } else if ((_RPM + mg) < RPM_medio) {
      Aceleracao = Desacelerando;
    } else if ((_RPM - mg) > RPM_medio) {
      Aceleracao = Acelerando;
    } else {
      Aceleracao = Estavel;
    }
  }

  void CorrigePotenciaMotor() {
    if (_FOC && !Revertendo && !_RampaAtivada && Aceleracao == Estavel && (((RPM + _Margem) < _RPM_SP) || ((RPM - _Margem) > _RPM_SP))) {
      _Potencia_SP_A = _Potencia_SP;
      float PA = PotenciaAcrescentar();
      _Potencia_SP += PA;

      if (_Potencia_SP > 100) {
        _Potencia_SP = 100;
      }
      else if (_Potencia_SP < 1) {
        _Potencia_SP = 1;
      }

      if (_Potencia_SP != _Potencia_SP_A) {
        ReiniciarAceleracaoArr();
        _RampaAtivada = true;
      }
    }
  }

  float PotenciaAcrescentar() {
    if (((RPM + _Margem) > _RPM_SP) && ((RPM - _Margem) < _RPM_SP)) {
      return 0.0;
    }

    float PercentualDistancia = (_RPM_SP / (RPM == 0 ? 1 : RPM));
    float AcrescimoPotencia = (PercentualDistancia * _Potencia_SP) - _Potencia_SP;
    if (AcrescimoPotencia > 100.0) {
      AcrescimoPotencia = 100.0;
    }
    else if (AcrescimoPotencia < -100.0) {
      AcrescimoPotencia = -100.0;
    }
    float AcrescimoPotenciaCorrigido = (AcrescimoPotencia / _FatorDivisor);
    return AcrescimoPotenciaCorrigido;
  }

  int newPotenciaAcrescentar() {
    if (((RPM + _Margem) > _RPM_SP) && ((RPM - _Margem) < _RPM_SP)) {
      return 0;
    }

    int Acrescentar = ((RPM + _Margem) > _RPM_SP) ? -_Precisao : _Precisao;
    return Acrescentar;
  }


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

  void RampaTask() {
    unsigned long previousMillisRampa = millis();
    double MultiplicadorPotencia = RampaMax / 100;
    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;
      }
      unsigned long currentMillis = millis();
      if (currentMillis - previousMillisRampa >= _Rampa) {
        previousMillisRampa = currentMillis;

        if (_RampaAtivada) {
          bool Limite = false;

          if ((_Sentido == Parado || _Sentido != _SentidoA) && PotenciaAtual > ZeroRampa) { // M1 estiver parando ou invertendo, e pwm for maior zero rampa
            PotenciaAtual -= _Precisao; // Decrementar para atingir ponto zero rampa
            if (PotenciaAtual < ZeroRampa)
              PotenciaAtual = ZeroRampa;
          }
          else if (_Sentido != _SentidoA && PotenciaAtual <= ZeroRampa) { // ZEROU AO MUDAR SENTIDO DE GIRO ANTES DE AUMENTAR
            _SentidoA = _Sentido;
          }
          else if (_Sentido != Parado && (PotenciaAtual + 1) > (_Potencia_SP * MultiplicadorPotencia) && (PotenciaAtual - 1) < (_Potencia_SP * MultiplicadorPotencia)) { // Estabiliza a potência do motor
            if (_Sentido == Parado) {
              PotenciaAtual = ZeroRampa;
              _Potencia_SP = ZeroRampa; //(RampaMax / RampaMin);
            }
            else {
              PotenciaAtual = (PotenciaAtual > RampaMax) ? RampaMax : (_Potencia_SP * MultiplicadorPotencia);
            }
            _SentidoA = _Sentido;
            Limite = true;
          }
          else if (_Sentido != Parado && PotenciaAtual < (_Potencia_SP * MultiplicadorPotencia)) { // abaixo do set point
            PotenciaAtual += _Precisao; // Incrementar para atingir o set point
            if (PotenciaAtual > RampaMax)
              PotenciaAtual = RampaMax;
          }
          else if (_Sentido != Parado && PotenciaAtual > (_Potencia_SP * MultiplicadorPotencia)) { // acima do set point
            PotenciaAtual -= _Precisao; // Decrementar para atingir o set point
            if (PotenciaAtual < ZeroRampa)
              PotenciaAtual = ZeroRampa;
          }
          else { // Estabiliza a potência do motor
            if (_Sentido == Parado) {
              PotenciaAtual = ZeroRampa;
              _Potencia_SP = ZeroRampa; //(RampaMax / RampaMin);
            }
            else {
              PotenciaAtual = (PotenciaAtual > RampaMax) ? RampaMax : (_Potencia_SP * MultiplicadorPotencia);
            }
            _SentidoA = _Sentido;
            Limite = true;
          }

          Atualizar();

          if (Limite) {
            _RampaAtivada = false;
          }
        }
        else {
          CorrigePotenciaMotor();
        }

        
      }
      
      // Aguarda metade do tempo de rampa para realizar a próxima verificação
      int wait = (_Rampa / 2) + 1;
      vTaskDelay(wait);
    }
  }

};

Motor M1("ET", 25);
Motor M2("EF", 35);
Motor M3("DT", 45);
Motor M4("DF", 55);

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

  IniciarMemoria();
  delay(10);
  //LimparMemoria();
  /*GravarMemoria(M1_US_Addr, (int)Horario);
  GravarMemoria(M2_US_Addr, (int)Antihorario);
  GravarMemoria(M3_US_Addr, (int)Antihorario);
  GravarMemoria(M4_US_Addr, (int)Horario);*/

  TaskHandle_t SerialTaskHandle = NULL;
  xTaskCreatePinnedToCore(SerialTask, "SerialTask", 4000, NULL, 20, &SerialTaskHandle, 1);
}

void loop() {
  float x = 1509 / 300;
}


void SerialTask(void *pvParameters) {
  unsigned long previousMillisSerial = millis();
  const int TempoLeitura = 100;
  while (1)
  {
    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();

          RampaMin = _RampaMin;
          RampaMax = _RampaMax;
          //PPR = _PPR;

          vTaskDelay(pdMS_TO_TICKS(100));
          Conectado = Conectar;
          EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
        }
        else {
          int canal = ((String)Protocolo[5]).toInt();
          bool _foc = (String)Protocolo[7] == "1";
          bool motor_ativado = (String)Protocolo[9] == "1";
          int _fd = ((String)Protocolo[11] + (String)Protocolo[12]).toInt();
          int pwm = ((String)Protocolo[14] + (String)Protocolo[15]).toInt();
          int vel = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();
          int dir = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
          int brk = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
          int stp = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();

          Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
          if (motor_ativado) {
            if (Conectar) {
              _motor->_canal = canal;
              _motor->_pinoPWM = pwm;
              _motor->_pinoVEL = vel;
              _motor->_pinoDIR = dir;
              _motor->_pinoBRK = brk;
              _motor->_pinoSTP = stp;
              _motor->_FOC = _foc;
              _motor->_FatorDivisor = _fd;
                
              _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 = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;

        _motor->_Potencia_SP = ((String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5]).toInt();
        _motor->_Sentido = (Sentido)((String)Protocolo[7]).toInt();
        _motor->_Rampa = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
        _motor->_Precisao = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15]).toInt();
        _motor->_RPM_SP = ((String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19]).toInt();
        _motor->_FOC = (String)Protocolo[21] == "1";
        _motor->ReiniciarAceleracaoArr();
        _motor->_RampaAtivada = true;
      }
      else if (_funcao == Tst) {
        String ID = ((String)Protocolo[0] +(String)Protocolo[1]);
          
        Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;

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

    vTaskDelay(pdMS_TO_TICKS(TempoLeitura));
  }
}