///|
pub struct CameraModel {
  fx : Int
  fy : Int
  cx : Int
  cy : Int
  scale : Int
} derive(Debug, Eq)

///|
pub fn CameraModel::new(
  fx~ : Int,
  fy~ : Int,
  cx~ : Int,
  cy~ : Int,
  scale? : Int = 1000,
) -> CameraModel {
  { fx, fy, cx, cy, scale: max_i(1, scale) }
}

///|
pub fn CameraModel::project(
  self : CameraModel,
  point : Point,
  depth : Int,
) -> Point {
  let z = max_i(1, depth)
  Point::new(
    x=self.cx + point.x * self.fx / z,
    y=self.cy + point.y * self.fy / z,
  )
}

///|
pub fn CameraModel::unproject(
  self : CameraModel,
  pixel : Point,
  depth : Int,
) -> Point {
  let z = max_i(1, depth)
  Point::new(
    x=(pixel.x - self.cx) * z / max_i(1, self.fx),
    y=(pixel.y - self.cy) * z / max_i(1, self.fy),
  )
}

///|
pub fn CameraModel::scale_to(self : CameraModel, size : Size) -> CameraModel {
  let sx = size.width * self.scale / max_i(1, self.cx * 2)
  let sy = size.height * self.scale / max_i(1, self.cy * 2)
  {
    fx: self.fx * sx / self.scale,
    fy: self.fy * sy / self.scale,
    cx: size.width / 2,
    cy: size.height / 2,
    scale: self.scale,
  }
}

///|
pub struct RadialDistortion {
  k1_per_100k : Int
  k2_per_100k : Int
  center : Point
  radius : Int
} derive(Debug, Eq)

///|
pub fn RadialDistortion::new(
  k1_per_100k~ : Int,
  k2_per_100k~ : Int,
  center~ : Point,
  radius~ : Int,
) -> RadialDistortion {
  { k1_per_100k, k2_per_100k, center, radius: max_i(1, radius) }
}

///|
pub fn RadialDistortion::distort(
  self : RadialDistortion,
  point : Point,
) -> Point {
  let dx = point.x - self.center.x
  let dy = point.y - self.center.y
  let r2 = (dx * dx + dy * dy) * 100000 / (self.radius * self.radius)
  let quadratic = r2 * r2 / 100000
  let factor = 100000 +
    self.k1_per_100k * r2 / 100000 +
    self.k2_per_100k * quadratic / 100000
  Point::new(
    x=self.center.x + dx * factor / 100000,
    y=self.center.y + dy * factor / 100000,
  )
}

///|
pub fn RadialDistortion::undistort(
  self : RadialDistortion,
  point : Point,
) -> Point {
  let mut estimate = point
  for _ in 0..<4 {
    let projected = self.distort(estimate)
    estimate = estimate.translate(point.x - projected.x, point.y - projected.y)
  }
  estimate
}

///|
pub fn GrayImage::apply_radial_distortion(
  self : GrayImage,
  distortion : RadialDistortion,
) -> GrayImage {
  let out = GrayImage::new(width=self.width, height=self.height)
  for y in 0.. Array[Point] {
  let out : Array[Point] = []
  for point in points {
    out.push(camera.project(point, depth))
  }
  out
}

///|
pub fn project_rect(camera : CameraModel, rect : Rect, depth : Int) -> Rect {
  let top_left = camera.project(Point::new(x=rect.x, y=rect.y), depth)
  let bottom_right = camera.project(
    Point::new(x=rect.right(), y=rect.bottom()),
    depth,
  )
  Rect::new(
    x=top_left.x,
    y=top_left.y,
    width=bottom_right.x - top_left.x,
    height=bottom_right.y - top_left.y,
  )
}

///|
pub fn project_boxes(
  camera : CameraModel,
  boxes : Array[Box2D],
  depth : Int,
  size : Size,
) -> Array[Box2D] {
  let out : Array[Box2D] = []
  for box in boxes {
    out.push(
      Box2D::new(
        label=box.label,
        rect=clamp_rect(project_rect(camera, box.rect, depth), size),
        occluded=box.occluded,
      ),
    )
  }
  out
}

///|
pub fn calibration_points(
  cols : Int,
  rows : Int,
  spacing : Int,
  origin : Point,
) -> Array[Point] {
  let out : Array[Point] = []
  let width = max_i(0, cols)
  let height = max_i(0, rows)
  for y in 0.. Array[Point] {
  let out : Array[Point] = []
  for y in 0.. SceneOutput {
  let projected = project_boxes(camera, scene.boxes, depth, scene.image.size())
  SceneOutput::new(
    plan=scene.plan,
    image=scene.image,
    mask=scene.mask,
    boxes=projected,
    flow=scene.flow,
  )
}