! copyright 2005,  J. E. Akin, all rights reserved.
!MODULE PRECISION_MODULE
!  IMPLICIT NONE
!  INTEGER, PARAMETER :: SP = KIND (1.0)  ! Single precision
!  INTEGER, PARAMETER :: DP = KIND (1.d0) ! Double precision
!END MODULE PRECISION_MODULE

SUBROUTINE FULL_SQ_FACTOR (N_D_FRE, S)        
! * * * * * * * * * * * * * * * * * * * * * * * * *
! CROUT FACTORIZATION OF FULL EQS, S = L*DIAG*LT
!          ALSO  SEE FULL_SQ_SOLVE
! NOTE: INEFFICIENT STORAGE, ONLY UPPER TRI USED
! * * * * * * * * * * * * * * * * * * * * * * * * *
Use Precision_Module
 IMPLICIT NONE
 INTEGER,  INTENT(IN)    :: N_D_FRE
 REAL(DP), INTENT(INOUT) :: S (N_D_FRE, N_D_FRE)
 INTEGER   :: I, J, K
 REAL(DP)  :: SUM
                                 
! DIAG    = DIAGONAL MATRIX STORED ON S; DIAG (1) = S (1,1)
! L       = LOWER TRIANGULAR MATRIX STORED ON S
! N_D_FRE = TOTAL NUMBER OF DEGREES OF FREEDOM
! S       = ORIGINAL FULL SYMMETRIC MATRIX
                                         
  DO I = 2, N_D_FRE
    DO J = 1, (I - 1)
      SUM = 0.D0
      IF ( J > 1 ) THEN
!        FACTOR COLUMN
        DO K = 1, (J - 1)
          SUM = SUM + S (K, K) * S (I, K) * S (J, K)
        END DO
      END IF
      S (I, J) = (S (I, J) - SUM) / S (J, J)
    END DO
!    FACTOR DIAGONAL
    SUM = 0.D0
    DO K = 1, (I - 1)
      SUM = SUM + S (K, K) * S (I, K) **2
    END DO
    S (I, I) = S (I, I) - SUM
  END DO
END SUBROUTINE FULL_SQ_FACTOR
                     
SUBROUTINE FULL_SYSTEM_MPC (N_D_FRE, S, C, NCD, NDX, A)
! * * * * * * * * * * * * * * * * * * * * * * * * * * * *
! APPLY LINEAR CONSTRAINTS TO FULL SYMMETRIC EQUATIONS
!             S*D = C,  WITH CONSTRAINT(S) ON D
! D(NDX(1))+A(1)*D(NDX(2))+...A(NCD-1)*D(NDX(NCD))=A(NCD)
! * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!    REVISED 2/96, ADDED AVE TO AVOID ILL-CONDITIONING
Use Precision_Module
 IMPLICIT NONE
 REAL(DP), PARAMETER     :: ZERO = 0.D0
 INTEGER,  INTENT(IN)    :: N_D_FRE, NCD, NDX (NCD)
 REAL(DP), INTENT(IN)    :: A (NCD)
 REAL(DP), INTENT(INOUT) :: S (N_D_FRE, N_D_FRE), C (N_D_FRE)
 REAL(DP)  :: AVE, CR, E, ESC, SRR, SRR_AVE
 INTEGER   :: I, J, K, L, NR
                                         
! NCD     = TOTAL NUMBER OF DOF IN CONSTRAINT EQUATION
! N_D_FRE = TOTAL NUMBER OF DEGREES OF FREEDOM
! NDX(I)  = SYS DOF NOS OF CONSTRAINT TERM I
! NR      = REDUNDANT DEGREE OF FREEDOM = NDX(1)
! A(J)    = NORMALIZED COEFF OF (J+1) TERM, A0 = 1.0
! AVE     = ILL-CONDITION CONTROL
                                                   
!          Three Partitions of Equations
! C = SYSTEM COLUMN MATRIX  | Sii Sir Sid | | Di | | Ci |
! S = SYSTEM SQUARE MATRIX  | Sri Srr Srd |*| Dr |=| Cr |
! r-redundant, d-dependent  | Sdi Sdr Sdd | | Dd | | Cd |
                                                         
!  SET CONSTANTS
  E = A (NCD) ; NR = NDX (1) ; SRR = S (NR, NR) ; CR = C (NR)

!  GET AVERAGE STIFFNESS TO AVOID ILL-CONDITIONING
  AVE = 0.D0
  DO J = 1, NCD
    AVE = AVE + S (NDX (J), NDX (J) )
  END DO
  AVE = AVE / NCD
  IF ( AVE < 1.0 ) AVE = 1.D0
  SRR_AVE = SRR + AVE
  ESC     = E * SRR_AVE - CR

!  FORM MODIFIED COLUMN MATRIX, Cx = Cx - E*Sxr, in row x
  IF ( E /= ZERO ) THEN
    DO I = 1, N_D_FRE
      C (I) = C (I) - E * S (I, NR)
    END DO
  END IF

!  ADDITIONAL COLUMN CHANGES FOR Cd AND Cr
  IF ( NCD > 1 .AND. ESC /= ZERO ) THEN
    DO K = 2, NCD
      C (NDX (K) ) = C (NDX (K) ) + A (K - 1) * ESC
    END DO
  END IF
  C (NR) = E * AVE

!    *** SQUARE MATRIX COLUMN MODIFICATIONS ***
  IF ( NCD > 1 ) THEN
!  FORM Sid,  BEGIN Sdd
    DO K = 2, NCD
      J = NDX (K)
      DO I = 1, N_D_FRE
        S (I, J) = S (I, J) - S (I, NR) * A (K - 1)
      END DO
    END DO
