/*
 * Venus GPS Controller Example
 *
 * Copyright (C) 2010 by Multi-Tech Systems
 *
 * Author: James Maki <jmaki@multitech.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 */

#define _GNU_SOURCE

#include <pthread.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>
#include <stdarg.h>
#include <poll.h>
#include <getopt.h>
#include <time.h>
#include <signal.h>
#include <string.h>
#include <sys/time.h>

#include "log.h"
#include "cbuffer.h"
#include "utils.h"
#include "venus_gps.h"
#include "venus_api.h"

#define BIT(nr)			(1UL << (nr))

#define ID_PREFIX_MAX		10
#define ID_MAX			20

#define SLEEP_AFTER_FAILURE	5

static char id[ID_MAX + 1];
static char id_prefix[ID_PREFIX_MAX + 1];

static char *device = "/dev/ttyS3";
static speed_t baud_rate = B9600;

#define SOCKET_CLIENTS_MAX	8
struct socket_client_args {
	char *host;
	int port;
	int type;
	pthread_t tid;
};
int n_socket_clients = 0;
struct socket_client_args socket_clients[SOCKET_CLIENTS_MAX];

static int gps_tcp_server_port = -1;
static char *gps_filename = NULL;
static char *gps_serial_port = NULL;

static uint8_t gpgga = 1;
static uint8_t gpgsa = 1;
static uint8_t gpgsv = 1;
static uint8_t gpgll = 1;
static uint8_t gprmc = 1;
static uint8_t gpvtg = 1;
#if CONFIG_HAVE_ZDA
static uint8_t gpzda = 1;
#endif

#if CONFIG_CAN_RESET
/*
 * XXX: I don't know if these work, yet. I haven't tried them out.
 *
 */
static uint8_t start_mode = (uint8_t) - 1;
static int16_t latitude = 45;
static int16_t longitude = -93;
static int16_t altitude = 256;
#endif

struct gps_msg {
	char gpgga[NMEA_SENTENCE_MAX + 1];
	char gpgsa[NMEA_SENTENCE_MAX + 1];
	char gpgsv[3][NMEA_SENTENCE_MAX + 1];
	char gpgll[NMEA_SENTENCE_MAX + 1];
	char gprmc[NMEA_SENTENCE_MAX + 1];
	char gpvtg[NMEA_SENTENCE_MAX + 1];
	struct timeval timestamp;
};

static struct {
	pthread_cond_t msg_cond;
	pthread_mutex_t msg_mutex;
	struct gps_msg *msg;
	uint32_t consumed;
	uint32_t consumers;

	struct gps_msg msgs_[2];
} producer = {
	.msg_cond = PTHREAD_COND_INITIALIZER,
	.msg_mutex = PTHREAD_MUTEX_INITIALIZER,
	.msg = NULL,
	.consumed = 0xFFFFFFFF,
	.consumers = 0,
};

static int fill_msg(int fd, struct gps_msg *msg)
{
	char buf[NMEA_SENTENCE_MAX + 1];
	int tmp;

	memset(msg, 0, sizeof(*msg));

	do {
		tmp = venus_read_nmea_sentence(fd, buf, sizeof(buf));
		if (tmp <= 0) {
			log_error("venus_read_nmea_sentence failed");
			return -1;
		}
	} while (strncmp(buf, "$GPGGA", 6));

	strcpy(msg->gpgga, buf);

	while (1) {
		tmp = venus_read_nmea_sentence(fd, buf, sizeof(buf));
		if (tmp <= 0) {
			log_error("venus_read_nmea_sentence failed");
			return -1;
		}
		if (!strncmp(buf, "$GPGGA", 6)) {
			strcpy(msg->gpgga, buf);
		} else if (!strncmp(buf, "$GPGSA", 6)) {
			strcpy(msg->gpgsa, buf);
		} else if (!strncmp(buf, "$GPGSV", 6)) {
			if (buf[9] == '1') {
				strcpy(msg->gpgsv[0], buf);
			} else if (buf[9] == '2') {
				strcpy(msg->gpgsv[1], buf);
			} else if (buf[9] == '3') {
				strcpy(msg->gpgsv[2], buf);
			}
		} else if (!strncmp(buf, "$GPGLL", 6)) {
			strcpy(msg->gpgll, buf);
		} else if (!strncmp(buf, "$GPRMC", 6)) {
			strcpy(msg->gprmc, buf);
		} else if (!strncmp(buf, "$GPVTG", 6)) {
			strcpy(msg->gpvtg, buf);
			break;
		}
	}

	return 0;
}

