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

// --- WiFi and MQTT Configuration ---
const char* WIFI_SSID = "Wokwi-GUEST";
const char* WIFI_PASSWORD = ""; // No password for Wokwi-Guest
const char* MQTT_BROKER = "broker.hivemq.com";
const int MQTT_PORT = 1883;
const char* MQTT_TOPIC = "wokwi/test/topic";

// --- Servo Configuration ---
Servo gateOneServo;
Servo gateTwoServo;
Servo gateThreeServo;

const int GATE_ONE_PIN = 4;
const int GATE_TWO_PIN = 16;
const int GATE_THREE_PIN = 23;

const int OPEN_ANGLE = 90;  // Angle to open the gate
const int CLOSE_ANGLE = 0;  // Angle to close the gate

// --- LED Configuration for Gate 2 and Gate 3 ---
// Choose some available GPIO pins for the LEDs
const int GATE_TWO_GREEN_LED = 32; 
const int GATE_TWO_RED_LED = 33;
const int GATE_THREE_GREEN_LED = 22;
const int GATE_THREE_RED_LED = 21;

// --- Objects ---
WiFiClient espClient;
PubSubClient client(espClient);

// --- Function Declarations ---
void setup_wifi();
void reconnect();
void callback(char* topic, byte* payload, unsigned int length);

// --- Setup ---
void setup() {
  Serial.begin(115200);

  // Set up servos
  gateOneServo.attach(GATE_ONE_PIN);
  gateTwoServo.attach(GATE_TWO_PIN);
  gateThreeServo.attach(GATE_THREE_PIN);

  // Set the initial state for all gates to closed
  gateOneServo.write(CLOSE_ANGLE);
  gateTwoServo.write(CLOSE_ANGLE);
  gateThreeServo.write(CLOSE_ANGLE);

  // Configure LED pins as outputs and set initial states
  pinMode(GATE_TWO_GREEN_LED, OUTPUT);
  pinMode(GATE_TWO_RED_LED, OUTPUT);
  pinMode(GATE_THREE_GREEN_LED, OUTPUT);
  pinMode(GATE_THREE_RED_LED, OUTPUT);

  // Set initial LED state: all gates are closed, so red LEDs are on and green LEDs are off
  digitalWrite(GATE_TWO_GREEN_LED, LOW);
  digitalWrite(GATE_TWO_RED_LED, HIGH);
  digitalWrite(GATE_THREE_GREEN_LED, LOW);
  digitalWrite(GATE_THREE_RED_LED, HIGH);

  // Connect to WiFi
  setup_wifi();

  // Set the MQTT server and callback function
  client.setServer(MQTT_BROKER, MQTT_PORT);
  client.setCallback(callback);
}

// --- Main Loop ---
void loop() {
  // Ensure the client is connected
  if (!client.connected()) {
    reconnect();
  }
  client.loop();
}

// --- WiFi Connection Function ---
void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(WIFI_SSID);

  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);

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

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

// --- MQTT Reconnection Function ---
void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("ESP32_Wokwi_Client")) {
      Serial.println("connected!");
      // Once connected, subscribe to the topic
      client.subscribe(MQTT_TOPIC);
      Serial.println("Subscribed to MQTT topic.");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" retrying in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

// --- MQTT Message Callback ---
void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived on topic: ");
  Serial.println(topic);

  // Create a String from the payload
  String message = "";
  for (int i = 0; i < length; i++) {
    message += (char)payload[i];
  }
  Serial.print("Message: ");
  Serial.println(message);

  // Process the command and control the servos and LEDs
  if (message == "OPEN_GATE1") {
    gateOneServo.write(OPEN_ANGLE);
    Serial.println("Gate 1 opened.");
  } else if (message == "CLOSE_GATE1") {
    gateOneServo.write(CLOSE_ANGLE);
    Serial.println("Gate 1 closed.");
  } else if (message == "OPEN_GATE2") {
    gateTwoServo.write(OPEN_ANGLE);
    // Turn on the green LED and turn off the red LED for gate 2
    digitalWrite(GATE_TWO_GREEN_LED, HIGH);
    digitalWrite(GATE_TWO_RED_LED, LOW);
    Serial.println("Gate 2 opened.");
  } else if (message == "CLOSE_GATE2") {
    gateTwoServo.write(CLOSE_ANGLE);
    // Turn on the red LED and turn off the green LED for gate 2
    digitalWrite(GATE_TWO_GREEN_LED, LOW);
    digitalWrite(GATE_TWO_RED_LED, HIGH);
    Serial.println("Gate 2 closed.");
  } else if (message == "OPEN_GATE3") {
    gateThreeServo.write(OPEN_ANGLE);
    // Turn on the green LED and turn off the red LED for gate 3
    digitalWrite(GATE_THREE_GREEN_LED, HIGH);
    digitalWrite(GATE_THREE_RED_LED, LOW);
    Serial.println("Gate 3 opened.");
  } else if (message == "CLOSE_GATE3") {
    gateThreeServo.write(CLOSE_ANGLE);
    // Turn on the red LED and turn off the green LED for gate 3
    digitalWrite(GATE_THREE_GREEN_LED, LOW);
    digitalWrite(GATE_THREE_RED_LED, HIGH);
    Serial.println("Gate 3 closed.");
  } else {
    Serial.println("Unknown command.");
  }
}