      Program TB0201

      Implicit None

*   Initial, Final Point
      Double Precision  xMin
      Parameter         (xMin = 0.0D0)
      Double Precision  xMax
      Parameter         (xMax = 1.0D1)
      Double Precision  yMin
      Parameter         (yMin = 1.0D0)
*   Data Dimension
      Integer           DNum
      Parameter         (DNum = 9)
*   Data Storage
      Integer           GridN(DNum)
      Double Precision  Gap(DNum), y4El(DNum), y5El(DNum), y4Ty(DNum),
     $                  y5Ty(DNum), y4Ip(DNum), y5Ip(DNum)
*   Dummy
      Integer           I
*   Function
      Double Precision  Func, Ext
      External          Func, Ext

      Open(11, File = '02-03.txt')
 1000 Format(F6.3,6F14.7)
 1010 Format(A6,6A14)
 1020 Format(A6,3A28)

*   Data Declaration
      Data GridN /20,50,100,200,500,1000,2000,5000,10000/
      Do 10 I = 1, DNum
        Gap(I) = (xMax - xMin) / DBLE(GridN(I))
  10  Continue

      Do 110 I = 1, DNum
        Call El(y4El(I), y5El(I), xMin, xMax, yMin, GridN(I), Gap(I))
        Call Ty(y4Ty(I), y5Ty(I), xMin, xMax, yMin, GridN(I), Gap(I))
        Call Ip(y4Ip(I), y5Ip(I), xMin, xMax, yMin, GridN(I), Gap(I))
  110 Continue

      Write(11,1020) '------', '----------------------------',
     $               '----------------------------',
     $               '----------------------------'
      Write(11,1020) ' ', 'Euler Method     ',
     $               'Taylor Series    ', 'Implicit Method   '
      Write(11,1020) ' ', 'Eq.(2.6)       ',
     $               'Eq.(2.10)      ', 'Eq.(2.18)      '
      Write(11,1010) 'h  ', 'y(4)  ', 'y(5)  ',  'y(4)  ',
     $               'y(5)  ', 'y(4)  ', 'y(5)  '
      Write(11,1020) '------', '----------------------------',
     $               '----------------------------',
     $               '----------------------------'
      Do 20 I = 1, DNum
        Write(11,1000) Gap(I), y4El(I), y5El(I), y4Ty(I), y5Ty(I),
     $                 y4Ip(I), y5Ip(I)
  20  Continue
      Write(11,1020) '------', '----------------------------',
     $               '----------------------------',
     $               '----------------------------'

      End Program TB0201

*-----------------------------------------------------------------------
*
      Subroutine El(y4, y5, xMin, xMax, yMin, GridN, Gap)

      Implicit None

      Double Precision  y4, y5, xMin, xMax, yMin, Gap
      Integer           GridN
*   Dummy
      Integer           I
*   Value for n+1, n+0
      Double Precision  xn1, yn1, xn0, yn0
*   Function
      Double Precision  Func, Ext
      External          Func, Ext

      xn0 = xMin
      yn0 = yMin
      Do 10 I = 1, GridN
        xn1 = DBLE(I) / DBLE(GridN) * xMax +
     $        (1.0D0 - DBLE(I) / DBLE(GridN)) * xMin
        yn1 = yn0 + (xn1 - xn0) * Func(xn0,yn0)
        If(DAbs(xn1-4.0D0).LT.1.0D-10) y4 = yn1 - Ext(xn1)
        If(DAbs(xn1-5.0D0).LT.1.0D-10) y5 = yn1 - Ext(xn1)
        xn0 = xn1
        yn0 = yn1
  10  Continue

      End Subroutine El

*-----------------------------------------------------------------------
*
      Subroutine Ty(y4, y5, xMin, xMax, yMin, GridN, Gap)

      Implicit None

      Double Precision  y4, y5, xMin, xMax, yMin, Gap
      Integer           GridN
*   Dummy
      Integer           I
*   Value for n+1, n+0
      Double Precision  xn1, yn1, xn0, yn0
*   Function
      Double Precision  Func, Ext, FuncDx, FuncDy
      External          Func, Ext, FuncDx, FuncDy

      xn0 = xMin
      yn0 = yMin
      Do 10 I = 1, GridN
        xn1 = DBLE(I) / DBLE(GridN) * xMax +
     $        (1.0D0 - DBLE(I) / DBLE(GridN)) * xMin
        yn1 = yn0 + (xn1 - xn0) * Func(xn0,yn0) + 0.5D0 * (xn1 - xn0) *
     $        (xn1 - xn0) * (FuncDx(xn0,yn0) + Func(xn0,yn0) *
     $        FuncDy(xn0,yn0))
        If(DAbs(xn1-4.0D0).LT.1.0D-10) y4 = yn1 - Ext(xn1)
        If(DAbs(xn1-5.0D0).LT.1.0D-10) y5 = yn1 - Ext(xn1)
        xn0 = xn1
        yn0 = yn1
  10  Continue

      End Subroutine Ty

*-----------------------------------------------------------------------
*
      Subroutine Ip(y4, y5, xMin, xMax, yMin, GridN, Gap)

      Implicit None

      Double Precision  y4, y5, xMin, xMax, yMin, Gap
      Integer           GridN
*   Dummy
      Integer           I
*   Value for n+1, n+0
      Double Precision  xn1, yn1, xn0, yn0
*   Function
      Double Precision  Func, Ext, FuncG
      External          Func, Ext, FuncG

      xn0 = xMin
      yn0 = yMin
      Do 10 I = 1, GridN
        xn1 = DBLE(I) / DBLE(GridN) * xMax +
     $        (1.0D0 - DBLE(I) / DBLE(GridN)) * xMin
        yn1 = yn0 * (1.0D0 + 0.5D0 * (xn1 - xn0) * FuncG(xn0)) /
     $        (1.0D0 - 0.5D0 * (xn1 - xn0) * FuncG(xn1))
        If(DAbs(xn1-4.0D0).LT.1.0D-10) y4 = yn1 - Ext(xn1)
        If(DAbs(xn1-5.0D0).LT.1.0D-10) y5 = yn1 - Ext(xn1)
        xn0 = xn1
        yn0 = yn1
  10  Continue

      End Subroutine Ip

*-----------------------------------------------------------------------
*
      Function Func(x,y)

      Implicit None
      Double Precision  Func, x, y

      Func = - x * y

      End Function Func

*-----------------------------------------------------------------------
*
      Function FuncDx(x,y)

      Implicit None
      Double Precision  FuncDx, x, y

      FuncDx = - y

      End Function FuncDx

*-----------------------------------------------------------------------
*
      Function FuncDy(x,y)

      Implicit None
      Double Precision  FuncDy, x, y

      FuncDy = - x

      End Function FuncDy

*-----------------------------------------------------------------------
*
      Function FuncG(x)

      Implicit None
      Double Precision  FuncG, x

      FuncG = - x

      End Function FuncG

*-----------------------------------------------------------------------
*
      Function Ext(x)

      Implicit None
      Double Precision  Ext, x

      Ext = DExp(-x*x/2.0D0)

      End Function Ext


