/***************************************************************************
 *   Copyright (C) 2005 by Joachim Friedrich                               *
 *   joachim.friedrich@biozentrum.uni-wuerzburg.de                         *
 *                                                                         *
 *   This file is part of profdist and cbcanalyzer                         *
 *                                                                         *
 *   Both profdist and cbcanalyzer are free software; you can redistribute * 
 *   it and/or modify it under the terms of the GNU General Public License * 
 *   as published by the Free Software Foundation; either version 2 of the * 
 *   License, or (at your option) any later version.                       *
 *                                                                         *
 *   Profdist and cbcanalyzer are distributed in the hope that it will be  *
 *   useful, but WITHOUT ANY WARRANTY; without even the implied warranty   *
 *   of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the      *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#ifndef PROFDIST_T_MATRIX_INL_INCLUDED
#define PROFDIST_T_MATRIX_INL_INCLUDED


//---------------------------------------------------------------------------
// INCLUDES
//---------------------------------------------------------------------------
//#include "tMatrix.h"

//---------------------------------------------------------------------------
// DEFINES, MAKROS
//---------------------------------------------------------------------------

#include <utility>
#include <stdexcept>
#include <limits>
#include <cmath>

// Boost is currently not used, but will be used at many places
// #include <boost/algorithm/string.hpp>
#include "trim.h"

template<class T>
tMatrix<T> operator + ( tMatrix <T> const& l, tMatrix<T> const& r )
{
  if ( r.nRows() != l.nRows() || l.nCols() != r.nCols() )
    throw std::runtime_error("Matrices have different sizes.");

  tMatrix<T> c( l.nRows(), l.nCols() );
  typename std::vector<T>::iterator it = c.begin();
  for( typename std::vector<T>::const_iterator it_l = l.begin(),   
        it_r = r.begin(),   
        e = r.end(); it_r != e; ++it_l, ++it_r, ++it )
    *it = *it_r + *it_l;
  return c;
}

template<class T>
tMatrix<T> operator - ( tMatrix <T> const& l, tMatrix<T> const& r )
{
  if ( r.nRows() != l.nRows() || l.nCols() != r.nCols() )
    throw std::runtime_error("Matrices have different sizes.");

  tMatrix<T> c( l.nRows(), l.nCols() );
  typename std::vector<T>::iterator it = c.begin();
  for( typename std::vector<T>::const_iterator it_l = l.begin(),   
        it_r = r.begin(),   
        e = r.end(); it_r != e; ++it_l, ++it_r, ++it )
    *it = *it_r - *it_l;
  return c;
}

template<class T>
T infNorm( tMatrix<T> const& M );


template<class T>
double correctionJukes( tMatrix<T> & M );

template<class T>
double correctionKimura( tMatrix<T> & M );

template<class T>
double correctionLogDet( tMatrix<T> & N, tMatrix < double > & Q, int expmType );

template<class T>
double correctionGTR( tMatrix<T> & N, tMatrix < double > & Q, int expmType, int fMax );

//---------------------------------------------------------------------------
//
//  name          : swap
//
//  description   : swap to elements of a matrix
//
//  input         : *this
//
//  return value  : void
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> operator * ( tMatrix<T> const&  A, tMatrix<T> const& B )
{
  if ( A.nCols() != B.nRows() )
    throw std::runtime_error("Matrices have incompatible sizes.");

  T sum;
  tMatrix<T> newMat( A.nRows(), B.nCols() );
  for ( std::size_t i = 0; i < A.nRows(); ++i )
  {
    for ( std::size_t j = 0; j < B.nCols(); ++j )
    {
      sum = 0;
      for ( std::size_t k = 0; k < A.nCols(); ++k )
        sum += A( i, k ) * B( k, j );
      newMat( i, j ) = sum;
    }
  }

  return newMat;
}

template<class T>
tMatrix<T> operator * ( tMatrix<T> const& M, T const& value )
{
  tMatrix<T> c( M.nRows(), M.nCols() );
  typename std::vector<T>::iterator  it = c.begin();
  for( typename std::vector<T>::const_iterator it_l = M.begin(),   
        e = M.end(); it_l != e; ++it_l, ++it )
    *it = *it_l * value;
  return c;
}

//---------------------------------------------------------------------------
//
//  name          : swap
//
//  description   : swap to elements of a matrix
//
//  input         : *this
//
//  return value  : void
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> transpose( tMatrix<T> const& M )
{
  tMatrix<T> newMat( M.nCols(), M.nRows() );
  for( std::size_t i = 0; i < M.nCols(); ++i )
    for( std::size_t j = 0; j < M.nRows(); ++j )
      newMat( i, j ) = M( j, i );

  return newMat;

} 

//---------------------------------------------------------------------------
//
//  name          : swap
//
//  description   : swap to elements of a matrix
//
//  input         : *this
//
//  return value  : void
//
//---------------------------------------------------------------------------

template<class T>
void tMatrix<T>::swap( size_t row1, size_t col1, size_t row2, size_t col2 )
{
  T temp;
  temp = ( * this ) ( row1, col1 );
  ( * this ) ( row1, col1 ) = ( * this ) ( row2, col2 );
  ( * this ) ( row2, col2 ) = temp;
}

template<class T>
T infNorm( tMatrix<T> const& M )
{
  std::size_t n = M.nRows();
  T value = 0;
  T rowSum = 0;
  for ( std::size_t i = 0; i < n; ++i )
  {
    value = 0;
    for ( std::size_t j = 0; j < n; ++j )
      value = value + fabs( M( i, j ) );
    if ( rowSum < value )
      rowSum = value;
  }
  return ( rowSum );
}

//---------------------------------------------------------------------------
//
//  name          : swap
//
//  description   : swap to elements of a matrix
//
//  input         : *this
//
//  return value  : void
//
//---------------------------------------------------------------------------

template<class T>
void tMatrix<T>::writeFile( std::ostream & outfile ) const
{
  // Festlegung der Ausgabenotation:
  outfile.setf( ios::scientific, ios::floatfield ); // scientific-Notation
  outfile.setf( ios::right, ios::adjustfield ); // rechtsb\u0081ndig
  outfile.setf( ios::showpoint ); // Dezimalpunkt und abschliessende Nullen ausgeben
  outfile.precision( 3 ); // Genauigkeit der Gleitkommawerte = 2

  outfile << mRows << endl;
  for ( std::size_t i = 0; i < mRows; ++i )
  {
    for ( std::size_t j = 0; j < mCols; ++j )
      outfile << setw( 11 ) << mMat[ i * mCols + j ];
    outfile << endl;
  }
  outfile << endl;
}



template<class T>
void tMatrix<T>::getIdentSeq( mapTypeVecII & mapIdentSeq, int iThreshold )
{
  double threshold;
  threshold = 10.0 / double( iThreshold );

  for ( std::size_t i = 0; i < mRows; ++i )
    for ( std::size_t j = i + 1; j < mCols; ++j )
    {
      if ( mMat[ i * mCols + j ] < threshold )
      {
        vector < int > vecTemp;
        vecTemp.push_back( i );
        vecTemp.push_back( j );
        mapIdentSeq[vecTemp] = true;
      }
    }
}
//---------------------------------------------------------------------------
//
//  name          : swap
//
//  description   : swap to elements of a matrix
//
//  input         : *this
//
//  return value  : void
//
//---------------------------------------------------------------------------

template<class T>
void tMatrix<T>::writeFile( std::ostream & outfile, vecTypeNames const& vecSeqNames ) const
{
  using namespace boost::algorithm;
  // Festlegung der Ausgabenotation:
  size_t maxLen;
  std::string strTemp;

  // Festlegung der Ausgabenotation:
  outfile.setf( ios::scientific, ios::floatfield ); // scientific-Notation
  outfile.setf( ios::adjustfield, ios::right ); // rechtsb\u0081ndig
  outfile.setf( ios::showpoint ); // Dezimalpunkt und abschliessende Nullen ausgeben
  outfile.precision( 3 ); // Genauigkeit der Gleitkommawerte = 2

  outfile << mRows << endl;

  maxLen = this->getMaxLen();

  size_t maxLenVec = 0;
  size_t vecLen = vecSeqNames.size();

  for ( size_t i = 0; i < vecLen; ++i )
  {
    if ( maxLenVec < vecSeqNames[i].size() )
      maxLenVec = vecSeqNames[i].size();
  }

  ++maxLenVec;

  for ( size_t i = 0; i < mRows; ++i )
  {
    outfile << setw( maxLenVec ) << replace_all_copy( trim_copy( vecSeqNames[i] ), ' ', '_') ;

    for ( size_t j = 0; j < mCols; ++j )
      outfile << setw( maxLen ) << mMat[ i * mCols + j ];
    outfile << endl;
  }
  outfile << endl;
}

//---------------------------------------------------------------------------
//
//  name          : swap
//
//  description   : swap to elements of a matrix
//
//  input         : *this
//
//  return value  : void
//
//---------------------------------------------------------------------------

template<class T>
void tMatrix<T>::writeFile4( std::ostream & outfile ) const
{
  // Festlegung der Ausgabenotation:
  outfile.setf( ios::scientific, ios::floatfield ); // scientific-Notation
  outfile.setf( ios::adjustfield, ios::right ); // rechtsb\u0081ndig
  outfile.setf( ios::showpoint ); // Dezimalpunkt und abschliessende Nullen ausgeben
  outfile.precision( 3 ); // Genauigkeit der Gleitkommawerte = 2

  for ( long i = 0; i < 4; ++i )
  {
    for ( long j = 0; j < 4; ++j )
      outfile << setw( 11 ) << mMat[ i * mCols + j ];
    outfile << endl;
  }
  outfile << endl;
}

//---------------------------------------------------------------------------
//
//  name          : inverse
//
//  description   : compute the inverse of a matrix
//
//  input         : *this
//
//  return value  : cMatrix => the inverse Matrix
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> inverse( tMatrix<T> & M )
{
  std::cout << "EEK IT UP" << std::endl;
  long mSize = M.nRows();
  tMatrix<T> input( M );
  tMatrix<T> output( mSize, mSize );
  output.setDiag( 1 );

  tVector < T > vecCol( mSize );
  tVector < T > vecRow( mSize );
  tVector < T > vecPivot( mSize );

  long i, col = 0, row = 0, j, k, p, q;
  double big, dum, pivot_inv; //, temp;

  for ( i = 0; i < mSize; ++i )
    vecPivot( i ) = 0;

  // main loop
  for ( i = 0; i < mSize; ++i )
  {
    big = 0.0;
    for ( j = 0; j < mSize; ++j )
      if ( vecPivot( j ) != 1 )
        for ( k = 0; k < mSize; ++k )
          if ( vecPivot( k ) == 0 )
            if ( fabs( input( j, k ) ) >= big )
            {
              big = fabs( input( j, k ) );
              row = j;
              col = k;
            }
#if defined(USE_EXCEPTIONS)
    else if ( vecPivot( k ) > 1 )
      throw singMat1;
#endif

    vecPivot( col ) = vecPivot( col ) + 1;
    // we have found the pivot we'll interchange rows, if needed. we put the pivot on the diagonal, then.

    if ( row != col )
    {
      for ( p = 0; p < mSize; ++p )
        input.swap( row, p, col, p );
      for ( p = 0; p < mSize; ++p )
        output.swap( row, p, col, p );
    }

    vecRow( i ) = row;
    vecCol( i ) = col;

#if defined(USE_EXCEPTIONS)
    if ( input( col, col ) == 0.0 )
      throw singMat2;
#endif

    pivot_inv = 1.0 / input( col, col );

    for ( p = 0; p < mSize; ++p )
      input( col, p ) = input( col, p ) * pivot_inv;
    for ( p = 0; p < mSize; ++p )
      output( col, p ) = output( col, p ) * pivot_inv;

    // time to reduce rows

    for ( q = 0; q < mSize; ++q )
      if ( q != col )
      {
        dum = input( q, col );
        for ( p = 0; p < mSize; ++p )
          input( q, p ) = input( q, p ) - input( col, p ) * dum;
        for ( p = 0; p < mSize; ++p )
          output( q, p ) = output( q, p ) - output( col, p ) * dum;
      }
    // end of the big for with index i
  }
  return output;
}

//---------------------------------------------------------------------------
//
//  name          : setDiag
//
//  description   : sets the diag of a matrix with a value
//
//  input         : *this
//                  myType value => the new value of the diag
//
//  return value  : *this
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> & tMatrix<T>::setDiag( const T value )
{
  for ( size_t i = 0; i < mRows; ++i )
  {
    for ( size_t j = 0; j < mCols; ++j )
      ( * this ) ( i, j ) = 0;
    ( * this ) ( i, i ) = value;
  }
  return * this;
}

template<class T>
void normalize_log_det( tMatrix<T> & n )
{
  std::size_t rows = n.nRows();
  std::size_t cols = n.nCols();
  for( std::size_t i = 0; i < rows; ++i )
  {
    double row_sum = 0;
    for( std::size_t j = 0; j < cols ; ++j )
      row_sum += n( i, j );

    if( std::abs(row_sum) > std::numeric_limits<double>::epsilon() )
    {
      row_sum = 1.0/row_sum;
      for( std::size_t j = 0; j < cols ; ++j )
        n( i, j ) *= row_sum;
    }
  }
}

//---------------------------------------------------------------------------
//
//  name          : expm
//
//  description   : sets the diag of a matrix with a value
//
//  input         : *this
//                  myType value => the new value of the diag
//
//  return value  : *this
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> expmTaylor( tMatrix<T> & M, T t )
{
  double norm = 1;
  double threshold;
  T skalar;
  double j = 2; // count steps in Taylor series
  int rows = M.nRows();

  tMatrix<T> result( M );
  tMatrix<T> A( M );
  tMatrix<T> B( rows, rows );

  // Berechnen des Treshold
  threshold = getThreshold( M );
  result = result * t;

  // j = 0
  // Addition der Einheitsmatrix
  for ( long i = 0; i < rows; ++i )
    result( i, i ) = result( i, i ) + 1;

  //j >= 2
  skalar = threshold + 1;
  norm = threshold + 1;
  while ( norm > threshold )
  {
    skalar = pow( t, j ) / factorial( j );
    A = A * M;
    B = A * skalar;
    result = result + B;

    if ( j > 6 )
      norm = getNorm( B );
    ++j;
  }

  return result;
}
//---------------------------------------------------------------------------
//
//  name          : padm
//
//  description   : compute the matrix exponential with padé approximation
//
//  input         : *this
//                  ifstream & infile => input file object
//
//  return value  : *this
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> expm( tMatrix<T> & M, const T t, int expmType )
{
#if defined(USE_EXCEPTIONS)
  if ( t == 0.0 || t == 0 )
    throw tZero;
#endif

  switch ( expmType )
  {
    case 0:
      return expmPade( M, t );
    case 1:
      return expmTaylor( M, t );
    default:
      return expmTaylor( M, t );
  }
}

//---------------------------------------------------------------------------
//
//  name          : padm
//
//  description   : compute the matrix exponential with padé approximation
//
//  input         : *this
//                  ifstream & infile => input file object
//
//  return value  : *this
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> expmPade( tMatrix<T> & M, const T t )
{
  int p = 6;
  int n = M.nRows();
  int odd = 1;
  T temp, temp1, temp2;
  T s;

  tMatrix<T> A( M * t );
  tMatrix<T> B( n, n );
  tMatrix<T> Q( n, n );
  tMatrix<T> P( n, n );
  tMatrix<T> I( n, n );
  tMatrix<T> output( n, n );

  tVector < T > vecC( p + 1 );
  // berechnen der Pade Koeffzienten
  vecC( 1 ) = 1;
  for ( int i = 2; i < p; ++i )
    vecC( i ) = vecC( i - 1 ) * ( p + 1 - i ) / ( i * ( 2 * p - i + 1 ) );

  // Scaling
  s = infNorm( M );

  if ( s > 0.5 )
  {
    temp = floor( log( double( s ) ) / log( double( 2 ) ) ) + 2;
    if ( temp > 0 )
      s = temp;
    else
      s = 0;
    A = A * pow( 2, -s );
  }

  // Horner evaluation
  B = A * A;
  Q.setDiag( vecC( p ) );
  P.setDiag( vecC( p - 1 ) );

  for ( int i = p - 2; i > 0; --i )
  {
    if ( odd == 1 )
      Q = Q * B + I.setDiag( vecC( i ) );
    else
      P = P * B + I.setDiag( vecC( i ) );
    odd = 1 - odd;
  }
  if ( odd == 1 )
  {
    Q = Q * A;
    Q = Q - P;
    temp1 = -1;
    temp2 = 2;
    output = ( I + ( inverse( Q ) * P ) * temp2 ) * temp1;
  }
  else
  {
    P = P * A;
    Q = Q - P;
    temp2 = 2;
    output = I.setDiag( 1 ) + ( ( inverse( Q ) * P ) * temp2 );
  }

  // Squaring
  for ( int i = 0; i <= s; ++i )
    output = output * output;

  return output;
}

//---------------------------------------------------------------------------
//
//  name          : treshold
//
//  description   : compute a threshold for computation of the Taylor Series
//
//  input         : *this
//
//  return value  : myType  => threshold
//
//---------------------------------------------------------------------------

template<class T>
double getNorm( tMatrix<T> & M )
{
  int n = M.nRows();
  double value = 0;
  for ( int i = 0; i < n; ++i )
    for ( int j = 0; j < n; ++j )
      value = value + pow( M( i, j ), double( 2 ) );
  return ( sqrt( value ) );
}

//---------------------------------------------------------------------------
//
//  name          : treshold
//
//  description   : compute a threshold for computation of the Taylor Series
//
//  input         : *this
//
//  return value  : myType  => threshold
//
//---------------------------------------------------------------------------

template<class T>
double getThreshold( tMatrix<T> const& M )
{
  int n = M.nRows();
  double temp;
  double min = fabs( M( 1, 1 ) );
  double sum = 0;
  for ( int i = 0; i < n; ++i )
    for ( int j = 0; j < n; ++j )
    {
      temp = fabs( M( i, j ) );
      if ( min > temp )
        min = temp;
      sum = sum + temp;
    }
  return min / sum;
}

//---------------------------------------------------------------------------
//
//  name          : readFilePhylip
//
//  description   : read in matrix in phylip file format
//
//  input         : *this
//                  ifstream & infile => input file object
//
//  return value  : *this
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> & tMatrix<T>::readPhylip( std::istream & infile )
{
  infile.seekg( 0, ios::beg ); // pos a the beginning of the file

  int n;
  T value;

  infile >> n;
  this->resize( n, n );

  for ( int i = 0; i < n; ++i )
    for ( int j = 0; j < n; ++j )
    {
      infile >> value;
      ( * this ) ( i, j ) = value;
    }
  return * this;
}

//---------------------------------------------------------------------------
//
//  name          : readFilePhylip
//
//  description   : read in matrix in phylip file format
//
//  input         : *this
//                  ifstream & infile => input file object
//
//  return value  : *this
//
//---------------------------------------------------------------------------

template<class T>
tMatrix<T> & tMatrix<T>::readPhylip( std::istream & infile, std::vector < std::string > & vecSeqNames )
{
  int n, j;
  string strTemp;
  T value;
  infile >> n;
  this->resize( n, n );

  for ( int i = 0; i < n; ++i )
  {
    infile >> strTemp;
    vecSeqNames.push_back( std::string( strTemp.c_str() ) );

    for ( j = 0; j < n; ++j )
    {
      infile >> value;
      ( * this ) ( i, j ) = value;
    }
  }
  return * this;
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double correction( tMatrix<T> & M, tMatrix < double > & Q, int model, int expmType, int fMax )
{
  double returnValue;

  tMatrix < double > N;

  N = createMatrixN( M );
  N = N + transpose( N );
  //std::string test;

  //test = N.output();

  //ofstream outfile( "_mat.txt", ofstream::out );
  //N.writeFile4( outfile );

  switch ( model )
  {
    case 0:
      returnValue = correctionJukes( N );
    break;
    case 1:
      returnValue = correctionKimura( N );
    break;
    case 2:
      returnValue = correctionGTR( N, Q, expmType, fMax ) / 100;
    break;
    case 3:
      returnValue = correctionLogDet( N, Q, expmType ) / 100;
    break;

    default:
      returnValue = 0;
  }

  return returnValue;
}


//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double correctionJukes( tMatrix<T> & M )
{
  double match = 0;
  double all = 0;

  for ( int i = 0; i < 4; ++i )
  {
    for ( int j = 0; j < 4; ++j )
    {
      all += M( i, j );
    }
    match += M( i, i );
  }
  double temp = 1 - ( 4.0 * ( all - match ) / ( all * 3.0 ) );
  if ( ! temp > 0 )
    throw std::runtime_error("Alignment data too divergent. Use a different correction model");
    
    return ( -0.75 * log( temp ) );
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double correctionKimura( tMatrix<T> & M )
{
  double P, Q;
  double all = 0;
  double term1, term2;

  for ( int i = 0; i < 4; ++i )
    for ( int j = 0; j < 4; ++j )
      all += M( i, j );

  P = ( M( 0, 2 ) + M( 2, 0 ) + M( 1, 3 ) + M( 3, 1 ) ) / all;
  Q = ( M( 0, 1 ) + M( 1, 0 ) + M( 0, 3 ) + M( 3, 0 ) + M( 1, 2 ) + M( 2, 1 ) + M( 2, 3 ) + M( 3, 2 ) ) / all;

  term1 = 1 / ( 1 - 2 * P - Q );
  term2 = 1 / ( 1 - 2 * Q );

#if defined(USE_EXCEPTIONS)
  if ( term1 <= 0 )
    throw kimura1;
  if ( term2 <= 0 )
    throw kimura2;
#endif

  return ( 0.5 * log( term1 ) + 0.25 * log( term2 ) );
}

//---------------------------------------------------------------------------
//
//  name          : correctionGTR
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double correctionGTR( tMatrix<T> & N, tMatrix < double > & Q, int expmType, int fMax )
{
  //tMatrix < double > f, F( 4, 4 ), temp;
  double returnValue = 0.0;

  switch ( fMax )
  {
    case 0:
      returnValue = fZeroDerivation( N, Q, expmType );
    break;
    case 1:
      returnValue = fZeroNewtonMethod( N, Q, expmType );
    break;
    case 2:
      returnValue = fZeroRobust( N, Q, expmType );
    break;
    case 3:
      returnValue = fZeroParabolic( N, Q, expmType );
    break;
    default:
      returnValue = 0;
  }

  /*std::string strTemp;
  temp = Q * 1000000.0;
  strTemp = temp.output();
  f = expm( temp, 1.0, expmType );
  strTemp = f.output();
  f = expm( temp, 1.0, 0 );
  strTemp = f.output();
  F = 0.0;
  for ( int i = 0; i < 4; ++i )
  {
    F( i, i ) = f( 0, i );
  }

  strTemp = F.output();
  temp = F * Q;
  strTemp = temp.output();
  tr = trace( F * Q );

  returnValue = -returnValue * tr;
*/
  return returnValue;
}

