// RPi1Server_61850 (koncentrator)

/* 
 * File:   main.cpp
 * Author: volodia
 * Edited: Jan Hudec
 *
 * Created on March 16, 2018, 11:29 PM
 * Edited on May 3, 2019, 2:13 AM
 */

// generic libraries
#include <cstdlib>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string.h>
#include <math.h>
#include <signal.h>

// iec dependent libraries
#include <hal_thread.h>
#include <iec61850_server.h>
#include <goose_publisher.h>
#include <goose_receiver.h>
#include <sv_subscriber.h>
//#include <goose_subscriber.h>
//#include <mms_value.h>

/* generated model */
#include "static_model.h"

using std::cout;
using std::endl;
using std::cin;

/* import IEC 61850 device model created from SCL-File */
extern IedModel iedModel;

static uint16_t sv_appId = 0x4000;
static int running = 0;
static IedServer iedServer = NULL;

uint32_t gs_sub_0_counter[2] = { 0 }; // [0] = stNum, [1] = sqNum
uint32_t gs_sub_1_counter[2] = { 0 }; // [0] = stNum, [1] = sqNum
uint32_t sv_sub_0_counter = 0;
uint32_t sv_sub_1_counter = 0;
uint32_t sv_sub_asdu_counter = 0;
uint64_t time_received_0 = 0;
uint64_t time_received_1 = 0;

int iterations = 3;

void sigint_handler(int signalId)
{
    running = 0;
}

static char intToChar(int i)
{
    char c;
    c = i + 48;
    return c;
}

static int charToInt(char c)
{
    int b = 0;
    b = (int)c - 48;
    return b;
}

static char* writeToFile(char* gpio_file, char* data)
{
    std::ofstream outfile(gpio_file,std::ofstream::binary);

    // get size of file
    long size = strlen(data);
    // allocate memory for file content
    char* buffer = new char[size];
    buffer = data;
    // write to outfile
    outfile.write(buffer, size);

    delete[] buffer;

    outfile.close();
    return 0;  
}

static char* readfile(char* gpio_file)
{
    std::ifstream is(gpio_file, std::ifstream::binary);
    if (is)
    {
        // get length of file:
        is.seekg(0, is.end);
        int length = is.tellg();
        is.seekg(0, is.beg);

        // allocate memory:
        char* bufferRead = new char[length];

        // read data as a block:
        is.read(bufferRead,length);

        is.close();

        return bufferRead;
    }
}

