#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"

#ifndef SensorSonarModel
#define SensorSonarModel

class SensorSonar {
  public:

    SensorSonar(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    // Definições
    String Mod_ID;
    String _ID;
    S_Code _componente = sSNR;
    PinoModel _pinoE;
    PinoModel _pinoT;
    PinoModel _pinoServoX;
    PinoModel _pinoServoY;
    int _delayAmostragem;
    int _prioridade;

    Servo _ServoX;
    Servo _ServoY;

    int _AgPlus;
    int _AgPosition;

    bool Iniciado = false;

    long _distancia = 0;
    long _angulo = 0;
    int _posicao = 0;

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

      _pinoE.Conectar();
      _pinoT.Conectar();
      if (_pinoServoX.Definido()) {
        _ServoX.attach(_pinoServoX.num);
        _ServoX.write(0);
      }
      if (_pinoServoY.Definido()) {
        _ServoY.attach(_pinoServoY.num);
        _ServoY.write(0);
      }
      
      xTaskCreatePinnedToCore(&SensorSonar::SSTaskWrapper, "SSTask", 5000, this, _prioridade, &SSTaskHandle, tskNO_AFFINITY);

      //EnviarDadosSerial(_ID + " Iniciado");

      Iniciado = true;

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

    void Desligar() {
      if (!Iniciado) {
        //EnviarDadosSerial(_ID + " nao esta inicializado");
        //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

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

      if (_pinoServoX.Definido()) {
        _ServoX.detach();
      }
      if (_pinoServoY.Definido()) {
        _ServoY.detach();
      }

      // Redefinir as configurações para os valores iniciais
      _pinoE.Desconectar();
      _pinoT.Desconectar();

      //EnviarDadosSerial(_ID + " Desligado");

      Iniciado = false;

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

    void RequisitarDados() {
      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sSNR, _ID, MontarDadosProtocoloSonar(_distancia, _angulo, _posicao)));
    }

  private:
    TaskHandle_t SSTaskHandle = NULL;
    static void SSTaskWrapper(void *pvParameters) {
      SensorSonar *sensor = static_cast<SensorSonar*>(pvParameters);
      sensor->SSTask();
    }

    /*int idx = 0;
    int distancias[36];
    int angulos[36];*/

    void SSTask() {
      bool Horario = true;
      bool Subindo = true;
      int position = 0;
      unsigned long firstMilis = millis();
      while (1)
      {
        if (!Iniciado) {
          // Caso não esteja conectado, aguarda 1 segundo até a próxima verificação, liberando uso de CPU
          vTaskDelay(1000);
          continue;
        }

        if (Subindo) {
          for (position = 0; position < 2; position++) {
            position = 1; // REMOVER
            _ServoY.write(position * _AgPosition); // Move o servo para o ângulo desejado
            //vTaskDelay(100); // Espera meio segundo para o servo se mover
            if (Horario) {
              for(int angle = 0; angle <= 180; angle += _AgPlus) { // Move o servo de 0 a 180 graus
                LeituraSonar(angle, position);
              }
              Horario = false;
            }
            else {
              for(int angle = 180; angle > 0; angle -= _AgPlus) { // Move o servo de 0 a 180 graus
                LeituraSonar(angle, position);
              }
              Horario = true;
            }
          }
          Subindo = false;
        }
        else {
          for (position = 2; position > 0; position--) {
            position = 1; // REMOVER
            _ServoY.write(position * _AgPosition); // Move o servo para o ângulo desejado
            //vTaskDelay(100); // Espera meio segundo para o servo se mover
            if (Horario) {
              for(int angle = 0; angle <= 180; angle += _AgPlus) { // Move o servo de 0 a 180 graus
                LeituraSonar(angle, position);
              }
              Horario = false;
            }
            else {
              for(int angle = 180; angle > 0; angle -= _AgPlus) { // Move o servo de 0 a 180 graus
                LeituraSonar(angle, position);
              }
              Horario = true;
            }
          }
          Subindo = true;
        }
        
      }
    }

    void LeituraSonar(int angle, int position) {
      long duration, distance;

      for (int i = 0; i < 3; i++) {
        _pinoT.set(false);
        delayMicroseconds(2);
        _pinoT.set(true);
        delayMicroseconds(10);
        _pinoT.set(false);

        duration = pulseIn(_pinoE.num, HIGH);
        distance = (duration / 2) / 29.1; // Calcula a distância em centímetros
        if (distance > 10) {
          break;
        }
      }

      /*angulos[idx] = angle;
      distancias[idx] = distance;
      idx++;*/

      _ServoX.write(angle); // Move o servo para o ângulo desejado
      vTaskDelay(30); // Espera meio segundo para o servo se mover

      _distancia = distance;
      _angulo = angle;
      _posicao = position;

      RequisitarDados();

      /*if (idx >= 35) {
        EnviarDadosSerial(MontarProtocoloSensor(sSNR, _ID, MontarDadosProtocoloSonarM(distancias, angulos, position)));
        idx = 0;
      }*/
    }

};

#endif