c =====================================================================
      subroutine rp1(maxmx,meqn,mwaves,mbc,mx,ql,qr,auxl,auxr,
     &             fwave,s,amdq,apdq)
c =====================================================================
c
c     # solve Riemann problems for the 1D shallow water equations
c     # with source-term resulting from variable topography b(x,t)
c     #   (h)_t + (u h)_x = 0
c     #   (uh)_t + (uuh + 0.5*gh**2)_x = -g*h*b_x
c
c
c     # On input,
c     # ql contains the state vector at the left edge of each cell
c     # qr contains the state vector at the right edge of each cell
c     #
c     # On output, wave contains the fwaves/s,
c     #            s the speeds,
c     #            amdq the  left-going flux difference  A**- \Delta q
c     #            apdq the right-going flux difference  A**+ \Delta q
c
c     # Note that the i'th Riemann problem has left state qr(i-1,:)
c     #                                    and right state ql(i,:)
c     # From the basic clawpack routine step1, rp1 is called with ql=qr=q.
c
c
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!                                                                           !
!      # This Riemann solver is for the shallow water equations.            !
!                                                                           !
!       It allows the user to easily select a Riemann solver in             !
!       riemannsolvers_geo.f. this routine initializes all the variables    !
!       for the shallow water equations, accounting for wet dry boundary    !
!       dry cells, wave speeds etc.                                         !
!                                                                           !
!           David George, Vancouver WA, Feb. 2009                           !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

      implicit none

      external riemann_sswave2

      !input
      integer maxmx,meqn,mwaves,mbc,mx

      double precision ql(1-mbc:maxmx+mbc, meqn)
      double precision qr(1-mbc:maxmx+mbc, meqn)
      double precision s(1-mbc:maxmx+mbc, mwaves)
      double precision fwave(1-mbc:maxmx+mbc, meqn, mwaves)
      double precision amdq(1-mbc:maxmx+mbc, meqn)
      double precision apdq(1-mbc:maxmx+mbc, meqn)
      double precision auxl(1-mbc:maxmx+mbc, *)
      double precision auxr(1-mbc:maxmx+mbc, *)

      double precision grav,drytolerance,drytol,g

      !local only
      integer m,i,mw,maxiter
      double precision wall(2)
      double precision fw(3,3)

      double precision hR,hL,huR,huL,uR,uL,hvR,hvL,vR,vL,phiR,phiL
      double precision bR,bL,sL,sR,sRoe1,sRoe2,sE1,sE2,uhat,chat
      double precision hstartest,hstarHLL,sLtest,sRtest

      common /geo/ grav,drytolerance

      g=grav
      drytol=drytolerance

      !loop through Riemann problems at each grid cell
      do i=2-mbc,mx+mbc

!-----------------------Initializing-----------------------------------
         !inform of a bad riemann problem from the start
         if((qr(i-1,1).lt.0.d0).or.(ql(i,1) .lt. 0.d0)) then
            write(*,*) 'Negative input: hl,hr,i=',qr(i-1,1),ql(i,1),i
         endif

         !Initialize Riemann problem for grid interface
         do mw=1,mwaves
              s(i,mw)=0.d0
              do m=1,meqn
                 fwave(i,m,mw)=0.d0
              enddo
         enddo

         !zero (small) negative values if they exist
         if (qr(i-1,1).lt.0.d0) then
            qr(i-1,1)=0.d0
            qr(i-1,2)=0.d0
         endif

         if (ql(i,1).lt.0.d0) then
            ql(i,1)=0.d0
            ql(i,2)=0.d0
         endif

         !skip problem if in a completely dry area
         if (qr(i-1,1).le.drytol.and.ql(i,1).le.drytol) then
            go to 30
         endif

         !Riemann problem variables
         hL = qr(i-1,1)
         hR = ql(i,1)
         huL = qr(i-1,2)
         huR = ql(i,2)
         bL = auxr(i-1,1)
         bR = auxl(i,1)

         hvL=0.d0
         hvR=0.d0

         !check for wet/dry boundary
         sE1= 1.d99
         sE2=-1.d99
         wall(2) = 1.d0
         wall(1) = 1.d0
         if (hR.le.drytol) then
            sLtest=min(-sqrt(g*hL),huL/hL-sqrt(g*hL)) !what would be the Einfeldt speed of wall problem
            hstartest=hL-(huL/sLtest) !what would be middle state in approx Riemann solution
            if (hstartest+bL.lt.bR) then !right state should become ghost values that mirror left for wall problem
               wall(2)=0.d0
               hR=hL
               huR=-huL
               bR=bL
               phiR=phiL
            endif
         elseif (hL.le.drytol) then ! right surface is lower than left topo
            sRtest=max(sqrt(g*hR),huR/hR+sqrt(g*hR)) !what would be the Einfeldt speed of wall
            hstartest= hR-(huR/sRtest) !what would be middle state in approx Rimeann solution
            if (hstartest+bR.lt.bL) then  !left state should become ghost values that mirror right
               wall(1)=0.d0
               hL=hR
               huL=-huR
               bL=bR
               phiL=phiR
            endif
         endif

         if (hR.gt.drytol) then
            uR=huR/hR
            vR=hvR/hR
            phiR = 0.5d0*g*hR**2 + huR**2/hR
         else
            hR = 0.d0
            huR = 0.d0
            uR = 0.d0
            vR = 0.d0
            phiR = 0.d0
            sE2 = max(sE2,huL/hL+2.d0*sqrt(g*hL))
         endif

         if (hL.gt.drytol) then
            uL=huL/hL
            vL=hvL/hL
            phiL = 0.5d0*g*hL**2 + huL**2/hL
         else
            hL=0.d0
            huL=0.d0
            uL=0.d0
            vL=0.d0
            phiL = 0.d0
            sE1 = min(sE1,huR/hR - 2.d0*sqrt(g*hR))
         endif

         !determine wave speeds
         sL=uL-sqrt(g*hL) ! 1 wave speed of left state
         sR=uR+sqrt(g*hR) ! 2 wave speed of right state

         uhat=(sqrt(g*hL)*uL + sqrt(g*hR)*uR)/(sqrt(g*hR)+sqrt(g*hL)) ! Roe average
         chat=sqrt(g*0.5d0*(hR+hL)) ! Roe average
         sRoe1=uhat-chat ! Roe wave speed 1 wave
         sRoe2=uhat+chat ! Roe wave speed 2 wave

         sE1 = min(sE1,min(sL,sRoe1)) ! Eindfeldt speed 1 wave
         sE2 = max(sE2,max(sR,sRoe2)) ! Eindfeldt speed 2 wave

         !--------------------end initializing...finally----------
         !solve Riemann problem.

         maxiter = 100

