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

#include <emscripten/emscripten.h>

#include "emc/motion/motion.h"
#include "emc/nml_intf/motion_types.h"
#include "emc/tp/tp.h"

static emcmot_status_t status;
static emcmot_config_t config;

static void dio_write(int index, char value)
{
    (void)index;
    (void)value;
}

static void aio_write(int index, double value)
{
    (void)index;
    (void)value;
}

static void set_rotary_unlock(int joint, int unlock)
{
    (void)joint;
    (void)unlock;
}

static int get_rotary_unlock(int joint)
{
    (void)joint;
    return 1;
}

static double axis_vel_limit(int axis)
{
    return axis >= 0 && axis < EMCMOT_MAX_AXIS ? 10.0 : 0.0;
}

static double axis_acc_limit(int axis)
{
    return axis >= 0 && axis < EMCMOT_MAX_AXIS ? 20.0 : 0.0;
}

static void init_motion_state(void)
{
    memset(&status, 0, sizeof(status));
    memset(&config, 0, sizeof(config));

    config.numJoints = 3;
    config.numSpindles = 1;
    config.numDIO = 4;
    config.numAIO = 4;
    config.arcBlendOptDepth = 0;
    config.arcBlendEnable = 0;
    config.arcBlendFallbackEnable = 0;
    config.arcBlendGapCycles = 4;
    config.arcBlendRampFreq = 20.0;
    config.arcBlendTangentKinkRatio = 0.1;
    config.maxFeedScale = 1.0;

    status.net_feed_scale = 1.0;
    status.feed_scale = 1.0;
    status.rapid_scale = 1.0;
    status.enables_new = FS_ENABLED | SS_ENABLED;
    status.enables_queued = status.enables_new;
    status.spindle_status[0].at_speed = 1;
    status.spindle_status[0].direction = 1;
    status.spindleSync = 0;
    status.jerk = 0.0;
    status.planner_type = 0;

    tpMotData(&status, &config);
    tpMotFunctions(
        dio_write,
        aio_write,
        set_rotary_unlock,
        get_rotary_unlock,
        axis_vel_limit,
        axis_acc_limit);
}

static int pose_near(const EmcPose *actual, const EmcPose *expected)
{
    return fabs(actual->tran.x - expected->tran.x) < 1e-9 &&
        fabs(actual->tran.y - expected->tran.y) < 1e-9 &&
        fabs(actual->tran.z - expected->tran.z) < 1e-9;
}

static void append(char *out, size_t size, size_t *offset, const char *text)
{
    if (*offset >= size) {
        return;
    }
    const int written = snprintf(out + *offset, size - *offset, "%s", text);
    if (written > 0) {
        *offset += (size_t)written;
    }
}

static void append_linear_probe(char *out, size_t size, size_t *offset)
{
    init_motion_state();

    TP_STRUCT tp;
    EmcPose start;
    EmcPose end;
    struct state_tag_t tag;
    memset(&tp, 0, sizeof(tp));
    memset(&start, 0, sizeof(start));
    memset(&end, 0, sizeof(end));
    memset(&tag, 0, sizeof(tag));

    end.tran.x = 1.0;

    const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 1);
    const int set_cycle_rc = tpSetCycleTime(&tp, 0.001);
    const int set_pos_rc = tpSetPos(&tp, &start);
    const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0);
    const int set_vlimit_rc = tpSetVlimit(&tp, 1.0);
    const int set_amax_rc = tpSetAmax(&tp, 10.0);
    const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0);
    const int add_line_rc = tpAddLine(
        &tp,
        end,
        EMC_MOTION_TYPE_FEED,
        1.0,
        1.0,
        10.0,
        100.0,
        status.enables_new,
        0,
        -1,
        tag);

    int cycle_rc = 0;
    int cycles = 0;
    for (; cycles < 10000 && !tpIsDone(&tp); ++cycles) {
        cycle_rc = tpRunCycle(&tp, 1000000);
        if (cycle_rc < 0) {
            break;
        }
    }

    EmcPose final_pos;
    memset(&final_pos, 0, sizeof(final_pos));
    const int get_pos_rc = tpGetPos(&tp, &final_pos);

    char line[1024];
    snprintf(line, sizeof(line),
        "tp_create=%d\n"
        "tp_set_cycle_time=%d\n"
        "tp_set_pos=%d\n"
        "tp_set_vmax=%d\n"
        "tp_set_vlimit=%d\n"
        "tp_set_amax=%d\n"
        "tp_set_term_cond=%d\n"
        "tp_add_line=%d\n"
        "tp_cycle_rc=%d\n"
        "tp_cycles=%d\n"
        "tp_get_pos=%d\n"
        "tp_done_after_line=%d\n"
        "tp_queue_depth=%d\n"
        "tp_final_pos=%.12g,%.12g,%.12g\n",
        create_rc,
        set_cycle_rc,
        set_pos_rc,
        set_vmax_rc,
        set_vlimit_rc,
        set_amax_rc,
        set_term_rc,
        add_line_rc,
        cycle_rc,
        cycles,
        get_pos_rc,
        tpIsDone(&tp),
        tpQueueDepth(&tp),
        final_pos.tran.x,
        final_pos.tran.y,
        final_pos.tran.z);
    append(out, size, offset, line);
}

