///|
/// MuJoCo MJCF serialization.

///|
/// One actuator declaration for the MJCF `` section.
///
/// The tuple fields are `(joint, name, ...)`; use the snake-case constructors
/// (`MjcfActuator::motor`, `::position`, `::velocity`) for readable call sites.
#derive(Debug, Eq)
pub(all) enum MjcfActuator {
  /// Torque/force motor: joint, name, ctrl_min, ctrl_max, gear.
  Motor(String, String, Double, Double, Double)
  /// Position servo: joint, name, kp, kv, ctrl_min, ctrl_max, forcerange.
  Position(String, String, Double, Double, Double, Double, Double?)
  /// Velocity servo: joint, name, kv, ctrl_min, ctrl_max, forcerange.
  Velocity(String, String, Double, Double, Double, Double?)
}

///|
pub fn MjcfActuator::motor(
  joint~ : String,
  name~ : String = "",
  ctrl_min~ : Double,
  ctrl_max~ : Double,
  gear~ : Double = 1.0,
) -> MjcfActuator {
  Motor(joint, name, ctrl_min, ctrl_max, gear)
}

///|
pub fn MjcfActuator::position(
  joint~ : String,
  name~ : String = "",
  kp~ : Double,
  kv~ : Double,
  ctrl_min~ : Double,
  ctrl_max~ : Double,
  forcerange? : Double,
) -> MjcfActuator {
  Position(joint, name, kp, kv, ctrl_min, ctrl_max, forcerange)
}

///|
pub fn MjcfActuator::velocity(
  joint~ : String,
  name~ : String = "",
  kv~ : Double,
  ctrl_min~ : Double,
  ctrl_max~ : Double,
  forcerange? : Double,
) -> MjcfActuator {
  Velocity(joint, name, kv, ctrl_min, ctrl_max, forcerange)
}

///|
/// Simulation-oriented MJCF options.
///
/// The defaults reproduce the original exporter's structure; the CAD compiler
/// fills these in for a standing-pose, contact-clean RL model.
pub(all) struct MjcfOptions {
  mesh_dir : String
  include_floor : Bool
  /// World Z lift for the root free body. Computed from the home pose's
  /// lowest collision geometry when `None` and `home_qpos` is non-empty.
  root_z_m : Double?
  /// Home/standing joint positions (joint name -> radians or metres).
  home_qpos : Map[String, Double]
  /// Explicit actuators. Empty keeps the legacy "motor for every limited joint"
  /// behavior.
  actuators : Array[MjcfActuator]
  /// Joints to omit from the model (welded bodies), e.g. cosmetic rotors.
  freeze_joints : Array[String]
  /// Default joint damping (N*m*s/rad) and reflected armature (kg*m^2).
  default_damping : Double
  default_armature : Double
  /// Emit `` pairs for adjacent and loop-constrained bodies.
  exclude_adjacent_contacts : Bool
}

///|
pub fn MjcfOptions::new(
  mesh_dir~ : String = "meshes",
  include_floor~ : Bool = true,
  root_z_m~ : Double? = None,
  home_qpos~ : Map[String, Double] = Map([]),
  actuators~ : Array[MjcfActuator] = [],
  freeze_joints~ : Array[String] = [],
  default_damping~ : Double = 0.0,
  default_armature~ : Double = 0.0,
  exclude_adjacent_contacts~ : Bool = false,
) -> MjcfOptions {
  {
    mesh_dir,
    include_floor,
    root_z_m,
    home_qpos,
    actuators,
    freeze_joints,
    default_damping,
    default_armature,
    exclude_adjacent_contacts,
  }
}

///|
priv struct MjcfAssets {
  meshes : Map[String, String]
  materials : Map[String, String]
}

///|
fn mjcf_quat(rpy : Vec3) -> String {
  let (roll, pitch, yaw) = rpy
  let cr = @math.cos(roll * 0.5)
  let sr = @math.sin(roll * 0.5)
  let cp = @math.cos(pitch * 0.5)
  let sp = @math.sin(pitch * 0.5)
  let cy = @math.cos(yaw * 0.5)
  let sy = @math.sin(yaw * 0.5)
  let w = cr * cp * cy + sr * sp * sy
  let x = sr * cp * cy - cr * sp * sy
  let y = cr * sp * cy + sr * cp * sy
  let z = cr * cp * sy - sr * sp * cy
  "\{fmt_num(w)} \{fmt_num(x)} \{fmt_num(y)} \{fmt_num(z)}"
}

///|
fn mjcf_basename(path : String) -> String {
  match path.rev_find("/") {
    Some(index) =>
      String::unsafe_substring(path, start=index + 1, end=path.length())
    None => path
  }
}

