!*******************************************************************************
!>
!  Jacobian partitioning using the DSM algorithm.
!
!### Reference
!  * Argonne National Laboratory. MINPACK Project. July 1983.
!    Thomas F. Coleman, Burton S. Garbow, Jorge J. More
!  * Thomas F. Coleman, Burton S. Garbow, Jorge J. More, "Algorithm 618:
!    FORTRAN subroutines for estimating sparse Jacobian Matrices",
!    ACM Transactions on Mathematical Software (TOMS),
!    Volume 10 Issue 3, Sept. 1984, Pages 346-347
!
!### History
!  * Jacob Williams, Nov. 2016, extensive refactoring into modern Fortran.

    module dsm_module

    use numdiff_kinds_module

    implicit none

    private

    public :: dsm
    public :: fdjs

    contains
!*******************************************************************************

!*******************************************************************************
!>
!  The purpose of `dsm` is to determine an optimal or near-
!  optimal consistent partition of the columns of a sparse
!  `m` by `n` matrix `a`.
!
!  the sparsity pattern of the matrix `a` is specified by
!  the arrays `indrow` and `indcol`. on input the indices
!  for the non-zero elements of `a` are
!
!  `indrow(k),indcol(k), k = 1,2,...,npairs`.
!
!  the (`indrow`,`indcol`) pairs may be specified in any order.
!  duplicate input pairs are permitted, but the subroutine
!  eliminates them.
!
!  the subroutine partitions the columns of `a` into groups
!  such that columns in the same group do not have a
!  non-zero in the same row position. a partition of the
!  columns of `a` with this property is consistent with the
!  direct determination of `a`.

    subroutine dsm(m,n,Npairs,Indrow,Indcol,Ngrp,Maxgrp,Mingrp,Info,Ipntr,Jpntr)

    implicit none

      integer,intent(in)  :: m       !! number of rows of `a` (>0)
      integer,intent(in)  :: n       !! number of columns of `a` (>0)
      integer,intent(in)  :: npairs  !! number of (`indrow`,`indcol`) pairs used
                                     !! to describe the sparsity pattern of `a` (>0)
      integer,intent(out) :: maxgrp  !! the number of groups in the partition
                                     !! of the columns of `a`.
      integer,intent(out) :: mingrp  !! a lower bound for the number of groups
                                     !! in any consistent partition of the
                                     !! columns of `a`.
      integer,intent(out) :: info    !! for normal termination `info = 1`.
                                     !! if `m`, `n`, or `npairs` is not positive,
                                     !! then `info = 0`. if the k-th element of
                                     !! `indrow` is not an integer between
                                     !! 1 and m or the k-th element of `indcol`
                                     !! is not an integer between 1 and n,
                                     !! then `info = -k`.
      integer,dimension(npairs),intent(inout) :: indrow  !! an integer array of length `npairs`. on input `indrow`
                                                         !! must contain the row indices of the non-zero elements of `a`.
                                                         !! on output `indrow` is permuted so that the corresponding
                                                         !! column indices are in non-decreasing order. the column
                                                         !! indices can be recovered from the array `jpntr`.
      integer,dimension(npairs),intent(inout) :: indcol  !! an integer array of length `npairs`. on input `indcol`
                                                         !! must contain the column indices of the non-zero elements of
                                                         !! `a`. on output `indcol` is permuted so that the corresponding
                                                         !! row indices are in non-decreasing order. the row indices
                                                         !! can be recovered from the array `ipntr`.
      integer,dimension(n),intent(out) :: ngrp   !! specifies the partition of the columns of `a`.
                                                 !! column `jcol` belongs to group `ngrp(jcol)`.
      integer,dimension(m+1),intent(out) :: ipntr   !! an integer output array of length `m + 1` which
                                                    !! specifies the locations of the column indices in `indcol`.
                                                    !! the column indices for row `i` are
                                                    !! `indcol(k), k = ipntr(i),...,ipntr(i+1)-1`.
                                                    !! note that `ipntr(m+1)-1` is then the number of non-zero
                                                    !! elements of the matrix `a`.
      integer,dimension(n+1),intent(out) :: jpntr   !! jpntr is an integer output array of length n + 1 which
                                                    !! specifies the locations of the row indices in indrow.
                                                    !! the row indices for column j are
                                                    !! `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
                                                    !! note that `jpntr(n+1)-1` is then the number of non-zero
                                                    !! elements of the matrix `a`.

    integer,dimension(max(m,6*n)) :: iwa     !! an integer work array
    integer :: i , ir , j , jp , k , maxclq , nnz , numgrp

    !  check the input data.

    Info = 0
    if ( m<1 .or. n<1 .or. Npairs<1 ) return
    do k = 1 , Npairs
        Info = -k
        if ( Indrow(k)<1 .or. Indrow(k)>m .or. Indcol(k)<1 .or. Indcol(k)>n ) return
    enddo
    Info = 1

    !  sort the data structure by columns.

    call srtdat(n,Npairs,Indrow,Indcol,Jpntr,Iwa)

    !  compress the data and determine the number of
    !  non-zero elements of a.

    do i = 1 , m
        Iwa(i) = 0
    enddo
    nnz = 1
    do j = 1 , n
        k = nnz
        do jp = Jpntr(j) , Jpntr(j+1) - 1
            ir = Indrow(jp)
            if ( Iwa(ir)/=j ) then
                Indrow(nnz) = ir
                nnz = nnz + 1
                Iwa(ir) = j
            endif
        enddo
        Jpntr(j) = k
    enddo
    Jpntr(n+1) = nnz

    !  extend the data structure to rows.

    call setr(m,n,Indrow,Jpntr,Indcol,Ipntr,Iwa)

    !  determine a lower bound for the number of groups.

    Mingrp = 0
    do i = 1 , m
        Mingrp = max(Mingrp,Ipntr(i+1)-Ipntr(i))
    enddo

    !  determine the degree sequence for the intersection
    !  graph of the columns of a.

    call degr(n,Indrow,Jpntr,Indcol,Ipntr,Iwa(5*n+1),Iwa(n+1))

    !  color the intersection graph of the columns of a
    !  with the smallest-last (sl) ordering.

    call slo(n,Indrow,Jpntr,Indcol,Ipntr,Iwa(5*n+1),Iwa(4*n+1),maxclq,&
                Iwa(1),Iwa(n+1),Iwa(2*n+1),Iwa(3*n+1))
    call seq(n,Indrow,Jpntr,Indcol,Ipntr,Iwa(4*n+1),Ngrp,Maxgrp,      &
                Iwa(n+1))
    Mingrp = max(Mingrp,maxclq)

    !  exit if the smallest-last ordering is optimal.

    if ( Maxgrp==Mingrp ) return

    !  color the intersection graph of the columns of a
    !  with the incidence-degree (id) ordering.

    call ido(m,n,Indrow,Jpntr,Indcol,Ipntr,Iwa(5*n+1),Iwa(4*n+1),     &
                maxclq,Iwa(1),Iwa(n+1),Iwa(2*n+1),Iwa(3*n+1))
    call seq(n,Indrow,Jpntr,Indcol,Ipntr,Iwa(4*n+1),Iwa(1),numgrp,    &
                Iwa(n+1))
    Mingrp = max(Mingrp,maxclq)

    !  retain the better of the two orderings so far.

    if ( numgrp<Maxgrp ) then
        Maxgrp = numgrp
        do j = 1 , n
            Ngrp(j) = Iwa(j)
        enddo

        !  exit if the incidence-degree ordering is optimal.

        if ( Maxgrp==Mingrp ) return
    endif

    !  color the intersection graph of the columns of a
    !  with the largest-first (lf) ordering.

    call numsrt(n,n-1,Iwa(5*n+1),-1,Iwa(4*n+1),Iwa(2*n+1),Iwa(n+1))
    call seq(n,Indrow,Jpntr,Indcol,Ipntr,Iwa(4*n+1),Iwa(1),numgrp,    &
                Iwa(n+1))

    !  retain the best of the three orderings and exit.

    if ( numgrp<Maxgrp ) then
        Maxgrp = numgrp
        do j = 1 , n
            Ngrp(j) = Iwa(j)
        enddo
    endif

    end subroutine dsm
!*******************************************************************************

!*******************************************************************************
!>
!  Given the sparsity pattern of an `m` by `n` matrix `a`,
!  this subroutine determines the degree sequence for
!  the intersection graph of the columns of `a`.
!
!  In graph-theory terminology, the intersection graph of
!  the columns of `a` is the loopless graph `g` with vertices
!  `a(j), j = 1,2,...,n` where `a(j)` is the `j`-th column of `a`
!  and with edge `(a(i),a(j))` if and only if columns `i` and `j`
!  have a non-zero in the same row position.
!
!@note The value of `m` is not needed by `degr` and is
!      therefore not present in the subroutine statement.

    subroutine degr(n,Indrow,Jpntr,Indcol,Ipntr,Ndeg,Iwa)

    implicit none

    integer,intent(in)                :: n       !! a positive integer input variable set to the number
                                                 !! of columns of `a`.
    integer,dimension(*),intent(in)   :: indrow  !! an integer input array which contains the row
                                                 !! indices for the non-zeroes in the matrix `a`.
    integer,dimension(n+1),intent(in) :: jpntr   !! an integer input array of length `n + 1` which
                                                 !! specifies the locations of the row indices in `indrow`.
                                                 !! the row indices for column `j` are
                                                 !! `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
                                                 !! **note** that `jpntr(n+1)-1` is then the number of non-zero
                                                 !! elements of the matrix `a`.
    integer,dimension(*),intent(in)   :: indcol  !! an integer input array which contains the
                                                 !! column indices for the non-zeroes in the matrix `a`.
    integer,dimension(*),intent(in)   :: ipntr   !! an integer input array of length `m + 1` which
                                                 !! specifies the locations of the column indices in `indcol`.
                                                 !! the column indices for row `i` are
                                                 !! `indcol(k), k = ipntr(i),...,ipntr(i+1)-1`.
                                                 !! **note** that `ipntr(m+1)-1` is then the number of non-zero
                                                 !! elements of the matrix `a`.
    integer,dimension(n),intent(out)   :: ndeg   !! an integer output array of length `n` which
                                                 !! specifies the degree sequence. the degree of the
                                                 !! `j`-th column of `a` is `ndeg(j)`.
    integer,dimension(n)               :: iwa    !! an integer work array of length `n`

    integer :: ic , ip , ir , jcol , jp

    ! initialization block.

    do jp = 1 , n
        Ndeg(jp) = 0
        Iwa(jp) = 0
    enddo

    ! compute the degree sequence by determining the contributions
    ! to the degrees from the current(jcol) column and further
    ! columns which have not yet been considered.

    do jcol = 2 , n
        Iwa(jcol) = n

        ! determine all positions (ir,jcol) which correspond
        ! to non-zeroes in the matrix.

        do jp = Jpntr(jcol) , Jpntr(jcol+1) - 1
            ir = Indrow(jp)

            ! for each row ir, determine all positions (ir,ic)
            ! which correspond to non-zeroes in the matrix.

            do ip = Ipntr(ir) , Ipntr(ir+1) - 1
                ic = Indcol(ip)

                ! array iwa marks columns which have contributed to
                ! the degree count of column jcol. update the degree
                ! counts of these columns as well as column jcol.

                if ( Iwa(ic)<jcol ) then
                    Iwa(ic) = jcol
                    Ndeg(ic) = Ndeg(ic) + 1
                    Ndeg(jcol) = Ndeg(jcol) + 1
                endif

            enddo
        enddo
    enddo

    end subroutine degr