static void *gps_msg_producer(void *arg)
{
	int fd;
	int tmp;
	struct gps_msg *msg;
	int msg_no;

	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);

	log_notice("Venus GPS NMEA Producer starting");

	fd = venus_open(device, baud_rate);
	if (fd < 0) {
		log_error("failed to open %s", device);
		goto error;
	}

	msg_no = 0;
	while (1) {
		msg = &producer.msgs_[msg_no];

		tmp = fill_msg(fd, msg);
		if (tmp < 0) {
			log_error("fill_msg failed");
			goto error;
		}

		tmp = gettimeofday(&msg->timestamp, NULL);
		if (tmp < 0) {
			log_error("gettimeofday failed: %m");
			goto error;
		}

		tmp = pthread_mutex_lock(&producer.msg_mutex);
		if (tmp) {
			log_emerg("pthread_mutex_lock failed: %d", tmp);
			exit(1);
		}

		producer.msg = msg;
		producer.consumed = 0;

		tmp = pthread_cond_broadcast(&producer.msg_cond);
		if (tmp) {
			log_error("pthread_cond_broadcast failed: %d", tmp);
			exit(1);
		}

		tmp = pthread_mutex_unlock(&producer.msg_mutex);
		if (tmp) {
			log_emerg("pthread_mutex_unlock failed: %d", tmp);
			exit(1);
		}

		msg_no = !msg_no;
	}

error:

	if (fd >= 0) {
		close(fd);
	}

	log_notice("exiting");

	pthread_exit((void *) 1);
}

struct gps_timeval {
	struct timeval gpgga;
	struct timeval gpgsa;
	struct timeval gpgsv;
	struct timeval gpgll;
	struct timeval gprmc;
	struct timeval gpvtg;
};

enum {
	WRITE_MSG_TCP,
	WRITE_MSG_UDP,
	WRITE_MSG_FILE,
};

int write_sentence(int fd, const char *buf, size_t len)
{
	int tmp;

	if (!*buf || len <= 0) {
		return 0;
	}

	if (*id_prefix) {
		tmp = full_write(fd, id_prefix, strlen(id_prefix));
		if (tmp < 0) {
			log_error("write id-prefix failed: %m");
			return tmp;
		}
	}
	if (*id) {
		tmp = full_write(fd, id, strlen(id));
		if (tmp < 0) {
			log_error("write id failed: %m");
			return tmp;
		}
	}
	tmp = full_write(fd, buf, len);
	if (tmp < 0) {
		log_error("write sentence failed: %m");
		return tmp;
	}

	return tmp;
}