///|
fn mjcf_mesh_name(path : String) -> String {
  let base = mjcf_basename(path)
  base
  .replace_all(old="\\", new="_")
  .replace_all(old=".", new="_")
  .replace_all(old="-", new="_")
  .replace_all(old=" ", new="_")
}

///|
fn mjcf_name(name : String) -> String {
  name
  .replace_all(old="/", new="_")
  .replace_all(old="\\", new="_")
  .replace_all(old=".", new="_")
  .replace_all(old="-", new="_")
  .replace_all(old=" ", new="_")
}

///|
fn mjcf_sorted_keys(map : Map[String, String]) -> Array[String] {
  let keys : Array[String] = []
  for key, _ in map {
    keys.push(key)
  }
  keys.sort_by(fn(a : String, b : String) -> Int32 {
    if a < b {
      -1
    } else if a > b {
      1
    } else {
      0
    }
  })
  keys
}

///|
fn mjcf_collect_assets(robot : Robot) -> MjcfAssets {
  let assets : MjcfAssets = { meshes: Map([]), materials: Map([]), }
  for material in robot.materials {
    match material.color {
      Some(color) => assets.materials.set(material.name, fmt_rgba(color))
      None => ()
    }
  }
  for link in robot.links {
    // Only the geoms that are actually emitted contribute assets: collisions
    // take precedence over visuals, matching `mjcf_write_body`.
    if link.collisions.length() > 0 {
      for collision in link.collisions {
        match collision.geometry {
          Mesh(path, _) =>
            assets.meshes.set(mjcf_mesh_name(path), mjcf_basename(path))
          _ => ()
        }
      }
    } else {
      for visual in link.visuals {
        match visual.geometry {
          Mesh(path, _) =>
            assets.meshes.set(mjcf_mesh_name(path), mjcf_basename(path))
          _ => ()
        }
        match visual.material {
          Some(material) =>
            match material.color {
              Some(color) =>
                if !assets.materials.contains(material.name) {
                  assets.materials.set(material.name, fmt_rgba(color))
                }
              None => ()
            }
          None => ()
        }
      }
    }
  }
  assets
}

///|
fn mjcf_write_inertial(
  sb : StringBuilder,
  inertial : Inertial,
  level : Int32,
) -> Unit {
  let (x, y, z) = inertial.origin.xyz
  let (roll, pitch, yaw) = inertial.origin.rpy
  pad(sb, level)
  sb.write_string(
    " 1.0e-12 {
      " quat=\"" + mjcf_quat((roll, pitch, yaw)) + "\""
    } else {
      ""
    }) +
    " mass=\"" +
    fmt_num(inertial.mass) +
    "\" fullinertia=\"" +
    fmt_num(inertial.inertia.ixx) +
    " " +
    fmt_num(inertial.inertia.iyy) +
    " " +
    fmt_num(inertial.inertia.izz) +
    " " +
    fmt_num(inertial.inertia.ixy) +
    " " +
    fmt_num(inertial.inertia.ixz) +
    " " +
    fmt_num(inertial.inertia.iyz) +
    "\"/>\n",
  )
}

///|
fn mjcf_is_frozen(options : MjcfOptions, joint : Joint) -> Bool {
  options.freeze_joints.contains(joint.name)
}

///|
fn mjcf_write_joint(
  sb : StringBuilder,
  joint : Joint,
  options : MjcfOptions,
  qpos_names : Array[String],
  level : Int32,
) -> Unit {
  if mjcf_is_frozen(options, joint) {
    return
  }
  match joint.joint_type {
    Fixed => ()
    Floating => {
      pad(sb, level)
      sb.write_string("\n")
    }
    Revolute | Continuous => {
      qpos_names.push(joint.name)
      pad(sb, level)
      sb.write_string(
        "
          match (limit.lower, limit.upper) {
            (Some(lower), Some(upper)) =>
              sb.write_string(
                " range=\"" +
                fmt_num(lower) +
                " " +
                fmt_num(upper) +
                "\" limited=\"true\"",
              )
            _ => sb.write_string(" limited=\"false\"")
          }
        None => sb.write_string(" limited=\"false\"")
      }
      sb.write_string("/>\n")
    }
    Prismatic => {
      qpos_names.push(joint.name)
      pad(sb, level)
      sb.write_string(
        "
          match (limit.lower, limit.upper) {
            (Some(lower), Some(upper)) =>
              sb.write_string(
                " range=\"" +
                fmt_num(lower) +
                " " +
                fmt_num(upper) +
                "\" limited=\"true\"",
              )
            _ => sb.write_string(" limited=\"false\"")
          }
        None => sb.write_string(" limited=\"false\"")
      }
      sb.write_string("/>\n")
    }
    Planar => {
      qpos_names.push(joint.name)
      qpos_names.push(joint.name + "_y")
      pad(sb, level)
      sb.write_string(
        "\n",
      )
      pad(sb, level)
      sb.write_string(
        "\n",
      )
    }
  }
}

