#define CHAR_WIDTH 8
#define CHAR_HEIGHT 12

static int xpos=0;
static int ypos=0;
static unsigned long attr=0x07;
static unsigned short *vidmem=0xB8000;

void cls();
void settextcolor(int bg,int fg);
void setcursorpos(int x,int y);
void screen_init();
void putch(char ch);
void puts(const char *str);

/* Read a byte from an I/O port */
static unsigned char screen_inb(unsigned short port)
{
	unsigned char ret;
	__asm__ __volatile__ ("inb %1, %0" : "=a" (ret) : "dN" (port));
	return ret;
}
/* Write a byte to an I/O port */
static void screen_outb(unsigned short port,unsigned char data)
{
	__asm__ __volatile__ ("outb %1, %0" : : "dN" (port), "a" (data));
}

static void screen_memsetw(short *ptr,unsigned short set_to,unsigned long nWords)
{
	while(nWords)
		*ptr++=set_to;
}
static void screen_memcpy(char *to,char *from,unsigned long nBytes)
{
	while(nBytes)
		*to++=*from++;
}

void screen_init()
{
	xpos=0;
	ypos=0;
	settextcolor(0,7);
	cls();
}

void cls()
{
	screen_memsetw((short*)vidmem,0x0720,2000);
	setcursorpos(0,0);
}

void kscreen_scroll()
{
	screen_memcpy((char*)vidmem,(char*)(vidmem+80),3840);
	screen_memsetw((short*)(vidmem+1920),0x0720,80);
}

void kscreen_cursor_update()
{
	unsigned short posval;
	posval=(ypos*80)+xpos;
	outb(0x3D4,14);
	outb(0x3D5,posval >> 8);
	outb(0x3D4,15);
	outb(0x3D5,(unsigned char)posval);
}

void kscreen_newline()
{
	if(ypos>=24)
	{
		kscreen_scroll();
		ypos=24;
	}
	else
		ypos++;
	xpos=0;
	kscreen_cursor_update();
}

void kscreen_advance()
{
	xpos++;
	if(xpos>=80)
	{
		xpos=0;
		kscreen_newline();
	}
	kscreen_cursor_update();
}

void kscreen_back()
{
	xpos--;
	if(xpos<0)
	{
		if(ypos>0)
		{
			xpos=79;
			ypos--;
		}
		else
			xpos=0;
	}
	kscreen_cursor_update();
}

void setcursorpos(int x,int y)
{
	xpos=x;
	ypos=y;
	kscreen_cursor_update();
}

void putch(char ch)
{
	if(ch=='\r')
		/* Do nothing */;
	else if(ch=='\n')
		kscreen_newline();
	else if(ch=='\b')
		kscreen_back();
	else
	{
		*(vidmem+(ypos*80)+xpos)=(attr<<8) | ch;
		kscreen_advance();
	}
}

void puts(const char *str)
{
	for(;*str;str++) putch(*str);
}

void settextcolor(int bg,int fg)
{
	attr=((bg & 0x0f) << 8) | (fg & 0x0f);
}