/**
 * @file fast_math.hpp
 * @brief Ultra-Fast Mathematical Primitives for Gate5000
 *
 * ═══════════════════════════════════════════════════════════════════════════════
 * OPTIMIZATION TECHNIQUES:
 * ═══════════════════════════════════════════════════════════════════════════════
 *
 * 1. FAST INVERSE SQUARE ROOT (Quake III style)
 *    - 4x faster than 1/sqrt(x)
 *    - Error < 1%
 *
 * 2. LOOKUP TABLES (LUT)
 *    - 256-entry sine/cosine tables
 *    - 64-entry exp/log tables
 *    - Linear interpolation for precision
 *
 * 3. POLYNOMIAL APPROXIMATIONS
 *    - Minimax polynomials for transcendentals
 *    - Padé approximants for rational functions
 *
 * 4. BIT MANIPULATION
 *    - Fast floor/ceil via bit ops
 *    - Fast abs/sign extraction
 *    - Fast modulo for powers of 2
 *
 * 5. SIMD-FRIENDLY LAYOUTS
 *    - 4-wide vector operations
 *    - Cache-aligned data structures
 *
 * ═══════════════════════════════════════════════════════════════════════════════
 */

#pragma once

#include <cstdint>
#include <cstring>
#include <array>
#include "constants.hpp"