///|
/// Contact class for one geom: tires/wheels/feet collide with the floor only;
/// structural parts collide with the floor if the robot falls over; internal
/// actuator hardware (stators, gears, bearings, screws) never collides, so a
/// bulky bounding box inside a wheel cannot lift the robot onto its calf.
fn mjcf_contact_attrs(link_name : String, part_name : String) -> String {
  let part = part_name.to_lower()
  let internal = part.contains("stator") ||
    part.contains("_act_") ||
    part.contains("act_a12") ||
    part.contains("planet") ||
    part.contains("herringbone") ||
    part.contains("ring") ||
    part.contains("sun") ||
    part.contains("gear") ||
    part.contains("moteus") ||
    part.contains("pcb") ||
    part.contains("bearing") ||
    part.contains("_pin") ||
    part.contains("shaft") ||
    part.contains("screw") ||
    part.contains("bolt") ||
    part.contains("iso4762")
  if internal {
    return " contype=\"0\" conaffinity=\"0\""
  }
  let lower = link_name.to_lower()
  if lower.contains("tire") || lower.contains("wheel") || lower.contains("foot") {
    " contype=\"2\" conaffinity=\"1\" friction=\"1.2 0.01 0.001\""
  } else {
    " contype=\"4\" conaffinity=\"1\" friction=\"1.0 0.01 0.001\""
  }
}

///|
fn mjcf_geom_min_z(t : Transform, geometry : Geometry) -> Double? {
  match geometry {
    Box(size) => {
      let (sx, sy, sz) = size
      let xs : Array[Double] = [-sx / 2.0, sx / 2.0]
      let ys : Array[Double] = [-sy / 2.0, sy / 2.0]
      let zs : Array[Double] = [-sz / 2.0, sz / 2.0]
      let mut lowest = 1.0e18
      for x in xs {
        for y in ys {
          for z in zs {
            let p = t.apply_point((x, y, z))
            if p.2 < lowest {
              lowest = p.2
            }
          }
        }
      }
      Some(lowest)
    }
    Cylinder(radius, length) => {
      let axis = t.apply_dir((0.0, 0.0, 1.0))
      let vertical = if axis.2.abs() > 1.0 { 1.0 } else { axis.2.abs() }
      let horizontal = (1.0 - vertical * vertical).sqrt()
      Some(t.trans.2 - (length / 2.0 * vertical + radius * horizontal))
    }
    Sphere(radius) => Some(t.trans.2 - radius)
    Mesh(_, _) => None
  }
}

///|
fn mjcf_min_world_z(robot : Robot, qpos : Map[String, Double]) -> Double? {
  let fk = robot.forward_kinematics(joint_positions=qpos)
  let mut found = false
  let mut lowest = 1.0e18
  for link in robot.links {
    let world = fk.get(link.name).unwrap_or(Transform::identity())
    let geoms : Array[(Origin, Geometry)] = if link.collisions.length() > 0 {
      link.collisions.map(fn(collision) {
        (collision.origin, collision.geometry)
      })
    } else {
      link.visuals.map(fn(visual) { (visual.origin, visual.geometry) })
    }
    for entry in geoms {
      let frame = world.compose(Transform::from_origin(entry.0))
      match mjcf_geom_min_z(frame, entry.1) {
        Some(z) => {
          if z < lowest {
            lowest = z
          }
          found = true
        }
        None => ()
      }
    }
  }
  if found {
    Some(lowest)
  } else {
    None
  }
}

///|
/// Sites used by `` for each loop constraint: the first
/// anchor lives on body1, the second on body2 (both in their link frames).
fn mjcf_loop_sites(robot : Robot) -> Map[String, Array[(String, Vec3)]] {
  let sites : Map[String, Array[(String, Vec3)]] = Map([])
  for constraint in robot.loop_constraints {
    let base = mjcf_name(constraint.name)
    let first = (base + "_site1", constraint.anchor1)
    let second = (base + "_site2", constraint.anchor2)
    match sites.get(constraint.body1) {
      Some(entries) => entries.push(first)
      None => sites.set(constraint.body1, [first])
    }
    match sites.get(constraint.body2) {
      Some(entries) => entries.push(second)
      None => sites.set(constraint.body2, [second])
    }
  }
  sites
}

