#include <math.h>
#include <grass/gis.h>
#include <grass/gmath.h>


#define TINY 1.0e-20;

/*!
 * \brief LU decomposition
 *
 * \param a double **
 * \param n int
 * \param indx int *
 * \param d double *
 *
 * \return 0 on singular matrix, 1 on success
 */
int G_ludcmp(double **a, int n, int *indx, double *d)
{
    int i, imax = 0, j, k;
    double big, dum, sum, temp;
    double *vv;
    int is_singular = FALSE;

    vv = G_alloc_vector(n);
    *d = 1.0;
/* this pragma works, but doesn't really help speed things up */
/* #pragma omp parallel for private(i, j, big, temp) shared(n, a, vv, is_singular) */
    for (i = 0; i < n; i++) {
	big = 0.0;
	for (j = 0; j < n; j++)
	    if ((temp = fabs(a[i][j])) > big)
		big = temp;

	if (big == 0.0) {
	    is_singular = TRUE;
	    break;
	}

	vv[i] = 1.0 / big;
    }
    if (is_singular) {
	*d = 0.0;
	return 0;	/* Singular matrix  */
    }

    for (j = 0; j < n; j++) {
	for (i = 0; i < j; i++) {
	    sum = a[i][j];
	    for (k = 0; k < i; k++)
		sum -= a[i][k] * a[k][j];
	    a[i][j] = sum;
	}

	big = 0.0;
/* not very efficient, but this pragma helps speed things up a bit */
#pragma omp parallel for private(i, k, sum, dum) shared(j, n, a, vv, big, imax)
	for (i = j; i < n; i++) {
	    sum = a[i][j];
	    for (k = 0; k < j; k++)
		sum -= a[i][k] * a[k][j];
	    a[i][j] = sum;
	    if ((dum = vv[i] * fabs(sum)) >= big) {
		big = dum;
		imax = i;
	    }
	}
	if (j != imax) {
	    for (k = 0; k < n; k++) {
		dum = a[imax][k];
		a[imax][k] = a[j][k];
		a[j][k] = dum;
	    }
	    *d = -(*d);
	    vv[imax] = vv[j];
	}
	indx[j] = imax;
	if (a[j][j] == 0.0)
	    a[j][j] = TINY;
	if (j != n) {
	    dum = 1.0 / (a[j][j]);
	    for (i = j + 1; i < n; i++)
		a[i][j] *= dum;
	}
    }
    G_free_vector(vv);

    return 1;
}

#undef TINY


/*!
 * \brief LU backward substitution
 *
 * \param a double **
 * \param n int
 * \param indx int *
 * \param b double []
 *
 * \return void
 */
void G_lubksb(double **a, int n, int *indx, double b[])
{
    int i, ii, ip, j;
    double sum;

    ii = -1;
    for (i = 0; i < n; i++) {
	ip = indx[i];
	sum = b[ip];
	b[ip] = b[i];
	if (ii >= 0)
	    for (j = ii; j < i; j++)
		sum -= a[i][j] * b[j];
	else if (sum)
	    ii = i;
	b[i] = sum;
    }
    for (i = n - 1; i >= 0; i--) {
	sum = b[i];
	for (j = i + 1; j < n; j++)
	    sum -= a[i][j] * b[j];
	b[i] = sum / a[i][i];
    }
}