      SUBROUTINE RODAS(N,FCN,IFCN,X,Y,XEND,H,
     &                  RTOL,ATOL,ITOL,
     &                  JAC ,IJAC,MLJAC,MUJAC,DFX,IDFX,
     &                  MAS ,IMAS,MLMAS,MUMAS,
     &                  SOLOUT,IOUT,
     &                  WORK,LWORK,IWORK,LIWORK,RPAR,IPAR,IDID)
C ----------------------------------------------------------
C     NUMERICAL SOLUTION OF A STIFF (OR DIFFERENTIAL ALGEBRAIC)
C     SYSTEM OF FIRST 0RDER ORDINARY DIFFERENTIAL EQUATIONS  MY'=F(X,Y).
C     THIS IS AN EMBEDDED ROSENBROCK METHOD OF ORDER (3)4
C     (WITH STEP SIZE CONTROL).
C     C.F. SECTIONS IV.7  AND VI.3
C
C     AUTHORS: E. HAIRER AND G. WANNER
C              UNIVERSITE DE GENEVE, DEPT. DE MATHEMATIQUES
C              CH-1211 GENEVE 24, SWITZERLAND
C              E-MAIL:  Ernst.Hairer@math.unige.ch
C                       Gerhard.Wanner@math.unige.ch
C
C     THIS CODE IS PART OF THE BOOK:
C         E. HAIRER AND G. WANNER, SOLVING ORDINARY DIFFERENTIAL
C         EQUATIONS II. STIFF AND DIFFERENTIAL-ALGEBRAIC PROBLEMS.
C         SPRINGER SERIES IN COMPUTATIONAL MATHEMATICS 14,
C         SPRINGER-VERLAG 1991, SECOND EDITION 1996.
C
C     VERSION OF OCTOBER 28, 1996
C
C     INPUT PARAMETERS
C     ----------------
C     N           DIMENSION OF THE SYSTEM
C
C     FCN         NAME (EXTERNAL) OF SUBROUTINE COMPUTING THE
C                 VALUE OF F(X,Y):
C                    SUBROUTINE FCN(N,X,Y,F,RPAR,IPAR)
C                    DOUBLE PRECISION X,Y(N),F(N)
C                    F(1)=...   ETC.
C                 RPAR, IPAR (SEE BELOW)
C
C     IFCN        GIVES INFORMATION ON FCN:
C                    IFCN=0: F(X,Y) INDEPENDENT OF X (AUTONOMOUS)
C                    IFCN=1: F(X,Y) MAY DEPEND ON X (NON-AUTONOMOUS)
C
C     X           INITIAL X-VALUE
C
C     Y(N)        INITIAL VALUES FOR Y
C
C     XEND        FINAL X-VALUE (XEND-X MAY BE POSITIVE OR NEGATIVE)
C
C     H           INITIAL STEP SIZE GUESS;
C                 FOR STIFF EQUATIONS WITH INITIAL TRANSIENT,
C                 H=1.D0/(NORM OF F'), USUALLY 1.D-2 OR 1.D-3, IS GOOD.
C                 THIS CHOICE IS NOT VERY IMPORTANT, THE CODE QUICKLY
C                 ADAPTS ITS STEP SIZE (IF H=0.D0, THE CODE PUTS H=1.D-6).
C
C     RTOL,ATOL   RELATIVE AND ABSOLUTE ERROR TOLERANCES. THEY
C                 CAN BE BOTH SCALARS OR ELSE BOTH VECTORS OF LENGTH N.
C
C     ITOL        SWITCH FOR RTOL AND ATOL:
C                   ITOL=0: BOTH RTOL AND ATOL ARE SCALARS.
C                     THE CODE KEEPS, ROUGHLY, THE LOCAL ERROR OF
C                     Y(I) BELOW RTOL*ABS(Y(I))+ATOL
C                   ITOL=1: BOTH RTOL AND ATOL ARE VECTORS.
C                     THE CODE KEEPS THE LOCAL ERROR OF Y(I) BELOW
C                     RTOL(I)*ABS(Y(I))+ATOL(I).
C
C     JAC         NAME (EXTERNAL) OF THE SUBROUTINE WHICH COMPUTES
C                 THE PARTIAL DERIVATIVES OF F(X,Y) WITH RESPECT TO Y
C                 (THIS ROUTINE IS ONLY CALLED IF IJAC=1; SUPPLY
C                 A DUMMY SUBROUTINE IN THE CASE IJAC=0).
C                 FOR IJAC=1, THIS SUBROUTINE MUST HAVE THE FORM
C                    SUBROUTINE JAC(N,X,Y,DFY,LDFY,RPAR,IPAR)
C                    DOUBLE PRECISION X,Y(N),DFY(LDFY,N)
C                    DFY(1,1)= ...
C                 LDFY, THE COLOMN-LENGTH OF THE ARRAY, IS
C                 FURNISHED BY THE CALLING PROGRAM.
C                 IF (MLJAC.EQ.N) THE JACOBIAN IS SUPPOSED TO
C                    BE FULL AND THE PARTIAL DERIVATIVES ARE
C                    STORED IN DFY AS
C                       DFY(I,J) = PARTIAL F(I) / PARTIAL Y(J)
C                 ELSE, THE JACOBIAN IS TAKEN AS BANDED AND
C                    THE PARTIAL DERIVATIVES ARE STORED
C                    DIAGONAL-WISE AS
C                       DFY(I-J+MUJAC+1,J) = PARTIAL F(I) / PARTIAL Y(J).
C
C     IJAC        SWITCH FOR THE COMPUTATION OF THE JACOBIAN:
C                    IJAC=0: JACOBIAN IS COMPUTED INTERNALLY BY FINITE
C                       DIFFERENCES, SUBROUTINE "JAC" IS NEVER CALLED.
C                    IJAC=1: JACOBIAN IS SUPPLIED BY SUBROUTINE JAC.
C
C     MLJAC       SWITCH FOR THE BANDED STRUCTURE OF THE JACOBIAN:
C                    MLJAC=N: JACOBIAN IS A FULL MATRIX. THE LINEAR
C                       ALGEBRA IS DONE BY FULL-MATRIX GAUSS-ELIMINATION.
C                    0<=MLJAC<N: MLJAC IS THE LOWER BANDWITH OF JACOBIAN
C                       MATRIX (>= NUMBER OF NON-ZERO DIAGONALS BELOW
C                       THE MAIN DIAGONAL).
C
C     MUJAC       UPPER BANDWITH OF JACOBIAN  MATRIX (>= NUMBER OF NON-
C                 ZERO DIAGONALS ABOVE THE MAIN DIAGONAL).
C                 NEED NOT BE DEFINED IF MLJAC=N.
C
C     DFX         NAME (EXTERNAL) OF THE SUBROUTINE WHICH COMPUTES
C                 THE PARTIAL DERIVATIVES OF F(X,Y) WITH RESPECT TO X
C                 (THIS ROUTINE IS ONLY CALLED IF IDFX=1 AND IFCN=1;
C                 SUPPLY A DUMMY SUBROUTINE IN THE CASE IDFX=0 OR IFCN=0).
C                 OTHERWISE, THIS SUBROUTINE MUST HAVE THE FORM
C                    SUBROUTINE DFX(N,X,Y,FX,RPAR,IPAR)
C                    DOUBLE PRECISION X,Y(N),FX(N)
C                    FX(1)= ...
C
C     IDFX        SWITCH FOR THE COMPUTATION OF THE DF/DX:
C                    IDFX=0: DF/DX IS COMPUTED INTERNALLY BY FINITE
C                       DIFFERENCES, SUBROUTINE "DFX" IS NEVER CALLED.
C                    IDFX=1: DF/DX IS SUPPLIED BY SUBROUTINE DFX.
C
C     ----   MAS,IMAS,MLMAS, AND MUMAS HAVE ANALOG MEANINGS      -----
C     ----   FOR THE "MASS MATRIX" (THE MATRIX "M" OF SECTION IV.8): -
C
C     MAS         NAME (EXTERNAL) OF SUBROUTINE COMPUTING THE MASS-
C                 MATRIX M.
C                 IF IMAS=0, THIS MATRIX IS ASSUMED TO BE THE IDENTITY
C                 MATRIX AND NEEDS NOT TO BE DEFINED;
C                 SUPPLY A DUMMY SUBROUTINE IN THIS CASE.
C                 IF IMAS=1, THE SUBROUTINE MAS IS OF THE FORM
C                    SUBROUTINE MAS(N,AM,LMAS,RPAR,IPAR)
C                    DOUBLE PRECISION AM(LMAS,N)
C                    AM(1,1)= ....
C                    IF (MLMAS.EQ.N) THE MASS-MATRIX IS STORED
C                    AS FULL MATRIX LIKE
C                         AM(I,J) = M(I,J)
C                    ELSE, THE MATRIX IS TAKEN AS BANDED AND STORED
C                    DIAGONAL-WISE AS
C                         AM(I-J+MUMAS+1,J) = M(I,J).
C
C     IMAS       GIVES INFORMATION ON THE MASS-MATRIX:
C                    IMAS=0: M IS SUPPOSED TO BE THE IDENTITY
C                       MATRIX, MAS IS NEVER CALLED.
C                    IMAS=1: MASS-MATRIX  IS SUPPLIED.
C
C     MLMAS       SWITCH FOR THE BANDED STRUCTURE OF THE MASS-MATRIX:
C                    MLMAS=N: THE FULL MATRIX CASE. THE LINEAR
C                       ALGEBRA IS DONE BY FULL-MATRIX GAUSS-ELIMINATION.
C                    0<=MLMAS<N: MLMAS IS THE LOWER BANDWITH OF THE
C                       MATRIX (>= NUMBER OF NON-ZERO DIAGONALS BELOW
C                       THE MAIN DIAGONAL).
C                 MLMAS IS SUPPOSED TO BE .LE. MLJAC.
C
C     MUMAS       UPPER BANDWITH OF MASS-MATRIX (>= NUMBER OF NON-
C                 ZERO DIAGONALS ABOVE THE MAIN DIAGONAL).
C                 NEED NOT BE DEFINED IF MLMAS=N.
C                 MUMAS IS SUPPOSED TO BE .LE. MUJAC.
C
C     SOLOUT      NAME (EXTERNAL) OF SUBROUTINE PROVIDING THE
C                 NUMERICAL SOLUTION DURING INTEGRATION.
C                 IF IOUT=1, IT IS CALLED AFTER EVERY SUCCESSFUL STEP.
C                 SUPPLY A DUMMY SUBROUTINE IF IOUT=0.
C                 IT MUST HAVE THE FORM
C                    SUBROUTINE SOLOUT (NR,XOLD,X,Y,CONT,LRC,N,
C                                       RPAR,IPAR,IRTRN)
C                    DOUBLE PRECISION X,Y(N),CONT(LRC)
C                    ....
C                 SOLOUT FURNISHES THE SOLUTION "Y" AT THE NR-TH
C                    GRID-POINT "X" (THEREBY THE INITIAL VALUE IS
C                    THE FIRST GRID-POINT).
C                 "XOLD" IS THE PRECEEDING GRID-POINT.
C                 "IRTRN" SERVES TO INTERRUPT THE INTEGRATION. IF IRTRN
C                    IS SET <0, RODAS RETURNS TO THE CALLING PROGRAM.
C
C          -----  CONTINUOUS OUTPUT: -----
C                 DURING CALLS TO "SOLOUT", A CONTINUOUS SOLUTION
C                 FOR THE INTERVAL [XOLD,X] IS AVAILABLE THROUGH
C                 THE FUNCTION
C                        >>>   CONTRO(I,S,CONT,LRC)   <<<
C                 WHICH PROVIDES AN APPROXIMATION TO THE I-TH
C                 COMPONENT OF THE SOLUTION AT THE POINT S. THE VALUE
C                 S SHOULD LIE IN THE INTERVAL [XOLD,X].
C
C     IOUT        GIVES INFORMATION ON THE SUBROUTINE SOLOUT:
C                    IOUT=0: SUBROUTINE IS NEVER CALLED
C                    IOUT=1: SUBROUTINE IS USED FOR OUTPUT
C
C     WORK        ARRAY OF WORKING SPACE OF LENGTH "LWORK".
C                 SERVES AS WORKING SPACE FOR ALL VECTORS AND MATRICES.
C                 "LWORK" MUST BE AT LEAST
C                             N*(LJAC+LMAS+LE1+14)+20
C                 WHERE
C                    LJAC=N              IF MLJAC=N (FULL JACOBIAN)
C                    LJAC=MLJAC+MUJAC+1  IF MLJAC<N (BANDED JAC.)
C                 AND
C                    LMAS=0              IF IMAS=0
C                    LMAS=N              IF IMAS=1 AND MLMAS=N (FULL)
C                    LMAS=MLMAS+MUMAS+1  IF MLMAS<N (BANDED MASS-M.)
C                 AND
C                    LE1=N               IF MLJAC=N (FULL JACOBIAN)
C                    LE1=2*MLJAC+MUJAC+1 IF MLJAC<N (BANDED JAC.).
C                 IN THE USUAL CASE WHERE THE JACOBIAN IS FULL AND THE
C                 MASS-MATRIX IS THE INDENTITY (IMAS=0), THE MINIMUM
C                 STORAGE REQUIREMENT IS
C                             LWORK = 2*N*N+14*N+20.
C                 IF IWORK(9)=M1>0 THEN "LWORK" MUST BE AT LEAST
C                          N*(LJAC+14)+(N-M1)*(LMAS+LE1)+20
C                 WHERE IN THE DEFINITIONS OF LJAC, LMAS AND LE1 THE
C                 NUMBER N CAN BE REPLACED BY N-M1.
C
C     LWORK       DECLARED LENGTH OF ARRAY "WORK".
C
C     IWORK       INTEGER WORKING SPACE OF LENGTH "LIWORK".
C                 "LIWORK" MUST BE AT LEAST N+20.
C
C     LIWORK      DECLARED LENGTH OF ARRAY "IWORK".
C
C     RPAR, IPAR  REAL AND INTEGER PARAMETERS (OR PARAMETER ARRAYS) WHICH
C                 CAN BE USED FOR COMMUNICATION BETWEEN YOUR CALLING
C                 PROGRAM AND THE FCN, DFX, JAC, MAS, SOLOUT SUBROUTINES.
C
C ----------------------------------------------------------------------
C
C     SOPHISTICATED SETTING OF PARAMETERS
C     -----------------------------------
C              SEVERAL PARAMETERS OF THE CODE ARE TUNED TO MAKE IT WORK
C              WELL. THEY MAY BE DEFINED BY SETTING WORK(1),..,WORK(4)
C              AS WELL AS IWORK(1),IWORK(2) DIFFERENT FROM ZERO.
C              FOR ZERO INPUT, THE CODE CHOOSES DEFAULT VALUES:
C
C    IWORK(1)  THIS IS THE MAXIMAL NUMBER OF ALLOWED STEPS.
C              THE DEFAULT VALUE (FOR IWORK(1)=0) IS 100000.
C
C    IWORK(2)  SWITCH FOR THE CHOICE OF THE COEFFICIENTS
C              IF IWORK(2).EQ.1  METHOD (SEE BOOK, PAGE 452)
C              IF IWORK(2).EQ.2  SAME METHOD WITH DIFFERENT PARAMETERS
C              IF IWORK(2).EQ.3  METHOD WITH COEFF. OF GERD STEINEBACH
C              THE DEFAULT VALUE (FOR IWORK(2)=0) IS IWORK(2)=1.
C
C    IWORK(3)  SWITCH FOR STEP SIZE STRATEGY
C              IF IWORK(3).EQ.1  MOD. PREDICTIVE CONTROLLER (GUSTAFSSON)
C              IF IWORK(3).EQ.2  CLASSICAL APPROACH
C              THE DEFAULT VALUE (FOR IWORK(3)=0) IS IWORK(3)=1.
C
C       IF THE DIFFERENTIAL SYSTEM HAS THE SPECIAL STRUCTURE THAT
C            Y(I)' = Y(I+M2)   FOR  I=1,...,M1,
C       WITH M1 A MULTIPLE OF M2, A SUBSTANTIAL GAIN IN COMPUTERTIME
C       CAN BE ACHIEVED BY SETTING THE PARAMETERS IWORK(9) AND IWORK(10).
C       E.G., FOR SECOND ORDER SYSTEMS P'=V, V'=G(P,V), WHERE P AND V ARE
C       VECTORS OF DIMENSION N/2, ONE HAS TO PUT M1=M2=N/2.
C       FOR M1>0 SOME OF THE INPUT PARAMETERS HAVE DIFFERENT MEANINGS:
C       - JAC: ONLY THE ELEMENTS OF THE NON-TRIVIAL PART OF THE
C              JACOBIAN HAVE TO BE STORED
C              IF (MLJAC.EQ.N-M1) THE JACOBIAN IS SUPPOSED TO BE FULL
C                 DFY(I,J) = PARTIAL F(I+M1) / PARTIAL Y(J)
C                FOR I=1,N-M1 AND J=1,N.
C              ELSE, THE JACOBIAN IS BANDED ( M1 = M2 * MM )
C                 DFY(I-J+MUJAC+1,J+K*M2) = PARTIAL F(I+M1) / PARTIAL Y(J+K*M2)
C                FOR I=1,MLJAC+MUJAC+1 AND J=1,M2 AND K=0,MM.
C       - MLJAC: MLJAC=N-M1: IF THE NON-TRIVIAL PART OF THE JACOBIAN IS FULL
C                0<=MLJAC<N-M1: IF THE (MM+1) SUBMATRICES (FOR K=0,MM)
C                     PARTIAL F(I+M1) / PARTIAL Y(J+K*M2),  I,J=1,M2
C                    ARE BANDED, MLJAC IS THE MAXIMAL LOWER BANDWIDTH
C                    OF THESE MM+1 SUBMATRICES
C       - MUJAC: MAXIMAL UPPER BANDWIDTH OF THESE MM+1 SUBMATRICES
C                NEED NOT BE DEFINED IF MLJAC=N-M1
C       - MAS: IF IMAS=0 THIS MATRIX IS ASSUMED TO BE THE IDENTITY AND
C              NEED NOT BE DEFINED. SUPPLY A DUMMY SUBROUTINE IN THIS CASE.
C              IT IS ASSUMED THAT ONLY THE ELEMENTS OF RIGHT LOWER BLOCK OF
C              DIMENSION N-M1 DIFFER FROM THAT OF THE IDENTITY MATRIX.
C              IF (MLMAS.EQ.N-M1) THIS SUBMATRIX IS SUPPOSED TO BE FULL
C                 AM(I,J) = M(I+M1,J+M1)     FOR I=1,N-M1 AND J=1,N-M1.
C              ELSE, THE MASS MATRIX IS BANDED
C                 AM(I-J+MUMAS+1,J) = M(I+M1,J+M1)
C       - MLMAS: MLMAS=N-M1: IF THE NON-TRIVIAL PART OF M IS FULL
C                0<=MLMAS<N-M1: LOWER BANDWIDTH OF THE MASS MATRIX
C       - MUMAS: UPPER BANDWIDTH OF THE MASS MATRIX
C                NEED NOT BE DEFINED IF MLMAS=N-M1
C
C    IWORK(9)  THE VALUE OF M1.  DEFAULT M1=0.
C
C    IWORK(10) THE VALUE OF M2.  DEFAULT M2=M1.
C
C    WORK(1)   UROUND, THE ROUNDING UNIT, DEFAULT 1.D-16.
C
C    WORK(2)   MAXIMAL STEP SIZE, DEFAULT XEND-X.
C
C    WORK(3), WORK(4)   PARAMETERS FOR STEP SIZE SELECTION
C              THE NEW STEP SIZE IS CHOSEN SUBJECT TO THE RESTRICTION
C                 WORK(3) <= HNEW/HOLD <= WORK(4)
C              DEFAULT VALUES: WORK(3)=0.2D0, WORK(4)=6.D0
C
C    WORK(5)   THE SAFETY FACTOR IN STEP SIZE PREDICTION,
C              DEFAULT 0.9D0.
C
C-----------------------------------------------------------------------
C
C     OUTPUT PARAMETERS
C     -----------------
C     X           X-VALUE WHERE THE SOLUTION IS COMPUTED
C                 (AFTER SUCCESSFUL RETURN X=XEND)
C
C     Y(N)        SOLUTION AT X
C
C     H           PREDICTED STEP SIZE OF THE LAST ACCEPTED STEP
C
C     IDID        REPORTS ON SUCCESSFULNESS UPON RETURN:
C                   IDID= 1  COMPUTATION SUCCESSFUL,
C                   IDID= 2  COMPUT. SUCCESSFUL (INTERRUPTED BY SOLOUT)
C                   IDID=-1  INPUT IS NOT CONSISTENT,
C                   IDID=-2  LARGER NMAX IS NEEDED,
C                   IDID=-3  STEP SIZE BECOMES TOO SMALL,
C                   IDID=-4  MATRIX IS REPEATEDLY SINGULAR.
C
C   IWORK(14)  NFCN    NUMBER OF FUNCTION EVALUATIONS (THOSE FOR NUMERICAL
C                      EVALUATION OF THE JACOBIAN ARE NOT COUNTED)
C   IWORK(15)  NJAC    NUMBER OF JACOBIAN EVALUATIONS (EITHER ANALYTICALLY
C                      OR NUMERICALLY)
C   IWORK(16)  NSTEP   NUMBER OF COMPUTED STEPS
C   IWORK(17)  NACCPT  NUMBER OF ACCEPTED STEPS
C   IWORK(18)  NREJCT  NUMBER OF REJECTED STEPS (DUE TO ERROR TEST),
C                      (STEP REJECTIONS IN THE FIRST STEP ARE NOT COUNTED)
C   IWORK(19)  NDEC    NUMBER OF LU-DECOMPOSITIONS (N-DIMENSIONAL MATRIX)
C   IWORK(20)  NSOL    NUMBER OF FORWARD-BACKWARD SUBSTITUTIONS
C ---------------------------------------------------------
C *** *** *** *** *** *** *** *** *** *** *** *** ***
C          DECLARATIONS
C *** *** *** *** *** *** *** *** *** *** *** *** ***
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION Y(N),ATOL(*),RTOL(*),WORK(LWORK),IWORK(LIWORK)
      DIMENSION RPAR(*),IPAR(*)
      LOGICAL AUTNMS,IMPLCT,JBAND,ARRET,PRED
      EXTERNAL FCN,JAC,DFX,MAS,SOLOUT
C *** *** *** *** *** *** ***
C        SETTING THE PARAMETERS
C *** *** *** *** *** *** ***
      NFCN=0
      NACCPT=0
      NREJCT=0
      NSTEP=0
      NJAC=0
      NDEC=0
      NSOL=0
      ARRET=.FALSE.
C -------- NMAX , THE MAXIMAL NUMBER OF STEPS -----
      IF(IWORK(1).EQ.0)THEN
         NMAX=100000
      ELSE
         NMAX=IWORK(1)
         IF(NMAX.LE.0)THEN
            WRITE(6,*)' WRONG INPUT IWORK(1)=',IWORK(1)
            ARRET=.TRUE.
         END IF
      END IF
C -------- METH   COEFFICIENTS OF THE METHOD
      IF(IWORK(2).EQ.0)THEN
         METH=1
      ELSE
         METH=IWORK(2)
         IF(METH.LE.0.OR.METH.GE.4)THEN
            WRITE(6,*)' CURIOUS INPUT IWORK(2)=',IWORK(2)
            ARRET=.TRUE.
         END IF
      END IF
C -------- PRED   STEP SIZE CONTROL
      IF(IWORK(3).LE.1)THEN
         PRED=.TRUE.
      ELSE
         PRED=.FALSE.
      END IF
C -------- PARAMETER FOR SECOND ORDER EQUATIONS
      M1=IWORK(9)
      M2=IWORK(10)
      NM1=N-M1
      IF (M1.EQ.0) M2=N
      IF (M2.EQ.0) M2=M1
      IF (M1.LT.0.OR.M2.LT.0.OR.M1+M2.GT.N) THEN
       WRITE(6,*)' CURIOUS INPUT FOR IWORK(9,10)=',M1,M2
       ARRET=.TRUE.
      END IF
C -------- UROUND   SMALLEST NUMBER SATISFYING 1.D0+UROUND>1.D0
      IF(WORK(1).EQ.0.D0)THEN
         UROUND=1.D-16
      ELSE
         UROUND=WORK(1)
         IF(UROUND.LT.1.D-16.OR.UROUND.GE.1.D0)THEN
            WRITE(6,*)' COEFFICIENTS HAVE 16 DIGITS, UROUND=',WORK(1)
            ARRET=.TRUE.
         END IF
      END IF
C -------- MAXIMAL STEP SIZE
      IF(WORK(2).EQ.0.D0)THEN
         HMAX=XEND-X
      ELSE
         HMAX=WORK(2)
      END IF
C -------  FAC1,FAC2     PARAMETERS FOR STEP SIZE SELECTION
      IF(WORK(3).EQ.0.D0)THEN
         FAC1=5.D0
      ELSE
         FAC1=1.D0/WORK(3)
      END IF
      IF(WORK(4).EQ.0.D0)THEN
         FAC2=1.D0/6.0D0
      ELSE
         FAC2=1.D0/WORK(4)
      END IF
      IF (FAC1.LT.1.0D0.OR.FAC2.GT.1.0D0) THEN
            WRITE(6,*)' CURIOUS INPUT WORK(3,4)=',WORK(3),WORK(4)
            ARRET=.TRUE.
         END IF
C --------- SAFE     SAFETY FACTOR IN STEP SIZE PREDICTION
      IF (WORK(5).EQ.0.0D0) THEN
         SAFE=0.9D0
      ELSE
         SAFE=WORK(5)
         IF (SAFE.LE.0.001D0.OR.SAFE.GE.1.0D0) THEN
            WRITE(6,*)' CURIOUS INPUT FOR WORK(5)=',WORK(5)
            ARRET=.TRUE.
         END IF
      END IF
C --------- CHECK IF TOLERANCES ARE O.K.
      IF (ITOL.EQ.0) THEN
          IF (ATOL(1).LE.0.D0.OR.RTOL(1).LE.10.D0*UROUND) THEN
              WRITE (6,*) ' TOLERANCES ARE TOO SMALL'
              ARRET=.TRUE.
          END IF
      ELSE
          DO I=1,N
             IF (ATOL(I).LE.0.D0.OR.RTOL(I).LE.10.D0*UROUND) THEN
                WRITE (6,*) ' TOLERANCES(',I,') ARE TOO SMALL'
                ARRET=.TRUE.
             END IF
          END DO
      END IF
C *** *** *** *** *** *** *** *** *** *** *** *** ***
C         COMPUTATION OF ARRAY ENTRIES
C *** *** *** *** *** *** *** *** *** *** *** *** ***
C ---- AUTONOMOUS, IMPLICIT, BANDED OR NOT ?
      AUTNMS=IFCN.EQ.0
      IMPLCT=IMAS.NE.0
      JBAND=MLJAC.LT.NM1
C -------- COMPUTATION OF THE ROW-DIMENSIONS OF THE 2-ARRAYS ---
C -- JACOBIAN AND MATRIX E
      IF(JBAND)THEN
         LDJAC=MLJAC+MUJAC+1
         LDE=MLJAC+LDJAC
      ELSE
         MLJAC=NM1
         MUJAC=NM1
         LDJAC=NM1
         LDE=NM1
      END IF
C -- MASS MATRIX
      IF (IMPLCT) THEN
          IF (MLMAS.NE.NM1) THEN
              LDMAS=MLMAS+MUMAS+1
              IF (JBAND) THEN
                 IJOB=4
              ELSE
                 IJOB=3
              END IF
          ELSE
              LDMAS=NM1
              IJOB=5
          END IF
C ------ BANDWITH OF "MAS" NOT LARGER THAN BANDWITH OF "JAC"
          IF (MLMAS.GT.MLJAC.OR.MUMAS.GT.MUJAC) THEN
              WRITE (6,*) 'BANDWITH OF "MAS" NOT LARGER THAN BANDWITH OF
     & "JAC"'
            ARRET=.TRUE.
          END IF
      ELSE
          LDMAS=0
          IF (JBAND) THEN
             IJOB=2
          ELSE
             IJOB=1
          END IF
      END IF
      LDMAS2=MAX(1,LDMAS)
C ------- PREPARE THE ENTRY-POINTS FOR THE ARRAYS IN WORK -----
      IEYNEW=21
      IEDY1=IEYNEW+N
      IEDY=IEDY1+N
      IEAK1=IEDY+N
      IEAK2=IEAK1+N
      IEAK3=IEAK2+N
      IEAK4=IEAK3+N
      IEAK5=IEAK4+N
      IEAK6=IEAK5+N
      IEFX =IEAK6+N
      IECON=IEFX+N
      IEJAC=IECON+4*N
      IEMAS=IEJAC+N*LDJAC
      IEE  =IEMAS+NM1*LDMAS
C ------ TOTAL STORAGE REQUIREMENT -----------
      ISTORE=IEE+NM1*LDE-1
      IF(ISTORE.GT.LWORK)THEN
         WRITE(6,*)' INSUFFICIENT STORAGE FOR WORK, MIN. LWORK=',ISTORE
         ARRET=.TRUE.
      END IF
C ------- ENTRY POINTS FOR INTEGER WORKSPACE -----
      IEIP=21
      ISTORE=IEIP+NM1-1
      IF(ISTORE.GT.LIWORK)THEN
         WRITE(6,*)' INSUFF. STORAGE FOR IWORK, MIN. LIWORK=',ISTORE
         ARRET=.TRUE.
      END IF
C ------ WHEN A FAIL HAS OCCURED, WE RETURN WITH IDID=-1
      IF (ARRET) THEN
         IDID=-1
         RETURN
      END IF
C -------- CALL TO CORE INTEGRATOR ------------
      CALL ROSCOR(N,FCN,X,Y,XEND,HMAX,H,RTOL,ATOL,ITOL,JAC,IJAC,
     &   MLJAC,MUJAC,DFX,IDFX,MAS,MLMAS,MUMAS,SOLOUT,IOUT,IDID,NMAX,
     &   UROUND,METH,IJOB,FAC1,FAC2,SAFE,AUTNMS,IMPLCT,JBAND,PRED,LDJAC,
     &   LDE,LDMAS2,WORK(IEYNEW),WORK(IEDY1),WORK(IEDY),WORK(IEAK1),
     &   WORK(IEAK2),WORK(IEAK3),WORK(IEAK4),WORK(IEAK5),WORK(IEAK6),
     &   WORK(IEFX),WORK(IEJAC),WORK(IEE),WORK(IEMAS),IWORK(IEIP),
     &   WORK(IECON),
     &   M1,M2,NM1,NFCN,NJAC,NSTEP,NACCPT,NREJCT,NDEC,NSOL,RPAR,IPAR)
      IWORK(14)=NFCN
      IWORK(15)=NJAC
      IWORK(16)=NSTEP
      IWORK(17)=NACCPT
      IWORK(18)=NREJCT
      IWORK(19)=NDEC
      IWORK(20)=NSOL
C ----------- RETURN -----------
      RETURN
      END
C
C
C
C  ----- ... AND HERE IS THE CORE INTEGRATOR  ----------
C
      SUBROUTINE ROSCOR(N,FCN,X,Y,XEND,HMAX,H,RTOL,ATOL,ITOL,JAC,IJAC,
     &  MLJAC,MUJAC,DFX,IDFX,MAS,MLMAS,MUMAS,SOLOUT,IOUT,IDID,NMAX,
     &  UROUND,METH,IJOB,FAC1,FAC2,SAFE,AUTNMS,IMPLCT,BANDED,PRED,LDJAC,
     &  LDE,LDMAS,YNEW,DY1,DY,AK1,AK2,AK3,AK4,AK5,AK6,FX,FJAC,E,FMAS,IP,
     &  CONT,
     &  M1,M2,NM1,NFCN,NJAC,NSTEP,NACCPT,NREJCT,NDEC,NSOL,RPAR,IPAR)
C ----------------------------------------------------------
C     CORE INTEGRATOR FOR RODAS
C     PARAMETERS SAME AS IN RODAS WITH WORKSPACE ADDED
C ----------------------------------------------------------
C         DECLARATIONS
C ----------------------------------------------------------
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION Y(N),YNEW(N),DY1(N),DY(N),AK1(N),
     *  AK2(N),AK3(N),AK4(N),AK5(N),AK6(N),FX(N),RPAR(*),IPAR(*),
     *  FJAC(LDJAC,N),E(LDE,NM1),FMAS(LDMAS,NM1),ATOL(*),RTOL(*)
      DIMENSION CONT(4*N)
      INTEGER IP(NM1)
      LOGICAL REJECT,AUTNMS,IMPLCT,BANDED,LAST,PRED
      COMMON/LINAL/MLE,MUE,MBJAC,MBB,MDIAG,MDIFF,MBDIAG
      COMMON /CONROS/XOLD,HOUT,NN
C *** *** *** *** *** *** ***
C  INITIALISATIONS
C *** *** *** *** *** *** ***
      NN=N
      NN2=2*N
      NN3=3*N
      LRC=4*N
C ------- COMPUTE MASS MATRIX FOR IMPLICIT CASE ----------
      IF (IMPLCT) CALL MAS (NM1,FMAS,LDMAS,RPAR,IPAR)
C ------ SET THE PARAMETERS OF THE METHOD -----
      CALL ROCOE(METH,A21,A31,A32,A41,A42,A43,A51,A52,A53,A54,
     &    C21,C31,C32,C41,C42,C43,C51,C52,C53,C54,C61,
     &    C62,C63,C64,C65,GAMMA,C2,C3,C4,D1,D2,D3,D4,
     &    D21,D22,D23,D24,D25,D31,D32,D33,D34,D35)
C --- INITIAL PREPARATIONS
      IF (M1.GT.0) IJOB=IJOB+10
      POSNEG=SIGN(1.D0,XEND-X)
      HMAXN=MIN(ABS(HMAX),ABS(XEND-X))
      IF (ABS(H).LE.10.D0*UROUND) H=1.0D-6
      H=MIN(ABS(H),HMAXN)
      H=SIGN(H,POSNEG)
      REJECT=.FALSE.
      LAST=.FALSE.
      NSING=0
      IRTRN=1
      IF (AUTNMS) THEN
         HD1=0.0D0
         HD2=0.0D0
         HD3=0.0D0
         HD4=0.0D0
      END IF
C -------- PREPARE BAND-WIDTHS --------
      MBDIAG=MUMAS+1
      IF (BANDED) THEN
          MLE=MLJAC
          MUE=MUJAC
          MBJAC=MLJAC+MUJAC+1
          MBB=MLMAS+MUMAS+1
          MDIAG=MLE+MUE+1
          MDIFF=MLE+MUE-MUMAS
      END IF
      IF (IOUT.NE.0) THEN
          XOLD=X
          IRTRN=1
          HOUT=H
          CALL SOLOUT(NACCPT+1,XOLD,X,Y,CONT,LRC,N,
     &                RPAR,IPAR,IRTRN)
          IF (IRTRN.LT.0) GOTO 179
      END IF
C --- BASIC INTEGRATION STEP
 1    IF (NSTEP.GT.NMAX) GOTO 178
      IF (0.1D0*ABS(H).LE.ABS(X)*UROUND) GOTO 177
      IF (LAST) THEN
          H=HOPT
          IDID=1
          RETURN
      END IF
      HOPT=H
      IF ((X+H*1.0001D0-XEND)*POSNEG.GE.0.D0) THEN
         H=XEND-X
         LAST=.TRUE.
      END IF
C *** *** *** *** *** *** ***
C  COMPUTATION OF THE JACOBIAN
C *** *** *** *** *** *** ***
      CALL FCN(N,X,Y,DY1,RPAR,IPAR)
      NFCN=NFCN+1
      NJAC=NJAC+1
      IF (IJAC.EQ.0) THEN
C --- COMPUTE JACOBIAN MATRIX NUMERICALLY
         IF (BANDED) THEN
C --- JACOBIAN IS BANDED
            MUJACP=MUJAC+1
            MD=MIN(MBJAC,N)
            DO MM=1,M1/M2+1
               DO K=1,MD
                  J=K+(MM-1)*M2
 12               AK2(J)=Y(J)
                  AK3(J)=DSQRT(UROUND*MAX(1.D-5,ABS(Y(J))))
                  Y(J)=Y(J)+AK3(J)
                  J=J+MD
                  IF (J.LE.MM*M2) GOTO 12
                  CALL FCN(N,X,Y,AK1,RPAR,IPAR)
                  J=K+(MM-1)*M2
                  J1=K
                  LBEG=MAX(1,J1-MUJAC)+M1
 14               LEND=MIN(M2,J1+MLJAC)+M1
                  Y(J)=AK2(J)
                  MUJACJ=MUJACP-J1-M1
                  DO L=LBEG,LEND
                     FJAC(L+MUJACJ,J)=(AK1(L)-DY1(L))/AK3(J)
                  END DO
                  J=J+MD
                  J1=J1+MD
                  LBEG=LEND+1
                  IF (J.LE.MM*M2) GOTO 14
               END DO
            END DO
         ELSE
C --- JACOBIAN IS FULL
            DO I=1,N
               YSAFE=Y(I)
               DELT=DSQRT(UROUND*MAX(1.D-5,ABS(YSAFE)))
               Y(I)=YSAFE+DELT
               CALL FCN(N,X,Y,AK1,RPAR,IPAR)
               DO J=M1+1,N
                 FJAC(J-M1,I)=(AK1(J)-DY1(J))/DELT
               END DO
               Y(I)=YSAFE
            END DO
         END IF
      ELSE
C --- COMPUTE JACOBIAN MATRIX ANALYTICALLY
         CALL JAC(N,X,Y,FJAC,LDJAC,RPAR,IPAR)
      END IF
      IF (.NOT.AUTNMS) THEN
         IF (IDFX.EQ.0) THEN
C --- COMPUTE NUMERICALLY THE DERIVATIVE WITH RESPECT TO X
            DELT=SQRT(UROUND*MAX(1.D-5,ABS(X)))
            XDELT=X+DELT
            CALL FCN(N,XDELT,Y,AK1,RPAR,IPAR)
            DO J=1,N
               FX(J)=(AK1(J)-DY1(J))/DELT
            END DO
         ELSE
C --- COMPUTE ANALYTICALLY THE DERIVATIVE WITH RESPECT TO X
            CALL DFX(N,X,Y,FX,RPAR,IPAR)
         END IF
      END IF
   2  CONTINUE
C *** *** *** *** *** *** ***
C  COMPUTE THE STAGES
C *** *** *** *** *** *** ***
      FAC=1.D0/(H*GAMMA)
      CALL DECOMR(N,FJAC,LDJAC,FMAS,LDMAS,MLMAS,MUMAS,
     &            M1,M2,NM1,FAC,E,LDE,IP,IER,IJOB,IMPLCT,IP)
      IF (IER.NE.0) GOTO 80
      NDEC=NDEC+1
C --- PREPARE FOR THE COMPUTATION OF THE 6 STAGES
      HC21=C21/H
      HC31=C31/H
      HC32=C32/H
      HC41=C41/H
      HC42=C42/H
      HC43=C43/H
      HC51=C51/H
      HC52=C52/H
      HC53=C53/H
      HC54=C54/H
      HC61=C61/H
      HC62=C62/H
      HC63=C63/H
      HC64=C64/H
      HC65=C65/H
      IF (.NOT.AUTNMS) THEN
         HD1=H*D1
         HD2=H*D2
         HD3=H*D3
         HD4=H*D4
      END IF
C --- THE STAGES
      CALL SLVROD(N,FJAC,LDJAC,MLJAC,MUJAC,FMAS,LDMAS,MLMAS,MUMAS,
     &    M1,M2,NM1,FAC,E,LDE,IP,DY1,AK1,FX,YNEW,HD1,IJOB,.FALSE.)
      DO  I=1,N
         YNEW(I)=Y(I)+A21*AK1(I)
      END DO
      CALL FCN(N,X+C2*H,YNEW,DY,RPAR,IPAR)
      DO I=1,N
         YNEW(I)=HC21*AK1(I)
      END DO
      CALL SLVROD(N,FJAC,LDJAC,MLJAC,MUJAC,FMAS,LDMAS,MLMAS,MUMAS,
     &    M1,M2,NM1,FAC,E,LDE,IP,DY,AK2,FX,YNEW,HD2,IJOB,.TRUE.)
      DO I=1,N
         YNEW(I)=Y(I)+A31*AK1(I)+A32*AK2(I)
      END DO
      CALL FCN(N,X+C3*H,YNEW,DY,RPAR,IPAR)
      DO I=1,N
         YNEW(I)=HC31*AK1(I)+HC32*AK2(I)
      END DO
      CALL SLVROD(N,FJAC,LDJAC,MLJAC,MUJAC,FMAS,LDMAS,MLMAS,MUMAS,
     &    M1,M2,NM1,FAC,E,LDE,IP,DY,AK3,FX,YNEW,HD3,IJOB,.TRUE.)
      DO I=1,N
         YNEW(I)=Y(I)+A41*AK1(I)+A42*AK2(I)+A43*AK3(I)
      END DO
      CALL FCN(N,X+C4*H,YNEW,DY,RPAR,IPAR)
      DO I=1,N
         YNEW(I)=HC41*AK1(I)+HC42*AK2(I)+HC43*AK3(I)
      END DO
      CALL SLVROD(N,FJAC,LDJAC,MLJAC,MUJAC,FMAS,LDMAS,MLMAS,MUMAS,
     &    M1,M2,NM1,FAC,E,LDE,IP,DY,AK4,FX,YNEW,HD4,IJOB,.TRUE.)
      DO I=1,N
         YNEW(I)=Y(I)+A51*AK1(I)+A52*AK2(I)+A53*AK3(I)+A54*AK4(I)
      END DO
      CALL FCN(N,X+H,YNEW,DY,RPAR,IPAR)
      DO I=1,N
         AK6(I)=HC52*AK2(I)+HC54*AK4(I)+HC51*AK1(I)+HC53*AK3(I)
      END DO
      CALL SLVROD(N,FJAC,LDJAC,MLJAC,MUJAC,FMAS,LDMAS,MLMAS,MUMAS,
     &    M1,M2,NM1,FAC,E,LDE,IP,DY,AK5,FX,AK6,0.D0,IJOB,.TRUE.)
C ------------ EMBEDDED SOLUTION ---------------
      DO I=1,N
         YNEW(I)=YNEW(I)+AK5(I)
      END DO
      CALL FCN(N,X+H,YNEW,DY,RPAR,IPAR)
      DO I=1,N
         CONT(I)=HC61*AK1(I)+HC62*AK2(I)+HC65*AK5(I)
     &              +HC64*AK4(I)+HC63*AK3(I)
      END DO
      CALL SLVROD(N,FJAC,LDJAC,MLJAC,MUJAC,FMAS,LDMAS,MLMAS,MUMAS,
     &    M1,M2,NM1,FAC,E,LDE,IP,DY,AK6,FX,CONT,0.D0,IJOB,.TRUE.)
C ------------ NEW SOLUTION ---------------
      DO  I=1,N
         YNEW(I)=YNEW(I)+AK6(I)
      END DO
      NSOL=NSOL+6
      NFCN=NFCN+5
C ------------ DENSE OUTPUT ----------
      IF (IOUT.NE.0) THEN
         DO I=1,N
            CONT(I)=Y(I)
            CONT(I+NN2)=D21*AK1(I)+D22*AK2(I)+D23*AK3(I)+D24*AK4(I)
     &               +D25*AK5(I)
            CONT(I+NN3)=D31*AK1(I)+D32*AK2(I)+D33*AK3(I)+D34*AK4(I)
     &               +D35*AK5(I)
         END DO
      END IF
C *** *** *** *** *** *** ***
C  ERROR ESTIMATION
C *** *** *** *** *** *** ***
      NSTEP=NSTEP+1
C ------------ COMPUTE ERROR ESTIMATION ----------------
      ERR=0.D0
      DO I=1,N
         IF (ITOL.EQ.0) THEN
            SK=ATOL(1)+RTOL(1)*MAX(ABS(Y(I)),ABS(YNEW(I)))
         ELSE
            SK=ATOL(I)+RTOL(I)*MAX(ABS(Y(I)),ABS(YNEW(I)))
         END IF
         ERR=ERR+(AK6(I)/SK)**2
      END DO
      ERR=SQRT(ERR/N)
C --- COMPUTATION OF HNEW
C --- WE REQUIRE .2<=HNEW/H<=6.
      FAC=MAX(FAC2,MIN(FAC1,(ERR)**0.25D0/SAFE))
      HNEW=H/FAC
C *** *** *** *** *** *** ***
C  IS THE ERROR SMALL ENOUGH ?
C *** *** *** *** *** *** ***
      IF (ERR.LE.1.D0) THEN
C --- STEP IS ACCEPTED
         NACCPT=NACCPT+1
         IF (PRED) THEN
C       --- PREDICTIVE CONTROLLER OF GUSTAFSSON
            IF (NACCPT.GT.1) THEN
               FACGUS=(HACC/H)*(ERR**2/ERRACC)**0.25D0/SAFE
               FACGUS=MAX(FAC2,MIN(FAC1,FACGUS))
               FAC=MAX(FAC,FACGUS)
               HNEW=H/FAC
            END IF
            HACC=H
            ERRACC=MAX(1.0D-2,ERR)
         END IF
         DO I=1,N
            Y(I)=YNEW(I)
         END DO
         XOLD=X
         X=X+H
         IF (IOUT.NE.0) THEN
            DO I=1,N
               CONT(NN+I)=Y(I)
            END DO
            IRTRN=1
            HOUT=H
            CALL SOLOUT(NACCPT+1,XOLD,X,Y,CONT,LRC,N,
     &                   RPAR,IPAR,IRTRN)
            IF (IRTRN.LT.0) GOTO 179
         END IF
         IF (ABS(HNEW).GT.HMAXN) HNEW=POSNEG*HMAXN
         IF (REJECT) HNEW=POSNEG*MIN(ABS(HNEW),ABS(H))
         REJECT=.FALSE.
         H=HNEW
         GOTO 1
      ELSE
C --- STEP IS REJECTED
         REJECT=.TRUE.
         LAST=.FALSE.
         H=HNEW
         IF (NACCPT.GE.1) NREJCT=NREJCT+1
         GOTO 2
      END IF
C --- SINGULAR MATRIX
  80  NSING=NSING+1
      IF (NSING.GE.5) GOTO 176
      H=H*0.5D0
      REJECT=.TRUE.
      LAST=.FALSE.
      GOTO 2
C --- FAIL EXIT
 176  CONTINUE
      WRITE(6,979)X
      WRITE(6,*) ' MATRIX IS REPEATEDLY SINGULAR, IER=',IER
      IDID=-4
      RETURN
 177  CONTINUE
      WRITE(6,979)X
      WRITE(6,*) ' STEP SIZE T0O SMALL, H=',H
      IDID=-3
      RETURN
 178  CONTINUE
      WRITE(6,979)X
      WRITE(6,*) ' MORE THAN NMAX =',NMAX,'STEPS ARE NEEDED'
      IDID=-2
      RETURN
C --- EXIT CAUSED BY SOLOUT
 179  CONTINUE
      WRITE(6,979)X
 979  FORMAT(' EXIT OF RODAS AT X=',E18.4)
      IDID=2
      RETURN
      END
C
      FUNCTION CONTRO(I,X,CONT,LRC)
C ----------------------------------------------------------
C     THIS FUNCTION CAN BE USED FOR CONTINUOUS OUTPUT IN CONNECTION
C     WITH THE OUTPUT-SUBROUTINE FOR RODAS. IT PROVIDES AN
C     APPROXIMATION TO THE I-TH COMPONENT OF THE SOLUTION AT X.
C ----------------------------------------------------------
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DIMENSION CONT(LRC)
      COMMON /CONROS/XOLD,H,N
      S=(X-XOLD)/H
      CONTRO=CONT(I)*(1-S)+S*(CONT(I+N)+(1-S)*(CONT(I+N*2)
     &      +S*CONT(I+N*3)))
      RETURN
      END
C
      SUBROUTINE ROCOE(METH,A21,A31,A32,A41,A42,A43,A51,A52,A53,A54,
     &  C21,C31,C32,C41,C42,C43,C51,C52,C53,C54,C61,
     &  C62,C63,C64,C65,GAMMA,C2,C3,C4,D1,D2,D3,D4,
     &  D21,D22,D23,D24,D25,D31,D32,D33,D34,D35)
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      GOTO (1,2,3), METH
 1      C2=0.386D0
        C3=0.21D0
        C4=0.63D0
        BET2P=0.0317D0
        BET3P=0.0635D0
        BET4P=0.3438D0
       D1= 0.2500000000000000D+00
       D2=-0.1043000000000000D+00
       D3= 0.1035000000000000D+00
       D4=-0.3620000000000023D-01
       A21= 0.1544000000000000D+01
       A31= 0.9466785280815826D+00
       A32= 0.2557011698983284D+00
       A41= 0.3314825187068521D+01
       A42= 0.2896124015972201D+01
       A43= 0.9986419139977817D+00
       A51= 0.1221224509226641D+01
       A52= 0.6019134481288629D+01
       A53= 0.1253708332932087D+02
       A54=-0.6878860361058950D+00
       C21=-0.5668800000000000D+01
       C31=-0.2430093356833875D+01
       C32=-0.2063599157091915D+00
       C41=-0.1073529058151375D+00
       C42=-0.9594562251023355D+01
       C43=-0.2047028614809616D+02
       C51= 0.7496443313967647D+01
       C52=-0.1024680431464352D+02
       C53=-0.3399990352819905D+02
       C54= 0.1170890893206160D+02
       C61= 0.8083246795921522D+01
       C62=-0.7981132988064893D+01
       C63=-0.3152159432874371D+02
       C64= 0.1631930543123136D+02
       C65=-0.6058818238834054D+01
       GAMMA= 0.2500000000000000D+00

       D21= 0.1012623508344586D+02
       D22=-0.7487995877610167D+01
       D23=-0.3480091861555747D+02
       D24=-0.7992771707568823D+01
       D25= 0.1025137723295662D+01
       D31=-0.6762803392801253D+00
       D32= 0.6087714651680015D+01
       D33= 0.1643084320892478D+02
       D34= 0.2476722511418386D+02
       D35=-0.6594389125716872D+01
      RETURN
C
 2      C2=0.3507221D0
        C3=0.2557041D0
        C4=0.6817790D0
        BET2P=0.0317D0
        BET3P=0.0047369D0
        BET4P=0.3438D0
       D1= 0.2500000000000000D+00
       D2=-0.6902209999999998D-01
       D3=-0.9671999999999459D-03
       D4=-0.8797900000000025D-01
       A21= 0.1402888400000000D+01
       A31= 0.6581212688557198D+00
       A32=-0.1320936088384301D+01
       A41= 0.7131197445744498D+01
       A42= 0.1602964143958207D+02
       A43=-0.5561572550509766D+01
       A51= 0.2273885722420363D+02
       A52= 0.6738147284535289D+02
       A53=-0.3121877493038560D+02
       A54= 0.7285641833203814D+00
       C21=-0.5104353600000000D+01
       C31=-0.2899967805418783D+01
       C32= 0.4040399359702244D+01
       C41=-0.3264449927841361D+02
       C42=-0.9935311008728094D+02
       C43= 0.4999119122405989D+02
       C51=-0.7646023087151691D+02
       C52=-0.2785942120829058D+03
       C53= 0.1539294840910643D+03
       C54= 0.1097101866258358D+02
       C61=-0.7629701586804983D+02
       C62=-0.2942795630511232D+03
       C63= 0.1620029695867566D+03
       C64= 0.2365166903095270D+02
       C65=-0.7652977706771382D+01
       GAMMA= 0.2500000000000000D+00

       D21=-0.3871940424117216D+02
       D22=-0.1358025833007622D+03
       D23= 0.6451068857505875D+02
       D24=-0.4192663174613162D+01
       D25=-0.2531932050335060D+01
       D31=-0.1499268484949843D+02
       D32=-0.7630242396627033D+02
       D33= 0.5865928432851416D+02
       D34= 0.1661359034616402D+02
       D35=-0.6758691794084156D+00
      RETURN
c
C Coefficients for RODAS with order 4 for linear parabolic problems
C Gerd Steinebach (1993)
  3     GAMMA = 0.25D0
	C2=3.d0*GAMMA
        C3=0.21D0
        C4=0.63D0
	BET2P=0.D0
	BET3P=c3*c3*(c3/6.d0-GAMMA/2.d0)/(GAMMA*GAMMA)
        BET4P=0.3438D0
       D1= 0.2500000000000000D+00
       D2=-0.5000000000000000D+00
       D3=-0.2350400000000000D-01
       D4=-0.3620000000000000D-01
       A21= 0.3000000000000000D+01
       A31= 0.1831036793486759D+01
       A32= 0.4955183967433795D+00
       A41= 0.2304376582692669D+01
       A42=-0.5249275245743001D-01
       A43=-0.1176798761832782D+01
       A51=-0.7170454962423024D+01
       A52=-0.4741636671481785D+01
       A53=-0.1631002631330971D+02
       A54=-0.1062004044111401D+01
       C21=-0.1200000000000000D+02
       C31=-0.8791795173947035D+01
       C32=-0.2207865586973518D+01
       C41= 0.1081793056857153D+02
       C42= 0.6780270611428266D+01
       C43= 0.1953485944642410D+02
       C51= 0.3419095006749676D+02
       C52= 0.1549671153725963D+02
       C53= 0.5474760875964130D+02
       C54= 0.1416005392148534D+02
       C61= 0.3462605830930532D+02
       C62= 0.1530084976114473D+02
       C63= 0.5699955578662667D+02
       C64= 0.1840807009793095D+02
       C65=-0.5714285714285717D+01
c
       D21= 0.2509876703708589D+02
       D22= 0.1162013104361867D+02
       D23= 0.2849148307714626D+02
       D24=-0.5664021568594133D+01
       D25= 0.0000000000000000D+00
       D31= 0.1638054557396973D+01
       D32=-0.7373619806678748D+00
       D33= 0.8477918219238990D+01
       D34= 0.1599253148779520D+02
       D35=-0.1882352941176471D+01
      RETURN
      END


module arcLengthLib

use funcAux

implicit none

private c1, c2, c3, Dlambda_acc, dlambda, arcLength
public lambda

real*8 , allocatable :: Gbar(:,:)
real*8 :: c1, c2, c3, Dlambda_acc, dlambda, arcLength, lambda, psi, GbarNormSq

contains

subroutine initArcLengthLib(psi, GbarType, NdimE)
	real*8, intent(in) :: psi
	integer, intent(in) :: GbarType
	integer, intent(in) :: NdimE

	allocate(Gbar(NdimE,NdimE))

	Gbar = 0.0d0

	select case(GbarType)
		case(1)
			Gbar(1,1) = 1.0d0
		case(2)
			Gbar(2,2) = 1.0d0
		case(3)
			Gbar(1,1) = 1.0d0
	end select

	GbarNormSq = dot_product2(Gbar,Gbar)

end subroutine

subroutine contributeCoeficientsSpherical_arcLength(dUf_Bar,dUf_Star,DUf_acc,NodG)
	real*8 , intent(in) :: dUf_Bar(:), dUf_Star(:), DUf_acc(:)
	integer , intent(in) :: NodG

	c1 =  c1 + dot_product(dUf_Bar,dUf_Bar) + GbarNormSq*psi**2.0
	c2 = c2 + 2.0d0*(dot_product(dUf_acc + dUf_Star, dUf_Bar) + Dlambda_acc*GbarNormSq*psi**2.0)
	c3 = c3 + dot_product(dUf_acc + dUf_Star, dUf_acc + dUf_Star) + &
		GbarNormSq*(Dlambda_acc*psi)**2.0 - arcLength*arcLength

end subroutine

end module


      program drsilm
c>> 2001-05-22 DRSILM  Krogh Minor change for making .f90 version.
c>> 1994-10-19 DRSILM  Krogh  Changes to use M77CON
c>> 1992-03-04 DRSILM  Krogh  Initial Code.
c--S replaces "?": DR?ILM, ?ILUPM
c Demonstration driver for SILUPM.
c Given a table of sin(xy), for x = 0, .08, .16, ..., and y = 0, .12, .2
c ... interpolates for (x,y) = (2.7, -.1), (.93, .05), (.765, .87) and
c obtains an error estimate.
c
      integer ND1, ND2, NDEG1, NDEG2, NUMTAB, NDIM, NX, NDIMT, NEOPT
      parameter (ND1=50, ND2=40, NDEG1=8, NDEG2=10, NUMTAB=ND1*ND2,
     1  NDIM=2, NX=3, NDIMT=2*NDIM+1, NEOPT=2*(2*NDIM+NDEG1)+1)
      integer  I, J, IOPT(4), NTAB(NDIMT), NDEG(NDIM), LUP(NDIM)
      real             X1(NX), X2(NX), YT(NUMTAB), YT2(ND2, ND1),
     1   XT(2*NDIM), X(NDIM), Y, EOPT(NEOPT), H1, H2, ANS
      parameter (H1 = .08E0, H2 = .12E0)
      equivalence (YT, YT2)
      data X1 / 2.7E0, .93E0, .765E0 /
      data X2 / -.1E0, .05E0, .87E0 /
c                       Set IOPT to get an error estimate.
      data IOPT / 0, NEOPT, 1, 0 /
      data NTAB(1), NTAB(2), NTAB(3) / ND1, ND2, 0 /
      data NDEG, LUP / NDEG1, NDEG2, 3, 3 /
c
c Compute the XT and YT tables
      XT(1) = 0.E0
      XT(2) = H1
      XT(3) = 0.E0
      XT(4) = H2
      do 20 I = 1, ND1
         do 10 J = 1, ND2
            YT2(J, I) = sin(real((I-1)*(J-1)) * H1 * H2)
   10    continue
   20 continue
      print *,
     1   'IOP(1)    X1      X2          Y     Est. Error True Error'
      do 50 I = 1, NX
         X(1) = X1(I)
         X(2) = X2(I)
         ANS = sin(X(1)*X(2))
            call SILUPM (NDIM,X,Y,NTAB,XT,YT,NDEG,LUP,IOPT,EOPT)
            print '(I5, 2F9.4, F12.8, 1P,E10.2, E11.2)',
     1       IOPT(1), X1(I), X2(I), Y, EOPT(1), Y - ANS
   50 continue
      stop
      end


C DISHIN.F - Gateway function for H_infinity or H_2 design of
C            discrete-time systems using SLICOT routines
C            SB10DD and SB10ED.
C
C RELEASE 2.0 of SLICOT Robust Control Toolbox.
C Based on SLICOT RELEASE 5.7. Copyright (c) 2002-2020 NICONET e.V.
C
C Matlab call:
C   [AK,BK,CK,DK,(RCOND)] = dishin(task,A,B,C,D,ncon,nmeas,(gamma))
C
C   task = 1 :  [AK,BK,CK,DK,(RCOND)] = dishin(1,A,B,C,D,ncon,nmeas,
C                                              gamma)
C   task = 2 :  [AK,BK,CK,DK,(RCOND)] = dishin(2,A,B,C,D,ncon,nmeas)
C
C Purpose:
C
C   task = 1:
C
C     To compute the matrices of an H-infinity (sub)optimal n-state
C     controller
C
C              | AK | BK |
C          K = |----|----|,
C              | CK | DK |
C
C     for the discrete-time system
C
C              | A  | B1  B2  |   | A | B |
C          P = |----|---------| = |---|---|
C              | C1 | D11 D12 |   | C | D |
C              | C2 | D21 D22 |
C
C     and for a given value of gamma, where B2 has column size of the
C     number of control inputs (ncon) and C2 has row size of the number
C     of measurements (nmeas) being provided to the controller.
C
C     It is assumed that
C
C     (A1) (A,B2) is stabilizable and (C2,A) is detectable,
C
C     (A2) D12 is full column rank and D21 is full row rank,
C
C               j*Theta
C     (A3) | A-e       *I  B2  | has full column rank for all
C          |    C1         D12 |
C
C          0 <= Theta < 2*Pi ,
C
C               j*Theta
C     (A4) | A-e       *I  B1  |  has full row rank for all
C          |    C2         D21 |
C
C          0 <= Theta < 2*Pi .
C
C   task = 2:
C
C     To compute the matrices of the H2 optimal n-state controller
C
C              | AK | BK |
C          K = |----|----|
C              | CK | DK |
C
C     for the discrete-time system
C
C              | A  | B1  B2  |   | A | B |
C          P = |----|---------| = |---|---|
C              | C1 |  0  D12 |   | C | D |
C              | C2 | D21 D22 |
C
C     where B2 has column size of the number of control inputs (ncon)
C     and C2 has row size of the number of measurements (nmeas) being
C     provided to the controller.
C
C     It is assumed that
C
C     (A1) (A,B2) is stabilizable and (C2,A) is detectable,
C
C     (A2) D12 is full column rank and D21 is full row rank,
C
C               j*Theta
C     (A3) | A-e       *I  B2  | has full column rank for all
C          |    C1         D12 |
C
C          0 <= Theta < 2*Pi ,
C
C               j*Theta
C     (A4) | A-e       *I  B1  |  has full row rank for all
C          |    C2         D21 |
C
C          0 <= Theta < 2*Pi .
C
C Input parameters:
C   task  - integer option to determine the type of the design:
C           = 1 : H_infinity design;
C           = 2 : H_2 design;
C   A     - the n-by-n system state matrix A.
C   B     - the n-by-m system input matrix B.
C   C     - the p-by-n system output matrix C.
C   D     - the p-by-m system matrix D.
C   ncon  - the number of control inputs. m >= ncon >= 0,
C           p-nmeas >= ncon.
C   nmeas - the number of measurements. p >= nmeas >= 0,
C           m-ncon >= nmeas.
C   gamma - (task 1 only) the parameter gamma used in H_infinity design.
C           It is assumed that gamma is sufficiently large so that the
C           controller is admissible. gamma >= 0.
C
C Output parameters:
C   AK    - the n-by-n controller state matrix AK.
C   BK    - the n-by-nmeas controller input matrix BK.
C   CK    - the ncon-by-n controller output matrix CK.
C   DK    - the ncon-by-nmeas controller matrix DK.
C   RCOND - (optional) a vector containing estimates of the reciprocal
C           condition numbers of the matrices which are to be inverted
C           and estimates of the reciprocal condition numbers of the
C           Riccati equations which have to be solved during the
C           computation of the controller. (See the description of
C           the algorithm in [1].)
C           If task = 1 then
C             RCOND(1) contains the reciprocal condition number of the
C                      matrix R3,
C             RCOND(2) contains the reciprocal condition number of the
C                      matrix R1 - R2'*inv(R3)*R2,
C             RCOND(3) contains the reciprocal condition number of the
C                      matrix V21,
C             RCOND(4) contains the reciprocal condition number of the
C                      matrix St3,
C             RCOND(5) contains the reciprocal condition number of the
C                      matrix V12,
C             RCOND(6) contains the reciprocal condition number of the
C                      matrix Im2 + DKHAT*D22
C             RCOND(7) contains the reciprocal condition number of the
C                      X-Riccati equation,
C             RCOND(8) contains the reciprocal condition number of the
C                      Z-Riccati equation.
C
C           If task = 2 then
C             RCOND(1) contains the reciprocal condition number of the
C                      control transformation matrix TU,
C             RCOND(2) contains the reciprocal condition number of the
C                      measurement transformation matrix TY.
C             RCOND(3) contains the reciprocal condition number of the
C                      matrix Im2 + B2'*X2*B2,
C             RCOND(4) contains the reciprocal condition number of the
C                      matrix Ip2 + C2*Y2*C2',
C             RCOND(5) contains the reciprocal condition number of the
C                      X-Riccati equation,
C             RCOND(6) contains the reciprocal condition number of the
C                      Y-Riccati equation,
C             RCOND(7) contains the reciprocal condition number of the
C                      matrix Im2 + DKHAT*D22 .
C
C References
C   [1] P.Hr. Petkov, D.W. Gu and M.M. Konstantinov. Fortran 77 routines
C       for Hinf and H2 design of linear discrete-time control systems.
C       Report99-8, Department of Engineering, Leicester University,
C       April 1999.
C
C Contributor:
C   P.Hr. Petkov, TU Sofia, Bulgaria, Sep. 1999.
C
C Revisions:
C   V. Sima, Research Institute for Informatics, Bucharest, Apr. 2009,
C   Dec. 2012.
C
C **********************************************************************
C
      SUBROUTINE MEXFUNCTION( NLHS, PLHS, NRHS, PRHS )
C
C .. Parameters ..
      DOUBLE PRECISION  ONE, ZERO
      PARAMETER         ( ONE = 1.0D0, ZERO = 0.0D0 )
C
C .. Mex-file interface parameters ..
      INTEGER           PLHS(*), PRHS(*)
      INTEGER*4         NLHS, NRHS
C
C .. Mex-file integer functions ..
      INTEGER           mxCreateDoubleMatrix, mxGetPr
      INTEGER*4         mxGetM, mxGetN, mxIsNumeric, mxIsComplex
C
C .. Scalar parameters used by SLICOT subroutines ..
      INTEGER           INFO, LDA, LDB, LDC, LDD, LDAK, LDBK, LDCK,
     $                  LDDK, LDR, LDWORK, LDX, LDZ, M, N, NCON, NMEAS,
     $                  P
      DOUBLE PRECISION  GAMMA, TOL
C
C .. Allocatable arrays ..
C !Fortran 90/95 (Fixed dimensions should be used with Fortran 77.)
      INTEGER,          ALLOCATABLE :: IWORK(:)
      LOGICAL,          ALLOCATABLE :: BWORK(:)
      DOUBLE PRECISION, ALLOCATABLE :: A(:,:), B(:,:), C(:,:), D(:,:),
     $                  AK(:,:), BK(:,:), CK(:,:), DK(:,:), X(:,:),
     $                  Z(:,:), DWORK(:)
C
C .. Local variables and constant dimension arrays ..
      CHARACTER*120     TEXT
      INTEGER           LBWORK, LIWORK, Q, TASK
      DOUBLE PRECISION  TEMP
      DOUBLE PRECISION  RCOND(8)
C
C .. External subroutines ..
      EXTERNAL          SB10DD, SB10ED
C
C ..Intrinsic functions..
      INTRINSIC         MAX, MIN
C
C Check for proper number of arguments.
C
      IF ( NRHS.LT.7 ) THEN
         CALL mexErrMsgTxt
     $        ( 'DISHIN requires at least 7 input arguments' )
      ELSE IF ( NLHS.LT.4 ) THEN
         CALL mexErrMsgTxt
     $        ( 'DISHIN requires at least 4 output arguments' )
      END IF
C
C Check dimensions of input parameters and read/set scalar parameters.
C
C   task
C
      IF ( mxGetM( PRHS(1) ).NE.1 .OR. mxGetN( PRHS(1) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 'TASK must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(1) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(1) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'TASK must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(1) ), TEMP, 1 )
      TASK = TEMP
      IF ( TASK.LT.1 .OR. TASK.GT.2 ) THEN
         CALL mexErrMsgTxt
     $        ( 'TASK has 1 or 2 the only admissible values' )
      END IF
C
      IF ( TASK.EQ.1 ) THEN
         IF ( NRHS.LT.8 ) THEN
            CALL mexErrMsgTxt
     $          ( 'DISHIN requires at least 8 input arguments' )
         END IF
      END IF
C
C   A(nxn), B(nxm), C(pxn), D(pxm)
C
      N = mxGetM( PRHS(2) )
      M = mxGetN( PRHS(3) )
      P = mxGetM( PRHS(4) )
      IF ( M.LE.0 ) THEN
         CALL mexErrMsgTxt( 'The system has no inputs' )
      END IF
      IF ( P.LE.0 ) THEN
         CALL mexErrMsgTxt( 'The system has no outputs' )
      END IF
C
      IF ( mxGetN( PRHS(2) ).NE.N ) THEN
         CALL mexErrMsgTxt( 'A must be a square matrix' )
      END IF
      IF ( mxIsNumeric( PRHS(2) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(2) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'A must be a real matrix' )
      END IF
      IF ( mxGetM( PRHS(3) ).NE.N ) THEN
         CALL mexErrMsgTxt( 'B must have the same row dimension as A' )
      END IF
      IF ( mxIsNumeric( PRHS(3) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(3) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'B must be a real matrix' )
      END IF
      IF ( mxGetN( PRHS(4) ).NE.N ) THEN
         CALL mexErrMsgTxt
     $       ('C must have the same column dimension as A' )
      END IF
      IF ( mxIsNumeric( PRHS(4) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(4) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'C must be a real matrix' )
      END IF
      IF ( mxGetM( PRHS(5) ).NE.P ) THEN
         CALL mexErrMsgTxt( 'D must have the same row dimension as C' )
      END IF
      IF ( mxGetN( PRHS(5) ).NE.M ) THEN
         CALL mexErrMsgTxt
     $       ( 'D must have the same column dimension as B' )
      END IF
      IF ( mxIsNumeric( PRHS(5) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(5) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'D must be a real matrix' )
      END IF
C
C     ncon
      IF ( mxGetM( PRHS(6) ).NE.1 .OR. mxGetN( PRHS(6) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 'NCON must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(6) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(6) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'NCON must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(6) ), TEMP, 1 )
      NCON = TEMP
      IF ( NCON.LT.1 ) THEN
         CALL mexErrMsgTxt
     $        ( 'NCON must be a positive integer' )
      END IF
C
C     nmeas
      IF ( mxGetM( PRHS(7) ).NE.1 .OR. mxGetN( PRHS(7) ).NE.1 ) THEN
         CALL mexErrMsgTxt( 'NMEAS must be a scalar' )
      END IF
      IF ( mxIsNumeric( PRHS(7) ).EQ.0 .OR.
     $     mxIsComplex( PRHS(7) ).EQ.1 ) THEN
         CALL mexErrMsgTxt( 'NMEAS must be an integer scalar' )
      END IF
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(7) ), TEMP, 1 )
      NMEAS = TEMP
      IF ( NMEAS.LT.1 ) THEN
         CALL mexErrMsgTxt
     $        ( 'NMEAS must be a positive integer' )
      END IF
C
C     gamma
      IF ( TASK.EQ.1 ) THEN
         IF ( mxGetM( PRHS(8) ).NE.1 .OR. mxGetN( PRHS(8) ).NE.1 ) THEN
            CALL mexErrMsgTxt( 'GAMMA must be a scalar' )
         END IF
         IF ( mxIsNumeric( PRHS(8) ).EQ.0 .OR.
     $        mxIsComplex( PRHS(8) ).EQ.1 ) THEN
            CALL mexErrMsgTxt( 'GAMMA must be a real scalar' )
         END IF
         CALL mxCopyPtrToReal8( mxGetPr( PRHS(8) ), GAMMA, 1 )
         IF ( GAMMA.LE.ZERO ) THEN
            CALL mexErrMsgTxt( 'GAMMA must be a positive scalar' )
         END IF
      END IF
C
      TOL = -ONE
C
C Determine the lenghts of working arrays.
C Use a larger value for LDWORK for enabling calls of block algorithms
C in DGEES.
C
      LDA  = MAX( 1, N )
      LDB  = MAX( 1, N )
      LDC  = MAX( 1, P )
      LDD  = MAX( 1, P )
      LDAK = MAX( 1, N )
      LDBK = MAX( 1, N )
      LDCK = MAX( 1, NCON )
      LDDK = MAX( 1, NCON )
      LDX  = MAX( 1, N )
      LDZ  = MAX( 1, N )
      LBWORK = 2*N
      Q = MAX( M - NCON, NCON, P - NMEAS, NMEAS )
      IF ( TASK.EQ.1 ) THEN
         LIWORK = MAX( 2*MAX ( NCON, N ), M, NCON + NMEAS, N*N )
         LDWORK = MAX( ( N + Q )*( N + Q + 6 ),
     $                 13*N*N + M*M + 2*Q*Q + N*( M + Q ) +
     $                 MAX( M*( M + 7*N ), 2*Q*( 8*N + M + 2*Q ) ) +
     $                      6*N + MAX( 14*N + 23, 16*N,
     $                                  2*N + MAX( M, 2*Q ),
     $                                  3*MAX( M, 2*Q ) ) )
      ELSE
         LIWORK = MAX ( 2*M, 2*N, N*N, P )
         LDWORK = 2*Q*( 3*Q + 2*N ) +
     $            MAX( 1, ( N + Q )*( N + Q + 6 ),
     $                 Q*( Q + MAX( N, Q, 5 ) + 1 ),
     $                 2*N*N +
     $                 MAX( 1, 14*N*N + 6*N +
     $                      MAX( 14*N + 23, 16*N ),
     $                      Q*( N + Q + MAX( Q, 3 ) ) ) )
      END IF
C
C Allocate variable dimension local arrays.
C !Fortran 90/95
C
      IF ( TASK.EQ.1 ) THEN
         ALLOCATE ( A( LDA, N ), B( LDB, M ), C( LDC, N ), D( LDD, M ),
     $              AK( LDAK, N ), BK( LDBK, NMEAS ), CK( LDCK, N ),
     $              DK( LDDK, NMEAS ), X( LDX, N ), Z( LDZ, N ) )
      ELSE
         ALLOCATE ( A( LDA, N ), B( LDB, M ), C( LDC, N ), D( LDD, M ),
     $              AK( LDAK, N ), BK( LDBK, NMEAS ), CK( LDCK, N ),
     $              DK( LDDK, NMEAS ) )
      END IF
      ALLOCATE ( BWORK( LBWORK ), IWORK( LIWORK ), DWORK( LDWORK ) )
C
C Copy inputs from MATLAB workspace to locally allocated arrays.
C
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(2) ), A, N*N )
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(3) ), B, N*M )
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(4) ), C, P*N )
      CALL mxCopyPtrToReal8( mxGetPr( PRHS(5) ), D, P*M )
