#!/bin/bash
# ============================================================================
#  start_saqr.sh — boot launcher for the Saqr / G1 bridge.
# ============================================================================
#
#  What it does:
#    1. Sources miniconda and activates the target env (default: marcus).
#    2. cd to the project root (parent of this scripts/ dir).
#    3. Execs `python -m robot.bridge` 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
#
#  Designed to be run by systemd at boot — see saqr-bridge.service.
#  Can also be run manually: scripts/start_saqr.sh
# ============================================================================

set -u

HERE="$(cd "$(dirname "$0")" && pwd)"
SAQR_DIR="${SAQR_DIR:-$(cd "$HERE/.." && pwd)}"

# Read defaults from config/robot_config.json (env vars override).
config_get() {
    # config_get dotted.key
    python3 -c "
import json, os, sys
with open('$SAQR_DIR/config/robot_config.json') as f:
    c = json.load(f)
for k in sys.argv[1].split('.'):
    c = c[k]
print(os.path.expandvars(str(c)))
" "$1"
}

CONDA_ROOT="${CONDA_ROOT:-$(config_get start_saqr.conda_root)}"
CONDA_ENV="${CONDA_ENV:-$(config_get start_saqr.conda_env)}"
DDS_IFACE="${DDS_IFACE:-$(config_get start_saqr.dds_iface)}"
SAQR_SOURCE="${SAQR_SOURCE:-$(config_get start_saqr.saqr_source)}"
STREAM_PORT="${STREAM_PORT:-$(config_get start_saqr.stream_port)}"

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

if [ ! -f "$SAQR_DIR/robot/bridge.py" ]; then
    echo "[start_saqr] FATAL: robot/bridge.py not found in $SAQR_DIR" >&2
    echo "                    (expected $SAQR_DIR to contain core/ apps/ robot/ utils/)" >&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

# 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 python3 -m robot.bridge \
    --iface "$DDS_IFACE" \
    --source "$SAQR_SOURCE" \
    --headless \
    -- --stream "$STREAM_PORT"