#!/usr/bin/env python3
"""
LLUAV SDK Demo: Real-time State Monitor

Continuously monitors drone state, HUD, and Lidar pose using periodic tasks.
Demonstrates:
  - Auto-connect & auto-reconnect
  - Periodic task scheduler
  - Graceful shutdown with Ctrl+C

Usage:
    python demo_state_monitor.py --host 192.168.1.100 --password your_password
"""

import argparse
import json
import sys
import time
from datetime import datetime

sys.path.insert(0, "../")
from lluav_sdk import LLUAVClient, LLUAVCommandError


def format_pose(pose_json):
    lp = pose_json.get("lidar_pose", {})
    return (f"POS({lp.get('x', '?'):>7},{lp.get('y', '?'):>7},{lp.get('z', '?'):>7}) "
            f"YAW={lp.get('yaw', '?'):>6} ROLL={lp.get('roll', '?'):>6} PITCH={lp.get('pitch', '?'):>6}")


def format_hud(hud_json):
    pos = hud_json.get("uav_pos_enu", {})
    extra = hud_json.get("extra", {})
    return (f"HUD  ENU({pos.get('x', '?'):.2f},{pos.get('y', '?'):.2f},{pos.get('z', '?'):.2f}) "
            f"{extra.get('volume_est', 'N/A')}")


def main():
    parser = argparse.ArgumentParser(description="LLUAV Real-time State Monitor")
    parser.add_argument("--host", default="localhost", help="Drone IP")
    parser.add_argument("--port", type=int, default=53001, help="WebSocket port")
    parser.add_argument("--password", default=None, help="Login password")
    parser.add_argument("--interval", type=float, default=2.0, help="Polling interval (seconds)")
    args = parser.parse_args()

    client = LLUAVClient(
        host=args.host,
        port=args.port,
        password=args.password,
        auto_reconnect=True,
        heartbeat_interval=5.0,
    )

    results = {"state": None, "hud": None, "pose": None}

    def on_state(resp):
        results["state"] = resp

    def on_hud(resp):
        results["hud"] = resp

    def on_pose(resp):
        results["pose"] = resp

    print(f"[{datetime.now().strftime('%H:%M:%S')}] Connecting to {client.url} ...")
    client.connect()
    print(f"[{datetime.now().strftime('%H:%M:%S')}] Connected. Starting monitor (interval={args.interval}s).")
    print("Press Ctrl+C to stop.\n")

    # Use non-blocking send_cmd with callbacks via Future.add_done_callback
    def poll():
        try:
            client.send_cmd("state", block=False).add_done_callback(
                lambda f: on_state(f.result()) if not f.exception() else None)
            client.send_cmd("hud1", block=False).add_done_callback(
                lambda f: on_hud(f.result()) if not f.exception() else None)
            client.send_cmd("lidarpose", block=False).add_done_callback(
                lambda f: on_pose(f.result()) if not f.exception() else None)
        except Exception as e:
            print(f"Poll error: {e}")

    # Register periodic task for polling
    client.add_periodic_task("poll_all", args.interval, "state")
    # We'll manually poll hud and pose in the main loop for demonstration

    try:
        while True:
            time.sleep(args.interval)
            poll()

            ts = datetime.now().strftime("%H:%M:%S")
            if results["pose"]:
                print(f"[{ts}] POSE  {format_pose(results['pose'])}")
            if results["hud"]:
                print(f"[{ts}] {format_hud(results['hud'])}")
            if results["state"]:
                # Print only connection/armed status from state
                st = results["state"].get("state", {})
                armed = st.get("armed", "?")
                ctrl = st.get("ctrl_state", "?")
                batt = st.get("battery_v", "?")
                print(f"[{ts}] STATE armed={armed} ctrl={ctrl} batt={batt}V")
            print()

    except KeyboardInterrupt:
        print("\nStopping monitor...")
    finally:
        client.disconnect()
        print("Disconnected.")


if __name__ == "__main__":
    main()