C
C Do the actual computations.
C
      IF ( TASK.EQ.1 ) THEN
         CALL SB10DD( N, M, P, NCON, NMEAS, GAMMA, A, LDA, B, LDB,
     $                C, LDC, D, LDD, AK, LDAK, BK, LDBK, CK, LDCK,
     $                DK, LDDK, X, LDX, Z, LDZ, RCOND, TOL, IWORK,
     $                DWORK, LDWORK, BWORK, INFO )
      ELSE
         CALL SB10ED( N, M, P, NCON, NMEAS, A, LDA, B, LDB, C, LDC,
     $                D, LDD, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK,
     $                RCOND, TOL, IWORK, DWORK, LDWORK, BWORK, INFO )
      END IF
C
C Copy output to MATLAB workspace.
C
      PLHS(1) = mxCreateDoubleMatrix( N, N, 0 )
      CALL mxCopyReal8ToPtr( AK, mxGetPr( PLHS(1) ), N*N )
      PLHS(2) = mxCreateDoubleMatrix( N, NMEAS, 0 )
      CALL mxCopyReal8ToPtr( BK, mxGetPr( PLHS(2) ), N*NMEAS )
      PLHS(3) = mxCreateDoubleMatrix( NCON, N, 0 )
      CALL mxCopyReal8ToPtr( CK, mxGetPr( PLHS(3) ), NCON*N )
      PLHS(4) = mxCreateDoubleMatrix( NCON, NMEAS, 0 )
      CALL mxCopyReal8ToPtr( DK, mxGetPr( PLHS(4) ), NCON*NMEAS )
      IF ( NLHS.GT.4 ) THEN
         IF ( TASK.EQ.1 ) THEN
            LDR = 8
         ELSE
            LDR = 7
         END IF
         PLHS(5) = mxCreateDoubleMatrix( LDR, 1, 0 )
         CALL mxCopyReal8ToPtr( RCOND, mxGetPr( PLHS(5) ), LDR )
      END IF
