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

// Definisi WiFi
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// Definisi MQTT
const char* mqttServer = "mqttmuti.sija-bridge.tech";
const int mqttPort = 1808;
const char* mqttTopic = "kinasih/device";
const char* mqttControlTopic = "kinasih/device/control";

// Pin Ultrasonik
const int trigPin = 18;
const int echoPin = 19;  // Pastikan pin ini tidak bentrok dengan pin lainnya

// LED pins
const int ledg = 2;
const int ledy = 4;
const int ledr = 5;

// Variabel kontrol sensor
bool sensorEnabled = true;

WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledg, OUTPUT);
  pinMode(ledy, OUTPUT);
  pinMode(ledr, OUTPUT);
  
  connectToInternet();
  setupMQTT();
}

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

void setupMQTT() {
  mqttClient.setServer(mqttServer, mqttPort);
  mqttClient.setCallback(callback);
}

void reconnectMQTT() {
  Serial.println("Connecting to MQTT Broker...");
  while (!mqttClient.connected()) {
    Serial.println("Attempting MQTT connection...");
    String clientId = "ESP32Client-";
    clientId += String(random(0xffff), HEX);
    
    if (mqttClient.connect(clientId.c_str())) {
      Serial.println("Connected to MQTT Broker.");
      mqttClient.subscribe(mqttControlTopic); // Subscribe to control topic
    } else {
      Serial.print("Failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" Retrying in 5 seconds...");
      delay(5000);
    }     
  }
}

long readUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.0343 / 2; 
  
  return distance;
}

void callback(char* topic, byte* payload, unsigned int length) {
  String messageTemp;
  for (int i = 0; i < length; i++) {
    messageTemp += (char)payload[i];
  }

  Serial.print("Message received on topic: ");
  Serial.println(topic);
  Serial.print("Message: ");
  Serial.println(messageTemp);

  if (String(topic) == mqttControlTopic) {
    if (messageTemp == "ON") {
      sensorEnabled = true;
      Serial.println("Sensor enabled");
    } else if (messageTemp == "OFF") {
      sensorEnabled = false;
      Serial.println("Sensor disabled");
    }
  }
}

void controlLEDs(float distance) {
  if (distance < 20) {
    digitalWrite(ledy, HIGH);
    digitalWrite(ledr, LOW);
    digitalWrite(ledg, LOW);
    Serial.println("Warning: Distance below 20 cm");
  } else if (distance > 100) {
    digitalWrite(ledy, LOW);
    digitalWrite(ledr, HIGH);
    digitalWrite(ledg, LOW);
    Serial.println("Danger: Distance above 100 cm");
  } else {
    digitalWrite(ledy, LOW);
    digitalWrite(ledr, LOW);
    digitalWrite(ledg, HIGH);
    Serial.println("Normal: Distance between 20 cm and 100 cm");
  }
}

void loop() {
  if (!mqttClient.connected()) {
    reconnectMQTT();
  }
  
  mqttClient.loop();  // Pastikan client MQTT selalu memproses pesan yang masuk
  
  if (sensorEnabled) {
    long distance = readUltrasonicDistance();

    if (distance >= 0) {
      controlLEDs(distance);

      StaticJsonDocument<100> doc;
      doc["distance"] = distance;

      char buffer[100];
      serializeJson(doc, buffer);

      mqttClient.publish(mqttTopic, buffer);
    }

    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
  } else {
    Serial.println("Sensor is off");
  }

  delay(200);
}