///|
fn mjcf_pair_key(a : String, b : String) -> String {
  if a < b {
    a + "|" + b
  } else {
    b + "|" + a
  }
}

///|
fn mjcf_add_pair(
  pairs : Array[(String, String)],
  keys : Array[String],
  a : String,
  b : String,
) -> Unit {
  if a == b {
    return
  }
  let key = mjcf_pair_key(a, b)
  if !keys.contains(key) {
    keys.push(key)
    pairs.push((a, b))
  }
}

///|
fn mjcf_contact_pairs(robot : Robot) -> Array[(String, String)] {
  let pairs : Array[(String, String)] = []
  let keys : Array[String] = []
  for joint in robot.joints {
    mjcf_add_pair(pairs, keys, joint.parent, joint.child)
  }
  for constraint in robot.loop_constraints {
    mjcf_add_pair(pairs, keys, constraint.body1, constraint.body2)
  }
  let groups : Map[String, Array[String]] = Map([])
  for joint in robot.joints {
    match groups.get(joint.parent) {
      Some(children) => children.push(joint.child)
      None => groups.set(joint.parent, [joint.child])
    }
  }
  for _parent, children in groups {
    for i in 0.. Unit {
  pad(sb, level)
  sb.write_string(" {
      let (sx, sy, sz) = size
      sb.write_string(
        " type=\"box\" size=\"" +
        fmt_vec3((sx / 2.0, sy / 2.0, sz / 2.0)) +
        "\"",
      )
    }
    Cylinder(radius, length) =>
      sb.write_string(
        " type=\"cylinder\" size=\"" +
        fmt_num(radius) +
        " " +
        fmt_num(length / 2.0) +
        "\"",
      )
    Sphere(radius) =>
      sb.write_string(" type=\"sphere\" size=\"" + fmt_num(radius) + "\"")
    Mesh(path, _) =>
      sb.write_string(
        " type=\"mesh\" mesh=\"" + xml_escape(mjcf_mesh_name(path)) + "\"",
      )
  }
  sb.write_string(" pos=\"" + fmt_vec3(origin.xyz) + "\"")
  let (roll, pitch, yaw) = origin.rpy
  if roll.abs() + pitch.abs() + yaw.abs() > 1.0e-12 {
    sb.write_string(" quat=\"" + mjcf_quat((roll, pitch, yaw)) + "\"")
  }
  match material {
    Some(mat) =>
      if assets.materials.contains(mat.name) {
        sb.write_string(" material=\"" + xml_escape(mat.name) + "\"")
      }
    None => ()
  }
  sb.write_string(mjcf_contact_attrs(link_name, part_name))
  sb.write_string("/>\n")
}

///|
fn mjcf_write_body(
  sb : StringBuilder,
  robot : Robot,
  link : Link,
  incoming_joint : Joint?,
  level : Int32,
  links : Map[String, Link],
  children : Map[String, Array[Joint]],
  assets : MjcfAssets,
  imu_sites : Array[String],
  options : MjcfOptions,
  root_z : Double,
  loop_sites : Map[String, Array[(String, Vec3)]],
  qpos_names : Array[String],
) -> Unit {
  pad(sb, level)
  sb.write_string("
      sb.write_string(
        " pos=\"" +
        fmt_vec3(joint.origin.xyz) +
        "\" quat=\"" +
        mjcf_quat(joint.origin.rpy) +
        "\"",
      )
    None => sb.write_string(" pos=\"0 0 " + fmt_num(root_z) + "\"")
  }
  sb.write_string(">\n")
  match incoming_joint {
    None => {
      pad(sb, level + 1)
      sb.write_string("\n")
    }
    Some(joint) => mjcf_write_joint(sb, joint, options, qpos_names, level + 1)
  }
  match link.inertial {
    Some(inertial) => mjcf_write_inertial(sb, inertial, level + 1)
    None => ()
  }
  // MuJoCo requires unique geom names; link name + ordinal is unique across
  // the model even when a STEP component repeats a layer name.
  let mut geom_index = 0
  if link.collisions.length() > 0 {
    for collision in link.collisions {
      mjcf_write_geom(
        sb,
        link.name + "_geom_" + geom_index.to_string(),
        collision.origin,
        collision.geometry,
        None,
        assets,
        link.name,
        match collision.name {
          Some(value) => value
          None => link.name
        },
        level + 1,
      )
      geom_index = geom_index + 1
    }
  } else {
    for visual in link.visuals {
      mjcf_write_geom(
        sb,
        link.name + "_geom_" + geom_index.to_string(),
        visual.origin,
        visual.geometry,
        visual.material,
        assets,
        link.name,
        link.name,
        level + 1,
      )
      geom_index = geom_index + 1
    }
  }
  if link.name.to_lower().contains("imu") {
    let site = link.name + "_imu"
    pad(sb, level + 1)
    sb.write_string(
      "\n",
    )
    imu_sites.push(site)
  }
  // Loop-constraint sites: `` ties these exact points.
  match loop_sites.get(link.name) {
    Some(entries) =>
      for entry in entries {
        pad(sb, level + 1)
        sb.write_string(
          "\n",
        )
      }
    None => ()
  }
  match children.get(link.name) {
    Some(outgoing) =>
      for joint in outgoing {
        match links.get(joint.child) {
          Some(child_link) =>
            mjcf_write_body(
              sb,
              robot,
              child_link,
              Some(joint),
              level + 1,
              links,
              children,
              assets,
              imu_sites,
              options,
              root_z,
              loop_sites,
              qpos_names,
            )
          None => ()
        }
      }
    None => ()
  }
  pad(sb, level)
  sb.write_string("\n")
}

///|
fn mjcf_forcerange(value : Double?) -> String {
  match value {
    Some(limit) =>
      " forcerange=\"-" + fmt_num(limit) + " " + fmt_num(limit) + "\""
    None => ""
  }
}

///|
fn mjcf_write_actuator(sb : StringBuilder, actuator : MjcfActuator) -> Unit {
  match actuator {
    Motor(joint, name, ctrl_min, ctrl_max, gear) => {
      pad(sb, 2)
      sb.write_string(
        " 1.0e-9 {
          " gear=\"" + fmt_num(gear) + "\""
        } else {
          ""
        }) +
        "/>\n",
      )
    }
    Position(joint, name, kp, kv, ctrl_min, ctrl_max, forcerange) => {
      pad(sb, 2)
      sb.write_string(
        "\n",
      )
    }
    Velocity(joint, name, kv, ctrl_min, ctrl_max, forcerange) => {
      pad(sb, 2)
      sb.write_string(
        "\n",
      )
    }
  }
}

