subroutine fitoff

      use fortranUtils
      use fitoffState
!c Define variables for main program; n = columns, m = rows
      IMPLICIT NONE      
      integer n,i, mmax
      integer k, iter
      real*8 threshx,threshy
      real*8 numerator, denominator, per_soln_length
      real*8 per_soln_length_last, delta_length
      logical change

      !!Arrays needed for processing
      double precision, allocatable, dimension(:,:) :: a,a_old
      double precision, allocatable, dimension(:) :: resx,resy
      double precision, allocatable, dimension(:) :: b_old,b
      double precision, allocatable, dimension(:) :: data,c,e
      double precision, allocatable, dimension(:,:) :: u
      integer, allocatable, dimension(:) :: s


      integer m, np, mp
      real*8 v(nmax,nmax),w(nmax),x_prev(nmax)

      integer n2,m2
      real*8 toler

      integer p
      real*8 rmsx,rmsy,xsdev,ysdev,sdev

      real*8 d(2),f(2),r_rotang,r_rtod,pi
      real*8 r_u(2),r_u2,r_rot(2,2),r_aff(2,2),r_scale1,r_scale2,r_skew
      logical sing

      pi = getPI()
      r_rtod = 180.d0/pi
      change = .true.

      per_soln_length = 0.

      !!Save initial number of lines in mmax
      mmax = 2*(imax+2)

      do i=1,imax
        x2o(i) = x1o(i) + dx(i)
        y2o(i) = y1o(i) + dy(i)

!!        print *, x1o(i), dx(i), y1o(i), dy(i), snr(i), r_covac(i), r_covdn(i),r_covx(i)
      enddo

!!      print *,'Params: ', nsig, maxrms, minpoint, l1norm
!!      print *, 'Iters:', miniter, maxiter
      if(imax .lt. 2) then
         print *,'fitoff.F:  Need at least 2 points'
         goto 105
      endif


      !!!Allocate the arrays
      allocate(a(mmax,nmax))
      allocate(a_old(mmax, nmax))
      allocate(b(mmax))
      allocate(b_old(mmax))
      allocate(c(mmax))
      allocate(resx(mmax))
      allocate(resy(mmax))
      allocate(e(mmax))
      allocate(u(mmax,nmax))
      allocate(s(mmax))
      allocate(data(mmax))

!c now setup matrices to solve overdetermined system of equations:
!c  [x2]   [m1 m2]   [x1]                      [m5]
!c  [  ] = [     ] x [  ]       +              [  ]
!c  [y2]   [m3 m4]   [y1]                      [m6]
!c
!c   ^      ^         ^                         ^
!c   |      |         X = solution vector       |
!c   B     A = affine                        translation
!c vector  transformation matrix               vector

      do iter =1,maxiter+1
         do k = 1,(2*imax)
            if (k.le.imax) then

               !matrix B
               b(k) = x2o(k)
               !Matrix A              
               a(k,1)=x1o(k)
               a(k,2)=y1o(k)
               a(k,3)=0.0d0
               a(k,4)=0.0d0
               a(k,5)=1.0
               a(k,6)=0.0d0
            else
               !matrix B
               b(k) = y2o(k-imax)
               !matrix A
               a(k,1)=0.0d0 
               a(k,2)=0.0d0
               a(k,3)=x1o(k-imax)
               a(k,4)=y1o(k-imax)
               a(k,5)=0.0d0
               a(k,6)=1.0
            endif
         end do

         np = nmax
         mp = mmax

         if (.not.(l1norm)) then
!c use L2 Norm to compute M matrix, from Numerical Recipes (p. 57)
!c n = number of columns, m = number of rows

            n = 6
            m = 2*imax
            
!c save the A matrix before using svdcmp, because it will be destroyed
            do k = 1,np
               do i = 1, m
                  a_old(i,k) = a(i,k)
               end do
            end do
            
            do k = 1,m
               b_old(k) = b(k)
            end do
          
            call dsvdcmp(a,m,n,mp,np,w,v)       
            
            do k = 1,n
               do i = 1, m
                  u(i,k) = a(i,k)
               end do
            end do
            
            call dsvbksb(u,w,v,m,n,mp,np,b,x)
            
         endif

!c use L1 norm to compute M matrix

         if (l1norm) then

            n  = 6
            m  = 2*imax
            n2 = n + 2
            m2 = m + 2
            toler = 1.0d-20

!c save b and a arrays since they are destroyed in subroutines
            do k = 1,n
               do i = 1, m
                  a_old(i,k) = a(i,k)
               end do
            end do
            
            do k = 1,m
               b_old(k) = b(k)
            end do
           
            call L1(M,N,M2,N2,A,B,TOLER,X,E,S,nmax,mmax)
            
         endif

