import { cubic_residual_times_derivative_accumulate } from "../linalg/cubic_residual_times_derivative_accumulate.js";
import { polynomial_cubic_horner_eval } from "../linalg/polynomial_cubic_horner_eval.js";
import { polynomial_real_roots_in_interval } from "../linalg/polynomial_real_roots_in_interval.js";
import { spline3_hermite_to_monomial } from "./spline3_hermite_to_monomial.js";

/*
 Dimension-agnostic curve-pair closest-approach / intersection enumerator
 (dim ≥ 2). This is the path the dispatcher uses for all dim ≥ 2: the former
 bespoke 2D Bezout specialization was removed because it enumerated interior
 *intersections* plus boundary minima only and so missed interior
 non-intersection closest approaches. Closest approach is the global minimum of

   Φ(s, t) = Σ_d (A_d(s) − B_d(t))²

 over the closed unit square [0, 1]². It can occur at:
   1. an interior critical point where ∇Φ = 0
   2. a boundary edge minimum (one parameter pinned at 0 or 1)
   3. a corner

 Interior critical points are roots of the bivariate gradient system

   F(s, t) = ½ ∂Φ/∂s = Σ_d (A_d(s) − B_d(t)) A_d′(s)         (bidegree (5, 3))
   G(s, t) = ½ ∂Φ/∂t = −Σ_d (A_d(s) − B_d(t)) B_d′(t)       (bidegree (3, 5))

 This module finds those interior critical points by 2D Newton started from a
 GRID_SIDE × GRID_SIDE grid in [0, 1]² — quadratically convergent once close,
 but reliant on the grid being dense enough to cover the basins of attraction
 of all real critical points (9×9 = 81 starts is dense enough for the
 geometric configurations seen in practice). A fully analytical alternative
 eliminates t through the 8×8 Sylvester resultant of F and G, yielding a
 univariate of degree ≤ 34 in s; it is exact and basin-independent but
 genuinely large code with delicate degree-34 root-finding numerics, so it is
 left as a possible future drop-in behind this same public API rather than
 implemented here.

 Boundary edges and corners are handled exactly: each edge reduces to a
 quintic-derivative root-finding (degree 5), and corners fall out as the
 endpoints tested inside each edge solve.
*/

const MAX_DIM = 16;

// Per-dim monomial coefficient buffers, one each for A and B. Length 4*MAX_DIM.
const _a_mono = new Float64Array(4 * MAX_DIM);
const _b_mono = new Float64Array(4 * MAX_DIM);

const _candidate_st = new Float64Array(2);
const _edge_fixed_point = new Float64Array(MAX_DIM);

const NEWTON_MAX_ITER = 32;
const NEWTON_GRADIENT_TOLERANCE = 1e-13;
const NEWTON_STEP_TOLERANCE = 1e-15;

const GRID_SIDE = 9; // 9x9 = 81 starting points

const EDGE_QUINTIC_LEN = 6;
const _edge_quintic = new Float64Array(EDGE_QUINTIC_LEN);
const _edge_roots = new Float64Array(5);

const CRITICAL_POINT_DEDUPE_TOLERANCE = 1e-7;
const _seen_s = new Float64Array(GRID_SIDE * GRID_SIDE + 8);
const _seen_t = new Float64Array(GRID_SIDE * GRID_SIDE + 8);

// The gradient system (F, G) = (½ ∂Φ/∂s, ½ ∂Φ/∂t) and the 2×2 Jacobian
// of that gradient are inlined directly into `newton_2d`'s hot loop — see
// the module preamble for the algebraic definitions of F, G, and J.

/**
 * 2D Newton on (F, G) from a starting (s, t). Writes the converged (s, t) into
 * `out` and returns true iff it converged inside [0, 1]² with small gradient.
 */
// Tolerance for declaring the 2x2 Jacobian singular during the inlined
// Newton solve. Matches `256 * FLT_EPSILON_64` used by
// solve_linear_system_GEPP_2x2 to keep behaviour identical on edge cases.
const NEWTON_SINGULAR_TOLERANCE = 256 * 1.1102230246251565e-16;

