/** \file
    Graphics-related functions

    Copyright (C) 2007 - 2008 Jeffrey P.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
**/
#include "pspcomic.h"

inline void* access_pixel(const SDL_Surface* surf, const Uint16 x, const Uint16 y) {
	return (void*)((char*)surf->pixels + x*surf->format->BytesPerPixel + surf->pitch * y);
}
inline void copy_pixel(const void *src, void *dst, Uint8 bpp) {
	#ifdef PSP
	while(bpp--) *((char*)dst++) = *((char*)src++);
	#else
	memcpy(dst,src,bpp);
	#endif
}
inline void blend(void *pixel, SDL_PixelFormat *fmt, Uint8 r, Uint8 g, Uint8 b, Uint8 a) {
	Uint8 old_r, old_g, old_b, old_a;
	Uint8 new_r, new_g, new_b, new_a;
	switch(fmt->BytesPerPixel) {
		case 1:
			SDL_GetRGBA(*(Uint8*)pixel,fmt,&old_r,&old_g,&old_b,&old_a);
		break;
		case 2:
			SDL_GetRGBA(*(Uint16*)pixel,fmt,&old_r,&old_g,&old_b,&old_a);
		break;
		case 3: {
			Uint32 buffer;
			buffer = ((Uint8*)pixel)[0] | (((Uint8*)pixel)[1] << 8) | (((Uint8*)pixel)[2] << 16);
			SDL_GetRGBA(buffer,fmt,&old_r,&old_g,&old_b,&old_a);
		} break;
		case 4:
			SDL_GetRGBA(*(Uint32*)pixel,fmt,&old_r,&old_g,&old_b,&old_a);
		break;
	}
	new_r = (Uint8) ((((Uint32)old_r << 8) + ((Uint32)r * a)) / (256 + a));
	new_g = (Uint8) ((((Uint32)old_g << 8) + ((Uint32)g * a)) / (256 + a));
	new_b = (Uint8) ((((Uint32)old_b << 8) + ((Uint32)b * a)) / (256 + a));
	new_a = a + old_a;
	Uint32 new_color = SDL_MapRGBA(fmt,new_r,new_g,new_b,new_a);
	switch(fmt->BytesPerPixel) {
		case 1: *(Uint8*)pixel = (Uint8)new_color; break;
		case 2: *(Uint16*)pixel = (Uint16)new_color; break;
		case 3: {
			((Uint8*)pixel)[0] = new_color;
			((Uint8*)pixel)[1] = new_color >> 8;
			((Uint8*)pixel)[2] = new_color >> 16;
		} break;
		case 4: *(Uint32*)pixel = (Uint32)new_color; break;
	}
}

