#include <errno.h>
#include <getopt.h>
#include <libgen.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <limits.h>
#include <stdint.h>

#include <net/if.h>

#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <sys/uio.h>

#include <linux/can.h>
#include <linux/can/raw.h>

#include "BinaryDecoder.h"
#include <ncurses.h>


extern int optind, opterr, optopt;


static int	s = -1;
static int	running = 1;


enum {
// 	VERSION_OPTION = CHAR_MAX + 1,
	FILTER_OPTION,
};


typedef struct{
  float frontWheelsSpeed;     // mean speed of the front wheels (in km/h)
  float rearLeftWheelSpeed;   // speed of the rear left wheel (in km/h)
  float rearRightWheelSpeed;  // speed of the rear right wheel (in km/h)
  float rpmFrontWheels;       // mean rpm of the front wheels (in tr/min)
}StructWheelSpeed;


static void print_usage(char *prg)
{
        fprintf(stderr, "Usage: %s [<can-interface>] [Options]\n"
		"Options:\n"
		" -f, --family=FAMILY\t"	"protocol family (default PF_CAN = %d)\n"
		" -t, --type=TYPE\t"		"socket type, see man 2 socket (default SOCK_RAW = %d)\n"
		" -p, --protocol=PROTO\t"	"CAN protocol (default CAN_RAW = %d)\n"
		"     --filter=id:mask[:id:mask]...\n"
		"\t\t\t"			"apply filter\n"
		" -h, --help\t\t"		"this help\n"
		" -o <filename>\t\t"		"output into filename\n"
		" -d\t\t\t"			"daemonize\n"
// 		"     --version\t\t"		"print version information and exit\n"
		" -s, --speed\t\t"              "decode the can frame 0x44D on Dyna and Carmen\n"
		" -y, --dynamic_display\t"     	"dynamic display of the 0x44D on Dyna and Carmen\n"
		" -a, --airplug\t\t"     	"allow frame's informations to be send to\n" 
		"\t\t\t"			"airplug (not able with dynamic display)\n",
		prg, PF_CAN, SOCK_RAW, CAN_RAW);
}


static void sigterm(int signo)
{
	running = 0;
}


static struct can_filter *filter_can = NULL;
static int filter_count = 0;


int add_filter(u_int32_t id, u_int32_t mask)
{
	filter_can = (can_filter*)realloc( filter_can, sizeof(struct can_filter ) * ( filter_count + 1));
	if(!filter_can)
		return -1;

	filter_can[filter_count].can_id = id;
	filter_can[filter_count].can_mask = mask;
	filter_count++;

	printf("id: 0x%08x mask: 0x%08x\n",id,mask);
	return 0;
}


int add_filter_0x44d()
{
	add_filter( 0x44d, 0x44d );
	return 0;
}


int add_dynamic_display()
{
 	initscr();
	return 0;
}


int add_airplug_communication()
{
	
	  
	return 0;
}


#define BUF_SIZ	(255)


