///|
/// A physical solid inside a component.
///
/// A `BodyPart` carries the exact CAD solid (`node`, millimeters/degrees) and
/// the matching URDF mass properties (`inertial`, meters/radians) so the same
/// geometry feeds both the STEP exporter and the robot simulator. Collisions
/// are cheap analytical primitives in the same local frame.
pub(all) struct BodyPart {
  name : String
  node : @step_cad.SceneNode
  density : @urdf.Density
  inertial : @urdf.Inertial
  collisions : Array[@urdf.Collision]
  /// Purchasing/manufacturing metadata for this solid.
  part : PartMeta?
}

///|
/// Builds a rectangular box body from millimeter dimensions and a
/// millimeter/degree pose.
pub fn BodyPart::box_mm(
  name : String,
  sx_mm : Double,
  sy_mm : Double,
  sz_mm : Double,
  density~ : @urdf.Density = @urdf.Density::aluminum_6061(),
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
  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,
) -> BodyPart {
  let origin = @urdf.Origin::from_mm_deg(
    x=x_mm,
    y=y_mm,
    z=z_mm,
    rx_deg~,
    ry_deg~,
    rz_deg~,
  )
  let node = transform_scene_node_urdf(
    centered_cuboid_node(sx_mm, sy_mm, sz_mm, name~, color~),
    @urdf.Transform::from_origin(origin),
  )
  let inertial = @urdf.Inertial::from_box_density(
    sx_mm / 1000.0,
    sy_mm / 1000.0,
    sz_mm / 1000.0,
    density~,
    origin~,
  )
  let collision = @urdf.Collision::new(
    @urdf.Geometry::box_mm(sx_mm, sy_mm, sz_mm),
    name=Some(name),
    origin~,
  )
  { name, node, density, inertial, collisions: [collision], part: None, }
}

///|
/// Builds a solid cylinder body, axis along Z, centered on its own frame.
pub fn BodyPart::cylinder_mm(
  name : String,
  diameter_mm : Double,
  length_mm : Double,
  density~ : @urdf.Density = @urdf.Density::aluminum_6061(),
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
  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,
) -> BodyPart {
  let origin = @urdf.Origin::from_mm_deg(
    x=x_mm,
    y=y_mm,
    z=z_mm,
    rx_deg~,
    ry_deg~,
    rz_deg~,
  )
  let node = transform_scene_node_urdf(
    centered_cylinder_node(diameter_mm, length_mm, name~, color~),
    @urdf.Transform::from_origin(origin),
  )
  let inertial = @urdf.Inertial::from_cylinder_density(
    diameter_mm / 2000.0,
    length_mm / 1000.0,
    density~,
    origin~,
  )
  let collision = @urdf.Collision::new(
    @urdf.Geometry::cylinder_mm(diameter_mm / 2.0, length_mm),
    name=Some(name),
    origin~,
  )
  { name, node, density, inertial, collisions: [collision], part: None, }
}

///|
/// Builds a hollow tube body, axis along Z, centered on its own frame.
pub fn BodyPart::tube_mm(
  name : String,
  outer_diam_mm : Double,
  inner_diam_mm : Double,
  length_mm : Double,
  density~ : @urdf.Density = @urdf.Density::aluminum_6061(),
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
  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,
) -> BodyPart {
  let origin = @urdf.Origin::from_mm_deg(
    x=x_mm,
    y=y_mm,
    z=z_mm,
    rx_deg~,
    ry_deg~,
    rz_deg~,
  )
  let node = transform_scene_node_urdf(
    centered_tube_node(outer_diam_mm, inner_diam_mm, length_mm, name~, color~),
    @urdf.Transform::from_origin(origin),
  )
  let inertial = @urdf.Inertial::from_tube_density(
    inner_diam_mm / 2000.0,
    outer_diam_mm / 2000.0,
    length_mm / 1000.0,
    density~,
    origin~,
  )
  let collision = @urdf.Collision::new(
    @urdf.Geometry::cylinder_mm(outer_diam_mm / 2.0, length_mm),
    name=Some(name),
    origin~,
  )
  { name, node, density, inertial, collisions: [collision], part: None, }
}

