/*
 * Copyright (C) 1997-2009, R3vis Corporation.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA, or visit http://www.gnu.org/copyleft/lgpl.html.
 *
 * Original Contributor:
 *   Wes Bethel, R3vis Corporation, Marin County, California
 *   http://www.r3vis.com/
 * Additional Contributor(s):
 *
 * The OpenRM project is located at http://openrm.sourceforge.net/.
 */
/*
 * $Id: rmlinpk.c,v 1.7 2005/06/09 00:45:29 wes Exp $
 * Version: $Name: v180-alpha-02 $
 * $Revision: 1.7 $
 * $Log: rmlinpk.c,v $
 * Revision 1.7  2005/06/09 00:45:29  wes
 * More compiler warning fixes turned up by Windows build.
 *
 * Revision 1.6  2005/02/19 16:22:50  wes
 * Distro sync and consolidation.
 *
 * Revision 1.5  2005/01/23 17:04:03  wes
 * Copyright update to 2005.
 *
 * Revision 1.4  2004/01/16 16:45:12  wes
 * Updated copyright line for 2004.
 *
 * Revision 1.3  2003/02/02 17:50:57  wes
 * Added bounding boxes to RMprimitives, as a supplement to node-level bboxes.
 * The RMprimitive level bboxes are needed for the retained-mode CR work.
 *
 * Revision 1.2  2003/02/02 02:07:15  wes
 * Updated copyright to 2003.
 *
 * Revision 1.1.1.1  2003/01/28 02:15:23  wes
 * Manual rebuild of rm150 repository.
 *
 * Revision 1.6  2003/01/16 22:21:17  wes
 * Updated all source files to reflect new organization of header files:
 * all header files formerly located in include/rmaux, include/rmi, include/rmv
 * are now located in include/rm.
 *
 * Revision 1.5  2002/04/30 19:31:59  wes
 * Updated copyright dates.
 *
 * Revision 1.4  2001/03/31 17:12:38  wes
 * v1.4.0-alpha-2 checkin.
 *
 * Revision 1.3  2000/12/03 22:35:38  wes
 * Mods for thread safety.
 *
 * Revision 1.2  2000/04/20 16:29:47  wes
 * Documentation additions/enhancements, some code rearragement.
 *
 * Revision 1.1.1.1  2000/02/28 21:29:40  wes
 * OpenRM 1.2 Checkin
 *
 * Revision 1.1.1.1  2000/02/28 17:18:48  wes
 * Initial entry - pre-RM120 release, source base for OpenRM 1.2.
 *
 */

#include <rm/rm.h>

/*
 * The stuff in this file has its origin in an old LINPACK
 * distribution (no Copyright). We took the original fortran code,
 * picked out the pieces needed, ran the fortran code through F2C,
 * then cleaned up the results by hand.
 *
 * The routines in this file are called from other RM routines to
 * do numerical work, like matrix inversion. There are no public
 * RM routines in this file.
 */


/* Table of constant values */

typedef int integer;
typedef char *address;
typedef short int shortint;
typedef float real;
typedef double doublereal;
typedef struct { real r, i; } complex;
typedef struct { doublereal r, i; } doublecomplex;
typedef long int logical;
typedef short int shortlogical;
typedef char logical1;
typedef char integer1;

#define abs(x) ((x) >= 0 ? (x) : -(x))
#define dabs(x) (doublereal)abs(x)


static integer c__1 = 1;


