/**
 * @file pwm.c (families/t06211/compact_src)
 * @brief Compact single-file implementation of the t06211 PWM control
 *        pipeline. Merges the 10 per-module files from
 *        families/t06211/src/ into one translation unit.
 *
 * Pipeline (t06211 FUN_77b3 @ 0x77b3):
 *   1. setpoint (FUN_7168)         — single-submap RPM-indexed interp
 *   2. supervisor (FUN_7beb)       — reset + counter + error + clamp
 *   3. publish_cl (FUN_7cd8)       — calls cl_correction, publishes est_angle
 *   4. pi_controller (FUN_67c4)    — open/closed-loop branch
 *   5. pwm_output (FUN_5314)       — eval+eval+combine+saturate+HW shadow
 *
 * Per-block address citations follow the families/t06211/src/ per-module
 * ports verbatim; see those files for expanded commentary.
 */
#include "pwm.h"

/* Forward decls for file-static helpers. */
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);

/* ═════════════════════════════════════════════════════════════════════
 * 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.
     * Mirrors FUN_76aa @ 0x76aa, which copies cal+0x108/0x106 into
     * RAM[0x0332]/[0x0334] (normalizers), cal+0x10A/0x10C into
     * RAM[0x0450]/[0x0452] (P-shape bounds; trim bytes at RAM[0x414]/
     * [0x416] have no writers in the ROM so the bases pass through),
     * cal+0x118/0x11A into RAM[0x0454]/[0x0456] (P-gain + integ step;
     * same trim-byte note for RAM[0x0410]/[0x0412]), and cal+0x11C
     * (byte, clamped to 15) into RAM[0x0330]. */
    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->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;  /* family-1 API parity; t06211 keeps Y-tables in cal */
    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);   /* accepted; unused */
    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);   /* accepted; unused */

    /* The b_fb_kw getter result drives the CAN-decoded setpoint chain
     * (FUN_64c3). Mirrors the real ROM where RAM[0x12c] is written by
     * the CAN parser before FUN_64c3 runs. */
    rt->can_raw_b_fb_kw = rt->inputs.b_fb_kw;
}

/* ═════════════════════════════════════════════════════════════════════
 * Interpolation — FUN_7168 (0x7168-0x71d7), fingerprint #3
 * Raw helper; also drives the setpoint stage via descriptor.
 * ═════════════════════════════════════════════════════════════════════ */

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);
    /* Disasm 0x71c5: ADD RW1C, -0x2[RW20]. After the disasm's pointer
     * advance to the y[] half (0x71b2 ADD RW20, RW1E), RW20 points one
     * past the breakpoint where the search settled, so -0x2[RW20] is
     * y at that breakpoint — which is y[k] in C's k-naming (k = first
     * index where in >= x[k] given descending x). Earlier this returned
     * y[k-1], producing wrong-polarity setpoints (e.g. pi_high_clamp_ceiling
     * = 1962 instead of 1195 at rpm=3355). */
    return (int16_t)(quot + y[k]);
}

/* ═════════════════════════════════════════════════════════════════════
 * Submap eval / bilinear combine / refine — t06211 FUN_6fb8 / FUN_7035 /
 * FUN_7014. Scratch layout matches the family-1 ROM convention.
 * ═════════════════════════════════════════════════════════════════════ */

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]) {
        /* Upper-clamp sentinel [_, 2, 2, 2] */
        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));
}

/* Descriptor-driven wrapper used only by the setpoint stage. */
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 1 — Setpoint (FUN_77b3:77b3-77c7 + FUN_7168)
 * ═════════════════════════════════════════════════════════════════════ */

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 1b — CAN-decoded setpoint (FUN_64c3 @ 0x64c3-0x650a)
 * Real ROM call site: FUN_6192:0x61cc (CAN parser dispatcher).
 * For the C model, invoked from pwm_service after read_inputs so the
 * harness's per-cycle b_fb_kw value flows into target before PI runs.
 * Skip the RE7 gate and error path (sim trusts caller).
 * ═════════════════════════════════════════════════════════════════════ */