module tensor_util_lib
contains
  subroutine next_ind(ind,n,d)
    integer, intent(in) :: d
    integer, intent(in) :: n(d)
    integer, intent(inout) :: ind(d)
    integer k
    k=1
    ind(1) = ind(1) + 1
    do while (ind(k) > n(k) .and.  k < d)
       ind(k)=ind(k)-n(k)
       ind(k+1) = ind(k+1) + 1
       k = k + 1
    end do
   !print *,'get_ind called, i=',i
   !print *,'get_ind called, d=',d
   !print *,'get_ind called, ind=',ind
  end subroutine next_ind

  subroutine permute(a,M,n,d,b,prm)
    integer, intent(in) :: d
    integer, intent(in) :: n(d)
    integer, intent(in) :: M
    integer, dimension(d), intent(in) :: prm
    double precision, intent(in) :: a(M)
    double precision, intent(out) :: b(M)
    double precision t1,t2
    integer i,j
    integer :: ind(d)
    integer :: tmp(d)
    !A really stupid code:  array (b) is just a permutation of the array a.
    !A one-to-one index correspondence is given as
    !a(i(1) + (i2-1)*n1 + (i3-1)*n1*n2 + ... + (id-1)*n1*...*n(d-1)) =
    !b(i(prm(1))+ (i(prm(2)))*n(prm(1))....
    !for all multidimensional indices, do:
    !b(dot(ind(prm), sz1(prm)))=a(dot(ind,sz1))
    tmp(2:d)=n(prm(1:d-1))
    tmp(1)=1
    do i=2,d
       tmp(i)=tmp(i-1)*tmp(i)
    end do
    ind(1:d)=1
    do i=1,M
       !call get_ind(i,ind,n,d)
       j=dot_product(tmp,ind(prm)-1)+1 !tmp = 1,2 (4x2) ind=(2,2) ind-1=(1,1)
       !1+2+1 = 4
       !ind = 1,4 (?) ind(prm) = 4,1 ind(prm-1)=3,0 3+2*0 + 1 =4 (?????)
       !2,2 -> 2 + 1*2 = 4
       !print *,'ind:',ind(1:d),'tmp:',tmp
       call next_ind(ind,n,d)
       !print *,'tmp',tmp
       !print *,'ind:',ind
       !print *,'prm:',prm
       !print *,'ind(prm):',ind(prm)
       !print *,'i=',i,'j=',j,'prm:',prm,'sz:',n
       b(j)=a(i)
    end do
  end subroutine permute
  subroutine get_ind(i,ind,n,d)
    integer, intent(in) :: d

    integer, intent(in) :: n(d)
    integer, intent(out) :: ind(d)
    integer, intent(in) :: i
    integer j
    j = i
    do k=1,d
       !print *,'inside get_ind loop,j=',j
       !print *,'inside get_ind loop,n=',n
       ind(k) = mod(j-1,n(k))+1
       j=(j-ind(k))/n(k)+1
    end do
   !print *,'get_ind called, i=',i
   !print *,'get_ind called, d=',d
   !print *,'get_ind called, ind=',ind

  end subroutine get_ind

end module tensor_util_lib


      SUBROUTINE COBYLA (CALCFC, N,M,X,RHOBEG,RHOEND,IPRINT,MAXFUN,
     & W,IACT,IFAIL,NFVALS,IOUT,IFILE,FF,GG)
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      INTEGER IFAIL,NFVALS,IOUT
      CHARACTER*(*) IFILE
      EXTERNAL CALCFC
      DIMENSION X(*),W(*),IACT(*),GG(*)
C
C     This subroutine minimizes an objective function F(X) subject to M
C     inequality constraints on X, where X is a vector of variables that has
C     N components. The algorithm employs linear approximations to the
C     objective and constraint functions, the approximations being formed by
C     linear interpolation at N+1 points in the space of the variables.
C     We regard these interpolation points as vertices of a simplex. The
C     parameter RHO controls the size of the simplex and it is reduced
C     automatically from RHOBEG to RHOEND. For each RHO the subroutine tries
C     to achieve a good vector of variables for the current size, and then
C     RHO is reduced until the value RHOEND is reached. Therefore RHOBEG and
C     RHOEND should be set to reasonable initial changes to and the required
C     accuracy in the variables respectively, but this accuracy should be
C     viewed as a subject for experimentation because it is not guaranteed.
C     The subroutine has an advantage over many of its competitors, however,
C     which is that it treats each constraint individually when calculating
C     a change to the variables, instead of lumping the constraints together
C     into a single penalty function. The name of the subroutine is derived
C     from the phrase Constrained Optimization BY Linear Approximations.
C
C     The user must set the values of N, M, RHOBEG and RHOEND, and must
C     provide an initial vector of variables in X. Further, the value of
C     IPRINT should be set to 0, 1, 2 or 3, which controls the amount of
C     printing during the calculation. Specifically, there is no output if
C     IPRINT=0 and there is output only at the end of the calculation if
C     IPRINT=1. Otherwise each new value of RHO and SIGMA is printed.
C     Further, the vector of variables and some function information are
C     given either when RHO is reduced or when each new value of F(X) is
C     computed in the cases IPRINT=2 or IPRINT=3 respectively. Here SIGMA
C     is a penalty parameter, it being assumed that a change to X is an
C     improvement if it reduces the merit function
C                F(X)+SIGMA*MAX(0.0,-C1(X),-C2(X),...,-CM(X)),
C     where C1,C2,...,CM denote the constraint functions that should become
C     nonnegative eventually, at least to the precision of RHOEND. In the
C     printed output the displayed term that is multiplied by SIGMA is
C     called MAXCV, which stands for 'MAXimum Constraint Violation'. The
C     argument MAXFUN is an integer variable that must be set by the user to a
C     limit on the number of calls of CALCFC, the purpose of this routine being
C     given below. The value of MAXFUN will be altered to the number of calls
C     of CALCFC that are made. The arguments W and IACT provide real and
C     integer arrays that are used as working space. Their lengths must be at
C     least N*(3*N+2*M+11)+4*M+6 and M+1 respectively.
C
C     In order to define the objective and constraint functions, we require
C     a subroutine that has the name and arguments
C                SUBROUTINE CALCFC (N,M,X,F,CON)
C                DIMENSION X(*),CON(*)  .
C     The values of N and M are fixed and have been defined already, while
C     X is now the current vector of variables. The subroutine should return
C     the objective and constraint functions at X in F and CON(1),CON(2),
C     ...,CON(M). Note that we are trying to adjust X so that F(X) is as
C     small as possible subject to the constraint functions being nonnegative.
C
C     Partition the working space array W to provide the storage that is needed
C     for the main calculation.
C
      MPP=M+2
      ICON=1
      ISIM=ICON+MPP
      ISIMI=ISIM+N*N+N
      IDATM=ISIMI+N*N
      IA=IDATM+N*MPP+MPP
      IVSIG=IA+M*N+N
      IVETA=IVSIG+N
      ISIGB=IVETA+N
      IDX=ISIGB+N
      IWORK=IDX+N
      CALL COBYLB (CALCFC,N,M,MPP,X,RHOBEG,RHOEND,IPRINT,MAXFUN,W(ICON),
     1  W(ISIM),W(ISIMI),W(IDATM),W(IA),W(IVSIG),W(IVETA),W(ISIGB),
     2  W(IDX),W(IWORK),IACT,IFAIL,NFVALS,IOUT,IFILE,FF,GG)
      RETURN
      END


!!$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
! TYPE  : MODULE
! NAME  : BasicMathFunc
! OBJECT: TYPE(bmathf)
! USED  :
! DATE  : 2017-12-05
! AUTHOR: Hengyue Li
!--------------
! DESCRIPTION:
!             some basic math functions
! STANDARD:
!
!
!
! USING LIST:
!            :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
!
!
! avalable sets:
!
! avalable gets:
!                  [fun] get_random_int(start,endto)
!                         Get a random integer in range [start,endto]
!                         if input are integer*4, return integer*4
!                         if input are integer*8, return integer*8
!
!                  [sub] get_x_range_equal_interval_p(nx,start,endto,x):
!                             integer(8)nx; real(8)::start,endto,x(nx)
!                             creat a list of real(8) value range from start to endto
!
!                  [sub] get_random_array(A)
!                        real(8)/complex(8)::A(:)
!
!                  [sub] get_normalized_array(A)
!                        The same as get_radom_array, but keepy <A|A> = 1
!
!                  [fun] bcombination(m,n)
!                        integer(8)::m,n  /  integer::m,n
!                        integer*8::bcombination
!                        bcombination=c^m_n
!
!                  [fun] get_min_array_index(n,x)
!                        integer::get_min_array_index,n
!                        real(8)::x(n)
!                        return the position of the min point in a array.
!
! avalable Is :
!                  [fun] IsTwoValuePercentageTheSame(E1,E2,rePre)
!                        check two real*8 E1,E2 are the same or not with relavant different rePre
!
!                  [fun] IsIntegerArrayTheSame(IntArry)
!                        input an integer array  check if all elemets are the same
!
!
!!$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$

module BasicMathFunc
    implicit none

    TYPE bmathf
         private

         contains
         procedure,pass::get_random_int8_p
         procedure,pass::get_random_int4_p
         generic::get_random_int => get_random_int8_p, get_random_int4_p

         procedure,pass::get_x_range_equal_interval=>get_x_range_equal_interval_p

         procedure,pass::get_min_array_index

         procedure,nopass::bcombination8
         procedure,nopass::bcombination4
         generic::bcombination => bcombination8 , bcombination4

         procedure,private,PASS::get_radom_array_r
         procedure,private,PASS::get_radom_array_C
         procedure,private,PASS::get_normalized_array_rp
         procedure,private,PASS::get_normalized_array_Cp
         generic::get_random_array=>get_radom_array_r,get_radom_array_C
         generic::get_normalized_array=>get_normalized_array_Rp,get_normalized_array_Cp

         procedure,nopass::IsTwoValuePercentageTheSame
         procedure,nopass::IsIntegerArrayTheSame

    END TYPE

   private::get_random_int8_p,get_random_int4_p,get_x_range_equal_interval_p
   PRIVATE::get_radom_array_r,get_radom_array_C,get_normalized_array_rp,get_normalized_array_Cp
   private::bcombination8,bcombination4
   private::get_min_array_index
   private::IsTwoValuePercentageTheSame
   private::IsIntegerArrayTheSame
  !-----------------------
    contains

    integer(8) function get_random_int8_p(self,start,endto)
               implicit none
               class(bmathf),intent(inout)::self
               integer(8),intent(in)::start,endto
               !------------------------------
               real(8)::r
               if ( start .le. endto    )then
                   call random_number(r)
                   get_random_int8_p = int( r * (  endto  -  start +1_8 ) ,8) + start
               else
                    write(*,*)"in get_random_int8, start <= endto should be kept,error201706051031";stop
               end if
    end function

    integer function get_random_int4_p(self,start,endto)
      implicit none
      class(bmathf),intent(inout)::self
      integer,intent(in)::start,endto
      !------------------------------
      real(8)::r
      if ( start .le. endto    )then
          call random_number(r)
          get_random_int4_p = int( r * (  endto  -  start +1 ) ,4) + start
      else
           write(*,*)"in get_random_int4, start <= endto should be kept,error201706051031";stop
      end if
end function

    subroutine get_x_range_equal_interval_p(self,nx,start,endto,x)
               implicit none
               class(bmathf),intent(inout)::self
               integer(8),intent(in)::nx
               real(8),intent(in)::start,endto
               real(8),intent(out)::x(nx)
               !----------------
               real(8)::deltax
               integer(8)::jc
               deltax = (endto - start)/nx
               do jc=1_8,nx
                  x(jc) = start + ( jc - 1_8 ) * deltax + deltax/2._8
               end do
    end subroutine

    subroutine get_radom_array_r(self,A)
               implicit none
               class(bmathf),intent(inout)::self
               real(8),intent(out)::A(:)
               !-------------------------------------
               call random_number(A)
    end subroutine

    subroutine get_radom_array_c(self,A)
               implicit none
               class(bmathf),intent(inout)::self
               COMPLEX(8),intent(out)::A(:)
               !-------------------------------------
               integer(8)::n
               real(8),allocatable::R(:),C(:)
               n = size(A)
               allocate( R(n) )
               allocate( C(n) )
               call get_radom_array_r(self,R)
               call get_radom_array_r(self,C)
               A = CMPLX( R  ,  C   ,8   )
               deallocate(R)
               deallocate(C)
    end subroutine

    subroutine get_normalized_array_rp(SELF,A)
               implicit none
               class(bmathf),intent(inout)::self
               real(8),intent(out)::A(:)
               !-------------------------------------
               call get_radom_array_r(self,A)
               A = A/dsqrt( sum(A**2) )
    end subroutine

    subroutine get_normalized_array_cp(SELF,A)
               implicit none
               class(bmathf),intent(inout)::self
               COMPLEX(8),intent(out)::A(:)
               !-------------------------------------
               call get_radom_array_c(self,A)
               A = A/dsqrt(  REAL(  sum(A*  CONJG(A) ) )   )
    end subroutine

    integer(8) function bcombination8(mu,n) !c^mu_n
           implicit none
           integer(8),intent(in)::mu,n
           !---------------------
           integer(8)::jc,m
           m = min( mu, n-mu )
           bcombination8=1_8
           do jc=n,n-m+1_8,-1_8
              bcombination8=bcombination8*jc   !;write(*,*)jc,bcombination
           end do
           do jc=1_8,m
              bcombination8=bcombination8/jc
           end do
    end function

    integer*8 function bcombination4(mu,n)
      implicit none
      integer,intent(in)::mu,n
      !---------------------
      integer*8::mu8,n8
      mu8 = mu * 1_8
      n8  = n  * 1_8
      bcombination4 = bcombination8(mu8,n8)
    endfunction

    integer function get_min_array_index(self,n,x)
            implicit none
            class(bmathf),intent(inout)::self
            integer,intent(in)::n
            real(8),intent(in)::x(n)
            !----------------------------------------
            integer::jc
            get_min_array_index = 1
            do jc = 2 , n
                if (  x(get_min_array_index) .ge.   x(jc)    )then
                    get_min_array_index = jc
                end if
            end do
    end function

    function IsTwoValuePercentageTheSame(e1,e2,Repre) result(CheckDiffSmall)
      implicit none
      real*8,intent(in)::E1,E2,rePre
      logical::CheckDiffSmall
      !----------------------------------------
      if (E1.eq.E2)then
        CheckDiffSmall = .true.
      else
        if ( abs(E1-E2) / max( abs(e1) , abs(e2)   ) .le.rePre)then
           CheckDiffSmall = .true.
        else
          CheckDiffSmall = .false.
        endif
      endif
    endfunction

    logical function IsIntegerArrayTheSame(IntArry)
      implicit none
      integer,intent(in)::IntArry(:)
      !----------------------------------------------
      integer::s ,jc
      integer,allocatable::t(:)
      s = size(IntArry)
      allocate(t(s))
      s = IntArry(1)
      if (  sum(abs(s - IntArry)).eq.0   )then
        IsIntegerArrayTheSame = .true.
      else
        IsIntegerArrayTheSame = .false.
      endif
    endfunction

end module


C$Procedure      PROP2B ( Propagate a two-body solution )

      SUBROUTINE PROP2B ( GM, PVINIT, DT, PVPROP )

C$ Abstract
C
C     Given a central mass and the state of massless body at time t_0,
C     this routine determines the state as predicted by a two-body
C     force model at time t_0 + DT.
C
C$ Disclaimer
C
C     THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE
C     CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S.
C     GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE
C     ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE
C     PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS"
C     TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY
C     WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A
C     PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC
C     SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE
C     SOFTWARE AND RELATED MATERIALS, HOWEVER USED.
C
C     IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA
C     BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT
C     LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND,
C     INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS,
C     REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE
C     REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY.
C
C     RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF
C     THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY
C     CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE
C     ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE.
C
C$ Required_Reading
C
C     None.
C
C$ Keywords
C
C     CONIC
C     EPHEMERIS
C     UTILITY
C
C$ Declarations

      DOUBLE PRECISION      GM
      DOUBLE PRECISION      PVINIT ( 6 )
      DOUBLE PRECISION      DT
      DOUBLE PRECISION      PVPROP ( 6 )

C$ Brief_I/O
C
C     Variable  I/O  Description
C     --------  ---  --------------------------------------------------
C     GM         I   Gravity of the central mass.
C     PVINIT     I   Initial state from which to propagate a state.
C     DT         I   Time offset from initial state to propagate to.
C     PVPROP     O   The propagated state.
C
C$ Detailed_Input
C
C     GM         is the gravitational constant G times the mass M of the
C                central body.
C
C     PVINIT     is the state at some specified time relative to the
C                central mass.  The mass of the object is assumed to
C                be negligible when compared to the central mass.
C
C     DT         is a offset in time from the time of the initial
C                state to which the two-body state should be
C                propagated. (The units of time and distance must be
C                the same in GM, PVINIT, and DT).
C
C$ Detailed_Output
C
C     PVPROP     is the two-body propagation of the initial state
C                DT units of time past the epoch of the initial state.
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     1) If GM is not positive, the error SPICE(NONPOSITIVEMASS) will
C        be signalled.
C
C     2) If the position of the initial state is the zero vector, the
C        error SPICE(ZEROPOSITION) will be signalled.
C
C     3) If the velocity of the initial state is the zero vector, the
C        error SPICE(ZEROVELOCITY) will be signalled.
C
C     4) If the cross product of the position and velocity of PVINIT
C        has squared length of zero, the error SPICE(NONCONICMOTION)
C        will be signalled.
C
C     5) The value of DT must be "reasonable".  In other words, DT
C        should be less than 10**20 seconds for realistic solar system
C        orbits specified in the MKS system.  (The actual bounds
C        on DT are much greater but require substantial computation.)
C        The "reasonableness" of DT is checked at run-time.  If DT is
C        so large that there is a danger of floating point overflow
C        during computation, the error SPICE(DTOUTOFRANGE) is
C        signalled and a message is generated describing the problem.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     This routine uses a universal variables formulation for the
C     two-body motion of an object in orbit about a central mass. It
C     propagates an initial state to an epoch offset from the
C     epoch of the initial state by time DT.
C
C     This routine does not suffer from the finite precision
C     problems of the machine that are inherent to classical
C     formulations based on the solutions to Kepler's equation:
C
C           n( t - T ) = E - e Sin(E)         elliptic case
C           n( t - T ) = e sinh(F) - F        hyperbolic case
C
C     The derivation used to determine the propagated state is a
C     slight variation of the derivation in Danby's book
C     `Fundamentals of Celestial Mechanics' [1] .
C
C$ Examples
C
C     When the eccentricity of an orbit is near 1, and the epoch
C     of classical elements is near the epoch of periapse, classical
C     formulations that propagate a state from elements tend to
C     lack robustness due to the finite precision of floating point
C     machines. In those situations it is better to use a universal
C     variables formulation to propagate the state.
C
C     By using this routine, you need not go from a state to elements
C     and back to a state. Instead, you can get the state from an
C     initial state.
C
C     If PV is your initial state and you want the state 3600
C     seconds later, the following call will suffice.
C
C          Look up GM somewhere
C
C          DT = 3600.0D0
C
C          CALL PROP2B ( GM, PV, DT, PVDT )
C
C     After the call, PVDT will contain the state of the
C     object 3600 seconds after the time it had state PV.
C
C$ Restrictions
C
C     Users should be sure that GM, PVINIT and DT are all in the
C     same system of units ( for example MKS ).
C
C$ Literature_References
C
C     [1] `Fundamentals of Celestial Mechanics', Second Edition
C         by J.M.A. Danby;  Willman-Bell, Inc., P.O. Box 35025
C         Richmond Virginia;  pp 168-180
C
C$ Author_and_Institution
C
C     W.L. Taber     (JPL)
C     N.J. Bachman   (JPL)
C
C$ Version
C
C-    SPICELIB Version 2.1.0, 31-AUG-2005 (NJB)
C
C        Updated to remove non-standard use of duplicate arguments
C        in VSCL call.
C
C-    SPICELIB Version 2.0.1, 22-AUG-2001 (EDW)
C
C        Corrected ENDIF to END IF.
C
C-    Spicelib Version 2.0.0 16-May-1995  (WLT)
C
C        The initial guess at a solution to Kepler's equation was
C        modified slightly and a loop counter was added to the
C        bisection loop together with logic that will force termination
C        of the bisection loop.
C
C-    Spicelib Version 1.0.0, 10-Mar-1992 (WLT)
C
C
C-&

