/*
 * udp_printf.c
 *
 *  Created on: 2022/11/28
 *      Author: T.Uemitsu
 */


#include "udp_cdc.h"
#include <stdint.h>
#include <stdio.h>
#include <stdarg.h>
#include <math.h>

// xprintf from ChaN
// (CC BY 3.0)

#define SZB_OUTPUT  32
#define SZB_PRNT_TXB 1024

#define XF_USE_OUTPUT   1   // 1: Enable output functions
#define XF_CRLF         0   // 1: Convert \n ==> \r\n in the output char
#define XF_USE_LLI      0   // 1: Enable long long integer in size prefix ll
#define XF_USE_FP       1   // 1: Enable support for floating point in type e and f
#define XF_DPC          '.' // Decimal separator for floating point


static int ilog10 (double n) {    // Calculate log10(n) in integer output
    int rv = 0;

    while (n >= 10) {   // Decimate digit in right shift
        if (n >= 100000) {
            n /= 100000; rv += 5;
        } else {
            n /= 10; rv++;
        }
    }
    while (n < 1) {     // Decimate digit in left shift
        if (n < 0.00001) {
            n *= 100000; rv -= 5;
        } else {
            n *= 10; rv--;
        }
    }
    return rv;
}

static double i10x (int n) { // Calculate 10^n
    double rv = 1;

    while (n > 0) {     // Left shift
        if (n >= 5) {
            rv *= 100000; n -= 5;
        } else {
            rv *= 10; n--;
        }
    }
    while (n < 0) {     // Right shift
        if (n <= -5) {
            rv /= 100000; n += 5;
        } else {
            rv /= 10; n++;
        }
    }
    return rv;
}

static void ftoa (
    char* buf,  // Buffer to output the generated string
    double val, // Real number to output
    int prec,   // Number of fractinal digits
    char fmt    // Notation
) {
    int d, e = 0, m = 0;
    char sign = 0;
    double w;
    const char *er = 0;

    if (isnan(val)) {           // Not a number?
        er = "NaN";
    } else {
        if (prec < 0) prec = 6; // Default precision (6 fractional digits)
        if (val < 0) {          // Nagative value?
            val = -val; sign = '-';
        } else {
            sign = '+';
        }
        if (isinf(val)) {       // Infinite?
            er = "INF";
        } else {
            if (fmt == 'f') {   // Decimal notation?
                val += i10x(-prec) / 2; // Round (nearest)
                m = ilog10(val);
                if (m < 0) m = 0;
                if (m + prec + 3 >= SZB_OUTPUT) er = "OV";  // Buffer overflow?
            } else {            // E notation
                if (val != 0) {     // Not a true zero?
                    val += i10x(ilog10(val) - prec) / 2;    // Round (nearest)
                    e = ilog10(val);
                    if (e > 99 || prec + 6 >= SZB_OUTPUT) { // Buffer overflow or E > +99?
                        er = "OV";
                    } else {
                        if (e < -99) e = -99;
                        val /= i10x(e); // Normalize
                    }
                }
            }
        }
        if (!er) {  // Not error condition
            if (sign == '-') *buf++ = sign; // Add a - if negative value
            do {                // Put decimal number
                w = i10x(m);                // Snip the highest digit d
                d = val / w; val -= d * w;
                if (m == -1) *buf++ = XF_DPC;   // Insert a decimal separarot if get into fractional part
                *buf++ = '0' + d;           // Put the digit
            } while (--m >= -prec);         // Output all digits specified by prec
            if (fmt != 'f') {   // Put exponent if needed
                *buf++ = fmt;
                if (e < 0) {
                    e = -e; *buf++ = '-';
                } else {
                    *buf++ = '+';
                }
                *buf++ = '0' + e / 10;
                *buf++ = '0' + e % 10;
            }
        }
    }
    if (er) {   // Error condition?
        if (sign) *buf++ = sign;        // Add sign if needed
        do *buf++ = *er++; while (*er); // Put error symbol
    }
    *buf = 0;   // Term
}

