#!/usr/bin/env python3
"""
Run Gazebo Bridge - Runs Gazebo bridge and controllers together.
Connects to Gazebo simulation running in another terminal.

Usage: python run_gazebo.py [--pattern PATTERN] [--speed SPEED]
"""

import argparse
import signal
import sys

import rclpy
from rclpy.executors import MultiThreadedExecutor

from gazebo_bridge import GazeboBridge
from drone_controller import DroneController
from rover_controller import RoverController, MovementPattern


def parse_args():
    parser = argparse.ArgumentParser(
        description='Run Gazebo bridge and controllers together',
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Examples:
  python run_gazebo.py                           # Stationary rover
  python run_gazebo.py --pattern circular        # Circular movement
  python run_gazebo.py --pattern square --speed 0.3
        """
    )
    parser.add_argument(
        '--pattern', type=str, default='stationary',
        choices=['stationary', 'linear', 'circular', 'random', 'square'],
        help='Rover movement pattern (default: stationary)'
    )
    parser.add_argument(
        '--speed', '-s', type=float, default=0.5,
        help='Rover speed in m/s (default: 0.5)'
    )
    parser.add_argument(
        '--amplitude', '-a', type=float, default=2.0,
        help='Rover movement amplitude in meters (default: 2.0)'
    )
    parser.add_argument(
        '--no-rover', action='store_true',
        help='Disable rover controller'
    )
    args, _ = parser.parse_known_args()
    return args


def main():
    args = parse_args()
    
    print("=" * 60)
    print("  Gazebo Bridge + Controllers")
    print("=" * 60)
    print(f"  Rover Pattern: {args.pattern}")
    print(f"  Rover Speed: {args.speed} m/s")
    print("=" * 60)
    print()
    
    rclpy.init()
    
    nodes = []
    executor = MultiThreadedExecutor(num_threads=4)
    
    try:
        # Create Gazebo bridge
        bridge = GazeboBridge()
        nodes.append(bridge)
        executor.add_node(bridge)
        print("[OK] Gazebo Bridge started")
        
        # Create drone controller
        drone = DroneController()
        nodes.append(drone)
        executor.add_node(drone)
        print("[OK] Drone Controller started")
        
        # Create rover controller (optional)
        if not args.no_rover:
            pattern = MovementPattern[args.pattern.upper()]
            rover = RoverController(
                pattern=pattern,
                speed=args.speed,
                amplitude=args.amplitude
            )
            nodes.append(rover)
            executor.add_node(rover)
            print("[OK] Rover Controller started")
        
        print()
        print("All systems running. Press Ctrl+C to stop.")
        print()
        
        # Handle Ctrl+C gracefully
        def signal_handler(sig, frame):
            print("\nShutting down...")
            executor.shutdown()
        
        signal.signal(signal.SIGINT, signal_handler)
        
        executor.spin()
        
    except Exception as e:
        print(f"Error: {e}")
        raise
    finally:
        print("Cleaning up...")
        for node in nodes:
            node.destroy_node()
        
        if rclpy.ok():
            rclpy.shutdown()
        
        print("Shutdown complete.")


if __name__ == '__main__':
    main()