#include "Vector3.hpp"

#include "String.hpp"

#include <stdlib.h>

#include <cmath>


#include "Basis.hpp"

namespace godot {


Vector3::Vector3(real_t x, real_t y, real_t z)
{
	this->x = x;
	this->y = y;
	this->z = z;
}

Vector3::Vector3()
{
	this->x = 0;
	this->y = 0;
	this->z = 0;
}

Vector3::Vector3(const Vector3& b)
{
	this->x = b.x;
	this->y = b.y;
	this->z = b.z;
}

const real_t& Vector3::operator[](int p_axis) const
{
	return coord[p_axis];
}

real_t& Vector3::operator[](int p_axis)
{
	return coord[p_axis];
}

Vector3& Vector3::operator+=(const Vector3& p_v)
{
	x += p_v.x;
	y += p_v.y;
	z += p_v.z;
	return *this;
}

Vector3 Vector3::operator+(const Vector3& p_v) const
{
	Vector3 v = *this;
	v += p_v;
	return v;
}

Vector3& Vector3::operator-=(const Vector3& p_v)
{
	x -= p_v.x;
	y -= p_v.y;
	z -= p_v.z;
	return *this;
}

Vector3 Vector3::operator-(const Vector3& p_v) const
{
	Vector3 v = *this;
	v -= p_v;
	return v;
}

Vector3& Vector3::operator*=(const Vector3& p_v)
{
	x *= p_v.x;
	y *= p_v.y;
	z *= p_v.z;
	return *this;
}

Vector3 Vector3::operator*(const Vector3& p_v) const
{
	Vector3 v = *this;
	v *= p_v;
	return v;
}

Vector3& Vector3::operator/=(const Vector3& p_v)
{
	x /= p_v.x;
	y /= p_v.y;
	z /= p_v.z;
	return *this;
}

Vector3 Vector3::operator/(const Vector3& p_v) const
{
	Vector3 v = *this;
	v /= p_v;
	return v;
}


Vector3& Vector3::operator*=(real_t p_scalar)
{
	*this *= Vector3(p_scalar, p_scalar, p_scalar);
	return *this;
}

Vector3 Vector3::operator*(real_t p_scalar) const
{
	Vector3 v = *this;
	v *= p_scalar;
	return v;
}

Vector3& Vector3::operator/=(real_t p_scalar)
{
	*this /= Vector3(p_scalar, p_scalar, p_scalar);
	return *this;
}

Vector3 Vector3::operator/(real_t p_scalar) const
{
	Vector3 v = *this;
	v /= p_scalar;
	return v;
}

Vector3 Vector3::operator-() const
{
	return Vector3(-x, -y, -z);
}

bool Vector3::operator==(const Vector3& p_v) const
{
	return (x==p_v.x && y==p_v.y && z==p_v.z);
}

bool Vector3::operator!=(const Vector3& p_v) const
{
	return (x!=p_v.x || y!=p_v.y || z!=p_v.z);
}

bool Vector3::operator<(const Vector3& p_v) const
{
	if (x==p_v.x) {
		if (y==p_v.y)
			return z<p_v.z;
		else
			return y<p_v.y;
	} else {
		return x<p_v.x;
	}
}

bool Vector3::operator<=(const Vector3& p_v) const
{
	if (x==p_v.x) {
		if (y==p_v.y)
			return z<=p_v.z;
		else
			return y<p_v.y;
	} else {
		return x<p_v.x;
	}
}

Vector3 Vector3::abs() const
{
	return Vector3(::fabs(x), ::fabs(y), ::fabs(z));
}

Vector3 Vector3::ceil() const
{
	return Vector3(::ceil(x), ::ceil(y), ::ceil(z));
}

Vector3 Vector3::cross(const Vector3& b) const
{
	Vector3 ret (
		(y * b.z) - (z * b.y),
		(z * b.x) - (x * b.z),
		(x * b.y) - (y * b.x)
	);

	return ret;
}

Vector3 Vector3::linear_interpolate(const Vector3& p_b,real_t p_t) const
{
	return Vector3(
		x+(p_t * (p_b.x-x)),
		y+(p_t * (p_b.y-y)),
		z+(p_t * (p_b.z-z))
	);
}

Vector3 Vector3::cubic_interpolate(const Vector3& b, const Vector3& pre_a, const Vector3& post_b, const real_t t) const
{
	Vector3 p0=pre_a;
	Vector3 p1=*this;
	Vector3 p2=b;
	Vector3 p3=post_b;

	real_t t2 = t * t;
	real_t t3 = t2 * t;

	Vector3 out;
	out = ( ( p1 * 2.0) +
	( -p0 + p2 ) * t +
	( p0 * 2.0 - p1 * 5.0 + p2 * 4 - p3 ) * t2 +
	( -p0 + p1 * 3.0 - p2 * 3.0 + p3 ) * t3 ) * 0.5;
	return out;
}

real_t Vector3::length() const
{
	real_t x2=x*x;
	real_t y2=y*y;
	real_t z2=z*z;

	return ::sqrt(x2+y2+z2);
}

real_t Vector3::length_squared() const
{
	real_t x2=x*x;
	real_t y2=y*y;
	real_t z2=z*z;

	return x2+y2+z2;
}

real_t Vector3::distance_squared_to(const Vector3& b) const
{
	return (b-*this).length();
}

real_t Vector3::distance_to(const Vector3& b) const
{
	return (b-*this).length_squared();
}

real_t Vector3::dot(const Vector3& b) const
{
	return x*b.x + y*b.y + z*b.z;
}

Vector3 Vector3::floor() const
{
	return Vector3(::floor(x), ::floor(y), ::floor(z));
}

Vector3 Vector3::inverse() const
{
	return Vector3( 1.0/x, 1.0/y, 1.0/z );
}




int Vector3::max_axis() const
{
	return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0);
}

int Vector3::min_axis() const
{
	return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2);
}

void Vector3::normalize()
{
	real_t l=length();
	if (l==0) {
		x=y=z=0;
	} else {
		x/=l;
		y/=l;
		z/=l;
	}
}

Vector3 Vector3::normalized() const
{
	Vector3 v = *this;
	v.normalize();
	return v;
}

Vector3 Vector3::reflect(const Vector3& by) const
{
	return by - *this * this->dot(by) * 2.0;
}

Vector3 Vector3::rotated(const Vector3& axis, const real_t phi) const
{
	Vector3 v = *this;
	v.rotate(axis, phi);
	return v;
}

void Vector3::rotate(const Vector3& p_axis,real_t p_phi)
{
	*this=Basis(p_axis,p_phi).xform(*this);
}

Vector3 Vector3::slide(const Vector3& by) const
{
	return by - *this * this->dot(by);
}

// this is ugly as well, but hey, I'm a simple man
#define _ugly_stepify(val, step) (step != 0 ? ::floor(val / step + 0.5) * step : val)

void Vector3::snap(real_t p_val)
{
	x = _ugly_stepify(x,p_val);
	y = _ugly_stepify(y,p_val);
	z = _ugly_stepify(z,p_val);
}

#undef _ugly_stepify

Vector3 Vector3::snapped(const float by)
{
	Vector3 v = *this;
	v.snap(by);
	return v;
}

Vector3::operator String() const
{
	return String(); // @Todo
}


}