c*******************************
c Subroutine to solve matrix equation Ax=b
c using Gaussian Elimination
c By CFurse
c EE553 Homework #2
c Date: 4-4-94
c file: /grad/furse/ee553.dir/gauss.f
c*******************************
      program solve
      include 'gauss.par' ! parameter file
      real A(Mmax, Mmax+1) ! Augmented matrix
      real Aold(Mmax,Mmax+1)
      real x(Mmax) ! vector of unknowns
     
c------------------------------------------
c     --Open and read file containing matrix equation--
c     --first line: m n = actual size of matrix --
c     --Should be of the form: A|b--
c     --Matrix printed in augmented form A|b
c------------------------------------------
      open(60,file='matrix.in')
      read(60,*) m,n
      do i=1,m
      do j=1,n
         read(60,*) a(i,j)
      enddo
      enddo
      do i=1,n
         read(60,*) a(i,n+1)
      enddo
      do 11 i=1,m ! copy to keep original matrix
      do 11 j=1,n+1
         Aold(i,j) = a(i,j)
11    continue

c      write(6,*) 'Original matrix:'
c      call print_matrix(a,m,n+1) !Output initial matrix

c----------------------------------
c solve using gaussian elimination
c----------------------------------
      call gaussel(A,m,n,x)

c-----------------------------
c Check solution by multiplying original A by x
c and comparing to b
c---------------------------
      call check(Aold,m,n,x,err)

c---------------------------------
c  Solve matrix using SOR
c---------------------------------
      call SOR(Aold,m,n,x)
      call check(aold,m,n,x,err)


      end
c******************************************
c Solve Ax=b using Gaussian elimination with
c scaling and column pivoting
c*******************************************
      subroutine gaussel(A,m,n,x) 
      include 'gauss.par' ! parameter file
      real A(Mmax, Mmax+1) ! Augmented matrix
      real Aold(Mmax,Mmax+1)
       real x(Mmax) ! vector of unknowns

c---------------------------------
c Chop to Ndig digits
c  (This option allows you to reduce the precision of your
c   numerical calculations to test effect of round-off errors.)
c---------------------------------
      Ndig = 99 ! significant digits used in calculations
      call chop(a,m,n+1,Ndig) ! Chop matrix to Ndig digits
c     write(6,*) 'Chopped matrix:'
c     call print_matrix(a,m,n+1)

c---------------------------------
c     --Scale matrix--
c----------------------------------
      call scale(a,m,m)
c     write(6,*) 'Scaled matrix:'
c     call print_matrix(A,m,n+1)

c---------------------------------
c Gaussian Elimination with Pivoting
c---------------------------------
      do 100 L=1,m-1
c        --Pivot matrix--
         call pivot(a,m,n+1,L)

c        write(6,*) 'Pivoted matrix:'
c        call print_matrix(a,m,n+1) ! output pivoted matrix

c        --Perform Gaussian Elimination on one column--
         call eliminate(a,m,n+1,L)

c        write(6,*) 'Eliminated matrix:'
c        call print_matrix(a,m,n+1) ! output eliminated matrix

c        --Chop to Ndig after each calculation--
         call chop(a,m,n+1,Ndig) ! chop matrix
c        write(6,*) 'Chopped:'
c        call print_matrix(A,m,n+1)

100   continue ! end Gaussian Elimination

c------------------------------
c Solve using Back Substitution
c  A is mxm upper triagonal matrix
c  b (Ax=b) is in mx1 column of augmented matrix A
c------------------------------
      call back_subst(a,m,n,n+1,x)

      return
      end
c******************************
c Output matrix 
c  A is the matrix to be printed
c  m is number of rows
c  n is number of columns
c******************************
      subroutine print_matrix(a,m,n)
      include 'gauss.par'
      real a(Mmax,Mmax)
      do i=1,m 
         write(6,6) (a(i,j),j=1,n)
      enddo
