/*
 * Copyright (C) 2022 EXOLIGENT (www.exoligent.com)
 * Author: Maxime Coroyer <maxime.coroyer@exoligent.com>
 */

#define _GNU_SOURCE     // <sched.h>
#include <sched.h>      // CPU_ZERO(), CPU_SET()
#include <pthread.h>    // pthread()

#include <open62541/plugin/pubsub_ethernet.h>   // UA_EthernetWriterGroupTransportDataType

#include <linux/if_link.h>
#include <linux/if_xdp.h>

//--!! TODO: should not be used
#include "../../open62541/ua_pubsub.h"          // UA_WriterGroup

#include "opc-rt.h"
#include "test.h"

#define US_TO_NS (1000)
#define MS_TO_NS (1000 * (US_TO_NS))
#define S_TO_NS (1000 * (MS_TO_NS))
#define SLEEP_S 5
#define CLOCKID CLOCK_TAI

static pthread_t thread_creation(UA_Int16 thr_prio, size_t core_affinity,
        void *(*thread) (void *), char *app_name, void *srv_cfg);
static void *pub_task(void *arg);
static void *sub_task(void *arg);
static void *app_task(void *arg);

static struct timespec basetime;
UA_Boolean is_basetime_init = false;

/**
 * Cyclic callback (add, remove, change period)
 */
UA_StatusCode opc_rt_cb_add(UA_Server *srv, UA_NodeId group_id,
    UA_ServerCallback cb, void *data, UA_Double interval_ms,
    UA_DateTime *baseTime, UA_TimerPolicy timerPolicy, UA_UInt64 *cb_id)
{
    /* Initialize arguments required for the thread to run */
    struct opc_rt_thread_args *thr_args = \
        (struct opc_rt_thread_args*) calloc(1, sizeof(struct opc_rt_thread_args));

    /* Pass the value required for the threads */
    thr_args->server       = srv;
    thr_args->data         = data;
    thr_args->callback     = cb;
    thr_args->interval_ms  = interval_ms;
    thr_args->callback_id  = cb_id;

    if (UA_NodeId_equal(&group_id, &sess.opc.pub.w_group_id)) {
        /* create the publisher thread */
        char thr_name[10] = "publisher";
        *cb_id = thread_creation(OPC_RT_PUB_SCHED_PRIORITY, OPC_RT_CORE_TWO,
                    pub_task, thr_name, thr_args);

        _print(opc, info, "%s task: thread id is %"PRIu64"\n", thr_name, *cb_id);
    }

    if (UA_NodeId_equal(&group_id, &sess.opc.sub.r_group_id)) {
        /* create the subscriber thread */
        char thr_name[11] = "subscriber";
        *cb_id = thread_creation(OPC_RT_SUB_SCHED_PRIORITY, OPC_RT_CORE_TWO,
                    sub_task, thr_name, thr_args);

        _print(opc, info, "%s task: thread id is %"PRIu64"\n", thr_name, *cb_id);
    }

    return UA_STATUSCODE_GOOD;
}

UA_StatusCode opc_rt_cb_change(UA_Server *srv, UA_NodeId group_id,
    UA_UInt64 cb_id, UA_Double interval_ms, UA_DateTime *baseTime,
    UA_TimerPolicy timerPolicy)
{
    /**
     * Callback interval need not be modified as it is thread based
     * implementation. The thread uses nanosleep for calculating cycle time
     * and modification in nanosleep value changes cycle time
     */
    return UA_STATUSCODE_GOOD;
}

void opc_rt_cb_remove(UA_Server *srv, UA_NodeId group_id, UA_UInt64 cb_id)
{
    if (cb_id && (pthread_join(cb_id, NULL) != 0))
        _print(opc, warn, "pthread_join failed (thread_id=%"PRIu64")\n", cb_id);
}

/**
 * If the external data source is written over the information model, the
 * externalDataWriteCallback will be triggered. The user has to take care and assure
 * that the write leads not to synchronization issues and race conditions.
 */
