#include <sta/printable_uart.hpp>

#include <sta/assert.hpp>
#include <sta/printf.hpp>

#include <cinttypes>
#include <cstring>



namespace sta
{
    PrintableUART::PrintableUART(UART * intf)
        : intf_{intf}
    {
        STA_ASSERT(intf != nullptr);
    }


    void PrintableUART::print(char c)
    {
        print(&c, 1);
    }

    void PrintableUART::print(bool b)
    {
        print(b ? "true" : "false");
    }

    void PrintableUART::print(double d)
    {
        char buffer[64];
        snprintf(buffer, sizeof(buffer), "%f", d);
        print(buffer);
    }

    void PrintableUART::print(uint8_t num, IntegerBase base /* = IntegerBase::DEC */)
    {
        printBase(num, base, "%" PRIu8, sizeof(num));
    }

    void PrintableUART::print(uint16_t num, IntegerBase base /* = IntegerBase::DEC */)
    {
        printBase(num, base, "%" PRIu16, sizeof(num));
    }

    void PrintableUART::print(uint32_t num, IntegerBase base /* = IntegerBase::DEC */)
    {
        printBase(num, base, "%" PRIu32, sizeof(num));
    }

    void PrintableUART::print(const char * str)
    {
        print(str, strlen(str));
    }

    void PrintableUART::print(const char * str, size_t length)
    {
        intf_->write(reinterpret_cast<const uint8_t *>(str), length);
    }


    void PrintableUART::println()
    {
        print("\r\n", 2);
    }

    void PrintableUART::println(char c)
    {
        print(&c, 1);
        println();
    }

    void PrintableUART::println(bool b)
    {
        print(b);
        println();
    }

    void PrintableUART::println(double d)
    {
        print(d);
        println();
    }

    void PrintableUART::println(uint8_t num, IntegerBase base /* = IntegerBase::DEC */)
    {
        print(num, base);
        println();
    }

    void PrintableUART::println(uint16_t num, IntegerBase base /* = IntegerBase::DEC */)
    {
        print(num, base);
        println();
    }

    void PrintableUART::println(uint32_t num, IntegerBase base /* = IntegerBase::DEC */)
    {
        print(num, base);
        println();
    }

    void PrintableUART::println(const char * str)
    {
        println(str, strlen(str));
    }

    void PrintableUART::println(const char * str, size_t length)
    {
        print(str, length);
        println();
    }



    void PrintableUART::printBase(uintmax_t num, IntegerBase base, const char * fmt, size_t size)
    {
        switch (base)
        {
        case IntegerBase::DEC:
            printDec(num, fmt);
            break;

        case IntegerBase::BIN:
            // Digits in base 2 = size in bytes * 8
            printBin(num, size * 8);
            break;

        case IntegerBase::HEX:
            // Digits in base 16 = size in bytes * 2
            printHex(num, size * 2);
            break;

        default:
            print("<invalid_base>");
        }
    }

    void PrintableUART::printDec(uintmax_t num, const char * fmt)
    {
        char buffer[64];
        snprintf(buffer, sizeof(buffer), fmt, static_cast<uint32_t>(num));
        print(buffer);
    }

    void PrintableUART::printBin(uintmax_t value, size_t digits)
    {
        // Need 8 digits for every byte
        char buffer[sizeof(value) * 8];

        // Check bounds
        if (digits > sizeof(buffer))
        {
            print("<bin_value_too_big>");
            return;
        }
        // Nothing to do
        if (digits == 0)
            return;

        for (size_t i = 0; i < digits; ++i)
        {
            // Convert bit to '0' or '1'
            // First digit in buffer is MSB in value, so shift from high to low
            buffer[i] = '0' + ((value >> (digits - 1 - i)) & 0x1);
        }

        print(buffer, digits);
    }

    void PrintableUART::printHex(uintmax_t value, size_t digits)
    {
        // Need 2 digits for every byte
        char buffer[sizeof(value) * 2];

        // Check bounds
        if (digits > sizeof(buffer))
        {
            print("<hex_value_too_big>");
            return;
        }
        // Nothing to do
        if (digits == 0)
            return;

        for (size_t i = 0; i < digits; ++i)
        {
            // Convert 4 bits to hex
            // First digit in buffer is 4 MSBs in value, so shift from high to low
            uint8_t hex = ((value >> ((digits - 1 - i) * 4)) & 0xF);
            if (hex > 9)
                buffer[i] = 'A' + (hex - 10);
            else
                buffer[i] = '0' + hex;
        }

        print(buffer, digits);
    }
} // namespace sta