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

#ifndef tools_mat3
#define tools_mat3

#include "mat"

//#include <cmath>

namespace tools {

template <class T>
class mat3 : public mat<T,3> {
  typedef mat<T,3> parent;
  typedef mat<T,3> pr;
public:
#ifdef TOOLS_MEM
  mat3(bool a_inc = true):parent(a_inc) {}
#else
  mat3():parent() {}
#endif
  mat3(const mat<T,3>& a_from):parent(a_from){}
  virtual ~mat3() {}
public:
  mat3(const mat3& a_from):parent(a_from){}
  mat3& operator=(const mat3& a_from){
    parent::operator=(a_from);
    return *this;
  }
public:
  mat3(const T& a_00,const T& a_01,const T& a_02, //first  row
       const T& a_10,const T& a_11,const T& a_12, //second row
       const T& a_20,const T& a_21,const T& a_22) //third  row
  {
    set_matrix(a_00,a_01,a_02,
               a_10,a_11,a_12,
               a_20,a_21,a_22);
  }
public:
  void set_matrix(const mat3<T>& a_m){parent::set_matrix(a_m);}
  void set_matrix(const T& a_00,const T& a_01,const T& a_02,   //1 row
                  const T& a_10,const T& a_11,const T& a_12,   //2 row
                  const T& a_20,const T& a_21,const T& a_22) { //3 row
    //a_<R><C>
    //pr::m_vec[R + C * 3];
    pr::m_vec[0] = a_00;pr::m_vec[3] = a_01;pr::m_vec[6] = a_02;
    pr::m_vec[1] = a_10;pr::m_vec[4] = a_11;pr::m_vec[7] = a_12;
    pr::m_vec[2] = a_20;pr::m_vec[5] = a_21;pr::m_vec[8] = a_22;
  }

  void set_scale(const T& a_s) {_set_scale(a_s,a_s,a_s,pr::m_vec);}
  void set_scale(const T& a_1,const T& a_2,const T& a_3) {_set_scale(a_1,a_2,a_3,pr::m_vec);}
  void set_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle,T(*a_sin)(T),T(*a_cos)(T)) {
    _set_rotate(a_x,a_y,a_z,a_angle,pr::m_vec,a_sin,a_cos);
  }

  bool get_rotate(T& a_x,T& a_y,T& a_z,T& a_angle,T(*a_acos)(T),T(*a_sin)(T),T(*a_sqrt)(T)) { //warning : acos and not cos.
    // used in tests/check_pauli. Same code as matTs/get_rotate.
    // we assume that we have a rotation matrix.
    // get (a_angle,a_n(x,y,z)) such that matrix is exp(a_angle*a_n*Rs) (passive(=coord) rotation) and then :
    //   exp(a_angle*n*Rs) = cos_angle*I3+(1-cos_angle)*n*n+sin_theta*n*Rs
    // trace = 3*cos_angle+(1-cos_angle)*n2
    T cos_angle = T(0.5)*(pr::m_vec[0]+pr::m_vec[4]+pr::m_vec[8]-T(1));
    if((cos_angle<T(-1))||(T(1)<cos_angle)) { //matrix is not a rotation :
      a_angle = T(0);
      a_x = 0;
      a_y = 0;
      a_z = 0;
      return false;
    }
    if(cos_angle==T(1)) {
      a_angle = T(0);
      a_x = 0;
      a_y = 0;
      a_z = 1;
      return true; //vector is undefined but we return true anyway.
    }
    T one_minus_cos_angle = T(1)-cos_angle;
    T x2 = (pr::m_vec[0]-cos_angle)/one_minus_cos_angle;
  //T y2 = (pr::m_vec[4]-cos_angle)/one_minus_cos_angle;
  //T z2 = (pr::m_vec[8]-cos_angle)/one_minus_cos_angle;
    /*
    if((x2<T(0))||(y2<T(0))||(z2<T(0))||(x2+y2+z2)!=T(1)) { //not a rotation :
      a_angle = T(0);
      a_x = 0;
      a_y = 0;
      a_z = 0;
      return false;
    }
    */
    a_angle = a_acos(cos_angle); //in ]0,pi].
    T sin_angle = a_sin(a_angle);  //in [0,1].
    if(sin_angle==T(0)) { //angle is pi. //sym part can determine vector, but up to a sign :
      // sym part is : cos_angle*I+(1-cos_angle)*n*n
      // cos_angle = -1
      T xy = T(0.5)*(pr::m_vec[3]+pr::m_vec[1])/one_minus_cos_angle;
      T xz = T(0.5)*(pr::m_vec[6]+pr::m_vec[2])/one_minus_cos_angle;
    //T yz = T(0.5)*(pr::m_vec[7]+pr::m_vec[5])/one_minus_cos_angle;
      a_x = a_sqrt(x2); //up to a sign.
      a_y = xy/a_x;
      a_z = xz/a_x;
      return true; //vector is defined up to a sign.
    }
    // antisym part is : sin(theta)*n.Rs
    a_z = T(0.5)*(pr::m_vec[3]-pr::m_vec[1])/sin_angle;
    a_y = T(-0.5)*(pr::m_vec[6]-pr::m_vec[2])/sin_angle; //warning : -1.
    a_x = T(0.5)*(pr::m_vec[7]-pr::m_vec[5])/sin_angle;
    return true;
  }

public:
  void mul_3(T& a_x,T& a_y,T& a_z) const {
    // a_[x,y,z] = this * a_[x,y,z]
    //pr::m_vec[R + C * 3];
    T x = pr::m_vec[0]*a_x+pr::m_vec[3]*a_y+pr::m_vec[6]*a_z;
    T y = pr::m_vec[1]*a_x+pr::m_vec[4]*a_y+pr::m_vec[7]*a_z;
    T z = pr::m_vec[2]*a_x+pr::m_vec[5]*a_y+pr::m_vec[8]*a_z;
    a_x = x;
    a_y = y;
    a_z = z;
  }

