#!/bin/bash

# SRMP User Workspace Setup Script
# This script sets up the user workspace for building srmp_moveit_plugin

set -e

# Colors for output
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
NC='\033[0m' # No Color

echo -e "${BLUE}========================================${NC}"
echo -e "${BLUE}  SRMP User Workspace Setup${NC}"
echo -e "${BLUE}========================================${NC}"
echo ""

# Check if core libraries are installed
echo -e "${BLUE}Step 1: Checking core library installation...${NC}"

if [ ! -f "/opt/ros/noetic/lib/libsearch.so" ]; then
    echo -e "${RED}Error: Search library not found. Please install ros-noetic-search-library package first.${NC}"
    echo "Run: sudo dpkg -i ros-noetic-search-library_1.0.0_amd64.deb"
    exit 1
fi

if [ ! -f "/opt/ros/noetic/lib/libmanipulation_planning.so" ]; then
    echo -e "${RED}Error: Manipulation planning library not found. Please install ros-noetic-manipulation-planning package first.${NC}"
    echo "Run: sudo dpkg -i ros-noetic-manipulation-planning_1.0.0_amd64.deb"
    exit 1
fi

echo -e "${GREEN}✓ Core libraries found${NC}"

# Check MoveIt installation
echo -e "${BLUE}Step 2: Checking MoveIt installation...${NC}"

if [ ! -d "/opt/ros/noetic" ]; then
    echo -e "${RED}Error: ROS Noetic not found. Please install ROS Noetic first.${NC}"
    exit 1
fi

# Check for required MoveIt packages
REQUIRED_PACKAGES=(
    "moveit_core"
    "moveit_ros_planning"
    "moveit_ros_planning_interface"
    "moveit_msgs"
)

MISSING_PACKAGES=()
for pkg in "${REQUIRED_PACKAGES[@]}"; do
    if ! dpkg -l | grep -q "ros-noetic-$pkg"; then
        MISSING_PACKAGES+=("ros-noetic-$pkg")
    fi
done

if [ ${#MISSING_PACKAGES[@]} -gt 0 ]; then
    echo -e "${RED}Error: Missing required MoveIt packages:${NC}"
    for pkg in "${MISSING_PACKAGES[@]}"; do
        echo "  - $pkg"
    done
    echo "Please install them with: sudo apt-get install ${MISSING_PACKAGES[*]}"
    exit 1
fi

echo -e "${GREEN}✓ MoveIt packages found${NC}"

# Source ROS environment
echo -e "${BLUE}Step 3: Setting up ROS environment...${NC}"
if [ -f "/opt/ros/noetic/setup.bash" ]; then
    source "/opt/ros/noetic/setup.bash"
    echo -e "${GREEN}✓ ROS Noetic environment sourced${NC}"
else
    echo -e "${RED}Error: ROS Noetic setup.bash not found${NC}"
    exit 1
fi

# Initialize workspace
echo -e "${BLUE}Step 4: Initializing workspace...${NC}"
cd "$(dirname "$0")"

if [ ! -f "src/CMakeLists.txt" ]; then
    catkin_init_workspace src
    echo -e "${GREEN}✓ Workspace initialized${NC}"
else
    echo -e "${GREEN}✓ Workspace already initialized${NC}"
fi

# Build workspace
echo -e "${BLUE}Step 5: Building workspace...${NC}"
catkin build

if [ $? -eq 0 ]; then
    echo -e "${GREEN}✓ Workspace built successfully${NC}"
else
    echo -e "${RED}✗ Build failed${NC}"
    exit 1
fi

# Source workspace
echo -e "${BLUE}Step 6: Sourcing workspace...${NC}"
source devel/setup.bash

echo ""
echo -e "${GREEN}========================================${NC}"
echo -e "${GREEN}  Workspace Setup Complete!${NC}"
echo -e "${GREEN}========================================${NC}"
echo ""
echo -e "${BLUE}Available packages:${NC}"
echo "✓ srmp_moveit_plugin - MoveIt plugin for SRMP"
echo "✓ mramp_moveit_resources - MoveIt configuration files"
echo ""
echo -e "${BLUE}To use this workspace:${NC}"
echo "1. Source the workspace: source devel/setup.bash"
echo "2. Test with MoveIt: roslaunch panda_two_binpick_moveit_config demo.launch"
echo ""
echo -e "${YELLOW}Note: This workspace builds against the installed core libraries.${NC}"
echo -e "${YELLOW}Make sure you have installed the core library packages first.${NC}"
