#include "sim_kinematics_motion.h"

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

static uint64_t motion_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 double normalized(double value) {
    return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
}

static void set_error(sim_kinematics_motion *adapter, int error,
                      int planner_error, int kinematics_error) {
    adapter->last_error = error;
    adapter->planner_error = planner_error;
    adapter->kinematics_error = kinematics_error;
}

int sim_kinematics_motion_reset(sim_kinematics_motion *adapter) {
    if (adapter == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
    if (adapter->kinematics == 0 || !adapter->kinematics->configured) return SIM_KIN_MOTION_ERR_CONFIGURATION;
    if (sim_tp_reset(&adapter->planner) != SIM_POSEMATH_OK) return SIM_KIN_MOTION_ERR_PLANNER;
    adapter->head = 0;
    adapter->count = 0;
    adapter->tick = 0;
    adapter->paused = 0;
    memset(&adapter->last_pose, 0, sizeof(adapter->last_pose));
    memset(adapter->last_joints, 0, sizeof(adapter->last_joints));
    set_error(adapter, SIM_KIN_MOTION_OK, SIM_POSEMATH_OK, SIM_KIN_OK);
    return SIM_KIN_MOTION_OK;
}

int sim_kinematics_motion_init(sim_kinematics_motion *adapter,
                               sim_kinematics *kinematics, double cycle_time,
                               double velocity_limit, double acceleration_limit) {
    uint32_t joint_count;
    int result;
    if (adapter == 0 || kinematics == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
    if (!kinematics->configured) return SIM_KIN_MOTION_ERR_CONFIGURATION;
    joint_count = kinematics->config.kinematics_joint_count == 0 ?
                  kinematics->config.joint_count : kinematics->config.kinematics_joint_count;
    if (joint_count == 0 || joint_count > SIM_KIN_MAX_JOINTS) return SIM_KIN_MOTION_ERR_CONFIGURATION;
    memset(adapter, 0, sizeof(*adapter));
    result = sim_tp_init(&adapter->planner, cycle_time, velocity_limit, acceleration_limit);
    if (result != SIM_POSEMATH_OK) return SIM_KIN_MOTION_ERR_CONFIGURATION;
    adapter->kinematics = kinematics;
    adapter->joint_count = joint_count;
    return sim_kinematics_motion_reset(adapter);
}

int sim_kinematics_motion_enqueue(sim_kinematics_motion *adapter,
                                  const sim_posemath_segment *segment,
                                  uint32_t *id_out) {
    int result;
    if (adapter == 0 || segment == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
    result = sim_tp_enqueue(&adapter->planner, segment, id_out);
    if (result != SIM_POSEMATH_OK) {
        set_error(adapter, result == SIM_POSEMATH_ERR_NONFINITE ? SIM_KIN_MOTION_ERR_NONFINITE : SIM_KIN_MOTION_ERR_PLANNER, result, SIM_KIN_OK);
        return adapter->last_error;
    }
    set_error(adapter, SIM_KIN_MOTION_OK, SIM_POSEMATH_OK, SIM_KIN_OK);
    return SIM_KIN_MOTION_OK;
}

int sim_kinematics_motion_pause(sim_kinematics_motion *adapter, uint32_t paused) {
    if (adapter == 0 || paused > 1) return SIM_KIN_MOTION_ERR_ARGUMENT;
    adapter->paused = paused;
    return SIM_KIN_MOTION_OK;
}

int sim_kinematics_motion_step(sim_kinematics_motion *adapter) {
    sim_tp planner_snapshot;
    sim_kinematics kinematics_snapshot;
    sim_tp_state planner_state;
    sim_kinematics_motion_sample sample;
    sim_kinematics_result_info info;
    uint32_t index;
    int result;
    if (adapter == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
    if (adapter->kinematics == 0 || !adapter->kinematics->configured) return SIM_KIN_MOTION_ERR_CONFIGURATION;
    if (adapter->paused) {
        set_error(adapter, SIM_KIN_MOTION_ERR_STATE, SIM_POSEMATH_OK, SIM_KIN_OK);
        return adapter->last_error;
    }
    if (adapter->count >= SIM_KIN_MOTION_QUEUE_CAPACITY) {
        set_error(adapter, SIM_KIN_MOTION_ERR_CAPACITY, SIM_POSEMATH_OK, SIM_KIN_OK);
        return adapter->last_error;
    }
    planner_snapshot = adapter->planner;
    kinematics_snapshot = *adapter->kinematics;
    result = sim_tp_step(&adapter->planner);
    if (result != SIM_POSEMATH_OK) {
        adapter->planner = planner_snapshot;
        set_error(adapter, SIM_KIN_MOTION_ERR_PLANNER, result, SIM_KIN_OK);
        return adapter->last_error;
    }
    if (sim_tp_read_state(&adapter->planner, &planner_state) != SIM_POSEMATH_OK) {
        adapter->planner = planner_snapshot;
        set_error(adapter, SIM_KIN_MOTION_ERR_PLANNER, SIM_POSEMATH_ERR_STATE, SIM_KIN_OK);
        return adapter->last_error;
    }
    memset(&sample, 0, sizeof(sample));
    sample.pose = planner_state.position;
    result = sim_kinematics_inverse_ex(adapter->kinematics, &sample.pose,
                                       adapter->joint_count, 0, sample.joints,
                                       &info);
    if (result != SIM_KIN_OK) {
        adapter->planner = planner_snapshot;
        *adapter->kinematics = kinematics_snapshot;
        set_error(adapter, result == SIM_KIN_ERR_NONFINITE ? SIM_KIN_MOTION_ERR_NONFINITE : SIM_KIN_MOTION_ERR_INVERSE, SIM_POSEMATH_OK, result);
        return adapter->last_error;
    }
    sample.joint_count = adapter->joint_count;
    sample.tick = planner_state.tick;
    sample.planner_id = planner_state.active_id;
    index = (adapter->head + adapter->count) % SIM_KIN_MOTION_QUEUE_CAPACITY;
    adapter->samples[index] = sample;
    adapter->count += 1;
    adapter->tick = sample.tick;
    adapter->last_pose = sample.pose;
    memcpy(adapter->last_joints, sample.joints, sizeof(sample.joints));
    set_error(adapter, SIM_KIN_MOTION_OK, SIM_POSEMATH_OK, SIM_KIN_OK);
    return SIM_KIN_MOTION_OK;
}

int sim_kinematics_motion_peek(const sim_kinematics_motion *adapter,
                               sim_kinematics_motion_sample *sample) {
    if (adapter == 0 || sample == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
    if (adapter->count == 0) return SIM_KIN_MOTION_ERR_STATE;
    *sample = adapter->samples[adapter->head];
    return SIM_KIN_MOTION_OK;
}

int sim_kinematics_motion_pop(sim_kinematics_motion *adapter,
                              sim_kinematics_motion_sample *sample) {
    int result = sim_kinematics_motion_peek(adapter, sample);
    if (result != SIM_KIN_MOTION_OK) return result;
    adapter->head = (adapter->head + 1) % SIM_KIN_MOTION_QUEUE_CAPACITY;
    adapter->count -= 1;
    return SIM_KIN_MOTION_OK;
}

uint32_t sim_kinematics_motion_depth(const sim_kinematics_motion *adapter) {
    return adapter == 0 ? 0 : adapter->count;
}

uint64_t sim_kinematics_motion_state_hash(const sim_kinematics_motion *adapter) {
    unsigned char bytes[16384];
    size_t offset = 0;
    uint64_t planner_hash;
    uint64_t kinematics_hash;
    uint32_t sample_index;
    if (adapter == 0 || adapter->kinematics == 0) return 0;
    planner_hash = sim_tp_state_hash(&adapter->planner);
    kinematics_hash = sim_kinematics_state_hash(adapter->kinematics, adapter->last_joints,
                                                adapter->joint_count, &adapter->last_pose);
#define APPEND(value) do { memcpy(bytes + offset, &(value), sizeof(value)); offset += sizeof(value); } while (0)
    APPEND(adapter->head); APPEND(adapter->count); APPEND(adapter->tick);
    APPEND(adapter->joint_count); APPEND(adapter->paused); APPEND(adapter->last_error);
    APPEND(adapter->planner_error); APPEND(adapter->kinematics_error);
    APPEND(planner_hash); APPEND(kinematics_hash);
    for (sample_index = 0; sample_index < adapter->count; ++sample_index) {
        const sim_kinematics_motion_sample *sample = &adapter->samples[(adapter->head + sample_index) % SIM_KIN_MOTION_QUEUE_CAPACITY];
        sim_posemath_pose pose = sample->pose;
        double joints[SIM_KIN_MAX_JOINTS] = {0};
        double *pose_values = &pose.tran.x;
        uint32_t joint_index;
        for (joint_index = 0; joint_index < 9; ++joint_index) pose_values[joint_index] = normalized(pose_values[joint_index]);
        for (joint_index = 0; joint_index < sample->joint_count; ++joint_index) joints[joint_index] = normalized(sample->joints[joint_index]);
        memcpy(bytes + offset, &pose, sizeof(pose)); offset += sizeof(pose);
        memcpy(bytes + offset, joints, sizeof(joints)); offset += sizeof(joints);
        APPEND(sample->joint_count); APPEND(sample->tick); APPEND(sample->planner_id);
    }
#undef APPEND
    return motion_fnv1a64(bytes, offset);
}