
#include <avr/io.h>
#include <stdlib.h>
#include <stdio.h>
#include <avr/pgmspace.h>

#define special_output_port (*((volatile char *)0x24))
#define special_input_port  (*((volatile char *)0x25))
#define special_abort_port  (*((volatile char *)0x22))
#define special_exitcode_port  (*((volatile char *)0x020))

#define USE_AVR_ASM 1

template<class T>
struct divmod10_t
{
    T quot;
    uint8_t rem;
};

template<class T>
inline static divmod10_t<T> divmodu10(T n)
{
    divmod10_t<T> res;
    res.quot = n >> 1;
    res.quot += res.quot >> 1;
    res.quot += res.quot >> 4;
    if(sizeof(T) > 1)
    res.quot += res.quot >> 8;
    if(sizeof(T) > 2)
    res.quot += res.quot >> 16;
    T qq = res.quot;
    res.quot >>= 3;
    res.rem = uint8_t(n - ((res.quot << 1) + (qq & ~7ul)));
    if(res.rem > 9)
    {
        res.rem -= 10;
        res.quot++;
    }
    return res;
}

template<class T>
__attribute__((noinline))
char * utoa_fast_div(T value, char *buffer)
{
    buffer += 11;
    *buffer = 0;
	divmod10_t<T> res;
	res.quot = value;
    do
    {
        res = divmodu10(res.quot);
        *--buffer = res.rem + '0';
    }
    while (res.quot);
    return buffer;
}


__attribute__((noinline))
char * utoa_divmod(uint32_t value, char *buffer)
{
    buffer += 11;
    *buffer = 0;
    do
    {
        ldiv_t res = ldiv(value, 10);
        *--buffer = res.rem + '0';
        value = res.quot;
    }
    while (value != 0);
    return buffer;
}

__attribute__((noinline))
char * utoa_divmod(uint16_t value, char *buffer)
{
    buffer += 11;
    *buffer = 0;
    do
    {
        div_t res = div(value, 10);
        *--buffer = res.rem + '0';
        value = res.quot;
    }
    while (value != 0);
    return buffer;
}

template<class T>
__attribute__((noinline))
char * utoa_builtin_div(T value, char *buffer)
{
    buffer += 11;
    *buffer = 0;
    do
    {
        *--buffer = value % 10 + '0';
        value /= 10;
    }
    while (value != 0);
    return buffer;
}

template<class T>
static T bcd_add(T a, T b)
{
    T carry = b + 0x66666666ul;
    carry ^= (a + carry) ^ a;
    a += b;
    carry &= 0x11111111ul;
    carry >>= 2;
    a += carry;
    carry >>= 1;
    a += carry;
    return a;
}

template<class T>
char *bcd_to_str(T bcd, char *buffer)
{
    char *ptr = buffer + sizeof(T) * 2 - 1;
    for(uint8_t i = 0; i < sizeof(T) * 2; i++)
    {
        *ptr-- = uint8_t(bcd & 0x0f) + '0';
        bcd >>= 4;
    }
    buffer[sizeof(T) * 2] = 0;
    while(buffer[0] == '0') ++buffer;
    return buffer;
}

const PROGMEM uint32_t pow2Table16[]=
{
    0x0001,
    0x0002,
    0x0004,
    0x0008,
    0x0016,
    0x0032,
    0x0064,
    0x0128,
    0x0256,
    0x0512,
    0x1024,
    0x2048,
    0x4096,
    0x8192,
    0,//0x00016384,
    0//0x00032768
};


const PROGMEM uint32_t pow2Table32[]=
{
    0x00000001,
    0x00000002,
    0x00000004,
    0x00000008,
    0x00000016,
    0x00000032,
    0x00000064,
    0x00000128,
    0x00000256,
    0x00000512,
    0x00001024,
    0x00002048,
    0x00004096,
    0x00008192,
    0x00016384,
    0x00032768,
    0x00065536,
    0x00131072,
    0x00262144,
    0x00524288,
    0x01048576,
    0x02097152,
    0x04194304,
    0x08388608,
    0x16777216,
    0x33554432,
    0x67108864,
    0,//0x134217728
    0,// 0x268435456
    0//0x4294967296
};

template<class T>
inline T Pow2Table(int i){return pgm_read_word(&pow2Table16[i]);}

template<>
inline uint32_t Pow2Table<uint32_t>(int i){return pgm_read_dword(&pow2Table32[i]);}

template<class T>
__attribute__((noinline))
static char *utoa_bcd_table(T value, char *buffer)
{
    T bcd=0;
	uint8_t i=0;
    while(value)
    {
        if(value & 1)
        {
            bcd = bcd_add<T>(bcd, Pow2Table<T>(i));
        }
        value >>= 1;
		i++;
    }
    return bcd_to_str(bcd, buffer);
}

