#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! ZUFCFT (Zomplex Unitary hessenberg Factored Compute First Transformation)
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine computes a two by two diagonal block of a unitary upper
! hessenberg matrix that is stored as a product of givens rotations
! and a complex diagonal matrix.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! INPUT VARIABLES:
!
!  N               INTEGER
!                    dimension of matrix
!
!  K               INTEGER
!                    index of the diagonal block
!
!  Q               REAL(8) array of dimension (3*(N-1))
!                    array of generators for givens rotations
!                    generators must be orthogonal to working precision
!
!  D               REAL(8) array of dimension (2*N)
!                    array of generators for complex diagonal matrix
!                    on output contains the eigenvalues
!
!  SHFT            COMPLEX(8)
!                    contains the shift need for the first transformation
!
! OUTPUT VARIABLES:
!
!  B               REAL(8) array of dimension (3)
!                    on exit contains the generators for the first
!                    transformation
!
!  INFO            INTEGER
!                   INFO = 0 implies successful computation
!                   INFO = -1 implies N is invalid
!                   INFO = -2 implies K is invalid
!                   INFO = -5 implies SHFT is invalid
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine ZUFCFT(N,K,Q,D,SHFT,B,INFO)

  implicit none

  ! input variables
  integer, intent(in) :: N, K
  integer, intent(inout) :: INFO
  real(8), intent(in) :: Q(3*N), D(2*N)
  complex(8), intent(in) :: SHFT
  real(8), intent(inout) :: B(3)

  ! compute variables
  real(8) :: nrm
  complex(8) :: block(2,2)

  ! initialize info
  INFO = 0

  ! check input in debug mode
  if (DEBUG) then

    ! check N
    call IARNAN(N,INFO)
    if (INFO.NE.0) then
      call UARERR(__FILE__,__LINE__,"N is invalid",INFO,-1)
      return
    end if
    call IARINF(N,INFO)
    if (INFO.NE.0) then
      call UARERR(__FILE__,__LINE__,"N is invalid",INFO,-1)
      return
    end if
    if (N < 2) then
      INFO = -1
      call UARERR(__FILE__,__LINE__,"N must be at least 2",INFO,INFO)
      return
    end if

    ! check K
    if ((K < 1).OR.(K > N-1)) then
      INFO = -2
      call UARERR(__FILE__,__LINE__,"K must be 1 <= K <= N-1",INFO,INFO)
      return
    end if

    ! check SHFT
    call ZARNAN(SHFT,INFO)
    if (INFO.NE.0) then
      call UARERR(__FILE__,__LINE__,"SHFT is invalid",INFO,-5)
      return
    end if
    call ZARINF(SHFT,INFO)
    if (INFO.NE.0) then
      call UARERR(__FILE__,__LINE__,"SHFT is invalid",INFO,-5)
      return
    end if

  end if

  ! get top block
  call ZUFTDB(N,K,Q,D,block,INFO)

  ! check INFO in debug mode
  if (DEBUG) then
    call UARERR(__FILE__,__LINE__,"ZUFTDB failed",INFO,INFO)
    if (INFO.NE.0) then
      return
    end if
  end if

  ! shift first entry
  block(1,1) = block(1,1) - SHFT

  ! bulge
  call ZARCG43(dble(block(1,1)),aimag(block(1,1)),dble(block(2,1)), &
    aimag(block(2,1)),B(1),B(2),B(3),nrm,INFO)

  ! check INFO in debug mode
  if (DEBUG) then
    call UARERR(__FILE__,__LINE__,"ZARCG43 failed",INFO,INFO)
    if (INFO.NE.0) then
      return
    end if
  end if

end subroutine ZUFCFT


!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      subroutine RKPMshape3d(shp,b,bd,cpt,cjt,haj,wjt)
!
!
!     This subroutine is to calculate the shape functions and
!     their derivatives for the moving least square method
!     ---- reproducing kernel method.
!     This code is only offering shape functions and its' 1st
!     order derivatives for 3-D case.
!
!     That is :  N_{jt}(xpt,ypt)
!     jt: the integer index for shape function, i.e.
!         we are computing N_{jt}; 1<jt<nep
!
!     Date: June, 1994
!
!     -arguments:
!
!  i      cjt(3):  the jt th particle's global coordinations;
!                  (jt is the integer index for shape function)
!                  i.e.  N_{jt},  1 < jt < nep
!
!                      such as x(jt):= cjt(1)
!                              y(jt):= cjt(2)
!                              z(jt):= cjt(3)
!
!  i      cpt(3): the point at where the shape function is
!                     evaluated.;
!                 x(pt):= cpt(1)
!                 y(pt):= cpt(2)
!                 z(pt):= cpt(3)
!
!
!  i      haj(3): the radius of particle's compact support.
!                 the radius at X direction: haj(1)
!                 the radius at Y direction: haj(2)
!                 the radius at Z direction: haj(3)
!
!  i      wjt: integration weight at particle jt;
!
!     o   coref: correct function at pt. cpt and particle cjt
!         C(cjt, cpt);
!
!
!     o   shp: shape functions and their 1st derivative
!                       at point cpt;
!
!     o   shpd(1): the X-derivative of shape function
!                          N_{jt},x  at cpt;
!
!     o   shpd(2): the Y-derivative of shape function
!                          N_{jt},y at cpt;
!
!     o   shpd(3): the Z-derivative of shape function
!                          N_{jt},z at cpt;
!
!     The subroutine is only designed for solving 3-D locally
!     linear interpolation moving field problem. i.e.
!
!    l   p0   := (1,0,0,0);
!
!    l   p(4) := (1,x,y,z);
!
!    There two different subroutines are called by this subroutine:
!
!    window2dr.f ----- the cartesian product type window function;
!
!    window2dc.f ----- the circular disc type window function
!
!**************************************************************
!
      implicit none
      real(8) :: cpt(3),cjt(3),shp,shpd(3)
      real(8) :: p(4),pdx(4),pdy(4),pdz(4)
      real(8) :: b(4),bd(3,4),haj(3)
      real(8) :: coref,cdx,cdy,cdz
      real(8) :: xp,yp,zp,xj,yj,zj
      real(8) :: a1,a2,a3
      real(8) :: aw,awdx,awdy,awdz
      real(8) :: awdxx,awdyy,awdzz
      real(8) :: awdxy,awdyz,awdzx
      real(8) :: wjt,xx,yy,zz
      integer :: i,nn

!.....find the polynominal p
      nn = 4
      xp = cpt(1)
      yp = cpt(2)
      zp = cpt(3)
      xj = cjt(1)
      yj = cjt(2)
      zj = cjt(3)
!.....weight
!.....assign value to working array
      a1 = haj(1)
      a2 = haj(2)
      a3 = haj(3)

      p(1) = 1.00
      p(2) = (xj - xp)/a1
      p(3) = (yj - yp)/a2
      p(4) = (zj - zp)/a3

      pdx(1) =   0.00
      pdx(2) = - 1.00/a1
      pdx(3) =   0.00
      pdx(4) =   0.00

      pdy(1) =   0.00
      pdy(2) =   0.00
      pdy(3) = - 1.00/a2
      pdy(4) =   0.00

      pdz(1) =   0.00
      pdz(2) =   0.00
      pdz(3) =   0.00
      pdz(4) = - 1.00/a3

!.....compute correct function  C
      coref = 0.00
      do i = 1, nn
       coref = coref + p(i)*b(i)
      enddo

      cdx = 0.00
      cdy = 0.00
      cdz = 0.00
      do i = 1, nn
       cdx = cdx + pdx(i)*b(i) + p(i)*bd(1,i)
       cdy = cdy + pdy(i)*b(i) + p(i)*bd(2,i)
       cdz = cdz + pdz(i)*b(i) + p(i)*bd(3,i)
      enddo

!.....check whether or not the point is within the compact support
      xx = abs(xj - xp)/a1
      yy = abs(yj - yp)/a2
      zz = abs(zj - zp)/a3

      if (xx .ge. 2.0 .or. yy .ge. 2.0 .or. zz .ge. 2.0) then
        shp      = 0.00
        shpd(1)  = 0.00
        shpd(2)  = 0.00
        shpd(3)  = 0.00
      else
          call window3d(aw,awdx,awdy,awdz,  &
              awdxx,awdyy,awdzz,            &
              awdxy,awdyz,awdzx,            &
              xp,yp,zp,xj,yj,zj,a1,a2,a3)

!.....calculate the shape functions
          shp   = coref*aw*wjt

!.....calculate the 1st derivatives of the shape function
          shpd(1) = (cdx*aw+coref*awdx)*wjt
          shpd(2) = (cdy*aw+coref*awdy)*wjt
          shpd(3) = (cdz*aw+coref*awdz)*wjt
      endif
  999 continue
      return
      end

!***********************************************************
      subroutine window3d(aw,awdx,awdy,awdz,  &
                          awdxx,awdyy,awdzz,  &
                          awdxy,awdyz,awdzx,  &
                  xp,yp,zp,xj,yj,zj,a1,a2,a3)
!***********************************************************
!
!   This is a subroutine to compute 3-D cubic spline window
!   function and its first and second derivatives.
!   In this subroutine, the window function is constructed by
!   Cartesian product.
!   The mathematical formulation of this code is based on:
!
!   ``Ten lectures on Wavelets'' by Ingrid Daubechies,
!     page 79.
!
!   arguments:
!
!   i         a1:  dilation parameter in X-direction;
!
!   i         a2:  dilation parameter in Y-direction;
!
!   i         a3:  dilation parameter in Z-direction;
!
!   Note: there is a difference between dilation parameter and
!         the dilation coefficient.
!
!      o      aw:  3-D cubic spline window function:
!
!   Note:     Phi_r := (1/rho) Phi(X/rho).
!
!      o    awdx:  d/dx(aw);
!
!      o    awdy:  d/dy(aw);
!
!      o    awdz:  d/dz(aw);
!
!      o   awdxx:  d^2/dx^2(aw);
!
!      o   awdyy:  d^2/dy^2(aw);
!
!      o   awdzz:  d^2/dz^2(aw);
!
!      o   awdxy:  d^2/dxdy(aw);
!
!      o   awdyz:  d^2/dydz(aw);
!
!      o   awdzx:  d^2/dzdx(aw);
!
!   i         xp:  X-coordinate for arbitary point;
!
!   i         yp:  Y-coordinate for arbitary point;
!
!   i         zp:  Z-coordinate for arbitary point;
!
!   i         xj:  X-coordinate for particle j;
!
!   i         yj:  Y-coordinate for particle j;
!
!   i         zj:  Y-coordinate for particle j;
!
!.....cartsian product window function
!
!    Author: Shaofan Li
!
!    Date: October, 1996
!
!
!*************************************************************
!
      implicit none

!.....global variables
      real(8) aw,awdx,awdy,awdz
      real(8) awdxx,awdyy,awdzz
      real(8) awdxy,awdyz,awdzx
      real(8) a1,a2,a3
      real(8) xj,yj,zj,xp,yp,zp

!.....Local variables
      real(8) hv,zero
      real(8) x1,x2,x3,y1,y2,y3,z1,z2,z3
      real(8) dx1,dx2,dy1,dy2,dz1,dz2
      real(8) onen,onep,twon,twop
      real(8) awx,awxd,awxdd
      real(8) awy,awyd,awydd
      real(8) awz,awzd,awzdd

!.....normalize the argument
      x1  = (xj-xp)/a1
      x2  =  x1*x1
      x3  =  x2*x1

      y1  = (yj-yp)/a2
      y2  =  y1 * y1
      y3  =  y2 * y1

      z1  = (zj-zp)/a3
      z2  =  z1 * z1
      z3  =  z2 * z1

      twon = -2.00
      onen = -1.00
      zero =  0.00
      onep =  1.00
      twop =  2.00

!.....dfx := d(xr)/dx; dfy:= d(yr)/dy
      dx1  = -1.00/a1
      dx2 = dx1 * dx1
      dy1  = -1.00/a2
      dy2 = dy1 * dy1
      dz1  = -1.00/a3
      dz2 = dz1 * dz1

      hv = 1.0/(a1*a2*a3)

      if((x1.ge.twon).and.(x1.lt.onen)) then
        awx   = (1.0/6.0)*(2.0 + x1)**3.
        awxd  = dx1*0.5*(2.0 + x1)**2.
        awxdd = dx2*(2.0 + x1)
      elseif ((x1.ge.onen).and.(x1.lt.zero)) then
        awx   = 2.0/3.0 - x2 - 0.5*x3
        awxd  = - dx1*(2.0*x1 + 1.5*x2)
        awxdd = - dx2*(2.0 + 3.0*x1 )
      elseif ((x1.ge.zero).and.(x1.lt.onep)) then
        awx   = 2.0/3.0 - x2 + 0.5*x3
        awxd  = - dx1*(2.0*x1 - 1.5*x2)
        awxdd = - dx2*(2.0 - 3.0*x1 )
      elseif ((x1.ge.onep).and.(x1.le.twop)) then
        awx   = (1.0/6.0)*(2.0 - x1)**3.
        awxd  = - dx1*0.5*(2.0 - x1)**2.
        awxdd = dx2*(2.0 - x1)
      else
        awx   = 0.00
        awxd  = 0.00
        awxdd = 0.00
      endif

      if((y1.ge.twon).and.(y1.lt.onen)) then
        awy   = (1.0/6.0)*(2.0 + y1)**3.
        awyd  = dy1*0.5*(2.0 + y1 )**2.
        awydd = dy2*(2.0 + y1 )
      elseif ((y1.ge.onen).and.(y1.lt.zero)) then
        awy   = 2.0/3.0 - y2 - 0.5*y3
        awyd  = - dy1*(2.0*y1 + 1.5*y2)
        awydd = - dy2*(2.0 + 3.0*y1 )
      elseif ((y1.ge.zero).and.(y1.lt.onep)) then
        awy   = 2.0/3.0 - y2 + 0.5*y3
        awyd  = - dy1*(2.0*y1 - 1.5*y2)
        awydd = - dy2*(2.0 - 3.0*y1 )
      elseif ((y1.ge.onep).and.(y1.le.twop)) then
        awy   = (1.0/6.0)*(2.0 - y1)**3.
        awyd  = - dy1*0.5*(2.0 - y1)**2.
        awydd = dy2*(2.0 - y1)
      else
        awy   = 0.00
        awyd  = 0.00
        awydd = 0.00
      endif

      if((z1.ge.twon).and.(z1.lt.onen)) then
        awz   = (1.0/6.0)*(2.0 + z1)**3.
        awzd  = dz1*0.5*(2.0 + z1 )**2.
        awzdd = dz2*(2.0 + z1 )
      elseif ((z1.ge.onen).and.(z1.lt.zero)) then
        awz   = 2.0/3.0 - z2 - 0.5*z3
        awzd  = - dz1*(2.0*z1 + 1.5*z2)
        awzdd = - dz2*(2.0 + 3.0*z1 )
      elseif ((z1.ge.zero).and.(z1.lt.onep)) then
        awz   = 2.0/3.0 - z2 + 0.5*z3
        awzd  = - dz1*(2.0*z1 - 1.5*z2)
        awzdd = - dz2*(2.0 - 3.0*z1 )
      elseif ((z1.ge.onep).and.(z1.le.twop)) then
        awz   = (1.0/6.0)*(2.0 - z1)**3.
        awzd  = - dz1*0.5*(2.0 - z1)**2.
        awzdd = dz2*(2.0 - z1)
      else
        awz   = 0.00
        awzd  = 0.00
        awzdd = 0.00
      endif

      aw    = awx*awy*awz*hv
      awdx  = awxd*awy*awz*hv
      awdy  = awx*awyd*awz*hv
      awdz  = awx*awy*awzd*hv
      awdxy = awxd*awyd*awz*hv
      awdyz = awx*awyd*awzd*hv
      awdzx = awxd*awy*awzd*hv
      awdxx = awxdd*awy*awz*hv
      awdyy = awx*awydd*awz*hv
      awdzz = awx*awy*awzdd*hv

      return
      end


      SUBROUTINE DIS2D8