int write_msg(int fd, struct gps_msg *msg, struct gps_timeval *gpstv, int where)
{
	struct timeval tmptv;
	int tmp = 0;
	int count = 0;

	if (where == WRITE_MSG_TCP) {
		tcp_cork(fd, 1);
	} else if (where == WRITE_MSG_UDP) {
		udp_cork(fd, 1);
	}

	if (gpgga && timercmp(&gpstv->gpgga, &msg->timestamp, <=)) {
		tmp = write_sentence(fd, msg->gpgga, strlen(msg->gpgga));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}

		tmptv.tv_sec = gpgga - 1;
		tmptv.tv_usec = 750000;
		timeradd(&msg->timestamp, &tmptv, &gpstv->gpgga);
		count++;
	}
	if (gpgsa && timercmp(&gpstv->gpgsa, &msg->timestamp, <=)) {
		tmp = write_sentence(fd, msg->gpgsa, strlen(msg->gpgsa));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}

		tmptv.tv_sec = gpgsa - 1;
		tmptv.tv_usec = 750000;
		timeradd(&msg->timestamp, &tmptv, &gpstv->gpgsa);
		count++;
	}
	if (gpgsv && timercmp(&gpstv->gpgsv, &msg->timestamp, <=)) {
		tmp = write_sentence(fd, msg->gpgsv[0], strlen(msg->gpgsv[0]));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}
		tmp = write_sentence(fd, msg->gpgsv[1], strlen(msg->gpgsv[1]));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}
		tmp = write_sentence(fd, msg->gpgsv[2], strlen(msg->gpgsv[2]));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}

		tmptv.tv_sec = gpgsv - 1;
		tmptv.tv_usec = 750000;
		timeradd(&msg->timestamp, &tmptv, &gpstv->gpgsv);
		count++;
	}
	if (gpgll && timercmp(&gpstv->gpgll, &msg->timestamp, <=)) {
		tmp = write_sentence(fd, msg->gpgll, strlen(msg->gpgll));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}

		tmptv.tv_sec = gpgll - 1;
		tmptv.tv_usec = 750000;
		timeradd(&msg->timestamp, &tmptv, &gpstv->gpgll);
		count++;
	}
	if (gprmc && timercmp(&gpstv->gprmc, &msg->timestamp, <=)) {
		tmp = write_sentence(fd, msg->gprmc, strlen(msg->gprmc));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}

		tmptv.tv_sec = gprmc - 1;
		tmptv.tv_usec = 750000;
		timeradd(&msg->timestamp, &tmptv, &gpstv->gprmc);
		count++;
	}
	if (gpvtg && timercmp(&gpstv->gpvtg, &msg->timestamp, <=)) {
		tmp = write_sentence(fd, msg->gpvtg, strlen(msg->gpvtg));
		if (tmp < 0) {
			log_error("write_sentence failed");
			goto error;
		}

		tmptv.tv_sec = gpvtg - 1;
		tmptv.tv_usec = 750000;
		timeradd(&msg->timestamp, &tmptv, &gpstv->gpvtg);
		count++;
	}

error:

	if (where == WRITE_MSG_TCP) {
		tcp_cork(fd, 0);
	} else if (where == WRITE_MSG_UDP) {
		udp_cork(fd, 0);
	}

	return tmp < 0 ? tmp : count;
}

static int gps_client_consume(int fd, int consumer, int where)
{
	struct gps_msg msg;
	struct gps_timeval gpstv;
	int tmp;

	memset(&gpstv, 0, sizeof(gpstv));

	while (1) {
		tmp = pthread_mutex_lock(&producer.msg_mutex);
		if (tmp) {
			log_emerg("pthread_mutex_lock failed: %d", tmp);
			exit(1);
		}

		while (producer.consumed & BIT(consumer)) {
			tmp = pthread_cond_wait(&producer.msg_cond, &producer.msg_mutex);
			if (tmp) {
				log_emerg("pthread_cond_wait failed: %d", tmp);
				exit(1);
			}
		}

		memcpy(&msg, producer.msg, sizeof(msg));
		producer.consumed |= BIT(consumer);

		tmp = pthread_mutex_unlock(&producer.msg_mutex);
		if (tmp) {
			log_emerg("pthread_mutex_unlock failed: %d", tmp);
			exit(1);
		}

		tmp = write_msg(fd, &msg, &gpstv, where);
		if (tmp < 0) {
			log_error("write_msg failed");
			return -1;
		}
	}

	return -1;
}

static int consumer_add(void)
{
	int tmp;
	int consumer;

	tmp = pthread_mutex_lock(&producer.msg_mutex);
	if (tmp) {
		log_emerg("pthread_mutex_lock failed: %d", tmp);
		exit(1);
	}

	consumer = ffs(~producer.consumers);
	if (consumer) {
		producer.consumers |= BIT(consumer - 1);
	}

	tmp = pthread_mutex_unlock(&producer.msg_mutex);
	if (tmp) {
		log_emerg("pthread_mutex_unlock failed: %d", tmp);
		exit(1);
	}

	return consumer - 1;
}

static void consumer_delete(int consumer)
{
	int tmp;

	tmp = pthread_mutex_lock(&producer.msg_mutex);
	if (tmp) {
		log_emerg("pthread_mutex_lock failed: %d", tmp);
		exit(1);
	}

	producer.consumers &= ~BIT(consumer);

	tmp = pthread_mutex_unlock(&producer.msg_mutex);
	if (tmp) {
		log_emerg("pthread_mutex_unlock failed: %d", tmp);
		exit(1);
	}
}

