# -*- coding: utf-8 -*-
"""
FB_LowPassFilter.scl 算法验证仿真
用 Python 1:1 复刻 SCL 源码的离散递推，验证：
  场景A：阶跃响应        —— 实测到达终值 63.2% 的时刻是否等于设定的 Tf
  场景B：噪声抑制        —— 滤波前后标准差对比（对照理论衰减 α/(2-α)）
  场景C：无扰启动 / 复位 —— 首次输出应等于当前输入，而非从 0 爬升
  场景D：超窗冻结        —— 模拟 4~20mA 断线，输出应保持而不跟着跑
  场景E：旁路 / 参数非法 —— bEnable=FALSE 与 Ts<=0 的处理
  场景F：NaN / ±Inf 防护 —— v1.1 位模式检测 vs v1 自比较写法的对照
末尾导出 SVG 曲线坐标（直接粘进图里用）。
"""

import math
import random
import struct

TS = 0.1          # 调用周期 s（对应 rTs）
SIM_END = 100     # 仿真步数


def real_bits(v):
    """把值按 S7 REAL（32 位单精度 IEEE 754）取位模式。
    Python 的 float 是 64 位，先压到单精度才是 S7 的真实行为。"""
    try:
        return struct.unpack('>I', struct.pack('>f', v))[0]
    except OverflowError:                   # 超出单精度范围 -> S7 里就是 ±Inf
        return 0x7F800000 if v > 0 else 0xFF800000


def is_nan_or_inf(v):
    """v1.1 的检测方法：指数位（bit30..bit23）全 1 = NaN 或 ±Inf"""
    return (real_bits(v) & 0x7F800000) == 0x7F800000


def is_nan_selfcompare(v):
    """v1 的旧写法：#rRaw <> #rRaw（Python 等价：v != v）"""
    return v != v


def lpf_fb(raw, st, tf, ts, enable=True, hold=False,
           range_chk=False, lo=0.0, hi=100.0, reset=False,
           detect=is_nan_or_inf):
    """1:1 复刻 FB_LowPassFilter 的每个周期
    返回 (rOut, rAlpha, bFrozen, bInvalid, bParaFault)"""
    # ---- 1. 参数检查：算 α ----
    para_fault = False
    tf_eff = tf
    if tf_eff < 0.0:
        tf_eff = 0.0

    if ts <= 0.0:
        para_fault = True
        alpha = 1.0
    elif tf_eff == 0.0:
        alpha = 1.0
    else:
        alpha = ts / (tf_eff + ts)

    # ---- 2. 状态初始化 ----
    if reset:
        st['rState'] = raw
        st['bPrimed'] = True
    if not st['bPrimed']:
        st['rState'] = raw
        st['bPrimed'] = True

    # ---- 3. 采样有效性 ----
    invalid = detect(raw)
    frozen = False
    if hold:
        frozen = True
    elif invalid:
        frozen = True
    elif range_chk and (raw < lo or raw > hi):
        frozen = True

    # ---- 4. 递推 ----
    if frozen:
        out = st['rState']
    else:
        if enable:
            st['rState'] = st['rState'] + alpha * (raw - st['rState'])
        else:
            st['rState'] = raw
        out = st['rState']

    return out, alpha, frozen, invalid, para_fault


def new_state():
    return {'rState': 0.0, 'bPrimed': False}


print('=' * 62)
print('场景A：阶跃响应 0 -> 100%，检验时间常数是否与设定一致')
print('=' * 62)
print(f'{"Tf[s]":>6} {"α":>8} {"63.2%时刻[s]":>13} {"95%时刻[s]":>12} {"2s时刻值":>10}')
for tf in (0.2, 0.5, 1.0, 2.0, 5.0):
    st = new_state()
    t63 = t95 = None
    for k in range(400):
        raw = 0.0 if k == 0 else 100.0
        y, a, _, _, _ = lpf_fb(raw, st, tf, TS)
        t = (k - 1) * TS          # 阶跃发生在第 1 步
        if k >= 1:
            if t63 is None and y >= 63.2:
                t63 = t
            if t95 is None and y >= 95.0:
                t95 = t
    print(f'{tf:>6.1f} {a:>8.3f} {t63:>13.2f} {t95:>12.2f} {y:>10.1f}')