!  COMPLETE Sdd
    DO K = 2, NCD
      I = NDX (K)
      DO L = 2, NCD
        J = NDX (L)
        S (I, J) = S (I, J) + SRR_AVE * A (K - 1) * A (L - 1) &
                            - A (K - 1) * S (J, NR)
      END DO
    END DO
!  ROW OPERATIONS
    DO K = 2, NCD
      I = NDX (K)
      DO L = 2, NCD
        J = NDX (L)
        S (J, I) = S (I, J)
      END DO
    END DO
    DO K = 2, NCD
      J = NDX (K)
      DO I = 1, N_D_FRE
        S (J, I) = S (I, J)
      END DO
    END DO
  END IF

!        *** INSERT CONSTRAINT EQUATION ***
! WARNING: NEXT LOOP NOT VALID FOR COUPLED LINEAR CONSTRAINTS
  DO I = 1, N_D_FRE
    S (I, NR) = ZERO
    S (NR, I) = ZERO
  END DO
  S (NR, NR) = AVE
  IF ( NCD > 1 ) THEN
    DO K = 2, NCD
      I = NDX (K)
      S (I, NR) = A (K - 1) * AVE
      S (NR, I) = A (K - 1) * AVE
    END DO

!   *** MODIFICATIONS COMPLETED, CHECK DIAGONAL ***
    DO K = 2, NCD
      I = NDX (K)
      IF ( S (I, I) <= ZERO) WRITE (6, *)                          &
          'FULL_SYSTEM_MPC: Negative diagonal for constraint set ',&
           NDX
    END DO
  END IF
END SUBROUTINE FULL_SYSTEM_MPC
                     
SUBROUTINE FULL_SYSTEM_ROW_SAVE (N_REACT, MAX_NP, N_D_FRE, &
                                 N_G_DOF, I_BC, S, C)
! * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
! SAVE INDEPENDENT REACTION EQUATIONS FROM FULL SYSTEM: S*D = C
!     (BEFORE CALLING FULL_SYSTEM_MPC TO ADD CONSTRAINTS TO D)
! * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Use Precision_Module
Use Interface_Header
 IMPLICIT NONE
 INTEGER,  INTENT(IN) :: N_REACT, MAX_NP, N_D_FRE, N_G_DOF
 INTEGER,  INTENT(IN) :: I_BC (MAX_NP)
 REAL(DP), INTENT(IN) :: S (N_D_FRE, N_D_FRE), C (N_D_FRE)
 INTEGER  :: INDEX (N_G_DOF), KODES (N_G_DOF)  ! TEMPOARY
 INTEGER  :: IG, INDX, J, K
                                                
! C       = SYSTEM COLUMN MATRIX
! D       = SYSTEM COLUMN MATRIX OF "UNKNOWNS"
! INDEX   = SYSTEM DEGREE OF FREEDOM NUMBERS ARRAY
! I_BC    = NODAL BOUNDARY RESTRAINT INDICATOR
! KODES   = LIST OF DOF RESTRAINT INDICATORS AT A NODE
! MAX_NP  = NUMBER OF SYSTEM NODES
! N_G_DOF = NUMBER OF DOF PER NODE
! N_REACT = SEQUENTIAL UNIT TO STORE BINARY REACTION DATA
! N_D_FRE = TOTAL NUMBER OF EQUATIONS
! S       = FULL SYSTEM SQUARE MATRIX
                                    
!  LOOP OVER EQUATIONS FOR POSSIBLE ESSENTIAL BC FLAG
  DO J = 1, MAX_NP                       ! AT EACH NODE
    IF ( I_BC (J) > 0) THEN              ! NON_ZERO FLAG
      CALL PT_CODE (J, I_BC (J), KODES)  ! EXPAND FLAG ENTRIES
!      EXPAND EQUATION NUMBERS AT NODE J INTO INDEX VECTOR
      !b INDEX = (/ (N_G_DOF * (J - 1) + IG, IG = 1, N_G_DOF) /)
      INDEX = GET_INDEX_AT_PT (J)
      DO IG = 1, N_G_DOF                 ! FOR EACH EQUATION
        IF ( KODES (IG) == 1) THEN       ! FOUND TYPE_ONE BC
!          WRITE NODE, PARAMETER, RANGE OF NON-ZEROS
          WRITE (N_REACT) J, IG, 1, N_D_FRE
!          SAVE ROW OF EQUILIBRIUM EQUATION WITH A BC
          INDX = INDEX (IG)              ! EQUATION NUMBER
          DO K = 1, N_D_FRE              ! FOR ALL COLUMNS
            WRITE (N_REACT) S (INDX, K)  ! COLUMN IN THE ROW
          END DO ! OVER COLUMNS
          WRITE (N_REACT) C (INDX)       ! ROW FROM R.H.S.
        END IF ! PARAMETER HAS BC
      END DO ! FOR PARAMETERS AT NODE
    END IF ! SOME BC AT THIS NODE
  END DO ! FOR EVERY NODE
END SUBROUTINE FULL_SYSTEM_ROW_SAVE
                     
SUBROUTINE FULL_SQ_SOLVE (N_D_FRE, S, C, D) 
! * * * * * * * * * * * * * * * * * * * * * * * * *
! FORWARD, BACK CROUT SUBSTITUTION FOR D, S*D = C
!          SEE FULL_SQ_FACTOR
! * * * * * * * * * * * * * * * * * * * * * * * * *
Use Precision_Module
 IMPLICIT NONE
 INTEGER,  INTENT(IN)  :: N_D_FRE
 REAL(DP), INTENT(IN)  :: S (N_D_FRE, N_D_FRE), C (N_D_FRE)
 REAL(DP), INTENT(OUT) :: D (N_D_FRE)
 INTEGER   :: I, K
 REAL(DP)  :: SUM
                               
