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

using namespace std;

#include "flist.h"
#include "fstring.h"

#include "can_vca.h"
#include "canmond.h"

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

static int server_sockfd = -1; 
static int candev_in = -1;
static int candev_out = -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
#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("canmond", level, format, ap);
}

static void mylog(int level, const char *format, ...)
{
    //static int cnt;
    //printf("mylog #%i: ", ++cnt);fflush(stdout);
    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 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-i named pipe\t--candev_in named pipe\n"
            "\t-o named pipe\t--candev_out named pipe\n"
            "\t\tname of pipe for communication with monitored CAN device\n"
            "\t\tdefault names are /tmp/canmond/candev-in and /tmp/canmond/candev-out\n"
            "\t\tif pipes don't exist, canmond creates the new ones\n"
            "\t\tcanmond writes data to the candev-in pipe\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";
            
//=================== client list ===========================
struct ClientConnection
{
    int fd;
    FString msg;
    //int sdoSignature; // signature of current SDO request | 4b srvcli port | 4b clisrv port | 7b node |
    // candevice can not serve more requests with the same signature simultaneously
    
    ClientConnection(int _fd) : fd(_fd) {}
    bool operator<(const ClientConnection& c) {return (fd < c.fd);}
    bool operator==(const ClientConnection& c) {return (fd == c.fd);}
    
    //static uint32_t signatureFromSDOMessage(const FString& sdo_msg);
};
//------------------------------------------------------------------------------------
struct ServedSDO
{
    uint32_t signature;
    int clientIndex;
    
    ServedSDO(uint32_t sig, int cli_ix) : signature(sig), clientIndex(cli_ix) {}
    ~ServedSDO() {
        mylog(LOG_DEB, "~ServedSDO(): signature=%x, clientIndex=%i\n", signature, clientIndex);
    }
};
//------------------------------------------------------------------------------------
static uint32_t signatureFromSDOMessage(const FString& sdo_msg)
{
    mylog(LOG_TRASH+1, "ClientConnection::signatureFromSDOMessage('%s'\n", sdo_msg.Str());
    uint32_t sig = 0;
    FString fs = sdo_msg.Trim();
    if(fs.getToken("{CANDTG") < 0) { // not a SDO request have signature 0
        int i = fs.getToken("{!~~!~~"); // skip '{SDOR DOWNLOAD 'and similar
        if(i < 0) throw FException("ClientConnection::signatureFromSDOMessage(): cann't make SDO signature from '%s'", fs.Str());
        fs = fs.Slice(i);
        int clisrv, srvcli, node;
        srvcli = fs.CutHex().toUInt(16);
        clisrv = fs.CutHex().toUInt(16);
        node   = fs.CutHex().toUInt(16);
        //srvcli port default 580 + node
        if(srvcli == 0) srvcli = 0x580 + node;
        //clisrv port default 600 + node
        if(clisrv == 0) clisrv = 0x600 + node;
        sig = srvcli;
        sig = (sig << 16) + clisrv;
    }
    mylog(LOG_TRASH+1, "\tsignature: %08x\n", sig);
    return sig;
}
//-------------------- client list ------------------
FList<ClientConnection> clientList;
//-------------------- served SDO list ------------------
FList<ServedSDO> servedSDOList;
//----------------------------------------------------
// requests
// {SDOR UPLOAD srvcli_port clisrv_port node index subindex}
// {SDOR DOWNLOAD srvcli_port clisrv_port node index subindex [data ...]}
// confirmations
// {SDOC UPLOAD srvcli_port clisrv_port node index subindex [data ...]}
// {SDOC DOWNLOAD srvcli_port clisrv_port node index subindex}
//----------------------------------------------------
void write_to_candevice(const FString& fs)
{
    //FString fs = s + "\n";
    if(candev_in < 0) return;
    mylog(LOG_DEB, "writing to candev pipe: '%s'\n", fs.Str());
    write(candev_in, fs.StrBuff(), fs.Len());
    write(candev_in, "\n", 1);
}
//----------------------------------------------------
void write_to_client(ClientConnection &cc, const FString& fs)
{
    int fd = cc.fd;
    mylog(LOG_DEB, "writing to client socket on fd %i : '%s'\n", fd, fs.Str());
    write(fd, fs.StrBuff(), fs.Len());
    write(fd, "\n", 1);
}
//----------------------------------------------------
void delete_client(int fd)
{
    // delete client on fd
    mylog(LOG_DEB, "closing fd %d\n", fd);
    close(fd);
    // find client
    int ix = clientList.Seek(fd);
    // delete served SDO in any
    if(ix != FEOFLIST) {
        for(int i=0; i<servedSDOList.Cnt(); i++) {
            if(servedSDOList[i].clientIndex == ix) {
                servedSDOList.Remove(i);
                break;
            }
        }
        // delete client
        mylog(LOG_MSG, "destroying client on fd %d\n", fd);
        clientList.Remove(ix);
        //mylog(LOG_DEB, ":(( after destroying client on fd %d\n", fd);
    }
    else {
        mylog(LOG_ERR, "INTERNAL ERROR: delete_client(%i): client not found\n", fd);
    }
}
//----------------------------------------------------
void clean_up(void)
{
    // close server socket
    if(server_sockfd >= 0) {
        mylog(LOG_DEB, "closing server socket\n");
        close(server_sockfd);
    }
    // remove all clients
    for(int i=0; i<clientList.Cnt(); i++) delete_client(clientList[i].fd);

    // close pipes
    if(candev_in >= 0) {
        mylog(LOG_DEB, "closing input pipe\n");
        close(candev_in);
    }
    if(candev_out >= 0) {
        mylog(LOG_DEB, "closing output pipe\n");
        close(candev_out);
    }
}
//----------------------------------------------------
/**
 * check_pipe - check if pipename exists ant try ti create new one if it doesn't
 * Return: 1 if pipe exists, 0 if pipe was created
 */
int check_pipe(const char *pipename)
{
    // create pipe dir if it does'n exist
    FStringList sl;
    sl.fromString(pipename, '/');
    //check if dir exists and create one if it does not
    FString fs, fs2;
    //printf("cnt: %i\n", sl.Cnt());
    for(int i=0; i<sl.Cnt()-1; i++) {
        fs2 = sl[i].Trim();
        //printf("fs2: '%s'\n", fs2.Str());
        // skip double /
        if(i> 0 && !fs2) continue;
        fs += fs2 + "/";
        int res = mkdir(fs.Str(), 0777);
        if(!res) {
            mylog(LOG_DEB, "creating dir '%s': OK\n", fs.Str());
        }
    }
    FString dir = fs, name = sl[-1];
    fs = fs + name;
    if(access(fs.Str(), F_OK) == -1) {
        mylog(LOG_DEB, "creating fifo '%s': \n", fs.Str());
        //int res = mkfifo("/home/fanda/tmp/pipe", 0777);
        int res = mkfifo(fs.Str(), 0777);
        if(res) fatal_error("Cann't create '%s'\n", fs.Str());
        mylog(LOG_DEB | VCA_LOGL_CONT, "OK\n");
        // nevim proc mkfifo nenastavi spravne MODE
        chmod(fs.Str(), 0777);
        return 0;
    }
    else return 1;
    return 0;
}
//----------------------------------------------------
int open_pipe(const char *pipename, int mode)
{
    mylog(LOG_INF, "openning '%s'in mode %i: ", pipename, mode);
    int 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;
}
//----------------------------------------------------

/*--- 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[])
{
    try {
        char *pipe_in_name = "/tmp/canmond/candev_in", *pipe_out_name = "/tmp/canmond/candev_out";
        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];
        //int cnt = 0;
        //char *s;
        int help_opt = 0;
        int port_opt = CANMOND_PORT;
        FString candev_msg;
    
        struct sigaction act;
    
        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'},
                    {"candev-in", required_argument, 0, 'i'},
                    {"candev-out", 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 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 'i':
                        pipe_in_name = optarg;
                        break;
                    case 'o':
                        pipe_out_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");
            }
        }
    
        /*------- register handler on SIGINT signal -------*/
        act.sa_handler = exithandler;
        sigemptyset(&act.sa_mask);
        act.sa_flags = 0;
        sigaction(SIGINT, &act, 0);
        /*---------------------------------------*/
    
        mylog(LOG_INF, "read from CAN device in '%s' pipe\n", pipe_out_name);
        mylog(LOG_INF, "write to CAN device via '%s' pipe\n", pipe_in_name);
        // try to open can device pipes
        mylog(LOG_INF, "waiting on fifos %s, %s\n", pipe_in_name, pipe_out_name);
        check_pipe(pipe_in_name);
        check_pipe(pipe_out_name);
        candev_in = open_pipe(pipe_in_name, O_WRONLY);
        candev_out = open_pipe(pipe_out_name, O_RDONLY | O_NONBLOCK);
    
        mylog(LOG_INF, "listenning port: %i\n", port_opt);
        mylog(LOG_INF, "log level: %i\n", vca_log_cutoff_level);
        /*  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);
        FD_SET(candev_out, &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;
            timeout.tv_sec = 1; timeout.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
                continue;
            }
            if(FD_ISSET(server_sockfd, &testfds)) { /*  request for a new connection */
                FString msg = "HELLO from canmond.\n";
                client_len = sizeof(client_address);
                int fd = accept(server_sockfd, (struct sockaddr *)&client_address, (socklen_t*)&client_len);
                FD_SET(fd, &readfds);
                clientList.Append(new ClientConnection(fd)); clientList.Sort();
                mylog(LOG_MSG, "adding client on fd %d\n", fd);
                write(fd, msg.Str(), msg.Len()); /* without terminating '\0' */
            }
            else if(FD_ISSET(candev_out, &testfds)) { // candevice activity
                int nread;
                ioctl(candev_out, FIONREAD, &nread);
                //printf("%i chars in client pipe\n", nread);
                if(nread <= 0) continue;
                FString fs;
                fs.setLen(nread); 
                read(candev_out, fs.StrBuff(), nread);
                fs = fs.Replace("\n", " ");
                //mylog(LOG_TRASH, "candevice appends:\n\t'%s'\n", fs.Str());
                candev_msg += fs;
                //mylog(LOG_TRASH, "candev_msg: '%s'(%i)\n", candev_msg.Str(), candev_msg.Len());
                FStringList fsl; fsl.fromString(candev_msg, '}', false); // do not strip;
                //for(int i=0; i<fsl.Cnt(); i++) mylog(LOG_TRASH, "\tfsl[%02i] == '%s'\n", i, fsl[i].Str());
                for(int i=0; i<fsl.Cnt()-1; i++) { // fsl contains +1 strings than candev_msg contains '}' chars
                    fs = (fsl[i] + '}').Trim();
                    mylog(LOG_INF, "candevice: '%s'\n", fs.Str());
                    uint32_t sig = signatureFromSDOMessage(fs);
                    if(sig == 0) {
                        // send rough can messagge to all
                        for(int i=0; i<clientList.Cnt(); i++) write_to_client(clientList[i], fs);
                    }
                    else {
                        // find client wth correct signature
                        int i;
                        for(i=0; i<servedSDOList.Cnt(); i++) {
                            mylog(LOG_TRASH, "\tservedSDOList[%i].signature: %x, .clientix: %i, looking for sig: %x\n", i, servedSDOList[i].signature, servedSDOList[i].clientIndex, sig);
                            if(servedSDOList[i].signature == sig) break;
                        }
                        if(i == servedSDOList.Cnt()) {
                            mylog(LOG_ERR, "ERROR: signature 0x%x not found in served SDO list\n", sig);
                        }
                        else {
                            int ix = servedSDOList[i].clientIndex;
                            if(ix >= 0 && ix < clientList.Cnt()) {
                                write_to_client(clientList[ix], fs);
                            }
                            else {
                                mylog(LOG_ERR, "ERROR: signature 0x%08x - client[%i] not found in client list\n", sig, ix);
                            }
                            mylog(LOG_DEB, "removing allready served SDO(%08x, %i)\n", sig, ix);
                            servedSDOList.Remove(i);
                        }
                    }
                }
                if(fsl.Cnt()) candev_msg = fsl[-1];
                else          candev_msg = "";
                mylog(LOG_TRASH, "candev_msg exit: '%s'\n", candev_msg.Str());
            }
            else {/* client activity */
                int cliix = FEOFLIST;
                for(int i=0; i<clientList.Cnt(); i++) {
                    int fd = clientList[i].fd;
                    if(FD_ISSET(fd, &testfds)) {cliix = i; break;}
                }
                if(cliix == FEOFLIST) {
                    mylog(LOG_ERR, "INTERNAL ERROR: request of unknown client\n");
                    continue;
                }
                int nread;
                ClientConnection &cc = clientList[cliix];
                ioctl(cc.fd, FIONREAD, &nread);
                if(nread == 0) {
                    // client closed its connection
                    FD_CLR(cc.fd, &readfds);
                    delete_client(cc.fd);
                }
                else {
                    FString fs;
                    fs.setLen(nread);
                    read(cc.fd, fs.StrBuff(), nread);
                    fs = fs.Replace("\n", " ");
                    //mylog(LOG_TRASH, "client on fd %d appends:\n\t'%s'\n", cc.fd, fs.Str());
                    cc.msg = cc.msg + fs;
                    //mylog(LOG_TRASH, "cc.msg: '%s'\n", cc.msg.Str());
                    FStringList fsl; fsl.fromString(cc.msg, '}', false); // do not strip
                    //for(int i=0; i<fsl.Cnt(); i++) mylog(LOG_TRASH, "\tfsl[%02i] == '%s'\n", i, fsl[i].Str());
                    for(int i=0; i<fsl.Cnt()-1; i++) {
                        fs = fsl[i];
                        //mylog(LOG_INF, "\tfsl[i]: '%s'\n", fs.Str());
                        fs = (fs + '}').Trim();
                        //mylog(LOG_INF, "\t(fs + '}').Trim(): '%s'\n", fs.Str());
                        mylog(LOG_INF, "client fd(%i): '%s'\n", cc.fd, fs.Str());
                        // check if this signature is not currently served
                        if(fs.getToken("{SDOR~") > 0) {
                            uint32_t sig = signatureFromSDOMessage(fs);
                            if(sig > 0) {
                                for(int i=0; i<servedSDOList.Cnt(); i++) {
                                    if(servedSDOList[i].signature == sig) {
                                        int ix = servedSDOList[i].clientIndex;
                                        if(ix >= 0 && ix < clientList.Cnt()) {
                                           int fd = clientList[ix].fd;
                                            // {SDOC DOWNLOAD srvcli_port clisrv_port node index subindex}
                                            // skip 7 tokens and append error message
                                            int pos = fs.getToken("!~~!~~!~~!~~!~~!~~!~~");
                                            fs = fs.Slice(0, pos);
                                            fs += "ERROR 0000 'This SDO request is currently served by client " + FString(fd) + ".'}";
                                            write_to_client(cc, fs);
                                            sig = 0;
                                        }
                                        else {
                                            // delete unused signature
                                            mylog(LOG_ERR, "found & remove unused SDO signature %08x\n", sig);
                                            servedSDOList.Remove(i);
                                        }
                                        break;
                                    }
                                }
                            }
                            if(sig) {
                                mylog(LOG_DEB, "Adding new served SDO (%08x, %i) to list\n", sig, cliix);
                                servedSDOList.Append(new ServedSDO(sig, cliix));
                                write_to_candevice(fs);
                            }
                        }
                        else if(fs.getToken("{CANDTG~") > 0) {
                            write_to_candevice(fs);
                        }
                        else {
                            fs = "ERROR unknown datagram: '" + fs + "'";
                            write_to_client(cc, fs);
                        }
                    }
                    if(fsl.Cnt()) cc.msg = fsl[-1];
                    else          cc.msg = "";
                    //mylog(LOG_TRASH, "cc.msg exit: '%s'\n", candev_msg.Str());
                }
            }
        }    
    }
    catch(FException &e) {
        mylog(LOG_ERR, "EXCEPTION: %s\n", e.Msg());
    }
    clean_up();
}