#include <stdio.h>
#include <stdarg.h>
#include "bsp/bsp_driver.h"
#include "logger.h"
#include "os/os_task.h"
#include "libs/utils.h"
#include "prot/can_message.h"

static uint32_t level_data[2];
static char log_cache[128];
//static co_queue_t log_queue = NULL;
int can_fputc(char c);


void plot_3data16(s16 v1, s16 v2, s16 v3) {
	u8 data[6] = {0};
	encode_s16(data, v1);
	encode_s16(data + 2, v2);
	encode_s16(data + 4, v3);
	shark_uart_write_frame(SHARK_UART0, data, sizeof(data));
}

void plot_2data16(s16 v1, s16 v2) {
	u8 data[4] = {0};
	encode_s16(data, v1);
	encode_s16(data + 2, v2);
	shark_uart_write_frame(SHARK_UART0, data, sizeof(data));
}

void plot_1data16(s16 v1) {
	u8 data[2] = {0};
	encode_s16(data, v1);
	shark_uart_write_frame(SHARK_UART0, data, sizeof(data));
}

static int _get_level(int mod){
	int index = mod_bit_start(mod)/(32+1);
	mod = mod_bit_start(mod) % 32;
	return (level_data[index] >> mod) & LEVEL_MASK;
}

void set_log_level(int mod, int l){
	int index = mod_bit_start(mod)/(32+1);
	mod = mod_bit_start(mod) % 32;
	level_data[index] = (level_data[index] & (~(LEVEL_MASK<<mod))) | ((l & LEVEL_MASK)<<mod);
}

static void log_out(char *fmt, va_list args){
	int len = vsnprintf(log_cache, sizeof(log_cache), fmt, args);
	for (int i = 0; i < len; i++) {
		can_fputc(log_cache[i]);
	}
}

void log_debug(int mod, char *fmt, ...){
	if (_get_level(mod) >= L_debug){
		va_list args;
		va_start(args, fmt);		
		log_out(fmt, args);
		va_end(args);
	}
}

void log_warning(int mod, char *fmt, ...){
	if (_get_level(mod) >= L_warning){
		va_list args;
		va_start(args, fmt);		
		log_out(fmt, args);
		va_end(args);
	}
}

void log_error(int mod, char *fmt, ...){
	if (_get_level(mod) >= L_error){
		va_list args;
		va_start(args, fmt);		
		log_out(fmt, args);
		va_end(args);
	}
}

#ifndef GD32_FOC_DEMO
static char log_buffer[8];
static int log_index = 0;
int can_fputc(char c){
	can_id_t frame_id;
	frame_id.id=0;
	frame_id.src 	= CAN_MY_ADDRESS;
	frame_id.type 		= ptype_indicater;

	if (c == '\n'){ //don't send '\n'
		if (log_index != 0){
			frame_id.dest 	= 0x72;
			can_send_message(frame_id.id, (u8 *)log_buffer, log_index, 0);
		}
		log_index = 0;
	}else if (log_index == sizeof(log_buffer)){
		frame_id.dest 	= 0x70;
		can_send_message(frame_id.id, (u8 *)log_buffer, log_index, 0);
		log_index = 0;
		log_buffer[log_index++] = c;
	}else{
		log_buffer[log_index++] = c;
	}
	return 1;
}
#else
int can_fputc(char c) {
	return 1;
}
#endif