///|
/// Represents a 2D point in image coordinates with double precision.
pub(all) struct Point2D {
  x : Double
  y : Double
} derive(Debug, Eq, Default)

///|
/// Creates a new Point2D with given coordinates.
pub fn Point2D::new(x : Double, y : Double) -> Point2D {
  { x, y }
}

///|
/// Computes Euclidean distance between two 2D points.
pub fn Point2D::distance(self : Point2D, other : Point2D) -> Double {
  let dx = self.x - other.x
  let dy = self.y - other.y
  (dx * dx + dy * dy).sqrt()
}

///|
/// Computes squared Euclidean distance between two 2D points.
pub fn Point2D::distance_sq(self : Point2D, other : Point2D) -> Double {
  let dx = self.x - other.x
  let dy = self.y - other.y
  dx * dx + dy * dy
}

///|
/// Computes vector addition of two 2D points.
pub fn Point2D::add(self : Point2D, other : Point2D) -> Point2D {
  { x: self.x + other.x, y: self.y + other.y }
}

///|
/// Computes vector subtraction of two 2D points.
pub fn Point2D::sub(self : Point2D, other : Point2D) -> Point2D {
  { x: self.x - other.x, y: self.y - other.y }
}

///|
/// Scales a 2D point by a scalar factor.
pub fn Point2D::scale(self : Point2D, s : Double) -> Point2D {
  { x: self.x * s, y: self.y * s }
}

///|
/// Computes the 2D dot product of two points treated as vectors.
pub fn Point2D::dot(self : Point2D, other : Point2D) -> Double {
  self.x * other.x + self.y * other.y
}

///|
/// Computes the 2D cross product (determinant) of two vectors.
pub fn Point2D::cross(self : Point2D, other : Point2D) -> Double {
  self.x * other.y - self.y * other.x
}

///|
/// Computes vector magnitude (length).
pub fn Point2D::norm(self : Point2D) -> Double {
  (self.x * self.x + self.y * self.y).sqrt()
}

///|
/// Returns a normalized unit vector, or (0, 0) if norm is near zero.
pub fn Point2D::normalized(self : Point2D) -> Point2D {
  let n = self.norm()
  if n > 1.0e-12 {
    { x: self.x / n, y: self.y / n }
  } else {
    { x: 0.0, y: 0.0 }
  }
}

///|
/// Linear interpolation between two 2D points.
pub fn Point2D::lerp(self : Point2D, other : Point2D, t : Double) -> Point2D {
  { x: self.x + (other.x - self.x) * t, y: self.y + (other.y - self.y) * t }
}

///|
/// Represents a 3D point in world/camera coordinates.
pub(all) struct Point3D {
  x : Double
  y : Double
  z : Double
} derive(Debug, Eq, Default)

///|
/// Creates a new Point3D with given coordinates.
pub fn Point3D::new(x : Double, y : Double, z : Double) -> Point3D {
  { x, y, z }
}

///|
/// Computes Euclidean distance between two 3D points.
pub fn Point3D::distance(self : Point3D, other : Point3D) -> Double {
  let dx = self.x - other.x
  let dy = self.y - other.y
  let dz = self.z - other.z
  (dx * dx + dy * dy + dz * dz).sqrt()
}

///|
/// Computes squared Euclidean distance between two 3D points.
pub fn Point3D::distance_sq(self : Point3D, other : Point3D) -> Double {
  let dx = self.x - other.x
  let dy = self.y - other.y
  let dz = self.z - other.z
  dx * dx + dy * dy + dz * dz
}

///|
/// Vector addition of two 3D points.
pub fn Point3D::add(self : Point3D, other : Point3D) -> Point3D {
  { x: self.x + other.x, y: self.y + other.y, z: self.z + other.z }
}

///|
/// Vector subtraction of two 3D points.
pub fn Point3D::sub(self : Point3D, other : Point3D) -> Point3D {
  { x: self.x - other.x, y: self.y - other.y, z: self.z - other.z }
}

///|
/// Scalar multiplication for 3D points.
pub fn Point3D::scale(self : Point3D, s : Double) -> Point3D {
  { x: self.x * s, y: self.y * s, z: self.z * s }
}

///|
/// Computes 3D dot product.
pub fn Point3D::dot(self : Point3D, other : Point3D) -> Double {
  self.x * other.x + self.y * other.y + self.z * other.z
}

///|
/// Computes 3D cross product.
pub fn Point3D::cross(self : Point3D, other : Point3D) -> Point3D {
  {
    x: self.y * other.z - self.z * other.y,
    y: self.z * other.x - self.x * other.z,
    z: self.x * other.y - self.y * other.x,
  }
}