C
C Deallocate local arrays.
C !Fortran 90/95
      IF ( TASK.EQ.1 ) THEN
         DEALLOCATE( A, B, C, D, AK, BK, CK, DK, X, Z, BWORK, IWORK,
     $               DWORK )
      ELSE
         DEALLOCATE( A, B, C, D, AK, BK, CK, DK, BWORK, IWORK, DWORK )
      END IF
C
C Error and warning handling.
C
      IF ( INFO.NE.0 ) THEN
         IF ( TASK.EQ.1 ) THEN
            WRITE( TEXT, '('' INFO = '',I4,'' ON EXIT FROM SB10DD'')'
     $           ) INFO
         ELSE
            WRITE( TEXT, '('' INFO = '',I4,'' ON EXIT FROM SB10ED'')'
     $           ) INFO
         END IF
      END IF
C
      IF ( INFO.NE.0 ) THEN
         CALL mexErrMsgTxt( TEXT )
      END IF
C
      RETURN
C *** Last line of DISHIN ***
      END


module check_lib
 implicit none
 double precision,parameter :: errval=-999.d0

 interface chk_abmi
  module procedure check_abmi_d,check_abmi_z
 end interface
 interface chk_abmc
  module procedure check_abmc_d,check_abmc_z
 end interface
 interface chk_ort
  module procedure check_ort_d,check_ort_z
 end interface
 interface nnz
  module procedure nnz_d,nnz_d2,nnz_d3
 end interface

 private :: check_abmi_d,check_abmi_z
 private :: check_abmc_d,check_abmc_z
 private :: check_ort_d,check_ort_z