// callback function for sv listener
static void svUpdateListener(SVSubscriber subscriber, void* parameter, SVSubscriber_ASDU asdu)
{
    bool run = false;
    bool process_frame = false;
    uint32_t smpcnt;
    
    // assign values to compare against based on APPID of received frame
    if(*(int*)parameter == 0x4000)
    {
        smpcnt = sv_sub_0_counter;
        run = true;
    }
    else if(*(int*)parameter == 0x4001)
    {
        smpcnt = sv_sub_1_counter;
        run = true;
    }
    else
    {
        printf("\nInvalid SV frame with APPID 0x%04x - unrecognized APPID!\n", *(int*)parameter);
    }
    
    if(run == true)
    {
        // if smpCnt is identical to previous one and is not zero
        if(SVSubscriber_ASDU_getSmpCnt(asdu) == smpcnt && smpcnt != 0 && sv_sub_asdu_counter >= iterations)
        {
            printf("\nInvalid SV frame with APPID 0x%04x - duplicated frames!\n"
                   "  Allowed ASDU units: %i\tCurrent ASDU unit: %u\n", *(int*)parameter, iterations, sv_sub_asdu_counter + 1);
        }
        // if smpCnt is identical to previous one (multiple ASDUs in one SV frame)
        else if(SVSubscriber_ASDU_getSmpCnt(asdu) == smpcnt)
        {
            sv_sub_asdu_counter += 1;
            process_frame = true;
        }
        // if smpCnt is subsequent to previous one
        else if(SVSubscriber_ASDU_getSmpCnt(asdu) == smpcnt + 1)
        {
            sv_sub_asdu_counter = 1;
            smpcnt = SVSubscriber_ASDU_getSmpCnt(asdu);
            process_frame = true;
        }
        // values differ from saved values
        else
        {
            // first run on program - save current values
            if(smpcnt == 0 && sv_sub_asdu_counter == 0)
            {
                smpcnt = SVSubscriber_ASDU_getSmpCnt(asdu);
                sv_sub_asdu_counter = 0;
            }
            else
            {
                printf("\nWarning - SV frame values for APPID 0x%04x differ from saved values!\n"
                       "  Last accepted - smpCnt: %i\n"
                       "  Received      - smpCnt: %i\n",
                       *(int*)parameter, smpcnt, SVSubscriber_ASDU_getSmpCnt(asdu));
            }
        }
    }
    
    // process frame
    if(process_frame == true)
    {
        if(*(int*)parameter == 0x4000)
        {
            char* quality;
            switch(SVSubscriber_ASDU_getQuality(asdu, 5))
            {
                case 0:   quality = "good"; break;
                case 1:   quality = "reserved"; break;
                case 2:   quality = "invalid"; break;
                case 3:   quality = "questionable"; break;
                case 4:   quality = "overflow"; break;
                case 8:   quality = "out of range"; break;
                case 16:  quality = "bad reference"; break;
                case 32:  quality = "oscillatory"; break;
                case 64:  quality = "failure"; break;
                case 128: quality = "old data"; break;
                case 256: quality = "inconsistent"; break;
                case 512: quality = "inaccurate"; break;
                default:  quality = "invalid + failure"; break;
            }

            Timestamp time_stamp = SVSubscriber_ASDU_getTimestamp(asdu, 9);
            time_t time_seconds = Timestamp_getTimeInSeconds(&time_stamp);
            struct tm* time_info = localtime(&time_seconds);
            char time_string[20];

            strftime(time_string, 20, "%d/%m/%Y %H:%M:%S", time_info);

            int smpcnt = static_cast<int>(SVSubscriber_ASDU_getSmpCnt(asdu));
            int sensorid = static_cast<int>(SVSubscriber_ASDU_getINT8(asdu, 0));
            float asdufloat = SVSubscriber_ASDU_getFLOAT32(asdu, 1);
            printf("\nTR1200IP SV, SmpCnt: %i, sensor: %i\n"
                   "  Temperature: %.2f °C\n"
                   "  Quality: %s\n"
                   "  Timestamp: %s\n", smpcnt, sensorid, asdufloat, quality, time_string);
            
            sv_sub_0_counter = smpcnt;
        }
        else
        {
            char* quality;
            switch(SVSubscriber_ASDU_getQuality(asdu, 5))
            {
                case 0:   quality = "good"; break;
                case 1:   quality = "reserved"; break;
                case 2:   quality = "invalid"; break;
                case 3:   quality = "questionable"; break;
                case 4:   quality = "overflow"; break;
                case 8:   quality = "out of range"; break;
                case 16:  quality = "bad reference"; break;
                case 32:  quality = "oscillatory"; break;
                case 64:  quality = "failure"; break;
                case 128: quality = "old data"; break;
                case 256: quality = "inconsistent"; break;
                case 512: quality = "inaccurate"; break;
                default:  quality = "invalid + failure"; break;
            }

            Timestamp time_stamp = SVSubscriber_ASDU_getTimestamp(asdu, 9);
            time_t time_seconds = Timestamp_getTimeInSeconds(&time_stamp);
            struct tm* time_info = localtime(&time_seconds);
            char time_string[20];

            strftime(time_string, 20, "%d/%m/%Y %H:%M:%S", time_info);

            int smpcnt = static_cast<int>(SVSubscriber_ASDU_getSmpCnt(asdu));
            int sensorid = static_cast<int>(SVSubscriber_ASDU_getINT8(asdu, 0));
            float asdufloat = SVSubscriber_ASDU_getFLOAT32(asdu, 1);
            printf("\nMEg44PAN SV, SmpCnt: %i, sensor: %i\n"
                   "  Temperature: %.2f °C\n"
                   "  Quality: %s\n"
                   "  Timestamp: %s\n", smpcnt, sensorid, asdufloat, quality, time_string);
            
            sv_sub_1_counter = smpcnt;
        }
    }
    
    // save temporary values to original variables
    if(*(int*)parameter == 0x4000)
    {
        sv_sub_0_counter = smpcnt;
    }
    else if(*(int*)parameter == 0x4001)
    {
        sv_sub_1_counter = smpcnt;
    }
}

