/* 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 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);
	}
	if (fp) 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) {
		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);

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

				gzprintf(fp,
					 "T=%lu "

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

					 "EPV=%.3f "
					 "TRK=%.3f "
					 "EPD=%.3f "
					 "SPD=%.3f "
					 "\n"
					 ,
					 (unsigned long)sentence->fix.time,
					 sentence->fix.mode,
					 sentence->fix.latitude,
					 sentence->fix.longitude,
					 sentence->fix.eph,
					 sentence->fix.altitude,
					 sentence->fix.epv,
					 sentence->fix.track,
					 sentence->fix.epd,
					 sentence->fix.speed
					);
			}
		}
	}

	/* print data to gui server if it is used */
	if (started_from_server) {
		static double prev_lat = 0.0;
		static double prev_lon = 0.0;
		double lat, lon;
		lat = round2(sentence->fix.latitude,6);
		lon = round2(sentence->fix.longitude,6);

		if ((prev_lat != lat || prev_lon != lon || 0 ) && sentence->fix.time ) {
		    prev_lat = lat;
		    prev_lon = lon;
		    printf(
		       "T=%lu "
		       "M=%d "
		       "LAT=%.6f "
		       "LON=%.6f "
		       "EPH=%.3f "

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

		       "\n"
		       ,
		       (unsigned long)sentence->fix.time,
		       sentence->fix.mode,
		       sentence->fix.latitude,
		       sentence->fix.longitude,
		       sentence->fix.eph,
		       sentence->fix.altitude,
		       sentence->fix.epv,
		       sentence->fix.track,
		       sentence->fix.epd,
		       sentence->fix.speed
			);
		}
	}
}


/* ----------------------------------------------------------------------- */
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';
	}

	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;
		}

		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;
	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();

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

		switch (c) {

		case 'b':
			daemonize = 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;

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


	/* 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 (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;

	if (file) {
	  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)) {
			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));
				}
			}
		}

		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);
}