program main

    use h5fortran,     only: hdf5_file, HSIZE_T
    use pyplot_module, only:  pyplot
    use fqs,           only: wp
    use fqs,           only: pi
    use fqs,           only: quat_t
    use fqs,           only: vect_t
    use fqs,           only: euler_t
    use fqs,           only: axis_angle_t
    use fqs,           only: linspace_fcn
    use fqs,           only: rotate

    implicit none

    integer,  parameter           :: num_pts = 500
    real(wp), parameter           :: period = 5.0_wp
    type(axis_angle_t)            :: axis_angle

    real(wp),      allocatable    :: t(:)
    real(wp),      allocatable    :: tmp(:)
    type(euler_t), allocatable    :: euler(:)
    type(euler_t), allocatable    :: euler_rot(:)
    type(pyplot)                  :: plt

    allocate(t(num_pts))
    allocate(tmp(num_pts))
    allocate(euler(num_pts))

    t = linspace_fcn(0.0_wp, 10.0_wp, num_pts)

    axis_angle % angle = -45.0_wp*pi/180_wp
    axis_angle % axis = vect_t(1.0_wp, 0.0_wp, 1.0_wp)

    euler % heading = 0.5_wp*pi*cos(2.0*pi*t/period)
    euler_rot = rotate(euler, axis_angle)

    call plt % initialize(grid=.true.,xlabel='t (sec)',ylabel='heading (deg)')

    tmp = euler % heading
    call plt % add_plot(t, tmp, label='chord le',linestyle=':b',markersize=5,linewidth=2)

    tmp = euler_rot % heading
    call plt % add_plot(t, tmp, label='chord le',linestyle='b',markersize=5,linewidth=2)

    tmp = euler % attitude
    call plt % add_plot(t, tmp, label='chord le',linestyle=':r',markersize=5,linewidth=2)

    tmp = euler_rot % attitude
    call plt % add_plot(t, tmp, label='chord le',linestyle='r',markersize=5,linewidth=2)

    call plt % showfig()

end program main


!-----------------------------------------------------------------------
!     Make center of mass motion off
!-----------------------------------------------------------------------

SUBROUTINE vcom( Velocity )

  use atom_module, only: Natom,ki_atom,zn_atom
  use cpmd_variables, only: pmass,AMU
  use calc_kine_temp_module, only: calc_kine

  implicit none
  real(8),intent(INOUT) :: Velocity(3,Natom)
  real(8) rescale,pm,kine0,kine,MG,VG(3)
  integer i

  call calc_kine( Velocity, kine0 )

  MG=0.0d0
  VG=0.0d0

  do i=1,Natom
     pm = pmass( zn_atom(ki_atom(i)) )*AMU
     MG = MG + pm
     VG(1) = VG(1) + Velocity(1,i)*pm
     VG(2) = VG(2) + Velocity(2,i)*pm
     VG(3) = VG(3) + Velocity(3,i)*pm
  end do

  VG(1:3)=VG(1:3)/MG
  do i=1,Natom
     Velocity(1,i) = Velocity(1,i) - VG(1)
     Velocity(2,i) = Velocity(2,i) - VG(2)
     Velocity(3,i) = Velocity(3,i) - VG(3)
  end do

  call calc_kine( Velocity, kine )

  rescale = sqrt( kine0/kine )

  Velocity(:,:) = Velocity(:,:)*rescale

END SUBROUTINE vcom


!==============================================================================!
  subroutine Turb_Mod_Src_F22_K_Eps_Zeta_F(turb, sol)
!------------------------------------------------------------------------------!
!   Calculate source terms in eliptic relaxation  equation                     !
!   for vi2 and imposing  boundary condition for f22                           !
!------------------------------------------------------------------------------!
  implicit none
!--------------------------------[Arguments]-----------------------------------!
  type(Turb_Type),   target :: turb
  type(Solver_Type), target :: sol
!----------------------------------[Locals]------------------------------------!
  type(Field_Type),  pointer :: flow
  type(Grid_Type),   pointer :: grid
  type(Var_Type),    pointer :: kin, eps, f22, zeta
  type(Matrix_Type), pointer :: a
  real,              pointer :: b(:)
  integer                    :: s, c, c1, c2
  real                       :: sor_11, f22hg
!==============================================================================!
!                                                                              !
!  The form of source terms are :                                              !
!                                                                              !
!      int( f22hg*dV ),                                                        !
!  where f22hg - f22hg homogenious is placed in a source coefficients b(c)     !
!                                                                              !
!      f22hg = (1.0 - Cv_1)*(vi2(c)/kin(c) - 2.0/3.0)/t_scale(c)     &         !
!              + 2.0*Cv2*p_kin(c)/(3.0*kin(c))                                 !
!                                                                              !
!    int( f22*dV ); this term is placed in a diagonal of coefficient matrix    !
!                                                                              !
!                                                                              !
!  Dimensions of certain variables                                             !
!                                                                              !
!     t_scale        [s]                                                       !
!     kin            [m^2/s^2]                                                 !
!     eps            [m^3/s^2]                                                 !
!     vi2            [m^2/s^2]                                                 !
!     f22            [-]                                                       !
!     l_scale        [m]                                                       !
!------------------------------------------------------------------------------!

  ! Take aliases
  flow => turb % pnt_flow
  grid => flow % pnt_grid
  call Turb_Mod_Alias_K_Eps_Zeta_F(turb, kin, eps, zeta, f22)
  call Solver_Mod_Alias_System    (sol, a, b)

  call Time_And_Length_Scale(grid, turb)

 ! Source term f22hg
 do c = 1, grid % n_cells
   f22hg = (1.0 - c_f1 - 0.65 * turb % p_kin(c) / flow % density(c)  &
         / (eps  % n(c) + TINY))                                     &
         * (zeta % n(c) - TWO_THIRDS)                                &
         / (turb % t_scale(c) + TINY)                                &
         + 0.0085 * (turb % p_kin(c) / flow % density(c)) / (kin % n(c) + TINY)
   b(c) = b(c) + f22hg * grid % vol(c) / (turb % l_scale(c)**2 + TINY)
 end do

 ! Source term f22hg
 do c = 1, grid % n_cells
   sor_11 = grid % vol(c)/(turb % l_scale(c)**2 + TINY)
   a % val(a % dia(c)) = a % val(a % dia(c)) + sor_11
 end do

 ! Imposing boundary condition for f22 on the wall
 do s = 1, grid % n_faces
   c1 = grid % faces_c(1,s)
   c2 = grid % faces_c(2,s)
   if(c2 < 0) then
     if(Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. WALL .or.  &
        Grid_Mod_Bnd_Cond_Type(grid,c2) .eq. WALLFL) then

       f22 % n(c2) = -2.0 * flow % viscosity(c1)        &
                   / flow % density(c1) * zeta % n(c1)  &
                   / grid % wall_dist(c1)**2

      ! Fill in a source coefficients

      ! Linearization of the near wall terms helps to get more
       ! stable solution, especially for small wall distance.
       b(c1) = b(c1) + a % fc(s) * f22 % n(c2)
     end if   ! end if of BC=wall
   end if    ! end if of c2<0
 end do

 end subroutine


C
C***********************************************************************
C                                                                      *
C            NACHOS II - A Finite Element Computer Program             *
C                        for Incompressible Flow Problems              *
C                                                                      *
C     Copyright (c) 1986,2019   National Technology & Engineering      *
C                               Solutions of Sandia, LLC (NTESS)       *
C                                                                      *
C                            All rights reserved.                      *
C                                                                      *
C     This software is distributed under the BSD 3-Clause License.     *
C                                                                      *
C***********************************************************************
C
      SUBROUTINE FLUX9 (NSTRSQ,SQ,TQ)
C
C     ******************************************************************
C
C     SUBROUTINE TO CALCULATE THE VALUES OF THE NINE NODE 'LAGRANGE'
C     SHAPE FUNCTIONS AND THEIR DERIVATIVES AT THE FLUX EVALUATION
C     POINTS.
C     THE POINTS MAY BE ON THE ELEMENT BOUNDARY OR AT INTERIOR POINTS
C
C     ******************************************************************
C
      COMMON /QUAD9/ F9Q(9,9),F9L(4,9),DF9QDS(9,9),DF9QDT(9,9)
C
      DIMENSION SQ(*), TQ(*)
      DIMENSION SI(8), TI(8)
C
      DATA (SI(I),I=1,8)/-1.,1.,1.,-1.,0.,1.,0.,-1./
      DATA (TI(I),I=1,8)/-1.,-1.,1.,1.,-1.,0.,1.,0./
C
C     STATEMENT FUNCTIONS FOR THE SHAPE FUNCTIONS AND THEIR DERIVATIVES
C
      FC(I,S,T)=.25*(1.+S*SI(I))*(1.+T*TI(I))*(S*SI(I)+T*TI(I)-1.)
      FM(I,S,T)=.50*((1.-S**2)*(1.+T*TI(I))*TI(I)**2+(1.-T**2)*(1.+S*SI
     1(I))*SI(I)**2)
      BUBBLE(S,T)=(1.-S**2)*(1.-T**2)
      G(I,S,T)=.25*(1.+S*SI(I))*(1.+T*TI(I))
      DFCDS(I,S,T)=.25*(1.+T*TI(I))*(2.*S*SI(I)+T*TI(I))*SI(I)
      DFMDS(I,S,T)=.50*((1.+T*TI(I))*(-2.*S)*TI(I)**2+(1.-T**2)*SI(I))
      DFCDT(I,S,T)=.25*(1.+S*SI(I))*(2.*T*TI(I)+S*SI(I))*TI(I)
      DFMDT(I,S,T)=.50*((1.+S*SI(I))*(-2.*T)*SI(I)**2+(1.-S**2)*TI(I))
      DBUBDS(S,T)=-2.*S*(1.-T**2)
      DBUBDT(S,T)=-2.*T*(1.-S**2)
C
C     ******************************************************************
C
C     EVALUATE CORNER FUNCTIONS
C
      DO 10 I=1,4
C
      DO 10 J=1,NSTRSQ
      DF9QDS(I,J)=DFCDS(I,SQ(J),TQ(J))+.25*DBUBDS(SQ(J),TQ(J))
      DF9QDT(I,J)=DFCDT(I,SQ(J),TQ(J))+.25*DBUBDT(SQ(J),TQ(J))
      F9Q(I,J)=FC(I,SQ(J),TQ(J))+.25*BUBBLE(SQ(J),TQ(J))
      F9L(I,J)=G(I,SQ(J),TQ(J))
   10 CONTINUE
C
C     EVALUATE MID-SIDE FUNCTIONS
C
      DO 20 I=5,8
C
      DO 20 J=1,NSTRSQ
      DF9QDS(I,J)=DFMDS(I,SQ(J),TQ(J))-.5*DBUBDS(SQ(J),TQ(J))
      DF9QDT(I,J)=DFMDT(I,SQ(J),TQ(J))-.5*DBUBDT(SQ(J),TQ(J))
      F9Q(I,J)=FM(I,SQ(J),TQ(J))-.5*BUBBLE(SQ(J),TQ(J))
   20 CONTINUE
C
C     EVALUATE CENTER  FUNCTION
C
      DO 30 J=1,NSTRSQ
      DF9QDS(9,J)=DBUBDS(SQ(J),TQ(J))
      DF9QDT(9,J)=DBUBDT(SQ(J),TQ(J))
      F9Q(9,J)=BUBBLE(SQ(J),TQ(J))
   30 CONTINUE
C
      RETURN
      END


