#!/usr/bin/env bash

# CMVR ES remote motion smoke tests.
#
# Safety properties:
#   * Motion is disabled unless --execute is supplied.
#   * The current device state is read immediately before every motion group.
#   * Arm tests change J6 only or Cartesian Z only.
#   * AGV tests use low speeds and short distances.
#   * Interactive confirmation is required before each motion group unless
#     --yes is supplied explicitly.
#   * StopAll tests are separate because the current server implementation
#     calls DeviceManager::stop() and leaves all devices stopped.

set -Eeuo pipefail

ENDPOINT="${ENDPOINT:-192.168.0.28:50052}"
ARM_ID="${ARM_ID:-aubo_arm}"
AGV_ID="${AGV_ID:-src1100}"
GRPCURL_BIN="${GRPCURL_BIN:-grpcurl}"

# Conservative defaults. Arm angles are radians; Cartesian/AGV distances are
# metres; angular rates are radians per second.
ARM_J6_DELTA_RAD="${ARM_J6_DELTA_RAD:-0.01}"
ARM_STOP_J6_DELTA_RAD="${ARM_STOP_J6_DELTA_RAD:-0.03}"
ARM_JOINT_VELOCITY="${ARM_JOINT_VELOCITY:-0.05}"
ARM_STOP_JOINT_VELOCITY="${ARM_STOP_JOINT_VELOCITY:-0.01}"
ARM_JOINT_ACCELERATION="${ARM_JOINT_ACCELERATION:-0.10}"
ARM_STATIONARY_VELOCITY_RAD_S="${ARM_STATIONARY_VELOCITY_RAD_S:-0.005}"
ARM_Z_DELTA_M="${ARM_Z_DELTA_M:-0.005}"
ARM_LINEAR_VELOCITY="${ARM_LINEAR_VELOCITY:-0.01}"
ARM_LINEAR_ACCELERATION="${ARM_LINEAR_ACCELERATION:-0.02}"
ARM_SPEED_DURATION_S="${ARM_SPEED_DURATION_S:-0.25}"
ARM_BASE_FRAME="${ARM_BASE_FRAME:-}"
ARM_TCP_FRAME="${ARM_TCP_FRAME:-}"

AGV_POSE_DELTA_M="${AGV_POSE_DELTA_M:-0.05}"
AGV_NAV_SPEED="${AGV_NAV_SPEED:-0.03}"
AGV_NAV_ACCELERATION="${AGV_NAV_ACCELERATION:-0.05}"
AGV_BODY_SPEED="${AGV_BODY_SPEED:-0.02}"
AGV_VELOCITY_DURATION_S="${AGV_VELOCITY_DURATION_S:-0.75}"
AGV_TRANSLATE_DISTANCE_M="${AGV_TRANSLATE_DISTANCE_M:-0.03}"
AGV_TEST_STATION="${AGV_TEST_STATION:-}"
AGV_PATH_SOURCE="${AGV_PATH_SOURCE:-}"
AGV_PATH_TARGET="${AGV_PATH_TARGET:-}"

STOP_DELAY_S="${STOP_DELAY_S:-0.50}"

EXECUTE=0
ASSUME_YES=0
USE_TLS=0
COMMAND="status"
LAST_RESPONSE=""
MOTION_ACTIVE=0
STOP_ALL_USED=0
BG_PID=""
BG_OUTPUT=""
BACKGROUND_RESPONSE=""

usage() {
  cat <<'EOF'
Usage:
  tools/grpc_remote_motion_test.sh [options] <command>

Commands:
  status                 Read system, arm, frame and AGV state (default).
  arm-motions            Test moveJ/moveL/speedJ/speedL/servoJ/stopMotion.
  actionqueue            Test an ActionQueue containing MoveJ and, when a
                         trustworthy TCP pose is available, MoveL.
  agv-motions            Test pose navigation, velocity, translate,
                         pause/resume/cancel and optional station/path APIs.
  all-safe               Run status, arm-motions, actionqueue, agv-motions.
                         StopAll tests are deliberately excluded.
  arm-stopall            Call StopAll during a slow, small J6 MoveJ.
  actionqueue-stopall    Call StopAll during an ActionQueue.
  agv-stopall            Call StopAll during low-speed AGV velocity motion.

Options:
  --endpoint HOST:PORT   gRPC endpoint (default: 192.168.0.28:50052).
  --arm-id ID            Arm device id (default: aubo_arm).
  --agv-id ID            AGV device id (default: src1100).
  --execute              Actually send motion requests. Without this option,
                         state is read and planned requests are printed only.
  --yes                  Skip interactive MOVE confirmation. Requires
                         --execute; use only with a physically supervised rig.
  --tls                  Use TLS. The default is grpcurl -plaintext.
  -h, --help             Show this help.

Optional AGV station/path tests:
  AGV_TEST_STATION=<id>
  AGV_PATH_SOURCE=<id> AGV_PATH_TARGET=<id>

Optional named frames for MoveL:
  ARM_BASE_FRAME=<name> ARM_TCP_FRAME=<name>

Examples:
  # Read-only connectivity and state check
  tools/grpc_remote_motion_test.sh status

  # Preview all non-StopAll requests, without moving
  tools/grpc_remote_motion_test.sh all-safe

  # Execute the ordinary motion tests, with confirmations
  tools/grpc_remote_motion_test.sh --execute arm-motions

  # Each StopAll test must be run separately. Restart cmvr_es afterwards.
  tools/grpc_remote_motion_test.sh --execute arm-stopall
EOF
}