namespace mind_farm {
namespace fast {

// ═══════════════════════════════════════════════════════════════════════════════
// I. FAST INVERSE SQUARE ROOT
// ═══════════════════════════════════════════════════════════════════════════════

/**
 * @brief Fast inverse square root (Quake III algorithm)
 * @param x Input value (must be positive)
 * @return Approximation of 1/sqrt(x), error < 1%
 */
inline float fast_inv_sqrt(float x) {
    float xhalf = 0.5f * x;
    int32_t i;
    std::memcpy(&i, &x, sizeof(float));
    i = 0x5f3759df - (i >> 1);  // Magic constant
    std::memcpy(&x, &i, sizeof(float));
    x = x * (1.5f - xhalf * x * x);  // Newton iteration
    return x;
}

/**
 * @brief Fast square root using inverse sqrt
 */
inline float fast_sqrt(float x) {
    if (x <= 0.0f) return 0.0f;
    return x * fast_inv_sqrt(x);
}

/**
 * @brief Fast reciprocal 1/x
 */
inline float fast_rcp(float x) {
    int32_t i;
    std::memcpy(&i, &x, sizeof(float));
    i = 0x7ef0e840 - i;  // Magic constant for reciprocal
    float y;
    std::memcpy(&y, &i, sizeof(float));
    y = y * (2.0f - x * y);  // Newton iteration
    return y;
}

// ═══════════════════════════════════════════════════════════════════════════════
// II. LOOKUP TABLES
// ═══════════════════════════════════════════════════════════════════════════════

// Precomputed sine table (256 entries for [0, 2π])
alignas(64) constexpr std::array<float, 256> SIN_LUT = []() {
    std::array<float, 256> lut{};
    for (int i = 0; i < 256; ++i) {
        double angle = i * 2.0 * PI / 256.0;
        lut[i] = static_cast<float>(std::sin(angle));
    }
    return lut;
}();

// Precomputed exp table (64 entries for [-4, 4])
alignas(64) constexpr std::array<float, 64> EXP_LUT = []() {
    std::array<float, 64> lut{};
    for (int i = 0; i < 64; ++i) {
        double x = (i - 32) * 0.125;  // Range [-4, 4]
        lut[i] = static_cast<float>(std::exp(x));
    }
    return lut;
}();

// Precomputed tanh table (64 entries for [-4, 4])
alignas(64) constexpr std::array<float, 64> TANH_LUT = []() {
    std::array<float, 64> lut{};
    for (int i = 0; i < 64; ++i) {
        double x = (i - 32) * 0.125;  // Range [-4, 4]
        lut[i] = static_cast<float>(std::tanh(x));
    }
    return lut;
}();

// Precomputed sigmoid table (64 entries for [-8, 8])
alignas(64) constexpr std::array<float, 64> SIGMOID_LUT = []() {
    std::array<float, 64> lut{};
    for (int i = 0; i < 64; ++i) {
        double x = (i - 32) * 0.25;  // Range [-8, 8]
        lut[i] = static_cast<float>(1.0 / (1.0 + std::exp(-x)));
    }
    return lut;
}();

/**
 * @brief Fast sine using LUT with linear interpolation
 */
inline float fast_sin(float x) {
    // Normalize to [0, 2π]
    x = x - static_cast<float>(2.0 * PI) * std::floor(x / static_cast<float>(2.0 * PI));
    float idx_f = x * (256.0f / static_cast<float>(2.0 * PI));
    int idx = static_cast<int>(idx_f) & 255;
    float frac = idx_f - std::floor(idx_f);
    return SIN_LUT[idx] * (1.0f - frac) + SIN_LUT[(idx + 1) & 255] * frac;
}

/**
 * @brief Fast cosine using LUT
 */
inline float fast_cos(float x) {
    return fast_sin(x + static_cast<float>(PI * 0.5));
}

/**
 * @brief Fast tanh using LUT with linear interpolation
 */
inline float fast_tanh(float x) {
    if (x < -4.0f) return -1.0f;
    if (x > 4.0f) return 1.0f;
    float idx_f = (x + 4.0f) * 8.0f;  // Map [-4,4] to [0,64]
    int idx = static_cast<int>(idx_f);
    if (idx >= 63) return TANH_LUT[63];
    float frac = idx_f - idx;
    return TANH_LUT[idx] * (1.0f - frac) + TANH_LUT[idx + 1] * frac;
}

/**
 * @brief Fast sigmoid using LUT
 */
inline float fast_sigmoid(float x) {
    if (x < -8.0f) return 0.0f;
    if (x > 8.0f) return 1.0f;
    float idx_f = (x + 8.0f) * 4.0f;  // Map [-8,8] to [0,64]
    int idx = static_cast<int>(idx_f);
    if (idx >= 63) return SIGMOID_LUT[63];
    float frac = idx_f - idx;
    return SIGMOID_LUT[idx] * (1.0f - frac) + SIGMOID_LUT[idx + 1] * frac;
}

// ═══════════════════════════════════════════════════════════════════════════════
// III. POLYNOMIAL APPROXIMATIONS
// ═══════════════════════════════════════════════════════════════════════════════

/**
 * @brief Fast exp using polynomial approximation
 * Valid for x ∈ [-4, 4], error < 0.1%
 */
inline float fast_exp(float x) {
    if (x < -10.0f) return 0.0f;
    if (x > 10.0f) return 22026.0f;

    // Reduce to [0, ln2] range
    float k = std::floor(x * 1.4426950408889634f);  // x / ln(2)
    float r = x - k * 0.6931471805599453f;          // x - k * ln(2)

    // Minimax polynomial for exp(r) on [0, ln2]
    float p = 1.0f + r * (1.0f + r * (0.5f + r * (0.166666667f + r * 0.041666667f)));

    // Reconstruct: exp(x) = 2^k * exp(r)
    int32_t i = static_cast<int32_t>(k);
    int32_t exp_bits = (127 + i) << 23;
    float scale;
    std::memcpy(&scale, &exp_bits, sizeof(float));
    return p * scale;
}

/**
 * @brief Fast log using polynomial approximation
 */
inline float fast_log(float x) {
    if (x <= 0.0f) return -1e10f;

    // Extract exponent and mantissa
    int32_t i;
    std::memcpy(&i, &x, sizeof(float));
    int32_t e = ((i >> 23) & 255) - 127;
    i = (i & 0x007fffff) | 0x3f800000;
    float m;
    std::memcpy(&m, &i, sizeof(float));

    // Polynomial approximation for log(m) where m ∈ [1, 2)
    m = m - 1.0f;
    float p = m * (1.0f - m * (0.5f - m * (0.333333f - m * 0.25f)));

    return p + e * 0.6931471805599453f;
}

/**
 * @brief Fast pow using exp(y * log(x))
 */
inline float fast_pow(float x, float y) {
    return fast_exp(y * fast_log(x));
}

// ═══════════════════════════════════════════════════════════════════════════════
// IV. BIT MANIPULATION TRICKS
// ═══════════════════════════════════════════════════════════════════════════════

/**
 * @brief Fast absolute value (branchless)
 */
inline float fast_abs(float x) {
    int32_t i;
    std::memcpy(&i, &x, sizeof(float));
    i &= 0x7fffffff;
    std::memcpy(&x, &i, sizeof(float));
    return x;
}

/**
 * @brief Fast sign extraction (returns -1, 0, or 1)
 */
inline int fast_sign(float x) {
    return (x > 0.0f) - (x < 0.0f);
}

/**
 * @brief Fast floor using bit manipulation
 */
inline int fast_floor(float x) {
    int i = static_cast<int>(x);
    return i - (x < i);
}

/**
 * @brief Fast ceil using bit manipulation
 */
inline int fast_ceil(float x) {
    int i = static_cast<int>(x);
    return i + (x > i);
}

/**
 * @brief Fast modulo for power-of-2 divisors
 */
inline uint32_t fast_mod_pow2(uint32_t x, uint32_t pow2) {
    return x & (pow2 - 1);
}

/**
 * @brief Fast clamp to [0, 1] (branchless)
 */
inline float fast_clamp01(float x) {
    x = x > 0.0f ? x : 0.0f;
    return x < 1.0f ? x : 1.0f;
}

/**
 * @brief Fast linear interpolation
 */
inline float fast_lerp(float a, float b, float t) {
    return a + t * (b - a);
}

// ═══════════════════════════════════════════════════════════════════════════════
// V. FAST VECTOR OPERATIONS (4-wide)
// ═══════════════════════════════════════════════════════════════════════════════

/**
 * @brief 4-element vector for SIMD-friendly operations
 */
struct alignas(16) Vec4 {
    float x, y, z, w;