static void append_arc_probe(char *out, size_t size, size_t *offset)
{
    init_motion_state();

    TP_STRUCT tp;
    EmcPose start;
    EmcPose end;
    PmCartesian center;
    PmCartesian normal;
    struct state_tag_t tag;
    memset(&tp, 0, sizeof(tp));
    memset(&start, 0, sizeof(start));
    memset(&end, 0, sizeof(end));
    memset(&center, 0, sizeof(center));
    memset(&normal, 0, sizeof(normal));
    memset(&tag, 0, sizeof(tag));

    start.tran.x = 1.0;
    end.tran.y = 1.0;
    normal.z = 1.0;

    const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 2);
    const int set_cycle_rc = tpSetCycleTime(&tp, 0.001);
    const int set_pos_rc = tpSetPos(&tp, &start);
    const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0);
    const int set_vlimit_rc = tpSetVlimit(&tp, 1.0);
    const int set_amax_rc = tpSetAmax(&tp, 10.0);
    const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0);
    const int add_circle_rc = tpAddCircle(
        &tp,
        end,
        center,
        normal,
        0,
        EMC_MOTION_TYPE_ARC,
        1.0,
        1.0,
        10.0,
        100.0,
        status.enables_new,
        0,
        tag);

    int cycle_rc = 0;
    int cycles = 0;
    for (; cycles < 10000 && !tpIsDone(&tp); ++cycles) {
        cycle_rc = tpRunCycle(&tp, 1000000);
        if (cycle_rc < 0) {
            break;
        }
    }

    EmcPose final_pos;
    memset(&final_pos, 0, sizeof(final_pos));
    const int get_pos_rc = tpGetPos(&tp, &final_pos);
    const int near_end = pose_near(&final_pos, &end);

    char line[1024];
    snprintf(line, sizeof(line),
        "tp_arc_create=%d\n"
        "tp_arc_set_cycle_time=%d\n"
        "tp_arc_set_pos=%d\n"
        "tp_arc_set_vmax=%d\n"
        "tp_arc_set_vlimit=%d\n"
        "tp_arc_set_amax=%d\n"
        "tp_arc_set_term_cond=%d\n"
        "tp_add_circle=%d\n"
        "tp_arc_cycle_rc=%d\n"
        "tp_arc_cycles=%d\n"
        "tp_arc_get_pos=%d\n"
        "tp_done_after_arc=%d\n"
        "tp_arc_queue_depth=%d\n"
        "tp_arc_final_pos_near_end=%d\n"
        "tp_arc_final_pos=%.12g,%.12g,%.12g\n",
        create_rc,
        set_cycle_rc,
        set_pos_rc,
        set_vmax_rc,
        set_vlimit_rc,
        set_amax_rc,
        set_term_rc,
        add_circle_rc,
        cycle_rc,
        cycles,
        get_pos_rc,
        tpIsDone(&tp),
        tpQueueDepth(&tp),
        near_end,
        final_pos.tran.x,
        final_pos.tran.y,
        final_pos.tran.z);
    append(out, size, offset, line);
}

