/*********************************************************************
*                    SEGGER Microcontroller GmbH                     *
*                        The Embedded Experts                        *
**********************************************************************
*                                                                    *
*            (c) 1995 - 2019 SEGGER Microcontroller GmbH             *
*                                                                    *
*       www.segger.com     Support: support@segger.com               *
*                                                                    *
**********************************************************************
*                                                                    *
*       SEGGER RTT * Real Time Transfer for embedded targets         *
*                                                                    *
**********************************************************************
*                                                                    *
* All rights reserved.                                               *
*                                                                    *
* SEGGER strongly recommends to not make any changes                 *
* to or modify the source code of this software in order to stay     *
* compatible with the RTT protocol and J-Link.                       *
*                                                                    *
* Redistribution and use in source and binary forms, with or         *
* without modification, are permitted provided that the following    *
* condition is met:                                                  *
*                                                                    *
* o Redistributions of source code must retain the above copyright   *
*   notice, this condition and the following disclaimer.             *
*                                                                    *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND             *
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,        *
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF           *
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE           *
* DISCLAIMED. IN NO EVENT SHALL SEGGER Microcontroller BE LIABLE FOR *
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR           *
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT  *
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;    *
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF      *
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT          *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE  *
* USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH   *
* DAMAGE.                                                            *
*                                                                    *
**********************************************************************
---------------------------END-OF-HEADER------------------------------
File    : SEGGER_RTT_printf.c
Purpose : Replacement for printf to write formatted data via RTT
Revision: $Rev: 17697 $
----------------------------------------------------------------------
*/
#include "SEGGER_RTT.h"
#include "SEGGER_RTT_Conf.h"

/*********************************************************************
 *
 *       Defines, configurable
 *
 **********************************************************************
 */

#ifndef SEGGER_RTT_PRINTF_BUFFER_SIZE
#define SEGGER_RTT_PRINTF_BUFFER_SIZE (64)
#endif

#include <stdarg.h>
#include <stdlib.h>

#define FORMAT_FLAG_LEFT_JUSTIFY (1u << 0)
#define FORMAT_FLAG_PAD_ZERO (1u << 1)
#define FORMAT_FLAG_PRINT_SIGN (1u << 2)
#define FORMAT_FLAG_ALTERNATE (1u << 3)

/*********************************************************************
 *
 *       Types
 *
 **********************************************************************
 */

typedef struct
{
    char * pBuffer;
    unsigned BufferSize;
    unsigned Cnt;

    int ReturnValue;

    unsigned RTTBufferIndex;
} SEGGER_RTT_PRINTF_DESC;

/*********************************************************************
 *
 *       Function prototypes
 *
 **********************************************************************
 */

/*********************************************************************
 *
 *       Static code
 *
 **********************************************************************
 */
/*********************************************************************
 *
 *       _StoreChar
 */
static void _StoreChar(SEGGER_RTT_PRINTF_DESC * p, char c)
{
    unsigned Cnt;

    Cnt = p->Cnt;
    if ((Cnt + 1u) <= p->BufferSize)
    {
        *(p->pBuffer + Cnt) = c;
        p->Cnt              = Cnt + 1u;
        p->ReturnValue++;
    }
    //
    // Write part of string, when the buffer is full
    //
    if (p->Cnt == p->BufferSize)
    {
        if (SEGGER_RTT_Write(p->RTTBufferIndex, p->pBuffer, p->Cnt) != p->Cnt)
        {
            p->ReturnValue = -1;
        }
        else
        {
            p->Cnt = 0u;
        }
    }
}

/*********************************************************************
 *
 *       _PrintUnsigned
 */
static void _PrintUnsigned(SEGGER_RTT_PRINTF_DESC * pBufferDesc, unsigned v, unsigned Base, unsigned NumDigits, unsigned FieldWidth,
                           unsigned FormatFlags)
{
    static const char _aV2C[16] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
    unsigned Div;
    unsigned Digit;
    unsigned Number;
    unsigned Width;
    char c;

    Number = v;
    Digit  = 1u;
    //
    // Get actual field width
    //
    Width = 1u;
    while (Number >= Base)
    {
        Number = (Number / Base);
        Width++;
    }
    if (NumDigits > Width)
    {
        Width = NumDigits;
    }
    //
    // Print leading chars if necessary
    //
    if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u)
    {
        if (FieldWidth != 0u)
        {
            if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && (NumDigits == 0u))
            {
                c = '0';
            }
            else
            {
                c = ' ';
            }
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                _StoreChar(pBufferDesc, c);
                if (pBufferDesc->ReturnValue < 0)
                {
                    break;
                }
            }
        }
    }
    if (pBufferDesc->ReturnValue >= 0)
    {
        //
        // Compute Digit.
        // Loop until Digit has the value of the highest digit required.
        // Example: If the output is 345 (Base 10), loop 2 times until Digit is 100.
        //
        while (1)
        {
            if (NumDigits > 1u)
            { // User specified a min number of digits to print? => Make sure we loop at least that often, before checking anything
              // else (> 1 check avoids problems with NumDigits being signed / unsigned)
                NumDigits--;
            }
            else
            {
                Div = v / Digit;
                if (Div < Base)
                { // Is our divider big enough to extract the highest digit from value? => Done
                    break;
                }
            }
            Digit *= Base;
        }
        //
        // Output digits
        //
        do
        {
            Div = v / Digit;
            v -= Div * Digit;
            _StoreChar(pBufferDesc, _aV2C[Div]);
            if (pBufferDesc->ReturnValue < 0)
            {
                break;
            }
            Digit /= Base;
        } while (Digit);
        //
        // Print trailing spaces if necessary
        //
        if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == FORMAT_FLAG_LEFT_JUSTIFY)
        {
            if (FieldWidth != 0u)
            {
                while ((FieldWidth != 0u) && (Width < FieldWidth))
                {
                    FieldWidth--;
                    _StoreChar(pBufferDesc, ' ');
                    if (pBufferDesc->ReturnValue < 0)
                    {
                        break;
                    }
                }
            }
        }
    }
}

