#include "sim_kinematics_home_limit.h"

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

static uint64_t home_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_home_limit_state *state) {
    memset(state, 0, sizeof(*state));
    state->last_error = SIM_KIN_HOME_OK;
    state->limit_error = SIM_KIN_OK;
    state->kinematics_error = SIM_KIN_OK;
}

int sim_kinematics_home_limit_init(sim_kinematics_home_limit *home_limit,
                                   sim_kinematics *kinematics,
                                   uint32_t joint_count) {
    uint32_t configured_count;
    if (home_limit == 0 || kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
    if (!kinematics->configured) return SIM_KIN_HOME_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_HOME_ERR_CONFIGURATION;
    memset(home_limit, 0, sizeof(*home_limit));
    home_limit->kinematics = kinematics;
    home_limit->state.joint_count = joint_count;
    state_zero(&home_limit->state);
    home_limit->state.joint_count = joint_count;
    return SIM_KIN_HOME_OK;
}

int sim_kinematics_home_limit_reset(sim_kinematics_home_limit *home_limit) {
    uint32_t joint_count;
    if (home_limit == 0 || home_limit->kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
    joint_count = home_limit->state.joint_count;
    state_zero(&home_limit->state);
    home_limit->state.joint_count = joint_count;
    return SIM_KIN_HOME_OK;
}

int sim_kinematics_home_limit_home(sim_kinematics_home_limit *home_limit,
                                   uint32_t home_sequence) {
    sim_posemath_pose pose;
    double joints[SIM_KIN_MAX_JOINTS] = {0};
    int result;
    if (home_limit == 0 || home_limit->kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
    if (home_sequence > 0xffffu) return SIM_KIN_HOME_ERR_SEQUENCE;
    result = sim_kinematics_home(home_limit->kinematics, joints,
                                 home_limit->state.joint_count, &pose);
    if (result != SIM_KIN_OK) {
        home_limit->state.last_error = SIM_KIN_HOME_ERR_FORWARD;
        home_limit->state.kinematics_error = result;
        return home_limit->state.last_error;
    }
    memcpy(home_limit->state.joints, joints, sizeof(double) * home_limit->state.joint_count);
    home_limit->state.pose = pose;
    home_limit->state.home_sequence = home_sequence;
    home_limit->state.homed_mask = home_limit->state.joint_count >= 32 ? UINT32_MAX : (UINT32_C(1) << home_limit->state.joint_count) - 1U;
    home_limit->state.homed = 1;
    home_limit->state.tick += 1;
    home_limit->state.last_error = SIM_KIN_HOME_OK;
    home_limit->state.limit_error = SIM_KIN_OK;
    home_limit->state.kinematics_error = SIM_KIN_OK;
    return SIM_KIN_HOME_OK;
}

int sim_kinematics_home_limit_home_joint(sim_kinematics_home_limit *home_limit,
                                         uint32_t home_sequence, uint32_t joint) {
    sim_posemath_pose pose;
    double joints[SIM_KIN_MAX_JOINTS] = {0};
    int result;
    if (home_limit == 0 || home_limit->kinematics == 0 || joint >= home_limit->state.joint_count) return SIM_KIN_HOME_ERR_ARGUMENT;
    if (home_sequence > 0xffffu) return SIM_KIN_HOME_ERR_SEQUENCE;
    if (home_limit->state.homed_mask != 0) memcpy(joints, home_limit->state.joints, sizeof(double) * home_limit->state.joint_count);
    else {
        result = sim_kinematics_home(home_limit->kinematics, joints, home_limit->state.joint_count, &pose);
        if (result != SIM_KIN_OK) return SIM_KIN_HOME_ERR_FORWARD;
    }
    joints[joint] = 0.0;
    result = sim_kinematics_forward(home_limit->kinematics, joints, home_limit->state.joint_count, &pose);
    if (result != SIM_KIN_OK) return SIM_KIN_HOME_ERR_FORWARD;
    memcpy(home_limit->state.joints, joints, sizeof(double) * home_limit->state.joint_count);
    home_limit->state.pose = pose;
    home_limit->state.home_sequence = home_sequence;
    home_limit->state.homed_mask |= UINT32_C(1) << joint;
    home_limit->state.homed = home_limit->state.homed_mask == (home_limit->state.joint_count >= 32 ? UINT32_MAX : (UINT32_C(1) << home_limit->state.joint_count) - 1U);
    home_limit->state.tick += 1;
    home_limit->state.last_error = SIM_KIN_HOME_OK;
    home_limit->state.limit_error = SIM_KIN_OK;
    home_limit->state.kinematics_error = SIM_KIN_OK;
    return SIM_KIN_HOME_OK;
}

int sim_kinematics_home_limit_check(sim_kinematics_home_limit *home_limit,
                                    const double *joints,
                                    uint32_t joint_count) {
    sim_posemath_pose pose;
    uint32_t index;
    int result;
    if (home_limit == 0 || joints == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
    if (!home_limit->state.homed) {
        home_limit->state.last_error = SIM_KIN_HOME_ERR_UNHOMED;
        return home_limit->state.last_error;
    }
    if (joint_count != home_limit->state.joint_count) {
        home_limit->state.last_error = SIM_KIN_HOME_ERR_CONFIGURATION;
        return home_limit->state.last_error;
    }
    for (index = 0; index < joint_count; ++index) {
        if (!isfinite(joints[index])) {
            home_limit->state.last_error = SIM_KIN_HOME_ERR_NONFINITE;
            return home_limit->state.last_error;
        }
        if (home_limit->kinematics->config.limit_enabled[index] &&
            (joints[index] < home_limit->kinematics->config.joint_min[index] ||
             joints[index] > home_limit->kinematics->config.joint_max[index])) {
            home_limit->state.last_error = SIM_KIN_HOME_ERR_LIMIT;
            home_limit->state.limit_error = SIM_KIN_ERR_RANGE;
            return home_limit->state.last_error;
        }
    }
    result = sim_kinematics_forward(home_limit->kinematics, joints, joint_count, &pose);
    if (result != SIM_KIN_OK) {
        home_limit->state.last_error = SIM_KIN_HOME_ERR_FORWARD;
        home_limit->state.kinematics_error = result;
        return home_limit->state.last_error;
    }
    memcpy(home_limit->state.joints, joints, sizeof(double) * joint_count);
    home_limit->state.pose = pose;
    home_limit->state.tick += 1;
    home_limit->state.last_error = SIM_KIN_HOME_OK;
    home_limit->state.limit_error = SIM_KIN_OK;
    home_limit->state.kinematics_error = SIM_KIN_OK;
    return SIM_KIN_HOME_OK;
}

int sim_kinematics_home_limit_read(const sim_kinematics_home_limit *home_limit,
                                   sim_kinematics_home_limit_state *state) {
    if (home_limit == 0 || state == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
    *state = home_limit->state;
    return SIM_KIN_HOME_OK;
}

uint64_t sim_kinematics_home_limit_state_hash(const sim_kinematics_home_limit *home_limit) {
    sim_kinematics_home_limit_state state;
    unsigned char bytes[sizeof(state)];
    uint32_t index;
    if (home_limit == 0) return 0;
    state = home_limit->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, &state, sizeof(state));
    return home_fnv1a64(bytes, sizeof(bytes));
}