!*******************************************************************************

!*******************************************************************************
!>
!  given the sparsity pattern of an `m` by `n` matrix `a`, this
!  subroutine determines an incidence-degree ordering of the
!  columns of `a`.
!
!  the incidence-degree ordering is defined for the loopless
!  graph `g` with vertices `a(j), j = 1,2,...,n` where `a(j)` is the
!  `j`-th column of `a` and with edge `(a(i),a(j))` if and only if
!  columns `i` and `j` have a non-zero in the same row position.
!
!  the incidence-degree ordering is determined recursively by
!  letting `list(k), k = 1,...,n` be a column with maximal
!  incidence to the subgraph spanned by the ordered columns.
!  among all the columns of maximal incidence, `ido` chooses a
!  column of maximal degree.

    subroutine ido(m,n,Indrow,Jpntr,Indcol,Ipntr,Ndeg,List,Maxclq, &
                   Iwa1,Iwa2,Iwa3,Iwa4)

    implicit none

    integer,intent(in)       :: m         !! a positive integer input variable set to the number
                                          !! of rows of `a`.
    integer,intent(in)       :: n         !! a positive integer input variable set to the number
                                          !! of columns of `a`.
    integer,intent(out)      :: Maxclq    !! an integer output variable set to the size
                                          !! of the largest clique found during the ordering.
    integer,dimension(*),intent(in) :: Indrow    !! an integer input array which contains the row
                                                 !! indices for the non-zeroes in the matrix `a`.
    integer,dimension(n+1),intent(in) :: Jpntr   !! an integer input array of length `n + 1` which
                                                 !! specifies the locations of the row indices in `indrow`.
                                                 !! the row indices for column `j` are
                                                 !! `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
                                                 !! **note** that `jpntr(n+1)-1` is then the number of non-zero
                                                 !! elements of the matrix `a`.
    integer,dimension(*),intent(in)     :: Indcol   !! an integer input array which contains the
                                                    !! column indices for the non-zeroes in the matrix `a`.
    integer,dimension(m+1),intent(in)   :: Ipntr    !! an integer input array of length `m + 1` which
                                                    !! specifies the locations of the column indices in `indcol`.
                                                    !! the column indices for row `i` are
                                                    !! `indcol(k), k = ipntr(i),...,ipntr(i+1)-1`.
                                                    !! **note** that `ipntr(m+1)-1` is then the number of non-zero
                                                    !! elements of the matrix `a`.
    integer,dimension(n),intent(in)     :: Ndeg     !! an integer input array of length `n` which specifies
                                                    !! the degree sequence. the degree of the `j`-th column
                                                    !! of `a` is `ndeg(j)`.
    integer,dimension(n),intent(out)    :: List     !! an integer output array of length `n` which specifies
                                                    !! the incidence-degree ordering of the columns of `a`. the `j`-th
                                                    !! column in this order is `list(j)`.
    integer,dimension(0:n-1) :: Iwa1      !! integer work array of length `n`.
    integer,dimension(n)     :: Iwa2      !! integer work array of length `n`.
    integer,dimension(n)     :: Iwa3      !! integer work array of length `n`.
    integer,dimension(n)     :: Iwa4      !! integer work array of length `n`.

    integer :: ic , ip , ir , jcol , jp , maxinc , maxlst , ncomp , &
               numinc , numlst , numord , numwgt

    ! sort the degree sequence.

    call numsrt(n,n-1,Ndeg,-1,Iwa4,Iwa2,Iwa3)

    ! initialization block.
    !
    ! create a doubly-linked list to access the incidences of the
    ! columns. the pointers for the linked list are as follows.
    !
    ! each un-ordered column ic is in a list (the incidence list)
    ! of columns with the same incidence.
    !
    ! iwa1(numinc) is the first column in the numinc list
    ! unless iwa1(numinc) = 0. in this case there are
    ! no columns in the numinc list.
    !
    ! iwa2(ic) is the column before ic in the incidence list
    ! unless iwa2(ic) = 0. in this case ic is the first
    ! column in this incidence list.
    !
    ! iwa3(ic) is the column after ic in the incidence list
    ! unless iwa3(ic) = 0. in this case ic is the last
    ! column in this incidence list.
    !
    ! if ic is an un-ordered column, then list(ic) is the
    ! incidence of ic to the graph induced by the ordered
    ! columns. if jcol is an ordered column, then list(jcol)
    ! is the incidence-degree order of column jcol.

    maxinc = 0
    do jp = n , 1 , -1
        ic = Iwa4(jp)
        Iwa1(n-jp) = 0
        Iwa2(ic) = 0
        Iwa3(ic) = Iwa1(0)
        if ( Iwa1(0)>0 ) Iwa2(Iwa1(0)) = ic
        Iwa1(0) = ic
        Iwa4(jp) = 0
        List(jp) = 0
    enddo

    ! DETERMINE THE MAXIMAL SEARCH LENGTH FOR THE LIST
    ! OF COLUMNS OF MAXIMAL INCIDENCE.

    maxlst = 0
    do ir = 1 , m
        maxlst = maxlst + (Ipntr(ir+1)-Ipntr(ir))**2
    enddo
    maxlst = maxlst/n
    Maxclq = 0
    numord = 1

    ! BEGINNING OF ITERATION LOOP.

    ! UPDATE THE SIZE OF THE LARGEST CLIQUE
    ! FOUND DURING THE ORDERING.

100 if ( maxinc==0 ) ncomp = 0
    ncomp = ncomp + 1
    if ( maxinc+1==ncomp ) Maxclq = max(Maxclq,ncomp)

    ! CHOOSE A COLUMN JCOL OF MAXIMAL DEGREE AMONG THE
    ! COLUMNS OF MAXIMAL INCIDENCE MAXINC.

200 jp = Iwa1(maxinc)
    if ( jp>0 ) then
        numwgt = -1
        do numlst = 1 , maxlst
            if ( Ndeg(jp)>numwgt ) then
                numwgt = Ndeg(jp)
                jcol = jp
            endif
            jp = Iwa3(jp)
            if ( jp<=0 ) exit
        enddo
        List(jcol) = numord
        numord = numord + 1

        ! TERMINATION TEST.

        if ( numord>n ) then

            ! INVERT THE ARRAY LIST.

            do jcol = 1 , n
                Iwa2(List(jcol)) = jcol
            enddo
            do jp = 1 , n
                List(jp) = Iwa2(jp)
            enddo

        else

            ! DELETE COLUMN JCOL FROM THE MAXINC LIST.

            if ( Iwa2(jcol)==0 ) then
                Iwa1(maxinc) = Iwa3(jcol)
            else
                Iwa3(Iwa2(jcol)) = Iwa3(jcol)
            endif
            if ( Iwa3(jcol)>0 ) Iwa2(Iwa3(jcol)) = Iwa2(jcol)

            ! FIND ALL COLUMNS ADJACENT TO COLUMN JCOL.

            Iwa4(jcol) = n

            ! DETERMINE ALL POSITIONS (IR,JCOL) WHICH CORRESPOND
            ! TO NON-ZEROES IN THE MATRIX.

            do jp = Jpntr(jcol) , Jpntr(jcol+1) - 1
                ir = Indrow(jp)

                ! FOR EACH ROW IR, DETERMINE ALL POSITIONS (IR,IC)
                ! WHICH CORRESPOND TO NON-ZEROES IN THE MATRIX.

                do ip = Ipntr(ir) , Ipntr(ir+1) - 1
                    ic = Indcol(ip)

                    ! ARRAY IWA4 MARKS COLUMNS WHICH ARE ADJACENT TO
                    ! COLUMN JCOL.

                    if ( Iwa4(ic)<numord ) then
                        Iwa4(ic) = numord

                        ! UPDATE THE POINTERS TO THE CURRENT INCIDENCE LISTS.

                        numinc = List(ic)
                        List(ic) = List(ic) + 1
                        maxinc = max(maxinc,List(ic))

                        ! DELETE COLUMN IC FROM THE NUMINC LIST.

                        if ( Iwa2(ic)==0 ) then
                            Iwa1(numinc) = Iwa3(ic)
                        else
                            Iwa3(Iwa2(ic)) = Iwa3(ic)
                        endif
                        if ( Iwa3(ic)>0 ) Iwa2(Iwa3(ic)) = Iwa2(ic)

                        ! ADD COLUMN IC TO THE NUMINC+1 LIST.

                        Iwa2(ic) = 0
                        Iwa3(ic) = Iwa1(numinc+1)
                        if ( Iwa1(numinc+1)>0 ) Iwa2(Iwa1(numinc+1)) = ic
                        Iwa1(numinc+1) = ic
                    endif
                enddo
            enddo

            ! END OF ITERATION LOOP.

            goto 100
        endif
    else
        maxinc = maxinc - 1
        goto 200
    endif

    end subroutine ido
!*******************************************************************************

!*******************************************************************************
!>
!  Given a sequence of integers, this subroutine groups
!  together those indices with the same sequence value
!  and, optionally, sorts the sequence into either
!  ascending or descending order.
!
!  The sequence of integers is defined by the array `num`,
!  and it is assumed that the integers are each from the set
!  `0,1,...,nmax`. on output the indices `k` such that `num(k) = l`
!  for any `l = 0,1,...,nmax` can be obtained from the arrays
!  last and next as follows.
!```fortran
!  k = last(l)
!  while (k /= 0) k = next(k)
!```
!  Optionally, the subroutine produces an array index so that
!  the sequence `num(index(i)), i = 1,2,...,n` is sorted.

    subroutine numsrt(n,Nmax,Num,Mode,Index,Last,Next)

    implicit none

    integer,intent(in)        :: n      !! a positive integer input variable.
    integer                   :: Nmax   !! a positive integer input variable.
    integer                   :: Mode   !! an integer input variable. the sequence `num` is
                                        !! sorted in ascending order if `mode` is positive and in
                                        !! descending order if `mode` is negative. if `mode` is 0,
                                        !! no sorting is done.
    integer,dimension(n)      :: Num    !! an input array of length `n` which contains the
                                        !! sequence of integers to be grouped and sorted. it
                                        !! is assumed that the integers are each from the set
                                        !! `0,1,...,nmax`.
    integer,dimension(n)      :: Index  !! an integer output array of length `n` set so
                                        !! that the sequence
                                        !! `num(index(i)), i = 1,2,...,n`
                                        !! is sorted according to the setting of mode.
                                        !! if `mode` is 0, `index` is not referenced.
    integer,dimension(0:Nmax) :: Last   !! an integer output array of length `nmax + 1`. the
                                        !! index of `num` for the last occurrence of `l` is `last(l)`
                                        !! for any `l = 0,1,...,nmax` unless `last(l) = 0`. in
                                        !! this case `l` does not appear in `num`.
    integer,dimension(n)      :: Next   !! an integer output array of length `n`. if
                                        !! `num(k) = l`, then the index of `num` for the previous
                                        !! occurrence of `l` is `next(k)` for any `l = 0,1,...,nmax`
                                        !! unless `next(k) = 0`. in this case there is no previous
                                        !! occurrence of `l` in `num`.

    integer :: i , j , jinc , jl , ju , k , l

    ! determine the arrays next and last.

    do i = 0 , Nmax
        Last(i) = 0
    enddo

    do k = 1 , n
        l = Num(k)
        Next(k) = Last(l)
        Last(l) = k
    enddo

    if ( Mode/=0 ) then

        ! store the pointers to the sorted array in index.
        i = 1
        if ( Mode>0 ) then
            jl = 0
            ju = Nmax
            jinc = 1
        else
            jl = Nmax
            ju = 0
            jinc = -1
        endif
        do j = jl , ju , jinc
            k = Last(j)
            do
                if (k==0) exit
                Index(i) = k
                i = i + 1
                k = Next(k)
            enddo
        enddo

    end if

    end subroutine numsrt
!*******************************************************************************

!*******************************************************************************
!>
!  given the sparsity pattern of an `m` by `n` matrix `a`, this
!  subroutine determines a consistent partition of the
!  columns of `a` by a sequential algorithm.
!
!  a consistent partition is defined in terms of the loopless
!  graph `g` with vertices `a(j), j = 1,2,...,n` where `a(j)` is the
!  `j`-th column of `a` and with edge `(a(i),a(j))` if and only if
!  columns `i` and `j` have a non-zero in the same row position.
!
!  a partition of the columns of a into groups is consistent
!  if the columns in any group are not adjacent in the graph `g`.
!  in graph-theory terminology, a consistent partition of the
!  columns of a corresponds to a coloring of the graph `g`.
!
!  the subroutine examines the columns in the order specified
!  by the array list, and assigns the current column to the
!  group with the smallest possible number.
!
!  note that the value of `m` is not needed by `seq` and is
!  therefore not present in the subroutine statement.

    subroutine seq(n,Indrow,Jpntr,Indcol,Ipntr,List,Ngrp,Maxgrp,Iwa)

    implicit none

    integer                :: n         !! a positive integer input variable set to the number
                                        !! of columns of `a`.
    integer                :: Maxgrp    !! an integer output variable which specifies the
                                        !! number of groups in the partition of the columns of `a`.
    integer,dimension(*)   :: Indrow    !! an integer input array which contains the row
                                        !! indices for the non-zeroes in the matrix `a`.
    integer,dimension(n+1) :: Jpntr     !! an integer input array of length `n + 1` which
                                        !! specifies the locations of the row indices in `indrow`.
                                        !! the row indices for column `j` are
                                        !! `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
                                        !! **note** that `jpntr(n+1)-1` is then the number of non-zero
                                        !! elements of the matrix `a`.
    integer,dimension(*)   :: Indcol    !! an integer input array which contains the
                                        !! column indices for the non-zeroes in the matrix `a`.
    integer,dimension(*)   :: Ipntr     !! an integer input array of length `m + 1` which
                                        !! specifies the locations of the column indices in `indcol`.
                                        !! the column indices for row `i` are
                                        !! `indcol(k), k = ipntr(i),...,ipntr(i+1)-1`.
                                        !! **note** that `ipntr(m+1)-1` is then the number of non-zero
                                        !! elements of the matrix `a`.
    integer,dimension(n)   :: List      !! an integer input array of length `n` which specifies
                                        !! the order to be used by the sequential algorithm.
                                        !! the `j`-th column in this order is `list(j)`.
    integer,dimension(n)   :: Ngrp      !! an integer output array of length `n` which specifies
                                        !! the partition of the columns of `a`. column `jcol` belongs
                                        !! to group `ngrp(jcol)`.
    integer,dimension(n)   :: Iwa       !! an integer work array of length `n`

    integer :: ic , ip , ir , j , jcol , jp

    ! initialization block.

    Maxgrp = 0
    do jp = 1 , n
        Ngrp(jp) = n
        Iwa(jp) = 0
    enddo

    ! beginning of iteration loop.
    do j = 1 , n

        jcol = List(j)

        ! find all columns adjacent to column jcol.
        !
        ! determine all positions (ir,jcol) which correspond
        ! to non-zeroes in the matrix.
        do jp = Jpntr(jcol) , Jpntr(jcol+1) - 1
            ir = Indrow(jp)
            ! for each row ir, determine all positions (ir,ic)
            ! which correspond to non-zeroes in the matrix.
            do ip = Ipntr(ir) , Ipntr(ir+1) - 1
                ic = Indcol(ip)
                ! array iwa marks the group numbers of the
                ! columns which are adjacent to column jcol.
                Iwa(Ngrp(ic)) = j
            enddo
        enddo

        ! assign the smallest un-marked group number to jcol.
        do jp = 1 , Maxgrp
            if ( Iwa(jp)/=j ) then
                Maxgrp = Maxgrp - 1
                exit
            end if
        enddo
        Maxgrp = Maxgrp + 1
        Ngrp(jcol) = jp

    enddo
    ! end of iteration loop.

    end subroutine seq
