#! /bin/bash
# ============================================================================
# Shell script to install ROS2 Jazzy Desktop on Raspberry Pi 5 Ubuntu 24.04
# Source: STEAM Clown - www.steamclown.org
# GitHub: https://github.com/jimTheSTEAMClown/Linux
# Hacker: Jim Burnham - STEAM Clown, Engineer, Maker, Propmaster & Adrenologist
# This example code is licensed under the CC BY-NC-SA 4.0, GNU GPL and EUPL
# https://creativecommons.org/licenses/by-nc-sa/4.0/
# https://www.gnu.org/licenses/gpl-3.0.en.html
# https://eupl.eu/
#
# Program/Design Name:   Pi5-Ubuntu-24.04-ROS2-Jazzy-Install.sh
# Description:           Installs ROS2 Jazzy Desktop on Raspberry Pi 5 running
#                        Ubuntu 24.04 LTS (ARM64 / aarch64) using the official
#                        binary apt package method (NOT a source build).
#
#                        Binary install takes ~5 minutes vs 2-4 hours for a
#                        source build, and is the recommended method for Pi 5.
#                        ARM64 is Tier 1 supported on ROS2 Jazzy + Ubuntu 24.04.
#
#                        Installs ros-jazzy-desktop (full install including
#                        RViz2, rqt, and all GUI tools).
#
#                        All output is logged to ~/ros2-jazzy-install-<timestamp>.log
#                        while also displaying in the terminal (tee).
#
# Target Hardware:       Raspberry Pi 5 (ARM64 / aarch64)
# Target OS:             Ubuntu 24.04.x LTS Desktop (64-bit)
# ROS2 Distribution:     Jazzy Jalisco (supported until May 2029)
# Install Method:        Binary apt package (NOT source build)
# Install Package:       ros-jazzy-desktop (full install with GUI tools)
#
# Dependencies:          Run as a normal user with sudo privileges
#                        Must have internet access
#                        Ubuntu 24.04 must be installed and updated
#                        Run Pi5-Ubuntu-24.04-New-Clean-Install.sh first
#
# Revision:
#  Revision 0.01 - Initial Pi 5 ROS2 Jazzy binary install script
#  Revision 0.02 - Confirmed logging active before any output
#                  Added PRE-FLIGHT update/upgrade before Step 1
#                  Added FINAL update/upgrade/autoremove after Step 11
#  Revision 0.03 - Fixed check_ros_pkg to use prefix grep instead of exact
#                  dpkg name match; fixes false NOT FOUND for ros-jazzy-rqt
#                  which installs as component packages not a single package
#
# Steps:
#  PRE-FLIGHT - Confirm Logging Active + Update and Upgrade
#  STEP  1 - Verify ARM64 Architecture and Ubuntu 24.04
#  STEP  2 - Fix Ubuntu Pi apt Sources (critical for Pi Ubuntu images)
#  STEP  3 - Set Locale to UTF-8
#  STEP  4 - Add Universe Repository
#  STEP  5 - Add ROS2 apt Repository (via ros-apt-source)
#  STEP  6 - Install ROS2 Jazzy Desktop (binary apt install)
#  STEP  7 - Install ROS2 Dev Tools (colcon, rosdep, etc.)
#  STEP  8 - Initialize rosdep
#  STEP  9 - Auto-Source ROS2 in .bashrc
#  STEP 10 - Verify ROS2 Install
#  STEP 11 - Run Talker / Listener Test
#  FINAL    - Final Update, Upgrade, and Autoremove
#
# Usage:
#   chmod +x Pi5-Ubuntu-24.04-ROS2-Jazzy-Install.sh
#   ./Pi5-Ubuntu-24.04-ROS2-Jazzy-Install.sh
#
# After install, test in two terminals:
#   Terminal 1: ros2 run demo_nodes_cpp talker
#   Terminal 2: ros2 run demo_nodes_py listener
#
# References:
#   https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html
#   https://docs.ros.org/en/jazzy/How-To-Guides/Installing-on-Raspberry-Pi.html
#   https://docs.ros.org/en/jazzy/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html
# ============================================================================

# ============================================================================
# LOGGING SETUP
# All output goes to terminal AND to a timestamped log file in home directory.
# Uses 'tee' so you see everything live in the terminal while it is captured.
# Log file:  ~/ros2-jazzy-install-YYYYMMDD-HHMMSS.log
#
# HOW THIS WORKS:
#   exec > >(tee -a "$LOG_FILE") 2>&1
#   This line must be the FIRST thing that runs after setting LOG_FILE.
#   It redirects:
#     stdout (>) through tee, which writes to both terminal and log file
#     stderr (2>&1) is merged into stdout so errors are also captured
#   Everything from this point forward — including all echo, apt-get,
#   curl, ros2 commands — goes to both the terminal and the log file.
# ============================================================================
LOG_FILE="$HOME/ros2-jazzy-install-$(date +%Y%m%d-%H%M%S).log"

# Redirect all stdout and stderr through tee to the log file.
# This MUST be before any echo or command output.
exec > >(tee -a "$LOG_FILE") 2>&1

# Confirm logging is active immediately with a timestamped entry
echo "============================================================"
echo "  LOGGING ACTIVE"
echo "  Log file : $LOG_FILE"
echo "  Started  : $(date)"
echo "  User     : $USER"
echo "  Hostname : $(hostname)"
echo "  All terminal output is captured to the log file above."
echo "  To view log later: cat $LOG_FILE"
echo "  To tail log live:  tail -f $LOG_FILE"
echo "============================================================"
echo " "

