ph-color 0.1.1

//! Shared fixed-point arithmetic for matrix, gain, and LUT work.
//!
//! Channel values are [`Q0_16`]. Coefficients are [`Q4_28`]. See those types
//! for their formats.
//!
//! Matrix and gain paths accumulate `coef × channel` products in [`i64`], then
//! apply **one** shift-round-saturate. There is no intermediate rounding.
//!
//! Every kernel here is branch-free and division-free on the value path: a
//! 32-bit target must not link `__aeabi_uidiv` / `__aeabi_uldivmod` for a
//! color op, and a per-pixel inner loop must not mispredict.

use crate::fixed::{Q0_16, Q4_28};

/// `n / 65535`, exact for `n <= 4294901759`, with no divide.
///
/// `1 / 65535 = 2^-16 + 2^-32 + 2^-48 + ...`. Truncating that series after
/// two terms and adding one ULP of bias is exact over this crate's proven
/// numerator range; it first diverges at `65535 << 16`, one past the largest
/// numerator any caller here can build. The additions cannot overflow [`u32`] there
/// (`4294901759 + 65535 + 1 < 2^32`), so the saturating forms below are the
/// named-operation spelling (`docs/NUMERICS.md`, "Arithmetic") of something
/// that never actually saturates.
#[must_use]
pub(crate) const fn div_65535(n: u32) -> u32 {
    n.saturating_add(n >> 16).saturating_add(1) >> 16
}

/// `n - q * 65535`, where `q` is [`div_65535`] of `n` or a clamp of it.
///
/// `n - q * 65535 == n + q - (q << 16)`. The `+ q` is applied first so the
/// intermediate never goes negative (`q << 16` alone can exceed `n`).
///
/// The result is returned as [`u16`], not [`u32`]. A true remainder is
/// `< 65535` and the clamped-index form used by [`crate::InterpLut::lookup`]
/// tops out at exactly `65535`, so the narrowing is lossless — and it is what
/// tells the optimizer that a later `rem * channel` product cannot overflow
/// [`u32`], which keeps that multiply a single 32-bit `muls` instead of a
/// widening overflow check.
#[must_use]
pub(crate) const fn rem_65535(n: u32, q: u32) -> u16 {
    // Proven bounded: `0..=65535`.
    n.saturating_add(q).saturating_sub(q << 16) as u16
}

/// Add one Q4.28 × UQ0.16 product into an [`i64`] accumulator.
///
/// The channel is treated as an integer `0..=65535` (full scale = 1.0). No
/// rounding happens here; call [`shift_round_sat_q0_16`] once after all
/// terms are accumulated.
///
/// The product is assembled from two 32-bit multiplies rather than one
/// `i64 * i64`, so a target without a widening multiply never calls
/// `__aeabi_lmul`. Splitting `coef` into `(coef >> 16, coef as u32 & 0xffff)`
/// is exact in two's complement, and both partial products are proven to fit
/// their 32-bit type: `|coef >> 16| <= 32768` and `32768 * 65535 < 2^31`,
/// `(coef as u32 & 0xffff) * 65535 <= 65535 * 65535 < 2^32`.
#[must_use]
pub(crate) const fn mul_acc(acc: i64, coef: Q4_28, ch: Q0_16) -> i64 {
    let raw = coef.to_raw();
    let ch = ch.to_raw() as u32;
    let hi = (raw >> 16).saturating_mul(ch as i32);
    let lo = (raw as u32 & 0xffff).saturating_mul(ch);
    let product = ((hi as i64) << 16).saturating_add(lo as i64);
    acc.saturating_add(product)
}

/// Accumulate three Q4.28 × UQ0.16 terms and apply one shift-round-saturate.
///
/// This is the matrix-row / RGB-gain kernel: `i64` products, then a single
/// round-half-away-from-zero shift into a saturating [`Q0_16`] channel.
#[must_use]
pub(crate) const fn mul_acc3(coefs: [Q4_28; 3], chs: [Q0_16; 3]) -> Q0_16 {
    let [c0, c1, c2] = coefs;
    let [x, y, z] = chs;
    let acc = mul_acc(0, c0, x);
    let acc = mul_acc(acc, c1, y);
    let acc = mul_acc(acc, c2, z);
    shift_round_sat_q0_16(acc)
}

