//! Small geometric solves for applying tracks: a 2-point similarity (Transform track rotation and
//! scale), a 3-point affine (Affine Corner Pin) and a 4-point homography (Perspective Corner Pin,
//! the direct linear transform with `h33 = 1`, Hartley & Zisserman §4.1).

/// Solve `a x = b` (n × n, row-major) by Gaussian elimination with partial pivoting.
pub fn solve_linear<const N: usize>(mut a: [[f64; N]; N], mut b: [f64; N]) -> Option<[f64; N]> {
    for col in 0..N {
        let piv = (col..N).max_by(|&i, &j| a[i][col].abs().total_cmp(&a[j][col].abs()))?;
        if a[piv][col].abs() < 1e-12 {
            return None;
        }
        a.swap(col, piv);
        b.swap(col, piv);
        for r in col + 1..N {
            let f = a[r][col] / a[col][col];
            if f == 0.0 {
                continue;
            }
            for c in col..N {
                a[r][c] -= f * a[col][c];
            }
            b[r] -= f * b[col];
        }
    }
    let mut x = [0.0; N];
    for r in (0..N).rev() {
        let mut s = b[r];
        for c in r + 1..N {
            s -= a[r][c] * x[c];
        }
        x[r] = s / a[r][r];
    }
    x.iter().all(|v| v.is_finite()).then_some(x)
}

/// A 3 × 3 projective transform (row-major), mapping `[x, y, 1]`.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Homography(pub [[f64; 3]; 3]);

impl Default for Homography {
    fn default() -> Self {
        Homography::IDENTITY
    }
}

impl Homography {
    pub const IDENTITY: Homography = Homography([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]);

    pub fn apply(&self, p: [f64; 2]) -> [f64; 2] {
        let m = &self.0;
        let w = m[2][0] * p[0] + m[2][1] * p[1] + m[2][2];
        let w = if w.abs() < 1e-12 { 1e-12 } else { w };
        [(m[0][0] * p[0] + m[0][1] * p[1] + m[0][2]) / w, (m[1][0] * p[0] + m[1][1] * p[1] + m[1][2]) / w]
    }

    /// `self ∘ other` (apply `other` first).
    pub fn then_after(&self, other: &Homography) -> Homography {
        Homography(mul(self.0, other.0)).normalized()
    }

    /// Composition `b ∘ a`: apply `a`, then `b`.
    pub fn compose(a: &Homography, b: &Homography) -> Homography {
        b.then_after(a)
    }

    /// Scale so that `h33 = 1` (when it isn't ≈ 0).
    pub fn normalized(self) -> Homography {
        let s = self.0[2][2];
        if s.abs() < 1e-12 || (s - 1.0).abs() < 1e-15 {
            return self;
        }
        Homography(self.0.map(|r| r.map(|v| v / s)))
    }

    /// The inverse map (`None` when singular).
    pub fn inverse(&self) -> Option<Homography> {
        let m = &self.0;
        let c00 = m[1][1] * m[2][2] - m[1][2] * m[2][1];
        let c01 = m[1][2] * m[2][0] - m[1][0] * m[2][2];
        let c02 = m[1][0] * m[2][1] - m[1][1] * m[2][0];
        let det = m[0][0] * c00 + m[0][1] * c01 + m[0][2] * c02;
        if det.abs() < 1e-18 || !det.is_finite() {
            return None;
        }
        let inv = [
            [c00, m[0][2] * m[2][1] - m[0][1] * m[2][2], m[0][1] * m[1][2] - m[0][2] * m[1][1]],
            [c01, m[0][0] * m[2][2] - m[0][2] * m[2][0], m[0][2] * m[1][0] - m[0][0] * m[1][2]],
            [c02, m[0][1] * m[2][0] - m[0][0] * m[2][1], m[0][0] * m[1][1] - m[0][1] * m[1][0]],
        ];
        Some(Homography(inv.map(|r| r.map(|v| v / det))).normalized())
    }

    /// Translation by `t`.
    pub fn translation(t: [f64; 2]) -> Homography {
        Homography([[1.0, 0.0, t[0]], [0.0, 1.0, t[1]], [0.0, 0.0, 1.0]])
    }

    /// Uniform scale `k` about `c`.
    pub fn scale_about(k: f64, c: [f64; 2]) -> Homography {
        Homography([[k, 0.0, c[0] * (1.0 - k)], [0.0, k, c[1] * (1.0 - k)], [0.0, 0.0, 1.0]])
    }

