      SUBROUTINE PLT_AREA_SIZE
     &          (mxlat,nrlat,mxlon,nrlon,
     &           ampl,xlat,xlon,
     &           value,area_a,area_o)

c Returns area enclosed by a specific signal level

c     mxlon         maximum number of points in longitude
c     mxlat         maximum number of points in latitude
c     nrlon         output  number of points in longitude
c     nrlat         output  number of points in latitude
c     ampl          array of signal levels
c     xlat(1)       latitude  of lower left  corner
c     xlon(1)       longitude of lower left  corner
c     xlat(2)       latitude  of upper right corner
c     xlon(2)       longitude of upper right corner
c     value         signal level
c     area_a        percentage of the total area enclosed by
c                   the signal level
c     area_o        percentage of the ocean area enclosed by
c                   the signal level

      include      'lwpc_geo.cmn'

      logical       inside,land

      dimension     ampl(mxlon,mxlat),xlat(2),xlon(2)

c     Determine the interval in latitude
      dlat=(xlat(2)-xlat(1))/(nrlat-1)

c     Determine the interval in longitude
      if (xlon(1) .gt. xlon(2)) then
         dlon=(xlon(1)-xlon(2))/(nrlon-1)
      else
         dlon=(xlon(1)-xlon(2)+360.)/(nrlon-1)
      end if

      ocean=0.
      total=0.
      area_a=0.
      area_o=0.

      xlt=xlat(1)
      do l=1,nrlat

         cos_lat=cos(xlt*.01745329252)
         xln=xlon(1)
         do k=1,nrlon

c           Find out where this point will be plotted
            call GEO_COORD
     &          (xlt,xln,
     &           geo_rng,geo_brng,geo_x,geo_y,
     &           plt_x,plt_y)

            SELECT CASE( geo_prjctn )

            CASE( 'A','G','O','S' )

c              Azimuthal Equidistant, Gnomonic, Orthographic or Stereographic

               if (plt_x   .ge.         0.  .and.
     &             plt_x   .le. geo_size_x  .and.
     &             plt_y   .ge.         0.  .and.
     &             plt_y   .le. geo_size_y  .and.
     &             geo_rng .le. geo_rng_max) then

                  inside=.true.
               else

                  inside=.false.
               end if

            CASE( 'M','R' )

c              Mercator or Rectangular

               if (plt_x   .ge.         0.  .and.
     &             plt_x   .le. geo_size_x  .and.
     &             plt_y   .ge.         0.  .and.
     &             plt_y   .le. geo_size_y)  then

                  inside=.true.
               else

                  inside=.false.
               end if

            END SELECT

            if (inside) then

c              This point is inside the mapped area
               call LAND_SEA (xln,xlt,land)

               total=total+cos_lat
               if (.not.land) ocean=ocean+cos_lat

               if (ampl(k,l) .ge. value) then

                  area_a=area_a+cos_lat
                  if (.not.land) area_o=area_o+cos_lat
               end if
            end if
            xln=xln-dlon
            if (xln .lt. -180.) xln=xln+360.
         end do
         xlt=xlt+dlat
      end do

      if (total .gt. 0.) area_a=100.*area_a/total
      if (ocean .gt. 0.) area_o=100.*area_o/ocean

      RETURN
      END      ! PLT_AREA_SIZE


!==========================================================================
elemental function gsw_kappa_t_exact (sa, t, p)
!==========================================================================
!
! isentropic compressibility of seawater
!
! sa     : Absolute Salinity                               [g/kg]
! t      : in-situ temperature                             [deg C]
! p      : sea pressure                                    [dbar]
!
! gsw_kappa_t_exact : isentropic compressibility           [1/Pa]
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_gibbs

use gsw_mod_kinds

implicit none

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

real (r8) :: gsw_kappa_t_exact

real (r8) :: g_tt, g_tp

integer, parameter :: n0=0, n1=1, n2=2

g_tt = gsw_gibbs(n0,n2,n0,sa,t,p)
g_tp = gsw_gibbs(n0,n1,n1,sa,t,p)

gsw_kappa_t_exact = (g_tp*g_tp - g_tt*gsw_gibbs(n0,n0,n2,sa,t,p)) / &
                         (gsw_gibbs(n0,n0,n1,sa,t,p)*g_tt)

return
end function

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


MODULE PS8PARA
    IMPLICIT NONE
    INTEGER:: I ! FOR ITER
    REAL(KIND=8), PARAMETER:: BETA=0.99, ALPHA=0.3, DELTA=0.025, EFF=0.3271
    REAL(KIND=8), DIMENSION(2), PARAMETER:: PROD_SHOCK=(/1.01, 0.99/), EMP_STATE=(/1.,0./)
    ! DROP: THE NUMBER OF PERIODS TO DROP INIT DEPENDENCE
    INTEGER, PARAMETER:: N=500, T=11000, DROP=1000

    REAL(KIND=8), PARAMETER:: K_MIN=0.01, K_MAX=15.01 ! DEFINE SMALL K GRIDS
    REAL(KIND=8), PARAMETER:: AGG_K_MIN=1.0, AGG_K_MAX=15.0
    INTEGER, PARAMETER:: NZ=SIZE(PROD_SHOCK), NY=SIZE(EMP_STATE), NK=100, NAK=10
    REAL(KIND=8), PARAMETER:: KSTEP=(K_MAX-K_MIN)/FLOAT(NK-1), AGG_KSTEP=(AGG_K_MAX-AGG_K_MIN)/FLOAT(NAK-1)
    REAL(KIND=8), DIMENSION(NK),PARAMETER:: K_GRID =(/(I*KSTEP, I=1,NK)/) + K_MIN - KSTEP
    REAL(KIND=8), DIMENSION(NAK), PARAMETER:: AK_GRID =(/(I*AGG_KSTEP, I=1,NAK)/) + AGG_K_MIN - AGG_KSTEP
    INTEGER, DIMENSION(T):: AGG_SHOCK_VEC
    INTEGER, DIMENSION(N, T):: IND_SHOCK_VEC
    ! ASD
    REAL(KIND=8),PARAMETER:: U_G=0.04, U_B=0.1
    REAL(KIND=8), DIMENSION(2):: LAB_SUP=(/1.-U_G, 1.-U_B/)*EFF
    REAL(KIND=8), DIMENSION(4,4):: TRANS_MAT=&
    TRANSPOSE(RESHAPE((/0.8507,0.1159,0.0243,0.0091,0.1229,0.8361,0.0021,&
    0.0389,0.5833,0.0313,0.2917,0.0938,0.0938,0.3500,0.0313,0.5250/), (/4,4/)))

    REAL(KIND=8), DIMENSION(NAK, NZ):: INTEREST, WAGE
    CONTAINS
        SUBROUTINE CAL_PRICE()
            INTEGER:: ZIDX, AKIDX
            REAL(KIND=8):: NORM_K
            ! UNRELATED TO INDIVIDUAL PROBLEMS
            DO AKIDX=1, NAK
                DO ZIDX=1, NZ
                    NORM_K = AK_GRID(AKIDX)/LAB_SUP(ZIDX)
                    INTEREST(AKIDX, ZIDX) = ALPHA*PROD_SHOCK(ZIDX)*(NORM_K**(ALPHA-1.))
                    WAGE(AKIDX, ZIDX) = (1.-ALPHA)*PROD_SHOCK(ZIDX)*(NORM_K**ALPHA)
                ENDDO
            ENDDO
        END SUBROUTINE
    END MODULE

MODULE PS8RES
    USE PS8PARA
    IMPLICIT NONE
    REAL(KIND=8), DIMENSION(NK, NY, NAK, NZ):: VFUNC=0., VFUNC_TMP
    REAL(KIND=8), DIMENSION(NK, NY, NAK, NZ, NK):: VFUNC_NEW
    INTEGER, DIMENSION(NK, NY, NAK, NZ):: PFUNC, PFUNC_NEW
    REAL(KIND=8), DIMENSION(2):: INTERCEPT=(/0.095, 0.085/), SLOPE=(/0.99, 0.99/)
    REAL(KIND=8):: R_SQ=0., ERROR=100.
    INTEGER, DIMENSION(T-DROP):: AGG_SHOCK_VEC_TRIM
    INTEGER, DIMENSION(N, T-DROP):: IND_TRIM
    REAL(KIND=8), DIMENSION(NAK, NZ):: KPR_EST
    REAL(KIND=8), DIMENSION(NK, NY, NAK, NZ, NK):: CONSUM, UTIL
    REAL(KIND=8), PARAMETER:: CRIT=1e-1
    REAL(KIND=8), DIMENSION(T):: SIM_AK
    REAL(KIND=8), DIMENSION(N,T):: SIM_SMALLK
    REAL(KIND=8), DIMENSION(NZ):: R_SQUARE
END MODULE

PROGRAM PS8
    USE PS8PARA
    USE PS8RES
    IMPLICIT NONE

    CALL CAL_PRICE()
    CALL INIT_SHOCKS()
    DO WHILE (ERROR>CRIT .OR. MAXVAL(R_SQUARE)<0.8)
        CALL VFI()
        CALL PSEUDO_PANEL()
        CALL CAL_ERRORS()
    ENDDO
    CALL WRITE_ALL()
END PROGRAM

