#include "mcdp.h"

int _ltostr(char *s, int size, unsigned long i, int base, char UpCase) {
  char *tmp;
  int j=0;

  s[--size]=0;
  tmp=s+size;
  if ((base==0)||(base>36)) base=10;
  if (!i) {
    *(--tmp)='0';
    j=1;
  }

  while((tmp>s)&&(i)) {
    tmp--;
    if ((*tmp=i%base+'0')>'9') *tmp+=(UpCase?'A':'a')-'9'-1;
    i=i/base;
    j++;
  }
  memmove(s,tmp,j+1);
  return j;
}

int _vsnprintf (char *str, size_t size, const char *format, va_list arg_ptr) {
  size_t apos,i;
  char ch,buf[1024];
  char *pb;
  char flag_in_sign,flag_upcase;
  char flag_hash,flag_left,flag_space,flag_sign,flag_dot;
  signed char flag_long;
  long number,width,preci=0,bu_len,pad;
  char padwith;

  size--;

  apos=0;
  while (apos<size)
  {
    ch=*format++;
    switch (ch)
    {
    case '%':
      flag_hash=flag_upcase=0;
      flag_left=0;
      flag_space=0;
      flag_sign=0;
      flag_dot=0;
      flag_in_sign=0;
      flag_long=0;

      width=0;
      padwith=' ';

inn_vsnprintf:
if (apos>=size) continue; /* ARGL !!! */

      ch=*format++;
      switch (ch)
      {
/* Format end ?!? */
      case 0:
       return -1;
       break;

/* Format flag chars */
      case '#':
        flag_hash=1;
        goto inn_vsnprintf;

      case 'h':
        --flag_long;
        goto inn_vsnprintf;
      case 'l':
        ++flag_long;
        goto inn_vsnprintf;

      case '0':
        padwith='0';
        goto inn_vsnprintf;

      case '-':
        flag_left=1;
        goto inn_vsnprintf;

      case ' ':
        flag_space=1;
        goto inn_vsnprintf;

      case '+':
        flag_sign=1;
        goto inn_vsnprintf;

      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
      case '8':
      case '9':
        if(flag_dot) return -1;
        width=strtol(--format,&pb,10);
        format=pb;
        goto inn_vsnprintf;

      case '*':
        width=va_arg(arg_ptr,int);
        goto inn_vsnprintf;

      case '.':
        flag_dot=1;
        if (*format=='*') {
          preci=va_arg(arg_ptr,int);
          ++format;
        } else {
          preci=strtol(format,&pb,10);
          format=pb;
        }
        goto inn_vsnprintf;

/* Format conversion chars */
      case 'c':
        ch=(char)va_arg(arg_ptr,int);
      case '%':
        if (str) str[apos]=ch; ++apos;
        break;

      case 's':
        pb=va_arg(arg_ptr,char *);
        bu_len=str_len(pb);
        if (flag_dot && bu_len>preci) bu_len=preci;
        if ((unsigned long)bu_len>size-apos) bu_len=size-apos;

print_out:
        if (str) {
          if (width && (!flag_left))
          {
            for (pad=width-bu_len; pad>0; --pad) str[apos++]=padwith;
          }
          for(i=0;i<(unsigned long)bu_len;++i) { str[apos++]=pb[i]; }
          if (width && (flag_left))
          { /* left pad ALLWAYS with blanks ... */
            for (pad=width-bu_len; pad>0; --pad) str[apos++]=' ';
          }
        } else {
          if (width) {
            apos+=width>bu_len?width:bu_len;
          } else {
            apos+=size>(unsigned long)bu_len?bu_len:size;
          }
        }

        break;

        /* Numbers */
      case 'b':
        i=2;
        goto num_vsnprintf;
      case 'p':
        flag_hash=1;
        width=sizeof(void *)<<1;
        padwith='0';
        ch='x';
      case 'X':
        flag_upcase=(ch=='X');
      case 'x':
        i=16;
        if (flag_hash)
        {
          if (str) {
            str[apos++]='0';
            str[apos++]=ch;
          } else
            apos+=2;
        }
        goto num_vsnprintf;
      case 'd':
      case 'i':
        flag_in_sign=1;
      case 'u':
        i=10;
        goto num_vsnprintf;
      case 'o':
        i=8;
        if (flag_hash) { if (str) str[apos]='0'; ++apos; }

num_vsnprintf:
        if (apos>=size) continue; /* ARGL !!! */

        if (flag_long>0) number=va_arg(arg_ptr,long);
        else number=va_arg(arg_ptr,int);

        if (flag_in_sign && (number<0))
        {
          number=-number;
          flag_in_sign=2;
        }
        if (flag_long<0) number&=0xffff;
        if (flag_long<-1) number&=0xff;
          bu_len=_ltostr(buf+1,sizeof(buf)-1,(unsigned long) number,i,flag_upcase);
        pb=buf+1;

        if (flag_in_sign==2)
        {
          *(--pb)='-';
          bu_len++;
        }
        else if ((flag_in_sign)&&(flag_sign || flag_space))
        {
          *(--pb)=(flag_sign)?'+':' ';
          bu_len++;
        }
        goto print_out;

      default:
        break;
      }
      break;
    case 0:
      if (str) str[apos]=0;
      return apos;
    default:
      if (str) str[apos]=ch; apos++;
      break;
    }
  }
  if (str) str[apos]=0;
  return apos;
}

int _snprintf(char *str,size_t size,const char *format,...) {
  int n;
  va_list arg_ptr;
  va_start(arg_ptr, format);
  n=_vsnprintf(str,size,format,arg_ptr);
  va_end (arg_ptr);
  return n;
}

int _sprintf(char *dest,const char *format,...) {
  int n;
  va_list arg_ptr;
  va_start(arg_ptr, format);
  n=_vsnprintf(dest,1000000,format,arg_ptr);
  va_end (arg_ptr);
  return n;
}

int _printf(const char *format,...) {
  int n;
  char *printf_buf;
  va_list arg_ptr;
  va_start(arg_ptr, format);
  n=_vsnprintf(0,1000000,format,arg_ptr);
  va_end (arg_ptr);
  va_start (arg_ptr, format);
  printf_buf=alloca(n+2);
  n=_vsnprintf(printf_buf,n+1,format,arg_ptr);
  write(1,printf_buf,n);
  va_end (arg_ptr);
  return n;
}
