#include "olcPGEX_Graphics2D.h"

namespace olc
{
	void GFX2D::DrawSprite(olc::Sprite *sprite, olc::GFX2D::Transform2D &transform, bool centered)
	{
		if (sprite == nullptr)
			return;

		// Work out bounding rectangle of sprite
		float ex=-1, ey=-1;
		float sx, sy;
		float px, py;

		float x0 = 0;
		float y0 = 0;
		if (centered) 
		{
			x0 += sprite->width / 2;
			y0 += sprite->height / 2;
		}

		transform.Forward(-x0, -y0, sx, sy);
		px = sx; py = sy;
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);
		
		transform.Forward((float)sprite->width-x0, (float)sprite->height-y0, px, py);
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);

		transform.Forward(-x0, (float)sprite->height-y0, px, py);
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);

		transform.Forward((float)sprite->width-x0, -y0, px, py);
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);

		// Perform inversion of transform if required
		transform.Invert();

		if (ex < sx) 
			std::swap(ex, sx);
		if (ey < sy) 
			std::swap(ey, sy);

		// Iterate through render space, and sample Sprite from suitable texel location
		for (float i = sx; i < ex; i++)
		{
			for (float j = sy; j < ey; j++)
			{
				float ox, oy;
				transform.Backward(i, j, ox, oy);
				pge->Draw((int32_t)i, (int32_t)j, sprite->GetPixel((int32_t)(x0+ox+0.5f), (int32_t)(y0+oy+0.5f)));
			}
		}
	}

	void GFX2D::DrawPartialSprite(olc::Sprite *sprite, int32_t x0, int32_t y0, int32_t w, int32_t h, olc::GFX2D::Transform2D &transform, bool centered)
	{
		if (sprite == nullptr)
			return;

		// Work out bounding rectangle of sprite
		float ex=-1, ey=-1;
		float sx, sy;
		float px, py;

		float xOff = 0;
		float yOff = 0;

		if (centered) 
		{
			xOff += w / 2;
			yOff += h / 2;
		}

		transform.Forward(-xOff, -yOff, sx, sy);
		px = sx; py = sy;
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);
		
		transform.Forward((float)w-xOff, (float)h-yOff, px, py);
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);

		transform.Forward(-xOff, (float)h-yOff, px, py);
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);

		transform.Forward((float)w-xOff, -yOff, px, py);
		sx = std::min(sx, px); sy = std::min(sy, py);
		ex = std::max(ex, px); ey = std::max(ey, py);

		// Perform inversion of transform if required
		transform.Invert();

		if (ex < sx) 
			std::swap(ex, sx);
		if (ey < sy) 
			std::swap(ey, sy);

		// Iterate through render space, and sample Sprite from suitable texel location
		for (float i = sx; i < ex; i++)
		{
			for (float j = sy; j < ey; j++)
			{
				float ox, oy;
				transform.Backward(i, j, ox, oy);
				int32_t xx = (int32_t)(xOff+x0+ox+0.5f);
				int32_t yy = (int32_t)(yOff+y0+oy+0.5f);
				if (xx >= x0 && xx < x0 + w && yy >= y0 && yy < y0 + h) 
					pge->Draw((int32_t)i, (int32_t)j, sprite->GetPixel(xx, yy));
			}
		}
	}

	olc::GFX2D::Transform2D::Transform2D()
	{
		Reset();
	}

	void olc::GFX2D::Transform2D::Reset()
	{
		nTargetMatrix = 0;
		nSourceMatrix = 1;
		bDirty = true;

		// Columns Then Rows

		// Matrices 0 & 1 are used as swaps in Transform accumulation
		matrix[0][0][0] = 1.0f; matrix[0][1][0] = 0.0f; matrix[0][2][0] = 0.0f;
		matrix[0][0][1] = 0.0f; matrix[0][1][1] = 1.0f; matrix[0][2][1] = 0.0f;
		matrix[0][0][2] = 0.0f; matrix[0][1][2] = 0.0f; matrix[0][2][2] = 1.0f;

		matrix[1][0][0] = 1.0f; matrix[1][1][0] = 0.0f; matrix[1][2][0] = 0.0f;
		matrix[1][0][1] = 0.0f; matrix[1][1][1] = 1.0f; matrix[1][2][1] = 0.0f;
		matrix[1][0][2] = 0.0f; matrix[1][1][2] = 0.0f; matrix[1][2][2] = 1.0f;

		// Matrix 2 is a cache matrix to hold the immediate transform operation
		// Matrix 3 is a cache matrix to hold the inverted transform
	}

	void olc::GFX2D::Transform2D::Multiply()
	{
		for (int c = 0; c < 3; c++)
		{
			for (int r = 0; r < 3; r++)
			{
				matrix[nTargetMatrix][c][r] = matrix[2][0][r] * matrix[nSourceMatrix][c][0] +
											  matrix[2][1][r] * matrix[nSourceMatrix][c][1] +
											  matrix[2][2][r] * matrix[nSourceMatrix][c][2];
			}
		}

		std::swap(nTargetMatrix, nSourceMatrix);
		bDirty = true; // Any transform multiply dirties the inversion
	}

	void olc::GFX2D::Transform2D::Rotate(float fTheta)
	{		
		// Construct Rotation Matrix
		matrix[2][0][0] = cosf(fTheta);  matrix[2][1][0] = sinf(fTheta); matrix[2][2][0] = 0.0f;
		matrix[2][0][1] = -sinf(fTheta); matrix[2][1][1] = cosf(fTheta); matrix[2][2][1] = 0.0f;
		matrix[2][0][2] = 0.0f;			 matrix[2][1][2] = 0.0f;		 matrix[2][2][2] = 1.0f;
		Multiply();		
	}

	void olc::GFX2D::Transform2D::Scale(float sx, float sy)
	{
		// Construct Scale Matrix
		matrix[2][0][0] = sx;   matrix[2][1][0] = 0.0f; matrix[2][2][0] = 0.0f;
		matrix[2][0][1] = 0.0f; matrix[2][1][1] = sy;   matrix[2][2][1] = 0.0f;
		matrix[2][0][2] = 0.0f;	matrix[2][1][2] = 0.0f;	matrix[2][2][2] = 1.0f;
		Multiply();
	}

	void olc::GFX2D::Transform2D::Shear(float sx, float sy)
	{
		// Construct Shear Matrix		
		matrix[2][0][0] = 1.0f; matrix[2][1][0] = sx;   matrix[2][2][0] = 0.0f;
		matrix[2][0][1] = sy;   matrix[2][1][1] = 1.0f; matrix[2][2][1] = 0.0f;
		matrix[2][0][2] = 0.0f;	matrix[2][1][2] = 0.0f;	matrix[2][2][2] = 1.0f;
		Multiply();
	}

	void olc::GFX2D::Transform2D::Translate(float ox, float oy)
	{
		// Construct Translate Matrix
		matrix[2][0][0] = 1.0f; matrix[2][1][0] = 0.0f; matrix[2][2][0] = ox;
		matrix[2][0][1] = 0.0f; matrix[2][1][1] = 1.0f; matrix[2][2][1] = oy;
		matrix[2][0][2] = 0.0f;	matrix[2][1][2] = 0.0f;	matrix[2][2][2] = 1.0f;
		Multiply();
	}

	void olc::GFX2D::Transform2D::Perspective(float ox, float oy)
	{
		// Construct Translate Matrix
		matrix[2][0][0] = 1.0f; matrix[2][1][0] = 0.0f; matrix[2][2][0] = 0.0f;
		matrix[2][0][1] = 0.0f; matrix[2][1][1] = 1.0f; matrix[2][2][1] = 0.0f;
		matrix[2][0][2] = ox;	matrix[2][1][2] = oy;	matrix[2][2][2] = 1.0f;
		Multiply();
	}

	void olc::GFX2D::Transform2D::Forward(float in_x, float in_y, float &out_x, float &out_y)
	{
		out_x = in_x * matrix[nSourceMatrix][0][0] + in_y * matrix[nSourceMatrix][1][0] + matrix[nSourceMatrix][2][0];
		out_y = in_x * matrix[nSourceMatrix][0][1] + in_y * matrix[nSourceMatrix][1][1] + matrix[nSourceMatrix][2][1];
		float out_z = in_x * matrix[nSourceMatrix][0][2] + in_y * matrix[nSourceMatrix][1][2] + matrix[nSourceMatrix][2][2];
		if (out_z != 0)
		{
			out_x /= out_z;
			out_y /= out_z;
		}
	} 

	void olc::GFX2D::Transform2D::Backward(float in_x, float in_y, float &out_x, float &out_y)
	{
		out_x = in_x * matrix[3][0][0] + in_y * matrix[3][1][0] + matrix[3][2][0];
		out_y = in_x * matrix[3][0][1] + in_y * matrix[3][1][1] + matrix[3][2][1];
		float out_z = in_x * matrix[3][0][2] + in_y * matrix[3][1][2] + matrix[3][2][2];
		if (out_z != 0)
		{
			out_x /= out_z;
			out_y /= out_z;
		}
	}

	void olc::GFX2D::Transform2D::Invert()
	{
		if (bDirty) // Obviously costly so only do if needed
		{			
			float det = matrix[nSourceMatrix][0][0] * (matrix[nSourceMatrix][1][1] * matrix[nSourceMatrix][2][2] - matrix[nSourceMatrix][1][2] * matrix[nSourceMatrix][2][1]) -
				        matrix[nSourceMatrix][1][0] * (matrix[nSourceMatrix][0][1] * matrix[nSourceMatrix][2][2] - matrix[nSourceMatrix][2][1] * matrix[nSourceMatrix][0][2]) +
				        matrix[nSourceMatrix][2][0] * (matrix[nSourceMatrix][0][1] * matrix[nSourceMatrix][1][2] - matrix[nSourceMatrix][1][1] * matrix[nSourceMatrix][0][2]);

			float idet = 1.0f / det;
			matrix[3][0][0] = (matrix[nSourceMatrix][1][1] * matrix[nSourceMatrix][2][2] - matrix[nSourceMatrix][1][2] * matrix[nSourceMatrix][2][1]) * idet;
			matrix[3][1][0] = (matrix[nSourceMatrix][2][0] * matrix[nSourceMatrix][1][2] - matrix[nSourceMatrix][1][0] * matrix[nSourceMatrix][2][2]) * idet;
			matrix[3][2][0] = (matrix[nSourceMatrix][1][0] * matrix[nSourceMatrix][2][1] - matrix[nSourceMatrix][2][0] * matrix[nSourceMatrix][1][1]) * idet;
			matrix[3][0][1] = (matrix[nSourceMatrix][2][1] * matrix[nSourceMatrix][0][2] - matrix[nSourceMatrix][0][1] * matrix[nSourceMatrix][2][2]) * idet;
			matrix[3][1][1] = (matrix[nSourceMatrix][0][0] * matrix[nSourceMatrix][2][2] - matrix[nSourceMatrix][2][0] * matrix[nSourceMatrix][0][2]) * idet;
			matrix[3][2][1] = (matrix[nSourceMatrix][0][1] * matrix[nSourceMatrix][2][0] - matrix[nSourceMatrix][0][0] * matrix[nSourceMatrix][2][1]) * idet;
			matrix[3][0][2] = (matrix[nSourceMatrix][0][1] * matrix[nSourceMatrix][1][2] - matrix[nSourceMatrix][0][2] * matrix[nSourceMatrix][1][1]) * idet;
			matrix[3][1][2] = (matrix[nSourceMatrix][0][2] * matrix[nSourceMatrix][1][0] - matrix[nSourceMatrix][0][0] * matrix[nSourceMatrix][1][2]) * idet;
			matrix[3][2][2] = (matrix[nSourceMatrix][0][0] * matrix[nSourceMatrix][1][1] - matrix[nSourceMatrix][0][1] * matrix[nSourceMatrix][1][0]) * idet;
			bDirty = false;
		}				
	}
}
