#include "EMsprintf.h"

static char *str_swap(char *str, int size);

/** \brief A light-weight version of sprintf()
 *
 * \param char *str -> the destination buffer
 * \param const char *format -> the string format
 * \param arguments ...
 * \return nothing
 *
 *  A light-weight version of sprintf() targeted toward micro-controller
 *  Only supports %s and %d
 */

void EMsprintf(char *str,const char *format, ...)
{
    va_list arg_p;
    char *pointer = NULL, *p_str_ref = str;
    const char *p_format_ref = NULL;
    int arg_int = 0;

    va_start(arg_p,format);
                                //will stop once it's NULL
                                //  |
                                //  V
    for(p_format_ref = format; *p_format_ref; p_format_ref++)
    {
       if(*p_format_ref != '%')
       {
            *p_str_ref++ = *p_format_ref;
            continue;
       }

       switch(*(++p_format_ref))
       {
       case 's':
           //get the arg as a string
           for(pointer = va_arg(arg_p, char *); *pointer; pointer++)
                *p_str_ref++ = *pointer;
            break;
       case 'd':
           //get the arg as an int
           arg_int = va_arg(arg_p, int);

           if(arg_int < 0)
           {
               *p_str_ref++ = '-';arg_int = -arg_int;
           }
		   {
				int temp = 0;
				char *p_start = p_str_ref, *p_end = NULL;
				if(arg_int)
                {
                    for(;arg_int;)
                    {
                        temp = (arg_int % 10);
                        *p_str_ref++ = temp + ('1'-1);
                        arg_int = (arg_int-temp) == 0 ? 0 : (arg_int-temp)/10;
                    }

                    p_end = p_str_ref-1;
                    int length = p_end-p_start;
                    p_start = str_swap(p_start, length);
                }
                else
                    *p_str_ref++ = '0';
		   }
            break;
       default:
           *p_str_ref++ = *p_format_ref;
            break;
       }
    }

    va_end(arg_p);

    *p_str_ref = '\0';
}

static char *str_swap(char *str, int size)
{
    int left_pos = 0, right_pos = 0;
    char c_temp, *p_start = str, *p_end = str + size;

    for(;left_pos + right_pos < size;left_pos++,right_pos++)
    {
        c_temp = *(p_start+left_pos);
        *(p_start+left_pos) = *(p_end-right_pos);
        *(p_end-right_pos) = c_temp;
    }

    return str;
}