/*
 * Copyright (c) 2019-2021 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <termios.h>

#include <jeffpc/types.h>
#include <jeffpc/error.h>
#include <jeffpc/io.h>

#include "iothread.h"
#include "ubx.h"

#define DEFAULT_UBXPORT	3

static void usage(const char *prog)
{
	fprintf(stderr, "Usage: %s [-cegrs] -d <device> [-u <ubxport>] "
		"-l <log>\n", prog);
	fprintf(stderr, "\n");
	fprintf(stderr, "  -c             enable BeiDou\n");
	fprintf(stderr, "  -e             enable Galileo\n");
	fprintf(stderr, "  -g             enable GPS\n");
	fprintf(stderr, "  -r             enable GLONASS\n");
	fprintf(stderr, "  -s             enable SBAS & QZSS\n");
	fprintf(stderr, "  -d <device>    path or - for standard in\n");
	fprintf(stderr, "  -u <ubxport>   port number (default %d; 0=DCC, "
		"1=UART, 3=USB, 4=SPI)\n", DEFAULT_UBXPORT);
	fprintf(stderr, "  -l <log>       path for framed UBX message log\n");
	fprintf(stderr, "  -v             verbose\n");
	exit(1);
}

static int cfg_port(int fd, bool ro, uint8_t ubxport, bool beidou,
		    bool galileo, bool gps, bool glonass, bool sbas)
{
	struct ubx_cfg_prt_uart prt_uart = {
		.port = ubxport,
		.mode = cpu32_to_le((0x00 << 12) | /* 1 stop bit */
				    (0x04 << 9) |  /* none */
				    (0x03 << 6)),  /* 8-bit */
		.baud_rate = cpu32_to_le(115200),
		.in_proto_mask = cpu16_to_le(0x01), /* UBX only */
		.out_proto_mask = cpu16_to_le(0x01), /* UBX only */
	};
	struct ubx_cfg_prt_usb prt_usb = {
		.port = 3,
		.in_proto_mask = cpu16_to_le(0x01), /* UBX only */
		.out_proto_mask = cpu16_to_le(0x01), /* UBX only */
	};
	struct ubx_cfg_nav5 nav5 = {
		.mask = cpu16_to_le(0x0001), /* only dynamics model */
		.dyn_model = 0, /* portable */
	};
	struct ubx_cfg_gnss gnss = {
		.num_trk_ch_use = 0xff,
		.num_cfg_blocks = UBX_CFG_GNSS_NUM_BLOCKS,
		.cfg = {
			{
				.gnssid = GNSSID_GPS,
				.res_trk_ch = 4,
				.max_trk_ch = 8,
				.flags = cpu32_to_le(0x00010000) /* L1 */ |
					 cpu32_to_le(gps ? 0x1 : 0x0),
			},
			{
				.gnssid = GNSSID_GALILEO,
				.res_trk_ch = 8,
				.max_trk_ch = 10,
				.flags = cpu32_to_le(0x00010000) /* E1 */ |
					 cpu32_to_le(galileo ? 0x1 : 0x0),
			},
			{
				.gnssid = GNSSID_GLONASS,
				.res_trk_ch = 6,
				.max_trk_ch = 8,
				.flags = cpu32_to_le(0x00010000) /* L1 */ |
					 cpu32_to_le(glonass ? 0x1 : 0x0),
			},
			{
				.gnssid = GNSSID_BEIDOU,
				.res_trk_ch = 6,
				.max_trk_ch = 8,
				.flags = cpu32_to_le(0x00010000) /* B1I */ |
					 cpu32_to_le(beidou ? 0x1 : 0x0),
			},
			{
				.gnssid = GNSSID_SBAS,
				.res_trk_ch = 3,
				.max_trk_ch = 4,
				.flags = cpu32_to_le(0x00010000) /* L1 */ |
					 cpu32_to_le(sbas ? 0x1 : 0x0),
			},
			{
				.gnssid = GNSSID_QZSS,
				.res_trk_ch = 4,
				.max_trk_ch = 8,
				.flags = cpu32_to_le(0x00010000) /* L1C */ |
					 cpu32_to_le(sbas ? 0x1 : 0x0),
			},
		},
	};
	void *prt;
	int ret;
	int i;

	if (ro)
		return 0;

	switch (ubxport) {
		case 0:
			panic("not yet implemented - DCC port");
		case 1:
			prt = &prt_uart;
			break;
		case 3:
			prt = &prt_usb;
			break;
		case 4:
			panic("not yet implemeted - SPI port");
		default:
			panic("unknown ubxport number");
	}

	/* request version info */
	ret = send_ubx(fd, UBX_MON_VER, NULL, 0);
	if (ret)
		return ret;

	/* request serial number */
	ret = send_ubx(fd, UBX_SEC_UNIQID, NULL, 0);
	if (ret)
		return ret;

	/* disable NMEA, enable UBX */
	ret = send_ubx_with_ack(fd, UBX_CFG_PRT, prt,
				sizeof(struct ubx_cfg_prt_uart));
	if (ret)
		return ret;

	/* set dynamics mode */
	ret = send_ubx_with_ack(fd, UBX_CFG_NAV5, &nav5,
				sizeof(struct ubx_cfg_nav5));
	if (ret)
		return ret;

	/* enable gnss systems */
	ret = send_ubx_with_ack(fd, UBX_CFG_GNSS, &gnss,
				sizeof(struct ubx_cfg_gnss));
	if (ret)
		return ret;

	struct {
		enum ubx_msg_id id;
		int rate;
	} enable_msgs[] = {
		{ UBX_MON_HW,     15 }, /* jamming info */
		{ UBX_NAV_CLOCK,   6 }, /* clock */
		{ UBX_NAV_POSECEF, 6 }, /* ECEF position */
		{ UBX_NAV_PVT,     1 }, /* position, velocity, time */
		{ UBX_NAV_SAT,     6 }, /* satellite info */
		{ UBX_RXM_RAWX,    1 }, /* raw measurement data */
		{ UBX_RXM_RLM,     1 }, /* SAR RLM */
		{ UBX_RXM_SFRBX,   1 }, /* raw subframes */
	};

	for (i = 0; i < ARRAY_LEN(enable_msgs); i++) {
		enum ubx_msg_id id = enable_msgs[i].id;
		int rate = enable_msgs[i].rate;

		fprintf(stderr, "Enabling %s (%04x)...\n", ubx_msg_name(id), id);

		ret = enable_ubx_msg(fd, id, ubxport, rate);
		if (ret)
			return ret;
	}

	return 0;
}

