#include "stdafx.h"
#include "Transform.h"
#include "Core/StrBuf.h"
#include <iomanip>

namespace storm {
	namespace geometry {

		Transform::Transform() {
			for (nat r = 0; r < 4; r++)
				for (nat c = 0; c < 4; c++)
					v(r, c) = (r == c) ? 1.0f : 0.0f;
		}

		Transform::Transform(const Transform &o) {
			memcpy(&v00, &o.v00, sizeof(v00)*4*4);
		}

		Transform::Transform(const Rotation &rotation) {
			Float x = rotation.x, y = rotation.y, z = rotation.z, w = rotation.w;
			Float tx = 2*x, ty = 2*y, tz = 2*z; // , tw = 2*w;
			Float txx = tx*x, txy = tx*y, txz = tx*z, txw = tx*w;
			Float tyy = ty*y, tyz = ty*z, tyw = ty*w;
			Float tzz = tz*z, tzw = tz*w;
			// Float tww = tw*w;

			v00 = 1 - (tyy + tzz);
			v01 = txy - tzw;
			v02 = txz + tyw;
			v03 = 0;

			v10 = txy + tzw;
			v11 = 1 - (txx + tzz);
			v12 = tyz - txw;
			v13 = 0;

			v20 = txz - tyw;
			v21 = tyz + txw;
			v22 = 1 - (txx + tyy);
			v23 = 0;

			v30 = 0;
			v31 = 0;
			v32 = 0;
			v33 = 1.0f;
		}

		Transform::Transform(Float src[16]) {
			Nat i = 0;
			for (Nat r = 0; r < 4; r++)
				for (Nat c = 0; c < 4; c++)
					v(r, c) = src[i++];
		}

		Transform::Transform(Float src[4][4]) {
			for (Nat i = 0; i < 4; i++)
				for (Nat j = 0; j < 4; j++)
					v(i, j) = src[i][j];
		}

		Transform::Transform(Array<Float> *src) {
			// Note: Will throw if the range is incorrect by using 'at'.
			Nat i = 0;
			for (Nat r = 0; r < 4; r++)
				for (Nat c = 0; c < 4; c++)
					v(r, c) = src->at(i++);
		}

		Transform::Transform(Int dummy, Float *data) {
			(void)dummy;
			memcpy(&v(0, 0), data, sizeof(Float)*16);
		}

		Transform *Transform::fromRaw(EnginePtr e, Float *data) {
			return new (e.v) Transform(0, data);
		}

		void Transform::toS(StrBuf *w) const {
			StrFmt old = w->format();
			*w << fixed(2);

			for (Nat r = 0; r < 4; r++) {
				*w << S("\n(");
				for (Nat c = 0; c < 4; c++) {
					if (c != 0)
						*w << S(" ");
					*w << right(7) << at(r, c);
				}
				*w << S(")");
			}

			w->format(old);
		}

		Transform *Transform::operator *(Transform *o) {
			float r[4][4];
			for (nat i = 0; i < 4; i++) {
				for (nat j = 0; j < 4; j++) {
					float sum = 0;
					for (int k = 0; k < 4; k++)
						sum += at(i, k) * o->at(k, j);
					r[i][j] = sum;
				}
			}
			return new (this) Transform(r);
		}

		Vector4 operator *(Vector4 o, Transform *tfm) {
			Float x = tfm->v00*o.x + tfm->v10*o.y + tfm->v20*o.z + tfm->v30*o.w;
			Float y = tfm->v01*o.x + tfm->v11*o.y + tfm->v21*o.z + tfm->v31*o.w;
			Float z = tfm->v02*o.x + tfm->v12*o.y + tfm->v22*o.z + tfm->v32*o.w;
			Float w = tfm->v03*o.x + tfm->v13*o.y + tfm->v23*o.z + tfm->v33*o.w;
			return Vector4(x, y, z, w);
		}

		Vector operator *(Vector o, Transform *tfm) {
			Float x = tfm->v00*o.x + tfm->v10*o.y + tfm->v20*o.z + tfm->v30;
			Float y = tfm->v01*o.x + tfm->v11*o.y + tfm->v21*o.z + tfm->v31;
			Float z = tfm->v02*o.x + tfm->v12*o.y + tfm->v22*o.z + tfm->v32;
			Float w = tfm->v03*o.x + tfm->v13*o.y + tfm->v23*o.z + tfm->v33;
			return Vector(x/w, y/w, z/w);
		}

		Point operator *(Point o, Transform *tfm) {
			float x = tfm->v00*o.x + tfm->v10*o.y + tfm->v30;
			float y = tfm->v01*o.x + tfm->v11*o.y + tfm->v31;
			float w = tfm->v03*o.x + tfm->v13*o.y + tfm->v33;
			return Point(x/w, y/w);
		}