/// One shift-round-saturate from a Q4.28×UQ0.16 accumulator to [`Q0_16`].
///
/// Right-shifts by [`Q4_28::FRAC_BITS`], rounds half away from zero, then
/// saturates into `0..=65535`. Negative results saturate to `0`.
#[must_use]
pub(crate) const fn shift_round_sat_q0_16(acc: i64) -> Q0_16 {
    sat_i64_to_q0_16(shift_round_away_from_zero(acc, Q4_28::FRAC_BITS))
}

/// Round-half-away-from-zero arithmetic right shift.
///
/// `shift` is a bit count in `0..=62` on every call from this crate, so the
/// divisor is `2^shift` and the whole operation is a bias plus two shifts —
/// no [`i64`] division, and no branch on the value.
///
/// Adding `+2^(shift-1)` to a non-negative value and `-2^(shift-1)` to a
/// negative one turns round-half-away-from-zero into truncation toward zero,
/// and truncation toward zero by `2^shift` is `(v + (v >> 63 & (2^shift-1))) >> shift`.
#[must_use]
pub(crate) const fn shift_round_away_from_zero(value: i64, shift: u32) -> i64 {
    if shift == 0 {
        return value;
    }
    let Some(divisor) = 1i64.checked_shl(shift) else {
        return if value < 0 { i64::MIN } else { i64::MAX };
    };
    // Bounded: `divisor` is `2^shift` with `shift` in `1..=62`, so `half` is
    // positive and `mask` is `2^shift - 1`.
    let half = divisor >> 1;
    let mask = divisor.saturating_sub(1);
    // `sign` is `0` for a non-negative value and `-1` for a negative one, so
    // `(half ^ sign) - sign` is `+half` / `-half` with no branch.
    let sign = value >> 63;
    let biased = value.saturating_add((half ^ sign).saturating_sub(sign));
    biased.saturating_add(biased >> 63 & mask) >> shift
}

/// Saturate a shifted accumulator into `0..=65535`, branch-free.
///
/// `(v - 1) >> 63` is all ones exactly when `v <= 0`, and `(65534 - v) >> 63`
/// is all ones exactly when `v >= 65535`; the two masks select `0`, `65535`,
/// or `v` with no branch.
const fn sat_i64_to_q0_16(value: i64) -> Q0_16 {
    let low = (value.saturating_sub(1) >> 63) as u64; // all ones when value <= 0
    let high = (65534i64.saturating_sub(value) >> 63) as u64; // all ones when value >= 65535
    let clamped = ((value as u64) & !low & !high) | (high & u16::MAX as u64);
    // Proven bounded: `0..=65535`.
    Q0_16::from_raw(clamped as u16)
}

/// Blend `a` toward `b` with UQ0.16 `t` (`0` = `a`, `65535` = `b`).
///
/// Rounding is round-half-up on the exact ` / 65535` division. `a`, `b`, and
/// `t` are all [`Q0_16`], so the numerator tops out at `65535 * 65535`, still
/// under [`u32::MAX`] — this stays in [`u32`] rather than promoting to
/// [`i64`] like the matrix/gain path, so it never touches a 64-bit multiply
/// or divide.
///
/// Round-half-up is folded into the quotient: `65535` is odd, so
/// `floor((num + 32767) / 65535)` is `round_half_up(num / 65535)`, which
/// removes the remainder, its comparison, and the `t == 0` / `t == 65535`
/// early exits — the general path already returns `a` and `b` exactly there.
#[must_use]
pub(crate) const fn lerp_q0_16(a: Q0_16, b: Q0_16, t: Q0_16) -> Q0_16 {
    let den = u16::MAX as u32;
    let t = t.to_raw() as u32;
    let num = (a.to_raw() as u32)
        .saturating_mul(den.saturating_sub(t))
        .saturating_add((b.to_raw() as u32).saturating_mul(t))
        .saturating_add(den >> 1);
    // Proven bounded: `num <= 65535 * 65535 + 32767` ⇒ quotient `<= 65535`.
    Q0_16::from_raw(div_65535(num) as u16)
}