# Verify the log file was actually created and is writable
if [ -f "$LOG_FILE" ]; then
    echo "  LOG FILE CHECK: $LOG_FILE exists and is being written"
    echo "  Log file size: $(wc -c < "$LOG_FILE") bytes so far"
else
    echo "  WARNING: Log file could not be created at $LOG_FILE"
    echo "  Check that $HOME is writable"
fi
echo " "

# ============================================================================
# PRE-FLIGHT - UPDATE AND UPGRADE
# Runs apt-get update and upgrade BEFORE any other steps to ensure the
# system has the latest package lists and security patches. This is critical
# because ROS2 dependencies may require newer versions of base Ubuntu packages.
#
# Also runs autoremove to clear any orphaned packages that would otherwise
# generate "no longer required" noise throughout the rest of the log.
#
# This runs AUTOMATICALLY (no prompt) as it is a required precondition.
# ============================================================================
echo "============================================================"
echo "PRE-FLIGHT - UPDATE AND UPGRADE"
echo "  Running before Step 1 to ensure system is current"
echo "  Required precondition for ROS2 installation"
echo "  Logged to: $LOG_FILE"
echo "============================================================"
echo " "

echo "----------------------------------------------------"
echo "PRE-FLIGHT 1/3: sudo apt-get update"
echo "  Refreshing package lists from all repositories"
echo "  Started: $(date)"
echo "----------------------------------------------------"
sudo apt-get update
echo "  Completed: $(date)"

echo " "
echo "----------------------------------------------------"
echo "PRE-FLIGHT 2/3: sudo apt-get upgrade -y"
echo "  Applying all available package upgrades"
echo "  Started: $(date)"
echo "----------------------------------------------------"
sudo apt-get upgrade -y
echo "  Completed: $(date)"

echo " "
echo "----------------------------------------------------"
echo "PRE-FLIGHT 3/3: sudo apt-get autoremove -y"
echo "  Removing orphaned packages to reduce log noise"
echo "  Started: $(date)"
echo "----------------------------------------------------"
sudo apt-get autoremove -y
echo "  Completed: $(date)"

echo " "
echo "----------------------------------------------------"
echo "Done: PRE-FLIGHT UPDATE AND UPGRADE"
echo "  System is current and ready for ROS2 installation"
echo "----------------------------------------------------"
echo " "

# ============================================================================
# HEADER BANNER
# ============================================================================
echo " "
echo "  ____  _  ____    ____  "
echo " (  _ \(_)( ___)  ( ___)  "
echo "  )___/ _  )__)    )__)   "
echo " (__) (_)(____)  (____)   "
echo "  ____  ____  ____   __   __  __  ____  ____  "
echo " (  _ \(  _ \(  _ \ / _) /  \(  )(_  _)/ ___) "
echo "  )   / ) _ ( ) _ (( (/\( () ))(__  )(  \___ \ "
echo " (__\_)(____/(____/ \__/ \__/(____)(__)  (____/ "
echo " "
echo "============================================================"
echo "  ROS2 Jazzy Desktop - Binary Install Script"
echo "  Revision 0.03"
echo "  Target: Pi 5 / ARM64 / Ubuntu 24.04 LTS"
echo "  Package: ros-jazzy-desktop (full install with GUI tools)"
echo "  Method:  Binary apt install (NOT a source build)"
echo "  ARM64 Tier 1 supported on Jazzy + Ubuntu 24.04"
echo "  For: Fire Breathing Robots / Mechatronics Curriculum"
echo "  Log: $LOG_FILE"
echo "============================================================"
echo " "
echo "  WHY BINARY INSTALL vs SOURCE BUILD:"
echo "    Binary apt install:  ~5 minutes,  low RAM usage"
echo "    Source build:        2-4 hours,   can exhaust RAM on 4GB Pi"
echo "    Binary is the correct method for teaching and deployment."
echo " "
pwd
ls
echo " "

# ============================================================================
# CONFIRM RUN
# ============================================================================
echo "----------------------------------------------------"
echo "Do you wish to run the ROS2 Jazzy Install Script?"
echo "----------------------------------------------------"
select yn in "Yes" "No"; do
    case $yn in
        Yes )
            echo "----------------------------------------------------"
            echo "Running ROS2 Jazzy Install Script"
            echo "----------------------------------------------------"
            break;;
        No )
            echo "----------------------------------------------------"
            echo "Exiting Without Changes"
            echo "----------------------------------------------------"
            exit;;
    esac
done

# ============================================================================
# STEP 1 - VERIFY ARM64 AND UBUNTU 24.04
# ROS2 Jazzy binary packages for arm64 require Ubuntu 24.04 (Noble Numbat).
# Other Ubuntu versions or architectures require source builds or Docker.
# This step checks your system and warns if it doesn't match.
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 1 - VERIFY ARM64 ARCHITECTURE AND UBUNTU 24.04"
echo "  Checking: uname -m (should be aarch64)"
echo "  Checking: /etc/os-release (should be Ubuntu 24.04 Noble)"
echo "============================================================"

ARCH=$(uname -m)
echo "  Architecture: $ARCH"

if [ -f /etc/os-release ]; then
    . /etc/os-release
    echo "  OS Name:      $NAME"
    echo "  OS Version:   $VERSION"
    echo "  Codename:     $VERSION_CODENAME"
else
    echo "  WARNING: Cannot read /etc/os-release"
fi

ARCH_OK=true
OS_OK=true

