/***************************************************************************
 *   Copyright (C) 2005 by Andreas Pokorny                                 *
 *   andreas.pokorny@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_FIXED_MATRIX_MATH_H_INCLUDED
#define PROFDIST_FIXED_MATRIX_MATH_H_INCLUDED

#include <algorithm>
#include <limits>
#include "fixed_matrix.h"
#include "types.h"
namespace profdist {

namespace detail { template<typename T> inline T sqr( T const v ) { return v*v; } }

template<typename ValueT, size_t Rows, size_t Columns>
double taylor_threshold( fixed_matrix<ValueT,Rows,Columns> const& M );

inline double inf_norm( small_float_count_matrix  const& M );
template<typename ValueT, size_t Rows, size_t Columns>
inline double inf_norm( fixed_matrix<ValueT,Rows,Columns> const& M );

inline double norm( small_float_count_matrix  const& M );

template<typename ValueT, size_t Rows, size_t Columns>
double norm( fixed_matrix<ValueT,Rows,Columns> const& M );
/**
 * @brief calculates M to the power of t using taylor approximation
 * @param M base matrix
 * @param t scalar exponent
 */
template<typename ValueT, size_t Rows, size_t Columns>
fixed_matrix<ValueT,Rows,Columns> exp_taylor( fixed_matrix<ValueT,Rows,Columns> const& M, ValueT const& t );

/**
 * @brief calculates M to the power of t using pade approximation
 * @param M base matrix
 * @param t scalar exponent
 */
template<typename ValueT, size_t Rows, size_t Columns>
fixed_matrix<ValueT,Rows,Columns> exp_pade( fixed_matrix<ValueT,Rows,Columns> const& M, ValueT const& t  );

/**
 * \brief calculates M to the power of t
 * \param[in] M base matrix
 * \param[in] t scalar exponent
 * \param[in] expm aproximation flag
 * You can control the calculation method using the third parameter
 * possible Values are Taylor and Pade, where Taylor is the default.
 */
