#pragma once

#include <psemek/math/detail/array.hpp>
#include <psemek/math/math.hpp>
#include <psemek/util/hash.hpp>
#include <psemek/util/span.hpp>

#include <iostream>
#include <cstddef>
#include <type_traits>
#include <cmath>
#include <compare>

namespace psemek::math
{

	template <typename T, std::size_t N>
	struct vector
	{
		using scalar_type = T;
		static constexpr std::size_t static_dimension = N;

		typename detail::array<T, N>::type coords;

		using dynamic_dimensions_type = detail::dynamic_dimensions<N>;

		// Internal
		vector(dynamic_dimensions_type dimensions)
			: coords(dimensions)
		{}

		vector() = default;

		explicit vector(std::size_t size) requires (N == dynamic)
			: coords({.size = size})
		{}

		template <typename ... Args>
		requires(N != dynamic && sizeof...(Args) == N && detail::all_convertible_to<T, Args...>::value)
		vector(Args && ... args)
		{
			auto out = values().begin();
			((*out++ = args), ...);
		}

		template <typename ... Args>
		requires(N == dynamic && detail::all_convertible_to<T, Args...>::value)
		vector(Args && ... args)
			: coords({.size = sizeof...(Args)})
		{
			auto out = values().begin();
			((*out++ = args), ...);
		}

		std::size_t dimension() const
		{
			return coords.size;
		}

		T & operator[](std::size_t i)
		{
			return coords[i];
		}

		T const & operator[](std::size_t i) const
		{
			return coords[i];
		}

		util::span<T> values()
		{
			return {&coords[0], dimension()};
		}

		util::span<T const> values() const
		{
			return {&coords[0], dimension()};
		}

		vector copy() const
		{
			if constexpr (N == dynamic)
			{
				vector result;
				result.coords = coords.copy();
				return result;
			}
			else
			{
				return *this;
			}
		}

		vector & operator *= (T const & s);
		vector & operator /= (T const & s);

		vector & operator += (vector const & v);
		vector & operator -= (vector const & v);

		static vector zero(dynamic_dimensions_type dimensions = {});
	};

	template <typename ... Args>
	vector(Args && ...) -> vector<std::common_type_t<Args...>, sizeof...(Args)>;

	template <typename T, std::size_t N>
	void check_dynamic_size(vector<T, N> const & v1, vector<T, N> const & v2)
	{
		check_dynamic_size(v1.coords.dimensions(), v2.coords.dimensions());
	}

	template <typename T1, typename T, std::size_t N>
	vector<T1, N> cast(vector<T, N> const & v)
	{
		vector<T1, N> r(v.coords.dimensions());
		for (std::size_t i = 0; i < v.dimension(); ++i)
			r[i] = static_cast<T1>(v[i]);
		return r;
	}

	template <typename T, std::size_t N>
	std::strong_ordering operator <=> (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		check_dynamic_size(v1, v2);

		for (std::size_t i = 0; i < v1.dimension(); ++i)
		{
			if (v1[i] < v2[i])
				return std::strong_ordering::less;
			else if (v1[i] > v2[i])
				return std::strong_ordering::greater;
		}
		return std::strong_ordering::equal;
	}

	template <typename T, std::size_t N>
	bool operator == (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		return (v1 <=> v2) == std::strong_ordering::equal;
	}

	template <typename T, std::size_t N>
	bool operator != (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		return !(v1 == v2);
	}

	template <typename T, std::size_t N>
	bool operator < (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		return (v1 <=> v2) == std::strong_ordering::less;
	}

	template <typename T, std::size_t N>
	bool operator > (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		return (v2 < v1);
	}

	template <typename T, std::size_t N>
	bool operator <= (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		return !(v2 < v1);
	}

	template <typename T, std::size_t N>
	bool operator >= (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		return !(v1 < v2);
	}

