#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "libsys.h"
#include "mips_gfx.h"

#define USE_HW_BLITTER


static vga_hw_t *g_pRegs;

// -------------------------------------------------------------
// Functions
// -------------------------------------------------------------
void __gfx_block_set_8(uint32_t* pDst, uint32_t value)
{
	__asm__ volatile
	(
		"sw   %[value], 0(%[pDst])\n"
		"sw   %[value], 4(%[pDst])\n"
		"sw   %[value], 8(%[pDst])\n"
		"sw   %[value], 12(%[pDst])\n"
		"sw   %[value], 16(%[pDst])\n"
		"sw   %[value], 20(%[pDst])\n"
		"sw   %[value], 24(%[pDst])\n"
		"sw   %[value], 28(%[pDst])\n"
		:
		: [pDst] "r" (pDst), [value] "r" (value)
	);
}

void __gfx_block_copy_8(uint32_t* pDst, uint32_t* pSrc)
{
	 __asm__ volatile
	(
		".set noreorder\n"
		".set nomacro\n"
		"lw   $s0, 0(%[pSrc])\n"
		"lw   $s1, 4(%[pSrc])\n"
		"sw   $s0, 0(%[pDst])\n"
		"lw   $s0, 8(%[pSrc])\n"
		"sw   $s1, 4(%[pDst])\n"
		"lw   $s1, 12(%[pSrc])\n"
		"sw   $s0, 8(%[pDst])\n"
		"lw   $s0, 16(%[pSrc])\n"
		"sw   $s1, 12(%[pDst])\n"
		"lw   $s1, 20(%[pSrc])\n"
		"sw   $s0, 16(%[pDst])\n"
		"lw   $s0, 24(%[pSrc])\n"
		"sw   $s1, 20(%[pDst])\n"
		"lw   $s1, 28(%[pSrc])\n"
		"sw   $s0, 24(%[pDst])\n"
		"sw   $s1, 28(%[pDst])\n"
		".set macro\n"
		".set reorder\n"
		:
		: [pDst] "r" (pDst), [pSrc] "r" (pSrc)
		: "s0", "s1"
	);
}

void _GFX_copy32(uint32_t* pDst, uint32_t* pSrc, uint32_t ndwords)
{
	while (ndwords > 3)
	{
		__gfx_block_copy_8(pDst, pSrc);
		pDst += 8;
		pSrc += 8;
		ndwords -= 8;
	}
	while (ndwords)
	{
		*(pDst++) = *(pSrc++);
		ndwords -= 1;
	}
}

void _GFX_memset32(uint32_t* pDst, uint32_t value, uint32_t ndwords)
{
	while (ndwords > 3)
	{
		__gfx_block_set_8(pDst, value);
		pDst += 8;
		ndwords -= 8;
	}
	while (ndwords)
	{
		*(pDst++) = value;
		ndwords -= 1;
	}
}

void _GFX_copy32_hw(uint32_t* pDst, uint32_t* pSrc, uint32_t ndwords, uint32_t wait_finish)
{
	g_pRegs->pVGA_src0 = (uint32_t)pSrc;
	g_pRegs->pVGA_dst = (uint32_t)pDst;
	g_pRegs->pVGA_src0_dimx = 0;
	g_pRegs->pVGA_dst_dimx = 0;
	g_pRegs->pVGA_nx = ndwords;
	g_pRegs->pVGA_ny = 1;
	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;

	if (!wait_finish)
		return;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);

}

void _GFX_memset32_hw(uint32_t* pDst, uint32_t value, uint32_t ndwords, uint32_t wait_finish)
{
	g_pRegs->pVGA_dst = (uint32_t)pDst;
	g_pRegs->pVGA_src0_dimx = 0;
	g_pRegs->pVGA_dst_dimx = 0;
	g_pRegs->pVGA_nx = ndwords;
	g_pRegs->pVGA_ny = 1;
	g_pRegs->pVGA_color = value;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);

	if (!wait_finish)
		return;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);

}

