#include "twin.hpp"

#include <cmath>
#include <cstring>
#include <xmmintrin.h>

#include "rotor_kernel.hpp"

namespace detkernel {

// ---------------------------------------------------------------------------
// Generator FUN_180533ec0 (double), then cvtpd2ps packing as in the twin prologue.
// ---------------------------------------------------------------------------
twin_coeff build_twin_coeff(double fs_total, double freq, double q, float sens_lin) {
    float f1 = std::sqrt(sens_lin);
    if (f1 <= 0.0f) f1 = 0.0f;
    const double f1d = static_cast<double>(f1);

    const double w0 = (freq < 2.0 ? 2.0 : freq) * 6.283185307179586 / fs_total;
    const double s  = std::sin(w0);
    const double c2 = std::cos(w0) * -2.0;
    const double p  = (s * 0.5) / q;

    const double a0 = 1.0 + p * f1d;
    const double a2 = 1.0 - p * f1d;
    const double b0 = 1.0 + p / f1d;
    const double b2 = 1.0 - p / f1d;

    twin_coeff c;
    c.A[0] = static_cast<float>(a0);
    c.A[1] = static_cast<float>(c2);
    c.A[2] = static_cast<float>(a2);
    c.B[0] = static_cast<float>(b0);
    c.B[1] = static_cast<float>(c2);
    c.B[2] = static_cast<float>(b2);
    return c;
}

// ---------------------------------------------------------------------------
// 0x181a77520: complex division  num/den = (num*conj(den)) * ref
//   per-lane: shufps-0x88/0xdd -> |den|^2; mulps bith; rcpps + 1 Newton step.
// ---------------------------------------------------------------------------
void cplx_div_exact(const cplxf& num, const cplxf& den, cplxf& out) {
    const float ar = den.re, ai = den.im;
    const float br = num.re, bi = num.im;

    const float ar2 = ar * ar;
    const float ai2 = ai * ai;
    const float den2 = ar2 + ai2;

    const float re = ar * br + ai * bi;   // Re{num*conj(den)}
    const float im = ar * bi - ai * br;   // Im{num*conj(den)}

    if (den2 == 0.0f) {
        const float qnan = 0.0f / 0.0f;
        out.re = qnan;
        out.im = qnan;
        return;
    }

    const float r0  = _mm_cvtss_f32(_mm_rcp_ss(_mm_set_ss(den2)));
    const float t1  = den2 * r0;
    const float t2  = 2.0f - t1;
    const float ref = r0 * t2;

    out.re = re * ref;
    out.im = im * ref;
}

// ---------------------------------------------------------------------------
// 0x18000ad60 scalar complex multiply (body @0x18000ae00):
//   t0=bi*ai; t1=bi*ar; re=fma(br,ar,-t0); im=fma(br,ai,+t1)
// ---------------------------------------------------------------------------
void cplx_mul_exact(const cplxf& a, const cplxf& b, cplxf& out) {
    const float ar = a.re, ai = a.im;
    const float br = b.re, bi = b.im;

    const float t0 = bi * ai;   // mulps
    const float t1 = bi * ar;   // mulps
    out.re = fma_f(br, ar, -t0);  // vfmaddsub213ps lane0 (subtract)
    out.im = fma_f(br, ai, t1);   // lane1 (add)
}

// ---------------------------------------------------------------------------
// 0x180535880 hot loop: seed A/B into acc, 2 Horner FMA stages, cplx-div, x2.
// `z` carries exp(+i*theta_k); the twin conjugates before use (0x1800018b0).
// ---------------------------------------------------------------------------
void twin_apply(const twin_coeff& c, const cplxf* z, size_t n, cplxf* out) {
    for (size_t i = 0; i < n; ++i) {
        // conj(z1) = conj(exp(+i*theta))  (0x1800018b0 negates imag)
        const cplxf z1 = { z[i].re, -z[i].im };
        cplxf z2;
        cplx_mul_exact(z1, z1, z2);   // conj(z1)^2

        // A accumulator (vfmadd213ss per scalar lane)
        cplxf accA = { c.A[0], 0.0f };
        accA.re = fma_f(c.A[1], z1.re, accA.re);
        accA.im = fma_f(c.A[1], z1.im, accA.im);
        accA.re = fma_f(c.A[2], z2.re, accA.re);
        accA.im = fma_f(c.A[2], z2.im, accA.im);

        // B accumulator
        cplxf accB = { c.B[0], 0.0f };
        accB.re = fma_f(c.B[1], z1.re, accB.re);
        accB.im = fma_f(c.B[1], z1.im, accB.im);
        accB.re = fma_f(c.B[2], z2.re, accB.re);
        accB.im = fma_f(c.B[2], z2.im, accB.im);

        cplx_div_exact(accB, accA, out[i]);   // B/A
        out[i].re *= 2.0f;                    // 2*B/A (caller-side scale, exact)
        out[i].im *= 2.0f;
    }
}

}  // namespace detkernel