MODULE PS7PARA
    IMPLICIT NONE
    REAL(KIND=8), PARAMETER:: BETA=0.99, SIGMA=1, ALPHA=0.36
    INTEGER, PARAMETER:: NK=40, NAK=40
    INTEGER:: I
    REAL(KIND=8), PARAMETER:: K_MIN=0.01, K_MAX=0.5, STEP=(0.5-0.01)/FLOAT(NK-1)
    REAL(KIND=8), DIMENSION(NK), PARAMETER:: K_GRID=(/(I*STEP, I=1, NK)/) +K_MIN - STEP
    REAL(KIND=8), PARAMETER:: KBAR_MIN=0.15, KBAR_MAX=0.25, BAR_STEP=0.1/FLOAT(NAK-1)
    REAL(KIND=8), DIMENSION(NAK), PARAMETER:: AK_GRID=(/(I*BAR_STEP, I=1, NAK)/) +KBAR_MIN - BAR_STEP
    REAL(KIND=8):: A0=0.01, A1=0.99
    REAL(KIND=8), PARAMETER:: CRIT=1E-3
END MODULE

MODULE PS7RES
    USE PS7PARA
    IMPLICIT NONE
    REAL(KIND=8), DIMENSION(NAK):: INTEREST, WAGE
    INTEGER, DIMENSION(NK, NAK):: PFUNC
    REAL(KIND=8), DIMENSION(NK, NAK):: VFUNC,  VFUNC_NEW
    REAL(KIND=8), DIMENSION(NK):: KPR_EST
    REAL(KIND=8), DIMENSION(NK, NAK, NK):: UTIL, CONSUM
    CONTAINS
        SUBROUTINE INIT_RW()
            DO I=1, NAK
                INTEREST(I)= ALPHA*(AK_GRID(I)**(ALPHA-1.))
                WAGE(I)= (1.-ALPHA)*(AK_GRID(I)**ALPHA)
            ENDDO
        END SUBROUTINE
END MODULE

PROGRAM PS7
    USE PS7PARA
    USE PS7RES
    USE OMP_LIB
    IMPLICIT NONE
    INTEGER:: KIDX, AKIDX, KPIDX
    REAL(KIND=8):: ERROR_VFI

        CALL INIT_RW()

        ! FOR PARALLEL CALCULATION, REDUCE PRIVATE VARIABLES IN MAIN LOOP
        ! PRE CALCULATE THE KPR, CONSUM, UTILTIY
        DO AKIDX=1,NAK
            KPR_EST(AKIDX) = AK_GRID(AKIDX) ! NO MOTION
            PRINT*, KPR_EST(AKIDX)
        ENDDO

        UTIL = -1e12
        !$OMP PARALLEL DO DEFAULT(SHARED) PRIVATE(AKIDX, KIDX, KPIDX)
        DO AKIDX=1,NAK
        DO KIDX=1,NK
        DO KPIDX=1,NK ! NEXT PERIOD K CHOICE
            CONSUM(KIDX, AKIDX, KPIDX) = (INTEREST(AKIDX))*K_GRID(KIDX)&
            +WAGE(AKIDX)-K_GRID(KPIDX)
            IF (CONSUM(KIDX, AKIDX, KPIDX)>0.) THEN
                UTIL(KIDX, AKIDX, KPIDX)= LOG(CONSUM(KIDX, AKIDX, KPIDX))
            ENDIF
        ENDDO
        ENDDO
        ENDDO
        !$OMP END PARALLEL DO

    ERROR_VFI = 100.
    DO AKIDX=1,NAK ! INIT VALUE
    DO KIDX=1,NK
        VFUNC(KIDX, AKIDX)= MAXVAL(UTIL(KIDX, AKIDX, :),1)
    ENDDO
    ENDDO
    DO WHILE (ERROR_VFI > CRIT)
        CALL VFI()
        ERROR_VFI = MAXVAL(ABS(VFUNC-VFUNC_NEW))
        VFUNC = VFUNC_NEW
    ENDDO

    CALL WRITE_ALL()
END PROGRAM PS7

SUBROUTINE VFI()
    USE PS7PARA
    USE PS7RES
    USE OMP_LIB
    IMPLICIT NONE
    INTEGER:: AKIDX, KIDX, KPIDX
    REAL(KIND=8), DIMENSION(1):: NEXTU, KPR, SMALL_KPR
    REAL(KIND=8), DIMENSION(NK, NAK, NK):: VFUNC_TMP

    !$OMP PARALLEL DO DEFAULT(SHARED) PRIVATE(AKIDX, KIDX, KPIDX)
    DO AKIDX=1,NAK
        KPR(1) = KPR_EST(AKIDX)
    DO KIDX=1,NK
    DO KPIDX=1,NK ! NEXT PERIOD K CHOICE
        SMALL_KPR(1) = K_GRID(KPIDX)
        CALL PWL_INTERP_2D(NK, NAK, K_GRID, AK_GRID, VFUNC, 1, SMALL_KPR, KPR, NEXTU)
        IF (NEXTU(1)>1E10 .OR. -1E10>NEXTU(1)) THEN
            PRINT*, SMALL_KPR(1),KPR(1), AK_GRID(NAK), K_GRID(NK),AK_GRID(1), K_GRID(1)
        ENDIF
        VFUNC_TMP(KIDX, AKIDX, KPIDX) = UTIL(KIDX, AKIDX, KPIDX)+BETA*NEXTU(1)
    ENDDO
        VFUNC_NEW(KIDX, AKIDX) = MAXVAL(VFUNC_TMP(KIDX, AKIDX, :),1)
        PFUNC(KIDX,AKIDX) = MAXLOC(VFUNC_TMP(KIDX, AKIDX, :),1)
    ENDDO
    ENDDO
    !$OMP END PARALLEL DO
END SUBROUTINE

subroutine pwl_interp_2d ( nxd, nyd, xd, yd, zd, ni, xi, yi, zi )

!*****************************************************************************80
!
!! PWL_INTERP_2D: piecewise linear interpolant to data defined on a 2D grid.
!
!  Discussion:
!
!    Thanks to Adam Hirst for pointing out an error in the formula that
!    chooses the interpolation triangle, 04 February 2018.
!
!  Licensing:
!
!    This code is distributed under the GNU LGPL license.
!
!  Modified:
!
!    04 February 2018
!
!  Author:
!
!    John Burkardt
!
!  Parameters:
!
!    Input, integer ( kind = 4 ) NXD, NYD, the number of X and Y data values.
!
!    Input, real ( kind = 8 ) XD(NXD), YD(NYD), the sorted X and Y data.
!
!    Input, real ( kind = 8 ) ZD(NXD,NYD), the Z data.
!
!    Input, integer ( kind = 4 ) NI, the number of interpolation points.
!
!    Input, real ( kind = 8 ) XI(NI), YI(NI), the coordinates of the
!    interpolation points.
!
!    Output, real ( kind = 8 ) ZI(NI), the value of the interpolant.
!
  implicit none

  integer ( kind = 4 ):: ni
  integer ( kind = 4 ):: nxd
  integer ( kind = 4 ):: nyd

  real ( kind = 8 ):: alpha1
  real ( kind = 8 ):: beta1
  real ( kind = 8 ):: det
  real ( kind = 8 ):: dxa
  real ( kind = 8 ):: dxb
  real ( kind = 8 ):: dxi
  real ( kind = 8 ):: dya
  real ( kind = 8 ):: dyb
  real ( kind = 8 ):: dyi
  real ( kind = 8 ):: gamma
  integer ( kind = 4 ):: i1
  integer ( kind = 4 ):: j
  integer ( kind = 4 ):: k
  real ( kind = 8 ):: r8_huge= 1.79769313486231571D+308
  integer ( kind = 4 ):: r8vec_bracket5
  real ( kind = 8 ):: xd(nxd)
  real ( kind = 8 ):: xi(ni)
  real ( kind = 8 ):: yd(nyd)
  real ( kind = 8 ):: yi(ni)
  real ( kind = 8 ):: zd(nxd,nyd)
  real ( kind = 8 ):: zi(ni)

  do k = 1, ni
!
!  For interpolation point (xi(k),yi(k)), find data intervals I and J so that:
!
!    xd(i) <= xi(k) <= xd(i+1),
!    yd(j) <= yi(k) <= yd(j+1).
!
!  But if the interpolation point is not within a data interval,
!  assign the dummy interpolant value zi(k) = infinity.
!
    i1 = r8vec_bracket5 ( nxd, xd, xi(k) )
    if ( i1 == -1 ) then
      zi(k) = r8_huge
      cycle
    end if

    j = r8vec_bracket5 ( nyd, yd, yi(k) )
    if ( j == -1 ) then
      zi(k) = r8_huge
      cycle
    end if
!
!  The rectangular cell is arbitrarily split into two triangles.
!  The linear interpolation formula depends on which triangle
!  contains the data point.
!
!    (I,J+1)--(I+1,J+1)
!      |\       |
!      | \      |
!      |  \     |
!      |   \    |
!      |    \   |
!      |     \  |
!    (I,J)---(I+1,J)
!
    if ( yi(k) < yd(j+1) &
      + ( yd(j) - yd(j+1) ) * ( xi(k) - xd(i1) ) / ( xd(i1+1) - xd(i1) ) ) then

      dxa = xd(i1+1) - xd(i1)
      dya = yd(j)   - yd(j)

      dxb = xd(i1)   - xd(i1)
      dyb = yd(j+1) - yd(j)

      dxi = xi(k)   - xd(i1)
      dyi = yi(k)   - yd(j)

      det = dxa * dyb - dya * dxb

      alpha1 = ( dxi * dyb - dyi * dxb ) / det
      beta1 =  ( dxa * dyi - dya * dxi ) / det
      gamma = 1.0D+00 - alpha1 - beta1

      zi(k) = alpha1 * zd(i1+1,j) + beta1 * zd(i1,j+1) + gamma * zd(i1,j)

    else

      dxa = xd(i1)   - xd(i1+1)
      dya = yd(j+1) - yd(j+1)

      dxb = xd(i1+1) - xd(i1+1)
      dyb = yd(j)   - yd(j+1)

      dxi = xi(k)   - xd(i1+1)
      dyi = yi(k)   - yd(j+1)

      det = dxa * dyb - dya * dxb

      alpha1 = ( dxi * dyb - dyi * dxb ) / det
      beta1 =  ( dxa * dyi - dya * dxi ) / det
      gamma = 1.0D+00 - alpha1 - beta1

      zi(k) = alpha1 * zd(i1,j+1) + beta1 * zd(i1+1,j) + gamma * zd(i1+1,j+1)

    end if
  end do
