#include "t_safety.h"

#include <assert.h>
#include <stdio.h>
#include <string.h>

#include "hal_i2c_acct.h"
#include "hal_time.h"
#include "i2cbus.h"
#include "rail.h"
#include "rail_cfg.h"
#include "slot.h"
#include "t_actuator.h"
#include "tasks.h"

/* 判据与四条规矩见 t_safety.h。这里只补一句 R2 最容易写错的：
   **断 VBUS 在锁内（它是 I²C 上的 PCA9555），关 eFuse 在锁外（它是 GPIO）。**
   两者以前是一对 GPIO，现在不是了。 */

static safety_ev_fn s_sink;
static void        *s_sink_ctx;

void t_safety_set_event_sink(safety_ev_fn fn, void *ctx)
{
    s_sink = fn;
    s_sink_ctx = ctx;
}

const char *t_safety_ev_str(safety_ev_t ev)
{
    switch (ev) {
    case SAFETY_EV_NONE:
        return "无";
    case SAFETY_EV_OVER_VOLT:
        return "过压关路(SR-5)";
    case SAFETY_EV_UNDER_VOLT:
        return "欠压关路(SR-5)";
    case SAFETY_EV_REVERSE:
        return "反灌断VBUS(SW-8)";
    case SAFETY_EV_PUSHED_UP:
        return "被顶高断VBUS(SW-8)";
    case SAFETY_EV_MUX_LOST:
        return "mux失联全断";
    case SAFETY_EV_SLOT_EMPTY:
        return "刀片不在位";
    case SAFETY_EV_BLADE_MUTE:
        return "刀片不应答关路";
    default:
        return "?";
    }
}

/* 记一条。**永远在动作之后调** —— 记录慢一步没关系，断电慢一步不行。
   `rail` 传 0 表示整板级事件（mux 失联），那时 V/I 记的是输入侧的。 */
static void emit(safety_snap_t *s, int rail, safety_ev_t ev, uint32_t now)
{
    const ina226_vi_t *vi = (rail >= 1) ? &s->rail[rail - 1] : &s->input;

    s->last.t_ms       = now;
    s->last.rail       = (uint8_t)rail;
    s->last.type       = (uint8_t)ev;
    s->last.bus_mv     = vi->bus_mv;
    s->last.current_ua = vi->current_ua;
    s->events++;

    if (s_sink != NULL) {
        s_sink(s_sink_ctx, &s->last);
    }
}

/* ---- 锁内：一路的读 + 判据 ----------------------------------------------- */

typedef struct {
    safety_snap_t *s;
    int            rail;
    uint32_t       now; /* 本圈统一的时刻：五路要拿同一个时刻去算「持续了多久」 */
} cycle_arg_t;

/* SW-8：只断这一路的 VBUS，**不碰 eFuse** —— 反灌的能量是从 USB 那一侧来的，
   把直供也一起关掉等于「因为充电器有问题就把手机关机」。
   ⚠️ 就在锁内、通道已选好的此刻断（规矩 3）。 */
static void cut_vbus_locked(safety_snap_t *s, int rail, safety_ev_t ev)
{
    safety_rail_t *st = &s->st[rail - 1];
    (void)slot_vbus_enable_locked(rail, false);
    /* 三个序列里只有 EDL 会去写 `VBUS_EN`，它正跑着的话必须打断，否则下一拍
       它就把 VBUS 又抬回来了。abort 会走 I²C（收尾要写 Port0），所以**留到锁外**。 */
    if (t_actuator_seq(rail) == ACT_SEQ_EDL) {
        st->pend_abort = true;
    }
    st->pend_ev = (uint8_t)ev;
}

/* SR-5：关这一路。**先断 VBUS（这里、锁内）再关 eFuse（锁外）** ——
   只关 eFuse 的话，插着 USB 的那几路会停在「手机只靠 VBUS 供着电」，
   那既不是开也不是关。 */
static void trip_rail_locked(safety_snap_t *s, int rail, safety_ev_t ev)
{
    safety_rail_t *st = &s->st[rail - 1];
    (void)slot_vbus_enable_locked(rail, false);
    st->pend_abort = true;
    st->pend_efuse = true;
    st->pend_ev = (uint8_t)ev;
}

