///|
/// Definition of a ChArUco Board (Chessboard + ArUco markers).
pub(all) struct CharucoBoard {
  squares_x : Int
  squares_y : Int
  square_length : Double
  marker_length : Double
  dictionary : MarkerDictionary
  chessboard_corners : Array[Point3D] // 3D coordinates of all internal chessboard corners
  marker_ids : Array[Int]
  marker_corners : Array[Array[Point3D]] // 4 corners per marker
} derive(Debug, Eq)

///|
/// Creates a new ChArUco Board.
pub fn CharucoBoard::new(
  squares_x : Int,
  squares_y : Int,
  square_length : Double,
  marker_length : Double,
  dictionary : MarkerDictionary,
) -> CharucoBoard {
  let chessboard_corners : Array[Point3D] = []

  // Internal chessboard intersection corners
  for y in 1.. CharucoCorners {
  let matched_obj : Array[Point3D] = []
  let matched_img : Array[Point2D] = []

  for det in marker_detections {
    for i in 0.. solve_pnp(matched_obj, matched_img, cam)
    None => {
      // Fallback estimate with default camera
      let default_cam = CameraIntrinsics::new(
        img.width.to_double(),
        img.width.to_double(),
        img.width.to_double() * 0.5,
        img.height.to_double() * 0.5,
      )
      solve_pnp(matched_obj, matched_img, default_cam)
    }
  }

  match board_pose {
    Some(pose) => {
      let active_cam = match camera {
        Some(cam) => cam
        None =>
          CameraIntrinsics::new(
            img.width.to_double(),
            img.width.to_double(),
            img.width.to_double() * 0.5,
            img.height.to_double() * 0.5,
          )
      }

      let res_ids : Array[Int] = []
      let res_img : Array[Point2D] = []
      let res_obj : Array[Point3D] = []

      for i in 0..= 5.0 &&
          expected_proj.x < img.width.to_double() - 5.0 &&
          expected_proj.y >= 5.0 &&
          expected_proj.y < img.height.to_double() - 5.0 {
          // Refine expected corner with subpixel corner finder
          let refined = refine_corner_subpixel(
            img,
            expected_proj,
            window_radius=3,
          )
          res_ids.push(i)
          res_img.push(refined)
          res_obj.push(pt_3d)
        }
      }

      { corner_ids: res_ids, image_corners: res_img, object_corners: res_obj }
    }
    None => { corner_ids: [], image_corners: [], object_corners: [] }
  }
}

///|
/// Estimates high-precision 3D pose of ChArUco board from interpolated corners.
pub fn estimate_charuco_pose(
  charuco_corners : CharucoCorners,
  camera : CameraIntrinsics,
) -> Pose3D? {
  if charuco_corners.image_corners.length() < 4 {
    return None
  }
  solve_pnp(
    charuco_corners.object_corners,
    charuco_corners.image_corners,
    camera,
  )
}