#include <stdlib.h>
#include <malloc.h>
#include <pspkernel.h>
#include <pspdisplay.h>
#include <psputils.h>
#include <png.h>
#include <pspgu.h>
#include <psptypes.h>
#include <psprtc.h>
#include "GUblit.h"

extern u32* g_vram_base;
Color* g_vram_base = (Color*) (0x40000000 | 0x04000000);


#define IS_ALPHA(color) (((color)&0xff000000)==0xff000000?0:1)
#define FRAMEBUFFER_SIZE (PSP_LINE_SIZE*SCREEN_HEIGHT*4)
#define MAX(X, Y) ((X) > (Y) ? (X) : (Y))

typedef struct
{
//	unsigned short u, v;
	unsigned int color;
	short x, y, z;
} Vertex;

extern u8 msx[];

unsigned int __attribute__((aligned(16))) list[262144];
static int dispBufferNumber;
static int initialized = 0;

static int getNextPower2(int width)
{
	int b = width;
	int n;
	for (n = 0; b != 0; n++) b >>= 1;
	b = 1 << n;
	if (b == 2 * width) b >>= 1;
	return b;
}

Color* getVramDrawBuffer()
{
	Color* vram = (Color*) g_vram_base;
	if (dispBufferNumber == 0) vram += FRAMEBUFFER_SIZE / sizeof(Color);
	return vram;
}

Color* getVramDisplayBuffer()
{
	Color* vram = (Color*) g_vram_base;
	if (dispBufferNumber == 1) vram += FRAMEBUFFER_SIZE / sizeof(Color);
	return vram;
}

void user_warning_fn(png_structp png_ptr, png_const_charp warning_msg)
{
}


Image* loadPngImageImpl(png_structp png_ptr)
{
	unsigned int sig_read = 0;
	png_uint_32 width, height, x, y;
	int bit_depth, color_type, interlace_type;
	u32* line;
	png_infop info_ptr;
	info_ptr = png_create_info_struct(png_ptr);
	if (info_ptr == NULL) {
		png_destroy_read_struct(&png_ptr, png_infopp_NULL, png_infopp_NULL);
		return NULL;
	}
	png_set_sig_bytes(png_ptr, sig_read);
	png_read_info(png_ptr, info_ptr);
	png_get_IHDR(png_ptr, info_ptr, &width, &height, &bit_depth, &color_type, &interlace_type, int_p_NULL, int_p_NULL);
	if (width > 512 || height > 512) {
		png_destroy_read_struct(&png_ptr, png_infopp_NULL, png_infopp_NULL);
		return NULL;
	}
	Image* image = (Image*) malloc(sizeof(Image));
	image->imageWidth = width;
	image->imageHeight = height;
	image->textureWidth = getNextPower2(width);
	image->textureHeight = getNextPower2(height);
	png_set_strip_16(png_ptr);
	png_set_packing(png_ptr);
	if (color_type == PNG_COLOR_TYPE_PALETTE) png_set_palette_to_rgb(png_ptr);
	if (color_type == PNG_COLOR_TYPE_GRAY && bit_depth < 8) png_set_gray_1_2_4_to_8(png_ptr);
	if (png_get_valid(png_ptr, info_ptr, PNG_INFO_tRNS)) png_set_tRNS_to_alpha(png_ptr);
	png_set_filler(png_ptr, 0xff, PNG_FILLER_AFTER);
	image->data = (Color*) memalign(16, image->textureWidth * image->textureHeight * sizeof(Color));
	if (!image->data) {
		free(image);
		png_destroy_read_struct(&png_ptr, png_infopp_NULL, png_infopp_NULL);
		return NULL;
	}
	line = (u32*) malloc(width * 4);
	if (!line) {
		free(image->data);
		free(image);
		png_destroy_read_struct(&png_ptr, png_infopp_NULL, png_infopp_NULL);
		return NULL;
	}
	for (y = 0; y < height; y++) {
		png_read_row(png_ptr, (u8*) line, png_bytep_NULL);
		for (x = 0; x < width; x++) {
			u32 color = line[x];
			image->data[x + y * image->textureWidth] =  color;
		}
	}
	free(line);
	png_read_end(png_ptr, info_ptr);
	png_destroy_read_struct(&png_ptr, &info_ptr, png_infopp_NULL);
			image->isSwizzled = 0;

	return image;
}