print('  -> 63.2% 时刻应≈Tf（离散化误差 <1 个 Ts 即合格）')
print()

print('=' * 62)
print('场景B：噪声抑制（信号 50%，白噪声 σ=1.5，Ts=0.1s，2000 步）')
print('=' * 62)
random.seed(20260914)
sig = [50.0 + random.gauss(0, 1.5) for _ in range(2000)]
sigma_in = math.sqrt(sum((x - 50.0) ** 2 for x in sig) / len(sig))
print(f'{"Tf[s]":>6} {"α":>8} {"输出σ":>9} {"实测衰减":>9} {"理论衰减":>9} {"滞后(步)":>9}')
for tf in (0.2, 0.5, 1.0, 2.0, 5.0):
    st = new_state()
    out = []
    for x in sig:
        y, a, _, _, _ = lpf_fb(x, st, tf, TS)
        out.append(y)
    seg = out[50:]                      # 跳过启动段
    sigma_out = math.sqrt(sum((y - 50.0) ** 2 for y in seg) / len(seg))
    theo = math.sqrt(a / (2 - a))       # 白噪声通过一阶滤波的方差衰减因子
    print(f'{tf:>6.1f} {a:>8.3f} {sigma_out:>9.2f} {sigma_out/sigma_in:>9.3f} '
          f'{theo:>9.3f} {tf/TS:>9.0f}')
print('  -> 输出σ/输入σ 与理论值吻合说明递推系数实现无误')
print()

print('=' * 62)
print('场景C：无扰启动与复位（输入稳定在 80%）')
print('=' * 62)
st = new_state()
y1, _, _, _, _ = lpf_fb(80.0, st, 2.0, TS)
print(f'  上电首次调用: 输入 80.00 -> 输出 {y1:.2f}   (应=80，非从 0 爬升)')
for _ in range(20):
    y2, _, _, _, _ = lpf_fb(30.0, st, 2.0, TS)
print(f'  输入由 80 突降到 30，跑一段时间后输出 = {y2:.2f}')
y3, _, _, _, _ = lpf_fb(95.0, st, 2.0, TS, reset=True)
print(f'  输入跳到 95 且 bReset=TRUE: 输出 = {y3:.2f}   (应=95，立即对齐)')
print()

print('=' * 62)
print('场景D：超窗冻结（模拟 4~20mA 断线，窗口 0~100%）')
print('=' * 62)
st = new_state()
log = []
for k in range(60):
    raw = 60.0 if k < 20 else 3276.7    # 断线 -> 输入飞到 3276.7
    y, _, fz, _, _ = lpf_fb(raw, st, 1.0, TS, range_chk=True, lo=0.0, hi=100.0)
    log.append((k, raw, y, fz))
print(f'  断线前输出 = {log[19][2]:.2f}%')
print(f'  断线第 1 步: 输入 {log[20][1]:.1f} -> 输出 {log[20][2]:.2f}%  冻结标志={log[20][3]}')
print(f'  断线第 40 步: 输出 {log[59][2]:.2f}%   (保持不跟随 -> 故障值没被滤进来)')
st2 = new_state()
for k in range(60):
    raw = 60.0 if k < 20 else 3276.7
    yb, _, _, _, _ = lpf_fb(raw, st2, 1.0, TS)
print(f'  对照(不启用窗口): 断线第 40 步输出 = {yb:.1f}%  (已被污染，需 3*Tf 才拉回)')
print()

print('=' * 62)
print('场景E：旁路与参数非法')
print('=' * 62)
st = new_state()
for _ in range(50):
    lpf_fb(70.0, st, 2.0, TS)
yb, ab, _, _, _ = lpf_fb(20.0, st, 2.0, TS, enable=False)
print(f'  bEnable=FALSE: 输入 20 -> 输出 {yb:.2f}  (应=20，直通)')
yg, ag, _, _, _ = lpf_fb(70.0, st, 2.0, TS)
print(f'  重新使能后第一周期: 输出 {yg:.2f}  (从 20 开始平滑，无跳变)')
zp, ap, _, _, pf = lpf_fb(70.0, st, 2.0, 0.0)
print(f'  rTs=0: 输出 {zp:.2f}, α={ap:.2f}, bParaFault={pf}  (按直通+报警)')
zn, an, _, _, _ = lpf_fb(70.0, st, -1.0, TS)
print(f'  rTf=-1: α={an:.2f}  (按直通处理，无非法系数)')
print()

