// Dispositivo: Atuador
// Versão Firmware: 1
// Ultima atualização: 23/01/2024

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#define D_Code Atu

#define _pinoLED RGB_BUILTIN
int _pinoReleGeral = -1;

long _baudRate = 115200;

bool Conectado = false;

int _TaxaAmostragem = 500;
int BicosAtivos = 10;
bool BicoLigado = false;

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

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

  bool Iniciado = false;
  bool Testando = false;

  // Consumo
  double NivelFluxo;
  bool StatusBico;

  // Entrada de dados
  bool _BicoAtuado = !BicoLigado;

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

    if (_pinoSolenoide > -1) {
      pinMode(_pinoSolenoide, OUTPUT);
      _BicoAtuado = !BicoLigado;
      digitalWrite(_pinoSolenoide, _BicoAtuado);
      StatusBico = digitalRead(_pinoSolenoide) == (BicoLigado ? 1 : 0);
    }
    if (_pinoSensor > -1) {
      pinMode(_pinoSensor, INPUT);
      NivelFluxo = 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);
    pinMode(_pinoSensor, INPUT);

    EnviarDadosSerial(_ID + " Desligado");

    Iniciado = false;

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

  void AlterarStatusBico() {
    digitalWrite(_pinoSolenoide, _BicoAtuado);
    StatusBico = digitalRead(_pinoSolenoide) == (BicoLigado ? 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(sFLX, _ID, MontarDadosProtocoloBicoPulverizador(StatusBico, NivelFluxo)));
      }


      vTaskDelay(1);
    }
  }

};

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

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 : 
    &B_10;
  return _bico;
}

void ZerarPinos() {
  const int gpios[] = {2, 4, 5, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};

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

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

  ZerarPinos();

  if (_pinoReleGeral > -1) {
    pinMode(_pinoReleGeral, OUTPUT);
    digitalWrite(_pinoReleGeral, LOW);
  }
}

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

        if (pinoReleGeral > -1) {
          pinMode(_pinoReleGeral, INPUT);
          _pinoReleGeral = pinoReleGeral;
          pinMode(_pinoReleGeral, OUTPUT);
          digitalWrite(_pinoReleGeral, Conectar);
        }

        vTaskDelay(pdMS_TO_TICKS(100));
        Conectado = Conectar;
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD ", Conectado ? "1" : "0"));
      }
      else {
        bool bico_ativado = (String)Protocolo[6] == "1";
        int posicao = ((String)Protocolo[8] + (String)Protocolo[9]).toInt();
        int solenoide = ((String)Protocolo[11] + (String)Protocolo[12]).toInt();
        int sensor = ((String)Protocolo[14] + (String)Protocolo[15]).toInt();
        
        BicoPulverizador* _bico = BicoPorID(ID);

        if (bico_ativado) {
          if (Conectar) {
            _bico->_posicao = posicao;
            _bico->_pinoSolenoide = solenoide;
            _bico->_pinoSensor = sensor;
                
            _bico->Inicializar();
          }
          else {
            _bico->Desligar();
          }
          vTaskDelay(pdMS_TO_TICKS(500));
        }
      }
    }
    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]);

      BicoPulverizador* _bico = BicoPorID(ID);

      _bico->_BicoAtuado = (String)Protocolo[4] == (BicoLigado ? "1" : "0");
      _bico->AlterarStatusBico();
    }
    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();
      }
        
    }
  }
}