// Dispositivo: Atuador
// Versão Firmware: 3
// Ultima atualização: 28/03/2024

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Utils.h"
#include "HX711.h"
#include <PID_v1.h>
#include <SimpleKalmanFilter.h>
#define D_Code Atu

#define _pinoLED RGB_BUILTIN

long _baudRate = 115200;

bool Conectado = false;

int _TaxaAmostragem = 500;
int BicosAtivos = 10;
bool AtuacaoNivelAlto = true;

static double _PressaoAtual = 0.0;

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

    // Definições
    String _ID;
    int _posicao;
    int _pinoSolenoide;

    bool Iniciado = false;
    bool Testando = false;

    // Consumo
    bool StatusBico;

    // Entrada de dados
    bool _BicoAtuado = !AtuacaoNivelAlto;

    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(_ID + " ja inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }
      
      if (_pinoSolenoide > -1) {
        pinMode(_pinoSolenoide, OUTPUT);
        _BicoAtuado = !AtuacaoNivelAlto;
        digitalWrite(_pinoSolenoide, _BicoAtuado);
        StatusBico = digitalRead(_pinoSolenoide) == (AtuacaoNivelAlto ? 1 : 0);
      }

      xTaskCreatePinnedToCore(&BicoPulverizador::SensorTaskWrapper, "SensorTask", 5000, this, 25 - _posicao, &SensorTaskHandle, 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(SensorTaskHandle);
      
      // Redefinir as configurações para os valores iniciais
      pinMode(_pinoSolenoide, INPUT);

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

    void AlterarStatusBico() {
      digitalWrite(_pinoSolenoide, _BicoAtuado);
      StatusBico = digitalRead(_pinoSolenoide) == (AtuacaoNivelAlto ? 1 : 0);
    }


  private:

    TaskHandle_t SensorTaskHandle = NULL;
    static void SensorTaskWrapper(void *pvParameters) {
      BicoPulverizador *bico = static_cast<BicoPulverizador*>(pvParameters);
      bico->SensorTask();
    }

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


        unsigned long currentMillis = millis();
        if ((currentMillis - previousMillisLeitura) >= _TaxaAmostragem) {
          previousMillisLeitura = currentMillis;

          EnviarDadosSerial(MontarProtocoloSensor(sBIC, _ID, MontarDadosProtocoloBicoPulverizador(StatusBico)));
        }


        vTaskDelay(1);
      }
    }

};

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

    // Definições
    String _ID;
    int _pinoEN;
    int _pinoCS;
    int _pinoINA;
    int _pinoINB;
    int _pinoPWM;

    double _Kp = 3.0;
    double _Ki = 2.2;
    double _Kd = 0.5;
    PID* _PID = nullptr;

    int ZeroRampa = 0;
    int RampaMax = 1023;

    bool Iniciado = false;
    bool Testando = false;
    int _canal = 0;

    // Consumo
    bool Ligado;
    double PotenciaAtual = 0;

    // Entrada de dados
    double _PressaoSP = 0;
    bool _BombaAtuada = false;

    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(_ID + " ja inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }
      
      if (_pinoEN > -1) {
        _BombaAtuada = false;
        pinMode(_pinoEN, OUTPUT);
        digitalWrite(_pinoEN, _BombaAtuada);
        Ligado = digitalRead(_pinoEN) == 1;
      }
      if (_pinoCS > -1) {
        pinMode(_pinoEN, INPUT);
      }
      if (_pinoINA > -1) {
        pinMode(_pinoINA, OUTPUT);
        digitalWrite(_pinoINA, LOW);
      }
      if (_pinoINB > -1) {
        pinMode(_pinoINB, OUTPUT);
        digitalWrite(_pinoINB, HIGH);
      }
      if (_pinoPWM > -1) {
        ledcSetup(_canal, 1000, 10);
        ledcAttachPin(_pinoPWM, _canal);
        PotenciaAtual = 0;
        _PressaoSP = 0;
        ledcWrite(_canal, PotenciaAtual);
      }

      _PID = new PID(&_PressaoAtual, &PotenciaAtual, &_PressaoSP, _Kp, _Ki, _Kd, DIRECT);
      _PID->SetOutputLimits(ZeroRampa, RampaMax);
      _PID->SetMode(AUTOMATIC);

      xTaskCreatePinnedToCore(&BombaPressurizadora::SensorTaskWrapper, "SensorTask", 15000, this, 25, &SensorTaskHandle, 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(SensorTaskHandle);
      
      // Desanexar o canal PWM
      if (_pinoPWM > -1) {
        ledcDetachPin(_pinoPWM);
      }

      // Redefinir as configurações para os valores iniciais
      pinMode(_pinoEN, INPUT);
      pinMode(_pinoCS, INPUT);
      pinMode(_pinoINA, INPUT);
      pinMode(_pinoINB, INPUT);
      pinMode(_pinoPWM, 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 AlterarStatusBomba() {
      digitalWrite(_pinoEN, _BombaAtuada);
      Ligado = digitalRead(_pinoEN) == 1;
    }


  private:

    TaskHandle_t SensorTaskHandle = NULL;
    static void SensorTaskWrapper(void *pvParameters) {
      BombaPressurizadora *bomba = static_cast<BombaPressurizadora*>(pvParameters);
      bomba->SensorTask();
    }

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

        if (_BombaAtuada/* && _PressaoAtual < _PressaoSP*/) {
          _PID->Compute();
          //PotenciaAtual = RampaMax * 0.5;
        }
        else {
          PotenciaAtual = ZeroRampa;
        }
        ledcWrite(_canal, PotenciaAtual);

        unsigned long currentMillis = millis();
        if ((currentMillis - previousMillisLeitura) >= _TaxaAmostragem) {
          previousMillisLeitura = currentMillis;

          AtualizarStatusSensor();
        }


        vTaskDelay(1);
      }
    }

    void AtualizarStatusSensor() {
      Ligado = digitalRead(_pinoEN) == 1;
      
      float PercentualPotencia = (PotenciaAtual / RampaMax) * 100;

      EnviarDadosSerial(MontarProtocoloSensor(sBMB, _ID, MontarDadosProtocoloBombaPressurizadora(Ligado, PercentualPotencia)));
    }

};

