# FROM isaac_ros_dev-aarch64
# This dockerfile can be configured via --build-arg
# Build context must be the /navigation2 root folder for COPY.
# Example build command:
# export UNDERLAY_MIXINS="debug ccache lld"
# export OVERLAY_MIXINS="debug ccache coverage-gcc lld"
# docker build -t nav2:latest \
#   --build-arg UNDERLAY_MIXINS \
#   --build-arg OVERLAY_MIXINS ./
ARG FROM_IMAGE=ros:rolling
ARG UNDERLAY_WS=/opt/underlay_ws
ARG OVERLAY_WS=/opt/overlay_ws

# multi-stage for caching
FROM $FROM_IMAGE AS cacher

# clone underlay source
ARG UNDERLAY_WS
WORKDIR $UNDERLAY_WS/src
# Clone the repositories
RUN cd /home && mkdir nav2_ws && cd nav2_ws && mkdir src && cd src && \
    git clone -b master https://github.com/BehaviorTree/BehaviorTree.CPP.git && \
    git clone -b ros2 https://github.com/ros/angles.git && \
    git clone -b rolling https://github.com/ros-perception/vision_opencv.git && \
    git clone -b ros2 https://github.com/ros/bond_core.git && \
    git clone -b ros2 https://github.com/ros/diagnostics.git && \
    git clone -b ros2 https://github.com/ros-geographic-info/geographic_info.git && \
    git clone -b main https://github.com/ompl/ompl.git && \
    git clone -b ros2 https://github.com/cra-ros-pkg/robot_localization.git && \
    git clone -b 1.0.1 https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git

# copy overlay source
ARG OVERLAY_WS
WORKDIR $OVERLAY_WS/src
RUN git clone https://github.com/ros-navigation/navigation2.git && git clone https://github.com/SteveMacenski/slam_toolbox.git -b ros2 

# copy manifests for caching
WORKDIR /opt
RUN find . -name "src" -type d \
      -mindepth 1 -maxdepth 2 -printf '%P\n' \
      | xargs -I % mkdir -p /tmp/opt/% && \
    find . -name "package.xml" \
      | xargs cp --parents -t /tmp/opt && \
    find . -name "COLCON_IGNORE" \
      | xargs cp --parents -t /tmp/opt || true

# multi-stage for building
FROM $FROM_IMAGE AS builder

# config dependencies install
ARG DEBIAN_FRONTEND=noninteractive
RUN echo '\
APT::Install-Recommends "0";\n\
APT::Install-Suggests "0";\n\
' > /etc/apt/apt.conf.d/01norecommend
ENV PYTHONUNBUFFERED 1

# install CI dependencies
ARG RTI_NC_LICENSE_ACCEPTED=yes
RUN apt-get update && \
    apt-get upgrade -y --with-new-pkgs && \
    apt-get install -y \
      ccache \
      lcov \
      lld \
      python3-pip \
      ros-$ROS_DISTRO-rmw-fastrtps-cpp \
      ros-$ROS_DISTRO-rmw-connextdds \
      ros-$ROS_DISTRO-rmw-cyclonedds-cpp \
      ros-$ROS_DISTRO-rmw-cyclonedds-cpp\ 
      ros-$ROS_DISTRO-rosidl-generator-dds-idl \ 
    && pip3 install --break-system-packages \
      fastcov \
      git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \
      git+https://github.com/ruffsl/colcon-clean.git@a7f1074d1ebc1a54a6508625b117974f2672f2a9 \
    && rosdep update \
    && colcon mixin update \
    && colcon metadata update \
    && rm -rf /var/lib/apt/lists/*

# install underlay dependencies
ARG UNDERLAY_WS
ENV UNDERLAY_WS $UNDERLAY_WS
WORKDIR $UNDERLAY_WS
COPY --from=cacher /tmp/$UNDERLAY_WS ./
RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
    apt-get update && rosdep install -q -y \
      --from-paths src \
      --skip-keys " \
        slam_toolbox \
        " \
      --ignore-src \
    && rm -rf /var/lib/apt/lists/*

# build underlay source
COPY --from=cacher $UNDERLAY_WS ./
ARG UNDERLAY_MIXINS="release ccache lld"
ARG CCACHE_DIR="$UNDERLAY_WS/.ccache"
RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
    colcon cache lock && \
    colcon build \
      --symlink-install \
      --mixin $UNDERLAY_MIXINS \
      --event-handlers console_direct+ \
      --cmake-args -DCMAKE_BUILD_TYPE=Release

# install overlay dependencies
ARG OVERLAY_WS
ENV OVERLAY_WS $OVERLAY_WS
WORKDIR $OVERLAY_WS
COPY --from=cacher /tmp/$OVERLAY_WS ./

RUN . $UNDERLAY_WS/install/setup.sh && \
    apt-get update && rosdep install -q -y \
      --from-paths src \
      --skip-keys " \
        slam_toolbox \
        "\
      --ignore-src \
    && rm -rf /var/lib/apt/lists/*

# multi-stage for testing
FROM builder AS tester

# build overlay source
COPY --from=cacher $OVERLAY_WS ./
ARG OVERLAY_MIXINS="release ccache lld"
ARG CCACHE_DIR="$OVERLAY_WS/.ccache"
RUN . $UNDERLAY_WS/install/setup.sh && \
    colcon cache lock && \
    colcon build \
      --symlink-install \
      --mixin $OVERLAY_MIXINS \
      --cmake-args -DCMAKE_BUILD_TYPE=Release

# source overlay from entrypoint
RUN sed --in-place \
      's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
      /ros_entrypoint.sh


# copy the go2py ros2 nodes
COPY ros2_nodes/m-explore-ros2 /home/nav2_ws/src/m-explore-ros2
# COPY ros2_nodes/sportmode_nav2 /home/nav2_ws/src/sportmode_nav2
# RUN cd /home/nav2_ws && source /opt/ros/rolling/setup.bash && \
# colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install

ENTRYPOINT ["/bin/bash"]