    Vec4() : x(0), y(0), z(0), w(0) {}
    Vec4(float v) : x(v), y(v), z(v), w(v) {}
    Vec4(float x_, float y_, float z_, float w_) : x(x_), y(y_), z(z_), w(w_) {}

    Vec4 operator+(const Vec4& o) const { return {x+o.x, y+o.y, z+o.z, w+o.w}; }
    Vec4 operator-(const Vec4& o) const { return {x-o.x, y-o.y, z-o.z, w-o.w}; }
    Vec4 operator*(const Vec4& o) const { return {x*o.x, y*o.y, z*o.z, w*o.w}; }
    Vec4 operator*(float s) const { return {x*s, y*s, z*s, w*s}; }

    float dot(const Vec4& o) const { return x*o.x + y*o.y + z*o.z + w*o.w; }
    float sum() const { return x + y + z + w; }
    float max() const { return std::max(std::max(x, y), std::max(z, w)); }

    Vec4 tanh() const {
        return {fast_tanh(x), fast_tanh(y), fast_tanh(z), fast_tanh(w)};
    }
    Vec4 sigmoid() const {
        return {fast_sigmoid(x), fast_sigmoid(y), fast_sigmoid(z), fast_sigmoid(w)};
    }
};

/**
 * @brief Fast 4x4 matrix-vector multiply
 */
inline Vec4 mat4_vec4(const float* M, const Vec4& v) {
    return Vec4(
        M[0]*v.x + M[1]*v.y + M[2]*v.z + M[3]*v.w,
        M[4]*v.x + M[5]*v.y + M[6]*v.z + M[7]*v.w,
        M[8]*v.x + M[9]*v.y + M[10]*v.z + M[11]*v.w,
        M[12]*v.x + M[13]*v.y + M[14]*v.z + M[15]*v.w
    );
}

// ═══════════════════════════════════════════════════════════════════════════════
// VI. FAST PHI COMPUTATIONS
// ═══════════════════════════════════════════════════════════════════════════════

/**
 * @brief Fast consciousness metric Φ = 1/(1 + |x - 1|)
 */
inline float fast_phi(float x) {
    float diff = fast_abs(x - 1.0f);
    return fast_rcp(1.0f + diff);
}

/**
 * @brief Fast phi gradient ∂Φ/∂x
 */
inline float fast_phi_grad(float x) {
    float diff = x - 1.0f;
    float abs_diff = fast_abs(diff);
    float denom = (1.0f + abs_diff);
    return -fast_sign(diff) * fast_rcp(denom * denom);
}

/**
 * @brief Batch phi computation (4 values at once)
 */
inline Vec4 fast_phi_batch(const Vec4& x) {
    Vec4 diff = x - Vec4(1.0f);
    Vec4 abs_diff = {fast_abs(diff.x), fast_abs(diff.y), fast_abs(diff.z), fast_abs(diff.w)};
    return {
        fast_rcp(1.0f + abs_diff.x),
        fast_rcp(1.0f + abs_diff.y),
        fast_rcp(1.0f + abs_diff.z),
        fast_rcp(1.0f + abs_diff.w)
    };
}

// ═══════════════════════════════════════════════════════════════════════════════
// VII. GOLDEN RATIO OPTIMIZATIONS
// ═══════════════════════════════════════════════════════════════════════════════

// Precomputed golden angle increments (for fast spiral iteration)
constexpr float GOLDEN_ANGLE = static_cast<float>(2.0 * PI * PHI_INV);
constexpr float GOLDEN_ANGLE_INV = static_cast<float>(1.0 / (2.0 * PI * PHI_INV));

/**
 * @brief Fast golden ratio modulation
 */
inline float golden_modulate(float x, uint32_t n) {
    return x * (1.0f + static_cast<float>(n) * static_cast<float>(PHI_INV) * 0.01f);
}

/**
 * @brief Golden angle rotation (complex multiply)
 */
inline void golden_rotate(float& re, float& im) {
    constexpr float cos_ga = static_cast<float>(std::cos(2.0 * PI * PHI_INV));
    constexpr float sin_ga = static_cast<float>(std::sin(2.0 * PI * PHI_INV));
    float new_re = re * cos_ga - im * sin_ga;
    float new_im = re * sin_ga + im * cos_ga;
    re = new_re;
    im = new_im;
}

// ═══════════════════════════════════════════════════════════════════════════════
// VIII. INFORMATION-THEORETIC FAST APPROXIMATIONS
// ═══════════════════════════════════════════════════════════════════════════════

/**
 * @brief Fast binary entropy H(p) = -p log p - (1-p) log(1-p)
 * Using polynomial approximation
 */
inline float fast_binary_entropy(float p) {
    if (p < 0.001f || p > 0.999f) return 0.0f;
    // Polynomial fit to binary entropy
    float q = 1.0f - p;
    float h = -p * fast_log(p) - q * fast_log(q);
    return h * 1.4426950408889634f;  // Convert to bits (log2)
}

/**
 * @brief Fast softmax for 4 elements
 */
inline Vec4 fast_softmax4(const Vec4& x) {
    float m = x.max();
    Vec4 e = {fast_exp(x.x - m), fast_exp(x.y - m), fast_exp(x.z - m), fast_exp(x.w - m)};
    float s = fast_rcp(e.sum());
    return e * s;
}

/**
 * @brief Fast KL divergence between two distributions (4 elements)
 */
inline float fast_kl4(const Vec4& p, const Vec4& q) {
    float kl = 0.0f;
    if (p.x > 0.001f && q.x > 0.001f) kl += p.x * (fast_log(p.x) - fast_log(q.x));
    if (p.y > 0.001f && q.y > 0.001f) kl += p.y * (fast_log(p.y) - fast_log(q.y));
    if (p.z > 0.001f && q.z > 0.001f) kl += p.z * (fast_log(p.z) - fast_log(q.z));
    if (p.w > 0.001f && q.w > 0.001f) kl += p.w * (fast_log(p.w) - fast_log(q.w));
    return kl;
}

} // namespace fast
} // namespace mind_farm