#ifndef vector_h
#define vector_h

#include <math.h>

#define inline __forceinline

#define FORWARD 0
#define RIGHT 1
#define UP 2

#define PITCH 0
#define YAW 1
#define ROLL 2

typedef float floatingtype;

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

class Vector {
public:
	floatingtype x, y, z;
	
	inline Vector(floatingtype ax, floatingtype ay, floatingtype az)
	{
		x = ax; y = ay; z = az;
	}
	
	inline Vector() {}

	inline void set(floatingtype ax, floatingtype ay, floatingtype az)
	{
		x = ax; y = ay; z = az;
	}

	
	inline floatingtype length()
	{
		return (floatingtype)sqrt(x*x+y*y+z*z);
	}

	inline floatingtype toYaw()
	{
		floatingtype yaw = (floatingtype)(atan2 (y,x) * 180 / M_PI);

		if (yaw < 0) yaw+=360;

		return (floatingtype) yaw;
	}

	inline floatingtype toPitch()
	{
		floatingtype pitch, forward;

		forward = (floatingtype)sqrt(x*x + y*y);
		pitch = (floatingtype)(atan2(z, forward) * 180 / M_PI);

		if (pitch < 0) pitch += 360;

		return -pitch;
	}

	inline Vector toAngles()
	{
		floatingtype forward, yaw, pitch;
		
		yaw = (floatingtype)(atan2(y,x) * 180 / M_PI);

		if (yaw < 0) yaw += 360;

		forward = (floatingtype)sqrt(x*x + y*y);
		pitch = (floatingtype)(atan2(z, forward) * 180 / M_PI);
		
		if (pitch < 0) pitch += 360;

		return Vector(-pitch, yaw, 0);
	}

	inline Vector toForwardVector()
	{
		floatingtype sp, sy, cp, cy, angle;

		angle = y * (floatingtype)(M_PI / 180);
		sy = (floatingtype) sin(angle);
		cy = (floatingtype) cos(angle);
		
		angle = x * (floatingtype)(M_PI / 180);
		cp = (floatingtype) cos(angle);
		sp = (floatingtype) sin(angle);

		return Vector(cp * cy, cp * sy, -sp);
	}
	
	inline Vector toUpVector()
	{
		floatingtype sp, sy, cp, cy, sr, cr, angle;

		angle = y * (floatingtype)(M_PI / 180);
		sy = (floatingtype) sin(angle);
		cy = (floatingtype) cos(angle);
		
		angle = x * (floatingtype)(M_PI / 180);
		cp = (floatingtype) cos(angle);
		sp = (floatingtype) sin(angle);

		angle = z * (floatingtype)(M_PI / 180);
		sr = (floatingtype) sin(angle);
		cr = (floatingtype) cos(angle);


		return Vector(cr*sp*cy + -sr*-sy, cr*sp*sy + -sr*cy, cr*cp);
	}

	inline Vector toRightVector()
	{
		floatingtype sp, sy, cp, cy, sr, cr, angle;

		angle = y * (floatingtype)(M_PI / 180);
		sy = (floatingtype) sin(angle);
		cy = (floatingtype) cos(angle);
		
		angle = x * (floatingtype)(M_PI / 180);
		cp = (floatingtype) cos(angle);
		sp = (floatingtype) sin(angle);

		angle = z * (floatingtype)(M_PI / 180);
		sr = (floatingtype) sin(angle);
		cr = (floatingtype) cos(angle);


		return Vector(-1*sr*sp*cy + -1*cr*-sy, -1*sr*sp*sy + -1*cr*cy, -1*sr*cp);
	}

	inline floatingtype length2()
	{
		return (x*x+y*y+z*z);
	}
	
	inline floatingtype normalize()
	{
		floatingtype invlength = 1.0f / (floatingtype)sqrt(x*x+y*y+z*z);
		x *= invlength;
		y *= invlength;
		z *= invlength;
		
		return invlength;
	}
	
	inline Vector normalized()
	{
		floatingtype invlength = 1.0f / (floatingtype)sqrt(x*x+y*y+z*z);
		Vector normalvec;
		normalvec.x = x * invlength;
		normalvec.y = y * invlength;
		normalvec.z = z * invlength;
		
		return normalvec;
	}

	inline void setLength(floatingtype b) {
		floatingtype f = (1.0f * b) / (floatingtype)sqrt(x*x+y*y+z*z);
		x *= f;
		y *= f;
		z *= f;
	}


	inline void setNull() {
		x = (floatingtype) 0.0;
		y = (floatingtype) 0.0;
		z = (floatingtype) 0.0;
	}
	
	inline void square()
	{
		x*=x;
		y*=y;
		z*=z;
	}

	inline Vector squared()
	{
		return Vector(x*x, y*y, z*z);
	}

	inline floatingtype& operator[](const int index)
	{
		if (index == 0) return x;
		if (index == 1) return y;
		return z;		
	}

	inline floatingtype operator*(const Vector &a) const
	{
		return x*a.x + y*a.y + z*a.z;
	}
	
	inline Vector operator*(const floatingtype s) const
	{
		return Vector(x*s, y*s, z*s);
	}
	
	inline Vector operator/(const floatingtype s) const
	{
		floatingtype invs = 1.0f / s;
		return Vector(x*invs, y*invs, z*invs);
	}
	
	inline Vector operator+(const Vector& a) const
	{
		return Vector(x+a.x, y+a.y, z+a.z);
	}
	
	inline Vector operator-(const Vector& a) const
	{
		return Vector(x-a.x, y-a.y, z-a.z);
	}
	
	inline Vector operator-()
	{
		x = -x;
		y = -y;
		z = -z;
		return *this;
	}
	
	inline Vector& operator+=(const Vector& a)
	{
		x += a.x;
		y += a.y;
		z += a.z;
		return *this;
	}
	
	inline Vector& operator-=(const Vector& a)
	{
		x -= a.x;
		y -= a.y;
		z -= a.z;
		return *this;
	}
	
	inline Vector& operator*=(const floatingtype a)
	{
		x *= a;
		y *= a;
		z *= a;
		return *this;
	}
	
	inline Vector& operator/=(const floatingtype a)
	{
		floatingtype b = 1.0f / a;
		x *= b;
		y *= b;
		z *= b;
		return *this;
	}
	
	inline Vector& operator=(const Vector& a)
	{
		x = a.x;
		y = a.y;
		z = a.z;
		return *this;
	}
	
	inline Vector operator^(const Vector& b)
	{
		return Vector(
			y*b.z-z*b.y,
			z*b.x-x*b.z,
			x*b.y-y*b.x);
	}

	inline bool operator==(const Vector& a)
	{
		if (x == a.x)
			if (y == a.y)
				if (z == a.z) return true;
		return false;
	}

	inline bool operator!=(const Vector& a)
	{
		if (x == a.x)
			if (y == a.y)
				if (z == a.z) return false;
		return true;
	}

	inline floatingtype distanceTo(const Vector &v)
	{
		return (v - *this).length();
	}
};

#endif