///|
/// Control export: joint-to-actuator bindings, `ros2_control` configuration,
/// a firmware pin map, and consistency checks.

///|
/// Maps one commandable robot joint onto an actuator, bus channel, and physical
/// pins, with direction, gear ratio, soft limits, and homing/limit inputs.
pub(all) struct ControlBinding {
  /// Joint name as it appears in the compiled robot.
  joint : String
  /// Actuator component name that drives the joint.
  actuator : String
  /// Bus or interface channel, e.g. `"can0:1"` or `"step_dir:0"`.
  channel : String
  /// Free-form pin assignment, e.g. `"STEP=PA0; DIR=PA1; EN=PA2"`.
  mcu_pins : String
  /// `"normal"` or `"inverted"`.
  direction : String
  /// Motor revolutions per joint revolution.
  gear_ratio : Double
  soft_lower : Double?
  soft_upper : Double?
  /// Input that homes this joint, e.g. `"limit_home"`.
  homing_input : String?
  /// Input that ends travel at the far end, e.g. `"limit_closed"`.
  limit_input : String?
}

///|
pub fn ControlBinding::new(
  joint : String,
  actuator~ : String = "",
  channel~ : String = "",
  mcu_pins~ : String = "",
  direction~ : String = "normal",
  gear_ratio~ : Double = 1.0,
  soft_lower~ : Double? = None,
  soft_upper~ : Double? = None,
  homing_input~ : String? = None,
  limit_input~ : String? = None,
) -> ControlBinding {
  {
    joint,
    actuator,
    channel,
    mcu_pins,
    direction,
    gear_ratio,
    soft_lower,
    soft_upper,
    homing_input,
    limit_input,
  }
}

///|
/// All control bindings declared anywhere in this component tree.
pub fn Component::control_bindings(self : Component) -> Array[ControlBinding] {
  let bindings : Array[ControlBinding] = []
  self.collect_control_bindings(bindings)
  bindings
}

///|
fn Component::collect_control_bindings(
  self : Component,
  bindings : Array[ControlBinding],
) -> Unit {
  for binding in self.control {
    bindings.push(binding)
  }
  for att in self.attachments {
    att.child.collect_control_bindings(bindings)
  }
}

///|
/// Adds a joint-to-actuator binding.
pub fn Component::add_control_binding(
  self : Component,
  binding : ControlBinding,
) -> Component {
  self.control.push(binding)
  self
}

///|
fn yaml_quote(value : String) -> String {
  "\"" +
  value.replace(old="\\", new="\\\\").replace(old="\"", new="\\\"") +
  "\""
}

///|
fn yaml_number(value : Double?) -> String {
  match value {
    Some(number) => number.to_string()
    None => "null"
  }
}

///|
fn yaml_string(value : String?) -> String {
  match value {
    Some(text) => yaml_quote(text)
    None => "null"
  }
}