!*******************************************************************************

!*******************************************************************************
!>
!  given a column-oriented definition of the sparsity pattern
!  of an `m` by `n` matrix `a`, this subroutine determines a
!  row-oriented definition of the sparsity pattern of `a`.
!
!  on input the column-oriented definition is specified by
!  the arrays `indrow` and `jpntr`. on output the row-oriented
!  definition is specified by the arrays `indcol` and `ipntr`.

    subroutine setr(m,n,Indrow,Jpntr,Indcol,Ipntr,Iwa)

    implicit none

    integer,intent(in)     :: m       !! a positive integer input variable set to the number
                                      !! of rows of `a`.
    integer,intent(in)     :: n       !! a positive integer input variable set to the number
                                      !! of columns of `a`.
    integer,dimension(*)   :: Indrow  !! an integer input array which contains the row
                                      !! indices for the non-zeroes in the matrix `a`.
    integer,dimension(n+1) :: Jpntr   !! an integer input array of length `n + 1` which
                                      !! specifies the locations of the row indices in `indrow`.
                                      !! the row indices for column `j` are
                                      !! `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
                                      !! **note** that `jpntr(n+1)-1` is then the number of non-zero
                                      !! elements of the matrix `a`.
    integer,dimension(*)   :: Indcol  !! an integer output array which contains the
                                      !! column indices for the non-zeroes in the matrix `a`.
    integer,dimension(m+1) :: Ipntr   !! an integer output array of length `m + 1` which
                                      !! specifies the locations of the column indices in `indcol`.
                                      !! the column indices for row `i` are
                                      !! `indcol(k), k = ipntr(i),...,ipntr(i+1)-1`.
                                      !! **note** that `ipntr(1)` is set to 1 and that `ipntr(m+1)-1` is
                                      !! then the number of non-zero elements of the matrix `a`.
    integer,dimension(m)   :: Iwa     !! an integer work array of length `m`.

    integer :: ir , jcol , jp

    ! store in array iwa the counts of non-zeroes in the rows.

    do ir = 1 , m
        Iwa(ir) = 0
    enddo
    do jp = 1 , Jpntr(n+1) - 1
        Iwa(Indrow(jp)) = Iwa(Indrow(jp)) + 1
    enddo

    ! set pointers to the start of the rows in indcol.

    Ipntr(1) = 1
    do ir = 1 , m
        Ipntr(ir+1) = Ipntr(ir) + Iwa(ir)
        Iwa(ir) = Ipntr(ir)
    enddo

    ! fill indcol.

    do jcol = 1 , n
        do jp = Jpntr(jcol) , Jpntr(jcol+1) - 1
            ir = Indrow(jp)
            Indcol(Iwa(ir)) = jcol
            Iwa(ir) = Iwa(ir) + 1
        enddo
    enddo

    end subroutine setr
!*******************************************************************************

!*******************************************************************************
!>
!  given the sparsity pattern of an `m` by `n` matrix `a`, this
!  subroutine determines the smallest-last ordering of the
!  columns of `a`.
!
!  the smallest-last ordering is defined for the loopless
!  graph `g` with vertices `a(j), j = 1,2,...,n` where `a(j)` is the
!  `j`-th column of `a` and with edge `(a(i),a(j))` if and only if
!  columns `i` and `j` have a non-zero in the same row position.
!
!  the smallest-last ordering is determined recursively by
!  letting `list(k), k = n,...,1` be a column with least degree
!  in the subgraph spanned by the un-ordered columns.
!
!  note that the value of `m` is not needed by `slo` and is
!  therefore not present in the subroutine statement.

    subroutine slo(n,Indrow,Jpntr,Indcol,Ipntr,Ndeg,List,Maxclq,Iwa1, &
                   Iwa2,Iwa3,Iwa4)

    implicit none

    integer                  :: n         !! a positive integer input variable set to the number
                                          !! of columns of `a`.
    integer                  :: Maxclq    !! an integer output variable set to the size
                                          !! of the largest clique found during the ordering.
    integer,dimension(*)     :: Indrow    !! an integer input array which contains the row
                                          !! indices for the non-zeroes in the matrix `a`.
    integer,dimension(n+1)   :: Jpntr     !! an integer input array of length `n + 1` which
                                          !! specifies the locations of the row indices in `indrow`.
                                          !! the row indices for column `j` are
                                          !! `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
                                          !! **note** that `jpntr(n+1)-1` is then the number of non-zero
                                          !! elements of the matrix `a`.
    integer,dimension(*)     :: Indcol    !! an integer input array which contains the
                                          !! column indices for the non-zeroes in the matrix `a`.
    integer,dimension(*)     :: Ipntr     !! an integer input array of length `m + 1` which
                                          !! specifies the locations of the column indices in `indcol`.
                                          !! the column indices for row `i` are
                                          !! `indcol(k), k = ipntr(i),...,ipntr(i+1)-1`.
                                          !! **note** that `ipntr(m+1)-1` is then the number of non-zero
                                          !! elements of the matrix `a`.
    integer,dimension(n)     :: Ndeg      !! an integer input array of length `n` which specifies
                                          !! the degree sequence. the degree of the `j`-th column
                                          !! of `a` is `ndeg(j)`.
    integer,dimension(n)     :: List      !! an integer output array of length `n` which specifies
                                          !! the smallest-last ordering of the columns of `a`. the `j`-th
                                          !! column in this order is `list(j)`.
    integer,dimension(0:n-1) :: Iwa1      !! integer work array of length `n`
    integer,dimension(n)     :: Iwa2      !! integer work array of length `n`
    integer,dimension(n)     :: Iwa3      !! integer work array of length `n`
    integer,dimension(n)     :: Iwa4      !! integer work array of length `n`

    integer :: ic , ip , ir , jcol , jp , mindeg , numdeg , numord

    ! INITIALIZATION BLOCK.

    mindeg = n
    do jp = 1 , n
        Iwa1(jp-1) = 0
        Iwa4(jp) = n
        List(jp) = Ndeg(jp)
        mindeg = min(mindeg,Ndeg(jp))
    enddo

    ! CREATE A DOUBLY-LINKED LIST TO ACCESS THE DEGREES OF THE
    ! COLUMNS. THE POINTERS FOR THE LINKED LIST ARE AS FOLLOWS.
    !
    ! EACH UN-ORDERED COLUMN IC IS IN A LIST (THE DEGREE LIST)
    ! OF COLUMNS WITH THE SAME DEGREE.
    !
    ! IWA1(NUMDEG) IS THE FIRST COLUMN IN THE NUMDEG LIST
    ! UNLESS IWA1(NUMDEG) = 0. IN THIS CASE THERE ARE
    ! NO COLUMNS IN THE NUMDEG LIST.
    !
    ! IWA2(IC) IS THE COLUMN BEFORE IC IN THE DEGREE LIST
    ! UNLESS IWA2(IC) = 0. IN THIS CASE IC IS THE FIRST
    ! COLUMN IN THIS DEGREE LIST.
    !
    ! IWA3(IC) IS THE COLUMN AFTER IC IN THE DEGREE LIST
    ! UNLESS IWA3(IC) = 0. IN THIS CASE IC IS THE LAST
    ! COLUMN IN THIS DEGREE LIST.
    !
    ! IF IC IS AN UN-ORDERED COLUMN, THEN LIST(IC) IS THE
    ! DEGREE OF IC IN THE GRAPH INDUCED BY THE UN-ORDERED
    ! COLUMNS. IF JCOL IS AN ORDERED COLUMN, THEN LIST(JCOL)
    ! IS THE SMALLEST-LAST ORDER OF COLUMN JCOL.

    do jp = 1 , n
        numdeg = Ndeg(jp)
        Iwa2(jp) = 0
        Iwa3(jp) = Iwa1(numdeg)
        if ( Iwa1(numdeg)>0 ) Iwa2(Iwa1(numdeg)) = jp
        Iwa1(numdeg) = jp
    enddo
    Maxclq = 0
    numord = n

    !  BEGINNING OF ITERATION LOOP.
    !
    !
    ! MARK THE SIZE OF THE LARGEST CLIQUE
    ! FOUND DURING THE ORDERING.