///|
/// Builds an extruded-profile body. Its mass comes from the true net profile
/// area; its inertia and collision use the profile's bounding box.
pub fn BodyPart::extruded_profile_mm(
  name : String,
  outer : Array[(Double, Double)],
  height_mm : Double,
  holes~ : Array[Array[(Double, Double)]] = [],
  density~ : @urdf.Density = @urdf.Density::aluminum_6061(),
  color~ : @step_cad.Rgb = @step_cad.Rgb::grey(),
  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,
) -> BodyPart {
  let origin = @urdf.Origin::from_mm_deg(
    x=x_mm,
    y=y_mm,
    z=z_mm,
    rx_deg~,
    ry_deg~,
    rz_deg~,
  )
  let node = transform_scene_node_urdf(
    centered_extruded_profile_node(outer, height_mm, holes~, name~, color~),
    @urdf.Transform::from_origin(origin),
  )
  // Net cross-sectional area in square millimeters.
  let mut area_mm2 = @step_cad.polygon_area_2d(outer).abs()
  for hole in holes {
    area_mm2 = area_mm2 - @step_cad.polygon_area_2d(hole).abs()
  }
  let mass = area_mm2 / 1.0e6 * (height_mm / 1000.0) * density.to_kg_per_m3()
  // Bounding-box extents of the profile.
  let mut min_x = outer[0].0
  let mut max_x = outer[0].0
  let mut min_y = outer[0].1
  let mut max_y = outer[0].1
  for pt in outer {
    min_x = if pt.0 < min_x { pt.0 } else { min_x }
    max_x = if pt.0 > max_x { pt.0 } else { max_x }
    min_y = if pt.1 < min_y { pt.1 } else { min_y }
    max_y = if pt.1 > max_y { pt.1 } else { max_y }
  }
  let dx = max_x - min_x
  let dy = max_y - min_y
  let inertial = @urdf.Inertial::from_box(
    dx / 1000.0,
    dy / 1000.0,
    height_mm / 1000.0,
    mass~,
    origin~,
  )
  let collision = @urdf.Collision::new(
    @urdf.Geometry::box_mm(dx, dy, height_mm),
    name=Some(name),
    origin~,
  )
  { name, node, density, inertial, collisions: [collision], part: None, }
}

///|
/// Builds a body from a pre-existing scene node and inertia.
pub fn BodyPart::custom(
  name : String,
  node : @step_cad.SceneNode,
  inertial : @urdf.Inertial,
  collisions~ : Array[@urdf.Collision] = [],
  density~ : @urdf.Density = @urdf.Density::aluminum_6061(),
  part~ : PartMeta? = None,
) -> BodyPart {
  { name, node, density, inertial, collisions, part, }
}

///|
/// Rigidly transforms a body (CAD node, inertia, and collision frames).
pub fn BodyPart::transform_by(
  self : BodyPart,
  urdf_xform : @urdf.Transform,
) -> BodyPart {
  let node = transform_scene_node_urdf(self.node, urdf_xform)
  let inertial = self.inertial.transform_by(urdf_xform)
  let collisions = self.collisions.map(fn(c) {
    let new_origin = urdf_xform
      .compose(@urdf.Transform::from_origin(c.origin))
      .to_origin()
    { ..c, origin: new_origin, }
  })
  { ..self, node, inertial, collisions, }
}

///|
/// Attaches purchasing/manufacturing metadata to this body.
pub fn BodyPart::with_part(self : BodyPart, part : PartMeta) -> BodyPart {
  { ..self, part: Some(part), }
}

///|
/// How an attached child component connects to its parent.
#derive(Debug)
pub(all) enum ConnectionKind {
  MergeRigid
  Fixed
  Revolute(@urdf.JointLimit)
  Continuous(@urdf.JointLimit?)
  Prismatic(@urdf.JointLimit)
}

///|
/// The kinematic parameters of an attachment plus the shared geometric mate
/// pair.
///
/// The pair owns the parent connector name, child connector name, offset, twist,
/// flip, and fit. `JointSpec` keeps only what is kinematic: the joint kind,
/// limits, dynamics, mimic, and transmission reduction.
#derive(Debug)
pub(all) struct JointSpec {
  kind : ConnectionKind
  pair : @step_common.MatePair
  dynamics : @urdf.JointDynamics?
  mimic : @urdf.JointMimic?
  transmission_reduction : Double?
}

///|
/// joint_pair builds the pair template used by the `JointSpec` constructors.
/// The parent and child names are filled in when the attachment is created.
fn joint_pair(
  offset_mm : Double,
  angle_deg : Double,
  flip_axis : Bool,
) -> @step_common.MatePair {
  @step_common.MatePair::new(
    parent="",
    child="",
    offset_mm~,
    twist_deg=angle_deg,
    flip_axis~,
  )
}