! C       = SOURCE OR FORCE VECTOR
! D       = SOLUTION VECTOR, RETURNED
! DIA     = DIAGONAL MATRIX STORED ON S
! L       = LOWER TRIANGULAR MATRIX STORED ON S
! N_D_FRE = TOTAL NUMBER OF DEGREES OF FREEDOM
! S       = FULL FACTORED MATRIX OF L*DIA*L^T
                                            
!  FORWARD SUBSTITUTION
  DO I = 1, N_D_FRE
    SUM = 0.D0
    IF ( I > 1 ) THEN
      DO K = 1, (I - 1)
        SUM = SUM + D (K) * S (I, K)
      END DO
    END IF
    D (I) = C (I) - SUM
  END DO
        
!  BACK SUBSTITUTION
  DO I = N_D_FRE, 1, - 1
    SUM = 0.D0
    IF ( I < N_D_FRE ) THEN
      DO K = 1, (N_D_FRE-I)
        SUM = SUM + D (I + K) * S (I + K, I)
      END DO
    END IF
    D (I) = D (I) / S (I, I) - SUM
  END DO
END SUBROUTINE FULL_SQ_SOLVE
                     
!SUBROUTINE STORE_COLUMN (N_D_FRE, N_EL_FRE, INDEX, C, CC)
!! * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!! STORE ELEMENT COLUMN MATRIX IN SYSTEM COLUMN MATRIX
!! * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!Use Precision_Module ! Defines DP for double precision
! IMPLICIT NONE
! INTEGER,  INTENT(IN)    :: N_D_FRE, N_EL_FRE
! INTEGER,  INTENT(IN)    :: INDEX (N_EL_FRE)
! REAL(DP), INTENT(IN)    :: C     (N_EL_FRE)
! REAL(DP), INTENT(INOUT) :: CC    (N_D_FRE)
! INTEGER :: I, J
!
!! N_D_FRE  = NO DEGREES OF FREEDOM IN THE SYSTEM
!! N_EL_FRE = NUMBER OF DEGREES OF FREEDOM PER ELEMENT
!! INDEX    = SYSTEM DOF NOS OF THE ELEMENT DOF
!! C        = ELEMENT COLUMN MATRIX
!! CC       = SYSTEM COLUMN MATRIX
!
!  DO I = 1, N_EL_FRE                     ! ELEMENT ROW
!    J = INDEX (I)                        ! SYSTEM ROW NUMBER
!    IF ( J > 0 ) CC (J) = CC (J) + C (I) ! SKIP INACTIVE ROW
!  END DO ! OVER ROWS
!END SUBROUTINE STORE_COLUMN

SUBROUTINE STORE_FULL_SQUARE (N_D_FRE, N_EL_FRE, S, SS, INDEX)
! * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!     STORE ELEMENT SQ MATRIX IN FULL SYSTEM SQ MATRIX
! * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Use Precision_Module ! Defines DP for double precision
 IMPLICIT NONE
 INTEGER,  INTENT(IN)    :: N_D_FRE, N_EL_FRE
 INTEGER,  INTENT(IN)    :: INDEX (N_EL_FRE)
 REAL(DP), INTENT(IN)    :: S     (N_EL_FRE, N_EL_FRE)
 REAL(DP), INTENT(INOUT) :: SS    (N_D_FRE,  N_D_FRE)
 INTEGER   :: I, II, J, JJ
                                       
! N_D_FRE  = TOTAL NO OF SYSTEM DEGREES OF FREEDOM
! N_EL_FRE = NO DEGREES OF FREEDOM PER ELEMENT
! INDEX    = SYSTEM DOF NOS OF ELEMENT PARAMETERS
! S        = FULL ELEMENT SQUARE MATRIX
! SS       = FULL SYSTEM SQUARE MATRIX
                                               
  DO I = 1, N_EL_FRE                     ! ELEMENT ROW
    II = INDEX (I)                       ! SYSTEM ROW NUMBER
    IF ( II > 0 ) THEN                   ! SKIP INACTIVE ROW
      DO J = 1, N_EL_FRE                 ! ELEMENT COLUMN
        JJ = INDEX (J)                   ! SYSTEM COLUMN
        IF ( JJ > 0 ) SS (II, JJ) = SS (II, JJ) + S (I, J)
      END DO ! OVER COLUMNS
    END IF
  END DO ! OVER ROWS
END SUBROUTINE STORE_FULL_SQUARE
                     
SUBROUTINE MODIFY_FULL_SQ (N_D_FRE, FULL, EQ_N, VALUE) 
! * * * * * * * * * * * * * * * * * * * * * * * * * * * *
! TYPE_1 CONSTRAINT EFFECT ON FULL SQUARE MATRIX ONLY 
! USE VALUE = 1 FOR MASS MATRIX, > N_D_FRE FOR STIFFNESS
! * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Use Precision_Module
 IMPLICIT NONE
 INTEGER,  INTENT(IN)    :: N_D_FRE, EQ_N
 REAL(DP), INTENT(IN)    :: VALUE  
 REAL(DP), INTENT(INOUT) :: FULL (N_D_FRE, N_D_FRE)
                                         
! N_D_FRE = TOTAL NUMBER OF DEGREES OF FREEDOM
! EQ_N     = EQUATION NUMBER WITH ESSENTIAL BC
! VALUE   = DUMMY NON-ZERO FLAG TO APPEAR IN EIGENVALUES
                                                   
  FULL (:, EQ_N) = 0.d0
  FULL (EQ_N, :) = 0.d0
  FULL (EQ_N, EQ_N) = VALUE
END SUBROUTINE MODIFY_FULL_SQ