/* *********************************************************************** */
integer isamax_(integer *n, real *sx, integer *incx)
{
    /* System generated locals */
    integer ret_val=0, i__1=0;
    real r__1=0;

    /* Local variables */
#if 0
    static real smax=0.;
    static integer i=0, ix=0;
#endif
    real smax=0.;
    integer i=0, ix=0;


/*     FINDS THE INDEX OF ELEMENT HAVING MAX. ABSOLUTE VALUE. */
/*     JACK DONGARRA, LINPACK, 3/11/78. */


    /* Parameter adjustments */
    --sx;

    /* Function Body */
    ret_val = 0;
    if (*n < 1) {
	return ret_val;
    }
    ret_val = 1;
    if (*n == 1) {
	return ret_val;
    }
    if (*incx == 1) {
	goto L20;
    }

/*        CODE FOR INCREMENT NOT EQUAL TO 1 */

    ix = 1;
    smax = dabs(sx[1]);
    ix += *incx;
    i__1 = *n;
    for (i = 2; i <= i__1; ++i) {
	if ((r__1 = sx[ix], dabs(r__1)) <= smax) {
	    goto L5;
	}
	ret_val = i;
	smax = (r__1 = sx[ix], dabs(r__1));
L5:
	ix += *incx;
/* L10: */
    }
    return ret_val;

/*        CODE FOR INCREMENT EQUAL TO 1 */

L20:
    smax = dabs(sx[1]);
    i__1 = *n;
    for (i = 2; i <= i__1; ++i) {
	if ((r__1 = sx[i], dabs(r__1)) <= smax) {
	    goto L30;
	}
	ret_val = i;
	smax = (r__1 = sx[i], dabs(r__1));
L30:
	;
    }
    return ret_val;
} /* isamax_ */

/* *********************************************************************** */
/* Subroutine */ int saxpy_(integer *n, real *sa, real *sx, integer *incx, 
	real *sy, integer *incy)
{
    /* System generated locals */
    integer i__1=0;

    /* Local variables */
/*    static integer i=0, m=0, ix=0, iy=0, mp1=0; */
    integer i=0, m=0, ix=0, iy=0, mp1=0;


/*     CONSTANT TIMES A VECTOR PLUS A VECTOR. */
/*     USES UNROLLED LOOP FOR INCREMENTS EQUAL TO ONE. */
/*     JACK DONGARRA, LINPACK, 3/11/78. */


    /* Parameter adjustments */
    --sy;
    --sx;

    /* Function Body */
    if (*n <= 0) {
	return 0;
    }
    if (*sa == 0.f) {
	return 0;
    }
    if (*incx == 1 && *incy == 1) {
	goto L20;
    }

/*        CODE FOR UNEQUAL INCREMENTS OR EQUAL INCREMENTS */
/*          NOT EQUAL TO 1 */

    ix = 1;
    iy = 1;
    if (*incx < 0) {
	ix = (-(*n) + 1) * *incx + 1;
    }
    if (*incy < 0) {
	iy = (-(*n) + 1) * *incy + 1;
    }
    i__1 = *n;
    for (i = 1; i <= i__1; ++i) {
	sy[iy] += *sa * sx[ix];
	ix += *incx;
	iy += *incy;
/* L10: */
    }
    return 0;

/*        CODE FOR BOTH INCREMENTS EQUAL TO 1 */


/*        CLEAN-UP LOOP */

L20:
    m = *n % 4;
    if (m == 0) {
	goto l40;
    }
    i__1 = m;
    for (i = 1; i <= i__1; ++i) {
	sy[i] += *sa * sx[i];
/* l30: */
    }
    if (*n < 4) {
	return 0;
    }
l40:
    mp1 = m + 1;
    i__1 = *n;
    for (i = mp1; i <= i__1; i += 4) {
	sy[i] += *sa * sx[i];
	sy[i + 1] += *sa * sx[i + 1];
	sy[i + 2] += *sa * sx[i + 2];
	sy[i + 3] += *sa * sx[i + 3];
/* l50: */
    }
    return 0;
} /* saxpy_ */