!c multiple A and X together and compute residues
         call mmul(M,N,A_OLD,X,C,nmax,mmax)

         do k = 1,imax
               resx(k) = c(k) - b_old(k)
               resy(k) = c(k+imax) - b_old(k+imax)
         end do 
         
         p    = imax
         rmsy = 0.0d0
         rmsx = 0.0d0

!c compute statistics for x coordinates: standard deviation, mean, & rms
         do k = 1,imax
            data(k)= resx(k)
            rmsx = rmsx + resx(k)**2.
         end do
        
         call dmoment(data,p,sdev)
         rmsx = sqrt(rmsx/imax)  
         xsdev = sdev
        
!         print *, 'Sdev1 :', sdev

!c compute statistics for y coordinates
                
         do k = 1,(imax)
            data(k)= resy(k)
            rmsy = rmsy + resy(k)**2.
         end do
         
         call dmoment(data,p,sdev)
         rmsy = sqrt(rmsy/imax)  
         ysdev = sdev
         
         if (rmsx.gt.maxrms) then
            threshx = nsig*xsdev
         else
            threshx = 99999
         endif

         if (rmsy.gt.maxrms) then
            threshy = nsig*ysdev
         else
            threshy = 99999
         endif

!         print *, 'Threshs: ', threshx, threshy, sdev
!c determine whether to remove points for next iteration
         if ((rmsx.gt.maxrms).or.(rmsy.gt.maxrms)) then
!c determine which points to save for next iteration
            i = 0
            do k = 1,imax
               if ((abs(resx(k)).lt.threshx)
     >              .and.(abs(resy(k)).lt.threshy)) then
                  i = i + 1
                  x2o(i) = x2o(k)
                  x1o(i) = x1o(k)
                  y2o(i) = y2o(k)
                  y1o(i) = y1o(k)
                  snr(i) = snr(k) 
                  r_covac(i) = r_covac(k)
                  r_covdn(i) = r_covdn(k)
                  r_covx(i)  = r_covx(k)
               endif
            end do
            imax = i
         endif

!c if fewer than minpoints, quit and output warning
         if (imax.le.minpoint) goto 97

!c if rms fit is good enough, then quit program
         if ((rmsx.lt.maxrms).and.(rmsy.lt.maxrms)) goto 99

         if (iter.gt.1) then
            numerator   = 0.0d0
            denominator = 0.0d0
!c if the soln. length does not change between iterations, and solution fit
!c doesn't match specified parameters, then quit

            do k = 1,6
               numerator   = numerator + (x(k) - x_prev(k))**2.
               denominator = (x_prev(k))**2. + denominator
            end do
            per_soln_length = sqrt(numerator/denominator)*100.
         end if
         
         if (iter.ge.miniter)  then 
            delta_length = (per_soln_length - 
     >           per_soln_length_last)
            
            if ((delta_length.eq.0).and.
     >           ((rmsx.gt.maxrms).or.(rmsy.gt.maxrms))) then
               change = .false.
               goto 96
            endif
            
         endif

         per_soln_length_last = per_soln_length
         
         do k = 1,6
            x_prev(k) = x(k)
         end do


      end do

!c exceeded maximum number of iterations, output garbage
      print *,'WARNING: Exceeded maximum number of iterations.'

!c solution length not changing and fit parameters not achieved
 96   if (.not.change) then
         print *,'WARNING: Solution length is not changing,'
         print *,'but does not meet fit criteria'
      endif

!c Fewer than minimum number of points, output garbage
 97   if (imax.le.minpoint) then
         print *, 'WARNING: Fewer than minimum points, there are only'
     >        ,imax
      endif

 99      print *,' '
      if (((iter.lt.maxiter).and.(imax.gt.minpoint)).and.
     >        (change)) then
         print *, '     << Fitoff Program >>    '

         print *, ' '
         print *, 'Number of points remaining =', imax
         print *, ' '
         print *, 'RMS in X = ', rmsx, '  RMS in Y = ', rmsy
         print *, ' '

!c     Decompose matrix and examine residuals

         print *, ' '
         print *, '      Matrix  Analysis       '
         print *, ' '
         print *, ' Affine Matrix '
         print *, ' '
         print *, x(1), x(2)
         print *, x(3), x(4)

 101     format(1x,f15.10,1x,f15.10)
         print *, ' '

         print *, 'Translation Vector'
         print *, ' '
         print *, x(5),x(6)

 102     format(1x,f11.3,1x,f11.3,1x)
         
