#include <stdarg.h>
#include <string.h>
#include <stdint.h>
#include "omw_dbg.h"
#include "omw_gpio.h"
#include "omw_uart.h"

#ifdef FUNCTION_CONTROL_DEBUG_ENABLE
#ifndef FUNCTION_CONTROL_DEBUG_PRINTF_ONLY_ENABLE
/**
  * To avoid uart lost info, we need add protocol to protect it, The input length must limit DEBUG_PROTOCL_PAYLOAD_ALLOW_LENGTH
  * To reduce timing, you should prepare payload into <uart_send_buf> in right point.
  *   1byte   1byte    2byte      nbytes     1byte
  * | 0xAA  | Type  | length  |  Payload   | 0xBB  |
  */
void omw_dbg_uart_protocol(uint8_t type, uint8_t* buf, uint16_t len, uint8_t* sub_buf, uint16_t sub_len)
{
    uint8_t uart_send_buf[4];
    uint8_t offset = 0;

    if(len == 0)
    {
        return;
    }

    *(uart_send_buf + offset) = 0xAA;
    offset ++;
    *(uart_send_buf + offset) = type;
    offset ++;
    *(uint16_t *)(uart_send_buf + offset) = len + sub_len;

    // send header
    omw_uart_send(OMW_LOG_UART, uart_send_buf, DEBUG_PROTOCL_PAYLOAD_START_OFFSET);

    // send payload
    omw_uart_send(OMW_LOG_UART, buf, len);

    // send sub payload
    if(sub_len != 0)
    {
        omw_uart_send(OMW_LOG_UART, sub_buf, sub_len);
    }

    // send trail
    *(uart_send_buf + 0) = 0xBB;
    omw_uart_send(OMW_LOG_UART, uart_send_buf, 1);
}

static void write_long_value(uint8_t* addr, uint32_t value)
{
	*(addr) = (value >> 0);
	*(addr + 1) = (value >> 8);
	*(addr + 2) = (value >> 16);
	*(addr + 3) = (value >> 24);
}

void omw_dbg_print_round_buffer(uint8_t index, uint32_t start_addr, uint32_t end_addr, uint32_t write_addr, uint32_t read_addr)
{
	uint8_t uart_send_buf[24];
	uint8_t offset = 0;

	*(uart_send_buf + offset + DEBUG_PROTOCL_PAYLOAD_START_OFFSET) = index;
	offset = offset + 1;
	write_long_value((uart_send_buf + offset + DEBUG_PROTOCL_PAYLOAD_START_OFFSET), start_addr);
	offset = offset + 4;
	write_long_value((uart_send_buf + offset + DEBUG_PROTOCL_PAYLOAD_START_OFFSET), end_addr);
	offset = offset + 4;
	write_long_value((uart_send_buf + offset + DEBUG_PROTOCL_PAYLOAD_START_OFFSET), write_addr);
	offset = offset + 4;
	write_long_value((uart_send_buf + offset + DEBUG_PROTOCL_PAYLOAD_START_OFFSET), read_addr);
	offset = offset + 4;
	// Send packet
	omw_dbg_uart_protocol(DEBUG_PROTOCL_TYPE_ROUND_BUFFER, uart_send_buf, offset, NULL, 0);
}

void omw_dbg_print_point(uint32_t value)
{
	// Send packet
    omw_dbg_uart_protocol(DEBUG_PROTOCL_TYPE_LOG, (uint8_t *)&value, 4, NULL, 0);
}

void omw_dbg_print_string(const char *format, ...)
{
	va_list ap;

	va_start(ap, format);

    omw_dbg_print_string_raw(format, ap);

    va_end(ap);
}

void omw_dbg_print_protocol(uint16_t log_level, uint16_t log_type, uint8_t* buf, int len)
{
	uint16_t header_buf[2];

	if(len == 0)
	{
		return;
	}

    header_buf[0] = log_level;
    header_buf[1] = log_type;

    omw_dbg_uart_protocol(DEBUG_PROTOCL_TYPE_PROTOCOL, (uint8_t *)header_buf, 4, buf, len);
}

