#pragma once

#include <psemek/math/affine_transform.hpp>

#include <psemek/util/assert.hpp>

namespace psemek::math
{

	template <typename T, std::size_t N>
	struct swap
	{
		std::size_t i, j;

		swap(std::size_t i, std::size_t j);
		swap(swap 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> v) const;
		point<T, N> operator()(point<T, N> p) const;

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

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

	template <typename T, std::size_t N>
	vector<T, N> swap<T, N>::operator()(vector<T, N> v) const
	{
		std::swap(v[i], v[j]);
		return v;
	}

	template <typename T, std::size_t N>
	point<T, N> swap<T, N>::operator()(point<T, N> p) const
	{
		std::swap(p[i], p[j]);
		return p;
	}

	template <typename T, std::size_t N>
	matrix<T, N, N + 1> swap<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> swap<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> swap<T, N>::translation_vector() const
	{
		return vector<T, N>::zero();
	}

	template <typename T, std::size_t N>
	matrix<T, N + 1, N + 1> swap<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> swap<T, N>::transform() const
	{
		return {affine_matrix()};
	}

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

	template <typename T, std::size_t N>
	swap<T, N> inverse(swap<T, N> const & s)
	{
		return s;
	}

}