if [ "$ARCH" != "aarch64" ]; then
    echo " "
    echo "  WARNING: Architecture is $ARCH, not aarch64"
    echo "  ROS2 Jazzy binary ARM64 packages require aarch64"
    echo "  On x86_64 use the standard Ubuntu desktop install guide instead"
    ARCH_OK=false
fi

if [ "$VERSION_CODENAME" != "noble" ]; then
    echo " "
    echo "  WARNING: Ubuntu codename is $VERSION_CODENAME, not noble (24.04)"
    echo "  ROS2 Jazzy requires Ubuntu 24.04 Noble Numbat"
    echo "  Jazzy on other Ubuntu versions requires source compilation"
    OS_OK=false
fi

if [ "$ARCH_OK" = false ] || [ "$OS_OK" = false ]; then
    echo " "
    echo "  Your system does not match the required configuration."
    echo "  Do you wish to continue anyway? (may fail)"
    select yn in "Yes - Continue Anyway" "No - Exit"; do
        case $yn in
            "Yes - Continue Anyway" ) break;;
            "No - Exit" ) echo "Exiting."; exit 1;;
        esac
    done
else
    echo " "
    echo "  Architecture: OK (aarch64)"
    echo "  Ubuntu:       OK (24.04 Noble)"
    echo "  This system is fully supported for ROS2 Jazzy binary install"
fi

echo " "
echo "----------------------------------------------------"
echo "Done: STEP 1 - VERIFY ARCHITECTURE AND OS"
echo "----------------------------------------------------"

# ============================================================================
# STEP 2 - FIX UBUNTU PI APT SOURCES
# CRITICAL for Raspberry Pi Ubuntu images.
# Ubuntu for Raspberry Pi does NOT include noble-updates and noble-backports
# in the apt sources by default. Without these, ROS2 packages cannot be found
# because some ROS2 dependencies live in those repos.
#
# This step checks your /etc/apt/sources.list.d/ubuntu.sources file and
# adds the missing Components if needed.
#
# Reference:
#   https://docs.ros.org/en/jazzy/How-To-Guides/Installing-on-Raspberry-Pi.html
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 2 - FIX UBUNTU PI APT SOURCES"
echo "  Ubuntu Pi images often missing noble-updates and"
echo "  noble-backports in apt sources, which ROS2 requires."
echo "  Checking and fixing /etc/apt/sources.list.d/ubuntu.sources"
echo "============================================================"
echo "Do you wish to check and fix Ubuntu apt sources? Enter y/Y or n/N"
read -p "Fix apt sources?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    SOURCES_FILE="/etc/apt/sources.list.d/ubuntu.sources"

    if [ ! -f "$SOURCES_FILE" ]; then
        echo "  WARNING: $SOURCES_FILE not found"
        echo "  Your system may use a different sources configuration"
        echo "  Check: ls /etc/apt/sources.list.d/"
        ls /etc/apt/sources.list.d/
    else
        echo " "
        echo "  Current contents of $SOURCES_FILE:"
        echo "----------------------------------------------------"
        cat "$SOURCES_FILE"
        echo "----------------------------------------------------"
        echo " "

        echo "  Checking for noble-updates..."
        if grep -q "noble-updates" "$SOURCES_FILE"; then
            echo "  noble-updates: FOUND - OK"
        else
            echo "  noble-updates: MISSING - adding..."
            sudo sed -i '/^Suites: noble$/s/noble/noble noble-updates/' "$SOURCES_FILE" 2>/dev/null || \
            echo "  Auto-fix failed - please manually add 'noble-updates' to Suites line"
        fi

        echo " "
        echo "  Checking for noble-backports..."
        if grep -q "noble-backports" "$SOURCES_FILE"; then
            echo "  noble-backports: FOUND - OK"
        else
            echo "  noble-backports: MISSING - adding..."
            sudo sed -i '/noble-updates/s/$/ noble-backports/' "$SOURCES_FILE" 2>/dev/null || \
            echo "  Auto-fix failed - please manually add 'noble-backports' to Suites line"
        fi

        echo " "
        echo "  Updated contents of $SOURCES_FILE:"
        echo "----------------------------------------------------"
        cat "$SOURCES_FILE"
        echo "----------------------------------------------------"
    fi

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 2 - FIX UBUNTU PI APT SOURCES"
    echo "----------------------------------------------------"
else
    echo "Skipping apt sources fix"
    echo "WARNING: ROS2 install may fail if noble-updates/backports are missing"
fi

# ============================================================================
# STEP 3 - SET LOCALE TO UTF-8
# ROS2 requires a UTF-8 locale. Ubuntu 24.04 Desktop usually has this set
# already, but this step ensures it is correct and verifies the setting.
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 3 - SET LOCALE TO UTF-8"
echo "  ROS2 requires UTF-8 locale"
echo "  Ubuntu 24.04 Desktop usually has this set already"
echo "  This step verifies and ensures correct locale settings"
echo "============================================================"
echo "Do you wish to set/verify locale? Enter y/Y or n/N"
read -p "Set locale?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    echo " "
    echo "----------------------------------------------------"
    echo "Current locale settings:"
    locale
    echo "----------------------------------------------------"

    echo " "
    echo "----------------------------------------------------"
    echo "Installing locales package and generating en_US.UTF-8"
    echo "Running: sudo apt-get install locales -y"
    echo "----------------------------------------------------"
    sudo apt-get update -qq
    sudo apt-get install locales -y
    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
    export LC_ALL=en_US.UTF-8

    echo " "
    echo "----------------------------------------------------"
    echo "Verified locale settings:"
    locale
    echo "----------------------------------------------------"

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 3 - SET LOCALE"
    echo "----------------------------------------------------"
