///|
/// Bridges between the CAD (STEP, millimeters/degrees) and robotics (URDF,
/// meters/radians) worlds, plus centered primitive builders.
///
/// `@step_cad` places `Cuboid` corners at the origin and `Cylinder`/`tube`
/// bases at `Z = 0`, while URDF geometry is centered on its frame. The
/// `centered_*_node` helpers reconcile that difference so a `BodyPart`'s CAD
/// node and its URDF inertia/collision share one center of mass.

///|
/// Converts a CAD RGB color to a URDF RGBA color with an optional alpha.
pub fn rgb_to_rgba(c : @step_cad.Rgb, a~ : Double = 1.0) -> @urdf.Rgba {
  @urdf.Rgba::new(c.r, c.g, c.b, a~)
}

///|
/// Converts a URDF RGBA color to a CAD RGB color, dropping alpha.
pub fn rgba_to_rgb(c : @urdf.Rgba) -> @step_cad.Rgb {
  @step_cad.Rgb::new(c.r, c.g, c.b)
}

///|
/// Converts a URDF rotation matrix (radians) to a CAD rotation matrix (degrees
/// are only applied when a transform is converted; the matrix itself is
/// identical).
pub fn urdf_to_step_matrix(m : @urdf.Matrix3x3) -> @step_cad.Matrix3x3 {
  {
    m00: m.m00,
    m01: m.m01,
    m02: m.m02,
    m10: m.m10,
    m11: m.m11,
    m12: m.m12,
    m20: m.m20,
    m21: m.m21,
    m22: m.m22,
  }
}

///|
/// Converts a CAD rotation matrix to a URDF rotation matrix.
pub fn step_to_urdf_matrix(m : @step_cad.Matrix3x3) -> @urdf.Matrix3x3 {
  {
    m00: m.m00,
    m01: m.m01,
    m02: m.m02,
    m10: m.m10,
    m11: m.m11,
    m12: m.m12,
    m20: m.m20,
    m21: m.m21,
    m22: m.m22,
  }
}

///|
/// Converts a URDF transform (meters) to a CAD transform (millimeters).
pub fn urdf_to_step_transform(t : @urdf.Transform) -> @step_cad.Transform {
  {
    rot: urdf_to_step_matrix(t.rot),
    trans: (t.trans.0 * 1000.0, t.trans.1 * 1000.0, t.trans.2 * 1000.0),
  }
}

///|
/// Converts a CAD transform (millimeters) to a URDF transform (meters).
pub fn step_to_urdf_transform(t : @step_cad.Transform) -> @urdf.Transform {
  {
    rot: step_to_urdf_matrix(t.rot),
    trans: (t.trans.0 / 1000.0, t.trans.1 / 1000.0, t.trans.2 / 1000.0),
  }
}

///|
/// Applies a CAD transform (millimeters, degrees) to a scene node by composing
/// it with the node's own placement and re-deriving translation and extrinsic
/// Euler angles.
pub fn transform_scene_node(
  node : @step_cad.SceneNode,
  xform_mm : @step_cad.Transform,
) -> @step_cad.SceneNode {
  let composed = xform_mm.compose(@step_cad.Transform::from_node(node))
  let (rx, ry, rz) = @step_cad.matrix_to_euler(composed.rot)
  let (tx, ty, tz) = composed.trans
  // SceneNode is read-only outside the step package, so rebuild it from its
  // shape with the composed extrinsic Euler angles and translation.
  node.shape
  .with_name(node.name)
  .with_color(node.color)
  .rotate_x(rx)
  .rotate_y(ry)
  .rotate_z(rz)
  .translate(x=tx, y=ty, z=tz)
}

///|
/// Applies a URDF transform (meters) to a scene node.
pub fn transform_scene_node_urdf(
  node : @step_cad.SceneNode,
  urdf_xform : @urdf.Transform,
) -> @step_cad.SceneNode {
  transform_scene_node(node, urdf_to_step_transform(urdf_xform))
}

