#!/usr/bin/env python3
"""
Controllers - Runs DroneController and RoverController together.
Usage: python controllers.py [--pattern PATTERN] [--speed SPEED] [--amplitude AMP]
"""

import argparse
import sys

import rclpy
from rclpy.executors import MultiThreadedExecutor

from drone_controller import DroneController
from rover_controller import RoverController, MovementPattern


def parse_args():
    parser = argparse.ArgumentParser(
        description='Run drone and rover controllers together',
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Rover Movement Patterns:
  stationary  - Rover stays at origin (easiest)
  linear      - Back and forth along X axis
  circular    - Circular path around origin
  random      - Random movement within bounds
  square      - Square pattern around origin

Examples:
  python controllers.py
  python controllers.py --pattern circular --speed 0.3
  python controllers.py --pattern random --speed 0.5 --amplitude 3.0
        """
    )
    parser.add_argument(
        '--pattern', '-p', type=str,
        choices=[p.value for p in MovementPattern],
        default='stationary',
        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)'
    )
    args, _ = parser.parse_known_args()
    return args


def main(args=None):
    cli_args = parse_args()
    
    print("\n" + "=" * 60)
    print("  DRONE LANDING SIMULATION - CONTROLLERS")
    print("=" * 60)
    print(f"  Rover Pattern: {cli_args.pattern}")
    print(f"  Rover Speed: {cli_args.speed} m/s")
    print(f"  Rover Amplitude: {cli_args.amplitude} m")
    print("=" * 60 + "\n")
    
    rclpy.init(args=args)
    
    drone_controller = None
    rover_controller = None
    executor = None
    
    try:
        # Create drone controller
        drone_controller = DroneController()
        
        # Create rover controller with specified pattern
        pattern = MovementPattern(cli_args.pattern)
        rover_controller = RoverController(
            pattern=pattern,
            speed=cli_args.speed,
            amplitude=cli_args.amplitude
        )
        
        # Use multi-threaded executor to run both nodes
        executor = MultiThreadedExecutor(num_threads=4)
        executor.add_node(drone_controller)
        executor.add_node(rover_controller)
        
        print("\nBoth controllers running. Press Ctrl+C to stop.\n")
        
        executor.spin()
        
    except KeyboardInterrupt:
        print('\nShutting down controllers...')
    except Exception as e:
        print(f'Error: {e}')
        sys.exit(1)
    finally:
        if executor:
            executor.shutdown()
        if drone_controller:
            drone_controller.destroy_node()
        if rover_controller:
            rover_controller.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
        print('Controllers stopped.')


if __name__ == '__main__':
    main()