///|
/// EPnP (Efficient Perspective-n-Point) Algorithm (Lepetit, Moreno-Noguer, Fua - IJCV 2009).
/// Solves general non-planar/planar PnP in O(N) time using 4 virtual control points.
pub fn solve_pnp_epnp(
  object_points : Array[Point3D],
  image_points : Array[Point2D],
  camera : CameraIntrinsics,
) -> Pose3D? {
  let n = object_points.length()
  if n < 4 || image_points.length() != n {
    return None
  }

  // Step 1: Select 4 control points in world space
  // C0 is centroid of object points
  let mut c0_x = 0.0
  let mut c0_y = 0.0
  let mut c0_z = 0.0
  for p in object_points {
    c0_x += p.x
    c0_y += p.y
    c0_z += p.z
  }
  let c0 = Point3D::new(
    c0_x / n.to_double(),
    c0_y / n.to_double(),
    c0_z / n.to_double(),
  )

  // PCA on object points relative to centroid to choose principal axes as C1, C2, C3
  let cov_mat = DenseMatrix::zeros(3, 3)
  for p in object_points {
    let dx = p.x - c0.x
    let dy = p.y - c0.y
    let dz = p.z - c0.z
    cov_mat.set(0, 0, cov_mat.get(0, 0) + dx * dx)
    cov_mat.set(0, 1, cov_mat.get(0, 1) + dx * dy)
    cov_mat.set(0, 2, cov_mat.get(0, 2) + dx * dz)
    cov_mat.set(1, 0, cov_mat.get(1, 0) + dy * dx)
    cov_mat.set(1, 1, cov_mat.get(1, 1) + dy * dy)
    cov_mat.set(1, 2, cov_mat.get(1, 2) + dy * dz)
    cov_mat.set(2, 0, cov_mat.get(2, 0) + dz * dx)
    cov_mat.set(2, 1, cov_mat.get(2, 1) + dz * dy)
    cov_mat.set(2, 2, cov_mat.get(2, 2) + dz * dz)
  }

  let (c1, c2, c3) = match cov_mat.svd() {
    Some((_u, s, v)) => {
      let scale0 = (s[0] / n.to_double()).sqrt()
      let scale1 = (s[1] / n.to_double()).sqrt()
      let scale2 = (s[2] / n.to_double()).sqrt()

      let v0 = Point3D::new(v.get(0, 0), v.get(1, 0), v.get(2, 0)).scale(scale0)
      let v1 = Point3D::new(v.get(0, 1), v.get(1, 1), v.get(2, 1)).scale(scale1)
      let v2 = Point3D::new(v.get(0, 2), v.get(1, 2), v.get(2, 2)).scale(scale2)

      (c0.add(v0), c0.add(v1), c0.add(v2))
    }
    None =>
      (
        c0.add(Point3D::new(1.0, 0.0, 0.0)),
        c0.add(Point3D::new(0.0, 1.0, 0.0)),
        c0.add(Point3D::new(0.0, 0.0, 1.0)),
      )
  }

  let c_world = [c0, c1, c2, c3]

  // Step 2: Compute barycentric coordinates (alphas) for each point: P_i = sum(alpha_ij * C_j)
  // [C1-C0, C2-C0, C3-C0] * alpha = P_i - C0
  let c_mat = DenseMatrix::zeros(3, 3)
  for col in 0..<3 {
    let diff = c_world[col + 1].sub(c0)
    c_mat.set(0, col, diff.x)
    c_mat.set(1, col, diff.y)
    c_mat.set(2, col, diff.z)
  }

  let c_inv = match c_mat.inverse() {
    Some(inv) => inv
    None => return None
  }

  let alphas = Array::make(n * 4, 0.0)
  for i in 0.. {
        let a1 = a[0]
        let a2 = a[1]
        let a3 = a[2]
        let a0 = 1.0 - a1 - a2 - a3
        alphas[i * 4 + 0] = a0
        alphas[i * 4 + 1] = a1
        alphas[i * 4 + 2] = a2
        alphas[i * 4 + 3] = a3
      }
      None => return None
    }
  }

  // Step 3: Build 2N x 12 linear system M * x = 0
  let m_mat = DenseMatrix::zeros(2 * n, 12)
  for i in 0.. {
      let sol = Array::make(12, 0.0)
      for k in 0..<12 {
        sol[k] = v.get(k, 11)
      }
      sol
    }
    None => return None
  }

  // Extract camera control points C^c
  let c_cam = [
    Point3D::new(v_null[0], v_null[1], v_null[2]),
    Point3D::new(v_null[3], v_null[4], v_null[5]),
    Point3D::new(v_null[6], v_null[7], v_null[8]),
    Point3D::new(v_null[9], v_null[10], v_null[11]),
  ]

  // Compute scale beta by comparing control point distances in world vs camera coordinates
  let dist_w01 = c_world[0].distance(c_world[1])
  let dist_c01 = c_cam[0].distance(c_cam[1])
  let beta = if dist_c01 > 1.0e-9 { dist_w01 / dist_c01 } else { 1.0 }

  let c_cam_scaled = [
    c_cam[0].scale(beta),
    c_cam[1].scale(beta),
    c_cam[2].scale(beta),
    c_cam[3].scale(beta),
  ]

  // Compute 3D camera points for all object points
  let points_cam : Array[Point3D] = []
  for i in 0.. {
      let vt = v.transpose()
      let r_est = vt.mul(u.transpose())
      match r_est {
        Some(r) => {
          let det = r.determinant()
          if det < 0.0 {
            let v_fix = v.clone()
            v_fix.set(0, 2, -v_fix.get(0, 2))
            v_fix.set(1, 2, -v_fix.get(1, 2))
            v_fix.set(2, 2, -v_fix.get(2, 2))
            match v_fix.transpose().mul(u.transpose()) {
              Some(rf) => Some(Matrix3x3::from_array(rf.data))
              None => None
            }
          } else {
            Some(Matrix3x3::from_array(r.data))
          }
        }
        None => None
      }
    }
    None => None
  }

  match rot_opt {
    Some(rot_mat) => {
      let rvec = rotation_matrix_to_rvec(rot_mat)
      let tvec = p_cam_c.sub(rot_mat.mul_vec(c0))
      let initial_pose = Pose3D::new(rvec, tvec)
      Some(
        solve_pnp_levenberg_marquardt(
          object_points, image_points, initial_pose, camera,
        ),
      )
    }
    None => None
  }
}