template<typename ValueT, size_t Rows, size_t Columns>
fixed_matrix<ValueT,Rows,Columns> exp( fixed_matrix<ValueT,Rows,Columns> const& M, ValueT const& t, profdist::ExpMType expmType )
{
  if ( t == 0.0 || t == 0 )
    throw runtime_error("zero in exponential of matrix");;

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

template<typename ValueT, size_t Rows, size_t Columns>
fixed_matrix<ValueT,Rows,Columns> exp_taylor( fixed_matrix<ValueT,Rows,Columns> const& M, ValueT const& t )
{
  typedef fixed_matrix<ValueT,Rows,Columns> matrix_type;
  matrix_type result( M );

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

  // j = 0
  // Addition der Einheitsmatrix
  std::size_t i = Rows;
  do{ --i; result[i][i] += 1; } while( i );

  //j >= 2
  double b_norm = threshold + 1;
  size_t j = 2
    ,  fac_j = 1;
  matrix_type A( M );
  while( b_norm > threshold )
  {
    fac_j*=j;
    A = A * M;
    matrix_type B( A * ( pow( t, double(j) ) / double(fac_j) ) );
    result += B;

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

  return result;
}




inline void set_diagonal( small_float_count_matrix & mat, double const& v ) {
  mat[0][0] = mat[1][1] = mat[2][2] = mat[3][3] = v;
  mat[0][1] = mat[0][2] = mat[0][3] = 0.0;
  mat[1][0] = mat[1][2] = mat[1][3] = 0.0;
  mat[2][0] = mat[2][1] = mat[2][3] = 0.0;
  mat[3][0] = mat[3][1] = mat[3][2] = 0.0;
}

template<class ValueT, size_t Size >
inline void set_diagonal( fixed_matrix<ValueT,Size,Size> & mat, ValueT const& v ) {
  for( std::size_t i = 0; i < Size; ++i ) {
    mat[i][i] = v;
    for( std::size_t j = i + 1; j < Size; ++j ) 
      mat[i][j] = mat[j][i] = 0;
  }
  std::cout << "Diag:" << mat << std::endl;
}

template<class ValueT, size_t Size >
inline void set_identity( fixed_matrix<ValueT,Size,Size> & mat ) {
  set_diagonal( mat, ValueT(1) );
}


inline void swap_column( small_float_count_matrix & mat, size_t i, size_t j ) {
  std::swap( mat[0][i], mat[0][j] );
  std::swap( mat[1][i], mat[1][j] );
  std::swap( mat[2][i], mat[2][j] );
  std::swap( mat[3][i], mat[3][j] );
}

template<class ValueT, size_t Size >
inline void swap_column( fixed_matrix<ValueT,Size,Size> & mat, size_t i, size_t j ) {
  for( std::size_t p = 0; p < Size; ++p )
    std::swap( mat[p][i], mat[p][j] );
}

inline void swap_row( small_float_count_matrix & mat, size_t i, size_t j ) {
  std::swap( mat[i][0], mat[j][0] );
  std::swap( mat[i][1], mat[j][1] );
  std::swap( mat[i][2], mat[j][2] );
  std::swap( mat[i][3], mat[j][3] );
}

template<class ValueT, size_t Size >
inline void swap_row( fixed_matrix<ValueT,Size,Size> & mat, size_t i, size_t j ) {
  for( std::size_t p = 0; p < Size; ++p )
    std::swap( mat[i][p], mat[j][p] );
}



template<class ValueT, size_t Size >
bool broken_inverse(fixed_matrix<ValueT,Size,Size>  const& M, fixed_matrix<ValueT,Size,Size> & output )
{
  typedef typename pick_next_math_type<ValueT>::type fl_t;
  typedef fixed_matrix<ValueT,Size,Size> matrix_type;
  matrix_type input( M );
  set_identity( output );

  size_t pivot_vector[Size] = {0};
  for( std::size_t i = 0; i < Size; ++i ) {
    fl_t biggest = 0.0;
    for( std::size_t pivot_index = i; pivot_index < Size; ++pivot_index )  {
      if( fabs( input[i][pivot_index] ) > biggest ) {
        biggest = fabs( input[i][pivot_index] );
        pivot_vector[i] = pivot_index;
      }
    }
    if( biggest == 0.0 )  // FIXME: use epsilon
      return false;

    if( i != pivot_vector[i] ) {
      swap_column( input, i, pivot_vector[i] );
      swap_column( output, i, pivot_vector[i] );
    }

    fl_t inv_pivot = fl_t(1.0) / fl_t(input[i][i]);
    for( size_t p = i; p < Size; ++p )  input[i][p] *= inv_pivot;
    for( size_t p = 0; p < Size; ++p )  output[i][p] *= inv_pivot;
    
    for( size_t  q = 0; q < Size; ++q )
      if( q != i )
      {
        fl_t dum = input[q][i];
        for( size_t p = i; p < Size; ++p ) input[q][p] -=  input[i][p] * dum;
        for( size_t p = 0; p < Size; ++p ) output[q][p] -=  output[i][p] * dum;
      }
  }

  std::size_t i = Size;
  do { --i;
    if( i != pivot_vector[i] ) {
      swap_column( input, i, pivot_vector[i] );
      swap_column( output, i, pivot_vector[i] );
    }
  }while (i);
}

template<class ValueT, size_t Size >
fixed_matrix<ValueT,Size,Size>  inverse(fixed_matrix<ValueT,Size,Size>  const&  M )
{
  typedef fixed_matrix<ValueT,Size,Size> matrix_type;
  matrix_type input( M ), output( 0) ;
  set_identity( output );


  size_t num_swaps = 0,  swaps[Size*2] = {0};
  size_t pivot_vector[Size] = {0};

  std::size_t column_index = 0, row_index = 0;
  // main loop
  for( std::size_t i = 0; i < Size; ++i )
  {
    double big = 0.0;
    for( std::size_t j = 0; j < Size; ++j )
      if( pivot_vector[j] != 1 )
        for( std::size_t k = 0; k < Size; ++k )
          if( pivot_vector[k] == 0 )
            if( fabs( input[j][k] ) >= big )
            {
              big = fabs( input[j][k] );
              row_index = j;
              column_index = k;
            }
#if defined(USE_EXCEPTIONS)
    else if( pivot_vector[k] > 1 )
      throw singMat1;
#endif

    ++(pivot_vector[column_index]);
    // we have found the pivot we'll interchange rows, if needed. we put the pivot on the diagonal, then.

    if( row_index != column_index )
    {
      for( std::size_t p = 0; p < Size; ++p )
        swap( input[row_index][p], input[column_index][p] );
      for( std::size_t p = 0; p < Size; ++p )
        swap( output[row_index][p], output[column_index][p] );
    }


#if defined(USE_EXCEPTIONS)
    if ( input[column_index][column_index] == 0.0 )
      throw singMat2;
#endif

    double pivot_inv = 1.0 / input[column_index][column_index];

    for( size_t p = 0; p < Size; ++p )
      input[column_index][p] *= pivot_inv;
    for( size_t p = 0; p < Size; ++p )
      output[column_index][p] *= pivot_inv;

    // time to reduce rows

    for( size_t  q = 0; q < Size; ++q )
      if( q != column_index )
      {
        double dum = input[q][column_index];
        for( size_t p = 0; p < Size; ++p )
          input[q][p] -=  input[column_index][p] * dum;
        for( size_t p = 0; p < Size; ++p )
          output[q][p] -=  output[column_index][p] * dum;
      }
    // end of the big for with index i
  }
  return output;
}

template<typename ValueT, size_t Rows, size_t Columns>
fixed_matrix<ValueT,Rows,Columns> exp_pade( fixed_matrix<ValueT,Rows,Columns> const& M, ValueT const& t  )
{
  typedef fixed_matrix<ValueT,Rows,Columns> matrix_type;
  const size_t p = 6;
  matrix_type A( M * t );

  ValueT pade_coefficient[p + 1] = {0};
  pade_coefficient[1] = 1;
  for( size_t i = 2; i != p; ++i )
    pade_coefficient[i] = pade_coefficient[i-1] * (p + 1 - i) /  ( i * ( 2 * p - i + 1 ) );
  
  // Scaling
  ValueT s = inf_norm( M );

  if ( s > 0.5 )
  {
    s = std::max(  floor( log( double( s ) ) / log( double( 2 ) ) ) + 2, 0.0 );
    A *= pow( 2, -s );
  }
  matrix_type B( A*A ), Q(0), P(0), output(0);
  // Horner evaluation
  set_diagonal( Q, pade_coefficient[ p ] );
  set_diagonal( P, pade_coefficient[ p-1 ] );

  matrix_type I(0);
  for( size_t i = p - 2; i > p%2; i-=2 )
  {
    set_diagonal( I,  pade_coefficient[ i ] );
    Q = Q * B + I; 
    set_diagonal( I,  pade_coefficient[ i -1 ] );
    P = P * B + I; 
  }
  if ( p%2 == 0  )
  {
    Q = Q*A - P;
    output = -( I + ( inverse(Q) * P ) * 2.0);
  }
  else
  {
    P *= A;
    Q -= P;
    set_diagonal( I, 1 );
    output = I + ( ( inverse(Q) * P ) * 2.0 );
  }

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

  return output;
}

inline void normalize_log_det( small_float_count_matrix & M )
{
  double a = M[0][0] + M[0][1] + M[0][2] + M[0][3];
  if( a > std::numeric_limits<small_float_count_matrix::value_type>::epsilon() ) {
    a = 1.0/a;
    M[0][0]*=a;M[0][1]*=a;M[0][2]*=a;M[0][3]*=a;
  }
  a = M[1][0] + M[1][1] + M[1][2] + M[1][3];
  if( a > std::numeric_limits<small_float_count_matrix::value_type>::epsilon() ) {
    a = 1.0/a;
    M[1][0]*=a;M[1][1]*=a;M[1][2]*=a;M[1][3]*=a;
  }
  a = M[2][0] + M[2][1] + M[2][2] + M[2][3];
  if( a > std::numeric_limits<small_float_count_matrix::value_type>::epsilon() ) {
    a = 1.0/a;
    M[2][0]*=a;M[2][1]*=a;M[2][2]*=a;M[2][3]*=a;
  }
  a = M[3][0] + M[3][1] + M[3][2] + M[3][3];
  if( a > std::numeric_limits<small_float_count_matrix::value_type>::epsilon() ) {
    a = 1.0/a;
    M[3][0]*=a;M[3][1]*=a;M[3][2]*=a;M[3][3]*=a;
  }
}
template<typename ValueT, size_t Rows, size_t Columns>
void normalize_log_det( profdist::fixed_matrix<ValueT, Rows,Columns> & n )
{
  size_t i = Rows;
  do { -- i;
    ValueT row_sum = 0;
    size_t j = Columns;
    do { --j;
      row_sum += n[i][j]; 
    }while(j);
    
    if( std::abs(row_sum) > std::numeric_limits<ValueT>::epsilon() ) {
      row_sum = 1.0/row_sum;
      j = Columns;
      do { --j;
        n[i][j] *= row_sum; 
      }while(j);
    }
  }while(i);
}

inline double inf_norm( small_float_count_matrix  const& M )
{
  return 
    std::max( fabs(M[0][0]) + fabs(M[0][1]) + fabs(M[0][2]) + fabs(M[0][3]), 
        std::max(fabs(M[1][0]) + fabs(M[1][1]) + fabs(M[1][2]) + fabs(M[1][3]), 
          std::max(fabs(M[2][0]) + fabs(M[2][1]) + fabs(M[2][2]) + fabs(M[2][3]), 
            fabs(M[3][0]) + fabs(M[3][1]) + fabs(M[3][2]) + fabs(M[3][3]) )));
}


template<typename ValueT, size_t Rows, size_t Columns>
inline double inf_norm( fixed_matrix<ValueT,Rows,Columns> const& M )
{
  std::size_t i = Rows;
  double max_sum = 0;
  do { --i;
    std::size_t j = Columns;
    double v = 0;
    do { --j;
      v += fabs(M[i][j]);
    }while(j);
    max_sum = std::max(v, max_sum );
  }while(i);

  return max_sum;
}


inline double norm( small_float_count_matrix  const& M )
{
  using detail::sqr;
  return sqrt( sqr(M[0][0]) + sqr(M[0][1]) + sqr(M[0][2]) + sqr(M[0][3])
      + sqr(M[1][0]) + sqr(M[1][1]) + sqr(M[1][2]) + sqr(M[1][3])
      + sqr(M[2][0]) + sqr(M[2][1]) + sqr(M[2][2]) + sqr(M[2][3])
      + sqr(M[3][0]) + sqr(M[3][1]) + sqr(M[3][2]) + sqr(M[3][3]) );
}


template<typename ValueT, size_t Rows, size_t Columns>
double norm( fixed_matrix<ValueT,Rows,Columns> const& M )
{
  double value = 0;
  std::size_t i = Rows;
  do { --i;
    std::size_t j = Columns;
    do { --j;
      value += M[i][j]*M[i][j];
    }while(j);
  }while(i);
  return sqrt( value );
}


template<typename ValueT, size_t Rows, size_t Columns>
double taylor_threshold( fixed_matrix<ValueT,Rows,Columns> const& M )
{
  double min = fabs( M[0][0] );
  double sum = 0;
  std::size_t i = Rows;
  do { --i;
    std::size_t j = Columns;
    do { --j;
      double temp = fabs( M[i][j] );
      if( min > temp ) min = temp;
      sum += temp;
    }while (j);
  }while(i);
    
  return min / sum;
}

namespace detail {
  template<typename ValueT, size_t Size>
  void pivot( profdist::fixed_matrix<ValueT, Size, Size> & M, size_t reference_row, ValueT & determinant, bool & det_is_zero )
  {
    det_is_zero = true;
    std::size_t target_row = reference_row;

    while( det_is_zero && ( ++target_row < Size ) )
    {
      if( fabs( M[target_row][reference_row] ) > std::numeric_limits<ValueT>::epsilon() )
      {
        swap_row( M, target_row, reference_row );
        det_is_zero = false;
        determinant = -determinant; // row swap adjustment to det
      }
    }
  }
}
template<typename ValueT, size_t Size>
ValueT det( profdist::fixed_matrix<ValueT, Size, Size> const& M )
{
  ValueT determinant = 1.0;
  fixed_matrix<ValueT, Size, Size>  temp_matrix( M );

  std::size_t row
    , reference_row = 0;

  bool det_is_zero = false;

  while( !( det_is_zero ) && ( reference_row < Size - 1 ) )
  {
    if ( fabs( temp_matrix[reference_row][reference_row] ) < std::numeric_limits<ValueT>::epsilon() )
      detail::pivot( temp_matrix, reference_row, determinant, det_is_zero );

    if( !det_is_zero )
      for( row = reference_row + 1; row < Size; ++row )
        if( fabs( temp_matrix[ row][reference_row] ) > std::numeric_limits<ValueT>::epsilon() )
        {
          ValueT coef = -temp_matrix[row][reference_row] / temp_matrix[reference_row][reference_row];
          for( size_t i = 0; i < Size ; ++i )
            temp_matrix[row][i] = temp_matrix( row, i ) + coef * temp_matrix[reference_row][i ];
        }
    determinant *= temp_matrix[reference_row][reference_row];
    ++reference_row;
  }
  return det_is_zero?0.0:determinant * temp_matrix[Size- 1][Size- 1 ];
}

}

#endif