//---------------------------------------------------------------------------
//
//  name          : det
//
//  description   : calculate the determinant of a matrix
//
//  input         :  tMatrix<T> M  =>  the matrix to be calculated
//
//  return value  : T   =>  the determinant of the matrix
//
//---------------------------------------------------------------------------

template<class T>
T det2( tMatrix<T> & M )
{
  long nRows = M.nRows();
  long nCols = M.nRows();

#if defined(USE_EXCEPTIONS)
  if ( nRows != nCols )
    throw matDet;
#endif
  if ( nRows == 1 ) return M( 0 );

  if ( nRows == 2 ) return M( 0 ) * M( 3 ) - M( 1 ) * M( 2 );

  T determinat = 0.0;
  for ( long i = 0; i < nRows; ++i )
  {
    //BTL_Matrix minor = Minor( i, 1 );
    tMatrix<T> subMat; //( i, 1 );
    subMat = minorRowCol( M, i, 0 );

    if ( i % 2 == 0 )
      determinat -= M( nCols * i ) * det( subMat );
    else
      determinat += M( nCols * i ) * det( subMat );
  }
  return determinat;
}
//---------------------------------------------------------------------------
//
//  name          : det
//
//  description   : calculate the determinant of a matrix
//
//  input         :  tMatrix<T> M  =>  the matrix to be calculated
//
//  return value  : T   =>  the determinant of the matrix
//
//---------------------------------------------------------------------------

