#!/usr/bin/env bash
# Сквозная проверка внутри контейнера: нода + проигрывание бэга + разбор вывода.
#
#   docker run --rm --network host -v <бэги>:/data -v <репо>/docker:/smoke:ro flyguard \
#       bash /smoke/demo_test.sh /data/doubleT_obstacle
set -u
# Управление заданиями: без него фоновые процессы неинтерактивного bash глухи к
# SIGINT, и узел не гас по `kill -INT` — его добивал `kill -9`, без итога.
set -m
BAG="${1:?укажите путь к бэгу}"
shift || true
LAUNCH_ARGS=("$@")

# NODE=external — узел уже запущен снаружи (другим контейнером, см.
# jury_cpu_test.sh): здесь только проигрывание и подсчёт.
LAUNCH_PID=""
if [ "${NODE:-}" != "external" ]; then
    echo "=== запуск ноды ==="
    ros2 launch flyguard detect.launch.py "${LAUNCH_ARGS[@]}" > /tmp/node.log 2>&1 &
    LAUNCH_PID=$!
    echo "параметры: ${LAUNCH_ARGS[*]:-по умолчанию}"
else
    : > /tmp/node.log
fi
for i in $(seq 1 30); do
    if ros2 topic list 2>/dev/null | grep -q '^/flyguard/obstacle$'; then
        echo "топик /flyguard/obstacle появился за ${i} с"
        break
    fi
    sleep 1
done
# Видеокарта поднимается в фоне, и узел принимает кадры, не дожидаясь её. Для
# замера скорости ждём её здесь, иначе первые кадры посчитает процессор:
# решения те же, а время кадра смешается. WAIT_GPU=0 — не ждать, как на стенде.
if [ "${WAIT_GPU:-1}" = 1 ] && grep -q "вычисления: пока процессор" /tmp/node.log 2>/dev/null; then
    for i in $(seq 1 90); do
        grep -qE "вычисления: (видеокарта|процессор \()|не поднялась" /tmp/node.log && break
        sleep 1
    done
    GPU_STATE=$(grep -oE 'готова через [0-9.]+ с|не поднялась|CUDA недоступна' /tmp/node.log | head -1)
    echo "видеокарта: ${GPU_STATE:-нет ответа за 90 с}"
fi

# Счётчик на rclpy: `ros2 topic echo` сам не успевает за потоком и теряет
# сообщения, из-за чего занижает оценку частоты.
python3 - > /tmp/count.txt 2>/dev/null <<'PY' &
import rclpy
from rclpy.node import Node
from flyguard_msgs.msg import ObstacleStatus
from diagnostic_msgs.msg import DiagnosticArray


class Counter(Node):
    def __init__(self):
        super().__init__("counter")
        self.n = self.hits = 0
        self.dist, self.ms, self.diag = [], [], {}
        self.create_subscription(ObstacleStatus, "/flyguard/obstacle", self.on_obs, 50)
        self.create_subscription(DiagnosticArray, "/flyguard/diagnostics", self.on_diag, 50)

    def on_obs(self, m):
        self.n += 1
        self.ms.append(m.processing_ms)
        if m.detected:
            self.hits += 1
            self.dist.append(m.distance)

    def on_diag(self, m):
        for st in m.status:
            for kv in st.values:
                if kv.key in ("dropped_frames", "frames_processed",
                               "frames_received", "frames_calibration",
                               "cycle_ms"):
                    self.diag[kv.key] = kv.value


rclpy.init()
c = Counter()
try:
    rclpy.spin(c)
except KeyboardInterrupt:
    pass
finally:
    import statistics
    ms = sorted(c.ms)
    print(f"сообщений: {c.n}")
    if c.n:
        print(f"кадров с обнаружением: {c.hits} ({c.hits / c.n:.1%})")
    if c.dist:
        print(f"дистанция: медиана {statistics.median(c.dist):.1f} м, "
              f"мин {min(c.dist):.1f} м, макс {max(c.dist):.1f} м")
    if ms:
        print(f"обработка кадра: медиана {statistics.median(ms):.1f} мс, "
              f"p95 {ms[int(0.95 * (len(ms) - 1))]:.1f} мс, макс {max(ms):.1f} мс")
    print("по данным самой ноды: " + ", ".join(f"{k}={v}" for k, v in sorted(c.diag.items())))
PY
COUNTER_PID=$!
sleep 2

echo "=== проигрывание бэга ==="
# read-ahead-queue-size: проигрыватель по умолчанию набивает очередь на 1000
# сообщений ДО старта. При кадре в 24 МБ это 24 ГБ, чтение занимает секунды,
# а часы записи уже идут — всё, что за это время «просрочилось», не публикуется.
PLAY_ARGS="${PLAY_ARGS:---read-ahead-queue-size 10}"
echo "аргументы проигрывания: $PLAY_ARGS"
T0=$(date +%s.%N)
ros2 bag play $PLAY_ARGS "$BAG" 2>&1 | grep -Ei "duration|Opened database" | head -3
T1=$(date +%s.%N)
awk -v a="$T0" -v b="$T1" 'BEGIN{printf "проиграно за %.1f с" ORS, b-a}'

sleep 3
kill -INT $COUNTER_PID 2>/dev/null || true
wait $COUNTER_PID 2>/dev/null || true
# SIGINT, а не SIGTERM: так launch гасит и узел, а узел печатает итог. От
# SIGTERM детектор переживал launch, и в check_all.sh узлы копились от записи
# к записи, считая кадры соседей и отбирая у них процессор. `kill -9` ниже —
# только страховка: с `set -m` узел гаснет за полсекунды.
if [ -n "$LAUNCH_PID" ]; then
    kill -INT $LAUNCH_PID 2>/dev/null || true
    for i in $(seq 1 10); do pgrep -f "lib/flyguard/detector" >/dev/null || break; sleep 1; done
    pkill -9 -f "lib/flyguard/detector" 2>/dev/null || true
    wait $LAUNCH_PID 2>/dev/null || true
fi

echo
echo "=== результат ==="
cat /tmp/count.txt

echo
echo "=== журнал ноды ==="
grep -Ei "вычисления|видеокарт|память|считывание|решётк|кадр лидара|подписка|итог|ошибк|error|traceback" /tmp/node.log | head -20

echo
echo "=== что узел сказал в консоль ==="
grep -E "ПРЕПЯТСТВИЕ|препятствие|ЭКСТРЕННОЕ|экстренное|путь свободен" /tmp/node.log | head -30