static void judge_locked(safety_snap_t *s, int rail, uint32_t now)
{
    safety_rail_t *st = &s->st[rail - 1];
    const bool     en = rail_efuse_is_on(rail);

    /* `EFUSE_EN` 的上升沿：缓启动窗口从这一刻算起。 */
    if (en && !st->en_on) {
        st->en_ms = now;
    }
    st->en_on = en;

    /* 三道闸。任何一道不过就把圈数清零 —— 「关着的那几秒」不能算进
       「持续 20 ms」里，不然一开机就跳闸。 */
    if (!en || !s->rail_ok[rail - 1] || (now - st->en_ms) < T_SAFETY_SETTLE_MS) {
        st->dev_n = 0;
        return;
    }

    const int32_t mv  = s->rail[rail - 1].bus_mv;
    const int32_t ua  = s->rail[rail - 1].current_ua;
    const int32_t dev = mv - (int32_t)rail_setpoint_mv(rail);

    /* ---- SW-8（先跑）：只在 VBUS 通着的时候武装 -------------------------
       反灌的能量只可能从 VBUS 那一侧来，断开之后判据自然就解除武装了。
       先跑它是因为过压那种情况两条判据都成立：该先撤掉能量来源（VBUS），
       20 ms 后 SR-5 再决定要不要连这一路一起关掉。 */
    if (slot_vbus_is_on(rail)) {
        if (ua < T_SAFETY_REV_UA) {
            cut_vbus_locked(s, rail, SAFETY_EV_REVERSE);
        } else if (dev > T_SAFETY_DEV_MV) {
            cut_vbus_locked(s, rail, SAFETY_EV_PUSHED_UP);
        }
    }

    /* ---- SR-5：+100 / −250 mV 连着 T_SAFETY_DEV_HOLD_CYCLES 圈（= 持续 20 ms）---- */
    if (dev > T_SAFETY_DEV_MV || dev < -T_SAFETY_UNDER_MV) {
        if (st->dev_n < 0xFF) {
            st->dev_n++;
        }
        if (st->dev_n >= T_SAFETY_DEV_HOLD_CYCLES) {
            trip_rail_locked(s, rail, dev > 0 ? SAFETY_EV_OVER_VOLT : SAFETY_EV_UNDER_VOLT);
            /* ⚠️ 这里**故意不**把 `dev_n` 清零。关掉之后下一圈 `en` 就是 false，
               上面那道闸自己会清；而万一那次写引脚失败了（影子留在 on），
               下一圈 `dev_n` 已经够数，会**立刻再关一次**。 */
        }
    } else {
        st->dev_n = 0; /* 「持续」的意思是连续，中间回到范围内就从头数 */
    }
}

/* 一路的锁内正文：读 V/I → 读两个输入端口 → 跑判据。
   ⚠️ 器件读失败**不提前返回**：PCA9555 读不到不代表 INA226 也读不到，
   而判据只要有新鲜的电压就能出（规矩 2）。 */
static hal_err_t cycle_rail(void *arg)
{
    cycle_arg_t   *a = (cycle_arg_t *)arg;
    safety_snap_t *s = a->s;
    const int      rail = a->rail;
    hal_err_t      first = HAL_OK;

    ina226_vi_t vi;
    hal_err_t   e = ina226_read_vi(rail_cfg(rail)->ina_addr, &vi);
    s->rail_ok[rail - 1] = (e == HAL_OK);
    if (e == HAL_OK) {
        s->rail[rail - 1] = vi; /* 失败就留着上一圈的值，别清零 */
    } else {
        s->i2c_errors++;
        first = e;
    }

    pca9555_in_t in;
    e = slot_read_inputs_locked(rail, &in);
    s->io_ok[rail - 1] = (e == HAL_OK);
    if (e == HAL_OK) {
        s->io[rail - 1] = in;
    } else {
        s->i2c_errors++;
        if (first == HAL_OK) {
            first = e;
        }
    }

    /* 整片刀片不应答（T_SAFETY_BLADE_MUTE_N 的理由）。**不走 `trip_rail_locked()`**：
       它要先写这片 PCA9555 断 VBUS，而这片刚刚 NAK 过 —— 再发一次只是白等十几 ms 的
       IDF NAK 超时、把本圈预算吃光（规矩 4）。只关 eFuse（锁外，GPIO 直连，永远关得掉）。
       判据本身不发任何事务：两个 ok 位是上面刚读出来的。 */
    safety_rail_t *st = &s->st[rail - 1];
    if (rail_efuse_is_on(rail) && !s->rail_ok[rail - 1] && !s->io_ok[rail - 1]) {
        if (st->mute_n < 0xFF) {
            st->mute_n++;
        }
        if (st->mute_n >= T_SAFETY_BLADE_MUTE_N) {
            st->pend_abort = true;
            st->pend_efuse = true;
            st->pend_ev = (uint8_t)SAFETY_EV_BLADE_MUTE;
            /* 同 SR-5：不清零。关掉之后下一圈 en 为 false 自然清；写引脚失败的话下一圈立刻再关。 */
        }
    } else {
        st->mute_n = 0;
    }

    /* 判据就在这里跑：断 VBUS 要在本路通道选好、锁还持着的此刻（规矩 3）。
       rail_ok 为 false 时它第一道闸就返回，不会盖掉上面攒下的 BLADE_MUTE。 */
    judge_locked(s, rail, a->now);
    return first;
}

