///|
/// Warps an entire image using a 3x3 homography matrix with bilinear interpolation.
pub fn warp_perspective(
  src : Image,
  h_mat : Matrix3x3,
  dst_width : Int,
  dst_height : Int,
  border_value? : Int = 0,
) -> Image {
  let res = Image::new(dst_width, dst_height)
  match h_mat.inverse() {
    Some(h_inv) => {
      for y in 0..= 0.0 &&
            src_pt.x < src.width.to_double() &&
            src_pt.y >= 0.0 &&
            src_pt.y < src.height.to_double() {
            let px = src.sample_bilinear(src_pt).round().to_int()
            res.set(x, y, px)
          } else {
            res.set(x, y, border_value)
          }
        }
      }
      res
    }
    None => res
  }
}

///|
/// Warps a 24-bit color image using a 3x3 homography matrix.
pub fn warp_perspective_color(
  src : ColorImage,
  h_mat : Matrix3x3,
  dst_width : Int,
  dst_height : Int,
) -> ColorImage {
  let res = ColorImage::new(dst_width, dst_height)
  match h_mat.inverse() {
    Some(h_inv) => {
      for y in 0..= 0.0 &&
            src_pt.x < src.width.to_double() &&
            src_pt.y >= 0.0 &&
            src_pt.y < src.height.to_double() {
            let sx = src_pt.x.floor().to_int()
            let sy = src_pt.y.floor().to_int()
            let col = src.get(sx, sy)
            res.set(x, y, col)
          }
        }
      }
      res
    }
    None => res
  }
}

///|
/// Inverse Perspective Mapping (IPM): Projects camera perspective view to an orthographic top-down ground plane.
pub fn inverse_perspective_mapping(
  img : Image,
  ground_corners : Quad,
  output_size : Size2D,
) -> Image? {
  let dst_quad = Quad::new(
    Point2D::new(0.0, 0.0),
    Point2D::new(output_size.width.to_double(), 0.0),
    Point2D::new(output_size.width.to_double(), output_size.height.to_double()),
    Point2D::new(0.0, output_size.height.to_double()),
  )

  match find_homography(ground_corners.corners, dst_quad.corners) {
    Some(h) =>
      Some(warp_perspective(img, h, output_size.width, output_size.height))
    None => None
  }
}