class SensorFluxo {
  public:
    SensorFluxo(String desc) {
      _ID = desc;
    }

    // Definições
    String _ID;
    S_Code _componente = sFLX;
    int _pino;
    int _delayAmostragem;
    int _prioridade;
    float _calibragem;

    bool Iniciado = false;

    // Consumo
    volatile int ultimaLeitura = 0;
    volatile unsigned int tempoAnterior = 0;

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

      if (_pino > -1) {
        pinMode(_pino, INPUT);
        ultimaLeitura = digitalRead(_pino);
        tempoAnterior = 0;
      }

      xTaskCreatePinnedToCore(&SensorFluxo::SFTaskWrapper, "SFTask", 10000, this, _prioridade, &SFTaskHandle, 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(SFTaskHandle);

      // Redefinir as configurações para os valores iniciais
      pinMode(_pino, INPUT);

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:
    TaskHandle_t SFTaskHandle = NULL;
    static void SFTaskWrapper(void *pvParameters) {
      SensorFluxo *sensor = static_cast<SensorFluxo*>(pvParameters);
      sensor->SFTask();
    }

    void SFTask() {
      unsigned long firstMilis = millis();
      volatile int pulsos = 0;
      volatile int pulsosA = 0;
      volatile float periodo = 0;
      volatile float periodoTotal = 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;
        }

        bool Leitura = digitalRead(_pino) == 1;
        if (Leitura != ultimaLeitura) {
          ultimaLeitura = Leitura;
          pulsos++;

          // Calcular o período em microssegundos
          unsigned long tempoAtual = micros();
          unsigned long periodoUs = pulsos == 1 ? 0 : (tempoAtual - tempoAnterior);
          periodo = periodoUs / 1000.0; // Converte de us para ms (2300 us para 2,3 ms)
          periodoTotal += periodo;

          //CalcularFluxo(periodo);

          tempoAnterior = tempoAtual;
        }
        
        long intMillis = (millis() - firstMilis);

        int Tempo = _TaxaAmostragem + _delayAmostragem;
        // A cada _TaxaAmostragem, enviar os dados para o software
        if (intMillis >= Tempo) {
          if (pulsos == pulsosA) {
            periodoTotal = 0;
            pulsos = 0;
          }

          EnviarDadosSerial(MontarProtocoloSensor(sFLX, _ID, MontarDadosProtocoloFluxo(pulsos, periodoTotal)));

          pulsosA = pulsos;
          firstMilis = millis();
        }

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

};

class SensorMassa {
  public:
    SensorMassa(String desc) {
      _ID = desc;
    }

