
      program findpos
      implicit real*8 (a-h,o-z)
      real*8 x(10),y(10),z(10),d(10)
      real*4 randgs,sd
      n=4
      x(1)=-8.4d0
      y(1)=0d0
      z(1)=60.2d0
      x(2)=.9d0
      y(2)=5.7d0
      z(2)=25.8d0
      x(3)=x(2)
      y(3)=-y(2)
      z(3)=z(2)
      x(4)=1d0
      y(4)=0d0
      z(4)=18.2d0

      sigma=30d-6
      c=60d0
 1    print *,'Type x,y'
      read *,xp,yp
      zp=(xp**2+yp**2)/(4d0*c)
      do i=1,n
         d(i)=sqrt((x(i)-xp)**2+(y(i)-yp)**2+(z(i)-zp)**2)
         d(i)=d(i)+randgs(0.,sngl(sigma))
      end do
      sd=1
      xpcalc=xp+randgs(0.,sd)
      ypcalc=yp+randgs(0.,sd)
      zpcalc=zp+randgs(0.,sd)
      call findpoint(x,y,z,d,n,sigma,xpcalc,ypcalc,zpcalc,sigmaxp,
     &   sigmayp,sigmazp)
      err=sqrt((xp-xpcalc)**2+(yp-ypcalc)**2+(zp-zpcalc)**2)
      print *,(xp-xpcalc)*1d6,(yp-ypcalc)*1d6,(zp-zpcalc)*1d6
      print *,'error (microns)=',err*1d6
      if (2.gt.1) go to 1
      stop
      end

      subroutine findpoint(x,y,z,d,n,sigma,xp,yp,zp,sigmaxp,sigmayp,
     &   sigmazp)
c
c  Given n laser ranging measurements d(i), each of rms accuracy sigma, 
c  obtained by n (>2) rangefinders located at positions (x(i),y(i),z(i)), 
c  this subroutine computes an estimate (xp,yp,zp) of Cartesian coordinates
c  of the target point.  It also estimates the standard errors (sigmaxp,
c  sigmayp, sigmazp) of the computed position [not yet implemented].
c
c  On entry to the subroutine initial guesses for xp, yp, and zp must be
c  supplied.  On exit, these values are replaced by the computed solution.
c
      implicit real*8 (a-h,o-z)
      parameter (nmax=10)
      real*8 x(n),y(n),z(n),d(n),dic(nmax),res(nmax)
      real*8 h(3,3),g(3)
      integer ipvt(3)
      logical quit
      itmax=10
      print *,'Initial guess:'
      print *,xp,yp,zp
      tol=1d-12
      do k=1,itmax 
         s=0d0
         do i=1,n
            dic(i)=sqrt((x(i)-xp)**2+(y(i)-yp)**2+(z(i)-zp)**2)
            res(i)=dic(i)-d(i)
            s=s+.5d0*res(i)**2
         end do
c  Form gradient vector and Hessian matrix of S:
         do i=1,3
            g(i)=0d0
         end do
         h(1,1)=0d0
         h(1,2)=0d0
         h(1,3)=0d0
         h(2,2)=0d0
         h(2,3)=0d0
         h(3,3)=0d0
         do i=1,n
            p1=(xp-x(i))/dic(i)
            p2=(yp-y(i))/dic(i)
            p3=(zp-z(i))/dic(i)
c     c     h(1,1)=h(1,1)+p1**2+res(i)*(1d0-p1**2)/dic(i)
c     c     h(2,2)=h(2,2)+p2**2+res(i)*(1d0-p2**2)/dic(i)
c     c     h(3,3)=h(3,3)+p3**2+res(i)*(1d0-p3**2)/dic(i)
c     c     h(1,2)=h(1,2)+p1*p2*(1d0-res(i)/dic(i))
c     c     h(1,3)=h(1,3)+p1*p3*(1d0-res(i)/dic(i))
c     c     h(2,3)=h(2,3)+p2*p3*(1d0-res(i)/dic(i))

            h(1,1)=h(1,1)+p1**2
            h(2,2)=h(2,2)+p2**2
            h(3,3)=h(3,3)+p3**2
            h(1,2)=h(1,2)+p1*p2
            h(1,3)=h(1,3)+p1*p3
            h(2,3)=h(2,3)+p2*p3

            g(1)=g(1)+res(i)*p1
            g(2)=g(2)+res(i)*p2
            g(3)=g(3)+res(i)*p3
         end do
         h(2,1)=h(1,2)
         h(3,1)=h(1,3)
         h(3,2)=h(2,3)         
         gn=sqrt(g(1)**2+g(2)**2+g(3)**2)
         print *,'It.',k,' S=',s,' gn=',gn
