#include <ESP32Servo.h>
#include <Ultrasonic.h>
#include <WiFi.h>
#include <PubSubClient.h>

// const int trigPin = 2;
// const int echoPin = 5;
#define SOUND_SPEED 0.034 
long duration;
float distanceCm;
Ultrasonic sensor1(2, 5);

// WiFi
const char* ssid ="Wokwi-GUEST";
const char* password = "";

// MQTT
const char* mqtt_server = "broker.hivemq.com";
const char* mqttTopicObject = "ultrasonic/object";
const char* mqttTopicServo = "tom/output/servo";

WiFiClient espClient;
PubSubClient client(espClient);

// Servo
Servo servo;  // create servo object to control a servo

int SERVO_PIN = 4;  // analog pin used to connect the potentiometer

void setup_wifi() {
  delay(10);
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
}

void reconnect() {
  while (!client.connected()) {
    Serial.println("Attempting MQTT connection...");
    String clientId = "esp32-dht22-clientId-";
    clientId += String(random(0xffff), HEX);
    if (client.connect(clientId.c_str())) {
      Serial.println("Connected");
      client.subscribe(mqttTopicServo);
    } else {
      delay(5000);
    }
  }
}

void callback(char *topic, byte *payload, unsigned int length) {
  Serial.print("Receive Topic: ");
  Serial.println(topic);

  Serial.print("Payload: ");
  Serial.println((char *)payload);
  if (!strcmp(topic, mqttTopicServo)) {
    if (!strncmp((char *)payload, "on", length)) {
      servo.write(90);
    } else if (!strncmp((char *)payload, "off", length)) {
      servo.write(0);
    }
  }
}

//Ultrasonic1
// void getDistance(){ 
  
//   digitalWrite(trigPin, LOW); 
// delayMicroseconds(2);
// digitalWrite(trigPin, HIGH); 
// delayMicroseconds(10); 
// digitalWrite(trigPin, LOW);

// duration = pulseIn(echoPin, HIGH);

// distanceCm = duration * SOUND_SPEED * 0.5;

//  Serial.print("Distance: "); 
//  Serial.print(distanceCm);
//   Serial.println("cm");
// }

void setup() {
  Serial.begin(115200);
  randomSeed(micros());
 pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
 

  servo.attach(SERVO_PIN, 500, 2400);  // attaches the servo on pin 4 to the servo object
  
  servo.write(0);

  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
}



void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  //getDistance();

  // Publish distance values to MQTT topics
     long distance1 = sensor1.read();
     Serial.print("Distance from Sensor 1: ");
     Serial.println(distance1);

  char message[50];

  snprintf(message, sizeof(message), "%ld", getDistance);
  
  client.publish(mqttTopicObject, message);

  delay(1000);
}