#include <WiFi.h>
#include <HTTPClient.h>
#include <ArduinoJson.h>
// #include <TinyGPS++.h> // Uncomment jika menggunakan modul GPS Asli (Neo-6M dll)

// Konfigurasi WiFi
const char* ssid = "WIFI_SSID_ANDA";
const char* password = "WIFI_PASSWORD_ANDA";

// Konfigurasi API Xieqa
const char* API_KEY = "MASUKKAN_API_KEY_ANDA_DISINI";
const char* DEVICE_ID = "esp32_car";
const char* BASE_URL = "http://IP_KOMPUTER_SERVER/xieqa/api/iot_map.php"; 

// Pin Motor Driver (Contoh L298N)
const int motorLeftFwd = 26;
const int motorLeftRev = 27;
const int motorRightFwd = 32;
const int motorRightRev = 33;

// Variabel Lokasi (Gunakan TinyGPS++ untuk mengubah nilai ini secara real-time)
float currentLat = -6.200000;
float currentLng = 106.816666;

// Variabel Target Waypoint
float targetLat = 0.0;
float targetLng = 0.0;
bool hasTarget = false;

unsigned long lastUpdate = 0;
const int UPDATE_INTERVAL = 2000; // Kirim/ambil data setiap 2 detik

void setup() {
  Serial.begin(115200);
  
  // Inisialisasi Pin Motor
  pinMode(motorLeftFwd, OUTPUT);
  pinMode(motorLeftRev, OUTPUT);
  pinMode(motorRightFwd, OUTPUT);
  pinMode(motorRightRev, OUTPUT);
  stopMotors();

  // Koneksi WiFi
  WiFi.begin(ssid, password);
  Serial.print("Menghubungkan ke WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\\nWiFi Terhubung!");
}

void loop() {
  // Update Data Modul GPS disini jika menggunakan hardware asli:
  // while (SerialGPS.available() > 0) { gps.encode(SerialGPS.read()); }
  // if (gps.location.isUpdated()) { currentLat = gps.location.lat(); currentLng = gps.location.lng(); }

  if (millis() - lastUpdate > UPDATE_INTERVAL) {
    lastUpdate = millis();
    
    if (WiFi.status() == WL_CONNECTED) {
      // 1. Laporkan Posisi Saat Ini
      sendLocation();
      
      // 2. Ambil Rute Jika Tidak Punya Target
      if (!hasTarget) {
        getRoute();
      }
    }
  }

  // 3. Logika Navigasi Fisik (Jika Punya Target)
  if (hasTarget) {
    navigateTowardsTarget();
  }
}

// ==========================================
// FUNGSI KOMUNIKASI API
// ==========================================

void sendLocation() {
  HTTPClient http;
  String url = String(BASE_URL) + "?k=" + API_KEY + "&d=" + DEVICE_ID + "&action=set_location";
  
  http.begin(url);
  http.addHeader("Content-Type", "application/json");

  // Format JSON menggunakan ArduinoJson
  StaticJsonDocument<200> doc;
  doc["lat"] = currentLat;
  doc["lng"] = currentLng;
  String requestBody;
  serializeJson(doc, requestBody);

  int httpResponseCode = http.POST(requestBody);
  if (httpResponseCode > 0) {
    Serial.println("Lokasi berhasil dikirim ke server.");
  } else {
    Serial.print("Error mengirim lokasi: ");
    Serial.println(httpResponseCode);
  }
  http.end();
}

void getRoute() {
  HTTPClient http;
  String url = String(BASE_URL) + "?k=" + API_KEY + "&d=" + DEVICE_ID + "&action=get_route";
  
  http.begin(url);
  int httpResponseCode = http.GET();
  
  if (httpResponseCode == 200) {
    String payload = http.getString();
    
    // Parsing JSON Rute
    DynamicJsonDocument doc(2048);
    DeserializationError error = deserializeJson(doc, payload);
    
    if (!error) {
      JsonArray routeArray = doc["route"];
      if (routeArray.size() > 0) {
        // Ambil waypoint pertama sebagai target
        targetLat = routeArray[0]["lat"];
        targetLng = routeArray[0]["lng"];
        hasTarget = true;
        Serial.print("Target baru diterima: ");
        Serial.print(targetLat, 6);
        Serial.print(", ");
        Serial.println(targetLng, 6);
      }
    }
  }
  http.end();
}

// ==========================================
// FUNGSI PENGENDALIAN KENDARAAN (MOBIL)
// ==========================================

void navigateTowardsTarget() {
  // Hitung selisih jarak
  float dLat = targetLat - currentLat;
  float dLng = targetLng - currentLng;
  
  // (Versi Sederhana) Kalkulasi jarak euclidean
  float distance = sqrt(dLat * dLat + dLng * dLng);
  
  // Jika jarak sangat dekat, anggap sampai target
  if (distance < 0.0001) {
    Serial.println("SAMPAI DI TARGET WAYPOINT!");
    stopMotors();
    hasTarget = false; // Minta rute baru di siklus berikutnya
    return;
  }

  // --- LOGIKA BERBELOK & MAJU ---
  // (Pada implementasi nyata, Anda butuh Kompas / Magnetometer HMC5883L 
  // untuk mengetahui 'Heading' / arah hadap mobil saat ini, lalu membandingkannya
  // dengan 'Bearing' ke titik target).
  
  // Karena ini adalah kerangka dasar, asumsikan mobil selalu menghadap utara:
  if (abs(dLng) > abs(dLat)) {
    if (dLng > 0) { turnRight(); } else { turnLeft(); }
  } else {
    if (dLat > 0) { moveForward(); } else { moveBackward(); }
  }
  
  delay(500); // Gerak 0.5 detik
  stopMotors(); 
  
  // Simulasi Update Posisi (Hapus baris ini jika menggunakan GPS Asli)
  currentLat += (dLat * 0.1); 
  currentLng += (dLng * 0.1);
}

void moveForward() {
  digitalWrite(motorLeftFwd, HIGH); digitalWrite(motorLeftRev, LOW);
  digitalWrite(motorRightFwd, HIGH); digitalWrite(motorRightRev, LOW);
}
void moveBackward() {
  digitalWrite(motorLeftFwd, LOW); digitalWrite(motorLeftRev, HIGH);
  digitalWrite(motorRightFwd, LOW); digitalWrite(motorRightRev, HIGH);
}
void turnLeft() {
  digitalWrite(motorLeftFwd, LOW); digitalWrite(motorLeftRev, HIGH);
  digitalWrite(motorRightFwd, HIGH); digitalWrite(motorRightRev, LOW);
}
void turnRight() {
  digitalWrite(motorLeftFwd, HIGH); digitalWrite(motorLeftRev, LOW);
  digitalWrite(motorRightFwd, LOW); digitalWrite(motorRightRev, HIGH);
}
void stopMotors() {
  digitalWrite(motorLeftFwd, LOW); digitalWrite(motorLeftRev, LOW);
  digitalWrite(motorRightFwd, LOW); digitalWrite(motorRightRev, LOW);
}