UA_StatusCode opc_rt_cb_ext_datawrite(UA_Server *srv,
    const UA_NodeId *sess_id, void *sess_ctx, const UA_NodeId *node_id,
    void *node_ctx, const UA_NumericRange *range,
    const UA_DataValue *data_value)
{
    // node values are updated by using variables in the memory
    // UA_Server_write is not used for updating node values.
    return UA_STATUSCODE_GOOD;
}

UA_StatusCode opc_rt_cb_ext_dataread_notif(UA_Server *srv,
    const UA_NodeId *sess_id, void *sess_ctx, const UA_NodeId *node_id,
    void *node_ctx, const UA_NumericRange *range)
{
    // allow read without any preparation
    return UA_STATUSCODE_GOOD;
}

int opc_rt_app_start(void *args)
{
    struct tst_session *sess = (struct tst_session *) args;
    char thr_name[11] = "pubsub_app";

    sess->opc.pubsub_thr_task = thread_creation(OPC_RT_APP_SCHED_PRIORITY,
        OPC_RT_CORE_THREE, app_task, thr_name, args);
    return 0;
}

int opc_rt_app_stop(void *args)
{
    struct tst_session *sess = (struct tst_session *) args;
    pthread_join(sess->opc.pubsub_thr_task, NULL);
    return 0;
}

static pthread_t thread_creation(UA_Int16 thr_prio, size_t core_affinity,
        void *(*thread) (void *), char *task_name, void *srv_cfg)
{
    UA_Int32 ret = 0;
    struct sched_param thr_param;
    pthread_t thr_id = pthread_self();
    cpu_set_t cpu_set;

    thr_param.sched_priority = thr_prio;
    if (pthread_setschedparam(thr_id, SCHED_FIFO, &thr_param))
    {
        _print(opc, err, "%s task: pthread_setschedparam failed\n", task_name);
        exit(1);
    }

    _print(opc, info, "%s task: thread priority is %d\n", task_name,
        thr_param.sched_priority);

    CPU_ZERO(&cpu_set);
    CPU_SET(core_affinity, &cpu_set);

    if ((ret = pthread_setaffinity_np(thr_id, sizeof(cpu_set_t), &cpu_set)))
    {
        fprintf(stderr, "%s: pthread_setaffinity_np error (%s)",
            task_name, strerror(ret));
        exit(1);
    }

    if (pthread_create(&thr_id, NULL, thread, srv_cfg))
        _print(opc, warn, "%s task: cannot create thread\n", task_name);

    if (CPU_ISSET(core_affinity, &cpu_set))
        _print(opc, info, "%s task: cpu_core=%ld\n", task_name, core_affinity);

    return thr_id;
}

/* nanosecond field in timespec is checked for overflowing */
static void ns_check_n_reformat(struct timespec *value)
{
    while (value->tv_nsec > (S_TO_NS - 1)) {
        value->tv_sec  += 1;
        value->tv_nsec -= S_TO_NS;
    }
}

/**
 * Publisher thread routine
 *
 * Note: This routine is used by the publisher thread to publish the data
 * at the user-defined cycle time (default=250us).
 */
