///|
/// Observation in bundle adjustment: a 2D image observation of a 3D point in a specific camera frame.
pub(all) struct BaObservation {
  camera_index : Int
  point_index : Int
  image_point : Point2D
} derive(Debug, Eq)

///|
/// Bundle Adjustment problem setup with camera keyframes and 3D landmark points.
pub(all) struct BundleAdjustmentProblem {
  camera_intrinsics : CameraIntrinsics
  camera_poses : Array[Pose3D]
  points_3d : Array[Point3D]
  observations : Array[BaObservation]
} derive(Debug, Eq)

///|
/// Creates a new BundleAdjustmentProblem.
pub fn BundleAdjustmentProblem::new(
  camera_intrinsics : CameraIntrinsics,
) -> BundleAdjustmentProblem {
  { camera_intrinsics, camera_poses: [], points_3d: [], observations: [] }
}

///|
/// Optimizes camera poses and 3D points simultaneously using Levenberg-Marquardt bundle adjustment.
pub fn BundleAdjustmentProblem::optimize(
  self : BundleAdjustmentProblem,
  max_iterations? : Int = 20,
) -> Unit {
  let num_cams = self.camera_poses.length()
  let num_pts = self.points_3d.length()
  let num_obs = self.observations.length()

  if num_cams == 0 || num_pts == 0 || num_obs == 0 {
    return
  }

  for _iter in 0..= 4 {
        let refined_pose = solve_pnp_levenberg_marquardt(
          cam_obj_pts,
          cam_img_pts,
          self.camera_poses[c],
          self.camera_intrinsics,
          max_iterations=5,
        )
        self.camera_poses[c] = refined_pose
      }
    }

    // 2. Optimize each 3D point via multi-view triangulation rays holding camera poses fixed
    for p in 0.. {
              match a_sum.add(proj_mat) {
                Some(new_a) => a_sum = new_a
                None => ()
              }
              let bc = proj_mat.mul_vec([c_center.x, c_center.y, c_center.z])
              match bc {
                Some(b_vec) =>
                  b_sum = b_sum.add(Point3D::new(b_vec[0], b_vec[1], b_vec[2]))
                None => ()
              }
              obs_count += 1
            }
            None => ()
          }
        }
      }

      if obs_count >= 2 {
        match a_sum.solve_linear([b_sum.x, b_sum.y, b_sum.z]) {
          Some(sol) => self.points_3d[p] = Point3D::new(sol[0], sol[1], sol[2])
          None => ()
        }
      }
    }
  }
}