/*
 * gps.c
 * Read gps device periodically, compute delay and jittering
 */


#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>

#include <native/task.h>
#include <native/timer.h>

#include <rtdm/rtserial.h>

#define MAIN_PREFIX   "main : "
#define STASK_PREFIX  "gps_task: "

#define GPS_FILE	"rtser0"

#define STATE_FILE_OPENED         1
#define STATE_TASK_CREATED        2

#define SSIZE	450

int gps_fd = -1;

unsigned int gps_state = 0;

/*                           --s-ms-us-ns */
RTIME gps_task_period_ns =    100000000llu;
RT_TASK gps_task;
RTIME trigger_time;

static const struct rtser_config gps_config = {
	.config_mask       = 0xFFFF,
	.baud_rate         = 115200,
	.parity            = RTSER_DEF_PARITY,
	.data_bits         = RTSER_DEF_BITS,
	.stop_bits         = RTSER_DEF_STOPB,
	.handshake         = RTSER_DEF_HAND,
	.fifo_depth        = RTSER_DEF_FIFO_DEPTH,
	.rx_timeout        = 40000000,	/* 40 ms */
	.tx_timeout        = RTSER_DEF_TIMEOUT,
	.event_timeout     = 100000000, /* 0.1 s */
	.timestamp_history = RTSER_RX_TIMESTAMP_HISTORY,
	.event_mask        = RTSER_EVENT_RXPEND,
};

static int close_file( int fd, char *name)
{
	int err, i=0;
	
	do {
		i++;
		err = rt_dev_close(fd);
		switch (err) {
		case -EAGAIN:
			printf(MAIN_PREFIX "%s -> EAGAIN (%d times)\n",
			       name, i);
			rt_task_sleep(50000); /* wait 50us */
			break;
		case 0:
			printf(MAIN_PREFIX "%s -> closed\n", name);
			break;
		default:
			printf(MAIN_PREFIX "%s -> %s\n", name,
			       strerror(-err));
			break;
		}
	} while (err == -EAGAIN && i < 10);

	return err;
}

void cleanup_all(void)
{
	if (gps_state & STATE_FILE_OPENED) {
		close_file(gps_fd, GPS_FILE " (gps)");
		gps_state &= ~STATE_FILE_OPENED;
	}
	
	if (gps_state & STATE_TASK_CREATED) {
		printf(MAIN_PREFIX "delete gps_task\n");
		rt_task_delete(&gps_task);
		gps_state &= ~STATE_TASK_CREATED;
	}
	
}

void catch_signal(int sig)
{
	cleanup_all();
	printf(MAIN_PREFIX "exit\n");
	return;
}

void gps_task_proc(void *arg)
{
	int err, nr = 0;
	ssize_t read = 0;
	char sdata[SSIZE];
	ssize_t ss = SSIZE*sizeof(char);
	RTIME irq_time = 0;
	RTIME read_time = 0;
	RTIME last_time = 0;
	struct rtser_event rx_event;

	while (1) {
 		/* waiting for event */
		err = rt_dev_ioctl(gps_fd, RTSER_RTIOC_WAIT_EVENT, &rx_event);
                if (err) {
                        printf(STASK_PREFIX
                               "error on RTSER_RTIOC_WAIT_EVENT, %s\n",
                               strerror(-err));
                        if (err == -ETIMEDOUT)
                                continue;
                        break;
                }

		irq_time = rx_event.rxpend_timestamp;

		read = rt_dev_read(gps_fd, sdata, ss);
		if (read >= 0) {
			read_time = rt_timer_read();
			printf(STASK_PREFIX "%3d | %3d | %16llu\n", nr, read,
			       irq_time - last_time);
/**
			printf(STASK_PREFIX "%3d | %3d | %16llu\n", nr, read,
			       read_time - irq_time);
			for(i=0;i<read;i++)
				printf("%x ", sdata[i]);
**/
			printf("\n");
		} else {
			printf(STASK_PREFIX "error on rt_dev_read, code %s\n",
			       strerror(-read));
			break;
		}
		last_time = irq_time;

		nr++;
	}

	if ((gps_state & STATE_FILE_OPENED) &&
	    close_file(gps_fd, GPS_FILE " (gps)") == 0)
		gps_state &= ~STATE_FILE_OPENED;

	printf(STASK_PREFIX "exit\n");
}

int main(int argc, char* argv[])
{
	int err = 0;

	signal(SIGTERM, catch_signal);
	signal(SIGINT, catch_signal);

	/* no memory-swapping for this programm */
	mlockall(MCL_CURRENT | MCL_FUTURE);

	/* open rtser0 */
	gps_fd = rt_dev_open( GPS_FILE, 0);
	if (gps_fd < 0) {
		printf(MAIN_PREFIX "can't open %s (gps), %s\n", GPS_FILE,
		       strerror(-gps_fd));
		goto error;
	}
	gps_state |= STATE_FILE_OPENED;
	printf(MAIN_PREFIX "gps-file opened\n");

	/* writing gps-config */
	err = rt_dev_ioctl(gps_fd, RTSER_RTIOC_SET_CONFIG, &gps_config);
	if (err) {
		printf(MAIN_PREFIX "error while RTSER_RTIOC_SET_CONFIG, %s\n",
		       strerror(-err));
		goto error;
	}
	printf(MAIN_PREFIX "gps-config written\n");


	/* create gps_task */
	err = rt_task_create(&gps_task, "gps_task", 0, 50, 0);
	if (err) {
		printf(MAIN_PREFIX "failed to create gps_task, %s\n",
		       strerror(-err));
		goto error;
	}
	gps_state |= STATE_TASK_CREATED;
	printf(MAIN_PREFIX "gps-task created\n");


	/* start gps_task */
	printf(MAIN_PREFIX "starting gps-task\n");
	err = rt_task_start(&gps_task, &gps_task_proc, NULL);
	if (err) {
		printf(MAIN_PREFIX "failed to start gps_task, %s\n",
		       strerror(-err));
		goto error;
	}

	pause();
	return 0;

 error:
	cleanup_all();
	return err;
}