
#include <msp430.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include "softusart.h"

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))
static 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))
static 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))
static 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))
static 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>
static 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))
static 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
    {
        a_fraction >>= 1;
        if(a & 1) a_fraction |= 0x80;
        a >>= 1;
        if(mask & 1)
        {
            t += a;
            t_fraction += a_fraction;
            if(t_fraction < a_fraction) t++;
        }
        mask >>= 1;
    }while(a_fraction || a);

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

static void print(const char *str)
{
    char c;
    while(c = *str++)
        SoftUsart<1, 9600>::Putch(c);
}

static void println(const char *str)
{
    char c;
    while(c = *str++)
        SoftUsart<1, 9600>::Putch(c);
	SoftUsart<1, 9600>::Putch('\r'); 
    SoftUsart<1, 9600>::Putch('\n'); 
}

// start timer1 with divider 1
inline void start_timer()
{
	TAR = 0;
	TACTL = ID_0 | TASSEL_2 | MC_2;
}

inline uint16_t stop_timer()
{
	TACTL = 0;
	return TAR;
}

static void Set_DCO(unsigned int Delta)            // Set DCO to selected frequency
{
  unsigned int Compare, Oldcapture = 0;

  BCSCTL1 |= DIVA_3;                        // ACLK = LFXT1CLK/8
  TACCTL0 = CM_1 + CCIS_1 + CAP;            // CAP, ACLK
  TACTL = TASSEL_2 + MC_2 + TACLR;          // SMCLK, cont-mode, clear

  while (1)
  {
    while (!(CCIFG & TACCTL0));             // Wait until capture occured
    TACCTL0 &= ~CCIFG;                      // Capture occured, clear flag
    Compare = TACCR0;                       // Get current captured SMCLK
    Compare = Compare - Oldcapture;         // SMCLK difference
    Oldcapture = TACCR0;                    // Save current captured SMCLK

    if (Delta == Compare)
      break;                                // If equal, leave "while(1)"
    else if (Delta < Compare)
    {
      DCOCTL--;                             // DCO is too fast, slow it down
      if (DCOCTL == 0xFF)                   // Did DCO roll under?
        if (BCSCTL1 & 0x0f)
          BCSCTL1--;                        // Select lower RSEL
    }
    else
    {
      DCOCTL++;                             // DCO is too slow, speed it up
      if (DCOCTL == 0x00)                   // Did DCO roll over?
        if ((BCSCTL1 & 0x0f) != 0x0f)
          BCSCTL1++;                        // Sel higher RSEL
    }
  }
  TACCTL0 = 0;                              // Stop TACCR0
  TACTL = 0;                                // Stop Timer_A
  BCSCTL1 &= ~DIVA_3;                       // ACLK = LFXT1CLK
}

static void SetUpClock()
{
	P2OUT |= (1 << 6) | (1 << 7);
    P2SEL |= (1 << 6) | (1 << 7);
	Set_DCO(16000000 / 4096);
}

const 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
};

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

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[12]={0};
char output_buffer[12]={0};

#define STR__(x) #x
#define STR(x) STR__(x)

int main()
{
    WDTCTL = WDTPW + WDTHOLD;
    SetUpClock();
    P1DIR |= (1 << 0) | (1 << 6);
    uint32_t total_cycles = 0;
    println(STR(FUNCTION));
    for(int i=0; i < tests_count; i++)
    {
		P1OUT ^= (1 << 0);
        uint32_t u32 = test_values[i];
        start_timer();
        char *result = FUNCTION(u32, test_buffer);
        uint16_t cycles = stop_timer();
        total_cycles += cycles;
        print(result);
        print("\t\t");
        println(utoa_builtin_div(cycles, output_buffer));
    }
    print("Avg cycles 32: ");
    println(utoa_builtin_div(total_cycles / tests_count, output_buffer));
	
	total_cycles = 0;
	for(int i=0; i < tests_count16; i++)
    {
		P1OUT ^= (1 << 0);
        uint16_t u16 = test_values16[i];
        start_timer();
        char *result = FUNCTION(u16, test_buffer);
        uint16_t cycles = stop_timer();
        total_cycles += cycles;
        print(result);
        print("\t\t");
        println(utoa_builtin_div(cycles, output_buffer));
    }
    print("Avg cycles 16: ");
    println(utoa_builtin_div(total_cycles / tests_count16, output_buffer));

	while(1)
	{
		delay_ns<1000000000u, 16000000u>();
		P1OUT ^= (1 << 0) | (1 << 6);
	}
	return 0;
}