C$ Index_Entries
C
C      Propagate state vector using two-body force model
C
C-&

C$ Revisions
C
C-    SPICELIB Version 2.1.0, 31-AUG-2005 (NJB)
C
C        Updated to remove non-standard use of duplicate arguments
C        in VSCL call.
C
C-&

C
C     SPICELIB functions
C
      DOUBLE PRECISION      BRCKTD
      DOUBLE PRECISION      DPMAX
      DOUBLE PRECISION      VDOT
      DOUBLE PRECISION      VNORM

      INTEGER               BRCKTI

      LOGICAL               RETURN
      LOGICAL               VZERO

C
C     Local Parameters
C
      INTEGER               BUFSIZ
      PARAMETER           ( BUFSIZ = 3 )

      INTEGER               MAXBIT
      PARAMETER           ( MAXBIT = 64 )

C
C     Local variables
C
C
C     The following quantities are needed in the solution of Kepler's
C     equation and in the propagation of the input state.  They are
C     described as they are introduced in the code below.
C
      DOUBLE PRECISION      B
      DOUBLE PRECISION      B2RV
      DOUBLE PRECISION      BQ
      DOUBLE PRECISION      BR
      DOUBLE PRECISION      BR0
      DOUBLE PRECISION      C0
      DOUBLE PRECISION      C1
      DOUBLE PRECISION      C2
      DOUBLE PRECISION      C3
      DOUBLE PRECISION      E
      DOUBLE PRECISION      EQVEC  ( 3 )
      DOUBLE PRECISION      F
      DOUBLE PRECISION      FX2
      DOUBLE PRECISION      H2
      DOUBLE PRECISION      HVEC   ( 3 )
      DOUBLE PRECISION      KFUN
      DOUBLE PRECISION      KFUNL
      DOUBLE PRECISION      KFUNU
      DOUBLE PRECISION      LOGBND
      DOUBLE PRECISION      LOWER
      DOUBLE PRECISION      OLDX
      DOUBLE PRECISION      PC
      DOUBLE PRECISION      PCDOT
      DOUBLE PRECISION      POS    ( 3 )
      DOUBLE PRECISION      Q
      DOUBLE PRECISION      QOVR0
      DOUBLE PRECISION      R0
      DOUBLE PRECISION      RV
      DOUBLE PRECISION      TMPVEC ( 3 )
      DOUBLE PRECISION      UPPER
      DOUBLE PRECISION      VC
      DOUBLE PRECISION      VCDOT
      DOUBLE PRECISION      VEL    ( 3 )
      DOUBLE PRECISION      X
      DOUBLE PRECISION      X2
      DOUBLE PRECISION      X3

C
C     The variables below store intermediate results that can be
C     reused if PVINIT is supplied more than once to this routine.
C     In this way, the number of redundant computations can be reduced.
C
      DOUBLE PRECISION      SAVEPV ( 6, BUFSIZ )
      DOUBLE PRECISION      SAVEGM (    BUFSIZ )
      DOUBLE PRECISION      SBOUND (    BUFSIZ )
      DOUBLE PRECISION      SB2RV  (    BUFSIZ )
      DOUBLE PRECISION      SBQ    (    BUFSIZ )
      DOUBLE PRECISION      SBR0   (    BUFSIZ )
      DOUBLE PRECISION      SF     (    BUFSIZ )
      DOUBLE PRECISION      SQOVR0 (    BUFSIZ )

      INTEGER               NEWEST (    BUFSIZ )
      INTEGER               NSAVED

C
C     Variables used to bracket X in our solution of Kepler's equation.
C
      DOUBLE PRECISION      BOUND
      DOUBLE PRECISION      FIXED
      DOUBLE PRECISION      LOGDPM
      DOUBLE PRECISION      LOGF
      DOUBLE PRECISION      LOGMXC
      DOUBLE PRECISION      MAXC
      DOUBLE PRECISION      ROOTF

      INTEGER               BUMPED
      INTEGER               I
      INTEGER               K
      INTEGER               MOSTC
      INTEGER               LCOUNT

      LOGICAL               NEW

C
C     Save everything.
C
      SAVE

C
C     Initial values
C
      DATA                  NSAVED  / 0       /
      DATA                  NEWEST  / 1, 2, 3 /

C
C     Standard SPICE error handling.
C
      IF ( RETURN () ) THEN
         RETURN
      ELSE
         CALL CHKIN ( 'PROP2B' )
      END IF

C
C     Life will be easier if we use POS and VEL to hold the state.
C
      POS(1) = PVINIT(1)
      POS(2) = PVINIT(2)
      POS(3) = PVINIT(3)

      VEL(1) = PVINIT(4)
      VEL(2) = PVINIT(5)
      VEL(3) = PVINIT(6)

C
C     If we propagate many states from the same initial state,
C     most of the variables used to propagate the state will
C     not change in value.
C
C     To save time needed to compute these variables, we recompute
C     variables that depend upon the initial state only when the
C     initial state is not one of those already buffered by this
C     routine.
C
C     Determine whether or not this GM and state are the same as the
C     one of those already buffered.  Note that we look through the
C     saved states and GM from the most recently input values of PVINIT
C     and GM to the oldest saved state and GM.
C
C     NEWEST(1)  contains the most recently input initial conditions
C     NEWEST(2)  contains the next most recently input intial conditions
C                etc.
C
C     Also note that when this routine starts up there will be no
C     buffered states or GMs.  Every time we encounter a new state, we
C     will increment the number of saved states NSAVED until we have
C     BUFSIZ states buffered.  From that point on, when a new state is
C     encountered we will overwrite the oldest buffered state.
C
      I   =  0
      NEW = .TRUE.

      DO WHILE ( ( I .LT. NSAVED ) .AND. NEW )

         I   = I + 1
         K   = NEWEST(I)

         NEW =     ( PVINIT(1) .NE. SAVEPV(1,K) )
     .        .OR. ( PVINIT(2) .NE. SAVEPV(2,K) )
     .        .OR. ( PVINIT(3) .NE. SAVEPV(3,K) )
     .        .OR. ( PVINIT(4) .NE. SAVEPV(4,K) )
     .        .OR. ( PVINIT(5) .NE. SAVEPV(5,K) )
     .        .OR. ( PVINIT(6) .NE. SAVEPV(6,K) )
     .        .OR. ( GM        .NE. SAVEGM(  K) )

      END DO

      IF ( .NOT. NEW ) THEN
C
C        We update the order vector NEWEST so that the state being
C        used this time becomes the "youngest" state.
C
         K      = I
         BUMPED = NEWEST(K)

         DO I = K, 2, -1
            NEWEST(I) = NEWEST(I-1)
         END DO

         NEWEST(1) = BUMPED
         K         = BUMPED

C
C        Now look up all of the other saved quantities.
C
         B2RV   = SB2RV (K)
         BOUND  = SBOUND(K)
         BQ     = SBQ   (K)
         BR0    = SBR0  (K)
         F      = SF    (K)
         QOVR0  = SQOVR0(K)

      ELSE

C
C        We have a new state, new GM or both.  First let's make sure
C        there is nothing obviously wrong with them.  (We buffer
C        only states, GMs and intermediate values that are "good.")
C        First check for nonpositive mass.
C
         IF ( GM .LE. 0.0D0 ) THEN
            CALL SIGERR ( 'SPICE(NONPOSITIVEMASS)'   )
            CALL CHKOUT ( 'PROP2B'                   )
            RETURN
         END IF

C
C        Next for a zero position vector
C
         IF ( VZERO(POS) ) THEN
            CALL SIGERR ( 'SPICE(ZEROPOSITION)'   )
            CALL CHKOUT ( 'PROP2B'                )
            RETURN
         END IF

C
C        Finally for a zero velocity vector
C
         IF ( VZERO(VEL) ) THEN
            CALL SIGERR ( 'SPICE(ZEROVELOCITY)'   )
            CALL CHKOUT ( 'PROP2B'                )
            RETURN
         END IF

C
C        Obvious problems have been checked. Here are the relevant
C        equations. Let ...
C
C           GM        be the gravitational attraction of the central
C                     mass.
C
C           POS and   be the initial position and velocity respectively
C           VEL       of the orbiting object.
C
C           R0       be the magnitude of the position vector POS
C
C           RV       be the value of the dot product  POS * VEL
C
         R0 = VNORM ( POS      )
         RV = VDOT  ( POS, VEL )

C
C        Let HVEC be the specific angular momentum vector and let Q be
C        the distance at periapse.
C
C                   1)    HVEC  =   POS  x  VEL
C
C                                       2
C                   2)    H2    = |HVEC|  =  GM*(1+E)*Q
C
C
         CALL VCRSS ( POS,  VEL,     HVEC )
         H2 = VDOT  ( HVEC, HVEC          )

C
C        Let's make sure we are not in the pathological case of
C        rectilinear motion.
C
         IF ( H2 .EQ. 0 ) THEN
            CALL SIGERR ( 'SPICE(NONCONICMOTION)'   )
            CALL CHKOUT ( 'PROP2B'                  )
            RETURN
         END IF

C
C        Let E be the eccentricity of the orbit.
C
C        Let QVEC be the unit vector that points toward perihelion, and
C        let EQVEC be QVEC scaled by E.
C
C                                   VEL X HVEC      POS
C                    1)  E*QVEC  =  ----------  -   ---
C                                       GM           R0
C
C
C                                         VEL X HVEC      POS
C                    2)  E       = NORM ( ----------  -   --- )
C                                            GM            R0
C
C
         CALL VCRSS ( VEL,       HVEC,                      TMPVEC  )
         CALL VLCOM ( 1.0D0/GM,  TMPVEC,  -1.0D0/R0,  POS,  EQVEC   )

         E  = VNORM ( EQVEC )

C
C        Solve the equation H2 = GM*Q*(1+E) for Q.
C
         Q  = H2 / ( GM * (1+E) )

C
C        From the discussion of the universal variables formulation in
C        Danby's book on pages 174 and 175 (see the reference listed
C        above) you can show that by making the substitutions
C
C              F  =  1 - E
C
C        and
C
C                       _____
C                      /  Q
C              S  =   / -----    X   = B * X
C                   \/   GM
C
C
C        that DT satisfies the universal variables Kepler's equation:
C
C                                   2     2     2        2
C              DT =  B*R0*X*C_1( F*X ) + B *RV*X C_2( F*X )
C
C                                               3        2
C                                      +   B*Q*X C_3( F*X )
C
C                 =  KFUN( X )
C
C        (where C_k is used to denote the Stumpff functions. This is
C        the universal variables formulation of Kepler's equation.
C        KFUN is our abbreviation for "Kepler function.")
C
C        (One might wonder, "Why make such a change of variables?"
C        By making this substitution early in the derivation supplied
C        in Danby's book, you can always deal with physically
C        meaningful quantities --- the pure numeric value of F and the
C        distance of periapse.  Thus one does not need to be concerned
C        about infinite or negative semi-major axes or with discussing
C        how to interpret these somewhat artificial artifacts of the
C        classical derivations for two body motion.)
C
C        Given the unique X for which this Kepler's equation is
C        satisfied, we can compute the state of the orbiting object
C        at a time DT past the epoch of the state POS and VEL.
C        Evidently we will need the constants:
C
         F      = 1.0D0 - E
         B      = DSQRT ( Q / GM )
         BR0    = B*R0
         B2RV   = B*B*RV
         BQ     = B*Q

