///////////////////////////////////////////////////////////////////////////////
// Vectors.h
// =========
// 2D/3D/4D vectors
//
//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
// CREATED: 2007-02-14
// UPDATED: 2013-01-20
//
// Copyright (C) 2007-2013 Song Ho Ahn
///////////////////////////////////////////////////////////////////////////////

#ifndef VECTORS_H_DEF
#define VECTORS_H_DEF

#include <cmath>
#include <iostream>

namespace lineag {

///////////////////////////////////////////////////////////////////////////////
// 2D vector
///////////////////////////////////////////////////////////////////////////////
struct Vector2 {
  float x;
  float y;

  // ctors
  Vector2() : x(0), y(0){};
  Vector2(float x, float y) : x(x), y(y){};

  // utils functions
  void set(float x, float y);
  float length() const;                           //
  float distance(const Vector2& vec) const;       // distance between two vectors
  Vector2& normalize();                           //
  float dot(const Vector2& vec) const;            // dot product
  bool equal(const Vector2& vec, float e) const;  // compare with epsilon

  // operators
  Vector2 operator-() const;                    // unary operator (negate)
  Vector2 operator+(const Vector2& rhs) const;  // add rhs
  Vector2 operator-(const Vector2& rhs) const;  // subtract rhs
  Vector2& operator+=(const Vector2& rhs);      // add rhs and update this object
  Vector2& operator-=(const Vector2& rhs);      // subtract rhs and update this object
  Vector2 operator*(const float scale) const;   // scale
  Vector2 operator*(const Vector2& rhs) const;  // multiply each element
  Vector2& operator*=(const float scale);       // scale and update this object
  Vector2& operator*=(const Vector2& rhs);      // multiply each element and update this object
  Vector2 operator/(const float scale) const;   // inverse scale
  Vector2& operator/=(const float scale);       // scale and update this object
  bool operator==(const Vector2& rhs) const;    // exact compare, no epsilon
  bool operator!=(const Vector2& rhs) const;    // exact compare, no epsilon
  bool operator<(const Vector2& rhs) const;     // comparison for sort
  float operator[](int index) const;            // subscript operator v[0], v[1]
  float& operator[](int index);                 // subscript operator v[0], v[1]

  friend Vector2 operator*(const float a, const Vector2 vec);
  friend std::ostream& operator<<(std::ostream& os, const Vector2& vec);
};

///////////////////////////////////////////////////////////////////////////////
// 3D vector
///////////////////////////////////////////////////////////////////////////////
struct Vector3 {
  float x;
  float y;
  float z;

  // ctors
  Vector3() : x(0), y(0), z(0){};
  Vector3(float x, float y, float z) : x(x), y(y), z(z){};

  // utils functions
  void set(float x, float y, float z);
  float length() const;                           //
  float distance(const Vector3& vec) const;       // distance between two vectors
  Vector3& normalize();                           //
  float dot(const Vector3& vec) const;            // dot product
  Vector3 cross(const Vector3& vec) const;        // cross product
  bool equal(const Vector3& vec, float e) const;  // compare with epsilon

  // operators
  Vector3 operator-() const;                    // unary operator (negate)
  Vector3 operator+(const Vector3& rhs) const;  // add rhs
  Vector3 operator-(const Vector3& rhs) const;  // subtract rhs
  Vector3& operator+=(const Vector3& rhs);      // add rhs and update this object
  Vector3& operator-=(const Vector3& rhs);      // subtract rhs and update this object
  Vector3 operator*(const float scale) const;   // scale
  Vector3 operator*(const Vector3& rhs) const;  // multiplay each element
  Vector3& operator*=(const float scale);       // scale and update this object
  Vector3& operator*=(const Vector3& rhs);      // product each element and update this object
  Vector3 operator/(const float scale) const;   // inverse scale
  Vector3& operator/=(const float scale);       // scale and update this object
  bool operator==(const Vector3& rhs) const;    // exact compare, no epsilon
  bool operator!=(const Vector3& rhs) const;    // exact compare, no epsilon
  bool operator<(const Vector3& rhs) const;     // comparison for sort
  float operator[](int index) const;            // subscript operator v[0], v[1]
  float& operator[](int index);                 // subscript operator v[0], v[1]