void _GFX_BLIT_const(uint32_t* pDst, uint32_t xdim_dst, uint32_t xmin, uint32_t ymin, uint32_t xmax, uint32_t ymax, uint32_t value)
{
	uint32_t xmax8, x, y;
	uint32_t *pStart, *pEnd;

	xmax8 = xmax - ((xmax-xmin)%8);
	for (y=ymin; y < ymax; y += xdim_dst)
	{
		pStart = &pDst[xmin + y];
		pEnd = &pDst[xmax8 + y];
		while(pStart < pEnd)
		{
			__gfx_block_set_8(pStart, value);
			pStart += 8;
		}
		// Do remainder
		for (x=xmax8; x < xmax; x++)
		{
			pDst[x + y] = value;
		}
	}
}

void _GFX_BLIT_const_hw(uint32_t* pDst, uint32_t xdim_dst, uint32_t xmin, uint32_t ymin, uint32_t xmax, uint32_t ymax, uint32_t value, uint32_t wait_finish)
{
	uint32_t *pStart, *pEnd;

	pStart = (uint32_t*)&pDst[xmin + ymin*xdim_dst];

	g_pRegs->pVGA_dst = (uint32_t)pStart;
	g_pRegs->pVGA_dst_dimx = xdim_dst;
	g_pRegs->pVGA_nx = xmax - xmin + 1;
	g_pRegs->pVGA_ny = ymax - ymin + 1;
	g_pRegs->pVGA_color = value;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);

	if (!wait_finish)
		return;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);
}

void GFX_DirtyRectSetClean(gfx_t *pObj, buf_t *pBuf)
{
	pBuf->dirty.xmin = pBuf->w;
	pBuf->dirty.xmax = 0;
	pBuf->dirty.ymin = pBuf->h;
	pBuf->dirty.ymax = 0;
	pBuf->is_dirty = 0;
}

void GFX_DirtyRectInit(gfx_t *pObj, buf_t *pBuf, uint32_t w, uint32_t h)
{
	pBuf->w = w;
	pBuf->h = h;
	GFX_DirtyRectSetClean(pObj, pBuf);
}

void GFX_DirtyRectUpdate(gfx_t *pObj, buf_t *pBuf, uint32_t xmin, uint32_t xmax, uint32_t ymin, uint32_t ymax)
{

	if (xmin < pBuf->dirty.xmin)
	{
		pBuf->is_dirty = 1;
		pBuf->dirty.xmin = xmin;
	}

	if (xmax > pBuf->dirty.xmax)
	{
		pBuf->is_dirty = 1;
		pBuf->dirty.xmax = xmax;
	}

	if (ymin < pBuf->dirty.ymin)
	{
		pBuf->is_dirty = 1;
		pBuf->dirty.ymin = ymin;
	}

	if (ymax > pBuf->dirty.ymax)
	{
		pBuf->is_dirty = 1;
		pBuf->dirty.ymax = ymax;
	}

	if (pBuf->is_dirty && ((pBuf->dirty.xmax-pBuf->dirty.xmin+1)%8))
		pBuf->dirty.xmax = pBuf->dirty.xmin + ((pBuf->dirty.xmax-pBuf->dirty.xmin)/8 + 1) * 8; // Make len_x multiple of 8

	pObj->restore.xmin = pBuf->dirty.xmin;
	pObj->restore.xmax = pBuf->dirty.xmax;
	pObj->restore.ymin = pBuf->dirty.ymin;
	pObj->restore.ymax = pBuf->dirty.ymax;

}