///|
/// Renders a `ros2_control` configuration mapping every declared binding.
///
/// The controller manager exposes a joint-state broadcaster and a joint
/// trajectory controller over the bound joints.  The hardware section carries
/// the actuator, channel, pins, direction, gear ratio, soft limits, homing and
/// limit inputs for each joint, plus the sensor interfaces.
pub fn Component::to_ros2_control_yaml(
  self : Component,
  controller_name~ : String = "joint_trajectory_controller",
  update_rate_hz~ : Double = 100.0,
) -> String {
  let bindings = self.control_bindings()
  let sensors = self.power_budget().sensors
  let mut out = "# Generated by gmlewis/cad (ros2_control)\n"
  out = out + "controller_manager:\n  ros__parameters:\n"
  out = out + "    update_rate: " + update_rate_hz.to_string() + "\n\n"
  out = out + "    joint_state_broadcaster:\n"
  out = out + "      type: joint_state_broadcaster/JointStateBroadcaster\n\n"
  out = out +
    "    " +
    controller_name +
    ":\n      type: joint_trajectory_controller/JointTrajectoryController\n\n"
  out = out + controller_name + ":\n  ros__parameters:\n    joints:\n"
  if bindings.length() == 0 {
    out = out + "      []\n"
  } else {
    for binding in bindings {
      out = out + "      - " + yaml_quote(binding.joint) + "\n"
    }
  }
  out = out + "    command_interfaces:\n      - position\n"
  out = out + "    state_interfaces:\n      - position\n      - velocity\n"
  out = out + "    allow_partial_joints_goal: false\n\n"
  out = out + "moonbit_robot_hardware:\n  ros__parameters:\n    joints:\n"
  if bindings.length() == 0 {
    out = out + "      []\n"
  } else {
    for binding in bindings {
      out = out + "      - " + yaml_quote(binding.joint) + "\n"
    }
  }
  for binding in bindings {
    out = out + "    " + yaml_quote(binding.joint) + ":\n"
    out = out + "      actuator: " + yaml_quote(binding.actuator) + "\n"
    out = out + "      channel: " + yaml_quote(binding.channel) + "\n"
    out = out + "      mcu_pins: " + yaml_quote(binding.mcu_pins) + "\n"
    out = out + "      direction: " + yaml_quote(binding.direction) + "\n"
    out = out + "      gear_ratio: " + binding.gear_ratio.to_string() + "\n"
    out = out + "      soft_lower: " + yaml_number(binding.soft_lower) + "\n"
    out = out + "      soft_upper: " + yaml_number(binding.soft_upper) + "\n"
    out = out +
      "      homing_input: " +
      yaml_string(binding.homing_input) +
      "\n"
    out = out + "      limit_input: " + yaml_string(binding.limit_input) + "\n"
  }
  out = out + "    sensors:\n"
  if sensors.length() == 0 {
    out = out + "      {}\n"
  } else {
    for line in sensors {
      let name = leaf_name(line.path)
      out = out + "      " + yaml_quote(name) + ":\n"
      out = out + "        model: " + yaml_quote(line.spec.model) + "\n"
      out = out + "        interface: " + yaml_quote(line.spec.interface) + "\n"
      out = out + "        voltage_v: " + line.spec.voltage_v.to_string() + "\n"
      out = out +
        "        current_ma: " +
        line.spec.current_ma.to_string() +
        "\n"
    }
  }
  out
}

///|
fn csv_number_or_empty(value : Double?) -> String {
  match value {
    Some(number) => number.to_string()
    None => "\"\""
  }
}

///|
/// Renders the firmware pin map as CSV: one row per binding.
pub fn Component::to_pin_map_csv(self : Component) -> String {
  let header : Array[String] = [
    "joint", "actuator", "channel", "mcu_pins", "direction", "gear_ratio", "soft_lower",
    "soft_upper", "homing_input", "limit_input",
  ]
  let rows : Array[String] = [csv_row(header.map(fn(v) { csv_field(v) }))]
  for binding in self.control_bindings() {
    rows.push(
      csv_row([
        csv_field(binding.joint),
        csv_field(binding.actuator),
        csv_field(binding.channel),
        csv_field(binding.mcu_pins),
        csv_field(binding.direction),
        binding.gear_ratio.to_string(),
        csv_number_or_empty(binding.soft_lower),
        csv_number_or_empty(binding.soft_upper),
        csv_field(binding.homing_input.unwrap_or("")),
        csv_field(binding.limit_input.unwrap_or("")),
      ]),
    )
  }
  rows.fold(init="", fn(acc, row) { acc + row + "\n" })
}

///|
fn Component::collect_port_names(
  self : Component,
  names : Array[String],
) -> Unit {
  for port in self.ports {
    names.push(port.name)
  }
  for att in self.attachments {
    att.child.collect_port_names(names)
  }
}

