/**
 * @file pwm.c (families/T06215/compact_src)
 * @brief Compact single-file implementation of the T06215 PWM control
 *        pipeline.
 *
 * Pipeline (pwm_service @ 0x7780):
 *   0. setpoint interp (FUN_7051)  — writes pi_high_clamp_ceiling (DAT_0344)
 *   1. supervisor (s_supervisor)   — reset + counter + error + clamp
 *   2. publish_cl  (s_publish_cl)  — calls cl_correction, publishes est_angle
 *   3. pi_update   (s_pi_update @ 0x542f) — open/closed-loop PI body
 *   4. pwm_output  (s_pwm_output) — eval+eval+combine+saturate+HW shadow
 *
 * The PI block (s_pi_update + s_recovery) was re-translated 1:1 from the
 * t06215 disasm at 0x542f / 0x53a2 on 2026-04-29. Earlier versions of this
 * file inherited a t06211-shaped body (separate s_pi_compensation /
 * s_pi_integrator_step / pi_flag_338 / pi_flag_c7) whose cited addresses
 * (0x67c4, 0x66ad, 0x672b, 0x7c85) do not exist in this binary.
 */
#include "pwm.h"

/* Forward decls. */
static void s_eval_submap(const pwm_submap_descr_t *d,
                           pwm_interp_slot_t *slot);
static int16_t s_combine(const int16_t *y_base,
                          const pwm_interp_slot_t *sa,
                          const pwm_interp_slot_t *sb);
static int16_t s_refine(const int16_t *y_base,
                         const pwm_interp_slot_t *slot);

static void s_setpoint   (pwm_runtime_t *rt);
static void s_supervisor (pwm_runtime_t *rt);
static void s_cl_correct (pwm_runtime_t *rt,
                           const pwm_calibration_t *cal);
static void s_publish_cl (pwm_runtime_t *rt,
                           const pwm_calibration_t *cal);
static void s_pi_update  (pwm_runtime_t *rt,
                           const pwm_calibration_t *cal);
static void s_pwm_output (pwm_runtime_t *rt,
                           const pwm_calibration_t *cal);
static void s_recovery   (pwm_runtime_t *rt,
                           const pwm_calibration_t *cal);

/* ═════════════════════════════════════════════════════════════════════
 * Init / binding
 * ═════════════════════════════════════════════════════════════════════ */

static void apply_cal(pwm_runtime_t *rt,
                      const pwm_calibration_t *cal)
{
    /* All boot-derived constants come from cal — no literals here. */
    rt->setpoint_offset       = cal->setpoint_offset;

    rt->p_shape_bound_pos     = cal->init_p_shape_bound_pos;
    rt->p_shape_bound_neg     = cal->init_p_shape_bound_neg;
    rt->p_gain_normal         = cal->init_p_gain_normal;
    rt->integ_step_normal     = cal->init_integ_step_normal;
    rt->p_slope_large_pos     = cal->init_p_slope_large_pos;
    rt->p_slope_large_neg     = cal->init_p_slope_large_neg;
    rt->integ_step_large_pos  = cal->init_integ_step_large_pos;
    rt->integ_step_large_neg  = cal->init_integ_step_large_neg;
    rt->open_loop_p_gain      = cal->init_open_loop_p_gain;

    rt->pos_error_normalizer  = cal->init_pos_error_normalizer;
    rt->neg_error_normalizer  = cal->init_neg_error_normalizer;
}

static void runtime_reset(pwm_runtime_t *rt)
{
    memset(rt, 0, sizeof(*rt));
}

void pwm_init(pwm_runtime_t *rt,
              const pwm_calibration_t   *cal,
              const pwm_flash_t         *flash,
              const pwm_input_getters_t *getters)
{
    (void)flash;
    runtime_reset(rt);
    rt->bound_cal     = cal;
    rt->bound_getters = getters;
    apply_cal(rt, cal);
    pwm_bind_submap_inputs(rt, pwm_submap_descrs, PWM_SUBMAP_COUNT);
}

