#include "command_message.h"

#include <string.h>
#include <zephyr/logging/log.h>

LOG_MODULE_REGISTER(command_message, LOG_LEVEL_INF);

void command_message_init(struct command_message_t *msg) {
	memset(msg, 0, sizeof(struct command_message_t));
	msg->prefix = COMMAND_PREFIX;
	msg->id = COMMAND_ID;
}

void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t *data) {
	// Ensure length doesn't exceed available space
	if (length > sizeof(msg->data) - 1) {
		return;
	}

	command_message_init(msg);
	msg->length = length;
	msg->command = command;

	// Copy the data
	if (data != NULL) {
		for (int i = 0; i < msg->length; i++) {
			msg->data[i] = data[i];
		}
	}
	msg->crc = command_calculate_crc(msg);
}

uint8_t command_calculate_crc(struct command_message_t *msg) {
	uint32_t sum = 0;
	uint8_t crc = 0;

	uint8_t *byte_ptr = (uint8_t *)msg;
	int loop_length = (sizeof(struct command_message_t) - sizeof(msg->data) + msg->length);

	for (int i = 0; i < loop_length; i++) {
		if (i == 4) { continue; }
		sum += byte_ptr[i];
	}

	crc = 0x100 - (sum & 0xff);

	return crc;
}

void command_create_ack(struct command_message_t *msg) {
	command_create_message(msg, 0, COMMAND_ACK, NULL);
}

void command_create_nack(struct command_message_t *msg) {
	command_create_message(msg, 0, COMMAND_NACK, NULL);
}

void command_log(struct command_message_t *msg) {
	if (msg->length > sizeof(msg->data) - 1) {
		LOG_ERR("Message length too long: %d", msg->length);
		return;
	}

	LOG_INF("Prefix: %d\n\r", msg->prefix);
	LOG_INF("Length: %d\n\r", msg->length);
	LOG_INF("COMMAND_ID: %d\n\r", msg->id);
	LOG_INF("Command: %d\n\r", msg->command);
	LOG_INF("Data:\n\r");

	for (int i = 0; i < msg->length; i++) {
		LOG_INF("%d", msg->data[i]);
	}

	LOG_INF("CRC: %d\n\r", msg->crc);
}