	template <typename T, std::size_t N>
	vector<T, N> operator * (vector<T, N> const & v, T const & s)
	{
		vector<T, N> r(v.coords.dimensions());
		for (std::size_t i = 0; i < v.dimension(); ++i)
			r[i] = v[i] * s;
		return r;
	}

	template <typename T, std::size_t N>
	vector<T, N> operator * (T const & s, vector<T, N> const & v)
	{
		vector<T, N> r(v.coords.dimensions());
		for (std::size_t i = 0; i < v.dimension(); ++i)
			r[i] = s * v[i];
		return r;
	}

	template <typename T, std::size_t N>
	vector<T, N> operator / (vector<T, N> const & v, T const & s)
	{
		vector<T, N> r(v.coords.dimensions());
		for (std::size_t i = 0; i < v.dimension(); ++i)
			r[i] = v[i] / s;
		return r;
	}

	template <typename T, std::size_t N>
	vector<T, N> operator - (vector<T, N> const & v)
	{
		vector<T, N> r(v.coords.dimensions());
		for (std::size_t i = 0; i < v.dimension(); ++i)
			r[i] = -v[i];
		return r;
	}

	template <typename T, std::size_t N>
	vector<T, N> operator + (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		check_dynamic_size(v1, v2);

		vector<T, N> r(v1.coords.dimensions());
		for (std::size_t i = 0; i < v1.dimension(); ++i)
			r[i] = v1[i] + v2[i];
		return r;
	}

	template <typename T, std::size_t N>
	vector<T, N> operator - (vector<T, N> const & v1, vector<T, N> const & v2)
	{
		check_dynamic_size(v1, v2);

		vector<T, N> r(v1.coords.dimensions());
		for (std::size_t i = 0; i < v1.dimension(); ++i)
			r[i] = v1[i] - v2[i];
		return r;
	}

	template <typename T, std::size_t N>
	vector<T, N> & vector<T, N>::operator *= (T const & s)
	{
		return (*this) = (*this) * s;
	}

	template <typename T, std::size_t N>
	vector<T, N> & vector<T, N>::operator /= (T const & s)
	{
		return (*this) = (*this) / s;
	}

	template <typename T, std::size_t N>
	vector<T, N> & vector<T, N>::operator += (vector<T, N> const & v)
	{
		return (*this) = (*this) + v;
	}

	template <typename T, std::size_t N>
	vector<T, N> & vector<T, N>::operator -= (vector<T, N> const & v)
	{
		return (*this) = (*this) - v;
	}

	template <typename T, std::size_t N>
	vector<T, N> vector<T, N>::zero(dynamic_dimensions_type dimensions)
	{
		vector<T, N> r(dimensions);
		for (std::size_t i = 0; i < r.dimension(); ++i)
			r[i] = T(0);
		return r;
	}

	template <typename T, std::size_t N>
	T dot(vector<T, N> const & v1, vector<T, N> const & v2)
	{
		check_dynamic_size(v1, v2);

		T r{};
		for (std::size_t i = 0; i < v1.dimension(); ++i)
			r += v1[i] * v2[i];
		return r;
	}

	template <typename T, std::size_t N>
	T length_sqr(vector<T, N> const & v)
	{
		return dot(v, v);
	}

	template <typename T, std::size_t N>
	T linf_norm(vector<T, N> const & v)
	{
		using std::abs;
		using std::max;
		T r = abs(v[0]);
		for (std::size_t i = 1; i < v.dimension(); ++i)
			make_max(r, abs(v[i]));
		return r;
	}

	template <typename T, std::size_t N>
	T length(vector<T, N> const & v)
	{
		T const max = linf_norm(v);
		if (max == T(0)) return max;

		T sum{};
		for (auto const & value : v.values())
			sum += sqr(value / max);

		using std::sqrt;
		return max * sqrt(sum);
	}

	template <typename T, std::size_t N>
	vector<T, N> normalized(vector<T, N> const & v)
	{
		return v / length(v);
	}