log() {
  printf '\n[%s] %s\n' "$(date '+%H:%M:%S')" "$*"
}

warn() {
  printf '\nWARNING: %s\n' "$*" >&2
}

die() {
  printf '\nERROR: %s\n' "$*" >&2
  exit 1
}

while (($#)); do
  case "$1" in
    --endpoint)
      (($# >= 2)) || die "--endpoint requires HOST:PORT"
      ENDPOINT="$2"
      shift 2
      ;;
    --arm-id)
      (($# >= 2)) || die "--arm-id requires a device id"
      ARM_ID="$2"
      shift 2
      ;;
    --agv-id)
      (($# >= 2)) || die "--agv-id requires a device id"
      AGV_ID="$2"
      shift 2
      ;;
    --execute)
      EXECUTE=1
      shift
      ;;
    --yes)
      ASSUME_YES=1
      shift
      ;;
    --tls)
      USE_TLS=1
      shift
      ;;
    -h|--help)
      usage
      exit 0
      ;;
    status|arm-motions|actionqueue|agv-motions|all-safe|arm-stopall|actionqueue-stopall|agv-stopall)
      COMMAND="$1"
      shift
      ;;
    *)
      die "unknown argument: $1 (use --help)"
      ;;
  esac
done

if ((ASSUME_YES && !EXECUTE)); then
  die "--yes is meaningful only together with --execute"
fi

command -v "$GRPCURL_BIN" >/dev/null 2>&1 || die "grpcurl not found: $GRPCURL_BIN"
command -v jq >/dev/null 2>&1 || die "jq is required"
command -v awk >/dev/null 2>&1 || die "awk is required"

require_positive_bounded() {
  local name="$1"
  local value="$2"
  local maximum="$3"
  awk -v v="$value" -v max="$maximum" '
    BEGIN { exit !(v ~ /^[0-9]+([.][0-9]+)?$/ && v > 0 && v <= max) }
  ' || die "$name must be numeric, > 0 and <= $maximum (actual: $value)"
}

# Keep environment overrides inside a smoke-test envelope. A typo such as
# 5 instead of 0.005 must not turn a diagnostic script into a large move.
require_positive_bounded ARM_J6_DELTA_RAD "$ARM_J6_DELTA_RAD" 0.03
require_positive_bounded ARM_STOP_J6_DELTA_RAD "$ARM_STOP_J6_DELTA_RAD" 0.05
require_positive_bounded ARM_JOINT_VELOCITY "$ARM_JOINT_VELOCITY" 0.10
require_positive_bounded ARM_STOP_JOINT_VELOCITY "$ARM_STOP_JOINT_VELOCITY" 0.05
require_positive_bounded ARM_JOINT_ACCELERATION "$ARM_JOINT_ACCELERATION" 0.30
require_positive_bounded ARM_STATIONARY_VELOCITY_RAD_S "$ARM_STATIONARY_VELOCITY_RAD_S" 0.02
require_positive_bounded ARM_Z_DELTA_M "$ARM_Z_DELTA_M" 0.01
require_positive_bounded ARM_LINEAR_VELOCITY "$ARM_LINEAR_VELOCITY" 0.03
require_positive_bounded ARM_LINEAR_ACCELERATION "$ARM_LINEAR_ACCELERATION" 0.10
require_positive_bounded ARM_SPEED_DURATION_S "$ARM_SPEED_DURATION_S" 0.50
require_positive_bounded AGV_POSE_DELTA_M "$AGV_POSE_DELTA_M" 0.05
require_positive_bounded AGV_NAV_SPEED "$AGV_NAV_SPEED" 0.05
require_positive_bounded AGV_NAV_ACCELERATION "$AGV_NAV_ACCELERATION" 0.10
require_positive_bounded AGV_BODY_SPEED "$AGV_BODY_SPEED" 0.03
require_positive_bounded AGV_VELOCITY_DURATION_S "$AGV_VELOCITY_DURATION_S" 1.00
require_positive_bounded AGV_TRANSLATE_DISTANCE_M "$AGV_TRANSLATE_DISTANCE_M" 0.05
require_positive_bounded STOP_DELAY_S "$STOP_DELAY_S" 2.00

GRPC_ARGS=()
if ((!USE_TLS)); then
  GRPC_ARGS+=(-plaintext)
fi

pretty_json_or_raw() {
  local input="$1"
  if jq . >/dev/null 2>&1 <<<"$input"; then
    jq . <<<"$input"
  else
    printf '%s\n' "$input"
  fi
}

rpc() {
  local method="$1"
  local payload="$2"
  local max_time="${3:-10}"
  local output
  local rc

  log "RPC $method"
  printf 'request: '
  jq -c . <<<"$payload"

  set +e
  output=$("$GRPCURL_BIN" "${GRPC_ARGS[@]}" -max-time "$max_time" \
    -d "$payload" "$ENDPOINT" "$method" 2>&1)
  rc=$?
  set -e

  LAST_RESPONSE="$output"
  printf 'response:\n'
  pretty_json_or_raw "$output"
  return "$rc"
}

response_is_success() {
  jq -e '
    if has("header") then (.header.success == true)
    else (.success == true)
    end
  ' >/dev/null 2>&1 <<<"$LAST_RESPONSE"
}

rpc_expect_success() {
  local method="$1"
  local payload="$2"
  local max_time="${3:-10}"
  if ! rpc "$method" "$payload" "$max_time"; then
    return 1
  fi
  response_is_success
}