Image* loadPng(const char* filename)
{
	png_structp png_ptr;
	FILE *fp;

	if ((fp = fopen(filename, "rb")) == NULL) return NULL;
	png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
	if (png_ptr == NULL) {
		fclose(fp);
		return NULL;;
	}
	png_init_io(png_ptr, fp);
	Image* image = loadPngImageImpl(png_ptr);
	fclose(fp);
    image->isSwizzled = 0;

	return image;
}

typedef struct {
	const unsigned char *data;
	png_size_t size;
	png_size_t seek;
} PngData;
	
static void ReadPngData(png_structp png_ptr, png_bytep data, png_size_t length)
{
png_size_t i;
	PngData *pngData = (PngData*) png_get_io_ptr(png_ptr);
	if (pngData) {
		for (i = 0; i < length; i++) {
			if (pngData->seek >= pngData->size) break;
			data[i] = pngData->data[pngData->seek++];
		}
	}
}

Image* loadImageFromMemory(const unsigned char* data, int len)
{
	if (len < 8) return NULL;
	
		png_structp png_ptr;
		png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
		if (png_ptr == NULL) {
			return NULL;;
		}
		PngData pngData;
		pngData.data = data;
		pngData.size = len;
		pngData.seek = 0;
		png_set_read_fn(png_ptr, (void *) &pngData, ReadPngData);
		Image* image = loadPngImageImpl(png_ptr);
		image->isSwizzled = 0;
		return image;

}

void swizzle_fast(Image *img)
{   u32 *out=NULL;
   unsigned int blockx, blocky;
   unsigned int j;
 
   unsigned int width_blocks = (img->textureWidth / 16);
   unsigned int height_blocks = (img->textureHeight / 8);
 
   unsigned int src_pitch = (img->textureWidth-16)/4;
   unsigned int src_row = img->textureWidth * 8;
 
   const u32* ysrc = img->data;
   u32* dst = (u32*)out;
 
   for (blocky = 0; blocky < height_blocks; ++blocky)
   {
      const u32* xsrc = ysrc;
      for (blockx = 0; blockx < width_blocks; ++blockx)
      {
         const u32* src = (u32*)xsrc;
         for (j = 0; j < 8; ++j)
         {
            *(dst++) = *(src++);
            *(dst++) = *(src++);
            *(dst++) = *(src++);
            *(dst++) = *(src++);
            src += src_pitch;
         }
         xsrc += 16;
     }
     ysrc += src_row;
   }
   		img->isSwizzled = 1;

free(img->data);
//img->data = valloc(sizeof(u32)*img->textureWidth*img->textureHeight);
img->data = malloc(sizeof(u32)*img->textureWidth*img->textureHeight);
memcpy(img->data, dst, sizeof(u32)*img->textureWidth*img->textureHeight);
free(out);
}


void drawSprite(int sx, int sy, int width, int height, Image* source, int dx, int dy)
{
	if (!initialized) return;

	sceKernelDcacheWritebackInvalidateAll();
	if (source->isSwizzled == 1)
		sceGuTexMode(GU_PSM_8888, 0, 0, GU_TRUE);

	sceGuTexImage(0, source->textureWidth, source->textureHeight, source->textureWidth, source->data);
   float u = 1.0f / ((float)source->textureWidth);
	float v = 1.0f / ((float)source->textureHeight);
	sceGuTexScale(u, v);
	
	int j = 0;
	while (j < width) {
		struct BlitVertex {
        unsigned short u,v;
        short x,y,z;
		} *vertices = (struct BlitVertex*) sceGuGetMemory(2 * sizeof(Vertex));
		int sliceWidth = 64;
		if (j + sliceWidth > width) sliceWidth = width - j;
		vertices[0].u = sx + j;
		vertices[0].v = sy;
		vertices[0].x = dx + j;
		vertices[0].y = dy;
		vertices[0].z = 0;
		vertices[1].u = sx + j + sliceWidth;
		vertices[1].v = sy + height;
		vertices[1].x = dx + j + sliceWidth;
		vertices[1].y = dy + height;
		vertices[1].z = 0;
		sceGuDrawArray(GU_SPRITES, GU_TEXTURE_16BIT | GU_VERTEX_16BIT | GU_TRANSFORM_2D, 2, 0, vertices);
		j += sliceWidth;
	}
	
}


