#include "sim_motion_queue.h"

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

#define MOTION_META_SLOTS 16u

typedef struct motion_meta {
    const sim_motion_queue *owner;
    int profile;
    uint32_t lookahead;
    double acceleration_limit;
    double jerk_limit;
    double feed_override;
    double current_velocity;
    double current_acceleration;
    uint32_t paused;
} motion_meta;

static motion_meta motion_metadata[MOTION_META_SLOTS];

static motion_meta *meta_for(const sim_motion_queue *queue, int create) {
    motion_meta *free_slot = NULL;
    size_t index;
    if (queue == NULL) return NULL;
    for (index = 0; index < MOTION_META_SLOTS; ++index) {
        if (motion_metadata[index].owner == queue) return &motion_metadata[index];
        if (free_slot == NULL && motion_metadata[index].owner == NULL) free_slot = &motion_metadata[index];
    }
    if (!create || free_slot == NULL) return NULL;
    memset(free_slot, 0, sizeof(*free_slot));
    free_slot->owner = queue;
    free_slot->profile = SIM_MOTION_PROFILE_BLEND;
    free_slot->lookahead = 1;
    free_slot->acceleration_limit = 1e9;
    free_slot->jerk_limit = 1e12;
    free_slot->feed_override = 1.0;
    free_slot->paused = 0;
    return free_slot;
}

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 ^= bytes[index]; hash *= UINT64_C(1099511628211); }
    return hash;
}

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

static double segment_length(const sim_posemath_segment *segment) {
    const double dx = segment->end.tran.x - segment->start.tran.x;
    const double dy = segment->end.tran.y - segment->start.tran.y;
    const double dz = segment->end.tran.z - segment->start.tran.z;
    double value = dx * dx + dy * dy + dz * dz +
        (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 (segment->motion_type == SIM_POSEMATH_CIRCULAR) {
        const double sx = segment->start.tran.x - segment->center.x;
        const double sy = segment->start.tran.y - segment->center.y;
        const double sz = segment->start.tran.z - segment->center.z;
        const double ex = segment->end.tran.x - segment->center.x;
        const double ey = segment->end.tran.y - segment->center.y;
        const double ez = segment->end.tran.z - segment->center.z;
        const double radius = sqrt(sx * sx + sy * sy + sz * sz);
        const double dot = sx * ex + sy * ey + sz * ez;
        const double cross_x = sy * ez - sz * ey;
        const double cross_y = sz * ex - sx * ez;
        const double cross_z = sx * ey - sy * ex;
        const double normal_length = sqrt(segment->normal.x * segment->normal.x + segment->normal.y * segment->normal.y + segment->normal.z * segment->normal.z);
        double angle;
        if (!isfinite(radius) || radius <= 1e-12 || !isfinite(normal_length) || normal_length <= 1e-12) return 0.0;
        angle = atan2((segment->normal.x * cross_x + segment->normal.y * cross_y + segment->normal.z * cross_z) / normal_length, dot);
        if (angle < 0.0) angle = -angle;
        if (angle <= 1e-12) angle = 2.0 * M_PI;
        if (segment->turn > 1 || segment->turn < -1) angle += 2.0 * M_PI * (fabs((double)segment->turn) - 1.0);
        return isfinite(radius * angle) && radius * angle > 0.0 ? radius * angle : 0.0;
    }
    return isfinite(value) && value > 0.0 ? sqrt(value) : 0.0;
}

static int poses_match(const sim_posemath_pose *left, const sim_posemath_pose *right) {
    const double *a = &left->tran.x;
    const double *b = &right->tran.x;
    size_t index;
    for (index = 0; index < 9; ++index) if (fabs(a[index] - b[index]) > 1e-12) return 0;
    return 1;
}

static void zero_state(sim_motion_state *state) {
    memset(state, 0, sizeof(*state));
    state->status = SIM_MOTION_OK;
}

int sim_motion_reset(sim_motion_queue *queue) {
    motion_meta *meta;
    if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
    meta = meta_for(queue, 1);
    if (meta == NULL) return SIM_MOTION_ERR_BUFFER;
    meta->profile = SIM_MOTION_PROFILE_BLEND;
    meta->lookahead = 1;
    meta->acceleration_limit = 1e9;
    meta->jerk_limit = 1e12;
    meta->feed_override = 1.0;
    meta->current_velocity = 0.0;
    meta->current_acceleration = 0.0;
    meta->paused = 0;
    queue->head = 0; queue->count = 0; queue->next_id = 1; queue->tick = 0;
    zero_state(&queue->state);
    return SIM_MOTION_OK;
}

int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_limit) {
    if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
    if (!valid_config(cycle_time, velocity_limit)) return SIM_MOTION_ERR_RANGE;
    queue->cycle_time = cycle_time; queue->velocity_limit = velocity_limit;
    return sim_motion_reset(queue);
}