template<class T>
__attribute__((noinline))
static char *utoa_bcd_add(T value, char *buffer)
{
    T bcd=0, pow2 = 1;
    while(value)
    {
        if(value & 1)
        {
            bcd = bcd_add(bcd, pow2);
        }
		pow2 = bcd_add(pow2, pow2);
        value >>= 1;
    }
	return bcd_to_str(bcd, buffer);
}


template<class T>
__attribute__((noinline))
static char *utoa_bcd_horner(T value, char *buffer)
{
    T bcd=0, add;
    for(uint8_t i=0; i < sizeof(T) * 8; i++)
    {
		add = !!(value & ( T(-1) ^ (T(-1)>>1)));
        bcd = bcd_add(add+bcd, bcd);
        //if(value & ( T(-1) ^ (T(-1)>>1) ) )
        //{
        //    bcd = bcd_add(bcd, T(1));
        //}
        value <<= 1;
    }
    return bcd_to_str(bcd, buffer);
}

const PROGMEM uint32_t  pow10Table32[]=
{
    1000000000ul,
    100000000ul,
    10000000ul,
    1000000ul,
    100000ul,
    10000ul,
    1000ul,
    100ul,
    10ul,
    1ul
};

const PROGMEM uint16_t pow10Table16[]=
{
    10000,
    1000,
    100,
    10,
    1
};

template<class T>
inline T Pow10Table(int i){return pgm_read_word(&pow10Table16[i]);}
template<>
inline uint32_t Pow10Table<uint32_t>(int i){return pgm_read_dword(&pow10Table32[i]);}

template<class T>
inline T Pow10TableSize(){return 5;}
template<>
inline uint32_t Pow10TableSize<uint32_t>(){return 10;}

template<class T>
__attribute__((noinline))
static char *utoa_cycle_sub(T value, char *buffer)
{
    if(value == 0)
    {
        buffer[0] = '0';
        buffer[1] = 0;
        return buffer;
    }
    char *ptr = buffer;
    uint8_t i = 0;
    do
    {
        T pow10 = Pow10Table<T>(i++);
        uint8_t count = 0;
        while(value >= pow10)
        {
            count ++;
            value -= pow10;
        }
        *ptr++ = count + '0';
    }while(i < Pow10TableSize<T>());
    *ptr = 0;
    while(buffer[0] == '0') ++buffer;
    return buffer;
}


__attribute__((noinline))
char *utoa_fract(uint32_t value, char *buffer)
{
    char *ptr = buffer;
    *ptr++ = '0';
    if(value == 0)
    {
        buffer[1] = 0;
        return buffer;
    }
    uint8_t correction = 0;
    if(value > 3199999999u)
    {
        correction = 2;
        buffer[0] = '3';
    }
    //uint32_t t = value * 1.34217728;
    // 1.01010111100110001110111000100011
    uint32_t mask = 0b11000100011101110001100111101010;
    uint32_t a = value;
    uint32_t t = value + 1;
    uint8_t a_fraction = 0;
    uint8_t t_fraction = 15;
    do
    {
    #if defined(USE_AVR_ASM) && USE_AVR_ASM
        asm volatile (
            "lsr	%D0	\n"
            "ror	%C0	\n"
            "ror	%B0	\n"
            "ror	%A0 \n"
            :"+r" (a)
        );
        asm volatile (
            "ror	%A0  \n"
            :"+r" (a_fraction)
        );
    #else
        a_fraction >>= 1;
        if(a & 1) a_fraction |= 0x80;
        a >>= 1;
    #endif
        if(mask & 1)
        {
    #if defined(USE_AVR_ASM) && USE_AVR_ASM

            asm volatile (
                "add	%A0, %A1  \n"
                :"+r" (t_fraction) // output
                :"r" (a_fraction)  // input
            );
            asm volatile (
                "adc	%A0, %A1	\n"
                "adc	%B0, %B1	\n"
                "adc	%C0, %C1	\n"
                "adc	%D0, %D1	\n"
                :"+r" (t) // output
                : "r" (a) // input
            );
    #else
            t += a;
            t_fraction += a_fraction;
            if(t_fraction < a_fraction) t++;
    #endif
        }
        mask >>= 1;
    }while(a_fraction || a);

    for(uint8_t i=0; i < 9; i++)
    {
        uint8_t hh = uint8_t(t >> 24);
        *ptr++ = (hh >> 3) + '0';
        t &=0x07ffffff;
		//t *= 10;
        t <<=1;
        t += t<<2;
    }
    ptr[0] = 0;
    buffer[1] += correction;
    while(buffer[1] > '9')
    {
        buffer[1] -= 10;
        buffer[0]++;
    }
    while(buffer[0] == '0') ++buffer;
    return buffer;
}

