// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.

#ifndef tools_vec3
#define tools_vec3

#include <cstddef> //size_t

namespace tools {

template <class T>
class vec3 {
public:
  typedef T elem_t;
  unsigned int dimension() const {return 3;}
public:
  vec3(){
    m_data[0] = T();
    m_data[1] = T();
    m_data[2] = T();
  }
  vec3(const T a_vec[3]) {
    m_data[0] = a_vec[0];
    m_data[1] = a_vec[1];
    m_data[2] = a_vec[2];
  }
  vec3(const T& a0,const T& a1,const T& a2) {
    m_data[0] = a0;
    m_data[1] = a1;
    m_data[2] = a2;
  }
  virtual ~vec3() {
  }
public:
  vec3(const vec3& a_from){
    m_data[0] = a_from.m_data[0];
    m_data[1] = a_from.m_data[1];
    m_data[2] = a_from.m_data[2];
  }
  vec3& operator=(const vec3& a_from) {
    m_data[0] = a_from.m_data[0];
    m_data[1] = a_from.m_data[1];
    m_data[2] = a_from.m_data[2];
    return *this;
  }
public:
  const T& v0() const { return m_data[0];}
  const T& v1() const { return m_data[1];}
  const T& v2() const { return m_data[2];}

  void v0(const T& a_value) { m_data[0] = a_value;}
  void v1(const T& a_value) { m_data[1] = a_value;}
  void v2(const T& a_value) { m_data[2] = a_value;}

  const T& x() const {return m_data[0];}
  const T& y() const {return m_data[1];}
  const T& z() const {return m_data[2];}

  T& x() {return m_data[0];}
  T& y() {return m_data[1];}
  T& z() {return m_data[2];}

  void set_value(const T& a0,const T& a1,const T& a2) {
    m_data[0] = a0;
    m_data[1] = a1;
    m_data[2] = a2;
  }
  void set_value(const T aV[3]) {
    m_data[0] = aV[0];
    m_data[1] = aV[1];
    m_data[2] = aV[2];
  }
  void value(T& a0,T& a1,T& a2) const {
    a0 = m_data[0];
    a1 = m_data[1];
    a2 = m_data[2];
  }

  T length(T(*a_sqrt)(T)) const {
    return a_sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]+m_data[2]*m_data[2]);
  }

  T normalize(T(*a_sqrt)(T)) {
    T norme = length(a_sqrt);
    if(norme==T()) return T();
    divide(norme);
    return norme;
  }

  T dot(const vec3& aV) const {
    return (m_data[0] * aV.m_data[0] +
            m_data[1] * aV.m_data[1] +
            m_data[2] * aV.m_data[2]);
  }

  void cross(const vec3<T>& aV,vec3<T>& a_value) const {
    a_value.set_value(m_data[1] * aV.m_data[2] - m_data[2] * aV.m_data[1],
                      m_data[2] * aV.m_data[0] - m_data[0] * aV.m_data[2],
                      m_data[0] * aV.m_data[1] - m_data[1] * aV.m_data[0]);
  }

  bool equal(const vec3& aV) const {
    if(m_data[0]!=aV.m_data[0]) return false;
    if(m_data[1]!=aV.m_data[1]) return false;
    if(m_data[2]!=aV.m_data[2]) return false;
    return true;
  }

  template <class PREC>
  bool equal_prec(const vec3& a_v,PREC a_prec,PREC(*a_fabs)(const T&)) const {
    if(&a_v==this) return true;
    for(unsigned int index=0;index<3;index++) {
      T diff = m_data[index]-a_v.m_data[index];
      if(a_fabs(diff)>=a_prec) return false;
    }
    return true;
  }

  vec3<T> _cross(const vec3<T>& aV) const {
    //not effective.
    return vec3<T>(m_data[1] * aV.m_data[2] - m_data[2] * aV.m_data[1],
                   m_data[2] * aV.m_data[0] - m_data[0] * aV.m_data[2],
                   m_data[0] * aV.m_data[1] - m_data[1] * aV.m_data[0]);
  }

  bool divide(const T& a_T) {
    if(a_T==T()) return false;
    m_data[0] /= a_T;
    m_data[1] /= a_T;
    m_data[2] /= a_T;
    return true;
  }

  void multiply(const T& a_T) {
    m_data[0] *= a_T;
    m_data[1] *= a_T;
    m_data[2] *= a_T;
  }

  void add(const vec3& a_v) {
    m_data[0] += a_v.m_data[0];
    m_data[1] += a_v.m_data[1];
    m_data[2] += a_v.m_data[2];
  }

  void add(const T& a0,const T& a1,const T& a2) {
    m_data[0] += a0;
    m_data[1] += a1;
    m_data[2] += a2;
  }

  void subtract(const vec3& a_v) {
    m_data[0] -= a_v.m_data[0];
    m_data[1] -= a_v.m_data[1];
    m_data[2] -= a_v.m_data[2];
  }

  void subtract(const T& a0,const T& a1,const T& a2) {
    m_data[0] -= a0;
    m_data[1] -= a1;
    m_data[2] -= a2;
  }

  bool cos_angle(const vec3& a_v,T& a_cos,T(*a_sqrt)(T)) const {
    //WARNING : if ret false, a_cos is not set.
    T this_length = length(a_sqrt);
    if(this_length==T()) return false;
    T a_v_length = a_v.length(a_sqrt);
    if(a_v_length==T()) return false;
    a_cos = dot(a_v)/(this_length*a_v_length);
    return true;
  }

  bool theta_phi(T& a_theta,T& a_phi,T(*a_sqrt)(T),T(*a_atan2)(T,T)) const {
    //WARNING : if ret false, a_theta, a_phi are not set.
    if(length(a_sqrt)==T()) return false;
    a_phi = a_atan2(m_data[1],m_data[0]);
    T xy = a_sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]);
    a_theta = a_atan2(xy,m_data[2]);
    return true;
  }