function newton_2d(dim, s_init, t_init, out) {
    let s = s_init;
    let t = t_init;

    // F, G at the current (s, t) — promoted out of the heap-resident
    // `_newton_FG` scratch into iteration-local doubles.
    let F = 0, G = 0;
    let fg_valid_at_current_st = false;

    for (let iter = 0; iter < NEWTON_MAX_ITER; iter++) {
        // ── fused F/G + Jacobian over all dimensions ──
        // A single per-iteration sweep over dims computes the gradient
        // (F, G) and the symmetric Hessian (Jss, J_cross, Jtt) from one
        // load of each cubic's 4 coefficients. Eagerly computing the
        // Hessian costs us one extra second-derivative + 3 accumulations
        // per dim on the iteration that ends up converging — cheap insurance
        // for skipping a duplicate cubic-eval sweep on every non-converging
        // iteration. J_st = J_ts so the symmetric cross term is computed
        // once into J_cross.
        F = 0;
        G = 0;
        let Jss = 0, J_cross = 0, Jtt = 0;

        for (let d = 0; d < dim; d++) {
            const base = 4 * d;
            const a0 = _a_mono[base];
            const a1 = _a_mono[base + 1];
            const a2 = _a_mono[base + 2];
            const a3 = _a_mono[base + 3];
            const b0 = _b_mono[base];
            const b1 = _b_mono[base + 1];
            const b2 = _b_mono[base + 2];
            const b3 = _b_mono[base + 3];

            const A_v  = a0 + s * (a1 + s * (a2 + s * a3));
            const A_p  = a1 + s * (2 * a2 + s * 3 * a3);
            const A_pp = 2 * a2 + 6 * a3 * s;
            const B_v  = b0 + t * (b1 + t * (b2 + t * b3));
            const B_p  = b1 + t * (2 * b2 + t * 3 * b3);
            const B_pp = 2 * b2 + 6 * b3 * t;

            const diff = A_v - B_v;
            F       += diff * A_p;
            G       -= diff * B_p;
            Jss     += A_p * A_p + diff * A_pp;
            J_cross += -A_p * B_p;
            Jtt     += B_p * B_p - diff * B_pp;
        }
        fg_valid_at_current_st = true;

        if (Math.abs(F) < NEWTON_GRADIENT_TOLERANCE && Math.abs(G) < NEWTON_GRADIENT_TOLERANCE) {
            break;
        }

        // ── inline solve_linear_system_GEPP_2x2 on column-major J ──
        //   J = [a00, a10; a01, a11] = [Jss, J_cross; J_cross, Jtt]; rhs = [F, G]
        // Partial pivoting on column 0, then forward eliminate, then back-sub.
        // The "exact zero" special cases handled by the full solver are skipped:
        // for a smooth residual Newton, exact zeros essentially never arise
        // (random + analytic inputs), and a singular Jacobian gets rejected
        // below the same way the full solver would.
        let a00 = Jss, a10 = J_cross, a01 = J_cross, a11 = Jtt;
        let b0 = F,   b1 = G;
        if (Math.abs(a10) > Math.abs(a00)) {
            // Row swap
            let tmp = a00; a00 = a10; a10 = tmp;
            tmp = a01; a01 = a11; a11 = tmp;
            tmp = b0;  b0 = b1;   b1 = tmp;
        }
        if (a00 === 0) {
            return false; // singular column 0
        }
        const f = -a10 / a00;
        a11 += a01 * f;
        b1  += b0  * f;
        if (Math.abs(a11) < NEWTON_SINGULAR_TOLERANCE) {
            return false;
        }
        const dt = b1 / a11;
        const ds = (b0 - a01 * dt) / a00;
        if (!Number.isFinite(ds) || !Number.isFinite(dt)) {
            return false;
        }

        s -= ds;
        t -= dt;

        // (s, t) has just changed → cached F, G no longer match.
        fg_valid_at_current_st = false;

        // Early bail: if (s, t) has wandered well outside [0, 1] the start is
        // diverging or homed in on a critical point of the gradient system
        // that lies far from the parameter square. Newton won't reel it back.
        if (s < -0.5 || s > 1.5 || t < -0.5 || t > 1.5) {
            return false;
        }

        if (Math.abs(ds) < NEWTON_STEP_TOLERANCE && Math.abs(dt) < NEWTON_STEP_TOLERANCE) {
            break;
        }
    }

    if (s < -1e-9 || s > 1 + 1e-9) return false;
    if (t < -1e-9 || t > 1 + 1e-9) return false;

    // Validate gradient at the final (s, t). When `fg_valid_at_current_st`
    // is true we exited the loop via the gradient-tolerance break and the
    // validation already passed (tolerance 1e-13 < 1e-6); otherwise we
    // exited via the step-tolerance break or maxed out — re-evaluate to
    // confirm.
    if (!fg_valid_at_current_st) {
        F = 0;
        G = 0;
        for (let d = 0; d < dim; d++) {
            const base = 4 * d;
            const a0 = _a_mono[base];
            const a1 = _a_mono[base + 1];
            const a2 = _a_mono[base + 2];
            const a3 = _a_mono[base + 3];
            const b0 = _b_mono[base];
            const b1 = _b_mono[base + 1];
            const b2 = _b_mono[base + 2];
            const b3 = _b_mono[base + 3];

            const A_v = a0 + s * (a1 + s * (a2 + s * a3));
            const A_p = a1 + s * (2 * a2 + s * 3 * a3);
            const B_v = b0 + t * (b1 + t * (b2 + t * b3));
            const B_p = b1 + t * (2 * b2 + t * 3 * b3);

            const diff = A_v - B_v;
            F += diff * A_p;
            G -= diff * B_p;
        }
        if (Math.abs(F) > 1e-6 || Math.abs(G) > 1e-6) return false;
    }

    out[0] = s < 0 ? 0 : (s > 1 ? 1 : s);
    out[1] = t < 0 ? 0 : (t > 1 ? 1 : t);
    return true;
}