c         call dpofa(h,3,3,info)
         call dgefa(h,3,3,ipvt,info)
         if (info.ne.0) then
            print *,'TROUBLE: info=',info,' in dpofa'
            stop
         end if
c         call dposl(h,3,3,g)
         call dgesl(h,3,3,ipvt,g,0)
         quit=abs(g(1)).lt.tol.and.abs(g(2)).lt.tol.and.abs(g(3)).lt.tol
         if (quit) go to 1
         xp=xp-g(1)
         yp=yp-g(2)
         zp=zp-g(3)
      end do
 1    print *,xp,yp,zp
      return
      end

      subroutine dpofa(a,lda,n,info)
      integer lda,n,info
      double precision a(lda,1)
c
c     dpofa factors a double precision symmetric positive definite
c     matrix.
c
c     dpofa is usually called by dpoco, but it can be called
c     directly with a saving in time if  rcond  is not needed.
c     (time for dpoco) = (1 + 18/n)*(time for dpofa) .
c
c     on entry
c
c        a       double precision(lda, n)
c                the symmetric matrix to be factored.  only the
c                diagonal and upper triangle are used.
c
c        lda     integer
c                the leading dimension of the array  a .
c
c        n       integer
c                the order of the matrix  a .
c
c     on return
c
c        a       an upper triangular matrix  r  so that  a = trans(r)*r
c                where  trans(r)  is the transpose.
c                the strict lower triangle is unaltered.
c                if  info .ne. 0 , the factorization is not complete.
c
c        info    integer
c                = 0  for normal return.
c                = k  signals an error condition.  the leading minor
c                     of order  k  is not positive definite.
c
c     linpack.  this version dated 08/14/78 .
c     cleve moler, university of new mexico, argonne national lab.
c
c     subroutines and functions
c
c     blas ddot
c     fortran dsqrt
c
c     internal variables
c
      double precision ddot,t
      double precision s
      integer j,jm1,k
c     begin block with ...exits to 40
c
c
         do 30 j = 1, n
            info = j
            s = 0.0d0
            jm1 = j - 1
            if (jm1 .lt. 1) go to 20
            do 10 k = 1, jm1
               t = a(k,j) - ddot(k-1,a(1,k),1,a(1,j),1)
               t = t/a(k,k)
               a(k,j) = t
               s = s + t*t
   10       continue
   20       continue
            s = a(j,j) - s
c     ......exit
            if (s .le. 0.0d0) go to 40
            a(j,j) = dsqrt(s)
   30    continue
         info = 0
   40 continue
      return
      end

      subroutine dposl(a,lda,n,b)
      integer lda,n
      double precision a(lda,1),b(1)
c
c     dposl solves the double precision symmetric positive definite
c     system a * x = b
c     using the factors computed by dpoco or dpofa.
c
c     on entry
c
c        a       double precision(lda, n)
c                the output from dpoco or dpofa.
c
c        lda     integer
c                the leading dimension of the array  a .
c
c        n       integer
c                the order of the matrix  a .
c
c        b       double precision(n)
c                the right hand side vector.
c
c     on return
c
c        b       the solution vector  x .
c
c     error condition
c
c        a division by zero will occur if the input factor contains
c        a zero on the diagonal.  technically this indicates
c        singularity but it is usually caused by improper subroutine
c        arguments.  it will not occur if the subroutines are called
c        correctly and  info .eq. 0 .
c
c     to compute  inverse(a) * c  where  c  is a matrix
c     with  p  columns
c           call dpoco(a,lda,n,rcond,z,info)
c           if (rcond is too small .or. info .ne. 0) go to ...
c           do 10 j = 1, p
c              call dposl(a,lda,n,c(1,j))
c        10 continue
c
c     linpack.  this version dated 08/14/78 .
c     cleve moler, university of new mexico, argonne national lab.
c
c     subroutines and functions
c
c     blas daxpy,ddot
c
c     internal variables
c
      double precision ddot,t
      integer k,kb
