/* 
 * Forward incoming CAN frames as concatenated strings to one 
 * or several servers over TCP sockets.
 * Copyright (C) 2017 NetModule AG, Switzerland
 */


/* CAN Interface options */
DEV = "can0"; /* CAN Interface to listen on */
BITRATE = 250000; /* run 250kbit bitrate */
LISTENONLY = 0; /* open in listen-only mode (normaly set to 1) */
RESTART_TIMEOUT = 0; /* restart timeout in case of bus-off (msecs, 0 = disabled) */
 
/* TCP Options */
/* number of retries when connecting to a slave */
RETRY = 3;	
/* the interval in seconds for each retry */
RETRY_INTERVAL = 3;	
/* recovery interval after slave got disconnected or retries were unsuccessful */
RECOVER_INTERVAL = 120;
 
 
/* 
 * Only changed things below if you know what you are doing 
 */
 
POLL_TIMEOUT = 10;  /* seconds */
REMOTE_PORT =2000;  /* port of the remote serial slaves (default 2000) */ 
 
/* slave template */
template slave 
{
    hostname = "";
    fd = -1;
    state = "down";
    since = 0; 
 
    void slave(string remotehost) 
    { 
        printf("Creating slave %s\n",remotehost);
        if (strlen(remotehost) > 0) {
            this.hostname = remotehost;
        } else {
            printf("ERROR: no hostname defined, slave could not be created\n");
        }
    }
 
    int jiffy()
    {
        sys = sysinfo();
        u = struct_get(sys, "uptime");
 
        if (is_void(u) || u < 1) {
            return 0;
        } else {
            return u;
        }
    } 
 
    int connect() {
        if (this.fd > -1) {
            close(this.fd);
            this.fd = -1;
        }
        printf("Connecting to %s\n", this.hostname);
 
        /* open TCP socket */
        sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
        setsockopt(sock, SOL_SOCKET, SO_KEEPALIVE,1);
        setsockopt(sock, SOL_TCP, TCP_KEEPIDLE,1);
        setsockopt(sock, SOL_TCP, TCP_KEEPINTVL,5);
        setsockopt(sock, SOL_TCP, TCP_KEEPCNT,5);

        if (sock < 0) {
            printf("ERROR: unable to open socket\n");
        }
        for (i = 0; i < RETRY; i++) {
	    printf("Connect now\n");
            if (connect(sock, this.hostname, REMOTE_PORT) < 0){
                printf("ERROR: could not connect to %s:%d\n", this.hostname, REMOTE_PORT);
                printf("Retrying to connect (trial #%i, waiting %s sec)\n", i, RETRY_INTERVAL);
                sleep(RETRY_INTERVAL);
            } else {
                printf("TCP connection established to host %s:%d\n", this.hostname, REMOTE_PORT);
                this.fd = sock;
                this.state = "up";
                return 0;
            }
        }
        printf("ERROR:  max. retries reached for %s, giving up\n", this.hostname);  
        this.since = this.jiffy();
        this.state = "down";
        close(this.fd);
        this.fd = -1;
 
        return -1;
    }
 
    int disconnect() {
        if (this.fd > -1) {
            close(this.fd);
            this.fd = -1;
        }
        this.state = "down";
        this.since = this.jiffy();
        printf("Disconnected host %s\n", this.hostname);  
        return 0;
    }
 
 
} /* endof template slave */
 
int start_can() 
{
	ret = nb_can_setattr(DEV, BITRATE, LISTENONLY, RESTART_TIMEOUT);
	if (ret != 0) {
    		printf("Unable to set attributes of %s\n", DEV);
    		exit(1);
	}
	printf("Attributes set for device %s\n", DEV);
	can_sock = nb_can_open(DEV);
	if ( can_sock < 0){
		printf("Unable to open %s\n", DEV);
		exit(1);	
	}


	// CAN Filtering can be done here
	if (0) {
    		FILTER_ID = 1;
    		FILTER_MASK = 0xFFFFFFFF;

    		if (nb_can_setfilter(sock, FILTER_ID, FILTER_MASK) < 0) {
        		printf("Unable to set filter on %s\n", DEV);
        		nb_can_close(can_sock);
        		exit(1);
    		}
	}

	return can_sock;
}
 
