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

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>

#include <slash/slash.h>

#include <csp/csp.h>
#include <csp/drivers/usart.h>
#include <csp/interfaces/csp_if_lo.h>
#include <csp/interfaces/csp_if_can.h>
#include <csp/interfaces/csp_if_mcast.h>
#include <csp/interfaces/csp_if_kiss.h>

#include <csp_if_slgnd.h>
#include <csp_if_udp.h>

#define SATCTL_PROMPT_FMT		"\033[94m[%s]\033[0m "
#define SATCTL_DEFAULT_ADDRESS		1
#define SATCTL_DEFAULT_TYPE		TYPE_NONE
#define SATCTL_DEFAULT_RATE		1000000

#define SATCTL_LINE_SIZE		128
#define SATCTL_HISTORY_SIZE		2048

#define SATCTL_DESC			"Satlab Control Application " SATCTL_VERSION "\n"
#define SATCTL_COPYRIGHT		"Copyright (c) 2014-2019 Satlab ApS <satlab@satlab.com>\n"

#define vmsg(_verbose, _fmt, ...) do { \
	if (_verbose) \
		printf("\033[1m" _fmt "\033[0m\n", ##__VA_ARGS__); \
	} while (0);

enum conntype {
	TYPE_NONE = 0,
	TYPE_CAN = 1,
	TYPE_UART = 2,
	TYPE_MCAST = 3,
	TYPE_UDP = 4,
	TYPE_SLGND = 5,
};

static csp_iface_t csp_if_kiss;
static csp_kiss_handle_t csp_kiss_driver;

static char hostname[32] = "satctl";

void usage(void)
{
	printf("usage: satctl [options] [command/file]\n");
	printf("\n");
	printf(SATCTL_DESC);
	printf(SATCTL_COPYRIGHT);
	printf("\n");
	printf("Options:\n");
	printf(" -c,\t\tUse CAN as transport interface\n");
	printf(" -m,\t\tUse MCAST as transport interface\n");
	printf(" -u,\t\tUse UART/KISS as transport interface\n");
	printf(" -s,\t\tUse SLGND as transport interface\n");
	printf(" -e,\t\tUse UDP as transport interface\n");
	printf(" -i ID,\t\tShow ID as prompt\n");
	printf(" -d DEVICE,\tUse DEVICE as CAN/UART/MCAST/UDP interface or SLGND host\n");
	printf(" -r RATE,\tUse RATE bps as CAN/UART/MCAST bitrate\n");
	printf(" -n NODE\tUse NODE as own CSP addres\n");
	printf(" -k KEY\t\tUse KEY as CSP XTEA/HMAC key\n");
	printf(" -h,\t\tPrint this help and exit\n");
	printf(" -v,\t\tEnable verbose output\n");
}

static void usart_rx(uint8_t *buf, int len, void *pxTaskWoken)
{
	csp_kiss_rx(&csp_if_kiss, buf, len, pxTaskWoken);
}

void *thread_csp_service(void *param)
{
	csp_socket_t *sock;
	csp_conn_t *conn;
	csp_packet_t *packet;

	/* Unused */
	(void)param;

	sock = csp_socket(CSP_SO_NONE);
	csp_bind(sock, CSP_ANY);
	csp_listen(sock, 10);

	while (1) {
		if ((conn = csp_accept(sock, 10000)) == NULL)
			continue;

		packet = csp_read(conn, 1000);
		if (packet != NULL)
			csp_service_handler(conn, packet);

		csp_close(conn);
	}
}

int configure_csp(uint8_t addr, unsigned int type, char *device, unsigned int rate, char *key)
{
	csp_iface_t *ifc;

	gethostname(hostname, sizeof(hostname));
	hostname[sizeof(hostname) - 1] = '\0';
	csp_set_hostname(hostname);
	csp_set_model("satctl");

	if (csp_buffer_init(25, 320) < 0)
		return -1;

	if (csp_init(addr) < 0)
		return -1;

	if (type == TYPE_NONE) {
		ifc = &csp_if_lo;
	} else if (type == TYPE_CAN) {
		struct csp_can_config can_conf = {
			.ifc = device ? device : "can0",
			.bitrate = rate,
		};

		if (csp_can_init(0, 0, CSP_CAN_MASKED, &can_conf) < 0)
			return -1;

		ifc = &csp_if_can;
	} else if (type == TYPE_UART) {
		struct csp_usart_conf usart_conf = {
			.device = device ? device : "/dev/ttyUSB0",
			.baudrate = rate,
		};

		/* Run USART init */
		csp_usart_init(&usart_conf);

		/* Setup CSP interface */
		csp_kiss_init(&csp_if_kiss, &csp_kiss_driver, csp_usart_putc, NULL, "KISS");
		csp_usart_set_callback(usart_rx);

		ifc = &csp_if_kiss;
	} else if (type == TYPE_MCAST) {
		if (!device)
			device = "localhost";
		if (csp_mcast_init(device) < 0)
			return -1;

		ifc = &csp_if_mcast;
	} else if (type == TYPE_UDP) {
		if (!device)
			device = "eth0";
		if (csp_udp_init(device) < 0)
			return -1;

		ifc = &csp_if_udp;
	} else if (type == TYPE_SLGND) {
		if (!device)
			device = "127.0.0.1";
		if (csp_slgnd_init(device) < 0)
			return -1;

		ifc = &csp_if_slgnd;
	} else {
		fprintf(stderr, "Unknown interface\n");
		return -1;
	}

	if (csp_route_set(CSP_DEFAULT_ROUTE, ifc, CSP_NODE_MAC) < 0)
		return -1;

	if (key) {
		if (csp_xtea_set_key(key, strlen(key)) < 0) {
			fprintf(stderr, "Failed to set XTEA key\n");
			return -1;
		}
		if (csp_hmac_set_key(key, strlen(key)) < 0) {
			fprintf(stderr, "Failed to set HMAC key\n");
			return -1;
		}
	}

	if (csp_route_start_task(0, 0) < 0)
		return -1;

	pthread_t csp_thread;
	pthread_create(&csp_thread, NULL, thread_csp_service, NULL);

	return 0;
}