SUBROUTINE JACOBI_GENERAL (A, B, X, EIGV, N, NS_MAX, NS_USED) 
! ......................................................................
!       F77 BY K.J. BATHE "F.E. PROCEDURES", PRENTICE-HALL, 1996        
!       F90 TRANSLATION BY J.E. AKIN, 2004                              
! .                                                                     
! .           SOLVE THE GENERALIZED EIGENPROBLEM (A - Lamda B) V = 0
! .           USING THE GENERALIZED JACOBI ITERATION
! .                                                                     
! . - -  INPUT VARIABLES -                                              
! .        A(N,N)  =  STIFFNESS MATRIX (ASSUMED POSITIVE DEFINITE)      
! .        B(N,N)  =  MASS MATRIX (ASSUMED POSITIVE DEFINITE)           
! .        X(N,N)  =  STORAGE FOR EIGENVECTORS                          
! .        EIGV(N) =  STORAGE FOR EIGENVALUES                           
! .        N       =  ORDER OF MATRICES A AND B                         
! .        NS_MAX  =  MAXIMUM NUMBER OF SWEEPS ALLOWED (USUALLY 15)
! .                                                                     
! . - -  OUTPUT - -                                                     
! .        A(N,N)  =  DIAGONALIZED STIFFNESS MATRIX                     
! .        B(N,N)  =  DIGONALIZED MASS MATRIX                           
! .        X(N,N)  =  EIGENVECTORS STORED COLUMNWISE                    
! .        EIGV(N) =  EIGENVALUES                                       
! .                                                                     
! . - - AUTOMATIC - -
! .        D(N)    =  WORKING VECTOR                                    
! .
! . - - CONTROLS - -
! .        N_PRT   =  UNIT NUMBER USED FOR OUTPUT                       
! .        ROT_TOL =  CONVERGENCE TOLERANCE  (USUALLY 10.**-12)   
! .        IF_PR   =  FLAG FOR PRINTING DURING ITERATION                
! .           EQ 0  NO PRINTING                                         
! .           EQ 1  INTERMEDIATE RESULTS ARE PRINTED                    
! ......................................................................
Use Precision_Module
IMPLICIT NONE
REAL(DP), PARAMETER :: EPS_MIN = EPSILON (1.d0)
REAL(DP) :: ROT_TOL 

INTEGER,  INTENT (IN)    :: N, NS_MAX
INTEGER,  INTENT (OUT)   :: NS_USED   
REAL(DP), INTENT (INOUT) :: A (N, N), B (N, N)
REAL(DP), INTENT (OUT)   :: X (N, N), EIGV (N)

INTEGER  :: N_PRT = 6, IF_PR = 0, N_WARN = 0
INTEGER  :: I, J, JJ, J_M1, J_P1, K, K_M1, K_P1, NS
REAL(DP) :: D (N), DEN, DIF, D1, D2, SCALE
REAL(DP) :: EPS, EPS_A, EPS_B, TOL
REAL(DP) :: AB_CH, AJJ_CH, AKK_CH, CA, CHECK, CG, SQCH
REAL(DP) :: AB, AJ, AJJ, AK, AKK, BJ, BK, XJ, XK
LOGICAL  :: DEBUG_JACOBI, NO_SCALE = .TRUE.

 ROT_TOL = SQRT ( EPS_MIN ) 
 DEBUG_JACOBI = .false.
 IF ( DEBUG_JACOBI ) THEN
   PRINT *, 'Entering JACOBI_GENERAL: Size, Sweeps', N, NS_MAX
   CALL RPRINT (A, N, N, 1)
   CALL RPRINT (B, N, N, 1)
   IF_PR = 1
 END IF
! ......................................................................
!                                                                       
!     INITIALIZE EIGENVALUE AND EIGENVECTOR MATRICES                    
!                                                                       
 X = 0.d0
 DO I = 1, N 
   IF ( A (I, I) <= 0.d0 ) THEN
     PRINT *, 'FATAL I, A(I,I) =', I, A (I, I)
     STOP 'JACOBI: INPUT MATRIX A NOT POSITIVE DEFINITE'
   ELSEIF ( B (I, I) <= 0.d0 ) THEN
     PRINT *, 'FATAL I, B(I,I) =', I, B (I, I)
     STOP 'JACOBI: INPUT MATRIX B NOT POSITIVE DEFINITE'
   END IF ! Invalid data
   D (I) = A (I, I) / B (I, I) 
   X (I, I) = 1.d0 
 END DO ! over I
 EIGV = D
 NS_USED = 0
 IF ( N == 1 ) THEN
   PRINT *,'WARNING: scalar argument passed to JACOBI_GENERAL'
   N_WARN = N_WARN + 1
   RETURN ! not an eigenproblem
 END IF
!                                                                       
!     INITIALIZE SWEEP COUNTER AND BEGIN ITERATION                      
!                                                                       
 IF ( DEBUG_JACOBI ) PRINT *, 'Begin sweeps'
 SWEEP: DO NS = 1, NS_MAX  ! ----> ----> ----> ----> ----> ---->
   IF ( IF_PR == 1) WRITE (N_PRT, 2000) NS 
   2000 FORMAT ('SWEEP NUMBER IN *JACOBI* = ',I8) 
!                                                                       
!     CHECK IF PRESENT OFF-DIAGONAL ELEMENT IS LARGE ENOUGH TO          
!     REQUIRE ZEROING                                                   
!                                                                       
   EPS = (0.01d0) ** (NS * 2) 
   IF ( EPS < EPS_MIN ) EPS = EPS_MIN 
   DO J = 1, (N-1)
     JJ = J + 1 
     DO K = JJ, N 
       IF ( NO_SCALE ) THEN
         EPS_A = A (J, K) **2
         EPS_B = B (J, K) **2
         IF  ( EPS_A < EPS * (A (J, J) * A (K, K))       &
         .AND. EPS_B < EPS * (B (J, J) * B (K, K)) ) CYCLE            
       ELSE
         EPS_A = (A (J, K) / A (J, J) ) * (A (J, K) / A (K, K) ) 
         EPS_B = (B (J, K) / B (J, J) ) * (B (J, K) / B (K, K) ) 
         IF ( EPS_A < EPS .AND. EPS_B < EPS ) CYCLE            
       END IF ! NO_SCALE