/* *********************************************************************** */
/* Subroutine */ void sgedi(real *a, integer *lda, integer *n, integer *ipvt, 
	real *det, real *work, integer *job)
{
    /* System generated locals */
    integer a_dim1=0, a_offset=0, i__1=0, i__2=0;

    /* Local variables */
#if 0
    static integer i, j, k, l;
    static real t=0.;
#endif
    integer i, j, k, l;
    real t=0.;
    
    extern /* Subroutine */ int sscal_(integer *, real *, real *, integer *), 
	    sswap_(integer *, real *, integer *, real *, integer *), saxpy_(
	    integer *, real *, real *, integer *, real *, integer *);
#if 0    
    static integer kb, kp1, nm1;
    static real ten=10.;
#endif
    integer kb, kp1, nm1;
    real ten=10.;


/*     SGEDI COMPUTES THE DETERMINANT AND INVERSE OF A MATRIX */
/*     USING THE FACTORS COMPUTED BY SGECO OR SGEFA. */

/*     ON ENTRY */

/*        A       REAL(LDA, N) */
/*                THE OUTPUT FROM SGECO OR SGEFA. */

/*        LDA     INTEGER */
/*                THE LEADING DIMENSION OF THE ARRAY  A . */

/*        N       INTEGER */
/*                THE ORDER OF THE MATRIX  A . */

/*        IPVT    INTEGER(N) */
/*                THE PIVOT VECTOR FROM SGECO OR SGEFA. */

/*        WORK    REAL(N) */
/*                WORK VECTOR.  CONTENTS DESTROYED. */

/*        JOB     INTEGER */
/*                = 11   BOTH DETERMINANT AND INVERSE. */
/*                = 01   INVERSE ONLY. */
/*                = 10   DETERMINANT ONLY. */

/*     ON RETURN */

/*        A       INVERSE OF ORIGINAL MATRIX IF REQUESTED. */
/*                OTHERWISE UNCHANGED. */

/*        DET     REAL(2) */
/*                DETERMINANT OF ORIGINAL MATRIX IF REQUESTED. */
/*                OTHERWISE NOT REFERENCED. */
/*                DETERMINANT = DET(1) * 10.0**DET(2) */
/*                WITH  1.0 .LE. ABS(DET(1)) .LT. 10.0 */
/*                OR  DET(1) .EQ. 0.0 . */

/*     ERROR CONDITION */

/*        A DIVISION BY ZERO WILL OCCUR IF THE INPUT FACTOR CONTAINS */
/*        A ZERO ON THE DIAGONAL AND THE INVERSE IS REQUESTED. */
/*        IT WILL NOT OCCUR IF THE SUBROUTINES ARE CALLED CORRECTLY */
/*        AND IF SGECO HAS SET RCOND .GT. 0.0 OR SGEFA HAS SET */
/*        INFO .EQ. 0 . */

/*     LINPACK. THIS VERSION DATED 08/14/78 . */
/*     CLEVE MOLER, UNIVERSITY OF NEW MEXICO, ARGONNE NATIONAL LAB. */

/*     SUBROUTINES AND FUNCTIONS */

/*     BLAS SAXPY,SSCAL,SSWAP */
/*     FORTRAN ABS,MOD */

/*     INTERNAL VARIABLES */



/*     COMPUTE DETERMINANT */

    /* Parameter adjustments */
    a_dim1 = *lda;
    a_offset = a_dim1 + 1;
    a -= a_offset;
    --ipvt;
    --det;
    --work;

    /* Function Body */
    if (*job / 10 == 0) {
	goto L70;
    }
    det[1] = 1.f;
    det[2] = 0.f;
    ten = 10.f;
    i__1 = *n;
    for (i = 1; i <= i__1; ++i) {
	if (ipvt[i] != i) {
	    det[1] = -(doublereal)det[1];
	}
	det[1] = a[i + i * a_dim1] * det[1];
/*        ...EXIT */
	if (det[1] == 0.f) {
	    goto L60;
	}
L10:
	if (dabs(det[1]) >= 1.f) {
	    goto L20;
	}
	det[1] = ten * det[1];
	det[2] += -1.f;
	goto L10;
L20:
L30:
	if (dabs(det[1]) < ten) {
	    goto L40;
	}
	det[1] /= ten;
	det[2] += 1.f;
	goto L30;
L40:
/* L50: */
	;
    }
L60:
L70:

/*     COMPUTE INVERSE(U) */

    if (*job % 10 == 0) {
	goto L150;
    }
    i__1 = *n;
    for (k = 1; k <= i__1; ++k) {
	a[k + k * a_dim1] = 1.f / a[k + k * a_dim1];
	t = -(doublereal)a[k + k * a_dim1];
	i__2 = k - 1;
	sscal_(&i__2, &t, &a[k * a_dim1 + 1], &c__1);
	kp1 = k + 1;
	if (*n < kp1) {
	    goto L90;
	}
	i__2 = *n;
	for (j = kp1; j <= i__2; ++j) {
	    t = a[k + j * a_dim1];
	    a[k + j * a_dim1] = 0.f;
	    saxpy_(&k, &t, &a[k * a_dim1 + 1], &c__1, &a[j * a_dim1 + 1], &
		    c__1);
/* L80: */
	}
L90:
/* L100: */
	;
    }

/*        FORM INVERSE(U)*INVERSE(L) */

    nm1 = *n - 1;
    if (nm1 < 1) {
	goto L140;
    }
    i__1 = nm1;
    for (kb = 1; kb <= i__1; ++kb) {
	k = *n - kb;
	kp1 = k + 1;
	i__2 = *n;
	for (i = kp1; i <= i__2; ++i) {
	    work[i] = a[i + k * a_dim1];
	    a[i + k * a_dim1] = 0.f;
/* L110: */
	}
	i__2 = *n;
	for (j = kp1; j <= i__2; ++j) {
	    t = work[j];
	    saxpy_(n, &t, &a[j * a_dim1 + 1], &c__1, &a[k * a_dim1 + 1], &
		    c__1);
/* L120: */
	}
	l = ipvt[k];
	if (l != k) {
	    sswap_(n, &a[k * a_dim1 + 1], &c__1, &a[l * a_dim1 + 1], &c__1);
	}
/* L130: */
    }
L140:
L150:
    return;
    /*    return 0; */
} /* sgedi_ */

