#include "sim_kinematics_feedback.h"

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

static uint64_t feedback_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 void state_zero(sim_kinematics_feedback_state *state) {
    memset(state, 0, sizeof(*state));
    state->last_error = SIM_KIN_FEEDBACK_OK;
    state->kinematics_error = SIM_KIN_OK;
}

int sim_kinematics_feedback_init(sim_kinematics_feedback *feedback,
                                 sim_kinematics *kinematics,
                                 uint32_t joint_count) {
    uint32_t configured_count;
    if (feedback == 0 || kinematics == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
    if (!kinematics->configured) return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
    configured_count = kinematics->config.kinematics_joint_count == 0 ?
                       kinematics->config.joint_count : kinematics->config.kinematics_joint_count;
    if (joint_count == 0 || joint_count != configured_count || joint_count > SIM_KIN_MAX_JOINTS)
        return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
    memset(feedback, 0, sizeof(*feedback));
    feedback->kinematics = kinematics;
    feedback->state.joint_count = joint_count;
    state_zero(&feedback->state);
    feedback->state.joint_count = joint_count;
    return SIM_KIN_FEEDBACK_OK;
}

int sim_kinematics_feedback_reset(sim_kinematics_feedback *feedback) {
    uint32_t joint_count;
    if (feedback == 0 || feedback->kinematics == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
    joint_count = feedback->state.joint_count;
    state_zero(&feedback->state);
    feedback->state.joint_count = joint_count;
    return SIM_KIN_FEEDBACK_OK;
}

int sim_kinematics_feedback_update(sim_kinematics_feedback *feedback,
                                   const double *joints, uint32_t joint_count,
                                   uint64_t inverse_flags,
                                   sim_kinematics_result_info *info) {
    sim_posemath_pose pose;
    sim_kinematics_result_info local_info;
    uint32_t index;
    int result;
    if (feedback == 0 || feedback->kinematics == 0 || joints == 0)
        return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
    if (joint_count != feedback->state.joint_count) return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
    for (index = 0; index < joint_count; ++index)
        if (!isfinite(joints[index])) {
            feedback->state.last_error = SIM_KIN_FEEDBACK_ERR_NONFINITE;
            feedback->state.kinematics_error = SIM_KIN_ERR_NONFINITE;
            return feedback->state.last_error;
        }
    memset(&pose, 0, sizeof(pose));
    result = sim_kinematics_forward_ex(feedback->kinematics, joints, joint_count,
                                       inverse_flags, &pose, &local_info);
    if (result != SIM_KIN_OK) {
        feedback->state.last_error = SIM_KIN_FEEDBACK_ERR_FORWARD;
        feedback->state.kinematics_error = result;
        if (info != 0) *info = local_info;
        return feedback->state.last_error;
    }
    feedback->state.pose = pose;
    memcpy(feedback->state.joints, joints, sizeof(double) * joint_count);
    feedback->state.forward_flags = local_info.forward_flags;
    feedback->state.inverse_flags = local_info.inverse_flags;
    feedback->state.tick += 1;
    feedback->state.valid = 1;
    feedback->state.last_error = SIM_KIN_FEEDBACK_OK;
    feedback->state.kinematics_error = SIM_KIN_OK;
    if (info != 0) *info = local_info;
    return SIM_KIN_FEEDBACK_OK;
}

int sim_kinematics_feedback_read(const sim_kinematics_feedback *feedback,
                                 sim_kinematics_feedback_state *state) {
    if (feedback == 0 || state == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
    if (!feedback->state.valid) return SIM_KIN_FEEDBACK_ERR_STATE;
    *state = feedback->state;
    return SIM_KIN_FEEDBACK_OK;
}

uint64_t sim_kinematics_feedback_state_hash(const sim_kinematics_feedback *feedback) {
    unsigned char bytes[sizeof(sim_kinematics_feedback_state)];
    sim_kinematics_feedback_state state;
    size_t offset = 0;
    uint32_t index;
    if (feedback == 0) return 0;
    state = feedback->state;
    for (index = 0; index < 9; ++index) {
        double *pose_value = &state.pose.tran.x;
        pose_value[index] = isfinite(pose_value[index]) ? nearbyint(pose_value[index] * 1000000000.0) / 1000000000.0 : pose_value[index];
        if (index < state.joint_count) state.joints[index] = isfinite(state.joints[index]) ? nearbyint(state.joints[index] * 1000000000.0) / 1000000000.0 : state.joints[index];
    }
    memcpy(bytes + offset, &state, sizeof(state)); offset += sizeof(state);
    return feedback_fnv1a64(bytes, offset);
}