100 if ( mindeg+1==numord .and. Maxclq==0 ) Maxclq = numord

    ! CHOOSE A COLUMN JCOL OF MINIMAL DEGREE MINDEG.

200 jcol = Iwa1(mindeg)
    if ( jcol>0 ) then
        List(jcol) = numord
        numord = numord - 1

        ! TERMINATION TEST.

        if ( numord==0 ) then

            ! INVERT THE ARRAY LIST.

            do jcol = 1 , n
                Iwa2(List(jcol)) = jcol
            enddo
            do jp = 1 , n
                List(jp) = Iwa2(jp)
            enddo

        else

            ! DELETE COLUMN JCOL FROM THE MINDEG LIST.

            Iwa1(mindeg) = Iwa3(jcol)
            if ( Iwa3(jcol)>0 ) Iwa2(Iwa3(jcol)) = 0

            ! FIND ALL COLUMNS ADJACENT TO COLUMN JCOL.

            Iwa4(jcol) = 0

            ! DETERMINE ALL POSITIONS (IR,JCOL) WHICH CORRESPOND
            ! TO NON-ZEROES IN THE MATRIX.

            do jp = Jpntr(jcol) , Jpntr(jcol+1) - 1
                ir = Indrow(jp)

                ! FOR EACH ROW IR, DETERMINE ALL POSITIONS (IR,IC)
                ! WHICH CORRESPOND TO NON-ZEROES IN THE MATRIX.

                do ip = Ipntr(ir) , Ipntr(ir+1) - 1
                    ic = Indcol(ip)

                    ! ARRAY IWA4 MARKS COLUMNS WHICH ARE ADJACENT TO
                    ! COLUMN JCOL.

                    if ( Iwa4(ic)>numord ) then
                        Iwa4(ic) = numord

                        ! UPDATE THE POINTERS TO THE CURRENT DEGREE LISTS.

                        numdeg = List(ic)
                        List(ic) = List(ic) - 1
                        mindeg = min(mindeg,List(ic))

                        ! DELETE COLUMN IC FROM THE NUMDEG LIST.

                        if ( Iwa2(ic)==0 ) then
                            Iwa1(numdeg) = Iwa3(ic)
                        else
                            Iwa3(Iwa2(ic)) = Iwa3(ic)
                        endif
                        if ( Iwa3(ic)>0 ) Iwa2(Iwa3(ic)) = Iwa2(ic)

                        ! ADD COLUMN IC TO THE NUMDEG-1 LIST.

                        Iwa2(ic) = 0
                        Iwa3(ic) = Iwa1(numdeg-1)
                        if ( Iwa1(numdeg-1)>0 ) Iwa2(Iwa1(numdeg-1)) = ic
                        Iwa1(numdeg-1) = ic
                    endif
                enddo
            enddo

            ! END OF ITERATION LOOP.

            goto 100
        endif
    else
        mindeg = mindeg + 1
        goto 200
    endif

    end subroutine slo
!*******************************************************************************

