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

#define _pinoLED RGB_BUILTIN
int _pinoReleGeral = 8;

long _baudRate = 115200;

bool Conectado = false;

int _TaxaAmostragem = 500;
const int PPR = 22; // Pulsos por revolução

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

int M1_US_Addr = 25;
int M2_US_Addr = 35;
int M3_US_Addr = 45;
int M4_US_Addr = 55;

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

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

  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 unsigned int tempoAnterior = 0;
  volatile float periodo = 0;

  // Entrada de dados
  bool _MalhaFechada;
  int _PotMap;
  double _RPM_SP;
  Sentido _SentidoSP = Parado;
  Sentido _SentidoM = Parado;
  Sentido _Sentido = Parado;
  int _US_Addr;
  bool _Freio = 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);
      PotenciaAtual = ZeroRampa;
      _PotMap = PotenciaAtual;
      _RPM_SP = 0;
      ledcWrite(_canal, _PotMap);
    }
    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, 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);
    }
    if (_pinoHLC > -1) {
      pinMode(_pinoHLC, INPUT);
    }
    if (_pinoTMP > -1) {
      xTaskCreatePinnedToCore(&Motor::DHT22TaskWrapper, "DHT22Task", 10000, this, 17 - _canal, &DHT22TaskHandle, tskNO_AFFINITY);
    }


    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);
    }
    if (_pinoTMP > -1) {
      vTaskDelete(DHT22TaskHandle);
    }

    // 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);
    pinMode(_pinoTMP, 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() {
    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 LeituraHa = digitalRead(_pinoHLA);
      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)));
        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 = Horario;
    }
    else if (LeituraHa == LeituraAnteriorHb && LeituraHb == LeituraAnteriorHa) {
      _Sentido = Antihorario;
    }
    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;
      }

      float _rpmA = RPM;
      if (SentidoContrario) {
        RPM = RPM * -1;
      }
      /*else if (Aceleracao == Desacelerando && AceleracaoA == Estavel) {
        RPM = 0;
      }*/
      _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 previousMillisRampa = 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) {
      Revertendo = _SentidoSP != Parado && _SentidoSP != _SentidoM;

      if (Revertendo) {
        digitalWrite(_pinoDIR, HIGH);
        vTaskDelay(1500);
        digitalWrite(_pinoDIR, LOW);

        _SentidoM = _SentidoSP;
        GravarMemoria(_US_Addr, (int)_SentidoM);
        
        Revertendo = false;
      }

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


  // TEMPERATURA E UMIDADE
  TaskHandle_t DHT22TaskHandle = NULL;
  static void DHT22TaskWrapper(void *pvParameters) {
    Motor *motor = static_cast<Motor*>(pvParameters);
    motor->DHT22Task();
  }

  void DHT22Task() {
    unsigned long firstMilis = millis();
    while (1)
    {
      if (!Conectado) {
        vTaskDelay(pdMS_TO_TICKS(1000));
        continue;
      }

      uint8_t data[5] = {0, 0, 0, 0, 0};

  // Tempo de inicialização
  pinMode(_pinoTMP, OUTPUT);
  digitalWrite(_pinoTMP, LOW);
  delayMicroseconds(1000);  // Pelo menos 1 ms
  digitalWrite(_pinoTMP, HIGH);
  delayMicroseconds(48);  // Pelo menos 20-40 µs
  pinMode(_pinoTMP, INPUT);

  // Tempo de espera para resposta do sensor
  delayMicroseconds(96);  // Pelo menos 80 µs

  // Leitura de bits
  for (int i = 0; i < 40; i++) {
    // Medir a duração do pulso
    unsigned long pulse_duration = pulseIn(_pinoTMP, LOW);

    // Processar a duração do pulso para determinar 0 ou 1
    if (pulse_duration > 40) {
      data[i / 8] <<= 1;
      data[i / 8] |= 1;
    } else {
      data[i / 8] <<= 1;
    }
  }

  float h = 0;
  float t = 0;
  float hic = 0;

  Serial.println();
  Serial.print(data[0]);Serial.print(" ");
  Serial.print(data[1]);Serial.print(" ");
  Serial.print(data[2]);Serial.print(" ");
  Serial.print(data[3]);Serial.print(" ");
  Serial.print(data[4]);Serial.print(" ");
  Serial.println();Serial.println();

  // Verificar o checksum
  if (data[4] == ((data[0] + data[1] + data[2] + data[3]) & 0xFF)) {
    h = ((data[0] << 8) + data[1]) / 10.0;
    t = (((data[2] & 0x7F) << 8) + data[3]) / 10.0;
    if (data[2] & 0x80) {
      t = -t;
    }
  } else {
    Serial.println("Erro na leitura do sensor DHT22");
  }

        EnviarDadosSerial(MontarProtocoloSensor(sTMP, _ID, MontarDadosProtocoloDHT22(t, h, hic)));

      vTaskDelay(5000);
    }
  }



};

Motor M1("ET", M1_US_Addr);
Motor M2("EF", M2_US_Addr);
Motor M3("DT", M3_US_Addr);
Motor M4("DF", M4_US_Addr);

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);*/
  
  pinMode(_pinoReleGeral, OUTPUT);
  digitalWrite(_pinoReleGeral, LOW);

}

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

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

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

        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";
        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();
        int tmp = ((String)Protocolo[50] + (String)Protocolo[51]).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->_pinoDIR = dir;
            _motor->_pinoBRK = brk;
            _motor->_pinoSTP = stp;
            _motor->_pinoHLA = hallA;
            _motor->_pinoHLB = hallB;
            _motor->_pinoHLC = hallC;
            _motor->_pinoTMP = tmp;
            _motor->_MalhaFechada = _foc;
            _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 = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;

      _motor->PotenciaAtual = ((String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5]).toInt();
      //_motor->_Potencia_SP = ((String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5]).toInt();
      _motor->_SentidoSP = (Sentido)((String)Protocolo[7]).toInt();
      _motor->_RPM_SP = ((String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19]).toInt();
      _motor->_MalhaFechada = (String)Protocolo[21] == "1";
      _motor->ReiniciarAceleracaoArr();
    }
    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();
      }
        
    }
  }
}