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

Servo myservo;
WiFiClient net;
MQTTClient client;

const int servoPin = 23;

const char ssid[] = "Wokwi-GUEST";
const char pass[] = "";

const int pinRed = 18;
const int pinGreen = 5;
const int pinBlue = 17;

char messageBuffer[20];


void setup() {
  Serial.begin(9600);
  pinMode(pinRed, OUTPUT);
  pinMode(pinGreen, OUTPUT);
  pinMode(pinBlue, OUTPUT);

  myservo.attach(servoPin);

  WiFi.begin(ssid, pass);
  client.begin("broker.emqx.io", net);
  client.onMessage(message);

  connect();

}

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

  client.onMessage(message);
  delay(10);
}

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

void connect() {
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
  Serial.println("Terhubung wifi");
  while (!client.connect("unix id")) {
    delay(500);
  }
  Serial.println("Terhubung docker");

  client.subscribe("system/otomatic/tol", 1);
}

void message(String topic, String data) {
  Serial.println(data);
  if (data.toInt() <= 250) {
    RGB(0, 0, 1);
    myservo.write(0);

    snprintf(messageBuffer, sizeof(messageBuffer), "Ada Mobil");
    publish(messageBuffer);

  } else {
    RGB (0, 1, 0);
    myservo.write(90);
    snprintf(messageBuffer, sizeof(messageBuffer), "Tidak Ada Mobil");
    publish(messageBuffer);
  }
}

void publish(const char* text) {
  client.publish("system/otomatic/tol/monitoring", text, true, 1);
}