#include <Wire.h>
#include <Arduino.h>
#include <WiFi.h>
#include <PubSubClient.h>

// Wi-Fi and MQTT Configuration
const char* ssid = "Wokwi-GUEST";
const char* password = "";

const char* mqttServer = "broker.hivemq.com";
int mqttPort = 1883;

// Pin definitions
const int LED_PIN = 15;
const int MOTOR_F_PIN = 19;
const int MOTOR_B_PIN = 18;
const int MOTOR_L_PIN = 4;
const int MOTOR_R_PIN = 2;

// MQTT Topics
const char* mqtt_topic_led = "b15/led";
const char* mqtt_topic_motor = "b15/motor";

// WiFi and MQTT clients
WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);

void setupMQTT() {
  mqttClient.setServer(mqttServer, mqttPort);
  mqttClient.setCallback(callback);
}

void reconnect() {
  Serial.println("Connecting to MQTT Broker...");
  while (!mqttClient.connected()) {
    Serial.println("Reconnecting to MQTT Broker...");
    String clientId = "ClientId-";
    clientId += String(random(0xffff), HEX);

    if (mqttClient.connect(clientId.c_str())) {
      Serial.println("Connected.");
      // Subscribe to topics
      mqttClient.subscribe(mqtt_topic_led);
      mqttClient.subscribe(mqtt_topic_motor);
    }
  }
}

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

  // Set pin modes
  pinMode(LED_PIN, OUTPUT);
  pinMode(MOTOR_F_PIN, OUTPUT);
  pinMode(MOTOR_B_PIN, OUTPUT);
  pinMode(MOTOR_L_PIN, OUTPUT);
  pinMode(MOTOR_R_PIN, OUTPUT);

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

  // Setup MQTT
  setupMQTT();
}

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

void callback(char* topic, byte* message, unsigned int length) {
  Serial.print("Message Received: [");
  Serial.print(topic);
  Serial.print("] ");
  String data = "";
  for (int i = 0; i < length; i++) {
    Serial.print((char)message[i]);
    data += (char)message[i];
  }
  Serial.println();

  // Control LED
  if (String(topic) == mqtt_topic_led) {
    if (data.startsWith("ON")) {
      digitalWrite(LED_PIN, HIGH);
    } else if (data.startsWith("OFF")) {
      digitalWrite(LED_PIN, LOW);
    }
  }
  // Control DC motors
  else if (String(topic) == mqtt_topic_motor) {
    if (data.startsWith("FORWARD")) {
      digitalWrite(MOTOR_F_PIN, HIGH);
      digitalWrite(MOTOR_B_PIN, LOW);
      digitalWrite(MOTOR_L_PIN, LOW);
      digitalWrite(MOTOR_R_PIN, LOW);
    } else if (data.startsWith("BACKWARD")) {
      digitalWrite(MOTOR_F_PIN, LOW);
      digitalWrite(MOTOR_B_PIN, HIGH);
      digitalWrite(MOTOR_L_PIN, LOW);
      digitalWrite(MOTOR_R_PIN, LOW);
    } else if (data.startsWith("LEFT")) {
      digitalWrite(MOTOR_L_PIN, HIGH);
      digitalWrite(MOTOR_R_PIN, LOW);
      digitalWrite(MOTOR_F_PIN, LOW);
      digitalWrite(MOTOR_B_PIN, LOW);
    } else if (data.startsWith("RIGHT")) {
      digitalWrite(MOTOR_L_PIN, LOW);
      digitalWrite(MOTOR_R_PIN, HIGH);
      digitalWrite(MOTOR_F_PIN, LOW);
      digitalWrite(MOTOR_B_PIN, LOW);
    }
  }
}