/*
 * Copyright (c) 2019-2020 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"

static const char *prog;
static char file_buffer[65536 + 8]; /* worst case message */
static uint8_t ubxport;
static bool input_readonly;

static void usage(void)
{
	fprintf(stderr, "Usage: %s <device> <ubxport> <rawlog>\n", prog);
	fprintf(stderr, "\n");
	fprintf(stderr, "  <device>       path or - for standard in\n");
	fprintf(stderr, "  <ubxport>      port number (0=DCC, 1=UART, 3=USB, 4=SPI)\n");
	fprintf(stderr, "  <rawlog>       path for raw UBX messages\n");
	exit(1);
}

static int cfg_port(FILE *file)
{
	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(0x00010001), /* L1, enable */
			},
			{
				.gnssid = GNSSID_GALILEO,
				.res_trk_ch = 8,
				.max_trk_ch = 10,
				.flags = cpu32_to_le(0x00010001), /* E1, enable */
			},
			{
				.gnssid = GNSSID_GLONASS,
				.res_trk_ch = 6,
				.max_trk_ch = 8,
				.flags = cpu32_to_le(0x00010001), /* L1, enable */
			},
			{
				.gnssid = GNSSID_BEIDOU,
				.res_trk_ch = 6,
				.max_trk_ch = 8,
				.flags = cpu32_to_le(0x00010000), /* B1I */
			},
			{
				.gnssid = GNSSID_SBAS,
				.res_trk_ch = 3,
				.max_trk_ch = 4,
				.flags = cpu32_to_le(0x00010001), /* L1, enable */
			},
			{
				.gnssid = GNSSID_QZSS,
				.res_trk_ch = 4,
				.max_trk_ch = 8,
				.flags = cpu32_to_le(0x00010001), /* L1C, enable */
			},
		},
	};
	void *prt;
	int ret;
	int i;

	if (input_readonly)
		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(file, UBX_MON_VER, NULL, 0);
	if (ret)
		return ret;

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

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

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

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

	struct {
		enum ubx_msg_id id;
		int rate;
	} enable_msgs[] = {
		{ 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(file, id, ubxport, rate);
		if (ret)
			return ret;
	}

	return 0;
}

int main(int argc, char **argv)
{
	struct stat stat;
	struct termios termios;
	FILE *ifile;
	FILE *rfile;
	int ret;
	int fd;

	prog = argv[0];

	if (argc != 4)
		usage();

	ret = str2u8(argv[2], &ubxport);
	if (ret) {
		fprintf(stderr, "Error: invalid ubxport - must be a number\n");
		return 2;
	}

	if (ubxport > 6) {
		fprintf(stderr, "Error: invalid ubxport - must be [0,6] (got %u)\n",
			ubxport);
		return 3;
	}

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

		input_readonly = !S_ISCHR(stat.st_mode);
	}

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

	if (!input_readonly) {
		if (tcgetattr(fd, &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(fd, TCSAFLUSH, &termios)) {
			fprintf(stderr, "Error: Failed to set attrs on device: %s\n",
				xstrerror(-errno));
			return 8;
		}
	}

	ifile = fdopen(fd, input_readonly ? "rb" : "wb+");
	if (ifile == NULL) {
		fprintf(stderr, "Error: Failed to fdopen: %s\n",
			xstrerror(-errno));
		return 9;
	}

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

	if (setvbuf(ifile, file_buffer, _IOFBF, sizeof(file_buffer)) == EOF)
		fprintf(stderr, "Warn: Failed to mark stream as buffered\n");

	ret = cfg_port(ifile);
	if (ret) {
		fprintf(stderr, "Error: Failed to configure port: %s\n",
			xstrerror(ret));
		return 10;
	}

	ret = iothread_start(ifile, rfile);
	if (ret) {
		fprintf(stderr, "Error: Failed to spawn I/O thread: %s\n",
			xstrerror(ret));
		return 11;
	}

	for (;;)
		sleep(3600);

	fclose(ifile);
	fclose(rfile);

	return 0;
}