///|
/// Computes vector norm of a 3D point.
pub fn Point3D::norm(self : Point3D) -> Double {
  (self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
}

///|
/// Normalizes a 3D vector.
pub fn Point3D::normalized(self : Point3D) -> Point3D {
  let n = self.norm()
  if n > 1.0e-12 {
    { x: self.x / n, y: self.y / n, z: self.z / n }
  } else {
    { x: 0.0, y: 0.0, z: 0.0 }
  }
}

///|
/// Linear interpolation between two 3D points.
pub fn Point3D::lerp(self : Point3D, other : Point3D, t : Double) -> Point3D {
  {
    x: self.x + (other.x - self.x) * t,
    y: self.y + (other.y - self.y) * t,
    z: self.z + (other.z - self.z) * t,
  }
}

///|
/// Represents image dimensions.
pub(all) struct Size2D {
  width : Int
  height : Int
} derive(Debug, Eq, Default)

///|
/// Creates a new Size2D with width and height.
pub fn Size2D::new(width : Int, height : Int) -> Size2D {
  { width, height }
}

///|
/// Represents a 2D bounding box (axis-aligned).
pub(all) struct Rect2D {
  x : Double
  y : Double
  width : Double
  height : Double
} derive(Debug, Eq, Default)

///|
/// Creates a new axis-aligned bounding box.
pub fn Rect2D::new(
  x : Double,
  y : Double,
  width : Double,
  height : Double,
) -> Rect2D {
  { x, y, width, height }
}

///|
/// Computes bounding box area.
pub fn Rect2D::area(self : Rect2D) -> Double {
  if self.width > 0.0 && self.height > 0.0 {
    self.width * self.height
  } else {
    0.0
  }
}

///|
/// Checks if a 2D point is inside the bounding box.
pub fn Rect2D::contains(self : Rect2D, p : Point2D) -> Bool {
  p.x >= self.x &&
  p.x <= self.x + self.width &&
  p.y >= self.y &&
  p.y <= self.y + self.height
}

///|
/// Computes Intersection over Union (IoU) of two bounding boxes.
pub fn Rect2D::iou(self : Rect2D, other : Rect2D) -> Double {
  let x1 = if self.x > other.x { self.x } else { other.x }
  let y1 = if self.y > other.y { self.y } else { other.y }
  let x2 = if self.x + self.width < other.x + other.width {
    self.x + self.width
  } else {
    other.x + other.width
  }
  let y2 = if self.y + self.height < other.y + other.height {
    self.y + self.height
  } else {
    other.y + other.height
  }
  let inter_w = if x2 - x1 > 0.0 { x2 - x1 } else { 0.0 }
  let inter_h = if y2 - y1 > 0.0 { y2 - y1 } else { 0.0 }
  let inter_area = inter_w * inter_h
  let union_area = self.area() + other.area() - inter_area
  if union_area > 1.0e-12 {
    inter_area / union_area
  } else {
    0.0
  }
}

///|
/// Represents a candidate quadrilateral with 4 ordered corners (TL, TR, BR, BL).
pub(all) struct Quad {
  corners : Array[Point2D]
} derive(Debug, Eq)

///|
/// Creates a Quad from 4 points.
pub fn Quad::new(
  p0 : Point2D,
  p1 : Point2D,
  p2 : Point2D,
  p3 : Point2D,
) -> Quad {
  { corners: [p0, p1, p2, p3] }
}

///|
/// Computes the perimeter length of the quadrilateral.
pub fn Quad::perimeter(self : Quad) -> Double {
  if self.corners.length() != 4 {
    return 0.0
  }
  let p0 = self.corners[0]
  let p1 = self.corners[1]
  let p2 = self.corners[2]
  let p3 = self.corners[3]
  p0.distance(p1) + p1.distance(p2) + p2.distance(p3) + p3.distance(p0)
}

///|
/// Computes the geometric area of the quadrilateral using Shoelace formula.
pub fn Quad::area(self : Quad) -> Double {
  if self.corners.length() != 4 {
    return 0.0
  }
  let p0 = self.corners[0]
  let p1 = self.corners[1]
  let p2 = self.corners[2]
  let p3 = self.corners[3]
  let val = p0.x * (p1.y - p3.y) +
    p1.x * (p2.y - p0.y) +
    p2.x * (p3.y - p1.y) +
    p3.x * (p0.y - p2.y)
  let a = val.abs() * 0.5
  a
}

///|
/// Computes the centroid center of the quad.
pub fn Quad::center(self : Quad) -> Point2D {
  if self.corners.length() != 4 {
    return { x: 0.0, y: 0.0 }
  }
  let mut cx = 0.0
  let mut cy = 0.0
  for p in self.corners {
    cx += p.x
    cy += p.y
  }
  { x: cx / 4.0, y: cy / 4.0 }
}

///|
/// Computes the bounding rect enclosing this quadrilateral.
pub fn Quad::bounding_rect(self : Quad) -> Rect2D {
  if self.corners.length() == 0 {
    return { x: 0.0, y: 0.0, width: 0.0, height: 0.0 }
  }
  let mut min_x = self.corners[0].x
  let mut max_x = self.corners[0].x
  let mut min_y = self.corners[0].y
  let mut max_y = self.corners[0].y
  for p in self.corners {
    if p.x < min_x {
      min_x = p.x
    }
    if p.x > max_x {
      max_x = p.x
    }
    if p.y < min_y {
      min_y = p.y
    }
    if p.y > max_y {
      max_y = p.y
    }
  }
  { x: min_x, y: min_y, width: max_x - min_x, height: max_y - min_y }
}

///|
/// Represents a 3x3 matrix in row-major order.
pub(all) struct Matrix3x3 {
  data : Array[Double]
} derive(Debug, Eq)

///|
/// Camera intrinsic parameters (Pinhole camera model with Brown-Conrady distortion).
pub(all) struct CameraIntrinsics {
  fx : Double
  fy : Double
  cx : Double
  cy : Double
  k1 : Double
  k2 : Double
  p1 : Double
  p2 : Double
  k3 : Double
} derive(Debug, Eq)

///|
/// Creates standard pinhole camera intrinsics without distortion.
pub fn CameraIntrinsics::new(
  fx : Double,
  fy : Double,
  cx : Double,
  cy : Double,
) -> CameraIntrinsics {
  { fx, fy, cx, cy, k1: 0.0, k2: 0.0, p1: 0.0, p2: 0.0, k3: 0.0 }
}

///|
/// Creates full pinhole camera intrinsics with radial & tangential distortion.
pub fn CameraIntrinsics::with_distortion(
  fx : Double,
  fy : Double,
  cx : Double,
  cy : Double,
  k1 : Double,
  k2 : Double,
  p1 : Double,
  p2 : Double,
  k3 : Double,
) -> CameraIntrinsics {
  { fx, fy, cx, cy, k1, k2, p1, p2, k3 }
}

///|
/// Represents 6-DOF camera pose with rotation vector (Rodrigues) and translation vector.
pub(all) struct Pose3D {
  rvec : Point3D
  tvec : Point3D
} derive(Debug, Eq, Default)

///|
/// Creates a Pose3D from rvec and tvec.
pub fn Pose3D::new(rvec : Point3D, tvec : Point3D) -> Pose3D {
  { rvec, tvec }
}

///|
/// Definition of a fiducial marker dictionary.
pub(all) struct MarkerDictionary {
  name : String
  grid_size : Int
  payload_bits : Int
  max_correction_bits : Int
  codewords : Array[Int64]
} derive(Debug, Eq)

///|
/// Represents a successfully decoded fiducial marker.
pub(all) struct MarkerDetection {
  id : Int
  quad : Quad
  pose : Pose3D?
  rotation_index : Int
  hamming_distance : Int
} derive(Debug, Eq)

///|
/// Represents full detection configuration parameters.
pub(all) struct DetectorParameters {
  adaptive_thresh_win_size_min : Int
  adaptive_thresh_win_size_max : Int
  adaptive_thresh_win_size_step : Int
  adaptive_thresh_constant : Int
  min_marker_perimeter_rate : Double
  max_marker_perimeter_rate : Double
  polygonal_approx_accuracy_rate : Double
  min_corner_distance_rate : Double
  min_distance_to_border : Int
  min_marker_distance_rate : Double
  corner_refinement_method : Int
  corner_refinement_win_size : Int
  corner_refinement_max_iterations : Int
  corner_refinement_min_accuracy : Double
  perspective_remove_pixel_per_cell : Int
  perspective_remove_ignored_margin_per_cell : Double
  max_erroneous_bits_in_border_rate : Double
  min_otsu_std_dev : Double
  error_correction_rate : Double
} derive(Debug, Eq)

///|
/// Creates default detector configuration parameters.
pub fn DetectorParameters::default() -> DetectorParameters {
  {
    adaptive_thresh_win_size_min: 3,
    adaptive_thresh_win_size_max: 23,
    adaptive_thresh_win_size_step: 10,
    adaptive_thresh_constant: 7,
    min_marker_perimeter_rate: 0.03,
    max_marker_perimeter_rate: 4.0,
    polygonal_approx_accuracy_rate: 0.03,
    min_corner_distance_rate: 0.05,
    min_distance_to_border: 3,
    min_marker_distance_rate: 0.05,
    corner_refinement_method: 1, // 0: None, 1: Subpix / Inverted Hessian, 2: Contour
    corner_refinement_win_size: 5,
    corner_refinement_max_iterations: 30,
    corner_refinement_min_accuracy: 0.01,
    perspective_remove_pixel_per_cell: 8,
    perspective_remove_ignored_margin_per_cell: 0.13,
    max_erroneous_bits_in_border_rate: 0.35,
    min_otsu_std_dev: 5.0,
    error_correction_rate: 0.6,
  }
}