C
C     2-D, 8 GRID POINT ISOPARAMETRIC STRUCTURAL ELEMENT
C     DIFFERENTIAL STIFFNESS MATRIX ROUTINE
C
      REAL             KX,KY
      DOUBLE PRECISION KIJ,G,B,XI,ETA,DNXI,DNETA,XX,TB,DNL,PT,H,XJB,
     1                 XXJB,DETERM,DNC,GSUBE,DUMARG,
     2                 BT,TEMPAR,TEMP,DNX,DNY,SAVE,TSAVE,
     3                 KWD(36),CID(18),CJD(18),KMULT(18),DHH,
     4                 THICK,PSTMUL(9),PREMUL(9),E1T
      DIMENSION        SIG(3),ALPHAS(3),ST(3),SEMP(9),TTB(9),STB(9),
     1                 BB(72),DB(72),S(6),R(9),SE1T(6),DN(8),
     2                 G(9),QQ(15),XI(8),ETA(8),TB(9),XY1(3),XY2(3),
     3                 B(12),BT(12),ECPT(1),TEMP(9),TEMPAR(1),DNX(1),
     4                 DNY(1),DNXI(1),DNETA(1),SAVE(72),TSAVE(72),
     5                 VEC(3),VVEC(3),VECI(3),VECJ(3),VECK(3),E1T(9),
     6                 IWS(2,3)
      COMMON /DS1AAA/  NPVT,ICSTM,NCSTM
      COMMON /DS1AET/  NECPT(1),NGRID(8),ID1,TH,MATID1,T,ISYS1,X1,Y1,Z1,
     1                 ISYS2,X2,Y2,Z2,ISYS3,X3,Y3,Z3,ISYS4,X4,Y4,Z4,
     2                 ISYS5,X5,Y5,Z5,ISYS6,X6,Y6,Z6,ISYS7,X7,Y7,Z7,
     3                 ISYS8,X8,Y8,Z8,TTEMP,EDT,ISETNO,TGRID(8),DISP(24)
      COMMON /MATIN /  MATID,INFLAG,ELTEMP,STRESS,SINTH,COSTH
      COMMON /MATOUT/  G11,G12,G13,G22,G23,G33,RHO,ALPHA1,ALPHA2,ALP12,
     1                 TREF,GE,KX,KY,C
      COMMON /DS1ADP/  KIJ(36),XX(16),DNC(16),DNL(16),XXJB(2,2),XJB(4),
     1                 PT(3),H(3),G,B,BT,TB,DETERM,GSUBE,DUMARG,TSAVE
      EQUIVALENCE      (ALPHAS(1),ALPHA1),
     1                 (ECPT(1),NECPT(1)),(TEMP(1),B(1)),
     2                 (DNC(1),DNXI(1)),(DNC(9),DNETA(1)),
     3                 (DNL(1),DNX(1)),(DNL(9),DNY(1)),(QQ(1),G11),
     4                 (TEMPAR(1),BT(1)),(XY1(1),X1),(XY2(1),X2)
      DATA     XI   /  -1.D0, 1.D0, 1.D0,-1.D0, 0.D0, 1.D0, 0.D0,-1.D0/
      DATA     ETA  /  -1.D0,-1.D0, 1.D0, 1.D0,-1.D0, 0.D0, 1.D0, 0.D0/
C
C     ECPT LIST
C                                      IN
C                                      THIS
C     ECPT       DESCRIPTION           ROUTINE        TYPE
C     ******************************************************************
C     ECPT( 1) = ELEMENT ID            NECPT(1)       INTEGER
C     ECPT( 2) = GRID POINT 1          NGRID(1)       INTEGER
C     ECPT( 3) = GRID POINT 2          NGRID(2)       INTEGER
C     ECPT( 4) = GRID POINT 3          NGRID(3)       INTEGER
C     ECPT( 5) = GRID POINT 4          NGRID(4)       INTEGER
C     ECPT( 6) = GRID POINT 5          NGRID(5)       INTEGER
C     ECPT( 7) = GRID POINT 6          NGRID(6)       INTEGER
C     ECPT( 8) = GRID POINT 7          NGRID(7)       INTEGER
C     ECPT( 9) = GRID POINT 8          NGRID(8)       INTEGER
C     ECPT(10) = COORD SYS ID-STRESS   ID1            INTEGER
C     ECPT(11) = ANIS. MATERIAL ANGLE  TH             REAL
C     ECPT(12) = MATERIAL ID           MATID1         INTEGER
C     ECPT(13) = THICKNESS             T              REAL
C     ECPT(14) = COORD SYS ID 1        ISYS1          INTEGER
C     ECPT(15) = X1                    X1             REAL
C     ECPT(16) = Y1                    Y1             REAL
C     ECPT(17) = Z1                    Z1             REAL
C     ECPT(18) = COORD SYS ID 2        ISYS2          INTEGER
C     ECPT(19) = X2                    X2             REAL
C     ECPT(20) = Y2                    Y2             REAL
C     ECPT(21) = Z2                    Z2             REAL
C     ECPT(22) = COORD SYS ID 3        ISYS3          INTEGER
C     ECPT(23) = X3                    X3             REAL
C     ECPT(24) = Y3                    Y3             REAL
C     ECPT(25) = Z3                    Z3             REAL
C     ECPT(26) = COORD SYS ID 4        ISYS4          INTEGER
C     ECPT(27) = X4                    X4             REAL
C     ECPT(28) = Y4                    Y4             REAL
C     ECPT(29) = Z4                    Z4             REAL
C     ECPT(30) = COORD SYS ID 5        ISYS5          INTEGER
C     ECPT(31) = X5                    X5             REAL
C     ECPT(32) = Y5                    Y5             REAL
C     ECPT(33) = Z5                    Z5             REAL
C     ECPT(34) = COORD SYS ID 6        ISYS6          INTEGER
C     ECPT(35) = X6                    XL             REAL
C     ECPT(36) = Y6                    Y6             REAL
C     ECPT(37) = Z6                    Z6             REAL
C     ECPT(38) = COORD SYS ID 7        ISYS7          INTEGER
C     ECPT(39) = X7                    X7             REAL
C     ECPT(40) = Y7                    Y7             REAL
C     ECPT(41) = Z7                    Z7             REAL
C     ECPT(42) = COORD SYS ID 8        ISYS8          INTEGER
C     ECPT(43) = X8                    X8             REAL
C     ECPT(44) = Y8                    Y8             REAL
C     ECPT(45) = Z8                    Z8             REAL
C     ECPT(46) = ELEMENT TEMP          TTEMP          REAL
C     ECPT(47) = 0.                    EDT            REAL
C     ECPT(48) = TEMPERATURE SET       ISETNO         INTEGER
C     ECPT(49) = *
C     ECPT(. ) = *  GRID POINT TEMPERATURES
C     ECPT(56) = *
C     ECPT(57) = *
C     ECPT(. ) = *  TRANSLATIONAL DOF-S OF GRIDS FOR THIS ELEMENT
C     ECPT(80) = *
C
C
C     TEST FOR PIVOT POINT
C
      DO 10 KK = 1,8
      IF (NGRID(KK) .EQ. NPVT) GO TO 20
   10 CONTINUE
C
C     IF FALL HERE NO ELEMENT GRID POINT IS THE PIVOT POINT
C
      CALL MESAGE (-30,34,ECPT(1))
C
C     UNIT I VECTOR IS FROM GRID POINT 1 TO GRID POINT 2
C
   20 DO 30 I = 1,3
      VECI(I) = XY2(I) - XY1(I)
   30 CONTINUE
      VECIL = SQRT(VECI(1)**2 + VECI(2)**2 + VECI(3)**2)
      IF (VECIL .EQ. 0.0) GO TO 60
      VECI(1) = VECI(1)/VECIL
      VECI(2) = VECI(2)/VECIL
      VECI(3) = VECI(3)/VECIL
C
C     K VECTOR IS OBTAINED BY CROSSING I INTO VECTOR FROM GRID PT. 1 TO
C     GRID
C
      VECK(1) = VECI(2)*(Z4-Z1) - VECI(3)*(Y4-Y1)
      VECK(2) = VECI(3)*(X4-X1) - VECI(1)*(Z4-Z1)
      VECK(3) = VECI(1)*(Y4-Y1) - VECI(2)*(X4-X1)
      VECKL=SQRT(VECK(1)**2 + VECK(2)**2 + VECK(3)**2)
      IF (VECKL .EQ. 0.0) GO TO 60
      VECK(1) = VECK(1)/VECKL
      VECK(2) = VECK(2)/VECKL
      VECK(3) = VECK(3)/VECKL
C
C     J VECTOR IS OBTAINED BY CROSSING K INTO I
C
      VECJ(1) = VECK(2)*VECI(3) - VECK(3)*VECI(2)
      VECJ(2) = VECK(3)*VECI(1) - VECK(1)*VECI(3)
      VECJ(3) = VECK(1)*VECI(2) - VECK(2)*VECI(1)
C
      E1T(1) = VECI(1)
      E1T(2) = VECI(2)
      E1T(3) = VECI(3)
      E1T(4) = VECJ(1)
      E1T(5) = VECJ(2)
      E1T(6) = VECJ(3)
      E1T(7) = VECK(1)
      E1T(8) = VECK(2)
      E1T(9) = VECK(3)
      DO 40 I = 1,6
   40 SE1T(I) = E1T(I)
C
C     STORE ELEMENT COORDS FOR GRIDS 1 AND 2
C
      XX(1) = 0.
      XX(2) = 0.
      XX(3) = VECIL
      XX(4) = 0.
C
C     FOR GRIDS 3-8, THE X COORDINATE IS THE DOT PRODUCT OF HTE VECTOR
C     FROM GRID POINT 1 TO THE GRID POINT AND THE I VECTOR. THE Y COORD.
C     IS THE L OF THE I VECTOR CROSSED INTO THE VECTOR FROM GRID 1 TO
C     THE GRID POINT.
C
      DO 50 I = 3,8
      IXX  = 2*I - 1
      ISUB = 4*I + 11
      VEC(1)  = ECPT(ISUB  ) - X1
      VEC(2)  = ECPT(ISUB+1) - Y1
      VEC(3)  = ECPT(ISUB+2) - Z1
      XX(IXX) = VEC(1)*VECI(1) + VEC(2)*VECI(2) + VEC(3)*VECI(3)
      VVEC(1) = VECI(2)*VEC(3) - VECI(3)*VEC(2)
      VVEC(2) = VECI(3)*VEC(1) - VECI(1)*VEC(3)
      VVEC(3) = VECI(1)*VEC(2) - VECI(2)*VEC(1)
      XX(IXX+1) = SQRT(VVEC(1)**2 + VVEC(2)**2 + VVEC(3)**2)
   50 CONTINUE
      GO TO 70
C
C     INAPPROPRIATE GEOMETRY
C
   60 CALL MESAGE (30,31,ECPT(1))
      NOGO = 1
      RETURN
C
C     COMPUTE MATERIAL PROPERTIES
C
   70 TTH   = TH*3.1415927/180.
      SINTH = SIN(TTH)
      COSTH = COS(TTH)
      ELTEMP= TTEMP
      INFLAG= 2
      MATID = MATID1
      CALL MAT (ECPT(1))
      DO 80 I = 1,3
   80 G(I)  = QQ(I)
      G(4)  = QQ(2)
      G(5)  = QQ(4)
      G(6)  = QQ(5)
      G(7)  = QQ(3)
      G(8)  = QQ(5)
      G(9)  = QQ(6)
      THICK = T
      DO 90 I = 1,9
   90 R(I)  = G(I)
      IF (ISETNO .NE. 0) CALL GMMATS (R,3,3,0,ALPHAS,3,1,0,ST)
C
C     ZERO OUT THE KIJ AND SAVE MATRICES
C
      DO 100 I = 1,36
      KWD(I) = 0.D0
  100 KIJ(I) = 0.D0
      DO 110 I = 1,72
  110 SAVE(I) = 0.D0
