C FISHPAK4 FROM PORTLIB                                  12/30/83
      SUBROUTINE HWSCYL (A,B,M,MBDCND,BDA,BDB,C,D,N,NBDCND,BDC,BDD,
     1                   ELMBDA,F,IDIMF,PERTRB,IERROR,W)
C
C
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C     *                                                               *
C     *                        F I S H P A K                          *
C     *                                                               *
C     *                                                               *
C     *     A PACKAGE OF FORTRAN SUBPROGRAMS FOR THE SOLUTION OF      *
C     *                                                               *
C     *      SEPARABLE ELLIPTIC PARTIAL DIFFERENTIAL EQUATIONS        *
C     *                                                               *
C     *                  (VERSION 3.1 , OCTOBER 1980)                  *
C     *                                                               *
C     *                             BY                                *
C     *                                                               *
C     *        JOHN ADAMS, PAUL SWARZTRAUBER AND ROLAND SWEET         *
C     *                                                               *
C     *                             OF                                *
C     *                                                               *
C     *         THE NATIONAL CENTER FOR ATMOSPHERIC RESEARCH          *
C     *                                                               *
C     *                BOULDER, COLORADO  (80307)  U.S.A.             *
C     *                                                               *
C     *                   WHICH IS SPONSORED BY                       *
C     *                                                               *
C     *              THE NATIONAL SCIENCE FOUNDATION                  *
C     *                                                               *
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C
C
C     * * * * * * * * *  PURPOSE    * * * * * * * * * * * * * * * * * *
C
C
C     SUBROUTINE HWSCYL SOLVES A FINITE DIFFERENCE APPROXIMATION TO THE
C     HELMHOLTZ EQUATION IN CYLINDRICAL COORDINATES:
C
C          (1/R)(D/DR)(R(DU/DR)) + (D/DZ)(DU/DZ)
C
C                                + (LAMBDA/R**2)U = F(R,Z)
C
C     THIS MODIFIED HELMHOLTZ EQUATION RESULTS FROM THE FOURIER
C     TRANSFORM OF THE THREE-DIMENSIONAL POISSON EQUATION.
C
C     * * * * * * * *    PARAMETER DESCRIPTION     * * * * * * * * * *
C
C             * * * * * *   ON INPUT    * * * * * *
C
C     A,B
C       THE RANGE OF R, I.E., A .LE. R .LE. B.  A MUST BE LESS THAN B
C       AND A MUST BE NON-NEGATIVE.
C
C     M
C       THE NUMBER OF PANELS INTO WHICH THE INTERVAL (A,B) IS
C       SUBDIVIDED.  HENCE, THERE WILL BE M+1 GRID POINTS IN THE
C       R-DIRECTION GIVEN BY R(I) = A+(I-1)DR, FOR I = 1,2,...,M+1,
C       WHERE DR = (B-A)/M IS THE PANEL WIDTH. M MUST BE GREATER THAN 3.
C
C     MBDCND
C       INDICATES THE TYPE OF BOUNDARY CONDITIONS AT R = A AND R = B.
C
C       = 1  IF THE SOLUTION IS SPECIFIED AT R = A AND R = B.
C       = 2  IF THE SOLUTION IS SPECIFIED AT R = A AND THE DERIVATIVE OF
C            THE SOLUTION WITH RESPECT TO R IS SPECIFIED AT R = B.
C       = 3  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO R IS
C            SPECIFIED AT R = A (SEE NOTE BELOW) AND R = B.
C       = 4  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO R IS
C            SPECIFIED AT R = A (SEE NOTE BELOW) AND THE SOLUTION IS
C            SPECIFIED AT R = B.
C       = 5  IF THE SOLUTION IS UNSPECIFIED AT R = A = 0 AND THE
C            SOLUTION IS SPECIFIED AT R = B.
C       = 6  IF THE SOLUTION IS UNSPECIFIED AT R = A = 0 AND THE
C            DERIVATIVE OF THE SOLUTION WITH RESPECT TO R IS SPECIFIED
C            AT R = B.
C
C       NOTE:  IF A = 0, DO NOT USE MBDCND = 3 OR 4, BUT INSTEAD USE
C              MBDCND = 1,2,5, OR 6  .
C
C     BDA
C       A ONE-DIMENSIONAL ARRAY OF LENGTH N+1 THAT SPECIFIES THE VALUES
C       OF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO R AT R = A.
C       WHEN MBDCND = 3 OR 4,
C
C            BDA(J) = (D/DR)U(A,Z(J)), J = 1,2,...,N+1  .
C
C       WHEN MBDCND HAS ANY OTHER VALUE, BDA IS A DUMMY VARIABLE.
C
C     BDB
C       A ONE-DIMENSIONAL ARRAY OF LENGTH N+1 THAT SPECIFIES THE VALUES
C       OF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO R AT R = B.
C       WHEN MBDCND = 2,3, OR 6,
C
C            BDB(J) = (D/DR)U(B,Z(J)), J = 1,2,...,N+1  .
C
C       WHEN MBDCND HAS ANY OTHER VALUE, BDB IS A DUMMY VARIABLE.
C
C     C,D
C       THE RANGE OF Z, I.E., C .LE. Z .LE. D.  C MUST BE LESS THAN D.
C
C     N
C       THE NUMBER OF PANELS INTO WHICH THE INTERVAL (C,D) IS
C       SUBDIVIDED.  HENCE, THERE WILL BE N+1 GRID POINTS IN THE
C       Z-DIRECTION GIVEN BY Z(J) = C+(J-1)DZ, FOR J = 1,2,...,N+1,
C       WHERE DZ = (D-C)/N IS THE PANEL WIDTH. N MUST BE GREATER THAN 3.
C
C     NBDCND
C       INDICATES THE TYPE OF BOUNDARY CONDITIONS AT Z = C AND Z = D.
C
C       = 0  IF THE SOLUTION IS PERIODIC IN Z, I.E., U(I,1) = U(I,N+1).
C       = 1  IF THE SOLUTION IS SPECIFIED AT Z = C AND Z = D.
C       = 2  IF THE SOLUTION IS SPECIFIED AT Z = C AND THE DERIVATIVE OF
C            THE SOLUTION WITH RESPECT TO Z IS SPECIFIED AT Z = D.
C       = 3  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO Z IS
C            SPECIFIED AT Z = C AND Z = D.
C       = 4  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO Z IS
C            SPECIFIED AT Z = C AND THE SOLUTION IS SPECIFIED AT Z = D.
C
C     BDC
C       A ONE-DIMENSIONAL ARRAY OF LENGTH M+1 THAT SPECIFIES THE VALUES
C       OF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO Z AT Z = C.
C       WHEN NBDCND = 3 OR 4,
C
C            BDC(I) = (D/DZ)U(R(I),C), I = 1,2,...,M+1  .
C
C       WHEN NBDCND HAS ANY OTHER VALUE, BDC IS A DUMMY VARIABLE.
C
C     BDD
C       A ONE-DIMENSIONAL ARRAY OF LENGTH M+1 THAT SPECIFIES THE VALUES
C       OF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO Z AT Z = D.
C       WHEN NBDCND = 2 OR 3,
C
C            BDD(I) = (D/DZ)U(R(I),D), I = 1,2,...,M+1  .
C
C       WHEN NBDCND HAS ANY OTHER VALUE, BDD IS A DUMMY VARIABLE.
C
C     ELMBDA
C       THE CONSTANT LAMBDA IN THE HELMHOLTZ EQUATION.  IF
C       LAMBDA .GT. 0, A SOLUTION MAY NOT EXIST.  HOWEVER, HWSCYL WILL
C       ATTEMPT TO FIND A SOLUTION.  LAMBDA MUST BE ZERO WHEN
C       MBDCND = 5 OR 6  .
C
C     F
C       A TWO-DIMENSIONAL ARRAY THAT SPECIFIES THE VALUES OF THE RIGHT
C       SIDE OF THE HELMHOLTZ EQUATION AND BOUNDARY DATA (IF ANY).  FOR
C       I = 2,3,...,M AND J = 2,3,...,N
C
C            F(I,J) = F(R(I),Z(J)).
C
C       ON THE BOUNDARIES F IS DEFINED BY
C
C            MBDCND   F(1,J)            F(M+1,J)
C            ------   ---------         ---------
C
C              1      U(A,Z(J))         U(B,Z(J))
C              2      U(A,Z(J))         F(B,Z(J))
C              3      F(A,Z(J))         F(B,Z(J))   J = 1,2,...,N+1
C              4      F(A,Z(J))         U(B,Z(J))
C              5      F(0,Z(J))         U(B,Z(J))
C              6      F(0,Z(J))         F(B,Z(J))
C
C            NBDCND   F(I,1)            F(I,N+1)
C            ------   ---------         ---------
C
C              0      F(R(I),C)         F(R(I),C)
C              1      U(R(I),C)         U(R(I),D)
C              2      U(R(I),C)         F(R(I),D)   I = 1,2,...,M+1
C              3      F(R(I),C)         F(R(I),D)
C              4      F(R(I),C)         U(R(I),D)
C
C       F MUST BE DIMENSIONED AT LEAST (M+1)*(N+1).
C
C       NOTE
C
C       IF THE TABLE CALLS FOR BOTH THE SOLUTION U AND THE RIGHT SIDE F
C       AT  A CORNER THEN THE SOLUTION MUST BE SPECIFIED.
C
C     IDIMF
C       THE ROW (OR FIRST) DIMENSION OF THE ARRAY F AS IT APPEARS IN THE
C       PROGRAM CALLING HWSCYL.  THIS PARAMETER IS USED TO SPECIFY THE
C       VARIABLE DIMENSION OF F.  IDIMF MUST BE AT LEAST M+1  .
C
C     W
C       A ONE-DIMENSIONAL ARRAY THAT MUST BE PROVIDED BY THE USER FOR
C       WORK SPACE.  W MAY REQUIRE UP TO 4*(N+1) +
C       (13 + INT(LOG2(N+1)))*(M+1) LOCATIONS.  THE ACTUAL NUMBER OF
C       LOCATIONS USED IS COMPUTED BY HWSCYL AND IS RETURNED IN LOCATION
C       W(1).
C
C
C             * * * * * *   ON OUTPUT     * * * * * *
C
C     F
C       CONTAINS THE SOLUTION U(I,J) OF THE FINITE DIFFERENCE
C       APPROXIMATION FOR THE GRID POINT (R(I),Z(J)), I = 1,2,...,M+1,
C       J = 1,2,...,N+1  .
C
C     PERTRB
C       IF ONE SPECIFIES A COMBINATION OF PERIODIC, DERIVATIVE, AND
C       UNSPECIFIED BOUNDARY CONDITIONS FOR A POISSON EQUATION
C       (LAMBDA = 0), A SOLUTION MAY NOT EXIST.  PERTRB IS A CONSTANT,
C       CALCULATED AND SUBTRACTED FROM F, WHICH ENSURES THAT A SOLUTION
C       EXISTS.  HWSCYL THEN COMPUTES THIS SOLUTION, WHICH IS A LEAST
C       SQUARES SOLUTION TO THE ORIGINAL APPROXIMATION.  THIS SOLUTION
C       PLUS ANY CONSTANT IS ALSO A SOLUTION.  HENCE, THE SOLUTION IS
C       NOT UNIQUE.  THE VALUE OF PERTRB SHOULD BE SMALL COMPARED TO THE
C       RIGHT SIDE F.  OTHERWISE, A SOLUTION IS OBTAINED TO AN
C       ESSENTIALLY DIFFERENT PROBLEM.  THIS COMPARISON SHOULD ALWAYS
C       BE MADE TO INSURE THAT A MEANINGFUL SOLUTION HAS BEEN OBTAINED.
C
C     IERROR
C       AN ERROR FLAG WHICH INDICATES INVALID INPUT PARAMETERS.  EXCEPT
C       FOR NUMBERS 0 AND 11, A SOLUTION IS NOT ATTEMPTED.
C
C       =  0  NO ERROR.
C       =  1  A .LT. 0  .
C       =  2  A .GE. B.
C       =  3  MBDCND .LT. 1 OR MBDCND .GT. 6  .
C       =  4  C .GE. D.
C       =  5  N .LE. 3
C       =  6  NBDCND .LT. 0 OR NBDCND .GT. 4  .
C       =  7  A = 0, MBDCND = 3 OR 4  .
C       =  8  A .GT. 0, MBDCND .GE. 5  .
C       =  9  A = 0, LAMBDA .NE. 0, MBDCND .GE. 5  .
C       = 10  IDIMF .LT. M+1  .
C       = 11  LAMBDA .GT. 0  .
C       = 12  M .LE. 3
C
C       SINCE THIS IS THE ONLY MEANS OF INDICATING A POSSIBLY INCORRECT
C       CALL TO HWSCYL, THE USER SHOULD TEST IERROR AFTER THE CALL.
C
C     W
C       W(1) CONTAINS THE REQUIRED LENGTH OF W.
C
C
C     * * * * * * *   PROGRAM SPECIFICATIONS    * * * * * * * * * * * *
C
C     DIMENSION OF   BDA(N+1),BDB(N+1),BDC(M+1),BDD(M+1),F(IDIMF,N+1),
C     ARGUMENTS      W(SEE ARGUMENT LIST)
C
C     LATEST         JUNE 1, 1976
C     REVISION
C
C     SUBPROGRAMS    HWSCYL,GENBUN,POISD2,POISN2,POISP2,COSGEN,MERGE,
C     REQUIRED      TRIX,TRI3,PIMACH
C
C     SPECIAL        NONE
C     CONDITIONS
C
C     COMMON         NONE
C     BLOCKS
C
C     I/O            NONE
C
C     PRECISION      SINGLE
C
C     SPECIALIST     ROLAND SWEET
C
C     LANGUAGE       FORTRAN
C
C     HISTORY        STANDARDIZED SEPTEMBER 1, 1973
C                    REVISED APRIL 1, 1976
C
C     ALGORITHM      THE ROUTINE DEFINES THE FINITE DIFFERENCE
C                    EQUATIONS, INCORPORATES BOUNDARY DATA, AND ADJUSTS
C                    THE RIGHT SIDE OF SINGULAR SYSTEMS AND THEN CALLS
C                    GENBUN TO SOLVE THE SYSTEM.
C
C     SPACE          5818(DECIMAL) = 13272(OCTAL) LOCATIONS ON THE NCAR
C     REQUIRED       CONTROL DATA 7600
C
C     TIMING AND        THE EXECUTION TIME T ON THE NCAR CONTROL DATA
C     ACCURACY       7600 FOR SUBROUTINE HWSCYL IS ROUGHLY PROPORTIONAL
C                    TO M*N*LOG2(N), BUT ALSO DEPENDS ON THE INPUT
C                    PARAMETERS NBDCND AND MBDCND.  SOME TYPICAL VALUES
C                    ARE LISTED IN THE TABLE BELOW.
C                       THE SOLUTION PROCESS EMPLOYED RESULTS IN A LOSS
C                    OF NO MORE THAN THREE SIGNIFICANT DIGITS FOR N AND
C                    M AS LARGE AS 64.  MORE DETAILED INFORMATION ABOUT
C                    ACCURACY CAN BE FOUND IN THE DOCUMENTATION FOR
C                    SUBROUTINE GENBUN WHICH IS THE ROUTINE THAT
C                    SOLVES THE FINITE DIFFERENCE EQUATIONS.
C
C
C                       M(=N)    MBDCND    NBDCND    T(MSECS)
C                       -----    ------    ------    --------
C
C                        32        1         0          31
C                        32        1         1          23
C                        32        3         3          36
C                        64        1         0         128
C                        64        1         1          96
C                        64        3         3         142
C
C     PORTABILITY    AMERICAN NATIONAL STANDARDS INSTITUTE FORTRAN.
C                    ALL MACHINE DEPENDENT CONSTANTS ARE LOCATED IN THE
C                    FUNCTION PIMACH.
C
C     REQUIRED       COS
C     RESIDENT
C     ROUTINES
C
C     REFERENCE      SWARZTRAUBER,P. AND R. SWEET, 'EFFICIENT FORTRAN
C                    SUBPROGRAMS FOR THE SOLUTION OF ELLIPTIC EQUATIONS'
C                    NCAR TN/IA-109, JULY, 1975, 138 PP.
C
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C
C
      DIMENSION       F(IDIMF,1)
      DIMENSION       BDA(1)     ,BDB(1)     ,BDC(1)     ,BDD(1)     ,
     1                W(1)
