#include "command_handler.h"
#include "command_message.h"
#include "led.h"
#include "servo.h"
#include "adc.h"
#include "usb.h"
#include "digital_out.h"
#include "power_monitor.h"

#include <zephyr/logging/log.h>

LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF);


// BUFFER (add ack and nack at the and as static)
#define CMD_MSG_BUFFER_SIZE 10
struct command_message_t cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 2];
struct command_message_t *cmd_msg_buffer_ptr;


int command_handler_init() {
	// BUFFER
	memset(cmd_msg_buffer, 0, sizeof(cmd_msg_buffer));
	cmd_msg_buffer_ptr = cmd_msg_buffer;

	// CREATE ACK/NACK
	command_create_ack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE]);
	command_create_nack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 1]);

	return 0;
}

int command_handler_rx(struct command_message_t *msg) {
	if (msg == NULL) {
		LOG_ERR("Received NULL message pointer");
		return -EINVAL;
	}

	LOG_DBG("Processing command: %d, length: %d", msg->command, msg->length);

	switch (msg->command) {
		case LED_TOGGLE: {
			// Toggle LED
			int led = msg->data[0];

			if (!led) {
				led0_toggle();
			}
			else {
				led1_toggle();
			}
			break;
		}

		case LED_SET: {
			// Set LED
			int led = msg->data[0];
			int state = msg->data[1];

			led_set(led, state);

			break;
		}

		case SERVO_SET: {
			// Set SERVO
			int servo = msg->data[0];
			float angle;
			memcpy(&angle, &msg->data[1], sizeof(float));

			servo_set_angle(servo, angle);

			break;
		}

		case SERVO_SET_ALL: {
			// Set ALL SERVO
			float angles[NUM_SERVO_CHANNELS];
			for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
				int start = i*4;
				memcpy(&angles[i], &msg->data[start], sizeof(float));
			}

			servo_set_all_angles(angles);

			break;
		}

		case ADC_READ_RAW: {
			int channel = msg->data[0];
			int value = adc_read_id(channel);

			struct command_message_t *reply = cmd_get_next_tx_buf();
			command_create_message(reply, sizeof(value), msg->command, (uint8_t *)&value);
			command_handler_tx(reply);

			break;
		}
		case ADC_READ_RAW_ALL: {
			int values[NUM_ADC_CHANNELS];
			for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
				values[i] = adc_read_id(i);
			}

			struct command_message_t *reply = cmd_get_next_tx_buf();
			command_create_message(reply, sizeof(values), msg->command, (uint8_t *)values);
			command_handler_tx(reply);
			
			break;
		}

		case ADC_READ: {
			uint32_t mask;
			memcpy(&mask, &msg->data[0], sizeof(uint32_t));

			adc_add_mask(mask);

			break;
		}

		case ADC_READ_ALL: {
			uint32_t mask = 0b111111111111111111;

			adc_add_mask(mask);
			
			break;
		}

		case ADC_SET_READ: {
			int interval_ms;
			memcpy(&interval_ms, &msg->data[0], sizeof(int));

			adc_timer_set(interval_ms);
			
			break;
		}

		case DIGITAL_LEG_OUT: {
			uint8_t mask = msg->data[0];

			digital_out_set_leg(mask);
			
			break;
		}

		case POWER_MONITOR_SET_READ: {
			int interval_ms;
			memcpy(&interval_ms, &msg->data[0], sizeof(int));

			power_monitor_timer_set(interval_ms);
			
			break;
		}

		default: {
			LOG_WRN("Unknown command received: %d", msg->command);
			return -EINVAL;
		}
	}

	return 0;
}

struct command_message_t* cmd_get_next_tx_buf() {
	struct command_message_t *buf = cmd_msg_buffer_ptr;

	// Increment the buffer pointer
	cmd_msg_buffer_ptr++;

	if (cmd_msg_buffer_ptr > &cmd_msg_buffer[CMD_MSG_BUFFER_SIZE-1]) {
		cmd_msg_buffer_ptr = cmd_msg_buffer;
	}

	return buf;
}

int command_handler_tx(struct command_message_t *msg) {

	// Send over USB if enabled and ready
	if (usb_is_tx_enabled()) {
		usb_send_command(msg);
	}

	return 0;
}