#!/usr/bin/env bash
# Run N grape->box pick-and-place episodes with varied grape & box positions, logging a
# per-episode SUCCESS/FAIL, then (optionally) concatenate into one labelled video.
#
# Usage:   OUT=/path/to/out  bash scripts/run_grape_episodes.sh
# Env:     OUT      output dir for ep*.mp4 + ep_results.txt   (default /tmp/yam_grape_eps)
#          CONDA_SH path to conda profile.d/conda.sh          (default ~/miniconda3/...)
set -e
REPO="$(cd "$(dirname "$0")/.." && pwd)"
OUT="${OUT:-/tmp/yam_grape_eps}"; mkdir -p "$OUT"
CONDA_SH="${CONDA_SH:-$HOME/miniconda3/etc/profile.d/conda.sh}"
source "$CONDA_SH"; conda activate env_isaaclab
export OMNI_KIT_ACCEPT_EULA=YES OMNI_KIT_ALLOW_ROOT=1 PYTHONUNBUFFERED=1 CUDA_HOME=$CONDA_PREFIX
cd "$REPO"

# grape (x,y) and box (x,y) per episode, env-local metres. Use "x,y"; negatives are fine.
# grape grasp is reliable near [0, 0.08..0.10] (root dist <= ~0.37); farther spots fail on purpose.
GX=("0.00,0.10" "-0.03,0.10" "0.00,0.08" "-0.03,0.10" "0.00,0.11" "-0.04,0.09" "0.02,0.10" "-0.02,0.11" "0.04,0.09" "-0.05,0.10")
BX=("0.06,-0.26" "-0.02,-0.24" "0.12,-0.22" "0.00,-0.28" "0.08,-0.25" "-0.04,-0.26" "0.10,-0.23" "0.02,-0.27" "-0.02,-0.25" "0.06,-0.28")
N=${#GX[@]}

> "$OUT/ep_results.txt"
for ((i=0;i<N;i++)); do
  echo "===== EPISODE $i  grape=${GX[$i]}  box=${BX[$i]} ====="
  for try in 1 2; do   # retry once (Isaac kernel bootstrap is occasionally flaky after a kill)
    for p in $(nvidia-smi --query-compute-apps=pid --format=csv,noheader 2>/dev/null); do kill -9 "$p" 2>/dev/null || true; done
    sleep 5
    timeout 900 python scripts/yam_grasp_prm.py --headless --obj grape --basket \
      --obj_xy="${GX[$i]}" --box_xy="${BX[$i]}" --episode "$i" \
      --video "$OUT/ep$i.mp4" > "$OUT/ep$i.log" 2>&1
    grep -q EPISODE_RESULT "$OUT/ep$i.log" && break
  done
  echo "EP$i grape=${GX[$i]} box=${BX[$i]} :: $(grep EPISODE_RESULT "$OUT/ep$i.log" | head -1)" | tee -a "$OUT/ep_results.txt"
done
echo "ALL_DONE  ($N episodes) -> $OUT"
echo "Concatenate with:  OUT=$OUT python scripts/concat_episodes.py"