c         call riemann_ssqfwave(maxiter,meqn+1,mwaves+1,hL,hR,huL,huR,
c     &      hvL,hvR,bL,bR,uL,uR,vL,vR,phiL,phiR,sE1,sE2,drytol,g,fw)

         call riemann_fwave(meqn+1,mwaves+1,hL,hR,huL,huR,hvL,hvR,
     &            bL,bR,uL,uR,vL,vR,phiL,phiR,sE1,sE2,drytol,g,fw)

         s(i,1) = sE1*wall(1)
         s(i,2) = sE2*wall(2)

         do m=1,meqn
            fwave(i,m,1)=fw(m,1)*wall(1)
            fwave(i,m,2)=fw(m,3)*wall(2)
         enddo

 30      continue
      enddo

      do i=2-mbc,mx+mbc
         do m=1,meqn
            amdq(i,m) = 0.d0
            apdq(i,m) = 0.d0
            do  mw=1,mwaves
               if (s(i,mw).lt.0.d0) then
                  amdq(i,m) = amdq(i,m) + fwave(i,m,mw)
               elseif (s(i,mw).gt.0.d0) then
                  apdq(i,m) = apdq(i,m) + fwave(i,m,mw)
	         else
		         amdq(i,m) = amdq(i,m) + .5d0*fwave(i,m,mw)
		         apdq(i,m) = apdq(i,m) + .5d0*fwave(i,m,mw)
               endif
            enddo
         enddo
      enddo

      return
      end subroutine


!***********************************************************************
!                                                                      *
      REAL(KIND(0.0D0)) FUNCTION FZALF (N, KAPPA, Z)
!                                                                      *
!   An estimate of the function  F (Z*\alpha) is computed here.        *
!                                                                      *
!   Call(s) to: [RCI92]: KLAMAQ, MOHR.                                 *
!                                                                      *
!   Written by Farid A Parpia, at Oxford    Last update: 09 Oct 1990   *
!                                                                      *
!***********************************************************************
!
!...Translated by Pacific-Sierra Research 77to90  4.3E  14:04:58   1/ 3/07
!...Modified by Charlotte Froese Fischer
!                     Gediminas Gaigalas  10/05/17
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY:  DOUBLE
!-----------------------------------------------
!   I n t e r f a c e   B l o c k s
!-----------------------------------------------
      USE mohr_I
      USE klamaq_I
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      INTEGER  :: N
      INTEGER  :: KAPPA
      REAL(DOUBLE)  :: Z
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER :: NEFF
      REAL(DOUBLE) :: VALUE
!-----------------------------------------------
!
      IF (N <= 2) THEN
         CALL MOHR (N, KAPPA, Z, VALUE)
      ELSE
         IF (KAPPA==(-1) .OR. KAPPA==1 .OR. KAPPA==(-2)) THEN
            NEFF = 2
            CALL MOHR (NEFF, KAPPA, Z, VALUE)
         ELSE
            CALL KLAMAQ (N, KAPPA, Z, VALUE)
         ENDIF
      ENDIF
!
      FZALF = VALUE
!
      RETURN
      END FUNCTION FZALF


C    Copyright(C) 1999-2020 National Technology & Engineering Solutions
C    of Sandia, LLC (NTESS).  Under the terms of Contract DE-NA0003525 with
C    NTESS, the U.S. Government retains certain rights in this software.
C
C    See packages/seacas/LICENSE for details

      SUBROUTINE ELPSPR (MP, KT, KNUM, COOR, LINKP, IPNTR1, IPNTR2,
     &   IPNTR3, IP3, XCEN, YCEN, THETA1, THETA2, TANG, ICCW, ICW,
     &   AVALUE, BVALUE, ERR)
C***********************************************************************

C  SUBROUTINE ELPSPR = THIS ROUTINE CALCULATES THE ELIPSE PARAMETERS

C***********************************************************************

C  VARIABLES USED:
C     TANG   = TOTAL ANGLE SCRIBED BY THE ARC
C     THETA1 = FIRST CCW ANGLE OF THE ARC
C     THETA2 = SECOND CCW ANGLE OF THE ARC
C     IPNTR1 = POINTER TO FIRST COORDINATE VALUE
C     IPNTR2 = POINTER TO SECOND COORDINATE VALUE
C     IPNTR3 = POINTER TO THIRD COORDINATE VALUE
C     IP3    = THE THIRD POINT NUMBER  (CAN BE NEGATED)

C***********************************************************************

      DIMENSION COOR (2, MP), LINKP (2, MP)

      LOGICAL ERR

      PI = ATAN2(0.0, -1.0)

      TWOPI = PI + PI
      ERR = .FALSE.

C  ELIPSE GOES FROM 1ST POINT TO 2ND IN *COUNTER-CLOCKWISE* DIRECTION.

      XCEN = COOR (1, IPNTR3)
      YCEN = COOR (2, IPNTR3)

C  CHECK TO MAKE SURE THAT THE BEGINNING AND ENDING RADIUS EXIST

      IF (( (COOR (1, IPNTR1) .EQ. XCEN).AND.
     &   (COOR (2,IPNTR1) .EQ. YCEN)).OR.
     &   ((COOR (1, IPNTR2) .EQ. XCEN).AND.
     &   (COOR (2, IPNTR2) .EQ. YCEN)))THEN
         CALL PLTFLU
         WRITE (*, 10000)ABS (KNUM)
         ERR = .TRUE.
         GOTO 100
      ENDIF
      THETA1 = ATAN2 (COOR (2, IPNTR1) - YCEN, COOR (1, IPNTR1) - XCEN)
      THETA2 = ATAN2 (COOR (2, IPNTR2) - YCEN, COOR (1, IPNTR2) - XCEN)

C  NOW CALCULATE THE MAJOR AXIS (AVALUE) AND THE MINOR AXIS (BVALUE)

      X1 = COOR (1, IPNTR1) - XCEN
      Y1 = COOR (2, IPNTR1) - YCEN
      X2 = COOR (1, IPNTR2) - XCEN
      Y2 = COOR (2, IPNTR2) - YCEN