C
      PT(1) =-0.57735027D0
      PT(2) =-PT(1)
      H(1)  = 1.D0
      H(2)  = 1.D0
      IF (ID1 .EQ. 2) GO TO 120
      PT(1) =-0.77459667D0
      PT(2) = 0.D0
      PT(3) =-PT(1)
      H(1)  = 5.D0/9.D0
      H(2)  = 8.D0/9.D0
      H(3)  = H(1)
C
C     2 OR 3 QUADRATURE POINTS
C
  120 DO 410 III = 1,ID1
      DO 410 JJJ = 1,ID1
C
C     COMPUTE GAUSS POINT STRESSES
C
C
C     COMPUTE DERIVATIVES WITH RESPECT TO XI AND ETA
C     EACH GRID POINT
C
      DO 130 N = 1,4
      DNXI(N)  = .25D0*XI(N)*(1.D0+PT(JJJ)*ETA(N))*
     1           (2.D0*PT(III)*XI(N)+PT(JJJ)*ETA(N))
      DNETA(N) = .25D0*ETA(N)*(1.D0+PT(III)*XI(N))*
     1           (PT(III)*XI(N)+2.D0*PT(JJJ)*ETA(N))
  130 CONTINUE
      DO 140 N = 5,7,2
C
      DNXI(N)  = -PT(III)*(1.D0+PT(JJJ)*ETA(N))
      DNETA(N) = .5D0*(1.D0-PT(III)*PT(III))*ETA(N)
  140 CONTINUE
C
      DO 150 N = 6,8,2
      DNXI(N)  = .5D0*XI(N)*(1.D0-PT(JJJ)*PT(JJJ))
      DNETA(N) = -PT(JJJ)*(1.D0+PT(III)*XI(N))
  150 CONTINUE
C
C     COMPUTE JACOBEAN
C
C           N1XI   N2XI   N3XI   N4XI   N5XI   N6XI   N7XI   N8XI
C     DNC = N1ETA  N2ETA  N3ETA  N4ETA  N5ETA  N6ETA  N7ETA  N8ETA
C
C          X1  Y1
C          X2  Y2
C          X3  Y3
C     XX = X4  Y4
C          X5  Y5
C          X6  Y6
C          X7  Y7
C          X8  Y8
C
      CALL GMMATD (DNC,2,8,0,XX,8,2,0,XJB)
C
C     XJB IS ROW-STORED-IT MUST BE COLUMN-STORED AND DOUBLY DIMENSIONED
C     FOR INVERSION
C
      K = 0
      DO 160 I = 1,2
      DO 160 J = 1,2
      K = K + 1
  160 XXJB(I,J) = XJB(K)
C
C     COMPUTE INVERSE AND DETERMINANT OF JACOBEAN
C
      CALL INVERD (2,XXJB,2,DUMARG,0,DETERM,ISING,IWS)
      IF (ISING .EQ. 2) CALL MESAGE (-30,143,ECPT(1))
      DHH = DETERM*H(III)*H(JJJ)
C
C     COMPUTE DERIVATIVES WITH RESPECT TO X AND Y
C
      K = 0
      DO 170 I = 1,2
      DO 170 J = 1,2
      K = K + 1
  170 XJB(K) = XXJB(I,J)
      CALL GMMATD (XJB,2,2,0,DNC,2,8,0,DNL)
C
C           N1X N2X N3X N4X N5X N6X N7X N8X
C     DNL = N1Y N2Y N3Y N4Y N5Y N6Y N7Y N8Y
C
      DO 180 I = 1,72
  180 BB(I) = 0.
C
C     SET UP INDICATOR FOR GRID POINT TEMPERATURES
C
      IDTEMP = 0
      DO 190 I = 1,8
      IF (TGRID(I) .NE. 0.) GO TO 200
  190 CONTINUE
      GO TO 210
  200 IDTEMP = 1
C
  210 DO 270 N = 1,8
C
      DO 220 I = 1,9
  220 SEMP(I) = 0.
      DO 230 I = 1,6
  230 S(I) = 0.
      S(1) = DNX(N)
      S(4) = DNY(N)
      S(5) = DNY(N)
      S(6) = DNX(N)
C
C     TRANSFORM TO ELEMENT COORDINATES
C
      IF (NECPT(4*N+10) .EQ. 0) GO TO 240
      CALL TRANSS (NECPT(4*N+10),TTB)
      CALL GMMATS (SE1T,2,3,0,TTB,3,3,0,STB)
      GO TO 260
  240 DO 250 I = 1,6
  250 STB(I) = SE1T(I)
  260 CALL GMMATS (S,3,2,0,STB,2,3,0,SEMP(1))
      N3 = 3*N
      BB(N3- 2) = SEMP(1)
      BB(N3- 1) = SEMP(2)
      BB(N3   ) = SEMP(3)
      BB(N3+22) = SEMP(4)
      BB(N3+23) = SEMP(5)
      BB(N3+24) = SEMP(6)
      BB(N3+46) = SEMP(7)
      BB(N3+47) = SEMP(8)
      BB(N3+48) = SEMP(9)
  270 CONTINUE
C
C     BRING IN G MATRIX
C
      CALL GMMATS (R,3,3,0,BB,3,24,0,DB)
C
C     COMPUTE STRESSES
C
      CALL GMMATS (DB,3,24,0,DISP,24,1,0,SIG)
C
C
C     COMPUTE GAUSS POINT  TEMPERATURES
C
      IF (ISETNO .EQ. 0) GO TO 350
      IF (IDTEMP .EQ. 1) GO TO 280
      RGTEMP = ELTEMP - TREF
      GO TO 330
C
C     ALL TEMPERATURES ARE DEFAULT VALUE
C
  280 DO 290 N = 1,4
      DN(N) = .25*(1.+PT(III)*XI(N))*(1.+PT(JJJ)*ETA(N))
     1        *(PT(III)*XI(N)+PT(JJJ)*ETA(N)-1.)
  290 CONTINUE
      DO 300 N = 5,7,2
      DN(N) = .5*(1.-PT(III)*PT(III))*(1.+PT(JJJ)*ETA(N))
  300 CONTINUE
      DO 310 N = 6,8,2
      DN(N) = .5*(1.+PT(III)*XI(N))*(1.-PT(JJJ)*PT(JJJ))
  310 CONTINUE
      GSTEMP = 0.
      DO 320 N = 1,8
      GSTEMP = GSTEMP + DN(N)*TGRID(N)
  320 CONTINUE
      RGTEMP = GSTEMP - TREF
  330 DO 340 I = 1,3
      SIG(I) = SIG(I) - ST(I)*RGTEMP
  340 CONTINUE
C
  350 CONTINUE
C
C     FORM KWD MATRIX
C
      KWD( 1) = SIG(2)
      KWD( 2) =-SIG(3)
      KWD( 7) =-SIG(3)
      KWD( 8) = SIG(1)
      KWD(15) = SIG(1) + SIG(2)
      KWD(16) =-SIG(3)
      KWD(17) = SIG(3)
      KWD(18) = SIG(1) - SIG(2)
      KWD(21) =-SIG(3)
      KWD(27) = SIG(3)
      KWD(33) = SIG(1) - SIG(2)
C
C     FORM CID FOR I = NPVT
C
      DO 360 I = 1,18
  360 CID( I) = 0.D0
      CID( 3) = DNY(KK)
      CID( 6) =-DNX(KK)
      CID( 7) =-.5*DNY(KK)
      CID( 8) = .5*DNX(KK)
      CID(10) = DNX(KK)
      CID(14) = DNY(KK)
      CID(16) =.5*DNY(KK)
      CID(17) =.5*DNX(KK)
C
      CALL GMMATD (CID,6,3,1,KWD,6,6,0,KMULT)
C
C     LOOP FOR THE 8 6X6 PARTITIONS CORRESPONDING TO THE PRESENT
C     PIVOT POINT
C
      DO 400 N = 1,8
C
      DO 370 I = 1,18
  370 CJD(I) = 0.D0
C
      CJD( 3) = DNY(N)
      CJD( 6) =-DNX(N)
      CJD( 7) =-.5*DNY(N)
      CJD( 8) = .5*DNX(N)
      CJD(10) = DNX(N)
      CJD(14) = DNY(N)
      CJD(16) =.5*DNY(N)
      CJD(17) =.5*DNX(N)
C
      CALL GMMATD (KMULT,3,6,0,CJD,6,3,0,TEMPAR(1))
C
C     THROW IN JACOBEAN DETERMINANT AND WEIGHT FACTORS
C
      DO 380 I = 1,9
      TEMPAR(I) = TEMPAR(I)*DHH
  380 CONTINUE
C
C     ADD THE RESULTS OF THIS INTEGRATION TO THE PREVIOUS RESULTS
C
      LL = 9*(N-1)
      DO 390 I = 1,9
      L = LL + I
      SAVE(L) = SAVE(L) + TEMPAR(I)
  390 CONTINUE
C
C     LOOP FOR MORE PARTITIONS
C
  400 CONTINUE
C
C     LOOP FOR MORE GAUSS POINTS
C
  410 CONTINUE
C
C     CHECK ON NECESSITY OF PRE-MULTIPLYING COORDINATE TRANSFORMATIONS
C
      ISUB = 4*KK + 10
      IF (NECPT(ISUB) .EQ. 0) GO TO 420
C
C     ELEMENT TO GLOBAL
C
      CALL TRANSD (NECPT(ISUB),TB)
      CALL GMMATD (E1T,3,3,0,TB,3,3,0,PREMUL)
      GO TO 440
  420 DO 430 I = 1,9
      PREMUL(I) = E1T(I)
  430 CONTINUE
  440 DO 460 N = 1,8
      LL = 9*N - 8
      CALL GMMATD (PREMUL,3,3,1,SAVE(LL),3,3,0,TEMP)
C
C     STORE THE 3 X 3 IN TSAVE
C
      DO 450 I = 1,9
      L = 9*N + I - 9
  450 TSAVE(L) = TEMP(I)
C
  460 CONTINUE
C
C     NOW CHECK ON THE NECESSITY FOR POST-MULTIPLYING TRANSFORMATIONS
C
      DO 500 N = 1,8
      ISUB = 4*N + 10
      LL = 9*N - 8
      IF (NECPT(ISUB) .EQ. 0) GO TO 470
C
C     GLOBAL TO ELEMENT
C
      CALL TRANSD (NECPT(ISUB),TB)
      CALL GMMATD (E1T,3,3,0,TB,3,3,0,PSTMUL)
      GO TO 490
  470 DO 480 I = 1,9
      PSTMUL(I) = E1T(I)
  480 CONTINUE
C
C     POST-MULTIPLY
C
  490 CALL GMMATD (TSAVE(LL),3,3,0,PSTMUL,3,3,0,TEMP)
C
C     FILL OUT THE 6 X 6 PARTITION
C
      KIJ( 1) = TEMP(1)*THICK
      KIJ( 2) = TEMP(2)*THICK
      KIJ( 3) = TEMP(3)*THICK
      KIJ( 7) = TEMP(4)*THICK
      KIJ( 8) = TEMP(5)*THICK
      KIJ( 9) = TEMP(6)*THICK
      KIJ(13) = TEMP(7)*THICK
      KIJ(14) = TEMP(8)*THICK
      KIJ(15) = TEMP(9)*THICK
C
C     INSERT INTO THE OVERALL STIFFNESS MATRIX
C
      CALL DS1B (KIJ,NECPT(N+1))
C
C     LOOP FOR MORE PARTITIONS
C
  500 CONTINUE
C
      RETURN
      END


      subroutine fvigox(fi,vc,gc,fmass,fwidth , fvi)
c
c This subroutine computes an off-shell fermion wavefunction from a
c flowing-IN external fermion and a vector boson in the case when
c they are all color octets.
c
c input:
c       complex fi(6)          : flow-in  fermion                   |fi>
c       complex vc(6)          : input    vector                      v
c       complex gc(2)          : coupling constants                  gvf
c       real    fmass          : mass  of output fermion f'
c       real    fwidth         : width of output fermion f'
c
c output:
c       complex fvi(6)         : off-shell fermion             |f',v,fi>
c
      implicit none
      double complex fi(6),vc(6),gc(2),fvi(6),sl1,sl2,sr1,sr2,d
      double precision pf(0:3),fmass,fwidth,pf2
      integer i

      double precision rZero, rOne
      parameter( rZero = 0.0d0, rOne = 1.0d0 )
      double complex cImag, cZero
      parameter( cImag = ( 0.0d0, 1.0d0 ), cZero = ( 0.0d0, 0.0d0 ) )

      fvi(5) = fi(5)-vc(5)
      fvi(6) = fi(6)-vc(6)

      pf(0) = dble( fvi(5))
      pf(1) = dble( fvi(6))
      pf(2) = dimag(fvi(6))
      pf(3) = dimag(fvi(5))
      pf2 = pf(0)**2-(pf(1)**2+pf(2)**2+pf(3)**2)

      d = -rOne/dcmplx( pf2-fmass**2, fmass*fwidth )
      sl1 =   (vc(1)+      vc(4))*fi(1)
     &      + (vc(2)-cImag*vc(3))*fi(2)
      sl2 =   (vc(2)+cImag*vc(3))*fi(1)
     &      + (vc(1)-      vc(4))*fi(2)

      if ( gc(2).ne.cZero ) then
         sr1 =   (vc(1)-      vc(4))*fi(3)
     &         - (vc(2)-cImag*vc(3))*fi(4)
         sr2 = - (vc(2)+cImag*vc(3))*fi(3)
     &         + (vc(1)+      vc(4))*fi(4)

         fvi(1) = ( gc(1)*((pf(0)-pf(3))*sl1 - dconjg(fvi(6))*sl2)
     &             +gc(2)*fmass*sr1 )*d
         fvi(2) = ( gc(1)*(      -fvi(6)*sl1 +  (pf(0)+pf(3))*sl2)
     &             +gc(2)*fmass*sr2 )*d
         fvi(3) = ( gc(2)*((pf(0)+pf(3))*sr1 + dconjg(fvi(6))*sr2)
     &             +gc(1)*fmass*sl1 )*d
         fvi(4) = ( gc(2)*(       fvi(6)*sr1 +  (pf(0)-pf(3))*sr2)
     &             +gc(1)*fmass*sl2 )*d

      else
         d = d * gc(1)
         fvi(1) = ((pf(0)-pf(3))*sl1 - dconjg(fvi(6))*sl2)*d
         fvi(2) = (      -fvi(6)*sl1 +  (pf(0)+pf(3))*sl2)*d
         fvi(3) = fmass*sl1*d
         fvi(4) = fmass*sl2*d
      end if