template<class T>
T det( tMatrix<T> const& M )
{
  T determinant = 1.0;
  std::size_t rows = M.nRows();

  if ( rows == M.nCols() )
  {
    tMatrix<T> tempM( M );

    T coef;
    std::size_t row
      , refRow = 0;

    bool detEqualsZero = false;

    while ( !( detEqualsZero ) && ( refRow < rows - 1 ) )
    {
      if ( fabs( tempM( refRow, refRow ) ) < ZERO )
        pivot( tempM, refRow, determinant, detEqualsZero );

      if ( !( detEqualsZero ) )
        for ( row = refRow + 1; row < rows; ++row )
          if ( fabs( tempM( row, refRow ) ) > ZERO )
          {
            coef = -tempM( row, refRow ) / tempM( refRow, refRow );
            for ( size_t i = 0; i < rows; ++i )
              tempM( row, i ) = tempM( row, i ) + coef * tempM( refRow, i );
          }
      determinant *= tempM( refRow, refRow );
      ++refRow;
    }
    if ( detEqualsZero )
      determinant = zeroc.real();
    else
      determinant = determinant * tempM( rows - 1, rows - 1 );
  }
#if defined(USE_EXCEPTIONS)
  else
    throw matDet;
#endif

  return determinant;
}

//---------------------------------------------------------------------------
//
//  name          : pivot
//
//  description   :
//
//  input         :  tMatrix<T> M  =>  the matrix to be calculated
//
//  return value  : T   =>  the determinant of the matrix
//
//---------------------------------------------------------------------------
/*
template<class T>
tMatrix<T> submatrix( tMatrix<T> M, long lr, long r, long lc, long c )
// returns a designated submatrix, sa, of matrix a
{
  long m = M.nRows();
  long n = M.ncols();

  if ( ( r > m ) || ( c > n ) )
  {
    cerror( "index out of range\n" );
    return a;
  }

  long r2 = r - lr + 1;
  long c2 = c - lc + 1;

  tMatrix<T> subMat( r2, c2 );

  long lrm1 = lr - 1;
  long lcm1 = lc - 1;

  for ( long i = 0; i < r2; ++i )
    for ( long j = 0; j < c2; ++j )
      subMat( i, j ) = M( lrm1 + i, lcm1 + j );
  return subMat;

} // submatrix(tmat<T> a, long lr, long r, long lc, long c)
*/
// return the matrix minor row i and colum j