!                                                                       
!     IF ZEROING IS REQUIRED, CALCULATE THE ROTATION MATRIX             
!     ELEMENTS CA AND CG                                                
!                                                                       
       AKK = A (K, K) * B (J, K) - B (K, K) * A (J, K) 
       AJJ = A (J, J) * B (J, K) - B (J, J) * A (J, K) 
       AB  = A (J, J) * B (K, K) - A (K, K) * B (J, J) 
       IF ( NO_SCALE ) THEN
         SCALE = 1.d0
         CHECK = (AB * AB + 4.d0 * AKK * AJJ) / 4.d0
       ELSE
         SCALE  = A (K, K) * B (K, K) 
         AB_CH  = AB  / SCALE 
         AKK_CH = AKK / SCALE 
         AJJ_CH = AJJ / SCALE 
         CHECK  = (AB_CH * AB_CH + 4.d0 * AKK_CH * AJJ_CH) / 4.d0 
       END IF ! NO_SCALE
       IF ( CHECK < 0.d0 ) STOP 'JACOBI 2: MATRIX NOT POSITIVE DEFINITE'
       SQCH = SCALE * SQRT (CHECK) 
       D1 = AB / 2.d0 + SQCH 
       D2 = AB / 2.d0 - SQCH 
       DEN = D1 
       IF ( ABS (D2)  > ABS (D1) ) DEN = D2 
       IF ( DEN == 0.d0 ) THEN 
         CA = 0.d0 
         CG = - A (J, K) / A (K, K) 
       ELSE 
         CA = AKK / DEN 
         CG = - AJJ / DEN 
       END IF ! DEN
!                                                                       
!     PERFORM THE GENERALIZED ROTATION TO ZERO ELEMENTS                 
!	                                                                
       IF ( N > 2 ) THEN
         J_P1 = J + 1 
         J_M1 = J - 1 
         K_P1 = K + 1 
         K_M1 = K - 1 
         IF ( J_M1 >= 1 ) THEN 
           DO I = 1, J_M1 
             AJ = A (I, J) 
             BJ = B (I, J) 
             AK = A (I, K) 
             BK = B (I, K) 
             A (I, J) = AJ + CG * AK 
             B (I, J) = BJ + CG * BK 
             A (I, K) = AK + CA * AJ	 
             B (I, K) = BK + CA * BJ	 
           END DO ! over I
         END IF ! J_M1
         IF ( K_P1 <= N ) THEN 
           DO I = K_P1, N 
             AJ = A (J, I) 
             BJ = B (J, I) 
             AK = A (K, I) 
             BK = B (K, I) 
             A (J, I) = AJ + CG * AK 
             B (J, I) = BJ + CG * BK 
             A (K, I) = AK + CA * AJ 
             B (K, I) = BK + CA * BJ 
           END DO ! over I
         END IF ! K_P1
         IF ( J_P1 <= K_M1 ) THEN 
           DO I = J_P1, K_M1 
             AJ = A (J, I) 
             BJ = B (J, I) 
             AK = A (I, K) 
             BK = B (I, K) 
             A (J, I) = AJ + CG * AK 
             B (J, I) = BJ + CG * BK 
             A (I, K) = AK + CA * AJ 
             B (I, K) = BK + CA * BJ 
           END DO ! over I
         END IF ! J_P1
       END IF ! N > 2
       AK = A (K, K) 
       BK = B (K, K) 
       A (K, K) = AK + 2.d0 * CA * A (J, K) + CA * CA * A (J, J) 
       B (K, K) = BK + 2.d0 * CA * B (J, K) + CA * CA * B (J, J) 
       A (J, J) = A (J, J) + 2.d0 * CG * A (J, K) + CG * CG * AK 
       B (J, J) = B (J, J) + 2.d0 * CG * B (J, K) + CG * CG * BK 
       A (J, K) = 0.d0 
       B (J, K) = 0.d0 
!                                                                       
!     UPDATE EIGENVECTOR MATRIX AFTER EACH ROTATION                     
!                                                                       
       DO I = 1, N 
         XJ = X (I, J) 
         XK = X (I, K) 
         X (I, J) = XJ + CG * XK	 
         X (I, K) = XK + CA * XJ	 
       END DO ! over I
     END DO ! over K
   END DO ! over J
!	                                                                
!     UPDATE THE EIGENVALUES AFTER EACH SWEEP	                        
!                                                                       
   DO I = 1, N 
     IF ( NO_SCALE ) THEN
       IF ( B (I, I) <= 0.d0 ) STOP                  &
          & 'JACOBI 3: MATRICES NOT POSITIVE DEFINITE'
     ELSE
       IF ( A (I, I) <= 0.d0 .OR. B (I, I) <= 0.d0 ) STOP &
          & 'JACOBI 3: MATRICES NOT POSITIVE DEFINITE'
     END IF ! NO_SCALE
     EIGV (I) = A (I, I) / B (I, I) 
   END DO ! over I
   IF ( IF_PR == 1 ) THEN 
     WRITE (N_PRT, 2030) 
     2030 FORMAT (' CURRENT EIGENVALUES IN *JACOBI* ARE',/) 
     WRITE (N_PRT, 2010) (EIGV (I), I = 1, N) 
     2010 FORMAT (' ',6E20.12) 
   END IF ! print
