#include "draw.h"

Color new_Color() {
	Color c = (Color) 0;
	c.grey = 0xF;
	return c;
}

Color new_Color_RGB(char r, char g, char b) {
	Color c = new_Color();
	c.r = r; c.g = g; c.b = b;
	c.grey = (((255-r) + ((255-g)<<1) + (255-b))>>6) & 0xF;
	return c;
}

Color new_Color_Gray(char g) {
	Color c = new_Color();
	c.grey = g;
	g <<= 4;
	c.r = g; c.g = g; c.b = g;
	return c;
}

Color RGB2Color(char r, char g, char b) {
	Color c = new_Color();
	c.r = r; c.g = g; c.b = b;
	c.grey = ((r + (g<<1) + b)>>6) & 0xF;
	return c;
}

MandelPoint new_MandelPoint(Color color, int k, double x, double y) {
	MandelPoint m = {.color = color, .k = k, .x = x, .y = y};
	return m;
}

inline void setPixel(int x, int y, Color c) {
	if(x < 0 || x >= 320 || y < 0 || y >= 240) return;
	if(is_cx) {
		unsigned short* p = (unsigned short*)(SCREEN_BASE_ADDRESS + (x << 1) + (y << 9) + (y << 7));
		*p = ((c.b) >> 3) | (((c.g) >> 2) << 5) | (((c.r) >> 3) << 11);
	}
	else {
		unsigned char* p = (unsigned char*)(SCREEN_BASE_ADDRESS  + ((x >> 1) + (y << 7) + (y << 5)));
		*p = (x & 1) ? ((*p & 0xF0) | c.grey) : ((*p & 0x0F) | (c.grey << 4));
	}
}

inline Color getPixel(int x, int y) {
	if(x < 0 || x >= 320 || y < 0 || y >= 240) return new_Color_Gray(0);
	if(is_cx) {
		unsigned short v = *((unsigned short*)(SCREEN_BASE_ADDRESS + (x << 1) + (y << 9) + (y << 7)));
		int r = ((v >> 11) << 3) & 0xFF;
		int g = ((v >> 5) << 2) & 0xFF;
		int b = (v << 3) & 0xFF;
		return new_Color_RGB(r, g, b);
	}
	else {
		unsigned char v = *((unsigned char*)(SCREEN_BASE_ADDRESS  + ((x >> 1) + (y << 7) + (y << 5))));
		return new_Color_Gray((x & 1) ? (v & 0x0F) : ((v & 0xF0) >>4));
	}
}

void clrscr(void) {
	if(lcd_isincolor())
		memset(SCREEN_BASE_ADDRESS, 0x00, SCREEN_BYTES_SIZE);
	else
		memset(SCREEN_BASE_ADDRESS, 0xFF, SCREEN_BYTES_SIZE);
}


void line(int x1, int y1, int x2, int y2, Color color){
	int x=x1, y=y1, Dx=sabs(x2-x1), Dy=sabs(y2-y1), fake, i;
	char xinc, yinc;
	xinc=(x1<x2)?1:-1;
	yinc=(y1<y2)?1:-1;
	
	if(Dx>Dy){
		fake = Dx/2;
		for(i=0;i<Dx;i++){
			x+=xinc;
			fake+= Dy;
			if(fake>Dx){
				fake-=Dx;
				y+=yinc;
			}
			setPixel(x,y,color);
		}
	}
	else{
		fake = Dy/2;
		for(i=0;i<Dy;i++){
			y+=yinc;
			fake+= Dx;
			if(fake>Dy){
				fake-=Dy;
				x+=xinc;
			}
			setPixel(x,y,color);
		}
	}
}

void malloc_screen_tab(void) {
	unsigned int j;
	screen_tab = malloc(240 * sizeof(MandelPoint*));
	for(j = 0; j < 240; j ++)
		screen_tab[j] = malloc(320 * sizeof(MandelPoint));
}

void free_screen_tab(void) {
	unsigned int j;
	for(j = 0; j < 240; j ++)
		free(screen_tab[j]);
	free(screen_tab);
}

void fill_screen_tab(MandelPoint point) {
	unsigned int i, j;
	for(i = 0; i < 320; i ++)
		for(j = 0; j < 240; j ++)
			screen_tab[j][i] = point;
}

void invalidate_screen_tab(void) {
	unsigned int i, j;
	for(i = 0; i < 320; i ++)
		for(j = 0; j < 240; j ++)
			screen_tab[j][i].color = (Color)0;	
}

void copyto_screen_tab(unsigned x1, unsigned y1, unsigned x2, unsigned y2) {
	unsigned int i, j;
	for(i = x1; i < x2; i ++)
		for(j = y1; j < y2; j ++)
			screen_tab[j][i] = new_MandelPoint(getPixel(i, j), 0, 0, 0);
}

