///|
/// Represents a calibrated binocular stereo camera rig (Left & Right cameras).
pub(all) struct StereoCameraRig {
  left_camera : CameraIntrinsics
  right_camera : CameraIntrinsics
  baseline : Double // Distance between left and right optical centers (meters)
  right_to_left_pose : Pose3D
} derive(Debug, Eq)

///|
/// Creates a standard parallel binocular stereo camera rig.
pub fn StereoCameraRig::new_parallel(
  camera : CameraIntrinsics,
  baseline : Double,
) -> StereoCameraRig {
  let right_to_left = Pose3D::new(
    Point3D::new(0.0, 0.0, 0.0),
    Point3D::new(-baseline, 0.0, 0.0),
  )
  {
    left_camera: camera,
    right_camera: camera,
    baseline,
    right_to_left_pose: right_to_left,
  }
}

///|
/// Triangulates a 3D point in left camera frame from corresponding left and right pixel observations.
pub fn StereoCameraRig::triangulate_point(
  self : StereoCameraRig,
  left_pixel : Point2D,
  right_pixel : Point2D,
) -> Point3D? {
  let disparity = left_pixel.x - right_pixel.x
  if disparity.abs() < 1.0e-3 {
    return None // Zero or negative disparity (infinite distance)
  }

  let fx = self.left_camera.fx
  let fy = self.left_camera.fy
  let cx = self.left_camera.cx
  let cy = self.left_camera.cy

  let z = fx * self.baseline / disparity
  let x = (left_pixel.x - cx) * z / fx
  let y = (left_pixel.y - cy) * z / fy

  Some(Point3D::new(x, y, z))
}

///|
/// Solves stereo joint 3D pose of a fiducial marker observed by both left and right cameras simultaneously.
/// Leverages stereo epipolar geometry to eliminate depth ambiguity and improve accuracy.
pub fn StereoCameraRig::estimate_marker_pose_stereo(
  self : StereoCameraRig,
  left_quad : Quad,
  right_quad : Quad,
  marker_length : Double,
) -> Pose3D? {
  if left_quad.corners.length() != 4 || right_quad.corners.length() != 4 {
    return None
  }

  // Triangulate all 4 corners into 3D points in left camera coordinate system
  let corners_3d : Array[Point3D] = []
  for c in 0..<4 {
    match self.triangulate_point(left_quad.corners[c], right_quad.corners[c]) {
      Some(pt) => corners_3d.push(pt)
      None => return None
    }
  }

  // Define ideal planar marker 3D model points
  let half_s = marker_length * 0.5
  let obj_pts = [
    Point3D::new(-half_s, half_s, 0.0),
    Point3D::new(half_s, half_s, 0.0),
    Point3D::new(half_s, -half_s, 0.0),
    Point3D::new(-half_s, -half_s, 0.0),
  ]

  // Direct 3D-to-3D absolute orientation solver (Umeyama alignment)
  let mut obj_centroid = Point3D::new(0.0, 0.0, 0.0)
  let mut cam_centroid = Point3D::new(0.0, 0.0, 0.0)
  for i in 0..<4 {
    obj_centroid = obj_centroid.add(obj_pts[i])
    cam_centroid = cam_centroid.add(corners_3d[i])
  }
  let obj_c = obj_centroid.scale(0.25)
  let cam_c = cam_centroid.scale(0.25)

  let h_cov = DenseMatrix::zeros(3, 3)
  for i in 0..<4 {
    let dw = obj_pts[i].sub(obj_c)
    let dc = corners_3d[i].sub(cam_c)
    h_cov.set(0, 0, h_cov.get(0, 0) + dw.x * dc.x)
    h_cov.set(0, 1, h_cov.get(0, 1) + dw.x * dc.y)
    h_cov.set(0, 2, h_cov.get(0, 2) + dw.x * dc.z)
    h_cov.set(1, 0, h_cov.get(1, 0) + dw.y * dc.x)
    h_cov.set(1, 1, h_cov.get(1, 1) + dw.y * dc.y)
    h_cov.set(1, 2, h_cov.get(1, 2) + dw.y * dc.z)
    h_cov.set(2, 0, h_cov.get(2, 0) + dw.z * dc.x)
    h_cov.set(2, 1, h_cov.get(2, 1) + dw.z * dc.y)
    h_cov.set(2, 2, h_cov.get(2, 2) + dw.z * dc.z)
  }

  match h_cov.svd() {
    Some((u, _s, v)) => {
      let vt = v.transpose()
      match vt.mul(u.transpose()) {
        Some(r_mat) => {
          let rot = Matrix3x3::from_array(r_mat.data)
          let rvec = rotation_matrix_to_rvec(rot)
          let tvec = cam_c.sub(rot.mul_vec(obj_c))
          Some(Pose3D::new(rvec, tvec))
        }
        None => None
      }
    }
    None => None
  }
}