void omw_dbg_print_hci_h4_raw(uint8_t isRcv, uint8_t type, uint8_t* buf, int len)
{
	uint8_t header_buf[2];
	header_buf[0] = isRcv;
	header_buf[1] = type;

    omw_dbg_uart_protocol(DEBUG_PROTOCL_TYPE_HCI_H4, header_buf, 2, buf, len);
}


void omw_dbg_print_hci_h4(uint8_t isRcv, uint8_t* buf, int len)
{
    omw_dbg_print_hci_h4_raw(isRcv, buf[0], buf+1, len - 1);
}

#endif

static void print_string_to_buf(uint8_t* uartSndBuf, uint8_t* uartSndBufUseLen, uint8_t* buf, int len)
{
	for(uint8_t i = 0; i < len; i ++)
	{
		if(*uartSndBufUseLen >= DEBUG_STRING_LOG_SINGLE_LINE_MAX_SIZE)
		{
			return;
		}
		*(uartSndBuf + *uartSndBufUseLen) = buf[i];

		*uartSndBufUseLen = *uartSndBufUseLen+1;
	}
}

void omw_dbg_print_string_raw(const char *format, va_list vaArgP)
{
	// For fast display log.
    DEBUG_POINT_POLLING_PRINT();
    uint32_t ulIdx, ulValue, ulPos, ulCount, ulBase, ulNeg;
    char *pcStr, pcBuf[16], cFill;
    char HexFormat = 0;
    static const char* const g_pcHex1 = "0123456789abcdef";
    static const char* const g_pcHex2 = "0123456789ABCDEF";
    uint8_t uart_tx_buff[DEBUG_STRING_LOG_SINGLE_LINE_MAX_SIZE];
    uint8_t uart_tx_local_ptr = 0;

    while(*format)
    {
        // Find the first non-% character, or the end of the string.
        for(ulIdx = 0; (format[ulIdx] != '%') && (format[ulIdx] != '\0');ulIdx++)
        {}

        // Write this portion of the string.
        if(ulIdx>0)
        {
            print_string_to_buf(uart_tx_buff, &uart_tx_local_ptr, (uint8_t*)format, ulIdx);
        }

        format += ulIdx;

        if(*format == '%')
        {
            format++;

            // Set the digit count to zero, and the fill character to space
            // (i.e. to the defaults).
            ulCount = 0;
            cFill = ' ';

        again:
           switch(*format++)
            {
                case '0':
                case '1':
                case '2':
                case '3':
                case '4':
                case '5':
                case '6':
                case '7':
                case '8':
                case '9':
                {
                    if((format[-1] == '0') && (ulCount == 0))
                    {
                        cFill = '0';
                    }

                    ulCount *= 10;
                    ulCount += format[-1] - '0';

                    goto again;
                }

                case 'c':
                {
                    ulValue = va_arg(vaArgP, unsigned long);
                    print_string_to_buf(uart_tx_buff, &uart_tx_local_ptr, (uint8_t*)&ulValue, 1);
                    break;
                }

                case 'd':
                case 'z':
                {
                    ulValue = va_arg(vaArgP, unsigned long);
                    ulPos = 0;

                    if((long)ulValue < 0)
                    {
                        ulValue = -(long)ulValue;
                        ulNeg = 1;
                    }
                    else
                    {
                        ulNeg = 0;
                    }

                    ulBase = 10;
                    goto convert;
                }

                case 's':
                {
                    pcStr = va_arg(vaArgP, char *);

                    for(ulIdx = 0; pcStr[ulIdx] != '\0'; ulIdx++)
                    {}

                    print_string_to_buf(uart_tx_buff, &uart_tx_local_ptr, (uint8_t*)pcStr, ulIdx);

                    if(ulCount > ulIdx)
                    {
                        ulCount -= ulIdx;
                        while(ulCount--)
                        {
                            print_string_to_buf(uart_tx_buff, &uart_tx_local_ptr, (uint8_t*)" ", 1);
                        }
                    }
                    break;
                }

                case 'u':
                {
                    ulValue = va_arg(vaArgP, unsigned long);
                    ulPos = 0;
                    ulBase = 10;
                    ulNeg = 0;
                    goto convert;
                }


                case 'X':
                {
                    ulValue = va_arg(vaArgP, unsigned long);
                    ulPos = 0;
                    ulBase = 16;
                    ulNeg = 0;
                    HexFormat='X';
                    goto convert;
                }

                case 'x':
                case 'p':
                {
                    ulValue = va_arg(vaArgP, unsigned long);
                    ulPos = 0;
                    ulBase = 16;
                    ulNeg = 0;
                     HexFormat='x';

        convert:
                    for(ulIdx = 1;
                        (((ulIdx * ulBase) <= ulValue) &&
                         (((ulIdx * ulBase) / ulBase) == ulIdx));
                        ulIdx *= ulBase, ulCount--)
                    {
                    }

                    if(ulNeg)
                    {
                        ulCount--;
                    }

                    if(ulNeg && (cFill == '0'))
                    {
                        pcBuf[ulPos++] = '-';
                        ulNeg = 0;
                    }

                    if((ulCount > 1) && (ulCount < 16))
                    {
                        for(ulCount--; ulCount; ulCount--)
                        {
                            pcBuf[ulPos++] = cFill;
                        }
                    }

                    if(ulNeg)
                    {
                        pcBuf[ulPos++] = '-';
                    }

                    for(; ulIdx; ulIdx /= ulBase)
                    {
                        if(HexFormat=='x')  pcBuf[ulPos++] = g_pcHex1[(ulValue / ulIdx) % ulBase];//x
                        else    pcBuf[ulPos++] = g_pcHex2[(ulValue / ulIdx) % ulBase];//X
                    }

                    print_string_to_buf(uart_tx_buff, &uart_tx_local_ptr, (uint8_t*)pcBuf, ulPos);
                    break;
                }

                case '%':
                {
                    print_string_to_buf(uart_tx_buff, &uart_tx_local_ptr, (uint8_t*)format - 1, 1);
                    break;
                }
                case 'l':
                {
                    goto again;
                }

                default:
                {
                    print_string_to_buf(uart_tx_buff, &uart_tx_local_ptr, (uint8_t*)"ERROR", 5);
                    break;
                }
            }//switch
        }//if
    }//while

    // Send packet
#if defined(DEBUG_STRING_NO_PROTOCOL) || defined(FUNCTION_CONTROL_DEBUG_PRINTF_ONLY_ENABLE)
    omw_uart_send(OMW_LOG_UART, uart_tx_buff, uart_tx_local_ptr);
#else
    omw_dbg_uart_protocol(DEBUG_PROTOCL_TYPE_STRING, uart_tx_buff, uart_tx_local_ptr, NULL, 0);
#endif
}

