/****************************************************************************

Copyright(c) 2019 by Aerospace C.Power (Chongqing) Microelectronics. ALL RIGHTS RESERVED.

This Information is proprietary to Aerospace C.Power (Chongqing) Microelectronics and MAY NOT
be copied by any method or incorporated into another program without
the express written consent of Aerospace C.Power. This Information or any portion
thereof remains the property of Aerospace C.Power. The Information contained herein
is believed to be accurate and Aerospace C.Power assumes no responsibility or
liability for its use in any way and conveys no license or title under
any patent or copyright and makes no representation or warranty that this
Information is free from patent or copyright infringement.

****************************************************************************/
/* os shim includes */
#include "os_types.h"
#include "os_mem.h"
#include "strformat.h"

#define HAVE_DOUBLE

#define HAVE_LONGLONG
#ifndef LARGEST_SIGNED
#ifdef HAVE_LONGLONG
#define LARGEST_SIGNED long
#else
#define LARGEST_UNSIGNED int
#endif
#endif

#ifndef LARGEST_UNSIGNED
#ifdef HAVE_LONGLONG
#define LARGEST_UNSIGNED unsigned long
#else
#define LARGEST_UNSIGNED unsigned int
#endif
#endif

#ifndef POINTER_INT
#define POINTER_INT unsigned long
#endif

typedef unsigned int FormatFlags;

#define MAKE_MASK(shift,size) (((1 << size) - 1) << (shift))

#define JUSTIFY_SHIFT    0
#define JUSTIFY_SIZE     1
#define JUSTIFY_RIGHT    0x0000
#define JUSTIFY_LEFT     0x0001
#define JUSTIFY_MASK     MAKE_MASK(JUSTIFY_SHIFT,JUSTIFY_SIZE)


/* How a positive number is prefixed */
#define POSITIVE_SHIFT   (JUSTIFY_SHIFT + JUSTIFY_SIZE)
#define POSITIVE_NONE    (0x0000 << POSITIVE_SHIFT)
#define POSITIVE_SPACE   (0x0001 << POSITIVE_SHIFT)
#define POSITIVE_PLUS    (0x0003 << POSITIVE_SHIFT)
#define POSITIVE_MASK    MAKE_MASK(POSITIVE_SHIFT, POSITIVE_SIZE)

#define POSITIVE_SIZE    2

#define ALTERNATE_FORM_SHIFT (POSITIVE_SHIFT + POSITIVE_SIZE)
#define ALTERNATE_FORM_SIZE 1
#define ALTERNATE_FORM  (0x0001 << ALTERNATE_FORM_SHIFT)


#define PAD_SHIFT    (ALTERNATE_FORM_SHIFT + ALTERNATE_FORM_SIZE)
#define PAD_SIZE     1
#define PAD_SPACE    (0x0000 << PAD_SHIFT)
#define PAD_ZERO     (0x0001 << PAD_SHIFT)

#define SIZE_SHIFT    (PAD_SHIFT + PAD_SIZE)
#define SIZE_SIZE     3
#define SIZE_CHAR     (0x0001 << SIZE_SHIFT)
#define SIZE_SHORT    (0x0002 << SIZE_SHIFT)
#define SIZE_INT      (0x0000 << SIZE_SHIFT)
#define SIZE_LONG     (0x0003 << SIZE_SHIFT)
#define SIZE_LONGLONG (0x0004 << SIZE_SHIFT)
#define SIZE_MASK    MAKE_MASK(SIZE_SHIFT,SIZE_SIZE)

#define CONV_SHIFT    (SIZE_SHIFT + SIZE_SIZE)
#define CONV_SIZE      3
#define CONV_INTEGER  (0x0001 << CONV_SHIFT)
#define CONV_FLOAT    (0x0002 << CONV_SHIFT)
#define CONV_POINTER  (0x0003 << CONV_SHIFT)
#define CONV_STRING   (0x0004 << CONV_SHIFT)
#define CONV_CHAR     (0x0005 << CONV_SHIFT)
#define CONV_PERCENT  (0x0006 << CONV_SHIFT)
#define CONV_WRITTEN  (0x0007 << CONV_SHIFT)
#define CONV_MASK     MAKE_MASK(CONV_SHIFT, CONV_SIZE)

#define RADIX_SHIFT    (CONV_SHIFT + CONV_SIZE)
#define RADIX_SIZE     2
#define RADIX_DECIMAL  (0x0001 << RADIX_SHIFT)
#define RADIX_OCTAL    (0x0002 << RADIX_SHIFT)
#define RADIX_HEX      (0x0003 << RADIX_SHIFT)
#define RADIX_MASK     MAKE_MASK(RADIX_SHIFT,RADIX_SIZE)