else
    echo "Skipping locale setup"
fi

# ============================================================================
# STEP 4 - ADD UNIVERSE REPOSITORY
# The Ubuntu Universe repository must be enabled before adding the ROS2 repo.
# It contains many of the dependencies that ROS2 packages rely on.
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 4 - ADD UNIVERSE REPOSITORY"
echo "  Ubuntu Universe repository required before adding ROS2 repo"
echo "  Contains dependencies that ROS2 packages rely on"
echo "  Running: sudo add-apt-repository universe"
echo "============================================================"
echo "Do you wish to add the Universe repository? Enter y/Y or n/N"
read -p "Add Universe repo?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    echo " "
    echo "----------------------------------------------------"
    echo "Installing software-properties-common"
    echo "Running: sudo apt-get install software-properties-common -y"
    echo "----------------------------------------------------"
    sudo apt-get install software-properties-common -y

    echo " "
    echo "----------------------------------------------------"
    echo "Adding Ubuntu Universe repository"
    echo "Running: sudo add-apt-repository universe"
    echo "----------------------------------------------------"
    sudo add-apt-repository universe -y

    sudo apt-get update -qq

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 4 - ADD UNIVERSE REPOSITORY"
    echo "----------------------------------------------------"
else
    echo "Skipping Universe repository setup"
fi

# ============================================================================
# STEP 5 - ADD ROS2 APT REPOSITORY
# Adds the official ROS2 apt repository to your system using the modern
# ros-apt-source method. This replaces the older curl/gpg approach.
#
# The ros-apt-source package:
#   - Adds the ROS2 GPG signing key
#   - Adds the ROS2 apt source list
#   - Auto-updates repository config when new versions are released
#
# Reference:
#   https://github.com/ros-infrastructure/ros-apt-source
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 5 - ADD ROS2 APT REPOSITORY"
echo "  Adding official ROS2 apt repository via ros-apt-source"
echo "  This is the modern method replacing older curl/gpg approach"
echo "  The ros-apt-source package manages ROS2 repo configuration"
echo "============================================================"
echo "Do you wish to add the ROS2 apt repository? Enter y/Y or n/N"
read -p "Add ROS2 apt repo?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    echo " "
    echo "----------------------------------------------------"
    echo "Getting latest ros-apt-source version from GitHub"
    echo "----------------------------------------------------"
    sudo apt-get install curl -y

    export 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}')

    echo "  ros-apt-source version: $ROS_APT_SOURCE_VERSION"

    if [ -z "$ROS_APT_SOURCE_VERSION" ]; then
        echo "  ERROR: Could not retrieve ros-apt-source version"
        echo "  Check internet connection and try again"
        echo "  Manual URL: https://github.com/ros-infrastructure/ros-apt-source/releases"
        exit 1
    fi

    echo " "
    echo "----------------------------------------------------"
    echo "Downloading ros2-apt-source .deb package"
    echo "----------------------------------------------------"
    UBUNTU_CODENAME=$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})
    ROS_DEB_URL="https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.${UBUNTU_CODENAME}_all.deb"

    echo "  Downloading from: $ROS_DEB_URL"
    curl -L -o /tmp/ros2-apt-source.deb "$ROS_DEB_URL"

    echo " "
    echo "----------------------------------------------------"
    echo "Installing ros2-apt-source .deb"
    echo "Running: sudo dpkg -i /tmp/ros2-apt-source.deb"
    echo "----------------------------------------------------"
    sudo dpkg -i /tmp/ros2-apt-source.deb

    echo " "
    echo "----------------------------------------------------"
    echo "Updating apt package lists with new ROS2 repository"
    echo "Running: sudo apt-get update"
    echo "----------------------------------------------------"
    sudo apt-get update

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 5 - ADD ROS2 APT REPOSITORY"
    echo "----------------------------------------------------"
else
    echo "Skipping ROS2 apt repository setup"
    echo "WARNING: ROS2 install will fail without the ROS2 apt repo"
fi

# ============================================================================
# STEP 6 - INSTALL ROS2 JAZZY DESKTOP
# Installs the full ros-jazzy-desktop package which includes:
#   - ROS2 core libraries and communication middleware (DDS)
#   - rclpy and rclcpp (Python and C++ client libraries)
#   - All standard ROS2 message and service types
#   - RViz2 - 3D visualization tool for robot sensors and state
#   - rqt   - Modular GUI toolkit (topic monitor, graph viewer, etc.)
#   - Gazebo integration packages
#   - Demo nodes (demo_nodes_cpp, demo_nodes_py) for testing
#   - Command-line tools: ros2 topic, ros2 node, ros2 run, etc.
#
# Alternative: ros-jazzy-ros-base (no GUI tools, smaller, better for
# headless server/robot-brain deployments)
#
# Disk space: ros-jazzy-desktop requires approximately 1.5-2GB
# Install time: approximately 5-10 minutes on Pi 5 with good wifi
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 6 - INSTALL ROS2 JAZZY DESKTOP"
echo "  Package: ros-jazzy-desktop (full install with GUI tools)"
echo "  Includes: RViz2, rqt, demo nodes, all client libraries"
echo "  Disk space required: ~1.5-2 GB"
echo "  Install time: ~5-10 minutes on Pi 5"
echo " "
echo "  Alternative (not installed here):"
echo "    ros-jazzy-ros-base = lean install, no GUI, better for"
echo "    headless robot-brain deployments"
echo "============================================================"
echo "Do you wish to install ros-jazzy-desktop? Enter y/Y or n/N"
read -p "Install ROS2 Jazzy Desktop?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    echo " "
    echo "----------------------------------------------------"
    echo "Running: sudo apt-get upgrade -y"
    echo "  Ensures all packages are current before ROS2 install"
    echo "----------------------------------------------------"
    sudo apt-get upgrade -y

    echo " "
    echo "----------------------------------------------------"
    echo "Installing ros-jazzy-desktop"
    echo "  This is the full ROS2 Jazzy install with GUI tools"
    echo "  Includes: RViz2, rqt, demo nodes, all libraries"
    echo "Running: sudo apt-get install ros-jazzy-desktop -y"
    echo "----------------------------------------------------"
    sudo apt-get install ros-jazzy-desktop -y

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 6 - INSTALL ROS2 JAZZY DESKTOP"
    echo "----------------------------------------------------"
