#!/usr/bin/env bash
# Measure the actual velocity response of the Webots mecanum robot and
# compare against the gym's first-order kinematics prediction.
#
# Uses HERDING_MODE=calibrate in the shepherd_dog controller, which applies
# a known fixed action for N steps, records GPS displacement, and computes
# the relative error vs gym prediction.
#
# Usage:
#   bash tools/calibrate_mecanum.sh [N_STEPS]
#     N_STEPS : steps to hold each action (default 150, ≈ 2.4 s real-time)
#
# Output:
#   calibrate_mecanum.log  — per-axis results printed and written here
#
# Target: < 10% relative error on each axis.
# If errors are high, tune coulombFriction / forceDependentSlip in
# tools/run_webots.sh (mecanum contactProperties block).

set -euo pipefail
N_STEPS="${1:-150}"
ROOT="$( cd "$( dirname "${BASH_SOURCE[0]}" )/.." && pwd )"
LOG="$ROOT/calibrate_mecanum.log"
export PATH="/home/jalf/miniconda3/envs/tir/bin:$PATH"

echo "Running mecanum calibration (N_STEPS=$N_STEPS)..."
echo "Results will be written to: $LOG"
truncate -s 0 "$LOG"

run_calib() {
    local vx="$1" vy="$2" om="$3"
    echo "  Testing vx=$vx vy=$vy om=$om ..."
    rm -f "$ROOT/training/.run_done"
    timeout --kill-after=15s 60 \
    xvfb-run -a \
    env WEBOTS_HEADLESS=1 WEBOTS_EXTRA_ARGS="--stdout --stderr" \
        HERDING_MODE=calibrate HERDING_DRIVE=mecanum HERDING_WORLD=field \
        CALIB_VX="$vx" CALIB_VY="$vy" CALIB_OM="$om" \
        CALIB_N_STEPS="$N_STEPS" \
    bash "$ROOT/tools/run_webots.sh" 0 calibrate mecanum field \
    2>&1 | grep -E "cmd=|gym|webots|error" || true
    pkill -9 -f "webots-bin|Xvfb" 2>/dev/null || true
    sleep 1
}

# Three test vectors: pure-x, pure-y, diagonal
run_calib 0.5  0.0 0.0
run_calib 0.0  0.5 0.0
run_calib 0.35 0.35 0.0

echo ""
echo "=== Calibration results ==="
cat "$LOG" 2>/dev/null || echo "(no results written — check controller output above)"
echo ""
echo "Target: <10% error on each axis."
echo "If errors are high, tune coulombFriction / forceDependentSlip in"
echo "tools/run_webots.sh (mecanum contactProperties block)."