C
C     CHECK FOR INVALID PARAMETERS.
C
      IERROR = 0
      IF (A .LT. 0.) IERROR = 1
      IF (A .GE. B) IERROR = 2
      IF (MBDCND.LE.0 .OR. MBDCND.GE.7) IERROR = 3
      IF (C .GE. D) IERROR = 4
      IF (N .LE. 3) IERROR = 5
      IF (NBDCND.LE.-1 .OR. NBDCND.GE.5) IERROR = 6
      IF (A.EQ.0. .AND. (MBDCND.EQ.3 .OR. MBDCND.EQ.4)) IERROR = 7
      IF (A.GT.0. .AND. MBDCND.GE.5) IERROR = 8
      IF (A.EQ.0. .AND. ELMBDA.NE.0. .AND. MBDCND.GE.5) IERROR = 9
      IF (IDIMF .LT. M+1) IERROR = 10
      IF (M .LE. 3) IERROR = 12
      IF (IERROR .NE. 0) RETURN
      MP1 = M+1
      DELTAR = (B-A)/FLOAT(M)
      DLRBY2 = DELTAR/2.
      DLRSQ = DELTAR**2
      NP1 = N+1
      DELTHT = (D-C)/FLOAT(N)
      DLTHSQ = DELTHT**2
      NP = NBDCND+1
C
C     DEFINE RANGE OF INDICES I AND J FOR UNKNOWNS U(I,J).
C
      MSTART = 2
      MSTOP = M
      GO TO (104,103,102,101,101,102),MBDCND
  101 MSTART = 1
      GO TO 104
  102 MSTART = 1
  103 MSTOP = MP1
  104 MUNK = MSTOP-MSTART+1
      NSTART = 1
      NSTOP = N
      GO TO (108,105,106,107,108),NP
  105 NSTART = 2
      GO TO 108
  106 NSTART = 2
  107 NSTOP = NP1
  108 NUNK = NSTOP-NSTART+1
C
C     DEFINE A,B,C COEFFICIENTS IN W-ARRAY.
C
      ID2 = MUNK
      ID3 = ID2+MUNK
      ID4 = ID3+MUNK
      ID5 = ID4+MUNK
      ID6 = ID5+MUNK
      ISTART = 1
      A1 = 2./DLRSQ
      IJ = 0
      IF (MBDCND.EQ.3 .OR. MBDCND.EQ.4) IJ = 1
      IF (MBDCND .LE. 4) GO TO 109
      W(1) = 0.
      W(ID2+1) = -2.*A1
      W(ID3+1) = 2.*A1
      ISTART = 2
      IJ = 1
  109 DO 110 I=ISTART,MUNK
         R = A+FLOAT(I-IJ)*DELTAR
         J = ID5+I
         W(J) = R
         J = ID6+I
         W(J) = 1./R**2
         W(I) = (R-DLRBY2)/(R*DLRSQ)
         J = ID3+I
         W(J) = (R+DLRBY2)/(R*DLRSQ)
         K = ID6+I
         J = ID2+I
         W(J) = -A1+ELMBDA*W(K)
  110 CONTINUE
      GO TO (114,111,112,113,114,112),MBDCND
  111 W(ID2) = A1
      GO TO 114
  112 W(ID2) = A1
  113 W(ID3+1) = A1*FLOAT(ISTART)
  114 CONTINUE
C
C     ENTER BOUNDARY DATA FOR R-BOUNDARIES.
C
      GO TO (115,115,117,117,119,119),MBDCND
  115 A1 = W(1)
      DO 116 J=NSTART,NSTOP
         F(2,J) = F(2,J)-A1*F(1,J)
  116 CONTINUE
      GO TO 119
  117 A1 = 2.*DELTAR*W(1)
      DO 118 J=NSTART,NSTOP
         F(1,J) = F(1,J)+A1*BDA(J)
  118 CONTINUE
  119 GO TO (120,122,122,120,120,122),MBDCND
  120 A1 = W(ID4)
      DO 121 J=NSTART,NSTOP
         F(M,J) = F(M,J)-A1*F(MP1,J)
  121 CONTINUE
      GO TO 124
  122 A1 = 2.*DELTAR*W(ID4)
      DO 123 J=NSTART,NSTOP
         F(MP1,J) = F(MP1,J)-A1*BDB(J)
  123 CONTINUE
C
C     ENTER BOUNDARY DATA FOR Z-BOUNDARIES.
C
  124 A1 = 1./DLTHSQ
      L = ID5-MSTART+1
      GO TO (134,125,125,127,127),NP
  125 DO 126 I=MSTART,MSTOP
         F(I,2) = F(I,2)-A1*F(I,1)
  126 CONTINUE
      GO TO 129
  127 A1 = 2./DELTHT
      DO 128 I=MSTART,MSTOP
         F(I,1) = F(I,1)+A1*BDC(I)
  128 CONTINUE
  129 A1 = 1./DLTHSQ
      GO TO (134,130,132,132,130),NP
  130 DO 131 I=MSTART,MSTOP
         F(I,N) = F(I,N)-A1*F(I,NP1)
  131 CONTINUE
      GO TO 134
  132 A1 = 2./DELTHT
      DO 133 I=MSTART,MSTOP
         F(I,NP1) = F(I,NP1)-A1*BDD(I)
  133 CONTINUE
  134 CONTINUE
C
C     ADJUST RIGHT SIDE OF SINGULAR PROBLEMS TO INSURE EXISTENCE OF A
C     SOLUTION.
C
      PERTRB = 0.
      IF (ELMBDA) 146,136,135
  135 IERROR = 11
      GO TO 146
  136 W(ID5+1) = .5*(W(ID5+2)-DLRBY2)
      GO TO (146,146,138,146,146,137),MBDCND
  137 W(ID5+1) = .5*W(ID5+1)
  138 GO TO (140,146,146,139,146),NP
  139 A2 = 2.
      GO TO 141
  140 A2 = 1.
  141 K = ID5+MUNK
      W(K) = .5*(W(K-1)+DLRBY2)
      S = 0.
      DO 143 I=MSTART,MSTOP
         S1 = 0.
         NSP1 = NSTART+1
         NSTM1 = NSTOP-1
         DO 142 J=NSP1,NSTM1
            S1 = S1+F(I,J)
  142    CONTINUE
         K = I+L
         S = S+(A2*S1+F(I,NSTART)+F(I,NSTOP))*W(K)
  143 CONTINUE
      S2 = FLOAT(M)*A+(.75+FLOAT((M-1)*(M+1)))*DLRBY2
      IF (MBDCND .EQ. 3) S2 = S2+.25*DLRBY2
      S1 = (2.+A2*FLOAT(NUNK-2))*S2
      PERTRB = S/S1
      DO 145 I=MSTART,MSTOP
         DO 144 J=NSTART,NSTOP
            F(I,J) = F(I,J)-PERTRB
  144    CONTINUE
  145 CONTINUE
  146 CONTINUE
C
C     MULTIPLY I-TH EQUATION THROUGH BY DELTHT**2 TO PUT EQUATION INTO
C     CORRECT FORM FOR SUBROUTINE GENBUN.
C
      DO 148 I=MSTART,MSTOP
         K = I-MSTART+1
         W(K) = W(K)*DLTHSQ
         J = ID2+K
         W(J) = W(J)*DLTHSQ
         J = ID3+K
         W(J) = W(J)*DLTHSQ
         DO 147 J=NSTART,NSTOP
            F(I,J) = F(I,J)*DLTHSQ
  147    CONTINUE
  148 CONTINUE
      W(1) = 0.
      W(ID4) = 0.
C
C     CALL GENBUN TO SOLVE THE SYSTEM OF EQUATIONS.
C
      CALL GENBUN (NBDCND,NUNK,1,MUNK,W(1),W(ID2+1),W(ID3+1),IDIMF,
     1             F(MSTART,NSTART),IERR1,W(ID4+1))
      W(1) = W(ID4+1)+3.*FLOAT(MUNK)
      IF (NBDCND .NE. 0) GO TO 150
      DO 149 I=MSTART,MSTOP
         F(I,NP1) = F(I,1)
  149 CONTINUE
  150 CONTINUE
      RETURN
      END


!##############################################################################
!# ****************************************************************************
!# <name> stokes </name>
!# ****************************************************************************
!#
!# <purpose>
!# This program is a simple test program for discretising the Stokes equation
!#
!#              $$- \nu Laplace(u) + \nabla p = f $$
!#              $$ \nabla \cdot u = 0$$
!#
!# on a 2D domain for a 2D function $u=(u_1,u_2)$ or
!# on a 3D domain for a 3D function $u=(u_1,u_2,u_3)$.
!#
!# A linear block system with 3 (for 2D) or 4 (for 3D) solution components
!# is set up, discretised and solved with multigrid.
!#
!# This program has a set of 2D (stokes2d_XXXX) and 3D (stokes3d_XXXX) stokes
!# examples. The naming convention is based on the one from the poisson
!# examples: stokesXd_method0_simple is the most simple stokes example using
!# a single-grid solver. All other examples use multi-grid solvers.
!# The stokesXd_method1_XXXX modules extend the stokesXd_method0_simple example
!# to more general situations, but still in the same style of a one-in-all
!# solver. The stokesXd_method2_XXXX modules decompose the different tasks
!# of the problem into different subroutines, thus bringing more structure
!# into the code and highlighting more and more features of the kernel.
!# </purpose>
!##############################################################################

program stokes

  use stokes2d_EL_QPW4P1TVDF_2D

  implicit none

  ! local variables
  character(len=SYS_STRLEN) :: slogdir,slogfile

  ! The very first thing in every application:
  ! Initialise system-wide settings:

  call sys_init()

  ! Initialise the output system.
  !
  ! Normally, we write all the output to the screen and to a file
  ! "./log/output.txt".
  ! In the case that environment variables "$logdir"/"$resultsfile" exists,
  ! we write all the output to that file. This can be used e.g. in
  ! regression tests to compare results to reference results.
  if (sys_getenv_string("LOGDIR",slogdir) .and. &
      sys_getenv_string("RESULTFILE",slogfile)) then
    call output_init (trim(slogdir)//"/"//trim(slogfile))
  else
    call output_init ("./log/output.txt")
  end if

  ! The very second thing in every program:
  ! Initialise the storage management:
  !
  ! 2.) Initialise FEAT 2.0 storage management:
  call storage_init(999, 100)

  ! Call the problem to solve.
  call output_lbrk()
  call output_line("Calculating 2D Stokes-Problem 3 - Divergence-free element")
  call output_line("---------------------------------------------------------")
  call test_stokes2d_EL_QPW4P1TVDF_2D

  ! Print out heap statistics - just to check if everything
  ! is cleaned up.
  ! This should display "Handles in use=0" and "Memory in use=0"!
  call output_lbrk()
  call storage_info(.true.)

  ! Clean up the storage management, finish
  call storage_done()

end program


      PROGRAM xmemcof
C     driver for routine memcof
      INTEGER N,M
      PARAMETER(N=1000,M=10)
      INTEGER i
      REAL pm,data(N),cof(M)
      open(7,file='SPCTRL.DAT',status='OLD')
      read(7,*) (data(i),i=1,N)
      close(7)
      call memcof(data,N,M,pm,cof)
      write(*,'(/1x,a/)') 'Coeff. for spectral estim. of SPCTRL.DAT'
      do 11 i=1,M
        write(*,'(1x,a,i2,a,f12.6)') 'a[',i,'] =',cof(i)
11    continue
      write(*,'(/1x,a,f12.6/)') 'a0 =',pm
      END

NR-Functions/Numerical Recipes- Example & Functions/Examples/xmemcof.for


************************************************************************
* This file contains various auxiliary routine for maintaining
* grid information.
************************************************************************

************************************************************************
* Average element midpoint coordinates
*
* The follwing function corrects the grid on domains where non-linear/
* curved boundary segments are used.
* When a boundary segment of the domain is a line, new boundary nodes
* are automatically positioned on the boundary. But when the boundary
* is a curve, a new boundary vertex is not automatically on the
* boundary, but it first has tobe moved to there. This is already
* performed in the refinement routine XSB0X. Unfortunately this
* procedure can lead to very anisotropic elements near the boundary,
* depending on how sharp the curved boundary is.
*
* AVEMPC now tries to reduce these effects of anisotropy. The element
* midpoint of the coarser element (which is at the same time the vertex
* where all the four finer elements meet) is taken as the average of
* the four edge midpoints that arise from natural refinement.
*
* o----------o----------o
* \          \          |
* |\         \          |
* | \      -> \         |
* |  o---------o--------o   Averaging of the element midpoint by
* | /      -> /         |   interpolation
* |/         /          |
* /          /          |
* o----------o----------o
*
* In:
*   DCORVG,
*   KVERT,
*   KADJ,
*   NEL     - Gemeotry information at level >= 2. DCORVG must contain
*             the coordinates of the vertices at level 1..current level
*
* Out:
*   DCORVG  - The coordinates of the element corners that correspond
*             to coarse grid element midpoints are corrected.
************************************************************************

      SUBROUTINE AVEMPC(DCORVG,KVERT,KADJ,NEL)

      IMPLICIT NONE

      INCLUDE 'cbasictria.inc'

C parameters

      DOUBLE PRECISION DCORVG(2,*)
      INTEGER KVERT(NNVE,*),KADJ(NNVE,*)
      INTEGER NEL

C local variables

      INTEGER IADJ3, IADJ4
      INTEGER IVT1, IVT2, IVT3, IVT4
      INTEGER IVTM, IEL
      DOUBLE PRECISION PX1,PX2,PX3,PX4,PY1,PY2,PY3,PY4,PXM,PYM,PX,PY

      DO IEL=1,NEL/4

        IADJ3=KADJ(2,IEL)
        IADJ4=KADJ(3,IEL)
        IVT1=KVERT(2,IEL)
        IVT2=KVERT(4,IEL)
        IVT3=KVERT(2,IADJ3)
        IVT4=KVERT(4,IADJ4)
        IVTM=KVERT(3,IEL)

        PX1=DCORVG(1,IVT1)
        PX2=DCORVG(1,IVT2)
        PX3=DCORVG(1,IVT3)
        PX4=DCORVG(1,IVT4)

        PY1=DCORVG(2,IVT1)
        PY2=DCORVG(2,IVT2)
        PY3=DCORVG(2,IVT3)
        PY4=DCORVG(2,IVT4)

        PXM=DCORVG(1,IVTM)
        PYM=DCORVG(2,IVTM)

        PX=0.25D0*(PX1+PX2+PX3+PX4)
        PY=0.25D0*(PY1+PY2+PY3+PY4)

        DCORVG(1,IVTM)=PX
        DCORVG(2,IVTM)=PY

      END DO

      END

************************************************************************
* Grid disturbance
*
* This routine stochastically disturbes the current grid.
* Every gridpoint is moved DIEPS percent in a - more or less random -
* direction. The grid is assumed to be uniform, the minimum cell
* size is computed with the help of NVT!
*
* In:
*  DCORVG, KNPR, NVT - as usual
*  DIEPS             - Rate of disturbing. 0.2D0 = 20%
*
* The current grid will be modified directly.
************************************************************************

      SUBROUTINE GRDIST(DCORVG,KNPR,NVT,DIEPS)

      IMPLICIT NONE