SUBROUTINE VFI()
        USE PS8PARA
        USE PS8RES
        USE OMP_LIB
        IMPLICIT NONE
        INTEGER:: KIDX, AKIDX, ZIDX, YIDX, KPIDX
        REAL(KIND=8):: ERROR_VFI

        ! FOR PARALLEL CALCULATION, REDUCE PRIVATE VARIABLES IN MAIN LOOP
        ! PRE CALCULATE THE KPR, CONSUM, UTILTIY
        DO AKIDX=1,NAK
        DO ZIDX=1, NZ
            KPR_EST(AKIDX, ZIDX) = EXP(INTERCEPT(ZIDX)+SLOPE(ZIDX)*LOG(AK_GRID(AKIDX)))
            IF (KPR_EST(AKIDX, ZIDX)<AGG_K_MIN) THEN
                KPR_EST(AKIDX, ZIDX) = AK_GRID(1)
            ELSE IF (KPR_EST(AKIDX, ZIDX)>AK_GRID(NAK)) THEN
                KPR_EST(AKIDX, ZIDX) = AGG_K_MAX
            ENDIF
        ENDDO
        ENDDO

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

        ERROR_VFI=100.
        VFUNC_NEW = -1e12
        VFUNC=UTIL(:,:,:,:,1)
        DO WHILE (ERROR_VFI> CRIT) ! VFI
            CALL BELLMAN()
            ERROR_VFI = MAXVAL(ABS(VFUNC_TMP-VFUNC))
            PRINT*, ERROR_VFI, MAXLOC(ABS(VFUNC_TMP-VFUNC))
            VFUNC = VFUNC_TMP
        ENDDO
END SUBROUTINE

SUBROUTINE BELLMAN()
    USE PS8PARA
    USE PS8RES
    USE OMP_LIB
    IMPLICIT NONE
    INTEGER:: AKIDX, ZIDX, KIDX,YIDX, ZZI,YYI, ROWIDX, NEXTROW, KPIDX
    REAL(KIND=8), DIMENSION(1):: KPR, SMALL_KPR !, NEXTUGE, NEXTUGN, NEXTUBE, NEXTUBN
    REAL(KIND=8), DIMENSION(1):: NEXTU
    REAL(KIND=8):: EXPU

    ! DEFINE SOME INTERPOLATIONS
    !$OMP PARALLEL DO DEFAULT(SHARED) PRIVATE(AKIDX, ZIDX, KIDX,YIDX, ZZI,YYI, ROWIDX, NEXTROW, KPIDX, KPR, SMALL_KPR, NEXTU,EXPU)
    DO AKIDX=1,NAK
    DO ZIDX=1, NZ
    DO YIDX=1,NY
    DO KIDX=1,NK
        KPR(1) = KPR_EST(AKIDX, ZIDX)
        DO KPIDX=1, NK
            SMALL_KPR(1) = K_GRID(KPIDX)
            EXPU = 0.
            ROWIDX =ZIDX + 2*(YIDX-1)
            DO ZZI=1, NZ
            DO YYI=1,NY ! SHOULD USE 4 INTER2D INSTEAD OF INCREMENT LOOP FOR PARALLEL
                NEXTROW = ZZI + 2*(YYI-1)
                CALL PWL_INTERP_2D(NK, NAK, K_GRID, AK_GRID, VFUNC(:,YYI, :, ZZI), 1, SMALL_KPR, KPR, NEXTU)
                EXPU = EXPU+TRANS_MAT(ROWIDX,NEXTROW)*NEXTU(1)
            ENDDO
            ENDDO
            VFUNC_NEW(KIDX, YIDX, AKIDX, ZIDX,KPIDX) = UTIL(KIDX, YIDX, AKIDX, ZIDX, KPIDX)+ BETA*EXPU
        ENDDO
        PFUNC(KIDX, YIDX, AKIDX, ZIDX) = MAXLOC(VFUNC_NEW(KIDX, YIDX, AKIDX, ZIDX, :),1)
        VFUNC_TMP(KIDX, YIDX, AKIDX, ZIDX) = MAXVAL(VFUNC_NEW(KIDX, YIDX, AKIDX, ZIDX, :),1)
    ENDDO
    ENDDO
    ENDDO
    ENDDO
    !$OMP END PARALLEL DO

END SUBROUTINE

SUBROUTINE INIT_SHOCKS()
    USE PS8PARA
    USE PS8RES
    IMPLICIT NONE
    INTEGER:: TIDX, NIDX, Z_PREV
    REAL(KIND=8):: RAND_TMP, THIS_ROW
    REAL(KIND=8):: TRANS_PROB
    INTEGER, PARAMETER:: RSEED=4200
    INTEGER:: OTHER_STATE, CUR_SHOCK, LAST_ROW, NEXT_STATE, LAST_ASHOCK

    CALL SRAND(RSEED)

    ! FIRST ONE, DETERMINISTIC EVERY RUN (WILL DROP ANYWAY, IS OK)
    AGG_SHOCK_VEC(1) = 1 ! STORE INDEX INSTEAD OF VALUE OF SHOCK HERE, START GOOD
    ! AGGREGATE SHOCKS
    DO TIDX=2,T
        Z_PREV = AGG_SHOCK_VEC(TIDX-1)
        CALL RANDOM_NUMBER(RAND_TMP)
        NEXT_STATE = Z_PREV
        IF (RAND_TMP<1./8.) THEN ! RANDOM NEXT STATE (SYMMETRIC)
            OTHER_STATE =1
            IF (Z_PREV==1) THEN
                OTHER_STATE = 2
            ENDIF
            NEXT_STATE = OTHER_STATE
        ENDIF
        AGG_SHOCK_VEC(TIDX) = NEXT_STATE
    ENDDO

    IND_SHOCK_VEC(:,1) = 1 ! EVERYONE EMPLOYED STATE
    ! INDIVIDUAL SHOCKS
    DO TIDX=2,T
        CUR_SHOCK = AGG_SHOCK_VEC(TIDX)
        LAST_ASHOCK = AGG_SHOCK_VEC(TIDX-1)
        DO NIDX=1,N
            CALL RANDOM_NUMBER(RAND_TMP)
            TRANS_PROB = RAND_TMP
            LAST_ROW = LAST_ASHOCK + 2*(IND_SHOCK_VEC(NIDX, TIDX-1)-1)
            IF (CUR_SHOCK==1) THEN
                THIS_ROW = TRANS_MAT(LAST_ROW,1)/(TRANS_MAT(LAST_ROW,1)+TRANS_MAT(LAST_ROW,3))
            ELSE
                THIS_ROW = TRANS_MAT(LAST_ROW,2)/(TRANS_MAT(LAST_ROW,2)+TRANS_MAT(LAST_ROW,4))
            ENDIF
            IF (THIS_ROW>TRANS_PROB) THEN ! IF MORE THAN, THEN UMEMPLOY
                IND_SHOCK_VEC(NIDX, TIDX) = 1
            ELSE
                IND_SHOCK_VEC(NIDX, TIDX) = 2
            ENDIF
        ENDDO
    ENDDO

    ! NEED TRIMMED TO DROP INITIAL DEPENDENCE
    AGG_SHOCK_VEC_TRIM = AGG_SHOCK_VEC((DROP+1):T)
    IND_TRIM = IND_SHOCK_VEC(:,DROP+1:T)
END SUBROUTINE

SUBROUTINE PSEUDO_PANEL()
    USE PS8PARA
    USE PS8RES
    USE OMP_LIB
    IMPLICIT NONE
    REAL(KIND=8), PARAMETER:: INIT_K=5.7163
    INTEGER:: TIDX, ZIDX, YIDX, NIDX, AKIDX
    REAL(KIND=8), DIMENSION(NK, NY, NAK, NZ):: KDECISION
    REAL(KIND=8), DIMENSION(1):: KPR, SMALL_KPR, SMALLK_TMP

    ! DON'T NEED TO WORK WITH SHOCKS AGAIN, ALWAYS WORK WITH SAME SET OF SHOCKS
    SIM_AK = 0.
    ! FIND AGGREGATE CAPITAL, USING THE THE FUNCTIONAL FORM
    SIM_AK(1) = INIT_K

    ! POPULATE SMALL K, USING THE POLICY FUNCTION FOUND IN BELLMAN
    DO YIDX=1,NY
    DO ZIDX=1,NZ
    DO AKIDX=1,NAK
    KDECISION(:,YIDX, AKIDX, ZIDX) = K_GRID(PFUNC(:,YIDX, AKIDX, ZIDX))
    ENDDO
    ENDDO
    ENDDO
    SIM_SMALLK(:,1)=INIT_K
    DO TIDX=2,T
    DO NIDX=1, N
        ZIDX= AGG_SHOCK_VEC(TIDX)
        YIDX= IND_SHOCK_VEC(NIDX,TIDX)
        KPR(1)= SIM_AK(TIDX-1)
        SMALL_KPR(1) = SIM_SMALLK(NIDX, TIDX-1)

        CALL PWL_INTERP_2D(NK, NAK, K_GRID, AK_GRID, KDECISION(:,YIDX,:,ZIDX), 1, SMALL_KPR, KPR, SMALLK_TMP)
        SIM_SMALLK(NIDX, TIDX) = SMALLK_TMP(1)
    ENDDO
        SIM_AK(TIDX) = SUM(SIM_SMALLK(:,TIDX),1)/N
    ENDDO