end subroutine

function r8vec_bracket5 ( nd, xd, xi )

!*****************************************************************************80
!
!! R8VEC_BRACKET5 brackets data between successive entries of a sorted R8VEC.
!
!  Discussion:
!
!    We assume XD is sorted.
!
!    If XI is contained in the interval [XD(1),XD(N)], then the returned
!    value B indicates that XI is contained in [ XD(B), XD(B+1) ].
!
!    If XI is not contained in the interval [XD(1),XD(N)], then B = -1.
!
!    This code implements a version of binary search which is perhaps more
!    understandable than the usual ones.
!
!  Licensing:
!
!    This code is distributed under the GNU LGPL license.
!
!  Modified:
!
!    14 October 2012
!
!  Author:
!
!    John Burkardt
!
!  Parameters:
!
!    Input, integer ( kind = 4 ) ND, the number of data values.
!
!    Input, real ( kind = 8 ) XD(N), the sorted data.
!
!    Input, real ( kind = 8 ) XD, the query value.
!
!    Output, integer ( kind = 4 ) R8VEC_BRACKET5, the bracket information.
!
  implicit none

  integer ( kind = 4 ) nd

  integer ( kind = 4 ) b
  integer ( kind = 4 ) l
  integer ( kind = 4 ) m
  integer ( kind = 4 ) r
  integer ( kind = 4 ) r8vec_bracket5
  real ( kind = 8 ) xd(nd)
  real ( kind = 8 ) xi

  if ( xi < xd(1) .or. xd(nd) < xi ) then

    b = -1

  else

    l = 1
    r = nd

    do while ( l + 1 < r )
      m = ( l + r ) / 2
      if ( xi < xd(m) ) then
        r = m
      else
        l = m
      end if
    end do

    b = l

  end if

  r8vec_bracket5 = b

  return
end

SUBROUTINE WRITE_ALL()
    USE PS7RES
    USE PS7PARA
    IMPLICIT NONE
    INTEGER:: SROWIDX
    CHARACTER(LEN=130):: PATH="/Users/chek_choi/Downloads/fortran/"
    CHARACTER(LEN=150):: FILE_NAME
    REAL(KIND=8), DIMENSION(1):: NEXTU, SMALL_KPR

        FILE_NAME = TRIM(PATH)//"VFUNC"
        OPEN(UNIT=1, FILE=FILE_NAME, STATUS='REPLACE') ! START WITH THE TWO VALUE FUNCTIONS
        DO SROWIDX=1, NAK
            WRITE(UNIT=1,FMT=*) VFUNC(16,SROWIDX)
        ENDDO
        CLOSE(UNIT=1)

        FILE_NAME = TRIM(PATH)//"VFUNC_SS"
        SMALL_KPR(1) = 0.1995
        OPEN(UNIT=3, FILE=FILE_NAME, STATUS='REPLACE') ! START WITH THE TWO VALUE FUNCTIONS
        DO SROWIDX=1, NAK
            CALL PWL_INTERP_2D(NK, NAK, K_GRID, AK_GRID, VFUNC, 1, SMALL_KPR, AK_GRID(SROWIDX), NEXTU)
                IF (NEXTU(1)> 1E12) THEN
                    PRINT*, AK_GRID(SROWIDX), K_GRID(NK), AK_GRID(NAK)
                ENDIF
            WRITE(UNIT=3,FMT=*) NEXTU(1)
        ENDDO
        CLOSE(UNIT=3)

        FILE_NAME = TRIM(PATH)//"PFUNC"
        OPEN(UNIT=2, FILE=FILE_NAME, STATUS='REPLACE') ! ALSO SAVE POLICY FUNCTIONS
        DO SROWIDX=1, NK
            WRITE(UNIT=2,FMT=*) K_GRID(PFUNC(SROWIDX,NAK/2))
        ENDDO
        CLOSE(UNIT=2)

        FILE_NAME = TRIM(PATH)//"KGRID"
        OPEN(UNIT=4, FILE=FILE_NAME, STATUS='REPLACE') ! ALSO SAVE POLICY FUNCTIONS
        DO SROWIDX=1, NAK
            WRITE(UNIT=4,FMT=*)   K_GRID(SROWIDX)
        ENDDO
        CLOSE(UNIT=4)

END SUBROUTINE


   PROGRAM max_min
   IMPLICIT NONE

   INTEGER (KIND=8),PARAMETER::nH=100,nS=2000
   INTEGER (KIND=8),PARAMETER::N=10000
   REAL (KIND=8),PARAMETER:: Jmp=0.44
   REAL (KIND=8),PARAMETER:: Jc=1./2.28, H0=0.43
   REAL (KIND=8),PARAMETER:: Smin=-1.,Smax=1,z=4.

   INTEGER (KIND=8) :: i,iS
   REAL    (KIND=8) :: T,J,S,delS,JMF

   REAL (KIND=8),DIMENSION(nS) :: Smp,P,F,FMF

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

!!!! =====================================================================================
!!! F by mean field

   OPEN(13,file='FMF.dat')

   !JMF=(1./z/Jc)*Jmp
   JMF=1.1/z

   WRITE(*,*)'JMF=',JMF

   IF (nS==1) THEN
      delS=0.
   ELSE
      delS=(Smax-Smin)/real(nS)
   END IF

   DO iS=1,nS
      S=Smin+delS*real(iS-0.5)

      !FMF=-0.5*z*JMF*S*S + S*(1.+exp(-2.*S))/(1.-exp(-2.*S))+0.5*log(1.-S*S)

      !FMF=-0.5*z*JMF*S*S + S*(exp(S)+exp(-S))/(exp(S)-exp(-S))+0.5*log(1.-(S*S))

      FMF(iS)=-0.5*z*JMF*S*S + S*0.5*log((1.+S)/(1.-S))+0.5*log(1.-(S*S))

      !FMF(iS)=-log(2.)+0.5*(1.-z*JMF)*S*S+(1./12.)*(S**4.)

   WRITE(13,*)S,FMF(iS)
   END DO

!!!! =====================================================================================
!!! F by numerical method

!!! Doc gia tri
   OPEN(unit=11,file='P044.txt')
   OPEN(unit=12,file='F.dat')

   DO i=1,nS
      READ(11,*)T,J,Smp(i),P(i)

      F(i)=log(P(i))/real(N)

   END DO

   DO i=1,nS
      !!WRITE(12,*)Smp(i)/N,F(i)*FMF(1001)/F(1001)

      WRITE(12,*)Smp(i)/N,F(i)
   END DO

   CLOSE(11)
   CLOSE(12)

   END PROGRAM max_min


subroutine lca(basis, stimuli, eta, lamb, nIter, softThresh, adapt, s, u, thresh, nBasis, nStimuli, length)
  implicit none
!
! Inputs
!
  integer, parameter :: dp = KIND(1.0d0)
  integer, parameter :: li = SELECTED_INT_KIND(8)
  real(dp), parameter :: alpha = 1.0
  real(dp), parameter :: beta = 0.0
  integer(li), intent(in) :: nIter, softThresh, nBasis, nStimuli, length
  real(dp), intent(in), dimension(0:nBasis-1, 0:length-1) :: basis
  real(dp), intent(in), dimension(0:nStimuli-1, 0:length-1) :: stimuli
  real(dp), intent(in) :: eta, lamb, adapt
!
! Outputs
!
  real(dp), intent(inout), dimension(0:nStimuli-1, 0:nBasis-1) :: u
  !f2py intent(inout) :: u
  real(dp), intent(inout), dimension(0:nStimuli-1, 0:nBasis-1) :: s
  !f2py intent(inout) :: s
  real(dp), intent(inout), dimension(0:nStimuli-1) :: thresh
  !f2py intent(inout) :: thresh

  real(dp), dimension(0:nStimuli-1, 0:nBasis-1) :: b, ci
  real(dp), dimension(0:nBasis-1,0:nBasis-1) :: c
  integer(li) :: ii,jj,kk

  external :: DGEMM, DSYMM
  real(dp), external :: DDOT

  call DGEMM("n","t",nBasis,nBasis,length,alpha,basis,nBasis,basis,nBasis,beta,c,nBasis)
  do ii=0,nbasis-1
     c(ii,ii) = 0.0
  end do
  call DGEMM("n","t",nStimuli,nBasis,length,alpha,stimuli,nStimuli,basis,nBasis,beta,b,nStimuli)
  do ii=0,nStimuli-1
    do jj=0,nBasis-1
      thresh(ii) = thresh(ii)+ABS(b(ii,jj))
    end do
    thresh(ii) = thresh(ii)/nBasis
  end do
  do jj=0,nIter-1
     call DSYMM("r","l",nStimuli,nBasis,alpha,c,nBasis,s,nStimuli,beta,ci,nStimuli)
     u = eta*(b-ci)+(1-eta)*u
     do kk=0,nBasis-1
        do ii=0,nStimuli-1
           if ((u(ii,kk) < thresh(ii)) .and. (u(ii,kk) > -thresh(ii))) then
              s(ii,kk) = 0.
           else if (softThresh .eq. 1) then
              s(ii,kk) = u(ii,kk)-sign(u(ii,kk),u(ii,kk))*thresh(ii)
           else
              s(ii,kk) = u(ii,kk)
           end if
        end do
     end do
     do ii=0,nStimuli-1
        if (thresh(ii) > lamb) then
           thresh(ii) = adapt*thresh(ii)
        end if
     end do
  end do
end subroutine


      SUBROUTINE broydn(x,n,check)
      INTEGER n,nn,NP,MAXITS
      REAL x(n),fvec,EPS,TOLF,TOLMIN,TOLX,STPMX
      LOGICAL check
      PARAMETER (NP=40,MAXITS=200,EPS=1.e-7,TOLF=1.e-4,TOLMIN=1.e-6,
     *TOLX=EPS,STPMX=100.)
      COMMON /newtv/ fvec(NP),nn
CU    USES fdjac,fmin,lnsrch,qrdcmp,qrupdt,rsolv
      INTEGER i,its,j,k
      REAL den,f,fold,stpmax,sum,temp,test,c(NP),d(NP),fvcold(NP),g(NP),
     *p(NP),qt(NP,NP),r(NP,NP),s(NP),t(NP),w(NP),xold(NP),fmin
      LOGICAL restrt,sing,skip
      EXTERNAL fmin
      nn=n
      f=fmin(x)
      test=0.
      do 11 i=1,n
        if(abs(fvec(i)).gt.test)test=abs(fvec(i))
