#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 "vca_od.h"
#include "ul_dbuff.h"
#include "vcasdo_fsm.h"
#include "canmond.h"
#include "vcasdo_msg.h"

#include "ul_gavl.h"
#include "ul_gavlcust.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; 

//====================== 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

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 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 srvcli_cob_id clisrv_cob_id node index subindex [data ...]}
// {SDOC DOWNLOAD srvcli_cob_id clisrv_cob_id 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->is_uploader) {
        ul_dbuff_strcat(dbuff, "UPLOAD ");
    }
    else { 
        ul_dbuff_strcat(dbuff, "DOWNLOAD ");
    }
    sprintf(str, "%x ", (unsigned)fsm->srvcli_cob_id); ul_dbuff_strcat(dbuff, str);
    sprintf(str, "%x ", (unsigned)fsm->clisrv_cob_id); 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;
        uint32_t 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->is_uploader) {
            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 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";
            
//=================== GAVL structures ===========================
//-------------------- client list ------------------
typedef struct {
    int fd;
    gavl_node_t my_node;
} client_item_t;

typedef struct {
  gavl_node_t *my_root;
  int my_info;
} client_root_t;

typedef int client_key_t;

GAVL_CUST_NODE_INT_DEC(client, client_root_t, client_item_t, client_key_t,
	my_root, my_node, fd, client_cmp_fnc)

inline int client_cmp_fnc(const client_key_t *a, const client_key_t *b)
{
  if (*a>*b) return 1;
  if (*a<*b) return -1;
  return 0;
}

GAVL_CUST_NODE_INT_IMP(client, client_root_t, client_item_t, client_key_t,
	my_root, my_node, fd, client_cmp_fnc)

// tree of clients connected to the server
client_root_t client_root;

//-------------------- gavlfsm list ------------------
typedef unsigned long gavlfsm_key_t;

typedef struct {
    int fd;  // fd of the fsm client
    gavlfsm_key_t cobid; // port:node
    vcasdo_fsm_t fsm;
    gavl_node_t my_node;
} gavlfsm_item_t;

typedef struct {
  gavl_node_t *my_root;
  int my_info;
} gavlfsm_root_t;

GAVL_CUST_NODE_INT_DEC(gavlfsm, gavlfsm_root_t, gavlfsm_item_t, gavlfsm_key_t,
	my_root, my_node, cobid, gavlfsm_cmp_fnc)

inline int gavlfsm_cmp_fnc(const gavlfsm_key_t *a, const gavlfsm_key_t *b)
{
  if (*a>*b) return 1;
  if (*a<*b) return -1;
  return 0;
}

GAVL_CUST_NODE_INT_IMP(gavlfsm, gavlfsm_root_t, gavlfsm_item_t, gavlfsm_key_t,
	my_root, my_node, cobid, gavlfsm_cmp_fnc)

// tree of active FSMs
gavlfsm_root_t gavlfsm_root;

//----------------------------------------------------
void destroy_gavlfsm(gavlfsm_item_t *gavlfsm, int unlink) 
{
    if(!gavlfsm) return;
    // unling fsm from gavlfsm_list
    if(unlink) {
        int ret = gavlfsm_delete(&gavlfsm_root, gavlfsm);
        mylog(LOG_DEB, "removing fsm %p from fsm tree, status [%i]\n", gavlfsm->fsm, ret);
    }
    // delete fsm
    mylog(LOG_DEB, "destroying fsm %p\n", gavlfsm->fsm);
    vcasdo_destroy_fsm(&gavlfsm->fsm);
    free(gavlfsm);
}
//----------------------------------------------------
// requests
// {SDOR UPLOAD srvcli_cob_id clisrv_cob_id node index subindex}
// {SDOR DOWNLOAD srvcli_cob_id clisrv_cob_id node index subindex [data ...]}
// confirmations
// {SDOC UPLOAD srvcli_cob_id clisrv_cob_id node index subindex [data ...]}
// {SDOC DOWNLOAD srvcli_cob_id clisrv_cob_id node index subindex}
gavlfsm_item_t *create_gavlfsm(int fd, const char *str, int *eaten_chars)
{
    int n, l = 0;
    unsigned srv_port, cli_port, node, index, subindex;
    int is_uploader = 0;
    gavlfsm_item_t *gavlfsm;
    vcasdo_fsm_t *fsm;

    *eaten_chars = -1;
    // create_gavlfsm creates fsm only if no fsm exists for this port & node
    
    if((n = vca_strmatch(str, "UPLOAD~")) > 0) {
        is_uploader = 1;
    }
    else if((n = vca_strmatch(str, "DOWNLOAD~")) > 0) {
        is_uploader = 0;
    }
    else {
        mylog(LOG_ERR, "create_gavlfsm::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;

    // create gavlfsm
    gavlfsm = (gavlfsm_item_t*)malloc(sizeof(gavlfsm_item_t));
    if(!gavlfsm) fatal_error("can't allocate new SDO FSM\n");
    
    fsm = &(gavlfsm->fsm);
    vcasdo_init_fsm(fsm, srv_port, cli_port, node);
    fsm->index = index;
    fsm->subindex = subindex;
    fsm->is_uploader = is_uploader;
    // reload srv_port & cli_port from fsm
    // str can contain 0 0 for default values
    srv_port = fsm->srvcli_cob_id;
    cli_port = fsm->clisrv_cob_id;
    
    gavlfsm->fd = fd;
    gavlfsm->cobid = (word)(srv_port | node);
    //check if port & node allready exists
    if(gavlfsm_insert(&gavlfsm_root, gavlfsm) <= 0) {
        // port & node exists
        ul_dbuff_t dbuff;
        mylog(LOG_ERR, "SDO FSM (client port: 0x%x) and node: %u exists, cann't create a second one\n",
                        cli_port, node);
        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
        free(gavlfsm);
        return NULL;
    }

    {
        ul_dbuff_t *db = &fsm->data;
        // load data to the gavlfsm.fsm->data
        if((n = vca_strmatch(str + l, "~[")) > 0) {
            unsigned u;
            l += n;
            while((n = vca_gethex(str + l, &u)) > 0) {
                ul_dbuff_append_byte(db, (byte)u);
                l += n;
            }
        }
        if((n = vca_strmatch(str + l, "~]")) > 0) l += n;

        mylog(LOG_DEB, "new SDO FSM (%p) created: srvcli_cob_id: 0x%x clisrv_cob_id: 0x%x node: %u\n", 
                    fsm, fsm->srvcli_cob_id, fsm->clisrv_cob_id, fsm->node);
        ul_dbuff_log_hex(db, LOG_DEB);
        *eaten_chars = l;
    }
    return gavlfsm;
}
//----------------------------------------------------
void delete_client(int fd)
{
    client_item_t *c;
    gavlfsm_item_t *gf;
    // delete client on fd
    mylog(LOG_DEB, "closing fd %d\n", fd);
    close(fd);
    mylog(LOG_MSG, "removing client on fd %d\n", fd);
    c = client_find(&client_root, &fd);
    if(c) {
        client_delete(&client_root, c);
        free(c);
    }
    
    // delete all fsm assigned to this fd
    for(gf=gavlfsm_first(&gavlfsm_root); gf; gf=gavlfsm_next(&gavlfsm_root, gf)) {
        if(gf->fd == fd) {
            //gavlfsm_delete(&gavlfsm_root, gf);
            destroy_gavlfsm(gf, 1); // unlink
        }
    }
}
//----------------------------------------------------
void clean_up(void)
{
    client_item_t *item;
    // 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
    for(item=client_first(&client_root); item; item = client_next(&client_root, item)) {
        delete_client(item->fd);
    }
    // remove all FSMs
    // all fsms should be deleted with clients
    if(!gavlfsm_is_empty(&gavlfsm_root)) {
        mylog(LOG_ERR, "clean_up() : FSM tree is not empty and should be.\n");
        //while(gavlfsm_delete_first(&gavlfsm_root));
    }
}
//----------------------------------------------------
static void send_to_can(canmsg_t *msg)
{
    char buff[64];
    if(vca_h2fd(canhandle) <= 0) return;
    vca_msg2str(msg, buff, 64);
    mylog(LOG_INF, "sent to CAN: %s\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;

    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 timeout = {.tv_sec = 1, .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
            gavlfsm_item_t *gavlfsm;
            struct timeval t;
            gettimeofday(&t, NULL);
            for(gavlfsm=gavlfsm_first(&gavlfsm_root); gavlfsm; gavlfsm = gavlfsm_next(&gavlfsm_root, gavlfsm)) {
                vcasdo_fsm_t *fsm = &gavlfsm->fsm;
                int d = subtimeval(&t, &(fsm->last_activity));
                if(d > SDO_TIMEOUT) {
                    ul_dbuff_t dbuff;
                    mylog(LOG_ERR, "SDO TIMEOUT fsm(%p)\n", fsm);
                    fsm->state = sdofsmError;
                    fsm->err_no = sdofsmErrTimeout;
                    ul_dbuff_init(&dbuff, 0);
                    sdo2str(&dbuff, fsm);
                    mylog(LOG_ERR, "destroying active FSM (%p)\n", fsm);
                    if(gavlfsm->fd > 0) write(gavlfsm->fd, dbuff.data, dbuff.len); //send also terminating zero
                    destroy_gavlfsm(gavlfsm, 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_item_t *nc = (client_item_t*)malloc(sizeof(client_item_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;
                client_insert(&client_root, nc);
                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 refused = 1;
	    gavlfsm_item_t *gavlfsm;
            vca_rec_msg_seq(canhandle, &readmsg, 1);
            // scan all active FSMs
            gavlfsm = gavlfsm_find(&gavlfsm_root, &(readmsg.id));
            if(gavlfsm) {
	        vcasdo_fsm_t *fsm;
                #ifdef _DEBUG
                char msg[100];
                vca_msg2str(&readmsg, msg, 100);
                mylog(LOG_INF, "got registered msg from CAN: %s\n", msg);
                #endif
                fsm = &gavlfsm->fsm;
                if(vcasdo_fsm_taste_msg(fsm, &readmsg)) {
                    client_item_t *c;
                    refused = 0;
                    // find appropriate client
                    c = client_find(&client_root, &(gavlfsm->fd));
                    if(!c) fatal_error("cann't find client with fd %d\n", gavlfsm->fd);
                    if(fsm->state == sdofsmRun) {
                        mylog(LOG_DEB, "SDO transfer running\n");
                        if(fsm->out_msg.length) send_to_can(&fsm->out_msg);
                    }
                    else {
                        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 == sdofsmError) {
                            // 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, "sending 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_gavlfsm(gavlfsm, 1); // destroy and unlink
                    }
                }
            }
            if(refused) {
                char msg[100];
                client_item_t *client;
                int str_msg_len;
                /* write msg to the string */
                str_msg_len = vca_msg2str(&readmsg, msg, 100);
                //mylog(LOG_DEB, "getting message: %s from CAN\n", msg);
                /* send message to all clients */
                for(client=client_first(&client_root); client; client = client_next(&client_root, client)) {
                    int fd = client->fd;
                    mylog(LOG_INF, "sending message to fd %i: %s\n", fd, msg);
                    write(fd, msg, str_msg_len + 1); //send also terminating zero
                    //sleep(1); 
                }
            }
        }
        else {
            /* client activity */
            int found = 0;
            client_item_t *client;
            for(client=client_first(&client_root); client; client = client_next(&client_root, client)) {
                int fd = client->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) {
			    const char *buff;
                            // if you got the whole message append terminating 0 & process it
                            ul_dbuff_append_byte(&rdbuff, '\0');
                            ul_dbuff_trim(&rdbuff);
                            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) {
			        gavlfsm_item_t *gavlfsm;
                                int l = n;
                                mylog(LOG_INF, "--------------------------------------------------------\n");
                                mylog(LOG_INF, "got SDO request %s\n", buff);
                                gavlfsm = create_gavlfsm(fd, buff + l, &n); // create and link
                                if(gavlfsm) {
                                    vcasdo_fsm_t *fsm = &gavlfsm->fsm;
                                    if(fsm->is_uploader) vcasdo_fsm_upload1(fsm);
                                    else                 vcasdo_fsm_download1(fsm, &fsm->data);
                                    // send the answer to the CAN
                                    send_to_can(&(fsm->out_msg));
                                }
                            }
                            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();
}