!c     decompose affine matrix to find rotation matrix using QR decomposition
!c     R is an upper triangular matrix and Q is an orthogonal matrix such
!c     that A = QR.  For our 2 X 2 matrix we can consider
!c      T
!c     Q A = R, where Q is a Housholder matrix, which is also a rotation matrix
!c     Subroutine qrdcmp ( Numerical recipes, pg 92) returns the u vectors
!c     used to compute Q1 in r_aff(1,1). r_aff(1,2), d(1) and d(2) are
!c     the diagonal terms of the R matrix, while r_aff(1,2) is the other
!c     point in the R matrix and these can be used to find the scale and
!c     skew terms 
         
         r_aff(1,1) = x(1)
         r_aff(1,2) = x(2)
         r_aff(2,1) = x(3)
         r_aff(2,2) = x(4)
         
         call qrdcmp(r_aff,2,2,f,d,sing)
         
         r_u(1) = r_aff(1,1)
         r_u(2) = r_aff(2,1)
         
         r_u2 = .5d0*(r_u(1)**2  + r_u(2)**2)
         
         r_rot(1,1) = (1.d0 - (r_u(1)**2/r_u2))
         r_rot(1,2) = -(r_u(1)*r_u(2))/r_u2
         r_rot(2,1) = -(r_u(1)*r_u(2))/r_u2
         r_rot(2,2) = (1.d0 - (r_u(2)**2/r_u2))
         
         if(d(1) .lt. 0)then
            r_rot(1,1) = -r_rot(1,1)
            r_rot(2,1) = -r_rot(2,1)
            d(1) = -d(1)
            r_aff(1,2) = -r_aff(1,2)
         elseif(d(2) .lt. 0)then
            r_rot(1,2) = -r_rot(1,2)
            r_rot(2,2) = -r_rot(2,2)
            d(2) = -d(2)
         endif         
         
         r_scale1 = abs(d(1))
         r_scale2 = abs(d(2))
         
         r_skew = r_aff(1,2)/d(1)
         
         r_rotang = atan2(r_rot(2,1),r_rot(1,1))
         
         print *, ' '
         print *, ' Rotation Matrix '
         print *, ' '
         print *, r_rot(1,1),r_rot(1,2)
         print *, r_rot(2,1),r_rot(2,2)

         print *, ' '

         print *, 'Rotation Angle (deg) = ',r_rotang*r_rtod
         print *, ' '
         print *, ' Axis Scale Factors'

         print *, ' '
         print *, r_scale1,r_scale2
 103     format(1x,f11.7,1x,f11.7)
         print *,' '
         print *,' Skew Term'

         print *, ' '
         print *, r_skew

 104     format(1x,f11.7)

      endif

      !!Deallocate arrays
      deallocate(a,a_old)
      deallocate(b,b_old)
      deallocate(c,resx,resy)
      deallocate(e,u,s,data)

 105  end

!C     ALGORITHM 478 COLLECTED ALGORITHMS FROM ACM.
!C     ALGORITHM APPEARED IN COMM. ACM, VOL. 17, NO. 06,
!C     P. 319.
!C  NOTE:  this version is modified to allow double precision
      SUBROUTINE L1(M,N,M2,N2,A,B,TOLER,X,E,S,NMAX,MMAX)       
