#include "zbus_message.h"
#include "servo.h"
#include "adc.h"
#include "uart.h"

#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/shell/shell.h>
#include <zephyr/zbus/zbus.h>
#include <stdlib.h>

LOG_MODULE_REGISTER(shell);

// ---------------------------------------------------------------------
// LEDs

static int cmd_led_set(const struct shell *shell, size_t argc, char **argv) {
	struct leds_message msg;
	int ret;

	/* Parse LED id */
	char *endptr;
	long id = strtol(argv[1], &endptr, 10);

	if (*endptr != '\0' || id < 0 || id > 1) {
		shell_error(shell, "Invalid LED id '%s'. Use 0 or 1.", argv[1]);
		return -EINVAL;
	}

	/* Parse state */
	if (strcmp(argv[2], "on") == 0) {
		msg.led_value = true;
	} else if (strcmp(argv[2], "off") == 0) {
		msg.led_value = false;
	} else {
		shell_error(shell, "Invalid state '%s'. Use 'on' or 'off'.", argv[2]);
		return -EINVAL;
	}

	msg.led_id = (uint8_t)id;

	ret = zbus_chan_pub(&leds_chan, &msg, K_MSEC(200));
	if (ret != 0) {
		shell_error(shell, "Failed to publish LED command: %d", ret);
		return ret;
	}

	shell_print(shell, "LED%d turned %s", msg.led_id, msg.led_value ? "ON" : "OFF");
	return 0;
}

static int cmd_led_all_on(const struct shell *shell, size_t argc, char **argv) {
	struct leds_message msg;
	int ret;

	for (uint8_t i = 0; i <= 1; i++) {
		msg.led_id    = i;
		msg.led_value = true;

		ret = zbus_chan_pub(&leds_chan, &msg, K_MSEC(200));
		if (ret != 0) {
			shell_error(shell, "Failed to turn on LED%d: %d", i, ret);
			return ret;
		}
	}

	shell_print(shell, "All LEDs turned ON");
	return 0;
}

static int cmd_led_all_off(const struct shell *shell, size_t argc, char **argv) {
	struct leds_message msg;
	int ret;

	for (uint8_t i = 0; i <= 1; i++) {
		msg.led_id    = i;
		msg.led_value = false;

		ret = zbus_chan_pub(&leds_chan, &msg, K_MSEC(200));
		if (ret != 0) {
			shell_error(shell, "Failed to turn off LED%d: %d", i, ret);
			return ret;
		}
	}

	shell_print(shell, "All LEDs turned OFF");
	return 0;
}

/* Subcommands for the "led" root command */
SHELL_STATIC_SUBCMD_SET_CREATE(sub_led,
	SHELL_CMD_ARG(set, NULL,
		"Set a single LED state.\n"
		"Usage: led set <id> <on|off>\n"
		"  id  : 0 or 1\n"
		"  state: on | off",
		cmd_led_set, 3, 0),
	SHELL_CMD(allon, NULL,
		"Turn all LEDs on.\n"
		"Usage: led allon",
		cmd_led_all_on),
	SHELL_CMD(alloff, NULL,
		"Turn all LEDs off.\n"
		"Usage: led alloff",
		cmd_led_all_off),
	SHELL_SUBCMD_SET_END
);

SHELL_CMD_REGISTER(led, &sub_led,
	"LED control commands.\n"
	"  led set <id> <on|off>  - Set LED state\n"
	"  led allon              - Turn all LEDs on\n"
	"  led alloff             - Turn all LEDs off",
	NULL);

// ---------------------------------------------------------------------
// SERVO