END SUBROUTINE

SUBROUTINE CAL_ERRORS()
    USE PS8PARA
    USE PS8RES
    IMPLICIT NONE
    REAL(KIND=8), DIMENSION((NZ*2)):: ERROR_PARA
    REAL(KIND=8), DIMENSION(2)::INTERCEPT_NEW, SLOPE_NEW
    REAL(KIND=8), DIMENSION(:,:), ALLOCATABLE :: SIM_GOOD, SIM_BAD
    INTEGER:: NGOOD, NBAD, COUNT_GOOD, COUNT_BAD
    INTEGER:: TIDX, IDX
    REAL(KIND=8):: R1, R2, R3, MEAN, S_RES

    ! CALCUALTE NEW INTERCEPT AND SLOPE FOR FUNCTIONAL FORMS
    ! 1. SEPARTE INTO GOOD AND BAD STATE
    NBAD = SUM(AGG_SHOCK_VEC_TRIM(:T-1-DROP)-1)
    NGOOD = SIZE(AGG_SHOCK_VEC_TRIM(:T-1-DROP)) - NBAD
    ALLOCATE(SIM_GOOD(NGOOD,2)) ! 1 IS X, 2 IS Y
    ALLOCATE(SIM_BAD(NBAD,2))
    SIM_BAD=1.
    SIM_GOOD=1.
    COUNT_GOOD=1
    COUNT_BAD=1
    DO TIDX=DROP+1,T-1
        IF (AGG_SHOCK_VEC(TIDX)==2) THEN ! IF BAD
            SIM_BAD(COUNT_BAD, 1) = SIM_AK(TIDX)
            SIM_BAD(COUNT_BAD,2) = SIM_AK(TIDX+1)
            COUNT_BAD = COUNT_BAD +1
        ELSE
            SIM_GOOD(COUNT_GOOD,1) = SIM_AK(TIDX)
            SIM_GOOD(COUNT_GOOD,2) = SIM_AK(TIDX+1)
            COUNT_GOOD = COUNT_GOOD +1
        ENDIF
    ENDDO

    ! 2. RUN OLS AND GET R SQUARE AND THE INTERCEPTS -GOOD
    MEAN = SUM(LOG(SIM_GOOD(:,2)))/N
    R1 =SUM((LOG(SIM_GOOD(:,2))-MEAN)**2)
    R2 =SUM((LOG(SIM_GOOD(:,2))-MEAN)*(LOG(SIM_GOOD(:,1))-SUM(LOG(SIM_GOOD(:,1)))/N))
    R3 =SUM((LOG(SIM_GOOD(:,1))-SUM(LOG(SIM_GOOD(:,1)))/N)**2)
    PRINT*, "RS", R1, R2, R3, MEAN
    SLOPE_NEW(1) = R2/R1 ! BETA = (X'Y)/(X'X)
    INTERCEPT_NEW(1) = (SUM((LOG(SIM_GOOD(:,2)))) - SLOPE_NEW(1)*SUM((LOG(SIM_GOOD(:,1)))))/NGOOD
    S_RES = 0.
    DO IDX=1, NGOOD
        S_RES = S_RES+ SIM_GOOD(IDX,2) - INTERCEPT_NEW(1)-SLOPE_NEW(1)*SIM_GOOD(IDX,1)
    ENDDO
    R_SQUARE(1) = 1- S_RES/R3 ! R_SQUARE
    ! SAME THING FOR BAD STATES
    MEAN = SUM(LOG(SIM_BAD(:,2)))/N
    PRINT*,"MEANB", MEAN, SUM(LOG(SIM_BAD(:,2)))
    R1 =SUM((LOG(SIM_BAD(:,2))-MEAN)**2)
    R2 =SUM((LOG(SIM_BAD(:,2))-MEAN)*(LOG(SIM_BAD(:,1))-SUM(LOG(SIM_BAD(:,1)))/N))
    R3 =SUM((LOG(SIM_BAD(:,1))-SUM(LOG(SIM_BAD(:,1)/N)))**2)
    SLOPE_NEW(2) = R2/R1
    INTERCEPT_NEW(2) = (SUM((LOG(SIM_BAD(:,2)))) - SLOPE_NEW(1)*SUM((LOG(SIM_BAD(:,1)))))/NBAD
    S_RES = 0.
    DO IDX=1, NBAD
        S_RES = S_RES+ SIM_BAD(IDX,2) - INTERCEPT_NEW(1)-SLOPE_NEW(1)*SIM_BAD(IDX,1)
    ENDDO
    R_SQUARE(2) = 1- S_RES/R3 ! R_SQUARE

    ! CALCULATE ERROR
    ERROR_PARA(1:2) = SLOPE_NEW-SLOPE
    ERROR_PARA(3:4) = INTERCEPT_NEW-INTERCEPT
    PRINT*,"RSQ", R_SQUARE
    PRINT*,"ERROR_PARA", ERROR_PARA
    PRINT*, "PARAS", INTERCEPT_NEW, SLOPE_NEW
    PRINT*, "=============================================================="
    ERROR = MAXVAL(ABS(ERROR_PARA))
    INTERCEPT=INTERCEPT_NEW
    SLOPE=SLOPE_NEW
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 PS8RES
    USE PS8PARA
    IMPLICIT NONE
    INTEGER:: SROWIDX
    CHARACTER(LEN=130):: PATH="/Users/chek_choi/Downloads/fortran/"
    CHARACTER(LEN=150):: FILE_NAME

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

        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,1, 2,1)), K_GRID(PFUNC(SROWIDX,1, 9,1))
        ENDDO
        CLOSE(UNIT=2)

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

        FILE_NAME = TRIM(PATH)//"SIM_SMALLK"
        OPEN(UNIT=4, FILE=FILE_NAME, STATUS='REPLACE') ! FOR HAVING THE X-AXIS OF PLOT
        DO SROWIDX=1, T
            WRITE(UNIT=4,FMT=*) SIM_SMALLK(10, SROWIDX)
        ENDDO
        CLOSE(UNIT=4)

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

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

        FILE_NAME = TRIM(PATH)//"CONSUM"
        OPEN(UNIT=65, FILE=FILE_NAME, STATUS='REPLACE') ! START WITH THE TWO VALUE FUNCTIONS
        DO SROWIDX=1, NK
            WRITE(UNIT=65,FMT=*) CONSUM(80, 1, 5, 1, SROWIDX), CONSUM(90, 1, 5, 1, SROWIDX)
        ENDDO
        CLOSE(UNIT=65)

       FILE_NAME = TRIM(PATH)//"UTIL"
        OPEN(UNIT=66, FILE=FILE_NAME, STATUS='REPLACE') ! START WITH THE TWO VALUE FUNCTIONS
        DO SROWIDX=1, NK
            WRITE(UNIT=66,FMT=*) UTIL(80, 1, 5, 1, SROWIDX), UTIL(90, 1, 5, 1, SROWIDX)
        ENDDO
        CLOSE(UNIT=66)

END SUBROUTINE


  subroutine testing
!
!  implicit none
  implicit real*8 (a-h,o-z),integer(i-n)
!
  logical :: fullset=.false.
  integer :: is,npoi
  double precision :: pi
  double precision :: s,theta,varphi,A_phi,A_theta,dA_phi_ds,dA_theta_ds,aiota,      &
                      R,Z,alam,dR_ds,dR_dt,dR_dp,dZ_ds,dZ_dt,dZ_dp,dl_ds,dl_dt,dl_dp
  double precision :: ds
  double precision, dimension(3,3) :: cmat,gV,g
  double precision, dimension(3) :: x,bder,hcovar,hctrvr,hcurl
  double precision, dimension(:), allocatable :: dummy,dummy1
  double precision, dimension(:,:), allocatable :: dummy2d
  double precision, dimension(:,:), allocatable :: dummy2d1
!
  pi=4.d0*atan2(1.d0,1.d0)
!
  npoi=1000
  ds=1.d0/npoi
  vartheta_c=0.5d0
  varphi_c=0.5d0
!
!if(.false.) then
if(.true.) then
  do is=2,npoi
    s=ds*is
    r=s
    mode_secders=2
!
    call splint_can_coord(fullset,mode_secders,r,vartheta_c,varphi_c,                      &
                          A_theta,A_phi,dA_theta_dr,dA_phi_dr,d2A_phi_dr2,d3A_phi_dr3,     &
                          sqg_c,dsqg_c_dr,dsqg_c_dt,dsqg_c_dp,                             &
                          B_vartheta_c,dB_vartheta_c_dr,dB_vartheta_c_dt,dB_vartheta_c_dp, &
                          B_varphi_c,dB_varphi_c_dr,dB_varphi_c_dt,dB_varphi_c_dp,         &
                          d2sqg_rr,d2sqg_rt,d2sqg_rp,d2sqg_tt,d2sqg_tp,d2sqg_pp,           &
                          d2bth_rr,d2bth_rt,d2bth_rp,d2bth_tt,d2bth_tp,d2bth_pp,           &
                          d2bph_rr,d2bph_rt,d2bph_rp,d2bph_tt,d2bph_tp,d2bph_pp,G_c)
