/* from http://maemo.org/development/documentation/how-tos/3-x/howto_connectivity_guide_bora.html */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include <errno.h>
#include <pthread.h>
#include <signal.h>

#include "zlib.h"   /* for file compression */
#include "gps.h"    /* for reading GPS data from daemon */

#include "gpsbt.h"  /* GPS BT package */


#define VERSION "1.0"

#define MAX_BUF 255
#define MAX_ERROR_BUF_LEN 255

static int started_from_server = 0;
static int dont_quit = 1;
static int restart = 0;
static int radius = 10;  /* how much (in metres) the user has to move before
			  * we update the coordinate
			  */
static gzFile fp = NULL;  /* output file is compressed to save space */
static unsigned long data_id;
static struct gps_data_t *gpsd;
static gpsbt_t ctx = {0};  /* clearing the context is important! */
static pthread_mutex_t quit_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t query_gpsd_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t query_gpsd_cond = PTHREAD_COND_INITIALIZER;

#define MAX_GPSDEV_LEN 64
static char gpsdev[MAX_GPSDEV_LEN+1] = {0};

/* ----------------------------------------------------------------------- */
static int debug_level;

#ifdef DEBUG
#if (__GNUC__ > 2) && ((__GNUC__ > 3) || (__GNUC_MINOR__ > 2))
#define PDEBUG(fmt...) do {						\
		if (debug_level) {					\
			struct timeval tv;				\
			gettimeofday(&tv, 0);				\
			printf("DEBUG[%d]:%ld.%ld:%s:%s():%d: ",	\
			       getpid(),				\
			       tv.tv_sec, tv.tv_usec,			\
			       __FILE__, __FUNCTION__, __LINE__);	\
			printf(fmt);					\
			fflush(stdout);					\
		}							\
	}while(0)
#else
#define PDEBUG(fmt...) do {						\
		if (debug_level) {					\
			struct timeval tv;				\
			gettimeofday(&tv, 0);				\
			printf("DEBUG[%d]:%ld.%ld:%s:%s():%d: ",	\
			       getpid(),				\
			       tv.tv_sec, tv.tv_usec,			\
			       __FILE__, __FUNCTION__, __LINE__);	\
			printf(##fmt);					\
			fflush(stdout);					\
		}							\
	}while(0)
#endif
#else
#define PDEBUG(fmt...)
#endif



/* ----------------------------------------------------------------------- */
static void usage(char *prg, char *str)
{
	if (str)
		printf(str);

	printf("Usage: %s -f <log file> [-s] [-g <gps device>] [-r <distance>]\n", prg);
	printf("\t-f <log file>, the GPS data is saved in compressed form in this file.\n");
	printf("\t-s the program is started by server\n");
	printf("\t-g <gps device>, the GPS device id i.e., GPS BT address or rfcomm address\n");
	printf("\t-p <BT gps device to pair>, GPS BT address of the device to be paired with\n");
	printf("\t-r <distance>, in metres (not used at the moment)\n");
	printf("\t-D turn debugging on in this program\n");
	printf("\t-F <timeout>, flush gzipped output file after timeout seconds, default is not to flush\n");
	printf("\t-x <gpsmgr library debug level>\n");
	printf("\t-y <gpsbt library debug level>\n");
	printf("\t-z <gpsd debug level>\n");
	printf("\t-b daemonize i.e., run in background\n");
	printf("\t-i <PID file>, save the pid to this file\n");
	printf("\n");
	exit(-1);
}


/* ----------------------------------------------------------------------- */
/* Can be called from main thread or from signal handler thread. Used because
 * it is possible that connection to gpsd is blocked which would mean that
 * main thread is also blocked.
 */
static int quit(void)
{
	int st;

	/* note that the mutex is not unlocked anywhere! */
	st = pthread_mutex_trylock(&quit_mutex);
	if (st != 0) {
		/* already locked, just wait the exit */
		pause(); /* the other thread will do the cleanup */
		exit(0);
	}

	/* tell the beast to stop */
	gps_close(gpsd);

	st = gpsbt_stop(&ctx);
	if (st<0) {
		printf("gpsbt_stop(): error: %s, %d\n", strerror(errno), errno);
	}

	gzclose(fp);
	fp = NULL;

	return 0;
}


