#!/bin/bash
# =============================================================================
# Скрипт установки системы для роботов (ROS2 Jazzy + Webots + Nav2 + SLAM)
# Ubuntu 24.04 LTS
# =============================================================================
set -e  # Остановить выполнение при любой ошибке

BASHRC="$HOME/.bashrc"

# Вспомогательная функция: добавить строку в ~/.bashrc, если её там ещё нет
append_bashrc() {
    local line="$1"
    if ! grep -qF "$line" "$BASHRC"; then
        echo "$line" >> "$BASHRC"
        echo "  -> Добавлено в ~/.bashrc: $line"
    else
        echo "  -> Уже есть в ~/.bashrc: $line"
    fi
}

echo ""
echo "============================================================"
echo "  Установка системы для роботов на Ubuntu 24.04"
echo "============================================================"
echo ""

# ─────────────────────────────────────────────────────────────────────────────
# ШАГ 0 — Установка базовых инструментов (git, curl и т.д.)
# ─────────────────────────────────────────────────────────────────────────────
echo ">>> [0/4] Установка базовых инструментов..."
sudo apt update -y
sudo apt install -y git curl

# ─────────────────────────────────────────────────────────────────────────────
# ШАГ 1 — Установка ROS2 Jazzy
# ─────────────────────────────────────────────────────────────────────────────
echo ""
echo ">>> [1/4] Установка ROS2 Jazzy..."

# Локаль
sudo apt install -y locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

# Репозиторий ROS2
sudo apt install -y software-properties-common
sudo add-apt-repository -y universe
sudo apt update -y

ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest \
    | grep -F "tag_name" | awk -F\" '{print $4}')
curl -L -o /tmp/ros2-apt-source.deb \
    "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb"
sudo dpkg -i /tmp/ros2-apt-source.deb

# Установка ROS2 Jazzy Desktop Full
# (исправлена опечатка оригинала: desctop -> desktop)
sudo apt update -y
sudo apt install -y ros-jazzy-desktop-full

# colcon
sudo apt install -y python3-colcon-common-extensions

# Source ROS2 в текущей сессии и в ~/.bashrc
source /opt/ros/jazzy/setup.bash
append_bashrc "source /opt/ros/jazzy/setup.bash"

echo "  ROS2 Jazzy установлен."

# ─────────────────────────────────────────────────────────────────────────────
# ШАГ 2 — Установка симулятора Webots + ROS2-интеграция
# ─────────────────────────────────────────────────────────────────────────────
echo ""
echo ">>> [2/4] Установка Webots..."

# GPG-ключ и репозиторий Cyberbotics
sudo mkdir -p /etc/apt/keyrings
sudo wget -q https://cyberbotics.com/Cyberbotics.asc \
    -O /etc/apt/keyrings/Cyberbotics.asc

echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/Cyberbotics.asc] \
https://cyberbotics.com/debian binary-amd64/" \
    | sudo tee /etc/apt/sources.list.d/Cyberbotics.list

sudo apt update -y
sudo apt install -y webots

# Переменная окружения Webots
append_bashrc "export WEBOTS_HOME=/snap/webots/current/usr/share/webots"

# Пакет интеграции ROS2 <-> Webots (без него ros2 launch не заработает)
sudo apt install -y ros-jazzy-webots-ros2

echo "  Webots установлен."

# ─────────────────────────────────────────────────────────────────────────────
# ШАГ 3 — Сборка рабочего пространства ros2_ws (пакеты симулятора)
# ─────────────────────────────────────────────────────────────────────────────
echo ""
echo ">>> [3/4] Сборка пакетов симулятора (ros2_ws)..."

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src   # <-- переходим в src перед клонированием

git clone https://gitlab.mobird.dev/fms_group/ar_webots_fms_ros2.git
git clone https://gitlab.mobird.dev/fms_group/ar_arm95_moveit_config.git
git clone https://gitlab.mobird.dev/fms_group/ar_aruco_detect_ros2.git
git clone https://gitlab.mobird.dev/fms_group/ar_dual_lidar_merge_ros2.git
git clone https://gitlab.mobird.dev/fms_group/ar_nav_ros2.git

cd ~/ros2_ws

# rosdep init может упасть, если уже был вызван — игнорируем ошибку
sudo rosdep init 2>/dev/null || true
rosdep update
rosdep install --from-paths src -y --ignore-src

# Обязательный source перед сборкой
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install

append_bashrc "source ~/ros2_ws/install/setup.bash"
echo "  ros2_ws собран."

# ─────────────────────────────────────────────────────────────────────────────
# ШАГ 4 — Сборка nav2_ws (навигация) и slam_ws (SLAM)
# ─────────────────────────────────────────────────────────────────────────────
echo ""
echo ">>> [4/4] Сборка nav2_ws и slam_ws..."

# --- Nav2 ---
mkdir -p ~/nav2_ws/src
cd ~/nav2_ws/src   # <-- переходим в src перед клонированием
git clone https://github.com/ros-planning/navigation2.git -b 1.3.10

cd ~/nav2_ws
sudo rosdep init 2>/dev/null || true
rosdep update
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro jazzy

echo "  Сборка Nav2 (~30 мин)..."
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install

append_bashrc "source ~/nav2_ws/install/setup.bash"
echo "  nav2_ws собран."

# --- SLAM Toolbox ---
mkdir -p ~/slam_ws/src
cd ~/slam_ws/src   # <-- переходим в src перед клонированием
git clone https://github.com/SteveMacenski/slam_toolbox.git -b 2.8.3

cd ~/slam_ws
sudo rosdep init 2>/dev/null || true
rosdep update
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro jazzy

echo "  Сборка SLAM Toolbox (~5 мин)..."
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install

append_bashrc "source ~/slam_ws/install/setup.bash"
echo "  slam_ws собран."

# ─────────────────────────────────────────────────────────────────────────────
# Готово
# ─────────────────────────────────────────────────────────────────────────────
echo ""
echo "============================================================"
echo "  Установка завершена успешно!"
echo ""
echo "  Выполните в текущем терминале:"
echo "    source ~/.bashrc"
echo ""
echo "  Или откройте новый терминал — все source-команды"
echo "  уже добавлены в ~/.bashrc и применятся автоматически."
echo "============================================================"
echo ""