/* 
 * LibPSn00b Example Programs
 *
 * Hello World Example
 * 2019-2020 Meido-Tek Productions / PSn00bSDK Project
 *
 * The obligatory hello world example normally included in nearly every
 * SDK package. This example should also get you started in how to manage
 * the display using psxgpu.
 *
 * Example by Lameguy64
 *
 *
 * Changelog:
 *
 *	  January 1, 2020 - Initial version
 */
 
#include <stdio.h>
#include <sys/types.h>
#include <psxetc.h>
#include <psxgte.h>
#include <psxgpu.h>
#include <inline_c.h>

#include "ps1mdl.h"

#define OTLEN	256

#define SCREENWIDTH		512
#define SCREENHEIGHT	240

#define NUMVERTEXNORMALS	162
static short anorms[NUMVERTEXNORMALS][4] = {
#include "ps1anorms.h"
};

// Define display/draw environments for double buffering
DISPENV disp[2];
DRAWENV draw[2];
int db;

u_long ot[2][OTLEN];	// Ordering tables, two arrays for double buffering. These are basically the buckets for bucket sorting of polygons.
						// You can also see them as "layers" in the modern 3D graphics sense, but they serve a more immediate role for polygon ordering.
char primbuff[2][65536];	// Primitive buffer, just a raw buffer of bytes to use as a pool for primitives
char *nextpri;

extern u_long mdl_player[];
extern u_long tim_player_f[];
extern u_long tim_player_b[];
extern u_long mdl_shambler[];
extern u_long tim_shambler_f[];
extern u_long tim_shambler_b[];

typedef struct
{
	ps1mdl_header_t* header;
	ps1mdl_texcoord_t* texCoords;
	ps1mdl_triangle_t* triangles;
	ps1mdl_vertex_t* vertices;

	VECTOR position;
	SVECTOR rotation;
} ps1mdl_t;

typedef struct
{
	RECT prect, crect;
	int mode;
	int uoffs, voffs;
} ps1texture_t;

typedef struct
{
	// Skin textures are divided into two pieces, so that each part is aligned to their own texture page
	ps1texture_t front, back;
} ps1skin_t;

ps1mdl_t playerModel = { .position = { -200, 200, -160 }, .rotation = { 0 } };
ps1mdl_t shamblerModel = { .position = { 100, 200, -160 }, .rotation = { 0 } };
ps1skin_t playerSkin, shamblerSkin;

const MATRIX identity = {
	ONE, 0, 0,
	0, ONE, 0,
	0, 0, ONE,
	0, 0, 0,
};

const MATRIX quake_swizzle = {
	ONE, 0, 0,
	0, 0, -ONE,
	0, ONE, 0,
	0, 0, 0,
};

MATRIX light_cols = {
	ONE, 0, 0,
	ONE, 0, 0,
	ONE, 0, 0
};

MATRIX light_dirs = {
	ONE, 0, 0,
	0, 0, 0,
	0, 0, 0
};

// Scale X coordinates to correct the aspect ratio for the chosen resolution
VECTOR aspect_scale = { SCREENWIDTH * ONE / 320 , ONE, ONE };

VECTOR cam_pos = { 0, 0, 0 };
SVECTOR cam_rot = { 0 };

void loadModel(const u_long* data, ps1mdl_t *mdl)
{
	const char *bytes = (const char*)data;

	mdl->header = (ps1mdl_header_t*)bytes;
	bytes += sizeof(ps1mdl_header_t);

	mdl->texCoords = (ps1mdl_texcoord_t*)bytes;
	bytes += sizeof(ps1mdl_texcoord_t) * mdl->header->vertexCount;

	mdl->triangles = (ps1mdl_triangle_t*)bytes;
	bytes += sizeof(ps1mdl_triangle_t) * mdl->header->triangleCount;

	mdl->vertices = (ps1mdl_vertex_t*)bytes;
}

void loadTexture(const u_long* tim, ps1texture_t *texture)
{
	TIM_IMAGE* tparam;
	GetTimInfo(tim, tparam);

	// This freezes up if the TIM image has weird dimensions, such as the ones from Quake...
	LoadImage(tparam->prect, tparam->paddr);

	// Upload CLUT for palettized images
	if (tparam->mode & 0x8)
	{
		LoadImage(tparam->crect, tparam->caddr);
	}

	DrawSync(0);

	texture->prect = *tparam->prect;
	texture->crect = *tparam->crect;
	texture->mode = tparam->mode;

	texture->uoffs = (texture->prect.x % 64) << (2 - (texture->mode & 0x3));
	texture->voffs = (texture->prect.y & 0xFF);
}

