#!/usr/bin/env bash
#
# Run the SIM S0 stock-rover smoke case inside a loopback-only network
# namespace. The script writes factual runtime artifacts to D and never grants
# actuator authority.

set -Eeuo pipefail

readonly EXPECTED_DISTRO="MissionCore-Sim"
readonly D_ROOT="${MISSIONCORE_SIM_D_ROOT:-/mnt/d/NDC_MISSIONCORE/simulation}"
readonly PX4_ROOT="${MISSIONCORE_SIM_PX4_ROOT:-${D_ROOT}/source/PX4-Autopilot}"
readonly AGENT_PREFIX="${MISSIONCORE_SIM_AGENT_PREFIX:-${D_ROOT}/runtime/micro-xrce-dds-agent/2.4.3}"
readonly PX4_MSGS_SETUP="${MISSIONCORE_SIM_PX4_MSGS_SETUP:-${D_ROOT}/build/ros2/px4_msgs/install/setup.bash}"
readonly ARTIFACT_ROOT="${MISSIONCORE_SIM_ARTIFACT_ROOT:-${D_ROOT}/artifacts/s0}"
readonly PROFILE_PATH="${MISSIONCORE_SIM_PROFILE_PATH:-${D_ROOT}/source/qualification-profile.yaml}"
readonly TIME_PROBE="${MISSIONCORE_SIM_TIME_PROBE:-${D_ROOT}/source/gazebo_time_control.py}"
readonly RUN_ID="${1:-$(date -u +%Y%m%dT%H%M%SZ)}"
readonly SPEED_FACTOR="${2:-1}"
readonly RUN_DIR="${ARTIFACT_ROOT}/${RUN_ID}"

if [[ "${SPEED_FACTOR}" != "1" && "${SPEED_FACTOR}" != "2" ]]; then
    echo "error: speed factor must be one of the reviewed values: 1 or 2" >&2
    exit 2
fi

if [[ "${MISSIONCORE_S0_NETNS:-0}" != "1" ]]; then
    if [[ "${EUID}" -ne 0 ]]; then
        echo "error: initial worker-smoke invocation must run as root to create the network namespace" >&2
        exit 2
    fi
    if [[ "${WSL_DISTRO_NAME:-}" != "${EXPECTED_DISTRO}" ]]; then
        echo "error: expected WSL distro ${EXPECTED_DISTRO}, observed ${WSL_DISTRO_NAME:-unset}" >&2
        exit 2
    fi

    exec unshare --net -- bash -c '
        set -e
        ip link set lo up
        exec runuser -u missioncore -- env MISSIONCORE_S0_NETNS=1 "$@"
    ' bash "$0" "$@"
fi

if [[ "${WSL_DISTRO_NAME:-}" != "${EXPECTED_DISTRO}" ]]; then
    echo "error: smoke case escaped the reviewed WSL distribution" >&2
    exit 2
fi