C
C        The state corresponding to the value of X that solves this
C        equation is given by
C
C              PC * POS + VC * VEL              ( position )
C
C        and
C
C              PCDOT * POS + VCDOT * VEL        ( velocity )
C
C        where
C                                            2        2
C           ( 1 )    PC    =  1  -  ( Q/R0 )X C_2( F*X )
C
C                                            3        2
C           ( 2 )    VC    =  DT -  ( B*Q  )X C_3( F*X )
C
C
C                                       Q               2
C           ( 3 )    PCDOT =     -  ( ------ ) X C_1( F*X )
C                                     B*R*R0
C
C                                      B*Q     2        2
C           ( 4 )    VCDOT =  1  -  (  ---  ) X C_2( F*X )
C                                      B*R
C
C        Here R denotes the distance from the center of CP*POS + CV*VEL
C        It turns out that R can be computed as:
C
C                                        2     2             2
C           ( 5 )   B*R    = B*R0 C_0(F*X ) + B *RV X C_1(F*X )
C
C                                                 2       2
C                                        +   B*Q X C_2(F*X )
C
C
C        Therefore we will also need the constant
C
         QOVR0   = Q / R0

C
C        We will have to find the unique value of X such that
C
C             DT = KFUN ( X )
C
C        where KFUN stands for the "Kepler function" defined by the
C        equation below:
C
C                                   2
C        KFUN(X) =   B*R0*X * C_1(FX )
C
C                   2     2        2
C                + B *RV*X * C_2(FX )
C
C                         3        2
C                +   B*Q*X * C_3(FX )
C
C
C        (There is a unique solution to this equation. KFUN(X) is
C        unbounded above and below and is an increasing function
C        over all real X for all non-rectilinear orbits. To see this
C        we note that the variable X is a function of DT and is given
C        by the integral from 0 to DT of the differential:
C
C                   dt
C                 ------
C                 B*R(t)
C
C        where R(t) is the range of the body as a function of time.
C        Therefore X is an increasing function of DT, and DT must
C        also be an increasing function of X.
C
C        Thus, there is a unique value of X  that solves this
C        equation).
C
C        If F is less than zero, we can have the computation of C0,...
C        overflow.  This is because for X < 0
C
C
C               C_0(X) = COSH( DSQRT(-X) )
C
C               C_1(X) = SINH( DSQRT(-X) )
C                        -----------------
C                              DSQRT(-X)
C
C
C
C        and from the recursion relationship we know that
C
C
C               C_2(X) =  ( 1/0! - C_0(X) ) / X
C
C               C_3(X) =  ( 1/1! - C_1(X) ) / X
C
C
C                         1 - COSH( DSQRT(-X) )
C               C_2(X) = ------------------------
C                                  X
C
C                         1  - SINH( DSQRT(-X) ) / DSQRT(-X)
C               C_3(X) = -----------------------------------
C                                    X
C
C        Clearly for negative values of F*X*X having large magnitude,
C        it is easy to get an overflow.
C
C        In the case when F is less than 0 we choose X so that we can
C        compute all of the following:
C
C               | COEF_0 * X**0 * C_0(FX**2) |
C
C               | COEF_1 * X**1 * C_1(FX**2) |
C
C               | COEF_2 * X**2 * C_2(FX**2) |
C
C               | COEF_3 * X**3 * C_3(FX**2) |
C
C
C         where COEF_n are coefficients that will be used in forming
C         linear combinations of X**n C_n(FX**2) terms.
C
C         The variable portion of the last 3 terms above can be
C         rewritten as:
C
C
C                                   SINH ( DSQRT(-F)*|X| )
C        | X**1 * C_1(FX**2) |  =   ----------------------
C                                          DSQRT(-F)
C
C
C
C                                   1 - COSH( DSQRT(-F)*|X| )
C        | X**2 * C_2(FX**2) |  =  ----------------------------
C                                             -F
C
C
C                                  DSQRT(-F)*|X|   - SINH(DSQRT(-F)*|X|)
C        | X**3 * C_3(FX**2) |  =  -------------------------------------
C                                              F*DSQRT(-F)
C
C
C        For large |X| the absolute values of these expressions are well
C        approximated by
C
C                                         0.0
C               COSH( DSQRT(-F)|X| ) * |F|
C
C                                         -0.5
C               SINH( DSQRT(-F)|X| ) * |F|
C
C                                         -1.0
C               COSH( DSQRT(-F)|X| ) * |F|
C
C                                         -1.5
C               SINH( DSQRT(-F)|X| ) * |F|
C
C
C        For large |X| the logarithms of these expressions are well
C        approximated by:
C
C
C               DSQRT(-F)|X| - LOG(2) - 0.0*LOG(-F)
C
C               DSQRT(-F)|X| - LOG(2) - 0.5*LOG(-F)
C
C               DSQRT(-F)|X| - LOG(2) - 1.0*LOG(-F)
C
C               DSQRT(-F)|X| - LOG(2) - 1.5*LOG(-F)
C
C        respectively.
C
C
C        To ensure that we can form a linear combination of these terms
C        we will require that:
C
C
C           |COEF_N*X**N * C_N(FX**2)| < DPMAX / 4
C
C
C
C        for N=0,1,2,3.  This is equivalent to
C
C              LOG ( X**N * C_N(FX**2) )   <      LOG ( DPMAX )
C            + LOG (|COEF_N|)                   - 2 LOG ( 2     )
C
C
C
C        or
C
C              LOG ( X**N * C_N(FX**2) )   <      LOG ( DPMAX    )
C                                             -   LOG ( |COEF_N| )
C                                             - 2*LOG ( 2        ).
C
C
C        Replacing the left hand side with the magnitude expressions
C        computed above we have:
C
C            DSQRT(-F)|X| - LOG(2) - N*0.5*LOG( -F )  <   LOG ( DPMAX  )
C                                                      -  LOG (|COEF_N|)
C                                                      -2*LOG ( 2      )
C
C         So that:
C
C
C            |X|  <    {   LOG ( DPMAX  )
C                        - LOG (|COEF_N|)
C                        - LOG (  2     )
C                        + LOG ( -F     )*N*0.5 } / DSQRT(-F)
C
C         Let MAXC be the maximum of 1.0D0 and the various coefficients
C         of the Stumpff functions.  We can then set our absolute value
C         bound on X to be:
C
C
C             MIN        LOG(DPMAX/2) - LOG(MAXC) + (n/2)LOG(-F)
C            n = 0,3  {  -----------------------------------------  }
C                               DSQRT(-F)
C
C        (Actually we know that the minimum must occur for n = 0 or
C        for n = 3).
C
C
         MAXC   = MAX   ( 1.0D0, DABS(BR0),
     .                           DABS(B2RV),
     .                           DABS(BQ),
     .                           DABS(QOVR0/BQ) )

         IF ( F .LT. 0 ) THEN

            LOGMXC = LOG   ( MAXC            )
            LOGDPM = LOG   ( DPMAX() / 2.0D0 )

            FIXED  = LOGDPM  - LOGMXC

            ROOTF  = DSQRT ( - F             )
            LOGF   = LOG   ( - F             )

            BOUND  = MIN (  (FIXED              )/ROOTF,
     .                      (FIXED + 1.5D0*LOGF )/ROOTF )

C
C           Note that in the above, we can always perform the division
C           by ROOTF.  To see this we note that -F is at least the
C           machine precision (we got it by subtracting E from 1.)
C           Thus its square root is a reasonably large number (if F is
C           10**-N then ROOTF is 10**(-N/2) )  The value of FIXED is
C           about 3*M where M is the largest exponent such that 2**M
C           is representable on the host machine.  Thus BOUND is at
C           worst M*10**(N/2)  This will always be computable.
C
        ELSE

C
C
C           In the case when F is non-negative we must be sure we
C           can compute all of the following.
C
C               | COEF_0 * X**0 * C_0(FX**2) | < | COEF_0          |
C
C               | COEF_1 * X**1 * C_1(FX**2) | < | COEF_1*|X|      |
C
C               | COEF_2 * X**2 * C_2(FX**2) | < | COEF_2*X**2 / 2 |
C
C               | COEF_3 * X**3 * C_3(FX**2) | < | COEF_3*X**3 / 6 |
C
C           If we assume that COEF_0 is computable, all of these are
C           bounded above by:
C
C                       | MAX(COEF_1,...COEF_3) * X**3 / 6 |
C
C           We want to make sure we can add these terms so we need to
C           make sure that
C
C              | MAX(COEF_1,...,COEF_3) * X**3 / 6 | < DPMAX() / 4.
C
C           Thus we need:
C
C              |X**3| <          1.5*DPMAX / MAX(COEF_1,...,COEF_3)
C              |X|    <  DCBRT ( 1.5*DPMAX / MAX(COEF_1,...,COEF_3) )
C
C           (We'll use logarithms to compute the upper bound for |X|.)
C
            LOGBND =      ( DLOG(1.5D0) + DLOG(DPMAX()) - DLOG(MAXC) )
     .               /      3.0D0

            BOUND  = DEXP ( LOGBND )

         END IF
C
C        All the obvious problems have been checked, move everybody
C        on the list down and put the new guy on top of the list.
C

         NSAVED = BRCKTI ( NSAVED+1, 1, BUFSIZ )
         BUMPED = NEWEST ( NSAVED              )

         DO I = NSAVED, 2, -1
            NEWEST(I) = NEWEST(I-1)
         END DO

         NEWEST(1) = BUMPED
         K         = BUMPED

         SAVEPV(1,K) = PVINIT(1)
         SAVEPV(2,K) = PVINIT(2)
         SAVEPV(3,K) = PVINIT(3)
         SAVEPV(4,K) = PVINIT(4)
         SAVEPV(5,K) = PVINIT(5)
         SAVEPV(6,K) = PVINIT(6)
         SAVEGM(  K) = GM

C
C        Finally we save the results of all of the above
C        computations so that we won't have to do them again,
C        if this initial state and GM are entered again.
C
         SB2RV (K) = B2RV
         SBOUND(K) = BOUND
         SBQ   (K) = BQ
         SBR0  (K) = BR0
         SF    (K) = F
         SQOVR0(K) = QOVR0

      END IF
C
C
C     We are now ready to find the unique value of X such that
C
C             DT = KFUN ( X )
C
C     First we must bracket the root. The basic idea is this:
C
C     1) KFUN(0) = 0 so we will let one endpoint of our initial
C        guess of a bracketing interval be 0.
C
C     2) We get our initial guess at the other endpoint of the
C        bracketing interval by recalling that
C
C                   dt
C         dX  =   ------
C                 B*R(t)
C
C        From this observation it follows that
C
C                   DT
C          X  <  -------
C                   B*Q
C
C        Thus the solution to
C
C             DT = KFUN ( X )
C
C        Satisifies
C
C                     DT
C         0 < X  <  -------
C                    B*Q
C
C
C        We now have a guess at a bracketing interval. In the case
C        DT is positive it looks like
C
C                0        X
C         -------[--------]-----------------------------
C
C        This is ok mathematically, but due to rounding etc it is
C        conceivable that we might not have bracketed the root.
C        We check and if not we will double the
C        endpoint farthest from zero and call this X, and make
C        the other endpoint the old value of X.
C
C
C                0
C         -------+--------[--------]--------------------
C
C
C        We continue this process ...
C
C                0
C         -------+-----------------[-----------------]--
C
C        ...until the root is bracketed. (One shift is certain
C        to do the job).
C
C        If we perform this interval shift, we will have to take
C        care that X does not run out of the domain for which
C        we can safely compute KFUN.  Thus we will make sure that
C        the endpoints of these shifted intervals always stay safely
C        inside the domain for which KFUN can be computed.
C
      X    = DT / BQ
      X    = BRCKTD ( X, -BOUND, BOUND )
      FX2  = F*X*X

      CALL STMP03 ( FX2, C0, C1, C2, C3 )

      KFUN =   X*(BR0*C1 + X*( B2RV*C2 + X*(BQ*C3) ) )

      IF ( DT .LT. 0 ) THEN

         UPPER = 0
         LOWER = X

         DO WHILE ( KFUN .GT. DT )

            UPPER = LOWER
            LOWER = LOWER * 2.0D0
            OLDX  = X
            X     = BRCKTD ( LOWER, -BOUND, BOUND )

C
C           Make sure we are making progress. (In other words make sure
C           we don't run into the boundary of values that X can assume.
C           If we do run into the boundary, X will be unchanged and
C           there's nothing further we can do.  We'll have to call it
C           quits and tell the user what happened.)
C
            IF ( X .EQ. OLDX ) THEN

               FX2 = F*BOUND*BOUND

               CALL STMP03 ( FX2, C0, C1, C2, C3 )

               KFUNL = -BOUND*(BR0*C1 - BOUND*(B2RV*C2 - BOUND*BQ*C3))
               KFUNU =  BOUND*(BR0*C1 + BOUND*(B2RV*C2 + BOUND*BQ*C3))

               CALL SETMSG ( 'The input delta time (DT) has a '       //
     .                       'value of #.  This is beyond the '       //
     .                       'range of DT for which we can '          //
     .                       'reliably propagate states. The limits ' //
     .                       'for this GM and initial state are '     //
     .                       'from # to #. '                          )
               CALL ERRDP  ( '#', DT                                  )
               CALL ERRDP  ( '#', KFUNL                               )
               CALL ERRDP  ( '#', KFUNU                               )
               CALL SIGERR ( 'SPICE(DTOUTOFRANGE)'                    )
               CALL CHKOUT ( 'PROP2B'                                 )
               RETURN
            END IF

            FX2   = F*X*X

            CALL STMP03 ( FX2, C0, C1, C2, C3 )

            KFUN = X*(BR0*C1 + X*( B2RV*C2 + X*(BQ*C3) ) )

         END DO

      ELSE IF ( DT .GT. 0 ) THEN

         LOWER = 0
         UPPER = X

         DO WHILE ( KFUN .LT. DT )

            LOWER = UPPER
            UPPER = UPPER * 2.0D0
            OLDX  = X
            X     = BRCKTD ( UPPER, -BOUND, BOUND )
