#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <fcntl.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_canopen.h"
#include "vca_od.h"
#include "ul_dbuff.h"
#include "vcasdo_fsm.h"
#include "vcasdo_msg.h"

#include "ul_gavl.h"
#include "ul_gavlcust.h"

#include "fcpickle_impl.h"
#include "candtgmsg.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"

//static int candevice_state = -1;
static vca_handle_t canhandle = 0;
static int in_pipe = -1; 
static int out_pipe = -1; 
static char *can_device_name = "/dev/can0";

//====================== 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("canmaster", 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);
}
//=============================================================
//--------------------------------------------------------------------
static int sdo2msg(ul_dbuff_t *dbuff, const vcasdo_fsm_t *fsm)
{
    SDOConfirmMsg_t *cmsg; // base class
    SDOUploadConfirmMsg_t umsg;   
    SDODownloadConfirmMsg_t dmsg;   
    
    SDOUploadConfirmMsg_init(&umsg);
    SDODownloadConfirmMsg_init(&dmsg);

    if(fsm->is_uploader) {
        cmsg = &(umsg.super);
    }
    else { 
        cmsg = &(dmsg.super);
    }
    
    cmsg->super.srvcliCobId = fsm->srvcli_cob_id;
    cmsg->super.clisrvCobId = fsm->clisrv_cob_id;
    cmsg->super.node = fsm->node;
    cmsg->super.index = fsm->index;
    cmsg->super.subindex = fsm->subindex;
    if(fsm->state == sdofsmAbort) {
        uint32_t code = 0;
        byte *data = fsm->out_msg.data;
        int i;
        for(i=0; i<4; i++) code = (code << 8) + data[7 - i];
        cmsg->code = code;
        cmsg->type = 2;
        cmsg->errmsg = vcasdo_abort_msg(code);
    }
    else if(fsm->state == sdofsmError) {
        cmsg->type = 1;
        cmsg->code = fsm->err_no;
        cmsg->errmsg = vcasdo_abort_msg(cmsg->code);
    } 
    else { 
        cmsg->type = 0;
    }

    if(fsm->is_uploader) {
        int ret;
        umsg.data = fsm->data.data;
        umsg.data_arraylen = fsm->data.len;
        ret = SDOUploadConfirmMsg_getPacketBufferSize(&umsg);
        if(ret < 0) {
            mylog(LOG_ERR, "sdo2msg() SDOUploadConfirmMsg_getPacketBufferSize() ERROR(%i)\n", ret);
            ul_dbuff_set_len(dbuff, 0);
        }
        ul_dbuff_set_len(dbuff, ret);
        ret = SDOUploadConfirmMsg_toNet(&umsg, dbuff->data, dbuff->len);
        if(ret < 0) {
            mylog(LOG_ERR, "sdo2msg() SDOUploadConfirmMsg_toNet() ERROR(%i)\n", ret);
            ul_dbuff_set_len(dbuff, 0);
        }
    }
    else {
        int ret;
        ul_dbuff_set_len(dbuff, SDODownloadConfirmMsg_getPacketBufferSize(&dmsg));
        ret = SDODownloadConfirmMsg_toNet(&dmsg, dbuff->data, dbuff->len);
        if(ret < 0) {
            mylog(LOG_ERR, "sdo2msg() SDODownloadConfirmMsg_toNet() ERROR(%i)\n", ret);
            ul_dbuff_set_len(dbuff, 0);
        }
    }

    return dbuff->len > 0;
}
//==============================================================
//--------------------------------------------------------------------
static char help_msg[] =
            "USAGE:\n"
            "canmaster [OPTION]\n\n"
			"OPTIONS:\n"
			"\t-h\n"
            "\t--help\tthis help screen\n"
            "\t-d\n"
            "\t--candev\tCAN device name (default /dev/can0)\n"
            "\t-i named pipe\t--in_pipe named pipe\n"
            "\t-o named pipe\t--out_pipe named pipe\n"
            "\t\tname of pipe for communication with monitoring program\n"
            "\t\tdefault names are /tmp/canmond/candev-in and /tmp/canmond/candev-out\n"
            "\t\tin_pipe is name of pipe where monitor feeds my input\n"
            "\t\tout_pipe is name of pipe where i send answer to the monitoring application\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"
			"\t-v --verbose same as --log_level 3\n";
            
