$$\phantom{******** If you see this on the webpage then the browser could not locate *********}$$
$$\phantom{******** jsMath/easy/load.js or the variable root is set wrong in this file *********}$$
$$\newcommand{\vector}[1]{\left[\begin{array}{c} #1 \end{array}\right]}$$ $$\newenvironment{matrix}{\left[\begin{array}{cccccccccc}} {\end{array}\right]}$$ $$\newcommand{\A}{{\cal A}}$$ $$\newcommand{\W}{{\cal W}}$$

/* Copyright 2008-2014 Research Foundation State University of New York   */

/* This file is part of QUB Express.                                      */

/* QUB Express is 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 3 of the License, or      */
/* (at your option) any later version.                                    */

/* QUB Express is 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,     */
/* named LICENSE.txt, in the QUB Express program directory.  If not, see  */
/* <http://www.gnu.org/licenses/>.                                        */

/*

*/

#ifdef _WIN32
  #include <windows.h>
#else
  #include <stdlib.h>
  #define BOOL int
  #define TRUE 1
  #define FALSE 0
#endif

#include <string.h>
#include <iostream>
#include <algorithm>

#define HAVE_INLINE
#include <gsl/gsl_eigen.h>

#include "ublas_plus.h"
#include "ublas_matrixutil.h"

using std::cerr;
using std::cout;
using std::endl;
using std::min;

void verify_inverse(double **A, matrix<double> Ainv, bool retry_gaussj=false)
{
  int n = (int) Ainv.size1();
  pointy_matrix<double> Am(n, n);
  matrix_from_arrays(Am.m, A, n, n);
  matrix<double> product(n, n);
  noalias(product) = prod(Am.m, Ainv);
  double ssd = 0.0;
  for (int i=0; i<n; ++i) {
    for (int j=0; j<n; ++j) {
      
      double d = product(i, j);
      if ( (i == j) && (fabs(d) > .1) )
	d -= 1.0;
      ssd += d*d;
    }
  }
  if ( ssd/(n*n) > 1.0 ) {
    cerr << "Bad matrix inverse: ssd = " << ssd << endl;
    cerr << Am.m << endl;
    cerr << product << endl;
    
    if ( retry_gaussj ) {
      cerr << "trying gaussj..." << endl;
      pointy_matrix<double> Ag(n, n);
      matrix_from_arrays(Ag.m, A, n, n);
      gaussj0(Ag.pp, n, NULL, 0, cerr);
      verify_inverse(A, Ag.m, false);
    }
  }
} 

QUBFAST_API void invm(matrix<double>& Q_in, matrix<double>& Q_out)
{
  assert( Q_in.size1() == Q_in.size2() );
  int n = (int) Q_in.size1();
  fq::matrix<double> Q(n, n);//, Qi(n, n);
  matrix_to_arrays(Q_in, (double**)Q, n, n);
  //sv_invm((double**)Q, n, (double**)Qi);
  std::ostringstream xerr;
  gaussj0(Q, n, NULL, 0, xerr);
  std::string err = xerr.str();
  if ( ((int) Q_out.size1()) != n || ((int) Q_out.size2()) != n )
    Q_out.resize(n, n);
  matrix_from_arrays(Q_out, (double**)Q, n, n);
  if ( err.size() ) {
    cerr << "in invm: " << err << endl;
    cerr << Q_in << endl << endl;
    cerr << Q_out << endl << endl;
  }
  //verify_inverse((double**)Q, Q_out);
}
  
int print_count = 0;

bool invm_chk(matrix<double>& Q_in, matrix<double>& Q_out)
{
  assert( Q_in.size1() == Q_in.size2() );
  int n = (int) Q_in.size1();
  fq::matrix<double> Q(n, n);//, Qi(n, n);
  matrix_to_arrays(Q_in, (double**)Q, n, n);
  //sv_invm((double**)Q, n, (double**)Qi);
  std::ostringstream xerr;
  gaussj0(Q, n, NULL, 0, xerr);
  std::string err = xerr.str();
  if ( ((int) Q_out.size1()) != n || ((int) Q_out.size2()) != n )
    Q_out.resize(n, n);
  matrix_from_arrays(Q_out, (double**)Q, n, n);
  if ( err.size() ) {
    cerr << "in invm_chk: " << err << endl;
    cerr << Q_in << endl << endl;
    cerr << Q_out << endl << endl;
    return false;
  }
  return true;
  //verify_inverse((double**)Q, Q_out);
}
  
QUBFAST_API bool eigen_vals_vecs(matrix<double>& Q_in, double *rvalues, double *ivalues, matrix<double>& vectors_out, matrix<double>& vectors_inv, std::ostream& output)
{
  assert( Q_in.size1() == Q_in.size2() );
  int n = (int) Q_in.size1();
  if ( ((int) vectors_out.size1()) != n || ((int) vectors_out.size2()) != n )
    vectors_out.resize(n, n);
  if ( ((int) vectors_inv.size1()) != n || ((int) vectors_inv.size2()) != n )
    vectors_inv.resize(n, n);

  gsl_set_error_handler_off();

  gsl_matrix *Q = gsl_matrix_alloc(n,n);
  for (int i=0; i<n; ++i)
    for (int j=0; j<n; ++j)
      gsl_matrix_set(Q,i,j, Q_in(i, j));
  gsl_matrix_complex *V = gsl_matrix_complex_alloc(n,n);
  gsl_vector_complex *L = gsl_vector_complex_alloc(n);
  gsl_eigen_nonsymmv_workspace *ws = gsl_eigen_nonsymmv_alloc(n);
  gsl_eigen_nonsymmv(Q, L, V, ws);
  for (int i=0; i<n; ++i) {
    rvalues[i] = GSL_REAL(gsl_vector_complex_get(L, i));
    ivalues[i] = 0.0; // GSL_IMAG(gsl_vector_complex_get(L, i));
    for (int j=0; j<n; ++j)
      vectors_out(i, j) = GSL_REAL(gsl_matrix_complex_get(V,i,j));
  }
  gsl_matrix_free(Q);
  gsl_matrix_complex_free(V);
  gsl_vector_complex_free(L);
  gsl_eigen_nonsymmv_free(ws);

  invm(vectors_out, vectors_inv);
  return TRUE;

  /*
  fq::matrix<double> Q(n, n);
  matrix_to_arrays(Q_in, (double**)Q, n, n);
  fq::vector<double> r_1base(n+1), i_1base(n+1);
  fq::matrix<double> V(n, n);
  bool result = eigen((double**)Q, n, (double*)r_1base, (double*)i_1base, V, output);
  matrix_from_arrays(vectors_out, (double**)V, n, n);
  invm(vectors_out, vectors_inv);
  memcpy(rvalues, r_1base+1, n*sizeof(double));
  memcpy(ivalues, i_1base+1, n*sizeof(double));
  return result;
  */
}

QUBFAST_API void expm_eig(double *rvalues, matrix<double>& vectors, matrix<double>& vectors_inv, double t,
	      matrix<double>& tmp_nxn_diag, matrix<double>& tmp_nxn, matrix<double>& Q_out)
{
  int n = (int) vectors.size1();
  for (int i=0; i<n; ++i) {
    tmp_nxn_diag(i,i) = exp(rvalues[i] * t);
  }
  
  noalias(tmp_nxn) = prod(vectors, tmp_nxn_diag);
  noalias(Q_out) = prod(tmp_nxn, vectors_inv);
}  

QUBFAST_API void expm(matrix<double>& Q_in, double t, matrix<double>& Q_out)
{
  int n = (int) Q_in.size1();
  fq::vector<double> rvalues(n), ivalues(n);
  matrix<double> vectors;
  matrix<double> vectors_inv;
  matrix<double> tmp_nn(n, n);
  matrix<double> tmp_diag(n, n);
  for (int i=0; i<n; ++i)
    for (int j=0; j<n; ++j)
      tmp_diag(i,j) = 0.0;
  
  eigen_vals_vecs(Q_in, rvalues, ivalues, vectors, vectors_inv, cerr);
  expm_eig(rvalues, vectors, vectors_inv, t, tmp_diag, tmp_nn, Q_out);
}

#define TINY 1e-8
  
/* SVD like linpack:
#   * V is transposed
# but all matrices have full dimension
# Then we repair and check it:
#   * tiny numbers are replaced with 0
#   * U and V need orthonormal columns
*/

bool sv_decomp_inner(double** uu, int Nr, int Nc, matrix<double>& U, matrix<double>& W, matrix<double>& Vi);

QUBFAST_API bool sv_decomp(matrix<double>& A, matrix<double>& U, matrix<double>& W, matrix<double>& Vi)
{
  int Nr = (int) A.size1();
  int Nc = (int) A.size2();
  fq::matrix<double> uu(Nr+1, Nc+1);
  matrix_to_arrays(A, (double**) uu, Nr, Nc);
  return sv_decomp_inner(uu, Nr, Nc, U, W, Vi);
}

QUBFAST_API bool sv_decomp(double** A, int Nr, int Nc, matrix<double>& U, matrix<double>& W, matrix<double>& Vi)
{
  fq::matrix<double> uu(Nr+1, Nc+1);
  for (int i=0; i<Nr; ++i)
    for (int j=0; j<Nc; ++j)
      uu[i][j] = A[i][j];
  return sv_decomp_inner(uu, Nr, Nc, U, W, Vi);
}

bool sv_decomp_inner(double** uu, int Nr, int Nc, matrix<double>& U, matrix<double>& W, matrix<double>& Vi)
{  
  // automatic storage
  fq::vector<double> _ww(Nc+1);
  fq::matrix<double> _vv(Nc+1, Nc+1);
  // as pointers
  double *ww = _ww, **vv = _vv;
  // shifted to 1-based
  for (int i=0; i<Nr; ++i)
    uu[i] -= 1;
  for (int i=0; i<Nc; ++i)
    vv[i] -= 1;
  uu -= 1; ww -= 1; vv -= 1;
  // SVD
  svdecomp(uu, Nr, Nc, ww, vv, cerr);
  // shifted to 0-based
  uu += 1; ww += 1; vv += 1;
  for (int i=0; i<Nr; ++i)
    uu[i] += 1;
  for (int i=0; i<Nc; ++i)
    vv[i] += 1;
  // outputs
  if ( ((int) U.size1()) != Nr || ((int) U.size2()) != Nc )
    U.resize(Nr, Nc);
  for (int i=0; i<Nr; ++i)
    for (int j=0; j<Nc; ++j)
      U(i, j) = (fabs(uu[i][j]) > TINY) ? uu[i][j] : 0.0;
  if ( ((int) W.size1()) != Nc || ((int) W.size2()) != Nc )
    W.resize(Nc, Nc);
  for (int i=0; i<Nc; ++i)
    for (int j=0; j<Nc; ++j)
      W(i, j) = ((i == j) && (fabs(ww[i]) > TINY)) ? ww[i] : 0.0;
  if ( ((int) Vi.size1()) != Nc || ((int) Vi.size2()) != Nc )
    Vi.resize(Nc, Nc);
  for (int i=0; i<Nc; ++i)
    for (int j=0; j<Nc; ++j)
      Vi(i, j) = (fabs(vv[i][j]) > TINY) ? vv[i][j] : 0.0;

  // checks a la MIL
  double s;
  for (int i=0; i<Nr; i++) {
    for (int j=Nr; j<Nc; j++) {
      if(fabs(uu[i][j])>TINY) {
        return false;
      }
    }
  }
  int Nlo = min(Nr, Nc);
  for (int i=0; i<Nlo; i++) { // make sure that u's columns are orthogonal unit vectors
    for (int j=0; j<Nlo; j++) { // changed from nx to mc Chris 6/2/2
      s=0.0;
      for (int k=0; k<Nr; k++) 
        s=s+uu[k][i]*uu[k][j];
      if( (i!=j&&fabs(s)>TINY) || (i==j&&fabs(s-1.0)>TINY) ) {
        return false;
      }
    }
  }
  for (int i=0; i<Nc; i++) { // make sure that v's columns are orthogonal unit vectors
    for (int j=0; j<Nc; j++) {
      s=0.0;
      for (int k=0; k<Nc; k++) 
        s=s+vv[k][i]*vv[k][j];
      if( (i!=j && fabs(s)>TINY) || (i==j && fabs(s-1.0)>TINY) ){
        return false;
      }
    }
  }
  for (int i=0; i<Nc; i++) {
    for (int j=0; j<Nc; j++) {
      s=0.0;
      for (int k=0; k<Nc; k++) 
        s=s+vv[i][k]*vv[j][k];
      if( (i!=j&&fabs(s)>TINY) || (i==j&&fabs(s-1.0)>TINY) ){
        return false;
      }
    }
  }  
  
  return true;
}

QUBFAST_API bool sv_invm(double** A, int n, double** Ai)
{
	matrix<double> U(n, n), W(n, n), Vi(n, n);
	bool result = sv_decomp(A, n, n, U, W, Vi);
	for (int i=0; i<n; ++i) {
		if ( W(i,i) )
			W(i,i) = 1.0 / W(i,i);
	}
	matrix<double> out(n, n);
	noalias(out) = prod(Vi, matrix<double>(prod(W, trans(U))));
	matrix_to_arrays(out, Ai, n, n);
	return result;
}