#include <algorithm>
#include <cctype>
#include <cmath>
#include <cstring>
#include <map>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#ifdef __EMSCRIPTEN__
#include <emscripten/emscripten.h>
#else
#define EMSCRIPTEN_KEEPALIVE
#endif

#ifdef KDL_WASM_HAS_OROCOS_KDL
#include "chain.hpp"
#include "chainfksolverpos_recursive.hpp"
#include "chainiksolverpos_lma.hpp"
#include "chainjnttojacsolver.hpp"
#include "jntarray.hpp"
#include "segment.hpp"
#endif

namespace {

std::string last_error =
    R"({"code":"KDL_OK","message":"No error","diagnostics":[]})";
int next_robot_handle = 1;

struct JsonValue {
  enum class Kind { Null, Bool, Number, String, Array, Object };

  Kind kind = Kind::Null;
  bool bool_value = false;
  double number_value = 0.0;
  std::string string_value;
  std::vector<JsonValue> array_value;
  std::map<std::string, JsonValue> object_value;

  const JsonValue* get(const std::string& key) const {
    if (kind != Kind::Object) {
      return nullptr;
    }
    const auto it = object_value.find(key);
    return it == object_value.end() ? nullptr : &it->second;
  }
};

class JsonParser {
 public:
  explicit JsonParser(const std::string& input) : input_(input) {}

  JsonValue parse() {
    JsonValue value = parse_value();
    skip_ws();
    if (pos_ != input_.size()) {
      throw std::runtime_error("Unexpected trailing JSON content");
    }
    return value;
  }

 private:
  const std::string& input_;
  std::size_t pos_ = 0;

  JsonValue parse_value() {
    skip_ws();
    if (pos_ >= input_.size()) {
      throw std::runtime_error("Unexpected end of JSON");
    }
    const char ch = input_[pos_];
    if (ch == '{') {
      return parse_object();
    }
    if (ch == '[') {
      return parse_array();
    }
    if (ch == '"') {
      JsonValue value;
      value.kind = JsonValue::Kind::String;
      value.string_value = parse_string();
      return value;
    }
    if (ch == 't' || ch == 'f') {
      return parse_bool();
    }
    if (ch == 'n') {
      expect_literal("null");
      return {};
    }
    return parse_number();
  }

  JsonValue parse_object() {
    consume('{');
    JsonValue value;
    value.kind = JsonValue::Kind::Object;
    skip_ws();
    if (match('}')) {
      return value;
    }

    while (true) {
      skip_ws();
      const std::string key = parse_string();
      skip_ws();
      consume(':');
      value.object_value.emplace(key, parse_value());
      skip_ws();
      if (match('}')) {
        return value;
      }
      consume(',');
    }
  }

  JsonValue parse_array() {
    consume('[');
    JsonValue value;
    value.kind = JsonValue::Kind::Array;
    skip_ws();
    if (match(']')) {
      return value;
    }

    while (true) {
      value.array_value.push_back(parse_value());
      skip_ws();
      if (match(']')) {
        return value;
      }
      consume(',');
    }
  }

  std::string parse_string() {
    consume('"');
    std::string out;
    while (pos_ < input_.size()) {
      const char ch = input_[pos_++];
      if (ch == '"') {
        return out;
      }
      if (ch != '\\') {
        out.push_back(ch);
        continue;
      }
      if (pos_ >= input_.size()) {
        throw std::runtime_error("Invalid JSON string escape");
      }
      const char escaped = input_[pos_++];
      switch (escaped) {
        case '"':
        case '\\':
        case '/':
          out.push_back(escaped);
          break;
        case 'b':
          out.push_back('\b');
          break;
        case 'f':
          out.push_back('\f');
          break;
        case 'n':
          out.push_back('\n');
          break;
        case 'r':
          out.push_back('\r');
          break;
        case 't':
          out.push_back('\t');
          break;
        case 'u':
          if (pos_ + 4 > input_.size()) {
            throw std::runtime_error("Invalid JSON unicode escape");
          }
          out.push_back('?');
          pos_ += 4;
          break;
        default:
          throw std::runtime_error("Invalid JSON string escape");
      }
    }
    throw std::runtime_error("Unterminated JSON string");
  }

  JsonValue parse_bool() {
    JsonValue value;
    value.kind = JsonValue::Kind::Bool;
    if (starts_with("true")) {
      value.bool_value = true;
      pos_ += 4;
      return value;
    }
    if (starts_with("false")) {
      value.bool_value = false;
      pos_ += 5;
      return value;
    }
    throw std::runtime_error("Invalid JSON boolean");
  }

