// Dispositivo: Movimentação e Direcional Unificados
// Versão Firmware: 4
// Ultima atualização: 18/06/2024
// Atualização: Sem uso da porta RS485 nos drivers

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ModbusService.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 Mvd
#define VERSION 4
String Mod_ID = "";

#define _pinoAddr0 36
#define _pinoAddr1 39

void AtualizarEnderecoModulo() {
  bool A0 = digitalRead(_pinoAddr0) == 1;
  bool A1 = digitalRead(_pinoAddr1) == 1;

  if (!A0 && !A1) {
    Mod_ID = "ET";
  }
  else if (A0 && !A1) {
    Mod_ID = "EF";
  }
  else if (!A0 && A1) {
    Mod_ID = "DT";
  }
  else if (A0 && A1) {
    Mod_ID = "DF";
  }
}

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


class MotorBLDC {
  public:
    String _ID = "Mov";
    int _canal = 4;

    volatile bool Iniciado = false;
    bool Testando = false;

    PinoModel _pinoEN;
    PinoModel _pinoFR;
    PinoModel _pinoSV;
    PinoModel _pinoBK;

    PinoModel _pinoTMP;
    SimpleKalmanFilter tempKalman = SimpleKalmanFilter(2, 2, 0.01);

    // Consumo
    double Temperatura;
    
    // Entrada de dados
    double _RPM_SP;
    bool _Ligado = false;
    bool _Freio = false;
    Sentido _SentidoSP = Parado;
    int _rpmMax;

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

      if (_pinoEN.Definido()) {
        _pinoEN.Conectar(true);
      }
      if (_pinoFR.Definido()) {
        _pinoFR.Conectar();
      }
      if (_pinoBK.Definido()) {
        _pinoBK.Conectar(true);
      }
      if (_pinoSV.Definido()) {
        AtualizarPWM();
      }

      if (_pinoTMP.Definido()) {
        _pinoTMP.Conectar();
      }

      AtualizarDadosControle(true);

      Iniciado = true;

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

    void Desligar() {
      if (!Iniciado) {
        //EnviarDadosSerial(Mod_ID, _ID + " nao esta inicializado");
        EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

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

      // Redefinir as configurações para os valores iniciais
      _pinoEN.Desconectar();
      _pinoFR.Desconectar();
      _pinoSV.Desconectar();
      _pinoBK.Desconectar();
      _pinoTMP.Desconectar();

      //EnviarDadosSerial(Mod_ID, _ID + " Desligado");

      Iniciado = false;

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

      AtualizarDadosControle(true);
    }

    void AtualizarDadosControle(bool reset = false) {
      if (!Iniciado) {
        return;
      }

      if (reset) {
        _Ligado = false;
        _Freio = false;
        _SentidoSP = Parado;
        _RPM_SP = 0;
      }

      int val = map(_RPM_SP, 0, _rpmMax, 0, 1023);
      ledcWrite(_canal, val);
      _pinoFR.set(_SentidoSP == Horario ? false : true);
      _pinoBK.set(!_Freio);
      _pinoEN.set(!_Ligado);
    }

    void RequisitarDados() {
      AferirTemperatura();

      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sBLD, _ID, MontarDadosProtocoloBLD(RPM, Temperatura, Tensao, CorrenteMax, Ligado, _Sentido, Freio, CodAlarme, TemperaturaDriver)));
    }

  private:
    void AferirTemperatura() {
      int leitura = _pinoTMP.get();
      float leituraNormalizada = tempKalman.updateEstimate(leitura);
      Temperatura = leituraNormalizada;
    }
};

class MotorPasso {
  public:
    String _ID = "Dir";
    int _canal = 0;

    volatile bool Iniciado = false;
    bool Testando = false;

    // Definições
    PinoModel _pinoENA;
    PinoModel _pinoDIR;
    PinoModel _pinoPUL;
    PinoModel _pinoEncoder;

    SimpleKalmanFilter encoderKalman = SimpleKalmanFilter(1, 2, 0.01);
    
    // 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;
    double _frequencia = 4000;

    int FrequenciaMaxima = 10000;
    int dutyCycle = 512;


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

      Iniciado = true;

      if (_pinoPUL.Definido()) {
        AtualizarPWM();
      }
      _pinoENA.Conectar(true);
      _pinoDIR.Conectar(false);
      _pinoEncoder.Conectar();

      _Angulo_SP = 0;
      _MotorLigado = false;
      VerificarAnguloSPparado(true);

      xTaskCreatePinnedToCore(&MotorPasso::DirTaskWrapper, "DirTask", 5000, this, 18, &DirTaskHandle, tskNO_AFFINITY);

      //EnviarDadosSerial(Mod_ID, _ID + " Iniciado");

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

    void Desligar() {
      if (!Iniciado) {
        //EnviarDadosSerial(Mod_ID, _ID + " nao esta inicializado");
        EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

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

      // Redefinir as configurações para os valores iniciais
      _pinoENA.Desconectar();
      _pinoDIR.Desconectar();
      _pinoPUL.Desconectar();
      _pinoEncoder.Desconectar();
      
      
      //EnviarDadosSerial(Mod_ID, _ID + " Desligado");

      Iniciado = false;

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

    void RequisitarDados() {
      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoderAbsoluto(_Sentido, _Angulo)));
    }

  
  private:

