#ifndef lint
static char *RCSid() { return RCSid("$Id: matrix.c,v 1.10 2004/07/01 17:10:06 broeker Exp $"); }
#endif

/*  NOTICE: Change of Copyright Status
 *
 *  The author of this module, Carsten Grammes, has expressed in
 *  personal email that he has no more interest in this code, and
 *  doesn't claim any copyright. He has agreed to put this module
 *  into the public domain.
 *
 *  Lars Hecking  15-02-1999
 */

/*
 *      Matrix algebra, part of
 *
 *      Nonlinear least squares fit according to the
 *      Marquardt-Levenberg-algorithm
 *
 *      added as Patch to Gnuplot (v3.2 and higher)
 *      by Carsten Grammes
 *      Experimental Physics, University of Saarbruecken, Germany
 *
 *      Previous copyright of this module:   Carsten Grammes, 1993
 *
 */

#include "matrix.h"

#include "alloc.h"
#include "fit.h"
#include "util.h"

/*****************************************************************/

#define Swap(a,b)   {double temp = (a); (a) = (b); (b) = temp;}
/* HBB 20010424: unused: */
/* #define WINZIG	      1e-30 */


/*****************************************************************
    internal prototypes
*****************************************************************/

static GP_INLINE int fsign __PROTO((double x));

/*****************************************************************
    first straightforward vector and matrix allocation functions
*****************************************************************/

/* allocates a double vector with n elements */
double *
vec(int n)
{
    double *dp;

    if (n < 1)
	return NULL;
    dp = gp_alloc(n * sizeof(double), "vec");
    return dp;
}


/* allocates a double matrix */
double **
matr(int rows, int cols)
{
    int i;
    double **m;

    if (rows < 1 || cols < 1)
	return NULL;
    m = gp_alloc(rows * sizeof(m[0]), "matrix row pointers");
    m[0] = gp_alloc(rows * cols * sizeof(m[0][0]), "matrix elements");
    for (i = 1; i < rows; i++)
	m[i] = m[i - 1] + cols;
    return m;
}


void
free_matr(double **m)
{
    free(m[0]);
    free(m);
}


double *
redim_vec(double **v, int n)
{
    if (n < 1)
	*v = NULL;
    else
	*v = gp_realloc(*v, n * sizeof((*v)[0]), "vec");
    return *v;
}

/* HBB: TODO: is there a better value for 'epsilon'? how to specify
 * 'inline'?  is 'fsign' really not available elsewhere? use
 * row-oriented version (p. 309) instead?
 */

static GP_INLINE int
fsign(double x)
{
    return (x > 0 ? 1 : (x < 0) ? -1 : 0);
}

/*****************************************************************

     Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
     (QR-decomposition). Direct implementation of the algorithm
     presented in H.R.Schwarz: Numerische Mathematik, 'equation'
     number (7.33)

     If 'd == NULL', d is not accesed: the routine just computes the QR
     decomposition of C and exits.

     If 'want_r == 0', r is not rotated back (\hat{r} is returned
     instead).

*****************************************************************/

void
Givens(
    double **C,
    double *d,
    double *x,
    double *r,
    int N,
    int n,
    int want_r)
{
    int i, j, k;
    double w, gamma, sigma, rho, temp;
    double epsilon = DBL_EPSILON;	/* FIXME (?) */

/*
 * First, construct QR decomposition of C, by 'rotating away'
 * all elements of C below the diagonal. The rotations are
 * stored in place as Givens coefficients rho.
 * Vector d is also rotated in this same turn, if it exists
 */
    for (j = 0; j < n; j++) {
	for (i = j + 1; i < N; i++) {
	    if (C[i][j]) {
		if (fabs(C[j][j]) < epsilon * fabs(C[i][j])) {
		    /* find the rotation parameters */
		    w = -C[i][j];
		    gamma = 0;
		    sigma = 1;
		    rho = 1;
		} else {
		    w = fsign(C[j][j]) * sqrt(C[j][j] * C[j][j] + C[i][j] * C[i][j]);
		    if (w == 0)
			Eex3("w = 0 in Givens();  Cjj = %g,  Cij = %g", C[j][j], C[i][j]);
		    gamma = C[j][j] / w;
		    sigma = -C[i][j] / w;
		    rho = (fabs(sigma) < gamma) ? sigma : fsign(sigma) / gamma;
		}
		C[j][j] = w;
		C[i][j] = rho;	/* store rho in place, for later use */
		for (k = j + 1; k < n; k++) {
		    /* rotation on index pair (i,j) */
		    temp = gamma * C[j][k] - sigma * C[i][k];
		    C[i][k] = sigma * C[j][k] + gamma * C[i][k];
		    C[j][k] = temp;

		}
		if (d) {	/* if no d vector given, don't use it */
		    temp = gamma * d[j] - sigma * d[i];		/* rotate d */
		    d[i] = sigma * d[j] + gamma * d[i];
		    d[j] = temp;
		}
	    }
	}
    }

    if (!d)			/* stop here if no d was specified */
	return;

    /* solve R*x+d = 0, by backsubstitution */
    for (i = n - 1; i >= 0; i--) {
	double s = d[i];

	r[i] = 0;		/* ... and also set r[i] = 0 for i