// Dispositivo: Sensoriamento
// Versão Firmware: 7
// Ultima atualização: 03/04/2024

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Utils.h"

#include <SimpleKalmanFilter.h>
#include <INA226.h>
#include <MechaQMC5883.h>
#include <Wire.h>

#define D_Code Sen

#define _pinoLED RGB_BUILTIN

long _baudRate = 115200;

bool Conectado = false;

int _TaxaAmostragem;
int FrequenciaMaxima = 10000;

int _pinoSDA = 32;
int _pinoSCL = 33;


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

    // Definições
    String _ID;
    S_Code _componente = sTMP;
    int _pino;
    int _delayAmostragem;
    int _prioridade;
    float Voffset = 0.0;

    bool Iniciado = false;

    SimpleKalmanFilter* tempKalman;

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

      if (_pino > -1) {
        pinMode(_pino, INPUT);
      }

      xTaskCreatePinnedToCore(&SensorTemperatura::STTaskWrapper, "STTask", 5000, this, _prioridade, &STTaskHandle, tskNO_AFFINITY);

      EnviarDadosSerial(_ID + " Iniciado");

      Iniciado = true;

      tempKalman = new SimpleKalmanFilter(2, 2, 0.01);

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

      delete tempKalman;

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

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:

    // Variáveis
    double Vs = 3.3;     // TENSÃO DE SAIDA DO ESP32
    double R1 = 10000;   //RESISTOR UTILIZADO NO DIVISOR DE TENSÃO
    double Beta = 3950;  // VALOR DE BETA
    double To = 298.15;  // VALOR EM KELVIN REFERENTE A 25° CELSIUS
    double Ro = 10000;
    double adcMax = 4095.0;

    TaskHandle_t STTaskHandle = NULL;
    static void STTaskWrapper(void *pvParameters) {
      SensorTemperatura *sensor = static_cast<SensorTemperatura*>(pvParameters);
      sensor->STTask();
    }

    void STTask() {
      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)) {
          AferirTemperatura();

          firstMilis = millis();
        }

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

    void AferirTemperatura() {
      double Vout, Rt = 0;
      double T, Tc, Tf, adc = 0;
      adc = analogRead(_pino);  // VARIÁVEL QUE RECEBE A LEITURA DO NTC10K
      float adcValue = tempKalman->updateEstimate(adc);
      //CALCULOS PARA CONVERSÃO DA LEITURA RECEBIDA PELO ESP32 EM TEMPERATURA EM °C
      Vout = (adcValue * Vs / adcMax) + Voffset;
      Rt = R1 * Vout / (Vs - Vout);
      T = 1 / (1 / To + log(Rt / Ro) / Beta);
      Tc = T - 273.15;
      Tf = Tc * 9 / 5 + 32;

      EnviarDadosSerial(MontarProtocoloSensor(sTMP, _ID, MontarDadosProtocoloNTC10K(Tc)));
    }
};

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

    // Variáveis
    const float Vcc = 3.3; // Tensão de alimentação
    float Voffset = 0.85;
    int R1 = 47000;
    int R2 = 10000;

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

    SimpleKalmanFilter* kalmanTEN;

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

      kalmanTEN = new SimpleKalmanFilter(2, 2, 0.01);

      xTaskCreatePinnedToCore(&SensorTensao::SVTaskWrapper, "SVTask", 5000, this, _prioridade, &SVTaskHandle, 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(SVTaskHandle);

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

      delete kalmanTEN;

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:
    TaskHandle_t SVTaskHandle = NULL;
    static void SVTaskWrapper(void *pvParameters) {
      SensorTensao *sensor = static_cast<SensorTensao*>(pvParameters);
      sensor->SVTask();
    }

    void SVTask() {
      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)) {
          AferirTensao();

          firstMilis = millis();
        }

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

    void AferirTensao() {
      float Razao = (R1 + R2) / (float)R2;
      int adcV = analogRead(_pino);
      float adcValueV = adcV; //float adcValueV = kalmanTEN->updateEstimate(adcV);

      float voltageV = (adcValueV * (Vcc / 4095.0)) + (Voffset / Razao); // Convertendo o valor ADC para tensão
      float realVoltage = voltageV * Razao;

      EnviarDadosSerial(MontarProtocoloSensor(sTEN, _ID, MontarDadosProtocoloTensao(adcValueV, voltageV, realVoltage)));
    }

};

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

    // Variáveis
    INA226* INA = nullptr;

    // Definições
    String _ID;
    S_Code _componente = sCOR;
    byte _Endereco;
    int _delayAmostragem;
    int _prioridade;

    bool Iniciado = false;

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

      INA = new INA226(_Endereco);
      Iniciado = INA->begin();
      if (Iniciado) {
        INA->setMaxCurrentShunt(1, 0.002);
        xTaskCreatePinnedToCore(&SensorCorrente::SATaskWrapper, "SATask", 5000, this, _prioridade, &SATaskHandle, tskNO_AFFINITY);
        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(SATaskHandle);

      delete INA;

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:
    TaskHandle_t SATaskHandle = NULL;
    static void SATaskWrapper(void *pvParameters) {
      SensorCorrente *sensor = static_cast<SensorCorrente*>(pvParameters);
      sensor->SATask();
    }

    void SATask() {
      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)) {
          AferirCorrente();

          firstMilis = millis();
        }

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

    void AferirCorrente() {
      float busVoltage = INA->getBusVoltage();
      float shuntVoltage = INA->getShuntVoltage_mV();
      float current = INA->getCurrent();
      float power = INA->getPower();

      EnviarDadosSerial(MontarProtocoloSensor(sCOR, _ID, MontarDadosProtocoloINA226(busVoltage, shuntVoltage, current, power)));
    }

};

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

    // Variáveis
    #define SOUND_SPEED 0.034

    // Definições
    String _ID;
    S_Code _componente = sULT;
    int _pinoE;
    int _pinoT;
    int _pino;
    int _delayAmostragem;
    int _prioridade;

    bool Iniciado = false;

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

      if (_pinoE > -1) {
        pinMode(_pinoE, INPUT);
      }
      if (_pinoT > -1) {
        pinMode(_pinoT, OUTPUT);
      }

      xTaskCreatePinnedToCore(&SensorUltrassom::SUTaskWrapper, "SUTask", 5000, this, _prioridade, &SUTaskHandle, 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(SUTaskHandle);

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

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:
    TaskHandle_t SUTaskHandle = NULL;
    static void SUTaskWrapper(void *pvParameters) {
      SensorUltrassom *sensor = static_cast<SensorUltrassom*>(pvParameters);
      sensor->SUTask();
    }

    void SUTask() {
      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)) {
          AferirDistancia();

          firstMilis = millis();
        }

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

    void AferirDistancia() {
      digitalWrite(_pinoT, LOW);
      delayMicroseconds(2);
      digitalWrite(_pinoT, HIGH);
      delayMicroseconds(10);
      digitalWrite(_pinoT, LOW);
      long duration = pulseIn(_pinoE, HIGH);
      float distanceCm = duration * SOUND_SPEED / 2;

      EnviarDadosSerial(MontarProtocoloSensor(sULT, _ID, MontarDadosProtocoloUltrassom(distanceCm, duration)));
    }

};

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

    // Variáveis
    MechaQMC5883* qmc = nullptr;

    // Definições
    String _ID;
    S_Code _componente = sMAG;
    byte _Endereco;
    int _delayAmostragem;
    int _prioridade;

    bool Iniciado = false;

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

      qmc = new MechaQMC5883();
      qmc->init();
      
      xTaskCreatePinnedToCore(&SensorMagnetometro::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;
      }

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

      delete qmc;

      EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

  private:
    TaskHandle_t SMTaskHandle = NULL;
    static void SMTaskWrapper(void *pvParameters) {
      SensorMagnetometro *sensor = static_cast<SensorMagnetometro*>(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)) {
          AferirOrientacao();

          firstMilis = millis();
        }

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

    void AferirOrientacao() {
      int x, y, z;
      float orientacao;
      qmc->read(&x, &y, &z,&orientacao);

      EnviarDadosSerial(MontarProtocoloSensor(sMAG, _ID, MontarDadosProtocoloMagnetometro(x, y, z, orientacao)));
    }

};

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

    String _ID;
    int _pino;
    bool Iniciado = false;

    StatusLED _Status = Apagado;
    int _Qtd = 0;
    int _TempoMs = 0;
    int _PausaMs = 0;

    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(_ID + " ja inicializado");
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }
      
      if (_pino > -1) {
        pinMode(_pino, OUTPUT);
        digitalWrite(_pino, LOW);
      }

      xTaskCreatePinnedToCore(&Sinaleiro::LEDTaskWrapper, "LEDTask", 5000, this, 6, &LEDTaskHandle, 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(LEDTaskHandle);

      // 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 LEDTaskHandle = NULL;
    static void LEDTaskWrapper(void *pvParameters) {
      Sinaleiro *led = static_cast<Sinaleiro*>(pvParameters);
      led->LEDTask();
    }

    void LEDTask() {
      unsigned long lastMillis = millis();
      bool ledState = false;
      int piscadasAtual = 0;
      bool emPausa = false;

      while (1) {
        unsigned long currentMillis = millis();

        if (_Status != Piscando) {
          digitalWrite(_pino, _Status == Aceso ? HIGH : LOW);
        }
        else if (_Status == Piscando && piscadasAtual < _Qtd) {
          if (!emPausa && (currentMillis - lastMillis >= _TempoMs)) {
            ledState = !ledState;
            digitalWrite(_pino, ledState ? HIGH : LOW);
            lastMillis = currentMillis;
            if (!ledState) { // Se o LED acabou de ser desligado, incrementa o contador de piscadas
              piscadasAtual++;
            }
          } else if (emPausa && (currentMillis - lastMillis >= _PausaMs)) {
            emPausa = false;
            lastMillis = currentMillis;
          }
        } else if (piscadasAtual >= _Qtd) {
          // Se alcançou a quantidade desejada de piscadas, entra em pausa
          if (!emPausa) {
            ledState = false;
            digitalWrite(_pino, LOW); // Garante que o LED está desligado
            emPausa = true;
            piscadasAtual = 0; // Reseta o contador de piscadas
            lastMillis = currentMillis;
          }
        }

        vTaskDelay(1); // Pequena pausa para liberar o CPU
      }
    }

};