C
C           Make sure we are making progress.
C
            IF ( X .EQ. OLDX ) THEN

               FX2 = F*BOUND*BOUND

               CALL STMP03 ( FX2, C0, C1, C2, C3 )

               KFUNL = -BOUND*(BR0*C1 - BOUND*(B2RV*C2 - BOUND*BQ*C3))
               KFUNU =  BOUND*(BR0*C1 + BOUND*(B2RV*C2 + BOUND*BQ*C3))

               CALL SETMSG ( 'The input delta time (DT) has a '       //
     .                       'value of #.  This is beyond the '       //
     .                       'range of DT for which we can '          //
     .                       'reliably propagate states. The limits ' //
     .                       'for this GM and initial state are '     //
     .                       'from # to #. '                          )
               CALL ERRDP  ( '#', DT                                  )
               CALL ERRDP  ( '#', KFUNL                               )
               CALL ERRDP  ( '#', KFUNU                               )
               CALL SIGERR ( 'SPICE(DTOUTOFRANGE)'                    )
               CALL CHKOUT ( 'PROP2B'                                 )
               RETURN
            END IF

            FX2  = F*X*X

            CALL STMP03 ( FX2, C0, C1, C2, C3 )

            KFUN = X*(BR0*C1 + X*( B2RV*C2 + X*BQ*C3 ) )

         END DO

      ELSE

         CALL VEQUG  ( PVINIT, 6, PVPROP )
         CALL CHKOUT ( 'PROP2B'          )
         RETURN

      END IF

C
C     Ok. We've bracketed the root.  Now for lack of anything more
C     clever, we just bisect to find the solution.
C
C     We add a loop counter so that we can ensure termination of the
C     loop below.
C
C     On some systems the computed midpoint is stored in an extended
C     precision register.  Thus the midpoint is always different from
C     UPPER and LOWER.  Yet when the new value of LOWER and UPPER
C     are assigned UPPER and LOWER do not change and hence the
C     loop fails to terminate.  With the loop counter we force
C     termination of the loop.
C
      X    = MIN ( UPPER, MAX ( LOWER, (LOWER + UPPER)/2.0D0) )
      FX2  = F*X*X

      CALL STMP03 ( FX2, C0, C1, C2, C3 )
      LCOUNT = 0
      MOSTC  = 1000

      DO WHILE (         X      .GT. LOWER
     .           .AND.   X      .LT. UPPER
     .           .AND.   LCOUNT .LT. MOSTC )

         KFUN = X*(BR0*C1 + X*( B2RV*C2 + X*BQ*C3 ) )

         IF ( KFUN .GT. DT ) THEN
            UPPER = X
         ELSE IF ( KFUN .LT. DT ) THEN
            LOWER = X
         ELSE
            UPPER = X
            LOWER = X
         END IF
C
C        As soon as the bracketting values move away from
C        zero we can modify the count limit.
C
         IF ( MOSTC .GT. MAXBIT ) THEN

            IF ( UPPER .NE. 0.0D0 .AND. LOWER .NE. 0.0D0 ) THEN
               MOSTC   = MAXBIT
               LCOUNT = 0
            END IF

         END IF

         X    = MIN ( UPPER, MAX ( LOWER, (LOWER + UPPER)/2.0D0 ) )
         FX2  = F*X*X

         CALL STMP03 ( FX2, C0, C1, C2, C3 )

         LCOUNT = LCOUNT + 1

      END DO

C
C     With X in hand we simply compute BR, PC, VC, PCDOT and VCDOT
C     described in equations (1) --- (5) above. (Note, by our choice
C     of BOUND above, one can show that none of the computations
C     below can cause an overflow).
C
      X2    = X *X
      X3    = X2*X
      BR    = BR0*C0 + X*( B2RV*C1  + X*(BQ*C2) )

      PC    = 1.0D0     -      QOVR0        * X2 * C2
      VC    = DT        -      BQ           * X3 * C3
      PCDOT =           -     (QOVR0 / BR ) * X  * C1
      VCDOT = 1.0D0     -     (BQ    / BR ) * X2 * C2

C
C     ... and compute the linear combinations needed to get PVPROP
C
      CALL VLCOM ( PC,    POS, VC,    VEL,       PVPROP(1) )
      CALL VLCOM ( PCDOT, POS, VCDOT, VEL,       PVPROP(4) )

      CALL CHKOUT ( 'PROP2B' )
      RETURN

      END


      SUBROUTINE spline_akima_int(x,y,xx,yy,npts,iflag)
! returns the integral from xx(1) to x of the function
! given by the Akima spline.
! 2012-07-27: Typo correction, J Geiger
      USE stel_kinds
      implicit none
!-----------------------------------------------
!   D u m m y   A r g u m e n t s
!-----------------------------------------------
      integer, intent(in) :: npts        ! number of active points
      integer, intent(inout) :: iflag
      real(rprec), intent(in)  :: x
      real(rprec), intent(out) :: y
      real(rprec), dimension(npts), intent(in) :: xx, yy
!-----------------------------------------------
!   L o c a l   V a r i a b l e s
!-----------------------------------------------
      real(rprec), dimension(-1:size(xx)+2) :: a, b, c, d
      real(rprec), dimension(-1:size(xx)+2) :: xloc, yloc,dxloc
      real(rprec), dimension(-1:size(xx)+2) :: m, t, dm, p, q
      real(rprec), dimension(-1:size(xx)+2) :: int_f
      integer :: i, ix, iv, ivm1
      real(rprec) :: cl,bl,cr,br,dx
      real(rprec), parameter :: onethird = 1._DP/3._DP
!-----------------------------------------------

      iflag = 0  !initialization
      ix = size(xx)
      if(npts > ix)
     >   stop 'spline_akima_int: more active points
     >         requested than available'
      if(ix /= size(yy)) stop 'size mismatch of xx and yy!'
      iv=npts
      ivm1  = iv-1
! initialize local variables
      a = 0._DP ; b = 0._DP ; c = 0._DP ; d = 0._DP
      xloc = 0._DP ; yloc = 0._DP
      m = 0._DP ; t = 0._DP ; dm = 0._DP
      p = 0._DP ; q = 0._DP

      xloc(1:iv)=xx
      xloc(-1)= 2*xloc(1)-xloc(3)
      xloc( 0)= xloc(1)+xloc(2)-xloc(3)
      xloc(iv+2)= 2*xloc(iv)-xloc(iv-2)
      xloc(iv+1)= xloc(iv)+xloc(iv-1)-xloc(iv-2)
      dxloc(-1:iv+1)=xloc(0:iv+2)-xloc(-1:iv+1)
      yloc(1:iv)=yy
! calculate linear derivatives as far as existent
      m(1:iv-1) = (yloc(2:iv)-yloc(1:iv-1))/
     >               dxloc(1:iv-1)
! values for i=0, -1 and iv, iv+1 by quadratic extrapolation:
      cl = (m(2)-m(1))/(xloc(3)-xloc(1))
      bl = m(1) - cl*(xloc(2)-xloc(1))
      cr = (m(iv-2)-m(iv-1))/(xloc(iv)-xloc(iv-2))
      br = m(iv-2) - cr*(xloc(iv-1)-xloc(iv-2))
      yloc( 0)=yloc(1)+bl*(xloc( 0)-xloc(1))+
     >               cl*(xloc( 0)-xloc(1))**2
      yloc(-1)=yloc(1)+bl*(xloc(-1)-xloc(1))+
     >               cl*(xloc(-1)-xloc(1))**2
      yloc(iv+1)=yloc(iv)+br*(xloc(iv+1)-xloc(iv))+
     >               cr*(xloc(iv+1)-xloc(iv))**2
      yloc(iv+2)=yloc(iv)+br*(xloc(iv+2)-xloc(iv))+
     >               cr*(xloc(iv+2)-xloc(iv))**2
! rest of linear derivatives
      m(-1) = (yloc(0)-yloc(-1))/(xloc(0)-xloc(-1))
      m( 0) = (yloc(1)-yloc( 0))/(xloc(1)-xloc( 0))
      m(iv  ) = (yloc(iv+1)-yloc(iv  ))/(xloc(iv+1)-xloc(iv  ))
      m(iv+1) = (yloc(iv+2)-yloc(iv+1))/(xloc(iv+2)-xloc(iv+1))
! calculate weights for derivatives
      dm(-1:iv)= abs(m(0:iv+1)-m(-1:iv))
      DO i = 1, iv
        IF (dm(iv)==0) CYCLE
        p(i) = dm(i)/(dm(i)+dm(i-2))
        q(i) = dm(i-2)/(dm(i)+dm(i-2))
        t(i) = p(i)*m(i-1)+q(i)*m(i)
        IF (p(i)+q(i) >= TINY(1._DP)) CYCLE
        t(i) = 0.5_DP*(m(i-1)+m(i))
      END DO
!      where (dm /= 0._DP) !exclude division by zero
!        p(1:iv) = dm(1:iv)/(dm(1:iv)+dm(-1:iv-2))
!      end where
!      where (dm /= 0._DP) !exclude division by zero
!        q(1:iv) = dm(-1:iv-2)/(dm(1:iv)+dm(-1:iv-2))
!      end where
!      t(1:iv) = p(1:iv)*m(0:iv-1)+q(1:iv)*m(1:iv)
!      where ( p(1:iv)+q(1:iv) < TINY(1._DP)) ! in case of two zeros give equal weight
!        t(1:iv) = 0.5_DP*m(0:iv-1)+0.5_DP*m(1:iv)
!      end where
! fix coefficients
      a = yloc
      b = t
      c(1:iv-1) = (3*m(1:iv-1)-t(2:iv)-2*t(1:iv-1))/
     >               (dxloc(1:iv-1))
      d(1:iv-1) = (t(2:iv)+t(1:iv-1)-2*m(1:iv-1))/
     >               (dxloc(1:iv-1))**2

! calculation
      int_f(1:iv-1) =  a(1:iv-1)*dxloc(1:iv-1)+
     >          0.5_DP*b(1:iv-1)*dxloc(1:iv-1)**2 +
     >        onethird*c(1:iv-1)*dxloc(1:iv-1)**3 +
     >         0.25_DP*d(1:iv-1)*dxloc(1:iv-1)**4
      if(x >= xloc(iv)) then
        y = sum(int_f(1:iv-1))
        iflag = 0
        if(x>xloc(iv)) iflag=-1
        return
      endif
      if(x <= xloc(1)) then
        y = 0._DP
        iflag = 0
        if(x<xloc(1)) iflag=-1
        return
      endif
      y=0._DP
      do i=1,iv-1
        if(x >= xloc(i+1))then
          y=y + int_f(i)
        else
          dx = x-xloc(i)
          y=y+dx*(a(i)+dx*(.5_DP*b(i)+dx*(
     >                     onethird*c(i)+.25_DP*d(i)*dx)))
          return
        endif
      enddo

      END SUBROUTINE spline_akima_int


C     Subroutine to perform a polar decomposition on the deformation gradient
C
C     Adapted from Sommer for an Abaqus subroutine. Downloaded from the
C     International Society for Biomechanics' website. This subroutine is
C     free for non-commercial use.
C     https://isbweb.org/software/movanal/sommer.txt
C
C     H.J. Sommer III, Professor of Mechanical Engineering, 207 Reber Building
C     The Pennsylvania State University, University Park, PA  16802
C     (814)865-2519, FAX (814)863-4848, Bitnet HJS@PSUECL, Internet HJS@ECL.PSU.EDU
C
c********1*********2*********3*********4*********5*********6*********7**
c
c	kpolarDecomp(G,R,S)
c
c	positive polar decomposition of 3x3 matrix  G = R * S
c	forcing positive orthonormal rotation matrix
c
c	INPUTS
c	G = 3x3 general matrix
c
c	OUTPUTS
c	R = 3x3 positive orthonormal matrix
c	S = 3x3 symmetric matrix
c
c	PRECISION:	single
c	COMMONS:	none
c	CALLS:		none
c	FUNCTIONS:	ABS, SQRT
c	REFERENCE:	Veldpaus, F.E., H.J. Woltring, and L.J.M.G. Dortmans,
c				A Least-Squares Algorithm for the Equiform
c				Transformation from Spatial Marker Coordinates,
c				J. Biomechanics, 21(1):45-54 (1988).
c	DATE:		10/8/92 - HJSIII
c
c
      SUBROUTINE kpolarDecomp(G,S,R)
c
c	declarations
      INCLUDE 'ABA_PARAM.INC'
      dimension G(3,3),R(3,3),S(3,3),COG(3,3),XP(3,3),ADP(3,3),
     1 XPBI(3,3)