c
c    Need sign flip for wavefunction to account for
c    difference between Helas convention and color
      do i=1,4
        fvi(i)=-fvi(i)
      enddo
      return
      end


!*******************************************************************
!                                                                  *
      SUBROUTINE NINE0(J1,J2,J3,L1,L2,L3,K1,K2,K3,AA)
!                                                                  *
!     THIS PACKAGE DETERMINES THE VALUES OF 9j COEFFICIENT         *
!                                                                  *
!     |  J1/2  J2/2  J3/2 |                                        *
!     |  L1/2  L2/2  L3/2 |                                        *
!     |  K1/2  K2/2    0  |                                        *
!                                                                  *
!   Written by G. Gaigalas,                                        *
!   Vilnius,  Lithuania                             March    1995  *
!   Transform to fortran 90/95 by G. Gaigalas       December 2012  *
!   The last modification made by G. Gaigalas       October  2017  *
!                                                                  *
!*******************************************************************
!
!-----------------------------------------------
!   M o d u l e s
!-----------------------------------------------
      USE vast_kind_param, ONLY: DOUBLE
      USE CONS_C,          ONLY: ZERO, ONE
!-----------------------------------------------
!   I n t e r f a c e   B l o c k s
!-----------------------------------------------
      USE sixj_I
      IMPLICIT NONE
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      INTEGER  :: J1, J2, J3, L1, L2, L3, K1, K2, K3
      REAL(DOUBLE), INTENT(OUT) :: AA
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      INTEGER      :: IFA
      REAL(DOUBLE) :: A, B
!-----------------------------------------------
      IF (J1 == 0) THEN
         CALL SIXJ (L2, K2, J2, K3, L3, L1, 0, A)
         B = DBLE((J2 + 1)*(L1 + 1))
         IFA = K2 + J2 + L3 + L1
      ELSE IF (J2 == 0) THEN
         CALL SIXJ (L3, K3, J3, K1, L1, L2, 0, A)
         B = DBLE((J3 + 1)*(L2 + 1))
         IFA = K3 + J3 + L1 + L2
      ELSE IF (J3 == 0) THEN
         CALL SIXJ (L1, K1, J1, K2, L2, L3, 0, A)
         B = DBLE((J1 + 1)*(L3 + 1))
         IFA = K1 + J1 + L2 + L3
      ELSE IF (L1 == 0) THEN
         CALL SIXJ (K2, J2, L2, J3, K3, K1, 0, A)
         B = DBLE((L2 + 1)*(K1 + 1))
         IFA = J2 + L2 + K3 + K1
      ELSE IF (L2 == 0) THEN
         CALL SIXJ (K3, J3, L3, J1, K1, K2, 0, A)
         B = DBLE((L3 + 1)*(K2 + 1))
         IFA = J3 + L3 + K1 + K2
      ELSE IF (L3 == 0) THEN
         CALL SIXJ (K1, J1, L1, J2, K2, K3, 0, A)
         B = DBLE((L1 + 1)*(K3 + 1))
         IFA = J1 + L1 + K2 + K3
      ELSE IF (K1 == 0) THEN
         CALL SIXJ (J2, J3, J1, L3, L2, K2, 0, A)
         B = DBLE((J1 + 1)*(K2 + 1))
         IFA = J3 + J1 + L2 + K2
      ELSE IF (K2 == 0) THEN
         CALL SIXJ (J3, J1, J2, L1, L3, K3, 0, A)
         B = DBLE((J2 + 1)*(K3 + 1))
         IFA = J1 + J2 + L3 + K3
      ELSE IF (K3 == 0) THEN
         CALL SIXJ (J1, J2, J3, L2, L1, K1, 0, A)
         B = DBLE((J3 + 1)*(K1 + 1))
         IFA = J2 + J3 + L1 + K1
      ELSE
         A = ZERO
         B = ONE
      ENDIF
      AA = A/DSQRT(B)
      IF (MOD(IFA,4) /= 0) AA = -AA
      RETURN
      END SUBROUTINE NINE0


!------------------------------------------------------------------------------------
! File:   S3M_Module_Phys_Snow_Apps_Density.f90
! Author:   Simone Gabellani, Fabio Delogu, Francesco Avanzi.
!
! Created on June 10, 2020 5:00 PM
! Last update on October 26, 2020 5:04 PM
!
! Snow-density applications module
!------------------------------------------------------------------------------------

!------------------------------------------------------------------------------------
! Module Header
module S3M_Module_Phys_Snow_Apps_Density

    !------------------------------------------------------------------------------------
    ! External module(s)
    use S3M_Module_Namelist,            only:   oS3M_Namelist
    use S3M_Module_Vars_Loader,         only:   oS3M_Vars

    use S3M_Module_Tools_Debug

    ! Implicit none for all subroutines in this module
    implicit none
    !------------------------------------------------------------------------------------------

contains

    !------------------------------------------------------------------------------------------
    ! Subroutine to compute snow density according to De Michele et al. 2013: https://tc.copernicus.org/articles/7/433/2013/
    subroutine S3M_Phys_Snow_Apps_Rho(iID, iRows, iCols, &
                                       sTime, iTime, iDtForcing, &
                                       a2dVarDem, &
                                       a2dVarTa, a2dVarSnowFall, a2dVarSWE_D, &
                                       a2dVarH_D, a2dVarRho_D, a2dVarRhoS0)

        !------------------------------------------------------------------------------------------
        ! Variable(s) declaration
        integer(kind = 4)   :: iID, iRows, iCols
        integer(kind = 4)   :: iTime, iDtForcing
        character(len = 19) :: sTime

        real(kind = 4)      :: dVarRhoW, dVarRhoSMax, dVarRhoSMin, dVarRhoS0Max
        real(kind = 4), dimension(iRows, iCols)         :: a2dVarDem
        real(kind = 4), dimension(iRows, iCols)         :: a2dVarTa, a2dVarSnowFall, a2dVarSWE_D
        real(kind = 4), dimension(iRows, iCols)         :: a2dVarRhoS0, a2dVarRho_D, a2dVarH_D
        real(kind = 4), dimension(iRows, iCols)         :: a2dVarSnowTemp
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Initialize variable(s)
        dVarRhoSMax = -9999.0; dVarRhoSMin = -9999.0; dVarRhoS0Max = -9999.0
        dVarRhoW = -99990; a2dVarSnowTemp = -9999.0
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Get information
        dVarRhoSMax = oS3M_Namelist(iID)%dRhoSnowMax
        dVarRhoS0Max = oS3M_Namelist(iID)%dRhoSnowFresh
        dVarRhoSMin = oS3M_Namelist(iID)%dRhoSnowMin
        dVarRhoW = oS3M_Namelist(iID)%dRhoW
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Info start
        call mprintf(.true., iINFO_Extra, ' Phys :: Snow :: Rho ... ' )
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Compute fresh-snow density
        where( (a2dVarDem.ge.0.0) .and. (a2dVarSnowFall.gt.0.0) )
            a2dVarRhoS0 = 67.9 + 51.3*exp(a2dVarTa/2.6)
        endwhere

        ! Check limits for fresh-snow density
        where( (a2dVarDem.ge.0.0) .and. (a2dVarRhoS0.gt.dVarRhoS0Max) )
            a2dVarRhoS0 = dVarRhoS0Max
        endwhere

        where( (a2dVarDem.ge.0.0) .and. (a2dVarRhoS0.lt.dVarRhoSMin) )
            a2dVarRhoS0 = dVarRhoSMin
        endwhere

        ! Set to 0 where there's no snowfall
        where( (a2dVarDem.ge.0.0) .and. (a2dVarSnowFall.eq.0.0) )
            a2dVarRhoS0 = 0.0
        endwhere
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Update bulk-snow density to account for fresh-snow density
        where( (a2dVarDem.ge.0.0) .and. &
            ((a2dVarSWE_D - a2dVarSnowFall).gt.1.0) .and. (a2dVarSnowFall.gt.1.0) .and. (a2dVarRho_D.gt.dVarRhoSMin) )

               a2dVarRho_D = (a2dVarSWE_D)/((a2dVarSnowFall/a2dVarRhoS0) + ((a2dVarSWE_D - a2dVarSnowFall)/a2dVarRho_D))
               ! This equation assumes that a2dVarSWE_D already includes a2dVarSnowFall, per an update that is supposed to
               ! take place in Module_Phys_Snow right after the computation of snowfall and rainfall. Accordingly, we must
               ! subtract a2dVarSnowFall from a2dVarSWE_D here to correctly weight pre-existing a2dVarRho_D
        endwhere
        ! This first "where" has updated dry-snow density where DEM > 0, where there were at least some snow on the ground,
        ! and some substantial snowfall. In all other cases, a2dVarRho_D has not changed yet. Where there is little snowfall
        ! over a large snowpack, this is OK, we assume fresh snow density plays a minimal role. But we need to include some
        ! provisions when there is very little snow on the ground, for example at the beginning of the snow season...

        where( (a2dVarDem.ge.0.0) .and. ((a2dVarSWE_D - a2dVarSnowFall).le.1.0) .and. (a2dVarSnowFall.gt.0.0) )
            a2dVarRho_D = a2dVarRhoS0
        endwhere
        ! Now we made sure that a2dVarRho_D has been updated wherever there was sone snowfall and no pre-existing SWE.

        ! Check rhos limit(s)
        where( (a2dVarDem.ge.0.0) .and. (a2dVarRho_D.gt.dVarRhoSMax) )
            a2dVarRho_D = dVarRhoSMax
        endwhere
        where( (a2dVarDem.ge.0.0) .and. (a2dVarRho_D.lt.dVarRhoSMin) )
            a2dVarRho_D = dVarRhoSMin
        endwhere
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Compute updated dry-snow height
        where( (a2dVarDem.ge.0.0) .and. (a2dVarRho_D.gt.0.0) )
            a2dVarH_D = ((a2dVarSWE_D/1000)*dVarRhoW)/a2dVarRho_D
        elsewhere(a2dVarDem.ge.0.0)
            a2dVarH_D = 0.0
        endwhere
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Compute linearly approximated snow temperature
        where( (a2dVarDem.ge.0.0) .and. (a2dVarTa.ge.0.0) )
            a2dVarSnowTemp = 0.0
        elsewhere( (a2dVarDem.ge.0.0) )
            a2dVarSnowTemp = 0.5*a2dVarTa
        endwhere
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Compute dry-snow-density compaction now
        where( (a2dVarDem.ge.0.0) .and. (a2dVarSWE_D.gt.0.0) )
            a2dVarRho_D = a2dVarRho_D + 0.66*(iDtForcing/3600)*0.001*a2dVarH_D*(a2dVarRho_D**2)*EXP(0.08*a2dVarSnowTemp - &
                                                    0.021*a2dVarRho_D)
        endwhere

        ! Check rhos limit(s)
        where( (a2dVarDem.ge.0.0) .and. (a2dVarRho_D.gt.dVarRhoSMax) )
            a2dVarRho_D = dVarRhoSMax
        endwhere
        where( (a2dVarDem.ge.0.0) .and. (a2dVarRho_D.lt.dVarRhoSMin) )
            a2dVarRho_D = dVarRhoSMin
        endwhere
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Re-compute updated dry-snow height
        where( (a2dVarDem.ge.0.0) .and. (a2dVarRho_D.gt.0.0) )
            a2dVarH_D = ((a2dVarSWE_D/1000)*dVarRhoW)/a2dVarRho_D
        elsewhere(a2dVarDem.ge.0.0)
            a2dVarH_D = 0.0
        endwhere
        !------------------------------------------------------------------------------------------

        !------------------------------------------------------------------------------------------
        ! Info end
        call mprintf(.true., iINFO_Extra, ' Phys :: Snow :: Rho ... OK' )
        !------------------------------------------------------------------------------------------

    end subroutine S3M_Phys_Snow_Apps_Rho
    !------------------------------------------------------------------------------------------

end module S3M_Module_Phys_Snow_Apps_Density
!------------------------------------------------------------------------------------


c> \file cauchy.f

c> \brief Compute the Generalized Cauchy Point along the projected gradient direction.
c>
c> For given x, l, u, g (with sbgnrm > 0), and a limited memory
c> BFGS matrix B defined in terms of matrices WY, WS, WT, and
c> scalars head, col, and theta, this subroutine computes the
c> generalized Cauchy point (GCP), defined as the first local
c> minimizer of the quadratic
c>
c>            Q(x + s) = g's + 1/2 s'Bs
c>
c> along the projected gradient direction P(x-tg,l,u).
c> The routine returns the GCP in xcp.
c>
c> @param n On entry n is the dimension of the problem.

c>          On exit n is unchanged.
c>
c> @param x On entry x is the starting point for the GCP computation.

c>          On exit x is unchanged.
c>
c> @param l On entry l is the lower bound of x.

c>          On exit l is unchanged.
c>
c> @param u On entry u is the upper bound of x.

c>          On exit u is unchanged.
c>
c> @param nbd On entry nbd represents the type of bounds imposed on the
c>               variables, and must be specified as follows:
c>               nbd(i)=

0 if x(i) is unbounded,

c>
1 if x(i) has only a lower bound,

c>
2 if x(i) has both lower and upper bounds, and

c>
3 if x(i) has only an upper bound.

c>            On exit nbd is unchanged.
c>
c> @param g On entry g is the gradient of f(x). g must be a nonzero vector.