static void s_setpoint_can_decode(pwm_runtime_t *rt,
                                   const pwm_calibration_t *cal)
{
    int16_t raw = rt->can_raw_b_fb_kw;
    /* [0x64ca-0x64d6] Bounds check */
    if (raw > cal->b_fb_kw_upper_bound) return;
    if (raw < cal->b_fb_kw_lower_bound) return;

    /* [0x64dd] half = raw >> 1 (SHRA — sign-extending) */
    int16_t half = shra16(raw, 1);
    rt->can_half_12a = half;

    /* [0x64e5-0x64ea] result = half + RAM[0x150] + RW42 */
    int16_t result = (int16_t)(half + rt->setpoint_offset + rt->rw42_state);

    /* [0x64ed-0x64f9] Lower clamp */
    if (result < cal->target_5e_min_clamp) {
        result = cal->target_5e_min_clamp;
    }
    rt->target_5e = result;
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 2 — Supervisor (FUN_7beb @ 0x7beb-0x7c41)
 * ═════════════════════════════════════════════════════════════════════ */

static void s_supervisor(pwm_runtime_t *rt)
{
    /* Reset-flag branch (0x7beb-0x7c0c). Disasm:
     *   7bf6 reset_flag = 0
     *   7bfb cl_enable_counter (DAT_033c) = 0
     *   7c00 supervisor_state (RW17E) = 0
     *   7c05 LD  RW1C, DAT_02b4               ; pi_integ_hi
     *   7c0a ST  RW1C, DAT_033a               ; snapshot pi_integ_hi
     * The DAT_033a store is a dead-store (no readers anywhere in the
     * ROM); kept here for parity. The earlier port assigned
     * rt->inputs.b_fb_kw to this slot — that was inherited from the
     * family-1 idiom and is wrong for this variant. */
    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 {
        /* Counter tick (0x7c11-0x7c1b) */
        rt->cl_enable_counter = (uint16_t)(rt->cl_enable_counter + 1u);
    }

    /* Error compute (0x7c20-0x7c2b)
     * Disasm: SUB RW1C, RW5E, DAT_02f8 — primary setpoint is target_5e. */
    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;

    /* Ceiling clamp (0x7c30-0x7c41)
     * Disasm: CMP RW1C, DAT_0336 — clamp uses RPM-derived ceiling. */
    if (error > rt->pi_high_clamp_ceiling) {
        rt->angle_error_raw = rt->pi_high_clamp_ceiling;
    }
}

/* ═════════════════════════════════════════════════════════════════════
 * Stage 3a — Closed-loop correction (FUN_5f1f @ 0x5f1f-0x5f66)
 * Fingerprint #10 bit-for-bit match to family-1 algorithm.
 * ═════════════════════════════════════════════════════════════════════ */

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

    /* Gate on low byte of cl_enable_counter (0x5f1f-0x5f24). */
    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 3 — Publish CL (FUN_7cd8 @ 0x7cd8-0x7cea)
 * ═════════════════════════════════════════════════════════════════════ */

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 4 — PI controller (FUN_67c4 @ 0x67c4 + FUN_66a8 + FUN_672b)
 * ═════════════════════════════════════════════════════════════════════ */

/* FUN_66a8 — saturation-latch recovery handler. Role-equivalent to the
 * default family's `s_recovery` / `fast_recovery`; same name across
 * variants for cross-family alignment. Disasm/source mapping (audit
 * table — keep the cooldown gate and gated-increment branches in this
 * exact order):
 *   66ad-66b2 CMPB pi_shape_flag, pi_flag_c6 ; JNE LAB_671d
 *   66b9-66be CMP  pi_state_118, [0x02c0]=cal+0x112 ; JNC LAB_66e6
 *               JNC ⇒ counter < threshold ⇒ gated-increment
 *               fall-through ⇒ counter ≥ threshold ⇒ latch+reset
 *   66c0-66e5 latch bit0 ; counter = 0 ; pi_state_c2 ← cal+0x114
 *   66e6-66f4 CMP rpm, [cal+0x11E] ; JLE exit
 *   66f6-6705 JBS bit4 / JBS bit5 — exit
 *   6706-670b CMP ZR, pi_state_c2 ; JNE exit
 *   670d-6717 pi_state_118 += 1
 *   671d-6725 JBS bit0 exit ; pi_state_118 = 0
 */
static void s_recovery(pwm_runtime_t *rt,
                       const pwm_calibration_t *cal)
{
    if (rt->pi_shape_flag != rt->pi_flag_c6) {
        if ((rt->system_flags_110 & 0x01u) == 0u) {
            rt->pi_state_118 = 0;
        }
        return;
    }
    /* Counter ≥ threshold → latch+reset. Threshold is boot-cached at
     * RAM[0x02c0] from cal+0x112 by FUN_6b7b:0x6bd4. */
    if ((uint16_t)rt->pi_state_118 >= (uint16_t)cal->pi_sat_count_threshold) {
        rt->system_flags_110 = (uint8_t)(rt->system_flags_110 | 0x01u);
        rt->pi_state_118 = 0;
        rt->pi_state_c2 = cal->error_thresh_114;
        return;
    }
    /* Gated increment: rpm > rpm_threshold_11E required. */
    if ((int16_t)rt->inputs.rpm <= cal->rpm_threshold_11E) return;
    /* Bits 4 or 5 of system_flags abort. */
    if ((rt->system_flags_110 & 0x30u) != 0u) return;
    /* c2 cooldown gate — counter advances only after c2 has decayed to 0. */
    if (rt->pi_state_c2 != 0) return;
    rt->pi_state_118 = (int16_t)(rt->pi_state_118 + 1);
}

/* FUN_7c85 @ 0x7c85-0x7cbf — PI integrator step. Anti-windup gates
 * the update direction against pi_flag_c7 (0=in-range, 1=clamped low,
 * 2=clamped high). Updates the {pi_integ_hi:pi_integ_lo} 32-bit pair.
 * Disasm:
 *   7c85 LD  RW1C, err
 *   7c8a MUL RL1C, [0x0456]            ; signed 32-bit, RL1C = err*integ_step_normal
 *   7c90 SHLL RL1C, #0x4               ; RL1C <<= 4
 *   7c93 JGE LAB_7ca1                  ; if signed result >= 0
 *   7c95-7c9d  if pi_flag_c7==1 RET   ; (clamped-low → don't push more negative)
 *   7c9f SJMP LAB_7cab                 ; else update
 *   LAB_7ca1: if pi_flag_c7==2 RET    ; (clamped-high → don't push more positive)
 *   LAB_7cab: integrator += RL1C       ; ADD lo, ADDC hi
 */
static void s_pi_integrator_step(pwm_runtime_t *rt)
{
    int32_t prod = MUL_S16(rt->angle_error_pi, rt->integ_step_normal);
    int32_t step = (int32_t)((uint32_t)prod << 4);

    if (step >= 0) {
        if (rt->pi_flag_c7 == 0x02) return;   /* anti-windup: clamped-high */
    } else {
        if (rt->pi_flag_c7 == 0x01) return;   /* anti-windup: clamped-low  */
    }

    /* Combine {hi:lo} as int32 -> add step -> split back. */
    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);
}

