// Dispositivo: Movimentação e Direcional Unificados
// Versão Firmware: 6
// Ultima atualização: 01/07/2024
// Atualização: Removido dados do motor de passo

#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 6
String Mod_ID = "";

#define _pinoAddr0 36
#define _pinoAddr1 39

int _pinModbusRX = 16;
int _pinModbusTX = 17;
int _pinModbusMD = 4;

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

    byte _endereco;
    int _numPolos;
    int _rampa;
    int _rpmMax;

    // Consumo
    double RPM;
    double RPM_SP;
    double Temperatura;
    double TemperaturaDriver;
    double Tensao;
    double CorrenteMax;
    bool Ligado;
    bool Freio;
    Sentido _Sentido = Parado;
    int CodAlarme;
    
    // Entrada de dados
    double _RPM_SP;
    bool _Ligado = false;
    bool _Freio = false;
    Sentido _SentidoSP = Parado;

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

      Iniciado = ConfigurarDriver();

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

      if (Iniciado) {
        AtualizarDadosControle(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);
      

      /*ComandoEnviado = true;
      EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0, false);
      EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP, false);
      EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0, false);
      EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0, false);
      vTaskDelay(100);
      ComandoEnviado = false;*/
    }

    void RequisitarDados() {
      AferirDadosDriver();
      AferirTemperatura();

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

  private:
    ModbusGetResponse EnviarComandoModbus(byte endereco, byte funcao, byte registro, int valor, bool aguardarResposta = true) {
      const int maxTentativas = 2;
      int tentativas = 0;
      bool sucesso = false;
      ModbusGetResponse resposta;

      while (tentativas < maxTentativas && !sucesso) {
        uint8_t response[256];
        uint8_t length = 0;

        sendModbusCommand(endereco, funcao, registro, valor, response, length, aguardarResposta);

        if (aguardarResposta) {
          if (funcao == 0x03) {
            if (length >= 5) {
              resposta._success = true;
              resposta._address = response[0];
              resposta._command_code = response[1];
              resposta._num_registers = response[2];

              if (resposta._num_registers == 2) {
                uint16_t value = (response[3] << 8) | response[4];
                resposta._value = static_cast<double>(value);
                sucesso = true;
              } else if (resposta._num_registers == 4 && length >= 7) {
                uint16_t highBytes = (response[3] << 8) | response[4];
                uint16_t lowBytes = (response[5] << 8) | response[6];
                
                // Combinar os bytes de alta e baixa ordem em um valor de 32 bits
                uint32_t combined = (static_cast<uint32_t>(highBytes) << 16) | lowBytes;

                // Reinterpretar o valor combinado como um float
                float floatValue;
                memcpy(&floatValue, &combined, sizeof(floatValue));

                resposta._value = static_cast<double>(floatValue);
                
                sucesso = true;
              } else {
                resposta._success = false;
              }
            } else {
              resposta._success = false;
            }
          } else if (funcao == 0x06) {
            if (length >= 8) {
              resposta._success = true;
              resposta._address = response[0];
              resposta._command_code = response[1];
              resposta._num_registers = 0;
              resposta._value = (response[4] << 8) | response[5];
              sucesso = true;
            } else {
              Serial.println("Erro ao enviar comando");
              resposta._success = false;
            }
          }

          tentativas++;
        } else {
          sucesso = true;
          resposta._success = sucesso;
        }
      }

      return resposta;
    }

    bool ConfigurarDriver() {
      ModbusGetResponse a = EnviarComandoModbus(_endereco, 0x06, 0x00B6, 1);
      if (a._success) {
        ModbusGetResponse b = EnviarComandoModbus(_endereco, 0x06, 0x0076, 0);
        if (b._success) {
          ModbusGetResponse c = EnviarComandoModbus(_endereco, 0x06, 0x0086, _numPolos);
          if (c._success) {
            ModbusGetResponse d = EnviarComandoModbus(_endereco, 0x06, 0x008A, _rampa);
            if (d._success) {
              ModbusGetResponse e = EnviarComandoModbus(_endereco, 0x06, 0x0092, _rpmMax);
              if (e._success) {
                ModbusGetResponse f = EnviarComandoModbus(_endereco, 0x06, 0x00B6, 0);
                return f._success;
              }
            }
          }
        }
      }
      return false;
    }

    void AferirDadosDriver() {
      AguardaEnvioComando();
      RPM_SP = EnviarComandoModbus(_endereco, 0x03, 0x0056, 1)._value;
      AguardaEnvioComando();
      RPM = EnviarComandoModbus(_endereco, 0x03, 0x005F, 1)._value;
      AguardaEnvioComando();
      Ligado = EnviarComandoModbus(_endereco, 0x03, 0x0066, 1)._value == 1;
      AguardaEnvioComando();
      Freio = EnviarComandoModbus(_endereco, 0x03, 0x006A, 1)._value == 1;
      AguardaEnvioComando();
      _Sentido = Ligado ? EnviarComandoModbus(_endereco, 0x03, 0x006D, 1)._value == 1 ? Horario : Antihorario : Parado;
      AguardaEnvioComando();
      CodAlarme = EnviarComandoModbus(_endereco, 0x03, 0x0076, 1)._value;
      AguardaEnvioComando();
      Tensao = EnviarComandoModbus(_endereco, 0x03, 0x00C8, 2)._value;
      AguardaEnvioComando();
      CorrenteMax = EnviarComandoModbus(_endereco, 0x03, 0x0096, 2)._value;
      AguardaEnvioComando();
      TemperaturaDriver = EnviarComandoModbus(_endereco, 0x03, 0x00D2, 2)._value;
    }

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

    void AguardaEnvioComando() {
      while (ComandoEnviado) {
        vTaskDelay(50);
      }
    }

};

