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

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

// MQTT configuration
const char* mqtt_server = "io.adafruit.com";
const int mqtt_port = 1883;
const char* aio_username = "sriharish05";
const char* aio_key = "aio_zQXf560JS1EuxpauUw8iDWAMzNdA";

// MQTT topics
const char* lock_command_topic = "sriharish05/feeds/locks.command";
const char* lock_status_topic = "sriharish05/feeds/locks.status";
const char* lock_log_topic = "sriharish05/feeds/locks.log";

// Servo configuration
#define LOCK1_PIN 16
#define LOCK2_PIN 17
#define LOCK3_PIN 18

// Create instances
WiFiClient espClient;
PubSubClient client(espClient);
ESP32Servo servo1, servo2, servo3;

// Function to send MQTT responses
void sendMQTTResponse(int lock_id, const char* action, const char* status) {
  char message[50];
  sprintf(message, "%d,%s", lock_id, status);
  client.publish(lock_status_topic, message);
  Serial.print("Published to Status: ");
  Serial.println(message);

  // Log the action
  char logMessage[100];
  sprintf(logMessage, "Lock %d -> Action: %s | Status: %s", lock_id, action, status);
  client.publish(lock_log_topic, logMessage);
  Serial.print("Logged: ");
  Serial.println(logMessage);
}

// MQTT callback function
void callback(char* topic, byte* payload, unsigned int length) {
  // Convert payload to string
  char message[length + 1];
  for (int i = 0; i < length; i++) {
    message[i] = (char)payload[i];
  }
  message[length] = '\0';
  
  Serial.print("Received MQTT Message: ");
  Serial.println(message);

  // Parse message
  char* lockIdStr = strtok(message, ",");
  char* action = strtok(NULL, ",");
  
  if (!lockIdStr || !action) {
    Serial.println("Invalid message format!");
    return;
  }

  int lock_id = atoi(lockIdStr);
  
  // Handle lock actions
  ESP32Servo* targetServo = NULL;
  switch(lock_id) {
    case 1: targetServo = &servo1; break;
    case 2: targetServo = &servo2; break;
    case 3: targetServo = &servo3; break;
    default: break;
  }
  
  if (targetServo != NULL) {
    if (strcmp(action, "lock") == 0) {
      targetServo->write(0);
      sendMQTTResponse(lock_id, action, "success");
    } 
    else if (strcmp(action, "unlock") == 0) {
      targetServo->write(90);
      sendMQTTResponse(lock_id, action, "success");
    }
    else {
      sendMQTTResponse(lock_id, action, "invalid_action");
    }
  } 
  else {
    sendMQTTResponse(lock_id, action, "invalid_lock");
  }
}

// Connect to MQTT
void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Connecting to MQTT... ");
    
    // Create a random client ID
    String clientId = "ESP32Client-";
    clientId += String(random(0xffff), HEX);
    
    // Attempt to connect
    if (client.connect(clientId.c_str(), aio_username, aio_key)) {
      Serial.println("connected");
      client.subscribe(lock_command_topic);
      Serial.print("Subscribed to: ");
      Serial.println(lock_command_topic);
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" retrying in 5 seconds");
      delay(5000);
    }
  }
}

void setup() {
  // Initialize serial
  Serial.begin(115200);
  delay(1000);
  Serial.println("ESP32 Smart Lock System Starting...");
  
  // Initialize servos
  servo1.attach(LOCK1_PIN);
  servo2.attach(LOCK2_PIN);
  servo3.attach(LOCK3_PIN);
  
  // Set initial position
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  
  // Setup WiFi
  Serial.printf("Connecting to WiFi (%s)...\n", ssid);
  WiFi.begin(ssid, password);
  
  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < 20) {
    delay(500);
    Serial.print(".");
    attempts++;
  }
  
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("\nWiFi connected");
    Serial.print("IP address: ");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println("\nFailed to connect to WiFi");
  }
  
  // Setup MQTT
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);
  
  // Initial connection
  if (WiFi.status() == WL_CONNECTED) {
    reconnect();
  }
}

void loop() {
  // Check MQTT connection
  if (!client.connected() && WiFi.status() == WL_CONNECTED) {
    reconnect();
  }
  
  // Process MQTT messages
  if (client.connected()) {
    client.loop();
  }
  
  delay(10); // Small delay for stability
}