      USE DAESL_INT
      USE MP_TYPES
      IMPLICIT NONE 
  
!              NEQ = Number of equations
!              NCON = Number of extra constraints
      INTEGER, PARAMETER :: NEQ=5, NCON = 2
      REAL, PARAMETER :: MASS=1.0, LENGTH=1.1, GRAVITY=9.806650
  
      REAL T, Y(NEQ), YPRIME(NEQ), TEND, ATOL(NEQ), TOL, LEN
      INTEGER NOUT, IDO, I, NSTEPS
      REAL, TARGET :: RPARAM(3)
      TYPE (S_FCN_DATA) USER_DATA
      EXTERNAL GCN 
      
!              Pass Mass, Pendulum length and G as parameters
      RPARAM(1) = MASS
      RPARAM(2) = LENGTH
      RPARAM(3) = GRAVITY
      USER_DATA%RDATA=>RPARAM
      
!              Initial values for y, guesses for initial y'
      Y = 0.0
      Y(1) = LENGTH
      
      YPRIME = 0.0
      TOL = 1.0E-5
      ATOL = TOL
      
      CALL UMACH(2, NOUT)
      WRITE (NOUT, 5) 

      NSTEPS = 5
!              Always set IDO=1 on first call
      IDO = 1
      DO I = 1, NSTEPS
!              Output solution at T=10,20,30,40,50
        T = 10.0 * (I-1)
        TEND = 10.0 * I 
        
!              Set IDO = 3 on last call
        IF (I.EQ.NSTEPS) IDO = 3
        
!              User-supplied Jacobian matrix (IUJAC=1)
!              Use new algorithm to get compatible y' 
        CALL DAESL (T, TEND, IDO, Y, YPRIME, GCN, NCON=NCON, RTOL=TOL, &
          ATOL=ATOL, IYPR=2, IUJAC=1, MAXSTEPS=50000, &
          FCN_DATA=USER_DATA)
          
!              LEN = pendulum length (should be constant) 
        LEN = SQRT(Y(1)**2 + Y(2)**2)
        WRITE (NOUT, 10) T, Y(1), Y(2), LEN
      END DO
      
    5 FORMAT (8X,'T',14X,'Y(1)',11X,'Y(2)',11X,'Length',/)
   10 FORMAT (4F15.7)
      END
  
  
      SUBROUTINE GCN (T, Y, YPRIME, DELTA, D, LDD, IRES, FCN_DATA)
      USE MP_TYPES
      IMPLICIT NONE
      
!              Simple swinging pendulum problem
      REAL T, Y(*), YPRIME(*), DELTA(*), D(LDD,*), MASS, &
           LENGTH, GRAVITY, MG, LSQ
      INTEGER IRES, LDD
      TYPE (S_FCN_DATA), OPTIONAL, INTENT(INOUT) :: FCN_DATA
      
      MASS = FCN_DATA%RDATA(1)
      LENGTH = FCN_DATA%RDATA(2)
      GRAVITY = FCN_DATA%RDATA(3)
      MG = MASS * GRAVITY
      LSQ = LENGTH**2
      
      SELECT CASE (IRES)
      
!              F_I defined here
      CASE(1)
        DELTA(1) = Y(3) - YPRIME(1)
        DELTA(2) = Y(4) - YPRIME(2)
        DELTA(3) = -Y(1) * Y(5) - MASS * YPRIME(3)
        DELTA(4) = -Y(2) * Y(5) - MASS * YPRIME(4) - MG
        DELTA(5) = MASS * (Y(3)**2 + Y(4)**2) - MG * Y(2) - LSQ * Y(5)
        
!              D(I,J) = D(F_I)/D(Y_J)
      CASE(2)
        D(1, 3) = 1.0
        D(2, 4) = 1.0
        D(3, 1) = -Y(5)
        D(3, 5) = -Y(1)
        D(4, 2) = -Y(5)
        D(4, 5) = -Y(2)
        D(5, 2) = -MG
        D(5, 3) = MASS * 2.0 * Y(3)
        D(5, 4) = MASS * 2.0 * Y(4)
        D(5, 5) = -LSQ
        
!              D(I,J) = D(F_I)/D(YPRIME_J)
      CASE(3)
        D(1, 1)= -1.0
        D(2, 2)= -1.0
        D(3, 3)= -MASS
        D(4, 4)= -MASS

        
!              DELTA(I) = D(F_I)/DT
      CASE(4)
        DELTA(1:5) = 0.0
        
!              DELTA(I) = G_I
!              D(I,J) = D(G_I)/D(Y_J)
      CASE(5)
        DELTA(1) = Y(1)**2 + Y(2)**2 - LSQ
        DELTA(2) = Y(1) * Y(3) + Y(2) * Y(4)
        D(1, 1) = 2.0 * Y(1)
        D(1, 2) = 2.0 * Y(2)
        D(1, 3) = 0.0
        D(1, 4) = 0.0
        D(1, 5) = 0.0
        D(2, 1) = Y(3)
        D(2, 2) = Y(4)
        D(2, 3) = Y(1)
        D(2, 4) = Y(2)
        D(2, 5) = 0.0
  
      END SELECT
      END