void loadSkin(const u_long* frontTim, const u_long* backTim, ps1skin_t *skin)
{
	loadTexture(frontTim, &skin->front);
	loadTexture(backTim, &skin->back);
}

// Init function
void init(void)
{
	// This not only resets the GPU but it also installs the library's
	// ISR subsystem to the kernel
	ResetGraph(0);

	SetVideoMode(MODE_NTSC);
	
	// Define display environments, first on top and second on bottom
	SetDefDispEnv(&disp[0], 0, 0, SCREENWIDTH, SCREENHEIGHT);
	SetDefDispEnv(&disp[1], 0, SCREENHEIGHT, SCREENWIDTH, SCREENHEIGHT);
	
	// Define drawing environments, first on bottom and second on top
	SetDefDrawEnv(&draw[0], 0, SCREENHEIGHT, SCREENWIDTH, SCREENHEIGHT);
	SetDefDrawEnv(&draw[1], 0, 0, SCREENWIDTH, SCREENHEIGHT);
	
	// Set and enable clear color
	setRGB0(&draw[0], 49, 77, 121);
	setRGB0(&draw[1], 49, 77, 121);
	draw[0].isbg = 1;
	draw[0].dtd = 1;
	draw[1].isbg = 1;
	draw[1].dtd = 1;

	// Clear double buffer counter
	db = 0;

	// Apply the GPU environments
	PutDispEnv(&disp[db]);
	PutDrawEnv(&draw[db]);
	
	// Load test font
	FntLoad(960, 0);
	
	// Open up a test font text stream of 100 characters
	FntOpen(0, 8, 320, 224, 0, 100);

	nextpri = primbuff[0];

	loadModel(mdl_player, &playerModel);
	loadSkin(tim_player_f, tim_player_b, &playerSkin);

	loadModel(mdl_shambler, &shamblerModel);
	loadSkin(tim_shambler_f, tim_shambler_b, &shamblerSkin);

	InitGeom();
	gte_SetGeomOffset(SCREENWIDTH >> 1, SCREENHEIGHT >> 1);
	gte_SetGeomScreen(180);	// Screen depth for FOV control. Essentially the Z position of the vanishing point.

	// Set texture page for the entire drawing environment. Nice in some cases perhaps, but not what we need.
	//draw[0].tpage = getTPage(playerFrontTex.mode & 0x3, 0, playerFrontTex.prect->x, playerFrontTex.prect->y);
	//draw[1].tpage = getTPage(playerFrontTex.mode & 0x3, 0, playerFrontTex.prect->x, playerFrontTex.prect->y);
}

// Display function
void display(void)
{
	// Wait for all drawing to complete
	// TODO: this should be interleaved with the double buffered ordering table draw commands, to minimize CPU idle time while waiting for the GPU
	DrawSync(0);
	
	// Wait for vertical sync to cap the logic to 60fps (or 50 in PAL mode)
	// and prevent screen tearing
	VSync(0);

	// Switch pages	
	PutDispEnv(&disp[db]);
	PutDrawEnv(&draw[db]);
	
	// Enable display output, ResetGraph() disables it by default
	SetDispMask(1);

	DrawOTag(ot[db] + OTLEN - 1);	// This performs a DMA transfer to quickly send all the primitives off to the GPU
	
	// Flip buffer index
	db = !db;

	nextpri = primbuff[db];
}

static void lerpcol(int r0, int g0, int b0, int r1, int g1, int b1, int lerp, int* rout, int* gout, int* bout)	// lerp = 0-4096
{
	int invlerp = 4096 - lerp;

	*rout = (r0 * invlerp + r1 * lerp) >> 12;
	*gout = (g0 * invlerp + g1 * lerp) >> 12;
	*bout = (b0 * invlerp + b1 * lerp) >> 12;
}

void addTile(int x, int y, int w, int h, int r, int g, int b)
{
	// Initialize tile primitive
	TILE *tile = (TILE*)nextpri;
	setTile(tile);
	setXY0(tile, x, y);
	setWH(tile, w, h);
	setRGB0(tile, r, g, b);
	
	addPrim(ot[db], tile);
	nextpri += sizeof(TILE);
}