///|
fn mjcf_write_actuators(
  sb : StringBuilder,
  robot : Robot,
  options : MjcfOptions,
) -> Unit {
  let explicit = options.actuators
  let legacy : Array[Joint] = []
  if explicit.length() == 0 {
    for joint in robot.joints {
      let actuated = match joint.joint_type {
        Revolute | Continuous | Prismatic => true
        _ => false
      }
      if actuated && !mjcf_is_frozen(options, joint) {
        match joint.limit {
          Some(limit) => if limit.effort > 0.0 { legacy.push(joint) }
          None => ()
        }
      }
    }
  }
  if explicit.length() == 0 && legacy.length() == 0 {
    return
  }
  pad(sb, 1)
  sb.write_string("\n")
  if explicit.length() > 0 {
    for actuator in explicit {
      mjcf_write_actuator(sb, actuator)
    }
  } else {
    for joint in legacy {
      let effort = match joint.limit {
        Some(limit) => limit.effort
        None => 0.0
      }
      mjcf_write_actuator(
        sb,
        MjcfActuator::motor(joint=joint.name, ctrl_min=-effort, ctrl_max=effort),
      )
    }
  }
  pad(sb, 1)
  sb.write_string("\n")
}

///|
/// Converts the robot to MuJoCo MJCF XML with simulation options.
pub fn Robot::to_mjcf_with_options(
  self : Robot,
  options : MjcfOptions,
) -> String {
  let root_z = match options.root_z_m {
    Some(z) => z
    None =>
      if options.home_qpos.length() > 0 {
        match mjcf_min_world_z(self, options.home_qpos) {
          Some(lowest) => if lowest < 0.0 { -lowest + 0.004 } else { 0.0 }
          None => 0.0
        }
      } else {
        0.0
      }
  }
  let sb = StringBuilder()
  sb.write_string("\n")
  sb.write_string("\n")
  pad(sb, 1)
  sb.write_string(
    "\n",
  )
  pad(sb, 1)
  sb.write_string("\n")
  sb.to_string()
}

///|
/// Converts the robot to MuJoCo MJCF XML with the legacy defaults.
///
/// The root link is floating (``), collisions are used as geoms
/// when present (visuals otherwise), and every `LoopConstraint` becomes a
/// native `` between two site frames (`site1`/`site2`)
/// placed at the constraint anchors.
pub fn Robot::to_mjcf_string(
  self : Robot,
  mesh_dir~ : String = "meshes",
  include_floor~ : Bool = true,
) -> String {
  self.to_mjcf_with_options(MjcfOptions::new(mesh_dir~, include_floor~))
}