#pragma once

#include <psemek/math/affine_transform.hpp>
#include <psemek/math/quaternion.hpp>

#include <psemek/util/assert.hpp>

namespace psemek::math
{

	// Shear in the (i,j)-plane that replaces x[i] with x[i] + x[j] * factor
	// E.g. shear(0, 1, m) is the usual 2D horizontal shear
	template <typename T, std::size_t N>
	struct shear
	{
		std::size_t i, j;
		T factor;

		shear();
		shear(std::size_t i, std::size_t j, T factor);
		shear(shear const &) = default;

		matrix<T, N, N + 1> affine_matrix() const;
		matrix<T, N, N> linear_matrix() const;
		vector<T, N> translation_vector() const;
		matrix<T, N + 1, N + 1> homogeneous_matrix() const;

		affine_transform<T, N, N> transform() const;

		vector<T, N> operator()(vector<T, N> const & v) const;
		point<T, N> operator()(point<T, N> const & p) const;

	private:
		template <typename Matrix>
		void fill_matrix(Matrix & m) const;
	};

	template <typename T, std::size_t N>
	shear<T, N>::shear()
		: i{0}
		, j{1}
		, factor{0}
	{
		assert(i < N);
		assert(j < N);
	}

	template <typename T, std::size_t N>
	shear<T, N>::shear(std::size_t i, std::size_t j, T factor)
		: i{i}
		, j{j}
		, factor{factor}
	{
		assert(i < N);
		assert(j < N);
	}

	template <typename T, std::size_t N>
	matrix<T, N, N + 1> shear<T, N>::affine_matrix() const
	{
		auto result = matrix<T, N, N + 1>::identity();
		fill_matrix(result);
		return result;
	}

	template <typename T, std::size_t N>
	matrix<T, N, N> shear<T, N>::linear_matrix() const
	{
		auto result = matrix<T, N, N>::identity();
		fill_matrix(result);
		return result;
	}

	template <typename T, std::size_t N>
	vector<T, N> shear<T, N>::translation_vector() const
	{
		return vector<T, N>::zero();
	}

	template <typename T, std::size_t N>
	matrix<T, N + 1, N + 1> shear<T, N>::homogeneous_matrix() const
	{
		auto result = matrix<T, N + 1, N + 1>::identity();
		fill_matrix(result);
		return result;
	}

	template <typename T, std::size_t N>
	affine_transform<T, N, N> shear<T, N>::transform() const
	{
		return {affine_matrix()};
	}

	template <typename T, std::size_t N>
	vector<T, N> shear<T, N>::operator()(vector<T, N> const & v) const
	{
		auto result = v;
		result[i] += result[j] * factor;
		return result;
	}

	template <typename T, std::size_t N>
	point<T, N> shear<T, N>::operator()(point<T, N> const & p) const
	{
		auto result = p;
		result[i] += result[j] * factor;
		return result;
	}

	template <typename T, std::size_t N>
	template <typename Matrix>
	void shear<T, N>::fill_matrix(Matrix & m) const
	{
		m[i][j] = factor;
	}

	template <typename T, std::size_t N>
	shear<T, N> inverse(shear<T, N> const & r)
	{
		return {r.i, r.j, -r.factor};
	}

}
