#include "sim_kinematics.h"

#include <math.h>
#include <string.h>

static uint64_t switch_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_mode(int mode) {
    return mode >= 0 && mode < (int)SIM_KIN_SWITCH_MAX_TYPES;
}

static void switch_info(sim_kinematics_result_info *info) {
    if (info != 0) memset(info, 0, sizeof(*info));
}

int sim_kinematics_switch_init(sim_kinematics_switch *switcher, int default_mode,
                               uint8_t fwd_iterates_mask) {
    if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!valid_mode(default_mode)) return SIM_KIN_ERR_INVALID_MODE;
    memset(switcher, 0, sizeof(*switcher));
    switcher->active_mode = default_mode;
    switcher->pending_mode = default_mode;
    switcher->fwd_iterates_mask = fwd_iterates_mask & 0x07u;
    return SIM_KIN_OK;
}

int sim_kinematics_switch_register(sim_kinematics_switch *switcher, int mode,
                                   sim_kinematics *module, uint8_t fwd_iterates) {
    if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
    if (module == 0 || !module->configured) return SIM_KIN_ERR_MISSING_MODULE;
    switcher->modules[mode] = module;
    switcher->registered[mode] = 1;
    if (fwd_iterates) switcher->fwd_iterates_mask |= (uint8_t)(1u << mode);
    else switcher->fwd_iterates_mask &= (uint8_t)~(1u << mode);
    return SIM_KIN_OK;
}

int sim_kinematics_switch_begin(sim_kinematics_switch *switcher, int mode) {
    if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
    if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
    if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
    switcher->pending_mode = mode;
    switcher->switching = (uint8_t)(mode != switcher->active_mode);
    return SIM_KIN_OK;
}

int sim_kinematics_switch_abort(sim_kinematics_switch *switcher) {
    if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
    switcher->pending_mode = switcher->active_mode;
    switcher->switching = 0;
    switcher->abort_count += 1;
    return SIM_KIN_OK;
}

int sim_kinematics_switch_commit(sim_kinematics_switch *switcher, double *joints,
                                 uint32_t joint_count, sim_posemath_pose *pose,
                                 sim_kinematics_result_info *info) {
    sim_kinematics *target;
    double remapped[SIM_KIN_MAX_JOINTS] = {0};
    sim_kinematics_result_info local_info;
    int result;
    switch_info(info);
    if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
    target = switcher->modules[switcher->pending_mode];
    if (target == 0 || !switcher->registered[switcher->pending_mode]) {
        switcher->switching = 0;
        switcher->pending_mode = switcher->active_mode;
        return SIM_KIN_ERR_MISSING_MODULE;
    }
    if (joint_count > SIM_KIN_MAX_JOINTS) return SIM_KIN_ERR_CONFIGURATION;
    result = sim_kinematics_inverse_ex(target, pose, joint_count, 0, remapped,
                                       &local_info);
    if (result != SIM_KIN_OK) {
        switcher->switching = 0;
        switcher->pending_mode = switcher->active_mode;
        if (info != 0) *info = local_info;
        return result;
    }
    memcpy(joints, remapped, sizeof(double) * joint_count);
    switcher->active_mode = switcher->pending_mode;
    switcher->pending_mode = switcher->active_mode;
    switcher->switching = 0;
    switcher->generation += 1;
    switcher->switch_count += 1;
    switcher->last_pose[switcher->active_mode] = *pose;
    switcher->have_last_pose[switcher->active_mode] = 1;
    if (info != 0) *info = local_info;
    return SIM_KIN_OK;
}

int sim_kinematics_switch_forward(sim_kinematics_switch *switcher,
                                  const double *joints, uint32_t joint_count,
                                  sim_posemath_pose *pose,
                                  sim_kinematics_result_info *info) {
    sim_kinematics *module;
    int result;
    switch_info(info);
    if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
    if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
    if (!valid_mode(switcher->active_mode)) return SIM_KIN_ERR_INVALID_MODE;
    module = switcher->modules[switcher->active_mode];
    if (module == 0 || !switcher->registered[switcher->active_mode]) return SIM_KIN_ERR_MISSING_MODULE;
    result = sim_kinematics_forward_ex(module, joints, joint_count, 0, pose, info);
    if (result == SIM_KIN_OK) {
        switcher->last_pose[switcher->active_mode] = *pose;
        switcher->have_last_pose[switcher->active_mode] = 1;
    }
    return result;
}

