/* linalg/condest.c

 * 

 * Copyright (C) 2016 Patrick Alken

 * 

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

 * 

 * This program 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

 * along with this program; if not, write to the Free Software

 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

 */

#include <config.h>

#include <stdlib.h>

#include <string.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_vector.h>

#include <gsl/gsl_matrix.h>

#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

/*

 * This module contains routines for estimating the condition number

 * of matrices in the 1-norm. The algorithm is based on the paper,

 *

 * [1] N. J. Higham, "FORTRAN codes for estimating the one-norm of

 * a real or complex matrix, with applications to condition estimation",

 * ACM Trans. Math. Soft., vol. 14, no. 4, pp. 381-396, December 1988.

 */

static double condest_tri_norm1(CBLAS_UPLO_t Uplo, const gsl_matrix * A);

static int condest_tri_rcond(CBLAS_UPLO_t Uplo, const gsl_matrix * A,

                             double * rcond, gsl_vector * work);

static int condest_same_sign(const gsl_vector * x, const gsl_vector * y);

static int condest_invtriu(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);

static int condest_invtril(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);

/*

gsl_linalg_tri_upper_rcond()

  Estimate reciprocal condition number of upper triangular matrix

Inputs: A     - upper triangular matrix, N-by-N

        rcond - (output) reciprocal condition number estimate

        work  - workspace, length 3*N

Return: success/error

*/

int

gsl_linalg_tri_upper_rcond(const gsl_matrix * A, double * rcond, gsl_vector * work)

{

  int status = condest_tri_rcond(CblasUpper, A, rcond, work);

  return status;

}

/*

gsl_linalg_tri_lower_rcond()

  Estimate reciprocal condition number of lower triangular matrix

Inputs: A     - lower triangular matrix, N-by-N

        rcond - (output) reciprocal condition number estimate

        work  - workspace, length 3*N

Return: success/error

*/

int

gsl_linalg_tri_lower_rcond(const gsl_matrix * A, double * rcond, gsl_vector * work)

{

  int status = condest_tri_rcond(CblasLower, A, rcond, work);

  return status;

}

/*

gsl_linalg_invnorm1()

  Estimate the 1-norm of ||A^{-1}||, where A is a square

N-by-N matrix

Inputs: N        - size of matrix

        Ainvx    - pointer to function which calculates:

                   x := A^{-1} x or x := A^{-t} x

        params   - parameters to pass to Ainvx

        Ainvnorm - (output) estimate of ||A^{-1}||_1

        work     - workspace, length 3*N

*/

int

gsl_linalg_invnorm1(const size_t N,

                    int (* Ainvx)(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params),

                    void * params, double * Ainvnorm, gsl_vector * work)

{

  if (work->size != 3 * N)

    {

      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);

    }

  else

    {

      const size_t maxit = 5;

      gsl_vector_view x = gsl_vector_subvector(work, 0, N);

      gsl_vector_view v = gsl_vector_subvector(work, N, N);

      gsl_vector_view xi = gsl_vector_subvector(work, 2*N, N);

      double gamma, gamma_old, temp;

      size_t i, k;

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

        gsl_vector_set(&x.vector, i, 1.0 / (double) N);

      /* compute v = A^{-1} x */

      gsl_vector_memcpy(&v.vector, &x.vector);

      (*Ainvx)(CblasNoTrans, &v.vector, params);

      /* gamma = ||v||_1 */

      gamma = gsl_blas_dasum(&v.vector);

      /* xi = sign(v) */

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

        {

          double vi = gsl_vector_get(&v.vector, i);

          gsl_vector_set(&xi.vector, i, GSL_SIGN(vi));

        }

      /* x = A^{-t} xi */

      gsl_vector_memcpy(&x.vector, &xi.vector);

      (*Ainvx)(CblasTrans, &x.vector, params);

      for (k = 0; k < maxit; ++k)

        {

          size_t j = (size_t) gsl_blas_idamax(&x.vector);

          /* v := A^{-1} e_j */

          gsl_vector_set_zero(&v.vector);

          gsl_vector_set(&v.vector, j, 1.0);

          (*Ainvx)(CblasNoTrans, &v.vector, params);

          gamma_old = gamma;

          gamma = gsl_blas_dasum(&v.vector);

          /* check for repeated sign vector (algorithm has converged) */

          if (condest_same_sign(&v.vector, &xi.vector) || (gamma < gamma_old))

            break;

          /* xi = sign(v) */

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

            {

              double vi = gsl_vector_get(&v.vector, i);

              gsl_vector_set(&xi.vector, i, GSL_SIGN(vi));

            }

          /* x = A^{-t} sign(v) */

          gsl_vector_memcpy(&x.vector, &xi.vector);

          (*Ainvx)(CblasTrans, &x.vector, params);

        }

      temp = 1.0; /* (-1)^i */

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

        {

          double term = 1.0 + (double) i / (N - 1.0);

          gsl_vector_set(&x.vector, i, temp * term);

          temp = -temp;

        }

      /* x := A^{-1} x */

      (*Ainvx)(CblasNoTrans, &x.vector, params);

      temp = 2.0 * gsl_blas_dasum(&x.vector) / (3.0 * N);

      if (temp > gamma)

        {

          gsl_vector_memcpy(&v.vector, &x.vector);

          gamma = temp;

        }

      *Ainvnorm = gamma;

      return GSL_SUCCESS;

    }

}