Image* createImage(int width, int height)
{
	Image* image = (Image*) malloc(sizeof(Image));
	if (!image) return NULL;
	image->imageWidth = width;
	image->imageHeight = height;
	image->textureWidth = getNextPower2(width);
	image->textureHeight = getNextPower2(height);
	image->data = (Color*) memalign(16, image->textureWidth * image->textureHeight * sizeof(Color));
	if (!image->data) return NULL;
	memset(image->data, 0, image->textureWidth * image->textureHeight * sizeof(Color));
	return image;
}

void freeImage(Image* image)
{
	free(image->data);
	free(image);
}

void clearImage(Color color, Image* image)
{
	int i;
	int size = image->textureWidth * image->textureHeight;
	Color* data = image->data;
	for (i = 0; i < size; i++, data++) *data = color;
}

void clearScreen(Color color)
{
	if (!initialized) return;
	sceGuClearColor(color);
	sceGuClearDepth(0);
	sceGuClear(GU_COLOR_BUFFER_BIT|GU_DEPTH_BUFFER_BIT);
	
}


void drawFilledRect(int x, int y, int width, int height, u32 color){
    sceKernelDcacheWritebackInvalidateAll();
    Vertex* vert = (Vertex*) sceGuGetMemory(2 * sizeof(Vertex));
    vert[0].x = x;
    vert[0].y = y;
    vert[0].z = 1;
    vert[0].color = color;
    vert[1].x = x+width;
    vert[1].y = y+height;
    vert[1].z = 1;
    vert[1].color = color;
    sceGuDisable(GU_TEXTURE_2D);
    sceGuEnable(GU_BLEND);
    sceGuDisable(GU_DEPTH_TEST);
    sceGuBlendFunc(GU_ADD, GU_SRC_ALPHA, GU_ONE_MINUS_SRC_ALPHA, 0, 0);
    sceGuDrawArray(GU_SPRITES, GU_COLOR_8888 | GU_VERTEX_16BIT | GU_TRANSFORM_2D, 2,0,vert);
    sceGuEnable(GU_TEXTURE_2D);
}	


void fadeImage(int inOut, int rate, int delay, Image *img){

	int temp=0;
    rate = rate/2;
    RGBA col;
    col.r = 0;
    col.g = 0;
    col.b = 0;
    col.a = 0;
   if (inOut == IN) col.a = 255;
   if (inOut == OUT) col.a = 0;
   if (inOut == IN_OUT) col.a = 255;
    while(1){
	guStart();
	clearScreen(0);
	drawSprite(0,0,480, 272, img, 0,0);
	drawFilledRect(0, 0, 480, 272, GU_RGBA(col.r, col.g, col.b, col.a));
	
	if (inOut == IN){
	   col.a -= rate;
	 }
	if (inOut == IN_OUT){
		temp = 1;
	   col.a -= rate;
	 }
	if (inOut == OUT){
		col.a += rate;
	}
	
   if ((col.a < 0)){
		sceKernelDelayThread(delay*1000*1000);
		if (inOut == IN_OUT)
			inOut = OUT;
		if (inOut == IN)
			break;
	}
	if ((col.a >255)){
		if (temp != 1)
			sceKernelDelayThread(delay*1000*1000);
		if (inOut == OUT)
			break;
	}
	drawToScreen();
	}
}

