///|
/// 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
}