void apply_screen_tab(unsigned x, unsigned y, int origin, MandelPoint new_point) { /* origin 0 = screen origin, origin 1 = screen_tab origin */
	unsigned i, j;
	MandelPoint **screen_tab_copy = malloc(240 * sizeof(MandelPoint*));
	for(j = 0; j < 240; j ++) {
		screen_tab_copy[j] = malloc(320 * sizeof(MandelPoint));
		for(i = 0; i < 320; i++)
			screen_tab_copy[j][i] = screen_tab[j][i];
	}
	copyto_screen_tab(0, 0, 320, 240);
	/* origin = 0 => x;y are taken in the screen_tab buffer */
	if(origin == 0) {
		for(i = x; i < 320; i ++)
			for(j = y; j < 240; j ++)
				setPixel(i-x, j-y, screen_tab[j][i].color);
		fill_screen_tab(new_point);
		for(i = x; i < 320; i ++)
			for(j = y; j < 240; j ++)
				screen_tab[j-y][i-x] = screen_tab_copy[j][i];
	}
	/* origin = 1 => x;y are taken in the screen buffer */
	else {
		for(i = x; i < 320; i ++)
			for(j = y; j < 240; j ++)
				setPixel(i, j, screen_tab[j-y][i-x].color);
		fill_screen_tab(new_point);
		for(i = x; i < 320; i ++)
			for(j = y; j < 240; j ++)
				screen_tab[j][i] = screen_tab_copy[j-y][i-x];
	}
	
	for(j = 0; j < 240; j ++)
		free(screen_tab_copy[j]);
	free(screen_tab_copy);
}

void init_pointeur(void) {
	pointeur_tab = malloc(CURSOR_HEIGHT * sizeof(Color*));
	unsigned int i, j;
	for(j = 0; j < CURSOR_HEIGHT; j ++) {
		pointeur_tab[j] = malloc(CURSOR_WIDTH * sizeof(Color));
		for(i = 0; i < CURSOR_WIDTH; i ++) {
			cr[j][i] = new_Color_Gray(cr_old[j][i]);
			pointeur_tab[j][i] = getPixel(i, j);
		}
	}
}

void fill_pointer_tab(unsigned x, unsigned y) {
	unsigned int w, h;
	for(w = 0; w < CURSOR_WIDTH; w++)
		for(h = 0;h < CURSOR_HEIGHT; h++)
			pointeur_tab[h][w] = getPixel(w+x,h+y);
}

void apply_pointer_tab(unsigned x, unsigned y) {
	unsigned int w, h;
	for(w = 0; w < CURSOR_WIDTH; w++)
		for(h = 0; h < CURSOR_HEIGHT; h++)
			setPixel(w+x, h+y, pointeur_tab[h][w]);
}

void pointeur(int x, int y)
{
	unsigned int i,j;
	for(i = 0; i < CURSOR_WIDTH; i ++)
		for(j = 0; j < CURSOR_HEIGHT; j ++)
			if (cr[j][i].grey != 15) setPixel(x+i, y+j, cr[j][i]);
}

void mandel(double xmin, double xmax, double ymin, double ymax, int iter, double zx, double zy, int mode){
	// Taille de l'écran en px
	double xcoo = 320.0, ycoo = 239.0;
	// Incrément x et y	
	double xinc = (xmax-xmin)/xcoo, yinc = (ymax-ymin)/ycoo;
	double i, j, xt, x, y, m;
	double xfact = xcoo/(xmax-xmin), yfact = ycoo/(ymax-ymin);
	int k;
	Color black = (Color)0;
	Color white = new_Color_Gray(15);
	for(i = xmin; i < xmax; i += xinc){
		int px = (i-xmin)*xfact;
		for(j = ymin; j < ymax; j += yinc){
			int py = (ymax-j)*yfact;
			MandelPoint backup = screen_tab[py][px];
			if(backup.color.color == black.color) {
				x = backup.x; y = backup.y; k = backup.k; m = 0;
				if(k >= 0) {
					double a, b;
					if(mode == JULIA) {
						if(k == 0) x = i, y = j; 
						a = zx, b = zy;
					}
					else {
						a = i, b = j;
					}					
					while(k < iter && m < 4.0){	// Tant que |z|<2
						m = (x*x+y*y);
						xt = x;					// On sauvegarde le résultat de x avant de
						x = x*x - y*y + a;			// faire le calcul de x+1
						y =  2.0*xt*y + b;			// et y+1
						k++;					// Incrémente le nombre de sortie (itérations)
					}
					Color color;
				// Le point apparatient à l'ensemble alors il est dessiné en noir
					if (m < 4.0)
						color = black;
				// Sinon on l'affiche en fonction de son nombre de sortie (k) en partant du blanc
					else
						color = new_Color_RGB(k<<3, k<<4, k<<2);
					setPixel(px, py, color);
					if(backup.k == 0) { // Enleve les glitchs
						setPixel(px+1, py, color);
						setPixel(px, py+1, color);
					}
					screen_tab[py][px] = new_MandelPoint(white, color.color == black.color ? iter : -1, x, y); // on note le pixel comme "fait"
				}
			}
		}
		// Si jamais une de ces touches est active, on arrtte tout
		if(getEvt()) break;
		//line(px + 3, 0, px + 3, 239, new_Color_RGB(255, 255, 255));

	}
}