void putPixelScreen(Color color, int x, int y)
{
    sceKernelDcacheWritebackInvalidateAll();
	
    Vertex* vert = (Vertex*) sceGuGetMemory(1 * sizeof(Vertex));
    vert[0].x = x;
    vert[0].y = y;
    vert[0].z = 1;
    vert[0].color = color;
    sceGuDisable(GU_TEXTURE_2D);
    sceGuEnable(GU_BLEND);
    sceGuDisable(GU_DEPTH_TEST);
    sceGuDrawArray(GU_POINTS, GU_COLOR_4444 | GU_VERTEX_16BIT | GU_TRANSFORM_2D, 1,0,vert);
    sceGuEnable(GU_TEXTURE_2D);
}

Color getPixelScreen(int x, int y)
{
	Color* vram = getVramDrawBuffer();
	return vram[PSP_LINE_SIZE * y + x];
}

void FPS( void )
{
	static char fpsDisplay[100];
	fps++;
	sceRtcGetCurrentTick( &fpsTickNow );

//printf("tick: %d, %d\n"	,(int)(fpsTickNow-fpsTickLast),tickResolution);
	if( ((fpsTickNow - fpsTickLast)/((float)tickResolution)) >= 1.0f )
	{
		fpsTickLast = fpsTickNow;
		sprintf( fpsDisplay, "FPS: %d", fps );
		fps = 0;
	}
//printf("tick: '%s'\n"	,fpsDisplay);
	
	sceGuFinish();
	sceGuSync(0,0);
	printAt(0,0, fpsDisplay, 0xFFFFFFFF );
	guStart();
 
}


void printAt(int x, int y, const char* text, u32 color)
{
	int i, j, l;
	size_t c;
	u8 *font;
	Color *vram_ptr;
	Color *vram;
	
	if (!initialized) return;

	for (c = 0; c < strlen(text); c++) {
		if (x < 0 || x + 8 > SCREEN_WIDTH || y < 0 || y + 8 > SCREEN_HEIGHT) break;
		char ch = text[c];
		vram = getVramDrawBuffer() + x + y * PSP_LINE_SIZE;
		
		font = &msx[ (int)ch * 8];
		for (i = l = 0; i < 8; i++, l += 8, font++) {
			vram_ptr  = vram;
			for (j = 0; j < 8; j++) {
				if ((*font & (128 >> j))) *vram_ptr = color;
				vram_ptr++;
			}
			vram += PSP_LINE_SIZE;
		}
		x += 8;
	}
}


void savePngImage(const char* filename, Color* data, int width, int height, int lineSize, int saveAlpha)
{
	png_structp png_ptr;
	png_infop info_ptr;
	FILE* fp;
	int i, x, y;
	u8* line;
	
	if ((fp = fopen(filename, "wb")) == NULL) return;
	png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
	if (!png_ptr) return;
	info_ptr = png_create_info_struct(png_ptr);
	if (!info_ptr) {
		png_destroy_write_struct(&png_ptr, (png_infopp)NULL);
		return;
	}
	png_init_io(png_ptr, fp);
	png_set_IHDR(png_ptr, info_ptr, width, height, 8,
		saveAlpha ? PNG_COLOR_TYPE_RGBA : PNG_COLOR_TYPE_RGB,
		PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT);
	png_write_info(png_ptr, info_ptr);
	line = (u8*) malloc(width * (saveAlpha ? 4 : 3));
	for (y = 0; y < height; y++) {
		for (i = 0, x = 0; x < width; x++) {
			Color color = data[x + y * lineSize];
			u8 r = color & 0xff; 
			u8 g = (color >> 8) & 0xff;
			u8 b = (color >> 16) & 0xff;
			u8 a = saveAlpha ? (color >> 24) & 0xff : 0xff;
			line[i++] = r;
			line[i++] = g;
			line[i++] = b;
			if (saveAlpha) line[i++] = a;
		}
		png_write_row(png_ptr, line);
	}
	free(line);
	png_write_end(png_ptr, info_ptr);
	png_destroy_write_struct(&png_ptr, (png_infopp)NULL);
	fclose(fp);
}


void drawToScreen()
{
	if (!initialized) return;
	//int items=
	sceGuFinish();
	//printf("sceGuFinish = %d\n",items);
	sceGuSync(0, 0);
	//printf("sceGuSync\n");
	sceGuSwapBuffers();
	//printf("sceGuSwap\n");
	dispBufferNumber ^= 1;
}

