#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <getopt.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>

//using namespace std;

#include "flist.h"
#include "fstring.h"

#include "can_vca.h"
#include "canmond.h"

// comuuniction objects
#include "fcpickle_impl.h"
#include "sdodownloadconfirmmsg.h"
#include "sdouploadconfirmmsg.h"
#include "sdodownloadrequestmsg.h"
#include "sdouploadrequestmsg.h"
 
#define CAN_DTG_MSG_CLASS "ocera.rtcan.CANDtgMsg"
#define SDO_UP_REQ_CLASS "ocera.rtcan.SDOUploadRequestMsg"
#define SDO_DN_REQ_CLASS "ocera.rtcan.SDODownloadRequestMsg"
#define SDO_UP_CNF_CLASS "ocera.rtcan.SDOUploadConfirmMsg"
#define SDO_DN_CNF_CLASS "ocera.rtcan.SDODownloadConfirmMsg"

#define RD_BUFF_INIT_LEN 128
// SDO FSM timeout in usec
#define SDO_TIMEOUT 1000000     

static int server_sockfd = -1; 
static int candev_in = -1;
static int candev_out = -1;

//====================== logging support =======================
static int log_is_cont = 0;

// -------------- log levels ------------------
#define LOG_FATAL   0
#define LOG_ERR     1
#define LOG_MSG     2
#define LOG_INF     3
#define LOG_DEB     4
#define LOG_TRASH   5

static void myvlog(int level, const char *format, va_list ap)
{
    if(log_is_cont) level |= VCA_LOGL_CONT;
    vca_vlog("canmond", level, format, ap);
}

static void mylog(int level, const char *format, ...)
{
    //static int cnt;
    //printf("mylog #%i: ", ++cnt);fflush(stdout);
    va_list ap; 
    va_start(ap, format); 
    myvlog(level, format, ap);
    va_end(ap);
}

static void fatal_error(const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    myvlog(LOG_FATAL, format, ap);
    va_end(ap);
    exit(-1);
}
//==============================================================
//--------------------------------------------------------------------
static char help_msg[] =
            "canmond [OPTION]\n\n"
			"OPTIONS:\n"
			"\t-h\n"
            "\t--help\tthis help screen\n"
			"\t-v --verbose\n"
			"\t-p\n"
            "\t--port [n]\tsets port where the server listens (default "mkstr(CANMOND_PORT)")\n"
            "\t-i named pipe\t--candev_in named pipe\n"
            "\t-o named pipe\t--candev_out named pipe\n"
            "\t\tname of pipe for communication with monitored CAN device\n"
            "\t\tdefault names are /tmp/canmond/candev-in and /tmp/canmond/candev-out\n"
            "\t\tif pipes don't exist, canmond creates the new ones\n"
            "\t\tcanmond writes data to the candev-in pipe\n"
			"\t-g\n"
            "\t--log_level [n]\tsets how many log messages you will see.\n"
            "\t\t0 - fatal errors only\n"
            "\t\t1 - level 0 + errors\n"
            "\t\t2 - level 1 + messages\n"
            "\t\t3 - level 2 + info messages\n"
            "\t\t4 - level 3 + debug messages\n"
            "\t\tlog level can be also set by environment variable CANMOND_LOG_LEVEL\n";
            
//=================== client list ===========================
struct ClientConnection
{
    int fd;
    FString msg;
    int expectedMsgLen;
    //int sdoSignature; // signature of current SDO request | 4b srvcli port | 4b clisrv port | 7b node |
    // candevice can not serve more requests with the same signature simultaneously
    
    ClientConnection(int _fd) : fd(_fd), expectedMsgLen(0) {}
    bool operator<(const ClientConnection& c) {return (fd < c.fd);}
    bool operator==(const ClientConnection& c) {return (fd == c.fd);}
    
    //static uint32_t signatureFromSDOMessage(const FString& sdo_msg);
};
//------------------------------------------------------------------------------------
struct ServedSDO
{
    uint32_t signature;
    int clientIndex;
    