public: //operators
  T& operator[](size_t a_index) {
    //WARNING : no check on a_index.
    return m_data[a_index];
  }
  const T& operator[](size_t a_index) const {
    //WARNING : no check on a_index.
    return m_data[a_index];
  }

  vec3& operator*=(const T& a_v) {
    m_data[0] *= a_v;
    m_data[1] *= a_v;
    m_data[2] *= a_v;
    return *this;
  }

  vec3 operator+(const vec3& a_v) const {
    return vec3(m_data[0]+a_v.m_data[0],
                m_data[1]+a_v.m_data[1],
                m_data[2]+a_v.m_data[2]);
  }

  vec3 operator-(const vec3& a_v) const {
    return vec3(m_data[0]-a_v.m_data[0],
                m_data[1]-a_v.m_data[1],
                m_data[2]-a_v.m_data[2]);
  }

  vec3 operator*(const T& a_v) const {
    return vec3(m_data[0]*a_v,
                m_data[1]*a_v,
                m_data[2]*a_v);
  }

  vec3 operator/(const T& a_v) const {
    if(a_v==T()) return vec3();
    return vec3(m_data[0]/a_v,
                m_data[1]/a_v,
                m_data[2]/a_v);
  }

  bool operator==(const vec3& a_v) const {return equal(a_v);}
  bool operator!=(const vec3& a_v) const {return !operator==(a_v);}

public: //for tools/sg/sf_vec
  typedef unsigned int size_type;
  size_type size() const {return 3;}
  const T* data() const {return m_data;}
  size_type data_size() const {return 3;} //for eqT.
public: //for iv2sg
  const T* getValue() const {return m_data;}
  void setValue(const T& a0,const T& a1,const T& a2) {
    m_data[0] = a0;
    m_data[1] = a1;
    m_data[2] = a2;
  }
  void getValue(T& a0,T& a1,T& a2) const {
    a0 = m_data[0];
    a1 = m_data[1];
    a2 = m_data[2];
  }
  void setValue(const vec3& a_v) {
    m_data[0] = a_v.m_data[0];
    m_data[1] = a_v.m_data[1];
    m_data[2] = a_v.m_data[2];
  }
  void setValue(const T aV[3]) {
    m_data[0] = aV[0];
    m_data[1] = aV[1];
    m_data[2] = aV[2];
  }

  vec3& setValue(const vec3& a_bary,
                 const vec3& a_v0,const vec3& a_v1,const vec3& a_v2) {
    m_data[0] = a_bary[0]*a_v0[0]+a_bary[1]*a_v1[0]+a_bary[2]*a_v2[0];
    m_data[1] = a_bary[0]*a_v0[1]+a_bary[1]*a_v1[1]+a_bary[2]*a_v2[1];
    m_data[2] = a_bary[0]*a_v0[2]+a_bary[1]*a_v1[2]+a_bary[2]*a_v2[2];
    return *this;
  }

