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

const char* ssid = "Wokwi-GUEST";
const char* password = "";

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

WiFiClient espClient;
PubSubClient client(espClient);

#define M1_IN1 19
#define M1_IN2 18
#define M1_IN3 5
#define M1_IN4 17

AccelStepper motor(AccelStepper::FULL4WIRE, M1_IN1, M1_IN2, M1_IN3, M1_IN4);

bool motorState = false; // Status motor (ON/OFF)
int motorSpeed = 200;    // Kecepatan motor

void setup() {
  Serial.begin(115200);
  
  // Inisialisasi pin motor sebagai output
  pinMode(M1_IN1, OUTPUT);
  pinMode(M1_IN2, OUTPUT);
  pinMode(M1_IN3, OUTPUT);
  pinMode(M1_IN4, OUTPUT);
  
  // Set properti motor
  motor.setMaxSpeed(500);
  motor.setAcceleration(200);
  motor.setSpeed(motorSpeed);

  // Koneksi WiFi
  Serial.print("Menghubungkan ke WiFi");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" Terhubung ke Wi-Fi!");
  
  // Print informasi koneksi WiFi
  Serial.print("IP Address: ");
  Serial.println(WiFi.localIP());
  Serial.print("Signal Strength (RSSI): ");
  Serial.println(WiFi.RSSI());

  // Setup MQTT
  client.setServer(mqtt_broker, 1883);
  client.setCallback(callback);

  // Koneksi ke MQTT Broker
  connectToMQTT();
}

void connectToMQTT() {
  while (!client.connected()) {
    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!");
      // Subscribe ke topic
      client.subscribe(topic);
      Serial.println("Subscribe Topic: " + String(topic));
    } else {
      Serial.print("Gagal Terhubung. Error code: ");
      Serial.println(client.state());
      delay(2000);
    }
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
  Serial.println("\n--------------------");
  Serial.print("Pesan yang diterima di topic: ");
  Serial.println(topic);

  String message = "";
  for (int i = 0; i < length; i++) {
    message += (char)payload[i];
  }

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

  if (message == "HIDUP") {
    motorState = true;
    Serial.println("Motor dinyalakan");
    Serial.print("Motor State: ");
    Serial.println(motorState);
    Serial.print("Motor Speed: ");
    Serial.println(motorSpeed);
    
    // Tambahan untuk memastikan motor bergerak
    motor.setSpeed(motorSpeed);
    motor.enableOutputs();
  } else if (message == "MATI") {
    motorState = false;
    motor.stop();
    motor.disableOutputs();
    Serial.println("Motor dimatikan");
    Serial.print("Motor State: ");
    Serial.println(motorState);
  } else {
    Serial.println("Perintah tidak dikenali!");
  }
  Serial.println("--------------------");
}

void loop() {
  // Reconnect jika koneksi terputus
  if (!client.connected()) {
    connectToMQTT();
  }
  client.loop();
  
  // Debug motor status setiap 5 detik
  static unsigned long lastDebug = 0;
  if (millis() - lastDebug > 5000) {
    Serial.print("Status Motor: ");
    Serial.println(motorState ? "HIDUP" : "MATI");
    Serial.print("Kecepatan: ");
    Serial.println(motorSpeed);
    lastDebug = millis();
  }

  // Kontrol motor
  if (motorState) {
    motor.setSpeed(motorSpeed); 
    motor.runSpeed();
  }
}