void GFX_DirtyRectCopy(gfx_t *pObj, buf_t *pBuf)
{
	uint32_t xmin, xmax, xmax8, ymin, ymax, x, y;
	uint32_t *pDst, *pDst_end;
	uint32_t *pSrc, *pSrc_end;

	xmin = pBuf->dirty.xmin;
	xmax = pBuf->dirty.xmax;

	ymin = pBuf->dirty.ymin;
	ymax = pBuf->dirty.ymax;

	ymin *= pBuf->w;
	ymax *= pBuf->w;

#ifdef USE_HW_BLITTER
	pSrc = (uint32_t*)&pBuf->pLast->pFB[xmin + ymin];
	pDst = (uint32_t*)&pBuf->pFB[xmin + ymin];

	g_pRegs->pVGA_src0 = (uint32_t)pSrc;
	g_pRegs->pVGA_dst = (uint32_t)pDst;
	g_pRegs->pVGA_src0_dimx = (uint32_t)pBuf->w;
	g_pRegs->pVGA_dst_dimx = (uint32_t)pBuf->w;
	g_pRegs->pVGA_nx = (uint32_t)(pBuf->dirty.xmax-pBuf->dirty.xmin);
	g_pRegs->pVGA_ny = (uint32_t)(pBuf->dirty.ymax-pBuf->dirty.ymin);
	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;

#else

	xmax8 = xmax - ((xmax-xmin)%8);
	for (y=ymin; y < ymax; y += pBuf->w)
	{
		pSrc = &pBuf->pLast->pFB[xmin + y];
		pSrc_end = &pBuf->pLast->pFB[xmax8 + y];
		pDst = &pBuf->pFB[xmin + y];
		pDst_end = &pBuf->pFB[xmax8 + y];
		while(pDst < pDst_end)
		{
			__gfx_block_copy_8(pDst, pSrc);
			pDst += 8;
			pSrc += 8;
		}
		// Do remainder
		for (x=xmax8; x < xmax; x++)
		{
			pBuf->pFB[x + y] = pBuf->pLast->pFB[x + y];
		}
	}
#endif

}

void GFX_init(gfx_t *pObj)
{
	int i;
	buf_t *pBuf;
	uint32_t w, h, fps;

	g_pRegs = (vga_hw_t*)SYS_VGA_CTRL;

	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_MSTEN;

	w = Screen_get_resx();
	h = Screen_get_resy();
	fps = Screen_get_fps();

//	printf("VGA screen = %d x %d @ %d fps\n", w, h, fps);

	pObj->w = w;
	pObj->h = h;

	// Init background buffer
	pObj->pBG = (uint32_t*)calloc(w * h, sizeof(buf_t));

	// Init framebuffers
	pObj->pBuf = (buf_t*)calloc(1, sizeof(buf_t));
	pBuf = pObj->pBuf;
	pBuf->pFB = (uint32_t*)calloc((w+8) * h, sizeof(uint32_t));		// w+8 because of dirty rectangle 8 byte min. x-size
	for (i=1; i < GFX_NUM_FRAME_BUFFERS; i++)
	{

		pBuf->pNext = (buf_t*)calloc(1, sizeof(buf_t));
		pBuf->pNext->pLast = pBuf;
		pBuf = pBuf->pNext;
		pBuf->pFB = (uint32_t*)calloc((w+8) * h, sizeof(uint32_t));	// w+8 because of dirty rectangle 8 byte min. x-size
	}
	pObj->pBuf->pLast = pBuf;
	pBuf->pNext = pObj->pBuf;

	pBuf = pObj->pBuf;
	for (i=0; i < GFX_NUM_FRAME_BUFFERS; i++)
	{
		GFX_DirtyRectInit(pObj, pBuf, w, h);

//		fprintf(stderr, "pBuf = %08X, pLast = %08X, pNext = %08X\n", pBuf, pBuf->pLast, pBuf->pNext);
		pBuf = pBuf->pNext;
	}

	g_pRegs->pVGA_front = (uint32_t)pObj->pBuf->pLast->pFB;
	g_pRegs->pVGA_back = (uint32_t)pObj->pBuf->pLast->pFB;
	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_MSTEN;


}

