/*
 * Copyright (c) 2016-2021 Satlab A/S <satlab@satlab.com>
 * All rights reserved. Do not distribute without permission.
 */

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <inttypes.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <util/csp_client_commands.h>
#include <csp/csp_endian.h>

#include <slash/slash.h>

#include <satlab/polaris.h>
#include <prop-client/prop_client_commands.h>

#include <btp/types.h>
#include <btp/cmd_btp.h>

/* Default node and timeout */
static uint8_t polaris_node = SL_POLARIS_DEFAULT_ADDRESS;
static uint32_t polaris_timeout = 1000;

static uint8_t get_type(const uint8_t *data)
{
	return data[0] >> 2;
}

static uint32_t get_mmsi(const uint8_t *data)
{
	uint32_t mmsi;
	memcpy(&mmsi, &data[1], sizeof(mmsi));

	return csp_ntoh32(mmsi) >> 2;
}

static void print_hex(const uint8_t *data, size_t length)
{
	size_t i;

	for (i = 0; i < length; i++)
		printf("%02hhx", data[i]);
	printf("\n");
}

static int frameprint_cb(sl_polaris_store_frame_t *frame, void *arg)
{
	int ret = 0;
	uint8_t type;
	uint32_t mmsi;
	struct slash *slash = arg;

	if (frame) {
		type = get_type(frame->data);
		mmsi = get_mmsi(frame->data);

		printf("Frame %u ch: %u type: %2u mmsi: %9u scale: %3u mean: %+4d time: %.9f\n",
			(unsigned)frame->seq,
			frame->channel,
			type,
			(unsigned)mmsi,
			frame->scale,
			frame->mean,
			frame->time / 1e9);
		print_hex(frame->data, frame->length);
	}

	/* Check for ^C */
	if (slash_wait_interruptible(slash, 0) == '\x03') {
		printf("\nInterrupted by user\n");
		ret = -EINTR;
	}

	return ret;
}


/* Main commands */
slash_command_group(polaris, "Polaris AIS subsystem commands");

static int cmd_polaris_node(struct slash *slash)
{
	uint8_t node;
	char *endptr;

	if (slash->argc < 2) {
		printf("Current node address: %hhu\n", polaris_node);
		return 0;
	}

	node = (uint8_t)strtoul(slash->argv[1], &endptr, 10);
	if (*endptr != '\0')
		return SLASH_EUSAGE;

	polaris_node = node;

	return 0;
}
slash_command_sub(polaris, node, cmd_polaris_node, "[address]",
		  "Set/read node address");

static int cmd_polaris_timeout(struct slash *slash)
{
	uint32_t timeout;
	char *endptr;

	if (slash->argc < 2) {
		printf("Current timeout: %"PRIu32" ms\n", polaris_timeout);
		return 0;
	}

	timeout = (uint32_t)strtoul(slash->argv[1], &endptr, 10);
	if (*endptr != '\0')
		return SLASH_EUSAGE;

	polaris_timeout = timeout;

	return 0;
}
slash_command_sub(polaris, timeout, cmd_polaris_timeout, "[ms]",
		  "Set/read communication timeout");


/* CSP commands */
SL_CSP_CLIENT_COMMANDS_GENERATE(polaris, &polaris_node, &polaris_timeout);

/* Properties commands */
SL_PROP_CLIENT_COMMANDS_GENERATE(polaris, &polaris_node, SL_PROP_DEFAULT_PORT, &polaris_timeout, &polaris_props);

/* BTP commands */
SL_BTP_CLIENT_COMMANDS_GENERATE(polaris, &polaris_node, BTP_DEFAULT_PORT);

/* Bootloader commands */
slash_command_subgroup(polaris, boot, "Bootloader commands");