!C THIS SUBROUTINE USES A MODIFICATION OF THE SIMPLEX METHOD
!C OF LINEAR PROGRAMMING TO CALCULATE AN L1 SOLUTION TO AN
!C OVER-DETERMINED SYSTEM OF LINEAR EQUATIONS.
!C DESCRIPTION OF PARAMETERS.
!C M      NUMBER OF EQUATIONS.
!C N      NUMBER OF UNKNOWNS (M.GE.N).
!C M2     SET EQUAL TO M+2 FOR ADJUSTABLE DIMENSIONS.
!C N2     SET EQUAL TO N+2 FOR ADJUSTABLE DIMENSIONS.
!C A      TWO DIMENSIONAL REAL ARRAY OF SIZE (M2,N2).
!C        ON ENTRY, THE COEFFICIENTS OF THE MATRIX MUST BE
!C        STORED IN THE FIRST M ROWS AND N COLUMNS OF A.
!C        THESE VALUES ARE DESTROYED BY THE SUBROUTINE.
!C B      ONE DIMENSIONAL REAL ARRAY OF SIZE M. ON ENTRY, B
!C        MUST CONTAIN THE RIGHT HAND SIDE OF THE EQUATIONS.
!C        THESE VALUES ARE DESTROYED BY THE SUBROUTINE.
!C TOLER  A SMALL POSITIVE TOLERANCE. EMPIRICAL EVIDENCE
!C        SUGGESTS TOLER=10**(-D*2/3) WHERE D REPRESENTS
!C        THE NUMBER OF DECIMAL DIGITS OF ACCURACY AVALABLE
!C        (SEE DESCRIPTION).
!C X      ONE DIMENSIONAL REAL ARRAY OF SIZE N. ON EXIT, THIS
!C        ARRAY CONTAINS A SOLUTION TO THE L1 PROBLEM.
!C E      ONE DIMENSIONAL REAL ARRAY OF SIZE M. ON EXIT, THIS
!C        ARRAY CONTAINS THE RESIDUALS IN THE EQUATIONS.
!C S      INTEGER ARRAY OF SIZE M USED FOR WORKSPACE.
!C ON EXIT FROM THE SUBROUTINE, THE ARRAY A CONTAINS THE
!C FOLLOWING INFORMATION.
!C A(M+1,N+1)  THE MINIMUM SUM OF THE ABSOLUTE VALUES OF
!C             THE RESIDUALS.
!C A(M+1,N+2)  THE RANK OF THE MATRIX OF COEFFICIENTS.
!C A(M+2,N+1)  EXIT CODE WITH VALUES.
!C             0 - OPTIMAL SOLUTION WHICH IS PROBABLY NON-
!C                 UNIQUE (SEE DESCRIPTION).
!C             1 - UNIQUE OPTIMAL SOLUTION.
!C             2 - CALCULATIONS TERMINATED PREMATURELY DUE TO
!C                 ROUNDING ERRORS.
!C A(M+2,N+2)  NUMBER OF SIMPLEX ITERATIONS PERFORMED.
      Implicit None
      INTEGER m,m1,m2,n,n1,n2,NMAX,MMAX
      double precision SUM, MIN, MAX
      double precision :: A(Mmax,Nmax)
      double precision :: X(Nmax), E(Mmax), B(Mmax)
!!      REAL*8 MIN, MAX, A(Mmax,Nmax), X(Nmax), E(Mmax), B(Mmax)
      integer :: S(Mmax)
      INTEGER OUT
      LOGICAL STAGE, TEST
!c define variables in program whose type were assumed implicitly
      integer i,j,kr,k,kl,kount,in,l
      double precision d, pivot,toler,big
!C BIG MUST BE SET EQUAL TO ANY VERY LARGE REAL CONSTANT.
!C ITS VALUE HERE IS APPROPRIATE FOR THE IBM 370.
!c      DATA BIG/1.E75/
!C ITS VALUE HERE IS APPROPRIATE FOR SGI
      DATA BIG/1.E38/
!C INITIALIZATION.
      M1 = M + 1
      N1 = N + 1
      DO 10 J=1,N
        A(M2,J) = J
        X(J) = 0.0d0
   10 CONTINUE
      DO 40 I=1,M
        A(I,N2) = N + I
        A(I,N1) = B(I)
        IF (B(I).GE.0.0d0) GO TO 30
        DO 20 J=1,N2
          A(I,J) = -A(I,J)
   20   CONTINUE
   30   E(I) = 0.0d0
   40 CONTINUE
!C COMPUTE THE MARGINAL COSTS.
      DO 60 J=1,N1
        SUM = 0.0D0
        DO 50 I=1,M
          SUM = SUM + A(I,J)
   50   CONTINUE
        A(M1,J) = SUM
   60 CONTINUE
!C STAGE I.
!C DETERMINE THE VECTOR TO ENTER THE BASIS.
      STAGE = .TRUE.
      KOUNT = 0
      KR = 1
      KL = 1
   70 MAX = -1.
      DO 80 J=KR,N
        IF (ABS(A(M2,J)).GT.N) GO TO 80
        D = ABS(A(M1,J))
        IF (D.LE.MAX) GO TO 80
        MAX = D
        IN = J
   80 CONTINUE
      IF (A(M1,IN).GE.0.0d0) GO TO 100
      DO 90 I=1,M2
        A(I,IN) = -A(I,IN)
   90 CONTINUE
!C DETERMINE THE VECTOR TO LEAVE THE BASIS.
  100 K = 0
      DO 110 I=KL,M
        D = A(I,IN)
        IF (D.LE.TOLER) GO TO 110
        K = K + 1
        B(K) = A(I,N1)/D
        S(K) = I
        TEST = .TRUE.
  110 CONTINUE
  120 IF (K.GT.0) GO TO 130
      TEST = .FALSE.
      GO TO 150
  130 MIN = BIG
      DO 140 I=1,K
        IF (B(I).GE.MIN) GO TO 140
        J = I
        MIN = B(I)
        OUT = S(I)
  140 CONTINUE
      B(J) = B(K)
      S(J) = S(K)
      K = K - 1