static void read_inputs(pwm_runtime_t *rt)
{
    const pwm_input_getters_t *g = rt->bound_getters;
    void *ctx = g->ctx;
    rt->inputs.ckp_in         = g->ckp_in        (ctx);
    rt->inputs.rpm            = g->rpm           (ctx);
    rt->inputs.angle_dec_cmd  = g->angle_dec_cmd (ctx);
    rt->inputs.inj_qty_demand = g->inj_qty_demand(ctx);
    rt->inputs.b_fb_kw        = g->b_fb_kw       (ctx);
    rt->inputs.cl_gate_input  = g->cl_gate_input (ctx);
    rt->inputs.supply_voltage = g->supply_voltage(ctx);
    rt->inputs.temperature    = g->temperature   (ctx);

    rt->can_raw_b_fb_kw = rt->inputs.b_fb_kw;
}

/* ═════════════════════════════════════════════════════════════════════
 * Interpolation (FUN_7168, fingerprint #3)
 * ═════════════════════════════════════════════════════════════════════ */

int16_t pwm_interp_lookup(const int16_t *x, const int16_t *y,
                                  uint16_t n, int16_t in)
{
    if (in >= x[0])         return y[0];
    if (in <= x[n - 1u])    return y[n - 1u];

    uint16_t k = 1u;
    while (k < n && in < x[k]) { k++; }

    int16_t x_k = x[k];
    int16_t num = (int16_t)(in - x_k);
    int16_t dx  = (int16_t)(x_k - x[k - 1u]);
    int16_t dy  = (int16_t)(y[k] - y[k - 1u]);

    int32_t prod = MUL_S16(num, dy);
    int16_t quot = (int16_t)(prod / (int32_t)dx);
    return (int16_t)(quot + y[k]);
}

/* ═════════════════════════════════════════════════════════════════════
 * Submap eval / bilinear combine / refine
 * ═════════════════════════════════════════════════════════════════════ */

static void s_eval_submap(const pwm_submap_descr_t *d,
                           pwm_interp_slot_t *slot)
{
    int16_t input = d->input_ptr ? *d->input_ptr : 0;
    slot->row_stride = (int16_t)(d->count * 2u);

    if (d->count == 0 || d->x == NULL) {
        slot->x_interval = 2;
        slot->x_offset   = 2;
        slot->y_byte_off = 2;
        return;
    }

    uint16_t k;
    for (k = 1u; k < d->count; k++) {
        if (input >= d->x[k]) break;
    }

    if (k == 1u && input >= d->x[0]) {
        slot->x_interval = 2;
        slot->x_offset   = 2;
        slot->y_byte_off = 2;
        return;
    }
    if (k >= d->count) { k = (uint16_t)(d->count - 1u); }

    slot->x_interval = (int16_t)(d->x[k - 1u] - d->x[k]);
    slot->x_offset   = (int16_t)(input - d->x[k]);
    slot->y_byte_off = (int16_t)(k * 2u);
}

static int16_t s_combine(const int16_t *y_base,
                          const pwm_interp_slot_t *sa,
                          const pwm_interp_slot_t *sb)
{
    if (!y_base || !sa || !sb) return 0;

    int16_t den_a = sa->x_interval ? sa->x_interval : 1;
    int16_t den_b = sb->x_interval ? sb->x_interval : 1;

    int32_t row_off = MUL_S16(sb->y_byte_off, sa->row_stride) / 2;
    const uint8_t *yp_b = (const uint8_t *)y_base + row_off + sa->y_byte_off;
    const int16_t *yp   = (const int16_t *)yp_b;

    int16_t y_here   = *yp;
    int16_t y_prev_a = *(const int16_t *)(yp_b - 2);
    int32_t diff_a   = MUL_S16((int16_t)(y_prev_a - y_here), sa->x_offset);
    int16_t rowB     = (int16_t)(y_here + (int32_t)(diff_a / (int32_t)den_a));

    const uint8_t *yp_b_prev = yp_b - sa->row_stride;
    const int16_t *yp_prev   = (const int16_t *)yp_b_prev;
    int16_t y_here_pb   = *yp_prev;
    int16_t y_prev_a_pb = *(const int16_t *)(yp_b_prev - 2);
    int32_t diff_a_pb   = MUL_S16((int16_t)(y_prev_a_pb - y_here_pb), sa->x_offset);
    int16_t rowBp       = (int16_t)(y_here_pb + (int32_t)(diff_a_pb / (int32_t)den_a));

    int32_t diff_b = MUL_S16((int16_t)(rowBp - rowB), sb->x_offset);
    return (int16_t)(rowB + (int32_t)(diff_b / (int32_t)den_b));
}

