#include <cmath>
#include <iostream>

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

namespace {

emcmot_status_t status{};
emcmot_config_t config{};
emcmot_joint_t joints[EMCMOT_MAX_JOINTS]{};

void dio_write(int, char) {}
void aio_write(int, double) {}
void set_rotary_unlock(int, int) {}
int get_rotary_unlock(int) { return 1; }

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

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

void init_motion_state()
{
    status = {};
    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);
}

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

void run_linear_probe()
{
    init_motion_state();

    TP_STRUCT tp{};
    EmcPose start{};
    EmcPose end{};
    struct state_tag_t 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{};
    const int get_pos_rc = tpGetPos(&tp, &final_pos);

    std::cout << "tp_create=" << create_rc << "\n";
    std::cout << "tp_set_cycle_time=" << set_cycle_rc << "\n";
    std::cout << "tp_set_pos=" << set_pos_rc << "\n";
    std::cout << "tp_set_vmax=" << set_vmax_rc << "\n";
    std::cout << "tp_set_vlimit=" << set_vlimit_rc << "\n";
    std::cout << "tp_set_amax=" << set_amax_rc << "\n";
    std::cout << "tp_set_term_cond=" << set_term_rc << "\n";
    std::cout << "tp_add_line=" << add_line_rc << "\n";
    std::cout << "tp_cycle_rc=" << cycle_rc << "\n";
    std::cout << "tp_cycles=" << cycles << "\n";
    std::cout << "tp_get_pos=" << get_pos_rc << "\n";
    std::cout << "tp_done_after_line=" << tpIsDone(&tp) << "\n";
    std::cout << "tp_queue_depth=" << tpQueueDepth(&tp) << "\n";
    std::cout << "tp_final_pos="
              << final_pos.tran.x << ","
              << final_pos.tran.y << ","
              << final_pos.tran.z << "\n";
}

void run_arc_probe()
{
    init_motion_state();

    TP_STRUCT tp{};
    EmcPose start{};
    EmcPose end{};
    PmCartesian center{};
    PmCartesian normal{};
    struct state_tag_t 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{};
    const int get_pos_rc = tpGetPos(&tp, &final_pos);
    const int near_end = pose_near(final_pos, end);

    std::cout << "tp_arc_create=" << create_rc << "\n";
    std::cout << "tp_arc_set_cycle_time=" << set_cycle_rc << "\n";
    std::cout << "tp_arc_set_pos=" << set_pos_rc << "\n";
    std::cout << "tp_arc_set_vmax=" << set_vmax_rc << "\n";
    std::cout << "tp_arc_set_vlimit=" << set_vlimit_rc << "\n";
    std::cout << "tp_arc_set_amax=" << set_amax_rc << "\n";
    std::cout << "tp_arc_set_term_cond=" << set_term_rc << "\n";
    std::cout << "tp_add_circle=" << add_circle_rc << "\n";
    std::cout << "tp_arc_cycle_rc=" << cycle_rc << "\n";
    std::cout << "tp_arc_cycles=" << cycles << "\n";
    std::cout << "tp_arc_get_pos=" << get_pos_rc << "\n";
    std::cout << "tp_done_after_arc=" << tpIsDone(&tp) << "\n";
    std::cout << "tp_arc_queue_depth=" << tpQueueDepth(&tp) << "\n";
    std::cout << "tp_arc_final_pos_near_end=" << near_end << "\n";
    std::cout << "tp_arc_final_pos="
              << final_pos.tran.x << ","
              << final_pos.tran.y << ","
              << final_pos.tran.z << "\n";
}

void run_queued_lines_probe()
{
    init_motion_state();

    TP_STRUCT tp{};
    EmcPose start{};
    EmcPose end1{};
    EmcPose end2{};
    struct state_tag_t 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{};
    const int get_pos_rc = tpGetPos(&tp, &final_pos);
    const int near_end = pose_near(final_pos, end2);

    std::cout << "tp_queue_create=" << create_rc << "\n";
    std::cout << "tp_queue_set_cycle_time=" << set_cycle_rc << "\n";
    std::cout << "tp_queue_set_pos=" << set_pos_rc << "\n";
    std::cout << "tp_queue_set_vmax=" << set_vmax_rc << "\n";
    std::cout << "tp_queue_set_vlimit=" << set_vlimit_rc << "\n";
    std::cout << "tp_queue_set_amax=" << set_amax_rc << "\n";
    std::cout << "tp_queue_set_term_cond=" << set_term_rc << "\n";
    std::cout << "tp_queue_add_line1=" << add_line1_rc << "\n";
    std::cout << "tp_queue_depth_after_line1=" << depth_after_line1 << "\n";
    std::cout << "tp_queue_add_line2=" << add_line2_rc << "\n";
    std::cout << "tp_queue_depth_after_line2=" << depth_after_line2 << "\n";
    std::cout << "tp_queue_cycle_rc=" << cycle_rc << "\n";
    std::cout << "tp_queue_cycles=" << cycles << "\n";
    std::cout << "tp_queue_get_pos=" << get_pos_rc << "\n";
    std::cout << "tp_done_after_queued_lines=" << tpIsDone(&tp) << "\n";
    std::cout << "tp_queue_final_depth=" << tpQueueDepth(&tp) << "\n";
    std::cout << "tp_queue_final_pos_near_end=" << near_end << "\n";
    std::cout << "tp_queue_final_pos="
              << final_pos.tran.x << ","
              << final_pos.tran.y << ","
              << final_pos.tran.z << "\n";
}

} // namespace

int main()
{
    TP_STRUCT tp{};
    TC_STRUCT tc{};
    EmcPose pose{};

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

    std::cout << "sizeof_TP_STRUCT=" << sizeof(tp) << "\n";
    std::cout << "sizeof_TC_STRUCT=" << sizeof(tc) << "\n";
    std::cout << "tp_default_queue_size=" << TP_DEFAULT_QUEUE_SIZE << "\n";
    std::cout << "tp_err_ok=" << TP_ERR_OK << "\n";
    std::cout << "tc_linear=" << TC_LINEAR << "\n";
    std::cout << "tc_circular=" << TC_CIRCULAR << "\n";
    std::cout << "pose_xyz=" << pose.tran.x << "," << pose.tran.y << "," << pose.tran.z << "\n";
    run_linear_probe();
    run_arc_probe();
    run_queued_lines_probe();
    return 0;
}
