import serial
import json
import threading
import time
from pynput import keyboard

SYSTEM_ID = 2

try:
    ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.1) 
    print("XBee 地面站啟動")
    print("[1] START  [2] STOP  [3] GOTO  [Esc] 退出程式")
except Exception as e:
    print(f"無法開啟序列埠: {e}")
    exit()

GPS_TYPE_LABELS = {
    0: "no GPS",
    1: "no fix",
    2: "2D",
    3: "3D",
    4: "DGPS",
    5: "RTK float",
    6: "RTK fixed",
}

def format_uav_data(uav_data):
    system_id = uav_data.get("s", "?")
    mode = uav_data.get("m", "?")
    battery = uav_data.get("b", "?")
    position = uav_data.get("p", "?")

    parts = [
        f"[UAV] ID:{system_id}",
        f"Mode:{mode}",
        f"Bat:{battery}%",
        f"Pos:{position}",
    ]

    if "a" in uav_data:
        armed_text = "ARMED" if int(uav_data.get("a", 0)) == 1 else "DISARMED"
        parts.append(f"Arm:{armed_text}")

    ypr = uav_data.get("ypr")
    if isinstance(ypr, list) and len(ypr) >= 3:
        parts.append(f"Height:{uav_data.get('h', '?')}m")
        parts.append(f"Yaw:{ypr[0]}deg")
        parts.append(f"Pitch:{ypr[1]}deg")
        parts.append(f"Roll:{ypr[2]}deg")
    elif "y" in uav_data:
        parts.append(f"Height:{uav_data.get('h', '?')}m")
        parts.append(f"Yaw:{uav_data.get('y')}deg")
    elif "h" in uav_data:
        parts.append(f"Heading:{uav_data.get('h')}")

    if "v" in uav_data:
        parts.append(f"Vel:{uav_data.get('v')}m/s")

    if "g" in uav_data:
        gps_type = uav_data.get("g")
        parts.append(f"GPS:{gps_type}({GPS_TYPE_LABELS.get(gps_type, 'unknown')})")

    dop = uav_data.get("d")
    if isinstance(dop, list) and len(dop) >= 2:
        parts.append(f"DOP(H/V):{dop[0]}/{dop[1]}")

    return " | ".join(parts)

def receive_thread():
    while True:
        try:
            if ser.in_waiting > 0:
                line = ser.readline().decode('utf-8', errors='ignore').strip()
                if line:
                    try:
                        data = json.loads(line)
                        # 判斷是確認訊息（ack）還是 UAV 狀態
                        if "result" in data and "command" in data and "message" in data:
                            # 這是確認訊息
                            result = data.get("result")
                            cmd = data.get("command")
                            msg = data.get("message")
                            print(f"\n[確認] 命令:{cmd} 狀態:{result} 訊息:{msg}", flush=True)
                        else:
                            # 這是 UAV 狀態訊息
                            print(format_uav_data(data), flush=True)
                    except json.JSONDecodeError:
                        pass 
        except Exception as e:
            print(f"\n[接收錯誤] {e}")
        time.sleep(0.01)

t = threading.Thread(target=receive_thread, daemon=True)
t.start()

def send_command(command, position=None):
    try:
        if command not in {"START", "STOP", "GOTO"}:
            raise ValueError("指令必須為大寫 START、STOP 或 GOTO")
        cmd = {
            "s": SYSTEM_ID,
            "c": command
        }
        if position is not None:
            cmd["p"] = position

        payload = json.dumps(cmd, separators=(',', ':')) + "\n"
        ser.write(payload.encode('utf-8'))
        print(f"\n>> [指令已發送] {payload.strip()}")
    except Exception as e:
        print(f"\n發送失敗: {e}")

def on_press(key):
    try:
        if key.char == "1":
            send_command("START")
        elif key.char == "2":
            send_command("STOP")
        elif key.char == "3":
            send_command("GOTO", [24.1199660, 120.677230])
    except AttributeError:
        if key == keyboard.Key.esc:
            print("\n程式結束")
            return False

with keyboard.Listener(on_press=on_press) as listener:
    listener.join()