#include <stdlib.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <signal.h>

#include "cs104_slave.h"

#include "hal_thread.h"
#include "hal_time.h"
#include "iec104_server.h"

static bool running;
#define IEC104_PERIODIC_INTERVAL 2

static void sigint_handler(int sino)
{
    running = false;
    fprintf(stderr, "received signal: %d\n", sino);
    fflush(stderr);
}

static const char * iec104_systime(char * tbuf, size_t tlen)
{
    int ret;
    time_t nowt;
    struct timespec spec;
    struct tm tim, * ptm;

    spec.tv_sec = 0;
    spec.tv_nsec = 0;
    ret = clock_gettime(CLOCK_REALTIME, &spec);
    if (ret == -1) {
        tbuf[0] = '\0';
        return tbuf;
    }

    nowt = spec.tv_sec;
    ptm = localtime_r(&nowt, &tim);

    if (ptm == NULL) {
        ret = snprintf(tbuf, tlen, "%d.%06d",
            (int) spec.tv_sec, (int) (spec.tv_nsec / 1000));
    } else {
        ret = snprintf(tbuf, tlen, "%d-%02d-%02d %02d:%02d:%02d.%06d",
            ptm->tm_year + 1900, ptm->tm_mon + 1, ptm->tm_mday,
            ptm->tm_hour, ptm->tm_min, ptm->tm_sec,
            (int) (spec.tv_nsec / 1000));
    }
    if (ret <= 0) {
        tbuf[0] = '\0';
        return tbuf;
    } else if (ret >= (int) tlen)
        ret = (int) (tlen - 1);
    tbuf[ret] = '\0';
    return tbuf;
}

static void printCP56Time2a(CP56Time2a time)
{
    char nowt[64];
    printf("[%s]: %02i:%02i:%02i %02i/%02i/%04i",
        iec104_systime(nowt, sizeof(nowt)),
        CP56Time2a_getHour(time),
        CP56Time2a_getMinute(time),
        CP56Time2a_getSecond(time),
        CP56Time2a_getDayOfMonth(time),
        CP56Time2a_getMonth(time),
        CP56Time2a_getYear(time) + 2000);
}

#if 0
/* Callback handler to log sent or received messages (optional) */
static void rawMessageHandler(void* parameter,
    IMasterConnection conneciton, uint8_t* msg, int msgSize, bool sent)
{
    if (sent)
        printf("SEND: ");
    else
        printf("RCVD: ");

    int i;
    for (i = 0; i < msgSize; i++) {
        printf("%02x ", msg[i]);
    }

    printf("\n");
}
#endif

static bool clockSyncHandler(void* parameter,
    IMasterConnection connection, CS101_ASDU asdu, CP56Time2a newTime)
{
    char nowt[64];
    printf("[%s]: Process time sync command with time ",
        iec104_systime(nowt, sizeof(nowt)));
    printCP56Time2a(newTime);
    printf("\n");

    /* uint64_t newSystemTimeInMs = CP56Time2a_toMsTimestamp(newTime); */

    /* Set time for ACT_CON message */
    CP56Time2a_setFromMsTimestamp(newTime, Hal_getTimeInMs());

    /* update system time here */

    return true;
}

typedef InformationObject (* infobj_create)(InformationObject iobj,
    int ioa, union iec104_data pd, QualityDescriptor qd);

static InformationObject infobj_float_create(InformationObject iobj,
    int ioa, union iec104_data pd, QualityDescriptor qd)
{
    return (InformationObject) MeasuredValueShort_create(
        (MeasuredValueShort) iobj, ioa, pd.val_float, qd);
}

static InformationObject infobj_int16_create(InformationObject iobj,
    int ioa, union iec104_data pd, QualityDescriptor qd)
{
    return (InformationObject) MeasuredValueScaled_create(
        (MeasuredValueScaled) iobj, ioa, pd.val_int32, qd);
}

static InformationObject infobj_int32_create(InformationObject iobj,
    int ioa, union iec104_data pd, QualityDescriptor qd)
{
    return (InformationObject) MeasuredValueShort_create(
        (MeasuredValueShort) iobj, ioa, (float) pd.val_int32, qd);
}

static InformationObject infobj_uint32_create(InformationObject iobj,
    int ioa, union iec104_data pd, QualityDescriptor qd)
{
    return (InformationObject) MeasuredValueShort_create(
        (MeasuredValueShort) iobj, ioa, (float) pd.val_uint32, qd);
}

