/*
 * Matrix.cpp
 *
 *  Created on: Jun 25, 2010
 *      Author: crueger
 */

#include "Matrix.hpp"
#include "Helpers/Assert.hpp"
#include "Exceptions/NotInvertibleException.hpp"
#include "Helpers/fast_functions.hpp"
#include "Helpers/Assert.hpp"
#include "vector.hpp"

#include <gsl/gsl_blas.h>
#include <cmath>
#include <iostream>

using namespace std;

Matrix::Matrix()
{
  content = gsl_matrix_calloc(NDIM, NDIM);
}

Matrix::Matrix(const double* src){
  content = gsl_matrix_alloc(NDIM, NDIM);
  set(0,0, src[0]);
  set(1,0, src[1]);
  set(2,0, src[2]);

  set(0,1, src[3]);
  set(1,1, src[4]);
  set(2,1, src[5]);

  set(0,2, src[6]);
  set(1,2, src[7]);
  set(2,2, src[8]);
}

Matrix::Matrix(const Matrix &src){
  content = gsl_matrix_alloc(NDIM, NDIM);
  gsl_matrix_memcpy(content,src.content);
}

Matrix::Matrix(gsl_matrix* _content) :
  content(_content)
{}

Matrix::~Matrix()
{
  gsl_matrix_free(content);
}

void Matrix::one(){
  gsl_matrix_free(content);
  content = gsl_matrix_calloc(NDIM, NDIM);
  for(int i = NDIM;i--;){
    set(i,i,1.);
  }
}

Matrix &Matrix::operator=(const Matrix &src){
  if(&src!=this){
    gsl_matrix_memcpy(content,src.content);
  }
  return *this;
}

Matrix &Matrix::operator+=(const Matrix &rhs){
  gsl_matrix_add(content, rhs.content);
  return *this;
}

Matrix &Matrix::operator-=(const Matrix &rhs){
  gsl_matrix_sub(content, rhs.content);
  return *this;
}

Matrix &Matrix::operator*=(const Matrix &rhs){
  (*this) = (*this)*rhs;
  return *this;
}

Matrix Matrix::operator+(const Matrix &rhs) const{
  Matrix tmp = *this;
  tmp+=rhs;
  return tmp;
}

Matrix Matrix::operator-(const Matrix &rhs) const{
  Matrix tmp = *this;
  tmp-=rhs;
  return tmp;
}

Matrix Matrix::operator*(const Matrix &rhs) const{
  gsl_matrix *res = gsl_matrix_alloc(NDIM, NDIM);
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, this->content, rhs.content, 0.0, res);
  return Matrix(res);
}

double &Matrix::at(size_t i, size_t j){
  ASSERT(i>=0&&i<NDIM,"Index i for Matrix access out of range");
  ASSERT(j>=0&&j<NDIM,"Index j for Matrix access out of range");
  return *gsl_matrix_ptr (content, i, j);
}

const double Matrix::at(size_t i, size_t j) const{
  ASSERT(i>=0&&i<NDIM,"Index i for Matrix access out of range");
  ASSERT(j>=0&&j<NDIM,"Index j for Matrix access out of range");
  return gsl_matrix_get(content, i, j);
}

void Matrix::set(size_t i, size_t j, const double value){
  ASSERT(i>=0&&i<NDIM,"Index i for Matrix access out of range");
  ASSERT(j>=0&&j<NDIM,"Index j for Matrix access out of range");
  gsl_matrix_set(content,i,j,value);
}

double Matrix::determinant() const{
  return at(0,0)*at(1,1)*at(2,2)
       + at(0,1)*at(1,2)*at(2,0)
       + at(0,2)*at(1,0)*at(2,1)
       - at(2,0)*at(1,1)*at(0,2)
       - at(2,1)*at(1,2)*at(0,0)
       - at(2,2)*at(1,0)*at(0,1);
}

Matrix Matrix::invert() const{
  double det = determinant();
  if(fabs(det)<MYEPSILON){
    throw NotInvertibleException(__FILE__,__LINE__);
  }

  double detReci = 1./det;
  Matrix res;
  res.set(0,0,  detReci*RDET2(at(1,1),at(2,1),at(1,2),at(2,2)));    // A_11
  res.set(1,0, -detReci*RDET2(at(1,0),at(2,0),at(1,2),at(2,2)));    // A_21
  res.set(2,0,  detReci*RDET2(at(1,0),at(2,0),at(1,1),at(2,1)));    // A_31
  res.set(0,1, -detReci*RDET2(at(0,1),at(2,1),at(0,2),at(2,2)));    // A_12
  res.set(1,1,  detReci*RDET2(at(0,0),at(2,0),at(0,2),at(2,2)));    // A_22
  res.set(2,1, -detReci*RDET2(at(0,0),at(2,0),at(0,1),at(2,1)));    // A_32
  res.set(0,2,  detReci*RDET2(at(0,1),at(1,1),at(0,2),at(1,2)));    // A_13
  res.set(1,2, -detReci*RDET2(at(0,0),at(1,0),at(0,2),at(1,2)));    // A_23
  res.set(2,2,  detReci*RDET2(at(0,0),at(1,0),at(0,1),at(1,1)));    // A_33
  return res;
}

Matrix &Matrix::operator*=(const double factor){
  gsl_matrix_scale(content, factor);
  return *this;
}

Matrix operator*(const double factor,const Matrix& mat){
  Matrix tmp = mat;
  tmp*=factor;
  return tmp;
}

Matrix operator*(const Matrix &mat,const double factor){
  return factor*mat;
}

/** Blows the 6-dimensional \a cell_size array up to a full NDIM by NDIM matrix.
 * \param *symm 6-dim array of unique symmetric matrix components
 * \return allocated NDIM*NDIM array with the symmetric matrix
 */
Matrix ReturnFullMatrixforSymmetric(const double * const symm)
{
  Matrix matrix;
  matrix.set(0,0, symm[0]);
  matrix.set(1,0, symm[1]);
  matrix.set(2,0, symm[3]);
  matrix.set(0,1, symm[1]);
  matrix.set(1,1, symm[2]);
  matrix.set(2,1, symm[4]);
  matrix.set(0,2, symm[3]);
  matrix.set(1,2, symm[4]);
  matrix.set(2,2, symm[5]);
  return matrix;
};

ostream &operator<<(ostream &ost,const Matrix &mat){
  for(int i = 0;i<NDIM;++i){
    ost << "\n";
    for(int j = 0; j<NDIM;++j){
      ost << mat.at(i,j);
      if(j!=NDIM-1)
        ost << "; ";
    }
  }
  return ost;
}

Vector operator*(const Matrix &mat,const Vector &vec){
  gsl_vector *res = gsl_vector_calloc(NDIM);
  gsl_blas_dgemv( CblasNoTrans, 1.0, mat.content, vec.content, 0.0, res);
  return Vector(res);
}

Vector &operator*=(Vector& lhs,const Matrix &mat){
  lhs = mat*lhs;
  return lhs;
}