static hal_err_t cycle_input(void *arg)
{
    safety_snap_t *s = (safety_snap_t *)arg;
    ina226_vi_t    vi;
    const hal_err_t e = ina226_read_vi(INA226_ADDR_INPUT, &vi);

    s->input_ok = (e == HAL_OK);
    if (e == HAL_OK) {
        s->input = vi;
    } else {
        s->i2c_errors++;
    }
    return e;
}

void t_safety_init(safety_snap_t *s)
{
    assert(s != NULL);
    memset(s, 0, sizeof(*s));
    for (int i = 0; i < PHONERAIL_RAIL_COUNT; i++) {
        s->present[i] = true; /* 与 slot.c 的初值一致：读到之前当它在位 */
    }
    /* 游标对齐到「现在」，否则第一圈会把启动期间（器件初始化）的事务算进来。 */
    s->bus_cursor = hal_i2c_acct_bits();
    s->txn_cursor = hal_i2c_acct_txns();
}

/* ---- 锁外：把锁内攒下的动作做掉 ------------------------------------------ */

static void apply_pending(safety_snap_t *s, uint32_t now)
{
    for (int rail = 1; rail <= PHONERAIL_RAIL_COUNT; rail++) {
        safety_rail_t *st = &s->st[rail - 1];
        if (st->pend_ev == (uint8_t)SAFETY_EV_NONE) {
            continue;
        }

        /* 顺序：先 abort 再关 eFuse。反过来有个真实的洞 —— 硬重启的 `HR_OFF`
           状态会在 4 s 后自己把 `EFUSE_EN` 抬回去，于是「关掉的路」几秒钟之后
           又带电了。abort 之后那一路只会走收尾（只写 Port0），再也够不到 EN 线。 */
        if (st->pend_abort) {
            t_actuator_abort(rail);
        }
        if (st->pend_efuse) {
            (void)rail_efuse_enable(rail, false);
        }
        emit(s, rail, (safety_ev_t)st->pend_ev, now);

        st->pend_ev = (uint8_t)SAFETY_EV_NONE;
        st->pend_efuse = false;
        st->pend_abort = false;
    }
}

/* ⭐ R2：刀片不在位就不许带电（计划 §5.8）。空槽上电 = 金手指裸露带 12V。
   只在「本来是开着的」时候动作，否则每圈都会对着五个空槽刷事件。 */
static void check_present(safety_snap_t *s, uint32_t now)
{
    /* ⚠️ 先把五根脚重读一遍再判。`slot_present()` 读的是缓存（读失败时要能
       保留上一次的值），不 poll 的话缓存停在 `slot_prsnt_init()` 那一刻，
       现象是**拔掉刀片永远判不出来**，而且一个错误码都不会有。
       这一步是纯 GPIO，不占总线，所以每圈都做。 */
    slot_prsnt_poll();

    for (int rail = 1; rail <= PHONERAIL_RAIL_COUNT; rail++) {
        s->present[rail - 1] = slot_present(rail);
        if (s->present[rail - 1] || !rail_efuse_is_on(rail)) {
            continue;
        }
        t_actuator_abort(rail);
        (void)rail_efuse_enable(rail, false);
        /* VBUS 也尽力断一次。⚠️ 刀片**真的**被拔了的话这一步必然 NAK
           （PCA9555 跟着刀片一起走了），那没关系；它挡的是另一种情形：
           `PRSNT#` 接触不良 / 金手指 B31 虚接 —— 刀片还在、PCA9555 还应答，
           这时只关 eFuse 会留下「直供断了但 USB 还灌着」的半开状态。
           放在关 eFuse **之后**：断电优先，而这一步要走 I²C、可能等锁。 */
        (void)slot_vbus_enable(rail, false);
        emit(s, rail, SAFETY_EV_SLOT_EMPTY, now);
    }
}

