/*
 * 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 <gsl/gsl_blas.h>
#include <cmath>

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

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

  this->at(0,1) = src[3];
  this->at(1,1) = src[4];
  this->at(2,1) = src[5];

  this->at(0,2) = src[6];
  this->at(1,2) = src[7];
  this->at(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);
}

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){
  return *gsl_matrix_ptr (content, i, j);
}

double Matrix::determinant(){
  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(){
  double det = determinant();
  if(fabs(det)<MYEPSILON){
    throw NotInvertibleException(__FILE__,__LINE__);
  }

  double detReci = 1./det;
  Matrix res;
  res.at(0,0) =  detReci*RDET2(at(1,1),at(2,1),at(1,2),at(2,2));    // A_11
  res.at(1,0) = -detReci*RDET2(at(1,0),at(2,0),at(1,2),at(2,2));    // A_21
  res.at(2,0) =  detReci*RDET2(at(1,0),at(2,0),at(1,1),at(2,1));    // A_31
  res.at(0,1) = -detReci*RDET2(at(0,1),at(2,1),at(0,2),at(2,2));    // A_12
  res.at(1,1) =  detReci*RDET2(at(0,0),at(2,0),at(0,2),at(2,2));    // A_22
  res.at(2,1) = -detReci*RDET2(at(0,0),at(2,0),at(0,1),at(2,1));    // A_32
  res.at(0,2) =  detReci*RDET2(at(0,1),at(1,1),at(0,2),at(1,2));    // A_13
  res.at(1,2) = -detReci*RDET2(at(0,0),at(1,0),at(0,2),at(1,2));    // A_23
  res.at(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;
}