// callback function for goose listener
static void gooseListener(GooseSubscriber subscriber, void* parameter)
{
    MmsValue* values;
    bool run = false;
    bool process_frame = false;
    uint32_t stnum;
    uint32_t sqnum;
    uint64_t time_received;
    
    // assign values to compare against based on APPID of received frame
    if(*(int*)parameter == 0x1000)
    {
        stnum = gs_sub_0_counter[0];
        sqnum = gs_sub_0_counter[1];
        time_received = (time_received_0 == 0) ? Hal_getTimeInMs() : time_received_0;
        run = true;
    }
    else if(*(int*)parameter == 0x1001)
    {
        stnum = gs_sub_1_counter[0];
        sqnum = gs_sub_1_counter[1];
        time_received = (time_received_1 == 0) ? Hal_getTimeInMs() : time_received_1;
        run = true;
    }
    else
    {
        printf("\nInvalid GOOSE frame with APPID 0x%04x - unrecognized APPID!", *(int*)parameter);
    }
    
    if(run == true)
    {
        // if stNum is identical and sqNum is identical to previous one and is not zero
        if(GooseSubscriber_getSqNum(subscriber) == sqnum && sqnum != 0)
        {
            printf("\nInvalid GOOSE frame with APPID 0x%04x - duplicated sqNum!\n"
                   "  Last accepted: %u\tReceived: %u", *(int*)parameter, sqnum, GooseSubscriber_getSqNum(subscriber));
        }
        // if stNum is identical and sqNum is subsequent to previous one
        else if(GooseSubscriber_getStNum(subscriber) == stnum && GooseSubscriber_getSqNum(subscriber) == sqnum + 1)
        {
            sqnum = GooseSubscriber_getSqNum(subscriber);
            process_frame = true;
        }
        // if sqNum is zero, check if stNum has changed
        else if(GooseSubscriber_getSqNum(subscriber) == 0 && GooseSubscriber_getStNum(subscriber) >= stnum)
        {
            // if stNum is identical to previous one
            if(GooseSubscriber_getStNum(subscriber) == stnum)
            {
                printf("\nInvalid GOOSE frame with APPID 0x%04x - duplicated stNum!\n"
                   "  Last accepted: %u\tReceived: %u", *(int*)parameter, stnum, GooseSubscriber_getStNum(subscriber));
            }
            // if stNum is subsequent to previous one
            else if(GooseSubscriber_getStNum(subscriber) == stnum + 1)
            {
                printf("5 ");
                stnum = GooseSubscriber_getStNum(subscriber);
                sqnum = 0;
                process_frame = true;
            }
        }
        // values differ from saved values
        else
        {
            // first run on program - save current values
            if(sqnum == 0 && stnum == 0)
            {
                sqnum = GooseSubscriber_getSqNum(subscriber);
                stnum = GooseSubscriber_getStNum(subscriber);
            }
            else
            {
                printf("\nWarning - GOOSE frame values for APPID 0x%04x differ from saved values!\n"
                       "  Last accepted - stNum: %i, sqNum: %i\n"
                       "  Received      - stNum: %i, sqNum: %i\n",
                       *(int*)parameter, stnum, sqnum, GooseSubscriber_getStNum(subscriber), GooseSubscriber_getSqNum(subscriber));
            }
        }
        
        // process frame
        if(process_frame == true)
        {    
            if(Hal_getTimeInMs() - time_received > GooseSubscriber_getTimeAllowedToLive(subscriber))
            {
                printf("\nInvalid GOOSE frame with APPID 0x%04x - TTL value exceeded, frame dropped!\n"
                       "  Valid TTL: %i\tReceived frame: %i", *(int*)parameter, GooseSubscriber_getTimeAllowedToLive(subscriber), Hal_getTimeInMs() - time_received);
            }
            else
            {
                if(*(int*)parameter == 0x1000)
                {                    
                    values = GooseSubscriber_getDataSetValues(subscriber);
                    std::stringstream ss;
                    ss << "\nTR1200IP GOOSE, stNum: " << GooseSubscriber_getStNum(subscriber)
                       << ", sqNum: " << GooseSubscriber_getSqNum(subscriber)
                       << ", delta t: " << Hal_getTimeInMs() - time_received
                       << "\n";

                    struct tm* time_info;
                    char time_string[20];
                    time_t time_seconds;

                    for(int i = 0; i < MmsValue_getArraySize(values) / 3; i++)
                    {
                        time_seconds = MmsValue_toUnixTimestamp(MmsValue_getElement(values, 2 + 3 * i));
                        time_info = localtime(&time_seconds);
                        strftime(time_string, 20, "%d/%m/%Y %H:%M:%S", time_info);

                        ss << "  Sensor: " << MmsValue_toInt32(MmsValue_getElement(values, 0 + 3 * i)) << "\n"
                           << "  Temperature: " << MmsValue_toFloat(MmsValue_getElement(values, 1 + 3 * i)) << " °C\n"
                           << "  Timestamp: " << time_string << "\n";
                    }

                    printf(ss.str().c_str());
                }
                else
                {
                    values = GooseSubscriber_getDataSetValues(subscriber);
                    std::stringstream ss;
                    ss << "\nMEg44PAN GOOSE, stNum: " << GooseSubscriber_getStNum(subscriber)
                       << ", sqNum: " << GooseSubscriber_getSqNum(subscriber)
                       << ", delta t: " << Hal_getTimeInMs() - time_received
                       << "\n";

                    struct tm* time_info;
                    char time_string[20];
                    time_t time_seconds;

                    for(int i = 0; i < MmsValue_getArraySize(values) / 3; i++)
                    {
                        time_seconds = MmsValue_toUnixTimestamp(MmsValue_getElement(values, 2 + 3 * i));
                        time_info = localtime(&time_seconds);
                        strftime(time_string, 20, "%d/%m/%Y %H:%M:%S", time_info);

                        ss << "  Sensor: " << MmsValue_toInt32(MmsValue_getElement(values, 0 + 3 * i)) << "\n"
                           << "  Voltage: " << MmsValue_toFloat(MmsValue_getElement(values, 1 + 3 * i)) << " V\n"
                           << "  Timestamp: " << time_string << "\n";
                    }

                    printf(ss.str().c_str());
                }
            }
        }
        
        // save temporary values to original variables
        if(*(int*)parameter == 0x1000)
        {
            gs_sub_0_counter[0] = stnum;
            gs_sub_0_counter[1] = sqnum;
            time_received_0 = Hal_getTimeInMs();
        }
        else
        {
            gs_sub_1_counter[0] = stnum;
            gs_sub_1_counter[1] = sqnum;
            time_received_1 = Hal_getTimeInMs();
        }
    }
}