#ifndef USE_GFX
SDL_Surface* resize(SDL_Surface *old, SDL_Rect *clip, Uint16 newW, Uint16 newH) {
	SDL_Rect src_rect;
	if(clip == NULL) {
		src_rect.x = 0;
		src_rect.y = 0;
		src_rect.w = old->w;
		src_rect.h = old->h;
	} else src_rect = *clip;
	if((newW == 0) && (newH != 0)) newW = (Uint16)(((float)newH/src_rect.h)*old->w);
	else if((newH == 0) && (newW != 0)) newH = (Uint16)(((float)newW/src_rect.w)*old->h);
	else if((newW == 0) && (newH == 0)) return old;
	if((clip == NULL) && (newW == old->w) && (newH == old->h)) return old;
	int current_resize_method = *access_int_global(access_resize);
	if((newW/old->w >= 2) && (newH/old->h >= 2)) current_resize_method = resize_nn;
	SDL_Surface *new_surf = SDL_CreateRGBSurface(
		SDL_SWSURFACE,
		newW, newH,
		((current_resize_method == resize_nn) || (old->format->BytesPerPixel >= 3)) ? old->format->BitsPerPixel :
		((old->format->BytesPerPixel == 1) && (old->flags & SDL_SRCCOLORKEY) && (current_resize_method == resample))? 32 : 24,
		(old->format->BytesPerPixel == 1)?0x00FF0000:old->format->Rmask,
		(old->format->BytesPerPixel == 1)?0x0000FF00:old->format->Gmask,
		(old->format->BytesPerPixel == 1)?0x000000FF:old->format->Bmask,
		(((old->format->BytesPerPixel == 1) && (old->flags & SDL_SRCCOLORKEY) && (current_resize_method == resample)) ||
		(old->format->BytesPerPixel == 4))?0xFF000000:0 //Not sure why it has to be done this way, but it does...
	);
	if(!new_surf) {
		sdl_error();
		return NULL;
	}
	if((new_surf->format->BytesPerPixel == 1) &&
		  !(copy_palette(old, new_surf) == 1)) {
		SDL_FreeSurface(new_surf);
		sdl_error();
		return NULL;
	}
	/* if(new_surf->format->BytesPerPixel == 4)
		SDL_SetAlpha(new_surf,SDL_SRCALPHA,SDL_ALPHA_TRANSPARENT); */
	SDL_LockSurface(old);
	SDL_LockSurface(new_surf);
	Uint32 i, j;
	Uint8 *new_pixel, *row_start;
	switch(current_resize_method) {
		case resize_nn: {
			Uint32 locationX, locationY;
			Uint8 bpp = new_surf->format->BytesPerPixel;
			Uint8 *old_pixel;
			new_pixel = new_surf->pixels;
			for(i = 0; i < newH; ++i) {
				row_start = new_pixel;
				for(j = 0; j < newW; ++j,new_pixel+=bpp) {
					locationX = (j<<16)/newW*(src_rect.w) + (src_rect.x<<16);
					locationY = (i<<16)/newH*(src_rect.h) + (src_rect.y<<16);
					old_pixel = access_pixel(old,(Uint16)(locationX>>16),(Uint16)(locationY>>16));
					copy_pixel(old_pixel,new_pixel,bpp);
				}
				new_pixel = row_start + new_surf->pitch;
			}
		} break;
		/* Bilinear interpolation. Doesn't work yet due to limitations of the compiler
		case resample: {
			float locationX, locationY;
			float cornerX02, cornerX13, cornerY01, cornerY23;
			float *corners = memalign(VFPU_ALIGNMENT,sizeof(float)*4);
			float value;
			float locationX1, locationY1;
			struct pspvfpu_context *ctx;
			ctx = pspvfpu_initcontext();
			pspvfpu_use_matrices(ctx, VMAT0|VMAT1|VMAT2, 0);
			SDL_Surface *buffer = NULL;
			if(old->format->BytesPerPixel < 3) {
				buffer = old;
				old = SDL_CreateRGBSurface(
					SDL_SWSURFACE,
					buffer->w, buffer->h, 32,
					0x00FF0000, 0x0000FF00,
					0x000000FF, 0xFF000000
				);
				SDL_SetAlpha(old,SDL_SRCALPHA,SDL_ALPHA_TRANSPARENT);
				SDL_UnlockSurface(buffer);
				SDL_BlitSurface(buffer,NULL,old,NULL);
				SDL_UpdateRect(old,0,0,0,0);
				SDL_LockSurface(old);
			}
			Uint8 k = new_surf->format->BytesPerPixel;
			Uint8 *old_pixel0, *old_pixel1;
			Uint8 *old_pixel2, *old_pixel3;
			for(i = newH; i--;) {
				new_pixel = access_pixel(new_surf,newW-1,i);
				for(j = newW; j--; new_pixel -= k*2-1) {
					locationX = ((float)j)/newW*(src_rect.w-1.0f) + (src_rect.x);
					locationY = ((float)i)/newH*(src_rect.h-1.0f) + (src_rect.y);
					cornerX02 = floorf(locationX);
					cornerX13 = cornerX02+1.0f;
					cornerY01 = floorf(locationY);
					cornerY23 = cornerY01+1.0f;
					old_pixel0 = access_pixel(old,(Uint16)cornerX02,(Uint16)cornerY01);
					old_pixel1 = old_pixel0+k;
					old_pixel2 = old_pixel0+old->pitch;
					old_pixel2 = old_pixel0;
					old_pixel3 = old_pixel2+k;
					locationX -= cornerX02;
					cornerX13 = 1.0f;
					locationY -= cornerY01;
					cornerY23 = 1.0f;
					corners[0] = *old_pixel0;
					corners[1] = *old_pixel1;
					corners[2] = *old_pixel2;
					corners[3] = *old_pixel3;
					locationX1 = 1 - locationX;
					locationY1 = 1 - locationY;

					asm volatile(
						"mtv %1, R000				\n"
						"mtv %2, R001				\n"
						"mtv %3, C100				\n"
						"mtv %4, C101				\n"
						"lv.q %5, M200				\n"
						"vmul.p R000, R000, M200	\n"
						"vmul.p S200, R000, C100	\n"
						"mfv %0, S200				\n"
						: "=r" (value) :
						"r" (locationX1), "r" (locationX),
						"r" (locationY1), "r" (locationY),
						"r" (corners));
					
					*new_pixel = (Uint8)value;

					corners[0] = *(old_pixel0+1);
					corners[1] = *(old_pixel1+1);
					corners[2] = *(old_pixel2+1);
					corners[3] = *(old_pixel3+1);

					asm volatile(
						"mtv %1, R000				\n"
						"mtv %2, R001				\n"
						"mtv %3, C100				\n"
						"mtv %4, C101				\n"
						"lv.q %5, M200				\n"
						"vmul.p R000, R000, M200	\n"
						"vmul.p S200, R000, C100	\n"
						"mfv %0, S200				\n"
						: "=r" (value) :
						"r" (locationX1), "r" (locationX),
						"r" (locationY1), "r" (locationY),
						"r" (corners));
					
					*(new_pixel+1) = (Uint8)value;

					corners[0] = *(old_pixel0+2);
					corners[1] = *(old_pixel1+2);
					corners[2] = *(old_pixel2+2);
					corners[3] = *(old_pixel3+2);

					asm volatile(
						"mtv %1, R000				\n"
						"mtv %2, R001				\n"
						"mtv %3, C100				\n"
						"mtv %4, C101				\n"
						"lv.q %5, M200				\n"
						"vmul.p R000, R000, M200	\n"
						"vmul.p S200, R000, C100	\n"
						"mfv %0, S200				\n"
						: "=r" (value) :
						"r" (locationX1), "r" (locationX),
						"r" (locationY1), "r" (locationY),
						"r" (corners));
					
					*(new_pixel+2) = (Uint8)value;

					if(k > 3) {
						corners[0] = *(old_pixel0+3);
						corners[1] = *(old_pixel1+3);
						corners[2] = *(old_pixel2+3);
						corners[3] = *(old_pixel3+3);
	
						asm volatile(
							"mtv %1, R000				\n"
							"mtv %2, R001				\n"
							"mtv %3, C100				\n"
							"mtv %4, C101				\n"
							"lv.q %5, M200				\n"
							"vmul.p R000, R000, M200	\n"
							"vmul.p S200, R000, C100	\n"
							"mfv %0, S200				\n"
							: "=r" (value) :
							"r" (locationX1), "r" (locationX),
							"r" (locationY1), "r" (locationY),
							"r" (corners));
						
						*(new_pixel+3) = (Uint8)value;
					}
				}
			}
			if(buffer) {
				SDL_FreeSurface(old);
				old = NULL;
			}
			pspvfpu_deletecontext(ctx);
			free(corners);
		} break;
		*/
		case resample: {
			Uint32 locationX, locationY;
			Uint16 cornerX0, cornerX1, cornerY0, cornerY1;
			Uint8 *old_pixel0, *old_pixel1;
			SDL_Surface *buffer = NULL;
			if(old->format->BytesPerPixel < 3) {
				buffer = old;
				old = SDL_CreateRGBSurface(
					SDL_SWSURFACE,
					buffer->w, buffer->h, 32,
					0x00FF0000, 0x0000FF00,
					0x000000FF, 0xFF000000
				);
				SDL_SetAlpha(old,SDL_SRCALPHA,SDL_ALPHA_TRANSPARENT);
				SDL_UnlockSurface(buffer);
				SDL_BlitSurface(buffer,NULL,old,NULL);
				SDL_UpdateRect(old,0,0,0,0);
				SDL_LockSurface(old);
			}
			Uint8 k = new_surf->format->BytesPerPixel;
			Uint16 val;
			new_pixel = access_pixel(new_surf,newW-1,newH-1);
			for(i = newH; i--;) {
				row_start = new_pixel;
				for(j = newW; j--; new_pixel -= k*2-1) {
					locationX = (j<<16)/newW*(src_rect.w-1) + (src_rect.x<<16);
					locationY = (i<<16)/newH*(src_rect.h-1) + (src_rect.y<<16);
					cornerX0 = (Uint16)(locationX>>16);
					cornerY0 = (Uint16)(locationY>>16);
					cornerX1 = cornerX0;
					cornerY1 = cornerY0;
					if(locationX-(cornerX0<<16)) ++cornerX1;
					if(locationY-(cornerY0<<16)) ++cornerY1;
					old_pixel0 = access_pixel(old,cornerX0,cornerY0);
					old_pixel1 = access_pixel(old,cornerX1,cornerY1);
					/* //SLOW! But at least it works now
					asm volatile(
						"lbu $t0, 0(%0)		\n"
						"lbu $t1, 0(%1)		\n"
						"add $t0, $t0, $t0	\n"
						"srl $t0, $t0, 1	\n"
						"sb  $t0, 1(%2)		\n"
						"addi $t2, %3, -1	\n"

						"lbu $t0, 1(%0)		\n"
						"lbu $t1, 1(%1)		\n"
						"add $t0, $t0, $t0	\n"
						"srl $t0, $t0, 1	\n"
						"sb  $t0, 1(%2)		\n"
						"addi $t2, $t2, -1	\n"

						"lbu $t0, 2(%0)		\n"
						"lbu $t1, 2(%1)		\n"
						"add $t0, $t0, $t0	\n"
						"srl $t0, $t0, 1	\n"
						"sb  $t0, 2(%2)		\n"
						"addi $t2, $t2, -1	\n"
						"blez $t2, resample_done \n"
						"sb  $t0, 1(%2)		\n"

						"lbu $t0, 3(%0)		\n"
						"lbu $t1, 3(%1)		\n"
						"add $t0, $t0, $t0	\n"
						"srl $t0, $t0, 1	\n"
						"sb  $t0, 3(%2)		\n"
						"resample_done:		\n"
						: : "r" (old_pixel1), "r" (old_pixel0), "r" (new_pixel), "r" (k));
					*/
					//Basically, this averages two pixels' values and puts it
					//into the new image.
					val = ((Uint16)(*(old_pixel0))+
						(Uint16)(*(old_pixel1)))>>1;
					*((Uint8*)new_pixel) = (Uint8)val;
					//Do it up to four times, and also pre-increment the
					//variables so that the pointer is right immediately
					val = ((Uint16)(*(++old_pixel0))+
						(Uint16)(*(++old_pixel1)))>>1;
					*((Uint8*)++new_pixel) = (Uint8)val;
					val = ((Uint16)(*(++old_pixel0))+
						(Uint16)(*(++old_pixel1)))>>1;
					*((Uint8*)++new_pixel) = (Uint8)val;
					if(k == 4) {
						val = ((Uint16)(*(++old_pixel0))+
							(Uint16)(*(++old_pixel1)))>>1;
						*((Uint8*)++new_pixel) = (Uint8)val;
					}
				}
				new_pixel = row_start - new_surf->pitch;
			}
			if(buffer) {
				SDL_FreeSurface(old);
				old = NULL;
			}
		} break;
	}
	if(old) SDL_UnlockSurface(old);
	SDL_UnlockSurface(new_surf);
	return new_surf;
}
SDL_Surface* rotate(SDL_Surface *old, char amount) {
	while(amount < 0) amount += 4;
	amount %= 4;
	if(amount == 0) return old;
	SDL_Surface *new_surf = SDL_CreateRGBSurface(
		SDL_SWSURFACE,
		(amount & 1?old->h:old->w), (amount & 1?old->w:old->h),
		old->format->BitsPerPixel,
		old->format->Rmask, old->format->Gmask, old->format->Bmask, old->format->Amask
	);
	if(!new_surf ||
	  ((new_surf->format->BytesPerPixel == 1) && !(copy_palette(old, new_surf) == 1))
	) {
		sdl_error();
		return NULL;
	} else if(new_surf->format->BytesPerPixel == 1) copy_palette(old, new_surf);
	SDL_LockSurface(old);
	SDL_LockSurface(new_surf);
	Uint16 i, j;
	void *old_pixel, *new_pixel;
	Uint8 bpp = old->format->BytesPerPixel;
	Uint32 pitch = old->pitch;
	for(i = 0; i < new_surf->w; ++i) {
		switch(amount) {
			case 1: old_pixel = access_pixel(old,0,old->h-i-1); break;
			case 2: old_pixel = access_pixel(old,old->w-i-1,old->h-1); break;
			case 3: old_pixel = access_pixel(old,old->w-1,i); break;
		}
		for(j = 0; j < new_surf->h; ++j) {
			new_pixel = access_pixel(new_surf,i,j);
			copy_pixel(old_pixel,new_pixel,bpp);
			switch(amount) {
				case 1: old_pixel = (char*)old_pixel + bpp; break;
				case 2: old_pixel = (char*)old_pixel - pitch; break;
				case 3: old_pixel = (char*)old_pixel - bpp; break;
			}
		}
	}
	SDL_UnlockSurface(old);
	SDL_UnlockSurface(new_surf);
	return new_surf;
}
#endif

