#!/usr/bin/env bash

# test script for anchor connectors (mock, serial, CAN)

set -o pipefail

repo_root="$(git rev-parse --show-toplevel)"

if [[ -z $repo_root ]]; then
	echo "script must be run from within the rover-ros2 repo" >&2
	exit 1
fi

cd "$repo_root"

BOLD='\033[1m'
RED='\033[1;31m'
GREEN='\033[1;32m'
YELLOW='\033[1;33m'
NC='\033[0m'

TESTS_PASSED=0
TESTS_FAILED=0

log() {
	echo -e "${BOLD}${YELLOW}info:${NC} ${1}"
}

pass() {
	echo -e "${BOLD}${GREEN}pass:${NC} ${1}"
	TESTS_PASSED=$((TESTS_PASSED + 1))
}

fail() {
	echo -e "${BOLD}${RED}fail:${NC} ${1}"
	TESTS_FAILED=$((TESTS_FAILED + 1))
}

cleanup() {
	log "cleaning up"
	if [[ -n $ANCHOR_PID ]]; then
		kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
		wait "$ANCHOR_PID" 2>/dev/null || true
	fi
	if [[ -n $SOCAT_PID ]]; then
		kill -INT "$SOCAT_PID" 2>/dev/null || true
		wait "$SOCAT_PID" 2>/dev/null || true
	fi
	rm -f /tmp/ttyACM9 /tmp/ttyOUT 2>/dev/null || true
}

trap cleanup EXIT

source_ros2() {
	source install/setup.bash
}

wait_for_topic() {
	local topic="$1"
	local timeout="${2:-5}"
	local count=0
	while ! ros2 topic list 2>/dev/null | grep -q "^${topic}$"; do
		sleep 0.5
		count=$((count + 1))
		if [[ $count -ge $((timeout * 2)) ]]; then
			return 1
		fi
	done
	return 0
}

test_mock_connector() {
	log "testing mock connector"

	log "starting anchor with mock connector"
	setsid ros2 run anchor_pkg anchor --ros-args -p connector:=mock &
	ANCHOR_PID=$!
	sleep 2

	if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
		fail "mock connector: anchor failed to start"
		return 1
	fi

	if ! wait_for_topic "/anchor/to_vic/relay" 10; then
		fail "mock connector: topics not available"
		kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
		return 1
	fi

	log "anchor started successfully"

	# relay -> debug
	log "testing relay -> debug"

	local debug_output
	debug_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/to_vic/debug &
		ECHO_PID=$!
		sleep 0.5
		ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 50, data: [1.0, 2.0, 3.0, 4.0]}" >/dev/null 2>&1
		wait $ECHO_PID
	' 2>/dev/null) || true

	if [[ -n "$debug_output" ]] && echo "$debug_output" | grep -q "can_relay_tovic,core,50"; then
		pass "mock connector: relay -> debug"
	else
		fail "mock connector: relay -> debug"
	fi

	# mock_mcu -> from_vic/core
	log "testing mock_mcu (core) -> from_vic/core"

	local core_output=""
	core_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/from_vic/core &
		ECHO_PID=$!
		sleep 0.5
		ros2 topic pub --once /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 10, data: [100.0, 200.0]}" >/dev/null 2>&1 || true
		wait $ECHO_PID || true
	' 2>/dev/null || true) || core_output=""

	if [[ -n "$core_output" ]] && echo "$core_output" | grep -q "mcu_name: core" && echo "$core_output" | grep -q "command_id: 10"; then
		pass "mock connector: mock_mcu -> from_vic/core"
	else
		fail "mock connector: mock_mcu -> from_vic/core"
	fi

	# mock_mcu -> from_vic/arm
	log "testing mock_mcu (arm) -> from_vic/arm"

	local arm_output
	arm_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/from_vic/arm &
		ECHO_PID=$!
		sleep 0.5
		ros2 topic pub --once /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN "{mcu_name: \"arm\", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}" >/dev/null 2>&1
		wait $ECHO_PID
	' 2>/dev/null) || true

	if [[ -n "$arm_output" ]] && echo "$arm_output" | grep -q "mcu_name: arm" && echo "$arm_output" | grep -q "command_id: 55"; then
		pass "mock connector: mock_mcu -> from_vic/arm"
	else
		fail "mock connector: mock_mcu -> from_vic/arm"
	fi

	# mock_mcu -> from_vic/bio
	log "testing mock_mcu (citadel) -> from_vic/bio"

	local bio_output
	bio_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/from_vic/bio &
		ECHO_PID=$!
		sleep 0.5
		ros2 topic pub --once /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN "{mcu_name: \"citadel\", command_id: 20, data: [5.0]}" >/dev/null 2>&1
		wait $ECHO_PID
	' 2>/dev/null) || true

	if echo "$bio_output" | grep -q "mcu_name: citadel" && echo "$bio_output" | grep -q "command_id: 20"; then
		pass "mock connector: mock_mcu -> from_vic/bio"
	else
		fail "mock connector: mock_mcu -> from_vic/bio"
	fi

	# relay_string -> debug
	log "testing relay_string -> debug"

	local relay_string_output
	relay_string_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/to_vic/debug &
		ECHO_PID=$!
		sleep 0.5
		ros2 topic pub --once /anchor/to_vic/relay_string std_msgs/msg/String "{data: \"test_raw_string_data\"}" >/dev/null 2>&1
		wait $ECHO_PID
	' 2>/dev/null) || true

	if [[ -n "$relay_string_output" ]] && echo "$relay_string_output" | grep -q "test_raw_string_data"; then
		pass "mock connector: relay_string -> debug"
	else
		fail "mock connector: relay_string -> debug"
	fi

	kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
	wait "$ANCHOR_PID" 2>/dev/null || true
	ANCHOR_PID=""
}

