#include "pdebug.h"
#include "driver_init.h"
#include <stdio.h>
#include <string.h>
#include <stdarg.h>

#define MAX_PRINTF_BUFFER 256
static struct io_descriptor *debug_io;

int pdebug_init(void)
{
	usart_sync_get_io_descriptor(&USART_DBG, &debug_io);
	usart_sync_enable(&USART_DBG);
	printf("Debug Initialized\n");

	return 0;
}

int pprintf(const char* fmt, ...)
{
	size_t size_str = strlen(fmt);
	if (size_str >= MAX_PRINTF_BUFFER)
	{
		return -1;
	}
	uint8_t printf_buffer[MAX_PRINTF_BUFFER];
	memset(printf_buffer, '\0', MAX_PRINTF_BUFFER);
	va_list args;
	va_start(args, fmt);
	vsprintf((char*)printf_buffer, fmt, args);
	va_end(args);
	return io_write(debug_io, (const uint8_t*)printf_buffer, strlen((const char*)printf_buffer));
}

void passert(const bool cond, const char* msg_failure, const char* file, const int line)
{
	if(!cond)
	{
		printf("Assert Failure at line %d, %s: %s\n",
			   line,
			   file,
			   msg_failure);
		for(;;){}
	}
}