!C CHECK FOR LINEAR DEPENDENCE IN STAGE I.
  150 IF (TEST .OR. .NOT.STAGE) GO TO 170
      DO 160 I=1,M2
        D = A(I,KR)
        A(I,KR) = A(I,IN)
        A(I,IN) = D
  160 CONTINUE
      KR = KR + 1
      GO TO 260
  170 IF (TEST) GO TO 180
      A(M2,N1) = 2.
      GO TO 350
  180 PIVOT = A(OUT,IN)
      IF (A(M1,IN)-PIVOT-PIVOT.LE.TOLER) GO TO 200
      DO 190 J=KR,N1
        D = A(OUT,J)
        A(M1,J) = A(M1,J) - D - D
        A(OUT,J) = -D
  190 CONTINUE
      A(OUT,N2) = -A(OUT,N2)
      GO TO 120
!C PIVOT ON A(OUT,IN).
  200 DO 210 J=KR,N1
        IF (J.EQ.IN) GO TO 210
        A(OUT,J) = A(OUT,J)/PIVOT
  210 CONTINUE
!c      DO 230 I=1,M1
!c        IF (I.EQ.OUT) GO TO 230
!c        D = A(I,IN)
!c        DO 220 J=KR,N1
!c          IF (J.EQ.IN) GO TO 220
!c          A(I,J) = A(I,J) - D*A(OUT,J)
!c  220   CONTINUE
!c  230 CONTINUE
!c impliment time saving change suggested in Barrodale and Roberts - collected 
!c algorithms from CACM
      DO 220 J = KR,N1
         IF (J.EQ.IN) GO TO 220
         CALL COL(A(1,J),A(1,IN),A(OUT,J),M1,OUT)
 220      CONTINUE
      DO 240 I=1,M1
        IF (I.EQ.OUT) GO TO 240
        A(I,IN) = -A(I,IN)/PIVOT
  240 CONTINUE
      A(OUT,IN) = 1./PIVOT
      D = A(OUT,N2)
      A(OUT,N2) = A(M2,IN)
      A(M2,IN) = D
      KOUNT = KOUNT + 1
      IF (.NOT.STAGE) GO TO 270
!C INTERCHANGE ROWS IN STAGE I.
      KL = KL + 1
      DO 250 J=KR,N2
        D = A(OUT,J)
        A(OUT,J) = A(KOUNT,J)
        A(KOUNT,J) = D
  250 CONTINUE
  260 IF (KOUNT+KR.NE.N1) GO TO 70
!C STAGE II.
      STAGE = .FALSE.
!C DETERMINE THE VECTOR TO ENTER THE BASIS.
  270 MAX = -BIG
      DO 290 J=KR,N
        D = A(M1,J)
        IF (D.GE.0.0d0) GO TO 280
        IF (D.GT.(-2.)) GO TO 290
        D = -D - 2.
  280   IF (D.LE.MAX) GO TO 290
        MAX = D
        IN = J
  290 CONTINUE
      IF (MAX.LE.TOLER) GO TO 310
      IF (A(M1,IN).GT.0.0d0) GO TO 100
      DO 300 I=1,M2
        A(I,IN) = -A(I,IN)
  300 CONTINUE
      A(M1,IN) = A(M1,IN) - 2.
      GO TO 100
!C PREPARE OUTPUT.
  310 L = KL - 1
      DO 330 I=1,L
        IF (A(I,N1).GE.0.0d0) GO TO 330
        DO 320 J=KR,N2
          A(I,J) = -A(I,J)
  320   CONTINUE
  330 CONTINUE
      A(M2,N1) = 0.0d0
      IF (KR.NE.1) GO TO 350
      DO 340 J=1,N
        D = ABS(A(M1,J))
        IF (D.LE.TOLER .OR. 2.-D.LE.TOLER) GO TO 350
  340 CONTINUE
      A(M2,N1) = 1.
  350 DO 380 I=1,M
        K = A(I,N2)
        D = A(I,N1)
        IF (K.GT.0) GO TO 360
        K = -K
        D = -D
  360   IF (I.GE.KL) GO TO 370
        X(K) = D
        GO TO 380
  370   K = K - N
        E(K) = D
  380 CONTINUE
      A(M2,N2) = KOUNT
      A(M1,N2) = N1 - KR
      SUM = 0.0D0
      DO 390 I=KL,M
        SUM = SUM + A(I,N1)
  390 CONTINUE
      A(M1,N1) = SUM
      RETURN
      END

      SUBROUTINE COL(V1,V2,MLT,M1,IOUT)
      IMPLICIT NONE
      INTEGER M1,I,IOUT
      REAL*8 V1(M1),V2(M1),MLT
      DO 1 I = 1,M1
         IF (I.EQ.IOUT) GO TO 1
         V1(I)=V1(I)-V2(I)*MLT
 1       CONTINUE
         RETURN
         END

