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

WiFiClient net;
MQTTClient client;
Servo myServo;

const char ssid[] = "Wokwi-GUEST";
const char pass[] = "";
const char brokerName[] = "broker.emqx.io";
const char clientID[] = "client-002";
const char topicMQTT1[] = "kelasiot/led";
const char topicMQTT2[] = "kelasiot/servo";
const char subTopic[] = "kelasiot/#";

const int pinRed = 2;
const int pinGreen = 4;
const int pinBlue = 16;
const int pinLEDbiasa = 13;
const int pinServo = 33;

void setup() {
  pinMode(pinRed, OUTPUT);
  pinMode(pinGreen, OUTPUT);
  pinMode(pinBlue, OUTPUT);
  pinMode(pinLEDbiasa, OUTPUT);
  myServo.attach(pinServo, 500, 2400);
  Serial.begin(9600);

  WiFi.begin(ssid, pass);
  client.begin(brokerName, net);
  
  client.onMessage(subscribe);
  connect();  
}

void loop() {
  client.loop();
  if (!client.connected()) {
    connect();
  }

  delay(10);
}

void subscribe(String &topic, String &data) {
  if (topic == topicMQTT1) {
    if (data == "on") {
      digitalWrite(pinLEDbiasa, 1);
    }
    else if (data == "off") {
      digitalWrite(pinLEDbiasa, 0);
    }
  }

  if (topic == topicMQTT2) {
    int posServo = data.toInt();
    myServo.write(posServo);
    Serial.println("Posisi Servo: "+String(posServo)+"°");
  }
}

void rgb(bool red, bool green, bool blue) {
  digitalWrite(pinRed, red);
  digitalWrite(pinGreen, green);
  digitalWrite(pinBlue, blue);
}

void connect() {
  rgb(1,0,0); // Merah
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    rgb(1,0,0); rgb(0,1,0); // Kuning
  }
  delay(1000);
  rgb(0,1,0); // Hijau

  while (!client.connect(clientID)) {
    delay(500);
  }
  delay(1000);
  rgb(0,0,1); // Biru
  
  client.subscribe(subTopic, 1); // Subscribe dengan QoS 1
}