#!/bin/bash
# ========================================================
# Script to setup Catkin workspace for pnkx_nav_core
# ========================================================

set -e

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
WORKSPACE_NAME="pnkx_nav_catkin_ws"
WORKSPACE_DIR="${SCRIPT_DIR}/../${WORKSPACE_NAME}"

echo "=========================================="
echo "Setting up Catkin workspace for pnkx_nav_core"
echo "=========================================="
echo ""

# Create workspace directory
echo "Creating workspace directory: ${WORKSPACE_DIR}"
mkdir -p "${WORKSPACE_DIR}/src"

# Check if src directory already has packages
if [ -d "${WORKSPACE_DIR}/src" ] && [ "$(ls -A ${WORKSPACE_DIR}/src 2>/dev/null)" ]; then
    echo "Warning: ${WORKSPACE_DIR}/src already contains packages"
    read -p "Do you want to continue? (y/n) " -n 1 -r
    echo
    if [[ ! $REPLY =~ ^[Yy]$ ]]; then
        echo "Aborted."
        exit 1
    fi
fi

# Create symlinks to packages that have package.xml
echo ""
echo "Creating symlinks to packages with package.xml..."

# Find all packages with package.xml in the source tree
PACKAGES=(
    "src/Libraries/tf3"
    "src/Libraries/nav_2d_utils"
    "src/Navigations/Libraries/nav_grid"
    "src/Navigations/Cores/nav_core"
    "src/Navigations/Cores/nav_core2"
    "src/Navigations/Cores/nav_core_adapter"
    "src/Navigations/Cores/move_base_core"
    "src/Navigations/Cores/nav_core_adapter"
    "src/Packages/move_base"
    "src/Navigations/Packages/move_base"
)

for PKG_PATH in "${PACKAGES[@]}"; do
    FULL_PATH="${SCRIPT_DIR}/${PKG_PATH}"
    if [ -f "${FULL_PATH}/package.xml" ]; then
        PKG_NAME=$(basename "${PKG_PATH}")
        TARGET="${WORKSPACE_DIR}/src/${PKG_NAME}"
        
        if [ -e "${TARGET}" ]; then
            echo "  Warning: ${TARGET} already exists, skipping..."
        else
            echo "  Linking: ${PKG_NAME} -> ${PKG_PATH}"
            ln -s "${FULL_PATH}" "${TARGET}"
        fi
    else
        echo "  Warning: ${PKG_PATH}/package.xml not found, skipping..."
    fi
done

# Create a README in the workspace
cat > "${WORKSPACE_DIR}/README.md" << EOF
# Catkin Workspace for pnkx_nav_core

This is a Catkin workspace containing ROS-compatible packages from pnkx_nav_core.

## Structure

\`\`\`
${WORKSPACE_NAME}/
├── src/              # Source packages (symlinked from pnkx_nav_core)
└── build/            # Build directory (created by catkin_make)
\`\`\`

## Building

\`\`\`bash
cd ${WORKSPACE_DIR}
catkin_make
\`\`\`

## Source the workspace

After building, source the setup file:

\`\`\`bash
source devel/setup.bash
\`\`\`

## Note

Packages in \`src/\` are symlinked from the main pnkx_nav_core project.
To add more packages, create symlinks in \`src/\` pointing to packages
that have both \`CMakeLists.txt\` and \`package.xml\`.
EOF

echo ""
echo "=========================================="
echo "Workspace setup complete!"
echo "=========================================="
echo ""
echo "Next steps:"
echo "  1. cd ${WORKSPACE_DIR}"
echo "  2. catkin_make"
echo "  3. source devel/setup.bash"
echo ""
echo "Note: Make sure all dependencies are built in the main project first"
echo "      using standalone CMake before building with catkin."
echo ""