#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.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 <pthread.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 "vca_od.h"
#include "vca_pdo.h"

#include "vca_hwmod.h"

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

//#define DEFAULT_SRVCLI_PORT 0x580
//#define DEFAULT_CLISRV_PORT  0x600

static vcaod_root_t od_root;
static vcaPDOProcessor_t pdo_processor;
static vca_handle_t canhandle = 0;
static int candevice_state = -1;
static char *eds_filename = NULL;
static char candev_name[] = "/dev/can0";
static int syncPeriod = 0; // us
static int syncCOB_ID = 0x80;

//static int terminate_threads = 0;
static pthread_t timer_thread = 0;
static void* timerThreadFn(void *threadid);

static pthread_t hwmodule_thread = 0;

static const char *sdo_request_names[] __attribute__ ((unused)) = {
    "DOWNLOAD_SEGMENT_R", "INIT_DOWNLOAD_R", "INIT_UPLOAD_R", "UPLOAD_SEGMENT_R", "ABORT"
};
static const char *sdo_confirmation_names[] = {
    "UPLOAD_SEGMENT_C", "DOWNLOAD_SEGMENT_C", "INIT_UPLOAD_C", "INIT_DOWNLOAD_C", "ABORT"
};

void clean_up(void);
//====================== 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

//------------- LOG SUPPORT -----------------
static void myvlog(int level, const char *format, va_list ap)
{
    if(log_is_cont) level |= VCA_LOGL_CONT;
    vca_vlog("canslave", 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);
    clean_up();
    exit(EXIT_FAILURE);
}
//=============================================================
//--------------------------------------------------------------------
static char help_msg[] =
            "USAGE:\n"
            "canslave [OPTION]\n\n"
			"OPTIONS:\n"
			"\t-h\n"
            "\t--help\tthis help screen\n"
			"\t-d\n"
            "\t--dump\tdumps loaded EDS back to log (debugging purposes)\n"
            "\t-n\n"
            "\t--node\tset node ID to n\n"
            "\t-e\n"
            "\t--eds\tEDS file name to load\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-v --verbose same as --log_level 3\n";
            
//=================== GAVL structures ===========================
//-------------------- sdoiface list ------------------
#include"sdoiface.h"

GAVL_CUST_NODE_INT_IMP(sdoifc, sdoifc_root_t, sdoifc_item_t, sdoifc_key_t,
	my_root, my_node, cobid, sdoifc_cmp_fnc)
// tree of active FSMs
sdoifc_root_t sdoifc_root;
//================================================================
//----------------------------------------------------
void sdoiface_init(sdoifc_item_t *iface, int node, unsigned srvcli_cobid, unsigned clisrv_cobid)
{
    if(!iface) fatal_error("init_sdoiface: iface is NULL");
    ul_dbuff_init(&iface->data, 0);
    vcasdo_init_fsm(&(iface->fsm), srvcli_cobid, clisrv_cobid, node);
    iface->cobid = iface->fsm.clisrv_cob_id;
    mylog(LOG_DEB, "sdoiface_init(): init port 0x%x to listen\n", iface->cobid);
}
//----------------------------------------------------
void sdoiface_destroy(sdoifc_item_t *iface, int unlink) 
{
    // delete fsm
    mylog(LOG_DEB, "destroy_sdoiface(): destroying fsm %p\n", iface->fsm);
    ul_dbuff_destroy(&iface->data);
    vcasdo_destroy_fsm(&iface->fsm);
    if(unlink) {
        int ret = sdoifc_delete(&sdoifc_root, iface);
        mylog(LOG_DEB, "removing SDO interface %p from tree, status [%i]\n", iface, ret);
    }
}
//----------------------------------------------------
void sdoiface_destroy_all() 
{
    // delete all SDO fsms
    while(!sdoifc_is_empty(&sdoifc_root)) {
        sdoifc_item_t *it = sdoifc_cut_first(&sdoifc_root);
        if(!it) continue;
        sdoiface_destroy(it, 0);
        free(it);
    }
}
//----------------------------------------------------
void clean_up(void)
{
    //sdoifc_item_t *it;

    mylog(LOG_DEB, "Clean Up ...\n");
    // close CAN
    if(vca_h2fd(canhandle) > 0) {
        mylog(LOG_DEB, "closing CAN\n");
        vca_close_handle(canhandle);
    }
    // destroy all FSMs
    //tady mi to dela segmentation falult !!!
    sdoiface_destroy_all();
    // free OD
    vcaod_od_free(&od_root);
    vcaPDOProcessor_destroy(&pdo_processor);
    
    // join threads
    //terminate_threads = 1;
    mylog(LOG_DEB, "waiting for threads to terminate\n");
    if(hwmodule_thread) {
        pthread_join(hwmodule_thread, NULL);
        mylog(LOG_DEB, "HW module thread: JOINED\n");
    }
    pthread_join(timer_thread, NULL);
    mylog(LOG_DEB, "Timer thread: JOINED\n");
}
//----------------------------------------------------
static int send_to_can(canmsg_t *msg)
{
    char buff[64];

    if(vca_h2fd(canhandle) <= 0) return -1;
    vca_msg2str(msg, buff, 64);
    //mylog(LOG_INF, "sending message %s to the CAN\n", buff);
    mylog(LOG_INF, "sent %s\n", buff);
    return vca_send_msg_seq(canhandle, msg, 1);
}
static int send_to_can_sdo(canmsg_t *msg)
{
    if(vca_h2fd(canhandle) <= 0) return -1;
    mylog(LOG_INF, "sent SDO: %s\n", sdo_confirmation_names[VCA_SDO_GET_COMMAND(msg->data[0])]);
    return send_to_can(msg);
}
//----------------------------------------------------