!	                                                                
!     CHECK FOR CONVERGENCE	                                        
!                                                                       
   DO I = 1, N 
     TOL = ROT_TOL * D (I) 
     DIF = ABS (EIGV (I) - D (I) ) 
     IF ( DIF > TOL ) THEN      
!                                                                       
!     UPDATE D MATRIX AND START NEW SWEEP, IF ALLOWED                   
!                                                                       
        D = EIGV
        IF ( NS < NS_MAX ) CYCLE SWEEP
          GO TO 2
     END IF ! dif
   END DO ! over I
!                                                                       
!  CHECK OFF-DIAGONAL ELEMENTS TO SEE IF ANOTHER SWEEP IS NEEDED     
!                                                                       
   EPS = ROT_TOL**2 
   IF ( EPS < EPS_MIN ) EPS = EPS_MIN 
   DO J = 1, (N-1)
     JJ = J + 1 
     DO K = JJ, N 
       IF ( NO_SCALE ) THEN
         EPS_A = A (J, K) **2
         EPS_B = B (J, K) **2
         IF  ( EPS_A < EPS * (A (J, J) * A (K, K))       &
         .AND. EPS_B < EPS * (B (J, J) * B (K, K)) ) CYCLE            
       ELSE
         EPS_A = (A (J, K) / A (J, J) ) * (A (J, K) / A (K, K) ) 
         EPS_B = (B (J, K) / B (J, J) ) * (B (J, K) / B (K, K) ) 
         IF ( EPS_A < EPS .AND. EPS_B < EPS ) CYCLE                
       END IF ! NO_SCALE
!                                                                       
!     UPDATE D MATRIX AND START NEW SWEEP, IF ALLOWED                   
!                                                                       
         D = EIGV
         IF ( NS < NS_MAX ) CYCLE SWEEP
           GO TO 2
     END DO ! over K
   END DO ! over J
!                                                                       
!   FILL OUT BOTTOM TRIANGLE OF RESULTANT MATRICES, 
!   SCALE EIGENVECTORS, AND EXIT
!                                                                       
2  CONTINUE
if ( DEBUG_JACOBI ) print *, 'At 2, NS = ', NS
   DO I = 1, N 
     DO J = I, N 
       A (J, I) = A (I, J) 
       B (J, I) = B (I, J) 
     END DO ! over J
   END DO ! over I
   DO J = 1, N 
     X (:, J) = X (:, J) / SQRT (B (J, J) )
   END DO ! over J
!
!     NOTE NUMBER OF REQUIRED SWEEPS
!
   NS_USED = NS
   IF ( NS_USED >= NS_MAX ) PRINT *, &
     & 'NOTE: MAX SWEEPS USED IN JACOBI_GENERAL'
   IF ( DEBUG_JACOBI ) PRINT *, 'SWEEPS USED = ', NS_USED
   RETURN 
 END DO SWEEP   ! <---- <---- <---- <---- <---- <---- <----
 !b IF ( DEBUG_JACOBI ) PRINT *, 'End sweeps', NS
END SUBROUTINE JACOBI_GENERAL

SUBROUTINE ASSEMBLE_FULL_EQS (NODES, S_FULL, CC, X, DD_OLD, &
                              ITER, M_FULL)
! * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
!           ASSEMBLE FULL SYSTEM EQUATIONS AND STORE POST
!                     SOLUTION ELEMENT DATA
! * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Use System_Constants
Use Elem_Type_Data       ! for PT (LT_PARM, LT_QP), WT (LT_QP),
                         ! G (LT_GEOM, LT_QP), H (LT_FREE),
                         ! DLG (LT_PARM, LT_GEOM, LT_QP),
                         ! DLH (LT_PARM, LT_FREE, LT_QP),
                         ! S (LT_FREE, LT_FREE), C (LT_FREE),
                         ! EL_M (LT_FREE, LT_FREE), DIAG_M (LT_FREE),
                         ! D (LT_FREE), ELEM_NODES (LT_N)
Use Sys_Properties_Data
Use Interface_Header
 IMPLICIT NONE
!                   ALWAYS USED
 INTEGER,  INTENT(IN)    :: ITER  ! Current iteration number
 INTEGER,  INTENT(IN)    :: NODES (L_S_TOT, NOD_PER_EL)
 REAL(DP), INTENT(IN)    :: X (MAX_NP, N_SPACE) !b
 REAL(DP), INTENT(IN)    :: DD_OLD (N_D_FRE)    !b
 REAL(DP), INTENT(INOUT) :: CC (N_D_FRE)
 REAL(DP), INTENT(INOUT) :: M_FULL (N_D_FRE, N_D_FRE)
 REAL(DP), INTENT(INOUT) :: S_FULL (N_D_FRE, N_D_FRE)
!                 Locals
 INTEGER  :: IE, LT, J
 REAL(DP) :: COL_SUMS   ! Error checking

