import requests
import time
import math

# Konfigurasi
API_KEY = "sk-iot-f10e8fcc7ca57b68aa273334e9a4abc5"  # <-- Wajib diisi dengan API Key IoT dari Xieqa web
DEVICE_ID = "esp32_car"
BASE_URL = "https://xieqa.com/xieqa/api/iot_map.php"
SPEED = 0.0003  # Kecepatan virtual mobil (perubahan derajat lat/long per detik, ~33m/detik)

current_location = {"lat": -6.200000, "lng": 106.816666} # Posisi awal (default)

def get_route():
    try:
        url = f"{BASE_URL}?k={API_KEY}&d={DEVICE_ID}&action=get_route"
        response = requests.get(url)
        data = response.json()
        if 'route' in data and len(data['route']) > 0:
            return data['route']
    except Exception as e:
        pass
    return []

def get_current_location_from_server():
    try:
        url = f"{BASE_URL}?k={API_KEY}&d={DEVICE_ID}&action=get_location"
        response = requests.get(url)
        data = response.json()
        if 'location' in data:
            loc = data['location']
            if loc.get('lat') != 0 and loc.get('lng') != 0:
                if -90 <= loc.get('lat') <= 90:
                    return loc
    except Exception as e:
        pass
    return None

current_heading = 0.0 # Arah hadap awal (0 = Utara)

def get_bearing(lat1, lng1, lat2, lng2):
    d_lng = lng2 - lng1
    d_lat = lat2 - lat1
    angle = math.degrees(math.atan2(d_lng, d_lat))
    return (angle + 360) % 360

def send_location(lat, lng, status_text="Berhenti"):
    try:
        url = f"{BASE_URL}?k={API_KEY}&d={DEVICE_ID}&action=set_location"
        payload = {"lat": lat, "lng": lng}
        requests.post(url, json=payload)
        print(f"🚕 {status_text} | Posisi: Lat {lat:.6f}, Lng {lng:.6f}")
    except Exception as e:
        print("Error mengirim lokasi:", e)

def move_towards(target_lat, target_lng):
    global current_location, current_heading
    
    d_lat = target_lat - current_location['lat']
    d_lng = target_lng - current_location['lng']
    distance = math.sqrt(d_lat**2 + d_lng**2)
    
    if distance < SPEED:
        current_location['lat'] = target_lat
        current_location['lng'] = target_lng
        return True, "✅ Sampai di Titik Target"
    else:
        # Kalkulasi arah
        target_bearing = get_bearing(current_location['lat'], current_location['lng'], target_lat, target_lng)
        diff = (target_bearing - current_heading + 360) % 360
        
        status_text = "⬆️ Bergerak Maju"
        if 10 < diff < 180:
            status_text = "➡️ Belok Kanan"
        elif 180 <= diff < 350:
            status_text = "⬅️ Belok Kiri"
            
        current_heading = target_bearing # Update arah hadap
        
        ratio = SPEED / distance
        current_location['lat'] += d_lat * ratio
        current_location['lng'] += d_lng * ratio
        return False, status_text

print("="*40)
print("     XIEQA ESP32 AUTONOMOUS SIMULATOR")
print("="*40)
if API_KEY == "MASUKKAN_API_KEY_ANDA_DISINI":
    print("⚠️  PENTING: Anda harus memasukkan API_KEY asli di dalam script ini (baris 6).")
else:
    print(f"Memulai koneksi ke server... (Device: {DEVICE_ID})")
    server_loc = get_current_location_from_server()
    if server_loc:
        current_location['lat'] = server_loc['lat']
        current_location['lng'] = server_loc['lng']
        print(f"[+] Lokasi awal disinkronkan dari server: Lat {current_location['lat']:.6f}, Lng {current_location['lng']:.6f}")

waypoints = []
current_target_index = 0