void omw_dbg_init(void)
{
    //de-init uart gpio
    omw_gpio_set_func(OMW_LOG_UART_RX_PIN, 0);
    omw_gpio_set_func(OMW_LOG_UART_TX_PIN, 0);

    omw_uart_init(OMW_LOG_UART, OMW_LOG_UART_BAUDRATE, 0);
    omw_gpio_set_func(OMW_LOG_UART_RX_PIN, OMW_GPIO_CFG_VAL(OMW_GPIO_DRIVE_STRENTH3, OMW_LOG_UART_RX_FUNC | OMW_GPIO_MODE_IN_PULL_UP));
    omw_gpio_set_func(OMW_LOG_UART_TX_PIN, OMW_GPIO_CFG_VAL(OMW_GPIO_DRIVE_STRENTH3, OMW_LOG_UART_TX_FUNC));
}

size_t strlen(const char * str)
{
    int c = 0;

    while(str[c++] != '\0');

    return (c - 1);
}

int putchar(int ch)
{
    int8_t u8_ch = ch;
    omw_uart_send(OMW_LOG_UART, &u8_ch, 1);

    return 0;
}

int puts(const char * str)
{
	omw_uart_send(OMW_LOG_UART, (void *)str, strlen(str));
    omw_uart_send(OMW_LOG_UART, "\n", 1);
    return 0;
}

int printf(const char *format, ...)
{
	va_list ap;

	va_start(ap, format);

    omw_dbg_print_string_raw(format, ap);

    va_end(ap);
    return 0;

}

#ifdef  OMW_USE_FULL_ASSERT
void assert_failed(uint8_t* file, uint32_t line)
{
    printf("assert failed: %s, %d\n", file, line);
    while (1);
}
#endif

#endif