static infobj_create infobj_create_get(tag_data_format_e datfmt)
{
    if (datfmt == TAG_TYPE_FLOAT ||
        datfmt == TAG_TYPE_FLOATI)
        return infobj_float_create;

    if (datfmt == TAG_TYPE_INT8 ||
        datfmt == TAG_TYPE_UINT8 ||
        datfmt == TAG_TYPE_INT16 ||
        datfmt == TAG_TYPE_INT16I)
        return infobj_int16_create;

    if (datfmt == TAG_TYPE_INT32)
        return infobj_int32_create;

    return infobj_uint32_create;
}

static bool interrogationHandler(void* parameter,
    IMasterConnection connection, CS101_ASDU asdu, uint8_t qoi)
{
    char nowt[64];
    int oaddr, oa_offset;
    tag_data_format_e datfmt;
    struct iec104_var * ivar;
    struct iec104_points * pts;
    struct iec104_point * pt;
    unsigned int count, curcnt;
    CS101_AppLayerParameters alParams;
    infobj_create infobj_create_p;

    count = 0;
    pts = NULL;
    curcnt = 0;
    alParams = NULL;
    ivar = (struct iec104_var *) parameter;

    oaddr = CS101_ASDU_getOA(asdu);
    printf("[%s]: Received interrogation for group %i, OA: %d\n",
        iec104_systime(nowt, sizeof(nowt)), qoi, oaddr);

    if (qoi != 20) { /* only handle station interrogation */
err0:
        IMasterConnection_sendACT_CON(connection, asdu, true);
        return true;
    }

    alParams = IMasterConnection_getApplicationLayerParameters(connection);
    if (alParams == NULL)
        goto err0;
    if (ivar->numgroup <= (unsigned int) oaddr || ivar->iec104_group == NULL)
        goto err0;
    pts = ivar->iec104_group[oaddr];
    if (pts != NULL)
        count = pts->numpoints;
    if (pts == NULL || count == 0)
        goto err0;
    pt = &pts->points[0];
    oa_offset = pts->iec104_offset;

    datfmt = pt->prop->raw_data_type;
    /* determine data format, integer or float */
    infobj_create_p = infobj_create_get(datfmt);

    IMasterConnection_sendACT_CON(connection, asdu, false);
    while (curcnt < count) {
        int added = 0;
        union iec104_data idat;
        InformationObject io = NULL;
        CS101_ASDU newAsdu = CS101_ASDU_create(alParams, false,
            CS101_COT_INTERROGATED_BY_STATION, oaddr, 1, false, false);

        idat.val_uint32 = 0;
        io = infobj_create_p(NULL, 0, idat, IEC60870_QUALITY_GOOD);
        while (curcnt < count) {
            const object_property_cfg_t * prop;
            prop = pt->prop;
            if (prop == NULL || prop->raw_data_type != datfmt) {
                pt++;
                curcnt++;
#if 0
                fprintf(stderr, "skipped property: %s, type: %d\n",
                    prop ? prop->identifier : "nil", prop ? (int) prop->raw_data_type : 0);
                fflush(stderr);
#endif
                continue;
            }
            InformationObject newio = infobj_create_p(io,
                curcnt + oa_offset, pt->pointd, IEC60870_QUALITY_GOOD);
            bool rval = CS101_ASDU_addInformationObject(newAsdu, newio);
            if (!rval)
                break;
            pt++;
            added++;
            curcnt++;
        }

        InformationObject_destroy(io);
        if (added > 0)
            IMasterConnection_sendASDU(connection, newAsdu);
        CS101_ASDU_destroy(newAsdu);
    }

    IMasterConnection_sendACT_TERM(connection, asdu);
    return true;
}

static bool counterinterrogationhandler(void * parameter,
    IMasterConnection connection, CS101_ASDU asdu, QualifierOfCIC qcc)
{
    char nowt[64];
    IEC60870_5_TypeID typeid;

    iec104_systime(nowt, sizeof(nowt));
    typeid = CS101_ASDU_getTypeID(asdu);
    fprintf(stdout, "[%s]: received counter interrogation, type: %d\n",
        nowt, (int) typeid);
    fflush(stdout);

    return true;
}