void drawLine(int x0, int y0, int x1, int y1, unsigned int color)
{
    sceKernelDcacheWritebackInvalidateAll();
    sceGuEnable(GU_LINE_SMOOTH);
    Vertex* vert = (Vertex*) sceGuGetMemory(2 * sizeof(Vertex));
    vert[0].x = x0;
    vert[0].y = y0;
    vert[0].z = 0;
    vert[0].color = color;
    vert[1].x = x1;
    vert[1].y = y1;
    vert[1].z = 0;
    vert[1].color = color;
    sceGuDisable(GU_TEXTURE_2D);
    sceGuEnable(GU_BLEND);
    sceGuDisable(GU_DEPTH_TEST);
    sceGuDrawArray(GU_LINES, GU_COLOR_4444 | GU_VERTEX_16BIT | GU_TRANSFORM_2D, 6,0,vert);
    sceGuEnable(GU_TEXTURE_2D);
}

#define BUF_WIDTH (512)
#define SCR_WIDTH (480)
#define SCR_HEIGHT (272)
#define PIXEL_SIZE (4) /* change this if you change to another screenmode */
#define FRAME_SIZE (BUF_WIDTH * SCR_HEIGHT * PIXEL_SIZE)
#define ZBUF_SIZE (BUF_WIDTH SCR_HEIGHT * 2) /* zbuffer seems to be 16-bit? */

void initGU()
{
	dispBufferNumber = 0;

	sceGuInit();

	guStart();
	sceGuDrawBuffer(GU_PSM_8888, (void*)FRAMEBUFFER_SIZE, PSP_LINE_SIZE);
	sceGuDispBuffer(SCREEN_WIDTH, SCREEN_HEIGHT, (void*)0, PSP_LINE_SIZE);
	sceGuDepthBuffer((void*) (FRAMEBUFFER_SIZE*2), PSP_LINE_SIZE);
	sceGuClear(GU_COLOR_BUFFER_BIT | GU_DEPTH_BUFFER_BIT);
	sceGuOffset(2048 - (SCREEN_WIDTH / 2), 2048 - (SCREEN_HEIGHT / 2));
	sceGuViewport(2048, 2048, SCREEN_WIDTH, SCREEN_HEIGHT);
	sceGuDepthRange(0xc350, 0x2710);
	sceGuScissor(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
	sceGuEnable(GU_SCISSOR_TEST);
	sceGuAlphaFunc(GU_GREATER, 0, 0xff);
	sceGuEnable(GU_ALPHA_TEST);
	sceGuDepthFunc(GU_GEQUAL);
	//sceGuEnable(GU_DEPTH_TEST);
	sceGuDisable(GU_DEPTH_TEST);
	sceGuFrontFace(GU_CW);
	sceGuShadeModel(GU_SMOOTH);
	sceGuEnable(GU_CULL_FACE);
	sceGuEnable(GU_TEXTURE_2D);
	sceGuEnable(GU_CLIP_PLANES);
	sceGuTexMode(GU_PSM_8888, 0, 0, 0);
	sceGuTexFunc(GU_TFX_REPLACE, GU_TCC_RGBA);
	sceGuTexFilter(GU_NEAREST, GU_NEAREST);
	sceGuAmbientColor(0xffffffff);
	sceGuEnable(GU_BLEND);
	sceGuBlendFunc(GU_ADD, GU_SRC_ALPHA, GU_ONE_MINUS_SRC_ALPHA, 0, 0);
	sceGuFinish();
	sceGuSync(0, 0);

	sceDisplayWaitVblankStart();
	sceGuDisplay(GU_TRUE);
	initialized = 1;
	
    
	sceRtcGetCurrentTick( &fpsTickLast );// for the FPS
	tickResolution = sceRtcGetTickResolution();
}

void disableGU()
{
	initialized = 0;
	sceGuTerm();
}

void guStart()
{
	sceGuStart(GU_DIRECT, list);
}
