///|
/// Converts Rodrigues rotation vector (rvec) to 3x3 Rotation Matrix using Rodrigues formula.
pub fn rvec_to_rotation_matrix(rvec : Point3D) -> Matrix3x3 {
  let theta = (rvec.x * rvec.x + rvec.y * rvec.y + rvec.z * rvec.z).sqrt()
  if theta < 1.0e-12 {
    return Matrix3x3::identity()
  }

  let kx = rvec.x / theta
  let ky = rvec.y / theta
  let kz = rvec.z / theta

  let cos_t = @math.cos(theta)
  let sin_t = @math.sin(theta)
  let omc = 1.0 - cos_t

  let r00 = cos_t + kx * kx * omc
  let r01 = kx * ky * omc - kz * sin_t
  let r02 = kx * kz * omc + ky * sin_t

  let r10 = ky * kx * omc + kz * sin_t
  let r11 = cos_t + ky * ky * omc
  let r12 = ky * kz * omc - kx * sin_t

  let r20 = kz * kx * omc - ky * sin_t
  let r21 = kz * ky * omc + kx * sin_t
  let r22 = cos_t + kz * kz * omc

  Matrix3x3::from_array([r00, r01, r02, r10, r11, r12, r20, r21, r22])
}

///|
/// Converts 3x3 Rotation Matrix to Rodrigues rotation vector (rvec).
pub fn rotation_matrix_to_rvec(rot : Matrix3x3) -> Point3D {
  let trace = rot.get(0, 0) + rot.get(1, 1) + rot.get(2, 2)
  let cos_t = (trace - 1.0) / 2.0
  let clamped_cos = if cos_t > 1.0 {
    1.0
  } else if cos_t < -1.0 {
    -1.0
  } else {
    cos_t
  }
  let theta = @math.acos(clamped_cos)

  if theta < 1.0e-6 {
    return { x: 0.0, y: 0.0, z: 0.0 }
  }

  let sin_t = @math.sin(theta)
  if sin_t.abs() < 1.0e-6 {
    return { x: theta, y: 0.0, z: 0.0 }
  }

  let rx = (rot.get(2, 1) - rot.get(1, 2)) / (2.0 * sin_t) * theta
  let ry = (rot.get(0, 2) - rot.get(2, 0)) / (2.0 * sin_t) * theta
  let rz = (rot.get(1, 0) - rot.get(0, 1)) / (2.0 * sin_t) * theta

  { x: rx, y: ry, z: rz }
}

///|
/// Projects a 3D point in world/marker space onto 2D image pixel coordinates with Brown-Conrady lens distortion.
pub fn project_3d_point(
  pt : Point3D,
  pose : Pose3D,
  camera : CameraIntrinsics,
) -> Point2D {
  let rot = rvec_to_rotation_matrix(pose.rvec)
  let pt_cam = rot.mul_vec(pt)

  let xc = pt_cam.x + pose.tvec.x
  let yc = pt_cam.y + pose.tvec.y
  let zc = pt_cam.z + pose.tvec.z

  if zc.abs() < 1.0e-12 {
    return { x: camera.cx, y: camera.cy }
  }

  let x = xc / zc
  let y = yc / zc

  // Brown-Conrady radial and tangential distortion
  let r2 = x * x + y * y
  let r4 = r2 * r2
  let r6 = r4 * r2

  let radial = 1.0 + camera.k1 * r2 + camera.k2 * r4 + camera.k3 * r6
  let x_dist = x * radial +
    2.0 * camera.p1 * x * y +
    camera.p2 * (r2 + 2.0 * x * x)
  let y_dist = y * radial +
    camera.p1 * (r2 + 2.0 * y * y) +
    2.0 * camera.p2 * x * y

  let px = camera.fx * x_dist + camera.cx
  let py = camera.fy * y_dist + camera.cy

  { x: px, y: py }
}

///|
/// Estimates 3D Pose (rvec, tvec) of a planar marker from 2D quad corners and camera intrinsics using DLT Homography decomposition.
pub fn estimate_marker_pose(
  quad : Quad,
  camera : CameraIntrinsics,
  marker_length : Double,
) -> Pose3D? {
  if quad.corners.length() != 4 || marker_length <= 0.0 {
    return None
  }

  let half_s = marker_length / 2.0
  let obj_pts = [
    // The camera/image convention in this package is X-right, Y-down.
    { x: -half_s, y: -half_s },
    { x: half_s, y: -half_s },
    { x: half_s, y: half_s },
    { x: -half_s, y: half_s },
  ]

  let norm_img_pts = Array::new()
  for i in 0..<4 {
    let px = quad.corners[i].x
    let py = quad.corners[i].y
    let u = (px - camera.cx) / camera.fx
    let v = (py - camera.cy) / camera.fy
    norm_img_pts.push({ x: u, y: v })
  }

  match find_homography(obj_pts, norm_img_pts) {
    Some(h) => {
      let h1_x = h.get(0, 0)
      let h1_y = h.get(1, 0)
      let h1_z = h.get(2, 0)
      let norm1 = (h1_x * h1_x + h1_y * h1_y + h1_z * h1_z).sqrt()

      if norm1 < 1.0e-12 {
        return None
      }

      let lambda = 1.0 / norm1

      let r00 = h.get(0, 0) * lambda
      let r10 = h.get(1, 0) * lambda
      let r20 = h.get(2, 0) * lambda

      let r01 = h.get(0, 1) * lambda
      let r11 = h.get(1, 1) * lambda
      let r21 = h.get(2, 1) * lambda

      let r02 = r10 * r21 - r20 * r11
      let r12 = r20 * r01 - r00 * r21
      let r22 = r00 * r11 - r10 * r01

      let rot_mat = Matrix3x3::from_array([
        r00, r01, r02, r10, r11, r12, r20, r21, r22,
      ])

      let rvec = rotation_matrix_to_rvec(rot_mat)
      let tvec = {
        x: h.get(0, 2) * lambda,
        y: h.get(1, 2) * lambda,
        z: h.get(2, 2) * lambda,
      }

      Some({ rvec, tvec })
    }
    None => None
  }
}