AI Flow: Reverse Engineering

//! Geometry: robust line fits, the photo-to-board homography, convex hulls, minimum-area rectangles and
//! polygon simplification.

pub type P = (f64, f64);

/// Fit `u = a * t + b` to (t, u) pairs, dropping outliers (residual over 3 x the median absolute
/// residual, floor `min_tol`) and refitting. Used for board edges, where a connector can hang over.
pub fn robust_line(pts: &[(f64, f64)], min_tol: f64) -> Option<(f64, f64)> {
    let fit = |p: &[(f64, f64)]| -> Option<(f64, f64)> {
        let n = p.len() as f64; if n < 2.0 { return None; }
        let (st, su) = p.iter().fold((0.0, 0.0), |a, q| (a.0 + q.0, a.1 + q.1));
        let (mt, mu) = (st / n, su / n);
        let (mut sxx, mut sxy) = (0.0, 0.0);
        for q in p { sxx += (q.0 - mt) * (q.0 - mt); sxy += (q.0 - mt) * (q.1 - mu); }
        if sxx < 1e-12 { return None; }
        let a = sxy / sxx; Some((a, mu - a * mt))
    };
    // consensus first (RANSAC over deterministic pairs): the edge with the most points within tolerance,
    // so a long overhanging connector cannot drag the fit; then refine on those inliers
    let mut cur: Vec<(f64, f64)> = pts.to_vec();
    if pts.len() >= 10 {
        let mut seed = 0x9e3779b97f4a7c15u64;
        let mut rnd = |n: usize| { seed ^= seed << 13; seed ^= seed >> 7; seed ^= seed << 17; (seed % n as u64) as usize };
        let mut best: (usize, f64, f64) = (0, 0.0, 0.0);
        for _ in 0..400 {
            let (i, j) = (rnd(pts.len()), rnd(pts.len()));
            let (p, q) = (pts[i], pts[j]);
            if (q.0 - p.0).abs() < 1e-9 || (q.0 - p.0).abs() < (pts.len() as f64).sqrt() { continue; }
            let a = (q.1 - p.1) / (q.0 - p.0); let b = p.1 - a * p.0;
            let n = pts.iter().filter(|r| (r.1 - (a * r.0 + b)).abs() <= min_tol).count();
            if n > best.0 { best = (n, a, b); }
        }
        if best.0 >= 2 { cur = pts.iter().filter(|r| (r.1 - (best.1 * r.0 + best.2)).abs() <= min_tol * 1.5).copied().collect(); }
    }
    let mut line = fit(&cur)?;
    for _ in 0..6 {
        let mut res: Vec<f64> = cur.iter().map(|q| (q.1 - (line.0 * q.0 + line.1)).abs()).collect();
        let mut s = res.clone(); s.sort_by(|a, b| a.partial_cmp(b).unwrap());
        let mad = s[s.len() / 2];
        let tol = (3.0 * mad).max(min_tol);
        let keep: Vec<(f64, f64)> = cur.iter().zip(res.drain(..)).filter(|(_, r)| *r <= tol).map(|(q, _)| *q).collect();
        if keep.len() == cur.len() || keep.len() < 2 { break; }
        cur = keep; line = fit(&cur)?;
    }
    Some(line)
}

/// 3x3 homography (row-major, h[8] = 1) mapping src[i] -> dst[i] for four point pairs.
pub fn homography(src: &[P; 4], dst: &[P; 4]) -> Option<[f64; 9]> {
    let mut a = [[0f64; 9]; 8];
    for i in 0..4 {
        let ((x, y), (u, v)) = (src[i], dst[i]);
        a[2 * i] = [x, y, 1.0, 0.0, 0.0, 0.0, -u * x, -u * y, u];
        a[2 * i + 1] = [0.0, 0.0, 0.0, x, y, 1.0, -v * x, -v * y, v];
    }
    // Gaussian elimination with partial pivoting on the 8x8 system
    for c in 0..8 {
        let p = (c..8).max_by(|i, j| a[*i][c].abs().partial_cmp(&a[*j][c].abs()).unwrap())?;
        if a[p][c].abs() < 1e-12 { return None; }
        a.swap(c, p);
        for r in 0..8 {
            if r == c { continue; }
            let f = a[r][c] / a[c][c];
            for k in c..9 { a[r][k] -= f * a[c][k]; }
        }
    }
    let mut h = [0f64; 9];
    for i in 0..8 { h[i] = a[i][8] / a[i][i]; }
    h[8] = 1.0;
    Some(h)
}

