#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
СИМУЛЯТОР ДЕТЕКТОРА ПАДЕНИЯ маячка (пара к Module_2_Beacon_v3_step2_GPS.ino).

Зачем это самый важный стенд проекта: маячок носит человек с деменцией, и
критическая величина здесь — не координата и не батарея, а РЕШЕНИЕ «человек
упал / не упал». Пропущенное падение — беда не замечена; ложная тревога —
человека дёргают зря и к сигналу перестают относиться серьёзно. Оба провала
живут в одном месте — в детекторе падения по акселерометру MPU6050, и увидеть
его работу на живом железе трудно (падать с платой никто не будет).

Стенд СИНТЕЗИРУЕТ след модуля ускорения |a| (в единицах g) для типовых событий
и прогоняет ТУ ЖЕ трёхфазную машину состояний, что зашита в прошивке
(struct FallDetector в .ino):

  ФАЗА 1 (невесомость): |a| < FF_THR дольше FF_MS  → началось свободное падение
  ФАЗА 2 (удар):        |a| > IMPACT_THR в окне IMPACT_WIN_MS после невесомости
  ФАЗА 3 (неподвижность): после успокоения SETTLE_MS дисперсия |a| за STILL_MS
                          мала (σ² < STILL_SIGMA²) → ПАДЕНИЕ ПОДТВЕРЖДЕНО

Ключевая идея — падение подтверждается ТОЛЬКО если сложились ВСЕ ТРИ фазы
подряд. Поэтому «уронил телефон» (удар без невесомости-стояния-после) и «сел
резко» (нет свободного падения) не дают тревоги, а настоящее падение с
последующей неподвижностью — даёт. Ученик двигает пороги ползунками и видит
на ROC-табло, как растут пропуски и ложные срабатывания.

Значения порогов — 1:1 с прошивкой (FP_* в FallDetector).

Запуск:
    uv run fall_detector_sim.py
Без uv:
    pip install numpy matplotlib && python fall_detector_sim.py
