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

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const int channel = 6;

const char* mqtt_broker = "broker.hivemq.com";
const char* topic = "wokwi/mqtt//motor/chavaus";

WiFiClient espClient;
PubSubClient client(espClient);

#define M1_IN1 33
#define M1_IN2 25
#define M1_IN3 26
#define M1_IN4 27

#define M2_IN1 19
#define M2_IN2 18
#define M2_IN3 5
#define M2_IN4 17

AccelStepper motor1(AccelStepper::FULL4WIRE, M1_IN1, M1_IN2, M1_IN3, M1_IN4);
AccelStepper motor2(AccelStepper::FULL4WIRE, M2_IN1, M2_IN2, M2_IN3, M2_IN4);

bool motor1State = false;
bool motor2State = false;
int motorDirection = 1;
int motorSpeed = 200;

void connectToWiFi() {
  int attempts = 0;
  const int maxAttempts = 10;
  
  WiFi.disconnect();
  delay(1000);
  WiFi.begin(ssid, password);
  
  Serial.print("Menghubungkan ke WiFi");
  
  while (WiFi.status() != WL_CONNECTED && attempts < maxAttempts) {
    delay(500);
    Serial.print(".");
    attempts++;
  }
  
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("\nTerhubung ke Wi-Fi!");
    Serial.print("IP Address: ");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println("\nGagal terhubung ke WiFi!");
    ESP.restart(); // Restart ESP jika gagal connect
  }
}

void connectToMQTT() {
  int attempts = 0;
  const int maxAttempts = 5;
  
  while (!client.connected() && attempts < maxAttempts) {
    Serial.println("Menghubungkan ke MQTT Broker...");
    String clientId = "ESP32Client_" + String(random(0xffff), HEX);
    
    if (client.connect(clientId.c_str())) {
      Serial.println("Terhubung ke MQTT Broker!");
      client.subscribe(topic);
      Serial.println("Subscribe topik: " + String(topic));
      return;
    } else {
      Serial.print("Gagal terhubung ke MQTT. Error: ");
      Serial.println(client.state());
      attempts++;
      delay(2000);
    }
  }
  
  if (!client.connected()) {
    Serial.println("Gagal terhubung ke MQTT setelah beberapa percobaan. Merestart ESP...");
    ESP.restart();
  }
}

void setup() {
  Serial.begin(115200);
  
  // Setup motor
  motor1.setMaxSpeed(500);
  motor1.setAcceleration(200);
  motor2.setMaxSpeed(500);
  motor2.setAcceleration(200);
  
  // Setup WiFi
  WiFi.mode(WIFI_STA);
  WiFi.disconnect(true);
  delay(1000);
  
  connectToWiFi();
  
  // Setup MQTT
  client.setServer(mqtt_broker, 1883);
  client.setCallback(callback);
  connectToMQTT();
}

void loop() {
  // Cek koneksi WiFi dan MQTT
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("Koneksi WiFi terputus. Mencoba menghubungkan kembali...");
    connectToWiFi();
  }
  
  if (!client.connected()) {
    connectToMQTT();
  }
  
  client.loop();

  // Kontrol motor
  if (motor1State) {
    motor1.setSpeed(motorSpeed * motorDirection);
    motor1.runSpeed();
  }
  if (motor2State) {
    motor2.setSpeed(motorSpeed * motorDirection);
    motor2.runSpeed();
  }
}

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

  Serial.print("Isi Pesan: ");
  Serial.println(message);

  if (message == "MAJU") {
    motor1State = true;
    motor2State = true;
    motorDirection = 1;
    Serial.println("Kedua motor maju");
  } else if (message == "MUNDUR") {
    motor1State = true;
    motor2State = true;
    motorDirection = -1;
    Serial.println("Kedua motor mundur");
  } else if (message == "KIRI") {
    motor1State = true;
    motor2State = false;
    motor2.stop();
    Serial.println("Hanya motor 1 (KIRI)");
  } else if (message == "KANAN") {
    motor1State = false;
    motor2State = true;
    motor1.stop();
    Serial.println("Hanya motor 2 (KANAN)");
  } else if (message == "STOP") {
    motor1State = false;
    motor2State = false;
    motor1.stop();
    motor2.stop();
    Serial.println("Kedua motor berhenti");
  } else {
    Serial.println("Perintah tidak dikenali!");
  }
}