MotorBLDC MotorMOV;

void AtualizarPWM() {
  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);
  //Serial.begin(_baudRate);

  analogReadResolution(10);

  delay(10);

  ConfigurarModbus(true, _pinModbusRX, _pinModbusTX, _pinModbusMD);

  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;

    if (!RequisitarDados && Conectado) {
      ProcessarReq();
    }
    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();
      break;
  }

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

void ProcessarChk(ProtocoloSerial Mensagem) {
  Conectado = MotorMOV.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();
    TiposBarramentos MtxB = (TiposBarramentos)Partes[8].toInt();
    int MtxP = Partes[9].toInt();
    TiposBarramentos MrxB = (TiposBarramentos)Partes[10].toInt();
    int MrxP = Partes[11].toInt();
    TiposBarramentos MmdB = (TiposBarramentos)Partes[12].toInt();
    int MmdP = Partes[13].toInt();
    _pinModbusMD = MmdP;
    _pinModbusRX = MrxP;
    _pinModbusTX = MtxP;

    //ConfigurarModbus(Conectar, _pinModbusRX, _pinModbusTX, _pinModbusMD);
  }
  else if (ID == MotorMOV._ID) {
    if (Conectar) {
      MotorMOV._endereco = stringToByte(Partes[2]);
      MotorMOV._numPolos = Partes[3].toInt();
      MotorMOV._rampa = Partes[4].toInt();
      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();
    }
  }
}

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

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

  if (MotorMOV.Iniciado) {
    //Serial.println("Enviando dados...");
    EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sBLD, Mod_ID, MontarDadosProtocoloBLD(
      MotorMOV.Iniciado, 
      MotorMOV.RPM, 
      MotorMOV.Temperatura, 
      MotorMOV.Tensao, 
      MotorMOV.CorrenteMax, 
      MotorMOV.Ligado, 
      MotorMOV._Sentido, 
      MotorMOV.Freio, 
      MotorMOV.CodAlarme, 
      MotorMOV.TemperaturaDriver
    )));
  }
}