	// TODO: generic implementation
	template <typename T>
	T det(vector<T, 2> const & v0, vector<T, 2> const & v1)
	{
		return v0[0] * v1[1] - v0[1] * v1[0];
	}

	template <typename T>
	T det(vector<T, 3> const & v0, vector<T, 3> const & v1, vector<T, 3> const & v2)
	{
		return
			+ v0[0] * v1[1] * v2[2]
			- v0[0] * v1[2] * v2[1]
			- v0[1] * v1[0] * v2[2]
			+ v0[1] * v1[2] * v2[0]
			+ v0[2] * v1[0] * v2[1]
			- v0[2] * v1[1] * v2[0]
			;
	}

	// TODO: generic implementation (shares internals with det)
	template <typename T>
	vector<T, 3> ort(vector<T, 3> const & v0, vector<T, 3> const & v1)
	{
		return vector<T, 3>{
			v0[1] * v1[2] - v0[2] * v1[1],
			v0[2] * v1[0] - v0[0] * v1[2],
			v0[0] * v1[1] - v0[1] * v1[0],
		};
	}

	// Any vector orthogonal to v
	// N.B.: discontinuous in odd dimensions
	template <typename T, std::size_t N>
	vector<T, N> ort(vector<T, N> const & v)
	{
		vector<T, N> result = vector<T, N>::zero(v.coords.dimensions());
		if ((N != dynamic && (N % 2) == 0) || (N == dynamic && (v.dimension() % 2) == 0))
		{
			for (std::size_t i = 0; i < v.dimension(); i += 2)
			{
				result[i] = -v[i + 1];
				result[i + 1] = v[i];
			}
		}
		else
		{
			std::size_t i = 0, j = 1;

			using std::abs;

			for (std::size_t k = 1; k < v.dimension(); ++k)
			{
				if (abs(v[k]) > abs(v[i]))
				{
					j = i;
					i = k;
				}
			}

			result[i] = -v[j];
			result[j] = v[i];
		}
		return result;
	}

	template <typename T>
	vector<T, 3> cross(vector<T, 3> const & v0, vector<T, 3> const & v1)
	{
		return ort(v0, v1);
	}

	template <typename T>
	vector<T, 2> direction(T angle)
	{
		using std::sin;
		using std::cos;
		return {cos(angle), sin(angle)};
	}

	template <typename T>
	T angle(vector<T, 2> const & v)
	{
		using std::atan2;
		return atan2(v[1], v[0]);
	}

	template <typename T>
	T angle(vector<T, 2> const & v0, vector<T, 2> const & v1)
	{
		using std::atan2;
		return atan2(det(v0, v1), dot(v0, v1));
	}

	template <typename T, std::size_t N>
	T angle(vector<T, N> const & v0, vector<T, N> const & v1)
	{
		using std::acos;
		using std::min;
		using std::max;
		return acos(min(T{1}, max(T{-1}, dot(v0, v1))));
	}

	template <typename T>
	vector<T, 2> rotate(vector<T, 2> const & v, T angle)
	{
		using std::sin;
		using std::cos;
		auto const c = cos(angle);
		auto const s = sin(angle);
		return {v[0] * c - v[1] * s, v[0] * s + v[1] * c};
	}

	template <typename F, std::size_t N, typename T, typename ... Ts>
	auto pointwise(F && f, vector<T, N> const & v0, vector<Ts, N> const & ... vs)
	{
		using R = decltype(f(v0[0], vs[0]...));

		(check_dynamic_size(v0, vs), ...);

		vector<R, N> result(v0.coords.dimensions());
		for (std::size_t i = 0; i < v0.dimension(); ++i)
			result[i] = f(v0[i], vs[i]...);
		return result;
	}

	template <typename T, std::size_t N>
	vector<T, N> pointwise_mult(vector<T, N> const & v0, vector<T, N> const & v1)
	{
		return pointwise([](T const & a, T const & b){ return a * b; }, v0, v1);
	}