/* FUN_672b @ 0x672b-0x67c3 — PI compensation + final clamp.
 * Two arms decided by (pi_flag_338==1 || pi_open_loop_flag==0):
 *   "fresh"   → reset pi_integ_hi from a new (err*open_loop_p_gain>>4)+ckp
 *   "settled" → step the integrator via FUN_7c85
 * Then compute pi_p_term = (p_gain_normal * err) >> 8 and
 * active_request = pi_p_term + pi_integ_hi, clamped to
 * [pi_low_clamp, pi_high_clamp_ceiling].
 */
static void s_pi_compensation(pwm_runtime_t *rt,
                                const pwm_calibration_t *cal)
{
    int16_t error = rt->angle_error_pi;

    /* [0x672b-0x673a] Branch select. */
    bool fresh = (rt->pi_flag_338 == 0x01) || (rt->pi_open_loop_flag == 0u);

    if (fresh) {
        /* [LAB_673c 0x673c-0x6762] Reset pi_integ_hi. Disasm:
         *   STB ZRlo, DAT_0338                         ; pi_flag_338 = 0
         *   LD  RW1C, err
         *   EXT RL1C                                   ; sign-extend to 32-bit
         *   MUL RL1C, [0x0330]                         ; signed: RL1C = err32 * open_loop_p_gain
         *   SHRA RW1C, #0x4                            ; LOW WORD shifted (high word discarded)
         *   ADD  RW1C, RW1C, ckp_in
         *   ST   RW1C, DAT_02b4                        ; pi_integ_hi = result
         */
        rt->pi_flag_338 = 0;
        int32_t prod = MUL_S16(error, rt->open_loop_p_gain);
        int16_t scaled = shra16((int16_t)(prod & 0xFFFF), 4);
        rt->pi_integ_hi = (int16_t)(scaled + rt->inputs.ckp_in);
    } else {
        /* [LAB_6764 0x6764] LCALL FUN_7c85 — integrator step. */
        s_pi_integrator_step(rt);
    }

    /* [LAB_6767 0x6767-0x6779] pi_p_term = (RL1C = p_gain_normal*err) >> 8.
     * Disasm:
     *   LD  RW1C, [0x0454]                             ; p_gain_normal (cal+0x118 default +480)
     *   MUL RL1C, err                                  ; RL1C = p_gain_normal * err (signed)
     *   SHRAL RL1C, #0x8                               ; arithmetic shift right 8 (32-bit)
     *   ST  RW1C, DAT_02b8                             ; pi_p_term = lo16
     */
    int32_t comp32 = MUL_S16(rt->p_gain_normal, error);
    int16_t comp = (int16_t)(shra32(comp32, 8) & 0xFFFF);
    rt->pi_p_term = comp;

    /* [0x677a-0x677f] active_request = pi_p_term + pi_integ_hi */
    int16_t combined = (int16_t)(comp + rt->pi_integ_hi);
    rt->active_request = combined;

    /* [0x6782-0x67a8] Lower clamp: if active_request < pi_low_clamp,
     * pin to pi_low_clamp and set pi_flag_c7=1. */
    if (rt->active_request < cal->pi_low_clamp) {
        rt->active_request = cal->pi_low_clamp;
        rt->pi_flag_c7 = 0x01;
        return;
    }

    /* [LAB_67a9 0x67a9-0x67bd] Upper clamp: if active_request >
     * pi_high_clamp_ceiling, pin to ceiling and set pi_flag_c7=2.
     * Disasm 0x67a9-0x67b0: CMP RW46, DAT_0336 — upper clamp uses
     * the RPM-derived ceiling, not the CAN-decoded primary setpoint. */
    if (rt->active_request > rt->pi_high_clamp_ceiling) {
        rt->active_request = rt->pi_high_clamp_ceiling;
        rt->pi_flag_c7 = 0x02;
        return;
    }

    /* [LAB_67be 0x67be-0x67c3] In-range: pi_flag_c7 = 0. */
    rt->pi_flag_c7 = 0;
}