else
    echo "Skipping ROS2 Jazzy Desktop install"
fi

# ============================================================================
# STEP 7 - INSTALL ROS2 DEV TOOLS
# Additional development tools required for building ROS2 packages,
# managing workspaces, and developing robot applications.
#
#   ros-dev-tools          - Meta-package including:
#                              colcon       (build tool for ROS2 workspaces)
#                              rosdep       (dependency management)
#                              vcstool      (version control for ROS2 repos)
#                              bloom        (release management)
#                              and more
#   python3-colcon-common-extensions - Tab completion and colcon mixins
#   python3-argcomplete    - Tab completion for ros2 CLI commands
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 7 - INSTALL ROS2 DEV TOOLS"
echo "  Installing:"
echo "    - ros-dev-tools (colcon, rosdep, vcstool, and more)"
echo "    - python3-colcon-common-extensions (tab completion)"
echo "    - python3-argcomplete (ros2 CLI tab completion)"
echo "============================================================"
echo "Do you wish to install ROS2 dev tools? Enter y/Y or n/N"
read -p "Install ROS2 dev tools?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    echo " "
    echo "----------------------------------------------------"
    echo "Installing ros-dev-tools"
    echo "  Includes colcon, rosdep, vcstool, and other ROS2 tools"
    echo "Running: sudo apt-get install ros-dev-tools -y"
    echo "----------------------------------------------------"
    sudo apt-get install ros-dev-tools -y

    echo " "
    echo "----------------------------------------------------"
    echo "Installing python3-colcon-common-extensions"
    echo "  Provides colcon tab completion and build mixins"
    echo "Running: sudo apt-get install python3-colcon-common-extensions -y"
    echo "----------------------------------------------------"
    sudo apt-get install python3-colcon-common-extensions -y

    echo " "
    echo "----------------------------------------------------"
    echo "Installing python3-argcomplete"
    echo "  Enables tab completion for ros2 CLI commands"
    echo "  After install, run: echo 'eval \"\$(register-python-argcomplete3 ros2)\"' >> ~/.bashrc"
    echo "Running: sudo apt-get install python3-argcomplete -y"
    echo "----------------------------------------------------"
    sudo apt-get install python3-argcomplete -y

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 7 - INSTALL ROS2 DEV TOOLS"
    echo "----------------------------------------------------"
else
    echo "Skipping ROS2 dev tools install"
fi

# ============================================================================
# STEP 8 - INITIALIZE ROSDEP
# rosdep is the ROS2 dependency management tool. It resolves and installs
# system dependencies for ROS2 packages you build from source.
# Must be initialized once before first use.
#
# NOTE: 'sudo rosdep init' fails if already initialized (file exists).
#       The script handles this gracefully.
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 8 - INITIALIZE ROSDEP"
echo "  rosdep manages system dependencies for ROS2 packages"
echo "  Must be initialized once before first use"
echo "  'sudo rosdep init' is skipped if already initialized"
echo "============================================================"
echo "Do you wish to initialize rosdep? Enter y/Y or n/N"
read -p "Initialize rosdep?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    echo " "
    echo "----------------------------------------------------"
    echo "Running: sudo rosdep init"
    echo "  (safe to ignore 'already initialized' message)"
    echo "----------------------------------------------------"
    sudo rosdep init 2>/dev/null || echo "  rosdep already initialized - skipping init"

    echo " "
    echo "----------------------------------------------------"
    echo "Running: rosdep update"
    echo "  Downloads the latest package dependency database"
    echo "----------------------------------------------------"
    rosdep update

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 8 - INITIALIZE ROSDEP"
    echo "----------------------------------------------------"
else
    echo "Skipping rosdep initialization"
fi

