import serial
import json
import math
import time

PORT = '/dev/ttyUSB0'  # 替換為實際的序列埠名稱
BAUD_RATE = 115200
SYSTEM_ID = 2

# 可切換為 "essential" 或 "full"；預設發送完整狀態。
STATUS_FORMAT = "full"

try:
    ser = serial.Serial(PORT, BAUD_RATE, timeout=0.1)
    print("XBee 無人機啟動...")
except Exception as e:
    print(f"無法開啟序列埠: {e}")
    exit()

def get_uav_status(status_format=None):
    status_format = status_format or STATUS_FORMAT
    essential_status = {
        "s": SYSTEM_ID,                 # system ID
        "m": "GUIDED",                  # mode
        "b": 78,                        # battery (%)
        "p": [24.1199660, 120.677230],  # [lat, lon] (deg)
        "y": 178.5,                     # yaw (deg)
    }
    full_status = {
        "s": SYSTEM_ID,                 # system ID
        "m": "GUIDED",                  # mode
        "a": 1,                         # 1=armed, 0=disarmed
        "b": 78,                        # battery (%)
        "h": 12.3,                      # height (m)
        "v": 4.2,                       # velocity (m/s)
        "p": [24.1199660, 120.677230],  # position: [lat, lon] (deg)
        "ypr": [178.5, -0.1, 0.0],      # yaw, pitch, roll (deg)
        "g": 3,                         # GPS type {0: no GPS, 1: no fix, 2: 2D, 3: 3D, 4: DGPS, 5: RTK float, 6: RTK fixed}
        "d": [0.8, 1.2],                # DOP: [hdop, vdop]
    }
    if status_format == "essential":
        return essential_status
    if status_format == "full":
        return full_status
    raise ValueError(
        f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")

def handle_command(cmd_obj):
    # 統一指令格式: {"s": <int>, "c": "START|STOP|GOTO"}
    target_s = cmd_obj.get("s")
    command = cmd_obj.get("c")
    ack = None

    if (isinstance(target_s, bool)
            or not isinstance(target_s, int)
            or target_s != SYSTEM_ID):
        ack = {
            "result": "ERROR",
            "command": command,
            "message": (
                f"指令目標 system_id={target_s} 不符，"
                f"本 UAV system_id={SYSTEM_ID}"
            ),
        }
    elif not isinstance(command, str) or command not in {"START", "STOP", "GOTO"}:
        print(f"\n[接收指令] 未知或格式錯誤的指令: {command}")
        ack = {
            "result": "ERROR",
            "command": command,
            "message": "指令 c 必須為大寫 START、STOP 或 GOTO",
        }
    elif command == "START":
        print("\n[接收指令] 開始任務")
        ack = {"result": "SUCCESS", "command": "START", "message": "成功"}
    elif command == "STOP":
        print("\n[接收指令] 停止任務")
        ack = {"result": "SUCCESS", "command": "STOP", "message": "成功"}
    elif command == "GOTO":
        # 優先使用 XBee 格式 p=[lat, lon]，並兼容舊版 lat/lon 欄位。
        position = cmd_obj.get("p")
        if isinstance(position, (list, tuple)) and len(position) >= 2:
            lat, lon = position[0], position[1]
        else:
            lat, lon = cmd_obj.get("lat"), cmd_obj.get("lon")

        try:
            if isinstance(lat, bool) or isinstance(lon, bool):
                raise ValueError
            lat = float(lat)
            lon = float(lon)
            if not math.isfinite(lat) or not math.isfinite(lon):
                raise ValueError
            if not -90.0 <= lat <= 90.0 or not -180.0 <= lon <= 180.0:
                raise ValueError
        except (TypeError, ValueError):
            print("\n[接收指令] GOTO 目標無效，需要有效的 p=[lat,lon]")
            ack = {
                "result": "ERROR",
                "command": "GOTO",
                "message": "GOTO 指令缺少或包含無效的 p=[lat,lon]",
            }
        else:
            target = [lat, lon]
            print(f"\n[接收指令] 導航任務 目標位置: {target}")
            ack = {
                "result": "SUCCESS",
                "command": "GOTO",
                "p": target,
                "message": "成功",
            }
    # 發送回覆（ack）到 GCS via serial
    if ack is not None:
        try:
            payload = json.dumps(ack, ensure_ascii=False) + "\n"
            ser.write(payload.encode('utf-8'))
            print(f"已發送回覆: {payload.strip()}")
        except Exception as e:
            print(f"發送回覆失敗: {e}")

try:
    while True:
        data = get_uav_status()
        json_payload = json.dumps(data, separators=(',', ':')) + "\n"
        ser.write(json_payload.encode('utf-8'))
        print(f"發送狀態: {json_payload.strip()}")
        for _ in range(10):
            if ser.in_waiting > 0:
                try:
                    cmd_line = ser.readline().decode('utf-8').strip()
                    if cmd_line:
                        cmd_obj = json.loads(cmd_line)
                        handle_command(cmd_obj)

                except json.JSONDecodeError:
                    pass 
            time.sleep(0.1)

except KeyboardInterrupt:
    print("發送停止")
finally:
    ser.close()