C  CHOOSE THE APPROPRIATE ELIPSE DEFINITION

      IF (Y1 * Y1 .EQ. Y2 * Y2) THEN
         AVALUE = SQRT (X1 * X1 + Y1 * Y1)
         BVALUE = AVALUE
      ELSEIF ((Y1 .EQ. 0.) .AND. (X2 .EQ. 0.)) THEN
         AVALUE = X1
         BVALUE = Y2
      ELSEIF ((Y2 .EQ. 0.) .AND. (X1 .EQ. 0.)) THEN
         AVALUE = X2
         BVALUE = Y1
      ELSE
         RATIO = SQRT (ABS ( (X1 * X1 - X2 * X2) /
     &      (Y1 * Y1 - Y2 * Y2) ))
         IF (RATIO .EQ. 0.) THEN
            AVALUE = SQRT (X1 * X1 + Y1 * Y1)
            BVALUE = AVALUE
         ELSEIF (RATIO .EQ. 1.0) THEN
            AVALUE = SQRT (X1 * X1 + Y1 * Y1)
            BVALUE = AVALUE
         ELSE
            IF ((Y2 .EQ. 0.) .OR. ((X2 .EQ. 0.) .AND. (Y1 .NE. 0.)) )
     &         THEN
               IF (RATIO .GT. 1.) THEN
                  VX = 1.
                  VY = - (1./RATIO**2) * (X1 / Y1)
               ELSE
                  VX = 1.
                  VY = - (RATIO**2) * (X1 / Y1)
               ENDIF
               D0 = SQRT (X1 * X1 + Y1 * Y1)
               A8 = ACOS ( ((X1 * VX) + (Y1 * VY)) /
     &            (D0 * SQRT (VX * VX + VY * VY)) )
               A7 = PI - A8
               A2 = ABS (ATAN2 (Y1, X1))
               THETA1 = ABS(ATAN2 (VY, 1.))
            ELSE
               IF (RATIO .GT. 1.) THEN
                  VX = 1.
                  VY = - (1./RATIO**2) * (X2 / Y2)
               ELSE
                  VX = 1.
                  VY = - (RATIO**2) * (X2 / Y2)
               ENDIF
               D0 = SQRT (X2 * X2 + Y2 * Y2)
               A8 = ACOS ( ((X2 * VX) + (Y2 * VY)) /
     &            (D0 * SQRT (VX * VX + VY * VY)) )
               A7 = PI - A8
               A2 = ABS (ATAN2 (Y2, X2))
               THETA1 = ABS(ATAN2 (VY, 1.))
            ENDIF

            RADMAX = MAX(A7,A8)
            CALL ETHETA (A7, A8, A2, THETA1, RADMAX, THETA, ERR)
            IF (ERR) THEN
               WRITE (*, 10010) ABS (KNUM)
               GOTO 100
            ENDIF

            CVALUE = D0 * SIN (A8 - THETA) / SIN (A2 - A8 + THETA)
            BVALUE = SQRT (ABS (CVALUE **2 / (RATIO **2 - 1)) )
            AVALUE = BVALUE * RATIO
         ENDIF
      ENDIF

C  NOW GET THE ANGLES GOING THE RIGHT WAY

      THETA1 = ATAN2 (COOR (2, IPNTR1) - YCEN, COOR (1, IPNTR1) - XCEN)
      THETA2 = ATAN2 (COOR (2, IPNTR2) - YCEN, COOR (1, IPNTR2) - XCEN)
      IF (IPNTR1 .EQ. IPNTR2) THEN
         THETA2 = THETA1 + TWOPI
      ELSEIF ( (IP3 .GE. 0) .AND. (THETA2 .LE. THETA1) ) THEN
         THETA2 = THETA2+TWOPI
      ELSEIF ( (IP3 .LT. 0) .AND. (THETA1 .LE. THETA2) ) THEN
         THETA1 = THETA1+TWOPI
      ENDIF
      TANG = THETA2 - THETA1
      IF (IP3 .LT. 0) THEN
         ICCW = IPNTR2
         ICW = IPNTR1
      ELSE
         ICCW = IPNTR1
         ICW = IPNTR2
      ENDIF

  100 CONTINUE

      RETURN

10000 FORMAT (' CENTER POINT FOR LINE', I5, ' LIES ON ONE OF',
     &   ' THE ENDPOINTS')
10010 FORMAT (' POINTS GIVEN FOR LINE', I5, ' DO NOT DEFINE AN ELIPSE')

      END


      SUBROUTINE DROTMGF (DD1,DD2,DX1,DY1,DPARAM)
C
C     CONSTRUCT THE MODIFIED GIVENS TRANSFORMATION MATRIX H WHICH ZEROS
C     THE SECOND COMPONENT OF THE 2-VECTOR  (DSQRT(DD1)*DX1,DSQRT(DD2)*
C     DY2)**T.
C     WITH DPARAM(1)=DFLAG, H HAS ONE OF THE FOLLOWING FORMS..
C
C     DFLAG=-1.D0     DFLAG=0.D0        DFLAG=1.D0     DFLAG=-2.D0
C
C       (DH11  DH12)    (1.D0  DH12)    (DH11  1.D0)    (1.D0  0.D0)
C     H=(          )    (          )    (          )    (          )
C       (DH21  DH22),   (DH21  1.D0),   (-1.D0 DH22),   (0.D0  1.D0).
C     LOCATIONS 2-4 OF DPARAM CONTAIN DH11, DH21, DH12, AND DH22
C     RESPECTIVELY. (VALUES OF 1.D0, -1.D0, OR 0.D0 IMPLIED BY THE
C     VALUE OF DPARAM(1) ARE NOT STORED IN DPARAM.)
C
C     THE VALUES OF GAMSQ AND RGAMSQ SET IN THE DATA STATEMENT MAY BE
C     INEXACT.  THIS IS OK AS THEY ARE ONLY USED FOR TESTING THE SIZE
C     OF DD1 AND DD2.  ALL ACTUAL SCALING OF DATA IS DONE USING GAM.
C
      DOUBLE PRECISION GAM,ONE,RGAMSQ,DD2,DH11,DH21,DPARAM,DP2,
     1 DQ2,DU,DY1,ZERO,GAMSQ,DD1,DFLAG,DH12,DH22,DP1,DQ1,
     2 DTEMP,DX1,TWO
      DIMENSION DPARAM(5)
C
      DATA ZERO,ONE,TWO /0.D0,1.D0,2.D0/
      DATA GAM,GAMSQ,RGAMSQ/4096.D0,16777216.D0,5.9604645D-8/
      IF(.NOT. DD1 .LT. ZERO) GO TO 10
C       GO ZERO-H-D-AND-DX1..
          GO TO 60
   10 CONTINUE
C     CASE-DD1-NONNEGATIVE
      DP2=DD2*DY1
      IF(.NOT. DP2 .EQ. ZERO) GO TO 20
          DFLAG=-TWO
          GO TO 260
C     REGULAR-CASE..
   20 CONTINUE
      DP1=DD1*DX1
      DQ2=DP2*DY1
      DQ1=DP1*DX1
C
      IF(.NOT. DABS(DQ1) .GT. DABS(DQ2)) GO TO 40
          DH21=-DY1/DX1
          DH12=DP2/DP1
C
          DU=ONE-DH12*DH21
C
          IF(.NOT. DU .LE. ZERO) GO TO 30
C         GO ZERO-H-D-AND-DX1..
               GO TO 60
   30     CONTINUE
               DFLAG=ZERO
               DD1=DD1/DU
               DD2=DD2/DU
               DX1=DX1*DU
C         GO SCALE-CHECK..
               GO TO 100
   40 CONTINUE
          IF(.NOT. DQ2 .LT. ZERO) GO TO 50
C         GO ZERO-H-D-AND-DX1..
               GO TO 60
   50     CONTINUE
               DFLAG=ONE
               DH11=DP1/DP2
               DH22=DX1/DY1
               DU=ONE+DH11*DH22
               DTEMP=DD2/DU
               DD2=DD1/DU
               DD1=DTEMP
               DX1=DY1*DU
C         GO SCALE-CHECK
               GO TO 100
C     PROCEDURE..ZERO-H-D-AND-DX1..
   60 CONTINUE
          DFLAG=-ONE
          DH11=ZERO
          DH12=ZERO
          DH21=ZERO
          DH22=ZERO
