/* DESC: This script shows an example of parsing a PGN and a specific SPN 
 * based on the FMS Standard 3.0
 * 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) */
MAX_MESSAGES = 1024; /* max number of incoming messages */
 
int start_can() 
{
	ret = nb_can_setattr(DEV, BITRATE, LISTENONLY, RESTART_TIMEOUT);
	if (ret != 0) {
    		nb_syslog("Unable to set attributes of %s\n", DEV);
    		exit(1);
	}
	can_sock = nb_can_open(DEV);
	if ( can_sock < 0){
		nb_syslog("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) {
        		nb_syslog("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; 
}


/* Parse an example CAN Message and get the PGN and the value of
   a specified SPN based on FMS */ 
 
int parse_can_frame(int can_identifier, array can_data){
        shifted_id = can_identifier >> 8;
        pgn = shifted_id & pgn_mask;
	// FMS Standard 3.0:
	// PGN 0x00FEF1 --> Cruise Control / Vehicle Speed: CCVS

        if(pgn == 0x00FEF1){
                /* Get Wheel based speed (DATA BYTES 2 &3) from Cruise Control/Vehicle Speed: CCVS
		   SPN: 84 */
                speed = ((can_data[2] << 8) + can_data[1])/256;
		/* Log the parsed message */		
		now = localtime(time());
		timestamp = strftime("%Y-%m-%d %H:%M:%S", now);
		fwrite(fp, sprintf("%s -- Wheel based speed: %f\n",timestamp, speed));
		return 0;
       	} else{
		return -1;
	}
}
 
/* ------------------------ main ------------------------------- */

/* Bitmask to get PGN from CAN Identifier */
pgn_mask = (1 << 18) -1;

pcount = 0;

printf("Listening on %s\n", DEV);
can_sock = start_can();

fp = fopen("/tmp/can_parser_results.log", "w+");
if (is_void(fp)){
	nb_syslog("unable to open %s", fp);
	exit(1);
}
 
for(count = 0; count < MAX_MESSAGES;) {
    	msg = nb_can_recvmsg(can_sock, 5);
	if (is_void(msg)) continue;
    	len = 0;
   
    	if (msg) {

 		/* Strip recived CAN frame  & parse an example */
    		id = struct_get(msg, "id") & CAN_EFF_MASK; /* extended frame format (EFF) */
    		len = struct_get(msg, "len");
    		data = struct_get(msg, "data");
	
		if(parse_can_frame(id, data) == 0 ){
			pcount++;	
		}
    	}
	count++;
}

fwrite(fp, sprintf("Parsed %i Frames accoding to your specifications\n", pcount));
fclose(fp);
stop_can();
exit(0);