static int16_t s_refine(const int16_t *y_base,
                         const pwm_interp_slot_t *slot)
{
    if (!y_base || !slot) return 0;
    int16_t den = slot->x_interval ? slot->x_interval : 1;

    const uint8_t *yp_b = (const uint8_t *)y_base + slot->y_byte_off;
    const int16_t *yp   = (const int16_t *)yp_b;
    int16_t y_here   = *yp;
    int16_t y_prev_a = *(const int16_t *)(yp_b - 2);
    int32_t diff = MUL_S16((int16_t)(y_prev_a - y_here), slot->x_offset);
    return (int16_t)(y_here + (int32_t)(diff / (int32_t)den));
}

static int16_t s_interp_descr(const pwm_submap_descr_t *d,
                               const int16_t *y_array)
{
    int16_t input = d->input_ptr ? *d->input_ptr : 0;
    return pwm_interp_lookup(d->x, y_array, d->count, input);
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 0 — Setpoint interp (pwm_service:0x7780 → FUN_7051(cal+0x142))
 * Writes pi_high_clamp_ceiling (DAT_0344 in ROM).
 * ═════════════════════════════════════════════════════════════════════ */

static void s_setpoint(pwm_runtime_t *rt)
{
    const pwm_submap_descr_t *d =
        &pwm_submap_descrs[PWM_SUBMAP_SETPOINT_INTERP];
    const int16_t *y = pwm_submap_y_of(PWM_SUBMAP_SETPOINT_INTERP);
    rt->pi_high_clamp_ceiling = s_interp_descr(d, y);
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 0b — CAN-decoded setpoint (FUN_64c3 family)
 * ═════════════════════════════════════════════════════════════════════ */

static void s_setpoint_can_decode(pwm_runtime_t *rt,
                                   const pwm_calibration_t *cal)
{
    int16_t raw = rt->can_raw_b_fb_kw;
    if (raw > cal->b_fb_kw_upper_bound) return;
    if (raw < cal->b_fb_kw_lower_bound) return;

    int16_t half = shra16(raw, 1);
    rt->can_half_12a = half;

    int16_t result = (int16_t)(half + rt->setpoint_offset + rt->rw42_state);
    if (result < cal->target_5e_min_clamp) {
        result = cal->target_5e_min_clamp;
    }
    rt->target_5e = result;
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 1 — Supervisor (s_supervisor @ 0x7d26)
 *
 * 1:1 re-translation 2026-04-29. Earlier port body was inherited from
 * t06211 and used `compensation_angle` (no t06215 producer) and a
 * `b_fb_kw_baseline` reset stash (no t06215 binding). The actual t06215
 * supervisor adds the **PI P-term** (DAT_0276) and stashes the
 * integrator high word into DAT_02ec on reset (a dead store, but kept
 * here for parity).
 *
 * Disasm:
 *   7d26 LDBZE pi_shape_flag-adjacent (R2E = reset_flag)
 *   7d2b CMP   #1 ; JNE LAB_7d4c
 *   ; --- reset path ---
 *   7d31 reset_flag = 0
 *   7d36 cl_enable_counter (DAT_02ee) = 0
 *   7d3b supervisor_state (RW17E) = 0
 *   7d40-7d45 DAT_02ec = pi_integ_hi (DAT_028a snapshot — dead store)
 *   7d4a SJMP LAB_7d5b
 *   ; --- non-reset path ---
 *   LAB_7d4c LD RW1C, cl_enable_counter; RW1E = RW1C+1; ST cl_enable_counter
 *   ; --- common publish ---
 *   LAB_7d5b SUB RW1C, RW5E, ckp_in
 *          ADD RW1C, pi_p_term (DAT_0276)
 *          ST  angle_error_raw (DAT_02f0), RW1C
 *          CMP RW1C, pi_high_clamp_ceiling (DAT_0344)
 *          JLE exit
 *          angle_error_raw = pi_high_clamp_ceiling
 * ═════════════════════════════════════════════════════════════════════ */

static void s_supervisor(pwm_runtime_t *rt)
{
    if (rt->reset_flag == 1u) {
        rt->reset_flag             = 0u;
        rt->cl_enable_counter      = 0u;
        rt->supervisor_state       = 0;
        rt->pi_integ_hi_snapshot   = rt->pi_integ_hi;  /* dead-store parity */
    } else {
        rt->cl_enable_counter = (uint16_t)(rt->cl_enable_counter + 1u);
    }

    int16_t error = (int16_t)(rt->target_5e - rt->inputs.ckp_in);
    error = (int16_t)(error + rt->pi_p_term);
    rt->angle_error_raw = error;

    if (error > rt->pi_high_clamp_ceiling) {
        rt->angle_error_raw = rt->pi_high_clamp_ceiling;
    }
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 2a — Closed-loop correction
 * ═════════════════════════════════════════════════════════════════════ */

static void s_cl_correct(pwm_runtime_t *rt,
                          const pwm_calibration_t *cal)
{
    int16_t correction;

    if ((rt->cl_enable_counter & 0xFFu) == 0u) {
        correction = 0;
    } else {
        int16_t normalizer = (rt->angle_error_raw > 0)
            ? rt->pos_error_normalizer
            : rt->neg_error_normalizer;

        int32_t product = MUL_S16(rt->angle_error_raw, cal->closed_loop_gain_const);
        product = (int32_t)((uint32_t)product << 4);
        correction = (normalizer != 0)
            ? (int16_t)(product / (int32_t)normalizer)
            : (int16_t)0;
    }

    rt->cl_correction_raw = correction;
    rt->supervisor_state  = (int16_t)(rt->supervisor_state + correction);
    rt->angle_offset      = shra16(rt->supervisor_state, 4u);
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 2 — Publish CL
 * ═════════════════════════════════════════════════════════════════════ */

static void s_publish_cl(pwm_runtime_t *rt,
                          const pwm_calibration_t *cal)
{
    s_cl_correct(rt, cal);
    rt->estimated_angle = (int16_t)(rt->inputs.ckp_in + rt->angle_offset);
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 3 — PI controller
 *
 * 1:1 re-translation from t06215 disasm s_pi_update @ 0x542f and
 * s_recovery @ 0x53a2 (live Ghidra session, 2026-04-29). Cross-reference:
 * the trailing byte rotate at LAB_56e8 (56e8–5708) saves bits 4,5 of the
 * current cycle into bits 6,7 for next cycle's s_recovery sustained-band
 * detector.
 * ═════════════════════════════════════════════════════════════════════ */

/* s_recovery @ 0x53a2.
 *
 * Disasm:
 *   53a2 LDBZE RW1C, pi_shape_flag      ; zero-extended into 16-bit
 *   53a7 SHL   RW1C, #2                 ; <<2 (in 16-bit reg)
 *   53aa LDBZE RW1E, pi_shape_flag
 *   53af AND   RW1C, RW1E               ; (flag<<2) & flag
 *   53b2 CMP   RW1C, #0x30
 *   53b6 JNH   LAB_5421                 ; <= 0x30 → "no sustained" path
 *   ; --- Sustained large-error detected (this+previous cycle bit4 or bit5) ---
 *   53b8 LD    RW1C, pi_state_118
 *   53bd CMP   RW1C, RAM[0x027a]        ; RAM[0x027a] = cal+0x112 boot-cached
 *   53c2 JNC   LAB_53ea                 ; counter < threshold → gated-increment
 *   ; --- Latch path: counter saturated ---
 *   53c4 system_flags_110 |= 1
 *   53d1 pi_state_118 = 0
 *   53e1 pi_state_c2 = cal[0x114]       ; reload cooldown
 *   53e9 RET
 *   LAB_53ea: ; counter still < threshold
 *   53f5 CMP   rpm, cal[0x126]
 *   53f8 JLE   LAB_542e (RET)           ; rpm <= threshold → no increment
 *   53fa-5407 if (system_flags_110 & 0x30) RET
 *   540a   if pi_state_c2 != 0   RET
 *   5411 pi_state_118 += 1
 *   5420 RET
 *   LAB_5421: ; no sustained large-error
 *   5421 if (system_flags_110 & 1) RET   ; latched → keep counter
 *   5429 pi_state_118 = 0
 *   542e RET
 */
static void s_recovery(pwm_runtime_t *rt,
                       const pwm_calibration_t *cal)
{
    uint16_t flag_u16 = (uint16_t)rt->pi_shape_flag;
    uint16_t fold = (uint16_t)((flag_u16 << 2) & flag_u16);

    if (fold > 0x30u) {
        /* Sustained large-error detected. */
        if ((uint16_t)rt->pi_state_118 < (uint16_t)cal->pi_sat_count_threshold) {
            /* Counter still below threshold — gated increment. */
            if ((int16_t)rt->inputs.rpm <= cal->rpm_threshold_recovery) return;
            if ((rt->system_flags_110 & 0x10u) != 0u) return;
            if ((rt->system_flags_110 & 0x20u) != 0u) return;
            if (rt->pi_state_c2 != 0) return;
            rt->pi_state_118 = (int16_t)(rt->pi_state_118 + 1);
        } else {
            /* Saturated — latch + reset. */
            rt->system_flags_110 = (uint8_t)(rt->system_flags_110 | 0x01u);
            rt->pi_state_118 = 0;
            rt->pi_state_c2  = cal->pi_state_c2_reload;
        }
    } else {
        /* No sustained large-error this cycle. */
        if ((rt->system_flags_110 & 0x01u) == 0u) {
            rt->pi_state_118 = 0;
        }
    }
}

/* s_pi_update @ 0x542f. See doc-comment at top of "Stage 3". */
static void s_pi_update(pwm_runtime_t *rt,
                         const pwm_calibration_t *cal)
{
    /* ── Phase A — Open-loop gate [0x542f-0x5489] ──
     * Disasm: bit-test R110.5; CMP RW130 < 0; CMP rpm < flash[0x605c]. */
    bool open_loop = false;
    if ((rt->system_flags_110 & 0x20u) != 0u)        open_loop = true;
    else if (rt->inputs.cl_gate_input < 0)           open_loop = true;
    else if ((int16_t)rt->inputs.rpm < cal->pi_cl_rpm_floor) open_loop = true;

    if (open_loop) {
        /* 0x544b-0x5453: pi_shape_flag |= 0x01 */
        rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x01u);
        /* 0x5458: DAT_0274 = RW5E */
        rt->pi_preclamp_out = rt->target_5e;
        /* 0x5468-0x5482: if (RW5E < cal[0x128]) DAT_0274 = cal[0x128] */
        if (rt->target_5e < cal->pi_low_clamp) {
            rt->pi_preclamp_out = cal->pi_low_clamp;
        }
        goto trailing_rotate;
    }

    /* ── Phase B — Error-band classify [0x5489-0x554e] ──
     * Disasm 0x5489: SUB RW1C, RW5E, DAT_02cc → DAT_0278. */
    int16_t err = (int16_t)(rt->target_5e - rt->estimated_angle);
    rt->angle_error_pi = err;

    if (err > cal->large_pos_error_thresh) {
        /* 0x54a4-0x54af: flag = (flag | 0x10) & 0xDF */
        rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag | 0x10u) & 0xDFu);
        s_recovery(rt, cal);
    } else if (err < cal->large_neg_error_thresh) {
        /* 0x54cd-0x54d8: flag = (flag | 0x20) & 0xEF */
        rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag | 0x20u) & 0xEFu);
        /* 0x54dd-0x54f9: JH check is unsigned; matches CMP RW44, cal[0x116]. */
        if ((uint16_t)rt->inputs.inj_qty_demand > (uint16_t)cal->inj_qty_thresh) {
            s_recovery(rt, cal);
        } else {
            rt->pi_state_118 = 0;
        }
    } else {
        /* 0x54fb-0x554e: flag &= 0xCF; decrement c2 (clear latch when 0); decrement 118 */
        rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xCFu);
        if ((uint16_t)rt->pi_state_c2 != 0u) {
            rt->pi_state_c2 = (int16_t)(rt->pi_state_c2 - 1);
        }
        if (rt->pi_state_c2 == 0) {
            rt->system_flags_110 = (uint8_t)(rt->system_flags_110 & 0xFEu);
        }
        if (rt->pi_state_118 > 0) {
            rt->pi_state_118 = (int16_t)(rt->pi_state_118 - 1);
        }
    }

    /* ── Phase C — P-shape into pi_p_term / pi_p_gain_active [0x554e-0x560c] ── */
    int32_t pterm32;  /* 32-bit accumulator for the SHRAL (>>8) */
    if (err > rt->p_shape_bound_pos) {
        /* 0x555a-0x558f: large-positive arm. */
        rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x02u);
        rt->pi_p_gain_active = rt->integ_step_large_pos;       /* DAT_0286 = DAT_0282 */
        int32_t slope_part  = MUL_S16((int16_t)(err - rt->p_shape_bound_pos),
                                       rt->p_slope_large_pos);
        int32_t anchor_part = MUL_S16(rt->p_gain_normal, rt->p_shape_bound_pos);
        pterm32 = slope_part + anchor_part;
        rt->pi_p_term = (int16_t)(shra32(pterm32, 8) & 0xFFFF);
    } else if (err < rt->p_shape_bound_neg) {
        /* 0x559d-0x55cc: large-negative arm. */
        rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag | 0x02u);
        rt->pi_p_gain_active = rt->integ_step_large_neg;       /* DAT_0286 = DAT_0284 */
        int32_t slope_part  = MUL_S16((int16_t)(err - rt->p_shape_bound_neg),
                                       rt->p_slope_large_neg);
        int32_t anchor_part = MUL_S16(rt->p_gain_normal, rt->p_shape_bound_neg);
        pterm32 = slope_part + anchor_part;
        rt->pi_p_term = (int16_t)(shra32(pterm32, 8) & 0xFFFF);
    } else {
        /* 0x55e2-0x5607: normal-range arm. */
        rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xFDu);
        rt->pi_p_gain_active = rt->integ_step_normal;          /* DAT_0286 = DAT_0456 */
        int32_t prod = MUL_S16(rt->p_gain_normal, err);
        rt->pi_p_term = (int16_t)(shra32(prod, 8) & 0xFFFF);
    }

    /* ── Phase D — Integrator gate [0x560c-0x5679] ──
     * Disasm 0x560c-0x5611: if pi_shape_flag.bit0 set (open-loop, but we
     *   returned earlier in that case) → take fast-path that overrides
     *   pi_integ_hi from open_loop_p_gain. We never enter here in the
     *   closed-loop path, but preserve the structure.
     * 0x563b onward: closed-loop integrator step with anti-windup. */
    if ((rt->pi_shape_flag & 0x01u) != 0u) {
        /* Open-loop fast-path (defensive — the open-loop early-return
         * above means this branch is unreachable from the closed-loop
         * path; preserved here in case a future cycle enters with bit 0
         * latched from elsewhere). */
        int32_t prod = MUL_S16(rt->open_loop_p_gain, err);
        int16_t shifted = shra16((int16_t)(prod & 0xFFFF), 4);
        rt->pi_integ_hi = (int16_t)(shifted + rt->inputs.ckp_in);
        rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xFEu);
    } else {
        /* 0x563b-0x5654: anti-windup gate via previous-cycle clamp bits. */
        bool gate_skip;
        if (err < 0) {
            gate_skip = (rt->pi_shape_flag & 0x04u) != 0u;  /* bit 2: clamped low */
        } else {
            gate_skip = (rt->pi_shape_flag & 0x08u) != 0u;  /* bit 3: clamped high */
        }
        if (!gate_skip) {
            /* 0x5657-0x5674: 32-bit signed integrator step.
             *   step = (err * pi_p_gain_active) << 4
             *   {hi:lo} += step
             * MCS-96 SHLL is logical, so cast through unsigned. */
            int32_t prod = MUL_S16(err, rt->pi_p_gain_active);
            int32_t step = (int32_t)((uint32_t)prod << 4);
            int32_t accum = ((int32_t)rt->pi_integ_hi << 16)
                          | (uint16_t)rt->pi_integ_lo;
            accum += step;
            rt->pi_integ_lo = (int16_t)(accum & 0xFFFF);
            rt->pi_integ_hi = (int16_t)((accum >> 16) & 0xFFFF);
        }
    }

    /* ── Phase E — Combine + clamp [0x5679-0x56e3] ── */
    rt->pi_preclamp_out = (int16_t)(rt->pi_p_term + rt->pi_integ_hi);

    if (rt->pi_preclamp_out < cal->pi_low_clamp) {
        /* 0x5696-0x56b6: clamp low; flag = (flag & 0xF7) | 4 */
        rt->pi_preclamp_out = cal->pi_low_clamp;
        rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag & 0xF7u) | 0x04u);
    } else if (rt->pi_preclamp_out > rt->pi_high_clamp_ceiling) {
        /* 0x56c2-0x56d9: clamp high; flag = (flag & 0xFB) | 8 */
        rt->pi_preclamp_out = rt->pi_high_clamp_ceiling;
        rt->pi_shape_flag = (uint8_t)((rt->pi_shape_flag & 0xFBu) | 0x08u);
    } else {
        /* 0x56db-0x56e0: in-range — clear bits 2,3 */
        rt->pi_shape_flag = (uint8_t)(rt->pi_shape_flag & 0xF3u);
    }