// zapis dat od klienta
static MmsDataAccessError writeAccessHandler(DataAttribute* dataAttribute, MmsValue* value, ClientConnection connection, void* parameter)
{
    if (dataAttribute == IEDMODEL_RPi1_GGIO1_NamPlt_vendor)
    {
        if (dataAttribute == IEDMODEL_RPi1_GGIO1_NamPlt_vendor)
        {
            cout << "Novy nazev zarizeni :" << MmsValue_toString(value) << endl; 
            return DATA_ACCESS_ERROR_SUCCESS;
        }
 
        return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }
    
    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn11_mag_f)
    {
        float newValue = MmsValue_toFloat(value);
        cout << "Nova hodnota AnIn11.f :" << newValue << endl;

        // check if value is inside of valid range
        if ((newValue >= 0.f) && (newValue <= 1000.1f))
            return DATA_ACCESS_ERROR_SUCCESS;
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }
    
    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn11_mag_i)
    {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn11.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc13_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn12_mag_i)
    {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn12.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc14_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn13_mag_i) {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn13.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc15_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn14_mag_i)
    {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn14.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc16_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn15_mag_i)
    {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn15.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc21_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn16_mag_i)
    {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn16.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc22_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn17_mag_i)
    {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn17.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc23_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    if (dataAttribute == IEDMODEL_RPi1_GGIO1_AnIn18_mag_i)
    {
        int newValue = MmsValue_toInt32(value);
        cout << "Nova hodnota AnIn18.i :" << newValue << endl;

        if ((newValue >= 0) && (newValue <= 1))
        {
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc24_stVal, newValue);
            return DATA_ACCESS_ERROR_SUCCESS;
        }
        else
            return DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
    }

    return DATA_ACCESS_ERROR_OBJECT_ACCESS_DENIED;
}