11    continue
      if(test.lt..01*TOLF)then
        check=.false.
        return
      endif
      sum=0.
      do 12 i=1,n
        sum=sum+x(i)**2
12    continue
      stpmax=STPMX*max(sqrt(sum),float(n))
      restrt=.true.
      do 44 its=1,MAXITS
        if(restrt)then
          call fdjac(n,x,fvec,NP,r)
          call qrdcmp(r,n,NP,c,d,sing)
          if(sing) pause 'singular Jacobian in broydn'
          do 14 i=1,n
            do 13 j=1,n
              qt(i,j)=0.
13          continue
            qt(i,i)=1.
14        continue
          do 18 k=1,n-1
            if(c(k).ne.0.)then
              do 17 j=1,n
                sum=0.
                do 15 i=k,n
                  sum=sum+r(i,k)*qt(i,j)
15              continue
                sum=sum/c(k)
                do 16 i=k,n
                  qt(i,j)=qt(i,j)-sum*r(i,k)
16              continue
17            continue
            endif
18        continue
          do 21 i=1,n
            r(i,i)=d(i)
            do 19 j=1,i-1
              r(i,j)=0.
19          continue
21        continue
        else
          do 22 i=1,n
            s(i)=x(i)-xold(i)
22        continue
          do 24 i=1,n
            sum=0.
            do 23 j=i,n
              sum=sum+r(i,j)*s(j)
23          continue
            t(i)=sum
24        continue
          skip=.true.
          do 26 i=1,n
            sum=0.
            do 25 j=1,n
              sum=sum+qt(j,i)*t(j)
25          continue
            w(i)=fvec(i)-fvcold(i)-sum
            if(abs(w(i)).ge.EPS*(abs(fvec(i))+abs(fvcold(i))))then
              skip=.false.
            else
              w(i)=0.
            endif
26        continue
          if(.not.skip)then
            do 28 i=1,n
              sum=0.
              do 27 j=1,n
                sum=sum+qt(i,j)*w(j)
27            continue
              t(i)=sum
28          continue
            den=0.
            do 29 i=1,n
              den=den+s(i)**2
29          continue
            do 31 i=1,n
              s(i)=s(i)/den
31          continue
            call qrupdt(r,qt,n,NP,t,s)
            do 32 i=1,n
              if(r(i,i).eq.0.) pause 'r singular in broydn'
              d(i)=r(i,i)
32          continue
          endif
        endif
        do 34 i=1,n
          sum=0.
          do 33 j=1,n
            sum=sum+qt(i,j)*fvec(j)
33        continue
          g(i)=sum
34      continue
        do 36 i=n,1,-1
          sum=0.
          do 35 j=1,i
            sum=sum+r(j,i)*g(j)
35        continue
          g(i)=sum
36      continue
        do 37 i=1,n
          xold(i)=x(i)
          fvcold(i)=fvec(i)
37      continue
        fold=f
        do 39 i=1,n
          sum=0.
          do 38 j=1,n
            sum=sum+qt(i,j)*fvec(j)
38        continue
          p(i)=-sum
39      continue
        call rsolv(r,n,NP,d,p)
        call lnsrch(n,xold,fold,g,p,x,f,stpmax,check,fmin)
        test=0.
        do 41 i=1,n
          if(abs(fvec(i)).gt.test)test=abs(fvec(i))
41      continue
        if(test.lt.TOLF)then
          check=.false.
          return
        endif
        if(check)then
          if(restrt)then
            return
          else
            test=0.
            den=max(f,.5*n)
            do 42 i=1,n
              temp=abs(g(i))*max(abs(x(i)),1.)/den
              if(temp.gt.test)test=temp
42          continue
            if(test.lt.TOLMIN)then
              return
            else
              restrt=.true.
            endif
          endif
        else
          restrt=.false.
          test=0.
          do 43 i=1,n
            temp=(abs(x(i)-xold(i)))/max(abs(x(i)),1.)
            if(temp.gt.test)test=temp
43        continue
          if(test.lt.TOLX)return
        endif
44    continue
      pause 'MAXITS exceeded in broydn'
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/broydn.for


module utility_interpolate

    use define_indexes
    use define_indexes
    use define_keys
    use define_globals
    use define_settings
    use utility_allocate

    implicit none

    public :: util_curve_lookup_singular
    public :: util_interpolate_linear

    private

contains
!%
!%==========================================================================
!%==========================================================================
!%

    real(8) function util_interpolate_linear(X, X1, X2, Y1, Y2) result (Y)
    !% This is a linear interpolation function
        real(8), intent(in) :: X, X1, X2, Y1, Y2
        if (Y1 == Y2) then
            Y = Y1
            return
        end if
        Y = Y1 + (X - X1) * (Y2 - Y1) / (X2 - X1)
    end function util_interpolate_linear
!%
!%==========================================================================
!%==========================================================================
!%
    subroutine util_curve_lookup_singular(curveID, er_inCol, er_outCol, &
        xVal_col, yVal_col)
        !%-----------------------------------------------------------------------------
        !% Description:
        !% look up from a curve and returns a interpolated value
        !% Inputs/Outputs:
        !%      curveID
        !%      er_inCol
        !%      er_outCol
        !%      xVal_col
        !%      yVal_col
        !%-----------------------------------------------------------------------------
        integer, intent(in) :: curveID, er_inCol, er_outCol, xVal_col, yVal_col
        real(8), pointer    :: x, y
        integer, pointer    :: nRows, ElemIdx
        real(8) :: x1, x2, y1, y2, slope
        integer :: ii

        character(64) :: subroutine_name = 'util_curve_lookup_singular'
        !%-----------------------------------------------------------------------------
        !% pointers:
        ElemIdx => curve(curveID)%ElemIdx
        nRows   => curve(curveID)%NumRows
        x       => elemR(ElemIdx,er_inCol)
        y       => elemR(ElemIdx,er_outCol)

        x1 = curve(curveID)%ValueArray(1,xVal_col)
        y1 = curve(curveID)%ValueArray(1,yVal_col)

        !% check if inVal is smaller than the first table entry
        if (x <= x1) then
            if (x1 > zeroR) then
                y = x/x1*y1
                return
            else
                y = y1
                return
            end if
        end if

        !% else loop through the table to find the position and interpolate
        do ii = 2,nRows
            x2 = curve(curveID)%ValueArray(ii,xVal_col)
            y2 = curve(curveID)%ValueArray(ii,yVal_col)
            if (x2 /= x1) slope = (y2 - y1) / (x2 - x1)

            if (x <= x2) then
                y = util_interpolate_linear(x, x1, x2, y1, y2)
                return
            end if
            x1 = x2
            y1 = y2
            if (slope < zeroR) slope = zeroR
            y = y1 + slope * (x - x1)
        end do

    end subroutine util_curve_lookup_singular
!%
!%==========================================================================
!%==========================================================================
!%
end module utility_interpolate


C
	SUBROUTINE ROTATE_X(ANGLE,X_INIT,X_TRANS)
C
	REAL X_INIT(3),X_TRANS(3)
C
	X_TRANS(1)=X_INIT(1)
	X_TRANS(2)=X_INIT(2)*COS(ANGLE)+X_INIT(3)*SIN(ANGLE)
	X_TRANS(3)=X_INIT(3)*COS(ANGLE)-X_INIT(2)*SIN(ANGLE)
C
	RETURN
	END


program matrixMultiplication
  implicit none
  integer, parameter :: n = 30000 ! Parameter N defining size of matrices
  real(kind=8),allocatable::m1(:,:),v1(:),v_res(:) ! Defining 1 matrix that will be allocatated in memory during executions
  ! plus two vectors
  integer(kind=4)::i, j, k ! Iterators
  real::start,finish ! Variables to make timing

allocate(m1(n,n),v1(n),v_res(n)) ! Alocating the matrices based on parameter n

! Filling up vpalues to matrix 1
do i = 1, n
 do j = 1, n
    m1(i, j) = i*i*1.7453+j*3.674528 ! This is a "random" operation in order not to allow the compiler taking advantage of the matrix value
  end do
end do

! Filling up values to matrix 2
do i = 1, n
    v1(i) = i*i*1.3569875*6.76512 ! This is a "random" operation in order not to allow the compiler taking advantage of the matrix value
end do

call cpu_time(start) ! Start timing
do i = 1, n
  do j = 1, n
      v_res(i) = v_res(i) + (m1(i,j) * v1(j))
  end do
end do
call cpu_time(finish) ! Finish timing
print '("Time to Row = ",f15.3," seconds. Started:",f6.3," - Finished:",f15.3,".")',finish-start,start,finish !Prints the time

call cpu_time(start) ! Start timing
do j = 1, n
  do i = 1, n
      v_res(i) = v_res(i) + (m1(i,j) * v1(j))
  end do
end do
call cpu_time(finish) ! Finish timing
print '("Time to Column = ",f15.3," seconds. Started:",f6.3," - Finished:",f15.3,".")',finish-start,start,finish !Prints the time

print*, 'Matrix Multiplication: Result Matrix' ! Show a result just to make sure...

print*, v_res(1), ' ' , v_res(2) , ' ' , v_res(3)

end program matrixMultiplication


module array_utils

  use numeric_kinds, only: dp
  use lapack_wrapper, only: lapack_generalized_eigensolver, lapack_matmul, lapack_matrix_vector, &
       lapack_qr, lapack_solver, lapack_sort
  implicit none

  !> \private
  private
  !> \public
  public :: concatenate, diagonal,eye, generate_diagonal_dominant, norm, &
       generate_preconditioner