SensorTemperatura TM1("TMVET");
SensorTemperatura TM2("TMVEF");
SensorTemperatura TM3("TMVDT");
SensorTemperatura TM4("TMVDF");
SensorTemperatura TBAT("TBAT ");
SensorTensao VBAT("VBAT ");
SensorCorrente AM1("AMVET");
SensorCorrente AM2("AMVEF");
SensorCorrente AM3("AMVDT");
SensorCorrente AM4("AMVDF");
SensorCorrente AMP("AMP  ");
SensorCorrente AAT("AAT  ");
SensorCorrente APC("APC  ");
SensorUltrassom SUESQ("SUESQ");
SensorUltrassom SUDIR("SUDIR");
SensorMagnetometro SMAG("SMAG ");
Sinaleiro LED_AT("LEDAT");
Sinaleiro LED_MN("LEDMN");
Sinaleiro LED_OP("LEDOP");


SensorTemperatura* STPorID(String ID) {
  SensorTemperatura* _sensor = 
    ID == "TMVET" ? &TM1 : 
    ID == "TMVEF" ? &TM2 : 
    ID == "TMVDT" ? &TM3 : 
    ID == "TMVDF" ? &TM4 : 
    ID == "TBAT " ? &TBAT : 
    nullptr;
  return _sensor;
}

