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

WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);
const char* ssid = "Wokwi-GUEST";
const char* password = "";

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

// Pin untuk kendali motor DC
#define MOTOR_A_MAJU_PIN 34
#define MOTOR_A_MUNDUR_PIN 32
#define MOTOR_B_KIRI_PIN 21
#define MOTOR_B_KANAN_PIN 19

// Definisikan topik MQTT untuk LED dan motor
const char* mqtt_topic_led = "esp32/onoff";
const char* mqtt_topic_motor = "esp32/motor";

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 = "ESP32Client-";
    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);
  pinMode(25, OUTPUT);
  pinMode(MOTOR_A_MAJU_PIN, OUTPUT);
  pinMode(MOTOR_A_MUNDUR_PIN, OUTPUT);
  pinMode(MOTOR_B_KIRI_PIN, OUTPUT);
  pinMode(MOTOR_B_KANAN_PIN, OUTPUT);

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

  setupMQTT();
}

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

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

  // Kontrol LED jika pesan diterima dari topik LED
  if (String(topic) == mqtt_topic_led)
  {
    if (data.startsWith("ON"))
    {
      digitalWrite(25, HIGH);
    }
    else if (data.startsWith("OFF"))
    {
      digitalWrite(25, LOW);
    }
  }
  // Kontrol motor DC jika pesan diterima dari topik motor
  else if (String(topic) == mqtt_topic_motor)
  {
    if (data.startsWith("MAJU"))
    {
      digitalWrite(MOTOR_A_MAJU_PIN, HIGH);
      digitalWrite(MOTOR_A_MUNDUR_PIN, LOW);
    }
    else if (data.startsWith("MUNDUR"))
    {
      digitalWrite(MOTOR_A_MAJU_PIN, LOW);
      digitalWrite(MOTOR_A_MUNDUR_PIN, HIGH);
    }
    else if (data.startsWith("KIRI"))
    {
      digitalWrite(MOTOR_B_KIRI_PIN, HIGH);
      digitalWrite(MOTOR_B_KANAN_PIN, LOW);
    }
    else if (data.startsWith("KANAN"))
    {
      digitalWrite(MOTOR_B_KIRI_PIN, LOW);
      digitalWrite(MOTOR_B_KANAN_PIN, HIGH);
    }
  }
}