///#include <can_config.h>

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

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

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

#include <sys/time.h>
#include <errno.h>

static int running = 1;

#define CAN_MSG_COUNT	50

#define CAN_MSG_ID		0x77
#define CAN_MSG_LEN		8


static void print_compare( 	struct can_frame exp,
							struct can_frame rec);
static void print_frame(struct can_frame frame);
static void Sleep(int t_time);


void print_usage(char *prg)
{
        fprintf(stderr, "Usage: %s <can-interface> \n"
		"\n"
		"Generates can frames on <can-interface> an expects response\n"
		"from the canecho_dut\n"
		"\n",
		prg);
}

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

int main(int argc, char **argv)
{
	struct can_frame tx_frames[CAN_MSG_COUNT];
	struct can_frame rx_frame;
	struct ifreq ifr;
	struct sockaddr_can addr;
	char *intf_name;
	int family = PF_CAN, type = SOCK_RAW, proto = CAN_RAW;
	int nbytes, i;
	int s;
	int send_pos = 0;
	int recv_pos = 0;
	int unprocessed = 0;
	unsigned char counter = 0;
	int verbose = 0;

	signal(SIGTERM, sigterm);
	signal(SIGHUP, sigterm);
	signal(SIGINT, sigterm);

	if (1 == argc) {
		print_usage(basename(argv[0]));
		exit(0);
	}

	intf_name = argv[1];

	printf("interface = %s, family = %d, type = %d, proto = %d\n",
	       intf_name, family, type, proto);


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

	addr.can_family = family;
	strcpy(ifr.ifr_name, intf_name);
	ioctl(s, SIOCGIFINDEX, &ifr);
	addr.can_ifindex = ifr.ifr_ifindex;

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

	while (running)
	{
		if (unprocessed < CAN_MSG_COUNT)
		{
			// still send messages
			tx_frames[send_pos].can_dlc = CAN_MSG_LEN;
			tx_frames[send_pos].can_id = CAN_MSG_ID;
			for(i=0; i<CAN_MSG_LEN; i++)
			{
				tx_frames[send_pos].data[i] = counter + i;
			}
			while( (nbytes=write(s, &tx_frames[send_pos], sizeof(*tx_frames))) !=
				   sizeof(*tx_frames) )
			{
				if(nbytes!=EAGAIN)
				{
					perror("write");
					return 1;
				}
			}

			/* increment to be equal to expected */
			tx_frames[send_pos].can_id++;
			for(i=0; i<CAN_MSG_LEN; i++)
			{
				tx_frames[send_pos].data[i]++;
			}

			send_pos++;
			if(send_pos == CAN_MSG_COUNT)
			{
				send_pos = 0;
			}
			unprocessed++;
			counter++;

			if( (counter%33) == 0 )
			{
				Sleep(3);
			}
			else
			{
				Sleep(1);
			}
		}
		else
		{
			if ((nbytes = read(s, &rx_frame, sizeof(rx_frame))) < 0)
			{
				perror("read");
				return 1;
			}
			if (verbose)
			{
				print_frame(rx_frame);
			}
			// compare with expected
			if( rx_frame.can_id != tx_frames[recv_pos].can_id )
			{
				printf("Message ID mismatch!\n");
				print_compare(tx_frames[recv_pos], rx_frame);
				running = 0;
			}
			else if( rx_frame.can_dlc != tx_frames[recv_pos].can_dlc )
			{
				printf("Message length mismatch!\n");
				print_compare(tx_frames[recv_pos], rx_frame);
				running = 0;
			}
			else
			{
				for(i=0; i<rx_frame.can_dlc; i++)
				{
					if(rx_frame.data[i] != tx_frames[recv_pos].data[i])
					{
						printf("Databyte %x mismatch !\n", i);
						print_compare(tx_frames[recv_pos], rx_frame);
						running = 0;
					}
				}
			}
			recv_pos++;
			if(recv_pos == CAN_MSG_COUNT)
			{
				recv_pos = 0;
			}
			unprocessed--;
		}
	}

	return 0;
}


static void print_compare( 	struct can_frame exp,
							struct can_frame rec)
{
	printf("expected: ");
	print_frame(exp);
	printf("received: ");
	print_frame(rec);
}

static void print_frame(struct can_frame frame)
{
	int i;
	printf("%04x: ", frame.can_id);
	if (frame.can_id & CAN_RTR_FLAG)
	{
		printf("remote request");
	}
	else
	{
		printf("[%d]", frame.can_dlc);
		for (i = 0; i < frame.can_dlc; i++)
		{
			printf(" %02x", frame.data[i]);
		}
	}
	printf("\n");
}

static void Sleep(int t_time)
{
	struct timespec rqtp, rmtp;
	int    error;
	if (t_time == 0 )
	{
		sched_yield();
		return;
	}
	/* sleep in ms */
	rqtp.tv_sec = t_time / 1000;
	rqtp.tv_nsec = (t_time % 1000) * 1000000;
	while( 0 != (error=nanosleep(&rqtp,&rmtp)) )
	{
		if( EINTR == error )
		{
			rqtp = rmtp;
		}
		else
		{
			break;
		}
	}
}