int main(int argc, char **argv)
{
	struct slash *slash;
	bool verbose = false;
	int remain, index, i, c, p = 0;
	char *ex, *endptr;
	FILE * fp;
	ssize_t nread;
	size_t len = 0;
	char *line = NULL;
	char prompt[32];

	unsigned int type = SATCTL_DEFAULT_TYPE;
	uint8_t addr = SATCTL_DEFAULT_ADDRESS;
	char *device = NULL;
	unsigned int rate = SATCTL_DEFAULT_RATE;
	char *crypto_key = NULL;

	snprintf(prompt, sizeof(prompt) - 1, SATCTL_PROMPT_FMT, "satctl");
	prompt[sizeof(prompt) - 1] = '\0';

	while ((c = getopt(argc, argv, "+hcmuesd:i:n:r:k:v")) != -1) {
		switch (c) {
		case 'h':
			usage();
			exit(EXIT_SUCCESS);
		case 'c':
			type = TYPE_CAN;
			break;
		case 'm':
			type = TYPE_MCAST;
			break;
		case 'u':
			type = TYPE_UART;
			break;
		case 'e':
			type = TYPE_UDP;
			break;
		case 's':
			type = TYPE_SLGND;
			break;
		case 'd':
			device = optarg;
			break;
		case 'i':
			snprintf(prompt, sizeof(prompt) - 1, SATCTL_PROMPT_FMT, optarg);
			prompt[sizeof(prompt) - 1] = '\0';
			break;
		case 'n':
			addr = strtoul(optarg, &endptr, 0);
			if (*endptr != '\0' || addr < 1 || addr > CSP_ID_HOST_MAX) {
				fprintf(stderr, "Invalid CSP address\n");
				exit(EXIT_FAILURE);
			}
			break;
		case 'r':
			rate = strtoul(optarg, &endptr, 0);
			if (*endptr != '\0') {
				fprintf(stderr, "Invalid rate\n");
				exit(EXIT_FAILURE);
			}
			break;
		case 'k':
			crypto_key = optarg;
			break;
		case 'v':
			verbose = true;
			break;
		case '?':
			if (optopt == 'c')
				fprintf(stderr, "Option -%c requires an argument\n", optopt);
			/* FALLTHROUGH */
		default:
			exit(EXIT_FAILURE);
		}
	}

	remain = argc - optind;
	index = optind;

	if (configure_csp(addr, type, device, rate, crypto_key) < 0) {
		fprintf(stderr, "Failed to init CSP\n");
		exit(EXIT_FAILURE);
	}

	slash = slash_create(SATCTL_LINE_SIZE, SATCTL_HISTORY_SIZE);
	if (!slash) {
		fprintf(stderr, "Failed to init slash\n");
		exit(EXIT_FAILURE);
	}

	/* Interactive or one-shot mode */
	if (remain > 0) {
		/* Try to open script file */	
		fp = fopen(argv[index], "r");
		if (fp != NULL) {
			if (remain > 1) {
				fprintf(stderr, "Only one file argument is allowed\n");
				exit(EXIT_FAILURE);
			}

			while ((nread = getline(&line, &len, fp)) != -1) {
				if (line[nread - 1] == '\n') {
					line[nread - 1] = '\0';
					nread--;
				}

				vmsg(verbose, "Running command \'%s\':", line);

				if (slash_execute(slash, line) < 0)
					exit(EXIT_FAILURE);
			}

			fclose(fp);
			free(line);
		} else {
			/* Build command string */
			ex = malloc(SATCTL_LINE_SIZE);
			if (!ex) {
				fprintf(stderr, "Failed to allocate command memory");
				exit(EXIT_FAILURE);
			}

			for (i = 0; i < remain; i++) {
				if (i > 0)
					p = ex - strncat(ex, " ", SATCTL_LINE_SIZE - p);
				p = ex - strncat(ex, argv[index + i], SATCTL_LINE_SIZE - p);
			}

			vmsg(verbose, "Running command \'%s\':", ex);

			if (slash_execute(slash, ex) < 0)
				exit(EXIT_FAILURE);

			free(ex);
		}
	} else {
		printf(SATCTL_DESC);
		printf(SATCTL_COPYRIGHT "\n");

		if (type == TYPE_NONE)
			printf("No interface selected, loopback only mode!\n\n");

		printf("Run 'help' for a list of available commands\n\n");

		slash_loop(slash, prompt, NULL);
	}

	slash_destroy(slash);

	return 0;
}
