#include "sim_kinematics.h"

#include <string.h>

#define COMMON (SIM_KIN_CAP_FORWARD | SIM_KIN_CAP_INVERSE | SIM_KIN_CAP_SWITCHABLE)
#define BASE_PARAMS (SIM_KIN_PARAM_MAPPING | SIM_KIN_PARAM_LIMITS)

static const sim_kinematics_capability registry[] = {
    {1, SIM_KIN_TRIVKINS, "trivkins", "", 1, 9, COMMON | SIM_KIN_CAP_IDENTITY | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS},
    {1, SIM_KIN_XYZAC_TRT, "xyzac-trt-kins", "XYZAC", 5, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_OFFSETS | SIM_KIN_PARAM_TOOL},
    {1, SIM_KIN_XYZBC_TRT, "xyzbc-trt-kins", "XYZBC", 5, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_OFFSETS | SIM_KIN_PARAM_TOOL},
    {1, SIM_KIN_XYZBCW, "xyzbcw", "XYZBCW", 6, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
    {1, SIM_KIN_COREXY, "corexykins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS},
    {1, SIM_KIN_ROSE, "rosekins", "XYZ", 3, 3, COMMON, BASE_PARAMS},
    {1, SIM_KIN_MAXKINS, "maxkins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
    {1, SIM_KIN_ROTATEKINS, "rotatekins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS},
    {1, SIM_KIN_5AXISKINS, "5axiskins", "XYZBCW", 6, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
    {1, SIM_KIN_SCARA, "scarakins", "XYZCAB", 6, 6, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
    {1, SIM_KIN_PUMA, "pumakins", "XYZABC", 6, 6, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
    {1, SIM_KIN_SCORBOT, "scorbot-kins", "XYZAB", 5, 5, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
    {1, SIM_KIN_GENSER, "genserkins", "XYZABC", 6, 6, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_DH | SIM_KIN_PARAM_SOLVER},
    {1, SIM_KIN_LINEAR_DELTA, "lineardeltakins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
    {1, SIM_KIN_ROTARY_DELTA, "rotarydeltakins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
    {1, SIM_KIN_TRIPOD, "tripodkins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
    {1, SIM_KIN_GENHEX, "genhexkins", "XYZABC", 6, 6, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY | SIM_KIN_PARAM_TOOL | SIM_KIN_PARAM_SOLVER},
    {1, SIM_KIN_PENTAPOD, "pentakins", "XYZAB", 5, 5, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY | SIM_KIN_PARAM_TOOL | SIM_KIN_PARAM_SOLVER},
};

static uint64_t registry_fnv1a64(const unsigned char *bytes, size_t length) {
    uint64_t hash = UINT64_C(1469598103934665603);
    size_t index;
    for (index = 0; index < length; ++index) {
        hash ^= bytes[index];
        hash *= UINT64_C(1099511628211);
    }
    return hash;
}

static int path_like(const char *name) {
    size_t index;
    if (name == 0 || name[0] == '\0') return 0;
    for (index = 0; name[index] != '\0'; ++index)
        if (name[index] == '/' || name[index] == '\\') return 1;
    return 0;
}

static int has_coordinate_casefold(const char *coordinates, char wanted) {
    size_t index;
    for (index = 0; coordinates[index] != '\0'; ++index) {
        char value = coordinates[index];
        if (value >= 'a' && value <= 'z') value = (char)(value - 'a' + 'A');
        if (value == wanted) return 1;
    }
    return 0;
}

uint32_t sim_kinematics_registry_count(void) {
    return (uint32_t)(sizeof(registry) / sizeof(registry[0]));
}

int sim_kinematics_registry_get(uint32_t index,
                                sim_kinematics_capability *capability) {
    if (capability == 0) return SIM_KIN_ERR_ARGUMENT;
    if (index >= sim_kinematics_registry_count()) return SIM_KIN_ERR_RANGE;
    *capability = registry[index];
    return SIM_KIN_OK;
}

int sim_kinematics_registry_find(const char *name,
                                 sim_kinematics_capability *capability) {
    uint32_t index;
    if (name == 0 || capability == 0) return SIM_KIN_ERR_ARGUMENT;
    if (path_like(name)) return SIM_KIN_ERR_DYNAMIC_MODULE;
    for (index = 0; index < sim_kinematics_registry_count(); ++index) {
        if (strcmp(name, registry[index].name) == 0) {
            *capability = registry[index];
            return SIM_KIN_OK;
        }
    }
    return SIM_KIN_ERR_UNKNOWN_TYPE;
}

int sim_kinematics_registry_require(const char *name, uint32_t capabilities,
                                    uint64_t parameter_mask,
                                    sim_kinematics_capability *capability) {
    sim_kinematics_capability found;
    int result = sim_kinematics_registry_find(name, &found);
    if (result != SIM_KIN_OK) return result;
    if ((found.capabilities & capabilities) != capabilities ||
        (found.parameter_mask & parameter_mask) != parameter_mask) return SIM_KIN_ERR_CAPABILITY;
    if (capability != 0) *capability = found;
    return SIM_KIN_OK;
}

int sim_kinematics_registry_validate_config(const char *name,
                                            const sim_kinematics_config *config,
                                            sim_kinematics_capability *capability) {
    sim_kinematics_capability found;
    sim_kinematics instance;
    const char *coordinates;
    uint32_t joint_count;
    size_t index;
    int result;
    if (config == 0) return SIM_KIN_ERR_ARGUMENT;
    result = sim_kinematics_registry_find(name, &found);
    if (result != SIM_KIN_OK) return result;
    if (config->type != found.type) return SIM_KIN_ERR_CONFIGURATION;
    joint_count = config->kinematics_joint_count == 0 ? config->joint_count : config->kinematics_joint_count;
    if (joint_count < found.min_joints || joint_count > found.max_joints) return SIM_KIN_ERR_CONFIGURATION;
    coordinates = config->coordinates[0] == '\0' ? "XYZABCUVW" : config->coordinates;
    for (index = 0; found.required_coordinates[index] != '\0'; ++index)
        if (!has_coordinate_casefold(coordinates, found.required_coordinates[index])) return SIM_KIN_ERR_CONFIGURATION;
    result = sim_kinematics_init(&instance, config);
    if (result != SIM_KIN_OK) return result;
    if (capability != 0) *capability = found;
    return SIM_KIN_OK;
}

uint64_t sim_kinematics_registry_state_hash(void) {
    return registry_fnv1a64((const unsigned char *)registry, sizeof(registry));
}

#undef BASE_PARAMS
#undef COMMON