c6    format(25(f7.4,1x))
 6    format(25(e12.4,1x))
      return
      end
c*******************************
c Pivot matrix on largest element in L column
c A is mxn matrix
c L is column to pivot on
c*******************************
      subroutine pivot(A,m,n,L)
      include 'gauss.par' ! parameter file
      real a(Mmax,Mmax+1)
     
c     --Find largest element in Lth column--
      imax = 0
      amax = 0
      do 10 i=L,m
         if (abs(a(i,L)).gt.amax) then ! larger element found
            amax = abs(a(i,L))
            imax = i
         endif
10    continue
  
      if (imax.eq.L) goto 999 ! no pivoting necessary

c     --Exchange rows L and imax
      do 20 j=L,n
         dummy = a(L,j)
         a(L,j) = a(imax,j)
         a(imax,j) = dummy
20    continue

999   return
      end
c*******************************
c Perform Gaussian Elimination
c A is mxn matrix to eliminate
c L is column to eliminate
c*******************************
      subroutine eliminate(A,m,n,L)
      include 'gauss.par' ! parameter file
      real a(Mmax,Mmax)

      do 10 i=L+1,m !rows
      do 10 j=n+1,L,-1 !columns
         a(i,j) = a(i,j) - a(L,j)*a(i,L)/a(L,L)
10    continue
      return
      end
c*******************************
c Solve Ax=b using Backsubstitution
c A is mxn matrix, augmented with b vectors
c x is vector of unknowns
c b is stored in n_b_col of augmented A matrix
c*******************************
      subroutine back_subst(A,m,n,n_b_col,x)
      include 'gauss.par'
      real A(Mmax,Mmax+1),x(Mmax)
      real b(Mmax)

      do 10 i=m,1,-1 ! Solve from bottom to top
         b(i) = a(i,n_b_col)
         sum = 0.
         do 5 j=i+1,n
            sum = sum + a(i,j)*x(j)
5        continue
         x(i) = (b(i) - sum)/a(i,i)
10    continue
      
c     --Output--
      open(70,file='gauss.out')
      write(70,*) 'x vector:'
      do 20 i=1,m
         write(70,*) x(i)
20    continue
      close(70)
      return
      end
c*****************************************
c Chop values to n digits
c A is an MAxNA matrix
c*****************************************
      subroutine chop(A,MA,NA,N)
      include 'gauss.par'
      real A(Mmax,Mmax+1)
      if (N.ge.99) goto 999 ! override chop

      do 10 i=1,MA
      do 10 j=1,NA
         x = a(i,j)
         if (x.eq.0) goto 10
         if (x.lt.0) then 
            x = -x
            i_sign = -1
         else
            i_sign = 1
         endif

      i_power = int(alog10(x)) ! x = d.ddddddd x 10**power
      if (i_power.lt.0) i_power = i_power -1
      x = x/(10.**i_power)
      x = x*(10.**(n-1)) ! x = ddddd.dddd
c       Number of digits      >     <
      x = float(int(x)) ! x = ddddd
      x = x/(10.**(n-1)) ! x = d.dddd
      x = x*(10.**i_power) ! x = d.dddd x 10.**i_power

         a(i,j)= x*i_sign

10    continue
999   return
      end
c*******************************
c Check solution by plugging into old matrix
c*******************************
      subroutine check(A,m,n,x,err)
      include 'gauss.par'
      real A(Mmax,Mmax+1),x(Mmax)
      
      errtotal = 0.
      do 10 i=1,m
         rowsum = 0.
         do 5 j=1,n
            rowsum = rowsum + a(i,j)*x(j)
5        continue
         bi = a(i,n+1)
         err_rowi = abs(bi - rowsum)
         errtotal = errtotal + err_rowi
10    continue
      write(6,*) 'total error =',errtotal
      write(6,*) 'average row error =',errtotal/m

