#include <WiFi.h>
#include <MQTT.h>
#include <ESP32Servo.h>

const char* ssid = "Wokwi-GUEST";           
const char* password = "";  
const char* mqtt_server = "broker.emqx.io";   

WiFiClient net;
MQTTClient client;

Servo servoX;
Servo servoY;

void connectWiFi() {
  Serial.print("Connecting to WiFi...");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nConnected to WiFi");
}

void messageReceived(String &topic, String &payload) {
  Serial.println("Received [" + topic + "]: " + payload);

  int posX = payload.substring(0, payload.indexOf(' ')).toInt();
  int posY = payload.substring(payload.indexOf(' ') + 1).toInt();

  if (posX >= 0 && posX <= 180) {
    servoX.write(posX);
  }
  if (posY >= 0 && posY <= 180) {
    servoY.write(posY);
  }
}

void connectMQTT() {
  Serial.print("Connecting to MQTT...");
  while (!client.connect("arduino")) {  
    Serial.print(".");
    delay(1000);
  }
  Serial.println("\nConnected to MQTT");

  client.subscribe("servo/position");
}

void setup() {
  Serial.begin(115200);

  connectWiFi();
  client.begin(mqtt_server, net);
  client.onMessage(messageReceived);

  connectMQTT();

  servoX.attach(26);  
  servoY.attach(27); 
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    connectWiFi();
  }

  if (!client.connected()) {
    connectMQTT();
  }

  client.loop();
}