///|
/// Checks the control map against the compiled robot: unknown joints or
/// actuators, unbound commandable joints, bad directions/gear ratios/limits,
/// and homing or limit inputs that are not declared ports.
///
/// Returns no warnings when the tree declares no bindings, so examples without
/// a control map are not forced to provide one.
pub fn Component::control_checks(self : Component) -> Array[String] {
  let warnings : Array[String] = []
  let bindings = self.control_bindings()
  if bindings.length() == 0 {
    return warnings
  }
  let robot = self.to_robot()
  let component_names : Array[String] = []
  self.collect_component_names(component_names)
  let port_names : Array[String] = []
  self.collect_port_names(port_names)
  let seen : Array[String] = []
  for binding in bindings {
    if array_has(seen, binding.joint) {
      warnings.push(
        "duplicate control binding for joint '" + binding.joint + "'",
      )
    } else {
      seen.push(binding.joint)
    }
    if robot.get_joint(binding.joint) is None {
      warnings.push(
        "control binding references unknown joint '" + binding.joint + "'",
      )
    }
    if binding.actuator == "" {
      warnings.push(
        "control binding for joint '" + binding.joint + "' has no actuator",
      )
    } else if !array_has(component_names, binding.actuator) {
      warnings.push(
        "control binding for joint '" +
        binding.joint +
        "' references unknown actuator '" +
        binding.actuator +
        "'",
      )
    }
    if binding.direction != "normal" && binding.direction != "inverted" {
      warnings.push(
        "control binding for joint '" +
        binding.joint +
        "' has direction '" +
        binding.direction +
        "'",
      )
    }
    if binding.gear_ratio <= 0.0 {
      warnings.push(
        "control binding for joint '" +
        binding.joint +
        "' has gear ratio " +
        binding.gear_ratio.to_string(),
      )
    }
    match binding.soft_lower {
      Some(lower) =>
        match binding.soft_upper {
          Some(upper) =>
            if lower > upper {
              warnings.push(
                "control binding for joint '" +
                binding.joint +
                "' has soft lower > upper",
              )
            }
          None => ()
        }
      None => ()
    }
    match binding.homing_input {
      Some(input) =>
        if !array_has(port_names, input) {
          warnings.push(
            "control binding for joint '" +
            binding.joint +
            "' references unknown homing input '" +
            input +
            "'",
          )
        }
      None => ()
    }
    match binding.limit_input {
      Some(input) =>
        if !array_has(port_names, input) {
          warnings.push(
            "control binding for joint '" +
            binding.joint +
            "' references unknown limit input '" +
            input +
            "'",
          )
        }
      None => ()
    }
  }
  for joint in robot.joints {
    let commandable = match joint.joint_type {
      Fixed | Floating | Planar => false
      Revolute | Continuous | Prismatic => joint.mimic is None
    }
    if commandable && !array_has(seen, joint.name) {
      warnings.push("robot joint '" + joint.name + "' has no control binding")
    }
  }
  warnings
}

///|
fn c_string(value : String) -> String {
  "\"" +
  value
  .replace(old="\\", new="\\\\")
  .replace(old="\"", new="\\\"")
  .replace(old="\n", new="\\n") +
  "\""
}

///|
fn c_float(value : Double) -> String {
  let text = value.to_string()
  if text.contains(".") || text.contains("e") || text.contains("E") {
    text + "f"
  } else {
    text + ".0f"
  }
}

///|
fn c_optional_string(value : String?) -> String {
  match value {
    Some(text) => c_string(text)
    None => "NULL"
  }
}