static void *pub_task(void *arg)
{
    struct opc_rt_thread_args *thr_args = (struct opc_rt_thread_args *) arg;
    struct timespec next_ns_time = {.tv_sec = 0, .tv_nsec = 0};
    UA_ServerCallback pub_cb;
    UA_Server *srv = NULL;
    UA_UInt64 pub_sleep_ns, pub_alive_ns, interval_ns, tx_time;
    UA_WriterGroup *w_group = NULL; //--!! TODO: remove w_group usage
    UA_EthernetWriterGroupTransportDataType eth_transp_settings;
    UA_ExtensionObject transp_settings;
    unsigned long last_us = 0;
    int is_first = 1;
#if defined(__QNX__) || defined(_WIN32) || defined(_WIN64)
    static uint64_t start_treatment, last_treatment = 0;
#else
    static struct timeval start_treatment,
            last_treatment = { .tv_sec = 0, .tv_usec = 0, };
#endif

    /**
     * Publisher will sleep for 60% of cycle time and then prepares
     * the tx packet within 40%
     */
    pub_sleep_ns = sess.opc.pub.publish_period_us * US_TO_NS * OPC_RT_PUB_WKUP_PERCENTAGE;
    pub_alive_ns = (sess.opc.pub.publish_period_us * US_TO_NS) - pub_sleep_ns;

    srv         = thr_args->server;
    pub_cb      = thr_args->callback;
    w_group     = (UA_WriterGroup *) thr_args->data;
    interval_ns = (UA_UInt64) (thr_args->interval_ms * MS_TO_NS);

    memset(&eth_transp_settings, 0, sizeof(UA_EthernetWriterGroupTransportDataType));
    memset(&transp_settings, 0, sizeof(UA_ExtensionObject));

    eth_transp_settings.transmission_time = 0;
    transp_settings.content.decoded.data = &eth_transp_settings;
    w_group->config.transportSettings = transp_settings;

    if (!is_basetime_init) {
        /* get current time and compute the next nanosleeptime */
        clock_gettime(CLOCKID, &basetime);
        basetime.tv_sec += SLEEP_S;
        basetime.tv_nsec = 0;
        is_basetime_init = true;
    }

    /* variable to nano sleep until 1ms before a 1 second boundary */
    next_ns_time.tv_sec  = basetime.tv_sec;
    next_ns_time.tv_nsec = basetime.tv_nsec + pub_sleep_ns;
    ns_check_n_reformat(&next_ns_time);

    while (sess.opc.running) {
        int err = clock_nanosleep(CLOCKID, TIMER_ABSTIME, &next_ns_time, NULL);
        if (err) {
            _print(opc, err, "pub_task: clock_nanosleep: %s\n",
                strerror(errno));
            sess.opc.running = false;
            continue;
        }

        _gettime(&start_treatment);
        tx_time = ((UA_UInt64) next_ns_time.tv_sec * S_TO_NS + \
                   (UA_UInt64) next_ns_time.tv_nsec) + \
                   pub_alive_ns + OPC_RT_QBV_OFFSET;

        /* update next w_group tx time */
        eth_transp_settings.transmission_time = tx_time;
        //_print(opc, info, "pub_task - pub_cb\n");

        UA_LOCK(&sess.opc.mu);
        if (sess.opc.pub.active) {
            pub_cb(srv, w_group);
            sess.stats.nb_pub_op++;
        }
        UA_UNLOCK(&sess.opc.mu);

        next_ns_time.tv_nsec += (__syscall_slong_t) interval_ns;
        ns_check_n_reformat(&next_ns_time);

        /* min/max occurence measurement */
        last_us = (is_first) ? 0 : get_elapsed_us(start_treatment, last_treatment);
        is_first = 0;
        last_treatment = start_treatment;
        if (last_us)
            update_min_max(last_us, &sess.stats.min_pub_op_per,
                &sess.stats.max_pub_op_per);
    }

    UA_free(thr_args);

    return (void*) NULL;
}

/**
 * Subscriber thread routine
 */