int main(int argc, char **argv)
{
	struct can_frame frame;
	struct ifreq ifr;
	struct sockaddr_can addr;
	FILE *out = stdout;
	char *interface = "can0";
	char *optout = NULL;
	char *ptr;
	char buf[BUF_SIZ];
	
	//test
	unsigned char dataBuf[ 8 ];
	unsigned short transform = 0;
	StructWheelSpeed data_;
	
	int family = PF_CAN, type = SOCK_RAW, proto = CAN_RAW;
	int n = 0, err;
	int nbytes, i;
	int opt, optdaemon = 0;
	int speed = 1, display = 1, airplug = 1;
	float speedFront = 0.0, speedRearLeft = 0.0, speedRearRight = 0.0, speedWheel = 0.0;
	uint32_t id, mask;

	signal(SIGPIPE, SIG_IGN);

	struct option		long_options[] = {
		{ "help", no_argument, 0, 'h' },
		{ "speed", no_argument, 0, 's' },
		{ "dynamic_display", no_argument, 0, 'y' },
		{ "airplug", no_argument, 0, 'a' },
		{ "family", required_argument, 0, 'f' },
		{ "protocol", required_argument, 0, 'p' },
		{ "type", required_argument, 0, 't' },
		{ "filter", required_argument, 0, FILTER_OPTION },
// 		{ "version", no_argument, 0, VERSION_OPTION},
		{ 0, 0, 0, 0},
	};
	
	while ((opt = getopt_long(argc, argv, "f:t:p:o:d:ayhs", long_options, NULL)) != -1) {
		switch (opt) {
		case 'd':
			optdaemon++;
			break;

		case 'h':
			print_usage(basename(argv[0]));
			exit(0);

		case 'f':
			family = strtoul(optarg, NULL, 0);
			break;

		case 't':
			type = strtoul(optarg, NULL, 0);
			break;

		case 'p':
			proto = strtoul(optarg, NULL, 0);
			break;

		case 'o':
			optout = optarg;
			break;

		case FILTER_OPTION:
			ptr = optarg;
			while(1) {
				id = strtoul(ptr, NULL, 0);
				ptr = strchr(ptr, ':');
				if(!ptr) {
					fprintf(stderr, "filter must be applied in the form id:mask[:id:mask]...\n");
					exit(1);
				}
				ptr++;
				mask = strtoul(ptr, NULL, 0);
				ptr = strchr(ptr, ':');
				add_filter(id,mask);
				if(!ptr)
					break;
				ptr++;
			}
			break;
			
		case 's':
			speed = add_filter_0x44d();
			break;
		
		case 'y':
		    	if ( airplug != 0 ) {
				speed = add_filter_0x44d();
				display = add_dynamic_display();
			} else {
				endwin();
				printf("Can not activate airplug communication with the dynamic display\nProgram will exit\n");
				exit(1);
			}
			break;
			
		case 'a':		
			if ( display != 0 ) {
				speed = add_filter_0x44d();
				airplug = 0;
			} else {
				endwin();
				printf("Can not activate airplug communication with the dynamic display\nProgram will exit\n");
				exit(1);
			}			
			break;

// 		case VERSION_OPTION:
// 			printf("candump %s\n",VERSION);
// 			exit(0);

		default:
			fprintf(stderr, "Unknown option %c\n", opt);
			break;
		}
	}

	if (optind != argc)
		interface = argv[optind];
		printf("interface = %s, family = %d, type = %d, proto = %d\n", interface, family, type, proto);

	if ((s = socket(family, type, proto)) < 0) {
		perror("socket");
		return 1;
	}

	addr.can_family = family;
	strncpy(ifr.ifr_name, interface, sizeof(ifr.ifr_name));
	
	if (ioctl(s, SIOCGIFINDEX, &ifr)) {
		perror("ioctl");
		return 1;
	}
	addr.can_ifindex = ifr.ifr_ifindex;

	if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		perror("bind");
		return 1;
	}

	if (filter_can) {
		if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, filter_can,
			       filter_count * sizeof(struct can_filter)) != 0) {
			perror("setsockopt");
			exit(1);
		}
	}

	if (optdaemon)
		daemon(1, 0);
	else {
		signal(SIGTERM, sigterm);
		signal(SIGHUP, sigterm);
	}

	if (optout) {
		out = fopen(optout, "a");
		if (!out) {
			perror("fopen");
			exit (EXIT_FAILURE);
		}
	}

	while (running) {
		if ((nbytes = read(s, &frame, sizeof(struct can_frame))) < 0) {
			perror("read");
			return 1;
		} else {
			if (frame.can_id & CAN_EFF_FLAG)
				n = snprintf(buf, BUF_SIZ, "<0x%08x> ", frame.can_id & CAN_EFF_MASK);
			else
				n = snprintf(buf, BUF_SIZ, "<0x%03x> ", frame.can_id & CAN_SFF_MASK);

			n += snprintf(buf + n, BUF_SIZ - n, "[%d] ", frame.can_dlc);
			for (i = 0; i < frame.can_dlc; i++) {
				n += snprintf(buf + n, BUF_SIZ - n, "%02x ", frame.data[i]);
			}
			if (frame.can_id & CAN_RTR_FLAG)
				n += snprintf(buf + n, BUF_SIZ - n, "remote request");
			
			if ( speed == 0 ) {			
				
 				memcpy( dataBuf, frame.data, sizeof( dataBuf ) );
			  
// 				speedFront = ( ( ( int )frame.data[ 0 ] ) * 256 + ( int )frame.data[ 1 ] ) * 0.01;
// 				speedRearLeft = ( ( ( int )frame.data[ 2 ] ) * 256 + ( int )frame.data[ 3 ] ) * 0.01;
// 				speedRearRight = ( ( ( int )frame.data[ 4 ] ) * 256 + ( int )frame.data[ 5 ] ) * 0.01;
// 				speedWheel = ( ( ( int )frame.data[ 6 ] ) * 256 + ( int )frame.data[ 7 ] ) * 0.04;
				if ( mDecodeToUI16(&transform, dataBuf, 7, 16) )
				      data_.frontWheelsSpeed = static_cast<float>(transform * 0.01);
				if ( mDecodeToUI16(&transform, dataBuf, 23, 16) )
				      data_.rearLeftWheelSpeed = static_cast<float>(transform * 0.01); 
				if ( mDecodeToUI16(&transform, dataBuf, 39, 16) )
				      data_.rearRightWheelSpeed = static_cast<float>(transform * 0.01); 
				if ( mDecodeToUI16(&transform, dataBuf, 55, 16) )
				      data_.rpmFrontWheels = static_cast<float>(transform * 0.04);
				
				if ( display == 0 ) {				
					move( 0, 0);
					printw( "Vitesse avant :          %.2f km/h\n", data_.frontWheelsSpeed );
					move( 1, 0);
					printw( "Vitesse arriere gauche : %.2f km/h\n", data_.rearLeftWheelSpeed );
					move( 2, 0);
					printw( "Vitesse arriere droite : %.2f km/h\n", data_.rearRightWheelSpeed );
					move( 3, 0);
					printw( "Vitesse roue :           %.2f tr/min\n", data_.rpmFrontWheels );
					refresh();
				} else if ( airplug == 0 ) {
					
					printf( "^pacpl~\\!f~t!T~SpeedFront!t~0!d~%.2f!\\!f~t!T~SpeedRearLeft!t~0!d~%.2f!\\!f~t!T~SpeedRearRight!t~0!d~%.2f!\\!f~t!T~SpeedWheel!t~0!d~%.2f!\\^\n", data_.frontWheelsSpeed, data_.rearLeftWheelSpeed, data_.rearRightWheelSpeed, data_.rpmFrontWheels );
				} else {
					fprintf( out, "Vitesse avant :          %.2f km/h\n", data_.frontWheelsSpeed );
					fprintf( out, "Vitesse arriere gauche : %.2f km/h\n", data_.rearLeftWheelSpeed );
					fprintf( out, "Vitesse arriere droite : %.2f km/h\n", data_.rearRightWheelSpeed );
					fprintf( out, "Vitesse roue :           %.2f tr/min\n", data_.rpmFrontWheels );
				}
				
			} else {
				fprintf(out, "%s\n", buf);
			}
				
			do {
				err = fflush(out);
				if (err == -1 && errno == EPIPE) {
					err = -EPIPE;
					fclose(out);
					out = fopen(optout, "a");
					if (!out)
						exit (EXIT_FAILURE);
				}
			} while (err == -EPIPE);

			n = 0;
		}
	}

	endwin();

	exit (EXIT_SUCCESS);
}