static int

condest_tri_rcond(CBLAS_UPLO_t Uplo, const gsl_matrix * A, double * rcond, gsl_vector * work)

{

  const size_t M = A->size1;

  const size_t N = A->size2;

  if (M != N)

    {

      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);

    }

  else if (work->size != 3 * N)

    {

      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);

    }

  else

    {

      int status;

      double Anorm = condest_tri_norm1(Uplo, A); /* ||A||_1 */

      double Ainvnorm;                           /* ||A^{-1}||_1 */

      *rcond = 0.0;

      /* don't continue if matrix is singular */

      if (Anorm == 0.0)

        return GSL_SUCCESS;

      /* estimate ||A^{-1}||_1 */

      if (Uplo == CblasUpper)

        status = gsl_linalg_invnorm1(N, condest_invtriu, (void *) A, &Ainvnorm, work);

      else

        status = gsl_linalg_invnorm1(N, condest_invtril, (void *) A, &Ainvnorm, work);

      if (status)

        return status;

      if (Ainvnorm != 0.0)

        *rcond = (1.0 / Anorm) / Ainvnorm;

      return GSL_SUCCESS;

    }

}

/* calculate 1 norm of triangular matrix */

static double

condest_tri_norm1(CBLAS_UPLO_t Uplo, const gsl_matrix * A)

{

  const size_t N = A->size2;

  double max = 0.0;

  size_t i, j;

  if (Uplo == CblasUpper)

    {

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

        {

          double sum = 0.0;

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

            {

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

              sum += fabs(Aij);

            }

          max = GSL_MAX(max, sum);

        }

    }

  else

    {

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

        {

          double sum = 0.0;

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

            {

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

              sum += fabs(Aij);

            }

          max = GSL_MAX(max, sum);

        }

    }

  return max;

}

/* return 1 if sign(x) = sign(y), 0 otherwise */

static int

condest_same_sign(const gsl_vector * x, const gsl_vector * y)

{

  const size_t n = x->size;

  size_t i;

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

    {

      double xi = gsl_vector_get(x, i);

      double yi = gsl_vector_get(y, i);

      if (GSL_SIGN(xi) != GSL_SIGN(yi))

        return 0;

    }

  return 1;

}

/* x := A^{-1} x, A upper triangular */

static int

condest_invtriu(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)

{

  gsl_matrix * A = (gsl_matrix *) params;

  return gsl_blas_dtrsv(CblasUpper, TransA, CblasNonUnit, A, x);

}

/* x := A^{-1} x, A lower triangular */

static int

condest_invtril(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)

{

  gsl_matrix * A = (gsl_matrix *) params;

  return gsl_blas_dtrsv(CblasLower, TransA, CblasNonUnit, A, x);

}