static int cmd_servo_set(const struct shell *shell, size_t argc, char **argv) {
	/* Parse SERVO id */
	char *endptr;
	long id = strtol(argv[1], &endptr, 10);

	if (*endptr != '\0' || id < 0 || id > NUM_SERVO_CHANNELS) {
		shell_error(shell, "Invalid SERVO id '%s'.", argv[1]);
		return -EINVAL;
	}

	long value = strtol(argv[2], &endptr, 10);

	if (*endptr != '\0' || value < SERVO_MIN_ANGLE || value > SERVO_MAX_ANGLE) {
		shell_error(shell, "Invalid SERVO value '%s'.(%d - %d)", argv[2], SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
		return -EINVAL;
	}

	servo_set_angle((int)id, (int)value);

	shell_print(shell, "SERVO%d turned %d", (int)id, (int)value);
	return 0;
}

/* Subcommands for the "servo" root command */
SHELL_STATIC_SUBCMD_SET_CREATE(sub_servo,
	SHELL_CMD_ARG(set, NULL,
		"Set a single SERVO state.\n"
		"Usage: servo set <id> <angle in deg>\n",
		cmd_servo_set, 3, 0),
	SHELL_SUBCMD_SET_END
);

SHELL_CMD_REGISTER(servo, &sub_servo,
	"SERVO control commands.\n"
	"  servo set <id> <angle in deg>  - Set SERVO state\n",
	NULL);

// ---------------------------------------------------------------------
// ADC

static int cmd_adc_read(const struct shell *shell, size_t argc, char **argv) {
	/* Parse ADC id */
	char *endptr;
	long id = strtol(argv[1], &endptr, 10);

	if (*endptr != '\0' || id < 0 || id > NUM_SERVO_CHANNELS) {
		shell_error(shell, "Invalid ADC id '%s'.", argv[1]);
		return -EINVAL;
	}

	int value = adc_read_id((int)id);

	shell_print(shell, "ADC%d value: %d", (int)id, (int)value);
	return 0;
}

static int cmd_adc_read_for(const struct shell *shell, size_t argc, char **argv) {
	/* Parse ADC id */
	char *endptr;
	long id = strtol(argv[1], &endptr, 10);

	if (*endptr != '\0' || id < 0 || id > NUM_SERVO_CHANNELS) {
		shell_error(shell, "Invalid ADC id '%s'.", argv[1]);
		return -EINVAL;
	}

	long count = strtol(argv[2], &endptr, 10);

	if (*endptr != '\0') {
		return -EINVAL;
	}

	for (int i = 0; i < count; i++) {
		int value = adc_read_id((int)id);
		shell_print(shell, "ADC%d value: %d", (int)id, (int)value);
	}

	return 0;
}

static int cmd_adc_scan(const struct shell *shell, size_t argc, char **argv) {
	for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
		int value = adc_read_id(i);
		// shell_print(shell, "ADC%d value: %d", i, (int)value);
		char buf[16];
		snprintf(buf, sizeof(buf), "ADC,%d,%d\n\r", i, (int)value);
		print_uart(buf);
	}
	return 0;
}

int buffer[18][1024];
uint32_t tick_buffer[18][1024];
uint32_t tick_speed;
static int cmd_adc_scan_for(const struct shell *shell, size_t argc, char **argv) {
	/* Parse ADC id */
	char *endptr;

	long count = strtol(argv[1], &endptr, 10);

	if (*endptr != '\0' || count > 1024) {
		return -EINVAL;
	}

	tick_speed = sys_clock_hw_cycles_per_sec();
	shell_print(shell, "Systick: %d", (int)tick_speed);

	for (int i = 0; i < count; i++) {
		for (int j = 0; j < NUM_ADC_CHANNELS; j++) {
			int value = adc_read_id(j);
			buffer[j][i] = value;
			tick_buffer[j][i] = k_cycle_get_32();
			// shell_print(shell, "ADC%d value: %d", j, (int)value);
			// char buf[16];
			// snprintf(buf, sizeof(buf), "%d,ADC,%d,%d\n\r", i, j, (int)value);
			// print_uart(buf);
		}
	}

	for (int i = 0; i < count; i++) {
		for (int j = 0; j < NUM_ADC_CHANNELS; j++) {
			char buf[32];
			snprintf(buf, sizeof(buf), "%d,ADC,%d,%d,%u\n\r", i, j, (int)buffer[j][i], tick_buffer[j][i]);
			print_uart(buf);
		}
	}

	return 0;
}

/* Subcommands for the "servo" root command */
SHELL_STATIC_SUBCMD_SET_CREATE(sub_adc,
	SHELL_CMD_ARG(read, NULL,
		"Read a single ADC state.\n"
		"Usage: adc read <id>\n",
		cmd_adc_read, 2, 0),
	SHELL_CMD_ARG(read_for, NULL,
		"Read X amount of ADC states.\n"
		"Usage: adc read_for <id> <count>\n",
		cmd_adc_read_for, 3, 0),
	SHELL_CMD(scan, NULL,
		"Read all ADC channels.\n"
		"Usage: adc scan",
		cmd_adc_scan),
	SHELL_CMD_ARG(scan_for, NULL,
		"Read all ADC, X amount of ADC states.\n"
		"Usage: adc scan_for <count>\n",
		cmd_adc_scan_for, 2, 0),
	SHELL_SUBCMD_SET_END
);

SHELL_CMD_REGISTER(adc, &sub_adc,
	"ADC control commands.\n"
	"  adc read <id> - Read ADC\n"
	"  adc read_for <id> <count> - Read ADC X amount of times\n"
	"  adc scan - Read all ADC channels\n"
	"  adc scan_for <count> - Read all ADC X amount of times\n",
	NULL);