/* multifit/covar.c

 * 

 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004, 2007 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.

 */

#include <config.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_errno.h>

#include <gsl/gsl_permutation.h>

#include <gsl/gsl_linalg.h>

#include <gsl/gsl_multifit_nlin.h>

/* Compute the covariance matrix

   cov = inv (J^T J)

   by QRP^T decomposition of J

*/

int

gsl_multifit_covar (const gsl_matrix * J, const double epsrel, gsl_matrix * covar)

{

  int status;

  gsl_matrix * r;

  gsl_vector * tau;

  gsl_vector * norm;

  gsl_permutation * perm;

  const size_t m = J->size1;

  const size_t n = J->size2;

  if (m < n) 

    {

      GSL_ERROR ("Jacobian be rectangular M x N with M >= N", GSL_EBADLEN);

    }

  if (covar->size1 != covar->size2 || covar->size1 != n)

    {

      GSL_ERROR ("covariance matrix must be square and match second dimension of jacobian", GSL_EBADLEN);

    }

  r = gsl_matrix_alloc (m, n);

  tau = gsl_vector_alloc (n);

  perm = gsl_permutation_alloc (n) ;

  norm = gsl_vector_alloc (n) ;

  {

    int signum = 0;

    gsl_matrix_memcpy (r, J);

    gsl_linalg_QRPT_decomp (r, tau, perm, &signum, norm);

  }

  status = gsl_multifit_covar_QRPT(r, perm, epsrel, covar);

  gsl_matrix_free (r);

  gsl_permutation_free (perm);

  gsl_vector_free (tau);

  gsl_vector_free (norm);

  return status;

}

int

gsl_multifit_covar_QRPT (gsl_matrix * r, gsl_permutation * perm,

                         const double epsrel, gsl_matrix * covar)

{

  /* Form the inverse of R in the full upper triangle of R */

  double tolr = epsrel * fabs(gsl_matrix_get(r, 0, 0));

  const size_t n = r->size2;

  size_t i, j, k;

  size_t kmax = 0;

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

    {

      double rkk = gsl_matrix_get(r, k, k);

      if (fabs(rkk) <= tolr)

        {

          break;

        }

      gsl_matrix_set(r, k, k, 1.0/rkk);

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

        {

          double t = gsl_matrix_get(r, j, k) / rkk;

          gsl_matrix_set (r, j, k, 0.0);

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

            {

              double rik = gsl_matrix_get (r, i, k);

              double rij = gsl_matrix_get (r, i, j);

              gsl_matrix_set (r, i, k, rik - t * rij);

            }

        }

      kmax = k;

    }

  /* Form the full upper triangle of the inverse of R^T R in the full

     upper triangle of R */

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

    {

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

        {

          double rjk = gsl_matrix_get (r, j, k);

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

            {

              double rij = gsl_matrix_get (r, i, j);

              double rik = gsl_matrix_get (r, i, k);

              gsl_matrix_set (r, i, j, rij + rjk * rik);

            }

        }

      {

        double t = gsl_matrix_get (r, k, k);

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

          {

            double rik = gsl_matrix_get (r, i, k);

            gsl_matrix_set (r, i, k, t * rik);

          };

      }

    }

  /* Form the full lower triangle of the covariance matrix in the

     strict lower triangle of R and in w */

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

    {

      size_t pj = gsl_permutation_get (perm, j);

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

        {

          size_t pi = gsl_permutation_get (perm, i);

          double rij;

          if (j > kmax)

            {

              gsl_matrix_set (r, i, j, 0.0);

              rij = 0.0 ;

            }

          else 

            {

              rij = gsl_matrix_get (r, i, j);

            }

          if (pi > pj)

            {

              gsl_matrix_set (r, pi, pj, rij); 

            } 

          else if (pi < pj)

            {

              gsl_matrix_set (r, pj, pi, rij);

            }

        }

      { 

        double rjj = gsl_matrix_get (r, j, j);

        gsl_matrix_set (covar, pj, pj, rjj);

      }

    }

  /* symmetrize the covariance matrix */

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

    {

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

        {

          double rji = gsl_matrix_get (r, j, i);

          gsl_matrix_set (covar, j, i, rji);

          gsl_matrix_set (covar, i, j, rji);

        }

    }

  return GSL_SUCCESS;

} /* gsl_multifit_covar_QRPT() */