#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"


//static int candevice_state = -1;
static vca_handle_t canhandle = 0;
static int in_pipe = -1; 
static int out_pipe = -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("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);
}
//=============================================================

// 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[] =
            "USAGE:\n"
            "canmaster [OPTION]\n\n"
			"OPTIONS:\n"
			"\t-h\n"
            "\t--help\tthis help screen\n"
			"\t--sync n\n"
            //"\t\tn - SYNC message period in us. If n==0 SYNC is not generated.\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 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(const char *msg, int len)
{
    if(out_pipe < 0) return;
    mylog(LOG_INF, "sending message to monitor: \n\t'%s'\n", msg);
    write(out_pipe, msg, len);
    write(out_pipe, "\n", 1); 
}
//----------------------------------------------------
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);
    }
}
//----------------------------------------------------
// 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(const char *str, int *eaten_chars)
{
    // create_gavlfsm creates fsm only if no fsm exists for this port & node
    int n, l = 0;
    unsigned srv_port, cli_port, node, index, subindex;
    int is_uploader = 0;
    gavlfsm_item_t *gavlfsm;
    vcasdo_fsm_t *fsm;
    ul_dbuff_t *db;
    
    *eaten_chars = -1;
    
    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);
        send_to_monitor(dbuff.data, dbuff.len-1); //do not send terminating zero
        free(gavlfsm);
        return NULL;
    }

    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;
}
//================================================================
/**
 * 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";
    const char *can_device_name = "/dev/can0";
    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'},
                //{"sync", required_argument, &syncPeriod, 0},
                {"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, "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 '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_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) { // 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;
                    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);
                    send_to_monitor(dbuff.data, dbuff.len-1);
                    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(in_pipe, &testfds)) { /* monitor request */
            int n, nread;
            int old_len = readdb.len;
            int new_len;
            ioctl(in_pipe, FIONREAD, &nread);
            if(nread == 0) continue;

            ul_dbuff_set_len(&msgdb, 0);
            new_len = old_len + nread;
            if(ul_dbuff_set_len(&readdb, new_len) == new_len) {
                nread = read(in_pipe, readdb.data + old_len, nread);
            }
            else fatal_error("Cann't allocate memory for client data\n");
            // find all {...} sequences in received data
            do {
                // try to find '}' in new data to check out the end of message
                ul_dbuff_cut_delimited(&readdb, &msgdb, '\n', '\0');
                if(msgdb.len > 0) {
                    const char *buff;
                    ul_dbuff_trim(&msgdb);
                    // skip empty lines
                    if(msgdb.len == 0) continue;

                    ul_dbuff_append_byte(&msgdb, '\0');
                    buff = msgdb.data;
                    mylog(LOG_INF, "Monitor says '%s'\n", msgdb.data);                
                
                    if(vca_strmatch(buff, "{CANDTG") > 0) {
                        vca_str2msg(&canmsg, buff);
                        send_to_can(&canmsg);
                    }
                    else if((n = vca_strmatch(buff, "{SDOR~")) > 0) {
                        int l = n;
                        gavlfsm_item_t *gavlfsm;
                        mylog(LOG_INF, "--------------------------------------------------------\n");
                        mylog(LOG_INF, "got SDO request %s\n", buff);
                        gavlfsm = create_gavlfsm(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));
                        }
                    }
                }
            } while(msgdb.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
                        sdo2str(&dbuff, fsm);
                        send_to_monitor(dbuff.data, dbuff.len-1); // don't send terminating '\0'
                        //mylog(LOG_MSG, "sending confirmation SDO to fd %i: %s\n", c->fd, dbuff.data);
                        // free fsm
                        destroy_gavlfsm(gavlfsm, 1); // destroy and unlink
                    }
                }
            }
            if(refused) {
                char msg[100];
                int str_msg_len;
                /* write msg to the string */
                str_msg_len = vca_msg2str(&canmsg, msg, 100);
                send_to_monitor(msg, str_msg_len);
            }
        }
    }
    clean_up();
}