// vector.h (Vector<> class definition)
//
//  The WorldForge Project
//  Copyright (C) 2001  The WorldForge Project
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//  For information about WorldForge and its authors, please contact
//  the Worldforge Web Site at http://www.worldforge.org.

// Author: Ron Steinke
// Created: 2001-12-7

// Extensive amounts of this material come from the Vector2D
// and Vector3D classes from stage/math, written by Bryce W.
// Harrington, Kosh, and Jari Sundell (Rakshasa).

#ifndef WFMATH_VECTOR_H
#define WFMATH_VECTOR_H

#include <wfmath/const.h>

#include <iosfwd>

#include <cmath>

namespace WFMath {

template<int dim>
Vector<dim>& operator+=(Vector<dim>& v1, const Vector<dim>& v2);

template<int dim>
Vector<dim>& operator-=(Vector<dim>& v1, const Vector<dim>& v2);

template<int dim>
Vector<dim>& operator*=(Vector<dim>& v, CoordType d);

template<int dim>
Vector<dim>& operator/=(Vector<dim>& v, CoordType d);

template<int dim>
Vector<dim> operator+(const Vector<dim>& v1, const Vector<dim>& v2);

template<int dim>
Vector<dim> operator-(const Vector<dim>& v1, const Vector<dim>& v2);

template<int dim>
Vector<dim> operator-(const Vector<dim>& v); // Unary minus
template<int dim>
Vector<dim> operator*(CoordType d, const Vector<dim>& v);

template<int dim>
Vector<dim> operator*(const Vector<dim>& v, CoordType d);

template<int dim>
Vector<dim> operator/(const Vector<dim>& v, CoordType d);

template<int dim>
CoordType Dot(const Vector<dim>& v1, const Vector<dim>& v2);

template<int dim>
CoordType Angle(const Vector<dim>& v, const Vector<dim>& u);

// The following are defined in rotmatrix_funcs.h
/// returns m * v
template<int dim>
// m * v
Vector<dim> Prod(const RotMatrix<dim>& m, const Vector<dim>& v);

/// returns m^-1 * v
template<int dim>
// m^-1 * v
Vector<dim> InvProd(const RotMatrix<dim>& m, const Vector<dim>& v);
/// returns v * m
/**
 * This is the function to use to rotate a Vector v using a Matrix m
 **/
template<int dim>
// v * m
Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m);

/// return v * m^-1
template<int dim>
// v * m^-1
Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m);

///
template<int dim>
Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v);

///
template<int dim>
Vector<dim> operator*(const Vector<dim>& v, const RotMatrix<dim>& m);

template<int dim>
Vector<dim> operator-(const Point<dim>& c1, const Point<dim>& c2);

template<int dim>
Point<dim> operator+(const Point<dim>& c, const Vector<dim>& v);

template<int dim>
Point<dim> operator-(const Point<dim>& c, const Vector<dim>& v);

template<int dim>
Point<dim> operator+(const Vector<dim>& v, const Point<dim>& c);

template<int dim>
Point<dim>& operator+=(Point<dim>& p, const Vector<dim>& v);

template<int dim>
Point<dim>& operator-=(Point<dim>& p, const Vector<dim>& v);

template<int dim>
std::ostream& operator<<(std::ostream& os, const Vector<dim>& v);

template<int dim>
std::istream& operator>>(std::istream& is, Vector<dim>& v);

template<typename Shape>
class ZeroPrimitive;

/// A dim dimensional vector
/**
 * This class implements the 'generic' subset of the interface in
 * the fake class Shape.
 **/
template<int dim = 3>
class Vector {
	friend class ZeroPrimitive<Vector<dim> >;

public:
	/// Construct an uninitialized vector
	Vector() : m_elem{}, m_valid(false) {}

	/// Construct a copy of a vector
	Vector(const Vector&) = default;

	/// Construct a vector from an object passed by Atlas
	explicit Vector(const AtlasInType& a);

	/// Construct a vector from a point.
	explicit Vector(const Point<dim>& point);

	/**
	 * @brief Provides a global instance preset to zero.
	 */
	static const Vector<dim>& ZERO();

	friend std::ostream& operator<<<dim>(std::ostream& os, const Vector& v);

