polydf 0.1.1

// Copyright (c) 2025 William Bradley
//! polydf: Distance queries for parametric curves (2D and 3D).
//!
//! - 2D curves: nearest point, unsigned and signed distance (using outward normal).
//! - 3D curves: nearest point and unsigned distance (no signed distance for space curves).
//!
//! Example (3D helix):
//!
//! ```no_run
//! use nalgebra::{Vector3, vector};
//! use polydf::{NearestOptions, nearest_t_3d, distance_to_curve_3d};
//!
//! fn helix(t: f32) -> Vector3<f32> {
//!     vector![t.cos(), t.sin(), 0.5 * t]
//! }
//!
//! let p = vector![2.0, 0.0, 0.0];
//! let t_range = -10.0f32..=10.0f32;
//! let opts = NearestOptions::default();
//! let res = nearest_t_3d(p, helix, t_range.clone(), opts);
//! println!("t* = {} distance = {}", res.t, res.distance);
//! let d = distance_to_curve_3d(p, helix, t_range, opts);
//! ```
//!
//! This crate provides routines to find the nearest parameter `t` and compute
//! the (optionally signed) distance from an arbitrary 2D point `p` to a
//! differentiable parametric curve `C(t): f32 -> (f32, f32)`.
//!
//! Key features:
//! - Works with any differentiable 2D parametric function (closures supported).
//! - Returns the nearest parameter `t`, closest point, and (signed) distance.
//! - Optional thresholded query with a conservative early-out if the point is
//!   provably farther than the threshold (requires a speed bound).
//!
//! Notes on signed distance:
//! - The sign is defined using the outward normal estimated from the curve
//!   tangent at the closest point. We take the outward normal as the tangent
//!   rotated by -90 degrees, and define `signed_distance = (p - C(t*)) · n`.
//!   For CCW-oriented closed curves, points outside are positive and inside are
//!   negative. For other orientations, the sign will be consistent but may flip
//!   depending on your curve's direction.

use core::ops::RangeInclusive;

use nalgebra::{Vector2, Vector3, vector};

/// Result of a nearest-point query on a parametric curve.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct NearestResult {
    /// The minimizing parameter value.
    pub t: f32,
    /// The closest point on the curve `C(t)`.
    pub point: Vector2<f32>,
    /// Unsigned Euclidean distance `|C(t) - p|`.
    pub distance: f32,
    /// Signed distance using outward normal (tangent rotated by -90 degrees).
    pub signed_distance: f32,
}

/// Result of a nearest-point query on a 3D parametric curve.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct NearestResult3D {
    /// The minimizing parameter value.
    pub t: f32,
    /// The closest point on the curve `C(t)`.
    pub point: Vector3<f32>,
    /// Unsigned Euclidean distance `|C(t) - p|`.
    pub distance: f32,
}

/// Options controlling the search behavior.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct NearestOptions {
    /// Number of uniform samples used to seed local minimizations.
    /// More samples increases robustness for highly non-unimodal curves.
    pub samples: usize,
    /// Tolerance on parameter `t` for local minimization.
    pub tol_t: f32,
    /// Tolerance on distance change for convergence (world units).
    /// If > 0, local minimization may stop early when improvement in
    /// distance is below this threshold.
    pub tol_d: f32,
    /// Maximum iterations for local minimization.
    pub max_iter: usize,
}

impl Default for NearestOptions {
    fn default() -> Self {
        Self {
            samples: 64,
            tol_t: 1e-4,
            tol_d: 1e-5,
            max_iter: 80,
        }
    }
}