static void *gps_socket_client(void *arg)
{
	int sd;
	int consumer;
	char *type_name = NULL;
	int write_type;
	struct socket_client_args *sc = (struct socket_client_args *) arg;

	switch (sc->type) {
	case SOCK_STREAM:
		type_name = "TCP";
		write_type = WRITE_MSG_TCP;
		break;
	case SOCK_DGRAM:
		type_name = "UDP";
		write_type = WRITE_MSG_UDP;
		break;
	default:
		goto error;
	}

	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);

	log_notice("Venus GPS %s Client connecting to %s:%d",
			type_name, sc->host, sc->port);

	consumer = consumer_add();
	if (consumer < 0) {
		log_error("no more free consumers: exiting");
		goto error;
	}

	while (1) {
		sd = inet_conn_str(sc->host, sc->port, sc->type);
		if (sd < 0) {
			log_error("inet_conn_str failed: %m");
			sleep(SLEEP_AFTER_FAILURE);
		} else {
			log_info("connection opened");
			gps_client_consume(sd, consumer, write_type);
			close(sd);
			log_info("connection closed");
		}
	}

error:

	if (consumer >= 0) {
		consumer_delete(consumer);
	}

	log_notice("exiting");

	pthread_exit((void *) 1);
}

static void *gps_tcp_server_conn(void *arg)
{
	int sd = (int) (long) arg;
	int consumer;

	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);

	log_notice("Venus GPS TCP Server Conn connected");

	consumer = consumer_add();
	if (consumer < 0) {
		log_error("no more free consumers: exiting");
		goto error;
	}

	gps_client_consume(sd, consumer, WRITE_MSG_TCP);

error:
	if (consumer >= 0) {
		consumer_delete(consumer);
	}

	close(sd);

	log_notice("exiting");

	pthread_exit((void *) 1);
}

static void *gps_tcp_server(void *arg)
{
	int tmp;
	int lsd;
	int sd;
	int cli_addr_len;
	int serv_addr_len;
	int reuse = 1;

	struct sockaddr_in serv_addr;
	struct sockaddr_in cli_addr;

	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);

	lsd = socket(PF_INET, SOCK_STREAM, 0);
	if (lsd == -1) {
		log_error("socket failed: %m");
		goto error;
	}

	memset(&serv_addr, 0, sizeof(serv_addr));
	serv_addr.sin_family = AF_INET;
	serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
	serv_addr.sin_port = htons(gps_tcp_server_port);

	tmp = setsockopt(lsd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
	if (tmp == -1) {
		log_error("setsockopt failed: %m");
		goto error;
	}

	serv_addr_len = sizeof(serv_addr);

	tmp = bind(lsd, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
	if (tmp == -1) {
		log_error("bind failed: %m");
		goto error;
	}

	tmp = listen(lsd, 4);
	if (tmp == -1) {
		log_error("listen failed: %m");
		goto error;
	}

	log_notice("Venus GPS TCP Server listening on port %d", gps_tcp_server_port);

	while (1) {
		cli_addr_len = sizeof(cli_addr);

		sd = accept(lsd, (struct sockaddr *) &cli_addr, (socklen_t *) & cli_addr_len);
		if (sd == -1) {
			log_error("accept failed: %m");
		} else {
			pthread_t tid;

			tmp = pthread_create(&tid, NULL, gps_tcp_server_conn, (void *) (long) sd);
			if (tmp) {
				log_error("pthread_create gps_tcp_server_conn failed: %d", tmp);
				close(sd);
				continue;
			}

			tmp = pthread_detach(tid);
			if (tmp) {
				log_error("pthread_detach failed: %d", tmp);
			}
		}
	}

error:
	if (lsd >= 0) {
		close(lsd);
	}

	log_notice("exiting");

	pthread_exit((void *) 1);
}

static void *gps_file_client(void *arg)
{
	int fd;
	int consumer = -1;

	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);

	log_notice("Venus GPS to file %s", gps_filename);

	if (!strcmp(gps_filename, "-")) {
		fd = fileno(stdout);
	} else {
		fd = open(gps_filename, O_WRONLY);
		if (fd < 0) {
			log_error("failed to open %s: %m", gps_filename);
			goto error;
		}
	}

	consumer = consumer_add();
	if (consumer < 0) {
		log_error("no more free consumers: exiting");
		goto error;
	}

	gps_client_consume(fd, consumer, WRITE_MSG_FILE);

error:
	if (consumer >= 0) {
		consumer_delete(consumer);
	}

	if (fd >= 0) {
		close(fd);
	}

	log_notice("exiting");

	pthread_exit((void *) 1);
}