!c The following three programs are used to find the L2 norm
      SUBROUTINE dsvbksb(u,w,v,m,n,mp,np,b,x)
      Implicit None
      INTEGER m,mp,n,np
!!      REAL*8 b(mmax),u(mmax,nmax),v(nmax,nmax),w(nmax),x(nmax)
      double precision :: b(mp),w(np),x(np)
      double precision :: u(mp,np),v(np,np)
      INTEGER i,j,jj
      DOUBLE PRECISION s,tmp(np)
      do 12 j=1,n
        s=0.0d0
        if(w(j).ne.0.0d0)then
          do 11 i=1,m
            s=s+u(i,j)*b(i)
11        continue
          s=s/w(j)
        endif
        tmp(j)=s
12    continue
      do 14 j=1,n
        s=0.0d0
        do 13 jj=1,n
          s=s+v(j,jj)*tmp(jj)
13      continue
        x(j)=s
14    continue
      return
      END

      SUBROUTINE dsvdcmp(a,m,n,mp,np,w,v)
      Implicit None
      INTEGER m,mp,n,np
!c      DOUBLE PRECISION a(mp,np),v(np,np),w(np)
!!      REAL*8 a(mmax,nmax),v(nmax,nmax),w(nmax)
      double precision :: w(np)
      double precision :: a(mp,np),v(np,np)
      double precision :: rv1(np)
!CU    USES dpythag
      INTEGER i,its,j,jj,k,l,nm
      DOUBLE PRECISION anorm,c,f,g,h,s,scale,x,y,z,dpythag

      g=0.0d0
      scale=0.0d0
      anorm=0.0d0
      do 25 i=1,n
        l=i+1
        rv1(i)=scale*g
        g=0.0d0
        s=0.0d0
        scale=0.0d0
        if(i.le.m)then
          do 11 k=i,m
            scale=scale+abs(a(k,i))
11        continue
          if(scale.ne.0.0d0)then
            do 12 k=i,m
              a(k,i)=a(k,i)/scale
              s=s+a(k,i)*a(k,i)
12          continue
            f=a(i,i)
            g=-sign(sqrt(s),f)
            h=f*g-s
            a(i,i)=f-g
            do 15 j=l,n
              s=0.0d0
              do 13 k=i,m
                s=s+a(k,i)*a(k,j)
13            continue
              f=s/h
              do 14 k=i,m
                a(k,j)=a(k,j)+f*a(k,i)
14            continue
15          continue
            do 16 k=i,m
              a(k,i)=scale*a(k,i)
16          continue
          endif
        endif
        w(i)=scale *g
        g=0.0d0
        s=0.0d0
        scale=0.0d0
        if((i.le.m).and.(i.ne.n))then
          do 17 k=l,n
            scale=scale+abs(a(i,k))
17        continue
          if(scale.ne.0.0d0)then
            do 18 k=l,n
              a(i,k)=a(i,k)/scale
              s=s+a(i,k)*a(i,k)
18          continue
            f=a(i,l)
            g=-sign(sqrt(s),f)
            h=f*g-s
            a(i,l)=f-g
            do 19 k=l,n
              rv1(k)=a(i,k)/h
19          continue
            do 23 j=l,m
              s=0.0d0
              do 21 k=l,n
                s=s+a(j,k)*a(i,k)
21            continue
              do 22 k=l,n
                a(j,k)=a(j,k)+s*rv1(k)
22            continue
23          continue
            do 24 k=l,n
              a(i,k)=scale*a(i,k)
24          continue
          endif
        endif
        anorm=max(anorm,(abs(w(i))+abs(rv1(i))))
25    continue
      do 32 i=n,1,-1
        if(i.lt.n)then
          if(g.ne.0.0d0)then
            do 26 j=l,n
              v(j,i)=(a(i,j)/a(i,l))/g
26          continue
            do 29 j=l,n
              s=0.0d0
              do 27 k=l,n
                s=s+a(i,k)*v(k,j)
27            continue
              do 28 k=l,n
                v(k,j)=v(k,j)+s*v(k,i)
28            continue
29          continue
          endif
          do 31 j=l,n
            v(i,j)=0.0d0
            v(j,i)=0.0d0
31        continue
        endif
        v(i,i)=1.0d0
        g=rv1(i)
        l=i
32    continue
      do 39 i=min(m,n),1,-1
        l=i+1
        g=w(i)
        do 33 j=l,n
          a(i,j)=0.0d0