static bool asduHandler(void* parameter,
    IMasterConnection connection, CS101_ASDU asdu)
{
    char nowt[64];
    IEC60870_5_TypeID typeid;
    typeid = CS101_ASDU_getTypeID(asdu);

    iec104_systime(nowt, sizeof(nowt)),
    fprintf(stdout, "[%s]: Received an message, type: %d (%#x)\n",
        nowt, (int) typeid, (unsigned int) typeid);
    if (typeid == C_SC_NA_1) {
        printf("received single command\n");

        if  (CS101_ASDU_getCOT(asdu) == CS101_COT_ACTIVATION) {
            InformationObject io = CS101_ASDU_getElement(asdu, 0);

            if (io) {
                if (InformationObject_getObjectAddress(io) == 5000) {
                    SingleCommand sc = (SingleCommand) io;

                    printf("IOA: %i switch to %i\n", InformationObject_getObjectAddress(io),
                            SingleCommand_getState(sc));

                    CS101_ASDU_setCOT(asdu, CS101_COT_ACTIVATION_CON);
                }
                else
                    CS101_ASDU_setCOT(asdu, CS101_COT_UNKNOWN_IOA);

                InformationObject_destroy(io);
            }
            else {
                printf("ERROR: message has no valid information object\n");
                return true;
            }
        }
        else
            CS101_ASDU_setCOT(asdu, CS101_COT_UNKNOWN_COT);

        IMasterConnection_sendASDU(connection, asdu);

        return true;
    }
    fflush(stdout);

    return false;
}

static bool connectionRequestHandler(void* parameter, const char* ipAddress)
{
    char nowt[64];
    fprintf(stdout, "[%s]: New connection request from %s\n",
        iec104_systime(nowt, sizeof(nowt)), ipAddress);
    fflush(stdout);
    return true;
}

static void connectionEventHandler(void* parameter,
    IMasterConnection con, CS104_PeerConnectionEvent event)
{
    char nowt[64];
    iec104_systime(nowt, sizeof(nowt));
    if (event == CS104_CON_EVENT_CONNECTION_OPENED) {
        printf("[%s]: Connection opened (%p)\n", nowt, con);
    }
    else if (event == CS104_CON_EVENT_CONNECTION_CLOSED) {
        printf("[%s]: Connection closed (%p)\n", nowt, con);
    }
    else if (event == CS104_CON_EVENT_ACTIVATED) {
        printf("[%s]: Connection activated (%p)\n", nowt, con);
    }
    else if (event == CS104_CON_EVENT_DEACTIVATED) {
        printf("[%s]: Connection deactivated (%p)\n", nowt, con);
    } else {
        printf("[%s]: unknown event: %d (%#x)\n", nowt, (int) event, (unsigned int) event);
    }
    fflush(stdout);
}

static void iec104_report_modified(int oaddr,
    struct iec104_points * pts, CS104_Slave slave)
{
    CS101_ASDU newAsdu;
    InformationObject pio;
    int count, idx, maxnum;
    union iec104_data idat;
    struct iec104_point * pt;
    tag_data_format_e datfmt;
    infobj_create infobj_create_p;
    CS101_AppLayerParameters alParams;

    count = 0;
    pio = NULL;
    newAsdu = NULL;
    pt = pts->points;
    datfmt = pt->prop->raw_data_type;
    maxnum = (unsigned int) pts->numpoints;
    infobj_create_p = infobj_create_get(datfmt);
    alParams = CS104_Slave_getAppLayerParameters(slave);

    idat.val_uint32 = 0;
    pio = infobj_create_p(NULL, 0, idat, IEC60870_QUALITY_GOOD);

    for (idx = 0; idx < maxnum; ++idx) {
        bool rval;
        InformationObject newio;
        unsigned int changed;
        changed = __atomic_exchange_n(&pt->changed,
            0u, __ATOMIC_SEQ_CST);
        if (changed == 0) {
            pt++;
            continue;
        }
        if (datfmt != pt->prop->raw_data_type) {
            pt++;
            continue;
        }

        if (newAsdu == NULL) {
            newAsdu = CS101_ASDU_create(alParams, false,
                CS101_COT_SPONTANEOUS, oaddr, 0, false, false);
        }
        newio = infobj_create_p(pio, idx + pts->iec104_offset,
            pt->pointd, IEC60870_QUALITY_GOOD);
        rval = CS101_ASDU_addInformationObject(newAsdu, newio);
        if (rval) {
            count++;
            pt++;
        } else {
            if (count > 0) {
                CS104_Slave_enqueueASDU(slave, newAsdu);
                CS101_ASDU_destroy(newAsdu);
                newAsdu = NULL;
            }
            count = 0;
            idx--; /* retry for current point value */
            __atomic_store_n(&pt->changed, changed, __ATOMIC_SEQ_CST);
        }
    }

