/*
 * Copyright (c) 2016-2023 Satlab A/S <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 <getopt.h>

#include <slash/slash.h>

#include <csp/csp.h>
#include <csp/drivers/usart.h>
#include <csp/drivers/can_socketcan.h>
#include <csp/crypto/csp_xtea.h>
#include <csp/crypto/csp_hmac.h>
#include <csp/interfaces/csp_if_lo.h>
#include <csp/interfaces/csp_if_can.h>
#include <csp/interfaces/csp_if_kiss.h>
#include <csp/interfaces/csp_if_mcast.h>
#include <csp/interfaces/csp_if_slgnd.h>
#include <csp/interfaces/csp_if_sludp.h>

#ifdef HAVE_LIBZMQ
#include <zmq.h>
#include <csp/interfaces/csp_if_zmqhub.h>
#endif

#define SATCTL_PROMPT_FMT		"\033[94m[%s]\033[0m "
#define SATCTL_DEFAULT_ADDRESS		1
#define SATCTL_DEFAULT_RATE		1000000
#define SATCTL_CSP_MTU			1024
#define SATCTL_CSP_BUFFERS		255

#define SATCTL_LINE_SIZE		256
#define SATCTL_HISTORY_SIZE		4096

#define SATCTL_DESC			"Satlab Control Application " SATCTL_VERSION "\n"
#define SATCTL_COPYRIGHT		"Copyright (c) 2014-2023 Satlab A/S <satlab@satlab.com>\n"

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

static void usage(void)
{
	printf("usage: satctl [options] [command]\n");
	printf("\n");
	printf(SATCTL_DESC);
	printf(SATCTL_COPYRIGHT);
	printf("\n");
	printf("Options:\n");
	printf("  -c, --can=DEVICE[@RATE]    Enable CAN transport interface\n");
	printf("  -m, --mcast=DEVICE         Enable MCAST transport interface\n");
	printf("  -u, --uart=DEVICE[@RATE]   Enable UART/KISS transport interface\n");
	printf("  -s, --slgnd=HOST[:PORT]    Enable SLGND transport interface\n");
	printf("  -e, --udp=DEVICE           Enable UDP transport interface\n");
	printf("      --can-promisc          Enable promiscuous mode CAN interfaces\n");
#ifdef HAVE_LIBZMQ
	printf("  -z, --zmqhub=HOST          Enable ZMQHUB transport interface\n");
	printf("  -Z, --zmqproxy             Start built-in ZMQPROXY for ZMQHUB\n");
#endif
	printf("  -i, --id=ID                Show ID as prompt and CSP ident hostname\n");
	printf("  -n, --node=NODE            Use NODE as own CSP address\n");
	printf("  -r, --routes=ROUTES        Use ROUTES as CSP routing table\n");
	printf("  -k, --key=KEY              Use KEY as CSP XTEA/HMAC key\n");
	printf("  -p, --pingalive=NODE       Ping NODE every second in background\n");
	printf("  -h, --help                 Print this help and exit\n");
	printf("  -v, --verbose              Enable verbose output\n");
	printf("      --version              Print satctl version number\n");
}

#ifdef HAVE_LIBZMQ
struct zmqproxy_args {
	void *ctx;
	void *frontend;
	void *backend;
};

static void *thread_csp_zmqproxy(void *param)
{
	struct zmqproxy_args *args = param;

	zmq_proxy(args->frontend, args->backend, NULL);
	zmq_ctx_destroy(args->ctx);
	free(args);

	return NULL;
}

static int start_zmqproxy(void)
{
	pthread_t zmqproxy;
	struct zmqproxy_args *args;

	args = calloc(1, sizeof(*args));
	if (!args) {
		fprintf(stderr, "Failed to allocate zmqproxy args\n");
		return -ENOMEM;
	}

	args->ctx = zmq_ctx_new();
	if (!args->ctx) {
		fprintf(stderr, "Failed to create zmq context\n");
		goto err_free;
	}

	args->frontend = zmq_socket(args->ctx, ZMQ_XSUB);
	if (!args->frontend) {
		fprintf(stderr, "Failed to create zmqproxy frontend socket\n");
		goto err_free_ctx;
	}

	args->backend = zmq_socket(args->ctx, ZMQ_XPUB);
	if (!args->backend) {
		fprintf(stderr, "Failed to create zmqproxy backend socket\n");
		goto err_close_frontend;
	}

	if (zmq_bind(args->frontend, "tcp://*:6000") != 0) {
		fprintf(stderr, "Failed to bind zmqproxy frontend\n");
		goto err_close_backend;
	}

	if (zmq_bind(args->backend, "tcp://*:7000") != 0) {
		fprintf(stderr, "Failed to bind zmqproxy backend\n");
		goto err_close_backend;
	}

	if (pthread_create(&zmqproxy, NULL, thread_csp_zmqproxy, args) != 0) {
		fprintf(stderr, "Failed to start zmqproxy thread\n");
		goto err_close_backend;
	}

	return 0;

err_close_backend:
	zmq_close(args->backend);
err_close_frontend:
	zmq_close(args->frontend);
err_free_ctx:
	zmq_ctx_destroy(args->ctx);
err_free:
	free(args);

	return -EIO;
}
#endif

static 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);
	}

	return NULL;
}

static void *thread_csp_pingalive(void *param)
{
	uint8_t *pingalive = param;
	uint32_t pingalive_interval = 1000;
	unsigned int pingalive_size = 1;

	while (1) {
		/* Sleep if ping succeeds */
		if (csp_ping(*pingalive, pingalive_interval, pingalive_size, 0) >= 0)
			usleep(pingalive_interval * 1000);
	}

	return NULL;
}