  JsonValue parse_number() {
    const std::size_t start = pos_;
    if (input_[pos_] == '-') {
      pos_ += 1;
    }
    while (pos_ < input_.size() && std::isdigit(static_cast<unsigned char>(input_[pos_]))) {
      pos_ += 1;
    }
    if (pos_ < input_.size() && input_[pos_] == '.') {
      pos_ += 1;
      while (pos_ < input_.size() && std::isdigit(static_cast<unsigned char>(input_[pos_]))) {
        pos_ += 1;
      }
    }
    if (pos_ < input_.size() && (input_[pos_] == 'e' || input_[pos_] == 'E')) {
      pos_ += 1;
      if (pos_ < input_.size() && (input_[pos_] == '+' || input_[pos_] == '-')) {
        pos_ += 1;
      }
      while (pos_ < input_.size() && std::isdigit(static_cast<unsigned char>(input_[pos_]))) {
        pos_ += 1;
      }
    }
    if (start == pos_) {
      throw std::runtime_error("Expected JSON number");
    }
    JsonValue value;
    value.kind = JsonValue::Kind::Number;
    value.number_value = std::stod(input_.substr(start, pos_ - start));
    return value;
  }

  void expect_literal(const char* literal) {
    if (!starts_with(literal)) {
      throw std::runtime_error(std::string("Expected JSON literal ") + literal);
    }
    pos_ += std::strlen(literal);
  }

  bool starts_with(const char* literal) const {
    const std::size_t len = std::strlen(literal);
    return input_.compare(pos_, len, literal) == 0;
  }

  void skip_ws() {
    while (pos_ < input_.size() && std::isspace(static_cast<unsigned char>(input_[pos_]))) {
      pos_ += 1;
    }
  }

  bool match(char expected) {
    if (pos_ < input_.size() && input_[pos_] == expected) {
      pos_ += 1;
      return true;
    }
    return false;
  }

