///|
/// Immutable zero-skew pinhole intrinsics and Brown-Conrady distortion.
/// The radius bounds ideal (undistorted) normalized coordinates, not pixels.
pub struct CameraModel {
  priv fx : Double
  priv fy : Double
  priv cx : Double
  priv cy : Double
  priv distortion : @brown.Distortion
  priv radius : Double
  priv perturbation_bound : Double
} derive(Debug)

///|
pub extend CameraModel with @debug.Debug::{to_repr}

///|
fn norm2(x : Double, y : Double) -> Double {
  let scale = x.abs().max(y.abs())
  if scale == 0.0 {
    return 0.0
  }
  scale * (x / scale * (x / scale) + y / scale * (y / scale)).sqrt()
}

///|
/// Construct a camera whose forward normalized distortion has a unique inverse
/// branch on the specified closed disk. The analytic bound
/// 3|k1|R² + 5|k2|R⁴ + 7|k3|R⁶ + 6R sqrt(p1²+p2²) < 1
/// bounds ||J_distortion-I||₂ there. Since the Brown Jacobian is symmetric,
/// its eigenvalues are positive and the forward map is strongly monotone.
/// Unsafe parameters are rejected rather than selecting an arbitrary root.
pub fn CameraModel::new(
  fx~ : Double,
  fy~ : Double,
  cx~ : Double,
  cy~ : Double,
  k1~ : Double = 0.0,
  k2~ : Double = 0.0,
  p1~ : Double = 0.0,
  p2~ : Double = 0.0,
  k3~ : Double = 0.0,
  max_normalized_radius~ : Double = 1.0,
) -> CameraModel raise @types.GroundError {
  for value in [fx, fy, cx, cy, k1, k2, p1, p2, k3, max_normalized_radius] {
    if !@types.is_finite(value) {
      raise @types.InvalidInput("camera parameters must be finite")
    }
  }
  if fx <= 0.0 || fy <= 0.0 || max_normalized_radius <= 0.0 {
    raise @types.InvalidInput(
      "camera focal lengths and radius must be positive",
    )
  }
  let r = max_normalized_radius
  let r2 = r * r
  let r4 = r2 * r2
  let r6 = r4 * r2
  if !@types.is_finite(r6) || !@types.is_finite(r4) || !@types.is_finite(r2) {
    raise @types.InvalidInput(
      "camera radius exceeds finite distortion arithmetic",
    )
  }
  let bound = 3.0 * (k1.abs() * r2) +
    5.0 * (k2.abs() * r4) +
    7.0 * (k3.abs() * r6) +
    6.0 * (r * norm2(p1, p2))
  if !@types.is_finite(bound) ||
    bound >= 1.0 ||
    !@types.is_finite((1.0 + bound) * r) {
    raise @types.InvalidInput(
      "camera distortion cannot certify a unique inverse within radius",
    )
  }
  {
    fx,
    fy,
    cx,
    cy,
    distortion: @brown.Distortion::brown_conrady(k1~, k2~, p1~, p2~, k3~),
    radius: r,
    perturbation_bound: bound,
  }
}

///|
/// Defensive parameter copy: fx, fy, cx, cy, k1, k2, p1, p2, k3.
pub fn CameraModel::parameters(self : CameraModel) -> Array[Double] {
  let d = self.distortion
  [self.fx, self.fy, self.cx, self.cy, d.k1, d.k2, d.p1, d.p2, d.k3]
}

///|
pub fn CameraModel::max_normalized_radius(self : CameraModel) -> Double {
  self.radius
}