#include <Arduino.h>
#include <WiFi.h>
#include <WiFiClientSecure.h>
#include <PubSubClient.h>
#include <ESP32Servo.h>  // Include the ESP32Servo library

// Wi-Fi credentials
const char* ssid = "ADHAM";
const char* password = "121269@ma#";

// MQTT broker details
const char* mqtt_broker = "0785cb996e8440269dfc410343b0d3ef.s1.eu.hivemq.cloud";
const int mqtt_port = 8883;
const char* mqtt_username = "shehab101";
const char* mqtt_password = "@inV6GzVjGtES2x";

// MQTT topics
const char* topic_publish_ir = "sensor/data";
const char* topic_subscribe_angle = "servo/angle";  // Topic for servo angle

// IR Sensor details
const int ir_sensor_pin = 34; // GPIO pin connected to the IR sensor

// Servo motor details
Servo servo_motor; // Create a servo object
const int servo_pin = 5; // GPIO pin connected to the servo motor

// Create instances
WiFiClientSecure wifiClient;
PubSubClient mqttClient(wifiClient);

// Variables for timing
long previous_time = 0;

// Callback function to handle incoming messages
void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("]: ");
  String message = "";
  for (int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
    message += (char)payload[i];
  }
  Serial.println();

  // Check if the message is from the servo angle topic
  if (String(topic) == topic_subscribe_angle) {
    int angle = message.toInt(); // Convert the message to an integer angle
    if (angle >= 0 && angle <= 180) {
      servo_motor.write(angle); // Set the servo motor to the desired angle
      Serial.print("Servo motor set to angle: ");
      Serial.println(angle);
    } else {
      Serial.println("Received angle is out of range (0-180)");
    }
  }
}

void setupMQTT() {
  mqttClient.setServer(mqtt_broker, mqtt_port);
  mqttClient.setCallback(callback);
}

void reconnect() {
  Serial.println("Connecting to MQTT Broker...");
  while (!mqttClient.connected()) {
    Serial.println("Reconnecting to MQTT Broker...");
    String clientId = "ESP32Client-";
    clientId += String(random(0xffff), HEX);
    
    if (mqttClient.connect(clientId.c_str(), mqtt_username, mqtt_password)) {
      Serial.println("Connected to MQTT Broker.");
      
      // Subscribe to the servo angle topic after connecting
      mqttClient.subscribe(topic_subscribe_angle);
    } else {
      Serial.print("Failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("Connected to Wi-Fi");

  // Initialize secure WiFiClient
  wifiClient.setInsecure(); // Use this only for testing, it allows connecting without a root certificate
  
  setupMQTT();

  pinMode(ir_sensor_pin, INPUT); // Set IR sensor pin as input
  servo_motor.attach(servo_pin); // Attach the servo motor to the specified pin
}

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

  long now = millis();
  if (now - previous_time > 1000) { // Publish every 10 seconds
    previous_time = now;

    // Read the IR sensor value
    int ir_sensor_value = analogRead(ir_sensor_pin);
    int distance = map(ir_sensor_value, 0, 4095, 0, 30);
    
    // Convert the value to a string
    String ir_value_str = String(distance);
    
    // Publish the sensor value to the MQTT topic
    Serial.print("IR Sensor Value: ");
    Serial.println(ir_value_str);
    mqttClient.publish(topic_publish_ir, ir_value_str.c_str());
  }
}