void addColoredTriangle(int x0, int y0, int x1, int y1, int x2, int y2, unsigned int rgb0, unsigned int rgb1, unsigned int rgb2, unsigned char depth)
{
	POLY_G3 *poly = (POLY_G3*)nextpri;
	setPolyG3(poly);
	setXY3(poly, x0, y0, x1, y1, x2, y2);
	setRGB0(poly, (rgb0 >> 16) & 0xFF, (rgb0 >> 8) & 0xFF, rgb0 & 0xFF);
	setRGB1(poly, (rgb1 >> 16) & 0xFF, (rgb1 >> 8) & 0xFF, rgb1 & 0xFF);
	setRGB2(poly, (rgb2 >> 16) & 0xFF, (rgb2 >> 8) & 0xFF, rgb2 & 0xFF);
	
	addPrim(ot[db] + depth, poly);
	nextpri += sizeof(POLY_G3);
}

void addTexturedTriangle(int x0, int y0, int x1, int y1, int x2, int y2, short u0, short v0, short u1, short v1, short u2, short v2, TIM_IMAGE *img)
{
	short uoffs = (img->prect->x % 64) << (2 - (img->mode & 0x3));
	short voffs = (img->prect->y & 0xFF);

	POLY_FT3 *poly = (POLY_FT3*)nextpri;
	setPolyFT3(poly);
	setXY3(poly, x0, y0, x1, y1, x2, y2);
	setUV3(poly, uoffs + u0, voffs + v0, uoffs + u1, voffs + v1, uoffs + u2, voffs + v2);
	setRGB0(poly, 128, 128, 128);
	setClut(poly, img->crect->x, img->crect->y);
	setTPage(poly, img->mode & 0x3, 0, img->prect->x, img->prect->y);
	
	addPrim(ot[db], poly);
	nextpri += sizeof(POLY_FT3);
}

void addGouraudTexturedTriangle(
	int x0, int y0, int x1, int y1, int x2, int y2, unsigned char depth,
	unsigned int rgb0, unsigned int rgb1, unsigned int rgb2, 
	short u0, short v0, short u1, short v1, short u2, short v2, TIM_IMAGE *img)
{
	if (img == NULL)
	{
		addColoredTriangle(x0, y0, x1, y1, x2, y2, rgb0, rgb1, rgb2, depth);
		return;
	}

	short uoffs = (img->prect->x % 64) << (2 - (img->mode & 0x3));
	short voffs = (img->prect->y & 0xFF);

	POLY_GT3 *poly = (POLY_GT3*)nextpri;
	setPolyGT3(poly);
	setXY3(poly, x0, y0, x1, y1, x2, y2);
	setUV3(poly, uoffs + u0, voffs + v0, uoffs + u1, voffs + v1, uoffs + u2, voffs + v2);
	setRGB0(poly, (rgb0 >> 16) & 0xFF, (rgb0 >> 8) & 0xFF, rgb0 & 0xFF);
	setRGB1(poly, (rgb1 >> 16) & 0xFF, (rgb1 >> 8) & 0xFF, rgb1 & 0xFF);
	setRGB2(poly, (rgb2 >> 16) & 0xFF, (rgb2 >> 8) & 0xFF, rgb2 & 0xFF);
	setClut(poly, img->crect->x, img->crect->y);
	setTPage(poly, img->mode & 0x3, 0, img->prect->x, img->prect->y);

	addPrim(ot[db] + depth, poly);
	nextpri += sizeof(POLY_GT3);
}

void addTexturedSprite(int x, int y, int w, int h, TIM_IMAGE *img)
{
	short uoffs = (img->prect->x % 64) << (2 - (img->mode & 0x3));
	short voffs = (img->prect->y & 0xFF);

	SPRT *sprite = (SPRT*)nextpri;
	setSprt(sprite);
	setXY0(sprite, x, y);
	setWH(sprite, w, h);
	setUV0(sprite, uoffs, voffs);
	setClut(sprite, img->crect->x, img->crect->y);
	setRGB0(sprite, 128, 128, 128);
	addPrim(ot[db], sprite);
	nextpri += sizeof(SPRT);

	DR_TPAGE *tpage = (DR_TPAGE*)nextpri;
	setDrawTPage(tpage, 0, 1, getTPage(img->mode & 0x3, 0, img->prect->x, img->prect->y));
	addPrim(ot[db], tpage);
	nextpri += sizeof(DR_TPAGE);
}

