#include "sim_kinematics.h"
#include "sim_program.h"
#include "sim_protocol.h"

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

static sim_kinematics_config config_for(int type) {
    static const double minimum[5] = {-200.0, -100.0, -120.0, -100.0, -36000.0};
    static const double maximum[5] = {200.0, 100.0, 120.0, 50.0, 36000.0};
    sim_kinematics_config config = {0};
    size_t index;
    config.type = type;
    config.joint_count = 5;
    config.kinematics_joint_count = 5;
    strcpy(config.coordinates, "XYZAC");
    config.y_offset = 20.0;
    config.z_offset = 10.0;
    for (index = 0; index < 5; ++index) {
        config.limit_enabled[index] = 1;
        config.joint_min[index] = minimum[index];
        config.joint_max[index] = maximum[index];
    }
    return config;
}

static void print_pose(const sim_posemath_pose *pose) {
    printf("{\"a\":%.17g,\"c\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
           pose->a, pose->c, pose->tran.x, pose->tran.y, pose->tran.z);
}

static void print_joints(const double *joints) {
    printf("[%.17g,%.17g,%.17g,%.17g,%.17g]", joints[0], joints[1], joints[2], joints[3], joints[4]);
}

int main(void) {
    static const char *commands[4] = {"M429", "M428", "M430", "M428"};
    static const int modes[4] = {0, 1, 2, 1};
    static const char source[] = "G21 G90 G94\nG0 X10 Y0 Z10 A0 C0\nG93\nG2 X10 Y0 Z5 A30 C45 I-10 J0 P3 F2\nM2\n";
    sim_kinematics identity = {0};
    sim_kinematics trt = {0};
    sim_kinematics userk = {0};
    sim_kinematics_switch switcher = {0};
    sim_kinematics_switch saved_switcher;
    sim_kinematics_result_info info = {0};
    sim_posemath_pose poses[4] = {{0}};
    sim_posemath_pose pose = {0};
    sim_posemath_pose saved_pose = {0};
    sim_posemath_segment arc = {0};
    sim_program program = {0};
    double joints[5] = {12.0, -5.0, 20.0, 30.0, 45.0};
    double state_joints[4][5] = {{0}};
    double saved_joints[5] = {0};
    uint64_t before_hash;
    uint64_t after_hash;
    size_t index;
    int have_arc = 0;

    assert(sim_kinematics_init(&identity, &(sim_kinematics_config){0}) != SIM_KIN_OK);
    {
        sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS);
        assert(sim_kinematics_init(&identity, &config) == SIM_KIN_OK);
        assert(sim_kinematics_init(&userk, &config) == SIM_KIN_OK);
        config = config_for(SIM_KIN_XYZAC_TRT);
        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);
    assert(sim_kinematics_switch_register(&switcher, 2, &userk, 0) == SIM_KIN_OK);

    sim_protocol_reset();
    assert(sim_protocol_accept(1, 1, "HELLO", "", 0) == SIM_PROTOCOL_OK);
    assert(sim_protocol_accept(1, 2, "SESSION_ATTACH", "", 0) == SIM_PROTOCOL_OK);
    assert(sim_protocol_accept(1, 3, "LOAD_WORKSPACE", "workspace", 0) == SIM_PROTOCOL_OK);
    for (index = 0; index < 4; ++index) {
        char operation[16];
        snprintf(operation, sizeof(operation), "switch-%u", (unsigned)(index + 1));
        assert(sim_protocol_accept(1, (uint32_t)(index + 4), "MDI_EXECUTE", operation, 0) == SIM_PROTOCOL_OK);
        assert(sim_kinematics_switch_select(&switcher, modes[index], joints, 5, &pose, &info) == SIM_KIN_OK);
        assert(sim_kinematics_switch_active(&switcher) == modes[index]);
        poses[index] = pose;
        memcpy(state_joints[index], joints, sizeof(joints));
    }

    assert(sim_program_init(&program) == SIM_PROGRAM_OK);
    assert(sim_program_load(&program, source, (uint32_t)(sizeof(source) - 1)) == SIM_PROGRAM_OK);
    assert(sim_program_start(&program) == SIM_PROGRAM_OK);
    while (program.status == SIM_PROGRAM_RUNNING) {
        assert(sim_program_step(&program) == SIM_PROGRAM_OK);
        if (program.has_motion && program.motion.motion_type == SIM_POSEMATH_CIRCULAR) {
            assert(sim_program_read_motion(&program, &arc) == SIM_PROGRAM_OK);
            have_arc = 1;
        }
    }
    assert(have_arc && arc.turn == -3);

    saved_switcher = switcher;
    saved_pose = pose;
    memcpy(saved_joints, joints, sizeof(joints));
    before_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &pose);
    assert(sim_kinematics_switch_select(&switcher, 0, joints, 5, &pose, &info) == SIM_KIN_OK);
    switcher = saved_switcher;
    pose = saved_pose;
    memcpy(joints, saved_joints, sizeof(joints));
    after_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &pose);
    assert(before_hash == after_hash && sim_kinematics_switch_active(&switcher) == 1);

    printf("{\"case\":\"xyzac-trt.ini\",\"levels\":{");
    printf("\"protocol\":[");
    for (index = 0; index < 4; ++index) {
        if (index) printf(",");
        printf("{\"command\":\"%s\",\"result\":\"verified\",\"seq\":%u,\"tick\":%u}", commands[index], (unsigned)(index + 4), (unsigned)(index + 1));
    }
    printf("],\"control\":[");
    for (index = 0; index < 4; ++index) {
        if (index) printf(",");
        printf("{\"active_type\":%d,\"command\":\"%s\",\"hal_pin\":\"motion.switchkins-type\",\"tick\":%u}", modes[index], commands[index], (unsigned)(index + 1));
    }
    printf("],\"state\":[");
    for (index = 0; index < 4; ++index) {
        if (index) printf(",");
        printf("{\"active_type\":%d,\"joints\":", modes[index]);
        print_joints(state_joints[index]);
        printf(",\"pose\":");
        print_pose(&poses[index]);
        printf(",\"tick\":%u}", (unsigned)(index + 1));
    }
    printf("],\"canonical\":[{\"end\":");
    print_pose(&arc.end);
    printf(",\"motion_type\":%d,\"source_line\":4,\"tick\":5,\"turn\":%d,\"velocity\":%.17g}],", arc.motion_type, arc.turn, arc.velocity);
    printf("\"recovery\":[{\"active_type\":%d,\"joints_equal\":true,\"pose_equal\":true,\"state_hash_equal\":true,\"tick\":6}]},", sim_kinematics_switch_active(&switcher));
    printf("\"schema\":\"xyzac-structured-trace.v1\",\"source\":\"linuxcnc-native-sim-core\",\"status\":\"verified\"}\n");
    return 0;
}