    InformationObject_destroy(pio);
    if (newAsdu != NULL) {
        CS104_Slave_enqueueASDU(slave, newAsdu);
        CS101_ASDU_destroy(newAsdu);
    }
}

/*
 * report modified data, the function is called with mutex held
 */
static void iec104_check_modified(struct iec104_var * ivar, CS104_Slave slave)
{
    char nowt[64];
    unsigned int idx, ngrp;

    ngrp = ivar->numgroup;
    for (idx = 0; idx < ngrp; ++idx) {
        uint64_t changed = 0;
        struct iec104_points * points;

        points = ivar->iec104_group[idx];
        if (points == NULL)
            continue;
        __atomic_load(&points->changed_time, &changed, __ATOMIC_SEQ_CST);
        if (points->updated_time >= changed)
            continue;
        points->updated_time = changed;
        if (points->report_change <= 0)
            continue;

        iec104_systime(nowt, sizeof(nowt));
        fprintf(stdout, "[%s] data at group %u has changed\n",
            nowt, idx);
        fflush(stdout);
        iec104_report_modified((int) idx, points, slave);
    }
}

static void iec104_report_periodic(int oaddr,
    struct iec104_points * pts, CS104_Slave slave)
{
    CS101_ASDU newAsdu;
    InformationObject pio;
    int count, idx, maxnum;
    union iec104_data idat;
    struct iec104_point * pt;
    tag_data_format_e datfmt;
    infobj_create infobj_create_p;
    CS101_AppLayerParameters alParams;

    count = 0;
    pio = NULL;
    newAsdu = NULL;
    pt = pts->points;
    datfmt = pt->prop->raw_data_type;
    maxnum = (unsigned int) pts->numpoints;
    infobj_create_p = infobj_create_get(datfmt);
    alParams = CS104_Slave_getAppLayerParameters(slave);

    idat.val_uint32 = 0;
    pio = infobj_create_p(NULL, 0, idat, IEC60870_QUALITY_GOOD);

    for (idx = 0; idx < maxnum; ++idx) {
        bool rval;
        InformationObject newio;
        if (datfmt != pt->prop->raw_data_type) {
            pt++;
            continue;
        }

        if (newAsdu == NULL) {
            newAsdu = CS101_ASDU_create(alParams, false,
                CS101_COT_PERIODIC, oaddr, 0, false, false);
        }
        newio = infobj_create_p(pio, idx + pts->iec104_offset,
            pt->pointd, IEC60870_QUALITY_GOOD);
        rval = CS101_ASDU_addInformationObject(newAsdu, newio);
        if (rval) {
            count++;
            pt++;
        } else {
            if (count > 0) {
                CS104_Slave_enqueueASDU(slave, newAsdu);
                CS101_ASDU_destroy(newAsdu);
                newAsdu = NULL;
            }
            count = 0;
            idx--; /* retry for current point value */
        }
    }

    InformationObject_destroy(pio);
    if (newAsdu != NULL) {
        CS104_Slave_enqueueASDU(slave, newAsdu);
        CS101_ASDU_destroy(newAsdu);
    }
}
/*
 * periodic IEC-104 data report
 */
static void iec104_check_periodic(struct iec104_var * ivar, CS104_Slave slave)
{
    char nowt[64];
    unsigned int idx, ngrp;

    ngrp = ivar->numgroup;
    for (idx = 0; idx < ngrp; ++idx) {
        int nowtim, period;
        struct iec104_points * points;

        points = ivar->iec104_group[idx];
        if (points == NULL)
            continue;
        period = points->report_period;
        if (period <= 0)
            continue;

        nowtim = (int) iec104_uptime(NULL, NULL);
        if (nowtim < (period + points->last_period))
            continue;
        points->last_period = nowtim;

        iec104_systime(nowt, sizeof(nowt));
        fprintf(stdout, "[%s]: periodic report of IEC-104 group %u\n",
            nowt, idx);
        fflush(stdout);
        iec104_report_periodic((int) idx, points, slave);
    }
}