static void *gps_serial_client(void *arg)
{
	int fd;
	struct termios tio;
	int consumer = -1;

	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);

	log_notice("Venus GPS to serial %s", gps_serial_port);

	fd = open(gps_serial_port, O_RDWR | O_NOCTTY);
	if (fd < 0) {
		log_error("failed to open %s: %m", gps_serial_port);
		goto error;
	}

	tcgetattr(fd, &tio);
	cfmakeraw(&tio);
	cfsetspeed(&tio, B115200);

	tio.c_cflag &= ~CSIZE;
	tio.c_cflag |= CS8;

	tio.c_cflag &= ~(PARENB | PARODD);
	tio.c_cflag |= 0;

	tio.c_cflag &= ~CSTOPB;
	tio.c_cflag |= 0;

	tio.c_cflag &= ~CRTSCTS;
	tio.c_cflag |= 0;

	tcsetattr(fd, TCSANOW, &tio);
	tcflush(fd, TCIOFLUSH);

	consumer = consumer_add();
	if (consumer < 0) {
		log_error("no more free consumers: exiting");
		goto error;
	}

	gps_client_consume(fd, consumer, WRITE_MSG_FILE);

error:
	if (consumer >= 0) {
		consumer_delete(consumer);
	}

	if (fd >= 0) {
		close(fd);
	}

	log_notice("exiting");

	pthread_exit((void *) 1);
}

static void print_version(const char *name)
{
	printf("%s (" PACKAGE ") " VERSION " (" __DATE__ " " __TIME__ ")\n", name);
	printf("Copyright (C) 2010 by Multi-Tech Systems\n");
	printf(
"This program is free software; you may redistribute it under the terms of\n"
"the GNU General Public License version 2 or (at your option) any later version.\n"
"This program has absolutely no warranty.\n");
}

static void usage(FILE * out)
{
	fprintf(out, "usage: venus-gps [ OPTIONS ... ]\n");
	fprintf(out, "where  OPTIONS := { \n");
	fprintf(out, "         --daemonize |\n");
	fprintf(out, "         -d, --device <device> (default: /dev/ttyS3) |\n");
	fprintf(out, "         --file <file> |\n");
	fprintf(out, "         --tcp-server |\n");
	fprintf(out, "         --tcp-client <host> |\n");
	fprintf(out, "         --udp-client <host> |\n");
	fprintf(out, "         --serial <device> |\n");
	fprintf(out, "         --id ID |\n");
	fprintf(out, "         --id-prefix PREFIX |\n");
#if CONFIG_CAN_DEFAULT
	fprintf(out, "         -f, --factory-defaults |\n");
#endif
#if CONFIG_CAN_RESET
	fprintf(out, "\n");
	fprintf(out, "         --start-mode 0-5 (default: 0) |\n");
	fprintf(out, "         --latitude VAL (default: ) |\n");
	fprintf(out, "         --longitude VAL (default: ) |\n");
	fprintf(out, "         --altitude VAL (default: ) |\n");
#endif
	fprintf(out, "\n");
	fprintf(out, "         --gpgga 0-255 (default: 1) |\n");
	fprintf(out, "         --gpgsa 0-255 (default: 1) |\n");
	fprintf(out, "         --gpgsv 0-255 (default: 1) |\n");
	fprintf(out, "         --gpgll 0-255 (default: 1) |\n");
	fprintf(out, "         --gprmc 0-255 (default: 1) |\n");
#if CONFIG_HAVE_ZDA
	fprintf(out, "         --gpzda 0-255 (default: 1) |\n");
#endif
	fprintf(out, "         --gpvtg 0-255 (default: 1)\n");
	fprintf(out, "       }\n");
	fprintf(out, "\n");
}

#define OPT_DEVICE		'd'
#define OPT_BAUD_RATE		'b'
#define OPT_PORT		'p'
#define OPT_FACTORY_DEFAULTS	'f'

