//
// This source code is property of the Computer Graphics and Visualization 
// chair of the TU Dresden. Do not distribute in modified or unmodified form! 
// Copyright (C) 2015 CGV TU Dresden - All Rights Reserved
//
/*************************************************************************
Template class for vectors.
This template class defines small vectors of a defined dimension with
an arbitrary data type. Basic operations such as addition of vectors and
multiplication of a vector and a scalar are supported via operator 
overloading. Also more advanced functions such as normalization and 
iterating through the elements are implemented.
At the end of this file the data types "point2d", "point3d" and 
"point4d" are specified as a tinyvec of doubles with the according dimension.
*************************************************************************/

#pragma once

#include <iostream>
#include <algorithm>

#pragma warning( disable : 4996 )


template <typename T,int N>
class tiny_vec
{

	T  _data[N];

public:
	typedef T              value_type;
	typedef T&             reference;
	typedef const T&       const_reference;
	typedef std::size_t    size_type;
	typedef std::ptrdiff_t difference_type;
	typedef T*             pointer;
	typedef const T*       const_pointer;
	typedef T* iterator;
	typedef const T* const_iterator;
	typedef std::reverse_iterator<iterator> reverse_iterator;
	typedef std::reverse_iterator<const_iterator> const_reverse_iterator;

	tiny_vec();

	template <typename InputIterator>
	tiny_vec(InputIterator first,InputIterator last)
	{
		std::copy(first,last,begin());
	}


	tiny_vec(size_t n, const T* marray);

	~tiny_vec();

	tiny_vec(const T&x,const T&y);

	tiny_vec(const T&x,const T&y, const T&z);

	tiny_vec(const T&x, const T&y, const T&z,const T& w);	

	int size() const;

	operator T*();

	operator const T*() const;

	void set(const T& x);

	void set(const T& x, const T& y);

	void set(const T& x, const T& y,const T& z);

	void set(const T& x, const T& y,const T& z, const T& w);

	tiny_vec& operator=(const T &s);

	tiny_vec& operator=(int s);

	tiny_vec& operator=(const tiny_vec & v);

	void fill(const T& s);

	void zeros(size_t m);

	void zeros();

	void ones();

	void ones(size_t m);

	iterator begin();

	iterator end();

	const_iterator begin() const;

	const_iterator end() const;

	reverse_iterator rbegin();

	reverse_iterator rend();

	const_reverse_iterator rbegin() const;

	const_reverse_iterator rend() const;	

	bool operator==(const tiny_vec& v) const;

	bool operator!=(const tiny_vec& v) const;

	tiny_vec operator-() const;

	tiny_vec& operator+=(const T& s);

	tiny_vec& operator+=(int s);

	tiny_vec operator+(const T& s) const;

	tiny_vec operator+(int s) const;

	tiny_vec& operator-=(const T& s);

	tiny_vec& operator-=(int s);

	tiny_vec operator-(const T& s) const;

	tiny_vec operator-(int s) const;

	tiny_vec& operator*=(const T& s);

	tiny_vec& operator*=(int s);

	tiny_vec operator*(const T& s) const;

	tiny_vec operator*(int s) const;

	tiny_vec& operator/=(const T& s);

	tiny_vec& operator/=(int s);

	tiny_vec operator/(const T& s) const;

	tiny_vec operator/(int s) const;

	tiny_vec& operator+=(const tiny_vec& v);

	tiny_vec operator+(const tiny_vec& v) const;

	tiny_vec& operator-=(const tiny_vec& v);

	tiny_vec operator-(const tiny_vec& v) const;

	tiny_vec& operator*=(const tiny_vec& v);

	tiny_vec operator*(const tiny_vec& v) const;

	tiny_vec& operator/=(const tiny_vec& v);

	tiny_vec operator/(const tiny_vec& v) const;

	T& operator()(size_t i);

	T operator()(size_t i) const;

	T& x() {return _data[0];};
	T& y() {return _data[1];};
	T& z() {return _data[2];};

	T x() const {return _data[0];};
	T y() const {return _data[1];};
	T z() const {return _data[2];};


	void sort();

	void random_shuffle();

	T& min_elem();

	size_t min_index();

	T& max_elem();

	size_t max_index();

	T& median_elem();

	size_t median_index();

	T sqr_length();

	T length();

	void normalize();

};



template <typename T, int N>
tiny_vec<T,N>::tiny_vec(){}


template <typename T, int N>
tiny_vec<T,N>::tiny_vec(size_t n, const T* marray)
{			
	std::copy(marray,marray+_size,begin());		
}

template <typename T, int N>
tiny_vec<T,N>::~tiny_vec(){}

