#include <WiFi.h>
#include <PubSubClient.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <ESP32Servo.h>

// WiFi and MQTT settings
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqttServer = "thingsboard.cloud";
const int mqttPort = 1883;
const char* token = "9kupdd5kfi7e6k892998";

// Ultrasonic sensor settings
const int triggerPin = 12;
const int echoPin = 14;
float sound_speed = 0.034; // cm/us

// MPU6050 instance
Adafruit_MPU6050 mpu;

// Servo settings
Servo servoMotor1;
int servoMin1 = 0;
int servoMax1 = 180;
Servo servoMotor2;
int servoMin2 = 0;
int servoMax2 = 180;

// LED settings
byte led_pins[] = {13, 15, 2, 4, 5, 18, 19, 21, 22, 23};

// Buzzer settings
int BUZZER = 27;

// WiFi and MQTT client
WiFiClient espClient;
PubSubClient client(espClient);

void setup() {
  Serial.begin(115200);

  // WiFi setup
  WiFi.begin(ssid, password);
  Serial.println("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("...");
  }
  Serial.println("Connected to WiFi");

  // MQTT setup
  client.setServer(mqttServer, mqttPort);

  // MPU6050 setup
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) delay(10);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);

  // Ultrasonic sensor setup
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Buzzer setup
  pinMode(BUZZER, OUTPUT);

  // LED setup
  for (int i = 0; i < sizeof(led_pins); i++) {
    pinMode(led_pins[i], OUTPUT);
  }

  // Servo setup - menggunakan pin berbeda untuk menghindari konflik
  servoMotor1.attach(25);  // Mengubah pin servo
  servoMotor2.attach(26);  // Mengubah pin servo

  // Indicator testing
  indicator_testing();
}

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

  // Read MPU6050 data
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Control servos based on gyroscope data
  int servoPos1 = map(g.gyro.x, -4.3, +4.3, servoMin1, servoMax1);
  int servoPos2 = map(g.gyro.y, -4.3, +4.3, servoMin2, servoMax2);
  servoMotor1.write(servoPos1);
  servoMotor2.write(servoPos2);

  // Ultrasonic sensor distance measurement dengan debug
  int distance = get_distance();
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Update LED indicator dan buzzer
  indicator(distance);
  buzzer_alert(distance);

  // Send data to ThingsBoard
  sendDataToThingsBoard(distance, a, g, temp);

  delay(100); // Mengurangi delay untuk pembacaan lebih responsif
}

int get_distance() {
  // Reset trigger pin
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  
  // Kirim pulsa ultrasonik
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  
  // Baca durasi pulsa echo
  long duration = pulseIn(echoPin, HIGH, 30000); // Timeout 30ms
  
  // Hitung jarak
  int distance = (duration * sound_speed) / 2;
  
  // Filter hasil yang tidak valid
  if (distance <= 0 || distance > 400) {
    return 400; // Nilai maksimum valid untuk HC-SR04
  }
  
  return distance;
}

void indicator(int distance) {
  // Reset semua LED dulu
  for (int i = 0; i < sizeof(led_pins); i++) {
    digitalWrite(led_pins[i], LOW);
  }
  
  // Hitung berapa LED yang harus menyala
  int ledsToLight = map(distance, 0, 150, sizeof(led_pins), 0);
  ledsToLight = constrain(ledsToLight, 0, sizeof(led_pins));
  
  // Nyalakan LED sesuai jarak
  for (int i = 0; i < ledsToLight; i++) {
    digitalWrite(led_pins[i], HIGH);
  }
}

void buzzer_alert(int distance) {
  if (distance <= 45 && distance > 30) {
    tone(BUZZER, 1000);
    delay(200);
    noTone(BUZZER);
    delay(200);
  } else if (distance <= 30 && distance > 15) {
    tone(BUZZER, 2000);
    delay(100);
    noTone(BUZZER);
    delay(100);
  } else if (distance <= 15) {
    tone(BUZZER, 3000);
    delay(50);
    noTone(BUZZER);
    delay(50);
  } else {
    noTone(BUZZER);
  }
}

void sendDataToThingsBoard(int distance, sensors_event_t a, sensors_event_t g, sensors_event_t temp) {
  char jsonBuffer[200];
  snprintf(jsonBuffer, sizeof(jsonBuffer),
           "{\"distance\":%d,\"acceleration_x\":%.2f,\"acceleration_y\":%.2f,\"acceleration_z\":%.2f,\"gyro_x\":%.2f,\"gyro_y\":%.2f,\"gyro_z\":%.2f,\"temperature\":%.2f}",
           distance, a.acceleration.x, a.acceleration.y, a.acceleration.z, g.gyro.x, g.gyro.y, g.gyro.z, temp.temperature);

  Serial.print("Sending data to ThingsBoard: ");
  Serial.println(jsonBuffer);

  if (client.publish("v1/devices/me/telemetry", jsonBuffer)) {
    Serial.println("Publish success");
  } else {
    Serial.println("Publish failed");
  }
}

void indicator_testing() {
  for (int i = 0; i < sizeof(led_pins); i++) {
    digitalWrite(led_pins[i], HIGH);
    delay(70);
  }
  for (int i = sizeof(led_pins) - 1; i >= 0; i--) {
    digitalWrite(led_pins[i], LOW);
    delay(70);
  }
}

void reconnect() {
  while (!client.connected()) {
    Serial.println("Attempting MQTT connection...");
    if (client.connect("ESP32Client", token, NULL)) {
      Serial.println("Connected to ThingsBoard MQTT");
    } else {
      Serial.print("Failed, rc=");
      Serial.print(client.state());
      Serial.println(" Retrying in 5 seconds...");
      delay(5000);
    }
  }
}