    /// `(1 - t) · self + t · other` element-wise (both normalised to `h33 = 1`): blends between
    /// nearby transforms.
    pub fn lerp(&self, other: &Homography, t: f64) -> Homography {
        let (a, b) = (self.normalized().0, other.normalized().0);
        let mut o = [[0.0; 3]; 3];
        for i in 0..3 {
            for j in 0..3 {
                o[i][j] = a[i][j] + (b[i][j] - a[i][j]) * t;
            }
        }
        Homography(o)
    }

    /// Map a Bezier tangent (relative to vertex `v`) through the transform: the tangent's end
    /// point is mapped and made relative to the mapped vertex.
    pub fn apply_tangent(&self, v: [f64; 2], tangent: [f64; 2]) -> [f64; 2] {
        if tangent == [0.0, 0.0] {
            return tangent;
        }
        let a = self.apply(v);
        let b = self.apply([v[0] + tangent[0], v[1] + tangent[1]]);
        [b[0] - a[0], b[1] - a[1]]
    }

    /// Largest absolute difference of the matrices (normalised).
    pub fn max_diff(&self, other: &Homography) -> f64 {
        let (a, b) = (self.normalized().0, other.normalized().0);
        let mut d: f64 = 0.0;
        for i in 0..3 {
            for j in 0..3 {
                d = d.max((a[i][j] - b[i][j]).abs());
            }
        }
        d
    }

    /// The homography taking `src[i]` to `dst[i]` (four points, no three collinear).
    pub fn from_points(src: [[f64; 2]; 4], dst: [[f64; 2]; 4]) -> Option<Homography> {
        // Normalise both sets (centroid at 0, mean distance √2) for conditioning.
        let (ns, ts) = normalise(&src);
        let (nd, td) = normalise(&dst);
        let mut a = [[0.0; 8]; 8];
        let mut b = [0.0; 8];
        for i in 0..4 {
            let ([x, y], [u, v]) = (ns[i], nd[i]);
            a[2 * i] = [x, y, 1.0, 0.0, 0.0, 0.0, -u * x, -u * y];
            b[2 * i] = u;
            a[2 * i + 1] = [0.0, 0.0, 0.0, x, y, 1.0, -v * x, -v * y];
            b[2 * i + 1] = v;
        }
        let h = solve_linear(a, b)?;
        let hn = [[h[0], h[1], h[2]], [h[3], h[4], h[5]], [h[6], h[7], 1.0]];
        // H = Td⁻¹ · Hn · Ts
        let m = mul(inv_similarity(td), mul(hn, ts));
        let s = m[2][2];
        if s.abs() < 1e-12 {
            return None;
        }
        Some(Homography(m.map(|r| r.map(|v| v / s))))
    }

    /// The affine map taking three points to three points.
    pub fn affine_from_points(src: [[f64; 2]; 3], dst: [[f64; 2]; 3]) -> Option<Homography> {
        let mut a = [[0.0; 3]; 3];
        for i in 0..3 {
            a[i] = [src[i][0], src[i][1], 1.0];
        }
        let x = solve_linear(a, [dst[0][0], dst[1][0], dst[2][0]])?;
        let y = solve_linear(a, [dst[0][1], dst[1][1], dst[2][1]])?;
        Some(Homography([x, y, [0.0, 0.0, 1.0]]))
    }

    /// The similarity (rotation, uniform scale, translation) taking two points to two points.
    pub fn similarity_from_points(src: [[f64; 2]; 2], dst: [[f64; 2]; 2]) -> Option<Homography> {
        let (ds, dd) = (sub(src[1], src[0]), sub(dst[1], dst[0]));
        let n = ds[0] * ds[0] + ds[1] * ds[1];
        if n < 1e-12 {
            return None;
        }
        // dd = [[a, -b], [b, a]] ds
        let a = (dd[0] * ds[0] + dd[1] * ds[1]) / n;
        let b = (dd[1] * ds[0] - dd[0] * ds[1]) / n;
        let tx = dst[0][0] - (a * src[0][0] - b * src[0][1]);
        let ty = dst[0][1] - (b * src[0][0] + a * src[0][1]);
        Some(Homography([[a, -b, tx], [b, a, ty], [0.0, 0.0, 1.0]]))
    }
}

