#!/bin/bash
# =============================================================================
# Docker Entrypoint - RDC Simulation
# =============================================================================
# Sets up the environment inside the container before running commands.
# =============================================================================

set -e

# Source ROS 2
if [ -f "/opt/ros/jazzy/setup.bash" ]; then
    source /opt/ros/jazzy/setup.bash
fi

# Source ArduPilot environment
if [ -f "$HOME/.ardupilot_env" ]; then
    source "$HOME/.ardupilot_env"
fi

# Activate Python venv
if [ -f "$HOME/RDC_Simulation/venv/bin/activate" ]; then
    source "$HOME/RDC_Simulation/venv/bin/activate"
fi

# Set up paths
export ARDUPILOT_HOME=$HOME/ardupilot
export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin
export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$HOME/RDC_Simulation/gazebo/models

# If no command provided, print help
if [ "$1" = "bash" ] || [ -z "$1" ]; then
    echo "=============================================="
    echo "  RDC Simulation Container"
    echo "=============================================="
    echo ""
    echo "Environment ready! Commands:"
    echo ""
    echo "  # Start Gazebo (Terminal 1)"
    echo "  ./scripts/run_ardupilot_sim.sh runway"
    echo ""
    echo "  # Start SITL (Terminal 2)"  
    echo "  sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console"
    echo ""
    echo "  # Run controller (Terminal 3)"
    echo "  python scripts/run_ardupilot.py --pattern square"
    echo ""
    echo "Or run all in one:"
    echo "  ./scripts/run_ardupilot_controller.sh"
    echo ""
    exec /bin/bash
else
    exec "$@"
fi