static int split_device_and_rate(const char *opt,
				 const char **device,
				 unsigned int *rate,
				 unsigned int rate_default)
{
	static char device_buf[100];
	size_t device_length;
	char *atptr, *endptr;

	atptr = strchr(opt, '@');

	if (atptr) {
		if (!rate)
			return -EINVAL;

		device_length = atptr - opt;
		if (device_length + 1 > sizeof(device_buf)) {
			fprintf(stderr, "Invalid device\n");
			return -EINVAL;
		}

		memcpy(device_buf, opt, device_length);
		device_buf[device_length] = '\0';
		*device = device_buf;

		*rate = strtoul(atptr + 1, &endptr, 0);
		if (*endptr != '\0') {
			fprintf(stderr, "Invalid rate\n");
			return -EINVAL;
		}
	} else {
		*device = opt;
		if (rate)
			*rate = rate_default;
	}

	return 0;
}

static int configure_csp(uint8_t addr,
			 const char *can_opt,
			 const char *usart_opt,
			 const char *mcast_opt,
			 const char *udp_opt,
			 const char *slgnd_opt,
			 const char *zmqhub_opt,
			 const char *rtable,
			 const char *key,
			 const char *hostname,
			 bool can_promisc)
{
	int ret;
	pthread_t csp_thread;
	csp_conf_t conf;
	csp_iface_t *ifc;
	unsigned int interfaces = 0;

	unsigned int rate;
	const char *device;

	csp_conf_get_defaults(&conf);
	conf.address = addr;
	conf.hostname = hostname;
	conf.model = "satctl";
	conf.revision = SATCTL_VERSION;
	conf.conn_queue_length = SATCTL_CSP_BUFFERS;
	conf.fifo_length = SATCTL_CSP_BUFFERS;
	conf.port_max_bind = 31;
	conf.buffers = SATCTL_CSP_BUFFERS;
	conf.buffer_data_size = SATCTL_CSP_MTU;

	if (csp_init(&conf) < 0)
		return -1;

	if (can_opt) {
		/* Use 0 as default rate, meaning "current rate" */
		if (split_device_and_rate(can_opt, &device, &rate, 0) < 0)
			return -1;
		ret = csp_can_socketcan_open_and_add_interface(device, CSP_IF_CAN_DEFAULT_NAME, rate, can_promisc, &ifc);
		if (ret != CSP_ERR_NONE)
			return -1;
		interfaces++;
	}

	if (usart_opt) {
		if (split_device_and_rate(usart_opt, &device, &rate, 1000000) < 0)
			return -1;
		csp_usart_conf_t usart_conf = {
			.device = device,
			.baudrate = rate,
			.databits = 8,
			.stopbits = 1,
			.paritysetting = 0,
			.checkparity = 0
		};
		ret = csp_usart_open_and_add_kiss_interface(&usart_conf, CSP_IF_KISS_DEFAULT_NAME, &ifc);
		if (ret != CSP_ERR_NONE)
			return -1;
		interfaces++;
	}

	if (mcast_opt) {
		if (split_device_and_rate(mcast_opt, &device, NULL, 0) < 0)
			return -1;
		if (csp_mcast_init(device, CSP_IF_MCAST_DEFAULT_NAME, &ifc) < 0)
			return -1;
		interfaces++;
	}

	if (udp_opt) {
		if (split_device_and_rate(udp_opt, &device, NULL, 0) < 0)
			return -1;
		if (csp_sludp_init(device, CSP_IF_SLUDP_DEFAULT_NAME, &ifc) < 0)
			return -1;
		interfaces++;
	}

	if (slgnd_opt) {
		if (split_device_and_rate(slgnd_opt, &device, NULL, 0) < 0)
			return -1;
		if (csp_slgnd_init(device, CSP_IF_SLGND_DEFAULT_NAME, &ifc) < 0)
			return -1;
		interfaces++;
	}

#ifdef HAVE_LIBZMQ
	if (zmqhub_opt) {
		if (split_device_and_rate(zmqhub_opt, &device, NULL, 0) < 0)
			return -1;
		if (csp_zmqhub_init(csp_get_address(), device, 0, &ifc) < 0)
			return -1;
		interfaces++;
	}
#endif

	if (rtable) {
		if (csp_rtable_load(rtable) < 0) {
			fprintf(stderr, "Failed to load routing table\n");
			return -1;
		}
	} else {
		if (interfaces == 0) {
			fprintf(stderr, "No interface selected, loopback only mode!\n\n");
		} else {
			if (interfaces > 1)
				fprintf(stderr, "Multiple interfaces selected, but no routing table given!\n\n");

			if (csp_rtable_set(CSP_DEFAULT_ROUTE, 0, ifc, CSP_NO_VIA_ADDRESS) < 0) {
				fprintf(stderr, "Failed to set routing entry\n");
				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) {
		fprintf(stderr, "Failed to start CSP router\n");
		return -1;
	}

	if (pthread_create(&csp_thread, NULL, thread_csp_service, NULL) != 0) {
		fprintf(stderr, "Failed to start CSP service handler\n");
		return -1;
	}

	return 0;
}

static int run_script_file(struct slash *slash, const char *filename)
{
	FILE * fp;
	char *line = NULL;
	ssize_t nread;
	size_t len = 0;
	int ret = SLASH_SUCCESS;

	fp = fopen(filename, "r");
	if (!fp) {
		perror("Failed to open script file");
		return SLASH_EIO;
	}

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

		ret = slash_execute(slash, line);
		if (ret < 0)
			break;
	}

	fclose(fp);
	free(line);

	return SLASH_SUCCESS;
}

static int cmd_exec(struct slash *slash)
{
	if (slash->argc != 2)
		return SLASH_EUSAGE;

	return run_script_file(slash, slash->argv[1]);
}
slash_command(exec, cmd_exec, "<file>",
	      "Execute commands from file");

int main(int argc, char **argv)
{
	struct slash *slash;
	bool verbose = false;
	int remain, index, i, c, p = 0;
	char *ex, *endptr;
	char prompt[32];
	const char *name = NULL;
	char hostname[32] = "satctl";

	uint8_t addr = SATCTL_DEFAULT_ADDRESS;
	uint8_t pingalive = UINT8_MAX;
	pthread_t csp_pingalive_thread;

	const char *rtable = NULL;
	const char *can_opt = NULL, *mcast_opt = NULL, *usart_opt = NULL;
	const char *udp_opt = NULL, *slgnd_opt = NULL, *zmqhub_opt = NULL;

	char *crypto_key = NULL;
#ifdef HAVE_LIBZMQ
	bool zmqproxy = false;
#endif
	bool can_promisc = false;

	const struct option options[] = {
		{"help",        no_argument,       NULL, 'h'},
		{"can",         required_argument, NULL, 'c'},
		{"mcast",       required_argument, NULL, 'm'},
		{"uart",        required_argument, NULL, 'u'},
		{"udp",         required_argument, NULL, 'e'},
		{"slgnd",       required_argument, NULL, 's'},
		{"can-promisc", no_argument,       NULL, 'C'},
#ifdef HAVE_LIBZMQ
		{"zmqhub",      required_argument, NULL, 'z'},
		{"zmqproxy",    no_argument,       NULL, 'Z'},
#endif
		{"name",        required_argument, NULL, 'i'},
		{"addr",        required_argument, NULL, 'n'},
		{"routes",      required_argument, NULL, 'r'},
		{"key",         required_argument, NULL, 'k'},
		{"pingalive",   required_argument, NULL, 'p'},
		{"verbose",     no_argument,       NULL, 'v'},
		{"version",     no_argument,       NULL, 'V'},
		{NULL,          0,                 NULL, '\0'},
	};

	while ((c = getopt_long(argc, argv, "+hc:m:u:e:s:z:Zi:n:r:k:p:vCV", options, NULL)) != -1) {
		switch (c) {
		case 'h':
			usage();
			exit(EXIT_SUCCESS);
		case 'c':
			can_opt = optarg;
			break;
		case 'm':
			mcast_opt = optarg;
			break;
		case 'u':
			usart_opt = optarg;
			break;
		case 'e':
			udp_opt = optarg;
			break;
		case 's':
			slgnd_opt = optarg;
			break;
#ifdef HAVE_LIBZMQ
		case 'z':
			zmqhub_opt = optarg;
			break;
		case 'Z':
			zmqproxy = true;
			break;
#endif
		case 'i':
			name = optarg;
			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':
			rtable = optarg;
			break;
		case 'k':
			crypto_key = optarg;
			break;
		case 'p':
			pingalive = strtoull(optarg, &endptr, 0);
			if (*endptr != '\0' || pingalive < 1 || pingalive > CSP_ID_HOST_MAX) {
				fprintf(stderr, "Invalid pingalive CSP address\n");
				exit(EXIT_FAILURE);
			}
			break;
		case 'v':
			verbose = true;
			break;
		case 'C':
			can_promisc = true;
			break;
		case 'V':
			printf(SATCTL_DESC);
			printf(SATCTL_COPYRIGHT);
			exit(EXIT_SUCCESS);
		default:
			exit(EXIT_FAILURE);
		}
	}

	remain = argc - optind;
	index = optind;

	/* Build prompt and hostname */
	if (name) {
		strncpy(hostname, name, sizeof(hostname) - 1);
		hostname[sizeof(hostname) - 1] = '\0';
	} else {
		name = "satctl";
		gethostname(hostname, sizeof(hostname));
		hostname[sizeof(hostname) - 1] = '\0';
	}

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

#ifdef HAVE_LIBZMQ
	if (zmqproxy && start_zmqproxy() < 0) {
		fprintf(stderr, "Failed to start ZMQPROXY\n");
		exit(EXIT_FAILURE);
	}
#endif

	if (configure_csp(addr,
			  can_opt,
			  usart_opt,
			  mcast_opt,
			  udp_opt,
			  slgnd_opt,
			  zmqhub_opt,
			  rtable,
			  crypto_key,
			  hostname,
			  can_promisc) < 0) {
		fprintf(stderr, "Failed to init CSP\n");
		exit(EXIT_FAILURE);
	}

	if (pingalive != UINT8_MAX) {
		printf("Starting background pingalive thread for address %hhu\n", pingalive);
		if (pthread_create(&csp_pingalive_thread, NULL, thread_csp_pingalive, &pingalive) != 0) {
			fprintf(stderr, "Failed to start pingalive thread\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) {
		/* Build command string */
		ex = calloc(SATCTL_LINE_SIZE, 1);
		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");

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

		slash_set_prompt(slash, prompt);
		slash_loop(slash);
	}

	slash_destroy(slash);

	return 0;
}
