#!/usr/bin/env python3
"""
LLUAV SDK Demo: Camera & Map Management

Demonstrates:
  - Capture single photo
  - SIYI gimbal control (center, pitch)
  - Toggle recording
  - List saved maps
  - Save current map
  - Set map localization path

Usage:
    python demo_camera_map.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 Camera & Map Management 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("--map-name", default="", help="Map name to save (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 Camera & Map Management Demo")
        print("=" * 50)

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

        # 1. List existing maps
        print("\n[2/7] Listing saved maps...")
        try:
            maps = client.list_maps()
            map_list = maps.get("maps", [])
            print(f"      Found {len(map_list)} map(s):")
            for m in map_list:
                print(f"        - {m}")
        except LLUAVCommandError as e:
            print(f"      Error listing maps: {e}")

        # 2. Capture photo
        print("\n[3/7] Capturing photo...")
        try:
            resp = client.capture()
            print(f"      Capture OK: ret={resp.get('ret', '?')}")
        except LLUAVCommandError as e:
            print(f"      Capture error: {e}")

        # 3. SIYI center
        print("\n[4/7] Centering SIYI gimbal...")
        try:
            resp = client.siyi_center()
            print(f"      Center OK: ret={resp.get('ret', '?')}")
        except LLUAVCommandError as e:
            print(f"      SIYI center error: {e}")

        # 4. SIYI pitch
        print("\n[5/7] Setting SIYI pitch to -30 degrees...")
        try:
            resp = client.siyi_downvideo(0)  # Just an example; use ara8_pitch if ARA8
            print(f"      SIYI command OK: ret={resp.get('ret', '?')}")
        except LLUAVCommandError as e:
            print(f"      SIYI error: {e}")

        # 5. Toggle recording
        print("\n[6/7] Toggling video recording...")
        try:
            resp = client.record_toggle()
            print(f"      Record toggle OK: ret={resp.get('ret', '?')}")
        except LLUAVCommandError as e:
            print(f"      Record error: {e}")

        # 6. Save map
        if args.map_name:
            print(f"\n[7/7] Saving current map to '{args.map_name}'...")
            try:
                resp = client.save_map(args.map_name)
                print(f"      Save map OK: ret={resp.get('ret', '?')}")
                if "path" in resp:
                    print(f"      Path: {resp['path']}")
            except LLUAVCommandError as e:
                print(f"      Save map error: {e}")
        else:
            print("\n[7/7] Skipping map save (use --map-name to specify)")

        print("\n" + "=" * 50)
        print("Camera & Map 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()