c
c     solve trans(r)*y = b
c
      do 10 k = 1, n
         t = ddot(k-1,a(1,k),1,b(1),1)
         b(k) = (b(k) - t)/a(k,k)
   10 continue
c
c     solve r*x = y
c
      do 20 kb = 1, n
         k = n + 1 - kb
         b(k) = b(k)/a(k,k)
         t = -b(k)
         call daxpy(k-1,t,a(1,k),1,b(1),1)
   20 continue
      return
      end
      subroutine daxpy(n,da,dx,incx,dy,incy)
c
c     constant times a vector plus a vector.
c     uses unrolled loops for increments equal to one.
c     jack dongarra, linpack, 3/11/78.
c
      double precision dx(1),dy(1),da
      integer i,incx,incy,ix,iy,m,mp1,n
c
      if(n.le.0)return
      if (da .eq. 0.0d0) return
      if(incx.eq.1.and.incy.eq.1)go to 20
c
c        code for unequal increments or equal increments
c          not equal to 1
c
      ix = 1
      iy = 1
      if(incx.lt.0)ix = (-n+1)*incx + 1
      if(incy.lt.0)iy = (-n+1)*incy + 1
      do 10 i = 1,n
        dy(iy) = dy(iy) + da*dx(ix)
        ix = ix + incx
        iy = iy + incy
   10 continue
      return
c
c        code for both increments equal to 1
c
c
c        clean-up loop
c
   20 m = mod(n,4)
      if( m .eq. 0 ) go to 40
      do 30 i = 1,m
        dy(i) = dy(i) + da*dx(i)
   30 continue
      if( n .lt. 4 ) return
   40 mp1 = m + 1
      do 50 i = mp1,n,4
        dy(i) = dy(i) + da*dx(i)
        dy(i + 1) = dy(i + 1) + da*dx(i + 1)
        dy(i + 2) = dy(i + 2) + da*dx(i + 2)
        dy(i + 3) = dy(i + 3) + da*dx(i + 3)
   50 continue
      return
      end
      DOUBLE PRECISION FUNCTION DDOT(N,DX,INCX,DY,INCY)
C
C     FORMS THE DOT PRODUCT OF TWO VECTORS.
C     USES UNROLLED LOOPS FOR INCREMENTS EQUAL TO ONE.
C     JACK DONGARRA, LINPACK, 3/11/78.
C
      DOUBLE PRECISION DX(1),DY(1),DTEMP
      INTEGER I,INCX,INCY,IX,IY,M,MP1,N
C
      DDOT = 0.0D0
      DTEMP = 0.0D0
      IF(N.LE.0)RETURN
      IF(INCX.EQ.1.AND.INCY.EQ.1)GO TO 20
C
C        CODE FOR UNEQUAL INCREMENTS OR EQUAL INCREMENTS
C          NOT EQUAL TO 1
C
      IX = 1
      IY = 1
      IF(INCX.LT.0)IX = (-N+1)*INCX + 1
      IF(INCY.LT.0)IY = (-N+1)*INCY + 1
      DO 10 I = 1,N
        DTEMP = DTEMP + DX(IX)*DY(IY)
        IX = IX + INCX
        IY = IY + INCY
   10 CONTINUE
      DDOT = DTEMP
      RETURN
C
C        CODE FOR BOTH INCREMENTS EQUAL TO 1
C
C
C        CLEAN-UP LOOP
C
   20 M = MOD(N,5)
      IF( M .EQ. 0 ) GO TO 40
      DO 30 I = 1,M
        DTEMP = DTEMP + DX(I)*DY(I)
   30 CONTINUE
      IF( N .LT. 5 ) GO TO 60
   40 MP1 = M + 1
      DO 50 I = MP1,N,5
        DTEMP = DTEMP + DX(I)*DY(I) + DX(I + 1)*DY(I + 1) +
     *   DX(I + 2)*DY(I + 2) + DX(I + 3)*DY(I + 3) + DX(I + 4)*DY(I + 4)
   50 CONTINUE
   60 DDOT = DTEMP
      RETURN
      END

      function randgs (xmean, sd)
