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

static const int servoPin = 5;
Servo servo1;

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "test.mosquitto.org";
WiFiClient espclient;

// MQTT callback function
void callback(char* topic, byte* payload, unsigned int length1) {
  for (int i = 0; i < length1; i++) {
    Serial.print(payload[i]);
    if (payload[0] == 49) {
      for (int posDegrees = 0; posDegrees <= 180; posDegrees++) {
        servo1.write(posDegrees);
        Serial.println(posDegrees);
        delay(20);
      }

      for (int posDegrees = 180; posDegrees >= 0; posDegrees--) {
        servo1.write(posDegrees);
        Serial.println(posDegrees);
        delay(20);
      }
    } // ASCII VALUE OF '1' IS 49
    else if (payload[0] == 50) {
      for (int posDegrees = 0; posDegrees <= 180; posDegrees++) {
        // Modify this part based on your '2' logic
        // ...
        Serial.println(posDegrees);
        delay(20);
      }
    } // ASCII VALUE OF '2' IS 50
    Serial.println();
  }
  // Additional Serial statements related to the callback can go here if needed
}


PubSubClient client(mqtt_server,1883,callback,espclient);

void setup() {
  Serial.begin(115200);
  servo1.attach(servoPin);

  // Connect to WIFI
  Serial.print("connecting to ");
  Serial.print(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println();

  // Connect to Node-RED
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);

  if (client.connect("ESP32test1")) {
    Serial.println("Connected to Node-RED");
    client.subscribe("Servomoteur");
  }
}

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

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

  client.loop();

  delay(5000);

  // Additional loop code can go here
}