#include <os.h>
#include <stdint.h>
#include <fdlibm.h>

// Useful defines and structures
typedef struct
{
	int x;
	int y;
} ScreenPoint;

typedef struct
{
	float x;
	float y;
} Vector;

#define POINT_OUT_MAP -1

#define abs(x) (x < 0 ? -x : x)
#define map(x, in_min, in_max, out_min, out_max) ((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

#define nRC_248mul(a,b) ((a*b) >> 8)
#define nRC_248div(a,b) ((a << 8) / b)

// Functions

// Trigonometry

#define DEG2RAD(x) ((float)(x) * M_PI / 180)
#define RAD2DEG(x) ((float)(x) * 180 / M_PI)

#define sinDeg(x) sin(DEG2RAD(x))
#define cosDeg(x) cos(DEG2RAD(x))
#define tanDeg(x) tan(DEG2RAD(x))

// Map-related

uint16_t* nRC_loadBMP(char *path)
{
	int size, offset, i;
	uint16_t *returnValue, color;
	FILE *temp = fopen(path, "rb");
	
	// Check if the file's 2 first char are BM (indicates bitmap)
	if(!(fgetc(temp) == 0x42 && fgetc(temp) == 0x4d))
	{
		printf("Image is not a bitmap\n");
		fclose(temp);
		return NULL;
	}
	
	// Check if the file is in 16-bits-per-pixel format
	fseek(temp, 0x1c, SEEK_SET);
	if(!(fgetc(temp) == 16))
	{
		printf("Wrong format\n");
		fclose(temp);
		return NULL;
	}
	
	// Gets the 4-bytes raw pixels size, situated at 0x22
	fseek(temp, 0x22, SEEK_SET);
	size = fgetc(temp) | (fgetc(temp) << 8) | (fgetc(temp) << 16) | (fgetc(temp) << 24);
	size >>= 1;
	
	// Gets the 4-bytes offset to the start of the pixel table, situated at 0x0a
	fseek(temp, 0x0a, SEEK_SET);
	offset = fgetc(temp) | (fgetc(temp) << 8) | (fgetc(temp) << 16) | (fgetc(temp) << 24);
	
	fseek(temp, offset, SEEK_SET);
	
	returnValue = malloc(size * sizeof(int));
	if(!returnValue)
	{
		printf("Couldn't allocate memory\n");
		fclose(temp);
		return NULL;
	}
	
	for(i = size - 1; i >= 0; i--)
	{
		color = (uint16_t)(fgetc(temp) | (fgetc(temp) << 8));
		returnValue[i] = (is_cx ? color : ((int)( 0.229 * (color >> 11) + 0.587 * ((color >> 6) & 0x1f) + 0.114 * (color & 0x1f)) >> 1));
	}
	fclose(temp);
	return returnValue;
}

int nRC_getTile(int *map, ScreenPoint point, ScreenPoint mapDimensions)
{
	if(point.x >= 0 && point.x < mapDimensions.x
		&& point.y >= 0 && point.y < mapDimensions.y)
	{
		return map[point.y * mapDimensions.x + point.x];
	}
	else return POINT_OUT_MAP;
}

int getTileFront(Vector pos, Vector dir, float speed, int *map, ScreenPoint mapDim)
{	
	ScreenPoint temp;
	int tile;
	temp.x = (int)(pos.x + dir.x * speed);
	temp.y = (int)pos.y;
	tile = nRC_getTile(map, temp, mapDim);
	if(tile) return tile;
	temp.y = (int)(pos.y + dir.y * speed);
	tile = nRC_getTile(map, temp, mapDim);
	return tile;
}

int getTileBehind(Vector pos, Vector dir, float speed, int *map, ScreenPoint mapDim)
{	
	ScreenPoint temp;
	int tile;
	temp.x = (int)(pos.x - dir.x * speed);
	temp.y = (int)pos.y;
	tile = nRC_getTile(map, temp, mapDim);
	if(tile) return tile;
	temp.y = (int)(pos.y - dir.y * speed);
	tile = nRC_getTile(map, temp, mapDim);
	return tile;
}

// Drawing functions

#define R5G6B5(r,g,b) (is_cx ? ((r << 11) | (g << 5) | b) : ((int)( 0.229 * r + 0.587 * (g >> 1) + 0.114 * b) >> 1)

void nRC_setPixel(ScreenPoint pixel, int color, char* buffer)
{
	int temp, r, g, b;
	if(pixel.x < 319 && pixel.x > 0 &&
		pixel.y < 239 && pixel.y > 0)
	{
		if(is_cx)	// color LCD, 16 bpp
		{
			buffer[temp = ((pixel.y * 320 + pixel.x) * 2)]  = color & 0xff;
			buffer[temp + 1] = color >> 8 & 0xff;
		}
		else		// monochrome LCD, 4 bpp
		{
			temp = (pixel.y * 160 + pixel.x/2);
			if(pixel.x & 1)
			{
				buffer[temp] = (buffer[temp] & 0xf0) | (color & 0x0f);
			}
			else
			{
				buffer[temp] = (buffer[temp] & 0x0f) | (color << 4 & 0xf0);
			}
		}
	}
}

