#define OLC_PGE_APPLICATION
#include "pixelGameEngine.h"
#include <strstream>
#include <algorithm>

using namespace olc;

struct vec2d
{
	float u,v;
};


struct vec3d
{
	float x=0; 
	float y=0; 
	float z=0;
	float w=1;
};

struct triangle
{
	vec3d p[3];
	vec2d uv[3];
	Pixel col;
};

struct mesh
{
	std::vector<triangle> tris;

	bool LoadFromObjectFile(std::string sFilename)
	{
		std::ifstream f(sFilename);
		if (!f.is_open())
			return false;

		// Local cache of verts
		std::vector<vec3d> verts;
		std::vector<vec2d> uvs;

		while (!f.eof())
		{
			char line[128];
			f.getline(line, 128);

			std::strstream s;
			s << line;

			char junk;

			if (line[0] == 'v')
			{
				if (line[1]=='t') {
					vec2d v;
					s >> junk >> junk >> v.u >> v.v;
					uvs.push_back(v);
					//std::cout<<"Line: "<<line<<"\n";
					//std::cout<<"Tex coords: "<<v.u<<","<<v.v<<"\n";
				} else
				if (line[1] != 'n')
				{
					vec3d v;
					s >> junk >> v.x >> v.y >> v.z;
					verts.push_back(v);
					//std::cout<<"Line: "<<line<<"\n";
					//std::cout<<"Vertex: "<<v.x<<","<<v.y<<","<<v.z<<"\n";
				}
			}
			if (line[0] == 'f')
				{
					s >> junk;

					std::string tokens[9];
					int nTokenCount = -1;


					while (!s.eof())
					{
						char c = s.get();
						if (c == ' ' || c == '/')
							nTokenCount++;
						else
							tokens[nTokenCount].append(1, c);
					}

					tokens[nTokenCount].pop_back();

					tris.push_back({ verts[stoi(tokens[0]) - 1], verts[stoi(tokens[3]) - 1], verts[stoi(tokens[6]) - 1],
					0,0,0
					/*uvs[stoi(tokens[1]) - 1], uvs[stoi(tokens[4]) - 1], uvs[stoi(tokens[7]) - 1]*/});
				}
		}

		return true;
	}

};

struct mat4x4
{
	float m[4][4] = { 0 };
};

class olcEngine3D : public PixelGameEngine
{

public:

	Decal*texture;
	olcEngine3D()
	{
		sAppName = "3D Demo";
	}


private:
	mesh meshCube;
	mat4x4 matProj;

	vec3d vCamera={0,0,0};
	vec3d vLookDir;

	float zOffset=2;

	float fTheta=0;
	float fYaw=0;

	vec3d Matrix_MultiplyVector(mat4x4 &m, vec3d &i)
	{
		vec3d v;
		v.x = i.x * m.m[0][0] + i.y * m.m[1][0] + i.z * m.m[2][0] + i.w * m.m[3][0];
		v.y = i.x * m.m[0][1] + i.y * m.m[1][1] + i.z * m.m[2][1] + i.w * m.m[3][1];
		v.z = i.x * m.m[0][2] + i.y * m.m[1][2] + i.z * m.m[2][2] + i.w * m.m[3][2];
		v.w = i.x * m.m[0][3] + i.y * m.m[1][3] + i.z * m.m[2][3] + i.w * m.m[3][3];
		return v;
	}

	mat4x4 Matrix_MakeIdentity()
	{
		mat4x4 matrix;
		matrix.m[0][0] = 1.0f;
		matrix.m[1][1] = 1.0f;
		matrix.m[2][2] = 1.0f;
		matrix.m[3][3] = 1.0f;
		return matrix;
	}

	mat4x4 Matrix_MakeRotationX(float fAngleRad)
	{
		mat4x4 matrix;
		matrix.m[0][0] = 1.0f;
		matrix.m[1][1] = cosf(fAngleRad);
		matrix.m[1][2] = sinf(fAngleRad);
		matrix.m[2][1] = -sinf(fAngleRad);
		matrix.m[2][2] = cosf(fAngleRad);
		matrix.m[3][3] = 1.0f;
		return matrix;
	}

	mat4x4 Matrix_MakeRotationY(float fAngleRad)
	{
		mat4x4 matrix;
		matrix.m[0][0] = cosf(fAngleRad);
		matrix.m[0][2] = sinf(fAngleRad);
		matrix.m[2][0] = -sinf(fAngleRad);
		matrix.m[1][1] = 1.0f;
		matrix.m[2][2] = cosf(fAngleRad);
		matrix.m[3][3] = 1.0f;
		return matrix;
	}

