/*
 * test_vac_ref_reanchor.c — V4.36.41 补丁草案单测 (gcc 隔离, DLD154V4B)
 *
 * 镜像 = TaskLoop.c V4.36.41 草案:  vac_ref() + origin_set_atomic() + env_resync_apply()
 * 覆盖 5 件事:
 *   ① 现象复现: env 重校准(黄灯常亮补偿)后判定参考未同步 ⇒ 进入线被拖低 Δ (修复前)
 *   ② 补丁效果: origin_set_atomic 同步 ⇒ 进入线立即回到 Origin−dlt (与断电上电同口径)
 *   ③ 回归: V4.36.33 的 1×dlt 门控语义不变; 小步跳变不吃掉 EMA 跟踪; 上电 EMA=0 语义
 *   ④ 新发现(2026-09-11): EMA 整数步进 d/128 有 ±127 counts 死区 ⇒ 真空 EMA 长期停在
 *      真空下方 ~127 counts(≈2.6×dlt) ⇒ vac_ref 偏低 ⇒ 触发高度持续漂移(断电才复位)
 *   ⑤ 修复 D(余数累加器) 后 EMA 无死区收敛到 0 counts 偏差
 *
 * 编译: gcc -O0 -Wall -o /tmp/t_vac tests/test_vac_ref_reanchor.c && /tmp/t_vac
 * 镜像宏与固件一致(改 VAC_/ORIGIN_JUMP_/SENS 时同步此处!)
 */
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <stdint.h>

/* ===== 镜像固件宏 (TaskLoop.h / TaskLoop.c) ===== */
static const uint16_t SensTable[4] = { 337, 71, 38, 25 };   /* SENS3 = 25 */
#define SENS_LEVEL            3
#define VAC_REF_DELTA_DLT     1        /* V4.36.33: vac_ref 重锚阈值 (×dlt) */
#define ORIGIN_JUMP_REANCH_DLT 1       /* V4.36.41 草案: 原子换 Origin 重锚阈值 (×dlt) */
#define VACUUM_EMA_ALFA       128      /* V4.36.31: EMA 平滑因子 (τ≈1.28s@10ms) */

/* ===== 镜像全局 ===== */
static uint32_t loop1_Origin = 0, loop1_CAPVD = 0, loop1_dlt_ORG = 0;
static uint32_t g_vacuum_ema = 0;
static int32_t  g_vac_frac   = 0;      /* 修复 D: EMA 余数累加器 */

static uint32_t calc_dlt(uint32_t origin)
{
    return (uint32_t)(((uint64_t)origin * SensTable[SENS_LEVEL]) >> 16);
}

/* ===== 镜像: vac_ref() (TaskLoop.c:287-299) ===== */
static uint32_t vac_ref(void)
{
    if (g_vacuum_ema == 0) return loop1_Origin;
    if ((int32_t)loop1_Origin - (int32_t)g_vacuum_ema >
        (int32_t)(loop1_dlt_ORG * VAC_REF_DELTA_DLT))
        return g_vacuum_ema;
    return loop1_Origin;
}

/* 进入线 (TaskLoop.c:1432 的判定阈值面) */
static uint32_t enter_line(void) { return vac_ref() - loop1_dlt_ORG; }

/* ===== 镜像: origin_set_atomic() (V4.36.41 草案, B1 护栏) ===== */
static void origin_set_atomic(uint32_t new_origin)
{
    int32_t _jump = (int32_t)new_origin - (int32_t)loop1_Origin;
    if (_jump < 0) _jump = -_jump;
    loop1_Origin = new_origin;
    if (g_vacuum_ema == 0 || loop1_dlt_ORG == 0 ||
        _jump > (int32_t)(loop1_dlt_ORG * ORIGIN_JUMP_REANCH_DLT)) {
        g_vacuum_ema = new_origin;
        g_vac_frac   = 0;
    }
}

/* ===== 镜像: env_resync_apply() 修复前 / 修复后 ===== */
static void env_resync_apply_old(uint32_t new_origin)   /* V4.36.40: 只换 Origin */
{
    loop1_Origin = new_origin;
}
static void env_resync_apply_new(uint32_t new_origin)   /* V4.36.41 草案 (A+B) */
{
    origin_set_atomic(new_origin);
}