///|
/// Folds the child's bodies directly into the parent link: no joint is emitted.
pub fn JointSpec::merge_rigid(
  offset_mm~ : Double = 0.0,
  angle_deg~ : Double = 0.0,
  flip_axis~ : Bool = false,
) -> JointSpec {
  {
    kind: MergeRigid,
    pair: joint_pair(offset_mm, angle_deg, flip_axis),
    dynamics: None,
    mimic: None,
    transmission_reduction: None,
  }
}

///|
/// Creates a fixed kinematic joint.
pub fn JointSpec::fixed(
  offset_mm~ : Double = 0.0,
  angle_deg~ : Double = 0.0,
  flip_axis~ : Bool = false,
) -> JointSpec {
  {
    kind: Fixed,
    pair: joint_pair(offset_mm, angle_deg, flip_axis),
    dynamics: None,
    mimic: None,
    transmission_reduction: None,
  }
}

///|
/// Creates a revolute joint with the given limit.
pub fn JointSpec::revolute(
  limit : @urdf.JointLimit,
  offset_mm~ : Double = 0.0,
  angle_deg~ : Double = 0.0,
  flip_axis~ : Bool = false,
  dynamics~ : @urdf.JointDynamics? = None,
  mimic~ : @urdf.JointMimic? = None,
  transmission_reduction~ : Double? = None,
) -> JointSpec {
  {
    kind: Revolute(limit),
    pair: joint_pair(offset_mm, angle_deg, flip_axis),
    dynamics,
    mimic,
    transmission_reduction,
  }
}

///|
/// Creates a continuous joint with an optional limit.
pub fn JointSpec::continuous(
  limit~ : @urdf.JointLimit? = None,
  offset_mm~ : Double = 0.0,
  angle_deg~ : Double = 0.0,
  flip_axis~ : Bool = false,
  dynamics~ : @urdf.JointDynamics? = None,
  mimic~ : @urdf.JointMimic? = None,
  transmission_reduction~ : Double? = None,
) -> JointSpec {
  {
    kind: Continuous(limit),
    pair: joint_pair(offset_mm, angle_deg, flip_axis),
    dynamics,
    mimic,
    transmission_reduction,
  }
}

///|
/// Creates a prismatic joint with the given limit.
pub fn JointSpec::prismatic(
  limit : @urdf.JointLimit,
  offset_mm~ : Double = 0.0,
  angle_deg~ : Double = 0.0,
  flip_axis~ : Bool = false,
  dynamics~ : @urdf.JointDynamics? = None,
  mimic~ : @urdf.JointMimic? = None,
  transmission_reduction~ : Double? = None,
) -> JointSpec {
  {
    kind: Prismatic(limit),
    pair: joint_pair(offset_mm, angle_deg, flip_axis),
    dynamics,
    mimic,
    transmission_reduction,
  }
}

///|
/// A child component snapped onto one of its parent's mates.
pub(all) struct Attachment {
  joint_name : String
  child : Component
  /// The resolved relationship: parent/child connector names, offset, twist,
  /// flip, and fit all live in the shared pair inside `spec`.
  spec : JointSpec
  /// False for children that belong to the parent as a purchased/assembled
  /// item (e.g. a motor rotor), so the BOM does not count them separately.
  bom_external : Bool
}

///|
/// A reusable, parameterized CAD + robotics building block.
///
/// Bodies provide B-Rep geometry and mass properties; mates are named frames
/// other components snap onto; attachments hold the child `Component` tree.
pub(all) struct Component {
  name : String
  description : String
  parameters : Map[String, String]
  bodies : Array[BodyPart]
  mates : Map[String, @step_common.MateConnector]
  attachments : Array[Attachment]
  /// Optional name for the exported robot/design when the root link name
  /// differs from the assembly name (e.g. `base_link` inside a Galileo robot).
  robot_name : String?
  /// Purchasing/manufacturing metadata when the whole component is one part.
  part : PartMeta?
  /// Typed electrical/mechanical/fluid/sensor interfaces.
  ports : Array[Port]
  /// Fastener schedule entries for this component.
  fasteners : Array[Fastener]
  /// Explicit fit records (nominal and allowed range).
  fits : Array[Fit]
  /// Electrical ratings when this component is an actuator.
  actuator : ActuatorSpec?
  /// Measuring characteristics when this component is a sensor.
  sensor : SensorSpec?
  /// PCB envelope metadata when this component is a circuit board.
  pcb : PcbSpec?
  /// Point-to-point cable runs belonging to this component.
  wiring : Array[WireRun]
  /// Joint-to-actuator control bindings declared on this component.
  control : Array[ControlBinding]
  /// Routed cable bundles belonging to this component.
  cables : Array[CableRoute]
}