///|
/// Builds a cuboid whose geometric center is at the origin.
pub fn centered_cuboid_node(
  sx_mm : Double,
  sy_mm : Double,
  sz_mm : Double,
  name~ : String = "cuboid",
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
) -> @step_cad.SceneNode {
  @step_cad.Cuboid::new(sx_mm, sy_mm, sz_mm)
  .with_name(name)
  .with_color(color)
  .translate(x=-sx_mm / 2.0, y=-sy_mm / 2.0, z=-sz_mm / 2.0)
}

///|
/// Builds a cylinder whose geometric center is at the origin, axis along Z.
pub fn centered_cylinder_node(
  diameter_mm : Double,
  length_mm : Double,
  name~ : String = "cylinder",
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
) -> @step_cad.SceneNode {
  @step_cad.Cylinder::new(diameter_mm, length_mm)
  .with_name(name)
  .with_color(color)
  .translate(z=-length_mm / 2.0)
}

///|
/// Four exact quarter arcs forming a counter-clockwise circle.
///
/// Unlike `@step_cad.tube` (a faceted `ExtrudedProfile`), this keeps the tube's
/// outer and inner walls as true cylindrical surfaces, and it stays in the
/// glTF-safe line/arc extrusion family.
fn circle_arc_segments(diameter_mm : Double) -> Array[@step_cad.ProfileSegment] {
  let r = diameter_mm / 2.0
  let center : (Double, Double) = (0.0, 0.0)
  [
    @step_cad.SegmentArc((r, 0.0), (0.0, r), center),
    @step_cad.SegmentArc((0.0, r), (-r, 0.0), center),
    @step_cad.SegmentArc((-r, 0.0), (0.0, -r), center),
    @step_cad.SegmentArc((0.0, -r), (r, 0.0), center),
  ]
}

///|
/// Builds a hollow tube whose geometric center is at the origin, axis along Z.
///
/// The walls are exact cylindrical surfaces (four-quarter-arc loops), not a
/// faceted polygon.
pub fn centered_tube_node(
  outer_diam_mm : Double,
  inner_diam_mm : Double,
  length_mm : Double,
  name~ : String = "tube",
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
) -> @step_cad.SceneNode {
  let outer = circle_arc_segments(outer_diam_mm)
  let shape = if inner_diam_mm > 0.0 {
    @step_cad.ExtrudedArcProfile::new(outer, length_mm, holes=[
      circle_arc_segments(inner_diam_mm),
    ]).as_shape()
  } else {
    @step_cad.ExtrudedArcProfile::new(outer, length_mm).as_shape()
  }
  shape.with_name(name).with_color(color).translate(z=-length_mm / 2.0)
}

///|
/// Builds an extruded profile whose geometric center is at the origin along Z.
pub fn centered_extruded_profile_node(
  outer : Array[(Double, Double)],
  height_mm : Double,
  holes~ : Array[Array[(Double, Double)]] = [],
  name~ : String = "extrusion",
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
) -> @step_cad.SceneNode {
  @step_cad.ExtrudedProfile::new(outer, height_mm, holes~)
  .with_name(name)
  .with_color(color)
  .translate(z=-height_mm / 2.0)
}

///|
/// Builds a URDF transform from a millimeter/degree origin.
pub fn urdf_transform_from_mm_deg(
  x_mm~ : Double = 0.0,
  y_mm~ : Double = 0.0,
  z_mm~ : Double = 0.0,
  rx_deg~ : Double = 0.0,
  ry_deg~ : Double = 0.0,
  rz_deg~ : Double = 0.0,
) -> @urdf.Transform {
  @urdf.Transform::from_origin(
    @urdf.Origin::from_mm_deg(x=x_mm, y=y_mm, z=z_mm, rx_deg~, ry_deg~, rz_deg~),
  )
}