#include <ESP32Servo.h>
#include <NewPing.h> 

#include <PubSubClient.h>
#include <EspMQTTClient.h>
#include <ArduinoJson.h>

// Define os pinos para o trigger e echo
// Início
#define pino_trigger 15
#define pino_echo 2

#define ledInicio 33

// Final
#define pino_trigger1 16
#define pino_echo1 17

#define SERVO 18
#define ledFim 5 // Led final

//Led das etapas 
#define ledBlue 12
#define ledYellow 14
#define ledOrange 27
#define ledGreen 25
#define piezo 26

DynamicJsonDocument dados(1024);

EspMQTTClient client(
  "Wokwi-GUEST",
  "",
  "mqtt.tago.io",
  "Default",
  "81d1ecd1-ad01-4e35-97c3-aef7ff468334",
  "montagem"
);

//MQTT (Função que recebe dados do Subscribe)
void mensagemRecebida(const String &topico){
}

//Se conecta ao tópico estabelecido entre ""
void onConnectionEstablished() {
client.subscribe("Sensor1e2", mensagemRecebida);
}

//Inicializa o sensor nos pinos definidos acima
NewPing ultrasonic(pino_trigger, pino_echo, 200); //Início
NewPing ultrasonic1(pino_trigger1, pino_echo1); //Final

Servo s; // Variável Servo

void setup()
{
  Serial.begin(9600);//No arduino é 11600

  s.attach(SERVO);
  s.write(0); // Inicia motor posição zero

  //Leds que demarcam INÍCIO e FIM da montagem
  pinMode(ledInicio, OUTPUT);
  pinMode(ledFim,    OUTPUT);

  //Sensor Ultrassônico INÍCIO
  pinMode(pino_trigger,  OUTPUT);
  pinMode(pino_echo,     INPUT);

  //Sensor Ultrassônico FINAL
  pinMode(pino_trigger1, OUTPUT);
  pinMode(pino_echo1,    INPUT);

  //Leds das etapas
  pinMode(ledBlue,   OUTPUT);
  pinMode(ledYellow, OUTPUT);
  pinMode(ledOrange, OUTPUT);
  pinMode(ledGreen,  OUTPUT);
}
 
void loop()
{
client.loop();
    // Lê as informacoes do sensor, em cm
    float cmMsec, cmMsec1;

    cmMsec = ultrasonic.ping_cm();

    cmMsec1 = ultrasonic1.ping_cm();

//Envia se estamos disponíveis ou não

      // Sensor de início
      if(cmMsec <= 10){//&& valorRecebido >= 1

        // Enviar informação de indisponibilidade a equipe 2
        Serial.print("\nDistancia em cm (INÍCIO): ");
        Serial.println(cmMsec);

        digitalWrite(ledInicio, HIGH);
        delay(1000);
        digitalWrite(ledInicio, LOW);
          digitalWrite(ledBlue, HIGH);//Primeira etapa
          delay(1500);
          digitalWrite(ledBlue, LOW);
            digitalWrite(ledYellow, HIGH);//Segunda etapa
            delay(1500);
            digitalWrite(ledYellow, LOW);
              digitalWrite(ledOrange, HIGH);//Terceira etapa
              delay(1500);
              digitalWrite(ledOrange, LOW);//Fim da terceira etapa
                delay(1000);
                  digitalWrite(ledGreen, HIGH);//Quarta etapa
                  tone(piezo, 8000);
                  delay(1000);
                    noTone(piezo);
                    digitalWrite(ledGreen, LOW);//Fim da quarta etapa
      }
      else {
        Serial.print("\nDEU BOSTA\n");
      }
      
      //Sensor Montagem Finalizada
        if(cmMsec1 <= 10){
          Serial.print("\nDistancia em cm (FINAL): ");
          Serial.print(cmMsec1);
          digitalWrite(ledFim, HIGH);
          delay(2000);
          s.write(0);

          // If para validar o valor recebido da equipe 4
          // led
              delay(500);
              s.write(180);
              digitalWrite(ledFim, LOW);
              //Serial.println("...Pronto para expedição...");
              delay(800);
        }
      //Dados ficam salvos no arquivo Json
      dados[0]["variable"] = "Distância Sensor Início";
      dados[0]["value"] = float(cmMsec);
      dados[1]["variable"] = "Distância Sensor Final";
      dados[1]["value"] = float(cmMsec1);

      String dadosJson;
      serializeJson(dados, dadosJson);

      client.publish("Sensor1e2", dadosJson);
}