/* ===== 镜像: EMA 步进 —— 现状公式 vs 修复 D ===== */
static void ema_step_cur(void)      /* V4.36.35 现状: d/128 整数截断 (死区 ±127) */
{
    int32_t d = (int32_t)loop1_CAPVD - (int32_t)g_vacuum_ema;
    int32_t lim = (int32_t)(loop1_dlt_ORG * 3);
    if (d > lim) d = lim; else if (d < -lim) d = -lim;
    if (d != 0) g_vacuum_ema += (uint32_t)(d / VACUUM_EMA_ALFA);
}
static void ema_step_fix(void)      /* 修复 D: 余数累加 (无死区) */
{
    int32_t d = (int32_t)loop1_CAPVD - (int32_t)g_vacuum_ema;
    int32_t lim = (int32_t)(loop1_dlt_ORG * 3);
    if (d > lim) d = lim; else if (d < -lim) d = -lim;
    if (d != 0) {
        g_vac_frac += d;
        int32_t st = g_vac_frac / VACUUM_EMA_ALFA;
        g_vac_frac -= st * VACUUM_EMA_ALFA;
        g_vacuum_ema += (uint32_t)st;
    }
}

/* ===== 夹具 ===== */
#define VAC_TRUE   130506UL      /* 真实真空 CAPVD (devlog J1 实测) */
#define VAC_ANCHOR 130636UL      /* env 学习后 Origin (锚高 130 counts = 2.65×dlt) */
static void reset_all(uint32_t origin)
{
    loop1_Origin  = origin;
    loop1_CAPVD   = origin;
    loop1_dlt_ORG = calc_dlt(origin);
    g_vacuum_ema  = origin;
    g_vac_frac    = 0;
}

