#include "sim_posemath.h"

#include "posemath.h"

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

static int finite_cart(const sim_posemath_cartesian *value) {
    return value != 0 && isfinite(value->x) && isfinite(value->y) && isfinite(value->z);
}

static int finite_pose(const sim_posemath_pose *value) {
    return value != 0 && finite_cart(&value->tran) && isfinite(value->a) &&
           isfinite(value->b) && isfinite(value->c) && isfinite(value->u) &&
           isfinite(value->v) && isfinite(value->w);
}

static int finite_segment(const sim_posemath_segment *value) {
    return value != 0 && finite_pose(&value->start) && finite_pose(&value->end) &&
           finite_cart(&value->center) && finite_cart(&value->normal) &&
           isfinite(value->velocity) && isfinite(value->acceleration);
}

static PmCartesian to_pm(const sim_posemath_cartesian *value) {
    PmCartesian result = {value->x, value->y, value->z};
    return result;
}

static void from_pm(const PmCartesian *value, sim_posemath_cartesian *output) {
    output->x = value->x;
    output->y = value->y;
    output->z = value->z;
}

int sim_posemath_is_finite(const sim_posemath_cartesian *value) {
    return finite_cart(value);
}

int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_cartesian *output) {
    PmCartesian source;
    PmCartesian result;
    double magnitude_squared = 0.0;
    if (input == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!finite_cart(input)) return SIM_POSEMATH_ERR_NONFINITE;
    source = to_pm(input);
    if (pmCartMagSq(&source, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
    if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
    if (pmCartUnit(&source, &result) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
    from_pm(&result, output);
    return SIM_POSEMATH_OK;
}

int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output) {
    PmCartesian lhs;
    PmCartesian rhs;
    if (left == 0 || right == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!finite_cart(left) || !finite_cart(right)) return SIM_POSEMATH_ERR_NONFINITE;
    lhs = to_pm(left);
    rhs = to_pm(right);
    if (pmCartCartDot(&lhs, &rhs, output) != 0 || !isfinite(*output)) return SIM_POSEMATH_ERR_NONFINITE;
    return SIM_POSEMATH_OK;
}

int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posemath_cartesian *end,
                            double fraction, sim_posemath_cartesian *output) {
    PmCartesian begin;
    PmCartesian finish;
    PmCartesian result;
    PmCartesian delta;
    double magnitude_squared = 0.0;
    if (start == 0 || end == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!finite_cart(start) || !finite_cart(end) || !isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
    if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
    begin = to_pm(start);
    finish = to_pm(end);
    if (pmCartCartSub(&finish, &begin, &delta) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
    if (pmCartMagSq(&delta, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
    if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
    if (pmCartScalMult(&delta, fraction, &delta) != 0 || pmCartCartAdd(&begin, &delta, &result)) return SIM_POSEMATH_ERR_DEGENERATE;
    if (!isfinite(result.x) || !isfinite(result.y) || !isfinite(result.z)) return SIM_POSEMATH_ERR_NONFINITE;
    from_pm(&result, output);
    return SIM_POSEMATH_OK;
}

static int finite_pose_value(const sim_posemath_pose *value) {
    return finite_pose(value);
}

int sim_posemath_pose_is_finite(const sim_posemath_pose *value) {
    return finite_pose_value(value);
}

int sim_posemath_pose_line_point(const sim_posemath_pose *start, const sim_posemath_pose *end,
                                 double fraction, sim_posemath_pose *output) {
    if (start == 0 || end == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!finite_pose_value(start) || !finite_pose_value(end) || !isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
    if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
    output->tran.x = start->tran.x + (end->tran.x - start->tran.x) * fraction;
    output->tran.y = start->tran.y + (end->tran.y - start->tran.y) * fraction;
    output->tran.z = start->tran.z + (end->tran.z - start->tran.z) * fraction;
    output->a = start->a + (end->a - start->a) * fraction;
    output->b = start->b + (end->b - start->b) * fraction;
    output->c = start->c + (end->c - start->c) * fraction;
    output->u = start->u + (end->u - start->u) * fraction;
    output->v = start->v + (end->v - start->v) * fraction;
    output->w = start->w + (end->w - start->w) * fraction;
    return finite_pose_value(output) ? SIM_POSEMATH_OK : SIM_POSEMATH_ERR_NONFINITE;
}

int sim_posemath_segment_validate(const sim_posemath_segment *segment) {
    double length_sq;
    double normal_sq;
    if (segment == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!finite_segment(segment)) return SIM_POSEMATH_ERR_NONFINITE;
    if (segment->motion_type < SIM_POSEMATH_LINEAR || segment->motion_type > SIM_POSEMATH_SPHERICAL) return SIM_POSEMATH_ERR_MOTION_TYPE;
    if (segment->velocity < 0.0 || segment->acceleration < 0.0 || segment->velocity > 1e9 || segment->acceleration > 1e12) return SIM_POSEMATH_ERR_RANGE;
    length_sq = (segment->end.tran.x - segment->start.tran.x) * (segment->end.tran.x - segment->start.tran.x) +
                (segment->end.tran.y - segment->start.tran.y) * (segment->end.tran.y - segment->start.tran.y) +
                (segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z) +
                (segment->end.a - segment->start.a) * (segment->end.a - segment->start.a) +
                (segment->end.b - segment->start.b) * (segment->end.b - segment->start.b) +
                (segment->end.c - segment->start.c) * (segment->end.c - segment->start.c) +
                (segment->end.u - segment->start.u) * (segment->end.u - segment->start.u) +
                (segment->end.v - segment->start.v) * (segment->end.v - segment->start.v) +
                (segment->end.w - segment->start.w) * (segment->end.w - segment->start.w);
    if (!isfinite(length_sq) || (length_sq <= 1e-24 && segment->motion_type != SIM_POSEMATH_CIRCULAR)) return SIM_POSEMATH_ERR_DEGENERATE;
    if (segment->motion_type == SIM_POSEMATH_CIRCULAR) {
        normal_sq = segment->normal.x * segment->normal.x + segment->normal.y * segment->normal.y + segment->normal.z * segment->normal.z;
        if (!isfinite(normal_sq) || normal_sq <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
        if (segment->turn < -1000000 || segment->turn > 1000000) return SIM_POSEMATH_ERR_RANGE;
    }
    return SIM_POSEMATH_OK;
}

int sim_posemath_segment_point(const sim_posemath_segment *segment, double fraction,
                               sim_posemath_pose *output) {
    int result = sim_posemath_segment_validate(segment);
    if (result != SIM_POSEMATH_OK) return result;
    if (output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
    if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
    if (segment->motion_type != SIM_POSEMATH_CIRCULAR)
        return sim_posemath_pose_line_point(&segment->start, &segment->end, fraction, output);
    {
        PmCartesian start = to_pm(&segment->start.tran);
        PmCartesian end = to_pm(&segment->end.tran);
        PmCartesian center = to_pm(&segment->center);
        PmCartesian normal = to_pm(&segment->normal);
        PmCircle circle;
        PmCartesian point;
        if (pmCircleInit(&circle, &start, &end, &center, &normal, segment->turn) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
        if (pmCirclePoint(&circle, circle.angle * fraction, &point) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
        output->tran.x = point.x; output->tran.y = point.y; output->tran.z = point.z;
        output->a = segment->start.a + (segment->end.a - segment->start.a) * fraction;
        output->b = segment->start.b + (segment->end.b - segment->start.b) * fraction;
        output->c = segment->start.c + (segment->end.c - segment->start.c) * fraction;
        output->u = segment->start.u + (segment->end.u - segment->start.u) * fraction;
        output->v = segment->start.v + (segment->end.v - segment->start.v) * fraction;
        output->w = segment->start.w + (segment->end.w - segment->start.w) * fraction;
        return finite_pose_value(output) ? SIM_POSEMATH_OK : SIM_POSEMATH_ERR_NONFINITE;
    }
}

static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
    uint64_t hash = UINT64_C(1469598103934665603);
    size_t index;
    for (index = 0; index < length; ++index) {
        hash ^= (uint64_t)bytes[index];
        hash *= UINT64_C(1099511628211);
    }
    return hash;
}

uint64_t sim_posemath_hash_state(const sim_tp_state *state) {
    unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) * 3 + sizeof(int) * 2];
    size_t offset = 0;
    const double *doubles;
    const uint32_t *uints;
    const int *ints;
    size_t index;
    if (state == 0) return 0;
    doubles = (const double *)&state->position;
    for (index = 0; index < 11; ++index) {
        memcpy(bytes + offset, doubles + index, sizeof(double));
        offset += sizeof(double);
    }
    uints = &state->tick;
    for (index = 0; index < 3; ++index) {
        memcpy(bytes + offset, uints + index, sizeof(uint32_t));
        offset += sizeof(uint32_t);
    }
    ints = &state->motion_type;
    for (index = 0; index < 2; ++index) {
        memcpy(bytes + offset, ints + index, sizeof(int));
        offset += sizeof(int);
    }
    return fnv1a64(bytes, offset);
}

static int valid_tp_config(double cycle_time, double velocity_limit, double acceleration_limit) {
    return isfinite(cycle_time) && isfinite(velocity_limit) && isfinite(acceleration_limit) &&
           cycle_time > 0.0 && cycle_time <= 60.0 && velocity_limit >= 0.0 && velocity_limit <= 1e9 &&
           acceleration_limit >= 0.0 && acceleration_limit <= 1e12;
}

int sim_tp_reset(sim_tp *tp) {
    if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    tp->head = 0; tp->count = 0; tp->next_id = 1; tp->active_id = 0; tp->tick = 0;
    tp->progress = 0.0; tp->current_velocity = 0.0; tp->motion_type = 0; tp->status = SIM_POSEMATH_OK;
    tp->position.tran.x = 0.0; tp->position.tran.y = 0.0; tp->position.tran.z = 0.0;
    tp->position.a = tp->position.b = tp->position.c = tp->position.u = tp->position.v = tp->position.w = 0.0;
    return SIM_POSEMATH_OK;
}

int sim_tp_init(sim_tp *tp, double cycle_time, double velocity_limit, double acceleration_limit) {
    if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (!valid_tp_config(cycle_time, velocity_limit, acceleration_limit)) return SIM_POSEMATH_ERR_RANGE;
    tp->cycle_time = cycle_time; tp->velocity_limit = velocity_limit; tp->acceleration_limit = acceleration_limit;
    return sim_tp_reset(tp);
}

int sim_tp_enqueue(sim_tp *tp, const sim_posemath_segment *segment, uint32_t *id_out) {
    uint32_t index;
    int result;
    if (tp == 0 || segment == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    result = sim_posemath_segment_validate(segment);
    if (result != SIM_POSEMATH_OK) return result;
    if (tp->count >= SIM_TP_QUEUE_CAPACITY) return SIM_POSEMATH_ERR_CAPACITY;
    if (segment->velocity > tp->velocity_limit || segment->acceleration > tp->acceleration_limit) return SIM_POSEMATH_ERR_RANGE;
    index = (tp->head + tp->count) % SIM_TP_QUEUE_CAPACITY;
    tp->queue[index] = *segment; tp->ids[index] = tp->next_id++;
    if (id_out != 0) *id_out = tp->ids[index];
    tp->count++;
    return SIM_POSEMATH_OK;
}

int sim_tp_step(sim_tp *tp) {
    sim_posemath_segment *segment;
    double length_sq;
    double step;
    int result;
    if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    if (tp->count == 0) { tp->tick++; tp->status = SIM_POSEMATH_ERR_STATE; return SIM_POSEMATH_ERR_STATE; }
    segment = &tp->queue[tp->head];
    result = sim_posemath_segment_validate(segment);
    if (result != SIM_POSEMATH_OK) { tp->status = result; return result; }
    if (tp->active_id == 0) { tp->active_id = tp->ids[tp->head]; tp->motion_type = segment->motion_type; }
    length_sq = (segment->end.tran.x - segment->start.tran.x) * (segment->end.tran.x - segment->start.tran.x) +
                (segment->end.tran.y - segment->start.tran.y) * (segment->end.tran.y - segment->start.tran.y) +
                (segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z);
    step = segment->velocity * tp->cycle_time / sqrt(length_sq);
    if (!isfinite(step) || step < 0.0) { tp->status = SIM_POSEMATH_ERR_NONFINITE; return tp->status; }
    if (step < 1e-15) step = 1e-15;
    tp->progress += step;
    if (tp->progress >= 1.0) {
        tp->progress = 1.0;
        result = sim_posemath_segment_point(segment, tp->progress, &tp->position);
        tp->head = (tp->head + 1) % SIM_TP_QUEUE_CAPACITY; tp->count--; tp->active_id = 0; tp->motion_type = 0; tp->progress = 0.0;
    } else {
        result = sim_posemath_segment_point(segment, tp->progress, &tp->position);
    }
    tp->current_velocity = segment->velocity; tp->tick++; tp->status = result;
    return result;
}

int sim_tp_read_state(const sim_tp *tp, sim_tp_state *state) {
    if (tp == 0 || state == 0) return SIM_POSEMATH_ERR_ARGUMENT;
    state->position = tp->position; state->velocity = tp->current_velocity; state->progress = tp->progress;
    state->tick = tp->tick; state->queue_depth = tp->count; state->active_id = tp->active_id;
    state->motion_type = tp->motion_type; state->status = tp->status;
    return SIM_POSEMATH_OK;
}

uint64_t sim_tp_state_hash(const sim_tp *tp) {
    sim_tp_state state;
    if (sim_tp_read_state(tp, &state) != SIM_POSEMATH_OK) return 0;
    return sim_posemath_hash_state(&state);
}