#include "sim_kinematics_motion.h"

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

static sim_posemath_segment line(double x0, double x1, double velocity) {
    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}, velocity, 10.0, SIM_POSEMATH_LINEAR, 0
    };
    return segment;
}

static sim_kinematics_config config_for(int type, const char *coordinates) {
    sim_kinematics_config config = {0};
    config.type = type;
    config.joint_count = (uint32_t)strlen(coordinates);
    strcpy(config.coordinates, coordinates);
    return config;
}

int main(void) {
    sim_kinematics kinematics = {0};
    sim_kinematics singular_kins = {0};
    sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
    sim_kinematics_motion adapter = {0};
    sim_kinematics_motion singular_adapter = {0};
    sim_kinematics_motion_sample sample = {0};
    sim_posemath_segment segment = line(0.0, 1.0, 1.0);
    uint32_t id = 0;
    uint32_t pause_tick;
    uint64_t nominal_hash;
    uint64_t nominal_repeat_hash;
    uint64_t full_planner_before;
    uint64_t full_planner_after;
    uint64_t full_hash;
    uint64_t singular_planner_before;
    uint64_t singular_planner_after;
    int pause_status;
    int resume_status;
    int full_status;
    int singular_status;
    int singular_kinematics_error;
    int nonfinite_status;

    assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
    assert(sim_kinematics_motion_init(&adapter, &kinematics, 0.1, 10.0, 10.0) == SIM_KIN_MOTION_OK);
    assert(sim_kinematics_motion_enqueue(&adapter, &segment, &id) == SIM_KIN_MOTION_OK && id == 1);
    assert(sim_kinematics_motion_pause(&adapter, 1) == SIM_KIN_MOTION_OK);
    pause_tick = adapter.planner.tick;
    pause_status = sim_kinematics_motion_step(&adapter);
    assert(pause_status == SIM_KIN_MOTION_ERR_STATE && adapter.planner.tick == pause_tick && adapter.count == 0);
    assert(sim_kinematics_motion_pause(&adapter, 0) == SIM_KIN_MOTION_OK);
    resume_status = sim_kinematics_motion_step(&adapter);
    assert(resume_status == SIM_KIN_MOTION_OK);
    assert(sim_kinematics_motion_peek(&adapter, &sample) == SIM_KIN_MOTION_OK);
    assert(fabs(sample.pose.tran.x - 0.1) < 1e-12 && fabs(sample.joints[0] - 0.1) < 1e-12);
    nominal_hash = sim_kinematics_motion_state_hash(&adapter);
    nominal_repeat_hash = sim_kinematics_motion_state_hash(&adapter);
    assert(sim_kinematics_motion_pop(&adapter, &sample) == SIM_KIN_MOTION_OK);

    assert(sim_kinematics_motion_reset(&adapter) == SIM_KIN_MOTION_OK);
    segment = line(0.0, 100.0, 1.0);
    assert(sim_kinematics_motion_enqueue(&adapter, &segment, 0) == SIM_KIN_MOTION_OK);
    for (uint32_t index = 0; index < SIM_KIN_MOTION_QUEUE_CAPACITY; ++index)
        assert(sim_kinematics_motion_step(&adapter) == SIM_KIN_MOTION_OK);
    full_planner_before = sim_tp_state_hash(&adapter.planner);
    full_status = sim_kinematics_motion_step(&adapter);
    full_planner_after = sim_tp_state_hash(&adapter.planner);
    full_hash = sim_kinematics_motion_state_hash(&adapter);
    assert(full_status == SIM_KIN_MOTION_ERR_CAPACITY);
    assert(full_planner_before == full_planner_after && adapter.count == SIM_KIN_MOTION_QUEUE_CAPACITY);

    config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
    assert(sim_kinematics_init(&singular_kins, &config) == SIM_KIN_OK);
    assert(sim_kinematics_motion_init(&singular_adapter, &singular_kins, 1.0, 10.0, 10.0) == SIM_KIN_MOTION_OK);
    segment = line(0.0, 1.0, 1.0);
    segment.end.b = 90.0;
    assert(sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0) == SIM_KIN_MOTION_OK);
    singular_planner_before = sim_tp_state_hash(&singular_adapter.planner);
    singular_status = sim_kinematics_motion_step(&singular_adapter);
    singular_planner_after = sim_tp_state_hash(&singular_adapter.planner);
    singular_kinematics_error = singular_adapter.kinematics_error;
    assert(singular_status == SIM_KIN_MOTION_ERR_INVERSE);
    assert(singular_kinematics_error == SIM_KIN_ERR_SINGULAR);
    assert(singular_planner_before == singular_planner_after && singular_adapter.count == 0);

    segment = line(0.0, 1.0, 1.0);
    segment.end.tran.x = NAN;
    nonfinite_status = sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0);
    assert(nonfinite_status == SIM_KIN_MOTION_ERR_NONFINITE);

    printf("{\"schema\":\"kinematics-motion-samples.v1\",\"nominal\":{\"pause_status\":%d,\"resume_status\":%d,\"tick\":%u,\"pose\":[%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f],\"state_hash\":%llu,\"repeat_hash\":%llu},\"capacity\":{\"status\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu,\"state_hash\":%llu},\"inverse_failure\":{\"status\":%d,\"kinematics_error\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu},\"nonfinite_status\":%d}\n",
           pause_status, resume_status, sample.tick, sample.pose.tran.x,
           sample.pose.tran.y, sample.pose.tran.z, sample.joints[0],
           sample.joints[1], sample.joints[2],
           (unsigned long long)nominal_hash, (unsigned long long)nominal_repeat_hash,
           full_status, adapter.count, (unsigned long long)full_planner_before,
           (unsigned long long)full_planner_after, (unsigned long long)full_hash,
           singular_status, singular_kinematics_error, singular_adapter.count,
           (unsigned long long)singular_planner_before,
           (unsigned long long)singular_planner_after, nonfinite_status);
    return 0;
}