int main(int argc, char** argv)
{
    int ret;
    unsigned int lasttime;
    struct iec104_var * ivar;

    running = true;
    /* Add Ctrl-C handler */
    signal(SIGINT, sigint_handler);

    ivar = iec104_server_init();
    if (ivar == NULL)
        return 1;

    CS104_APCIParameters apciParams = CS104_Slave_getdefault_ConnectionParameters();
    apciParams->k = 320;
    /* create a new slave/server instance with default connection parameters and
     * default message queue size */
    CS104_Slave slave = CS104_Slave_create(100, 100);

    CS104_Slave_setLocalAddress(slave, "0.0.0.0");
    /* Set mode to a single redundancy group
     * NOTE: library has to be compiled with CONFIG_CS104_SUPPORT_SERVER_MODE_SINGLE_REDUNDANCY_GROUP enabled (=1)
     */
    CS104_Slave_setServerMode(slave, CS104_MODE_SINGLE_REDUNDANCY_GROUP);

    /* get the connection parameters - we need them to create correct ASDUs -
     * you can also modify the parameters here when default parameters are not to be used */
    /* CS101_AppLayerParameters alParams = CS104_Slave_getAppLayerParameters(slave); */

    /* when you have to tweak the APCI parameters (t0-t3, k, w) you can access them here */
    apciParams = CS104_Slave_getConnectionParameters(slave);

    printf("APCI parameters:\n");
    printf("  t0: %i\n", apciParams->t0);
    printf("  t1: %i\n", apciParams->t1);
    printf("  t2: %i\n", apciParams->t2);
    printf("  t3: %i\n", apciParams->t3);
    printf("  k: %i\n", apciParams->k);
    printf("  w: %i\n", apciParams->w);

    /* set the callback handler for the clock synchronization command */
    CS104_Slave_setClockSyncHandler(slave, clockSyncHandler, NULL);

    /* set the callback handler for the interrogation command */
    CS104_Slave_setInterrogationHandler(slave, interrogationHandler, ivar);

    /* set the callback handler for the counter interrogation command */
    CS104_Slave_setCounterInterrogationHandler(slave, counterinterrogationhandler, ivar);

    /* set handler for other message types */
    CS104_Slave_setASDUHandler(slave, asduHandler, ivar);

    /* set handler to handle connection requests (optional) */
    CS104_Slave_setConnectionRequestHandler(slave, connectionRequestHandler, NULL);

    /* set handler to track connection events (optional) */
    CS104_Slave_setConnectionEventHandler(slave, connectionEventHandler, NULL);

    /* uncomment to log messages */
    /* CS104_Slave_setRawMessageHandler(slave, rawMessageHandler, NULL); */

    CS104_Slave_start(slave);
    if (!CS104_Slave_isRunning(slave)) {
        printf("Starting server failed!\n");
        goto exit_program;
    }

    /* acquire mutex lock */
    ret = pthread_mutex_lock(&ivar->thread_lock);
    if (ret != 0) {
        fprintf(stderr, "Error, failed to acquire mutex: %d\n", ret);
        fflush(stderr);
        goto exit1_program;
    }

    lasttime = iec104_uptime(NULL, NULL);
    /* main loop */
    while (running) {
        unsigned int nowt;

        nowt = iec104_uptime(NULL, NULL);
        if (nowt >= lasttime) {
            iec104_check_periodic(ivar, slave);
            lasttime += IEC104_PERIODIC_INTERVAL;
            if (nowt >= lasttime)
                lasttime = nowt + IEC104_PERIODIC_INTERVAL;
        }
        ret = lnxall_condvar_timedwait(&ivar->thread_cond,
            &ivar->thread_lock, IEC104_PERIODIC_INTERVAL * 1000);
        if (ret == ETIMEDOUT)
            continue;
        if (ret != 0) {
            fprintf(stderr, "Error, cond_timedwait has failed: %d\n", ret);
            fflush(stderr);
            break;
        }
        iec104_check_modified(ivar, slave);
    }

    ret = pthread_mutex_unlock(&ivar->thread_lock);
    if (ret != 0) {
        fprintf(stderr, "Error, failed to release mutex: %d\n", ret);
        fflush(stderr);
    }
exit1_program:
    CS104_Slave_stop(slave);

exit_program:
    CS104_Slave_destroy(slave);
    iec104_server_free(ivar);
    Thread_sleep(500);
    return 2;
}