#pragma once

#include <psemek/math/vector.hpp>
#include <psemek/math/point.hpp>
#include <psemek/math/matrix.hpp>

namespace psemek::math
{

	template <typename T, std::size_t D>
	vector<T, D+1> homogeneous(vector<T, D> const & v)
	{
		vector<T, D+1> result;
		for (std::size_t i = 0; i < D; ++i)
			result[i] = v[i];
		result[D] = T(0);
		return result;
	}

	template <typename T, std::size_t D>
	vector<T, D+1> homogeneous(point<T, D> const & v)
	{
		vector<T, D+1> result;
		for (std::size_t i = 0; i < D; ++i)
			result[i] = v[i];
		result[D] = T(1);
		return result;
	}

	template <typename T, std::size_t D>
	matrix<T, D+1, D+1> homogeneous(matrix<T, D, D> const & m)
	{
		matrix<T, D+1, D+1> result = matrix<T, D+1, D+1>::zero();
		for (std::size_t i = 0; i < D; ++i)
		{
			for (std::size_t j = 0; j < D; ++j)
			{
				result[i][j] = m[i][j];
			}
		}
		result[D][D] = T(1);
		return result;
	}

	template <typename T, std::size_t D>
	vector<T, D - 1> as_vector(vector<T, D> const & v)
	{
		static_assert(D > 1);
		vector<T, D - 1> result;
		for (std::size_t i = 0; i < D - 1; ++i)
			result[i] = v[i];
		return result;
	}

	template <typename T, std::size_t D>
	point<T, D - 1> as_point(vector<T, D> const & v)
	{
		static_assert(D > 1);
		point<T, D - 1> result;
		for (std::size_t i = 0; i < D - 1; ++i)
			result[i] = v[i] / v[D - 1];
		return result;
	}

	template <typename T, std::size_t D>
	matrix<T, D-1, D-1> homogeneous_to_linear(matrix<T, D, D> const & m)
	{
		return submatrix<0, 0, D-1, D-1>(m);
	}

	template <typename T, std::size_t D>
	matrix<T, D-1, D> homogeneous_to_affine(matrix<T, D, D> const & m)
	{
		return submatrix<0, 0, D-1, D>(m);
	}

}
