#include "sim_kinematics.h"

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

int main(void) {
    sim_kinematics_capability capability = {0};
    sim_kinematics_config config = {0};
    uint32_t count = sim_kinematics_registry_count();
    uint32_t verified = 0;
    uint64_t capability_union = 0;
    uint64_t parameter_union = 0;
    int lookup_status;
    int iterative_require;
    int capability_error;
    int parameter_error;
    int type_mismatch;
    int coordinate_error;
    int joint_error;
    int unknown_name;
    int path_name;
    int range_error;

    assert(count == 18);
    printf("{\"schema\":\"kinematics-registry-samples.v1\",\"entries\":[");
    for (uint32_t index = 0; index < count; ++index) {
        assert(sim_kinematics_registry_get(index, &capability) == SIM_KIN_OK);
        memset(&config, 0, sizeof(config));
        config.type = capability.type;
        config.joint_count = capability.min_joints;
        strcpy(config.coordinates, capability.required_coordinates[0] == '\0' ? "X" : capability.required_coordinates);
        if (sim_kinematics_registry_validate_config(capability.name, &config, 0) == SIM_KIN_OK) verified += 1;
        capability_union |= capability.capabilities;
        parameter_union |= capability.parameter_mask;
        printf("%s{\"name\":\"%s\",\"type\":%d,\"required\":\"%s\",\"min\":%u,\"max\":%u,\"capabilities\":%u,\"parameters\":%llu}",
               index == 0 ? "" : ",", capability.name, capability.type,
               capability.required_coordinates, capability.min_joints,
               capability.max_joints, capability.capabilities,
               (unsigned long long)capability.parameter_mask);
    }
    lookup_status = sim_kinematics_registry_find("xyzac-trt-kins", &capability);
    iterative_require = sim_kinematics_registry_require("genserkins", SIM_KIN_CAP_ITERATIVE, SIM_KIN_PARAM_DH, &capability);
    capability_error = sim_kinematics_registry_require("trivkins", SIM_KIN_CAP_ITERATIVE, 0, 0);
    parameter_error = sim_kinematics_registry_require("trivkins", 0, SIM_KIN_PARAM_TOOL, 0);
    memset(&config, 0, sizeof(config));
    config.type = SIM_KIN_TRIVKINS; config.joint_count = 5; strcpy(config.coordinates, "XYZAC");
    type_mismatch = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
    config.type = SIM_KIN_XYZAC_TRT; strcpy(config.coordinates, "XYZAQ");
    coordinate_error = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
    strcpy(config.coordinates, "XYZAC"); config.joint_count = 4;
    joint_error = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
    unknown_name = sim_kinematics_registry_find("missing-kins", &capability);
    path_name = sim_kinematics_registry_find("plugins/user.so", &capability);
    range_error = sim_kinematics_registry_get(count, &capability);
    printf("],\"count\":%u,\"verified\":%u,\"capability_union\":%llu,\"parameter_union\":%llu,\"registry_hash\":%llu,\"lookup_status\":%d,\"iterative_require\":%d,\"capability_error\":%d,\"parameter_error\":%d,\"type_mismatch\":%d,\"coordinate_error\":%d,\"joint_error\":%d,\"unknown_name\":%d,\"path_name\":%d,\"range_error\":%d}\n",
           count, verified, (unsigned long long)capability_union,
           (unsigned long long)parameter_union,
           (unsigned long long)sim_kinematics_registry_state_hash(), lookup_status,
           iterative_require, capability_error, parameter_error, type_mismatch,
           coordinate_error, joint_error, unknown_name, path_name, range_error);
    return 0;
}