    ServedSDO(uint32_t sig, int cli_ix) : signature(sig), clientIndex(cli_ix) {}
    ~ServedSDO() {
        mylog(LOG_DEB, "~ServedSDO(): signature=%x, clientIndex=%i\n", signature, clientIndex);
    }
};
//------------------------------------------------------------------------------------
static uint32_t signatureFromSDOMessage(const SDOMsgBase_t *msg)
{
    mylog(LOG_TRASH+1, "ClientConnection::signatureFromSDOMessage()\n");
    uint32_t sig = 0;
    int clisrv, srvcli;
    srvcli = msg->srvcliCobId;
    clisrv = msg->clisrvCobId;
    //srvcli port default 580 + node
    if(srvcli == 0) srvcli = 0x580 + msg->node;
    //clisrv port default 600 + node
    if(clisrv == 0) clisrv = 0x600 + msg->node;
    sig = srvcli;
    sig = (sig << 16) + clisrv;
    mylog(LOG_TRASH+1, "\tclisrv: 0x%x\n", clisrv);
    mylog(LOG_TRASH+1, "\tsrvcli: 0x%x\n", srvcli);
    mylog(LOG_TRASH+1, "\tnode: 0x%x\n", msg->node);
    mylog(LOG_TRASH+1, "\tsignature: %08x\n", sig);
    return sig;
}
//-------------------- client list ------------------
FList<ClientConnection> clientList;
//-------------------- served SDO list ------------------
FList<ServedSDO> servedSDOList;
//----------------------------------------------------
// requests
// {SDOR UPLOAD srvcli_port clisrv_port node index subindex}
// {SDOR DOWNLOAD srvcli_port clisrv_port node index subindex [data ...]}
// confirmations
// {SDOC UPLOAD srvcli_port clisrv_port node index subindex [data ...]}
// {SDOC DOWNLOAD srvcli_port clisrv_port node index subindex}
//----------------------------------------------------
void write_to_candevice(const FString& fs)
{
    //FString fs = s + "\n";
    if(candev_in < 0) return;
    mylog(LOG_DEB, "writing to candev pipe: %s\n", fs.hexDump().Str());
    write(candev_in, fs.StrBuff(), fs.Len());
}
//----------------------------------------------------
void write_to_client(ClientConnection &cc, const FString& fs)
{
    int fd = cc.fd;
    mylog(LOG_DEB, "writing to client socket on fd %i : %s\n", fd, fs.hexDump().Str());
    write(fd, fs.StrBuff(), fs.Len());
}
//----------------------------------------------------
void delete_client(int fd)
{
    // delete client on fd
    mylog(LOG_DEB, "closing fd %d\n", fd);
    close(fd);
    // find client
    int ix = clientList.Seek(fd);
    // delete served SDO in any
    if(ix != FEOFLIST) {
        for(int i=0; i<servedSDOList.Cnt(); i++) {
            if(servedSDOList[i].clientIndex == ix) {
                servedSDOList.Remove(i);
                break;
            }
        }
        // delete client
        mylog(LOG_MSG, "destroying client on fd %d\n", fd);
        clientList.Remove(ix);
        //mylog(LOG_DEB, ":(( after destroying client on fd %d\n", fd);
    }
    else {
        mylog(LOG_ERR, "INTERNAL ERROR: delete_client(%i): client not found\n", fd);
    }
}
//----------------------------------------------------
void clean_up(void)
{
    // close server socket
    if(server_sockfd >= 0) {
        mylog(LOG_DEB, "closing server socket\n");
        close(server_sockfd);
    }
    // remove all clients
    for(int i=0; i<clientList.Cnt(); i++) delete_client(clientList[i].fd);

    // close pipes
    if(candev_in >= 0) {
        mylog(LOG_DEB, "closing input pipe\n");
        close(candev_in);
    }
    if(candev_out >= 0) {
        mylog(LOG_DEB, "closing output pipe\n");
        close(candev_out);
    }
}
//----------------------------------------------------
/**
 * check_pipe - check if pipename exists ant try ti create new one if it doesn't
 * Return: 1 if pipe exists, 0 if pipe was created
 */