template<class T>
tMatrix<T> minorRowCol( tMatrix<T> M, long i, long j )
{
  long nRows = M.nRows();
  long nCols = M.nCols();

  //if ( nRows != nCols )
  //  FATAL_ERROR( "Cannot calculate Minor of non-square matrix." );

  // if ( i < 0 || i >= nRows || j < 0 || j >= nCols )
  //  FATAL_ERROR( "One or both indexes are out of range" );

  tMatrix<T> result( nRows - 1, nCols - 1 );

  for ( long row = 0; row < nRows; ++row )
    for ( long col = 0; col < nCols; ++col )
    {
      if ( row < i )
      {
        if ( col < j )
          result( row, col ) = M( row, col );
        else if ( col > j )
          result( row, col - 1 ) = M( row, col );
      }
      else if ( row > i )
      {
        if ( col < j )
          result( row - 1, col ) = M( row, col );
        else if ( col > j )
          result( row - 1, col - 1 ) = M( row, col );
      }
    }
  return result;
}

//---------------------------------------------------------------------------
//
//  name          : pivot
//
//  description   :
//
//  input         :  tMatrix<T> M  =>  the matrix to be calculated
//
//  return value  : T   =>  the determinant of the matrix
//
//---------------------------------------------------------------------------

template<class T>
void pivot( tMatrix<T> & M, size_t refRow, T & determinant, bool & detEqualsZero )
{
  detEqualsZero = true;

  std::size_t newRow = refRow;
  std::size_t rows = M.nRows();

  while ( detEqualsZero && ( ++newRow < rows ) )
  {
    if ( fabs( M( newRow, refRow ) ) > ZERO )
    {
      for ( size_t i = 0; i < M.nRows(); ++i )
      {
        T temp = M( newRow, i );
        M( newRow, i ) = M( refRow, i );
        M( refRow, i ) = temp;
      }
      detEqualsZero = false;
      determinant = -determinant; // row swap adjustment to det
    }
  }

} // Pivot(Matrix<T> &M, long refRow, T & determinant, bool & detEqualsZero)

