#include <Adafruit_MPU6050.h>
#include <PubSubClient.h>
#include <WiFi.h>
#include <ESP32Servo.h>
#define light_sensor 25
#define trig_hc 33
#define echo_hc 32
#define SOUND_SPEED 0.034

Adafruit_MPU6050 mpu;

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "broker.hivemq.com";
const int Motor_1 = 12;
const int Motor_2 = 26;
const float accelScale = 16384.0;
const float gyroScale = 131.0;
int pos = 0;
unsigned long lastTime = 0;
float dt, pitch, roll, yaw;

WiFiClient espClient;
PubSubClient client(espClient);
Servo servo1;
Servo servo2;

void callback(char* topic, byte* payload, unsigned int length) {
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    String clientId = "ESP32Client-";
    clientId += String(random(0xffff), HEX);
    if (client.connect(clientId.c_str())) {
      Serial.println("Connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void connectWifi() {
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Koneksi WiFi...");
  }
  Serial.println("Terhubung ke WiFi");
}

void connectMPU() {
  if (!mpu.begin()) {
    Serial.println("Sensor init failed");
    while (1)
      yield();
  }
}

float readDistance() {
  digitalWrite(trig_hc, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_hc, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_hc, LOW);
  int duration = pulseIn(echo_hc, HIGH);
  return duration * SOUND_SPEED / 2;
}

void setup() {
  Serial.begin(115200);
  pinMode(light_sensor, INPUT);
  pinMode(trig_hc, OUTPUT);
  pinMode(echo_hc, INPUT);
  servo1.attach(Motor_1, 500, 2400);
  servo2.attach(Motor_2, 500, 2400);
  connectWifi();
  connectMPU();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
}

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

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  String status;

  if (digitalRead(light_sensor) == HIGH) {
    // THIS SHOULD BE USED INTERRUPT, SINCE FOR PROTOTYPE TASK, NVM.
    for (pos = 0; pos <= 180; pos += 1) {
      servo1.write(pos);
      servo2.write(pos);
      delay(10);
    }
    for (pos = 180; pos >= 0; pos -= 1) {
      servo1.write(pos);
      servo2.write(pos);
      delay(10);
    }
    status = "ON";
    float distances = readDistance();
    float aX = a.acceleration.x / accelScale;
    float aY = a.acceleration.y / accelScale;
    float aZ = a.acceleration.z / accelScale;

    float gX = g.gyro.x / gyroScale;
    float gY = g.gyro.y / gyroScale;
    float gZ = g.gyro.z / gyroScale;

    // Calculate delta time
    unsigned long currentTime = millis();
    dt = (currentTime - lastTime) / 1000.0;
    lastTime = currentTime;

    // Complementary filter
    float alpha = 0.98;

    // Integrate gyroscope data
    pitch += gX * dt;
    roll += gY * dt;
    yaw += gZ * dt;

    // Use accelerometer data to correct the pitch and roll angles
    float pitchAcc = atan2(aY, aZ) * 180 / PI;
    float rollAcc = atan2(-aX, sqrt(aY * aY + aZ * aZ)) * 180 / PI;

    pitch = alpha * pitch + (1.0 - alpha) * pitchAcc;
    roll = alpha * roll + (1.0 - alpha) * rollAcc;
    client.publish("/thariq/agv/status", status.c_str());
    client.publish("/thariq/agv/distance", String(distances).c_str());
    client.publish("/thariq/agv/roll", String(roll).c_str());
    client.publish("/thariq/agv/pitch", String(pitch).c_str());
    client.publish("/thariq/agv/yaw", String(yaw).c_str());
  } else {
    status = "OFF";
    client.publish("/thariq/agv/status", status.c_str());
  }
  delay(20);
}