#include "sim_kinematics.h"

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

static sim_kinematics_config config_for(const char *coordinates) {
    sim_kinematics_config config = {0};
    config.joint_count = (uint32_t)strlen(coordinates);
    strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
    return config;
}

static sim_kinematics_user_descriptor identity_descriptor(void) {
    sim_kinematics_user_descriptor descriptor = {0};
    descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
    descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
                              SIM_KIN_USER_CAP_STATIC;
    descriptor.implementation = SIM_KIN_USER_IDENTITY;
    strcpy(descriptor.name, "userk.identity");
    descriptor.config = config_for("XYZAC");
    return descriptor;
}

static sim_kinematics_user_descriptor genser_descriptor(void) {
    sim_kinematics_user_descriptor descriptor = {0};
    descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
    descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
                              SIM_KIN_USER_CAP_STATIC;
    descriptor.implementation = SIM_KIN_USER_GENSER;
    strcpy(descriptor.name, "ugenser.static");
    descriptor.config = config_for("XYZABC");
    descriptor.config.dh_joint_count = 6;
    descriptor.config.max_iterations = 200;
    descriptor.config.convergence_tolerance = 1e-7;
    descriptor.config.dh_a[1] = 85.0;
    descriptor.config.dh_a[2] = 380.0;
    descriptor.config.dh_a[3] = 100.0;
    descriptor.config.dh_alpha[1] = -1.5707963267948966;
    descriptor.config.dh_alpha[3] = -1.5707963267948966;
    descriptor.config.dh_alpha[4] = 1.5707963267948966;
    descriptor.config.dh_alpha[5] = -1.5707963267948966;
    descriptor.config.dh_d[0] = 350.0;
    descriptor.config.dh_d[3] = 425.0;
    descriptor.config.dh_d[5] = 235.0;
    return descriptor;
}

int main(void) {
    sim_kinematics_user identity = {0};
    sim_kinematics_user genser = {0};
    sim_kinematics_user uninitialized = {0};
    sim_kinematics_user_descriptor descriptor = identity_descriptor();
    sim_kinematics_result_info info = {0};
    sim_kinematics base = {0};
    sim_kinematics_config base_config = config_for("XYZAC");
    sim_kinematics_switch switcher = {0};
    sim_posemath_pose identity_pose = {0};
    sim_posemath_pose genser_pose = {0};
    double identity_joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
    double identity_roundtrip[5] = {0};
    double genser_joints[6] = {0.0, 10.0, -20.0, 30.0, 15.0, -10.0};
    double genser_roundtrip[6] = {0};
    uint64_t identity_hash;
    uint64_t identity_repeat_hash;
    uint64_t genser_hash;
    uint64_t genser_repeat_hash;
    int switch_to_user;
    int abi_error;
    int capability_error;
    int path_name_error;
    int missing_implementation;
    int dynamic_path_error;
    int uninitialized_error;

    assert(sim_kinematics_user_init(&identity, &descriptor) == SIM_KIN_OK);
    assert(sim_kinematics_user_forward(&identity, identity_joints, 5, &identity_pose, &info) == SIM_KIN_OK);
    assert(sim_kinematics_user_inverse(&identity, &identity_pose, identity_roundtrip, 5, &info) == SIM_KIN_OK);
    assert(memcmp(identity_joints, identity_roundtrip, sizeof(identity_joints)) == 0);
    identity_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);
    identity_repeat_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);

    base_config.type = SIM_KIN_TRIVKINS;
    assert(sim_kinematics_init(&base, &base_config) == SIM_KIN_OK);
    assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
    assert(sim_kinematics_switch_register(&switcher, 0, &base, 0) == SIM_KIN_OK);
    assert(sim_kinematics_user_bind_switch(&identity, &switcher, 2, 0) == SIM_KIN_OK);
    switch_to_user = sim_kinematics_switch_select(&switcher, 2, identity_roundtrip, 5, &identity_pose, &info);
    assert(switch_to_user == SIM_KIN_OK && sim_kinematics_switch_active(&switcher) == 2);

    descriptor = genser_descriptor();
    assert(sim_kinematics_user_init(&genser, &descriptor) == SIM_KIN_OK);
    assert(sim_kinematics_user_forward(&genser, genser_joints, 6, &genser_pose, &info) == SIM_KIN_OK);
    assert(sim_kinematics_user_inverse(&genser, &genser_pose, genser_roundtrip, 6, &info) == SIM_KIN_OK);
    assert(sim_kinematics_user_forward(&genser, genser_roundtrip, 6, &genser_pose, &info) == SIM_KIN_OK);
    genser_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);
    genser_repeat_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);

    descriptor = identity_descriptor();
    descriptor.abi_version += 1;
    abi_error = sim_kinematics_user_init(&uninitialized, &descriptor);
    descriptor = identity_descriptor();
    descriptor.capabilities &= ~SIM_KIN_USER_CAP_STATIC;
    capability_error = sim_kinematics_user_init(&uninitialized, &descriptor);
    descriptor = identity_descriptor();
    strcpy(descriptor.name, "plugins/user.so");
    path_name_error = sim_kinematics_user_init(&uninitialized, &descriptor);
    descriptor = identity_descriptor();
    descriptor.implementation = 99;
    missing_implementation = sim_kinematics_user_init(&uninitialized, &descriptor);
    dynamic_path_error = sim_kinematics_user_reject_dynamic("/tmp/userkins.so");
    uninitialized_error = sim_kinematics_user_forward(&uninitialized, identity_joints, 5, &identity_pose, &info);

    printf("{\"schema\":\"userkins-samples.v1\",\"abi_version\":%u,\"capabilities\":%u,\"identity\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"state_hash\":%llu,\"repeat_hash\":%llu},\"ugenser\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"iterations\":%u,\"residual\":%.12f,\"state_hash\":%llu,\"repeat_hash\":%llu},\"switch_to_user\":%d,\"active_mode\":%d,\"abi_error\":%d,\"capability_error\":%d,\"path_name_error\":%d,\"missing_implementation\":%d,\"dynamic_path_error\":%d,\"uninitialized_error\":%d}\n",
           SIM_KIN_USER_ABI_VERSION, identity.descriptor.capabilities,
           identity_pose.tran.x, identity_pose.tran.y, identity_pose.tran.z, identity_pose.a, identity_pose.c,
           identity_roundtrip[0], identity_roundtrip[1], identity_roundtrip[2], identity_roundtrip[3], identity_roundtrip[4],
           identity.forward_calls, identity.inverse_calls, (unsigned long long)identity_hash, (unsigned long long)identity_repeat_hash,
           genser_pose.tran.x, genser_pose.tran.y, genser_pose.tran.z, genser_pose.a, genser_pose.b, genser_pose.c,
           genser.forward_calls, genser.inverse_calls, info.iterations, info.residual,
           (unsigned long long)genser_hash, (unsigned long long)genser_repeat_hash,
           switch_to_user, sim_kinematics_switch_active(&switcher), abi_error, capability_error,
           path_name_error, missing_implementation, dynamic_path_error, uninitialized_error);
    return 0;
}