#include "LedMatrix.h"

LedMatrix::LedMatrix(uint8_t h, uint8_t w)
    : height(h), width(w), num_leds(h * w), leds(num_leds)
{
    addLeds<WS2812B, MATRIX_PIN, RGB>(leds.data(), num_leds);
}

LedMatrix::~LedMatrix()
{
}

void LedMatrix::drawPixel(int x, int y, CRGB color)
{
    if (x >= 0 && x < 64 && y >= 0 && y < 64)
    {
        int index = y * 64 + x; // Row-major order
        leds[index] = color;
    }
}

void LedMatrix::drawChar(int x, int y, char c, CRGB color)
{
    u_int8_t char_width_pixel = getCharWidth(c);
    // Serial.printf("Char '%c' -> width %d\n\r", c, char_width_pixel);

    const uint8_t *fontData = getFontChar(c);
    for (int row = 0; row < 7; row++)
    {
        uint8_t line = fontData[row];
        for (int col = 0; col < 5 /*char_width_pixel*/; col++)
        {
            drawPixel(x + col, y + row, CRGB::SlateGrey);
            if (line & (1 << (4 - col)))
            {
                drawPixel(x + col, y + row, color);
            }
        }
    }
}

void LedMatrix::_drawChar_(int x, int y, char c, CRGB color)
{
    u_int8_t char_width_pixel = getCharWidth(c);
    // Serial.printf("Char '%c' -> width %d\n\r", c, char_width_pixel);

    const uint8_t *fontData = getFontChar(c);
    for (int row = 0; row < 7; row++)
    {
        uint8_t line = fontData[row];
        for (int col = 5 - char_width_pixel; col < 5; col++)
        {
            drawPixel(x + col, y + row, CRGB::SlateGrey);
            if (line & (1 << (4 - col)))
            {
                drawPixel(x + col, y + row, CRGB::GhostWhite);
            }
        }
    }
}

// Draw text string
void LedMatrix::drawText(int x, int y, const char *text, CRGB color)
{
    int cursorX = x;
    for (int i = 0; text[i] != '\0'; i++)
    {
        drawChar(cursorX, y, text[i], color);
        _drawChar_(cursorX, 8 + y, text[i], color);
        cursorX += 6; // 5 pixels width + 1 pixel spacing
    }
}

vector<uint8_t> LedMatrix::number_to_bitarray_msb(uint16_t number, int bits)
{
    /** Convert 8/16-bit number to bit array (MSB first) */
    std::vector<uint8_t> byte;
    byte.reserve(bits);

    for (int i = bits - 1; i >= 0; i--)
    {
        byte.push_back((number >> i) & 1);
    }

    // Debug, Ausgabe des Bytes
    Serial.printf("0x%02X\n\r", number);
    uint8_t idx = 0;
    for (auto bit : byte)
    {
        Serial.printf("%d", bit);
        idx++;
        if (!(idx % 4))
            Serial.printf(" ");
    }
    Serial.println();

    return byte;
}