uint32_t sim_motion_size(void) { return (uint32_t)sizeof(sim_motion_queue); }
uint32_t sim_motion_command_size(void) { return (uint32_t)sizeof(sim_motion_command); }
uint32_t sim_motion_state_size(void) { return (uint32_t)sizeof(sim_motion_state); }

int sim_motion_set_profile(sim_motion_queue *queue, int profile) {
    motion_meta *meta = meta_for(queue, 1);
    if (meta == NULL || (profile != SIM_MOTION_PROFILE_BLEND && profile != SIM_MOTION_PROFILE_EXACT_STOP)) return SIM_MOTION_ERR_ARGUMENT;
    meta->profile = profile;
    return SIM_MOTION_OK;
}

int sim_motion_set_lookahead(sim_motion_queue *queue, uint32_t depth) {
    motion_meta *meta = meta_for(queue, 1);
    if (meta == NULL || depth == 0 || depth > SIM_MOTION_QUEUE_CAPACITY) return SIM_MOTION_ERR_RANGE;
    meta->lookahead = depth;
    return SIM_MOTION_OK;
}

int sim_motion_set_dynamics(sim_motion_queue *queue, double acceleration_limit, double jerk_limit) {
    motion_meta *meta = meta_for(queue, 1);
    if (meta == NULL || !isfinite(acceleration_limit) || !isfinite(jerk_limit) || acceleration_limit <= 0.0 || jerk_limit <= 0.0) return SIM_MOTION_ERR_RANGE;
    meta->acceleration_limit = acceleration_limit;
    meta->jerk_limit = jerk_limit;
    return SIM_MOTION_OK;
}

int sim_motion_set_feed_override(sim_motion_queue *queue, double feed_override) {
    motion_meta *meta = meta_for(queue, 1);
    if (meta == NULL || !isfinite(feed_override) || feed_override < 0.0 || feed_override > 2.0) return SIM_MOTION_ERR_RANGE;
    meta->feed_override = feed_override;
    return SIM_MOTION_OK;
}

int sim_motion_set_paused(sim_motion_queue *queue, uint32_t paused) {
    motion_meta *meta = meta_for(queue, 1);
    if (meta == NULL || paused > 1u) return SIM_MOTION_ERR_ARGUMENT;
    meta->paused = paused;
    if (paused != 0u) {
        meta->current_velocity = 0.0;
        meta->current_acceleration = 0.0;
        queue->state.velocity = 0.0;
    }
    return SIM_MOTION_OK;
}

int sim_motion_peek(const sim_motion_queue *queue, uint32_t offset, sim_motion_command *command) {
    uint32_t index;
    if (queue == NULL || command == NULL) return SIM_MOTION_ERR_ARGUMENT;
    if (offset >= queue->count) return SIM_MOTION_ERR_RANGE;
    index = (queue->head + offset) % SIM_MOTION_QUEUE_CAPACITY;
    *command = queue->commands[index];
    return SIM_MOTION_OK;
}

static double planned_velocity(const sim_motion_queue *queue, uint32_t offset) {
    motion_meta *meta = meta_for(queue, 0);
    double velocity = 0.0;
    uint32_t index;
    if (queue == NULL || offset >= queue->count) return 0.0;
    index = (queue->head + offset) % SIM_MOTION_QUEUE_CAPACITY;
    velocity = queue->commands[index].segment.velocity * (meta == NULL || (queue->commands[index].tag & SIM_MOTION_COMMAND_TAG_RAPID) != 0u ? 1.0 : meta->feed_override);
    if (meta == NULL || meta->profile == SIM_MOTION_PROFILE_EXACT_STOP) return velocity;
    for (index = 1; index <= meta->lookahead && offset + index < queue->count; ++index) {
        uint32_t next = (queue->head + offset + index) % SIM_MOTION_QUEUE_CAPACITY;
        const double next_velocity = queue->commands[next].segment.velocity * ((queue->commands[next].tag & SIM_MOTION_COMMAND_TAG_RAPID) != 0u ? 1.0 : meta->feed_override);
        if (next_velocity < velocity) velocity = next_velocity;
    }
    return velocity;
}

