#!/bin/bash
# ============================================================================
#  start_saqr.sh — boot launcher for the Saqr/G1 bridge.
# ============================================================================
#
#  What it does:
#    1. Sources miniconda and activates the `marcus` env (which has
#       unitree_sdk2py + ultralytics + pyrealsense2 installed).
#    2. cd ~/Saqr
#    3. Execs saqr_g1_bridge.py with the production flags. The bridge will:
#         - init the G1 arm + audio + LowState DDS clients
#         - announce "Saqr is running. Press R2 plus X to start." via TtsMaker
#         - sit idle until R2+X is pressed
#         - if R2+X is pressed but no camera is plugged, announce
#           "Camera not connected. Please plug in the camera and try again."
#           and stay idle ready for the next press
#
#  Designed to be run by systemd at boot — see saqr-bridge.service.
#  Can also be run manually:  ~/Saqr/start_saqr.sh
# ============================================================================

set -u

# ── Config (override via env if needed) ──────────────────────────────────────
SAQR_DIR="${SAQR_DIR:-$HOME/Saqr}"
CONDA_ROOT="${CONDA_ROOT:-$HOME/miniconda3}"
CONDA_ENV="${CONDA_ENV:-marcus}"
DDS_IFACE="${DDS_IFACE:-eth0}"
SAQR_SOURCE="${SAQR_SOURCE:-realsense}"
STREAM_PORT="${STREAM_PORT:-8080}"

# ── Sanity checks ────────────────────────────────────────────────────────────
if [ ! -d "$SAQR_DIR" ]; then
    echo "[start_saqr] FATAL: SAQR_DIR not found: $SAQR_DIR" >&2
    exit 1
fi

if [ ! -f "$SAQR_DIR/saqr_g1_bridge.py" ]; then
    echo "[start_saqr] FATAL: saqr_g1_bridge.py not found in $SAQR_DIR" >&2
    exit 1
fi

if [ ! -f "$CONDA_ROOT/etc/profile.d/conda.sh" ]; then
    echo "[start_saqr] FATAL: conda not found at $CONDA_ROOT" >&2
    exit 1
fi

# ── Activate conda ───────────────────────────────────────────────────────────
# shellcheck disable=SC1091
source "$CONDA_ROOT/etc/profile.d/conda.sh"
conda activate "$CONDA_ENV" || {
    echo "[start_saqr] FATAL: failed to activate conda env: $CONDA_ENV" >&2
    exit 1
}

cd "$SAQR_DIR" || {
    echo "[start_saqr] FATAL: cd $SAQR_DIR failed" >&2
    exit 1
}

echo "[start_saqr] env=$CONDA_ENV cwd=$PWD iface=$DDS_IFACE source=$SAQR_SOURCE stream=$STREAM_PORT"
echo "[start_saqr] launching bridge..."

# Exec so this script's PID is replaced by the bridge — systemd then
# tracks the bridge directly and signals reach Python correctly.
exec python3 saqr_g1_bridge.py \
    --iface "$DDS_IFACE" \
    --source "$SAQR_SOURCE" \
    --headless \
    -- --stream "$STREAM_PORT"