C
          DD1=ZERO
          DD2=ZERO
          DX1=ZERO
C         RETURN..
          GO TO 220
C     PROCEDURE..FIX-H..
   70 CONTINUE
      IF(.NOT. DFLAG .GE. ZERO) GO TO 90
C
          IF(.NOT. DFLAG .EQ. ZERO) GO TO 80
          DH11=ONE
          DH22=ONE
          DFLAG=-ONE
          GO TO 90
   80     CONTINUE
          DH21=-ONE
          DH12=ONE
          DFLAG=-ONE
   90 CONTINUE
      GO TO IGO,(120,150,180,210)
C     PROCEDURE..SCALE-CHECK
  100 CONTINUE
  110     CONTINUE
          IF(.NOT. DD1 .LE. RGAMSQ) GO TO 130
               IF(DD1 .EQ. ZERO) GO TO 160
               ASSIGN 120 TO IGO
C              FIX-H..
               GO TO 70
  120          CONTINUE
               DD1=DD1*GAM**2
               DX1=DX1/GAM
               DH11=DH11/GAM
               DH12=DH12/GAM
          GO TO 110
  130 CONTINUE
  140     CONTINUE
          IF(.NOT. DD1 .GE. GAMSQ) GO TO 160
               ASSIGN 150 TO IGO
C              FIX-H..
               GO TO 70
  150          CONTINUE
               DD1=DD1/GAM**2
               DX1=DX1*GAM
               DH11=DH11*GAM
               DH12=DH12*GAM
          GO TO 140
  160 CONTINUE
  170     CONTINUE
          IF(.NOT. DABS(DD2) .LE. RGAMSQ) GO TO 190
               IF(DD2 .EQ. ZERO) GO TO 220
               ASSIGN 180 TO IGO
C              FIX-H..
               GO TO 70
  180          CONTINUE
               DD2=DD2*GAM**2
               DH21=DH21/GAM
               DH22=DH22/GAM
          GO TO 170
  190 CONTINUE
  200     CONTINUE
          IF(.NOT. DABS(DD2) .GE. GAMSQ) GO TO 220
               ASSIGN 210 TO IGO
C              FIX-H..
               GO TO 70
  210          CONTINUE
               DD2=DD2/GAM**2
               DH21=DH21*GAM
               DH22=DH22*GAM
          GO TO 200
  220 CONTINUE
          IF(DFLAG)250,230,240
  230     CONTINUE
               DPARAM(3)=DH21
               DPARAM(4)=DH12
               GO TO 260
  240     CONTINUE
               DPARAM(2)=DH11
               DPARAM(5)=DH22
               GO TO 260
  250     CONTINUE
               DPARAM(2)=DH11
               DPARAM(3)=DH21
               DPARAM(4)=DH12
               DPARAM(5)=DH22
  260 CONTINUE
          DPARAM(1)=DFLAG
          RETURN
      END


*> \brief \b SSGT01
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       SUBROUTINE SSGT01( ITYPE, UPLO, N, M, A, LDA, B, LDB, Z, LDZ, D,
*                          WORK, RESULT )
*
*       .. Scalar Arguments ..
*       CHARACTER          UPLO
*       INTEGER            ITYPE, LDA, LDB, LDZ, M, N
*       ..
*       .. Array Arguments ..
*       REAL               A( LDA, * ), B( LDB, * ), D( * ), RESULT( * ),
*      $                   WORK( * ), Z( LDZ, * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> SSGT01 checks a decomposition of the form
*>
*>    A Z   =  B Z D or
*>    A B Z =  Z D or
*>    B A Z =  Z D
*>
*> where A is a symmetric matrix, B is
*> symmetric positive definite, Z is orthogonal, and D is diagonal.
*>
*> One of the following test ratios is computed:
*>
*> ITYPE = 1:  RESULT(1) = | A Z - B Z D | / ( |A| |Z| n ulp )
*>
*> ITYPE = 2:  RESULT(1) = | A B Z - Z D | / ( |A| |Z| n ulp )
*>
*> ITYPE = 3:  RESULT(1) = | B A Z - Z D | / ( |A| |Z| n ulp )
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] ITYPE
*> \verbatim
*>          ITYPE is INTEGER
*>          The form of the symmetric generalized eigenproblem.
*>          = 1:  A*z = (lambda)*B*z
*>          = 2:  A*B*z = (lambda)*z
*>          = 3:  B*A*z = (lambda)*z
*> \endverbatim
*>
*> \param[in] UPLO
*> \verbatim
*>          UPLO is CHARACTER*1
*>          Specifies whether the upper or lower triangular part of the
*>          symmetric matrices A and B is stored.
*>          = 'U':  Upper triangular
*>          = 'L':  Lower triangular
*> \endverbatim
*>
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The order of the matrix A.  N >= 0.
*> \endverbatim
*>
*> \param[in] M
*> \verbatim
*>          M is INTEGER
*>          The number of eigenvalues found.  0 <= M <= N.
*> \endverbatim
*>
*> \param[in] A
*> \verbatim
*>          A is REAL array, dimension (LDA, N)
*>          The original symmetric matrix A.
*> \endverbatim
*>
*> \param[in] LDA
*> \verbatim
*>          LDA is INTEGER
*>          The leading dimension of the array A.  LDA >= max(1,N).
*> \endverbatim
*>
*> \param[in] B
*> \verbatim
*>          B is REAL array, dimension (LDB, N)
*>          The original symmetric positive definite matrix B.
*> \endverbatim
*>
*> \param[in] LDB
*> \verbatim
*>          LDB is INTEGER
*>          The leading dimension of the array B.  LDB >= max(1,N).
*> \endverbatim
*>
*> \param[in] Z
*> \verbatim
*>          Z is REAL array, dimension (LDZ, M)
*>          The computed eigenvectors of the generalized eigenproblem.
*> \endverbatim
*>
*> \param[in] LDZ
*> \verbatim
*>          LDZ is INTEGER
*>          The leading dimension of the array Z.  LDZ >= max(1,N).
*> \endverbatim
*>
*> \param[in] D
*> \verbatim
*>          D is REAL array, dimension (M)
*>          The computed eigenvalues of the generalized eigenproblem.
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is REAL array, dimension (N*N)
*> \endverbatim
*>
*> \param[out] RESULT
*> \verbatim
*>          RESULT is REAL array, dimension (1)
*>          The test ratio as described above.
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \ingroup single_eig
*
*  =====================================================================
      SUBROUTINE SSGT01( ITYPE, UPLO, N, M, A, LDA, B, LDB, Z, LDZ, D,
     $                   WORK, RESULT )
*
*  -- LAPACK test routine --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*
*     .. Scalar Arguments ..
      CHARACTER          UPLO
      INTEGER            ITYPE, LDA, LDB, LDZ, M, N