enum {
	OPT_VERSION = 128,
	OPT_HELP,
	OPT_GPGGA,
	OPT_GPGSA,
	OPT_GPGSV,
	OPT_GPGLL,
	OPT_GPRMC,
	OPT_GPVTG,
	OPT_GPZDA,
	OPT_START_MODE,
	OPT_LATITUDE,
	OPT_LONGITUDE,
	OPT_ALTITUDE,
	OPT_DAEMONIZE,
	OPT_FILE,
	OPT_TCP_SERVER,
	OPT_TCP_CLIENT,
	OPT_UDP_CLIENT,
	OPT_SERIAL_CLIENT,
	OPT_ID_PREFIX,
	OPT_ID,
};

static char *short_options = "b:d:p:f";
static struct option long_options[] = {
	{"daemonize", 0, 0, OPT_DAEMONIZE},
	{"device", 1, 0, OPT_DEVICE},
	{"baud-rate", 1, 0, OPT_BAUD_RATE},
	{"port", 1, 0, OPT_PORT},
	{"file", 1, 0, OPT_FILE},
	{"tcp-server", 0, 0, OPT_TCP_SERVER},
	{"tcp-client", 1, 0, OPT_TCP_CLIENT},
	{"udp-client", 1, 0, OPT_UDP_CLIENT},
	{"serial-client", 1, 0, OPT_SERIAL_CLIENT},
	{"id", 1, 0, OPT_ID},
	{"id-prefix", 1, 0, OPT_ID_PREFIX},
#if CONFIG_CAN_DEFAULT
	{"factory-defaults", 0, 0, OPT_FACTORY_DEFAULTS},
#endif
	{"gpgga", 1, 0, OPT_GPGGA},
	{"gpgsa", 1, 0, OPT_GPGSA},
	{"gpgsv", 1, 0, OPT_GPGSV},
	{"gpgll", 1, 0, OPT_GPGLL},
	{"gprmc", 1, 0, OPT_GPRMC},
	{"gpvtg", 1, 0, OPT_GPVTG},
#if CONFIG_HAVE_ZDA
	{"gpzda", 1, 0, OPT_GPZDA},
#endif
#if CONFIG_CAN_RESET
	{"start-mode", 1, 0, OPT_START_MODE},
	{"latitude", 1, 0, OPT_LATITUDE},
	{"longitude", 1, 0, OPT_LONGITUDE},
	{"altitude", 1, 0, OPT_ALTITUDE},
#endif
	{"version", 0, NULL, OPT_VERSION},
	{"help", 0, NULL, OPT_HELP},
	{0, 0, 0, 0},
};