print_plan() {
  local label="$1"
  local method="$2"
  local payload="$3"
  log "DRY RUN: $label"
  printf 'method: %s\nrequest:\n' "$method"
  jq . <<<"$payload"
}

confirm_motion() {
  local label="$1"
  local detail="$2"
  local answer

  if ((!EXECUTE)); then
    return 0
  fi
  if ((ASSUME_YES)); then
    warn "--yes active; executing: $label"
    return 0
  fi

  printf '\n============================================================\n' >&2
  printf '即将执行: %s\n%s\n' "$label" "$detail" >&2
  printf '请确认：现场无人、机械臂/底盘周围无障碍物，急停按钮可立即触达。\n' >&2
  printf '输入 MOVE 继续，其他输入取消: ' >&2
  read -r answer
  [[ "$answer" == "MOVE" ]] || die "operator canceled: $label"
}

run_control() {
  local label="$1"
  local method="$2"
  local payload="$3"
  local max_time="${4:-30}"

  if ((!EXECUTE)); then
    print_plan "$label" "$method" "$payload"
    return 0
  fi

  MOTION_ACTIVE=1
  if ! rpc_expect_success "$method" "$payload" "$max_time"; then
    warn "$label failed. Emergency cleanup will invoke StopAll."
    return 1
  fi
  MOTION_ACTIVE=0
}

run_optional_control() {
  local label="$1"
  local method="$2"
  local payload="$3"
  local max_time="${4:-10}"

  if ((!EXECUTE)); then
    print_plan "$label" "$method" "$payload"
    return 0
  fi

  MOTION_ACTIVE=1
  if rpc_expect_success "$method" "$payload" "$max_time"; then
    log "$label: supported and successful"
    MOTION_ACTIVE=0
    return 0
  fi
  MOTION_ACTIVE=0
  warn "$label returned unsupported/error; continuing capability test"
  return 1
}

run_stop_motion() {
  local label="$1"
  local payload="$2"
  if ((!EXECUTE)); then
    print_plan "$label" "cmvr.api.ArmService/stopMotion" "$payload"
    return 0
  fi
  rpc_expect_success "cmvr.api.ArmService/stopMotion" "$payload" 10
}

stop_all_now() {
  local reason="$1"
  local payload='{"header":{}}'

  warn "invoking SystemService.StopAll: $reason"
  if rpc_expect_success "cmvr.api.SystemService/StopAll" "$payload" 30; then
    STOP_ALL_USED=1
    MOTION_ACTIVE=0
    warn "StopAll succeeded. The server called DeviceManager::stop(); restart cmvr_es before any further test."
    return 0
  fi
  warn "StopAll failed; use the physical emergency stop immediately if anything is still moving"
  return 1
}

cleanup() {
  local rc=$?
  trap - EXIT INT TERM

  if ((MOTION_ACTIVE && !STOP_ALL_USED)); then
    warn "script exited while motion may still be active"
    stop_all_now "automatic cleanup after interruption/failure" || true
  fi
  if [[ -n "$BG_PID" ]]; then
    wait "$BG_PID" 2>/dev/null || true
  fi
  if [[ -n "$BG_OUTPUT" && -f "$BG_OUTPUT" ]]; then
    rm -f -- "$BG_OUTPUT"
  fi
  exit "$rc"
}
trap cleanup EXIT INT TERM

read_system_status() {
  rpc_expect_success "cmvr.api.SystemService/GetSystemStatus" '{}' 10 || \
    die "cannot read system status from $ENDPOINT"
}

read_arm_joint_state() {
  local payload
  payload=$(jq -cn --arg id "$ARM_ID" '{header:{deviceId:$id}}')
  rpc_expect_success "cmvr.api.ArmService/getJointState" "$payload" 10 || \
    die "cannot read joint state for $ARM_ID"
  jq -e '
    (.state.position | type == "array") and
    (.state.position | length == 6) and
    all(.state.position[]; type == "number")
  ' >/dev/null <<<"$LAST_RESPONSE" || \
    die "invalid joint state: expected six numeric joint positions"
  ARM_JOINT_RESPONSE="$LAST_RESPONSE"
}

require_arm_ready_and_stopped() {
  read_arm_joint_state
  jq -e --argjson limit "$ARM_STATIONARY_VELOCITY_RAD_S" '
    (.state.velocity | type == "array") and
    (.state.velocity | length == 6) and
    all(.state.velocity[];
      type == "number" and . >= -$limit and . <= $limit)
  ' >/dev/null <<<"$ARM_JOINT_RESPONSE" || \
    die "arm is not stationary: all six joint velocities must be within +/-${ARM_STATIONARY_VELOCITY_RAD_S} rad/s"
}

