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

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

// Define os pinos para o trigger e echo do inicio
#define pino_trigger 15
#define pino_echo 2
#define ledInicio 33

// Final
#define pino_trigger1 16
#define pino_echo1 17
#define ledFim 5 // Led final

#define SERVO 18

//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){
}

void onConnectionEstablished() {
client.subscribe("Sensor1e2", mensagemRecebida);
}

//Inicializa o sensor nos pinos definidos acima

NewPing ultrasonic1(pino_trigger1, pino_echo1);
NewPing ultrasonic(pino_trigger, pino_echo);

Servo s; // Variável Servo

void setup()
{
  Serial.begin(9600);
  s.attach(SERVO);
  s.write(0);
  pinMode(ledInicio, OUTPUT);
  pinMode(ledFim, OUTPUT);

  pinMode(ledBlue, OUTPUT);
  pinMode(ledYellow, OUTPUT);
  pinMode(ledOrange, OUTPUT);
  pinMode(ledGreen, OUTPUT);

  //Declaração do primeiro sensor
  pinMode(pino_trigger, OUTPUT);
  pinMode(pino_echo, INPUT);

  //Declaraç~so do segundo sensor
  pinMode(pino_trigger1, OUTPUT);
  pinMode(pino_echo1, INPUT);
}
 
void loop()
{

client.loop();

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
        digitalWrite(ledInicio, HIGH);
        Serial.print("\nInicio de processo de montagem ");
        delay(500);
        Serial.print("\nA distância do objeto é de: ");
        Serial.println(cmMsec);
        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.println("\n\nNada foi detectado");

          for(int i=5; i>0; i--){
            Serial.print("\nIniciando novamente em: ");
            Serial.print(i);
            delay(1000);
          }

        Serial.print("...");
        return;
      }

      //Dados ficam salvos no arquivo Json
      dados[0]["variable"] = "Distância Sensor Início";
      dados[0]["value"] = float(cmMsec);

      //Sensor Montagem Finalizada

        if(cmMsec1 <= 10){
          Serial.print("\n\nDistancia em cm do sensor final: ");
          Serial.print(cmMsec1);
          digitalWrite(ledFim, HIGH);
          delay(2000);
          Serial.println("...Montagem finalizada...");

          // If para validar o valor recebido da equipe 4, se já podemos enviar o produto

          digitalWrite(ledFim, LOW);
          Serial.println("...Enviando para expedição...");
          delay(800);
        }
      
        if(cmMsec <= 10){
        s.write(180);
        }

      //Dados ficam salvos no arquivo Json
      dados[1]["variable"] = "Distância Sensor Final";
      dados[1]["value"] = float(cmMsec1);

      String dadosJson;
      serializeJson(dados, dadosJson);

      client.publish("Sensor1e2", dadosJson);

	    //If para se o produto já saiu da nossa linha de produção e pode iniciar novamente 
}