#[cfg(test)]
mod tests {
    use super::*;

    const COEF_FRAC_BITS: u32 = Q4_28::FRAC_BITS;
    const HALF_LSB: i64 = 1 << (COEF_FRAC_BITS - 1);
    const ZERO: Q4_28 = Q4_28::ZERO;
    const ONE: Q4_28 = Q4_28::ONE;

    fn ch(raw: u16) -> Q0_16 {
        Q0_16::from_raw(raw)
    }

    #[test]
    fn identity_gain_preserves_full_scale() {
        assert_eq!(
            mul_acc3([ONE, ZERO, ZERO], [ch(65535), ch(0), ch(0)]),
            ch(65535)
        );
        assert_eq!(mul_acc3([ONE, ZERO, ZERO], [ch(0), ch(0), ch(0)]), ch(0));
        assert_eq!(mul_acc3([ONE, ZERO, ZERO], [ch(1), ch(0), ch(0)]), ch(1));
    }

    #[test]
    fn positive_midpoint_rounds_away_from_zero() {
        // (q << 28) + 2^27 is exactly q + 0.5 ULP of the shifted result.
        let acc = (3i64 << COEF_FRAC_BITS).saturating_add(HALF_LSB);
        assert_eq!(shift_round_away_from_zero(acc, COEF_FRAC_BITS), 4);
        assert_eq!(shift_round_sat_q0_16(acc), ch(4));
    }

    #[test]
    fn negative_midpoint_rounds_away_from_zero() {
        let acc = (-3i64 << COEF_FRAC_BITS).saturating_sub(HALF_LSB);
        assert_eq!(shift_round_away_from_zero(acc, COEF_FRAC_BITS), -4);
        // Public channel path saturates negatives to 0.
        assert_eq!(shift_round_sat_q0_16(acc), ch(0));
    }

    #[test]
    fn below_half_does_not_round_away() {
        let acc = (3i64 << COEF_FRAC_BITS).saturating_add(HALF_LSB.saturating_sub(1));
        assert_eq!(shift_round_away_from_zero(acc, COEF_FRAC_BITS), 3);
        let neg = (-3i64 << COEF_FRAC_BITS).saturating_add(1 - HALF_LSB);
        assert_eq!(shift_round_away_from_zero(neg, COEF_FRAC_BITS), -3);
    }

    #[test]
    fn half_unit_channel_rounds_away() {
        // coef = 0.5 Q4.28, ch = 1 → product = 2^27, which is +0.5 ULP → 1.
        let half = Q4_28::from_raw(ONE.to_raw() / 2);
        assert_eq!(shift_round_sat_q0_16(mul_acc(0, half, ch(1))), ch(1));
    }

    #[test]
    fn positive_overflow_saturates_to_u16_max() {
        let max = Q4_28::from_raw(i32::MAX);
        let acc = mul_acc(0, max, ch(65535));
        let acc = mul_acc(acc, max, ch(65535));
        let acc = mul_acc(acc, max, ch(65535));
        assert!(
            acc > i64::from(i32::MAX),
            "three-term product must not fit in i32"
        );
        assert_eq!(shift_round_sat_q0_16(acc), ch(65535));
        assert_eq!(
            mul_acc3([max, max, max], [ch(65535), ch(65535), ch(65535)]),
            ch(65535)
        );
    }

    #[test]
    fn negative_overflow_saturates_to_zero() {
        let min = Q4_28::from_raw(i32::MIN);
        assert_eq!(
            mul_acc3([min, min, min], [ch(65535), ch(65535), ch(65535)]),
            ch(0)
        );
    }