static void *sub_task(void *arg)
{
    struct opc_rt_thread_args *thr_args = (struct opc_rt_thread_args *) arg;
    struct timespec next_ns_time = {.tv_sec = 0, .tv_nsec = 0};
    UA_ServerCallback sub_cb;
    UA_Server *srv = NULL;
    UA_UInt64 sub_sleep_ns, interval_ns;
    void *r_group = NULL;
    unsigned long last_us = 0;
    int is_first = 1;
#if defined(__QNX__) || defined(_WIN32) || defined(_WIN64)
    static uint64_t start_treatment, last_treatment = 0;
#else
    static struct timeval start_treatment,
            last_treatment = { .tv_sec = 0, .tv_usec = 0, };
#endif

    /**
     * Subscriber will wakeup only during start of cycle and check whether
     * the packets are received
     */
    sub_sleep_ns = sess.opc.pub.publish_period_us * US_TO_NS * OPC_RT_SUB_WKUP_PERCENTAGE;
    interval_ns = (UA_UInt64) (sess.opc.pub.publish_period_us * US_TO_NS);

    srv     = thr_args->server;
    sub_cb  = thr_args->callback;
    r_group = thr_args->data;

    if (!is_basetime_init) {
        /* get current time and compute the next nanosleeptime */
        clock_gettime(CLOCKID, &basetime);
        basetime.tv_sec += SLEEP_S;
        basetime.tv_nsec = 0;
        is_basetime_init = true;
    }

    /* variable to nano sleep until 1ms before a 1 second boundary */
    next_ns_time.tv_sec  = basetime.tv_sec;
    next_ns_time.tv_nsec = basetime.tv_nsec + sub_sleep_ns;
    ns_check_n_reformat(&next_ns_time);

    while (sess.opc.running) {
        clock_nanosleep(CLOCKID, TIMER_ABSTIME, &next_ns_time, NULL);

        _gettime(&start_treatment);
        //_print(opc, info, "sub_task - sub_cb\n");
        UA_LOCK(&sess.opc.mu);
        sub_cb(srv, r_group);
        sess.stats.nb_sub_op++;
        UA_UNLOCK(&sess.opc.mu);
        next_ns_time.tv_nsec += (__syscall_slong_t) interval_ns;
        ns_check_n_reformat(&next_ns_time);

        /* min/max occurence measurement */
        last_us = (is_first) ? 0 : get_elapsed_us(start_treatment, last_treatment);
        is_first = 0;
        last_treatment = start_treatment;
        if (last_us)
            update_min_max(last_us, &sess.stats.min_sub_op_per,
                &sess.stats.max_sub_op_per);
    }

    UA_free(thr_args);

    return (void*) NULL;
}

/**
 * Subscriber thread routine
 */
