///|
/// Summary metrics of geometric reprojection error.
pub(all) struct ReprojectionMetrics {
  rmse : Double
  mae : Double
  max_error : Double
  point_errors : Array[Double]
} derive(Debug, Eq)

///|
/// Computes comprehensive reprojection error metrics for 3D-to-2D correspondences.
pub fn compute_reprojection_metrics(
  object_points : Array[Point3D],
  image_points : Array[Point2D],
  pose : Pose3D,
  camera : CameraIntrinsics,
) -> ReprojectionMetrics {
  let n = object_points.length()
  if n == 0 || image_points.length() != n {
    return { rmse: 0.0, mae: 0.0, max_error: 0.0, point_errors: [] }
  }

  let point_errors = Array::make(n, 0.0)
  let mut sum_sq = 0.0
  let mut sum_abs = 0.0
  let mut max_err = 0.0

  for i in 0.. max_err {
      max_err = dist
    }
  }

  let rmse = (sum_sq / n.to_double()).sqrt()
  let mae = sum_abs / n.to_double()

  { rmse, mae, max_error: max_err, point_errors }
}

///|
/// Back-projects a 2D image pixel into a 3D unit ray in world coordinates.
pub fn backproject_pixel_to_ray(
  pixel : Point2D,
  pose : Pose3D,
  camera : CameraIntrinsics,
) -> (Point3D, Point3D) {
  // Undistorted normalized camera ray
  let u = (pixel.x - camera.cx) / camera.fx
  let v = (pixel.y - camera.cy) / camera.fy
  let ray_cam = Point3D::new(u, v, 1.0).normalized()

  // Camera center in world coordinates: C = -R^T * t
  let rot = rvec_to_rotation_matrix(pose.rvec)
  let rot_t = rot.transpose()
  let t = pose.tvec
  let cam_center = Point3D::new(
    -(rot_t.get(0, 0) * t.x + rot_t.get(0, 1) * t.y + rot_t.get(0, 2) * t.z),
    -(rot_t.get(1, 0) * t.x + rot_t.get(1, 1) * t.y + rot_t.get(1, 2) * t.z),
    -(rot_t.get(2, 0) * t.x + rot_t.get(2, 1) * t.y + rot_t.get(2, 2) * t.z),
  )

  // Ray direction in world coordinates: d_world = R^T * ray_cam
  let ray_world = rot_t.mul_vec(ray_cam).normalized()

  (cam_center, ray_world)
}

///|
/// Intersects a camera ray with a 3D plane defined by normal and distance d (normal . X + d = 0).
pub fn intersect_ray_with_plane(
  ray_origin : Point3D,
  ray_dir : Point3D,
  plane_normal : Point3D,
  plane_d : Double,
) -> Point3D? {
  let denom = plane_normal.dot(ray_dir)
  if denom.abs() < 1.0e-12 {
    return None // Ray parallel to plane
  }

  let t = -(plane_normal.dot(ray_origin) + plane_d) / denom
  if t < 0.0 {
    return None // Intersection behind camera
  }

  Some(ray_origin.add(ray_dir.scale(t)))
}