// select before operate
static CheckHandlerResult checkHandler(void* parameter, MmsValue* ctlVal, bool test, bool interlockCheck, ClientConnection connection)
{
    printf("check handler called!\n");

    if (interlockCheck)
        printf("  with interlock check bit set!\n");

    if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO1)
        return CONTROL_ACCEPTED;

    if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO2)
        return CONTROL_ACCEPTED;

    if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO3)
        return CONTROL_ACCEPTED;

    if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO4)
        return CONTROL_ACCEPTED;

    return CONTROL_OBJECT_UNDEFINED;
}

// operate (control)
static ControlHandlerResult controlHandlerForBinaryOutput(void* parameter, MmsValue* value, bool test)
{
    if (test)
        return CONTROL_RESULT_FAILED;

    
    if (MmsValue_getType(value) == MMS_BOOLEAN)
    {        
        uint64_t timeStamp = Hal_getTimeInMs();
        char* part;

        if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO1)
        {
            IedServer_updateUTCTimeAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO1_t, timeStamp);
            IedServer_updateAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO1_stVal, value);
            part = "SPCSO1";
        }
        else if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO2)
        {
            IedServer_updateUTCTimeAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO2_t, timeStamp);
            IedServer_updateAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO2_stVal, value);
            part = "SPCSO2";
        }
        else if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO3)
        {
            IedServer_updateUTCTimeAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO3_t, timeStamp);
            IedServer_updateAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO3_stVal, value);
            part = "SPCSO3";
        }
        else if (parameter == IEDMODEL_RPi1_GGIO1_SPCSO4)
        {
            IedServer_updateUTCTimeAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO4_t, timeStamp);
            IedServer_updateAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO4_stVal, value);
            part = "SPCSO4";
        }
        
        printf("Received binary control command: %s %s\n", part, (MmsValue_getBoolean(value)) ? "ON" : "OFF");

        return CONTROL_RESULT_OK;
    }
    else
        return CONTROL_RESULT_FAILED;
}

static void connectionHandler(IedServer self, ClientConnection connection, bool connected, void* parameter)
{
    if (connected)
        printf("Connection opened\n");
    else
        printf("Connection closed\n");
}