/* ⭐ R2：mux 是单点（计划 §5.8）。它一挂，五路的电压电流**全部看不见**，
   SR-5 的整条监控链断了 —— 这时「继续供电但看不见」比「停机」危险。
   ⚠️ 先脉冲一次 `MUX_RST#` 再判：一次总线毛刺就停五台机器，代价太大。 */
static void check_mux(safety_snap_t *s, uint32_t now)
{
    if (i2cbus_select_fail_streak() < T_SAFETY_MUX_FAIL_N) {
        s->mux_step = 0; /* 选中过一次 ⇒ 这一轮结束（判据数的是「连续」，不是「累计」） */
        return;
    }
    if (s->mux_step == 0) {
        /* 这一轮还没试过复位 —— 先脉冲一次 `MUX_RST#`，下一圈再看。
           ⚠️ 这一步走到哪了要记在 `mux_step` 里，**不能拿 `i2cbus_recoveries()` 和
           `s->mux_recovers` 比**：复位之后把后者同步成前者，两个数就永远相等，
           于是每一圈都走这条分支 —— `MUX_RST#` 每 20 ms 脉冲一次、五路永远断不掉。 */
        (void)i2cbus_recover();
        s->mux_recovers++;
        s->mux_step = 1;
        return;
    }
    if (s->mux_step >= 2) {
        /* 这一轮已经全断过了。再断一次没有任何用处，而每 20 ms 刷一条 MUX_LOST
           会把事件日志那块 flash 写穿 —— 判据「一直在响」和判据「不灵」一样糟。
           等某一次选通成功（streak 归零）之后才重新武装。 */
        return;
    }
    /* 复位过了还是连续失败：认定失联。 */
    s->mux_step = 2;
    s->mux_trips++;
    for (int rail = 1; rail <= PHONERAIL_RAIL_COUNT; rail++) {
        t_actuator_abort(rail);
    }
    (void)rail_all_off(); /* 前半段（断 VBUS）多半也会失败，那没关系 —— eFuse 是 GPIO */
    emit(s, 0, SAFETY_EV_MUX_LOST, now);
}

/* 本圈还允许在等锁上花多久：单次上限与「整圈预算的余额」取小的那个。
   ⚠️ 这里量的是**墙钟**，所以事务本身花掉的时间也算在内 —— 那是对的：
   预算要挡住的是「这一圈把整个周期占满」，而不只是「等锁等太久」。 */
static uint32_t lock_budget_left(uint32_t t0)
{
    const uint32_t spent = hal_time_now_ms() - t0;
    if (spent >= T_SAFETY_LOCK_BUDGET_MS) {
        return 0;
    }
    const uint32_t left = T_SAFETY_LOCK_BUDGET_MS - spent;
    return left < T_SAFETY_LOCK_TIMEOUT_MS ? left : T_SAFETY_LOCK_TIMEOUT_MS;
}

