///|
fn normalize_matrix(
  values : Array[Double],
) -> Array[Double] raise @types.GroundError {
  if values.length() != 9 {
    raise @types.GroundError::InvalidInput(
      "homography needs 9 row-major values",
    )
  }
  let mut magnitude = 0.0
  for value in values {
    if !@types.is_finite(value) {
      raise @types.GroundError::InvalidInput("homography values must be finite")
    }
    magnitude = magnitude.max(value.abs())
  }
  if magnitude == 0.0 {
    raise @types.GroundError::InvalidInput("homography must be nonzero")
  }
  let normalized = values.map(fn(value) { value / magnitude })
  let determinant = normalized[0] *
    (normalized[4] * normalized[8] - normalized[5] * normalized[7]) -
    normalized[1] *
    (normalized[3] * normalized[8] - normalized[5] * normalized[6]) +
    normalized[2] *
    (normalized[3] * normalized[7] - normalized[4] * normalized[6])
  if !@types.is_finite(determinant) || determinant == 0.0 {
    raise @types.GroundError::InvalidInput("homography is singular")
  }
  normalized
}

///|
fn map_unchecked(
  h : Array[Double],
  point : @types.Point2,
) -> @types.Point2 raise @types.GroundError {
  let x = h[0] * point.x + h[1] * point.y + h[2]
  let y = h[3] * point.x + h[4] * point.y + h[5]
  let w = h[6] * point.x + h[7] * point.y + h[8]
  if !@types.is_finite(x) ||
    !@types.is_finite(y) ||
    !@types.is_finite(w) ||
    w == 0.0 {
    raise @types.GroundError::InvalidInput(
      "homogeneous projection is not finite",
    )
  }
  @types.Point2::new(x / w, y / w)
}

///|
fn jacobian_unchecked(
  h : Array[Double],
  point : @types.Point2,
) -> Array[Double] raise @types.GroundError {
  let mapped = map_unchecked(h, point)
  let w = h[6] * point.x + h[7] * point.y + h[8]
  let j = [
    (h[0] - h[6] * mapped.x) / w,
    (h[1] - h[7] * mapped.x) / w,
    (h[3] - h[6] * mapped.y) / w,
    (h[4] - h[7] * mapped.y) / w,
  ]
  for value in j {
    if !@types.is_finite(value) {
      raise @types.GroundError::InvalidInput(
        "projection Jacobian is not finite",
      )
    }
  }
  j
}

///|
/// Stable largest singular value of a row-major two by two matrix.
fn largest_singular_value(j : Array[Double]) -> Double {
  let mut scale = 0.0
  for value in j {
    scale = scale.max(value.abs())
  }
  if scale == 0.0 {
    return 0.0
  }
  let a = j[0] / scale
  let b = j[1] / scale
  let c = j[2] / scale
  let d = j[3] / scale
  let first = a * a + b * b
  let second = c * c + d * d
  let off_diagonal = a * c + b * d
  let discriminant = ((first - second) * (first - second) +
  4.0 * off_diagonal * off_diagonal).sqrt()
  scale * ((first + second + discriminant) / 2.0).sqrt()
}

///|
fn check_jacobian(
  j : Array[Double],
  limit : Double,
) -> Unit raise @types.GroundError {
  let scale = largest_singular_value(j)
  if !@types.is_finite(scale) || scale <= 0.0 || scale > limit {
    raise @types.GroundError::InvalidInput(
      "projection exceeds metres-per-pixel limit",
    )
  }
  let determinant = j[0] / scale * (j[3] / scale) -
    j[1] / scale * (j[2] / scale)
  if !@types.is_finite(determinant) || determinant.abs() <= 1.0e-12 {
    raise @types.GroundError::InvalidInput("projection is locally degenerate")
  }
}