int main(int argc, char** argv)
{
    char* interface;
    int tr_sv_app_id,
        tr_gs_app_id,
        ie_sv_app_id,
        ie_gs_app_id;
    
    if(argc == 3)
    {
        interface = argv[1];
        iterations = std::stoi(argv[2]);
    }
    else if(argc == 7)
    {
        interface = argv[1];
        iterations = std::stoi(argv[2]);
        tr_sv_app_id = strtol(argv[3], NULL, 16);
        tr_gs_app_id = strtol(argv[4], NULL, 16);
        ie_sv_app_id = strtol(argv[5], NULL, 16);
        ie_gs_app_id = strtol(argv[6], NULL, 16);
    }
    else
    {
        interface = "ens33";
        iterations = 3;
        tr_sv_app_id = 0x4000;
        tr_gs_app_id = 0x1000;
        ie_sv_app_id = 0x4001;
        ie_gs_app_id = 0x1001;
    }
    
    // hello message
    printf("IEC 61850 Concentrator, using libiec61850 library\n");
    printf("Running. Press CTRL+C to quit.\n");

    iedServer = IedServer_create(&iedModel);
    IedServer_setGooseInterfaceId(iedServer, interface);

    // set handlers for operate command
    IedServer_setControlHandler(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO1, (ControlHandler) controlHandlerForBinaryOutput, IEDMODEL_RPi1_GGIO1_SPCSO1);
    IedServer_setControlHandler(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO2, (ControlHandler) controlHandlerForBinaryOutput, IEDMODEL_RPi1_GGIO1_SPCSO2);
    IedServer_setControlHandler(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO3, (ControlHandler) controlHandlerForBinaryOutput, IEDMODEL_RPi1_GGIO1_SPCSO3);
    IedServer_setControlHandler(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO4, (ControlHandler) controlHandlerForBinaryOutput, IEDMODEL_RPi1_GGIO1_SPCSO4);

    IedServer_setConnectionIndicationHandler(iedServer, (IedConnectionIndicationHandler)connectionHandler, NULL);

    // MMS server will be instructed to start listening to client connections
    IedServer_start(iedServer, 102);
    // allow access to SP variables by default
    IedServer_setWriteAccessPolicy(iedServer, IEC61850_FC_ALL, ACCESS_POLICY_ALLOW);

    // instruct the server that we will be informed if a clients writes to a certain variables we are interested in.
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_NamPlt_vendor, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn11_mag_f, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn11_mag_i, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn12_mag_i, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn13_mag_i, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn14_mag_i, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn15_mag_i, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn16_mag_i, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn17_mag_i, writeAccessHandler, NULL);
    IedServer_handleWriteAccess(iedServer, IEDMODEL_RPi1_GGIO1_AnIn18_mag_i, writeAccessHandler, NULL);
        
    if (!IedServer_isRunning(iedServer))
    {
        printf("Starting server failed! Exit.\n");
        IedServer_destroy(iedServer);
        exit(-1);
    }
      
    //IedServer_enableGoosePublishing(iedServer);
    
    // sample values
    // create sv subscribers and set listeners
    SVSubscriber sv_sub_0 = SVSubscriber_create(NULL, tr_sv_app_id);
    SVSubscriber sv_sub_1 = SVSubscriber_create(NULL, ie_sv_app_id);
    SVSubscriber_setListener(sv_sub_0, svUpdateListener, &tr_sv_app_id);
    SVSubscriber_setListener(sv_sub_1, svUpdateListener, &ie_sv_app_id);
    
    // create sv receiver, set interface and add subscribers
    SVReceiver sv_rec = SVReceiver_create();
    SVReceiver_setInterfaceId(sv_rec, interface);
    SVReceiver_addSubscriber(sv_rec, sv_sub_0);
    SVReceiver_addSubscriber(sv_rec, sv_sub_1);
    SVReceiver_start(sv_rec);
    
    // goose messages
    // create goose subscribers and set listeners
    GooseSubscriber gs_sub_0 = GooseSubscriber_create("TR1200IPMEAS/LLN0$GO$TempAndRelay", NULL);
    GooseSubscriber gs_sub_1 = GooseSubscriber_create("MEg44PANLD0/LLN0$GO$Temperature", NULL);
    GooseSubscriber_setAppId(gs_sub_0, tr_gs_app_id);
    GooseSubscriber_setAppId(gs_sub_1, ie_gs_app_id);
    GooseSubscriber_setListener(gs_sub_0, gooseListener, &tr_gs_app_id);
    GooseSubscriber_setListener(gs_sub_1, gooseListener, &ie_gs_app_id);
    
    // create goose receiver, set interface and add subscribers
    GooseReceiver gs_rec = GooseReceiver_create();
    GooseReceiver_setInterfaceId(gs_rec, interface);
    GooseReceiver_addSubscriber(gs_rec, gs_sub_0);
    GooseReceiver_addSubscriber(gs_rec, gs_sub_1);
    GooseReceiver_start(gs_rec);
    
    running = 1;

    signal(SIGINT, sigint_handler);
    float t = 0.f;
    
    // Smycka
    static char* gpio_data;
    while (running)
    { 
        uint64_t timestamp = Hal_getTimeInMs();

        gpio_data = readfile("soubor_gpio.txt");

        // Zapis Analogovych a Bitovych vstupu  
        IedServer_lockDataModel(iedServer);

        Timestamp iecTimestamp;

        Timestamp_clearFlags(&iecTimestamp);
        Timestamp_setTimeInMilliseconds(&iecTimestamp, timestamp);
        Timestamp_setLeapSecondKnown(&iecTimestamp, true);

        /* toggle clock-not-synchronized flag in timestamp */
        if (((int)t % 2) == 0)
            Timestamp_setClockNotSynchronized(&iecTimestamp, true);

        // čtení digitálních vstupů 26,27,28,29
        int gpio26 = charToInt(gpio_data[8]);
        if (IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind1_stVal) != gpio26)
        {
            IedServer_updateTimestampAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind1_t, &iecTimestamp);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind1_stVal, gpio26);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc1_stVal, gpio26);
            cout << "Zmena GPIO26: " << IedServer_getBooleanAttributeValue(iedServer,IEDMODEL_RPi1_GGIO1_Ind1_stVal) << endl;
        }

        int gpio27 = charToInt(gpio_data[10]);
        if (IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind2_stVal) != gpio27)
        {
            IedServer_updateTimestampAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind2_t, &iecTimestamp);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind2_stVal, gpio27);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc2_stVal, gpio27);            
            cout << "Zmena GPIO27: " << IedServer_getBooleanAttributeValue(iedServer,IEDMODEL_RPi1_GGIO1_Ind2_stVal) << endl;
        }

        int gpio28 = charToInt(gpio_data[12]);
        if (IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind3_stVal) != gpio28)
        {
            IedServer_updateTimestampAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind3_t, &iecTimestamp);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind3_stVal, gpio28);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc3_stVal, gpio28);            
            cout << "Zmena GPIO28: " << IedServer_getBooleanAttributeValue(iedServer,IEDMODEL_RPi1_GGIO1_Ind3_stVal) << endl;
        }

        int gpio29 = charToInt(gpio_data[14]);
        if (IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind4_stVal) != gpio29)
        {
            IedServer_updateTimestampAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind4_t, &iecTimestamp);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_Ind4_stVal, gpio29);
            IedServer_updateBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc4_stVal, gpio29);
            cout << "Zmena GPIO29: " << IedServer_getBooleanAttributeValue(iedServer,IEDMODEL_RPi1_GGIO1_Ind4_stVal) << endl;
        }

        IedServer_unlockDataModel(iedServer);

        if ((IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc12_stVal) == 1) || 
            (IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_LLN0_Loc20_stVal) == 1))
        {
            MmsValue* value = MmsValue_newBoolean(0);
            IedServer_updateAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO1_stVal, value);
            IedServer_updateAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO2_stVal, value);
        }

        // zápis digitálních vstupů 2,3,12,13
        int gpio2 = 0;
        gpio2 = IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO1_stVal);
        gpio_data[0] = intToChar(gpio2);

        int gpio3 = 0;
        gpio3 = IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO2_stVal);
        gpio_data[2] = intToChar(gpio3);

        int gpio12 = 0;
        gpio12 = IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO3_stVal);
        gpio_data[4] = intToChar(gpio12);

        int gpio13 = 0;
        gpio13 = IedServer_getBooleanAttributeValue(iedServer, IEDMODEL_RPi1_GGIO1_SPCSO4_stVal);
        gpio_data[6] = intToChar(gpio13);

        writeToFile("soubor_gpio.txt", gpio_data);
    
        Thread_sleep(1000);
    }
    
    // stop MMS server - close TCP server socket and all client sockets
    IedServer_stop(iedServer);
    IedServer_destroy(iedServer);
    
    SVReceiver_stop(sv_rec);
    SVReceiver_destroy(sv_rec);
    GooseReceiver_stop(gs_rec);
    GooseReceiver_destroy(gs_rec);
    
    return 0;
}