	friend std::istream& operator>><dim>(std::istream& is, Vector& v);

	/// Create an Atlas object from the vector
	AtlasOutType toAtlas() const;

	/// Set the vector's value to that given by an Atlas object
	void fromAtlas(const AtlasInType& a);

	Vector& operator=(const Vector& v) = default;

	bool isEqualTo(const Vector& v, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const;

	bool operator==(const Vector& v) const { return isEqualTo(v); }

	bool operator!=(const Vector& v) const { return !isEqualTo(v); }

	bool isValid() const { return m_valid; }

	/// make isValid() return true if you've initialized the vector by hand
	void setValid(bool valid = true) { m_valid = valid; }

	/// Zero the components of a vector
	Vector& zero();

	// Math operators

	/// Add the second vector to the first
	friend Vector& operator+=<dim>(Vector& v1, const Vector& v2);

	/// Subtract the second vector from the first
	friend Vector& operator-=<dim>(Vector& v1, const Vector& v2);

	/// Multiply the magnitude of v by d
	friend Vector& operator*=<dim>(Vector& v, CoordType d);

	/// Divide the magnitude of v by d
	friend Vector& operator/=<dim>(Vector& v, CoordType d);

	/// Take the sum of two vectors
	friend Vector operator+<dim>(const Vector& v1, const Vector& v2);

	/// Take the difference of two vectors
	friend Vector operator-<dim>(const Vector& v1, const Vector& v2);

	/// Reverse the direction of a vector
	friend Vector operator-<dim>(const Vector& v); // Unary minus
	/// Multiply a vector by a scalar
	friend Vector operator* <dim>(CoordType d, const Vector& v);

	/// Multiply a vector by a scalar
	friend Vector operator* <dim>(const Vector& v, CoordType d);

	/// Divide a vector by a scalar
	friend Vector operator/<dim>(const Vector& v, CoordType d);

	// documented outside the class definition
	friend Vector Prod<dim>(const RotMatrix<dim>& m, const Vector& v);

	friend Vector InvProd<dim>(const RotMatrix<dim>& m, const Vector& v);

	/// Get the i'th element of the vector
	CoordType operator[](const int i) const { return m_elem[i]; }

	/// Get the i'th element of the vector
	CoordType& operator[](const int i) { return m_elem[i]; }

	/// Find the vector which gives the offset between two points
	friend Vector operator-<dim>(const Point<dim>& c1, const Point<dim>& c2);

	/// Find the point at the offset v from the point c
	friend Point<dim> operator+<dim>(const Point<dim>& c, const Vector& v);

	/// Find the point at the offset -v from the point c
	friend Point<dim> operator-<dim>(const Point<dim>& c, const Vector& v);

	/// Find the point at the offset v from the point c
	friend Point<dim> operator+<dim>(const Vector& v, const Point<dim>& c);

	/// Shift a point by a vector
	friend Point<dim>& operator+=<dim>(Point<dim>& p, const Vector& rhs);

	/// Shift a point by a vector, in the opposite direction
	friend Point<dim>& operator-=<dim>(Point<dim>& p, const Vector& rhs);

	friend CoordType Cross(const Vector<2>& v1, const Vector<2>& v2);

	friend Vector<3> Cross(const Vector<3>& v1, const Vector<3>& v2);

	/// The dot product of two vectors
	friend CoordType Dot<dim>(const Vector& v1, const Vector& v2);

	/// The angle between two vectors
	friend CoordType Angle<dim>(const Vector& v, const Vector& u);

	/// The squared magnitude of a vector
	CoordType sqrMag() const;

	/// The magnitude of a vector
	CoordType mag() const { return std::sqrt(sqrMag()); }

	/// Normalize a vector
	Vector& normalize(CoordType norm = 1.0) {
		CoordType themag = mag();
		return (*this *= norm / themag);
	}

	/// An approximation to the magnitude of a vector
	/**
	 * The sloppyMag() function gives a value between
	 * the true magnitude and sloppyMagMax multiplied by the
	 * true magnitude. sloppyNorm() uses sloppyMag() to normalize
	 * the vector. This is currently only implemented for
	 * dim = {1, 2, 3}. For all current implementations,
	 * sloppyMagMax is greater than or equal to one.
	 * The constant sloppyMagMaxSqrt is provided for those
	 * who want to most closely approximate the true magnitude,
	 * without caring whether it's too low or too high.
	 **/
	CoordType sloppyMag() const;
	/// Approximately normalize a vector
	/**
	 * Normalize a vector using sloppyMag() instead of the true magnitude.
	 * The new length of the vector will be between norm/sloppyMagMax()
	 * and norm.
	 **/
	Vector& sloppyNorm(CoordType norm = 1.0);

	// Can't seem to implement these as constants, implementing
	// inline lookup functions instead.
	/// The maximum ratio of the return value of sloppyMag() to the true magnitude
	static CoordType sloppyMagMax();
	/// The square root of sloppyMagMax()
	/**
	 * This is provided for people who want to obtain maximum accuracy from
	 * sloppyMag(), without caring whether the answer is high or low.
	 * The result sloppyMag()/sloppyMagMaxSqrt() will be within sloppyMagMaxSqrt()
	 * of the true magnitude.
	 **/
	static CoordType sloppyMagMaxSqrt();

	/// Rotate the vector in the (axis1, axis2) plane by the angle theta
	Vector& rotate(int axis1, int axis2, CoordType theta);

	/// Rotate the vector in the (v1, v2) plane by the angle theta
	/**
	 * This throws CollinearVectors if v1 and v2 are parallel.
	 **/
	Vector& rotate(const Vector& v1, const Vector& v2, CoordType theta);

	/// Rotate the vector using a matrix
	Vector& rotate(const RotMatrix<dim>&);

	// mirror image functions

	/// Reflect a vector in the direction of the i'th axis
	Vector& mirror(const int i) {
		m_elem[i] *= -1;
		return *this;
	}

	/// Reflect a vector in the direction specified by v
	Vector& mirror(const Vector& v) { return operator-=(*this, 2 * v * Dot(v, *this) / v.sqrMag()); }
	/// Reflect a vector in all directions simultaneously.
	/**
	 * This is a nice way to implement the parity operation if dim is odd.
	 **/
	Vector& mirror() { return operator*=(*this, -1); }

	// Specialized 2D/3D stuff starts here

	// The following functions are defined only for
	// two dimensional (rotate(CoordType), Vector<>(CoordType, CoordType))
	// and three dimensional (the rest of them) vectors.
	// Attempting to call these on any other vector will
	// result in a linker error.

	/// 2D only: construct a vector from (x, y) coordinates
	Vector(CoordType x, CoordType y);

	/// 3D only: construct a vector from (x, y, z) coordinates
	Vector(CoordType x, CoordType y, CoordType z);

	/// 2D only: rotate a vector by an angle theta
	Vector& rotate(CoordType theta);

	/// 3D only: rotate a vector about the x axis by an angle theta
	Vector& rotateX(CoordType theta);

	/// 3D only: rotate a vector about the y axis by an angle theta
	Vector& rotateY(CoordType theta);

	/// 3D only: rotate a vector about the z axis by an angle theta
	Vector& rotateZ(CoordType theta);

	/// 3D only: rotate a vector about the i'th axis by an angle theta
	Vector& rotate(const Vector& axis, CoordType theta);

	/// 3D only: rotate a vector using a Quaternion
	Vector& rotate(const Quaternion& q);

	// Label the first three components of the vector as (x,y,z) for
	// 2D/3D convienience

	/// Access the first component of a vector
	CoordType x() const { return m_elem[0]; }

	/// Access the first component of a vector
	CoordType& x() { return m_elem[0]; }

	/// Access the second component of a vector
	CoordType y() const { return m_elem[1]; }

	/// Access the second component of a vector
	CoordType& y() { return m_elem[1]; }

	/// Access the third component of a vector
	CoordType z() const;

	/// Access the third component of a vector
	CoordType& z();

	/// Flip the x component of a vector
	Vector& mirrorX() { return mirror(0); }

	/// Flip the y component of a vector
	Vector& mirrorY() { return mirror(1); }

	/// Flip the z component of a vector
	Vector& mirrorZ();

	/// 2D only: construct a vector from polar coordinates
	Vector& polar(CoordType r, CoordType theta);

	/// 2D only: convert a vector to polar coordinates
	void asPolar(CoordType& r, CoordType& theta) const;

	/// 3D only: construct a vector from polar coordinates
	Vector& polar(CoordType r, CoordType theta, CoordType z);

	/// 3D only: convert a vector to polar coordinates
	void asPolar(CoordType& r, CoordType& theta, CoordType& z) const;

	/// 3D only: construct a vector from shperical coordinates
	Vector& spherical(CoordType r, CoordType theta, CoordType phi);

	/// 3D only: convert a vector to shperical coordinates
	void asSpherical(CoordType& r, CoordType& theta, CoordType& phi) const;

	const CoordType* elements() const { return m_elem; }

private:
	double _scaleEpsilon(const Vector& v, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const { return _ScaleEpsilon(m_elem, v.m_elem, dim, epsilon); }

	CoordType m_elem[dim];
	bool m_valid;
};

template<>
inline CoordType Vector<3>::z() const {
	return m_elem[2];
}

template<>
inline CoordType& Vector<3>::z() {
	return m_elem[2];
}

template<>
inline Vector<3>& Vector<3>::mirrorZ() {
	return mirror(2);
}

/// 2D only: get the z component of the cross product of two vectors
CoordType Cross(const Vector<2>& v1, const Vector<2>& v2);

/// 3D only: get the cross product of two vectors
Vector<3> Cross(const Vector<3>& v1, const Vector<3>& v2);

/// Check if two vectors are parallel
/**
 * Returns true if the vectors are parallel. For parallel
 * vectors, same_dir is set to true if they point the same
 * direction, and false if they point opposite directions
 **/
template<int dim>
bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2, bool& same_dir);

/// Check if two vectors are parallel
/**
 * Convienience wrapper if you don't care about same_dir
 **/
template<int dim>
bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2);