contains

    pure function eye(m, n, alpha)
    !> Create a matrix with ones in the diagonal and zero everywhere else
    !> \param m: number of rows
    !> \param n: number of colums
    !> \param alpha: optional diagonal value
    !> \return matrix of size n x m
    integer, intent(in) :: n, m
    real(dp), dimension(m, n) :: eye
    real(dp), intent(in), optional :: alpha

    !local variable
    integer :: i, j
    real(dp) :: x

    ! check optional values
    x = 1.d0
    if (present(alpha)) x = alpha

    do i=1, m
       do j=1, n
          if (i /= j) then
             eye(i, j) = 0.d0
          else
             eye(i, i) = x
          end if
       end do
    end do

  end function eye

  pure function norm(vector)
    !> compute the norm-2 of a vector
    real(dp), dimension(:), intent(in) :: vector
    real(dp) :: norm

    norm = sqrt(sum(vector ** 2.d0))

  end function norm

  subroutine concatenate(arr, brr)

    !> Concatenate two matrices
    !> \param arr: first array
    !> \param brr: second array
    !> \return arr concatenate brr (overwrites arr)

    real(dp), dimension(:, :), intent(inout), allocatable :: arr
    real(dp), dimension(:, :), intent(in) :: brr
    real(dp), dimension(:, :), allocatable :: tmp_array
    integer :: new_dim, dim_cols, dim_rows

    ! dimension
    dim_rows = size(arr, 1)
    dim_cols = size(arr, 2)

    ! Number of columns of the new matrix
    new_dim = dim_cols + size(brr, 2)

    ! move to temporal array
    allocate(tmp_array(dim_rows, new_dim))
    tmp_array(:, :dim_cols) = arr

    ! Move to new expanded matrix
    deallocate(arr)
    call move_alloc(tmp_array, arr)

    arr(:, dim_cols + 1:) = brr

  end subroutine concatenate

  function generate_diagonal_dominant(m, sparsity, diag_val) result(arr)
    !> Generate a diagonal dominant square matrix of dimension m
    !> \param m dimension of the matrix
    !> \param sparsity magnitude order of the off-diagonal values

    integer, intent(in) :: m ! size of the square matrix
    real(dp), optional :: diag_val
    integer :: i, j
    real(dp) :: sparsity
    real(dp), dimension(m, m) :: arr
    call random_number(arr)

    arr = arr * sparsity
    do j=1, m
       do i=1, m
          if (i > j) then
             arr(i, j) = arr(j, i)
          else if(i == j) then
            if (present(diag_val))then
              arr(i,i) = diag_val
            else
             arr(i, i) = i
            end if
          end if
       end do
    end do

  end function generate_diagonal_dominant

  function diagonal(matrix)
    !> return the diagonal of a matrix
    real(dp), dimension(:, :), intent(in) :: matrix
    real(dp), dimension(size(matrix, 1)) :: diagonal

    ! local variables
    integer :: i, j, m

    ! dimension of the matrix
    m = size(matrix, 1)

    do i=1,m
       do j=1,m
          if  (i == j) then
             diagonal(i) = matrix(i, j)
          end if
       end do
    end do

  end function diagonal

  function generate_preconditioner(diag, dim_sub) result(precond)
    !> \brief generates a diagonal preconditioner for `matrix`.
    !> \return diagonal matrix

    ! input variable
    real(dp), dimension(:), intent(inout) :: diag
    integer, intent(in) :: dim_sub

    ! local variables
    real(dp), dimension(size(diag), dim_sub) :: precond
    integer, dimension(size(diag)) :: keys
    integer :: i, k

    ! sort diagonal
    keys = lapack_sort('I', diag)
    ! Fill matrix with zeros
    precond = 0.0_dp

    ! Add one depending on the order of the matrix diagonal
    do i=1, dim_sub
       k = search_key(keys, i)
       precond(k, i) = 1.d0
    end do

  end function generate_preconditioner

  function search_key(keys, i) result(k)
    !> \brief Search for a given index `i` in a vector `keys`
    !> \param keys Vector of index
    !> \param i Index to search for
    !> \return index of i inside keys

    integer, dimension(:), intent(in) :: keys
    integer, intent(in) :: i
    integer :: j, k

    do j=1,size(keys)
       if (keys(j) == i) then
          k = j
          exit
       end if
    end do

  end function search_key

end module array_utils


      SUBROUTINE NON_LINEAR09 (IS, L, M, U, X, Y)

      IMPLICIT REAL*8 (A-H, O-Z)
      DIMENSION U(1), X(1), Y(1)
C
C   Set initial parameters.
C
      N = 2 ** M
      K = U(1)
      NU = K / 64
      N1 = N / 2
      LK = 2 ** (L - 1)
      LI = 2 ** (M - L)
      LJ = 2 * LI
      KU = LI + 1
      KN = KU + NU
C
      DO 100 K = 0, LK - 1
        I11 = K * LJ + 1
        I12 = I11 + LI
        I21 = K * LI + 1
        I22 = I21 + N1
C
C   This loop is vectorizable.
C
        DO 100 I = 0, LI - 1
          U1 = U(KU+I)
          U2 = IS * U(KN+I)
          X11 = X(I11+I)
          X12 = X(I11+I+N)
          X21 = X(I12+I)
          X22 = X(I12+I+N)
          T1 = X11 - X21
          T2 = X12 - X22
          Y(I21+I) = X11 + X21
          Y(I21+I+N) = X12 + X22
          Y(I22+I) = U1 * T1 - U2 * T2
          Y(I22+I+N) = U1 * T2 + U2 * T1
 100  CONTINUE
C
      RETURN
      END


PROGRAM SELF_Geometry1D_IO

USE SELF_Constants
USE SELF_Lagrange
USE SELF_Mesh
USE SELF_Geometry

  IMPLICIT NONE
  INTEGER, PARAMETER :: N = 7 ! Polynomial degree of solution
  INTEGER, PARAMETER :: quadrature = GAUSS ! Quadrature
  INTEGER, PARAMETER :: M = 15 ! Number of points in the uniform plotting mesh
  INTEGER, PARAMETER :: nXe = 1 ! Number of elements in the x-direction
  INTEGER, PARAMETER :: nvar = 1 ! The number of tracer fields
  REAL(prec), PARAMETER :: Lx = 1.0_prec ! Length of the domain in the x-direction
  REAL(prec), PARAMETER :: tolerance=10.0_prec*epsilon(1.0_prec) ! Error tolerance

  TYPE(Lagrange),TARGET :: interp
  TYPE(Mesh1D),TARGET :: mesh
  TYPE(Geometry1D),TARGET :: geometry
  REAL(prec) :: dxdsExpect
  REAL(prec) :: error
  INTEGER :: i, iEl
  LOGICAL :: fail

    fail = .FALSE.

    ! Create an interpolant
    CALL interp % Init(N,quadrature,M,UNIFORM)

    ! Create a uniform block mesh
    CALL mesh % UniformBlockMesh(N,nXe,(/0.0_prec,Lx/))

    ! Generate geometry (metric terms) from the mesh elements
    CALL geometry % Init(interp,mesh % nElem)
    CALL geometry % GenerateFromMesh(mesh)

    dxdsExpect = 0.5_prec*Lx/REAL(nXe,prec)
    error = 0.0_prec
    DO iEl = 1, mesh % nElem
      DO i = 0, interp % N
        error = MAX(ABS(geometry % dxds % interior % hostData(i,1,iEl)-dxdsExpect),error)
      ENDDO
    ENDDO

    IF( error > tolerance )THEN
      1000 FORMAT('Element interior metric term error max(|dxds-exact|) greater than tolerance : ',E12.5, ' > ', E12.5)
      WRITE(*,1000) error, tolerance
      fail=.TRUE.
    ENDIF

    error = 0.0_prec
    DO iEl = 1, mesh % nElem
      error = MAX(ABS(geometry % dxds % boundary % hostData(1,1,iEl)-dxdsExpect),error)
      error = MAX(ABS(geometry % dxds % boundary % hostData(1,2,iEl)-dxdsExpect),error)
    ENDDO

    IF( error > tolerance )THEN
      1001 FORMAT('Element boundary metric term error max(|dxds-exact|) greater than tolerance : ',E12.5, ' > ', E12.5)
      WRITE(*,1001) error, tolerance
      fail=.TRUE.
    ENDIF

    CALL geometry % Write()
    CALL geometry % WriteTecplot()

    CALL geometry % Free()
    CALL mesh % Free()
    CALL interp % Free()

    IF( fail )THEN
      STOP 1
    ENDIF

END PROGRAM SELF_Geometry1D_IO


subroutine relative_vorticity(m,n,o,u,v,deltax,deltay,output_array)
implicit none
integer :: m,n,o,mone,none
integer :: x,y,t
double precision ::deltax(n),deltay
double precision :: u(m,n,o)
double precision :: v(m,n,o)
double precision :: output_array(m,n,o)
double precision :: a
a= 0.5
mone=m-2
none=n-2
do t=1,o
 do x=2,mone
  do y=2,none
   output_array(x,y,t) = (-1.00 *a * v(x,y-1,t)/deltax(y)) &
    + (a * v(x,y+1,t)/deltax(y)) &
    - (-1.00 * a * u(x-1,y,t)/deltay) &
    + (a * u(x+1,y,t)/deltay)
  end do
 end do
end do
end subroutine relative_vorticity


PROGRAM SPIRAL

  IMPLICIT NONE

  INTEGER, PARAMETER :: size = 5
  INTEGER :: i, x = 0, y = 1, count = size, n = 0
  INTEGER :: array(size,size)

  DO i = 1, count
    x = x + 1
      array(x,y) = n
    n = n + 1
  END DO

  DO
    count = count  - 1
      DO i = 1, count
        y = y + 1
        array(x,y) = n
        n = n + 1
      END DO
      DO i = 1, count
        x = x - 1
        array(x,y) = n
        n = n + 1
      END DO
      IF (n > size*size-1) EXIT
      count = count - 1
      DO i = 1, count
        y = y - 1
        array(x,y) = n
        n = n + 1
      END DO
      DO i = 1, count
        x = x + 1
        array(x,y) = n
        n = n + 1
      END DO
      IF (n > size*size-1) EXIT
  END DO

  DO y = 1, size
    DO x = 1, size
      WRITE (*, "(I4)", ADVANCE="NO") array (x, y)
    END DO
    WRITE (*,*)
  END DO

END PROGRAM SPIRAL


C@a====================================================================*
      PROGRAM profile
*----------------------------------------------------------------------*
C!  Outputs the velocity profile to be used as input for T-Rex         *
C@e--------------------------------------------------------------------*
      IMPLICIT NONE
*----------------------------------------------------------------------*
      REAL*8  L, x, v
      INTEGER N, i
c#rcs-----------------------
      character*80 rcs1,rcs2
      data rcs1/
     >'$Id: profile.f,v 1.3 2002/06/01 17:17:40 bojan Exp $'/
      data rcs2/
     >'$Source: /home/IUS/mhadziabdic/.CVSROOT/T-Rex/Test/profile.f,v $'/
C@d====================================================================*

* Number of points
      N=200

      write(*,'(A20)') '#==================#'
      write(*,'(A20)') '# Number of points #'
      write(*,'(A20)') '#==================#'
      write(*,*) N
      write(*,'(A13)') '#===========#'
      write(*,'(A13)') '# Direction #'
      write(*,'(A13)') '#===========#'
      write(*,*) 'z'
      write(*,'(A20)') '#==================#'
      write(*,'(A20)') '# coordinate U V W #'
      write(*,'(A20)') '#==================#'
