/* Modified Cholesky Decomposition

 *

 * Copyright (C) 2016 Patrick Alken

 *

 * This 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, or (at your option) any

 * later version.

 *

 * This source 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.

 */

#include <config.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_errno.h>

#include <gsl/gsl_vector.h>

#include <gsl/gsl_matrix.h>

#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

#include <gsl/gsl_permutation.h>

#include <gsl/gsl_permute_vector.h>

#include "cholesky_common.c"

/*

 * This module contains routines related to the Modified Cholesky

 * Decomposition, which factors a symmetric indefinite matrix A as

 *

 * P (A + E) P^T = L D L^T

 *

 * where:

 * P: permutation matrix

 * E: small, non-negative diagonal matrix

 * L: unit lower triangular matrix

 * D: strictly positive diagonal matrix

 *

 * These routines follow these works closely:

 *

 * [1] P. E. Gill, W. Murray, M. H. Wright, Practical Optimization,

 *     Academic Press, 1981.

 *

 * [2] Dennis and Schnabel, Numerical Methods for Unconstrained Optimization

 *     and Nonlinear Equations, SIAM, 1996

 */

static size_t mcholesky_maxabs(const gsl_vector * v, double *maxabs);

/*

gsl_linalg_mcholesky_decomp()

  Perform Pivoted Modified Cholesky LDLT decomposition of a symmetric positive

indefinite matrix:

P (A + E) P^T = L D L^T

Inputs: A - (input) symmetric, positive indefinite matrix,

                    stored in lower triangle

            (output) lower triangle contains L; diagonal contains D

        p - (output) permutation matrix P

        E - (output) perturbation matrix E

Return: success/error

Notes:

1) Based on algorithm 4.2.2 (Outer Product LDLT with Pivoting) of

Golub and Van Loan, Matrix Computations (4th ed), with modifications

described in [1] and [2]

2) E can be set to NULL if not required

*/

int

gsl_linalg_mcholesky_decomp (gsl_matrix * A, gsl_permutation * p,

                             gsl_vector * E)

{

  const size_t N = A->size1;

  if (N != A->size2)

    {

      GSL_ERROR("LDLT decomposition requires square matrix", GSL_ENOTSQR);

    }

  else if (p->size != N)

    {

      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);

    }

  else

    {

      const double delta = GSL_DBL_EPSILON;

      double beta;

      double gamma = 0.0;

      double xi = 0.0;

      gsl_vector_view diag = gsl_matrix_diagonal(A);

      size_t i, j;

      /* save a copy of A in upper triangle (for later rcond calculation) */

      gsl_matrix_transpose_tricpy('L', 0, A, A);

      gsl_permutation_init(p);

      /* compute:

       * gamma = max | A_{ii} |

       * xi = max_{i \ne j} | A_{ij} |

       */

      for (i = 0; i < N; ++i)

        {

          double aii = gsl_matrix_get(A, i, i);

          gamma = GSL_MAX(gamma, fabs(aii));

          for (j = 0; j < i; ++j)

            {

              double aij = gsl_matrix_get(A, i, j);

              xi = GSL_MAX(xi, fabs(aij));

            }

        }

      /* compute:

       * beta = sqrt[ max { gamma, xi/nu, eps } ]

       * with: nu = max{ sqrt(N^2 - 1), 1 }

       */

      if (N == 1)

        {

          beta = GSL_MAX(GSL_MAX(gamma, xi), GSL_DBL_EPSILON);

        }

      else

        {

          double nu = sqrt(N*N - 1.0);

          beta = GSL_MAX(GSL_MAX(gamma, xi / nu), GSL_DBL_EPSILON);

        }

      beta = sqrt(beta);

      for (j = 0; j < N; ++j)

        {

          double ajj, thetaj, u, alpha, alphainv;

          gsl_vector_view w;

          size_t q;

          /* compute q = max_idx { A_jj, ..., A_nn } */

          w = gsl_vector_subvector(&diag.vector, j, N - j);

          q = mcholesky_maxabs(&w.vector, NULL) + j;

          gsl_permutation_swap(p, q, j);

          cholesky_swap_rowcol(A, q, j);

          /* theta_j = max_{j+1 <= i <= n} |A_{ij}| */

          if (j < N - 1)

            {

              w = gsl_matrix_subcolumn(A, j, j + 1, N - j - 1);

              mcholesky_maxabs(&w.vector, &thetaj;;

            }

          else

            {

              thetaj = 0.0;

            }

          u = thetaj / beta;

          /* compute alpha = d_j */

          ajj = gsl_matrix_get(A, j, j);

          alpha = GSL_MAX(GSL_MAX(delta, fabs(ajj)), u * u);

          alphainv = 1.0 / alpha;

          if (j < N - 1)

            {

              /* v = A(j+1:n, j) */

              gsl_vector_view v = gsl_matrix_subcolumn(A, j, j + 1, N - j - 1);

              /* m = A(j+1:n, j+1:n) */

              gsl_matrix_view m = gsl_matrix_submatrix(A, j + 1, j + 1, N - j - 1, N - j - 1);

              /* m = m - v v^T / alpha */

              gsl_blas_dsyr(CblasLower, -alphainv, &v.vector, &m.matrix);

              /* v = v / alpha */

              gsl_vector_scale(&v.vector, alphainv);

            }

          if (E)

            gsl_vector_set(E, j, alpha - ajj);

          gsl_matrix_set(A, j, j, alpha);

        }

      if (E)

        {

          /* we currently have: P A P^T + E = L D L^T, permute E

           * so that we have: P (A + E) P^T = L D L^T */

          gsl_permute_vector_inverse(p, E);

        }

      return GSL_SUCCESS;

    }

}

int

gsl_linalg_mcholesky_solve(const gsl_matrix * LDLT,

                           const gsl_permutation * p,

                           const gsl_vector * b,

                           gsl_vector * x)

{

  int status = gsl_linalg_pcholesky_solve(LDLT, p, b, x);

  return status;

}

int

gsl_linalg_mcholesky_svx(const gsl_matrix * LDLT,

                         const gsl_permutation * p,

                         gsl_vector * x)

{

  int status = gsl_linalg_pcholesky_svx(LDLT, p, x);

  return status;

}

int

gsl_linalg_mcholesky_rcond (const gsl_matrix * LDLT, const gsl_permutation * p,

                            double * rcond, gsl_vector * work)

{

  int status = gsl_linalg_pcholesky_rcond(LDLT, p, rcond, work);

  return status;

}

int

gsl_linalg_mcholesky_invert(const gsl_matrix * LDLT, const gsl_permutation * p,

                            gsl_matrix * Ainv)

{

  int status = gsl_linalg_pcholesky_invert(LDLT, p, Ainv);

  return status;

}

/*

mcholesky_maxabs()

  Compute:

val = max_i |v_i|

Inputs: v      - vector

        maxabs - (output) max abs value

Return: index corresponding to max_i |v_i|

*/

static size_t

mcholesky_maxabs(const gsl_vector * v, double *maxabs)

{

  const size_t n = v->size;

  size_t i;

  size_t idx = 0;

  double max = gsl_vector_get(v, idx);

  for (i = 1; i < n; ++i)

    {

      double vi = gsl_vector_get(v, i);

      double absvi = fabs(vi);

      if (absvi > max)

        {

          max = absvi;

          idx = i;

        }

    }

  if (maxabs)

    *maxabs = max;

  return idx;

}