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

ROOT_DIR="$(cd "$(dirname "$0")/../.." && pwd)"
LINUXCNC_ROOT="${LINUXCNC_ROOT:-$ROOT_DIR/../linuxcnc}"
MACHINE_DIR="$LINUXCNC_ROOT/configs/sim/axis/vismach/millturn"
INI_FILE="$MACHINE_DIR/millturn.ini"
RUN_DIR="$ROOT_DIR/build/native/millturn-user-m-runtime"
LINUXCNC_STDOUT="$RUN_DIR/linuxcnc.stdout.log"
LINUXCNC_STDERR="$RUN_DIR/linuxcnc.stderr.log"

required_commands=(tclsh halrun halcmd linuxcnc)
missing_commands=()
LINUXCNC_PID=""

cleanup() {
  if [[ -n "$LINUXCNC_PID" ]]; then
    kill "$LINUXCNC_PID" 2>/dev/null || true
    wait "$LINUXCNC_PID" 2>/dev/null || true
  fi
}
trap cleanup EXIT

command_path() {
  command -v "$1" 2>/dev/null || true
}

csv_join() {
  local IFS=,
  printf '%s' "$*"
}

print_kv() {
  printf '%s=%s\n' "$1" "$2"
}

runtime_requirements=()
for command_name in "${required_commands[@]}"; do
  path="$(command_path "$command_name")"
  if [[ -n "$path" ]]; then
    runtime_requirements+=("$command_name:1")
    print_kv "millturn_user_m_runtime_${command_name}_path" "$path"
  else
    runtime_requirements+=("$command_name:0")
    missing_commands+=("$command_name")
  fi
done

source_ready=1
for rel in \
  millturn.ini \
  mcodes/M128 \
  mcodes/M129 \
  remap_subs/428remap.ngc \
  remap_subs/429remap.ngc; do
  if [[ ! -f "$MACHINE_DIR/$rel" ]]; then
    source_ready=0
    print_kv "millturn_user_m_missing_source_${rel//[^A-Za-z0-9]/_}" "$MACHINE_DIR/$rel"
  fi
done

runtime_ready=0
if [[ "${#missing_commands[@]}" -eq 0 ]]; then
  runtime_ready=1
fi

print_kv millturn_user_m_runtime_requirements "$(csv_join "${runtime_requirements[@]}")"
if [[ "${#missing_commands[@]}" -eq 0 ]]; then
  print_kv millturn_user_m_missing_requirements "-"
else
  print_kv millturn_user_m_missing_requirements "$(csv_join "${missing_commands[@]}")"
fi
print_kv millturn_user_m_source_proof_ready "$source_ready"
print_kv millturn_user_m_runtime_ready "$runtime_ready"
print_kv millturn_user_m_execution_enabled 0
print_kv millturn_user_m_promotion_allowed 0

if [[ "$source_ready" != "1" ]]; then
  print_kv millturn_user_m_runtime_probe_status "blocked_missing_source"
  print_kv millturn_user_m_runtime_probe_note "missing_source_requirements_keep_user_m_process_blocked"
  exit 0
fi

if [[ "$runtime_ready" != "1" ]]; then
  print_kv millturn_user_m_runtime_probe_status "skipped_missing_host_runtime"
  print_kv millturn_user_m_runtime_probe_note "missing_host_runtime_requirements_keep_user_m_process_blocked"
  exit 0
fi

if [[ "${ENABLE_MILLTURN_USER_M_RUNTIME_PROBE:-0}" != "1" ]]; then
  print_kv millturn_user_m_runtime_probe_status "ready_disabled_by_default"
  print_kv millturn_user_m_runtime_probe_note "set_ENABLE_MILLTURN_USER_M_RUNTIME_PROBE_1_to_run_full_process_probe"
  exit 0
fi

mkdir -p "$RUN_DIR"

wait_for_hal_pin() {
  local pin="$1"
  local limit="${MILLTURN_USER_M_RUNTIME_WAIT_SECONDS:-30}"
  local end=$((SECONDS + limit))
  while (( SECONDS < end )); do
    if halcmd getp "$pin" >/dev/null 2>&1; then
      return 0
    fi
    sleep 0.25
  done
  return 1
}

read_hal_pin() {
  halcmd getp "$1" 2>/dev/null | tr -d '[:space:]'
}