///|
/// A C header for firmware: joint-to-actuator bindings and sensor interfaces.
///
/// The tables are derived from `ControlBinding`s and `SensorSpec`s in the tree.
pub fn Component::firmware_header(self : Component) -> String {
  let bindings = self.control_bindings()
  let sensors = self.power_budget().sensors
  let mut out = "// Generated by gmlewis/cad for " +
    self.display_name() +
    "\n#pragma once\n\n#include \n#include \n\n"
  out = out + "#define MB_JOINT_COUNT " + bindings.length().to_string() + "\n"
  out = out + "#define MB_SENSOR_COUNT " + sensors.length().to_string() + "\n\n"
  out = out + "typedef struct {\n"
  out = out + "  const char *joint;\n"
  out = out + "  const char *actuator;\n"
  out = out + "  const char *channel;\n"
  out = out + "  const char *pins;\n"
  out = out + "  const char *direction;\n"
  out = out + "  float gear_ratio;\n"
  out = out + "  uint8_t has_soft_limits;\n"
  out = out + "  float soft_lower;\n"
  out = out + "  float soft_upper;\n"
  out = out + "  const char *homing_input;\n"
  out = out + "  const char *limit_input;\n"
  out = out + "} mb_joint_binding;\n\n"
  out = out +
    "static const mb_joint_binding mb_joint_bindings[MB_JOINT_COUNT] = {\n"
  for binding in bindings {
    let has_limits = match binding.soft_lower {
      Some(_) =>
        match binding.soft_upper {
          Some(_) => true
          None => false
        }
      None => false
    }
    let lower = match binding.soft_lower {
      Some(value) => c_float(value)
      None => "0.0f"
    }
    let upper = match binding.soft_upper {
      Some(value) => c_float(value)
      None => "0.0f"
    }
    out = out +
      "  {" +
      c_string(binding.joint) +
      ", " +
      c_string(binding.actuator) +
      ", " +
      c_string(binding.channel) +
      ", " +
      c_string(binding.mcu_pins) +
      ", " +
      c_string(binding.direction) +
      ", " +
      c_float(binding.gear_ratio) +
      ", " +
      (if has_limits { "1" } else { "0" }) +
      ", " +
      lower +
      ", " +
      upper +
      ", " +
      c_optional_string(binding.homing_input) +
      ", " +
      c_optional_string(binding.limit_input) +
      "},\n"
  }
  out = out + "};\n\n"
  out = out + "typedef struct {\n"
  out = out + "  const char *name;\n"
  out = out + "  const char *model;\n"
  out = out + "  const char *interface;\n"
  out = out + "  float voltage_v;\n"
  out = out + "  float current_ma;\n"
  out = out + "} mb_sensor;\n\n"
  out = out + "static const mb_sensor mb_sensors[MB_SENSOR_COUNT] = {\n"
  for line in sensors {
    out = out +
      "  {" +
      c_string(leaf_name(line.path)) +
      ", " +
      c_string(line.spec.model) +
      ", " +
      c_string(line.spec.interface) +
      ", " +
      c_float(line.spec.voltage_v) +
      ", " +
      c_float(line.spec.current_ma) +
      "},\n"
  }
  out = out + "};\n"
  out
}

///|
/// One joint's exported control constants.
#derive(Debug, Eq)
pub(all) struct ControlJointConstants {
  joint : String
  actuator : String
  channel : String
  gear_ratio : Double
  lower : Double?
  upper : Double?
  effort : Double
  velocity : Double
}

///|
/// Whole-robot constants for firmware and LQR/EKF models.
#derive(Debug, Eq)
pub(all) struct ControlConstants {
  robot_name : String
  total_mass_kg : Double
  com_x_m : Double
  com_y_m : Double
  com_z_m : Double
  ixx_kgm2 : Double
  iyy_kgm2 : Double
  izz_kgm2 : Double
  /// Lowest wheel/foot contact in the zero pose (0.0 without contacts).
  ground_z_m : Double
  joints : Array[ControlJointConstants]
}

///|
/// Builds the control constants from the current component tree: a static
/// analysis at the zero pose plus the limits and binding of every commandable
/// joint (the set exported in `control.yaml` and `pins.csv`).
pub fn Component::control_constants(
  self : Component,
  joint_positions~ : Map[String, Double] = Map([]),
) -> ControlConstants {
  let analysis = analyze_robot(self, [("default", joint_positions)])
  let pose = analysis.poses[0]
  let robot = self.to_robot()
  let bindings = self.control_bindings()
  let joints : Array[ControlJointConstants] = []
  for binding in bindings {
    let (lower, upper, effort, velocity) = match
      robot.get_joint(binding.joint) {
      Some(joint) =>
        match joint.limit {
          Some(limit) =>
            (limit.lower, limit.upper, limit.effort, limit.velocity)
          None => (None, None, 0.0, 0.0)
        }
      None => (None, None, 0.0, 0.0)
    }
    joints.push({
      joint: binding.joint,
      actuator: binding.actuator,
      channel: binding.channel,
      gear_ratio: binding.gear_ratio,
      lower,
      upper,
      effort,
      velocity,
    })
  }
  let mut ground_z = 0.0
  if pose.contacts.length() > 0 {
    let mut lowest = pose.contacts[0].min_z_m
    for contact in pose.contacts {
      if contact.min_z_m < lowest {
        lowest = contact.min_z_m
      }
    }
    ground_z = lowest
  }
  {
    robot_name: self.display_name(),
    total_mass_kg: pose.total_mass_kg,
    com_x_m: pose.com_m.0,
    com_y_m: pose.com_m.1,
    com_z_m: pose.com_m.2,
    ixx_kgm2: pose.inertia.ixx,
    iyy_kgm2: pose.inertia.iyy,
    izz_kgm2: pose.inertia.izz,
    ground_z_m: ground_z,
    joints,
  }
}