C parameters

      DOUBLE PRECISION DCORVG(2,*),DIEPS
      INTEGER KNPR(*),NVT

C local variables

      DOUBLE PRECISION H, HDIST
      INTEGER IVT

      H=1D0/(SQRT(DBLE(NVT))-1)
      HDIST=DIEPS*H

      DO IVT=1,NVT
        IF (KNPR(IVT).EQ.0) THEN
          DCORVG(1,IVT)=DCORVG(1,IVT)+DBLE((-1)**MOD(IVT,17))*HDIST
          DCORVG(2,IVT)=DCORVG(2,IVT)+DBLE((-1)**IVT)*HDIST
        ENDIF
      END DO

      END


      SUBROUTINE zbrak(fx,x1,x2,n,xb1,xb2,nb)
      INTEGER n,nb
      REAL x1,x2,xb1(nb),xb2(nb),fx
      EXTERNAL fx
      INTEGER i,nbb
      REAL dx,fc,fp,x
      nbb=0
      x=x1
      dx=(x2-x1)/n
      fp=fx(x)
      do 11 i=1,n
        x=x+dx
        fc=fx(x)
        if(fc*fp.le.0.) then
          nbb=nbb+1
          xb1(nbb)=x-dx
          xb2(nbb)=x
          if(nbb.eq.nb)goto 1
        endif
        fp=fc
11    continue
1     continue
      nb=nbb
      return
      END

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


program Metodos
  implicit none

!!!!!!!!!!!!!!!!!
  !              AUTOR: BYRON J. ENCINAS
  !
  !                    PROPÓSITO:
  !      APROXIMAR RAÍCES REALES PARA FUNCIONES
  !      MEDIANTE MÉTODOS DE BISECCIÓN Y FALSA POSICIÓN
  !
!!!!!!!!!!!!!!!!!!
  real::a,b,ET,x
  CHARACTER::m,O
  INTEGER::i
  !!son los limtes

  write(*,*)'Introduce un intervalo para realizar una busqueda de la raiz' !! I = [a,b]
 READ*,a
READ*, b
O = "b"

DO
	IF(O=="a")EXIT
	WRITE(*,*)'¿Qué método prefieres? (responde "a" o "b")' !! Eleccion del metodo

	WRITE(*,*)'        a) Bisección'

	WRITE(*,*)'        b) Falsa posición'
	READ*, m

		IF(m=="a") then
		WRITE(*,*)'Puedes introducir un Error de Truncamiento en caso 			necesario. &
		&Si no, presiona 0 y el programa se detendra en la iteracion #200'
		READ*,ET
		O="a"
		ELSE IF(m == "b") then

		WRITE(*,*)'Puedes introducir porcentaje de error, en caso 			necesario. &
		&Si no, presiona 0 y el programa se detendra en la iteracion #200'

		READ*,ET
		O="a"
		ELSE

		WRITE(*,*)'Tienes que introducir un método, trata de nuevo o salte'

		WRITE(*,*)'a) SALIR'
		WRITE(*,*)'b) OTRA VEZ'
		READ*, O
		END IF

END DO

	IF(m=="a")THEN
	call bis(a,b,ET,x)
	ELSE
	IF (m=="b")THEN
	CALL falsapos(a,b,ET,x)
	ELSE
	WRITE(*,*)'ESE NO ES UN MÉTODO'
	END IF

END IF

END PROGRAM Metodos


!==============================================================================!
  recursive subroutine Sort_Mod_2_Real_Carry_Int(a1, a2, b)
!------------------------------------------------------------------------------!
!   Quick sort two real arrays and carry one integer array along               !
!                                                                              !
!   Adapted from: https://gist.github.com/1AdAstra1  (good work Olga)          !
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  real    :: a1(:), a2(:)
  integer :: b(:)
!-----------------------------------[Locals]-----------------------------------!
  real    :: x1, x2
  integer :: i, j, n
!------------------------------------------------------------------------------!

  n = size(a1, 1)
  x1 = a1( (1+n) / 2 )
  x2 = a2( (1+n) / 2 )
  i = 1
  j = n

  do
    do while ( Math_Mod_Smaller_Real(a1(i),x1)  .or.   &
               Math_Mod_Approx_Real (a1(i),x1)  .and.  &
               Math_Mod_Smaller_Real(a2(i),x2) )
      i = i + 1
    end do
    do while ( Math_Mod_Smaller_Real(x1,a1(j))  .or.  &
               Math_Mod_Approx_Real (x1,a1(j))  .and.  &
               Math_Mod_Smaller_Real(x2,a2(j)) )
      j = j - 1
    end do
    if (i >= j) exit

    ! Swap values in a and b
    call Swap_Real(a1(i), a1(j))
    call Swap_Real(a2(i), a2(j))
    call Swap_Int (b (i), b (j))

    i = i + 1
    j = j - 1
  end do

  if (1 < i - 1) call Sort_Mod_2_Real_Carry_Int(a1(1:i-1),  &
                                                a2(1:i-1),  &
                                                b (1:i-1))
  if (j + 1 < n) call Sort_Mod_2_Real_Carry_Int(a1(j+1:n),  &
                                                a2(j+1:n),  &
                                                b (j+1:n))

  end subroutine


subroutine dc1(a, b)
real, intent(inout) :: a(:,:)
integer :: N, i, j
N = size(a)
do concurrent (i=1:N, j=1:N, i<j)
    a(i,j) = a(j,i)
end do
end subroutine

subroutine dc2(a, b)
real, intent(inout) :: a(:), b(:)
integer :: i
real :: x
x = 1.0
do concurrent (i=1:10) local(x)
    if (a(i) > 0) then
        x = sqrt(a(i))
        a(i) = a(i) - x**2
    end if
    b(i) = b(i) - a(i)
end do
print *, x
end subroutine

subroutine dc3(a, b)
real, intent(inout) :: a(:), b(:)
integer :: i
real :: x
x = 1.0
do concurrent (i=1:10) shared(i) local(x) default(none)
    if (a(i) > 0) then
        x = sqrt(a(i))
        a(i) = a(i) - x**2
    end if
    b(i) = b(i) - a(i)
end do
print *, x
end subroutine

subroutine dc4(a, b)
real, intent(inout) :: a(:), b(:)
integer :: i
real :: x
x = 1.0
do concurrent (i=1:10) default(none) local(x) shared(i)
    if (a(i) > 0) then
        x = sqrt(a(i))
        a(i) = a(i) - x**2
    end if
    b(i) = b(i) - a(i)
end do
print *, x
end subroutine


! cable_air.f90
!
! Source file containing air routines for CABLE
!
! Development by Ying-Ping Wang, Eva Kowalczyk, Ray Leuning
! Gab Abramowitz, Martin Dix, Harvey Davies, Mike Raupach,
!
! bugs to bernard.pak@csiro.au
!
! This file contains modules:
!   air_module,
! The subroutines included are:
!   define_air,
!
! Most user-defined types (e.g. met%tk) are defined in cable_types module
! in cable_variables.f90

MODULE cable_air
  USE cable_physical_constants
  USE cable_dimensions, ONLY: mp_patch, r_1
  USE cable_types
  IMPLICIT NONE
  PRIVATE
  PUBLIC define_air
CONTAINS
  !-----------------------------------------------------------------------
  SUBROUTINE define_air(met,air)
    TYPE (air_type), INTENT(INOUT) :: air ! air_type variables
    TYPE (met_type), INTENT(IN)  :: met ! meteorological variables
    REAL(r_1), DIMENSION(mp_patch) :: es ! sat vapour pressure (mb)
    ! Calculate saturation vapour pressure
    es = tetena * EXP(tetenb * (met%tvair-tfrz)/(tetenc + (met%tvair-tfrz)))
    ! Calculate conversion factor from from m/s to mol/m2/s
    air%cmolar = met%pmb * 100.0 / (rgas * (met%tvair))
    ! Calculate dry air density:
    air%rho = MIN(1.3,rmair * air%cmolar)
    ! molar volume (m^3/mol)
    air%volm = rgas * (met%tvair) / (100.0 * met%pmb)
    ! latent heat for water (j/kg)
    ! air%rlam = (2501.0 - 2.38 * (met%tvair- tfrz)) * 1000.0
	air%rlam  = 1.91846e6*((met%tvair)/((met%tvair)-33.91))**2  ! Henderson-Sellers, QJRMS, 1984
    !air%rlam=2.5104e6
    ! saturation specific humidity
    air%qsat = (rmh2o / rmair) * es / met%pmb
    ! d(qsat)/dT ((kg/kg)/K)
    air%epsi = (air%rlam / capp) * (rmh2o / rmair) * es * tetenb * tetenc / &
         &  (tetenc + (met%tvair - tfrz)) ** 2 / met%pmb
    ! air kinematic viscosity (m^2/s)
    air%visc = 1e-5 * MAX(1.0, 1.35 + 0.0092 * (met%tvair - tfrz))
    ! psychrometric constant
    air%psyc = met%pmb * 100.0 * capp * rmair / air%rlam / rmh2o
    ! d(es)/dT (mb/K)
    air%dsatdk = 100.0*(tetena*tetenb*tetenc)/((met%tvair-tfrz)+tetenc)**2 &
         * EXP(tetenb*(met%tvair-tfrz)/((met%tvair-tfrz) + tetenc))
    !      air%dsatdk = (610.078 * 17.27 * 237.3) / ((met%tvair-tfrz)+237.2)** 2 * &
    !                 & EXP(17.27 * (met%tvair-tfrz) / ((met%tvair-tfrz) + 237.3))
  END SUBROUTINE define_air
END MODULE cable_air


subroutine nbody_drag_terms(position,velocity,acceleration)
! Calculates approximation to 3 drag terms given a migration timescale
! Migration drag at timescale tmig
! Eccentricity damping on timescale tmig/10
! Inclincation damping on timescale tmig/10
! (cf Alibert et al 2013)

! Forces not calculated on body 1 (the star)
! Forces not calculated if embryo is finished

use embryodata
implicit none

integer :: ibody,ix
real,dimension(3,nbodies),intent(in) :: position,velocity
real,dimension(3,nbodies),intent(inout) :: acceleration
real,dimension(nbodies) :: vdotr,rmag

! Migration drag first

do ix=1,3
    do ibody=2,nbodies
    if(embryo(ibody-1)%tmig>small .and. embryo(ibody-1)%finished==0) then
       acceleration(ix,ibody) = acceleration(ix,ibody)- &
            velocity(ix,ibody)/(2.0*embryo(ibody-1)%tmig)
    endif
    enddo
enddo

! Eccentricity damping
vdotr(:) =  0.0
do ix=1,3
   vdotr(:) = vdotr(:)+ position(ix,:)*velocity(ix,:)
enddo

rmag(:) = 0.0

rmag(:) = position(1,:)*position(1,:) + &
     position(2,:)*position(2,:) + &
     position(3,:)*position(3,:)

do ix=1,3

do ibody=2,nbodies

 if (embryo(ibody-1)%tmig*rmag(ibody)>small .and.  embryo(ibody-1)%finished==0) then
acceleration(ix,ibody) = acceleration(ix,ibody) - &
     2.0*vdotr(ibody)*position(ix,ibody)/(rmag(ibody)*rmag(ibody)*dampfac*embryo(ibody-1)%tmig)
endif
enddo
enddo

! Inclination damping

do ibody=2,nbodies
!if(embryo(ibody-1)%finished==1) cycle
if(embryo(ibody-1)%tmig>small .and. embryo(ibody-1)%finished==0) then
    acceleration(3,ibody) = acceleration(3,ibody) - &
         2.0*vel(3,ibody)/(dampfac*embryo(ibody-1)%tmig)
endif
end do

end subroutine nbody_drag_terms


program test_dirac
use types, only: dp
use bsplines, only: bspline, bspline_der, bspline_der2
use mesh, only: meshexp
use quadrature, only: gauss_pts, gauss_wts
use linalg, only: eigh
use utils, only: stop_error
implicit none

integer, parameter :: n = 30, k = 6, Nq=7
integer, parameter :: N_intervals = n-k+1
integer, parameter :: Nq_total = Nq*N_intervals, Nb=n-1
real(dp) :: t(n+k), rmin, rmax, a
real(dp) :: xiq(Nq), wtq(Nq), xa, xb, jac, x(Nq)
real(dp), allocatable :: xq(:), wq(:), hq(:)
real(dp), allocatable :: B(:,:), Bp(:,:), Bpp(:,:), Bi(:), Bj(:), Bip(:), Bjp(:)
real(dp), allocatable :: Bipp(:), Bjpp(:)
real(dp), allocatable :: Vq(:), Vqp(:)
real(dp), allocatable :: Am(:,:), Bm(:,:), sol(:,:), lam(:)
real(dp), allocatable :: solsP(:,:), solsQ(:,:)
real(dp) :: En, c
integer :: i, j, kappa, Z, l, relat, u

allocate(Am(2*Nb,2*Nb), Bm(2*Nb,2*Nb), sol(2*Nb,2*Nb), lam(2*Nb))
allocate(xq(Nq_total), wq(Nq_total), hq(Nq_total))
allocate(B(Nq_total, Nb), Bp(Nq_total, Nb), Bpp(Nq_total, Nb))
allocate(Bi(Nq_total), Bj(Nq_total), Bip(Nq_total), Bjp(Nq_total), Vq(Nq_total))
allocate(Bipp(Nq_total), Bjpp(Nq_total), Vqp(Nq_total))
allocate(solsP(Nq_total,Nb))
allocate(solsQ(Nq_total,Nb))

rmin = 1e-15_dp
rmax = 10
rmin = 0
rmax = 10
a = 6e5
a = 1e4
Z = 83
kappa = 2
c = 137.03599907_dp

t(:k-1) = rmin
t(k:n+1) = meshexp(rmin, rmax, a, N_intervals)
!t(k:n+1) = -1
t(n+2:) = rmax
!do i = 0, N_intervals
!    t(i+k) = (rmax-rmin)*(real(i, dp)/n)**6+rmin
!end do

print *, "Constructing quadrature rule"
! Loop over knot spans (intervals), and constract a global quadrature rule
! Integrals of a function hq evaluated at the points xq are calculated using:
! sum(wq*hq)
xiq = gauss_pts(Nq)
wtq = gauss_wts(Nq)
do i = 1, n-k+1
    xa = t(i+k-1)
    xb = t(i+k)
    jac = (xb-xa)/2
    x = (xiq(:)+1) * jac + xa
    xq((i-1)*Nq+1:i*Nq) = x
    wq((i-1)*Nq+1:i*Nq) = wtq*jac
end do