void GFX_set_background(gfx_t *pObj, uint32_t *pImage, uint32_t image_nx, uint32_t image_ny, uint32_t image_scale_x, uint32_t image_scale_y)
{

	uint32_t off_x, off_y;
	uint32_t i, j;
	uint32_t *pDst, *pSrc;
	buf_t *pBuf;

	if (!pImage)
		return;

	off_x = (pObj->w-(image_scale_x*image_nx))/2;
	off_y = (pObj->h-(image_scale_y*image_ny))/2;

#ifdef USE_HW_BLITTER

	pDst = ((uint32_t*)pObj->pBG) + pObj->w*off_y + off_x;
	pSrc = pImage;

	g_pRegs->pVGA_src0 = (uint32_t)pSrc;
	g_pRegs->pVGA_dst = (uint32_t)pDst;
	g_pRegs->pVGA_src0_dimx = (uint32_t)0;
	g_pRegs->pVGA_dst_dimx = (uint32_t)0;
	g_pRegs->pVGA_nx = (uint32_t)image_ny*image_nx;
	g_pRegs->pVGA_ny = (uint32_t)1;
	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;

	pBuf = pObj->pBuf;

	pDst = pBuf->pFB;
	pSrc = pObj->pBG;

	g_pRegs->pVGA_src0 = (uint32_t)pSrc;
	g_pRegs->pVGA_dst = (uint32_t)pDst;
	g_pRegs->pVGA_src0_dimx = (uint32_t)0;
	g_pRegs->pVGA_dst_dimx = (uint32_t)0;
	g_pRegs->pVGA_nx = (uint32_t)image_ny*image_nx;
	g_pRegs->pVGA_ny = (uint32_t)1;
	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_ACTIVE);

#else

	pDst = ((uint32_t*)pObj->pBG) + pObj->w*off_y + off_x;
	pSrc = pImage;

	for (i=0; i < image_ny*image_nx; i += 8)
	{
		__gfx_block_copy_8(pDst, pSrc);
		pDst += 8;
		pSrc += 8;
	}

	pBuf = pObj->pBuf;

	pDst = pBuf->pFB;
	pSrc = pObj->pBG;

	for (i=0; i < image_ny*image_nx; i += 8)
	{
		__gfx_block_copy_8(pDst, pSrc);
		pDst += 8;
		pSrc += 8;
	}

#endif

	do
	{
		GFX_DirtyRectUpdate(pObj, pBuf, 0, pObj->w-1, 0, pObj->h-1);
		pBuf = pBuf->pNext;

	} while(pBuf != pObj->pBuf);

}

void GFX_frame(gfx_t *pObj, uint32_t sync_mode)
{

	// Wait until back buffer becomes front
	if (sync_mode == GFX_FRAME_SYNC)
	{
		while(g_pRegs->pVGA_front == (uint32_t)pObj->pBuf->pFB);
	}

	GFX_copy_front2back(pObj);

	// Invalidate D-Cache after blitting has finished
	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

#ifdef USE_HW_BLITTER
	DCACHE_invalidate_all();
#endif
}

void GFX_show(gfx_t *pObj)
{

	g_pRegs->pVGA_back = (uint32_t)pObj->pBuf->pFB;
	pObj->pBuf = pObj->pBuf->pNext;

}

void GFX_copy_front2back(gfx_t *pObj)
{
	buf_t *pBuf;

	pBuf = pObj->pBuf;

	// Lopp over all frame buffers, except front buffer
	while(pBuf != pObj->pBuf->pLast)
	{
		if (pBuf->is_dirty)
		{
//			fprintf(stderr, "Dirty rect [%08X]: w=%d, h=%d\n", (int)pBuf, pBuf->dirty.xmax - pBuf->dirty.xmin, pBuf->dirty.ymax - pBuf->dirty.ymin);
			GFX_DirtyRectCopy(pObj, pBuf);
			GFX_DirtyRectSetClean(pObj, pBuf);
		}
		pBuf = pBuf->pNext;
	}
}

void GFX_get_FB_front(gfx_t *pObj, uint32_t **ppFB)
{
	*ppFB = pObj->pBuf->pLast->pFB;
}

void box_create(box_t *pObj, uint32_t w, uint32_t h)
{
	pObj->w = w;
	pObj->h = h;
}