SensorTensao* SVPorID(String ID) {
  SensorTensao* _sensor = 
    ID == "VBAT " ? &VBAT : 
    nullptr;
  return _sensor;
}

SensorCorrente* SAPorID(String ID) {
  SensorCorrente* _sensor = 
    ID == "AMVET" ? &AM1 : 
    ID == "AMVEF" ? &AM2 : 
    ID == "AMVDT" ? &AM3 : 
    ID == "AMVDF" ? &AM4 : 
    ID == "AMP  " ? &AMP : 
    ID == "AAT  " ? &AAT : 
    ID == "APC  " ? &APC : 
    nullptr;
  return _sensor;
}

SensorUltrassom* SUPorID(String ID) {
  SensorUltrassom* _sensor = 
    ID == "SUESQ" ? &SUESQ : 
    ID == "SUDIR" ? &SUDIR : 
    nullptr;
  return _sensor;
}

SensorMagnetometro* SMPorID(String ID) {
  SensorMagnetometro* _sensor = 
    ID == "SMAG " ? &SMAG : 
    nullptr;
  return _sensor;
}

Sinaleiro* SinaleiroPorID(String ID) {
  Sinaleiro* _led = 
    ID == "LEDAT" ? &LED_AT : 
    ID == "LEDMN" ? &LED_MN : 
    ID == "LEDOP" ? &LED_OP : 
    nullptr;
  return _led;
}