static int cmd_polaris_boot_flash(struct slash *slash)
{
	int ret;
	const char *filename;
	uint32_t crc;

	if (slash->argc < 2)
		return SLASH_EUSAGE;

	filename = slash->argv[1];

	printf("Flashing \"%s\" to alternate boot partition\n", filename);

	ret = sl_polaris_bootloader_flash_alternate(polaris_node, polaris_timeout, filename, &crc);
	if (ret < 0) {
		printf("Failed to flash image: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	printf("Flashed image, checksum = %08"PRIx32"\n", crc);

	return 0;
}
slash_command_subsub(polaris, boot, flash, cmd_polaris_boot_flash, "<filename>",
		     "Flash image from polaris SD-card to alternate partition");

static int cmd_polaris_boot_erase(struct slash *slash)
{
	int ret;

	ret = sl_polaris_bootloader_erase_alternate(polaris_node, polaris_timeout);
	if (ret < 0) {
		printf("Failed to erase image: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	printf("Success - alternate image was erased\n");

	return 0;
}
slash_command_subsub(polaris, boot, erase, cmd_polaris_boot_erase,
		     NULL, "Erase alternate partition");

static int cmd_polaris_boot_checksum(struct slash *slash)
{
	int ret;
	uint32_t size, crc;
	char *endptr;

	if (slash->argc < 2)
		return SLASH_EUSAGE;

	size = (uint32_t)strtoul(slash->argv[1], &endptr, 10);
	if (*endptr != '\0')
		return SLASH_EUSAGE;

	ret = sl_polaris_bootloader_checksum_alternate(polaris_node, polaris_timeout, size, &crc);
	if (ret < 0) {
		printf("Failed to checksum image: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	printf("Image checksum = %08"PRIx32"\n", crc);

	return 0;
}
slash_command_subsub(polaris, boot, checksum, cmd_polaris_boot_checksum,
		     "<size>", "Calculate checksum of alternate partition");

static int cmd_polaris_boot_verify(struct slash *slash)
{
	int ret;
	uint8_t partition = 1;
	uint32_t size, checksum;

	if (slash->argc > 1) {
		if (!strcmp(slash->argv[1], "primary")) {
			partition = 0;
		} else if (!strcmp(slash->argv[1], "alternate")) {
			partition = 1;
		} else {
			return SLASH_EUSAGE;
		}
	}

	ret = sl_polaris_bootloader_verify(polaris_node, polaris_timeout, partition, &size, &checksum);
	if (ret < 0) {
		printf("Failed to verify image: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	printf("%s image is %s! (size %u, checksum %08X)\n",
	       partition == 0 ? "Primary" : "Alternate",
	       ret == 0 ? "OK" : "corrupt",
	       size, checksum);

	return ret == 0 ? 0 : SLASH_EIO;
}
slash_command_subsub(polaris, boot, verify, cmd_polaris_boot_verify,
		     "[partition]", "Verify image in partition");

static int cmd_polaris_boot_set(struct slash *slash)
{
	int ret;
	uint8_t boots = 1;
	unsigned long boots_parsed;
	char *endptr;

	if (slash->argc > 1) {
		boots_parsed = strtoul(slash->argv[1], &endptr, 10);
		if (*endptr != '\0')
			return SLASH_EUSAGE;

		if (boots_parsed > 255) {
			fprintf(stderr, "Maximum number of boots is 255\n");
			return SLASH_EUSAGE;
		}

		boots = boots_parsed;
	}

	printf("Setting alternate firmware boot for %hhu boots\n", boots);

	ret = sl_polaris_bootloader_set_alternate(polaris_node, polaris_timeout, boots);
	if (ret < 0) {
		fprintf(stderr, "Failed to set boot image: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	printf("Success - Reboot to boot alternate image\n");

	return 0;
}
slash_command_subsub(polaris, boot, set, cmd_polaris_boot_set,
		     "[boots]", "Set number of boots on alternate partition");


/* Sample commands */
slash_command_subgroup(polaris, sample, "Sample buffer commands");

static int cmd_polaris_sample_read(struct slash *slash)
{
	int c, ret, fd, nbytes;
	size_t samples = 8192;
	uint8_t source = SL_POLARIS_SAMPLEBUF_SOURCE_RAW;
	uint32_t *sample_mem;
	bool wait_pps = false;

	while ((c = slash_getopt(slash, "s:n:p")) != -1) {
		switch (c) {
		case 's':
			if (!strcmp(slash->optarg, "raw")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_RAW;
			} else if (!strcmp(slash->optarg, "ch1")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH1;
			} else if (!strcmp(slash->optarg, "ch2")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH2;
			} else if (!strcmp(slash->optarg, "ch3")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH3;
			} else if (!strcmp(slash->optarg, "ch4")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH4;
			} else {
				source = atoi(slash->optarg);
			}
			break;
		case 'n':
			samples = atoi(slash->optarg);
			break;
		case 'p':
			wait_pps = true;
			break;
		case '?':
		default:
			return SLASH_EUSAGE;
		}
	}

	if (slash->argc - slash->optind != 1)
		return SLASH_EUSAGE;

	fd = open(slash->argv[slash->optind], O_WRONLY | O_CREAT | O_TRUNC,
		  S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
	if (fd < 0) {
		printf("Failed to open output file\n");
		return SLASH_EIO;
	}

	sample_mem = calloc(samples, sizeof(*sample_mem));
	if (!sample_mem) {
		printf("Failed to allocate memory\n");
		return SLASH_EIO;
	}

	ret = sl_polaris_sample_read(polaris_node, polaris_timeout, source, sample_mem, samples * sizeof(*sample_mem), wait_pps);
	if (ret < 0) {
		free(sample_mem);
		printf("Failed to read sample: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	nbytes = write(fd, sample_mem, samples * sizeof(*sample_mem));
	if (nbytes != samples * sizeof(*sample_mem)) {
		free(sample_mem);
		printf("Failed to write sample to output file\n");
	}

	free(sample_mem);

	return 0;
}
slash_command_subsub(polaris, sample, read, cmd_polaris_sample_read,
		     "[-s <source>] [-n <samples>] [-p] <filename>",
		     "Read sample buffer and write to file");

static int cmd_polaris_sample_store(struct slash *slash)
{
	int c, ret;
	size_t samples = 8192;
	uint8_t source = SL_POLARIS_SAMPLEBUF_SOURCE_RAW;
	uint8_t dest = SL_POLARIS_SAMPLEBUF_DEST_MEM;
	uint32_t id;
	char *endptr;
	bool wait_pps = false;

	while ((c = slash_getopt(slash, "s:d:n:p")) != -1) {
		switch (c) {
		case 's':
			if (!strcmp(slash->optarg, "raw")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_RAW;
			} else if (!strcmp(slash->optarg, "ch1")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH1;
			} else if (!strcmp(slash->optarg, "ch2")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH2;
			} else if (!strcmp(slash->optarg, "ch3")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH3;
			} else if (!strcmp(slash->optarg, "ch4")) {
				source = SL_POLARIS_SAMPLEBUF_SOURCE_CH4;
			} else {
				source = atoi(slash->optarg);
			}
			break;
		case 'd':
			if (!strcmp(slash->optarg, "mem")) {
				dest = SL_POLARIS_SAMPLEBUF_DEST_MEM;
			} else if (!strcmp(slash->optarg, "sd")) {
				dest = SL_POLARIS_SAMPLEBUF_DEST_SDCARD;
			} else {
				dest = atoi(slash->optarg);
			}
			break;
		case 'n':
			samples = atoi(slash->optarg);
			break;
		case 'p':
			wait_pps = true;
			break;
		case '?':
		default:
			return SLASH_EUSAGE;
		}
	}

	if (slash->argc - slash->optind != 1)
		return SLASH_EUSAGE;

	id = (uint32_t)strtoul(slash->argv[slash->optind], &endptr, 10);

	ret = sl_polaris_sample_store(polaris_node, polaris_timeout, source, dest, samples, id, wait_pps);
	if (ret < 0) {
		printf("Failed to store sample: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	return 0;
}
slash_command_subsub(polaris, sample, store, cmd_polaris_sample_store,
		     "[-s <source>] [-d <dest>] [-n <samples>] [-p] <id>",
		     "Save sample buffer on host");


/* Store commands */
slash_command_subgroup(polaris, store, "Store commands");

static int cmd_polaris_store_mirror(struct slash *slash)
{
	int ret;

	ret = sl_polaris_store_mirror_recv(polaris_node, SL_POLARIS_PORT_STORE_MIRROR_RECV, polaris_timeout, frameprint_cb, slash);
	if (ret < 0) {
		if (ret != -EINTR)
		       printf("Failed to request frames: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	return 0;
}
slash_command_subsub(polaris, store, mirror, cmd_polaris_store_mirror,
		     NULL, "Request mirror of received frames");

static int cmd_polaris_store_read(struct slash *slash)
{
	int c, ret, num = 1, bytes;
	uint8_t bitmap[SL_POLARIS_STORE_BITMAP_SIZE] = {0};
	uint32_t start = 0;
	char *endptr, *bitmapstr = NULL;
	size_t i, bitmapstrlen;

	while ((c = slash_getopt(slash, "b:n:s:")) != -1) {
		switch (c) {
		case 'b':
			bitmapstr = slash->optarg;
			break;
		case 's':
			start = (uint32_t)strtoul(slash->optarg, &endptr, 10);
			if (*endptr != '\0')
				return SLASH_EUSAGE;
			break;
		case 'n':
			num = (int)strtoul(slash->optarg, &endptr, 10);
			if (*endptr != '\0' || num < 1 || num > 8 * sizeof(bitmap))
				return SLASH_EUSAGE;
			break;
		case '?':
		default:
			return SLASH_EUSAGE;
		}
	}

	if (bitmapstr) {
		bitmapstrlen = strlen(bitmapstr);
		if (bitmapstrlen % 2 || bitmapstrlen / 2 > sizeof(bitmap))
			return SLASH_EUSAGE;

		bitmapstrlen = bitmapstrlen / 2;

		for (i = 0; i < bitmapstrlen; i++) {
			ret = sscanf(&bitmapstr[i * 2], "%2hhx", &bitmap[i]);
			if (ret != 1)
				return SLASH_EUSAGE;
		}
	}

	if (num > 0) {
		bytes = num / 8;
		memset(bitmap, 0xff, bytes);
		num -= bytes * 8;
		if (num > 0)
			bitmap[bytes] = ~((1 << (8 - num)) - 1);
	}

	ret = sl_polaris_store_read(polaris_node, polaris_timeout, start, bitmap, frameprint_cb, NULL);
	if (ret < 0) {
		printf("Failed to read frames: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	return 0;
}
slash_command_subsub(polaris, store, read, cmd_polaris_store_read,
		     "[-s start] [-n num] [-b bitmap]",
		     "Request frames from store");

static int cmd_polaris_run(struct slash *slash)
{
	int ret;
	const char *cmd;

	if (slash->argc != 2)
		return SLASH_EUSAGE;

	cmd = slash->argv[1];

	ret = sl_polaris_shell_run(polaris_node, polaris_timeout, cmd);
	if (ret < 0) {
		printf("Failed to run command: %s\n", strerror(-ret));
		return SLASH_EIO;
	}

	return 0;
}
slash_command_sub(polaris, run, cmd_polaris_run, "<command>",
		  "Run command on remote system");
