#include <rtlibc_conf.h>

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <printf.h>
#include <assert.h>

#include <math.h>

#include <../src/stdio_internal.h>


static const char l_flags[] = "-+ #0";
static const char l_length_chars[] = "hljzt";

#define max(a, b) (((uint32_t)(a) > (uint32_t)(b)) ? (uint32_t)(a) : (uint32_t)(b))
#define min(a, b) (((uint32_t)(a) < (uint32_t)(b)) ? (uint32_t)(a) : (uint32_t)(b))


//printf %c
static int32_t fsh_c(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	int32_t rv = 0;
	char val = (char)va_arg(*arg, int32_t);
	OUTPUT_CHARS(user, &val, 1);
	return rv;
}


//printf %s
static int32_t fsh_s(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	int32_t rv = 0;
	
	if (s->width == SPECIFIED_AS_ARG)
		s->width = va_arg(*arg, uint32_t);
	else if (s->width == NOT_SPECIFIED)
		s->width = 0;

	if (s->precision == SPECIFIED_AS_ARG)
		s->precision = va_arg(*arg, uint32_t);
		
	const char *str = va_arg(*arg, const char*);
	
	const size_t print_length = (s->precision == NOT_SPECIFIED) ? strlen(str) : strnlen(str, s->precision);

	const uint32_t width_total = max(s->width, print_length);
	uint32_t width_padding = width_total - print_length;
	
	if (s->flags & FLAG_MINUS)
		OUTPUT_CHARS(user, str, print_length);
	
	while(width_padding--)
		OUTPUT_CHARS(user, " ", 1);
	
	if ((s->flags & FLAG_MINUS) == 0)
		OUTPUT_CHARS(user, str, print_length);

	return rv;
}

//print %f
static int32_t fsh_float(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	int32_t rv = 0;
	
	if (s->width == SPECIFIED_AS_ARG)
		s->width = va_arg(*arg, uint32_t);
	else if (s->width == NOT_SPECIFIED)
		s->width = 0;
		
	if (s->precision == SPECIFIED_AS_ARG)
		s->precision = va_arg(*arg, uint32_t);
	else if (s->precision == NOT_SPECIFIED)
		s->precision = 6;

	double val = va_arg(*arg, double);
	char sign = 0;
	
	if (val < 0)
	{
		sign = '-';
		val = -val;
	}
	
	if ((s->flags & FLAG_PLUS) && (sign == 0))
		sign = '+';

	//round the number to the precision specified
	uint64_t p = 1;
	for(int32_t i = 0; i < s->precision; i++)
		p *= 10;


	uint64_t integer = (uint64_t)val;
	const double frac_d = (val - integer) * p;
	uint64_t frac = (uint64_t)frac_d;
	const double diff = frac_d - frac;
	
	if (diff > 0.5)
	{
		frac++;
		
		if (frac >= p)
		{
			frac = 0;
			integer++;
		}
	}
	else if (diff < 0.5)
	{
	}
	else if ((frac == 0) || (frac & 1))
	{
		//banker's rounding rule
		frac++;
	}
	
	
	if (s->precision == 0)
	{
		const double diff = val - integer;
		if (!((diff < 0.5) || (diff > 0.5)) && (integer & 1))
			integer++;
	}

	
	//get the digits
	char str[40];		//will fit 10^38 (integer part only)
	char *ptr = str + sizeof(str);
	if (integer)
	{
		while(integer)
		{
			*--ptr = (integer % 10) + '0';
			integer /= 10;
		}
	}
	else
		*--ptr = '0';
	
	//get the decimals
	char decimals[s->precision + 1];	
	if (s->precision)
	{
		char *p_decimal = decimals + sizeof(decimals);
		
		for(int32_t i = 0; i < s->precision; i++)
		{
			*--p_decimal = (frac % 10) + '0';
			frac /= 10;
		}
			
		*--p_decimal = '.';
	}
	
	
	const uint32_t significant_digits = str + sizeof(str) - ptr;						//total number of significant digits (without the sign)
	const uint32_t total_chars = significant_digits + (sign ? 1 : 0) + (s->precision ? s->precision + 1 : 0);
	
	const uint32_t width_total = max(s->width, total_chars);	
	uint32_t width_padding = width_total - total_chars;


	if (s->flags & FLAG_MINUS)
	{
		//left alignment
		if (sign)
			OUTPUT_CHARS(user, &sign, 1);
	
		OUTPUT_CHARS(user, ptr, significant_digits);
		
		if (s->precision)
			OUTPUT_CHARS(user, decimals, s->precision + 1);
		
		while(width_padding--)
			OUTPUT_CHARS(user, " ", 1);
	}
	else
	{
		const char pad_char = (s->flags & FLAG_ZERO) ? '0' : ' ';
		
		//right alignment
		if ((s->flags & FLAG_ZERO) && sign)
			OUTPUT_CHARS(user, &sign, 1);
		
		while(width_padding--)
			OUTPUT_CHARS(user, &pad_char, 1);
		
		if (!(s->flags & FLAG_ZERO) && sign)
			OUTPUT_CHARS(user, &sign, 1);
		
		OUTPUT_CHARS(user, ptr, significant_digits);

		if (s->precision)
			OUTPUT_CHARS(user, decimals, s->precision + 1);
	}


	return rv;
}