void setup() {
  Serial.begin(_baudRate);
  Wire.begin(_pinoSDA, _pinoSCL);
}

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 pinoSDA = ((String)Protocolo[14] + (String)Protocolo[15]).toInt();
        int pinoSCL = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();

        if (pinoSDA > -1 && pinoSCL > -1) {
          _pinoSDA = pinoSDA;
          _pinoSCL = pinoSCL;
          Wire.begin(_pinoSDA, _pinoSCL);
        }

        vTaskDelay(pdMS_TO_TICKS(100));
        Conectado = Conectar;
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
      }
      else {
        Sinaleiro* _led = SinaleiroPorID(ID);

        if (_led != nullptr) {
          int pino = ((String)Protocolo[8] + (String)Protocolo[9]).toInt();
          if (pino > -1) {
            if (Conectar) {
              _led->_pino = pino;
              _led->Inicializar();
            }
            else {
              _led->Desligar();
            }
          }
          
        }
        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 sTMP: {
              SensorTemperatura* _sensor = STPorID(ID);
              if (sensor_ativado) {
                double _voffset = ((String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31] + (String)Protocolo[32] + (String)Protocolo[33] + (String)Protocolo[34]).toDouble();
                _sensor->Voffset = (float)_voffset;
                if (Conectar) {
                  _sensor->_pino = pino;
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }
            case sTEN: {
              SensorTensao* _sensor = SVPorID(ID);
              if (sensor_ativado) {
                double _voffset = ((String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31] + (String)Protocolo[32] + (String)Protocolo[33] + (String)Protocolo[34]).toDouble();
                double _r1 = ((String)Protocolo[36] + (String)Protocolo[37] + (String)Protocolo[38] + (String)Protocolo[39] + (String)Protocolo[40] + (String)Protocolo[41]).toDouble();
                double _r2 = ((String)Protocolo[43] + (String)Protocolo[44] + (String)Protocolo[45] + (String)Protocolo[46] + (String)Protocolo[47] + (String)Protocolo[48]).toDouble();
                _sensor->Voffset = (float)_voffset;
                _sensor->R1 = (int)_r1;
                _sensor->R2 = (int)_r2;
                if (Conectar) {
                  _sensor->_pino = pino;
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }
            case sCOR: {
              SensorCorrente* _sensor = SAPorID(ID);
              if (sensor_ativado) {
                String _endereco = ((String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31] + (String)Protocolo[32] + (String)Protocolo[33] + (String)Protocolo[34]);
                if (Conectar) {
                  _sensor->_Endereco = stringToByte(_endereco);
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }
            case sULT: {
              SensorUltrassom* _sensor = SUPorID(ID);
              if (sensor_ativado) {
                if (Conectar) {
                  _sensor->_pinoE = pino;
                  _sensor->_pinoT = pino2;
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }
            case sMAG: {
              SensorMagnetometro* _sensor = SMPorID(ID);
              if (sensor_ativado) {
                if (Conectar) {
                  _sensor->_prioridade = prioridade;
                  _sensor->_delayAmostragem = delayAmostragem;
                      
                  _sensor->Inicializar();
                }
                else {
                  _sensor->Desligar();
                }
              }
              break;
            }

          }
        }

      }

    } else if (_funcao == Cmd) {
      String ID = ((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4]);

      Sinaleiro* _led = SinaleiroPorID(ID);

      if (_led != nullptr) {
        _led->_Status = (StatusLED)((String)Protocolo[6]).toInt();
        _led->_Qtd = ((String)Protocolo[8]).toInt();
        _led->_TempoMs = ((String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13]).toInt();
        _led->_PausaMs = ((String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();
      }
    } else if (_funcao == Tst) {
    }
  }
}