# ============================================================================
# STEP 9 - AUTO-SOURCE ROS2 IN .BASHRC
# Adds the ROS2 setup script to ~/.bashrc so it is automatically sourced
# every time a new terminal opens. Without this, you must manually run
# 'source /opt/ros/jazzy/setup.bash' in every new terminal.
#
# Also adds:
#   - ros2 CLI tab completion via python3-argcomplete
#   - colcon tab completion
#   - ROS_DOMAIN_ID=0 (default domain; change if multiple robots on network)
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 9 - AUTO-SOURCE ROS2 IN .BASHRC"
echo "  Adds ROS2 setup to ~/.bashrc for automatic sourcing"
echo "  Without this you must run 'source /opt/ros/jazzy/setup.bash'"
echo "  in every new terminal manually"
echo "  Also adds: ros2 tab completion, colcon tab completion"
echo "  Also sets: ROS_DOMAIN_ID=0 (default)"
echo "============================================================"
echo "Do you wish to auto-source ROS2 in .bashrc? Enter y/Y or n/N"
read -p "Auto-source ROS2 in .bashrc?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    echo " "
    echo "----------------------------------------------------"
    echo "Adding ROS2 Jazzy source to ~/.bashrc"
    echo "----------------------------------------------------"

    if ! grep -q "ros/jazzy/setup.bash" "$HOME/.bashrc"; then
        echo '' >> "$HOME/.bashrc"
        echo '# ROS2 Jazzy - auto source on terminal open' >> "$HOME/.bashrc"
        echo 'source /opt/ros/jazzy/setup.bash' >> "$HOME/.bashrc"
        echo "  Added: source /opt/ros/jazzy/setup.bash"
    else
        echo "  ROS2 Jazzy already in ~/.bashrc - skipping"
    fi

    echo " "
    echo "----------------------------------------------------"
    echo "Adding ros2 CLI tab completion to ~/.bashrc"
    echo "----------------------------------------------------"
    if ! grep -q "register-python-argcomplete3 ros2" "$HOME/.bashrc"; then
        echo '# ROS2 CLI tab completion' >> "$HOME/.bashrc"
        echo 'eval "$(register-python-argcomplete3 ros2)"' >> "$HOME/.bashrc"
        echo "  Added: ros2 tab completion"
    else
        echo "  ros2 tab completion already in ~/.bashrc - skipping"
    fi

    echo " "
    echo "----------------------------------------------------"
    echo "Adding colcon tab completion to ~/.bashrc"
    echo "----------------------------------------------------"
    if ! grep -q "colcon/completion" "$HOME/.bashrc" && \
       [ -f "/usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" ]; then
        echo '# colcon tab completion' >> "$HOME/.bashrc"
        echo 'source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash' >> "$HOME/.bashrc"
        echo "  Added: colcon tab completion"
    else
        echo "  colcon completion already in ~/.bashrc or not installed - skipping"
    fi

    echo " "
    echo "----------------------------------------------------"
    echo "Setting ROS_DOMAIN_ID in ~/.bashrc"
    echo "  ROS_DOMAIN_ID=0 is the default domain"
    echo "  Change this if you have multiple robots on the same network"
    echo "  Each robot should use a unique domain ID (0-232)"
    echo "----------------------------------------------------"
    if ! grep -q "ROS_DOMAIN_ID" "$HOME/.bashrc"; then
        echo '# ROS2 Domain ID - change if multiple robots on same network' >> "$HOME/.bashrc"
        echo 'export ROS_DOMAIN_ID=0' >> "$HOME/.bashrc"
        echo "  Added: ROS_DOMAIN_ID=0"
    else
        echo "  ROS_DOMAIN_ID already in ~/.bashrc - skipping"
    fi

    echo " "
    echo "  Sourcing ~/.bashrc for this session..."
    source "$HOME/.bashrc" 2>/dev/null || true
    source /opt/ros/jazzy/setup.bash 2>/dev/null || true

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 9 - AUTO-SOURCE ROS2 IN .BASHRC"
    echo "----------------------------------------------------"
else
    echo "Skipping .bashrc auto-source setup"
    echo "To source manually in each terminal run:"
    echo "  source /opt/ros/jazzy/setup.bash"
fi

# ============================================================================
# STEP 10 - VERIFY ROS2 INSTALL
# Checks that ROS2 Jazzy is correctly installed and accessible.
# Verifies: ros2 CLI, demo nodes, RViz2, rqt, and key Python packages.
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 10 - VERIFY ROS2 INSTALL"
echo "  Checking ROS2 Jazzy installation and key components"
echo "============================================================"
echo "Do you wish to verify the ROS2 install? Enter y/Y or n/N"
read -p "Verify ROS2 install?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    # Source ROS2 for this verification step
    if [ -f "/opt/ros/jazzy/setup.bash" ]; then
        source /opt/ros/jazzy/setup.bash
    fi

    echo " "
    echo "----------------------------------------------------"
    echo "Checking ROS2 environment variables"
    echo "----------------------------------------------------"
    echo "  ROS_DISTRO:    ${ROS_DISTRO:-NOT SET}"
    echo "  ROS_VERSION:   ${ROS_VERSION:-NOT SET}"
    echo "  ROS_DOMAIN_ID: ${ROS_DOMAIN_ID:-NOT SET (defaults to 0)}"
    echo "  AMENT_PREFIX:  ${AMENT_PREFIX_PATH:0:50}..."

    echo " "
    echo "----------------------------------------------------"
    echo "Checking installed ROS2 packages"
    echo "----------------------------------------------------"

    check_ros_cmd() {
        local label=$1
        local cmd=$2
        printf "  %-30s " "$label:"
        if command -v "$cmd" &>/dev/null; then
            echo "FOUND - $(which $cmd)"
        else
            echo "NOT FOUND"
        fi
    }

    # check_ros_pkg uses a prefix grep against the full dpkg list rather than
    # an exact package name match. This handles ROS2 meta-packages like rqt
    # which are installed as component packages (ros-jazzy-rqt-gui, etc.)
    # rather than a single top-level package, so an exact dpkg -l match fails
    # even though rqt is fully installed. The prefix grep finds any installed
    # package whose name starts with the given string.
    check_ros_pkg() {
        local label=$1
        local pkg=$2
        printf "  %-30s " "$label:"
        if dpkg -l 2>/dev/null | grep -q "^ii.*${pkg}"; then
            local match
            match=$(dpkg -l 2>/dev/null | grep "^ii.*${pkg}" | head -1 | awk '{print $2}')
            echo "INSTALLED ($match)"
        else
            echo "NOT FOUND"
        fi
    }

    check_ros_cmd "ros2 CLI"           ros2
    check_ros_cmd "colcon"             colcon
    check_ros_cmd "rosdep"             rosdep
    check_ros_cmd "rviz2"              rviz2
    check_ros_cmd "rqt"                rqt

    echo " "
    check_ros_pkg "ros-jazzy-desktop"          ros-jazzy-desktop
    check_ros_pkg "ros-jazzy-demo-nodes-cpp"   ros-jazzy-demo-nodes-cpp
    check_ros_pkg "ros-jazzy-demo-nodes-py"    ros-jazzy-demo-nodes-py
    check_ros_pkg "ros-jazzy-rviz2"            ros-jazzy-rviz2
    # ros-jazzy-rqt is a meta-package installed as component packages
    # (ros-jazzy-rqt-gui, ros-jazzy-rqt-common-plugins, etc.)
    # Prefix search finds any installed ros-jazzy-rqt-* package
    check_ros_pkg "ros-jazzy-rqt"              ros-jazzy-rqt
    check_ros_pkg "ros-dev-tools"              ros-dev-tools

    echo " "
    echo "----------------------------------------------------"
    echo "Checking ROS2 Python packages"
    echo "----------------------------------------------------"
    check_py() {
        local label=$1
        local mod=$2
        printf "  %-30s " "$label:"
        python3 -c "import $mod" 2>/dev/null && echo "OK" || echo "NOT FOUND"
    }
    check_py "rclpy"      rclpy
    check_py "std_msgs"   std_msgs.msg

    echo " "
    echo "----------------------------------------------------"
    echo "ros2 --version output:"
    ros2 --version 2>/dev/null || echo "  ros2 not found - source /opt/ros/jazzy/setup.bash first"
    echo "----------------------------------------------------"

    echo " "
    echo "----------------------------------------------------"
    echo "Done: STEP 10 - VERIFY ROS2 INSTALL"
    echo "----------------------------------------------------"