expect_hal_pin() {
  local label="$1"
  local pin="$2"
  local expected="$3"
  local actual
  actual="$(read_hal_pin "$pin")"
  print_kv "millturn_user_m_${label}_${pin//[^A-Za-z0-9]/_}" "$actual"
  [[ "$actual" == "$expected" ]]
}

set_switchkins_target() {
  local target="$1"
  local guard_pin="kinstype.is-$target"
  if ! halcmd setp motion.switchkins-type "$target" >/dev/null 2>&1; then
    halcmd unlinkp motion.switchkins-type >/dev/null 2>&1 || true
    halcmd setp motion.switchkins-type "$target"
  fi
  for _ in $(seq 1 40); do
    if [[ "$(read_hal_pin "$guard_pin")" =~ ^(TRUE|1)$ ]]; then
      return 0
    fi
    sleep 0.05
  done
  return 1
}

run_user_m_case() {
  local code="$1"
  local mode="$2"
  local switchkins_target="$3"
  local expected_x_min="$4"
  local expected_x_max="$5"
  local expected_y_min="$6"
  local expected_y_max="$7"
  local expected_z_min="$8"
  local expected_z_max="$9"

  print_kv "millturn_user_m_${code}_runtime_mode" "$mode"
  if ! set_switchkins_target "$switchkins_target"; then
    print_kv "millturn_user_m_${code}_switchkins_target_ready" 0
    return 1
  fi
  print_kv "millturn_user_m_${code}_switchkins_target_ready" 1

  INI_FILE_NAME="$INI_FILE" "$MACHINE_DIR/mcodes/$code"

  local ok=1
  expect_hal_pin "${code}" ini.x.min_limit "$expected_x_min" || ok=0
  expect_hal_pin "${code}" ini.x.max_limit "$expected_x_max" || ok=0
  expect_hal_pin "${code}" ini.y.min_limit "$expected_y_min" || ok=0
  expect_hal_pin "${code}" ini.y.max_limit "$expected_y_max" || ok=0
  expect_hal_pin "${code}" ini.z.min_limit "$expected_z_min" || ok=0
  expect_hal_pin "${code}" ini.z.max_limit "$expected_z_max" || ok=0
  expect_hal_pin "${code}" ini.x.max_velocity "60" || ok=0
  expect_hal_pin "${code}" ini.y.max_velocity "60" || ok=0
  expect_hal_pin "${code}" ini.z.max_velocity "60" || ok=0
  expect_hal_pin "${code}" ini.x.max_acceleration "400" || ok=0
  expect_hal_pin "${code}" ini.y.max_acceleration "400" || ok=0
  expect_hal_pin "${code}" ini.z.max_acceleration "400" || ok=0

  print_kv "millturn_user_m_${code}_runtime_state_ok" "$ok"
  [[ "$ok" == "1" ]]
}

print_kv millturn_user_m_runtime_probe_ini "$INI_FILE"
print_kv millturn_user_m_runtime_probe_linuxcnc_stdout "$LINUXCNC_STDOUT"
print_kv millturn_user_m_runtime_probe_linuxcnc_stderr "$LINUXCNC_STDERR"

linuxcnc -r "$INI_FILE" >"$LINUXCNC_STDOUT" 2>"$LINUXCNC_STDERR" &
LINUXCNC_PID="$!"

if ! wait_for_hal_pin ini.x.min_limit || ! wait_for_hal_pin motion.switchkins-type; then
  print_kv millturn_user_m_runtime_probe_status "runtime_state_probe_failed"
  print_kv millturn_user_m_runtime_probe_note "linuxcnc_started_but_required_hal_pins_did_not_appear"
  exit 1
fi

probe_ok=1
run_user_m_case M128 mill 0 -300 300 -100 100 -240 0 || probe_ok=0
run_user_m_case M129 turn 1 -240 0 -100 100 -300 300 || probe_ok=0

if [[ "$probe_ok" == "1" ]]; then
  print_kv millturn_user_m_runtime_probe_status "runtime_state_probe_passed"
  print_kv millturn_user_m_runtime_probe_note "linuxcnc_owned_M128_M129_tcl_hal_state_probe_passed_without_promotion"
  exit 0
fi

print_kv millturn_user_m_runtime_probe_status "runtime_state_probe_failed"
print_kv millturn_user_m_runtime_probe_note "linuxcnc_owned_M128_M129_tcl_hal_state_probe_failed"
exit 1