c     --Define "error" measure for possible later use--
      err = errtotal
c     err = errtotal/m ! alternative
      return
      end
c**********************************
c Scale matrix
c**********************************
      subroutine scale(A,m,n)
      include 'gauss.par' ! parameter file
      real A(Mmax, Mmax+1) ! Augmented matrix
      real b(Mmax)
      integer OM(Mmax) ! order of magnitude of b vector

c     --Separate b vector--
      do 10 i=1,m
         b(i) = A(i,n+1)
10    continue

c     --find Order of Magnitude--
      maxOM = -99
      minOM = 99
      do 20 i=1,m
         if (b(i).eq.0) then ! override log function
            OM(i) = 0
         else
            b(i) = abs(b(i)) ! working with magnitude only
c            --OM = truncated integer (log base 10 (b))
            OM(i) = int(alog10(b(i)))
            if (OM(i).lt.0) OM(i) = OM(i)-1
            maxOM = max(maxOM,OM(i))
            minOM = min(minOM,OM(i))
         endif
20    continue

c     --Find maximum difference--
      diff = maxOM - minOM
      write(6,*) '  maxOM,minOM=',maxOM,minOM

c     --Scale if diff>2--
      if (diff.gt.2) then ! scale
         do 30 i=1,m
            do 30 j=1,n+1
               a(i,j) = a(i,j)*(10.**(-OM(i)))
30       continue
      endif

      return
      end
c**********************************
c Solve Ax=b Using SOR
c**********************************
      subroutine SOR(A,m,n,x)
      include 'gauss.par'
      real A(Mmax, Mmax+1) ! Augmented matrix
      real x(Mmax) ! vector of unknowns


      write(6,*) 'What w do you want to use?'
      read(5,*) w
c---------------------------------
c  Prompt for SOR variables
c---------------------------------
      write(6,*) 'Enter maximum number of iterations allowed'
      read(5,*) maxit
      write(6,*) 'Enter type of error'
      write(6,*) '  1: err = Ax-b'
      write(6,*) '  2: err = xchange'
      read(5,*) ierr
      write(6,*) 'Enter maximum error allowed'
      read(5,*) eps

c--------------------------------
c Set up initial guess for x
c----------------------------------
5     do 10 i=1,m
         x(i) = 0.
10    continue
  
c-----------------------------------
c Iterate
c-----------------------------------
      total_it = 0 ! total numer of iterations
1     do 20 k=1,maxit
         xchange = 0. ! amount of change in x vector
         total_it = total_it + 1
         do 50 i=1,m
            Ri = 0. ! Residual R(i)
            do 30 j=1,m
               Ri = Ri - a(i,j)*x(j)
30          continue
            Ri = Ri + a(i,n+1)
            Ri = Ri/a(i,i)
            xold = x(i)
            x(i) = x(i) + w*Ri
            xchange = xchange + abs(xold-x(i))
50       continue
         call check(a,m,n,x,err)
         if (ierr.eq.1) then
            if (abs(err).le.eps) goto 21
         else
             if (xchange.le.eps) goto 21
         endif
      write(6,*) (x(i),i=1,m)
20    continue

c-------------------------------
c Prompt for more iterations
c-------------------------------
100   write(6,*) 'Maximum iterations reached'
      write(6,*) '   Err, xchange=',err,xchange
      write(6,*) 'Enter additional iterations (0 to stop)'
      read(5,*) maxit
      if (maxit.gt.0) then
          goto 1
      else
         goto 200
      endif

c------------------------------
c Error bound reached
c------------------------------
21    write(6,*) 'Error bound reached'

c---------------------------
c Output
c---------------------------
200   write(6,*) 'SOR iterations =',total_it
         write(6,*) 'err,xchange =',err,xchange
      open(100,file='sor.out')
      write(6,*) 'x='
      do 110 i=1,m
         write(100,*) x(i)
110   continue
   
      return
      end