  void consume(char expected) {
    if (!match(expected)) {
      throw std::runtime_error(std::string("Expected JSON character ") + expected);
    }
  }
};

struct NativeJoint {
  std::string name;
  std::string type;
  std::string child;
  double xyz[3] = {0.0, 0.0, 0.0};
  double rpy[3] = {0.0, 0.0, 0.0};
  double axis[3] = {1.0, 0.0, 0.0};
  bool active = false;
};

struct RobotRecord {
  std::string model_json;
  std::string robot_id;
  std::string name;
  std::string base_link;
  std::string tip_link;
  std::vector<std::string> active_joint_names;
  std::vector<NativeJoint> joints;
  std::vector<std::string> link_order;
#ifdef KDL_WASM_HAS_OROCOS_KDL
  KDL::Chain chain;
  std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
  std::unique_ptr<KDL::ChainJntToJacSolver> jac_solver;
  std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
#endif
};

std::map<int, RobotRecord> robots;

void set_last_error(const char* code, const char* message) {
  last_error = std::string("{\"code\":\"") + code + "\",\"message\":\"" +
               message + "\",\"diagnostics\":[{\"severity\":\"error\",\"code\":\"" +
               code + "\",\"message\":\"" + message + "\"}]}";
}

void clear_last_error() {
  last_error = R"({"code":"KDL_OK","message":"No error","diagnostics":[]})";
}

int write_json(const std::string& json, char* out_json, int out_len) {
  if (out_json == nullptr || out_len <= 0) {
    set_last_error("KDL_BUFFER_TOO_SMALL", "Output buffer is not writable");
    return -1;
  }

  const int required = static_cast<int>(json.size()) + 1;
  if (out_len < required) {
    set_last_error("KDL_BUFFER_TOO_SMALL", "Output buffer is too small");
    return -1;
  }

  std::memcpy(out_json, json.c_str(), static_cast<std::size_t>(required));
  return 0;
}

int not_implemented(const char* function_name) {
  set_last_error("KDL_NOT_IMPLEMENTED", function_name);
  return -1;
}

std::string json_escape(const std::string& value) {
  std::string escaped;
  for (const char ch : value) {
    if (ch == '"' || ch == '\\') {
      escaped.push_back('\\');
    }
    escaped.push_back(ch);
  }
  return escaped;
}

#ifdef KDL_WASM_HAS_OROCOS_KDL
void append_pose_json(std::ostringstream& json, const KDL::Frame& frame) {
  double x = 0.0;
  double y = 0.0;
  double z = 0.0;
  double w = 1.0;
  frame.M.GetQuaternion(x, y, z, w);
  json << "{\"position\":[" << frame.p.x() << "," << frame.p.y() << "," << frame.p.z()
       << "],\"quaternion\":[" << x << "," << y << "," << z << "," << w << "]}";
}
#endif

const JsonValue& required_object_property(const JsonValue& object, const std::string& key) {
  const JsonValue* value = object.get(key);
  if (!value) {
    throw std::runtime_error("Missing model property: " + key);
  }
  return *value;
}

std::string string_property(const JsonValue& object, const std::string& key) {
  const JsonValue& value = required_object_property(object, key);
  if (value.kind != JsonValue::Kind::String) {
    throw std::runtime_error("Expected string model property: " + key);
  }
  return value.string_value;
}

std::vector<std::string> string_array_property(const JsonValue& object, const std::string& key) {
  const JsonValue& value = required_object_property(object, key);
  if (value.kind != JsonValue::Kind::Array) {
    throw std::runtime_error("Expected string array model property: " + key);
  }

  std::vector<std::string> out;
  out.reserve(value.array_value.size());
  for (const JsonValue& item : value.array_value) {
    if (item.kind != JsonValue::Kind::String) {
      throw std::runtime_error("Expected string item in model property: " + key);
    }
    out.push_back(item.string_value);
  }
  return out;
}

double number_property(const JsonValue& object, const std::string& key) {
  const JsonValue& value = required_object_property(object, key);
  if (value.kind != JsonValue::Kind::Number) {
    throw std::runtime_error("Expected number model property: " + key);
  }
  return value.number_value;
}

double number_property_or(const JsonValue& object, const std::string& key, double fallback) {
  const JsonValue* value = object.get(key);
  if (!value) {
    return fallback;
  }
  if (value->kind != JsonValue::Kind::Number) {
    throw std::runtime_error("Expected numeric property: " + key);
  }
  return value->number_value;
}

void copy_number_triple(const JsonValue& object, const std::string& key, double out[3]) {
  const JsonValue& value = required_object_property(object, key);
  if (value.kind != JsonValue::Kind::Array || value.array_value.size() != 3) {
    throw std::runtime_error("Expected numeric triple model property: " + key);
  }
  for (std::size_t index = 0; index < 3; index += 1) {
    if (value.array_value[index].kind != JsonValue::Kind::Number) {
      throw std::runtime_error("Expected numeric triple model property: " + key);
    }
    out[index] = value.array_value[index].number_value;
  }
}

std::vector<NativeJoint> parse_joints(const JsonValue& model,
                                      const std::vector<std::string>& active_joint_names) {
  const JsonValue& joints_value = required_object_property(model, "joints");
  if (joints_value.kind != JsonValue::Kind::Array) {
    throw std::runtime_error("Expected joints array");
  }

  std::vector<NativeJoint> joints;
  joints.reserve(joints_value.array_value.size());
  for (const JsonValue& joint_value : joints_value.array_value) {
    if (joint_value.kind != JsonValue::Kind::Object) {
      throw std::runtime_error("Expected joint object");
    }
    NativeJoint joint;
    joint.name = string_property(joint_value, "name");
    joint.type = string_property(joint_value, "type");
    joint.child = string_property(joint_value, "child");

    const JsonValue& origin = required_object_property(joint_value, "origin");
    if (origin.kind != JsonValue::Kind::Object) {
      throw std::runtime_error("Expected joint origin object");
    }
    copy_number_triple(origin, "xyz", joint.xyz);
    copy_number_triple(origin, "rpy", joint.rpy);
    copy_number_triple(joint_value, "axis", joint.axis);
    joint.active = std::find(active_joint_names.begin(), active_joint_names.end(), joint.name) !=
                   active_joint_names.end();
    joints.push_back(joint);
  }
  return joints;
}

#ifdef KDL_WASM_HAS_OROCOS_KDL
KDL::Frame frame_from_xyz_rpy(const double xyz[3], const double rpy[3]) {
  return KDL::Frame(
      KDL::Rotation::RPY(rpy[0], rpy[1], rpy[2]),
      KDL::Vector(xyz[0], xyz[1], xyz[2]));
}

KDL::Joint kdl_joint_from_model(const NativeJoint& joint) {
  if (joint.type == "fixed") {
    return KDL::Joint(joint.name, KDL::Joint::Fixed);
  }
  const KDL::Vector axis(joint.axis[0], joint.axis[1], joint.axis[2]);
  if (joint.type == "revolute" || joint.type == "continuous") {
    return KDL::Joint(joint.name, KDL::Vector(0.0, 0.0, 0.0), axis, KDL::Joint::RotAxis);
  }
  if (joint.type == "prismatic") {
    return KDL::Joint(joint.name, KDL::Vector(0.0, 0.0, 0.0), axis, KDL::Joint::TransAxis);
  }
  throw std::runtime_error("Unsupported KDL joint type: " + joint.type);
}

void build_kdl_chain(RobotRecord& record) {
  record.chain = KDL::Chain();
  record.link_order.clear();
  record.link_order.push_back(record.base_link);
  for (const NativeJoint& joint : record.joints) {
    const KDL::Frame origin = frame_from_xyz_rpy(joint.xyz, joint.rpy);
    if (joint.xyz[0] != 0.0 || joint.xyz[1] != 0.0 || joint.xyz[2] != 0.0 ||
        joint.rpy[0] != 0.0 || joint.rpy[1] != 0.0 || joint.rpy[2] != 0.0) {
      record.chain.addSegment(KDL::Segment(joint.name + "_origin", KDL::Joint(KDL::Joint::Fixed), origin));
    }
    record.chain.addSegment(KDL::Segment(joint.child, kdl_joint_from_model(joint), KDL::Frame::Identity()));
    record.link_order.push_back(joint.child);
  }
  record.fk_solver = std::make_unique<KDL::ChainFkSolverPos_recursive>(record.chain);
  record.jac_solver = std::make_unique<KDL::ChainJntToJacSolver>(record.chain);
  record.ik_solver = std::make_unique<KDL::ChainIkSolverPos_LMA>(record.chain);
}

KDL::JntArray jnt_array_from_input(const RobotRecord& record, const double* joints, int n) {
  if (joints == nullptr) {
    throw std::runtime_error("Joint input is required");
  }
  if (n != static_cast<int>(record.active_joint_names.size())) {
    throw std::runtime_error("Joint vector dimension does not match robot DOF");
  }
  KDL::JntArray q(static_cast<unsigned int>(n));
  for (int index = 0; index < n; index += 1) {
    q(static_cast<unsigned int>(index)) = joints[index];
  }
  return q;
}

void write_pose7(const KDL::Frame& frame, double* out_pose7) {
  if (out_pose7 == nullptr) {
    throw std::runtime_error("Output pose buffer is required");
  }
  out_pose7[0] = frame.p.x();
  out_pose7[1] = frame.p.y();
  out_pose7[2] = frame.p.z();
  frame.M.GetQuaternion(out_pose7[3], out_pose7[4], out_pose7[5], out_pose7[6]);
}

KDL::Frame frame_from_pose7(const double* pose7) {
  if (pose7 == nullptr) {
    throw std::runtime_error("Target pose buffer is required");
  }
  const double norm =
      std::sqrt(pose7[3] * pose7[3] + pose7[4] * pose7[4] + pose7[5] * pose7[5] + pose7[6] * pose7[6]);
  if (norm <= 0.0) {
    throw std::runtime_error("Target pose quaternion must be non-zero");
  }
  return KDL::Frame(
      KDL::Rotation::Quaternion(pose7[3] / norm, pose7[4] / norm, pose7[5] / norm, pose7[6] / norm),
      KDL::Vector(pose7[0], pose7[1], pose7[2]));
}
#endif

RobotRecord parse_robot_record(const char* model_json) {
  if (model_json == nullptr) {
    throw std::runtime_error("Robot model JSON is required");
  }

  RobotRecord record;
  record.model_json = model_json;
  const JsonValue model = JsonParser(record.model_json).parse();
  if (model.kind != JsonValue::Kind::Object) {
    throw std::runtime_error("Robot model must be a JSON object");
  }

  record.robot_id = string_property(model, "robotId");
  record.name = string_property(model, "name");
  record.base_link = string_property(model, "baseLink");
  record.tip_link = string_property(model, "tipLink");
  record.active_joint_names = string_array_property(model, "activeJointNames");
  record.joints = parse_joints(model, record.active_joint_names);

  return record;
}

std::map<int, RobotRecord>::iterator find_robot(int robot_handle) {
  const auto it = robots.find(robot_handle);
  if (it == robots.end()) {
    set_last_error("KDL_INVALID_HANDLE", "RobotHandle does not exist");
  }
  return it;
}

double distance_for_velocity_change(double from_velocity, double to_velocity, double acceleration) {
  return std::max(0.0, (to_velocity * to_velocity - from_velocity * from_velocity) / (2.0 * acceleration));
}

double clamp01(double value) {
  return std::max(0.0, std::min(1.0, value));
}

struct TrapProfile {
  std::string type;
  double length = 0.0;
  double sample_time = 0.0;
  double start_velocity = 0.0;
  double end_velocity = 0.0;
  double max_acceleration = 0.0;
  double duration = 0.0;
  double t_accel = 0.0;
  double t_const = 0.0;
  double t_decel = 0.0;
  double v_peak = 0.0;
  bool triangle_diagnostic = false;
  bool zero_length_diagnostic = false;
};

struct TrapPoint {
  double distance = 0.0;
  double velocity = 0.0;
  double acceleration = 0.0;
};

TrapProfile make_trap_profile(double length, const char* options_json) {
  if (options_json == nullptr) {
    throw std::runtime_error("Trap profile options JSON is required");
  }
  const JsonValue options = JsonParser(options_json).parse();
  if (options.kind != JsonValue::Kind::Object) {
    throw std::runtime_error("Trap profile options must be a JSON object");
  }

  TrapProfile profile;
  profile.length = length;
  profile.sample_time = number_property(options, "sampleTime");
  profile.start_velocity = number_property_or(options, "startVelocity", 0.0);
  profile.end_velocity = number_property_or(options, "endVelocity", 0.0);
  const double max_velocity = number_property(options, "maxVelocity");
  profile.max_acceleration = number_property(options, "maxAcceleration");

  if (!std::isfinite(length) || length < 0.0) {
    throw std::runtime_error("Trap profile length must be a finite non-negative number");
  }
  if (!std::isfinite(max_velocity) || max_velocity <= 0.0) {
    throw std::runtime_error("maxVelocity must be a finite positive number");
  }
  if (!std::isfinite(profile.max_acceleration) || profile.max_acceleration <= 0.0) {
    throw std::runtime_error("maxAcceleration must be a finite positive number");
  }
  if (!std::isfinite(profile.sample_time) || profile.sample_time <= 0.0) {
    throw std::runtime_error("sampleTime must be a finite positive number");
  }
  if (length == 0.0 && (profile.start_velocity > 0.0 || profile.end_velocity > 0.0)) {
    throw std::runtime_error("Zero-length trap profile requires zero startVelocity and endVelocity");
  }
  if (!std::isfinite(profile.start_velocity) || profile.start_velocity < 0.0 ||
      profile.start_velocity > max_velocity) {
    throw std::runtime_error("startVelocity must be finite, non-negative, and no greater than maxVelocity");
  }
  if (!std::isfinite(profile.end_velocity) || profile.end_velocity < 0.0 ||
      profile.end_velocity > max_velocity) {
    throw std::runtime_error("endVelocity must be finite, non-negative, and no greater than maxVelocity");
  }

  if (length == 0.0) {
    profile.type = "triangle";
    profile.zero_length_diagnostic = true;
    return profile;
  }

  const double d_accel_to_max =
      distance_for_velocity_change(profile.start_velocity, max_velocity, profile.max_acceleration);
  const double d_decel_from_max =
      distance_for_velocity_change(profile.end_velocity, max_velocity, profile.max_acceleration);
  profile.type = "trapezoid";
  profile.v_peak = max_velocity;

  if (d_accel_to_max + d_decel_from_max <= length) {
    profile.t_const = (length - d_accel_to_max - d_decel_from_max) / max_velocity;
  } else {
    profile.type = "triangle";
    profile.triangle_diagnostic = true;
    profile.v_peak = std::sqrt(std::max(
        0.0,
        profile.max_acceleration * length +
            (profile.start_velocity * profile.start_velocity +
             profile.end_velocity * profile.end_velocity) /
                2.0));
    if (profile.v_peak + 1e-12 < std::max(profile.start_velocity, profile.end_velocity)) {
      throw std::runtime_error("Profile length is too short for the requested startVelocity/endVelocity");
    }
    profile.t_const = 0.0;
  }

  profile.t_accel = std::max(0.0, (profile.v_peak - profile.start_velocity) / profile.max_acceleration);
  profile.t_decel = std::max(0.0, (profile.v_peak - profile.end_velocity) / profile.max_acceleration);
  profile.duration = profile.t_accel + profile.t_const + profile.t_decel;
  return profile;
}

TrapPoint sample_trap_at_time(const TrapProfile& profile, double time) {
  const double accel_distance =
      profile.start_velocity * profile.t_accel +
      0.5 * profile.max_acceleration * profile.t_accel * profile.t_accel;
  const double const_distance = profile.v_peak * profile.t_const;
  const double accel_end = profile.t_accel;
  const double const_end = profile.t_accel + profile.t_const;

  if (time <= accel_end) {
    return {
        profile.start_velocity * time + 0.5 * profile.max_acceleration * time * time,
        profile.start_velocity + profile.max_acceleration * time,
        profile.max_acceleration};
  }

  if (time <= const_end) {
    const double local_time = time - profile.t_accel;
    return {accel_distance + profile.v_peak * local_time, profile.v_peak, 0.0};
  }

  const double local_time = std::min(time - const_end, profile.t_decel);
  return {
      accel_distance + const_distance + profile.v_peak * local_time -
          0.5 * profile.max_acceleration * local_time * local_time,
      std::max(profile.end_velocity, profile.v_peak - profile.max_acceleration * local_time),
      -profile.max_acceleration};
}

std::string trap_profile_json(const TrapProfile& profile) {
  std::ostringstream json;
  json << "{\"ok\":true,\"type\":\"" << profile.type << "\",\"length\":" << profile.length
       << ",\"duration\":" << profile.duration << ",\"tAccel\":" << profile.t_accel
       << ",\"tConst\":" << profile.t_const << ",\"tDecel\":" << profile.t_decel
       << ",\"vPeak\":" << profile.v_peak << ",\"samples\":[";

  if (profile.duration == 0.0) {
    json << "{\"index\":0,\"time\":0,\"s\":0,\"sd\":0,\"sdd\":0}";
  } else {
    std::vector<double> times;
    times.push_back(0.0);
    for (double time = profile.sample_time; time < profile.duration - 1e-12;
         time += profile.sample_time) {
      times.push_back(time);
    }
    times.push_back(profile.duration);
    for (std::size_t index = 0; index < times.size(); index += 1) {
      if (index > 0) {
        json << ",";
      }
      const TrapPoint sample = sample_trap_at_time(profile, times[index]);
      const double s =
          index == 0 ? 0.0 : index == times.size() - 1 ? 1.0 : clamp01(sample.distance / profile.length);
      json << "{\"index\":" << index << ",\"time\":" << times[index] << ",\"s\":" << s
           << ",\"sd\":" << sample.velocity / profile.length
           << ",\"sdd\":" << sample.acceleration / profile.length << "}";
    }
  }

  json << "],\"diagnostics\":[";
  bool wrote_diagnostic = false;
  if (profile.zero_length_diagnostic) {
    json << "{\"severity\":\"info\",\"code\":\"KDL_TRAP_ZERO_LENGTH\","
            "\"message\":\"Trap profile length is zero\"}";
    wrote_diagnostic = true;
  }
  if (profile.triangle_diagnostic) {
    if (wrote_diagnostic) {
      json << ",";
    }
    json << "{\"severity\":\"info\",\"code\":\"KDL_TRAP_TRIANGLE_PROFILE\","
            "\"message\":\"Trap profile length is too short to reach maxVelocity; using triangle profile\"}";
  }
  json << "]}";
  return json.str();
}

}  // namespace