/* *********************************************************************** */
/* Subroutine */ void sgefa(real *a, integer *lda, integer *n, integer *ipvt, 
	integer *info)
{
    /* System generated locals */
    integer a_dim1, a_offset, i__1, i__2, i__3;

    /* Local variables */
#if 0
    static integer j, k, l;
    static real t;
#endif
    integer j, k, l;
    real t;
    
    extern /* Subroutine */ int sscal_(integer *, real *, real *, integer *), 
	    saxpy_(integer *, real *, real *, integer *, real *, integer *);
    extern integer isamax_(integer *, real *, integer *);
#if 0
    static integer kp1, nm1;
#endif
    integer kp1, nm1;


/*     SGEFA FACTORS A REAL MATRIX BY GAUSSIAN ELIMINATION. */

/*     SGEFA IS USUALLY CALLED BY SGECO, BUT IT CAN BE CALLED */
/*     DIRECTLY WITH A SAVING IN TIME IF  RCOND  IS NOT NEEDED. */
/*     (TIME FOR SGECO) = (1 + 9/N)*(TIME FOR SGEFA). */

/*     ON ENTRY */

/*        A       REAL(LDA, N) */
/*                THE MATRIX TO BE FACTORED. */

/*        LDA     INTEGER */
/*                THE LEADING DIMENSION OF THE ARRAY  A . */

/*        N       INTEGER */
/*                THE ORDER OF THE MATRIX  A . */

/*     ON RETURN */

/*        A       AN UPPER TRIANGULAR MATRIX AND THE MULTIPLIERS */
/*                WHICH WERE USED TO OBTAIN IT. */
/*                THE FACTORIZATION CAN BE WRITTEN  A = L*U  WHERE */
/*                L  IS A PRODUCT OF PERMUTATION AND UNIT LOWER */
/*                TRIANGULAR MATRICES AND  U  IS UPPER TRIANGULAR. */

/*        IPVT    INTEGER(N) */
/*                AN INTEGER VECTOR OF PIVOT INDICES. */

/*        INFO    INTEGER */
/*                = 0  NORMAL VALUE. */
/*                = K  IF  U(K,K) .EQ. 0.0 .  THIS IS NOT AN ERROR */
/*                     CONDITION FOR THIS SUBROUTINE, BUT IT DOES */
/*                     INDICATE THAT SGESL OR SGEDI WILL DIVIDE BY ZERO */
/*                     IF CALLED.  USE  RCOND  IN SGECO FOR A RELIABLE */
/*                     INDICATION OF SINGULARITY. */

/*     LINPACK. THIS VERSION DATED 08/14/78 . */
/*     CLEVE MOLER, UNIVERSITY OF NEW MEXICO, ARGONNE NATIONAL LAB. */

/*     SUBROUTINES AND FUNCTIONS */

/*     BLAS SAXPY,SSCAL,ISAMAX */

/*     INTERNAL VARIABLES */



/*     GAUSSIAN ELIMINATION WITH PARTIAL PIVOTING */

    /* Parameter adjustments */
    a_dim1 = *lda;
    a_offset = a_dim1 + 1;
    a -= a_offset;
    --ipvt;

    /* Function Body */
    *info = 0;
    nm1 = *n - 1;
    if (nm1 < 1) {
	goto L70;
    }
    i__1 = nm1;
    for (k = 1; k <= i__1; ++k) {
	kp1 = k + 1;

/*        FIND L = PIVOT INDEX */

	i__2 = *n - k + 1;
	l = isamax_(&i__2, &a[k + k * a_dim1], &c__1) + k - 1;
	ipvt[k] = l;

/*        ZERO PIVOT IMPLIES THIS COLUMN ALREADY TRIANGULARIZED */

	if (a[l + k * a_dim1] == 0.f) {
	    goto L40;
	}

/*           INTERCHANGE IF NECESSARY */

	if (l == k) {
	    goto L10;
	}
	t = a[l + k * a_dim1];
	a[l + k * a_dim1] = a[k + k * a_dim1];
	a[k + k * a_dim1] = t;
L10:

/*           COMPUTE MULTIPLIERS */

	t = -1.f / a[k + k * a_dim1];
	i__2 = *n - k;
	sscal_(&i__2, &t, &a[k + 1 + k * a_dim1], &c__1);

/*           ROW ELIMINATION WITH COLUMN INDEXING */

	i__2 = *n;
	for (j = kp1; j <= i__2; ++j) {
	    t = a[l + j * a_dim1];
	    if (l == k) {
		goto L20;
	    }
	    a[l + j * a_dim1] = a[k + j * a_dim1];
	    a[k + j * a_dim1] = t;
L20:
	    i__3 = *n - k;
	    saxpy_(&i__3, &t, &a[k + 1 + k * a_dim1], &c__1, &a[k + 1 + j * 
		    a_dim1], &c__1);
/* L30: */
	}
	goto L50;
L40:
	*info = k;
L50:
/* L60: */
	;
    }
L70:
    ipvt[*n] = *n;
    if (a[*n + *n * a_dim1] == 0.f) {
	*info = *n;
    }
    /*    return 0; */
} /* sgefa_ */

