///|
fn CameraModel::project_disk(
  self : CameraModel,
  p : @geometry.Point2,
) -> @geometry.Point2 {
  let norm = norm2(p.x, p.y)
  if norm <= self.radius {
    p
  } else {
    let ratio = self.radius / norm
    @geometry.Point2::new(x=p.x * ratio, y=p.y * ratio)
  }
}

///|
fn CameraModel::solve_normalized(
  self : CameraModel,
  point : @types.Point2,
) -> @geometry.Point2 raise @types.GroundError {
  point.validate()
  let target = @geometry.Point2::new(
    x=(point.x - self.cx) / self.fx,
    y=(point.y - self.cy) / self.fy,
  )
  let target_norm = norm2(target.x, target.y)
  if !@types.is_finite(target.x) ||
    !@types.is_finite(target.y) ||
    !@types.is_finite(target_norm) {
    raise @types.InvalidInput("camera normalized pixel is not finite")
  }
  // f(0)=0 and ||J_f|| <= 1+L on the disk, so points beyond this
  // outer radius cannot possess an inverse in the declared domain.
  if target_norm / self.radius > 1.0 + self.perturbation_bound + 1.0e-12 {
    raise @types.InvalidInput("pixel has no inverse within camera radius")
  }
  let mut estimate = self.project_disk(target)
  // Projection is non-expansive. Thus Proj_disk(target - (f(x)-x))
  // is a contraction with constant L. A boundary fixed point without a small
  // forward residual is not an inverse and must never be accepted.
  for _ in 0..<512 {
    let forward = @brown.distort_point(estimate, self.distortion)
    let dx = target.x - forward.x
    let dy = target.y - forward.y
    let residual = norm2(dx, dy)
    if !@types.is_finite(forward.x) ||
      !@types.is_finite(forward.y) ||
      !@types.is_finite(residual) {
      raise @types.InvalidInput("camera inverse arithmetic is not finite")
    }
    // Work relative to the declared radius: an absolute normalized-coordinate
    // tolerance would silently enlarge tiny domains (and clip distant inputs).
    if residual / self.radius / (1.0 - self.perturbation_bound) <= 1.0e-12 {
      if norm2(estimate.x, estimate.y) / self.radius > 1.0 + 1.0e-12 {
        raise @types.InvalidInput("rectified point exceeds camera radius")
      }
      return estimate
    }
    let next = self.project_disk(
      @geometry.Point2::new(x=estimate.x + dx, y=estimate.y + dy),
    )
    if !@types.is_finite(next.x) || !@types.is_finite(next.y) {
      raise @types.InvalidInput("camera inverse arithmetic is not finite")
    }
    if next.x == estimate.x && next.y == estimate.y {
      raise @types.InvalidInput("camera inverse did not converge within radius")
    }
    estimate = next
  }
  raise @types.InvalidInput("camera inverse did not converge in 512 iterations")
}

///|
fn CameraModel::rectified_pixel(
  self : CameraModel,
  normalized : @geometry.Point2,
) -> @types.Point2 raise @types.GroundError {
  @types.Point2::new(
    self.fx * normalized.x + self.cx,
    self.fy * normalized.y + self.cy,
  )
}

///|
fn focal_scaled(
  value : Double,
  numerator : Double,
  denominator : Double,
) -> Double {
  if value == 0.0 {
    return 0.0
  }
  let ratio = numerator / denominator
  if @types.is_finite(ratio) && ratio > 0.0 {
    value * ratio
  } else {
    // A finite equal-focal ratio must be formed before multiplying, otherwise
    // value/very-small-focal can overflow even when the final result is small.
    // When the ratio itself is unrepresentable, the divided-first order may
    // still succeed; the caller rejects any remaining non-finite result.
    value / denominator * numerator
  }
}

///|
fn CameraModel::inverse_pixel_jacobian(
  self : CameraModel,
  normalized : @geometry.Point2,
) -> Array[Double] raise @types.GroundError {
  let forward = @brown.distortion_jacobian(normalized, self.distortion)
  let a = forward.m00
  let b = forward.m01
  let c = forward.m10
  let d = forward.m11
  let scale = a.abs().max(b.abs()).max(c.abs()).max(d.abs())
  if !@types.is_finite(scale) ||
    !@types.is_finite(a) ||
    !@types.is_finite(b) ||
    !@types.is_finite(c) ||
    !@types.is_finite(d) ||
    a <= 0.0 ||
    d <= 0.0 ||
    scale <= 0.0 {
    raise @types.InvalidInput(
      "camera inverse Jacobian is not finite positive definite",
    )
  }
  let determinant = a / scale * (d / scale) - b / scale * (c / scale)
  if !@types.is_finite(determinant) || determinant <= 0.0 {
    raise @types.InvalidInput("camera inverse Jacobian is singular")
  }
  let inverse = [
    d / scale / determinant / scale,
    -(b / scale) / determinant / scale,
    -(c / scale) / determinant / scale,
    a / scale / determinant / scale,
  ]
  let result = [
    inverse[0],
    focal_scaled(inverse[1], self.fx, self.fy),
    focal_scaled(inverse[2], self.fy, self.fx),
    inverse[3],
  ]
  for value in result {
    if !@types.is_finite(value) {
      raise @types.InvalidInput("camera pixel Jacobian is not finite")
    }
  }
  result
}

///|
/// Correct one raw pixel using the certified branch. A maximum of 512
/// projected contraction steps is allowed; residual/R/(1-L) must be at most
/// 1e-12. Domain comparisons use the same relative radius scale. Unresolved
/// points raise an error.
pub fn CameraModel::rectify(
  self : CameraModel,
  point : @types.Point2,
) -> @types.Point2 raise @types.GroundError {
  self.rectified_pixel(self.solve_normalized(point))
}

///|
/// Analytic raw-to-rectified pixel Jacobian, row-major two by two.
pub fn CameraModel::jacobian(
  self : CameraModel,
  point : @types.Point2,
) -> Array[Double] raise @types.GroundError {
  self.inverse_pixel_jacobian(self.solve_normalized(point))
}