/* ----------------------------------------------------------------------- */
void *signal_handler(void *arg)
{
	sigset_t set;
	int sig, st;
	char *pidfile = (char *)arg;
	int dont_quit2 = 2;

	sigemptyset(&set);
	sigaddset(&set, SIGHUP);
	sigaddset(&set, SIGTERM);
	sigaddset(&set, SIGINT);

	if (pidfile) {
		pid_t pid = getpid();
		FILE *pidf;

		pidf = fopen(pidfile, "w");
		if (!pidf) {
			printf("Cannot write pid to pidfile (%s)\n", pidfile);
			perror(pidfile);
			exit(-1);
		}
		fprintf(pidf, "%lu", (unsigned long)pid);
		fclose(pidf);
	}


	/* If we get a second quit signal, then we quit the process because
	 * this condition probably means that the main thread is blocking
	 * and unable to stop.
	 */
	while (dont_quit2) {
		st = sigwait(&set, &sig);
		if (!st) {
			switch (sig) {
			case SIGHUP:
				printf("Restarting\n");
				restart = 1;
				break;
			case SIGTERM:
			case SIGINT:
				printf("Quitting\n");
				dont_quit = 0;
				dont_quit2--;
				break;
			default:
				printf("Got signal %d\n", sig);
				break;
			}
		}
	}

	if (!dont_quit2) {
		quit();
		exit(0);
	}

	pthread_exit(NULL);  /* we should not get here if quit() is called */
}


/* ----------------------------------------------------------------------- */
/* using only n. digits precision i.e., truncate to n digits */
static inline double round2(double val, int digits)
{
	unsigned long val2;
	double val3;
	int i;
	int multiple = 1;

	if (digits>6)
		return val;

	for (i=0; i<digits; i++)
		multiple = multiple * 10;

	val2 = (unsigned long)(val * (double)multiple);
	val3 = (double)val2 / (double)multiple;

#if 0
	printf("val=%f, val2=%ld, val3=%f\n", val, val2, val3);
#endif

	return val3;
}

