#include "Vector.hpp"
#include "Matrix.hpp"
#include <iomanip>

const Vector Vector::X(1.0);

const Vector Vector::Y(0.0, 1.0);

const Vector Vector::Z(0.0, 0.0, 1.0);

const Vector Vector::Zero;

Vector::Vector(
  const double x,
  const double y,
  const double z
) {
  set(
    x,
    y,
    z
  );
}

Vector& Vector::set(
  const double x,
  const double y,
  const double z
) {
  this->x = x;
  this->y = y;
  this->z = z;
  return *this;
}

Vector& Vector::neg() {
  return set(
    -x,
    -y,
    -z
  );
}

Vector& Vector::zero() {
  return set();
}

Vector& Vector::round(const double precision) {
  if (precision > 1) {
    *this *= precision;
  }
  x = ::round(x);
  y = ::round(y);
  z = ::round(z);
  if (precision > 1) {
    *this /= precision;
  }
  return *this;
}

Vector& Vector::randomize(const Vector& factor) {
  #define RND(axis) \
    axis += std::abs(factor.axis) * Math::rand() - std::abs(factor.axis) / 2;
  RND(x);
  RND(x);
  RND(x);
  #undef D
  return *this;
}

const double Vector::getScalar(const Vector& v) const {
  return x * v.x + y * v.y + z * v.z;
}

const double Vector::getSquaredLength() const {
  return getScalar(*this);
}

const double Vector::getLength() const {
  return sqrt(
    getSquaredLength()
  );
}

Vector& Vector::normalize() {
  if (x != 0 || y != 0 || z != 0) {
    *this /= getLength();
  }
  return *this;
}

const double Vector::getCos(const Vector& v) const {
  const double d = sqrt(
    getSquaredLength() * v.getSquaredLength()
  );
  return d ? getScalar(v) / d : 1;
}

const double Vector::getSin(const Vector& v) const {
  static Vector vp;
  vp = *this;
  vp *= v;
  return sqrt(
    vp.getSquaredLength() / (
      getSquaredLength() * v.getSquaredLength()
    )
  );
}

const double Vector::getDistance(const Vector& v) const {
  static Vector ab;
  ab = *this;
  ab -= v;
  return ab.getLength();
}

const Vector& Vector::getCenter(const Vector& v) const {
  static Vector ret;
  ret = *this;
  ret += v;
  ret /= 2;
  return ret;
}

Vector& Vector::rotateX(const double rad) {
  static Matrix m;
  return *this *= m.setXRotation(rad);
}

Vector& Vector::rotateY(const double rad) {
  static Matrix m;
  return *this *= m.setYRotation(rad);
}

Vector& Vector::rotateZ(const double rad) {
  static Matrix m;
  return *this *= m.setZRotation(rad);
}

Vector& Vector::scale(const Vector& v) {
  x *= v.x;
  y *= v.y;
  z *= v.z;
  return *this;
}

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

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

Vector& Vector::operator *= (const double factor) {
  scale(
    Vector(
      factor,
      factor,
      factor
    )
  );
  return *this;
}

Vector& Vector::operator /= (const double factor) {
  x /= factor;
  y /= factor;
  z /= factor;
  return *this;
}

Vector& Vector::operator *= (const Matrix& m) {
  static Vector ret;
  return *this = ret.set(
    x * m._4x4._11 + y * m._4x4._21 + z * m._4x4._31 + m._4x4._14,
    x * m._4x4._12 + y * m._4x4._22 + z * m._4x4._32 + m._4x4._24,
    x * m._4x4._13 + y * m._4x4._23 + z * m._4x4._33 + m._4x4._34
  );
}

Vector& Vector::operator *= (const Vector& v) {
  static Vector ret;
  return *this = ret.set(
    y * v.z - z * v.y,
    z * v.x - x * v.z,
    x * v.y - y * v.x
  );
}

std::ostream& Vector::operator >> (std::ostream& os) const {
  return os << std::setw(8) << x << " " << std::setw(8) << y << " " << std::setw(8) << z;
}

std::ostream& operator << (
  std::ostream& os,
  const Vector& v
) {
  return v >> os;
}

const bool operator == (const Vector& a, const Vector& b) {
  return a.x == b.x && a.y == b.y && a.z == b.z;
}

const bool operator != (const Vector& a, const Vector& b) {
  return !(a == b);
}