c
c generate a normally distributed random number, i.e., generate random
c numbers with a gaussian distribution.  these random numbers are not
c exceptionally good -- especially in the tails of the distribution,
c but this implementation is simple and suitable for most applications.
c see r. w. hamming, numerical methods for scientists and engineers,
c mcgraw-hill, 1962, pages 34 and 389.
c
c             input arguments --
c xmean  the mean of the gaussian distribution.
c sd     the standard deviation of the gaussian function
c          exp (-1/2 * (x-xmean)**2 / sd**2)
c
      external rand
c
      randgs = -6.
      do 10 i=1,12
        randgs = randgs + rand(0.)
 10   continue
c
      randgs = xmean + sd*randgs
c
      return
      end
      function rand (r)
c
c      this pseudo-random number generator is portable amoung a wide
c variety of computers.  rand(r) undoubtedly is not as good as many
c readily available installation dependent versions, and so this
c routine is not recommended for widespread usage.  its redeeming
c feature is that the exact same random numbers (to within final round-
c off error) can be generated from machine to machine.  thus, programs
c that make use of random numbers can be easily transported to and
c checked in a new environment.
c      the random numbers are generated by the linear congruential
c method described, e.g., by knuth in seminumerical methods (p.9),
c addison-wesley, 1969.  given the i-th number of a pseudo-random
c sequence, the i+1 -st number is generated from
c             x(i+1) = (a*x(i) + c) mod m,
c where here m = 2**22 = 4194304, c = 1731 and several suitable values
c of the multiplier a are discussed below.  both the multiplier a and
c random number x are represented in double precision as two 11-bit
c words.  the constants are chosen so that the period is the maximum
c possible, 4194304.
c      in order that the same numbers be generated from machine to
c machine, it is necessary that 23-bit integers be reducible modulo
c 2**11 exactly, that 23-bit integers be added exactly, and that 11-bit
c integers be multiplied exactly.  furthermore, if the restart option
c is used (where r is between 0 and 1), then the product r*2**22 =
c r*4194304 must be correct to the nearest integer.
c      the first four random numbers should be .0004127026,
c .6750836372, .1614754200, and .9086198807.  the tenth random number
c is .5527787209, and the hundredth is .3600893021 .  the thousandth
c number should be .2176990509 .
c      in order to generate several effectively independent sequences
c with the same generator, it is necessary to know the random number
c for several widely spaced calls.  the i-th random number times 2**22,
c where i=k*p/8 and p is the period of the sequence (p = 2**22), is
c still of the form l*p/8.  in particular we find the i-th random
c number multiplied by 2**22 is given by
c i   =  0  1*p/8  2*p/8  3*p/8  4*p/8  5*p/8  6*p/8  7*p/8  8*p/8
c rand=  0  5*p/8  2*p/8  7*p/8  4*p/8  1*p/8  6*p/8  3*p/8  0
c thus the 4*p/8 = 2097152 random number is 2097152/2**22.
c      several multipliers have been subjected to the spectral test
c (see knuth, p. 82).  four suitable multipliers roughly in order of
c goodness according to the spectral test are
c    3146757 = 1536*2048 + 1029 = 2**21 + 2**20 + 2**10 + 5
c    2098181 = 1024*2048 + 1029 = 2**21 + 2**10 + 5
c    3146245 = 1536*2048 +  517 = 2**21 + 2**20 + 2**9 + 5
c    2776669 = 1355*2048 + 1629 = 5**9 + 7**7 + 1
c
c      in the table below log10(nu(i)) gives roughly the number of
c random decimal digits in the random numbers considered i at a time.
c c is the primary measure of goodness.  in both cases bigger is better.
c
c                   log10 nu(i)              c(i)
c       a       i=2  i=3  i=4  i=5    i=2  i=3  i=4  i=5
c
c    3146757    3.3  2.0  1.6  1.3    3.1  1.3  4.6  2.6
c    2098181    3.3  2.0  1.6  1.2    3.2  1.3  4.6  1.7
c    3146245    3.3  2.2  1.5  1.1    3.2  4.2  1.1  0.4
c    2776669    3.3  2.1  1.6  1.3    2.5  2.0  1.9  2.6
c   best
c    possible   3.3  2.3  1.7  1.4    3.6  5.9  9.7  14.9
c
c             input argument --
c r      if r=0., the next random number of the sequence is generated.
c        if r.lt.0., the last generated number will be returned for
c          possible use in a restart procedure.
c        if r.gt.0., the sequence of random numbers will start with the
c          seed r mod 1.  this seed is also returned as the value of
c          rand provided the arithmetic is done exactly.
c
c             output value --
c rand   a pseudo-random number between 0. and 1.
c
c ia1 and ia0 are the hi and lo parts of a.  ia1ma0 = ia1 - ia0.
      data ia1, ia0, ia1ma0 /1536, 1029, 507/
      data ic /1731/
      data ix1, ix0 /0, 0/