*     ..
*     .. Array Arguments ..
      REAL               A( LDA, * ), B( LDB, * ), D( * ), RESULT( * ),
     $                   WORK( * ), Z( LDZ, * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      REAL               ZERO, ONE
      PARAMETER          ( ZERO = 0.0E0, ONE = 1.0E0 )
*     ..
*     .. Local Scalars ..
      INTEGER            I
      REAL               ANORM, ULP
*     ..
*     .. External Functions ..
      REAL               SLAMCH, SLANGE, SLANSY
      EXTERNAL           SLAMCH, SLANGE, SLANSY
*     ..
*     .. External Subroutines ..
      EXTERNAL           SSCAL, SSYMM
*     ..
*     .. Executable Statements ..
*
      RESULT( 1 ) = ZERO
      IF( N.LE.0 )
     $   RETURN
*
      ULP = SLAMCH( 'Epsilon' )
*
*     Compute product of 1-norms of A and Z.
*
      ANORM = SLANSY( '1', UPLO, N, A, LDA, WORK )*
     $        SLANGE( '1', N, M, Z, LDZ, WORK )
      IF( ANORM.EQ.ZERO )
     $   ANORM = ONE
*
      IF( ITYPE.EQ.1 ) THEN
*
*        Norm of AZ - BZD
*
         CALL SSYMM( 'Left', UPLO, N, M, ONE, A, LDA, Z, LDZ, ZERO,
     $               WORK, N )
         DO 10 I = 1, M
            CALL SSCAL( N, D( I ), Z( 1, I ), 1 )
   10    CONTINUE
         CALL SSYMM( 'Left', UPLO, N, M, ONE, B, LDB, Z, LDZ, -ONE,
     $               WORK, N )
*
         RESULT( 1 ) = ( SLANGE( '1', N, M, WORK, N, WORK ) / ANORM ) /
     $                 ( N*ULP )
*
      ELSE IF( ITYPE.EQ.2 ) THEN
*
*        Norm of ABZ - ZD
*
         CALL SSYMM( 'Left', UPLO, N, M, ONE, B, LDB, Z, LDZ, ZERO,
     $               WORK, N )
         DO 20 I = 1, M
            CALL SSCAL( N, D( I ), Z( 1, I ), 1 )
   20    CONTINUE
         CALL SSYMM( 'Left', UPLO, N, M, ONE, A, LDA, WORK, N, -ONE, Z,
     $               LDZ )
*
         RESULT( 1 ) = ( SLANGE( '1', N, M, Z, LDZ, WORK ) / ANORM ) /
     $                 ( N*ULP )
*
      ELSE IF( ITYPE.EQ.3 ) THEN
*
*        Norm of BAZ - ZD
*
         CALL SSYMM( 'Left', UPLO, N, M, ONE, A, LDA, Z, LDZ, ZERO,
     $               WORK, N )
         DO 30 I = 1, M
            CALL SSCAL( N, D( I ), Z( 1, I ), 1 )
   30    CONTINUE
         CALL SSYMM( 'Left', UPLO, N, M, ONE, B, LDB, WORK, N, -ONE, Z,
     $               LDZ )
*
         RESULT( 1 ) = ( SLANGE( '1', N, M, Z, LDZ, WORK ) / ANORM ) /
     $                 ( N*ULP )
      END IF
*
      RETURN
*
*     End of SSGT01
*
      END


!***********************************************************
!*               Copyright (c) Wenbin Yu                   *
!*              All rights reserved                        *
!*          Mechanical and Aerospace Engineering           *
!*                                                         *
!*                Utah State University                    *
!***********************************************************

!***************************************************************
!*                                                             *
!> This module assemles the system including the coefficient matrix (jacobian matrix) and the right hand side (negative of the equation values)                                     *
!* Outputs:coef, rhs                                           *
!*                                                             *
!***************************************************************
MODULE System

USE InternalData
USE PrescribedCondition
USE Member

IMPLICIT NONE

PRIVATE       ! So everything is private, except declared by PUBLIC

PUBLIC AssembleJacobian,AssembleRHS,ne,irn,jcn,coef

REAL(DBL)::x_pt(NSTRN)         !< the solution from the previous step for a key point.
INTEGER  ::kp_dof(NSTRN)       !< prescribed dof
REAL(DBL)::kp_cond(NSTRN)      !< prescribed value
INTEGER  ::kp_follower(NSTRN)  !< follower condition

INTEGER::ne !< Number of nonzero coefficients
INTEGER,ALLOCATABLE::irn(:) !< line index of nonzero coefficients
INTEGER,ALLOCATABLE::jcn(:) !< column index of nonzero coefficients
REAL(DBL),ALLOCATABLE::coef(:)  !< value of nonzero coefficients

!=============================================
CONTAINS

!************************************************************
!*                                                          *
!>  Assemble the coefficient matrix of the beam system      *
!*															*
!************************************************************
SUBROUTINE AssembleJacobian(ndof_el,niter,memb_info,v_root_a,omega_a,member,error,&
			& ncond_mb,mb_condition,distr_fun,dof_con,x,aero_flag,grav_flag,init_cond)

INTEGER,INTENT(IN)::ndof_el !<#ioaero::ndof_el
INTEGER,INTENT(IN)::niter   !<#ioaero::niter
INTEGER,INTENT(IN)::aero_flag   !<#ioaero::aero_flag
INTEGER,INTENT(IN)::grav_flag   !<#ioaero::grav_flag
REAL(DBL),INTENT(IN)::v_root_a(:)   !< linear velocity of frame a
REAL(DBL),INTENT(IN)::omega_a(:)    !< angular velocity of frame a
TYPE (MemberInf),INTENT(IN)::memb_info(:)   !< contains the member parameters of the whole structure

REAL(DBL),INTENT(IN) ::distr_fun(:,:)   !<#ioaero::distr_fun
INTEGER,INTENT(IN)   ::member(:,:)  !<#ioaero::member
INTEGER,INTENT(IN)   ::ncond_mb !<#ioaero::ncond_mb
INTEGER              ::dof_con(:) !< note dof_con is passed by value, hence what is changed in this subroutine will not affect the original value.

REAL(DBL),INTENT(IN) :: x(:)    !< solution vector
TYPE(PrescriInf),INTENT(IN)::mb_condition(:)    !<#ioaero::mb_condition
CHARACTER(*),INTENT(OUT)::error !<#ioaero::error

REAL(DBL),OPTIONAL,INTENT(IN)::init_cond(:,:)   !<#ioaero::init_cond

INTEGER::i,j

INTEGER::pre_dof !< prescibed dof of the connection point

REAL(DBL),ALLOCATABLE::coef_memb(:) !< coefficient matrix of a member

INTEGER:: nrow,ncol,nrow_end,ncol_end
INTEGER,ALLOCATABLE::irn_memb(:),jcn_memb(:)

INTEGER::nz_memb

REAL(DBL)::coef_tm(NDOF_ND,ndof_el)

ALLOCATE(coef(neMax),STAT=allo_stat)
IF(MemoryError('coef',error)) GOTO 9999

ALLOCATE(irn(neMax),STAT=allo_stat)
IF(MemoryError('irn',error)) GOTO 9999

ALLOCATE(jcn(neMax),STAT=allo_stat)
IF(MemoryError('jcn',error)) GOTO 9999

ne=0

DO i=1, SIZE(member,1)

   CALL ExtractMemberProperties(i,memb_info(i),member,ncond_mb,mb_condition,distr_fun,error,init_cond)

   ALLOCATE(coef_memb(ndiv*nzElemMax),STAT=allo_stat)
   IF(MemoryError('coef_memb',error)) GOTO 9999
   ALLOCATE(irn_memb(ndiv*nzElemMax),STAT=allo_stat)
   IF(MemoryError('irn_memb',error)) GOTO 9999
   ALLOCATE(jcn_memb(ndiv*nzElemMax),STAT=allo_stat)
   IF(MemoryError('jcn_memb',error)) GOTO 9999

   ! Dealing with the starting point
   !-----------------------------------
   !Extract the solution from the previous step for the member, excluding the terminal points
   !If the starting point is not used yet, then the first NSTRN values belong to the point
   !If the starting point has been used already in the assembly, then x_memb starts with ncol+1
   !------------------------------------------------------------------------------------------
   nrow=index_mb(i,1)  ! the starting row of the member block excluding the first blocking connecting to the trailing point
   ncol=index_mb(i,2)  ! the starting column of the member block
   nrow_end=nrow+ncol_memb-NDOF_ND  ! the starting row of the ending block of the member
   ncol_end=ncol+ncol_memb-ndof_el  ! the starting column of the ending block of the member

!------------------------------------------------------------------------------------------
   CALL AssembleMemberJacobian(ndof_el,niter,memb_info(i),v_root_a,omega_a,x(ncol:ncol+ncol_memb-1),&
                           & nz_memb,irn_memb,jcn_memb,coef_memb,aero_flag,grav_flag) ! assemble the coefficient matrix within the member

   CALL  Extract2DElement(nz_memb,irn_memb,jcn_memb,coef_memb,coef_tm,0,0)
   CALL AssemblePointJ(-1,member(i,1),nrow-NSTRN,ncol) ! assemble the Jacobian related with the starting point

   !insert the member coef into the global matrices excluding the starting portion of the first element
   !and the ending portion of the last element. If there is only one element for static analysis, it is not necessary
   !-------------------------------------------------------------------------------------------------------------------
!	IF(ncol_memb>NDOF_ND) coef(nrow:nrow_end-1,ncol:ncol+ncol_memb-1)=coef_memb(NDOF_ND+1:ncol_memb,:)

	IF(ncol_memb>NDOF_ND) THEN
	   DO j=1,nz_memb
	      IF(irn_memb(j)>NDOF_ND.AND.irn_memb(j)<=ncol_memb) THEN
		      ne=ne+1
		      irn(ne)=nrow-1+irn_memb(j)-NDOF_ND
			  jcn(ne)=ncol-1+jcn_memb(j)
              coef(ne)=coef_memb(j)
		  ENDIF
	   ENDDO
	ENDIF

   ! Dealing with the ending point
   !-----------------------------------
   CALL  Extract2DElement(nz_memb,irn_memb,jcn_memb,coef_memb,coef_tm,ncol_memb,ncol_memb-ndof_el)

   CALL AssemblePointJ(1,member(i,2),nrow_end,ncol_end)

   DEALLOCATE(jcn_memb,irn_memb,coef_memb)
   IF(init_flag/=0) DEALLOCATE(init_memb)

ENDDO

! If some connection points have prescribed displacements, coef should be modified correspondingly
! know the corresponding equilibrium equation will be eliminated and replaced with an identity equation
! for the prescribed displacement
!-------------------------------------------------------------------------------------------------------------------
IF(assemble_flag==0) THEN
	DO i=1,SIZE(dof_con)
	  IF(dof_con(i)/=0) THEN ! a connection point
		kp_dof=dof_all(i,:)

		nrow=index_kp(i,1)-1
        ncol=index_kp(i,2)-1
        DO j=1,NSTRN
		    pre_dof=kp_dof(j)
			IF(pre_dof>0.AND.pre_dof<=NSTRN) THEN ! a displacement/rotation condition
			  !coef(nrow+pre_dof,:)=0.0D0
			  WHERE(irn==nrow+pre_dof) coef=0.0D0
			  !coef(nrow+pre_dof,ncol+pre_dof)=1.0D0
			  ne=ne+1
              irn(ne)=nrow+pre_dof
			  jcn(ne)=ncol+pre_dof
			  coef(ne)=1.0D0
			ENDIF
        ENDDO
	ENDIF

  ENDDO
ENDIF

IF(DEBUG)THEN
	WRITE(IOUT,*) 'COEFFICIENT MATRIX'
	WRITE(IOUT,*)ne
	DO j=1, ne
		WRITE(IOUT,'(1X,2i5,ES20.12)')irn(j),jcn(j),coef(j)
	ENDDO
ENDIF

9999 IF(error/='')THEN
    WRITE(0,*) 'System.f90 : ',error
    ERROR STOP 125
ENDIF

CONTAINS

!> Assemble the Jacobian related with trailing points
!-----------------------------------------------------------------
SUBROUTINE  AssemblePointJ(flag,kp,nrow_mb,ncol_mb)

INTEGER,INTENT(IN)::flag !< indicating whether it is the starting point or the ending point: -1-starting, 1-ending
INTEGER,INTENT(IN)::kp !< the point number
INTEGER,INTENT(IN)::nrow_mb,ncol_mb

INTEGER::nrow_kp,ncol_kp

REAL(DBL)::ekttek(NDIM,NDIM,NDIM)   ! e_k.\theta^T+ \theta.e_k^T
REAL(DBL)::theta(NDIM)
REAL(DBL)::eCTtheta(NDIM,NDIM,NDIM)  !derivatives of C^T w.r.t theta, eCTtheta(k,:,:)= d_C^T/d_thetak
INTEGER:: i,j

!Extract point information
!----------------------------
nrow_kp=index_kp(kp,1)  ! starting row for the point
ncol_kp=index_kp(kp,2)  ! starting column for the point

x_pt=x(ncol_kp:ncol_kp+5) ! the point variables
kp_dof=dof_all(kp,:) ! the prescribed dof
kp_cond=cond_all(kp,:)  ! the prescribed values
kp_follower=follower_all(kp,:)  ! whether it is a follower

! Deal with follower conditions
!-----------------------------------------
IF(ANY(kp_follower==1)) THEN
     IF(kp_dof(1)==7)  THEN
		theta=x_pt(4:6)
	 ELSE IF (kp_dof(1)==10) THEN
        theta=x_pt(1:3)
	 ENDIF
	 CALL CT_THETA(theta,DirCosineTRodrigues(theta),ekttek,eCTtheta)
ENDIF

IF(dof_con(kp)==0) THEN  !If the trailing point is a boundary point,

!	coef(nrow_kp:nrow_kp+11,ncol_mb:ncol_mb+ndof_el-1)=coef_tm
	CALL Insert1DElement(ne,coef_tm,irn,jcn,coef,nrow_kp-1,ncol_mb-1)
	IF(assemble_flag==0)THEN
		DO j=1,NSTRN
			pre_dof=kp_dof(j)
			IF(pre_dof/=0) THEN
				IF(pre_dof>NSTRN) THEN
					!coef(nrow_kp+pre_dof-1,ncol_kp+j-1)=flag ! force type conditions
					ne=ne+1
					irn(ne)=nrow_kp+pre_dof-1
					jcn(ne)=ncol_kp+j-1
					coef(ne)=flag
				 ENDIF
				IF(pre_dof<=NSTRN) THEN
					!coef(nrow_kp+pre_dof-1,ncol_kp+j-1)=-flag ! displacement type conditions
					ne=ne+1
					irn(ne)=nrow_kp+pre_dof-1
					jcn(ne)=ncol_kp+j-1
					coef(ne)=-flag
				ENDIF
			ENDIF
		ENDDO

		IF(ANY(kp_follower==1))  CALL PointFollowerJ(flag,nrow_kp-1,ncol_kp-1,eCTtheta)  ! add the contribution due to follower forces/moments
	ENDIF

ELSE IF(dof_con(kp)==1) THEN ! the trailing point  is a connection point

	    dof_con(kp)=2 ! indicated the connection point has been used

		!coef(nrow_kp:nrow_kp+11,ncol_mb:ncol_mb+ndof_el-1)=coef_tm
		CALL Insert1DElement(ne,coef_tm,irn,jcn,coef,nrow_kp-1,ncol_mb-1)

		IF(assemble_flag==0)THEN
			DO i=1,NSTRN
			    ne=ne+1
				!coef(nrow_kp+5+i,ncol_kp+i-1)=flag
				irn(ne)=nrow_kp+5+i
				jcn(ne)=ncol_kp+i-1
				coef(ne)=flag
			ENDDO

			IF(ANY(kp_dof>NSTRN).AND.ANY(kp_follower==1))&
			& CALL PointFollowerJ(1,nrow_kp-1,ncol_kp-1,eCTtheta)  ! add the contribution due to follower forces/moments
		ENDIF

ELSE IF(dof_con(kp)==2) THEN

	IF(assemble_flag==0)THEN
		DO i=1,NSTRN
			!coef(nrow_mb+i-1,ncol_kp+i-1)=flag
			ne=ne+1
			irn(ne)=nrow_mb+i-1
			jcn(ne)=ncol_kp+i-1
			coef(ne)=flag
		ENDDO
	ENDIF
   	!coef(nrow_kp:nrow_kp+5,ncol_mb:ncol_mb+ndof_el-1)=coef_tm(:NSTRN,:)
	CALL Insert1DElement(ne,coef_tm(:NSTRN,:),irn,jcn,coef,nrow_kp-1,ncol_mb-1)
	!coef(nrow_mb:nrow_mb+5,ncol_mb:ncol_mb+ndof_el-1)=coef_tm(7:NDOF_ND,:)
	CALL Insert1DElement(ne,coef_tm(7:NDOF_ND,:),irn,jcn,coef,nrow_mb-1,ncol_mb-1)
ENDIF

END SUBROUTINE  AssemblePointJ
!----------------------------------------------------------

END SUBROUTINE AssembleJacobian
!***********************************************************

!************************************************************
!*                                                          *
!>  Assemble the right hand side
!*															*
!************************************************************
SUBROUTINE AssembleRHS(ndof_el,memb_info,v_root_a,omega_a,member,error,&
			& ncond_mb,mb_condition,distr_fun,dof_con,x,rhs,aero_flag,grav_flag,init_cond)

INTEGER,INTENT(IN)::ndof_el !<#ioaero::ndof_el
INTEGER,INTENT(IN)::aero_flag    !<#ioaero::aero_flag
INTEGER,INTENT(IN)::grav_flag   !<#ioaero::grav_flag
TYPE (MemberInf),INTENT(IN)::memb_info(:)   !< contains the member parameters of the whole structure

REAL(DBL),INTENT(IN)::v_root_a(:)   !< linear velocity of frame a
REAL(DBL),INTENT(IN)::omega_a(:)    !< angular velocity of frame a
REAL(DBL),INTENT(IN) ::distr_fun(:,:)   !<#ioaero::distr_fun
INTEGER,INTENT(IN)   ::member(:,:)  !<#ioaero::member
INTEGER,INTENT(IN)   ::ncond_mb     !<#ioaero::ncond_mb
INTEGER              ::dof_con(:)
REAL(DBL),INTENT(IN) :: x(:)    !<solution vector
TYPE(PrescriInf),INTENT(IN)::mb_condition(:)    !<#ioaero::mb_condition

REAL(DBL),INTENT(OUT)::rhs(:)
CHARACTER(*),INTENT(OUT)::error

REAL(DBL),OPTIONAL,INTENT(IN)::init_cond(:,:)   !<#ioaero::init_cond

REAL(DBL),ALLOCATABLE::rhs_memb(:)    !< rhs for a member

INTEGER::i,j
INTEGER:: nrow,ncol,nrow_end
INTEGER::pre_dof !< prescibed dof of the connection point

rhs=0.0D0

DO i=1, SIZE(member,1)

   CALL ExtractMemberProperties(i,memb_info(i),member,ncond_mb,mb_condition,distr_fun,error,init_cond)

   ALLOCATE(rhs_memb(ncol_memb+NDOF_ND),STAT=allo_stat)
   IF(MemoryError('rhs_memb',error)) GOTO 9999

   nrow=index_mb(i,1)  ! the starting row of the member block
   ncol=index_mb(i,2)  ! the starting column of the member block
   nrow_end=nrow+ncol_memb-NDOF_ND  ! the starting row of the ending block of the member

   CALL AssembleMemberRHS(ndof_el,memb_info(i),v_root_a,omega_a,x(ncol:ncol+ncol_memb-1),rhs_memb,aero_flag,grav_flag,init_cond)

   CALL AssemblePointRHS(-1,member(i,1),rhs_memb(:NDOF_ND),nrow-NSTRN) ! Assemble the starting point

   !insert the member rhs into the global matrices excluding the starting portion of the first element
   !and the ending portion of the last element. If there is only one element within the member
   ! for static analysis, it is not necessary
   !-------------------------------------------------------------------------------------------------------------------
   IF(ncol_memb>NDOF_ND) rhs(nrow:nrow_end-1)=rhs_memb(NDOF_ND+1:ncol_memb)

   CALL AssemblePointRHS(1,member(i,2),rhs_memb(ncol_memb+1:),nrow_end) ! assemble the ending point

   DEALLOCATE(rhs_memb)
   IF(init_flag/=0) DEALLOCATE(init_memb)

ENDDO

! If some connection points have prescribed displacements/rotations, rhs should be modified correspondingly
!-------------------------------------------------------------------------------------------------------------------
DO i=1,SIZE(dof_con)
	IF(dof_con(i)/=0) THEN ! a connection point
		kp_dof=dof_all(i,:)
        kp_cond=cond_all(i,:)

		nrow=index_kp(i,1)-1
        DO j=1,NSTRN
		    pre_dof=kp_dof(j)
			IF(pre_dof>0.AND.pre_dof<=NSTRN) rhs(nrow+pre_dof)=kp_cond(j)
        ENDDO
	ENDIF

ENDDO

IF(DEBUG)THEN
	WRITE(IOUT,*) 'RIGHT HAND SIDE'
	write(iout,*)nsize
	DO j=1, nsize
		WRITE(IOUT,'(1X,i5,1ES20.12)')j,	rhs(j)
	ENDDO
ENDIF

9999 IF(error/='')THEN
    WRITE(0,*) 'System.f90 : ',error
    ERROR STOP 126
ENDIF

CONTAINS

!>  Assemble the Jacobian related with trailing points
!----------------------------------------------------------
SUBROUTINE  AssemblePointRHS(flag,term_pt,rhs_tm,nrow_mb)

INTEGER,INTENT(IN)::flag !< indicating whether it is the starting point or the ending point: -1-starting, 1-ending
INTEGER,INTENT(IN)::term_pt !< the point number
REAL(DBL),INTENT(IN)::rhs_tm(:) !< the right hand side of the member associated with the end point
INTEGER,INTENT(IN)::nrow_mb

INTEGER::nrow_kp,ncol_kp
INTEGER::free_dof,for_dof
INTEGER:: i,j

nrow_kp=index_kp(term_pt,1)  ! starting row for the point
ncol_kp=index_kp(term_pt,2)  ! starting column for the point

x_pt=x(ncol_kp:ncol_kp+5) ! the point variables
kp_dof=dof_all(term_pt,:) ! the prescribed dof
kp_cond=cond_all(term_pt,:)  ! the prescribed values

IF(ANY(follower_all(term_pt,:)==1)) &
& kp_cond=UpdateFollower(kp_dof,follower_all(term_pt,:),kp_cond,x_pt)

!If the trailing point is a boundary point,
!insert the corresponding portion of the attaching  element
!--------------------------------------------------------
IF(dof_con(term_pt)==0) THEN
	DO j=1,NSTRN

		pre_dof=kp_dof(j)
		IF(pre_dof>NSTRN) THEN ! force type conditions

			free_dof=pre_dof-NSTRN
			rhs(nrow_kp+free_dof-1)=rhs(nrow_kp+free_dof-1)+kp_cond(j)  ! prescribed conditions are external force/moments at the boundary
			rhs(nrow_kp+pre_dof-1)=rhs(nrow_kp+pre_dof-1)-flag*x_pt(j)

		ELSE IF(pre_dof<=NSTRN) THEN  ! displacement type conditions

			free_dof=pre_dof+NSTRN
			rhs(nrow_kp+free_dof-1)=rhs(nrow_kp+free_dof-1)-kp_cond(j)*flag
 			rhs(nrow_kp+pre_dof-1)=rhs(nrow_kp+pre_dof-1)+flag*x_pt(j) ! solved values are interal forces/moments

		ENDIF
	ENDDO

	rhs(nrow_kp:nrow_kp+11)=rhs(nrow_kp:nrow_kp+11)+rhs_tm

ELSE IF(dof_con(term_pt)==1) THEN ! it is a connection point not used yet

    dof_con(term_pt)=2 ! indicated the connection point has been used

	DO i=1,NSTRN
	! assemble force type conditions into rhs
	!----------------------------------------------
		for_dof=kp_dof(i)-NSTRN
		IF(for_dof>0) rhs(nrow_kp+for_dof-1)=rhs(nrow_kp+for_dof-1)+kp_cond(i)
	ENDDO

	rhs(nrow_kp+NSTRN:nrow_kp+11)=rhs(nrow_kp+NSTRN:nrow_kp+11)-flag*x_pt  ! correction from the calculated displacement/rotations at the rotation point
	rhs(nrow_kp:nrow_kp+11)=rhs(nrow_kp:nrow_kp+11)+rhs_tm

ELSE

	rhs(nrow_kp:nrow_kp+5)=rhs(nrow_kp:nrow_kp+5)+rhs_tm(1:NSTRN)

	rhs(nrow_mb:nrow_mb+5)=rhs(nrow_mb:nrow_mb+5)+rhs_tm(NSTRN+1:NDOF_ND)-flag*x_pt

ENDIF

END SUBROUTINE  AssemblePointRHS
!---------------------------------------

END SUBROUTINE AssembleRHS
!***********************************************************

!************************************************************
!*                                                          *
!>  Add the contribution to Jacobian matrix due to follower
!! point force or moments
!*                                                          *
!************************************************************
SUBROUTINE PointFollowerJ(flag,nrow,ncol,eCTtheta)

INTEGER,INTENT(IN)::flag
INTEGER,INTENT(IN)::nrow,ncol
REAL(DBL),INTENT(IN)::eCTtheta(:,:,:)

INTEGER:: str_row_fol !< starting row for inserting the jacobian due to follower conditions

IF(ANY(kp_follower(1:3)==1)) THEN
   IF(kp_dof(1)==7) str_row_fol=nrow
   IF(kp_dof(1)==10) str_row_fol=nrow+3
!   coef(str_row_fol+1:str_row_fol+3,ncol+4:ncol+6)=&
!  & coef(str_row_fol+1:str_row_fol+3,ncol+4:ncol+6)-flag*FollowerJ(kp_follower(1:3),flag*kp_cond(1:3),eCTtheta)
  CALL Insert1DElement(ne,-flag*FollowerJ(kp_follower(1:3),flag*kp_cond(1:3),eCTtheta),&
                & irn,jcn,coef,str_row_fol,ncol+3)

 !& coef(str_row_fol+1:str_row_fol+3,ncol+4:ncol+6)-flag*FollowerJ(kp_follower(1:3),kp_cond(1:3),eCTtheta)
ENDIF

IF(ANY(kp_follower(4:6)==1)) THEN
   IF(kp_dof(4)==7) str_row_fol=nrow
   IF(kp_dof(4)==10) str_row_fol=nrow+3
!   coef(str_row_fol+1:str_row_fol+3,ncol+4:ncol+6)=&
!   & coef(str_row_fol+1:str_row_fol+3,ncol+4:ncol+6)-flag*FollowerJ(kp_follower(4:6),flag*kp_cond(4:6),eCTtheta)
   !coef(str_row_fol+1:str_row_fol+3,ncol+4:ncol+6)=-flag*FollowerJ(kp_follower(4:6),flag*kp_cond(4:6),eCTtheta)
   CALL Insert1DElement(ne,-flag*FollowerJ(kp_follower(4:6),flag*kp_cond(4:6),eCTtheta),&
                     & irn,jcn,coef,str_row_fol,ncol+3)
! & coef(str_row_fol+1:str_row_fol+3,ncol+4:ncol+6)-flag*FollowerJ(kp_follower(4:6),kp_cond(4:6),eCTtheta)
ENDIF

END SUBROUTINE PointFollowerJ
!************************************************************

END MODULE System