#!/usr/bin/env python3
"""
Run Bridge - Runs ROS bridge and controllers together.
Connects to PyBullet simulator running on another machine or terminal.

Usage: python run_bridge.py [--host HOST] [--pattern PATTERN] [--speed SPEED]
"""

import argparse
import signal
import sys

import rclpy
from rclpy.executors import MultiThreadedExecutor

from ros_bridge import ROS2SimulatorBridge
from drone_controller import DroneController
from rover_controller import RoverController, MovementPattern


def parse_args():
    parser = argparse.ArgumentParser(
        description='Run ROS bridge and controllers together',
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Examples:
  python run_bridge.py                           # Connect to local simulator
  python run_bridge.py --host 192.168.1.100      # Connect to remote simulator
  python run_bridge.py --pattern circular        # With moving rover
        """
    )
    parser.add_argument(
        '--host', '-H', type=str, default='0.0.0.0',
        help='Simulator host IP (default: 0.0.0.0)'
    )
    parser.add_argument(
        '--port', '-p', type=int, default=5555,
        help='Simulator port (default: 5555)'
    )
    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("  ROS Bridge + Controllers")
    print("=" * 60)
    print(f"  Simulator: {args.host}:{args.port}")
    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 bridge
        bridge = ROS2SimulatorBridge(
            simulator_host=args.host,
            simulator_port=args.port
        )
        nodes.append(bridge)
        executor.add_node(bridge)
        print("[OK] ROS 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:
            if hasattr(node, 'shutdown'):
                node.shutdown()
            node.destroy_node()
        
        if rclpy.ok():
            rclpy.shutdown()
        
        print("Shutdown complete.")


if __name__ == '__main__':
    main()