contains
 double precision function check_abmi_d(a,b) result (err)
  implicit none
  double precision,intent(in) :: a(:,:),b(:,:)
  double precision,allocatable :: c(:,:)
  integer :: i,m,n
  double precision,external :: dnrm2
  if(size(a,1).ne.size(b,2) .or. size(a,2).ne.size(b,1))then
   err=errval; return
  end if
  m=size(a,1); n=size(a,2)
  allocate(c(m,m))
  c=matmul(a,b)
  !call dgemm('n','n',m,m,n,1.d0,a,m,b,n,0.d0,c,m)
  forall(i=1:m)c(i,i)=c(i,i)-1.d0
  err=dnrm2(m*m,c,1)/dsqrt(dble(m))
  deallocate(c)
  return
 end function check_abmi_d

 double precision function check_abmi_z(a,b) result (err)
  implicit none
  complex(8),intent(in) :: a(:,:),b(:,:)
  complex(8),allocatable :: c(:,:)
  integer :: i,m,n
  double precision,external :: dznrm2
  if(size(a,1).ne.size(b,2) .or. size(a,2).ne.size(b,1))then
   err=errval; return
  end if
  m=size(a,1); n=size(a,2)
  allocate(c(m,m))
  c=matmul(a,b)
  forall(i=1:m)c(i,i)=c(i,i)-(1.d0,0.d0)
  err=dznrm2(m*m,c,1)/dsqrt(dble(m))
  deallocate(c)
  return
 end function check_abmi_z

 double precision function check_abmc_d(a,b,c) result (err)
  implicit none
  double precision,intent(in) :: a(:,:),b(:,:),c(:,:)
  double precision,allocatable :: ab(:,:)
  integer :: m,n,k
  double precision :: nrm
  double precision,external :: dnrm2
  if(size(a,2).ne.size(b,1) .or. size(a,1).ne.size(c,1) .or. size(b,2).ne.size(c,2))then
   err=errval; return
  end if
  m=size(a,1); k=size(a,2); n=size(b,2)
  nrm=dnrm2(m*n,c,1)
  if(nrm.le.0.d0)then
   err=errval; return
  end if
  allocate(ab(m,n))
  ab=matmul(a,b)
  ab=ab-c; err=dnrm2(m*n,ab,1)/nrm
  deallocate(ab)
  return
 end function check_abmc_d
 double precision function check_abmc_z(a,b,c) result (err)
  implicit none
  complex(8),intent(in) :: a(:,:),b(:,:),c(:,:)
  complex(8),allocatable :: ab(:,:)
  integer :: m,n,k
  double precision :: nrm
  double precision,external :: dznrm2
  if(size(a,2).ne.size(b,1) .or. size(a,1).ne.size(c,1) .or. size(b,2).ne.size(c,2))then
   err=errval; return
  end if
  m=size(a,1); k=size(a,2); n=size(b,2)
  nrm=dznrm2(m*n,c,1)
  if(nrm.le.0.d0)then
   err=errval; return
  end if
  allocate(ab(m,n))
  ab=matmul(a,b)
  ab=ab-c; err=dznrm2(m*n,ab,1)/nrm
  deallocate(ab)
  return
 end function check_abmc_z
 double precision function check_ort_d(a) result (err)
  implicit none
  double precision,intent(in) :: a(:,:)
  double precision,allocatable :: g(:,:)
  integer :: m,n,i
  double precision,external :: dnrm2
  m=size(a,1); n=size(a,2)
  if(m.lt.n)then
   err=errval; return
  end if
  allocate(g(n,n))
  call dgemm('t','n',n,n,m,1.d0,a,m,a,m,0.d0,g,n)
  forall(i=1:n)g(i,i)=g(i,i)-1.d0
  err=dnrm2(n*n,g,1)/dsqrt(dble(n))
  deallocate(g)
  return
 end function check_ort_d
 double precision function check_ort_z(a) result (err)
  implicit none
  complex(8),intent(in) :: a(:,:)
  complex(8),allocatable :: g(:,:)
  integer :: m,n,i
  double precision,external :: dznrm2
  m=size(a,1); n=size(a,2)
  if(m.lt.n)then
   err=errval; return
  end if
  allocate(g(n,n))
  call zgemm('c','n',n,n,m,(1.d0,0.d0),a,m,a,m,(0.d0,0.d0),g,n)
  forall(i=1:n)g(i,i)=g(i,i)-(1.d0,0.d0)
  err=dznrm2(n*n,g,1)/dsqrt(dble(n))
  deallocate(g)
  return
 end function check_ort_z

 double precision function check_sym_d(n,a) result (err)
  implicit none
  integer,intent(in) :: n
  double precision,intent(in) :: a(n,n)
  integer :: i,j
  double precision,external :: dnrm2
  err=0.d0
  do j=1,n
   do i=1,n
    err=err+(a(i,j)-a(j,i))**2
   end do
  end do
  err=dsqrt(err)
  if(err.gt.0.d0)err=err/dnrm2(n*n,a,1)
 end function

 integer function nnz_d(x,tol) result (nnz)
  implicit none
  double precision,intent(in) :: x(:)
  double precision,intent(in),optional :: tol
  double precision :: eps,nrm
  integer :: i
  if(present(tol))then;eps=tol;else;eps=1.d-13;endif
  nrm=maxval(abs(x));eps=eps*nrm
  nnz=0
  do i=1,size(x)
   if(dabs(x(i)).gt.eps)nnz=nnz+1
  enddo
 end function
 integer function nnz_d2(x,tol) result (nnz)
  implicit none
  double precision,intent(in) :: x(:,:)
  double precision,intent(in),optional :: tol
  double precision :: eps,nrm
  integer :: i,j
  if(present(tol))then;eps=tol;else;eps=1.d-13;endif
  nrm=maxval(abs(x));eps=eps*nrm
  nnz=0
  do j=1,size(x,2)
   do i=1,size(x,1)
    if(dabs(x(i,j)).gt.eps)nnz=nnz+1
   enddo
  enddo
 end function
 integer function nnz_d3(x,tol) result (nnz)
  implicit none
  double precision,intent(in) :: x(:,:,:)
  double precision,intent(in),optional :: tol
  double precision :: eps,nrm
  integer :: i,j,k
  if(present(tol))then;eps=tol;else;eps=1.d-13;endif
  nrm=maxval(abs(x));eps=eps*nrm
  nnz=0
  do k=1,size(x,3)
   do j=1,size(x,2)
    do i=1,size(x,1)
     if(dabs(x(i,j,k)).gt.eps)nnz=nnz+1
    enddo
   enddo
  enddo
 end function

