#pragma once

#include <psemek/math/scale.hpp>
#include <psemek/math/rotation.hpp>
#include <psemek/math/translation.hpp>

namespace psemek::math
{

	// NB: composition of TRS triplets is not defined as the result
	//     might not be expressible in TRS form

	template <typename T, std::size_t N>
	struct trs;

	template <typename T>
	struct trs<T, 3>
	{
		vector<T, 3> translation = {T{0}, T{0}, T{0}};
		quaternion<T> rotation = quaternion<T>::identity();
		vector<T, 3> scale = {T{1}, T{1}, T{1}};

		static trs<T, 3> identity()
		{
			return {};
		}

		static trs<T, 3> from(matrix<T, 3, 4> const & m)
		{
			trs<T, 3> result;

			result.translation[0] = m[0][3];
			result.translation[1] = m[1][3];
			result.translation[2] = m[2][3];

			result.scale[0] = length(vector{m[0][0], m[1][0], m[2][0]});
			result.scale[1] = length(vector{m[0][1], m[1][1], m[2][1]});
			result.scale[2] = length(vector{m[0][2], m[1][2], m[2][2]});

			matrix<T, 3, 3> r;
			for (int i = 0; i < 3; ++i)
				for (int j = 0; j < 3; ++j)
					r[i][j] = m[i][j] / result.scale[j];

			result.rotation = quaternion<T>::rotation(r);

			return result;
		}

		matrix<T, 3, 4> affine_matrix() const
		{
			return transform().affine_matrix();
		}

		matrix<T, 3, 3> linear_matrix() const
		{
			return transform().linear_matrix();
		}

		vector<T, 3> translation_vector() const
		{
			return translation;
		}

		matrix<T, 4, 4> homogeneous_matrix() const
		{
			return transform().homogeneous_matrix();
		}

		vector<T, 3> operator()(vector<T, 3> const & v) const
		{
			return rotate(rotation, pointwise_mult(v, scale));
		}

		point<T, 3> operator()(point<T, 3> const & p) const
		{
			return p.zero() + rotate(rotation, pointwise_mult(p - p.zero(), scale)) + translation;
		}

		affine_transform<T, 3, 3> transform() const
		{
			return math::translation(translation).transform()
				* quaternion_rotation(rotation).transform()
				* math::scale(scale).transform();
		}
	};

}