// Simple format output
int udp_printf (const char *fmt, ...) {
    int len = 0;
    unsigned int r, i, j, w, f, l = 0;
    int n, prec;
    char str[SZB_OUTPUT], c, d, *p, pad;
    static char buf[SZB_PRNT_TXB];
#if XF_USE_LLI
    long long v;
    unsigned long long uv;
#else
    long v;
    unsigned long uv;
#endif

    va_list arp;
    va_start(arp, fmt);

    for (;;) {
        c = *fmt++;                 // Get a format character
        if (!c) break;              // End of format?
        if (c != '%') {             // Pass it through if not a % sequense
            buf[l++] = c; continue;
        }
        f = w = 0;                  // Clear parms
        pad = ' '; prec = -1;
        c = *fmt++;                 // Get first char of the sequense
        if (c == '0') {             // Flag: left '0' padded
            pad = '0'; c = *fmt++;
        } else {
            if (c == '-') {         // Flag: left justified
                f = 2; c = *fmt++;
            }
        }
        if (c == '*') {             // Minimum width from an argument
            n = va_arg(arp, int);
            if (n < 0) {            // Flag: left justified
                n = 0 - n; f = 2;
            }
            w = n; c = *fmt++;
        } else {
            while (c >= '0' && c <= '9') {  // Minimum width
                w = w * 10 + c - '0';
                c = *fmt++;
            }
        }
        if (c == '.') {             // Precision
            c = *fmt++;
            if (c == '*') {             // Precision from an argument
                prec = va_arg(arp, int);
                c = *fmt++;
            } else {
                prec = 0;
                while (c >= '0' && c <= '9') {
                    prec = prec * 10 + c - '0';
                    c = *fmt++;
                }
            }
        }
        if (c == 'l') {     // Prefix: Size is long
            f |= 4; c = *fmt++;
#if XF_USE_LLI
            if (c == 'l') { // Prefix: Size is long long
                f |= 8; c = *fmt++;
            }
#endif
        }
        if (!c) break;              // End of format?
        switch (c) {                // Type is...
            case 'b':                   // Unsigned binary
                r = 2; break;
            case 'o':                   // Unsigned octal
                r = 8; break;
            case 'd':                   // Signed decimal
            case 'u':                   // Unsigned decimal
                r = 10; break;
            case 'x':                   // Hexdecimal (lower case)
            case 'X':                   // Hexdecimal (upper case)
                r = 16; break;
            case 'c':                   // A character
                buf[l++] = (char)va_arg(arp, int); continue;
            case 's':                   // String
                p = va_arg(arp, char*);     // Get a pointer argument
                if (!p) p = "";             // Null ptr generates a null string
                j = strlen(p);
                if (prec >= 0 && j > (unsigned int)prec) j = prec;  // Limited length of string body
                for ( ; !(f & 2) && j < w; j++) buf[l++] = pad;  // Left pads
                while (*p && prec--) buf[l++] = *p++;// String body
                while (j++ < w) buf[l++] = ' ';      // Right pads
                continue;
#if XF_USE_FP
            case 'f':                   // Float (decimal)
            case 'e':                   // Float (e)
            case 'E':                   // Float (E)
                ftoa(p = str, va_arg(arp, double), prec, c);    // Make fp string
                for (j = strlen(p); !(f & 2) && j < w; j++) buf[l++] = pad;  // Left pads
                while (*p) buf[l++] = *p++;      // Value
                while (j++ < w) buf[l++] = ' ';  // Right pads
                continue;
#endif
            default:                    // Unknown type (passthrough)
                buf[l++] = c; continue;
        }

        // Get an integer argument and put it in numeral
#if XF_USE_LLI
        if (f & 8) {    // long long argument?
            v = (long long)va_arg(arp, long long);
        } else {
            if (f & 4) {    // long argument?
                v = (c == 'd') ? (long long)va_arg(arp, long) : (long long)va_arg(arp, unsigned long);
            } else {        // int/short/char argument
                v = (c == 'd') ? (long long)va_arg(arp, int) : (long long)va_arg(arp, unsigned int);
            }
        }
#else
        if (f & 4) {    // long argument?
            v = (long)va_arg(arp, long);
        } else {        // int/short/char argument
            v = (c == 'd') ? (long)va_arg(arp, int) : (long)va_arg(arp, unsigned int);
        }
#endif
        if (c == 'd' && v < 0) {    // Negative value?
            v = 0 - v; f |= 1;
        }
        i = 0; uv = v;
        do {    // Make an integer number string
            d = (char)(uv % r); uv /= r;
            if (d > 9) d += (c == 'x') ? 0x27 : 0x07;
            str[i++] = d + '0';
        } while (uv != 0 && i < sizeof str);
        if (f & 1) str[i++] = '-';                  // Sign
        for (j = i; !(f & 2) && j < w; j++) buf[l++] = pad;  // Left pads
        do buf[l++] = str[--i]; while (i != 0);  // Value
        while (j++ < w) buf[l++] = ' ';          // Right pads
    }
    va_end(arp);
    udp_putsb ((uint8_t *)buf, (int)l);

    return len;
}
