#!/usr/bin/env python3
"""AI Teammate ROS2 Bridge CLI"""

import argparse
import os
import sys
from pathlib import Path


def cmd_init(args):
    """Initialize .env configuration."""
    env_path = Path(args.dir) / ".env"
    os.makedirs(args.dir, exist_ok=True)

    lines = [
        f"DEVICE_ID={args.device_id}",
        f"GATEWAY_URL={args.gateway}",
        f"API_KEY={args.api_key}",
        f"CONNECTION_MODE={args.mode}",
        "STATUS_REPORT_INTERVAL=1.0",
        f"CMD_VEL_TOPIC={args.cmd_vel_topic}",
        f"IMU_TOPIC={args.imu_topic}",
        f"ODOM_TOPIC={args.odom_topic}",
        f"SCAN_TOPIC={args.scan_topic}",
    ]

    if env_path.exists() and not args.force:
        print(f".env already exists at {env_path}")
        print("Use --force to overwrite")
        sys.exit(1)

    env_path.write_text("\n".join(lines) + "\n")
    print(f"Config written to {env_path}")
    print(f"  DEVICE_ID={args.device_id}")
    print(f"  GATEWAY_URL={args.gateway}")
    print(f"  MODE={args.mode}")
    print()
    print(f"Start bridge:")
    print(f"  cd {args.dir} && ai-teammate-bridge")


def cmd_run(args):
    """Run the bridge."""
    from ai_teammate_ros2_bridge.config import DEVICE_ID, GATEWAY_URL, API_KEY

    if not DEVICE_ID or not GATEWAY_URL or not API_KEY:
        print("Error: DEVICE_ID, GATEWAY_URL, API_KEY must be set.")
        print("Run: ai-teammate-bridge init --device-id=xxx --api-key=dk_xxx")
        sys.exit(1)

    from ai_teammate_ros2_bridge import DeviceBridge
    bridge = DeviceBridge.from_env()
    bridge.run()


def cmd_version(args):
    """Print version."""
    from ai_teammate_ros2_bridge import __version__
    print(f"ai-teammate-ros2-bridge v{__version__}")


def cmd_status(args):
    """Check current config."""
    from ai_teammate_ros2_bridge.config import DEVICE_ID, GATEWAY_URL, API_KEY, MODE, ROTATION_SCALE, DISTANCE_SCALE
    print(f"DEVICE_ID:      {DEVICE_ID or '(not set)'}")
    print(f"GATEWAY_URL:    {GATEWAY_URL or '(not set)'}")
    print(f"API_KEY:        {API_KEY[:10] + '...' if API_KEY else '(not set)'}")
    print(f"MODE:           {MODE}")
    print(f"ROTATION_SCALE: {ROTATION_SCALE}")
    print(f"DISTANCE_SCALE: {DISTANCE_SCALE}")


def main():
    parser = argparse.ArgumentParser(
        prog="ai-teammate-bridge",
        description="AI Teammate ROS2 Device Bridge",
    )
    sub = parser.add_subparsers(dest="command")

    # init
    p_init = sub.add_parser("init", help="Initialize .env configuration")
    p_init.add_argument("--device-id", required=True, help="Device ID (e.g. my-robot-01)")
    p_init.add_argument("--api-key", required=True, help="API key (dk_xxx)")
    p_init.add_argument("--gateway", default="wss://ai-teammate.net/gw", help="Gateway WebSocket URL")
    p_init.add_argument("--mode", default="rclpy", choices=["rclpy", "simulator"], help="Connection mode")
    p_init.add_argument("--cmd-vel-topic", default="/cmd_vel", help="cmd_vel topic")
    p_init.add_argument("--odom-topic", default="/odom", help="Odometry topic (e.g. /base_controller/odom)")
    p_init.add_argument("--imu-topic", default="/imu/data", help="IMU topic")
    p_init.add_argument("--scan-topic", default="/scan", help="LiDAR scan topic")
    p_init.add_argument("--dir", default=os.path.expanduser("~/ai-teammate-bridge"), help="Config directory")
    p_init.add_argument("--force", action="store_true", help="Overwrite existing .env")

    # version
    sub.add_parser("version", help="Print version")

    # status
    sub.add_parser("status", help="Show current configuration")

    args = parser.parse_args()

    if args.command == "init":
        cmd_init(args)
    elif args.command == "version":
        cmd_version(args)
    elif args.command == "status":
        cmd_status(args)
    else:
        # Default: run bridge (backward compatible)
        cmd_run(args)


if __name__ == '__main__':
    main()
