# -*- coding: utf-8 -*-
"""
FB_PID_Control.scl 算法验证仿真
用 Python 1:1 复刻 SCL 源码中的离散方程，在典型温度对象上验证：
  场景A：设定值阶跃（50%），考察超调与收敛
  场景B：输出限幅下的抗积分饱和（对照：无抗饱和版本）
  场景C：手/自动无扰切换
对象模型：一阶惯性 + 纯滞后 (FOPDT)，温度类典型对象
  dy/dt = (-y + Kp_plant * u(t - delay)) / T
"""

import math

TS = 1.0          # 采样周期 s（对应 rTs）
SIM_END = 1200    # 仿真时长 s

def pid_fb(sp, pv, st, kp, ti, td, tf, ts, out_max, out_min,
           manual, manual_out, reset):
    """1:1 复刻 FB_PID_Control 的每个周期"""
    if reset:
        st['rIntegral'] = 0.0; st['rDFilt'] = 0.0; st['rPrevPV'] = pv
        st['bFirst'] = True
        return 0.0, {'rError': 0.0, 'rP': 0.0, 'rI': 0.0, 'rD': 0.0}

    if st['bFirst']:
        st['rPrevPV'] = pv
        st['bFirst'] = False

    error = sp - pv
    rP = kp * error

    # 微分先行 + 一阶低通
    rDFilt = st['rDFilt']
    if td > 0.0 and ts > 0.0:
        rDRaw = -kp * td * (pv - st['rPrevPV']) / ts
        if tf > 0.0:
            alpha = ts / (tf + ts)
            rDFilt = rDFilt + alpha * (rDRaw - rDFilt)
        else:
            rDFilt = rDRaw
    else:
        rDFilt = 0.0
    st['rPrevPV'] = pv

    # 条件积分抗饱和
    rIntStep = 0.0
    if (not manual) and ti > 0.0 and ts > 0.0:
        rIntStep = kp * ts / ti * error
    rRaw = rP + st['rIntegral'] + rIntStep + rDFilt
    if rRaw >= out_max and error > 0.0:
        rIntStep = 0.0
    elif rRaw <= out_min and error < 0.0:
        rIntStep = 0.0
    st['rIntegral'] = st['rIntegral'] + rIntStep

    # 手/自动
    if manual:
        output = manual_out
        st['rIntegral'] = manual_out - rP - rDFilt
    else:
        rRaw = rP + st['rIntegral'] + rDFilt
        output = min(max(rRaw, out_min), out_max)

    return output, {'rError': error, 'rP': rP, 'rI': st['rIntegral'], 'rD': rDFilt}


def simulate(mode='normal'):
    """mode: normal / no_antivindup / manual_switch"""
    kp, ti, td, tf = 2.0, 300.0, 40.0, 1.0
    out_max, out_min = 40.0 if mode == 'no_antivindup' else 100.0, 0.0

    # 对象：K=1.5 %/%, T=80s, 滞后=5s（温度类典型对象）
    K, T, delay = 1.5, 80.0, 5
    u_buf = [0.0] * (delay + 1)
    pv = 0.0
    st = {'rIntegral': 0.0, 'rDFilt': 0.0, 'rPrevPV': 0.0, 'bFirst': True}

    manual = False
    manual_out = 0.0
    pv_log, out_log, bump = [], [], None
    prev_out = None
    out_max_eff = out_max

    for k in range(SIM_END):
        sp = 50.0 if k >= 10 else 0.0

        if mode == 'manual_switch':
            # 前 200s 手动输出 30%，之后切自动
            manual = k < 200
            manual_out = 30.0

        u, _ = pid_fb(sp, pv, st, kp, ti, td, tf, TS,
                      out_max, out_min, manual, manual_out, False)

        if mode == 'manual_switch' and k == 200:
            bump = abs(u - prev_out)   # 切换瞬间的输出跳变量

        u_buf.append(u)
        u_delayed = u_buf.pop(0)
        # 对象离散化（一阶欧拉）
        pv = pv + TS / T * (-pv + K * u_delayed)

        pv_log.append(pv); out_log.append(u); prev_out = u

    return pv_log, out_log, bump, out_max_eff


def analyze(pv, out, label, sp=50.0, settle_band=2.0):
    peak = max(pv)
    overshoot = (peak - sp) / sp * 100
    settle = None
    for i in range(len(pv) - 1, -1, -1):
        if abs(pv[i] - sp) > settle_band:
            settle = (i + 1)
            break
    settle_str = f'{settle}s 内未稳定' if settle and settle >= SIM_END else f'{settle if settle else 0}s 后进入±{settle_band}%带'
    final = pv[-1]
    print(f'[{label}]')
    print(f'  峰值 PV = {peak:.1f}%  |  超调 = {overshoot:.1f}%')
    print(f'  收敛: {settle_str}')
    print(f'  1200s 末值 PV = {final:.2f}% (目标 {sp}%)')
    print(f'  输出最大 = {max(out):.1f}%, 末值 = {out[-1]:.1f}%')
    print()


print('=' * 56)
print('场景A：设定值阶跃 0 -> 50%，Kp=2 Ti=300 Td=40 Ts=1')
print('=' * 56)
pv, out, _, _ = simulate('normal')
analyze(pv, out, 'A 常规阶跃响应')

print('=' * 56)
print('场景B：输出硬限幅 0~40%（强制饱和），考察抗积分饱和')
print('=' * 56)
pv2, out2, _, _ = simulate('normal')
# 重跑一遍：手写一个没有抗饱和的对照
K, T, delay = 1.5, 80.0, 5
for label, anti_windup in [('B 有抗饱和(本FB)', True), ('B 对照:无抗饱和', False)]:
    u_buf = [0.0] * (delay + 1)
    pv = 0.0; integ = 0.0; d_filt = 0.0; prev_pv = 0.0
    pv_log, out_log = [], []
    for k in range(SIM_END):
        sp = 50.0 if k >= 10 else 0.0
        error = sp - pv
        rP = 2.0 * error
        rDRaw = -2.0 * 40.0 * (pv - prev_pv) / TS
        d_filt += (TS / (1.0 + TS)) * (rDRaw - d_filt)
        prev_pv = pv
        integ += 2.0 * TS / 300.0 * error
        raw = rP + integ + d_filt
        output = min(max(raw, 0.0), 40.0)
        if anti_windup:
            if raw >= 40.0 and error > 0: integ -= 2.0 * TS / 300.0 * error
            elif raw <= 0.0 and error < 0: integ -= 2.0 * TS / 300.0 * error
        u_buf.append(output)
        u_d = u_buf.pop(0)
        pv = pv + TS / T * (-pv + K * u_d)
        pv_log.append(pv); out_log.append(output)
    analyze(pv_log, out_log, label)

print('=' * 56)
print('场景C：手动 30% 运行 200s 后切自动（考察无扰切换）')
print('=' * 56)
pv3, out3, bump, _ = simulate('manual_switch')
print(f'  切换瞬间输出跳变量 = {bump:.2f}%  （<1% 即判定无扰）')
analyze(pv3, out3, 'C 手动->自动')