static void append_queued_lines_probe(char *out, size_t size, size_t *offset)
{
    init_motion_state();

    TP_STRUCT tp;
    EmcPose start;
    EmcPose end1;
    EmcPose end2;
    struct state_tag_t tag;
    memset(&tp, 0, sizeof(tp));
    memset(&start, 0, sizeof(start));
    memset(&end1, 0, sizeof(end1));
    memset(&end2, 0, sizeof(end2));
    memset(&tag, 0, sizeof(tag));

    end1.tran.x = 1.0;
    end2.tran.x = 1.0;
    end2.tran.y = 1.0;

    const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 3);
    const int set_cycle_rc = tpSetCycleTime(&tp, 0.001);
    const int set_pos_rc = tpSetPos(&tp, &start);
    const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0);
    const int set_vlimit_rc = tpSetVlimit(&tp, 1.0);
    const int set_amax_rc = tpSetAmax(&tp, 10.0);
    const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0);
    const int add_line1_rc = tpAddLine(
        &tp,
        end1,
        EMC_MOTION_TYPE_FEED,
        1.0,
        1.0,
        10.0,
        100.0,
        status.enables_new,
        0,
        -1,
        tag);
    const int depth_after_line1 = tpQueueDepth(&tp);
    const int add_line2_rc = tpAddLine(
        &tp,
        end2,
        EMC_MOTION_TYPE_FEED,
        1.0,
        1.0,
        10.0,
        100.0,
        status.enables_new,
        0,
        -1,
        tag);
    const int depth_after_line2 = tpQueueDepth(&tp);

    int cycle_rc = 0;
    int cycles = 0;
    for (; cycles < 20000 && !tpIsDone(&tp); ++cycles) {
        cycle_rc = tpRunCycle(&tp, 1000000);
        if (cycle_rc < 0) {
            break;
        }
    }

    EmcPose final_pos;
    memset(&final_pos, 0, sizeof(final_pos));
    const int get_pos_rc = tpGetPos(&tp, &final_pos);
    const int near_end = pose_near(&final_pos, &end2);

    char line[1024];
    snprintf(line, sizeof(line),
        "tp_queue_create=%d\n"
        "tp_queue_set_cycle_time=%d\n"
        "tp_queue_set_pos=%d\n"
        "tp_queue_set_vmax=%d\n"
        "tp_queue_set_vlimit=%d\n"
        "tp_queue_set_amax=%d\n"
        "tp_queue_set_term_cond=%d\n"
        "tp_queue_add_line1=%d\n"
        "tp_queue_depth_after_line1=%d\n"
        "tp_queue_add_line2=%d\n"
        "tp_queue_depth_after_line2=%d\n"
        "tp_queue_cycle_rc=%d\n"
        "tp_queue_cycles=%d\n"
        "tp_queue_get_pos=%d\n"
        "tp_done_after_queued_lines=%d\n"
        "tp_queue_final_depth=%d\n"
        "tp_queue_final_pos_near_end=%d\n"
        "tp_queue_final_pos=%.12g,%.12g,%.12g\n",
        create_rc,
        set_cycle_rc,
        set_pos_rc,
        set_vmax_rc,
        set_vlimit_rc,
        set_amax_rc,
        set_term_rc,
        add_line1_rc,
        depth_after_line1,
        add_line2_rc,
        depth_after_line2,
        cycle_rc,
        cycles,
        get_pos_rc,
        tpIsDone(&tp),
        tpQueueDepth(&tp),
        near_end,
        final_pos.tran.x,
        final_pos.tran.y,
        final_pos.tran.z);
    append(out, size, offset, line);
}

EMSCRIPTEN_KEEPALIVE
char *lctp_run_probe(void)
{
    char *out = (char *)malloc(8192);
    if (!out) {
        return NULL;
    }
    out[0] = '\0';
    size_t offset = 0;

    TP_STRUCT tp;
    TC_STRUCT tc;
    EmcPose pose;
    memset(&tp, 0, sizeof(tp));
    memset(&tc, 0, sizeof(tc));
    memset(&pose, 0, sizeof(pose));

    pose.tran.x = 1.0;
    pose.tran.y = 2.0;
    pose.tran.z = 3.0;

    char line[512];
    snprintf(line, sizeof(line),
        "sizeof_TP_STRUCT=%zu\n"
        "sizeof_TC_STRUCT=%zu\n"
        "tp_default_queue_size=%d\n"
        "tp_err_ok=%d\n"
        "tc_linear=%d\n"
        "tc_circular=%d\n"
        "pose_xyz=%.12g,%.12g,%.12g\n",
        sizeof(tp),
        sizeof(tc),
        TP_DEFAULT_QUEUE_SIZE,
        TP_ERR_OK,
        TC_LINEAR,
        TC_CIRCULAR,
        pose.tran.x,
        pose.tran.y,
        pose.tran.z);
    append(out, 8192, &offset, line);
    append_linear_probe(out, 8192, &offset);
    append_arc_probe(out, 8192, &offset);
    append_queued_lines_probe(out, 8192, &offset);
    return out;
}

EMSCRIPTEN_KEEPALIVE
void lctp_free_string(char *value)
{
    free(value);
}