    #[test]
    fn div_and_rem_65535_match_hardware_over_the_proven_range() {
        // The largest numerator any caller here builds is `65535 * 65535 +
        // 32767` (the lerp path). Walk the whole range on a stride that is
        // coprime with 65535 and 2^16, plus every boundary.
        const MAX: u32 = 65535 * 65535 + 32767;
        let mut n: u32 = 0;
        while n <= MAX {
            let q = div_65535(n);
            assert_eq!(q, n / 65535, "quotient at n={n}");
            assert_eq!(u32::from(rem_65535(n, q)), n % 65535, "remainder at n={n}");
            n = match n.checked_add(65_521) {
                Some(next) => next,
                None => break,
            };
        }
        for n in [
            0u32,
            1,
            65534,
            65535,
            65536,
            65535 * 65535 - 1,
            65535 * 65535,
            MAX,
        ] {
            let q = div_65535(n);
            assert_eq!(q, n / 65535, "quotient at boundary n={n}");
            assert_eq!(u32::from(rem_65535(n, q)), n % 65535, "remainder at n={n}");
        }
    }

    #[test]
    fn rem_65535_of_a_clamped_index_is_full_scale() {
        // `InterpLut::lookup` clamps the segment index, so the remainder can
        // reach exactly 65535 — the value that makes the blend return `b`.
        let pos: u32 = 65535 * 256;
        assert_eq!(rem_65535(pos, 255), u16::MAX);
    }

    #[test]
    fn lerp_matches_explicit_round_half_up() {
        // The fused `+32767` must reproduce divide-then-round-half-up exactly.
        for a in [0u16, 1, 12345, 32768, 65534, 65535] {
            for b in [0u16, 1, 999, 40000, 65535] {
                for t in [0u16, 1, 16384, 32767, 32768, 65534, 65535] {
                    let num = u32::from(a) * (65535 - u32::from(t)) + u32::from(b) * u32::from(t);
                    let (q, r) = (num / 65535, num % 65535);
                    let want = if r * 2 >= 65535 { q + 1 } else { q };
                    assert_eq!(
                        lerp_q0_16(ch(a), ch(b), ch(t)).to_raw(),
                        want as u16,
                        "a={a} b={b} t={t}"
                    );
                }
            }
        }
    }

    #[test]
    fn lerp_endpoints_need_no_special_case() {
        for a in [0u16, 7, 30000, 65535] {
            for b in [0u16, 9, 51234, 65535] {
                assert_eq!(lerp_q0_16(ch(a), ch(b), Q0_16::ZERO), ch(a));
                assert_eq!(lerp_q0_16(ch(a), ch(b), Q0_16::ONE), ch(b));
            }
        }
    }

    #[test]
    fn mul_acc_is_the_exact_widening_product() {
        // The split-product form must equal `coef as i64 * ch as i64` for
        // every coefficient extreme, including out-of-contract magnitudes.
        for coef in [
            i32::MIN,
            i32::MIN + 1,
            -(7 << 28),
            -1,
            0,
            1,
            7 << 28,
            i32::MAX,
            1 << 28,
        ] {
            for chan in [0u16, 1, 2, 32767, 32768, 65534, 65535] {
                assert_eq!(
                    mul_acc(0, Q4_28::from_raw(coef), ch(chan)),
                    i64::from(coef) * i64::from(chan),
                    "coef={coef} ch={chan}"
                );
            }
        }
    }

    #[test]
    fn saturation_boundaries_are_exact() {
        assert_eq!(sat_i64_to_q0_16(i64::MIN), Q0_16::ZERO);
        assert_eq!(sat_i64_to_q0_16(-1), Q0_16::ZERO);
        assert_eq!(sat_i64_to_q0_16(0), Q0_16::ZERO);
        assert_eq!(sat_i64_to_q0_16(1), ch(1));
        assert_eq!(sat_i64_to_q0_16(65534), ch(65534));
        assert_eq!(sat_i64_to_q0_16(65535), Q0_16::ONE);
        assert_eq!(sat_i64_to_q0_16(i64::MAX), Q0_16::ONE);
    }

    #[test]
    fn exact_integers_do_not_round() {
        let acc = 5i64 << COEF_FRAC_BITS;
        assert_eq!(shift_round_away_from_zero(acc, COEF_FRAC_BITS), 5);
        assert_eq!(shift_round_sat_q0_16(acc), ch(5));
    }
}