/*******************************************************************************
*
*       This file is part of the General Hidden Markov Model Library,
*       GHMM version __VERSION__, see http://ghmm.org
*
*       Filename: ghmm/ghmm/matrixop.c
*       Authors:  Christoph Hafemeister
*
*       Copyright (C) 2007-2008 Alexander Schliep
*	Copyright (C) 2007-2008 Max-Planck-Institut fuer Molekulare Genetik,
*                               Berlin
*
*       Contact: schliep@ghmm.org
*
*       This library is free software; you can redistribute it and/or
*       modify it under the terms of the GNU Library General Public
*       License as published by the Free Software Foundation; either
*       version 2 of the License, or (at your option) any later version.
*
*       This library 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
*       Library General Public License for more details.
*
*       You should have received a copy of the GNU Library General Public
*       License along with this library; if not, write to the Free
*       Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*******************************************************************************/

#ifdef HAVE_CONFIG_H
#  include "../config.h"
#endif

#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <string.h>

#include "matrixop.h"

#ifdef DO_WITH_GSL
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#endif

#ifdef DO_WITH_ATLAS
#include <clapack.h>
#endif

/*============================================================================*/
int ighmm_invert_det(double *sigmainv, double *det, int length, double *cov)
{
#define CUR_PROC "invert_det"
#ifdef DO_WITH_GSL
  int i, j, s;
  gsl_matrix *tmp;
  gsl_matrix *inv;
  tmp = gsl_matrix_alloc(length, length);
  inv = gsl_matrix_alloc(length, length);
  gsl_permutation *permutation = gsl_permutation_calloc(length);

  for (i=0; i<length; ++i) {
    for (j=0; j<length; ++j) {
#ifdef DO_WITH_GSL_DIAGONAL_HACK
        if (i == j){
          gsl_matrix_set(tmp, i, j, cov[i*length+j]);
        }else{
          gsl_matrix_set(tmp, i, j, 0.0);
        }
#else
        gsl_matrix_set(tmp, i, j, cov[i*length+j]);
#endif
    }
  }

  gsl_linalg_LU_decomp(tmp, permutation, &s);
  gsl_linalg_LU_invert(tmp, permutation, inv);
  *det = gsl_linalg_LU_det(tmp, s);
  gsl_matrix_free(tmp);
  gsl_permutation_free(permutation);

  for (i=0; i<length; ++i) {
    for (j=0; j<length; ++j) {
       sigmainv[i*length+j] = gsl_matrix_get(inv, i, j);
    }
  }

  gsl_matrix_free(inv);
#elif defined HAVE_CLAPACK_DGETRF && HAVE_CLAPACK_DGETRI
  char sign;
  int info, i;
  int *ipiv;
  double det_tmp;
  
  ipiv = malloc(length * sizeof *ipiv);
  
  /* copy cov. matrix entries to result matrix, the rest is done in-place */
  memcpy(sigmainv, cov, length * length * sizeof *cov);
  
  /* perform in-place LU factorization of covariance matrix */
  info = clapack_dgetrf(CblasRowMajor, length, length, sigmainv, length, ipiv);
  
  /* determinant */
  sign = 1;
  for( i=0; i<length; ++i)
    if( ipiv[i]!=i )
      sign *= -1;        
  det_tmp = sigmainv[0];
  for( i=length+1; i<(length*length); i+=length+1 )
    det_tmp *= sigmainv[i];
  *det = det_tmp * sign;
  
  /* use the LU factorization to get inverse */
  info = clapack_dgetri(CblasRowMajor, length, sigmainv, length, ipiv);
  
  free(ipiv);
#else
  *det = ighmm_determinant(cov, length);
  ighmm_inverse(cov, length, *det, sigmainv);
#endif

  return 0;
#undef CUR_PROC
}

/*============================================================================*/
/* calculate determinant of a square matrix */
double ighmm_determinant(double *cov, int n)
{
  int j1, i, j, jm;
  double * m;
  double det;

  if (n == 1)
    return cov[0];
  if (n == 2)
    return cov[0] * cov[3] - cov[1] * cov[2];
  /* matrix dimension is bigger than 2 - we have to do some work */
  det = 0;
  for (j1=0;j1<n;j1++) {
    m = malloc((n-1)*(n-1)*sizeof(double));
    for (i=1;i<n;i++) {
      jm = 0;
      for (j=0;j<n;j++) {
        if (j == j1)
          continue;
        m[(i-1)*(n-1)+jm] = cov[i*n+j];
        jm++;
      }
    }
    det += pow(-1.0,1.0+j1+1.0) * cov[j1] * ighmm_determinant(m,n-1);
    free(m);
  }
  return det;
}