//====================== candevice NMT states ====================
static void set_candevice_state(int new_state)
{
    static const char *state_names[] = {"STATE_INITIALISING", "STATE_PREOPERATIONAL", "STATE_OPERATIONAL", "STATE_STOPPED"};
    if(candevice_state >= 0) mylog(LOG_DEB, "leaving state %s\n", state_names[candevice_state]);
    candevice_state = new_state;
    mylog(LOG_MSG, "entering state %s\n", state_names[candevice_state]);
}
//----------------------------------------------------
// see CANopen draft 301, page 9-54
/**
 * reset_application - In this state the parameters of the manufacturer
 * specific profile area and of the standardised device profile area are 
 * set to their power-on values. After setting of the power-on values 
 * the state Reset_Communication is entered autonomously.
 */
static void reset_application()
{
    mylog(LOG_INF, "RESET_APPLICATION\n");
    // load EDS
    mylog(LOG_MSG, "Opening EDS: %s\n", eds_filename);
    if(vcaod_load_eds(&od_root, eds_filename)) {
		fatal_error("ERROR opening EDS file '%s'\n", eds_filename);
    }
    vcaPDOProcessor_setOD(&pdo_processor, &od_root);
    // create hw module thread
    if(!hwmodule_thread) {
        vcaHwModule_workingThread_fn_t* fn;
        mylog(LOG_INF, "creating HW module thread\n");
        fn = vcaHwModule_getWorkingThreadFunction();
        if(fn) {
            int ret = pthread_create(&hwmodule_thread, NULL, fn, (void *)NULL);
            if (ret){
                 fatal_error("ERROR creating HW module thread; return code: %d\n", ret);
            }
        }
    }
    // create timer thread
    if(!timer_thread) {
        int ret;
        mylog(LOG_INF, "creating timer thread\n");
        ret = pthread_create(&timer_thread, NULL, timerThreadFn, (void *)NULL);
        if (ret){
             fatal_error("ERROR creating timer thread; return code: %d\n", ret);
        }
    }
}
//----------------------------------------------------
/**
 * reset_communication - In this state the parameters of 
 * the communication profile area are set to their power-on values. 
 * After this the state Initialising is entered autonomously.
 */