c>          On exit g is unchanged.
c>
c> @param iorder iorder will be used to store the breakpoints in the piecewise
c>               linear path and free variables encountered.

c>               On exit,

iorder(1),...,iorder(nleft)
c>                               are indices of breakpoints
c>                               which have not been encountered;

c>
iorder(nleft+1),...,iorder(nbreak)
c>                               are indices of
c>                               encountered breakpoints; and

c>
iorder(nfree),...,iorder(n)
c>                               are indices of variables which
c>                               have no bound constraits along the search direction.

c>
c> @param iwhere On entry iwhere indicates only the permanently fixed (iwhere=3)
c>                  or free (iwhere= -1) components of x.

c>               On exit iwhere records the status of the current x variables.
c>                  iwhere(i)=

-3  if x(i) is free and has bounds, but is not moved

c>
 0   if x(i) is free and has bounds, and is moved

c>
 1   if x(i) is fixed at l(i), and l(i) .ne. u(i)

c>
 2   if x(i) is fixed at u(i), and u(i) .ne. l(i)

c>
 3   if x(i) is always fixed, i.e.,  u(i)=x(i)=l(i)

c>
-1  if x(i) is always free, i.e., it has no bounds.

c>
c> @param t working array; will be used to store the break points.
c>
c> @param d the Cauchy direction P(x-tg)-x
c>
c> @param xcp returns the GCP on exit
c>
c> @param m On entry m is the maximum number of variable metric corrections
c>             used to define the limited memory matrix.

c>          On exit m is unchanged.
c>
c> @param ws On entry this stores S, a set of s-vectors, that defines the
c>              limited memory BFGS matrix.

c>           On exit this array is unchanged.
c>
c> @param wy On entry this stores Y, a set of y-vectors, that defines the
c>              limited memory BFGS matrix.

c>           On exit this array is unchanged.
c>
c> @param sy On entry this stores S'Y, that defines the
c>              limited memory BFGS matrix.

c>           On exit this array is unchanged.
c>
c> @param wt On entry this stores the
c>              Cholesky factorization of (theta*S'S+LD^(-1)L'), that defines the
c>              limited memory BFGS matrix.

c>           On exit this array is unchanged.
c>
c> @param theta On entry theta is the scaling factor specifying B_0 = theta I.

c>              On exit theta is unchanged.
c>
c> @param col On entry col is the actual number of variable metric
c>               corrections stored so far.

c>            On exit col is unchanged.
c>
c> @param head On entry head is the location of the first s-vector (or y-vector)
c>                in S (or Y).

c>             On exit col is unchanged.
c>
c> @param p will be used to store the vector p = W^(T)d.
c>
c> @param c will be used to store the vector c = W^(T)(xcp-x).
c>
c> @param wbp will be used to store the row of W corresponding
c>              to a breakpoint.
c>
c> @param v working array
c>
c> @param nseg On exit nseg records the number of quadratic segments explored
c>                in searching for the GCP.
c>
c> @param iprint variable that must be set by the user.

c>       It controls the frequency and type of output generated:
c>

iprint<0    no output is generated;

c>
iprint=0    print only one line at the last iteration;

c>
0<iprint<99 print also f and |proj g| every iprint iterations;

c>
iprint=99   print details of every iteration except n-vectors;

c>
iprint=100  print also the changes of active set and final x;

c>
iprint>100  print details of every iteration including x and g;

c>       When iprint > 0, the file iterate.dat will be created to
c>                        summarize the iteration.
c>
c> @param sbgnrm On entry sbgnrm is the norm of the projected gradient at x.

c>               On exit sbgnrm is unchanged.
c>
c> @param info On entry info is 0.
c>             On exit info =

0       for normal return,

c>
= nonzero for abnormal return when the the system
c>                                     used in routine bmv is singular.

c> @param epsmch machine precision epsilon
      subroutine cauchy(n, x, l, u, nbd, g, iorder, iwhere, t, d, xcp,
     +                  m, wy, ws, sy, wt, theta, col, head, p, c, wbp,
     +                  v, nseg, iprint, sbgnrm, info, epsmch)
      implicit none
      integer          n, m, head, col, nseg, iprint, info,
     +                 nbd(n), iorder(n), iwhere(n)
      double precision theta, epsmch,
     +                 x(n), l(n), u(n), g(n), t(n), d(n), xcp(n),
     +                 wy(n, col), ws(n, col), sy(m, m),
     +                 wt(m, m), p(2*m), c(2*m), wbp(2*m), v(2*m)
c
c     References:
c
c       [1] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited
c       memory algorithm for bound constrained optimization'',
c       SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208.
c
c       [2] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN
c       Subroutines for Large Scale Bound Constrained Optimization''
c       Tech. Report, NAM-11, EECS Department, Northwestern University,
c       1994.
c
c       (Postscript files of these papers are available via anonymous
c        ftp to eecs.nwu.edu in the directory pub/lbfgs/lbfgs_bcm.)
c
c                           *  *  *
c
c     NEOS, November 1994. (Latest revision June 1996.)
c     Optimization Technology Center.
c     Argonne National Laboratory and Northwestern University.
c     Written by
c                        Ciyou Zhu
c     in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal.
c
c
c     ************

      logical          xlower,xupper,bnded
      integer          i,j,col2,nfree,nbreak,pointr,
     +                 ibp,nleft,ibkmin,iter
      double precision f1,f2,dt,dtm,tsum,dibp,zibp,dibp2,bkmin,
     +                 tu,tl,wmc,wmp,wmw,ddot,tj,tj0,neggi,sbgnrm,
     +                 f2_org
      double precision one,zero
      parameter        (one=1.0d0,zero=0.0d0)

c     Check the status of the variables, reset iwhere(i) if necessary;
c       compute the Cauchy direction d and the breakpoints t; initialize
c       the derivative f1 and the vector p = W'd (for theta = 1).

      if (sbgnrm .le. zero) then
         if (iprint .ge. 0) write (6,*) 'Subgnorm = 0.  GCP = X.'
         call dcopy(n,x,1,xcp,1)
         return
      endif
      bnded = .true.
      nfree = n + 1
      nbreak = 0
      ibkmin = 0
      bkmin = zero
      col2 = 2*col
      f1 = zero
      if (iprint .ge. 99) write (6,3010)

c     We set p to zero and build it up as we determine d.

      do 20 i = 1, col2
         p(i) = zero
  20  continue

c     In the following loop we determine for each variable its bound
c        status and its breakpoint, and update p accordingly.
c        Smallest breakpoint is identified.

      do 50 i = 1, n
         neggi = -g(i)
         if (iwhere(i) .ne. 3 .and. iwhere(i) .ne. -1) then
c             if x(i) is not a constant and has bounds,
c             compute the difference between x(i) and its bounds.
            if (nbd(i) .le. 2) tl = x(i) - l(i)
            if (nbd(i) .ge. 2) tu = u(i) - x(i)

c           If a variable is close enough to a bound
c             we treat it as at bound.
            xlower = nbd(i) .le. 2 .and. tl .le. zero
            xupper = nbd(i) .ge. 2 .and. tu .le. zero

c              reset iwhere(i).
            iwhere(i) = 0
            if (xlower) then
               if (neggi .le. zero) iwhere(i) = 1
            else if (xupper) then
               if (neggi .ge. zero) iwhere(i) = 2
            else
               if (abs(neggi) .le. zero) iwhere(i) = -3
            endif
         endif
         pointr = head
         if (iwhere(i) .ne. 0 .and. iwhere(i) .ne. -1) then
            d(i) = zero
         else
            d(i) = neggi
            f1 = f1 - neggi*neggi
c             calculate p := p - W'e_i* (g_i).
            do 40 j = 1, col
               p(j) = p(j) +  wy(i,pointr)* neggi
               p(col + j) = p(col + j) + ws(i,pointr)*neggi
               pointr = mod(pointr,m) + 1
  40        continue
            if (nbd(i) .le. 2 .and. nbd(i) .ne. 0
     +                        .and. neggi .lt. zero) then
c                                 x(i) + d(i) is bounded; compute t(i).
               nbreak = nbreak + 1
               iorder(nbreak) = i
               t(nbreak) = tl/(-neggi)
               if (nbreak .eq. 1 .or. t(nbreak) .lt. bkmin) then
                  bkmin = t(nbreak)
                  ibkmin = nbreak
               endif
            else if (nbd(i) .ge. 2 .and. neggi .gt. zero) then
c                                 x(i) + d(i) is bounded; compute t(i).
               nbreak = nbreak + 1
               iorder(nbreak) = i
               t(nbreak) = tu/neggi
               if (nbreak .eq. 1 .or. t(nbreak) .lt. bkmin) then
                  bkmin = t(nbreak)
                  ibkmin = nbreak
               endif
            else
c                x(i) + d(i) is not bounded.
               nfree = nfree - 1
               iorder(nfree) = i
               if (abs(neggi) .gt. zero) bnded = .false.
            endif
         endif
  50  continue

c     The indices of the nonzero components of d are now stored
c       in iorder(1),...,iorder(nbreak) and iorder(nfree),...,iorder(n).
c       The smallest of the nbreak breakpoints is in t(ibkmin)=bkmin.

      if (theta .ne. one) then
c                   complete the initialization of p for theta not= one.
         call dscal(col,theta,p(col+1),1)
      endif

c     Initialize GCP xcp = x.

      call dcopy(n,x,1,xcp,1)

      if (nbreak .eq. 0 .and. nfree .eq. n + 1) then
c                  is a zero vector, return with the initial xcp as GCP.
         if (iprint .gt. 100) write (6,1010) (xcp(i), i = 1, n)
         return
      endif

c     Initialize c = W'(xcp - x) = 0.

      do 60 j = 1, col2
         c(j) = zero
  60  continue

c     Initialize derivative f2.

      f2 =  -theta*f1
      f2_org  =  f2
      if (col .gt. 0) then
         call bmv(m,sy,wt,col,p,v,info)
         if (info .ne. 0) return
         f2 = f2 - ddot(col2,v,1,p,1)
      endif
      dtm = -f1/f2
      tsum = zero
      nseg = 1
      if (iprint .ge. 99)
     +   write (6,*) 'There are ',nbreak,'  breakpoints '

c     If there are no breakpoints, locate the GCP and return.

      if (nbreak .eq. 0) goto 888

      nleft = nbreak
      iter = 1

      tj = zero

c------------------- the beginning of the loop -------------------------

 777  continue

c     Find the next smallest breakpoint;
c       compute dt = t(nleft) - t(nleft + 1).

      tj0 = tj
      if (iter .eq. 1) then
c         Since we already have the smallest breakpoint we need not do
c         heapsort yet. Often only one breakpoint is used and the
c         cost of heapsort is avoided.
         tj = bkmin
         ibp = iorder(ibkmin)
      else
         if (iter .eq. 2) then
c             Replace the already used smallest breakpoint with the
c             breakpoint numbered nbreak > nlast, before heapsort call.
            if (ibkmin .ne. nbreak) then
               t(ibkmin) = t(nbreak)
               iorder(ibkmin) = iorder(nbreak)
            endif
c        Update heap structure of breakpoints
c           (if iter=2, initialize heap).
         endif
         call hpsolb(nleft,t,iorder,iter-2)
         tj = t(nleft)
         ibp = iorder(nleft)
      endif

      dt = tj - tj0

      if (dt .ne. zero .and. iprint .ge. 100) then
         write (6,4011) nseg,f1,f2
         write (6,5010) dt
         write (6,6010) dtm
      endif

c     If a minimizer is within this interval, locate the GCP and return.

      if (dtm .lt. dt) goto 888

c     Otherwise fix one variable and
c       reset the corresponding component of d to zero.

      tsum = tsum + dt
      nleft = nleft - 1
      iter = iter + 1
      dibp = d(ibp)
      d(ibp) = zero
      if (dibp .gt. zero) then
         zibp = u(ibp) - x(ibp)
         xcp(ibp) = u(ibp)
         iwhere(ibp) = 2
      else
         zibp = l(ibp) - x(ibp)
         xcp(ibp) = l(ibp)
         iwhere(ibp) = 1
      endif
      if (iprint .ge. 100) write (6,*) 'Variable  ',ibp,'  is fixed.'
      if (nleft .eq. 0 .and. nbreak .eq. n) then
c                                             all n variables are fixed,
c                                                return with xcp as GCP.
         dtm = dt
         goto 999
      endif

c     Update the derivative information.

      nseg = nseg + 1
      dibp2 = dibp**2

c     Update f1 and f2.

c        temporarily set f1 and f2 for col=0.
      f1 = f1 + dt*f2 + dibp2 - theta*dibp*zibp
      f2 = f2 - theta*dibp2

      if (col .gt. 0) then
c                          update c = c + dt*p.
         call daxpy(col2,dt,p,1,c,1)

c           choose wbp,
c           the row of W corresponding to the breakpoint encountered.
         pointr = head
         do 70 j = 1,col
            wbp(j) = wy(ibp,pointr)
            wbp(col + j) = theta*ws(ibp,pointr)
            pointr = mod(pointr,m) + 1
  70     continue

c           compute (wbp)Mc, (wbp)Mp, and (wbp)M(wbp)'.
         call bmv(m,sy,wt,col,wbp,v,info)
         if (info .ne. 0) return
         wmc = ddot(col2,c,1,v,1)
         wmp = ddot(col2,p,1,v,1)
         wmw = ddot(col2,wbp,1,v,1)

c           update p = p - dibp*wbp.
         call daxpy(col2,-dibp,wbp,1,p,1)

c           complete updating f1 and f2 while col > 0.
         f1 = f1 + dibp*wmc
         f2 = f2 + 2.0d0*dibp*wmp - dibp2*wmw
      endif

      f2 = max(epsmch*f2_org,f2)
      if (nleft .gt. 0) then
         dtm = -f1/f2
         goto 777
c                 to repeat the loop for unsearched intervals.
      else if(bnded) then
         f1 = zero
         f2 = zero
         dtm = zero
      else
         dtm = -f1/f2
      endif