    // Definições
    String _ID;
    S_Code _componente = sMAS;
    int _pinoDT;
    int _pinoSCK;
    int _delayAmostragem;
    int _prioridade;

    HX711* balanca = nullptr;
    SimpleKalmanFilter* filtro = nullptr;

    bool Iniciado = false;

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

      if (_pinoDT > -1 && _pinoSCK > -1) {
        balanca = new HX711();
        filtro = new SimpleKalmanFilter(2, 2, 0.01);
        balanca->begin(_pinoDT, _pinoSCK);
        balanca->set_scale();
        //balanca->tare();
      }

      xTaskCreatePinnedToCore(&SensorMassa::SMTaskWrapper, "SMTask", 5000, this, _prioridade, &SMTaskHandle, 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;
      }

      delete balanca;
      delete filtro;

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

      // Redefinir as configurações para os valores iniciais
      pinMode(_pinoDT, INPUT);
      pinMode(_pinoSCK, INPUT);

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:
    TaskHandle_t SMTaskHandle = NULL;
    static void SMTaskWrapper(void *pvParameters) {
      SensorMassa *sensor = static_cast<SensorMassa*>(pvParameters);
      sensor->SMTask();
    }

    void SMTask() {
      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 + _delayAmostragem)) {
          AferirMassa();

          firstMilis = millis();
        }

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

    void AferirMassa() {
      float leitura = balanca->get_units(5);
      double leituraNormalizada = filtro->updateEstimate(leitura);
      EnviarDadosSerial(MontarProtocoloSensor(sMAS, _ID, MontarDadosProtocoloMassa(leituraNormalizada)));
    }

};

class SensorPressao {
  public:
    SensorPressao(String desc) {
      _ID = desc;
    }

    // Definições
    String _ID;
    S_Code _componente = sPRS;
    int _pino;
    int _delayAmostragem;
    int _prioridade;

    double _Vmin = 0.0;
    double _Vmax = 4.5;
    double _Pmax = 174.045;

    SimpleKalmanFilter* filtro = nullptr;

    bool Iniciado = false;

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

      if (_pino > -1) {
        pinMode(_pino, INPUT);
        filtro = new SimpleKalmanFilter(2, 2, 0.01);
      }

      xTaskCreatePinnedToCore(&SensorPressao::SPTaskWrapper, "SPTask", 5000, this, _prioridade, &SPTaskHandle, 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;
      }

      delete filtro;

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

      // Redefinir as configurações para os valores iniciais
      pinMode(_pino, INPUT);

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:
    double leituraNormalizada = 0.0;

    TaskHandle_t SPTaskHandle = NULL;
    static void SPTaskWrapper(void *pvParameters) {
      SensorPressao *sensor = static_cast<SensorPressao*>(pvParameters);
      sensor->SPTask();
    }

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

        AferirPressao();
        
        long intMillis = (millis() - firstMilis);

        // A cada _TaxaAmostragem, enviar os dados para o software
        if (intMillis >= (_TaxaAmostragem + _delayAmostragem)) {
          EnviarDadosSerial(MontarProtocoloSensor(sPRS, _ID, MontarDadosProtocoloPressao(_PressaoAtual, leituraNormalizada)));

          firstMilis = millis();
        }

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

    void AferirPressao() {
      int leitura = analogRead(_pino);
      leituraNormalizada = filtro->updateEstimate(leitura);

      double tensao = fmap(leituraNormalizada, 0.0, 4095.0, 0.0, 3300.0) / 1000.0;
      _PressaoAtual = fmap(tensao, _Vmin, _Vmax, 0.0, _Pmax);
    }

};

