#include <stdio.h>
#include <conio.h>
#include <dos.h>
#include <string.h>
#include <stdlib.h>

typedef unsigned char uint8;
typedef unsigned uint16;
typedef unsigned long uint32;

struct vbeInfo {
	char Signature[4];
	uint16 Version;
	uint32 OemString;
	uint8 Capabilities[4];
	uint32 VideoModes;
	uint16 TotalMem;
};

struct modeInfo {
	uint16 attributes;
	uint8 winA,winB;
	uint16 granularity;
	uint16 winsize;
	uint16 segmentA, segmentB;
	uint32 realFctPtr;
	uint16 pitch; // uint8s per scanline

	uint16 Xres, Yres;
	uint8 Wchar, Ychar, planes, bpp, banks;
	uint8 memory_model, bank_size, image_pages;
	uint8 reserved0;

	uint8 red_mask, red_position;
	uint8 green_mask, green_position;
	uint8 blue_mask, blue_position;
	uint8 rsv_mask, rsv_position;
	uint8 directcolor_attributes;

	uint32 physbase;  // your LFB address ;)
	uint32 reserved1;
	uint16 reserved2;
};

uint16 GetInfo (vbeInfo far* info)
{
	REGS regs;
	SREGS segregs;
	
	regs.x.ax = 0x4f00;
	regs.x.di = FP_OFF(info);
	segregs.es = FP_SEG(info);
	int86x(0x10, &regs, &regs, &segregs);
	
	return regs.x.ax;
}

uint16 GetModeInfo (modeInfo far* info, uint16 mode)
{
	REGS regs;
	SREGS segregs;
	
	regs.x.ax = 0x4f01;
	regs.x.cx = mode;
	regs.x.di = FP_OFF(info);
	segregs.es = FP_SEG(info);
	int86x(0x10, &regs, &regs, &segregs);
	
	return regs.x.ax;
}

void SetMode (uint16 mode)
{
	REGS regs;
	
	regs.x.ax = 0x4F02;
	regs.x.bx = mode;
	int86(0x10, &regs, &regs);
}

uint16 bank_current = 0xffff;
void SwitchBank (uint16 bank)
{
	if (bank == bank_current) return;
	
	REGS regs;
	regs.x.ax = 0x4F05;
	regs.x.bx = 0;
	regs.x.dx = bank;
	int86(0x10, &regs, &regs);
	
	bank_current = bank;
}

modeInfo* CurrentMode;

void PutPixel (int x, int y, uint16 color)
{
	unsigned address = (y * CurrentMode->Xres) + x;
	unsigned bank_size = CurrentMode->granularity * 1024;
	unsigned bank_number = address / bank_size;
	unsigned bank_offset = address % bank_size;
	
	SwitchBank (bank_number);
	
	uint16 far* ptr = (uint16 far*) (bank_offset + 0xA0000l);
	*ptr = color;
}

void SetBestMode (uint16 X, uint16 Y, uint8 bpp)
{
	vbeInfo* info = (vbeInfo*) malloc (sizeof(vbeInfo) * 2);
	modeInfo* modeinf = (modeInfo*) malloc (sizeof(modeInfo) * 4);
	
	strncpy(info->Signature, "VBE2", 4);
	
	if (GetInfo(info) != 0x4F) {
		printf("Error\n"); return;
	}
		
	uint16 far* modes = (uint16 far*) info->VideoModes;
	uint16 best = 0x13;
	uint16 bestpixdiff = (320 * 200) - (X * Y);
	uint16 bestdppdiff = 8 >= bpp ? 8 - bpp : (bpp - 8) * 2;
	
	for (int i = 0; modes[i] != 0xFFFF; i++)
	{
		if (GetModeInfo(modeinf, modes[i]) != 0x4F) continue;	
		if ((modeinf->attributes & 0x90) != 0x90) continue;
		if (modeinf->memory_model != 4 && modeinf->memory_model != 6) continue;
		
		if (X == modeinf->Xres && Y == modeinf->Yres && bpp == modeinf->bpp) {
			best = modes[i]; break;
		}
		
		uint16 pixdiff = (modeinf->Xres * modeinf->Yres) - (X*Y);
		uint16 dppdiff = (modeinf->bpp >= bpp) ? (modeinf->bpp - bpp) : (bpp - modeinf->bpp);
		if ( bestpixdiff > pixdiff || (bestpixdiff == pixdiff && bestdppdiff > dppdiff) )
		{
			best = modes[i];
			bestpixdiff = pixdiff;
			bestdppdiff = dppdiff;
		}
	}
	
	CurrentMode = modeinf;
	GetModeInfo(CurrentMode, best);
	printf("Best mode found: %ux%u %ubpp\n", CurrentMode->Xres, CurrentMode->Yres, CurrentMode->bpp);

	// Set mode
	SetMode (best);
	
	free (info);
}

int main()
{
	SetBestMode(800, 600, 15);
	
	for (int i = 0; i < 700; i++)
		for (int j = 0; j < 500; j++)
			PutPixel (i, j, 0x7FFF - i - j);
	
	free (CurrentMode);
	return 0;
}