///|
fn c_optional_number(value : Double?) -> String {
  match value {
    Some(number) => c_float(number)
    None => "NAN"
  }
}

///|
/// A C/C++ header with the robot's mass, CoM, inertia, and joint constants.
pub fn Component::control_params_header(
  self : Component,
  joint_positions~ : Map[String, Double] = Map([]),
) -> String {
  let constants = self.control_constants(joint_positions~)
  let mut out = "// Generated by gmlewis/cad for " +
    constants.robot_name +
    "\n#pragma once\n\n#include \n#include \n#include \n\n"
  out = out +
    "#define ROBOT_TOTAL_MASS_KG " +
    c_float(constants.total_mass_kg) +
    "\n"
  out = out + "#define ROBOT_COM_X_M " + c_float(constants.com_x_m) + "\n"
  out = out + "#define ROBOT_COM_Y_M " + c_float(constants.com_y_m) + "\n"
  out = out + "#define ROBOT_COM_Z_M " + c_float(constants.com_z_m) + "\n"
  out = out + "#define ROBOT_IXX_KGM2 " + c_float(constants.ixx_kgm2) + "\n"
  out = out + "#define ROBOT_IYY_KGM2 " + c_float(constants.iyy_kgm2) + "\n"
  out = out + "#define ROBOT_IZZ_KGM2 " + c_float(constants.izz_kgm2) + "\n"
  out = out + "#define ROBOT_GROUND_Z_M " + c_float(constants.ground_z_m) + "\n"
  out = out +
    "#define ROBOT_JOINT_COUNT " +
    constants.joints.length().to_string() +
    "\n\n"
  out = out + "typedef struct {\n"
  out = out + "  const char *joint;\n"
  out = out + "  const char *actuator;\n"
  out = out + "  const char *can_id;\n"
  out = out + "  float gear_ratio;\n"
  out = out + "  uint8_t has_limits;\n"
  out = out + "  float lower;\n"
  out = out + "  float upper;\n"
  out = out + "  float effort_nm;\n"
  out = out + "  float velocity_rad_s;\n"
  out = out + "} robot_joint_constants;\n\n"
  out = out +
    "static const robot_joint_constants robot_joints[ROBOT_JOINT_COUNT] = {\n"
  for joint in constants.joints {
    let has_limits = match joint.lower {
      Some(_) =>
        match joint.upper {
          Some(_) => true
          None => false
        }
      None => false
    }
    out = out +
      "  {" +
      c_string(joint.joint) +
      ", " +
      c_string(joint.actuator) +
      ", " +
      c_string(joint.channel) +
      ", " +
      c_float(joint.gear_ratio) +
      ", " +
      (if has_limits { "1" } else { "0" }) +
      ", " +
      c_optional_number(joint.lower) +
      ", " +
      c_optional_number(joint.upper) +
      ", " +
      c_float(joint.effort) +
      ", " +
      c_float(joint.velocity) +
      "},\n"
  }
  out = out + "};\n"
  out
}

///|
fn py_number(value : Double) -> String {
  let text = value.to_string()
  if text.contains(".") || text.contains("e") || text.contains("E") {
    text
  } else {
    text + ".0"
  }
}

