#pragma once
// Scalar twiddle loader — FUN_18014ec20 transcription
// Computes cos/sin for each input float angle, writing doubles to out_re, out_im
// Based on constants extracted from soothe2_x64.vst3 memory dump
#include <cmath>
#include <cstdint>

namespace soothe {

// Magic number for fast floor: 2^52 + 2^51
static constexpr double MAGIC_FLOOR = 6.755399441055744e+15;

// Cody-Waite 4-level range reduction constants
static constexpr double PI_HI  = 3.1415925025939941;
static constexpr double PI_MD1 = 1.5099578831723193e-07;
static constexpr double PI_MD2 = 1.078060505991553e-14;
static constexpr double PI_LO  = 6.564007085747001e-22;

// Sin polynomial coefficients (minimax, 7 terms)
// sin(t) = t + t²·(c0 + t²·(c1 + t²·(c2 + t²·(c3 + t²·(c4 + t²·(c5 + t²·c6)))))·t
static constexpr double SIN_C0 = -1.666666666666618e-01;  // ≈ -1/6
static constexpr double SIN_C1 =  8.333333333285186e-03;  // ≈  1/120
static constexpr double SIN_C2 = -1.984126982494424e-04;  // ≈ -1/5040
static constexpr double SIN_C3 =  2.755731658449074e-06;  // ≈  1/362880
static constexpr double SIN_C4 = -2.505187912674299e-08;  // ≈ -1/39916800
static constexpr double SIN_C5 =  1.604805557697937e-10;  // ≈  1/6227020800
static constexpr double SIN_C6 = -7.372809097265070e-13;  // ≈ -1/1307674368000

// Fast floor using magic number
static inline double fast_floor(double x) {
    double y = x + MAGIC_FLOOR;
    double f = y - MAGIC_FLOOR;
    if (f > x) f -= 1.0;
    return f;
}

// Cody-Waite sin/cos for a single double-precision angle
inline void sincos_double(double x, double& out_cos, double& out_sin) {
    // Cody-Waite 4-level range reduction: reduce to |t| ≤ π/2
    double fn = fast_floor(x * (1.0 / PI_HI) + 0.5);
    double r1 = x - fn * PI_HI;
    double r2 = r1 - fn * PI_MD1;
    double r3 = r2 - fn * PI_MD2;
    double t  = r3 - fn * PI_LO;

    // sin(t) = t + t²·P(t²)·t where P is the 7-term minimax polynomial
    double t2 = t * t;
    double p = SIN_C6;
    p = p * t2 + SIN_C5;
    p = p * t2 + SIN_C4;
    p = p * t2 + SIN_C3;
    p = p * t2 + SIN_C2;
    p = p * t2 + SIN_C1;
    p = p * t2 + SIN_C0;
    double sin_t = t + t * t2 * p;

    // cos(t) = 1 + t²·(-1/2 + t²·(1/24 + t²·(-1/720)))
    double cos_t = 1.0 + t2 * (-1.0/2.0 + t2 * (1.0/24.0 + t2 * (-1.0/720.0)));

    // Quadrant correction: fn mod 4
    // The reduction is x = fn·π + t, |t| ≤ π/2
    // fn even: sin(x)=sin(t), cos(x)=cos(t)
    // fn odd:  sin(x)=-sin(t), cos(x)=-cos(t)
    int q = static_cast<int>(fn) & 3;
    switch (q) {
        case 0: out_cos =  cos_t; out_sin =  sin_t; break;
        case 1: out_cos = -cos_t; out_sin = -sin_t; break;
        case 2: out_cos =  cos_t; out_sin =  sin_t; break;
        case 3: out_cos = -cos_t; out_sin = -sin_t; break;
    }
}

// Twiddle loader: compute cos/sin for an array of float angles
void twiddle_load(const float* angles, double* out_re, double* out_im, uint32_t count) {
    for (uint32_t i = 0; i < count; i++) {
        sincos_double(static_cast<double>(angles[i]), out_re[i], out_im[i]);
    }
}

// Version that takes double angles directly
void twiddle_load_d(const double* angles, double* out_re, double* out_im, uint32_t count) {
    for (uint32_t i = 0; i < count; i++) {
        sincos_double(angles[i], out_re[i], out_im[i]);
    }
}

}  // namespace soothe