  friend Vector3 operator*(const float a, const Vector3 vec);
  friend std::ostream& operator<<(std::ostream& os, const Vector3& vec);
};

///////////////////////////////////////////////////////////////////////////////
// 4D vector
///////////////////////////////////////////////////////////////////////////////
struct Vector4 {
  float x;
  float y;
  float z;
  float w;

  // ctors
  Vector4() : x(0), y(0), z(0), w(0){};
  Vector4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w){};

  // utils functions
  void set(float x, float y, float z, float w);
  float length() const;                           //
  float distance(const Vector4& vec) const;       // distance between two vectors
  Vector4& normalize();                           //
  float dot(const Vector4& vec) const;            // dot product
  bool equal(const Vector4& vec, float e) const;  // compare with epsilon

  // operators
  Vector4 operator-() const;                    // unary operator (negate)
  Vector4 operator+(const Vector4& rhs) const;  // add rhs
  Vector4 operator-(const Vector4& rhs) const;  // subtract rhs
  Vector4& operator+=(const Vector4& rhs);      // add rhs and update this object
  Vector4& operator-=(const Vector4& rhs);      // subtract rhs and update this object
  Vector4 operator*(const float scale) const;   // scale
  Vector4 operator*(const Vector4& rhs) const;  // multiply each element
  Vector4& operator*=(const float scale);       // scale and update this object
  Vector4& operator*=(const Vector4& rhs);      // multiply each element and update this object
  Vector4 operator/(const float scale) const;   // inverse scale
  Vector4& operator/=(const float scale);       // scale and update this object
  bool operator==(const Vector4& rhs) const;    // exact compare, no epsilon
  bool operator!=(const Vector4& rhs) const;    // exact compare, no epsilon
  bool operator<(const Vector4& rhs) const;     // comparison for sort
  float operator[](int index) const;            // subscript operator v[0], v[1]
  float& operator[](int index);                 // subscript operator v[0], v[1]

  friend Vector4 operator*(const float a, const Vector4 vec);
  friend std::ostream& operator<<(std::ostream& os, const Vector4& vec);
};

// fast math routines from Doom3 SDK
inline float invSqrt(float x) {
  float xhalf = 0.5f * x;
  int i = *(int*)&x;               // get bits for floating value
  i = 0x5f3759df - (i >> 1);       // gives initial guess
  x = *(float*)&i;                 // convert bits back to float
  x = x * (1.5f - xhalf * x * x);  // Newton step
  return x;
}

///////////////////////////////////////////////////////////////////////////////
// inline functions for Vector2
///////////////////////////////////////////////////////////////////////////////
inline Vector2 Vector2::operator-() const { return Vector2(-x, -y); }

inline Vector2 Vector2::operator+(const Vector2& rhs) const { return Vector2(x + rhs.x, y + rhs.y); }

inline Vector2 Vector2::operator-(const Vector2& rhs) const { return Vector2(x - rhs.x, y - rhs.y); }

inline Vector2& Vector2::operator+=(const Vector2& rhs) {
  x += rhs.x;
  y += rhs.y;
  return *this;
}

inline Vector2& Vector2::operator-=(const Vector2& rhs) {
  x -= rhs.x;
  y -= rhs.y;
  return *this;
}

inline Vector2 Vector2::operator*(const float a) const { return Vector2(x * a, y * a); }

inline Vector2 Vector2::operator*(const Vector2& rhs) const { return Vector2(x * rhs.x, y * rhs.y); }

inline Vector2& Vector2::operator*=(const float a) {
  x *= a;
  y *= a;
  return *this;
}

inline Vector2& Vector2::operator*=(const Vector2& rhs) {
  x *= rhs.x;
  y *= rhs.y;
  return *this;
}

inline Vector2 Vector2::operator/(const float a) const { return Vector2(x / a, y / a); }

inline Vector2& Vector2::operator/=(const float a) {
  x /= a;
  y /= a;
  return *this;
}

inline bool Vector2::operator==(const Vector2& rhs) const { return (x == rhs.x) && (y == rhs.y); }

inline bool Vector2::operator!=(const Vector2& rhs) const { return (x != rhs.x) || (y != rhs.y); }

inline bool Vector2::operator<(const Vector2& rhs) const {
  if (x < rhs.x) return true;
  if (x > rhs.x) return false;
  if (y < rhs.y) return true;
  if (y > rhs.y) return false;
  return false;
}

inline float Vector2::operator[](int index) const { return (&x)[index]; }

inline float& Vector2::operator[](int index) { return (&x)[index]; }