c------------------- the end of the loop -------------------------------

 888  continue
      if (iprint .ge. 99) then
         write (6,*)
         write (6,*) 'GCP found in this segment'
         write (6,4010) nseg,f1,f2
         write (6,6010) dtm
      endif
      if (dtm .le. zero) dtm = zero
      tsum = tsum + dtm

c     Move free variables (i.e., the ones w/o breakpoints) and
c       the variables whose breakpoints haven't been reached.

      call daxpy(n,tsum,d,1,xcp,1)

 999  continue

c     Update c = c + dtm*p = W'(x^c - x)
c       which will be used in computing r = Z'(B(x^c - x) + g).

      if (col .gt. 0) call daxpy(col2,dtm,p,1,c,1)
      if (iprint .gt. 100) write (6,1010) (xcp(i),i = 1,n)
      if (iprint .ge. 99) write (6,2010)

 1010 format ('Cauchy X =  ',/,(4x,1p,6(1x,d11.4)))
 2010 format (/,'---------------- exit CAUCHY----------------------',/)
 3010 format (/,'---------------- CAUCHY entered-------------------')
 4010 format ('Piece    ',i3,' --f1, f2 at start point ',1p,2(1x,d11.4))
 4011 format (/,'Piece    ',i3,' --f1, f2 at start point ',
     +        1p,2(1x,d11.4))
 5010 format ('Distance to the next break point =  ',1p,d11.4)
 6010 format ('Distance to the stationary point =  ',1p,d11.4)

      return

      end


c
c
c     =================================================
      subroutine cellave(xlow,ylow,dx,dy,wl,time)
c     =================================================
      implicit double precision (a-h,o-z)
      external fss
      logical fl(5),alll,allr
      dimension x(10),y(10),xx(5),yy(5)
      common/fsscorn/ xc0,yc0,xc1,yc1
c
c     # compute wl, fraction of cell that lies in left state.
c     # For initial data with two states ql and qr separated by a
c     # discontinuity. The curve along which the discontinuity lies is
c     # specified by the function fdisc, which should return a value that
c     # is negative on the side where ql lies and positive on the qr side.
c
c     # xlow,ylow is the coordinate of the lower left corner of the cell.
c     # dx, dy are grid spacing in x and y.
c
      xx(1) = xlow
      xx(2) = xlow
      xx(3) = xlow+dx
      xx(4) = xlow+dx
      xx(5) = xx(1)
      yy(1) = ylow
      yy(2) = ylow+dy
      yy(3) = ylow+dy
      yy(4) = ylow
      yy(5) = yy(1)
      alll = .true.
      allr = .true.
c
      do 20 i=1,4
         fl(i) = fdisc(xx(i),yy(i),time) .lt. 0.d0
         alll = alll .and. fl(i)
         allr = allr .and. (.not. fl(i))
   20    continue
      fl(5) = fl(1)
c
      if (alll) then
         wl = 1.d0
         return
         endif
      if (allr) then
         wl = 0.d0
         return
         endif
c
      iv = 0
      do 40 i=1,4
          if (fl(i)) then
               iv = iv+1
               x(iv) = xx(i)
               y(iv) = yy(i)
               endif
          if (fl(i).xor.fl(i+1)) then
               iv = iv+1
               xc0 = xx(i)
               yc0 = yy(i)
               xc1 = xx(i+1)
               yc1 = yy(i+1)
               ss = zeroin(0.d0, 1.d0, fss, 1d-8)
c              write(27,*) 'xc,yc,ss:',xc0,yc0,xc1,yc1,ss
               x(iv) = xx(i) + ss*(xx(i+1)-xx(i))
               y(iv) = yy(i) + ss*(yy(i+1)-yy(i))
               endif
   40     continue
c
c     # compute area:
c
      if (iv.eq.0) then
         wl = 0.d0
         return
         endif
c
      x(iv+1) = x(1)
      y(iv+1) = y(1)
      area = 0.d0
      do 50 i=1,iv
         area = area + .5d0*(y(i)+y(i+1))*(x(i+1)-x(i))
c        write(27,*) '  x,y:',x(i),y(i)
   50    continue
c
      wl = area / (dx*dy)
      wl = min(1.d0,wl)  ! roundoff
c     write(27,*) 'area,wl:',area,wl
c
      return
      end


module step1m
implicit none

interface
   subroutine rp(mx, mbc, ql, qr, s, wave, amdq, apdq, context)
     use iso_c_binding
     integer(c_int), intent(in), value :: mx, mbc
     real(c_double), intent(in) :: &
          ql(1, 1-mbc:mx+mbc), &
          qr(1, 1-mbc:mx+mbc)
     type(c_ptr), intent(in), value :: context

     real(c_double), intent(out) :: &
          s(1, 1-mbc:mx+mbc), &
          wave(1, 1, 1-mbc:mx+mbc), &
          apdq(1, 1-mbc:mx+mbc), &
          amdq(1, 1-mbc:mx+mbc)
   end subroutine rp
end interface

contains

  ! ===================================================================
  subroutine step1(num_eqn,num_waves,num_ghost,num_aux,mx,q,aux,dx, &
       dt,method,mthlim,cfl,f,wave,s,amdq,apdq,dtdx,use_fwave,c_rp1,context) &
       bind(C, name='step1_c')
    ! ===================================================================

    !     # Take one time step, updating q.

    !     method(1) = 1   ==>  Godunov method
    !     method(1) = 2   ==>  Slope limiter method
    !     mthlim(p)  controls what limiter is used in the pth family

    !     amdq, apdq, wave, s, and f are used locally:

    !     amdq(1-num_ghost:mx+num_ghost, num_eqn) = left-going flux-differences
    !     apdq(1-num_ghost:mx+num_ghost, num_eqn) = right-going flux-differences
    !        e.g. amdq(m,i) = m'th component of A^- \Delta q from i'th Riemann
    !                         problem (between cells i-1 and i).

    !     wave(1-num_ghost:mx+num_ghost, num_eqn, num_waves) = waves from solution of
    !                                           Riemann problems,
    !            wave(m,mw,i) = mth component of jump in q across
    !                           wave in family mw in Riemann problem between
    !                           states i-1 and i.

    !     s(1-num_ghost:mx+num_ghost, num_waves) = wave speeds,
    !            s(m,iw) = speed of wave in family mw in Riemann problem between
    !                      states i-1 and i.

    !     f(1-num_ghost:mx+num_ghost, num_eqn) = correction fluxes for second order method
    !            f(m,i) = mth component of flux at left edge of ith cell
    !     --------------------------------------------------------------------

  use iso_c_binding

    logical(c_bool), intent(in) :: use_fwave
    integer(c_int), intent(in) :: &
         num_eqn, num_waves, num_ghost, num_aux, mx, &
         method(7), mthlim(num_waves)

    real(c_double), intent(in) :: &
         aux(num_aux,1-num_ghost:mx+num_ghost)

    real(c_double), intent(inout) :: &
         q(num_eqn,1-num_ghost:mx+num_ghost)

    type(c_funptr), intent(in)     :: c_rp1
    type(c_ptr), intent(in)        :: context

    real(c_double), intent(out) :: &
         cfl, &
         s(num_waves,1-num_ghost:mx+num_ghost), &
         wave(num_eqn, num_waves,1-num_ghost:mx+num_ghost), &
         amdq(num_eqn,1-num_ghost:mx+num_ghost), &
         apdq(num_eqn,1-num_ghost:mx+num_ghost), &
         dtdx(1-num_ghost:mx+num_ghost), &
         f(num_eqn,1-num_ghost:mx+num_ghost), &
         dx, &
         dt

    logical :: limit
    procedure(rp), pointer :: rp1
    real(c_double) :: dtdxave
    integer(c_int) :: i, m, mw, index_capa

    call c_f_procpointer(c_rp1, rp1)

!     # check if any limiters are used:
    limit = .false.
    do mw=1,num_waves
       if (mthlim(mw) > 0) limit = .TRUE.
    end do

    index_capa = method(6)
    do i=1-num_ghost,mx+num_ghost
        if (index_capa > 0) then
            if (aux(index_capa,i) <= 0.d0) then
                write(6,*) 'Error -- capa must be positive'
                stop
            endif
            dtdx(i) = dt / (dx*aux(index_capa,i))
        else
            dtdx(i) = dt/dx
        endif
    end do

!     # solve Riemann problem at each interface
!     -----------------------------------------

    call rp1(mx,num_ghost,q,q,s,wave,amdq,apdq,context)

!     # Modify q for Godunov update:
!     # Note this may not correspond to a conservative flux-differencing
!     # for equations not in conservation form.  It is conservative if
!     # amdq + apdq = f(q(i)) - f(q(i-1)).

    forall(i=1:mx+1, m=1:num_eqn)
       q(m,i) = q(m,i) - dtdx(i)*apdq(m,i)
       q(m,i-1) = q(m,i-1) - dtdx(i-1)*amdq(m,i)
    end forall

!     # compute maximum wave speed:
    cfl = 0.d0
    do mw=1,num_waves
        do i=1,mx+1
        !          # if s>0 use dtdx(i) to compute CFL,
        !          # if s<0 use dtdx(i-1) to compute CFL:
            cfl = dmax1(cfl, dtdx(i)*s(mw,i), -dtdx(i-1)*s(mw,i))
        end do
    end do

    if (method(2) == 1) then
       return
    endif
!     # compute correction fluxes for second order q_{xx} terms:
!     ----------------------------------------------------------

    forall(i=1-num_ghost:mx+num_ghost, m=1:num_eqn)
       f(m,i) = 0.d0
    end forall

!      # apply limiter to waves:
    if (limit) call limiter(mx,num_eqn,num_waves,num_ghost,mx, &
         wave,s,mthlim)

    if (use_fwave.eqv. .FALSE. ) then
       do i=1,mx+1
          do m=1,num_eqn
             do mw=1,num_waves
                dtdxave = 0.5d0 * (dtdx(i-1) + dtdx(i))
                f(m,i) = f(m,i) + 0.5d0 * dabs(s(mw,i)) &
                     * (1.d0 - dabs(s(mw,i))*dtdxave) * wave(m,mw,i)
             end do
          end do
       end do
    else
        do i=1,mx+1
           do m=1,num_eqn
              do mw=1,num_waves
                 dtdxave = 0.5d0 * (dtdx(i-1) + dtdx(i))
                 f(m,i) = f(m,i) + 0.5d0 * dsign(1.d0,s(mw,i)) &
                      * (1.d0 - dabs(s(mw,i))*dtdxave) * wave(m,mw,i)
              end do
           end do
        end do
     endif

     ! update q by differencing correction fluxes
     ! ============================================

     ! (Note:  Godunov update has already been performed above)

    forall(i=1:mx+1, m=1:num_eqn)
       q(m,i) = q(m,i) - dtdx(i) * (f(m,i+1) - f(m,i))
    end forall

  end subroutine step1
end module step1m


Module vec4_class

    use constants, only : wp

    !not fully implmented vec4 class
    type :: vec4
        real(kind=wp) :: x, y, z, p
        contains

        generic   :: operator(.dot.) => vec_dot_vec
        generic   :: operator(/)     => vec_div_scal_r4, vec_div_scal_int
        generic   :: operator(*)     => vec_mult_vec, vec_mult_scal, scal_mult_vec
        generic   :: operator(+)     => vec_add_vec, vec_add_scal, scal_add_vec
        generic   :: operator(-)     => vec_minus_vec, vec_minus_scal, scal_minus_vec

        procedure, pass(a), private :: vec_dot_vec

        procedure, pass(a), private :: vec_div_scal_r4
        procedure, pass(a), private :: vec_div_scal_int

        procedure, pass(a), private :: vec_mult_vec
        procedure, pass(a), private :: vec_mult_scal
        procedure, pass(b), private :: scal_mult_vec

        procedure, pass(a), private :: vec_add_vec
        procedure, pass(a), private :: vec_add_scal
        procedure, pass(b), private :: scal_add_vec

        procedure, pass(a), private :: vec_minus_vec
        procedure, pass(a), private :: vec_minus_scal
        procedure, pass(b), private :: scal_minus_vec

    end type vec4

    interface sin
        module procedure sin_vec
    end interface sin

    private
    public :: vec4, sin

    contains

        type(vec4) pure elemental function sin_vec(p)

            implicit none

            type(vec4), intent(IN) :: p

            sin_vec = vec4(sin(p%x), sin(p%y), sin(p%z), sin(p%p))

        end function sin_vec

        type(vec4) function vec_minus_vec(a, b)

            implicit none

            class(vec4), intent(IN) :: a
            type(vec4),  intent(IN) :: b

            vec_minus_vec = vec4(a%x - b%x, a%y - b%y, a%z - b%z, a%p - b%p)

        end function vec_minus_vec

        type(vec4) function vec_add_scal(a, b)

            implicit none

            class(vec4),     intent(IN) :: a
            real(kind=wp),   intent(IN) :: b

            vec_add_scal = vec4(a%x + b, a%y + b, a%z + b, a%p + b)

        end function vec_add_scal

        type(vec4) function scal_add_vec(a, b)

            implicit none

            class(vec4),   intent(IN) :: b
            real(kind=wp), intent(IN) :: a

            scal_add_vec = vec4(b%x + a, b%y + a, b%z + a,  b%p + a)

        end function scal_add_vec

        type(vec4) function vec_minus_scal(a, b)

            implicit none

            class(vec4),   intent(IN) :: a
            real(kind=wp), intent(IN) :: b

            vec_minus_scal = vec4(a%x - b, a%y - b, a%z - b, a%p - b)

        end function vec_minus_scal

        type(vec4) function scal_minus_vec(a, b)

            implicit none

            class(vec4),   intent(IN) :: b
            real(kind=wp), intent(IN) :: a

            scal_minus_vec = vec4(b%x - a, b%y - a, b%z - a, b%p - a)

        end function scal_minus_vec

        type(vec4) function vec_add_vec(a, b)

            implicit none

            class(vec4), intent(IN) :: a
            type(vec4),  intent(IN) :: b

            vec_add_vec = vec4(a%x + b%x, a%y + b%y, a%z + b%z, a%p + b%p)

        end function vec_add_vec

        elemental function vec_dot_vec(a, b) result (dot)

            implicit none

            class(vec4), intent(IN) :: a
            type(vec4),  intent(IN) :: b
            real(kind=wp) :: dot

            dot = (a%x * b%x) + (a%y * b%y) + (a%z * b%z) + (a%p * b%p)

        end function vec_dot_vec

        type(vec4) function vec_mult_vec(a, b)

            implicit none

            class(vec4), intent(IN) :: a
            type(vec4),  intent(IN) :: b

            vec_mult_vec = vec4(a%x * b%x, a%y * b%y, a%z * b%z, a%p * b%p)

        end function vec_mult_vec

        type(vec4) function vec_mult_scal(a, b)

            implicit none

            class(vec4),   intent(IN) :: a
            real(kind=wp), intent(IN) :: b

            vec_mult_scal = vec4(a%x * b, a%y * b, a%z * b, a%p * b)

        end function vec_mult_scal

        type(vec4) function scal_mult_vec(a, b)

            implicit none

            class(vec4),   intent(IN) :: b
            real(kind=wp), intent(IN) :: a

            scal_mult_vec = vec4(a * b%x, a * b%y, a * b%z, a * b%p)

        end function scal_mult_vec

        type(vec4) function vec_div_scal_r4(a, b)

            implicit none

            class(vec4),   intent(IN) :: a
            real(kind=wp), intent(IN) :: b

            vec_div_scal_r4 = vec4(a%x / b, a%y / b, a%z / b, a%p / b)

        end function vec_div_scal_r4

        type(vec4) function vec_div_scal_int(a, b)

            implicit none

            class(vec4), intent(IN) :: a
            integer,       intent(IN) :: b

            vec_div_scal_int = vec4(a%x / real(b, kind=wp), a%y / real(b, kind=wp), a%z / real(b, kind=wp), a%p / real(b, kind=wp))

        end function vec_div_scal_int

        type(vec4) function magnitude_fn(this)

            implicit none

            class(vec4) :: this

            real(kind=wp) :: tmp

            tmp = sqrt(this%x**2 + this%y**2 + this%z**2 + this%p**2)
            magnitude_fn = this / tmp

        end function magnitude_fn

        real function length(this)

            implicit none

            class(vec4) :: this

            length = sqrt(this%x**2 + this%y**2 + this%z**2 + this%p**2)

        end function length
