/*
  Todo:
  > Make everything inline
*/
#ifndef __VEC3_H
  #define __VEC3_H

#include <math.h>

typedef float Scalar;
typedef Scalar Vec2[2];

class Vec3
{
public:
  // Constructors
  Vec3(Scalar px=0, Scalar py=0, Scalar pz=0) {v[0] = px; v[1] = py; v[2] = pz;}
  Vec3(const Vec3 &pVec) {v[0] = pVec.v[0]; v[1] = pVec.v[1]; v[2] = pVec.v[2];}
  Vec3(const Scalar *pVec) {v[0] = pVec[0]; v[1] = pVec[1]; v[2] = pVec[2];}

  // Indexing into the array, no bound checks
  const Scalar &operator[](int ndx) const {return v[ndx];}
  operator Scalar*(void) {return v;}
  Scalar &operator[](int ndx) {return v[ndx];}

  const Scalar x(Scalar val) {v[0] = val; return v[0];}
  const Scalar y(Scalar val) {v[1] = val; return v[1];}
  const Scalar z(Scalar val) {v[2] = val; return v[2];}

  const Scalar x() const {return v[0];}
  const Scalar y() const {return v[1];}
  const Scalar z() const {return v[2];}

  // Assign
  Vec3 operator=(const Vec3 &pVec) {return Vec3(v[0] = pVec.v[0], v[1] = pVec.v[1], v[2] = pVec.v[2]);}
  Vec3 operator=(const Scalar *ptr) {return Vec3(v[0] = ptr[0], v[1] = ptr[1], v[2] = ptr[2]);}

  //  Equality
  const bool operator==(const Vec3 &pVec) const {return (v[0] == pVec.v[0] && v[1] == pVec.v[1] && v[2] == pVec.v[2]);}
  const bool operator==(const Scalar *ptr) const {return (v[0] == ptr[0] && v[1] == ptr[1] && v[2] == ptr[2]);}

  inline const bool operator!=(const Vec3 &pVec) const {return !((*this) == pVec);}
  inline const bool operator!=(const Scalar *ptr) const {return !((*this) == ptr);}

  // Greater / Less
  bool operator<(const Vec3 vec) {return ((v[0] < vec[0]) && (v[1] < vec[1]) && (v[2] < vec[2]));}
  bool operator<=(const Vec3 vec) {return ((v[0] <= vec[0]) && (v[1] <= vec[1]) && (v[2] <= vec[2]));}
  bool operator>(const Vec3 vec) {return ((v[0] > vec[0]) && (v[1] > vec[1]) && (v[2] > vec[2]));}
  bool operator>=(const Vec3 vec) {return ((v[0] >= vec[0]) && (v[1] >= vec[1]) && (v[2] >= vec[2]));}

  // + - / * operations
  const Vec3 operator+(const Vec3 &pVec) const {return Vec3(v[0] + pVec.v[0], v[1] + pVec.v[1], v[2] + pVec.v[2]);}
  const Vec3 operator+() const {return Vec3(*this);}

  const Vec3 operator-(const Vec3 &pVec) const {return Vec3(v[0] - pVec.v[0], v[1] - pVec.v[1], v[2] - pVec.v[2]);}
  const Vec3 operator-() const {return Vec3(-v[0], -v[1], -v[2]);}

  const Vec3 operator*(const Vec3 &pVec) const {return Vec3(v[0] * pVec.v[0], v[1] * pVec.v[1], v[2] * pVec.v[2]);}
  const Vec3 operator*(Scalar val) const {return Vec3(v[0] * val, v[1] * val, v[2] * val);}
  friend inline const Vec3 operator*(const Scalar &s, const Vec3 &vec) {return vec*s;}

  const Vec3 operator/(const Vec3 &pVec) const {return Vec3(v[0] / pVec.v[0], v[1] / pVec.v[1], v[2] / pVec.v[2]);}
  const Vec3 operator/(Scalar val) const {val = 1/val; return Vec3(v[0] * val, v[1] * val, v[2] * val);}