    double _Margem = 0.5;
    double _AgMin = 95;
    double _AgMax = 855;
    int RefMin = -90;
    int RefMax = 90;

    double _anguloAnterior = 0;
    double _anguloAtual = 0;
    double _taxaMudancaAngulo = 0;

    TaskHandle_t DirTaskHandle = NULL;
    static void DirTaskWrapper(void *pvParameters) {
      MotorPasso* motor = static_cast<MotorPasso*>(pvParameters);
      motor->DirTask();
    }

    void DirTask() {
      unsigned long previousMillisRampa = millis();
    
      while (true) {
        if (!Conectado) {
          vTaskDelay(1000);
          continue;
        }

        AferirPosicao();
        DefinirSentidoDeGiro();
        VerificarAnguloSP();
        Atualizar();

        /*if (millis() - previousMillisRampa >= _TaxaAmostragem) {
          previousMillisRampa = millis();

          RequisitarDados();
        }*/

        vTaskDelay(1);
      }
    }

    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 > RefMax) angulo_com_offset -= 180;
        while (angulo_com_offset < RefMin) angulo_com_offset += 180;

        _Angulo = angulo_com_offset;
      }
    }

    void DefinirSentidoDeGiro() {
      _anguloAtual = _Angulo;

      // Calcular a taxa de mudança do ângulo
      _taxaMudancaAngulo = (_anguloAtual - _anguloAnterior) / (1.0 / 1000.0); // Supondo que a leitura é feita a cada 1 ms

      if (_MotorLigado) {
        if (_taxaMudancaAngulo > 0) {
          _Sentido = Horario;
        } else if (_taxaMudancaAngulo < 0) {
          _Sentido = Antihorario;
        } else {
          _Sentido = Parado;
        }
      } else {
        _Sentido = Parado;
      }

      _anguloAnterior = _anguloAtual;
    }

    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 >= _TaxaAmostragem) {
          previousMillisCorrecao = millis();

          VerificarAnguloSPparado(_Estabilizar);
        }
      }
    }

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

    void Atualizar() {
      _pinoENA.set(!_MotorLigado);
      _pinoDIR.set(_Sentido_SP != Antihorario);
    }


};

MotorBLDC MotorMOV;
MotorPasso MotorDIR;

void AtualizarPWM() {
  if (MotorDIR.Iniciado && MotorDIR._pinoPUL.Definido()) {
    ledcSetup(MotorDIR._canal, MotorDIR._frequencia, 10);
    ledcAttachPin(MotorDIR._pinoPUL.num, MotorDIR._canal);
    ledcWrite(MotorDIR._canal, MotorDIR.dutyCycle);
  }
  if (MotorMOV.Iniciado && MotorMOV._pinoSV.Definido()) {
    ledcSetup(MotorMOV._canal, 1000, 10);
    ledcAttachPin(MotorMOV._pinoSV.num, MotorMOV._canal);
    ledcWrite(MotorMOV._canal, MotorMOV._RPM_SP);
  }
}

void SensoriamentoTask(void *pvParameters);
TaskHandle_t SensoriamentoTaskHandle = NULL;

void setup() {
  pinMode(_pinoAddr0, INPUT);
  pinMode(_pinoAddr1, INPUT);

  AtualizarEnderecoModulo();

  Serial.begin(_baudRate, SERIAL_8N1, 1, 3);

  analogReadResolution(10);

  delay(10);

  if (xTaskCreatePinnedToCore(SensoriamentoTask, "SensoriamentoTask", 8000, NULL, 5, &SensoriamentoTaskHandle, tskNO_AFFINITY) != pdPASS) {
    Serial.println("Erro ao criar tarefa SensoriamentoTask");
  }
}

void SensoriamentoTask(void *pvParameters) {
  while (1)
  {
    Conectado = MotorMOV.Iniciado || MotorDIR.Iniciado;

    if (!RequisitarDados && Conectado) {
      
      if (MotorMOV.Iniciado) {
        MotorMOV.RequisitarDados();
      }
      if (MotorDIR.Iniciado) {
        MotorDIR.RequisitarDados();
      }

      EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sMVD, Mod_ID, MontarDadosProtocoloMVD(
        MotorMOV.Iniciado, 
        MotorMOV.Temperatura, 
        MotorDIR.Iniciado, 
        MotorDIR._Sentido, 
        MotorDIR._Angulo
      )));
    }
    vTaskDelay(_TaxaAmostragem);
  }
}

void loop() {
  std::vector<ProtocoloSerial> protocolos = LerBufferSerialFila(Mod_ID);

  for (int i = 0; i < protocolos.size(); i++) {
    ProcessarProtocolo(protocolos[i]);
  }

  delay(1);
}