static void s_pi_update(pwm_runtime_t *rt,
                         const pwm_calibration_t *cal)
{
    /* PI CL-gate threshold = cached ROM[0x605c]. Disasm site:
     * FUN_67c4:67d9 `CMP RW1C, DAT_605c` (literal 0x01A4 = 420). In the
     * real ECU, cl_gate_input is almost always negative (CAN inactive
     * sentinel) which forces OL at line 67d4 before this RPM gate is
     * reached. Lifted to cal->pi_cl_rpm_floor. */
    int16_t rpm_floor = cal->pi_cl_rpm_floor;
    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 < rpm_floor) open_loop = true;

    if (open_loop) {
        rt->pi_open_loop_flag = 0;
        /* Disasm 0x67e5: LD RW46, RW5E — open-loop assigns the CAN-decoded
         * primary setpoint, NOT the RPM-derived ceiling. */
        rt->active_request    = rt->target_5e;
        /* cal+0x120 is a LOWER bound here (disasm: JLT clamps when
         * active_request < limit). Same constant also serves as the
         * Block-4 low clamp, hence `pi_low_clamp`. See
         * families/t06211/src/pi_controller_t06211.c for the full-disasm
         * annotation. */
        if (rt->active_request < cal->pi_low_clamp) {
            rt->active_request = cal->pi_low_clamp;
        }
        goto final_flag;
    }

    /* Disasm 0x680a: SUB RW1C, RW5E, DAT_02cc — CL error uses target. */
    rt->angle_error_pi = (int16_t)(rt->target_5e - rt->estimated_angle);
    int16_t err = rt->angle_error_pi;

    if (err > cal->large_pos_error_thresh) {
        rt->pi_shape_flag = 0;
        s_recovery(rt, cal);
    } else if (err < cal->large_neg_error_thresh) {
        rt->pi_shape_flag = 0x01;
        if (rt->inputs.inj_qty_demand <= cal->pi_thresh_116) {
            rt->pi_state_118 = 0;
        } else {
            s_recovery(rt, cal);
        }
    } else {
        rt->pi_shape_flag = 0x20;
        if (rt->pi_state_c2 > 0) {
            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);
        }
    }

    /* ── Block 4: error-window decision [0x68b7-0x68ef] ──
     * Strict > / < per disasm JLE 68c9 / JGE 68ed. P-shape bounds at
     * DAT_0450 / DAT_0452 are set once at boot by FUN_76aa — see
     * docs/open-questions.md §2 closeout. */
    if (err > rt->p_shape_bound_pos) {
        rt->active_request = cal->pi_high_clamp;
        rt->pi_flag_338    = 0x01;
    } else if (err < rt->p_shape_bound_neg) {
        rt->active_request = cal->pi_low_clamp;
        rt->pi_flag_338    = 0x01;
    } else {
        s_pi_compensation(rt, cal);
    }