//print integers%d, %u, %x, %X, %o, %p
static int32_t fsh_integer(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s, const char *charset, int32_t is_signed)
{
	int32_t rv = 0;
	
	const uint32_t base = strlen(charset);

	char str[32];		//will fit a 32 bit number represented in binary
	char* const str_end = str + sizeof(str);

	if (s->width == SPECIFIED_AS_ARG)
		s->width = va_arg(*arg, uint32_t);
	else if (s->width == NOT_SPECIFIED)
		s->width = 0;
		
	if (s->precision == SPECIFIED_AS_ARG)
		s->precision = va_arg(*arg, uint32_t);
	else if (s->precision == NOT_SPECIFIED)
		s->precision = 1;

	uint64_t val;
	char sign = 0;

	if (is_signed)
	{
		int64_t v;
		
		if (s->length == 'l')
			v = va_arg(*arg, int64_t);
		else
			v = va_arg(*arg, int32_t);
		
		if (v < 0)
		{
			val = -v;
			sign = '-';
		}
		else
			val = v;
	}
	else
	{
		if (s->length == 'l')
			val = va_arg(*arg, uint64_t);
		else
			val = va_arg(*arg, uint32_t);
	}
	

	if ((s->flags & FLAG_PLUS) && (sign == 0))
		sign = '+';
	
	char *ptr = str_end;
	while(val)
	{
		*--ptr = charset[val % base];
		val /= base;
	}

	const uint32_t significant_digits = str_end - ptr;						//total number of significant digits (without the sign)
	uint32_t precision_digits = max(s->precision, significant_digits);		//total number of digits including the precision padding
	uint32_t precision_padding = precision_digits - significant_digits;		//how many zeros to pad with
	const uint32_t precision_digits_sign = sign ? (precision_digits + 1) : precision_digits;		//make space for sign

	const uint32_t width_total = max(s->width, precision_digits_sign);
	uint32_t width_padding = width_total - precision_digits_sign;
	
	if (s->flags & FLAG_MINUS)
	{
		//left alignment
		if (sign)
			OUTPUT_CHARS(user, &sign, 1);
	
		while(precision_padding--)
			OUTPUT_CHARS(user, "0", 1);
					
		OUTPUT_CHARS(user, ptr, significant_digits);
	
		while(width_padding--)
			OUTPUT_CHARS(user, " ", 1);
	}
	else
	{
		const char pad_char = (s->flags & FLAG_ZERO) ? '0' : ' ';
		
		//right alignment
		if ((s->flags & FLAG_ZERO) && sign)
			OUTPUT_CHARS(user, &sign, 1);
		
		while(width_padding--)
			OUTPUT_CHARS(user, &pad_char, 1);
		
		if (!(s->flags & FLAG_ZERO) && sign)
			OUTPUT_CHARS(user, &sign, 1);
		
		while(precision_padding--)
			OUTPUT_CHARS(user, "0", 1);

		OUTPUT_CHARS(user, ptr, significant_digits);
	}

	return rv;
}


//print %u
static int32_t fsh_u(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	return fsh_integer(output_handler, user, arg, s, "0123456789", 0);
}

//print %d
static int32_t fsh_d(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	return fsh_integer(output_handler, user, arg, s, "0123456789", 1);
}


//printf %x
static int32_t fsh_x(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	return fsh_integer(output_handler, user, arg, s, "0123456789abcdef", 0);
}


