#include <math.h>
#include <assert.h>
#include "vector3.h"


using namespace Geometry;
using namespace std;

// Aux. function that translates a value to a valid interval
double normalize(double min, double max, double value)
{
  double distance = max - min;
  if (distance <= 0)
    {
      cerr << __FILE__ << ": min > max in \"normalize\" function" << endl;
      return value;
    }

  while (value > max)
    value -= distance;

  while (value < min)
    value += distance;

  return value;
}




// Default constructor
template <class T> Vector3D<T>::Vector3D()
{
	_data[0] = _data[1] = _data[2] = (T) 0.0;
}

// Constructor
template <class T> Vector3D<T>::Vector3D<T>(const T& x, const T& y, const T& z)
{
	_data[0] = x;
	_data[1] = y;
	_data[2] = z;
}

// Constructor
template <class T> Vector3D<T>::Vector3D<T>(const T data[3])
{
	_data[0] = data[0];
	_data[1] = data[1];
	_data[2] = data[2];
}

// Copy constructor
template <class T> Vector3D<T>::Vector3D<T>(const Vector3D<T>& v)
{
	_data[0] = v._data[0];
	_data[1] = v._data[1];
	_data[2] = v._data[2];
}

// Constructor
template <class T> Vector3D<T>::Vector3D<T>(const Vector3D<T>* v)
{
	_data[0] = v->_data[0];
	_data[1] = v->_data[1];
	_data[2] = v->_data[2];
}

// Copy operator
template <class T> Vector3D<T>& Vector3D<T>::operator=(const Vector3D<T>& src)
{
	_data[0] = src._data[0];
	_data[1] = src._data[1];
	_data[2] = src._data[2];

	return *this;
}

// Comparison operator
template <class T> bool Vector3D<T>::operator==(const Vector3D<T>& src) const
{
	return ABS(_data[0] - src._data[0]) < ALMOST_ZERO &&
		ABS(_data[1] - src._data[1]) < ALMOST_ZERO &&
		ABS(_data[2] - src._data[2]) < ALMOST_ZERO;
}

// Normalizes a vector (sets the module to 1)
template <class T> void Vector3D<T>::normalize()
{
	double mod = module();

	_data[0] = (T) (_data[0] / mod);
	_data[1] = (T) (_data[1] / mod);
	_data[2] = (T) (_data[2] / mod);
}

// Returns the module of the vector
template <class T> double Vector3D<T>::module() const
{
  return sqrt(_data[0]*_data[0] + _data[1]*_data[1] + _data[2]*_data[2]);
}

// Equivalent to module(), for when only relative comparisons are needed
template <class T> double Vector3D<T>::squaredModule() const
{
  return _data[0]*_data[0] + _data[1]*_data[1] + _data[2]*_data[2];
}


// Accesses an element of the vector
template <class T> T& Vector3D<T>::operator[](int index)
{
	return _data[index];
}

// Calculates the angle between 2 vectors
template <class T> double Vector3D<T>::angle(const Vector3D<T>& v) const
{
	double m = module();
	double mv = v.module();
	double dp = dotProduct(v);

	if ( (m*mv) < ALMOST_ZERO )
		return 0.0;

  return acos( dp / (m*mv) );
}

// Calculates the distance between 2 vectors
template <class T> double Vector3D<T>::distance(const Vector3D<T>& v) const
{
  double x = _data[0] - v._data[0];
  double y = _data[1] - v._data[1];
  double z = _data[2] - v._data[2];

  return sqrt(x*x + y*y + z*z);
}

// Faster version than ::distance, for when only comparisons are needed
template <class T> double Vector3D<T>::squaredDistance(const Vector3D<T>& v) const
{
  double x = _data[0] - v._data[0];
  double y = _data[1] - v._data[1];
  double z = _data[2] - v._data[2];

  return x*x + y*y + z*z;
}

// Computes the infinite-degree distance to 'v'
template <class T> double Vector3D<T>::infDistance(const Vector3D<T>& v) const
{
  double x = _data[0] - v._data[0];
  double y = _data[1] - v._data[1];
  double z = _data[2] - v._data[2];

	x = ABS(x);
	y = ABS(y);
	z = ABS(z);

  return MAX(x,MAX(y,z));
}

// Computes the degree-1 (Manhattan) distance to 'v'
template <class T> double Vector3D<T>::manhattanDistance(const Vector3D<T>& v) const
{
  double x = _data[0] - v._data[0];
  double y = _data[1] - v._data[1];
  double z = _data[2] - v._data[2];

  return ABS(x) + ABS(y) + ABS(z);
}

// Rotates a vector around the X axis
template <class T> void Vector3D<T>::rotateX(double angle)
{
  double y = _data[1];
  double z = _data[2];
	double cosine = cos(angle);
	double sine = sin(angle);

  _data[1] = (T) (y*cosine - z*sine);
  _data[2] = (T) (y*sine + z*cosine);
}

// Rotates a vector around the Y axis
template <class T> void Vector3D<T>::rotateY(double angle)
{
  double x = _data[0];
  double z = _data[2];
	double cosine = cos(angle);
	double sine = sin(angle);

  _data[2] = (T) (z*cosine - x*sine);
  _data[0] = (T) (z*sine + x*cosine);
}

// Rotates a vector around the Z axis
template <class T> void Vector3D<T>::rotateZ(double angle)
{
  double x = _data[0];
  double y = _data[1];
	double cosine = cos(angle);
	double sine = sin(angle);

  _data[0] = (T) (x*cosine - y*sine);
  _data[1] = (T) (x*sine + y*cosine);
}