public:
  static const vec3<T>& s_x() {static const vec3<T> s_v(1,0,0);return s_v;}
  static const vec3<T>& s_y() {static const vec3<T> s_v(0,1,0);return s_v;}
  static const vec3<T>& s_z() {static const vec3<T> s_v(0,0,1);return s_v;}
protected:
  T m_data[3];

private:static void check_instantiation() {vec3<float> v;}
};

//for sf, mf :
template <class T>
inline const T* get_data(const vec3<T>& a_v) {return a_v.data();}

template <class T>
inline void get_normal(const vec3<T>& a_p0,const vec3<T>& a_p1,const vec3<T>& a_p2,vec3<T>& a_nm,
                       vec3<T>& a_tmp_1,vec3<T>& a_tmp_2,T(*a_sqrt)(T)) {
  // Used to optimize sg::bin().
  //(a_p1-a_p0).cross(a_p2-a_p1,a_nm);

  a_tmp_1 = a_p1;
  a_tmp_1.subtract(a_p0);

  a_tmp_2 = a_p2;
  a_tmp_2.subtract(a_p1);

  a_tmp_1.cross(a_tmp_2,a_nm);

  a_nm.normalize(a_sqrt);
}

template <class VEC3>
inline void direction(const VEC3& a_p0,const VEC3& a_p1,const VEC3& a_p2,VEC3& a_value) {
  // Orientation is computed by taking (p1 - p0) x (p2 - p0)
  VEC3 P = a_p1;
  P.subtract(a_p0);
  VEC3 P2 = a_p2;
  P2.subtract(a_p0);
  P.cross(P2,a_value);
}

template <class VEC3>
inline void area(const VEC3& a_p0,const VEC3& a_p1,const VEC3& a_p2,typename VEC3::elem_t& a_value,
                 VEC3& a_tmp_1,VEC3& a_tmp_2,VEC3& a_tmp_3) {
  // area of the triangle (a_p0,a_p1,a_p2)
  typedef typename VEC3::elem_t T;

  a_tmp_1 = a_p1;
  a_tmp_1.subtract(a_p0);

  a_tmp_2 = a_p2;
  a_tmp_2.subtract(a_p1);

  a_tmp_1.cross(a_tmp_2,a_tmp_3);

  a_value = a_tmp_3.length()/T(2);
}

template <class VEC3>
inline void area(const VEC3& a_p0,const VEC3& a_p1,const VEC3& a_p2,typename VEC3::elem_t& a_value) {
  VEC3 tmp1,tmp2,tmp3;
  area(a_p0,a_p1,a_p2,a_value,tmp1,tmp2,tmp3);
}

template <class T>
inline void direction(const T& a_0_x,const T& a_0_y,const T& a_0_z,
                      const T& a_1_x,const T& a_1_y,const T& a_1_z,
                      const T& a_2_x,const T& a_2_y,const T& a_2_z,vec3<T>& a_value) {
  direction(vec3<T>(a_0_x,a_0_y,a_0_z),
            vec3<T>(a_1_x,a_1_y,a_1_z),
            vec3<T>(a_2_x,a_2_y,a_2_z),a_value);
}

}

#include <ostream>

namespace tools {

// for sf_vec::dump().
template <class T>
inline std::ostream& operator<<(std::ostream& a_out,const vec3<T>& a_this){
  a_out << "x = " << a_this.v0()
        << ",y = " << a_this.v1()
        << ",z = " << a_this.v2();
  return a_out;
}

}

#endif