c-----------------------------------------------------------------------
c\BeginDoc
c
c\Name: igraphdstqrb
c
c\Description:
c  Computes all eigenvalues and the last component of the eigenvectors
c  of a symmetric tridiagonal matrix using the implicit QL or QR method.
c
c  This is mostly a modification of the LAPACK routine dsteqr.
c  See Remarks.
c
c\Usage:
c  call igraphdstqrb
c     ( N, D, E, Z, WORK, INFO )
c
c\Arguments
c  N       Integer.  (INPUT)
c          The number of rows and columns in the matrix.  N >= 0.
c
c  D       Double precision array, dimension (N).  (INPUT/OUTPUT)
c          On entry, D contains the diagonal elements of the
c          tridiagonal matrix.
c          On exit, D contains the eigenvalues, in ascending order.
c          If an error exit is made, the eigenvalues are correct
c          for indices 1,2,...,INFO-1, but they are unordered and
c          may not be the smallest eigenvalues of the matrix.
c
c  E       Double precision array, dimension (N-1).  (INPUT/OUTPUT)
c          On entry, E contains the subdiagonal elements of the
c          tridiagonal matrix in positions 1 through N-1.
c          On exit, E has been destroyed.
c
c  Z       Double precision array, dimension (N).  (OUTPUT)
c          On exit, Z contains the last row of the orthonormal
c          eigenvector matrix of the symmetric tridiagonal matrix.
c          If an error exit is made, Z contains the last row of the
c          eigenvector matrix associated with the stored eigenvalues.
c
c  WORK    Double precision array, dimension (max(1,2*N-2)).  (WORKSPACE)
c          Workspace used in accumulating the transformation for
c          computing the last components of the eigenvectors.
c
c  INFO    Integer.  (OUTPUT)
c          = 0:  normal return.
c          < 0:  if INFO = -i, the i-th argument had an illegal value.
c          > 0:  if INFO = +i, the i-th eigenvalue has not converged
c                              after a total of  30*N  iterations.
c
c\Remarks
c  1. None.
c
c-----------------------------------------------------------------------
c
c\BeginLib
c
c\Local variables:
c     xxxxxx  real
c
c\Routines called:
c     daxpy   Level 1 BLAS that computes a vector triad.
c     dcopy   Level 1 BLAS that copies one vector to another.
c     dswap   Level 1 BLAS that swaps the contents of two vectors.
c     lsame   LAPACK character comparison routine.
c     dlae2   LAPACK routine that computes the eigenvalues of a 2-by-2
c             symmetric matrix.
c     dlaev2  LAPACK routine that eigendecomposition of a 2-by-2 symmetric
c             matrix.
c     dlamch  LAPACK routine that determines machine constants.
c     dlanst  LAPACK routine that computes the norm of a matrix.
c     dlapy2  LAPACK routine to compute sqrt(x**2+y**2) carefully.
c     dlartg  LAPACK Givens rotation construction routine.
c     dlascl  LAPACK routine for careful scaling of a matrix.
c     dlaset  LAPACK matrix initialization routine.
c     dlasr   LAPACK routine that applies an orthogonal transformation to
c             a matrix.
c     dlasrt  LAPACK sorting routine.
c     dsteqr  LAPACK routine that computes eigenvalues and eigenvectors
c             of a symmetric tridiagonal matrix.
c     xerbla  LAPACK error handler routine.
c
c\Authors
c     Danny Sorensen               Phuong Vu
c     Richard Lehoucq              CRPC / Rice University
c     Dept. of Computational &     Houston, Texas
c     Applied Mathematics
c     Rice University
c     Houston, Texas
c
c\SCCS Information: @(#)
c FILE: stqrb.F   SID: 2.5   DATE OF SID: 8/27/96   RELEASE: 2
c
c\Remarks
c     1. Starting with version 2.5, this routine is a modified version
c        of LAPACK version 2.0 subroutine SSTEQR. No lines are deleted,
c        only commeted out and new lines inserted.
c        All lines commented out have "c$$$" at the beginning.
c        Note that the LAPACK version 1.0 subroutine SSTEQR contained
c        bugs.
c
c\EndLib
c
c-----------------------------------------------------------------------
c
      subroutine igraphdstqrb ( n, d, e, z, work, info )
c
c     %------------------%
c     | Scalar Arguments |
c     %------------------%
c
      integer    info, n
c
c     %-----------------%
c     | Array Arguments |
c     %-----------------%
c
      Double precision
     &           d( n ), e( n-1 ), z( n ), work( 2*n-2 )
c
c     .. parameters ..
      Double precision
     &                   zero, one, two, three
      parameter          ( zero = 0.0D+0, one = 1.0D+0,
     &                     two = 2.0D+0, three = 3.0D+0 )
      integer            maxit
      parameter          ( maxit = 30 )
c     ..
c     .. local scalars ..
      integer            i, icompz, ii, iscale, j, jtot, k, l, l1, lend,
     &                   lendm1, lendp1, lendsv, lm1, lsv, m, mm, mm1,
     &                   nm1, nmaxit
      Double precision
     &                   anorm, b, c, eps, eps2, f, g, p, r, rt1, rt2,
     &                   s, safmax, safmin, ssfmax, ssfmin, tst
c     ..
c     .. external functions ..
      logical            lsame
      Double precision
     &                   dlamch, dlanst, dlapy2
      external           lsame, dlamch, dlanst, dlapy2
c     ..
c     .. external subroutines ..
      external           dlae2, dlaev2, dlartg, dlascl, dlaset, dlasr,
     &                   dlasrt, dswap, xerbla
c     ..
c     .. intrinsic functions ..
      intrinsic          abs, max, sign, sqrt
c     ..
c     .. executable statements ..
c
c     test the input parameters.
c
      info = 0
c
c$$$      IF( LSAME( COMPZ, 'N' ) ) THEN
c$$$         ICOMPZ = 0
c$$$      ELSE IF( LSAME( COMPZ, 'V' ) ) THEN
c$$$         ICOMPZ = 1
c$$$      ELSE IF( LSAME( COMPZ, 'I' ) ) THEN
c$$$         ICOMPZ = 2
c$$$      ELSE
c$$$         ICOMPZ = -1
c$$$      END IF
c$$$      IF( ICOMPZ.LT.0 ) THEN
c$$$         INFO = -1
c$$$      ELSE IF( N.LT.0 ) THEN
c$$$         INFO = -2
c$$$      ELSE IF( ( LDZ.LT.1 ) .OR. ( ICOMPZ.GT.0 .AND. LDZ.LT.MAX( 1,
c$$$     $         N ) ) ) THEN
c$$$         INFO = -6
c$$$      END IF
c$$$      IF( INFO.NE.0 ) THEN
c$$$         CALL XERBLA( 'SSTEQR', -INFO )
c$$$         RETURN
c$$$      END IF
c
c    *** New starting with version 2.5 ***
c
      icompz = 2
c    *************************************
c
c     quick return if possible
c
      if( n.eq.0 )
     $   return
c
      if( n.eq.1 ) then
         if( icompz.eq.2 )  z( 1 ) = one
         return
      end if
c
c     determine the unit roundoff and over/underflow thresholds.
c
      eps = dlamch( 'e' )
      eps2 = eps**2
      safmin = dlamch( 's' )
      safmax = one / safmin
      ssfmax = sqrt( safmax ) / three
      ssfmin = sqrt( safmin ) / eps2
c
c     compute the eigenvalues and eigenvectors of the tridiagonal
c     matrix.
c
c$$      if( icompz.eq.2 )
c$$$     $   call dlaset( 'full', n, n, zero, one, z, ldz )
c
c     *** New starting with version 2.5 ***
c
      if ( icompz .eq. 2 ) then
         do 5 j = 1, n-1
            z(j) = zero
  5      continue
         z( n ) = one
      end if
c     *************************************
c
      nmaxit = n*maxit
      jtot = 0
c
c     determine where the matrix splits and choose ql or qr iteration
c     for each block, according to whether top or bottom diagonal
c     element is smaller.
c
      l1 = 1
      nm1 = n - 1
c
   10 continue
      if( l1.gt.n )
     $   go to 160
      if( l1.gt.1 )
     $   e( l1-1 ) = zero
      if( l1.le.nm1 ) then
         do 20 m = l1, nm1
            tst = abs( e( m ) )
            if( tst.eq.zero )
     $         go to 30
            if( tst.le.( sqrt( abs( d( m ) ) )*sqrt( abs( d( m+
     $          1 ) ) ) )*eps ) then
               e( m ) = zero
               go to 30
            end if
   20    continue
      end if
      m = n
c
   30 continue
      l = l1
      lsv = l
      lend = m
      lendsv = lend
      l1 = m + 1
      if( lend.eq.l )
     $   go to 10
c
c     scale submatrix in rows and columns l to lend
c
      anorm = dlanst( 'i', lend-l+1, d( l ), e( l ) )
      iscale = 0
      if( anorm.eq.zero )
     $   go to 10
      if( anorm.gt.ssfmax ) then
         iscale = 1
         call dlascl( 'g', 0, 0, anorm, ssfmax, lend-l+1, 1, d( l ), n,
     $                info )
         call dlascl( 'g', 0, 0, anorm, ssfmax, lend-l, 1, e( l ), n,
     $                info )
      else if( anorm.lt.ssfmin ) then
         iscale = 2
         call dlascl( 'g', 0, 0, anorm, ssfmin, lend-l+1, 1, d( l ), n,
     $                info )
         call dlascl( 'g', 0, 0, anorm, ssfmin, lend-l, 1, e( l ), n,
     $                info )
      end if
c
c     choose between ql and qr iteration
c
      if( abs( d( lend ) ).lt.abs( d( l ) ) ) then
         lend = lsv
         l = lendsv
      end if
c
      if( lend.gt.l ) then
c
c        ql iteration
c
c        look for small subdiagonal element.
c
   40    continue
         if( l.ne.lend ) then
            lendm1 = lend - 1
            do 50 m = l, lendm1
               tst = abs( e( m ) )**2
               if( tst.le.( eps2*abs( d( m ) ) )*abs( d( m+1 ) )+
     $             safmin )go to 60
   50       continue
         end if
c
         m = lend
c
   60    continue
         if( m.lt.lend )
     $      e( m ) = zero
         p = d( l )
         if( m.eq.l )
     $      go to 80
c
c        if remaining matrix is 2-by-2, use dlae2 or dlaev2
c        to compute its eigensystem.
c
         if( m.eq.l+1 ) then
            if( icompz.gt.0 ) then
               call dlaev2( d( l ), e( l ), d( l+1 ), rt1, rt2, c, s )
               work( l ) = c
               work( n-1+l ) = s
c$$$               call dlasr( 'r', 'v', 'b', n, 2, work( l ),
c$$$     $                     work( n-1+l ), z( 1, l ), ldz )
c
c              *** New starting with version 2.5 ***
c
               tst      = z(l+1)
               z(l+1) = c*tst - s*z(l)
               z(l)   = s*tst + c*z(l)
c              *************************************
            else
               call dlae2( d( l ), e( l ), d( l+1 ), rt1, rt2 )
            end if
            d( l ) = rt1
            d( l+1 ) = rt2
            e( l ) = zero
            l = l + 2
            if( l.le.lend )
     $         go to 40
            go to 140
         end if
c
         if( jtot.eq.nmaxit )
     $      go to 140
         jtot = jtot + 1
c
c        form shift.
c
         g = ( d( l+1 )-p ) / ( two*e( l ) )
         r = dlapy2( g, one )
         g = d( m ) - p + ( e( l ) / ( g+sign( r, g ) ) )
c
         s = one
         c = one
         p = zero
c
c        inner loop
c
         mm1 = m - 1
         do 70 i = mm1, l, -1
            f = s*e( i )
            b = c*e( i )
            call dlartg( g, f, c, s, r )
            if( i.ne.m-1 )
     $         e( i+1 ) = r
            g = d( i+1 ) - p
            r = ( d( i )-g )*s + two*c*b
            p = s*r
            d( i+1 ) = g + p
            g = c*r - b
c
c           if eigenvectors are desired, then save rotations.
c
            if( icompz.gt.0 ) then
               work( i ) = c
               work( n-1+i ) = -s
            end if
c
   70    continue
c
c        if eigenvectors are desired, then apply saved rotations.
c
         if( icompz.gt.0 ) then
            mm = m - l + 1
c$$$            call dlasr( 'r', 'v', 'b', n, mm, work( l ), work( n-1+l ),
c$$$     $                  z( 1, l ), ldz )
c
c             *** New starting with version 2.5 ***
c
              call dlasr( 'r', 'v', 'b', 1, mm, work( l ),
     &                    work( n-1+l ), z( l ), 1 )
c             *************************************
         end if
c
         d( l ) = d( l ) - p
         e( l ) = g
         go to 40
c
c        eigenvalue found.
c
   80    continue
         d( l ) = p
c
         l = l + 1
         if( l.le.lend )
     $      go to 40
         go to 140
c
      else
c
c        qr iteration
c
c        look for small superdiagonal element.
c
   90    continue
         if( l.ne.lend ) then
            lendp1 = lend + 1
            do 100 m = l, lendp1, -1
               tst = abs( e( m-1 ) )**2
               if( tst.le.( eps2*abs( d( m ) ) )*abs( d( m-1 ) )+
     $             safmin )go to 110
  100       continue
         end if
c
         m = lend
c
  110    continue
         if( m.gt.lend )
     $      e( m-1 ) = zero
         p = d( l )
         if( m.eq.l )
     $      go to 130
c
c        if remaining matrix is 2-by-2, use dlae2 or dlaev2
c        to compute its eigensystem.
c
         if( m.eq.l-1 ) then
            if( icompz.gt.0 ) then
               call dlaev2( d( l-1 ), e( l-1 ), d( l ), rt1, rt2, c, s )
c$$$               work( m ) = c
c$$$               work( n-1+m ) = s
c$$$               call dlasr( 'r', 'v', 'f', n, 2, work( m ),
c$$$     $                     work( n-1+m ), z( 1, l-1 ), ldz )
c
c               *** New starting with version 2.5 ***
c
                tst      = z(l)
                z(l)   = c*tst - s*z(l-1)
                z(l-1) = s*tst + c*z(l-1)
c               *************************************
            else
               call dlae2( d( l-1 ), e( l-1 ), d( l ), rt1, rt2 )
            end if
            d( l-1 ) = rt1
            d( l ) = rt2
            e( l-1 ) = zero
            l = l - 2
            if( l.ge.lend )
     $         go to 90
            go to 140
         end if
c
         if( jtot.eq.nmaxit )
     $      go to 140
         jtot = jtot + 1
c
c        form shift.
c
         g = ( d( l-1 )-p ) / ( two*e( l-1 ) )
         r = dlapy2( g, one )
         g = d( m ) - p + ( e( l-1 ) / ( g+sign( r, g ) ) )
c
         s = one
         c = one
         p = zero
c
c        inner loop
c
         lm1 = l - 1
         do 120 i = m, lm1
            f = s*e( i )
            b = c*e( i )
            call dlartg( g, f, c, s, r )
            if( i.ne.m )
     $         e( i-1 ) = r
            g = d( i ) - p
            r = ( d( i+1 )-g )*s + two*c*b
            p = s*r
            d( i ) = g + p
            g = c*r - b
c
c           if eigenvectors are desired, then save rotations.
c
            if( icompz.gt.0 ) then
               work( i ) = c
               work( n-1+i ) = s
            end if
c
  120    continue
c
c        if eigenvectors are desired, then apply saved rotations.
c
         if( icompz.gt.0 ) then
            mm = l - m + 1
c$$$            call dlasr( 'r', 'v', 'f', n, mm, work( m ), work( n-1+m ),
c$$$     $                  z( 1, m ), ldz )
c
c           *** New starting with version 2.5 ***
c
            call dlasr( 'r', 'v', 'f', 1, mm, work( m ), work( n-1+m ),
     &                  z( m ), 1 )
c           *************************************
         end if
c
         d( l ) = d( l ) - p
         e( lm1 ) = g
         go to 90
c
c        eigenvalue found.
c
  130    continue
         d( l ) = p
c
         l = l - 1
         if( l.ge.lend )
     $      go to 90
         go to 140
c
      end if
c
c     undo scaling if necessary
c
  140 continue
      if( iscale.eq.1 ) then
         call dlascl( 'g', 0, 0, ssfmax, anorm, lendsv-lsv+1, 1,
     $                d( lsv ), n, info )
         call dlascl( 'g', 0, 0, ssfmax, anorm, lendsv-lsv, 1, e( lsv ),
     $                n, info )
      else if( iscale.eq.2 ) then
         call dlascl( 'g', 0, 0, ssfmin, anorm, lendsv-lsv+1, 1,
     $                d( lsv ), n, info )
         call dlascl( 'g', 0, 0, ssfmin, anorm, lendsv-lsv, 1, e( lsv ),
     $                n, info )
      end if
c
c     check for no convergence to an eigenvalue after a total
c     of n*maxit iterations.
c
      if( jtot.lt.nmaxit )
     $   go to 10
      do 150 i = 1, n - 1
         if( e( i ).ne.zero )
     $      info = info + 1
  150 continue
      go to 190
c
c     order eigenvalues and eigenvectors.
c
  160 continue
      if( icompz.eq.0 ) then
c
c        use quick sort
c
         call dlasrt( 'i', n, d, info )
c
      else
c
c        use selection sort to minimize swaps of eigenvectors
c
         do 180 ii = 2, n
            i = ii - 1
            k = i
            p = d( i )
            do 170 j = ii, n
               if( d( j ).lt.p ) then
                  k = j
                  p = d( j )
               end if
  170       continue
            if( k.ne.i ) then
               d( k ) = d( i )
               d( i ) = p
c$$$               call dswap( n, z( 1, i ), 1, z( 1, k ), 1 )
c           *** New starting with version 2.5 ***
c
               p    = z(k)
               z(k) = z(i)
               z(i) = p
c           *************************************
            end if
  180    continue
      end if
c
  190 continue
      return
c
c     %---------------%
c     | End of igraphdstqrb |
c     %---------------%
c
      end


C * * * * * * * * * * * * * * * * * * * * * * * * *
C --- DRIVER FOR RADAU5 ON PLATE PROBLEM
C * * * * * * * * * * * * * * * * * * * * * * * * *
        IMPLICIT REAL*8 (A-H,O-Z)
C --- PARAMETERS FOR RADAU5 (FULL JACOBIAN)
      PARAMETER(MX=8,MY=5,MACHS1=2,MACHS2=4)
        PARAMETER (ND=2*MX*MY,LWORK=4*ND*ND+12*ND+20,LIWORK=3*ND+20)
        DIMENSION Y(ND),WORK(LWORK),IWORK(LIWORK),ISTAT(20)
C -------- END PARAMETER LIST --------
        REAL*4 TARRAY(2),TRESULT
        EXTERNAL FPLATE,JPLATE,JPLATS,JPLATSB,SOLOUT
C -------- CONSTANTES ------------------
      COMMON/TRANS/NX,NXM1,NY,NYM1,NDEMI,NACHS1,NACHS2,NDUMMY,
     &       OMEGA,STIFFN,DELX,USH4,FAC,WEIGHT
      NX=MX
      NY=MY
      NACHS1=MACHS1
      NACHS2=MACHS2
      NXM1=NX-1
      NYM1=NY-1
      NDEMI=NX*NY
      OMEGA=1000.D0
      STIFFN=100.D0
      WEIGHT=200.D0
      DENOM=NX+1
      DELX=2.D0/DENOM
      USH4=1.D0/(DELX**4)
      FAC=STIFFN*USH4
c ------ FILE DE DONNEES ----------
        OPEN(8,FILE='res_rad5')
        REWIND 8
C -------- TWO OPTIONS -------
C --- LOOP FOR DIFFERENT TOLERANCES
        NTOLMN=2
        NTOLMX=10
        NTOLDF=4
        NRLOOP=(NTOLMX-NTOLMN)*NTOLDF+1
        TOLST=0.1D0**NTOLMN
        TOLFC=0.1D0**(1.D0/NTOLDF)
        DO 30 NTOL=1,NRLOOP
C --- DIMENSION OF THE SYSTEM
        N=ND
C --- DIFFERENTIAL EQUATION IS IN EXPLICIT FORM
        IMAS=0
C --- OUTPUT ROUTINE IS NOT USED DURING INTEGRATION
        IOUT=0
C --- INITIAL VALUES
        X=0.0D0
      DO 1 I=1,N
  1   Y(I)=0.D0
C --- REQUIRED TOLERANCE
        RTOL=TOLST
        ATOL=RTOL*1.0D-3
        ITOL=0
C --- INITIAL STEP SIZE
        H=1.0D-2
C --- SET DEFAULT VALUES
        DO I=1,20
           WORK(I)=0.D0
           IWORK(I)=0
           ISTAT(I)=0
        END DO
C --- SECOND ORDER OPTION AND BANDED
           IJAC=1
           IWORK(9)=N/2
           MLJAC=2*MX
           MUJAC=2*MX
c          MLJAC=N
C --- ENDPOINT OF INTEGRATION
        XEND=7.D0
        CALL DTIME(TARRAY,TRESULT)
C --- CALL OF THE SUBROUTINE RADAU5
        CALL RADAU5(N,FPLATE,X,Y,XEND,H,
     &                  RTOL,ATOL,ITOL,
c     &                  JPLATE,IJAC,MLJAC,MUJAC,
     &                  JPLATSB,IJAC,MLJAC,MUJAC,
     &                  FPLATE,IMAS,MLMAS,MUMAS,
     &                  SOLOUT,IOUT,
     &                  WORK,LWORK,IWORK,LIWORK,RPAR,IPAR,IDID)
C --- PRINT SOLUTION
      DO 15 K=1,N
 15   WRITE (8,*)Y(K)
      CALL DTIME(TARRAY,TRESULT)
         DO J=14,20
            ISTAT(J)=ISTAT(J)+IWORK(J)
         END DO
        WRITE(8,*)TARRAY(1)
        WRITE (8,*)(ISTAT(J),J=14,20)
        WRITE(6,*)' ***** TOL=',RTOL,'  ELAPSED TIME=',TARRAY(1),' ****'
        WRITE (6,91) (ISTAT(J),J=14,20)
 91     FORMAT(' fcn=',I5,' jac=',I4,' step=',I4,
     &        ' accpt=',I4,' rejct=',I3,' dec=',I4,
     &        ' sol=',I5)
C -------- NEW TOLERANCE ---
        TOLST=TOLST*TOLFC
 30     CONTINUE
        STOP
        END
C
        SUBROUTINE SOLOUT (NR,XOLD,X,Y,CONT,LRC,N,RPAR,IPAR,IRTRN)
        IMPLICIT REAL*8 (A-H,O-Z)
        DIMENSION Y(N),CONT(LRC)
      WRITE (6,991) X,NR-1
 991  FORMAT(1X,'X =',F9.5,'    NSTEP =',I4)
C      WRITE(6,992)(Y(I),I=1,N,N/10)
 992  FORMAT(10X,10F14.10)
        RETURN
        END


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!> A Module For Computing The Inverse of a Matrix.
MODULE InverseSolversModule
  USE DataTypesModule, ONLY : NTREAL
  USE LoadBalancerModule, ONLY : PermuteMatrix, UndoPermuteMatrix
  USE LoggingModule, ONLY : EnterSubLog, ExitSubLog, WriteHeader, &
       & WriteElement, WriteListElement, WriteCitation
  USE PMatrixMemoryPoolModule, ONLY : MatrixMemoryPool_p, &
       & DestructMatrixMemoryPool
  USE PSMatrixAlgebraModule, ONLY : MatrixMultiply, IncrementMatrix, &
       & MatrixNorm, MatrixSigma, ScaleMatrix
  USE PSMatrixModule, ONLY : Matrix_ps, ConstructEmptyMatrix, CopyMatrix, &
       & DestructMatrix, FillMatrixIdentity, PrintMatrixInformation
  USE SolverParametersModule, ONLY : SolverParameters_t, PrintParameters, &
       & DestructSolverParameters
  IMPLICIT NONE
  PRIVATE
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !! Solvers
  PUBLIC :: Invert
  PUBLIC :: PseudoInverse
CONTAINS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !> Compute the inverse of a matrix.
  !> An implementation of the method of Hotelling \cite palser1998canonical.
  SUBROUTINE Invert(Mat, InverseMat, solver_parameters_in)
    !> The matrix to invert.
    TYPE(Matrix_ps), INTENT(IN)  :: Mat
    !> The inverse of that matrix.
    TYPE(Matrix_ps), INTENT(INOUT) :: InverseMat
    !> Parameters for the solver
    TYPE(SolverParameters_t), INTENT(IN), OPTIONAL :: solver_parameters_in
    !! Handling Optional Parameters
    TYPE(SolverParameters_t) :: solver_parameters
    !! Local Variables
    REAL(NTREAL) :: sigma
    TYPE(Matrix_ps) :: Temp1,Temp2,Identity
    TYPE(Matrix_ps) :: BalancedMat
    !! Temporary Variables
    INTEGER :: outer_counter
    REAL(NTREAL) :: norm_value
    TYPE(MatrixMemoryPool_p) :: pool

    !! Optional Parameters
    IF (PRESENT(solver_parameters_in)) THEN
       solver_parameters = solver_parameters_in
    ELSE
       solver_parameters = SolverParameters_t()
    END IF

    IF (solver_parameters%be_verbose) THEN
       CALL WriteHeader("Inverse Solver")
       CALL EnterSubLog
       CALL WriteCitation("palser1998canonical")
       CALL PrintParameters(solver_parameters)
    END IF

    !! Construct All The Necessary Matrices
    CALL ConstructEmptyMatrix(InverseMat, Mat)
    CALL ConstructEmptyMatrix(Temp1, Mat)
    CALL ConstructEmptyMatrix(Temp2, Mat)
    CALL ConstructEmptyMatrix(Identity, Mat)
    CALL ConstructEmptyMatrix(BalancedMat, Mat)
    CALL FillMatrixIdentity(Identity)

    !! Load Balancing Step
    IF (solver_parameters%do_load_balancing) THEN
       CALL PermuteMatrix(Identity, Identity, &
            & solver_parameters%BalancePermutation, memorypool_in=pool)
       CALL PermuteMatrix(Mat, BalancedMat, &
            & solver_parameters%BalancePermutation, memorypool_in=pool)
    ELSE
       CALL CopyMatrix(Mat,BalancedMat)
    END IF

    !! Compute Sigma
    CALL MatrixSigma(BalancedMat,sigma)

    !! Create Inverse Guess
    CALL CopyMatrix(BalancedMat,InverseMat)
    CALL ScaleMatrix(InverseMat,sigma)

    !! Iterate
    IF (solver_parameters%be_verbose) THEN
       CALL WriteHeader("Iterations")
       CALL EnterSubLog
    END IF
    outer_counter = 1
    norm_value = solver_parameters%converge_diff + 1.0_NTREAL
    DO outer_counter = 1,solver_parameters%max_iterations
       IF (solver_parameters%be_verbose .AND. outer_counter .GT. 1) THEN
          CALL WriteListElement(key="Round", VALUE=outer_counter-1)
          CALL EnterSubLog
          CALL WriteListElement(key="Convergence", VALUE=norm_value)
          CALL ExitSubLog
       END IF

       CALL MatrixMultiply(InverseMat,BalancedMat,Temp1, &
            & threshold_in=solver_parameters%threshold, memory_pool_in=pool)

       !! Check if Converged
       CALL CopyMatrix(Identity,Temp2)
       CALL IncrementMatrix(Temp1,Temp2,-1.0_NTREAL)
       norm_value = MatrixNorm(Temp2)

       CALL DestructMatrix(Temp2)
       CALL MatrixMultiply(Temp1,InverseMat,Temp2,alpha_in=-1.0_NTREAL, &
            & threshold_in=solver_parameters%threshold,memory_pool_in=pool)

       !! Save a copy of the last inverse matrix
       CALL CopyMatrix(InverseMat,Temp1)

       CALL ScaleMatrix(InverseMat,2.0_NTREAL)

       CALL IncrementMatrix(Temp2,InverseMat, &
            & threshold_in=solver_parameters%threshold)

       IF (norm_value .LE. solver_parameters%converge_diff) THEN
          EXIT
       END IF
    END DO
    IF (solver_parameters%be_verbose) THEN
       CALL ExitSubLog
       CALL WriteElement(key="Total_Iterations", VALUE=outer_counter-1)
       CALL PrintMatrixInformation(InverseMat)
    END IF

    !! Undo Load Balancing Step
    IF (solver_parameters%do_load_balancing) THEN
       CALL UndoPermuteMatrix(InverseMat,InverseMat, &
            & solver_parameters%BalancePermutation, memorypool_in=pool)
    END IF

    IF (solver_parameters%be_verbose) THEN
       CALL ExitSubLog
    END IF

    !! Cleanup
    CALL DestructMatrix(Temp1)
    CALL DestructMatrix(Temp2)
    CALL DestructMatrix(BalancedMat)
    CALL DestructMatrixMemoryPool(pool)
    CALL DestructSolverParameters(solver_parameters)
  END SUBROUTINE Invert
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !> Compute the pseudoinverse of a matrix.
  !> An implementation of the method of Hotelling \cite palser1998canonical.
  SUBROUTINE PseudoInverse(Mat, InverseMat, solver_parameters_in)
    !> The matrix to compute the pseudo inverse of.
    TYPE(Matrix_ps), INTENT(IN)  :: Mat
    !> The pseudoinverse of the input matrix.
    TYPE(Matrix_ps), INTENT(INOUT) :: InverseMat
    !> Parameters for the solver
    TYPE(SolverParameters_t), INTENT(IN), OPTIONAL :: solver_parameters_in
    !! Handling Optional Parameters
    TYPE(SolverParameters_t) :: solver_parameters
    !! Local Variables
    REAL(NTREAL) :: sigma
    TYPE(Matrix_ps) :: Temp1,Temp2,Identity
    TYPE(Matrix_ps) :: BalancedMat
    !! Temporary Variables
    INTEGER :: outer_counter
    REAL(NTREAL) :: norm_value
    TYPE(MatrixMemoryPool_p) :: pool

    !! Optional Parameters
    IF (PRESENT(solver_parameters_in)) THEN
       solver_parameters = solver_parameters_in
    ELSE
       solver_parameters = SolverParameters_t()
    END IF

    IF (solver_parameters%be_verbose) THEN
       CALL WriteHeader("Inverse Solver")
       CALL EnterSubLog
       CALL WriteCitation("palser1998canonical")
       CALL PrintParameters(solver_parameters)
    END IF

    !! Construct All The Necessary Matrices
    CALL ConstructEmptyMatrix(InverseMat, Mat)
    CALL ConstructEmptyMatrix(Temp1, Mat)
    CALL ConstructEmptyMatrix(Temp2, Mat)
    CALL ConstructEmptyMatrix(Identity, Mat)
    CALL ConstructEmptyMatrix(BalancedMat, Mat)
    CALL FillMatrixIdentity(Identity)

    !! Load Balancing Step
    IF (solver_parameters%do_load_balancing) THEN
       CALL PermuteMatrix(Identity, Identity, &
            & solver_parameters%BalancePermutation, memorypool_in=pool)
       CALL PermuteMatrix(Mat, BalancedMat, &
            & solver_parameters%BalancePermutation, memorypool_in=pool)
    ELSE
       CALL CopyMatrix(Mat,BalancedMat)
    END IF

    !! Compute Sigma
    CALL MatrixSigma(BalancedMat,sigma)

    !! Create Inverse Guess
    CALL CopyMatrix(BalancedMat,InverseMat)
    CALL ScaleMatrix(InverseMat,sigma)

    !! Iterate
    IF (solver_parameters%be_verbose) THEN
       CALL WriteHeader("Iterations")
       CALL EnterSubLog
    END IF
    outer_counter = 1
    norm_value = solver_parameters%converge_diff + 1.0_NTREAL
    DO outer_counter = 1,solver_parameters%max_iterations
       IF (solver_parameters%be_verbose .AND. outer_counter .GT. 1) THEN
          CALL WriteListElement(key="Round", VALUE=outer_counter-1)
          CALL EnterSubLog
          CALL WriteListElement(key="Convergence", VALUE=norm_value)
          CALL ExitSubLog
       END IF

       CALL MatrixMultiply(InverseMat,BalancedMat,Temp1, &
            & threshold_in=solver_parameters%threshold, memory_pool_in=pool)
       CALL MatrixMultiply(Temp1,InverseMat,Temp2,alpha_in=-1.0_NTREAL, &
            & threshold_in=solver_parameters%threshold,memory_pool_in=pool)

       !! Save a copy of the last inverse matrix
       CALL CopyMatrix(InverseMat,Temp1)

       CALL ScaleMatrix(InverseMat,2.0_NTREAL)
       CALL IncrementMatrix(Temp2,InverseMat, &
            & threshold_in=solver_parameters%threshold)

       !! Check if Converged
       CALL IncrementMatrix(InverseMat,Temp1,-1.0_NTREAL)
       norm_value = MatrixNorm(Temp1)

       !! Sometimes the first few values don't change so much, so that's why
       !! I added the outer counter check
       IF (norm_value .LE. solver_parameters%converge_diff .AND. &
            & outer_counter .GT. 3) THEN
          EXIT
       END IF
    END DO
    IF (solver_parameters%be_verbose) THEN
       CALL ExitSubLog
       CALL WriteElement(key="Total_Iterations", VALUE=outer_counter-1)
       CALL PrintMatrixInformation(InverseMat)
    END IF

    !! Undo Load Balancing Step
    IF (solver_parameters%do_load_balancing) THEN
       CALL UndoPermuteMatrix(InverseMat,InverseMat, &
            & solver_parameters%BalancePermutation, memorypool_in=pool)
    END IF

    IF (solver_parameters%be_verbose) THEN
       CALL ExitSubLog
    END IF

    !! Cleanup
    CALL DestructMatrix(Temp1)
    CALL DestructMatrix(Temp2)
    CALL DestructMatrix(BalancedMat)
    CALL DestructMatrixMemoryPool(pool)
    CALL DestructSolverParameters(solver_parameters)
  END SUBROUTINE PseudoInverse
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
END MODULE InverseSolversModule


        SUBROUTINE QRSLQF(QT,R,B,X,N)
C
C SUBROUTINE QRSLQF SOLVES THE SYSTEM  R*S = QT*B  FOR  S.
C
C
C ON INPUT:
C
C QT(1:N,1:N)  CONTAINS QT IN THE EQUATION ABOVE.
C
C R(1:N*(N+1)/2)  CONTAINS THE UPPER TRIANGLE OF  R  IN THE EQUATION
C     ABOVE, STORED BY ROWS.
C
C B(1:N)  CONTAINS  B  IN THE EQUATION ABOVE.
C
C N   IS THE DIMENSION OF THE PROBLEM.
C
C
C ON OUTPUT:
C
C QT  AND  R  ARE UNCHANGED.
C
C B   CONTAINS THE SOLUTION VECTOR S.
C
C X(1:N)  IS A WORK ARRAY WHICH CONTAINS QT*B ON OUTPUT.
C
C
C CALLS DDOT.
C
C ***** DECLARATIONS *****
C
C     FUNCTION DECLARATIONS
C
        DOUBLE PRECISION DDOT
C
C     LOCAL VARIABLES
C
        DOUBLE PRECISION TAU
        INTEGER INDEXR, I, J
C
C     SCALAR ARGUMENTS
C
        INTEGER N
C
C     ARRAY DECLARATIONS
C
        DOUBLE PRECISION QT(N,N),R(N*(N+1)/2),B(N),X(N)
C
C ***** END OF DECLARATIONS *****
C
C ***** FIRST EXECUTABLE STATEMENT *****
C
C X = QT*B.
C
        DO 10 I=1,N
          X(I) = DDOT(N,QT(I,1),N,B,1)
  10    CONTINUE
C
C COMPUTE S USING BACK SUBSTITUTION.
C
        INDEXR = N*(N+1)/2
        B(N) = X(N)/R(INDEXR)
        INDEXR = INDEXR - 1
        DO 30 I=N-1,1,-1
          TAU = X(I)
          DO 20 J=N,I+1,-1
            TAU = TAU - R(INDEXR)*B(J)
            INDEXR = INDEXR - 1
  20     CONTINUE
          B(I) = TAU/R(INDEXR)
          INDEXR = INDEXR - 1
  30    CONTINUE
        RETURN
C
C ***** END OF SUBROUTINE QRSLQF *****
        END


SUBROUTINE CONGRADC()

  ! **  SUBROUTINE CONGRAD SOLVES THE EXTERNAL MODE BY A CONJUGATE
  ! **  GRADIENT SCHEME

  !----------------------------------------------------------------------!
  ! CHANGE RECORD
  ! DATE MODIFIED     BY               DESCRIPTION
  !----------------------------------------------------------------------!
  ! 2014-08           D H CHUNG          SET EXPLICIT PRECISIONS OF INTEGER & REAL
  ! 2011-03           Paul M. Craig      Rewritten to F90

  USE GLOBAL
  USE EFDCOUT

  ! *** DS-INTL
  IMPLICIT NONE

  INTEGER    :: L, NMD
  INTEGER    :: LHOST,LCHNU,LCHNV

  REAL       :: RPCGN, RPCG, PAPCG, ALPHA, BETA, RSQ
  REAL(RKD)  :: TTDS         ! MODEL TIMING TEMPORARY VARIABLE
  REAL(RKD), EXTERNAL   :: DSTIME

  REAL,SAVE,ALLOCATABLE,DIMENSION(:) :: PNORTH
  REAL,SAVE,ALLOCATABLE,DIMENSION(:) :: PSOUTH
  REAL,SAVE,ALLOCATABLE,DIMENSION(:) :: TMPCG
  REAL,SAVE,ALLOCATABLE,DIMENSION(:) :: PCG
  REAL,SAVE,ALLOCATABLE,DIMENSION(:) :: RCG
  REAL,SAVE,ALLOCATABLE,DIMENSION(:) :: SRPCGN
  REAL,SAVE,ALLOCATABLE,DIMENSION(:) :: SRSQ

  IF(  .NOT. ALLOCATED(PNORTH) )THEN
    ALLOCATE(PNORTH(LCM))
    ALLOCATE(PSOUTH(LCM))
    ALLOCATE(PCG(LCM))
    ALLOCATE(RCG(LCM))
    ALLOCATE(SRPCGN(NDM))
    ALLOCATE(SRSQ(NDM))
    ALLOCATE(TMPCG(LCM))
    PNORTH=0.0
    PSOUTH=0.0
    PCG=0.0
    RCG=0.0
    SRSQ=0.0
    TMPCG=0.0
  ENDIF

  TTDS=DSTIME(0)
  DO L=2,LA
    PNORTH(L)=P(LNC(L))
    PSOUTH(L)=P(LSC(L))
  ENDDO
  DO L=2,LA
    RCG(L) = FPTMP(L) - CCC(L)*P(L) - CCN(L)*PNORTH(L) - CCS(L)*PSOUTH(L) &
                                    - CCW(L)*P(LWC(L)) - CCE(L)*P(LEC(L))
  ENDDO
  IF( MDCHH >= 1 )THEN
    DO NMD=1,MDCHH
      LHOST=LMDCHH(NMD)
      LCHNU=LMDCHU(NMD)
      LCHNV=LMDCHV(NMD)

      !         X-DIRECTION CHANNEL
      IF( MDCHTYP(NMD) == 1 )THEN
        RCG(LCHNU)=RCG(LCHNU)+CCCCHH(NMD)*P(LHOST)
        RCG(LHOST)=RCG(LHOST)+CCCCHH(NMD)*P(LCHNU)
      ENDIF

      !         Y-DIRECTION CHANNEL
      IF( MDCHTYP(NMD) == 2 )THEN
        RCG(LCHNV)=RCG(LCHNV)+CCCCHH(NMD)*P(LHOST)
        RCG(LHOST)=RCG(LHOST)+CCCCHH(NMD)*P(LCHNV)
      ENDIF
    ENDDO
  ENDIF

  DO L=2,LA
    PCG(L)=RCG(L)*CCCI(L)
  ENDDO
  RPCG=0.
  DO L=2,LA
    RPCG=RPCG+RCG(L)*PCG(L)
  ENDDO
  IF( RPCG == 0.0)RETURN
  ITER=0

100 CONTINUE
  ITER=ITER+1
  DO L=2,LA
    PNORTH(L)=PCG(LNC(L))
    PSOUTH(L)=PCG(LSC(L))
  ENDDO
  DO L=2,LA
    APCG(L)=CCC(L)*PCG(L)+CCS(L)*PSOUTH(L)+CCN(L)*PNORTH(L) &
        +CCW(L)*PCG(LWC(L))+CCE(L)*PCG(LEC(L))
  ENDDO
  IF( MDCHH >= 1 )THEN
    DO NMD=1,MDCHH
      LHOST=LMDCHH(NMD)
      LCHNU=LMDCHU(NMD)
      LCHNV=LMDCHV(NMD)

      !         X-DIRECTION CHANNEL
      IF( MDCHTYP(NMD) == 1 )THEN
        APCG(LCHNU)=APCG(LCHNU)+CCCCHH(NMD)*PCG(LHOST)
        APCG(LHOST)=APCG(LHOST)+CCCCHH(NMD)*PCG(LCHNU)
      ENDIF

      !         Y-DIRECTION CHANNEL
      IF( MDCHTYP(NMD) == 2 )THEN
        APCG(LCHNV)=APCG(LCHNV)+CCCCHH(NMD)*PCG(LHOST)
        APCG(LHOST)=APCG(LHOST)+CCCCHH(NMD)*PCG(LCHNV)
      ENDIF
    ENDDO
  ENDIF

  PAPCG=0.
  DO L=2,LA
    PAPCG=PAPCG+APCG(L)*PCG(L)
  ENDDO
  ALPHA=RPCG/PAPCG
  DO L=2,LA
    P(L)=P(L)+ALPHA*PCG(L)
  ENDDO
  DO L=2,LA
    RCG(L)=RCG(L)-ALPHA*APCG(L)
  ENDDO
  DO L=2,LA
    TMPCG(L)=CCCI(L)*RCG(L)
  ENDDO
  RPCGN=0.
  RSQ=0.
  DO L=2,LA
    RPCGN=RPCGN+RCG(L)*TMPCG(L)
    RSQ=RSQ+RCG(L)*RCG(L)
  ENDDO
  IF( RSQ  <=  RSQM) GOTO 200
  IF( ITER  >=  ITERM )THEN
    OPEN(8,FILE=OUTDIR//'EFDCLOG.OUT',POSITION='APPEND')
    WRITE(6,600)
    WRITE(8,*)'  I   J   CCS   CCW    CCC    CCE    CCN   FPTMP'
    DO L=2,LA
      WRITE(8,800)IL(L),JL(L),CCS(L),CCW(L),CCC(L),CCE(L),CCN(L),FPTMP(L)
    ENDDO
    CLOSE(8)
    CALL EE_LINKAGE(-1)
    STOP
  ENDIF

  BETA=RPCGN/RPCG
  RPCG=RPCGN
  DO L=2,LA
    PCG(L)=TMPCG(L)+BETA*PCG(L)
  ENDDO

  GOTO 100
  600 FORMAT('  MAXIMUM ITERATIONS EXCEEDED IN EXTERNAL SOLUTION')

  ! ** CALCULATE FINAL RESIDUAL
  200 CONTINUE

  IF( ISLOG >= 1 )THEN
    DO L=2,LA
      PNORTH(L)=P(LNC(L))
      PSOUTH(L)=P(LSC(L))
    ENDDO
    RSQ=0.
    DO L=2,LA
      RCG(L)=CCC(L)*P(L)+CCS(L)*PSOUTH(L)+CCN(L)*PNORTH(L) &
        +CCW(L)*P(LWC(L))+CCE(L)*P(LEC(L))-FPTMP(L)
    ENDDO
    IF( MDCHH >= 1 )THEN
      DO NMD=1,MDCHH
        LHOST=LMDCHH(NMD)
        LCHNU=LMDCHU(NMD)
        LCHNV=LMDCHV(NMD)

        !         X-DIRECTION CHANNEL
        IF( MDCHTYP(NMD) == 1 )THEN
          RCG(LCHNU)=RCG(LCHNU)-CCCCHH(NMD)*P(LHOST)
          RCG(LHOST)=RCG(LHOST)-CCCCHH(NMD)*P(LCHNU)
        ENDIF

        !         Y-DIRECTION CHANNEL
        IF( MDCHTYP(NMD) == 2 )THEN
          RCG(LCHNV)=RCG(LCHNV)-CCCCHH(NMD)*P(LHOST)
          RCG(LHOST)=RCG(LHOST)-CCCCHH(NMD)*P(LCHNV)
        ENDIF
      ENDDO
    ENDIF
    DO L=2,LA
      RCG(L)=RCG(L)*CCCI(L)
    ENDDO
    DO L=2,LA
      RSQ=RSQ+RCG(L)*RCG(L)
    ENDDO
  ENDIF

  TCONG=TCONG+(DSTIME(0)-TTDS)

  800 FORMAT(2I6,6E13.4)

  RETURN
END


c*************************************************************************
c                          ANAL_ENERGY.F
c*************************************************************************
c Calculates the energy of the total system (massive bodies) wrt time.
c returns the total energy of n objects by direct pairwise summation
c G = 1., and we allow diff. masses.  Also returns square of total ang. mom.
c
c      Input:
c            t             ==>  current time
c            nbod          ==>  number of massive bodies (int scalar)
c            mass          ==>  mass of bodies (real array)
c            j2rp2         ==>  scaled value of j2 moment (real*8 scalar)
c            j4rp4         ==>  scaled value of j4 moment (real*8 scalar)
c            xh,yh,zh      ==>  current position in heliocentric coord
c                               (real arrays)
c            vxh,vyh,vzh   ==>  current velocity in heliocentric coord
c                               (real arrays)
c
c      Output:
c            ke            ==>  kinetic energy
c            pot           ==>  potential energy
c            energy        ==>  Total energy
c            eltot         ==>  components of total angular momentum
c                               (real array)
c
c Remarks:
c Authors:  Martin Duncan
c Date:  ?
c Last revision:  1/24/97 HFL

      subroutine anal_energy(nbod,mass,j2rp2,j4rp4,xh,yh,zh,
     &           vxh,vyh,vzh,ke,pot,energy,eltot)

      include '../swift.inc'

c...  Inputs:
      integer nbod
      real*8 mass(nbod),j2rp2,j4rp4
      real*8 xh(nbod),yh(nbod),zh(nbod)
      real*8 vxh(nbod),vyh(nbod),vzh(nbod)

c...  Output
      real*8 energy,eltot(3),ke,pot

c...  Internals
      real*8 elx,ely,elz
      real*8 xx,yy,zz,rr2,oblpot,msys,irh(NTPMAX),ir3h(NTPMAX)
      real*8 xb(NTPMAX),yb(NTPMAX),zb(NTPMAX)       ! Used NTPMAX for symba
      real*8 vxb(NTPMAX),vyb(NTPMAX),vzb(NTPMAX)
      integer i,j

c----
c...  Executable code

      call coord_h2b(nbod,mass,xh,yh,zh,vxh,vyh,vzh,
     &           xb,yb,zb,vxb,vyb,vzb,msys)

      eltot(1)=(yb(nbod)*vzb(nbod)-zb(nbod)*vyb(nbod))*mass(nbod)
      eltot(2)=(zb(nbod)*vxb(nbod)-xb(nbod)*vzb(nbod))*mass(nbod)
      eltot(3)=(xb(nbod)*vyb(nbod)-yb(nbod)*vxb(nbod))*mass(nbod)

      ke = 0.5*mass(nbod)*(vxb(nbod)**2 + vyb(nbod)**2 + vzb(nbod)**2)
      pot= 0.d0

      do i = 1,nbod-1

         elx=(yb(i)*vzb(i)-zb(i)*vyb(i))*mass(i)
         ely=(zb(i)*vxb(i)-xb(i)*vzb(i))*mass(i)
         elz=(xb(i)*vyb(i)-yb(i)*vxb(i))*mass(i)
         eltot(1) = eltot(1) + elx
         eltot(2) = eltot(2) + ely
         eltot(3) = eltot(3) + elz

         ke = ke + 0.5*mass(i)*(vxb(i)**2 + vyb(i)**2 + vzb(i)**2)
         do j = i+1,nbod

	    xx = xb(i) - xb(j)
	    yy = yb(i) - yb(j)
	    zz = zb(i) - zb(j)
	    rr2 = xx**2 + yy**2 + zz**2
            if((mass(i).ne.0.0d0).and.(mass(j).ne.0.0d0)) then
               pot = pot - mass(i)*mass(j)/(sqrt(rr2))
            endif
         enddo
      enddo

      if(j2rp2.ne.0.0d0) then
         call getacch_ir3(nbod,2,xh,yh,zh,ir3h,irh)
         call obl_pot(nbod,mass,j2rp2,j4rp4,xh,yh,zh,irh,
     &                oblpot)
         pot = pot + oblpot
      endif

      energy = ke + pot

      return
      end      ! anal_energy
c-----------------------------------------------------------------------


module time_step_mod
  implicit none

  private

  public invoke_time_step

contains

  subroutine invoke_time_step(istp, ssha, ssha_u, ssha_v, &
                              sshn_t, sshn_u, sshn_v, &
                              hu, hv, ht, ua, va, un, vn)
    use kind_params_mod
    use dl_timer
    use field_mod
    use grid_mod
    use model_mod,       only: rdt, cbfr, visc
    use physical_params_mod, only: g, omega, d2r
!    use momentum_mod,    only: momentum_v_code
!    use momentum_mod,    only: momentum_u_code
!    use continuity_mod,  only: continuity_code
!    use time_update_mod, only: next_sshu_code, next_sshv_code
    use boundary_conditions_mod
    implicit none
    real(go_wp),        intent(in)    :: istp
    type(r2d_field), intent(inout) :: un, vn, sshn_t, sshn_u, sshn_v
    type(r2d_field), intent(inout) :: ua, va, ssha, ssha_u, ssha_v
    type(r2d_field), intent(in)    :: hu, hv, ht
    ! Locals
    integer :: ji, jj, jiu, jiv
    integer :: M, N
    integer :: cont_timer, mom_timer, bc_timer, next_timer
    ! Locals for momentum
    REAL(go_wp) :: u_e, u_w, v_n, v_s
    real(go_wp) :: v_nc, v_sc
    real(go_wp) :: depe, depw, deps, depn
    real(go_wp) :: hpg, adv, cor, vis
    real(go_wp) :: dudx_e, dudx_w, dudy_s, dudy_n
    real(go_wp) :: uu_e, uu_n, uu_s, uu_w
    real(go_wp) :: u_ec, u_wc, vv_e, vv_n, vv_s, vv_w
    real(go_wp) :: dvdx_e, dvdx_w, dvdy_n, dvdy_s
    real(go_wp) :: rtmp1, rtmp2, rtmp3, rtmp4
    ! end locals for momentum
    ! Locals for BCs
    real(go_wp) :: amp_tide, omega_tide, rtime

    M  = ssha%grid%subdomain%global%nx
    N  = ssha%grid%subdomain%global%ny

    ! In the general case we have to reason about whether or not the
    ! domain has PBCs and what sort of offset convention the kernels
    ! use. However, this is a middle layer specific to NEMOLite2D and
    ! therefore we know that we have no periodic BCs and are using a
    ! NE stagger
    !txstart = 2 ! grid%simulation_domain%xstart
    !tystart = 2 ! grid%simulation_domain%ystart

    !uxstart = 2 ! grid%simulation_domain%xstart
    !uxstop  = M - 1
    !uystart = 2 ! grid%simulation_domain%ystart
    !uystop  = N

    !vxstart = 2 ! grid%simulation_domain%xstart
    !vxstop  = M
    !vystart = 2 ! grid%simulation_domain%ystart
    !vystop  = N - 1

    !uwhole_xstart = 1 ! uxstart - NBOUNDARY
    !uwhole_xstop  = M ! uxstop  + NBOUNDARY
    !uwhole_ystart = 1 ! uystart - NBOUNDARY
    !uwhole_ystop  = N+1 ! uystop  + NBOUNDARY

    !vwhole_xstart = 1 ! vxstart - NBOUNDARY
    !vwhole_xstop  = M+1 ! vxstop  + NBOUNDARY
    !vwhole_ystart = 1 ! vystart - NBOUNDARY
    !vwhole_ystop  = N ! vystop  + NBOUNDARY

    call timer_start(cont_timer, label='Continuity')

!    do jj = ssha%internal%ystart, ssha%internal%ystop, 1
!      do ji = ssha%internal%xstart, ssha%internal%xstop, 1
    do jj = 2, N, 1
      do ji = 2, M, 1

!        call continuity_code(ji, jj,                             &
!                             ssha%data, sshn_t%data,             &
!                             sshn_u%data, sshn_v%data,           &
!                             hu%data, hv%data, un%data, vn%data, &
!                             sshn_t%grid%area_t)
         rtmp1 = (sshn_u%data(ji  ,jj ) + hu%data(ji  ,jj  ))*un%data(ji  ,jj)
         rtmp2 = (sshn_u%data(ji-1,jj ) + hu%data(ji-1,jj  ))*un%data(ji-1,jj)
         rtmp3 = (sshn_v%data(ji ,jj ) + hv%data(ji  ,jj  ))*vn%data(ji ,jj)
         rtmp4 = (sshn_v%data(ji ,jj-1) + hv%data(ji  ,jj-1))*vn%data(ji,jj-1)

         ssha%data(ji,jj) = sshn_t%data(ji,jj) + (rtmp2 - rtmp1 + rtmp4 - rtmp3) * &
                       rdt / sshn_t%grid%area_t(ji,jj)
      end do
    end do

    call timer_stop(cont_timer)

    call timer_start(mom_timer, label='Momentum')

!    do jj = ua%internal%ystart, ua%internal%ystop, 1
!      do ji = ua%internal%xstart, ua%internal%xstop, 1
!dir$ safe_address
    do jj = 2, N, 1
! SIMD
!dir$ vector always
      do ji = 2, M-1, 1

!!$        call momentum_u_code(ji, jj, &
!!$                             ua%data, un%data, vn%data, &
!!$                             hu%data, hv%data, ht%data, &
!!$                             ssha_u%data, sshn_t%data,  &
!!$                             sshn_u%data, sshn_v%data,  &
!!$                             un%grid%tmask,  &
!!$                             un%grid%dx_u,   &
!!$                             un%grid%dx_v,   &
!!$                             un%grid%dx_t,   &
!!$                             un%grid%dy_u,   &
!!$                             un%grid%dy_t,   &
!!$                             un%grid%area_u, &
!!$                             un%grid%gphiu)

    IF(un%grid%tmask(ji,jj) + un%grid%tmask(ji+1,jj) <= 0)  CYCLE   !jump over non-computational domain
    IF(un%grid%tmask(ji,jj) <= 0 .OR. un%grid%tmask(ji+1,jj) <= 0)  CYCLE !jump over boundary u

    u_e  = 0.5 * (un%data(ji,jj) + un%data(ji+1,jj)) * un%grid%dy_t(ji+1,jj)   !add length scale.
    depe = ht%data(ji+1,jj) + sshn_t%data(ji+1,jj)

    u_w  = 0.5 * (un%data(ji,jj) + un%data(ji-1,jj)) * un%grid%dy_t(ji,jj)     !add length scale
    depw = ht%data(ji,jj) + sshn_t%data(ji,jj)

    v_sc = 0.5_go_wp * (vn%data(ji,jj-1) + vn%data(ji+1,jj-1))
    v_s  = 0.5_go_wp * v_sc * (un%grid%dx_v(ji,jj-1) + un%grid%dx_v(ji+1,jj-1))
    deps = 0.5_go_wp * (hv%data(ji,jj-1) + sshn_v%data(ji,jj-1) + hv%data(ji+1,jj-1) + &
                     sshn_v%data(ji+1,jj-1))

    v_nc = 0.5_go_wp * (vn%data(ji,jj) + vn%data(ji+1,jj))
    v_n  = 0.5_go_wp * v_nc * (un%grid%dx_v(ji,jj) + un%grid%dx_v(ji+1,jj))
    depn = 0.5_go_wp * (hv%data(ji,jj) + sshn_v%data(ji,jj) + hv%data(ji+1,jj) + &
                     sshn_v%data(ji+1,jj))

    ! -advection (currently first order upwind)
    uu_w = (0.5_go_wp - SIGN(0.5_go_wp, u_w)) * un%data(ji,jj)              + &
         & (0.5_go_wp + SIGN(0.5_go_wp, u_w)) * un%data(ji-1,jj)
    uu_e = (0.5_go_wp + SIGN(0.5_go_wp, u_e)) * un%data(ji,jj)              + &
         & (0.5_go_wp - SIGN(0.5_go_wp, u_e)) * un%data(ji+1,jj)

    IF(un%grid%tmask(ji,jj-1) <=0 .OR. un%grid%tmask(ji+1,jj-1) <= 0) THEN
       uu_s = (0.5_go_wp - SIGN(0.5_go_wp, v_s)) * un%data(ji,jj)
    ELSE
       uu_s = (0.5_go_wp - SIGN(0.5_go_wp, v_s)) * un%data(ji,jj)              + &
            & (0.5_go_wp + SIGN(0.5_go_wp, v_s)) * un%data(ji,jj-1)
    END If

    IF(un%grid%tmask(ji,jj+1) <=0 .OR. un%grid%tmask(ji+1,jj+1) <= 0) THEN
       uu_n = (0.5_go_wp + SIGN(0.5_go_wp, v_n)) * un%data(ji,jj)
    ELSE
       uu_n = (0.5_go_wp + SIGN(0.5_go_wp, v_n)) * un%data(ji,jj)              + &
            & (0.5_go_wp - SIGN(0.5_go_wp, v_n)) * un%data(ji,jj+1)
    END IF

    adv = uu_w * u_w * depw - uu_e * u_e * depe + &
          uu_s * v_s * deps - uu_n * v_n * depn
    !end kernel u adv

    ! -viscosity

    !kernel  u vis
    dudx_e = (un%data(ji+1,jj) - un%data(ji,  jj)) / un%grid%dx_t(ji+1,jj) * &
             (ht%data(ji+1,jj) + sshn_t%data(ji+1,jj))
    dudx_w = (un%data(ji,  jj) - un%data(ji-1,jj)) / un%grid%dx_t(ji,  jj) * &
             (ht%data(ji,  jj) + sshn_t%data(ji,  jj))
    IF(un%grid%tmask(ji,jj-1) <=0 .OR. un%grid%tmask(ji+1,jj-1) <= 0) THEN
       dudy_s = 0.0_go_wp !slip boundary
    ELSE
       dudy_s = (un%data(ji,jj) - un%data(ji,jj-1)) / (un%grid%dy_u(ji,jj) + un%grid%dy_u(ji,jj-1)) * &
            & (hu%data(ji,jj) + sshn_u%data(ji,jj) + hu%data(ji,jj-1) + sshn_u%data(ji,jj-1))
    END IF

    IF(un%grid%tmask(ji,jj+1) <= 0 .OR. un%grid%tmask(ji+1,jj+1) <= 0) THEN
       dudy_n = 0.0_go_wp ! slip boundary
    ELSE
       dudy_n = (un%data(ji,jj+1) - un%data(ji,jj)) / (un%grid%dy_u(ji,jj) + un%grid%dy_u(ji,jj+1)) * &
            & (hu%data(ji,jj) + sshn_u%data(ji,jj) + hu%data(ji,jj+1) + sshn_u%data(ji,jj+1))
    END If

    vis = (dudx_e - dudx_w ) * un%grid%dy_u(ji,jj)  + &
         & (dudy_n - dudy_s ) * un%grid%dx_u(ji,jj) * 0.5_go_wp
    vis = visc * vis   !visc will be an array visc(1:jpijglou)
    !for variable viscosity, such as turbulent viscosity
    !End  kernel u vis

    ! -Coriolis' force (can be implemented implicitly)
    !kernel cor
    cor = 0.5_go_wp * (2._go_wp * omega * SIN(un%grid%gphiu(ji,jj) * d2r) * (v_sc + v_nc)) * &
         & un%grid%area_u(ji,jj) * (hu%data(ji,jj) + sshn_u%data(ji,jj))
    !end kernel cor

    ! -pressure gradient
    !start kernel hpg
    hpg = -g * (hu%data(ji,jj) + sshn_u%data(ji,jj)) * un%grid%dy_u(ji,jj) * &
           (sshn_t%data(ji+1,jj) - sshn_t%data(ji,jj))
    !end kernel hpg
    ! -linear bottom friction (implemented implicitly.
    !kernel ua calculation
    ua%data(ji,jj) = (un%data(ji,jj) * (hu%data(ji,jj) + sshn_u%data(ji,jj)) + rdt * &
                 (adv + vis + cor + hpg) / un%grid%area_u(ji,jj)) / &
                (hu%data(ji,jj) + ssha_u%data(ji,jj)) / (1.0_go_wp + cbfr * rdt)

      end do
    end do

!dir$ safe_address
    do jj = 2, N-1, 1
! SIMD
!dir$ vector always
      do ji = 2, M, 1

!!$        call momentum_v_code(ji, jj, &
!!$                             va%data, un%data, vn%data, &
!!$                             hu%data, hv%data, ht%data, &
!!$                             ssha_v%data, sshn_t%data,  &
!!$                             sshn_u%data, sshn_v%data,  &
!!$                             vn%grid%tmask,    &
!!$                             vn%grid%dx_v,     &
!!$                             vn%grid%dx_t,     &
!!$                             vn%grid%dy_u,     &
!!$                             vn%grid%dy_v,     &
!!$                             vn%grid%dy_t,     &
!!$                             vn%grid%area_v,   &
!!$                             vn%grid%gphiv)

    IF(vn%grid%tmask(ji,jj) + vn%grid%tmask(ji+1,jj) <= 0)  cycle !jump over non-computatinal domain
    IF(vn%grid%tmask(ji,jj) <= 0 .OR. vn%grid%tmask(ji,jj+1) <= 0) cycle !jump over v boundary cells

    ! kernel v adv
    v_n  = 0.5 * (vn%data(ji,jj) + vn%data(ji,jj+1)) * vn%grid%dx_t(ji,jj+1)  !add length scale.
    depn = ht%data(ji,jj+1) + sshn_t%data(ji,jj+1)

    v_s  = 0.5 * (vn%data(ji,jj) + vn%data(ji,jj-1)) * vn%grid%dx_t(ji,jj)    !add length scale
    deps = ht%data(ji,jj) + sshn_t%data(ji,jj)

    u_wc = 0.5_go_wp * (un%data(ji-1,jj) + un%data(ji-1,jj+1))
    u_w  = 0.5_go_wp * u_wc * (vn%grid%dy_u(ji-1,jj) + vn%grid%dy_u(ji-1,jj+1))
    depw = 0.50_go_wp * (hu%data(ji-1,jj) + sshn_u%data(ji-1,jj) + &
                      hu%data(ji-1,jj+1) + sshn_u%data(ji-1,jj+1))

    u_ec = 0.5_go_wp * (un%data(ji,jj) + un%data(ji,jj+1))
    u_e  = 0.5_go_wp * u_ec * (vn%grid%dy_u(ji,jj) + vn%grid%dy_u(ji,jj+1))
    depe = 0.50_go_wp * (hu%data(ji,jj) + sshn_u%data(ji,jj) + &
                      hu%data(ji,jj+1) + sshn_u%data(ji,jj+1))

    ! -advection (currently first order upwind)
    vv_s = (0.5_go_wp - SIGN(0.5_go_wp, v_s)) * vn%data(ji,jj)     + &
         & (0.5_go_wp + SIGN(0.5_go_wp, v_s)) * vn%data(ji,jj-1)
    vv_n = (0.5_go_wp + SIGN(0.5_go_wp, v_n)) * vn%data(ji,jj)     + &
         & (0.5_go_wp - SIGN(0.5_go_wp, v_n)) * vn%data(ji,jj+1)

    IF(vn%grid%tmask(ji-1,jj) <= 0 .OR. vn%grid%tmask(ji-1,jj+1) <= 0) THEN
       vv_w = (0.5_go_wp - SIGN(0.5_go_wp, u_w)) * vn%data(ji,jj)
    ELSE
       vv_w = (0.5_go_wp - SIGN(0.5_go_wp, u_w)) * vn%data(ji,jj)    + &
            & (0.5_go_wp + SIGN(0.5_go_wp, u_w)) * vn%data(ji-1,jj)
    END If

    IF(vn%grid%tmask(ji+1,jj) <= 0 .OR. vn%grid%tmask(ji+1,jj+1) <= 0) THEN
       vv_e = (0.5_go_wp + SIGN(0.5_go_wp, u_e)) * vn%data(ji,jj)
    ELSE
       vv_e = (0.5_go_wp + SIGN(0.5_go_wp, u_e)) * vn%data(ji,jj)  + &
              (0.5_go_wp - SIGN(0.5_go_wp, u_e)) * vn%data(ji+1,jj)
    END IF

    adv = vv_w * u_w * depw - vv_e * u_e * depe + &
          vv_s * v_s * deps - vv_n * v_n * depn

    !end kernel v adv

    ! -viscosity

    !kernel v dis
    dvdy_n = (vn%data(ji,jj+1) - vn%data(ji,  jj)) / vn%grid%dy_t(ji,jj+1) * &
                          (ht%data(ji,jj+1) + sshn_t%data(ji,jj+1))
    dvdy_s = (vn%data(ji,  jj) - vn%data(ji,jj-1)) / vn%grid%dy_t(ji,  jj) * &
                          (ht%data(ji,  jj) + sshn_t%data(ji,  jj))

    IF(vn%grid%tmask(ji-1,jj) <= 0 .OR. vn%grid%tmask(ji-1,jj+1) <= 0) THEN
       dvdx_w = 0.0_go_wp !slip boundary
    ELSE
       dvdx_w = (vn%data(ji,jj) - vn%data(ji-1,jj)) / &
                (vn%grid%dx_v(ji,jj) + vn%grid%dx_v(ji-1,jj)) * &
                (hv%data(ji,jj) + sshn_v%data(ji,jj) + hv%data(ji-1,jj) + sshn_v%data(ji-1,jj))
    END IF

    IF(vn%grid%tmask(ji+1,jj) <= 0 .OR. vn%grid%tmask(ji+1,jj+1) <= 0) THEN
       dvdx_e = 0.0_go_wp ! slip boundary
    ELSE
       dvdx_e = (vn%data(ji+1,jj) - vn%data(ji,jj)) / (vn%grid%dx_v(ji,jj) + vn%grid%dx_v(ji+1,jj)) * &
                  (hv%data(ji,jj) + sshn_v%data(ji,jj) + hv%data(ji+1,jj) + sshn_v%data(ji+1,jj))
    END If

    vis = (dvdy_n - dvdy_s ) * vn%grid%dx_v(ji,jj)  + &
          (dvdx_e - dvdx_w ) * vn%grid%dy_v(ji,jj) * 0.5_go_wp

    vis = visc * vis   !visc will be a array visc(1:jpijglou)
    !for variable viscosity, such as turbulent viscosity
    !end kernel v dis

    ! -Coriolis' force (can be implemented implicitly)
    !kernel v cor
    cor = -0.5_go_wp*(2._go_wp * omega * SIN(vn%grid%gphiv(ji,jj) * d2r) * (u_ec + u_wc)) * &
               vn%grid%area_v(ji,jj) * (hv%data(ji,jj) + sshn_v%data(ji,jj))
    !end kernel v cor

    ! -pressure gradient
    !kernel v hpg
    hpg = -g * (hv%data(ji,jj) + sshn_v%data(ji,jj)) * vn%grid%dx_v(ji,jj) * &
           (sshn_t%data(ji,jj+1) - sshn_t%data(ji,jj))
    !kernel v hpg

    ! -linear bottom friction (implemented implicitly.
    !kernel ua calculation
    va%data(ji,jj) = (vn%data(ji,jj) * (hv%data(ji,jj) + sshn_v%data(ji,jj)) + &
                 rdt * (adv + vis + cor + hpg) / vn%grid%area_v(ji,jj) ) / &
                 ((hv%data(ji,jj) + ssha_v%data(ji,jj))) / (1.0_go_wp + cbfr * rdt)

      end do
    end do

    call timer_stop(mom_timer)

    ! Apply open and solid boundary conditions

    call timer_start(bc_timer, label='BCs')

!    DO jj = ssha%internal%ystart, ssha%internal%ystop
!       DO ji = ssha%internal%xstart, ssha%internal%xstop
    DO jj = 2, N
! SIMD
       DO ji = 2, M
!          call bc_ssh_code(ji, jj, &
!                           istp, ssha%data, sshn_t%grid%tmask)

          amp_tide   = 0.2_go_wp
          omega_tide = 2.0_go_wp * 3.14159_go_wp / (12.42_go_wp * 3600._go_wp)
          rtime = istp * rdt

          if(sshn_t%grid%tmask(ji,jj) <= 0) cycle

          IF     (sshn_t%grid%tmask(ji,jj-1) < 0) THEN
             ssha%data(ji,jj) = amp_tide * sin(omega_tide * rtime)
          ELSE IF(sshn_t%grid%tmask(ji,jj+1) < 0) THEN
             ssha%data(ji,jj) = amp_tide * sin(omega_tide * rtime)
          ELSE IF(sshn_t%grid%tmask(ji+1,jj) < 0) THEN
             ssha%data(ji,jj) = amp_tide * sin(omega_tide * rtime)
          ELSE IF(sshn_t%grid%tmask(ji-1,jj) < 0) THEN
             ssha%data(ji,jj) = amp_tide * sin(omega_tide * rtime)
          END IF

       END DO
    END DO

!    do jj = uwhole_ystart, uwhole_ystop, 1
!       do ji = uwhole_xstart, uwhole_xstop, 1
!dir$ safe_address
    do jj = 1, N+1, 1
       do ji = 1, M, 1
!          call bc_solid_u_code(ji, jj, &
!                               ua%data, va%grid%tmask)

          if(sshn_t%grid%tmask(ji,jj) * sshn_t%grid%tmask(ji+1,jj) == 0)then
             ua%data(ji,jj) = 0._go_wp
          end if

       end do
    end do

!    DO jj = va%whole%ystart, va%whole%ystop, 1
!       DO ji = va%whole%xstart, va%whole%xstop, 1
!    do jj = vwhole_ystart, vwhole_ystop, 1
!       do ji = vwhole_xstart, vwhole_xstop, 1
!dir$ safe_address
    do jj = 1, N, 1
       do ji = 1, M+1, 1
!          call bc_solid_v_code(ji,jj, &
!                               va%data, ua%grid%tmask)
    if(sshn_t%grid%tmask(ji,jj) * sshn_t%grid%tmask(ji,jj+1) == 0)then
       va%data(ji,jj) = 0._go_wp
    end if

      end do
    end do

!    DO jj = uwhole_ystart, uwhole_ystop, 1
!       DO ji = uwhole_xstart, uwhole_xstop, 1
!dir$ safe_address
    DO jj = 1, N+1, 1
       DO ji = 1, M, 1
!          call bc_flather_u_code(ji,jj, &
!                                 ua%data, hu%data, sshn_u%data, &
!                                 sshn_u%grid%tmask)
          ! Check whether this point lies within the domain
          if(sshn_t%grid%tmask(ji,jj) + sshn_t%grid%tmask(ji+1,jj) <= -1) cycle

          if(sshn_t%grid%tmask(ji,jj) < 0) then
             jiu = ji + 1
             ua%data(ji,jj) = ua%data(jiu,jj) + sqrt(g/hu%data(ji,jj))* &
                  (sshn_u%data(ji,jj) - sshn_u%data(jiu,jj))
          else if(sshn_t%grid%tmask(ji+1,jj )< 0) then
             jiu = ji - 1
             ua%data(ji,jj) = ua%data(jiu,jj) + sqrt(g/hu%data(ji,jj)) * &
                  (sshn_u%data(ji,jj) - sshn_u%data(jiu,jj))
          end if
       END DO
    END DO

!    DO jj = va%whole%ystart, va%whole%ystop, 1
!       DO ji = va%whole%xstart, va%whole%xstop, 1
!     DO jj = vwhole_ystart, vwhole_ystop, 1
!       DO ji = vwhole_xstart, vwhole_xstop, 1
!dir$ safe_address
     DO jj = 1, N, 1
       DO ji = 1, M+1, 1
!          call bc_flather_v_code(ji,jj, &
!                                 va%data, hv%data, sshn_v%data, &
!                                 sshn_v%grid%tmask)
          IF(sshn_t%grid%tmask(ji,jj) + sshn_t%grid%tmask(ji,jj+1) <= -1) cycle

          IF(sshn_t%grid%tmask(ji,jj) < 0) THEN
             jiv = jj + 1
             va%data(ji,jj) = va%data(ji,jiv) + SQRT(g/hv%data(ji,jj)) * &
                  (sshn_v%data(ji,jj) - sshn_v%data(ji,jiv))
          ELSE IF(sshn_t%grid%tmask(ji,jj+1) < 0) THEN
             jiv = jj - 1
             va%data(ji,jj) = va%data(ji,jiv) + SQRT(g/hv%data(ji,jj)) * &
                  (sshn_v%data(ji,jj) - sshn_v%data(ji,jiv))
          END IF

       END DO
    END DO

    call timer_stop(bc_timer)

    ! Time update of fields

    call timer_start(next_timer, label='Next')

!    call copy_field(ua, un)
!    call copy_field(va, vn)
!    call copy_field(ssha, sshn_t)
    un%data = ua%data
    vn%data = va%data
    sshn_t%data = ssha%data

!dir$ safe_address
    do jj = 2, N, 1
!dir$ vector always
      do ji = 2, M-1, 1

!         call next_sshu_code(ji, jj, sshn_u%data, sshn_t%data, &
!                            sshn_t%grid%tmask,                 &
!                            sshn_t%grid%area_t, sshn_t%grid%area_u)

         if(sshn_t%grid%tmask(ji,jj) + &
            sshn_t%grid%tmask(ji+1,jj) <= 0) cycle !jump over non-computational domain

         IF(sshn_t%grid%tmask(ji,jj) * sshn_t%grid%tmask(ji+1,jj) > 0) THEN
            rtmp1 = sshn_t%grid%area_t(ji,jj) * sshn_t%data(ji,jj) + &
                 sshn_t%grid%area_t(ji+1,jj) * sshn_t%data(ji+1,jj)
            sshn_u%data(ji,jj) = 0.5_go_wp * rtmp1 / sshn_t%grid%area_u(ji,jj)
         ELSE IF(sshn_t%grid%tmask(ji,jj) <= 0) THEN
            sshn_u%data(ji,jj) = sshn_t%data(ji+1,jj)
         ELSE IF(sshn_t%grid%tmask(ji+1,jj) <= 0) THEN
            sshn_u%data(ji,jj) = sshn_t%data(ji,jj)
         END IF

      end do
    end do

!dir$ safe_address
    do jj = 2, N-1, 1
!dir$ vector always
      do ji = 2, M, 1

!        call next_sshv_code(ji, jj,                   &
!                            sshn_v%data, sshn_t%data, &
!                            sshn_t%grid%tmask,        &
!                            sshn_t%grid%area_t, sshn_t%grid%area_v)

         if(sshn_t%grid%tmask(ji,jj) + &
            sshn_t%grid%tmask(ji,jj+1) <= 0)  cycle !jump over non-computational domain
         if(sshn_t%grid%tmask(ji,jj) * sshn_t%grid%tmask(ji,jj+1) > 0) then
            rtmp1 = sshn_t%grid%area_t(ji,jj)*sshn_t%data(ji,jj) + &
                 sshn_t%grid%area_t(ji,jj+1) * sshn_t%data(ji,jj+1)
            sshn_v%data(ji,jj) = 0.5_go_wp * rtmp1 / sshn_t%grid%area_v(ji,jj)
         else if(sshn_t%grid%tmask(ji,jj) <= 0) then
            sshn_v%data(ji,jj) = sshn_t%data(ji,jj+1)
         else if(sshn_t%grid%tmask(ji,jj+1) <= 0) then
            sshn_v%data(ji,jj) = sshn_t%data(ji,jj)
         end if
      end do
    end do

    call timer_stop(next_timer)

  end subroutine invoke_time_step

end module time_step_mod


c
c Last update: Sep 29, 2007
c
************************************************************************
* Subroutine to compute the momenta p1,....,p6 of six particles of mass
* xm1,...,xm6 given a point in the 14 dimensional hypercube [0,1]^14
* Returns also xjac, the weight due to the jacobian.
*
* The phase space mapping corresponds to
*      [1t,a-->p6]+[5(rm1) --> 4(rm11)+p5 --> 3(rm111)+p4+p5
*                                         --> 2(rm1111,p1,p2)+p3+p4+p5]
*      ("t-channel process")
*   and
*      [5(rm1) --> 4(rm11)+p5 --> 3(rm111)+p4+p5
*                             --> 2(rm1111,p1,p2)+p3+p4+p5]
*      ("s-channel process")
*
* where rm1,rm11,rm111, rm1111 are the masses of the intermediate
*  resonances
*
* rm1,rm11,rm111 can resonate at top mass, rm1111 can resonate at W mass
*
* One exponent (expa) is available for mapping the t-channel propagator.
* In case of an "s-channel process", expa is set equal to 0 in order the
* mapping on the t-channel to be flat.
* rmta is the corresponding t-channel mass.
* This mapping is applied for 1.d0>cos(theta)>cos_sep. cos_sep is a
*   PARAMETER
* Outside this range the cosine is generated flat.
*
* If in the main x had dimension 15 with x(1) and x(2) fixing the
*  initial state particle momentum fraction the subrotine can be called
*  as call phsp1_5to1_4to31(x(3),shat,.......)
*
* INPUT:
*    x,shat        : total energy squared
*    xm(8),xmsq(8) : masses and masses squared of the particles
*                        in the order used in the main program
*    rmN,gamN      : N=1,11,111,1111 mass and width for intermediate
*                        resonance N.
*    spmNlo,spmNhi : N=1,11,111,1111 low and high limit of resonance
*                       mapping. outside this range the mapping is flat
*    pt6min_in(3)  : array of possible minimum pt's of particle 6,
*                    depending on its flavour. The first element refers
*                    to jets, the second one to charged leptons and the
*                    third one to neutrinos.
*    iorder(6)     : specifies the permutation that relates internal and
*                    external momenta: p = p(i,iorder(1))
*    id1,id2       : idp(iorder(1)),idp(iorder(2)) used to determine the
*                    direction of the incoming particles involved in
*                    the t-channel
*    id6           : idp(iorder(8)) used to determine which external
*                    particle
*
* OUTPUT:
*    p(0:3,8)      : momenta in the hard scattering CM.
*                        Includes initial momenta.
*    xjac          : full jacobian
*
* INTERNAL VARIABLES
* xmasq,xmbsq are the masses squared of the two incoming particles
* (a --> 6  in the t)
* The routine determines from iorder whether
* pa=sqrt(shat)/2*(1,0,0,1), pb=sqrt(shat)/2*(1,0,0,-1) or viceversa
* in the massless limit.
************************************************************************

      subroutine phsp1_5to1_4to31_multi_c(x,shat,
     &                         rm1,gam1,spm1lo,spm1hi,
     &                         rm11,gam11,spm11lo,spm11hi,
     &                         rm111,gam111,spm111lo,spm111hi,
     &                         rm1111,gam1111,spm1111lo,spm1111hi,
     &                         rmta,expa_in,spta,
     &                         pt6min_in,
     &                         id1,id2,id6,
     &                         xmasq_in,xmbsq_in,
     &                         xm,xmsq,iorder,
     &                         p,xjac)

      IMPLICIT NONE
      REAL*8 x(13),shat,
     &     rm1,gam1,spm1lo,spm1hi,
     &     rm11,gam11,spm11lo,spm11hi,
     &     rm111,gam111,spm111lo,spm111hi,
     &     rm1111,gam1111,spm1111lo,spm1111hi,
     &     rmta,expa,expa_in,
     &     pt6min_in(3),
     &     pt6min,
     &     xmasq_in,xmbsq_in,
     &     xm(8),xmsq(8),
     &     p(0:3,8),xjac
      INTEGER iorder(6),id1,id2,id6
* Local variables
      REAL*8 e_sup,e_inf,x_max,x_min,app,
     &       s1111,s111,s11,s1,
     &       e1111,e111,e11,e1,
     &       q1111(0:3),q111(0:3),q11(0:3),q1(0:3),
     &       p1111mod,p111mod,p11mod,p1mod,pmod,p0mod,
     &       t_inf,t_sup,t1,
     &       pi,twopi,fourpi,
     &       costh,sinth,ph,ran2,
     &       xmasq,xmbsq,rmtasq,
     &       signfactor,smallmass

      INTEGER idum,io1,io2,io3,io4,io5,io6,iaux,ischannel,i

      PARAMETER (pi=3.141592653589793238462643d0,twopi=2.d0*pi,
     &           fourpi=4.d0*pi,smallmass=1.d-3)

* Multimapping
      REAL*8 alfa1,alfa2,auxalfa2,
     &       xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max,
     &       vegas_x
* Multimapping t
      REAL*8 alfa,xt1_min,xt1_max,xt2_min,xt2_max,spta
      REAL*8 cos_sep
      PARAMETER(cos_sep=0.4d0)

      COMMON /pharand/ idum
c giuseppe 29/09/2007
      INTEGER maxnresonant
      PARAMETER (maxnresonant=4)
      INTEGER isresonant(maxnresonant),imapregion(maxnresonant)
      COMMON /iphsp_fxn/ isresonant,imapregion

      DO i=1,maxnresonant
         isresonant(i)=0
         imapregion(i)=0
      ENDDO
c end giuseppe 29/09/2007

      io1=iorder(1)
      io2=iorder(2)
      io3=iorder(3)
      io4=iorder(4)
      io5=iorder(5)
      io6=iorder(6)

c Determine which minimum pt must be assigned to t-channel, depending
c on final-state fermion's flavour
      IF(abs(id6).LE.5 .OR. id6.EQ.21)THEN
        pt6min=pt6min_in(1)
      ELSEIF(abs(id6).EQ.11)THEN
        pt6min=pt6min_in(2)
      ELSEIF(abs(id6).EQ.12)THEN
        pt6min=pt6min_in(3)
      ELSE
        print*,'***phsp1_5_to1_4to31 ERROR'
        STOP
      ENDIF

      ischannel=0

c In case of an s-channel process, the exponential mapping has to be
c suppressed. The following statement allows the input parameter
c (expa_in) NOT to be modified (thus avoiding possible undesired
c effects).
      expa=expa_in

* Determine which particle is paired with 6 in t-channel
      IF(id6.EQ.21)THEN   ! 6 is a  gluon. It couples to another gluon
        iaux=id6
        IF(id1.EQ.iaux)THEN
          xmasq=xmasq_in
          xmbsq=xmbsq_in
          signfactor=1.d0
        ELSEIF(id2.EQ.iaux)THEN
          xmasq=xmbsq_in
          xmbsq=xmasq_in
          signfactor=-1.d0
        ELSE
          PRINT*,'WRONG ASSIGNEMENT IN PHASE SPACE 1_5to1_4to31'
          STOP
        ENDIF
      ELSE                      ! 6 is a quark. Use rmta to find partner
        IF(rmta.LT.smallmass)THEN ! t channel boson is neutral
          iaux=-id6
          IF(id1.EQ.iaux)THEN
            xmasq=xmasq_in
            xmbsq=xmbsq_in
            signfactor=1.d0
          ELSEIF(id2.EQ.iaux)THEN
            xmasq=xmbsq_in
            xmbsq=xmasq_in
            signfactor=-1.d0
c Notice: when the phase space 1_5to1_4to31 is used to cover an
c s-channel process, the value of the parameter rmta in procini.f is
c assumed to be zero. This is the reason for the following check holds
c for rmta.LT.smallmass only.
          ELSEIF( (id1+id2).EQ.0 .OR. (id1+id2).EQ.1 .OR.
     &           (id1+id2).EQ.-1)THEN
            ischannel=1         ! s-channel process
          ELSE
            PRINT*,'WRONG ASSIGNEMENT IN PHASE SPACE 1_5to1_4to31'
            STOP
          ENDIF
        ELSE                    ! t channel boson is charged
	  IF(mod(abs(id6),2).EQ.0)THEN ! id6 even: 6 is up-type
	    IF(id6.GT.0)THEN    ! quark
              iaux=-(id6-1)
	    ELSE                ! antiquark
	      iaux=-(id6+1)
	    ENDIF
          ELSE                  ! id6 odd: 6 is down-type
	    IF(id6.GT.0)THEN    ! quark
              iaux=-(id6+1)
	    ELSE                ! antiquark
	      iaux=-(id6-1)
	    ENDIF
          ENDIF
          IF(id1.EQ.iaux)THEN
            xmasq=xmasq_in
            xmbsq=xmbsq_in
            signfactor=1.d0
          ELSEIF(id2.EQ.iaux)THEN
            xmasq=xmbsq_in
            xmbsq=xmasq_in
            signfactor=-1.d0
          ELSE
            PRINT*,'WRONG ASSIGNEMENT IN PHASE SPACE 1_5to1_4to31'
            STOP
	  ENDIF
        ENDIF                   !IF(rmta.LT.smallmass)
      ENDIF                     ! IF(id6.EQ.21)

      IF(ischannel.EQ.1)THEN
        xmasq=xmasq_in
        xmbsq=xmbsq_in
        signfactor=1.d0
* In case of s-channel process, set expa=0 in order to avoid mapping on
* the t-channel variable
        expa=0.d0
      ENDIF                     ! IF(ischannel.EQ.1)

      xjac=1.d0

* s1 = (q11+p5)**2 = q1**2
      e_sup = sqrt(shat)-xm(io6)
      e_inf = xm(io1)+xm(io2)+xm(io3)+xm(io4)+xm(io5)
      IF(e_sup.LE.e_inf)THEN
        xjac=0.d0
        RETURN
      ENDIF
      CALL PHSP_INI(e_sup,e_inf,
     &              spm1lo,spm1hi,rm1,gam1,
     &              alfa1,alfa2,auxalfa2,
     &              xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max)
      IF(x(1).LE.alfa1.AND.alfa1.GT.0.d0)THEN
        vegas_x=x(1)/alfa1
        e1=(xf1_max-xf1_min)*vegas_x+xf1_min
        s1=e1*e1
        xjac=xjac*(xf1_max-xf1_min)*2.d0*e1/alfa1
      ELSEIF(x(1).LE.alfa2.AND.alfa2.GT.0.d0)THEN
        vegas_x=(x(1)-alfa1)/(1.d0-alfa1)/auxalfa2
        x_max=xw1_max
        x_min=xw1_min
        s1=(gam1*rm1*tan((x_max-x_min)*vegas_x+x_min)+rm1**2)
        IF(s1.LE.0.d0)THEN
          xjac=0.d0
          RETURN
        ENDIF
        xjac=xjac*(x_max-x_min)*((s1-rm1**2)**2+
     &       (gam1*rm1)**2)/(gam1*rm1)/(1.d0-alfa1)/auxalfa2
c giuseppe 29/09/2007
        isresonant(1)=1
        imapregion(1)=2
c end giuseppe 29/09/2007
      ELSE
        vegas_x=(x(1)-alfa2)/(1.d0-alfa1)/(1.d0-auxalfa2)
        e1=(xf2_max-xf2_min)*vegas_x+xf2_min
        s1=e1*e1
        xjac=xjac*(xf2_max-xf2_min)*2.d0*e1
     &                  /(1.d0-alfa1)/(1.d0-auxalfa2)
      ENDIF
      xjac=xjac/twopi

* pa+pb --> p6 + q1; (pa-p6)**2 = (pb-q1)**2 = t1
      rmtasq=rmta*rmta
      app=((shat-s1-xmsq(io6))**2-4.d0*s1*xmsq(io6))/(4.d0*shat)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      pmod=sqrt(app)
      app=((shat-xmasq-xmbsq)**2-4.d0*xmasq*xmbsq)/(4.d0*shat)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p0mod=sqrt(app)

      IF(pt6min.LE.0.d0)THEN
        t_inf = xmbsq+s1-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                (shat+s1-xmsq(io6))
     &                -2.d0*p0mod*pmod
        app=(xmbsq-s1)*(xmasq-xmsq(io6))
     &                +(s1-xmbsq+xmasq-xmsq(io6))*
     &                (s1*xmasq-xmbsq*xmsq(io6))/shat
        t_sup = app/t_inf
      ELSE
        IF(pt6min.GE.pmod)THEN
          xjac=0.d0
          RETURN
        ENDIF
        app=sqrt((pmod-pt6min)*(pmod+pt6min))
        t_inf = xmbsq+s1-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                (shat+s1-xmsq(io6))
     &               -2.d0*p0mod*app
        t_sup = xmbsq+s1-0.5d0/shat*(shat+xmbsq-xmasq)*
     &                (shat+s1-xmsq(io6))
     &               +2.d0*p0mod*app
      ENDIF

      spta=xmbsq+s1-0.5d0/shat*(shat+xmbsq-xmasq)*
     &     (shat+s1-xmsq(io6))
     &     +2.d0*p0mod*pmod*cos_sep

* From now on we use t1 = (pa-p6)**2 - rmtasq = (pb-q1)**2 - rmtasq
      t_inf=t_inf-rmtasq
      t_sup=t_sup-rmtasq
      spta=spta-rmtasq
      CALL T_INI(t_inf,t_sup,spta,expa,
     &           alfa,xt1_min,xt1_max,xt2_min,xt2_max)
      IF(x(2).LE.alfa .AND. alfa.GT.0.d0)THEN
        vegas_x=x(2)/alfa
        t1=(xt1_max-xt1_min)*vegas_x+xt1_min
        xjac=xjac*0.5d0*(xt1_max-xt1_min)/p0mod/pmod
        xjac=xjac/alfa
      ELSE
        vegas_x=(x(2)-alfa)/(1.d0-alfa)
        IF(expa.eq.0.d0)THEN
          t1=(xt2_max-xt2_min)*vegas_x+xt2_min
          xjac=xjac*0.5d0*(xt2_max-xt2_min)/p0mod/pmod
        ELSEIF(expa.ne.1.d0)THEN
          t1=-((xt2_max-xt2_min)*vegas_x+xt2_min)**(1.d0/(1.d0-expa))
          xjac=xjac*0.5d0*(xt2_max-xt2_min)
     &         /(1.d0-expa)/p0mod/pmod*(-t1)**expa
        ELSE
          t1=-exp((xt2_max-xt2_min)*vegas_x+xt2_min)
          xjac=xjac*0.5d0*(xt2_max-xt2_min)/p0mod/pmod*(-t1)
        ENDIF
        xjac=xjac/(1.d0-alfa)
      ENDIF

      costh=0.5*(t1+rmtasq-xmbsq-s1
     &     +0.5d0/shat*(shat+xmbsq-xmasq)*(shat+s1-xmsq(io6)))
     &     /p0mod/pmod
      costh=costh*signfactor

* q1 and p6
      costh=min(costh,1.d0)
      costh=max(costh,-1.d0)
      sinth=sqrt(1.d0-costh*costh)
      ph=ran2(idum)*twopi
      p(0,io6)=sqrt(pmod*pmod+xmsq(io6))
      p(1,io6)=pmod*sinth*cos(ph)
      p(2,io6)=pmod*sinth*sin(ph)
      p(3,io6)=pmod*costh
      q1(0)=sqrt(pmod*pmod+s1)
      q1(1)=-p(1,io6)
      q1(2)=-p(2,io6)
      q1(3)=-p(3,io6)
* beta/2/(4*pi)**2
      xjac=xjac*pmod/sqrt(shat)/(fourpi)**2
      xjac=xjac*twopi

* s11 = (q111+p4)**2 = q11**2
      e_sup = sqrt(s1)-xm(io5)
      e_inf = xm(io1)+xm(io2)+xm(io3)+xm(io4)
      IF(e_sup.LE.e_inf)THEN
        xjac=0.d0
        RETURN
      ENDIF
      CALL PHSP_INI(e_sup,e_inf,
     &              spm11lo,spm11hi,rm11,gam11,
     &              alfa1,alfa2,auxalfa2,
     &              xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max)
      IF(x(3).LE.alfa1.AND.alfa1.GT.0.d0)THEN
        vegas_x=x(3)/alfa1
        e11=(xf1_max-xf1_min)*vegas_x+xf1_min
        s11=e11*e11
        xjac=xjac*(xf1_max-xf1_min)*2.d0*e11/alfa1
      ELSEIF(x(3).LE.alfa2.AND.alfa2.GT.0.d0)THEN
        vegas_x=(x(3)-alfa1)/(1.d0-alfa1)/auxalfa2
        x_max=xw1_max
        x_min=xw1_min
        s11=(gam11*rm11*tan((x_max-x_min)*vegas_x+x_min)+rm11**2)
        IF(s11.LE.0.d0)THEN
          xjac=0.d0
          RETURN
        ENDIF
        xjac=xjac*(x_max-x_min)*((s11-rm11**2)**2+
     &       (gam11*rm11)**2)/(gam11*rm11)/(1.d0-alfa1)/auxalfa2
c giuseppe 29/09/2007
        isresonant(2)=1
        imapregion(2)=2
c end giuseppe 29/09/2007
      ELSE
        vegas_x=(x(3)-alfa2)/(1.d0-alfa1)/(1.d0-auxalfa2)
        e11=(xf2_max-xf2_min)*vegas_x+xf2_min
        s11=e11*e11
        xjac=xjac*(xf2_max-xf2_min)*2.d0*e11
     &                  /(1.d0-alfa1)/(1.d0-auxalfa2)
      ENDIF
      xjac=xjac/twopi

* s111 = (q1111+p5)**2 = q111**2
      e_sup = sqrt(s11)-xm(io4)
      e_inf = xm(io1)+xm(io2)+xm(io3)
      IF(e_sup.LE.e_inf)THEN
        xjac=0.d0
        RETURN
      ENDIF
      CALL PHSP_INI(e_sup,e_inf,
     &              spm111lo,spm111hi,rm111,gam111,
     &              alfa1,alfa2,auxalfa2,
     &              xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max)
      IF(x(4).LE.alfa1.AND.alfa1.GT.0.d0)THEN
        vegas_x=x(4)/alfa1
        e111=(xf1_max-xf1_min)*vegas_x+xf1_min
        s111=e111*e111
        xjac=xjac*(xf1_max-xf1_min)*2.d0*e111/alfa1
      ELSEIF(x(4).LE.alfa2.AND.alfa2.GT.0.d0)THEN
        vegas_x=(x(4)-alfa1)/(1.d0-alfa1)/auxalfa2
        x_max=xw1_max
        x_min=xw1_min
        s111=(gam111*rm111*tan((x_max-x_min)*vegas_x+x_min)+rm111**2)
        IF(s111.LE.0.d0)THEN
          xjac=0.d0
          RETURN
        ENDIF
        xjac=xjac*(x_max-x_min)*((s111-rm111**2)**2+
     &       (gam111*rm111)**2)/(gam111*rm111)/(1.d0-alfa1)/auxalfa2
c giuseppe 29/09/2007
        isresonant(3)=1
        imapregion(3)=2
c end giuseppe 29/09/2007
      ELSE
        vegas_x=(x(4)-alfa2)/(1.d0-alfa1)/(1.d0-auxalfa2)
        e111=(xf2_max-xf2_min)*vegas_x+xf2_min
        s111=e111*e111
        xjac=xjac*(xf2_max-xf2_min)*2.d0*e111
     &                  /(1.d0-alfa1)/(1.d0-auxalfa2)
      ENDIF
      xjac=xjac/twopi

* s1111 = (p1+p2)**2 = q1111**2
      e_sup = sqrt(s111)-xm(io3)
      e_inf = xm(io1)+xm(io2)
      IF(e_sup.LE.e_inf)THEN
        xjac=0.d0
        RETURN
      ENDIF
      CALL PHSP_INI(e_sup,e_inf,
     &              spm1111lo,spm1111hi,rm1111,gam1111,
     &              alfa1,alfa2,auxalfa2,
     &              xf1_min,xf1_max,xw1_min,xw1_max,xf2_min,xf2_max)
      IF(x(5).LE.alfa1.AND.alfa1.GT.0.d0)THEN
        vegas_x=x(5)/alfa1
        e1111=(xf1_max-xf1_min)*vegas_x+xf1_min
        s1111=e1111*e1111
        xjac=xjac*(xf1_max-xf1_min)*2.d0*e1111/alfa1
      ELSEIF(x(5).LE.alfa2.AND.alfa2.GT.0.d0)THEN
        vegas_x=(x(5)-alfa1)/(1.d0-alfa1)/auxalfa2
        x_max=xw1_max
        x_min=xw1_min
        s1111=(gam1111*rm1111*tan((x_max-x_min)*vegas_x+x_min)
     &       +rm1111**2)
        IF(s1111.LE.0.d0)THEN
          xjac=0.d0
          RETURN
        ENDIF
        xjac=xjac*(x_max-x_min)*((s1111-rm1111**2)**2+
     &      (gam1111*rm1111)**2)/(gam1111*rm1111)/(1.d0-alfa1)/auxalfa2
c giuseppe 29/09/2007
        isresonant(4)=1
        imapregion(4)=2
c end giuseppe 29/09/2007
      ELSE
        vegas_x=(x(5)-alfa2)/(1.d0-alfa1)/(1.d0-auxalfa2)
        e1111=(xf2_max-xf2_min)*vegas_x+xf2_min
        s1111=e1111*e1111
        xjac=xjac*(xf2_max-xf2_min)*2.d0*e1111
     &                  /(1.d0-alfa1)/(1.d0-auxalfa2)
      ENDIF
      xjac=xjac/twopi

* q11 and p5
      app=((s1-s11-xmsq(io5))**2-4.d0*s11*xmsq(io5))/(4.d0*s1)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p1mod=sqrt(app)
      CALL TWOBODY(q1,s1,p1mod,s11,xmsq(io5),x(6),x(7),q11,p(0,io5))
* beta/2/(4*pi)**2
      xjac=xjac*p1mod/sqrt(s1)/(fourpi)**2
      xjac=xjac*fourpi

* q111 and p4
      app=((s11-s111-xmsq(io4))**2-4.d0*s111*xmsq(io4))/(4.d0*s11)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p11mod=sqrt(app)
      CALL TWOBODY(q11,s11,p11mod,s111,xmsq(io4),x(8),x(9),q111,
     &     p(0,io4))
* beta/2/(4*pi)**2
      xjac=xjac*p11mod/sqrt(s11)/(fourpi)**2
      xjac=xjac*fourpi

* q1111 and p3
      app=((s111-s1111-xmsq(io3))**2-4.d0*s1111*xmsq(io3))/(4.d0*s111)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p111mod=sqrt(app)
      CALL TWOBODY(q111,s111,p111mod,s1111,xmsq(io3),x(10),x(11),q1111,
     &     p(0,io3))
* beta/2/(4*pi)**2
      xjac=xjac*p111mod/sqrt(s111)/(fourpi)**2
      xjac=xjac*fourpi

* q1111 --> p1 and p2
      app=((s1111-xmsq(io1)-xmsq(io2))**2-4.d0*xmsq(io1)*xmsq(io2))
     &     /(4.d0*s1111)
      IF(app.LE.0.d0)THEN
        xjac=0.d0
        RETURN
      ENDIF
      p1111mod=sqrt(app)
      CALL TWOBODY(q1111,s1111,p1111mod,xmsq(io1),xmsq(io2),x(12),
     &     x(13),p(0,io1),p(0,io2))
* beta/2/(4*pi)**2
      xjac=xjac*p1111mod/sqrt(s1111)/(fourpi)**2
      xjac=xjac*fourpi

      RETURN
      END


      subroutine runCF_iii(i1,i2,i3,m0sq,Gr,Bzero3,N0)
C---  Expression for rearrangement of extension of Eq. 5.69
C---  Calculates Ciii, requires C00iii
C---  Small terms of order Gr(i,j)*Cijklm
C---  Denominator m0sq
      implicit none
      include 'TRconstants.f'
      include 'pvCnames.f'
      include 'pvCv.f'
      include 'Carraydef.f'
      include 'Carrays.f'
      include 'pvweenumber.f'
      integer ep,N0,i1,i2,i3,m,n,np
      parameter(np=2)
      double precision m0sq,Gr(np,np)
      double complex Bzero3(z3max,-2:0),bit,pole

      do ep=-2,0
      bit=czip
      do n=1,np
      do m=1,np
      bit=bit+Gr(n,m)*Cv(ciiiii(z5(n,m,i1,i2,i3))+N0,ep)
      enddo
      enddo

      pole=czip
      if (ep .gt. -2) pole=4d0*Cv(czziii(z3(i1,i2,i3))+N0,ep-1)

      Cv(ciii(z3(i1,i2,i3))+N0,ep)=
     . (20d0*Cv(czziii(z3(i1,i2,i3))+N0,ep)
     . -pole
     . -2d0*Bzero3(z3(i1,i2,i3),ep)
     . +bit)/(2d0*m0sq)

      enddo

      return
      end


      subroutine phase3m(r,p1,p2,p3,p4,p5,p6,p7,m3,m4,m5,wt)
c----generate phase space for 2-->3 process with masses m3,m4,m5
c----r(mxdim),p1(4),p2(4) are inputs
c----incoming p1 and p2 reversed in sign from physical values
c----i.e. phase space for -p1-p2 --> p3+p4+p5
c----with all 2 pi's (ie 1/(2*pi)^5)
c----(p4,p5) are dummies
      implicit none
      include 'constants.f'
      include 'mxdim.f'

      integer j
      double precision r(mxdim)
      double precision p1(4),p2(4),p3(4),p4(4),p5(4),p6(4),p7(4)
      double precision p12(4),p34(4),smin
      double precision wt,wt125,wt34,wt0,m3,m4,m5
      parameter(wt0=1d0/twopi)

      do j=1,4
      p12(j)=-p1(j)-p2(j)
      p6(j)=0d0
      p7(j)=0d0
      enddo
      smin=(m3+m4)**2

c---generate p5 and p34,
c---smin is the minimum inv mass of 34 system
c---m5 is the mass of p5
      call phi1_2m(m5,r(1),r(2),r(3),smin,p12,p5,p34,wt125,*99)

c---decay 34-system
      call phi3m(r(4),r(5),p34,p3,p4,m3,m4,wt34,*99)

      wt=wt0*wt125*wt34
      return
 99   continue
      wt=0d0
      return
      end


program hmipt_matsubara
  USE DMFT_IPT
  USE SCIFOR
  USE DMFT_TOOLS
  implicit none

  integer                :: i,ik,Lk,iloop,L
  logical                :: converged,bcsflag
  complex(8)             :: zeta,zeta1,zeta2,x1,x2,zsqrt
  real(8)                :: n,delta,D,cdet,delta0,Ds(2),alpha
  !
  complex(8),allocatable :: fg(:,:),fg0(:,:),sigma(:,:),calG(:,:),Gdet(:),calG_prev(:,:)
  !
  real(8),allocatable    :: wt(:),epsik(:),wm(:)

  call parse_input_variable(L,"L","inputIPT.conf",default=4096)
  call parse_input_variable(D,"wband","inputIPT.conf",default=1d0)
  call parse_input_variable(Lk,"Lk","inputIPT.conf",default=1000)
  call parse_input_variable(alpha,"alpha","inputIPT.conf",default=0.5d0)
  call parse_input_variable(bcsflag,"BCSFLAG","inputIPT.conf",default=.false.)
  call read_input("inputIPT.conf")

  allocate(wm(L))
  wm  = pi/beta*(2*arange(1,L)-1)

  allocate(fg(2,L))
  allocate(fg0(2,L),sigma(2,L),calG(2,L),Gdet(L),calG_prev(2,L))

  allocate(wt(Lk),epsik(Lk))
  call bethe_lattice(wt,epsik,Lk,D)

  call get_initial_sigma

  iloop=0 ; converged=.false.
  do while (.not.converged)
     iloop=iloop+1
     write(*,"(A,i5)",advance="no")"DMFT-loop",iloop

     fg=zero
     do i=1,L
        zeta =  xi*wm(i) + xmu - sigma(1,i)
        fg(:,i)=zero
        do ik=1,Lk
           cdet = abs(zeta-epsik(ik))**2 + (sigma(2,i))**2
           fg(1,i)=fg(1,i) + wt(ik)*(conjg(zeta)-epsik(ik))/cdet
           fg(2,i)=fg(2,i) - wt(ik)*sigma(2,i)/cdet
        enddo
     enddo

     n = ipt_measure_dens_matsubara(fg(1,:))
     delta = Uloc(1)*ipt_measure_phi_matsubara(fg(2,:))

     Ds(1) = get_ds1()
     Ds(2) = get_ds2()

     Gdet     = abs(fg(1,:))**2 + (fg(2,:))**2
     fg0(1,:) =  conjg(fg(1,:))/Gdet + sigma(1,:)
     fg0(2,:) =  fg(2,:)/Gdet        + sigma(2,:) + delta

     Gdet      =  abs(fg0(1,:))**2 + (fg0(2,:))**2
     calG(1,:) =  conjg(fg0(1,:))/Gdet
     calG(2,:) =  fg0(2,:)/Gdet

     write(*,"(5f14.9)",advance="no")2*n,delta,Ds(1),Ds(2)
     if(iloop>1)calG= alpha*calG + (1d0-alpha)*calG_prev
     calG_prev =  calG

     if(bcsflag)then
        sigma(2,:)=-delta ; sigma(1,:)=zero
     else
        sigma =  ipt_solve_matsubara(calG,delta)
     endif
     converged = check_convergence(calG(1,:)+calG(2,:),eps=dmft_error,N1=Nsuccess,N2=Nloop)

  enddo

  call splot("Sigma_iw.ipt",wm,sigma(1,:),append=.false.)
  call splot("Self_iw.ipt",wm,sigma(2,:),append=.false.)
  call splot("G_iw.ipt",wm,fg(1,:),append=.false.)
  call splot("F_iw.ipt",wm,fg(2,:),append=.false.)
  call splot("calG_iw.ipt",wm,calG(1,:),append=.false.)
  call splot("calF_iw.ipt",wm,calG(2,:),append=.false.)
  call splot("observables.ipt",uloc(1),n,delta,Ds(1),Ds(2),append=.false.)

  call get_sc_internal_energy

contains

  subroutine get_initial_sigma()
    logical :: check1,check2,check
    inquire(file="Sigma_iw.restart",exist=check1)
    inquire(file="Self_iw.restart",exist=check2)
    check=check1.AND.check2
    if(check)then
       write(*,*)"Reading Sigma in input:"
       call sread("Sigma_iw.restart",wm,sigma(1,:))
       call sread("Self_iw.restart",wm,sigma(2,:))
    else
       print*,"Using Hartree-Fock self-energy"
       print*,"===================================="
       n=0.5d0 ; delta=deltasc
       sigma(2,:)=-delta ; sigma(1,:)=zero
    endif
  end subroutine get_initial_sigma

  function get_ds1 result(Ds)
    integer    :: i,ik
    real(8)    :: Ds,nk,vk,Dssum,matssum,dens,chisum,Chij
    real(8)    :: Ekin
    complex(8) :: iw,gkw,fkw,g0kw,f0kw
    real(8)    :: Sigma_infty,S_infty,det,det_infty,csi,Ei,free
    !Get asymptotic self-energies
    Sigma_infty =   dreal(sigma(1,L))
    S_infty     =   dreal(sigma(2,L))
    !
    !1. Ds = 4/beta\sum_mats \int_ek N(ek)V(ek)F(ek,iw)F(ek,iw)
    !1.:
    !N(ek) = wtk(ik) (DOS)
    !V(ek) = (D**2 - ek**2)/3.d0
    Ds=0.d0                       ! superfluid stiffness (Bethe)
    do ik=1,Lk
       Dssum   = 0.d0
       nk = wt(ik)
       vk = (D**2-epsik(ik)**2)/3.d0
       do i=1,L
          iw       = xi*wm(i)
          det      = abs(iw+xmu-epsik(ik)-sigma(1,i))**2 + dreal(sigma(2,i))**2
          gkw = (-iw+xmu - epsik(ik) - conjg(sigma(1,i)) )/det
          fkw = -sigma(2,i)/det
          Dssum   = Dssum    +  fkw*fkw
       enddo
       Ds=Ds + 2d0*4d0/beta*nk*vk*Dssum
    enddo
  end function get_ds1

  function get_ds2 result(Ds)
    integer    :: i,ik
    real(8)    :: Ds,nk,vk,Dssum,matssum,dens,chisum,Chij
    real(8)    :: Ekin
    complex(8) :: zeta,g11kw,g22kw,fkw,g0kw,f0kw
    real(8)    :: Sigma_infty,S_infty,det,det_infty,csi,Ei,free
    !Get asymptotic self-energies
    Sigma_infty =   dreal(sigma(1,L))
    S_infty     =   dreal(sigma(2,L))
    !
    !2. Ds = -Ekin - \chi_jj(q=0,\omega=0)
    !
    !2.
    ! - get Ekin:
    Ekin=0d0
    Chij=0d0
    do ik=1,Lk
       vk    = (D**2-epsik(ik)**2)/3.d0
       csi   = epsik(ik)-(xmu-Sigma_infty)
       Ei    = dsqrt(csi**2 + S_infty**2)
       free  = 0.5d0*(1d0 - csi/Ei)*dtanh(0.5d0*beta*Ei)
       matssum=0d0
       chisum =0d0
       do i=1,L
          zeta      = xi*wm(i)+xmu-sigma(1,i)
          det       = abs(zeta-epsik(ik))**2 + dreal(sigma(2,i))**2
          g11kw     = conjg(zeta - epsik(ik))/det
          fkw       = -sigma(2,i)/det
          !
          det_infty = wm(i)**2 + (epsik(ik)-(xmu-Sigma_infty))**2 + S_infty**2
          g0kw      = (-xi*wm(i) - (epsik(ik)-(xmu-Sigma_infty)))/det_infty
          f0kw      =-S_infty/det_infty
          !
          matssum   =  matssum +  dreal(g11kw)-dreal(g0kw)
          chisum    =  chisum  + abs(g11kw)**2 + dreal(fkw)**2
       enddo
       nk    = 4d0/beta*matssum + 2d0*free
       chisum=-2d0/beta*chisum
       Ekin  = Ekin   + wt(ik)*nk*epsik(ik)
       Chij  = Chij   + wt(ik)*vk*chisum
    enddo
    Ds = -Ekin-Chij
  end function get_ds2

  subroutine get_sc_internal_energy
    integer    :: i,ik
    real(8)    :: matssum,fmatssum,checkP,checkdens,vertex,Dssum
    complex(8) :: iw,gkw,fkw,g0kw,f0kw
    real(8)    :: Epot,Etot,Eint,kin,kinsim,Ds,docc
    real(8)    :: Sigma_infty,S_infty,det,det_infty,csi,Ei,thermal_factor
    real(8)    :: free(Lk),Ffree(Lk),n_k(Lk)

    !Get asymptotic self-energies
    Sigma_infty =   real(sigma(1,L),8)
    S_infty     =   real(sigma(2,L),8)

    checkP=0.d0 ; checkdens=0.d0 ;          ! test variables

    kin=0.d0                      ! kinetic energy (generic)
    Ds=0.d0                       ! superfluid stiffness (Bethe)

    do ik=1,Lk

       csi            = epsik(ik)-(xmu-Sigma_infty)
       Ei             = dsqrt(csi**2 + S_infty**2)
       thermal_factor = dtanh(0.5d0*beta*Ei)
       free(ik)        = 0.5d0*(1.d0 - csi/Ei)*thermal_factor
       Ffree(ik)       =-(0.5d0*S_infty)/Ei*thermal_factor

       fmatssum= 0.d0
       matssum = 0.d0
       Dssum   = 0.d0

       vertex=(D**2-epsik(ik)**2)/3.d0

       do i=1,L
          iw       = xi*wm(i)
          det      = abs(iw+xmu-epsik(ik)-sigma(1,i))**2 + real(sigma(2,i),8)**2
          det_infty= wm(i)**2 + (epsik(ik)-(xmu-Sigma_infty))**2 + S_infty**2

          gkw = (-iw+xmu - epsik(ik) - conjg(sigma(1,i)) )/det
          fkw = -sigma(2,i)/det

          g0kw= (-iw - (epsik(ik)-(xmu-Sigma_infty)))/det_infty
          f0kw=-S_infty/det_infty

          matssum =  matssum +  real(gkw,8)-real(g0kw,8)
          !        matssum =matssum + real(gkw,8)        ! without tails corrections

          fmatssum= fmatssum +  real(fkw,8)-real(f0kw,8)
          Dssum   = Dssum    +  fkw*fkw

       enddo

       n_k(ik)   = 4.d0/beta*matssum + 2.d0*free(ik)
       !    n_k(ik)   = 4.d0/beta*matssum + 1.d0     ! without tails corrections
       checkP    = checkP    - wt(ik)*(2.d0/Beta*fmatssum+Ffree(ik))
       !    print*,checkP,Ffree(ik)
       checkdens = checkdens + wt(ik)*n_k(ik)
       kin    = kin    + wt(ik)*n_k(ik)*epsik(ik)
       Ds=Ds + 8.d0/beta* wt(ik)*vertex*Dssum
    enddo

    !  call splot("nk0_distribution.last",epsik,2.d0*free)
    !  call splot("fnk0_distribution.last",epsik,Ffree)

    kinsim=0
    kinsim = sum(fg(1,:)*fg(1,:)+conjg(fg(1,:))*conjg(fg(1,:))-2.d0*fg(2,:)*fg(2,:))*D**2/beta
    ! kinsim = kinsim - 1.d0/(2*pi*wm(L)) !check out where this term comes from??!!

    Epot=zero
    Epot = sum(fg(1,:)*sigma(1,:) + fg(2,:)*sigma(2,:))/beta*2.d0

    docc = n**2
    if(uloc(1) > 0.01d0)docc=-Epot/uloc(1) + n - 0.25d0

    Eint=kin+Epot

    ! Ds=zero
    ! Ds = sum(fg(2,:)*fg(2,:))/beta*2.d0

    ! kinsim=zero
    ! do i=1,L
    !    kinsim=kinsim + fg(1,i)*fg(1,i) + conjg(fg(1,i))*conjg(fg(1,i)) - 2.d0*fg(2,i)*fg(2,i)
    ! enddo
    ! kinsim=0.5d0*kinsim/Beta
    ! kinsim=kinsim- 1.d0/(2*pi*om(Iwmax2))

    write(*,*)'========================================='
    write(*,*)"Asymptotic Self-Energies",Sigma_infty, S_infty
    write(*,*)'========================================='
    write(*,*)"n,delta",n,delta
    write(*,*)"Dn% ,Ddelta%",(n-0.5d0*checkdens)/n,(delta + uloc*checkP)/delta ! u is positive
    write(*,*)'========================================='
    write(*,*)"Kinetic energy",kin
    write(*,*)'========================================='
    write(*,*)"double occupancy   =",docc
    write(*,*)'========================================='
    write(*,*) 'Kinetic Energy TEST (simple formula)'
    write(*,*) '###ACTHUNG: FOR BETHE ONLY####',kinsim
    write(*,*) 'Dkin%',(kin-kinsim)/kin
    write(*,*)'========================================='
    write(*,*) 'Superfluid stiffness',Ds
    write(*,*) 'Potential Energy U(n_up-1/2)(n_do-1/2)',Epot
    write(*,*) 'Internal Energy',Eint
    write(*,*)'========================================='
    call splot("nk_distribution.ipt",epsik,n_k,2.d0*free)
    call splot("thermodynamics.ipt",L,n,0.5d0*checkdens,kin,kinsim,docc,Ds,append=.true.)

    !  call splot("fnk0_distribution.ipt",epsik,Ffree)
    !   write(42,143)xmu,totdens,checkdens,dreal(kinsim),Tcheck,double
    ! 143 format(f6.4,5(1x,f12.9))
    return
  end subroutine get_sc_internal_energy

end program hmipt_matsubara