#!/usr/bin/env bash
set -euo pipefail

# Defaults
ACTION_NAME="/ArmSpaceControl"
FRAME_ID="base_link"
X=0.5
Y=0.0
Z=0.3
YAW_DEG=0.0
ANGLE_UNIT="deg"   # deg|rad
DRY_RUN=0
LOG_LEVEL="info"

usage(){
  cat <<EOF
Usage: ${0##*/} [options]

Options:
  -a, --action NAME     Action name (default: /arm_space_control)
  -f, --frame ID        Target frame_id (default: base_link)
  --x VALUE             Position x (m) (default: 0.5)
  --y VALUE             Position y (m) (default: 0.0)
  --z VALUE             Position z (m) (default: 0.3)
  --yaw VALUE           Yaw angle (default: 0.0)
  --rad                 Interpret yaw as radians (default is degrees)
  --deg                 Interpret yaw as degrees (default)
  --log-level LEVEL     ROS 2 log level (default: info)
  --dry-run             Print goal YAML and exit
  -h, --help            Show this help

Examples:
  ${0##*/} --x 0.5 --y 0.0 --z 0.3 --yaw 90 --deg --frame base_link
  ${0##*/} --action /ArmSpaceControl --yaw 1.57 --rad
EOF
}

# Parse args
while [[ $# -gt 0 ]]; do
  case "$1" in
    -a|--action) ACTION_NAME="$2"; shift 2 ;;
    -f|--frame) FRAME_ID="$2"; shift 2 ;;
    --x) X="$2"; shift 2 ;;
    --y) Y="$2"; shift 2 ;;
    --z) Z="$2"; shift 2 ;;
    --yaw) YAW_DEG="$2"; shift 2 ;;
    --rad) ANGLE_UNIT="rad"; shift ;;
    --deg) ANGLE_UNIT="deg"; shift ;;
    --log-level) LOG_LEVEL="$2"; shift 2 ;;
    --dry-run) DRY_RUN=1; shift ;;
    -h|--help) usage; exit 0 ;;
    *) echo "Unknown option: $1" >&2; usage; exit 2 ;;
  esac
done

# Compute quaternion from yaw only (roll=pitch=0)
# qx=qy=0, qz=sin(yaw/2), qw=cos(yaw/2)
python_quat='import math,sys; yaw=float(sys.argv[1]); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")'

if [[ "$ANGLE_UNIT" == "deg" ]]; then
  # Convert degrees to radians in Python for accuracy
  read -r QZ QW < <(python3 -c 'import math,sys; yaw_deg=float(sys.argv[1]); yaw=math.radians(yaw_deg); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")' "$YAW_DEG")
else
  read -r QZ QW < <(python3 -c "$python_quat" "$YAW_DEG")
fi

# Build YAML (avoid shell-escaping issues; no quotes around frame_id)
GOAL_YAML=$(cat <<YAML
{target: {header: {frame_id: ${FRAME_ID}}, pose: {position: {x: ${X}, y: ${Y}, z: ${Z}}, orientation: {x: 0.0, y: 0.0, z: ${QZ}, w: ${QW}}}}}
YAML
)

if (( DRY_RUN )); then
  echo "Will send to action: ${ACTION_NAME}"
  echo "Goal YAML:"
  echo "$GOAL_YAML"
  exit 0
fi

# Source workspace
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
WS_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
if [[ -f "${WS_ROOT}/install/setup.bash" ]]; then
  # shellcheck source=/dev/null
  source "${WS_ROOT}/install/setup.bash"
else
  echo "install/setup.bash not found at ${WS_ROOT}/install/setup.bash" >&2
  exit 1
fi

# Send goal
set -x
ros2 action send_goal --feedback "${ACTION_NAME}" interfaces/action/ArmSpaceControl "$GOAL_YAML" --ros-args --log-level "${LOG_LEVEL}"
set +x