* Parabolic profile in range 0:2, extrema=1 *
c      L = 2.d0
c      do i=0,N
c        x = real(i) * L/real(n)
c        v = 1.d0 - (x-L/2.d0)**2
c        write(*,'(4F18.6)') x, v, 0.d0, 0.d0
c      end do

* Parabolic profile in range 0:3, extrema=1 *
!      L = 3.d0
!      do i=0,N
!        x = real(i) * L/real(N)
!        v = (2.25-(x-1.5)*(x-1.5))/2.25
!        write(*,'(4F18.6)') x, v, 0.d0, 0.d0
!      end do

* Parabolic profile in range 0:1, extrema=1 *
      L = 1.d0
      do i=0,N
        x = real(i) * L/real(n)
        v = 6.d0 * x * (1.d0 - x)
        write(*,'(5F18.6)') x, v, 0.d0, 0.d0, 0.d0
      end do

      END


module numerical

implicit none

contains

elemental logical function isprime(i)

integer, intent(in) :: i
integer :: j

isprime = .false.

!! trivial tests

if (i<=1) return

if (i==2 .or. i==3) then
  isprime = .true.
  return
endif

if (modulo(i,2) == 0 .or. modulo(i,3) == 0) return

!! 6k+1 test

do j = 5, int(sqrt(real(i))), 6
  if (modulo(i,j) == 0 .or. modulo(i,j+2) == 0) return
enddo

isprime = .true.

end function isprime

end module numerical


program sqrt

use, intrinsic :: iso_c_binding
implicit none

character(len=5) :: arg
integer :: nargs

interface
    integer(c_int) function passarrays (in, out) bind (c)
      use iso_c_binding
      implicit none
      real(c_double), intent(in) :: in(4)
      real(c_double), intent(out) :: out(4)
    end function
    function initializer () bind (c)
      use iso_c_binding
      integer(c_int) :: initializer
    end function
    function nr_sqrt ( input, x0, iterations, printIts ) bind (c)
      use iso_c_binding
      real(c_double), value :: input
      real(c_double), value :: x0
      integer(c_int), value :: iterations
      logical(c_bool), value :: printIts
      real(c_double) :: nr_sqrt
    end function nr_sqrt
end interface

! arguments for the c routines
integer(c_int) :: errstat
real(c_double) :: input
real(c_double) :: x0 = 10
integer(c_int) :: iterations = 10
logical(c_bool) :: printIts = .TRUE.
real(c_double) :: result
integer ( c_int ) :: result_int
real(C_DOUBLE), dimension(0:3) :: in, out

! passing arrays to c and back
in = (/1,2,3,4/)
print *, "uninitialized out: ", out
result_int = passarrays(in, out)
print *, "after c out: ", out

! initialize the c routines
errstat = initializer()

! read the input argument and convert to real
nargs = command_argument_count()
if (nargs.ne.1) then
  print *, "Usage: sqrt N where N is a real number"
  stop 1
endif

call getarg(1, arg)
read(arg,*) input

! get the square root from newton raphson method
result = nr_sqrt(input, x0, iterations, printIts)

! display the results
print "(3A,F10.4)", "The square root of ",trim(arg)," is ",result

end program


!+MODULE NAME
!  INTERPOLATION
!
!+MODULE DESCRIPTION
!   This module contains routines for trilinear and tricubic interpolation of 3D gridded
!   data. Trilinear interpolation is a triply-iterated linear interpolation. Tricubic is
!   an implementation of a local tricubic interpolation scheme based on the paper
!   by F. Leiken and J. Marsden, "Tricubic Interpolation in Three Dimensions".
!   This implementation differs from others (e.g., iterated cubic spline) because it is purely local --
!   rather than using multiple points, it relies on multiple derivatives. When these derivatives are
!   not explicitly available, they can be approximated using finite difference methods. Currently,
!   finite differences are approximated using the FINITE_DIFFERENCES_CS2 module which estimates the
!   derivatives using a quadratic polynomial fit.
!
!   This code is not currently configured to directly handle AMR grids, so such data must be
!   pre-processed onto a cartesian mesh.
!
!+GLOBAL DATA
!  GRID data type
!
!+ROUTINES
!  TRICUBIC_INIT          Subroutine    -> Calcluate tricubic interpolation coefficients for a given grid cell
!  GRID_INIT              Subroutine    -> Initialize a grid object
!  TRILINEAR_INTERPOLATE  Subroutine    -> Evaluate a trilinear interpolant using the data in a GRID object
!  TRICUBIC_INTERPOLATE   Subroutine    -> Evaluate a tricubic interpolant using pre-calculated coefficients from TRICUBIC_INIT and the data in a GRID object
!  LOCATE                 Function      -> Search an ordered array for the lower bracketing index, i.e. the index of the
!                                          element of the array whose value is closest to a specified value without going over
!
!+DEPENDENCIES
!  NONE
!
!+USAGE
!   1. Initialize the grid by calling GRID_INIT()
!   2. Determine the local interpolation coefficients by calling TRICUBIC_INIT()
!      with appropriate inputs
!   3. Calculate the local value of the desired quantities using trilinear_interpolate() and
!      tricubic_interpolate() routines with appropriate inputs
!   4. Note that repeated evaluations of the tricubic interpolant can be carried out for different values
!      of the independent variables so long as they remain within the region of validity for the grid object
!
!+AUTHOR
!  Jesse Woodroffe
!  jwoodroffe@lanl.gov
!
!+REVISION HISTORY
!   16 January 2015: Original code
!   17 January 2015: Added tricubic_init and removed calculation of B_INV from tricubic_setup
!   18 January 2015: Added matrix_inverse module with matrix inversion routine
!                    Added INTERPOLANT data type and code snippets for use in future implementations
!   20 January 2015: Changed code to fully support use of interpolant data type and added test program
!   21 January 2015: ::CODE FORKED TO VERSION 2:: Revised derivative scaling in test program. Modified
!                    interpolant object and moved derivative scaling to the evaluation procedures so
!                    that interpolation is transparent to the user. The example program has been
!                    modified to minimize the number of times tricubic_setup() has to be called.
!   22 January 2015: ::CODE FORKED TO VERSION 3:: Added setup and evaluation methods to INTERPOLANT class.
!                    The interpolation process has been converted to a more object-oriented flow that
!                    permits the simultaneous existence of multiple interpolants. Added an explicit definiton
!                    of the B_INV matrix and removed the routines originally used to calculate it.
!   23 January 2015: Added locate function to search ordered but non-uniformly spaced ("stretched") grids.
!   11 March 2015:   Fixed an error in some of the mixed derivative routines
!   17 March 2015:   Intoroduced GRID object to streamline data sharing amongst interpolants. Added trilinear
!                    interpolation routine for electric fields
!   04 March 2015:   Changed interpolant object to 64-element array, altered some variable names.
!   13 August 2015:  Fixed error in B_INV array

module interpolation

use global

! The B_INV array is a bunch of constants that are used to determine the interpolation weights.
! There is no need to recalculate this array, so I've included it here as a monstrously large constant.
! Note that the square bracket [] notation for array definition is only supported by the FORTRAN standard
! since 2008, but the Intel ifort compiler doesn't complain.

