///|
/// Levenberg-Marquardt Non-Linear Pose Optimization:
/// Minimizes geometric reprojection error sum ||p_i - pi(K, exp(omega)*R, t + delta_t, P_i)||^2.
pub fn solve_pnp_levenberg_marquardt(
  object_points : Array[Point3D],
  image_points : Array[Point2D],
  initial_pose : Pose3D,
  camera : CameraIntrinsics,
  max_iterations? : Int = 30,
  epsilon? : Double = 1.0e-6,
) -> Pose3D {
  let n = object_points.length()
  if n < 4 || image_points.length() != n {
    return initial_pose
  }

  let mut current_pose = initial_pose
  let mut lambda = 1.0e-3

  for _iter in 0.. {
        // Add LM damping
        for d in 0..<6 {
          let diag = j_tj.get(d, d)
          j_tj.set(
            d,
            d,
            diag + lambda * (if diag > 1.0e-6 { diag } else { 1.0 }),
          )
        }

        // J^T * r
        match jt.mul_vec(r_vec) {
          Some(j_tr) =>
            match j_tj.solve_linear(j_tr) {
              Some(delta) => {
                let d_omega = Point3D::new(delta[0], delta[1], delta[2])
                let d_t = Point3D::new(delta[3], delta[4], delta[5])

                // Update pose
                let d_rot = rvec_to_rotation_matrix(d_omega)
                let new_rot = d_rot.mul(rot)
                let new_rvec = rotation_matrix_to_rvec(new_rot)
                let new_tvec = Point3D::new(
                  t.x + d_t.x,
                  t.y + d_t.y,
                  t.z + d_t.z,
                )
                let candidate_pose = Pose3D::new(new_rvec, new_tvec)

                // Evaluate candidate error
                let mut candidate_err_sq = 0.0
                for i in 0.. break
            }
          None => break
        }
      }
      None => break
    }
  }

  current_pose
}

///|
/// Solves Perspective-n-Point problem for general planar/non-planar targets.
pub fn solve_pnp(
  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
  }

  // Check if planar target (z variance < 1e-6)
  let mut z_sum = 0.0
  for p in object_points {
    z_sum += p.z
  }
  let z_avg = z_sum / n.to_double()
  let mut is_planar = true
  for p in object_points {
    if (p.z - z_avg).abs() > 1.0e-3 {
      is_planar = false
      break
    }
  }

  let initial_pose = if is_planar {
    match solve_pnp_ippe(object_points, image_points, camera) {
      Some(res) =>
        if res.reproj_error1 <= res.reproj_error2 {
          res.pose1
        } else {
          res.pose2
        }
      None => return None
    }
  } else {
    // Non-planar DLT initialization
    let p0 = object_points[0]
    let p1 = object_points[1]
    let quad = Quad::new(
      image_points[0],
      image_points[1],
      image_points[2],
      image_points[3],
    )
    let side = p0.distance(p1)
    match estimate_marker_pose(quad, camera, side) {
      Some(pose) => pose
      None => return None
    }
  }

  // Refine with Levenberg-Marquardt
  Some(
    solve_pnp_levenberg_marquardt(
      object_points, image_points, initial_pose, camera,
    ),
  )
}