#!/usr/bin/env bash
# Convert Flexiv Elements .traj → arm_application YAML (standard entry for all trajectories).
# Usage:
#   ./scripts/convert_traj.sh ~/文档/my_task.traj
#   ./scripts/convert_traj.sh ~/文档/my_task.traj -o /path/to/out.yaml
#   ./scripts/convert_traj.sh ~/文档/my_task.traj --min-seg 2.5 --no-copy
set -eo pipefail

ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PY="${ROOT}/scripts/convert_flexiv_traj.py"

if [[ $# -lt 1 ]]; then
  echo "Usage: $0 INPUT.traj [-o OUT.yaml] [--min-seg SECONDS] [--no-copy]"
  echo "Example: $0 ~/文档/my_task.traj"
  exit 1
fi

INPUT="$(realpath "$1")"
shift
BASENAME="$(basename "${INPUT}" .traj)"
OUT_DEFAULT="${ROOT}/src/arm_application/config/${BASENAME}_trajectory.yaml"
TRAJ_COPY="${ROOT}/src/arm_application/trajectories/${BASENAME}.traj"

OUT="${OUT_DEFAULT}"
MIN_SEG="3.0"
DO_COPY=1
EXTRA=()

while [[ $# -gt 0 ]]; do
  case "$1" in
    -o|--output)
      OUT="$(realpath -m "$2")"
      shift 2
      ;;
    --min-seg)
      MIN_SEG="$2"
      shift 2
      ;;
    --no-copy)
      DO_COPY=0
      shift
      ;;
    *)
      EXTRA+=("$1")
      shift
      ;;
  esac
done

if [[ ! -f "${INPUT}" ]]; then
  echo "ERROR: input not found: ${INPUT}" >&2
  exit 1
fi

python3 "${PY}" "${INPUT}" -o "${OUT}" --min-seg "${MIN_SEG}" "${EXTRA[@]}"

if [[ "${DO_COPY}" -eq 1 ]]; then
  mkdir -p "$(dirname "${TRAJ_COPY}")"
  cp -f "${INPUT}" "${TRAJ_COPY}"
  echo "Copied source traj → ${TRAJ_COPY}"
fi

echo ""
echo "Next (stack must be running):"
echo "  conda activate ros_flexiv"
echo "  source ${ROOT}/install/setup.bash"
echo "  export LD_LIBRARY_PATH=~/flexiv_install/lib:\$LD_LIBRARY_PATH"
echo "  ros2 run arm_application run_joint_trajectory --ros-args \\"
echo "    --params-file ${OUT}"