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

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
WS_DIR="$(cd "${SCRIPT_DIR}/.." && pwd)"
STATE_DIR="/tmp/linker_hand_control"
PID_FILE="${STATE_DIR}/linker_hand.pid"
PGID_FILE="${STATE_DIR}/linker_hand.pgid"
LOG_FILE="${WS_DIR}/log/linker_hand_control.log"
CAN_INTERFACES=(can0 can1)
CAN_BITRATE=1000000

# Keep this driver isolated from ROS 2 nodes on other machines in the same LAN.
# Without this, DDS discovery can pick up stale linker_hand_sdk nodes from the
# subnet and make the same topic appear to have multiple publishers.
export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST

usage() {
  echo "Usage: $0 {start|stop|status}"
}

is_running() {
  [[ -f "${PID_FILE}" ]] || return 1
  local pid
  pid="$(cat "${PID_FILE}")"
  [[ -n "${pid}" ]] || return 1
  kill -0 "${pid}" 2>/dev/null
}

source_workspace() {
  set +u
  if [[ -n "${ROS_DISTRO:-}" && -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then
    # shellcheck disable=SC1090
    source "/opt/ros/${ROS_DISTRO}/setup.bash"
  elif [[ -f "/opt/ros/jazzy/setup.bash" ]]; then
    # shellcheck disable=SC1091
    source "/opt/ros/jazzy/setup.bash"
  fi
  set -u

  if [[ ! -f "${WS_DIR}/install/setup.bash" ]]; then
    echo "Workspace is not built: ${WS_DIR}/install/setup.bash not found"
    echo "Run: cd ${WS_DIR} && colcon build --packages-select linker_hand_ros2_sdk"
    exit 1
  fi

  set +u
  # shellcheck disable=SC1091
  source "${WS_DIR}/install/setup.bash"
  set -u
}

restart_ros_daemon_localhost() {
  ros2 daemon stop >/dev/null 2>&1 || true
  ros2 daemon start >/dev/null 2>&1 || true
}

start_can() {
  for can_if in "${CAN_INTERFACES[@]}"; do
    if ! ip link show "${can_if}" >/dev/null 2>&1; then
      echo "CAN interface not found: ${can_if}"
      exit 1
    fi

    echo "Starting ${can_if} at ${CAN_BITRATE} bps..."
    sudo ip link set "${can_if}" down 2>/dev/null || true
    sudo ip link set "${can_if}" type can bitrate "${CAN_BITRATE}"
    sudo ip link set "${can_if}" up
  done
}

stop_can() {
  for can_if in "${CAN_INTERFACES[@]}"; do
    if ip link show "${can_if}" >/dev/null 2>&1; then
      echo "Stopping ${can_if}..."
      sudo ip link set "${can_if}" down 2>/dev/null || true
    fi
  done
}

start_driver() {
  mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")"

  if is_running; then
    echo "linker_hand service is already running, pid=$(cat "${PID_FILE}")"
    return 0
  fi

  source_workspace
  restart_ros_daemon_localhost
  start_can

  echo "Starting linker_hand service..."
  echo "Log: ${LOG_FILE}"

  : > "${LOG_FILE}"
  setsid bash -lc "export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST; set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch linker_hand_ros2_sdk linker_hand_double.launch.py" \
    >> "${LOG_FILE}" 2>&1 &

  local pid=$!
  local pgid
  pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')"
  echo "${pid}" > "${PID_FILE}"
  echo "${pgid:-${pid}}" > "${PGID_FILE}"

  sleep 1
  if is_running; then
    echo "linker_hand service started, pid=${pid}, pgid=$(cat "${PGID_FILE}")"
  else
    echo "linker_hand service failed to start. Check log: ${LOG_FILE}"
    rm -f "${PID_FILE}" "${PGID_FILE}"
    stop_can
    exit 1
  fi
}

stop_driver() {
  if ! [[ -f "${PID_FILE}" ]]; then
    echo "linker_hand service is not running"
    stop_can
    return 0
  fi

  local pid pgid
  pid="$(cat "${PID_FILE}")"
  pgid="${pid}"
  if [[ -f "${PGID_FILE}" ]]; then
    pgid="$(cat "${PGID_FILE}")"
  fi

  if ! kill -0 "${pid}" 2>/dev/null; then
    echo "linker_hand process is not alive, cleaning state"
    rm -f "${PID_FILE}" "${PGID_FILE}"
    stop_can
    return 0
  fi

  echo "Stopping linker_hand service, pid=${pid}, pgid=${pgid}..."
  kill -TERM -- "-${pgid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true

  for _ in {1..30}; do
    if ! kill -0 "${pid}" 2>/dev/null; then
      rm -f "${PID_FILE}" "${PGID_FILE}"
      echo "linker_hand service stopped"
      stop_can
      return 0
    fi
    sleep 0.2
  done

  echo "linker_hand service did not exit after SIGTERM, forcing stop..."
  kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
  rm -f "${PID_FILE}" "${PGID_FILE}"
  echo "linker_hand service stopped"
  stop_can
}

status_driver() {
  if is_running; then
    echo "linker_hand service is running, pid=$(cat "${PID_FILE}"), pgid=$(cat "${PGID_FILE}" 2>/dev/null || cat "${PID_FILE}")"
  else
    echo "linker_hand service is not running"
  fi

  echo "ROS_AUTOMATIC_DISCOVERY_RANGE=${ROS_AUTOMATIC_DISCOVERY_RANGE}"

  for can_if in "${CAN_INTERFACES[@]}"; do
    if ip link show "${can_if}" >/dev/null 2>&1; then
      ip -details link show "${can_if}" | sed -n '1,2p'
    else
      echo "${can_if}: not found"
    fi
  done
}

case "${1:-}" in
  start)
    start_driver
    ;;
  stop)
    stop_driver
    ;;
  status)
    status_driver
    ;;
  *)
    usage
    exit 1
    ;;
esac