// Dispositivo: Direcional
// Versão Firmware: 9
// Ultima atualização: 14/05/2024
// Atualização: Inclusão do potenciometro para aferição do angulo

#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 <SimpleKalmanFilter.h>
#define D_Code Dir
#define VERSION 9

#define _pinoLED RGB_BUILTIN

long _baudRate = 115200;

bool Conectado = false;

int _TaxaAmostragem;
int FrequenciaMaxima = 10000;
double _Margem = 0.5;
double _AgMin = 380;
double _AgMax = 3430;
int RefMin = -90;
int RefMax = 90;

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 _pinoEncoder;

    SimpleKalmanFilter* encoderKalman;
    
    bool Iniciado = false;
    bool Testando = false;
    bool Referenciando = false;

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

    bool Referenciado = 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);
      _pinoEncoder.Conectar();

      encoderKalman = new SimpleKalmanFilter(1, 2, 0.01);
      
      xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 20 - _canal, &RampaTaskHandle, tskNO_AFFINITY);

      ResetRef(_Referenciar);

      EnviarDadosSerial(_ID + " Iniciado");

      Iniciado = true;

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

    void ResetRef(bool _Ref) {
      _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);

      delete encoderKalman;

      // Redefinir as configurações para os valores iniciais
      _pinoENA.Desconectar();
      _pinoDIR.Desconectar();
      _pinoPUL.Desconectar();
      _pinoEncoder.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();

        // 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, MontarDadosProtocoloEncoderAbsoluto(_Sentido, _Angulo, 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 != Antihorario);
    }

    void AferirPosicao() {
      if (_pinoEncoder.Definido()) {
        int leitura = _pinoEncoder.get();
        float leituraNormalizada = encoderKalman->updateEstimate(leitura);
        double _angulo = fmap(leituraNormalizada, _AgMin, _AgMax, RefMin, RefMax);

        double angulo_com_offset = _angulo - _Angulo_Offset;

        // Normaliza o ângulo para o intervalo de -90 a 90 graus
        while (angulo_com_offset > 90) angulo_com_offset -= 180;
        while (angulo_com_offset < -90) angulo_com_offset += 180;

        _Angulo = angulo_com_offset;

        DefinirSentidoDeGiro();
      }
    }


    double _anguloAnterior = 0;
    int leituras = 0;
    void DefinirSentidoDeGiro() {
      double _anguloAtual = _Angulo;

      if (leituras > 100) {
        if (_MotorLigado) {
          if (_anguloAtual > _anguloAnterior) {
            _Sentido = Horario;
          }
          else if (_anguloAtual < _anguloAnterior) {
            _Sentido = Antihorario;
          }
        }
        else {
          _Sentido = Parado;
        }

        _anguloAnterior = _anguloAtual;

        leituras = 0;
      }
      leituras++;
      
    }

    


    unsigned long previousMillisCorrecao = millis();
    void VerificarAnguloSP() {
      bool ModoSP = (_Angulo_SP > -1);
      if (_MotorLigado) {
        double AnguloComparar = ModoSP ? _Angulo_SP : RefMax;
        if (_Sentido_SP == Horario && _Angulo >= AnguloComparar) {
          _MotorLigado = false;
        }
        else if (_Sentido_SP == Antihorario && _Angulo <= (AnguloComparar * -1)) {
          _MotorLigado = false;
        }
      }
      else if (ModoSP) {
        if (millis() - previousMillisCorrecao >= 1500) {
          previousMillisCorrecao = millis();

          bool NoRangeZero = (_Angulo >= -_Margem && _Angulo <= _Margem);
          bool NoRangeSP = (_Angulo >= (_Angulo_SP - _Margem) && _Angulo <= (_Angulo_SP + _Margem));
          if (_Angulo_SP == 0 && !NoRangeZero && _Estabilizar) {
            _Sentido_SP = _Angulo > 0 ? Antihorario : Horario;
            _MotorLigado = true;
          }
          else if (!NoRangeSP) {
            _MotorLigado = true;
          }
        }
      }
    }

    void ReferenciarMotor() {
      _Angulo_SP = 0;
      Referenciado = false;

      if (Referenciando && (_Angulo >= -_Margem && _Angulo <= _Margem)) {
        Referenciando = false;
        Referenciado = true;
        _MotorLigado = false;
      }
      else {
        _Sentido_SP = (_Angulo > 0) ? Antihorario : Horario;
      }
    }



};

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

      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 encB = (TiposBarramentos)Partes[13].toInt();
      int encoder = Partes[14].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->_pinoEncoder = PinoModel(encoder, encB, _INPUT, ANALOGICO);
          _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));

}