#include <types.h>
#include <mm/paging.h>
#include <stdio.h>
#include <port_io.h>
#include <math.h>

#include <drivers/graphics/vga.h>

#define VBE_DISPI_IOPORT_INDEX 0x01CE
#define VBE_DISPI_IOPORT_DATA  0x01CF

#define VBE_DISPI_INDEX_ID           0x0
#define VBE_DISPI_INDEX_XRES         0x1
#define VBE_DISPI_INDEX_YRES         0x2
#define VBE_DISPI_INDEX_BPP          0x3
#define VBE_DISPI_INDEX_ENABLE       0x4
#define VBE_DISPI_INDEX_BANK         0x5
#define VBE_DISPI_INDEX_VIRT_WIDTH   0x6
#define VBE_DISPI_INDEX_VIRT_HEIGHT  0x7
#define VBE_DISPI_INDEX_X_OFFSET     0x8
#define VBE_DISPI_INDEX_Y_OFFSET     0x9

#define VBE_DISPI_DISABLED 0x00
#define VBE_DISPI_ENABLED  0x01
#define VBE_DISPI_LFB_ENABLED 0x40

static bool bga_init = false;
static bool bga_error = false;

uint32_t current_bpp = 0;
uint32_t current_xres = 0;
uint32_t current_yres = 0;

static struct bga_framebuffer bga_fb;

void init_bga_framebuffer(uint32_t* _p)
{
	uint32_t width  = 1024;
	uint32_t height = 768;
	uint32_t bpp    = 32;

	uint32_t pitch = width * (bpp / 8);

	uint32_t fb_size = pitch * height;

	fb_size = (fb_size + 0xFFF) & ~0xFFF;

	uint8_t* bp = (uint8_t*) _p;

	for (uint32_t off = 0; off < fb_size; off += 0x1000)
	{
		map_page(bp + off, bp + off);
	}


	bga_fb.addr = _p;
	bga_fb.bpp = bpp;
	bga_fb.height = height;
	bga_fb.width = width;
	bga_fb.pitch = pitch;
	
	bga_init = true;
}

uint8_t* bga_get_framebuffer(void)
{
	return bga_fb.addr;
}

void bga_write_register(uint16_t index, uint16_t data)
{
	outw(VBE_DISPI_IOPORT_INDEX, index);
    outw(VBE_DISPI_IOPORT_DATA, data);
}

uint16_t bga_read_register(uint16_t index)
{
    outw(VBE_DISPI_IOPORT_INDEX, index);
    return inw(VBE_DISPI_IOPORT_DATA);
}

void bga_set_video_mode(uint32_t width, uint32_t height, uint32_t bit_depth)
{
	bga_write_register(VBE_DISPI_INDEX_ENABLE, VBE_DISPI_DISABLED);
	bga_write_register(VBE_DISPI_INDEX_XRES, width);
	bga_write_register(VBE_DISPI_INDEX_YRES, height);
	bga_write_register(VBE_DISPI_INDEX_BPP, bit_depth);
	bga_write_register(VBE_DISPI_INDEX_ENABLE, VBE_DISPI_ENABLED | VBE_DISPI_LFB_ENABLED);

	/* now we check to see if any info we gave was rejected, in which case everything would be the way it was before we wrote the configuration info. */
	uint16_t bpp = bga_read_register(VBE_DISPI_INDEX_BPP);
	if (bpp != bit_depth)
	{
		bga_error = true;
	}
	else
	{
		current_bpp = bit_depth;
		current_xres = width;
		current_yres = height;
	}
}

void bga_defaults(void)
{
    // Disable device while changing settings
    bga_write_register(VBE_DISPI_INDEX_ENABLE, VBE_DISPI_DISABLED);

    // Set resolution
    bga_write_register(VBE_DISPI_INDEX_XRES, 1024);
    bga_write_register(VBE_DISPI_INDEX_YRES, 768);
    bga_write_register(VBE_DISPI_INDEX_BPP, 32);

    // Enable graphics + linear framebuffer
    bga_write_register(
        VBE_DISPI_INDEX_ENABLE,
        VBE_DISPI_ENABLED | VBE_DISPI_LFB_ENABLED
    );
}

/*void bga_putpixel(uint32_t x, uint32_t y, uint32_t color)
{
	if (current_bpp == 16)
	{
		uint16_t* fb = (uint16_t*) bga_framebuffer;
		uint32_t idx = (y * current_xres + x) * 2;

		*(fb + idx) = (uint16_t) color;
	}
}*/


void bga_putpixel(uint32_t x, uint32_t y, uint32_t color)
{
    uint32_t bytes_per_pixel = bga_fb.bpp / 8;

    uint8_t* pixel = (uint8_t*)bga_fb.addr + (y * bga_fb.pitch) + (x * bytes_per_pixel);

    *(volatile uint32_t*)pixel = color;
}

void bga_drawline(int x0, int y0, int x1, int y1, uint32_t color)
{
    int dx = abs(x1 - x0);
    int dy = abs(y1 - y0);

    int sx = (x0 < x1) ? 1 : -1;
    int sy = (y0 < y1) ? 1 : -1;

    int err = dx - dy;

    while (1)
    {
        bga_putpixel(x0, y0, color);

        if (x0 == x1 && y0 == y1)
            break;

        int e2 = err * 2;

        if (e2 > -dy)
        {
            err -= dy;
            x0 += sx;
        }

        if (e2 < dx)
        {
            err += dx;
            y0 += sy;
        }
    }
}