		Transform *Transform::inverted() const {
			Float inv[4][4];

			inv[0][0] = v11 * v22 * v33 -
				v11 * v32 * v23 -
				v12 * v21 * v33 +
				v12 * v31 * v23 +
				v13 * v21 * v32 -
				v13 * v31 * v22;

			inv[0][1] = -v01 * v22 * v33 +
				v01 * v32 * v23 +
				v02 * v21 * v33 -
				v02 * v31 * v23 -
				v03 * v21 * v32 +
				v03 * v31 * v22;

			inv[0][2] = v01 * v12 * v33 -
				v01 * v32 * v13 -
				v02 * v11 * v33 +
				v02 * v31 * v13 +
				v03 * v11 * v32 -
				v03 * v31 * v12;

			inv[0][3] = -v01 * v12 * v23 +
				v01 * v22 * v13 +
				v02 * v11 * v23 -
				v02 * v21 * v13 -
				v03 * v11 * v22 +
				v03 * v21 * v12;

			inv[1][0] = -v10 * v22 * v33 +
				v10 * v32 * v23 +
				v12 * v20 * v33 -
				v12 * v30 * v23 -
				v13 * v20 * v32 +
				v13 * v30 * v22;

			inv[1][1] = v00 * v22 * v33 -
				v00 * v32 * v23 -
				v02 * v20 * v33 +
				v02 * v30 * v23 +
				v03 * v20 * v32 -
				v03 * v30 * v22;

			inv[1][2] = -v00 * v12 * v33 +
				v00 * v32 * v13 +
				v02 * v10 * v33 -
				v02 * v30 * v13 -
				v03 * v10 * v32 +
				v03 * v30 * v12;

			inv[1][3] = v00 * v12 * v23 -
				v00 * v22 * v13 -
				v02 * v10 * v23 +
				v02 * v20 * v13 +
				v03 * v10 * v22 -
				v03 * v20 * v12;

			inv[2][0] = v10 * v21 * v33 -
				v10 * v31 * v23 -
				v11 * v20 * v33 +
				v11 * v30 * v23 +
				v13 * v20 * v31 -
				v13 * v30 * v21;

			inv[2][1] = -v00 * v21 * v33 +
				v00 * v31 * v23 +
				v01 * v20 * v33 -
				v01 * v30 * v23 -
				v03 * v20 * v31 +
				v03 * v30 * v21;

			inv[2][2] = v00 * v11 * v33 -
				v00 * v31 * v13 -
				v01 * v10 * v33 +
				v01 * v30 * v13 +
				v03 * v10 * v31 -
				v03 * v30 * v11;

			inv[2][3] = -v00 * v11 * v23 +
				v00 * v21 * v13 +
				v01 * v10 * v23 -
				v01 * v20 * v13 -
				v03 * v10 * v21 +
				v03 * v20 * v11;

			inv[3][0] = -v10 * v21 * v32 +
				v10 * v31 * v22 +
				v11 * v20 * v32 -
				v11 * v30 * v22 -
				v12 * v20 * v31 +
				v12 * v30 * v21;

			inv[3][1] = v00 * v21 * v32 -
				v00 * v31 * v22 -
				v01 * v20 * v32 +
				v01 * v30 * v22 +
				v02 * v20 * v31 -
				v02 * v30 * v21;

			inv[3][2] = -v00 * v11 * v32 +
				v00 * v31 * v12 +
				v01 * v10 * v32 -
				v01 * v30 * v12 -
				v02 * v10 * v31 +
				v02 * v30 * v11;

			inv[3][3] = v00 * v11 * v22 -
				v00 * v21 * v12 -
				v01 * v10 * v22 +
				v01 * v20 * v12 +
				v02 * v10 * v21 -
				v02 * v20 * v11;

			float det = v00 * inv[0][0] + v10 * inv[0][1] + v20 * inv[0][2] + v30 * inv[0][3];

			if (det == 0) {
				STORM_FAIL_S("Non-invertible matrix found!");
				return new (this) Transform();
			}

			det = 1.0f / det;

			for (nat i = 0; i < 4; i++)
				for (nat j = 0; j < 4; j++)
					inv[i][j] *= det;

			return new (this) Transform(inv);
		}