static int normToRGB(const int *norm)
{
	int r = ((norm[0] + 4096) >> 5) & 0xFF;
	int g = ((norm[1] + 4096) >> 5) & 0xFF;
	int b = ((norm[2] + 4096) >> 5) & 0xFF;
	return r << 16 | g << 8 | b;
}

static int dot(const int *a, const int *b)
{
	int c0 = (a[0] * b[0]) >> 12;
	int c1 = (a[1] * b[1]) >> 12;
	int c2 = (a[2] * b[2]) >> 12;
	return c0 + c1 + c2;
}

static int fakeLight(const int *norm)
{
	const int light[3] = { ONE, 0, 0 };
	int lit = dot(norm, light) >> 4;	// Calculate light intensity and normalize to [-256..255]
	lit = lit < 0 ? 0 : lit;	// Clamp to [0..255]
	lit += 48;	// Add some ambient light
	lit = lit > 255 ? 255 : lit;	// Clamp to [0..255] again
	return lit;
}

SVECTOR outPos;

void drawModel(MATRIX *view_matrix, ps1mdl_t *model, ps1skin_t *skin, int frameCounter)
{
	ps1texture_t *tex;
	short u0, u1, u2, uoffs, voffs;
	int p;

	short halfSkinWidth = model->header->skinWidth >> 1;
	unsigned short numTriangles = model->header->triangleCount;
	int frameNum = frameCounter % model->header->frameCount;
	ps1mdl_vertex_t *baseVert = &model->vertices[frameNum * model->header->vertexCount];

	int *scale = model->header->scale;
	int *translate = model->header->translate;

	MATRIX model_mtx;
	RotMatrix(&model->rotation, &model_mtx);
	TransMatrix(&model_mtx, &model->position);

	MATRIX light_mtx;
	MulMatrix0(&light_dirs, &model_mtx, &light_mtx);
	gte_SetLightMatrix(&light_mtx);

	CompMatrixLV(view_matrix, &model_mtx, &model_mtx);
	
	PushMatrix();

	gte_SetRotMatrix(&model_mtx);
	gte_SetTransMatrix(&model_mtx);
	
	for (unsigned short triIdx = 0; triIdx < numTriangles; ++triIdx)
	{
		ps1mdl_triangle_t *tri = &model->triangles[triIdx];
		
		ps1mdl_vertex_t *v0 = &baseVert[tri->vertexIndex[0]];
		ps1mdl_vertex_t *v1 = &baseVert[tri->vertexIndex[1]];
		ps1mdl_vertex_t *v2 = &baseVert[tri->vertexIndex[2]];

		SVECTOR pos0 = { v0->position[0], v0->position[1], v0->position[2], 0 };
		SVECTOR pos1 = { v1->position[0], v1->position[1], v1->position[2], 0 };
		SVECTOR pos2 = { v2->position[0], v2->position[1], v2->position[2], 0 };

		gte_ldv3(&pos0, &pos1, &pos2);
		gte_rtpt();	// Rotation, Translation and Perspective triplet (all three vertices at once)

		// Normal clipping for backface culling
		gte_nclip();
		gte_stopz(&p);
		if (p < 0)
			continue;

		// Average Z for depth sorting
		gte_avsz3();
		gte_stotz(&p);
		unsigned short depth = p;//p >> 2;
		if (depth < 0 || depth >= OTLEN)
			continue;

		ps1mdl_texcoord_t *tc0 = &model->texCoords[tri->vertexIndex[0]];
		ps1mdl_texcoord_t *tc1 = &model->texCoords[tri->vertexIndex[1]];
		ps1mdl_texcoord_t *tc2 = &model->texCoords[tri->vertexIndex[2]];

		if (tri->frontFace)
		{
			u0 = tc0->u, u1 = tc1->u, u2 = tc2->u;
			tex = &skin->front;
		}
		else
		{
			// Since we have the texture split into two parts, we need to correct the UVs that are *not* on the seam, instead of the other way round
			u0 = tc0->u - !tc0->onSeam * halfSkinWidth;
			u1 = tc1->u - !tc1->onSeam * halfSkinWidth;
			u2 = tc2->u - !tc2->onSeam * halfSkinWidth;
			tex = &skin->back;
		}

		// TODO: we could get rid of these silly offsets if we just ensure that textures are always page-aligned
		uoffs = tex->uoffs, voffs = tex->voffs;

		POLY_GT3 *poly = (POLY_GT3*)nextpri;
		setPolyGT3(poly);

		// Store transformed vertex coordinates in screen space
		gte_stsxy3_gt3(poly);

		// Copy some values for on-screen debugging
		outPos.vx = poly->x0;
		outPos.vy = poly->y0;
		outPos.vz = p;

		// Calculate vertex color based on normal
		// TODO: we could probably speed this up by precalculating the lighting for each normal, if we need to draw more than 162 vertices
		gte_ldrgb(&poly->r0);
		gte_ldv3(anorms[v0->normalIndex], anorms[v1->normalIndex], anorms[v2->normalIndex]);
		gte_nct();
		gte_strgb3(&poly->r0, &poly->r1, &poly->r2);

		// Set texture parameters
		setUV3(poly, uoffs + u0, voffs + tc0->v, uoffs + u1, voffs + tc1->v, uoffs + u2, voffs + tc2->v);
		setClut(poly, tex->crect.x, tex->crect.y);
		setTPage(poly, tex->mode & 0x3, 0, tex->prect.x, tex->prect.y);

		addPrim(ot[db] + depth, poly);
		nextpri += sizeof(POLY_GT3);
	}

	PopMatrix();
}

