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

#include <stdlib.h>     // malloc(), free()
#include <string.h>     // memset()
#include <unistd.h>     // close()
#include <pthread.h>    // pthread()

#include <open62541/server_config_default.h>                // UA_Server_new()

#include "gw.h"

#include <open62541/plugin/pubsub_udp.h>                    // UA_PubSubTransportLayerUDPMP()
#ifdef UA_ENABLE_PUBSUB_ETH_UADP
#   include <open62541/plugin/pubsub_ethernet.h>            // UA_PubSubTransportLayerEthernet()
#   ifdef OPC_ENABLE_REALTIME_CAP
#       include <linux/if_link.h>
#       include <linux/if_xdp.h>
#       include "opc-rt.h"
#   endif
#endif

static void opc_cleanup_task(void *arg);
static void *opc_task(void *arg);

int opc_pub_start(struct gateway *gw)
{
    pthread_attr_t thr_attr;
    struct sched_param thr_param;

    if (!gw)
        return -1;

    if (gw->opc.running)
        return -1;

    /* app - thread creation */
    if (pthread_attr_init(&thr_attr)) {
        _print(opc, err, "[failed] pthread_attr_init()\n");
        return -1; // errno is already set
    }

    pthread_attr_setinheritsched(&thr_attr, PTHREAD_EXPLICIT_SCHED);
    pthread_attr_getschedparam (&thr_attr, &thr_param);
    //pthread_attr_setschedparam(&thr_attr, &thr_param);

    /* thread: loop task */
    if (pthread_create(&gw->opc.thr_task, &thr_attr, opc_task, (void*) gw)) {
        pthread_attr_destroy(&thr_attr);
        _print(opc, err, "[failed] pthread_create()\n");
        return -1; // errno is already set
    }

    pthread_attr_destroy(&thr_attr);
    return 0;
}

int opc_pub_stop(struct gateway *gw)
{
    if (!gw)
        return -1;

    if (!gw->opc.running)
        return -1;

    gw->opc.running = false;
    pthread_join(gw->opc.thr_task, NULL);
    opc_cleanup_task(gw);
    return gw->opc.status == UA_STATUSCODE_GOOD ? 0 : -1;
}

void opc_display_update(struct gateway *gw)
{
    printf("OPCUA: Publisher(%s) Subscriber(%s)\n",
        gw->opc.pub.url_str, gw->opc.sub.url_str);
    /* TODO */
}

static void opc_log(void *ctx, UA_LogLevel level, UA_LogCategory cat,
    const char *msg, va_list args)
{
    msg_type type;

    if (ctx != NULL && (UA_LogLevel)(uintptr_t)ctx > level)
        return;

    if (level < UA_LOGLEVEL_WARNING)
        type = info;
    else if (level > UA_LOGLEVEL_WARNING)
        type = err;
    else
        type = warn;

#if UA_MULTITHREADING >= 200
    pthread_mutex_lock(&printf_mutex);
#endif

    _vprint(opc, type, msg, args);
    printf("\n");
    //fflush(stdout);

#if UA_MULTITHREADING >= 200
    pthread_mutex_unlock(&printf_mutex);
#endif
}

