u12x4.h

Go to the documentation of this file.

#pragma once
 
// Unsigned 12.4 fixed-point arithmetic.
// All operations are integer-only in the hot path.
 
#include "fl/stl/stdint.h"
#include "fl/math/fixed_point/isqrt.h"
#include "fl/stl/compiler_control.h"
#include "fl/math/fixed_point/traits.h"
#include "fl/stl/noexcept.h"
#include "fl/stl/undef.h"  // Undefine abs/min/max macros from Arduino.h
 
FL_OPTIMIZATION_LEVEL_O3_BEGIN
 
namespace fl {
 
// Unsigned 12.4 fixed-point value type.
class u12x4 {
  public:
    static constexpr int INT_BITS = 12;
    static constexpr int FRAC_BITS = 4;
    static constexpr i32 SCALE = static_cast<i32>(1) << FRAC_BITS;
 
    // ---- Construction ------------------------------------------------------
 
    constexpr u12x4() FL_NOEXCEPT = default;
 
    explicit constexpr u12x4(float f) FL_NOEXCEPT
        : mValue(static_cast<u16>(f * (SCALE))) {}
 
    // Integer constructor — any integer width (portable: AVR 16-bit int, ARM/x86 32-bit).
    // Compile error if constexpr value exceeds INT_BITS range.
    template <typename IntT, detail::enable_if_integer_t<IntT> = 0>
    explicit constexpr u12x4(IntT n) FL_NOEXCEPT
        : mValue(detail::int_to_fixed<INT_BITS, FRAC_BITS>::from_unsigned(n)) {}
 
    // Raw constructor for C++11 constexpr from_raw
    struct RawTag {};
    constexpr explicit u12x4(u16 raw, RawTag) FL_NOEXCEPT : mValue(raw) {}
 
    static constexpr FASTLED_FORCE_INLINE u12x4 from_raw(u16 raw) FL_NOEXCEPT {
        return u12x4(raw, RawTag());
    }
 
    // ---- Access ------------------------------------------------------------
 
    constexpr u16 raw() const FL_NOEXCEPT { return mValue; }
    constexpr u16 to_int() const FL_NOEXCEPT { return mValue >> FRAC_BITS; }
    constexpr float to_float() const FL_NOEXCEPT { return static_cast<float>(mValue) / (SCALE); }
 
    // ---- Fixed-point arithmetic --------------------------------------------
 
    constexpr FASTLED_FORCE_INLINE u12x4 operator*(u12x4 b) const FL_NOEXCEPT {
        return from_raw(static_cast<u16>(
            (static_cast<u32>(mValue) * b.mValue) >> FRAC_BITS));
    }
 
    constexpr FASTLED_FORCE_INLINE u12x4 operator/(u12x4 b) const FL_NOEXCEPT {
        return from_raw(static_cast<u16>(
            (static_cast<u32>(mValue) * (static_cast<u32>(1) << FRAC_BITS)) / b.mValue));
    }
 
    constexpr FASTLED_FORCE_INLINE u12x4 operator+(u12x4 b) const FL_NOEXCEPT {
        return from_raw(mValue + b.mValue);
    }
 
    constexpr FASTLED_FORCE_INLINE u12x4 operator-(u12x4 b) const FL_NOEXCEPT {
        return from_raw(mValue - b.mValue);
    }
 
    constexpr FASTLED_FORCE_INLINE u12x4 operator>>(int shift) const FL_NOEXCEPT {
        return from_raw(mValue >> shift);
    }
 
    constexpr FASTLED_FORCE_INLINE u12x4 operator<<(int shift) const FL_NOEXCEPT {
        return from_raw(mValue << shift);
    }
 
    // ---- Compound assignment operators -------------------------------------
 
    FASTLED_FORCE_INLINE u12x4& operator+=(u12x4 b) FL_NOEXCEPT {
        mValue += b.mValue;
        return *this;
    }
 
    FASTLED_FORCE_INLINE u12x4& operator-=(u12x4 b) FL_NOEXCEPT {
        mValue -= b.mValue;
        return *this;
    }
 
    FASTLED_FORCE_INLINE u12x4& operator*=(u12x4 b) FL_NOEXCEPT {
        mValue = static_cast<u16>(
            (static_cast<u32>(mValue) * b.mValue) >> FRAC_BITS);
        return *this;
    }
 
    FASTLED_FORCE_INLINE u12x4& operator/=(u12x4 b) FL_NOEXCEPT {
        mValue = static_cast<u16>(
            (static_cast<u32>(mValue) * (static_cast<u32>(1) << FRAC_BITS)) / b.mValue);
        return *this;
    }
 
    // ---- Scalar multiply (no fixed-point shift) ----------------------------
 