/*********************************************************************
 *
 *       _PrintInt
 */
static void _PrintInt(SEGGER_RTT_PRINTF_DESC * pBufferDesc, int v, unsigned Base, unsigned NumDigits, unsigned FieldWidth,
                      unsigned FormatFlags)
{
    unsigned Width;
    int Number;

    Number = (v < 0) ? -v : v;

    //
    // Get actual field width
    //
    Width = 1u;
    while (Number >= (int) Base)
    {
        Number = (Number / (int) Base);
        Width++;
    }
    if (NumDigits > Width)
    {
        Width = NumDigits;
    }
    if ((FieldWidth > 0u) && ((v < 0) || ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN)))
    {
        FieldWidth--;
    }

    //
    // Print leading spaces if necessary
    //
    if ((((FormatFlags & FORMAT_FLAG_PAD_ZERO) == 0u) || (NumDigits != 0u)) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u))
    {
        if (FieldWidth != 0u)
        {
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                _StoreChar(pBufferDesc, ' ');
                if (pBufferDesc->ReturnValue < 0)
                {
                    break;
                }
            }
        }
    }
    //
    // Print sign if necessary
    //
    if (pBufferDesc->ReturnValue >= 0)
    {
        if (v < 0)
        {
            v = -v;
            _StoreChar(pBufferDesc, '-');
        }
        else if ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN)
        {
            _StoreChar(pBufferDesc, '+');
        }
        else
        {
        }
        if (pBufferDesc->ReturnValue >= 0)
        {
            //
            // Print leading zeros if necessary
            //
            if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) &&
                ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0u) && (NumDigits == 0u))
            {
                if (FieldWidth != 0u)
                {
                    while ((FieldWidth != 0u) && (Width < FieldWidth))
                    {
                        FieldWidth--;
                        _StoreChar(pBufferDesc, '0');
                        if (pBufferDesc->ReturnValue < 0)
                        {
                            break;
                        }
                    }
                }
            }
            if (pBufferDesc->ReturnValue >= 0)
            {
                //
                // Print number without sign
                //
                _PrintUnsigned(pBufferDesc, (unsigned) v, Base, NumDigits, FieldWidth, FormatFlags);
            }
        }
    }
}

/*********************************************************************
 *
 *       Public code
 *
 **********************************************************************
 */
/*********************************************************************
 *
 *       SEGGER_RTT_vprintf
 *
 *  Function description
 *    Stores a formatted string in SEGGER RTT control block.
 *    This data is read by the host.
 *
 *  Parameters
 *    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")
 *    sFormat      Pointer to format string
 *    pParamList   Pointer to the list of arguments for the format string
 *
 *  Return values
 *    >= 0:  Number of bytes which have been stored in the "Up"-buffer.
 *     < 0:  Error
 */