hal_err_t t_safety_step(safety_snap_t *s)
{
    assert(s != NULL);
    s->cycles++;

    const uint32_t now = hal_time_now_ms();
    hal_err_t      first = HAL_OK;
    bool           skipped = false;

    /* 本圈还能在等锁上花多久（`T_SAFETY_LOCK_BUDGET_MS` 那一段的理由）。
       返回 0 表示预算用完了 —— 剩下的路这一圈不读了。 */
    const uint32_t t0 = now;

    /* ⛔ 空槽**不走总线**（2026-09-27 上板实测）。原来五路无条件各读一次：空槽的 INA226 /
       PCA9555 必然 NAK，而 IDF 驱动每次 NAK 要十几 ms（含它自己的错误日志），
       槽 1 那两次就把 10 ms 整圈预算吃光 ⇒ 槽 2~5 **和主干上的输入侧 0x43** 一圈都没读到，
       现象是 `txns=2`、每圈 skipped、「最迟 57 ms」—— 空机时输入侧的监控整个是瞎的。
       在位脚是纯 GPIO，先 poll 再按它挑。`check_present()` 在后面照旧再 poll 一次、照旧负责断电。 */
    slot_prsnt_poll();

    /* 五路，一路一次 `xact`（通道查表，**不许算** —— rail_cfg.h 那条禁令）。 */
    for (int rail = 1; rail <= PHONERAIL_RAIL_COUNT; rail++) {
        if (!slot_present(rail)) {
            s->rail_ok[rail - 1] = false; /* 没读 ≠ 读到了：别让上一圈的旧值冒充新鲜 */
            s->io_ok[rail - 1] = false;
            continue;
        }
        const uint32_t budget = lock_budget_left(t0);
        if (budget == 0) {
            skipped = true; /* 整圈预算用完：剩下的路本圈作废，下一拍重来 */
            break;
        }
        cycle_arg_t a = { s, rail, now };
        const hal_err_t e = i2cbus_xact(slot_mux_ch(rail), budget, cycle_rail, &a);
        if (e == HAL_ERR_TIMEOUT) {
            skipped = true; /* 规矩 1：不死等。这一路这一圈作废 */
        }
        if (e != HAL_OK && first == HAL_OK) {
            first = e;
        }
    }

    /* 第六片：输入侧，挂在主干上（不在任何通道后面）。 */
    {
        const uint32_t budget = lock_budget_left(t0);
        if (budget == 0) {
            skipped = true;
        } else {
            const hal_err_t e = i2cbus_xact(I2CBUS_CH_TRUNK, budget, cycle_input, s);
            if (e == HAL_ERR_TIMEOUT) {
                skipped = true;
            }
            if (e != HAL_OK && first == HAL_OK) {
                first = e;
            }
        }
    }

    if (skipped) {
        s->skipped++;
    }

    /* 放锁之后：关 eFuse（GPIO）、打断序列、记事件，再跑两条整板级判据。 */
    apply_pending(s, now);
    check_present(s, now);
    check_mux(s, now);

    s->bus_us = hal_i2c_acct_window_us(&s->bus_cursor);
    {
        const uint32_t t = hal_i2c_acct_txns();
        s->txns = t - s->txn_cursor;
        s->txn_cursor = t;
    }
    return first;
}

uint32_t t_safety_cycle_txns(void)
{
    /* 每路：选通道(1) + INA226 读 V(1) + 读 I(1) + PCA9555 读两个输入端口(1) = 4
       主干：选通道(1) + INA226 读 V(1) + 读 I(1) = 3 */
    return (uint32_t)PHONERAIL_RAIL_COUNT * 4u + 3u;
}

uint32_t t_safety_budget_us(void)
{
    /* 逐条数，按事务形状分两类：
         mux 选通  —— 纯写 1 字节（**没有寄存器指针**，见 tca9548a.h）
         寄存器读  —— 写 1 字节寄存器号 + 读 2 字节 */
    const uint32_t selects = (uint32_t)PHONERAIL_RAIL_COUNT + 1u; /* 五路 + 回主干 */
    const uint32_t reads   = t_safety_cycle_txns() - selects;
    const uint64_t bits = (uint64_t)selects * hal_i2c_txn_bits(1, 0) +
                          (uint64_t)reads * hal_i2c_txn_bits(1, 2);
    return (uint32_t)hal_i2c_bits_to_us(bits);
}

uint32_t t_safety_load_permille(uint32_t bus_us, uint32_t period_ms)
{
    if (period_ms == 0) {
        return 0;
    }
    return (uint32_t)(((uint64_t)bus_us * 1000ull) / ((uint64_t)period_ms * 1000ull));
}

int t_safety_format(const safety_snap_t *s, char *buf, size_t len)
{
    assert(s != NULL && buf != NULL && len > 0);
    const uint32_t period = task_desc(TASK_SAFETY)->period_ms;
    const uint32_t pm = t_safety_load_permille(s->bus_us, period);
    return snprintf(buf, len,
                    "t_safety: cycles=%u skipped=%u i2c_err=%u txns=%u bus=%u us/%u ms "
                    "load=%u.%u%% events=%u mux(rst=%u trip=%u)",
                    (unsigned)s->cycles, (unsigned)s->skipped, (unsigned)s->i2c_errors,
                    (unsigned)s->txns, (unsigned)s->bus_us, (unsigned)period,
                    (unsigned)(pm / 10u), (unsigned)(pm % 10u), (unsigned)s->events,
                    (unsigned)s->mux_recovers, (unsigned)s->mux_trips);
}