! The B_INV matrix relates the derivatives to the coefficients of the interpolant. This is a geometrical constant for a 1 x 1 x 1 cube, so no need to actually calculate it on the fly.
! The bracket assignment notation is not supported by all Fortran compilers, so use with others may require us to put the array in a file and read it once at the beginning of the simulation.
real(dp),dimension(64,64), parameter :: B_INV = reshape([&
[   1,   0,  -3,   2,   0,   0,   0,   0,  -3,   0,   9,  -6,   2,   0,  -6,   4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   0,   9,  -6,   0,   0,   0,   0,   9,   0, -27,  18,  -6,   0,  18, -12,   2,   0,  -6,   4,   0,   0,   0,   0,  -6,   0,  18, -12,   4,   0, -12,   8], &
[   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,   6,  -4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,   0,   0,   0,   0,  27, -18,   0,   0, -18,  12,   0,   0,   6,  -4,   0,   0,   0,   0,   0,   0, -18,  12,   0,   0,  12,  -8], &
[   0,   0,   0,   0,   0,   0,   0,   0,   3,   0,  -9,   6,  -2,   0,   6,  -4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -9,   0,  27, -18,   6,   0, -18,  12,   0,   0,   0,   0,   0,   0,   0,   0,   6,   0, -18,  12,  -4,   0,  12,  -8], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0,  -6,   4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, -27,  18,   0,   0,  18, -12,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  18, -12,   0,   0, -12,   8], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,   0,  -9,   6,   0,   0,   0,   0,  -9,   0,  27, -18,   6,   0, -18,  12,  -2,   0,   6,  -4,   0,   0,   0,   0,   6,   0, -18,  12,  -4,   0,  12,  -8], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0,   0,   0,   0,   0, -27,  18,   0,   0,  18, -12,   0,   0,  -6,   4,   0,   0,   0,   0,   0,   0,  18, -12,   0,   0, -12,   8], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,   0, -27,  18,  -6,   0,  18, -12,   0,   0,   0,   0,   0,   0,   0,   0,  -6,   0,  18, -12,   4,   0, -12,   8], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  27, -18,   0,   0, -18,  12,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, -18,  12,   0,   0,  12,  -8], &
[   0,   1,  -2,   1,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   2,  -4,   2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   0,   0,   0,   0,   9, -18,   9,   0,  -6,  12,  -6,   0,   2,  -4,   2,   0,   0,   0,   0,   0,  -6,  12,  -6,   0,   4,  -8,   4], &
[   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,  -2,   2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,   0,   0,   0,   0,  -9,   9,   0,   0,   6,  -6,   0,   0,  -2,   2,   0,   0,   0,   0,   0,   0,   6,  -6,   0,   0,  -4,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -6,   3,   0,  -2,   4,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -9,  18,  -9,   0,   6, -12,   6,   0,   0,   0,   0,   0,   0,   0,   0,   0,   6, -12,   6,   0,  -4,   8,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   2,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -9,   0,   0,  -6,   6,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -6,   6,   0,   0,   4,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -6,   3,   0,   0,   0,   0,   0,  -9,  18,  -9,   0,   6, -12,   6,   0,  -2,   4,  -2,   0,   0,   0,   0,   0,   6, -12,   6,   0,  -4,   8,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   0,   0,   0,   0,   9,  -9,   0,   0,  -6,   6,   0,   0,   2,  -2,   0,   0,   0,   0,   0,   0,  -6,   6,   0,   0,   4,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9, -18,   9,   0,  -6,  12,  -6,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -6,  12,  -6,   0,   4,  -8,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -9,   9,   0,   0,   6,  -6,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   6,  -6,   0,   0,  -4,   4], &
[   0,   0,   0,   0,   1,   0,  -3,   2,  -2,   0,   6,  -4,   1,   0,  -3,   2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   0,   9,  -6,   6,   0, -18,  12,  -3,   0,   9,  -6,   0,   0,   0,   0,   2,   0,  -6,   4,  -4,   0,  12,  -8,   2,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   0,   3,  -2,   0,   0,  -6,   4,   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,  18, -12,   0,   0,  -9,   6,   0,   0,   0,   0,   0,   0,   6,  -4,   0,   0, -12,   8,   0,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,  -1,   0,   3,  -2,   1,   0,  -3,   2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,   0,  -9,   6,  -3,   0,   9,  -6,   0,   0,   0,   0,   0,   0,   0,   0,  -2,   0,   6,  -4,   2,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   2,   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0,  -9,   6,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -6,   4,   0,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,   0,  -9,   6,  -6,   0,  18, -12,   3,   0,  -9,   6,   0,   0,   0,   0,  -2,   0,   6,  -4,   4,   0, -12,   8,  -2,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0, -18,  12,   0,   0,   9,  -6,   0,   0,   0,   0,   0,   0,  -6,   4,   0,   0,  12,  -8,   0,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   0,   9,  -6,   3,   0,  -9,   6,   0,   0,   0,   0,   0,   0,   0,   0,   2,   0,  -6,   4,  -2,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,   9,  -6,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   6,  -4,   0,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,   0,  -3,   2,   0,   0,   0,   0,  -3,   0,   9,  -6,   2,   0,  -6,   4,  -2,   0,   6,  -4,   0,   0,   0,   0,   6,   0, -18,  12,  -4,   0,  12,  -8,   1,   0,  -3,   2,   0,   0,   0,   0,  -3,   0,   9,  -6,   2,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,   6,  -4,   0,   0,  -6,   4,   0,   0,   0,   0,   0,   0,  18, -12,   0,   0, -12,   8,   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,   0,  -9,   6,  -2,   0,   6,  -4,   0,   0,   0,   0,   0,   0,   0,   0,  -6,   0,  18, -12,   4,   0, -12,   8,   0,   0,   0,   0,   0,   0,   0,   0,   3,   0,  -9,   6,  -2,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0,  -6,   4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, -18,  12,   0,   0,  12,  -8,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   0,   3,  -2,   0,   0,   0,   0,   3,   0,  -9,   6,  -2,   0,   6,  -4,   1,   0,  -3,   2,   0,   0,   0,   0,  -3,   0,   9,  -6,   2,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   2,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0,  -6,   4,   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   0,   9,  -6,   2,   0,  -6,   4,   0,   0,   0,   0,   0,   0,   0,   0,   3,   0,  -9,   6,  -2,   0,   6,  -4], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -9,   6,   0,   0,   6,  -4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   9,  -6,   0,   0,  -6,   4], &
[   0,   0,   0,   0,   0,   1,  -2,   1,   0,  -2,   4,  -2,   0,   1,  -2,   1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   6, -12,   6,   0,  -3,   6,  -3,   0,   0,   0,   0,   0,   2,  -4,   2,   0,  -4,   8,  -4,   0,   2,  -4,   2], &
[   0,   0,   0,   0,   0,   0,  -1,   1,   0,   0,   2,  -2,   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,  -6,   6,   0,   0,   3,  -3,   0,   0,   0,   0,   0,   0,  -2,   2,   0,   0,   4,  -4,   0,   0,  -2,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   2,  -1,   0,   1,  -2,   1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -6,   3,   0,  -3,   6,  -3,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -2,   4,  -2,   0,   2,  -4,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -1,   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   3,  -3,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   2,  -2,   0,   0,  -2,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -6,   3,   0,  -6,  12,  -6,   0,   3,  -6,   3,   0,   0,   0,   0,   0,  -2,   4,  -2,   0,   4,  -8,   4,   0,  -2,   4,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   6,  -6,   0,   0,  -3,   3,   0,   0,   0,   0,   0,   0,   2,  -2,   0,   0,  -4,   4,   0,   0,   2,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   3,  -6,   3,   0,   0,   0,   0,   0,   0,   0,   0,   0,   2,  -4,   2,   0,  -2,   4,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,  -3,   3,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -2,   2,   0,   0,   2,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -2,   1,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   2,  -4,   2,   0,  -2,   4,  -2,   0,   0,   0,   0,   0,   6, -12,   6,   0,  -4,   8,  -4,   0,   1,  -2,   1,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   2,  -4,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,  -2,   2,   0,   0,   2,  -2,   0,   0,   0,   0,   0,   0,  -6,   6,   0,   0,   4,  -4,   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,  -2,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -6,   3,   0,  -2,   4,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -6,  12,  -6,   0,   4,  -8,   4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -6,   3,   0,  -2,   4,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   2,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   6,  -6,   0,   0,  -4,   4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   2,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   2,  -1,   0,   0,   0,   0,   0,   3,  -6,   3,   0,  -2,   4,  -2,   0,   1,  -2,   1,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   2,  -4,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -1,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   2,  -2,   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,  -2,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   6,  -3,   0,   2,  -4,   2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -6,   3,   0,  -2,   4,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -3,   0,   0,  -2,   2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   3,   0,   0,   2,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,   0,  -3,   2,  -2,   0,   6,  -4,   1,   0,  -3,   2,   0,   0,   0,   0,  -2,   0,   6,  -4,   4,   0, -12,   8,  -2,   0,   6,  -4,   0,   0,   0,   0,   1,   0,  -3,   2,  -2,   0,   6,  -4,   1,   0,  -3,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -2,   0,   0,  -6,   4,   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,  -6,   4,   0,   0,  12,  -8,   0,   0,  -6,   4,   0,   0,   0,   0,   0,   0,   3,  -2,   0,   0,  -6,   4,   0,   0,   3,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   0,   3,  -2,   1,   0,  -3,   2,   0,   0,   0,   0,   0,   0,   0,   0,   2,   0,  -6,   4,  -2,   0,   6,  -4,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   0,   3,  -2,   1,   0,  -3,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   2,   0,   0,   3,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   6,  -4,   0,   0,  -6,   4,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   2,   0,   0,   3,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   0,   3,  -2,   2,   0,  -6,   4,  -1,   0,   3,  -2,   0,   0,   0,   0,   1,   0,  -3,   2,  -2,   0,   6,  -4,   1,   0,  -3,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   2,   0,   0,   6,  -4,   0,   0,  -3,   2,   0,   0,   0,   0,   0,   0,   3,  -2,   0,   0,  -6,   4,   0,   0,   3,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,   0,  -3,   2,  -1,   0,   3,  -2,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   0,   3,  -2,   1,   0,  -3,   2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   3,  -2,   0,   0,  -3,   2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -3,   2,   0,   0,   3,  -2], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -2,   1,   0,  -2,   4,  -2,   0,   1,  -2,   1,   0,   0,   0,   0,   0,  -2,   4,  -2,   0,   4,  -8,   4,   0,  -2,   4,  -2,   0,   0,   0,   0,   0,   1,  -2,   1,   0,  -2,   4,  -2,   0,   1,  -2,   1], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   1,   0,   0,   2,  -2,   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   2,  -2,   0,   0,  -4,   4,   0,   0,   2,  -2,   0,   0,   0,   0,   0,   0,  -1,   1,   0,   0,   2,  -2,   0,   0,  -1,   1], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   2,  -1,   0,   1,  -2,   1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   2,  -4,   2,   0,  -2,   4,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   2,  -1,   0,   1,  -2,   1], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -1,   0,   0,  -1,   1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -2,   2,   0,   0,   2,  -2,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -1,   0,   0,  -1,   1], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   2,  -1,   0,   2,  -4,   2,   0,  -1,   2,  -1,   0,   0,   0,   0,   0,   1,  -2,   1,   0,  -2,   4,  -2,   0,   1,  -2,   1], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -1,   0,   0,  -2,   2,   0,   0,   1,  -1,   0,   0,   0,   0,   0,   0,  -1,   1,   0,   0,   2,  -2,   0,   0,  -1,   1], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -2,   1,   0,  -1,   2,  -1,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   2,  -1,   0,   1,  -2,   1], &
[   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  -1,   1,   0,   0,   1,  -1,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,  -1,   0,   0,  -1,   1]],[64,64])

contains

  subroutine grid_init(myGrid, im, jm, km, lm, dx, dy, dz, dt)
  ! Initialize a grid object used in trilinear and tricubic interpolation
  implicit none

  type(grid) :: myGrid
  integer, intent(in) :: im, jm, km, lm
  real(dp), intent(in) :: dx, dy, dz, dt

  myGrid%im = im
  myGrid%jm = jm
  myGrid%km = km
  myGrid%lm = lm
  myGrid%dx = dx
  myGrid%dy = dy
  myGrid%dz = dz
  myGrid%dt = dt

  return

  end subroutine grid_init

!

  function trilinear_interpolate(myGrid,xw,yw,zw,F) result(h)
  ! Find the value of a three-dimensional function at a point using
  ! trilinear interpolation of gridded values
  implicit none

  type(grid) :: myGrid
  real(dp), intent(in) :: xw, yw, zw
  real(dp), dimension(2,2,2) :: F
  real(dp) :: x, y, z
  real(dp) :: f00, f10, f01, f11
  real(dp) :: g0, g1
  real(dp) :: h

  x = (xw-xgrid(myGrid%im))/myGrid%dx
  y = (yw-ygrid(myGrid%jm))/myGrid%dy
  z = (zw-zgrid(myGrid%km))/myGrid%dz

  f00 = (1-x)*F(1,1,1)+x*F(2,1,1)
  f10 = (1-x)*F(1,2,1)+x*F(2,2,1)
  f01 = (1-x)*F(1,1,2)+x*F(2,1,2)
  f11 = (1-x)*F(1,2,2)+x*F(2,2,2)
  g0  = (1-y)*f00+y*f10
  g1  = (1-y)*f01+y*f11
  h   = (1-z)*g0+z*g1

  return

  end function trilinear_interpolate