int main(int argc, char *argv[])
{
	int i;
	int tmp;
	char *cp;
	int option_index;
	int tty;
	struct venus_msg msg;

	struct socket_client_args *sc;

#if CONFIG_CAN_DEFAULT
	int factory_defaults = 0;
#endif
	int daemonize = 0;
	int port = 5445;

	while ((i = getopt_long(argc, argv, short_options, long_options, &option_index)) >= 0) {
		switch (i) {
		case 0:
			break;

		case OPT_DAEMONIZE:
			daemonize = 1;
			break;

		case OPT_DEVICE:
			device = optarg;
			break;

		case OPT_PORT:
			port = atoi(optarg);
			if (port <= 0 || port >= (1 << 16)) {
				log_error("invalid port");
				usage(stderr);
				exit(1);
			}
			break;

		case OPT_FILE:
			gps_filename = optarg;
			break;

		case OPT_SERIAL_CLIENT:
			gps_serial_port = optarg;
			break;

		case OPT_TCP_SERVER:
			gps_tcp_server_port = port;
			break;

		case OPT_TCP_CLIENT:
			if (n_socket_clients >= SOCKET_CLIENTS_MAX) {
				log_error("max clients exceeded: %d",
						SOCKET_CLIENTS_MAX);
				usage(stderr);
				exit(1);
			}
			sc = &socket_clients[n_socket_clients++];

			cp = strchr(optarg, ':');
			if (cp) {
				sc->host = strndup(optarg, cp - optarg);

				cp++;

				sc->port = atoi(cp);
				if (sc->port <= 0 || sc->port >= (1 << 16)) {
					log_error("invalid port");
					usage(stderr);
					exit(1);
				}
			} else {
				sc->host = strdup(optarg);
				sc->port = port;
			}

			sc->type = SOCK_STREAM;

			break;

		case OPT_UDP_CLIENT:
			if (n_socket_clients >= SOCKET_CLIENTS_MAX) {
				log_error("max clients exceeded: %d",
						SOCKET_CLIENTS_MAX);
				usage(stderr);
				exit(1);
			}
			sc = &socket_clients[n_socket_clients++];

			cp = strchr(optarg, ':');
			if (cp) {
				sc->host = strndup(optarg, cp - optarg);

				cp++;

				sc->port = atoi(cp);
				if (sc->port <= 0 || sc->port >= (1 << 16)) {
					log_error("invalid port");
					usage(stderr);
					exit(1);
				}
			} else {
				sc->host = strdup(optarg);
				sc->port = port;
			}

			sc->type = SOCK_DGRAM;

			break;

		case OPT_BAUD_RATE:
			baud_rate = atoi(optarg);
			baud_rate = value_to_baud(baud_rate);
			if (baud_rate == (speed_t) - 1) {
				usage(stderr);
				exit(1);
			}
			break;

		case OPT_ID_PREFIX:
			tmp = strlen(optarg);
			if (tmp > ID_PREFIX_MAX) {
				log_error("id-prefix is too long max: %d > %d",
						tmp, ID_PREFIX_MAX);
				usage(stderr);
				exit(1);
			}
			snprintf(id_prefix, sizeof(id_prefix), "%s", optarg);
			break;

		case OPT_ID:
			tmp = strlen(optarg);
			if(tmp > ID_MAX) {
				log_error("id is too long max: %d > %d",
						tmp, ID_MAX);
				usage(stderr);
				exit(1);
			}
			if(strcspn(optarg, "$&") != tmp) {
				log_error("id contains invalid char $&");
				usage(stderr);
				exit(1);
			}
			snprintf(id, sizeof(id), "%s", optarg);
			break;

		case OPT_GPGGA:
			gpgga = atoi(optarg);
			break;

		case OPT_GPGSA:
			gpgsa = atoi(optarg);
			break;

		case OPT_GPGSV:
			gpgsv = atoi(optarg);
			break;

		case OPT_GPGLL:
			gpgll = atoi(optarg);
			break;

		case OPT_GPRMC:
			gprmc = atoi(optarg);
			break;

		case OPT_GPVTG:
			gpvtg = atoi(optarg);
			break;

#if CONFIG_HAVE_ZDA
		case OPT_GPZDA:
			gpzda = atoi(optarg);
			break;
#endif

#if CONFIG_CAN_RESET
		case OPT_START_MODE:
			start_mode = (uint8_t) atoi(optarg);
			break;

		case OPT_LATITUDE:
			latitude = (int16_t) atoi(optarg);
			break;

		case OPT_LONGITUDE:
			longitude = (int16_t) atoi(optarg);
			break;

		case OPT_ALTITUDE:
			altitude = (int16_t) atoi(optarg);
			break;
#endif

#if CONFIG_CAN_DEFAULT
		case OPT_FACTORY_DEFAULTS:
			factory_defaults = 1;
			break;
#endif

		case OPT_VERSION:
			print_version("venus-gps");
			exit(0);
			break;

		case OPT_HELP:
			usage(stdout);
			exit(0);
			break;

		default:
			usage(stderr);
			exit(1);
		}
	}

	if (optind < argc) {
		usage(stderr);
		exit(1);
	}

	if (daemonize) {
#if CONFIG_USE_SYSLOG
		tmp = daemon(0, 0);
#else
		tmp = daemon(0, 1);
#endif
	}
#if CONFIG_USE_SYSLOG
	openlog("VenusGPS", LOG_NDELAY, LOG_FACILITY);
# if DEBUG
	setlogmask(LOG_UPTO(LOG_DEBUG));
# else
	setlogmask(LOG_UPTO(LOG_INFO));
# endif
#endif

	tty = venus_open(device, baud_rate);
	if (tty < 0) {
		log_error("failed to open tty: %m");
		exit(1);
	}

	tmp = venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM);
	if (tmp < 0) {
		log_error("failed to change to binary format");
		exit(1);
	}

	tmp = venus_query_sw_version(tty, SW_TYPE_RESERVED, &msg);
	if (tmp < 0) {
		log_error("failed to query venus sw version info");
		exit(1);
	}
	log_notice("Venus Software Type: 0x%02X", msg.data[1]);
	log_notice("Venus Kernel Version: 0x%02X%02X%02X%02X",
			msg.data[2], msg.data[3], msg.data[4], msg.data[5]);
	log_notice("Venus ODM version: 0x%02X%02X%02X%02X",
			msg.data[6], msg.data[7], msg.data[8], msg.data[9]);
	log_notice("Venus Revision: 0x%02X%02X%02X%02X",
			msg.data[10], msg.data[11], msg.data[12], msg.data[13]);
	venus_msg_data_free(&msg);

