///|
/// Helper for Zhang's camera calibration: constructs constraint row v_ij from homography column vectors h_i and h_j.
fn create_v_row(h : Matrix3x3, i : Int, j : Int) -> Array[Double] {
  let hi0 = h.get(0, i)
  let hi1 = h.get(1, i)
  let hi2 = h.get(2, i)

  let hj0 = h.get(0, j)
  let hj1 = h.get(1, j)
  let hj2 = h.get(2, j)

  [
    hi0 * hj0,
    hi0 * hj1 + hi1 * hj0,
    hi1 * hj1,
    hi2 * hj0 + hi0 * hj2,
    hi2 * hj1 + hi1 * hj2,
    hi2 * hj2,
  ]
}

///|
/// Calibrates camera intrinsics K and per-frame extrinsics from multi-view planar board frames using Zhang's method.
pub fn calibrate_camera_zhang(
  calibrator : CameraCalibrator,
) -> CalibrationResult? {
  let num_frames = calibrator.frames.length()
  if num_frames < 3 {
    return None // Need at least 3 distinct views for full intrinsics
  }

  // Step 1: Compute Homography for each frame
  let homographies : Array[Matrix3x3] = []
  for f in calibrator.frames {
    let obj_2d : Array[Point2D] = []
    for p in f.object_points {
      obj_2d.push(Point2D::new(p.x, p.y))
    }
    match find_homography_dlt(obj_2d, f.image_points) {
      Some(h) => homographies.push(h)
      None => return None
    }
  }

  // Step 2: Build 2K x 6 constraint matrix V: [v_12^T; (v_11 - v_22)^T] * b = 0
  let v_mat = DenseMatrix::zeros(2 * num_frames, 6)
  for k in 0..
      [
        v.get(0, 5),
        v.get(1, 5),
        v.get(2, 5),
        v.get(3, 5),
        v.get(4, 5),
        v.get(5, 5),
      ]
    None => return None
  }

  let b11 = b_vec[0]
  let b12 = b_vec[1]
  let b22 = b_vec[2]
  let b13 = b_vec[3]
  let b23 = b_vec[4]
  let b33 = b_vec[5]

  // Step 4: Extract intrinsic parameters from B matrix
  let cy = (b12 * b13 - b11 * b23) / (b11 * b22 - b12 * b12)
  let lambda = b33 - (b13 * b13 + cy * (b12 * b13 - b11 * b23)) / b11
  if lambda.abs() < 1.0e-12 || b11.abs() < 1.0e-12 {
    return None
  }

  let fx_sq = lambda / b11
  let fy_sq = lambda * b11 / (b11 * b22 - b12 * b12)
  if fx_sq <= 0.0 || fy_sq <= 0.0 {
    return None
  }

  let fx = fx_sq.sqrt()
  let fy = fy_sq.sqrt()
  let cx = -b13 * (fx * fx) / lambda - b12 * cy * (fx * fx) / (lambda * b11)

  let intrinsics = CameraIntrinsics::new(fx, fy, cx, cy)

  // Step 5: Compute extrinsic poses for each frame
  let extrinsics : Array[Pose3D] = []
  let per_view_errors : Array[Double] = []
  let mut total_sq_err = 0.0
  let mut total_pts = 0

  for k in 0.. {
        extrinsics.push(pose)
        let metrics = compute_reprojection_metrics(
          f.object_points,
          f.image_points,
          pose,
          intrinsics,
        )
        per_view_errors.push(metrics.rmse)
        total_sq_err += metrics.rmse *
          metrics.rmse *
          f.object_points.length().to_double()
        total_pts += f.object_points.length()
      }
      None => {
        extrinsics.push(
          Pose3D::new(Point3D::new(0.0, 0.0, 0.0), Point3D::new(0.0, 0.0, 1.0)),
        )
        per_view_errors.push(0.0)
      }
    }
  }

  let overall_rmse = if total_pts > 0 {
    (total_sq_err / total_pts.to_double()).sqrt()
  } else {
    0.0
  }

  Some({
    camera_intrinsics: intrinsics,
    extrinsic_poses: extrinsics,
    per_view_errors,
    overall_rmse,
  })
}