///|
/// Represents a 4x4 transformation matrix in homogeneous coordinates (OpenGL/DirectX/Unity convention).
pub(all) struct Matrix4x4 {
  m00 : Double
  m01 : Double
  m02 : Double
  m03 : Double
  m10 : Double
  m11 : Double
  m12 : Double
  m13 : Double
  m20 : Double
  m21 : Double
  m22 : Double
  m23 : Double
  m30 : Double
  m31 : Double
  m32 : Double
  m33 : Double
} derive(Debug, Eq)

///|
/// Creates a Matrix4x4 from a 6DOF Pose3D (Rodrigues rotation + translation).
pub fn Matrix4x4::from_pose(pose : Pose3D) -> Matrix4x4 {
  let rot = rvec_to_rotation_matrix(pose.rvec)
  let t = pose.tvec
  {
    m00: rot.get(0, 0),
    m01: rot.get(0, 1),
    m02: rot.get(0, 2),
    m03: t.x,
    m10: rot.get(1, 0),
    m11: rot.get(1, 1),
    m12: rot.get(1, 2),
    m13: t.y,
    m20: rot.get(2, 0),
    m21: rot.get(2, 1),
    m22: rot.get(2, 2),
    m23: t.z,
    m30: 0.0,
    m31: 0.0,
    m32: 0.0,
    m33: 1.0,
  }
}

///|
/// Converts an OpenCV right-handed camera pose (X-right, Y-down, Z-forward) to Unity left-handed pose (X-right, Y-up, Z-forward).
pub fn convert_opencv_pose_to_unity(pose : Pose3D) -> Pose3D {
  let rvec_unity = Point3D::new(-pose.rvec.x, pose.rvec.y, -pose.rvec.z)
  let tvec_unity = Point3D::new(pose.tvec.x, -pose.tvec.y, pose.tvec.z)
  Pose3D::new(rvec_unity, tvec_unity)
}

///|
/// Exports detected marker 3D bounding cubes to Wavefront OBJ format string for 3D visualization in Blender/MeshLab.
pub fn export_markers_to_obj(
  detections : Array[MarkerDetection],
  marker_length : Double,
) -> String {
  let buf = StringBuilder()
  buf.write_string("# MoonBit Fiducial Marker 3D Scene Export\n")
  buf.write_string("# Total Markers: \{detections.length()}\n\n")

  let half_s = marker_length * 0.5

  for i in 0.. {
        let rot = rvec_to_rotation_matrix(pose.rvec)
        let t = pose.tvec

        buf.write_string("o Marker_\{det.id}\n")

        // 8 cube vertices
        let local_corners = [
          Point3D::new(-half_s, half_s, 0.0),
          Point3D::new(half_s, half_s, 0.0),
          Point3D::new(half_s, -half_s, 0.0),
          Point3D::new(-half_s, -half_s, 0.0),
          Point3D::new(-half_s, half_s, half_s),
          Point3D::new(half_s, half_s, half_s),
          Point3D::new(half_s, -half_s, half_s),
          Point3D::new(-half_s, -half_s, half_s),
        ]

        for p in local_corners {
          let p_rot = rot.mul_vec(p)
          let p_world = p_rot.add(t)
          buf.write_string("v \{p_world.x} \{p_world.y} \{p_world.z}\n")
        }

        let base_v = i * 8 + 1
        // Base quad face
        buf.write_string(
          "f \{base_v} \{base_v + 1} \{base_v + 2} \{base_v + 3}\n",
        )
        // Top quad face
        buf.write_string(
          "f \{base_v + 4} \{base_v + 5} \{base_v + 6} \{base_v + 7}\n\n",
        )
      }
      None => ()
    }
  }

  buf.to_string()
}