for required_path in "${PX4_ROOT}" "${AGENT_PREFIX}" "${PX4_MSGS_SETUP}" "${ARTIFACT_ROOT}"; do
    if [[ "${required_path}" != /mnt/d/NDC_MISSIONCORE/* ]]; then
        echo "error: mutable or executable path escaped D-only root: ${required_path}" >&2
        exit 2
    fi
done

readonly AGENT_BIN="${AGENT_PREFIX}/bin/MicroXRCEAgent"
readonly AGENT_LOG="${RUN_DIR}/micro-xrce-dds-agent.log"
readonly PX4_LOG="${RUN_DIR}/px4-rover.log"
readonly BRIDGE_LOG="${RUN_DIR}/ros-gz-clock-bridge.log"
readonly TOPICS_LOG="${RUN_DIR}/ros-topics.txt"
readonly VEHICLE_STATUS_LOG="${RUN_DIR}/vehicle-status.yaml"
readonly CLOCK_LOG="${RUN_DIR}/clock.yaml"
readonly NETWORK_LOG="${RUN_DIR}/network.txt"
readonly RESOURCE_LOG="${RUN_DIR}/resource-snapshot.txt"
readonly GZ_SERVICES_LOG="${RUN_DIR}/gazebo-services.txt"
readonly GZ_TOPICS_LOG="${RUN_DIR}/gazebo-topics.txt"
readonly TIME_CONTROL_LOG="${RUN_DIR}/time-control.json"
readonly RESOURCE_BASELINE_LOG="${RUN_DIR}/resource-baseline.json"
readonly TIME_PROBE_STDOUT="${RUN_DIR}/time-probe.stdout"
readonly RESULT_LOG="${RUN_DIR}/result.env"

for required_file in \
    "${PX4_ROOT}/build/px4_sitl_default/bin/px4" \
    "${PX4_ROOT}/Tools/simulation/gz/worlds/rover.sdf" \
    "${AGENT_BIN}" \
    "/opt/ros/jazzy/setup.bash" \
    "${PX4_MSGS_SETUP}" \
    "${PROFILE_PATH}" \
    "${TIME_PROBE}"; do
    if [[ ! -e "${required_file}" ]]; then
        echo "error: required S0 runtime input is missing: ${required_file}" >&2
        exit 2
    fi
done

mkdir -p "${RUN_DIR}"

agent_pid=""
px4_pgid=""
bridge_pgid=""

stop_process_group() {
    local pgid="${1:-}"
    if [[ -z "${pgid}" ]]; then
        return
    fi
    if kill -0 -- "-${pgid}" 2>/dev/null; then
        kill -INT -- "-${pgid}" 2>/dev/null || true
        for _ in {1..20}; do
            if ! kill -0 -- "-${pgid}" 2>/dev/null; then
                return
            fi
            sleep 0.25
        done
        kill -TERM -- "-${pgid}" 2>/dev/null || true
        sleep 1
        kill -KILL -- "-${pgid}" 2>/dev/null || true
    fi
}

cleanup() {
    set +e
    exec 3>&-
    stop_process_group "${bridge_pgid}"
    stop_process_group "${px4_pgid}"
    if [[ -n "${agent_pid}" ]] && kill -0 "${agent_pid}" 2>/dev/null; then
        kill -TERM "${agent_pid}" 2>/dev/null
        wait "${agent_pid}" 2>/dev/null
    fi
}
trap cleanup EXIT INT TERM

wait_for_pattern() {
    local file="$1"
    local pattern="$2"
    local attempts="$3"
    for ((attempt = 0; attempt < attempts; attempt += 1)); do
        if grep -Fq "${pattern}" "${file}" 2>/dev/null; then
            return 0
        fi
        sleep 0.5
    done
    return 1
}

env \
    LD_LIBRARY_PATH="${AGENT_PREFIX}/lib" \
    "${AGENT_BIN}" udp4 -p 8888 -v 4 >"${AGENT_LOG}" 2>&1 &
agent_pid="$!"
sleep 1
if ! kill -0 "${agent_pid}" 2>/dev/null; then
    echo "error: Micro XRCE-DDS Agent exited during startup" >&2
    exit 1
fi

{
    echo "network_namespace=loopback-only"
    ip -brief address
    ss -lunp
} >"${NETWORK_LOG}"

coproc PX4_PROCESS {
    exec setsid env HEADLESS=1 PX4_SIM_SPEED_FACTOR="${SPEED_FACTOR}" make -C "${PX4_ROOT}" \
        px4_sitl gz_rover_ackermann >"${PX4_LOG}" 2>&1
}
px4_pgid="${PX4_PROCESS_PID}"
exec 3>&"${PX4_PROCESS[1]}"

if ! wait_for_pattern "${PX4_LOG}" "Startup script returned successfully" 120; then
    echo "error: PX4 stock rover did not reach startup completion" >&2
    exit 1
fi

sleep 3
printf '%s\n' \
    "ver all" \
    "commander status" >&3

if ! wait_for_pattern "${AGENT_LOG}" "session established" 60 \
    || ! wait_for_pattern \
        "${PX4_LOG}" \
        "successfully created rt/fmu/out/vehicle_status_v1 data writer" \
        60; then
    echo "error: PX4 uXRCE-DDS client did not connect to the loopback agent" >&2
    exit 1
fi
printf '%s\n' "uxrce_dds_client status" >&3
if ! wait_for_pattern "${PX4_LOG}" "Running, connected" 20; then
    echo "error: PX4 did not report the established uXRCE-DDS session" >&2
    exit 1
fi
{
    echo
    echo "runtime_listeners=loopback-only-netns"
    ss -lunp
} >>"${NETWORK_LOG}"
gz service -l >"${GZ_SERVICES_LOG}"
gz topic -l >"${GZ_TOPICS_LOG}"

set +u
# shellcheck source=/dev/null
source /opt/ros/jazzy/setup.bash
# shellcheck source=/dev/null
source "${PX4_MSGS_SETUP}"
set -u
export ROS_DOMAIN_ID=0
export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
unset ROS_LOCALHOST_ONLY

setsid ros2 run ros_gz_bridge parameter_bridge \
    '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock' >"${BRIDGE_LOG}" 2>&1 &
bridge_pgid="$!"

timeout 20s ros2 topic list -t >"${TOPICS_LOG}"
vehicle_status_topic="/fmu/out/vehicle_status"
if grep -Fq "/fmu/out/vehicle_status_v1" "${TOPICS_LOG}"; then
    vehicle_status_topic="/fmu/out/vehicle_status_v1"
fi
timeout 20s ros2 topic echo --once \
    "${vehicle_status_topic}" px4_msgs/msg/VehicleStatus >"${VEHICLE_STATUS_LOG}"
timeout 20s ros2 topic echo --once \
    /clock rosgraph_msgs/msg/Clock >"${CLOCK_LOG}"

python3 "${TIME_PROBE}" \
    --profile "${PROFILE_PATH}" \
    --speed-factor "${SPEED_FACTOR}" \
    --time-output "${TIME_CONTROL_LOG}" \
    --resource-output "${RESOURCE_BASELINE_LOG}" \
    --storage-root "${D_ROOT}" \
    --owned-pid "${agent_pid}" \
    --owned-pgid "${px4_pgid}" \
    --owned-pgid "${bridge_pgid}" >"${TIME_PROBE_STDOUT}"

{
    echo "sample=non-acceptance-point-snapshot"
    free -b
    ps -g "${px4_pgid}" -o pid,ppid,pgid,rss,vsz,pcpu,comm,args
    ps -p "${agent_pid}" -o pid,ppid,pgid,rss,vsz,pcpu,comm,args
    ps -g "${bridge_pgid}" -o pid,ppid,pgid,rss,vsz,pcpu,comm,args
    if command -v nvidia-smi >/dev/null; then
        nvidia-smi \
            --query-gpu=memory.used,utilization.gpu \
            --format=csv,noheader,nounits
    fi
} >"${RESOURCE_LOG}"

printf '%s\n' "shutdown" >&3
if ! wait_for_pattern "${PX4_LOG}" "Exiting NOW." 20; then
    echo "error: PX4 did not acknowledge the shutdown command" >&2
    exit 1
fi

cleanup
trap - EXIT INT TERM

px4_status="fail"
dds_status="fail"
vehicle_status="fail"
clock_status="fail"
time_control_status="fail"
resource_baseline_status="fail"
lifecycle_status="fail"

grep -Fq "world: rover, model: rover_ackermann_0" "${PX4_LOG}" && px4_status="pass"
if grep -Fq "session established" "${AGENT_LOG}" \
    && grep -Fq \
        "successfully created rt/fmu/out/vehicle_status_v1 data writer" \
        "${PX4_LOG}"; then
    dds_status="pass"
fi
grep -Fq "arming_state:" "${VEHICLE_STATUS_LOG}" && vehicle_status="pass"
grep -Fq "clock:" "${CLOCK_LOG}" && clock_status="pass"
grep -Fq '"verdict": "pass"' "${TIME_CONTROL_LOG}" && time_control_status="pass"
grep -Fq '"verdict": "pass"' "${RESOURCE_BASELINE_LOG}" \
    && resource_baseline_status="pass"
if ! pgrep -f "${PX4_ROOT}/build/px4_sitl_default/bin/px4" >/dev/null \
    && ! pgrep -f "gz sim.*rover.sdf" >/dev/null \
    && ! pgrep -f "${AGENT_BIN}" >/dev/null; then
    lifecycle_status="pass"
fi

{
    echo "run_id=${RUN_ID}"
    echo "network_scope=loopback-only-netns"
    echo "actuator_authority=false"
    echo "speed_factor=${SPEED_FACTOR}"
    echo "px4_stock_rover=${px4_status}"
    echo "uxrce_dds=${dds_status}"
    echo "ros2_vehicle_status=${vehicle_status}"
    echo "gazebo_clock=${clock_status}"
    echo "pause_step_speed=${time_control_status}"
    echo "resource_baseline=${resource_baseline_status}"
    echo "clean_lifecycle=${lifecycle_status}"
} >"${RESULT_LOG}"

if [[ "${px4_status}" != "pass" \
    || "${dds_status}" != "pass" \
    || "${vehicle_status}" != "pass" \
    || "${clock_status}" != "pass" \
    || "${time_control_status}" != "pass" \
    || "${resource_baseline_status}" != "pass" \
    || "${lifecycle_status}" != "pass" ]]; then
    echo "error: one or more S0 smoke checks failed; inspect ${RUN_DIR}" >&2
    exit 1
fi

echo "SIM S0 smoke PASS: ${RUN_DIR}"