#define nRC_drawHorizontalLine(origin, end, constant, color, buffer) nRC_drawLineShared(origin, end, constant, color, buffer, 0)
#define nRC_drawVerticalLine(origin, end, constant, color, buffer) nRC_drawLineShared(origin, end, constant, color, buffer, 1)

void nRC_drawLineShared(int origin, int end, int constant, int color, char *buffer, int side)
{
	ScreenPoint pixel;
	int i,j;
	
	if(constant > 0 && constant < (side ? 319 : 239))
	{
		i = min(origin, end);
		j = max(origin, end);
		
		if(!side)
		{
			pixel.y = constant;
			for(pixel.x = i; pixel.x < j; pixel.x++) nRC_setPixel(pixel, color, buffer);
		}
		else
		{
			pixel.x = constant;
			for(pixel.y = i; pixel.y < j; pixel.y++) nRC_setPixel(pixel, color, buffer);
		}
	}
}

void nRC_drawLine(ScreenPoint pt1, ScreenPoint pt2, int color, char* buffer)
{
    int dx, dy, inx, iny, e;
     
    dx = pt2.x - pt1.x;
    dy = pt2.y - pt1.y;
    inx = dx > 0 ? 1 : -1;
    iny = dy > 0 ? 1 : -1;
 
    dx = abs(dx);
    dy = abs(dy);
     
    if(dx >= dy) {
        dy <<= 1;
        e = dy - dx;
        dx <<= 1;
        while (pt1.x != pt2.x) {
            nRC_setPixel(pt1, color, buffer);
            if(e >= 0) {
                pt1.y += iny;
                e-= dx;
            }
            e += dy; pt1.x += inx;
        }
    } else {
        dx <<= 1;
        e = dx - dy;
        dy <<= 1;
        while (pt1.y != pt2.y) {
            nRC_setPixel(pt1, color, buffer);
            if(e >= 0) {
				pt1.x += inx;
                e -= dy;
            }
            e += dx; pt1.y += iny;
        }
    }
    nRC_setPixel(pt1, color, buffer);
}

void nRC_fillTriangle(ScreenPoint pt1, ScreenPoint pt2, ScreenPoint pt3, int color, char *buffer)
{
	int dx1, dx2, x1, x2, y, i;
	ScreenPoint intermediate;
	
	// Sort points from lowest to highest Y
	if(pt1.y > pt2.y)
	{
		intermediate = pt1;
		pt1 = pt2;
		pt2 = intermediate;
	}
	if(pt2.y > pt3.y)
	{
		intermediate = pt2;
		pt2 = pt3;
		pt3 = intermediate;
	}
	if(pt1.y > pt2.y)
	{
		intermediate = pt1;
		pt1 = pt2;
		pt2 = intermediate;
	}
	
	// dx1 = (x2 - x1) / (y2 - y1)
	dx1 = ((pt2.x - pt1.x) << 8) / ((pt2.y != pt1.y) ? pt2.y - pt1.y : 1);
	// dx2 = (x3 - x1) / (y3 - y1)
	dx2 = ((pt3.x - pt1.x) << 8) / ((pt3.y != pt1.y) ? pt3.y - pt1.y : 1);	
	x1 = x2 = pt1.x << 8;
	y = pt1.y;
	
	// X values are multiplied by 256 to handle a sort of a decimal part to make calculations, and thus slopes,
	// more accurate. It's called a fixed .8 part, since it's only 8 bits.
	for(i = 0; i < 2; i++)
	{
		do
		{
			nRC_drawHorizontalLine(x1 >> 8, x2 >> 8, y, color, buffer);
			x1 += dx1;
			x2 += dx2;
			y++;
		} while(y < pt2.y);
		
		// dx1 = (x3 - x2) / (y3 - y2)
		dx1 = ((pt3.x - pt2.x) << 8) / ((pt3.y != pt2.y) ? pt3.y - pt2.y : 1);
		pt2.y = pt3.y;
	}
}

inline void nRC_clearBuf(char *buffer, int w)
{
	memset(buffer, w ? 255 : 0, SCREEN_BYTES_SIZE);
}

inline void nRC_dispBuf(char *buffer)
{
	memcpy(SCREEN_BASE_ADDRESS, buffer, SCREEN_BYTES_SIZE);
}

// Ray casting

#define TEXTUREW 64
#define TEXTUREH TEXTUREW

