///|
/// Result of Infinitesimal Plane-based Pose Estimation (IPPE).
/// Contains two possible poses due to planar projective ambiguity and their reprojection errors.
pub(all) struct IppeResult {
  pose1 : Pose3D
  pose2 : Pose3D
  reproj_error1 : Double
  reproj_error2 : Double
  ambiguity_ratio : Double
} derive(Debug, Eq)

///|
/// Solves planar PnP using Infinitesimal Plane-based Pose Estimation (IPPE).
/// Returns two analytical pose solutions and their reprojection error ratio.
pub fn solve_pnp_ippe(
  object_points : Array[Point3D],
  image_points : Array[Point2D],
  camera : CameraIntrinsics,
) -> IppeResult? {
  let n = object_points.length()
  if n < 4 || image_points.length() != n {
    return None
  }

  // Convert 3D planar object points (z ~ 0) to 2D object points
  let obj_2d : Array[Point2D] = []
  for p in object_points {
    obj_2d.push(Point2D::new(p.x, p.y))
  }

  // Normalize image points with camera intrinsics
  let norm_img : Array[Point2D] = []
  for p in image_points {
    let u = (p.x - camera.cx) / camera.fx
    let v = (p.y - camera.cy) / camera.fy
    norm_img.push(Point2D::new(u, v))
  }

  // Compute Homography H from obj_2d to norm_img
  match find_homography_dlt(obj_2d, norm_img) {
    Some(h) => {
      // Decompose Homography into two motion solutions
      let h00 = h.get(0, 0)
      let h10 = h.get(1, 0)
      let h20 = h.get(2, 0)
      let h01 = h.get(0, 1)
      let h11 = h.get(1, 1)
      let h21 = h.get(2, 1)
      let h02 = h.get(0, 2)
      let h12 = h.get(1, 2)
      let h22 = h.get(2, 2)

      let norm1 = (h00 * h00 + h10 * h10 + h20 * h20).sqrt()
      let norm2 = (h01 * h01 + h11 * h11 + h21 * h21).sqrt()
      let lambda = 2.0 / (norm1 + norm2)

      if lambda < 1.0e-12 {
        return None
      }

      let r1 = Point3D::new(h00 * lambda, h10 * lambda, h20 * lambda)
      let r2 = Point3D::new(h01 * lambda, h11 * lambda, h21 * lambda)
      let t = Point3D::new(h02 * lambda, h12 * lambda, h22 * lambda)

      let r3 = r1.cross(r2)

      // Solution 1: Direct orthonormalized frame
      let rot1 = Matrix3x3::from_array([
        r1.x,
        r2.x,
        r3.x,
        r1.y,
        r2.y,
        r3.y,
        r1.z,
        r2.z,
        r3.z,
      ])
      let rvec1 = rotation_matrix_to_rvec(rot1)
      let pose1 = Pose3D::new(rvec1, t)

      // Solution 2: Ambiguous symmetric reflection
      let r1_b = Point3D::new(-r1.x, -r1.y, -r1.z)
      let r2_b = Point3D::new(-r2.x, -r2.y, -r2.z)
      let r3_b = r1_b.cross(r2_b)
      let rot2 = Matrix3x3::from_array([
        r1_b.x,
        r2_b.x,
        r3_b.x,
        r1_b.y,
        r2_b.y,
        r3_b.y,
        r1_b.z,
        r2_b.z,
        r3_b.z,
      ])
      let rvec2 = rotation_matrix_to_rvec(rot2)
      let t2 = Point3D::new(-t.x, -t.y, t.z)
      let pose2 = Pose3D::new(rvec2, t2)

      // Compute RMS reprojection errors
      let mut err1_sum = 0.0
      let mut err2_sum = 0.0
      for i in 0.. 1.0e-9 { err2 / err1 } else { 1.0e6 }

      Some({
        pose1,
        pose2,
        reproj_error1: err1,
        reproj_error2: err2,
        ambiguity_ratio: ratio,
      })
    }
    None => None
  }
}