int stop_can() 
{
    if (can_sock > -1) {
        nb_can_close(can_sock);
        can_sock = -1;
    }
    return 0; 
}
 
void usage()
{
    printf("usage: can-tcp-broadcast.are <remote-ip-1>  <remote-ip-2> .... <remote-ip-n>\n");
    exit(1);
}

/* ------------------------ main ------------------------------- */

if (strlen(argv[1]) == 0) {
    printf("ERROR: no remote host specified\n");
    usage();
}
 
/* create slaves */
printf("Creating slaves\n");
for (i = 1; i < argc; i++) {
    slaves[i-1] = new slave(argv[i]);
}
 
/* connect slave */
for (i = 0; i < length(slaves); i++) {
    printf("connecting slave %d\n", i);
    rc = slaves[i].connect();
    if (rc < 0) {
        printf("ERROR: could not connect %s\n", slaves[i].hostname);   
    }
}
 
printf("Starting can\n");
can_sock = start_can();
 
/* create file descriptor array */
fds = mkarray();
tcp_fds = mkarray();
j = 0;
for (i = 0; i < length(slaves); i++) {
     if (slaves[i].state == "up") {
        tcp_fds[j] = slaves[i].fd;
        j++;   
    }
}
fds = array_merge(can_sock, tcp_fds);
 

 
/* loop infinitely */
while (1) {
    buffer = "";
    rc = select(fds, POLL_TIMEOUT);
    if (rc == -1) {
        printf("ERROR: select failed\n");
    } else if (rc == can_sock) {
        /* received something from CAN interface */
    	msg = nb_can_recvmsg(can_sock, 5);
    	len = 0;
    	msgbuffer = "";
    	if (msg) {
 		/* get recived CAN message  & */
    		id = struct_get(msg, "id") & CAN_EFF_MASK; /* extended frame format (EFF) */
    		len = struct_get(msg, "len");
    		data = struct_get(msg, "data");
	
		/* Concatenate the CAN Identifier & CAN Data
	    	to later send the string over a TCP socket
		*/
		msgbuffer = sprintf("CAN ID: %04X DLC: %i DATA: ",id, len); 
		for (i = 0; i < len; i++) {
			msgbuffer = sprintf("%s%02X ", msgbuffer, data[i]);
		}
		msgbuffer = sprintf("%s\n", msgbuffer);
		msg = msgbuffer;
    	}
    } else if (rc == 0){
		// Implement a timeout routine if required below 
		// printf("Timeout\n");	
    }

 
    /* send TCP packet to every slave */
    if (len > 0) {
        for (i = 0; i < length(slaves); i++) {
            if (slaves[i].state == "up" ) {
  		// Sending CAN Message over TCP Socket 
                sent = send(slaves[i].fd, msg);
                if (sent < 0) {
                    printf("ERROR: failed to send packet to %s\n", slaves[i].hostname);
                    /* disconnect slave */
                    slaves[i].disconnect();   
                }
            }
        }
    }
    /* check if we have an error on the socket */
    for (i = 0; i < length(slaves); i++) {
        if (slaves[i].state == "up" ) {
            opt = getsockopt(slaves[i].fd, SOL_SOCKET, 4);
            if (opt != 0) {
               printf("ERROR: getsockopt(%d) on slave %s failed, reconnecting\n", opt, slaves[i].hostname);
               slaves[i].disconnect();
            }
        }
    }
 
    /* reconnect to disconnected slaves */
    for ( i=0; i < length(slaves); i++) {
        if (slaves[i].state == "down" ) {
            if ( (slaves[i].since + RECOVER_INTERVAL) < slaves[i].jiffy() ) {
              slaves[i].connect();
            }
        }
    }
 
    /* re-create file descriptor array */
    fds = mkarray();
    tcp_fds = mkarray();
    j = 0;
    for (i = 0; i < length(slaves); i++) {
         if (slaves[i].state == "up") {
             tcp_fds[j] = slaves[i].fd;
             j++;
        }
    }
    fds = array_merge(can_sock, tcp_fds);
}
stop_can();
exit(0);