int main(void)
{
    printf("test_vac_ref_reanchor (V4.36.41 草案: A+B 同步 + D 死区)\n");

    /* ================= 场景 1: 复现 + 补丁效果 ================= */
    reset_all(VAC_ANCHOR);
    uint32_t dlt_anchor = loop1_dlt_ORG;
    uint32_t dlt_true   = calc_dlt(VAC_TRUE);
    printf("1. 参数: VAC_TRUE=%u(真空) VAC_ANCHOR=%u(补偿后 Origin) Δ=%u counts | dlt=%u(SENS3)\n",
           (unsigned)VAC_TRUE, (unsigned)VAC_ANCHOR, (unsigned)(VAC_ANCHOR - VAC_TRUE),
           (unsigned)dlt_anchor);

    reset_all(VAC_TRUE);                      /* 补偿前: Origin=真空, EMA=真空 */
    env_resync_apply_old(VAC_ANCHOR);         /* 修复前: 只换 Origin, EMA 停旧真空 */
    uint32_t line_old = enter_line();
    printf("   ① 修复前: Origin=%u EMA=%u 进入线=%u | 正确应为 %u ⇒ 低 %u counts"
           " = %.2f×dlt = %.3f%% Δf/f\n",
           (unsigned)loop1_Origin, (unsigned)g_vacuum_ema, (unsigned)line_old,
           (unsigned)(VAC_ANCHOR - dlt_anchor),
           (unsigned)((VAC_ANCHOR - dlt_anchor) - line_old),
           ((VAC_ANCHOR - dlt_anchor) - line_old) / (double)dlt_anchor,
           ((VAC_ANCHOR - dlt_anchor) - line_old) * 100.0 / VAC_ANCHOR);
    assert(g_vacuum_ema == VAC_TRUE);                       /* EMA 未同步(缺陷) */
    assert(line_old == VAC_TRUE - dlt_true);                /* 进入线被留在旧真空 */
    assert((VAC_ANCHOR - dlt_anchor) - line_old == 130);    /* 整整低 Δ=130 */

    reset_all(VAC_TRUE);
    env_resync_apply_new(VAC_ANCHOR);         /* 修复后 (A+B) */
    printf("   ② 修复后: Origin=%u EMA=%u 进入线=%u | 与断电上电口径一致 ⇒ 低 0 counts\n",
           (unsigned)loop1_Origin, (unsigned)g_vacuum_ema, (unsigned)enter_line());
    assert(g_vacuum_ema == VAC_ANCHOR);
    assert(vac_ref() == VAC_ANCHOR);
    assert(enter_line() == VAC_ANCHOR - dlt_anchor);
    printf("1. 复现+修复 OK（补偿后进入线由'低 Δ'恢复为 Origin−dlt）\n");

    /* ================= 场景 2: B1 护栏（大步重锚 / 小步不动） ================= */
    reset_all(VAC_TRUE);
    origin_set_atomic(VAC_TRUE - 20);         /* 步长 20 ≤ 1×dlt ⇒ EMA 不动 */
    printf("2. 小步 20(≤1×dlt): Origin=%u EMA=%u ⇒ EMA 未动 ✓\n",
           (unsigned)loop1_Origin, (unsigned)g_vacuum_ema);
    assert(g_vacuum_ema == VAC_TRUE);
    origin_set_atomic(VAC_TRUE + 130);        /* 步长 150 > 1×dlt ⇒ 重锚 */
    printf("   大步 150(>1×dlt): Origin=%u EMA=%u ⇒ 已重锚 ✓\n",
           (unsigned)loop1_Origin, (unsigned)g_vacuum_ema);
    assert(g_vacuum_ema == VAC_TRUE + 130);
    printf("2. 护栏 OK（不吃掉 EMA 的正常跟踪，也不容忍大偏差）\n");

    /* ================= 场景 3: V4.36.33 门控语义回归 ================= */
    reset_all(VAC_ANCHOR);
    g_vacuum_ema = VAC_ANCHOR - 30;           /* 边缘悬停拉低 0.61×dlt */
    printf("3. Origin−EMA=30(0.61×dlt): vac_ref=%u ⇒ 用 Origin ✓(门控保留)\n",
           (unsigned)vac_ref());
    assert(vac_ref() == VAC_ANCHOR);
    g_vacuum_ema = VAC_ANCHOR - 130;          /* 老锚错 2.65×dlt */
    printf("   Origin−EMA=130(2.65×dlt): vac_ref=%u ⇒ 用 EMA ✓(老场景保留)\n",
           (unsigned)vac_ref());
    assert(vac_ref() == VAC_ANCHOR - 130);
    printf("3. 门控回归 OK\n");

    /* ================= 场景 4: 上电/判稳语义 ================= */
    reset_all(VAC_ANCHOR);
    g_vacuum_ema = 0;                          /* INIT 清零 */
    assert(vac_ref() == VAC_ANCHOR);
    g_vacuum_ema = loop1_Origin;               /* 判稳完成建立初值 */
    assert(vac_ref() == VAC_ANCHOR && enter_line() == VAC_ANCHOR - loop1_dlt_ORG);
    printf("4. 上电(EMA=0→Origin)语义 OK: 进入线=%u\n", (unsigned)enter_line());

    /* ================= 场景 5: EMA 整数步进死区（新发现） ================= */
    reset_all(VAC_ANCHOR);
    loop1_CAPVD  = VAC_ANCHOR;                 /* 真空就在 Origin */
    g_vacuum_ema = VAC_ANCHOR - 200;           /* 被拖低 200 counts 后追赶 */
    { int i; for (i = 0; i < 4000; i++) ema_step_cur(); }
    { uint32_t gap = loop1_Origin - g_vacuum_ema;
      printf("5. 现状公式 d/128 追赶 4000 样本(40s): EMA=%u 停在真空下方 %u counts"
             " (%.2f×dlt) ⇒ vac_ref=%s ⇒ 进入线低 %u counts\n",
             (unsigned)g_vacuum_ema, (unsigned)gap, gap / (double)loop1_dlt_ORG,
             (vac_ref() == loop1_Origin) ? "Origin" : "EMA", (unsigned)gap);
      assert(gap == 127);                      /* 死区: |d|<128 ⇒ 步进 0 */
      assert(vac_ref() == g_vacuum_ema);       /* 127 > 1×dlt ⇒ 判定参考被拉低 */
      assert(enter_line() == g_vacuum_ema - loop1_dlt_ORG);
    }
    reset_all(VAC_ANCHOR);
    loop1_CAPVD  = VAC_ANCHOR;
    g_vacuum_ema = VAC_ANCHOR - 200;
    g_vac_frac   = 0;
    { int i; for (i = 0; i < 4000; i++) ema_step_fix(); }
    { uint32_t gap = loop1_Origin - g_vacuum_ema;
      printf("   修复 D(余数累加)同条件: EMA=%u 偏差 %u counts ⇒ vac_ref=%s ⇒ 进入线=%u ✓\n",
             (unsigned)g_vacuum_ema, (unsigned)gap,
             (vac_ref() == loop1_Origin) ? "Origin" : "EMA", (unsigned)enter_line());
      assert(gap == 0);
      assert(vac_ref() == loop1_Origin);
      assert(enter_line() == loop1_Origin - loop1_dlt_ORG);
    }
    printf("5. 死区实测 OK（现状 127 counts 常驻偏差 → 修复 D 归零）\n");

    printf("ALL PASS (A+B 同步 / 护栏 / 门控回归 / 上电语义 / 死区)\n");
    return 0;
}