/* *********************************************************************** */
/* Subroutine */ int sscal_(integer *n, real *sa, real *sx, integer *incx)
{
    /* System generated locals */
    integer i__1, i__2;

    /* Local variables */
/*    static integer i, m, nincx, mp1; */
    integer i, m, nincx, mp1;


/*     SCALES A VECTOR BY A CONSTANT. */
/*     USES UNROLLED LOOPS FOR INCREMENT EQUAL TO 1. */
/*     JACK DONGARRA, LINPACK, 3/11/78. */


    /* Parameter adjustments */
    --sx;

    /* Function Body */
    if (*n <= 0) {
	return 0;
    }
    if (*incx == 1) {
	goto L20;
    }

/*        CODE FOR INCREMENT NOT EQUAL TO 1 */

    nincx = *n * *incx;
    i__1 = nincx;
    i__2 = *incx;
    for (i = 1; i__2 < 0 ? i >= i__1 : i <= i__1; i += i__2) {
	sx[i] = *sa * sx[i];
/* L10: */
    }
    return 0;

/*        CODE FOR INCREMENT EQUAL TO 1 */


/*        CLEAN-UP LOOP */

L20:
    m = *n % 5;
    if (m == 0) {
	goto L40;
    }
    i__2 = m;
    for (i = 1; i <= i__2; ++i) {
	sx[i] = *sa * sx[i];
/* L30: */
    }
    if (*n < 5) {
	return 0;
    }
L40:
    mp1 = m + 1;
    i__2 = *n;
    for (i = mp1; i <= i__2; i += 5) {
	sx[i] = *sa * sx[i];
	sx[i + 1] = *sa * sx[i + 1];
	sx[i + 2] = *sa * sx[i + 2];
	sx[i + 3] = *sa * sx[i + 3];
	sx[i + 4] = *sa * sx[i + 4];
/* L50: */
    }
    return 0;
} /* sscal_ */