    constexpr FASTLED_FORCE_INLINE u12x4 operator*(u16 scalar) const FL_NOEXCEPT {
        return from_raw(mValue * scalar);
    }
 
    friend constexpr u12x4 operator*(u16 scalar, u12x4 fp) FL_NOEXCEPT {
        return u12x4::from_raw(scalar * fp.mValue);
    }
 
    // ---- Comparisons -------------------------------------------------------
 
    constexpr bool operator<(u12x4 b) const FL_NOEXCEPT { return mValue < b.mValue; }
    constexpr bool operator>(u12x4 b) const FL_NOEXCEPT { return mValue > b.mValue; }
    constexpr bool operator<=(u12x4 b) const FL_NOEXCEPT { return mValue <= b.mValue; }
    constexpr bool operator>=(u12x4 b) const FL_NOEXCEPT { return mValue >= b.mValue; }
    constexpr bool operator==(u12x4 b) const FL_NOEXCEPT { return mValue == b.mValue; }
    constexpr bool operator!=(u12x4 b) const FL_NOEXCEPT { return mValue != b.mValue; }
 
    // ---- Math ---------------------------------------------------------------
 
    static constexpr FASTLED_FORCE_INLINE u12x4 mod(u12x4 a, u12x4 b) FL_NOEXCEPT {
        return from_raw(a.mValue % b.mValue);
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 floor(u12x4 x) FL_NOEXCEPT {
        return from_raw(x.mValue & ~(u16((SCALE) - 1)));
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 ceil(u12x4 x) FL_NOEXCEPT {
        return from_raw((x.mValue & ~(u16((SCALE) - 1))) +
                        ((x.mValue & u16((SCALE) - 1)) ? (SCALE) : 0));
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 fract(u12x4 x) FL_NOEXCEPT {
        return from_raw(x.mValue & u16((SCALE) - 1));
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 abs(u12x4 x) FL_NOEXCEPT {
        // Unsigned values are always non-negative
        return x;
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 min(u12x4 a, u12x4 b) FL_NOEXCEPT {
        return a < b ? a : b;
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 max(u12x4 a, u12x4 b) FL_NOEXCEPT {
        return a > b ? a : b;
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 lerp(u12x4 a, u12x4 b, u12x4 t) FL_NOEXCEPT {
        return a + (b - a) * t;
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 clamp(u12x4 x, u12x4 lo, u12x4 hi) FL_NOEXCEPT {
        return x < lo ? lo : (x > hi ? hi : x);
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 step(u12x4 edge, u12x4 x) FL_NOEXCEPT {
        return x < edge ? u12x4() : u12x4(1.0f);
    }
 
    static FASTLED_FORCE_INLINE u12x4 smoothstep(u12x4 edge0, u12x4 edge1, u12x4 x) FL_NOEXCEPT {
        constexpr u12x4 zero(0.0f);
        constexpr u12x4 one(1.0f);
        constexpr u12x4 two(2.0f);
        constexpr u12x4 three(3.0f);
        u12x4 t = clamp((x - edge0) / (edge1 - edge0), zero, one);
        return t * t * (three - two * t);
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 sqrt(u12x4 x) FL_NOEXCEPT {
        return x.mValue == 0 ? u12x4() : from_raw(static_cast<u16>(
            fl::isqrt32(static_cast<u32>(x.mValue) << FRAC_BITS)));
    }
 
    static constexpr FASTLED_FORCE_INLINE u12x4 rsqrt(u12x4 x) FL_NOEXCEPT {
        return sqrt(x).mValue == 0
            ? u12x4()
            : from_raw(static_cast<u16>(1) << FRAC_BITS) / sqrt(x);
    }
 
    static FASTLED_FORCE_INLINE u12x4 pow(u12x4 base, u12x4 exp) FL_NOEXCEPT {
        if (base.mValue == 0) return u12x4();
        constexpr u12x4 one(1.0f);
        if (exp.mValue == 0) return one;
        if (base == one) return one;
        // Snap base values within ~2 ULPs of 1.0 to exactly 1.0 to dodge the
        // log2(1+t) minimax polynomial's upper-endpoint residual (#2969).
        constexpr u16 kOneRaw = static_cast<u16>(SCALE);
        if (base.mValue >= static_cast<u16>(kOneRaw - 2) &&
            base.mValue <= kOneRaw) {
            return one;
        }
        return exp2_fp(exp * log2_fp(base));
    }
 
  private:
    u16 mValue = 0;
 
    // Returns 0-based position of highest set bit, or -1 if v==0.
    static constexpr FASTLED_FORCE_INLINE int highest_bit(u32 v) FL_NOEXCEPT {
        return v == 0 ? -1 : _highest_bit_step(v, 0);
    }
 
    static constexpr int _highest_bit_step(u32 v, int r) FL_NOEXCEPT {
        return (v & 0xFFFF0000u) ? _highest_bit_step(v >> 16, r + 16)
             : (v & 0x0000FF00u) ? _highest_bit_step(v >> 8,  r + 8)
             : (v & 0x000000F0u) ? _highest_bit_step(v >> 4,  r + 4)
             : (v & 0x0000000Cu) ? _highest_bit_step(v >> 2,  r + 2)
             : (v & 0x00000002u) ? r + 1
             : r;
    }
 
    // Fixed-point log base 2 for positive values.
    // Uses 4-term minimax polynomial for log2(1+t), t in [0,1).
    // Horner evaluation uses u32 intermediates (12 frac bits) to minimize
    // rounding error, then converts back to 4 frac bits.
    static FASTLED_FORCE_INLINE u12x4 log2_fp(u12x4 x) FL_NOEXCEPT {
        u32 val = static_cast<u32>(x.mValue);
        int msb = highest_bit(val);
        u32 int_part = msb - FRAC_BITS;
        u32 t;
        if (msb >= FRAC_BITS) {
            t = static_cast<u32>(
                (val >> (msb - FRAC_BITS)) - (SCALE));
        } else {
            t = static_cast<u32>(
                (val << (FRAC_BITS - msb)) - (SCALE));
        }
        // 4-term minimax coefficients for log2(1+t), t in [0,1).
        // Stored as u32 with 12 fractional bits. Max product ~2^21, fits u32 comfortably.
        constexpr int IFRAC = 12;
        constexpr u32 c0 = 5907;    // 1.44179 * 2^12
        constexpr u32 c1 = 2864;    // 0.69907 * 2^12 (no negation for unsigned)
        constexpr u32 c2 = 1489;    // 0.36348 * 2^12
        constexpr u32 c3 = 437;     // 0.10660 * 2^12 (no negation for unsigned)
        // Extend t from 4 to 12 frac bits.
        u32 t12 = static_cast<u32>(t) << (IFRAC - FRAC_BITS);
        // Simplified Horner for unsigned: t * (c0 + t * c1 + t^2 * c2 + t^3 * c3)
        u32 acc = c3;
        acc = c2 + ((acc * t12) >> IFRAC);
        acc = c1 + ((acc * t12) >> IFRAC);
        acc = c0 + ((acc * t12) >> IFRAC);
        u32 frac_part = (acc * t12) >> IFRAC;
        // Convert from 12 frac bits back to 4.
        u16 frac4 = static_cast<u16>(frac_part >> (IFRAC - FRAC_BITS));
        return from_raw(static_cast<u16>((int_part << FRAC_BITS) + frac4));
    }
 
    // Fixed-point 2^x. Uses 4-term minimax polynomial for 2^t, t in [0,1).
    // Horner evaluation uses u32 intermediates (12 frac bits) to minimize
    // rounding error, then converts back to 4 frac bits.
    static FASTLED_FORCE_INLINE u12x4 exp2_fp(u12x4 x) FL_NOEXCEPT {
        u12x4 fl_val = floor(x);
        u12x4 fr = x - fl_val;
        u32 n = fl_val.mValue >> FRAC_BITS;
        if (n >= INT_BITS - 1) return from_raw(0xFFFF);
        u32 int_pow = static_cast<u32>(SCALE) << n;
        // 4-term minimax coefficients for 2^t - 1, t in [0,1).
        // Stored as u32 with 12 fractional bits.
        constexpr int IFRAC = 12;
        constexpr u32 d0 = 2839;    // 0.69316 * 2^12
        constexpr u32 d1 = 986;     // 0.24071 * 2^12
        constexpr u32 d2 = 219;     // 0.05336 * 2^12
        constexpr u32 d3 = 52;      // 0.01276 * 2^12
        // Extend fr from 4 to 12 frac bits.
        u32 fr12 = static_cast<u32>(fr.mValue) << (IFRAC - FRAC_BITS);
        // Horner: 1 + fr * (d0 + fr * (d1 + fr * (d2 + fr * d3)))
        u32 acc = d3;
        acc = d2 + ((acc * fr12) >> IFRAC);
        acc = d1 + ((acc * fr12) >> IFRAC);
        acc = d0 + ((acc * fr12) >> IFRAC);
        constexpr u32 one12 = 1 << IFRAC;
        u32 frac_pow12 = one12 + ((acc * fr12) >> IFRAC);
        // Convert from 12 frac bits to 4 frac bits, then scale by int_pow.
        u32 frac_pow4 = frac_pow12 >> (IFRAC - FRAC_BITS);
        u32 result =
            (int_pow * frac_pow4) >> FRAC_BITS;
        return from_raw(static_cast<u16>(result));
    }
};
 
} // namespace fl
 
FL_OPTIMIZATION_LEVEL_O3_END