	mat4x4 Matrix_MakeRotationZ(float fAngleRad)
	{
		mat4x4 matrix;
		matrix.m[0][0] = cosf(fAngleRad);
		matrix.m[0][1] = sinf(fAngleRad);
		matrix.m[1][0] = -sinf(fAngleRad);
		matrix.m[1][1] = cosf(fAngleRad);
		matrix.m[2][2] = 1.0f;
		matrix.m[3][3] = 1.0f;
		return matrix;
	}

	mat4x4 Matrix_MakeTranslation(float x, float y, float z)
	{
		mat4x4 matrix;
		matrix.m[0][0] = 1.0f;
		matrix.m[1][1] = 1.0f;
		matrix.m[2][2] = 1.0f;
		matrix.m[3][3] = 1.0f;
		matrix.m[3][0] = x;
		matrix.m[3][1] = y;
		matrix.m[3][2] = z;
		return matrix;
	}

	mat4x4 Matrix_MakeProjection(float fFovDegrees, float fAspectRatio, float fNear, float fFar)
	{
		float fFovRad = 1.0f / tanf(fFovDegrees * 0.5f / 180.0f * 3.14159f);
		mat4x4 matrix;
		matrix.m[0][0] = fAspectRatio * fFovRad;
		matrix.m[1][1] = fFovRad;
		matrix.m[2][2] = fFar / (fFar - fNear);
		matrix.m[3][2] = (-fFar * fNear) / (fFar - fNear);
		matrix.m[2][3] = 1.0f;
		matrix.m[3][3] = 0.0f;
		return matrix;
	}

	mat4x4 Matrix_MultiplyMatrix(mat4x4 &m1, mat4x4 &m2)
	{
		mat4x4 matrix;
		for (int c = 0; c < 4; c++)
			for (int r = 0; r < 4; r++)
				matrix.m[r][c] = m1.m[r][0] * m2.m[0][c] + m1.m[r][1] * m2.m[1][c] + m1.m[r][2] * m2.m[2][c] + m1.m[r][3] * m2.m[3][c];
		return matrix;
	}

	mat4x4 Matrix_PointAt(vec3d &pos, vec3d &target, vec3d &up)
	{
		// Calculate new forward direction
		vec3d newForward = Vector_Sub(target, pos);
		newForward = Vector_Normalise(newForward);

		// Calculate new Up direction
		vec3d a = Vector_Mul(newForward, Vector_DotProduct(up, newForward));
		vec3d newUp = Vector_Sub(up, a);
		newUp = Vector_Normalise(newUp);

		// New Right direction is easy, its just cross product
		vec3d newRight = Vector_CrossProduct(newUp, newForward);

		// Construct Dimensioning and Translation Matrix
		mat4x4 matrix;
		matrix.m[0][0] = newRight.x;	matrix.m[0][1] = newRight.y;	matrix.m[0][2] = newRight.z;	matrix.m[0][3] = 0.0f;
		matrix.m[1][0] = newUp.x;		matrix.m[1][1] = newUp.y;		matrix.m[1][2] = newUp.z;		matrix.m[1][3] = 0.0f;
		matrix.m[2][0] = newForward.x;	matrix.m[2][1] = newForward.y;	matrix.m[2][2] = newForward.z;	matrix.m[2][3] = 0.0f;
		matrix.m[3][0] = pos.x;			matrix.m[3][1] = pos.y;			matrix.m[3][2] = pos.z;			matrix.m[3][3] = 1.0f;
		return matrix;

	}

	mat4x4 Matrix_QuickInverse(mat4x4 &m) // Only for Rotation/Translation Matrices
	{
		mat4x4 matrix;
		matrix.m[0][0] = m.m[0][0]; matrix.m[0][1] = m.m[1][0]; matrix.m[0][2] = m.m[2][0]; matrix.m[0][3] = 0.0f;
		matrix.m[1][0] = m.m[0][1]; matrix.m[1][1] = m.m[1][1]; matrix.m[1][2] = m.m[2][1]; matrix.m[1][3] = 0.0f;
		matrix.m[2][0] = m.m[0][2]; matrix.m[2][1] = m.m[1][2]; matrix.m[2][2] = m.m[2][2]; matrix.m[2][3] = 0.0f;
		matrix.m[3][0] = -(m.m[3][0] * matrix.m[0][0] + m.m[3][1] * matrix.m[1][0] + m.m[3][2] * matrix.m[2][0]);
		matrix.m[3][1] = -(m.m[3][0] * matrix.m[0][1] + m.m[3][1] * matrix.m[1][1] + m.m[3][2] * matrix.m[2][1]);
		matrix.m[3][2] = -(m.m[3][0] * matrix.m[0][2] + m.m[3][1] * matrix.m[1][2] + m.m[3][2] * matrix.m[2][2]);
		matrix.m[3][3] = 1.0f;
		return matrix;
	}