///|
fn py_optional(value : Double?) -> String {
  match value {
    Some(number) => py_number(number)
    None => "None"
  }
}

///|
/// A ready-to-run Python module with the same constants and a linearized
/// inverted-pendulum state-space helper.
pub fn Component::lqr_model_python(
  self : Component,
  joint_positions~ : Map[String, Double] = Map([]),
) -> String {
  let constants = self.control_constants(joint_positions~)
  let mut out = "\"\"\"Generated by gmlewis/cad for " +
    constants.robot_name +
    ".\"\"\"\n\nfrom dataclasses import dataclass\n\nGRAVITY = 9.80665\n\n\n"
  out = out + "@dataclass(frozen=True)\nclass RobotConstants:\n"
  out = out + "    name: str = " + py_str(constants.robot_name) + "\n"
  out = out +
    "    total_mass_kg: float = " +
    py_number(constants.total_mass_kg) +
    "\n"
  out = out + "    com_x_m: float = " + py_number(constants.com_x_m) + "\n"
  out = out + "    com_y_m: float = " + py_number(constants.com_y_m) + "\n"
  out = out + "    com_z_m: float = " + py_number(constants.com_z_m) + "\n"
  out = out + "    ixx_kgm2: float = " + py_number(constants.ixx_kgm2) + "\n"
  out = out + "    iyy_kgm2: float = " + py_number(constants.iyy_kgm2) + "\n"
  out = out + "    izz_kgm2: float = " + py_number(constants.izz_kgm2) + "\n"
  out = out +
    "    ground_z_m: float = " +
    py_number(constants.ground_z_m) +
    "\n\n\n"
  out = out + "@dataclass(frozen=True)\nclass JointConstants:\n"
  out = out + "    joint: str\n"
  out = out + "    actuator: str\n"
  out = out + "    can_id: str\n"
  out = out + "    gear_ratio: float\n"
  out = out + "    lower: float | None\n"
  out = out + "    upper: float | None\n"
  out = out + "    effort_nm: float\n"
  out = out + "    velocity_rad_s: float\n\n\n"
  out = out + "ROBOT = RobotConstants()\n\n"
  out = out + "JOINTS: list[JointConstants] = [\n"
  for joint in constants.joints {
    out = out +
      "    JointConstants(\n" +
      "        joint=" +
      py_str(joint.joint) +
      ",\n" +
      "        actuator=" +
      py_str(joint.actuator) +
      ",\n" +
      "        can_id=" +
      py_str(joint.channel) +
      ",\n" +
      "        gear_ratio=" +
      py_number(joint.gear_ratio) +
      ",\n" +
      "        lower=" +
      py_optional(joint.lower) +
      ",\n" +
      "        upper=" +
      py_optional(joint.upper) +
      ",\n" +
      "        effort_nm=" +
      py_number(joint.effort) +
      ",\n" +
      "        velocity_rad_s=" +
      py_number(joint.velocity) +
      ",\n" +
      "    ),\n"
  }
  out = out + "]\n\n\n"
  out = out +
    "def inverted_pendulum_AB(robot: RobotConstants = ROBOT, gravity: float = GRAVITY):\n"
  out = out + "    \"\"\"Linearized upright inverted-pendulum state space.\n\n"
  out = out +
    "    State is (theta, theta_dot) with theta measured from upright;\n"
  out = out +
    "    input is joint torque about the ground contact. Use it as a\n"
  out = out + "    starting point for LQR.\n"
  out = out + "    \"\"\"\n"
  out = out + "    length = max(robot.com_z_m - robot.ground_z_m, 1.0e-3)\n"
  out = out +
    "    inertia = max(robot.iyy_kgm2 + robot.total_mass_kg * length * length, 1.0e-6)\n"
  out = out + "    a = robot.total_mass_kg * gravity * length / inertia\n"
  out = out + "    return [[0.0, 1.0], [a, 0.0]], [[0.0], [1.0 / inertia]]\n"
  out
}

///|
fn py_str(value : String) -> String {
  "\"" +
  value.replace(old="\\", new="\\\\").replace(old="\"", new="\\\"") +
  "\""
}