/*
 * 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 <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 "log.h"
#include "cbuffer.h"
#include "utils.h"
#include "venus_gps.h"
#include "venus_api.h"

static sig_atomic_t timer_expired;
static void sigalarm_handler(int arg)
{
	timer_expired = 1;
}

static sig_atomic_t sigpipe = 0;
static void sigpipe_handler(int arg)
{
	sigpipe = 1;
}

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

static uint8_t gga = 1;
static uint8_t gsa = 1;
static uint8_t gsv = 1;
static uint8_t gll = 1;
static uint8_t rmc = 1;
static uint8_t vtg = 1;
static uint8_t zda = 1;

#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


static int exchange_data(int fd0, int fd1)
{
	int i;
	int err;
	struct pollfd fds[2];
	nfds_t nfds = 2;
	struct cbuffer bufs[2];

	log_debug("starting");

	memset(fds, 0, sizeof(fds));

	for (i = 0; i < nfds; i++) {
		memset(&fds[i], 0, sizeof(fds[i]));
		bufs[i].read_index = bufs[i].write_index = 0;
	}

	fds[0].fd = fd0;
	fds[1].fd = fd1;

	while (1) {
		for (i = 0; i < nfds; i++) {
			fds[i].events = 0;
		}

		for (i = 0; i < nfds; i++) {
			if (buffer_free(&bufs[i])) {
				fds[i].events |= POLLIN;
			}
			if (buffer_used(&bufs[i])) {
				fds[DIR_REV(i)].events |= POLLOUT;
			}
		}

		err = poll(fds, nfds, -1);
		if (err <= 0) {
			log_error("poll returned %d %m", err);
			break;
		}

		log_debug("poll returned %d", err);

		for (i = 0; i < nfds; i++) {
			if (fds[i].revents) {
				log_debug("fds[%d]: %d revents: %04X", i, fds[i].fd, fds[i].revents);

				if (fds[i].revents & POLLHUP) {
					log_error("fds[%d]: %d POLLHUP", i, fds[i].fd);
					return 0;
				} else if (fds[i].revents & POLLOUT) {
					log_debug("fds[%d]: %d POLLOUT buffer_used: %lu",
							i, fds[i].fd, (unsigned long) buffer_used(&bufs[DIR_REV(i)]));

					err = buffered_write(fds[i].fd, &bufs[DIR_REV(i)]);
					if (err < 0) {
						log_error("fds[%d]: %d safe_write failed: %m", i, fds[i].fd);
						return -1;
					}
				} else if (fds[i].revents & POLLIN) {
					log_debug("fds[%d]: %d POLLIN buffer_free: %lu",
							i, fds[i].fd, (unsigned long) buffer_free(&bufs[i]));

					err = buffered_read(fds[i].fd, &bufs[i]);
					if (err <= 0) {
						log_debug("fds[%d]: %d safe_read failed: %m", i, fds[i].fd);
						return -1;
					}
				}
			}
		}
	}

	return -1;
}

static int udp_send_msgs(int sd, int tty)
{
	char buf[NMEA_SENTENCE_MAX];
	int count;
	int err;

	while (1) {
		count = venus_read_nmea_sentence(tty, buf, sizeof(buf));
		if (count <= 0) {
			log_error("venus_read_nmea_sentence failed: quiting");
			return -1;
		}
		err = send(sd, buf, count, 0);
		if (err < 0) {
			log_debug("send failed: %m");
			return -1;
		}
	}

	return -1;
}

static int gps_to_udp_client(const char *host, int port)
{
	int tty;
	int sd;

	log_notice("Venus GPS UDP Client connecting to %s:%d", host, port);

	while (1) {
		sd = inet_conn_str(host, port, SOCK_DGRAM);
		if (sd < 0) {
			log_error("inet_conn_str failed: %m");
			sleep(60);
		} else {
			log_debug("connection opened");

			tty = venus_open(device, baud_rate);

			set_nonblocking(sd);

			udp_send_msgs(sd, tty);

			close(sd);
			close(tty);
		}
	}

	return -1;
}

static int gps_to_tcp_client(const char *host, int port)
{
	int tty;
	int sd;

	log_notice("Venus GPS TCP Client connecting to %s:%d", host, port);

	while (1) {
		sd = inet_conn_str(host, port, SOCK_STREAM);
		if (sd < 0) {
			log_error("inet_conn_str failed: %m");
			sleep(60);
		} else {
			log_debug("connection opened");

			tty = venus_open(device, baud_rate);

			set_nonblocking(tty);
			set_nonblocking(sd);

			exchange_data(sd, tty);

			close(sd);
			close(tty);
		}
	}

	return -1;
}

static int gps_to_tcp_server(int port)
{
	int err;
	int tty;
	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;

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

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

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

	serv_addr_len = sizeof(serv_addr);

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

	err = listen(lsd, 1);
	if (err == -1) {
		log_error("listen failed: %m");
		exit(1);
	}

	log_notice("Venus GPS TCP Server listening on port %d", 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 {
			log_debug("accepted");

			tty = venus_open(device, baud_rate);

			set_nonblocking(tty);
			set_nonblocking(sd);

			exchange_data(sd, tty);

			close(sd);
			close(tty);
		}
	}

	close(lsd);

	return -1;
}

static int gps_to_file(const char *file)
{
	int tty;
	int fd;

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

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

	set_nonblocking(fd);

	while (1) {
		tty = venus_open(device, baud_rate);

		set_nonblocking(tty);

		exchange_data(fd, tty);

		close(tty);
	}

	return -1;
}

int gps_to_serial(const char *port)
{
	int tty;
	int fd;
	struct termios tio;

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

	fd = open(port, O_RDWR | O_NOCTTY);
	if (fd < 0) {
		log_error("failed to open %s: %m", port);
		return -1;
	}

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

	set_nonblocking(fd);

	while (1) {
		tty = venus_open(device, baud_rate);

		set_nonblocking(tty);

		exchange_data(fd, tty);

		close(tty);
	}

	return -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");
#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, "         --gga 0-255 (default: 1) |\n");
	fprintf(out, "         --gsa 0-255 (default: 1) |\n");
	fprintf(out, "         --gsv 0-255 (default: 1) |\n");
	fprintf(out, "         --gll 0-255 (default: 1) |\n");
	fprintf(out, "         --rmc 0-255 (default: 1) |\n");
#if CONFIG_HAVE_ZDA
	fprintf(out, "         --zda 0-255 (default: 1) |\n");
#endif
	fprintf(out, "         --vtg 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_GGA,
	OPT_GSA,
	OPT_GSV,
	OPT_GLL,
	OPT_RMC,
	OPT_VTG,
	OPT_ZDA,
	OPT_START_MODE,
	OPT_LATITUDE,
	OPT_LONGITUDE,
	OPT_ALTITUDE,
	OPT_DAEMONIZE,
	OPT_FILE,
	OPT_TCP_SERVER,
	OPT_TCP_CLIENT,
	OPT_UDP_CLIENT,
	OPT_SERIAL,
};

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", 1, 0, OPT_SERIAL},
#if CONFIG_CAN_DEFAULT
	{"factory-defaults", 0, 0, OPT_FACTORY_DEFAULTS},
#endif
	{"gga", 1, 0, OPT_GGA},
	{"gsa", 1, 0, OPT_GSA},
	{"gsv", 1, 0, OPT_GSV},
	{"gll", 1, 0, OPT_GLL},
	{"rmc", 1, 0, OPT_RMC},
	{"vtg", 1, 0, OPT_VTG},
#if CONFIG_HAVE_ZDA
	{"zda", 1, 0, OPT_ZDA},
#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;

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

	char *file = "-";
	char tcp_server_enabled = 0;
	int tcp_server_port = port;
	char *tcp_client_host = NULL;
	int tcp_client_port = port;
	char *udp_client_host = NULL;
	int udp_client_port = port;
	char *serial_port = NULL;

	struct sigaction sa;

	sa.sa_handler = sigalarm_handler;
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = 0;
	sigaction(SIGALRM, &sa, NULL);

	sa.sa_handler = sigpipe_handler;
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = 0;
	sigaction(SIGPIPE, &sa, NULL);

	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:
			file = optarg;
			break;

		case OPT_SERIAL:
			serial_port = optarg;
			break;

		case OPT_TCP_SERVER:
			tcp_server_enabled = 1;
			tcp_server_port = port;
			break;

		case OPT_TCP_CLIENT:
			cp = strchr(optarg, ':');
			if (cp) {
				tcp_client_host = strndup(optarg, cp - optarg);

				cp++;

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

		case OPT_UDP_CLIENT:
			cp = strchr(optarg, ':');
			if (cp) {
				udp_client_host = strndup(optarg, cp - optarg);

				cp++;

				udp_client_port = atoi(cp);
				if (udp_client_port <= 0 || udp_client_port >= (1 << 16)) {
					log_error("invalid port");
					usage(stderr);
					exit(1);
				}
			} else {
				udp_client_host = strdup(optarg);
				udp_client_port = port;
			}
			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_GGA:
			gga = atoi(optarg);
			break;

		case OPT_GSA:
			gsa = atoi(optarg);
			break;

		case OPT_GSV:
			gsv = atoi(optarg);
			break;

		case OPT_GLL:
			gll = atoi(optarg);
			break;

		case OPT_RMC:
			rmc = atoi(optarg);
			break;

		case OPT_VTG:
			vtg = atoi(optarg);
			break;

#if CONFIG_HAVE_ZDA
		case OPT_ZDA:
			zda = 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);
	}

	venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM);

	tmp = venus_query_sw_version(tty, SW_TYPE_RESERVED, &msg);
	if (tmp < 0) {
		log_error("failed to query venus sw version info");
		exit(1);
	}
	if (msg.len == 14 && msg.data[0] == ID_SW_VERSION) {
		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) {
		venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT);
		log_notice("setting factory defaults");
		exit(0);
	}
#endif
#if CONFIG_CAN_RESET
	if (start_mode != (typeof(start_mode)) - 1) {
		venus_system_restart(tty, start_mode, time(NULL), latitude, longitude, altitude);
		log_notice("issuing system restart");
		exit(0);
	}
#endif

	venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM);

	venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM);

	close(tty);

	/*
	 * FIXME: Support more than one mode at a time
	 */
	if (tcp_server_enabled) {
		gps_to_tcp_server(tcp_server_port);
	} else if (tcp_client_host) {
		gps_to_tcp_client(tcp_client_host, tcp_client_port);
	} else if (udp_client_host) {
		gps_to_udp_client(udp_client_host, udp_client_port);
	} else if (serial_port) {
		gps_to_serial(serial_port);
	} else if (file) {
		gps_to_file(file);
	}

	return 0;
}