final_flag:
    rt->pi_open_loop_flag = 0x01;
    rt->pi_flag_c6        = rt->pi_shape_flag;
}

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

static void s_pwm_output(pwm_runtime_t *rt,
                          const pwm_calibration_t *cal)
{
    /* A. eval pwm_A */
    s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_A],
                  &rt->pwm_slot_a);

    /* B. eval pwm_B */
    s_eval_submap(&pwm_submap_descrs[PWM_SUBMAP_PWM_B],
                  &rt->pwm_slot_b);

    /* C. combine bilinear */
    int16_t duty = s_combine(cal->pwm_y_table, &rt->pwm_slot_a, &rt->pwm_slot_b);
    rt->pwm_duty = (uint16_t)duty;

    /* D. Duty-range classification [0x5361-0x538b] — based on pre-shape 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;

    /* E1. RPM-window three-phase matcher [0x53be-0x552a].
     *
     * Phase 1 (strict-band): rpm strictly inside any of 4 bands at
     *   cal+0xF2 → slew_increment = +pwm_slew_step.
     * Phase 2 (hysteresis margin): rpm in any band's halfwidth-extended
     *   region → HOLD (preserve previous slew_increment).
     * Phase 3 (deep-out): rpm clear of every extended band AND
     *   pwm_period < pwm_period_max → slew_increment = -pwm_slew_step.
     *
     * Slew applied as `pwm_period -= slew_increment` (subtraction;
     * positive increment shrinks period → faster PWM).
     * pwm_slew_increment lives in rt and persists across cycles so the
     * HOLD path keeps the previous direction.
     *
     * Disasm note: high-edge tests use JC (db) at 0x5463/0x5497 for
     * bands 0/1 and JNC (d3) at 0x54cb/0x54fd for bands 2/3 — these
     * encodings produce equivalent control flow (HOLD on rpm < hi+hw,
     * advance on rpm >= hi+hw). See open-questions.md §5 closeout. */
    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;

    /* Phase 1 — strict-band test [0x53be-0x542d] */
    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) {
        /* Phase 2 — fine hysteresis margin [0x5434-0x54fe] */
        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;  /* below extended low: try next band */
            }
            if (rpm_s < (int16_t)(hi + halfwidth)) {
                phase3 = 0;  /* HOLD */
                break;
            }
            /* rpm_s >= hi + halfwidth: advance to next band */
        }
        if (phase3 && *pwm_period < cal->pwm_period_max) {
            /* Phase 3 — deep-out negative slew [0x54ff-0x551b] */
            rt->pwm_slew_increment = (int16_t)(-step_mag);
        }
        /* else: HOLD (no write) */
    }

    /* Slew application [0x5520-0x552a] */
    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;

    /* Final clamp [0x552f-0x5575] */
    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;

    /* E2. Shape detail [0x557a-0x5608] */
    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;

    /* E3. Shape composition additive [0x55cb-0x560f]. ROM formula:
     *   slope     = (period_max - period_min) >> 8
     *   numerator = ((period_max - pwm_period) >> 8) * shape_height
     *   pwm_duty += numerator / slope
     * Shifts happen before multiply (MCS-96 16-bit intermediate). */
    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;

    /* F. Duty bounds clamp [0x5614-0x5636] — ROM reads RAM[0x6058]/[0x605a]
     * (producers open-questions §3). Lifted to cal->pwm_min/cal->pwm_max
     * (defaults 205/3890, same as the default family). */
    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;

    /* G. HW shadow writes [0x563b-0x565f] using the slewed pwm_period. */
    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: query the ROM Y-table directly with (rpm, fbkw)
 * as the two axes, applying the same eval+combine+clamp the PWM stage
 * does. Useful for plotting the static (rpm, fbkw) → duty surface
 * without the PI controller in the loop.
 * ═════════════════════════════════════════════════════════════════════ */

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 FUN_77b3 (0x77b3-0x77d8) — 5-call linear dispatcher.
 * ═════════════════════════════════════════════════════════════════════ */

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

    const pwm_calibration_t *cal = rt->bound_cal;

    s_setpoint(rt);                /* [0x77b3-0x77c7] pi_high_clamp_ceiling = setpoint_interp(rpm) */
    s_setpoint_can_decode(rt, cal); /* CAN-decoded target (FUN_64c3 in ROM) */
    s_supervisor(rt);              /* [0x77cc]        */
    s_publish_cl(rt, cal);/* [0x77cf]        */
    s_pi_update(rt, cal); /* [0x77d2]        */
    s_pwm_output(rt, cal);/* [0x77d5]        */
}