//---------------------------------------------------------------------------
//
//  name          : expm
//
//  description   : sets the diag of a matrix with a value
//
//  input         : *this
//                  myType value => the new value of the diag
//
//  return value  : *this
//
//---------------------------------------------------------------------------

template<class T>
tMatrix < double > createMatrixN( tMatrix<T> & M )
{
  std::size_t dim = 4;
  tMatrix < double > newMat( dim, dim );

  for( std::size_t i = 0; i < dim; ++i )
  {
    for( std::size_t j = 0; j < dim; ++j )
    {
      newMat( i, j ) = M( i, j );
    }
  }
  return newMat;

  //mMapDNA["V"] = 6; //{ACG}
  //mMapDNA["H"] = 7; //{ACT}
  //mMapDNA["M"] = 8; //{AC}
  //mMapDNA["R"] = 9; //{AG}
  //mMapDNA["D"] = 10; //{AGT}
  //mMapDNA["W"] = 11; //{AT}
  //mMapDNA["S"] = 12; //{CG}
  //mMapDNA["B"] = 13; //{CGT}
  //mMapDNA["Y"] = 14; //{CT}
  //mMapDNA["K"] = 15; //{GT}
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double fZeroRobust( tMatrix<T> & N, tMatrix<T> & Q, int expmType )
{
  double t, lastLt, Lt, delta;
  delta = 1;

  //t = 0;
  int x = int( correctionJukes( N ) * 100 );
  t = double( x );

  Lt = -999999999;

  do
  {
    do
    {
      t += delta;
      lastLt = Lt;
      Lt = logLike( N, Q, t, expmType );
    }
    while ( lastLt < Lt );
    Lt = lastLt;
    t -= delta;
    delta /= 2;
  }
  while ( delta > 0.0001 );

  return t;
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double fZeroDerivation( tMatrix<T> & N, tMatrix<T> & Q, int expmType )
{
  double t, Lt, delta;
  delta = 1;

  //t = 0;
  int x = int( correctionJukes( N ) * 100 );

  if ( x == 0 )
  {
    x = 1;
    delta = 0.33;
  }
  else if ( x == 1 )
  {
    delta = 0.33;
  }

  t = double( x );

  Lt = logLikeDeriv1( N, Q, t, expmType );

  if ( Lt < 0 )
    do
    {
      t -= delta;
      Lt = logLikeDeriv1( N, Q, t, expmType );
    }
    while ( Lt < 0 );

  do
  {
    do
    {
      t += delta;
      Lt = logLikeDeriv1( N, Q, t, expmType );
      if ( Lt == 0 )
        break;
    }
    while ( Lt > 0 );
    t -= delta;
    delta /= 2;
  }
  while ( delta > 0.0001 );

  return t;
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double fZeroNewtonMethod( tMatrix<T> & N, tMatrix<T> & Q, int expmType )
{
  double x, xLast, xLast2, delta, deltaAbs, delta1, delta2;

  x = correctionJukes( N ) * 100;
  xLast = 0;
  xLast2 = 0;
  do
  {
    delta1 = logLikeDeriv1( N, Q, x, expmType );
    delta2 = logLikeDeriv2( N, Q, x, expmType );
    delta = delta1 / delta2;
    //delta = logLikeDeriv1( N, Q, x, expmType ) / logLikeDeriv2( N, Q, x, expmType );
    x = x - delta;
    if ( delta > 0 )
      deltaAbs = delta;
    else
      deltaAbs = delta * -1;
    if ( x - xLast2 < 0.000001 )
      break;
    xLast2 = xLast;
    xLast = x;
  }
  while ( deltaAbs > 0.0001 );

  return x;
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double correctionLogDet( tMatrix<T> & N, tMatrix < double > & Q, int expmType )
{
  double detPdata, detP;
  tMatrix < double > Pdata;
  tMatrix < double > P;

  P = expm( Q, 1.0, expmType );
  Pdata = N + transpose( N );
  normalize_log_det( Pdata );

  detPdata = det( Pdata );
  detP = det( P );

  if( ! detP > 0 )
    throw std::runtime_error("Bad Ratematrix used," );
  if( ! detPdata > 0 )
    throw std::runtime_error("Alignment data too divergent. Use a different correction model");
  
  return log( detPdata ) / log( detP );
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double fZeroParabolic( tMatrix<T> & N, tMatrix<T> & Q, int expmType )
{
  double a, b, u, x, w, v, corJ, xm, fw, fv, fx, fu, tol, tol1, tol2, r, q, p;
  double cGold, e, etemp, d = 0.0;

  corJ = correctionJukes( N ) * 100;
  tol = 0.0001;
  cGold = 0.3819660;
  e = 0.0;

  a = 0;
  b = 200;
  x = w = v = corJ;

  fw = logLike( N, Q, corJ, expmType );
  fv = fw;
  fx = fv; //logLike( N, Q, corJ );

  for ( int i = 1; i < 101; ++i )
  {
    xm = 0.5 * ( a + b );
    tol1 = tol * fabs( corJ ) + ZERO;
    tol2 = 2.0 * tol1;
    if ( fabs( x - xm ) <= ( tol2 - 0.5 * ( b - a ) ) )
      return x;

    if ( fabs( e ) > tol1 )
    {
      r = ( x - w ) * ( fx - fv );
      q = ( x - v ) * ( fx - fw );
      p = ( x - v ) * q - ( x - w ) * r;
      q = 2.0 * ( q - r );
      if ( q > 0.0 )
        p = -p;

      q = fabs( q );
      etemp = e;
      e = d;

      if ( fabs( p ) >= fabs( 0.5 * q * etemp ) || p <= q * ( a - x ) || p >= q * ( b - x ) )
        d = cGold * ( e = ( x >= xm ? a - x : b - x ) );
      else
      {
        d = p / q;
        u = x + d;
        if ( u - a < tol2 || b - u < tol2 )
          d = SIGN( tol1, xm - x );
      }
    }
    else
      d = cGold * ( e = ( x >= xm ? a - x : b - x ) );

    u = ( fabs( d ) >= tol1 ? x + d : x + SIGN( tol1, d ) );
    fu = logLike( N, Q, u, expmType );
    if ( fu >= fx )
    {
      if ( u >= x )
        a = x;
      else
        b = x;
      SHIFT( v, w, x, u ) SHIFT( fv, fw, fx, fu )
    }
    else
    {
      if ( u < x ) a = u;
      else
        b = u;
      if ( fu >= fw || w == x )
      {
        v = w;
        w = u;
        fv = fw;
        fw = fu;
      }
      else if ( fu >= fv || v == x || v == w )
      {
        v = u;
        fv = fu;
      }
    }
  }
  return x;
}

//---------------------------------------------------------------------------
//
//  name          : logLike
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------
/* template<class T> double logLike( tMatrix < int > & N, tMatrix<T> & Q, T t ) { double Lt = 0;

int rows = Q.nRows(); int cols = Q.nCols();

tMatrix < double > P;

P = expm( Q, t );

for ( int i = 0; i < rows; ++i ) for ( int j = 0; j < cols; ++j ) Lt += N( i, j ) * log( P( i, j ) );

return Lt; }*/

//---------------------------------------------------------------------------
//
//  NAME  :  correctionGTR
//
//---------------------------------------------------------------------------

template<class T>
double logLike( tMatrix<T> & N, tMatrix<T> & Q, T t, int expmType )
{
  double Lt = 0;

  int rows = N.nRows();
  int cols = N.nCols();

  tMatrix < double > P;

  P = expm( Q, t, expmType );

  for ( int i = 0; i < rows; ++i )
    for ( int j = 0; j < cols; ++j )
      Lt += N( i, j ) * log( P( i, j ) );

  return Lt;
}

//---------------------------------------------------------------------------
//
//  NAME  :  correctionGTR
//
//---------------------------------------------------------------------------

template<class T>
double logLikeDeriv1( tMatrix<T> & N, tMatrix<T> & Q, T t, int expmType )
{
  double dtLt = 0;

  size_t rows = N.nRows();
  size_t cols = N.nCols();

  tMatrix < double > P;
  tMatrix < double > PQ;

  P = expm( Q, t, expmType );

  //std::string test = P.output();

  PQ = P * Q;

  for ( size_t i = 0; i < rows; ++i )
    for ( size_t j = 0; j < cols; ++j )
    {
#if defined(USE_EXCEPTIONS)
      if ( P( i, j ) == 0.0 )
        throw deriv1;
#endif

      dtLt += N( i, j ) * PQ( i, j ) / P( i, j );
    }

  return dtLt;
}

//---------------------------------------------------------------------------
//
//  NAME  :  correctionGTR
//
//---------------------------------------------------------------------------

template<class T>
double logLikeDeriv2( tMatrix<T> & N, tMatrix<T> & Q, T t, int expmType )
{
  double ddtLt = 0;
  double powerP, powerPQ;

  int rows = N.nRows();
  int cols = N.nCols();

  tMatrix < double > P;
  tMatrix < double > PQ;
  tMatrix < double > PQQ;

  P = expm( Q, t, expmType );
  PQ = P * Q;
  PQQ = PQ * Q;

  for ( int i = 0; i < rows; ++i )
    for ( int j = 0; j < cols; ++j )
    {
      powerP = std::pow( P( i, j ), 2.0 );
      powerPQ = std::pow( PQ( i, j ), 2.0 );

#if defined(USE_EXCEPTIONS)
      if ( powerP == 0.0 )
        throw deriv2;
#endif

      ddtLt += N( i, j ) * ( PQQ( i, j ) * P( i, j ) - powerPQ ) / powerP;
    }
  return ddtLt;
}

template<class T>
std::string tMatrix<T>::output( vecTypeNames const& vecSeqNames ) const
{
  using namespace boost::algorithm;
  // Festlegung der Ausgabenotation:
  long maxLen;

  std::stringstream outStream;

  // Festlegung der Ausgabenotation:
/*  outStream.setf( ios::scientific, ios::floatfield ); // scientific-Notation
  outStream.setf( ios::adjustfield, ios::right ); // rechtsb\u0081ndig
  outStream.setf( ios::showpoint ); // Dezimalpunkt und abschliessende Nullen ausgeben
  outStream.precision( 3 ); // Genauigkeit der Gleitkommawerte = 2
*/
  outStream << mRows << endl;

  maxLen = this->getMaxLen();

  size_t maxLenVec = 0, vecLen = vecSeqNames.size();

  for ( size_t i = 0; i < vecLen; ++i )
  {
    if ( maxLenVec < vecSeqNames[i].size() )
      maxLenVec = vecSeqNames[i].size();
  }

  ++maxLenVec;

  for ( size_t i = 0; i < mRows; ++i )
  {
    if( i < vecSeqNames.size () )
      outStream << setw( maxLenVec ) << replace_all_copy( trim_copy( vecSeqNames[i] ), ' ', '_' )<< ' ';

    for ( size_t j = 0; j < mCols; ++j )
      outStream << setw( maxLen ) << mMat[ i * mCols + j ];
    outStream << endl;
  }
  outStream << endl;
  return outStream.str();
}


//---------------------------------------------------------------------------
//
//  name          : swap
//
//  description   : swap to elements of a matrix
//
//  input         : *this
//
//  return value  : void
//
//---------------------------------------------------------------------------

template<class T>
std::string tMatrix<T>::output() const
{
  // Festlegung der Ausgabenotation:
  long maxLen;
  std::ostringstream outStream;

  // Festlegung der Ausgabenotation:
/*  outStream.setf( ios::scientific, ios::floatfield ); // scientific-Notation
  outStream.setf( ios::adjustfield, ios::right ); // rechtsb\u0081ndig
  outStream.setf( ios::showpoint ); // Dezimalpunkt und abschliessende Nullen ausgeben
  outStream.precision( 3 ); // Genauigkeit der Gleitkommawerte = 2
*/
  outStream << mRows << endl;

  maxLen = this->getMaxLen();

  for ( long i = 0; i < mRows; ++i )
  {
    for ( long j = 0; j < mCols; ++j )
      outStream << setw( maxLen ) << mMat[ i * mCols + j ];
    outStream << endl;
  }
  outStream << endl;
  return outStream.str();
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
T tMatrix<T>::getMax() const
{
  return std::max_element( mMat.begin(), mMat.end() );
}

//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
double trace( tMatrix<T> M )
{
  T returnValue = 0;

  for ( int i = 0; i < 4; ++i ) // FIXME EEK!?
    returnValue += M( i, i );


  return returnValue;
}
//---------------------------------------------------------------------------
//
//  name          : correctionKimura
//
//  description   : compute the kimura coorection with a countmatrix N
//
//  input         : *this
//                  SquareMatrix & M =>
//
//  return value  : myType => kimura correction
//
//---------------------------------------------------------------------------

template<class T>
size_t tMatrix<T>::getMaxLen() const
{
  size_t maxLen = 0;
  for ( size_t i = 0; i < mRows * mCols; ++i )
  {
    std::ostringstream temp;
    temp << ( * this ) ( i );
    maxLen = std::max( maxLen, temp.str().length() );
  }
  return maxLen + 1;
}

template<class T>
tMatrix<T> exp( tMatrix<T> const& M, T const& t, profdist::ExpMType expmType )
{
  if ( t == 0.0 || t == 0 )
    throw tZero;

  switch ( expmType )
  {
    case profdist::Pade:    return exp_pade( M, t );
    case profdist::Taylor:  return exp_taylor( M, t );
  }
}

template<class T>
tMatrix<T> exp_taylor( tMatrix<T> const& M, T const& t )
{
  double norm = 1;
  double threshold;
  T skalar;
  double j = 2; // count steps in Taylor series
  int rows = M.nRows();

  tMatrix<T> result( M );
  tMatrix<T> A( M );
  tMatrix<T> B( rows, rows );

  // Berechnen des Treshold
  threshold = getThreshold( M );
  result = result * t;

  // j = 0
  // Addition der Einheitsmatrix
  for ( long i = 0; i < rows; ++i )
    result( i, i ) = result( i, i ) + 1;

  //j >= 2
  skalar = threshold + 1;
  norm = threshold + 1;
  while ( norm > threshold )
  {
    skalar = pow( t, j ) / factorial( j );
    A = A * M;
    B = A * skalar;
    result = result + B;

    if ( j > 6 )
      norm = getNorm( B );
    ++j;
  }

  return result;
}

template<class T>
tMatrix<T> exp_pade( tMatrix<T> const& M, T const& t )
{
  int p = 6;
  int n = M.nRows();
  int odd = 1;
  T temp, temp1, temp2;
  T s;

  tMatrix<T> A( M * t );
  tMatrix<T> B( n, n );
  tMatrix<T> Q( n, n );
  tMatrix<T> P( n, n );
  tMatrix<T> I( n, n );
  tMatrix<T> output( n, n );

  tVector < T > vecC( p + 1 );
  // berechnen der Pade Koeffzienten
  vecC( 1 ) = 1;
  for ( int i = 2; i < p; ++i )
    vecC( i ) = vecC( i - 1 ) * ( p + 1 - i ) / ( i * ( 2 * p - i + 1 ) );

  // Scaling
  s = infNorm( M );

  if ( s > 0.5 )
  {
    temp = floor( log( double( s ) ) / log( double( 2 ) ) ) + 2;
    if ( temp > 0 )
      s = temp;
    else
      s = 0;
    A = A * pow( 2, -s );
  }

  // Horner evaluation
  B = A * A;
  Q.setDiag( vecC( p ) );
  P.setDiag( vecC( p - 1 ) );

  for ( int i = p - 2; i > 0; --i )
  {
    if ( odd == 1 )
      Q = Q * B + I.setDiag( vecC( i ) );
    else
      P = P * B + I.setDiag( vecC( i ) );
    odd = 1 - odd;
  }
  if ( odd == 1 )
  {
    Q = Q * A;
    Q = Q - P;
    temp1 = -1;
    temp2 = 2;
    output = ( I + ( inverse( Q ) * P ) * temp2 ) * temp1;
  }
  else
  {
    P = P * A;
    Q = Q - P;
    temp2 = 2;
    output = I.setDiag( 1 ) + ( ( inverse( Q ) * P ) * temp2 );
  }

  // Squaring
  for ( int i = 0; i <= s; ++i )
    output = output * output;

  return output;
}


#endif