#define SIGNED_SHIFT   (RADIX_SHIFT + RADIX_SIZE)
#define SIGNED_SIZE    1
#define SIGNED_NO      (0x0000 << SIGNED_SHIFT)
#define SIGNED_YES     (0x0001 << SIGNED_SHIFT)
#define SIGNED_MASK    MAKE_MASK(SIGNED_SHIFT,SIGNED_SIZE)

#define CAPS_SHIFT     (SIGNED_SHIFT + SIGNED_SIZE)
#define CAPS_SIZE      1
#define CAPS_NO        (0x0000 << CAPS_SHIFT)
#define CAPS_YES       (0x0001 << CAPS_SHIFT)
#define CAPS_MASK      MAKE_MASK(CAPS_SHIFT,CAPS_SIZE)

#define FLOAT_SHIFT    (CAPS_SHIFT + CAPS_SIZE)
#define FLOAT_SIZE     2
#define FLOAT_NORMAL   (0x0000 << FLOAT_SHIFT)
#define FLOAT_EXPONENT  (0x0001 << FLOAT_SHIFT)
#define FLOAT_DEPENDANT (0x0002 << FLOAT_SHIFT)
#define FLOAT_HEX       (0x0003 << FLOAT_SHIFT)
#define FLOAT_MASK      MAKE_MASK(FLOAT_SHIFT, FLOAT_SIZE)

static FormatFlags
parse_flags(const char **posp)
{
  FormatFlags flags = 0;
  const char *pos = *posp;
  while (1) {
    switch(*pos) {
    case '-':
      flags |= JUSTIFY_LEFT;
      break;
    case '+':
      flags |= POSITIVE_PLUS;
      break;
    case ' ':
      flags |= POSITIVE_SPACE;
      break;
    case '#':
      flags |= ALTERNATE_FORM;
      break;
    case '0':
      flags |= PAD_ZERO;
      break;
    default:
      *posp = pos;
      return flags;
    }
    pos++;
  }

}

static unsigned int
parse_uint(const char **posp)
{
  unsigned v = 0;
  const char *pos = *posp;
  char ch;
  while((ch = *pos) >= '0' && ch <= '9') {
    v = v * 10 + (ch - '0');
    pos++;
  }
  *posp = pos;
  return v;
}

#define MAXCHARS_HEX ((sizeof(LARGEST_UNSIGNED) * 8) / 4 )

/* Largest number of characters needed for converting an unsigned integer.
 */
#define MAXCHARS ((sizeof(LARGEST_UNSIGNED) * 8  + 2) / 3 )

static unsigned int
output_uint_decimal(char **posp, LARGEST_UNSIGNED v)
{
  unsigned int len;
  char *pos = *posp;
  while (v > 0) {
    *--pos = (v % 10) + '0';
    v /= 10;
  }
  len = *posp - pos;
  *posp = pos;
  return len;
}

static unsigned int
output_uint_hex(char **posp, LARGEST_UNSIGNED v, unsigned int flags)
{
  unsigned int len;
  const char *hex = (flags & CAPS_YES) ?"0123456789ABCDEF":"0123456789abcdef";
  char *pos = *posp;
  while (v > 0) {
    *--pos = hex[(v % 16)];
    v /= 16;
  }
  len = *posp - pos;
  *posp = pos;
  return len;
}

static unsigned int
output_uint_octal(char **posp, LARGEST_UNSIGNED v)
{
  unsigned int len;
  char *pos = *posp;
  while (v > 0) {
    *--pos = (v % 8) + '0';
    v /= 8;
  }
  len = *posp - pos;
  *posp = pos;
  return len;
}

static StrFormatResult fill_space(const str_format_context *ctxt, unsigned int len)
{
  StrFormatResult res;
  static const char buffer[17] = "                ";
  while(len > 16) {
    res = ctxt->write_str(ctxt->user_data, buffer, 16);
    if (res != STRFORMAT_OK) return res;
    len -= 16;
  }
  if (len == 0) return STRFORMAT_OK;
  return ctxt->write_str(ctxt->user_data, buffer, len);
}

static StrFormatResult fill_zero(const str_format_context *ctxt, unsigned int len)
{
  StrFormatResult res;
  static const char buffer[17] = "0000000000000000";
  while(len > 16) {
    res = ctxt->write_str(ctxt->user_data, buffer, 16);
    if (res != STRFORMAT_OK) return res;
    len -= 16;
  }
  if (len == 0) return STRFORMAT_OK;
  return ctxt->write_str(ctxt->user_data, buffer, len);
}