  const Vec3 &operator+=(const Vec3 &pVec) {*this = *this + pVec; return *this;}
  const Vec3 &operator-=(const Vec3 &pVec) {*this = *this - pVec; return *this;}
  const Vec3 &operator*=(const Vec3 &pVec) {*this = *this * pVec; return *this;}
  const Vec3 &operator*=(Scalar val)        {*this = *this * val; return *this;}
  const Vec3 &operator/=(const Vec3 &pVec) {*this = *this / pVec; return *this;}
  const Vec3 &operator/=(Scalar val)        {*this = *this / val; return *this;}

  // Length
  const Scalar Length() const {return (Scalar)sqrt((double)((v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2])));}
  const Scalar operator!() const {return (Scalar)sqrt((double)((v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2])));}

  // Normalize this vector
  void Normalize() {(*this) /= Length();}
  // Return the unit vector
  const Vec3 UnitVector() const {return (*this) / Length();}

  const Scalar Dot(const Vec3 &pVec) const {return v[0] * pVec.v[0] + v[1] * pVec.v[1] + v[2] * pVec.v[2];}
  const Scalar operator%(const Vec3 &pVec) const {return v[0] * pVec.v[0] + v[1] * pVec.v[1] + v[2] * pVec.v[2];}

  // Cross product
  const Vec3 CrossProduct(const Vec3 &vec) const {return Vec3(v[1]*vec.v[2] - v[2]*vec.v[1], v[2]*vec.v[0] - v[0]*vec.v[2], v[0]*vec.v[1] - v[1]*vec.v[0]);}
  const Vec3 operator^(const Vec3 &vec) const {return Vec3(v[1]*vec.v[2] - v[2]*vec.v[1], v[2]*vec.v[0] - v[0]*vec.v[2], v[0]*vec.v[1] - v[1]*vec.v[0]);}

  // Return vector with specified length
  const Vec3 operator | (const Scalar length) const { return *this * (length / !(*this)); }

  // Set length of vector equal to length
  const Vec3 operator |= (const Scalar length) {return *this = *this | length;}

  // Return angle between two vectors
  const Scalar inline Angle(const Vec3& normal) const {return acosf(*this % normal);}

  // Reflect this vector off surface with normal vector
  const Vec3 inline Reflection(const Vec3& normal) const
  {
    const Vec3 vec(*this | 1);     // normalize this vector
    return (vec - normal * 2.0 * (vec % normal)) * !(*this);    // Why the * 2.0 ?
  }

  // Misc.
  void Clear() {v[0] = v[1] = v[2] = 0;}
  void Set(Scalar x, Scalar y, Scalar z) {v[0] = x; v[1] = y; v[2] = z;}
  void Set(Vec3 &pVec) {v[0] = pVec[0]; v[1] = pVec[1]; v[2] = pVec[2];}
  void Fabs() {v[0] = (Scalar) fabs(v[0]); v[1] = (Scalar) fabs(v[1]); v[2] = (Scalar) fabs(v[2]);}

  void RotateX(float amnt);
  void RotateY(float amnt);
  void RotateZ(float amnt);

protected:
  Scalar v[3];
};


inline void Vec3::RotateX(float amnt)
{
   float s = ARIsin(amnt);
   float c = ARIcos(amnt);
   float y = v[1];
   float z = v[2];

   v[1] = (y * c) - (z * s);
   v[2] = (y * s) + (z * c);
}

inline void Vec3::RotateY(float amnt)
{
   float s = ARIsin(amnt);
   float c = ARIcos(amnt);
   float x = v[0];
   float z = v[2];

   v[0] = (x * c) + (z * s);
   v[2] = (z * c) - (x * s);
}

inline void Vec3::RotateZ(float amnt)
{
   float s = ARIsin(amnt);
   float c = ARIcos(amnt);
   float x = v[0];
   float y = v[1];

   v[0] = (x * c) - (y * s);
   v[1] = (y * c) + (x * s);
}

#endif