//printf %X
static int32_t fsh_x_cap(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	return fsh_integer(output_handler, user, arg, s, "0123456789ABCDEF", 0);
}

//printf %o
static int32_t fsh_o(rtprintf_output_handler_t output_handler, void *user, va_list *arg, rtformat_specifier_t *s)
{
	return fsh_integer(output_handler, user, arg, s, "01234567", 0);
}



/*
//print %r	-	fixed point positive (number of fraction bits precedes the value in the variable argument list)
static int fsh_ufxp_com(FILE *f, va_list *arg, format_specifier_t *s, uint8_t shortest, uint8_t signed_print)
{
	char integer_str[12];		//will fit a 32 bit number
	char fraction_str[32];		//will fit a 32 bit fractional part

	if (s->width == SPECIFIED_AS_ARG)
		s->width = va_arg(*arg, uint32_t);
	else if (s->width == NOT_SPECIFIED)
		s->width = 0;
	if (s->precision == SPECIFIED_AS_ARG)
		s->precision = va_arg(*arg, uint32_t);
	else if (s->precision == NOT_SPECIFIED)
	{
		shortest = 1;
		s->precision = sizeof(fraction_str);
	}
		
	s->precision = min(s->precision, sizeof(fraction_str));			//TODO

	uint32_t frac_bits = va_arg(*arg, uint32_t);			//this is also the maximum number of decimal fraction digits
	uint32_t val;
	char sign;
	

	if (signed_print)	
	{
		int32_t signed_val = va_arg(*arg, int32_t);
		
		if (signed_val < 0)
		{
			val = -signed_val;
			sign = '-';
		}
		else
		{
			val = signed_val;
			sign = (s->flags & FLAG_PLUS) ? '+' : 0;	
		}
	}
	else
	{
		val = va_arg(*arg, uint32_t);
		sign = (s->flags & FLAG_PLUS) ? '+' : 0;
	}

	
	uint32_t fraction = val & ((1 << frac_bits) - 1);
	val >>= frac_bits;
	
	char *integer_ptr = integer_str + sizeof(integer_str);
	if (val)
	{
		while(val)
		{
			*--integer_ptr = (val % 10) + '0';
			val /= 10;
		}
	}
	else
		*--integer_ptr = '0';
	
	char *fraction_ptr = fraction_str;
	const uint32_t mask = (1 << frac_bits) - 1;
	while((fraction || !shortest) && s->precision)
	{
		s->precision--;
		
		fraction *= 10;
		*fraction_ptr++ = (fraction >> frac_bits) + '0';
		fraction &= mask;
	}
	
	const uint32_t integer_digits = integer_str + sizeof(integer_str) - integer_ptr;		//total number of significant digits (without the sign)
	const uint32_t fraction_digits = fraction_ptr - fraction_str;

	const uint32_t total_digits = integer_digits + 
									fraction_digits + ((fraction_digits) ? 1 : 0);		//if we have fraction digits we need one space for '.'

	const uint32_t width_total = max(s->width, total_digits);
	uint32_t width_padding = width_total - total_digits;
	
	if (s->flags & FLAG_MINUS)
	{
		//left alignment
		if (sign)
			if (fputc(sign, f) < 0)
				return -1;
	
		if (fwrite(integer_ptr, integer_digits, 1, f) != 1)
			return -1;

		if (fraction_digits)
		{
			if (fputc('.', f) < 0)
				return -1;

			if (fwrite(fraction_str, fraction_digits, 1, f) != 1)
				return -1;
		}

		while(width_padding--)
			if (fputc(' ', f) < 0)
				return -1;
	}
	else
	{
		const char pad_char = (s->flags & FLAG_ZERO) ? '0' : ' ';
		
		//right alignment
		if ((s->flags & FLAG_ZERO) && sign)
			if (fputc(sign, f) < 0)
				return -1;
		
		while(width_padding--)
			if (fputc(pad_char, f) < 0)
				return -1;
		
		if (!(s->flags & FLAG_ZERO) && sign)
			if (fputc(sign, f) < 0)
				return -1;
				
		if (fwrite(integer_ptr, integer_digits, 1, f) != 1)
			return -1;

		if (fraction_digits)
		{
			if (fputc('.', f) < 0)
				return -1;

			if (fwrite(fraction_str, fraction_digits, 1, f) != 1)
				return -1;
		}
	}

	return width_total;
}

static int fsh_r(FILE *f, va_list *arg, format_specifier_t *s)
{
	return fsh_ufxp_com(f, arg, s, 1, 0);
}

static int fsh_R(FILE *f, va_list *arg, format_specifier_t *s)
{
	return fsh_ufxp_com(f, arg, s, 0, 0);
}

static int fsh_q(FILE *f, va_list *arg, format_specifier_t *s)
{
	return fsh_ufxp_com(f, arg, s, 1, 1);
}

static int fsh_Q(FILE *f, va_list *arg, format_specifier_t *s)
{
	return fsh_ufxp_com(f, arg, s, 0, 1);
}
*/


