#ifndef __SL_EULER_H
#define __SL_EULER_H

#include "slMathTraits.h"
#include "slVector3D.h"

namespace sl {

template<class T>
class euler_angle {
 public :
  static const unsigned int ZXY = 0;
  static const unsigned int XYZ = 1;
 private :
  T data[3];
 public :
  inline const T& heading(void)const { return data[0]; }
  inline const T& pitch(void)const { return data[1]; }
  inline const T& bank(void)const { return data[2]; }
  inline const T& roll(void)const { return data[2]; }
 public :
  bool from_vector(const vector3D<T>& v);
  bool from_vector_lw(const vector3D<T>& v);
 public :
  inline T& operator [](int index) { return data[index]; }
  inline const T& operator [](int index)const { return data[index]; }
 public :
  euler_angle& operator =(const euler_angle<T>& e);
 public :
  euler_angle();
  euler_angle(const euler_angle& e);
 ~euler_angle();
};

template<class T>
inline euler_angle<T>::euler_angle()
{
}

template<class T>
inline euler_angle<T>::euler_angle(const euler_angle& e)
{
 data[0] = e.data[0];
 data[1] = e.data[1];
 data[2] = e.data[2];
}

template<class T>
inline euler_angle<T>::~euler_angle()
{
}

template<class T>
inline euler_angle<T>& euler_angle<T>::operator =(const euler_angle<T>& e)
{
 if(this == &e) return *this;
 data[0] = e.data[0];
 data[1] = e.data[1];
 data[2] = e.data[2];
 return *this;
}

template<class T>
inline bool euler_angle<T>::from_vector(const vector3D<T>& v)
{
 // copy vector
 T x = v[0];
 T y = v[1];
 T z = v[2];

 // copy and compute length
 T denom = std::sqrt(x*x + y*y + z*z);
 if(denom == 0) return false;

 // normalize vector
 T scale = math_traits<T>::one()/denom;
 x *= scale;
 y *= scale;
 z *= scale;

 // compute angles
 data[0] = std::acos(-z);
 data[1] = math_traits<T>::zero();
 data[2] = atan2(-y, -x);

 return true;
}

template<class T>
inline bool euler_angle<T>::from_vector_lw(const vector3D<T>& v)
{
 // copy vector
 T x = v[0];
 T y = v[1];
 T z = v[2];

 // copy and compute length
 T denom = std::sqrt(x*x + y*y + z*z);
 if(denom == 0) return false;

 // normalize vector
 T scale = math_traits<T>::one()/denom;
 x *= scale;
 y *= scale;
 z *= scale;

 // compute angles
 data[0] = std::asin(x);
 data[1] = std::atan2(-y, z);
 data[2] = math_traits<T>::zero();

 return true;
}

};

#endif