void nRC_rayCasting(int *map, Vector player, ScreenPoint mapDimensions, Vector dir, Vector planeVec, uint16_t *textures, char *buffer)
{
	Vector rayPos, rayDir, sideDist, deltaDist;
	ScreenPoint mapSquare, step, pixel;
	float cameraX, deltaWall, wallX;
	int texNum, texX, texY, color, d, wallH, sliceStart, sliceEnd;
	char hit, side;
	
	for(pixel.x = 0; pixel.x < 320; pixel.x++)
	{
		cameraX = 2 * pixel.x / 320.0 - 1;
		rayPos.x = player.x;
		rayPos.y = player.y;
		rayDir.x = dir.x + planeVec.x * cameraX;
		rayDir.y = dir.y + planeVec.y * cameraX;
		
		mapSquare.x = (int)rayPos.x;
		mapSquare.y = (int)rayPos.y;
		
		deltaDist.x = sqrt(1 + (rayDir.y * rayDir.y) / (rayDir.x * rayDir.x));
		deltaDist.y = sqrt(1 + (rayDir.x * rayDir.x) / (rayDir.y * rayDir.y));
		
		hit = 0;
		
		// Calculate step and initial ray positions
		if(rayDir.x < 0)
		{
			step.x = -1;
			sideDist.x = (rayPos.x - mapSquare.x) * deltaDist.x;
		}
		else
		{
			step.x = 1;
			sideDist.x = (mapSquare.x + 1.0 - rayPos.x) * deltaDist.x;
		}
		if(rayDir.y < 0)
		{
			step.y = -1;
			sideDist.y = (rayPos.y - mapSquare.y) * deltaDist.y;
		}
		else
		{
			step.y = 1;
			sideDist.y = (mapSquare.y + 1.0 - rayPos.y) * deltaDist.y;
		}
		
		while(!hit)
		{
			// jump to the next map square in X or Y direction
			if(sideDist.x < sideDist.y)
			{
				sideDist.x += deltaDist.x;
				mapSquare.x += step.x;
				side = 0;
			}
			else
			{
				sideDist.y += deltaDist.y;
				mapSquare.y += step.y;
				side = 1;
			}
			hit = nRC_getTile(map, mapSquare, mapDimensions);
		}
		
		// Calculate distance projected on camera direction
		if(!side) deltaWall = fabs((mapSquare.x - rayPos.x + (1 - step.x) / 2) / rayDir.x);
		else deltaWall = fabs((mapSquare.y - rayPos.y + (1 - step.y) / 2) / rayDir.y);
		
		wallH = abs((int)(240 / deltaWall));
		
		// Texturing
		texNum = hit - 1;
		if(side) wallX = rayPos.x + ((mapSquare.y - rayPos.y + (1 - step.y) / 2) / rayDir.y) * rayDir.x;
		else wallX = rayPos.y + ((mapSquare.x - rayPos.x + (1 - step.x) / 2) / rayDir.x) * rayDir.y;
		wallX -= (int)wallX;
		
		texX = (int)(wallX * (float)TEXTUREW);
		if(!side && rayDir.x > 0) texX = TEXTUREW - texX - 1;
		if(side && rayDir.y < 0) texX = TEXTUREW - texX - 1;
		
		sliceStart = max((240 - wallH) >> 1, 0);
		sliceEnd = min((240 + wallH) >> 1, 239);
		
		for(pixel.y = sliceStart; pixel.y < sliceEnd; pixel.y++)
		{
			d = pixel.y * 256 - 240 * 128 + wallH * 128;
			texY = ((d * TEXTUREH) / wallH) / 256;
			color = textures[texNum * TEXTUREW * TEXTUREH + texY * TEXTUREW + texX];
			nRC_setPixel(pixel, color, buffer);
		}
	}
	
}

void nRC_moveForward(Vector *pos, Vector *dir, Vector *plane, float speed, int *map, ScreenPoint mapDim)
{
	ScreenPoint temp;
	temp.x = (int)(pos->x + dir->x * speed);
	temp.y = (int)pos->y;
	if(!nRC_getTile(map, temp, mapDim)) pos->x += dir->x * speed;
	temp.x = (int)pos->x;
	temp.y = (int)(pos->y + dir->y * speed);
	if(!nRC_getTile(map, temp, mapDim)) pos->y += dir->y * speed;
}

void nRC_moveBackward(Vector *pos, Vector *dir, Vector *plane, float speed, int *map, ScreenPoint mapDim)
{
	ScreenPoint temp;
	temp.x = (int)(pos->x - dir->x * speed);
	temp.y = (int)pos->y;
	if(!nRC_getTile(map, temp, mapDim)) pos->x -= dir->x * speed;
	temp.x = (int)pos->x;
	temp.y = (int)(pos->y - dir->y * speed);
	if(!nRC_getTile(map, temp, mapDim)) pos->y -= dir->y * speed;
}

void nRC_lookRight(Vector *dir, Vector *plane, float speed)
{
	float oldX = dir->x;
	dir->x = dir->x * cosDeg(speed) - dir->y * sinDeg(speed);
	dir->y = oldX * sinDeg(speed) + dir->y * cosDeg(speed);
	oldX = plane->x;
	plane->x = plane->x * cosDeg(speed) - plane->y * sinDeg(speed);
	plane->y = oldX * sinDeg(speed) + plane->y * cosDeg(speed);
}

void nRC_lookLeft(Vector *dir, Vector *plane, float speed)
{
	float oldX = dir->x;
	dir->x = dir->x * cosDeg(-speed) - dir->y * sinDeg(-speed);
	dir->y = oldX * sinDeg(-speed) + dir->y * cosDeg(-speed);
	oldX = plane->x;
	plane->x = plane->x * cosDeg(-speed) - plane->y * sinDeg(-speed);
	plane->y = oldX * sinDeg(-speed) + plane->y * cosDeg(-speed);
}