#include "mini_printf.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_usart.h"

void mini_printf_init(void)
{
    USART_ITConfig(USART3, USART_IT_TC, DISABLE);
    USART_ClearITPendingBit(USART3, USART_IT_TC);
    USART_DMACmd(USART3, USART_DMAReq_Tx, DISABLE);
}

static void uart_putc_raw(char c) {
    while (USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET);
    USART_SendData(USART3, (uint16_t)c);
}

static void uart_putc(char c) {
    if (c == '\n')
        uart_putc_raw('\r');
    uart_putc_raw(c);
}

#include <stdbool.h>
#include <stdarg.h>
void mini_printf(const char *fmt, ...)
{
    va_list va;
    va_start(va, fmt);

    while (*fmt) {
        char c = *fmt++;
        if (c == '%') {
            char c2 = *fmt++;
            switch (c2) {
                case '%':
                    uart_putc(c);
                    break;
                case 's': {
                    const char *v = va_arg(va, const char *);
                    while (*v) {
                        uart_putc(*v++);
                    }
                    break;
                }
                case 'd': {
                    int v = va_arg(va, int);
                    if (v < 0) {
                        v = -v;
                        uart_putc('-');
                    }
                    static const int tens[] = {
                            1000000000,
                            100000000,
                            1000000,
                            100000,
                            10000,
                            1000,
                            100,
                            10,
                            1,
                            };
                    if (!v) {
                        uart_putc('0');
                    } else {
                        bool had = false;
                        for(int i = 0; i < sizeof(tens) / sizeof(tens[0]); i++) {
                            int d = 0;
                            while (tens[i] <= v) {
                                v -= tens[i];
                                d++;
                            }
                            if (d || had) {
                                uart_putc((char)('0'+d));
                                had = true;
                            }
                        }
                    }
                    break;
                }
                case 'p': {
                    uint32_t v = va_arg(va, uint32_t);
                    for(int pos=7;pos>=0;pos--) {
                        int d = (v >> (pos * 4)) & 0xf;
                        if (d < 10) uart_putc((char)('0'+d));
                        else uart_putc((char)('a'+d - 10));
                    }
                    break;
                }
                case '0': {
                    uint32_t v = va_arg(va, uint32_t);
                    if (fmt[0] > '0' && fmt[0] < '9' && fmt[1] == 'x') {
                        int zeros = fmt[0]-'1';
                        bool had_digit=false;
                        for(int pos=7;pos>=0;pos--) {
                            int d = (v >> (pos * 4)) & 0xf;
                            if (!d && pos > zeros && !had_digit) continue;
                            had_digit = true;
                            if (d < 10) uart_putc((char)('0'+d));
                            else uart_putc((char)('a'+d - 10));
                        }
                        fmt+=2;
                        break;
                    }
                    __attribute__((fallthrough));
                }
                default:
                    uart_putc('%');
                    uart_putc(c2);
                    uart_putc('?');
                    uart_putc('?');
            }
        } else {
            uart_putc(c);
        }
    }
    va_end(va);
}

void mini_puts(const char *str) {
    while (*str) {
        uart_putc(*str++);
    }
    uart_putc('\n');
}