#include "sim_linuxcnc_tp.h"

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>

static sim_posemath_pose pose(double x, double y, double z) {
    sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0};
    return value;
}

int main(void) {
    sim_linuxcnc_tp *planner = calloc(1, sim_linuxcnc_tp_size());
    sim_tp_state state = {0};
    char canonical[512] = {0};
    sim_posemath_pose origin = pose(0, 0, 0);
    sim_posemath_pose first = pose(0, 0, 10);
    sim_posemath_pose second = pose(20, 20, 10);
    assert(planner != NULL);
    assert(sim_linuxcnc_tp_init(planner, 0.001, 35.0, 300.0) == 0);
    assert(sim_linuxcnc_tp_queue_capacity(planner) > 0);
    assert(sim_linuxcnc_tp_velocity_mm_min(planner) == 0.0);
    assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0);
    assert(sim_linuxcnc_tp_set_profile(planner, 0, 0.0) == 0);
    assert(sim_linuxcnc_tp_add_line(planner, &first, 20.0, 300.0, 16, 1) == 0);
    assert(sim_linuxcnc_tp_add_line(planner, &second, 35.0, 300.0, 18, 1) == 0);
    for (int index = 0; index < 5000; ++index) {
        int result = sim_linuxcnc_tp_step(planner);
        if (result != 0) fprintf(stderr, "tp_step[%d]=%d\\n", index, result);
        assert(result == 0);
        assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
        if (state.queue_depth == 0) break;
    }
    assert(state.queue_depth == 0);
    assert(state.position.tran.x > 19.9 && state.position.tran.y > 19.9 && state.position.tran.z > 9.9);
    assert(sim_linuxcnc_tp_state_hash(planner) != 0);
    assert(sim_linuxcnc_tp_canonical(planner, canonical, sizeof(canonical)) > 1);
    assert(sim_linuxcnc_tp_reset(planner) == 0);
    assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0);
    assert(sim_linuxcnc_tp_set_axis_limit(planner, 0, -5.0, 5.0) == 0);
    assert(sim_linuxcnc_tp_set_jog_scale(planner, 0.5) == 0);
    assert(sim_linuxcnc_tp_jog(planner, 0, 1, 1.0, 21) == 0);
    assert(sim_linuxcnc_tp_step(planner) == 0);
    assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
    assert(state.position.tran.x > 0.0 && sim_linuxcnc_tp_jog(planner, 0, 1, 100.0, 22) != 0);
    puts(canonical);
    free(planner);
    return 0;
}