trailing_rotate:
    /* ── Trailing rotate [LAB_56e8 0x56e8-0x5708] ──
     * Disasm 0x56e8: ST  DAT_0274, RW46          ; publish active_request
     *        0x56ed-0x5703: pi_shape_flag = (flag & 0x3F) | ((flag<<2) & 0xC0)
     *   Saves this cycle's bits 4,5 into bits 6,7 for the next cycle's
     *   s_recovery sustained-band detector.
     */
    rt->active_request = rt->pi_preclamp_out;
    {
        uint8_t f = rt->pi_shape_flag;
        rt->pi_shape_flag = (uint8_t)((f & 0x3Fu) | (uint8_t)((f << 2) & 0xC0u));
    }
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 4 — PWM output (FUN_5314 @ 0x5314-0x565f)
 * ═════════════════════════════════════════════════════════════════════ */

static void s_pwm_output(pwm_runtime_t *rt,
                          const pwm_calibration_t *cal)
{
    s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A],
                  &rt->pwm_slot_a);
    s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B],
                  &rt->pwm_slot_b);
    int16_t duty = s_combine(cal->pwm_y_table, &rt->pwm_slot_a, &rt->pwm_slot_b);
    rt->pwm_duty = (uint16_t)duty;

    if (rt->pwm_duty < 0x29u)        rt->pwm_duty_range_flag = 1u;
    else if (rt->pwm_duty > 0xFD7u)  rt->pwm_duty_range_flag = 2u;
    else                              rt->pwm_duty_range_flag = 0u;

    uint16_t *pwm_period = (uint16_t *)&rt->pwm_shape_state[0];
    const int16_t rpm_s    = (int16_t)rt->inputs.rpm;
    const int16_t halfwidth = cal->pwm_window_halfwidth;
    const int16_t step_mag = cal->pwm_slew_step;

    int slew_set = 0;
    for (int bi = 0; bi < 4; bi++) {
        int16_t lo = cal->pwm_rpm_windows[bi * 2];
        int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1];
        if (rpm_s > lo && rpm_s < hi) {
            rt->pwm_slew_increment = step_mag;
            slew_set = 1;
            break;
        }
    }

    if (!slew_set) {
        int phase3 = 1;
        for (int bi = 0; bi < 4; bi++) {
            int16_t lo = cal->pwm_rpm_windows[bi * 2];
            int16_t hi = cal->pwm_rpm_windows[bi * 2 + 1];
            if (rpm_s <= (int16_t)(lo - halfwidth)) {
                continue;
            }
            if (rpm_s < (int16_t)(hi + halfwidth)) {
                phase3 = 0;
                break;
            }
        }
        if (phase3 && *pwm_period < cal->pwm_period_max) {
            rt->pwm_slew_increment = (int16_t)(-step_mag);
        }
    }

    int32_t period_next = (int32_t)*pwm_period - (int32_t)rt->pwm_slew_increment;
    if (period_next < 0)     period_next = 0;
    if (period_next > 0xFFFF) period_next = 0xFFFF;
    *pwm_period = (uint16_t)period_next;

    if (*pwm_period < cal->pwm_period_min) *pwm_period = cal->pwm_period_min;
    if (*pwm_period > cal->pwm_period_max) *pwm_period = cal->pwm_period_max;

    pwm_interp_slot_t shape_slot;
    s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_SHAPE_EVAL],
                  &shape_slot);
    int16_t shape_height = s_refine(cal->shape_y_table, &shape_slot);
    if (shape_height < 0)     shape_height = 0;
    if (shape_height > 0x199) shape_height = 0x199;
    rt->pwm_shape_state[5] = shape_height;

    int16_t slope     = (int16_t)(((int32_t)cal->pwm_period_max
                                   - (int32_t)cal->pwm_period_min) >> 8);
    int16_t pmx_delta = (int16_t)(((int32_t)cal->pwm_period_max
                                   - (int32_t)*pwm_period) >> 8);
    int32_t shape_add = slope ? (MUL_S16(pmx_delta, shape_height)
                                 / (int32_t)slope)
                               : 0;
    int32_t duty_new = (int32_t)rt->pwm_duty + shape_add;

    if (duty_new < (int32_t)cal->pwm_min) duty_new = (int32_t)cal->pwm_min;
    if (duty_new > (int32_t)cal->pwm_max) duty_new = (int32_t)cal->pwm_max;
    rt->pwm_duty = (uint16_t)duty_new;

    uint32_t on_product = (uint32_t)(*pwm_period) * (uint32_t)rt->pwm_duty;
    rt->pwm_on_time     = (uint16_t)(on_product / 0xFFFu);
    rt->pwm_off_time    = (uint16_t)(*pwm_period - rt->pwm_on_time);
    rt->pwm_period      = *pwm_period;
}

