/*
 * debug.c
 */

#include "main.h"
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdarg.h>
#include "debug.h"
#include "serial_control.h"
#include "usart.h"

#define DEBUG_BUFFER_SIZE 1024
#define DEBUG_BUFFER_MAX_COUNT 128
#define LOG_BUFFER_SIZE 128

typedef struct {
	uint8_t buffer[DEBUG_BUFFER_SIZE];
	volatile uint16_t write_index;
	volatile uint16_t read_index;
	volatile uint16_t count;
} DebugBuffer_t;

static DebugBuffer_t debug_buffer = {
	.buffer = {0},
	.write_index = 0,
	.read_index = 0,
	.count = 0
};

static uint8_t log_buffer[LOG_BUFFER_SIZE];

static void debug_uart1_write(const uint8_t *data, uint16_t len)
{
	/* Best-effort debug mirror to USART1 (PA9/PA10), safe for IRQ context. */
	for (uint16_t i = 0; i < len; i++) {
		uint32_t timeout = 10000U;
		while (((USART1->SR & USART_SR_TXE) == 0U) && (timeout > 0U)) {
			timeout--;
		}
		if (timeout == 0U) {
			return;
		}
		USART1->DR = data[i];
	}
}

#if defined(__GNUC__)
int _write(int fd, char * ptr, int len)
{
    (void)fd;
	debug_buffer_add((const uint8_t*)ptr, (uint16_t)len);
    return len;
}
#endif

void debug_buffer_add(const uint8_t* data, uint16_t len)
{
	debug_uart1_write(data, len);
	__disable_irq();
	for (uint16_t i = 0; i < len; i++) {
		if (debug_buffer.count >= DEBUG_BUFFER_SIZE) {
			debug_buffer.read_index = (debug_buffer.read_index + 1U) % DEBUG_BUFFER_SIZE;
			debug_buffer.count--;
		}
		debug_buffer.buffer[debug_buffer.write_index] = data[i];
		debug_buffer.write_index = (debug_buffer.write_index + 1U) % DEBUG_BUFFER_SIZE;
		debug_buffer.count++;
	}
	__enable_irq();
}

uint16_t debug_buffer_available(void)
{
	__disable_irq();
	uint16_t count = debug_buffer.count;
	__enable_irq();
	return count;
}

void debug_buffer_send(void)
{
	__disable_irq();
	if (debug_buffer.count == 0U) {
		__enable_irq();
		return;
	}

	uint16_t bytes_to_send = debug_buffer.count;
	if (bytes_to_send > DEBUG_BUFFER_MAX_COUNT) {
		bytes_to_send = DEBUG_BUFFER_MAX_COUNT;
	}

	uint16_t bytes_to_end = DEBUG_BUFFER_SIZE - debug_buffer.read_index;
	if (bytes_to_send > bytes_to_end) {
		bytes_to_send = bytes_to_end;
	}

	if (bytes_to_send == debug_buffer.count) {
		SC_SendPacket(&debug_buffer.buffer[debug_buffer.read_index], bytes_to_send, CMD_GET_LOG);
	} else {
		SC_SendPacket(&debug_buffer.buffer[debug_buffer.read_index], bytes_to_send, CMD_GET_LOG_CONTINUE);
	}

	debug_buffer.read_index = (debug_buffer.read_index + bytes_to_send) % DEBUG_BUFFER_SIZE;
	debug_buffer.count -= bytes_to_send;
	__enable_irq();
}

int log_printf(int level, const char *format, ...)
{
	va_list args;
	int result;
	const char *tag;
	int written;
	EDCAN_LogLevel_t current_level;
	EDCAN_LogLevel_t msg_level;

	if(level < LOG_EMERG || level > LOG_DEBUG){
		return 0;
	}
	msg_level = (EDCAN_LogLevel_t)level;
	current_level = EDCAN_GetLogLevel();
	if(msg_level > current_level){
		return 0;
	}

	switch(level){
	case LOG_EMERG:
		tag = "[EMR] ";
		break;
	case LOG_ALERT:
		tag = "[ALT] ";
		break;
	case LOG_CRIT:
		tag = "[CRT] ";
		break;
	case LOG_ERR:
		tag = "[ERR] ";
		break;
	case LOG_WARN:
		tag = "[WRN] ";
		break;
	case LOG_NOTICE:
		tag = "[NTC] ";
		break;
	case LOG_INFO:
		tag = "[INF] ";
		break;
	case LOG_DEBUG:
		tag = "[DBG] ";
		break;
	default:
		tag = "[LOG] ";
		break;
	}

	log_buffer[0] = (uint8_t)level;
	written = snprintf((char*)&log_buffer[1], LOG_BUFFER_SIZE - 2, "EV %s", tag);
	if(written < 0){
		return written;
	}
	if(written >= (LOG_BUFFER_SIZE - 2)){
		written = LOG_BUFFER_SIZE - 2;
	}

	va_start(args, format);
	result = vsnprintf((char*)&log_buffer[1 + written], LOG_BUFFER_SIZE - 2 - written, format, args);
	va_end(args);

	if (result < 0) {
		return result;
	}
	if (result >= (LOG_BUFFER_SIZE - 2 - written)) {
		result = LOG_BUFFER_SIZE - 2 - written;
	}

	log_buffer[1 + written + result] = '\0';
	debug_buffer_add(log_buffer, (uint16_t)(2 + written + result));
	return result + written;
}