static void reset_communication()
{
    sdoifc_item_t *sdoport1 = (sdoifc_item_t*)(malloc(sizeof(sdoifc_item_t)));
    
    mylog(LOG_INF, "RESET_COMMUNICATION\n");
    // trivial case, GAVL contains only 1 item,
    // I thing, we should support more than 1 SDO port generaly
    // All SDO ports are in GAVL sorted by their COBID
    sdoiface_init(sdoport1, pdo_processor.node_id, 0, 0);
    sdoiface_destroy_all();
    sdoifc_insert(&sdoifc_root, sdoport1);
}
//----------------------------------------------------
/**
 * 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");
}
//----------------------------------------------------
static void enter_state_init(int entry_point)
{
    mylog(LOG_INF, "entering state INIT_DEVICE\n");
    //if(entry_point < 1) entry_point = 1;
    switch(entry_point) {
        default:
        case 1: reset_application();
        case 2: reset_communication();
        case 3: init_device();
    }
    set_candevice_state(candevStateInitializing);
}
//----------------------------------------------------
static void enter_state_preoperational()
{
    mylog(LOG_INF, "entering state PRE-OPERATIONAL\n");
    // create PDO structures
    if(vcaPDOProcessor_createPDOList(&pdo_processor)) {
		fatal_error("ERROR creating PDO structures\n");
    }
    vcaPDOProcessor_makeDinfoLinks(&pdo_processor);
    // init SYNC structures
    {
        int l;
        uint32_t abort_code;
        uint32_t val;
        syncPeriod = 0;
        syncCOB_ID = 0;
        // get SYNC COB_ID
        mylog(LOG_DEB, "Reading SYNC object definition.\n");
        l = vcaod_get_value(&od_root, 0x1005, 0, &val, sizeof(val), &abort_code);
        mylog(LOG_TRASH, "\t1005: %x\n", val);
        if(l < 0) mylog(LOG_ERR, "ERROR read SYNC COB_ID: %s\n", vcasdo_abort_msg(abort_code));
        else if((val & (1<<30))) {
            // device generates SYNC object
            syncCOB_ID = val & 0x1FFFFFFF;
            // get SYNC period
            l = vcaod_get_value(&od_root, 0x1006, 0, &val, sizeof(val), &abort_code);
            mylog(LOG_TRASH, "\t1006: %i\n", val);
            if(l < 0) mylog(LOG_ERR, "ERROR read SYNC period: %s\n", vcasdo_abort_msg(abort_code));
            else syncPeriod = val;
        }
        if(syncCOB_ID == 0) syncPeriod = 0;
    }
    mylog(LOG_MSG, "SYNC COB_ID: %x, SYNC period: %i\n", syncCOB_ID, syncPeriod);
    set_candevice_state(candevStatePreoperational);
}
//----------------------------------------------------
//static void enter_state_operational() __attribute__ ((unused));
static void enter_state_operational()
{
    mylog(LOG_INF, "entering state OPERATIONAL\n");
    set_candevice_state(candevStateOperational);
}
//----------------------------------------------------
static void enter_state_stopped()  __attribute__ ((unused));
static void enter_state_stopped()
{
    mylog(LOG_INF, "entering state STOPPED\n");
    set_candevice_state(candevStateStopped);
}
//================================================================

//----------------------------------------------------

/*--- 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 help_opt = 0, dump_opt = 0;
    int ret;

    struct sigaction act;

    #ifdef _DEBUG
    vca_log_cutoff_level = LOG_DEB;
    #else
    vca_log_cutoff_level = LOG_MSG;
    #endif
    
    pdo_processor.node_id = 1;
    /* parse cmd line arguments */
    while(1) { 
        static struct option long_options[] = {
            {"verbose", no_argument, 0, 'v'},
            {"node", required_argument, 0, 'n'},
            {"eds", required_argument, 0, 'e'},
            {"log_level", required_argument, 0, 'g'},
            {"dump", no_argument, 0, 'd'},
            {"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, "n:e:g:hvd", 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);
                break;
            case 'v':
                vca_log_cutoff_level = LOG_INF;
                break;
            case 'e':
                eds_filename = optarg;
                break;
            case 'n':
                pdo_processor.node_id = atoi(optarg);
                break;
            case 'd':
                dump_opt = 1;
                break;
            case 'h':
                help_opt = 1;
                break;
            case '?':
                /* getopt_long already printed an error message. */
                break;
            default:
                mylog(LOG_FATAL, "cmd line parsing error\n");
                exit(EXIT_FAILURE);
        }
    }
    if(help_opt) {
        printf(help_msg);
        exit(EXIT_SUCCESS);
    } 

    printf("CANSLAVE - CAN slave\n");

    if(!eds_filename) {
        printf("\nYou should specify EDS file name\n");
        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, "node ID: %i\n", pdo_processor.node_id);
    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);
	/*---------------------------------------*/
    
    if(dump_opt) {
        // load EDS
        mylog(LOG_MSG, "Opening EDS: %s\n", eds_filename);
        if(vcaod_load_eds(&od_root, eds_filename)) {
            fatal_error("ERROR opening EDS file '%s'\n", eds_filename);
        }
        vca_log_cutoff_level = LOG_DEB;
        vcaod_dump_od(&od_root);
        exit(EXIT_SUCCESS);
    }

    /* open can device */
    mylog(LOG_MSG, "Opening CAN driver: %s\n", candev_name);
	if (vca_open_handle(&canhandle, candev_name, NULL, 0) != VCA_OK) {
		perror("open can driver");
		mylog(LOG_FATAL, "Error opening %s\n", candev_name);
		exit(1);	
	}  
    
    pdo_processor.send_to_can_fnc = send_to_can;
    
    enter_state_init(0);	
    enter_state_preoperational();	
    enter_state_operational();
    
    do {
        while(1) {
            // read CAN driver loop
            struct canmsg_t readmsg;
            ret = vca_rec_msg_seq(canhandle, &readmsg, 1);
            if(ret <= 0) continue;
            
            if(candevice_state == candevStateInitializing) {
                continue;
            }
            if(candevice_state == candevStateStopped) {
                // stop the communication altogether (except node guarding and heartbeat, if active)
                continue;
            }
            if(candevice_state == candevStateOperational) {
                if(readmsg.id == 0x80) {
                    mylog(LOG_DEB, "SYNC object arived\n");
                    continue;
                }
                // check PDO messages
                ret = vcaPDOProcessor_processMsg(&pdo_processor, &readmsg);
                if(!ret) continue;
            }
            if(candevice_state == candevStatePreoperational || candevice_state == candevStateOperational) {
                // check SDO messages
                sdoifc_item_t *ifc = sdoifc_find(&sdoifc_root, &(readmsg.id));
                if(!ifc) {
                    mylog(LOG_DEB, "Unregistered COBID 0x%x\n", readmsg.id);
                }
                else {
                    vcasdo_fsm_t *fsm = &ifc->fsm;
                    mylog(LOG_DEB, "Got registered SDO COBID 0x%x\n", readmsg.id);
                    #ifdef _DEBUG
                    char msg[100];
                    vca_msg2str(&readmsg, msg, 100);
                    mylog(LOG_INF, "got  %s %s\n", sdo_request_names[VCA_SDO_GET_COMMAND(readmsg.data[0])], msg);
                    #endif
                    if(fsm->state == sdofsmIdle) {
                        // start communication
                        int cmd;
                        vcasdo_read_multiplexor(readmsg.data + 1, &fsm->index, &fsm->subindex);
                        cmd = VCA_SDO_GET_COMMAND(readmsg.data[0]);
                        if(cmd == VCA_SDO_INIT_UPLOAD_R) {
                            uint32_t abort_code;
                            ul_dbuff_t *db = &fsm->data;
                            int l;
                            mylog(LOG_INF, "--------------------------------------------------------\n");
                            mylog(LOG_DEB, "try to load value [%04x:%02x] from OD\n", fsm->index, fsm->subindex);
                            l = vcaod_get_value(&od_root, fsm->index, fsm->subindex, db->data, db->len, &abort_code);
                            if(l > (int)db->len) {
                                ul_dbuff_set_len(db, l);
                                l = vcaod_get_value(&od_root, fsm->index, fsm->subindex, db->data, db->len, &abort_code);
                            }
                            if(l > (int)db->len) {
                                mylog(LOG_ERR, "[%04x:%02x] Cann't load whole object data (size: %i) from OD to dbuff (dbuff size: %i),"
                                               "data vill be truncated (%i)\n", fsm->index, fsm->subindex, l, db->len, db->len);
                                l = db->len;
                            }
                            if(l <= 0) {
                                mylog(LOG_ERR, "[%04x:%02x] not found, ABORTING transfer\n", fsm->index, fsm->subindex);
                                vcasdo_fsm_abort(fsm, abort_code);
                            } 
                            else {
                                ul_dbuff_log_hex(db, LOG_DEB);
                                mylog(LOG_DEB, "LOAD OK [%04x:%02x] %i pcs of data moved OD -> FSM\n", fsm->index, fsm->subindex, l);
                                vcasdo_fsm_run(fsm);
                            }
                        }
                        else if(cmd == VCA_SDO_INIT_DOWNLOAD_R) {
                            mylog(LOG_INF, "--------------------------------------------------------\n");
                            mylog(LOG_INF, "Got INIT_DOWNLOAD request [%04x:%02x]\n", fsm->index, fsm->subindex);
                            vcasdo_fsm_run(fsm);
                        }
                    }
                    if(vcasdo_fsm_taste_msg(fsm, &readmsg)) {
                        if(fsm->state == sdofsmDone) {
                            // SDO transfer complete
                            mylog(LOG_INF, "SDO transfer done\n");
                            if(!fsm->is_uploader) {
                                // store downloaded data to OD
                                uint32_t abort_code;
                                ul_dbuff_t *db = &fsm->data;
                                int l;
                                ul_dbuff_log_hex(db, LOG_DEB);
                                mylog(LOG_DEB, "Try to store downloaded data of [%04x:%02x] to the OD\n", fsm->index, fsm->subindex);
                                l = vcaod_set_value(&od_root, fsm->index, fsm->subindex, db->data, db->len, &abort_code);
                                if(l < 0) {
                                    mylog(LOG_ERR, "[%04x:%02x] ERROR '%s'\n", fsm->index, fsm->subindex, vcasdo_abort_msg(abort_code));
                                    vcasdo_fsm_abort(fsm, abort_code);
                                }
                                else {
                                    mylog(LOG_DEB, "STORE OK [%04x:%02x] %i pcs of data moved FSM -> OD\n", fsm->index, fsm->subindex, l);
                                }
                            }
                            ul_dbuff_set_len(&fsm->data, 0);
                        }
                        else if(fsm->state == sdofsmAbort) {
                            // SDO transfer aborted
                            mylog(LOG_MSG, "SDO transfer ABORTED: error %x '%s'\n", fsm->err_no, vcasdo_abort_msg(fsm->err_no));
                        }
                        else if(fsm->state == sdofsmError) {
                            // SDO transfer error
                            mylog(LOG_MSG, "SDO transfer ERROR: error %x '%s'\n", fsm->err_no, vcasdo_abort_msg(fsm->err_no));
                        }
                        else if(fsm->state == sdofsmRun) {
                            mylog(LOG_DEB, "SDO transfer RUNNING\n");
                        }
                        else {
                            // unexpected state
                            mylog(LOG_ERR, "SDO FSM unexpected state: %i\n", fsm->state);
                            fsm->out_msg.length = 0;
                        }
                        
                        if(fsm->out_msg.length > 0) {
                            send_to_can_sdo(&fsm->out_msg);
                        }
                        
                        if(fsm->state != sdofsmRun) {
                            vcasdo_fsm_idle(fsm);
                        }
                    }
                    else {
                        mylog(LOG_DEB, "message REFUSED\n");
                    }
                }
            }
        } 
    } while(0);
    clean_up();
    exit(EXIT_FAILURE);
}
//==========================================================================================
void* timerThreadFn(void *threadid)
{
    struct timeval last_wakeup;
    struct timespec timeout;
    gettimeofday(&last_wakeup, NULL);
    if(syncPeriod) while(1) {
        struct timeval t;
        int d;
        gettimeofday(&t, NULL);
        d = vca_subtimeval(&t, &last_wakeup);
        d = syncPeriod - d;
        if(d <= 0) d = 1; // A.S.A.P.
        else d *= 1000; //us->ns
        mylog(LOG_TRASH, "TimerThread: next wake up after %i us\n", d);
        timeout.tv_sec = d / 1000000000; 
        timeout.tv_nsec = d % 1000000000;
        nanosleep(&timeout, NULL);
        gettimeofday(&last_wakeup, NULL);
        mylog(LOG_TRASH, "TimerThread: wake up\n");
        
        // send SYNC
        if(candevice_state == candevStateOperational) {
            canmsg_t msg;
            mylog(LOG_TRASH, "sending SYNC\n");
            memset(&msg, 0, sizeof(msg));
            msg.id = syncCOB_ID;
            send_to_can(&msg);
        }
    }
    pthread_exit(NULL);
}
//==========================================================================================