/// Attempt to quickly reject queries that are provably farther than `threshold`.
///
/// This uses a conservative bound that requires a known upper bound `L` on the
/// curve speed `|C'(t)|` over the given range. If the bound concludes the
/// point must be beyond `threshold` for all `t`, returns `true` (safe reject).
/// If `L` is `None`, or the bound is inconclusive, returns `false`.
pub fn is_definitely_far<F>(
    p: Vector2<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    samples: usize,
) -> bool
where
    F: Fn(f32) -> Vector2<f32> + Copy,
{
    let l_opt = speed_upper_bound;
    let Some(l) = l_opt else { return false };
    if !t_range.start().is_finite() || !t_range.end().is_finite() {
        return false;
    }
    let a = *t_range.start();
    let b = *t_range.end();
    if !(a < b) {
        return false;
    }

    let n = samples.max(2);
    let dt = (b - a) / (n as f32 - 1.0);
    let mut prev_t = a;
    let mut prev_d = (f(prev_t) - p).norm();
    for i in 1..n {
        let t = a + (i as f32) * dt;
        let d = (f(t) - p).norm();

        // Conservative lower bound on distance over [prev_t, t]
        // Using: for any s in [prev_t, t], |C(s) - p| >= min(|C(prev_t)-p|, |C(t)-p|) - L * (t - prev_t)
        let lb = (prev_d.min(d) - l * (t - prev_t)).max(0.0);
        if lb <= threshold {
            // Cannot safely reject — interval could contain a point within threshold.
            return false;
        }

        prev_t = t;
        prev_d = d;
    }

    // All intervals have lower bounds above threshold => safe reject.
    true
}

/// 3D variant: attempt to quickly reject queries that are provably farther
/// than `threshold`, given an upper bound on the curve speed.
pub fn is_definitely_far_3d<F>(
    p: Vector3<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    samples: usize,
) -> bool
where
    F: Fn(f32) -> Vector3<f32> + Copy,
{
    let l_opt = speed_upper_bound;
    let Some(l) = l_opt else { return false };
    if !t_range.start().is_finite() || !t_range.end().is_finite() {
        return false;
    }
    let a = *t_range.start();
    let b = *t_range.end();
    if !(a < b) {
        return false;
    }

    let n = samples.max(2);
    let dt = (b - a) / (n as f32 - 1.0);
    let mut prev_t = a;
    let mut prev_d = (f(prev_t) - p).norm();
    for i in 1..n {
        let t = a + (i as f32) * dt;
        let d = (f(t) - p).norm();

        // Conservative lower bound on distance over [prev_t, t]
        let lb = (prev_d.min(d) - l * (t - prev_t)).max(0.0);
        if lb <= threshold {
            return false;
        }

        prev_t = t;
        prev_d = d;
    }
    true
}