!                   VARIABLES:
! CC         = SYSTEM EQUATIONS COLUMN MATRIX
! COL_SUMS   + SUM OF ALL COLUMN MATRICES
! D          = NODAL PARAMETERS ASSOCIATED WITH AN ELEMENT
! DD_OLD     = SYSTEM NODAL PARAMETERS FROM LAST ITERATION
! DELETED    = TRUE IF THE ELEMENT HAS BEEN DELETED
! DGH        = GLOBAL DERIVATIVES INTERPOLATION FUNCTIONS
! DIAG_M     = DIAGONALIZED ELEMENT MASS MATRIX (LT_FREE)
! DLG        = LOCAL DERIVATIVES GEOMETRIC INTERPOLATION
! DLH        = LOCAL DERIVATIVES INTERPOLATION FUNCTIONS
! E          = CONSTITUTIVE MATRIX
! EB         = PRODUCT OF E*B
! ELEM_NODES = THE NOD_PER_EL INCIDENCES OF THE ELEMENT
! EL_M       = ELEMENT MASS MATRIX
! FLT_EL     = REAL PROPERTIES OF ELEMENTS
! FLT_MISC   = MISCELLANEOUS REAL PROPERTIES OF SYSTEM
! FLT_NP     = REAL PROPERTIES OF SYSTEM NODES
! G          = GEOMETRIC INTERPOLATION FUNCTIONS
! H          = SOLUTION INTERPOLATION FUNCTIONS
! H_INTG     = INTEGRAL OF INTERPOLATION FUNCTIONS
! ITER       = CURRENT ITERATION NUMBER (not used)
! INDEX      = SYSTEM DOF NUMBERS ASSOCIATED WITH ELEMENT
! L_HOMO     = 1, IF ELEMENT PROPERTIES ARE HOMOGENEOUS
! L_S_TOT    = TOTAL NUMBER OF ELEMENTS & THEIR SEGMENTS
! LP_FIX     = SYSTEM ARRAY OF INTEGER ELEM PROPERTIES
! L_PROP     = ARRAY INTEGER ELEMENT PROPERTIES
! LP_TEST    > 0, IF ELEMENT PROPERTIES HAVE BEEN DEFINED
! L_SHAPE    = SHAPE FLAG FOR QUADRATURE RULE SELECTION
! LT_DATA    = ARRAY OF CONSTANTS FOR EACH ELEMENT TYPE
! LT         = ELEMENT TYPE NUMBER (IF USED)
! LT_QP      = NUMBER OF QUADRATURE PTS FOR ELEMENT TYPE
! LT_USER    = OPTIONAL USER CODE ASSOCIATED WITH TYPE
! L_TYPE     = ELEMENT TYPE NUMBER
! MAX_NP     = NUMBER OF SYSTEM NODES
! MISC_FIX   = MISCELLANEOUS INTEGER SYSTEM PROPERTIES
! NOD_PER_EL = NUMBER OF NODES PER ELEMENT
! N_COEFF    = TOTAL NUMBER OF TERMS IN SS
! N_D_FRE    = TOTAL NUMBER OF SYSTEM DEGREES OF FREEDOM
! N_ELEMS    = NUMBER OF ELEMENTS
! N_EL_FRE   = NUMBER OF DEGREES OF FREEDOM PER ELEMENT
! N_G_DOF    = NUMBER OF PARAMETERS PER NODE
! N_GEOM     = NUMBER OF GEOMETRY NODES
! N_HOMO     = 1, IF NODAL PROPERTIES ARE HOMOGENEOUS
! N_ITER     = NO. OF ITERATIONS TO BE RUN (USUALLY 1)
! N_MAT      = NUMBER OF MATERIAL TYPES
! NODES      = NODAL INCIDENCES OF ALL ELEMENTS
! N_PARM     = DIMENSION OF PARAMETRIC SPACE
! NP_FIX     = INTEGER PROPERTIES AT ALL NODES
! N_QP       = NUMBER OF QUADRATURE POINTS, >= LT_QP
! N_R_B      = NUMBER OF ROWS IN B AND E MATRICES
! N_SPACE    = DIMENSION OF SPACE
! N_FILE1    = UNIT FOR POST SOLUTION MATRICES STORAGE
! N_FILE2    = OPTIONAL UNITS FOR USER (USED WHEN > 0)
! N_FILE3,4  = OPTIONAL UNITS FOR USER (USED WHEN > 0)
! NUL_COL    = 1, IF ELEMENT COLUMN MATRIX IS ALWAYS ZERO
! PT         = QUADRATURE COORDINATES
! S          = ELEMENT SQUARE MATRIX
! M_FULL     = SYSTEM EQUATIONS FULL MASS MATRIX
! S_FULL     = SYSTEM EQUATIONS FULL STIFFNESS MATRIX
! STRAIN     = STRAIN OR GRADIENT VECTOR
! STRAIN_0   = INITIAL STRAIN OR GRADIENT VECTOR
! STRESS     = STRESS VECTOR
! WT         = QUADRATURE WEIGHTS
! X          = COORDINATES OF SYSTEM NODES
! XYZ        = SPACE COORDINATES AT A POINT
 
! GENERATE ELEMENT EQUATIONS & POST SOLUTION MATRICES
  WRITE (N_PRT, "(/, 'BEGINNING ASSEMBLY')")
  IF ( I_BUG > 0 .AND. N_ITER > 1 ) PRINT *, 'ITERATION ', ITER
  IF ( U_FLUX  > 0 ) REWIND U_FLUX  
  IF ( N_FILE1 > 0 ) REWIND N_FILE1 
  IF ( N_FILE2 > 0 ) REWIND N_FILE2 
  COL_SUMS = 0.d0

  LT = 1 ; LAST_LT = 0 ! INITIALIZE ELEMENT TYPE

!     LOOP OVER ALL STANDARD ELEMENTS FIRST, 
!     THEN ANY OF THEIR BOUNDARY SEGMENT ELEMENTS

  WRITE (N_PRT, "('BEGINNING STANDARD ELEMENT ASSEMBLY')")
  DO IE = 1, L_S_TOT ! for elements and any boundary segments
!b call at(125)
    IF ( DELETED (IE) ) CYCLE ! to skip deleted element
    CALL SET_THIS_ELEMENT_NUMBER (IE)
    IF ( N_MIXED > 0 .AND. IE == (N_ELEMS + 1) ) WRITE (N_PRT, &
      "('BEGINNING MIXED_BC SEGMENTS ASSEMBLY')")
    IF ( N_F_SEG > 0 .AND. IE == (N_ELEMS + N_MIXED + 1) ) &
      WRITE (N_PRT, "('BEGINNING FLUX BOUNDARY SEGMENTS ASSEMBLY')")