// Calculates the dot product of 2 vectors
template <class T> double Vector3D<T>::dotProduct(const Vector3D<T>& v) const
{
	return (*this) * v;
}

// Calculates the dot product of 2 vectors
template <class T> double Vector3D<T>::operator*(const Vector3D<T>& v) const
{
	return _data[0]*v._data[0] + _data[1]*v._data[1] + _data[2]*v._data[2];
}

// Divides a vector by a scalar factor
template <class T> Vector3D<T> Vector3D<T>::operator/(T factor) const
{
	return Vector3D<T>(_data[0]/factor, _data[1]/factor, _data[2]/factor);
}

// Multiplies a vector by a scalar factor and assigns the result to itself
template <class T> const Vector3D<T>& Vector3D<T>::operator/=(T factor)
{
	_data[0] /= factor;
	_data[1] /= factor;
	_data[2] /= factor;

	return *this;
}

// Multiplies a vector by a scalar factor
template <class T> Vector3D<T> Vector3D<T>::operator*(T factor) const
{
	return Vector3D<T>(_data[0]*factor, _data[1]*factor, _data[2]*factor);
}

// Multiplies a vector by a scalar factor and assigns the result to itself
template <class T> const Vector3D<T>& Vector3D<T>::operator*=(T factor)
{
	_data[0] *= factor;
	_data[1] *= factor;
	_data[2] *= factor;

	return *this;
}

// Returns the cross product of 2 vectors
template <class T> Vector3D<T> Vector3D<T>::crossProduct(const Vector3D<T>& v) const
{
	return (*this)^v;
}

// Returns the cross product of 2 vectors
template <class T> Vector3D<T> Vector3D<T>::operator^(const Vector3D<T>& v) const
{
	return Vector3D<T>( (_data[1] * v._data[2]) - (v._data[1] * _data[2]),
											(_data[2] * v._data[0]) - (v._data[2] * _data[0]),
											(_data[0] * v._data[1]) - (v._data[0] * _data[1]) );
}

// Returns the difference vector between 2 vectors
template <class T> Vector3D<T> Vector3D<T>::difference(const Vector3D<T>& v) const
{
	return (*this) - v;
}

// Returns the difference vector between 2 vectors
template <class T> Vector3D<T> Vector3D<T>::operator-(const Vector3D<T>& v) const
{
	return Vector3D<T>( _data[0] - v._data[0],
											_data[1] - v._data[1],
											_data[2] - v._data[2] );
}

// Substracts one vector to another
template <class T> const Vector3D<T>& Vector3D<T>::substract(const Vector3D<T>& v)
{
	return (*this) -= v;
}

// Substracts one vector to another
template <class T> const Vector3D<T>& Vector3D<T>::operator-=(const Vector3D<T>& v)
{
	_data[0] -= v._data[0];
	_data[1] -= v._data[1];
	_data[2] -= v._data[2];

	return *this;
}

// Returns a vector summation
template <class T> Vector3D<T> Vector3D<T>::sum(const Vector3D<T>& v) const
{
	return (*this) + v;
}

// Returns a vector summation
template <class T> Vector3D<T> Vector3D<T>::operator+(const Vector3D<T>& v) const
{
	return Vector3D<T>( _data[0] + v._data[0],
											_data[1] + v._data[1],
											_data[2] + v._data[2] );
}

// Adds a vector to another one
template <class T> const Vector3D<T>& Vector3D<T>::add(const Vector3D<T>& v)
{
	return (*this) += v;
}

// Adds a vector to another one
template <class T> const Vector3D<T>& Vector3D<T>::operator+=(const Vector3D<T>& v)
{
	_data[0] += v._data[0];
	_data[1] += v._data[1];
	_data[2] += v._data[2];

	return *this;
}

// Calculates the spherical coordinates for the vector, except the distance
template <class T> void Vector3D<T>::getSpheric(double& theta, double& phi) const
{
	double distance = sqrt( x()*x() + y()*y() + z()*z() );
	assert(distance != 0.0);

	theta = atan2(y(), x());
	phi = acos(z() / distance);

//	theta = ::normalize(-M_PI, M_PI, theta);
//	phi = ::normalize(0, M_PI, phi);
}

// Calculates the spherical coordinates for the vector
template <class T> void Vector3D<T>::getSpheric(double& distance, double& theta, double& phi) const
{
	distance = sqrt( x()*x() + y()*y() + z()*z() );
	theta = atan2(y(), x());
	phi = acos(z() / distance);

  assert( (theta >= -M_PI) && (theta <= M_PI) );
  assert( (phi >= 0) && (phi <= M_PI) );
}

// Sets the values of the vector given its spheric coordinates
template <class T> void Vector3D<T>::setSpheric(double distance, double theta, double phi)
{
//	theta = ::normalize(-M_PI, M_PI, theta);
//	phi = ::normalize(0, M_PI, phi);
	//assert( (theta >= -M_PI) && (theta <= M_PI) );
  //assert( (phi >= 0) && (phi <= M_PI) );

	double sinPhi = sin(phi);

	x( (T) (distance * cos(theta) * sinPhi) );
	y( (T) (distance * sin(theta) * sinPhi) );
	z( (T) (distance * cos(phi)) );
}



// Instantiation of templates, needed by gcc
template class Vector3D<double>;
template class Vector3D<float>;
//template class Vector3D<int>;
//template class Vector3D<short>;
//template class Vector3D<char>;