c
c	constants
      EPS=1.0E-5
c
c	cofactors and determinant of g
      COG(1,1)=G(2,2)*G(3,3)-G(2,3)*G(3,2)
      COG(2,1)=G(1,3)*G(3,2)-G(1,2)*G(3,3)
      COG(3,1)=G(1,2)*G(2,3)-G(1,3)*G(2,2)
      COG(1,2)=G(2,3)*G(3,1)-G(2,1)*G(3,3)
      COG(2,2)=G(1,1)*G(3,3)-G(1,3)*G(3,1)
      COG(3,2)=G(1,3)*G(2,1)-G(1,1)*G(2,3)
      COG(1,3)=G(2,1)*G(3,2)-G(2,2)*G(3,1)
      COG(2,3)=G(1,2)*G(3,1)-G(1,1)*G(3,2)
      COG(3,3)=G(1,1)*G(2,2)-G(1,2)*G(2,1)
      G3=G(1,1)*COG(1,1)+G(2,1)*COG(2,1)+G(3,1)*COG(3,1)
c
c	P = trans(G) * G = S * S
      DO 10000 I=1,3
      XP(I,1)=G(1,I)*G(1,1)+G(2,I)*G(2,1)+G(3,I)*G(3,1)
      XP(I,2)=G(1,I)*G(1,2)+G(2,I)*G(2,2)+G(3,I)*G(3,2)
      XP(I,3)=G(1,I)*G(1,3)+G(2,I)*G(2,3)+G(3,I)*G(3,3)
10000 CONTINUE
c
c	adjoint of P
      ADP(1,1)=XP(2,2)*XP(3,3)-XP(2,3)*XP(3,2)
      ADP(2,2)=XP(1,1)*XP(3,3)-XP(1,3)*XP(3,1)
      ADP(3,3)=XP(1,1)*XP(2,2)-XP(1,2)*XP(2,1)
c
c	G invariants
      G1SQ=XP(1,1)+XP(2,2)+XP(3,3)
      G1=SQRT(G1SQ)
      G2SQ=ADP(1,1)+ADP(2,2)+ADP(3,3)
      G2=SQRT(G2SQ)
c
c	initialize iteration
      H1=G2/G1SQ
      H2=G3*G1/G2SQ
      X=1.0
      Y=1.0
c
c	iteration loop
10001 CONTINUE
      DEN=2.0*(X*Y-H1*H2)
      RES1=1.0-X*X+2.0*H1*Y
      RES2=1.0-Y*Y+2.0*H2*X
      DX=(Y*RES1+H1*RES2)/DEN
      DY=(H2*RES1+X*RES2)/DEN
      X=X+DX
      Y=Y+DY
      IF(ABS(DX/X).GT.EPS.OR.ABS(DY/Y).GT.EPS)GO TO 10001
c
c	BETA invariants
      BETA1=X*G1
      BETA2=Y*G2
c
c	invert ( trans(G) * G + BETA2 * identity )
      XP(1,1)=XP(1,1)+BETA2
      XP(2,2)=XP(2,2)+BETA2
      XP(3,3)=XP(3,3)+BETA2
      XPBI(1,1)=XP(2,2)*XP(3,3)-XP(2,3)*XP(3,2)
      XPBI(1,2)=XP(1,3)*XP(3,2)-XP(1,2)*XP(3,3)
      XPBI(1,3)=XP(1,2)*XP(2,3)-XP(1,3)*XP(2,2)
      XPBI(2,1)=XP(2,3)*XP(3,1)-XP(2,1)*XP(3,3)
      XPBI(2,2)=XP(1,1)*XP(3,3)-XP(1,3)*XP(3,1)
      XPBI(2,3)=XP(1,3)*XP(2,1)-XP(1,1)*XP(2,3)
      XPBI(3,1)=XP(2,1)*XP(3,2)-XP(2,2)*XP(3,1)
      XPBI(3,2)=XP(1,2)*XP(3,1)-XP(1,1)*XP(3,2)
      XPBI(3,3)=XP(1,1)*XP(2,2)-XP(1,2)*XP(2,1)
      DETPBI=XP(1,1)*XPBI(1,1)+XP(2,1)*XPBI(1,2)+XP(3,1)*XPBI(1,3)
c
c	R = (cofac(G)+BETA1*G) * inv(trans(G)*G+BETA2*identity)
      DO 10002 I=1,3
      R(I,1)=((COG(I,1)+BETA1*G(I,1))*XPBI(1,1)
     1       +(COG(I,2)+BETA1*G(I,2))*XPBI(2,1)
     2       +(COG(I,3)+BETA1*G(I,3))*XPBI(3,1))/DETPBI
      R(I,2)=((COG(I,1)+BETA1*G(I,1))*XPBI(1,2)
     1       +(COG(I,2)+BETA1*G(I,2))*XPBI(2,2)
     2       +(COG(I,3)+BETA1*G(I,3))*XPBI(3,2))/DETPBI
      R(I,3)=((COG(I,1)+BETA1*G(I,1))*XPBI(1,3)
     1       +(COG(I,2)+BETA1*G(I,2))*XPBI(2,3)
     2       +(COG(I,3)+BETA1*G(I,3))*XPBI(3,3))/DETPBI
10002 CONTINUE
c
c	S = trans(R) * G
      DO 10003 I=1,3
      S(I,1)=R(1,I)*G(1,1)+R(2,I)*G(2,1)+R(3,I)*G(3,1)
      S(I,2)=R(1,I)*G(1,2)+R(2,I)*G(2,2)+R(3,I)*G(3,2)
      S(I,3)=R(1,I)*G(1,3)+R(2,I)*G(2,3)+R(3,I)*G(3,3)
10003 CONTINUE
c
c	done
      RETURN
      END

Lesson C - fibre reinforced anistropic models/Abaqus/kpolarDecomp.f


      double precision function Li4(x)
C     returns Li_4(x) for real x, minf < x < 1
      implicit none
      double precision x
      double complex wgplg

      if (x .gt. 1d0) then
        write(6,*) 'x>1 in Li4 function, src/Lib/Li4.f'
        stop
      endif

      Li4 = dble(wgplg(3,1,x))
      end


program problem19
  implicit none
  integer :: year,month,day,count=0

  ! January 1, 1901 was a Tuesday
  day=2

  do year=1901,2000
     do month=1,12
        if (mod(day,7)==0) then
           count=count+1
        end if
        select case (month)
        case (9,4,6,11)
           day=day+30
        case (2)
           if (mod(year,4)==0 .and. (mod(year,100)/=0 .or. mod(year,400)==0)) then
              day=day+29
           else
              day=day+28
           end if
        case default
           day=day+31
        end select
     end do
  end do

  print *, count

end program problem19


! PR middle-end/56015
! { dg-do run }
! { dg-options "-Ofast -fno-inline" }

program pr56015
  implicit none
  complex*16 p(10)
  p(:) = (0.1d0, 0.2d0)
  p(:) = (0.0d0, 1.0d0) * p(:)
  call foo (p)
contains
  subroutine foo (p)
    complex*16 p(10)
    if (any (p .ne. (-0.2d0, 0.1d0))) STOP 1
  end subroutine
end program pr56015


C
C *** LAST REVISED ON 16-MAR-1988 08:21:37.04
C *** SOURCE FILE: [DL.GRAPHICS.LONGLIB]MAPLIB.FOR
C
C ***********************************************************************
C
C	SOURCE CODE FOR THE WORLD MAP ROUTINES OF THE LONGLIB GRAPHICS
C	LIBRARY.  THESE ROUTINES ACCESS A DATA FILE WITHIN THE LONGLIB
C	GRAPHICS DIRECTORY.  TO ACCESS THIS FILE THE LOGICAL NAME
C	LONGLOC: MUST BE DEFINED TO BE THIS DIRECTORY.
C
C	ROUTINES IN THIS FILE ARE 100 PERCENT FORTRAN-77 COMPATIBLE
C
C ***********************************************************************
C
	SUBROUTINE EARTH3D(R,FLAT)
C
C	WRITTEN BY D. LONG   JULY 1985
C	DRAWS DATA FROM LAND-EDGE MAP OF THE EARTH IN 3d WITH THE OPTION
C	OF A SPHERICAL OR ELLIPSOIDAL EARTH
C
C	R	(R): EARTH RADIUS (IN PLOT UNITS)
C	FLAT	(R): EARTH FLATNESS
C			= 0 FOR SPHERICAL EARTH
C			= 3.3528132E-3 FOR ELLIPSOIDAL EARTH
C
C	READS THE DATA FILE EARTH.DAT IN THE LONGLIB GRAPHICS
C	DIRECTORY.  REQUIRES LONGLOC: TO BE DEFINED AS THIS DIRECTORY.
C	USES FORTRAN FILE UNIT NUMBER 2 FOR READING A FILE.
C
C	USES THE INIT3D 3D PACKAGE BY CALLING PLOT3D
C
	REAL DATIN(10),V(4)
C
C	DEFINE DEGREE TO RADIAN CONVERSION
C
	DEGRAD(X)=X*3.141592654/180.0
C
C	OPEN FILE CONTAINING EARTH LOCATION DATA
C
	OPEN(UNIT=2,FILE='LONGLOC:EARTH.DAT',FORM='FORMATTED',
     $		STATUS='OLD',READONLY)
C
C	BEGIN READING DATA
C
	ID=3
   10	READ(2,1005,END=999) DATIN
 1005	FORMAT(10F8.0)
C
	DO 20 I=1,10,2
		IF(IFIX(DATIN(I)).EQ.0.AND.
     $		   IFIX(DATIN(I+1)).EQ.0)GOTO 20
		IF(DATIN(I+1).EQ.888.8) THEN
C
C	END OF LINE SEGMENT
C
			ID=3
			GO TO 20
		ENDIF
		IF (ABS(DATIN(I)).GT.180.5.OR.
     $		    ABS(DATIN(I+1)).GT.91.) THEN
C	BAD DATA POINT
			IPT=3
			GO TO 20
		ENDIF
C
C	CONVERT INPUT LAT/LONG TO 3D VECTOR
C
		ALONG=DEGRAD(DATIN(I))
		ALAT=DEGRAD(DATIN(I+1))
C
C	COMPUTE EARTH RADIUS ASSUMING ELLIPSOIDAL EARTH
C
		RAD=R*(1.-FLAT*SIN(ALAT))
		ALAT=DEGRAD(DATIN(I+1)-90.)
		CALL SPRECT1(V,ALONG,ALAT,RAD)
		CALL PLOT3D(V(1),V(2),V(3),ID)
		ID=2
   20	CONTINUE
	GO TO 10
  999	CONTINUE
C
	CLOSE(2)
	RETURN
	END
C
C
	SUBROUTINE SPRECT1(V,T,P,R)
C
C	CONVERT FROM SPHERICAL TO RECTANGULAR COORDINATES
C	(LATTITUDE-LONGITUDE TYPE SPHERICAL COORDINATES)
C
C	V	(R):	OUTPUT VECTOR (RECTANGULAR COOR.) V(3)
C	T	(R):	THETA INPUT ANGLE (RADIANS)
C	P	(R):	PHI INPUT ANGLE (CONE ANGLE) (RADIANS)
C	R	(R):	RADIUS INPUT
C
	DIMENSION V(3)
	V(1)=R*COS(T)*SIN(P)
	V(2)=R*SIN(T)*SIN(P)
	V(3)=R*COS(P)
	RETURN
	END
C
C ****************************************************************************
C
	SUBROUTINE LANDMAP(YLAT,XLONG,SLONG)
C
C	WRITTEN BY D. LONG JULY 1985  JPL
C
C	DRAWS A LAND-EDGE MAP USING DATA FROM VECTOR LAND MAP FILE
C	THIS ROUTINE USES A LINEAR PROJECTION, I.E. LAT/LONGS ARE ASSUMED
C	TO BE X,Y POINTS ON A PLANE.
C
C	YLAT	REAL	SCALE FACTOR OF LONGITUDE DEGREES/INCH
C	XLONG 	REAL	SCALE FACTOR OF LATITUDE DEGREES/INCH
C	SLONG	REAL	LEFT-MOST LONGITUDE (-180 TO +180)
C
C	ASSUMES THAT LONGLOC HAS BEEN ASSIGNED THE DIRECTORY OF THE
C	EARTH.DAT FILE STORING THE EARTH LAND-EDGE MAP.  USES FORTRAN
C	FILE UNIT NUMBER 2.
C
C	NOTE: SINCE MAP DATA HAS ITS ORIGIN FROM -180 TO 180, IF WE WANT
C	THE MAP TO START FROM SOME OTHER POINT WE NEED TO DO SOME FANCY
C	TRICKS
C
	REAL DATIN(10)
	LOGICAL LAST,PENUP
C
C	OPEN DATA FILE
C
	OPEN(UNIT=2,FILE='LONGLOC:EARTH.DAT',FORM='FORMATTED',
     $		STATUS='OLD',READONLY)
	IPT=3
	LAST=.FALSE.
	X=2.*XLONG
C
C	READ THE DATA
C
   10	READ(2,1005,END=999) DATIN
 1005	FORMAT(10F8.0)