#define CHECKCB(res) {if ((res) != STRFORMAT_OK) {va_end(ap); return -1;}}

int format_str(const str_format_context *ctxt, const char *format, ...)
{
  int ret;
  va_list ap;
  va_start(ap, format);
  ret = format_str_v(ctxt, format, ap);
  va_end(ap);
  return ret;
}

int format_str_v(const str_format_context *ctxt, const char *format, va_list ap)
{
  unsigned int written = 0;
  const char *pos = format;
  va_list head = ap;
  while(*pos != '\0') {
    FormatFlags flags;
    unsigned int minwidth = 0;
    int precision = -1; /* Negative means no precision */
    char ch;
    const char *start = pos;
    while( (ch = *pos) != '\0' && ch != '%') pos++;
    if (pos != start) {
      CHECKCB(ctxt->write_str(ctxt->user_data, start, pos - start));
      written += pos - start;
    }
    if (*pos == '\0') {
      va_end(ap);
      return written;
    }
    pos++;
    if (*pos == '\0') {
      va_end(ap);
      return written;
    }
    flags = parse_flags(&pos);

    /* parse width */
    if (*pos >= '1' && *pos <= '9') {
      minwidth = parse_uint(&pos);
    } else if (*pos == '*') {
      int w = va_arg(ap,int);
      if (w < 0) {
         flags |= JUSTIFY_LEFT;
         minwidth = w;
      } else {
         minwidth = w;
      }
      pos ++;
    }

    /* parse precision */
    if (*pos == '.') {
      pos++;
      if (*pos >= '0' && *pos <= '9') {
          precision = parse_uint(&pos);
      } else if (*pos == '*') {
          pos++;
          precision = va_arg(ap,int);
      }
    }
    if (*pos == 'l') {
        pos++;
        if (*pos == 'l') {
            flags |= SIZE_LONGLONG;
            pos++;
        } else {
           flags |= SIZE_LONG;
        }
    } else if (*pos == 'h') {
        pos++;
        if (*pos == 'h') {
            flags |= SIZE_CHAR;
            pos++;
        } else {
            flags |= SIZE_SHORT;
        }
    }

    /* parse conversion specifier */
    switch(*pos) {
    case 'd':
    case 'i':
      flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_YES;
      break;
    case 'u':
      flags |= CONV_INTEGER | RADIX_DECIMAL | SIGNED_NO;
      break;
    case 'o':
      flags |= CONV_INTEGER | RADIX_OCTAL | SIGNED_NO;
      break;
    case 'x':
      flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO;
      break;
    case 'X':
      flags |= CONV_INTEGER | RADIX_HEX | SIGNED_NO | CAPS_YES;
      break;
#ifdef HAVE_DOUBLE
    case 'f':
      flags |= CONV_FLOAT | FLOAT_NORMAL;
      break;
    case 'F':
      flags |= CONV_FLOAT | FLOAT_NORMAL | CAPS_YES;
      break;
    case 'e':
      flags |= CONV_FLOAT | FLOAT_EXPONENT;
      break;
    case 'E':
      flags |= CONV_FLOAT | FLOAT_EXPONENT | CAPS_YES;
      break;
    case 'g':
      flags |= CONV_FLOAT | FLOAT_DEPENDANT;
      break;
    case 'G':
      flags |= CONV_FLOAT | FLOAT_DEPENDANT | CAPS_YES;
      break;
    case 'a':
      flags |= CONV_FLOAT | FLOAT_HEX;
      break;
    case 'A':
      flags |= CONV_FLOAT | FLOAT_HEX | CAPS_YES;
      break;
#endif
    case 'c':
      flags |= CONV_CHAR;
      break;
    case 's':
      flags |= CONV_STRING;
      break;
    case 'p':
      flags |= CONV_POINTER;
      break;
    case 'n':
      flags |= CONV_WRITTEN;
      break;
    case '%':
      flags |= CONV_PERCENT;
      break;
    case '\0':
      va_end(ap);
      return written;
    }
    pos++;
    switch(flags & CONV_MASK) {
        case CONV_PERCENT:
          CHECKCB(ctxt->write_str(ctxt->user_data, "%", 1));
          written++;
          break;
        case CONV_INTEGER:
        {
            /* unsigned integers */
            char *prefix = 0; /* sign, "0x" or "0X" */
            unsigned int prefix_len = 0;
            char buffer[MAXCHARS];
            char *conv_pos = buffer + MAXCHARS;
            unsigned int conv_len = 0;
            unsigned int width = 0;
            unsigned int precision_fill;
            unsigned int field_fill;
            LARGEST_UNSIGNED uvalue = 0;
            int negative = 0;

            if (precision < 0) precision = 1;
            else flags &= ~PAD_ZERO;

            if (flags & SIGNED_YES) {
              /* signed integers */
              LARGEST_SIGNED value = 0;
              switch(flags & SIZE_MASK) {
              case SIZE_CHAR:
                value = (signed char)va_arg(ap, int);
                break;
              case SIZE_SHORT:
                value = (short)va_arg(ap, int);
                break;
              case SIZE_INT:
                value = va_arg(ap, int);
                break;
#ifndef HAVE_LONGLONG
              case SIZE_LONGLONG:    /* Treat long long the same as long */
#endif
              case SIZE_LONG:
                value = va_arg(ap, long);
                break;
#ifdef HAVE_LONGLONG
              case SIZE_LONGLONG:
                value = (LARGEST_SIGNED)va_arg(ap, long long);
                break;
#endif
              }
              if (value < 0) {
                uvalue = -value;
                negative = 1;
              } else {
                uvalue = value;
              }
            } else {

              switch(flags & SIZE_MASK) {
              case SIZE_CHAR:
                uvalue = (unsigned char)va_arg(ap,unsigned int);
                break;
              case SIZE_SHORT:
                uvalue = (unsigned short)va_arg(ap,unsigned int);
                break;
              case SIZE_INT:
                uvalue = va_arg(ap,unsigned int);
                break;
#ifndef HAVE_LONGLONG
              case SIZE_LONGLONG:    /* Treat long long the same as long */
#endif
              case SIZE_LONG:
                uvalue = va_arg(ap,unsigned long);
                break;
#ifdef HAVE_LONGLONG
              case SIZE_LONGLONG:
                uvalue = (LARGEST_UNSIGNED)va_arg(ap,unsigned long long);
                break;
#endif
              }
            }

            switch(flags & (RADIX_MASK)) {
            case RADIX_DECIMAL:
              conv_len = output_uint_decimal(&conv_pos,uvalue);
              break;
            case RADIX_OCTAL:
              conv_len = output_uint_octal(&conv_pos,uvalue);
              break;
            case RADIX_HEX:
              conv_len = output_uint_hex(&conv_pos,uvalue, flags);
              break;
            }

            width += conv_len;
            precision_fill = ((unsigned int)precision > conv_len) ?
                precision - conv_len : 0;
            if ((flags & (RADIX_MASK | ALTERNATE_FORM))
                == (RADIX_OCTAL | ALTERNATE_FORM)) {
              if (precision_fill < 1) precision_fill = 1;
            }

            width += precision_fill;

            if ((flags & (RADIX_MASK | ALTERNATE_FORM))
                == (RADIX_HEX | ALTERNATE_FORM) && uvalue != 0) {
              prefix_len = 2;
              if (flags & CAPS_YES) {
                prefix = "0X";
              } else {
                prefix = "0x";
              }
            }

            if (flags & SIGNED_YES) {
              if (negative) {
                prefix = "-";
                prefix_len = 1;
              } else {
                switch(flags & POSITIVE_MASK) {
                case POSITIVE_SPACE:
                  prefix = " ";
                  prefix_len = 1;
                  break;
                case POSITIVE_PLUS:
                  prefix = "+";
                  prefix_len = 1;
                  break;
                }
              }
            }

            width += prefix_len;

            field_fill = (minwidth > width) ? minwidth - width : 0;

            if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
              if (flags & PAD_ZERO) {
                precision_fill += field_fill;
                field_fill = 0;  /* Do not double count padding */
              } else {
                CHECKCB(fill_space(ctxt,field_fill));
              }
            }

            if (prefix_len > 0)
              CHECKCB(ctxt->write_str(ctxt->user_data, prefix, prefix_len));
            written += prefix_len;

            CHECKCB(fill_zero(ctxt,precision_fill));
            written += precision_fill;

            CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len));
            written += conv_len;

            if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
              CHECKCB(fill_space(ctxt,field_fill));
            }
            written += field_fill;
        }
        break;
        case CONV_STRING:
        {
            unsigned int field_fill;
            unsigned int len;
            char *str = va_arg(ap,char *);
            if (str) {
              char *tmp_pos = str;
              while(*tmp_pos != '\0') tmp_pos++;
              len = tmp_pos - str;
            } else {
              str = "(null)";
              len = 6;
            }
            if (precision >= 0 && (unsigned int)precision < len) len = precision;
            field_fill = (minwidth > len) ? minwidth - len : 0;
            if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
              CHECKCB(fill_space(ctxt,field_fill));
            }
            CHECKCB(ctxt->write_str(ctxt->user_data, str,len));
            written += len;
            if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
              CHECKCB(fill_space(ctxt,field_fill));
            }
            written += field_fill;
         }
        break;
        case CONV_POINTER:
        {
            LARGEST_UNSIGNED uvalue =
              (LARGEST_UNSIGNED)(POINTER_INT)va_arg(ap,void *);
            char buffer[MAXCHARS_HEX + 3];
            char *conv_pos = buffer + MAXCHARS_HEX+3;
            unsigned int conv_len;
            unsigned int field_fill;

            conv_len = output_uint_hex(&conv_pos,uvalue,flags);
            if (conv_len == 0) {
              *--conv_pos = '0';
              conv_len++;
            }
            *--conv_pos = 'x';
            *--conv_pos = '0';
            *--conv_pos = '#';
            conv_len += 3;

            field_fill = (minwidth > conv_len) ? minwidth - conv_len : 0;

            if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
              CHECKCB(fill_space(ctxt,field_fill));
            }

            CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len));
            written += conv_len;

            if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
              CHECKCB(fill_space(ctxt,field_fill));
            }
            written += field_fill;
        }
        break;
        case CONV_CHAR:
        {
            char tmp_ch = (char)va_arg(ap, int);
            unsigned int field_fill = (minwidth > 1) ? minwidth - 1 : 0;
            if ((flags & JUSTIFY_MASK) == JUSTIFY_RIGHT) {
              CHECKCB(fill_space(ctxt,field_fill));
              written += field_fill;
            }

            CHECKCB(ctxt->write_str(ctxt->user_data, &tmp_ch, 1));
            written++;

            if ((flags & JUSTIFY_MASK) == JUSTIFY_LEFT) {
              CHECKCB(fill_space(ctxt,field_fill));
            }
            written+= field_fill;
        }
        break;
        case CONV_WRITTEN:
        {
            int *p = va_arg(ap,int*);
            *p = written;
        }
        break;
        case CONV_FLOAT:
        {
            char buffer[MAXCHARS];
            char *conv_pos = buffer + MAXCHARS;
            unsigned int conv_len = 0;
            double fval ;
            if ((ap - head + sizeof(int)) & sizeof(int))
                ap += sizeof(int);
            ap += sizeof(double);
            fval = *(double *)(ap - sizeof(double));
            int Inte = (int)fval;
            int Deci = (int)(100000*(fval - Inte));
            int Remain = 0;
            int Zeronum = 4;
            if(Deci % 10 >= 5)  {
                Deci = Deci / 10 + 1;
            }  else  {
                Deci = Deci / 10;
            }

            if(Inte < 0 ) {
                Inte = Inte *(-1);
                Deci = Deci *(-1);
                CHECKCB(ctxt->write_str(ctxt->user_data, "-", 1));
                written += 1;
            }
            if(Inte == 0 ){
                if(Deci < 0){
                    Inte = Inte *(-1);
                    Deci = Deci *(-1);
                    CHECKCB(ctxt->write_str(ctxt->user_data, "-", 1));
                    written += 1;
                }
                CHECKCB(ctxt->write_str(ctxt->user_data, "0", 1));
                written += 1;
            } else {
                conv_pos = buffer + MAXCHARS;;
                conv_len = output_uint_decimal(&conv_pos, Inte);
                CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len));
                written += conv_len;
            }
            CHECKCB(ctxt->write_str(ctxt->user_data, ".", 1));
            written += 1;

            Remain = Deci;
            while((Remain > 0)&&(Zeronum > 0)) {
                Zeronum--;
                Remain = Remain / 10;
            }
            while(Zeronum > 0) {
                CHECKCB(ctxt->write_str(ctxt->user_data, "0", 1));
                written += 1;
                Zeronum--;
            }
            if(Deci == 0){
                 CHECKCB(ctxt->write_str(ctxt->user_data, "0", 1));
                 written += 1;
            } else {
                conv_pos = buffer + MAXCHARS;;
                conv_len = output_uint_decimal(&conv_pos, Deci);
                CHECKCB(ctxt->write_str(ctxt->user_data, conv_pos,conv_len));
                written += conv_len;
           }
        }
        break;
        }
  }

  return written;
}