c
      if (r.lt.0.) go to 10
      if (r.gt.0.) go to 20
c
c           a*x = 2**22*ia1*ix1 + 2**11*(ia1*ix1 + (ia1-ia0)*(ix0-ix1)
c                   + ia0*ix0) + ia0*ix0
c
      iy0 = ia0*ix0
      iy1 = ia1*ix1 + ia1ma0*(ix0-ix1) + iy0
      iy0 = iy0 + ic
      ix0 = mod (iy0, 2048)
      iy1 = iy1 + (iy0-ix0)/2048
      ix1 = mod (iy1, 2048)
c
 10   rand = ix1*2048 + ix0
      rand = rand / 4194304.
      return
c
 20   ix1 = amod(r,1.)*4194304. + 0.5
      ix0 = mod (ix1, 2048)
      ix1 = (ix1-ix0)/2048
      go to 10
c
      end

      SUBROUTINE DGEFA(A,LDA,N,IPVT,INFO)
      INTEGER LDA,N,IPVT(1),INFO
      DOUBLE PRECISION A(LDA,1)
C
C     DGEFA FACTORS A DOUBLE PRECISION MATRIX BY GAUSSIAN ELIMINATION.
C
C     DGEFA IS USUALLY CALLED BY DGECO, BUT IT CAN BE CALLED
C     DIRECTLY WITH A SAVING IN TIME IF  RCOND  IS NOT NEEDED.
C     (TIME FOR DGECO) = (1 + 9/N)*(TIME FOR DGEFA) .
C
C     ON ENTRY
C
C        A       DOUBLE PRECISION(LDA, N)
C                THE MATRIX TO BE FACTORED.
C
C        LDA     INTEGER
C                THE LEADING DIMENSION OF THE ARRAY  A .
C
C        N       INTEGER
C                THE ORDER OF THE MATRIX  A .
C
C     ON RETURN
C
C        A       AN UPPER TRIANGULAR MATRIX AND THE MULTIPLIERS
C                WHICH WERE USED TO OBTAIN IT.
C                THE FACTORIZATION CAN BE WRITTEN  A = L*U  WHERE
C                L  IS A PRODUCT OF PERMUTATION AND UNIT LOWER
C                TRIANGULAR MATRICES AND  U  IS UPPER TRIANGULAR.
C
C        IPVT    INTEGER(N)
C                AN INTEGER VECTOR OF PIVOT INDICES.
C
C        INFO    INTEGER
C                = 0  NORMAL VALUE.
C                = K  IF  U(K,K) .EQ. 0.0 .  THIS IS NOT AN ERROR
C                     CONDITION FOR THIS SUBROUTINE, BUT IT DOES
C                     INDICATE THAT DGESL OR DGEDI WILL DIVIDE BY ZERO
C                     IF CALLED.  USE  RCOND  IN DGECO FOR A RELIABLE
C                     INDICATION OF SINGULARITY.
C
C     LINPACK. THIS VERSION DATED 08/14/78 .
C     CLEVE MOLER, UNIVERSITY OF NEW MEXICO, ARGONNE NATIONAL LAB.
C
C     SUBROUTINES AND FUNCTIONS
C
C     BLAS DAXPY,DSCAL,IDAMAX
C
C     INTERNAL VARIABLES
C
      DOUBLE PRECISION T
      INTEGER IDAMAX,J,K,KP1,L,NM1
C
C
C     GAUSSIAN ELIMINATION WITH PARTIAL PIVOTING
C
      INFO = 0
      NM1 = N - 1
      IF (NM1 .LT. 1) GO TO 70
      DO 60 K = 1, NM1
         KP1 = K + 1
C
C        FIND L = PIVOT INDEX
C
         L = IDAMAX(N-K+1,A(K,K),1) + K - 1
         IPVT(K) = L
