#include "sim_kinematics_feedback.h"

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

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 = {0};
    sim_kinematics_feedback feedback = {0};
    sim_kinematics_feedback singular_feedback = {0};
    sim_kinematics_feedback_state state = {0};
    sim_kinematics_result_info info = {0};
    sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
    double joints[3] = {1.0, 2.0, 3.0};
    double bad_joints[3] = {1.0, NAN, 3.0};
    double singular_joints[5] = {0, 0, 0, 90, 0};
    uint64_t hash;
    uint64_t repeat_hash;
    uint32_t valid_tick;
    int update_status;
    int bad_status;
    int singular_status;
    int stale_read_status;
    int count_status;

    assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
    assert(sim_kinematics_feedback_init(&feedback, &kinematics, 3) == SIM_KIN_FEEDBACK_OK);
    stale_read_status = sim_kinematics_feedback_read(&feedback, &state);
    update_status = sim_kinematics_feedback_update(&feedback, joints, 3, 0, &info);
    assert(update_status == SIM_KIN_FEEDBACK_OK);
    assert(sim_kinematics_feedback_read(&feedback, &state) == SIM_KIN_FEEDBACK_OK);
    valid_tick = state.tick;
    hash = sim_kinematics_feedback_state_hash(&feedback);
    repeat_hash = sim_kinematics_feedback_state_hash(&feedback);
    assert(state.valid && state.tick == 1 && fabs(state.pose.tran.x - 1.0) < 1e-12);
    bad_status = sim_kinematics_feedback_update(&feedback, bad_joints, 3, 0, &info);
    assert(bad_status == SIM_KIN_FEEDBACK_ERR_NONFINITE && feedback.state.tick == valid_tick);
    count_status = sim_kinematics_feedback_update(&feedback, joints, 2, 0, &info);
    assert(count_status == SIM_KIN_FEEDBACK_ERR_CONFIGURATION);

    config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
    config.mode = SIM_KIN_MODE_INVERSE_ONLY;
    assert(sim_kinematics_init(&singular, &config) == SIM_KIN_OK);
    assert(sim_kinematics_feedback_init(&singular_feedback, &singular, 5) == SIM_KIN_FEEDBACK_OK);
    singular_status = sim_kinematics_feedback_update(&singular_feedback, singular_joints, 5, 0, &info);
    assert(singular_status == SIM_KIN_FEEDBACK_ERR_FORWARD && singular_feedback.state.valid == 0 && singular_feedback.state.kinematics_error == SIM_KIN_ERR_UNSUPPORTED);

    printf("{\"schema\":\"kinematics-feedback-samples.v1\",\"nominal\":{\"status\":%d,\"tick\":%u,\"valid\":%u,\"pose\":[%.9f,%.9f,%.9f],\"flags\":[%llu,%llu],\"state_hash\":%llu,\"repeat_hash\":%llu},\"stale_read\":%d,\"nonfinite\":%d,\"count_error\":%d,\"singular\":{\"status\":%d,\"kinematics_error\":%d,\"valid\":%u}}\n",
           update_status, state.tick, state.valid, state.pose.tran.x, state.pose.tran.y, state.pose.tran.z,
           (unsigned long long)state.forward_flags, (unsigned long long)state.inverse_flags,
           (unsigned long long)hash, (unsigned long long)repeat_hash, stale_read_status,
           bad_status, count_status, singular_status, singular_feedback.state.kinematics_error,
           singular_feedback.state.valid);
    return 0;
}