C
C	CHECK THE STORED DATA COMMAND
C
	DO 20 I=1,10,2
		IF (IFIX(DATIN(I)).EQ.0.AND.
     $ 		    IFIX(DATIN(I+1)).EQ.0) GOTO 20
		IF (DATIN(I+1).EQ.888.8) THEN
C
C	 END OF LINE SEGMENT
C
			IPT=3
			CALL PLOT(X,Y,IPT)
			PENUP=.TRUE.
			LAST=.FALSE.
			X=XLONG*2.
			GO TO 20
		ENDIF
		IF (ABS(DATIN(I)).GT.180.5.OR.
     $		    ABS(DATIN(I+1)).GT.91.) THEN
C	BAD POINT
			IPT=3
			CALL PLOT(X,Y,IPT)
			PENUP=.TRUE.
			LAST=.FALSE.
			X=XLONG*2.
			GO TO 20
		ENDIF
		PENUP=.FALSE.
		IF (DATIN(I).LT.SLONG) THEN
C
C	LEFT OF DESIRED LONGITUDE
C
			IF (LAST) THEN
C
C	CLIP INTERSECTION LINE
C
			    X1=(DATIN(I)+360.-SLONG)/360.*XLONG
				Y1=(DATIN(I+1)+90.)/180.*YLAT
				Y2=Y1-(Y-Y1)*X1/(X-X1)
				CALL PLOT(X,Y2,IPT)
				CALL PLOT(X1,Y2,3)
				CALL PLOT(X1,Y1,2)
				X=X1
				Y=Y1
			ELSE
			    X=(DATIN(I)+360.-SLONG)/360.*XLONG
				Y=(DATIN(I+1)+90.)/180.*YLAT
				CALL PLOT(X,Y,IPT)
			ENDIF
			IPT=2
			LAST=.FALSE.
		ELSE
C
C	RIGHT OF DESIRED LONGITUDE
C
			IF (LAST) THEN
				X=(DATIN(I)-SLONG)/360.*XLONG
				Y=(DATIN(I+1)+90.)/180.*YLAT
				CALL PLOT(X,Y,IPT)
			ELSE
C
C	CLIP INTERSECTION LINE
C
				X1=(DATIN(I)-SLONG)/360.*XLONG
				Y1=(DATIN(I+1)+90.)/180.*YLAT
				Y2=Y-(Y1-Y)*X/(X1-X)
				CALL PLOT(X,Y2,IPT)
				CALL PLOT(X1,Y2,3)
				IPT=3
				IF (PENUP) IPT=3
				CALL PLOT(X1,Y1,IPT)
				X=X1
				Y=Y1
			ENDIF
			IPT=2
			LAST=.TRUE.
		ENDIF
   20	CONTINUE
	GO TO 10
  999	CONTINUE
	CLOSE(2)
C
	RETURN
	END
C
C
C ****************************************************************************
C
	SUBROUTINE POLARMAP(X0,Y0,RAD,ANG)
C
C	WRITTEN BY D. LONG JULY 1985  JPL
C
C	DRAWS A LAND-EDGE MAP USING DATA FROM VECTOR LAND MAP FILE
C	THIS ROUTINE USES A POLAR PROJECTION, I.E. LATTITUDE IS ASSUMED
C	TO BE A LINEAR RADIUS FROM THE POLE.
C
C	X0,Y0	REAL	ORIGIN OF THE POLE
C	RAD	REAL	RADIUS OF EQUATOR
C			> 0 NORTHERN HEMISPHERE
C			< 0 SOUTHERN HEMISPHERE
C	ANG	REAL	ANGLE OF PRIME MERIDAN FROM HORIZONTAL (DEG CCW)
C
C	ASSUMES THAT LONGLOC HAS BEEN ASSIGNED THE DIRECTORY OF THE
C	EARTH.DAT FILE STORING THE EARTH LAND-EDGE MAP.  USES FORTRAN
C	FILE UNIT NUMBER 2.
C
	REAL DATIN(10)
	LOGICAL LAST
C
C	OPEN DATA FILE
C
	OPEN(UNIT=2,FILE='LONGLOC:EARTH.DAT',FORM='FORMATTED',
     $		STATUS='OLD',READONLY)
	DTR=3.1415926/180.0
	AR=ABS(RAD)/90.0
	IPT=3
	LAST=.FALSE.
C
C	READ THE DATA
C
   10	READ(2,1005,END=999) DATIN
 1005	FORMAT(10F8.0)
C
C	CHECK THE STORED DATA COMMAND
C
	DO 20 I=1,10,2
		IF (IFIX(DATIN(I)).EQ.0.AND.
     $		    IFIX(DATIN(I+1)).EQ.0) GOTO 20
		IF (DATIN(I+1).EQ.888.8) THEN
			IPT=3
			GO TO 20
		ENDIF
		IF (ABS(DATIN(I)).GT.180.5.OR.
     $		    ABS(DATIN(I+1)).GT.91.) THEN
			IPT=3
			GO TO 20
		ENDIF
		IF (SIGN(1.,DATIN(I+1)).NE.SIGN(1.,RAD)) THEN
			IPT=3
		ELSE
			ALONG=DATIN(I)
			ALAT=DATIN(I+1)
			R=(90.0-ABS(ALAT))*AR
			A=DTR*(ALONG*SIGN(1.,RAD)+ANG)
			X=COS(A)*R+X0
			Y=SIN(A)*R+Y0
			CALL PLOT(X,Y,IPT)
			IPT=2
		ENDIF
   20	CONTINUE
	GOTO 10
  999	CONTINUE
	CLOSE(2)
C
	RETURN
	END
C
C *********************************************************************
C
C THE FOLLOWING ARE ROUTINES USING THE CIA LAND/SEA BIT MAP FILE
C SEE THE COMMENTS IN THE BITMAP AND LNDSEA ROUTINES
C
C *********************************************************************
C
	SUBROUTINE BITMAP(CLONG,XLONG,YLAT,NXPIX,NYPIX,ILAND)
C
C	BITMAP USES THE FILE "LNDSEA1.DAT" TO DRAW AN EARTH LAND MASK WHERE
C	THE LAND SURFACE OF THE EARTH IS DRAWN USING HORIZONTAL LINES
C	ON A RECTANGULAR PROJECTION OF THE EARTH'S SURFACE (SEE LANDMAP).
C	A GRID OF PIXELS IS USED TO LOCATE POINTS WHICH ARE "LOOKED UP"
C	IN LNDSEA1.DAT (WHICH IS INTERPOLATED) TO DETERMINE IF EACH PIXEL
C	IS LAND/SEA.  USING HORIZONTAL SCANS, A PLOT LINE IS GENERATED OF
C	THE LAND/SEA AREA WHICH IS OUTPUT WHEN THE TYPE CHANGES.  NOTE
C	FOR HIGH DENSITY PLOTS THIS OPERATION IS VERY SLOW DUE TO THE HIGH
C	NUMBER OF FILE READS OF LNDSEA1.DAT REQUIRED.
C
C	INPUTS:
C		CLONG	(R)	LEFT SIDE LONGITUDE OF MAP (DEGREES -180,180)
C		XLONG	(R)	LENGTH OF LONGITUDE AXIS (INCHES)
C		YLAT	(R)	LENGTH OF LATTITUDE AXIS (INCHES)
C		NXPIX	(I)	NUMBER OF PIXELS IN X DIRECTION
C		NYPIX	(I)	NUMBER OF PIXELS IN Y DIRECTION
C		ILAND	(I)	LAND/SEA FLAG
C				 = 0 PLOT LAND
C				 = 1 PLOT SEA
C	OUPUTS: (NONE)
C	CALLS:
C		LNDSEA
C		PLOT
C
	LOGICAL PENUP,LAND,OPTION
C
	XPIX=NXPIX
	YPIX=NYPIX
	CCC=XLONG*(CLONG+180.)/360.
	OPTION=.TRUE.
	IF (ILAND.NE.0) OPTION=.FALSE.
C
	DO 100 ILINE=1,NYPIX
		LINE=NYPIX-ILINE+1
		RLAT=(90.-FLOAT(LINE-1)/YPIX*180.)
		PENUP=.TRUE.
		Y=(YPIX-FLOAT(LINE-1))*YLAT/YPIX
		X0=0.
		Y0=Y
		DO 50 I=1,NXPIX
			RLON=(FLOAT(I-1)/XPIX)*360.-180.
			X=(FLOAT(I-1)*XLONG/XPIX)
			IF(RLON.LT.0.) RLON=RLON+360.
			IMAP=LNDSEA(RLAT,RLON)
			LAND=.TRUE.
			IF (IMAP.EQ.0) LAND=.FALSE.
			IF (((.NOT.LAND).AND.OPTION).OR.
     $			    ((.NOT.OPTION).AND.LAND)) THEN
				IF (PENUP) THEN
					X0=X
					Y0=Y
					PENUP=.FALSE.
				ENDIF
			ELSE
				IF (.NOT.PENUP) THEN
					X1=X0-CCC
					Y1=Y0
					X2=X-CCC
					Y2=Y
					IF (X1.LE.0.) THEN
					    IF (X2.LT.0.) THEN
						X1=X1+XLONG
						X2=X2+XLONG
						CALL PLOT(X1,Y1,3)
						CALL PLOT(X2,Y2,2)
					    ELSE
						X1=X1+XLONG
						CALL PLOT(0.,Y1,3)
						CALL PLOT(X2,Y2,2)
						CALL PLOT(X1,Y1,3)
						CALL PLOT(XLONG,Y2,2)
					    ENDIF
					ELSE
					    IF (X2.GT.0.) THEN
						CALL PLOT(X1,Y1,3)
						CALL PLOT(X2,Y2,2)
					    ELSE
						X2=X2+XLONG
						CALL PLOT(0.,Y2,3)
						CALL PLOT(X1,Y1,2)
						CALL PLOT(X2,Y2,3)
						CALL PLOT(XLONG,Y1,2)
					    ENDIF
					ENDIF
					PENUP=.TRUE.
				ENDIF
			ENDIF
   50		CONTINUE
		IF (.NOT.PENUP) THEN
			CALL PLOT(X0,Y0,3)
			CALL PLOT(X,Y,2)
			PENUP=.TRUE.
		ENDIF
  100	CONTINUE
	RETURN
	END
C
C ************************************************************************
C
	INTEGER FUNCTION LNDSEA(ALAT,ALON)
C
C	THIS ROUTINE RETURNS A FLAG INDICATING IF THE POINT
C	(ALAT,ALON) IS EITHER ALL LAND OR ALL SEA.
C
C	INPUTS:
C		ALAT	(R)	LATTITUDE (DEGREES)
C		ALON	(R)	LONGITUDE (DEGREES)
C	OUTPUTS:
C		LNDSEA	(I)	LAND/SEA FLAG
C				  = 0 FOR OCEAN
C				 <> 0 FOR LAND
C	CALLS: (NONE)
C
C	NOTE: USES FORTRAN FILE UNIT #1
C
 	INTEGER*4 ALLSAM(481),CIADAT(4,120),BIT(32)
	EQUIVALENCE (ALLSAM(2),CIADAT(1,1))
	DATA IOPEN/0/,LU/1/,IRECLA/0/
C
C	OPEN BITMAP IMAGE FILE OF LAND/SEA
C
	IF (IOPEN.EQ.0) THEN
		IOPEN=1
	        OPEN(UNIT=LU,FILE='LONGLOC:LNDSEA1.DAT',
     &		 ACCESS='DIRECT',STATUS='OLD',READONLY,
     &		 MAXREC=648,ERR=99)
C
C	LOAD DECODING BITMAP
C
		DO 10 I=1,32
			IZ=0
			BIT(I)=IBSET(IZ,32-I)
10		CONTINUE
	ENDIF
C
C   ENSURE VALID INPUT ANGLES
C
	BLON=AMOD(ALON,360.)
	IF (BLON.LT.0.0) BLON=BLON+360.
	BLAT=AMOD(ALAT+90.0,360.)
	IF (BLAT.GT.180.0) BLAT=BLAT-180.0
	IF (BLAT.LT.0.0) BLAT=BLAT+180.0
C
C   SET UP FILE POINTERS
C
	JLAT0=IFIX(BLAT)/10
	IF (JLAT0.EQ.18) JLAT0=17
	JLON0=IFIX(BLON)/10
	IF (JLON0.EQ.360) JLON0=1
	IREC=((JLAT0)*36)+JLON0+1
C
C   SEE IF A NEW RECORD MUST BE FETCHED
C
	IF (IREC.NE.IRECLA) THEN
		READ(LU,REC=IREC,ERR=99) ALLSAM
		IRECLA=IREC
	ENDIF
C
C   CHECK FOR 'ALL LAND' OR 'ALL SEA'
C
	IF (ALLSAM(1).NE.-1) THEN
 		LNDSEA=ALLSAM(1)
		RETURN
	ENDIF
C
C	COMPUTE ADDRESS IN BUFFER
C
	JLAT=INT((BLAT-FLOAT(10*JLAT0))*12)+1
	IF (JLAT.EQ.121) JLAT=120
	JLON=INT((BLON-FLOAT(10*JLON0))*12)+1
	IWORD=(JLON+29)/30
	IBIT=JLON-(30*(IWORD-1))
