///|
/// Solves an 8x8 linear system Ax = b using Gaussian elimination with partial pivoting.
fn solve_8x8_system(
  matrix_a : Array[Double],
  vector_b : Array[Double],
) -> Array[Double]? {
  let a = matrix_a.copy()
  let b = vector_b.copy()
  let n = 8

  for i in 0.. max_val {
        max_val = val
        max_row = k
      }
    }

    if max_val < 1.0e-12 {
      return None
    }

    // Swap pivot rows
    if max_row != i {
      for j in 0..= 0 {
    let mut sum = b[i]
    for j in (i + 1).. Matrix3x3? {
  if src_pts.length() != 4 || dst_pts.length() != 4 {
    return None
  }

  let a = Array::make(64, 0.0)
  let b = Array::make(8, 0.0)

  for i in 0..<4 {
    let u = src_pts[i].x
    let v = src_pts[i].y
    let x = dst_pts[i].x
    let y = dst_pts[i].y

    let r1 = i * 2
    let r2 = i * 2 + 1

    // Row 1: [u, v, 1, 0, 0, 0, -x*u, -x*v] = x
    a[r1 * 8 + 0] = u
    a[r1 * 8 + 1] = v
    a[r1 * 8 + 2] = 1.0
    a[r1 * 8 + 3] = 0.0
    a[r1 * 8 + 4] = 0.0
    a[r1 * 8 + 5] = 0.0
    a[r1 * 8 + 6] = -x * u
    a[r1 * 8 + 7] = -x * v
    b[r1] = x

    // Row 2: [0, 0, 0, u, v, 1, -y*u, -y*v] = y
    a[r2 * 8 + 0] = 0.0
    a[r2 * 8 + 1] = 0.0
    a[r2 * 8 + 2] = 0.0
    a[r2 * 8 + 3] = u
    a[r2 * 8 + 4] = v
    a[r2 * 8 + 5] = 1.0
    a[r2 * 8 + 6] = -y * u
    a[r2 * 8 + 7] = -y * v
    b[r2] = y
  }

  match solve_8x8_system(a, b) {
    Some(sol) => {
      let h_arr = [
        sol[0],
        sol[1],
        sol[2],
        sol[3],
        sol[4],
        sol[5],
        sol[6],
        sol[7],
        1.0,
      ]
      Some(Matrix3x3::from_array(h_arr))
    }
    None => None
  }
}

///|
/// Hartley Isotropic Coordinate Normalization: Translates centroid to origin and scales average distance to sqrt(2).
/// Returns (normalized_points, 3x3_normalization_matrix).
pub fn normalize_points_hartley(
  points : Array[Point2D],
) -> (Array[Point2D], Matrix3x3) {
  let n = points.length()
  if n == 0 {
    return ([], Matrix3x3::identity())
  }

  // Compute centroid
  let mut cx = 0.0
  let mut cy = 0.0
  for p in points {
    cx += p.x
    cy += p.y
  }
  cx /= n.to_double()
  cy /= n.to_double()

  // Compute average distance from centroid
  let mut dist_sum = 0.0
  for p in points {
    let dx = p.x - cx
    let dy = p.y - cy
    dist_sum += (dx * dx + dy * dy).sqrt()
  }
  let avg_dist = dist_sum / n.to_double()
  let scale = if avg_dist > 1.0e-12 {
    1.4142135623730951 / avg_dist // sqrt(2) / avg_dist
  } else {
    1.0
  }

  let norm_pts : Array[Point2D] = []
  for p in points {
    norm_pts.push(Point2D::new((p.x - cx) * scale, (p.y - cy) * scale))
  }

  let t_mat = Matrix3x3::from_array([
    scale,
    0.0,
    -scale * cx,
    0.0,
    scale,
    -scale * cy,
    0.0,
    0.0,
    1.0,
  ])

  (norm_pts, t_mat)
}

///|
/// Normalized Direct Linear Transform (DLT) for N >= 4 point correspondences.
pub fn find_homography_dlt(
  src_pts : Array[Point2D],
  dst_pts : Array[Point2D],
) -> Matrix3x3? {
  let n = src_pts.length()
  if n < 4 || dst_pts.length() != n {
    return None
  }

  let (norm_src, t_src) = normalize_points_hartley(src_pts)
  let (norm_dst, t_dst) = normalize_points_hartley(dst_pts)

  // Build 2N x 9 matrix A
  let a_mat = DenseMatrix::zeros(2 * n, 9)
  for i in 0.. {
      // V is 9x9, last column is index 8
      let h_norm = Matrix3x3::from_array([
        v.get(0, 8),
        v.get(1, 8),
        v.get(2, 8),
        v.get(3, 8),
        v.get(4, 8),
        v.get(5, 8),
        v.get(6, 8),
        v.get(7, 8),
        v.get(8, 8),
      ])

      // Denormalize: H = T_dst^(-1) * H_norm * T_src
      match t_dst.inverse() {
        Some(t_dst_inv) => {
          let h_temp = t_dst_inv.mul(h_norm)
          let h = h_temp.mul(t_src)
          let h33 = h.get(2, 2)
          if h33.abs() > 1.0e-12 {
            let inv_h33 = 1.0 / h33
            let h_final = Matrix3x3::from_array([
              h.get(0, 0) * inv_h33,
              h.get(0, 1) * inv_h33,
              h.get(0, 2) * inv_h33,
              h.get(1, 0) * inv_h33,
              h.get(1, 1) * inv_h33,
              h.get(1, 2) * inv_h33,
              h.get(2, 0) * inv_h33,
              h.get(2, 1) * inv_h33,
              1.0,
            ])
            Some(h_final)
          } else {
            Some(h)
          }
        }
        None => None
      }
    }
    None => None
  }
}

///|
/// Warps perspective of src_img inside src_quad into a normalized NxN grid image.
pub fn warp_perspective_grid(
  src_img : Image,
  src_quad : Quad,
  grid_size : Int,
) -> Image? {
  if src_quad.corners.length() != 4 || grid_size <= 0 {
    return None
  }

  let dst_corners = [
    { x: 0.0, y: 0.0 },
    { x: grid_size.to_double(), y: 0.0 },
    { x: grid_size.to_double(), y: grid_size.to_double() },
    { x: 0.0, y: grid_size.to_double() },
  ]

  // Find Homography mapping destination grid coordinates -> source image quad coordinates
  match find_homography(dst_corners, src_quad.corners) {
    Some(h_inv) => {
      let sampled_img = Image::new(grid_size, grid_size)
      for gy in 0.. None
  }
}