else
    echo "Skipping ROS2 verification"
fi

# ============================================================================
# STEP 11 - TALKER / LISTENER TEST
# The classic ROS2 hello-world test. Runs a C++ talker node that publishes
# "Hello World: N" messages, and a Python listener node that subscribes and
# prints them. If both work, your ROS2 install is confirmed good.
#
# Because this requires TWO simultaneous terminal sessions, this step:
#   1. Prints the exact commands to run in each terminal
#   2. Offers to launch the talker in the background so you can run the
#      listener in this terminal to see messages flowing
#   3. Cleans up the background talker when done
#
# Manual test commands (run in two separate terminals after install):
#   Terminal 1:  source /opt/ros/jazzy/setup.bash
#                ros2 run demo_nodes_cpp talker
#
#   Terminal 2:  source /opt/ros/jazzy/setup.bash
#                ros2 run demo_nodes_py listener
# ============================================================================
echo " "
echo "============================================================"
echo "STEP 11 - TALKER / LISTENER TEST"
echo "  The classic ROS2 hello-world verification test"
echo "  A C++ talker publishes messages; a Python listener receives them"
echo " "
echo "  TO RUN MANUALLY (recommended - open two terminals):"
echo " "
echo "  TERMINAL 1 (Talker - C++ node):"
echo "    source /opt/ros/jazzy/setup.bash"
echo "    ros2 run demo_nodes_cpp talker"
echo " "
echo "  TERMINAL 2 (Listener - Python node):"
echo "    source /opt/ros/jazzy/setup.bash"
echo "    ros2 run demo_nodes_py listener"
echo " "
echo "  You should see:"
echo "    Talker:   [INFO] Publishing: 'Hello World: 1'"
echo "    Listener: [INFO] I heard: [Hello World: 1]"
echo " "
echo "  Press Ctrl+C in each terminal to stop the nodes."
echo "============================================================"
echo "Run automated talker/listener test in THIS terminal? Enter y/Y or n/N"
echo "(Runs talker in background for 10 seconds, listener in foreground)"
read -p "Run automated test?: " yesInstall

if [ "$yesInstall" == "y" ] || [ "$yesInstall" == "Y" ]; then

    # Source ROS2 for this step
    if [ -f "/opt/ros/jazzy/setup.bash" ]; then
        source /opt/ros/jazzy/setup.bash
    else
        echo "  ERROR: /opt/ros/jazzy/setup.bash not found"
        echo "  ROS2 may not be installed - check Step 6"
        echo "  Skipping talker/listener test"
    fi

    if [ -n "$ROS_DISTRO" ]; then

        echo " "
        echo "----------------------------------------------------"
        echo "Starting C++ talker node in background..."
        echo "----------------------------------------------------"
        ros2 run demo_nodes_cpp talker &
        TALKER_PID=$!
        echo "  Talker PID: $TALKER_PID"

        sleep 2

        echo " "
        echo "----------------------------------------------------"
        echo "Starting Python listener node (runs for 8 seconds)..."
        echo "Press Ctrl+C to stop early"
        echo "----------------------------------------------------"
        echo " "

        # Run listener with a timeout so the script doesn't hang forever
        timeout 8 ros2 run demo_nodes_py listener || true

        echo " "
        echo "----------------------------------------------------"
        echo "Stopping talker node (PID: $TALKER_PID)..."
        echo "----------------------------------------------------"
        kill $TALKER_PID 2>/dev/null || true
        wait $TALKER_PID 2>/dev/null || true
        echo "  Talker stopped"

        echo " "
        echo "----------------------------------------------------"
        echo "Talker/Listener Test Complete"
        echo "  If you saw 'Publishing' and 'I heard' messages above"
        echo "  then ROS2 Jazzy is working correctly on your Pi 5!"
        echo "----------------------------------------------------"

    else
        echo "  ROS2 environment not set - cannot run test"
        echo "  Open a new terminal and run the manual test commands above"
    fi