int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out) {
    uint32_t index;
    int result;
    if (queue == 0 || command == 0) return SIM_MOTION_ERR_ARGUMENT;
    result = sim_posemath_segment_validate(&command->segment);
    if (result == SIM_POSEMATH_ERR_MOTION_TYPE) return SIM_MOTION_ERR_MOTION_TYPE;
    if (result != SIM_POSEMATH_OK) return result == SIM_POSEMATH_ERR_NONFINITE ? SIM_MOTION_ERR_NONFINITE : SIM_MOTION_ERR_GEOMETRY;
    if (queue->count >= SIM_MOTION_QUEUE_CAPACITY) return SIM_MOTION_ERR_CAPACITY;
    if (command->segment.velocity > queue->velocity_limit) return SIM_MOTION_ERR_RANGE;
    if (queue->count > 0) {
        index = (queue->head + queue->count - 1) % SIM_MOTION_QUEUE_CAPACITY;
        if (!poses_match(&queue->commands[index].segment.end, &command->segment.start)) return SIM_MOTION_ERR_STATE;
    } else if (queue->state.tick > 0 && !poses_match(&queue->state.position, &command->segment.start)) {
        return SIM_MOTION_ERR_STATE;
    }
    index = (queue->head + queue->count) % SIM_MOTION_QUEUE_CAPACITY;
    queue->commands[index] = *command;
    queue->commands[index].id = queue->next_id++;
    if (id_out != 0) *id_out = queue->commands[index].id;
    queue->count++;
    return SIM_MOTION_OK;
}