void drawStuff(int counter)
{
	ClearOTagR(ot[db], OTLEN);

	FntPrint(-1, "Image x %d y %d w %d h %d mode 0x%x\n", playerSkin.front.prect.x, playerSkin.front.prect.y, playerSkin.front.prect.w, playerSkin.front.prect.h, playerSkin.front.mode);
	FntPrint(-1, "Model: %d tris, %d verts\n", playerModel.header->triangleCount, playerModel.header->vertexCount);
	FntPrint(-1, "Trsf: (%d, %d, %d)\n", outPos.vx, outPos.vy, outPos.vz);
	
	// Draw the last created text stream
	FntFlush(-1);

	gte_SetBackColor(48, 48, 48);	// Ambient light color
	gte_SetColorMatrix(&light_cols);	// Light color (up to three different lights)

	// Expected order of things:
	// - Initial scale matrix to correct aspect ratio
	// - Coordinate system swizzle (scale) (can probably be combined with the above)
	// - Camera inverse translation
	// - Camera inverse rotation (apply on top of camera translation)
	// - Multiply light matrix (lights will be in Quake coordinate system)
	// - Push matrix
	// - Entity local rotation
	// - Entity local translation
	// - Model internal scale
	// - Model internal translate
	// - Render
	// - Pop matrix

	MATRIX proj_matrix = quake_swizzle;	// Swizzle coordinates so that everything is in Quake coordinate system (Z up)
	ScaleMatrixL(&proj_matrix, &aspect_scale);	// Apply aspect ratio correction for the current resolution

	MATRIX view_matrix;
	SVECTOR trot = { 0, 0, 0, 0 };	// TODO: camera rotation (in Quake coordinates)
	VECTOR tpos = { 0, 0, 0 };		// TODO: camera position (in Quake coordinates)
	RotMatrix(&trot, &view_matrix);		// Set camera rotation part of the view matrix
	ApplyMatrixLV(&view_matrix, &tpos, &tpos);	// Apply camera rotation to camera position
	TransMatrix(&view_matrix, &tpos);	// Apply transformed position to the translation part of the view matrix

	// Compose view and projection matrices to obtain a combined view-projection matrix
	CompMatrixLV(&proj_matrix, &view_matrix, &view_matrix);

	gte_SetRotMatrix(&view_matrix);
	gte_SetTransMatrix(&view_matrix);

	drawModel(&view_matrix, &playerModel, &playerSkin, counter >> 2);
	drawModel(&view_matrix, &shamblerModel, &shamblerSkin, counter >> 2);
}

// Main function, program entrypoint
int main(int argc, const char *argv[])
{
	int counter;

	// Init stuff	
	init();
	
	// Main loop
	counter = 0;
	while(1)
	{
		drawStuff(counter);
		
		// Update display
		display();
		
		// Increment the counter
		counter++;
	}
	
	return 0;
}