template <typename T, int N>
tiny_vec<T,N>::tiny_vec(const T&x,const T&y)
{
	_data[0]=x;
	_data[1]=y;
}

template <typename T, int N>
tiny_vec<T,N>::tiny_vec(const T&x,const T&y, const T&z)
{
	_data[0]=x;
	_data[1]=y;
	_data[2]=z;
}

template <typename T, int N>
tiny_vec<T,N>::tiny_vec(const T&x, const T&y, const T&z,const T& w)
{
	_data[0]=x;
	_data[1]=y;
	_data[2]=z;
	_data[3]=w;
}

template <typename T, int N>
void tiny_vec<T,N>::set(const T& x)
{
	_data[0]=x;
}

template <typename T, int N>
void tiny_vec<T,N>::set(const T& x, const T& y)
{
	_data[0]=x;
	_data[1]=y;

}
template <typename T, int N>
void tiny_vec<T,N>::set(const T& x, const T& y,const T& z)
{
	_data[0]=x;
	_data[1]=y;
	_data[2]=z;

}
template <typename T, int N>
void tiny_vec<T,N>::set(const T& x, const T& y,const T& z, const T& w)
{
	_data[0]=x;
	_data[1]=y;
	_data[2]=z;
	_data[3]=w;
}

template <typename T, int N>
int tiny_vec<T,N>::size() const
{
	return N;
}

template <typename T, int N>
tiny_vec<T,N>::operator T*()
{
	return _data;
}


