#include "sim_kinematics.h"

#include <string.h>

static uint64_t user_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 valid_static_name(const char *name) {
    size_t index;
    if (name == 0 || name[0] == '\0') return 0;
    for (index = 0; index < SIM_KIN_USER_NAME_CAPACITY && name[index] != '\0'; ++index) {
        unsigned char value = (unsigned char)name[index];
        if (value < 0x21 || value > 0x7e || value == '/' || value == '\\') return 0;
    }
    return index < SIM_KIN_USER_NAME_CAPACITY;
}

int sim_kinematics_user_init(sim_kinematics_user *user,
                             const sim_kinematics_user_descriptor *descriptor) {
    sim_kinematics_config config;
    uint32_t required = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
                        SIM_KIN_USER_CAP_STATIC;
    int result;
    if (user == 0 || descriptor == 0) return SIM_KIN_ERR_ARGUMENT;
    if (descriptor->abi_version != SIM_KIN_USER_ABI_VERSION) return SIM_KIN_ERR_ABI_VERSION;
    if ((descriptor->capabilities & required) != required ||
        (descriptor->capabilities & ~required) != 0) return SIM_KIN_ERR_CAPABILITY;
    if (!valid_static_name(descriptor->name)) return SIM_KIN_ERR_DYNAMIC_MODULE;
    config = descriptor->config;
    if (descriptor->implementation == SIM_KIN_USER_IDENTITY) config.type = SIM_KIN_TRIVKINS;
    else if (descriptor->implementation == SIM_KIN_USER_GENSER) config.type = SIM_KIN_GENSER;
    else return SIM_KIN_ERR_MISSING_MODULE;
    result = sim_kinematics_init(&user->module, &config);
    if (result != SIM_KIN_OK) return result;
    user->descriptor = *descriptor;
    user->descriptor.config = user->module.config;
    user->forward_calls = 0;
    user->inverse_calls = 0;
    user->initialized = 1;
    return SIM_KIN_OK;
}

int sim_kinematics_user_bind_switch(sim_kinematics_user *user,
                                    sim_kinematics_switch *switcher, int mode,
                                    uint8_t fwd_iterates) {
    if (user == 0 || switcher == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
    return sim_kinematics_switch_register(switcher, mode, &user->module, fwd_iterates);
}

int sim_kinematics_user_forward(sim_kinematics_user *user, const double *joints,
                                uint32_t joint_count, sim_posemath_pose *pose,
                                sim_kinematics_result_info *info) {
    if (user == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
    user->forward_calls += 1;
    return sim_kinematics_forward_ex(&user->module, joints, joint_count, 0, pose, info);
}

int sim_kinematics_user_inverse(sim_kinematics_user *user,
                                const sim_posemath_pose *pose, double *joints,
                                uint32_t joint_count,
                                sim_kinematics_result_info *info) {
    if (user == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
    user->inverse_calls += 1;
    return sim_kinematics_inverse_ex(&user->module, pose, joint_count, 0, joints, info);
}

int sim_kinematics_user_reject_dynamic(const char *module_path) {
    return module_path == 0 ? SIM_KIN_ERR_ARGUMENT : SIM_KIN_ERR_DYNAMIC_MODULE;
}

uint64_t sim_kinematics_user_state_hash(const sim_kinematics_user *user,
                                        const double *joints, uint32_t joint_count,
                                        const sim_posemath_pose *pose) {
    unsigned char bytes[sizeof(uint32_t) * 5 + sizeof(int) + SIM_KIN_USER_NAME_CAPACITY + sizeof(uint64_t)];
    uint64_t module_hash;
    size_t offset = 0;
    if (user == 0 || joints == 0 || pose == 0 || !user->initialized) return 0;
    module_hash = sim_kinematics_state_hash(&user->module, joints, joint_count, pose);
    memcpy(bytes + offset, &user->descriptor.abi_version, sizeof(user->descriptor.abi_version)); offset += sizeof(user->descriptor.abi_version);
    memcpy(bytes + offset, &user->descriptor.capabilities, sizeof(user->descriptor.capabilities)); offset += sizeof(user->descriptor.capabilities);
    memcpy(bytes + offset, &user->descriptor.implementation, sizeof(user->descriptor.implementation)); offset += sizeof(user->descriptor.implementation);
    memcpy(bytes + offset, user->descriptor.name, sizeof(user->descriptor.name)); offset += sizeof(user->descriptor.name);
    memcpy(bytes + offset, &user->initialized, sizeof(user->initialized)); offset += sizeof(user->initialized);
    memcpy(bytes + offset, &user->forward_calls, sizeof(user->forward_calls)); offset += sizeof(user->forward_calls);
    memcpy(bytes + offset, &user->inverse_calls, sizeof(user->inverse_calls)); offset += sizeof(user->inverse_calls);
    memcpy(bytes + offset, &module_hash, sizeof(module_hash)); offset += sizeof(module_hash);
    return user_fnv1a64(bytes, offset);
}