static void opc_fip_objects_create(struct gateway *gw)
{
    char path_str[255];
    int i = 0;

    /**
     * Node object
     */
    UA_ObjectAttributes n_attr = UA_ObjectAttributes_default;
    n_attr.displayName = UA_LOCALIZEDTEXT("en-US", gw->fip.name_str);
    UA_Server_addObjectNode(gw->opc.srv,                /* server */
        UA_NODEID_STRING(1, "fip"),                     /* requestedNewNodeId */
        UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),   /* parentNodeId */
        UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),       /* referenceTypeId */
        UA_QUALIFIEDNAME(1, "fip"),                     /* browseName */
        UA_NODEID_STRING(1, "fip.otype.node"),           /* typeDefinition */
        n_attr,                                         /* attr */
        (void *) gw->fip.n,                             /* nodeContext */
        NULL);                                          /* outNewNodeId */

    /**
     * Variable objects
     */
    /* => add cons var objects */
    for (i = 0; i < gw->fip.cons_varcnt; ++i) {
        struct pwrfip_var *var = gw->fip.cons_varlist[i];
        struct gw_var_cfg *gw_var_cfg = \
            (struct gw_var_cfg *) var->user_ctx;

        memset(path_str, 0, sizeof(path_str));
        sprintf(path_str, "fip.var.cons.%04x", var->id);

        UA_ObjectAttributes v_attr = UA_ObjectAttributes_default;
        v_attr.displayName = UA_LOCALIZEDTEXT("en-US", gw_var_cfg->name);
        UA_Server_addObjectNode(gw->opc.srv,
            UA_NODEID_STRING(1, path_str),
            UA_NODEID_STRING(1, "fip.var.cons_folder"),
            UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
            UA_QUALIFIEDNAME(1, path_str),
            UA_NODEID_STRING(1, "fip.otype.var"),
            v_attr,
            (void *) gw->fip.cons_varlist[i],
            NULL);
    }
    /* => add prod var objects */
    for (i = 0; i < gw->fip.prod_varcnt; ++i) {
        struct pwrfip_var *var = gw->fip.prod_varlist[i];
        struct gw_var_cfg *gw_var_cfg = \
            (struct gw_var_cfg *) var->user_ctx;

        memset(path_str, 0, sizeof(path_str));
        sprintf(path_str, "fip.var.prod.%04x", var->id);

        UA_ObjectAttributes v_attr = UA_ObjectAttributes_default;
        v_attr.displayName = UA_LOCALIZEDTEXT("en-US", gw_var_cfg->name);
        UA_Server_addObjectNode(gw->opc.srv,
            UA_NODEID_STRING(1, path_str),
            UA_NODEID_STRING(1, "fip.var.prod_folder"),
            UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
            UA_QUALIFIEDNAME(1, path_str),
            UA_NODEID_STRING(1, "fip.otype.var"),
            v_attr,
            (void *) gw->fip.prod_varlist[i],
            NULL);
    }
}