end Module vec4_class


      SUBROUTINE fouri(grpmn, gsource, amatrix, amatsq, bvec, wint,
     1                 ndim, ns)
      USE vacmod
      IMPLICIT NONE
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      INTEGER, INTENT(in) :: ns, ndim
      REAL(rprec), DIMENSION(mnpd,nv,nu3,ndim), INTENT(in) :: grpmn
      REAL(rprec), DIMENSION(nuv), INTENT(in) :: gsource
      REAL(rprec), DIMENSION(mnpd,mnpd,ndim**2), INTENT(out) :: amatrix
      REAL(rprec), DIMENSION(mnpd2,mnpd2), INTENT(out) :: amatsq
      REAL(rprec), DIMENSION(0:mf,-nf:nf,ndim), INTENT(inout) :: bvec
      REAL(rprec), DIMENSION(*) :: wint
C-----------------------------------------------
C   L o c a l   P a r a m e t e r s
C-----------------------------------------------
C     interior  (int_ext=-1), exterior  (int_ext=+1)  neumann problem
      REAL(rprec), PARAMETER :: int_ext = 1
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      INTEGER :: k, i, j, n, kvi, kui, mn, m, mn0, isym
      REAL(rprec), ALLOCATABLE, DIMENSION(:,:,:) :: bcos, bsin, source
      REAL(rprec), ALLOCATABLE :: actemp(:,:,:,:), astemp(:,:,:,:)
      REAL(rprec) :: cosn, sinn, cosm, sinm
C-----------------------------------------------
!
!     AMATRIX(,1) = A(Sin)(Sin');  AMATRIX(,2) = A(Sin)(Cos');
!     AMATRIX(,3) = A(Cos)(Sin');  AMATRIX(,4) = A(Cos)(Cos')
!
!     ARG OF TRIG FUNCTIONS: m'u' - n'v' CORRESPONDING TO THE PRIMED MESH IN
!     PKM EQ.(2.14), AND mu  - nv (UNPRIMED MESH) IN EQ. (2.16)
!
!     ON ENTRY, GRPMN(MN,...) HAS BEEN FOURIER-TRANSFORMED OVER THE UNPRIMED
!     COORDINATES (IN FOURP), SO THE FIRST INDEX OF GRPMN CORRESPONDS TO THE FIRST
!     INDEX OF AMATRIX. THUS, THE FOURIER TRANSFORMS OF GRPMN HERE ARE OVER THE PRIMED MESH.
!
!     IN CONTRAST, THE INTEGRAL OF THE SOURCE TERM OVER THE PRIMED MESH WAS ALREADY
!     DONE (IN SCALPOT), SO HERE THE FT ARE OVER THE UNPRIMED MESH FOR THE SOURCE.
!
      ALLOCATE (bcos(nu2,-nf:nf,ndim), bsin(nu2,-nf:nf,ndim),
     1   actemp(mnpd,-nf:nf,nu3,ndim), astemp(mnpd,-nf:nf,nu3,ndim),
     2   source(nv,nu2,ndim), stat = i)
      IF (i .ne. 0) STOP 'allocation error in fouri'

!
!     SYMMETRIZE SOURCE TERMS (with respect to u,v and -u,-v)
!     INDEX (1) IS ANTI-SYMMETRIC, INDEX (2) IS SYMMETRIC, PART
!
!     GSOURCE = - (2pi)**2 B * dS (h - hsing) * WINT
!
!     WINT: needed to normalize integral over angles to unity
!
      k = 0
      DO i = 1, nu2
         DO j = 1, nv
            k = k + 1
            source(j,i,1) = gsource(k) - gsource(imirr(k))      !anti-symmetric part
            IF (lasym)
     1      source(j,i,2) = gsource(k) + gsource(imirr(k))      !symmetric part
         END DO
      END DO

      source = p5*onp*source

!
!     INITIALIZE RUNNING-SUM ARRAYS
!
      bcos = 0;      bsin = 0
      actemp = 0;    astemp = 0
      amatrix = 0

!
!     PERFORM KV (TOROIDAL ANGLE) TRANSFORM
!
      DO n = 0, nf
         DO kvi = 1, nv
            cosn = cosv(n,kvi)
            sinn = sinv(n,kvi)
            DO isym = 1, ndim
               bcos(:,n,isym) = bcos(:,n,isym) + cosn*source(kvi,:,isym)
               bsin(:,n,isym) = bsin(:,n,isym) + sinn*source(kvi,:,isym)
               actemp(:,n,:,isym) = actemp(:,n,:,isym)
     1                            + cosn*grpmn(:,kvi,:,isym)
               astemp(:,n,:,isym) = astemp(:,n,:,isym)
     1                            + sinn*grpmn(:,kvi,:,isym)

               IF (n .ne. 0) THEN
                  bcos(:,(-n),isym) =  bcos(:,n,isym)
                  bsin(:,(-n),isym) = -bsin(:,n,isym)
                  actemp(:,(-n),:,isym) =  actemp(:,n,:,isym)
                  astemp(:,(-n),:,isym) = -astemp(:,n,:,isym)
               ENDIF

            END DO
         END DO
      END DO

!
!     PERFORM KU (POLOIDAL ANGLE) TRANSFORM
!
      DO m = 0, mf
         DO kui = 1, nu2
            cosm = cosui(m,kui)
            sinm = sinui(m,kui)
            bvec(m,-nf:nf,1) = bvec(m,-nf:nf,1) +
     1      bcos(kui,-nf:nf,1)*sinm - bsin(kui,-nf:nf,1)*cosm
            IF (lasym) THEN
               bvec(m,-nf:nf,2) = bvec(m,-nf:nf,2) +
     1         bcos(kui,-nf:nf,2)*cosm + bsin(kui,-nf:nf,2)*sinm
            END IF
         END DO

!
!     RECALL, LAST INDEX OF AS,CTEMP
!                        = 1 CORRESPONDS TO SIN (UNPRIMED) TRANSFORM (FIRST INDEX OF AMATRIX)
!                        = 2 CORRESPONDS TO COS (UNPRIMED) TRANSFORM
         DO kui = 1, nu3
            cosm = cosu(m,kui)*wint(kui*ns*nv)
            sinm = sinu(m,kui)*wint(kui*ns*nv)
            amatrix(:,m+1:mnpd:mf1,1) = amatrix(:,m+1:mnpd:mf1,1) +
     1         sinm*actemp(:,-nf:nf,kui,1) - cosm*astemp(:,-nf:nf,kui,1)       !SIN SIN'
            IF (.not.lasym) CYCLE
            amatrix(:,m+1:mnpd:mf1,2) = amatrix(:,m+1:mnpd:mf1,2) +
     1         cosm*actemp(:,-nf:nf,kui,1) + sinm*astemp(:,-nf:nf,kui,1)       !SIN COS'
            amatrix(:,m+1:mnpd:mf1,3) = amatrix(:,m+1:mnpd:mf1,3) +
     1         sinm*actemp(:,-nf:nf,kui,2) - cosm*astemp(:,-nf:nf,kui,2)       !COS SIN'
            amatrix(:,m+1:mnpd:mf1,4) = amatrix(:,m+1:mnpd:mf1,4) +
     1         cosm*actemp(:,-nf:nf,kui,2) + sinm*astemp(:,-nf:nf,kui,2)       !COS COS'
         END DO
      END DO

      DEALLOCATE (bcos, bsin, actemp, astemp, source, stat=i)

      amatrix = (pi2*pi2)*amatrix
!
!     ZERO BVEC(0,n) FOR n < 0
!
      bvec(0,:0,1:ndim) = 0
!     bvec(0,:-1,1:ndim) = 0

!
!     ZERO AMATRIX(0,n,m',n') M = 0 MODES FOR n < 0
!
      mn0 = 1 + mf1*nf                                             !Index of m=0,n=0
      amatrix(1:mn0-1:mf1,:,1:ndim*ndim) = 0
!     amatrix(:(nf-1)*mf1+1:mf1,:,1:ndim*ndim) = 0

!
!     ADD DIAGONAL TERMS TO AMATRIX [THE FIRST TERM IN EQ(3.2) OF PKM]
!
      DO mn = 1, mnpd
         amatrix(mn,mn,1) = amatrix(mn,mn,1) + pi3*int_ext
      END DO

      IF (lasym) THEN
         DO mn = 1, mnpd
            amatrix(mn,mn,4) = amatrix(mn,mn,4) + pi3*int_ext
         END DO
         amatrix(mn0,mn0,4) = amatrix(mn0,mn0,4) + pi3*int_ext     !!COS(0-0) mode *2
      END IF

!
!     PUT ELEMENTS INTO SQUARE MATRIX
!
      amatsq(:mnpd,:mnpd) = amatrix(:,:,1)                      !Sin-Sin'

      IF (.not.lasym) RETURN

      amatsq(:mnpd,1+mnpd:mnpd2) = amatrix(:,:,2)               !Sin-Cos'
      amatsq(1+mnpd:mnpd2,:mnpd) = amatrix(:,:,3)               !Cos-Sin'
      amatsq(1+mnpd:mnpd2,1+mnpd:mnpd2) = amatrix(:,:,4)        !Cos-Cos'

      END SUBROUTINE fouri


#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! d_rot2_swapdiag
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine passes the generator for a Givens rotation represented
! by 2 real numbers: a strictly real cosine and a scrictly real sine
! through a 2x2 diagonal matrix with entries +/-1.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! INPUT VARIABLES:
!
!  D               REAL(8) array of dimension (2)
!                    array of generators for complex diagonal matrix
!                    the entries of D must be +/-1
!
!  B               REAL(8) array of dimension (2)
!                    generator for a Givens rotation
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine d_rot2_swapdiag(D,B)

  implicit none

  ! input variables
  real(8), intent(in) :: D(2)
  real(8), intent(inout) :: B(2)

  ! change sign in D not scalar
  if (D(1).NE.D(2)) then
    B(2) = -B(2)
  end if

end subroutine d_rot2_swapdiag


!-----------------------------------------------------------------------------!
!          Alternative Symmetric sparse matrix manipulation routines          !
!   Authors: Dorival Piedade Neto and Rodrigo Ribeiro Paccola                 !
!   University of Sao Paulo - Sao Carlos Engineering School - October 2014    !
!-----------------------------------------------------------------------------!