int sim_motion_step(sim_motion_queue *queue) {
    sim_motion_command *command;
    double length;
    double delta;
    double desired_velocity;
    double acceleration;
    motion_meta *meta;
    int result;
    if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
    meta = meta_for(queue, 1);
    if (meta->paused) { queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
    if (queue->count == 0) { queue->tick++; queue->state.tick = queue->tick; queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
    command = &queue->commands[queue->head];
    length = segment_length(&command->segment);
    if (length <= 0.0) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
    queue->state.active_id = command->id; queue->state.active_tag = command->tag & ~SIM_MOTION_COMMAND_TAG_RAPID; queue->state.motion_type = command->segment.motion_type;
    desired_velocity = planned_velocity(queue, 0);
    acceleration = (desired_velocity - meta->current_velocity) / queue->cycle_time;
    if (acceleration > meta->current_acceleration + meta->jerk_limit * queue->cycle_time) acceleration = meta->current_acceleration + meta->jerk_limit * queue->cycle_time;
    if (acceleration < meta->current_acceleration - meta->jerk_limit * queue->cycle_time) acceleration = meta->current_acceleration - meta->jerk_limit * queue->cycle_time;
    if (acceleration > meta->acceleration_limit) acceleration = meta->acceleration_limit;
    if (acceleration < -meta->acceleration_limit) acceleration = -meta->acceleration_limit;
    meta->current_acceleration = acceleration;
    meta->current_velocity += acceleration * queue->cycle_time;
    if ((desired_velocity - meta->current_velocity) * (desired_velocity - meta->current_velocity) < 1e-18) meta->current_velocity = desired_velocity;
    if (meta->current_velocity < 0.0) meta->current_velocity = 0.0;
    delta = meta->current_velocity * queue->cycle_time / length;
    if (!isfinite(delta) || delta < 0.0) { queue->state.status = SIM_MOTION_ERR_NONFINITE; return queue->state.status; }
    queue->state.progress += delta;
    if (queue->state.progress >= 1.0) {
        queue->state.progress = 1.0;
        result = sim_posemath_segment_point(&command->segment, 1.0, &queue->state.position);
        if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
        queue->head = (queue->head + 1) % SIM_MOTION_QUEUE_CAPACITY;
        queue->count--;
        queue->state.progress = 0.0;
        queue->state.active_id = 0;
        queue->state.active_tag = 0;
        queue->state.motion_type = 0;
    } else {
        result = sim_posemath_segment_point(&command->segment, queue->state.progress, &queue->state.position);
        if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
    }
    queue->state.velocity = meta->current_velocity;
    if (queue->count == 0) { meta->current_velocity = 0.0; meta->current_acceleration = 0.0; queue->state.velocity = 0.0; }
    queue->tick++;
    queue->state.tick = queue->tick;
    queue->state.queue_depth = queue->count;
    queue->state.status = SIM_MOTION_OK;
    return SIM_MOTION_OK;
}

int sim_motion_read_state(const sim_motion_queue *queue, sim_motion_state *state) {
    if (queue == 0 || state == 0) return SIM_MOTION_ERR_ARGUMENT;
    *state = queue->state;
    state->queue_depth = queue->count;
    return SIM_MOTION_OK;
}

uint32_t sim_motion_queue_depth(const sim_motion_queue *queue) {
    return queue == 0 ? 0 : queue->count;
}

uint64_t sim_motion_state_hash(const sim_motion_queue *queue) {
    sim_motion_state state;
    unsigned char bytes[sizeof(sim_motion_state)];
    if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
    memset(bytes, 0, sizeof(bytes));
    memcpy(bytes, &state.position, sizeof(state.position));
    memcpy(bytes + sizeof(state.position), &state.velocity, sizeof(double));
    memcpy(bytes + sizeof(state.position) + sizeof(double), &state.progress, sizeof(double));
    memcpy(bytes + sizeof(state.position) + sizeof(double) * 2, &state.tick, sizeof(uint32_t));
    memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t), &state.queue_depth, sizeof(uint32_t));
    memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 2, &state.active_id, sizeof(uint32_t));
    memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 3, &state.active_tag, sizeof(uint32_t));
    memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4, &state.motion_type, sizeof(int));
    memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int), &state.status, sizeof(int));
    return fnv1a64(bytes, sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int) * 2);
}

uint64_t sim_motion_plan_hash(const sim_motion_queue *queue) {
    motion_meta *meta = meta_for(queue, 0);
    unsigned char bytes[sizeof(uint32_t) * 3 + sizeof(int) + SIM_MOTION_QUEUE_CAPACITY * sizeof(double)];
    size_t offset = 0;
    uint32_t index;
    if (queue == NULL || meta == NULL) return 0;
    memcpy(bytes + offset, &meta->profile, sizeof(meta->profile)); offset += sizeof(meta->profile);
    memcpy(bytes + offset, &meta->lookahead, sizeof(meta->lookahead)); offset += sizeof(meta->lookahead);
    memcpy(bytes + offset, &queue->count, sizeof(queue->count)); offset += sizeof(queue->count);
    for (index = 0; index < queue->count; ++index) { double velocity = planned_velocity(queue, index); memcpy(bytes + offset, &velocity, sizeof(velocity)); offset += sizeof(velocity); }
    return fnv1a64(bytes, offset);
}

uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity) {
    sim_motion_state state;
    char discard[1];
    char *destination = output == 0 ? discard : output;
    uint32_t destination_capacity = output == 0 ? 1 : capacity;
    int written;
    if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
    {
        motion_meta *meta = meta_for(queue, 0);
        const char *profile = meta != NULL && meta->profile == SIM_MOTION_PROFILE_EXACT_STOP ? "exact_stop" : "blend";
        uint32_t lookahead = meta == NULL ? 1u : meta->lookahead;
        written = snprintf(destination, destination_capacity,
        "{\"active_id\":%u,\"active_tag\":%u,\"lookahead\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"profile\":\"%s\",\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
        state.active_id, state.active_tag, lookahead, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
        profile, state.progress, state.queue_depth, state.status, state.tick, state.velocity);
    }
    if (written < 0) return 0;
    if (output == 0 || capacity <= (uint32_t)written) return (uint32_t)written + 1;
    return (uint32_t)written + 1;
}