!
    bmod2=(B_varphi_c*dA_theta_dr-B_vartheta_c*dA_phi_dr)/sqg_c
    dbmod2_dr=(dB_varphi_c_dr*dA_theta_dr-dB_vartheta_c_dr*dA_phi_dr-B_vartheta_c*d2A_phi_dr2-bmod2*dsqg_c_dr)/sqg_c
    dbmod2_dt=(dB_varphi_c_dt*dA_theta_dr-dB_vartheta_c_dt*dA_phi_dr-bmod2*dsqg_c_dt)/sqg_c
    dbmod2_dp=(dB_varphi_c_dp*dA_theta_dr-dB_vartheta_c_dp*dA_phi_dr-bmod2*dsqg_c_dp)/sqg_c
!
    bmod=sqrt(bmod2)
    twobmod=2.d0*bmod
    dbmod_dr=dbmod2_dr/twobmod
    dbmod_dt=dbmod2_dt/twobmod
    dbmod_dp=dbmod2_dp/twobmod
!
    if(mode_secders.eq.1) then
      d2bmod2_rr=(d2bph_rr*dA_theta_dr-d2bth_rr*dA_phi_dr-2.d0*dB_vartheta_c_dr*d2A_phi_dr2-B_vartheta_c*d3A_phi_dr3 &
                 - 2.d0*dsqg_c_dr*dbmod2_dr-bmod2*d2sqg_rr)/sqg_c
!
      d2bmod_rr=d2bmod2_rr/twobmod-dbmod_dr**2/bmod
    elseif(mode_secders.eq.2) then
      d2bmod2_rr=(d2bph_rr*dA_theta_dr-d2bth_rr*dA_phi_dr-2.d0*dB_vartheta_c_dr*d2A_phi_dr2-B_vartheta_c*d3A_phi_dr3 &
                 - 2.d0*dsqg_c_dr*dbmod2_dr-bmod2*d2sqg_rr)/sqg_c
      d2bmod2_rt=(d2bph_rt*dA_theta_dr-d2bth_rt*dA_phi_dr-dB_vartheta_c_dt*d2A_phi_dr2                               &
                 - dsqg_c_dr*dbmod2_dt-dsqg_c_dt*dbmod2_dr-bmod2*d2sqg_rt)/sqg_c
      d2bmod2_rp=(d2bph_rp*dA_theta_dr-d2bth_rp*dA_phi_dr-dB_vartheta_c_dp*d2A_phi_dr2                               &
                 - dsqg_c_dr*dbmod2_dp-dsqg_c_dp*dbmod2_dr-bmod2*d2sqg_rp)/sqg_c
      d2bmod2_tt=(d2bph_tt*dA_theta_dr-d2bth_tt*dA_phi_dr-2.d0*dsqg_c_dt*dbmod2_dt-bmod2*d2sqg_tt)/sqg_c
      d2bmod2_tp=(d2bph_tp*dA_theta_dr-d2bth_tp*dA_phi_dr-dsqg_c_dt*dbmod2_dp-dsqg_c_dp*dbmod2_dt-bmod2*d2sqg_tp)/sqg_c
      d2bmod2_pp=(d2bph_pp*dA_theta_dr-d2bth_pp*dA_phi_dr-2.d0*dsqg_c_dp*dbmod2_dp-bmod2*d2sqg_pp)/sqg_c
!
      d2bmod_rr=d2bmod2_rr/twobmod-dbmod_dr**2/bmod
      d2bmod_rt=d2bmod2_rt/twobmod-dbmod_dr*dbmod_dt/bmod
      d2bmod_rp=d2bmod2_rp/twobmod-dbmod_dr*dbmod_dp/bmod
      d2bmod_tt=d2bmod2_tt/twobmod-dbmod_dt**2/bmod
      d2bmod_tp=d2bmod2_tp/twobmod-dbmod_dt*dbmod_dp/bmod
      d2bmod_pp=d2bmod2_pp/twobmod-dbmod_dp**2/bmod
    endif
!
    write (1001,*) s,d2bmod_rr,d2bmod_rt,d2bmod_rp,d2bmod_tt,d2bmod_tp,d2bmod_pp
!
!
    call splint_can_coord(fullset,mode_secders,r,vartheta_c+1d-4,varphi_c,                      &
                          A_theta,A_phi,dA_theta_dr,dA_phi_dr,d2A_phi_dr2,d3A_phi_dr3,     &
                          sqg_c,dsqg_c_dr,dsqg_c_dt,dsqg_c_dp,                             &
                          B_vartheta_c,dB_vartheta_c_dr,dB_vartheta_c_dt,dB_vartheta_c_dp, &
                          B_varphi_c,dB_varphi_c_dr,dB_varphi_c_dt,dB_varphi_c_dp,         &
                          d2sqg_rr,d2sqg_rt,d2sqg_rp,d2sqg_tt,d2sqg_tp,d2sqg_pp,           &
                          d2bth_rr,d2bth_rt,d2bth_rp,d2bth_tt,d2bth_tp,d2bth_pp,           &
                          d2bph_rr,d2bph_rt,d2bph_rp,d2bph_tt,d2bph_tp,d2bph_pp,G_c)
!
    bmod2=(B_varphi_c*dA_theta_dr-B_vartheta_c*dA_phi_dr)/sqg_c
    dbmod2_dr=(dB_varphi_c_dr*dA_theta_dr-dB_vartheta_c_dr*dA_phi_dr-B_vartheta_c*d2A_phi_dr2-bmod2*dsqg_c_dr)/sqg_c
    dbmod2_dt=(dB_varphi_c_dt*dA_theta_dr-dB_vartheta_c_dt*dA_phi_dr-bmod2*dsqg_c_dt)/sqg_c
    dbmod2_dp=(dB_varphi_c_dp*dA_theta_dr-dB_vartheta_c_dp*dA_phi_dr-bmod2*dsqg_c_dp)/sqg_c
!
    bmod=sqrt(bmod2)
    twobmod=2.d0*bmod
    dbmod_dr_ph=dbmod2_dr/twobmod
    dbmod_dt_ph=dbmod2_dt/twobmod
    dbmod_dp_ph=dbmod2_dp/twobmod
!
    call splint_can_coord(fullset,mode_secders,r,vartheta_c-1d-4,varphi_c,                      &
                          A_theta,A_phi,dA_theta_dr,dA_phi_dr,d2A_phi_dr2,d3A_phi_dr3,     &
                          sqg_c,dsqg_c_dr,dsqg_c_dt,dsqg_c_dp,                             &
                          B_vartheta_c,dB_vartheta_c_dr,dB_vartheta_c_dt,dB_vartheta_c_dp, &
                          B_varphi_c,dB_varphi_c_dr,dB_varphi_c_dt,dB_varphi_c_dp,         &
                          d2sqg_rr,d2sqg_rt,d2sqg_rp,d2sqg_tt,d2sqg_tp,d2sqg_pp,           &
                          d2bth_rr,d2bth_rt,d2bth_rp,d2bth_tt,d2bth_tp,d2bth_pp,           &
                          d2bph_rr,d2bph_rt,d2bph_rp,d2bph_tt,d2bph_tp,d2bph_pp,G_c)
!
    bmod2=(B_varphi_c*dA_theta_dr-B_vartheta_c*dA_phi_dr)/sqg_c
    dbmod2_dr=(dB_varphi_c_dr*dA_theta_dr-dB_vartheta_c_dr*dA_phi_dr-B_vartheta_c*d2A_phi_dr2-bmod2*dsqg_c_dr)/sqg_c
    dbmod2_dt=(dB_varphi_c_dt*dA_theta_dr-dB_vartheta_c_dt*dA_phi_dr-bmod2*dsqg_c_dt)/sqg_c
    dbmod2_dp=(dB_varphi_c_dp*dA_theta_dr-dB_vartheta_c_dp*dA_phi_dr-bmod2*dsqg_c_dp)/sqg_c
!
    bmod=sqrt(bmod2)
    twobmod=2.d0*bmod
    dbmod_dr_mh=dbmod2_dr/twobmod
    dbmod_dt_mh=dbmod2_dt/twobmod
    dbmod_dp_mh=dbmod2_dp/twobmod
!
    write (1002,*) s,5d3*(dbmod_dr_ph-dbmod_dr_mh),5d3*(dbmod_dt_ph-dbmod_dt_mh),5d3*(dbmod_dp_ph-dbmod_dp_mh)
!
!
!
    call splint_can_coord(fullset,mode_secders,r,vartheta_c,varphi_c+1d-4,                      &
                          A_theta,A_phi,dA_theta_dr,dA_phi_dr,d2A_phi_dr2,d3A_phi_dr3,     &
                          sqg_c,dsqg_c_dr,dsqg_c_dt,dsqg_c_dp,                             &
                          B_vartheta_c,dB_vartheta_c_dr,dB_vartheta_c_dt,dB_vartheta_c_dp, &
                          B_varphi_c,dB_varphi_c_dr,dB_varphi_c_dt,dB_varphi_c_dp,         &
                          d2sqg_rr,d2sqg_rt,d2sqg_rp,d2sqg_tt,d2sqg_tp,d2sqg_pp,           &
                          d2bth_rr,d2bth_rt,d2bth_rp,d2bth_tt,d2bth_tp,d2bth_pp,           &
                          d2bph_rr,d2bph_rt,d2bph_rp,d2bph_tt,d2bph_tp,d2bph_pp,G_c)