end module


      PROGRAM xmiser
C     driver for routine miser
      INTEGER idum,j,n,ndim,nt,ntries
      REAL ave,dith,sumav,sumsd,var,xoff
      COMMON /ranno/ idum
      COMMON /tofunc/ xoff,ndim
      REAL region(20)
      EXTERNAL func
      write(*,*) 'IDUM='
      read(*,*) idum
      idum=-abs(idum)
1     write(*,*) 'ENTER N,NDIM,XOFF,DITH,NTRIES'
      read(*,*,END=999) n,ndim,xoff,dith,ntries
      sumav=0.
      sumsd=0.
      do 12 nt=1,ntries
        do 11 j=1,ndim
          region(j)=0.
          region(j+ndim)=1.
11      continue
        call miser(func,region,ndim,n,dith,ave,var)
        sumav=sumav+(ave-1.)**2
        sumsd=sumsd+sqrt(abs(var))
12    continue
      sumav=sqrt(sumav/ntries)
      sumsd=sumsd/ntries
      write(*,*) 'FRACTIONAL ERROR: ACTUAL,INDICATED=',sumav,sumsd
      goto 1
999   write(*,*) 'NORMAL COMPLETION'
      STOP
      END
      REAL FUNCTION func(pt)
      INTEGER j,ndim
      REAL sum,xoff
      COMMON /tofunc/ xoff,ndim
      REAL pt(*)
      sum=0.
      do 11 j=1,ndim
        sum=sum+100.*(pt(j)-xoff)**2