pub fn apply(h: &[f64; 9], p: P) -> P {
    let w = h[6] * p.0 + h[7] * p.1 + h[8];
    ((h[0] * p.0 + h[1] * p.1 + h[2]) / w, (h[3] * p.0 + h[4] * p.1 + h[5]) / w)
}

pub fn invert(h: &[f64; 9]) -> Option<[f64; 9]> {
    let m = h;
    let det = m[0] * (m[4] * m[8] - m[5] * m[7]) - m[1] * (m[3] * m[8] - m[5] * m[6]) + m[2] * (m[3] * m[7] - m[4] * m[6]);
    if det.abs() < 1e-15 { return None; }
    let inv = [
        (m[4] * m[8] - m[5] * m[7]) / det, (m[2] * m[7] - m[1] * m[8]) / det, (m[1] * m[5] - m[2] * m[4]) / det,
        (m[5] * m[6] - m[3] * m[8]) / det, (m[0] * m[8] - m[2] * m[6]) / det, (m[2] * m[3] - m[0] * m[5]) / det,
        (m[3] * m[7] - m[4] * m[6]) / det, (m[1] * m[6] - m[0] * m[7]) / det, (m[0] * m[4] - m[1] * m[3]) / det,
    ];
    Some(inv.map(|v| v / inv[8]))
}

/// Intersection of x = a1*y + b1 (a near-vertical edge) and y = a2*x + b2 (a near-horizontal edge).
pub fn cross_vh(v: (f64, f64), hz: (f64, f64)) -> P {
    // x = a1*(a2*x + b2) + b1  =>  x (1 - a1 a2) = a1 b2 + b1
    let x = (v.0 * hz.1 + v.1) / (1.0 - v.0 * hz.0);
    (x, hz.0 * x + hz.1)
}

pub fn dist(a: P, b: P) -> f64 { ((a.0 - b.0).powi(2) + (a.1 - b.1).powi(2)).sqrt() }

/// Convex hull (monotone chain), counter-clockwise in y-down coordinates' mirror; order is consistent.
pub fn hull(pts: &[P]) -> Vec<P> {
    let mut p: Vec<P> = pts.to_vec();
    p.sort_by(|a, b| a.partial_cmp(b).unwrap());
    p.dedup();
    if p.len() < 3 { return p; }
    let cross = |o: P, a: P, b: P| (a.0 - o.0) * (b.1 - o.1) - (a.1 - o.1) * (b.0 - o.0);
    let mut lo: Vec<P> = vec![];
    for q in &p { while lo.len() >= 2 && cross(lo[lo.len() - 2], lo[lo.len() - 1], *q) <= 0.0 { lo.pop(); } lo.push(*q); }
    let mut up: Vec<P> = vec![];
    for q in p.iter().rev() { while up.len() >= 2 && cross(up[up.len() - 2], up[up.len() - 1], *q) <= 0.0 { up.pop(); } up.push(*q); }
    lo.pop(); up.pop(); lo.extend(up); lo
}

/// Minimum-area enclosing rectangle: (centre, long side, short side, angle of the long side in degrees, 0..180).
pub fn min_rect(pts: &[P]) -> (P, f64, f64, f64) {
    let h = hull(pts);
    if h.len() < 3 { let c = h.first().copied().unwrap_or((0.0, 0.0)); return (c, 0.0, 0.0, 0.0); }
    let mut best = (f64::MAX, (0.0, 0.0), 0.0, 0.0, 0.0);
    for i in 0..h.len() {
        let (a, b) = (h[i], h[(i + 1) % h.len()]);
        let ang = (b.1 - a.1).atan2(b.0 - a.0);
        let (c, s) = (ang.cos(), ang.sin());
        let (mut x0, mut x1, mut y0, mut y1) = (f64::MAX, f64::MIN, f64::MAX, f64::MIN);
        for q in &h { let u = q.0 * c + q.1 * s; let v = -q.0 * s + q.1 * c; x0 = x0.min(u); x1 = x1.max(u); y0 = y0.min(v); y1 = y1.max(v); }
        let area = (x1 - x0) * (y1 - y0);
        if area < best.0 {
            let (cu, cv) = ((x0 + x1) / 2.0, (y0 + y1) / 2.0);
            let centre = (cu * c - cv * s, cu * s + cv * c);
            let (w, hh) = (x1 - x0, y1 - y0);
            let (long, short, a) = if w >= hh { (w, hh, ang) } else { (hh, w, ang + std::f64::consts::FRAC_PI_2) };
            best = (area, centre, long, short, a.to_degrees().rem_euclid(180.0));
        }
    }
    (best.1, best.2, best.3, best.4)
}