/* ----------------------------------------------------------------------- */
/* This function is called periodically by gpsd library */
void gps_callback(struct gps_data_t *sentence, char *buf, size_t len, int level)
{
	if (!sentence) {
		PDEBUG("Sentence not set, len=%d\n", len);
		return;
	}

	if (!restart && fp) {

		/* data good for latitude, longitude & altitude */
		/* the libgps tells us if the position has changed */
		if (sentence->status &&

		    sentence->online &&
		    sentence->fix.mode>1 &&
		    (sentence->set & (LATLON_SET | ALTITUDE_SET))) {

			/* We only print data if it has been changed and
			 * if latitude and longitude are real numbers.
			 */
			static double prev_lat = 0.0;
			static double prev_lon = 0.0;
			static double prev_alt = 0.0;

			double lat, lon, alt;
			lat = round2(sentence->fix.latitude,4);
			lon = round2(sentence->fix.longitude,4);
			alt = round(sentence->fix.altitude);

			PDEBUG("recv %d bytes, level=%d, online=%f, status=%s (%d), fix.mode=%d, set=0x%X\n",
			       len, level, sentence->online,
			       sentence->status==0 ? "NO FIX" :
			        (sentence->status==1 ? "FIX" :
				 (sentence->status==2 ? "DGPS FIX" : "<unknown>")),
			       sentence->status,
			       sentence->fix.mode, sentence->set);

			if ((prev_lat != lat ||
			     prev_lon != lon ||
			     prev_alt != alt)) {
				prev_lat = lat;
				prev_lon = lon;
				prev_alt = alt;

				PDEBUG("fix: "

				       "T=%lu "
				       "M=%d "
				       "EPT=%.3f "
				       "LAT=%.6f "
				       "LON=%.6f "
				       "EPH=%.3f "

				       "ALT=%.3f "
				       "EPV=%.3f "
				       "TRK=%.3f "
				       "EPD=%.3f "
				       "SPD=%.3f "
				       "EPS=%.3f "

				       "CLMB=%.3f "
				       "EPC=%.3f "
				       "PTCH=%.3f "
				       "ROLL=%.3f "
				       "DIP=%.3f "
				       "SU=%d "

				       "S=%d "
				       "HS=%c\n"
				       ,
				       (unsigned long)sentence->fix.time,
				       sentence->fix.mode,
				       sentence->fix.ept,
				       sentence->fix.latitude,
				       sentence->fix.longitude,
				       sentence->fix.eph,
				       sentence->fix.altitude,
				       sentence->fix.epv,
				       sentence->fix.track,
				       sentence->fix.epd,
				       sentence->fix.speed,
				       sentence->fix.eps,
				       sentence->fix.climb,
				       sentence->fix.epc,
				       sentence->fix.pitch,
				       sentence->fix.roll,
				       sentence->fix.dip,
				       sentence->satellites_used,
				       sentence->satellites,
				       (sentence->headingStatus==' ' || sentence->headingStatus=='\0') ? '?' : sentence->headingStatus
					);
				
				gzprintf(fp,
					 "T=%lu "

					 "M=%d "
					 "EPT=%.3f "
					 "LAT=%.6f "
					 "LON=%.6f "
					 "EPH=%.3f "
					 "ALT=%.3f "

					 "EPV=%.3f "
					 "TRK=%.3f "
					 "EPD=%.3f "
					 "SPD=%.3f "
					 "EPS=%.3f "
					 "CLMB=%.3f "

					 "EPC=%.3f "
					 "PTCH=%.3f "
					 "ROLL=%.3f "
					 "DIP=%.3f "
					 "SU=%d "
					 "S=%d "

					 "HS=%c\n"
					 ,
					 (unsigned long)sentence->fix.time,	/* Time of update, seconds since Unix epoch */
					 sentence->fix.mode,		/* Mode of fix, 0-3 unseen/none/2D/3D */
					 sentence->fix.ept,		/* Expected time uncertainty */
					 sentence->fix.latitude,	/* Latitude in degrees (valid if mode >= 2) */
					 sentence->fix.longitude,	/* Longitude in degrees (valid if mode >= 2) */
					 sentence->fix.eph,		/* Horizontal position uncertainty, meters */
					 sentence->fix.altitude,	/* Altitude in meters (valid if mode == 3) */
					 sentence->fix.epv,		/* Vertical position uncertainty, meters */
					 sentence->fix.track,		/* Course made good (relative to true north) */
					 sentence->fix.epd,		/* Track uncertainty, degrees */
					 sentence->fix.speed,		/* Speed over ground, meters/sec */
					 sentence->fix.eps,		/* Speed uncertainty, meters/sec */
					 sentence->fix.climb,		/* Vertical speed, meters/sec */
					 sentence->fix.epc,		/* Vertical speed uncertainty */
					 sentence->fix.pitch,		/* Pitch angle in degrees */
					 sentence->fix.roll,		/* Roll angle in degrees */
					 sentence->fix.dip,		/* Dip angle in degrees */
					 sentence->satellites_used,
					 sentence->satellites,
					 (sentence->headingStatus==' ' || sentence->headingStatus=='\0') ? '?' : sentence->headingStatus
					);
			} else {
				PDEBUG("Same coordinate as before: lat=%f, lon=%f, alt=%f\n", lat, lon, alt);
			}
		} else {
#if 0   /* too much data so do not print */
			PDEBUG("Status (%d), Not online (%f), invalid fix (%d) or not changed (0x%X)\n",
			       sentence->status, sentence->online, sentence->fix.mode, sentence->set);
#else
			;
#endif
		}
	} else {
		PDEBUG("Restarting or fp not set, restart=%d\n", restart);
	}

	/* print data to gui server if it is used */
	if (started_from_server) {
		printf("%lu DATA "

		       "T=%lu "
		       "M=%d "
		       "EPT=%.3f "
		       "LAT=%.6f "
		       "LON=%.6f "
		       "EPH=%.3f "

		       "ALT=%.3f "
		       "EPV=%.3f "
		       "TRK=%.3f "
		       "EPD=%.3f "
		       "SPD=%.3f "
		       "EPS=%.3f "

		       "CLMB=%.3f "
		       "EPC=%.3f "
		       "PTCH=%.3f "
		       "ROLL=%.3f "
		       "DIP=%.3f "
		       "SU=%d "

		       "S=%d "
		       "HS=%c\n"
		       ,
		       data_id++,
		       (unsigned long)sentence->fix.time,
		       sentence->fix.mode,
		       sentence->fix.ept,
		       sentence->fix.latitude,
		       sentence->fix.longitude,
		       sentence->fix.eph,
		       sentence->fix.altitude,
		       sentence->fix.epv,
		       sentence->fix.track,
		       sentence->fix.epd,
		       sentence->fix.speed,
		       sentence->fix.eps,
		       sentence->fix.climb,
		       sentence->fix.epc,
		       sentence->fix.pitch,
		       sentence->fix.roll,
		       sentence->fix.dip,
		       sentence->satellites_used,
		       sentence->satellites,
		       (sentence->headingStatus==' ' || sentence->headingStatus=='\0') ? '?' : sentence->headingStatus
			);
	}
}