/// Angle (degrees, y down = clockwise positive like After Effects' Rotation) and length of `b - a`.
pub fn angle_and_length(a: [f64; 2], b: [f64; 2]) -> (f64, f64) {
    let d = sub(b, a);
    (d[1].atan2(d[0]).to_degrees(), (d[0] * d[0] + d[1] * d[1]).sqrt())
}

/// `angle` moved by multiples of 360° to be closest to `prev` (continuous rotation curves).
pub fn unwrap_degrees(prev: f64, angle: f64) -> f64 {
    angle + ((prev - angle) / 360.0).round() * 360.0
}

fn sub(a: [f64; 2], b: [f64; 2]) -> [f64; 2] {
    [a[0] - b[0], a[1] - b[1]]
}

pub(crate) fn mul(a: [[f64; 3]; 3], b: [[f64; 3]; 3]) -> [[f64; 3]; 3] {
    let mut o = [[0.0; 3]; 3];
    for i in 0..3 {
        for j in 0..3 {
            o[i][j] = (0..3).map(|k| a[i][k] * b[k][j]).sum();
        }
    }
    o
}

fn normalise(p: &[[f64; 2]; 4]) -> ([[f64; 2]; 4], [[f64; 3]; 3]) {
    let c = [p.iter().map(|q| q[0]).sum::<f64>() / 4.0, p.iter().map(|q| q[1]).sum::<f64>() / 4.0];
    let d = p.iter().map(|q| ((q[0] - c[0]).powi(2) + (q[1] - c[1]).powi(2)).sqrt()).sum::<f64>() / 4.0;
    let s = if d > 1e-12 { std::f64::consts::SQRT_2 / d } else { 1.0 };
    let t = [[s, 0.0, -s * c[0]], [0.0, s, -s * c[1]], [0.0, 0.0, 1.0]];
    (p.map(|q| [s * (q[0] - c[0]), s * (q[1] - c[1])]), t)
}

fn inv_similarity(t: [[f64; 3]; 3]) -> [[f64; 3]; 3] {
    let s = t[0][0];
    [[1.0 / s, 0.0, -t[0][2] / s], [0.0, 1.0 / s, -t[1][2] / s], [0.0, 0.0, 1.0]]
}

#[cfg(test)]
mod tests {
    use super::*;

    fn close(a: [f64; 2], b: [f64; 2], tol: f64) -> bool {
        (a[0] - b[0]).abs() < tol && (a[1] - b[1]).abs() < tol
    }

    #[test]
    fn homography_recovers_known_projection() {
        let h = Homography([[0.9, 0.12, 30.0], [-0.08, 1.1, -12.0], [0.0004, -0.0002, 1.0]]);
        let src = [[100.0, 100.0], [700.0, 120.0], [680.0, 500.0], [90.0, 520.0]];
        let dst = src.map(|p| h.apply(p));
        let got = Homography::from_points(src, dst).unwrap();
        for i in 0..3 {
            for j in 0..3 {
                assert!((got.0[i][j] - h.0[i][j]).abs() < 1e-6 * (1.0 + h.0[i][j].abs()), "{:?}", got.0);
            }
        }
        for p in [[0.0, 0.0], [400.0, 300.0], [1000.0, 50.0]] {
            assert!(close(got.apply(p), h.apply(p), 1e-6));
        }
        // Degenerate (collinear) input has no solution.
        assert!(Homography::from_points([[0.0, 0.0], [1.0, 1.0], [2.0, 2.0], [3.0, 3.0]], dst).is_none());
    }

    #[test]
    fn affine_and_similarity() {
        let a = Homography::affine_from_points([[0.0, 0.0], [10.0, 0.0], [0.0, 10.0]], [[5.0, 5.0], [15.0, 7.0], [3.0, 15.0]]).unwrap();
        assert!(close(a.apply([10.0, 10.0]), [13.0, 17.0], 1e-9));
        let s = Homography::similarity_from_points([[0.0, 0.0], [10.0, 0.0]], [[1.0, 1.0], [1.0, 21.0]]).unwrap();
        assert!(close(s.apply([0.0, 10.0]), [-19.0, 1.0], 1e-9));
        let (ang, len) = angle_and_length([1.0, 1.0], [1.0, 21.0]);
        assert!((ang - 90.0).abs() < 1e-9 && (len - 20.0).abs() < 1e-9);
        assert!((unwrap_degrees(350.0, -5.0) - 355.0).abs() < 1e-9);
    }
}