/* linalg/hh.c

 * 

 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, Brian Gough

 * 

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

 */

/* Author:  G. Jungman */

#include <config.h>

#include <stdlib.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_vector.h>

#include <gsl/gsl_matrix.h>

#include <gsl/gsl_linalg.h>

#define REAL double

/* [Engeln-Mullges + Uhlig, Alg. 4.42]

 */

int

gsl_linalg_HH_solve (gsl_matrix * A, const gsl_vector * b, gsl_vector * x)

{

  if (A->size1 > A->size2)

    {

      /* System is underdetermined. */

      GSL_ERROR ("System is underdetermined", GSL_EINVAL);

    }

  else if (A->size2 != x->size)

    {

      GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);

    }

  else

    {

      int status ;

      gsl_vector_memcpy (x, b);

      status = gsl_linalg_HH_svx (A, x);

      return status ;

    }

}

int

gsl_linalg_HH_svx (gsl_matrix * A, gsl_vector * x)

{

  if (A->size1 > A->size2)

    {

      /* System is underdetermined. */

      GSL_ERROR ("System is underdetermined", GSL_EINVAL);

    }

  else if (A->size2 != x->size)

    {

      GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);

    }

  else

    {

      const size_t N = A->size1;

      const size_t M = A->size2;

      size_t i, j, k;

      REAL *d = (REAL *) malloc (N * sizeof (REAL));

      if (d == 0)

        {

          GSL_ERROR ("could not allocate memory for workspace", GSL_ENOMEM);

        }

      /* Perform Householder transformation. */

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

        {

          const REAL aii = gsl_matrix_get (A, i, i);

          REAL alpha;

          REAL f;

          REAL ak;

          REAL max_norm = 0.0;

          REAL r = 0.0;

          for (k = i; k < M; k++)

            {

              REAL aki = gsl_matrix_get (A, k, i);

              r += aki * aki;

            }

          if (r == 0.0)

            {

              /* Rank of matrix is less than size1. */

              free (d);

              GSL_ERROR ("matrix is rank deficient", GSL_ESING);

            }

          alpha = sqrt (r) * GSL_SIGN (aii);

          ak = 1.0 / (r + alpha * aii);

          gsl_matrix_set (A, i, i, aii + alpha);

          d[i] = -alpha;

          for (k = i + 1; k < N; k++)

            {

              REAL norm = 0.0;

              f = 0.0;

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

                {

                  REAL ajk = gsl_matrix_get (A, j, k);

                  REAL aji = gsl_matrix_get (A, j, i);

                  norm += ajk * ajk;

                  f += ajk * aji;

                }

              max_norm = GSL_MAX (max_norm, norm);

              f *= ak;

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

                {

                  REAL ajk = gsl_matrix_get (A, j, k);

                  REAL aji = gsl_matrix_get (A, j, i);

                  gsl_matrix_set (A, j, k, ajk - f * aji);

                }

            }

          if (fabs (alpha) < 2.0 * GSL_DBL_EPSILON * sqrt (max_norm))

            {

              /* Apparent singularity. */

              free (d);

              GSL_ERROR("apparent singularity detected", GSL_ESING);

            }

          /* Perform update of RHS. */

          f = 0.0;

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

            {

              f += gsl_vector_get (x, j) * gsl_matrix_get (A, j, i);

            }

          f *= ak;

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

            {

              REAL xj = gsl_vector_get (x, j);

              REAL aji = gsl_matrix_get (A, j, i);

              gsl_vector_set (x, j, xj - f * aji);

            }

        }

      /* Perform back-substitution. */

      for (i = N; i-- > 0;)

        {

          REAL xi = gsl_vector_get (x, i);

          REAL sum = 0.0;

          for (k = i + 1; k < N; k++)

            {

              sum += gsl_matrix_get (A, i, k) * gsl_vector_get (x, k);

            }

          gsl_vector_set (x, i, (xi - sum) / d[i]);

        }

      free (d);

      return GSL_SUCCESS;

    }

}