#!/bin/bash
# ============================================================================
#  Saqr PPE Detection - Run on Unitree G1 Robot
# ============================================================================
#
#  Run on the robot's physical terminal (with monitor):
#    ./run_robot.sh
#    ./run_robot.sh --headless            # no display
#    ./run_robot.sh --source realsense    # use pyrealsense2 SDK
#
# ============================================================================

set -e

SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
cd "$SCRIPT_DIR"

# ── Defaults ──────────────────────────────────────────────────────────────────
SOURCE="realsense"
MODEL="models/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

# ── Parse args ────────────────────────────────────────────────────────────────
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

# ── Check model ───────────────────────────────────────────────────────────────
if [ ! -f "$MODEL" ]; then
    echo "[ERROR] Model not found: $MODEL"
    echo "  Deploy from dev machine: ./deploy.sh"
    exit 1
fi

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

# ── Fix clock if needed ──────────────────────────────────────────────────────
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

# ── Setup display ─────────────────────────────────────────────────────────────
if [ "$HEADLESS" = true ]; then
    export QT_QPA_PLATFORM=offscreen
    HEADLESS_FLAG="--headless"
    echo "Mode: HEADLESS (no display, results saved to 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 "  Camera : RealSense D435I"
echo "============================================"
echo ""
echo "  Press q to quit, s to save frame."
echo ""

# ── Run ───────────────────────────────────────────────────────────────────────
python saqr.py \
    --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