print *, "Evaluating basis functions"
! Evaluate basis functions and their derivatives on quadrature grid
! Skip the first and last B-spline (that's why Nb=n-2).
do i = 1, Nb
    B(:,i)   = bspline     (t, i, k, xq)
    Bp(:,i)  = bspline_der (t, i, k, xq)
    Bpp(:,i) = bspline_der2(t, i, k, xq)
end do

print *, "Assembly"
! Construct matrices A and B
do i = 1, Nb
    do j = 1, Nb
        Bi = B(:,i)
        Bj = B(:,j)
        Bip = Bp(:,i)
        Bjp = Bp(:,j)
        Bipp = Bpp(:,i)
        Bjpp = Bpp(:,j)
        Vq = -Z/xq
        Vqp = Z/xq**2
        ! A11
        hq = -c**2*(Bi*xq**2*Bjpp +2*(Bi*xq*Bjp)-kappa*(kappa+1)*Bi*Bj) &
            +c**4*Bi*xq**2*Bj+Bi*xq**2*Vq**2*Bj+2*c**2*Bi*xq**2*Vq*Bj
        Am(i,j) = sum(wq*hq)
        ! A12
        hq = -c*(2*Bi*xq**2*Vq*Bjp+2*(1-kappa)*Bi*xq*Vq*Bj+Bi*xq**2*Vqp*Bj)
        Am(i,j+Nb) = sum(wq*hq)
        ! A21
        hq = c*(2*Bi*xq**2*Vq*Bjp+2*(1+kappa)*Bi*xq*Vq*Bj+Bi*xq**2*Vqp*Bj)
        Am(i+Nb,j) = sum(wq*hq)
        ! A22
        hq = -c**2*(Bi*xq**2*Bjpp +2*(Bi*xq*Bjp)-(-kappa)*(-kappa+1)*Bi*Bj) &
            +c**4*Bi*xq**2*Bj+Bi*xq**2*Vq**2*Bj-2*c**2*Bi*xq**2*Vq*Bj
        Am(i+Nb,j+Nb) = sum(wq*hq)

        ! B11
        hq = B(:,i)*B(:,j)*xq**2
        Bm(i,j) = sum(wq*hq)
        ! B22
        Bm(i+Nb,j+Nb) = Bm(i,j)
    end do
end do

print *, "Checking symmetry"
do j = 1, 2*Nb
    do i = 1, j-1
        if (max(abs(Am(i,j)), abs(Am(j,i))) > tiny(1._dp)) then
            if (abs(Am(i,j)-Am(j,i)) / max(abs(Am(i,j)), abs(Am(j,i))) &
                    > 1e-8_dp) then
                print *, i, j, Am(i,j)-Am(j,i), Am(i,j), Am(j,i)
                call stop_error("Am not symmetric")
            end if
        end if
        if (abs(Bm(i,j)-Bm(j,i)) > 1e-12_dp) call stop_error("Bm not symmetric")
   end do
end do

print *, "Eigensolver"
! Solve an eigenproblem
call eigh(Am, Bm, lam, sol)

print *, "n, energy, exact energy, error"
do i = 1, Nb
    if (kappa > 0) then
        l = kappa
        relat = 3
    else
        l = -kappa-1
        relat = 2
    end if
    En = E_nl(c, l+i, l, real(Z, dp), relat)
    lam(i) = sqrt(lam(i)) - c**2
    print "(i4, f30.8, f18.8, es12.2)", i, lam(i), En, abs(lam(i)-En)
end do

do i = 1, Nb
    solsP(:,i) = 0
    do j = 1, Nb
        solsP(:,i) = solsP(:,i) + sol(j,i)*B(:,j)
    end do
end do

open(newunit=u, file="dirac_sol.txt", status="replace")
write(u,*) xq
do i = 1, Nb
    write(u,*) solsP(:,i)
end do
close(u)

contains

real(dp) function E_nl(c, n, l, Z, relat)
! Calculates exact energy for the radial Schroedinger/Dirac equations
real(dp), intent(in) :: c, Z ! speed of light in atomic units
integer, intent(in) :: n, l, relat
! quantum numbers (n, l), atomic number (z)
! relat == 0 ... Schroedinger equation
! relat == 2 ... Dirac equation, spin up
! relat == 3 ... Dirac equation, spin down

integer :: skappa
real(dp) :: beta
if (.not. (l >= 0)) call stop_error("'l' must be positive or zero")
if (.not. (n > l)) call stop_error("'n' must be greater than 'l'")
if (l == 0 .and. relat == 3) call stop_error("Spin must be up for l==0.")
if (relat == 0) then
    E_nl = - Z**2 / (2.0_dp * n**2)
else
    if (relat == 2) then
        skappa = -l - 1
    else
        skappa = -l
    end if
    beta = sqrt(skappa**2 - Z**2 / c**2)
    E_nl = c**2/sqrt(1+Z**2/(n + skappa + beta)**2/c**2) - c**2
end if
end function

end program


#include <default_macro.h>

 module fluids_lattices_mod

!***********************************************************************
!
!     LBsoft module containing the lattice definition of the LB model
!
!     licensed under the 3-Clause BSD License (BSD-3-Clause)
!     author: M. Lauricella
!     last modification May 2020
!
!***********************************************************************

 implicit none

 private

#if LATTICE==319
 integer, parameter, public :: links=18

 character(len=6), parameter, public :: latt_name="d3q19 "

 integer, parameter, public :: latt_dim=3

 integer, parameter, public :: nbuff=2

 real(kind=PRC), parameter, public :: cssq = ( ONE / THREE )

 real(kind=PRC), parameter :: p0 = ( ONE / THREE )
 real(kind=PRC), parameter :: p1 = ( ONE / EIGHTEEN )
 real(kind=PRC), parameter :: p2 = ( ONE / THIRTYSIX )
 real(kind=PRC), dimension(0:links), parameter, public :: &
  p = (/p0,p1,p1,p1,p1,p1,p1,p2,p2,p2,p2,p2,p2,p2,p2,p2,p2,p2,p2/)

 !real(kind=PRC), parameter :: b_xi = real(2.d0,kind=PRC)  !Liu, Valocchi
 real(kind=PRC), parameter :: b_xi = real(0.5d0,kind=PRC) !Leclaire
 real(kind=PRC), parameter :: b0 = -(TWO+TWO*b_xi)/(THREE*b_xi+TWELVE)
 real(kind=PRC), parameter :: b1 = (b_xi)/(SIX*b_xi+TWENTYFOUR)
 real(kind=PRC), parameter :: b2 = ONE/(SIX*b_xi+TWENTYFOUR)
 real(kind=PRC), dimension(0:links), parameter, public :: &
  b_l = (/b0,b1,b1,b1,b1,b1,b1,b2,b2,b2,b2,b2,b2,b2,b2,b2,b2,b2,b2/)

 real(kind=PRC), parameter :: psi0 = -(FIVE/TWO)
 real(kind=PRC), parameter :: psi1 = -(ONE/SIX)
 real(kind=PRC), parameter :: psi2 = (ONE/TWENTYFOUR)
 real(kind=PRC), dimension(0:links), parameter, public :: &
  psi_CG = (/psi0,psi1,psi1,psi1,psi1,psi1,psi1,psi2,psi2,psi2,psi2, &
  psi2,psi2,psi2,psi2,psi2,psi2,psi2,psi2/)

 real(kind=PRC), parameter :: xi0 = ZERO
 real(kind=PRC), parameter :: xi1 = (ONE/FOUR)
 real(kind=PRC), parameter :: xi2 = (ONE/EIGHT)
 real(kind=PRC), dimension(0:links), parameter, public :: &
  xi_CG = (/xi0,xi1,xi1,xi1,xi1,xi1,xi1,xi2,xi2,xi2,xi2,xi2,xi2,xi2, &
  xi2,xi2,xi2,xi2,xi2/)

 real(kind=PRC), parameter :: phi0 = ZERO
 real(kind=PRC), parameter :: phi1 = (ONE/TWELVE)
 real(kind=PRC), parameter :: phi2 = (ONE/TWENTYFOUR)
 real(kind=PRC), dimension(0:links), parameter, public :: &
  phi_CG = (/phi0,phi1,phi1,phi1,phi1,phi1,phi1,phi2,phi2,phi2,phi2, &
  phi2,phi2,phi2,phi2,phi2,phi2,phi2,phi2/)

 real(kind=PRC), parameter :: varphi0 = ONE
 real(kind=PRC), parameter :: varphi1 = -(ONE/TWELVE)
 real(kind=PRC), parameter :: varphi2 = -(ONE/TWENTYFOUR)
 real(kind=PRC), dimension(0:links), parameter, public :: &
  varphi_CG = (/varphi0,varphi1,varphi1,varphi1,varphi1,varphi1, &
  varphi1,varphi2,varphi2,varphi2,varphi2,varphi2,varphi2,varphi2, &
  varphi2,varphi2,varphi2,varphi2,varphi2/)

 real(kind=PRC), dimension(0:links), parameter, public :: &
  a = (/ZERO,p1/cssq,p1/cssq,p1/cssq,p1/cssq,p1/cssq,p1/cssq,p2/cssq, &
  p2/cssq,p2/cssq,p2/cssq,p2/cssq,p2/cssq,p2/cssq,p2/cssq,p2/cssq, &
  p2/cssq,p2/cssq,p2/cssq/)

 !lattice vectors
 integer, dimension(0:links), parameter, public :: &
   !      0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12,13,14,15,16,17,18
  ex = (/ 0, 1,-1, 0, 0, 0, 0, 1,-1,-1, 1, 1,-1,-1, 1, 0, 0, 0, 0/)
 integer, dimension(0:links), parameter, public :: &
  ey = (/ 0, 0, 0, 1,-1, 0, 0, 1,-1, 1,-1, 0, 0, 0, 0, 1,-1,-1, 1/)
 integer, dimension(0:links), parameter, public :: &
  ez = (/ 0, 0, 0, 0, 0, 1,-1, 0, 0, 0, 0, 1,-1, 1,-1, 1,-1, 1,-1/)
 integer, dimension(0:links), parameter, public :: &
  opp =(/ 0, 2, 1, 4, 3, 6, 5, 8, 7,10, 9,12,11,14,13,16,15,18,17/)

 real(kind=PRC), dimension(0:links), parameter, public :: &
  dex = real(ex,kind=PRC)
 real(kind=PRC), dimension(0:links), parameter, public :: &
  dey = real(ey,kind=PRC)
 real(kind=PRC), dimension(0:links), parameter, public :: &
  dez = real(ez,kind=PRC)
#endif

 integer, parameter, public :: linksd3q27=26

 !lattice vectors
 integer, dimension(0:linksd3q27), parameter, public :: &
   !           0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26
  exd3q27 = (/ 0, 1,-1, 0, 0, 0, 0, 1,-1,-1, 1, 1,-1,-1, 1, 0, 0, 0, 0, 1,-1,-1, 1, 1,-1, 1,-1/)
 integer, dimension(0:linksd3q27), parameter, public :: &
  eyd3q27 = (/ 0, 0, 0, 1,-1, 0, 0, 1,-1, 1,-1, 0, 0, 0, 0, 1,-1,-1, 1, 1,-1, 1,-1,-1, 1, 1,-1/)
 integer, dimension(0:linksd3q27), parameter, public :: &
  ezd3q27 = (/ 0, 0, 0, 0, 0, 1,-1, 0, 0, 0, 0, 1,-1, 1,-1, 1,-1, 1,-1, 1,-1, 1,-1, 1,-1,-1, 1/)

 real(kind=PRC), dimension(0:linksd3q27), parameter, public :: &
  dexd3q27 = real(exd3q27,kind=PRC)
 real(kind=PRC), dimension(0:linksd3q27), parameter, public :: &
  deyd3q27 = real(eyd3q27,kind=PRC)
 real(kind=PRC), dimension(0:linksd3q27), parameter, public :: &
  dezd3q27 = real(ezd3q27,kind=PRC)

 real(kind=PRC), parameter :: p0d3q27 = ( TWO / THREE )**THREE
 real(kind=PRC), parameter :: p1d3q27 = (( TWO / THREE )**TWO ) * ( ONE/ SIX )
 real(kind=PRC), parameter :: p2d3q27 = (( ONE / SIX )**TWO) * ( TWO/ THREE)
 real(kind=PRC), parameter :: p3d3q27 = ( ONE / SIX )**THREE
 real(kind=PRC), dimension(0:linksd3q27), parameter, public :: &
  pd3q27 = (/p0d3q27,p1d3q27,p1d3q27,p1d3q27,p1d3q27,p1d3q27,p1d3q27, &
  p2d3q27,p2d3q27,p2d3q27,p2d3q27,p2d3q27,p2d3q27,p2d3q27,p2d3q27, &
  p2d3q27,p2d3q27,p2d3q27,p2d3q27,p3d3q27,p3d3q27,p3d3q27,p3d3q27, &
  p3d3q27,p3d3q27,p3d3q27,p3d3q27/)

 real(kind=PRC), dimension(0:linksd3q27), parameter, public :: &
  ad3q27 = (/ZERO,p1d3q27/cssq,p1d3q27/cssq,p1d3q27/cssq, &
  p1d3q27/cssq,p1d3q27/cssq,p1d3q27/cssq, &
  p2d3q27/cssq,p2d3q27/cssq,p2d3q27/cssq,p2d3q27/cssq,p2d3q27/cssq, &
  p2d3q27/cssq,p2d3q27/cssq,p2d3q27/cssq, &
  p2d3q27/cssq,p2d3q27/cssq,p2d3q27/cssq,p2d3q27/cssq,p3d3q27/cssq, &
  p3d3q27/cssq,p3d3q27/cssq,p3d3q27/cssq, &
  p3d3q27/cssq,p3d3q27/cssq,p3d3q27/cssq,p3d3q27/cssq/)

 end module fluids_lattices_mod


!
! Copyright (C) 2001-2007 Quantum ESPRESSO group
! This file is distributed under the terms of the
! GNU General Public License. See the file `License'
! in the root directory of the present distribution,
! or http://www.gnu.org/copyleft/gpl.txt .
!
!
!--------------------------------------------------------------------
subroutine sph_bes (msh, r, q, l, jl)
  !--------------------------------------------------------------------
  !
  ! ... input:
  ! ...   msh     = number of grid points points
  ! ...   r(1:msh)= radial grid
  ! ...   q       = q
  ! ...   l       = angular momentum (-1 <= l <= 6)
  ! ... output:
  ! ...   jl(1:msh) = j_l(q*r(i))  (j_l = spherical bessel function)
  !
  use kinds, only: DP
  USE constants, ONLY : eps14
  !
  implicit none
  !
  integer :: msh, l
  real(DP) :: r (msh), q, jl (msh)
  !
  ! xseries = convergence radius of the series for small x of j_l(x)
  real(DP) :: x, xl, xseries = 0.05_dp
  integer :: ir, ir0
  integer, external:: semifact
  !
#if defined (__MASS)
  real(DP) :: qr(msh), sin_qr(msh), cos_qr(msh)
#endif

  !  case q=0

  if (abs (q) < eps14) then
     if (l == -1) then
        call errore ('sph_bes', 'j_{-1}(0) ?!?', 1)
     elseif (l == 0) then
        jl(:) = 1.d0
     else
        jl(:) = 0.d0
     endif
     return
  end if

  !  case l=-1

  if (l == - 1) then
     if (abs (q * r (1) ) < eps14) call errore ('sph_bes', 'j_{-1}(0) ?!?',1)

#if defined (__MASS)

     qr = q * r
     call vcos( cos_qr, qr, msh)
     jl = cos_qr / qr

#else

     jl (:) = cos (q * r (:) ) / (q * r (:) )

#endif

     return

  end if

  ! series expansion for small values of the argument
  ! ir0 is the first grid point for which q*r(ir0) > xseries
  ! notice that for small q it may happen that q*r(msh) < xseries !

  ir0 = msh+1
  do ir = 1, msh
     if ( abs (q * r (ir) ) > xseries ) then
        ir0 = ir
        exit
     end if
  end do

  do ir = 1, ir0 - 1
     x = q * r (ir)
     if ( l == 0 ) then
        xl = 1.0_dp
     else
        xl = x**l
     end if
     jl (ir) = xl/semifact(2*l+1) * &
                ( 1.0_dp - x**2/1.0_dp/2.0_dp/(2.0_dp*l+3) * &
                ( 1.0_dp - x**2/2.0_dp/2.0_dp/(2.0_dp*l+5) * &
                ( 1.0_dp - x**2/3.0_dp/2.0_dp/(2.0_dp*l+7) * &
                ( 1.0_dp - x**2/4.0_dp/2.0_dp/(2.0_dp*l+9) ) ) ) )
  end do

  ! the following shouldn't be needed but do you trust compilers
  ! to do the right thing in this special case ? I don't - PG

  if ( ir0 > msh ) return

  if (l == 0) then

#if defined (__MASS)

     qr = q * r
     call vsin( sin_qr, qr, msh)
     jl (ir0:) = sin_qr(ir0:) / (q * r (ir0:) )

#else

     jl (ir0:) = sin (q * r (ir0:) ) / (q * r (ir0:) )

#endif

  elseif (l == 1) then

#if defined (__MASS)

     qr = q * r
     call vcos( cos_qr, qr, msh)
     call vsin( sin_qr, qr, msh)
     jl (ir0:) = ( sin_qr(ir0:) / (q * r (ir0:) ) - &
                   cos_qr(ir0:) ) / (q * r (ir0:) )

#else

     jl (ir0:) = (sin (q * r (ir0:) ) / (q * r (ir0:) ) - &
                  cos (q * r (ir0:) ) ) / (q * r (ir0:) )

#endif

  elseif (l == 2) then

#if defined (__MASS)

     qr = q * r
     call vcos( cos_qr, qr, msh)
     call vsin( sin_qr, qr, msh)
     jl (ir0:) = ( (3.d0 / (q*r(ir0:)) - (q*r(ir0:)) ) * sin_qr(ir0: ) - &
                    3.d0 * cos_qr(ir0:) ) / (q*r(ir0:))**2

#else

     jl (ir0:) = ( (3.d0 / (q*r(ir0:)) - (q*r(ir0:)) ) * sin (q*r(ir0:)) - &
                    3.d0 * cos (q*r(ir0:)) ) / (q*r(ir0:))**2

#endif

  elseif (l == 3) then

#if defined (__MASS)

     qr = q * r
     call vcos( cos_qr, qr, msh)
     call vsin( sin_qr, qr, msh)
     jl (ir0:) = (sin_qr (ir0:) * &
                  (15.d0 / (q*r(ir0:)) - 6.d0 * (q*r(ir0:)) ) + &
                  cos_qr (ir0:) * ( (q*r(ir0:))**2 - 15.d0) ) / &
                  (q*r(ir0:))**3

#else

     jl (ir0:) = (sin (q*r(ir0:)) * &
                  (15.d0 / (q*r(ir0:)) - 6.d0 * (q*r(ir0:)) ) + &
                  cos (q*r(ir0:)) * ( (q*r(ir0:))**2 - 15.d0) ) / &
                  (q*r(ir0:)) **3

#endif

  elseif (l == 4) then

#if defined (__MASS)

     qr = q * r
     call vcos( cos_qr, qr, msh)
     call vsin( sin_qr, qr, msh)
     jl (ir0:) = (sin_qr (ir0:) * &
                  (105.d0 - 45.d0 * (q*r(ir0:))**2 + (q*r(ir0:))**4) + &
                  cos_qr (ir0:) * &
                  (10.d0 * (q*r(ir0:))**3 - 105.d0 * (q*r(ir0:))) ) / &
                    (q*r(ir0:))**5

#else

     jl (ir0:) = (sin (q*r(ir0:)) * &
                  (105.d0 - 45.d0 * (q*r(ir0:))**2 + (q*r(ir0:))**4) + &
                  cos (q*r(ir0:)) * &
                  (10.d0 * (q*r(ir0:))**3 - 105.d0 * (q*r(ir0:))) ) / &
                     (q*r(ir0:))**5
#endif

  elseif (l == 5) then

#if defined (__MASS)
     qr = q * r
     call vcos( cos_qr, qr, msh)
     call vsin( sin_qr, qr, msh)
     jl (ir0:) = (-cos_qr(ir0:) - &
                  (945.d0*cos_qr(ir0:)) / (q*r(ir0:)) ** 4 + &
                  (105.d0*cos_qr(ir0:)) / (q*r(ir0:)) ** 2 + &
                  (945.d0*sin_qr(ir0:)) / (q*r(ir0:)) ** 5 - &
                  (420.d0*sin_qr(ir0:)) / (q*r(ir0:)) ** 3 + &
                  ( 15.d0*sin_qr(ir0:)) / (q*r(ir0:)) ) / (q*r(ir0:))
#else
     jl (ir0:) = (-cos(q*r(ir0:)) - &
                  (945.d0*cos(q*r(ir0:))) / (q*r(ir0:)) ** 4 + &
                  (105.d0*cos(q*r(ir0:))) / (q*r(ir0:)) ** 2 + &
                  (945.d0*sin(q*r(ir0:))) / (q*r(ir0:)) ** 5 - &
                  (420.d0*sin(q*r(ir0:))) / (q*r(ir0:)) ** 3 + &
                  ( 15.d0*sin(q*r(ir0:))) / (q*r(ir0:)) ) / (q*r(ir0:))
#endif

  elseif (l == 6) then

#if defined (__MASS)

     qr = q * r
     call vcos( cos_qr, qr, msh)
     call vsin( sin_qr, qr, msh)
     jl (ir0:) = ((-10395.d0*cos_qr(ir0:)) / (q*r(ir0:))**5 + &
                  (  1260.d0*cos_qr(ir0:)) / (q*r(ir0:))**3 - &
                  (    21.d0*cos_qr(ir0:)) / (q*r(ir0:))    - &
                             sin_qr(ir0:)                   + &
                  ( 10395.d0*sin_qr(ir0:)) / (q*r(ir0:))**6 - &
                  (  4725.d0*sin_qr(ir0:)) / (q*r(ir0:))**4 + &
                  (   210.d0*sin_qr(ir0:)) / (q*r(ir0:))**2 ) / (q*r(ir0:))
#else

     jl (ir0:) = ((-10395.d0*cos(q*r(ir0:))) / (q*r(ir0:))**5 + &
                  (  1260.d0*cos(q*r(ir0:))) / (q*r(ir0:))**3 - &
                  (    21.d0*cos(q*r(ir0:))) / (q*r(ir0:))    - &
                             sin(q*r(ir0:))                   + &
                  ( 10395.d0*sin(q*r(ir0:))) / (q*r(ir0:))**6 - &
                  (  4725.d0*sin(q*r(ir0:))) / (q*r(ir0:))**4 + &
                  (   210.d0*sin(q*r(ir0:))) / (q*r(ir0:))**2 ) / (q*r(ir0:))
#endif

  else

     call errore ('sph_bes', 'not implemented', abs(l))

  endif
  !
  return
end subroutine sph_bes

integer function semifact(n)
  ! semifact(n) = n!!
  implicit none
  integer :: n, i

  semifact = 1
  do i = n, 1, -2
     semifact = i*semifact
  end do
  return
end function semifact


      SUBROUTINE SUBDIN(A,LDA,NA,AS,LDAS,IND,NI,BS,LDBS,INV,LDINV,IERR,
     .                  DET)
      IMPLICIT NONE
*
*  -- user defined driver routine --
*
*
*     .. Scalar Arguments ..
      INTEGER            NA, NI, IERR, LDA, LDAS, LDINV, LDBS
*     ..
*     .. Array Arguments ..
      INTEGER            IND(*)
      DOUBLE PRECISION   A(LDA,*), AS(LDAS,*), BS(LDBS,*), INV(LDINV,*),
     .                   DET(*)
*     ..
*
*  Purpose
*  =======
*
*  SUBDIN computes the determinant and the inverse of the actual
*  submatrix using the Cholesky factorization.
*
*
*  Arguments
*  =========
*
*  A        (input) DOUBLE PRECISION array of DIMENSION (NA,NA).
*           Before entry, the array A must contain matrix of
*           covariances.
*           Unchanged on exit.
*
*  NA       (input) INTEGER.
*           On entry, NA specifies the number of rows/columns of A
*           as declared in the calling (sub) program. NA >= 1.
*           Unchanged on exit.
*
*  AS       (work array) DOUBLE PRECISION, DIMENSION (NA,NA).
*            AS stored the actual submatrix.
*           Changed on exit.
*
*  IND      (input) INTEGER vector, DIMENSION (NA).
*           The first NI elements of the vector correspond to the indices
*           which form the submatrix AS.
*           Unchanged on exit.
*
*  NI       (input) INTEGER.
*           On entry, NI specifies the indices contained in the
*           actual solution. 0 <= NI <= NA.
*           Unchanged on exit.
*
*  BS       (work array) DOUBLE PRECISION, DIMENSION (NA,NA).
*           Cholesky decomposition of the matrix A.
*
*  INV      (output) DOUBLE PRECISION array of DIMENSION (NI,NI).
*           On exit, the array INV contains the inverse of the submatrix AS.
*
*  IERR     (output) INTEGER
*           the error indicator from DPOFA.  this will be
*           zero unless the matrix being factorized is
*           not positive definite.
*
*  DET      (output) DOUBLE PRECISION vector, DIMENSION (2).
*           The first argument stores the mantissa and the second
*           gives the exponent of the determinant of the upper
*           triangular matrix BS.
*
*
*  =====================================================================
*
*     ..
*     .. Parameters ..
*     ..
*     .. Local Scalars ..
      INTEGER            I, J
*     ..
*     .. Local Arrays ..
*     ..
*     .. External Functions ..
*     ..
*     .. External Subroutines ..
      EXTERNAL           PSUBM, CHOL1, CH2INV1, CH2DET
*     ..
*     .. Intrinsic Functions ..
*     ..
*     .. Executable Statements ..
*
*
*     Computing the actual submatrix

      CALL PSUBM(A,LDA,NA,AS,LDAS,IND,NI)

*     Cholesky factorization

      CALL CHOL1(AS,LDA,NI,BS,LDBS,IERR)

*     Determining the inverse  of the submatrix
*     using the output of the Cholesky factorization

      CALL CH2INV1(BS,LDBS,NI,INV,LDINV,IERR)

*     Determining the determinant of the submatrix
*     using the output of the Cholesky factorization

      CALL CH2DET(BS,LDBS,NI,DET,IERR)

      RETURN
      END


subroutine readin(nsin, ierflag)

      use stel_kinds, only: dp
      use name0,      only: czero,    &
                            c2p0
      use name1,      only: mpol,     &
                            mpol1,    &
                            nmax,     &
                            nsd,      &
                            ntheta1,  &
                            nzeta
      use inputdat,   only: nfp,      &
                            ncurr,    &
                            niter,    &
                            nstep,    &
                            ftol,     &
                            gam,      &
                            nvacskip, &
                            am,       &
                            ai,       &
                            ac, rb, zb, raxis, zaxis
      use magfield,   only: phiedge,  &
                            curpol,   &
                            curtor

      implicit none

      integer, intent(out) :: nsin
      integer, intent(out) :: ierflag

      integer       :: i, j, m, n
      integer       :: n1 ! abs(n)
      integer       :: isgn
      integer       :: mnbound ! unused?
      logical       :: done
      real(kind=dp) :: FOURNORM
      real(kind=dp) :: rc(0:mpol1,-nmax:nmax)
      real(kind=dp) :: zs(0:mpol1,-nmax:nmax)
      real(kind=dp) :: rmag(-nmax:nmax)
      real(kind=dp) :: zmag(-nmax:nmax)

      logical :: print_input_geometry = .false.

      !         INPUT DATA AND RECOMMENDED VALUES
      !
      ! ftol    value of fsq = fr**2 + fz**2 at which iteration ends
      ! gam     value of compressibility index (gam=0 => pressure prescribed)
      ! ncurr   flux conserving (=0) or no net toroidal current (=1)
      ! nfp     number of toroidal field periods
      ! niter   number of iterations (used to terminate run)
      ! nsin    number of radial intervals used to obtain initial guess
      !         for interpolation onto finer grid; <2, no interpolation
      ! ns      number of radial mesh points (>2)
      ! nstep   number of timesteps between printouts on screen
      ! rc      boundary coefficients of cos(m*theta-n*zeta) for R
      ! zs      boundary coefficients of sin(m*theta-n*zeta) for Z

      ! INITIALIZATION BLOCK (NEEDED IF VMEC CALLED REPETITIVELY)
      do n = -nmax,nmax
        rmag(n) = czero
        zmag(n) = czero
        do m = 0,mpol1
          rc(m,n) = czero
          zs(m,n) = czero
        enddo
      enddo

      do n = 0, nmax
        raxis(n) = czero
        zaxis(n) = czero
        do m = 0, mpol1
          rb(n,m,1) = czero
          rb(n,m,2) = czero
          zb(n,m,1) = czero
          zb(n,m,2) = czero
        enddo
      enddo

      ! READ IN DATA FROM INDATA FILE
      read(5,*)
      read(5,*) nfp, mnbound, ncurr, nsin, niter, nstep, ftol
      read(5,*)
      read(5,*) gam, phiedge, curpol, curtor

      print   15, nsd, ntheta1, nzeta, mpol, nmax, &
                  nfp, gam, phiedge, curpol, curtor
      write(3,15) nsd, ntheta1, nzeta, mpol, nmax, &
                  nfp, gam, phiedge, curpol, curtor
 15   format(/,' COMPUTATION PARAMETERS: (u = theta, v = zeta)'//,       &
               '     ns     nu     nv     mu     mv'//,                  &
               5i7,//,                                                   &
               ' CONFIGURATION PARAMETERS:'//,                           &
               '    nfp      gamma    phiedge     curpol     curtor',//, &
               i7,1p4e11.3)

      nvacskip = nfp

      print   20, ncurr, niter, nsin, nstep, nvacskip, ftol
      write(3,20) ncurr, niter, nsin, nstep, nvacskip, ftol
 20   format(/' CONTROL PARAMETERS:'//,                              &
              '  ncurr  niter   nsin  nstep  nvacskip      ftol',//, &
              4i7,i10,1pe10.2,/)

      write(3,25)
 25   format(' MASS PROFILE EXPANSION COEFFICIENTS (am):',/)
      read(5,*)
      read(5,*)(am(i),i=0,5)
      read(5,*)
      write(3,40)(am(i),i=0,5)

      write(3,45)
 45   format(' IOTA PROFILE EXPANSION COEFFICIENTS (ai):',/)
      read(5,*)(ai(i),i=0,5)
      read(5,*)
      write(3,40)(ai(i),i=0,5)

      WRITE(3,46)
 46   format(' CURRENT PROFILE EXPANSION COEFFICIENTS (ac):',/)
      READ(5,*)(AC(I),I=1,5)
      READ(5,*)
      WRITE(3,40)(AC(I),I=1,5)
 40   format(1p6e12.3,/)

      ! DO I=1, MNBOUND
      DO I=1, MPOL1*(2*NMAX+1)
        READ(5,*,END=51) M, N, RC(M,N), ZS(M,N)

        if (print_input_geometry) &
          write(*,*)     M, N, RC(M,N), ZS(M,N)

        if (m.gt.mpol1 .or. m.lt.0) &
          ierflag = 3
        if (n.gt.nmax .or. n.lt.(-nmax)) &
          ierflag = 4
        if (ierflag.ne.0) &
          return

        if (m.eq.0) then
          backspace 5 ! TODO: circumvent this somehow...
          read(5,*) m, n, rc(m,n), zs(m,n), rmag(n), zmag(n)
        endif
      enddo

 51   WRITE(3,55)
 55   format(/,'   mb  nb     rbc         zbs        raxis       zaxis',/)

      ! A_NORMIERUNG SGG
      IF ((RC(1,0) + ZS(1,0)).EQ.0) &
        STOP 'KANN FOURKOEF. NICHT NORMIEREN'

      ! TODO: computation of norm relies on sum of _abs_ values not being 0 ???
      FOURNORM = c2p0/(abs(RC(1,0)) + abs(ZS(1,0)))

      DO I = 0, MPOL1
        DO J = -NMAX, NMAX
          RC(I,J) = RC(I,J)*FOURNORM
          ZS(I,J) = ZS(I,J)*FOURNORM
        enddo
      enddo

      DO J = -NMAX, NMAX
        RMAG(J) = RMAG(J)*FOURNORM
        ZMAG(J) = ZMAG(J)*FOURNORM
      enddo
      ! E_NORMIERUNG SGG

      done = .false.
      do m = 0, mpol1
        do n = -nmax, nmax
          n1 = iabs(n)

          isgn = 1
          if (n.lt.0) &
            isgn = -1

          if (m.eq.0) then
            raxis(n1) = raxis(n1) + rmag(n)
            zaxis(n1) = zaxis(n1) - zmag(n)*isgn
          endif

          rb(n1,m,1) = rb(n1,m,1) +      rc(m,n)
          rb(n1,m,2) = rb(n1,m,2) + isgn*rc(m,n)
          zb(n1,m,1) = zb(n1,m,1) - isgn*zs(m,n)
          zb(n1,m,2) = zb(n1,m,2) +      zs(m,n)

          if (n.eq.0 .or. m.eq.0) &
            rb(n1,m,2) = czero
          if (n.eq.0) &
            zb(n1,m,1) = czero
          if (m.eq.0) &
            zb(n1,m,2) = czero

          if (rc(m,n).ne.czero .or. zs(m,n).ne.czero) then
            ! only write non-zero coefficients to screen
            if (m.eq.0) then
              write(3,65) m, n, rc(m,n), zs(m,n), rmag(n), zmag(n)
            else
              write(3,65) m, n, rc(m,n), zs(m,n)
            endif
          end if
        enddo

        if (done) &
          exit
      enddo
 65   format(i5,i4,1p4e12.4)

      print   70
      WRITE(3,70)
 70   format(/,' LEGEND: FSQR, FSQZ = Normalized Physical Force Residuals',/, &
               '         fsqr, fsqz =      Preconditioned Force Residuals'/)

      return

end


************************************************************************
*              Support routines for quadric surfaces                   *
************************************************************************
* EAM Jun 1997	- initial version, supports version 2.4(alpha) of render
* EAM May 1998	- additional error checking to go with Parvati/rastep
* EAM Jan 1999	- version 2.4i
* EAM Mar 2008	- Gfortran optimization breaks the object accounting.
*		  No solution yet.  Break qinp.f into separate file
************************************************************************
*
* Quadric surfaces include spheres, cones, ellipsoids, paraboloids, and
* hyperboloids.  The motivation for this code was to allow rendering
* thermal ellipsoids for atoms, so the other shapes have not been
* extensively tested.
* A quadric surface is described by 10 parameters (A ... J).
* For efficiency during rendering it is also useful to know the center and
* a bounding sphere. So a QUADRIC descriptor to render has 17 parameters:
* 14 (object type QUADRIC)
*    X Y Z RADLIM RED GRN BLU
*    A B C D E F G H I J
*
* The surface itself is the set of points for which Q(x,y,z) = 0
* where
*      Q(x,y,z)	=  A*x^2 +  B*y^2 +  C*z^2
*         	+ 2D*x*y + 2E*y*z + 2F*z*x
*         	+ 2G*x   + 2H*y   + 2I*z
*         	+  J
*
* It is convenient to store this information in a matrix QQ
*	| QA QD QF QG |		QA = A    QB = B    QC = C
* QQ =	| QD QB QE QH |		QD = D    QE = E    QF = F
*	| QF QE QC QI |		QG = G    QH = H    QI = I
*	| QG QH QI QJ |		QJ = J
*
* Then Q(x,y,x) = XT*QQ*X	where X = (x,y,z,1)
* The point of this is that a 4x4 homogeneous transformation T can be
* applied to QQ by matrix multiplication:     QQ' = TinvT * QQ * Tinv
*
* The surface normal is easily found by taking the partial derivatives
* of Q(x,y,z) at the point of interest.
************************************************************************
* TO DO:
*	- can we distinguish an ellipsoid from other quadrics? Do we care?
*	- fix optimization problems with qinp
************************************************************************

CCC     Process single object descriptor during input phase
CC
C
	function qinp( buf, detail, shadow, sdtail )
*       Use MODULE LISTS not COMMON /LISTS/ for dynamic allocation
	USE LISTS
*
	IMPLICIT NONE
	logical  qinp
	real    buf(100)
	real    detail(17), sdtail(14)
	logical shadow
*
	real    QQ(4,4), QP(4,4), QT(4,4)
	real    xq, yq, zq, radlim, red, grn, blu
	real    xc, yc, zc, rc
	real    xr, yr, zr, xs, ys, zs, rs
	real    pfac
*
	integer  ix,iy,ixlo,ixhi,iylo,iyhi
c	VOLATILE ix,iy,ixlo,ixhi,iylo,iyhi
*
*     Array sizes
      INCLUDE 'parameters.incl'
*
      EXTERNAL PERSP
      REAL     PERSP
*
      COMMON /RASTER/ NTX,NTY,NPX,NPY
      INTEGER         NTX,NTY,NPX,NPY
*
      COMMON /MATRICES/ XCENT, YCENT, SCALE, EYEPOS, SXCENT, SYCENT,
     &                  TMAT, TINV, TINVT, SROT, SRTINV, SRTINVT
     &                 ,RAFTER, TAFTER
      REAL   XCENT, YCENT, SCALE, EYEPOS, SXCENT, SYCENT
      REAL   TMAT(4,4), TINV(4,4),   TINVT(4,4)
      REAL   SROT(4,4), SRTINV(4,4), SRTINVT(4,4)
      REAL   RAFTER(4,4), TAFTER(3)
*
C     Replace COMMON with MODULE
C     COMMON /LISTS/ KOUNT, MOUNT, TTRANS, ISTRANS
C     INTEGER KOUNT(MAXNTX,MAXNTY), MOUNT(NSX,NSY)
C     INTEGER TTRANS(MAXNTX,MAXNTY), ISTRANS
*
      COMMON /NICETIES/ TRULIM,      ZLIM,    FRONTCLIP, BACKCLIP
     &                , ISOLATION
      REAL              TRULIM(3,2), ZLIM(2), FRONTCLIP, BACKCLIP
      LOGICAL           ISOLATION
*
* Assume this is legitimate
	qinp = .TRUE.
*
* Update limits (have to trust the center coords)
*
	xq     = buf(1)
	yq     = buf(2)
	zq     = buf(3)
	radlim = buf(4)
	call assert(radlim.ge.0,'limiting radius < 0 in quadric')
	if (radlim.gt.0) then
	    trulim(1,1) = MIN( trulim(1,1), xq)
	    trulim(1,2) = MAX( trulim(1,2), xq)
	    trulim(2,1) = MIN( trulim(2,1), yq)
	    trulim(2,2) = MAX( trulim(2,2), yq)
	    trulim(3,1) = MIN( trulim(3,1), zq)
	    trulim(3,2) = MAX( trulim(3,2), zq)
	endif
*
* Standard color checks
*
	red    = buf(5)
	grn    = buf(6)
	blu    = buf(7)
	call assert(red.ge.0,'red < 0 in quadric')
	call assert(red.le.1,'red > 1 in quadric')
	call assert(grn.ge.0,'grn < 0 in quadric')
	call assert(grn.le.1,'grn > 1 in quadric')
	call assert(blu.ge.0,'blu < 0 in quadric')
	call assert(blu.le.1,'blu > 1 in quadric')
*
* Transform center before saving
* (But can we deal with perspective????)
*
	call transf (xq, yq, zq)
	radlim = radlim / TMAT(4,4)
	if (eyepos.gt.0) then
c	    pfac = 1./(1.-zq/eyepos)
	    pfac = persp(zq)
	    xq = xq * pfac
	    yq = yq * pfac
	    zq = zq * pfac
	    radlim = radlim * pfac
	endif
	xc = xq * scale + xcent
	yc = yq * scale + ycent
	zc = zq * scale
	rc = radlim * scale
*	save transformed Z limits
	zlim(1) = min( zlim(1), zc )
	zlim(2) = max( zlim(2), zc )
*
*	check for Z-clipping
	if (zc.gt.FRONTCLIP .or. zc.lt.BACKCLIP) then
	    qinp = .FALSE.
	    return
	endif
*
	detail(1) = xc
	detail(2) = yc
	detail(3) = zc
	detail(4) = rc
	detail(5) = red
	detail(6) = grn
	detail(7) = blu
*
* This is a terrible kludge, but necessary if called from normal3d -size BIGxBIG
* Thes test should really be if we are called from normal3d but no flag for that
	if (ntx.gt.size(kount,1) .or. nty.gt.size(kount,2)) goto 101
*
* Tally for tiles the object might impinge on
* Again we are relying on the correctness of the center coordinates
*
	ixlo = INT(xc-rc) / npx + 1
	ixhi = INT(xc+rc) / npx + 1
	iylo = INT(yc-rc) / npy + 1
	iyhi = INT(yc+rc) / npy + 1
	if (ixlo.lt.1)   ixlo = 1
	if (ixlo.gt.NTX) goto 101
	if (ixhi.lt.1)   goto 101
	if (ixhi.gt.NTX) ixhi = NTX
	if (iylo.lt.1)   iylo = 1
	if (iylo.gt.NTY) goto 101
	if (iyhi.lt.1)   goto 101
	if (iyhi.gt.NTY) iyhi = NTY
	do iy = iylo,iyhi
	do ix = ixlo,ixhi
	    KOUNT(ix,iy) = KOUNT(ix,iy) + 1
	    TTRANS(ix,iy) = TTRANS(ix,iy) + istrans
	enddo
	enddo
101	continue
*
* build matrix from coeffients describing quadric surface in standard form
*
	qq(1,1) = buf(8)
	qq(2,2) = buf(9)
	qq(3,3) = buf(10)
	qq(1,2) = buf(11)
	qq(2,1) = buf(11)
	qq(2,3) = buf(12)
	qq(3,2) = buf(12)
	qq(1,3) = buf(13)
	qq(3,1) = buf(13)
	qq(1,4) = buf(14)
	qq(4,1) = buf(14)
	qq(2,4) = buf(15)
	qq(4,2) = buf(15)
	qq(3,4) = buf(16)
	qq(4,3) = buf(16)
	qq(4,4) = buf(17)
*
* Transformed matrix QP = TINV(Transpose) * QQ * TINV
* where TINV is the inverse of TMAT
*
	call tmul4( qt, qq,  tinv )
	call tmul4( qp, tinvt, qt )
CD	noise = 0
CD	write (noise,191) 'QT  ',((QT(i,j),j=1,4),i=1,4)
CD 191	format(a,4(/,4f8.4))
CD	write (noise,191) 'QP  ',((QP(i,j),j=1,4),i=1,4)
*
* Save components of quadric surface built from transformed matrix
* for use during rendering
*
	detail(8)  = qp(1,1)
	detail(9)  = qp(2,2)
	detail(10) = qp(3,3)
	detail(11) = qp(1,2)
	detail(12) = qp(2,3)
	detail(13) = qp(1,3)
	detail(14) = qp(1,4)
	detail(15) = qp(2,4)
	detail(16) = qp(3,4)
	detail(17) = qp(4,4)
*
* Do it all over again for the shadow buffers NOT TESTED YET!
* (since I'm a little confused about what transformation
* I need to apply to the QQ matrix in shadow space)
*
	if (.not.shadow) return
*	first transform center and limiting sphere
	xr = srot(1,1)*xq + srot(1,2)*yq + srot(1,3)*zq
	yr = srot(2,1)*xq + srot(2,2)*yq + srot(2,3)*zq
	zr = srot(3,1)*xq + srot(3,2)*yq + srot(3,3)*zq
	xs = xr * scale + sxcent
	ys = yr * scale + sycent
	zs = zr * scale
	rs = radlim * scale
	sdtail(1) = xs
	sdtail(2) = ys
	sdtail(3) = zs
	sdtail(4) = rs
*	tally shadow tiles the object might impinge on
	ixlo = INT(xs-rs) / npx + 1
	ixhi = INT(xs+rs) / npx + 1
	iylo = INT(ys-rs) / npy + 1
	iyhi = INT(ys+rs) / npy + 1
	if (ixlo.lt.1)   ixlo = 1
	if (ixlo.gt.NSX) goto 209
	if (ixhi.lt.1)   goto 209
	if (ixhi.gt.NSX) ixhi = NSX
	if (iylo.lt.1)   iylo = 1
	if (iylo.gt.NSY) goto 209
	if (iyhi.lt.1)   goto 209
	if (iyhi.gt.NSY) iyhi = NSY
	do iy = iylo,iyhi
	do ix = ixlo,ixhi
	    MOUNT(ix,iy) = MOUNT(ix,iy) + 1
	enddo
	enddo
*	transform QQ into shadow space as well
	call tmul4( qt, qp, srtinv )
	call tmul4( qp, srtinvt, qt )
	sdtail(5)  = qp(1,1)
	sdtail(6)  = qp(2,2)
	sdtail(7)  = qp(3,3)
	sdtail(8)  = qp(1,2)
	sdtail(9)  = qp(2,3)
	sdtail(10) = qp(1,3)
	sdtail(11) = qp(1,4)
	sdtail(12) = qp(2,4)
	sdtail(13) = qp(3,4)
	sdtail(14) = qp(4,4)
*
 209	continue
*
* Done with input
*
	return
	end


subroutine tara2dScrSer(arg)

use cfgio_mod, only: cfg_t, parse_cfg

implicit none

type(cfg_t):: cfg

real ( kind = 8 ), parameter :: pi=3.14159265358979323846d0

include "fftw3.f"

integer (kind=4) Np,Nx,Ny,Nh

integer ( kind = 4 ) i,j,t
real ( kind = 8 ) Lx,Ly,dx,dy,kx,ky,time,time_min,time_max,dt,alpha,nu,lambda,C0,C1,C2,C3,W0,m,d,G,Gb
REAL ( kind = 8 ) Energy,Energy0,yEnergy,yEnergy0
real (kind = 8), dimension (:), allocatable :: x,y
real (kind=8), dimension(:, :), allocatable :: psi,omega,omega_dum,ux,uy,ux_dum,uy_dum
complex (kind=8), dimension(:, :), allocatable :: omegak,omegak_dum,omegak_new,dt_omegak_old,dt_omegak_new
complex (kind=8), dimension(:, :), allocatable :: psik,psik_dum,ukx,uky,ukx_dum,uky_dum
integer ( kind = 8 ) plan_forward,plan_backward

character (len=90) :: filename
character (len=32) :: arg

cfg = parse_cfg(arg)

call cfg%get("particle","Np",Np)
call cfg%get("grid","Nx",Nx)
call cfg%get("grid","Ny",Ny)

Nh = (Nx/2) + 1

allocate(x(Nx))
allocate(y(Ny))

allocate(psi(Nx,Ny))
allocate(omega(Nx,Ny))
allocate(omega_dum(Nx,Ny))
allocate(ux(Nx,Ny))
allocate(uy(Nx,Ny))
allocate(ux_dum(Nx,Ny))
allocate(uy_dum(Nx,Ny))

allocate(omegak(Nh,Ny))
allocate(omegak_dum(Nh,Ny))
allocate(omegak_new(Nh,Ny))
allocate(dt_omegak_old(Nh,Ny))
allocate(dt_omegak_new(Nh,Ny))
allocate(psik(Nh,Ny))
allocate(psik_dum(Nh,Ny))
allocate(ukx(Nh,Ny))
allocate(uky(Nh,Ny))
allocate(ukx_dum(Nh,Ny))
allocate(uky_dum(Nh,Ny))

open(unit=10,file='output/Initial_Condition_Omega_REP.dat',status='unknown')
open(unit=15,file='output/Initial_Condition_Velocity.dat',status='unknown')
open(unit=40,file='output/Energy.dat',status='unknown')

!===================== USER INPUTS ============================================

call cfg%get("length","Lx",Lx)
call cfg%get("length","Ly",Ly)

call cfg%get("resistivity","nu",nu)

call cfg%get("time","time_min",time_min)
call cfg%get("time","time_max",time_max)
call cfg%get("time","dt",dt)

call cfg%get("initialProfile","W0",W0)
call cfg%get("initialProfile","m0",m)
call cfg%get("initialProfile","d",d)

Lx = Lx*pi
Ly = Ly*pi

dx = Lx/dfloat(Nx)
dy = Ly/dfloat(Ny)

d = d*pi

alpha = 1.0d0
lambda = 10.0d0

do i = 1, Nx
  x(i)=0.0d0+real(i-1)*dx
  do j = 1, Ny
    y(j)=0.0+real(j-1)*dy
    ux(i,j) = 0.0d0
    uy(i,j) = 0.0d0
    !read(25,*) x(i),y(j),omega(i,j)
    !omega(i,j) = 2.0d0*sin(x(i))*cos(y(j))
    !read(50,*) x(i),y(j),omega(i,j)
    omega(i,j) = W0/dcosh((y(j)+0.50d0*pi)/d)**2.0-W0/dcosh((y(j)-0.50d0*pi)/d)**2.0
    omega(i,j) = omega(i,j) - W0/dcosh((y(j)+1.50d0*pi)/d)**2.0+W0/dcosh((y(j)-1.50d0*pi)/d)**2.0
    omega(i,j) = omega(i,j)+0.01*dcos(m*x(i))
    omega_dum(i,j) = omega(i,j)
    write(15,*) x(i),y(j),omega(i,j),ux(i,j),uy(i,j)
  end do
end do

!===================== INITIAL TIME DATA ===============================================

  call dfftw_plan_dft_r2c_2d_ (plan_forward, Nx, Ny, omega_dum, omegak, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_forward)
  call dfftw_destroy_plan_ (plan_forward)

do i = 1,Nx/2+1
  do j = 1,Ny/2
    kx = 2.0d0*pi*dfloat(i-1)/Lx
    ky = 2.0d0*pi*dfloat(j-1)/Ly
      if (i == 1 .and. j == 1) then
      psik(i,j) = (0.0d0,0.0d0)
      ukx(i,j) = (0.0d0,0.0d0)
      uky(i,j) = (0.0d0,0.0d0)
      else
      psik(i,j) = omegak(i,j)/( kx*kx + ky*ky )
      ukx(i,j) = + (0.0d0,1.0d0) * ky * psik(i,j)
      uky(i,j) = - (0.0d0,1.0d0) * kx * psik(i,j)
      endif
    psik_dum(i,j) = psik(i,j)
    omegak_dum(i,j) = omegak(i,j)
    ukx_dum(i,j) = ukx(i,j)
    uky_dum(i,j) = uky(i,j)
  enddo
  do j = Ny/2+1,Ny
    kx = 2.0d0*pi*dfloat(i-1)/Lx
    ky = 2.0d0*pi*dfloat((j-1)-Ny)/Ly
    psik(i,j) = omegak(i,j)/( kx*kx + ky*ky )
    ukx(i,j) = + (0.0d0,1.0d0) * ky * psik(i,j)
    uky(i,j) = - (0.0d0,1.0d0) * kx * psik(i,j)
    psik_dum(i,j) = psik(i,j)
    omegak_dum(i,j) = omegak(i,j)
    ukx_dum(i,j) = ukx(i,j)
    uky_dum(i,j) = uky(i,j)
  enddo
enddo

  call dfftw_plan_dft_c2r_2d_ (plan_backward, Nx, Ny, psik_dum, psi, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_backward)
  call dfftw_destroy_plan_ (plan_backward)

  call dfftw_plan_dft_c2r_2d_ (plan_backward, Nx, Ny, omegak_dum, omega, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_backward)
  call dfftw_destroy_plan_ (plan_backward)

  call dfftw_plan_dft_c2r_2d_ (plan_backward, Nx, Ny, ukx_dum, ux, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_backward)
  call dfftw_destroy_plan_ (plan_backward)

  call dfftw_plan_dft_c2r_2d_ (plan_backward, Nx, Ny, uky_dum, uy, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_backward)
  call dfftw_destroy_plan_ (plan_backward)

Energy0 = 0.0d0
 C0 = 0.0d0
 C1 = 0.0d0
 C2 = 0.0d0
 C3 = 0.0d0

do i = 1, Nx
  do j = 1, Ny
  psi(i,j) = psi(i,j)/(dfloat(Nx)*dfloat(Ny))
  omega(i,j) = omega(i,j)/(dfloat(Nx)*dfloat(Ny))
  ux(i,j) = ux(i,j)/(dfloat(Nx)*dfloat(Ny))
  uy(i,j) = uy(i,j)/(dfloat(Nx)*dfloat(Ny))
  Energy0 = Energy0 + (ux(i,j)**2 + uy(i,j)**2)
  yEnergy0 = yEnergy0 + (uy(i,j)**2)
  C0 = C0 + omega(i,j)**0.0d0
  C1 = C1 + omega(i,j)**1.0d0
  C2 = C2 + omega(i,j)**2.0d0
  C3 = C3 + omega(i,j)**3.0d0
  write(10,*) x(i),y(j),omega(i,j),psi(i,j),ux(i,j),uy(i,j)
  end do
end do

!write(40,*) 0, Energy0, yEnergy0, C0,C1,C2,C3

!======================= MAIN PROGRAM =====================================================

do time = time_min,time_max,dt

t = nint(time/dt) - int(time_min/dt)

!if (mod(t,100) == 0) then
!print*, "time = ", time!, t/100
!endif

!====================================================================================
  call derive2css (Nx,Ny,Nh,alpha,nu,time,omegak,omegak_new,dt_omegak_old,dt_omegak_new)
!  call rk42css (Nx,Ny,Nh,alpha,nu,time,dt,omegak,omegak_new,dt_omegak_old,dt_omegak_new)
  call ab2css (Nx,Ny,Nh,alpha,nu,time,dt,omegak,omegak_new,dt_omegak_old,dt_omegak_new)
!====================================================================================

do i = 1,Nx/2+1
  do j = 1,Ny
    dt_omegak_old(i,j) = dt_omegak_new(i,j)
    omegak(i,j) = omegak_new(i,j)
  enddo
enddo

do i = 1,Nx/2+1
  do j = 1,Ny/2
    kx = 2.0d0*pi*dfloat(i-1)/Lx
    ky = 2.0d0*pi*dfloat(j-1)/Ly
      if (i == 1 .and. j == 1) then
      psik(i,j) = (0.0d0,0.0d0)
      ukx(i,j) = (0.0d0,0.0d0)
      uky(i,j) = (0.0d0,0.0d0)
      else
      psik(i,j) = omegak(i,j)/( kx*kx + ky*ky + lambda)
      ukx(i,j) = + (0.0d0,1.0d0) * ky * psik(i,j)
      uky(i,j) = - (0.0d0,1.0d0) * kx * psik(i,j)
      endif
    omegak_dum(i,j) = (kx*kx + ky*ky) * psik(i,j)
    ukx_dum(i,j) = ukx(i,j)
    uky_dum(i,j) = uky(i,j)
  enddo
  do j = Ny/2+1,Ny
    kx = 2.0d0*pi*dfloat(i-1)/Lx
    ky = 2.0d0*pi*dfloat((j-1)-Ny)/Ly
    psik(i,j) = omegak(i,j)/( kx*kx + ky*ky )
    ukx(i,j) = + (0.0d0,1.0d0) * ky * psik(i,j)
    uky(i,j) = - (0.0d0,1.0d0) * kx * psik(i,j)
    omegak_dum(i,j) = (kx*kx + ky*ky) * psik(i,j)
    ukx_dum(i,j) = ukx(i,j)
    uky_dum(i,j) = uky(i,j)
  enddo
enddo

  call dfftw_plan_dft_c2r_2d_ (plan_backward, Nx, Ny, omegak_dum, omega, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_backward)
  call dfftw_destroy_plan_ (plan_backward)

  call dfftw_plan_dft_c2r_2d_ (plan_backward, Nx, Ny, ukx_dum, ux, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_backward)
  call dfftw_destroy_plan_ (plan_backward)

  call dfftw_plan_dft_c2r_2d_ (plan_backward, Nx, Ny, uky_dum, uy, FFTW_ESTIMATE)
  call dfftw_execute_ (plan_backward)
  call dfftw_destroy_plan_ (plan_backward)

Energy = 0.0d0
 C0 = 0.0d0
 C1 = 0.0d0
 C2 = 0.0d0
 C3 = 0.0d0

do i = 1,Nx
  do j = 1,Ny
  omega(i,j) = omega(i,j)/(dfloat(Nx)*dfloat(Ny))
  ux(i,j) = ux(i,j)/(dfloat(Nx)*dfloat(Ny))
  uy(i,j) = uy(i,j)/(dfloat(Nx)*dfloat(Ny))
  Energy = Energy + (ux(i,j)**2 + uy(i,j)**2)
  yEnergy = yEnergy + (uy(i,j)**2)
  C0 = C0 + omega(i,j)**0.0d0
  C1 = C1 + omega(i,j)**1.0d0
  C2 = C2 + omega(i,j)**2.0d0
  C3 = C3 + omega(i,j)**3.0d0
    if (mod(dfloat(t),100.0) == 0.0) then
      write(filename, '("output/fort.",I8.8)') t+100
      open(unit=t+100,file=filename,status='unknown')
      write(t+100,*) x(i),y(j),ux(i,j),uy(i,j),omega(i,j)
    endif
  enddo
enddo
    if (mod(dfloat(t),100.0) == 0.0) then
    close(t+100)
    endif

WRITE(40,*) time,Energy, yEnergy!,C0,C1,C2,C3

enddo ! time

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

end subroutine tara2dScrSer


cc Copyright (C) 2004-2009: Leslie Greengard and June-Yub Lee
cc Contact: greengard@cims.nyu.edu
cc
cc This software is being released under a FreeBSD license
cc (see license.txt in this directory).
cc
************************************************************************
      subroutine dirft4d1(nj,xj,yj,zj,pj,cj,iflag,ms,mt,mu,mv,fk)
      implicit none
      integer nj, iflag, ms, mt, mu, mv
      real*8 xj(nj), yj(nj), zj(nj), pj(nj)
      complex*16 cj(nj),fk(-ms/2:(ms-1)/2,-mt/2:(mt-1)/2,-mu/2:(mu-1)/2,
     $           -mv/2:(mv-1)/2)
c ----------------------------------------------------------------------
c     direct computation of nonuniform FFT
c
c                        nj
c     fk(k1,k2,k3,k4) =  SUM cj(j) exp(+/-i k1 xj(j)) *
c                        j=1       exp(+/-i k2 yj(j)) *
c                                  exp(+/-i k3 zj(j)) *
c                                  exp(+/-i k4 pj(j))
c
c     for -ms/2 <= k1 <= (ms-1)/2,
c         -mt/2 <= k2 <= (mt-1)/2
c         -mu/2 <= k3 <= (mu-1)/2
c         -mv/2 <= k4 <= (mv-1)/2
c
c     If (iflag .ge.0) the + sign is used in the exponential.
c     If (iflag .lt.0) the - sign is used in the exponential.
c
************************************************************************
      integer j, k1, k2, k3, k4
      complex*16 zf,cm1,cm2,cm3,z1n(-ms/2:(ms-1)/2),
     $          z2n(-mt/2:(mt-1)/2),z3n(-mu/2:(mu-1)/2)
c
      do k4 = -mv/2, (mv-1)/2
         do k3 = -mu/2, (mu-1)/2
            do k2 = -mt/2, (mt-1)/2
               do k1 = -ms/2, (ms-1)/2
                  fk(k1,k2,k3,k4) = dcmplx(0d0,0d0)
               enddo
            enddo
         enddo
      enddo
c
      do j = 1, nj
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k1 xj)
c     ----------------------------------------------------------
c
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(xj(j)),+dsin(xj(j)))
         else
            zf = dcmplx(dcos(xj(j)),-dsin(xj(j)))
         endif
         z1n(0) = (1d0,0d0)
         do k1 = 1, (ms-1)/2
            z1n(k1) = zf*z1n(k1-1)
            z1n(-k1)= dconjg(z1n(k1))
         enddo
         if (ms/2*2.eq.ms) z1n(-ms/2) = dconjg(zf*z1n(ms/2-1))
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k2 yj)
c     ----------------------------------------------------------
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(yj(j)),+dsin(yj(j)))
         else
            zf = dcmplx(dcos(yj(j)),-dsin(yj(j)))
         endif
         z2n(0) = (1d0,0d0)
         do k1 = 1, (mt-1)/2
            z2n(k1) = zf*z2n(k1-1)
            z2n(-k1)= dconjg(z2n(k1))
         enddo
         if (mt/2*2.eq.mt) z2n(-mt/2) = dconjg(zf*z2n(mt/2-1))
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k3 zj)
c     ----------------------------------------------------------
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(zj(j)),+dsin(zj(j)))
         else
            zf = dcmplx(dcos(zj(j)),-dsin(zj(j)))
         endif
         z3n(0) = (1d0,0d0)
         do k1 = 1, (mu-1)/2
            z3n(k1) = zf*z3n(k1-1)
            z3n(-k1)= dconjg(z3n(k1))
         enddo
         if (mu/2*2.eq.mu) z3n(-mu/2) = dconjg(zf*z3n(mu/2-1))
c
c     ----------------------------------------------------------
c     Loop over k4 for pj
c     ----------------------------------------------------------
c
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(pj(j)),+dsin(pj(j)))
         else
            zf = dcmplx(dcos(pj(j)),-dsin(pj(j)))
         endif
c
         cm3 = cj(j)
         do k4 = 0, (mv-1)/2
            do k3 = -mu/2, (mu-1)/2
               cm2 = cm3 * z3n(k3)
               do k2 = -mt/2, (mt-1)/2
                  cm1 = cm2 * z2n(k2)
                  do k1 = -ms/2, (ms-1)/2
                     fk(k1,k2,k3,k4) = fk(k1,k2,k3,k4)
     &                               + cm1 * z1n(k1)
                  enddo
               enddo
            enddo
            cm3 = zf*cm3
         enddo
c
         zf = dconjg(zf)
         cm3 = cj(j)
         do k4 = -1, -mv/2, -1
            cm3 = zf*cm3
            do k3 = -mu/2, (mu-1)/2
                cm2 = cm3 * z3n(k3)
                do k2 = -mt/2, (mt-1)/2
                    cm1 = cm2 * z2n(k2)
                    do k1 = -ms/2, (ms-1)/2
                        fk(k1,k2,k3,k4) = fk(k1,k2,k3,k4)
     &                                  + cm1 * z1n(k1)
                    enddo
                enddo
            enddo
        enddo

      enddo
      end
c
c
c
c
c
************************************************************************
      subroutine dirft4d2(nj,xj,yj,zj,pj,cj,iflag,ms,mt,mu,mv,fk)
      implicit none
      integer nj, iflag, ms, mt, mu, mv
      real*8 xj(nj), yj(nj), zj(nj), pj(nj)
      complex*16 cj(nj),fk(-ms/2:(ms-1)/2,-mt/2:(mt-1)/2,-mu/2:(mu-1)/2,
     $           -mv/2:(mv-1)/2)
c ----------------------------------------------------------------------
c     direct computation of nonuniform FFT
c
c
c     cj(j) = SUM SUM SUM SUM  fk(k1,k2,k3,k4) exp(+/-i k1 xj(j)) *
c             k1  k2  k3  k4                   exp(+/-i k2 yj(j)) *
c                                              exp(+/-i k3 zj(j)) *
c                                              exp(+/-i k4 pj(j))
c
c                            for j = 1,...,nj
c
c     where -ms/2 <= k1 <= (ms-1)/2
c           -mt/2 <= k2 <= (mt-1)/2
c           -mu/2 <= k3 <= (mu-1)/2
c           -mv/2 <= k4 <= (mv-1)/2
c
c
c     If (iflag .ge.0) the + sign is used in the exponential.
c     If (iflag .lt.0) the - sign is used in the exponential.
c
************************************************************************
      integer j, k1, k2, k3, k4
      complex*16 zf, cm1,cm2,cm3,cm4,z1n(-ms/2:(ms-1)/2),
     $               z2n(-mt/2:(mt-1)/2),z3n(-mu/2:(mu-1)/2)
c
      do j = 1, nj
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k1 xj)
c     ----------------------------------------------------------
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(xj(j)),+dsin(xj(j)))
         else
            zf = dcmplx(dcos(xj(j)),-dsin(xj(j)))
         endif
         z1n(0) = (1d0,0d0)
         do k1 = 1, (ms-1)/2
            z1n(k1) = zf*z1n(k1-1)
            z1n(-k1)= dconjg(z1n(k1))
         enddo
         if (ms/2*2.eq.ms) z1n(-ms/2) = dconjg(zf*z1n(ms/2-1))
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k2 yj)
c     ----------------------------------------------------------
c
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(yj(j)),+dsin(yj(j)))
         else
            zf = dcmplx(dcos(yj(j)),-dsin(yj(j)))
         endif
         z2n(0) = (1d0,0d0)
         do k2 = 1, (mt-1)/2
            z2n(k2) = zf*z2n(k2-1)
            z2n(-k2)= dconjg(z2n(k2))
         enddo
         if (mt/2*2.eq.mt) z2n(-mt/2) = dconjg(zf*z2n(mt/2-1))
c
c     ----------------------------------------------------------
c     Precompute exponential for exp(+/-i k3 zj)
c     ----------------------------------------------------------
c
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(zj(j)),+dsin(zj(j)))
         else
            zf = dcmplx(dcos(zj(j)),-dsin(zj(j)))
         endif
         z3n(0) = (1d0,0d0)
         do k3 = 1, (mu-1)/2
            z3n(k3) = zf*z3n(k3-1)
            z3n(-k3)= dconjg(z3n(k3))
         enddo
         if (mu/2*2.eq.mu) z3n(-mu/2) = dconjg(zf*z3n(mu/2-1))
c
c     ----------------------------------------------------------
c     Loop over k4 for pj
c     ----------------------------------------------------------
         if (iflag .ge. 0) then
            zf = dcmplx(dcos(pj(j)),+dsin(pj(j)))
         else
            zf = dcmplx(dcos(pj(j)),-dsin(pj(j)))
         endif
c
         cm3 = (0d0, 0d0)
         do k3 = -mu/2, (mu-1)/2
            cm2 = (0d0, 0d0)
            do k2 = -mt/2, (mt-1)/2
                cm1 = (0d0, 0d0)
                do k1 = -ms/2, (ms-1)/2
                    cm1 = cm1 + z1n(k1) * fk(k1,k2,k3,0)
                enddo
                cm2 = cm2 + z2n(k2) * cm1
            enddo
            cm3 = cm3 + z3n(k3) * cm2
         enddo
         cj(j) = cm3
c
         cm4 = zf
         do k4 = 1, (mv-1)/2
            cm3 = (0d0, 0d0)
            do k3 = -mu/2, (mu-1)/2
                cm2 = (0d0, 0d0)
                do k2 = -mt/2, (mt-1)/2
                    cm1 = (0d0, 0d0)
                    do k1 = -ms/2, (ms-1)/2
                        cm1 = cm1 + z1n(k1) * fk(k1,k2,k3,k4)
                    enddo
                    cm2 = cm2 + z2n(k2) * cm1
                enddo
                cm3 = cm3 + z3n(k3) * cm2
            enddo
            cj(j) = cj(j) + cm4 * cm3

            cm3 = (0d0, 0d0)
            do k3 = -mu/2, (mu-1)/2
                cm2 = (0d0, 0d0)
                do k2 = -mt/2, (mt-1)/2
                    cm1 = (0d0, 0d0)
                    do k1 = -ms/2, (ms-1)/2
                        cm1 = cm1 + z1n(k1) * fk(k1,k2,k3,-k4)
                    enddo
                    cm2 = cm2 + z2n(k2) * cm1
                enddo
                cm3 = cm3 + z3n(k3) * cm2
            enddo
            cj(j) = cj(j) + dconjg(cm4) * cm3
            cm4 = cm4*zf
        enddo
c
         if (mv/2*2.eq.mv) then
            cm3 = (0d0, 0d0)
            do k3 = -mu/2, (mu-1)/2
               cm2 = (0d0, 0d0)
                  do k2 = -mt/2, (mt-1)/2
                    cm1 = (0d0, 0d0)
                    do k1 = -ms/2, (ms-1)/2
                        cm1 = cm1 + z1n(k1) * fk(k1,k2,k3,-mv/2)
                    enddo
                    cm2 = cm2 + z2n(k2) * cm1
                enddo
                cm3 = cm3 + z3n(k3) * cm2
            enddo
            cj(j) = cj(j) + dconjg(cm4) * cm3
         endif
      enddo
      end
c
c
c
c
c
************************************************************************
      subroutine dirft4d3(nj,xj,yj,zj,pj,cj,iflag,nk,sk,tk,uk,vk,fk)
      implicit none
      integer nj,iflag,nk
      real*8 xj(nj),yj(nj),zj(nj),pj(nj),sk(nk),tk(nk),uk(nk),vk(nk)
      complex*16 cj(nj),fk(nk)
c ----------------------------------------------------------------------
c     direct computation of nonuniform FFT
c
c              nj
c     fk(k) = SUM cj(j) exp(+/-i s(k) xj(j)) *
c             j=1       exp(+/-i t(k) yj(j)) *
c                       exp(+/-i u(k) zj(j)) *
c                       exp(+/-i v(k) pj(j))
c
c                    for k = 1, ..., nk
c
c     If (iflag .ge.0) the + sign is used in the exponential.
c     If (iflag .lt.0) the - sign is used in the exponential.
c
************************************************************************
      integer k, j
      real*8 ssk, stk, suk, svk
c
      do k = 1, nk
         if (iflag .ge. 0) then
            ssk =  sk(k)
            stk =  tk(k)
            suk =  uk(k)
            svk =  vk(k)
         else
            ssk =  -sk(k)
            stk =  -tk(k)
            suk =  -uk(k)
            svk =  -vk(k)
         endif
c
         fk(k) = dcmplx(0d0,0d0)
         do j = 1, nj
            fk(k) = fk(k) + cj(j) * dcmplx
     &        ( dcos(ssk*xj(j)+stk*yj(j)+suk*zj(j)+svk*pj(j)),
     &          dsin(ssk*xj(j)+stk*yj(j)+suk*zj(j)+svk*pj(j)) )
         enddo
      enddo
      end


      REAL FUNCTION W3FA09 (TK)
C$$$  SUBROUTINE DOCUMENTATION BLOCK ***
C
C SUBR: W3FA09   - TEMPERATURE TO SATURATION VAPOR PRESSURE
C   AUTHOR: CHASE, P.          ORG: W345          DATE:    OCT 78
C   UPDATE: JONES, R.E.        ORG: W342          DATE: 26 JUN 84
C
C ABSTRACT: COMPUTES SATURATION VAPOR PRESSURE IN KILOPASCALS GIVEN
C   TEMPERATAURE IN KELVINS.
C
C PROGRAM HISTORY LOG:
C   78-10-01  P.CHASE
C   84-06-26  R.E.JONES  CHANGE TO IBM VS FORTRAN
C   84-06-26  R.E.JONES  CHANGE TO MICROSOFT FORTRAN 4.10
C   90-06-08  R.E.JONES  CHANGE TO SUN FORTRAN 1.3
C   91-03-29  R.E.JONES  CONVERT TO SiliconGraphic FORTRAN
C   93-03-29  R.E.JONES  ADD SAVE STATEMENT
C   95-09-25  R.E.JONES  CHANGE TK TO CRAY 64 BIT REAL, CHANGE DOUBLE
C                        PRECISION TO CRAY 64 BIT REAL.
C
C USAGE:  VP = W3FA09 (TK)
C
C   INPUT:
C     'TK' - REAL*8 TEMPERATURE IN KELVINS. IF TK < 223.16, THE VALUE
C            223.16 WILL BE USED.  IF TK > 323.16, THE VALUE 323.16
C            WILL BE USED AS THE ARGUMENT. 'TK' ITSELF IS UNCHANGED.
C   OUTPUT:
C     'VP' - SATURATION VAPOR PRESSURE IN KILOPASCALS.
C            0.0063558 < VP < 12.3395
C
C   NOTES: W3FA09 MAY BE DECLARED REAL*8 SO THAT A REAL*8 VALUE IS
C     RETURNED, BUT NO INCREASE IN ACCURACY IS IMPLIED.
C
C   EXIT STATES: NONE
C
C   EXTERNAL REFERENCES: NONE
C
C ATTRIBUTES:
C   LANGUAGE: CRAY CFT77 FORTRAN
C   MACHINE:  CRAY C916/256, J916/2048
C
C$$$
C
C THE CHEBYSHEV COEFFICIENTS ARE IN ARRAY C, LOW-ORDER TERM FIRST.
C
      REAL   C(9)
      REAL   ARG,H0,H1,H2
C
      SAVE
C
      DATA  C     /
     & 0.313732865927E+01, 0.510038215244E+01, 0.277816535655E+01,
     & 0.102673379933E+01, 0.254577145215E+00, 0.396055201295E-01,
     & 0.292209288468E-02,-0.119497199712E-03,-0.352745603496E-04/
C
C SCALE TK TO RANGE -2, +2 FOR SERIES EVALUATION.  INITIALIZE TERMS.
C
      ARG = -1.09264E1+4.0E-2*AMAX1(223.16,AMIN1(323.16,TK))
      H0  = 0.0
      H1  = 0.0
C
C EVALUATE CHEBYSHEV POLYNOMIAL
C
      DO 10 I=1,9
        H2 = H1
        H1 = H0
        H0 = ARG * H1 - H2 + C(10-I)
 10   CONTINUE
      W3FA09 = 0.5 * (C(1) - H2 + H0)
      RETURN
      END


!==========================================================================
elemental function gsw_sound_speed_ice (t, p)
!==========================================================================
!
!  Calculates the compression speed of sound in ice.
!
!  t   =  in-situ temperature (ITS-90)                            [ deg C ]
!  p   =  sea pressure                                             [ dbar ]
!         ( i.e. absolute pressure - 10.1325 dbar )
!
!  sound_speed_ice  =  compression speed of sound in ice            [ m/s ]
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_gibbs_ice

use gsw_mod_kinds

implicit none

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

real (r8) :: gsw_sound_speed_ice

real (r8) :: gi_tp, gi_tt

gi_tt = gsw_gibbs_ice(2,0,t,p)
gi_tp = gsw_gibbs_ice(1,1,t,p)

gsw_sound_speed_ice = gsw_gibbs_ice(0,1,t,p) * &
               sqrt(gi_tt/(gi_tp*gi_tp - gi_tt*gsw_gibbs_ice(0,2,t,p)))

return
end function

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


module NumericalMethods

use parameters
use constants
use derivatives

implicit none

private adaptive_shrink, adaptive_grow

public rk_geodesic
contains

subroutine rk_geodesic(y,c,b)
!Arguments
! http://astroa.physics.metu.edu.tr/MANUALS/intel_ifc/mergedProjects/optaps_for/fortran/optaps_prg_arrs_f.htm
real(kind=dp), dimension(:) :: y !Input vector of general size.
real(kind=dp), dimension(:) :: c !Some constants and stepsizes.
real(kind=dp), dimension(:) :: b ! Extras for backwards RT
!Other
real(kind=dp), dimension(size(y)) :: y1,y2,y3,y4,y5,y6
real(kind=dp), dimension(size(y)) :: dy1,dy2,dy3,dy4,dy5,dy6
real(kind=dp), dimension(size(y)) :: k1,k2,k3,k4,k5,k6
real(kind=dp), dimension(size(y)) :: ynew, yerr,deltaErr, yscal, ratio
real(kind=dp) :: h, errmax,xOUT, xT, rT,dx

!Define some stuff from arrays
xT = b(1) ! X coordinate of the target point
rT = b(2) ! R coordinate. !Dont actually use this!

!Set the stepsize
h = c(4)

11 continue

! Y1
y1 = y
call geodesic(y1,c,dy1)
k1 = h * dy1

!Y2
y2 = y1 + B21*k1
call geodesic(y2,c,dy2)
k2 = h * dy2

!Y3
y3 = y1 + B31*k1 + B32*k2
call geodesic(y3,c,dy3)
k3 = h * dy3

!stop

!Y4
y4 = y1 + B41*k1 + B42*k2 + B43*k3
call geodesic(y4,c,dy4)
k4 = h * dy4

!Y5
y5 = y1 + B51*k1 + B52*k2 + B53*k3 + B54*k4
call geodesic(y5,c,dy5)
k5 = h * dy5

!Y6
y6 = y1 + B61*k1 + B62*k2 + B63*k3 + B64*k4 + B65*k5
call geodesic(y6,c,dy6)
k6 = h * dy6

!Update
ynew = y1 + c1*k1  + c3*k3 + c4*k4  +c6*k6
yerr = y1 + cbar1*k1 + cbar3*k3 + cbar4*k4 + cbar5*k5 + cbar6*k6

deltaErr = abs(ynew - yerr)

yscal = abs(y1) + abs(k1) + 1.0d-3
ratio = deltaErr/yscal
errmax = escal * maxval(ratio)

if (RayTracingDirection .EQ. -1.0_dp) then

    !Backwards RT
        xOUT = sqrt(ynew(1)**2 + a**2)*sin(ynew(2))*cos(ynew(3))
        dx = xOUT - xT

        if (abs(dx) .LT. dx_eps) then

 !       print *, 'Good!', dx, xT

        b(2) = 1.0_dp
        y = ynew
        !Leave this subroutine
        return
        endif

        if (dx .LT. 0.0_dp) then
        h = h*0.50_dp
        goto 11
        endif

endif

if (errmax .GT. 1) then
!The error is too big. Reduce the step size and exit without updating the variable vector
call adaptive_shrink(errmax,h)
goto 11 !?
else
!The error is OK. Grow the stepsize a little and set the variables for the next integration step

tau = tau + h !Update the proper time for MPD
call adaptive_grow(errmax,h)
y = ynew

endif

c(4) = h !Update stepsize. Only if using adaptive stepsize

end subroutine rk_geodesic

subroutine adaptive_shrink(errmax,dh)
real(kind=dp) :: errmax, htemp,dh

htemp = S*dh*(errmax**PSHRINK)

dh = sign(max(abs(htemp), 0.10_dp*abs(dh)),dh)

end subroutine adaptive_shrink

subroutine adaptive_grow(errmax,dh)
real(kind=dp) errmax,ERRCON,hnext,dh

ERRCON = (5.0_dp/S)**(1.0_dp/PGROW)

!print *, errmax ,ERRCON
if (errmax .GT. ERRCON) then
    hnext = S*dh*errmax**PGROW
 !   print *, 'errcon', S*errmax**PGROW
else
    hnext = 5.0_dp*dh
endif

dh = hnext
END SUBROUTINE adaptive_grow

end module NumericalMethods