char *sprintf_wrapper(uint32_t value, char *buffer)
{
	sprintf(buffer, "%lu", value);
	return buffer;
}

char *utoa_wrapper(uint32_t value, char *buffer)
{
	ultoa(value, buffer, 10);
	return buffer;
}

char *sprintf_wrapper(uint16_t value, char *buffer)
{
	sprintf(buffer, "%u", value);
	return buffer;
}

char *utoa_wrapper(uint16_t value, char *buffer)
{
	utoa(value, buffer, 10);
	return buffer;
}

void print(const char *str)
{
    char c;
    while(c = *str++)
        special_output_port = c;
}

void println(const char *str)
{
    char c;
    while(c = *str++)
        special_output_port = c;
    special_output_port = '\n';
}


// start timer1 with divider 1
inline void start_timer()
{
	TCNT1 = 0;
	TCCR1B = 1 << CS10;
}

uint16_t inline stop_timer()
{
	TCCR1B = 0;
	return TCNT1;
}

struct test_item
{
	char* (*function)(uint32_t, char*);
	const char * name;
	uint32_t total_cycles;
};

test_item functions[]=
{
	{sprintf_wrapper, "sprintf", 0},
	{utoa_wrapper,    "utoa", 0},
	{utoa_bcd_table,  "utoa_bcd_table", 0},
	{utoa_bcd_add,    "utoa_bcd_add", 0},
	{utoa_bcd_horner, "utoa_bcd_horner", 0},
	{utoa_fast_div,   "utoa_fast_div", 0},
	{utoa_builtin_div,"utoa_builtin_div", 0},
	{utoa_divmod,     "utoa_divmod", 0},
	{utoa_cycle_sub,  "utoa_cycle_sub", 0},
	{utoa_fract,      "utoa_fract", 0}
};

uint32_t test_values[]=
{
	1, 2, 3, 5, 9, 123, 128, 511, 999, 200, 9999, 
	999999, 12345678, 87654321, 0x7fffffff, 10000000, 0x00010000, 99999999,
	1234, 4321, 0x00010000, 0x00100000, 55555, 5555555, 123456789, 777777777
};


struct test_item16
{
	char* (*function)(uint16_t, char*);
	const char * name;
	uint32_t total_cycles;
};

test_item16 functions16[]=
{
	{sprintf_wrapper, "sprintf", 0},
	{utoa_wrapper,    "utoa", 0},
	{utoa_bcd_table,  "utoa_bcd_table", 0},
	{utoa_bcd_add,    "utoa_bcd_add", 0},
	{utoa_bcd_horner, "utoa_bcd_horner", 0},
	{utoa_fast_div,   "utoa_fast_div", 0},
	{utoa_builtin_div,"utoa_builtin_div", 0},
	{utoa_divmod,     "utoa_divmod", 0},
	{utoa_cycle_sub,  "utoa_cycle_sub", 0},
	//{utoa_fract,      "utoa_fract", 0}
};

uint16_t test_values16[]=
{
	1, 2, 3, 5, 9, 123, 128, 511, 999, 200, 9999, 
	1234, 4321, 0x1000, 0x0100, 55555, 4321, 7777, 48888
};

const int tests_count = sizeof(test_values)/sizeof(test_values[0]);
const int tests_count16 = sizeof(test_values16)/sizeof(test_values16[0]);

char test_buffer[15]={0};
char output_buffer[30]={0};

int main()
{    
	for(int j = 0; j < sizeof(functions)/sizeof(functions[0]); j++)
	{
		test_item &item = functions[j];
		println(item.name);
		for(int i=0; i < tests_count; i++)
		{
			uint32_t u32 = test_values[i];
	   		start_timer();
			char *result = item.function(u32, test_buffer);
			uint16_t cycles = stop_timer();
			item.total_cycles += cycles;
			sprintf(output_buffer, "result = %10s\ttime = %u", result, cycles);
			println(output_buffer);
		}
		sprintf(output_buffer, "Average '%s' cycles per convertion: %lu", item.name, item.total_cycles / tests_count);
		println(output_buffer);
	}

	println("\n\n16 bit verssions");
	for(int j = 0; j < sizeof(functions16)/sizeof(functions16[0]); j++)
	{
		test_item16 &item = functions16[j];
		println(item.name);
		for(int i=0; i < tests_count; i++)
		{
			uint16_t u16 = test_values16[i];
	   		start_timer();
			char *result = item.function(u16, test_buffer);
			uint16_t cycles = stop_timer();
			item.total_cycles += cycles;
			sprintf(output_buffer, "result = %10s\ttime = %u", result, cycles);
			println(output_buffer);
		}
		sprintf(output_buffer, "Average '%s' cycles per convertion: %lu", item.name, item.total_cycles / tests_count);
		println(output_buffer);
	}

    special_exitcode_port = 0;

	return 0;
}