!-->     GET ELEMENT TYPE NUMBER
    IF ( N_L_TYPE > 1 ) LT = L_TYPE (IE)
!    SAME AS LAST TYPE ?
    IF ( LT /= LAST_LT ) THEN ! this is a new type, or first element
!b call at(137)
!      GET CONTROLS FOR THIS TYPE, AND ALLOCATE ITS ARRAYS
      CALL SET_ELEM_TYPE_INFO (LT) ! including LAST_LT
    END IF ! a new element type

!        INITIALIZE AND CHECK IF ACTIVE
    !b    call list_type_alloc_status !b
    S = 0.d0 ; EL_M = 0.d0 ; C = 0.d0

!-->     EXTRACT ELEMENT NODE NUMBERS
      ELEM_NODES = GET_ELEM_NODES (IE, LT_N, NODES)    

!-->     CALCULATE DEGREE OF FREEDOM NUMBERS
      INDEX = GET_ELEM_INDEX (LT_N, ELEM_NODES)   

!-->     GENERATE ELEMENT PROBLEM DEPENDENT MATRICES
      IF ( EXAMPLE > 0 ) THEN ! Select an old example
        CALL GENERATE_EL_MATRICES_NEW (IE, X, DD_OLD)
        IF ( DEBUG_EL_COL .OR. DEBUG_EL_SQ ) THEN
          PRINT *, 'Exiting GENERATE_EL_MATRICES_NEW eith C, S:'
          IF ( IE <= 3 )  CALL RPRINT (S,    LT_FREE, LT_FREE, 1) !b
          IF ( IE <= 3 )  CALL RPRINT (EL_M, LT_FREE, LT_FREE, 1) !b
        END IF
      ELSE
        CALL GENERATE_EL_MATRICES     (IE, X, DD_OLD)
      END IF ! Select an old example

!-->     STORE THE MATRICES IN SYSTEM EQUATIONS
      IF ( JACOBI ) THEN
        IF ( I_BUG > 0 ) PRINT *,'BEGIN JACOBI EIGEN ANALYSIS'

!           ASSEMBLE STIFFNESS AND MASS ONLY          
        CALL STORE_FULL_SQUARE (N_D_FRE, LT_FREE, S, S_FULL, INDEX) !b

        IF ( AVERAGE_MASS ) THEN  ! average consistent & diagonal forms
          CALL DIAGONALIZE_SQ_MATRIX (LT_FREE, EL_M, DIAG_M) 
          EL_M = 0.5d0 * EL_M
          DO J = 1, LT_FREE
            EL_M (J, J) = EL_M (J, J) + DIAG_M (J) * 0.5d0
          END DO
        END IF
        IF ( DIAGONAL_MASS ) THEN  ! use the scaled diagonal mass form
          IF ( DEBUG_EL_SQ ) PRINT *,'BEFORE DIAG_M AT ', THIS_EL
          CALL DIAGONALIZE_SQ_MATRIX (LT_FREE, EL_M, DIAG_M) 
          EL_M = 0.d0
          DO J = 1, LT_FREE
            EL_M (J, J) = DIAG_M (J) 
          END DO
        END IF
        CALL STORE_FULL_SQUARE (N_D_FRE, LT_FREE, EL_M, M_FULL, INDEX) 

!b      ELSE IF (STATIC ) THEN ! unlikely
!b
!b!           ASSEMBLE STIFFNESS AND CC ONLY          
!b        CALL STORE_FULL_SQUARE (N_D_FRE, LT_FREE, S, S_FULL, INDEX)
!b        CALL STORE_COLUMN (N_D_FRE, LT_FREE, INDEX, C, CC)            
!b
!b        IF ( DEBUG_EL_COL ) THEN
!b          PRINT *,'EL, CC ', THIS_EL
!b          !b CALL RPRINT (F, 1, LT_FREE, 1) !b
!b          CALL RPRINT (CC, 1, N_D_FRE, 1) !b
!b        END IF
!b
!b        IF ( NUL_COL == 0 .AND. COL_SUMS == 0.d0 ) THEN
!b          PRINT *, 'WARNING: ALL ELEMENT SOURCE VECTORS ARE NULL,'
!b          PRINT *, 'PUT el_no_col IN KEYWORD CONTROL'            
!b          N_WARN = N_WARN + 1 ! INCREMENT WARNING
!b        END IF
!b        IF ( DEBUG_EL_COL .AND. IE <= 3 )  &
!b          CALL RPRINT (CC, 1, N_D_FRE, 1) !b

      ELSE ! UNKNOWN DEFAULT
          STOP 'ASSEMBLE_FULL_EQS: ELEMENT ASSEMBLY CLASS UNDEFINED'
      END IF ! ANALYSIS CLASS

      IF ( IE == N_ELEMS ) WRITE (N_PRT,                 &
                "('COMPLETED STANDARD ELEMENT ASSEMBLY')")
      IF ( N_MIXED > 0 .AND. IE == (N_ELEMS + N_MIXED) )        &
        WRITE (N_PRT, "('COMPLETED MIXED_BC SEGMENTS ASSEMBLY')")
      IF ( N_F_SEG > 0 .AND.                                   &
           IE == (N_ELEMS + N_MIXED + N_F_SEG) ) WRITE (N_PRT, &
           "('COMPLETED FLUX BOUNDARY SEGMENTS ASSEMBLY')")
      IF ( IE == L_S_TOT .AND. (N_MIXED > 0 .OR. N_F_SEG > 0) ) &
        WRITE (N_PRT, "('ASSEMBLY COMPLETE.', /)")
  END DO ! over all elements and boundary segments

  WRITE (N_PRT, "(/, 'FULL ASSEMBLY COMPLETED')")
END SUBROUTINE ASSEMBLE_FULL_EQS 

