

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <sys/stat.h>
#include <stm32f10x.h>
#include <string.h>

static bool Putch(uint8_t c);
   
inline void DelayLoop(uint32_t delayLoops)
{
    __asm__ __volatile__
    (
    "1: \n"
    " CBZ %[delayLoops], 2f \n"
    " SUB %[delayLoops], %[delayLoops], #1 \n"
    " B 1b \n"
    "2: \n"
    : [delayLoops] "+r"(delayLoops)
    );
}

extern "C" int _close(int file) { return -1; }

extern "C" int _fstat(int file, struct stat *st) 
{
	st->st_mode = S_IFCHR;
	return 0;
}

extern "C" int _isatty(int file) { return 1; }

extern "C" int _lseek(int file, int ptr, int dir) { return 0; }

extern "C" int _open(const char *name, int flags, int mode) { return -1; }

extern "C" int _read(int file, char *ptr, int len) {return 0;}

char *heap_end = 0;
extern "C" caddr_t _sbrk(int incr) 
{
	extern char __heap_start__; /* Defined by the linker */
	extern char __heap_end__; /* Defined by the linker */
	char *prev_heap_end;

	if (heap_end == 0) 
	{
		heap_end = &__heap_start__;
	}
	prev_heap_end = heap_end;
	if (heap_end + incr > &__heap_end__) 
	{
		/* Heap and stack collision */
		return (caddr_t)0;
	}
	heap_end += incr;
	return (caddr_t) prev_heap_end;
}

extern "C" int _write(int file, char *ptr, int len) 
{
	for (int i = 0; i < len; i++) 
	{
		Putch(ptr[i]);
	}
	return len;
}

static bool Putch(uint8_t c)
{
    if(c == '\n')
        Putch('\r');
    while(( USART1->SR & USART_SR_TXE) == 0);
    USART1->DR = c;
    return true;
}

static void UsartInit(unsigned baud, unsigned flags)
{
    unsigned brr = 8000000 / baud;
    USART1->BRR = brr;
    USART1->CR1 = (flags & 0xffff) | USART_CR1_UE;
    USART1->CR2 = (flags >> 16) & 0xffff;
}

enum Configuration
{
    AnalogIn = 0,
    In = 0x04,
    PullUpOrDownIn = 0x08,
    Out = 0x03, // default Out is Out50Mhz
    Out10Mhz = 0x01,
    Out2Mhz = 0x02,
    Out50Mhz = 0x03,
    OpenDrainOut = 0x07,
    OpenDrainOut10Mhz = 0x05,
    OpenDrainOut2Mhz = 0x06,
    OpenDrainOut50Mhz = 0x07,
    AltOut = 0x0B,
    AltOut10Mhz = 0x09,
    AltOut2Mhz = 0x0A,
    AltOut50Mhz = 0x0B,
    AltOpenDrain = 0x0f,
    AltOpenDrain10Mhz = 0x0C,
    AltOpenDrain2Mhz = 0x0E,
    AltOpenDrain50Mhz = 0x0f
};

enum UsartMode
{
    DataBits8 = 0,
    DataBits9 = USART_CR1_M,

    NoneParity = 0,
    EvenParity = USART_CR1_PCE,
    OddParity  = USART_CR1_PS | USART_CR1_PCE,

    NoClock = 0,
    
    Disabled = 0,
    RxEnable = USART_CR1_RE,
    TxEnable = USART_CR1_TE,
    RxTxEnable  = USART_CR1_RE | USART_CR1_TE,
    Default = RxTxEnable,
    
    OneStopBit         = 0,
    HalfStopBit        = USART_CR2_STOP_0 << 16,
    TwoStopBits        = USART_CR2_STOP_1 << 16,
    OneAndHalfStopBits = ((USART_CR2_STOP_0 << 16) | (USART_CR2_STOP_1 << 16))
};

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 uint32_t pow2Table16[]=
{
    0x0001,
    0x0002,
    0x0004,
    0x0008,
    0x0016,
    0x0032,
    0x0064,
    0x0128,
    0x0256,
    0x0512,
    0x1024,
    0x2048,
    0x4096,
    0x8192,
    0,//0x00016384,
    0//0x00032768
};


const 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 (pow2Table16[i]);}

template<>
inline uint32_t Pow2Table<uint32_t>(int i){return (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 uint32_t  pow10Table32[]=
{
    1000000000ul,
    100000000ul,
    10000000ul,
    1000000ul,
    100000ul,
    10000ul,
    1000ul,
    100ul,
    10ul,
    1ul
};

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

template<class T>
inline T Pow10Table(int i){return (pow10Table16[i]);}
template<>
inline uint32_t Pow10Table<uint32_t>(int i){return (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;
	while(value < Pow10Table<T>(i)) i++;
    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 t = ((value * 5764607523ull) >> 32) +1;
    for(uint8_t i=0; i < 9; i++)
    {
        *ptr++ = (t >> 27) + '0';
        t = (t & 0x07ffffff) * 10;
    }
    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;
}

// start timer1 with divider 1
inline void start_timer()
{
	RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
	TIM1->CNT = 0;
	TIM1->CR1 = TIM_CR1_CEN;
}

uint16_t inline stop_timer()
{
	TIM1->CR1 = 0;
	return TIM1->CNT;
}

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

test_item functions[]=
{
	{sprintf_wrapper, "sprintf", 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
};

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

char test_buffer[15]={0};

int main()
{
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN | RCC_APB2ENR_IOPCEN | RCC_APB2ENR_USART1EN;
    // Configure PA9 as alternative output for usart
    GPIOA->CRH = (AltOut << 1*4); 
    // Pc8 and Pc9 as output
    GPIOC->CRH = (Out << 4*0) | (Out << 4*1) ; 
    UsartInit(9600, RxTxEnable | DataBits8 | TwoStopBits);
	
	for(unsigned j = 0; j < sizeof(functions)/sizeof(functions[0]); j++)
	{
		test_item &item = functions[j];
		printf("%s\n", 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;
			printf("result = %10s\ttime = %u\n", result, cycles);
		}
		printf("Average '%s' cycles per convertion: %lu\n", item.name, item.total_cycles / tests_count);
	}

    while(1)
    {
       GPIOC->ODR ^= (1 << 8) | (1 << 9);
       DelayLoop(100000);
    }
}