// ── boundary edge: nearest point on (varying-axis) curve to a fixed N-D point ──

/**
 * Minimise ‖(curve_mono...)(t) − point[]‖² over t ∈ [0, 1], where the curve
 * is `dim`-dimensional with monomial coefficients packed as
 * `[α0_0, α1_0, α2_0, α3_0, α0_1, α1_1, ..., α3_{dim-1}]`.
 *
 * The squared distance is a degree-6 polynomial in t; its derivative is a
 * quintic. We hand the quintic to the polynomial root finder, then test the
 * roots plus the endpoints t = 0 and t = 1.
 *
 * Writes `[t_min, dist²]` into `out` (length ≥ 2).
 */
function nearest_t_to_fixed_point(curve_mono, dim, point, point_offset, out) {
    for (let i = 0; i < EDGE_QUINTIC_LEN; i++){
        _edge_quintic[i] = 0;
    }

    for (let d = 0; d < dim; d++) {
        cubic_residual_times_derivative_accumulate(
            _edge_quintic, curve_mono, 4 * d, point[point_offset + d]
        );
    }

    // Aberth iteration cap of 32 is plenty for the degree-5 quintic that
    // comes out of `(p(t) - offset)·p'(t)` on a smooth Hermite cubic — the
    // routine's own convergence check still fires early in the common case.
    // Default cap (80) is way more than needed and is the worst-case work
    // budget; lowering it is a flat ~10-20% saving on degenerate inputs.
    const root_count = polynomial_real_roots_in_interval(
        _edge_quintic, 5, 0, 1, _edge_roots, 0,
        32
    );

    let best_t = 0;
    let best_d2 = Number.POSITIVE_INFINITY;

    for (let i = -2; i < root_count; i++) {

        const t = i === -2 ? 0 : (i === -1 ? 1 : _edge_roots[i]);
        let d2 = 0;

        for (let d = 0; d < dim; d++) {
            const base = 4 * d;
            const v = polynomial_cubic_horner_eval(curve_mono, base, t);
            const dv = v - point[point_offset + d];
            d2 += dv * dv;
        }

        if (d2 < best_d2) {
            best_d2 = d2;
            best_t = t;
        }

    }

    out[0] = best_t;
    out[1] = best_d2;
}

// ── public ────────────────────────────────────────────────────────────────

/**
 * Upper bound on the number of (s, t) pairs the ND variant can return:
 * one per starting point of the GRID_SIDE × GRID_SIDE grid Newton, plus the
 * four boundary edges, plus a small slack. The corresponding result-buffer
 * size in floats is `2 * ND_MAX_ROOTS`.
 *
 * @type {number}
 */
export const ND_MAX_ROOTS = GRID_SIDE * GRID_SIDE + 8;