int check_pipe(const char *pipename)
{
    // create pipe dir if it does'n exist
    FStringList sl;
    sl.fromString(pipename, '/');
    //check if dir exists and create one if it does not
    FString fs, fs2;
    //printf("cnt: %i\n", sl.Cnt());
    for(int i=0; i<sl.Cnt()-1; i++) {
        fs2 = sl[i].Trim();
        //printf("fs2: '%s'\n", fs2.Str());
        // skip double /
        if(i> 0 && !fs2) continue;
        fs += fs2 + "/";
        int res = mkdir(fs.Str(), 0777);
        if(!res) {
            mylog(LOG_DEB, "creating dir '%s': OK\n", fs.Str());
        }
    }
    FString dir = fs, name = sl[-1];
    fs = fs + name;
    if(access(fs.Str(), F_OK) == -1) {
        mylog(LOG_DEB, "creating fifo '%s': \n", fs.Str());
        //int res = mkfifo("/home/fanda/tmp/pipe", 0777);
        int res = mkfifo(fs.Str(), 0777);
        if(res) fatal_error("Cann't create '%s'\n", fs.Str());
        mylog(LOG_DEB | VCA_LOGL_CONT, "OK\n");
        // nevim proc mkfifo nenastavi spravne MODE
        chmod(fs.Str(), 0777);
        return 0;
    }
    else return 1;
    return 0;
}
//----------------------------------------------------
int open_pipe(const char *pipename, int mode)
{
    mylog(LOG_INF, "openning '%s'in mode %i: ", pipename, mode);
    int res = open(pipename, mode);
    mylog(LOG_INF | VCA_LOGL_CONT, "%s\n", (res < 0)? "ERROR": "OK");
    if(res < 0) fatal_error("Cann't open '%s' in mode %i\n", pipename, mode);
    else {
        if((mode | O_RDONLY) && (mode | O_NONBLOCK)) {
            // read all trash from pipe
            int val;
            while(read(res, &val, sizeof(val)) > 0);
        }
    }
    return res;
}
//----------------------------------------------------

/*--- handler on SIGINT signal : the program quit with CTL-C ---*/
void exithandler(int sig)
{   
	mylog(LOG_MSG, "Terminated by user\n");
    clean_up();
	exit(EXIT_SUCCESS);
}
//=================================================================

