// Dispositivo: Sensoriamento
// Versão Firmware: 1
// Ultima atualização: 28/11/2023

#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
#define D_Code Sen

#define _pinoLED RGB_BUILTIN

long _baudRate = 115200;

bool Conectado = false;

int _TaxaAmostragem;
int FrequenciaMaxima = 10000;


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

  // Definições
  String _ID;
  S_Code _componente;
  int _prioridade;
  int _delayAmostragem;
  bool _I2C;
  int _pinoLeitura1;
  int _pinoLeitura2;
  int _pinoLeitura3;
  int _pinoLeitura4;
  
  bool Iniciado = false;
  bool Testando = false;

  // Consumo
  
  // Entrada de dados


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

    if (_pinoLeitura1 > -1) {
      pinMode(_pinoLeitura1, INPUT);
    }
    if (_pinoLeitura2 > -1) {
      pinMode(_pinoLeitura2, INPUT);
    }
    if (_pinoLeitura3 > -1) {
      pinMode(_pinoLeitura3, INPUT);
    }
    if (_pinoLeitura4 > -1) {
      pinMode(_pinoLeitura4, INPUT);
    }
    

    xTaskCreatePinnedToCore(&Sensor::LeituraTaskWrapper, "LeituraTask", 5000, this, _prioridade, &LeituraTaskHandle, tskNO_AFFINITY);


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

    // Redefinir as configurações para os valores iniciais
    if (_pinoLeitura1 > -1) {
      pinMode(_pinoLeitura1, INPUT);
    }
    if (_pinoLeitura2 > -1) {
      pinMode(_pinoLeitura2, INPUT);
    }
    if (_pinoLeitura3 > -1) {
      pinMode(_pinoLeitura3, INPUT);
    }
    if (_pinoLeitura4 > -1) {
      pinMode(_pinoLeitura4, INPUT);
    }
    
    
    EnviarDadosSerial(_ID + " Desligado");

    Iniciado = false;

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

  void Testar() {
    Testando = true;

    Testando = false;
  }

  

private:

  TaskHandle_t LeituraTaskHandle = NULL;
  static void LeituraTaskWrapper(void *pvParameters) {
    Sensor *sensor = static_cast<Sensor*>(pvParameters);
    sensor->LeituraTask();
  }

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

      // LEITURA AQUI

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

        EnviarDadosSerial(MontarProtocoloSensor(_componente, _ID, MontarDadosProtocoloSensorGenerico("")));
      }

      vTaskDelay(1);

    }
  }


};

Sensor sVBAT("VBAT");
Sensor sABAT("ABAT");


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

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]);
      bool Conectar = (String)Protocolo[3] == "1";
      if (ID == "MD") {
        _TaxaAmostragem = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7] + (String)Protocolo[8] + (String)Protocolo[9]).toInt();

        vTaskDelay(pdMS_TO_TICKS(100));
        Conectado = Conectar;

        EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
      }
      else {
        Sensor* _sensor = ID == "VBAT" ? &sVBAT : ID == "ABAT" ? &sABAT : nullptr;

        if (_sensor == nullptr) {
          return;
        }

        S_Code componente = (S_Code)((String)Protocolo[5]).toInt();
        int prioridade = ((String)Protocolo[7] + (String)Protocolo[8]).toInt();
        int delayAmostragem = ((String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13] + (String)Protocolo[14]).toInt();
        bool aferir = ((String)Protocolo[16]) == "1";
        bool i2c = ((String)Protocolo[18]) == "1";
        int lei_1 = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
        int lei_2 = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
        int lei_3 = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();
        int lei_4 = ((String)Protocolo[29] + (String)Protocolo[30]).toInt();

        if (aferir) {
          if (Conectar) {
            _sensor->_prioridade = prioridade;
            _sensor->_delayAmostragem = delayAmostragem;
            _sensor->_I2C = i2c;
            _sensor->_pinoLeitura1 = lei_1;
            _sensor->_pinoLeitura2 = lei_2;
            _sensor->_pinoLeitura3 = lei_3;
            _sensor->_pinoLeitura4 = lei_4;
            _sensor->Inicializar();
          }
          else {
            _sensor->Desligar();
          }
          vTaskDelay(pdMS_TO_TICKS(500));
        }
      }
    }
    else if (_funcao == Cmd) {
      
    }
    else if (_funcao == Tst) {
      
    }

  }
}