/**
 * Critical-point enumerator for the dimension-agnostic Hermite curve-pair
 * intersection / closest-approach problem (dim ≥ 2; the dispatcher routes all
 * dim ≥ 2 here). Writes (s, t) pairs sequentially into `result` starting at
 * `result_offset` and returns the count.
 *
 * What is reported (deduplicated within
 * CRITICAL_POINT_DEDUPE_TOLERANCE = 1e-7 in each parameter):
 *   - Interior critical points of squared distance found by 2D Newton
 *     started from a GRID_SIDE × GRID_SIDE grid in [0,1]² and accepted
 *     when they converge inside [0,1]² with small gradient.
 *   - Four boundary closest-approach pairs, one per edge of [0,1]², each
 *     coming from a quintic root-find of (curve − fixed point)·curve′.
 *
 * Caller is responsible for evaluating both curves at each (s, t) and
 * picking whichever pair(s) they care about (closest, within threshold,
 * etc.).
 *
 * Required buffer size:
 *   `result.length >= result_offset + 2 * ND_MAX_ROOTS` floats (currently 178).
 *
 * @param {Float64Array|number[]} a length 4*dim
 * @param {Float64Array|number[]} b length 4*dim
 * @param {number} dim ≥ 2
 * @param {Float64Array|number[]} result length >= result_offset + 2 * ND_MAX_ROOTS
 * @param {number} result_offset
 * @returns {number} number of (s, t) pairs written
 */
export function spline3_hermite_intersection_spline3_hermite_nd(
    a, b, dim,
    result, result_offset
) {
    if (dim > MAX_DIM) {
        throw new Error(`dim=${dim} exceeds MAX_DIM=${MAX_DIM}`);
    }

    // Pack monomial coefficients per-axis into the module buffers.
    for (let d = 0; d < dim; d++) {
        const off = 4 * d;
        spline3_hermite_to_monomial(_a_mono, off, 1, a[off], a[off + 1], a[off + 2], a[off + 3]);
        spline3_hermite_to_monomial(_b_mono, off, 1, b[off], b[off + 1], b[off + 2], b[off + 3]);
    }

    // We dedupe near-identical critical points discovered from different grid
    // starts. _seen_s / _seen_t double as the "candidates written so far" and
    // are mirrored into the caller's `result` buffer as we go.
    let count = 0;
    let write = result_offset;

    const try_candidate = (s, t) => {
        for (let i = 0; i < count; i++) {
            if (Math.abs(_seen_s[i] - s) < CRITICAL_POINT_DEDUPE_TOLERANCE
                && Math.abs(_seen_t[i] - t) < CRITICAL_POINT_DEDUPE_TOLERANCE) {
                return;
            }
        }
        _seen_s[count] = s;
        _seen_t[count] = t;
        result[write++] = s;
        result[write++] = t;
        count++;
    };

    // Interior critical points via Newton from a grid.
    const step = 1 / (GRID_SIDE - 1);
    for (let i = 0; i < GRID_SIDE; i++) {
        const s_init = i * step;
        for (let j = 0; j < GRID_SIDE; j++) {
            const t_init = j * step;
            if (newton_2d(dim, s_init, t_init, _candidate_st)) {
                try_candidate(_candidate_st[0], _candidate_st[1]);
            }
        }
    }

    // Boundary edges. Each edge problem reduces to "nearest point on the
    // *other* curve to a fixed point on the boundary curve".
    const tmp_pt = _edge_fixed_point;

    // Edge s = 0: fixed point is A(0); minimise over t on B.
    for (let d = 0; d < dim; d++){
        tmp_pt[d] = _a_mono[4 * d]; // α0
    }
    nearest_t_to_fixed_point(_b_mono, dim, tmp_pt, 0, _candidate_st);
    try_candidate(0, _candidate_st[0]);

    // Edge s = 1: A(1) = α0 + α1 + α2 + α3.
    for (let d = 0; d < dim; d++) {
        const base = 4 * d;
        tmp_pt[d] = _a_mono[base] + _a_mono[base + 1] + _a_mono[base + 2] + _a_mono[base + 3];
    }
    nearest_t_to_fixed_point(_b_mono, dim, tmp_pt, 0, _candidate_st);
    try_candidate(1, _candidate_st[0]);

    // Edge t = 0: fixed point is B(0); minimise over s on A.
    for (let d = 0; d < dim; d++){
        tmp_pt[d] = _b_mono[4 * d];
    }
    nearest_t_to_fixed_point(_a_mono, dim, tmp_pt, 0, _candidate_st);
    try_candidate(_candidate_st[0], 0);

    // Edge t = 1.
    for (let d = 0; d < dim; d++) {
        const base = 4 * d;
        tmp_pt[d] = _b_mono[base] + _b_mono[base + 1] + _b_mono[base + 2] + _b_mono[base + 3];
    }
    nearest_t_to_fixed_point(_a_mono, dim, tmp_pt, 0, _candidate_st);
    try_candidate(_candidate_st[0], 1);

    return count;
}