template <typename T, int N>
tiny_vec<T,N>::operator const T*() const
{
	return _data;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator=(const T &s)
{
	for(int i=0;i < N;i++)
		_data[i]=s;

	return *this;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator=(int s)
{
	for(int i=0;i<N;i++)
		_data[i]=(T)s;

	return *this;
}


template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator=(const tiny_vec & v)
{
	if(&v== this) return *this;

	std::copy(v.begin(),v.end(),begin());
	return *this;
}



template <typename T, int N>
void tiny_vec<T,N>::fill(const T& s)
{
	std::fill(begin(),end(),s);
}

template <typename T, int N>
void tiny_vec<T,N>::zeros(size_t m)
{
	T zero;
	zero=0;
	std::fill(begin(), end(), zero);
}

template <typename T, int N>
void tiny_vec<T,N>::zeros()
{	
	T zero;
	zero=0;
	std::fill(begin(), end(), zero);
}

template <typename T, int N>
void tiny_vec<T,N>::ones()
{
	T one;
	one = 1;
	std::fill(begin(), end(), one);
}

template <typename T, int N>
void tiny_vec<T,N>::ones(size_t m)
{
	resize(m);
	T one;
	one = 1;
	std::fill(begin(), end(), one);
}

template <typename T, int N>
typename tiny_vec<T,N>::iterator tiny_vec<T,N>::begin()
{
	return _data;
}

template <typename T, int N>
typename tiny_vec<T,N>::const_iterator tiny_vec<T,N>::begin() const
{
	return _data;
}

template <typename T, int N>
typename tiny_vec<T,N>::iterator tiny_vec<T,N>::end()
{
	return _data + N;
}

template <typename T, int N>
typename tiny_vec<T,N>::const_iterator tiny_vec<T,N>::end() const
{
	return _data +N;
}

template <typename T, int N>
typename tiny_vec<T,N>::reverse_iterator tiny_vec<T,N>::rbegin()
{
	return reverse_iterator(end());
}

template <typename T, int N>
typename tiny_vec<T,N>::reverse_iterator tiny_vec<T,N>::rend()
{
	return reverse_iterator(begin());
}

template <typename T, int N>
typename tiny_vec<T,N>::const_reverse_iterator tiny_vec<T,N>::rbegin() const
{
	return const_reverse_iterator(end());
}

template <typename T, int N>
typename tiny_vec<T,N>::const_reverse_iterator tiny_vec<T,N>:: rend() const
{
	return const_reverse_iterator(begin());
}

template <typename T, int N>
bool tiny_vec<T,N>::operator==(const tiny_vec<T,N>& v) const
{

	return std::equal(begin(),end(),v.begin());
}


template <typename T, int N>
bool tiny_vec<T,N>::operator!=(const tiny_vec<T,N>& v) const
{
	return !(*this == v);
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator-() const
{
	return  (T)-1* *this;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator+=(const T& s)
{
#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem+=s;});
#else
	for(int i=0;i<size();i++)
		_data[i]+=s;
#endif
	return *this;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator+=(int s)
{
#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem+=(T)s;});
#else
	for(int i=0;i<size();i++)
		_data[i]+=s;
#endif
	return *this;
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator+(const T& s) const
{
	tiny_vec r = *this;
	r += s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator+(int s) const
{
	tiny_vec r = *this;
	r += s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator-=(const T& s)
{
#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem-=s;});
#else
	for(int i=0; i < N;i++)
		_data[i]-=s;
#endif
	return *this;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator-=(int s)
{

#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem-=(T)s;});
#else
	for(int i=0;i<N;i++)
		_data[i]-=(T)s;
#endif
	return *this;
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator-(const T& s) const
{
	tiny_vec r = *this;
	r -= s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator-(int s) const
{
	tiny_vec r = *this;
	r -= (T)s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator*=(const T& s)
{
#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem *= s;});
#else
	for(int i=0;i < N;i++)
		_data[i]*=s;
#endif
	return *this;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator*=(int s)
{
#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem *= (T)s;});
#else
	for(int i=0; i <N; i++)
		_data[i]*=(T)s;
#endif
	return *this;
}


template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator*(const T& s) const
{
	tiny_vec<T,N> r = *this;
	r *= s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator*(int s) const
{
	tiny_vec<T,N> r = *this;
	r *= (T)s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator/=(const T& s)
{
#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem/=s;});
#else
	for(int i=0; i <N; i++)
		_data[i]/=s;
#endif
	return *this;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator/=(int s)
{
#ifdef _HAS_CPP0X
	std::for_each(begin(),end(),[&s](T& elem){elem/=(T)s;});
#else
	for(int i=0;i <N; i++)
		_data[i]/=(T)s;
#endif
	return *this;
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator/(const T& s) const
{
	tiny_vec<T,N> r = *this;
	r /= s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator/(int s) const
{
	tiny_vec<T,N> r = *this;
	r /= (T)s;
	return r;
}

template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator+=(const tiny_vec& v)
{
#ifdef _HAS_CPP0X
	const_iterator vit=v.begin();
	std::for_each(begin(),end(),[&vit](T& elem){elem+=*vit++;});
#else
	for(int i=0; i <N;i++)
		_data[i] += v[i];
#endif
	return *this;
}


template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator+(const tiny_vec& v) const
{
	tiny_vec r = *this;
	r += v;
	return r;
}


template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator-=(const tiny_vec& v)
{
#ifdef _HAS_CPP0X
	const_iterator vit=v.begin();
	std::for_each(begin(),end(),[&vit](T& elem){elem-=*vit++;});
#else
	for(int i =0; i < N; i++)
		_data[i]-= v[i];
#endif
	return *this;
}


template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator-(const tiny_vec& v) const
{
	tiny_vec r = *this;
	r -= v;
	return r;
}


template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator*=(const tiny_vec& v)
{
#ifdef _HAS_CPP0X
	auto vit=v.begin();
	std::for_each(begin(),end(),[&vit](T& elem){elem*=*vit++;});
#else
	for(int i=0; i < N; i++)
		_data[i]*= v[i];
#endif
	return *this;
}


template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator*(const tiny_vec& v) const
{
	tiny_vec<T,N> r = *this;
	r *= v;
	return r;
}


template <typename T, int N>
tiny_vec<T,N>& tiny_vec<T,N>::operator/=(const tiny_vec& v)
{
#ifdef _HAS_CPP0X
	auto vit=v.begin();
	std::for_each(begin(),end(),[&vit](T& elem){elem/=*vit++;});
#else
	for(int i = 0; i < N; i++)
		_data[i] /= v[i];
#endif
	return *this;
}


template <typename T, int N>
tiny_vec<T,N> tiny_vec<T,N>::operator/(const tiny_vec<T,N>& v) const
{
	tiny_vec r = *this;
	r /= v;
	return r;
}


template <typename T, int N>
T& tiny_vec<T,N>::operator()(size_t i)
{
	assert(i >= 0 && i < N);
	return _data[i];
}

template <typename T, int N>
T tiny_vec<T,N>::operator()(size_t i) const
{
	assert(i >= 0 && i < N);
	return _data[i];
}

template <typename T, int N>
void tiny_vec<T,N>::sort()
{
	std::sort(begin(),end());
}

template <typename T, int N>
void tiny_vec<T,N>::random_shuffle()
{
	std::random_shuffle(begin(),end());
}

template <typename T, int N>
T& tiny_vec<T,N>::min_elem()
{
	return *std::min_element(begin(),end());
}

template <typename T, int N>
size_t tiny_vec<T,N>::min_index()
{
	return std::distance(begin(),std::min_element(begin(),end()));
}

template <typename T, int N>
T& tiny_vec<T,N>::max_elem()
{
	return *std::max_element(begin(),end());
}

template <typename T, int N>
size_t tiny_vec<T,N>::max_index()
{
	return std::distance(begin(),std::max_element(begin(),end()));
}

template <typename T, int N>
T& tiny_vec<T,N>::median_elem()
{
	iterator it= begin() + _size/2;
	std::nth_element(begin(),it,end());
	return *it;
}

template <typename T, int N>
size_t tiny_vec<T,N>::median_index()
{
	return begin() +_size/2;
}

template <typename T, int N>
T tiny_vec<T,N>::sqr_length()
{
	iterator it= begin();
	T l = 0;
	while(it != end())
		l += (*it)*(*it++);

	return l;
}

template <typename T, int N>
T tiny_vec<T,N>::length()
{
	T l = sqr_length();
	return sqrt(l);
}


template <typename T, int N>
tiny_vec<T,N> operator+(const T& s, const tiny_vec<T,N>& v)
{
	return v+s;
}


template <typename T, int N>
tiny_vec<T,N> operator+(int s, const tiny_vec<T,N>& v)
{
	return v+(T)s;
}

template <typename T, int N>
tiny_vec<T,N> operator-(const T& s, const tiny_vec<T,N>& v)
{
	return -v+s;
}


template <typename T, int N>
tiny_vec<T,N> operator-(int s, const tiny_vec<T,N>& v)
{
	return -v+(T)s;
}


template <typename T, int N>
tiny_vec<T,N> operator*(const T& s, const tiny_vec<T,N>& v)
{
	return v*s;
}


template <typename T, int N>
tiny_vec<T,N> operator*(int s, const tiny_vec<T,N>& v)
{
	return v*(T)s;
}

template <typename T, int N>
tiny_vec<T,N> operator/(const T& s, const tiny_vec<T,N>& v)
{
	return v*(T)(1.0/s);
}

template <typename T, int N>
tiny_vec<T,N> operator/(int s, const tiny_vec<T,N>& v)
{
	return v*(T)(1.0/s);
}

template <typename T, int N>
T dot(const tiny_vec<T,N>& v1, const tiny_vec<T,N>& v2)
{
	return std::inner_product(v1.begin(),v1.end(),v2.begin(),(T)0);
}



template <typename T>
tiny_vec<T,3> cross(const tiny_vec<T,3>& b, const tiny_vec<T,3>& c)
{

	tiny_vec<T,3> a;
	a[0] = b(1)*c(2) - b(2)*c(1);
	a[1] = b(2)*c(0) - b(0)*c(2);
	a[2] = b(0)*c(1) - b(1)*c(0);
	return a;
}


template <typename T, int N>
tiny_vec<T,N+1> homog(const tiny_vec<T,N>& v)
{
	tiny_vec<T,N+1> vh;
	std::copy(v.begin(),v.end(),vh.begin());
	vh(v.size()) = (T)1;
	return vh;
}

template <typename T, int N>
tiny_vec<T,N-1> unhomog(const tiny_vec<T,N>& v)
{
	tiny_vec<T,N-1> vh;
	T w = *(v.end()-1);
#ifdef _HAS_CPP0X
	auto vit=v.begin();
	std::for_each(vh.begin(),vh.end(),[&](T& elem){elem=*vit++/w;});
#else
	for(int i = 0; i < N-1; i++)
		vh[i] = v[i]/w;
#endif
	return vh;
}




template <typename T, int N>
std::ostream& operator<<(std::ostream& out, const tiny_vec<T,N>& v)
{
#ifdef _HAS_CPP0X
	auto it = v.begin();
	if(it != v.end())
		out << *it ++; 
	std::for_each(it, v.end(),[&out](const T& elem){ out << " " << elem;});
#else
	if(v.size() > 0)
		out << v[0];
	for(int i = 1; i < v.size(); i++)
		out <<" " <<v[i];
#endif
	return out;
}


template <typename T,int N>
std::istream& operator>>(std::istream& in, tiny_vec<T,N>& v)
{
#ifdef _HAS_CPP0X
	std::for_each(v.begin(), v.end(),[&in](T& elem){ in >> elem;});
#else
	for(int i = 0; i < v.size(); i++)
		in >> v[i];
#endif
	return in;
}

template <typename T, int N>
void tiny_vec<T,N>::normalize()
{
	T l = length();
	if(l != 0)
		operator/=(l);
}



typedef tiny_vec<double, 2> point2d;
typedef tiny_vec<double, 3> point3d;
typedef tiny_vec<double, 4> point4d;