#if CONFIG_CAN_DEFAULT
	if (factory_defaults) {
		log_notice("setting factory defaults");
		tmp = venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT);
		if (tmp < 0) {
			log_error("failed to set factory defaults");
			exit(1);
		}
		exit(0);
	}
#endif
#if CONFIG_CAN_RESET
	if (start_mode != (typeof(start_mode)) - 1) {
		log_notice("issuing system restart");
		tmp = venus_system_restart(tty, start_mode, time(NULL),
				latitude, longitude, altitude);
		if (tmp < 0) {
			log_error("system restart failed");
			exit(1);
		}
		exit(0);
	}
#endif

	tmp = venus_conf_nmea(tty, 1, 1, 1, 1, 1, 1, 0, UPDATE_ATTR_SRAM);
	if (tmp < 0) {
		log_error("failed to configure nmea sentences");
		exit(1);
	}

	tmp = venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM);
	if (tmp < 0) {
		log_error("failed to change to nmea format");
		exit(1);
	}

	venus_close(tty);

	sigset_t sigset;
	sigemptyset(&sigset);
	sigaddset(&sigset, SIGTERM);
	sigaddset(&sigset, SIGINT);
	sigaddset(&sigset, SIGPIPE);
	pthread_sigmask(SIG_BLOCK, &sigset, NULL);

	pthread_t gps_msg_producer_tid;
	pthread_t gps_serial_client_tid;
	pthread_t gps_file_client_tid;
	pthread_t gps_tcp_server_tid;

	tmp = pthread_create(&gps_msg_producer_tid, NULL, gps_msg_producer, NULL);
	if (tmp) {
		log_error("pthread_create gps_msg_producer failed: %d", tmp);
		exit(1);
	}

	for (i = 0; i < n_socket_clients; i++) {
		tmp = pthread_create(&socket_clients[i].tid, NULL,
				gps_socket_client, (void *) &socket_clients[i]);
		if (tmp) {
			log_error("pthread_create gps_socket_client failed: %d", tmp);
		}
	}

	if (gps_serial_port) {
		tmp = pthread_create(&gps_serial_client_tid, NULL,
				gps_serial_client, NULL);
		if (tmp) {
			log_error("pthread_create gps_serial_client failed: %d", tmp);
		}
	}
	if (gps_filename) {
		tmp = pthread_create(&gps_file_client_tid, NULL,
				gps_file_client, NULL);
		if (tmp) {
			log_error("pthread_create gps_file_client failed: %d", tmp);
		}
	}
	if (gps_tcp_server_port >= 0) {
		tmp = pthread_create(&gps_tcp_server_tid, NULL,
				gps_tcp_server, NULL);
		if (tmp) {
			log_error("pthread_create gps_tcp_server failed: %d", tmp);
		}
	}

	while (1) {
		siginfo_t info;
		struct timespec timeout = {
			.tv_sec = 1,
			.tv_nsec = 0,
		};

		tmp = sigtimedwait(&sigset, &info, &timeout);
		switch (tmp) {
		case -1:
			if (errno != EAGAIN) {
				log_error("sigtimedwait failed: %m");
			}
			break;
		case SIGINT:
			log_notice("SIGINT caught");
			exit(0);
			break;
		case SIGTERM:
			log_notice("SIGTERM caught");
			exit(0);
			break;
		case SIGPIPE:
			log_notice("SIGPIPE caught");
			break;
		default:
			log_error("unhandled signal %d caught", tmp);
			break;
		}

		/* Check for exited threads */
	}

	return 0;
}