inline void Vector2::set(float x, float y) {
  this->x = x;
  this->y = y;
}

inline float Vector2::length() const { return sqrtf(x * x + y * y); }

inline float Vector2::distance(const Vector2& vec) const {
  return sqrtf((vec.x - x) * (vec.x - x) + (vec.y - y) * (vec.y - y));
}

inline Vector2& Vector2::normalize() {
  //@@const float EPSILON = 0.000001f;
  float xxyy = x * x + y * y;
  //@@if(xxyy < EPSILON)
  //@@    return *this;

  // float invLength = invSqrt(xxyy);
  float invLength = 1.0f / sqrtf(xxyy);
  x *= invLength;
  y *= invLength;
  return *this;
}

inline float Vector2::dot(const Vector2& rhs) const { return (x * rhs.x + y * rhs.y); }

inline bool Vector2::equal(const Vector2& rhs, float epsilon) const {
  return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon;
}

inline Vector2 operator*(const float a, const Vector2& vec) { return Vector2(a * vec.x, a * vec.y); }

inline std::ostream& operator<<(std::ostream& os, const Vector2& vec) {
  os << "(" << vec.x << ", " << vec.y << ")";
  return os;
}
// END OF VECTOR2 /////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// inline functions for Vector3
///////////////////////////////////////////////////////////////////////////////
inline Vector3 Vector3::operator-() const { return Vector3(-x, -y, -z); }

inline Vector3 Vector3::operator+(const Vector3& rhs) const { return Vector3(x + rhs.x, y + rhs.y, z + rhs.z); }

inline Vector3 Vector3::operator-(const Vector3& rhs) const { return Vector3(x - rhs.x, y - rhs.y, z - rhs.z); }

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

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

inline Vector3 Vector3::operator*(const float a) const { return Vector3(x * a, y * a, z * a); }

inline Vector3 Vector3::operator*(const Vector3& rhs) const { return Vector3(x * rhs.x, y * rhs.y, z * rhs.z); }

inline Vector3& Vector3::operator*=(const float a) {
  x *= a;
  y *= a;
  z *= a;
  return *this;
}

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

inline Vector3 Vector3::operator/(const float a) const { return Vector3(x / a, y / a, z / a); }

inline Vector3& Vector3::operator/=(const float a) {
  x /= a;
  y /= a;
  z /= a;
  return *this;
}

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

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

inline bool Vector3::operator<(const Vector3& rhs) const {
  if (x < rhs.x) return true;
  if (x > rhs.x) return false;
  if (y < rhs.y) return true;
  if (y > rhs.y) return false;
  if (z < rhs.z) return true;
  if (z > rhs.z) return false;
  return false;
}

inline float Vector3::operator[](int index) const { return (&x)[index]; }

inline float& Vector3::operator[](int index) { return (&x)[index]; }

inline void Vector3::set(float x, float y, float z) {
  this->x = x;
  this->y = y;
  this->z = z;
}

inline float Vector3::length() const { return sqrtf(x * x + y * y + z * z); }

inline float Vector3::distance(const Vector3& vec) const {
  return sqrtf((vec.x - x) * (vec.x - x) + (vec.y - y) * (vec.y - y) + (vec.z - z) * (vec.z - z));
}

inline Vector3& Vector3::normalize() {
  //@@const float EPSILON = 0.000001f;
  float xxyyzz = x * x + y * y + z * z;
  //@@if(xxyyzz < EPSILON)
  //@@    return *this; // do nothing if it is ~zero vector

  // float invLength = invSqrt(xxyyzz);
  float invLength = 1.0f / sqrtf(xxyyzz);
  x *= invLength;
  y *= invLength;
  z *= invLength;
  return *this;
}

inline float Vector3::dot(const Vector3& rhs) const { return (x * rhs.x + y * rhs.y + z * rhs.z); }

inline Vector3 Vector3::cross(const Vector3& rhs) const {
  return Vector3(y * rhs.z - z * rhs.y, z * rhs.x - x * rhs.z, x * rhs.y - y * rhs.x);
}

inline bool Vector3::equal(const Vector3& rhs, float epsilon) const {
  return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon && fabs(z - rhs.z) < epsilon;
}

inline Vector3 operator*(const float a, const Vector3& vec) { return Vector3(a * vec.x, a * vec.y, a * vec.z); }

