#include "stm32f4xx.h"

#include <stdarg.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>

typedef struct
{
    char *buffer;
    size_t capacity;
    size_t length;
} OutputBuffer;

static void output_char(OutputBuffer *output, char value)
{
    if (output->capacity != 0U && output->length + 1U < output->capacity)
    {
        output->buffer[output->length] = value;
    }
    output->length++;
}

static void output_unsigned(OutputBuffer *output, uint32_t value, unsigned int base,
                            unsigned int width, char pad, int uppercase)
{
    char digits[16];
    unsigned int count = 0U;
    const char *alphabet = uppercase ? "0123456789ABCDEF" : "0123456789abcdef";

    do
    {
        digits[count++] = alphabet[value % base];
        value /= base;
    } while (value != 0U);

    while (count < width)
    {
        output_char(output, pad);
        width--;
    }
    while (count != 0U)
    {
        output_char(output, digits[--count]);
    }
}

int vsnprintf(char *buffer, size_t size, const char *format, va_list args)
{
    OutputBuffer output = {buffer, size, 0U};

    while (*format != '\0')
    {
        unsigned int width = 0U;
        char pad = ' ';
        int is_long = 0;
        char specifier;

        if (*format != '%')
        {
            output_char(&output, *format++);
            continue;
        }
        format++;
        if (*format == '%')
        {
            output_char(&output, *format++);
            continue;
        }
        if (*format == '0')
        {
            pad = '0';
            format++;
        }
        while (*format >= '0' && *format <= '9')
        {
            width = width * 10U + (unsigned int)(*format++ - '0');
        }
        if (*format == 'l')
        {
            is_long = 1;
            format++;
        }
        specifier = *format++;
        if (specifier == 's')
        {
            const char *text = va_arg(args, const char *);
            if (text == NULL)
            {
                text = "(null)";
            }
            while (*text != '\0')
            {
                output_char(&output, *text++);
            }
        }
        else if (specifier == 'c')
        {
            output_char(&output, (char)va_arg(args, int));
        }
        else if (specifier == 'd' || specifier == 'i')
        {
            int32_t value = is_long ? (int32_t)va_arg(args, long) : (int32_t)va_arg(args, int);
            uint32_t magnitude;
            if (value < 0)
            {
                output_char(&output, '-');
                magnitude = 0U - (uint32_t)value;
                if (width != 0U)
                {
                    width--;
                }
            }
            else
            {
                magnitude = (uint32_t)value;
            }
            output_unsigned(&output, magnitude, 10U, width, pad, 0);
        }
        else if (specifier == 'u' || specifier == 'x' || specifier == 'X')
        {
            uint32_t value = is_long ? (uint32_t)va_arg(args, unsigned long)
                                     : (uint32_t)va_arg(args, unsigned int);
            unsigned int base = specifier == 'u' ? 10U : 16U;
            output_unsigned(&output, value, base, width, pad, specifier == 'X');
        }
        else if (specifier == 'p')
        {
            uintptr_t value = (uintptr_t)va_arg(args, void *);
            output_char(&output, '0');
            output_char(&output, 'x');
            output_unsigned(&output, (uint32_t)value, 16U, 8U, '0', 0);
        }
        else
        {
            output_char(&output, '%');
            output_char(&output, specifier);
        }
    }

    if (output.capacity != 0U)
    {
        size_t terminator = output.length < output.capacity ? output.length : output.capacity - 1U;
        output.buffer[terminator] = '\0';
    }
    return (int)output.length;
}

int snprintf(char *buffer, size_t size, const char *format, ...)
{
    int result;
    va_list args;
    va_start(args, format);
    result = vsnprintf(buffer, size, format, args);
    va_end(args);
    return result;
}

int sprintf(char *buffer, const char *format, ...)
{
    int result;
    va_list args;
    va_start(args, format);
    result = vsnprintf(buffer, (size_t)-1, format, args);
    va_end(args);
    return result;
}