test_serial_connector() {
	log "testing serial connector"

	log "creating virtual serial ports with socat"
	socat pty,raw,echo=0,link=/tmp/ttyACM9 pty,raw,echo=0,link=/tmp/ttyOUT 2>/dev/null &
	SOCAT_PID=$!
	sleep 2

	if ! kill -0 "$SOCAT_PID" 2>/dev/null; then
		fail "serial connector: failed to create virtual serial ports"
		return 1
	fi

	log "starting anchor with serial connector (override: /tmp/ttyACM9)"
	setsid ros2 run anchor_pkg anchor --ros-args -p connector:=serial -p serial_override:=/tmp/ttyACM9 &
	ANCHOR_PID=$!
	sleep 2

	if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
		fail "serial connector: anchor failed to start"
		kill -INT "$SOCAT_PID" 2>/dev/null || true
		return 1
	fi

	if ! wait_for_topic "/anchor/to_vic/relay" 10; then
		fail "serial connector: topics not available"
		kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
		kill -INT "$SOCAT_PID" 2>/dev/null || true
		return 1
	fi

	pass "serial connector: anchor starts with virtual serial"

	# relay_string -> debug
	log "testing relay_string -> debug"

	local relay_string_output
	relay_string_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/to_vic/debug &
		ECHO_PID=$!
		sleep 0.5
		ros2 topic pub --once /anchor/to_vic/relay_string std_msgs/msg/String "{data: \"serial_test_string\"}" >/dev/null 2>&1
		wait $ECHO_PID
	' 2>/dev/null) || true

	if [[ -n "$relay_string_output" ]] && echo "$relay_string_output" | grep -q "serial_test_string"; then
		pass "serial connector: relay_string -> debug"
	else
		fail "serial connector: relay_string -> debug"
	fi

	log "serial data transfer tests skipped (virtual pty limitation)"

	kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
	wait "$ANCHOR_PID" 2>/dev/null || true
	ANCHOR_PID=""
	kill -INT "$SOCAT_PID" 2>/dev/null || true
	wait "$SOCAT_PID" 2>/dev/null || true
	SOCAT_PID=""
}

