#include "VGA.hpp"

#include <cstdint>

using namespace drv;

VGA::VGA()
    : miscPort(0x3c2),
      crtcIndexPort(0x3d4),
      crtcDataPort(0x3d5),
      sequencerIndexPort(0x3c4),
      sequencerDataPort(0x3c5),
      graphicsControllerIndexPort(0x3ce),
      graphicsControllerDataPort(0x3cf),
      attrControllerIndexPort(0x3c0),
      attrControllerReadPort(0x3c1),
      attrControllerWritePort(0x3c0),
      attrControllerResetPort(0x3da)
{
}

void
VGA::writeRegisters(std::uint8_t *registers)
{
    // Misc.
    miscPort.write(*(registers++));

    // Sequencer.
    for (std::uint8_t i = 0; i < 5; ++i) {
        sequencerIndexPort.write(i);
        sequencerDataPort.write(*(registers++));
    }

    // Cathode ray tube controller.
    crtcIndexPort.write(0x03);
    crtcDataPort.write(crtcDataPort.read() | 0x80);
    crtcIndexPort.write(0x11);
    crtcDataPort.write(crtcDataPort.read() & ~0x80);

    registers[0x03] = registers[0x03] | 0x80;
    registers[0x11] = registers[0x11] & ~0x80;

    for (std::uint8_t i = 0; i < 25; ++i) {
        crtcIndexPort.write(i);
        crtcDataPort.write(*(registers++));
    }

    // Graphics controller.
    for (std::uint8_t i = 0; i < 9; ++i) {
        graphicsControllerIndexPort.write(i);
        graphicsControllerDataPort.write(*(registers++));
    }

    // Attribute controller.
    for (std::uint8_t i = 0; i < 21; ++i) {
        attrControllerResetPort.read();
        attrControllerIndexPort.write(i);
        attrControllerWritePort.write(*(registers++));
    }

    attrControllerResetPort.read();
    attrControllerIndexPort.write(0x20);
}

bool
VGA::supportsMode(const Mode &mode)
{
    return mode.width == 320 && mode.height == 200 && mode.colorDepth == 8;
}

bool
VGA::setMode(const Mode &mode)
{
    if (!supportsMode(mode)) {
        return false;
    }

    std::uint8_t mode_320x200x256[] = {
        // MISC
        0x63,
        // SEQ
        0x03, 0x01, 0x0f, 0x00, 0x0e,
        // CRTC
        0x5f, 0x4f, 0x50, 0x82, 0x54, 0x80, 0xbf, 0x1f,
        0x00, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x9c, 0x0e, 0x8f, 0x28, 0x40, 0x96, 0xb9, 0xa3,
        0xff,
        // GC
        0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0f,
        0xff,
        // AC
        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
        0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
        0x41, 0x00, 0x0f, 0x00, 0x00
    };

    writeRegisters(mode_320x200x256);

    currentMode = mode;
    return true;
}

std::uint8_t *
VGA::getFrameBufferSegment()
{
    graphicsControllerIndexPort.write(0x06);
    std::uint8_t segmentNumber = graphicsControllerDataPort.read() & (3 << 2);
    switch (segmentNumber) {
        default:
        case 0 << 2: return reinterpret_cast<std::uint8_t *>(0x00000);
        case 1 << 2: return reinterpret_cast<std::uint8_t *>(0xa0000);
        case 2 << 2: return reinterpret_cast<std::uint8_t *>(0xb0000);
        case 3 << 2: return reinterpret_cast<std::uint8_t *>(0xb8000);
    }
}

void
VGA::beginDrawing()
{
    for (int i = 0; i < currentMode.width*currentMode.height; ++i) {
        buffer[i] = 0;
    }
}

void
VGA::endDrawing()
{
    std::uint8_t *pixelAddress = getFrameBufferSegment();
    for (int i = 0; i < currentMode.width*currentMode.height; ++i) {
        *pixelAddress++ = buffer[i];
    }
}

void
VGA::putPixel(Position pos, Color color)
{
    putPixel(pos, getColorIndex(color));
}

void
VGA::fillRectangle(Position pos, Dimentions size, Color color)
{
    for (int y = pos.y; y < pos.y + size.h; ++y) {
        for (int x = pos.x; x < pos.x + size.w; ++x) {
            putPixel(drv::Position { x, y }, color);
        }
    }
}

std::uint8_t
VGA::getColorIndex(Color color)
{
    if (color.r == 0x00 && color.g == 0x00 && color.b == 0x00) {
        return 0x00; // black
    }
    if (color.r == 0x00 && color.g == 0x00 && color.b == 0xa8) {
        return 0x01; // blue
    }
    if (color.r == 0x00 && color.g == 0xa8 && color.b == 0x00) {
        return 0x02; // green
    }
    if (color.r == 0xa8 && color.g == 0x00 && color.b == 0x00) {
        return 0x04; // red
    }
    if (color.r == 0xff && color.g == 0xff && color.b == 0xff) {
        return 0x3f; // white
    }
    return 0x00;
}

void
VGA::putPixel(Position pos, std::uint8_t colorIndex)
{
    if (pos.x < 0 || pos.x >= currentMode.width ||
        pos.y < 0 || pos.y >= currentMode.height) {
        return;
    }

    std::uint8_t *pixelAddress = buffer + currentMode.width*pos.y + pos.x;
    *pixelAddress = colorIndex;
}