11    continue
      if (sum.lt.80.) then
        func=exp(-sum)
      else
        func=0.
      endif
      func=func*(5.64189**ndim)
      return
      END

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


  SUBROUTINE lwc2vapor(lwc,sh,t,p,thresh,lwc_m,sh_m,rh_m)

  ! Subroutine to convert cloud water to vapor.

    IMPLICIT NONE

    ! Inputs:

    REAL, INTENT(IN)    :: lwc    ! Cloud water mixing ratio (kg/kg)
    REAL, INTENT(IN)    :: sh     ! Specific humidity (kg/kg)
    REAL, INTENT(IN)    :: t      ! Temperature (K)
    REAL, INTENT(IN)    :: p      ! Pressure (Pa)
    REAL, INTENT(IN)    :: thresh ! Saturation factor
              ! Set thresh to 1.0 to convert cloud water up to
              ! vapor saturation.  1.1 will allow 110% RH, and so
              ! forth

    ! Outputs:

    REAL, INTENT(OUT)   :: lwc_m  ! Adjusted lwc
    REAL, INTENT(OUT)   :: sh_m   ! Adjusted specific humidity
    REAL, INTENT(OUT)   :: rh_m   ! Adjusted RH (%)

    ! Locals

    REAL :: shsat,mrmax,mr,mr_m,mrsat
    REAL, EXTERNAL :: ssh,make_rh

    ! Set saturation specific humidity for this point

    shsat = ssh(p,t-273.15)*0.001

    ! Convert specific humidity to mixing ratio
    mrsat = shsat/(1.-shsat)
    mr = sh/(1.-sh)

    mrmax = mrsat*thresh

    ! Create modified mixing ratio (mr_m) by adding cloud liquid

    mr_m = mr + lwc

    ! Zero out the modified cloud water

    lwc_m = 0.

    ! If mr_m exceeds mrmax, convert the excess amount
    ! back to cloud water

    IF (mr_m .GT. mrmax) THEN
      lwc_m = mr_m - mrmax
      mr_m = mrmax
    ENDIF

    ! Compute RH from modified mixing ratio
    rh_m = (mr_m/mrsat)*100.

    ! Convert modified mixing ratio back to specific humidity
    sh_m = mr_m/(1.+mr_m)

    RETURN
  END SUBROUTINE lwc2vapor


      SUBROUTINE QZHES(NM,N,A,B,LOW,IGH,MATZ,Z)
CSE
C
      INTEGER I,J,K,L,N,LB,L1,NM,NK1,LOW,IGH,IGHM1,IGHM2
      REAL*8 A(NM,N),B(NM,N),Z(NM,N)
      REAL*8 R,S,T,U1,U2,V1,V2,RHO
      REAL*8 DSQRT,DABS,DSIGN
      LOGICAL MATZ
C
C     THIS SUBROUTINE IS THE FIRST STEP OF THE QZ ALGORITHM
C     FOR SOLVING GENERALIZED MATRIX EIGENVALUE PROBLEMS,
C     SIAM J. NUMER. ANAL. 10, 241-256(1973) BY MOLER AND STEWART.
C
CPS   PURPOSE:
C
C     THIS SUBROUTINE ACCEPTS A PAIR OF REAL GENERAL MATRICES AND
C     REDUCES ONE OF THEM TO UPPER HESSENBERG FORM AND THE OTHER
C     TO UPPER TRIANGULAR FORM USING ORTHOGONAL TRANSFORMATIONS.
C     IT IS PRECEEDED BY BALGEN AND IS USUALLY FOLLOWED BY
C     QZIT,  QZVAL  AND, POSSIBLY,  QZVEC.
C
CPE
CAS            ****  ARGUMENT LIST  ****
C     ON INPUT:
C
C        NM MUST BE SET TO THE ROW DIMENSION OF TWO-DIMENSIONAL
C          ARRAY PARAMETERS AS DECLARED IN THE CALLING PROGRAM
C          DIMENSION STATEMENT;
C
C        N IS THE ORDER OF THE MATRICES;
C
C        A CONTAINS A REAL GENERAL MATRIX;
C
C        B CONTAINS A REAL GENERAL MATRIX;
C
C        LOW IS THE STARTING INDEX FOR THE SCALED SUBROUTINES OF
C          A AND B OBTAINED FROM THE BALANCING ALGORTHIM.
C          IF THE MATRICES HAVE NOT BEEN BALANCED, SET LOW TO 1.
C
C        IGH IS THE ENDING INDEX FOR THE SCALED SUBMATRICES OF
C          A AND B OBTAINED FROM THE BALANCING ALGORTHIM.
C          IF THE MATRICES HAVE NOT BEEN BALLANCED, SET IGH TO N.
C
C        MATZ SHOULD BE SET TO .TRUE. IF THE RIGHT HAND TRANSFORMATIONS
C          ARE TO BE ACCUMULATED FOR LATER USE IN COMPUTING
C          EIGENVECTORS, AND TO .FALSE. OTHERWISE.
C
C     ON OUTPUT:
C
C        A HAS BEEN REDUCED TO UPPER HESSENBERG FORM.  THE ELEMENTS
C          BELOW THE FIRST SUBDIAGONAL HAVE BEEN SET TO ZERO;
C
C        B HAS BEEN REDUCED TO UPPER TRIANGULAR FORM.  THE ELEMENTS
C          BELOW THE MAIN DIAGONAL HAVE BEEN SET TO ZERO;
C
C        Z CONTAINS THE PRODUCT OF THE RIGHT HAND TRANSFORMATIONS IF
C          MATZ HAS BEEN SET TO .TRUE.  OTHERWISE, Z IS NOT REFERENCED.
C
C     QUESTIONS AND COMMENTS SHOULD BE DIRECTED TO B. S. GARBOW,
C     APPLIED MATHEMATICS DIVISION, ARGONNE NATIONAL LABORATORY
C
C     STANDARD 'EISPAC' SUBROUTINE QZHES HAS BEEN MODIFIED TO
C     ACCOMMODATE PARTITIONING GENERATED BY WARD'S BALANCING ALGORITHM
C     'BALGEN'
C
C     ------------------------------------------------------------------
C
C     :::::::::: INITIALIZE Z ::::::::::
      IF (.NOT. MATZ) GO TO 10
C
      DO 3 I = 1, N
C
         DO 2 J = 1, N
            Z(I,J) = 0.0D0
    2    CONTINUE
C
         Z(I,I) = 1.0D0
    3 CONTINUE
C     :::::::::: REDUCE B TO UPPER TRIANGULAR FORM ::::::::::
   10 IF (LOW .EQ. IGH) GO TO 170
      IGHM1 = IGH - 1
C
      DO 100 L = LOW, IGHM1
         L1 = L + 1
         S = 0.0D0
C
         DO 20 I = L1, IGH
            S = S + DABS(B(I,L))
   20    CONTINUE
C
         IF (S .EQ. 0.0D0) GO TO 100
         S = S + DABS(B(L,L))
         R = 0.0D0
C
         DO 25 I = L, IGH
            B(I,L) = B(I,L) / S
            R = R + B(I,L)**2
   25    CONTINUE
C
         R = DSIGN(DSQRT(R),B(L,L))
         B(L,L) = B(L,L) + R
         RHO = R * B(L,L)
C
         DO 50 J = L1, N
            T = 0.0D0
C
            DO 30 I = L, IGH
               T = T + B(I,L) * B(I,J)
   30       CONTINUE
C
            T = -T / RHO
C
            DO 40 I = L, IGH
               B(I,J) = B(I,J) + T * B(I,L)
   40       CONTINUE
C
   50    CONTINUE
C
         DO 80 J = LOW, N
            T = 0.0D0
C
            DO 60 I = L, IGH
               T = T + B(I,L) * A(I,J)
   60       CONTINUE
C
            T = -T / RHO
C
            DO 70 I = L, IGH
               A(I,J) = A(I,J) + T * B(I,L)
   70       CONTINUE
C
   80    CONTINUE
C
         B(L,L) = -S * R
C
         DO 90 I = L1, IGH
            B(I,L) = 0.0D0
   90    CONTINUE
C
  100 CONTINUE
C     :::::::::: REDUCE A TO UPPER HESSENBERG FORM, WHILE
C                KEEPING B TRIANGULAR ::::::::::
      IF (LOW .EQ. IGHM1) GO TO 170
      IGHM2 = IGH - 2
C
      DO 160 K = LOW, IGHM2
         NK1 = IGHM1 - K
C     :::::::::: FOR L=N-1 STEP -1 UNTIL K+1 DO -- ::::::::::
         DO 150 LB = 1, NK1
            L = IGH - LB
            L1 = L + 1
C     :::::::::: ZERO A(L+1,K) ::::::::::
            S = DABS(A(L,K)) + DABS(A(L1,K))
            IF (S .EQ. 0.0D0) GO TO 150
            U1 = A(L,K) / S
            U2 = A(L1,K) / S
            R = DSIGN(DSQRT(U1*U1+U2*U2),U1)
            V1 =  -(U1 + R) / R
            V2 = -U2 / R
            U2 = V2 / V1
C
            DO 110 J = K, N
               T = A(L,J) + U2 * A(L1,J)
               A(L,J) = A(L,J) + T * V1
               A(L1,J) = A(L1,J) + T * V2
  110       CONTINUE
C
            A(L1,K) = 0.0D0
C
            DO 120 J = L, N
               T = B(L,J) + U2 * B(L1,J)
               B(L,J) = B(L,J) + T * V1
               B(L1,J) = B(L1,J) + T * V2
  120       CONTINUE
C     :::::::::: ZERO B(L+1,L) ::::::::::
            S = DABS(B(L1,L1)) + DABS(B(L1,L))
            IF (S .EQ. 0.0D0) GO TO 150
            U1 = B(L1,L1) / S
            U2 = B(L1,L) / S
            R = DSIGN(DSQRT(U1*U1+U2*U2),U1)
            V1 =  -(U1 + R) / R
            V2 = -U2 / R
            U2 = V2 / V1
C
            DO 130 I = 1, L1
               T = B(I,L1) + U2 * B(I,L)
               B(I,L1) = B(I,L1) + T * V1
               B(I,L) = B(I,L) + T * V2
  130       CONTINUE
C
            B(L1,L) = 0.0D0
C
            DO 140 I = 1, IGH
               T = A(I,L1) + U2 * A(I,L)
               A(I,L1) = A(I,L1) + T * V1
               A(I,L) = A(I,L) + T * V2
  140       CONTINUE
C
            IF (.NOT. MATZ) GO TO 150
C
            DO 145 I = LOW, IGH
               T = Z(I,L1) + U2 * Z(I,L)
               Z(I,L1) = Z(I,L1) + T * V1
               Z(I,L) = Z(I,L) + T * V2
  145       CONTINUE
C
  150    CONTINUE
C
  160 CONTINUE
C
  170 RETURN
C     :::::::::: LAST CARD OF QZHES ::::::::::
      END


MODULE nrtype
! Definition of types taken from Numerical Recipes
  INTEGER, PARAMETER :: DP = SELECTED_REAL_KIND(12,100)
  REAL(DP), PARAMETER :: PI=3.141592653589793238462643383279502884197_dp
  REAL(DP), PARAMETER :: PIO2=1.57079632679489661923132169163975144209858_dp
  REAL(DP), PARAMETER :: TWOPI=6.283185307179586476925286766559005768394_dp
  REAL(DP), PARAMETER :: SQRT2=1.41421356237309504880168872420969807856967_dp
  REAL(DP), PARAMETER :: EULER=0.5772156649015328606065120900824024310422_dp
  REAL(DP), PARAMETER :: ONE = 1,  ZERO = 0
END MODULE nrtype

MODULE neo_precision
  USE nrtype
END MODULE neo_precision

MODULE neo_parameters
  USE neo_precision
  USE nrtype
END MODULE neo_parameters

MODULE neo_input
! Input from data files (Boozer)
  USE neo_precision
  INTEGER, DIMENSION(:),     ALLOCATABLE :: ixm, ixn
  INTEGER, DIMENSION(:),     ALLOCATABLE :: pixm, pixn
  INTEGER, DIMENSION(:),     ALLOCATABLE :: i_m, i_n

  REAL(kind=dp),    DIMENSION(:),     ALLOCATABLE :: es
  REAL(kind=dp),    DIMENSION(:),     ALLOCATABLE :: pprime
  REAL(kind=dp),    DIMENSION(:),     ALLOCATABLE :: sqrtg00

  REAL(kind=dp),    DIMENSION(:,:),   ALLOCATABLE :: rmnc,  zmnc,  lmnc
  REAL(kind=dp),    DIMENSION(:,:),   ALLOCATABLE :: bmnc

  REAL(kind=dp) :: flux

  INTEGER  :: m0b, n0b
  INTEGER  :: ns, mnmax, nfp
  INTEGER  :: m_max, n_max
