/*
 * 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 <jeffpc/io.h>
#include <jeffpc/hexdump.h>
#include <jeffpc/synch.h>
#include <jeffpc/list.h>

#include "ubx.h"
#include "rfc1145.h"

static LOCK_CLASS(ack_queue_lc);
static struct lock ack_lock;
static struct list ack_waiters;

struct ack_waiter {
	struct list_node node;
	struct cond cond;
	enum ubx_msg_id id;
	bool finished;
	bool success;
};

void ubx_init_queue(void)
{
	MXINIT(&ack_lock, &ack_queue_lc);

	list_create(&ack_waiters, sizeof(struct ack_waiter),
		    offsetof(struct ack_waiter, node));
}

const char *ubx_msg_name(enum ubx_msg_id id)
{
	const char *names[0xffff] = {
		[UBX_ACK_ACK]     = "UBX-ACK-ACK",
		[UBX_ACK_NAK]     = "UBX-ACK-NAK",
		[UBX_CFG_GNSS]    = "UBX-CFG-GNSS",
		[UBX_CFG_PRT]     = "UBX-CFG-PRT",
		[UBX_CFG_MSG]     = "UBX-CFG-MSG",
		[UBX_CFG_NAV5]    = "UBX-CFG-NAV5",
		[UBX_MON_HW]      = "UBX-MON-HW",
		[UBX_MON_VER]     = "UBX-MON-VER",
		[UBX_NAV_CLOCK]   = "UBX-NAV-CLOCK",
		[UBX_NAV_POSECEF] = "UBX-NAV-POSECEF",
		[UBX_NAV_PVT]     = "UBX-NAV-PVT",
		[UBX_NAV_SAT]     = "UBX-NAV-SAT",
		[UBX_RXM_MEASX]   = "UBX-RXM-MEASX",
		[UBX_RXM_RAWX]    = "UBX-RXM-RAWX",
		[UBX_RXM_RLM]     = "UBX-RXM-RLM",
		[UBX_RXM_SFRBX]   = "UBX-RXM-SFRBX",
		[UBX_SEC_UNIQID]  = "UBX-SEC-UNIQID",
	};

	if ((id > ARRAY_LEN(names)) || (names[id] == NULL))
		return "???";

	return names[id];
}

static int __send_ubx(int fd, enum ubx_msg_id id, const void *data,
		      size_t len)
{
	struct ubx_header hdr = {
		.sync = { UBX_SYNC_BYTE_1, UBX_SYNC_BYTE_2 },
		.class = id >> 8,
		.id = id & 0xff,
		.len = cpu16_to_le(len),
	};
	uint16_t cksum;
	int ret;

	{
		char out[len*2+1];
		hexdumpz(out, data, len, false);
		fprintf(stderr, "> %02x%02x %02x %02x (%s) %04x %s\n",
			hdr.sync[0], hdr.sync[1],
			hdr.class, hdr.id,
			ubx_msg_name((hdr.class << 8) | hdr.id),
			le16_to_cpu(hdr.len), out);
	}

	ret = xwrite(fd, &hdr, sizeof(hdr));
	if (ret)
		return ret;

	ret = xwrite(fd, data, len);
	if (ret)
		return ret;

	cksum = cpu16_to_le(rfc1145_cksum(hdr.class, hdr.id,
					  hdr.len & 0xff, hdr.len >> 8,
					  data, len));

	ret = xwrite(fd, &cksum, sizeof(cksum));
	if (ret)
		return ret;

	return 0;
}

static bool __read_ubx_ack(int fd, enum ubx_msg_id id)
{
	struct ack_waiter waiter;

	CONDINIT(&waiter.cond);
	waiter.finished = false;
	waiter.id = id;

	MXLOCK(&ack_lock);
	list_insert_tail(&ack_waiters, &waiter);

	while (!waiter.finished)
		CONDWAIT(&waiter.cond, &ack_lock);
	MXUNLOCK(&ack_lock);

	CONDDESTROY(&waiter.cond);

	return waiter.success;
}

void notify_ubx_ack(const uint8_t *data, size_t len, bool success)
{
	struct ack_waiter *cur, *tmp;
	enum ubx_msg_id id;

	ASSERT3U(len, ==, 2);

	id = mkmsgid(data[0], data[1]);

	fprintf(stderr, "%s for %02x %02x (%s)\n", success ? "ACK" : "NAK",
		data[0], data[1], ubx_msg_name(id));

	MXLOCK(&ack_lock);
	list_for_each_safe(cur, tmp, &ack_waiters) {
		if (cur->id != id)
			continue;

		cur->finished = true;
		cur->success = success;
		CONDSIG(&cur->cond);

		/* remove from queue */
		list_remove(&ack_waiters, cur);
	}
	MXUNLOCK(&ack_lock);
}

int send_ubx(int fd, enum ubx_msg_id id, const void *data, size_t len)
{
	return __send_ubx(fd, id, data, len);
}

int send_ubx_with_ack(int fd, enum ubx_msg_id id, const void *data,
		      size_t len)
{
	bool ack;
	int ret;

	ret = __send_ubx(fd, id, data, len);
	if (ret)
		return ret;

	ack = __read_ubx_ack(fd, id);

	return ack ? 0 : -ENOTSUP;
}

int enable_ubx_msg(int fd, enum ubx_msg_id id, int port, int rate)
{
	uint8_t raw[8] = { id >> 8, id & 0xff, 0, 0, 0, 0, 0, 0 };

	raw[2 + port] = rate;

	return send_ubx_with_ack(fd, UBX_CFG_MSG, raw, sizeof(raw));
}

bool parse_and_process_ubx_message(const uint8_t *buf, size_t len,
				   const uint64_t tick,
				   void(*f)(const struct ubx_header *,
					    const uint8_t *, size_t,
					    enum ubx_msg_id,
					    uint64_t))
{
	struct ubx_header hdr;
	uint16_t ubx_len;

	memcpy(&hdr, buf, sizeof(hdr));

	if ((hdr.sync[0] != UBX_SYNC_BYTE_1) ||
	    (hdr.sync[1] != UBX_SYNC_BYTE_2)) {
		fprintf(stderr, "UBX message sync byte mismatch %02x %02x\n",
                        hdr.sync[0], hdr.sync[1]);
		return false;
        }

	ubx_len = le16_to_cpu(hdr.len);

	if (ubx_len != (len - sizeof(hdr) - 2)) {
		fprintf(stderr, "UBX message length differs (got %u exp %zu)\n",
			ubx_len, len - sizeof(hdr) - 2);
		return false;
	}

	if (f)
		f(&hdr, &buf[sizeof(hdr)], ubx_len,
		  mkmsgid(hdr.class, hdr.id), tick);

	return true;
}