	vec3d Vector_Add(vec3d &v1, vec3d &v2)
	{
		return { v1.x + v2.x, v1.y + v2.y, v1.z + v2.z };
	}

	vec3d Vector_Sub(vec3d &v1, vec3d &v2)
	{
		return { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z };
	}

	vec3d Vector_Mul(vec3d &v1, float k)
	{
		return { v1.x * k, v1.y * k, v1.z * k };
	}

	vec3d Vector_Div(vec3d &v1, float k)
	{
		return { v1.x / k, v1.y / k, v1.z / k };
	}

	float Vector_DotProduct(vec3d &v1, vec3d &v2)
	{
		return v1.x*v2.x + v1.y*v2.y + v1.z * v2.z;
	}

	float Vector_Length(vec3d &v)
	{
		return sqrtf(Vector_DotProduct(v, v));
	}

	vec3d Vector_Normalise(vec3d &v)
	{
		float l = Vector_Length(v);
		return { v.x / l, v.y / l, v.z / l };
	}

	vec3d Vector_CrossProduct(vec3d &v1, vec3d &v2)
	{
		vec3d v;
		v.x = v1.y * v2.z - v1.z * v2.y;
		v.y = v1.z * v2.x - v1.x * v2.z;
		v.z = v1.x * v2.y - v1.y * v2.x;
		return v;
	}


public:
	bool OnUserCreate() override
	{
		texture = new Decal(new Sprite("Body.png"));
		meshCube.LoadFromObjectFile("Nia.obj");

		matProj=Matrix_MakeProjection(90.0f,(float)ScreenHeight() / (float)ScreenWidth(),0.1f,1000.0f);

		return true;
	}