print('=' * 62)
print('场景F：NaN / ±Inf 防护  —— v1.1 位模式 vs v1 自比较')
print('=' * 62)
print(f'{"测试值":>16} {"位模式":>12} {"v1.1 位模式":>12} {"v1 自比较":>11}')
for label, v in (('正常 60.0', 60.0), ('NaN', float('nan')),
                 ('+Inf', float('inf')), ('-Inf', float('-inf')),
                 ('1e39(单精度溢出)', 1e39)):
    print(f'{label:>16} {real_bits(v):>12X} {str(is_nan_or_inf(v)):>12} '
          f'{str(is_nan_selfcompare(v)):>11}')
print('  -> ±Inf 在"自比较"写法下全部漏网，这正是 v1.1 换写法的原因')
print()

for label, detect in (('v1.1 本版（位模式）', is_nan_or_inf),
                      ('v1   旧版（自比较）', is_nan_selfcompare)):
    st = new_state()
    row = {}
    for k in range(130):
        if k < 20:
            raw = 60.0                    # 正常
        elif k == 20:
            raw = float('nan')            # 注入 NaN
        elif k == 40:
            raw = float('inf')            # 注入 +Inf
        else:
            raw = 60.0                    # 故障消失，恢复正常输入
        y, _, fz, inv, _ = lpf_fb(raw, st, 1.0, TS, detect=detect)
        row[k] = (y, fz, inv)
    print(f'[{label}]')
    print(f'   正常时(第19步)   : 输出 {row[19][0]:>12.2f}   冻结={row[19][1]}')
    print(f'   注入 NaN(第20步) : 输出 {row[20][0]:>12.2f}   冻结={row[20][1]}  bInvalid={row[20][2]}')
    print(f'   注入 +Inf(第40步): 输出 {row[40][0]:>12.2f}   冻结={row[40][1]}  bInvalid={row[40][2]}')
    if row[40][0] == float('inf'):
        print(f'   恢复输入(第120步): 输出 {row[120][0]:>12.2f}   <-- 永久污染，再也回不来')
    else:
        print(f'   故障清除后(第60步): 输出 {row[60][0]:>12.2f}   <-- 状态干净，继续正常滤波')
    print()

# ==================== 导出 SVG 曲线坐标 ====================
# 绘图约定：x 0~10s -> 70~620px；y 由 ylo~yhi 映射到 220~40px
def px(t, y, ylo=0.0, yhi=100.0):
    x = 70 + (t / 10.0) * 550
    py = 220 - ((y - ylo) / (yhi - ylo)) * 180
    return f'{x:.1f},{py:.1f}'


print('=' * 62)
print('SVG 数据（阶跃响应 / 噪声抑制）')
print('=' * 62)
for tf in (0.2, 0.5, 2.0):
    st = new_state()
    pts = []
    for k in range(SIM_END):
        raw = 0.0 if k == 0 else 100.0
        y, _, _, _, _ = lpf_fb(raw, st, tf, TS)
        if k % 2 == 0:
            pts.append(px(k * TS, y))
    print(f'SVG_STEP_TF{tf}={" ".join(pts)}')

random.seed(7)
st = new_state()
raw_pts, flt_pts = [], []
YLO, YHI = 30.0, 85.0          # 噪声图 y 轴放大到 30~85%，看波动更清楚
for k in range(SIM_END):
    raw = 50.0 + random.gauss(0, 6.0)
    y, _, _, _, _ = lpf_fb(raw, st, 0.5, TS)
    if k % 2 == 0:
        raw_pts.append(px(k * TS, raw, YLO, YHI))
        flt_pts.append(px(k * TS, y, YLO, YHI))
print(f'SVG_NOISE_RAW={" ".join(raw_pts)}')
print(f'SVG_NOISE_FLT={" ".join(flt_pts)}')