void ProcessarProtocolo(ProtocoloSerial Mensagem) {
  switch (Mensagem.funcao) {
    case Chk:
      ProcessarChk(Mensagem);
      break;
    case Cfg:
      ProcessarCfg(Mensagem);
      break;
    case Cmd:
      ProcessarCmd(Mensagem);
      break;
    case Req:
      ProcessarReq(Mensagem);
      break;
  }

  if (Mensagem.idMensagem > 0) {
    EnviarDadosSerial(Mod_ID, MontarProtocoloMensagemRecebida(Mensagem.idMensagem));
  }
}

void ProcessarChk(ProtocoloSerial Mensagem) {
  Conectado = MotorMOV.Iniciado || MotorDIR.Iniciado;

  EnviarDadosSerial(Mod_ID, MontarProtocoloVerificacao(D_Code, VERSION, Conectado));
}

void ProcessarCfg(ProtocoloSerial Mensagem) {
  std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);

  String ID = Partes[0];
  bool Conectar = Partes[1] == "1";

  if (ID == Mod_ID) {
    _TaxaAmostragem = Partes[2].toInt();
    RequisitarDados = Partes[3].toInt();
    TiposBarramentos txB = (TiposBarramentos)Partes[4].toInt();
    int txP = Partes[5].toInt();
    TiposBarramentos rxB = (TiposBarramentos)Partes[6].toInt();
    int rxP = Partes[7].toInt();
  }
  else if (ID == MotorMOV._ID) {
    if (Conectar) {
      MotorMOV._rpmMax = Partes[5].toInt();
      MotorMOV._pinoTMP = PinoModel(Partes[7].toInt(), (TiposBarramentos)Partes[6].toInt(), _INPUT, ANALOGICO);
      MotorMOV._pinoEN = PinoModel(Partes[9].toInt(), (TiposBarramentos)Partes[8].toInt(), _OUTPUT);
      MotorMOV._pinoFR = PinoModel(Partes[11].toInt(), (TiposBarramentos)Partes[10].toInt(), _OUTPUT);
      MotorMOV._pinoSV = PinoModel(Partes[13].toInt(), (TiposBarramentos)Partes[12].toInt(), _PWM);
      MotorMOV._pinoBK = PinoModel(Partes[15].toInt(), (TiposBarramentos)Partes[14].toInt(), _OUTPUT);
            
      MotorMOV.Inicializar();
    }
    else {
      MotorMOV.Desligar();
    }
  }
  else if (ID == MotorDIR._ID) {
    if (Conectar) {
      MotorDIR._Estabilizar = Partes[2] == "1";
      MotorDIR._Angulo_Offset = Partes[3].toInt();
      MotorDIR._pinoPUL = PinoModel(Partes[5].toInt(), (TiposBarramentos)Partes[4].toInt(), _PWM);
      MotorDIR._pinoDIR = PinoModel(Partes[7].toInt(), (TiposBarramentos)Partes[6].toInt(), _OUTPUT);
      MotorDIR._pinoENA = PinoModel(Partes[9].toInt(), (TiposBarramentos)Partes[8].toInt(), _OUTPUT);
      MotorDIR._pinoEncoder = PinoModel(Partes[11].toInt(), (TiposBarramentos)Partes[10].toInt(), _INPUT, ANALOGICO);

      MotorDIR.Inicializar();
    }
    else {
      MotorDIR.Desligar();
    }
  }
}

void ProcessarCmd(ProtocoloSerial Mensagem) {
  std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);

  String ID = Partes[0];

  if (ID == MotorMOV._ID && MotorMOV.Iniciado) {
    MotorMOV._SentidoSP = (Sentido)Partes[1].toInt();
    MotorMOV._Ligado = MotorMOV._SentidoSP != Parado;
    MotorMOV._RPM_SP = Partes[2].toInt();
    MotorMOV._Freio = Partes[3] == "1";

    MotorMOV.AtualizarDadosControle();
  }
  else if (ID == MotorDIR._ID && MotorDIR.Iniciado) {
    MotorDIR._Sentido_SP = (Sentido)Partes[1].toInt();
    MotorDIR._Angulo_SP = Partes[2].toInt();
    MotorDIR._frequencia = (Partes[3].toInt() / 100.0) * MotorDIR.FrequenciaMaxima;

    AtualizarPWM();

    MotorDIR._MotorLigado = MotorDIR._Sentido_SP != Parado;
  }
}

void ProcessarReq(ProtocoloSerial Mensagem) {
  //std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);
  //Serial.println("Processando req...");
  if (MotorMOV.Iniciado) {
    MotorMOV.RequisitarDados();
  }
  if (MotorDIR.Iniciado) {
    MotorDIR.RequisitarDados();
  }

  if (MotorMOV.Iniciado || MotorDIR.Iniciado) {
    //Serial.println("Enviando dados...");
    EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sMVD, Mod_ID, MontarDadosProtocoloMVD(
      MotorMOV.Iniciado, 
      MotorMOV.Temperatura, 
      MotorDIR.Iniciado, 
      MotorDIR._Sentido, 
      MotorDIR._Angulo
    )));
  }
}