#!/bin/bash
# ============================================================================
#  Saqr PPE Detection - Run on Unitree G1 Robot (no bridge, direct saqr run)
# ============================================================================
#
#  Run on the robot's physical terminal (with monitor) or via ssh -X:
#    scripts/run_robot.sh
#    scripts/run_robot.sh --headless              # no display
#    scripts/run_robot.sh --source /dev/video2    # V4L2 fallback
#
#  For the production R2+X / R2+Y workflow, use scripts/start_saqr.sh instead.
# ============================================================================

set -e

HERE="$(cd "$(dirname "$0")" && pwd)"
PROJECT_ROOT="$(cd "$HERE/.." && pwd)"
cd "$PROJECT_ROOT"

SOURCE="realsense"
MODEL="saqr_best.pt"
CONF="0.35"
HEADLESS=false
MAX_MISSING=120
MATCH_DIST=300
CONFIRM=7
DEVICE="0"
IMGSZ=320
HALF=true
STREAM_PORT=0

while [[ $# -gt 0 ]]; do
    case $1 in
        --source)         SOURCE="$2"; shift 2 ;;
        --model)          MODEL="$2"; shift 2 ;;
        --conf)           CONF="$2"; shift 2 ;;
        --headless)       HEADLESS=true; shift ;;
        --max-missing)    MAX_MISSING="$2"; shift 2 ;;
        --match-distance) MATCH_DIST="$2"; shift 2 ;;
        --confirm)        CONFIRM="$2"; shift 2 ;;
        --device)         DEVICE="$2"; shift 2 ;;
        --imgsz)          IMGSZ="$2"; shift 2 ;;
        --no-half)        HALF=false; shift ;;
        --stream)         STREAM_PORT="$2"; shift 2 ;;
        --cpu)            DEVICE="cpu"; HALF=false; shift ;;
        *)                echo "Unknown arg: $1"; exit 1 ;;
    esac
done

source ~/miniconda3/etc/profile.d/conda.sh 2>/dev/null || true
conda activate marcus 2>/dev/null || conda activate teleimager 2>/dev/null || true

YEAR=$(date +%Y)
if [ "$YEAR" -lt 2025 ]; then
    echo "[WARN] System clock is wrong (year=$YEAR). Fixing..."
    echo "123" | sudo -S date -s "2026-04-10 16:00:00" 2>/dev/null || true
fi

if [ "$HEADLESS" = true ]; then
    export QT_QPA_PLATFORM=offscreen
    HEADLESS_FLAG="--headless"
    echo "Mode: HEADLESS (no display, results saved to runtime/captures/)"
else
    xhost + >/dev/null 2>&1 || true
    export DISPLAY=:0
    HEADLESS_FLAG=""
    echo "Mode: DISPLAY (OpenCV window on monitor)"
fi

HALF_FLAG=""
if [ "$HALF" = true ]; then
    HALF_FLAG="--half"
fi

STREAM_FLAG=""
if [ "$STREAM_PORT" -gt 0 ]; then
    STREAM_FLAG="--stream $STREAM_PORT"
fi

echo "============================================"
echo "  Saqr PPE Detection - Unitree G1 Robot"
echo "============================================"
echo "  Source : $SOURCE"
echo "  Model  : $MODEL"
echo "  Device : $DEVICE (half=$HALF, imgsz=$IMGSZ)"
echo "  Conf   : $CONF"
echo "  Stream : ${STREAM_PORT:-disabled}"
echo "============================================"
echo ""

python -m saqr.apps.saqr_cli \
    --source "$SOURCE" \
    --model "$MODEL" \
    --conf "$CONF" \
    --max-missing "$MAX_MISSING" \
    --match-distance "$MATCH_DIST" \
    --status-confirm-frames "$CONFIRM" \
    --device "$DEVICE" \
    --imgsz "$IMGSZ" \
    $HALF_FLAG \
    $STREAM_FLAG \
    $HEADLESS_FLAG