while True:
    if API_KEY == "MASUKKAN_API_KEY_ANDA_DISINI":
        time.sleep(5)
        continue

    if current_target_index >= len(waypoints):
        # Minta rute baru dari server
        new_route = get_route()
        if new_route and new_route != waypoints:
            print(f"\\n[+] RUTE BARU DITERIMA! Jumlah titik target: {len(new_route)}")
            waypoints = new_route
            current_target_index = 0
            
            # Teleport mobil ke titik awal rute yang baru digambar
            if len(waypoints) > 0:
                 target0 = waypoints[0]
                 is_canvas = 'x_cm' in target0
                 
                 current_location['lat'] = target0['x_cm'] if is_canvas else target0['lat']
                 current_location['lng'] = target0['y_cm'] if is_canvas else target0['lng']
                 print("[+] Mobil disetel ulang ke titik awal rute.")
                 send_location(current_location['lat'], current_location['lng'], "Setel Ulang")
                 
                 # Hitung langkah mikro ke titik kedua agar UI bisa kalkulasi arah hadap awal
                 if len(waypoints) > 1:
                     target1 = waypoints[1]
                     t1_lat = target1['x_cm'] if is_canvas else target1['lat']
                     t1_lng = target1['y_cm'] if is_canvas else target1['lng']
                     
                     d_lat = t1_lat - current_location['lat']
                     d_lng = t1_lng - current_location['lng']
                     
                     # Bergerak super kecil (0.1%) menuju target untuk memancing UI menghitung sudut
                     current_location['lat'] += d_lat * 0.001
                     current_location['lng'] += d_lng * 0.001
                     
                     print("[+] Memutar kemudi menghadap titik tujuan...")
                     send_location(current_location['lat'], current_location['lng'], "Memutar Kemudi")
                     
                     # Beri waktu UI (CSS transition 1 detik) untuk memutar mobil secara visual sebelum melaju
                     time.sleep(1.5)
                     
                 # Langsung pindah ke target index 1 karena kita sudah di titik 0
                 current_target_index = 1
        else:
            # Jika rute habis atau kosong, periksa apakah ada update lokasi manual dari server/user
            server_loc = get_current_location_from_server()
            if server_loc:
                if abs(server_loc['lat'] - current_location['lat']) > 1e-7 or abs(server_loc['lng'] - current_location['lng']) > 1e-7:
                    current_location['lat'] = server_loc['lat']
                    current_location['lng'] = server_loc['lng']
                    print(f"[+] Lokasi mobil disinkronkan dari server: Lat {current_location['lat']:.6f}, Lng {current_location['lng']:.6f}")
            
            # Melapor posisi
            send_location(current_location['lat'], current_location['lng'])
            time.sleep(2)
            continue
            
    # Bergerak jika masih ada rute
    if current_target_index < len(waypoints):
        target = waypoints[current_target_index]
        
        # Deteksi Mode: Canvas (cm) atau GPS (lat/lng)
        is_canvas = 'x_cm' in target
        target_lat = target['x_cm'] if is_canvas else target['lat']
        target_lng = target['y_cm'] if is_canvas else target['lng']
        
        # Sesuaikan Kecepatan: 50 cm per detik untuk kanvas, 0.0003 derajat per detik untuk GPS
        current_speed = 50.0 if is_canvas else 0.0003

        d_lat = target_lat - current_location['lat']
        d_lng = target_lng - current_location['lng']
        distance = math.sqrt(d_lat**2 + d_lng**2)
        
        if distance < current_speed:
            current_location['lat'] = target_lat
            current_location['lng'] = target_lng
            send_location(current_location['lat'], current_location['lng'], "✅ Sampai di Titik Target")
            print(f"✅ Titik {current_target_index+1} dari {len(waypoints)} Tercapai!")
            current_target_index += 1
        else:
            target_bearing = get_bearing(current_location['lat'], current_location['lng'], target_lat, target_lng)
            diff = (target_bearing - current_heading + 360) % 360
            
            status_text = "⬆️ Bergerak Maju"
            if 10 < diff < 180:
                status_text = "➡️ Belok Kanan"
            elif 180 <= diff < 350:
                status_text = "⬅️ Belok Kiri"
                
            current_heading = target_bearing
            
            ratio = current_speed / distance
            current_location['lat'] += d_lat * ratio
            current_location['lng'] += d_lng * ratio
            
            # Format unit khusus log console
            unit = "cm" if is_canvas else "deg"
            status_print = f"[{unit}] {status_text}"
            send_location(current_location['lat'], current_location['lng'], status_print)
            
    time.sleep(1) # Tunda 1 detik untuk mensimulasikan waktu nyata