/* ═════════════════════════════════════════════════════════════════════
 * Bypass-PI LUT helper
 * ═════════════════════════════════════════════════════════════════════ */

uint16_t pwm_lut_duty(const pwm_calibration_t *cal,
                              uint16_t rpm, int16_t fbkw)
{
    pwm_submap_descr_t a = pwm_submap_descrs[PWM_SUBMAP_PWM_A];
    pwm_submap_descr_t b = pwm_submap_descrs[PWM_SUBMAP_PWM_B];
    int16_t rpm_signed = (int16_t)rpm;
    a.input_ptr = &rpm_signed;
    b.input_ptr = &fbkw;
    pwm_interp_slot_t sa, sb;
    s_eval_submap(&a, &sa);
    s_eval_submap(&b, &sb);
    int16_t duty = s_combine(cal->pwm_y_table, &sa, &sb);
    if (duty < 205)  duty = 205;
    if (duty > 3890) duty = 3890;
    return (uint16_t)duty;
}

/* ═════════════════════════════════════════════════════════════════════
 * Public entry — pwm_service (mirrors pwm_service @ 0x7780)
 * ═════════════════════════════════════════════════════════════════════ */

void pwm_service(pwm_runtime_t *rt)
{
    read_inputs(rt);

    const pwm_calibration_t *cal = rt->bound_cal;

    s_setpoint(rt);                /* writes pi_high_clamp_ceiling */
    s_setpoint_can_decode(rt, cal);
    s_supervisor(rt);
    s_publish_cl(rt, cal);
    s_pi_update(rt, cal);
    s_pwm_output(rt, cal);
}