	template <typename T, std::size_t N>
	vector<T, N> pointwise_divide(vector<T, N> const & v0, vector<T, N> const & v1)
	{
		return pointwise([](T const & a, T const & b){ return a / b; }, v0, v1);
	}

	template <typename T, std::size_t N>
	vector<T, N> pointwise_log(vector<T, N> const & v)
	{
		return pointwise([](T const & a){ using std::log; return log(a); }, v);
	}

	template <typename T, std::size_t N>
	vector<T, N> pointwise_exp(vector<T, N> const & v)
	{
		return pointwise([](T const & a){ using std::exp; return exp(a); }, v);
	}

	template <typename T, std::size_t N>
	vector<T, N> pointwise_pow(vector<T, N> const & v0, vector<T, N> const & v1)
	{
		return pointwise([](T const & a, T const & b){ using std::pow; return pow(a, b); }, v0, v1);
	}

	template <typename T, std::size_t N>
	vector<T, N> lerp(vector<T, N> const & v0, vector<T, N> const & v1, T const & t)
	{
		return v0 * (T(1) - t) + v1 * t;
	}

	// NB: v0 and v1 are assumed to be normalized
	template <typename T, std::size_t N>
	vector<T, N> slerp(vector<T, N> const & v0, vector<T, N> const & v1, T const & t)
	{
		using std::sin;
		using std::acos;

		auto const d = dot(v0, v1);

		// prevent division by zero
		if (d >= T{1})
			return lerp(v0, v1, t);

		auto const omega = acos(d);

		// NB: the case of omega ~ pi is ambiguous and isn't handled in any special way
		auto const s = sin(omega);
		auto const w0 = std::sin((1 - t) * omega) / s;
		auto const w1 = (sin(t * omega) / s);
		return w0 * v0 + w1 * v1;
	}

	// Return vector orthogonal to n
	template <typename T, std::size_t N>
	vector<T, N> project_from(vector<T, N> const & v, vector<T, N> const & n)
	{
		return v - n * (dot(v, n) / dot(n, n));
	}

	template <typename T, std::size_t N>
	std::ostream & operator << (std::ostream & os, vector<T, N> const & v)
	{
		if constexpr (N == 0)
		{
			os << "()";
			return os;
		}

		os << '(' << v[0];
		for (std::size_t i = 1; i < v.dimension(); ++i)
			os << ", " << v[i];
		os << ')';
		return os;
	}

	template <typename T, std::size_t N>
	bool isfinite(vector<T, N> const & v)
	{
		using std::isfinite;
		for (std::size_t i = 0; i < v.dimension(); ++i)
			if (!isfinite(v[i]))
				return false;
		return true;
	}

}

namespace std
{

	template <typename T, std::size_t N>
	struct hash<::psemek::math::vector<T, N>>
	{
		std::uint64_t operator()(::psemek::math::vector<T, N> const & v) const noexcept
		{
			std::hash<T> h;
			std::uint64_t r = 0;
			for (std::size_t i = 0; i < v.dimension(); ++i)
				::psemek::util::hash_combine(r, h(v[i]));
			return r;
		}
	};

	template <typename T, std::size_t N, typename Char>
	struct formatter<::psemek::math::vector<T, N>, Char>
		: formatter<T, Char>
	{
		using formatter<T, Char>::parse;

		template <typename FormatContext>
		auto format(::psemek::math::vector<T, N> const & v, FormatContext & ctx) const
		{
			if constexpr (N == 0)
			{
				return std::format_to(ctx.out(), "()");
			}
			else
			{
				ctx.advance_to(std::format_to(ctx.out(), "("));
				for (std::size_t i = 0; i < v.dimension(); ++i)
				{
					if (i > 0) ctx.advance_to(std::format_to(ctx.out(), ", "));
					ctx.advance_to(formatter<T, Char>::format(v[i], ctx));
				}
				return std::format_to(ctx.out(), ")");
			}
		}
	};

}
