!****************************************************************
! MODULE mod_contour
!****************************************************************

MODULE mod_contour
use f90_kind
implicit none

type contour_level_curve_pos_type
  real(rkind)      :: x_indx                        ! real valued index of x-position
  real(rkind)      :: y_indx                        ! real valued index of y-position
  real(rkind)      :: r                             ! x-dimension (major radius)
  real(rkind)      :: z                             ! y-dimension (z)
end type contour_level_curve_pos_type

type contour_level_curve_type
  integer(ikind)   :: N_pos
  type(contour_level_curve_pos_type), dimension(:), allocatable :: pos
end type contour_level_curve_type

type contour_level_type
  integer(ikind)   :: N_curves                      ! number of curves found
  real(rkind)      :: amp                           ! level height
  type(contour_level_curve_type), dimension(:), allocatable :: curve
end type contour_level_type

type contour_type
  integer(ikind)   :: N_levels
  type(contour_level_type), dimension(:), allocatable :: level
end type contour_type

!private ::
public  :: contourgq,                   &
           contourgq_non_eqd,           &
           contouring,                  &
           contour_indx2rz,             &
           map_contour_indx_to_rz

CONTAINS

subroutine contourgq(ier, a, mx, nx, nz, lx, idim, &
                        ncsmax, h, xpl, zpl, ncs, npt)
! file=/afs/ipp/u/pem/libpem/Plotsource/contourgq.f

