///|
/// Result of RANSAC PnP estimation.
pub(all) struct RansacPnpResult {
  pose : Pose3D
  inliers : Array[Bool]
  inlier_count : Int
  reprojection_error : Double
} derive(Debug, Eq)

///|
/// Robust Perspective-n-Point estimation using RANSAC.
pub fn solve_pnp_ransac(
  object_points : Array[Point3D],
  image_points : Array[Point2D],
  camera : CameraIntrinsics,
  max_iterations? : Int = 500,
  inlier_threshold? : Double = 3.0,
  seed? : Int = 42,
) -> RansacPnpResult? {
  let n = object_points.length()
  if n < 4 || image_points.length() != n {
    return None
  }

  let mut rng_state = seed.to_int64()
  let next_rand = fn() -> Int {
    rng_state = rng_state * 6364136223846793005L + 1442695040888963407L
    let val = if rng_state < 0L { -rng_state } else { rng_state }
    (val % n.to_int64()).to_int()
  }

  let mut best_inlier_count = 0
  let mut best_inliers = Array::make(n, false)
  let mut best_pose = Pose3D::new(
    Point3D::new(0.0, 0.0, 0.0),
    Point3D::new(0.0, 0.0, 1.0),
  )
  let mut best_error = 1.0e30

  let thresh_sq = inlier_threshold * inlier_threshold
  let mut iters = 0

  while iters < max_iterations {
    iters += 1

    // Sample 4 unique points
    let idxs = Array::make(4, 0)
    idxs[0] = next_rand()
    let mut count = 1
    while count < 4 {
      let r = next_rand()
      let mut dup = false
      for i in 0.. {
        let inliers = Array::make(n, false)
        let mut inlier_cnt = 0
        let mut total_err_sq = 0.0

        for i in 0.. best_inlier_count ||
          (inlier_cnt == best_inlier_count && total_err_sq < best_error) {
          best_inlier_count = inlier_cnt
          best_inliers = inliers
          best_pose = pose_cand
          best_error = total_err_sq
        }

        if inlier_cnt.to_double() / n.to_double() > 0.85 {
          break // Early exit on high consensus
        }
      }
      None => ()
    }
  }

  if best_inlier_count >= 4 {
    // Re-estimate on all inliers
    let inlier_obj : Array[Point3D] = []
    let inlier_img : Array[Point2D] = []
    for i in 0.. 0 {
      (best_error / best_inlier_count.to_double()).sqrt()
    } else {
      0.0
    }

    Some({
      pose: final_pose,
      inliers: best_inliers,
      inlier_count: best_inlier_count,
      reprojection_error: rmse,
    })
  } else {
    None
  }
}