int SEGGER_RTT_vprintf(unsigned BufferIndex, const char * sFormat, va_list * pParamList)
{
    char c;
    SEGGER_RTT_PRINTF_DESC BufferDesc;
    int v;
    unsigned NumDigits;
    unsigned FormatFlags;
    unsigned FieldWidth;
    char acBuffer[SEGGER_RTT_PRINTF_BUFFER_SIZE];

    BufferDesc.pBuffer        = acBuffer;
    BufferDesc.BufferSize     = SEGGER_RTT_PRINTF_BUFFER_SIZE;
    BufferDesc.Cnt            = 0u;
    BufferDesc.RTTBufferIndex = BufferIndex;
    BufferDesc.ReturnValue    = 0;

    do
    {
        c = *sFormat;
        sFormat++;
        if (c == 0u)
        {
            break;
        }
        if (c == '%')
        {
            //
            // Filter out flags
            //
            FormatFlags = 0u;
            v           = 1;
            do
            {
                c = *sFormat;
                switch (c)
                {
                case '-':
                    FormatFlags |= FORMAT_FLAG_LEFT_JUSTIFY;
                    sFormat++;
                    break;
                case '0':
                    FormatFlags |= FORMAT_FLAG_PAD_ZERO;
                    sFormat++;
                    break;
                case '+':
                    FormatFlags |= FORMAT_FLAG_PRINT_SIGN;
                    sFormat++;
                    break;
                case '#':
                    FormatFlags |= FORMAT_FLAG_ALTERNATE;
                    sFormat++;
                    break;
                default:
                    v = 0;
                    break;
                }
            } while (v);
            //
            // filter out field with
            //
            FieldWidth = 0u;
            do
            {
                c = *sFormat;
                if ((c < '0') || (c > '9'))
                {
                    break;
                }
                sFormat++;
                FieldWidth = (FieldWidth * 10u) + ((unsigned) c - '0');
            } while (1);

            //
            // Filter out precision (number of digits to display)
            //
            NumDigits = 0u;
            c         = *sFormat;
            if (c == '.')
            {
                sFormat++;
                do
                {
                    c = *sFormat;
                    if ((c < '0') || (c > '9'))
                    {
                        break;
                    }
                    sFormat++;
                    NumDigits = NumDigits * 10u + ((unsigned) c - '0');
                } while (1);
            }
            //
            // Filter out length modifier
            //
            c = *sFormat;
            do
            {
                if ((c == 'l') || (c == 'h'))
                {
                    sFormat++;
                    c = *sFormat;
                }
                else
                {
                    break;
                }
            } while (1);
            //
            // Handle specifiers
            //
            switch (c)
            {
            case 'c': {
                char c0;
                v  = va_arg(*pParamList, int);
                c0 = (char) v;
                _StoreChar(&BufferDesc, c0);
                break;
            }
            case 'd':
                v = va_arg(*pParamList, int);
                _PrintInt(&BufferDesc, v, 10u, NumDigits, FieldWidth, FormatFlags);
                break;
            case 'u':
                v = va_arg(*pParamList, int);
                _PrintUnsigned(&BufferDesc, (unsigned) v, 10u, NumDigits, FieldWidth, FormatFlags);
                break;
            case 'x':
            case 'X':
                v = va_arg(*pParamList, int);
                _PrintUnsigned(&BufferDesc, (unsigned) v, 16u, NumDigits, FieldWidth, FormatFlags);
                break;
            case 's': {
                const char * s = va_arg(*pParamList, const char *);
                do
                {
                    c = *s;
                    s++;
                    if (c == '\0')
                    {
                        break;
                    }
                    _StoreChar(&BufferDesc, c);
                } while (BufferDesc.ReturnValue >= 0);
            }
            break;
            case 'p':
                v = va_arg(*pParamList, int);
                _PrintUnsigned(&BufferDesc, (unsigned) v, 16u, 8u, 8u, 0u);
                break;
            case '%':
                _StoreChar(&BufferDesc, '%');
                break;
            default:
                break;
            }
            sFormat++;
        }
        else
        {
            _StoreChar(&BufferDesc, c);
        }
    } while (BufferDesc.ReturnValue >= 0);

    if (BufferDesc.ReturnValue > 0)
    {
        //
        // Write remaining data, if any
        //
        if (BufferDesc.Cnt != 0u)
        {
            SEGGER_RTT_Write(BufferIndex, acBuffer, BufferDesc.Cnt);
        }
        BufferDesc.ReturnValue += (int) BufferDesc.Cnt;
    }
    return BufferDesc.ReturnValue;
}

/*********************************************************************
 *
 *       SEGGER_RTT_printf
 *
 *  Function description
 *    Stores a formatted string in SEGGER RTT control block.
 *    This data is read by the host.
 *
 *  Parameters
 *    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")
 *    sFormat      Pointer to format string, followed by the arguments for conversion
 *
 *  Return values
 *    >= 0:  Number of bytes which have been stored in the "Up"-buffer.
 *     < 0:  Error
 *
 *  Notes
 *    (1) Conversion specifications have following syntax:
 *          %[flags][FieldWidth][.Precision]ConversionSpecifier
 *    (2) Supported flags:
 *          -: Left justify within the field width
 *          +: Always print sign extension for signed conversions
 *          0: Pad with 0 instead of spaces. Ignored when using '-'-flag or precision
 *        Supported conversion specifiers:
 *          c: Print the argument as one char
 *          d: Print the argument as a signed integer
 *          u: Print the argument as an unsigned integer
 *          x: Print the argument as an hexadecimal integer
 *          s: Print the string pointed to by the argument
 *          p: Print the argument as an 8-digit hexadecimal integer. (Argument shall be a pointer to void.)
 */
int SEGGER_RTT_printf(unsigned BufferIndex, const char * sFormat, ...)
{
    int r;
    va_list ParamList;

    va_start(ParamList, sFormat);
    r = SEGGER_RTT_vprintf(BufferIndex, sFormat, &ParamList);
    va_end(ParamList);
    return r;
}
/*************************** End of file ****************************/