!
    bmod2=(B_varphi_c*dA_theta_dr-B_vartheta_c*dA_phi_dr)/sqg_c
    dbmod2_dr=(dB_varphi_c_dr*dA_theta_dr-dB_vartheta_c_dr*dA_phi_dr-B_vartheta_c*d2A_phi_dr2-bmod2*dsqg_c_dr)/sqg_c
    dbmod2_dt=(dB_varphi_c_dt*dA_theta_dr-dB_vartheta_c_dt*dA_phi_dr-bmod2*dsqg_c_dt)/sqg_c
    dbmod2_dp=(dB_varphi_c_dp*dA_theta_dr-dB_vartheta_c_dp*dA_phi_dr-bmod2*dsqg_c_dp)/sqg_c
!
    bmod=sqrt(bmod2)
    twobmod=2.d0*bmod
    dbmod_dr_ph=dbmod2_dr/twobmod
    dbmod_dt_ph=dbmod2_dt/twobmod
    dbmod_dp_ph=dbmod2_dp/twobmod
!
    call splint_can_coord(fullset,mode_secders,r,vartheta_c,varphi_c-1d-4,                      &
                          A_theta,A_phi,dA_theta_dr,dA_phi_dr,d2A_phi_dr2,d3A_phi_dr3,     &
                          sqg_c,dsqg_c_dr,dsqg_c_dt,dsqg_c_dp,                             &
                          B_vartheta_c,dB_vartheta_c_dr,dB_vartheta_c_dt,dB_vartheta_c_dp, &
                          B_varphi_c,dB_varphi_c_dr,dB_varphi_c_dt,dB_varphi_c_dp,         &
                          d2sqg_rr,d2sqg_rt,d2sqg_rp,d2sqg_tt,d2sqg_tp,d2sqg_pp,           &
                          d2bth_rr,d2bth_rt,d2bth_rp,d2bth_tt,d2bth_tp,d2bth_pp,           &
                          d2bph_rr,d2bph_rt,d2bph_rp,d2bph_tt,d2bph_tp,d2bph_pp,G_c)
!
    bmod2=(B_varphi_c*dA_theta_dr-B_vartheta_c*dA_phi_dr)/sqg_c
    dbmod2_dr=(dB_varphi_c_dr*dA_theta_dr-dB_vartheta_c_dr*dA_phi_dr-B_vartheta_c*d2A_phi_dr2-bmod2*dsqg_c_dr)/sqg_c
    dbmod2_dt=(dB_varphi_c_dt*dA_theta_dr-dB_vartheta_c_dt*dA_phi_dr-bmod2*dsqg_c_dt)/sqg_c
    dbmod2_dp=(dB_varphi_c_dp*dA_theta_dr-dB_vartheta_c_dp*dA_phi_dr-bmod2*dsqg_c_dp)/sqg_c
!
    bmod=sqrt(bmod2)
    twobmod=2.d0*bmod
    dbmod_dr_mh=dbmod2_dr/twobmod
    dbmod_dt_mh=dbmod2_dt/twobmod
    dbmod_dp_mh=dbmod2_dp/twobmod
!
    write (1003,*) s,5d3*(dbmod_dr_ph-dbmod_dr_mh),5d3*(dbmod_dt_ph-dbmod_dt_mh),5d3*(dbmod_dp_ph-dbmod_dp_mh)
!
!
if(.false.) then
    x(1)=r
    x(2)=vartheta_c
    x(3)=varphi_c
!
    call magfie_can(x,bmod,sqrtg,bder,hcovar,hctrvr,hcurl)
!
    write (1005,*) s,bmod*hcurl(1),bmod*(hcurl(1)-(hcovar(2)*bder(3)-hcovar(3)*bder(2))/sqrtg),bmod
!
    call can_to_vmec(r,vartheta_c,varphi_c,theta_vmec,varphi_vmec)
!
    x(2)=theta_vmec
    x(3)=varphi_vmec
!
    call magfie_vmec(x,bmod,sqrtg,bder,hcovar,hctrvr,hcurl)
!
    write (1006,*) s,bmod*hcurl(1),bmod*(hcurl(1)-(hcovar(2)*bder(3)-hcovar(3)*bder(2))/sqrtg),bmod
endif
!
  enddo