//----------------------------------------------------
static void log_dbuff(ul_dbuff_t *buf, int log_level)
{
    int i;
    log_level |= VCA_LOGL_CONT;
    mylog(log_level, "[");
    for(i=0; i<buf->len; i++) {
        char c = buf->data[i];
        if(i > 0)   vca_log("dbuff", log_level, " ");
        if(c > ' ') vca_log("dbuff", log_level, " %c", c);
        else        vca_log("dbuff", log_level, "%02x", (byte)c);
    }
    vca_log("dbuff", log_level, "]\n");
}
//----------------------------------------------------
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);
}
//----------------------------------------------------
static void send_to_monitor(ul_dbuff_t *msg)
{
    if(out_pipe < 0) return;
    mylog(LOG_INF, "sending message to monitor: \n\t");
    log_dbuff(msg, LOG_INF);
    write(out_pipe, msg->data, msg->len);
}
//----------------------------------------------------
int open_pipe(const char *pipename, int mode)
{
    int res;
    mylog(LOG_INF, "openning '%s' in mode %i: ", pipename, mode);
    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;
}
//=================== GAVL structures ===========================
//-------------------- 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);
}
//----------------------------------------------------
static void gavlfsm_remove_all() 
{
    // remove all active FSMs
    if(!gavlfsm_is_empty(&gavlfsm_root)) {
        gavlfsm_item_t *fsm;
        while((fsm = gavlfsm_cut_first(&gavlfsm_root))) destroy_gavlfsm(fsm, 0);
    }
}
//----------------------------------------------------
gavlfsm_item_t *create_gavlfsm(ul_dbuff_t *packet)
{
    // create_gavlfsm creates fsm only if no fsm exists for this port & node
    unsigned srv_port, cli_port;//, node, index, subindex;
    int is_uploader = 0;
    gavlfsm_item_t *gavlfsm;
    vcasdo_fsm_t *fsm;
    int ret;
    
    // all structures have a same base fields
    SDOMsgBase_t *msg = NULL;
    SDOUploadRequestMsg_t umsg;
    SDODownloadRequestMsg_t dmsg;

    const char *class_name = fcpickle_getClassName(packet->data, packet->len);
    if(strcmp(class_name, SDO_UP_REQ_CLASS) == 0) {
        mylog(LOG_TRASH, "%s() Upload request '%s' unpickling packet.\n", __func__, class_name);
        ret = SDOUploadRequestMsg_fromNet(&umsg, packet->data, packet->len);
        msg = &(umsg.super);
        is_uploader = 1;
    }
    else if(strcmp(class_name, SDO_DN_REQ_CLASS) == 0) {
        mylog(LOG_TRASH, "%s() Download request '%s' unpickling packet.\n", __func__, class_name);
        ret = SDODownloadRequestMsg_fromNet(&dmsg, packet->data, packet->len);
        msg = &(dmsg.super);
        is_uploader = 0;
    }
    else {
        mylog(LOG_INF, "%s() unknown message object '%s', no FSM created.\n", __func__, class_name);
        return NULL;
    }
    if(ret < 0) {
        mylog(LOG_ERR, "%s() unpickling object ERROR %i\n", __func__, ret);
        return NULL;
    }
    mylog(LOG_TRASH, "%s() '%s' successfully unpickled.\n", __func__, class_name);
    
    // 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, msg->srvcliCobId, msg->clisrvCobId, msg->node);
    fsm->index = msg->index;
    fsm->subindex = msg->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->cobid = (word)(srv_port | msg->node);
    
    //check if port & node allready exists
    if(gavlfsm_insert(&gavlfsm_root, gavlfsm) <= 0) {
        // port & node exists
        ul_dbuff_t dbuff;
        ul_dbuff_init(&dbuff, 0);
        mylog(LOG_ERR, "SDO FSM (client port: 0x%x) and node: %u exists, cann't create a second one\n",
                        cli_port, msg->node);
        fsm->state = sdofsmError;
        fsm->err_no = sdofsmErrFsmExists;
        if(sdo2msg(&dbuff, fsm)) send_to_monitor(&dbuff);
        free(gavlfsm);
        return NULL;
    }

    {
        // load data to the gavlfsm.fsm->data
        ul_dbuff_t *db = &fsm->data;
        if(!is_uploader) {
            int i;
            for(i=0; i<dmsg.data_arraylen; i++) ul_dbuff_append_byte(db, dmsg.data[i]);
        }
    
        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);
    }
    return gavlfsm;
}
//================================================================
/**
 * init_device - This is the first sub-state the device enters after power-on or hardware reset.
 * After finishing the basic node initialisation the device executes the write boot-up
 * object service and enters the state PRE-OPERATIONAL autonomously.
 */