/* ----------------------------------------------------------------------- */
static int cmd_error(char *id, char *fmt, char *str)
{
	if (id)
		printf("%s ", id);
	printf("ERROR ");
	printf(fmt, str);
	printf("\n");

	return -1;
}


/* ----------------------------------------------------------------------- */
static int cmd_ok(char *id)
{
	if (id)
		printf("%s ", id);
	printf("OK\n");
	return 0;
}


/* ----------------------------------------------------------------------- */
static inline int cmd_version(char *id)
{
	printf("%s %s\n", id, VERSION);
	return 0;
}


/* ----------------------------------------------------------------------- */
static inline int cmd_start(char *id)
{
	return cmd_error(id, "Not implemented yet: %s", "START");
}


/* ----------------------------------------------------------------------- */
static inline int cmd_stop(char *id)
{
#if 1
	dont_quit = 0;
	return cmd_ok(id);
#else
	return cmd_error(id, "Not implemented yet: %s", "STOP");
#endif
}


/* ----------------------------------------------------------------------- */
static inline int cmd_restart(char *id)
{
#if 1
	restart = 1;
	return cmd_ok(id);
#else
	return cmd_error(id, "Not implemented yet: %s", "RESTART");
#endif
}


/* ----------------------------------------------------------------------- */
static inline int cmd_radius(char *id, char *params)
{
	return cmd_error(id, "Not implemented yet: %s", "RADIUS");
}


/* ----------------------------------------------------------------------- */
static inline int cmd_device(char *id, char *params)
{
#if 1
	strncpy(gpsdev, params, MAX_GPSDEV_LEN);
	restart = 1;
	return cmd_ok(id);
#else
	return cmd_error(id, "Not implemented yet: %s", "GPS");
#endif
}



/* ----------------------------------------------------------------------- */
static int call(char *line)
{
	char *id=NULL, *cmd, *params, *p;

	p = strstr(line, " ");
	if (!p) {
		return cmd_error(line, "Syntax error: %s", line);
	}
	*p = '\0';
	p++;
	id = line;
	cmd = p;
	p = strstr(p, " ");
	if (!p) {
                /* eol */
		int cmd_len = strlen(cmd);
		if (cmd_len > 0)
			cmd[cmd_len-1] = '\0';
		params = NULL;
		
	} else {
		int params_len;
		*p++ = '\0';
		params = p;
		params_len = strlen(params);
		if (params_len > 0)
			params[params_len-1] = '\0';
	}

	PDEBUG("id=%s, cmd=%s, params=%s\n", id, cmd, params==0 ? "<N/A>" : params);

	if (!strcasecmp(cmd, "VERSION")) {
		return cmd_version(id);
	}

	if (!strcasecmp(cmd, "GPS")) {
		return cmd_device(id, params);
	}

	if (!strcasecmp(cmd, "START")) {
		return cmd_start(id);
	}

	if (!strcasecmp(cmd, "STOP")) {
		return cmd_stop(id);
	}

	if (!strcasecmp(cmd, "RESTART")) {
		return cmd_restart(id);
	}

	if (!strcasecmp(cmd, "RADIUS")) {
		return cmd_radius(id, params);
	}

	return cmd_error(id, "Invalid command: %s", cmd);
}


