Dual Simultaneous Engagement (dim=40)

def _clamp(x: float, lo: float, hi: float) -> float:
    if x < lo:
        return lo
    if x > hi:
        return hi
    return x

def _sigmoid_window(t: float, t_center: float, tau: float) -> float:
    """Unit pulse centred at *t_center*, width ~4*tau, Lipschitz-continuous."""
    arg = (t - t_center) / max(tau, 1e-12)
    s = 1.0 / (1.0 + np.exp(-arg))
    return 4.0 * s * (1.0 - s)

def rhs(t, y):
    d = np.empty(dim)

    # ============== Engagement 1 ==============
    xt1, yt1, zt1 = y[0], y[1], y[2]
    vxt1, vyt1, vzt1 = y[3], y[4], y[5]
    xi1, yi1, zi1 = y[6], y[7], y[8]
    vxi1, vyi1, vzi1 = y[9], y[10], y[11]
    xh1, yh1, zh1 = y[12], y[13], y[14]
    vxh1, vyh1, vzh1 = y[15], y[16], y[17]

    # Target 1: 3-D weave
    d[0] = vxt1
    d[1] = vyt1
    d[2] = vzt1
    d[3] = a_t1 * np.sin(omega_t1 * t)
    d[4] = a_t1 * np.cos(omega_t1 * t)
    d[5] = a_t1 * 0.3 * np.sin(0.7 * omega_t1 * t)

    # Interceptor 1: PN
    rx1 = xh1 - xi1
    ry1 = yh1 - yi1
    rz1 = zh1 - zi1
    vrx1 = vxh1 - vxi1
    vry1 = vyh1 - vyi1
    vrz1 = vzh1 - vzi1
    a_y1, a_z1, lam_h1 = _pn_accel_3d(rx1, ry1, rz1, vrx1, vry1, vrz1)
    cos_h1, sin_h1 = np.cos(lam_h1), np.sin(lam_h1)

    d[6] = vxi1
    d[7] = vyi1
    d[8] = vzi1
    d[9] = -a_y1 * sin_h1
    d[10] = a_y1 * cos_h1
    d[11] = a_z1

    # EKF 1 propagation
    d[12] = vxh1
    d[13] = vyh1
    d[14] = vzh1
    d[15] = 0.0
    d[16] = 0.0
    d[17] = 0.0

    # ============== Engagement 2 ==============
    xt2, yt2, zt2 = y[18], y[19], y[20]
    vxt2, vyt2, vzt2 = y[21], y[22], y[23]
    xi2, yi2, zi2 = y[24], y[25], y[26]
    vxi2, vyi2, vzi2 = y[27], y[28], y[29]
    xh2, yh2, zh2 = y[30], y[31], y[32]
    vxh2, vyh2, vzh2 = y[33], y[34], y[35]

    # Target 2: crossing maneuver
    d[18] = vxt2
    d[19] = vyt2
    d[20] = vzt2
    d[21] = a_t2 * 0.5 * np.sin(omega_t2 * t)
    d[22] = a_t2 * np.cos(omega_t2 * t)
    d[23] = a_t2 * 0.4 * np.cos(0.5 * omega_t2 * t)

    # Interceptor 2: PN
    rx2 = xh2 - xi2
    ry2 = yh2 - yi2
    rz2 = zh2 - zi2
    vrx2 = vxh2 - vxi2
    vry2 = vyh2 - vyi2
    vrz2 = vzh2 - vzi2
    a_y2, a_z2, lam_h2 = _pn_accel_3d(rx2, ry2, rz2, vrx2, vry2, vrz2)
    cos_h2, sin_h2 = np.cos(lam_h2), np.sin(lam_h2)

    d[24] = vxi2
    d[25] = vyi2
    d[26] = vzi2
    d[27] = -a_y2 * sin_h2
    d[28] = a_y2 * cos_h2
    d[29] = a_z2

    # EKF 2 propagation
    d[30] = vxh2
    d[31] = vyh2
    d[32] = vzh2
    d[33] = 0.0
    d[34] = 0.0
    d[35] = 0.0

    # ============== Scheduler ==============
    T_dw1 = y[36]
    T_dw2 = y[37]
    P_det1 = y[38]
    P_det2 = y[39]

    # Required dwell time grows as track quality degrades
    R1 = np.sqrt(rx1**2 + ry1**2 + rz1**2 + 0.01)
    R2 = np.sqrt(rx2**2 + ry2**2 + rz2**2 + 0.01)
    # SNR proxy: dwell requirement proportional to R^4 (radar equation)
    T_req1 = T_frame * 0.5 * (R1 / 10000.0) ** 2
    T_req2 = T_frame * 0.5 * (R2 / 10000.0) ** 2

    # Resource constraint: if both demand too much, scale proportionally
    T_total_req = T_req1 + T_req2
    if T_total_req > T_frame:
        scale = T_frame / T_total_req
        T_req1 *= scale
        T_req2 *= scale

    d[36] = (T_req1 - T_dw1) / tau_sched
    d[37] = (T_req2 - T_dw2) / tau_sched

    # Detection probability tracks dwell allocation
    P_req1 = _clamp(T_dw1 / max(T_req1, 1e-6), 0.0, 1.0)
    P_req2 = _clamp(T_dw2 / max(T_req2, 1e-6), 0.0, 1.0)
    d[38] = (P_req1 - P_det1) / (tau_sched * 2.0)
    d[39] = (P_req2 - P_det2) / (tau_sched * 2.0)

    # ============== Measurement updates ==============
    k = int(t / T_update + 0.5)
    k = min(k, n_updates - 1)
    t_k = update_times[k]
    w = _sigmoid_window(t, t_k, tau_update)

    if w > 1e-6:
        rate = w / max(tau_update, 1e-6)

        # EKF gains scale with detection probability (coupling mechanism)
        K_pos1 = K_pos_base * _clamp(P_det1, 0.1, 1.0)
        K_vel1 = K_vel_base * _clamp(P_det1, 0.1, 1.0)
        K_pos2 = K_pos_base * _clamp(P_det2, 0.1, 1.0)
        K_vel2 = K_vel_base * _clamp(P_det2, 0.1, 1.0)

        _apply_update_3d(
            d, 12, xi1, yi1, zi1, xt1, yt1, zt1,
            xh1, yh1, zh1,
            noise_r1[k], noise_az1[k], noise_el1[k],
            K_pos1, K_vel1, rate,
        )
        _apply_update_3d(
            d, 30, xi2, yi2, zi2, xt2, yt2, zt2,
            xh2, yh2, zh2,
            noise_r2[k], noise_az2[k], noise_el2[k],
            K_pos2, K_vel2, rate,
        )

    return d