C
C        ZERO PIVOT IMPLIES THIS COLUMN ALREADY TRIANGULARIZED
C
         IF (A(L,K) .EQ. 0.0D0) GO TO 40
C
C           INTERCHANGE IF NECESSARY
C
            IF (L .EQ. K) GO TO 10
               T = A(L,K)
               A(L,K) = A(K,K)
               A(K,K) = T
   10       CONTINUE
C
C           COMPUTE MULTIPLIERS
C
            T = -1.0D0/A(K,K)
            CALL DSCAL(N-K,T,A(K+1,K),1)
C
C           ROW ELIMINATION WITH COLUMN INDEXING
C
            DO 30 J = KP1, N
               T = A(L,J)
               IF (L .EQ. K) GO TO 20
                  A(L,J) = A(K,J)
                  A(K,J) = T
   20          CONTINUE
               CALL DAXPY(N-K,T,A(K+1,K),1,A(K+1,J),1)
   30       CONTINUE
         GO TO 50
   40    CONTINUE
            INFO = K
   50    CONTINUE
   60 CONTINUE
   70 CONTINUE
      IPVT(N) = N
      IF (A(N,N) .EQ. 0.0D0) INFO = N
      RETURN
      END
      SUBROUTINE DGESL(A,LDA,N,IPVT,B,JOB)
      INTEGER LDA,N,IPVT(1),JOB
      DOUBLE PRECISION A(LDA,1),B(1)
C
C     DGESL SOLVES THE DOUBLE PRECISION SYSTEM
C     A * X = B  OR  TRANS(A) * X = B
C     USING THE FACTORS COMPUTED BY DGECO OR DGEFA.
C
C     ON ENTRY
C
C        A       DOUBLE PRECISION(LDA, N)
C                THE OUTPUT FROM DGECO OR DGEFA.
C
C        LDA     INTEGER
C                THE LEADING DIMENSION OF THE ARRAY  A .
C
C        N       INTEGER
C                THE ORDER OF THE MATRIX  A .
C
C        IPVT    INTEGER(N)
C                THE PIVOT VECTOR FROM DGECO OR DGEFA.
C
C        B       DOUBLE PRECISION(N)
C                THE RIGHT HAND SIDE VECTOR.
C
C        JOB     INTEGER
C                = 0         TO SOLVE  A*X = B ,
C                = NONZERO   TO SOLVE  TRANS(A)*X = B  WHERE
C                            TRANS(A)  IS THE TRANSPOSE.
C
C     ON RETURN
C
C        B       THE SOLUTION VECTOR  X .
C
C     ERROR CONDITION
C
C        A DIVISION BY ZERO WILL OCCUR IF THE INPUT FACTOR CONTAINS A
C        ZERO ON THE DIAGONAL.  TECHNICALLY THIS INDICATES SINGULARITY
C        BUT IT IS OFTEN CAUSED BY IMPROPER ARGUMENTS OR IMPROPER
C        SETTING OF LDA .  IT WILL NOT OCCUR IF THE SUBROUTINES ARE
C        CALLED CORRECTLY AND IF DGECO HAS SET RCOND .GT. 0.0
C        OR DGEFA HAS SET INFO .EQ. 0 .
C
C     TO COMPUTE  INVERSE(A) * C  WHERE  C  IS A MATRIX
C     WITH  P  COLUMNS
C           CALL DGECO(A,LDA,N,IPVT,RCOND,Z)
C           IF (RCOND IS TOO SMALL) GO TO ...
C           DO 10 J = 1, P
C              CALL DGESL(A,LDA,N,IPVT,C(1,J),0)
C        10 CONTINUE
C
C     LINPACK. THIS VERSION DATED 08/14/78 .
C     CLEVE MOLER, UNIVERSITY OF NEW MEXICO, ARGONNE NATIONAL LAB.
C
C     SUBROUTINES AND FUNCTIONS
C
C     BLAS DAXPY,DDOT
C
C     INTERNAL VARIABLES
C
      DOUBLE PRECISION DDOT,T
      INTEGER K,KB,L,NM1
C
      NM1 = N - 1
      IF (JOB .NE. 0) GO TO 50