		Transform *Transform::forNormal() const {
			// What we do here is that we invert the matrix and transpose it. As a final step, we
			// make the matrix assume that the w component of the vector is 0 (it is a direction).

			// Note: The code below is a copy of the generic inverse, but we know that we only need
			// the topmost 3x3 part of the matrix.
			Float inv[4][4];

			inv[0][0] = v11 * v22 * v33 -
				v11 * v32 * v23 -
				v12 * v21 * v33 +
				v12 * v31 * v23 +
				v13 * v21 * v32 -
				v13 * v31 * v22;

			inv[0][1] = -v01 * v22 * v33 +
				v01 * v32 * v23 +
				v02 * v21 * v33 -
				v02 * v31 * v23 -
				v03 * v21 * v32 +
				v03 * v31 * v22;

			inv[0][2] = v01 * v12 * v33 -
				v01 * v32 * v13 -
				v02 * v11 * v33 +
				v02 * v31 * v13 +
				v03 * v11 * v32 -
				v03 * v31 * v12;

			// Need it for the determinant.
			Float i03 = -v01 * v12 * v23 +
				v01 * v22 * v13 +
				v02 * v11 * v23 -
				v02 * v21 * v13 -
				v03 * v11 * v22 +
				v03 * v21 * v12;
			inv[0][3] = 0;

			inv[1][0] = -v10 * v22 * v33 +
				v10 * v32 * v23 +
				v12 * v20 * v33 -
				v12 * v30 * v23 -
				v13 * v20 * v32 +
				v13 * v30 * v22;

			inv[1][1] = v00 * v22 * v33 -
				v00 * v32 * v23 -
				v02 * v20 * v33 +
				v02 * v30 * v23 +
				v03 * v20 * v32 -
				v03 * v30 * v22;

			inv[1][2] = -v00 * v12 * v33 +
				v00 * v32 * v13 +
				v02 * v10 * v33 -
				v02 * v30 * v13 -
				v03 * v10 * v32 +
				v03 * v30 * v12;

			inv[1][3] = 0;

			inv[2][0] = v10 * v21 * v33 -
				v10 * v31 * v23 -
				v11 * v20 * v33 +
				v11 * v30 * v23 +
				v13 * v20 * v31 -
				v13 * v30 * v21;

			inv[2][1] = -v00 * v21 * v33 +
				v00 * v31 * v23 +
				v01 * v20 * v33 -
				v01 * v30 * v23 -
				v03 * v20 * v31 +
				v03 * v30 * v21;

			inv[2][2] = v00 * v11 * v33 -
				v00 * v31 * v13 -
				v01 * v10 * v33 +
				v01 * v30 * v13 +
				v03 * v10 * v31 -
				v03 * v30 * v11;

			inv[2][3] = 0;

			inv[3][0] = 0;
			inv[3][1] = 0;
			inv[3][2] = 0;
			inv[3][3] = 1;

			float det = v00 * inv[0][0] + v10 * inv[0][1] + v20 * inv[0][2] + v30 * i03;

			if (det == 0) {
				STORM_FAIL_S("Non-invertible matrix found!");
				return new (this) Transform();
			}

			det = 1.0f / det;

			for (nat i = 0; i < 3; i++)
				for (nat j = 0; j < 3; j++)
					inv[i][j] *= det;

			// Transpose the result:
			std::swap(inv[0][1], inv[1][0]);
			std::swap(inv[0][2], inv[2][0]);
			std::swap(inv[1][2], inv[2][1]);

			// Then we are done. It would be nice if we could use w for a generic scale factor, but
			// I don't think that works.
			return new (this) Transform(inv);
		}