BicoPulverizador B_01("B01  ");
BicoPulverizador B_02("B02  ");
BicoPulverizador B_03("B03  ");
BicoPulverizador B_04("B04  ");
BicoPulverizador B_05("B05  ");
BicoPulverizador B_06("B06  ");
BicoPulverizador B_07("B07  ");
BicoPulverizador B_08("B08  ");
BicoPulverizador B_09("B09  ");
BicoPulverizador B_10("B10  ");
BombaPressurizadora BOMBA("BOMBA");
SensorFluxo SenFlx("FLXLN");
SensorMassa SenMas("MASRS");
SensorPressao SenPrs("PRSLN");

BicoPulverizador* BicoPorID(String ID) {
  BicoPulverizador* _bico = 
    ID == "B01  " ? &B_01 : 
    ID == "B02  " ? &B_02 : 
    ID == "B03  " ? &B_03 : 
    ID == "B04  " ? &B_04 : 
    ID == "B05  " ? &B_05 :
    ID == "B06  " ? &B_06 : 
    ID == "B07  " ? &B_07 : 
    ID == "B08  " ? &B_08 : 
    ID == "B09  " ? &B_09 : 
    ID == "B10  " ? &B_10 : 
    nullptr;
  return _bico;
}

BombaPressurizadora* BombaPorID(String ID) {
  BombaPressurizadora* _bomba = 
    ID == "BOMBA" ? &BOMBA :
    nullptr;
  return _bomba;
}

SensorFluxo* SFPorID(String ID) {
  SensorFluxo* _sensor = 
    ID == "FLXLN" ? &SenFlx :
    nullptr;
  return _sensor;
}

SensorMassa* SMPorID(String ID) {
  SensorMassa* _sensor = 
    ID == "MASRS" ? &SenMas :
    nullptr;
  return _sensor;
}

SensorPressao* SPPorID(String ID) {
  SensorPressao* _sensor = 
    ID == "PRSLN" ? &SenPrs :
    nullptr;
  return _sensor;
}

static bool ExisteBicoAtuado() {
  if (B_01.Iniciado && ((AtuacaoNivelAlto && B_01.StatusBico) || (!AtuacaoNivelAlto && !B_01.StatusBico))) {
    return true;
  }
  else if (B_02.Iniciado && ((AtuacaoNivelAlto && B_02.StatusBico) || (!AtuacaoNivelAlto && !B_02.StatusBico))) {
    return true;
  }
  else if (B_03.Iniciado && ((AtuacaoNivelAlto && B_03.StatusBico) || (!AtuacaoNivelAlto && !B_03.StatusBico))) {
    return true;
  }
  else if (B_04.Iniciado && ((AtuacaoNivelAlto && B_04.StatusBico) || (!AtuacaoNivelAlto && !B_04.StatusBico))) {
    return true;
  }

  return false;
}

void ZerarPinos() {
  //const int gpios[] = {2, 4, 5, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
  const int gpios[] = {4, 5, 6, 7, 15, 16, 17, 18, 8, 3, 46, 9, 10, 11, 12, 13, 14, 19, 20, 21, 47, 48, 45, 0, 35, 36, 37, 38, 39, 40, 41, 42, 2, 1};

  for (int i = 0; i < sizeof(gpios) / sizeof(gpios[0]); i++) {
    pinMode(gpios[i], OUTPUT);
    digitalWrite(gpios[i], !AtuacaoNivelAlto);
  }
}