!*******************************************************************************
!>
!  given the non-zero elements of an `m` by `n` matrix `a` in
!  arbitrary order as specified by their row and column
!  indices, this subroutine permutes these elements so
!  that their column indices are in non-decreasing order.
!
!  on input it is assumed that the elements are specified in
!
!  `indrow(k),indcol(k), k = 1,...,nnz`.
!
!  on output the elements are permuted so that `indcol` is
!  in non-decreasing order. in addition, the array `jpntr`
!  is set so that the row indices for column `j` are
!
!  `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
!
!  note that the value of `m` is not needed by srtdat and is
!  therefore not present in the subroutine statement.

    subroutine srtdat(n,Nnz,Indrow,Indcol,Jpntr,Iwa)

    implicit none

    integer                :: n       !! a positive integer input variable set to the number
                                      !! of columns of `a`.
    integer                :: Nnz     !! a positive integer input variable set to the number
                                      !! of non-zero elements of `a`.
    integer,dimension(Nnz) :: Indrow  !! an integer array of length `nnz`. on input `indrow`
                                      !! must contain the row indices of the non-zero elements of `a`.
                                      !! on output `indrow` is permuted so that the corresponding
                                      !! column indices of `indcol` are in non-decreasing order.
    integer,dimension(Nnz) :: Indcol  !! an integer array of length `nnz`. on input `indcol`
                                      !! must contain the column indices of the non-zero elements
                                      !! of `a`. on output `indcol` is permuted so that these indices
                                      !! are in non-decreasing order.
    integer,dimension(n+1) :: Jpntr   !! an integer output array of length `n + 1` which
                                      !! specifies the locations of the row indices in the output
                                      !! `indrow`. the row indices for column `j` are
                                      !! `indrow(k), k = jpntr(j),...,jpntr(j+1)-1`.
                                      !! **note** that `jpntr(1)` is set to 1 and that `jpntr(n+1)-1`
                                      !! is then `nnz`.
    integer,dimension(n)   :: Iwa     !! an integer work array of length `n`.

    integer :: i , j , k , l

    ! store in array iwa the counts of non-zeroes in the columns.

    do j = 1 , n
        Iwa(j) = 0
    enddo
    do k = 1 , Nnz
        Iwa(Indcol(k)) = Iwa(Indcol(k)) + 1
    enddo

    ! set pointers to the start of the columns in indrow.

    Jpntr(1) = 1
    do j = 1 , n
        Jpntr(j+1) = Jpntr(j) + Iwa(j)
        Iwa(j) = Jpntr(j)
    enddo
    k = 1

    ! begin in-place sort.

    do
        j = Indcol(k)
        if ( k>=Jpntr(j) ) then
            ! current element is in position. now examine the
            ! next element or the first un-sorted element in
            ! the j-th group.
            k = max(k+1,Iwa(j))
        else
            ! current element is not in position. place element
            ! in position and make the displaced element the
            ! current element.
            l = Iwa(j)
            Iwa(j) = Iwa(j) + 1
            i = Indrow(k)
            Indrow(k) = Indrow(l)
            Indcol(k) = Indcol(l)
            Indrow(l) = i
            Indcol(l) = j
        endif
        if ( k>Nnz ) exit
    end do

    end subroutine srtdat
!*******************************************************************************

!*******************************************************************************
!>
!  Given a consistent partition of the columns of an `m` by `n`
!  jacobian matrix into groups, this subroutine computes
!  approximations to those columns in a given group.  the
!  approximations are stored into either a column-oriented
!  or a row-oriented pattern.
!
!  a partition is consistent if the columns in any group
!  do not have a non-zero in the same row position.
!
!  approximations to the columns of the jacobian matrix in a
!  given group can be obtained by specifying a difference
!  parameter array `d` with `d(jcol)` non-zero if and only if
!  `jcol` is a column in the group, and an approximation to
!  `jac*d` where `jac` denotes the jacobian matrix of a mapping f.
!
!  `d` can be defined with the following segment of code.
!```fortran
!  do jcol = 1, n
!  d(jcol) = 0.0
!  if (ngrp(jcol) == numgrp) d(jcol) = eta(jcol)
!  end do
!```
!  in the above code `numgrp` is the given group number,
!  `ngrp(jcol)` is the group number of column `jcol`, and
!  `eta(jcol)` is the difference parameter used to
!  approximate column `jcol` of the jacobian matrix.
!  suitable values for the array `eta` must be provided.
!
!  as mentioned above, an approximation to `jac*d` must
!  also be provided. for example, the approximation
!```fortran
!  f(x+d) - f(x)
!```
!  corresponds to the forward difference formula at `x`.

    subroutine fdjs(m,n,Col,Ind,Npntr,Ngrp,Numgrp,d,Fjacd,Fjac)

    implicit none

    integer,intent(in)    :: m        !! a positive integer input variable set to the number
                                      !! of rows of the jacobian matrix.
    integer,intent(in)    :: n        !! a positive integer input variable set to the number
                                      !! of columns of the jacobian matrix.
    integer               :: Numgrp   !! a positive integer input variable set to a group
                                      !! number in the partition. the columns of the jacobian
                                      !! matrix in this group are to be estimated on this call.
    integer,dimension(*)  :: Ind      !! an integer input array which contains the row
                                      !! indices for the non-zeroes in the jacobian matrix
                                      !! if `col` is true, and contains the column indices for
                                      !! the non-zeroes in the jacobian matrix if `col` is false.
    integer,dimension(*)  :: Npntr    !! an integer input array which specifies the
                                      !! locations of the row indices in `ind` if `col` is true, and
                                      !! specifies the locations of the column indices in `ind` if
                                      !! `col` is false. if `col` is true, the indices for column `j` are
                                      !!       `ind(k), k = npntr(j),...,npntr(j+1)-1`.
                                      !! if `col` is false, the indices for row `i` are
                                      !!       `ind(k), k = npntr(i),...,npntr(i+1)-1`.
                                      !! ***Note*** that `npntr(n+1)-1` if `col` is true, or `npntr(m+1)-1`
                                      !! if `col` is false, is then the number of non-zero elements
                                      !! of the jacobian matrix.
    integer,dimension(n)  :: Ngrp     !! an integer input array of length `n` which specifies
                                      !! the partition of the columns of the jacobian matrix.
                                      !! column `jcol` belongs to group `ngrp(jcol)`.
    real(wp),dimension(n) :: d        !! an input array of length `n` which contains the
                                      !! difference parameter vector for the estimate of
                                      !! the jacobian matrix columns in group `numgrp`.
    real(wp),dimension(m) :: Fjacd    !! an input array of length `m` which contains
                                      !! an approximation to the difference vector `jac*d`,
                                      !! where `jac` denotes the jacobian matrix.
    real(wp),dimension(*) :: Fjac     !! an output array of length `nnz`, where `nnz` is the
                                      !! number of its non-zero elements. at each call of `fdjs`,
                                      !! `fjac` is updated to include the non-zero elements of the
                                      !! jacobian matrix for those columns in group `numgrp`. `fjac`
                                      !! should not be altered between successive calls to `fdjs`.
    logical,intent(in)    :: Col      !! a logical input variable. if `col` is set true, then the
                                      !! jacobian approximations are stored into a column-oriented
                                      !! pattern. if `col` is set false, then the jacobian
                                      !! approximations are stored into a row-oriented pattern.

    integer :: ip , irow , jcol , jp

    ! compute estimates of jacobian matrix columns in group
    ! numgrp. the array fjacd must contain an approximation
    ! to jac*d, where jac denotes the jacobian matrix and d
    ! is a difference parameter vector with d(jcol) non-zero
    ! if and only if jcol is a column in group numgrp.

    if ( Col ) then  ! column orientation.

        do jcol = 1 , n
            if ( Ngrp(jcol)==Numgrp ) then
                do jp = Npntr(jcol) , Npntr(jcol+1) - 1
                    irow = Ind(jp)
                    Fjac(jp) = Fjacd(irow)/d(jcol)
                enddo
            endif
        enddo

    else  ! row orientation.

        do irow = 1 , m
            do ip = Npntr(irow) , Npntr(irow+1) - 1
                jcol = Ind(ip)
                if ( Ngrp(jcol)==Numgrp ) then
                    Fjac(ip) = Fjacd(irow)/d(jcol)
                    exit
                endif
            enddo
        enddo

    endif

    end subroutine fdjs
!*******************************************************************************

!*******************************************************************************
    end module dsm_module
!*******************************************************************************


module djkstra_mod
    use,intrinsic :: iso_fortran_env
    use heap_map_int_int_mod
    use vector_mod
    implicit none
contains
    function djkstra(graph, cost, start) result(dist)
        ! start頂点からの各頂への最短距離をdjkstraで計算、distに格納し、返却
        integer(int64):: start
        integer(int64), allocatable:: dist(:)
        integer(int64),parameter:: inf = 10_int64**18
        type(vector):: graph(:), cost(:)
        type(heap_map_int_int):: que

        allocate(dist(size(graph)), source=inf)
        dist(start) = 0
        call que%push(0_int64, start)

        do while(que%remain())
            block
                integer(int64):: d, from, i
                call que%pop(d, from)
                if (dist(from) < d) cycle

                do i=1,graph(from)%size()
                    block
                        integer(int64):: to, c

                        to = graph(from)%at(i)
                        c = cost(from)%at(i)
                        if (dist(from) + c < dist(to)) then
                            dist(to) = dist(from) + c
                            call que%push(dist(to), to)
                        end if
                    end block
                end do

            end block
        end do

    end function
end module


      DOUBLE PRECISION FUNCTION dln1px(a)
C**********************************************************************
C
C     DOUBLE PRECISION FUNCTION DLN1PX(X)
C               Double precision LN(1+X)
C
C
C                              Function
C
C
C     Returns ln(1+x)
C     Note that the obvious code of
C               LOG(1.0+X)
C     won't work for small X because 1.0+X loses accuracy
C
C
C                              Arguments
C
C
C     X --> Value for which ln(1-x) is desired.
C                                        X is DOUBLE PRECISION
C
C
C                              Method
C
C
C     Renames ALNREL from:
C     DiDinato, A. R. and Morris,  A.   H.  Algorithm 708: Significant
C     Digit Computation of the Incomplete  Beta  Function Ratios.  ACM
C     Trans. Math.  Softw. 18 (1993), 360-373.
C
C**********************************************************************
C-----------------------------------------------------------------------
C            EVALUATION OF THE FUNCTION LN(1 + A)
C-----------------------------------------------------------------------
C     .. Scalar Arguments ..
      DOUBLE PRECISION a
C     ..
C     .. Local Scalars ..
      DOUBLE PRECISION p1,p2,p3,q1,q2,q3,t,t2,w,x
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC abs,dble,dlog
C     ..
C     .. Data statements ..
      DATA p1/-.129418923021993D+01/,p2/.405303492862024D+00/,
     +     p3/-.178874546012214D-01/
      DATA q1/-.162752256355323D+01/,q2/.747811014037616D+00/,
     +     q3/-.845104217945565D-01/
C     ..
C     .. Executable Statements ..
C--------------------------
      IF (abs(a).GT.0.375D0) GO TO 10
      t = a/ (a+2.0D0)
      t2 = t*t
      w = (((p3*t2+p2)*t2+p1)*t2+1.0D0)/ (((q3*t2+q2)*t2+q1)*t2+1.0D0)
      dln1px = 2.0D0*t*w
      RETURN
C
   10 x = 1.D0 + dble(a)
      dln1px = dlog(x)
      RETURN

      END


SUBROUTINE eop_interp (n_interp, filename, EOP_sol, mjd , x_int,y_int,ut1_int)

! ----------------------------------------------------------------------
! Subroutine:	eop_interp.f90
! ----------------------------------------------------------------------
! Purpose:
!  Form arrays of EOP data which are to be used for EOP data interpolation
!  Corrections due to ocean tidal and libration effects
! ----------------------------------------------------------------------
! Input arguments:
! - n_interp:		number of points to be used for interpolation
! - filename:		EOP data file name e.g. 'eopc04_IAU2000.62-now'
! - mjd:			Modified Julian Day number of the input epoch (in UTC)
!
! Output arguments:
! - x_int,y_int:	Polar motion coordinates (arcsec) at input epoch considering
!					corrections due to ocean tidal and libration effects
! - ut1_int:		UT1-UTC difference (sec) at input epoch considering
!					corrections due to ocean tidal and libration effects
! ----------------------------------------------------------------------
! Dr. Thomas Papanikolaou, Geoscience Australia            December 2015
! ----------------------------------------------------------------------

      USE mdl_precision
      USE mdl_num
      IMPLICIT NONE

! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      INTEGER (KIND = prec_int4), INTENT(IN) :: n_interp
      INTEGER (KIND = prec_int1), INTENT(IN) :: EOP_sol
      CHARACTER (LEN=512), INTENT(IN) :: filename
      REAL (KIND = prec_d), INTENT(IN) :: mjd
! OUT
      DOUBLE PRECISION x_int, y_int, ut1_int
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
! ----------------------------------------------------------------------
      INTEGER (KIND = prec_int8) :: MJD_day, MJD_i, n_cent
      INTEGER (KIND = prec_int4) :: i
      REAL (KIND = prec_d) :: EOP_i(7)
      REAL (KIND = prec_d) :: MJDint_ar(n_interp), xint_ar(n_interp), yint_ar(n_interp), UT1int_ar(n_interp)
! ----------------------------------------------------------------------

      MJD_day = INT (mjd)

! ----------------------------------------------------------------------
! Epochs center definition
      IF (n_interp/2 - INT(n_interp/2) == 0) THEN
	     n_cent = n_interp/2
      ELSE
         n_cent = INT(n_interp/2) + 1
      END IF
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
      DO i = 1 , n_interp
         MJD_i = MJD_day - n_cent + i
         CALL eop_rd (filename, EOP_sol, MJD_i , EOP_i)
         MJDint_ar(i) = MJD_i
         xint_ar(i) = EOP_i(2)
         yint_ar(i) = EOP_i(3)
         UT1int_ar(i) = EOP_i(4)
      END DO
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! interp.f: Ocean tidal and libration effects at interpolation epoch
      CALL INTERP_iers (MJDint_ar, xint_ar, yint_ar, UT1int_ar, n_interp, mjd, x_int,y_int,ut1_int)
! ----------------------------------------------------------------------

END


	function dccos(z1)
	double complex dccos, z1, z2
	double precision dummy(2), a, b, c, d
	equivalence (a,z2,dummy(1)), (b,dummy(2))

	z2 = z1
	c = dexp(b)
	d = 1.0d0/c
	b = dsin(a)*(d-c)/2.0d0
	a = dcos(a)*(c+d)/2.0d0
	dccos = z2
	return
	end


*     MB04FD EXAMPLE PROGRAM TEXT
*     Copyright (c) 2002-2020 NICONET e.V.
*
*     .. Parameters ..
      INTEGER            NIN, NOUT
      PARAMETER          ( NIN = 5, NOUT = 6 )
      INTEGER            NMAX
      PARAMETER          ( NMAX = 50 )
      INTEGER            LDA, LDB, LDDE, LDFG, LDQ, LDWORK
      PARAMETER          ( LDA  = NMAX/2, LDB = NMAX/2, LDDE = NMAX/2,
     $                     LDFG = NMAX/2, LDQ = NMAX, LDWORK =
     $                     3*NMAX*NMAX/4 )
*
*     .. Local Scalars ..
      CHARACTER          COMPQ, JOB
      INTEGER            I, INFO, J, N
*
*     .. Local Arrays ..
      DOUBLE PRECISION   A( LDA, NMAX/2 ),  ALPHAI( NMAX/2 ),
     $                   ALPHAR( NMAX/2 ),  B( LDB, NMAX/2 ),
     $                   BETA( NMAX/2 ),  DE( LDDE, NMAX/2+1 ),
     $                   DWORK( LDWORK ), FG( LDFG, NMAX/2+1 ),
     $                   Q( LDQ, NMAX )
      INTEGER            IWORK( NMAX/2+1 )
*
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
*
*     .. External Subroutines ..
      EXTERNAL           MB04FD
*
*     .. Intrinsic Functions ..
      INTRINSIC          MOD
*
*     .. Executable statements ..
*
      WRITE( NOUT, FMT = 99999 )
*
*     Skip first line in data file.
*
      READ( NIN, FMT = * )
      READ( NIN, FMT = * ) JOB, COMPQ, N
      READ( NIN, FMT = * ) ( (  A( I, J ), J = 1, N/2 ),   I = 1, N/2 )
      READ( NIN, FMT = * ) ( ( DE( I, J ), J = 1, N/2+1 ), I = 1, N/2 )
      READ( NIN, FMT = * ) ( (  B( I, J ), J = 1, N/2 ),   I = 1, N/2 )
      READ( NIN, FMT = * ) ( ( FG( I, J ), J = 1, N/2+1 ), I = 1, N/2 )
      IF( LSAME( COMPQ, 'U' ) )
     $   READ( NIN, FMT = * ) ( ( Q( I, J ), J = 1, N ), I = 1, N )
      IF( N.LT.0 .OR. N.GT.NMAX .OR. MOD( N, 2 ).NE.0 ) THEN
         WRITE( NOUT, FMT = 99998 ) N
      ELSE
*
*        Test of MB04FD.
*
         CALL MB04FD( JOB, COMPQ, N, A, LDA, DE, LDDE, B, LDB, FG, LDFG,
     $                Q, LDQ, ALPHAR, ALPHAI, BETA, IWORK, DWORK,
     $                LDWORK, INFO )
         IF( INFO.NE.0 ) THEN
            WRITE( NOUT, FMT = 99997 ) INFO
         ELSE
            IF( LSAME( JOB, 'T' ) ) THEN
               WRITE( NOUT, FMT = 99996 )
               DO 10 I = 1, N/2
                  WRITE( NOUT, FMT = 99995 ) ( A( I, J ), J = 1, N/2 )
   10          CONTINUE
            END IF
            WRITE( NOUT, FMT = 99994 )
            DO 20 I = 1, N/2
               WRITE( NOUT, FMT = 99995 ) ( DE( I, J ), J = 1, N/2+1 )
   20       CONTINUE
            IF( LSAME( JOB, 'T' ) ) THEN
               WRITE( NOUT, FMT = 99993 )
               DO 30 I = 1, N/2
                  WRITE( NOUT, FMT = 99995 ) ( B( I, J ), J = 1, N/2 )
   30          CONTINUE
            END IF
            WRITE( NOUT, FMT = 99992 )
            DO 40 I = 1, N/2
               WRITE( NOUT, FMT = 99995 ) ( FG( I, J ), J = 1, N/2+1 )
   40       CONTINUE
            IF( .NOT.LSAME( COMPQ, 'N' ) ) THEN
               WRITE( NOUT, FMT = 99991 )
               DO 50 I = 1, N
                  WRITE( NOUT, FMT = 99995 ) ( Q( I, J ), J = 1, N )
   50          CONTINUE
            END IF
            WRITE( NOUT, FMT = 99990 )
            WRITE( NOUT, FMT = 99995 ) ( ALPHAR(I), I = 1, N/2 )
            WRITE( NOUT, FMT = 99995 ) ( ALPHAI(I), I = 1, N/2 )
            WRITE( NOUT, FMT = 99995 ) (   BETA(I), I = 1, N/2 )
         END IF
      END IF
      STOP
99999 FORMAT ( 'MB04FD EXAMPLE PROGRAM RESULTS', 1X )
99998 FORMAT ( 'N is out of range.', /, 'N = ', I5 )
99997 FORMAT ( 'INFO on exit from MB04FD = ', I2 )
99996 FORMAT (/' The transformed matrix A is' )
99995 FORMAT ( 51( 1X, F8.4 ) )
99994 FORMAT (/' The transformed matrix DE is' )
99993 FORMAT (/' The transformed matrix B is' )
99992 FORMAT (/' The transformed matrix FG is' )
99991 FORMAT (/' The transformed matrix Q is ' )
99990 FORMAT (/' The real, imaginary, and beta parts of eigenvalues are'
     $       )
      END


        subroutine iddp_asvd(lw,eps,m,n,a,winit,krank,iu,iv,is,w,ier)
c
c       constructs a rank-krank SVD  U Sigma V^T  approximating a
c       to precision eps, where U is an m x krank matrix whose
c       columns are orthonormal, V is an n x krank matrix whose
c       columns are orthonormal, and Sigma is a diagonal krank x krank
c       matrix whose entries are all nonnegative.
c       The entries of U are stored in w, starting at w(iu);
c       the entries of V are stored in w, starting at w(iv).
c       The diagonal entries of Sigma are stored in w,
c       starting at w(is). This routine uses a randomized algorithm.
c
c       input:
c       lw -- maximum usable length (in real*8 elements)
c             of the array w
c       eps -- precision of the desired approximation
c       m -- number of rows in a
c       n -- number of columns in a
c       a -- matrix to be approximated; the present routine does not
c            alter a
c       winit -- initialization array that has been constructed
c                by routine idd_frmi
c
c       output:
c       krank -- rank of the SVD constructed
c       iu -- index in w of the first entry of the matrix
c             of orthonormal left singular vectors of a
c       iv -- index in w of the first entry of the matrix
c             of orthonormal right singular vectors of a
c       is -- index in w of the first entry of the array
c             of singular values of a
c       w -- array containing the singular values and singular vectors
c            of a; w doubles as a work array, and so must be at least
c            max( (krank+1)*(3*m+5*n+1)+25*krank**2, (2*n+1)*(n2+1) )
c            real*8 elements long, where n2 is the greatest integer
c            less than or equal to m, such that n2 is
c            a positive integer power of two; krank is the rank output
c            by this routine
c       ier -- 0 when the routine terminates successfully;
c              -1000 when lw is too small;
c              other nonzero values when idd_id2svd bombs
c
c       _N.B._: w must be at least
c               max( (krank+1)*(3*m+5*n+1)+25*krank^2, (2*n+1)*(n2+1) )
c               real*8 elements long, where n2 is the greatest integer
c               less than or equal to m, such that n2 is
c               a positive integer power of two;
c               krank is the rank output by this routine.
c               Also, the algorithm used by this routine is randomized.
c
        implicit none
        integer m,n,krank,lw,ilist,llist,iproj,lproj,icol,lcol,
     1          iwork,lwork,k,ier,lw2,iu,iv,is,iui,ivi,isi,lu,lv,ls
        real*8 eps,a(m,n),winit(17*m+70),w(*)
c
c
c       Allocate memory in w.
c
        lw2 = 0
c
        ilist = lw2+1
        llist = n
        lw2 = lw2+llist
c
        iproj = lw2+1
c
c
c       ID a.
c
        call iddp_aid(eps,m,n,a,winit,krank,w(ilist),w(iproj))
c
c
        if(krank .gt. 0) then
c
c
c         Allocate more memory in w.
c
          lproj = krank*(n-krank)
          lw2 = lw2+lproj
c
          icol = lw2+1
          lcol = m*krank
          lw2 = lw2+lcol
c
          iui = lw2+1
          lu = m*krank
          lw2 = lw2+lu
c
          ivi = lw2+1
          lv = n*krank
          lw2 = lw2+lv
c
          isi = lw2+1
          ls = krank
          lw2 = lw2+ls
c
          iwork = lw2+1
          lwork = (krank+1)*(m+3*n)+26*krank**2
          lw2 = lw2+lwork
c
c
          if(lw .lt. lw2) then
            ier = -1000
            return
          endif
c
c
          call iddp_asvd0(m,n,a,krank,w(ilist),w(iproj),
     1                    w(iui),w(ivi),w(isi),ier,w(icol),w(iwork))
          if(ier .ne. 0) return
c
c
          iu = 1
          iv = iu+lu
          is = iv+lv
c
c
c         Copy the singular values and singular vectors
c         into their proper locations.
c
          do k = 1,lu
            w(iu+k-1) = w(iui+k-1)
          enddo ! k
c
          do k = 1,lv
            w(iv+k-1) = w(ivi+k-1)
          enddo ! k
c
          do k = 1,ls
            w(is+k-1) = w(isi+k-1)
          enddo ! k
c
c
        endif ! krank .gt. 0
c
c
        return
        end
c
c
c
c


#include "scalar.fpp"

!=====================================================================!
! Module that contains functions related to spring mass damper system
!
! Author: Komahan Boopathy (komahan@gatech.edu)
!=====================================================================!

module pendulum_functions_class

  use function_class, only  : abstract_function

  implicit none

  private
  public :: time_period

  !-------------------------------------------------------------------!
  ! Type that models any physical phenomenon
  !-------------------------------------------------------------------!

  type, extends(abstract_function) :: time_period

     type(scalar) :: m = 1.0d0
     type(scalar) :: g = 10.0d0

     type(scalar) :: pi = 3.1415926535897932384626433832d0

   contains

     procedure :: getFunctionValue
     procedure :: addFuncDVSens
     procedure :: addFuncSVSens
     procedure :: addDFdU
     procedure :: addDFdUDot
     procedure :: addDFdUDDot

  end type time_period

contains

  !-------------------------------------------------------------------!
  ! Evaluate the kinetic energy for the supplied state and design
  ! variables KE = 0.5 m v^2
  ! -------------------------------------------------------------------!

  pure subroutine getFunctionValue(this, f, time, x, u, udot, uddot)

    class(time_period), intent(inout) :: this
    type(scalar), intent(inout) :: f
    type(scalar), intent(in), dimension(:) :: x, u, udot, uddot
    real(dp), intent(in) :: time

    f = (2.0d0*this %pi) * sqrt(x(1)/this%g)

  end subroutine getFunctionValue

  !-------------------------------------------------------------------!
  ! Evaluate  dF/dX
  ! -------------------------------------------------------------------!

  subroutine addFuncDVSens(this, res, scale, time, x, u, udot, uddot)

    class(time_period) :: this
    type(scalar), intent(inout), dimension(:) :: res
    type(scalar), intent(in), dimension(:)    :: x, u, udot, uddot
    real(dp), intent(in) :: time
    type(scalar) :: scale

    !res(1) = res(1) - scale*sqrt(this%g/(x(1)*x(1)*x(1)))/(4.0d0*this%pi)
    res(1) = res(1) + scale*this%pi/sqrt(this%g*x(1))

  end subroutine addFuncDVSens

  !-------------------------------------------------------------------!
  ! Evaluate alpha dF/dU + beta dF/dUDot + gamma dF/dUDDOT
  ! -------------------------------------------------------------------!

  subroutine addFuncSVSens(this, res, alpha, beta, gamma, &
       & time, x, u, udot, uddot)

    class(time_period) :: this
    type(scalar), intent(inout), dimension(:) :: res
    real(dp), intent(in) :: time
    type(scalar), intent(in), dimension(:)    :: x, u, udot, uddot
    type(scalar), intent(in) :: alpha, beta, gamma

  end subroutine addFuncSVSens

  !-------------------------------------------------------------------!
  ! Evaluate  dF/dU
  ! -------------------------------------------------------------------!

  subroutine addDfdU(this, res, scale, time, x, u, udot, uddot)

    class(time_period) :: this
    type(scalar), intent(inout), dimension(:) :: res
    real(dp), intent(in) :: time
    type(scalar), intent(in), dimension(:)    :: x, u, udot, uddot
    type(scalar) :: scale

    stop

  end subroutine addDfdU

  !-------------------------------------------------------------------!
  ! Evaluate  dF/dUdot
  ! -------------------------------------------------------------------!

  subroutine addDfdUDot(this, res, scale, time, x, u, udot, uddot)

    class(time_period) :: this
    type(scalar), intent(inout), dimension(:) :: res
    type(scalar), intent(in), dimension(:)    :: x, u, udot, uddot
    real(dp), intent(in) :: time
    type(scalar) :: scale

    stop

  end subroutine addDFdUDot

  !-------------------------------------------------------------------!
  ! Evaluate  dF/dUDDot
  ! -------------------------------------------------------------------!

  subroutine addDfdUDDot(this, res, scale, time, x, u, udot, uddot)

    class(time_period) :: this
    type(scalar), intent(inout), dimension(:) :: res
    type(scalar), intent(in), dimension(:)    :: x, u, udot, uddot
    real(dp), intent(in)                      :: time
    type(scalar) :: scale

    stop

  end subroutine addDfdUDDot

end module pendulum_functions_class


SUBROUTINE sumImageWithinCircle(ndiv, nx, ny, xmin, xmax, ymin, ymax, r, cx, cy, img, tot)
    ! Import standard modules ...
    USE ISO_C_BINDING

    IMPLICIT NONE

    ! Declare inputs/outputs ...
    INTEGER(kind = C_LONG_LONG), INTENT(in)                                     :: ndiv
    INTEGER(kind = C_LONG_LONG), INTENT(in)                                     :: nx
    INTEGER(kind = C_LONG_LONG), INTENT(in)                                     :: ny
    REAL(kind = C_DOUBLE), INTENT(in)                                           :: xmin
    REAL(kind = C_DOUBLE), INTENT(in)                                           :: xmax
    REAL(kind = C_DOUBLE), INTENT(in)                                           :: ymin
    REAL(kind = C_DOUBLE), INTENT(in)                                           :: ymax
    REAL(kind = C_DOUBLE), INTENT(in)                                           :: r
    REAL(kind = C_DOUBLE), INTENT(in)                                           :: cx
    REAL(kind = C_DOUBLE), INTENT(in)                                           :: cy
    REAL(kind = C_DOUBLE), DIMENSION(ny, nx), INTENT(in)                        :: img
    REAL(kind = C_DOUBLE), INTENT(out)                                          :: tot

    ! Declare internal variables ...
    INTEGER(kind = C_LONG_LONG)                                                 :: ix
    INTEGER(kind = C_LONG_LONG)                                                 :: iy
    REAL(kind = C_DOUBLE)                                                       :: dx
    REAL(kind = C_DOUBLE)                                                       :: dy
    REAL(kind = C_DOUBLE)                                                       :: frac
    REAL(kind = C_DOUBLE), DIMENSION(nx + 1)                                    :: xaxis
    REAL(kind = C_DOUBLE), DIMENSION(ny + 1)                                    :: yaxis
    REAL(kind = C_DOUBLE), DIMENSION(ny + 1, nx + 1)                            :: dist

    ! Calculate size of pixels ...
    dx = (xmax - xmin) / REAL(nx, kind = C_DOUBLE)
    dy = (ymax - ymin) / REAL(ny, kind = C_DOUBLE)

    ! Initialize total ...
    tot = 0.0e0_C_DOUBLE

    !$omp parallel                                                              &
    !$omp default(none)                                                         &
    !$omp private(frac)                                                         &
    !$omp private(ix)                                                           &
    !$omp private(iy)                                                           &
    !$omp shared(cx)                                                            &
    !$omp shared(cy)                                                            &
    !$omp shared(dist)                                                          &
    !$omp shared(dx)                                                            &
    !$omp shared(dy)                                                            &
    !$omp shared(img)                                                           &
    !$omp shared(ndiv)                                                          &
    !$omp shared(nx)                                                            &
    !$omp shared(ny)                                                            &
    !$omp shared(r)                                                             &
    !$omp shared(xaxis)                                                         &
    !$omp shared(xmin)                                                          &
    !$omp shared(yaxis)                                                         &
    !$omp shared(ymin)                                                          &
    !$omp reduction(+:tot)
        !$omp do                                                                &
        !$omp schedule(dynamic)
            ! Create nodes relative to the centre of the circle ...
            DO ix = 1_C_LONG_LONG, nx + 1_C_LONG_LONG
                xaxis(ix) = xmin + REAL(ix - 1_C_LONG_LONG, kind = C_DOUBLE) * dx - cx
            END DO
        !$omp end do

        !$omp do                                                                &
        !$omp schedule(dynamic)
            ! Create nodes relative to the centre of the circle ...
            DO iy = 1_C_LONG_LONG, ny + 1_C_LONG_LONG
                yaxis(iy) = ymin + REAL(iy - 1_C_LONG_LONG, kind = C_DOUBLE) * dy - cy
            END DO
        !$omp end do

        !$omp do                                                                &
        !$omp schedule(dynamic)
            ! Find out the distance of each node to the centre of the circle ...
            DO ix = 1_C_LONG_LONG, nx + 1_C_LONG_LONG
                DO iy = 1_C_LONG_LONG, ny + 1_C_LONG_LONG
                    dist(iy, ix) = HYPOT(xaxis(ix), yaxis(iy))
                END DO
            END DO
        !$omp end do

        !$omp do                                                                &
        !$omp schedule(dynamic)
            ! Loop over x-axis ...
            DO ix = 1_C_LONG_LONG, nx
                ! Loop over y-axis ...
                DO iy = 1_C_LONG_LONG, ny
                    ! Skip this pixel if it is empty ...
                    IF(img(iy, ix) == 0.0e0_C_DOUBLE)THEN
                        CYCLE
                    END IF

                    ! Skip this pixel if it is all outside of the circle ...
                    IF(ALL(dist(iy:iy + 1_C_LONG_LONG, ix:ix + 1_C_LONG_LONG) >= r))THEN
                        CYCLE
                    END IF

                    ! Check if this pixel is entirely within the circle or if it
                    ! straddles the circumference ...
                    IF(ALL(dist(iy:iy + 1_C_LONG_LONG, ix:ix + 1_C_LONG_LONG) <= r))THEN
                        ! Add all of the value to total ...
                        tot = tot + img(iy, ix)
                    ELSE
                        ! Add part of the value to total ...
                        CALL findFractionOfPixelWithinCircle(                   &
                            ndiv = ndiv,                                        &
                            xmin = xaxis(ix),                                   &
                            xmax = xaxis(ix + 1_C_LONG_LONG),                   &
                            ymin = yaxis(iy),                                   &
                            ymax = yaxis(iy + 1_C_LONG_LONG),                   &
                            r = r,                                              &
                            cx = 0.0e0_C_DOUBLE,                                &
                            cy = 0.0e0_C_DOUBLE,                                &
                            frac = frac                                         &
                        )
                        tot = tot + img(iy, ix) * frac
                    END IF
                END DO
            END DO
        !$omp end do
    !$omp end parallel
END SUBROUTINE sumImageWithinCircle


c=======================================================================
c
c     subroutine COVEXP
c
c     (Empirical) Exponentially Weighted Covariance matrix
c
c                       n
c                       ---
c     cov(i,j) = (1-L)  \    L^(k-1)*[X(k,i) - rho(i)] * [X(k,j) - rho(j)]
c                       /
c                       ---
c                       k=1
c
c                  n
c              1  ---
c     rho(j) = -  \   X(i,j) for j = 1,...,p
c              n  /
c                 ---
c                 i=1
c
c     L : exponential parameter
c
c-----------------------------------------------------------------------
      SUBROUTINE covexp ( n, p, X, lambda, dwork, cov, info)
c-----------------------------------------------------------------------
c
c     INPUT
c            n      : number of value(s) (n > 1)                 integer
c            p      : number of asset(s) (p >= 1)                integer
c            X      : value(s) (n*p)                              double
c            lambda : exponential parameter                       double
c
c     WORKSPACE
c            dwork  : vector( p*(n + 1) )                         double
c
c     OUTPUT
c            cov    : exp. weighted covariance matrix (p*p)       double
c            info   : diagnostic argument                        integer
c
c     CALL
c            MCM    : mean of each column of a vectorized full matrix
c            EMPCOV : empirical covariance matrix
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     arguments i/o
      INTEGER n, p, info
      DOUBLE PRECISION lambda, X(n, *), cov(p, *)
c
c     workspaces
      DOUBLE PRECISION dwork(*)
c
c     local variables
      integer pdwork, pdrho
c
c     external subroutines
      EXTERNAL MCM, empcovexp
c
c-----------------------------------------------------------------------
c
c     initialisations
      info = 0
c
c     pointers for double precision work space  : dwork
c     -------------------------------------------------
      pdwork = 1
c     pdwork : pointer for relatives rents, so ( n*p ) more
      pdrho = pdwork + ( n*p )
c     pdrho  : pointer for mean return rho (p)
c
c     Total size of dwork array = p*(n + 1)
c
c----------------------------------------------------------------------
c
c     means return(s)
      CALL MCM ( n, p, X, dwork(pdrho) )
c
c     empirical exp. weighted covariance matrix
      CALL empcovexp ( n, p, lambda, X, dwork(pdrho), dwork(pdwork),
     &                 cov, info )
c
      RETURN
      END


module velocity_verlet
  ! module to solve Newton mechanics for a N-body planetary system
  ! using velocity verlet integration
  implicit none

  public :: verlet_n

  private :: compute_new_position, compute_half_step_velocity, &
       compute_new_velocity

contains

  subroutine verlet_n(bodies,nbodies,n,dt)
    ! solver for a N-body system
    use body
    implicit none
    integer, intent(in) :: nbodies
    type(planetary_body), dimension(nbodies), intent(inout) :: bodies
    integer, intent(in) :: n
    real(8), intent(in) :: dt
    real(8), dimension(nbodies) :: m
    real(8), dimension(nbodies,3) :: vh, r, newforces
    real(8), dimension(3) :: pos, vel, f
    integer :: z
    ! half step velocities and new positions
    do z=1,nbodies
       call get_body_fields(bodies(z),n,pos,vel,f,m(z))
       vh(z,:) = compute_half_step_velocity(vel,f,m(z),dt)
       r(z,:) = compute_new_position(pos,vh(z,:),dt)
    end do
    ! new forces and velocities
    newforces = calc_forces(m,r,nbodies)
    do z=1,nbodies
       bodies(z)%f(n+1,:) = newforces(z,:)
       bodies(z)%vel(n+1,:) = compute_new_velocity(bodies(z)%f(n+1,:),m(z),vh(z,:),dt)
       bodies(z)%pos(n+1,:) = r(z,:)
    end do
  end subroutine verlet_n

  function compute_half_step_velocity(v0,f0,m,dt) result(v_half)
    ! Velocity at half time step from initial
    ! velocity and force
    implicit none
    real(8), dimension(3), intent(in) :: v0, f0
    real(8), intent(in) :: m, dt
    real(8), dimension(3) :: v_half
    v_half = v0 + 0.5*f0/m*dt
  end function compute_half_step_velocity

  function compute_new_position(r0,v_half,dt) result(r1)
    ! New position from initial position
    ! and half step velocity
    implicit none
    real(8), dimension(3), intent(in) :: r0, v_half
    real(8), intent(in) :: dt
    real(8), dimension(3) :: r1
    r1 = r0 + v_half*dt
  end function compute_new_position

  function compute_new_velocity(f1,m,v_half,dt) result(v1)
    ! Velocity at full step
    real(8), dimension(3), intent(in) :: f1, v_half
    real(8), intent(in) :: m, dt
    real(8), dimension(3) :: v1
    v1 = v_half + 0.5*f1/m*dt
  end function compute_new_velocity

end module velocity_verlet


      subroutine ddasco (a, lda, neq, wt, delta)
c Copyright (c) 2006, Math a la Carte, Inc.
c>> 2006-05-20 ddasco Hanson Fix bug for computing norm
c>> 2006-05-18 ddasco Hanson return residual norm value in delta(neq+1)
c>> 2006-04-26 ddasco Hanson installed pivoting and test for def rank
c>> 2003-03-06 ddasco Hanson accounted for use of reciprocal weights
c>> 2002-03-19 ddasco Krogh  Added sqrt on the weights.
c>> 2001-11-23 ddasco Krogh  Changed many names per library conventions.
c>> 2001-11-04 ddasco Krogh  Fixes for F77 and conversion to single
c>> 2001-11-01 ddasco Hanson Provide code to Math a la Carte.
c--D replaces "?":?dasco, ?nrm2, ?dot, ?scal, ?rotg, ?rot, ?copy, ?swap,
c--& ?axpy, i?amax
c      implicit none
c     Compute a single Newton step:
c     Solve an underdetermined system.
c     The number of constraint rows is NC=LDD-NEQ.
c
c     On input the first neq entries of delta contain the reciprocals of
C     the weights.  The next entries contain the constraint residuals.
c     On the other hand, the matrix, a, contains only the partials for
C     the constraint rows.
c
      external idamax
      integer idamax
      integer    i, imax, j, lda, neq, nc, nrank
      double precision a(lda,*), wt(*), delta(*), c, s, z, tn
      external ddot, dnrm2
      double precision ddot, dnrm2
      nc  = lda - neq
      if (nc .le. 0) return
c     Use weights derived from error norm.
      do 10  j=1, neq
         call dscal (nc, sqrt(wt(j)), a(1,j), 1)
   10 continue

c     Triangularize constraint matrix using right-handed
c     plane rotations.  These are stored in the entry
c     eliminated and then reconstructed.
      do 30  i=1, min(neq,nc - 1)
c     Do row pivoting to maximize diagonal term (in L2 norm)
c     so that smallest pivots are at the SE part of the lower triangle.
         do 25  j=i,nc
            delta(j)=ddot(neq-i+1,a(j,i),lda,a(j,i),lda)
   25    continue
         imax = idamax(nc-i+1,delta(i),1)
c     If found a row with larger L2 norm, interchange this row
c     and the right-hand side.
         if(imax .gt. 1) then
            call dswap(neq,a(i,1),lda,a(i+imax-1,1),lda)
            call dswap(1, delta(neq+i),1,delta(neq+i+imax-1),1)
         end if
c     Eliminate the non-zero entries and store the rotation
c     in the place eliminated.  These are used to compute
c     the least-distance move back to the constraints.
c     Note that this uses an often overlooked feature of ?rotg.
         do 20  j=i + 1, neq
            call drotg (a(i,i), a(i,j), c, s)
            call drot (nc-i, a(i+1,i), 1, a(i+1,j), 1, c, s)
   20    continue
   30 continue
c     This is a special elimination step for the last row.
      do 40  j=nc + 1, neq
         call drotg (a(nc,nc), a(nc,j), c, s)
   40 continue
c     System is now lower triangular.  Forward solve step.
c     The right-hand side for this system is in DELTA(NEQ+I),
c     I=1,NC.
      DELTA(1)=0.D0
      CALL DCOPY(NEQ, DELTA, 0, DELTA, 1)
      nrank = min(nc,neq)
      do 50  i=1, nc
c     Due to the weighting used, a small number is .le. 1 for
c     a diagonal term.
          if(abs(a(i,i)) .le. 1.d0) then
            nrank=i-1
            go to 55
          end if
         delta(i) = delta(neq+i)
   50 continue
   55 continue
c Solve for the update.
      do 60  i=1, nrank - 1
         z = delta(i)/a(i,i)
         call daxpy (nrank-i, -z, a(i+1,i), 1, delta(i+1), 1)
         delta(i)=z
   60 continue
      delta(nrank) = delta(nrank)/a(nrank,nrank)
c     Compute residuals on those equations not solved.
c     They should be consistent but may not due to
c     modelling errors or large numerical errors.
      do 70 i=nrank+1,nc
         delta(neq+i)=delta(neq+i)-ddot(nrank,a(i,1),lda,delta,1)
   70 continue
c     Now apply the Givens transformations in reverse order.
c     They are stored in one entry form so the values are
c     reconstructed as in the 1979 Level 1 BLAS paper or
c     LINPACK, page A.5.
      do 90  i=min(neq,nc), 1, -1
         do 80  j=neq, i + 1, -1
            z = a(i,j)
            if (abs(z) .lt. 1.D0) then
               s = z
               c = sqrt((1.D0-s)*(1.D0+s))
            else if (abs(z) .gt. 1.D0) then
               c = 1.D0/z
               s = sqrt((1.D0-c)*(1.D0+c))
            else if (z .eq. 1.D0) then
               c = 0.D0
               s = 1.D0
            end if
            z        = c*delta(i) - s*delta(j)
            delta(j) = s*delta(i) + c*delta(j)
            delta(i) = z
   80    continue
   90 continue

c     Compute the amount of inconsistency in the constraint
c     equations.  Return a factor that allows for testing.
      if(nrank .lt. nc) then
        tn = dnrm2(nc-nrank, delta(neq+nrank+1),1)
      else
        tn = 0.d0
      end if
c     Store the value in delta(neq+1) for testing after returning.
c     Make that error test based on the size of delta(neq+1).
      delta(neq+1) = tn
c     Apply weights used for step.
C     They were also input in DELTA(:).
      do 100  j=1, neq
          delta(j) = delta(j)*sqrt(wt(j))
  100 continue
      return
      end


      Subroutine dirpopol(xq,xu,dirpol)
c
c This subroutine computes the plane of polarization

      real xu,xq,dirpol,pi

      pi=acos(-1.)

      if (abs(xq).lt.0.00001)then
        if (xu.gt.0.)then
          dirpol=45.0
        else
          dirpol=-45.0
        endif
        return
      endif

      if (abs(xu).lt.0.00001)then
        if (xq.gt.0.)then
          dirpol=0.0
        else
          dirpol=90.0
        endif
        return
      endif

      if (xq.gt.0.)then
        dirpol=90./pi*atan(xu/xq)
        return
      endif

      if (xu.gt.0.)then
        dirpol=90.+90./pi*atan(xu/xq)
      else
        dirpol=-90.+90./pi*atan(xu/xq)
      endif

      return
      end