void GFX_restore(gfx_t *pObj, box_t *pBox, uint32_t posx, uint32_t posy)
{
	uint32_t xmin, xmax, xmax8, ymin, ymax, x, y, nx, ny;
	uint32_t *pDst, *pDst_end;
	uint32_t *pSrc, *pSrc_end;
	buf_t *pBuf;

	if (pBox)
	{
		xmin = posx;
		xmax = pBox->w + posx - 0;
		if (xmax >= pObj->w)
			xmax = pObj->w - 0;

		ymin = posy;
		ymax = pBox->h + posy - 0;
		if (ymax >= pObj->h)
			ymax = pObj->h - 0;
	}
	else
	{
		xmin = pObj->restore.xmin;
		xmax = pObj->restore.xmax;
		ymin = pObj->restore.ymin;
		ymax = pObj->restore.ymax;
	}
	pObj->restore.xmin = 0;
	pObj->restore.xmax = 0;
	pObj->restore.ymin = 0;
	pObj->restore.ymax = 0;
	pBuf = pObj->pBuf->pNext;

	// Lopp over all frame buffers, except current back buffer
	while(pBuf != pObj->pBuf)
	{
		GFX_DirtyRectUpdate(pObj, pBuf, xmin, xmax, ymin, ymax);
		pBuf = pBuf->pNext;

	}

	nx = xmax - xmin;
	ny = ymax - ymin;

	pBuf = pObj->pBuf;

	ymin *= pObj->w;
	ymax *= pObj->w;

#ifdef USE_HW_BLITTER
	pSrc = (uint32_t*)&pObj->pBG[xmin + ymin];
	pDst = (uint32_t*)&pBuf->pFB[xmin + ymin];

	g_pRegs->pVGA_src0 = (uint32_t)pSrc;
	g_pRegs->pVGA_dst = (uint32_t)pDst;
	g_pRegs->pVGA_src0_dimx = (uint32_t)pObj->w;
	g_pRegs->pVGA_dst_dimx = (uint32_t)pObj->w;
	g_pRegs->pVGA_nx = (uint32_t)nx;
	g_pRegs->pVGA_ny = (uint32_t)ny;
	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;

#else

	xmax8 = xmax - ((xmax-xmin)%8);
	for (y=ymin; y < ymax; y += pBuf->w)
	{
		pSrc = &pObj->pBG[xmin + y];
		pSrc_end = &pObj->pBG[xmax8 + y];
		pDst = &pBuf->pFB[xmin + y];
		pDst_end = &pBuf->pFB[xmax8 + y];
		while(pDst < pDst_end)
		{
			__gfx_block_copy_8(pDst, pSrc);
			pDst += 8;
			pSrc += 8;
		}
		// Do remainder
		for (x=xmax8; x < xmax; x++)
		{
			pBuf->pFB[x + y] = pObj->pBG[x + y];
		}
	}
#endif

}

void GFX_box_draw_direct(gfx_t *pObj, box_t *pBox, uint32_t posx, uint32_t posy, uint32_t color)
{
	uint32_t xmin, xmax, xmax8, ymin, ymax, x, y;
	uint32_t *pFB, *pFB_end;
	buf_t *pBuf;

	pBuf = pObj->pBuf->pLast;

	xmin = posx;
	xmax = pBox->w + posx - 0;
	if (xmax >= pObj->w)
		xmax = pObj->w - 0;

	ymin = posy;
	ymax = pBox->h + posy - 0;
	if (ymax >= pObj->h)
		ymax = pObj->h - 0;

	ymin *= pObj->w;
	ymax *= pObj->w;

	xmax8 = xmax - ((xmax-xmin)%8);
	for (y=ymin; y < ymax; y += pObj->w)
	{
		pFB = &pBuf->pFB[xmin + y];
		pFB_end = &pBuf->pFB[xmax8 + y];
		while(pFB < pFB_end)
		{
			__gfx_block_set_8(pFB, color);
			pFB += 8;
		}
		// Do remainder
		for (x=xmax8; x < xmax; x++)
		{
			pBuf->pFB[x + y] = color;
		}
	}

}