END MODULE neo_input

MODULE neo_work
! Working parameters
  USE neo_precision

  REAL(kind=dp) ::   theta_start
  REAL(kind=dp) ::   theta_end
  REAL(kind=dp) ::   theta_int

  REAL(kind=dp) ::   phi_start
  REAL(kind=dp) ::   phi_end
  REAL(kind=dp) ::   phi_int

  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: cosmth,sinmth
  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: cosnph,sinnph
  REAL(kind=dp),    DIMENSION(:),       ALLOCATABLE :: theta_arr,phi_arr
  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: r,z,l,b
  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: r_tb,z_tb,p_tb,b_tb
  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: r_pb,z_pb,p_pb,b_pb
  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: gtbtb,gpbpb,gtbpb
  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: isqrg,sqrg11,kg,pard
  REAL(kind=dp),    DIMENSION(:,:),     ALLOCATABLE :: bqtphi
END MODULE neo_work

MODULE neo_exchange
! Working parameters
  USE neo_precision

  REAL(kind=dp)              :: b_min, b_max
  INTEGER                    :: nper
  REAL(kind=dp)              :: rt0, rt0_g
  REAL(kind=dp)              :: bmref, bmref_g
  INTEGER                    :: nstep_per, nstep_min, nstep_max
  INTEGER                    :: write_integrate
  INTEGER                    :: write_diagnostic
  INTEGER                    :: write_cur_inte
  REAL(kind=dp)              :: acc_req
  INTEGER                    :: no_bins
  INTEGER                    :: psi_ind
  INTEGER                    :: calc_nstep_max
  REAL(kind=dp)              :: theta_bmin, phi_bmin
  REAL(kind=dp)              :: theta_bmax, phi_bmax
  REAL(kind=dp),    DIMENSION(:),     ALLOCATABLE :: iota
  REAL(kind=dp),    DIMENSION(:),     ALLOCATABLE :: curr_pol
  REAL(kind=dp),    DIMENSION(:),     ALLOCATABLE :: curr_tor
  REAL(kind=dp)              :: fac
  INTEGER                    :: calc_cur
  INTEGER                    :: hit_rat, nfp_rat, nfl_rat
  REAL(kind=dp)              :: delta_theta_rat
END MODULE neo_exchange

MODULE neo_output
  USE neo_precision
  REAL(kind=dp), DIMENSION(:), ALLOCATABLE ::  epspar
  REAL(kind=dp)                            ::  epstot,ctrone,ctrtot
  REAL(kind=dp)                            ::  bareph,barept,drdpsi
  REAL(kind=dp)                            ::  yps
  INTEGER                                  ::  nintfp
  INTEGER                                  ::  ierr
  REAL(kind=dp)                            ::  lambda_b
  REAL(kind=dp)                            ::  lambda_b1, lambda_b2
  REAL(kind=dp)                            ::  lambda_ps1, lambda_ps2
  REAL(kind=dp)                            ::  avnabpsi
END MODULE neo_output

MODULE neo_spline
! Spline arrays
  USE neo_precision
  INTEGER, PARAMETER       ::   mt = 1
  INTEGER, PARAMETER       ::   mp = 1
  INTEGER                  ::   theta_ind, phi_ind
  INTEGER                  ::   ierr

  REAL(kind=dp) ::  theta_d, phi_d

! Spline array for modb
  REAL(kind=dp),    DIMENSION(:,:,:,:), ALLOCATABLE :: b_spl
! Spline array for geodesic curviture
  REAL(kind=dp),    DIMENSION(:,:,:,:), ALLOCATABLE :: k_spl
! Spline array for sqrg11
  REAL(kind=dp),    DIMENSION(:,:,:,:), ALLOCATABLE :: g_spl
! Spline array for parallel derivative
  REAL(kind=dp),    DIMENSION(:,:,:,:), ALLOCATABLE :: p_spl
! Spline array for quasi-toroidal phi component of b
  REAL(kind=dp),    DIMENSION(:,:,:,:), ALLOCATABLE :: q_spl
END MODULE neo_spline

MODULE neo_control
! Control parameters from inpt file
  USE neo_precision
  CHARACTER(120)                     :: in_file                             !SPH
  CHARACTER(120)                     :: out_file                            !SPH
  CHARACTER(120)                      :: cur_file
  INTEGER                            :: theta_n
  INTEGER                            :: phi_n
  INTEGER                            :: s_ind_in
  INTEGER                            :: write_progress
  INTEGER                            :: write_output_files
  INTEGER                            :: spline_test
  INTEGER                            :: max_m_mode, max_n_mode
  INTEGER                            :: lab_swi, inp_swi, ref_swi, eout_swi
  INTEGER                            :: no_fluxs
  INTEGER, DIMENSION(:), ALLOCATABLE :: fluxs_arr
END MODULE neo_control

MODULE neo_units
! Units and Formats
  USE neo_precision
  INTEGER, PARAMETER ::   r_u2  = 4
  INTEGER, PARAMETER ::   r_us  = 5
  INTEGER, PARAMETER ::   r_u23 = 23
  INTEGER, PARAMETER ::   r_ua  = 21
  INTEGER, PARAMETER ::   w_u4  = 10
  INTEGER, PARAMETER ::   r_u10 = 3
  INTEGER, PARAMETER ::   w_us  = 6
  INTEGER, PARAMETER ::   w_u10 = 7
  INTEGER, PARAMETER ::   w_u20 = 8
  INTEGER, PARAMETER ::   w_u30 = 9
  INTEGER, PARAMETER ::   w_u50 = 11
  INTEGER, PARAMETER ::   w_u60 = 12
  INTEGER, PARAMETER ::   w_u70 = 13
  INTEGER, PARAMETER ::   w_u80 = 14
  INTEGER, PARAMETER ::   w_u90 = 15

  INTEGER            ::   w_u6_open, w_u1 = w_u10, w_u2 = w_u20, w_u3 = w_u30,      &
                          w_u5 = w_u50,w_u6 = w_u60, w_u7 = w_u70, w_u8 = w_u80,    &
                          w_u9 = w_u90, r_u1 = r_u10

  CHARACTER(20),PARAMETER :: format220="(500d18.5)"

  CHARACTER*(120) :: arg1, extension    ! LPK
  INTEGER  :: numargs                   ! LPK

END MODULE neo_units

MODULE sizey_bo
  USE neo_precision
! Definition for rk4d_bo also used in main routine neo
  INTEGER            ::  npart
  INTEGER            ::  multra
  INTEGER            ::  ndim
  INTEGER, PARAMETER ::  npq = 4
END MODULE sizey_bo

MODULE partpa_bo
  USE neo_precision
! Exchange between flint_bo and rhs_bo
  USE sizey_bo
  INTEGER                                  :: ipmax
  INTEGER,       DIMENSION(:), ALLOCATABLE :: isw,ipa,icount
  REAL(kind=dp)                            :: pard0,bmod0
  REAL(kind=dp), DIMENSION(:), ALLOCATABLE :: eta
END MODULE partpa_bo

MODULE sizey_cur
  USE neo_precision
! Definition for rk4d_bo also used in main routine neo
  INTEGER            ::  npart_cur
  INTEGER            ::  ndim_cur
  INTEGER, PARAMETER ::  npq_cur = 8
  INTEGER            ::  alpha_cur
END MODULE sizey_cur

MODULE partpa_cur
  USE neo_precision
! Exchange between flint_cur and rhs_cur
  USE sizey_cur
  REAL(kind=dp)                            :: bmod0
  REAL(kind=dp), DIMENSION(:), ALLOCATABLE :: y_part, yfac, sqyfac
  REAL(kind=dp), DIMENSION(:), ALLOCATABLE :: k_fac1, k_fac2
END MODULE partpa_cur

MODULE neo_rk4dbo
  INTERFACE rk4d_bo
     SUBROUTINE rk4d_bo1(x,y,h)
       USE neo_precision
       REAL(dp),               INTENT(inout) ::  x
       REAL(dp),               INTENT(in)    ::  h
       REAL(dp), DIMENSION(:), INTENT(inout) ::  y
     END SUBROUTINE rk4d_bo1
  END INTERFACE
END MODULE neo_rk4dbo

MODULE neo_rk4dcur
  INTERFACE rk4d_cur
     SUBROUTINE rk4d_cur1(x,y,h)
       USE neo_precision
       REAL(dp),               INTENT(inout) ::  x
       REAL(dp),               INTENT(in)    ::  h
       REAL(dp), DIMENSION(:), INTENT(inout) ::  y
     END SUBROUTINE rk4d_cur1
  END INTERFACE
END MODULE neo_rk4dcur

MODULE neo_rhsbo
  INTERFACE rhs_bo
     SUBROUTINE rhs_bo1(phi,y,dery)
       USE neo_precision
       REAL(dp),                             INTENT(in)  ::  phi
       REAL(dp), DIMENSION(:),               INTENT(in)  ::  y
       REAL(dp), DIMENSION(SIZE(y)), TARGET, INTENT(out) ::  dery
     END SUBROUTINE rhs_bo1
  END INTERFACE
END MODULE neo_rhsbo

MODULE neo_rhscur
  INTERFACE rhs_cur
     SUBROUTINE rhs_cur1(phi,y,dery)
       USE neo_precision
       REAL(dp),                             INTENT(in)  ::  phi
       REAL(dp), DIMENSION(:), TARGET,       INTENT(in)  ::  y
       REAL(dp), DIMENSION(SIZE(y)), TARGET, INTENT(out) ::  dery
     END SUBROUTINE rhs_cur1
  END INTERFACE
END MODULE neo_rhscur


program test_fe_he

use types, only: dp
use sto, only: stoints2, get_basis2
use utils, only: assert
use radialscf, only: doscf, kinetic_energy
use hfprint, only: printall, printlam
use mesh, only: meshexp
use feutils, only: define_connect, get_quad_pts, get_parent_quad_pts_wts, &
        get_parent_nodes
use fe, only: feints
implicit none

integer, allocatable :: nbfl(:)
real(dp), allocatable :: focc(:, :)

real(dp), allocatable :: S(:, :, :), T(:, :, :), V(:, :, :), slater(:, :)
integer :: n, Z, m, Nscf, Lmax, ndof
real(dp) :: alpha, Etot, tolE, tolP, Ekin
real(dp), allocatable :: H(:, :, :), P_(:, :, :), C(:, :, :), lam(:, :)

real(dp), allocatable :: xin(:)       ! parent basis nodes
real(dp), allocatable :: xe(:)        ! element coordinates
integer, allocatable :: ib(:, :)       ! basis connectivity: ib(i,j) = index of
   ! basis function associated with local basis function i of element j. 0 = no
   ! associated basis fn.
integer, allocatable :: in(:, :)
real(dp), allocatable :: xiq(:)       ! quadrature points
real(dp), allocatable :: wtq(:)       ! quadrature weights
real(dp), allocatable :: xq(:, :)
integer :: p, Ne, Nb, Nq
real(dp) :: rmin, rmax, a
integer :: l

Lmax = 0
allocate(nbfl(0:Lmax), focc(1, 0:Lmax))
nbfl = 0
focc = 0
focc(:1, 0) = [2]

Z = 2
tolE = 1e-10_dp
tolP = 1e-4_dp
alpha = 0.6_dp
Nscf = 100

rmin = 0
rmax = 30
a = 200
Ne = 4
Nq = 53
p = 9

allocate(xe(Ne+1))
xe = meshexp(rmin, rmax, a, Ne)
allocate(xin(p+1))
call get_parent_nodes(2, p, xin)
allocate(in(p+1, Ne), ib(p+1, Ne))
call define_connect(1, 1, Ne, p, in, ib)
Nb = maxval(ib)
do l = 0, Lmax
    nbfl(l) = Nb
end do
print *, "radial DOFs =", Nb
allocate(xiq(Nq), wtq(Nq), xq(Nq, Ne))
call get_parent_quad_pts_wts(1, Nq, xiq, wtq)
call get_quad_pts(xe, xiq, xq)

n = maxval(nbfl)
ndof = sum(nbfl)
print *, "total  DOFs =", ndof
allocate(H(n, n, 0:Lmax), S(n, n, 0:Lmax), T(n, n, 0:Lmax), V(n, n, 0:Lmax))
m = ndof*(ndof+1)/2
allocate(slater(m*(m+1)/2, 0:2*Lmax))
call feints(Z, xin, xe, ib, xiq, wtq, S, T, V, slater)

allocate(P_(n, n, 0:Lmax), C(n, n, 0:Lmax), lam(n, 0:Lmax))

H = T + V

print *, "SCF cycle:"
call doscf(nbfl, H, slater, S, focc, Nscf, tolE, tolP, alpha, C, P_, lam, Etot)
Ekin = kinetic_energy(nbfl, P_, T)
call printlam(nbfl, lam, Ekin, Etot)

call assert(abs(Etot - (-2.861679995_dp)) < 1e-6_dp)

end program


c  This program finds the force on each of a set of particles interacting
c  via a long-range 1/r**2 force law.
c
c  The number of processes must be even, and the total number of points
c  must be exactly divisible by the number of processes.
c
c  This is the MPI version.
c
        program nbody
        implicit none
        include 'mpif.h'
        integer myrank, ierr, nprocs, npts, nlocal
        integer pseudohost, NN, MM, PX, PY, PZ, FX, FY, FZ
        real G
        parameter (pseudohost = 0)
        parameter (NN=10000, G = 1.0)
        parameter (MM=0, PX=1, PY=2, PZ=3, FX=4, FY=5, FZ=6)
        real dx(0:NN-1), dy(0:NN-1), dz(0:NN-1)
        real dist(0:NN-1), sq(0:NN-1)
        real fac(0:NN-1), tx(0:NN-1), ty(0:NN-1), tz(0:NN-1)
        real p(0:6,0:NN-1), q(0:6,0:NN-1)
        integer i, j, k, dest, src
        double precision timebegin, timeend
        integer status(MPI_STATUS_SIZE)
        integer newtype
        double precision ran
        integer iran
c
c  Initialize MPI, find rank of each process, and the number of processes
c
        call mpi_init (ierr)
        call mpi_comm_rank (MPI_COMM_WORLD, myrank, ierr)
        call mpi_comm_size (MPI_COMM_WORLD, nprocs, ierr)
c
c  One process acts as the host and reads in the number of particles
c
        if (myrank .eq. pseudohost) then
           open (4,file='nbody.input',status='old',err=998)
           if (mod(nprocs,2) .eq. 0) then
              read  (4,*) npts
              if (npts .gt. nprocs*NN) then
                 print *,'Warning!! Size out of bounds!!'
                 npts = -1
              else if (mod(npts,nprocs) .ne. 0) then
                 print *,'Number of processes must divide npts'
                 npts = -1
              end if
           else
              print *,'Number of processes must be even'
              npts = -1
           end if
        end if
