#!/usr/bin/env python3
"""
LLUAV SDK Demo: Automated Flight Mission

Demonstrates a complete flight workflow:
  1. Connect & login
  2. Preflight check
  3. Set task name
  4. Takeoff
  5. Send setpoint (NED)
  6. Execute a mission plan
  7. RTL (Return to Launch)
  8. Disconnect

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

import argparse
import json
import sys
import time

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


def main():
    parser = argparse.ArgumentParser(description="LLUAV Automated Flight Mission Demo")
    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("--sp-x", type=float, default=5.0, help="Setpoint X (m)")
    parser.add_argument("--sp-y", type=float, default=0.0, help="Setpoint Y (m)")
    parser.add_argument("--sp-z", type=float, default=-5.0, help="Setpoint Z (m, negative=up)")
    parser.add_argument("--sp-yaw", type=float, default=0.0, help="Setpoint Yaw (rad)")
    parser.add_argument("--plan", default="", help="Plan string (optional)")
    args = parser.parse_args()

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

    try:
        print("=" * 50)
        print("LLUAV Flight Mission Demo")
        print("=" * 50)

        # 1. Connect
        print("\n[1/8] Connecting...")
        client.connect()
        print(f"      Connected to {client.url}")

        # 2. Preflight check
        print("\n[2/8] Running preflight check...")
        try:
            preflight = client.preflight_check()
            ret = preflight.get("preflight_check", -1)
            msg = preflight.get("preflight_check_msg", "Unknown")
            print(f"      Result: {msg} (code={ret})")
            if ret != 0:
                print("      WARNING: Preflight check failed! Proceed with caution.")
        except LLUAVCommandError as e:
            print(f"      Preflight check error: {e}")

        # 3. Set task name
        print("\n[3/8] Setting task name...")
        task_name = f"demo_mission_{int(time.time())}"
        client.set_taskname(task_name)
        print(f"      Task name set to: {task_name}")

        # Show initial state
        print("\n[4/8] Getting initial state...")
        state = client.get_state()
        st = state.get("state", {})
        print(f"      Armed: {st.get('armed', '?')}  CtrlState: {st.get('ctrl_state', '?')}")

        # 5. Takeoff
        print("\n[5/8] Commanding TAKEOFF...")
        print("      >>> Press Enter to confirm takeoff, or Ctrl+C to abort <<<")
        try:
            input()
        except KeyboardInterrupt:
            print("\nAborted.")
            client.disconnect()
            return

        resp = client.takeoff()
        print(f"      Takeoff acknowledged: ret={resp.get('ret', '?')}")
        print("      Waiting 5 seconds for climb...")
        time.sleep(5)

        # 6. Set setpoint
        print(f"\n[6/8] Setting setpoint NED: x={args.sp_x}, y={args.sp_y}, z={args.sp_z}, yaw={args.sp_yaw}")
        resp = client.set_sp_ned(args.sp_x, args.sp_y, args.sp_z, args.sp_yaw)
        print(f"      Setpoint acknowledged: ret={resp.get('ret', '?')}")
        print("      Waiting 5 seconds...")
        time.sleep(5)

        # 7. Execute plan (if provided)
        if args.plan:
            print(f"\n[7/8] Executing plan: {args.plan}")
            resp = client.start_plan(args.plan)
            print(f"      Plan acknowledged: ret={resp.get('ret', '?')}")
        else:
            print("\n[7/8] No plan provided, skipping plan execution.")

        # 8. RTL
        print("\n[8/8] Commanding RTL...")
        print("      >>> Press Enter to confirm RTL <<<")
        try:
            input()
        except KeyboardInterrupt:
            pass

        resp = client.rtl()
        print(f"      RTL acknowledged: ret={resp.get('ret', '?')}")

        print("\n" + "=" * 50)
        print("Mission demo completed.")
        print("=" * 50)

    except LLUAVCommandError as e:
        print(f"\nCommand failed: {e} (ret={e.ret_code})")
        if e.response:
            print(f"Response: {json.dumps(e.response, indent=2)}")
    except Exception as e:
        print(f"\nError: {e}")
    finally:
        client.disconnect()
        print("Disconnected.")


if __name__ == "__main__":
    main()