int main(int argc, char **argv)
{
	struct stat stat;
	struct termios termios;
	bool input_readonly;
	const char *device;
	const char *log;
	bool enable_beidou;
	bool enable_galileo;
	bool enable_gps;
	bool enable_glonass;
	bool enable_sbas;
	uint8_t ubxport;
	bool verbose;
	int ifile;
	FILE *rfile;
	int opt;
	int ret;

	device = NULL;
	log = NULL;
	enable_beidou = false;
	enable_galileo = false;
	enable_gps = false;
	enable_glonass = false;
	enable_sbas = false;
	ubxport = DEFAULT_UBXPORT;
	verbose = false;

	while ((opt = getopt(argc, argv, "+cegrsd:l:u:v")) != -1) {
		switch (opt) {
			case 'c':
				enable_beidou = true;
				break;
			case 'e':
				enable_galileo = true;
				break;
			case 'g':
				enable_gps = true;
				break;
			case 'r':
				enable_glonass = true;
				break;
			case 's':
				enable_sbas = true;
				break;
			case 'd':
				device = optarg;
				break;
			case 'l':
				log = optarg;
				break;
			case 'u':
				if (str2u8(optarg, &ubxport)) {
					fprintf(stderr, "Error: '%s' is not "
						"a valid ublox port.\n",
						optarg);
					usage(argv[0]);
				}

				if (ubxport > 6) {
					fprintf(stderr, "Error: invalid ubxport"
						" - must be [0,6] (got %u)\n",
						ubxport);
					usage(argv[0]);
				}
				break;
			case 'v':
				verbose = true;
				break;
			default:
				usage(argv[0]);
		}
	}

	if (optind != argc)
		usage(argv[0]);

	if (!device) {
		fprintf(stderr, "Missing device name\n");
		usage(argv[0]);
	}

	if (!log) {
		fprintf(stderr, "Missing log name\n");
		usage(argv[0]);
	}

	if (!enable_beidou && !enable_galileo && !enable_gps &&
	    !enable_glonass && !enable_sbas) {
		fprintf(stderr, "Missing GNSS constellation selection\n");
		usage(argv[0]);
	}

	if (!strcmp(device, "/dev/stdin") || !strcmp(device, "-")) {
		device = "/dev/stdin";
		input_readonly = true;
	} else {
		ret = xstat(device, &stat);
		if (ret < 0) {
			fprintf(stderr, "Error: failed to stat input: %s\n",
				xstrerror(ret));
			return 4;
		}

		input_readonly = !S_ISCHR(stat.st_mode);
	}

	ifile = xopen(device, input_readonly ? O_RDONLY : O_RDWR, 0);
	if (ifile < 0) {
		fprintf(stderr, "Error: Could not open device %s: %s\n",
			device, xstrerror(ifile));
		usage(argv[0]);
	}

	if (!input_readonly) {
		if (tcgetattr(ifile, &termios) < 0) {
			fprintf(stderr, "Error: Failed to get tty attrs: %s\n",
				xstrerror(-errno));
			return 6;
		}

		memset(&termios, 0, sizeof(termios));
		termios.c_cc[VTIME] = 0;
		termios.c_cc[VMIN] = 1;
		termios.c_iflag = IGNPAR;
		termios.c_oflag = 0;
		termios.c_cflag = CS8 | CREAD | CLOCAL;
		termios.c_lflag = 0;

		if (cfsetspeed(&termios, B115200) < 0) {
			fprintf(stderr, "Error: Failed to set baud rate: %s\n",
				xstrerror(-errno));
			return 7;
		}

		if (tcsetattr(ifile, TCSAFLUSH, &termios)) {
			fprintf(stderr, "Error: Failed to set attrs on device: %s\n",
				xstrerror(-errno));
			return 8;
		}
	}

	rfile = fopen(log, "wb");
	if (rfile == NULL) {
		fprintf(stderr, "Error: Failed to fopen log: %s\n",
			xstrerror(-errno));
		return 9;
	}

	/* make sure ack queuing is set up */
	ubx_init_queue();

	/* start the iothread before we make any writes to the device */
	ret = iothread_start(ifile, rfile, verbose, !input_readonly);
	if (ret) {
		fprintf(stderr, "Error: Failed to spawn I/O thread: %s\n",
			xstrerror(ret));
		return 10;
	}

	ret = cfg_port(ifile, input_readonly, ubxport, enable_beidou,
		       enable_galileo, enable_gps, enable_glonass,
		       enable_sbas);
	if (ret) {
		fprintf(stderr, "Error: Failed to configure port: %s\n",
			xstrerror(ret));
		return 11;
	}

	for (;;)
		sleep(3600);

	xclose(ifile);
	fclose(rfile);

	return 0;
}