static void init_device()
{
    mylog(LOG_INF, "INIT_DEVICE\n");
    gavlfsm_remove_all();
}
//================================================================

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 pipes
    if(in_pipe >= 0) {
        mylog(LOG_DEB, "closing input pipe\n");
        close(in_pipe);
    }
    if(out_pipe >= 0) {
        mylog(LOG_DEB, "closing output pipe\n");
        close(out_pipe);
    }
    gavlfsm_remove_all();
}
//----------------------------------------------------
// 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[])
{
    const char *in_pipe_name = "/tmp/canmond/candev_in";
    const char *out_pipe_name = "/tmp/canmond/candev_out";
    int result;
    fd_set readfds, testfds;
    int help_opt = 0;

	struct canmsg_t canmsg;
	struct sigaction act;

    //#define RD_BUFF_INIT_LEN 128
    ul_dbuff_t readdb, msgdb;
    ul_dbuff_init(&readdb, 0); //ul_dbuff_set_capacity(&readdb, RD_BUFF_INIT_LEN);
    ul_dbuff_init(&msgdb, 0);
    
	printf("CANMASTER - CANopen master\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'},
                {"candev", required_argument, 0, 'd'},
                {"verbose", required_argument, 0, 'v'},
                {"in_pipe", required_argument, 0, 'i'},
                {"out_pipe", 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, "d:p:g:hv", long_options, &option_index);
            /* Detect the end of the options. */
            if (c == -1) break;
            switch (c) {
                case 0:
                    /*
                    if (long_options[option_index].flag == &syncPeriod) {
                        syncPeriod = atoi(optarg);
                    }
                    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 'i':
                    in_pipe_name = optarg;
                    break;
                case 'd':
                    can_device_name = optarg;
                    break;
                case 'o':
                    out_pipe_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");
        }
    }

    mylog(LOG_INF, "input pipe: %s\n", in_pipe_name);
    mylog(LOG_INF, "output pipe: %s\n", out_pipe_name);
    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 driver */
    mylog(LOG_INF, "opening: %s\n", can_device_name);
	if (vca_open_handle(&canhandle, can_device_name, NULL, 0) != VCA_OK) {
		perror(can_device_name);
		mylog(LOG_FATAL, "Error opening %s\n", can_device_name);
		//exit(1);	
	}
    
    // open monitor pipes
    in_pipe = open_pipe(in_pipe_name, O_RDONLY | O_NONBLOCK);
    out_pipe = open_pipe(out_pipe_name, O_WRONLY);// | O_NONBLOCK);
    //send_to_monitor("hello from canmaster\n", sizeof("hello from canmaster\n"));

    init_device();
    
    FD_ZERO(&readfds);
    if(vca_h2fd(canhandle) > 0) FD_SET(vca_h2fd(canhandle), &readfds);
    if(in_pipe >= 0) FD_SET(in_pipe, &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 = {.tv_sec = 1, .tv_usec = 0};
        testfds = readfds;

        //mylog(LOG_TRASH, "ENTERING SELECT\n");
        result = select(FD_SETSIZE, &testfds, (fd_set *)0, (fd_set *)0, &timeout);
        //mylog(LOG_TRASH, "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) { // timeout
            gavlfsm_item_t *gavlfsm;
            //mylog(LOG_DEB, "timeout\n");
            struct timeval t;
            gettimeofday(&t, NULL);
            // check last activity time of all FSMs
            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));
                // SDO FSM timeout in usec
                #define SDO_TIMEOUT 1000000      
                if(d > SDO_TIMEOUT) {
                    ul_dbuff_t dbuff;
                    ul_dbuff_init(&dbuff, 0);
                    mylog(LOG_ERR, "SDO TIMEOUT fsm(%p)\n", fsm);
                    fsm->state = sdofsmError;
                    fsm->err_no = 0x05040000; //SDO ErrTimeout see vcasdo_msg.c
                    if(sdo2msg(&dbuff, fsm)) send_to_monitor(&dbuff);
                    mylog(LOG_ERR, "destroying active FSM (%p)\n", fsm);
                    destroy_gavlfsm(gavlfsm, 1);
                    ul_dbuff_destroy(&dbuff);
                }
            }
            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(in_pipe, &testfds)) { /* monitor request */
            int i, nread;
            static int packet_len;

            ioctl(in_pipe, FIONREAD, &nread);
            if(nread == 0) continue;

            if(ul_dbuff_set_len(&readdb, nread) == nread) {
                nread = read(in_pipe, readdb.data, nread);
            }
            else fatal_error("Cann't allocate memory for client data\n");
            
            for(i=0; i<readdb.len; i++) {
                unsigned char b = readdb.data[i];
                if(msgdb.len < FCPICKLE_PACKET_HEAD_SIZE) {
                // read packet head
                    ul_dbuff_append_byte(&msgdb, b);
                    if(msgdb.len == FCPICKLE_PACKET_HEAD_SIZE) {
                        fcpickle_packet_head_t head;
                        // check head if it is correct
                        if(fcpickle_getHead(&head, msgdb.data, msgdb.len) < 0) {
                            // incorrect head
                            // shift data in head one byte right and try it again with a next byte
                            int j;
                            for(j=0; j<FCPICKLE_PACKET_HEAD_SIZE-1; j++) msgdb.data[j] = msgdb.data[j+1];
                            msgdb.len--;
                            continue;
                        }
                        packet_len = head.packetSize;
                    }
                    continue;
                }
                // read whole packet
                ul_dbuff_append_byte(&msgdb, b);
                if(msgdb.len < packet_len) continue;
                
                // now I have whole packet
                {
                    const char *class_name = fcpickle_getClassName(msgdb.data, msgdb.len);
                    mylog(LOG_DEB, "Object from master received: %s\n", class_name);
                    if(strcmp(class_name, CAN_DTG_MSG_CLASS) == 0) {
                        int ret;
                        CANDtgMsg_t candtg;
                        CANDtgMsg_init(&candtg);
                        ret = CANDtgMsg_fromNet(&candtg, msgdb.data, msgdb.len);
                        if(ret > 0) {
                            canmsg_t canmsg;
                            memset(&canmsg, 0, sizeof(canmsg));
                            canmsg.id = candtg.id;
                            canmsg.length = candtg.length;
                            memcpy(&canmsg.data, &candtg.data, canmsg.length);
                            send_to_can(&canmsg);
                        }
                    }
                    else {
                        gavlfsm_item_t *gavlfsm = create_gavlfsm(&msgdb); // 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(&msgdb, 0);
                packet_len = 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, &canmsg, 1);
            // scan all active FSMs
            gavlfsm = gavlfsm_find(&gavlfsm_root, &(canmsg.id));
            if(gavlfsm) {
                vcasdo_fsm_t *fsm;
                #ifdef _DEBUG
                char msg[100];
                vca_msg2str(&canmsg, msg, 100);
                mylog(LOG_INF, "got registered msg from CAN: %s\n", msg);
                #endif
                fsm = &gavlfsm->fsm;
                if(vcasdo_fsm_taste_msg(fsm, &canmsg)) {
                    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) {
                            mylog(LOG_INF, "SDO transfer done\n");
                        }
                        else if(fsm->state == sdofsmAbort) {
                            mylog(LOG_MSG, "SDO transfer aborted\n");
                        }
                        else if(fsm->state == sdofsmError) {
                            mylog(LOG_ERR, "SDO transfer error\n");
                        }
                        else {
                            mylog(LOG_ERR, "SDO FSM unexpected state: %i\n", fsm->state);
                        }
                        // send data to the client
                        if(sdo2msg(&dbuff, fsm)) send_to_monitor(&dbuff);
                        //mylog(LOG_MSG, "sending confirmation SDO to fd %i: %s\n", c->fd, dbuff.data);
                        // free fsm
                        destroy_gavlfsm(gavlfsm, 1); // destroy and unlink
                        ul_dbuff_destroy(&dbuff);
                    }
                }
            }
            //if(refused) 
            {
                // send all CAN messages to monitor (monitoring, debugging)
                int len;
                CANDtgMsg_t msg;
                CANDtgMsg_init(&msg);
                len = CANDtgMsg_getPacketBufferSize(NULL); // fixed len message
                if(len > 0) { // this should be allways true
                    ul_dbuff_t dbuff;
                    ul_dbuff_init(&dbuff, 0);
                    ul_dbuff_set_len(&dbuff, len);
                    msg.id = canmsg.id;
                    msg.length = canmsg.length;
                    memcpy(&msg.data, canmsg.data, CAN_MSG_LENGTH);
                    len = CANDtgMsg_toNet(&msg, dbuff.data, dbuff.len);
                    if(len > 0) { // this should be allways true
                        send_to_monitor(&dbuff);
                    }
                    ul_dbuff_destroy(&dbuff);
                }
            }
        }
    }
    clean_up();
}