	bool OnUserUpdate(float fElapsedTime) override
	{
		if (GetKey(olc::DOWN).bHeld) {
			vCamera.y-=8*fElapsedTime;
		}
		if (GetKey(olc::UP).bHeld) {
			vCamera.y+=8*fElapsedTime;
		}
		if (GetKey(olc::RIGHT).bHeld) {
			vCamera.x+=8*fElapsedTime;
		}
		if (GetKey(olc::LEFT).bHeld) {
			vCamera.x-=8*fElapsedTime;
		}
		vec3d vForward=Vector_Mul(vLookDir,8*fElapsedTime);
		if (GetKey(olc::W).bHeld) {
			vCamera=Vector_Add(vCamera,vForward);
		}
		if (GetKey(olc::S).bHeld) {
			vCamera=Vector_Sub(vCamera,vForward);
		}
		if (GetKey(olc::A).bHeld) {
			fYaw-=2*fElapsedTime;
		}
		if (GetKey(olc::D).bHeld) {
			fYaw+=2*fElapsedTime;
		}

		// Set up rotation matrices
		mat4x4 matRotZ, matRotX, matTrans, matWorld;

		matRotZ=Matrix_MakeRotationZ(fTheta*0.5f);
		matRotX=Matrix_MakeRotationX(fTheta);

		matTrans=Matrix_MakeTranslation(0.0f,0.0f,5.0f);
		matWorld=Matrix_MakeIdentity();
		matWorld=Matrix_MultiplyMatrix(matRotZ,matRotX);
		matWorld=Matrix_MultiplyMatrix(matWorld,matTrans);

		vec3d vUp={0,1,0};
		vec3d vTarget={0,0,1};
		mat4x4 matCameraRot=Matrix_MakeRotationY(fYaw);
		vLookDir=Matrix_MultiplyVector(matCameraRot,vTarget);
		vTarget=Vector_Add(vCamera,vLookDir);
		mat4x4 matCamera = Matrix_PointAt(vCamera,vTarget,vUp);
		mat4x4 matView=Matrix_QuickInverse(matCamera);

		std::vector<triangle>vecTrianglesToRaster;

		// Draw Triangles
		for (auto&tri : meshCube.tris)
		{
			triangle triProjected, triTransformed,triViewed;

			triTransformed.p[0]=Matrix_MultiplyVector(matWorld,tri.p[0]);
			triTransformed.p[1]=Matrix_MultiplyVector(matWorld,tri.p[1]);
			triTransformed.p[2]=Matrix_MultiplyVector(matWorld,tri.p[2]);

			vec3d normal,line1,line2;
			line1=Vector_Sub(triTransformed.p[1],triTransformed.p[0]);
			line2=Vector_Sub(triTransformed.p[2],triTransformed.p[0]);

			normal=Vector_CrossProduct(line1,line2);
			normal=Vector_Normalise(normal);

			vec3d vCameraRay=Vector_Sub(triTransformed.p[0],vCamera);

			if (Vector_DotProduct(normal,vCameraRay)<0) {
				vec3d light_dir=Vector_Mul(vCameraRay,-1);
				light_dir=Vector_Normalise(light_dir);

				float dp = std::max(0.1f,Vector_DotProduct(light_dir,normal));

				triTransformed.col=Pixel(255*dp*dp,255*dp*dp,255*dp*dp);

				triViewed.p[0]=Matrix_MultiplyVector(matView,triTransformed.p[0]);
				triViewed.p[1]=Matrix_MultiplyVector(matView,triTransformed.p[1]);
				triViewed.p[2]=Matrix_MultiplyVector(matView,triTransformed.p[2]);


				// Project triangles from 3D --> 2D
				triProjected.p[0]=Matrix_MultiplyVector(matProj,triViewed.p[0]);
				triProjected.p[1]=Matrix_MultiplyVector(matProj,triViewed.p[1]);
				triProjected.p[2]=Matrix_MultiplyVector(matProj,triViewed.p[2]);
				triProjected.p[0]=Vector_Div(triProjected.p[0],triProjected.p[0].w);
				triProjected.p[1]=Vector_Div(triProjected.p[1],triProjected.p[1].w);
				triProjected.p[2]=Vector_Div(triProjected.p[2],triProjected.p[2].w);
				triProjected.col=triTransformed.col;

				triProjected.p[0].x*=-1.0f;
				triProjected.p[1].x*=-1.0f;
				triProjected.p[2].x*=-1.0f;
				triProjected.p[0].y*=-1.0f;
				triProjected.p[1].y*=-1.0f;
				triProjected.p[2].y*=-1.0f;

				// Scale into view
				vec3d vOffsetView={1,1,0};
				triProjected.p[0] = Vector_Add(triProjected.p[0],vOffsetView);
				triProjected.p[1] = Vector_Add(triProjected.p[1],vOffsetView);
				triProjected.p[2] = Vector_Add(triProjected.p[2],vOffsetView);
				triProjected.p[0].x *= 0.5f * (float)ScreenWidth();
				triProjected.p[0].y *= 0.5f * (float)ScreenHeight();
				triProjected.p[1].x *= 0.5f * (float)ScreenWidth();
				triProjected.p[1].y *= 0.5f * (float)ScreenHeight();
				triProjected.p[2].x *= 0.5f * (float)ScreenWidth();
				triProjected.p[2].y *= 0.5f * (float)ScreenHeight();

				vecTrianglesToRaster.push_back(triProjected);

			}
		}

		std::sort(vecTrianglesToRaster.begin(),vecTrianglesToRaster.end(),[](triangle&t1,triangle&t2){return (t1.p[0].z+t1.p[1].z+t1.p[2].z)/3.0f>(t2.p[0].z+t2.p[1].z+t2.p[2].z)/3.0f;});

		for (auto&triProjected:vecTrianglesToRaster) {

			// Rasterize triangle
			SetDecalStructure(DecalStructure::LIST);
			SetDecalMode(DecalMode::NORMAL);
			DrawPolygonDecal(nullptr,{
				{triProjected.p[0].x, triProjected.p[0].y},
				{triProjected.p[1].x, triProjected.p[1].y},
				{triProjected.p[2].x, triProjected.p[2].y}
			},{
				{triProjected.uv[0].u,triProjected.uv[0].v},
				{triProjected.uv[1].u,triProjected.uv[1].v},
				{triProjected.uv[2].u,triProjected.uv[2].v},
			},triProjected.col);
			/*SetDecalMode(DecalMode::WIREFRAME);
			DrawPolygonDecal(nullptr,{
				{triProjected.p[0].x, triProjected.p[0].y},
				{triProjected.p[1].x, triProjected.p[1].y},
				{triProjected.p[2].x, triProjected.p[2].y}
			},{
				{0,0},
				{0,0},
				{0,0},
			},BLACK);*/
			SetDecalStructure(DecalStructure::FAN);
		}

		SetDecalMode(DecalMode::NORMAL);
		DrawStringDecal({0,0},"Triangles: "+std::to_string(meshCube.tris.size()));


		return true;
	}

};




int main()
{
	olcEngine3D demo;
	if (demo.Construct(1280, 720, 1, 1))
		demo.Start();
    return 0;
}