/// Douglas-Peucker simplification of a closed ring.
pub fn simplify(ring: &[P], tol: f64) -> Vec<P> {
    if ring.len() < 4 { return ring.to_vec(); }
    fn rec(p: &[P], tol: f64, out: &mut Vec<P>) {
        let (a, b) = (p[0], p[p.len() - 1]);
        let (mut idx, mut dmax) = (0, 0.0);
        let l = dist(a, b).max(1e-9);
        for (i, q) in p.iter().enumerate().take(p.len() - 1).skip(1) {
            let d = ((b.0 - a.0) * (a.1 - q.1) - (a.0 - q.0) * (b.1 - a.1)).abs() / l;
            if d > dmax { dmax = d; idx = i; }
        }
        if dmax > tol { rec(&p[..=idx], tol, out); out.pop(); rec(&p[idx..], tol, out); } else { out.push(a); out.push(b); }
    }
    // split the ring at its farthest point from the start so both halves are open chains
    let far = (1..ring.len()).max_by(|i, j| dist(ring[0], ring[*i]).partial_cmp(&dist(ring[0], ring[*j])).unwrap()).unwrap();
    let mut a = vec![]; rec(&ring[..=far], tol, &mut a);
    let mut second: Vec<P> = ring[far..].to_vec(); second.push(ring[0]);
    let mut b = vec![]; rec(&second, tol, &mut b);
    a.pop(); b.pop(); a.extend(b); a
}

pub fn polygon_area(p: &[P]) -> f64 {
    let mut s = 0.0;
    for i in 0..p.len() { let (a, b) = (p[i], p[(i + 1) % p.len()]); s += a.0 * b.1 - b.0 * a.1; }
    s.abs() / 2.0
}

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn homography_round_trip() {
        let src = [(10.0, 12.0), (410.0, 5.0), (420.0, 330.0), (3.0, 320.0)];
        let dst = [(0.0, 0.0), (400.0, 0.0), (400.0, 320.0), (0.0, 320.0)];
        let h = homography(&src, &dst).unwrap();
        for i in 0..4 { let q = apply(&h, src[i]); assert!(dist(q, dst[i]) < 1e-6, "{q:?}"); }
        let hi = invert(&h).unwrap();
        assert!(dist(apply(&hi, (200.0, 160.0)), apply(&hi, apply(&h, apply(&hi, (200.0, 160.0))))) < 1e-6);
    }
    #[test]
    fn rect_and_line() {
        // a 30 x 10 rectangle turned 30 degrees
        let (c, s) = (30f64.to_radians().cos(), 30f64.to_radians().sin());
        let mut pts = vec![];
        for i in 0..=30 { for j in 0..=10 { let (x, y) = (i as f64 - 15.0, j as f64 - 5.0); pts.push((50.0 + x * c - y * s, 40.0 + x * s + y * c)); } }
        let (ctr, l, w, a) = min_rect(&pts);
        assert!((l - 30.0).abs() < 0.01 && (w - 10.0).abs() < 0.01 && (a - 30.0).abs() < 0.01 && dist(ctr, (50.0, 40.0)) < 0.01);
        let mut line: Vec<(f64, f64)> = (0..50).map(|t| (t as f64, 2.0 * t as f64 + 1.0)).collect();
        line.push((25.0, 500.0)); // an overhanging connector
        let (a, b) = robust_line(&line, 0.5).unwrap();
        assert!((a - 2.0).abs() < 1e-6 && (b - 1.0).abs() < 1e-6);
        let sq = vec![(0.0, 0.0), (5.0, 0.01), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)];
        assert_eq!(simplify(&sq, 0.1).len(), 4);
        assert_eq!(polygon_area(&simplify(&sq, 0.1)), 100.0);
    }
}