else
    echo " "
    echo "Skipping automated test"
    echo " "
    echo "  Run the test manually using two terminals:"
    echo " "
    echo "  TERMINAL 1:"
    echo "    source /opt/ros/jazzy/setup.bash"
    echo "    ros2 run demo_nodes_cpp talker"
    echo " "
    echo "  TERMINAL 2:"
    echo "    source /opt/ros/jazzy/setup.bash"
    echo "    ros2 run demo_nodes_py listener"
fi

# ============================================================================
# FINAL - FINAL UPDATE, UPGRADE, AND AUTOREMOVE
# Runs a final apt-get update, upgrade, and autoremove after all ROS2
# packages are installed. This ensures:
#   - Any packages pulled in by ROS2 are fully up to date
#   - Any newly orphaned packages are cleaned up
#   - The system is in a clean consistent state after the full install
#   - The final state of all packages is captured in the log
# ============================================================================
echo " "
echo "============================================================"
echo "FINAL - FINAL UPDATE, UPGRADE, AND AUTOREMOVE"
echo "  Running final system cleanup after all ROS2 steps"
echo "  Ensures system is in a clean consistent state"
echo "  This output is captured in: $LOG_FILE"
echo "============================================================"
echo " "

echo "----------------------------------------------------"
echo "FINAL 1/3: sudo apt-get update"
echo "  Refreshing package lists after ROS2 install"
echo "  Started: $(date)"
echo "----------------------------------------------------"
sudo apt-get update
echo "  Completed: $(date)"

echo " "
echo "----------------------------------------------------"
echo "FINAL 2/3: sudo apt-get upgrade -y"
echo "  Applying any remaining upgrades after ROS2 install"
echo "  Started: $(date)"
echo "----------------------------------------------------"
sudo apt-get upgrade -y
echo "  Completed: $(date)"

echo " "
echo "----------------------------------------------------"
echo "FINAL 3/3: sudo apt-get autoremove -y"
echo "  Cleaning up any orphaned packages"
echo "  Started: $(date)"
echo "----------------------------------------------------"
sudo apt-get autoremove -y
echo "  Completed: $(date)"

echo " "
echo "----------------------------------------------------"
echo "Done: FINAL UPDATE, UPGRADE, AND AUTOREMOVE"
echo "  System is fully updated and clean"
echo "  Total install log: $LOG_FILE"
echo "  Log size: $(wc -l < "$LOG_FILE") lines"
echo "----------------------------------------------------"
echo " "

# ============================================================================
# DONE BANNER
# ============================================================================
echo " "
echo "  ____    __  _  _  ____  "
echo " (  _ \  /  \( \( )( ___) "
echo "  )(_) )( () ))  (  )__)  "
echo " (____/  \__/(_)\_)(____)  "
echo " "
echo "============================================================"
echo "  Done: ROS2 Jazzy Desktop Install - Rev 0.03"
echo " "
echo "  INSTALL LOG:"
echo "  $LOG_FILE"
echo "  Lines logged: $(wc -l < "$LOG_FILE")"
echo "  To review: cat $LOG_FILE | less"
echo "  To search: grep ERROR $LOG_FILE"
echo " "
echo "  WHAT WAS INSTALLED:"
echo "    /opt/ros/jazzy/         - ROS2 Jazzy installation"
echo "    ros-jazzy-desktop       - Full desktop with RViz2 and rqt"
echo "    ros-dev-tools           - colcon, rosdep, vcstool"
echo " "
echo "  EVERY NEW TERMINAL needs ROS2 sourced."
echo "  If Step 9 was run, this happens automatically."
echo "  To source manually: source /opt/ros/jazzy/setup.bash"
echo " "
echo "  QUICK REFERENCE - COMMON ROS2 COMMANDS:"
echo "    ros2 run <pkg> <node>          Run a node"
echo "    ros2 topic list                List all active topics"
echo "    ros2 topic echo /chatter       Print messages on a topic"
echo "    ros2 node list                 List all running nodes"
echo "    ros2 pkg list | grep jazzy     List installed Jazzy packages"
echo "    rviz2                          Launch 3D visualizer"
echo "    rqt                            Launch GUI toolkit"
echo "    colcon build                   Build a ROS2 workspace"
echo "    rosdep install --from-paths src --ignore-src -y"
echo "                                   Install workspace dependencies"
echo " "
echo "  TALKER / LISTENER TEST (two terminals):"
echo "    Terminal 1: ros2 run demo_nodes_cpp talker"
echo "    Terminal 2: ros2 run demo_nodes_py listener"
echo " "
echo "  NEXT STEPS:"
echo "    1. Open a NEW terminal (auto-sources ROS2 from .bashrc)"
echo "    2. Run: ros2 run demo_nodes_cpp talker"
echo "    3. Open another terminal and run: ros2 run demo_nodes_py listener"
echo "    4. Try: ros2 topic list  and  ros2 node list"
echo "    5. Launch RViz2 with: rviz2"
echo "    6. Explore tutorials: https://docs.ros.org/en/jazzy/Tutorials.html"
echo "============================================================"