33      continue
        if(g.ne.0.0d0)then
          g=1.0d0/g
          do 36 j=l,n
            s=0.0d0
            do 34 k=l,m
              s=s+a(k,i)*a(k,j)
34          continue
            f=(s/a(i,i))*g
            do 35 k=i,m
              a(k,j)=a(k,j)+f*a(k,i)
35          continue
36        continue
          do 37 j=i,m
            a(j,i)=a(j,i)*g
37        continue
        else
          do 38 j= i,m
            a(j,i)=0.0d0
38        continue
        endif
        a(i,i)=a(i,i)+1.0d0
39    continue
      do 49 k=n,1,-1
        do 48 its=1,30
          do 41 l=k,1,-1
            nm=l-1
            if((abs(rv1(l))+anorm).eq.anorm)  goto 2
            if((abs(w(nm))+anorm).eq.anorm)  goto 1
41        continue
1         c=0.0d0
          s=1.0d0
          do 43 i=l,k
            f=s*rv1(i)
            rv1(i)=c*rv1(i)
            if((abs(f)+anorm).eq.anorm) goto 2
            g=w(i)
            h=dpythag(f,g)
            w(i)=h
            h=1.0d0/h
            c= (g*h)
            s=-(f*h)
            do 42 j=1,m
              y=a(j,nm)
              z=a(j,i)
              a(j,nm)=(y*c)+(z*s)
              a(j,i)=-(y*s)+(z*c)
42          continue
43        continue
2         z=w(k)
          if(l.eq.k)then
            if(z.lt.0.0d0)then
              w(k)=-z
              do 44 j=1,n
                v(j,k)=-v(j,k)
44            continue
            endif
            goto 3
          endif
