#!/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"
BAG_PID_FILE="${STATE_DIR}/rosbag.pid"
BAG_PGID_FILE="${STATE_DIR}/rosbag.pgid"
LOG_FILE="${WS_DIR}/log/linker_hand_control.log"
BAG_LOG_FILE="${WS_DIR}/log/linker_hand_rosbag.log"
CAN_INTERFACES=(can0 can1)
CAN_BITRATE=1000000
ROSBAG_TOPICS=(
  /l20_left_hand_control_cmd
  /l20_left_hand_state
  /l20_left_hand_matrix_touch_mass
  /l20_right_hand_control_cmd
  /l20_right_hand_state
  /l20_right_hand_matrix_touch_mass
)

# 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
}

is_bag_running() {
  [[ -f "${BAG_PID_FILE}" ]] || return 1
  local pid
  pid="$(cat "${BAG_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_bag_recorder() {
  mkdir -p "${STATE_DIR}" "$(dirname "${BAG_LOG_FILE}")"

  if is_bag_running; then
    echo "rosbag recorder is already running, pid=$(cat "${BAG_PID_FILE}")"
    return 0
  fi

  echo "Starting rosbag recorder..."
  echo "Rosbag log: ${BAG_LOG_FILE}"

  : > "${BAG_LOG_FILE}"
  setsid env \
    WS_DIR="${WS_DIR}" \
    ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE}" \
    bash -lc '
      set -euo pipefail
      set +u
      source "${WS_DIR}/install/setup.bash"
      set -u
      while true; do
        start_time=$(date +%Y%m%d_%H%M%S)
        output=/tmp/L20_Hand_${start_time}.mcap
        echo "Starting rosbag: ${output}"
        set +e
        timeout 3600 ros2 bag record \
          --storage mcap \
          --output "${output}" \
          --polling-interval 2 \
          --include-unpublished-topics \
          --disable-keyboard-controls \
          --log-level warn \
          --topics \
            /l20_left_hand_control_cmd \
            /l20_left_hand_state \
            /l20_left_hand_matrix_touch_mass \
            /l20_right_hand_control_cmd \
            /l20_right_hand_state \
            /l20_right_hand_matrix_touch_mass
        rc=$?
        set -e
        if [[ ${rc} -ne 124 ]]; then
          exit ${rc}
        fi
      done
    ' >> "${BAG_LOG_FILE}" 2>&1 &

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

  sleep 1
  if is_bag_running; then
    echo "rosbag recorder started, pid=${pid}, pgid=$(cat "${BAG_PGID_FILE}")"
  else
    echo "rosbag recorder failed to start. Check log: ${BAG_LOG_FILE}"
    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
    return 1
  fi
}

stop_bag_recorder() {
  if ! [[ -f "${BAG_PID_FILE}" ]]; then
    return 0
  fi

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

  if ! kill -0 "${pid}" 2>/dev/null; then
    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
    return 0
  fi

  echo "Stopping rosbag recorder, 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 "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
      echo "rosbag recorder stopped"
      return 0
    fi
    sleep 0.2
  done

  echo "rosbag recorder did not exit after SIGTERM, forcing stop..."
  kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
  rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
  echo "rosbag recorder stopped"
}

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

  if is_running; then
    echo "linker_hand service is already running, pid=$(cat "${PID_FILE}")"
    start_bag_recorder
    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}")"
    start_bag_recorder
  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_bag_recorder
    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_bag_recorder
    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_bag_recorder
      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_bag_recorder
  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}"

  if is_bag_running; then
    echo "rosbag recorder is running, pid=$(cat "${BAG_PID_FILE}"), pgid=$(cat "${BAG_PGID_FILE}" 2>/dev/null || cat "${BAG_PID_FILE}")"
  else
    echo "rosbag recorder is not running"
  fi

  echo "rosbag topics: ${ROSBAG_TOPICS[*]}"

  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