static void *app_task(void *arg)
{
    struct tst_session *sess = (struct tst_session *) arg;
    struct timespec next_ns_time = {.tv_sec = 0, .tv_nsec = 0};
    UA_Server *srv = sess->opc.srv;
    struct opc_data *r_data = &sess->opc.sub.data;
    struct opc_data *w_data = &sess->opc.pub.data;
    UA_StatusCode srv_status = UA_STATUSCODE_GOOD;
    UA_UInt64 app_sleep_ns, interval_ns;
    unsigned long last_us = 0;
    int is_first = 1;
#if defined(__QNX__) || defined(_WIN32) || defined(_WIN64)
    static uint64_t start_treatment, last_treatment = 0;
#else
    static struct timeval start_treatment,
            last_treatment = { .tv_sec = 0, .tv_usec = 0, };
#endif

    /**
     * Application will sleep for 30% of cycle time
     */
    app_sleep_ns = sess->opc.pub.publish_period_us * US_TO_NS * OPC_RT_APP_WKUP_PERCENTAGE;
    interval_ns = (UA_UInt64) (sess->opc.pub.publish_period_us * US_TO_NS);

    if (!is_basetime_init) {
        /* get current time and compute the next nanosleeptime */
        clock_gettime(CLOCKID, &basetime);
        basetime.tv_sec += SLEEP_S;
        basetime.tv_nsec = 0;
        is_basetime_init = true;
    }

    next_ns_time.tv_sec  = basetime.tv_sec;
    next_ns_time.tv_nsec = basetime.tv_nsec + app_sleep_ns;
    ns_check_n_reformat(&next_ns_time);

    if (!r_data || !w_data || !srv)
        sess->opc.running = false;

    if (UA_NodeId_isNull(&r_data->v_node_id) ||
        UA_NodeId_isNull(&w_data->v_node_id))
        sess->opc.running = false;

    while (sess->opc.running) {
        clock_nanosleep(CLOCKID, TIMER_ABSTIME, &next_ns_time, NULL);
        //_print(opc, info, "app_task\n");

        _gettime(&start_treatment);

        ///* test */
        //UA_ByteString *s = &r_data->data;
        //UA_ByteString *s_rt = &r_data->rt_data;
        //UA_ByteString *p = &w_data->data;
        //UA_ByteString *p_rt = &w_data->rt_data;
        //_printf(opc, info, "s=%02x s_rt=%02x p=%02x p_rt=%02x\n",
        //    (uint8_t) s->data[0],
        //    (uint8_t) s_rt->data[0],
        //    (uint8_t) p->data[0],
        //    (uint8_t) p_rt->data[0]);
        ///* do not work */
        ////UA_ByteString_copy(&r_data->rt_data, &w_data->rt_data);

        // copy sub->pub operation
        UA_LOCK(&sess->opc.mu);
        sess->stats.nb_app_op++;
        {
            UA_Variant out;
            UA_Variant_init(&out);
            srv_status = UA_Server_readValue(srv, r_data->v_node_id, &out);
            if (srv_status != UA_STATUSCODE_GOOD ||
                !UA_Variant_hasScalarType(&out, &UA_TYPES[UA_TYPES_BYTESTRING])) {
                _print(opc, warn,
                    "failed to read subscribe value (srv=%p,"
                    " srv_status=0x%08x, is_byte_string=%d)\n",
                    srv, srv_status,
                    UA_Variant_hasScalarType(&out, &UA_TYPES[UA_TYPES_BYTESTRING]));
                UA_Variant_clear(&out);
                UA_UNLOCK(&sess->opc.mu);
                continue;
            }

            UA_ByteString *p = (UA_ByteString *) out.data;
            UA_ByteString *q = (UA_ByteString *) &w_data->rt_data;

            if (p)
            {
                if (p->data && (p->length == 8))
                {
                    // compare previous w_data with new r_data
                    if (q->data && (q->length == 8)) {
                        uint64_t prev_w_data = *((uint64_t*) q->data);
                        uint64_t new_r_data  = *((uint64_t*) p->data);
                        if (prev_w_data != new_r_data) {
                            if (prev_w_data + 1 != new_r_data) {
                                int64_t diff = new_r_data - prev_w_data;
                                _print(opc, warn,
                                    "ping/pong anomaly: sub=%"PRIu64
                                    " --> pub=%"PRIu64
                                    " [diff=%"PRIi64"]\n",
                                    new_r_data, prev_w_data, diff);
                            }
                        }
                        UA_ByteString_clear(q);
                        UA_ByteString_copy(p, q);
                    } else {
                        _print(opc, warn,
                            "incorrect subscribe dest value (q->data=%p,"
                            " q->length=%ld)\n", q->data, q->length);
                    }

                    UA_ByteString_clear(p);
                    sess->opc.pub.active = 1;

                    //size_t j;
                    //printf("srv_data: ");
                    //for (j = 0; j < p->length; ++j)
                    //    printf("%02x ", (uint8_t) p->data[j]);
                    //printf("\n");
                }
            } else {
                _print(opc, warn,
                    "incorrect subscribe value (p->data=%p,"
                    " p->length=%ld)\n", p->data, p->length);
            }
            UA_Variant_clear(&out);
        }
        UA_UNLOCK(&sess->opc.mu);

        next_ns_time.tv_nsec += (__syscall_slong_t) interval_ns;
        ns_check_n_reformat(&next_ns_time);

        /* min/max occurence measurement */
        last_us = (is_first) ? 0 : get_elapsed_us(start_treatment, last_treatment);
        is_first = 0;
        last_treatment = start_treatment;
        if (last_us)
            update_min_max(last_us, &sess->stats.min_app_op_per,
                &sess->stats.max_app_op_per);
    }

    return (void*) NULL;
}