#ifndef MAT_HH_INCLUDED
#define MAT_HH_INCLUDED

#include "Vec.hh"
#include <cmath>
#include "TString.h"




/*! Basic 3x3 matrix */

struct Mat
{
  double R00, R01, R02;
  double R10, R11, R12;
  double R20, R21, R22;

  Mat()               { set(0.0); }
  Mat(double a)       { set(a); }
  Mat(const Vec& v)   { set(v); }
  Mat(double m[3][3]) { set(m); }
  Mat(double r00, double r01, double r02, 
      double r10, double r11, double r12, 
      double r20, double r21, double r22 ) :
    R00 (r00), R01 (r01), R02 (r02),
    R10 (r10), R11 (r11), R12 (r12),
    R20 (r20), R21 (r21), R22 (r22) {}
  
  Mat& set(double a)  // set diagonal to a, rest to 0
  {
    R00=R11=R22=a;
    R01=R02=R10=R12=R20=R21=0;
    return *this;
  }

  Mat& set( double m[3][3] )
  {
    R00=m[0][0]; R01=m[0][1]; R02=m[0][2];
    R10=m[1][0]; R11=m[1][1]; R12=m[1][2];
    R20=m[2][0]; R21=m[2][1]; R22=m[2][2];
    return *this;
  }

  inline Mat& set(double r00, double r01, double r02, 
		  double r10, double r11, double r12, 
		  double r20, double r21, double r22 )
  {
    R00 = r00; R01 = r01; R02 = r02;  
    R10 = r10; R11 = r11; R12 = r12;  
    R20 = r20; R21 = r21; R22 = r22;
    return *this;
  }

  Mat& set(Vec v)     // set matrix R, so that R * (0,0,1) = v
  {
    v.normalize();
    if ( v.z == 1 ) 
      {
        set(1);
      }
    else 
     { 
      const double cos_theta = v.z;
      const double sin_theta = sqrt( 1 - v.z*v.z);
      const double sin_phi   = v.y / sin_theta;
      const double cos_phi   = v.x / sin_theta;

      R00 = cos_phi *cos_theta;    R01 = -sin_phi;    R02 = v.x;
      R10 = sin_phi *cos_theta;    R11 = cos_phi;     R12 = v.y;
      R20 = -sin_theta;            R21 = 0;           R22 = v.z;
    }

    return *this;
  }

  Mat& set_rotation( Vec v, double cos_angle ) // rotation matrix for rotation around v
  {
    //http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
    v.normalize();
    const double c = cos_angle;
    const double s = sqrt(1-c*c); 
    const double t = 1 - c;
    
    set( t*v.x*v.x + c ,	t*v.x*v.y - v.z*s ,	t*v.x*v.z + v.y*s,
	 t*v.x*v.y + v.z*s,	t*v.y*v.y + c, 	        t*v.y*v.z - v.x*s,
	 t*v.x*v.z - v.y*s, 	t*v.y*v.z + v.x*s, 	t*v.z*v.z + c );

    return *this;
  }

  Mat& set_rotation( Vec v1, Vec v2 )
  {
    // Produce a rotation matrix M , so that M * v1 = v2.
    // Nb: the matrix is not uniquely defined by v1 and v2; the result
    // with be the shortest-angle rotation that does the job.
    // http://stackoverflow.com/questions/286274/direct3d-geometry-rotation-matrix-from-two-vectors

    double c = v1.normalize().dot(v2.normalize());
    Vec axis = v1.cross(v2).normalize();

    // deal with aligning v1 and v2... 
    if ( c < -0.999999999 || c > 0.999999999 ) 
    	{
	    Vec v = v1;
    	v.normalize();
    	if ( v.z < 0.99 && v.z > -.99 ) axis =  v.cross(Vec(0,0,1)).normalize();
    	else axis= v.cross(Vec(0,1,0)).normalize();
    	}
    
    return set_rotation( axis, c );
  }


  /*! Return matrix determinant. */
  double det()
  {
    return 
      R00 * R11 * R22  +  R01 * R12 * R20  +  R02 * R10 * R21  -
      R02 * R11 * R20  -  R01 * R10 * R22  -  R00 * R12 * R21 ;
  }

  /*! Return inverse of matrix */
  
  Mat inv()
  {
    Mat r;  
    r.set( R11*R22-R21*R12, R02*R21-R01*R22, R01*R12-R02*R11,
	   R12*R20-R10*R22, R00*R22-R02*R20, R10*R02-R00*R12,
	   R10*R21-R20*R11, R20*R01-R00*R21, R00*R11-R10*R01 );

    r *= (1.0/det() );
    return r;
  }

  /*! return transposed matrix */
  Mat T()
  {
    return Mat( R00, R10, R20,
		R01, R11, R21,
		R02, R12, R22 );
  }

  
  
  Mat& operator*=(double a)
  {
    R00*=a;  R01*=a; R02*=a;
    R10*=a;  R11*=a; R12*=a;
    R20*=a;  R21*=a; R22*=a;
    return *this;
  }

  Mat& operator+=( Mat& m )
  {
    R00+=m.R00; R01+=m.R01; R02+=m.R02; 
    R10+=m.R10; R11+=m.R11; R12+=m.R12; 
    R20+=m.R20; R21+=m.R21; R22+=m.R22; 
    return *this;
  }

  Mat& operator-=( Mat& m )
  {
    R00-=m.R00; R01-=m.R01; R02-=m.R02; 
    R10-=m.R10; R11-=m.R11; R12-=m.R12; 
    R20-=m.R20; R21-=m.R21; R22-=m.R22; 
    return *this;
  }
  
  const char* __str__() const
  {
    TString fmt(" %5.4lf");
    return Form(("\n|"+fmt+fmt+fmt+" |\n|"+fmt+fmt+fmt+" |\n|"+fmt+fmt+fmt+TString(" |")).Data(),
		R00,R01,R02, R10,R11,R12, R20,R21,R22 );    
  }
   
  void print(std::ostream& out = std::cout ) const
  {
    out << __str__() << std::endl;
  }

  ClassDefNV(Mat,1)
};


inline Vec operator*(const Mat& m, const Vec& v)
{
  return Vec( m.R00 * v.x + m.R01 * v.y + m.R02 * v.z,
	      m.R10 * v.x + m.R11 * v.y + m.R12 * v.z,
	      m.R20 * v.x + m.R21 * v.y + m.R22 * v.z );
}

inline Mat operator*(const Mat& a, const Mat& b )
{
  Mat r;
  r.set( a.R00*b.R00 + a.R01*b.R20 + a.R02*b.R20, 
	 a.R00*b.R01 + a.R01*b.R11 + a.R02*b.R21, 
	 a.R00*b.R02 + a.R01*b.R12 + a.R02*b.R22,
	 
	 a.R20*b.R00 + a.R11*b.R20 + a.R12*b.R20, 
	 a.R10*b.R01 + a.R11*b.R11 + a.R12*b.R21, 
	 a.R20*b.R02 + a.R11*b.R12 + a.R12*b.R22,
	 
	 a.R20*b.R00 + a.R21*b.R20 + a.R22*b.R20, 
	 a.R10*b.R01 + a.R21*b.R11 + a.R22*b.R21, 
	 a.R20*b.R02 + a.R21*b.R12 + a.R22*b.R22 );
  
  return r;
}

#endif