extern "C" {

EMSCRIPTEN_KEEPALIVE
int kdl_init(const char* options_json) {
  (void)options_json;
  robots.clear();
  next_robot_handle = 1;
  clear_last_error();
  return 0;
}

EMSCRIPTEN_KEEPALIVE
int kdl_create_robot(const char* model_json) {
  try {
    RobotRecord record = parse_robot_record(model_json);
    const int handle = next_robot_handle++;
    auto [it, inserted] = robots.emplace(handle, std::move(record));
    (void)inserted;
#ifdef KDL_WASM_HAS_OROCOS_KDL
    build_kdl_chain(it->second);
#endif
    clear_last_error();
    return handle;
  } catch (const std::exception& error) {
    set_last_error("KDL_INVALID_MODEL", error.what());
    return -1;
  }
}

EMSCRIPTEN_KEEPALIVE
int kdl_destroy_robot(int robot_handle) {
  if (robots.erase(robot_handle) == 0) {
    set_last_error("KDL_INVALID_HANDLE", "RobotHandle does not exist");
    return -1;
  }
  clear_last_error();
  return 0;
}

EMSCRIPTEN_KEEPALIVE
int kdl_get_robot_info(int robot_handle, char* out_json, int out_len) {
  const auto it = find_robot(robot_handle);
  if (it == robots.end()) {
    return -1;
  }
  const RobotRecord& record = it->second;

  std::ostringstream json;
  json << "{\"handle\":" << robot_handle << ",\"robotId\":\"" << json_escape(record.robot_id)
       << "\",\"name\":\"" << json_escape(record.name) << "\",\"baseLink\":\""
       << json_escape(record.base_link) << "\",\"tipLink\":\"" << json_escape(record.tip_link)
       << "\",\"dof\":" << record.active_joint_names.size() << ",\"jointNames\":[";
  for (std::size_t index = 0; index < record.active_joint_names.size(); index += 1) {
    if (index > 0) {
      json << ",";
    }
    json << "\"" << json_escape(record.active_joint_names[index]) << "\"";
  }
  json << "],\"limits\":[],\"nativeState\":\""
#ifdef KDL_WASM_HAS_OROCOS_KDL
       << "kdl_chain"
#else
       << "model_cached"
#endif
       << "\"}";
  const int result = write_json(json.str(), out_json, out_len);
  if (result == 0) {
    clear_last_error();
  }
  return result;
}

EMSCRIPTEN_KEEPALIVE
int kdl_fk(int robot_handle, const double* joints, int n, double* out_pose7) {
#ifdef KDL_WASM_HAS_OROCOS_KDL
  try {
    const auto it = find_robot(robot_handle);
    if (it == robots.end()) {
      return -1;
    }
    RobotRecord& record = it->second;
    const KDL::JntArray q = jnt_array_from_input(record, joints, n);
    KDL::Frame frame;
    const int result = record.fk_solver->JntToCart(q, frame);
    if (result != 0) {
      set_last_error("KDL_FK_FAILED", "Native KDL FK solver failed");
      return -1;
    }
    write_pose7(frame, out_pose7);
    clear_last_error();
    return 0;
  } catch (const std::exception& error) {
    set_last_error("KDL_FK_FAILED", error.what());
    return -1;
  }
#else
  (void)robot_handle;
  (void)joints;
  (void)n;
  (void)out_pose7;
  return not_implemented("kdl_fk requires KDL_SOURCE_DIR");
#endif
}

EMSCRIPTEN_KEEPALIVE
int kdl_fk_all_links(int robot_handle, const double* joints, int n, char* out_json,
                     int out_len) {
#ifdef KDL_WASM_HAS_OROCOS_KDL
  try {
    const auto it = find_robot(robot_handle);
    if (it == robots.end()) {
      return -1;
    }
    RobotRecord& record = it->second;
    const KDL::JntArray q = jnt_array_from_input(record, joints, n);
    std::vector<KDL::Frame> frames(record.chain.getNrOfSegments());
    const int result = record.fk_solver->JntToCart(q, frames);
    if (result != 0) {
      set_last_error("KDL_FK_FAILED", "Native KDL link FK solver failed");
      return -1;
    }

    std::ostringstream json;
    json << "{\"ok\":true,\"linkPoses\":[";
    json << "{\"link\":\"" << json_escape(record.base_link)
         << "\",\"pose\":{\"position\":[0,0,0],\"quaternion\":[0,0,0,1]}}";
    for (std::size_t segment_index = 0; segment_index < frames.size(); segment_index += 1) {
      const KDL::Segment& segment = record.chain.getSegment(static_cast<unsigned int>(segment_index));
      const std::string& segment_name = segment.getName();
      if (segment_name.size() >= 7 &&
          segment_name.compare(segment_name.size() - 7, 7, "_origin") == 0) {
        continue;
      }
      json << ",{\"link\":\"" << json_escape(segment_name) << "\",\"pose\":";
      append_pose_json(json, frames[segment_index]);
      json << "}";
    }
    json << "],\"diagnostics\":[]}";
    const int write_result = write_json(json.str(), out_json, out_len);
    if (write_result == 0) {
      clear_last_error();
    }
    return write_result;
  } catch (const std::exception& error) {
    set_last_error("KDL_FK_FAILED", error.what());
    return -1;
  }
#else
  (void)robot_handle;
  (void)joints;
  (void)n;
  (void)out_json;
  (void)out_len;
  return not_implemented("kdl_fk_all_links requires KDL_SOURCE_DIR");
#endif
}

EMSCRIPTEN_KEEPALIVE
int kdl_jacobian(int robot_handle, const double* joints, int n, double* out_matrix) {
#ifdef KDL_WASM_HAS_OROCOS_KDL
  try {
    const auto it = find_robot(robot_handle);
    if (it == robots.end()) {
      return -1;
    }
    RobotRecord& record = it->second;
    if (out_matrix == nullptr) {
      throw std::runtime_error("Output Jacobian buffer is required");
    }
    const KDL::JntArray q = jnt_array_from_input(record, joints, n);
    KDL::Jacobian jac(static_cast<unsigned int>(n));
    const int result = record.jac_solver->JntToJac(q, jac);
    if (result != 0) {
      set_last_error("KDL_JACOBIAN_FAILED", "Native KDL Jacobian solver failed");
      return -1;
    }
    for (int col = 0; col < n; col += 1) {
      for (int row = 0; row < 6; row += 1) {
        out_matrix[row * n + col] = jac(static_cast<unsigned int>(row), static_cast<unsigned int>(col));
      }
    }
    clear_last_error();
    return 0;
  } catch (const std::exception& error) {
    set_last_error("KDL_JACOBIAN_FAILED", error.what());
    return -1;
  }
#else
  (void)robot_handle;
  (void)joints;
  (void)n;
  (void)out_matrix;
  return not_implemented("kdl_jacobian requires KDL_SOURCE_DIR");
#endif
}

EMSCRIPTEN_KEEPALIVE
int kdl_ik(int robot_handle, const double* seed, int n, const double* target_pose7,
           const char* options_json, double* out_joints) {
#ifdef KDL_WASM_HAS_OROCOS_KDL
  (void)options_json;
  try {
    const auto it = find_robot(robot_handle);
    if (it == robots.end()) {
      return -1;
    }
    RobotRecord& record = it->second;
    if (out_joints == nullptr) {
      throw std::runtime_error("Output joint buffer is required");
    }
    const KDL::JntArray q_seed = jnt_array_from_input(record, seed, n);
    const KDL::Frame target = frame_from_pose7(target_pose7);
    KDL::JntArray q_out(static_cast<unsigned int>(n));
    const int result = record.ik_solver->CartToJnt(q_seed, target, q_out);
    if (result != 0) {
      set_last_error("KDL_IK_FAILED", record.ik_solver->strError(result));
      return -1;
    }
    for (int index = 0; index < n; index += 1) {
      out_joints[index] = q_out(static_cast<unsigned int>(index));
    }
    clear_last_error();
    return 0;
  } catch (const std::exception& error) {
    set_last_error("KDL_IK_FAILED", error.what());
    return -1;
  }
#else
  (void)robot_handle;
  (void)seed;
  (void)n;
  (void)target_pose7;
  (void)options_json;
  (void)out_joints;
  return not_implemented("kdl_ik requires KDL_SOURCE_DIR");
#endif
}

EMSCRIPTEN_KEEPALIVE
int kdl_plan_movej(int robot_handle, const char* request_json, char* out_json,
                   int out_len) {
  (void)robot_handle;
  (void)request_json;
  (void)out_json;
  (void)out_len;
  return not_implemented("kdl_plan_movej is pending KW-008 native binding");
}

EMSCRIPTEN_KEEPALIVE
int kdl_plan_movel(int robot_handle, const char* request_json, char* out_json,
                   int out_len) {
  (void)robot_handle;
  (void)request_json;
  (void)out_json;
  (void)out_len;
  return not_implemented("kdl_plan_movel is pending KW-009 native binding");
}

EMSCRIPTEN_KEEPALIVE
int kdl_plan_movec(int robot_handle, const char* request_json, char* out_json,
                   int out_len) {
  (void)robot_handle;
  (void)request_json;
  (void)out_json;
  (void)out_len;
  return not_implemented("kdl_plan_movec is pending KW-010 native binding");
}

EMSCRIPTEN_KEEPALIVE
int kdl_plan_path(int robot_handle, const char* request_json, char* out_json,
                  int out_len) {
  (void)robot_handle;
  (void)request_json;
  (void)out_json;
  (void)out_len;
  return not_implemented("kdl_plan_path is pending KW-011 native binding");
}

EMSCRIPTEN_KEEPALIVE
int kdl_sample_trap(double length, const char* options_json, char* out_json,
                    int out_len) {
  try {
    const TrapProfile profile = make_trap_profile(length, options_json);
    const int result = write_json(trap_profile_json(profile), out_json, out_len);
    if (result == 0) {
      clear_last_error();
    }
    return result;
  } catch (const std::exception& error) {
    set_last_error("KDL_INVALID_TRAP_PROFILE", error.what());
    return -1;
  }
}

EMSCRIPTEN_KEEPALIVE
int kdl_last_error(char* out_json, int out_len) {
  return write_json(last_error, out_json, out_len);
}

}