static void opc_pubsub_co_add(struct gateway *gw)
{
    /**
     * Add Publisher Connection
     */

#ifdef UA_ENABLE_PUBSUB_ETH_UADP
    /* accept eth transport layer for publisher */
    UA_ServerConfig_addPubSubTransportLayer(gw->opc.cfg,
        UA_PubSubTransportLayerEthernet());
#else
    /* accept udp transport layer for publisher */
    UA_ServerConfig_addPubSubTransportLayer(gw->opc.cfg,
        UA_PubSubTransportLayerUDPMP());
#endif

    UA_PubSubConnectionConfig pub_co_cfg;
    memset(&pub_co_cfg, 0, sizeof(pub_co_cfg));
    /* connection name */
    pub_co_cfg.name = UA_STRING(OPC_PUB_NAME);
    /* transport profile */
    pub_co_cfg.transportProfileUri = UA_STRING(OPC_TRANSPORT_PROFILE);
    pub_co_cfg.enabled = UA_TRUE;
    /* network address url -
     * ex: opc.udp://224.0.0.22:4840 */
    gw->opc.pub.url.networkInterface = UA_STRING(gw->opc.pub.net_str);
    gw->opc.pub.url.url = UA_STRING(gw->opc.pub.url_str);
    UA_Variant_setScalar(&pub_co_cfg.address, &gw->opc.pub.url,
        &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
    /* publisher identifier - ex: 1234 */
    pub_co_cfg.publisherId.numeric = gw->opc.pub.publisher_id;

#ifdef UA_ENABLE_PUBSUB_ETH_UADP
#   ifdef OPC_ENABLE_REALTIME_CAP
    /* connection options are given as key/value pairs */
    UA_KeyValuePair co_opts[2];
    UA_Int32 socket_prio = OPC_RT_SOCKET_PRIORITY;
    UA_Boolean so_tx_time_cap = UA_TRUE;
    /* ETF configuration settings */
    co_opts[0].key = UA_QUALIFIEDNAME(0, "sockpriority");
    UA_Variant_setScalar(&co_opts[0].value, &socket_prio,
        &UA_TYPES[UA_TYPES_UINT32]);
    co_opts[1].key = UA_QUALIFIEDNAME(0, "enablesotxtime");
    UA_Variant_setScalar(&co_opts[1].value, &so_tx_time_cap,
        &UA_TYPES[UA_TYPES_BOOLEAN]);
    pub_co_cfg.connectionProperties = co_opts;
    pub_co_cfg.connectionPropertiesSize = 2;
#   endif
#endif
    /* add connection */
    UA_Server_addPubSubConnection(gw->opc.srv, &pub_co_cfg,
        &gw->opc.pub.co_ident);

    /**
     * Add Subscriber Connection
     */

#ifdef UA_ENABLE_PUBSUB_ETH_UADP
    /* accept eth transport layer for publisher */
    UA_ServerConfig_addPubSubTransportLayer(gw->opc.cfg,
        UA_PubSubTransportLayerEthernet());
#else
    /* accept udp transport layer for subscriber */
    UA_ServerConfig_addPubSubTransportLayer(gw->opc.cfg,
        UA_PubSubTransportLayerUDPMP());
#endif

    UA_PubSubConnectionConfig sub_co_cfg;
    memset(&sub_co_cfg, 0, sizeof(sub_co_cfg));
    /* connection name */
    sub_co_cfg.name = UA_STRING(OPC_SUB_NAME);

#ifdef UA_ENABLE_PUBSUB_ETH_UADP
#   ifdef OPC_ENABLE_REALTIME_CAP
#      ifdef OPC_RX_XDP_ENABLE
    /* connection options are given as key/value pairs */
    UA_KeyValuePair co_opt[4];
    UA_Boolean xdp_cap = UA_TRUE;
    UA_UInt32 flags = XDP_FLAGS_SKB_MODE;
    UA_UInt32 rx_queue = 2;
    UA_UInt32 bind_flags = XDP_COPY;
    /* XDP configuration settings */
    co_opt[0].key = UA_QUALIFIEDNAME(0, "enableXdpSocket");
    UA_Variant_setScalar(&co_opt[0].value, &xdp_cap, &UA_TYPES[UA_TYPES_BOOLEAN]);
    co_opt[1].key = UA_QUALIFIEDNAME(0, "xdpflag");
    UA_Variant_setScalar(&co_opt[1].value, &flags, &UA_TYPES[UA_TYPES_UINT32]);
    co_opt[2].key = UA_QUALIFIEDNAME(0, "hwreceivequeue");
    UA_Variant_setScalar(&co_opt[2].value, &rx_queue, &UA_TYPES[UA_TYPES_UINT32]);
    co_opt[3].key = UA_QUALIFIEDNAME(0, "xdpbindflag");
    UA_Variant_setScalar(&co_opt[3].value, &bind_flags, &UA_TYPES[UA_TYPES_UINT16]);
    sub_co_cfg.connectionProperties = co_opt;
    sub_co_cfg.connectionPropertiesSize = 4;
#      endif
#   endif
#endif

    /* transport profile */
    sub_co_cfg.transportProfileUri = UA_STRING(OPC_TRANSPORT_PROFILE);
    sub_co_cfg.enabled = UA_TRUE;
    /* network address url -
     * ex: opc.udp://224.0.0.22:4840 */
    gw->opc.sub.url.networkInterface = UA_STRING(gw->opc.sub.net_str);
    gw->opc.sub.url.url = UA_STRING(gw->opc.sub.url_str);
    UA_Variant_setScalar(&sub_co_cfg.address, &gw->opc.sub.url,
        &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
    /* publisher identifier - ex: 1234 */
    sub_co_cfg.publisherId.numeric = gw->opc.sub.publisher_id;
    /* add connection */
    UA_Server_addPubSubConnection(gw->opc.srv, &sub_co_cfg,
        &gw->opc.sub.co_ident);
}

static void opc_pub_dataset_n_group_create(struct gateway *gw)
{
    UA_NodeId w_id;
    UA_NodeId w_ds_id;

    /* do not create dataset if no data to publish */
    if (gw->fip.cons_varcnt < 1)
        return;

    /* => create a new published dataset */
    UA_PublishedDataSetConfig pub_dataset_cfg;
    memset(&pub_dataset_cfg, 0, sizeof(pub_dataset_cfg));
    pub_dataset_cfg.publishedDataSetType = UA_PUBSUB_DATASET_PUBLISHEDITEMS;
    pub_dataset_cfg.name = UA_STRING("PubDataSet - Fip.Var.Cons");
    UA_Server_addPublishedDataSet(gw->opc.srv,
        &pub_dataset_cfg, &w_ds_id);

    /* => add all fip consumed variables to the publisher dataset */
    for (UA_Int32 i = 0; i < gw->fip.cons_varcnt; i++)
    {
        struct pwrfip_var *v = NULL;
        struct gw_var_cfg *v_cfg;
        char alias_str[255];

        v = gw->fip.cons_varlist[i];
        v_cfg = (struct gw_var_cfg *) v->user_ctx;
        memset(alias_str, 0, sizeof(alias_str));
        sprintf(alias_str, "fip.var.cons.%04x.data", v->id);

        UA_DataSetFieldConfig dsf_cfg;
        memset(&dsf_cfg, 0, sizeof(dsf_cfg));
#if defined OPC_ENABLE_REALTIME_CAP
        /* note: for realtime cap, the node_id in publishParameters must
         * point to a node with external data source backend defined */
        dsf_cfg.field.variable.rtValueSource.rtInformationModelNode = UA_TRUE;
        //note: test[3]
        //dsf_cfg.field.variable.rtValueSource.rtFieldSourceEnabled = UA_TRUE;
        //dsf_cfg.field.variable.rtValueSource.staticValueSource = &v_cfg->rt_value;
#else
        dsf_cfg.dataSetFieldType = UA_PUBSUB_DATASETFIELD_VARIABLE;
        dsf_cfg.field.variable.fieldNameAlias = UA_STRING(alias_str);
        dsf_cfg.field.variable.promotedField = UA_FALSE;
        dsf_cfg.field.variable.publishParameters.attributeId = \
            UA_ATTRIBUTEID_VALUE;
#endif
        dsf_cfg.field.variable.publishParameters.publishedVariable = \
            v_cfg->v_node_id;
        UA_Server_addDataSetField(gw->opc.srv, w_ds_id, &dsf_cfg, NULL);
    }

    /* => create a new writer group */
    UA_UadpWriterGroupMessageDataType *w_group_msg = \
        UA_UadpWriterGroupMessageDataType_new();
    UA_WriterGroupConfig w_group_cfg;
    memset(&w_group_cfg, 0, sizeof(w_group_cfg));
    w_group_cfg.name = UA_STRING(OPC_WRITER_GROUP_NAME);
    w_group_cfg.publishingInterval = \
        ((UA_Double) gw->opc.pub.publish_period_us) / 1000;
    w_group_cfg.enabled = UA_FALSE;
    w_group_cfg.writerGroupId = gw->opc.pub.writer_group_id;
    w_group_cfg.encodingMimeType = UA_PUBSUB_ENCODING_UADP;
#if defined OPC_ENABLE_REALTIME_CAP
    w_group_cfg.rtLevel = UA_PUBSUB_RT_DIRECT_VALUE_ACCESS;
    w_group_cfg.pubsubManagerCallback.addCustomCallback = opc_rt_cb_add;
    w_group_cfg.pubsubManagerCallback.changeCustomCallback = opc_rt_cb_change;
    w_group_cfg.pubsubManagerCallback.removeCustomCallback = opc_rt_cb_remove;
#endif
    w_group_cfg.messageSettings.encoding = UA_EXTENSIONOBJECT_DECODED;
    w_group_cfg.messageSettings.content.decoded.type = \
        &UA_TYPES[UA_TYPES_UADPWRITERGROUPMESSAGEDATATYPE];
    w_group_cfg.messageSettings.content.decoded.data = w_group_msg;
    w_group_msg->networkMessageContentMask = \
        (UA_UadpNetworkMessageContentMask)
            (UA_UADPNETWORKMESSAGECONTENTMASK_PUBLISHERID |
        (UA_UadpNetworkMessageContentMask)
            UA_UADPNETWORKMESSAGECONTENTMASK_GROUPHEADER |
        (UA_UadpNetworkMessageContentMask)
            UA_UADPNETWORKMESSAGECONTENTMASK_WRITERGROUPID |
        (UA_UadpNetworkMessageContentMask)
            UA_UADPNETWORKMESSAGECONTENTMASK_PAYLOADHEADER);
    UA_Server_addWriterGroup(gw->opc.srv, gw->opc.pub.co_ident, &w_group_cfg,
        &gw->opc.pub.w_group_id);

    /* => add dataset to the writer group */
    UA_DataSetWriterConfig dataset_w_cfg;
    memset(&dataset_w_cfg, 0, sizeof(dataset_w_cfg));
    dataset_w_cfg.name = UA_STRING(OPC_DATASET_WRITER_NAME);
    dataset_w_cfg.dataSetWriterId = gw->opc.pub.dataset_writer_id;
    dataset_w_cfg.keyFrameCount = 10;
    UA_Server_addDataSetWriter(gw->opc.srv, gw->opc.pub.w_group_id,
        w_ds_id, &dataset_w_cfg, &w_id);

    /* => operational + freeze writer group */
    UA_Server_freezeWriterGroupConfiguration(gw->opc.srv, gw->opc.pub.w_group_id);
    UA_Server_setWriterGroupOperational(gw->opc.srv, gw->opc.pub.w_group_id);
    UA_UadpWriterGroupMessageDataType_delete(w_group_msg);
}

static void opc_sub_dataset_n_group_create(struct gateway *gw)
{
    UA_NodeId r_id;

    /* do not create dataset if no data to subscribe */
    if (gw->fip.prod_varcnt < 1)
        return;

    /* => create a new reader group */
    UA_ReaderGroupConfig r_group_cfg;
    memset(&r_group_cfg, 0, sizeof(r_group_cfg));
    r_group_cfg.name = UA_STRING(OPC_READER_GROUP_NAME);
    r_group_cfg.subscribingInterval = \
        ((UA_Double) gw->opc.pub.publish_period_us) / 1000; /* in ms */
#if defined OPC_RX_BLOCKING_SOCKET
    r_group_cfg.enableBlockingSocket = UA_TRUE; /* blocking socket */
    r_group_cfg.timeout = 0;
#else
    /* not a blocking socket mode, so we set the socket timeout to 1/4
     * of the suscribing interval */
    r_group_cfg.enableBlockingSocket = UA_FALSE;
    r_group_cfg.timeout = \
        ((UA_Double) gw->opc.pub.publish_period_us) / 5;
#endif
#if defined OPC_ENABLE_REALTIME_CAP
    r_group_cfg.rtLevel = UA_PUBSUB_RT_DIRECT_VALUE_ACCESS;
    r_group_cfg.pubsubManagerCallback.addCustomCallback = opc_rt_cb_add;
    r_group_cfg.pubsubManagerCallback.changeCustomCallback = opc_rt_cb_change;
    r_group_cfg.pubsubManagerCallback.removeCustomCallback = opc_rt_cb_remove;
#endif
    //r_group_cfg.securityParameters = ;
    UA_Server_addReaderGroup(gw->opc.srv, gw->opc.sub.co_ident, &r_group_cfg,
        &gw->opc.sub.r_group_id);

    /* => add dataset to the reader group */
    UA_UadpDataSetReaderMessageDataType *dataset_r_msg = \
        UA_UadpDataSetReaderMessageDataType_new();
    UA_DataSetReaderConfig dataset_r_cfg;
    UA_UInt16 pub_id = gw->opc.sub.publisher_id;
    memset(&dataset_r_cfg, 0, sizeof(dataset_r_cfg));
    dataset_r_cfg.name = UA_STRING(OPC_DATASET_READER_NAME);
    dataset_r_cfg.publisherId.type = &UA_TYPES[UA_TYPES_UINT16];
    dataset_r_cfg.publisherId.data = &pub_id;
    dataset_r_cfg.writerGroupId    = gw->opc.sub.writer_group_id;
    dataset_r_cfg.dataSetWriterId  = gw->opc.sub.dataset_writer_id;
    dataset_r_cfg.messageSettings.encoding = UA_EXTENSIONOBJECT_DECODED;
    dataset_r_cfg.messageSettings.content.decoded.type = \
        &UA_TYPES[UA_TYPES_UADPDATASETREADERMESSAGEDATATYPE];
    dataset_r_cfg.messageSettings.content.decoded.data = dataset_r_msg;
    dataset_r_msg->networkMessageContentMask = \
        (UA_UadpNetworkMessageContentMask)
            (UA_UADPNETWORKMESSAGECONTENTMASK_PUBLISHERID |
        (UA_UadpNetworkMessageContentMask)
            UA_UADPNETWORKMESSAGECONTENTMASK_GROUPHEADER |
        (UA_UadpNetworkMessageContentMask)
            UA_UADPNETWORKMESSAGECONTENTMASK_WRITERGROUPID |
        (UA_UadpNetworkMessageContentMask)
            UA_UADPNETWORKMESSAGECONTENTMASK_PAYLOADHEADER);

    /* note: here, we setting up meta data configuration for dataset reader */
    UA_DataSetMetaDataType *dataset_mdata = &dataset_r_cfg.dataSetMetaData;
    UA_DataSetMetaDataType_init(dataset_mdata);
    dataset_mdata->name = UA_STRING("DataSet 1");
    dataset_mdata->fieldsSize = gw->fip.prod_varcnt;
    dataset_mdata->fields = \
        (UA_FieldMetaData*) UA_Array_new(dataset_mdata->fieldsSize,
            &UA_TYPES[UA_TYPES_FIELDMETADATA]);
    UA_FieldTargetVariable *tgt_vars = (UA_FieldTargetVariable *)
        UA_calloc(dataset_mdata->fieldsSize, sizeof(UA_FieldTargetVariable));

    /* => add all fip produced variables to the subscriber dataset */
    for (UA_Int32 i = 0; i < gw->fip.prod_varcnt; i++)
    {
        struct pwrfip_var *v = NULL;
        struct gw_var_cfg *v_cfg;
        char alias_str[255];

        v = gw->fip.prod_varlist[i];
        v_cfg = (struct gw_var_cfg *) v->user_ctx;
        memset(alias_str, 0, sizeof(alias_str));
        sprintf(alias_str, "fip.var.prod.%04x.data", v->id);

        /* => meta data configuration for dataset reader */
        UA_FieldMetaData_init(&dataset_mdata->fields[i]);
        UA_NodeId_copy(&UA_TYPES[UA_TYPES_BYTESTRING].typeId,
            &dataset_mdata->fields[i].dataType);
        dataset_mdata->fields[i].builtInType = UA_NS0ID_BYTESTRING;
        dataset_mdata->fields[i].name = UA_STRING(alias_str);
        dataset_mdata->fields[i].valueRank = UA_VALUERANK_SCALAR;
        /* => create target variable to attach to the subscriber */
        UA_FieldTargetDataType_init(&tgt_vars[i].targetVariable);
        tgt_vars[i].targetVariable.attributeId  = UA_ATTRIBUTEID_VALUE;
        tgt_vars[i].targetVariable.targetNodeId = v_cfg->v_node_id;
    }

    /* finally, here we add dataset to the reader group */
    UA_Server_addDataSetReader(gw->opc.srv, gw->opc.sub.r_group_id,
        &dataset_r_cfg, &r_id);

    /* => add all fip produced variables to the subscriber dataset */
    UA_Server_DataSetReader_createTargetVariables(gw->opc.srv, r_id,
        dataset_mdata->fieldsSize, tgt_vars);

    /* => operational + freeze reader group */
    UA_Server_freezeReaderGroupConfiguration(gw->opc.srv, gw->opc.sub.r_group_id);
    UA_Server_setReaderGroupOperational(gw->opc.srv, gw->opc.sub.r_group_id);

    /* free temporary objects */
    for (UA_Int32 i = 0; i < dataset_mdata->fieldsSize; ++i)
        UA_FieldTargetDataType_clear(&tgt_vars[i].targetVariable);
    UA_free(tgt_vars);
    UA_free(dataset_mdata->fields);
    UA_UadpDataSetReaderMessageDataType_delete(dataset_r_msg);
}

UA_StatusCode opc_value_trywrite(UA_Server *srv,
    UA_QualifiedName target_name, const UA_Variant value,
    const UA_NodeId *node_id_start, UA_NodeId *node_id_found)
{
    /* Find the NodeId of the status child variable */
    UA_RelativePathElement rpe;
    UA_RelativePathElement_init(&rpe);
    rpe.referenceTypeId = UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT);
    rpe.isInverse = false;
    rpe.includeSubtypes = false;
    rpe.targetName = target_name;

    UA_BrowsePath bp;
    UA_BrowsePath_init(&bp);
    bp.startingNode = *node_id_start;
    bp.relativePath.elementsSize = 1;
    bp.relativePath.elements = &rpe;

    UA_BrowsePathResult bpr = UA_Server_translateBrowsePathToNodeIds(srv, &bp);
    if (bpr.statusCode != UA_STATUSCODE_GOOD || bpr.targetsSize < 1)
        return bpr.statusCode;

    UA_Server_writeValue(srv, bpr.targets[0].targetId.nodeId, value);

    if (node_id_found != NULL) {
        UA_NodeId_init(node_id_found);
        UA_NodeId_copy(&bpr.targets[0].targetId.nodeId, node_id_found);
    }

    UA_BrowsePathResult_clear(&bpr);
    return UA_STATUSCODE_GOOD;

}

static void opc_cleanup_task(void *arg)
{
    struct gateway *gw = (struct gateway *) arg;

#if defined OPC_ENABLE_REALTIME_CAP
    for (int i = 0; i < gw->cfg_varcnt; ++i)
    {
        struct gw_var_cfg *v_cfg = &gw->cfg_varlist[i];

        /* clear rt_data */
        //UA_ByteString_clear(&v_cfg->rt_data);

        /* clear rt_value */
        if (v_cfg->rt_value) {
            UA_free(v_cfg->rt_value);
            v_cfg->rt_value = NULL;
        }
    }
#endif

    if (gw->opc.srv)
        UA_Server_delete(gw->opc.srv);

    if (gw->opc.dt_members)
        UA_free(gw->opc.dt_members);

    if (gw->opc.dt_types)
        UA_free(gw->opc.dt_types);

    gw->opc.srv = NULL;
    gw->opc.dt_members = NULL;
    gw->opc.dt_types = NULL;
}

static void *opc_task(void *arg)
{
    struct gateway *gw = (struct gateway *) arg;

    pthread_cleanup_push(opc_cleanup_task, (void *) gw);

    /* opcua server creation */
    gw->opc.srv = UA_Server_new();
    gw->opc.cfg = UA_Server_getConfig(gw->opc.srv);
    gw->opc.cfg->logger.log = opc_log;
    UA_ServerConfig_setDefault(gw->opc.cfg);

    /* make fip custom datatype known to the stack */
    gw->opc.dt_types = (UA_DataType *) UA_malloc(1 * sizeof(UA_DataType));
    gw->opc.dt_members = (UA_DataTypeMember* ) \
        UA_malloc(OPC_FIPSTATS_DT_MEMBERS_MAX * sizeof(UA_DataTypeMember));
    for (UA_Int32 i = 0; i < OPC_FIPSTATS_DT_MEMBERS_MAX; ++i)
        gw->opc.dt_members[i] = FipStatisticsSummary_members[i];

    gw->opc.dt_types[0] = FipStatisticsSummaryType;
    gw->opc.dt_types[0].members = gw->opc.dt_members;

    UA_DataTypeArray custom_data_types = \
        {gw->opc.cfg->customDataTypes, 1, gw->opc.dt_types};
    gw->opc.cfg->customDataTypes = &custom_data_types;

    opc_fip_types_init(gw->opc.srv);
    opc_fip_objects_create(gw);

    gw->opc.running = true;
    opc_pubsub_co_add(gw);
    /* => output */
    opc_pub_dataset_n_group_create(gw);
    /* => input */
    opc_sub_dataset_n_group_create(gw);

#if defined OPC_ENABLE_REALTIME_CAP
    opc_rt_app_start(gw);
#endif

    _print(opc, info, "[ok] publisher  start (%s) \n", gw->opc.pub.url_str);
    _print(opc, info, "[ok] subscriber start (%s) \n", gw->opc.sub.url_str);
    gw->opc.status = UA_Server_run(gw->opc.srv, &gw->opc.running);

#if defined OPC_ENABLE_REALTIME_CAP
    opc_rt_app_stop(gw);
#endif

    UA_Server_unfreezeReaderGroupConfiguration(gw->opc.srv,
        gw->opc.sub.r_group_id);

    pthread_cleanup_pop(1);
    return NULL;
}
