#include <cmath> 

#define MATRIXTLPGEO tlp::geo::Matrix<Obj,SIZE>


//===================================================================
//Specialisation
namespace tlp {
  namespace geo {
    template<typename Obj>
    class Matrix<Obj,1>:public Vector<Vector<Obj,1>,1> {
    public:
      Obj determinant() {return (*this)[0][0];}
      Matrix<Obj,1>& fill(Obj obj) {return *this;}
      Matrix<Obj,1>& inverse() {return *this;}
      Matrix<Obj,1>& transpose() {return *this;}
      Matrix<Obj,1>& operator*=(const Matrix<Obj,1> &mat) {return *this;}
      Matrix<Obj,1>& operator/=(const Obj &obj){return *this;}
      Matrix<Obj,1> cofactor() {return *this;}
    };
  }
}

//===================================================================
//Specialisation
template<typename Obj,unsigned int SIZE>
MATRIXTLPGEO & MATRIXTLPGEO::fill(Obj obj) {
  for (int i=0;i<SIZE;++i)
    (*this)[i].fill(obj);
  return (*this);
}
//===================================================================
template<typename Obj,unsigned int SIZE>
MATRIXTLPGEO & MATRIXTLPGEO::operator*=(const MATRIXTLPGEO &mat) {
  MATRIXTLPGEO tmpMat(*this);
  fill(0);
  for (int i=0;i<SIZE;++i)
    for (int j=0;j<SIZE;++j)
      for (int k=0;k<SIZE;++k)
	(*this)[i][j]+=tmpMat[i][k]*mat[k][j];
  return (*this);
}
//=====================================================================================
template<typename Obj,unsigned int SIZE>
MATRIXTLPGEO & MATRIXTLPGEO::operator/=(const Obj &obj) {
  for (int i=0;i<SIZE;++i)
    (*this)[i]/=obj;
  return (*this);
}
//=====================================================================================
template<typename Obj,unsigned int SIZE>
Obj MATRIXTLPGEO::determinant() {
  switch (SIZE) {
  case 2:
    return (*this)[0][0] * (*this)[1][1] - (*this)[1][0] * (*this)[0][1];
    break;
  case 3:
    return (*this)[0][0] * ((*this)[1][1]*(*this)[2][2] - (*this)[1][2] * (*this)[2][1])
      - (*this)[0][1] * ((*this)[1][0]*(*this)[2][2] - (*this)[1][2] * (*this)[2][0])
      + (*this)[0][2] * ((*this)[1][0]*(*this)[2][1] - (*this)[1][1] * (*this)[2][0]) ;
    break;
  default:
    int j2;
    Obj det = 0;
    for (int j1=0;j1<SIZE;++j1) {
      tlp::geo::Matrix<Obj, SIZE - 1> m;
      for (int i=1;i<SIZE;i++) {
	j2 = 0;
	for (int j=0;j<SIZE;++j) {
	  if (j == j1)
	    continue;
	  m[i-1][j2] = (*this)[i][j];
	  ++j2;
	}
      }

      if (j1 & 1)
	det += (*this)[0][j1] * m.determinant();
      else
	det -= (*this)[0][j1] * m.determinant();
    }
    return(det);
  }
}
//=====================================================================================
template<typename Obj,unsigned int SIZE>
MATRIXTLPGEO MATRIXTLPGEO::cofactor() {
  MATRIXTLPGEO result;
  switch (SIZE){
  case 2:
    (result)[0][0] = (*this)[1][1];	
    (result)[0][1] = - (*this)[1][0];	
    (result)[1][0] = - (*this)[0][1];	
    (result)[1][1] = (*this)[0][0];
    break;
  case 3:
    (result)[0][0] = (*this)[1][1]*(*this)[2][2] - (*this)[1][2]*(*this)[2][1];
    (result)[0][1] = - ((*this)[1][0]*(*this)[2][2] - (*this)[2][0]*(*this)[1][2]);
    (result)[0][2] = (*this)[1][0]*(*this)[2][1] - (*this)[1][1]*(*this)[2][0];
    (result)[1][0] = - ((*this)[0][1]*(*this)[2][2] - (*this)[0][2]*(*this)[2][1]);
    (result)[1][1] = (*this)[0][0]*(*this)[2][2] - (*this)[0][2]*(*this)[2][0];
    (result)[1][2] = - ((*this)[0][0]*(*this)[2][1] - (*this)[0][1]*(*this)[2][0]);
    (result)[2][0] = (*this)[0][1]*(*this)[1][2] - (*this)[0][2]*(*this)[1][1];
    (result)[2][1] = - ((*this)[0][0]*(*this)[1][2] - (*this)[0][2]*(*this)[1][0]); 
    (result)[2][2] = (*this)[0][0]*(*this)[1][1] - (*this)[0][1]*(*this)[1][0];
    break;
  default :
    int i1,j1;
    tlp::geo::Matrix<Obj,SIZE - 1> c;
    for (int j=0;j<SIZE;++j) {
      for (int i=0;i<SIZE;++i) {
	i1 = 0;
	for (int ii=0;ii<SIZE;++ii) {
	  if (ii == i)
	    continue;
	  j1 = 0;
	  for (int jj=0;jj<SIZE;jj++) {
	    if (jj == j)
	      continue;
	    c[i1][j1] = (*this)[ii][jj];
	    ++j1;
	  }
	    ++i1;
	}
	if ((i+j) & 1)  result[i][j]=c.determinant(); else result[i][j]=-c.determinant();
      }
    }
    break;
  }
  return result;
}

template<typename Obj,unsigned int SIZE>
MATRIXTLPGEO & MATRIXTLPGEO::transpose() {
  register Obj tmp;
  for (int i=1;i<SIZE;++i) {
    for (int j=0;j<i;++j) {
      tmp = (*this)[i][j];
      (*this)[i][j] = (*this)[j][i];
      (*this)[j][i] = tmp;
    }
  }
  return (*this);
}
template<typename Obj,unsigned int SIZE>
MATRIXTLPGEO & MATRIXTLPGEO::inverse() {
  (*this)=(*this).cofactor().transpose()/=(*this).determinant();
  return (*this);
}
template<typename Obj,unsigned int SIZE>
MATRIXTLPGEO MATRIXTLPGEO::operator*(const MATRIXTLPGEO &mat2){
  return  MATRIXTLPGEO(*this)*=mat2;
}
template<typename Obj,unsigned int SIZE>
tlp::geo::Vector<Obj,SIZE> MATRIXTLPGEO::operator*(const tlp::geo::Vector<Obj,SIZE> &vec) {
  tlp::geo::Vector<Obj,SIZE> result;
  for (int j=0;j<SIZE;++j) {
    result[j]=vec[0]*(*this)[j][0];
  }
  for (int i=1;i<SIZE;++i) {
    for (int j=0;j<SIZE;++j) {
      result[j]+=vec[j]*(*this)[j][i];
    }
  }
  return  result;
}
