#pragma once

#include "data_math.hpp"

namespace saw {
template<uint64_t D>
class iterator final {
public:
	template<typename Func
	void apply(Func&& func){
	/// TODO !!!!	
	}
};
namespace math {

/*
template<typename T, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<typename T::InnerType,Encoding> norm_2(const data<T,Encoding>& d){
	return {};
}
*/

template<typename T, uint64_t D, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Scalar<T>, Encoding> dot(const data<schema::Vector<T,D>, Encoding>& left, const data<schema::Vector<T,D>, Encoding>& right){
	data<schema::Scalar<T>,Encoding> val;
	auto& inner = val({});
	for(uint64_t i = 0u; i < D; ++i){
		inner = inner + left({{i}}) * right({{i}});
	}

	return val;
}

template<typename T,typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Scalar<T>,Encoding> sqrt(const data<schema::Scalar<T>,Encoding>& inp){
	data<schema::Scalar<T>,Encoding> out;
	out.at({}).set(std::sqrt(inp.at({}).get()));
	return out;
}

template<typename T, uint64_t D, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Vector<T,D>, Encoding> normalize(const data<schema::Vector<T,D>>& input ) {
	auto inp_dot = dot<T,D,Encoding>(input,input);
	if(inp_dot.at({}).get() <= 0){
		return input;
	}
	auto sqrt_inp_dot = sqrt<T>(inp_dot);

	saw::data<schema::Vector<T,D>, Encoding> out;
	for(uint64_t i = 0u; i < D; ++i){
		out.at({{i}}) = (input.at({{i}}) / sqrt_inp_dot.at({}));
	}
	return out;
}

template<typename T, uint64_t D, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Vector<T,D>, Encoding> vectorize_data(const data<schema::FixedArray<T,D>>& dat){
	data<schema::Vector<T,D>,Encoding> vec_data;
	for(uint64_t i{0u}; i < D; ++i){
		vec_data.at({{i}}) = dat.at({{i}});
	}

	return vec_data;
}

template<typename T, uint64_t M, uint64_t N, uint64_t K, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Matrix<T,M,N>, Encoding> multiply(const data<schema::Matrix<T,M,K>, Encoding>& left, const data<schema::Matrix<T,K,N>, Encoding>& right){
	data<schema::Matrix<T,M,N>, Encoding> lr;

	for(uint64_t i = 0u; i < M; ++i){
		for(uint64_t j = 0u; j < N; ++j){
			for(uint64_t k = 0u; k < K; ++k){
				lr.at({{i,j}}) = lr.at({{i,j}}) + left.at({{i,k}}) * right.at({{k,j}});
			}
		}
	}

	return lr;
}

template<typename T, uint64_t M, uint64_t N, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Vector<T,M>, Encoding> multiply(const data<schema::Matrix<T,M,N>, Encoding>& left, const data<schema::Vector<T,N>, Encoding>& right){
	data<schema::Vector<T,M>, Encoding> lr;

	for(uint64_t i = 0u; i < M; ++i){
		for(uint64_t j = 0u; j < N; ++j){
			lr.at({{i}}) = lr.at({{i}}) + left.at({{i,j}}) * right.at({{j}});
		}
	}

	return lr;
}

template<typename T, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Vector<T,3u>, Encoding> cross(
	const data<schema::Vector<T,3u>, Encoding> lh,
	const data<schema::Vector<T,3u>, Encoding> rh
){
	data<schema::Vector<T,3u>, Encoding> cross_prod;

	cross_prod.at({{0u}}) = lh.at({{1u}}) * rh.at({{2u}}) - lh.at({{2u}}) * rh.at({{1u}});
	cross_prod.at({{1u}}) = lh.at({{2u}}) * rh.at({{0u}}) - lh.at({{0u}}) * rh.at({{2u}});
	cross_prod.at({{2u}}) = lh.at({{0u}}) * rh.at({{1u}}) - lh.at({{1u}}) * rh.at({{0u}});

	return cross_prod;
}

template<typename T, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Scalar<T>,Encoding> cross(
	const data<schema::Vector<T,2u>, Encoding> lh,
	const data<schema::Vector<T,2u>, Encoding> rh
){
	data<schema::Scalar<T>, Encoding> cross_prod;

	cross_prod.at({}) = lh.at({{0u}}) * rh.at({{1u}}) - lh.at({{1u}}) * rh.at({{0u}});

	return cross_prod;
}

template<typename T, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Scalar<T>,Encoding> cos(
	const data<schema::Scalar<T>,Encoding>& val
){
	data<schema::Scalar<T>,Encoding> ret;
	ret.at({}).set(std::cos(val.at({}).get()));
	return ret;
}

template<typename T, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Scalar<T>,Encoding> sin(
	const data<schema::Scalar<T>,Encoding>& val
){
	data<schema::Scalar<T>,Encoding> ret;
	ret.at({}).set(std::sin(val.at({}).get()));
	return ret;
}


template<typename T, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Vector<T,2u>,Encoding> rotate(
	const data<schema::Vector<T,2u>, Encoding> vec,
	const data<schema::Scalar<T>, Encoding> rot
){
	data<schema::Vector<T,2u>, Encoding> rot_vec;
	rot_vec.at({{0u}}) = vec.at({{0u}}) * cos(rot).at({});
	rot_vec.at({{1u}}) = vec.at({{1u}}) * sin(rot).at({});
	return rot_vec;
}

template<typename T, uint64_t D, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Vector<T,D>,Encoding> scale(
	const data<schema::Vector<T,D>, Encoding> vec,
	const data<schema::Scalar<T>, Encoding> scale
){
	data<schema::Vector<T,D>, Encoding> sc_vec;
	for(uint64_t i = 0u; i< D; ++i){
		sc_vec.at({{i}}) = vec.at({{i}}) * scale;
	}
	return sc_vec;
}

template<typename T, uint64_t... Ds, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
data<schema::Tensor<T,Ds...>, Encoding> fill(
	const data<T,Encoding>& filler){
	data<schema::Tensor<T,Ds...>, Encoding> tbf;

	iterator<sizeof...(Ds)>::apply([&](const auto& index){
		tbf.at(index) = filler;
	}, {}, {{Ds...}});
	return tbf;
}
}
}