#!/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"

# ── Clear competing voice/audio processes ───────────────────────────────────
# The G1 firmware voice service (TtsMaker / PlayStream) goes into a
# "busy" state — returning rc=3104 or silently dropping audio — when
# another process on the robot holds the audio channel. Known offenders
# on this robot: sanad_voice.service, sanad_webserver, marcus_voice,
# and stale saqr bridge/saqr_cli instances left by previous runs.
# Kill them all before starting so the firmware starts from a clean
# state. All commands are best-effort (|| true) and non-interactive
# (sudo -n) so they never block.
echo "[start_saqr] clearing competing voice/audio processes..."
killed=0
for svc in sanad_voice.service sanad_webserver.service marcus_voice.service; do
    if sudo -n systemctl stop  "$svc" >/dev/null 2>&1; then
        echo "[start_saqr]   stopped systemd unit: $svc"
        killed=1
    fi
done
for pattern in sanad_voice sanad_webserver marcus_voice gemini_voice \
               'python.*robot\.bridge' 'python.*apps\.saqr_cli'; do
    if pkill -f "$pattern" >/dev/null 2>&1; then
        echo "[start_saqr]   killed: $pattern"
        killed=1
    fi
done
if [ "$killed" -eq 1 ]; then
    # Give the firmware a beat to release the voice RPC service.
    sleep 1
fi

echo "[start_saqr] launching bridge..."

exec python3 -m robot.bridge \
    --iface "$DDS_IFACE" \
    --source "$SAQR_SOURCE" \
    --headless \
    -- --stream "$STREAM_PORT"