! -----------------------------------------------------------------------------
! -- 
! -- 
! -----------------------------------------------------------------------------

MODULE lin_alg

PUBLIC :: Solve_System

CONTAINS

! -----------------------------------------------------------------------------
! -- Algorithm is from Burden, Faires and Reynolds, Numerical Analysis,
!    Second Edition, Prindle, Weber and Schmidt, Algorithm 6.1 :


SUBROUTINE Solve_System(coeffs, rhs, soln)
  REAL, DIMENSION(:,:), INTENT(IN)  :: coeffs
  REAL, DIMENSION(:),   INTENT(IN)  :: rhs
  REAL, DIMENSION(:),   INTENT(OUT) :: soln
                                           ! ...assumed shape arrays: 
                                           !    subprogramme dummy arguments...

  INTEGER :: n
  INTEGER :: p, i, j, k

  ! -- the augmented matrix, A|b :
  REAL, DIMENSION(SIZE(coeffs,1),SIZE(coeffs,2)+1) :: Aug_A
  REAL, DIMENSION(SIZE(coeffs,1),SIZE(coeffs,2))   :: m
                                           ! ...automatic arrays: local,
                                           !    dynamically-sized...

  REAL :: hold, sum
 
  n = SIZE(coeffs,1)

  Aug_A(1:3,1:3) = coeffs(1:3,1:3)
  Aug_A(1:3,4) = rhs(1:3)

  PRINT *, "Aug_A is : "
  PRINT *, Aug_A(1,1), Aug_A(1,2), Aug_A(1,3), Aug_A(1,4)
  PRINT *, Aug_A(2,1), Aug_A(2,2), Aug_A(2,3), Aug_A(2,4)
  PRINT *, Aug_A(3,1), Aug_A(3,2), Aug_A(3,3), Aug_A(3,4)
  PRINT *

  ! -- Gaussian Elimination: 
  PRINT *, "Beginning Gaussian Elimination :  "
  PRINT *

  DO i = 1, n - 1

      ! -- determine p :
      DO p = 1, n
          IF (coeffs(p,i) /= 0.0) THEN
              EXIT
          END IF
      END DO

!      PRINT *, "p is : ", p
!      PRINT *

      IF (p /= 1) THEN
!          PRINT *, "p is NOT eq 1 : ", p
!          PRINT *
          ! -- swap Aug_A(i,*) and Aug_A(p,*) :
          DO j = 1, n + 1
              hold = Aug_A(i,j)
              Aug_A(i,j) = Aug_A(p,j)
              Aug_A(p,j) = hold
          END DO
      END IF
       
      DO j = i + 1, n
          m(j,i) = Aug_A(j,i)/Aug_A(i,i)
  
!          PRINT *, "i, j, m are : ", i, j, m(j,i)
!          PRINT *

          ! -- Aug_A(j,*) = Aug_A(j,*) - m(j,i)*Aug_A(i,*)
          DO k = 1, n + 1
              Aug_A(j,k) = Aug_A(j,k) - m(j,i)*Aug_A(i,k)
          END DO

          PRINT *, "Aug_A, row ", j, " now is :"
          PRINT *, Aug_A(j,1), Aug_A(j,2), Aug_A(j,3), Aug_A(j,4)
          PRINT *

      END DO
      
  END DO

  PRINT *, "Aug_A now is :"
  PRINT *, Aug_A(1,1), Aug_A(1,2), Aug_A(1,3), Aug_A(1,4)
  PRINT *, Aug_A(2,1), Aug_A(2,2), Aug_A(2,3), Aug_A(2,4)
  PRINT *, Aug_A(3,1), Aug_A(3,2), Aug_A(3,3), Aug_A(3,4)
  PRINT *

  ! -- Check :
  IF (Aug_A(n,n) == 0.0) THEN
      PRINT *, "No unique solution exists. "
      STOP
  END IF
  
  ! -- Backward substitution :

  PRINT *, "Beginning backwards substitution :  "
  PRINT *

  soln(n) = Aug_A(n,n+1)/Aug_A(n,n)

  DO i = n - 1, 1, -1
      sum = 0

      DO j = i + 1, n
          sum = sum + Aug_A(i,j) * soln(j)
      END DO

      soln(i) = (Aug_A(i,n+1) - sum) / Aug_A(i,i)
  END DO

  PRINT *, "Soln is :  "
  PRINT *, soln(1), soln(2), soln(3)
  PRINT *

  
END SUBROUTINE Solve_System


! -----------------------------------------------------------------------------
END MODULE lin_alg
! -----------------------------------------------------------------------------
