#include "sim_motion_queue.h"

#include <assert.h>

static sim_posemath_segment line(double x0, double x1) {
    sim_posemath_segment segment = {
        {{x0, 0, 0}, 0, 0, 0, 0, 0, 0},
         {{x1, 0, 0}, 0, 0, 0, 0, 0, 0},
         {0, 0, 0}, {0, 0, 1}, 1.0, 1.0, SIM_POSEMATH_LINEAR, 0
    };
    return segment;
}

int main(void) {
    sim_motion_queue queue = {0};
    sim_motion_command command = {0};
    sim_motion_state state = {0};
    char canonical[512] = {0};
    uint32_t id = 0;
    command.segment = line(0, 1);
    command.tag = 42;
    assert(sim_motion_init(&queue, 0.1, 10.0) == SIM_MOTION_OK);
    assert(sim_motion_enqueue(&queue, &command, &id) == SIM_MOTION_OK && id == 1);
    command.segment = line(1, 2);
    assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_OK);
    assert(sim_motion_step(&queue) == SIM_MOTION_OK);
    assert(sim_motion_read_state(&queue, &state) == SIM_MOTION_OK);
    assert(state.tick == 1 && state.queue_depth == 2 && state.position.tran.x > 0.09);
    assert(sim_motion_state_hash(&queue) != 0);
    assert(sim_motion_canonical(&queue, canonical, sizeof(canonical)) > 1);
    command.segment = line(9, 10);
    assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_ERR_STATE);
    command.segment.motion_type = 99;
    assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_ERR_MOTION_TYPE);
    return 0;
}