int main(int argc, char *argv[])
{
    try {
        char *pipe_in_name = "/tmp/canmond/candev_in", *pipe_out_name = "/tmp/canmond/candev_out";
        int server_len, client_len;
        struct sockaddr_in server_address;
        struct sockaddr_in client_address;
        int result;
        fd_set readfds, testfds;
        //char rdbuff[RD_BUFF_LEN];
        //char *s;
        int help_opt = 0;
        int port_opt = CANMOND_PORT;
        FString candev_msg;
        int expected_candevmsg_len = 0;
    
        printf("CANMOND - CAN monitor server\n");
        {
            #ifdef DEBUG
            vca_log_cutoff_level = LOG_DEB;
            #else
            vca_log_cutoff_level = LOG_MSG;
            #endif
            /* parse cmd line arguments */
            while(1) { 
                static struct option long_options[] = {
                    {"port", required_argument, 0, 'p'},
                    {"verbose", required_argument, 0, 'v'},
                    {"candev-in", required_argument, 0, 'i'},
                    {"candev-out", required_argument, 0, 'o'},
                    {"log-level", required_argument, 0, 'g'},
                    {"help", no_argument, 0, 'h'},
                    {0, 0, 0, 0}
                };
                /* getopt_long stores the option index here. */
                int option_index = 0;
                int c;
                c = getopt_long(argc, argv, "p:g:hv", long_options, &option_index);
                /* Detect the end of the options. */
                if (c == -1) break;
                switch (c) {
                    case 0:
                        /* If this option set a flag, do nothing else now. */
                        /* if (long_options[option_index].flag != 0) break;
                        printf ("option %s", long_options[option_index].name);
                        if (optarg) printf (" with arg %s", optarg);
                        printf ("\n"); */
                        break;
                    case 'g':
                        vca_log_cutoff_level = atoi(optarg);
                        mylog(LOG_DEB, "option -g with value `%s'\n", optarg);
                        break;
                    case 'p':
                        port_opt = atoi(optarg);
                        mylog(LOG_DEB, "option -p with value `%s'\n", optarg);
                        break;
                    case 'i':
                        pipe_in_name = optarg;
                        break;
                    case 'o':
                        pipe_out_name = optarg;
                        break;
                    case 'v':
                        vca_log_cutoff_level = LOG_INF;
                        break;
                    case 'h':
                        help_opt = 1;
                        mylog(LOG_DEB, "option -h\n");
                        break;
                    case '?':
                        /* getopt_long already printed an error message. */
                        break;
                    default:
                        mylog(LOG_FATAL, "cmd line parsing error\n");
                        abort ();
                }
            }
            if(help_opt) {
                printf(help_msg);
                exit(EXIT_SUCCESS);
            } 
            /* Print any remaining command line arguments (not options). */
            if (optind < argc) {
                mylog(LOG_INF, "non-option ARGV-elements: ");
                while (optind < argc) mylog(LOG_INF, "%s ", argv[optind++]);
                mylog(LOG_INF, "\n");
            }
        }
    
        /*------- register handler on SIGINT signal -------*/
        signal(SIGINT, exithandler);
        /*---------------------------------------*/
    
        mylog(LOG_INF, "read from CAN device in '%s' pipe\n", pipe_out_name);
        mylog(LOG_INF, "write to CAN device via '%s' pipe\n", pipe_in_name);
        // try to open can device pipes
        mylog(LOG_INF, "waiting on fifos %s, %s\n", pipe_in_name, pipe_out_name);
        check_pipe(pipe_in_name);
        check_pipe(pipe_out_name);
        candev_in = open_pipe(pipe_in_name, O_WRONLY);
        candev_out = open_pipe(pipe_out_name, O_RDONLY | O_NONBLOCK);
    
        mylog(LOG_INF, "listenning port: %i\n", port_opt);
        mylog(LOG_INF, "log level: %i\n", vca_log_cutoff_level);
        /*  Create and name a socket for the server.  */
        server_sockfd = socket(AF_INET, SOCK_STREAM, 0);
        
        int yes = 1;
        // reuse old addr when restarting server, otherwise bind() complains for little time (1 min)
        if(setsockopt(server_sockfd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) == -1) {
            perror("setsockopt");
            exit(1);
        }
        
        server_address.sin_family = AF_INET;
        server_address.sin_addr.s_addr = htonl(INADDR_ANY);
        server_address.sin_port = htons(port_opt);
        server_len = sizeof(server_address);
    
        if(bind(server_sockfd, (struct sockaddr *)&server_address, server_len) < 0) {
            perror("ERROR: bind");
            exit(EXIT_FAILURE);
        }
    
        /*  Create a connection queue and initialize readfds to handle input from server_sockfd.  */
        if(listen(server_sockfd, 5) < 0) {
            perror("listen");
            exit(EXIT_FAILURE);
        }
    
        FD_ZERO(&readfds);
        FD_SET(server_sockfd, &readfds);
        FD_SET(candev_out, &readfds);
    
        /*  Now wait for clients and requests.
        Since we have passed a null pointer as the timeout parameter, no timeout will occur.
        The program will exit and report an error if select returns a value of less than 1.  */
        while(1) {
            struct timeval timeout;
            timeout.tv_sec = 1; timeout.tv_usec = 0;
            testfds = readfds;
    
            //mylog(LOG_DEB, "ENTERING SELECT[%i]\n", ++cnt);
            result = select(FD_SETSIZE, &testfds, (fd_set *)0, (fd_set *)0, &timeout);
            //mylog(LOG_DEB, "select result %i\n", result);
    
            if(result < 0) {
                mylog(LOG_ERR, "exit with select result %i\n", result);
                mylog(LOG_ERR, strerror(errno));
                exit(1);
            }
            if(result == 0) {
                //mylog(LOG_DEB, "timeout\n");
                // check last activity time of all FSMs
                continue;
            }
            if(FD_ISSET(server_sockfd, &testfds)) { /*  request for a new connection */
                // this message is not usefull it only cheks that communication is robust as expected
                //FString msg = "HELLO from canmond.\n";
                client_len = sizeof(client_address);
                int fd = accept(server_sockfd, (struct sockaddr *)&client_address, (socklen_t*)&client_len);
                FD_SET(fd, &readfds);
                clientList.Append(new ClientConnection(fd)); clientList.Sort();
                mylog(LOG_MSG, "adding client on fd %d\n", fd);
                //write(fd, msg.Str(), msg.Len()); /* without terminating '\0' */
            }
            else if(FD_ISSET(candev_out, &testfds)) { // candevice activity
                int nread;
                ioctl(candev_out, FIONREAD, &nread);
                //printf("%i chars in client pipe\n", nread);
                if(nread <= 0) continue;
                FString fs;
                fs.setLen(nread); 
                read(candev_out, fs.StrBuff(), nread);
                mylog(LOG_TRASH, "candevice appends:\n%s\n", fs.hexDump().Str());
                // canmond have to read and parse whole message, because it have to know,
                // who is ansvering. In other case it cann't know to which client send the answer
                for(int i=0; i<fs.Len(); i++) {
                    char c = fs[i];
                    // get head
                    if(candev_msg.Len() < FCPICKLE_PACKET_HEAD_SIZE) {
                        candev_msg += c;
                        if(candev_msg.Len() == FCPICKLE_PACKET_HEAD_SIZE) {
                            fcpickle_packet_head_t head;
                            // check head if it is correct
                            candev_msg.Touch();
                            byte *buff = candev_msg.Buff();
                            if(fcpickle_getHead(&head, buff, candev_msg.Len()) < 0) {
                                // incorrect head
                                // shift data in head one byte right and try it again with a next byte
                                for(int j=0; j<FCPICKLE_PACKET_HEAD_SIZE-1; j++) buff[j] = buff[j+1];
                                candev_msg.setLen(candev_msg.Len() - 1);
                            }
                            else {
                                expected_candevmsg_len = head.packetSize;
                            }
                        }
                        continue;
                    } 
                    // read rest of packet
                    candev_msg += c;
                    if(candev_msg.Len() < expected_candevmsg_len) continue;
                    
                    // now I have whole packet
                    byte *buff = candev_msg.Buff();
                    int buff_len = candev_msg.Len();
                    FString classname = fcpickle_getClassName(buff, buff_len);
                    if(classname == CAN_DTG_MSG_CLASS) {
                        // send rough can messagge to all
                        for(int i=0; i<clientList.Cnt(); i++) write_to_client(clientList[i], candev_msg);
                    }
                    else if(classname == SDO_UP_CNF_CLASS || classname == SDO_DN_CNF_CLASS) {
                        // skip head and unpickle class definition
                        int len = FCPICKLE_PACKET_HEAD_SIZE;
                        fcpickle_class_def_t cn;
                        len += fcpickle_getClassDef(&cn, buff+len, buff_len-len);
                        SDOMsgBase_t msg;
                        SDOMsgBase_unpickleObject(&msg, buff+len, buff_len-len);
                        uint32_t sig = signatureFromSDOMessage(&msg);
                        if(sig > 0) {
                            // find client with correct signature
                            int i;
                            for(i=0; i<servedSDOList.Cnt(); i++) {
                                mylog(LOG_TRASH, "\tservedSDOList[%i].signature: %x, .clientix: %i, looking for sig: %x\n", i, servedSDOList[i].signature, servedSDOList[i].clientIndex, sig);
                                if(servedSDOList[i].signature == sig) break;
                            }
                            if(i == servedSDOList.Cnt()) {
                                mylog(LOG_ERR, "ERROR: signature 0x%x not found in served SDO list\n", sig);
                            }
                            else {
                                int ix = servedSDOList[i].clientIndex;
                                if(ix >= 0 && ix < clientList.Cnt()) {
                                    write_to_client(clientList[ix], candev_msg);
                                }
                                else {
                                    mylog(LOG_ERR, "ERROR: signature 0x%08x - client[%i] not found in client list\n", sig, ix);
                                }
                                mylog(LOG_DEB, "removing served SDO(%08x, %i)\n", sig, ix);
                                servedSDOList.Remove(i);
                            }
                        }
                    }
                    candev_msg = "";
                }
            }
            else {/* client activity */
                int cliix = FEOFLIST;
                for(int i=0; i<clientList.Cnt(); i++) {
                    int fd = clientList[i].fd;
                    if(FD_ISSET(fd, &testfds)) {cliix = i; break;}
                }
                if(cliix == FEOFLIST) {
                    mylog(LOG_ERR, "INTERNAL ERROR: request of unknown client\n");
                    continue;
                }
                int nread;
                ClientConnection &cc = clientList[cliix];
                ioctl(cc.fd, FIONREAD, &nread);
                if(nread == 0) {
                    // client closed its connection
                    FD_CLR(cc.fd, &readfds);
                    delete_client(cc.fd);
                }
                else {
                    FString fs;
                    fs.setLen(nread);
                    read(cc.fd, fs.StrBuff(), nread);
                    mylog(LOG_TRASH, "client on fd %d sends: %s\n", cc.fd, fs.hexDump().Str());
                    // canmond have to read and parse whole message, because it have to know,
                    // who is requesting. In other case it cann't know to which client send the answer
                    for(int i=0; i<fs.Len(); i++) {
                        //mylog(LOG_TRASH+1, "cc.msg: %s\n", cc.msg.hexDump().Str());
                        char c = fs[i];
                        if(cc.msg.Len() < FCPICKLE_PACKET_HEAD_SIZE) {
                            cc.msg += c;
                            if(cc.msg.Len() == FCPICKLE_PACKET_HEAD_SIZE) {
                                fcpickle_packet_head_t head;
                                // check head if it is correct
                                cc.msg.Touch();
                                byte *buff = cc.msg.Buff();
                                if(fcpickle_getHead(&head, buff, cc.msg.Len()) < 0) {
                                    // incorrect head
                                    // shift data in head one byte right and try it again with a next byte
                                    for(int j=0; j<FCPICKLE_PACKET_HEAD_SIZE-1; j++) buff[j] = buff[j+1];
                                    cc.msg.setLen(cc.msg.Len() - 1);
                                }
                                else {
                                    cc.expectedMsgLen = head.packetSize;
                                }
                            }
                            continue;
                        } 
                        // read rest of packet
                        cc.msg += c;
                        if(cc.msg.Len() < cc.expectedMsgLen) continue;
                        
                        // now I have whole packet
                        byte *buff = cc.msg.Buff();
                        int buff_len = cc.msg.Len();
                        FString classname = fcpickle_getClassName(buff, buff_len);
                        if(classname == CAN_DTG_MSG_CLASS) {
                            write_to_candevice(cc.msg);
                        }
                        else if(classname == SDO_UP_REQ_CLASS || classname == SDO_DN_REQ_CLASS) {
                            // skip head and unpickle class definition
                            int len = FCPICKLE_PACKET_HEAD_SIZE;
                            int ret;
                            fcpickle_class_def_t cn;
                            ret = fcpickle_getClassDef(&cn, buff+len, buff_len-len);
                            if(ret < 0) {
                                mylog(LOG_ERR, "ERROR unpickling '%s' class def got from client fd(%i)\n", classname.Str(), cc.fd);
                                cc.msg = "";
                                continue;
                            }
                            len += ret;
                            // unpickle only base class SDOMsgBase
                            SDOMsgBase_t msg;
                            ret = SDOMsgBase_unpickleObject(&msg, buff+len, buff_len-len);
                            if(ret < 0) {
                                mylog(LOG_ERR, "ERROR unpickling '%s' object got from client fd(%i)\n", classname.Str(), cc.fd);
                                cc.msg = "";
                                continue;
                            }
                            uint32_t sig = signatureFromSDOMessage(&msg);
                            if(sig > 0) {
                                // check if this signature is allready served
                                for(int i=0; i<servedSDOList.Cnt(); i++) {
                                    if(servedSDOList[i].signature == sig) {
                                        // yes it is
                                        int ix = servedSDOList[i].clientIndex;
                                        if(ix >= 0 && ix < clientList.Cnt()) {
                                            // client is still alive
                                            // tell him that request with this signature is allready served
                                            int fd = clientList[ix].fd;
                                            FString fs = "This SDO request is currently served by client " + FString(fd) + ".";
                                            mylog(LOG_DEB, "%s\n", fs.Str());
                                            SDOConfirmMsg_t abort;
                                            abort.super = msg;
                                            abort.type = 2; // abort
                                            abort.code = 0;
                                            abort.errmsg = fs.Str();
                                            int size = SDOConfirmMsg_getPacketBufferSize(&abort);
                                            FString pickled; pickled.setLen(size);
                                            ret = SDOConfirmMsg_toNet(&abort, pickled.Buff(), pickled.Len());
                                            if(ret < 0) {
                                                mylog(LOG_ERR, "ERROR pickling SDOConfirmMsg ret: %i\n", ret);
                                            }
                                            else {
                                                write_to_client(cc, pickled);
                                            }
                                            sig = 0;
                                        }
                                        else {
                                            // delete unused signature
                                            mylog(LOG_ERR, "found & remove unused SDO signature %08x\n", sig);
                                            servedSDOList.Remove(i);
                                        }
                                        break;
                                    }
                                }
                            }
                            if(sig) {
                                // introduce new signature
                                mylog(LOG_DEB, "Adding new served SDO (%08x, %i) to list\n", sig, cliix);
                                servedSDOList.Append(new ServedSDO(sig, cliix));
                                write_to_candevice(cc.msg);
                            }
                        }
                        cc.msg = "";
                    }
                }
            }
        }    
    }
    catch(FException &e) {
        mylog(LOG_ERR, "EXCEPTION: %s\n", e.Msg());
    }
    clean_up();
}