module Sparse

    implicit none

    type list
        integer(4),dimension(:),allocatable::terms
    end type

    type lists
        type(list),dimension(:),allocatable::terms
    end type

    type sparse_matrix
        integer(4)::nnodes,nelems,nterms,ngl,iflag
        real(8)::ti,tf,tacum
        integer(4),dimension(:),allocatable::len_nodes,len_elems,acum,row,col,vec_,cum
        real(8),dimension(:),allocatable::val
        type(list),dimension(:),allocatable::conec,n_n_incid,n_e_incid
        type(lists),dimension(:),allocatable::conec_new
    end type sparse_matrix

    contains

    subroutine prepare_01(sp_matrix, nnodes, nelems, ngl, iflag)
        implicit none

        integer(4)::i
        integer(4),intent(in)::nnodes,nelems,ngl,iflag
        type(sparse_matrix),intent(inout)::sp_matrix

        call cpu_time(sp_matrix%ti)

        sp_matrix%nnodes = nnodes
        sp_matrix%nelems = nelems
        sp_matrix%ngl = ngl
        sp_matrix%iflag = iflag

        allocate(sp_matrix%conec(nelems))
        allocate(sp_matrix%len_elems(nelems))
        allocate(sp_matrix%n_n_incid(nnodes))
        allocate(sp_matrix%n_e_incid(nnodes))
        allocate(sp_matrix%len_nodes(nnodes))
        allocate(sp_matrix%acum(nnodes))

        allocate(sp_matrix%cum(ngl*nnodes+1))

        allocate(sp_matrix%vec_(ngl+1))
        sp_matrix%vec_ = 0
        do i=2,ngl
            sp_matrix%vec_(i+1) = sp_matrix%vec_(i) + i - 1
        enddo

        call cpu_time(sp_matrix%tf)
        if (sp_matrix%iflag.eq.0) then
            write(*,*)'prepare_01 time: ',sp_matrix%tf - sp_matrix%ti
        end if
    end subroutine prepare_01

    subroutine prepare_02(sp_matrix)
        implicit none

        integer(4)::i,j,k,acum,n,ix,ij,jj,counter,accumulated
        integer(4),dimension(:),allocatable::node_aux
        type(sparse_matrix),intent(inout)::sp_matrix

        call cpu_time(sp_matrix%ti)

        do i=1,sp_matrix%nelems
            do j=1,sp_matrix%len_elems(i)
                call add_item(sp_matrix%n_e_incid(sp_matrix%conec(i)%terms(j))%terms,i)
            enddo
        enddo

        allocate(node_aux(sp_matrix%nnodes))
        acum = 0
        do i=1,sp_matrix%nnodes
            node_aux = 0
            do j=1,size(sp_matrix%n_e_incid(i)%terms)
                do k=1,sp_matrix%len_elems(sp_matrix%n_e_incid(i)%terms(j))
                    if (i.le.sp_matrix%conec(sp_matrix%n_e_incid(i)%terms(j))%terms(k)) then
                        node_aux(sp_matrix%conec(sp_matrix%n_e_incid(i)%terms(j))%terms(k)) = 1
                    endif
                enddo
            enddo
            n = size(pack([(ix,ix=1,sp_matrix%nnodes)],node_aux == 1))
            sp_matrix%len_nodes(i) = n
            acum = acum + n
            sp_matrix%acum(i) = acum
            allocate(sp_matrix%n_n_incid(i)%terms(n))
            sp_matrix%n_n_incid(i)%terms = pack([(ix,ix=1,sp_matrix%nnodes)],node_aux == 1)
        enddo
        deallocate(node_aux)

        sp_matrix%nterms = 0
        do i=1,sp_matrix%nnodes
            do ij=1,sp_matrix%ngl
                do j=1,size(sp_matrix%n_n_incid(i)%terms)
                    if (sp_matrix%n_n_incid(i)%terms(j).eq.i) then
                        do jj=ij,sp_matrix%ngl
                            sp_matrix%nterms = sp_matrix%nterms + 1
                        enddo
                    else
                        do jj=1,sp_matrix%ngl
                            sp_matrix%nterms = sp_matrix%nterms + 1
                        enddo
                    endif
                enddo
            enddo
        enddo

        acum = 0
        allocate(sp_matrix%row(sp_matrix%nterms))
        allocate(sp_matrix%col(sp_matrix%nterms))
        allocate(sp_matrix%val(sp_matrix%nterms))
        sp_matrix%val = 0.0d0
        do i=1,sp_matrix%nnodes
            do ij=1,sp_matrix%ngl
                do j=1,size(sp_matrix%n_n_incid(i)%terms)
                    if (sp_matrix%n_n_incid(i)%terms(j).eq.i) then
                        do jj=ij,sp_matrix%ngl
                             acum = acum + 1
                             sp_matrix%row(acum) = (i-1)*sp_matrix%ngl+ij
                             sp_matrix%col(acum) = (sp_matrix%n_n_incid(i)%terms(j)-1)*sp_matrix%ngl+jj
                        enddo
                    else
                        do jj=1,sp_matrix%ngl
                             acum = acum + 1
                             sp_matrix%row(acum) = (i-1)*sp_matrix%ngl+ij
                             sp_matrix%col(acum) = (sp_matrix%n_n_incid(i)%terms(j)-1)*sp_matrix%ngl+jj
                        enddo
                    endif
                enddo
            enddo
        enddo

        allocate(node_aux(1))
        allocate(sp_matrix%conec_new(sp_matrix%nelems))
        do i=1,sp_matrix%nelems
            allocate(sp_matrix%conec_new(i)%terms(sp_matrix%len_elems(i)-1))
            do j=1,sp_matrix%len_elems(i)-1
                allocate(sp_matrix%conec_new(i)%terms(j)%terms(sp_matrix%len_elems(i)-j))
                do k=j+1,sp_matrix%len_elems(i)
                    node_aux = pack([(ix,ix=1,sp_matrix%len_nodes(min(sp_matrix%conec(i)%terms(j),sp_matrix%conec(i)%terms(k))))],sp_matrix%n_n_incid(min(sp_matrix%conec(i)%terms(j),sp_matrix%conec(i)%terms(k)))%terms == max(sp_matrix%conec(i)%terms(j),sp_matrix%conec(i)%terms(k)))
                    sp_matrix%conec_new(i)%terms(j)%terms(k-j) = node_aux(1)
                enddo
            enddo
        enddo
        deallocate(node_aux)

        sp_matrix%cum(1) = 1
        counter = 1
        accumulated = 1
        do i=2,sp_matrix%nterms
            accumulated = accumulated + 1
            if (sp_matrix%row(i) > sp_matrix%row(i-1)) then
                counter = counter + 1
                sp_matrix%cum(counter) = accumulated
            end if
        end do
        sp_matrix%cum(sp_matrix%nnodes*sp_matrix%ngl+1) = accumulated + 1

        call cpu_time(sp_matrix%tf)
        if (sp_matrix%iflag.eq.0) then
            write(*,*)'prepare_02 time: ',sp_matrix%tf - sp_matrix%ti
        end if
    end subroutine prepare_02

    subroutine add_matrix(sp_matrix,id,matrix)
        implicit none

        integer(4)::ino,jno,igl,jgl,ia,ib,imin,imax,ngl,acum,pos
        integer(4),intent(in)::id
        real(8),dimension(:,:),allocatable,intent(in)::matrix
        type(sparse_matrix),intent(inout)::sp_matrix

        call cpu_time(sp_matrix%ti)

        ngl = sp_matrix%ngl
        do ino=1,sp_matrix%len_elems(id)
            ia = ngl * (ino - 1)
            do jno=1,sp_matrix%len_elems(id)
                ib = ngl * (jno - 1)
                imin = sp_matrix%conec(id)%terms(ino)
                imax = sp_matrix%conec(id)%terms(jno)
                if (imax.ge.imin) then
                    if (imin.eq.1) then
                        acum = 0
                    else
                        acum = sp_matrix%acum(imin-1) * (ngl ** 2) - (imin - 1) * sp_matrix%vec_(ngl+1)
                    endif
                    do igl=1,ngl
                        if (imax.eq.imin) then
                            do jgl=igl,ngl
                                pos = acum + sp_matrix%len_nodes(imin) * ngl * (igl - 1) - sp_matrix%vec_(igl) + jgl - igl + 1
                                sp_matrix%val(pos) = sp_matrix%val(pos) + matrix(ia + igl,ib + jgl)
                            enddo
                        else
                            do jgl=1,ngl
                                pos = acum + sp_matrix%len_nodes(imin) * ngl * (igl - 1) - sp_matrix%vec_(igl) + (sp_matrix%conec_new(id)%terms(min(ino,jno))%terms(abs(ino - jno)) - 1) * ngl + jgl - igl + 1
                                sp_matrix%val(pos) = sp_matrix%val(pos) + matrix(ia + igl,ib + jgl)
                            enddo
                        endif
                         !if (imax.eq.imin) then
                         !    do jgl=igl,ngl
                         !        pos = acum + sp_matrix%len_nodes(imin) * ngl * (igl - 1) - sp_matrix%vec_(igl) + jgl - igl + 1
                         !        if (ia + igl .le. ib + jgl) then
                         !            sp_matrix%val(pos) = sp_matrix%val(pos) + matrix(ia + igl,ib + jgl)
                         !        else if (ia + igl .gt. ib + jgl) then
                         !            sp_matrix%val(pos) = sp_matrix%val(pos) + matrix(ib + jgl,ia + igl)
                         !        end if
                         !    enddo
                         !else
                         !    do jgl=1,ngl
                         !        pos = acum + sp_matrix%len_nodes(imin) * ngl * (igl - 1) - sp_matrix%vec_(igl) + &
                         !              (sp_matrix%conec_new(id)%terms(min(ino,jno))%terms(abs(ino - jno)) - 1) * ngl + jgl - igl + 1
                         !        if (ia + igl .le. ib + jgl) then
                         !            sp_matrix%val(pos) = sp_matrix%val(pos) + matrix(ia + igl,ib + jgl)
                         !        else if (ia + igl .gt. ib + jgl) then
                         !            sp_matrix%val(pos) = sp_matrix%val(pos) + matrix(ib + jgl,ia + igl)
                         !        end if
                         !    enddo
                         !endif
                    enddo
                endif
            enddo
        enddo

        call cpu_time(sp_matrix%tf)
        sp_matrix%tacum = sp_matrix%tacum + sp_matrix%tf - sp_matrix%ti
    end subroutine add_matrix

    subroutine add_elem_conec(sp_matrix,id,len,conec)
        implicit none

        integer(4),intent(in)::id,len
        integer(4),dimension(:),allocatable,intent(inout)::conec
        type(sparse_matrix),intent(inout)::sp_matrix

        call move_alloc(conec,sp_matrix%conec(id)%terms)
        sp_matrix%len_elems(id) = len
        !allocate(sp_matrix%conec(id)%terms(len))
        !sp_matrix%conec(id)%terms = conec
    end subroutine add_elem_conec

    subroutine clear_data(sp_matrix)
        implicit none

        type(sparse_matrix),intent(inout)::sp_matrix

        sp_matrix%val = 0.0d0
        sp_matrix%tacum = 0.0d0
    end subroutine clear_data

    subroutine solve_system_of_equation(sp_matrix, vector)
        ! Solves the system of equation A x = b, given by the sp_matrix
        ! (A) and the vector (b), resulting in the solution (x), using the
        ! direct solver Pardiso from Intel MKL.
        implicit none
        type(sparse_matrix),intent(in)::sp_matrix
        real(8),dimension(sp_matrix%nnodes*sp_matrix%ngl),intent(inout)::vector
        integer(4) :: mtype,maxfct,mnum,phase,n,msglvl,error,nrhs
        integer(8),dimension(:),allocatable :: pt
        integer(4),dimension(:),allocatable :: iparm,perm
        real(8),dimension(:),allocatable :: x
        mtype = -2  !matrix type (-2 <= symmetric indefinite; 2 <= symmetric positive definite)
        maxfct = 1
        mnum = 1
        nrhs = 1
        n = sp_matrix%nnodes*sp_matrix%ngl     !number of equation
        msglvl = 0      !message level information
        allocate(x(n),pt(64),iparm(64),perm(n))
        perm = 0
        call pardisoinit(pt,mtype,iparm)
        phase = 13  !controls the execution of the solver (see MKL reference manual)
        iparm(6) = 1    !The solver stores the solution on 'b' (A.x = b)
        call pardiso(pt,maxfct,mnum,mtype,phase,n,sp_matrix%val,sp_matrix%cum,&
                     sp_matrix%col,perm,nrhs,iparm,msglvl,vector,x,error)
        phase = -1
        call pardiso(pt,maxfct,mnum,mtype,phase,n,sp_matrix%val,sp_matrix%cum,&
                     sp_matrix%col,perm,nrhs,iparm,msglvl,vector,x,error)
    end subroutine solve_system_of_equation

    subroutine add_item(list, term)
        implicit none

        integer(4),dimension(:),allocatable,intent(inout)::list
        integer(4),dimension(:),allocatable::tmp
        integer(4),intent(in) :: term

        if (.not.allocated(list)) then
            allocate(list(1))
            list(1) = term
        else
            call move_alloc(list,tmp)
            allocate(list(size(tmp)+1))
            list(1:size(tmp)) = tmp
            list(size(list)) = term
        end if
    end subroutine add_item

    subroutine set_value_to_row(sp_matrix,row,value)
        ! Sets the given value to all sparse matrix terms in the given row.
        implicit none
        type(sparse_matrix), intent(inout)::sp_matrix
        integer(4),intent(in)::row
        real(8),intent(in)::value
        integer(4)::ind, inf, sup
        inf = sp_matrix%cum(row)
        sup = sp_matrix%cum(row+1)-1
        do ind = inf,sup
            sp_matrix%val(ind) = value
        enddo
    end subroutine set_value_to_row

    subroutine set_value_to_col(sp_matrix,col,value)
        ! Sets the given value to all sparse matrix terms in the given column.
        implicit none
        type(sparse_matrix),intent(inout)::sp_matrix
        integer(4),intent(in)::col
        real(8),intent(in)::value
        integer(4)::ind, inf, sup, row_ind
        do row_ind = 1, col
            inf = sp_matrix%cum(row_ind)
            sup = sp_matrix%cum(row_ind+1)-1
            if ((sp_matrix%col(inf).le.col).and.(sp_matrix%col(sup).ge.col)) then
                do ind=inf,sup
                    if (sp_matrix%col(ind).eq.col) then
                        sp_matrix%val(ind) = value
                        exit
                    elseif (sp_matrix%col(ind).gt.col) then
                        exit
                    endif
                end do
            endif
        enddo
    end subroutine set_value_to_col

    subroutine set_value_in_term(sp_matrix,row,col,value)
        ! Sets the given value in the sparse matrix term, if it exists
        implicit none
        type(sparse_matrix),intent(inout)::sp_matrix
        integer(4),intent(in)::row, col
        real(8),intent(in)::value
        integer(4)::ind
        do ind=sp_matrix%cum(row),sp_matrix%cum(row+1)-1
            if (sp_matrix%col(ind)==col) then
                sp_matrix%val(ind) = value
                return
            end if
        end do
    end subroutine set_value_in_term

end module SPARSE