/// Check if two vectors are perpendicular
template<int dim>
bool Perpendicular(const Vector<dim>& v1, const Vector<dim>& v2);

template<int dim>
inline Vector<dim> operator+(const Vector<dim>& v1, const Vector<dim>& v2) {
	Vector<dim> ans(v1);

	ans += v2;

	return ans;
}

template<int dim>
inline Vector<dim> operator-(const Vector<dim>& v1, const Vector<dim>& v2) {
	Vector<dim> ans(v1);

	ans -= v2;

	return ans;
}

template<int dim>
inline Vector<dim> operator*(const Vector<dim>& v, CoordType d) {
	Vector<dim> ans(v);

	ans *= d;

	return ans;
}

template<int dim>
inline Vector<dim> operator*(CoordType d, const Vector<dim>& v) {
	Vector<dim> ans(v);

	ans *= d;

	return ans;
}

template<int dim>
inline Vector<dim> operator/(const Vector<dim>& v, CoordType d) {
	Vector<dim> ans(v);

	ans /= d;

	return ans;
}

template<int dim>
inline bool Parallel(const Vector<dim>& v1,
					 const Vector<dim>& v2,
					 bool& same_dir) {
	CoordType dot = Dot(v1, v2);

	same_dir = (dot > 0);

	return Equal(dot * dot, v1.sqrMag() * v2.sqrMag());
}

template<int dim>
inline bool Parallel(const Vector<dim>& v1, const Vector<dim>& v2) {
	bool same_dir;

	return Parallel(v1, v2, same_dir);
}

template<>
inline CoordType Vector<1>::sloppyMagMax() {
	return 1.f;
}

template<>
inline CoordType Vector<2>::sloppyMagMax() {
	return 1.082392200292393968799446410733f;
}

template<>
inline CoordType Vector<3>::sloppyMagMax() {
	return 1.145934719303161490541433900265f;
}

template<>
inline CoordType Vector<1>::sloppyMagMaxSqrt() {
	return 1.f;
}

template<>
inline CoordType Vector<2>::sloppyMagMaxSqrt() {
	return 1.040380795811030899095785063701f;
}

template<>
inline CoordType Vector<3>::sloppyMagMaxSqrt() {
	return 1.070483404496847625250328653179f;
}

} // namespace WFMath

#endif // WFMATH_VECTOR_H