///|
fn validate_mapping_domain(
  h : Array[Double],
  domain : ConvexRoi,
  maximum_scale : Double,
) -> Double raise @types.GroundError {
  let first_w = h[6] * domain.vertices[0].x + h[7] * domain.vertices[0].y + h[8]
  let mut min_w = first_w.abs()
  let mut max_w = first_w.abs()
  let numerators = [0.0, 0.0, 0.0, 0.0]
  for point in domain.vertices {
    let w = h[6] * point.x + h[7] * point.y + h[8]
    if !@types.is_finite(w) || w == 0.0 || (w > 0.0) != (first_w > 0.0) {
      raise @types.GroundError::InvalidInput(
        "ROI intersects projective horizon",
      )
    }
    min_w = min_w.min(w.abs())
    max_w = max_w.max(w.abs())
    check_jacobian(jacobian_unchecked(h, point), maximum_scale)
    let n = [
      (h[0] * h[7] - h[6] * h[1]) * point.y + h[0] * h[8] - h[6] * h[2],
      (h[1] * h[6] - h[7] * h[0]) * point.x + h[1] * h[8] - h[7] * h[2],
      (h[3] * h[7] - h[6] * h[4]) * point.y + h[3] * h[8] - h[6] * h[5],
      (h[4] * h[6] - h[7] * h[3]) * point.x + h[4] * h[8] - h[7] * h[5],
    ]
    for i in 0..<4 {
      if !@types.is_finite(n[i]) {
        raise @types.GroundError::InvalidInput(
          "ROI mapping bound is not finite",
        )
      }
      numerators[i] = numerators[i].max(n[i].abs())
    }
  }
  if min_w / max_w < 1.0e-9 {
    raise @types.GroundError::InvalidInput(
      "ROI is too close to projective horizon",
    )
  }
  // Numerators are affine and w has one sign on a convex polygon, so vertex
  // extrema provide a conservative bound everywhere in the ROI. Divide twice
  // to avoid squaring a small homogeneous denominator prematurely.
  let bound = largest_singular_value(
    numerators.map(fn(value) { value / min_w / min_w }),
  )
  if !@types.is_finite(bound) || bound > maximum_scale {
    raise @types.GroundError::InvalidInput("ROI exceeds metres-per-pixel limit")
  }
  bound
}

///|
/// Import a pixel-to-metre matrix. The ROI must be a simple strictly convex
/// polygon. Its boundary is included; mappings outside it are rejected.
pub fn GroundCalibration::from_homography(
  matrix : Array[Double],
  roi : Array[@types.Point2],
  max_meters_per_pixel~ : Double = 0.5,
  ground_noise_floor_m~ : Double = 0.001,
) -> GroundCalibration raise @types.GroundError {
  if !@types.is_finite(max_meters_per_pixel) || max_meters_per_pixel <= 0.0 {
    raise @types.GroundError::InvalidInput(
      "metres-per-pixel limit must be finite and positive",
    )
  }
  let variance = ground_noise_floor_m * ground_noise_floor_m
  if !@types.is_finite(ground_noise_floor_m) ||
    ground_noise_floor_m <= 0.0 ||
    !@types.is_finite(variance) ||
    variance <= 0.0 {
    raise @types.GroundError::InvalidInput(
      "ground noise floor must have finite positive variance",
    )
  }
  let coefficients = normalize_matrix(matrix)
  let domain = make_roi(roi)
  let bound = validate_mapping_domain(
    coefficients, domain, max_meters_per_pixel,
  )
  {
    coefficients,
    domain,
    maximum_scale: max_meters_per_pixel,
    noise_floor_variance: variance,
    report_value: {
      fit_count_value: 0,
      check_count_value: 0,
      fit_rms_value: None,
      fit_max_value: None,
      check_rms_value: None,
      check_max_value: None,
      roi_scale_bound_value: bound,
    },
  }
}

///|
/// Map one already-undistorted pixel point within the calibrated ROI.
pub fn GroundCalibration::map_point(
  self : GroundCalibration,
  point : @types.Point2,
) -> @types.Point2 raise @types.GroundError {
  require_domain_point(self.domain, point)
  map_unchecked(self.coefficients, point)
}

///|
/// Analytic row-major [dX/dx, dX/dy, dY/dx, dY/dy], in metres per pixel.
pub fn GroundCalibration::jacobian(
  self : GroundCalibration,
  point : @types.Point2,
) -> Array[Double] raise @types.GroundError {
  require_domain_point(self.domain, point)
  let j = jacobian_unchecked(self.coefficients, point)
  check_jacobian(j, self.maximum_scale)
  j
}

///|
/// Propagate pixel covariance as J Sigma J^T and add the configured isotropic
/// ground noise floor. This does not estimate calibration-parameter error.
pub fn GroundCalibration::project(
  self : GroundCalibration,
  point : @types.Point2,
  covariance : @types.Covariance2,
) -> GroundPoint raise @types.GroundError {
  covariance.validate()
  let position = self.map_point(point)
  let j = self.jacobian(point)
  let a = j[0]
  let b = j[1]
  let c = j[2]
  let d = j[3]
  let t00 = a * covariance.xx + b * covariance.xy
  let t01 = a * covariance.xy + b * covariance.yy
  let t10 = c * covariance.xx + d * covariance.xy
  let t11 = c * covariance.xy + d * covariance.yy
  for value in [t00, t01, t10, t11] {
    if !@types.is_finite(value) {
      raise @types.GroundError::InvalidInput(
        "covariance propagation overflowed",
      )
    }
  }
  let xx = t00 * a + t01 * b + self.noise_floor_variance
  let xy = (t00 * c + t01 * d) / 2.0 + (t10 * a + t11 * b) / 2.0
  let yy = t10 * c + t11 * d + self.noise_floor_variance
  {
    position_value: position,
    covariance_value: @types.Covariance2::new(xx, xy, yy),
  }
}