void setup() {
  Serial.begin(_baudRate);

  ZerarPinos();
}

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] + (String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4]);
      bool Conectar = (String)Protocolo[6] == "1";
      if (ID == "MD   ") {
        _TaxaAmostragem = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12]).toInt();
        int pinoReleGeral = ((String)Protocolo[14] + (String)Protocolo[15]).toInt();
        bool _AtuaNivelAlto = (String)Protocolo[17] == "1";

        AtuacaoNivelAlto = _AtuaNivelAlto;

        vTaskDelay(pdMS_TO_TICKS(100));
        Conectado = Conectar;
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD ", Conectado ? "1" : "0"));
      }
      else {
        BicoPulverizador* _bico = BicoPorID(ID);
        BombaPressurizadora* _bomba = BombaPorID(ID);
        
        if (_bico != nullptr) {
          bool bico_ativado = (String)Protocolo[8] == "1";
          int posicao = ((String)Protocolo[10] + (String)Protocolo[11]).toInt();
          int solenoide = ((String)Protocolo[13] + (String)Protocolo[14]).toInt();
          if (bico_ativado) {
            if (Conectar) {
              _bico->_posicao = posicao;
              _bico->_pinoSolenoide = solenoide;
                  
              _bico->Inicializar();
            }
            else {
              _bico->Desligar();
            }
            vTaskDelay(pdMS_TO_TICKS(500));
          }
        }
        else if (_bomba != nullptr) {
          bool bomba_ativada = (String)Protocolo[8] == "1";
          double pressaoSP = ((String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12]).toDouble();
          int pinoEN = ((String)Protocolo[14] + (String)Protocolo[15]).toInt();
          int pinoCS = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();
          int pinoINA = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
          int pinoINB = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
          int pinoPWM = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();
          if (bomba_ativada) {
            if (Conectar) {
              _bomba->_PressaoSP = pressaoSP;
              _bomba->_pinoEN = pinoEN;
              _bomba->_pinoCS = pinoCS;
              _bomba->_pinoINA = pinoINA;
              _bomba->_pinoINB = pinoINB;
              _bomba->_pinoPWM = pinoPWM;
                  
              _bomba->Inicializar();
            }
            else {
              _bomba->Desligar();
            }
            vTaskDelay(pdMS_TO_TICKS(500));
          }
        }
        else {
          S_Code componente = (S_Code)((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10]).toInt();
          bool sensor_ativado = (String)Protocolo[12] == "1";
          int delayAmostragem = ((String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();
          int prioridade = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
          int pino = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
          int pino2 = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();

          switch (componente) {
            case sFLX: {
              SensorFluxo* _sensor = SFPorID(ID);
              if (sensor_ativado) {
                if (Conectar) {
                  _sensor->_pino = pino;
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }
            case sMAS: {
              SensorMassa* _sensor = SMPorID(ID);
              if (sensor_ativado) {
                if (Conectar) {
                  _sensor->_pinoDT = pino;
                  _sensor->_pinoSCK = pino2;
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }
            case sPRS: {
              SensorPressao* _sensor = SPPorID(ID);
              if (sensor_ativado) {
                double Vmin = ((String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31] + (String)Protocolo[32] + (String)Protocolo[33] + (String)Protocolo[34]).toDouble();
                double Vmax = ((String)Protocolo[36] + (String)Protocolo[37] + (String)Protocolo[38] + (String)Protocolo[39] + (String)Protocolo[40] + (String)Protocolo[41]).toDouble();
                double Pmax = ((String)Protocolo[43] + (String)Protocolo[44] + (String)Protocolo[45] + (String)Protocolo[46] + (String)Protocolo[47] + (String)Protocolo[48]).toDouble();
                if (Conectar) {
                  _sensor->_Vmin = Vmin;
                  _sensor->_Vmax = Vmax;
                  _sensor->_Pmax = Pmax;

                  _sensor->_pino = pino;
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }

          }
        }
        
      }
    }
    else if (_funcao == Cmd) {
      //canal;potencia;sentido;rampa;precisao;rpm
      //0;000;0;000;000;000
      String ID = ((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4]);

      BicoPulverizador* _bico = BicoPorID(ID);
      BombaPressurizadora* _bomba = BombaPorID(ID);

      if (_bico != nullptr) {
        _bico->_BicoAtuado = (String)Protocolo[6] == (AtuacaoNivelAlto ? "1" : "0");
        _bico->AlterarStatusBico();
      }
      else if (_bomba != nullptr) {
        _bomba->_BombaAtuada = (String)Protocolo[6] == "1";
        _bomba->_PressaoSP = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10]).toDouble();
        _bomba->AlterarStatusBomba();
      }
    }
    else if (_funcao == Tst) {
      String ID = ((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2]);
          
      BicoPulverizador* _bico = BicoPorID(ID);

      bool EmTeste = _bico->Testando;
      if (!EmTeste) {
        //_motor->Testar();
      }
        
    }
  }
}