#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 "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;
#ifdef KDL_WASM_HAS_OROCOS_KDL
  KDL::Chain chain;
  std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
  std::unique_ptr<KDL::ChainJntToJacSolver> jac_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;
}

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

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();
  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.fk_solver = std::make_unique<KDL::ChainFkSolverPos_recursive>(record.chain);
  record.jac_solver = std::make_unique<KDL::ChainJntToJacSolver>(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]);
}
#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;
}

}  // 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) {
  (void)robot_handle;
  (void)joints;
  (void)n;
  (void)out_json;
  (void)out_len;
  return not_implemented("kdl_fk_all_links is pending native link-pose serialization");
}

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) {
  (void)robot_handle;
  (void)seed;
  (void)n;
  (void)target_pose7;
  (void)options_json;
  (void)out_joints;
  return not_implemented("kdl_ik is pending native IK binding");
}

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) {
  (void)length;
  (void)options_json;
  (void)out_json;
  (void)out_len;
  return not_implemented("kdl_sample_trap is pending KW-007 native binding");
}

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

}