"""

# /// script
# requires-python = ">=3.11"
# dependencies = ["numpy>=1.26", "matplotlib>=3.8"]
# ///

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider, Button

# --- Пороги детектора (1:1 с FallDetector в Module_2_Beacon_v3_step2_GPS.ino) ---
FF_THR_G_DEF = 0.4        # порог невесомости, g
FF_MS = 60                # невесомость должна длиться, мс
IMPACT_THR_G_DEF = 2.8    # порог удара, g
IMPACT_WIN_MS = 500       # удар должен прийти в этом окне после невесомости
SETTLE_MS = 1500          # «звон» после удара не считаем
STILL_MS = 8000           # длительность окна оценки неподвижности
STILL_SIGMA_DEF = 0.05    # порог СКО неподвижности, g (в коде sigma^2 < 0.05^2)

RATE_HZ = 100.0           # частота выборки MPU (SMPLRT в прошивке ~100 Гц)
DT_MS = 1000.0 / RATE_HZ


# =====================================================================
# СИНТЕЗ следа |a| для типовых событий
# =====================================================================
def add_walk(a, t, t0, t1, rng):
    """Обычная ходьба: |a| колеблется вокруг 1g."""
    m = (t >= t0) & (t < t1)
    a[m] += 0.25 * np.sin(2 * np.pi * 2.0 * (t[m] - t0)) + rng.normal(0, 0.06, m.sum())


def add_fall(a, t, tc, rng, ff_dur=0.12, impact=6.0):
    """Настоящее падение: невесомость → удар → неподвижность (лежит)."""
    # 1) свободное падение: |a| проваливается почти в 0 на ff_dur секунд
    ff = (t >= tc) & (t < tc + ff_dur)
    a[ff] = 0.15 + rng.normal(0, 0.03, ff.sum())
    # 2) удар: резкий пик сразу после невесомости
    imp_t = tc + ff_dur
    imp = (t >= imp_t) & (t < imp_t + 0.05)
    a[imp] = impact + rng.normal(0, 0.5, imp.sum())
    # 3) звон 1.5 c, затем лежит неподвижно (|a|≈1g, очень малый разброс)
    ring = (t >= imp_t + 0.05) & (t < imp_t + 0.05 + 1.5)
    a[ring] = 1.0 + 0.4 * np.exp(-3 * (t[ring] - imp_t)) * rng.normal(0, 1, ring.sum())
    still = (t >= imp_t + 1.55) & (t < imp_t + 1.55 + 9.0)
    a[still] = 1.0 + rng.normal(0, 0.015, still.sum())   # почти неподвижно


def add_drop(a, t, tc, rng):
    """Ложное: УРОНИЛ устройство — удар есть, но после него подобрали (движение)."""
    ff = (t >= tc) & (t < tc + 0.10)
    a[ff] = 0.15 + rng.normal(0, 0.03, ff.sum())
    imp = (t >= tc + 0.10) & (t < tc + 0.15)
    a[imp] = 5.0 + rng.normal(0, 0.5, imp.sum())
    # после падения устройство ПОДБИРАЮТ — идёт движение, НЕ неподвижность
    move = (t >= tc + 0.15) & (t < tc + 0.15 + 9.0)
    a[move] = 1.0 + rng.normal(0, 0.35, move.sum())


def add_sit(a, t, tc, rng):
    """Ложное: СЕЛ резко — удар средний, но БЕЗ свободного падения перед ним."""
    imp = (t >= tc) & (t < tc + 0.08)
    a[imp] = 3.2 + rng.normal(0, 0.3, imp.sum())          # удар без невесомости
    still = (t >= tc + 0.08) & (t < tc + 0.08 + 9.0)
    a[still] = 1.0 + rng.normal(0, 0.02, still.sum())     # потом сидит спокойно


def build_scene(seed):
    """Возвращает (t, a, truth_falls) — сцена с 1 падением и 2 провокациями."""
    rng = np.random.default_rng(seed)
    dur = 40.0
    n = int(dur * RATE_HZ)
    t = np.arange(n) / RATE_HZ
    a = np.ones(n)
    add_walk(a, t, 0, dur, rng)
    events = [
        ("fall", 8.0),     # настоящее падение (ground truth)
        ("drop", 20.0),    # уронил устройство (провокация)
        ("sit", 30.0),     # сел резко (провокация)
    ]
    truth = []
    for kind, tc in events:
        if kind == "fall":
            add_fall(a, t, tc, rng); truth.append(tc)
        elif kind == "drop":
            add_drop(a, t, tc, rng)
        elif kind == "sit":
            add_sit(a, t, tc, rng)
    return t, a, truth


# =====================================================================
# ДЕТЕКТОР: копия трёхфазной FSM из прошивки
# =====================================================================
def run_fall_fsm(t, a, ff_thr, impact_thr, still_sigma):
    """
    Повторяет FallDetector::update(). Возвращает список (t_confirm, phase_log),
    где phase_log — массив номеров фаз (0..3) для окраски графика.
    """
    FP_IDLE, FP_FREEFALL, FP_STILL = 0, 1, 2
    phase = FP_IDLE
    ff_active = False
    ff_start = 0.0
    t_phase = 0.0
    ssum = ssum2 = 0.0
    n = 0
    phase_log = np.zeros(len(t), dtype=int)
    confirms = []

    for i, (ti, g) in enumerate(zip(t, a)):
        now_ms = ti * 1000.0
        if phase == FP_IDLE:
            if g < ff_thr:
                if not ff_active:
                    ff_active = True
                    ff_start = now_ms
                elif now_ms - ff_start >= FF_MS:
                    phase = FP_FREEFALL
                    t_phase = now_ms
            else:
                ff_active = False
        elif phase == FP_FREEFALL:
            if g > impact_thr:
                phase = FP_STILL
                t_phase = now_ms
                ssum = ssum2 = 0.0
                n = 0
            elif now_ms - t_phase > IMPACT_WIN_MS:
                phase = FP_IDLE
                ff_active = False
        elif phase == FP_STILL:
            dt = now_ms - t_phase
            if dt >= SETTLE_MS:
                ssum += g
                ssum2 += g * g
                n += 1
                if dt >= SETTLE_MS + STILL_MS:
                    mean = ssum / n
                    var = ssum2 / n - mean * mean
                    fall = (n > 100) and (var < still_sigma ** 2)
                    if fall:
                        confirms.append(ti)
                    phase = FP_IDLE
                    ff_active = False
        phase_log[i] = phase
    return confirms, phase_log


def score(confirms, truth, tol=10.0):
    """Падение засчитано, если подтверждение пришло в пределах tol c после старта."""
    hits = 0
    matched = [False] * len(confirms)
    for tm in truth:
        for k, tc in enumerate(confirms):
            if tm <= tc <= tm + tol:
                hits += 1
                matched[k] = True
                break
    false_alarms = sum(1 for m in matched if not m)
    misses = len(truth) - hits
    return hits, misses, false_alarms


# =====================================================================
# Визуализация
# =====================================================================
def main():
    st = {"seed": 1, "ff": FF_THR_G_DEF, "impact": IMPACT_THR_G_DEF,
          "sigma": STILL_SIGMA_DEF}

    fig = plt.figure(figsize=(12, 7))
    fig.suptitle("Маячок SOS: детектор падения — 3 фазы и настройка порогов",
                 fontsize=13, fontweight="bold")
    ax = fig.add_axes([0.08, 0.40, 0.88, 0.48])

    phase_colors = {1: ("#3b82f6", "невесомость"),
                    2: ("#f59e0b", "удар→неподвижность")}

    def redraw(_=None):
        t, a, truth = build_scene(st["seed"])
        confirms, plog = run_fall_fsm(t, a, st["ff"], st["impact"], st["sigma"])
        hits, misses, fa = score(confirms, truth)

        ax.clear()
        ax.plot(t, a, "-", lw=0.8, color="#334155", label="|a|, g")
        # пороги
        ax.axhline(st["ff"], color="#3b82f6", ls=":", lw=1.3)
        ax.text(0.2, st["ff"] + 0.1, f"невесомость <{st['ff']:.2f}g",
                color="#3b82f6", fontsize=8)
        ax.axhline(st["impact"], color="#ef4444", ls=":", lw=1.3)
        ax.text(0.2, st["impact"] + 0.1, f"удар >{st['impact']:.1f}g",
                color="#ef4444", fontsize=8)
        ax.axhline(1.0, color="#999", lw=0.6)
        # подсветка активных фаз FSM
        for ph, (col, _lab) in phase_colors.items():
            mask = plog == ph
            if mask.any():
                ax.fill_between(t, 0, a, where=mask, color=col, alpha=0.15, step="mid")
        # ground truth и подтверждения
        for tm in truth:
            ax.axvline(tm, color="#111", ls="--", lw=1.0, alpha=0.7)
        for tc in confirms:
            ax.axvline(tc, color="#22c55e", lw=2.0, alpha=0.8)
        ax.set_xlim(0, t[-1])
        ax.set_ylim(-0.3, max(7.0, st["impact"] + 1))
        ax.set_xlabel("время, c")
        ax.set_ylabel("|ускорение|, g")
        ax.set_title(
            f"Падений (правда): {len(truth)}  |  поймано: {hits}  |  "
            f"пропущено: {misses}  |  ложных: {fa}\n"
            "чёрный пунктир — реальное падение, зелёная линия — подтверждение FSM, "
            "провокации на t=20 (уронил) и t=30 (сел)",
            fontsize=9)
        ax.grid(alpha=0.2)
        ax.legend(loc="upper right", fontsize=8)
        fig.canvas.draw_idle()

    specs = [
        ("Порог невесомости, g", "ff", 0.1, 0.9, FF_THR_G_DEF, 0.05),
        ("Порог удара, g", "impact", 1.5, 5.0, IMPACT_THR_G_DEF, 0.1),
        ("Порог неподвижности σ, g", "sigma", 0.01, 0.30, STILL_SIGMA_DEF, 0.01),
    ]
    for i, (label, key, lo, hi, init, step) in enumerate(specs):
        axs = fig.add_axes([0.14, 0.26 - i * 0.05, 0.6, 0.03])
        s = Slider(axs, label, lo, hi, valinit=init, valstep=step)

        def make_cb(k):
            def cb(v):
                st[k] = v
                redraw()
            return cb
        s.on_changed(make_cb(key))

    ax_new = fig.add_axes([0.80, 0.13, 0.15, 0.05])
    b_new = Button(ax_new, "Новый шум")
    b_new.on_clicked(lambda _: (st.__setitem__("seed", st["seed"] + 1), redraw()))

    redraw()
    plt.show()


if __name__ == "__main__":
    main()