stop
endif
!
end


      SUBROUTINE TG01ID( JOBOBS, COMPQ, COMPZ, N, M, P, A, LDA, E, LDE,
     $                   B, LDB, C, LDC, Q, LDQ, Z, LDZ, NOBSV, NIUOBS,
     $                   NLBLCK, CTAU, TOL, IWORK, DWORK, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To compute orthogonal transformation matrices Q and Z which
C     reduce the N-th order descriptor system (A-lambda*E,B,C)
C     to the form
C
C                ( Ano  * )             ( Eno  * )           ( Bno )
C       Q'*A*Z = (        ) ,  Q'*E*Z = (        ) ,  Q'*B = (     ) ,
C                ( 0   Ao )             ( 0   Eo )           ( Bo  )
C
C          C*Z = ( 0   Co ) ,
C
C     where the NOBSV-th order descriptor system (Ao-lambda*Eo,Bo,Co)
C     is a finite and/or infinite observable. The pencil
C     Ano - lambda*Eno is regular of order N-NOBSV and contains the
C     unobservable finite and/or infinite eigenvalues of the pencil
C     A-lambda*E.
C
C     For JOBOBS = 'O' or 'I', the pencil ( Eo-lambda*Ao ) has full
C                                         (      Co      )
C     column rank NOBSV for all finite lambda and is in a staircase form
C     with
C                     _      _            _      _
C                   ( Ek,k   Ek,k-1   ... Ek,2   Ek,1   )
C                   ( _      _            _      _      )
C       ( Eo ) =    ( Ek-1,k Ek-1,k-1 ... Ek-1,2 Ek-1,1 ) ,  (1)
C       ( Co )      (     ...         ... _      _      )
C                   (  0       0      ... E1,2   E1,1   )
C                   (                            _      )
C                   (  0       0      ... 0      E0,1   )
C                     _          _      _
C                   ( Ak,k  ...  Ak,2   Ak,1 )
C                   (       ...  _      _    )
C         Ao      = (   0   ...  A2,2   A2,1 ) ,             (2)
C                   (                   _    )
C                   (   0   ...    0    A1,1 )
C           _
C     where Ei-1,i is a CTAU(i-1)-by-CTAU(i) full column rank matrix
C                            _
C     (with CTAU(0) = P) and Ai,i is a CTAU(i)-by-CTAU(i)
C     upper triangular matrix.
C
C     For JOBOBS = 'F', the pencil ( Ao-lambda*Eo ) has full
C                                  (      Co      )
C     column rank NOBSV for all finite lambda and is in a staircase form
C     with
C                     _      _            _      _
C                   ( Ak,k   Ak,k-1   ... Ak,2   Ak,1   )
C                   ( _      _            _      _      )
C       ( Ao ) =    ( Ak-1,k Ak-1,k-1 ... Ak-1,2 Ak-1,1 ) ,  (3)
C       ( Co )      (     ...         ... _      _      )
C                   (  0       0      ... A1,2   A1,1   )
C                   (                            _      )
C                   (  0       0      ... 0      A0,1   )
C                     _          _      _
C                   ( Ek,k  ...  Ek,2   Ek,1 )
C                   (       ...  _      _    )
C         Eo      = (   0   ...  E2,2   E2,1 ) ,             (4)
C                   (                   _    )
C                   (   0   ...    0    E1,1 )
C           _
C     where Ai-1,i is a CTAU(i-1)-by-CTAU(i) full column rank matrix
C                            _
C     (with CTAU(0) = P) and Ei,i is a CTAU(i)-by-CTAU(i)
C     upper triangular matrix.
C
C     For JOBOBS = 'O', the (N-NOBSV)-by-(N-NOBSV) regular pencil
C     Ano - lambda*Eno has the form
C
C                         ( Afno - lambda*Efno         *          )
C      Ano - lambda*Eno = (                                       ) ,
C                         (        0           Aino - lambda*Eino )
C
C     where:
C       1) the NIUOBS-by-NIUOBS regular pencil Aino - lambda*Eino,
C          with Aino upper triangular and nonsingular, contains the
C          unobservable infinite eigenvalues of A - lambda*E;
C       2) the (N-NOBSV-NIUOBS)-by-(N-NOBSV-NIUOBS) regular pencil
C          Afno - lambda*Efno, with Efno upper triangular and
C          nonsingular, contains the unobservable finite
C          eigenvalues of A - lambda*E.
C
C     Note: The significance of the two diagonal blocks can be
C           interchanged by calling the routine with the
C           arguments A and E interchanged. In this case,
C           Aino - lambda*Eino contains the unobservable zero
C           eigenvalues of A - lambda*E, while Afno - lambda*Efno
C           contains the unobservable nonzero finite and infinite
C           eigenvalues of A - lambda*E.
C
C     For JOBOBS = 'F', the pencil Ano - lambda*Eno has the form
C
C        Ano - lambda*Eno = Afno - lambda*Efno ,
C
C     where the regular pencil Afno - lambda*Efno, with Efno
C     upper triangular and nonsingular, contains the unobservable
C     finite eigenvalues of A - lambda*E.
C
C     For JOBOBS = 'I', the pencil Ano - lambda*Eno has the form
C
C        Ano - lambda*Eno = Aino - lambda*Eino ,
C
C     where the regular pencil Aino - lambda*Eino, with Aino
C     upper triangular and nonsingular, contains the unobservable
C     nonzero finite and infinite eigenvalues of A - lambda*E.
C
C     The left and/or right orthogonal transformations Q and Z
C     performed to reduce the system matrices can be optionally
C     accumulated.
C
C     The reduced order descriptor system (Ao-lambda*Eo,Bo,Co) has
C     the same transfer-function matrix as the original system
C     (A-lambda*E,B,C).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOBOBS   CHARACTER*1
C             = 'O':  separate both finite and infinite unobservable
C                     eigenvalues;
C             = 'F':  separate only finite unobservable eigenvalues;
C             = 'I':  separate only nonzero finite and infinite
C                     unobservable eigenvalues.
C
C     COMPQ   CHARACTER*1
C             = 'N':  do not compute Q;
C             = 'I':  Q is initialized to the unit matrix, and the
C                     orthogonal matrix Q is returned;
C             = 'U':  Q must contain an orthogonal matrix Q1 on entry,
C                     and the product Q1*Q is returned.
C
C     COMPZ   CHARACTER*1
C             = 'N':  do not compute Z;
C             = 'I':  Z is initialized to the unit matrix, and the
C                     orthogonal matrix Z is returned;
C             = 'U':  Z must contain an orthogonal matrix Z1 on entry,
C                     and the product Z1*Z is returned.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The dimension of the descriptor state vector; also the
C             order of square matrices A and E, the number of rows of
C             matrix B, and the number of columns of matrix C.  N >= 0.
C
C     M       (input) INTEGER
C             The dimension of descriptor system input vector; also the
C             number of columns of matrix B.  M >= 0.
C
C     P       (input) INTEGER
C             The dimension of descriptor system output vector; also the
C             number of rows of matrix C.  P >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the N-by-N state matrix A.
C             On exit, the leading N-by-N part of this array contains
C             the transformed state matrix Q'*A*Z,
C
C                                ( Ano  *  )
C                       Q'*A*Z = (         ) ,
C                                ( 0    Ao )
C
C             where Ao is NOBSV-by-NOBSV and Ano is
C             (N-NOBSV)-by-(N-NOBSV).
C             If JOBOBS = 'F', the matrix ( Ao ) is in the observability
C                                         ( Co )
C             staircase form (3).
C             If JOBOBS = 'O' or 'I', the submatrix Ao is upper
C             triangular.
C             If JOBOBS = 'O', the submatrix Ano has the form
C
C                             ( Afno   *  )
C                       Ano = (           ) ,
C                             (  0   Aino )
C
C             where the NIUOBS-by-NIUOBS matrix Aino is nonsingular and
C             upper triangular.
C             If JOBOBS = 'I', Ano is nonsingular and upper triangular.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     E       (input/output) DOUBLE PRECISION array, dimension (LDE,N)
C             On entry, the leading N-by-N part of this array must
C             contain the N-by-N descriptor matrix E.
C             On exit, the leading N-by-N part of this array contains
C             the transformed state matrix Q'*E*Z,
C
C                                ( Eno  *  )
C                       Q'*E*Z = (         ) ,
C                                ( 0    Eo )
C
C             where Eo is NOBSV-by-NOBSV and Eno is
C             (N-NOBSV)-by-(N-NOBSV).
C             If JOBOBS = 'O' or 'I', the matrix ( Eo ) is in the
C                                                ( Co )
C             observability staircase form (1).
C             If JOBOBS = 'F', the submatrix Eo is upper triangular.
C             If JOBOBS = 'O', the Eno matrix has the form
C
C                             ( Efno   *  )
C                       Eno = (           ) ,
C                             (  0   Eino )
C
C             where the NIUOBS-by-NIUOBS matrix Eino is nilpotent
C             and the (N-NOBSV-NIUOBS)-by-(N-NOBSV-NIUOBS) matrix Efno
C             is nonsingular and upper triangular.
C             If JOBOBS = 'F', Eno is nonsingular and upper triangular.
C
C     LDE     INTEGER
C             The leading dimension of array E.  LDE >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension
C             (LDB,MAX(M,P))
C             On entry, the leading N-by-M part of this array must
C             contain the N-by-M input matrix B.
C             On exit, the leading N-by-M part of this array contains
C             the transformed input matrix Q'*B.
C
C     LDB     INTEGER
C             The leading dimension of array B.
C             LDB >= MAX(1,N) if M > 0 or LDB >= 1 if M = 0.
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the state/output matrix C.
C             On exit, the leading P-by-N part of this array contains
C             the transformed matrix
C
C                     C*Z = (  0   Co ) ,
C
C             where Co is P-by-NOBSV.
C             If JOBOBS = 'O' or 'I', the matrix ( Eo ) is in the
C                                                ( Co )
C             observability staircase form (1).
C             If JOBOBS = 'F', the matrix ( Ao ) is in the observability
C                                         ( Co )
C             staircase form (3).
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,M,P).
C
C     Q       (input/output) DOUBLE PRECISION array, dimension (LDQ,N)
C             If COMPQ = 'N': Q is not referenced.
C             If COMPQ = 'I': on entry, Q need not be set;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix Q,
C                             where Q' is the product of transformations
C                             which are applied to A, E, and B on
C                             the left.
C             If COMPQ = 'U': on entry, the leading N-by-N part of this
C                             array must contain an orthogonal matrix
C                             Qc;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix
C                             Qc*Q.
C
C     LDQ     INTEGER
C             The leading dimension of array Q.
C             LDQ >= 1,        if COMPQ = 'N';
C             LDQ >= MAX(1,N), if COMPQ = 'U' or 'I'.
C
C     Z       (input/output) DOUBLE PRECISION array, dimension (LDZ,N)
C             If COMPZ = 'N': Z is not referenced.
C             If COMPZ = 'I': on entry, Z need not be set;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix Z,
C                             which is the product of transformations
C                             applied to A, E, and C on the right.
C             If COMPZ = 'U': on entry, the leading N-by-N part of this
C                             array must contain an orthogonal matrix
C                             Zc;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix
C                             Zc*Z.
C
C     LDZ     INTEGER
C             The leading dimension of array Z.
C             LDZ >= 1,        if COMPZ = 'N';
C             LDZ >= MAX(1,N), if COMPZ = 'U' or 'I'.
C
C     NOBSV   (output) INTEGER
C             The order of the reduced matrices Ao and Eo, and the
C             number of columns of reduced matrix Co; also the order of
C             observable part of the pair (C, A-lambda*E).
C
C     NIUOBS  (output) INTEGER
C             For JOBOBS = 'O', the order of the reduced matrices
C             Aino and Eino; also the number of unobservable
C             infinite eigenvalues of the pencil A - lambda*E.
C             For JOBOBS = 'F' or 'I', NIUOBS has no significance
C             and is set to zero.
C
C     NLBLCK  (output) INTEGER
C             For JOBOBS = 'O' or 'I', the number k, of full column rank
C                    _
C             blocks Ei-1,i in the staircase form of the pencil
C             (Eo-lambda*Ao) (see (1) and (2)).
C             (    Co      )
C             For JOBOBS = 'F', the number k, of full column rank blocks
C             _
C             Ai-1,i in the staircase form of the pencil (Ao-lambda*Eo)
C                                                        (     Co     )
C             (see (3) and (4)).
C
C     CTAU    (output) INTEGER array, dimension (N)
C             CTAU(i), for i = 1, ..., NLBLCK, is the column dimension
C                                           _         _
C             of the full column rank block Ei-1,i or Ai-1,i in the
C             staircase form (1) or (3) for JOBOBS = 'O' or 'I', or
C             for JOBOBS = 'F', respectively.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used in rank determinations when
C             transforming (A'-lambda*E',C')'. If the user sets TOL > 0,
C             then the given value of TOL is used as a lower bound for
C             reciprocal condition numbers in rank determinations; a
C             (sub)matrix whose estimated condition number is less than
C             1/TOL is considered to be of full rank.  If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance,
C             defined by  TOLDEF = N*N*EPS,  is used instead, where EPS
C             is the machine precision (see LAPACK Library routine
C             DLAMCH).  TOL < 1.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (P)
C
C     DWORK   DOUBLE PRECISION array, dimension MAX(N,2*P)
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The subroutine is based on the dual of the reduction
C     algorithms of [1].
C
C     REFERENCES
C
C     [1] A. Varga
C         Computation of Irreducible Generalized State-Space
C         Realizations.
C         Kybernetika, vol. 26, pp. 89-106, 1990.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically backward stable and requires
C     0( N**3 )  floating point operations.
C
C     FURTHER COMMENTS
C
C     If the system matrices A, E and C are badly scaled, it is
C     generally recommendable to scale them with the SLICOT routine
C     TG01AD, before calling TG01ID.
C
C     CONTRIBUTOR
C
C     C. Oara, University "Politehnica" Bucharest.
C     A. Varga, German Aerospace Center, DLR Oberpfaffenhofen.
C     March 1999. Based on the RASP routine RPDSCF.
C
C     REVISIONS
C
C     July 1999, V. Sima, Research Institute for Informatics, Bucharest.
C     May 2003, March 2004, V. Sima.
C
C     KEYWORDS
C
C     Observability, minimal realization, orthogonal canonical form,
C     orthogonal transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER          COMPQ, COMPZ, JOBOBS
      INTEGER            INFO, LDA, LDB, LDC, LDE, LDQ, LDZ,
     $                   M, N, NIUOBS, NLBLCK, NOBSV, P
      DOUBLE PRECISION   TOL
C     .. Array Arguments ..
      INTEGER            CTAU( * ), IWORK( * )
      DOUBLE PRECISION   A( LDA, * ), B( LDB, * ), C( LDC, *  ),
     $                   DWORK( * ), E( LDE, * ), Q( LDQ, * ),
     $                   Z( LDZ, * )
C     .. Local Scalars ..
      CHARACTER          JOBQ, JOBZ
      LOGICAL            FINOBS, ILQ, ILZ, INFOBS
      INTEGER            I, ICOMPQ, ICOMPZ, LBA, LBE, NR
C     .. Local Arrays ..
      DOUBLE PRECISION   DUM(1)
C     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
C     .. External Subroutines ..
      EXTERNAL           AB07MD, DSWAP, MA02BD, MA02CD, TB01XD,
     $                   TG01HX, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC          MAX
C
C     .. Executable Statements ..
C
C     Decode JOBOBS.
C
      IF( LSAME( JOBOBS, 'O') ) THEN
         FINOBS = .TRUE.
         INFOBS = .TRUE.
      ELSE IF( LSAME( JOBOBS, 'F') ) THEN
         FINOBS = .TRUE.
         INFOBS = .FALSE.
      ELSE IF( LSAME( JOBOBS, 'I') ) THEN
         FINOBS = .FALSE.
         INFOBS = .TRUE.
      ELSE
         FINOBS = .FALSE.
         INFOBS = .FALSE.
      END IF
C
C     Decode COMPQ.
C
      IF( LSAME( COMPQ, 'N' ) ) THEN
         ILQ = .FALSE.
         ICOMPQ = 1
      ELSE IF( LSAME( COMPQ, 'U' ) ) THEN
         ILQ = .TRUE.
         ICOMPQ = 2
      ELSE IF( LSAME( COMPQ, 'I' ) ) THEN
         ILQ = .TRUE.
         ICOMPQ = 3
      ELSE
         ICOMPQ = 0
      END IF
C
C     Decode COMPZ.
C
      IF( LSAME( COMPZ, 'N' ) ) THEN
         ILZ = .FALSE.
         ICOMPZ = 1
      ELSE IF( LSAME( COMPZ, 'U' ) ) THEN
         ILZ = .TRUE.
         ICOMPZ = 2
      ELSE IF( LSAME( COMPZ, 'I' ) ) THEN
         ILZ = .TRUE.
         ICOMPZ = 3
      ELSE
         ICOMPZ = 0
      END IF
C
C     Test the input scalar parameters.
C
      INFO = 0
      IF( .NOT.FINOBS .AND. .NOT.INFOBS ) THEN
         INFO = -1
      ELSE IF( ICOMPQ.LE.0 ) THEN
         INFO = -2
      ELSE IF( ICOMPZ.LE.0 ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( M.LT.0 ) THEN
         INFO = -5
      ELSE IF( P.LT.0 ) THEN
         INFO = -6
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDE.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( LDB.LT.1 .OR. ( M.GT.0 .AND. LDB.LT.N ) ) THEN
         INFO = -12
      ELSE IF( LDC.LT.MAX( 1, M, P ) ) THEN
         INFO = -14
      ELSE IF( ( ILQ .AND. LDQ.LT.N ) .OR. LDQ.LT.1 ) THEN
         INFO = -16
      ELSE IF( ( ILZ .AND. LDZ.LT.N ) .OR. LDZ.LT.1 ) THEN
         INFO = -18
      ELSE IF( TOL.GE.ONE ) THEN
         INFO = -23
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'TG01ID', -INFO )
         RETURN
      END IF
C
      JOBQ = COMPQ
      JOBZ = COMPZ
C
C     Build the dual system.
C
      CALL AB07MD( 'Z', N, M, P, A, LDA, B, LDB, C, LDC, DUM, 1,
     $             INFO )
      DO 10 I = 2, N
         CALL DSWAP( I-1, E(I,1), LDE, E(1,I), 1 )
   10 CONTINUE
C
      IF( FINOBS ) THEN
C
C        Perform finite observability form reduction.
C
         CALL TG01HX( JOBZ, JOBQ, N, N, P, M, N, MAX( 0, N-1 ), A, LDA,
     $                E, LDE, B, LDB, C, LDC, Z, LDZ, Q, LDQ, NR,
     $                NLBLCK, CTAU, TOL, IWORK, DWORK, INFO )
         IF( NLBLCK.GT.1 ) THEN
            LBA = CTAU(1) + CTAU(2) - 1
         ELSE IF( NLBLCK.EQ.1 ) THEN
            LBA = CTAU(1) - 1
         ELSE
            LBA = 0
         END IF
         IF( ILQ ) JOBQ = 'U'
         IF( ILZ ) JOBZ = 'U'
         LBE = 0
      ELSE
         NR = N
         LBA = MAX( 0, N-1 )
         LBE = LBA
      END IF
C
      IF( INFOBS ) THEN
C
C        Perform infinite observability form reduction.
C
         CALL TG01HX( JOBZ, JOBQ, N, N, P, M, NR, LBA, E, LDE,
     $                A, LDA, B, LDB, C, LDC, Z, LDZ, Q, LDQ, NOBSV,
     $                NLBLCK, CTAU, TOL, IWORK, DWORK, INFO )
         IF( FINOBS ) THEN
            NIUOBS = NR - NOBSV
         ELSE
            NIUOBS = 0
         END IF
         IF( NLBLCK.GT.1 ) THEN
            LBE = CTAU(1) + CTAU(2) - 1
         ELSE IF( NLBLCK.EQ.1 ) THEN
            LBE = CTAU(1) - 1
         ELSE
            LBE = 0
         END IF
         LBA = 0
      ELSE
         NOBSV = NR
         NIUOBS = 0
      END IF
C
C     Compute the pertransposed dual system exploiting matrix shapes.
C
      LBA = MAX( LBA, NIUOBS-1, N-NOBSV-NIUOBS-1 )
      IF ( P.EQ.0 .OR. NR.EQ.0 )
     $   LBE = MAX( 0, N - 1 )
      CALL TB01XD( 'Z', N, P, M, LBA, MAX( 0, N-1 ), A, LDA, B, LDB,
     $             C, LDC, DUM, 1, INFO )
      CALL MA02CD( N, LBE, MAX( 0, N-1 ), E, LDE )
      IF( ILZ ) CALL MA02BD( 'Right', N, N, Z, LDZ )
      IF( ILQ ) CALL MA02BD( 'Right', N, N, Q, LDQ )
      RETURN
C *** Last line of TG01ID ***
      END


      subroutine eos(rho,cs,temp,kappa,mu,gamma)
!-----------------------------------------------------------------------
! Reads and interpolates the equation of state tables to give
! temperatures, opacities ***USING c_s ***
! Updated to fortran 90 PJC 22/05/2008
! Extended to opacities DHF 01/09/2009
!-----------------------------------------------------------------------

     use unitdata

      implicit none

      integer :: i,j
      real :: rho, cs,temp,kappa,mu,gamma
      real :: mT,mT1,mT2,cT,cT1,cT2,T1,T2
      real :: mmu,mmu1,mmu2,cmu,cmu1,cmu2,mu1,mu2
      real :: mkap,mkap1,mkap2,ckap,ckap1,ckap2,kap1,kap2
      real :: mgamma,mgamma1,mgamma2,cgamma,cgamma1,cgamma2,gamma1,gamma2

! Find the relevant records in the table...
! ... for rho

!      IF(t/yr> 999.0) print*, rho, eostable(1,1,1), cs, eostable(1,1,3)
         if (rho < eostable(1,1,1)) rho = 1.01*eostable(1,1,1)
         i = 1
         do
            if ((eostable(i,1,1) >= rho).or.(i == nrhopoints)) exit
            i = i + 1
         enddo

! ... and for internal energy
         if (cs < eostable(1,1,3)) cs = 1.01*eostable(1,1,3)
         j = 1
         do
            if ((eostable(i-1,j,3) >= cs).or.(j==nUpoints)) exit
            j = j + 1
         enddo
         IF(j==1) j=2

! Interpolate over the j value at i-1

         mT1 = (eostable(i-1,j-1,2) - eostable(i-1,j,2))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
         cT1 = eostable(i-1,j,2) - mT1*eostable(i-1,j,3)
         T1 = mT1*cs + cT1

         mmu1 = (eostable(i-1,j-1,4) - eostable(i-1,j,4))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
         cmu1 = eostable(i-1,j,4) - mmu1*eostable(i-1,j,3)
         mu1 = mmu1*cs + cmu1

	 mkap1 = (eostable(i-1,j-1,6) - eostable(i-1,j,6))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
	 ckap1 = eostable(i-1,j,6) - mkap1*eostable(i-1,j,3)
	 kap1 = mkap1*cs + ckap1

	 mgamma1 = (eostable(i-1,j-1,5) - eostable(i-1,j,5))/ &
              (eostable(i-1,j-1,3) - eostable(i-1,j,3))
	cgamma1 = eostable(i-1,j,5) - mgamma1*eostable(i-1,j,3)
	gamma1 = mgamma1*cs + cgamma1

! Then interpolate over the j value at i
! Update j value as necessary
         j = 1
         do
            if ((eostable(i,j,3) >= cs).or.(j==nUpoints)) exit
            j = j + 1
         enddo
         IF(j==1) j=2

         mT2 = (eostable(i,j-1,2) - eostable(i,j,2))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         cT2 = eostable(i,j,2) - mT2*eostable(i,j,3)
         T2 = mT2*cs + cT2

         mmu2 = (eostable(i,j-1,4) - eostable(i,j,4))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         cmu2 = eostable(i,j,4) - mmu2*eostable(i,j,3)
         mu2 = mmu2*cs + cmu2

         mkap2 = (eostable(i,j-1,6) - eostable(i,j,6))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         ckap2 = eostable(i,j,6) - mkap2*eostable(i,j,3)
         kap2 = mkap2*cs + ckap2

	 mgamma2 = (eostable(i,j-1,5) - eostable(i,j,5))/ &
              (eostable(i,j-1,3) - eostable(i,j,3))
         cgamma2 = eostable(i,j,5) - mgamma2*eostable(i,j,3)
         gamma2 = mgamma2*cs + cgamma2

! Finally interpolate over i at the fractional j value
         mT = (T2 - T1) / (eostable(i,1,1)-eostable(i-1,1,1))
         cT = T2 - mT*eostable(i,1,1)
         temp = mT*rho + cT

         mmu = (mu2 - mu1) / (eostable(i,1,1)-eostable(i-1,1,1))
         cmu = mu2 - mmu*eostable(i,1,1)
         mu = mmu*rho + cmu

	mkap = (kap2 - kap1) / (eostable(i,1,1)-eostable(i-1,1,1))
         ckap = kap2 - mkap*eostable(i,1,1)
         kappa = mkap*rho + ckap

	mgamma = (gamma2 - gamma1) / (eostable(i,1,1)-eostable(i-1,1,1))
         cgamma = gamma2 - mgamma*eostable(i,1,1)
         gamma = mgamma*rho + cgamma

         ! Edit - remove opacity gap (Armitage et al 2001)
         kappa = 0.02*temp**0.8
      end subroutine eos


      double complex function a6treea(j1,j2,j3,j4,j5,j6,za,zb)
      implicit none
c---  DKS Eq. 2.8 (multiplied by a factor of (-i))
      include 'constants.f'
      include 'sprods_com.f'
      include 'zprods_decl.f'
      integer j1,j2,j3,j4,j5,j6
      integer i1,i2,i3,i4
      double complex z2
      double precision t
      z2(i1,i2,i3,i4)=za(i1,i2)*zb(i2,i4)+za(i1,i3)*zb(i3,i4)
      a6treea=za(j1,j3)*zb(j2,j5)*z2(j6,j2,j5,j4)
     . /(s(j3,j4)*s(j5,j6)*t(j1,j3,j4))

      return
      end

      double complex function a6treeb(j1,j2,j3,j4,j5,j6,za,zb)
      implicit none
c---  DKS Eq. 2.9 (multiplied by a factor of (-i))
      include 'constants.f'
      include 'sprods_com.f'
      include 'zprods_decl.f'
      integer j1,j2,j3,j4,j5,j6
      integer i1,i2,i3,i4
      double complex z2
      z2(i1,i2,i3,i4)=za(i1,i2)*zb(i2,i4)+za(i1,i3)*zb(i3,i4)

      a6treeb=(za(j1,j3)*zb(j2,j5)*z2(j6,j2,j5,j4)
     .        +zb(j2,j4)*za(j1,j6)*z2(j3,j1,j6,j5)
     . )/(s(j1,j2)*s(j3,j4)*s(j5,j6))

      return
      end

      double complex function a6trees(j1,j2,j3,j4,j5,j6,za,zb)
      implicit none
      include 'constants.f'
      include 'zprods_decl.f'
      integer j1,j2,j3,j4,j5,j6
      double complex a6treea
      a6trees=a6treea(j1,j2,j3,j4,j5,j6,za,zb)
     .       +a6treea(j1,j2,j6,j5,j4,j3,za,zb)
      return
      end


program main
    use ops
    use fstack
    use strings
    type(fs_stack) :: mainstack
    character(len = 1000) :: input
    character(len=15) :: numbers = "1234567890."
    character(len = 5) :: prompt = ">>>"
    character(len = 30),  dimension(1000) :: args
    character :: delims
    real :: tempreal, tempnum
    integer :: nargs, var, sizecounter, i = 0
    mainstack = fs_create_stack(5)
    sizecounter = 5
    call fs_push(mainstack, 0.0)
    call fs_push(mainstack, 0.0)
    do ! repeat the calculator until the user exits
        write(* ,'(A)', advance="no") prompt
        read(*,'(A)') input ! Read user input
        delims = ' '
        call parse(input, delims, args, nargs) ! Split user imput into an array
        do var = 1, nargs
            i = 1
            if (((verify(trim(args(var)), numbers)) == 0)) then ! Check if array(var) contains only numbers
                call fs_realloc_stack(mainstack, sizecounter + 1) ! If yes, push that number onto the stack
                sizecounter = sizecounter + 1
                read(args(var), *) tempnum
                call fs_push(mainstack, real(tempnum))
                i = 0
            else if (mainstack%point > 1) then ! If no, and we have the opertunity
                select case (trim(args(var))) ! Check what operator the user may have entered
                    case ("*")
                        call ops_mul(mainstack)
                        i = 0
                    case ('+')
                        call ops_add(mainstack)
                        i = 0
                    case ('-')
                        call ops_sub(mainstack)
                        i = 0
                    case ('/')
                        call ops_div(mainstack)
                        i = 0
                    case ('^')
                        call ops_pow(mainstack)
                        i = 0
                    case ('%')
                        call ops_mod(mainstack)
                        i = 0
                    case ('#')
                        call ops_inv_pow(mainstack)
                        i = 0
                    case('.sin')
                        call ops_sin(mainstack)
                        i = 0
                    case('.cos')
                        call ops_cos(mainstack)
                        i = 0
                    case ('.tan')
                        call ops_tan(mainstack)
                        i = 0
                    case ('.rtan')
                        call ops_rtan(mainstack)
                        i = 0
                    case ('.rsin')
                        call ops_rsin(mainstack)
                        i = 0
                    case ('.rcos')
                        call ops_rcos(mainstack)
                        i = 0
                    case ('.atan')
                        call ops_atan(mainstack)
                        i = 0
                    case ('.asin')
                        call ops_asin(mainstack)
                        i = 0
                    case ('.acos')
                        call ops_acos(mainstack)
                        i = 0
                    case ('.ratan')
                        call ops_atanr(mainstack)
                        i = 0
                    case ('.rasin')
                        call ops_asinr(mainstack)
                        i = 0
                    case ('.racos')
                        call ops_acosr(mainstack)
                        i = 0
                    case ('.loge')
                        call ops_loge(mainstack)
                        i = 0
                    case ('.log10')
                        call ops_log10(mainstack)
                        i = 0
                    case ('.log')
                        call ops_log(mainstack)
                        i = 0
                    case default ! If no operator was entered, set i to something other than 0
                        i = 100
                end select
            end if
            if (i /= 0) then ! If i is not zero, check if the user imputted a command
                select case(trim(args(var)))
                    case ('pp')
                        print *, "PRINT : ", mainstack%point
                    case ('p')
                        call fs_peek(mainstack, tempreal)
                        print *, "PRINT : ",  tempreal
                    case ('r')
                        call fs_reset_stack(mainstack)
                        call fs_realloc_stack(mainstack, 5)
                        sizecounter = 2
                    case ('q')
                        stop
                    case ('help')
                        print *, "PRINT : Reverse Poland notation calculator."
                        print *, "PRINT : Avalible ops: ", " + - * / ^ # .sin .cos .tan .rsin .rcos .rtan .asin .acos .atan"
                        print *, "PRINT : .rasin .racos .ratan .loge .log10 .log"
                        print *, "PRINT : Avalible commands:", " p (print top of stack) r (reset stack) q (quit) pp (print pointer)"
                    case ('pop')
                        call fs_pop(mainstack, tempreal)
                    case ('.pi')
                        call fs_push(mainstack, real(4.D0*datan(1.D0)))
                    case ('.tau')
                        call fs_push(mainstack, real(4.D0 * datan(1.D0) * 2.D0))
                    case ('.e')
                        call fs_push(mainstack,  exp(1.0))
                    case ('.gr')
                        call fs_push(mainstack, sind(54.0) * 2)
                    case default ! If no, we do not know what they inputted, so we tell them that.
                        print *, "? (", args(var), ") unknown"
                end select
            end if
        end do
    end do
end program main