!                                  C O N T O U R G Q
! input:
!   a     - real matrix
!   mx    - leading dimension of matrix a
!   nx    - integer(ikind) ::, number of grid points in r-direction, nx > 3
!   nz    - integer(ikind) ::, number of grid points in z-direction, nz > 3
!   lx    - aux. integer(ikind) :: vector of length  idim
!   idim  - integer(ikind) ::, dimension of vectors  xpl, zpl, lx
!           (suggested: idim = 4*(nx+nz)
!   ncsmax- integer(ikind) ::, dimension of output vector npt
!   h     - real value for which contours in a are searched
! output:
!   ier   - integer(ikind) ::, error code. ier = 0  okay
!           ier = 1 : nx < 4  oder nz < 4
!           ier = 2 : idim insufficient
!           ier = 3 : ncsmax insufficient
!   xpl, zpl
!         - r-values of plot points, dimension idim
!           z-values of plot points, dimension idim
!   ncs   - integer(ikind) :: number of curves found
!   npt   - integer(ikind) :: vector, number of points for curves found
!
!                                                    revised aug.1995
      real(rkind),   intent(in) :: a(*), h
      integer(ikind),intent(in) :: mx, nx, nz, idim, ncsmax
      real(rkind),   intent(out):: xpl(*), zpl(*)
      integer(ikind),intent(out):: lx(*), npt(*)
      integer(ikind),intent(out):: ncs, ier

      integer(ikind) :: ja(6), ix, ixa, mxr, mxrt, mpl, mpllast, jx, jm, in, &
              jabs, kx, ikx, jfor, is, ie, k, i, j, instart, jnb, npl
      real(rkind) :: adn, a1, b1, dp, p1, p2

      if(nx < 4  .or. nz < 4) then
         ier=1
         return
      end if

      ier=0
      ix=0
      ixa=1
      mxr=mx*(nz-1)
      mxrt=mxr+mx
      ncs=0
      mpl=0
      mpllast=0

! find intersects contour line - grid lines: are stored in table lx
! 1st: look to the grid boundaries

      do jx=2,nx-1
         if((ix+(mxr-jx)/mx+1) > idim)then
           ier=2
           return
         endif
         do jm=jx,mxr,mx
            j=jm+mx
            if( ((a(j)-h)*(a(jm)-h))  <= 0.0d0) then
               ix=ix+1
!              if(ix > idim) goto 6000
               lx(ix)=-j
            end if
         end do
      end do

      do jx=2,nx
         if((ix+(mxr-jx-mx)/mx+1) > idim)then
           ier=2
           return
         endif
         do jm=jx+mx,mxr,mx
            if( ((a(jm)-h)*(a(jm-1)-h))  <= 0.0d0) then
               ix=ix+1
!              if(ix > idim) goto 6000
               lx(ix)=jm
            end if
         end do
      end do

      do jx=2,nx
         if((ix+(mxrt-jx)/mxr+1) > idim)then
           ier=2
           return
         endif
         do jm=jx,mxrt,mxr
            if( ((a(jm)-h)*(a(jm-1)-h))  <= 0.0d0) then
               ix=ix+1
!              if(ix > idim) goto 6000
               lx(ix)=jm
            end if
         end do
      end do

      do jx=1,nx,nx-1
         if((ix+(mxr-jx)/mx+1) > idim)then
           ier=2
           return
         endif
         do jm=jx,mxr,mx
            j=jm+mx
            if( ((a(j)-h)*(a(jm)-h))  <= 0.0d0) then
               ix=ix+1
!              if(ix > idim) goto 6000
               lx(ix)=-j
            end if
         end do
      end do

      if(ix < 2) return

! start a new curve

  do ! 108 continue
      if(ncs == ncsmax) then
         ier=3
         return
      end if
      ncs=ncs+1
      in=ix
      jx=lx(in)
      jfor=0
      instart=in
      npl=0

! continue curve

   30 continue
      jabs=iabs(jx)
      kx=(jabs-1)/mx+1
      ikx= jabs-mx*(kx-1)
      if(jx < 0) jnb=jabs-mx
      if(jx > 0) jnb=jabs-1

      if(mpl == idim)then
        ier=2
        return
      endif

      mpl=mpl+1
      npl=npl+1

! interpolation for point found
! linear interpolation at boundaries, else quadr. interpolation.
! note: for method used, check only for upper boundaries required.
      if(jx < 0) then
         if((jabs+mx) <= mxrt) then
            a1=(a(jabs+mx)-2.0d0*a(jabs)+a(jabs-mx))*2.0d0
            if(a1 /= 0.0d0) then
               b1=a(jabs+mx)-a(jabs-mx)
               dp=sqrt(b1**2 - 4.0d0*a1*(a(jabs)-h))
               p1= (dp-b1)/a1
               p2=-(dp+b1)/a1
               if(abs(p1+0.5d0) > abs(p2+0.5d0)) p1=p2
               zpl(mpl)=dble(kx-1)+p1
            else
               adn=a(jabs)-a(jabs-mx)
               if(adn /= 0.0d0) p1=-(a(jabs)-h)/adn
               if(adn == 0.0d0) p1=-0.5d0
               zpl(mpl)=dble(kx-1)+p1
            end if
         else
            adn=a(jabs)-a(jabs-mx)
            if(adn /= 0.0d0) p1=-(a(jabs)-h)/adn
            if(adn == 0.0d0) p1=-0.5d0
            zpl(mpl)=dble(kx-1)+p1
         end if
         xpl(mpl)=dble(ikx-1)
      else
         if(ikx < nx) then
            a1=(a(jabs+1)-2.0d0*a(jabs)+a(jabs-1))*2.0d0
            if(a1 /= 0.0d0) then
               b1=a(jabs+1)-a(jabs-1)
               dp=sqrt(b1**2 - 4.0d0*a1*(a(jabs)-h))
               p1= (dp-b1)/a1
               p2=-(dp+b1)/a1
               if(abs(p1+0.5d0) > abs(p2+0.5d0)) p1=p2
               xpl(mpl)=dble(ikx-1)+p1
            else
               adn=a(jabs)-a(jabs-1)
               if(adn /= 0.0d0) p1=-(a(jabs)-h)/adn
               if(adn == 0.0d0) p1=-0.5d0
               xpl(mpl)=dble(ikx-1)+p1
            end if
         else
            adn=a(jabs)-a(jabs-1)
            if(adn /= 0.0d0) p1=-(a(jabs)-h)/adn
            if(adn == 0.0d0) p1=-0.5d0
            xpl(mpl)=dble(ikx-1)+p1
         end if
         zpl(mpl)=dble(kx-1)
      end if

! look for next point

      is=1
      ie=6
      if(jx <= 0) then
         ja(1)=iabs(jx)
         ja(2)=-(iabs(jx)-1)
         ja(3)=ja(1)-mx

         ja(4)=-(iabs(jx)+1)
         ja(5)=iabs(jx)+1
         ja(6)=ja(5)-mx
         if(ikx == 1) is=4
         if(ikx == nx) ie=3
      else
         ja(1)=jx+mx
         ja(2)=-(jx+mx)
         ja(3)=-(iabs(ja(2))-1)
         ja(4)=-jx
         ja(5)=jx-mx
         ja(6)=-(jx-1)
         if(kx == 1) ie=3
         if(kx == nz) is=4
      end if
! make this to an internel function
      do  k=is,ie
         if(ja(k) /= jfor) then
            do i=ixa,ix
               if(lx(i) == ja(k)) then
                  if(jfor /= 0) lx(in)=0
                  jfor=jx
                  jx=lx(i)
                  in = i
                  go to 30
               end if
            end do
         end if
      end do
! hier exit
! hier 30 enddo
! curve found

      if(npl > 1) then
         npt(ncs)=npl
         mpllast=mpl
      else
         ncs=ncs-1
         mpl=mpllast
      end if

! remove used intersects from table lx and try to find another
! curve line for contour value

      if(in == instart) lx(instart)=0
      if(in > 0) lx(in)=0

!    6 continue
!      if(lx(ix) == 0) then
!         ix=ix-1
!         if(ix > ixa) goto 6
!      end if
     do
       if(lx(ix) /= 0) exit
       ix=ix-1
       if(ix <= ixa) exit
     enddo

!    7 continue
!      if(lx(ixa) == 0) then
!         ixa=ixa+1
!         if(ixa < ix) goto 7
!      end if
     do
       if(lx(ixa) /= 0) exit
       ixa=ixa+1
       if(ixa >= ix) exit
     enddo

!     if((ix-ixa+1) >= 2) goto 108
     if((ix-ixa+1) < 2) exit
   enddo ! 108

end subroutine contourgq

!*****************************************************************

subroutine contourgq_non_eqd(ier, a, xcoord, zcoord, mx, nx, nz, lx, idim, &
                        ncsmax, h, xpl, zpl, ncs, npt)
! file=/afs/ipp/u/pem/libpem/Plotsource/contourgq.f

!                                  C O N T O U R G Q
! input:
!   a     - real matrix -  read in as a vector
!   mx    - leading dimension of matrix a
!   nx    - integer(KIND=4) ::, number of grid points in r-direction, nx > 3
!   nz    - integer(KIND=4) ::, number of grid points in z-direction, nz > 3
!   lx    - aux. integer(KIND=4) :: vector of length  idim - stores the coordinates of the penetration points to be interpolated
!   idim  - integer(KIND=4) ::, dimension of vectors  xpl, zpl, lx
!           (suggested: idim = 4*(nx+nz)
!   ncsmax- integer(KIND=4) ::, dimension of output vector npt
!   h     - real value for which contours in a are searched
! output:
!   ier   - integer(KIND=4) ::, error code. ier = 0  okay
!           ier = 1 : nx < 4  oder nz < 4
!           ier = 2 : idim insufficient
!           ier = 3 : ncsmax insufficient
!   xpl, zpl
!         - r-values of plot points, dimension idim
!           z-values of plot points, dimension idim
!   ncs   - integer(KIND=4) ::, number of curves found
!   npt   - integer(KIND=4) :: vector, number of points for curves found
!
!                                                    revised aug.1995
! Minor revisions 20.12.12: S. Denk
      real(rkind), dimension(*),  intent(in) :: a
      real(rkind), dimension(:), intent(in) :: xcoord, zcoord
      real (rkind), intent(in)              :: h
      integer(ikind),intent(in) :: mx, nx, nz, idim, ncsmax
      real(rkind), dimension(:),  intent(out):: xpl, zpl
      integer(ikind), dimension(:), intent(out):: lx, npt
      integer(ikind),intent(out):: ncs, ier

      integer(ikind) :: pos_neighbour(6), & ! look up table to find the next point on the curve
                         pnt_cnt, & ! penetration points found so far, that are not yet interpolated -
                         !this is counted down when we start interpolating
                         lower_boundary, & ! index for lx, all points below this index are already interpolated
                         mxr, mxrt, pnt_int, & !counts the points interpolated so fa
                         pnt_int_last, & ! saves the pnt_int from the last curve - used to eliminate curves with just 1 point
                         jx, & ! used as coordinates in a - negative coordinates singal horizontal sign changes in a
                         jm, in, &
                         jabs, kx, ikx, jfor, is, ie, k, i, j, instart, jnb, npl, startcoord
      real(rkind) :: adn, &
                      a1, a2, a3, &! Newton interpolation coefficients
                      dp, p1, p2, x1,x2,x3
      logical :: found_neighbour

      if(nx < 4  .or. nz < 4) then
         ier=1
         return
      end if
      ier=0
      pnt_cnt=0
      lower_boundary=1
      mxr=mx*(nz-1)
      mxrt=mxr+mx
      ncs=0
      pnt_int=0
      pnt_int_last=0

! find intersects contour line - grid lines: are stored in table lx
! The order in which we store the points is important, since it is important
! to start all curves at the edges. Since we step backwards through the points found,
! the edges of the matrix are added last.
! First
! Saving points that are right of a horizontal sign change of a - h
! Disregarding the left and the right column
      do jx=2,nx-1 ! here jx is the x coordinate
         if((pnt_cnt+(mxr-jx)/mx+1) > idim)then
           ier=2
           return
         endif
         do jm=jx,mxr,mx ! jm is the x + z coordinate
            j=jm+mx
            if( ((a(j)-h)*(a(jm)-h))  <= 0.0d0) then
              !  if( (a(j)-h)*(a(jm)-h) == 0) print*, pnt_cnt + 1
               pnt_cnt=pnt_cnt+1
!              if(pnt_cnt > idim) goto 6000
               lx(pnt_cnt)=-j
            end if
         end do
      end do
! Second
! Saving points that are below a vertical sign change of a - h
! Disregarding the bottom and the top row
      do jx=2,nx
         if((pnt_cnt+(mxr-jx-mx)/mx+1) > idim)then
           ier=2
           return
         endif
         do jm=jx+mx,mxr,mx
            if( ((a(jm)-h)*(a(jm-1)-h))  <= 0.0d0) then
              !  if(  ((a(jm)-h)*(a(jm-1)-h)) == 0) print*, pnt_cnt + 1
               pnt_cnt=pnt_cnt+1
!              if(pnt_cnt > idim) goto 6000
               lx(pnt_cnt)=jm
            end if
         end do
      end do
! Third
! Now only tread the right and the left column (again horizontal)
      do jx=2,nx
         if((pnt_cnt+(mxrt-jx)/mxr+1) > idim)then
           ier=2
           return
         endif
         do jm=jx,mxrt,mxr
            if( ((a(jm)-h)*(a(jm-1)-h))  <= 0.0d0) then
               pnt_cnt=pnt_cnt+1
!              if(pnt_cnt > idim) goto 6000
               lx(pnt_cnt)=jm
            end if
         end do
      end do
! Four
! Now only tread the top and the bottom column (finally vertical)
      do jx=1,nx,nx-1
         if((pnt_cnt+(mxr-jx)/mx+1) > idim)then
           ier=2
           return
         endif
         do jm=jx,mxr,mx
            j=jm+mx
            if( ((a(j)-h)*(a(jm)-h))  <= 0.0d0) then
               pnt_cnt=pnt_cnt+1
!              if(pnt_cnt > idim) goto 6000
               lx(pnt_cnt)=-j
            end if
         end do
      end do

      if(pnt_cnt < 2) return
! start a new curve
    !print*, "found ", pnt_cnt, "points"
! Here we start interpolating
! We start with the last point we found. After that we follow the path of the curve.
! When the curve is finished we add the starting point once more, if the curve is a closed
! loop. Then we start a new curve. The first point of the new curve is the one that has the
! highest index and has not been interpolated yet.

   in=pnt_cnt
   jx=lx(in) ! here jx is the complete coordinate in the matrix
   startcoord = jx
   jfor=0
   instart=in
   npl=0

   do ! 108 continue
      if(ncs == ncsmax) then
         ier=3
         return
      end if
      ncs=ncs+1
      in=pnt_cnt
      jx=lx(in)
      startcoord = jx
      jfor=0
      instart=in
      npl=0
! continue curve

   30 continue
      jabs=iabs(jx)
      kx=(jabs-1)/mx+1
      ikx= jabs-mx*(kx-1)
      !if(jx < 0) jnb=jabs-mx
      !if(jx > 0) jnb=jabs-1

      if(pnt_int == idim)then
        ier=2
        return
      endif

      pnt_int=pnt_int+1
      npl=npl+1
! interpolation for point found
! linear interpolation at boundaries, else quadr. interpolation.
! note: for method used, check only for upper boundaries required.
      if(jx < 0) then
         x1 = zcoord(kx - 1) ! kx is the z + 1 coordinate
         x2 = zcoord(kx)
         a1 = a(jabs - mx) ! first newton interpolation coeff.
         a2 = (a(jabs) - a1) / (x2 - x1) ! second coeff.
         if((jabs+mx) <= mxrt) then !(only if not at the boarder)
         ! Note that we can never have a penetration point at the left boarder,
         ! since penetration points are always left of the saved point
            x3 = zcoord(kx + 1)
            a3 = ((a(jabs + mx) - a(jabs)) / (x3 - x2) - a2) &
                / ( x3 - x1)! third coeff.
            if(a3 /= 0.0d0) then
               dp =sqrt(a2 ** 2 + a3 *(-4 * a1 + 4 * h + a3 * (x1 - x2)**2) + &
                2 * a2 * a3 * (x1 - x2)) ! root in the
                  ! quadratic equation
               p1 = -(dp + a2 - a3 * x1 - a3 * x2)/(2 * a3) ! - solution
               p2 = (dp - a2 + a3 * x1 + a3 * x2)/(2 * a3) ! + solution
               if(p1 >= x1 .and. p1 <= x2) then ! in most cases only one of them
               ! is within the interval
               ! If both are, we arbitrarily take the first one
                  zpl(pnt_int) = p1
               else
                  zpl(pnt_int) = p2
               end if
            else
               ! linear interpolation if the curvature is zero
               if(a2 /= 0.0d0) p1 = (h - a1 + a2 * x1) / a2
               if(a2 == 0.0d0) p1 = x1 + (x2 - x1) * 0.5
               ! just the midpoint if the slope is also zero
               if(p1 /= 0.0d0) zpl(pnt_int) = p1
               if(p1 == 0.0d0) zpl(pnt_int) = x1 + (x2 - x1) * 0.5
            end if
         else
            if(a2 /= 0.0d0) p1 = (h - a1 + a2 * x1) / a2
            if(a2 == 0.0d0) p1 = x1 + (x2 - x1) * 0.5
            if(p1 /= 0.0d0) zpl(pnt_int) = p1
            if(p1 == 0.0d0) zpl(pnt_int) = x1 + (x2 - x1) * 0.5
         end if
         xpl(pnt_int) = xcoord(ikx)
      else
         x1 = xcoord(ikx - 1)
         x2 = xcoord(ikx)
         a1 = a(jabs - 1)
         a2 = (a(jabs) - a1) / (x2 - x1)
         if(ikx < nx) then
            x3 = xcoord(ikx + 1)
            a3 = ((a(jabs + 1) - a(jabs)) / (x3 - x2) - a2) &
                / ( x3 - x1)
            if(a3 /= 0.0d0) then
               dp =sqrt(a2 ** 2 + a3 *(-4 * a1 + 4 * h + a3 * (x1 - x2)**2) + &
                2 * a2 * a3 * (x1 - x2)) ! root in the
                  ! quadratic equation
               p1 = -(dp + a2 - a3 * x1 - a3 * x2)/(2 * a3)
               p2 = (dp - a2 + a3 * x1 + a3 * x2)/(2 * a3)
               if(p1 >= x1 .and. p1 <= x2) then
                  xpl(pnt_int) = p1
               else
                  xpl(pnt_int) = p2
               end if
            else
               if(a2 /= 0.0d0) p1 = (h - a1 + a2 * x1) / a2
               if(a2 == 0.0d0) p1 = x1 + (x2 - x1) * 0.5
               if(p1 /= 0.0d0) xpl(pnt_int) = p1
               if(p1 == 0.0d0) xpl(pnt_int) = x1 + (x2 - x1) * 0.5
            end if
         else
            if(a2 /= 0.0d0) p1 = (h - a1 + a2 * x1) / a2
            if(a2 == 0.0d0) p1 = x1 + (x2 - x1) * 0.5
            if(p1 /= 0.0d0) xpl(pnt_int) = p1
            if(p1 == 0.0d0) xpl(pnt_int) = x1 + (x2 - x1) * 0.5
         end if
         zpl(pnt_int) = zcoord(kx)
      end if
    !if( xpl(pnt_int) == 0.0d0 .or. zpl(pnt_int)== 0.0d0) print*, xcoord(ikx - 1), zcoord(kx - 1), &
    !ikx,kx, jx
    ! look for next point
! Here we create a look-up-table and then see if any of the points we found
! qualify as the next point within our curve.
! Here the order is important again.
! Since we have to test every point for the first direction first,
! before we can try another this step is the most expensive in this code
      found_neighbour = .false.
      is=1
      ie=6
      if(jx <= 0) then ! horizontal sign change
         pos_neighbour(1)=iabs(jx) ! base - vertical
         pos_neighbour(2)=-(iabs(jx)-1) ! top- horizontal
         pos_neighbour(3)=pos_neighbour(1)-mx ! left- vertical

         pos_neighbour(4)=-(iabs(jx)+1) ! bottom - horizontal
         pos_neighbour(5)=iabs(jx)+1 ! bottom - vertical
         pos_neighbour(6)=pos_neighbour(5)-mx ! bottom left - vertical
         if(ikx == 1) is=4
         if(ikx == nx) ie=3
      else ! vertical sign change
         pos_neighbour(1)=jx+mx ! right - vertical
         pos_neighbour(2)=-(jx+mx) ! right - horizontal
         pos_neighbour(3)=-(iabs(pos_neighbour(2))-1) ! right - top - horizontal
         pos_neighbour(4)=-jx ! base - horizontal
         pos_neighbour(5)=jx-mx ! left - vertical
         pos_neighbour(6)=-(jx-1) ! top - horizontal
         if(kx == 1) ie=3
         if(kx == nz) is=4
      end if
! make this to an internel function
      do  k=is,ie
        !if(pos_neighbour(k) /= jfor) then
            do i=lower_boundary,pnt_cnt
               if(lx(i) == pos_neighbour(k)) then
                  lx(in)=0
                  if(npl > 2) lx(instart) = startcoord
                  jfor=jx
                  jx=lx(i)
                  in = i
                  go to 30
               end if
            end do
         !end if
      end do
! hier exit
! hier 30 enddo
! curve found
      if(npl > 1) then
         npt(ncs)=npl
         pnt_int_last=pnt_int
      else
         !print*, "erased single point"
         ncs=ncs-1
         pnt_int=pnt_int_last
      end if

! remove used intersects from table lx and try to find another
! curve line for contour value

      if(in == instart) lx(instart)=0
      if(in > 0) lx(in)=0

!    6 continue
!      if(lx(pnt_cnt) == 0) then
!         pnt_cnt=pnt_cnt-1
!         if(pnt_cnt > lower_boundary) goto 6
!      end if
     do
       if(lx(pnt_cnt) /= 0) exit
       pnt_cnt=pnt_cnt-1
       if(pnt_cnt <= lower_boundary) exit
     enddo

!    7 continue
!      if(lx(lower_boundary) == 0) then
!         lower_boundary=lower_boundary+1
!         if(lower_boundary < pnt_cnt) goto 7
!      end if
     do
       if(lx(lower_boundary) /= 0) exit
       lower_boundary=lower_boundary+1
       if(lower_boundary >= pnt_cnt) exit
     enddo

!     if((pnt_cnt-lower_boundary+1) >= 2) goto 108
     if((pnt_cnt-lower_boundary+1) < 2) exit
   enddo ! 108

end subroutine contourgq_non_eqd

!*****************************************************************

subroutine contouring(A, h, contour, xcoord, zcoord)
! evaluates contour lines of A at amplitudes h(:)
! if present(xcoord, zcoord) contour lines in x/z coordinates
!   otherwise contour lines in real valued index coordinates (starts at 1 and ends at N)
use f90_kind
implicit none
real(rkind),    dimension(:,:), intent(in)    :: A          ! matrix
real(rkind),    dimension(:),   intent(in)    :: h          ! values for which contours in A are searched
type(contour_type),             intent(inout) :: contour
real(rkind),    dimension(:),   intent(in), optional :: xcoord, zcoord  ! coordinates of 1st(x) and 2nd(y) index of A
integer(ikind), parameter                  :: ncsmax = 20 ! dimension of output vector npt
integer(ikind), dimension(ncsmax)          :: npt         ! number of points for curves found
real(rkind),    dimension(:), allocatable  :: xpl, zpl    ! r- and z-values of plot points
integer(ikind), dimension(:), allocatable  :: lx          ! aux. vector of length idim
integer(ikind) :: mx   ! leading dimension of matrix A
integer(ikind) :: nx   ! number of grid points in r-direction, nx > 3
integer(ikind) :: nz   ! number of grid points in z-direction, nz > 3
integer(ikind) :: ier  ! error code. ier = 0  okay
                       !             ier = 1 : nx < 4  oder nz < 4
                       !             ier = 2 : idim insufficient
                       !             ier = 3 : ncsmax insufficient
integer(ikind) :: idim ! dimension of vectors  xpl, zpl, lx
integer(ikind) :: i, i1, i2, j, k

mx = size(A, dim=1)   ! first dimension of A
nx = mx               ! number of grid points in r-direction
nz = size(A, dim=2)   ! second dimension of A
!idim = 4*(nx+nz)      ! 2*(nx+nz) is not sufficient close to the separatrix
idim = 10*(nx+nz)      ! 2*(nx+nz) is not sufficient close to the separatrix
allocate(lx(idim), xpl(idim), zpl(idim))

! allocate structure contour
!---------------------------
contour%N_levels = size(h)
allocate(contour%level(contour%N_levels))

do k = 1, contour%N_levels
  contour%level(k)%amp = h(k)

  if (.not.present(xcoord)) then     ! contour lines as a function of real valued indices
    call contourgq(ier, A, mx, nx, nz, lx, idim, &
                   ncsmax, contour%level(k)%amp, xpl, zpl, contour%level(k)%N_curves, npt)
    xpl = xpl + 1   ! first indx is xpl=0
    zpl = zpl + 1

  else                               ! contour lines as a function of xcoord and zcoord
    if (.not.present(zcoord)) stop "sub contouring: either both or none of xcoord and zcoord should be present!"
    call contourgq_non_eqd(ier, A, xcoord, zcoord, mx, nx, nz, lx, idim, &
                           ncsmax, contour%level(k)%amp, xpl, zpl, contour%level(k)%N_curves, npt)
  endif

  allocate(contour%level(k)%curve( contour%level(k)%N_curves ))

  ! map: npt(:), xpl(:), zpl(:) to structure contour
  !---------------------------
  i1 = 1
  !print*,"jkasghdfkgahkjdhl",k,contour%level(k)%N_curves,npt(1:contour%level(k)%N_curves)
  do j = 1, contour%level(k)%N_curves
    contour%level(k)%curve(j)%N_pos         = npt(j)
    allocate(contour%level(k)%curve(j)%pos( contour%level(k)%curve(j)%N_pos ))
    i2                                      = i1 + contour%level(k)%curve(j)%N_pos - 1
    contour%level(k)%curve(j)%pos(:)%x_indx = xpl(i1:i2)
    contour%level(k)%curve(j)%pos(:)%y_indx = zpl(i1:i2)
    i1 = i2 + 1               ! next first index
  enddo

  !do i = 1, sum(npt(1:contour%level(k)%N_curves))
  !  write(6,'(i3,2f9.3)')i, xpl(i), zpl(i)
  !enddo
  !print*,h(k), maxval(A), minval(A)

  !write(6,'(a,i3)')"nuber of curves found =", contour%level(k)%N_curves
  !write(6,'(a,<contour%level(k)%N_curves>i5)')"nuber of points found =", npt(1:contour%level(k)%N_curves)
  !write(6,'(a,i3)')"contouring error =", ier

enddo ! k

deallocate(lx, xpl, zpl)

!stop "sub contouring"
end subroutine contouring

!*****************************************************************

subroutine contour_indx2rz(r_grid, z_grid, contour)
! evaluate for each contour point (r,z) from (x,y)_indx
use f90_kind
implicit none
real(rkind), dimension(:), intent(in)    :: r_grid, z_grid
type(contour_type),        intent(inout) :: contour
real(rkind)    :: dr, dz
integer(ikind) :: j, jr, jz, k, l

dr = r_grid(2) - r_grid(1)
dz = z_grid(2) - z_grid(1)

do l = 1, size(contour%level)
  do k = 1, contour%level(l)%N_curves
    do j = 1, size(contour%level(l)%curve(k)%pos)
      jr = INT(contour%level(l)%curve(k)%pos(j)%x_indx)
      jz = INT(contour%level(l)%curve(k)%pos(j)%y_indx)
      if (jr < 1 .or. jz < 1) then
        write(6,'(a,5i4,2f16.8)')"sub contour_indx2rz:",l, k, j, jr, jz, contour%level(l)%curve(k)%pos(j)%x_indx, contour%level(l)%curve(k)%pos(j)%y_indx
      endif

      contour%level(l)%curve(k)%pos(j)%r = r_grid(jr) + dr * (contour%level(l)%curve(k)%pos(j)%x_indx - jr)
      contour%level(l)%curve(k)%pos(j)%z = z_grid(jz) + dz * (contour%level(l)%curve(k)%pos(j)%y_indx - jz)
      !write(91,'(5i4,6f14.6)')l, k, j, jr, jz, contour%level(l)%curve(k)%pos(j)%r, contour%level(l)%curve(k)%pos(j)%z, &
      !    contour%level(l)%curve(k)%pos(j)%x_indx,  contour%level(l)%curve(k)%pos(j)%y_indx, &
      !    contour%level(l)%curve(k)%pos(j)%x_indx - jr,  contour%level(l)%curve(k)%pos(j)%y_indx - jz
    enddo
  enddo !k = 1, contour%level(l)%N_curves
enddo !l = 1, size(contour%level)

!stop "sub contour_indx2rz"
end subroutine contour_indx2rz

!*****************************************************************

subroutine map_contour_indx_to_rz(r_grid, z_grid, r_contour_point, z_contour_point, r, z)
use interpolation_routines,      only: linear_interpolation
implicit none
real(rkind), dimension(:),   intent(in)  :: r_grid, z_grid
real(rkind), dimension(:),   intent(in)  :: r_contour_point  ! r-values of points on contour line [real valued indx]
real(rkind), dimension(:),   intent(in)  :: z_contour_point  ! z-values of points on contour line [real valued indx]
real(rkind), dimension(:),   intent(out) :: r, z             ! [m] (r,z) coordinates of contour points
real(rkind)    :: krr, kzr
integer(ikind) :: i, kr, kz, Nr, Nz
integer(ikind) :: Nc             ! number of points on contour line

Nr = size(r_grid)                ! number of R points on grid
Nz = size(z_grid)                ! number of z points on grid
Nc = size(r_contour_point)       ! number of points on contour line

do i = 1, Nc
  kr  = INT(r_contour_point(i))
  kz  = INT(z_contour_point(i))
  krr = real(kr,kind(1.d0))       ! real version for interpolation
  kzr = real(kz,kind(1.d0))       ! real version for interpolation

  if (kr < 1 .or. kr > Nr-1) then
    write(6,'(a,i4,a,i4)')"kr =", kr, "   Nr-1 =", Nr-1
    stop "sub integration_on_closed_flux_surface: index kr not within [1, Nr-1]!"
  endif
  if (kz < 1 .or. kz > Nz-1) then
    write(6,'(a,i4,a,i4)')"kz =", kz, "   Nz-1 =", Nz-1
    stop "sub integration_on_closed_flux_surface: index kz not within [1, Nz-1]!"
  endif

  call linear_interpolation(krr, krr+1, r_grid(kr), r_grid(kr+1), r_contour_point(i), r(i))
  call linear_interpolation(kzr, kzr+1, z_grid(kz), z_grid(kz+1), z_contour_point(i), z(i))
  !write(6,'(i4,e12.4,i4,4e12.4)')i,r_contour_point(i),INT(r_contour_point(i)),r_grid(INT(r_contour_point(i))),r(i), r_grid(kr), r_grid(kr+1)
  !write(6,'(i4,e12.4,i4,4e12.4)')i,z_contour_point(i),INT(z_contour_point(i)),z_grid(INT(z_contour_point(i))),z(i), z_grid(kz), z_grid(kz+1)
enddo

!stop "sub map_contour_indx_to_rz"
end subroutine map_contour_indx_to_rz

!*****************************************************************

END MODULE mod_contour

!********************************************************************
!********************************************************************
!********************************************************************


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 FLUXBC (KIND,IN,VALUE,NGAUSS,GS,GSWT,X,Y,F,KINDBC)
C
C     ******************************************************************
C
C     SUBROUTINE TO CALCULATE CONSISTENT NODAL POINT FORCES AND FLUXES
C
C     ******************************************************************
C
      COMMON /PRBDAT/ IAXSYM,ITMDEP,IFORCE,IFREE,IVAR1,IVAR2,IPFUNC,
     1                IPNLTY,PNLTY
      COMMON /ELMDAT/ NNELM(6),NNCOR(6),NNMID(6),NNCTR(6),NSIDET(5),
     1                NSIDEQ(7),NNSIDE(6,4,3)
C
      DIMENSION GS(*), GSWT(*)
      DIMENSION X(*), Y(*), F(*)
      DIMENSION FF(3), FX(3), FY(3)
      DIMENSION XX(3), YY(3), INODE(3)
C
C     STATEMENT FUNCTION - ONE-DIMENSIONAL SHAPE FUNCTION
C
      FS(I,S)=(S**2-S)*.5*(I-2)*(I-3)*.5-(1.-S**2)*(I-1)*(I-3)+(S**2+S)*
     1.5*(I-1)*(I-2)*.5
C
C     ******************************************************************
C
C     LOCATE DATA FOR ELEMENT SIDE
C
      NN=NNELM(KIND)
      DO 10 J=1,3
      INODE(J)=NNSIDE(KIND,IN,J)
   10 CONTINUE
      DO 20 J=1,3
      I=INODE(J)
      XX(J)=X(I)
      YY(J)=Y(I)
   20 CONTINUE
C
      IF (KINDBC.GT.0) GO TO 110
      GO TO (30, 70, 30, 70, 30, 70), KIND
C
C     NODAL POINT FORCES - SUBPARAMETRIC ELEMENT
C
   30 CONTINUE
      XDIFF=(XX(3)-XX(1))*.5
      YDIFF=(YY(3)-YY(1))*.5
      DO 40 I=1,3
      FF(I)=0.
      DO 40 J=1,NGAUSS
      S=GS(J)
      WW=GSWT(J)
      RAD=(1.-S)*.5*XX(1)+(1.+S)*.5*XX(3)
      IF (IAXSYM.EQ.0) RAD=1.0
      B=FS(I,S)
      FF(I)=FF(I)+WW*RAD*B
   40 CONTINUE
      IF (KINDBC.EQ.-1) THEN
C
C     NORMAL STRESSES
C
      DO 50 J=1,3
      I=INODE(J)
      F(I)=F(I)-FF(J)*YDIFF*VALUE
      F(I+NN)=F(I+NN)+FF(J)*XDIFF*VALUE
   50 CONTINUE
      RETURN
       ELSE
C
C     TANGENTIAL STRESSES
C
      DO 60 J=1,3
      I=INODE(J)
      F(I)=F(I)+FF(J)*XDIFF*VALUE
      F(I+NN)=F(I+NN)-FF(J)*YDIFF*VALUE
   60 CONTINUE
      RETURN
      END IF
C
C     NODAL POINT FORCES - ISOPARAMETRIC ELEMENT
C
   70 CONTINUE
      DO 80 I=1,3
      FX(I)=0.
      FY(I)=0.
      DO 80 J=1,NGAUSS
      S=GS(J)
      WW=GSWT(J)
      RAD=(S**2-S)*.5*XX(1)+(1.-S**2)*XX(2)+(S**2+S)*.5*XX(3)
      IF (IAXSYM.EQ.0) RAD=1.0
      B=FS(I,S)
      C=(2.*S-1.)*.5*XX(1)+(-2.*S)*XX(2)+(2.*S+1.)*.5*XX(3)
      D=(2.*S-1.)*.5*YY(1)+(-2.*S)*YY(2)+(2.*S+1.)*.5*YY(3)
      FX(I)=FX(I)-WW*RAD*B*D
      FY(I)=FY(I)+WW*RAD*B*C
   80 CONTINUE
      IF (KINDBC.EQ.-1) THEN
C
C     NORMAL STRESSES
C
      DO 90 J=1,3
      I=INODE(J)
      F(I)=F(I)+FX(J)*VALUE
      F(I+NN)=F(I+NN)+FY(J)*VALUE
   90 CONTINUE
      RETURN
       ELSE
C
C     TANGENTIAL STRESSES
C
      DO 100 J=1,3
      I=INODE(J)
      F(I)=F(I)+FY(J)*VALUE
      F(I+NN)=F(I+NN)+FX(J)*VALUE
  100 CONTINUE
      RETURN
      END IF
C
  110 CONTINUE
      GO TO (120, 140, 120, 140, 120, 140), KIND
C
C     NODAL POINT FLUXES - SUBPARAMETRIC ELEMENT
C
  120 CONTINUE
      XDIFF=(XX(3)-XX(1))*.5
      YDIFF=(YY(3)-YY(1))*.5
      DELTA=SQRT(XDIFF**2+YDIFF**2)
      IF (IAXSYM.EQ.0) THEN
      FX(1)=DELTA/3.
      FX(2)=DELTA*4./3.
      FX(3)=DELTA/3.
       ELSE
      FX(1)=DELTA*XX(1)/3.
      FX(2)=DELTA*(XX(1)+XX(3))*2./3.
      FX(3)=DELTA*XX(3)/3.
      END IF
      DO 130 J=1,3
      I=INODE(J)
      F(I)=F(I)+VALUE*FX(J)
  130 CONTINUE
      RETURN
C
C     NODAL POINT FLUXES - ISOPARAMETRIC ELEMENT
C
  140 CONTINUE
      DO 150 I=1,3
      FF(I)=0.
      DO 150 J=1,NGAUSS
      S=GS(J)
      WW=GSWT(J)
      RAD=(S**2-S)*.5*XX(1)+(1.-S**2)*XX(2)+(S**2+S)*.5*XX(3)
      IF (IAXSYM.EQ.0) RAD=1.0
      B=FS(I,S)
      C=(2.*S-1.)*.5*XX(1)+(-2.*S)*XX(2)+(2.*S+1.)*.5*XX(3)
      D=(2.*S-1.)*.5*YY(1)+(-2.*S)*YY(2)+(2.*S+1.)*.5*YY(3)
      DELTA=SQRT(C**2+D**2)
      FF(I)=FF(I)+WW*RAD*B*DELTA
  150 CONTINUE
      DO 160 J=1,3
      I=INODE(J)
      F(I)=F(I)+VALUE*FF(J)
  160 CONTINUE
C
      RETURN
      END


program test
    use integrators
    implicit none
    integer, parameter :: n = 2
    real :: q = 1e-3
    real, dimension(n) :: masses
    real, dimension(n, 3) :: positions, velocities, poses, vels, poses_low, &
            vels_low, poses_high, vels_high, accels
    real :: dt
    real :: x
    masses = (/1., 0./)
    positions = reshape((/-q, 0., 0., 1. - q, 0., 0./), shape(positions))
    velocities = reshape((/0., -q, 0., 0., 1. - q, 0./), shape(velocities))
    dt = 1e-2
    print *, velocities
    call euler_forward(2, masses, positions, velocities, dt, poses, vels, accels)
    print *, "Euler forward:"
    print *, poses
    print *, vels

end program test


      subroutine phi1_2bw(x1,x2,x3,x4,p1,p2,p3,bwmass,bwwidth,wt,*)
c     massive particle p1 decaying into p2 mass m2 and p3 mass m3.
c     with invariant mass of particle two s2 and particle three s3 integrated over.
c
c     Modified version of phi1_2 such that p2 decays using a BW form with
c     parameters given in the arguments above, and p3 decays without BW
c
c     vectors returned p2 and p3 are in the same frame as p1 is supplied
c     Expression evaluated is
c     ds2 ds3 d^4 p2 d^4 p3 (2 pi)^4 delta(p1-p2-p3)/(2 pi)^6
c     delta(p2^2-s2) delta(p3^2-s3)
c
      implicit none
      include 'constants.f'
      include 'heavyflav.f'
      include 'masses.f'
      include 'process.f'
      include 'zerowidth.f'
      include 'verbose.f'
      include 'breit.f'
      include 'limits.f'
      include 'dm_params.f'
      include 'first.f'
      double precision p1(4),p2(4),p3(4),p3cm(4)
      double precision x1,x2,x3,x4,costh,sinth,phi,cphi,sphi
      double precision wt,wt0,w2,w3
      double precision s2max,s2min,s3max,s3min
      double precision m1,m2,s1,s2,s3,lambda,bwmass,bwwidth
      integer j
      logical oldzerowidth
      common/lambda/lambda,s1,s2,s3
      parameter(wt0=one/8d0/pi)
!$omp threadprivate(/lambda/)

      if (verbose) then
        if(first) then
        write(6,*) 'phase sp. using phi1_2bw: mass,width',bwmass,bwwidth
        first=.false.
        endif
      endif

      wt=0d0
      s1=p1(4)**2-p1(1)**2-p1(2)**2-p1(3)**2
      if (s1 .lt. 0d0) return 1
      m1=dsqrt(s1)

      if (m1 .lt. 1d-8) return 1 ! for safety

      s2min=1d-8
      s2max=s1

c--- adjust limits to match input file if it is more restrictive
      s2min=max(s2min,m3456min**2)
      s2max=min(s2max,m3456max**2)
      if (s2max .lt. s2min)  return 1 ! for safety

      call breitw(x1,s2min,s2max,bwmass,bwwidth,s2,w2)

      m2=dsqrt(s2)
      s3min=1d-8
      s3max=(m2-m1)**2

      if (s3max .lt. s3min) return 1 ! for safety

      w3=s3max-s3min
      s3=s3max*x2+s3min*(1d0-x2)

      costh=two*x3-one
      phi=twopi*x4
      sinth=dsqrt(one-costh**2)
      cphi=dcos(phi)
      sphi=dsin(phi)
      lambda=((s1-s2-s3)**2-4d0*s2*s3)

      if (lambda .lt. 0d0) then
        return 1
      endif

      lambda=dsqrt(lambda)
      wt=wt0*w2*w3*lambda/s1

      p3cm(4)=m1/two*(s1+s3-s2)/s1
      p3cm(1)=m1/two*lambda/s1*sinth*sphi
      p3cm(2)=m1/two*lambda/s1*sinth*cphi
      p3cm(3)=m1/two*lambda/s1*costh
      call boost(m1,p1,p3cm,p3)
      do j=1,4
      p2(j)=p1(j)-p3(j)
      enddo
      if (  (p1(4) .lt. 0d0)
     & .or. (p2(4) .lt. 0d0)
     & .or. (p3(4) .lt. 0d0)) then
       if (case(1:5) .ne. 'vlchk') then
        write(6,*) '   m1=',m1
        write(6,*) 's2min=',s2min
        write(6,*) 's2max=',s2max
        write(6,*) 's3min=',s3min
        write(6,*) 's3max=',s3max
        write(6,*) 'p1',p1(4),p1(4)**2-p1(1)**2-p1(2)**2-p1(3)**2,s1
        write(6,*) 'p2',p2(4),p2(4)**2-p2(1)**2-p2(2)**2-p2(3)**2,s2
        write(6,*) 'p3',p3(4),p3(4)**2-p3(1)**2-p3(2)**2-p3(3)**2,s3
        write(6,*) 'n2,n3',n2,n3
        write(6,*) 'in phi1_2bw.f'
       endif
       return 1
      endif

      return
      end


!~ ==========================================================

        !~ Hamiltonian of XS2    VII version

!~ ==========================================================
subroutine Hamiltonianofxs2VII_DATA ( neigenvalues,  XK, Yk, tdp, tdd, tpp,Delta0,Delta1,Delta2,DeltaP,DeltaZ, Vonset, eig, vec)
use moduleone
Implicit real (kind=DP) (a-h,o-z)
!----------------------------------------------------------------------------------------------------------------------
        !!~ integer(kind=4):: Numlayers
        !!~ integer(kind=4):: neigenvalues=Numlayers*11
        integer(kind=4):: neigenvalues
        real(kind=DP),dimension(0:neigenvalues-1):: eig
        complex(kind=DP):: vec(0:neigenvalues-1,0:neigenvalues-1)
        complex(kind=DP):: H(0:(neigenvalues*(neigenvalues+1))/2-1)
        complex(kind=DP):: cwork(0:2*neigenvalues-1)
        real(kind=DP):: aux(0:7*neigenvalues-1)
        integer:: iwork(0:5*neigenvalues-1),ifail(0:neigenvalues-1)
        real(kind=DP),parameter:: abstol=2*tiny(abstol)  ! see ZHPEVX

        !!! =========  Define more parameters     =====
        integer(kind=4):: NdimensionVonset,  lneigenvalues
        real(kind=DP):: XK, Yk
        real(kind=DP):: tdp(1:3,1:5,1:6),tdd(1:5,1:5,1:6),tpp(1:3,1:3,1:10)
        real(kind=DP)::Delta0,Delta1,Delta2,DeltaP,DeltaZ
        real(kind=DP):: v12, v23
        real(kind=DP), dimension ( 1:3, 1:(neigenvalues/11) ):: Vonset    !~! Allocate(  Vonset  ( 1:3, Nlayer  )     )

        complex(kind=DP), dimension(neigenvalues,neigenvalues)::Hamiltonian, Hamiltonian0, Hamiltonian1

        complex(kind=DP), dimension(9:11, 12:14)::  Hamiltonian1inter,  Hamiltonian0inter

        complex(kind=DP)::Expon_KRalpha, Expon_KRbeta,Expon_KRgemma,  phase

!======================================================================================

!~ CALL MKL_SET_NUM_THREADS(1)
!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
!      Hamiltonian0
!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xRalpha   =   -Vec2X        ;      yRalpha    =   -Vec2Y
xRbeta    =    Vec1X+Vec2X  ;      yRbeta     =    Vec1Y+Vec2Y
xRgemma   =   -Vec1X        ;      yRgemma    =   -Vec1Y

CS_KRalpha      =   COS(xk*xRalpha+yk*yRalpha)
CS_KRbeta       =   COS(xk*xRbeta+yk*yRbeta)
CS_KRgemma      =   COS(xk*xRgemma+yk*yRgemma)

!!%%%%%%%%%%%%%%        ST-----ST            %%%%%%%%%%%%%%%%

Hamiltonian0(1,1)=2.0_DP*( tpp(1,1,6)*CS_KRalpha+tpp(1,1,4)*CS_KRbeta+tpp(1,1,2)*CS_KRgemma )+DeltaP  !+v23
Hamiltonian0(1,2)=2.0_DP*( tpp(1,2,6)*CS_KRalpha+tpp(1,2,4)*CS_KRbeta+tpp(1,2,2)*CS_KRgemma )
Hamiltonian0(1,3)=2.0_DP*( tpp(1,3,6)*CS_KRalpha+tpp(1,3,4)*CS_KRbeta+tpp(1,3,2)*CS_KRgemma )

Hamiltonian0(2,2)=2.0_DP*( tpp(2,2,6)*CS_KRalpha+tpp(2,2,4)*CS_KRbeta+tpp(2,2,2)*CS_KRgemma )+DeltaP   !+v23
Hamiltonian0(2,3)=2.0_DP*( tpp(2,3,6)*CS_KRalpha+tpp(2,3,4)*CS_KRbeta+tpp(2,3,2)*CS_KRgemma )
Hamiltonian0(3,3)=2.0_DP*( tpp(3,3,6)*CS_KRalpha+tpp(3,3,4)*CS_KRbeta+tpp(3,3,2)*CS_KRgemma )+DeltaZ   !+v23

!!%%%%%%%%%%%%%%        SB-----SB            %%%%%%%%%%%%%%%%

Hamiltonian0(9,9) =2.0_DP*( tpp(1,1,6)*CS_KRalpha+tpp(1,1,4)*CS_KRbeta+tpp(1,1,2)*CS_KRgemma )+DeltaP   !-v12
Hamiltonian0(9,10)=Hamiltonian0(1,2)
Hamiltonian0(9,11)=Hamiltonian0(1,3)

Hamiltonian0(10,10)=2.0_DP*( tpp(2,2,6)*CS_KRalpha+tpp(2,2,4)*CS_KRbeta+tpp(2,2,2)*CS_KRgemma )+DeltaP  !-v12
Hamiltonian0(10,11)=Hamiltonian0(2,3)
Hamiltonian0(11,11)=2.0_DP*( tpp(3,3,6)*CS_KRalpha+tpp(3,3,4)*CS_KRbeta+tpp(3,3,2)*CS_KRgemma )+DeltaZ  !-v12

!!%%%%%%%%%%%%%%        Mo-----Mo            %%%%%%%%%%%%%%%%

Hamiltonian0(4,4)=2.0_DP*( tdd(1,1,6)*CS_KRalpha+tdd(1,1,4)*CS_KRbeta+tdd(1,1,2)*CS_KRgemma )+Delta2  !  v2=0 eV
Hamiltonian0(4,5)=2.0_DP*( tdd(1,2,6)*CS_KRalpha+tdd(1,2,4)*CS_KRbeta+tdd(1,2,2)*CS_KRgemma )
Hamiltonian0(4,6)=2.0_DP*( tdd(1,3,6)*CS_KRalpha+tdd(1,3,4)*CS_KRbeta+tdd(1,3,2)*CS_KRgemma )
Hamiltonian0(4,7)=2.0_DP*( tdd(1,4,6)*CS_KRalpha+tdd(1,4,4)*CS_KRbeta+tdd(1,4,2)*CS_KRgemma )
Hamiltonian0(4,8)=2.0_DP*( tdd(1,5,6)*CS_KRalpha+tdd(1,5,4)*CS_KRbeta+tdd(1,5,2)*CS_KRgemma )

Hamiltonian0(5,5)=2.0_DP*( tdd(2,2,6)*CS_KRalpha+tdd(2,2,4)*CS_KRbeta+tdd(2,2,2)*CS_KRgemma )+Delta1  !  v2=0 eV
Hamiltonian0(5,6)=2.0_DP*( tdd(2,3,6)*CS_KRalpha+tdd(2,3,4)*CS_KRbeta+tdd(2,3,2)*CS_KRgemma )
Hamiltonian0(5,7)=2.0_DP*( tdd(2,4,6)*CS_KRalpha+tdd(2,4,4)*CS_KRbeta+tdd(2,4,2)*CS_KRgemma )
Hamiltonian0(5,8)=2.0_DP*( tdd(2,5,6)*CS_KRalpha+tdd(2,5,4)*CS_KRbeta+tdd(2,5,2)*CS_KRgemma )

Hamiltonian0(6,6)=2.0_DP*( tdd(3,3,6)*CS_KRalpha+tdd(3,3,4)*CS_KRbeta+tdd(3,3,2)*CS_KRgemma )+Delta1    !  v2=0 eV
Hamiltonian0(6,7)=2.0_DP*( tdd(3,4,6)*CS_KRalpha+tdd(3,4,4)*CS_KRbeta+tdd(3,4,2)*CS_KRgemma )
Hamiltonian0(6,8)=2.0_DP*( tdd(3,5,6)*CS_KRalpha+tdd(3,5,4)*CS_KRbeta+tdd(3,5,2)*CS_KRgemma )

Hamiltonian0(7,7)=2.0_DP*( tdd(4,4,6)*CS_KRalpha+tdd(4,4,4)*CS_KRbeta+tdd(4,4,2)*CS_KRgemma )+Delta2    !  v2=0 eV
Hamiltonian0(7,8)=2.0_DP*( tdd(4,5,6)*CS_KRalpha+tdd(4,5,4)*CS_KRbeta+tdd(4,5,2)*CS_KRgemma )
Hamiltonian0(8,8)=2.0_DP*( tdd(5,5,6)*CS_KRalpha+tdd(5,5,4)*CS_KRbeta+tdd(5,5,2)*CS_KRgemma )+Delta0    !  v2=0 eV

!!%%%%%%%%%%%%%%        Mo-----> ST    dega+        %%%%%%%%%%%%%%%%

xRalpha   =  -0.5*sqrt(3.0_DP)*carbondistance          ; yRalpha   =   -0.5*carbondistance
xRbeta    =   0                                        ; yRbeta    =    carbondistance
xRgemma   =   0.5*sqrt(3.0_DP)*carbondistance          ; yRgemma   =   -0.5*carbondistance

Expon_KRalpha      =exp(  -AI*(xk*xRalpha+yk*yRalpha )  )
Expon_KRbeta       =exp(  -AI*(xk*xRbeta+yk*yRbeta )    )
Expon_KRgemma      =exp(  -AI*(xk*xRgemma+yk*yRgemma)   )

phase =exp(AI*(xk*xRbeta+yk*yRbeta ))     !!!!   Bloch theory in one Unit Cell

Hamiltonian0(1,4)=tdp(1,1,3)*Expon_KRalpha+tdp(1,1,2)*Expon_KRgemma+tdp(1,1,1)*Expon_KRbeta
Hamiltonian0(1,5)=tdp(1,2,3)*Expon_KRalpha+tdp(1,2,2)*Expon_KRgemma+tdp(1,2,1)*Expon_KRbeta
Hamiltonian0(1,6)=tdp(1,3,3)*Expon_KRalpha+tdp(1,3,2)*Expon_KRgemma+tdp(1,3,1)*Expon_KRbeta
Hamiltonian0(1,7)=tdp(1,4,3)*Expon_KRalpha+tdp(1,4,2)*Expon_KRgemma+tdp(1,4,1)*Expon_KRbeta
Hamiltonian0(1,8)=tdp(1,5,3)*Expon_KRalpha+tdp(1,5,2)*Expon_KRgemma+tdp(1,5,1)*Expon_KRbeta

Hamiltonian0(2,4)=tdp(2,1,3)*Expon_KRalpha+tdp(2,1,2)*Expon_KRgemma+tdp(2,1,1)*Expon_KRbeta
Hamiltonian0(2,5)=tdp(2,2,3)*Expon_KRalpha+tdp(2,2,2)*Expon_KRgemma+tdp(2,2,1)*Expon_KRbeta
Hamiltonian0(2,6)=tdp(2,3,3)*Expon_KRalpha+tdp(2,3,2)*Expon_KRgemma+tdp(2,3,1)*Expon_KRbeta
Hamiltonian0(2,7)=tdp(2,4,3)*Expon_KRalpha+tdp(2,4,2)*Expon_KRgemma+tdp(2,4,1)*Expon_KRbeta
Hamiltonian0(2,8)=tdp(2,5,3)*Expon_KRalpha+tdp(2,5,2)*Expon_KRgemma+tdp(2,5,1)*Expon_KRbeta

Hamiltonian0(3,4)=tdp(3,1,3)*Expon_KRalpha+tdp(3,1,2)*Expon_KRgemma+tdp(3,1,1)*Expon_KRbeta
Hamiltonian0(3,5)=tdp(3,2,3)*Expon_KRalpha+tdp(3,2,2)*Expon_KRgemma+tdp(3,2,1)*Expon_KRbeta
Hamiltonian0(3,6)=tdp(3,3,3)*Expon_KRalpha+tdp(3,3,2)*Expon_KRgemma+tdp(3,3,1)*Expon_KRbeta
Hamiltonian0(3,7)=tdp(3,4,3)*Expon_KRalpha+tdp(3,4,2)*Expon_KRgemma+tdp(3,4,1)*Expon_KRbeta
Hamiltonian0(3,8)=tdp(3,5,3)*Expon_KRalpha+tdp(3,5,2)*Expon_KRgemma+tdp(3,5,1)*Expon_KRbeta
Hamiltonian0(1:3 , 4:8 )=Hamiltonian0(1:3 , 4:8 )*phase

!!%%%%%%%%%%%%%%        SB -----> Mo            %%%%%%%%%%%%%%%%

xRalpha   = -0.5*sqrt(3.0_DP)*carbondistance          ; yRalpha   =   -0.5*carbondistance
xRbeta    =  0.d0                                     ; yRbeta    =    carbondistance
xRgemma   =  0.5*sqrt(3.0_DP)*carbondistance          ; yRgemma   =   -0.5*carbondistance

Expon_KRalpha      =exp(AI*(xk*xRalpha+yk*yRalpha ))
Expon_KRbeta       =exp(AI*(xk*xRbeta+yk*yRbeta))
Expon_KRgemma      =exp(AI*(xk*xRgemma+yk*yRgemma))

phase =exp(-AI*(xk*xRbeta+yk*yRbeta ))    !!!!   Bloch theory in one Unit Cell

Hamiltonian0(4,9)  =tdp(1,1,6)*Expon_KRalpha+tdp(1,1,5)*Expon_KRgemma+tdp(1,1,4)*Expon_KRbeta
Hamiltonian0(5,9)  =tdp(1,2,6)*Expon_KRalpha+tdp(1,2,5)*Expon_KRgemma+tdp(1,2,4)*Expon_KRbeta
Hamiltonian0(6,9)  =tdp(1,3,6)*Expon_KRalpha+tdp(1,3,5)*Expon_KRgemma+tdp(1,3,4)*Expon_KRbeta
Hamiltonian0(7,9)  =tdp(1,4,6)*Expon_KRalpha+tdp(1,4,5)*Expon_KRgemma+tdp(1,4,4)*Expon_KRbeta
Hamiltonian0(8,9)  =tdp(1,5,6)*Expon_KRalpha+tdp(1,5,5)*Expon_KRgemma+tdp(1,5,4)*Expon_KRbeta

Hamiltonian0(4,10)  =tdp(2,1,6)*Expon_KRalpha+tdp(2,1,5)*Expon_KRgemma+tdp(2,1,4)*Expon_KRbeta
Hamiltonian0(5,10)  =tdp(2,2,6)*Expon_KRalpha+tdp(2,2,5)*Expon_KRgemma+tdp(2,2,4)*Expon_KRbeta
Hamiltonian0(6,10)  =tdp(2,3,6)*Expon_KRalpha+tdp(2,3,5)*Expon_KRgemma+tdp(2,3,4)*Expon_KRbeta
Hamiltonian0(7,10)  =tdp(2,4,6)*Expon_KRalpha+tdp(2,4,5)*Expon_KRgemma+tdp(2,4,4)*Expon_KRbeta
Hamiltonian0(8,10)  =tdp(2,5,6)*Expon_KRalpha+tdp(2,5,5)*Expon_KRgemma+tdp(2,5,4)*Expon_KRbeta

Hamiltonian0(4,11)  =tdp(3,1,6)*Expon_KRalpha+tdp(3,1,5)*Expon_KRgemma+tdp(3,1,4)*Expon_KRbeta
Hamiltonian0(5,11)  =tdp(3,2,6)*Expon_KRalpha+tdp(3,2,5)*Expon_KRgemma+tdp(3,2,4)*Expon_KRbeta
Hamiltonian0(6,11)  =tdp(3,3,6)*Expon_KRalpha+tdp(3,3,5)*Expon_KRgemma+tdp(3,3,4)*Expon_KRbeta
Hamiltonian0(7,11)  =tdp(3,4,6)*Expon_KRalpha+tdp(3,4,5)*Expon_KRgemma+tdp(3,4,4)*Expon_KRbeta
Hamiltonian0(8,11)  =tdp(3,5,6)*Expon_KRalpha+tdp(3,5,5)*Expon_KRgemma+tdp(3,5,4)*Expon_KRbeta
Hamiltonian0(4:8 , 9:11)  =Hamiltonian0(4:8 , 9:11)*phase

!!%%%%%%%%%%%%%%        SB -----> ST           %%%%%%%%%%%%%%%%
!!    SB----> ST

    Hamiltonian0(1,9) =tpp(1,1,10)
    Hamiltonian0(1,10)=tpp(1,2,10)
    Hamiltonian0(1,11)=tpp(1,3,10)

    Hamiltonian0(2,9) =tpp(2,1,10)
    Hamiltonian0(2,10)=tpp(2,2,10)
    Hamiltonian0(2,11)=tpp(2,3,10)

    Hamiltonian0(3,9) =tpp(3,1,10)
    Hamiltonian0(3,10)=tpp(3,2,10)
    Hamiltonian0(3,11)=tpp(3,3,10)

!!%%%%%%%%%%%%%%        ST one layer -----> SB up layer    daga+       %%%%%%%%%%%%%%%%

    xRalpha   = -0.5*sqrt(3.0_DP)*carbondistance          ; yRalpha   =   -0.5*carbondistance
    xRbeta    =  0.d0                                     ; yRbeta    =    carbondistance
    xRgemma   =  0.5*sqrt(3.0_DP)*carbondistance          ; yRgemma   =   -0.5*carbondistance

    Expon_KRalpha      =exp(AI*(xk*xRalpha+yk*yRalpha ))
    Expon_KRbeta       =exp(AI*(xk*xRbeta+yk*yRbeta ))
    Expon_KRgemma      =exp(AI*(xk*xRgemma+yk*yRgemma))

    phase =exp(-AI*(xk*xRbeta+yk*yRbeta ))    !!!!   Bloch theory in one Unit Cell

    Hamiltonian0inter(9,12)=tpp(1,1,9)*Expon_KRalpha+tpp(1,1,8)*Expon_KRgemma+tpp(1,1,7)*Expon_KRbeta
    Hamiltonian0inter(9,13)=tpp(1,2,9)*Expon_KRalpha+tpp(1,2,8)*Expon_KRgemma+tpp(1,2,7)*Expon_KRbeta
    Hamiltonian0inter(9,14)=tpp(1,3,9)*Expon_KRalpha+tpp(1,3,8)*Expon_KRgemma+tpp(1,3,7)*Expon_KRbeta

    Hamiltonian0inter(10,12)=tpp(2,1,9)*Expon_KRalpha+tpp(2,1,8)*Expon_KRgemma+tpp(2,1,7)*Expon_KRbeta
    Hamiltonian0inter(10,13)=tpp(2,2,9)*Expon_KRalpha+tpp(2,2,8)*Expon_KRgemma+tpp(2,2,7)*Expon_KRbeta
    Hamiltonian0inter(10,14)=tpp(2,3,9)*Expon_KRalpha+tpp(2,3,8)*Expon_KRgemma+tpp(2,3,7)*Expon_KRbeta

    Hamiltonian0inter(11,12)=tpp(3,1,9)*Expon_KRalpha+tpp(3,1,8)*Expon_KRgemma+tpp(3,1,7)*Expon_KRbeta
    Hamiltonian0inter(11,13)=tpp(3,2,9)*Expon_KRalpha+tpp(3,2,8)*Expon_KRgemma+tpp(3,2,7)*Expon_KRbeta
    Hamiltonian0inter(11,14)=tpp(3,3,9)*Expon_KRalpha+tpp(3,3,8)*Expon_KRgemma+tpp(3,3,7)*Expon_KRbeta
    Hamiltonian0inter( 9:11, 12:14)=phase*Hamiltonian0inter( 9:11, 12:14)

!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
!         Hamiltonian1
!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

!======================================================================================
    xRalpha   =   -Vec2X            ;       yRalpha    =   -Vec2Y
    xRbeta    =    Vec1X+Vec2X      ;       yRbeta     =    Vec1Y+Vec2Y
    xRgemma   =   -Vec1X            ;       yRgemma    =   -Vec1Y

    xRalpha = -xRalpha  ;   yRalpha  = -yRalpha
    xRbeta  = -xRbeta   ;   yRbeta   = -yRbeta
    xRgemma = -xRgemma  ;   yRgemma  = -yRgemma

    CS_KRalpha      =   COS(xk*xRalpha+yk*yRalpha)
    CS_KRbeta       =   COS(xk*xRbeta+yk*yRbeta)
    CS_KRgemma      =   COS(xk*xRgemma+yk*yRgemma)

!!%%%%%%%%%%%%%%        ST-----ST            %%%%%%%%%%%%%%%%

    Hamiltonian1(1,1)=2.0_DP*( tpp(1,1,6)*CS_KRalpha+tpp(1,1,4)*CS_KRbeta+tpp(1,1,2)*CS_KRgemma )+DeltaP !+v23
    Hamiltonian1(1,2)=2.0_DP*( tpp(1,2,6)*CS_KRalpha+tpp(1,2,4)*CS_KRbeta+tpp(1,2,2)*CS_KRgemma )
    Hamiltonian1(1,3)=2.0_DP*( tpp(1,3,6)*CS_KRalpha+tpp(1,3,4)*CS_KRbeta+tpp(1,3,2)*CS_KRgemma )

    Hamiltonian1(2,2)=2.0_DP*( tpp(2,2,6)*CS_KRalpha+tpp(2,2,4)*CS_KRbeta+tpp(2,2,2)*CS_KRgemma )+DeltaP !+v23
    Hamiltonian1(2,3)=2.0_DP*( tpp(2,3,6)*CS_KRalpha+tpp(2,3,4)*CS_KRbeta+tpp(2,3,2)*CS_KRgemma )
    Hamiltonian1(3,3)=2.0_DP*( tpp(3,3,6)*CS_KRalpha+tpp(3,3,4)*CS_KRbeta+tpp(3,3,2)*CS_KRgemma )+DeltaZ !+v23

!!%%%%%%%%%%%%%%        SB-----SB            %%%%%%%%%%%%%%%%

    Hamiltonian1(9,9)  =2.0_DP*( tpp(1,1,6)*CS_KRalpha+tpp(1,1,4)*CS_KRbeta+tpp(1,1,2)*CS_KRgemma )+DeltaP !-v12
    Hamiltonian1(9,10)=Hamiltonian1(1,2)
    Hamiltonian1(9,11)=Hamiltonian1(1,3)

    Hamiltonian1(10,10)=2.0_DP*( tpp(2,2,6)*CS_KRalpha+tpp(2,2,4)*CS_KRbeta+tpp(2,2,2)*CS_KRgemma )+DeltaP !-v12
    Hamiltonian1(10,11)=Hamiltonian1(2,3)
    Hamiltonian1(11,11)=2.0_DP*( tpp(3,3,6)*CS_KRalpha+tpp(3,3,4)*CS_KRbeta+tpp(3,3,2)*CS_KRgemma )+DeltaZ !-v12

!!%%%%%%%%%%%%%%        Mo-----Mo            %%%%%%%%%%%%%%%%

    Hamiltonian1(4,4)=2.0_DP*( tdd(1,1,6)*CS_KRalpha+tdd(1,1,4)*CS_KRbeta+tdd(1,1,2)*CS_KRgemma )+Delta2  !  v2=0 eV
    Hamiltonian1(4,5)=2.0_DP*( tdd(1,2,6)*CS_KRalpha+tdd(1,2,4)*CS_KRbeta+tdd(1,2,2)*CS_KRgemma )
    Hamiltonian1(4,6)=2.0_DP*( tdd(1,3,6)*CS_KRalpha+tdd(1,3,4)*CS_KRbeta+tdd(1,3,2)*CS_KRgemma )
    Hamiltonian1(4,7)=2.0_DP*( tdd(1,4,6)*CS_KRalpha+tdd(1,4,4)*CS_KRbeta+tdd(1,4,2)*CS_KRgemma )
    Hamiltonian1(4,8)=2.0_DP*( tdd(1,5,6)*CS_KRalpha+tdd(1,5,4)*CS_KRbeta+tdd(1,5,2)*CS_KRgemma )

    Hamiltonian1(5,5)=2.0_DP*( tdd(2,2,6)*CS_KRalpha+tdd(2,2,4)*CS_KRbeta+tdd(2,2,2)*CS_KRgemma )+Delta1  !  v2=0 eV
    Hamiltonian1(5,6)=2.0_DP*( tdd(2,3,6)*CS_KRalpha+tdd(2,3,4)*CS_KRbeta+tdd(2,3,2)*CS_KRgemma )
    Hamiltonian1(5,7)=2.0_DP*( tdd(2,4,6)*CS_KRalpha+tdd(2,4,4)*CS_KRbeta+tdd(2,4,2)*CS_KRgemma )
    Hamiltonian1(5,8)=2.0_DP*( tdd(2,5,6)*CS_KRalpha+tdd(2,5,4)*CS_KRbeta+tdd(2,5,2)*CS_KRgemma )

    Hamiltonian1(6,6)=2.0_DP*( tdd(3,3,6)*CS_KRalpha+tdd(3,3,4)*CS_KRbeta+tdd(3,3,2)*CS_KRgemma )+Delta1    !  v2=0 eV
    Hamiltonian1(6,7)=2.0_DP*( tdd(3,4,6)*CS_KRalpha+tdd(3,4,4)*CS_KRbeta+tdd(3,4,2)*CS_KRgemma )
    Hamiltonian1(6,8)=2.0_DP*( tdd(3,5,6)*CS_KRalpha+tdd(3,5,4)*CS_KRbeta+tdd(3,5,2)*CS_KRgemma )

    Hamiltonian1(7,7)=2.0_DP*( tdd(4,4,6)*CS_KRalpha+tdd(4,4,4)*CS_KRbeta+tdd(4,4,2)*CS_KRgemma )+Delta2    !  v2=0 eV
    Hamiltonian1(7,8)=2.0_DP*( tdd(4,5,6)*CS_KRalpha+tdd(4,5,4)*CS_KRbeta+tdd(4,5,2)*CS_KRgemma )
    Hamiltonian1(8,8)=2.0_DP*( tdd(5,5,6)*CS_KRalpha+tdd(5,5,4)*CS_KRbeta+tdd(5,5,2)*CS_KRgemma )+Delta0    !  v2=0 eV

!!%%%%%%%%%%%%%%        Mo-----> ST    dega+        %%%%%%%%%%%%%%%%

    xRalpha   = -0.5*sqrt(3.0_DP)*carbondistance          ; yRalpha   =   -0.5*carbondistance
    xRbeta    =   0                                       ; yRbeta    =    carbondistance
    xRgemma   =  0.5*sqrt(3.0_DP)*carbondistance          ; yRgemma   =   -0.5*carbondistance

    xRalpha = -xRalpha  ;   yRalpha  = -yRalpha
    xRbeta  = -xRbeta   ;   yRbeta   = -yRbeta
    xRgemma = -xRgemma  ;   yRgemma  = -yRgemma

    Expon_KRalpha   = exp(-AI*(xk*xRalpha+yk*yRalpha ))
    Expon_KRbeta    = exp(-AI*(xk*xRbeta+yk*yRbeta ))
    Expon_KRgemma   = exp(-AI*(xk*xRgemma+yk*yRgemma))

    phase = exp(AI*(xk*xRbeta+yk*yRbeta ))     !!!!   Bloch theory in one Unit Cell

    Hamiltonian1(1,4)=tdp(1,1,3)*Expon_KRalpha+tdp(1,1,2)*Expon_KRgemma+tdp(1,1,1)*Expon_KRbeta
    Hamiltonian1(1,5)=tdp(1,2,3)*Expon_KRalpha+tdp(1,2,2)*Expon_KRgemma+tdp(1,2,1)*Expon_KRbeta
    Hamiltonian1(1,6)=tdp(1,3,3)*Expon_KRalpha+tdp(1,3,2)*Expon_KRgemma+tdp(1,3,1)*Expon_KRbeta
    Hamiltonian1(1,7)=tdp(1,4,3)*Expon_KRalpha+tdp(1,4,2)*Expon_KRgemma+tdp(1,4,1)*Expon_KRbeta
    Hamiltonian1(1,8)=tdp(1,5,3)*Expon_KRalpha+tdp(1,5,2)*Expon_KRgemma+tdp(1,5,1)*Expon_KRbeta

    Hamiltonian1(2,4)=tdp(2,1,3)*Expon_KRalpha+tdp(2,1,2)*Expon_KRgemma+tdp(2,1,1)*Expon_KRbeta
    Hamiltonian1(2,5)=tdp(2,2,3)*Expon_KRalpha+tdp(2,2,2)*Expon_KRgemma+tdp(2,2,1)*Expon_KRbeta
    Hamiltonian1(2,6)=tdp(2,3,3)*Expon_KRalpha+tdp(2,3,2)*Expon_KRgemma+tdp(2,3,1)*Expon_KRbeta
    Hamiltonian1(2,7)=tdp(2,4,3)*Expon_KRalpha+tdp(2,4,2)*Expon_KRgemma+tdp(2,4,1)*Expon_KRbeta
    Hamiltonian1(2,8)=tdp(2,5,3)*Expon_KRalpha+tdp(2,5,2)*Expon_KRgemma+tdp(2,5,1)*Expon_KRbeta

    Hamiltonian1(3,4)=tdp(3,1,3)*Expon_KRalpha+tdp(3,1,2)*Expon_KRgemma+tdp(3,1,1)*Expon_KRbeta
    Hamiltonian1(3,5)=tdp(3,2,3)*Expon_KRalpha+tdp(3,2,2)*Expon_KRgemma+tdp(3,2,1)*Expon_KRbeta
    Hamiltonian1(3,6)=tdp(3,3,3)*Expon_KRalpha+tdp(3,3,2)*Expon_KRgemma+tdp(3,3,1)*Expon_KRbeta
    Hamiltonian1(3,7)=tdp(3,4,3)*Expon_KRalpha+tdp(3,4,2)*Expon_KRgemma+tdp(3,4,1)*Expon_KRbeta
    Hamiltonian1(3,8)=tdp(3,5,3)*Expon_KRalpha+tdp(3,5,2)*Expon_KRgemma+tdp(3,5,1)*Expon_KRbeta
    Hamiltonian1(1:3 , 4:8 )=Hamiltonian1(1:3 , 4:8 )*phase

!!%%%%%%%%%%%%%%        SB -----> Mo            %%%%%%%%%%%%%%%%

    xRalpha   = -0.5*sqrt(3.0_DP)*carbondistance          ; yRalpha   =   -0.5*carbondistance
    xRbeta    =   0                                       ; yRbeta    =    carbondistance
    xRgemma   =  0.5*sqrt(3.0_DP)*carbondistance          ; yRgemma   =   -0.5*carbondistance

    xRalpha = -xRalpha  ;   yRalpha  = -yRalpha
    xRbeta  = -xRbeta   ;   yRbeta   = -yRbeta
    xRgemma = -xRgemma  ;   yRgemma  = -yRgemma

    Expon_KRalpha   = exp(AI*(xk*xRalpha+yk*yRalpha ))
    Expon_KRbeta    = exp(AI*(xk*xRbeta+yk*yRbeta))
    Expon_KRgemma   = exp(AI*(xk*xRgemma+yk*yRgemma))

    phase = exp(-AI*(xk*xRbeta+yk*yRbeta ))    !!!!   Bloch theory in one Unit Cell

    Hamiltonian1(4,9)  =tdp(1,1,6)*Expon_KRalpha+tdp(1,1,5)*Expon_KRgemma+tdp(1,1,4)*Expon_KRbeta
    Hamiltonian1(5,9)  =tdp(1,2,6)*Expon_KRalpha+tdp(1,2,5)*Expon_KRgemma+tdp(1,2,4)*Expon_KRbeta
    Hamiltonian1(6,9)  =tdp(1,3,6)*Expon_KRalpha+tdp(1,3,5)*Expon_KRgemma+tdp(1,3,4)*Expon_KRbeta
    Hamiltonian1(7,9)  =tdp(1,4,6)*Expon_KRalpha+tdp(1,4,5)*Expon_KRgemma+tdp(1,4,4)*Expon_KRbeta
    Hamiltonian1(8,9)  =tdp(1,5,6)*Expon_KRalpha+tdp(1,5,5)*Expon_KRgemma+tdp(1,5,4)*Expon_KRbeta

    Hamiltonian1(4,10)  =tdp(2,1,6)*Expon_KRalpha+tdp(2,1,5)*Expon_KRgemma+tdp(2,1,4)*Expon_KRbeta
    Hamiltonian1(5,10)  =tdp(2,2,6)*Expon_KRalpha+tdp(2,2,5)*Expon_KRgemma+tdp(2,2,4)*Expon_KRbeta
    Hamiltonian1(6,10)  =tdp(2,3,6)*Expon_KRalpha+tdp(2,3,5)*Expon_KRgemma+tdp(2,3,4)*Expon_KRbeta
    Hamiltonian1(7,10)  =tdp(2,4,6)*Expon_KRalpha+tdp(2,4,5)*Expon_KRgemma+tdp(2,4,4)*Expon_KRbeta
    Hamiltonian1(8,10)  =tdp(2,5,6)*Expon_KRalpha+tdp(2,5,5)*Expon_KRgemma+tdp(2,5,4)*Expon_KRbeta

    Hamiltonian1(4,11)  =tdp(3,1,6)*Expon_KRalpha+tdp(3,1,5)*Expon_KRgemma+tdp(3,1,4)*Expon_KRbeta
    Hamiltonian1(5,11)  =tdp(3,2,6)*Expon_KRalpha+tdp(3,2,5)*Expon_KRgemma+tdp(3,2,4)*Expon_KRbeta
    Hamiltonian1(6,11)  =tdp(3,3,6)*Expon_KRalpha+tdp(3,3,5)*Expon_KRgemma+tdp(3,3,4)*Expon_KRbeta
    Hamiltonian1(7,11)  =tdp(3,4,6)*Expon_KRalpha+tdp(3,4,5)*Expon_KRgemma+tdp(3,4,4)*Expon_KRbeta
    Hamiltonian1(8,11)  =tdp(3,5,6)*Expon_KRalpha+tdp(3,5,5)*Expon_KRgemma+tdp(3,5,4)*Expon_KRbeta
    Hamiltonian1(4:8 , 9:11)  =Hamiltonian1(4:8 , 9:11)*phase

!!%%%%%%%%%%%%%%        SB -----> ST           %%%%%%%%%%%%%%%%
!!    SB----> ST

    Hamiltonian1(1,9)=tpp(1,1,10)
    Hamiltonian1(1,10)=tpp(1,2,10)
    Hamiltonian1(1,11)=tpp(1,3,10)

    Hamiltonian1(2,9)=tpp(2,1,10)
    Hamiltonian1(2,10)=tpp(2,2,10)
    Hamiltonian1(2,11)=tpp(2,3,10)

    Hamiltonian1(3,9)=tpp(3,1,10)
    Hamiltonian1(3,10)=tpp(3,2,10)
    Hamiltonian1(3,11)=tpp(3,3,10)

!!%%%%%%%%%%%%%%        ST one layer -----> SB up layer    daga+       %%%%%%%%%%%%%%%%

    xRalpha   = -0.5*sqrt(3.0_DP)*carbondistance          ; yRalpha   =   -0.5*carbondistance
    xRbeta    =  0.d0                                     ; yRbeta    =    carbondistance
    xRgemma   =  0.5*sqrt(3.0_DP)*carbondistance          ; yRgemma   =   -0.5*carbondistance

    Expon_KRalpha      =exp(AI*(xk*xRalpha+yk*yRalpha ))
    Expon_KRbeta       =exp(AI*(xk*xRbeta+yk*yRbeta ))
    Expon_KRgemma      =exp(AI*(xk*xRgemma+yk*yRgemma))

    phase =exp(-AI*(xk*xRbeta+yk*yRbeta ))    !!!!   Bloch theory in one Unit Cell

    Hamiltonian1inter(9,12)=tpp(1,1,9)*Expon_KRalpha+tpp(1,1,8)*Expon_KRgemma+tpp(1,1,7)*Expon_KRbeta
    Hamiltonian1inter(9,13)=tpp(1,2,9)*Expon_KRalpha+tpp(1,2,8)*Expon_KRgemma+tpp(1,2,7)*Expon_KRbeta
    Hamiltonian1inter(9,14)=tpp(1,3,9)*Expon_KRalpha+tpp(1,3,8)*Expon_KRgemma+tpp(1,3,7)*Expon_KRbeta

    Hamiltonian1inter(10,12)=tpp(2,1,9)*Expon_KRalpha+tpp(2,1,8)*Expon_KRgemma+tpp(2,1,7)*Expon_KRbeta
    Hamiltonian1inter(10,13)=tpp(2,2,9)*Expon_KRalpha+tpp(2,2,8)*Expon_KRgemma+tpp(2,2,7)*Expon_KRbeta
    Hamiltonian1inter(10,14)=tpp(2,3,9)*Expon_KRalpha+tpp(2,3,8)*Expon_KRgemma+tpp(2,3,7)*Expon_KRbeta

    Hamiltonian1inter(11,12)=tpp(3,1,9)*Expon_KRalpha+tpp(3,1,8)*Expon_KRgemma+tpp(3,1,7)*Expon_KRbeta
    Hamiltonian1inter(11,13)=tpp(3,2,9)*Expon_KRalpha+tpp(3,2,8)*Expon_KRgemma+tpp(3,2,7)*Expon_KRbeta
    Hamiltonian1inter(11,14)=tpp(3,3,9)*Expon_KRalpha+tpp(3,3,8)*Expon_KRgemma+tpp(3,3,7)*Expon_KRbeta

    Hamiltonian1inter( 9:11, 12:14)=phase*Hamiltonian1inter( 9:11, 12:14)

    Hamiltonian1inter(9,14)=-Hamiltonian1inter(9,14)
    Hamiltonian1inter(10,14)=-Hamiltonian1inter(10,14)
    Hamiltonian1inter(11,12)=-Hamiltonian1inter(11,12)
    Hamiltonian1inter(11,13)=-Hamiltonian1inter(11,13)

!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
!           Fu ZHI
!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
!  Chu SHI Values
do i= 1, neigenvalues
        Hamiltonian(  i ,  1: neigenvalues  )= ( 0.d0, 0.d0 )
end do

If ( neigenvalues==11 ) then
        lneigenvalues= 11
        Hamiltonian(  lneigenvalues-10, lneigenvalues-10: lneigenvalues  )= Hamiltonian0( 1, 1:11  )
        Hamiltonian(  lneigenvalues-9, lneigenvalues-9: lneigenvalues  )= Hamiltonian0( 2, 2:11  )
        Hamiltonian(  lneigenvalues-8, lneigenvalues-8: lneigenvalues  )= Hamiltonian0( 3, 3:11  )
        Hamiltonian(  lneigenvalues-7, lneigenvalues-7: lneigenvalues  )= Hamiltonian0( 4, 4:11  )
        Hamiltonian(  lneigenvalues-6, lneigenvalues-6: lneigenvalues  )= Hamiltonian0( 5, 5:11  )
        Hamiltonian(  lneigenvalues-5, lneigenvalues-5: lneigenvalues  )= Hamiltonian0( 6, 6:11  )
        Hamiltonian(  lneigenvalues-4, lneigenvalues-4: lneigenvalues  )= Hamiltonian0( 7, 7:11  )
        Hamiltonian(  lneigenvalues-3, lneigenvalues-3: lneigenvalues  )= Hamiltonian0( 8, 8:11  )
        Hamiltonian(  lneigenvalues-2, lneigenvalues-2: lneigenvalues  )= Hamiltonian0( 9, 9:11  )
        Hamiltonian(  lneigenvalues-1, lneigenvalues-1: lneigenvalues  )= Hamiltonian0( 10, 10:11  )
        Hamiltonian(  lneigenvalues , lneigenvalues  )= Hamiltonian0( 11, 11  )

elseif (  neigenvalues==22      ) then

        lneigenvalues= 22
        Hamiltonian(  lneigenvalues-21, lneigenvalues-21: lneigenvalues-11  )= Hamiltonian0( 1, 1:11  )
        Hamiltonian(  lneigenvalues-20, lneigenvalues-20: lneigenvalues-11  )= Hamiltonian0( 2, 2:11  )
        Hamiltonian(  lneigenvalues-19, lneigenvalues-19: lneigenvalues-11  )= Hamiltonian0( 3, 3:11  )
        Hamiltonian(  lneigenvalues-18, lneigenvalues-18: lneigenvalues-11  )= Hamiltonian0( 4, 4:11  )
        Hamiltonian(  lneigenvalues-17, lneigenvalues-17: lneigenvalues-11  )= Hamiltonian0( 5, 5:11  )
        Hamiltonian(  lneigenvalues-16, lneigenvalues-16: lneigenvalues-11  )= Hamiltonian0( 6, 6:11  )
        Hamiltonian(  lneigenvalues-15, lneigenvalues-15: lneigenvalues-11  )= Hamiltonian0( 7, 7:11  )
        Hamiltonian(  lneigenvalues-14, lneigenvalues-14: lneigenvalues-11  )= Hamiltonian0( 8, 8:11  )
        Hamiltonian(  lneigenvalues-13, lneigenvalues-13: lneigenvalues-11  )= Hamiltonian0( 9, 9:11  )
        Hamiltonian(  lneigenvalues-12, lneigenvalues-12: lneigenvalues-11  )= Hamiltonian0( 10, 10:11  )
        Hamiltonian(  lneigenvalues-11 , lneigenvalues-11  )= Hamiltonian0( 11, 11  )

        Hamiltonian(  lneigenvalues-10, lneigenvalues-10: lneigenvalues  )= Hamiltonian1( 1, 1:11  )
        Hamiltonian(  lneigenvalues-9, lneigenvalues-9: lneigenvalues  )= Hamiltonian1( 2, 2:11  )
        Hamiltonian(  lneigenvalues-8, lneigenvalues-8: lneigenvalues  )= Hamiltonian1( 3, 3:11  )
        Hamiltonian(  lneigenvalues-7, lneigenvalues-7: lneigenvalues  )= Hamiltonian1( 4, 4:11  )
        Hamiltonian(  lneigenvalues-6, lneigenvalues-6: lneigenvalues  )= Hamiltonian1( 5, 5:11  )
        Hamiltonian(  lneigenvalues-5, lneigenvalues-5: lneigenvalues  )= Hamiltonian1( 6, 6:11  )
        Hamiltonian(  lneigenvalues-4, lneigenvalues-4: lneigenvalues  )= Hamiltonian1( 7, 7:11  )
        Hamiltonian(  lneigenvalues-3, lneigenvalues-3: lneigenvalues  )= Hamiltonian1( 8, 8:11  )
        Hamiltonian(  lneigenvalues-2, lneigenvalues-2: lneigenvalues  )= Hamiltonian1( 9, 9:11  )
        Hamiltonian(  lneigenvalues-1, lneigenvalues-1: lneigenvalues  )= Hamiltonian1( 10, 10:11  )
        Hamiltonian(  lneigenvalues , lneigenvalues  )= Hamiltonian1( 11, 11  )

        Hamiltonian(  lneigenvalues-21 : lneigenvalues-19,    lneigenvalues-10: lneigenvalues-8  )=Hamiltonian0inter(9:11 , 12:14)
        Hamiltonian(  lneigenvalues-13 : lneigenvalues-11,    lneigenvalues-2: lneigenvalues  )=Hamiltonian1inter(9:11 , 12:14)

end if

!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
!           Vonset
!!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    do i=1, neigenvalues
        m=mod( i, 11 )
        j = (i-1)/11+1
        if(  m>0 .and. m<4   ) then
            Hamiltonian( i, i )=Hamiltonian( i, i )+Vonset( 3,  j )
            else if (  m>3 .and. m<9 ) then
            Hamiltonian( i, i )=Hamiltonian( i, i )+Vonset( 2,  j )
            else if (  (m>8 .and. m<11) .or. (m==0)  ) then
            Hamiltonian( i, i )=Hamiltonian( i, i )+Vonset( 1,  j )
        end if
    !~ print*
    !~ print*, Hamiltonian( i, i )
    end do

!!==================================================================
!                            End     Fu ZHI
!!==================================================================
!write(*, '(2E16.8, 2E16.8, /, 2E16.8, 2E16.8)')   ((Hamiltonian(M,N), M=1,2),N=1,2)
!write(*, *)  Hamiltonian
!open (unit =UNIT4, file = FILENAME4,  status="UNKNOWN" , ACTION="WRITE", iostat=ierror)

!~ do i= 1, neigenvalues
        !~ Hamiltonian(  i ,  1: neigenvalues  )= ( 0.d0, 0.d0 )
!~ end do

	open (unit =UNIT4, file = FILENAME4,  status="UNKNOWN" , ACTION="WRITE", iostat=ierror)
	open (unit =UNIT17, file = FILENAME17,  status="UNKNOWN" , ACTION="WRITE", iostat=ierror)

	do i=1, neigenvalues
		write(UNIT4,  '(  22(f8.3) )'  )  ( real(Hamiltonian( i, j )),  j=1, neigenvalues)
		!~ write(UNIT4, '( a )' )
		write(UNIT17,  '(  22(f8.3) )'  )  ( aimag(Hamiltonian( i, j )),  j=1, neigenvalues)
		!~ write(UNIT17, '( a )' )
	end do
	close(UNIT4)
    close(UNIT17)
!!!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

!~ K=0
!~ do i=1,neigenvalues
   !~ do j=1,i
   !~ H(K)=Hamiltonian(j,i)
   !~ K=K+1
   !~ end do
!~ end do

        !~ call ZHPEVX('V','A','U',neigenvalues,H,Vlower,Vupper,1,neigenvalues,abstol,Mfound,eig,vec,neigenvalues,cwork,aux,iwork,ifail,info)
        !~ if(info.ne.0) then
        !~ write(6,*) 'info: ',info
        !~ write(6,*) 'Mfound ',Mfound
        !~ do i=0,3
        !~ write(6,*) 'i,ifail: ',i,ifail(i)
        !~ enddo
        !~ stop
        !~ endif
!     write(6,*) eig
!	write(6,*)

end subroutine Hamiltonianofxs2VII_DATA


      subroutine txint(p,vel,dep,j,t,x)
c----------------------------------------------------------------------
c  this routine calculates travel time-t and distance-x for a ray
c  of ray parameter p.   these are calculated using a linear gradient
c  approximation and accumulating incremental travel times for the top
c  j layers.  if p and j are inconsistent t and x are given the large
c  value flag.  this is a convenient means of flagging low velocity
c  zones.
c
c  arguements-
c    p  -ray parameter of ray for which travel time and distance
c        is to be calculated.
c    vel-array of velocities
c    dep-array of depths corresponding to entries in vel.
c    j  -travel times and distances are acculated for a ray traveling
c        from the surface down to depth of dep(j).
c    t  -travel time
c    x  -distance
c
c  modifications-
c    sept. 1980 - almost completely written in structure to use if else
c                 construction to run 1977 ansi standard fortran.
c    nov. 1980  - earlier version did not calculate travel times and
c                 distance correctly when the velocity gradient was
c                 negative.  (i.e. at the top of a low velocity zone)
c                 replaced slight inefficiency of earlier version in
c                 which square roots (variable names rad1 and rad2)
c                 where unnecessarily calculated twice.
c   March 1997 - fixed bug that occasionally caused problems when a
c                low gradient zone was present.  This caused a
c		divide by zero error.  Fixed by adding condtional
c		on rad2 (see below)
c----------------------------------------------------------------------
      real vel(j),dep(j)
      double precision si1,si2,rad1,rad2
      parameter(cutoff=1.0 e-10)
c--cutoff is a processor dependent constant used to prevent
c--unintentional roundoff errors in calculating ray parameters
c--as the reciprocal of vel(j) points.  cutoff should be set
c--to about  1.0 e-(nsigf-2) where nsigf is the number of significant
c--figures for a single precision floating point number.
c--CORRECTION:  double precsion, not single since  si1,si2,rad1,and
c--rad2 are doubles
      parameter(flag=1.0e20)
      parameter(gradmin=0.0001)
c--special branch for first entry
      if(j.le.1) then
           t=0.
           x=0.
           return
      endif
c--calculate travel times and distance
c--careful of totally illegal ray values.
      ndt=j-1
      dx=0.
      dt=0.
      si1=p*vel(1)
      if(si1.gt.1.0) go to 600
      rad1=dsqrt(1.0-si1)*dsqrt(1.0+si1)
      do 500 i=1,ndt
           si2=p*vel(i+1)
           vgrad=(vel(i+1)-vel(i))/(dep(i+1)-dep(i))
c--watch out for rounding errors in calculating si2
           if(abs(si2-1.0) .lt. cutoff) si2 = 1.0
c--branch out for lvz
           if(si2.gt.1.0) go to 600
           if(abs(vgrad).lt.gradmin) then
c--branch for very small gradient layers
                ddep=dep(i+1)-dep(i)
                dx=dx+p*vel(i)*ddep/rad1
                dt=dt+ddep/(vel(i)*rad1)
                rad2 = rad1
           else
c--come here for normal velocity points
c--note forms for dt and dx give the same answer for positive or
c--negative vgrad given the same pairs of velocity, depth points.
                rad2=dsqrt(1.0-si2)*dsqrt(1.0+si2)
                if(p.gt.0.) dx=dx+(rad1-rad2)/(vgrad*p)
                dt=dt+(1./vgrad)*dlog((dble(vel(i+1))/dble(vel(i)))
     $                       *((1.+rad1)/(1.+rad2)))
           endif
c--the bottom velocity grid point will be the top grid point when
c--we loop back so reset constants si1,and rad1 appropriately.
c--this is slightly more efficient than recalculating the square roots
c--over each time.
c--march 1997:  added conditional to avoid ever letting rad1 be le 0.0
           si1 = si2
           if(rad2.gt.0.0) then
           	rad1 = rad2
           else
		rad1 = cutoff
           endif
  500      continue
      t=dt
      x=dx
      return
c--come here for rays trapped in lvz
  600 t=flag
      x=flag
      return
      end

c $Id$


c*************************************************************************
c                        GETACCH_AH1.F
c*************************************************************************
c This subroutine calculates the 1st term of acceleration
c on the massive particles in the HELIOCENTRIC frame.
c             Input:
c                 nbod        ==>  number of massive bodies (int scalor)
c                 mass        ==>  mass of bodies (real array)
c                 xh,yh,zh    ==>  position in heliocentric coord (real array)
c                 xj,yj,zj    ==>  position in jacobi coord (real array)
c                 ir3h        ==> inv radii in heliocentric coord (real array)
c                 ir3j        ==> inv radii in jacobi coord (real array)
c             Output:
c                 axh1,ayh1,azh1 ==>  1st term acceleration in helio coord
c                                    (real array)
c
c Author:  Hal Levison
c Date:    2/2/93
c Last revision: 2/2/93

      subroutine getacch_ah1(nbod,mass,xh,yh,zh,xj,yj,zj,ir3h,ir3j,
     &                 axh1,ayh1,azh1)

      include '../../swift.inc'

c...  Inputs:
      integer nbod
      real*8 mass(nbod),ir3h(nbod),ir3j(nbod)
      real*8 xj(nbod),yj(nbod),zj(nbod)
      real*8 xh(nbod),yh(nbod),zh(nbod)

c...  Outputs:
      real*8 axh1(nbod),ayh1(nbod),azh1(nbod)

c...  Internals:
      integer i
      real*8 ah1h,ah1j

c----
c...  Executable code

      axh1(1) = 0.0
      ayh1(1) = 0.0
      azh1(1) = 0.0

      axh1(2) = 0.0     ! because xj=xh
      ayh1(2) = 0.0
      azh1(2) = 0.0

      do i=3,nbod

         ah1j = xj(i)*ir3j(i)
         ah1h = xh(i)*ir3h(i)
         axh1(i) = mass(1)*(ah1j - ah1h)

         ah1j = yj(i)*ir3j(i)
         ah1h = yh(i)*ir3h(i)
         ayh1(i) = mass(1)*(ah1j - ah1h)

         ah1j = zj(i)*ir3j(i)
         ah1h = zh(i)*ir3h(i)
         azh1(i) = mass(1)*(ah1j - ah1h)

      enddo

      return
      end   ! getacch_ah1

c---------------------------------------------------------------------


module define_fields
  use parameters
  use utils
  implicit none

#include "configure.h"

  type FRWmetric
     !!define the metric in synchronous gauge
     real(dl) a !!scale factor, a function of time
     real(dl) dx !!grid size, comoving dx
     real(dl) physdx !!physical dx
     real(dl) y,piy
 !!if using physical time: y = a^(3/2), piy = -8/3 dy/dt
 !!if using conformal time: y=a, piy = -6 dy/dt
  end type FRWmetric

  type simulation_parameters
     integer(IB) nsteps
     real(dl) time
     real(dl) dt,boxsize
     !!fields/gravity kinetic energy, gradient energy, and fields potential energy
     real(dl) fke,gke,fge,gge,pe
     !!the cach to temporarily store calculated metric g^{ij} and the derivatives of metric on three slices on x-y plane (in z direction indices=k-1,k,k+1)
#if METRIC_PERTURB
#if DIS_SCHEME == HLATTICE1
     real(dl) cach_dg(n,n,0:2)  !!\sqrt(g)
     real(dl) cach_dgup(6,n,n,0:2)  !!\sqrt(g)g^{ij}
     real(dl) cach_ddgup(6,6,n,n,0:2) !!\frac{\paritial \sqrt(g) g^{ij}}{\partial h_{ij}}
     integer cach_ind(-1:1)
#elif DIS_SCHEME == HLATTICE2
     real(dl) cach_dg(n,n,0:4)  !!\sqrt(g)
     real(dl) cach_dgup(6,n,n,0:4)  !!\sqrt(g)g^{ij}
     real(dl) cach_ddgup(6,6,n,n,0:4) !!\frac{\paritial \sqrt(g) g^{ij}}{\partial h_{ij}}
     integer cach_ind(-2:2)
#endif
#endif
  end type simulation_parameters

  Type(FRWmetric) metric
  type(simulation_parameters)::sipar
  real(dl),dimension(:,:,:,:),allocatable::fields_f
  real(dl),dimension(:,:,:,:),allocatable::fields_p
  real(dl),dimension(:,:,:,:),allocatable::metric_h
  real(dl),dimension(:,:,:,:),allocatable::metric_p
#if METRIC_OPTION == FRW_PERTURB_ADAPTIVE
  real(dl),dimension(:,:,:,:),allocatable::gauge_lambda
#endif

contains

!! input/output utils
   subroutine fields_dump()
     type(file_pointer) fp
     DEFINE_IND
     fp = open_file("data/"//trim(run_name)//"_fields.log","u")
     write(fp%unit) n
     write(fp%unit) ns
     do i=1,n
        write(fp%unit) fields_f(:,:,:,i)
        write(fp%unit) fields_p(:,:,:,i)
     enddo
     call close_file(fp)
   end subroutine fields_dump

   subroutine fields_load()
     type(file_pointer) fp
     DEFINE_IND
     fp = open_file("data/"//trim(run_name)//"_fields.log","u")
     read(fp%unit) i
     read(fp%unit) j
     if(i.ne.n .or. j.ne.ns) goto 100
     do i=1,n
        read(fp%unit) fields_f(:,:,:,i)
        read(fp%unit) fields_p(:,:,:,i)
     enddo
     call close_file(fp)
     write(*,*) "Fields configuration loaded."
     return
100  write(*,*) "Configuration does not match. Can not load the fields."
     stop
   end subroutine fields_load

   subroutine metric_dump_term(filename,term)
     character(LEN=*) filename
     type(file_pointer) fp
     integer(IB)::term
     DEFINE_IND
     fp = open_file(filename, 'u')
     select case(term)
     case(0)
        write(fp%unit) n
        write(fp%unit) metric%a
        write(fp%unit) metric%dx
        write(fp%unit) metric%physdx
        write(fp%unit) metric%y
        write(fp%unit) metric%piy
     case(1)
        write(fp%unit) n
        do i=1,n
           write(fp%unit) metric_h(:,:,:,i)
        enddo
     case(2)
        write(fp%unit) n
        do i=1,n
           write(fp%unit) metric_p(:,:,:,i)
        enddo
     end select
     call close_file(fp)
     return
   end subroutine metric_dump_term

   subroutine metric_load_term(filename,term)
     character(LEN=*) filename
     type(file_pointer) fp
     integer(IB) term
     DEFINE_IND
     fp = open_file(filename, 'u')
     read(fp%unit) i
     if(i.ne. n) goto 100
     select case(term)
     case(0)
        read(fp%unit) metric%a
        read(fp%unit) metric%dx
        read(fp%unit) metric%physdx
        read(fp%unit,END=100) metric%y
        read(fp%unit,END=100) metric%piy
     case(1)
        do i=1,n
           read(fp%unit) metric_h(:,:,:,i)
        enddo
     case(2)
        do i=1,n
           read(fp%unit) metric_p(:,:,:,i)
        enddo
     end select
     call close_file(fp)
     return
100  write(*,*) "Configuration does not match. Can not load the fields."
     stop
   end subroutine metric_load_term

   subroutine metric_dump()
     call metric_dump_term("data/"//trim(run_name)//"_metric_homo.log",0)
#if HIJ_DEFINED
     call metric_dump_term("data/"//trim(run_name)//"_metric_hij.log",1)
     call metric_dump_term("data/"//trim(run_name)//"_metric_pij.log",2)
#endif
   end subroutine metric_dump

   subroutine metric_load()
     call metric_load_term("data/"//trim(run_name)//"_metric_homo.log",0)
#if HIJ_DEFINED
     call metric_load_term("data/"//trim(run_name)//"_metric_hij.log",1)
     call metric_load_term("data/"//trim(run_name)//"_metric_pij.log",2)
     write(*,*) "metric loaded"
#endif
   end subroutine metric_load

   subroutine print_fields(f)
     real(dl) f(ns)
     character(LEN=32) fmt
     fmt = "("//trim(int2str(ns))//"G12.6)"
     write(*,fmt) f
   end subroutine print_fields

   subroutine deallocate_fields()
     if(allocated(fields_f)) deallocate(fields_f)
     if(allocated(fields_p)) deallocate(fields_p)
   end subroutine deallocate_fields

   subroutine allocate_fields()
     call deallocate_fields()
     allocate(fields_f(ns,n,n,n))
     allocate(fields_p(ns,n,n,n))
   end subroutine allocate_fields

   subroutine deallocate_metric_h()
     if(allocated(metric_h)) deallocate(metric_h)
   end subroutine deallocate_metric_h

   subroutine deallocate_metric_p()
     if(allocated(metric_p)) deallocate(metric_p)
   end subroutine deallocate_metric_p

   subroutine deallocate_metric()
     call deallocate_metric_h()
     call deallocate_metric_p()
   end subroutine deallocate_metric

   subroutine allocate_metric_h()
#if HIJ_DEFINED
     allocate(metric_h(6,n,n,n))
#endif
   end subroutine allocate_metric_h

   subroutine allocate_metric_p()
#if HIJ_DEFINED
     allocate(metric_p(6,n,n,n))
#endif
   end subroutine allocate_metric_p

   subroutine allocate_metric()
     call deallocate_metric()
#if HIJ_DEFINED
     call allocate_metric_h()
     call allocate_metric_p()
#endif
   end subroutine allocate_metric

end module define_fields


! File VersionID:
!   $Id: temperature.for 176 2010-03-13 11:36:14Z kroes006 $
! ----------------------------------------------------------------------
      subroutine temperature(task)
! ----------------------------------------------------------------------
!     date               : november 2004
!     purpose            : calculate soil temperatures
! ----------------------------------------------------------------------
      use variables
      implicit none

! --- global
      integer task
! --- local
      integer i,lay, ierror
      real*8  tmpold(macp),tab(mabbc*2),dummy,afgen,gmineral
      real*8  thoma(macp),thomb(macp),thomc(macp),thomf(macp)
      real*8  theave(macp),heacap(macp),heacnd(macp),heacon(macp)
      real*8  heaconbot,qhbot
      real*8  apar, dzsnw, heaconsnw, Rosnw

      character messag*200

      save
! ----------------------------------------------------------------------

      goto (1000,2000) task

1000  continue

! === initialization ===================================================

! --- determine initial temperature profile

      if (swcalt.eq.1) then
! ---   analytical solution ---
        do i = 1,numnod
          tsoil(i) = tmean + tampli*(sin(0.0172d0*(daynr-timref+91.0d0)+
     &              z(i)/ddamp)) / exp(-z(i)/ddamp)
        enddo
      else
! ---   numerical solution, use specified soil temperatures ---
        if (swinco.ne.3) then
          do i = 1, nheat
            tab(i*2) = tsoil(i)
            tab(i*2-1) = abs(zh(i))
          end do
          do i = 1, numnod
            tsoil(i) = afgen(tab,macp*2,abs(z(i)))
          end do
        end if
      endif

      if (swcalt.eq.2) then
! ---   initialize dry bulk density and volume fractions sand, clay and organic matter
        do i = 1, numnod
          lay = layer(i)
          dummy = orgmat(lay)/(1.0d0 - orgmat(lay))
          gmineral = (1.0d0 - thetas(i)) / (0.370d0 + 0.714d0*dummy)
          fquartz(i) = (psand(lay) + psilt(lay))*gmineral/2.7d0
          fclay(i) = pclay(lay)*gmineral/2.7d0
          forg(i) = dummy*gmineral/1.4d0
        end do

      endif

      return

2000  continue

! === soil temperature rate and state variables ========================

      if (swcalt .eq. 2) then
!   - numerical solution

! ---   use specified soil surface temperatures as top boundary condition
        if (swtopbhea .eq. 2) then
! ---   use specified soil surface temperatures as top boundary condition
           TeTop = afgen (temtoptab,2*mabbc,t1900+dt)
        elseif (abs(ssnow).gt.1.0d-10) then
! --- air temperature can not be used with a snow layer, calculate
! --- temperature on soil- snow interface
           Rosnw = 170.0d0
           heaconsnw = 2.86d-6 * 864.0d0 * Rosnw**2.d0
           dzsnw = ssnow / 0.170d0
           if (heacon(1).lt.1.d-10) heacon(1) = 100.0d0
           apar = (0.5d0*heaconsnw*dz(1)) / (heacon(1)*dzsnw)
           TeTop = (Tsoil(1) + apar*Tav) / (1.d0+apar)
        else
           TeTop = Tav
        endif
!
        if (SwBotbHea.eq.1) then
! ---   no heat flow through bottom of profile assumed
           TeBot = Tsoil(Numnod)
        elseif (SwBotbHea.eq.2) then
! ---   bottom temperature is prescribed
           TeBot = afgen (tembtab,2*mabbc,t1900+dt)
        endif

! --- save old temperature profile
        do i = 1,numnod
          tmpold(i) = tsoil(i)
        enddo

! --- compute heat conductivity and capacity ---------------------------

        do i = 1,numnod
          theave(i) = 0.5d0 * (theta(i) + thetm1(i))
        enddo

! --- calculate nodal heat capacity and thermal conductivity
        call devries(numnod,theave,thetas,heacap,heacnd,fquartz,
     &        fclay,forg)
        heacon(1) = heacnd(1)
        do i = 2,numnod
          heacon(i) = 0.5d0 * (heacnd(i) + heacnd(i-1))
        enddo

! ---   calculate new temperature profile --------------------------------

! ---   calculation of coefficients for node = 1
        i = 1
! ---   temperature fixed at soil surface
        thoma(i) = - dt * heacon(i) / (dz(i) * disnod(i))
        thomc(i) = - dt * heacon(i+1) / (dz(i) * disnod(i+1))
        thomb(i) = heacap(i) - thoma(i) - thomc(i)
        thomf(i) = heacap(i) * tmpold(i) - thoma(i) * TeTop

! ---   calculation of coefficients for 2 < node < numnod
        do i = 2,numnod-1
          thoma(i) = - dt * heacon(i) / (dz(i) * disnod(i))
          thomc(i) = - dt * heacon(i+1) / (dz(i) * disnod(i+1))
          thomb(i) = heacap(i) - thoma(i) - thomc(i)
          thomf(i) = heacap(i) * tmpold(i)
        enddo

! ---   calculation of coefficients for node = numnod
        i = numnod
        if (SwBotbHea.eq.1) then
! ---   no heat flow through bottom of profile assumed
           qhbot = 0.0d0
           thoma(i) = - dt * heacon(i) / (dz(i) * disnod(i))
           thomb(i) = heacap(i) - thoma(i)
           thomf(i) = heacap(i) * tmpold(i) - (qhbot * dt)/dz(i)
        elseif (SwBotbHea.eq.2) then
! ---   bottom temperature is prescribed
           heaconBot = heacnd(i)
           thoma(i)  = - dt * heacon(i) / (dz(i) * disnod(i))
           thomc(i)  = - dt * heaconBot / (dz(i) * 0.5d0 * dz(i))
           thomb(i)  = heacap(i) - thoma(i) - thomc(i)
           thomf(i)  = heacap(i) * tmpold(i) - thomc(i) * TeBot
        endif

! ---   solve for vector tsoil a tridiagonal linear set
        call tridag (numnod, thoma, thomb, thomc, thomf, tsoil,ierror)
        if(ierror.ne.0)then
           messag =
     &       'During a call from Temperature an error occured in TriDag'
           call fatalerr ('Temperature',messag)
        end if
      else

! ---   analytical solution temperature profile ---
        do i = 1,numnod
          tsoil(i) = tmean + tampli*(sin(0.0172d0*(daynr-timref+91.0d0)+
     &              z(i)/ddamp)) / exp(-z(i)/ddamp)
        enddo

      endif

      return
      end

!***********************************************************************
      Subroutine Devries (NumNod,theta,THETAS,HeaCap,HeaCon,FQUARTZ,
     &           FCLAY,FORG)
!***********************************************************************
!* Purpose:    Calculate soil heat capacity and conductivity for each  *
!*             compartment by full de Vries model                      *
!* References:                                                         *
!* Description:                                                        *
!* de Vries model for soil heat capcity and thermal conductivity.      *
!* Heat capacity is calculated as average of heat capacities for each  *
!* soil component. Thermal conductivity is calculated as weighted      *
!* average of conductivities for each component. If theta > 0.05 liquid*
!* water is assumed to be the main transport medium in calculating the *
!* weights. If theta < 0.02 air is assumed to be the main transport    *
!* medium (there is also an empirical adjustment to the conductivity). *
!* For 0.02 < theta < 0.05 conductivity is interpolated.               *
!* See: Heat and water transfer at the bare soil surface, H.F.M Ten    *
!* Berge (pp 48-54 and Appendix 2)                                     *
!***********************************************************************
!* Input:                                                              *
!* NumNod - number of compartments (-)                                 *
!* theta/THETAS - volumetric soil moisture/ saturated vol. s. moist (-)*
!* Fquartz, Fclay and Forg - volume fractions of sand, clay and org.ma.*
!* Output:                                                             *
!* HeaCap - heat capacity (J/m3/K)                                     *
!* HeaCon - thermal conductivity (W/m/K)                               *
!***********************************************************************
      Implicit None
      Include 'arrays.fi'

!     (i) Global declarations
!     (i.i) Input
      Integer NumNod
      real*8 theta(1:MACP),THETAS(1:MACP)
!     (i.ii)
      real*8 HeaCap(MACP), HeaCon(MACP)
!     (ii) Local declarations
      Integer Node
      real*8 kqa,kca,kwa,koa,kaa,kqw,kcw,kww,kow,kaw
      Parameter (kaa = 1.0d0, kww = 1.0d0)
      real*8 fAir(MACP),fClay(MACP),fQuartz(MACP),fOrg(MACP)
      real*8 HeaConDry,HeaConWet
!     (iii) Parameter declarations
!     (iii.i) Physical constants
!     Specific heats (J/kg/K)
      real*8  cQuartz,cClay,cWat,cAir,cOrg
      Parameter (cQuartz = 800.0d0, cClay = 900.0d0, cWat = 4180.0d0,
     &           cAir = 1010.0d0, cOrg = 1920.0d0)
!     Density (kg/m3)
      real*8 dQuartz, dClay, dWat, dAir, dOrg
      Parameter (dQuartz = 2660.0d0, dClay = 2650.0d0, dWat = 1000.0d0,
     &           dAir = 1.2d0, dOrg = 1300.0d0)
!     Thermal conductivities (W/m/K)
      real*8  kQuartz,kClay,kWat,kAir,kOrg
      Parameter (kQuartz = 8.8d0, kClay = 2.92d0, kWat = 0.57d0,
     &           kAir = 0.025d0, kOrg = 0.25d0)
!
      real*8  GQuartz,GClay,GWat,GAir,GOrg
      Parameter (GQuartz = 0.14d0, GClay = 0.0d0 , GWat = 0.14d0,
     &           GAir = 0.2d0, GOrg = 0.5d0)
!     (iii.ii) theta 0.02, 0.05 and 1.00
      real*8thetaDry,thetaWet
      Parameter (thetaDry = 0.02d0, thetaWet = 0.05d0)
! ----------------------------------------------------------------------
!     (0) Weights for each component in conductivity calculations
!     (Calculate these but define as parameters in later version)
      kaw = 0.66d0 / (1.0d0 + ((kAir/kWat) - 1.0d0) * GAir) + 0.33d0 /
     &    (1.0d0 + ((kAir/kWat) - 1.0d0) * (1.0d0 - 2.0d0 * GAir))
      kqw = 0.66d0 / (1.0d0 + ((kQuartz/kWat)-1.0d0) * GQuartz)+0.33d0 /
     &    (1.0d0 + ((kQuartz/kWat) - 1.0d0) * (1.0d0 - 2.0d0 * GQuartz))
      kcw = 0.66d0 / (1.0d0 + ((kClay/kWat) - 1.0d0) * GClay) + 0.33d0 /
     &    (1.0d0 + ((kClay/kWat) - 1.0d0) * (1.0d0 - 2.0d0 * GClay))
      kow = 0.66d0 / (1.0d0 + ((kOrg/kWat) - 1.0d0) * GOrg) + 0.33d0 /
     &    (1.0d0 + ((kOrg/kWat) - 1.0d0) * (1.0d0 - 2.0d0 * GOrg))
      kwa = 0.66d0 / (1.0d0 + ((kWat/kAir) - 1.0d0) * GWat) + 0.33d0 /
     &    (1.0d0 + ((kWat/kAir) - 1.0d0) * (1.0d0 - 2.0d0 * GWat))
      kqa = 0.66d0 / (1.0d0 + ((kQuartz/kAir)-1.0d0) * GQuartz)+0.33d0 /
     &    (1.0d0 + ((kQuartz/kAir) - 1.0d0) * (1.0d0 - 2.0d0 * GQuartz))
      kca = 0.66d0 / (1.0d0 + ((kClay/kAir) - 1.0d0) * GClay) + 0.33d0 /
     &    (1.0d0 + ((kClay/kAir) - 1.0d0) * (1.0d0 - 2.0d0 * GClay))
      koa = 0.66d0 / (1.0d0 + ((kOrg/kAir) - 1.0d0) * GOrg) + 0.33d0 /
     &    (1.0d0 + ((kOrg/kAir) - 1.0d0) * (1.0d0 - 2.0d0 * GOrg))

      Do Node = 1,NumNod

!        (1) Air fraction
         fAir(Node) = THETAS(Node) - theta(Node)

!        (2) Heat capacity (W/m3/K) is average of heat capacities for
!        all components (multiplied by density for correct units)
         HeaCap(Node) = fQuartz(Node)*dQuartz*cQuartz + fClay(Node)*
     &                  dClay*cClay + theta(Node)*dWat*cWat +
     &                  fAir(Node)*dAir*cAir +
     &                  fOrg(Node)*dOrg*cOrg

!        (3) Thermal conductivity (W/m/K) is weighted average of
!        conductivities of all components
!        (3.1) Dry conditions (include empirical correction) (eq. 3.44)
         If (theta(Node).LE.thetaDry) Then
            HeaCon(Node) = 1.25d0 * (kqa*fQuartz(Node)*kQuartz +
     &                      kca*fClay(Node)*kClay +
     &                      kaa*fAir(Node)*kAir +
     &                      koa*fOrg(Node)*kOrg +
     &                      kwa*theta(Node)*kWat) /
     &   (kqa * fQuartz(Node) + kca * fClay(Node) + kaa * fAir(Node) +
     &    koa * fOrg(Node) + kwa * theta(Node))

!        (3.2) Wet conditions  (eq. 3.43)
         Else If (theta(Node).GE.thetaWet) Then
            HeaCon(Node) = (kqw*fQuartz(Node)*kQuartz +
     &                      kcw*fClay(Node)*kClay +
     &                      kaw*fAir(Node)*kAir +
     &                      kow*fOrg(Node)*kOrg +
     &                      kww*theta(Node)*kWat) /
     &   (kqw * fQuartz(Node) + kcw * fClay(Node) + kaw * fAir(Node) +
     &    kow * fOrg(Node) + kww * theta(Node))

!        (3.3) dry < theta < wet (interpolate)
         Else
!           (3.3.1) Conductivity for theta = 0.02
            HeaConDry = 1.25d0 * (kqa*fQuartz(Node)*kQuartz +
     &                      kca*fClay(Node)*kClay +
     &                      kaa*fAir(Node)*kAir +
     &                      koa*fOrg(Node)*kOrg +
     &                      kwa*thetaDry*kWat) /
     &   (kqa * fQuartz(Node) + kca * fClay(Node) + kaa * fAir(Node) +
     &    koa * fOrg(Node) + kwa * thetaDry)
!           (3.3.1) Conductivity for theta = 0.05
            HeaConWet = (kqw*fQuartz(Node)*kQuartz +
     &                      kcw*fClay(Node)*kClay +
     &                      kaw*fAir(Node)*kAir +
     &                      kow*fOrg(Node)*kOrg +
     &                      kww*thetaWet*kWat) /
     &   (kqw * fQuartz(Node) + kcw * fClay(Node) + kaw * fAir(Node) +
     &    kow * fOrg(Node) + kww * thetaWet)
!         (3.3.3) Interpolate
          HeaCon(Node) = HeaConDry + (theta(Node)-thetaDry) *
     &                   (HeaConWet - HeaConDry)
     &                 / (thetaWet - thetaDry)
         End If

! ---    conversion of capacity from J/m3/K to J/cm3/K
         HEACAP(NODE) = HEACAP(NODE)*1.0d-6

! ---    conversion of conductivity from W/m/K to J/cm/K/d
         HEACON(NODE) = HEACON(NODE)*864.0d0

      End Do

      Return
      End


      SUBROUTINE LAND_MODEL_INIT (fmask_l,alb0)
C--
C--   SUBROUTINE LAND_MODEL_INIT (fmask_l,alb0)
C--
C--   Purpose : Initialization of land model
C--   Initialized common blocks: LAND_MC
C--

      include "atparam.h"

      PARAMETER ( NLON=IX, NLAT=IL, NGP=NLON*NLAT )

C     Input variables

      real fmask_l(nlon,nlat)            ! land mask (fraction of land)
      real    alb0(nlon,nlat)            ! annual-mean albedo

      include "com_cplcon_land.h"

C     Auxiliary variables

      real    dmask(nlon,nlat)           ! domain mask

C--
C--   1. Set heat capacities and dissipation times for
C--      soil and ice-sheet layers

C     Model parameters (default values)

C     soil layer depth (m)
      depth_soil = 1.0

C     land-ice depth (m)
      depth_lice = 5.0

C     Dissipation time (days) for land-surface temp. anomalies
      tdland  = 40.

C     Minimum fraction of land for the definition of anomalies
      flandmin = 1./3.

C     Reset model parameters

      include "cls_inland.h"

C     Heat capacities per m^2 (depth*heat_cap/m^3)
      hcapl  = depth_soil*2.50e+6
      hcapli = depth_lice*1.93e+6

C--
C--   2. Compute constant fields

C     Set domain mask (blank out sea points)

      dmask(:,:) = 1.

      do j=1,nlat
        do i=1,nlon
           if (fmask_l(i,j).lt.flandmin) dmask(i,j) = 0
        enddo
      enddo

C     Set time_step/heat_capacity and dissipation fields

      do j=1,nlat
        do i=1,nlon
           if (alb0(i,j).lt.0.4) then
              rhcapl(i,j) = 86400./hcapl
           else
              rhcapl(i,j) = 86400./hcapli
           endif
        enddo
      enddo

      cdland(:,:) = dmask(:,:)*tdland/(1.+dmask(:,:)*tdland)

      return
      end

      SUBROUTINE LAND_MODEL
C--
C--   SUBROUTINE LAND_MODEL
C--
C--   Purpose : Integrate slab land-surface model for one day

      include "atparam.h"

      PARAMETER ( NLON=IX, NLAT=IL, NGP=NLON*NLAT )

c      real vland_input(nlon,nlat,3), vland_output(nlon,nlat,2)

      include "com_cplcon_land.h"

      include "com_cplvar_land.h"

C     Input variables:

      real   stl0(nlon,nlat)    ! land temp. at initial time
      real hfland(nlon,nlat)    ! land sfc. heat flux between t0 and t1
      real stlcl1(nlon,nlat)    ! clim. land temp. at final time

      equivalence    (stl0,vland_input(1,1))
      equivalence  (hfland,vland_input(1,2))
      equivalence  (stlcl1,vland_input(1,3))

C     Output variables

      real  stl1(nlon,nlat)     ! land temp. at final time

      equivalence  (stl1,vland_output(1,1))

C     Auxiliary variables

      real hflux(nlon,nlat)   ! net sfc. heat flux
      real tanom(nlon,nlat)   ! sfc. temperature anomaly

C--
C--   1. Land-surface (soil/ice-sheet) layer

C     Net heat flux
c     (snow correction to be added?)
      hflux(:,:) = hfland(:,:)

C     Anomaly w.r.t final-time climatological temp.
      tanom(:,:) = stl0(:,:) - stlcl1(:,:)

C     Time evoloution of temp. anomaly
      tanom(:,:) = cdland(:,:)*(tanom(:,:)+rhcapl(:,:)*hflux(:,:))

C     Full SST at final time
      stl1(:,:) = tanom(:,:) + stlcl1(:,:)

      return
      end


!==========================================================================
elemental function gsw_internal_energy_ice (t, p)
!==========================================================================
!
!  Calculates the specific internal energy of ice.
!
!  t  =  in-situ temperature (ITS-90)                             [ deg C ]
!  p  =  sea pressure                                              [ dbar ]
!         ( i.e. absolute pressure - 10.1325 dbar )
!
!  internal_energy_ice  =  specific internal energy (u)              [J/kg]
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_gibbs_ice

use gsw_mod_teos10_constants, only : gsw_p0, gsw_t0, db2pa

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: t, p

real (r8) :: gsw_internal_energy_ice

gsw_internal_energy_ice = gsw_gibbs_ice(0,0,t,p) &
                          - (gsw_t0 + t)*gsw_gibbs_ice(1,0,t,p) &
                          - (db2pa*p + gsw_p0)*gsw_gibbs_ice(0,1,t,p)

return
end function

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


!
!  Copyright 2016 ARTED developers
!
!  Licensed under the Apache License, Version 2.0 (the "License");
!  you may not use this file except in compliance with the License.
!  You may obtain a copy of the License at
!
!      http://www.apache.org/licenses/LICENSE-2.0
!
!  Unless required by applicable law or agreed to in writing, software
!  distributed under the License is distributed on an "AS IS" BASIS,
!  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
!  See the License for the specific language governing permissions and
!  limitations under the License.
!
subroutine PSE_Conjugate_Gradient(iter_cg_max)
  use global_variables
  implicit none
  real(8),parameter :: delta_cg=1d-15
  integer :: iter_cg,iter_cg_max
  real(8) :: xkHxk,gkgk,pkHpk
  real(8) :: s,uk,ev
  complex(8) :: cx,cp,xkHpk
  complex(8) :: zs
  integer :: ik,ib,ibt
  real(8) :: esp_var_l(NB,NK)

  esp_var_l=0d0

  call PSE_pre_hpsi

  K_points :do ik=NK_s,NK_e
  Band :    do ib=1,NB

    do ibt=1,ib-1
      zs=sum(conjg(zu_GS(:,ibt,ik))*zu_GS(:,ib,ik))*H123
      zu_GS(1:NL,ib,ik)=zu_GS(1:NL,ib,ik)-zu_GS(1:NL,ibt,ik)*zs
    end do
    s=sum(abs(zu_GS(:,ib,ik))**2)*H123
    xk(1:NL)=zu_GS(1:NL,ib,ik)/sqrt(s)
    tpsi(:)=xk(:)
    call PSE_hpsi(ik)
    hxk(:)=htpsi(:)
    xkHxk=sum(conjg(xk(:))*hxk(:))*H123

    do iter_cg=1,iter_cg_max
      gk(:)=hxk(:)-xkHxk*xk(:)
      do ibt=1,ib-1
        zs=sum(conjg(zu_GS(:,ibt,ik))*gk(:))*H123
        gk(1:NL)=gk(1:NL)-zu_GS(1:NL,ibt,ik)*zs
      end do
      s=sum(abs(gk(:))**2)*H123
      select case (iter_cg)
      case(1)
        pk(1:NL)=gk(1:NL)
      case default
        uk=s/gkgk
        pk(1:NL)=gk(1:NL)+uk*pk(1:NL)
      end select
      gkgk=s
      zs=sum(conjg(xk(:))*pk(:))*H123
      pko(1:NL)=pk(1:NL)-xk(1:NL)*zs
      s=sum(abs(pko(:))**2)*H123
      pko(1:NL)=pko(1:NL)/sqrt(s)
      tpsi(1:NL)=pko(1:NL); call PSE_hpsi(ik)
      xkHpk=sum(conjg(xk(:))*htpsi(:))*H123
      pkHpk=sum(conjg(pko(:))*htpsi(:))*H123
      ev=0.5d0*((xkHxk+pkHpk)-sqrt((xkHxk-pkHpk)**2+4*abs(xkHpk)**2))
      cx=xkHpk/(ev-xkHxk)
      cp=1.d0/sqrt(1.d0+abs(cx)**2)
      cx=cx*cp
      if(abs(ev-xkHxk)<delta_cg) exit
      xk(1:NL)=cx*xk(1:NL)+cp*pko(1:NL)
      hxk(1:NL)=cx*hxk(1:NL)+cp*htpsi(1:NL)
      xkHxk=sum(conjg(xk(:))*hxk(:))*H123
    end do
      s=sum(abs(xk(:))**2)*H123
      zu_GS(1:NL,ib,ik)=xk(1:NL)/sqrt(s)
      tpsi(1:NL)=zu_GS(1:NL,ib,ik); call PSE_hpsi(ik)
      xkHxk=sum(conjg(tpsi(:))*htpsi(:))*H123
      esp_var_l(ib,ik)=sqrt(sum(abs(htpsi(:)-xkHxk*tpsi(:))**2)*H123)*occ(ib,ik)
  end do Band
  end do K_points

  call MPI_ALLREDUCE(esp_var_l,esp_var,NB*NK,MPI_REAL8,MPI_SUM,MPI_COMM_WORLD,ierr)

  return
end subroutine PSE_Conjugate_Gradient