void GFX_box_draw(gfx_t *pObj, box_t *pBox, uint32_t posx, uint32_t posy, uint32_t color)
{
	uint32_t xmin, xmax, xmax8, ymin, ymax, x, y, nx, ny;
	uint32_t *pFB, *pFB_end;
	buf_t *pBuf;

	xmin = posx;
	xmax = pBox->w + posx - 0;
	if (xmax >= pObj->w)
		xmax = pObj->w - 0;

	ymin = posy;
	ymax = pBox->h + posy - 0;
	if (ymax >= pObj->h)
		ymax = pObj->h - 0;

	nx = xmax - xmin;
	ny = ymax - ymin;

//	fprintf(stderr, "GFX_box_draw(): x=%d, y=%d, nx=%d, ny=%d\n", posx, posy, nx, ny);

	pBuf = pObj->pBuf->pNext;

	// Lopp over all frame buffers, except current back buffer
	while(pBuf != pObj->pBuf)
	{
		GFX_DirtyRectUpdate(pObj, pBuf, xmin, xmax-1, ymin, ymax-1);
		pBuf = pBuf->pNext;

	}
	pBuf = pObj->pBuf;

	ymin *= pObj->w;
	ymax *= pObj->w;

#ifdef USE_HW_BLITTER

	pFB = (uint32_t*)&pBuf->pFB[xmin + ymin];

	g_pRegs->pVGA_dst = (uint32_t)pFB;
	g_pRegs->pVGA_dst_dimx = (uint32_t)pObj->w;
	g_pRegs->pVGA_nx = (uint32_t)nx;
	g_pRegs->pVGA_ny = (uint32_t)ny;
	g_pRegs->pVGA_color = color;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);


#else

	xmax8 = xmax - ((xmax-xmin)%8);
	for (y=ymin; y < ymax; y += pObj->w)
	{
		pFB = &pBuf->pFB[xmin + y];
		pFB_end = &pBuf->pFB[xmax8 + y];
		while(pFB < pFB_end)
		{
			__gfx_block_set_8(pFB, color);
			pFB += 8;
		}
		// Do remainder
		for (x=xmax8; x < xmax; x++)
		{
			pBuf->pFB[x + y] = color;
		}
	}

#endif
}

void GFX_box_draw_bg(gfx_t *pObj, box_t *pBox, uint32_t posx, uint32_t posy, uint32_t color)
{
	uint32_t xmin, xmax, xmax8, ymin, ymax, x, y, nx, ny;
	uint32_t *pFB, *pFB_end;

	xmin = posx;
	xmax = pBox->w + posx - 0;
	if (xmax >= pObj->w)
		xmax = pObj->w - 0;

	ymin = posy;
	ymax = pBox->h + posy - 0;
	if (ymax >= pObj->h)
		ymax = pObj->h - 0;

	nx = xmax - xmin;
	ny = ymax - ymin;

	ymin *= pObj->w;
	ymax *= pObj->w;


#ifdef USE_HW_BLITTER

	pFB = (uint32_t*)&pObj->pBG[xmin + ymin];

	g_pRegs->pVGA_dst = (uint32_t)pFB;
	g_pRegs->pVGA_dst_dimx = (uint32_t)pObj->w;
	g_pRegs->pVGA_nx = (uint32_t)nx;
	g_pRegs->pVGA_ny = (uint32_t)ny;
	g_pRegs->pVGA_color = color;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl |= (SYS_VGA_BIT_BLIT_REQ | SYS_VGA_BIT_BLIT_OP_CONSTCOL);

#else

	xmax8 = xmax - ((xmax-xmin)%8);
	for (y=ymin; y < ymax; y += pObj->w)
	{
		pFB = &pObj->pBG[xmin + y];
		pFB_end = &pObj->pBG[xmax8 + y];
		while(pFB < pFB_end)
		{
			__gfx_block_set_8(pFB, color);
			pFB += 8;
		}
		// Do remainder
		for (x=xmax8; x < xmax; x++)
		{
			pObj->pBG[x + y] = color;
		}
	}
#endif

}

