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

#include "can_vca.h"
#include "ul_gsa.h"
#include "ul_dbuff.h"
#include "vcasdo_fsm.h"
#include "canmond.h"
#include "vcasdo_msg.h"

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

static vca_handle_t canhandle = 0;
static int server_sockfd = -1; 

static int log_is_cont = 0;

//====================== logging support =======================
// -------------- log levels ------------------
#define LOG_FATAL   0
#define LOG_ERR     1
#define LOG_MSG     2
#define LOG_INF     3
#define LOG_DEB     4

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, ...)
{
    va_list ap; 
    va_start(ap, format); 
    myvlog(level, format, ap);
    va_end(ap);
}
/*
static void write_and_log(int fd, int level, const byte *buff, int len)
{
    write(fd, buff, len);
    mylog(level, "%s", 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);
}

// confirmations
// {SDOC UPLOAD server_port client_port node index subindex [data ...]}
// {SDOC DOWNLOAD server_port client_port node index subindex}
static void sdo2str(ul_dbuff_t *dbuff, const vcasdo_fsm_t *fsm)
{
    char str[32];
    int n;
    ul_dbuff_strcpy(dbuff, "{SDOC ");
    if(fsm->type == sdofsmUploader) 
        ul_dbuff_strcat(dbuff, "UPLOAD ");
    else if(fsm->type == sdofsmDownloader) 
        ul_dbuff_strcat(dbuff, "DOWNLOAD ");
    sprintf(str, "%x ", (unsigned)fsm->server_port); ul_dbuff_strcat(dbuff, str);
    sprintf(str, "%x ", (unsigned)fsm->client_port); ul_dbuff_strcat(dbuff, str);
    sprintf(str, "%x ", (unsigned)fsm->node); ul_dbuff_strcat(dbuff, str);
    sprintf(str, "%x ", fsm->index); ul_dbuff_strcat(dbuff, str);
    sprintf(str, "%x ", fsm->subindex); ul_dbuff_strcat(dbuff, str);
    if(fsm->state == sdofsmAbort) {
        // find abort code
        byte *data = fsm->out_msg.data;
        __u32 code = 0;
        int i;
        ul_dbuff_strcat(dbuff, "ABORT ");
        for(i=0; i<4; i++) code = (code << 8) + data[7 - i];
        sprintf(str, "%x '", (unsigned)code); ul_dbuff_strcat(dbuff, str);
        // find abort description
        ul_dbuff_strcat(dbuff, vcasdo_abort_msg(code));
        ul_dbuff_strcat(dbuff, "'");
    }
    else if(fsm->state == sdofsmError) {
        ul_dbuff_strcat(dbuff, "ERROR ");
        sprintf(str, "%x '", fsm->err_no); ul_dbuff_strcat(dbuff, str);
        ul_dbuff_strcat(dbuff, vcasdo_error_msg(fsm->err_no));        
        ul_dbuff_strcat(dbuff, "'");
    }
    else { 
        if(fsm->type == sdofsmUploader) {
            ul_dbuff_strcat(dbuff, "[");
            for(n=0; n<fsm->data.len; n++) {
                if(n > 0) ul_dbuff_strcat(dbuff, " ");
                sprintf(str, "%x", (unsigned)(fsm->data.data[n])); ul_dbuff_strcat(dbuff, str);
            }
            ul_dbuff_strcat(dbuff, "]");
        }
    }
    ul_dbuff_strcat(dbuff, "}");
}
//==============================================================
//--------------------------------------------------------------------
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-g\n"
            "\t--log_level [n]\tsets how many log messages you will see.\n"
            "\t\t0 - fatal errors onyl\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";
            
//=================== GSA structures ===========================
//-------------------- client list ------------------
typedef struct {
    int fd;
} client_gsa_t;
typedef GSA_ARRAY_FOR(client_gsa_t) client_list_t;

__inline__ gsa_array_t * clientlist2gsa(client_list_t *arr)
{
  return (gsa_array_t *)arr;
}
// list of clients connected to the server
client_list_t client_list;

//-------------------- sdofsm list ------------------
typedef struct {
    vcasdo_fsm_t *fsm;
    int fd;  // fd of its client
} fsm_gsa_t;
typedef GSA_ARRAY_FOR(fsm_gsa_t) fsm_list_t;

__inline__ gsa_array_t * fsmlist2gsa(fsm_list_t *arr)
{
  return (gsa_array_t *)arr;
}

// list of clients connected to the server
fsm_list_t fsm_list;

//----------------------------------------------------
void destroy_gsafsm(fsm_gsa_t *gsafsm, int unlink) 
{
    // unling fsm from fsm_list
    if(unlink) {
        unsigned u;
        for(u=0; u<fsm_list.count; u++) {
            fsm_gsa_t *g = fsm_list.items[u];
            if(g == gsafsm) {
                gsa_delete_at(fsmlist2gsa(&fsm_list), u);
                mylog(LOG_DEB, "removing fsm %p from fsm list\n", gsafsm->fsm);
            }
        }
    }
    // delete fsm
    mylog(LOG_DEB, "destroying fsm %p\n", gsafsm->fsm);
    vcasdo_destroy_fsm(gsafsm->fsm);
    free(gsafsm);
}
//----------------------------------------------------
// requests
// {SDOR UPLOAD server_port client_port node index subindex}
// {SDOR DOWNLOAD server_port client_port node index subindex [data ...]}
// confirmations
// {SDOC UPLOAD server_port client_port node index subindex [data ...]}
// {SDOC DOWNLOAD server_port client_port node index subindex}
fsm_gsa_t *create_gsafsm(int fd, const char *str, int *eaten_chars)
{
    *eaten_chars = -1;
    // create_gsafsm creates fsm only if no fsm exists for this port & node
    int n, l = 0;
    unsigned srv_port, cli_port, node, index, subindex;
    int type;
    
    if((n = vca_strmatch(str, "UPLOAD~")) > 0) {
        type = sdofsmUploader;
    }
    else if((n = vca_strmatch(str, "DOWNLOAD~")) > 0) {
        type = sdofsmDownloader;
    }
    else {
        mylog(LOG_ERR, "create_gsafsm::unknown SDO FSM type: %s\n", str);
        return NULL;
    }
    l += n;
    n = vca_gethex(str + l, &srv_port); l += n;
    n = vca_gethex(str + l, &cli_port); l += n;
    n = vca_gethex(str + l, &node); l += n;
    n = vca_gethex(str + l, &index); l += n;
    n = vca_gethex(str + l, &subindex); l += n;

    vcasdo_fsm_t *fsm = vcasdo_init_fsm(NULL, srv_port, cli_port, node);
    fsm->index = index;
    fsm->subindex = subindex;
    fsm->type = type;
    // reload srv_port & cli_port from fsm
    // str can contain 0 0 for default values
    srv_port = fsm->server_port;
    cli_port = fsm->client_port;
    //check port & node
    unsigned u;
    for(u=0; u<fsm_list.count; u++) {
        fsm_gsa_t *gsafsm = fsm_list.items[u];
        vcasdo_fsm_t *fsm = gsafsm->fsm;
        if(fsm) {
            if(fsm->server_port == srv_port && fsm->node == node) break;
            if(fsm->client_port == cli_port && fsm->node == node) break;
        }
    }
    if(u < fsm_list.count) {
        // port & node exists
        mylog(LOG_ERR, "SDO FSM (server port: 0x%x or client port: 0x%x) and node: %u exists, cann't create a second one\n",
                        srv_port, cli_port, node);
        ul_dbuff_t dbuff;
        ul_dbuff_init(&dbuff, 0);
        fsm->state = sdofsmError;
        fsm->err_no = sdofsmErrFsmExists;
        sdo2str(&dbuff, fsm);
        if(fd > 0) write(fd, dbuff.data, dbuff.len); //send also terminating zero
        vcasdo_destroy_fsm(fsm);
        return NULL;
    }

    // create gsafsm
    fsm_gsa_t *gsafsm = (fsm_gsa_t*)malloc(sizeof(fsm_gsa_t));
    if(!gsafsm) {
        mylog(LOG_FATAL, "can't allocate new SDO FSM\n");
        exit(EXIT_FAILURE);
    }
    gsafsm->fd = fd;
    gsafsm->fsm = fsm;
    
    // load data to the fsm->data
    if((n = vca_strmatch(str + l, "~[")) > 0) {
        l += n;
        while((n = vca_gethex(str + l, &u)) > 0) {
            ul_dbuff_append_byte(&(fsm->data), (byte)u);
            l += n;
        }
    }
    if((n = vca_strmatch(str + l, "~]")) > 0) l += n;

    mylog(LOG_DEB, "new SDO FSM (%p) created\n", gsafsm->fsm);
    mylog(LOG_DEB, "  server_port: 0x%x client_port: 0x%x node: %u\n", 
                    fsm->server_port, fsm->client_port, fsm->node);
    mylog(LOG_DEB, "adding fsm %p to fsm list\n", gsafsm->fsm);
    gsa_insert(fsmlist2gsa(&fsm_list), gsafsm, 0);
    *eaten_chars = l;
    return gsafsm;
}
//----------------------------------------------------
void delete_client(int fd)
{
    // delete client on fd
    mylog(LOG_DEB, "closing fd %d\n", fd);
    close(fd);
    mylog(LOG_MSG, "removing client on fd %d\n", fd);
    client_gsa_t *c;
    c = gsa_find_first(clientlist2gsa(&client_list), &fd);
    if(c) {
        gsa_delete(clientlist2gsa(&client_list), c);
        free(c);
    }
    
    // delete all fsm assigned to this fd
    fsm_gsa_t *gsafsm;
    while((gsafsm = gsa_find_first(fsmlist2gsa(&fsm_list), &fd))) {
        gsa_delete(fsmlist2gsa(&fsm_list), gsafsm);
        destroy_gsafsm(gsafsm, 0); // no unlink
    }
}
//----------------------------------------------------
void clean_up(void)
{
    // close CAN
    if(vca_h2fd(canhandle) > 0) {
        mylog(LOG_DEB, "closing CAN\n");
        vca_close_handle(canhandle);
    }
    // close server socket
    if(server_sockfd >= 0) {
        mylog(LOG_DEB, "closing server socket\n");
        close(server_sockfd);
    }
    // remove all clients
    while(client_list.count) {
        client_gsa_t *c = client_list.items[0];
        delete_client(c->fd);
    }
    // remove all FSMs
    // all fsms should be deleted with clients
    if(fsm_list.count) {
        mylog(LOG_ERR, "%d FSMs left, should be deleted with clients\n", client_list.count);
    }
}
//----------------------------------------------------
void send_to_can(canmsg_t *msg)
{
    if(vca_h2fd(canhandle) <= 0) return;
    char buff[64];
    vca_msg2str(msg, buff, 64);
    mylog(LOG_DEB, "sending message %s to the CAN\n", buff);
    vca_send_msg_seq(canhandle, msg, 1);
}
//----------------------------------------------------
// returns t1-t2 in usec
int subtimeval(struct timeval *t1, struct timeval *t2)
{
    int sec = t1->tv_sec - t2->tv_sec;
    int usec = t1->tv_usec - t2->tv_usec;
    //mylog(LOG_DEB, "sec: %i usec: %i\n", sec, usec);
    return sec * 1000000 + usec;
}
//----------------------------------------------------

/*--- 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[])
{
    int client_sockfd;
    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];
    ul_dbuff_t rdbuff;
    //int cnt = 0;
    //char *s;
    int help_opt = 0;
    int port_opt = CANMOND_PORT;

	struct canmsg_t readmsg;
	struct sigaction act;
    
    gsa_struct_init(clientlist2gsa(&client_list), GSA_OFFSET(client_gsa_t, fd), gsa_cmp_int);
    gsa_struct_init(fsmlist2gsa(&fsm_list), GSA_OFFSET(fsm_gsa_t, fd), gsa_cmp_int);
    
    ul_dbuff_init(&rdbuff, 0);
    ul_dbuff_set_capacity(&rdbuff, RD_BUFF_INIT_LEN);
    
	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'},
                {"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 '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");
        }
    }

    mylog(LOG_INF, "listenning port: %i\n", port_opt);
    mylog(LOG_INF, "log level: %i\n", vca_log_cutoff_level);

    /*------- register handler on SIGINT signal -------*/
	act.sa_handler = exithandler;
	sigemptyset(&act.sa_mask);
	act.sa_flags = 0;
	sigaction(SIGINT, &act, 0);
	/*---------------------------------------*/	

    /* open can device */
	if (vca_open_handle(&canhandle, "/dev/can0", NULL, 0) != VCA_OK) {
		perror("open /dev/can");
		mylog(LOG_FATAL, "Error opening /dev/can0\n");
		//exit(1);	
	}
    
    /*  Create and name a socket for the server.  */
    server_sockfd = socket(AF_INET, SOCK_STREAM, 0);

    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);
    if(vca_h2fd(canhandle) > 0) FD_SET(vca_h2fd(canhandle), &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) {
        int nread;
        struct timeval tv;

        tv.tv_sec = 6;
        testfds = readfds;
        struct timeval timeout = {.tv_sec = 1, .tv_usec = 0};

        //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
            struct timeval t;
            gettimeofday(&t, NULL);
            unsigned u;
            for(u=0; u<fsm_list.count; u++) {
                fsm_gsa_t *gsafsm = fsm_list.items[u];
                vcasdo_fsm_t *fsm = gsafsm->fsm;
                int d = subtimeval(&t, &(fsm->last_activity));
                if(d > SDO_TIMEOUT) {
                    mylog(LOG_ERR, "SDO TIMEOUT fsm(%p)\n", fsm);
                    fsm->state = sdofsmError;
                    fsm->err_no = sdofsmErrTimeout;
                    ul_dbuff_t dbuff;
                    ul_dbuff_init(&dbuff, 0);
                    sdo2str(&dbuff, fsm);
                    mylog(LOG_ERR, "destroying active FSM (%p)\n", fsm);
                    if(gsafsm->fd > 0) write(gsafsm->fd, dbuff.data, dbuff.len); //send also terminating zero
                    destroy_gsafsm(gsafsm, 1);
                }
            }
            continue;
        }

        /*  Once we know we've got activity,
	        we find which descriptor it's on by checking each in turn using FD_ISSET.  */
        if(FD_ISSET(server_sockfd, &testfds)) { /*  request for a new connection */
            client_gsa_t *nc = (client_gsa_t*)malloc(sizeof(client_gsa_t));
            if(nc) {
                char msg[] = "HELLO from canmond.";
                client_len = sizeof(client_address);
                client_sockfd = accept(server_sockfd, (struct sockaddr *)&client_address, (socklen_t*)&client_len);
                FD_SET(client_sockfd, &readfds);
                nc->fd = client_sockfd;
                gsa_insert(clientlist2gsa(&client_list), nc, 0);
                //clients.Append(new VCanClient(client_sockfd));
                mylog(LOG_MSG, "adding client on fd %d\n", client_sockfd);
                write(client_sockfd, msg, sizeof(msg) - 1); /* without terminating '\0' */
            }
        }
        else if(vca_h2fd(canhandle) > 0 && FD_ISSET(vca_h2fd(canhandle), &testfds)) {/* CAN message */
            int ret = vca_rec_msg_seq(canhandle, &readmsg, 1);
            int i;
            mylog(LOG_DEB, "%i CAN messages arrived!\n", ret);
            for(i=0; i<ret; i++) {
                char msg[100];
                int n;
                unsigned u;
                int refused = 1, taste;
                // scan all active FSMs
                for(u=0; u<fsm_list.count; u++) {
                    fsm_gsa_t *gsafsm = fsm_list.items[u];
                    vcasdo_fsm_t *fsm = gsafsm->fsm;
                    if((taste = vcasdo_fsm_taste_msg(fsm, &readmsg)) != sdofsmMsgRefuse) {
                        refused = 0;
                        client_gsa_t *c;
                        // find appropriate client
                        c = gsa_find_first(clientlist2gsa(&client_list), &(gsafsm->fd));
                        if(!c) fatal_error("cann't find client with fd %d\n", gsafsm->fd);
                        if(fsm->state == sdofsmRun) break;
                        ul_dbuff_t dbuff;
                        ul_dbuff_init(&dbuff, 0);
                        if(fsm->state == sdofsmDone) {
                            // SDO transfer complete
                            mylog(LOG_INF, "SDO transfer done\n");
                        }
                        else if(fsm->state == sdofsmAbort) {
                            // SDO transfer aborted
                            mylog(LOG_MSG, "SDO transfer aborted\n");
                        }
                        else if(fsm->state == sdofsmAbort) {
                            // SDO transfer error
                            mylog(LOG_ERR, "SDO transfer error\n");
                        }
                        else {
                            // unexpected state
                            mylog(LOG_ERR, "SDO FSM unexpected state: %i\n", fsm->state);
                        }
                        // send data to the client
                        sdo2str(&dbuff, fsm);
                        mylog(LOG_MSG, "sendinf confirmation SDO to fd %i: %s\n", c->fd, dbuff.data);
                        write(c->fd, dbuff.data, dbuff.len); //send also terminating zero
                        // free fsm
                        destroy_gsafsm(gsafsm, 1); // destroy and unlink
                        break; // only one FSM can eat the message
                    }
                }
                if(refused) {
                    /* write msg to the string */
                    n = vca_msg2str(&readmsg, msg, 100);
                    printf("CAN message read: %s\n", msg);
    
                    /* send message to all clients */
                    for(u=0; u<client_list.count; u++) {
                        int fd = client_list.items[u]->fd;
                        mylog(LOG_INF, "sending message to fd %i\n", fd);
                        write(fd, msg, n + 1); //send also terminating zero
                    }
                }
            }
        }
        else {
            /* client activity */
            unsigned u;
            int found = 0;
            //static char whole_packet;
            for(u=0; u<client_list.count; u++) {
                int fd = client_list.items[u]->fd;
                if(FD_ISSET(fd, &testfds)) {
                    ioctl(fd, FIONREAD, &nread);
                    if(nread == 0) {
                        // delete client
                        delete_client(fd);
                        ul_dbuff_set_len(&rdbuff, 0);
                        FD_CLR(fd, &readfds);
                    }
                    else {
                        int n;
                        int old_len = rdbuff.len;
                        int new_len = old_len + nread;
                        int eofmsg = 0;
                        if(ul_dbuff_set_len(&rdbuff, new_len) == new_len) {
                            nread = read(fd, rdbuff.data + old_len, nread);
                            // try to find '}' in new data to check out the ond of message
                            for(n = nread-1; n>=0; n--) if((rdbuff.data + old_len)[n] == '}') {eofmsg = 1; break;}
                        }
                        else fatal_error("Cann't allocate memory for client data\n");
                        if(eofmsg) {
                            // if you got the whole message append terminating 0 & process it
                            ul_dbuff_append_byte(&rdbuff, '\0');
                            ul_dbuff_trim(&rdbuff);
                            const char *buff = rdbuff.data;
                            mylog(LOG_INF, "client on fd %d says '%s'\n", fd, buff);
                        
                            if(vca_strmatch(buff, "{CANDTG") > 0) {
                                vca_str2msg(&readmsg, buff);
                                send_to_can(&readmsg);
                            }
                            else if((n = vca_strmatch(buff, "{SDOR~")) > 0) {
                                int l = n;
                                mylog(LOG_DEB, "got SDO request %s\n", buff);
                                fsm_gsa_t *gsafsm = create_gsafsm(fd, buff + l, &n); // create and link
                                if(gsafsm) {
                                    vcasdo_fsm_t *fsm = gsafsm->fsm;
                                    //mylog(LOG_DEB, "UPLOAD of object 0x%x:0x%x started\n", fsm->index, fsm->subindex);
                                    if(vcasdo_run(fsm) != 0) 
                                        // send the answer to the CAN
                                        send_to_can(&(fsm->out_msg));
                                    else 
                                        destroy_gsafsm(gsafsm, 1); // destroy and unlink
                                }
                            }
                            ul_dbuff_set_len(&rdbuff, 0); // reset buffer for the new command
                        }
                    }
                    found = 1;
                    break;
                }
            }
            if(!found) {
                // this should never happen if the program work well
                // outread unexpected data from buffer
                int fd;
                for(fd = 0; fd < FD_SETSIZE; fd++) {
                    if(FD_ISSET(fd, &testfds)) {
                        ul_dbuff_set_len(&rdbuff, 128);
                        nread = read(fd, rdbuff.data, rdbuff.len-1);
                        rdbuff.data[nread] = '\0';
                        mylog(LOG_ERR, "ERROR: %s read from unregistered fd %i\n", rdbuff.data, fd);
                    }
                }
            }
        }
    }
    clean_up();
}