import asyncio
import websockets
import json
import sys

WS_URL = "ws://127.0.0.1:8765"
SYSTEM_ID = 2
GOTO_POSITION = [24.1234567, 120.1234567]


class WebSocketClient:
    def __init__(self, ws_url=WS_URL, system_id=SYSTEM_ID):
        self.ws_url = ws_url
        if isinstance(system_id, bool) or not isinstance(system_id, int):
            raise ValueError("system_id 必須為整數")
        self.system_id = system_id
        
    def process_websocket_message(self, data):
        """解析 server 的 Essential／Full UAV 狀態或命令 ACK。"""
        if isinstance(data, str):
            try:
                data = json.loads(data)
            except json.JSONDecodeError as e:
                print(f"JSON 格式錯誤: {e}")
                return

        if not isinstance(data, dict):
            print(f"未知訊息格式: {data}")
            return

        if all(key in data for key in ("result", "command", "message")):
            print(
                f"ACK | Command:{data.get('command', '-')} | "
                f"Result:{data.get('result', '-')} | "
                f"Message:{data.get('message', '-')}"
            )
            return

        if "s" not in data:
            print(f"未知 JSON 訊息: {data}")
            return

        position = data.get("p")
        if isinstance(position, (list, tuple)) and len(position) >= 2:
            position_text = f"{position[0]}, {position[1]}"
        else:
            position_text = "-"

        ypr = data.get("ypr")
        if isinstance(ypr, (list, tuple)) and len(ypr) >= 3:
            yaw, pitch, roll = ypr[0], ypr[1], ypr[2]
        else:
            yaw, pitch, roll = data.get("y"), None, None

        dop = data.get("d")
        if isinstance(dop, (list, tuple)) and len(dop) >= 2:
            dop_text = f"{dop[0]}/{dop[1]}"
        else:
            dop_text = "-"

        armed = data.get("a")
        if armed is None:
            armed_text = "-"
        else:
            armed_text = "ARMED" if bool(armed) else "DISARMED"

        def value_or_dash(value, suffix=""):
            return f"{value}{suffix}" if value is not None else "-"

        print(
            f"UAV | ID:{data.get('s', '-')} | "
            f"Mode:{value_or_dash(data.get('m'))} | "
            f"Arm:{armed_text} | "
            f"Battery:{value_or_dash(data.get('b'), '%')} | "
            f"Position:{position_text} | "
            f"Height:{value_or_dash(data.get('h'), 'm')} | "
            f"Velocity:{value_or_dash(data.get('v'), 'm/s')} | "
            f"Yaw:{value_or_dash(yaw, 'deg')} | "
            f"Pitch:{value_or_dash(pitch, 'deg')} | "
            f"Roll:{value_or_dash(roll, 'deg')} | "
            f"GPS:{value_or_dash(data.get('g'))} | DOP(H/V):{dop_text}"
        )

    async def send_commands_loop(self, websocket):
        """從 stdin 讀取按鍵 (輸入後按 Enter) 並發送對應的 JSON 指令"""
        loop = asyncio.get_event_loop()
        print("輸入 1 = START, 2 = STOP, 3 = GOTO (然後按 Enter)")
        while True:
            # readline 會包含換行
            try:
                line = await loop.run_in_executor(None, sys.stdin.readline)
            except Exception:
                return
            if not line:
                # EOF
                return
            cmd = line.strip()
            if cmd == '1':
                msg = {"s": self.system_id, "c": "START"}
            elif cmd == '2':
                msg = {"s": self.system_id, "c": "STOP"}
            elif cmd == '3':
                msg = {
                    "s": self.system_id,
                    "c": "GOTO",
                    "p": list(GOTO_POSITION),
                }
            else:
                print(f"未知指令: {cmd}")
                continue

            try:
                await websocket.send(json.dumps(msg))
                print(f"已發送: {msg}")
            except Exception as e:
                print(f"發送失敗: {e}")
                return

    async def ws_client_loop(self):
        retry_count = 0
        max_retries = 5
        base_delay = 1.0
        while retry_count < max_retries:
            try:
                async with websockets.connect(self.ws_url) as websocket:
                    print(f"WebSocket connected: {self.ws_url}")
                    retry_count = 0
                    # 啟動後台任務來接收 stdin 指令並發送
                    send_task = asyncio.create_task(self.send_commands_loop(websocket))
                    try:
                        async for message in websocket:
                            # message 已經是 string
                            self.process_websocket_message(message)
                    finally:
                        send_task.cancel()
                        await asyncio.gather(send_task, return_exceptions=True)
                            
            except websockets.exceptions.ConnectionClosedError:
                print("WebSocket connection closed")
                break
            except Exception as e:
                retry_count += 1
                delay = base_delay * (2 ** min(retry_count, 4))
                print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
                await asyncio.sleep(delay)
        
        print("WebSocket client stopped after maximum retries")

    async def start(self):
        """啟動 WebSocket 客戶端"""
        await self.ws_client_loop()

if __name__ == "__main__":
    client = WebSocketClient()
    asyncio.run(client.start())