int printf(const char *format, ...)
{
    char buffer[192];
    int result;
    size_t count;
    va_list args;

    va_start(args, format);
    result = vsnprintf(buffer, sizeof(buffer), format, args);
    va_end(args);
    count = result < 0 ? 0U : (size_t)result;
    if (count >= sizeof(buffer))
    {
        count = sizeof(buffer) - 1U;
    }
    for (size_t index = 0U; index < count; index++)
    {
        while ((USART2->SR & USART_SR_TC) == 0U)
        {
        }
        USART2->DR = (uint8_t)buffer[index];
    }
    return result;
}

void *memcpy(void *destination, const void *source, size_t count)
{
    uint8_t *to = (uint8_t *)destination;
    const uint8_t *from = (const uint8_t *)source;
    while (count-- != 0U)
    {
        *to++ = *from++;
    }
    return destination;
}

void *memset(void *destination, int value, size_t count)
{
    uint8_t *to = (uint8_t *)destination;
    while (count-- != 0U)
    {
        *to++ = (uint8_t)value;
    }
    return destination;
}

int memcmp(const void *left, const void *right, size_t count)
{
    const uint8_t *lhs = (const uint8_t *)left;
    const uint8_t *rhs = (const uint8_t *)right;
    while (count-- != 0U)
    {
        if (*lhs != *rhs)
        {
            return (int)*lhs - (int)*rhs;
        }
        lhs++;
        rhs++;
    }
    return 0;
}

size_t strlen(const char *string)
{
    const char *end = string;
    while (*end != '\0')
    {
        end++;
    }
    return (size_t)(end - string);
}

char *strcpy(char *destination, const char *source)
{
    char *result = destination;
    do
    {
        *destination++ = *source;
    } while (*source++ != '\0');
    return result;
}

int abs(int value)
{
    return value < 0 ? -value : value;
}

double sqrt(double value)
{
    double estimate;
    if (value <= 0.0)
    {
        return value == 0.0 ? 0.0 : (0.0 / 0.0);
    }
    estimate = value > 1.0 ? value : 1.0;
    for (unsigned int iteration = 0U; iteration < 12U; iteration++)
    {
        estimate = 0.5 * (estimate + value / estimate);
    }
    return estimate;
}

double pow(double base, double exponent)
{
    int power = (int)exponent;
    unsigned int magnitude;
    double result = 1.0;
    if ((double)power != exponent)
    {
        return 0.0 / 0.0;
    }
    magnitude = power < 0 ? (unsigned int)(-power) : (unsigned int)power;
    while (magnitude != 0U)
    {
        if ((magnitude & 1U) != 0U)
        {
            result *= base;
        }
        base *= base;
        magnitude >>= 1U;
    }
    return power < 0 ? 1.0 / result : result;
}

double atan(double value)
{
    const double half_pi = 1.5707963267948966;
    const double quarter_pi = 0.7853981633974483;
    double sign = 1.0;
    double result;
    if (value < 0.0)
    {
        value = -value;
        sign = -1.0;
    }
    if (value > 1.0)
    {
        double reciprocal = 1.0 / value;
        result = half_pi - reciprocal * (quarter_pi + 0.273 * (1.0 - reciprocal));
    }
    else
    {
        result = value * (quarter_pi + 0.273 * (1.0 - value));
    }
    return sign * result;
}

double tan(double value)
{
    const double pi = 3.1415926535897932;
    double squared;
    double sine;
    double cosine;
    while (value > 1.5707963267948966)
    {
        value -= pi;
    }
    while (value < -1.5707963267948966)
    {
        value += pi;
    }
    squared = value * value;
    sine = value * (1.0 + squared * (-1.0 / 6.0 + squared * (1.0 / 120.0 +
           squared * (-1.0 / 5040.0 + squared * (1.0 / 362880.0)))));
    cosine = 1.0 + squared * (-1.0 / 2.0 + squared * (1.0 / 24.0 +
             squared * (-1.0 / 720.0 + squared * (1.0 / 40320.0))));
    return sine / cosine;
}