/* The MIT License (MIT)
 *
 * Copyright (c) 2014 Valentin Verdier
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:

 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef MATRIX4D_HPP_INCLUDED
#define MATRIX4D_HPP_INCLUDED

//#include <iostream>
#include <cmath>

#include <Vector3D.hpp>
#include <Vector4D.hpp>

namespace Xr {
	template<typename T>
	class Matrix4D {
		public:
			Matrix4D() {
				T v[16] = {0};
				set(v);
			}

			template<typename T2>
			Matrix4D(const Matrix4D<T2>& mat) {
				set(mat);
			}

			template<typename T2>
			Matrix4D(T2* v) {
				set(v);
			}

			template<typename T2>
			void set(T2* v) {
				for(unsigned long i = 0; i < 16; i++) {
					m_values[i] = static_cast<T>(v[i]);
				}
			}

			template<typename T2>
			void set(const Matrix4D<T2>& mat) {
				T2 v[16];
				mat.get(v);
				set(v);
			}

			void get(T* v) const {
				for(unsigned long i = 0; i < 16; i++) {
					v[i] = m_values[i];
				}
			}

			void setIdentity() {
				T v[16] = {
					1, 0, 0, 0,
					0, 1, 0, 0,
					0, 0, 1, 0,
					0, 0, 0, 1
				};
				set(v);
			}

			void setProjection(double fov, double aspect, double near, double far) {
				double f = (1 / tan((fov / 2) * M_PI / 180));
				T v[16] = {
					f / aspect, 0, 0, 0,
					0, f, 0, 0,
					0, 0, (far + near) / (near - far), (2 * far * near) / (near - far),
					0, 0, -1, 0
				};
				*this *= Matrix4D<T>(v);
			}

			void setLookAt(const Vector3D<double>& eye, const Vector3D<double>& center, const Vector3D<double>& up) {
				Vector3D<double> look(center - eye);
				Vector3D<double> normal(look.cross(up));
				Vector3D<double> up2(normal.cross(look));

				look.normalize();
				normal.normalize();
				up2.normalize();

				T v[16] = {
					normal.x, normal.y, normal.z, 0,
					up2.x, up2.y, up2.z, 0,
					-look.x, -look.y, -look.z, 1
				};
				*this *= Matrix4D<T>(v);
			}

			/*void print() const {
				for(unsigned long m = 0; m < 4; m++) {
					std::cout << "[ ";
					for(unsigned long n = 0; n < 4; n++) {
						std::cout << m_values[m * 4 + n] << " ";
					}
					std::cout << "]\n";
				}
				std::cout << "\n";
			}*/

			Matrix4D<T> operator+(const Matrix4D<T>& mat) const {
				return Matrix4D<T>(*this) += mat;
			}

			Matrix4D<T> operator-(const Matrix4D<T>& mat) const {
				return Matrix4D<T>(*this) -= mat;
			}

			Matrix4D<T> operator*(const Matrix4D<T>& mat) const {
				return Matrix4D<T>(*this) *= mat;
			}

			Vector4D<T> operator*(const Vector4D<T>& vec) const {
				Vector4D<T> res;
				res.x = vec.x * m_values[0] + vec.y * m_values[1] + vec.z * m_values[2] + vec.w * m_values[3];
				res.y = vec.x * m_values[4] + vec.y * m_values[5] + vec.z * m_values[6] + vec.w * m_values[7];
				res.z = vec.x * m_values[8] + vec.y * m_values[9] + vec.z * m_values[10] + vec.w * m_values[11];
				res.w = vec.x * m_values[12] + vec.y * m_values[13] + vec.z * m_values[14] + vec.w * m_values[15];
				return res;
			}

			template<typename T2>
			Matrix4D<T>& operator=(const Matrix4D<T2>& mat) {
				set(mat);
				return *this;
			}

			Matrix4D<T>& operator+=(const Matrix4D<T>& mat) {
				T v[16];
				mat.get(v);
				for(unsigned long i = 0; i < 16; i++) {
					m_values[i] += v[i];
				}
				return *this;
			}

			Matrix4D<T>& operator-=(const Matrix4D<T>& mat) {
				T v[16];
				mat.get(v);
				for(unsigned long i = 0; i < 16; i++) {
					m_values[i] -= v[i];
				}
				return *this;
			}

			Matrix4D<T>& operator*=(const Matrix4D<T>& mat) {
				T v[16];
				T v2[16];
				mat.get(v2);

				for(unsigned long n = 0; n < 4; n++) {
					for(unsigned long m = 0; m < 4; m++) {
						v[m * 4 + n] = 0;
						for(unsigned long k = 0; k < 4; k++) {
							v[m * 4 + n] += m_values[m * 4 + k] * v2[k * 4 + n];
						}
					}
				}
				set(v);
				return *this;
			}

			void translate(const Vector3D<T>& vec) {
				T v[16] = {
					1, 0, 0, vec.x,
					0, 1, 0, vec.y,
					0, 0, 1, vec.z,
					0, 0, 0, 1
				};
				*this *= Matrix4D<T>(v);
			}

			void scale(const Vector3D<T>& vec) {
				T v[16] = {
					vec.x, 0, 0, 0,
					0, vec.y, 0, 0,
					0, 0, vec.z, 0,
					0, 0, 0, 1
				};
				*this *= Matrix4D<T>(v);
			}

			void rotate(double angle, const Vector3D<T>& vr) {
				Vector3D<double> vec(vr);
				vec.normalize();

				double c =	cos(angle);
				double s = sin(angle);

				T v[16] = {
					vec.x * vec.x * (1 - c) + c, vec.x * vec.y * (1 - c) - vec.z * s, vec.x * vec.z * (1 - c) + vec.y * s, 0,
                    vec.y * vec.x * (1 - c) + vec.z * s, vec.y * vec.y * (1 - c) + c, vec.y * vec.z * (1 - c) - vec.x * s, 0,
                    vec.z * vec.x * (1 - c) - vec.y * s, vec.z * vec.y * (1 - c) + vec.x * s, vec.z * vec.z * (1 - c) + c, 0,
                    0, 0, 0, 1
				};

				*this *= Matrix4D<T>(v);
			}

		private:
			T m_values[16];
	};
}

#endif // MATRIX4D_HPP_INCLUDED