!          if(its.eq.30) pause 'no convergence in svdcmp'
          if(its.eq.30) then
             write (6,*) 'fitoff: no convergence in svdcmp, quitting'
             stop
          endif
          x=w(l)
          nm=k-1
          y=w(nm)
          g=rv1(nm)
          h=rv1(k)
          f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0d0*h*y)
          g=dpythag(f,1.0d0)
          f=((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x
          c=1.0d0
          s=1.0d0
          do 47 j=l,nm
            i=j+1
            g=rv1(i)
            y=w(i)
            h=s*g
            g=c*g
            z=dpythag(f,h)
            rv1(j)=z
            c=f/z
            s=h/z
            f= (x*c)+(g*s)
            g=-(x*s)+(g*c)
            h=y*s
            y=y*c
            do 45 jj=1,n
              x=v(jj,j)
              z=v(jj,i)
              v(jj,j)= (x*c)+(z*s)
              v(jj,i)=-(x*s)+(z*c)
45          continue
            z=dpythag(f,h)
            w(j)=z
            if(z.ne.0.0d0)then
              z=1.0d0/z
              c=f*z
              s=h*z
            endif
            f= (c*g)+(s*y)
            x=-(s*g)+(c*y)
            do 46 jj=1,m
              y=a(jj,j)
              z=a(jj,i)
              a(jj,j)= (y*c)+(z*s)
              a(jj,i)=-(y*s)+(z*c)
46          continue
47        continue
          rv1(l)=0.0d0
          rv1(k)=f
          w(k)=x
48      continue
3       continue
49    continue
      return
      END

      FUNCTION dpythag(a,b)
      Implicit None
!c      DOUBLE PRECISION a,b,dpythag
!c      DOUBLE PRECISION absa,absb
      Real*8 a,b,dpythag
      Real*8 absa,absb
      absa=abs(a)
      absb=abs(b)
      if(absa.gt.absb)then
        dpythag=absa*sqrt(1.0d0+(absb/absa)**2)
      else
        if(absb.eq.0.0d0)then
          dpythag=0.0d0
        else
          dpythag=absb*sqrt(1.0d0+(absa/absb)**2)
        endif
      endif
      return
      END

      SUBROUTINE MMUL (M,N,A_OLD,X,C,nmax,mmax)
      Implicit None

!C     *****PARAMETERS:
      Integer nmax, mmax, M, N
      double precision :: a_old(MMAX,NMAX)
      double precision :: x(NMAX),c(MMAX)
!!      REAL*8 a_old(mmax,nmax),x(nmax),c(mmax)

      INTEGER NA,NB,NC,L

!C     *****LOCAL VARIABLES:
      INTEGER I,K

      NA = M
      NB = nmax
      NC = M
      N  = nmax
      L  = 1

!C     *****SUBROUTINES CALLED:
!C     NONE
!C
!C     ------------------------------------------------------------------
!C
!C     *****PURPOSE:
!C     THIS SUBROUTINE COMPUTES THE MATRIX PRODUCT A*B AND STORES THE
!C        RESULT IN THE ARRAY C.  A IS M X N, B IS N X L, AND C IS
!C        M X L.  THE ARRAY C MUST BE DISTINCT FROM BOTH A AND B.
!C
!C     *****PARAMETER DESCRIPTION:
!C     ON INPUT:
!C        NA    ROW DIMENSION OF THE ARRAY CONTAINING A AS DECLARED
!C              IN THE CALLING PROGRAM DIMENSION STATEMENT;
!C
!C        NB    ROW DIMENSION OF THE ARRAY CONTAINING B AS DECLARED
!C              IN THE CALLING PROGRAM DIMENSION STATEMENT;
!C
!C        NC    ROW DIMENSION OF THE ARRAY CONTAINING C AS DECLARED
!C              IN THE CALLING PROGRAM DIMENSION STATEMENT;
!C
!C        L     NUMBER OF COLUMNS OF THE MATRICES B AND C;
!C
!C        M     NUMBER OF ROWS OF THE MATRICES A AND C;
!C
!C        N     NUMBER OF COLUMNS OF THE MATRIX A AND NUMBER OF ROWS
!C              OF THE MATRIX B;
!C
!C        A     AN M X N MATRIX;
!C
!C        B     AN N X L MATRIX.
!C
!C     ON OUTPUT:
!C
!C        C     AN M X L ARRAY CONTAINING A*B.
!C
!C     *****HISTORY:
!C     WRITTEN BY ALAN J. LAUB (ELEC. SYS. LAB., M.I.T., RM. 35-331,
!C     CAMBRIDGE, MA 02139,  PH.: (617)-253-2125), SEPTEMBER 1977.
!C     MOST RECENT VERSION: SEP. 21, 1977.
!C
!C     ------------------------------------------------------------------
!C
         DO 10 I=1,M
            C(I)=0.0d0
10       CONTINUE
         DO 30 K=1,N
            DO 20 I=1,M
               C(I)=C(I)+a_old(I,K)*x(K)
20          CONTINUE
30       CONTINUE
      RETURN

      END

!c Modify Numerical Recipes program moment.f to compute only 
!c standard deviation and allow double precision
      SUBROUTINE dmoment(data,p,sdev)
      Implicit None
      INTEGER p
      REAL*8 adev,ave,curt,sdev,skew,var,data(p)
      INTEGER j
      REAL*8 t,s,ep
!      if(p.le.1)pause 'p must be at least 2 in moment'
      if(p.le.1) then
         write (6,*) 'fitoff: p must be at least 2 in moment'
         write (6,*) '      culling points failed'
         stop
      endif
      s=0.0d0
      do 11 j=1,p
        s=s+data(j)
11    continue
      ave=s/p
      adev=0.0d0
      var=0.0d0
      skew=0.0d0
      curt=0.0d0
      ep=0.
      do 12 j=1,p
        s=data(j)-ave
        t=s*s
        var=var+t
12    continue
      adev=adev/p
      var=(var-ep**2/p)/(p-1)
      sdev=sqrt(var)
      return
      END

!c This program is used to find the rotation matrix from the affine matrix
      SUBROUTINE qrdcmp(a,n,np,c,d,sing)
      INTEGER n,np
      REAL*8 a(np,np),c(n),d(n)
      LOGICAL sing
      INTEGER i,j,k
      REAL*8 scale,sigma,sum,tau
      sing=.false.
      scale=0.
      do 17 k=1,n-1
        do 11 i=k,n
          scale=max(scale,abs(a(i,k)))
11      continue
        if(scale.eq.0.)then
          sing=.true.
          c(k)=0.
          d(k)=0.
        else
          do 12 i=k,n
            a(i,k)=a(i,k)/scale
12        continue
          sum=0.
          do 13 i=k,n
            sum=sum+a(i,k)**2
13        continue
          sigma=sign(sqrt(sum),a(k,k))
          a(k,k)=a(k,k)+sigma
          c(k)=sigma*a(k,k)
          d(k)=-scale*sigma
          do 16 j=k+1,n
            sum=0.
            do 14 i=k,n
              sum=sum+a(i,k)*a(i,j)
14          continue
            tau=sum/c(k)
            do 15 i=k,n
              a(i,j)=a(i,j)-tau*a(i,k)
15          continue
16        continue
        endif
17    continue
      d(n)=a(n,n)
      if(d(n).eq.0.)sing=.true.
      return
      END
!C  (C) Copr. 1986-92 Numerical Recipes Software $23#1yR.3Z9.