read_arm_pose() {
  local payload
  payload=$(jq -cn --arg id "$ARM_ID" '{header:{deviceId:$id}}')
  if ! rpc_expect_success "cmvr.api.ArmService/getPose" "$payload" 10; then
    warn "cannot read TCP pose for $ARM_ID"
    return 1
  fi

  # Proto3 omits zero-valued JSON fields. Treat an all-zero pose as invalid;
  # using it as an absolute MoveL target is unsafe even if the server marks
  # the response successful.
  if ! jq -e '
      [.pose.x // 0, .pose.y // 0, .pose.z // 0,
       .pose.rx // 0, .pose.ry // 0, .pose.rz // 0] as $p |
      all($p[]; type == "number") and any($p[]; . != 0)
    ' >/dev/null <<<"$LAST_RESPONSE"; then
    warn "getPose returned an empty/all-zero pose; refusing absolute MoveL for safety"
    return 1
  fi
  ARM_POSE_RESPONSE="$LAST_RESPONSE"
}

list_arm_frames() {
  local payload
  payload=$(jq -cn --arg id "$ARM_ID" '{header:{deviceId:$id}}')
  if ! rpc_expect_success "cmvr.api.ArmService/ListBaseFrame" "$payload" 10; then
    warn "ListBaseFrame failed"
  fi
  if ! rpc_expect_success "cmvr.api.ArmService/ListTCPFrame" "$payload" 10; then
    warn "ListTCPFrame failed"
  fi
}

read_agv_state() {
  local payload
  payload=$(jq -cn --arg id "$AGV_ID" '{header:{deviceId:$id}}')
  rpc_expect_success "cmvr.api.AgvService/getRuntimeState" "$payload" 10 || \
    die "cannot read AGV state for $AGV_ID"
  AGV_STATE_RESPONSE="$LAST_RESPONSE"
}

require_agv_ready_and_stopped() {
  read_agv_state
  jq -e '
    (.state.connected == true) and
    (.state.localized == true) and
    ((.state.moving // false) == false) and
    ((.state.fault // false) == false) and
    (((.state.emergency_stopped == true) or
      (.state.emergencyStopped == true)) | not)
  ' >/dev/null <<<"$AGV_STATE_RESPONSE" || \
    die "AGV is not safe to move: require connected, localized, stopped, no fault and no emergency stop"
}

wait_for_agv_stopped() {
  local attempts="${1:-20}"
  local i
  for ((i = 1; i <= attempts; ++i)); do
    read_agv_state
    if jq -e '
        ((.state.moving // false) == false) and
        ((.state.fault // false) == false) and
        (((.state.emergency_stopped == true) or
          (.state.emergencyStopped == true)) | not)
      ' >/dev/null <<<"$AGV_STATE_RESPONSE"; then
      return 0
    fi
    sleep 0.25
  done
  return 1
}

status_test() {
  log "endpoint=$ENDPOINT arm=$ARM_ID agv=$AGV_ID execute=$EXECUTE"
  read_system_status
  read_arm_joint_state
  list_arm_frames
  if ! read_arm_pose; then
    warn "Cartesian MoveL tests will be skipped until getPose returns a real TCP pose"
  fi
  read_agv_state
}

build_movej_request() {
  local positions="$1"
  local velocity="$2"
  local acceleration="$3"
  jq -cn \
    --arg id "$ARM_ID" \
    --argjson q "$positions" \
    --argjson v "$velocity" \
    --argjson a "$acceleration" \
    '{header:{deviceId:$id},target:{position:$q},options:{velocity:$v,acceleration:$a,asynchronous:false}}'
}

build_movel_request() {
  local pose="$1"
  jq -cn \
    --arg id "$ARM_ID" \
    --argjson p "$pose" \
    --argjson v "$ARM_LINEAR_VELOCITY" \
    --argjson a "$ARM_LINEAR_ACCELERATION" \
    --arg base "$ARM_BASE_FRAME" \
    --arg tcp "$ARM_TCP_FRAME" '
      {header:{deviceId:$id},target:$p,
       options:{velocity:$v,acceleration:$a,asynchronous:false},
       frame:"ARM_FRAME_BASE"}
      + (if $base == "" then {} else {baseFrame:$base} end)
      + (if $tcp == "" then {} else {tcpFrame:$tcp} end)
    '
}

arm_movej_pair() {
  local q0 q1 forward back
  require_arm_ready_and_stopped
  q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
  q1=$(jq -c --argjson d "$ARM_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
  forward=$(build_movej_request "$q1" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
  back=$(build_movej_request "$q0" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")

  confirm_motion "Arm moveJ" \
    "仅 J6 从 $(jq -r '.[5]' <<<"$q0") rad 增加 ${ARM_J6_DELTA_RAD} rad，然后回到读取到的起点。"
  run_control "moveJ J6 +delta" "cmvr.api.ArmService/moveJ" "$forward" 30
  read_arm_joint_state
  run_control "moveJ return to start" "cmvr.api.ArmService/moveJ" "$back" 30
  read_arm_joint_state
}

arm_movel_pair() {
  local p0 p1 forward back
  require_arm_ready_and_stopped
  if ! read_arm_pose; then
    warn "SKIP moveL: no trustworthy current TCP pose"
    return 0
  fi
  p0=$(jq -c '{x:(.pose.x // 0),y:(.pose.y // 0),z:(.pose.z // 0),rx:(.pose.rx // 0),ry:(.pose.ry // 0),rz:(.pose.rz // 0)}' <<<"$ARM_POSE_RESPONSE")
  p1=$(jq -c --argjson d "$ARM_Z_DELTA_M" '.z += $d' <<<"$p0")
  forward=$(build_movel_request "$p1")
  back=$(build_movel_request "$p0")

  confirm_motion "Arm moveL" \
    "保持 X/Y/姿态不变，仅 Z 增加 ${ARM_Z_DELTA_M} m，然后回到读取到的起点。"
  run_control "moveL Z +delta" "cmvr.api.ArmService/moveL" "$forward" 30
  read_arm_pose || die "cannot verify TCP pose after moveL"
  run_control "moveL return to start" "cmvr.api.ArmService/moveL" "$back" 30
  read_arm_pose || die "cannot verify TCP pose after moveL return"
}

arm_speed_and_servo_capability_tests() {
  local q0 qservo speedj speedl servoj stop_req return_req
  require_arm_ready_and_stopped
  q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
  qservo=$(jq -c '.[5] += 0.002' <<<"$q0")
  stop_req=$(jq -cn --arg id "$ARM_ID" '{deviceId:$id}')
  return_req=$(build_movej_request "$q0" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
  speedj=$(jq -cn \
    --arg id "$ARM_ID" \
    --argjson v "$ARM_JOINT_VELOCITY" \
    --argjson a "$ARM_JOINT_ACCELERATION" \
    --argjson t "$ARM_SPEED_DURATION_S" '
      {header:{deviceId:$id},velocity:{velocity:[0,0,0,0,0,$v]},acceleration:$a,duration:$t}
    ')
  speedl=$(jq -cn \
    --arg id "$ARM_ID" \
    --argjson v "$ARM_LINEAR_VELOCITY" \
    --argjson a "$ARM_LINEAR_ACCELERATION" \
    --argjson t "$ARM_SPEED_DURATION_S" '
      {header:{deviceId:$id},velocity:{vx:0,vy:0,vz:$v,wx:0,wy:0,wz:0},acceleration:$a,duration:$t,frame:"ARM_FRAME_BASE"}
    ')
  servoj=$(jq -cn --arg id "$ARM_ID" --argjson q "$qservo" \
    '{header:{deviceId:$id},target:{position:$q}}')

  confirm_motion "Arm speedJ/speedL/servoJ capability" \
    "仅 J6 或 Z 以极低速度短时运动；每条命令后立即 stopMotion，并用 moveJ/moveL 恢复。Aubo 当前预期返回 unsupported。"

  run_optional_control "speedJ (J6 only)" "cmvr.api.ArmService/speedJ" "$speedj" 10 || true
  run_stop_motion "stopMotion after speedJ" "$stop_req" || \
    die "stopMotion failed after speedJ"
  if ((EXECUTE)); then
    run_control "restore joints after speedJ" "cmvr.api.ArmService/moveJ" "$return_req" 30
  else
    print_plan "restore joints after speedJ" "cmvr.api.ArmService/moveJ" "$return_req"
  fi

  if read_arm_pose; then
    local pose_before pose_return
    pose_before=$(jq -c '{x:(.pose.x // 0),y:(.pose.y // 0),z:(.pose.z // 0),rx:(.pose.rx // 0),ry:(.pose.ry // 0),rz:(.pose.rz // 0)}' <<<"$ARM_POSE_RESPONSE")
    pose_return=$(build_movel_request "$pose_before")
    run_optional_control "speedL (Z only)" "cmvr.api.ArmService/speedL" "$speedl" 10 || true
    run_stop_motion "stopMotion after speedL" "$stop_req" || \
      die "stopMotion failed after speedL"
    if ((EXECUTE)); then
      run_control "restore TCP pose after speedL" "cmvr.api.ArmService/moveL" "$pose_return" 30
    else
      print_plan "restore TCP pose after speedL" "cmvr.api.ArmService/moveL" "$pose_return"
    fi
  else
    warn "SKIP speedL: a real TCP pose is required to guarantee a safe return"
  fi

  run_optional_control "servoJ (J6 +0.002 rad)" "cmvr.api.ArmService/servoJ" "$servoj" 10 || true
  run_stop_motion "stopMotion after servoJ" "$stop_req" || \
    die "stopMotion failed after servoJ"
  if ((EXECUTE)); then
    run_control "restore joints after servoJ" "cmvr.api.ArmService/moveJ" "$return_req" 30
  else
    print_plan "restore joints after servoJ" "cmvr.api.ArmService/moveJ" "$return_req"
  fi
}

arm_motion_tests() {
  arm_movej_pair
  arm_movel_pair
  arm_speed_and_servo_capability_tests
}

actionqueue_test() {
  local q0 q1 movej1 movej2 p0 p1 movel1 movel2 request details
  require_arm_ready_and_stopped
  q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
  q1=$(jq -c --argjson d "$ARM_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
  movej1=$(build_movej_request "$q1" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
  movej2=$(build_movej_request "$q0" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")

  request=$(jq -cn --argjson j1 "$movej1" --argjson j2 "$movej2" '
    {steps:[{timeoutMs:20000,armMoveJ:$j1},{timeoutMs:20000,armMoveJ:$j2}],totalTimeoutMs:60000}
  ')
  details="队列前两步仅让 J6 增加 ${ARM_J6_DELTA_RAD} rad 并返回。"

  if read_arm_pose; then
    p0=$(jq -c '{x:(.pose.x // 0),y:(.pose.y // 0),z:(.pose.z // 0),rx:(.pose.rx // 0),ry:(.pose.ry // 0),rz:(.pose.rz // 0)}' <<<"$ARM_POSE_RESPONSE")
    p1=$(jq -c --argjson d "$ARM_Z_DELTA_M" '.z += $d' <<<"$p0")
    movel1=$(build_movel_request "$p1")
    movel2=$(build_movel_request "$p0")
    request=$(jq -c --argjson l1 "$movel1" --argjson l2 "$movel2" '
      .steps += [{timeoutMs:20000,armMoveL:$l1},{timeoutMs:20000,armMoveL:$l2}] |
      .totalTimeoutMs = 90000
    ' <<<"$request")
    details+=" 后两步仅让 Z 增加 ${ARM_Z_DELTA_M} m 并返回。"
  else
    warn "ActionQueue will test MoveJ only; MoveL is skipped because getPose is not trustworthy"
  fi

  confirm_motion "SystemService.ExecuteActionQueue" "$details"
  run_control "ActionQueue" "cmvr.api.SystemService/ExecuteActionQueue" "$request" 100
  if ((EXECUTE)); then
    jq -e '.result == "ACTION_RESULT_CODE_COMPLETED" and (.header.success == true)' \
      >/dev/null <<<"$LAST_RESPONSE" || die "ActionQueue did not complete"
  fi
  read_arm_joint_state
}

start_background_rpc() {
  local method="$1"
  local payload="$2"
  local max_time="${3:-120}"
  BG_OUTPUT=$(mktemp -t cmvr-grpc-test.XXXXXX)
  "$GRPCURL_BIN" "${GRPC_ARGS[@]}" -max-time "$max_time" \
    -d "$payload" "$ENDPOINT" "$method" >"$BG_OUTPUT" 2>&1 &
  BG_PID=$!
}

show_background_result() {
  local rc=0
  if [[ -n "$BG_PID" ]]; then
    wait "$BG_PID" || rc=$?
  fi
  log "background RPC result (exit=$rc)"
  if [[ -n "$BG_OUTPUT" && -f "$BG_OUTPUT" ]]; then
    BACKGROUND_RESPONSE="$(<"$BG_OUTPUT")"
    pretty_json_or_raw "$BACKGROUND_RESPONSE"
    rm -f -- "$BG_OUTPUT"
  fi
  BG_PID=""
  BG_OUTPUT=""
}

arm_stopall_test() {
  local q0 q1 request
  require_arm_ready_and_stopped
  q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
  q1=$(jq -c --argjson d "$ARM_STOP_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
  request=$(build_movej_request "$q1" "$ARM_STOP_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")

  confirm_motion "Arm MoveJ + StopAll" \
    "仅 J6 低速移动 ${ARM_STOP_J6_DELTA_RAD} rad；${STOP_DELAY_S}s 后调用 StopAll。调用后所有设备停止，必须重启 cmvr_es。"
  if ((!EXECUTE)); then
    print_plan "background slow MoveJ" "cmvr.api.ArmService/moveJ" "$request"
    print_plan "StopAll after ${STOP_DELAY_S}s" "cmvr.api.SystemService/StopAll" '{"header":{}}'
    return 0
  fi

  MOTION_ACTIVE=1
  start_background_rpc "cmvr.api.ArmService/moveJ" "$request" 60
  sleep "$STOP_DELAY_S"
  stop_all_now "arm-stopall test"
  show_background_result
}

actionqueue_stopall_test() {
  local q0 q1 j1 j2 request
  require_arm_ready_and_stopped
  q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
  q1=$(jq -c --argjson d "$ARM_STOP_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
  j1=$(build_movej_request "$q1" "$ARM_STOP_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
  j2=$(build_movej_request "$q0" "$ARM_STOP_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
  request=$(jq -cn --argjson j1 "$j1" --argjson j2 "$j2" '
    {steps:[{timeoutMs:30000,armMoveJ:$j1},{timeoutMs:30000,armMoveJ:$j2}],totalTimeoutMs:60000}
  ')

  confirm_motion "ActionQueue + StopAll" \
    "队列仅低速移动 J6；${STOP_DELAY_S}s 后调用 StopAll，验证活动步骤取消及后续步骤清空。随后必须重启 cmvr_es。"
  if ((!EXECUTE)); then
    print_plan "background ActionQueue" "cmvr.api.SystemService/ExecuteActionQueue" "$request"
    print_plan "StopAll after ${STOP_DELAY_S}s" "cmvr.api.SystemService/StopAll" '{"header":{}}'
    return 0
  fi

  MOTION_ACTIVE=1
  start_background_rpc "cmvr.api.SystemService/ExecuteActionQueue" "$request" 90
  sleep "$STOP_DELAY_S"
  stop_all_now "actionqueue-stopall test"
  show_background_result
  jq -e '.result == "ACTION_RESULT_CODE_CANCELED"' \
    >/dev/null 2>&1 <<<"$BACKGROUND_RESPONSE" || \
    die "ActionQueue did not report ACTION_RESULT_CODE_CANCELED after StopAll"
}

build_agv_options() {
  local async="$1"
  jq -cn \
    --argjson speed "$AGV_NAV_SPEED" \
    --argjson accel "$AGV_NAV_ACCELERATION" \
    --argjson async "$async" '
      {maxSpeed:$speed,maxAngularSpeed:0.1,maxAcceleration:$accel,
       maxAngularAcceleration:0.1,reachDistance:0.01,reachAngle:0.03,
       speedRatio:0.2,asynchronous:$async,waitTimeoutMs:30000,
       pollIntervalMs:200,blockedTimeoutMs:5000}
    '
}

agv_velocity_test() {
  local start stop
  require_agv_ready_and_stopped
  start=$(jq -cn --arg id "$AGV_ID" --argjson v "$AGV_BODY_SPEED" \
    '{header:{deviceId:$id},velocity:{vx:$v,vy:0,wz:0}}')
  stop=$(jq -cn --arg id "$AGV_ID" '{deviceId:$id}')

  confirm_motion "AGV setVelocity/stopVelocityControl" \
    "底盘以 vx=${AGV_BODY_SPEED} m/s 前进 ${AGV_VELOCITY_DURATION_S}s（预计位移很小），随后停止。"
  if ((!EXECUTE)); then
    print_plan "setVelocity" "cmvr.api.AgvService/setVelocity" "$start"
    print_plan "stopVelocityControl after ${AGV_VELOCITY_DURATION_S}s" "cmvr.api.AgvService/stopVelocityControl" "$stop"
    return 0
  fi

  MOTION_ACTIVE=1
  rpc_expect_success "cmvr.api.AgvService/setVelocity" "$start" 10 || \
    die "setVelocity failed"
  sleep "$AGV_VELOCITY_DURATION_S"
  rpc_expect_success "cmvr.api.AgvService/stopVelocityControl" "$stop" 10 || \
    die "stopVelocityControl failed"
  wait_for_agv_stopped || die "AGV still reports moving after stopVelocityControl"
  MOTION_ACTIVE=0
}

agv_translate_test() {
  local request stop wait_s
  require_agv_ready_and_stopped
  request=$(jq -cn \
    --arg id "$AGV_ID" \
    --argjson d "$AGV_TRANSLATE_DISTANCE_M" \
    --argjson v "$AGV_BODY_SPEED" '
      {header:{deviceId:$id},translation:{distance:$d,vx:$v,vy:0,mode:"AGV_TRANSLATION_MODE_ODOMETRY"}}
    ')
  stop=$(jq -cn --arg id "$AGV_ID" '{deviceId:$id}')
  wait_s=$(awk -v d="$AGV_TRANSLATE_DISTANCE_M" -v v="$AGV_BODY_SPEED" 'BEGIN {printf "%.2f", d/v+2.0}')

  confirm_motion "AGV translate" \
    "底盘沿车体 X 正方向平移 ${AGV_TRANSLATE_DISTANCE_M} m，速度 ${AGV_BODY_SPEED} m/s。"
  if ((!EXECUTE)); then
    print_plan "translate" "cmvr.api.AgvService/translate" "$request"
    return 0
  fi

  MOTION_ACTIVE=1
  rpc_expect_success "cmvr.api.AgvService/translate" "$request" 10 || \
    die "translate was not accepted"
  sleep "$wait_s"
  rpc_expect_success "cmvr.api.AgvService/stopVelocityControl" "$stop" 10 || true
  wait_for_agv_stopped || die "AGV still reports moving after translate/stopVelocityControl"
  MOTION_ACTIVE=0
}

agv_navigate_pose_test() {
  local p0 p1 options go back
  require_agv_ready_and_stopped
  p0=$(jq -c '{x:(.state.pose.x // 0),y:(.state.pose.y // 0),theta:(.state.pose.theta // 0)}' <<<"$AGV_STATE_RESPONSE")
  p1=$(jq -c --argjson d "$AGV_POSE_DELTA_M" '.x += $d' <<<"$p0")
  options=$(build_agv_options false)
  go=$(jq -cn --arg id "$AGV_ID" --argjson p "$p1" --argjson o "$options" \
    '{header:{deviceId:$id},pose:$p,options:$o}')
  back=$(jq -cn --arg id "$AGV_ID" --argjson p "$p0" --argjson o "$options" \
    '{header:{deviceId:$id},pose:$p,options:$o}')

  confirm_motion "AGV navigateToPose" \
    "地图坐标 X 增加 ${AGV_POSE_DELTA_M} m，Y/航向不变，然后返回读取到的起点。请确认这两个点间无障碍物。"
  run_control "navigateToPose +X" "cmvr.api.AgvService/navigateToPose" "$go" 40
  MOTION_ACTIVE=1
  require_agv_ready_and_stopped
  MOTION_ACTIVE=0
  run_control "navigateToPose return" "cmvr.api.AgvService/navigateToPose" "$back" 40
  MOTION_ACTIVE=1
  require_agv_ready_and_stopped
  MOTION_ACTIVE=0
}

agv_pause_resume_cancel_test() {
  local p0 p1 options request header
  require_agv_ready_and_stopped
  p0=$(jq -c '{x:(.state.pose.x // 0),y:(.state.pose.y // 0),theta:(.state.pose.theta // 0)}' <<<"$AGV_STATE_RESPONSE")
  p1=$(jq -c --argjson d "$AGV_POSE_DELTA_M" '.x += (2*$d)' <<<"$p0")
  options=$(build_agv_options true)
  request=$(jq -cn --arg id "$AGV_ID" --argjson p "$p1" --argjson o "$options" \
    '{header:{deviceId:$id},pose:$p,options:$o}')
  header=$(jq -cn --arg id "$AGV_ID" '{deviceId:$id}')

  confirm_motion "AGV pause/resume/cancel" \
    "异步目标最多偏移 ${AGV_POSE_DELTA_M}*2 m；短时运行后暂停、恢复，再取消。"
  if ((!EXECUTE)); then
    print_plan "async navigateToPose" "cmvr.api.AgvService/navigateToPose" "$request"
    print_plan "pauseNavigation" "cmvr.api.AgvService/pauseNavigation" "$header"
    print_plan "resumeNavigation" "cmvr.api.AgvService/resumeNavigation" "$header"
    print_plan "cancelNavigation" "cmvr.api.AgvService/cancelNavigation" "$header"
    return 0
  fi

  MOTION_ACTIVE=1
  rpc_expect_success "cmvr.api.AgvService/navigateToPose" "$request" 10 || die "async navigation failed"
  sleep 0.5
  rpc_expect_success "cmvr.api.AgvService/pauseNavigation" "$header" 10 || die "pauseNavigation failed"
  read_agv_state
  rpc_expect_success "cmvr.api.AgvService/resumeNavigation" "$header" 10 || die "resumeNavigation failed"
  sleep 0.5
  rpc_expect_success "cmvr.api.AgvService/cancelNavigation" "$header" 10 || die "cancelNavigation failed"
  sleep 0.5
  wait_for_agv_stopped || die "AGV still reports moving after cancelNavigation"
  MOTION_ACTIVE=0
}

agv_optional_station_tests() {
  local header stations options request
  header=$(jq -cn --arg id "$AGV_ID" '{header:{deviceId:$id}}')
  if ! rpc_expect_success "cmvr.api.AgvService/listStations" "$header" 15; then
    warn "listStations failed; station/path tests skipped"
    return 0
  fi
  stations="$LAST_RESPONSE"
  options=$(build_agv_options false)

  if [[ -n "$AGV_TEST_STATION" ]]; then
    jq -e --arg id "$AGV_TEST_STATION" 'any(.stations[]?; .id == $id)' \
      >/dev/null <<<"$stations" || die "AGV_TEST_STATION not found: $AGV_TEST_STATION"
    require_agv_ready_and_stopped
    request=$(jq -cn --arg id "$AGV_ID" --arg station "$AGV_TEST_STATION" --argjson o "$options" \
      '{header:{deviceId:$id},stationId:$station,options:$o}')
    confirm_motion "AGV navigateToStation" \
      "目标站点=${AGV_TEST_STATION}。该路径可能超过小范围，必须由现场人员核对地图和路线。"
    run_control "navigateToStation" "cmvr.api.AgvService/navigateToStation" "$request" 120
  else
    warn "SKIP navigateToStation: set AGV_TEST_STATION after checking listStations output"
  fi

  if [[ -n "$AGV_PATH_SOURCE" || -n "$AGV_PATH_TARGET" ]]; then
    [[ -n "$AGV_PATH_SOURCE" && -n "$AGV_PATH_TARGET" ]] || \
      die "set both AGV_PATH_SOURCE and AGV_PATH_TARGET"
    jq -e --arg s "$AGV_PATH_SOURCE" --arg t "$AGV_PATH_TARGET" '
      any(.stations[]?; .id == $s) and any(.stations[]?; .id == $t)
    ' >/dev/null <<<"$stations" || die "AGV path source/target is not present in listStations"
    require_agv_ready_and_stopped
    request=$(jq -cn \
      --arg id "$AGV_ID" --arg s "$AGV_PATH_SOURCE" --arg t "$AGV_PATH_TARGET" --argjson o "$options" '
        {header:{deviceId:$id},path:[{sourceStation:$s,targetStation:$t}],options:$o}
      ')
    confirm_motion "AGV followPath" \
      "路径=${AGV_PATH_SOURCE} -> ${AGV_PATH_TARGET}。该路径可能超过小范围，必须由现场人员核对。"
    run_control "followPath" "cmvr.api.AgvService/followPath" "$request" 180
  else
    warn "SKIP followPath: set AGV_PATH_SOURCE and AGV_PATH_TARGET after checking listStations output"
  fi
}

agv_motion_tests() {
  agv_navigate_pose_test
  agv_velocity_test
  agv_translate_test
  agv_pause_resume_cancel_test
  agv_optional_station_tests
}

agv_stopall_test() {
  local start
  require_agv_ready_and_stopped
  start=$(jq -cn --arg id "$AGV_ID" --argjson v "$AGV_BODY_SPEED" \
    '{header:{deviceId:$id},velocity:{vx:$v,vy:0,wz:0}}')

  confirm_motion "AGV setVelocity + StopAll" \
    "底盘以 vx=${AGV_BODY_SPEED} m/s 低速前进；${STOP_DELAY_S}s 后调用 StopAll。随后必须重启 cmvr_es。"
  if ((!EXECUTE)); then
    print_plan "setVelocity" "cmvr.api.AgvService/setVelocity" "$start"
    print_plan "StopAll after ${STOP_DELAY_S}s" "cmvr.api.SystemService/StopAll" '{"header":{}}'
    return 0
  fi

  MOTION_ACTIVE=1
  rpc_expect_success "cmvr.api.AgvService/setVelocity" "$start" 10 || die "setVelocity failed"
  sleep "$STOP_DELAY_S"
  stop_all_now "agv-stopall test"
}

case "$COMMAND" in
  status)
    status_test
    ;;
  arm-motions)
    arm_motion_tests
    ;;
  actionqueue)
    actionqueue_test
    ;;
  agv-motions)
    agv_motion_tests
    ;;
  all-safe)
    status_test
    arm_motion_tests
    actionqueue_test
    agv_motion_tests
    ;;
  arm-stopall)
    arm_stopall_test
    ;;
  actionqueue-stopall)
    actionqueue_stopall_test
    ;;
  agv-stopall)
    agv_stopall_test
    ;;
esac

if ((STOP_ALL_USED)); then
  warn "TEST COMPLETE: StopAll was used. Restart remote cmvr_es before the next test."
elif ((!EXECUTE)); then
  log "dry run complete; no motion request was sent (add --execute to run)"
else
  log "test complete"
fi