/* ----------------------------------------------------------------------- */
/* Wait data from user interface process */
static void *command_handler(void *arg)
{
	char buf[MAX_BUF+1];
	int idx = 0, st;
	int fd = fileno(stdin);

	while (dont_quit) {
		st = read(fd, &buf[idx], MAX_BUF-idx);
		if (!st) {
			dont_quit = 0; /* quit on eof */
			break;
		}

		PDEBUG("cmd received: \"%s\"\n", &buf[idx]);

		call(&buf[idx]);
		memset(buf, 0, MAX_BUF);
	}

	pthread_exit(NULL);
}


/* ----------------------------------------------------------------------- */
/* Query GPS daemon for coordinate. This is done in separate thread so that
 * main thread is not blocked if gps_query() blocks.
 */
static void *gpsd_query_handler(void *arg)
{
	int st;

	while (dont_quit) {

		/* we wait until the main thread triggers us to do the query */
		st = pthread_cond_wait(&query_gpsd_cond, &query_gpsd_mutex);
		if (st) {
			; /* error, what to do now */
		}

		/* Note that gpsd timeouts the connection after one minute
		 * if there is no traffic to the client i.e., to this process,
		 * so ask position data every 30 seconds.
		 */
		gps_query(gpsd, "p");  /* current position */
	}

	pthread_exit(NULL);
}


/* ----------------------------------------------------------------------- */
static int daemon_init(void)
{
	pid_t pid;
	if ((pid=fork())<0)
		return -1;
	else if (pid!=0)
		exit(0); /* parent exists */

	setsid();
	chdir("/");
	umask(0);
	return(0);
}