int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher,
                                  const sim_posemath_pose *pose, double *joints,
                                  uint32_t joint_count,
                                  sim_kinematics_result_info *info) {
    sim_kinematics *module;
    int result;
    switch_info(info);
    if (switcher == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
    if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
    if (!valid_mode(switcher->active_mode)) return SIM_KIN_ERR_INVALID_MODE;
    module = switcher->modules[switcher->active_mode];
    if (module == 0 || !switcher->registered[switcher->active_mode]) return SIM_KIN_ERR_MISSING_MODULE;
    result = sim_kinematics_inverse_ex(module, pose, joint_count, 0, joints, info);
    if (result == SIM_KIN_OK) {
        switcher->last_pose[switcher->active_mode] = *pose;
        switcher->have_last_pose[switcher->active_mode] = 1;
    }
    return result;
}

int sim_kinematics_switch_inverse_mode(const sim_kinematics_switch *switcher, int mode,
                                       const sim_posemath_pose *pose, double *joints,
                                       uint32_t joint_count,
                                       sim_kinematics_result_info *info) {
    if (switcher == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
    if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
    return sim_kinematics_inverse_ex(switcher->modules[mode], pose, joint_count, 0, joints, info);
}

int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode,
                                 double *joints, uint32_t joint_count,
                                 sim_posemath_pose *pose,
                                 sim_kinematics_result_info *info) {
    sim_posemath_pose current_pose;
    int result;
    switch_info(info);
    if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
    if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
    if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
    if (mode == switcher->active_mode) return sim_kinematics_switch_forward(switcher, joints, joint_count, pose, info);
    result = sim_kinematics_switch_forward(switcher, joints, joint_count, &current_pose, info);
    if (result != SIM_KIN_OK) return result;
    result = sim_kinematics_switch_begin(switcher, mode);
    if (result != SIM_KIN_OK) return result;
    result = sim_kinematics_switch_commit(switcher, joints, joint_count, &current_pose, info);
    if (result == SIM_KIN_OK) *pose = current_pose;
    return result;
}

int sim_kinematics_switch_select_physical(sim_kinematics_switch *switcher, int mode,
                                          const double *joints, uint32_t joint_count,
                                          sim_posemath_pose *pose,
                                          sim_kinematics_result_info *info) {
    int result;
    if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
    if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
    if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
    switcher->active_mode = mode;
    switcher->pending_mode = mode;
    switcher->switching = 0;
    switcher->generation += 1;
    switcher->switch_count += 1;
    result = sim_kinematics_switch_forward(switcher, joints, joint_count, pose, info);
    if (result != SIM_KIN_OK) return result;
    switcher->last_pose[mode] = *pose;
    switcher->have_last_pose[mode] = 1;
    return SIM_KIN_OK;
}

int sim_kinematics_switch_active(const sim_kinematics_switch *switcher) {
    return switcher == 0 ? SIM_KIN_ERR_ARGUMENT : switcher->active_mode;
}

uint64_t sim_kinematics_switch_state_hash(const sim_kinematics_switch *switcher,
                                          const double *joints, uint32_t joint_count,
                                          const sim_posemath_pose *pose) {
    unsigned char bytes[256];
    size_t offset = 0;
    uint64_t module_hash;
    if (switcher == 0 || joints == 0 || pose == 0 || joint_count > SIM_KIN_MAX_JOINTS) return 0;
    if (!valid_mode(switcher->active_mode) || switcher->modules[switcher->active_mode] == 0) return 0;
    module_hash = sim_kinematics_state_hash(switcher->modules[switcher->active_mode], joints, joint_count, pose);
    memcpy(bytes + offset, &switcher->active_mode, sizeof(switcher->active_mode)); offset += sizeof(switcher->active_mode);
    memcpy(bytes + offset, &switcher->pending_mode, sizeof(switcher->pending_mode)); offset += sizeof(switcher->pending_mode);
    memcpy(bytes + offset, &switcher->switching, sizeof(switcher->switching)); offset += sizeof(switcher->switching);
    memcpy(bytes + offset, &switcher->generation, sizeof(switcher->generation)); offset += sizeof(switcher->generation);
    memcpy(bytes + offset, &switcher->switch_count, sizeof(switcher->switch_count)); offset += sizeof(switcher->switch_count);
    memcpy(bytes + offset, &switcher->abort_count, sizeof(switcher->abort_count)); offset += sizeof(switcher->abort_count);
    memcpy(bytes + offset, &module_hash, sizeof(module_hash)); offset += sizeof(module_hash);
    return switch_fnv1a64(bytes, offset);
}