!

  subroutine tricubic_init(myCoeffs,myGrid,F,FX,FY,FZ,FXY,FXZ,FYZ,FXYZ)
  ! Calculate the interpolation coefficients for an interpolant object
  implicit none

  real(dp), dimension(64) :: myCoeffs
  type(grid) :: myGrid
  real(dp), dimension(2,2,2), intent(in) :: F,FX,FY,FZ,FXY,FXZ,FYZ,FXYZ
  real(dp), dimension(64) :: bbar
  integer :: ix, iy, iz, irow

  !Determine the coefficients of the interpolation using abar = B_INV . bbar
  ! We only need to save the results of this calculation, bbar is not needed later.

  irow=0
  do iz=1,2
    do iy=1,2
      do ix=1,2
        irow=irow+1
        bbar(irow   ) =    F(ix,iy,iz)
        bbar(irow+8 ) =   FX(ix,iy,iz)*myGrid%dx
        bbar(irow+16) =   FY(ix,iy,iz)*myGrid%dy
        bbar(irow+24) =   FZ(ix,iy,iz)*myGrid%dz
        bbar(irow+32) =  FXY(ix,iy,iz)*myGrid%dx*myGrid%dy
        bbar(irow+40) =  FXZ(ix,iy,iz)*myGrid%dx*myGrid%dz
        bbar(irow+48) =  FYZ(ix,iy,iz)*myGrid%dy*myGrid%dz
        bbar(irow+56) = FXYZ(ix,iy,iz)*myGrid%dx*myGrid%dy*myGrid%dz
      enddo
    enddo
  enddo

  ! This is a matrix-vector multiply between a 64x64 matrix and a 64 element vector. Tests indicate that
  ! vendor-optimized matmuls (such as mkl) are more efficient than a call to the L2 BLAS DGEMV routine
  ! (results may differ for an optimized BLAS library, but the times weren't even particularly close).

  myCoeffs = matmul(B_INV,bbar)

  return

  end subroutine tricubic_init

!

   subroutine tricubic_interpolate(myCoeffs, myGrid, x0, y0, z0, f0, gradf)
  ! Evaluate a tricubic interpolant at a point. You can also request the gradient of
  ! the function by including the optional gradf vector.
  implicit none

  real(dp), dimension(64) :: myCoeffs
  type(grid) :: myGrid
  real(dp), intent(in) :: x0, y0, z0
  real(dp), intent(out) :: f0
  real(dp), dimension(3), intent(out), optional :: gradf
  real(dp) :: x, y, z
  real(dp) :: xpow, ypow, zpow
  real(dp) :: xder, yder, zder
  integer :: i, j, k, idex

  ! Map the simulation cell to a unit cube
  x = (x0-xgrid(myGrid%im))/myGrid%dx
  y = (y0-ygrid(myGrid%jm))/myGrid%dy
  z = (z0-zgrid(myGrid%km))/myGrid%dz

  ! Calculate the value of the function based on tricubic interpolation coefficients
  ! previously calculated during a call to tricubic_setup. Also calculate the gradient
  ! of the function if the gradf argument is present

  f0 = 0.d0
  if(present(gradf)) gradf = 0.d0

  ! These #pow and #der variables are used to avoid using the exponentiation operator,
  ! and they turn out to be a fairly efficient way of expressing the algorithm.

  idex=0
  zder = 0.d0
  zpow = 1.d0
  do k=0,3
    yder = 0.d0
    ypow = 1.d0
    do j=0,3
      xder = 0.d0
      xpow = 1.d0
      do i=0,3
        idex = idex+1
        f0 = f0+myCoeffs(idex)*xpow*ypow*zpow
        if(present(gradf)) then ! Calculate the gradient
          gradf(1) = gradf(1)+myCoeffs(idex)*i*xder*ypow*zpow
          gradf(2) = gradf(2)+myCoeffs(idex)*j*xpow*yder*zpow
          gradf(3) = gradf(3)+myCoeffs(idex)*k*xpow*ypow*zder
        endif
        xder = xpow
        xpow = xpow*x
      enddo
      yder = ypow
      ypow = ypow*y
    enddo
    zder = zpow
    zpow = zpow*z
  enddo

  if(present(gradf)) then ! Scale derivatives by actual size of the cell
    gradf(1) = gradf(1)/myGrid%dx
    gradf(2) = gradf(2)/myGrid%dy
    gradf(3) = gradf(3)/myGrid%dz
  endif

  return

  end subroutine tricubic_interpolate

!

  function locate(v,x,ih) result(i0)
  ! Seach for the lower bracketing index, i.e. the index of array element of a monotonically
  ! increasing array v that is closest to the value x without being larger than x. If you know
  ! approximately where to look, you can specify an optional "hunting index". This function
  ! is essentially an amalgam of the Numerical Recipes "locate" and "hunt" routines.
  implicit none

  real(dp), dimension(:), intent(in) :: v
  real(dp), intent(in) :: x
  integer, intent(in), optional :: ih
  integer :: dh
  integer :: i0
  integer :: ilo, ihi, imid
  integer :: n

  n = size(v)

  ! Check that the value is actually bracketed. It's up to the user to handle these exceptional cases.
  if(x<v(1)) then
    i0 = 0
    return
  else if(x>v(n)) then
    i0 = n+1
    return
  endif

  ! If the user has specified a starting index, we will hunt from that point to determine the bracket
  if(present(ih)) then ! Hunt
    dh = 1 ! Initialize the search range to one cell; if we're lucky, we'll already be bracketing the value
    ihi = ih
    do
      ilo = ihi
      ihi = ilo+dh
      if(ihi > n) then ! That's as far as we can go, truncate search range to last element
        ihi = n
        exit
      endif
      if(v(ihi) > x) exit ! Hunt is successful
      dh = 2*dh ! The hunt hasn't yet succeeded, so we expand the search range and try again
    enddo
    ilo = ih
    ihi = ih+dh
  else ! We don't know where to start, so use the entire interval
    ilo = 1
    ihi = n
  endif
  ! Bisection search
  do
    if(ihi - ilo == 1) exit
    imid = (ihi+ilo)/2
    if(v(imid) == x) then
      ilo = imid
      exit
    else if(v(imid) > x) then
      ihi = imid
    else
      ilo = imid
    endif
  enddo
  i0 = ilo
  return
  end function locate

end module interpolation


program ch0811
    ! Rank 2 Arrays and the Sum Intrinsic Funcoltion
    implicit none

    integer, parameter :: nrow = 5
    integer, parameter :: ncol = 6
    real, dimension(1:nrow*ncol) :: results = [ &
        50., 47., 28., 89., 30., 46., 37., 67., 34., 65., 68., &
        98., 25., 45., 26., 48., 10., 36., 89., 56., 33., 45., &
        30., 65., 68., 78., 38., 76., 98., 65. ]
    real, dimension(1:nrow,1:ncol) :: exam_results = 0.
    real, dimension(1:nrow) :: people_average = 0.
    real, dimension(1:ncol) :: subject_average = 0.

    exam_results = reshape(results, [nrow,ncol], [0.,0.], [2,1])
    exam_results(1:nrow,3) = exam_results(1:nrow,3)*2.5
    people_average = sum(exam_results, 2)
    people_average = people_average/ncol
    subject_average = sum(exam_results, 1)
    subject_average = subject_average/nrow
    print *, ' People averages'
    print *, people_average
    print *, ' Subject averages'
    print *, subject_average
end program


PROGRAM test_cst
!     Test of sine, cosine & tangent
! 1. sin(2.x)  = 2.sin(x).cos(x)
! 2. cos(2.x)  = (cos(x) + sin(x)).(cos(x) - sin(x))
! 3. sec(x)**2 = 1 + tan(x)**2

USE quadruple_precision
IMPLICIT NONE

TYPE (quad)          :: x, twox, sine, cosine, tangent, lhs, rhs, diff
REAL (dp)            :: half = 0.5_dp, small = EPSILON(half), r
INTEGER, ALLOCATABLE :: seed(:)
INTEGER              :: k, i

!     Set the random number seed.

CALL RANDOM_SEED(size=k)
ALLOCATE (seed(k))
CALL RANDOM_SEED(get=seed)
WRITE(*, *)'Old random number seeds: ', seed

WRITE(*, '(1x, a, i4, a)') 'Enter ', k, ' integers as random number seeds: '
READ(*, *) seed
CALL RANDOM_SEED(put=seed)

DO i = 1, 100
  CALL RANDOM_NUMBER(x%hi)
  CALL RANDOM_NUMBER(r)
  x%hi = (x%hi - half) / r
  x%lo = x%hi * small
                             ! sin(2.x)  = 2.sin(x).cos(x)
  twox = 2._dp * x
  lhs = SIN(twox)
  sine = SIN(x)
  cosine = COS(x)
  rhs = sine * cosine
  rhs = 2._dp * rhs
  diff = lhs - rhs
  WRITE(*, '(" sin(2x)   lhs =", g13.5, "  Diff. =", g12.4)') lhs%hi, diff%hi
                             ! cos(2.x)  = (cos(x) + sin(x)).(cos(x) - sin(x))
  lhs = COS(twox)
  rhs = (cosine + sine) * (cosine - sine)
  diff = lhs - rhs
  WRITE(*, '(" cos(2x)   lhs =", g13.5, "  Diff. =", g12.4)') lhs%hi, diff%hi
                             ! sec(x)**2 = 1 + tan(x)**2
  diff%hi = 1._dp
  diff%lo = 0._dp
  lhs = diff / cosine
  lhs = lhs * lhs
  tangent = TAN(x)
  rhs = diff + tangent * tangent
  diff = lhs - rhs
  WRITE(*, '(" sec(x)^2  lhs =", g13.5, "  Diff. =", g12.4)') lhs%hi, diff%hi
END DO

STOP
END PROGRAM test_cst


module constants
  real, parameter :: pi=3.1415926
  real, parameter :: twopi=2.*pi
  real, parameter :: rad=pi/180.
  real, parameter :: ftm=12./39.37
  real, parameter :: mtf=1/ftm
  real, parameter :: rpm=60./twopi
  ! ft/s^2
  real, parameter :: g=32.179
  real, parameter :: circ=9.125
  ! radius of ball in ft
  real, parameter :: radius=circ/twopi/12
end module constants

program test

  use constants

  ! Original version by Alan Nathan
  ! Modifcations by Chris Long

  interface dist
     subroutine dist(t,x,range,time)
       real, dimension(:), intent(in) :: t
       real, dimension(:,:), intent(in) :: x
       real, intent(out) :: range
       real, intent(out) :: time
     end subroutine dist
  end interface dist

  !parameter(noptmax=7)
  integer, parameter :: noptmax=7

  !parameter(pi=3.1415926,twopi=2.*pi,rad=pi/180,ftm=12./39.37,
  !  mtf=1/ftm,rpm=60./twopi)
  !real, parameter :: pi=3.1415926
  !real, parameter :: twopi=2.*pi

  !parameter(rad=pi/180,ftm=12./39.37,mtf=1/ftm,rpm=60./twopi)
  !real, parameter :: rad=pi/180
  !real, parameter :: ftm=12./39.37
  !real, parameter :: mtf=1/ftm
  !real, parameter :: rpm=60./twopi

  real :: rhodata,vdata,thetadata,vwind
  common/data/ rhodata,vdata,thetadata,vwind
  common/flag/iflag

  !parameter(const0=5.283e-3,rho0=1.175)
  real, parameter :: const0=5.283e-3
  real, parameter :: rho0=1.175

  common/spin/ctht,stht,cphi,sphi

  ! local version of best fit trajectory parameters
  common/fitting/cd0,cddot,cdspin,w0,theta0,tau0

  ! local version of input parameters
  ! in mph,det,kg/m^3,mph,mph
  common/input/v0,theta,rho,const,vwx,vwy

  ! in ft/s^2,ft
  !common/constants/g,radius

  !parameter(nterms=8,npts=1000)
  integer, parameter :: nterms=8
  integer, parameter :: npts=1000

  common/initial/xstart(nterms)
  common/path/t(npts),x(nterms,npts)

  ! ft/s^2
  !g=32.179
  !circ=9.125
  ! radius of ball in ft
  !radius=circ/twopi/12

  ! no wind
  vwx=0.
  vwy=0.

  ! index i:
  !   1,2,3 ==> xi,yi,zi
  !   4,5,6 ==> vx,vy,vz
  !   7,8 ==> r*wb,r*ws

  ! wb=backspin rate
  ! ws=sidespin rate

  ! note on the right-handed coordinate:
  !   z is vertical with positive upward
  !   y is catcher-pitcher line
  !   x points to catcher's right

  ! NOTE:  This current version is 2D, so that x=vx=ws=0
  ! It is easy to reinstate these to make it full 3D

  ! read in parameters for Cd,wb

  ! cd0, cddot, cdspin, w0, theta0, tau0

  ! input paramters
  open(unit=20,file='parameters.csv',status='old')
  read(20,*)
  read(20,*) cd0, cddot, cdspin, w0, theta0, tau0
  close(20)

  ! output file
  !open(unit=21,file='TrajectoryCalculatorOutput.csv',status='unknown')
  ! Default status should be unknown
  ! output file
  open(unit=21,file='TrajectoryCalculatorOutput.csv')

  ! rhodata, vdata, thetadata, vwind

  open(unit=20,file='trajectory_input.csv',status='old')
  ! input data
  ! skip header
  read(20,*)
  read(20,*) rhodata, vdata, thetadata, vwind
  ndata=1
  tmax=10.
  ! write header
  write(21,251)

  rho=rhodata
  const=const0*(rho/rho0)
  v0=vdata
  theta=thetadata
  vwy=vwind*1.467
  vwx=0.
  xstart(1)=0.
  xstart(2)=2.
  xstart(3)=3.
  xstart(4)=0.
  ctht=cos(theta*rad)
  stht=sin(theta*rad)
  xstart(5)=(v0*1.467)*ctht
  xstart(6)=(v0*1.467)*stht
  cphi=1.
  sphi=0.
  spin=(w0/rpm)*(v0/100.)*(theta-theta0)/(27.5-theta0)
  xstart(7)=radius*spin
  xstart(8)=0.

  ! spin rate=w0 at v0=100. and theta=27.5 and 0 at theta=aparm(5)

  ! integrate to get trajectory, then distance

  call rkdumb(xstart,nterms,0.,tmax,npts)

  call dist(t,x,range,time)

  sfact=xstart(7)/(1.467*v0)
  cdi=cd0*(1.+cddot*(100.-v0))*(1.+cdspin*sfact**2)

  write(21,250) rho, v0, theta, spin*rpm, sfact, cdi, vwind, range, time
  write(6,250) rho, v0, theta, spin*rpm, sfact, cdi, vwind, range, time

251 format(' rho, v0, theta, spin, sfact, cdi, vwind, distance, tof')
250 format(f8.3,2(',',f8.1),1(',',f8.0),2(',',f8.3),3(',',f8.1))
  close(21)
  !stop

end program test

subroutine derivs(t,x,dxdt)

  use constants

  ! Given the values of t and x, returns derivatives dxdt=dv/dt

  ! local version of fitting parameters
  common/fitting/cd0,cddot,cdspin,w0,theta0,tau0

  ! local version of input parameters
  ! in mph,det,kg/m^3,mph,mph
  common/input/v0,theta,rho,const,vwx,vwy

  !in ft/s^2,ft
  !common/constants/g,radius
  common/spin/ctht,stht,cphi,sphi

  dimension x(8),dxdt(8)
  real drag,lift

  ! Change to constants?
  !parameter(pi=3.1415926)
  !parameter(twopi=2.*pi)
  !parameter(rad=pi/180,ftm=12./39.37,rpm=60./twopi)

  ! 1,2,4 ==> x,y,z
  ! 4,5,6 ==> vx,vy,vz
  ! 7,8 ==> r*wb,r*ws

  ! NOTE:  this is 2D problem, so x(1),dxdt(1) are both fixed at 0

  ! vwx,wwy are x,y components of wind speed

  ! components of radius*spin

  rwb=x(7)
  rws=x(8)
  romega=sqrt(rwb**2+rws**2)

  ! speed of ball
  vt=sqrt((x(4)-vwx)**2+(x(5)-vwy)**2+x(6)**2)

  ! spin factor
  sfact=romega/vt

  ! note that spin is pure backspin (i.e, rws=0)

  cd=cd0*(1.+cddot*(100.-v0))*(1.+cdspin*sfact**2)
  ! Cross prescription
  cl=sfact/(2.32*sfact+0.4)
  !print*,cd,cl,sfact,g,x(7),vt
  !pause

  ! actually drag/(m*vt)
  drag=const*cd*vt
  ! actually lift/(m*vt)
  lift=const*cl*vt

  ! components of spin
  ! NOTE:  For 2D, ws=0; cphi=1; sphi=0; this means wy=yz=0 and wx=wb

  wx1=rwb
  wy1=-rws*stht
  wz=rws*ctht
  wx=wx1*cphi+wy1*sphi
  wy=-wx1*sphi+wy1*cphi

  ! the preceding five statement assure the the spin vector is normal
  ! to the initial velocity vector

  !=0 for 2D
  liftx=lift*(wy*x(6)-wz*(x(5)-vwy))/romega
  lifty=lift*(wz*(x(4)-vwx)-wx*x(6))/romega
  liftz=lift*(wx*(x(5)-vwy)-wy*(x(4)-vwx))/romega
  !=0 for 2D
  dxdt(1)=x(4)
  dxdt(2)=x(5)
  dxdt(3)=x(6)
  !=0 for 2D
  dxdt(4)=-drag*(x(4)-vwx)+liftx
  dxdt(5)=-drag*(x(5)-vwy)+lifty
  dxdt(6)=-drag*x(6)+liftz-g
  !no spin-down
  dxdt(7)=0.
  !no spin-down
  dxdt(8)=0.

  return
end subroutine derivs

subroutine RK4(Y,DYDX,N,X,H,YOUT)

  ! Given values for N variables y and their derivatives DYDX known
  ! at X, use the fourth-order Runge-Kutta method to advance the
  ! solution over an interval H and return the incremented variables
  ! as YOUT, which need not be a distinct array from Y.  The user
  ! supplies the subroutine DERIVS(X,Y,DYDX) which returns the
  ! derivatives DYDX at X

  ! Routine taken from Numerical Recipes, 15.1, p. 553

  ! set to the maximum number of functions
  parameter (nmax=10)
  dimension Y(N),DYDX(N),YOUT(N),YT(NMAX),DYT(NMAX),DYM(NMAX)

  hh=h*0.5
  h6=h/6.
  xh=x+hh

  ! first step
  do i=1,n
     yt(i)=y(i)+hh*dydx(i)
  end do
  ! second step
  call derivs(xh,yt,dyt)
  do i=1,n
     yt(i)=y(i)+hh*dyt(i)
  end do
  ! third step
  call derivs(xh,yt,dym)
  do i=1,n
     yt(i)=y(i)+h*dym(i)
     dym(i)=dyt(i)+dym(i)
  end do
  ! fourth step
  call derivs(x+h,yt,dyt)
  do i=1,n
     yout(i)=y(i)+h6*(dydx(i)+dyt(i)+2.*dym(i))
  end do
  return
end subroutine RK4

subroutine rkdumb(vstart, nvar, x1, x2, nstep)

  ! Starting from initial values VSTART for NVAR functions, known at
  ! X1, use fourth-order Runge-Kutta to advance npts equal increments
  ! to X2.  The user supplied subroutine DERIVS(X,V,DVDX) evaluates
  ! derivatives.  Results are stored in the common block PATH.  Be sure
  ! to dimension the common block appropriately

  ! Routine taken from Numerical Recipes, 15.1, p. 554
  common/coeff/cdprime,cm,cl,rey,sfact,vt

  !set to the maximum number of functions
  parameter(nmax=10)
  parameter(nterms=8)
  parameter(npts=1000)
  !nterms functions; npts values
  common/path/xx(npts),y(nterms,npts)
  dimension vstart(nvar),v(nmax),dv(nmax)

  ! load starting values
  do i=1,nvar
     v(i)=vstart(i)
     y(i,1)=v(i)
  end do
  xx(1)=x1
  x=x1
  h=(x2-x1)/nstep

  ! take nsetp steps
  do k=1,nstep-1
     call derivs(x,v,dv)
     call rk4(v,dv,nvar,x,h,v)

     ! Commented out - replace with error code
     !if (x+h.eq.x)pause 'Stepsize not significant in RKDUMB'
     x=x+h
     !store intermediate steps
     xx(k+1)=x
     do i=1,nvar
        y(i,k+1)=v(i)
     end do

  end do
  return
end subroutine rkdumb

subroutine dist(t,x,range,time)

  use constants

  real, dimension(:), intent(in) :: t
  real, dimension(:,:), intent(in) :: x
  real, intent(out) :: range
  real, intent(out) :: time

  !parameter(nterms=8,pi=3.1415926,twopi=2.*pi,rad=pi/180.)

  integer :: npts
  npts=size(t)

  ! Fortran does not have repeat-until
  do i=1,npts
     if(x(3,i).le.0..and.i.gt.1) exit
  end do

  ! linear interpolation to get range
  frac=x(3,i-1)/(x(3,i-1)-x(3,i))
  d1=sqrt(x(2,i-1)**2+x(1,i-1)**2)
  d2=sqrt(x(2,i)**2+x(1,i)**2)
  range=d1+frac*(d2-d1)
  time=t(i)
  return
end subroutine dist