// Dispositivo: Atuador
// Versão Firmware: 2
// Ultima atualização: 11/03/2024

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "HX711.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 AtuacaoNivelAlto = true;

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(sFLX, _ID, MontarDadosProtocoloBicoPulverizador(StatusBico)));
        }


        vTaskDelay(1);
      }
    }

};

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;
    volatile float periodo = 0;
    volatile float fluxo = 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;
        periodo = 0;
      }

      xTaskCreatePinnedToCore(&SensorFluxo::SFTaskWrapper, "SFTask", 5000, 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();
      int pulsos = 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 = tempoAtual - tempoAnterior;
          periodo = periodoUs / 1000.0; // Converte de us para ms (2300 us para 2,3 ms)

          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 == 0) {
            periodo = 0;
            fluxo = 0;
          }
          pulsos = 0;

          EnviarDadosSerial(MontarProtocoloSensor(sFLX, _ID, MontarDadosProtocoloFluxo(fluxo, periodo)));

          firstMilis = millis();
        }

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

    void CalcularFluxo(float periodo) {
      // Supondo que calibracao seja o número de pulsos por litro (450 pulsos/litro neste exemplo)
      fluxo = (1.0 / _calibragem) * (60 * 1000 / periodo); // Calcula o fluxo em L/min
    }

};

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;

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

      // 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 Massa = balanca->get_units(5);
      EnviarDadosSerial(MontarProtocoloSensor(sMAS, _ID, MontarDadosProtocoloMassa(Massa)));
    }

};

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

    // Definições
    String _ID;
    S_Code _componente = sPRS;
    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 (_pino > -1) {
        pinMode(_pino, INPUT);
      }

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

      // 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:
    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;
        }
        
        long intMillis = (millis() - firstMilis);

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

          firstMilis = millis();
        }

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

    void AferirPressao() {
      int valorSensor = analogRead(_pino);
      // Converte o valor lido (0 a 1023) para tensão (0.5 a 4.5V)
      float tensao = map(valorSensor, 0, 1023, 500, 4500) / 1000.0;
      // Converte a tensão para pressão em MPa
      // A tensão de 0,5V corresponde a 0 MPa e 4,5V corresponde a 1,2 MPa
      float pressao = (tensao - 0.5) * (1.2 / (4.5 - 0.5));
      EnviarDadosSerial(MontarProtocoloSensor(sPRS, _ID, MontarDadosProtocoloPressao(pressao, valorSensor)));
    }

};

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

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

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

  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] + (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;

        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 {
        BicoPulverizador* _bico = BicoPorID(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 {
          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) {
                double calibragem = ((String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31] + (String)Protocolo[32] + (String)Protocolo[33] + (String)Protocolo[34]).toDouble();
                _sensor->_calibragem = (float)calibragem;
                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) {
                if (Conectar) {
                  _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] + "  ");

      BicoPulverizador* _bico = BicoPorID(ID);

      _bico->_BicoAtuado = (String)Protocolo[4] == (AtuacaoNivelAlto ? "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();
      }
        
    }
  }
}