/*============================================================================*/
/*
  The inverse of a square matrix A with a non zero determinant is the adjoint
  matrix divided by the determinant.
  The adjoint matrix is the transpose of the cofactor matrix.
  The cofactor matrix is the matrix of determinants of the minors Aij
  multiplied by -1^(i+j).
  The i,j'th minor of A is the matrix A without the i'th column or
  the j'th row.
*/
int ighmm_inverse(double *cov, int n, double det, double *inv)
{
  int i, j, ic, jc, actrow, actcol;
  double * m;

  if (n == 1) {
    inv[0] = 1/cov[0];
    return 0;
  }
  if (n == 2) {
    inv[0] = cov[3] / (cov[0]*cov[3] - cov[1]*cov[2]);
    inv[1] = - cov[1] / (cov[0]*cov[3] - cov[1]*cov[2]);
    inv[2] = - cov[2] / (cov[0]*cov[3] - cov[1]*cov[2]);
    inv[3] = cov[0] / (cov[0]*cov[3] - cov[1]*cov[2]);
    return 0;
  }
  for (i=0;i<n;i++) {
    for (j=0;j<n;j++) {
      /* calculate minor i,j */
      m = malloc((n-1)*(n-1)*sizeof(double));
      actrow = 0;
      for (ic=0;ic<n;ic++) {
        if (ic == i)
          continue;
        actcol = 0;
        for (jc=0;jc<n;jc++) {
          if (jc == j)
            continue;
          m[actrow*(n-1)+actcol] = cov[ic*n+jc];
          actcol++;
        }
        actrow++;
      }
      /* cofactor i,j is determinant of m times -1^(i+j) */
      inv[j*n+i] = pow(-1.0,i+j+2.0) * ighmm_determinant(m,n-1) / det;
      free(m);
    }
  }
  return 0;
}

/*============================================================================*/
int ighmm_cholesky_decomposition (double *sigmacd, int dim, double *cov) {
#ifdef DO_WITH_GSL
  int i, j;
  gsl_matrix *tmp = gsl_matrix_alloc(dim, dim);

  /* temp matrix */
  for (i=0;i<dim;i++) {
    for (j=0;j<dim;j++) {
      gsl_matrix_set(tmp, i, j, cov[i*dim+j]);
    }
  }
  /* cholesky decomposition of temp matrix */
  gsl_linalg_cholesky_decomp(tmp);
  /* copy to sigmacd and set upper triangular to 0 */
  for (i=0;i<dim;i++) {
    for (j=0;j<dim;j++) {
      if (j>i)
        sigmacd[i*dim+j] = 0;
      else
        sigmacd[i*dim+j] = gsl_matrix_get(tmp, i, j);
    }
  }
  gsl_matrix_free(tmp);

#elif defined HAVE_CLAPACK_DPOTRF
  int info;
  /* copy cov. matrix entries to result matrix, the rest is done in-place */
  memcpy(sigmacd, cov, dim * dim * sizeof *cov);
  info = clapack_dpotrf(CblasRowMajor, CblasUpper, dim, sigmacd, dim);
#else
  int row, j, k;
  double sum;

  /* copy cov to sigmacd */
  for (row=0; row<dim; row++) {
    for (j=0; j<dim; j++) {
      sigmacd[row*dim+j] = cov[row*dim+j];
    }
  }

  for (row=0; row<dim; row++) {
    /* First compute U[row][row] */
    sum = cov[row*dim+row];
    for (j=0; j<(row-1); j++) sum -= sigmacd[j*dim+row]*sigmacd[j*dim+row];
    if (sum > DBL_MIN) {
      sigmacd[row*dim+row] = sqrt(sum);
      /* Now find elements sigmacd[row*dim+k], k > row. */
      for (k=(row+1); k<dim; k++) {
        sum = cov[row*dim+k];
        for (j=0; j<(row-1); j++)
          sum -= sigmacd[j*dim+row]*sigmacd[j*dim+k];
        sigmacd[row*dim+k] = sum/sigmacd[row*dim+row];
      }
    }
    else {  /* blast off the entire row. */
      for (k=row; k<dim; k++) sigmacd[row*dim+k] = 0.0;
    }
  }
#endif

  return 0;
}