C
C        JOB = 0 , SOLVE  A * X = B
C        FIRST SOLVE  L*Y = B
C
         IF (NM1 .LT. 1) GO TO 30
         DO 20 K = 1, NM1
            L = IPVT(K)
            T = B(L)
            IF (L .EQ. K) GO TO 10
               B(L) = B(K)
               B(K) = T
   10       CONTINUE
            CALL DAXPY(N-K,T,A(K+1,K),1,B(K+1),1)
   20    CONTINUE
   30    CONTINUE
C
C        NOW SOLVE  U*X = Y
C
         DO 40 KB = 1, N
            K = N + 1 - KB
            B(K) = B(K)/A(K,K)
            T = -B(K)
            CALL DAXPY(K-1,T,A(1,K),1,B(1),1)
   40    CONTINUE
      GO TO 100
   50 CONTINUE
C
C        JOB = NONZERO, SOLVE  TRANS(A) * X = B
C        FIRST SOLVE  TRANS(U)*Y = B
C
         DO 60 K = 1, N
            T = DDOT(K-1,A(1,K),1,B(1),1)
            B(K) = (B(K) - T)/A(K,K)
   60    CONTINUE
C
C        NOW SOLVE TRANS(L)*X = Y
C
         IF (NM1 .LT. 1) GO TO 90
         DO 80 KB = 1, NM1
            K = N - KB
            B(K) = B(K) + DDOT(N-K,A(K+1,K),1,B(K+1),1)
            L = IPVT(K)
            IF (L .EQ. K) GO TO 70
               T = B(L)
               B(L) = B(K)
               B(K) = T
   70       CONTINUE
   80    CONTINUE
   90    CONTINUE
  100 CONTINUE
      RETURN
      END

      INTEGER FUNCTION IDAMAX(N,DX,INCX)
C
C     FINDS THE INDEX OF ELEMENT HAVING MAX. ABSOLUTE VALUE.
C     JACK DONGARRA, LINPACK, 3/11/78.
C
      DOUBLE PRECISION DX(1),DMAX
      INTEGER I,INCX,IX,N
C
      IDAMAX = 0
      IF( N .LT. 1 ) RETURN
      IDAMAX = 1
      IF(N.EQ.1)RETURN
      IF(INCX.EQ.1)GO TO 20
C
C        CODE FOR INCREMENT NOT EQUAL TO 1
C
      IX = 1
      DMAX = DABS(DX(1))
      IX = IX + INCX
      DO 10 I = 2,N
         IF(DABS(DX(IX)).LE.DMAX) GO TO 5
         IDAMAX = I
         DMAX = DABS(DX(IX))
    5    IX = IX + INCX
   10 CONTINUE
      RETURN
C
C        CODE FOR INCREMENT EQUAL TO 1
C
   20 DMAX = DABS(DX(1))
      DO 30 I = 2,N
         IF(DABS(DX(I)).LE.DMAX) GO TO 30
         IDAMAX = I
         DMAX = DABS(DX(I))
   30 CONTINUE
      RETURN
      END

      SUBROUTINE  DSCAL(N,DA,DX,INCX)
C
C     SCALES A VECTOR BY A CONSTANT.
C     USES UNROLLED LOOPS FOR INCREMENT EQUAL TO ONE.
C     JACK DONGARRA, LINPACK, 3/11/78.
C
      DOUBLE PRECISION DA,DX(1)
      INTEGER I,INCX,M,MP1,N,NINCX
C
      IF(N.LE.0)RETURN
      IF(INCX.EQ.1)GO TO 20
C
C        CODE FOR INCREMENT NOT EQUAL TO 1
C
      NINCX = N*INCX
      DO 10 I = 1,NINCX,INCX
        DX(I) = DA*DX(I)
   10 CONTINUE
      RETURN
C
C        CODE FOR INCREMENT EQUAL TO 1
C
C
C        CLEAN-UP LOOP
C
   20 M = MOD(N,5)
      IF( M .EQ. 0 ) GO TO 40
      DO 30 I = 1,M
        DX(I) = DA*DX(I)
   30 CONTINUE
      IF( N .LT. 5 ) RETURN
   40 MP1 = M + 1
      DO 50 I = MP1,N,5
        DX(I) = DA*DX(I)
        DX(I + 1) = DA*DX(I + 1)
        DX(I + 2) = DA*DX(I + 2)
        DX(I + 3) = DA*DX(I + 3)
        DX(I + 4) = DA*DX(I + 4)
   50 CONTINUE
      RETURN
      END