test_can_connector() {
	log "testing CAN connector"

	log "starting anchor with CAN connector (override: vcan0)"
	setsid ros2 run anchor_pkg anchor --ros-args -p connector:=can -p can_override:=vcan0 &
	ANCHOR_PID=$!
	sleep 2

	if ! kill -0 "$ANCHOR_PID" 2>/dev/null; then
		fail "CAN connector: anchor failed to start"
		return 1
	fi

	if ! wait_for_topic "/anchor/to_vic/relay" 10; then
		fail "CAN connector: topics not available"
		kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
		return 1
	fi

	log "anchor started successfully"
	sleep 1

	# relay -> CAN bus
	log "testing relay -> CAN bus"

	local can_output
	can_output=$(timeout 8 bash -c '
		candump -n 1 vcan0 &
		DUMP_PID=$!
		sleep 1
		ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 30, data: [1, 2, 3, 4]}" >/dev/null 2>&1
		sleep 0.5
		ros2 topic pub --once /anchor/to_vic/relay astra_msgs/msg/VicCAN "{mcu_name: \"core\", command_id: 30, data: [1, 2, 3, 4]}" >/dev/null 2>&1
		wait $DUMP_PID
	' 2>/dev/null) || true

	# core=1, int16x4=2, cmd=30 -> id = (1<<8)|(2<<6)|30 = 0x19E
	if echo "$can_output" | grep -qi "19E"; then
		pass "CAN connector: relay -> CAN bus"
	else
		fail "CAN connector: relay -> CAN bus (got: $can_output)"
	fi

	# CAN -> from_vic/core
	log "testing CAN bus -> from_vic/core"

	local core_output
	core_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/from_vic/core &
		ECHO_PID=$!
		sleep 1
		cansend vcan0 18F#000A0014001E0028
		sleep 0.5
		cansend vcan0 18F#000A0014001E0028
		wait $ECHO_PID
	' 2>/dev/null) || true

	if echo "$core_output" | grep -q "mcu_name: core" && echo "$core_output" | grep -q "command_id: 15"; then
		pass "CAN connector: CAN -> from_vic/core"
	else
		fail "CAN connector: CAN -> from_vic/core"
	fi

	# CAN -> from_vic/arm
	log "testing CAN bus -> from_vic/arm"

	local arm_output
	arm_output=$(timeout 5 bash -c '
		ros2 topic echo --once /anchor/from_vic/arm &
		ECHO_PID=$!
		sleep 1
		cansend vcan0 294#00640096012C01F4
		sleep 0.5
		cansend vcan0 294#00640096012C01F4
		wait $ECHO_PID
	' 2>/dev/null) || true

	if echo "$arm_output" | grep -q "mcu_name: arm" && echo "$arm_output" | grep -q "command_id: 20"; then
		pass "CAN connector: CAN -> from_vic/arm"
	else
		fail "CAN connector: CAN -> from_vic/arm"
	fi

	# double data type
	log "testing CAN double data type"

	local double_output
	double_output=$(timeout 8 bash -c '
		ros2 topic echo --once /anchor/from_vic/core &
		ECHO_PID=$!
		sleep 1
		cansend vcan0 105#3FF0000000000000
		sleep 0.5
		cansend vcan0 105#3FF0000000000000
		sleep 0.5
		cansend vcan0 105#3FF0000000000000
		wait $ECHO_PID
	' 2>/dev/null) || true

	if echo "$double_output" | grep -q "mcu_name: core" && echo "$double_output" | grep -q "command_id: 5"; then
		pass "CAN connector: double data type"
	else
		fail "CAN connector: double data type"
	fi

	# float32x2 data type
	log "testing CAN float32x2 data type"

	local float_output
	float_output=$(timeout 8 bash -c '
		ros2 topic echo --once /anchor/from_vic/core &
		ECHO_PID=$!
		sleep 1
		cansend vcan0 14A#3F80000040000000
		sleep 0.5
		cansend vcan0 14A#3F80000040000000
		sleep 0.5
		cansend vcan0 14A#3F80000040000000
		wait $ECHO_PID
	' 2>/dev/null) || true

	if echo "$float_output" | grep -q "mcu_name: core" && echo "$float_output" | grep -q "command_id: 10"; then
		pass "CAN connector: float32x2 data type"
	else
		fail "CAN connector: float32x2 data type"
	fi

	kill -INT -- -"$ANCHOR_PID" 2>/dev/null || true
	wait "$ANCHOR_PID" 2>/dev/null || true
	ANCHOR_PID=""
}

check_prerequisites() {
	log "checking prerequisites"
	local missing=0

	if [[ ! -f install/setup.bash ]]; then
		fail "install/setup.bash not found; run 'colcon build' first"
		missing=1
	fi

	if ! command -v socat &>/dev/null; then
		fail "socat not found; install it or use 'nix develop'"
		missing=1
	fi

	if ! command -v cansend &>/dev/null || ! command -v candump &>/dev/null; then
		fail "can-utils (cansend/candump) not found; install it or use 'nix develop'"
		missing=1
	fi

	if ! ip link show vcan0 &>/dev/null; then
		fail "vcan0 interface not found"
		log "  create it with:"
		log "    sudo ip link add dev vcan0 type vcan"
		log "    sudo ip link set vcan0 up"
		missing=1
	elif ! ip link show vcan0 | grep -q ",UP"; then
		fail "vcan0 exists but is not UP"
		log "  enable it with: sudo ip link set vcan0 up"
		missing=1
	fi

	if [[ $missing -eq 1 ]]; then
		echo ""
		log "prerequisites not met"
		exit 1
	fi

	log "all prerequisites met"
}

main() {
	echo ""
	log "anchor connector test suite"
	echo ""

	check_prerequisites

	log "sourcing ROS2 workspace"
	source_ros2

	test_mock_connector
	test_serial_connector
	test_can_connector

	echo ""
	log "test summary"
	echo -e "${BOLD}${GREEN}passed:${NC} $TESTS_PASSED"
	echo -e "${BOLD}${RED}failed:${NC} $TESTS_FAILED"
	echo ""

	if [[ $TESTS_FAILED -gt 0 ]]; then
		exit 1
	fi
	exit 0
}

main "$@"