module mod_rgrd4u
    use mod_rgrd3u
    implicit none

contains
    !

    ! Code converted using TO_F90 by Alan Miller
    ! Date: 2013-09-18  Time: 12:23:37

    ! ... file rgrd4u.f

    !     this file contains documentation for subroutine rgrd4u followed by
    !     fortran code for rgrid4u and additional subroutines.

    ! ... author

    !     John C. Adams (NCAR 1999)

    ! ... subroutine rgrd4u(nx,ny,nz,nt,p,mx,my,mz,mt,q,intpol,w,lw,iw,liw,ier)

    ! ... purpose

    !     subroutine rgrd4u interpolates the nx by ny by nz by nt array p onto
    !     the mx by my by mz by mt array q.  it is assumed that p and q are
    !     values on uniform nx by ny by nz by nt and mx by my by mz by mt grids
    !     which are superimposed on the same box region (INCLUDING BOUNDARIES).
    !     if p and q are values on nonuniform orthogonal grids and/or if the grid
    !     on which q is defined lies within the p grid then subroutine rgrd4
    !     (see file rgrd4.f) should be used.

    ! ... language

    !     coded in portable FORTRAN77 and FORTRAN90

    ! ... test program

    !     file trgrd4u.f on regridpack includes a test program for subroutine rgrd4u

    ! ... method

    !     linear or cubic interpolation (see intpol) is used in each
    !     direction for which the q grid is not a subgrid of the p grid.
    !     [the mx (my,mz,mt) uniform grid is a subgrid of the nx (ny,nz,nt)
    !     uniform grid if and only if mx-1 (my-1,nz-1,nt-1) divides nx-1
    !     (ny-1,nz-1,nt-1)].  Values are set directly without (the need for)
    !     interpolation in subgrid directions.

    ! ... required files

    !     files rgrd3u.f,rgrd2u.f and rgrd1u.f must be loaded with rgrd4u.f.  they
    !     include subroutines called by the routines in rgrd4u.f

    ! ... efficiency

    !     inner most loops in regridpack software vectorize.  If the
    !     integer arguments mx,my,mz,mt (see below) have different values,
    !     optimal vectorization will be achieved if they are arranged so that
    !     mx > my > mz > mt.

    ! *** input arguments ***

    ! ... nx

    !     the integer first dimension of p.  nx > 1 if intpol(1) = 1 or
    !     nx > 3 if intpol(1) = 3 is required (see ier = 2).

    ! ... ny

    !     the integer second dimension of p.  ny > 1 if intpol(2) = 1 or
    !     ny > 3 if intpol(2) = 3 is required (see ier = 2).

    ! ... nz

    !     the integer third dimension of p.  nz > 1 if intpol(3) = 1 or
    !     nz > 3 if intpol(3) = 3 is required (see ier = 2)

    ! ... nt

    !     the integer fourth dimension of p.  nt > 1 if intpol(4) = 1 or
    !     nt > 3 if intpol(4) = 3 is required (see ier=2)

    ! ... p

    !     a real (kind=8) nx by ny by nz by nt array of given values

    ! ... mx

    !     the integer first dimension of q.  mx > 1 is required (see ier = 1)

    ! ... my

    !     the integer second dimension of q. my > 1 is required (see ier = 1)

    ! ... mz

    !     the integer third dimension of q. mz > 1 is required (see ier = 1)

    ! ... mt

    !     the integer fourth dimension of q. mt > 1 is required (see ier = 1)

    ! ... intpol

    !     an integer vector of dimension 4 which sets linear or cubic
    !     interpolation in each of the x,y,z,t directions as follows:

    !        intpol(1) = 1 sets linear interpolation in the x direction
    !        intpol(1) = 3 sets cubic interpolation in the x direction.

    !        intpol(2) = 1 sets linear interpolation in the y direction
    !        intpol(2) = 3 sets cubic interpolation in the y direction.

    !        intpol(3) = 1 sets linear interpolation in the z direction
    !        intpol(3) = 3 sets cubic interpolation in the z direction.

    !        intpol(4) = 1 sets linear interpolation in the t direction
    !        intpol(4) = 3 sets cubic interpolation in the t direction.

    !     values other than 1 or 3 in intpol are not allowed (ier = 3).

    ! ... w

    !     a real (kind=8) work space of length at least lw which must be provided in the
    !     routine calling rgrd4u

    ! ... lw

    !     the integer length of the work space w.

    !          let lwx = 1 if mx-1 divides nx-1; otherwise
    !          let lwx = mx if intpol(1) = 1 or
    !          let lwx = 4*mx if intpol(1) = 3

    !          let lwy = 0 if my-1 divides ny-1; otherwise
    !          let lwy = my+2*mx if intpol(2) = 1 or
    !          let lwy = 4*(mx+my) if intpol(2) = 3

    !          let lwz = 0 if mz-1 divides nz-1; otherwise
    !          let lwz = 2*mx*my+mz if intpol(3) = 1 or
    !          let lwz = 4*(mx*my+mz) if intpol(3) = 3

    !          let lwt = 0 if mt-1 divides nt-1; otherwise
    !          let lwt = 2*mx*my*mz+mt if intpol(4) = 1 or
    !          let lwt = 4*(mx*my*mz+mt) if intpol(4) = 3

    !     then lw must be greater than or equal to lwx+lwy+lwz+lwt

    ! ... iw

    !     an integer work space of length at least liw which must be provided in the
    !     routine calling rgrd4u

    ! ... liw

    !     the integer length of the integer work space iw.  liw must be at least
    !     mx+my+mz+mt

    ! *** output arguments ***

    ! ... q

    !     a real (kind=8) mx by my by mz by mt array of values which are interpolated
    !     from p.

    ! ... ier

    !     an integer error flag set as follows:

    !     ier = 0 if no errors in input arguments are detected

    !     ier = 1 if  min0(mx,my,mz,mt) < 2

    !     ier = 2 if nx < 2 when intpol(1)=1 or nx < 4 when intpol(1)=3 (or)
    !                ny < 2 when intpol(2)=1 or ny < 4 when intpol(2)=3 (or)
    !                nz < 2 when intpol(3)=1 or nz < 4 when intpol(3)=3 (or)
    !                nt < 2 when intpol(4)=1 or nt < 4 when intpol(4)=3.

    !     ier = 3 if any of intpol(1),intpol(2),intpol(3),intpol(4)  is not
    !             equal to 1 or 3.

    !     ier = 4 if lw or liw is too small (insufficient work space)

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

    !     end of rgrd4u documentation, fortran code follows:

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

    SUBROUTINE rgrd4u(nx,ny,nz,nt,p,mx,my,mz,mt,q,intpol, w,lw,iw,liw,ier)

    IMPLICIT NONE
    INTEGER, INTENT(IN)                      :: nx
    INTEGER, INTENT(IN)                      :: ny
    INTEGER, INTENT(IN)                      :: nz
    INTEGER, INTENT(IN)                      :: nt
    real (kind=8), INTENT(IN)                :: p(nx,ny,nz,nt)
    INTEGER, INTENT(IN)                      :: mx
    INTEGER, INTENT(IN)                      :: my
    INTEGER, INTENT(IN)                      :: mz
    INTEGER, INTENT(IN)                      :: mt
    real (kind=8), INTENT(OUT)               :: q(mx,my,mz,mt)
    INTEGER, INTENT(IN)                      :: intpol(4)
    real (kind=8), INTENT(INOUT)             :: w(lw)
    INTEGER, INTENT(IN)                      :: lw
    INTEGER, INTENT(IN OUT)                  :: iw(liw)
    INTEGER, INTENT(IN)                      :: liw
    INTEGER, INTENT(OUT)                     :: ier

    INTEGER :: inmx,jnmy,knmz,lnmt,isubx,jsuby,ksubz,lsubt
    INTEGER :: mxmy,mxmymz,lwx,lwy,lwz,lwt,jy,kz,LT
    INTEGER :: i2,i3,i4,i5
    INTEGER :: j2,j3,j4,j5,j6,j7,j8,j9
    INTEGER :: k2,k3,k4,k5,k6,k7,k8,k9
    INTEGER :: l2,l3,l4,l5,l6,l7,l8,l9

    !     check input arguments

    ier = 1

    !     check mx,my,mz,mt

    IF (MIN0(mx,my,mz,mt) < 1) RETURN

    !     check intpol

    ier = 3
    IF (intpol(1) /= 1 .AND. intpol(1) /= 3) RETURN
    IF (intpol(2) /= 1 .AND. intpol(2) /= 3) RETURN
    IF (intpol(3) /= 1 .AND. intpol(3) /= 3) RETURN
    IF (intpol(4) /= 1 .AND. intpol(4) /= 3) RETURN

    !     check nx,ny,nz,nt

    ier = 2
    IF (intpol(1) == 1 .AND. nx < 2) RETURN
    IF (intpol(1) == 3 .AND. nx < 4) RETURN
    IF (intpol(2) == 1 .AND. ny < 2) RETURN
    IF (intpol(2) == 3 .AND. ny < 4) RETURN
    IF (intpol(3) == 1 .AND. nz < 2) RETURN
    IF (intpol(3) == 3 .AND. nz < 4) RETURN
    IF (intpol(4) == 1 .AND. nt < 2) RETURN
    IF (intpol(4) == 3 .AND. nt < 4) RETURN

    !     set subgrid indicators

    inmx = (nx-1)/(mx-1)
    jnmy = (ny-1)/(my-1)
    knmz = (nz-1)/(mz-1)
    lnmt = (nt-1)/(mt-1)
    isubx = nx - inmx*(mx-1)
    jsuby = ny - jnmy*(my-1)
    ksubz = nz - knmz*(mz-1)
    lsubt = nt - lnmt*(mt-1)

    !     check work space length input

    ier = 4
    mxmy = mx*my
    mxmymz = mxmy*mz
    lwx = 1
    IF (isubx /= 1) THEN
      IF (intpol(1) == 1) THEN
        lwx = mx
      ELSE
        lwx = 4*mx
      END IF
    END IF
    lwy = 0
    IF (jsuby /= 1) THEN
      IF (intpol(2) == 1) THEN
        lwy = (2*mx+my)
      ELSE
        lwy = 4*my+4*mx
      END IF
    END IF
    lwz = 0
    IF (ksubz /= 1) THEN
      IF (intpol(3) == 1) THEN
        lwz = (2*mxmy+mz)
      ELSE
        lwz = 4*mxmy+4*mz
      END IF
    END IF
    lwt = 0
    IF (lsubt /= 1) THEN
      IF (intpol(4) == 1) THEN
        lwt = (2*mxmymz+mt)
      ELSE
        lwt = 4*mxmymz+4*mt
      END IF
    END IF

    IF (lw < lwx+lwy+lwz+lwt) RETURN
    IF (liw < mx+my+mz+mt) RETURN

    !     arguments o.k.

    ier = 0
    jy = mx+1
    kz = mx+my+1
    LT = mx+my+mz+1

    IF (intpol(4) == 1) THEN

    !     linearly interpolate in nt, set work space pointers and scales

      l2 = 1
      l3 = l2
      l4 = l3+mt
      l5 = l4
      l6 = l5
      l7 = l6
      l8 = l7+mxmymz
      l9 = l8+mxmymz
      CALL linmxu(nt,mt,iw(LT),w(l3))
      k2 = l9
      IF (intpol(3) == 1) THEN
    !     linear in z
        k3 = k2
        k4 = k3+mz
        k5 = k4
        k6 = k5
        k7 = k6
        k8 = k7+mxmy
        k9 = k8+mxmy
        CALL linmxu(nz,mz,iw(kz),w(k3))
        j2 = k9
      ELSE
    !     cubic in z
        k3 = k2+mz
        k4 = k3+mz
        k5 = k4+mz
        k6 = k5+mz
        k7 = k6+mxmy
        k8 = k7+mxmy
        k9 = k8+mxmy
        CALL cubnmxu(nz,mz,iw(kz),w(k2),w(k3),w(k4),w(k5))
        j2 = k9+mxmy
      END IF

      IF (intpol(2) == 1) THEN
    !     linear in y
        j3 = j2
        j4 = j3+my
        j5 = j4
        j6 = j5
        j7 = j6
        j8 = j7+mx
        j9 = j8+mx
        CALL linmxu(ny,my,iw(jy),w(j3))
        i2 = j9
      ELSE
    !     cubic in y
        j3 = j2+my
        j4 = j3+my
        j5 = j4+my
        j6 = j5+my
        j7 = j6+mx
        j8 = j7+mx
        j9 = j8+mx
        CALL cubnmxu(ny,my,iw(jy),w(j2),w(j3),w(j4),w(j5))
        i2 = j9+mx
      END IF

      IF (intpol(1) == 1) THEN
    !     linear in x
        i3 = i2
        i4 = i3
        i5 = i4
        CALL linmxu(nx,mx,iw,w(i3))
      ELSE
    !     cubic in x
        i3 = i2+mx
        i4 = i3+mx
        i5 = i4+mx
        CALL cubnmxu(nx,mx,iw,w(i2),w(i3),w(i4),w(i5))
      END IF

    !     linearly interpolate in t

      CALL lint4u(nx,ny,nz,nt,p,mx,my,mz,mt,mxmy,mxmymz,q,intpol,  &
          iw(LT),w(l3),w(l7),w(l8),  &
          iw(kz),w(k2),w(k3),w(k4),w(k5),w(k6),w(k7),w(k8),w(k9),  &
          iw(jy),w(j2),w(j3),w(j4),w(j5),w(j6),w(j7),w(j8),w(j9),  &
          iw,w(i2),w(i3),w(i4),w(i5), inmx,jnmy,knmz,lnmt,isubx,jsuby,ksubz,lsubt)
      RETURN

    ELSE

    !     cubically interpolate in t

      l2 = 1
      l3 = l2+mt
      l4 = l3+mt
      l5 = l4+mt
      l6 = l5+mt
      l7 = l6+mxmymz
      l8 = l7+mxmymz
      l9 = l8+mxmymz
      CALL cubnmxu(nt,mt,iw(LT),w(l2),w(l3),w(l4),w(l5))
      k2 = l9+mxmymz

      IF (intpol(3) == 1) THEN
    !     linear in z
        k3 = k2
        k4 = k3+mz
        k5 = k4
        k6 = k5
        k7 = k6
        k8 = k7+mxmy
        k9 = k8+mxmy
        CALL linmxu(nz,mz,iw(kz),w(k3))
        j2 = k9
      ELSE
    !     cubic in z
        k3 = k2+mz
        k4 = k3+mz
        k5 = k4+mz
        k6 = k5+mz
        k7 = k6+mxmy
        k8 = k7+mxmy
        k9 = k8+mxmy
        CALL cubnmxu(nz,mz,iw(kz),w(k2),w(k3),w(k4),w(k5))
        j2 = k9+mxmy
      END IF

      IF (intpol(2) == 1) THEN
        j3 = j2
        j4 = j3+my
        j5 = j4
        j6 = j5
        j7 = j6
        j8 = j7+mx
        j9 = j8+mx
        CALL linmxu(ny,my,iw(jy),w(j3))
        i2 = j9
      ELSE
        j3 = j2+my
        j4 = j3+my
        j5 = j4+my
        j6 = j5+my
        j7 = j6+mx
        j8 = j7+mx
        j9 = j8+mx
        CALL cubnmxu(ny,my,iw(jy),w(j2),w(j3),w(j4),w(j5))
        i2 = j9+mx
      END IF

    !     set work space portion and indices which depend on x interpolation

      IF (intpol(1) == 1) THEN
        i3 = i2
        i4 = i3
        i5 = i4
        CALL linmxu(nx,mx,iw,w(i3))
      ELSE
        i3 = i2+mx
        i4 = i3+mx
        i5 = i4+mx
        CALL cubnmxu(nx,mx,iw,w(i2),w(i3),w(i4),w(i5))
      END IF

    !     cubically interpolate in t

      CALL cubt4u(nx,ny,nz,nt,p,mx,my,mz,mt,mxmy,mxmymz,q,intpol,  &
          iw(LT),w(l2),w(l3),w(l4),w(l5),w(l6),w(l7),w(l8),w(l9),  &
          iw(kz),w(k2),w(k3),w(k4),w(k5),w(k6),w(k7),w(k8),w(k9),  &
          iw(jy),w(j2),w(j3),w(j4),w(j5),w(j6),w(j7),w(j8),w(j9),  &
          iw,w(i2),w(i3),w(i4),w(i5), inmx,jnmy,knmz,lnmt,isubx,jsuby,ksubz,lsubt)
      RETURN
    END IF
    END SUBROUTINE rgrd4u

    SUBROUTINE lint4u(nx,ny,nz,nt,p,mx,my,mz,mt,mxmy,mxmymz,q,intpol,  &
        LT,dt,pt,ptp,kz,dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,  &
        jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
        inmx,jnmy,knmz,lnmt,isubx,jsuby,ksubz,lsubt)

    !     linearly interpolate in t direction

    IMPLICIT NONE
    INTEGER, INTENT(IN)                      :: nx
    INTEGER, INTENT(IN)                      :: ny
    INTEGER, INTENT(IN)                      :: nz
    INTEGER, INTENT(IN)                      :: nt
    real (kind=8), INTENT(IN)                :: p(nx,ny,nz,nt)
    INTEGER, INTENT(IN)                      :: mx
    INTEGER, INTENT(IN)                      :: my
    INTEGER, INTENT(IN)                      :: mz
    INTEGER, INTENT(IN)                      :: mt
    INTEGER, INTENT(IN OUT)                  :: mxmy
    INTEGER, INTENT(IN)                      :: mxmymz
    real (kind=8), INTENT(OUT)               :: q(mxmymz,mt)
    INTEGER, INTENT(IN)                      :: intpol(4)
    INTEGER, INTENT(IN)                      :: LT(mt)
    real (kind=8), INTENT(IN)                :: dt(mt)
    real (kind=8), INTENT(OUT)               :: pt(mxmymz)
    real (kind=8), INTENT(IN OUT)            :: ptp(mxmymz)
    INTEGER, INTENT(IN OUT)                  :: kz(mz)
    real (kind=8), INTENT(IN OUT)            :: dzm(mz)
    real (kind=8), INTENT(IN OUT)            :: dz(mz)
    real (kind=8), INTENT(IN OUT)            :: dzp(mz)
    real (kind=8), INTENT(IN OUT)            :: dzpp(mz)
    real (kind=8), INTENT(IN OUT)            :: pkm(mxmy)
    real (kind=8), INTENT(IN OUT)            :: pk(mxmy)
    real (kind=8), INTENT(IN OUT)            :: pkp(mxmy)
    real (kind=8), INTENT(IN OUT)            :: pkpp(mxmy)
    INTEGER, INTENT(IN OUT)                  :: jy(my)
    real (kind=8), INTENT(IN OUT)            :: dym(my)
    real (kind=8), INTENT(IN OUT)            :: dy(my)
    real (kind=8), INTENT(IN OUT)            :: dyp(my)
    real (kind=8), INTENT(IN OUT)            :: dypp(my)
    real (kind=8), INTENT(IN OUT)            :: pjm(mx)
    real (kind=8), INTENT(IN OUT)            :: pj(mx)
    real (kind=8), INTENT(IN OUT)            :: pjp(mx)
    real (kind=8), INTENT(IN OUT)            :: pjpp(mx)
    INTEGER, INTENT(IN OUT)                  :: ix(mx)
    real (kind=8), INTENT(IN OUT)            :: dxm(mx)
    real (kind=8), INTENT(IN OUT)            :: dx(mx)
    real (kind=8), INTENT(IN OUT)            :: dxp(mx)
    real (kind=8), INTENT(IN OUT)            :: dxpp(mx)
    INTEGER, INTENT(IN OUT)                  :: inmx
    INTEGER, INTENT(IN OUT)                  :: jnmy
    INTEGER, INTENT(IN OUT)                  :: knmz
    INTEGER, INTENT(IN)                      :: lnmt
    INTEGER, INTENT(IN OUT)                  :: isubx
    INTEGER, INTENT(IN OUT)                  :: jsuby
    INTEGER, INTENT(IN OUT)                  :: ksubz
    INTEGER, INTENT(IN)                      :: lsubt

    INTEGER :: l,ll,lsave,iijjkk

    IF (intpol(3) == 1) THEN

    !     linear in z

      IF (lsubt == 1) THEN

    !     mt grid is subset of nt grid

        DO ll=1,mt
          l = lnmt*(ll-1)+1
          CALL lint3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,q(1,ll),intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END DO
        RETURN
      END IF

      lsave = -1
      DO ll=1,mt
        l = LT(ll)
        IF (l == lsave) THEN

    !     l pointer has not moved since last pass (no updates or interpolation)

        ELSE IF (l == lsave+1) THEN

    !     update l and interpolate l+1

          DO iijjkk=1,mxmymz
            pt(iijjkk) = ptp(iijjkk)
          END DO
          CALL lint3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp  &
              ,inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE

    !     interpolate l,l+1 in pt,ptp on xx,yy,zz mesh

          CALL lint3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,pt,intpol,kz,dz,  &
              pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL lint3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp  &
              ,inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END IF

    !     save l pointer for next pass

        lsave = l

    !     linearly interpolate q(ii,jj,,kk,ll) from pt,ptp in t direction

        DO iijjkk=1,mxmymz
          q(iijjkk,ll) = pt(iijjkk)+dt(ll)*(ptp(iijjkk)-pt(iijjkk))
        END DO
      END DO
      RETURN

    ELSE

    !     cubic in z

      IF (lsubt == 1) THEN

    !     mt grid is subset of nt grid

        DO ll=1,mt
          l = lnmt*(ll-1)+1
          CALL cubt3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,q(1,ll),intpol,  &
              kz,dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,  &
              pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END DO
        RETURN
      END IF
      lsave = -1
      DO ll=1,mt
        l = LT(ll)
        IF (l == lsave) THEN

    !     l pointer has not moved since last pass (no updates or interpolation)

        ELSE IF (l == lsave+1) THEN

    !     update l and interpolate l+1

          DO iijjkk=1,mxmymz
            pt(iijjkk) = ptp(iijjkk)
          END DO
          CALL cubt3u(nx,ny,nt,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,  &
              kz,dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,  &
              jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE

    !     interpolate l,l+1 in pt,ptp on xx,yy,zz mesh

          CALL cubt3u(nx,ny,nt,p(1,1,1,l),mx,my,mxmy,mz,pt,intpol,  &
              kz,dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,  &
              jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL cubt3u(nx,ny,nt,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,  &
              kz,dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,  &
              jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END IF

    !     save l pointer for next pass

        lsave = l

    !     linearly interpolate q(ii,jj,kk,ll) from pt,ptp in t direction

        DO iijjkk=1,mxmymz
          q(iijjkk,ll) = pt(iijjkk)+dt(ll)*(ptp(iijjkk)-pt(iijjkk))
        END DO

      END DO
      RETURN

    END IF
    END SUBROUTINE lint4u

    SUBROUTINE cubt4u(nx,ny,nz,nt,p,mx,my,mz,mt,mxmy,mxmymz,q,intpol,  &
        LT,dtm,dt,dtp,dtpp,ptm,pt,ptp,ptpp, kz,dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,  &
        jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp, ix,dxm,dx,dxp,dxpp,  &
        inmx,jnmy,knmz,lnmt,isubx,jsuby,ksubz,lsubt)

    !     cubically interpolate in t

    IMPLICIT NONE
    INTEGER, INTENT(IN)                      :: nx
    INTEGER, INTENT(IN)                      :: ny
    INTEGER, INTENT(IN)                      :: nz
    INTEGER, INTENT(IN)                      :: nt
    real (kind=8), INTENT(IN)                :: p(nx,ny,nz,nt)
    INTEGER, INTENT(IN)                      :: mx
    INTEGER, INTENT(IN)                      :: my
    INTEGER, INTENT(IN)                      :: mz
    INTEGER, INTENT(IN)                      :: mt
    INTEGER, INTENT(IN OUT)                  :: mxmy
    INTEGER, INTENT(IN)                      :: mxmymz
    real (kind=8), INTENT(OUT)                        :: q(mxmymz,mt)
    INTEGER, INTENT(IN)                      :: intpol(4)
    INTEGER, INTENT(IN)                      :: LT(mt)
    real (kind=8), INTENT(IN)                :: dtm(mt)
    real (kind=8), INTENT(IN)                :: dt(mt)
    real (kind=8), INTENT(IN OUT)            :: dtp(mt)
    real (kind=8), INTENT(IN OUT)            :: dtpp(mt)
    real (kind=8), INTENT(OUT)               :: ptm(mxmymz)
    real (kind=8), INTENT(IN OUT)            :: pt(mxmymz)
    real (kind=8), INTENT(IN OUT)            :: ptp(mxmymz)
    real (kind=8), INTENT(IN OUT)            :: ptpp(mxmymz)
    INTEGER, INTENT(IN OUT)                  :: kz(mz)
    real (kind=8), INTENT(IN OUT)            :: dzm(mz)
    real (kind=8), INTENT(IN OUT)            :: dz(mz)
    real (kind=8), INTENT(IN OUT)            :: dzp(mz)
    real (kind=8), INTENT(IN OUT)            :: dzpp(mz)
    real (kind=8), INTENT(IN OUT)            :: pkm(mxmy)
    real (kind=8), INTENT(IN OUT)            :: pk(mxmy)
    real (kind=8), INTENT(IN OUT)            :: pkp(mxmy)
    real (kind=8), INTENT(IN OUT)            :: pkpp(mxmy)
    INTEGER, INTENT(IN OUT)                  :: jy(my)
    real (kind=8), INTENT(IN OUT)            :: dym(my)
    real (kind=8), INTENT(IN OUT)            :: dy(my)
    real (kind=8), INTENT(IN OUT)            :: dyp(my)
    real (kind=8), INTENT(IN OUT)            :: dypp(my)
    real (kind=8), INTENT(IN OUT)            :: pjm(mx)
    real (kind=8), INTENT(IN OUT)            :: pj(mx)
    real (kind=8), INTENT(IN OUT)            :: pjp(mx)
    real (kind=8), INTENT(IN OUT)            :: pjpp(mx)
    INTEGER, INTENT(IN OUT)                  :: ix(mx)
    real (kind=8), INTENT(IN OUT)            :: dxm(mx)
    real (kind=8), INTENT(IN OUT)            :: dx(mx)
    real (kind=8), INTENT(IN OUT)            :: dxp(mx)
    real (kind=8), INTENT(IN OUT)            :: dxpp(mx)
    INTEGER, INTENT(IN OUT)                  :: inmx
    INTEGER, INTENT(IN OUT)                  :: jnmy
    INTEGER, INTENT(IN OUT)                  :: knmz
    INTEGER, INTENT(IN)                      :: lnmt
    INTEGER, INTENT(IN OUT)                  :: isubx
    INTEGER, INTENT(IN OUT)                  :: jsuby
    INTEGER, INTENT(IN OUT)                  :: ksubz
    INTEGER, INTENT(IN)                      :: lsubt

    INTEGER :: l,ll,iijjkk,lsave

    IF (intpol(3) == 1) THEN

    !     linear in z

      IF (lsubt == 1) THEN

    !     mt grid is subset of nt grid

        DO ll=1,mt
          l = lnmt*(ll-1)+1
          CALL lint3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,q(1,ll),intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END DO
        RETURN
      END IF
      lsave = -3
      DO ll=1,mt
        l = LT(ll)
        IF (l == lsave) THEN

    !     l pointer has not moved since last pass (no updates or interpolation)

        ELSE IF (l == lsave+1) THEN

    !     update l-1,l,l+1 and interpolate l+2

          DO iijjkk=1,mxmymz
            ptm(iijjkk) = pt(iijjkk)
            pt(iijjkk) = ptp(iijjkk)
            ptp(iijjkk) = ptpp(iijjkk)
          END DO
          CALL lint3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE IF (l == lsave+2) THEN

    !     update l-1,l and interpolate l+1,l+2

          DO iijjkk=1,mxmymz
            ptm(iijjkk) = ptp(iijjkk)
            pt(iijjkk) = ptpp(iijjkk)
          END DO
          CALL lint3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL lint3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE IF (l == lsave+3) THEN

    !     update l-1 and interpolate l,l+1,l+2

          DO iijjkk=1,mxmymz
            ptm(iijjkk) = ptpp(iijjkk)
          END DO
          CALL lint3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,pt,intpol,kz,dz,  &
              pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL lint3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL lint3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE

    !     interpolate all four l-1,l,l+1,l+2

          CALL lint3u(nx,ny,nz,p(1,1,1,l-1),mx,my,mxmy,mz,ptm,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL lint3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,pt,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL lint3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL lint3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dz,pk,pkp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp,  &
              inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END IF

    !     save l pointer for next pass

        lsave = l

    !     cubically interpolate q(ii,jj,kk,ll) from ptm,pt,ptp,ptpp in t direction

        DO iijjkk=1,mxmymz
          q(iijjkk,ll) = dtm(ll)*ptm(iijjkk) + dt(ll)*pt(iijjkk) +  &
              dtp(ll)*ptp(iijjkk) + dtpp(ll)*ptpp(iijjkk)
        END DO
      END DO
      RETURN

    ELSE

    !     cubic in z

      IF (lsubt == 1) THEN

    !     mt grid is subset of nt grid

        DO ll=1,mt
          l = lnmt*(ll-1)+1
          CALL cubt3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,q(1,ll),intpol,  &
              kz,dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,  &
              pjm,pj,pjp,pjpp,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END DO
        RETURN
      END IF
      lsave = -3
      DO ll=1,mt
        l = LT(ll)
        IF (l == lsave) THEN

    !     l pointer has not moved since last pass (no updates or interpolation)

        ELSE IF (l == lsave+1) THEN

    !     update l-1,l,l+1 and interpolate l+2

          DO iijjkk=1,mxmymz
            ptm(iijjkk) = pt(iijjkk)
            pt(iijjkk) = ptp(iijjkk)
            ptp(iijjkk) = ptpp(iijjkk)
          END DO
          CALL cubt3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE IF (l == lsave+2) THEN

    !     update l-1,l and interpolate l+1,l+2

          DO iijjkk=1,mxmymz
            ptm(iijjkk) = ptp(iijjkk)
            pt(iijjkk) = ptpp(iijjkk)
          END DO
          CALL cubt3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL cubt3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE IF (l == lsave+3) THEN

    !     update l-1 and interpolate l,l+1,l+2

          DO iijjkk=1,mxmymz
            ptm(iijjkk) = ptpp(iijjkk)
          END DO
          CALL cubt3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,pt,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL cubt3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL cubt3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
        ELSE

    !     interpolate all four l-1,l,l+1,l+2

          CALL cubt3u(nx,ny,nz,p(1,1,1,l-1),mx,my,mxmy,mz,ptm,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL cubt3u(nx,ny,nz,p(1,1,1,l),mx,my,mxmy,mz,pt,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL cubt3u(nx,ny,nz,p(1,1,1,l+1),mx,my,mxmy,mz,ptp,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
          CALL cubt3u(nx,ny,nz,p(1,1,1,l+2),mx,my,mxmy,mz,ptpp,intpol,kz,  &
              dzm,dz,dzp,dzpp,pkm,pk,pkp,pkpp,jy,dym,dy,dyp,dypp,pjm,pj,pjp,pjpp  &
              ,ix,dxm,dx,dxp,dxpp, inmx,jnmy,knmz,isubx,jsuby,ksubz)
        END IF

    !     save l pointer for next pass

        lsave = l

    !     cubically interpolate q(ii,jj,kk,ll) from ptm,pt,ptp,ptpp in t direction

        DO iijjkk=1,mxmymz
          q(iijjkk,ll) = dtm(ll)*ptm(iijjkk) + dt(ll)*pt(iijjkk) +  &
              dtp(ll)*ptp(iijjkk) + dtpp(ll)*ptpp(iijjkk)
        END DO
      END DO
      RETURN
    END IF

    END SUBROUTINE cubt4u
end module


! Copyright 2019 NREL

! 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.
! -------------------------------------------------------------------------------------------
! This module contains basic functions used by the controller

! Functions:
!       AeroDynTorque: Calculate aerodynamic torque
!       ColemanTransform: Perform Colemant transform
!       ColemanTransformInverse: Perform inverse Colemant transform
!       CPFunction: Find Cp using parameterized surface
!       Debug: Debug the controller
!       DFController: DF control
!       identity: Make identity matrix
!       interp1d: 1-d interpolation
!       interp2d: 2-d interpolation
!       matinv3: 3x3 matrix inverse
!       PIController: implement a PI controller
!       ratelimit: Rate limit signal
!       saturate: Saturate signal

MODULE Functions

USE Constants

IMPLICIT NONE

CONTAINS
!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION saturate(inputValue, minValue, maxValue)
    ! Saturates inputValue. Makes sure it is not smaller than minValue and not larger than maxValue

        IMPLICIT NONE

        REAL(DbKi), INTENT(IN)     :: inputValue
        REAL(DbKi), INTENT(IN)     :: minValue
        REAL(DbKi), INTENT(IN)     :: maxValue

        saturate = REAL(MIN(MAX(inputValue,minValue), maxValue),DbKi)

    END FUNCTION saturate

!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION ratelimit(inputSignal, inputSignalPrev, minRate, maxRate, DT)
    ! Saturates inputValue. Makes sure it is not smaller than minValue and not larger than maxValue
        IMPLICIT NONE

        REAL(DbKi), INTENT(IN)     :: inputSignal
        REAL(DbKi), INTENT(IN)     :: inputSignalPrev
        REAL(DbKi), INTENT(IN)     :: minRate
        REAL(DbKi), INTENT(IN)     :: maxRate
        REAL(DbKi), INTENT(IN)     :: DT
        ! Local variables
        REAL(DbKi)                 :: rate

        rate = (inputSignal - inputSignalPrev)/DT                       ! Signal rate (unsaturated)
        rate = saturate(rate, minRate, maxRate)                 ! Saturate the signal rate
        ratelimit = inputSignalPrev + rate*DT                       ! Saturate the overall command using the rate limit

    END FUNCTION ratelimit

!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, piP, reset, inst)
        USE ROSCO_Types, ONLY : piParams

    ! PI controller, with output saturation

        IMPLICIT NONE
        ! Allocate Inputs
        REAL(DbKi),    INTENT(IN)         :: error
        REAL(DbKi),    INTENT(IN)         :: kp
        REAL(DbKi),    INTENT(IN)         :: ki
        REAL(DbKi),    INTENT(IN)         :: minValue
        REAL(DbKi),    INTENT(IN)         :: maxValue
        REAL(DbKi),    INTENT(IN)         :: DT
        INTEGER(IntKi), INTENT(INOUT)      :: inst
        REAL(DbKi),    INTENT(IN)         :: I0
        TYPE(piParams), INTENT(INOUT)  :: piP
        LOGICAL,    INTENT(IN)         :: reset
        ! Allocate local variables
        !REMOVED BECAUSE UNUSED !!! INTEGER(IntKi)                      :: i                                            ! Counter for making arrays
        REAL(DbKi)                         :: PTerm                                        ! Proportional term

        ! Initialize persistent variables/arrays, and set inital condition for integrator term
        IF (reset) THEN
            piP%ITerm(inst) = I0
            piP%ITermLast(inst) = I0

            PIController = I0
        ELSE
            PTerm = kp*error
            piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error
            piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue)
            PIController = saturate(PTerm + piP%ITerm(inst), minValue, maxValue)

            piP%ITermLast(inst) = piP%ITerm(inst)
        END IF
        inst = inst + 1

    END FUNCTION PIController

!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, piP, reset, inst)
    ! PI controller, with output saturation.
    ! Added error2 term for additional integral control input
        USE ROSCO_Types, ONLY : piParams

        IMPLICIT NONE
        ! Allocate Inputs
        REAL(DbKi), INTENT(IN)         :: error
        REAL(DbKi), INTENT(IN)         :: error2
        REAL(DbKi), INTENT(IN)         :: kp
        REAL(DbKi), INTENT(IN)         :: ki2
        REAL(DbKi), INTENT(IN)         :: ki
        REAL(DbKi), INTENT(IN)         :: minValue
        REAL(DbKi), INTENT(IN)         :: maxValue
        REAL(DbKi), INTENT(IN)         :: DT
        INTEGER(IntKi), INTENT(INOUT)   :: inst
        REAL(DbKi), INTENT(IN)         :: I0
        TYPE(piParams), INTENT(INOUT) :: piP
        LOGICAL, INTENT(IN)         :: reset
        ! Allocate local variables
        !INTEGER(IntKi)                      :: i                                            ! Counter for making arrays
        REAL(DbKi)                         :: PTerm                                        ! Proportional term

        ! Initialize persistent variables/arrays, and set inital condition for integrator term
        IF (reset) THEN
            piP%ITerm(inst) = I0
            piP%ITermLast(inst) = I0
            piP%ITerm2(inst) = I0
            piP%ITermLast2(inst) = I0

            PIIController = I0
        ELSE
            PTerm = kp*error
            piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error
            piP%ITerm2(inst) = piP%ITerm2(inst) + DT*ki2*error2
            piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue)
            piP%ITerm2(inst) = saturate(piP%ITerm2(inst), minValue, maxValue)
            PIIController = PTerm + piP%ITerm(inst) + piP%ITerm2(inst)
            PIIController = saturate(PIIController, minValue, maxValue)

            piP%ITermLast(inst) = piP%ITerm(inst)
        END IF
        inst = inst + 1

    END FUNCTION PIIController

!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION interp1d(xData, yData, xq, ErrVar)
    ! interp1d 1-D interpolation (table lookup), xData should be strictly increasing

        USE ROSCO_Types, ONLY : ErrorVariables
        IMPLICIT NONE

        ! Inputs
        REAL(DbKi), DIMENSION(:), INTENT(IN)       :: xData        ! Provided x data (vector), to be interpolated
        REAL(DbKi), DIMENSION(:), INTENT(IN)       :: yData        ! Provided y data (vector), to be interpolated
        REAL(DbKi), INTENT(IN)                     :: xq           ! x-value for which the y value has to be interpolated
        INTEGER(IntKi)                              :: I            ! Iteration index

        ! Error Catching
        TYPE(ErrorVariables), INTENT(INOUT)     :: ErrVar
        INTEGER(IntKi)                              :: I_DIFF

        CHARACTER(*), PARAMETER                 :: RoutineName = 'interp1d'

        ! Catch Errors
        ! Are xData and yData the same size?
        IF (SIZE(xData) .NE. SIZE(yData)) THEN
            ErrVar%aviFAIL = -1
            ErrVar%ErrMsg  = ' xData and yData are not the same size'
            WRITE(ErrVar%ErrMsg,"(A,I2,A,I2,A)") " SIZE(xData) =", SIZE(xData), &
            ' and SIZE(yData) =', SIZE(yData),' are not the same'
        END IF

        ! Is xData non decreasing
        DO I_DIFF = 1, size(xData) - 1
            IF (xData(I_DIFF + 1) - xData(I_DIFF) <= 0) THEN
                ErrVar%aviFAIL = -1
                ErrVar%ErrMsg  = ' xData is not strictly increasing'
                EXIT
            END IF
        END DO

        ! Interpolate
        IF (xq <= MINVAL(xData)) THEN
            interp1d = yData(1)
        ELSEIF (xq >= MAXVAL(xData)) THEN
            interp1d = yData(SIZE(xData))
        ELSE
            DO I = 1, SIZE(xData)
                IF (xq <= xData(I)) THEN
                    interp1d = yData(I-1) + (yData(I) - yData(I-1))/(xData(I) - xData(I-1))*(xq - xData(I-1))
                    EXIT
                ELSE
                    CONTINUE
                END IF
            END DO
        END IF

        ! Add RoutineName to error message
        IF (ErrVar%aviFAIL < 0) THEN
            ErrVar%ErrMsg = RoutineName//':'//TRIM(ErrVar%ErrMsg)
        ENDIF

    END FUNCTION interp1d

!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION interp2d(xData, yData, zData, xq, yq, ErrVar)
    ! interp2d 2-D interpolation (table lookup). Query done using bilinear interpolation.
    ! Note that the interpolated matrix with associated query vectors may be different than "standard", - zData should be formatted accordingly
    ! - xData follows the matrix from left to right
    ! - yData follows the matrix from top to bottom
    ! A simple case: xData = [1 2 3], yData = [4 5 6]
    !        | 1    2   3
    !       -------------
    !       4| a    b   c
    !       5| d    e   f
    !       6| g    H   i

        USE ROSCO_Types, ONLY : ErrorVariables
        USE ieee_arithmetic

        IMPLICIT NONE

        ! Inputs
        REAL(DbKi), DIMENSION(:),   INTENT(IN)     :: xData        ! Provided x data (vector), to find query point (should be strictly increasing)
        REAL(DbKi), DIMENSION(:),   INTENT(IN)     :: yData        ! Provided y data (vector), to find query point (should be strictly increasing)
        REAL(DbKi), DIMENSION(:,:), INTENT(IN)     :: zData        ! Provided z data (vector), to be interpolated
        REAL(DbKi),                 INTENT(IN)     :: xq           ! x-value for which the z value has to be interpolated
        REAL(DbKi),                 INTENT(IN)     :: yq           ! y-value for which the z value has to be interpolated

        ! Allocate variables
        INTEGER(IntKi)                              :: i            ! Iteration index & query index, x-direction
        INTEGER(IntKi)                              :: ii           ! Iteration index & second que .  ry index, x-direction
        INTEGER(IntKi)                              :: j            ! Iteration index & query index, y-direction
        INTEGER(IntKi)                              :: jj           ! Iteration index & second query index, y-direction
        REAL(DbKi), DIMENSION(2,2)                 :: fQ           ! zData value at query points for bilinear interpolation
        REAL(DbKi), DIMENSION(1)                   :: fxy           ! Interpolated z-data point to be returned
        REAL(DbKi)                                 :: fxy1          ! zData value at query point for bilinear interpolation
        REAL(DbKi)                                 :: fxy2          ! zData value at query point for bilinear interpolation
        !LOGICAL                                 :: edge

        ! Error Catching
        TYPE(ErrorVariables), INTENT(INOUT)     :: ErrVar
        INTEGER(IntKi)                              :: I_DIFF

        CHARACTER(*), PARAMETER                 :: RoutineName = 'interp2d'

        ! Error catching
        ! Are xData and zData(:,1) the same size?
        IF (SIZE(xData) .NE. SIZE(zData,2)) THEN
            ErrVar%aviFAIL = -1
            WRITE(ErrVar%ErrMsg,"(A,I4,A,I4,A)") " SIZE(xData) =", SIZE(xData), &
            ' and SIZE(zData,1) =', SIZE(zData,2),' are not the same'
        END IF

        ! Are yData and zData(1,:) the same size?
        IF (SIZE(yData) .NE. SIZE(zData,1)) THEN
            ErrVar%aviFAIL = -1
            WRITE(ErrVar%ErrMsg,"(A,I4,A,I4,A)") " SIZE(yData) =", SIZE(yData), &
            ' and SIZE(zData,2) =', SIZE(zData,1),' are not the same'
        END IF

        ! Is xData non decreasing
        DO I_DIFF = 1, size(xData) - 1
            IF (xData(I_DIFF + 1) - xData(I_DIFF) <= 0) THEN
                ErrVar%aviFAIL = -1
                ErrVar%ErrMsg  = ' xData is not strictly increasing'
                EXIT
            END IF
        END DO

        ! Is yData non decreasing
        DO I_DIFF = 1, size(yData) - 1
            IF (yData(I_DIFF + 1) - yData(I_DIFF) <= 0) THEN
                ErrVar%aviFAIL = -1
                ErrVar%ErrMsg  = ' yData is not strictly increasing'
                EXIT
            END IF
        END DO

        ! ---- Find corner indices surrounding desired interpolation point -----
            ! x-direction
        IF (xq <= MINVAL(xData) .OR. (ieee_is_nan(xq))) THEN       ! On lower x-bound, just need to find zData(yq)
            j = 1
            jj = 1
            interp2d = interp1d(yData,zData(:,j),yq,ErrVar)
            RETURN
        ELSEIF (xq >= MAXVAL(xData)) THEN   ! On upper x-bound, just need to find zData(yq)
            j = size(xData)
            jj = size(xData)
            interp2d = interp1d(yData,zData(:,j),yq,ErrVar)
            RETURN
        ELSE
            DO j = 1,size(xData)
                IF (xq == xData(j)) THEN ! On axis, just need 1d interpolation
                    jj = j
                    interp2d = interp1d(yData,zData(:,j),yq,ErrVar)
                    RETURN
                ELSEIF (xq < xData(j)) THEN
                    jj = j
                    EXIT
                ELSE
                    CONTINUE
                END IF
            END DO
        ENDIF
        j = j-1 ! Move j back one
            ! y-direction
        IF (yq <= MINVAL(yData) .OR. (ieee_is_nan(yq))) THEN       ! On lower y-bound, just need to find zData(xq)
            i = 1
            ii = 1
            interp2d = interp1d(xData,zData(i,:),xq,ErrVar)
            RETURN
        ELSEIF (yq >= MAXVAL(yData)) THEN   ! On upper y-bound, just need to find zData(xq)
            i = size(yData)
            ii = size(yData)
            interp2d = interp1d(xData,zData(i,:),xq,ErrVar)
            RETURN
        ELSE
            DO i = 1,size(yData)
                IF (yq == yData(i)) THEN    ! On axis, just need 1d interpolation
                    ii = i
                    interp2d = interp1d(xData,zData(i,:),xq,ErrVar)
                    RETURN
                ELSEIF (yq < yData(i)) THEN
                    ii = i
                    EXIT
                ELSE
                    CONTINUE
                END IF
            END DO
        ENDIF
        i = i-1 ! move i back one

        ! ---- Do bilinear interpolation ----
        ! Find values at corners
        fQ(1,1) = zData(i,j)
        fQ(2,1) = zData(ii,j)
        fQ(1,2) = zData(i,jj)
        fQ(2,2) = zData(ii,jj)
        ! Interpolate
        fxy1 = (xData(jj) - xq)/(xData(jj) - xData(j))*fQ(1,1) + (xq - xData(j))/(xData(jj) - xData(j))*fQ(1,2)
        fxy2 = (xData(jj) - xq)/(xData(jj) - xData(j))*fQ(2,1) + (xq - xData(j))/(xData(jj) - xData(j))*fQ(2,2)
        fxy = (yData(ii) - yq)/(yData(ii) - yData(i))*fxy1 + (yq - yData(i))/(yData(ii) - yData(i))*fxy2

        interp2d = fxy(1)

        ! Add RoutineName to error message
        IF (ErrVar%aviFAIL < 0) THEN
            ErrVar%ErrMsg = RoutineName//':'//TRIM(ErrVar%ErrMsg)
        ENDIF

    END FUNCTION interp2d

!-------------------------------------------------------------------------------------------------------------------------------
    FUNCTION matinv3(A) RESULT(B)
    ! Performs a direct calculation of the inverse of a 3×3 matrix.
    ! Source: http://fortranwiki.org/fortran/show/Matrix+inversion
        REAL(DbKi), INTENT(IN) :: A(3,3)   !! Matrix
        REAL(DbKi)             :: B(3,3)   !! Inverse matrix
        REAL(DbKi)             :: detinv

        ! Calculate the inverse determinant of the matrix
        detinv = 1/(A(1,1)*A(2,2)*A(3,3) - A(1,1)*A(2,3)*A(3,2)&
                - A(1,2)*A(2,1)*A(3,3) + A(1,2)*A(2,3)*A(3,1)&
                + A(1,3)*A(2,1)*A(3,2) - A(1,3)*A(2,2)*A(3,1))

        ! Calculate the inverse of the matrix
        B(1,1) = +detinv * (A(2,2)*A(3,3) - A(2,3)*A(3,2))
        B(2,1) = -detinv * (A(2,1)*A(3,3) - A(2,3)*A(3,1))
        B(3,1) = +detinv * (A(2,1)*A(3,2) - A(2,2)*A(3,1))
        B(1,2) = -detinv * (A(1,2)*A(3,3) - A(1,3)*A(3,2))
        B(2,2) = +detinv * (A(1,1)*A(3,3) - A(1,3)*A(3,1))
        B(3,2) = -detinv * (A(1,1)*A(3,2) - A(1,2)*A(3,1))
        B(1,3) = +detinv * (A(1,2)*A(2,3) - A(1,3)*A(2,2))
        B(2,3) = -detinv * (A(1,1)*A(2,3) - A(1,3)*A(2,1))
        B(3,3) = +detinv * (A(1,1)*A(2,2) - A(1,2)*A(2,1))
    END FUNCTION matinv3

!-------------------------------------------------------------------------------------------------------------------------------
    FUNCTION identity(n) RESULT(A)
    ! Produces an identity matrix of size n x n

        INTEGER, INTENT(IN)         :: n
        REAL(DbKi), DIMENSION(n, n)    :: A
        INTEGER                     :: i
        INTEGER                     :: j

        ! Build identity matrix
        DO i=1,n
            DO j = 1,n
                IF (i == j) THEN
                    A(i,j) = 1.0
                ELSE
                    A(i,j) = 0.0
                ENDIF
            ENDDO
        ENDDO

    END FUNCTION identity

!-------------------------------------------------------------------------------------------------------------------------------
    SUBROUTINE ColemanTransform(rootMOOP, aziAngle, nHarmonic, axTOut, axYOut)
    ! The Coleman or d-q axis transformation transforms the root out of plane bending moments of each turbine blade
    ! to a direct axis and a quadrature axis

        IMPLICIT NONE
        ! Inputs
        REAL(DbKi), INTENT(IN)     :: rootMOOP(3)                      ! Root out of plane bending moments of each blade
        REAL(DbKi), INTENT(IN)     :: aziAngle                         ! Rotor azimuth angle
        INTEGER(IntKi), INTENT(IN)  :: nHarmonic                        ! The harmonic number, nP
        ! Outputs
        REAL(DbKi), INTENT(OUT)    :: axTOut, axYOut               ! Direct axis and quadrature axis outputted by this transform
        ! Local
        REAL(DbKi), PARAMETER      :: phi2 = 2.0/3.0*PI                ! Phase difference from first to second blade
        REAL(DbKi), PARAMETER      :: phi3 = 4.0/3.0*PI                ! Phase difference from first to third blade

        ! Body
        axTOut  = 2.0/3.0 * (cos(nHarmonic*(aziAngle))*rootMOOP(1) + cos(nHarmonic*(aziAngle+phi2))*rootMOOP(2) + cos(nHarmonic*(aziAngle+phi3))*rootMOOP(3))
        axYOut  = 2.0/3.0 * (sin(nHarmonic*(aziAngle))*rootMOOP(1) + sin(nHarmonic*(aziAngle+phi2))*rootMOOP(2) + sin(nHarmonic*(aziAngle+phi3))*rootMOOP(3))

    END SUBROUTINE ColemanTransform

!-------------------------------------------------------------------------------------------------------------------------------
    SUBROUTINE ColemanTransformInverse(axTIn, axYIn, aziAngle, nHarmonic, aziOffset, PitComIPC)
    ! The inverse Coleman or d-q axis transformation transforms the direct axis and quadrature axis
    ! back to root out of plane bending moments of each turbine blade
        IMPLICIT NONE
        ! Inputs
        REAL(DbKi), INTENT(IN)     :: axTIn, axYIn         ! Direct axis and quadrature axis
        REAL(DbKi), INTENT(IN)     :: aziAngle                     ! Rotor azimuth angle
        REAL(DbKi), INTENT(IN)     :: aziOffset                    ! Phase shift added to the azimuth angle
        INTEGER(IntKi), INTENT(IN)  :: nHarmonic                    ! The harmonic number, nP
        ! Outputs
        REAL(DbKi), INTENT(OUT)    :: PitComIPC(3)                   ! Commanded individual pitch (deg)
        ! Local
        REAL(DbKi), PARAMETER      :: phi2 = 2.0/3.0*PI                ! Phase difference from first to second blade
        REAL(DbKi), PARAMETER      :: phi3 = 4.0/3.0*PI                ! Phase difference from first to third blade

        ! Body
        PitComIPC(1) = cos(nHarmonic*(aziAngle+aziOffset))*axTIn + sin(nHarmonic*(aziAngle+aziOffset))*axYIn
        PitComIPC(2) = cos(nHarmonic*(aziAngle+aziOffset+phi2))*axTIn + sin(nHarmonic*(aziAngle+aziOffset+phi2))*axYIn
        PitComIPC(3) = cos(nHarmonic*(aziAngle+aziOffset+phi3))*axTIn + sin(nHarmonic*(aziAngle+aziOffset+phi3))*axYIn

    END SUBROUTINE ColemanTransformInverse

!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION CPfunction(CP, lambda)
    ! Paremeterized Cp(lambda) function for a fixed pitch angle. Circumvents the need of importing a look-up table
        IMPLICIT NONE

        ! Inputs
        REAL(DbKi), INTENT(IN) :: CP(4)    ! Parameters defining the parameterizable Cp(lambda) function
        REAL(DbKi), INTENT(IN) :: lambda    ! Estimated or measured tip-speed ratio input

        ! Lookup
        CPfunction = exp(-CP(1)/lambda)*(CP(2)/lambda-CP(3))+CP(4)*lambda
        CPfunction = saturate(CPfunction, 0.001_DbKi, 1.0_DbKi)

    END FUNCTION CPfunction

!-------------------------------------------------------------------------------------------------------------------------------
    REAL(DbKi) FUNCTION AeroDynTorque(RotSpeed, BldPitch, LocalVar, CntrPar, PerfData, ErrVar)
    ! Function for computing the aerodynamic torque, divided by the effective rotor torque of the turbine, for use in wind speed estimation

        USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, PerformanceData, ErrorVariables
        IMPLICIT NONE

        ! Inputs
        TYPE(ControlParameters), INTENT(IN) :: CntrPar
        TYPE(LocalVariables), INTENT(IN) :: LocalVar
        TYPE(PerformanceData), INTENT(IN) :: PerfData
        TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar

        REAL(DbKi), INTENT(IN)  :: RotSpeed
        REAL(DbKi), INTENT(IN)  :: BldPitch

        ! Local
        REAL(DbKi) :: RotorArea
        REAL(DbKi) :: Cp
        REAL(DbKi) :: Lambda

        CHARACTER(*), PARAMETER                 :: RoutineName = 'AeroDynTorque'

        ! Find Torque
        RotorArea = PI*CntrPar%WE_BladeRadius**2
        Lambda = RotSpeed*CntrPar%WE_BladeRadius/LocalVar%WE_Vw

        ! Compute Cp
        Cp = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, BldPitch*R2D, Lambda, ErrVar)

        AeroDynTorque = 0.5*(CntrPar%WE_RhoAir*RotorArea)*(LocalVar%WE_Vw**3/RotSpeed)*Cp
        AeroDynTorque = MAX(AeroDynTorque, 0.0_DbKi)

        ! Add RoutineName to error message
        IF (ErrVar%aviFAIL < 0) THEN
            ErrVar%ErrMsg = RoutineName//':'//TRIM(ErrVar%ErrMsg)
        ENDIF

    END FUNCTION AeroDynTorque

!-------------------------------------------------------------------------------------------------------------------------------
    ! Copied from NWTC_IO.f90
!> This function returns a character string encoded with today's date in the form dd-mmm-ccyy.
FUNCTION CurDate( )

    ! Function declaration.

    CHARACTER(11)                :: CurDate                                      !< 'dd-mmm-yyyy' string with the current date

    ! Local declarations.

    CHARACTER(8)                 :: CDate                                        ! String to hold the returned value from the DATE_AND_TIME subroutine call.

    !  Call the system date function.

    CALL DATE_AND_TIME ( CDate )

    !  Parse out the day.

    CurDate(1:3) = CDate(7:8)//'-'

    !  Parse out the month.

    SELECT CASE ( CDate(5:6) )
    CASE ( '01' )
        CurDate(4:6) = 'Jan'
    CASE ( '02' )
        CurDate(4:6) = 'Feb'
    CASE ( '03' )
        CurDate(4:6) = 'Mar'
    CASE ( '04' )
        CurDate(4:6) = 'Apr'
    CASE ( '05' )
        CurDate(4:6) = 'May'
    CASE ( '06' )
        CurDate(4:6) = 'Jun'
    CASE ( '07' )
        CurDate(4:6) = 'Jul'
    CASE ( '08' )
        CurDate(4:6) = 'Aug'
    CASE ( '09' )
        CurDate(4:6) = 'Sep'
    CASE ( '10' )
        CurDate(4:6) = 'Oct'
    CASE ( '11' )
        CurDate(4:6) = 'Nov'
    CASE ( '12' )
        CurDate(4:6) = 'Dec'
    END SELECT

    !  Parse out the year.

    CurDate(7:11) = '-'//CDate(1:4)

    RETURN
    END FUNCTION CurDate

!=======================================================================
!> This function returns a character string encoded with the time in the form "hh:mm:ss".
    FUNCTION CurTime( )

    ! Function declaration.

    CHARACTER(8)                 :: CurTime                                      !< The current time in the form "hh:mm:ss".

    ! Local declarations.

    CHARACTER(10)                :: CTime                                        ! String to hold the returned value from the DATE_AND_TIME subroutine call.

    CALL DATE_AND_TIME ( TIME=CTime )

    CurTime = CTime(1:2)//':'//CTime(3:4)//':'//CTime(5:6)

    RETURN
    END FUNCTION CurTime

!=======================================================================
! This function checks whether an array is non-decreasing
    LOGICAL Function NonDecreasing(Array)

    IMPLICIT NONE

    REAL(DbKi), DIMENSION(:)            :: Array
    INTEGER(IntKi)         :: I_DIFF

    NonDecreasing = .TRUE.
    ! Is Array non decreasing
    DO I_DIFF = 1, size(Array) - 1
        IF (Array(I_DIFF + 1) - Array(I_DIFF) <= 0) THEN
            NonDecreasing = .FALSE.
            RETURN
        END IF
    END DO

    RETURN
    END FUNCTION NonDecreasing

!=======================================================================
!> This routine converts all the text in a string to upper case.
    SUBROUTINE Conv2UC ( Str )

        ! Argument declarations.

     CHARACTER(*), INTENT(INOUT)  :: Str                                          !< The string to be converted to UC (upper case).

        ! Local declarations.

     INTEGER                      :: IC                                           ! Character index

     DO IC=1,LEN_TRIM( Str )

        IF ( ( Str(IC:IC) >= 'a' ).AND.( Str(IC:IC) <= 'z' ) )  THEN
           Str(IC:IC) = CHAR( ICHAR( Str(IC:IC) ) - 32 )
        END IF

     END DO ! IC

     RETURN
     END SUBROUTINE Conv2UC

!=======================================================================
     !> This function returns a left-adjusted string representing the passed numeric value.
    !! It eliminates trailing zeroes and even the decimal point if it is not a fraction. \n
    !! Use Num2LStr (nwtc_io::num2lstr) instead of directly calling a specific routine in the generic interface.
    FUNCTION Int2LStr ( Num )

        CHARACTER(11)                :: Int2LStr                                     !< string representing input number.

        ! Argument declarations.

        INTEGER, INTENT(IN)          :: Num                                          !< The number to convert to a left-justified string.

        WRITE (Int2LStr,'(I11)')  Num

        Int2Lstr = ADJUSTL( Int2LStr )

        RETURN
        END FUNCTION Int2LStr

END MODULE Functions


C Appendix-2 : Test routine to check displacement calculation by DC3D
C   available at http://www.bosai.go.jp/study/application/dc3d/download/DC3Dmanual.pdf
C----- Check DC3D
      X=10.
      Y=20.
      Z=-30.
      DEPTH=50.
      DIP=70.
      AL1=-80.
      AL2=120.
      AW1=-30.
      AW2=25.
      DISL1=200.
      DISL2=-150.
      DISL3=100.
C-----
      WRITE(6,*) '*** OUTPUT OF DC3D ***'
      WRITE(6,*)
      WRITE(6,*) 'DEPTH,DIP=',DEPTH,DIP
      WRITE(6,*) 'AL1,AL2,AW1,AW2=',AL1,AL2,AW1,AW2
      WRITE(6,*) 'DISL1,DISL2,DISL3=',DISL1,DISL2,DISL3
      WRITE(6,*) 'X=',X,' Y=',Y,' Z=',Z
C-----
      ALPHA=2./3.
      CALL DC3D(ALPHA,X,Y,Z,DEPTH,DIP,
     * AL1,AL2,AW1,AW2,DISL1,DISL2,DISL3,
     * UX,UY,UZ,UXX,UYX,UZX,UXY,UYY,UZY,UXZ,UYZ,UZZ,IRET)
      WRITE(6,*)
      WRITE(6,*) 'IRET=',IRET
      WRITE(6,*) 'UX,UY,UZ=',UX,UY,UZ
      WRITE(6,*) ' ANSWER = -37.8981 63.1789 14.9607'
      STOP
      END


c     ============================================
      subroutine setaux(maxmx,mbc,mx,xlower,dx,maux,aux)
c     ============================================
c
c     # set auxiliary arrays
c     # variable coefficient acoustics
c     #  aux(i,1) = density rho in i'th cell
c     #  aux(i,2) = sound speed c in i'th cell
c
c     # Piecewise constant medium with single interface at x=0
c     # Density and sound speed to left and right are set in setprob.f
c
c
      implicit double precision (a-h,o-z)
      dimension aux(1-mbc:maxmx+mbc, 2)
      common /comaux/ rhol,cl,rhor,cr

      open(unit=31,file='fort.aux',status='unknown',form='formatted')
c

       do i=1-mbc,mx+mbc
          xcell = xlower + (i-0.5d0)*dx
          if (xcell .lt. 0.0d0) then
              aux(i,1) = rhol
              aux(i,2) = cl
	    else
              aux(i,1) = rhor
              aux(i,2) = cr
            endif
	  enddo

	do i=1,mx
          write(31,701) aux(i,1), aux(i,2)
  701     format(2e16.6)
          enddo

       close(unit=31)
c
       return
       end


!>@brief Calculate velocities in 3D space ()
subroutine set_vels()
  USE prec_mod
  USE comm1, ONLY: xp, u
  USE tmp_mod
  implicit none
  integer :: i, j, k
  real(prec) :: summ, dtmp

  do j = 1, 3
    do i= 1, tcell
      summ = 0.0d0
      do k= 1, nmodes
        dtmp = DOT_PRODUCT(b_m(1:3,k),xp(1:3,i)) + c_m(k)
        summ = summ + ac_m(j,k) * COS( dtmp) + as_m(j,k) * SIN( dtmp)
      enddo
      u(j,i) = 2.0d0 * summ
    enddo
  enddo
  RETURN
end subroutine set_vels

!>@brief Calculate velocities in 4Dim. space (XYZ+Time)
subroutine set_vels_time_space(icase)
  USE prec_mod
  USE comm1, ONLY: xp, u
  USE tmp_mod
  implicit none
  integer, OPTIONAL :: icase
  integer :: i, j, k, icell, it, ilog, nkall, ic
  real(prec) :: summ, dtmp
  real(prec),dimension(1:3,1:tcell) :: u_tmp

  ! WRITE(*,*) "Switch Time coefficiet in set_vels_time_space !!!"

  ! if (PRESENT(icase)) then
  !   ic = icase
  ! else
  !   ic = 4
  ! endif
  do i = 1, ntimes
    icell = tcell
    ! write(*,'(a,4i7)') " In Cycle time new_vec", i , ntimes,tcell, icell
    CALL set_vels_at_space_time2(dtim(i),icell,xp(1:3,1:icell),u_tmp(1:3,1:tcell))
    u(1:3,tcell*(i-1)+1:tcell*i) = u_tmp(1:3,1:tcell)
  end do
  return

  ! IF (ic .eq. 4) THEN
    nkall = nkmod
    ilog = 0
    do i= 1, tcell * ntimes
      icell = MOD(i-1 , tcell) + 1
      it = INT((i-1) / tcell) + 1
      if (it /= ilog) THEN
        write(*,*) " In Cycle time new",it, icell
        ilog = it
      END IF

      ! Call the same routine as in USEINI
      CALL set_vels_at_space_time(dtim(it),xp(1:3,icell),u(1:3,i))
      ! CALL set_vels_at_space_time2(dtim(it),1,xp(1:3,icell),u(1:3,i))

    enddo
  ! ELSE IF (ic .eq. 5) THEN
  !   ! Call the same routine as in USEINI
  !   CALL set_vels_at_space_time(dtim(it),xp(1:3,icell),u(1:3,i))
  ! END IF !(ic .eq. 5) THEN

  RETURN
end subroutine set_vels_time_space

!>@brief Calculate velocities in 4Dim. space (XYZ+Time)
subroutine set_vels_at_time(dtim_in)

  USE comm0, ONLY: ncell,nsp,nep
  USE comm1, ONLY: xp, u
  USE prec_mod
  USE fire_mpi_var
  USE tmp_mod

  implicit none

  real(prec), intent(in) :: dtim_in !< current time step

  integer :: i, j, k, icell, it, ilog, nkall, il
  real(prec) :: summ, dtmp

  ! WRITE(*,*) "Switch Time coefficiet in set_vels_time_space !!!"

  nkall = nkmod
  ilog = 0

  ilog = INT(ncell**(0.66666667))
  il = INT(ncell**(1.d0/3.0d0))
  ! cycle over cells
  do i= nsp(1), nep(1) !!!!* ntimes
    icell = MOD(i-1 , tcell) + 1
    ! it = INT((i-1) / tcell) + 1
    ! if (it /= ilog) THEN
    !   write(*,*) " In Cycle time",it, icell
    !   ilog = it
    ! END IF
    k = MOD(icell-1,ilog)
    if (mpi_master .and. k.eq.0) then
      write(*,'(2(a20,i7))') "Gen vels : ",INT(icell/ilog),"/",il
    endif
    cs_m(1:nkall) =  b_m(1,1:nkall)*xp(1,icell) + &
                     b_m(2,1:nkall)*xp(2,icell) + &
                     b_m(3,1:nkall)*xp(3,icell) + &
                     c_m(1:nkall) * dtim_in
    do j= 1, 3
      u(j,i) = 2.0d0 * SUM( ac_m(j,1:nkall) * COS(cs_m(1:nkall)) &
        + as_m(j,1:nkall) * SIN(cs_m(1:nkall)) )
    enddo
  enddo

  ! CALL exchng(u,3,1)
  ! CALL exchng(u,3,2)
  ! CALL exchng(u,3,3)
  RETURN
end subroutine set_vels_at_time

!>@brief Calculate velocities in 4Dim. space (XYZ+Time)
subroutine set_vels_at_space_time(dtim_in,dxx,vels)

  USE comm0, ONLY: ncell
  ! USE comm1, ONLY: xp, u
  USE prec_mod
  USE fire_mpi_var
  USE tmp_mod

  implicit none

  real(prec), intent(in) :: dtim_in !< current time step
  real(prec),dimension(1:3), intent(in) :: dxx !< current time step
  real(prec),dimension(1:3), intent(out) :: vels !< current time step

  integer :: i, j, k, icell, it, ilog, nkall, il
  real(prec),dimension(1:3) :: dv_cos!< current time step
  real(prec) :: summ, dtmp

  ! 1 - set number of modes in all times
  nkall = nkmod

  ! 2 - COS predicted vars
  cs_m(1:nkall) = c_m(1:nkall)

  ! 3 - Calc $ K \cdot x + w t$
  CALL dgemm('N','N',1,nkall,3,1.0d0,dxx,1,b_m,3,dtim_in,cs_m,1)
  ! write(*,'(a,8es13.5)')"A1: ", cs_m(1:8)
  ! 4 - calc sin and cos
  CALL vdsincos(nkall,cs_m(1),sin_m(1),cos_m(1))
  ! 5 - Calc COS  like $AC \cdot \cos{ CS }$
  CALL dgemm('N','N',3,1,nkall,1.0d0,ac_m,3,cos_m,nkall,0.0d0,dv_cos,3)
  ! 6 - Calc SIN  like $AS \cdot \sin{ CS }$ and add it to prev val.
  CALL dgemm('N','N',3,1,nkall,1.0d0,as_m,3,sin_m,nkall,1.0d0,dv_cos,3)
  vels(1:3) = dv_cos(1:3)
  RETURN
end subroutine set_vels_at_space_time

!>@brief Calculate velocities in 4Dim. space (XYZ+Time)
subroutine set_vels_at_space_time2(dtim_in,inel,dxx,vels)

  USE comm0, ONLY: ncell
  ! USE comm1, ONLY: xp, u
  USE prec_mod
  USE fire_mpi_var
  USE tmp_mod

  implicit none

  real(prec), intent(in) :: dtim_in !< current time step
  integer, intent(in) :: inel !< current time step
  real(prec),dimension(1:3,1:inel), intent(in) :: dxx !< current time step
  real(prec),dimension(1:3,1:inel), intent(out) :: vels !< current time step
  !------
  integer :: i, j, k, icell, it, ilog, nkall, nel

  real(prec),dimension(1:3,1:inel) :: dv_cos!< current time step
  ! real(prec),dimension(1:nkmod,1:inel) :: dcs!< current time step
  real(prec),dimension(1:nkmod,1:inel) :: dcos_v!< current time step
  real(prec),dimension(1:nkmod,1:inel) :: dsin_v!< current time step
  real(prec),dimension(1:nkmod,1:inel) :: dcs1 !< current time step
  ! real(prec),dimension(1:nmodes*ntimes*inel) :: dcos_v!< current time step
  ! real(prec),dimension(1:nmodes*ntimes*inel) :: dsin_v!< current time step
  real(prec) :: summ, dtmp

  ! 1 - set number of modes in all times
  nkall = nkmod
  nel = inel
  ! dcs(1:nkmod,1:inel) = 0.0d0

  do i = 1, nel
    dcs1(1:nkall,i) = c_m(1:nkall)
  enddo
  ! 2 - COS predicted vars
  ! cs_m(1:nkall) = c_m(1:nkall)

  ! 3 - Calc $ K \cdot x + w t$
  ! CALL dgemm('T','N',nkall,nel,3,1.0d0,b_m,3,dxx,nel,dtim_in,cs_m,nel)
  ! CALL dgemm('N','N',nel,nkall,3,1.0d0,dxx,nel,b_m,3,dtim_in,cs_m,nel)
  CALL dgemm('T','N',nkall,nel,3,1.0d0,b_m,3,dxx,3,dtim_in,dcs1,nkall)
  ! call gemm(b_m, dxx, cs_m ,transa='T',transb='N')
  ! write(*,'(a,8es13.5)')"A2: ", dcs(1:8,1)
  ! write(*,'(a,8es13.5)')"A2: ", cs_m(1:8)
  ! 4 - calc sin and cos
  CALL vdsincos(nkall*nel,dcs1(1,1),dsin_v(1,1),dcos_v(1,1))
  ! do i = 1, inel
  !   j = nkall * (i-1)
  !   CALL vdsincos(nkall,dcs(j),dsin_v(j),dcos_v(j))
  ! enddo
  ! 5 - Calc COS  like $AC \cdot \cos{ CS }$
  ! CALL dgemm('N','N',3,1,nkall,1.0d0,ac_m,3,cos_m,nkall,0.0d0,dv_cos,3)
  CALL dgemm('N','N',3,nel,nkall,1.0d0,ac_m,3,dcos_v,nkall,0.0d0,dv_cos,3)
  ! 6 - Calc SIN  like $AS \cdot \sin{ CS }$ and add it to prev val.
  CALL dgemm('N','N',3,nel,nkall,1.0d0,as_m,3,dsin_v,nkall,1.0d0,dv_cos,3)
  vels(1:3,1:inel) = dv_cos(1:3,1:inel)

  RETURN
end subroutine set_vels_at_space_time2


module scf
use types, only: dp
use linalg, only: eigh
use utils, only: stop_error, str
implicit none
private
public doscf, electronic_energy, get_nuclear_energy, ijkl2intindex, &
        ijkl2intindex2, kinetic_energy, create_intindex_sym4

contains

function formP(C, Noccupied) result(P)
! Forms the density matrix from occupied eigenvectors
real(dp), intent(in) :: C(:, :) ! C(i, j) is the i-th component
    ! of the j-th eigenvector
integer, intent(in) :: Noccupied ! the number of occupied orbitals
real(dp) :: P(size(C, 1), size(C, 1))
integer :: i, j
do i = 1, size(C, 1)
    do j = 1, size(C, 1)
        P(i, j) = 2 * sum(C(i, :Noccupied) * C(j, :Noccupied))
    end do
end do
end function

function formG(P, TT) result(G)
! Assembles Coulomb and exchange term
real(dp), intent(in) :: P(:, :) ! Density matrix
real(dp), intent(in) :: TT(:, :, :, :) ! T(k, l, i, j) = (ij|kl) - 0.5*(il|kj)
real(dp) :: G(size(P, 1), size(P, 1)) ! G(i,j)=sum_kl P(k,l) * T(k, l, i, j)
integer :: i, j
do i = 1, size(P, 1)
    do j = 1, size(P, 1)
        G(i, j) = sum(P * TT(:, :, i, j))
    end do
end do
end function

function formG2(P, int2) result(G)
! Assembles Coulomb and exchange term
! The same as formG, but doesn't require the big matrix TT(:, :, :, :)
real(dp), intent(in) :: P(:, :) ! Density matrix
real(dp), intent(in) :: int2(:)
real(dp), dimension(size(P, 1), size(P, 1)) :: G, TT
integer :: i, j, k, l, n
n = size(P, 1)
do i = 1, n
    do j = 1, n
        do k = 1, n
            do l = 1, n
                TT(k, l) = int2(ijkl2intindex(i, j, k, l)) &
                      - 0.5_dp * int2(ijkl2intindex(i, l, k, j))
            end do
        end do
        G(i, j) = sum(P * TT)
    end do
end do
end function

real(dp) function electronic_energy(P, H, F) result(E0)
! Calculates electronic energy
real(dp), intent(in) :: P(:, :), H(:, :), F(:, :)
E0 = 0.5_dp * sum(P*(H + F))
end function

real(dp) function kinetic_energy(P, T) result(Ekin)
! Calculates kinetic energy
real(dp), intent(in) :: P(:, :), T(:, :)
Ekin = sum(P * T)
end function

subroutine doscf(H, int2, S, Noccupied, Nscf, tolE, tolP, alpha, C, P, lam, E0)
! Runs the SCF cycle
real(dp), intent(in) :: H(:, :), int2(:), S(:, :)
integer, intent(in) :: Noccupied, Nscf
real(dp), intent(in) :: tolE, tolP, alpha
real(dp), intent(out) :: C(:, :), P(:, :), lam(:), E0
real(dp), dimension(size(H, 1), size(H, 1)) :: G, F, Pold
real(dp), dimension(size(H, 1), size(H, 1), size(H, 1), size(H, 1)) :: TT
integer :: i, j, k, l, n
real(dp) :: dP_, Escf(Nscf), Emin, Emax
n = size(H, 1)
print *, "Precalculating J-0.5*K ..."
do i = 1, n
    do j = 1, n
        do k = 1, n
            do l = 1, n
!                print *, i, j, k, l, int2(ijkl2intindex(i, j, k, l)), &
!                    int2(ijkl2intindex(i, l, k, j))
                TT(k, l, i, j) = int2(ijkl2intindex(i, j, k, l)) &
                      - 0.5_dp * int2(ijkl2intindex(i, l, k, j))
!                if (abs(int2(ijkl2intindex(i, j, k, l)) - &
!                    int2(ijkl2intindex(i, l, k, j))) > 1e-10) then
!                    print *, int2(ijkl2intindex(1, 1, 2, 2))
!                    print *, int2(ijkl2intindex(1, 2, 2, 1))
!                    print *, int2(ijkl2intindex(1, 2, 1, 2))
!                    print *, int2(ijkl2intindex(2, 1, 1, 2))
!                    print *, int2(ijkl2intindex(2, 1, 2, 1))
!                    print *, int2(ijkl2intindex(2, 2, 1, 1))
!                    stop "OK"
!                end if
            end do
        end do
    end do
end do
print *, "Done."
P = 0
do i = 1, Nscf
    G = formG(P, TT)
    !G = formG2(P, int2)
    F = H + G
    E0 = electronic_energy(P, H, F)
    Escf(i) = E0
    print *, i, E0
    call eigh(F, S, lam, C)
    Pold = P
    P = formP(C, Noccupied)
    dP_ = sqrt(sum((P - Pold)**2)/4)
    if (i > 3) then
        Emax = maxval(Escf(i-3:i))
        Emin = minval(Escf(i-3:i))
        if (dP_ < tolP .and. Emax-Emin < tolE) then
            write(*,*)  "Self-consistent convergence criterion achieved."
            print *, "SCF Iteration:", i
            print *, "tolE:", tolE
            print *, "tolP:", tolP
            print *, "dP:", dP_
            print *, "Emax-Emin:", Emax-Emin
            return
        end if
    end if
    ! Use linear mixing
    P = (1-alpha)*Pold + alpha*P
end do
call stop_error("SCF did not converge in " // str(Nscf) // " iterations.")
end subroutine

real(dp) function get_nuclear_energy(atno, xyz) result(Enuc)
integer, intent(in) :: atno(:)
real(dp), intent(in) :: xyz(:, :)
integer :: i, j
real(dp) :: R(3)
Enuc = 0
do i = 1, size(atno)
    do j = 1, i-1
        R = xyz(:, i) - xyz(:, j)
        Enuc = Enuc + atno(i)*atno(j) / sqrt(dot_product(R, R))
    end do
end do
end function

integer recursive function ijkl2intindex_slow(i, j, k, l) result(r)
! Index into the int2(:) array using the chemistry notation (ij|kl)
! Here is how to iterate over all (ij|kl) combinations:
!    ijkl = 1
!    do i = 1, n
!        do j = 1, i
!            do k = 1, n
!                do l = 1, k
!                    if ((i-1)*i/2 + j < (k-1)*k/2 + l) cycle
!                    call assert(ijkl2intindex(i, j, k, l) == ijkl)
!                    ijkl = ijkl + 1
!                end do
!            end do
!        end do
!    end do
integer, intent(in) :: i, j, k, l
integer :: ij, kl
if (i < j) then
    r = ijkl2intindex(j, i, k, l)
elseif (k < l) then
    r = ijkl2intindex(i, j, l, k)
else
    ij = (i-1)*i/2 + j
    kl = (k-1)*k/2 + l
    if (ij < kl) then
        r = (kl-1)*kl/2 + ij
    else
        r = (ij-1)*ij/2 + kl
    end if
end if
end function

integer pure function ijkl2intindex(i_, j_, k_, l_) result(r)
! Index into the int2(:) array using the chemistry notation (ij|kl)
! Here is how to iterate over all (ij|kl) combinations:
!    ijkl = 1
!    do i = 1, n
!        do j = 1, i
!            do k = 1, n
!                do l = 1, k
!                    if ((i-1)*i/2 + j < (k-1)*k/2 + l) cycle
!                    call assert(ijkl2intindex(i, j, k, l) == ijkl)
!                    ijkl = ijkl + 1
!                end do
!            end do
!        end do
!    end do
integer, intent(in) :: i_, j_, k_, l_
integer :: i, j, k, l
integer :: ij, kl
if (i_ < j_) then
    i = j_
    j = i_
else
    i = i_
    j = j_
end if
if (k_ < l_) then
    k = l_
    l = k_
else
    k = k_
    l = l_
end if
ij = (i-1)*i/2 + j
kl = (k-1)*k/2 + l
if (ij < kl) then
    r = (kl-1)*kl/2 + ij
else
    r = (ij-1)*ij/2 + kl
end if
end function

integer recursive function ijkl2intindex2(i, j, k, l) result(r)
! Index into the int2(:) array using the chemistry notation (ij|kl)
! Only the 4 general symmetries are assumed.
! Here is how to iterate over all (ij|kl) combinations:
!    ijkl = 1
!    do i = 1, n
!        do j = 1, i
!            do k = 1, i
!                do l = 1, i
!                    if (i == j .and. k < l) cycle
!                    if (i == k .and. j < l) cycle
!                    if (i == l .and. j < k) cycle
!                    call assert(ijkl2intindex2(i, j, k, l) == ijkl)
!                    ijkl = ijkl + 1
!                end do
!            end do
!        end do
!    end do
integer, intent(in) :: i, j, k, l
if (i < j .or. (i == j .and. k < l)) then
    r = ijkl2intindex2(j, i, l, k)
else if (i < k .or. (i == k .and. j < l)) then
    r = ijkl2intindex2(k, l, i, j)
else if (i < l .or. (i == l .and. j < k)) then
    r = ijkl2intindex2(l, k, j, i)
else
    r = getni(i) + getnj(i, j) + getnk(i, j, k) + getnl(i, j, k, l)
end if
end function

integer pure function getnl(i, j, k, l) result(n)
integer, intent(in) :: i, j, k, l
n = l
if (i == j) n = min(k, n)
if (i == k) n = min(j, n)
if (i <= n .and. j < k) n = n - 1
end function

integer pure function getnk(i, j, k_) result(n)
integer, intent(in) :: i, j, k_
integer :: k
n = 0
do k = 1, k_ - 1
    n = n + getnl(i, j, k, i)
end do
end function

integer pure function getnj(i, j_) result(n)
integer, intent(in) :: i, j_
integer :: j
n = 0
do j = 1, j_ - 1
    n = n + getnk(i, j, i+1)
end do
end function

integer pure function getni(i) result(n)
integer, intent(in) :: i
n = (i-1)**2 * ((i-1)**2 + 3) / 4
end function

subroutine create_intindex_sym4(intindex)
integer, intent(out) :: intindex(:, :, :, :)
integer :: i, j, k, l, ijkl, n
n = size(intindex, 1)
! Uncomment the commented lines to run checks:
!intindex = -1
ijkl = 1
do i = 1, n
    do j = 1, i
        do k = 1, i
            do l = 1, i
                if (i == j .and. k < l) cycle
                if (i == k .and. j < l) cycle
                if (i == l .and. j < k) cycle
!                call assert(ijkl2intindex2(i, j, k, l) == ijkl)
                intindex(i, j, k, l) = ijkl
                intindex(j, i, l, k) = ijkl
                intindex(k, l, i, j) = ijkl
                intindex(l, k, j, i) = ijkl
                ijkl = ijkl + 1
            end do
        end do
    end do
end do
!call assert(.not. (any(intindex == -1)))
end subroutine

end module


program catalog_no21

  integer,parameter :: n=139

  real*8 a(n, n), b(n, n), c(n, n)
  !$xev blocking block(k,8,i,8)
  do j=1,n,4
     do k=1,n,4
        do i=1,n
           a(i,j) = a(i,j) + b(i,k)*c(k,j) &
                          + b(i,k+1)*c(k+1,j) &
                          + b(i,k+2)*c(k+2,j) &
                          + b(i,k+3)*c(k+3,j)
           a(i,j+1) = a(i,j+1) + b(i,k)*c(k,j+1) &
                          + b(i,k+1)*c(k+1,j+1) &
                          + b(i,k+2)*c(k+2,j+1) &
                          + b(i,k+3)*c(k+3,j+1)
           a(i,j+2) = a(i,j+2) + b(i,k)*c(k,j+2) &
                          + b(i,k+1)*c(k+1,j+2) &
                          + b(i,k+2)*c(k+2,j+2) &
                          + b(i,k+3)*c(k+3,j+2)
           a(i,j+3) = a(i,j+3) + b(i,k)*c(k,j+3) &
                          + b(i,k+1)*c(k+1,j+3) &
                          + b(i,k+2)*c(k+2,j+3) &
                          + b(i,k+3)*c(k+3,j+3)
         end do
      end do
   end do

end program catalog_no21


!***********************************/
!	Name:セル中心における基礎変数勾配を求めるプログラム
!	Alias:GetGradient
!	Description:※勾配計算の式はスカラーに対するガウスの発散定理から導出される
!	Type:CellCenter
!	Input:Geometory,CCPrimitibeVariable
!	Output:CC%GradientOfVariable
!	Note:法線ベクトルについての考慮は式自体に内包してあるのでいらない
!	Author:Akitaka Toyota
!	Date:2017.10.26
!	Update:2017.11.09
!	Other:
!***********************************/
subroutine GetGradient(Geom,CC)
    use StructVar_Mod
    use LoopVar_Mod
    use ConstantVar_Mod, only:CoreNumberOfCPU
    implicit none
    type(Geometry), intent(in) :: Geom
    type(CellCenter), intent(inout) :: CC
    double precision :: InverseVolume

    nullify(iVar,iCX,iFX,iCY,iFY,iCZ,iFZ)
    iVar => iVariable
    iCX => iCenterX
    iCY => iCenterY
    iCZ => iCenterZ

    InverseVolume = 1.0d0 / Geom%Volume(1)

    if(Geom%Dimension == 1) then
        do iCenterX = 1, Geom%CellNumber(1)
            CC%GradientOfVariable(:,iCX,1,1,1) = 0.5d0*InverseVolume *  &
            &   (CC%PrimitiveVariable(:,iCX+1,1,1) - CC%PrimitiveVariable(:,iCX-1,1,1))
        end do

    else if(Geom%Dimension == 2) then
!$omp parallel num_threads(CoreNumberOfCPU),shared(Geom,CC,InverseVolume),private(iCenterX,iCenterY)
!$omp do
        do iCenterY = 1 , Geom%CellNumber(2)
            do iCenterX = 1, Geom%CellNumber(1)
                CC%GradientOfVariable(:,iCenterX,iCenterY,1,1) = 0.5d0*InverseVolume * Geom%Area(1) * &
                &   (CC%PrimitiveVariable(:,iCenterX+1,iCenterY,1) - CC%PrimitiveVariable(:,iCenterX-1,iCenterY,1))

                CC%GradientOfVariable(:,iCenterX,iCenterY,1,2) = 0.5d0*InverseVolume * Geom%Area(2) * &
                &   (CC%PrimitiveVariable(:,iCenterX,iCenterY+1,1) - CC%PrimitiveVariable(:,iCenterX,iCenterY-1,1))
            end do
        end do
!$omp end do
!$omp end parallel

    else if(Geom%Dimension == 3) then
        do iCenterZ = 1, Geom%CellNumber(3)
            do iCenterY = 1 , Geom%CellNumber(2)
                do iCenterX = 1, Geom%CellNumber(1)
                    CC%GradientOfVariable(:,iCX,iCY,iCZ,1) = 0.5d0*InverseVolume * Geom%Area(1) * &
                    &   (CC%PrimitiveVariable(:,iCX+1,iCY,iCZ) - CC%PrimitiveVariable(:,iCX-1,iCY,iCZ))

                    CC%GradientOfVariable(:,iCX,iCY,iCZ,2) = 0.5d0*InverseVolume * Geom%Area(2) * &
                    &   (CC%PrimitiveVariable(:,iCX,iCY+1,iCZ) - CC%PrimitiveVariable(:,iCX,iCY-1,iCZ))

                    CC%GradientOfVariable(:,iCX,iCY,iCZ,3) = 0.5d0*InverseVolume * Geom%Area(3) * &
                    &   (CC%PrimitiveVariable(:,iCX,iCY,iCZ+1) - CC%PrimitiveVariable(:,iCX,iCY,iCZ-1))
                end do
            end do
        end do
    end if

return
end subroutine GetGradient


#INCLUDE 'MR_H_ALIGN_PADDING.H'
!***********************************************************************************************************************************
! UNIT:
!
!  (MODULE)
!
! PURPOSE:
!
!   TO
!
! DEFINITION OF VARIABLES:
!
!
!
! RECORD OF REVISIONS:
!
!      DATE       |    PROGRAMMER    |    DESCRIPTION OF CHANGE
!      ====       |    ==========    |    =====================
!   20XX-XX-XX    |     DR. HYDE     |    ORIGINAL CODE.
!
!***********************************************************************************************************************************
  MODULE MR_MOD_CALC_EQD_ADV_N_DIF_Z

    USE MR_KINDS

    USE MR_DEF_CURVED_GEOS
    USE MR_DEF_CONSTS_N_REF_PARS
    USE MR_DEF_FIELD_VARS
    USE MR_DEF_ACTIVITY
    USE MR_DEF_TIMING

    USE MR_MOC_K_EPS

    IMPLICIT NONE

    PRIVATE

    PUBLIC :: MR_CALC_EQD_ADV_N_DIF_Z

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

  CONTAINS

!***********************************************************************************************************************************
! UNIT:
!
!  (SUBROUTINE)
!
! PURPOSE:
!
!   TO
!
! DEFINITION OF VARIABLES:
!
!
!
! RECORD OF REVISIONS:
!
!      DATE       |    PROGRAMMER    |    DESCRIPTION OF CHANGE
!      ====       |    ==========    |    =====================
!   20XX-XX-XX    |     DR. HYDE     |    ORIGINAL CODE.
!
!***********************************************************************************************************************************
  SUBROUTINE MR_CALC_EQD_ADV_N_DIF_Z( NI , NJ , NK , EQD_ADV_Z , EQD_DIF_Z )

    USE MR_MOD_OPERATOR_UV

    USE MR_MOD_CALC_QADV_N_QDIF_Z
    USE MR_MOD_CALC_GRAD_Z

    IMPLICIT NONE

    INTEGER(IJID_KIND) , INTENT(IN ) :: NI , NJ
    INTEGER(KKID_KIND) , INTENT(IN ) :: NK

    REAL   (FDRD_KIND) , INTENT(OUT) , DIMENSION(1:NI1(NI,FDRD_KIND),1:NJ,1:NK) :: EQD_ADV_Z , EQD_DIF_Z

    REAL   (FDRD_KIND) , DIMENSION(1:NI1(NI,FDRD_KIND),1:NJ,0:NK) :: EQD_QADV_Z_W , EQD_QDIF_Z_W

    INTEGER(IJID_KIND) :: I , J

    CALL MR_CALC_QADV_N_QDIF_Z_SS_W( NI , NJ , NK , DI , RB , W , EQD_QADV_Z_W , EKZ/SID , VZW , EQD_QDIF_Z_W )

  ! CALCULATE ADVECTION
    CALL MR_CALC_GRAD_Z_SS( NI , NJ , NK , EQD_QADV_Z_W , EQD_ADV_Z )

  ! CALCULATE DIFFUSION
    CALL MR_CALC_GRAD_Z_SS( NI , NJ , NK , EQD_QDIF_Z_W , EQD_DIF_Z )

  END SUBROUTINE MR_CALC_EQD_ADV_N_DIF_Z

  END MODULE MR_MOD_CALC_EQD_ADV_N_DIF_Z


!-------------------------------------------------------------------------------
!> Module Held-Suarez forcing
!!
!! @par Description
!!          This module contains subroutines for forcing term of Held-Suarez test
!!
!! @author NICAM developers, Team SCALE
!<
!-------------------------------------------------------------------------------
module mod_af_heldsuarez
  !-----------------------------------------------------------------------------
  !
  !++ Used modules
  !
  use scale_precision
  use scale_stdio
  use scale_prof

  use mod_adm, only: &
     ADM_LOG_FID
  !-----------------------------------------------------------------------------
  implicit none
  private
  !-----------------------------------------------------------------------------
  !
  !++ Public procedure
  !
  public :: af_heldsuarez_init
  public :: af_heldsuarez

  !-----------------------------------------------------------------------------
  !
  !++ Public parameters & variables
  !
  !-----------------------------------------------------------------------------
  !
  !++ Private procedures
  !
  !-----------------------------------------------------------------------------
  !
  !++ Private parameters & variables
  !
  !-----------------------------------------------------------------------------
contains
  !-----------------------------------------------------------------------------
  subroutine af_heldsuarez_init
    implicit none
    !---------------------------------------------------------------------------

    return
  end subroutine af_heldsuarez_init

  !-----------------------------------------------------------------------------
  subroutine af_heldsuarez( &
       ijdim, &
       lat,   &
       pre,   &
       tem,   &
       vx,    &
       vy,    &
       vz,    &
       fvx,   &
       fvy,   &
       fvz,   &
       fw,    &
       fe     )
    use mod_adm, only: &
       kdim => ADM_kall, &
       kmin => ADM_kmin, &
       kmax => ADM_kmax
    use scale_const, only: &
       Rdry  => CONST_Rdry,  &
       CP    => CONST_CPdry, &
       CV    => CONST_CVdry, &
       PRE00 => CONST_PRE00
    implicit none

    integer, intent(in)  :: ijdim
    real(RP), intent(in)  :: lat(ijdim)
    real(RP), intent(in)  :: pre(ijdim,kdim)
    real(RP), intent(in)  :: tem(ijdim,kdim)
    real(RP), intent(in)  :: vx (ijdim,kdim)
    real(RP), intent(in)  :: vy (ijdim,kdim)
    real(RP), intent(in)  :: vz (ijdim,kdim)
    real(RP), intent(out) :: fvx(ijdim,kdim)
    real(RP), intent(out) :: fvy(ijdim,kdim)
    real(RP), intent(out) :: fvz(ijdim,kdim)
    real(RP), intent(out) :: fw (ijdim,kdim)
    real(RP), intent(out) :: fe (ijdim,kdim)

    real(RP) :: T_eq, acl, asl, ap0
    real(RP), parameter :: T_eq2 = 200.0_RP
    real(RP), parameter :: DT_y  =  60.0_RP ! [K]
    real(RP), parameter :: Dth_z =  10.0_RP ! [K]

    real(RP) :: k_t, k_v
    real(RP) :: sigma, fact_sig
    real(RP), parameter :: sigma_b = 0.7_RP
    real(RP), parameter :: k_f     = 1.0_RP / ( 1.0_RP * 86400.0_RP )
    real(RP), parameter :: k_a     = 1.0_RP / (40.0_RP * 86400.0_RP )
    real(RP), parameter :: k_s     = 1.0_RP / ( 4.0_RP * 86400.0_RP )

    real(RP) :: kappa

    integer :: n, k
    !---------------------------------------------------------------------------

    KAPPA = Rdry / CP

    fvx(:,:) = 0.0_RP
    fvy(:,:) = 0.0_RP
    fvz(:,:) = 0.0_RP
    fw (:,:) = 0.0_RP
    fe (:,:) = 0.0_RP

    do k = kmin, kmax
    do n = 1,    ijdim
       asl = abs( sin(lat(n)) )
       acl = abs( cos(lat(n)) )
       ap0 = abs( pre(n,k) / PRE00 )

       T_eq = (315.0_RP - DT_y*asl*asl - Dth_z*log(ap0)*acl*acl ) * ap0**KAPPA
       T_eq = max(T_eq,T_eq2)

       sigma    = pre(n,k) / ( 0.5_RP * ( pre(n,kmin) + pre(n,kmin-1) ) )
       fact_sig = max( 0.0_RP, (sigma-sigma_b) / (1.0_RP-sigma_b) )

       k_v = k_f * fact_sig

       fvx(n,k) = -k_v * vx(n,k)
       fvy(n,k) = -k_v * vy(n,k)
       fvz(n,k) = -k_v * vz(n,k)

       k_t = k_a + ( k_s-k_a ) * fact_sig * acl**4

       fe(n,k) = -k_t * CV * ( tem(n,k)-T_eq )
    enddo
    enddo

  end subroutine af_heldsuarez

end module mod_af_heldsuarez