SDL_Surface* rotozoom(SDL_Surface *old, SDL_Rect *clip, Uint16 newW, Uint16 newH, const char rotation) {
	#ifndef USE_GFX
	SDL_Surface *buffer = resize(old,clip,newW,newH);
	if(!buffer) return NULL;
	SDL_Surface *buffer2 = rotate(buffer,rotation);
	if((buffer2 != buffer) && (buffer != old)) SDL_FreeSurface(buffer);
	if(!buffer2) return NULL;
	return buffer2;
	#else
	int current_resize_method = *access_int_global(access_resize);
	SDL_Rect temp;
	if(clip == NULL) {
		temp.x = 0;
		temp.y = 0;
		temp.w = old->w;
		temp.h = old->h;
		clip = &temp;
	}
	double x_zoom, y_zoom;
	if((newW == 0) && (newH != 0)) x_zoom = y_zoom= newH/(double)clip->h;
	else if((newH == 0) && (newW != 0)) x_zoom = y_zoom= newW/(double)clip->w;
	else if((newW == 0) && (newH == 0)) return x_zoom = y_zoom = 1;
	else {
		x_zoom= newW/(double)clip->w;
		y_zoom= newH/(double)clip->h;
	}
	if((clip == &temp) && (x_zoom == 1) && (y_zoom == 1) && (rotation == 0))
		return old;
	SDL_Surface *buffer;
	if(clip == &temp) buffer = old;
	else {
		buffer = SDL_CreateRGBSurface(
			SDL_SWSURFACE,
			clip->w, clip->h,
			((current_resize_method == resize_nn) || (old->format->BytesPerPixel >= 3)) ? old->format->BitsPerPixel :
			((old->format->BytesPerPixel == 1) && (old->flags & SDL_SRCCOLORKEY) && (current_resize_method == resample))? 32 : 24,
			(old->format->BytesPerPixel == 1)?0x00FF0000:old->format->Rmask,
			(old->format->BytesPerPixel == 1)?0x0000FF00:old->format->Gmask,
			(old->format->BytesPerPixel == 1)?0x000000FF:old->format->Bmask,
			(((old->format->BytesPerPixel == 1) && (old->flags & SDL_SRCCOLORKEY) && (current_resize_method == resample)) ||
			(old->format->BytesPerPixel == 4))?0xFF000000:0 //Not sure why it has to be done this way, but it does...
		);
		if(!buffer) {
			sdl_error();
			return NULL;
		}
		if((buffer->format->BytesPerPixel == 1) &&
			  !(copy_palette(old, buffer) == 1)) {
			SDL_FreeSurface(buffer);
			sdl_error();
			return NULL;
		}
	}
	SDL_Rect all = { 0, 0, buffer->w, buffer->h };
	if(SDL_BlitSurface(old,clip,buffer,&all) != 0) {
		sdl_error();
		return NULL;
	}
	SDL_UpdateRect(buffer,0,0,0,0);
	return rotozoomSurfaceXY(buffer,(rotation%4)*(-90.0),x_zoom,y_zoom,(current_resize_method != resize_nn));
	#endif
}
int copy_palette(SDL_Surface *old, SDL_Surface *new_surf) {
	if((old->format->BytesPerPixel != 1) || (new_surf->format->BytesPerPixel != 1))
		return -1;
	SDL_Palette *pal = old->format->palette;
	SDL_Color *colors = malloc(sizeof(SDL_Color)*pal->ncolors);
	if(!colors) {
		error_message(get_message(mesg_out_of_memory));
		return -2;
	}
	memcpy(colors,pal->colors,pal->ncolors*sizeof(SDL_Color));
	if(old->flags & SDL_SRCCOLORKEY)
		SDL_SetColorKey(new_surf, SDL_SRCCOLORKEY|SDL_RLEACCEL, old->format->colorkey);
	return SDL_SetPalette(new_surf, SDL_LOGPAL|SDL_PHYSPAL, colors, 0, pal->ncolors);
}
void blend_rect(SDL_Surface *surf, SDL_Rect rect, Uint8 r, Uint8 g, Uint8 b, Uint8 a) {
	if((rect.w == 0) || (rect.h == 0)) return;
	SDL_LockSurface(surf);
	Uint16 i;
	void *pixel;
	pixel = access_pixel(surf,rect.x,rect.y);
	for(i = 0; i < rect.w; i++) {
		blend(pixel,surf->format,r,g,b,a);
		pixel += surf->format->BytesPerPixel;
	}
	if(rect.h > 1) {
		pixel = access_pixel(surf,rect.x,rect.y+1);
		for(i = 1; i < rect.h; i++) {
			blend(pixel,surf->format,r,g,b,a);
			pixel += surf->pitch;
		}
		if(rect.w > 1) {
			pixel = access_pixel(surf,rect.x+rect.w-1,rect.y+1);
			for(i = 1; i < rect.h; i++) {
				blend(pixel,surf->format,r,g,b,a);
				pixel += surf->pitch;
			}
			pixel = access_pixel(surf,rect.x+1,rect.y+rect.h-1);
			for(i = 1; i < rect.w-1; i++) {
				blend(pixel,surf->format,r,g,b,a);
				pixel += surf->format->BytesPerPixel;
			}
		}
	}
	SDL_UnlockSurface(surf);
}