		Transform *translate(EnginePtr e, Vector v) {
			float d[] = {
				1,   0,   0,   0,
				0,   1,   0,   0,
				0,   0,   1,   0,
				v.x, v.y, v.z, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *translate(EnginePtr e, Point v) {
			return translate(e, Vector(v));
		}

		Transform *translate(EnginePtr e, Size v) {
			return translate(e, Point(v));
		}

		Transform *rotateX(EnginePtr e, Angle angle) {
			float s = sin(angle.rad()), c = cos(angle.rad());
			float d[] = {
				1, 0, 0, 0,
				0, c, s, 0,
				0, -s, c, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *rotateX(EnginePtr e, Angle angle, Vector origin) {
			float s = sin(angle.rad()), c = cos(angle.rad());
			float d[] = {
				1, 0, 0, 0,
				0, c, s, 0,
				0, -s, c, 0,
				0, -origin.y*c + origin.z*s + origin.y, -origin.y*s - origin.z*c + origin.z, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *rotateY(EnginePtr e, Angle angle) {
			float s = sin(angle.rad()), c = cos(angle.rad());
			float d[] = {
				c, 0, -s, 0,
				0, 1, 0, 0,
				s, 0, c, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *rotateY(EnginePtr e, Angle angle, Vector origin) {
			float s = sin(angle.rad()), c = cos(angle.rad());
			float d[] = {
				c, 0, -s, 0,
				0, 1, 0, 0,
				s, 0, c, 0,
				-origin.x*c - origin.z*s + origin.x, 0, origin.x*s - origin.z*c + origin.z, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *rotateZ(EnginePtr e, Angle angle) {
			float s = sin(angle.rad()), c = cos(angle.rad());
			float d[] = {
				c, s, 0, 0,
				-s, c, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *rotateZ(EnginePtr e, Angle angle, Vector origin) {
			float s = sin(angle.rad()), c = cos(angle.rad());
			float d[] = {
				c, s, 0, 0,
				-s, c, 0, 0,
				0, 0, 1, 0,
				-origin.x*c + origin.y*s + origin.x, -origin.x*s - origin.y*c + origin.y, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *rotate(EnginePtr e, Angle angle) {
			return rotateZ(e, angle);
		}

		Transform *rotate(EnginePtr e, Angle angle, Point origin) {
			return rotateZ(e, angle, origin);
		}

		Transform *scale(EnginePtr e, Float scale) {
			float d[] = {
				scale, 0, 0, 0,
				0, scale, 0, 0,
				0, 0, scale, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *scale(EnginePtr e, Vector scale) {
			float d[] = {
				scale.x, 0, 0, 0,
				0, scale.y, 0, 0,
				0, 0, scale.z, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *scale(EnginePtr e, Float scale, Vector center) {
			float d[] = {
				scale, 0, 0, 0,
				0, scale, 0, 0,
				0, 0, scale, 0,
				center.x - scale*center.x, center.y - scale*center.y, center.z - scale*center.z, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *scale(EnginePtr e, Vector scale, Vector center) {
			float d[] = {
				scale.x, 0, 0, 0,
				0, scale.y, 0, 0,
				0, 0, scale.z, 0,
				center.x - scale.x*center.x, center.y - scale.y*center.y, center.z - scale.z*center.z, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *scale(EnginePtr e, Size s) {
			return scale(e, Vector(s.w, s.h, 1.0f));
		}

		Transform *scale(EnginePtr e, Size s, Point p) {
			return scale(e, Vector(s.w, s.h, 1.0f), Vector(p.x, p.y, 0.0f));
		}

		Transform *skewX(EnginePtr e, Angle angle) {
			float t = -tan(angle.rad());
			float d[] = {
				1, 0, 0, 0,
				t, 1, 0, 0,
				t, 0, 1, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *skewY(EnginePtr e, Angle angle) {
			float t = -tan(angle.rad());
			float d[] = {
				1, t, 0, 0,
				0, 1, 0, 0,
				0, t, 1, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *skewZ(EnginePtr e, Angle angle) {
			float t = -tan(angle.rad());
			float d[] = {
				1, 0, t, 0,
				0, 1, t, 0,
				0, 0, 1, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *orthoProjectXY(EnginePtr e) {
			float d[] = {
				1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 0, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *orthoProjectXZ(EnginePtr e) {
			float d[] = {
				1, 0, 0, 0,
				0, 0, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *orthoProjectYZ(EnginePtr e) {
			float d[] = {
				0, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1,
			};
			return new (e.v) Transform(d);
		}

		Transform *shadowProjectXY(EnginePtr e, Vector light) {
			// See XZ version for explanation.
			float d[] = {
				light.z, 0, 0, 0,
				0, light.z, 0, 0,
				-light.x, -light.y, 0, -1,
				0, 0, 0, light.z,
			};
			return new (e.v) Transform(d);
		}

		Transform *shadowProjectXZ(EnginePtr e, Vector light) {
			// Observation: equation for transforming coords are:
			// x' = height * x / (height - y).
			// This results in the matrix below:
			// float d[] = {
			// 	height, 0, 0, 0,
			// 	0, 0, 0, -1,
			// 	0, 0, height, 0,
			// 	0, 0, 0, height,
			// };
			// What we really return is: translate(-light) * T * translate(light):
			float d[] = {
				light.y, 0, 0, 0,
				-light.x, 0, -light.z, -1,
				0, 0, light.y, 0,
				0, 0, 0, light.y,
			};
			return new (e.v) Transform(d);
		}

		Transform *shadowProjectYZ(EnginePtr e, Vector light) {
			// See XZ version for explanation.
			float d[] = {
				0, -light.y, -light.z, -1,
				0, light.x, 0, 0,
				0, 0, light.x, 0,
				0, 0, 0, light.x,
			};
			return new (e.v) Transform(d);
		}

		Transform *perspectiveProject(EnginePtr e, Angle fovY, Float aspect, Float zNear, Float zFar) {
			float yScale = 1 / tan(fovY * 0.5f);
			float xScale = yScale / aspect;
			float d[] = {
				xScale, 0, 0, 0,
				0, yScale, 0, 0,
				0, 0, zFar / (zFar - zNear), 1,
				0, 0, -zNear * zFar / (zFar - zNear), 0
			};
			return new (e.v) Transform(d);
		}

	}
}