/// Find nearest parameter `t` to point `p` on curve `f(t)` over `t_range`.
///
/// - Uses multi-start local minimization of the squared distance seeded by
///   uniform sampling. This is robust for most curves without requiring
///   derivatives. If you have an analytic derivative, consider
///   [`nearest_t_with_derivative`] for potentially faster convergence.
/// - Returns [`NearestResult`] with the closest point and (signed) distance.
pub fn nearest_t<F>(
    p: Vector2<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult
where
    F: Fn(f32) -> Vector2<f32> + Copy,
{
    nearest_t_impl(p, f, None::<fn(f32) -> Vector2<f32>>, t_range, options)
}

/// Find nearest parameter `t` to point `p` on a 3D curve `f(t)` over `t_range`.
/// Returns the nearest parameter and unsigned distance.
pub fn nearest_t_3d<F>(
    p: Vector3<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult3D
where
    F: Fn(f32) -> Vector3<f32> + Copy,
{
    nearest_t_impl_3d(p, f, t_range, options)
}

/// Convenience wrapper for curves defined as `Fn(f32) -> (f32, f32)` and points as tuples.
pub fn nearest_t_tuple<F>(
    p: (f32, f32),
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult
where
    F: Fn(f32) -> (f32, f32) + Copy,
{
    let p = vector![p.0, p.1];
    let fv = move |t: f32| -> Vector2<f32> {
        let xy = f(t);
        vector![xy.0, xy.1]
    };
    nearest_t(p, fv, t_range, options)
}

/// Convenience wrapper for 3D curves defined as `Fn(f32) -> (f32, f32, f32)`
/// and points as tuples.
pub fn nearest_t_tuple_3d<F>(
    p: (f32, f32, f32),
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult3D
where
    F: Fn(f32) -> (f32, f32, f32) + Copy,
{
    let p = vector![p.0, p.1, p.2];
    let fv = move |t: f32| -> Vector3<f32> {
        let xyz = f(t);
        vector![xyz.0, xyz.1, xyz.2]
    };
    nearest_t_3d(p, fv, t_range, options)
}

/// Find nearest parameter `t` using both the curve `f` and its derivative `df`.
///
/// Providing `df` can improve the signed distance stability and speed.
pub fn nearest_t_with_derivative<F, D>(
    p: Vector2<f32>,
    f: F,
    df: D,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult
where
    F: Fn(f32) -> Vector2<f32> + Copy,
    D: Fn(f32) -> Vector2<f32> + Copy,
{
    nearest_t_impl(p, f, Some(df), t_range, options)
}

/// Convenience wrapper for tuple-based curve and derivative.
pub fn nearest_t_with_derivative_tuple<F, D>(
    p: (f32, f32),
    f: F,
    df: D,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult
where
    F: Fn(f32) -> (f32, f32) + Copy,
    D: Fn(f32) -> (f32, f32) + Copy,
{
    let p = vector![p.0, p.1];
    let fv = move |t: f32| -> Vector2<f32> {
        let xy = f(t);
        vector![xy.0, xy.1]
    };
    let dfv = move |t: f32| -> Vector2<f32> {
        let xy = df(t);
        vector![xy.0, xy.1]
    };
    nearest_t_with_derivative(p, fv, dfv, t_range, options)
}

/// Thresholded query: if `speed_upper_bound` is provided and the point is
/// provably farther than `threshold`, returns `None` quickly. Otherwise falls
/// back to a full nearest search and returns the result.
pub fn nearest_t_within<F>(
    p: Vector2<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    options: NearestOptions,
) -> Option<NearestResult>
where
    F: Fn(f32) -> Vector2<f32> + Copy,
{
    if is_definitely_far(
        p,
        f,
        t_range.clone(),
        threshold,
        speed_upper_bound,
        options.samples,
    ) {
        return None;
    }
    Some(nearest_t(p, f, t_range, options))
}

/// 3D variant of thresholded query. If `speed_upper_bound` is provided and the
/// point is provably farther than `threshold`, returns `None`. Otherwise runs a
/// full nearest search and returns the result.
pub fn nearest_t_within_3d<F>(
    p: Vector3<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    options: NearestOptions,
) -> Option<NearestResult3D>
where
    F: Fn(f32) -> Vector3<f32> + Copy,
{
    if is_definitely_far_3d(
        p,
        f,
        t_range.clone(),
        threshold,
        speed_upper_bound,
        options.samples,
    ) {
        return None;
    }
    Some(nearest_t_3d(p, f, t_range, options))
}

/// Return the unsigned distance from point `p` to the curve `f(t)` over `t_range`.
pub fn distance_to_curve<F>(
    p: Vector2<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> f32
where
    F: Fn(f32) -> Vector2<f32> + Copy,
{
    nearest_t(p, f, t_range, options).distance
}

/// 3D: Return the unsigned distance from point `p` to the curve `f(t)`.
pub fn distance_to_curve_3d<F>(
    p: Vector3<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> f32
where
    F: Fn(f32) -> Vector3<f32> + Copy,
{
    nearest_t_3d(p, f, t_range, options).distance
}

/// Return `true` if the point is within `threshold` distance of the curve.
///
/// Uses the same conservative early-out as [`nearest_t_within`]. If the point is
/// provably farther, returns `false` without running the full search. Otherwise,
/// computes the true nearest distance and compares to `threshold`.
pub fn within_threshold<F>(
    p: Vector2<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    options: NearestOptions,
) -> bool
where
    F: Fn(f32) -> Vector2<f32> + Copy,
{
    if is_definitely_far(
        p,
        f,
        t_range.clone(),
        threshold,
        speed_upper_bound,
        options.samples,
    ) {
        return false;
    }
    let res = nearest_t(p, f, t_range, options);
    res.distance <= threshold
}

/// 3D: Return `true` if the point is within `threshold` distance of the curve.
pub fn within_threshold_3d<F>(
    p: Vector3<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    options: NearestOptions,
) -> bool
where
    F: Fn(f32) -> Vector3<f32> + Copy,
{
    if is_definitely_far_3d(
        p,
        f,
        t_range.clone(),
        threshold,
        speed_upper_bound,
        options.samples,
    ) {
        return false;
    }
    let res = nearest_t_3d(p, f, t_range, options);
    res.distance <= threshold
}

/// Convenience wrapper for thresholded query using tuple-based curve and point.
pub fn nearest_t_within_tuple<F>(
    p: (f32, f32),
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    options: NearestOptions,
) -> Option<NearestResult>
where
    F: Fn(f32) -> (f32, f32) + Copy,
{
    let p = vector![p.0, p.1];
    let fv = move |t: f32| -> Vector2<f32> {
        let xy = f(t);
        vector![xy.0, xy.1]
    };
    nearest_t_within(p, fv, t_range, threshold, speed_upper_bound, options)
}

/// Tuple-friendly unsigned distance from `(x, y)` to curve `f(t)` in `t_range`.
pub fn distance_to_curve_tuple<F>(
    p: (f32, f32),
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> f32
where
    F: Fn(f32) -> (f32, f32) + Copy,
{
    let p = vector![p.0, p.1];
    let fv = move |t: f32| -> Vector2<f32> {
        let xy = f(t);
        vector![xy.0, xy.1]
    };
    distance_to_curve(p, fv, t_range, options)
}

/// Tuple-friendly predicate: is `(x, y)` within `threshold` distance of curve `f`?
pub fn within_threshold_tuple<F>(
    p: (f32, f32),
    f: F,
    t_range: RangeInclusive<f32>,
    threshold: f32,
    speed_upper_bound: Option<f32>,
    options: NearestOptions,
) -> bool
where
    F: Fn(f32) -> (f32, f32) + Copy,
{
    let p = vector![p.0, p.1];
    let fv = move |t: f32| -> Vector2<f32> {
        let xy = f(t);
        vector![xy.0, xy.1]
    };
    within_threshold(p, fv, t_range, threshold, speed_upper_bound, options)
}

fn nearest_t_impl<F, DOption>(
    p: Vector2<f32>,
    f: F,
    df_opt: Option<DOption>,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult
where
    F: Fn(f32) -> Vector2<f32> + Copy,
    DOption: Fn(f32) -> Vector2<f32> + Copy,
{
    assert!(
        t_range.start().is_finite() && t_range.end().is_finite(),
        "t_range must be finite"
    );
    let a = *t_range.start();
    let b = *t_range.end();
    assert!(a < b, "t_range must satisfy start < end");

    // Distance squared function
    let d2 = |t: f32| -> f32 {
        let q = f(t) - p;
        q.dot(&q)
    };

    // Seed via uniform samples
    let n = options.samples.max(8);
    let dt = (b - a) / (n as f32 - 1.0);
    let mut best_t = a;
    let mut best_d2 = d2(a);
    let mut prev_d = (f(a) - p).norm();

    // Track candidate brackets [l, r] around sampled local dips
    let mut brackets: Vec<(f32, f32)> = Vec::with_capacity(128);

    for i in 1..n {
        let t = a + (i as f32) * dt;
        let v = f(t);
        let dist = (v - p).norm();
        let cur_d2 = dist * dist;
        if cur_d2 < best_d2 {
            best_d2 = cur_d2;
            best_t = t;
        }

        // Detect a dip by comparing with neighbors (simple heuristic)
        if i >= 2 {
            let t_left = t - dt;
            let d_left = (f(t_left) - p).norm();
            if dist <= d_left && dist <= prev_d {
                // Place a small bracket around local dip
                let l = (t - dt).max(a);
                let r = (t + dt).min(b);
                brackets.push((l, r));
            }
        }

        prev_d = dist;
    }

    // If we didn't find any brackets (e.g., monotonic distance), create a few.
    if brackets.is_empty() {
        let thirds = [
            (a, a + (b - a) / 3.0),
            (a + (b - a) / 3.0, a + 2.0 * (b - a) / 3.0),
            (a + 2.0 * (b - a) / 3.0, b),
        ];
        for (l, r) in thirds {
            brackets.push((l, r));
        }
    }

    // Locally minimize d2 on each bracket using a compact Brent-like search.
    for (l0, r0) in brackets.into_iter() {
        let (t_star, d2_star) =
            minimize_1d(d2, l0, r0, options.tol_t, options.tol_d, options.max_iter);
        if d2_star < best_d2 {
            best_d2 = d2_star;
            best_t = t_star;
        }
    }

    let closest = f(best_t);
    let distance = best_d2.sqrt();

    // Estimate signed distance using outward normal (tangent rotated by -90 degrees).
    let tangent = match df_opt {
        Some(df) => df(best_t),
        None => numerical_derivative(f, best_t),
    };
    let tnorm = tangent.norm();
    let signed_distance = if tnorm > 1e-8 {
        let n_outward = rotate_right_90(tangent) / tnorm; // unit outward normal
        (p - closest).dot(&n_outward)
    } else {
        0.0
    };

    NearestResult {
        t: best_t,
        point: closest,
        distance,
        signed_distance,
    }
}

fn nearest_t_impl_3d<F>(
    p: Vector3<f32>,
    f: F,
    t_range: RangeInclusive<f32>,
    options: NearestOptions,
) -> NearestResult3D
where
    F: Fn(f32) -> Vector3<f32> + Copy,
{
    assert!(
        t_range.start().is_finite() && t_range.end().is_finite(),
        "t_range must be finite"
    );
    let a = *t_range.start();
    let b = *t_range.end();
    assert!(a < b, "t_range must satisfy start < end");

    // Distance squared function
    let d2 = |t: f32| -> f32 {
        let q = f(t) - p;
        q.dot(&q)
    };

    // Seed via uniform samples
    let n = options.samples.max(8);
    let dt = (b - a) / (n as f32 - 1.0);
    let mut best_t = a;
    let mut best_d2 = d2(a);
    let mut prev_d = (f(a) - p).norm();

    // Track candidate brackets [l, r] around sampled local dips
    let mut brackets: Vec<(f32, f32)> = Vec::with_capacity(128);

    for i in 1..n {
        let t = a + (i as f32) * dt;
        let v = f(t);
        let dist = (v - p).norm();
        let cur_d2 = dist * dist;
        if cur_d2 < best_d2 {
            best_d2 = cur_d2;
            best_t = t;
        }

        // Detect a dip by comparing with neighbors (simple heuristic)
        if i >= 2 {
            let t_left = t - dt;
            let d_left = (f(t_left) - p).norm();
            if dist <= d_left && dist <= prev_d {
                // Place a small bracket around local dip
                let l = (t - dt).max(a);
                let r = (t + dt).min(b);
                brackets.push((l, r));
            }
        }

        prev_d = dist;
    }

    // If we didn't find any brackets (e.g., monotonic distance), create a few.
    if brackets.is_empty() {
        let thirds = [
            (a, a + (b - a) / 3.0),
            (a + (b - a) / 3.0, a + 2.0 * (b - a) / 3.0),
            (a + 2.0 * (b - a) / 3.0, b),
        ];
        for (l, r) in thirds {
            brackets.push((l, r));
        }
    }

    // Locally minimize d2 on each bracket using the same 1D method.
    for (l0, r0) in brackets.into_iter() {
        let (t_star, d2_star) =
            minimize_1d(d2, l0, r0, options.tol_t, options.tol_d, options.max_iter);
        if d2_star < best_d2 {
            best_d2 = d2_star;
            best_t = t_star;
        }
    }

    let closest = f(best_t);
    let distance = best_d2.sqrt();

    NearestResult3D {
        t: best_t,
        point: closest,
        distance,
    }
}

fn minimize_1d<F>(f: F, mut a: f32, mut c: f32, tol: f32, tol_d: f32, max_iter: usize) -> (f32, f32)
where
    F: Fn(f32) -> f32,
{
    // Brent's method (simplified, f32). We do not strictly require unimodality
    // but it improves convergence; we use small brackets around suspected dips.
    let phi = 0.5 * (3.0_f32.sqrt() - 1.0); // ~0.618
    let mut x = a + phi * (c - a);
    let mut w = x;
    let mut v = x;
    let mut fx = f(x);
    let mut fw = fx;
    let mut fv = fx;
    let mut d: f32 = 0.0;
    let mut e: f32 = 0.0;
    let mut best_fx = fx.min(fw.min(fv));
    let use_tol_d = tol_d > 0.0;
    let tol_d2 = tol_d * tol_d;
    let mut iters = 0usize;

    for _ in 0..max_iter {
        iters += 1;
        let m = 0.5 * (a + c);
        let tol1 = tol * x.abs() + 1e-8_f32;
        let tol2 = 2.0 * tol1;

        if (x - m).abs() <= tol2 - 0.5 * (c - a) {
            break;
        }

        let mut u;
        if e.abs() > tol1 {
            // Fit parabola through (v, fv), (w, fw), (x, fx)
            let r = (x - w) * (fx - fv);
            let q = (x - v) * (fx - fw);
            let p = (x - v) * q - (x - w) * r;
            let q = 2.0 * (q - r);
            let (p, q) = if q > 0.0 { (-p, q) } else { (p, -q) };
            let etemp = e;
            e = d;
            if p.abs() < 0.5 * q * etemp && p > q * (a - x) && p < q * (c - x) {
                // Parabolic step
                d = p / q;
                u = x + d;
                // Don't evaluate too close to boundaries
                if (u - a) < tol2 || (c - u) < tol2 {
                    d = if x < m { tol1 } else { -tol1 };
                }
            } else {
                // Golden-section
                e = if x < m { c - x } else { a - x };
                d = phi * e;
            }
        } else {
            // Golden-section
            e = if x < m { c - x } else { a - x };
            d = phi * e;
        }

        u = if d.abs() >= tol1 {
            x + d
        } else {
            x + if d > 0.0 { tol1 } else { -tol1 }
        };
        let fu = f(u);

        if fu <= fx {
            if u < x {
                c = x;
            } else {
                a = x;
            }
            v = w;
            fv = fw;
            w = x;
            fw = fx;
            x = u;
            fx = fu;
        } else {
            if u < x {
                a = u;
            } else {
                c = u;
            }
            if fu <= fw || w == x {
                v = w;
                fv = fw;
                w = u;
                fw = fu;
            } else if fu <= fv || v == x || v == w {
                v = u;
                fv = fu;
            }
        }

        // Early exit if improvement in distance is below tol_d (checked in squared space).
        if use_tol_d && iters >= 4 {
            let new_best = fx.min(fw.min(fv));
            let improvement = best_fx - new_best;
            best_fx = new_best;
            if improvement <= tol_d2 {
                break;
            }
        }
    }

    (x, fx)
}

fn numerical_derivative<F>(f: F, t: f32) -> Vector2<f32>
where
    F: Fn(f32) -> Vector2<f32> + Copy,
{
    // Central difference with adaptive step based on t scale.
    let h = (1e-3_f32 * (1.0 + t.abs())).max(1e-6);
    let f1 = f(t + h);
    let f0 = f(t - h);
    (f1 - f0) / (2.0 * h)
}

#[inline]
fn rotate_right_90(v: Vector2<f32>) -> Vector2<f32> {
    // Rotate by -90 degrees: (x, y) -> (y, -x)
    vector![v.y, -v.x]
}

#[cfg(test)]
mod tests {
    use core::f32::consts::PI;

    use super::*;

    fn circle(t: f32) -> Vector2<f32> {
        vector![t.cos(), t.sin()]
    }
    fn circle_d(t: f32) -> Vector2<f32> {
        vector![-t.sin(), t.cos()]
    }

    #[test]
    fn nearest_on_circle_basic() {
        let p = vector![1.5, 0.0];
        let res = nearest_t_with_derivative(
            p,
            circle,
            circle_d,
            0.0..=2.0 * PI,
            NearestOptions::default(),
        );
        // Nearest point should be near (1, 0) at t ~ 0 or 2π
        assert!(
            res.point.x > 0.999 && res.point.y.abs() < 1e-3,
            "closest point = {:?}",
            res.point
        );
        assert!(
            (res.distance - 0.5).abs() < 1e-3,
            "distance = {}",
            res.distance
        );
        // With CCW circle and outward normal (rotate right), outside is positive
        assert!(res.signed_distance > 0.0);
    }

    #[test]
    fn nearest_on_circle_inside_negative() {
        let p = vector![0.5, 0.0];
        let res = nearest_t_with_derivative(
            p,
            circle,
            circle_d,
            0.0..=2.0 * PI,
            NearestOptions::default(),
        );
        assert!((res.distance - 0.5).abs() < 1e-3);
        assert!(res.signed_distance < 0.0);
    }

    #[test]
    fn threshold_reject_with_speed_bound() {
        let p = vector![-3.0, 0.0];
        let opts = NearestOptions {
            samples: 32,
            ..Default::default()
        };
        // For unit circle, |C'(t)| = 1 for all t
        let rejected = is_definitely_far(p, circle, 0.0..=2.0 * PI, 0.2, Some(1.0), opts.samples);
        assert!(rejected);
        // The combined API should return None when thresholded
        let res = nearest_t_within(p, circle, 0.0..=2.0 * PI, 0.2, Some(1.0), opts);
        assert!(res.is_none());
    }

    // 3D tests
    fn line3d(t: f32) -> Vector3<f32> {
        // Line along x-axis
        vector![t, 0.0, 0.0]
    }

    fn helix(t: f32) -> Vector3<f32> {
        // Unit-radius helix
        vector![t.cos(), t.sin(), 0.5 * t]
    }

    #[test]
    fn nearest_on_line3d_basic() {
        let p = vector![0.0, 1.0, 2.0];
        let res = nearest_t_3d(p, line3d, -10.0..=10.0, NearestOptions::default());
        assert!((res.t - 0.0).abs() < 1e-3, "t = {}", res.t);
        assert!((res.distance - (1.0_f32.hypot(2.0))).abs() < 1e-3);
        assert!((res.point - vector![0.0, 0.0, 0.0]).norm() < 1e-3);
    }

    #[test]
    fn nearest_on_helix_far_point() {
        let p = vector![2.0, 0.0, 0.0];
        let res = nearest_t_3d(p, helix, -PI..=PI, NearestOptions::default());
        // Expect near t ~ 0 (point ~ [1,0,0]) with distance ~ 1
        assert!(res.t.abs() < 0.2, "t = {}", res.t);
        assert!((res.distance - 1.0).abs() < 0.1, "d = {}", res.distance);
    }

    #[test]
    fn threshold_reject_3d_with_speed_bound() {
        // For line3d, |C'(t)| = 1
        let p = vector![100.0, 100.0, 100.0];
        let opts = NearestOptions {
            samples: 16,
            ..Default::default()
        };
        let rejected = is_definitely_far_3d(p, line3d, -10.0..=10.0, 50.0, Some(1.0), opts.samples);
        assert!(rejected);
        let res = nearest_t_within_3d(p, line3d, -10.0..=10.0, 50.0, Some(1.0), opts);
        assert!(res.is_none());
    }
}