///|
/// Creates an empty component with a default `"origin"` mate.
pub fn Component::new(name : String, description~ : String = "") -> Component {
  let mates : Map[String, @step_common.MateConnector] = Map([])
  mates.set("origin", @step_common.MateConnector::new("origin"))
  {
    name,
    description,
    parameters: Map([]),
    bodies: [],
    mates,
    attachments: [],
    robot_name: None,
    part: None,
    ports: [],
    fasteners: [],
    fits: [],
    actuator: None,
    sensor: None,
    pcb: None,
    wiring: [],
    control: [],
    cables: [],
  }
}

///|
/// Returns the name used for the exported robot and design: the explicit robot
/// name when set, otherwise the root component name.
pub fn Component::display_name(self : Component) -> String {
  self.robot_name.unwrap_or(self.name)
}

///|
/// Sets the exported robot/design name independently of the root link name.
pub fn Component::with_robot_name(self : Component, name : String) -> Component {
  { ..self, robot_name: Some(name), }
}

///|
/// Replaces the component description.
pub fn Component::with_description(
  self : Component,
  description : String,
) -> Component {
  { ..self, description, }
}

///|
/// Records a design parameter for documentation and STEP headers.
pub fn Component::with_param(
  self : Component,
  key : String,
  value : String,
) -> Component {
  self.parameters.set(key, value)
  self
}

///|
/// Appends a body to this component.
pub fn Component::add_body(self : Component, body : BodyPart) -> Component {
  self.bodies.push(body)
  self
}

///|
/// Appends several bodies to this component.
pub fn Component::add_bodies(
  self : Component,
  bodies : Array[BodyPart],
) -> Component {
  for body in bodies {
    self.bodies.push(body)
  }
  self
}

///|
/// Registers a mate connector on this component.
pub fn Component::add_mate(
  self : Component,
  mate : @step_common.MateConnector,
) -> Component {
  self.mates.set(mate.name, mate)
  self
}

///|
/// Registers a mate from millimeter/degree inputs.
pub fn Component::add_mate_mm_deg(
  self : Component,
  name : String,
  x~ : Double = 0.0,
  y~ : Double = 0.0,
  z~ : Double = 0.0,
  rx_deg~ : Double = 0.0,
  ry_deg~ : Double = 0.0,
  rz_deg~ : Double = 0.0,
  axis~ : (Double, Double, Double) = (0.0, 0.0, 1.0),
) -> Component {
  self.add_mate(
    @step_common.MateConnector::from_mm_deg(
      name,
      x~,
      y~,
      z~,
      rx_deg~,
      ry_deg~,
      rz_deg~,
      axis~,
    ),
  )
}

///|
/// Looks up a mate connector by name.
pub fn Component::get_mate(
  self : Component,
  name : String,
) -> @step_common.MateConnector? {
  self.mates.get(name)
}

///|
/// Attaches a child component onto a parent mate.
pub fn Component::attach(
  self : Component,
  joint_name : String,
  parent_mate~ : String,
  child : Component,
  child_mate~ : String = "origin",
  spec~ : JointSpec = JointSpec::fixed(),
  bom_external~ : Bool = true,
) -> Component {
  let pair = @step_common.MatePair::new(
    parent=parent_mate,
    child=child_mate,
    offset_mm=spec.pair.offset_mm,
    twist_deg=spec.pair.twist_deg,
    flip_axis=spec.pair.flip_axis,
    fit=spec.pair.fit,
    description=spec.pair.description,
  )
  let spec = { ..spec, pair, }
  self.attachments.push({ joint_name, child, spec, bom_external, })
  self
}

///|
/// Attaches a child to a named component anywhere in this component's tree.
///
/// The tree's attachment arrays are shared, so this lets an assembly builder
/// reach a nested link (for example a motor's rotor) without rebuilding the
/// intermediate components.
pub fn Component::attach_to(
  self : Component,
  target_name : String,
  joint_name : String,
  parent_mate~ : String,
  child : Component,
  child_mate~ : String = "origin",
  spec~ : JointSpec = JointSpec::fixed(),
  bom_external~ : Bool = true,
) -> Component {
  if self.name == target_name {
    return self.attach(
      joint_name,
      parent_mate~,
      child,
      child_mate~,
      spec~,
      bom_external~,
    )
  }
  for i in 0.. Double {
  let mut mass = 0.0
  for body in self.bodies {
    mass = mass + body.inertial.mass
  }
  for att in self.attachments {
    mass = mass + att.child.total_mass_kg()
  }
  mass
}