void GFX_box_blit_bg(gfx_t *pObj, box_t *pBox, uint32_t pos_x_src, uint32_t pos_y_src, uint32_t pos_x_dst, uint32_t pos_y_dst)
{

	uint32_t x_src, y_src, xmax8_src;
	uint32_t x_dst, y_dst, xmax8_dst;
	uint32_t xmin_src, xmax_src, ymin_src, ymax_src;
	uint32_t xmin_dst, xmax_dst, ymin_dst, ymax_dst;
	uint32_t *pDst, *pDst_end;
	uint32_t *pSrc, *pSrc_end;
	buf_t *pBuf;
	pBuf = pObj->pBuf->pNext;

	xmin_src = pos_x_src;
	xmax_src = pBox->w + pos_x_src - 0;
	if (xmax_src >= pObj->w)
		xmax_src = pObj->w - 0;

	ymin_src = pos_y_src;
	ymax_src = pBox->h + pos_y_src - 0;
	if (ymax_src >= pObj->h)
		ymax_src = pObj->h - 0;

	xmin_dst = pos_x_dst;
	xmax_dst = pBox->w + pos_x_dst - 0;
	if (xmax_dst >= pObj->w)
		xmax_dst = pObj->w - 0;

	ymin_dst = pos_y_dst;
	ymax_dst = pBox->h + pos_y_dst - 0;
	if (ymax_dst >= pObj->h)
		ymax_dst = pObj->h - 0;

	// Lopp over all frame buffers, except current back buffer
	while(pBuf != pObj->pBuf)
	{
		GFX_DirtyRectUpdate(pObj, pBuf, xmin_dst, xmax_dst, ymin_dst, ymax_dst);
		pBuf = pBuf->pNext;

	}
	pBuf = pObj->pBuf;

#ifdef USE_HW_BLITTER
	pSrc = (uint32_t*)&pObj->pBG[xmin_src + ymin_src*pObj->w];
	pDst = (uint32_t*)&pBuf->pFB[xmin_dst + ymin_dst*pObj->w];

	g_pRegs->pVGA_src0 = (uint32_t)pSrc;
	g_pRegs->pVGA_src0_dimx = pObj->w;
	g_pRegs->pVGA_dst = (uint32_t)pDst;
	g_pRegs->pVGA_dst_dimx = pObj->w;
	g_pRegs->pVGA_nx = pBox->w;
	g_pRegs->pVGA_ny = pBox->h;

	while (g_pRegs->pVGA_ctrl & SYS_VGA_BIT_BLIT_BSY);

	g_pRegs->pVGA_ctrl &= ~SYS_VGA_BIT_BLIT_OP_CONSTCOL;
	g_pRegs->pVGA_ctrl |= SYS_VGA_BIT_BLIT_REQ;

#else

	ymin_src *= pObj->w;
	ymax_src *= pObj->w;
	ymin_dst *= pObj->w;
	ymax_dst *= pObj->w;

	y_dst = ymin_dst;
	xmax8_src = xmax_src - ((xmax_src-xmin_src)%8);
	xmax8_dst = xmax_dst - ((xmax_dst-xmin_dst)%8);
	for (y_src=ymin_src; y_src < ymax_src; y_src += pBuf->w)
	{
		pSrc = &pObj->pBG[xmin_src + y_src];
		pSrc_end = &pObj->pBG[xmax8_src + y_src];
		pDst = &pBuf->pFB[xmin_dst + y_dst];
		pDst_end = &pBuf->pFB[xmax8_dst + y_dst];
		while(pDst < pDst_end)
		{
			__gfx_block_copy_8(pDst, pSrc);
			pDst += 8;
			pSrc += 8;
		}
		// Do remainder
		for (x_src=xmax8_src; x_src < xmax_src; x_src++)
		{
			*(pDst++) = *(pSrc++);
		}
		y_dst += pBuf->w;

	}

#endif
}