#!/usr/bin/env bash
set -e

PROJECT="$HOME/esarobotech/r1"
INTERFACE="${1:-enp3s0}"

cd "$PROJECT"

PIDS=()

cleanup() {
    echo
    echo "[R1] Stopping all services..."

    for pid in "${PIDS[@]}"; do
        if kill -0 "$pid" 2>/dev/null; then
            kill "$pid" 2>/dev/null || true
        fi
    done

    wait 2>/dev/null || true

    echo "[R1] All services stopped."
}

trap cleanup EXIT INT TERM

echo "======================================"
echo " R1 complete stack"
echo "======================================"
echo "Interface: $INTERFACE"
echo

echo "[R1] Starting camera..."
./scripts/start_camera.sh "$INTERFACE" &
PIDS+=("$!")

sleep 1

echo "[R1] Starting YOLO..."
./scripts/start_yolo.sh &
PIDS+=("$!")

sleep 1

echo "[R1] Starting LLM..."
./scripts/start_llm.sh &
PIDS+=("$!")

echo "[R1] Waiting for LLM server..."

for i in {1..60}; do
    if curl -fsS \
        http://127.0.0.1:8080/health \
        >/dev/null 2>&1; then

        echo "[R1] LLM ready."
        break
    fi

    sleep 1

    if [[ "$i" -eq 60 ]]; then
        echo "[ERROR] LLM failed to become ready."
        exit 1
    fi
done

echo "[R1] Waiting for camera frame..."

for i in {1..20}; do
    if [[ -s /dev/shm/robot_frame.jpg ]]; then
        echo "[R1] Camera ready."
        break
    fi

    sleep 0.5

    if [[ "$i" -eq 20 ]]; then
        echo "[WARN] Camera frame not available yet."
    fi
done

echo "[R1] Starting voice assistant..."
./scripts/start_voice.sh "$INTERFACE" &
PIDS+=("$!")

echo "[R1] Starting motion..."
./scripts/start_motion.sh "$INTERFACE" &
PIDS+=("$!")

sleep 1

echo
echo "======================================"
echo " R1 stack running"
echo "======================================"
echo "Camera: /dev/shm/robot_frame.jpg"
echo "YOLO:   /dev/shm/r1_detections.json"
echo "LLM:    http://127.0.0.1:8080"
echo
echo "Press Ctrl+C to stop everything."
echo

# Wait until one service exits.
wait -n "${PIDS[@]}"

echo "[R1] A service exited; shutting down the stack."