inline std::ostream& operator<<(std::ostream& os, const Vector3& vec) {
  os << "(" << vec.x << ", " << vec.y << ", " << vec.z << ")";
  return os;
}
// END OF VECTOR3 /////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// inline functions for Vector4
///////////////////////////////////////////////////////////////////////////////
inline Vector4 Vector4::operator-() const { return Vector4(-x, -y, -z, -w); }

inline Vector4 Vector4::operator+(const Vector4& rhs) const {
  return Vector4(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
}

inline Vector4 Vector4::operator-(const Vector4& rhs) const {
  return Vector4(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
}

inline Vector4& Vector4::operator+=(const Vector4& rhs) {
  x += rhs.x;
  y += rhs.y;
  z += rhs.z;
  w += rhs.w;
  return *this;
}

inline Vector4& Vector4::operator-=(const Vector4& rhs) {
  x -= rhs.x;
  y -= rhs.y;
  z -= rhs.z;
  w -= rhs.w;
  return *this;
}

inline Vector4 Vector4::operator*(const float a) const { return Vector4(x * a, y * a, z * a, w * a); }

inline Vector4 Vector4::operator*(const Vector4& rhs) const {
  return Vector4(x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w);
}

inline Vector4& Vector4::operator*=(const float a) {
  x *= a;
  y *= a;
  z *= a;
  w *= a;
  return *this;
}

inline Vector4& Vector4::operator*=(const Vector4& rhs) {
  x *= rhs.x;
  y *= rhs.y;
  z *= rhs.z;
  w *= rhs.w;
  return *this;
}

inline Vector4 Vector4::operator/(const float a) const { return Vector4(x / a, y / a, z / a, w / a); }

inline Vector4& Vector4::operator/=(const float a) {
  x /= a;
  y /= a;
  z /= a;
  w /= a;
  return *this;
}

inline bool Vector4::operator==(const Vector4& rhs) const {
  return (x == rhs.x) && (y == rhs.y) && (z == rhs.z) && (w == rhs.w);
}

inline bool Vector4::operator!=(const Vector4& rhs) const {
  return (x != rhs.x) || (y != rhs.y) || (z != rhs.z) || (w != rhs.w);
}

inline bool Vector4::operator<(const Vector4& rhs) const {
  if (x < rhs.x) return true;
  if (x > rhs.x) return false;
  if (y < rhs.y) return true;
  if (y > rhs.y) return false;
  if (z < rhs.z) return true;
  if (z > rhs.z) return false;
  if (w < rhs.w) return true;
  if (w > rhs.w) return false;
  return false;
}

inline float Vector4::operator[](int index) const { return (&x)[index]; }

inline float& Vector4::operator[](int index) { return (&x)[index]; }

inline void Vector4::set(float x, float y, float z, float w) {
  this->x = x;
  this->y = y;
  this->z = z;
  this->w = w;
}

inline float Vector4::length() const { return sqrtf(x * x + y * y + z * z + w * w); }

inline float Vector4::distance(const Vector4& vec) const {
  return sqrtf((vec.x - x) * (vec.x - x) + (vec.y - y) * (vec.y - y) + (vec.z - z) * (vec.z - z) +
               (vec.w - w) * (vec.w - w));
}

inline Vector4& Vector4::normalize() {
  // NOTE: leave w-component untouched
  //@@const float EPSILON = 0.000001f;
  float xxyyzz = x * x + y * y + z * z;
  //@@if(xxyyzz < EPSILON)
  //@@    return *this; // do nothing if it is zero vector

  // float invLength = invSqrt(xxyyzz);
  float invLength = 1.0f / sqrtf(xxyyzz);
  x *= invLength;
  y *= invLength;
  z *= invLength;
  return *this;
}

inline float Vector4::dot(const Vector4& rhs) const { return (x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w); }

inline bool Vector4::equal(const Vector4& rhs, float epsilon) const {
  return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon && fabs(z - rhs.z) < epsilon &&
         fabs(w - rhs.w) < epsilon;
}

inline Vector4 operator*(const float a, const Vector4& vec) {
  return Vector4(a * vec.x, a * vec.y, a * vec.z, a * vec.w);
}

inline std::ostream& operator<<(std::ostream& os, const Vector4& vec) {
  os << "(" << vec.x << ", " << vec.y << ", " << vec.z << ", " << vec.w << ")";
  return os;
}
// END OF VECTOR4 /////////////////////////////////////////////////////////////

}  // namespace lineag

#endif