  void mul_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle,T(*a_sin)(T),T(*a_cos)(T)) {
    T rot[9];
    _set_rotate(a_x,a_y,a_z,a_angle,rot,a_sin,a_cos);
    parent::_mul_mtx(rot);
  }

  void left_mul_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle,T(*a_sin)(T),T(*a_cos)(T)) {
    T _m[9];
    _set_rotate(a_x,a_y,a_z,a_angle,_m,a_sin,a_cos);
    parent::_left_mul_mtx(_m);
  }

  void v00(const T& a_value){pr::m_vec[0+0*3] = a_value;}
  void v10(const T& a_value){pr::m_vec[1+0*3] = a_value;}
  void v20(const T& a_value){pr::m_vec[2+0*3] = a_value;}

  void v01(const T& a_value){pr::m_vec[0+1*3] = a_value;}
  void v11(const T& a_value){pr::m_vec[1+1*3] = a_value;}
  void v21(const T& a_value){pr::m_vec[2+1*3] = a_value;}

  void v02(const T& a_value){pr::m_vec[0+2*3] = a_value;}
  void v12(const T& a_value){pr::m_vec[1+2*3] = a_value;}
  void v22(const T& a_value){pr::m_vec[2+2*3] = a_value;}

  const T& v00() const {return pr::m_vec[0+0*3];}
  const T& v10() const {return pr::m_vec[1+0*3];}
  const T& v20() const {return pr::m_vec[2+0*3];}

  const T& v01() const {return pr::m_vec[0+1*3];}
  const T& v11() const {return pr::m_vec[1+1*3];}
  const T& v21() const {return pr::m_vec[2+1*3];}

  const T& v02() const {return pr::m_vec[0+2*3];}
  const T& v12() const {return pr::m_vec[1+2*3];}
  const T& v22() const {return pr::m_vec[2+2*3];}

protected:
  static void _set_scale(const T& a_1,const T& a_2,const T& a_3,T v[]) {
    v[0] = a_1;v[3] =   0;v[6] =   0;
    v[1] =   0;v[4] = a_2;v[7] =   0;
    v[2] =   0;v[5] =   0;v[8] = a_3;
  }

  static void _set_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle,T v[],T(*a_sin)(T),T(*a_cos)(T)) {
    //WARNING : it models the rotation of an object (active rotation) = exp(-a_angle*n(a_x,a_y,a_z)*Rs).
    //WARNING : (a_x,a_y,a_z) must be a normalized vector.
    T si = a_sin(a_angle);
    T co = a_cos(a_angle);
    T x = a_x;
    T y = a_y;
    T z = a_z;
    T x2 = x*x;
    T y2 = y*y;
    T z2 = z*z;
    T xy = x*y;
    T xz = x*z;
    T yz = y*z;
    v[0] =   x2+(1-x2)*co;v[3] = xy*(1-co)-z*si;v[6] = xz*(1-co)+y*si;
    v[1] = xy*(1-co)+z*si;v[4] =   y2+(1-y2)*co;v[7] = yz*(1-co)-x*si;
    v[2] = xz*(1-co)-y*si;v[5] = yz*(1-co)+x*si;v[8] = z2+(1-z2)*co;

    // If :
    //     n =(x,y,z)
    //    n2 = x2+y2+z2 = 1
    //   n.E = x*E1+y*E2+z*E3
    // with :
    //   E1            E2             E3
    //    0  0  0       0  0 -1        0  1  0
    //    0  0  1       0  0  0       -1  0  0
    //    0 -1  0       1  0  0        0  0  0
    //
    // R(r,c) = cos(theta)*Id(r,c)+(1-cos(theta))*nr*nc-sin(theta)*(n.E)(r,c)
    //
    // R = exp(-theta*(n.E))

  }

public:
private:static void check_instantiation() {mat3<float> dummy;}
};

////////////////////////////////////////////////
/// common matrices : //////////////////////////
////////////////////////////////////////////////

template <class T>
inline const mat3<T>& mat3_zero() {
  static const mat3<T> s_v(false); //inc mem count = false
  return s_v;
}

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

}

#include <ostream>

namespace tools {

template <class T>
inline std::ostream& operator<<(std::ostream& a_out,const mat3<T>& a_mtx){
  const T* v = a_mtx.data();
  a_out << v[0] << "," << v[3] << "," << v[6] << std::endl
        << v[1] << "," << v[4] << "," << v[7] << std::endl
        << v[2] << "," << v[5] << "," << v[8] << std::endl;
  return a_out;
}

}

#endif