/* ----------------------------------------------------------------------- */
int main(int argc, char **argv)
{
	int c;
	char *file = 0;
	int ret = 0, st;
	struct gps_data_t *gps;
	pthread_t th_sig, th_cmd, th_gps, th_ask;
	int radius_arg;
	sigset_t set;
	int daemonize = 0;
	char *pidfile = NULL;
	int do_some_testing = 0;
	unsigned int flush_timeout = 0;
	unsigned long count = 0;

	/* GPS BT vars */
	int gpsbt_debug_level = 0;
	int gpsmgr_debug_level = 0;
	int gpsd_debug_level = 0;
	char errbuf[MAX_ERROR_BUF_LEN+1] = {0};
	char *bda = NULL;

	srand(time(0));
	data_id = (unsigned long)rand();
	debug_level = 0;

	while ((c=getopt(argc, argv, "bDf:F:g:hi:p:r:sx:y:z:T"))>-1) {

		switch (c) {

		case 'b':
			daemonize = 1;
			break;

		case 'D':
			debug_level = 1;
			break;

		case 'f':
			file = optarg;
			break;

		case 'F':
			flush_timeout = atoi(optarg);
			break;

		case 'g':
			strncpy(gpsdev, optarg, MAX_GPSDEV_LEN);
			break;

		case 'i':
			pidfile = optarg;
			break;

		case 'p':
			bda = optarg;
			break;

		case 'r':
			radius_arg = atoi(optarg);
			if (radius_arg!=0)
				radius=radius_arg;
			break;

		case 's':
			started_from_server = 1;
			break;

		case 'x':
			gpsmgr_debug_level = atoi(optarg);
			break;

		case 'y':
			gpsbt_debug_level = atoi(optarg);
			break;

		case 'z':
			gpsd_debug_level = atoi(optarg);
			break;

		case 'T':
			do_some_testing=1;
			break;

		default:
			printf("Invalid option found.\n");
		case 'h':
			usage(argv[0], 0);
			break;
		}
	}

	if (do_some_testing) {
#if 1
		printf("-T parameter not in use at the moment\n");
#else
#define MAX_SENTENCE_LEN 64
#define R 1.23456789 /* some random value */
#define LAT 60.23456789
#define LON 24.23456789
#define ALT 10.23456789
		struct gps_data_t sentence = {0};
		char buf[MAX_SENTENCE_LEN+1] = {0};

		snprintf(buf, MAX_SENTENCE_LEN, "foo bar");

		restart = 0;
		fp = stdout;

		sentence.fix.time = (double)time(0);
		sentence.fix.mode = 2;
		sentence.fix.ept = R;
		sentence.fix.latitude = LAT;
		sentence.fix.longitude = LON;
		sentence.fix.eph = R;
		sentence.fix.altitude = ALT;
		sentence.fix.epv = R;
		sentence.fix.track = R;
		sentence.fix.speed = R;

		sentence.status = 1; /* fix */
		sentence.online = 1;
		sentence.set |= LATLON_SET | ALTITUDE_SET;
	
		gps_callback(&sentence, buf, MAX_SENTENCE_LEN, 1);
		printf("stop\n");
		exit(1);
#endif
	}

	/* Then use the libgpsbt library which starts the gps daemon */
	if (bda) {
		printf("Initiating pairing to %s, restart this program after pairing is finished.\n", bda);
		gpsbt_init_pairing(bda);
		exit(1); /* wait the pairing to finish and then restart this program */
	}

	if (!file) {
		usage(argv[0], "File name must be given.\n");
	}


	if (daemonize)
		daemon_init();

	/* Signal handler checks normal signals so we must block them now */
	sigemptyset(&set);
	sigaddset(&set, SIGHUP);
	sigaddset(&set, SIGTERM);
	sigaddset(&set, SIGINT);
	sigprocmask(SIG_BLOCK, &set, NULL);

	/* Following threads are started:
	 *    - command thread (only if started with -s option)
	 *    - signal handler
	 */
	pthread_create(&th_sig, NULL, signal_handler, (void *)pidfile);
	pthread_create(&th_ask, NULL, gpsd_query_handler, NULL);

	if (started_from_server)
		pthread_create(&th_cmd, NULL, command_handler, NULL);

	if (gpsmgr_debug_level)
		gpsmgr_set_debug_mode(gpsmgr_debug_level);

	/* Note that output file is not closed in restart case because gzip
	 * file will be truncated. Also "a" (=append) flag cannot be used
	 * because it seems to confuse gzip library (gunzip says the file is
	 * corrupted).
	 */
	fp = NULL;

	fp = gzopen(file, "w");
	if (fp == NULL) {
		perror("gzopen");
		exit(-1);
	}

RESTART:
	restart = 0;
	count = 0;

	st = gpsbt_start(gpsdev[0] ? gpsdev : NULL,
			 gpsbt_debug_level,
			 gpsd_debug_level,
			 0, /* port */
			 errbuf, MAX_ERROR_BUF_LEN,
			 0, /* timeout */
			 &ctx);
	if (st<0) {
		printf("gpsbt_start(): error: %s, %d [%s]\n", strerror(errno), errno, errbuf);
		goto OUT;
	}

	/* let the gpsd process start */
	sleep(1);

	gpsd = gps_open("127.0.0.1", DEFAULT_GPSD_PORT);  /* default port is 2947 */

	gps_set_callback(gpsd, gps_callback, &th_gps);

	/* Then just wait the data from gps device */
	while (dont_quit) {

		if (restart) {
			gps_close(gpsd);

			st = gpsbt_stop(&ctx);
			if (st<0) {
				printf("gpsbt_stop(): error: %s, %d\n", strerror(errno), errno);
			}

			goto RESTART;
		}

		if (!dont_quit)
			break;

		/* restart the beast if gpsd stops */
		if (!gpsmgr_is_gpsd_running(&ctx.mgr, NULL, GPSMGR_MODE_JUST_CHECK)) {
			PDEBUG("ERROR, gpsd is not running, restarting\n");
			restart = 1;
			continue;
		}

		/* Flush gzip file if necessary */
		if (flush_timeout && !(time(0) % flush_timeout)) {
			if (fp) {
				st = gzflush(fp, Z_SYNC_FLUSH);
				if (st != Z_OK) {
					int val = 0;
					printf("ERROR: %s\n", gzerror(fp, &val));
				} else {
					PDEBUG("Flushed %s\n", file);
				}
			}
		}

		if (!dont_quit)
			break;

		/* ask current position from gpsd */
		if (!(count % 30)) {
			pthread_cond_signal(&query_gpsd_cond);
		}

		sleep(1);
		count++;
	}

	quit();

OUT:
	exit(ret);
}