C
C	EXTRACT BIT
C
	LNDSEA=IAND(CIADAT(IWORD,JLAT),BIT(IBIT))
	IF (LNDSEA.NE.0) LNDSEA=1
	RETURN
C
C	FILE ERRORS
C
99	WRITE(*,45)
45	FORMAT(' *** ERROR OPENING/READING LNDSEA1.DAT FILE ***')
	RETURN
	END


C ======================================================================
C  Diffusion tensor
C ======================================================================
      Integer Function Ddiff(x, y, z, label, DDATA, IDATA, iSYS, Coef)
      include 'fem3Dtet.fd'
C ======================================================================
C  Diffusion tensor is passed through DDATA array. DDATA is filled
C  in main.f using subroutine Drotate at the end of this file.
C ======================================================================
      Real*8  x, y, z, DDATA(3,*), Coef(9, 9)
      Integer label, IDATA(*), iSYS(*)
C ======================================================================
      iSYS(1) = 3
      iSYS(2) = 3

c HMFEM needs inverted diffusion tensor; we use Lapack for this
      Do i = 1, 3
         Do j = i, 3
            Coef(i, j) = DDATA(i, j)
         End do
      End do

      Call invertSPDmatrix(3, Coef, 9)

      Ddiff = TENSOR_SYMMETRIC

      Return
      End

C ======================================================================
C Boundary condition
C ======================================================================
      Integer Function Dbc(x, y, z, label, DDATA, IDATA, iSYS, eBC)
      Include 'assemble.fd'
C ======================================================================
      Real*8  x, y, z, DDATA(*), eBC(*)
      Integer label, IDATA(*), iSYS(*)
C ======================================================================
      iSYS(1) = 1
      iSYS(2) = 1

      If (label.ge.1.and.label.le.6) then
          eBC(1) = 0D0
          Dbc = BC_DIRICHLET
      Else if (label.ge.7.and.label.le.12) then
          eBC(1) = 2D0
          Dbc = BC_DIRICHLET
      Else
          stop 'wrong labelF'
      End if

      Return
      End

C ======================================================================
C Right hand side
C ======================================================================
      Integer Function Drhs(x, y, z, label, DDATA, IDATA, iSYS, F)
      Include 'fem3Dtet.fd'
C ======================================================================
      Real*8  x, y, z, DDATA(*), F(*)
      Integer label, IDATA(*), iSYS(*)
C ======================================================================
      iSYS(1) = 1
      iSYS(2) = 1

      F(1) = 0D0
      Drhs = TENSOR_SCALAR

      Return
      End

C ======================================================================
c Templated routine for elemental matrix
C ======================================================================
      Subroutine FEM3Dext(
     &           XY1, XY2, XY3, XY4,
     &           lbE, lbF, lbR, lbP, DDATA, IDATA, iSYS,
     &           LDA, A, F, nRow, nCol,
     &           templateR, templateC)
      Implicit none
      Include 'fem3Dtet.fd'
C ======================================================================
      Real*8  XY1(*), XY2(*), XY3(*), XY4(*)

      Integer lbE, lbF(4), lbR(6), lbP(4)
      Real*8  DDATA(*)
      Integer IDATA(*), iSYS(*)

      Integer LDA, nRow, nCol
      Real*8  A(LDA, *), F(*)
      Integer templateR(*), templateC(*)

C Local variables
      Integer  Ddiff, Drhs, Dbc
      External Ddiff, Drhs, Dbc
      Real*8   tri_area
      External tri_area

      Real*8   Q(4, 4), B(4), L(4), G(1), det, alpha, s
      Real*8   x, y, z, eBC(1)
      Integer  i, j, k, ir,ic, INFO
C ======================================================================
      nRow = 4
      nCol = 4

c ... set up templates (unknowns are located on faces)
      Do i = 1, 4
         templateR(i) = Fdof
         templateC(i) = Fdof
      End do

c ... compute the mass matrix (Q)
c     pass a copy of iSYS which is changed inside fem3Dtet
      Call fem3Dtet(XY1, XY2, XY3, XY4,
     &              IDEN, FEM_RT0, IDEN, FEM_RT0,
     &              lbE, Ddiff, DDATA, IDATA, iSYS, 2,
     &              4, Q, ir, ic)

c ... compute right hand side
c     pass a copy of iSYS which is changed inside fem3Dtet
      Call fem3Dtet(XY1, XY2, XY3, XY4,
     &              IDEN, FEM_P0, IDEN, FEM_P0,
     &              lbE, Drhs, DDATA, IDATA, iSYS, 2,
     &              1, G, ir, ic)

c ... compute the constraint matrix
      B(1) = tri_area(XY1, XY2, XY3)
      B(2) = tri_area(XY2, XY3, XY4)
      B(3) = tri_area(XY3, XY4, XY1)
      B(4) = tri_area(XY4, XY1, XY2)

c ... elliminate fluxes: invert the mass matrix first
      call invertSPDmatrix(4,Q,4)

      Do i = 1, 4
         Do j = 1, 4
            Q(i, j) = Q(i, j) * B(j)
         End do
      End do

      Do i = 1, 4
         L(i) = Q(1, i) * B(1) + Q(2, i) * B(2)
     &        + Q(3, i) * B(3) + Q(4, i) * B(4)
      End do

c ... elliminate central pressure
      alpha = L(1) + L(2) + L(3) + L(4)

      Do i = 1, 4
         Do j = i, 4
            A(i, j) = B(i) * Q(i, j) - L(i) * L(j) / alpha
            A(j, i) = A(i, j)
         End do
         F(i) = L(i) * G(1) / alpha
      End do

c ... impose boundary conditions (assume nRow = nCol)
      Do k = 1, 4
         If(lbF(k).GT.0) Then
            If(k.EQ.1) Then
               x = (XY1(1) + XY2(1) + XY3(1)) / 3
               y = (XY1(2) + XY2(2) + XY3(2)) / 3
               z = (XY1(3) + XY2(3) + XY3(3)) / 3
            ElseIf(k.EQ.2) Then
               x = (XY2(1) + XY3(1) + XY4(1)) / 3
               y = (XY2(2) + XY3(2) + XY4(2)) / 3
               z = (XY2(3) + XY3(3) + XY4(3)) / 3
            ElseIf(k.EQ.3) Then
               x = (XY3(1) + XY4(1) + XY1(1)) / 3
               y = (XY3(2) + XY4(2) + XY1(2)) / 3
               z = (XY3(3) + XY4(3) + XY1(3)) / 3
            ElseIf(k.EQ.4) Then
               x = (XY4(1) + XY1(1) + XY2(1)) / 3
               y = (XY4(2) + XY1(2) + XY2(2)) / 3
               z = (XY4(3) + XY1(3) + XY2(3)) / 3
            End if

c           pass of copy of iSYS which is changed inside Dbc
            i = Dbc(x, y, z, lbF(k), DDATA, IDATA, iSYS, eBC)

            Do i = 1, nRow
               F(i) = F(i) - A(i, k) * eBC(1)
               A(k, i) = 0
               A(i, k) = 0
            End do

            A(k, k) = 1
            F(k) = eBC(1)
         End if
      End do

      Return
      End

C ======================================================================
C  The routine takes an input matrix D and rotates it about i-th
C  coordinate axis by angle theta.
C ======================================================================
      Subroutine Drotate(D, i, theta)
C ======================================================================
      Real*8  D(3, 3), theta, c, s, a, b
C ======================================================================
      j = i + 1
      If( j.GT.3 ) j = 1

      c = dcos(theta)
      s = dsin(theta)

      Do k = 1, 3
         a = D(k, i)
         b = D(k, j)

         D(k, i) = a * c - b * s
         D(k, j) = a * s + b * c
      End do

      Do k = 1, 3
         a = D(i, k)
         b = D(j, k)

         D(i, k) = a * c - b * s
         D(j, k) = a * s + b * c
      End do

      Return
      End

c ==========================================================
      Subroutine HMFEMrecoverUP(nv, vrt, nt, tet, material, DDATA,IDATA,
     &                          SOL, SOLatTets, VECatTets, iW, MaxWi)
c ==========================================================
c  Recovers pressures SOLatTets at cell centers and velocities
c  VECatTets at cells centers from Lagrange multipliers SOL
c  defined on mesh faces
c ==========================================================
      Implicit none
      include 'fem3Dtet.fd'

      Integer  nv, nt, MaxWi
      Real*8   vrt(3,*), DDATA(*)
      Integer  tet(4,*), material(*), IDATA(*),  iW(*)
      Real*8   SOL(*), SOLatTets(*), VECatTets(3,*)

      EXTERNAL FEM3Dext, Ddiff, Drhs
      Integer  Ddiff, Drhs

      Real*8    tri_area, calVol
      External  tri_area, calVol

C=====LOCAL VARIABLES======
      Integer   n,i,j,k, i1,i2,i3,i4, ir,ic, ifc, info
      Integer   nf, iIFE, inEP, iIEP, iiW, iSYS(5)

      Real*8    localA(5, 5), localF(5), xyzc(3), rW(100), s
      Real*8    face_area(4), vol

      Integer   Face2OpPnt(4)
      Data      Face2OpPnt / 4, 1, 2, 3 /

c ==========================================================
c     make a list E -> F
      iIFE = 1
      inEP = iIFE + 4 * nt
      iIEP = inEP + nv
      iiW  = iIEP + 4 * nt
      If(iiW .gt.MaxWi) Stop 'Please increase MaxWi'

      Call listE2F(nv, nf, nt, tet, iW(iIFE), iW(inEP), iW(iIEP))

c     loop over mesh cells
      Do n = 1, nt
         i1 = tet(1, n)
         i2 = tet(2, n)
         i3 = tet(3, n)
         i4 = tet(4, n)

c        compute mass matrix using material properties
c        we use the user-specified routine Ddiff()
         iSYS(1) = n
         iSYS(2) = i1
         iSYS(3) = i2
         iSYS(4) = i3
         iSYS(5) = i4

         Call fem3Dtet(
     &        vrt(1,i1), vrt(1,i2), vrt(1,i3), vrt(1,i4),
     &        IDEN, FEM_RT0, IDEN, FEM_RT0,
     &        material(n), Ddiff, DDATA, IDATA, iSYS, 2,
     &        5, localA, ir, ic)

c        add mass conservation equation (constraint matrix)
c        we use the divergent theorem here as we did in forlibfem.f
         localA(1, 5) = tri_area(vrt(1,i1), vrt(1,i2), vrt(1,i3))
         localA(2, 5) = tri_area(vrt(1,i2), vrt(1,i3), vrt(1,i4))
         localA(3, 5) = tri_area(vrt(1,i3), vrt(1,i4), vrt(1,i1))
         localA(4, 5) = tri_area(vrt(1,i4), vrt(1,i1), vrt(1,i2))

         localA(5, 5) = 0D0

         Do i = 1, 4
            face_area(i) = localA(i, 5)
         End do

c        compute right hand side (Lagrange muptipliers are in SOL)
  !       if(n.eq.227) then

!         write(*,*)iW(iIFE + 4*(n-1) + 0),iW(iIFE + 4*(n-1) + 1),
!     & iW(iIFE + 4*(n-1) + 2),iW(iIFE + 4*(n-1) + 3)
 !        write(*,*) tet(1,n),tet(2,n),tet(3,n),tet(4,n)
 !       endif

         Do i = 1, 4
            ifc = iW(iIFE + 4*(n-1) + i-1)

            localF(i) = localA(i, 5) * SOL(ifc)
         End do

c        we use the user-specified routine Drhs()
         Call fem3Dtet(
     &        vrt(1,i1), vrt(1,i2), vrt(1,i3), vrt(1,i4),
     &        IDEN, FEM_P0, IDEN, FEM_P0,
     &        material(n), Drhs, DDATA, IDATA, iSYS, 2,
     &        1, localF(5), ir, ic)

c        solve the 5x5 system localA x = localF. The first four
c        entries in the solution x are velocities on tetrahedron faces.
c        The 5-th entry is the solution p at the mass center of the
c        tetrahedron. The user may use the LAPACK routine dsysv.

         Call dsysv('U', 5, 1, localA, 5, iW, localF, 5, rW, 100, info)
         If (info.ne.0) stop 'error in dsysv'

c        recover pressure
         SOLatTets(n) = localF(5)

c        recover velocity vector
         vol = calVol(vrt(1,i1), vrt(1,i2), vrt(1,i3), vrt(1,i4))
         vol = dabs(vol)

         Do i = 1, 3
            xyzc(i) = (vrt(i,i1) + vrt(i,i2) + vrt(i,i3) + vrt(i,i4))/4
            VECatTets(i,n) = 0d0
         End do

         Do j = 1, 4 ! loop over faces
            k = Face2OpPnt(j)

            s = face_area(k) / (3*vol) * localF(j)

            i1 = tet(k, n)
            Do i = 1, 3
               VECatTets(i,n) = VECatTets(i,n) + s * (xyzc(i)-vrt(i,i1))
            End do
         End do
      End do

      Return
      End