c
c  The number of particles is broadcast to all processes
c
        call mpi_bcast (npts, 1, MPI_INTEGER, pseudohost,
     #                  MPI_COMM_WORLD, ierr)
c
c  Abort if number of processes and/or particles is incorrect
c
        if (npts .eq. -1) goto 999
c
c  Work out number of particles in each process
c
        nlocal  = npts/nprocs
c
c  The pseudocode hosts initializes the particle data and sends each
c  process its particles.
c
        if (myrank .eq. pseudohost) then
           iran = myrank + 111
           do i=0,nlocal-1
              p(MM,i) = sngl(ran(iran))
              p(PX,i) = sngl(ran(iran))
              p(PY,i) = sngl(ran(iran))
              p(PZ,i) = sngl(ran(iran))
              p(FX,i) = 0.0
              p(FY,i) = 0.0
              p(FZ,i) = 0.0
           end do
           do k=0,nprocs-1
              if (k .ne. pseudohost) then
                 do i=0,nlocal-1
                    q(MM,i) = sngl(ran(iran))
                    q(PX,i) = sngl(ran(iran))
                    q(PY,i) = sngl(ran(iran))
                    q(PZ,i) = sngl(ran(iran))
                    q(FX,i) = 0.0
                    q(FY,i) = 0.0
                    q(FZ,i) = 0.0
                 end do
                 call mpi_send (q, 7*nlocal, MPI_REAL,
     #                          k, 100, MPI_COMM_WORLD, ierr)
              end if
           end do
        else
           call mpi_recv (p, 7*nlocal, MPI_REAL,
     #                    pseudohost, 100, MPI_COMM_WORLD, status, ierr)
        end if
c
c  Initialization is now complete. Start the clock and begin work.
c  First each process makes a copy of its particles.
c
        timebegin = mpi_wtime ()
        do i= 0,nlocal-1
            q(MM,i) = p(MM,i)
            q(PX,i) = p(PX,i)
            q(PY,i) = p(PY,i)
            q(PZ,i) = p(PZ,i)
            q(FX,i) = 0.0
            q(FY,i) = 0.0
            q(FZ,i) = 0.0
        end do
c
c  Now the interactions between the particles in a single process are
c  computed.
c
        do i=0,nlocal-1
          do j=i+1,nlocal-1
            dx(i) = p(PX,i) - q(PX,j)
            dy(i) = p(PY,i) - q(PY,j)
            dz(i) = p(PZ,i) - q(PZ,j)
            sq(i) = dx(i)**2+dy(i)**2+dz(i)**2
            dist(i) = sqrt(sq(i))
            fac(i) = p(MM,i) * q(MM,j) / (dist(i) * sq(i))
            tx(i) = fac(i) * dx(i)
            ty(i) = fac(i) * dy(i)
            tz(i) = fac(i) * dz(i)
            p(FX,i) = p(FX,i)-tx(i)
            q(FX,j) = q(FX,j)+tx(i)
            p(FY,i) = p(FY,i)-ty(i)
            q(FY,j) = q(FY,j)+ty(i)
            p(FZ,i) = p(FZ,i)-tz(i)
            q(FZ,j) = q(FZ,j)+tz(i)
          end do
        end do
c
c  The processes are arranged in a ring. Data will be passed in an
C  anti-clockwise direction around the ring.
c
        dest = mod (nprocs+myrank-1, nprocs)
        src  = mod (myrank+1, nprocs)
c
c  Each process interacts with the particles from its nprocs/2-1
c  anti-clockwise neighbors. At the end of this loop p(i) in each
c  process has accumulated the force from interactions with particles
c  i+1, ...,nlocal-1 in its own process, plus all the particles from its
c  nprocs/2-1 anti-clockwise neighbors. The "home" of the q array is
C  regarded as the process from which it originated. At the end of
c  this loop q(i) has accumulated the force from interactions with
C  particles 0,...,i-1 in its home process, plus all the particles from the
C  nprocs/2-1 processes it has rotated to.
c
        do k=0,nprocs/2-2
          call mpi_sendrecv_replace (q, 7*nlocal, MPI_REAL, dest, 200,
     #                         src, 200, MPI_COMM_WORLD, status, ierr)
          do i=0,nlocal-1
            do j=0,nlocal-1
              dx(i) = p(PX,i) - q(PX,j)
              dy(i) = p(PY,i) - q(PY,j)
              dz(i) = p(PZ,i) - q(PZ,j)
              sq(i) = dx(i)**2+dy(i)**2+dz(i)**2
              dist(i) = sqrt(sq(i))
              fac(i) = p(MM,i) * q(MM,j) / (dist(i) * sq(i))
              tx(i) = fac(i) * dx(i)
              ty(i) = fac(i) * dy(i)
              tz(i) = fac(i) * dz(i)
              p(FX,i) = p(FX,i)-tx(i)
              q(FX,j) = q(FX,j)+tx(i)
              p(FY,i) = p(FY,i)-ty(i)
              q(FY,j) = q(FY,j)+ty(i)
              p(FZ,i) = p(FZ,i)-tz(i)
              q(FZ,j) = q(FZ,j)+tz(i)
            end do
          end do
        end do
c
c  Now q is rotated once more so it is diametrically opposite its home
c  process. p(i) accumulates forces from the interaction with particles
c  0,..,i-1 from its opposing process. q(i) accumulates force from the
c  interaction of its home particles with particles i+1,...,nlocal-1 in
c  its current location.
c
        if (nprocs .gt. 1) then
          call mpi_sendrecv_replace (q, 7*nlocal, MPI_REAL, dest, 300,
     #                          src, 300, MPI_COMM_WORLD, status, ierr)
          do i=nlocal-1,0,-1
            do j=i-1,0,-1
              dx(i) = p(PX,i) - q(PX,j)
              dy(i) = p(PY,i) - q(PY,j)
              dz(i) = p(PZ,i) - q(PZ,j)
              sq(i) = dx(i)**2+dy(i)**2+dz(i)**2
              dist(i) = sqrt(sq(i))
              fac(i) = p(MM,i) * q(MM,j) / (dist(i) * sq(i))
              tx(i) = fac(i) * dx(i)
              ty(i) = fac(i) * dy(i)
              tz(i) = fac(i) * dz(i)
              p(FX,i) = p(FX,i)-tx(i)
              q(FX,j) = q(FX,j)+tx(i)
              p(FY,i) = p(FY,i)-ty(i)
              q(FY,j) = q(FY,j)+ty(i)
              p(FZ,i) = p(FZ,i)-tz(i)
              q(FZ,j) = q(FZ,j)+tz(i)
            end do
          end do
c
c  In half the processes we include the interaction of each particle with
c  the corresponding particle in the opposing process.
c
          if (myrank .lt. nprocs/2) then
            do i=0,nlocal-1
              dx(i) = p(PX,i) - q(PX,i)
              dy(i) = p(PY,i) - q(PY,i)
              dz(i) = p(PZ,i) - q(PZ,i)
              sq(i) = dx(i)**2+dy(i)**2+dz(i)**2
              dist(i) = sqrt(sq(i))
              fac(i) = p(MM,i) * q(MM,i) / (dist(i) * sq(i))
              tx(i) = fac(i) * dx(i)
              ty(i) = fac(i) * dy(i)
              tz(i) = fac(i) * dz(i)
              p(FX,i) = p(FX,i)-tx(i)
              q(FX,i) = q(FX,i)+tx(i)
              p(FY,i) = p(FY,i)-ty(i)
              q(FY,i) = q(FY,i)+ty(i)
              p(FZ,i) = p(FZ,i)-tz(i)
              q(FZ,i) = q(FZ,i)+tz(i)
            end do
          endif
c
c  Now the q array is returned to its home process.
c
          dest = mod (nprocs+myrank-nprocs/2, nprocs)
          src  = mod (myrank+nprocs/2, nprocs)
          call mpi_sendrecv_replace (q, 7*nlocal, MPI_REAL, dest, 400,
     #                         src, 400, MPI_COMM_WORLD, status, ierr)
        end if
c
c  The p and q arrays are summed to give the total force on each particle.
c
        do i=0,nlocal-1
          p(FX,i) = p(FX,i) + q(FX,i)
          p(FY,i) = p(FY,i) + q(FY,i)
          p(FZ,i) = p(FZ,i) + q(FZ,i)
        end do
c
c  Stop clock and write out timings
c
        timeend = mpi_wtime ()
        print *,'Node', myrank,'  Elapsed time: ',
     #          timeend-timebegin,' seconds'
c
c  Do a barrier to make sure the timings are written out first
c
        call mpi_barrier (MPI_COMM_WORLD, ierr)
c
c  Each process returns its forces to the pseudohost which prints them out.
c
        if (myrank .eq. pseudohost) then
           open (7,file='nbody.output')
           write (7,100) (p(FX,i),p(FY,i),p(FZ,i),i=0,nlocal-1)
           call mpi_type_vector (nlocal, 3, 7, MPI_REAL, newtype, ierr)
           call mpi_type_commit (newtype, ierr)
           do k=0,nprocs-1
              if (k .ne. pseudohost) then
                 call mpi_recv (q(FX,0), 1, newtype,
     #                          k, 100, MPI_COMM_WORLD, status, ierr)
                 write (7,100) (q(FX,i),q(FY,i),q(FZ,i),i=0,nlocal-1)
              end if
           end do
        else
           call mpi_type_vector (nlocal, 3, 7, MPI_REAL, newtype, ierr)
           call mpi_type_commit (newtype, ierr)
           call mpi_send (p(FX,0), 1, newtype,
     #                    pseudohost, 100, MPI_COMM_WORLD, ierr)
        end if

c
c  Close MPI
c
 999       call mpi_finalize (ierr)

        stop

 100       format(3e15.6)

c
c  Abort if no input file
c
 998          print *,'input file nbody.input missing or invalid'
        call mpi_abort (ierr)

        end


c
c**************************
c
        subroutine ltslv(a,n,b,m)

c       linear equation solver for real lower triangular system
c       the equation to solve is ax=b where a is n x n and
c       b is n x m; a is lower triangular in symmetric storage mode
c       result is returned in b

        real a(6),b(n,m)

        do 50 i=1,m

           do 20 j=1,n

           j1=j*(j-1)/2+1
           jl=j1+j-2

             temp=0.0
             do 10 k=j1,jl
             temp=temp+a(k)*b(k-j1+1,i)
10           continue

           b(j,i)=(b(j,i)-temp)/a(jl+1)
20         continue

50      continue
        return
        end


module core
  ! Apparently the use of module constants is broken in F2PY despite the use of F2PY specific type declarations.
  ! With those declarations the Python module compiles but segfaults when loaded.
  ! https://stackoverflow.com/questions/19983850/f2py-access-module-parameter-from-subroutine
  ! Therefore these constants have been re-declared in the functions.
  ! With more time this issue could be debugged further.

  implicit none
!  integer, parameter :: DIMS = 3
!  integer, parameter :: REAL_KIND = 8
!  real(kind=REAL_KIND), parameter :: G = 1
!  real(kind=REAL_KIND), parameter :: MIN_DIST = 0.01
!
!  integer, parameter :: OUTPUT_FILE_UNIT = 11
!  integer, parameter :: MAX_PATH_LEN = 200
!  character(len=*), parameter :: DEFAULT_OUTPUT_PATH = "../run/output"
contains
  function force(x, m, i, n_objs, g, min_dist)
    implicit none

    ! For some reason replacing these with the f2py variable declarations
    ! does not work like it does in the other functions
    integer, parameter :: DIMS = 3
    integer, parameter :: REAL_KIND = 8
    ! real(kind=REAL_KIND), parameter :: G = 1
    ! real(kind=REAL_KIND), parameter :: MIN_DIST = 0.01

    integer, intent(in) :: i, n_objs
    real(kind=REAL_KIND), intent(in) :: x(DIMS,n_objs), m(n_objs), g, min_dist

    real(kind=REAL_KIND) :: force(DIMS), dist
    integer :: j
    force = 0

    do j=1,n_objs
      dist = sqrt(sum((x(:,i) - x(:,j))**2))
      ! Clipping prevents overflow
      if (dist > min_dist) then
        force = force + g*m(i)*m(j)*(x(:,j) - x(:,i)) / dist**3
      end if
    end do
  end function force

  subroutine iterate(x, v, a, m, dt, n_steps, n_objs, g, min_dist, print_interval, write_interval, path)
    ! Note that on Python side the argument n_objs is optional, since it can be automatically determined from the input arrays
    implicit none

    integer, parameter :: DIMS = 3
    integer, parameter :: REAL_KIND = 8
    integer, parameter :: MAX_PATH_LEN = 200
    character(len=*), parameter :: DEFAULT_OUTPUT_PATH = "../run/output"

    ! Double precision is used for NumPy compatibility and more accurate results
    integer, intent(in) :: n_steps, n_objs
    integer, intent(in), optional :: print_interval, write_interval
    character(len=*), intent(in), optional :: path
    real(kind=REAL_KIND), intent(inout) :: x(DIMS,n_objs), v(DIMS,n_objs), a(DIMS,n_objs)
    real(kind=REAL_KIND), intent(in) :: m(n_objs), dt, g, min_dist

    integer :: i, iter, print_interval_checked, write_interval_checked, written
    character(len=MAX_PATH_LEN) :: path_checked
    real(kind=REAL_KIND) :: a_prev(DIMS)
    written = 0

    ! Processing of optional arguments
    if(present(print_interval)) then
      print_interval_checked = print_interval
    else
      print_interval_checked = 0
    end if
    if(present(write_interval)) then
      write_interval_checked = write_interval
    else
      write_interval_checked = 0
    end if
    if(present(path)) then
      path_checked = path
    else
      path_checked = DEFAULT_OUTPUT_PATH
    end if

    ! Simulation loop
    do iter=1,n_steps
      x = x + v*dt + 0.5*a*dt**2
      do i=1,n_objs
        a_prev = a(:,i)
        a(:,i) = force(x, m, i, n_objs, g, min_dist) / m(i)
        v(:,i) = v(:,i) + 0.5*(a(:,i) + a_prev)*dt
      end do

      ! Writing and printing
      if (write_interval_checked /= 0 .and. mod(iter, write_interval_checked) == 0) then
        call write_progress(x, v, a, iter, n_objs, path)
        written = written + 1
      end if
      if (print_interval_checked /= 0 .and. mod(iter, print_interval_checked) == 0) then
        call print_progress(x, v, a, iter, dt, n_steps, n_objs)
      end if
    end do
  end subroutine iterate

  subroutine print_arr_1d(arr, name, unit)
    implicit none
    integer, parameter :: REAL_KIND = 8

    real(kind=REAL_KIND), intent(in) :: arr(:)
    character(len=*), intent(in), optional :: name
    integer, intent(in), optional :: unit

    integer :: i, unit_checked

    if (present(unit)) then
      unit_checked = unit
    else
      unit_checked = 6
    end if

    if(present(name)) then
      write(unit_checked, *) name
    end if

    do i=1, size(arr)
      write(unit_checked, *) arr(i)
    end do
  end subroutine print_arr_1d

  subroutine print_arr_2d(arr, name, unit)
    implicit none
    integer, parameter :: REAL_KIND = 8

    real(kind=REAL_KIND), intent(in) :: arr(:, :)
    character(len=*), intent(in), optional :: name
    integer, intent(in), optional :: unit

    integer :: arr_shape(2)
    integer :: i, unit_checked
    arr_shape = shape(arr)

    if (present(unit)) then
      unit_checked = unit
    else
      unit_checked = 6
    end if

    if(present(name)) then
      write(unit_checked, *) name
    end if

    do i=1, arr_shape(2)
      write(unit_checked, *) arr(:, i)
    end do

  end subroutine

  subroutine print_progress(x, v, a, i, dt, n_steps, n_objs)
    implicit none

    integer, parameter :: DIMS = 3
    integer, parameter :: REAL_KIND = 8

    integer, intent(in) :: i, n_steps, n_objs
    real(kind=REAL_KIND), intent(in) :: x(DIMS,n_objs), v(DIMS,n_objs), a(DIMS,n_objs)
    real(kind=REAL_KIND), intent(in) :: dt

    print *, "Iteration: ", i, "/", n_steps
    print *, "Time: ", i*dt
    print *, "Objects: ", n_objs
    call print_arr_2d(x, "x")
    call print_arr_2d(v, "v")
    call print_arr_2d(a, "a")
  end subroutine print_progress

  subroutine write_progress(x, v, a, i, n_objs, path)
    implicit none
    integer, parameter :: DIMS = 3
    integer, parameter :: REAL_KIND = 8
    integer, parameter :: OUTPUT_FILE_UNIT = 11
    integer, parameter :: MAX_PATH_LEN = 200

    integer, intent(in) :: i, n_objs
    real(kind=REAL_KIND), intent(in) :: x(DIMS,n_objs), v(DIMS,n_objs), a(DIMS,n_objs)
    character(len=*), intent(in) :: path

    integer :: ios
    character(len=MAX_PATH_LEN) :: full_path

    write(full_path, "(a,'/',I0.6,'.txt')", iostat=ios) trim(path), i
    if (ios /= 0) then
      print *,"Failed to create full path for output file: ", full_path
      stop
    end if
    open(OUTPUT_FILE_UNIT, file=full_path, iostat=ios, status="new", form="formatted")
    if (ios /= 0) then
      print *, "Failed to create output file to path: ", full_path
    end if
    call print_arr_2d(x, "x", OUTPUT_FILE_UNIT)
    call print_arr_2d(v, "v", OUTPUT_FILE_UNIT)
    call print_arr_2d(a, "a", OUTPUT_FILE_UNIT)

    close(OUTPUT_FILE_UNIT)
  end subroutine write_progress
end module core