#include "sim_kinematics.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);
    strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
    return config;
}

int main(void) {
    sim_kinematics identity = {0};
    sim_kinematics trt = {0};
    sim_kinematics_config config;
    sim_kinematics_switch switcher = {0};
    sim_kinematics_result_info info = {0};
    sim_posemath_pose initial = {0};
    sim_posemath_pose switched = {0};
    sim_posemath_pose returned = {0};
    double joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
    uint64_t initial_hash;
    uint64_t switched_hash;
    uint64_t returned_hash;
    uint64_t returned_repeat_hash;
    int forward_status;
    int select_to_trt;
    int select_to_identity;
    int invalid_mode;
    int missing_module;
    int register_missing;
    int begin_status;
    int interrupted_begin;
    int abort_status;
    int interrupted_commit;

    config = config_for(SIM_KIN_TRIVKINS, "XYZAC");
    assert(sim_kinematics_init(&identity, &config) == SIM_KIN_OK);
    config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
    assert(sim_kinematics_init(&trt, &config) == SIM_KIN_OK);
    assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
    assert(sim_kinematics_switch_register(&switcher, 0, &identity, 0) == SIM_KIN_OK);
    assert(sim_kinematics_switch_register(&switcher, 1, &trt, 0) == SIM_KIN_OK);
    register_missing = sim_kinematics_switch_register(&switcher, 2, 0, 0);
    forward_status = sim_kinematics_switch_forward(&switcher, joints, 5, &initial, &info);
    assert(forward_status == SIM_KIN_OK);
    initial_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &initial);
    select_to_trt = sim_kinematics_switch_select(&switcher, 1, joints, 5, &switched, &info);
    assert(select_to_trt == SIM_KIN_OK);
    assert(fabs(switched.tran.x - initial.tran.x) < 1e-9);
    assert(fabs(switched.tran.y - initial.tran.y) < 1e-9);
    assert(fabs(switched.tran.z - initial.tran.z) < 1e-9);
    switched_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &switched);
    select_to_identity = sim_kinematics_switch_select(&switcher, 0, joints, 5, &returned, &info);
    assert(select_to_identity == SIM_KIN_OK);
    assert(fabs(returned.tran.x - initial.tran.x) < 1e-9);
    assert(fabs(returned.tran.y - initial.tran.y) < 1e-9);
    assert(fabs(returned.tran.z - initial.tran.z) < 1e-9);
    returned_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
    returned_repeat_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
    invalid_mode = sim_kinematics_switch_select(&switcher, 3, joints, 5, &returned, &info);
    missing_module = sim_kinematics_switch_select(&switcher, 2, joints, 5, &returned, &info);
    begin_status = sim_kinematics_switch_begin(&switcher, 1);
    interrupted_begin = sim_kinematics_switch_begin(&switcher, 0);
    abort_status = sim_kinematics_switch_abort(&switcher);
    assert(sim_kinematics_switch_begin(&switcher, 1) == SIM_KIN_OK);
    abort_status = sim_kinematics_switch_abort(&switcher);
    interrupted_commit = sim_kinematics_switch_commit(&switcher, joints, 5, &returned, &info);
    assert(sim_kinematics_switch_active(&switcher) == 0);

    printf("{\"schema\":\"switchkins-samples.v1\",\"registered\":[%u,%u,%u],\"initial_mode\":0,\"active_mode\":%d,\"forward_status\":%d,\"switch_to_trt\":%d,\"switch_to_identity\":%d,\"initial_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"switched_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"returned_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"initial_hash\":%llu,\"switched_hash\":%llu,\"returned_hash\":%llu,\"returned_repeat_hash\":%llu,\"invalid_mode\":%d,\"missing_module\":%d,\"register_missing\":%d,\"begin_status\":%d,\"interrupted_begin\":%d,\"abort_status\":%d,\"interrupted_commit\":%d,\"generation\":%u,\"switch_count\":%u,\"abort_count\":%u}\n",
           switcher.registered[0], switcher.registered[1], switcher.registered[2], switcher.active_mode,
           forward_status, select_to_trt, select_to_identity,
           initial.tran.x, initial.tran.y, initial.tran.z, initial.a, initial.b, initial.c,
           switched.tran.x, switched.tran.y, switched.tran.z, switched.a, switched.b, switched.c,
           returned.tran.x, returned.tran.y, returned.tran.z, returned.a, returned.b, returned.c,
           (unsigned long long)initial_hash, (unsigned long long)switched_hash, (unsigned long long)returned_hash, (unsigned long long)returned_repeat_hash,
           invalid_mode, missing_module, register_missing, begin_status, interrupted_begin, abort_status,
           interrupted_commit, switcher.generation, switcher.switch_count, switcher.abort_count);
    return 0;
}