/* *********************************************************************** */
/* Subroutine */ int sswap_(integer *n, real *sx, integer *incx, real *sy, 
	integer *incy)
{
    /* System generated locals */
    integer i__1;

    /* Local variables */
#if 0
    static integer i, m;
    static real stemp;
    static integer ix, iy, mp1;
#endif
    integer i, m;
    real stemp;
    integer ix, iy, mp1;


/*     INTERCHANGES TWO VECTORS. */
/*     USES UNROLLED LOOPS FOR INCREMENTS EQUAL TO 1. */
/*     JACK DONGARRA, LINPACK, 3/11/78. */


    /* Parameter adjustments */
    --sy;
    --sx;

    /* Function Body */
    if (*n <= 0) {
	return 0;
    }
    if (*incx == 1 && *incy == 1) {
	goto L20;
    }

/*       CODE FOR UNEQUAL INCREMENTS OR EQUAL INCREMENTS NOT EQUAL */
/*         TO 1 */

    ix = 1;
    iy = 1;
    if (*incx < 0) {
	ix = (-(*n) + 1) * *incx + 1;
    }
    if (*incy < 0) {
	iy = (-(*n) + 1) * *incy + 1;
    }
    i__1 = *n;
    for (i = 1; i <= i__1; ++i) {
	stemp = sx[ix];
	sx[ix] = sy[iy];
	sy[iy] = stemp;
	ix += *incx;
	iy += *incy;
/* L10: */
    }
    return 0;

/*       CODE FOR BOTH INCREMENTS EQUAL TO 1 */


/*       CLEAN-UP LOOP */

L20:
    m = *n % 3;
    if (m == 0) {
	goto L40;
    }
    i__1 = m;
    for (i = 1; i <= i__1; ++i) {
	stemp = sx[i];
	sx[i] = sy[i];
	sy[i] = stemp;
/* L30: */
    }
    if (*n < 3) {
	return 0;
    }
L40:
    mp1 = m + 1;
    i__1 = *n;
    for (i = mp1; i <= i__1; i += 3) {
	stemp = sx[i];
	sx[i] = sy[i];
	sy[i] = stemp;
	stemp = sx[i + 1];
	sx[i + 1] = sy[i + 1];
	sy[i + 1] = stemp;
	stemp = sx[i + 2];
	sx[i + 2] = sy[i + 2];
	sy[i + 2] = stemp;
/* L50: */
    }
    return 0;
} /* sswap_ */
/* EOF */
