// Dispositivo: Movimentação
// Versão Firmware: 19
// Ultima atualização: 03/06/2024
// Atualização: Drivers BLD-300R

#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 <HardwareSerial.h>
#include <SimpleKalmanFilter.h>

#define D_Code Mov
#define VERSION 19

#define _pinoLED RGB_BUILTIN

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


class Motor {
  public:
    // Construtor
    Motor(String desc) {
      _ID = desc;
    }

    // Definições
    String _ID;

    bool Iniciado = false;
    bool Testando = false;

    PinoModel _pinoTMP;
    SimpleKalmanFilter* tempKalman;

    int _canal;
    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;
    
    
    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(_ID + " ja inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      Iniciado = ConfigurarDriver();

      if (Iniciado) {
        if (_pinoTMP.Definido()) {
          _pinoTMP.Conectar();
          tempKalman = new SimpleKalmanFilter(2, 2, 0.01);
        }

        xTaskCreatePinnedToCore(&Motor::CheckTaskWrapper, "CheckTask", 5000, this, 21 - _canal + 4, &CheckTaskHandle, tskNO_AFFINITY);

        AtualizarDadosControle(true);

        EnviarDadosSerial(_ID + " Iniciado");
      }

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

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

      // Outras redefinições de variáveis de estado, se necessário
      delete tempKalman;

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

      AtualizarDadosControle(true);
    }

    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) {
            // GET
            if (funcao == 0x03) {
                if (length >= 5) { // Verifica se a resposta tem pelo menos 5 bytes (endereco, codigo_comando, numero_registros, 2 bytes de dados, 2 bytes de CRC)
                    resposta._success = true;
                    resposta._address = response[0];
                    resposta._command_code = response[1];
                    resposta._num_registers = response[2];

                    if (resposta._num_registers == 2) { // Caso de 2 bytes (sem ponto flutuante)
                        uint16_t value = (response[3] << 8) | response[4]; // Combina os dois bytes
                        resposta._value = static_cast<double>(value); // Converte para double
                        sucesso = true;
                    } else if (resposta._num_registers == 4 && length >= 7) { // Caso de 4 bytes (com ponto flutuante)
                        uint16_t highBytes = (response[3] << 8) | response[4]; // Combina os dois primeiros bytes
                        uint16_t lowBytes = (response[5] << 8) | response[6];  // Combina os dois últimos bytes

                        // Converte para double
                        resposta._value = highBytes + (lowBytes / 10000.0); // Supondo que os valores após a vírgula são representados em 4 dígitos decimais
                        sucesso = true;
                    } else {
                        Serial.println("Numero de registros inesperado ou resposta incompleta.");
                        resposta._success = false;
                    }
                } else {
                    Serial.println("Resposta incompleta.");
                    resposta._success = false;
                }
            }
            // SET
            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++;
            if (!sucesso) {
                Serial.print("Tentativa ");
                Serial.print(tentativas);
                Serial.println(" falhou, tentando novamente...");
            }
          }
          else {
            sucesso = true;
            resposta._success = sucesso;
          }

      }

      if (!sucesso) {
          Serial.println("Todas as tentativas falharam.");
      }

      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);
              return e._success;
            }
          }
        }
      }
      return false;
    }

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

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

      /*if (_SentidoSP != Parado && _SentidoSP != _Sentido) {
        int std = EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0)._value;
        _Sentido = !Ligado ? Parado : std == 0 ? Horario : Antihorario;
      }
      if (_RPM_SP != RPM_SP) {
        RPM_SP = EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP)._value;
      }
      if (_Freio != Freio) {
        Freio = EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0)._value == 1;
      }
      if (_Ligado != Ligado) {
        Ligado = EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0)._value == 1;
      }*/
      /*if (_SentidoSP != Parado && _SentidoSP != _Sentido) {
        EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0, false);
      }
      if (_RPM_SP != RPM_SP) {
        EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP, false);
      }
      if (_Freio != Freio) {
        EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0, false);
      }
      if (_Ligado != Ligado) {
        EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0, false);
      }*/

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

  private:

    TaskHandle_t CheckTaskHandle = NULL;
    static void CheckTaskWrapper(void *pvParameters) {
      Motor *motor = static_cast<Motor*>(pvParameters);
      motor->CheckTask();
    }

    void CheckTask() {
      unsigned long firstMilis = 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;
        }

        long intMillis = (millis() - firstMilis);

        // A cada _TaxaAmostragem, enviar os dados para o software
        if (intMillis >= _TaxaAmostragem) {

          AferirTemperatura();
          //AferirDadosDriver();

          EnviarDadosSerial(MontarProtocoloSensor(sBLD, _ID, MontarDadosProtocoloBLD(RPM, Temperatura, Tensao, CorrenteMax, Ligado, _Sentido, Freio, CodAlarme, TemperaturaDriver)));
          firstMilis = millis();
        }

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

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

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

};

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

  delay(10);

  InicializarModbus();
}

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 {
      int canal = Partes[2].toInt();
      String addr = Partes[3];
      int num_polos = Partes[4].toInt();
      int rampa = Partes[5].toInt();
      int rpm_max = Partes[6].toInt();
      TiposBarramentos tmpB = (TiposBarramentos)Partes[7].toInt();
      int tmp = Partes[8].toInt();
      
      Motor* _motor = MotorPorID(ID);
      if (Conectar) {
        _motor->_canal = canal;
        _motor->_endereco = stringToByte(addr);
        _motor->_numPolos = num_polos;
        _motor->_rampa = rampa;
        _motor->_rpmMax = rpm_max;
        _motor->_pinoTMP = PinoModel(tmp, tmpB, _INPUT, ANALOGICO);
              
        _motor->Inicializar();
      }
      else {
        _motor->Desligar();
      }
      vTaskDelay(pdMS_TO_TICKS(500));
    }
  }
  else if (_funcao == Cmd) {
    std::vector<String> Partes = SplitString(Protocolo, (char*)",");

    String ID = Partes[0];

    Motor* _motor = MotorPorID(ID);

    _motor->_SentidoSP = (Sentido)Partes[1].toInt();
    _motor->_Ligado = _motor->_SentidoSP != Parado;
    _motor->_RPM_SP = Partes[2].toInt();
    _motor->_Freio = Partes[3] == "1";

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

}