run

#!/bin/bash
LOG_FILE="command_output.log"

# Function to print messages in green
print_green() {
    echo -e "\e[0m\e[32m$1\e[0m"
}

# Function to print messages in blue
print_blue() {
    echo -e "\e[34m$1\e[0m"
}

# Function to print messages in red
print_red() {
    echo -e "\e[0m\e[31m$1\e[0m"
}

# Print the blue welcome message
print_blue "------------------------------------------"
print_blue "Purdue RoboMaster Club: Auto Aiming Suite"
print_blue "------------------------------------------"

# Function to clean the workspace
clean() {
    print_green "Cleaning workspace..."
    rm -rf build install log Testing $LOG_FILE
    print_green "Clean complete."
}

# Function to build the project
build() {
    print_green "[*] Building project with Release configuration and optimization flags."

    build_args="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS_RELEASE=-O3 -DPYTHON_EXECUTABLE=/usr/bin/python3.8"

    # Add the DEBUG flag if '--debug' or '-d' was provided
    if [[ "$debug" == "true" ]]; then
        build_args="-DCMAKE_CXX_FLAGS=-DDEBUG $build_args"
    fi

    if [[ "$quiet" == "true" ]]; then
        colcon build --symlink-install --cmake-args $build_args --allow-overriding opencv_armor_detector >> "$LOG_FILE" 2>&1
    else
        colcon build --symlink-install --cmake-args $build_args --allow-overriding opencv_armor_detector
    fi

    if [ $? -eq 0 ]; then
        # print checkmark
        print_green "[☺ ] Build successful."
        source install/setup.sh
    else
        print_red "Build failed. See $LOG_FILE for more information."
        exit 1
    fi
}

# Function to run tests for each ROS module (non-recursively at first)
test() {
    print_green "Running tests for ROS modules..."

    # Run build first
    build

    # Check if a package name is provided
    local package_name=$1

    # Run tests
    if [[ -z "$package_name" ]]; then
        print_green "Running all tests..."
    else
        print_green "Running tests for package: $package_name"
    fi
    if [[ "$quiet" == "true" ]]; then
        if [[ -n "$package_name" ]]; then
            colcon test --packages-select "$package_name" >> "$LOG_FILE" 2>&1
        else
            colcon test >> "$LOG_FILE" 2>&1
        fi
        colcon test-result --verbose >> "$LOG_FILE" 2>&1
    else
        if [[ -n "$package_name" ]]; then
            colcon test --packages-select "$package_name"
        else
            colcon test
        fi
        colcon test-result --verbose
    fi

    # Check if tests passed
    if [ $? -ne 0 ]; then
        print_red "Tests failed. See $LOG_FILE for more information."
        exit 1
    fi

    print_green "Test execution complete."
}

# Function to run a ROS2 launch command
run() {
    if [[ -z "$1" ]]; then
        print_red "Error: No launch argument provided."
        echo "Usage: $0 run <launch_file>"
        exit 1
    fi

    launch_file="$1"
    
    # Run build first
    build

    print_green "[*] Running ROS2 launch: prm_launch $launch_file"
    ros2 launch prm_launch "$launch_file"
    if [ $? -ne 0 ]; then
        print_red "Launch failed."
        exit 1
    fi
}

# Parse script options
while [[ $# -gt 0 ]]; do
    case "$1" in
        -q|--quiet)
            quiet=true
            shift
            ;;
        -d|--debug)
            debug=true
            shift
            ;;
        build)
            build
            shift
            ;;
        clean)
            clean
            shift
            ;;
        test)
            shift
            test "$1" # Pass the next argument (package name) to the test function
            shift
            ;;
        run)
            shift
            run "$@"
            shift $#
            ;;
        *)
            echo "Usage: $0 [--quiet] [--debug] {build|clean|test [package_name]|run <launch_file>}"
            exit 1
            ;;
    esac
done

exit 0