static char l_fs_chars[16] = "duxXocs";
static rtformat_specifier_handler_t l_fs_handlers[sizeof(l_fs_chars)] = {fsh_d, fsh_u, fsh_x, fsh_x_cap, fsh_o, fsh_c, fsh_s};


static inline const char* parse_format_specifier(const char *str, rtformat_specifier_t *s)
{
	const char *ptr = str + 1;
	
	//parse flag
	s->flags = 0;

	char *pos;
	while(*ptr && (pos = strchr(l_flags, *ptr)))
	{
		s->flags |= (1 << (pos - l_flags));
		ptr++;
	}
	
	//parse width
	s->width = NOT_SPECIFIED;			//default
	
	if (*ptr == 0)
		return str;
	else if (*ptr == '*')
	{
		s->width = SPECIFIED_AS_ARG;
		ptr++;
	}
	else if (strchr("123456789", *ptr))
	{
		char *end;
		s->width = strtoul(ptr, &end, 10);
		ptr = end;
	}

		
	//parse precision
	s->precision = NOT_SPECIFIED;		//default
		
	if (*ptr == '.')
	{
		ptr++;
		
		if (*ptr == 0)
			return str;
		else if (*ptr == '*')
		{
			s->precision = SPECIFIED_AS_ARG;
			ptr++;
		}
		else if (strchr("0123456789", *ptr))
		{
			char *end;
			s->precision = strtoul(ptr, &end, 10);
			ptr = end;
		}
		else
			return str;
	}
	

	//parse length
	s->length = 0;
	
	if (*ptr == 0)
		return str;
	else if (strchr(l_length_chars, *ptr))
		s->length = *ptr++;

	//parse type
	const char *c;
	
	if (*ptr == 0)
		return str;
	else if ((c = strchr(l_fs_chars, *ptr)))
	{
		s->specifier_index = c - l_fs_chars;
		ptr++;
	}
	else
		return str;

	return ptr;
}



int32_t rtprintf(rtprintf_output_handler_t output_handler, void *user, const char *format, va_list args)
{
	int32_t rv = 0;
	rtformat_specifier_t spec;

	while(*format)
	{
		ptrdiff_t offset = 0;
		const char *specifier = strchr(format + offset, '%');
		
		if (specifier)		//% char found
		{
			//print the string up to the % char
			const size_t output_size = specifier - format;
			if (output_size)
				OUTPUT_CHARS(user, format, output_size);

			
			const char *new_pos = parse_format_specifier(specifier, &spec);
			
			if (new_pos != specifier)
			{
				//we have a valid specifier
			 	const int32_t rv_temp = l_fs_handlers[spec.specifier_index](output_handler, user, &args, &spec);
			 	if (rv_temp < 0)
			 		return rv_temp;
		 		else
				 	rv += rv_temp;
				 	
			 	format = new_pos;			
			}
			else
			{
				//invalid specifier so print it
				format = specifier + 1;

				if (*format)
				{
					OUTPUT_CHARS(user, format, 1);
					format++;
				}
			}
		}
		else				
		{
			//no % char found
			//print the remainder of the string
			const char* str = format;
			while(*format)
				format++;

			OUTPUT_CHARS(user, str, format - str);
		}
	}

	return rv;
}



int32_t rtprintf_add_format(char specifier, rtformat_specifier_handler_t handler)
{
	const size_t i = strlen(l_fs_chars);
	
	if (i >= (sizeof(l_fs_chars) - 1))
		return -1;

	l_fs_chars[i] = specifier;
	l_fs_chars[i + 1] = 0;	
	l_fs_handlers[i] = handler;

	return 0;
}


void rtprintf_include_float(void)
{
	assert(rtprintf_add_format('f', fsh_float) == 0);
}