!*****************************************************************************************
!>
!  COBYLA: **C**onstrained **O**ptimization **BY** **L**inear **A**pproximations.
!
!  Minimize an objective function F([X1,X2,...,XN]) subject to M inequality constraints.
!
!# References
!  * "[A direct search optimization method that models the objective and constraint
!    functions by linear interpolation](http://link.springer.com/chapter/10.1007/978-94-015-8330-5_4)",
!    *Advances in Optimization and Numerical Analysis*
!    (eds. Susana Gomez and Jean-Pierre Hennart), Kluwer Academic Publishers (1994).
!
!# History
!  * Mike Powell (May 7th, 1992) -- There are no restrictions on the use of the
!    software, nor do I offer any guarantees of success.
!  * Jacob Williams, July 2015 : refactoring of the code into modern Fortran.
!
!@note There is a need for a linear programming problem to be solved subject to a
!      Euclidean norm trust region constraint. Therefore SUBROUTINE TRSTLP is provided,
!      but you may have some software that you prefer to use instead.

module cobyla_module

    use kind_module, only: wp

    private

    abstract interface
        subroutine func (n, m, x, f, con)  !! calcfc interface
            import :: wp
            implicit none
            integer,intent(in)                :: n
            integer,intent(in)                :: m
            real(wp),dimension(*),intent(in)  :: x
            real(wp),intent(out)              :: f
            real(wp),dimension(*),intent(out) :: con
        end subroutine func
    end interface

    public :: cobyla
    public :: cobyla_test

contains

!*****************************************************************************************
!>
!  This subroutine minimizes an objective function F(X) subject to M
!  inequality constraints on X, where X is a vector of variables that has
!  N components. The algorithm employs linear approximations to the
!  objective and constraint functions, the approximations being formed by
!  linear interpolation at N+1 points in the space of the variables.
!  We regard these interpolation points as vertices of a simplex. The
!  parameter RHO controls the size of the simplex and it is reduced
!  automatically from RHOBEG to RHOEND. For each RHO the subroutine tries
!  to achieve a good vector of variables for the current size, and then
!  RHO is reduced until the value RHOEND is reached. Therefore RHOBEG and
!  RHOEND should be set to reasonable initial changes to and the required
!  accuracy in the variables respectively, but this accuracy should be
!  viewed as a subject for experimentation because it is not guaranteed.
!
!  The subroutine has an advantage over many of its competitors, however,
!  which is that it treats each constraint individually when calculating
!  a change to the variables, instead of lumping the constraints together
!  into a single penalty function.

    subroutine cobyla (n, m, x, rhobeg, rhoend, iprint, maxfun, calcfc)

        implicit none

        integer,intent(in)                  :: n      !! number of variables
        integer,intent(in)                  :: m      !! number of inequality constraints
        real(wp),dimension(*),intent(inout) :: x      !! Initial values of the variables must be set in X(1),X(2),...,X(N).
                                                      !! On return they will be changed to the solution.
        real(wp),intent(in)                 :: rhobeg !! reasonable initial change to variables (see description of RHO)
        real(wp),intent(in)                 :: rhoend !! required accuracy (see description of RHO)
        integer,intent(in)                  :: iprint !! IPRINT should be set to 0, 1, 2 or 3, which controls the amount of
                                                      !! printing during the calculation. Specifically, there is no output if
                                                      !! IPRINT=0 and there is output only at the end of the calculation if
                                                      !! IPRINT=1. Otherwise each new value of RHO and SIGMA is printed.
                                                      !! Further, the vector of variables and some function information are
                                                      !! given either when RHO is reduced or when each new value of F(X) is
                                                      !! computed in the cases IPRINT=2 or IPRINT=3 respectively. Here SIGMA
                                                      !! is a penalty parameter, it being assumed that a change to X is an
                                                      !! improvement if it reduces the merit function
                                                      !!     F(X)+SIGMA*MAX(0.0,-C1(X),-C2(X),...,-CM(X)),
                                                      !! where C1,C2,...,CM denote the constraint functions that should become
                                                      !! nonnegative eventually, at least to the precision of RHOEND. In the
                                                      !! printed output the displayed term that is multiplied by SIGMA is
                                                      !! called MAXCV, which stands for 'MAXimum Constraint Violation'.
        integer,intent(inout)               :: maxfun !! MAXFUN is an integer variable that must be set by the user to a
                                                      !! limit on the number of calls of CALCFC.
                                                      !! The value of MAXFUN will be altered to the number of calls
                                                      !! of CALCFC that are made.
        procedure (func)                    :: calcfc !! In order to define the objective and constraint functions, we require
                                                      !! a subroutine that has the name and arguments
                                                      !!     SUBROUTINE CALCFC (N,M,X,F,CON)
                                                      !!     DIMENSION X(*),CON(*)
                                                      !! The values of N and M are fixed and have been defined already, while
                                                      !! X is now the current vector of variables. The subroutine should return
                                                      !! the objective and constraint functions at X in F and CON(1),CON(2),
                                                      !! ...,CON(M). Note that we are trying to adjust X so that F(X) is as
                                                      !! small as possible subject to the constraint functions being nonnegative.

        integer,dimension(:),allocatable  :: iact
        real(wp),dimension(:),allocatable :: w
        integer :: mpp,icon,isim,isimi,idatm,ia,ivsig,iveta,isigb,idx,iwork

        !W and IACT provide real and integer arrays that are used as working space.
        allocate(w(N*(3*N+2*M+11)+4*M+6))
        allocate(iact(M+1))

        ! Partition the working space array W to provide the storage that is needed
        ! for the main calculation.

        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 (n, m, mpp, x, rhobeg, rhoend, iprint, maxfun, w(icon), w(isim), &
                     w(isimi), w(idatm), w(ia), w(ivsig), w(iveta), w(isigb), w(idx), &
                     w(iwork), iact, calcfc)

        deallocate(iact)
        deallocate(w)

    end subroutine cobyla
!*****************************************************************************************

    subroutine cobylb (n, m, mpp, x, rhobeg, rhoend, iprint, maxfun, con, sim, simi, &
                       datmat, a, vsig, veta, sigbar, dx, w, iact, calcfc)

        implicit real (wp) (a-h, o-z)

        dimension x (*), con (*), sim (n,*), simi (n,*), datmat (mpp,*), a (n,*), vsig(*),&
         veta (*), sigbar (*), dx (*), w (*), iact (*)
        procedure (func) :: calcfc
!
!     Set the initial values of some parameters. The last column of SIM holds
!     the optimal vertex of the current simplex, and the preceding N columns
!     hold the displacements from the optimal vertex to the other vertices.
!     Further, SIMI holds the inverse of the matrix that is contained in the
!     first N columns of SIM.
!
        iptem = min (n, 5)
        iptemp = iptem + 1
        np = n + 1
        mp = m + 1
        alpha = 0.25_wp
        beta = 2.1_wp
        gamma = 0.5_wp
        delta = 1.1_wp
        rho = rhobeg
        parmu = 0.0_wp
        if (iprint >= 2) print 10, rho
10      format (/ 3 x, 'The initial value of RHO is', 1 pe13.6, 2 x,&
        'and PARMU is set to zero.')
        nfvals = 0
        temp = 1.0_wp / rho
        do i = 1, n
            sim (i, np) = x (i)
            do j = 1, n
                sim (i, j) = 0.0_wp
                simi (i, j) = 0.0_wp
            end do
            sim (i, i) = rho
            simi (i, i) = temp
        end do
        jdrop = np
        ibrnch = 0
!
!     Make the next call of the user-supplied subroutine CALCFC. These
!     instructions are also used for calling CALCFC during the iterations of
!     the algorithm.
!
40      if (nfvals >= maxfun .and. nfvals > 0) then
            if (iprint >= 1) print 50
50          format (/ 3 x, 'Return from subroutine COBYLA because the ',&
            'MAXFUN limit has been reached.')
            go to 600
        end if
        nfvals = nfvals + 1
        call calcfc (n, m, x, f, con)
        resmax = 0.0_wp
        if (m > 0) then
            do k = 1, m
                resmax = max (resmax,-con(k))
            end do
        end if
        if (nfvals == iprint-1 .or. iprint == 3) then
            print 70, nfvals, f, resmax, (x(i), i=1, iptem)
70          format (/ 3 x, 'NFVALS =', i5, 3 x, 'F =', 1 pe13.6, 4 x, 'MAXCV =', 1 pe13.6 &
           & / 3 x, 'X =', 1 pe13.6, 1 p4e15.6)
            if (iptem < n) print 80, (x(i), i=iptemp, n)
80          format (1 pe19.6, 1 p4e15.6)
        end if
        con (mp) = f
        con (mpp) = resmax
        if (ibrnch == 1) go to 440
!
!     Set the recently calculated function values in a column of DATMAT. This
!     array has a column for each vertex of the current simplex, the entries of
!     each column being the values of the constraint functions (if any)
!     followed by the objective function and the greatest constraint violation
!     at the vertex.
!
        do k = 1, mpp
            datmat (k, jdrop) = con (k)
        end do
        if (nfvals > np) go to 130
!
!     Exchange the new vertex of the initial simplex with the optimal vertex if
!     necessary. Then, if the initial simplex is not complete, pick its next
!     vertex and calculate the function values there.
!
        if (jdrop <= n) then
            if (datmat(mp, np) <= f) then
                x (jdrop) = sim (jdrop, np)
            else
                sim (jdrop, np) = x (jdrop)
                do k = 1, mpp
                    datmat (k, jdrop) = datmat (k, np)
                    datmat (k, np) = con (k)
                end do
                do k = 1, jdrop
                    sim (jdrop, k) = - rho
                    temp = 0.0_wp
                    do i = k, jdrop
                        temp = temp - simi (i, k)
                    end do
                    simi (jdrop, k) = temp
                end do
            end if
        end if
        if (nfvals <= n) then
            jdrop = nfvals
            x (jdrop) = x (jdrop) + rho
            go to 40
        end if
130     ibrnch = 1
!
!     Identify the optimal vertex of the current simplex.
!
140     phimin = datmat (mp, np) + parmu * datmat (mpp, np)
        nbest = np
        do j = 1, n
            temp = datmat (mp, j) + parmu * datmat (mpp, j)
            if (temp < phimin) then
                nbest = j
                phimin = temp
            else if (temp == phimin .and. parmu == 0.0_wp) then
                if (datmat(mpp, j) < datmat(mpp, nbest)) nbest = j
            end if
        end do
!
!     Switch the best vertex into pole position if it is not there already,
!     and also update SIM, SIMI and DATMAT.
!
        if (nbest <= n) then
            do i = 1, mpp
                temp = datmat (i, np)
                datmat (i, np) = datmat (i, nbest)
                datmat (i, nbest) = temp
            end do
            do i = 1, n
                temp = sim (i, nbest)
                sim (i, nbest) = 0.0_wp
                sim (i, np) = sim (i, np) + temp
                tempa = 0.0_wp
                do k = 1, n
                    sim (i, k) = sim (i, k) - temp
                    tempa = tempa - simi (k, i)
                end do
                simi (nbest, i) = tempa
            end do
        end if
!
!     Make an error return if SIGI is a poor approximation to the inverse of
!     the leading N by N submatrix of SIG.
!
        error = 0.0_wp
        do i = 1, n
            do j = 1, n
                temp = 0.0_wp
                if (i == j) temp = temp - 1.0_wp
                do k = 1, n
                    temp = temp + simi (i, k) * sim (k, j)
                end do
                error = max (error, abs(temp))
            end do
        end do
        if (error > 0.1_wp) then
            if (iprint >= 1) print 210
210         format (/ 3 x, 'Return from subroutine COBYLA because ',&
            'rounding errors are becoming damaging.')
            go to 600
        end if
!
!     Calculate the coefficients of the linear approximations to the objective
!     and constraint functions, placing minus the objective function gradient
!     after the constraint gradients in the array A. The vector W is used for
!     working space.
!
        do k = 1, mp
            con (k) = - datmat (k, np)
            do j = 1, n
                w (j) = datmat (k, j) + con (k)
            end do
            do i = 1, n
                temp = 0.0_wp
                do j = 1, n
                    temp = temp + w (j) * simi (j, i)
                end do
                if (k == mp) temp = - temp
                a (i, k) = temp
            end do
        end do
!
!     Calculate the values of sigma and eta, and set IFLAG=0 if the current
!     simplex is not acceptable.
!
        iflag = 1
        parsig = alpha * rho
        pareta = beta * rho
        do j = 1, n
            wsig = 0.0_wp
            weta = 0.0_wp
            do i = 1, n
                wsig = wsig + simi (j, i) ** 2
                weta = weta + sim (i, j) ** 2
            end do
            vsig (j) = 1.0_wp / sqrt (wsig)
            veta (j) = sqrt (weta)
            if (vsig(j) < parsig .or. veta(j) > pareta) iflag = 0
        end do
!
!     If a new vertex is needed to improve acceptability, then decide which
!     vertex to drop from the simplex.
!
        if (ibrnch == 1 .or. iflag == 1) go to 370
        jdrop = 0
        temp = pareta
        do j = 1, n
            if (veta(j) > temp) then
                jdrop = j
                temp = veta (j)
            end if
        end do
        if (jdrop == 0) then
            do j = 1, n
                if (vsig(j) < temp) then
                    jdrop = j
                    temp = vsig (j)
                end if
            end do
        end if
!
!     Calculate the step to the new vertex and its sign.
!
        temp = gamma * rho * vsig (jdrop)
        do i = 1, n
            dx (i) = temp * simi (jdrop, i)
        end do
        cvmaxp = 0.0_wp
        cvmaxm = 0.0_wp
        do k = 1, mp
            sum = 0.0_wp
            do i = 1, n
                sum = sum + a (i, k) * dx (i)
            end do
            if (k < mp) then
                temp = datmat (k, np)
                cvmaxp = max (cvmaxp,-sum-temp)
                cvmaxm = max (cvmaxm, sum-temp)
            end if
        end do
        dxsign = 1.0_wp
        if (parmu*(cvmaxp-cvmaxm) > sum+sum) dxsign = - 1.0_wp
!
!     Update the elements of SIM and SIMI, and set the next X.
!
        temp = 0.0_wp
        do i = 1, n
            dx (i) = dxsign * dx (i)
            sim (i, jdrop) = dx (i)
            temp = temp + simi (jdrop, i) * dx (i)
        end do
        do i = 1, n
            simi (jdrop, i) = simi (jdrop, i) / temp
        end do
        do j = 1, n
            if (j /= jdrop) then
                temp = 0.0_wp
                do i = 1, n
                    temp = temp + simi (j, i) * dx (i)
                end do
                do i = 1, n
                    simi (j, i) = simi (j, i) - temp * simi (jdrop, i)
                end do
            end if
            x (j) = sim (j, np) + dx (j)
        end do
        go to 40
!
!     Calculate DX=x(*)-x(0). Branch if the length of DX is less than 0.5*RHO.
!
370     iz = 1
        izdota = iz + n * n
        ivmc = izdota + n
        isdirn = ivmc + mp
        idxnew = isdirn + n
        ivmd = idxnew + n
        call trstlp (n, m, a, con, rho, dx, ifull, iact, w(iz), w(izdota), w(ivmc), &
       & w(isdirn), w(idxnew), w(ivmd))
        if (ifull == 0) then
            temp = 0.0_wp
            do i = 1, n
                temp = temp + dx (i) ** 2
            end do
            if (temp < 0.25_wp*rho*rho) then
                ibrnch = 1
                go to 550
            end if
        end if
!
!     Predict the change to F and the new maximum constraint violation if the
!     variables are altered from x(0) to x(0)+DX.
!
        resnew = 0.0_wp
        con (mp) = 0.0_wp
        do k = 1, mp
            sum = con (k)
            do i = 1, n
                sum = sum - a (i, k) * dx (i)
            end do
            if (k < mp) resnew = max (resnew, sum)
        end do
!
!     Increase PARMU if necessary and branch back if this change alters the
!     optimal vertex. Otherwise PREREM and PREREC will be set to the predicted
!     reductions in the merit function and the maximum constraint violation
!     respectively.
!
        barmu = 0.0_wp
        prerec = datmat (mpp, np) - resnew
        if (prerec > 0.0_wp) barmu = sum / prerec
        if (parmu < 1.5_wp*barmu) then
            parmu = 2.0_wp * barmu
            if (iprint >= 2) print 410, parmu
410         format (/ 3 x, 'Increase in PARMU to', 1 pe13.6)
            phi = datmat (mp, np) + parmu * datmat (mpp, np)
            do j = 1, n
                temp = datmat (mp, j) + parmu * datmat (mpp, j)
                if (temp < phi) go to 140
                if (temp == phi .and. parmu == 0.0_wp) then
                    if (datmat(mpp, j) < datmat(mpp, np)) go to 140
                end if
            end do
        end if
        prerem = parmu * prerec - sum
!
!     Calculate the constraint and objective functions at x(*). Then find the
!     actual reduction in the merit function.
!
        do i = 1, n
            x (i) = sim (i, np) + dx (i)
        end do
        ibrnch = 1
        go to 40
440     vmold = datmat (mp, np) + parmu * datmat (mpp, np)
        vmnew = f + parmu * resmax
        trured = vmold - vmnew
        if (parmu == 0.0_wp .and. f == datmat(mp, np)) then
            prerem = prerec
            trured = datmat (mpp, np) - resmax
        end if
!
!     Begin the operations that decide whether x(*) should replace one of the
!     vertices of the current simplex, the change being mandatory if TRURED is
!     positive. Firstly, JDROP is set to the index of the vertex that is to be
!     replaced.
!
        ratio = 0.0_wp
        if (trured <= 0.0_wp) ratio = 1.0_wp
        jdrop = 0
        do j = 1, n
            temp = 0.0_wp
            do i = 1, n
                temp = temp + simi (j, i) * dx (i)
            end do
            temp = abs (temp)
            if (temp > ratio) then
                jdrop = j
                ratio = temp
            end if
            sigbar (j) = temp * vsig (j)
        end do
!
!     Calculate the value of ell.
!
        edgmax = delta * rho
        l = 0
        do j = 1, n
            if (sigbar(j) >= parsig .or. sigbar(j) >= vsig(j)) then
                temp = veta (j)
                if (trured > 0.0_wp) then
                    temp = 0.0_wp
                    do i = 1, n
                        temp = temp + (dx(i)-sim(i, j)) ** 2
                    end do
                    temp = sqrt (temp)
                end if
                if (temp > edgmax) then
                    l = j
                    edgmax = temp
                end if
            end if
        end do
        if (l > 0) jdrop = l
        if (jdrop == 0) go to 550
!
!     Revise the simplex by updating the elements of SIM, SIMI and DATMAT.
!
        temp = 0.0_wp
        do i = 1, n
            sim (i, jdrop) = dx (i)
            temp = temp + simi (jdrop, i) * dx (i)
        end do
        do i = 1, n
            simi (jdrop, i) = simi (jdrop, i) / temp
        end do
        do j = 1, n
            if (j /= jdrop) then
                temp = 0.0_wp
                do i = 1, n
                    temp = temp + simi (j, i) * dx (i)
                end do
                do i = 1, n
                    simi (j, i) = simi (j, i) - temp * simi (jdrop, i)
                end do
            end if
        end do
        do k = 1, mpp
            datmat (k, jdrop) = con (k)
        end do
!
!     Branch back for further iterations with the current RHO.
!
        if (trured > 0.0_wp .and. trured >= 0.1_wp*prerem) go to 140
550     if (iflag == 0) then
            ibrnch = 0
            go to 140
        end if
!
!     Otherwise reduce RHO if it is not at its least value and reset PARMU.
!
        if (rho > rhoend) then
            rho = 0.5_wp * rho
            if (rho <= 1.5_wp*rhoend) rho = rhoend
            if (parmu > 0.0_wp) then
                denom = 0.0_wp
                do k = 1, mp
                    cmin = datmat (k, np)
                    cmax = cmin
                    do i = 1, n
                        cmin = min (cmin, datmat(k, i))
                        cmax = max (cmax, datmat(k, i))
                    end do
                    if (k <= m .and. cmin < 0.5_wp*cmax) then
                        temp = max (cmax, 0.0_wp) - cmin
                        if (denom <= 0.0_wp) then
                            denom = temp
                        else
                            denom = min (denom, temp)
                        end if
                    end if
                end do
                if (denom == 0.0_wp) then
                    parmu = 0.0_wp
                else if (cmax-cmin < parmu*denom) then
                    parmu = (cmax-cmin) / denom
                end if
            end if
            if (iprint >= 2) print 580, rho, parmu
580         format (/ 3 x, 'Reduction in RHO to', 1 pe13.6, '  and PARMU =', 1 pe13.6)
            if (iprint == 2) then
                print 70, nfvals, datmat (mp, np), datmat (mpp, np), (sim(i, np), i=1, &
               & iptem)
                if (iptem < n) print 80, (x(i), i=iptemp, n)
            end if
            go to 140
        end if
!
!     Return the best calculated values of the variables.
!
        if (iprint >= 1) print 590
590     format (/ 3 x, 'Normal return from subroutine COBYLA')
        if (ifull == 1) go to 620
600     do i = 1, n
            x (i) = sim (i, np)
        end do
        f = datmat (mp, np)
        resmax = datmat (mpp, np)
620     if (iprint >= 1) then
            print 70, nfvals, f, resmax, (x(i), i=1, iptem)
            if (iptem < n) print 80, (x(i), i=iptemp, n)
        end if
        maxfun = nfvals

    end subroutine cobylb

    subroutine trstlp (n, m, a, b, rho, dx, ifull, iact, z, zdota, vmultc, sdirn, dxnew, &
                       vmultd)

        implicit real (wp) (a-h, o-z)

        dimension a (n,*), b (*), dx (*), iact (*), z (n,*), zdota (*), vmultc (*), &
                  sdirn (*), dxnew (*), vmultd (*)
!
!     This subroutine calculates an N-component vector DX by applying the
!     following two stages. In the first stage, DX is set to the shortest
!     vector that minimizes the greatest violation of the constraints
!       A(1,K)*DX(1)+A(2,K)*DX(2)+...+A(N,K)*DX(N) .GE. B(K), K=2,3,...,M,
!     subject to the Euclidean length of DX being at most RHO. If its length is
!     strictly less than RHO, then we use the resultant freedom in DX to
!     minimize the objective function
!              -A(1,M+1)*DX(1)-A(2,M+1)*DX(2)-...-A(N,M+1)*DX(N)
!     subject to no increase in any greatest constraint violation. This
!     notation allows the gradient of the objective function to be regarded as
!     the gradient of a constraint. Therefore the two stages are distinguished
!     by MCON .EQ. M and MCON .GT. M respectively. It is possible that a
!     degeneracy may prevent DX from attaining the target length RHO. Then the
!     value IFULL=0 would be set, but usually IFULL=1 on return.
!
!     In general NACT is the number of constraints in the active set and
!     IACT(1),...,IACT(NACT) are their indices, while the remainder of IACT
!     contains a permutation of the remaining constraint indices. Further, Z is
!     an orthogonal matrix whose first NACT columns can be regarded as the
!     result of Gram-Schmidt applied to the active constraint gradients. For
!     J=1,2,...,NACT, the number ZDOTA(J) is the scalar product of the J-th
!     column of Z with the gradient of the J-th active constraint. DX is the
!     current vector of variables and here the residuals of the active
!     constraints should be zero. Further, the active constraints have
!     nonnegative Lagrange multipliers that are held at the beginning of
!     VMULTC. The remainder of this vector holds the residuals of the inactive
!     constraints at DX, the ordering of the components of VMULTC being in
!     agreement with the permutation of the indices of the constraints that is
!     in IACT. All these residuals are nonnegative, which is achieved by the
!     shift RESMAX that makes the least residual zero.
!
!     Initialize Z and some other variables. The value of RESMAX will be
!     appropriate to DX=0, while ICON will be the index of a most violated
!     constraint if RESMAX is positive. Usually during the first stage the
!     vector SDIRN gives a search direction that reduces all the active
!     constraint violations by one simultaneously.
!
        ifull = 1
        mcon = m
        nact = 0
        resmax = 0.0_wp
        do i = 1, n
            do j = 1, n
                z (i, j) = 0.0_wp
            end do
            z (i, i) = 1.0_wp
            dx (i) = 0.0_wp
        end do
        if (m >= 1) then
            do k = 1, m
                if (b(k) > resmax) then
                    resmax = b (k)
                    icon = k
                end if
            end do
            do k = 1, m
                iact (k) = k
                vmultc (k) = resmax - b (k)
            end do
        end if
        if (resmax == 0.0_wp) go to 480
        do i = 1, n
            sdirn (i) = 0.0_wp
        end do
!
!     End the current stage of the calculation if 3 consecutive iterations
!     have either failed to reduce the best calculated value of the objective
!     function or to increase the number of active constraints since the best
!     value was calculated. This strategy prevents cycling, but there is a
!     remote possibility that it will cause premature termination.
!
60      optold = 0.0_wp
        icount = 0
70      if (mcon == m) then
            optnew = resmax
        else
            optnew = 0.0_wp
            do i = 1, n
                optnew = optnew - dx (i) * a (i, mcon)
            end do
        end if
        if (icount == 0 .or. optnew < optold) then
            optold = optnew
            nactx = nact
            icount = 3
        else if (nact > nactx) then
            nactx = nact
            icount = 3
        else
            icount = icount - 1
            if (icount == 0) go to 490
        end if
!
!     If ICON exceeds NACT, then we add the constraint with index IACT(ICON) to
!     the active set. Apply Givens rotations so that the last N-NACT-1 columns
!     of Z are orthogonal to the gradient of the new constraint, a scalar
!     product being set to zero if its nonzero value could be due to computer
!     rounding errors. The array DXNEW is used for working space.
!
        if (icon <= nact) go to 260
        kk = iact (icon)
        do i = 1, n
            dxnew (i) = a (i, kk)
        end do
        tot = 0.0_wp
        k = n
100     if (k > nact) then
            sp = 0.0_wp
            spabs = 0.0_wp
            do i = 1, n
                temp = z (i, k) * dxnew (i)
                sp = sp + temp
                spabs = spabs + abs (temp)
            end do
            acca = spabs + 0.1_wp * abs (sp)
            accb = spabs + 0.2_wp * abs (sp)
            if (spabs >= acca .or. acca >= accb) sp = 0.0_wp
            if (tot == 0.0_wp) then
                tot = sp
            else
                kp = k + 1
                temp = sqrt (sp*sp+tot*tot)
                alpha = sp / temp
                beta = tot / temp
                tot = temp
                do i = 1, n
                    temp = alpha * z (i, k) + beta * z (i, kp)
                    z (i, kp) = alpha * z (i, kp) - beta * z (i, k)
                    z (i, k) = temp
                end do
            end if
            k = k - 1
            go to 100
        end if
!
!     Add the new constraint if this can be done without a deletion from the
!     active set.
!
        if (tot /= 0.0_wp) then
            nact = nact + 1
            zdota (nact) = tot
            vmultc (icon) = vmultc (nact)
            vmultc (nact) = 0.0_wp
            go to 210
        end if
!
!     The next instruction is reached if a deletion has to be made from the
!     active set in order to make room for the new active constraint, because
!     the new constraint gradient is a linear combination of the gradients of
!     the old active constraints. Set the elements of VMULTD to the multipliers
!     of the linear combination. Further, set IOUT to the index of the
!     constraint to be deleted, but branch if no suitable index can be found.
!
        ratio = - 1.0_wp
        k = nact
130     zdotv = 0.0_wp
        zdvabs = 0.0_wp
        do i = 1, n
            temp = z (i, k) * dxnew (i)
            zdotv = zdotv + temp
            zdvabs = zdvabs + abs (temp)
        end do
        acca = zdvabs + 0.1_wp * abs (zdotv)
        accb = zdvabs + 0.2_wp * abs (zdotv)
        if (zdvabs < acca .and. acca < accb) then
            temp = zdotv / zdota (k)
            if (temp > 0.0_wp .and. iact(k) <= m) then
                tempa = vmultc (k) / temp
                if (ratio < 0.0_wp .or. tempa < ratio) then
                    ratio = tempa
                    iout = k
                end if
            end if
            if (k >= 2) then
                kw = iact (k)
                do i = 1, n
                    dxnew (i) = dxnew (i) - temp * a (i, kw)
                end do
            end if
            vmultd (k) = temp
        else
            vmultd (k) = 0.0_wp
        end if
        k = k - 1
        if (k > 0) go to 130
        if (ratio < 0.0_wp) go to 490
!
!     Revise the Lagrange multipliers and reorder the active constraints so
!     that the one to be replaced is at the end of the list. Also calculate the
!     new value of ZDOTA(NACT) and branch if it is not acceptable.
!
        do k = 1, nact
            vmultc (k) = max (0.0_wp, vmultc(k)-ratio*vmultd(k))
        end do
        if (icon < nact) then
            isave = iact (icon)
            vsave = vmultc (icon)
            k = icon
170         kp = k + 1
            kw = iact (kp)
            sp = 0.0_wp
            do i = 1, n
                sp = sp + z (i, k) * a (i, kw)
            end do
            temp = sqrt (sp*sp+zdota(kp)**2)
            alpha = zdota (kp) / temp
            beta = sp / temp
            zdota (kp) = alpha * zdota (k)
            zdota (k) = temp
            do i = 1, n
                temp = alpha * z (i, kp) + beta * z (i, k)
                z (i, kp) = alpha * z (i, k) - beta * z (i, kp)
                z (i, k) = temp
            end do
            iact (k) = kw
            vmultc (k) = vmultc (kp)
            k = kp
            if (k < nact) go to 170
            iact (k) = isave
            vmultc (k) = vsave
        end if
        temp = 0.0_wp
        do i = 1, n
            temp = temp + z (i, nact) * a (i, kk)
        end do
        if (temp == 0.0_wp) go to 490
        zdota (nact) = temp
        vmultc (icon) = 0.0_wp
        vmultc (nact) = ratio
!
!     Update IACT and ensure that the objective function continues to be
!     treated as the last active constraint when MCON>M.
!
210     iact (icon) = iact (nact)
        iact (nact) = kk
        if (mcon > m .and. kk /= mcon) then
            k = nact - 1
            sp = 0.0_wp
            do i = 1, n
                sp = sp + z (i, k) * a (i, kk)
            end do
            temp = sqrt (sp*sp+zdota(nact)**2)
            alpha = zdota (nact) / temp
            beta = sp / temp
            zdota (nact) = alpha * zdota (k)
            zdota (k) = temp
            do i = 1, n
                temp = alpha * z (i, nact) + beta * z (i, k)
                z (i, nact) = alpha * z (i, k) - beta * z (i, nact)
                z (i, k) = temp
            end do
            iact (nact) = iact (k)
            iact (k) = kk
            temp = vmultc (k)
            vmultc (k) = vmultc (nact)
            vmultc (nact) = temp
        end if
!
!     If stage one is in progress, then set SDIRN to the direction of the next
!     change to the current vector of variables.
!
        if (mcon > m) go to 320
        kk = iact (nact)
        temp = 0.0_wp
        do i = 1, n
            temp = temp + sdirn (i) * a (i, kk)
        end do
        temp = temp - 1.0_wp
        temp = temp / zdota (nact)
        do i = 1, n
            sdirn (i) = sdirn (i) - temp * z (i, nact)
        end do
        go to 340
!
!     Delete the constraint that has the index IACT(ICON) from the active set.
!
260     if (icon < nact) then
            isave = iact (icon)
            vsave = vmultc (icon)
            k = icon
270         kp = k + 1
            kk = iact (kp)
            sp = 0.0_wp
            do i = 1, n
                sp = sp + z (i, k) * a (i, kk)
            end do
            temp = sqrt (sp*sp+zdota(kp)**2)
            alpha = zdota (kp) / temp
            beta = sp / temp
            zdota (kp) = alpha * zdota (k)
            zdota (k) = temp
            do i = 1, n
                temp = alpha * z (i, kp) + beta * z (i, k)
                z (i, kp) = alpha * z (i, k) - beta * z (i, kp)
                z (i, k) = temp
            end do
            iact (k) = kk
            vmultc (k) = vmultc (kp)
            k = kp
            if (k < nact) go to 270
            iact (k) = isave
            vmultc (k) = vsave
        end if
        nact = nact - 1
!
!     If stage one is in progress, then set SDIRN to the direction of the next
!     change to the current vector of variables.
!
        if (mcon > m) go to 320
        temp = 0.0_wp
        do i = 1, n
            temp = temp + sdirn (i) * z (i, nact+1)
        end do
        do i = 1, n
            sdirn (i) = sdirn (i) - temp * z (i, nact+1)
        end do
        go to 340
!
!     Pick the next search direction of stage two.
!
320     temp = 1.0_wp / zdota (nact)
        do i = 1, n
            sdirn (i) = temp * z (i, nact)
        end do
!
!     Calculate the step to the boundary of the trust region or take the step
!     that reduces RESMAX to zero. The two statements below that include the
!     factor 1.0E-6 prevent some harmless underflows that occurred in a test
!     calculation. Further, we skip the step if it could be zero within a
!     reasonable tolerance for computer rounding errors.
!
340     dd = rho * rho
        sd = 0.0_wp
        ss = 0.0_wp
        do i = 1, n
            if (abs(dx(i)) >= 1.0e-6_wp*rho) dd = dd - dx (i) ** 2
            sd = sd + dx (i) * sdirn (i)
            ss = ss + sdirn (i) ** 2
        end do
        if (dd <= 0.0_wp) go to 490
        temp = sqrt (ss*dd)
        if (abs(sd) >= 1.0e-6_wp*temp) temp = sqrt (ss*dd+sd*sd)
        stpful = dd / (temp+sd)
        step = stpful
        if (mcon == m) then
            acca = step + 0.1_wp * resmax
            accb = step + 0.2_wp * resmax
            if (step >= acca .or. acca >= accb) go to 480
            step = min (step, resmax)
        end if
!
!     Set DXNEW to the new variables if STEP is the steplength, and reduce
!     RESMAX to the corresponding maximum residual if stage one is being done.
!     Because DXNEW will be changed during the calculation of some Lagrange
!     multipliers, it will be restored to the following value later.
!
        do i = 1, n
            dxnew (i) = dx (i) + step * sdirn (i)
        end do
        if (mcon == m) then
            resold = resmax
            resmax = 0.0_wp
            do k = 1, nact
                kk = iact (k)
                temp = b (kk)
                do i = 1, n
                    temp = temp - a (i, kk) * dxnew (i)
                end do
                resmax = max (resmax, temp)
            end do
        end if
!
!     Set VMULTD to the VMULTC vector that would occur if DX became DXNEW. A
!     device is included to force VMULTD(K)=0.0 if deviations from this value
!     can be attributed to computer rounding errors. First calculate the new
!     Lagrange multipliers.
!
        k = nact
390     zdotw = 0.0_wp
        zdwabs = 0.0_wp
        do i = 1, n
            temp = z (i, k) * dxnew (i)
            zdotw = zdotw + temp
            zdwabs = zdwabs + abs (temp)
        end do
        acca = zdwabs + 0.1_wp * abs (zdotw)
        accb = zdwabs + 0.2_wp * abs (zdotw)
        if (zdwabs >= acca .or. acca >= accb) zdotw = 0.0_wp
        vmultd (k) = zdotw / zdota (k)
        if (k >= 2) then
            kk = iact (k)
            do i = 1, n
                dxnew (i) = dxnew (i) - vmultd (k) * a (i, kk)
            end do
            k = k - 1
            go to 390
        end if
        if (mcon > m) vmultd (nact) = max (0.0_wp, vmultd(nact))
!
!     Complete VMULTC by finding the new constraint residuals.
!
        do i = 1, n
            dxnew (i) = dx (i) + step * sdirn (i)
        end do
        if (mcon > nact) then
            kl = nact + 1
            do k = kl, mcon
                kk = iact (k)
                sum = resmax - b (kk)
                sumabs = resmax + abs (b(kk))
                do i = 1, n
                    temp = a (i, kk) * dxnew (i)
                    sum = sum + temp
                    sumabs = sumabs + abs (temp)
                end do
                acca = sumabs + 0.1_wp * abs (sum)
                accb = sumabs + 0.2_wp * abs (sum)
                if (sumabs >= acca .or. acca >= accb) sum = 0.0_wp
                vmultd (k) = sum
            end do
        end if
!
!     Calculate the fraction of the step from DX to DXNEW that will be taken.
!
        ratio = 1.0_wp
        icon = 0
        do k = 1, mcon
            if (vmultd(k) < 0.0_wp) then
                temp = vmultc (k) / (vmultc(k)-vmultd(k))
                if (temp < ratio) then
                    ratio = temp
                    icon = k
                end if
            end if
        end do
!
!     Update DX, VMULTC and RESMAX.
!
        temp = 1.0_wp - ratio
        do i = 1, n
            dx (i) = temp * dx (i) + ratio * dxnew (i)
        end do
        do k = 1, mcon
            vmultc (k) = max (0.0_wp, temp*vmultc(k)+ratio*vmultd(k))
        end do
        if (mcon == m) resmax = resold + ratio * (resmax-resold)
!
!     If the full step is not acceptable then begin another iteration.
!     Otherwise switch to stage two or end the calculation.
!
        if (icon > 0) go to 70
        if (step == stpful) return
480     mcon = m + 1
        icon = mcon
        iact (mcon) = mcon
        vmultc (mcon) = 0.0_wp
        go to 60
!
!     We employ any freedom that may be available to reduce the objective
!     function before returning a DX whose length is less than RHO.
!
490     if (mcon == m) go to 480
        ifull = 0

    end subroutine trstlp

!*****************************************************************************************
!>
!  Test routine for [[cobyla]].
!
!  From: Report DAMTP 1992/NA5.

    subroutine cobyla_test ()

        implicit none

        real(wp),dimension(10) :: x,xopt
        integer :: nprob,n,m,i,icase,iprint,maxfun
        real(wp) :: rhobeg,rhoend,temp,tempa,tempb,tempc,tempd

        do nprob = 1, 10

            if (nprob == 1) then
    !
    !     minimization of a simple quadratic function of two variables.
    !
                print 10
10              format (/ 7 x, 'Output from test problem 1 (Simple quadratic)')
                n = 2
                m = 0
                xopt (1) = - 1.0_wp
                xopt (2) = 0.0_wp

            else if (nprob == 2) then
    !
    !     Easy two dimensional minimization in unit circle.
    !
                print 20
20              format (/ 7 x, 'Output from test problem 2 (2D unit circle ',&
                'calculation)')
                n = 2
                m = 1
                xopt (1) = sqrt (0.5_wp)
                xopt (2) = - xopt (1)

            else if (nprob == 3) then
    !
    !     Easy three dimensional minimization in ellipsoid.
    !
                print 30
30              format (/ 7 x, 'Output from test problem 3 (3D ellipsoid ',&
                'calculation)')
                n = 3
                m = 1
                xopt (1) = 1.0_wp / sqrt (3.0_wp)
                xopt (2) = 1.0_wp / sqrt (6.0_wp)
                xopt (3) = - 1.0_wp / 3.0_wp

            else if (nprob == 4) then
    !
    !     Weak version of Rosenbrock's problem.
    !
                print 40
40              format (/ 7 x, 'Output from test problem 4 (Weak Rosenbrock)')
                n = 2
                m = 0
                xopt (1) = - 1.0_wp
                xopt (2) = 1.0_wp

            else if (nprob == 5) then
    !
    !     Intermediate version of Rosenbrock's problem.
    !
                print 50
50              format (/ 7 x, 'Output from test problem 5 (Intermediate ', 'Rosenbrock)')
                n = 2
                m = 0
                xopt (1) = - 1.0_wp
                xopt (2) = 1.0_wp

            else if (nprob == 6) then
    !
    !     This problem is taken from Fletcher's book Practical Methods of
    !     Optimization and has the equation number (9.1.15).
    !
                print 60
60              format (/ 7 x, 'Output from test problem 6 (Equation ',&
                '(9.1.15) in Fletcher)')
                n = 2
                m = 2
                xopt (1) = sqrt (0.5_wp)
                xopt (2) = xopt (1)

            else if (nprob == 7) then
    !
    !     This problem is taken from Fletcher's book Practical Methods of
    !     Optimization and has the equation number (14.4.2).
    !
                print 70
70              format (/ 7 x, 'Output from test problem 7 (Equation ',&
                '(14.4.2) in Fletcher)')
                n = 3
                m = 3
                xopt (1) = 0.0_wp
                xopt (2) = - 3.0_wp
                xopt (3) = - 3.0_wp

            else if (nprob == 8) then
    !
    !     This problem is taken from page 66 of Hock and Schittkowski's book Test
    !     Examples for Nonlinear Programming Codes. It is their test problem Number
    !     43, and has the name Rosen-Suzuki.
    !
                print 80
80              format (/ 7 x, 'Output from test problem 8 (Rosen-Suzuki)')
                n = 4
                m = 3
                xopt (1) = 0.0_wp
                xopt (2) = 1.0_wp
                xopt (3) = 2.0_wp
                xopt (4) = - 1.0_wp

            else if (nprob == 9) then
    !
    !     This problem is taken from page 111 of Hock and Schittkowski's
    !     book Test Examples for Nonlinear Programming Codes. It is their
    !     test problem Number 100.
    !
                print 90
90              format (/ 7 x, 'Output from test problem 9 (Hock and ',&
                'Schittkowski 100)')
                n = 7
                m = 4
                xopt (1) = 2.330499_wp
                xopt (2) = 1.951372_wp
                xopt (3) = - 0.4775414_wp
                xopt (4) = 4.365726_wp
                xopt (5) = - 0.624487_wp
                xopt (6) = 1.038131_wp
                xopt (7) = 1.594227_wp

            else if (nprob == 10) then
    !
    !     This problem is taken from page 415 of Luenberger's book Applied
    !     Nonlinear Programming. It is to maximize the area of a hexagon of
    !     unit diameter.
    !
                print 100
100             format (/ 7 x, 'Output from test problem 10 (Hexagon area)')
                n = 9
                m = 14
            end if
            do icase = 1, 2
                do i = 1, n
                    x (i) = 1.0_wp
                end do
                rhobeg = 0.5_wp
                rhoend = 0.001_wp
                if (icase == 2) rhoend = 0.0001_wp
                iprint = 1
                maxfun = 2000
                call cobyla (n, m, x, rhobeg, rhoend, iprint, maxfun, calcfc)
                if (nprob == 10) then
                    tempa = x (1) + x (3) + x (5) + x (7)
                    tempb = x (2) + x (4) + x (6) + x (8)
                    tempc = 0.5_wp / sqrt (tempa*tempa+tempb*tempb)
                    tempd = tempc * sqrt (3.0_wp)
                    xopt (1) = tempd * tempa + tempc * tempb
                    xopt (2) = tempd * tempb - tempc * tempa
                    xopt (3) = tempd * tempa - tempc * tempb
                    xopt (4) = tempd * tempb + tempc * tempa
                    do i = 1, 4
                        xopt (i+4) = xopt (i)
                    end do
                end if
                temp = 0.0_wp
                do i = 1, n
                    temp = temp + (x(i)-xopt(i)) ** 2
                end do
                print 150, sqrt (temp)
150             format (/ 5 x, 'Least squares error in variables =', 1 pe16.6)
            end do
            print 170
170         format (2 x, '----------------------------------------------',&
            '--------------------')

        end do

    contains

        subroutine calcfc (n, m, x, f, con)

            implicit none

            integer,intent(in)                :: n
            integer,intent(in)                :: m
            real(wp),dimension(*),intent(in)  :: x
            real(wp),intent(out)              :: f
            real(wp),dimension(*),intent(out) :: con

            if (nprob == 1) then
    !
    !     Test problem 1 (Simple quadratic)
    !
                f = 10.0_wp * (x(1)+1.0_wp) ** 2 + x (2) ** 2

            else if (nprob == 2) then
    !
    !    Test problem 2 (2D unit circle calculation)
    !
                f = x (1) * x (2)
                con (1) = 1.0_wp - x (1) ** 2 - x (2) ** 2

            else if (nprob == 3) then
    !
    !     Test problem 3 (3D ellipsoid calculation)
    !
                f = x (1) * x (2) * x (3)
                con (1) = 1.0_wp - x (1) ** 2 - 2.0_wp * x (2) ** 2 - 3.0_wp * x (3) ** 2

            else if (nprob == 4) then
    !
    !     Test problem 4 (Weak Rosenbrock)
    !
                f = (x(1)**2-x(2)) ** 2 + (1.0_wp+x(1)) ** 2

            else if (nprob == 5) then
    !
    !     Test problem 5 (Intermediate Rosenbrock)
    !
                f = 10.0_wp * (x(1)**2-x(2)) ** 2 + (1.0_wp+x(1)) ** 2

            else if (nprob == 6) then
    !
    !     Test problem 6 (Equation (9.1.15) in Fletcher's book)
    !
                f = - x (1) - x (2)
                con (1) = x (2) - x (1) ** 2
                con (2) = 1.0_wp - x (1) ** 2 - x (2) ** 2

            else if (nprob == 7) then
    !
    !     Test problem 7 (Equation (14.4.2) in Fletcher's book)
    !
                f = x (3)
                con (1) = 5.0_wp * x (1) - x (2) + x (3)
                con (2) = x (3) - x (1) ** 2 - x (2) ** 2 - 4.0_wp * x (2)
                con (3) = x (3) - 5.0_wp * x (1) - x (2)

            else if (nprob == 8) then
    !
    !     Test problem 8 (Rosen-Suzuki)
    !
                f = x (1) ** 2 + x (2) ** 2 + 2.0_wp * x (3) ** 2 + x (4) ** 2 - 5.0_wp * &
               & x (1) - 5.0_wp * x (2) - 21.0_wp * x (3) + 7.0_wp * x (4)
                con (1) = 8.0_wp - x (1) ** 2 - x (2) ** 2 - x (3) ** 2 - x (4) ** 2 - x &
               & (1) + x (2) - x (3) + x (4)
                con (2) = 10.0_wp - x (1) ** 2 - 2.0_wp * x (2) ** 2 - x (3) ** 2 - &
               & 2.0_wp * x (4) ** 2 + x (1) + x (4)
                con (3) = 5.0_wp - 2.0_wp * x (1) ** 2 - x (2) ** 2 - x (3) ** 2 - 2.0_wp &
               & * x (1) + x (2) + x (4)

            else if (nprob == 9) then
    !
    !     Test problem 9 (Hock and Schittkowski 100)
    !
                f = (x(1)-10.0_wp) ** 2 + 5.0_wp * (x(2)-12.0_wp) ** 2 + x (3) ** 4 + &
               & 3.0_wp * (x(4)-11.0_wp) ** 2 + 10.0_wp * x (5) ** 6 + 7.0_wp * x (6) ** &
               & 2 + x (7) ** 4 - 4.0_wp * x (6) * x (7) - 10.0_wp * x (6) - 8.0_wp * x &
               & (7)
                con (1) = 127.0_wp - 2.0_wp * x (1) ** 2 - 3.0_wp * x (2) ** 4 - x (3) - &
               & 4.0_wp * x (4) ** 2 - 5.0_wp * x (5)
                con (2) = 282.0_wp - 7.0_wp * x (1) - 3.0_wp * x (2) - 10.0_wp * x (3) ** &
               & 2 - x (4) + x (5)
                con (3) = 196.0_wp - 23.0_wp * x (1) - x (2) ** 2 - 6.0_wp * x (6) ** 2 + &
               & 8.0_wp * x (7)
                con (4) = - 4.0_wp * x (1) ** 2 - x (2) ** 2 + 3.0_wp * x (1) * x (2) - &
               & 2.0_wp * x (3) ** 2 - 5.0_wp * x (6) + 11.0_wp * x (7)

            else if (nprob == 10) then
    !
    !     Test problem 10 (Hexagon area)
    !
                f = - 0.5_wp * &
                   (x(1)*x(4)-x(2)*x(3)+x(3)*x(9)-x(5)*x(9)+x(5)*x(8)-x(6)*x(7))
                con (1) = 1.0_wp - x (3) ** 2 - x (4) ** 2
                con (2) = 1.0_wp - x (9) ** 2
                con (3) = 1.0_wp - x (5) ** 2 - x (6) ** 2
                con (4) = 1.0_wp - x (1) ** 2 - (x(2)-x(9)) ** 2
                con (5) = 1.0_wp - (x(1)-x(5)) ** 2 - (x(2)-x(6)) ** 2
                con (6) = 1.0_wp - (x(1)-x(7)) ** 2 - (x(2)-x(8)) ** 2
                con (7) = 1.0_wp - (x(3)-x(5)) ** 2 - (x(4)-x(6)) ** 2
                con (8) = 1.0_wp - (x(3)-x(7)) ** 2 - (x(4)-x(8)) ** 2
                con (9) = 1.0_wp - x (7) ** 2 - (x(8)-x(9)) ** 2
                con (10) = x (1) * x (4) - x (2) * x (3)
                con (11) = x (3) * x (9)
                con (12) = - x (5) * x (9)
                con (13) = x (5) * x (8) - x (6) * x (7)
                con (14) = x (9)

            end if

        end subroutine calcfc

    end subroutine cobyla_test
!*****************************************************************************************

end module cobyla_module


      PROGRAM PSITST
C--------------------------------------------------------------------
C  Fortran 77 program to test PSI
C
C  Data required
C
C     None
C
C  Subprograms required from this package
C
C     MACHAR - an environmental inquiry program providing
C              information on the floating-point arithmetic
C              system.  Note that the call to MACHAR can
C              be deleted provided the following five
C              parameters are assigned the values indicated
C
C                 IBETA  - the radix of the floating-point system
C                 IT     - the number of base-IBETA digits in the
C                          significand of a floating-point number
C                 MINEXP - the largest in magnitude negative
C                          integer such that  FLOAT(IBETA)**MINEXP
C                          is a positive floating-point number
C                 EPS    - the smallest positive floating-point
C                          number such that 1.0+EPS .NE. 1.0
C                 EPSNEG - the smallest positive floating-point
C                          number such that 1.0-EPSNEG .NE. 1.0
C
C     REN(K) - a function subprogram returning random real
C              numbers uniformly distributed over (0,1)
C
C
C  Intrinsic Fortran functions required are:
C
C         ABS, DBLE, LOG, MAX, REAL, SQRT
C
C  Reference: "Performance evaluation of programs related
C              to the real gamma function", W. J. Cody,
C              submitted for publication.
C
C  Latest modification: March 14, 1992
C
C  Author: W. J. Cody
C          Mathematics and Computer Science Division
C          Argonne National Laboratory
C          Argonne, IL 60439
C
C--------------------------------------------------------------------
      INTEGER I,IBETA,IEXP,IOUT,IRND,IT,J,JT,K1,K2,K3,
     1        MACHEP,MAXEXP,MINEXP,N,NDUM,NEGEP,NGRD
CS    REAL
      DOUBLE PRECISION
     1   A,AIT,ALBETA,ALL9,B,BETA,CONV,DEL,EIGHT,EPS,
     2   EPSNEG,HALF,ONE,ONE7,ONE6,PSI,REN,R6,R7,THREE,
     3   TWENTY,Y,V0,W,X,XH,XL,XL2,XMAX,XMIN,XN,XX,X0,
     4   X01,X1,Z,ZERO,ZH,ZZ
CS    DATA ZERO,ONE,THREE/0.0E0,1.0E0,3.0E0/,
CS   1   HALF,EIGHT,TWENTY,ALL9/0.5E0,8.0E0,20.0E0,-999.0E0/,
CS   2   XL2/6.9314718055994530942E-1/,
CS   3   ONE7,ONE6/-17.625E0,-16.875E0/,
CS   4   X0,X01,V0/374.0E0,256.0E0,-6.7240239024288040437E-04/
      DATA ZERO,ONE,THREE/0.0D0,1.0D0,3.0D0/,
     1   HALF,EIGHT,TWENTY,ALL9/0.5D0,8.0D0,20.0D0,-999.0D0/,
     2   XL2/6.9314718055994530942D-1/,
     3   ONE7,ONE6/-17.625D0,-16.875D0/,
     4   X0,X01,V0/374.0D0,256.0D0,-6.7240239024288040437D-04/
      DATA IOUT/6/
C--------------------------------------------------------------------
C  Statement functions for conversion between integer and float
C--------------------------------------------------------------------
CS    CONV(NDUM) = REAL(NDUM)
      CONV(NDUM) = DBLE(NDUM)
C--------------------------------------------------------------------
C  Determine machine parameters and set constants
C--------------------------------------------------------------------
      CALL MACHAR(IBETA,IT,IRND,NGRD,MACHEP,NEGEP,IEXP,MINEXP,
     1            MAXEXP,EPS,EPSNEG,XMIN,XMAX)
      BETA = CONV(IBETA)
      ALBETA = LOG(BETA)
      AIT = CONV(IT)
      JT = 0
C--------------------------------------------------------------------
C     Random argument accuracy tests
C--------------------------------------------------------------------
      DO 300 J = 1, 4
         K1 = 0
         K3 = 0
         X1 = ZERO
         R6 = ZERO
         R7 = ZERO
         N = 2000
         IF (J .EQ. 1) THEN
               A = ZERO
               B = ONE
            ELSE IF (J .EQ. 2) THEN
               A = B + B
               B = EIGHT
            ELSE IF (J .EQ. 3) THEN
               A = B
               B = TWENTY
            ELSE
               A = ONE7
               B = ONE6
               N = 500
         END IF
         XN = CONV(N)
         DEL = (B - A) / XN
         XL = A
         DO 200 I = 1, N
            X = DEL * REN(JT) + XL
C--------------------------------------------------------------------
C  Carefully purify arguments and evaluate identity
C--------------------------------------------------------------------
            XX = X * HALF
            XH = XX + HALF
            XX = XH - HALF
            X = XX + XX
            Z = PSI(X)
            ZH = PSI(XH)
            ZZ = PSI(XX)
            ZZ = (ZZ+ZH)*HALF + XL2
C--------------------------------------------------------------------
C  Accumulate results
C--------------------------------------------------------------------
            W = (ZZ - Z) / ZZ
            IF (W .GT. ZERO) THEN
                  K1 = K1 + 1
               ELSE IF (W .LT. ZERO) THEN
                  K3 = K3 + 1
            END IF
            W = ABS(W)
            IF (W .GT. R6) THEN
               R6 = W
               X1 = X
            END IF
            R7 = R7 + W * W
            XL = XL + DEL
  200    CONTINUE
C--------------------------------------------------------------------
C  Process and output statistics
C--------------------------------------------------------------------
         K2 = N - K3 - K1
         R7 = SQRT(R7/XN)
         IF (2*(J/2) .NE. J) WRITE (IOUT,1000)
         WRITE (IOUT,1001)
         WRITE (IOUT,1010) N,A,B
         WRITE (IOUT,1011) K1,K2,K3
         WRITE (IOUT,1020) IT,IBETA
         IF (R6 .NE. ZERO) THEN
               W = LOG(ABS(R6))/ALBETA
            ELSE
               W = ALL9
         END IF
         WRITE (IOUT,1021) R6,IBETA,W,X1
         W = MAX(AIT+W,ZERO)
         WRITE (IOUT,1022) IBETA,W
         IF (R7 .NE. ZERO) THEN
               W = LOG(ABS(R7))/ALBETA
            ELSE
               W = ALL9
         END IF
         WRITE (IOUT,1023) R7,IBETA,W
         W = MAX(AIT+W,ZERO)
         WRITE (IOUT,1022) IBETA,W
  300 CONTINUE
C--------------------------------------------------------------------
C  Special tests
C--------------------------------------------------------------------
      WRITE (IOUT,1030)
         X = X0/X01
         Y = PSI(X)
         Z = (Y-V0)/V0
         IF (Z .NE. ZERO) THEN
               W = LOG(ABS(Z))/ALBETA
            ELSE
               W = ALL9
         END IF
         W = MAX(AIT+W,ZERO)
         WRITE (IOUT,1031) X,Y,IBETA,W
      WRITE (IOUT,1033)
      IF (XMAX*XMIN .GE. ONE) THEN
            X = XMIN
         ELSE
            X = ONE / XMAX
      END IF
      WRITE (IOUT,1035) X
      Y = PSI(X)
      WRITE (IOUT,1036) Y
      X = XMAX
      WRITE (IOUT,1035) X
      Y = PSI(X)
      WRITE (IOUT,1036) Y
C--------------------------------------------------------------------
C  Test of error returns
C--------------------------------------------------------------------
      WRITE (IOUT,1037)
      X = ZERO
      WRITE (IOUT,1034) X
      Y = PSI(X)
      WRITE (IOUT,1036) Y
      X = -THREE/EPS
      WRITE (IOUT,1034) X
      Y = PSI(X)
      WRITE (IOUT,1036) Y
      WRITE (IOUT,1100)
      STOP
C--------------------------------------------------------------------
 1000 FORMAT('1')
 1001 FORMAT(' Test of PSI(X) vs (PSI(X/2)+PSI(X/2+1/2))/2 + ln(2)'
     1 //)
 1010 FORMAT(I7,' random arguments were tested from the interval ',
     1 '(',F5.1,',',F5.1,')',//)
 1011 FORMAT(' ABS(PSI(X)) was larger',I6,' times', /
     1     21X,' agreed',I6,' times, and'/
     1   17X,'was smaller',I6,' times.'//)
 1020 FORMAT(' There are',I4,' base',I4,
     1    ' significant digits in a floating-point number.' //)
 1021 FORMAT(' The maximum relative error of',E15.4E3,3H = ,I4,3H **,
     1  F7.2/4X,'occurred for X =',E13.6E3)
 1022 FORMAT(' The estimated loss of base',I4,
     1  ' significant digits is',F7.2//)
 1023 FORMAT(' The root mean square relative error was',E15.4E3,
     1    3H = ,I4,3H **,F7.2)
 1030 FORMAT('1Special Tests'//)
 1031 FORMAT(' Accuracy near positive zero'//' PSI(',E14.7E3,') = ',
     1    E24.17E3/13X,'Loss of base',I3,' digits = ',F7.2/)
 1033 FORMAT(//' Test with extreme arguments'/)
 1034 FORMAT(' PSI will be called with the argument ',E17.10E3/
     1     ' This may stop execution.'/)
 1035 FORMAT(' PSI will be called with the argument ',E17.10E3/
     1     ' This should not stop execution.'/)
 1036 FORMAT(' PSI returned the value',E25.17E3//)
 1037 FORMAT(//' Test of error returns'//)
 1100 FORMAT(' This concludes the tests.')
C---------- Last card of PSI test program ----------
      END


!---------------------------------------------------
!   Applies boundary conds to prognostic variables
!              Jonathan L. Mitchell
!             Rodrigo Caballero Augi
!---------------------------------------------------

subroutine bc(jm, km, theta, U, V, q, psi, &
             theta_bc, U_bc, V_bc, q_bc, psi_bc)

implicit none

! In
integer :: jm, km

! In
real,dimension(jm,km) :: theta, U, V, q, psi

! Out
real,dimension(0:jm+1,0:km+1) :: theta_bc, U_bc, V_bc, q_bc, psi_bc

! Boundary condition for theta
theta_bc(1:jm,1:km) = theta
theta_bc(   0,   :) = theta_bc( 2, :)
theta_bc(jm+1,   :) = theta_bc(jm-1, :)
theta_bc(   :,   0) = theta_bc( :, 1)
theta_bc(   :,km+1) = theta_bc( :,km)

! Boundary condition for q
q_bc(1:jm,1:km) = q
q_bc(   0,   :) = q_bc( 2, :)
q_bc(jm+1,   :) = q_bc(jm, :)
q_bc(   :,   0) = q_bc( :, 1)
q_bc(   :,km+1) = q_bc( :,km)

! Boundary condition for U
U_bc(  1:jm,1:  km) = U
U_bc(   0,       :) = U_bc( 1,   :)
U_bc(jm+1,       :) = U_bc(jm, :)
U_bc(   :,    km+1) = U_bc( :,  km)
U_bc(   :,       0) = U_bc( :,   1)

! Boundary condition for V
V_bc(  1:jm,1:  km) = V
V_bc(   0,       :) = -V_bc( 1,   :)
V_bc(jm+1,       :) = -V_bc(jm, :)
V_bc(   :,    km+1) = -V_bc( :,  km)
V_bc(   :,       0) = -V_bc( :,   1)

! Antisymmetric boundary conds (ensures psi=0. on boundary)
psi_bc(1:jm,1:km) = psi
psi_bc(0,   :) = 0.!-psi_bc(1, :)
psi_bc(jm+1,:) = 0.!-psi_bc(jm,:)
psi_bc(:,   0) = 0.!-psi_bc(:, 1)
psi_bc(:,km+1) = 0.!-psi_bc(:,km)

end subroutine bc


C  =====================================================================
C  pgm: SHCDAT .. Check date (yr-mon-day) for correctness (2-digit year)
C
C  use:     CALL SHCDAT(LYEAR,LMON,LDAY,KSTAT)
C
C   in: LYEAR ...... year number (1753-2199) - INT
C   in: LMON ....... month number (1-12) - INT
C   in: LDAY ....... day number (1-31) - INT
C  out: KSTAT ...... status of given date - INT
C  out:                0 = no error
C  out:                1 = year number error
C  out:                2 = month number error
C  out:                3 = day number error
C  =====================================================================
      SUBROUTINE SHCDAT(LYEAR,LMON,LDAY,KSTAT)

      INTEGER        LYEAR,LMON,LDAY,KSTAT,NODIM,NOD(12)
C
C    ================================= RCS keyword statements ==========
      CHARACTER*68     RCSKW1,RCSKW2
      DATA             RCSKW1,RCSKW2 /                                 '
     .$Source: /fs/hseb/ob5/rfc/ofs/src/shefpars_driv/RCS/shcdat.f,v $
     . $',                                                             '
     .$Id: shcdat.f,v 1.3 1998/04/07 19:11:49 page Exp $
     . $' /
C    ===================================================================
C

      DATA       NOD / 31,29,31,30,31,30,31,31,30,31,30,31 /

C                   Check for correct year, then month, then day

        IF (LYEAR.LT.1753 .OR. LYEAR.GT.2199) THEN
            KSTAT = 1
        ELSEIF (LMON.LT.1 .OR. LMON.GT.12) THEN
            KSTAT = 2
        ELSE
          NODIM = NOD(LMON)
          IF (LMON .EQ. 2) THEN
            NODIM = NODIM+LYEAR/4-(LYEAR+3)/4
            IF (LYEAR .EQ. 1900) NODIM = 28
            IF (LYEAR .EQ. 1800) NODIM = 28
            IF (LYEAR .EQ. 2100) NODIM = 28
          ENDIF
          IF (LDAY.LT.1 .OR. LDAY.GT.NODIM) THEN
            KSTAT = 3
          ELSE
            KSTAT = 0
          ENDIF
        ENDIF

      RETURN
      END


subroutine lev_cvgunk(a,itask)
   use typre
   use Mod_LevelSet
   use def_parame
   use MPI
   implicit none
   class(LevelSetProblem) :: a
   integer(ip) :: itask

   integer(ip), save       :: ipass=0
   real(rp)                :: ta,to,numer,denom,rilev
   integer(ip)             :: npoinLocal
   integer                 :: ierr

   select case(itask)

   !Check convergence of the inner iterations, always in the L2 norm:
   !|| L(n,i,j) - L(n,i,j-1)|| / ||L(n,i,j)||
   case(1)
      call a%Mesh%GetNpoinLocal(npoinLocal)
      call vecresMPI(a%kfl_normc,npoinLocal,a%unkno,a%level(1,1),rilev,zelev,a%kfl_MPIComType,a%MPIcomm,a%MPIroot,a%MPIrank,a%MPIsize)

      if (a%MPIrank == a%MPIroot) then
         if(ipass==0) then
            ipass=1
            write(a%lun_conve,100)
         end if
         write(a%lun_conve,101) a%istep,a%itera,a%ctime,rilev

         ! Write in the log file.
         if (a%kfl_flush == 1) call flush(a%lun_conve)
      endif

      if((rilev<a%cotol).or.(a%itera>=a%maxit)) a%kfl_goite = 0

      CALL MPI_BCAST(a%kfl_goite, 1, MPI_INTEGER4, a%MPIroot, a%MPIcomm, ierr)

   !Check convergence of the outer iterations in the norm selected by the user:
   !|| L(n,i,*) - L(n,i-1,*)|| / ||L(n,i,*)||
   case(2)
      call a%Mesh%GetNpoinLocal(npoinLocal)
      call vecresMPI(a%kfl_normc,npoinLocal,a%level(1,1),a%level(1,2),rilev,zelev,a%kfl_MPIComType,a%MPIcomm,a%MPIroot,a%MPIrank,a%MPIsize)

   !Check residual of the time evolution, always in the L2 norm:
   !|| L(n,*,*) - L(n-1,*,*)|| / ||L(n,*,*)||
   case(3)
      call a%Mesh%GetNpoinLocal(npoinLocal)
      call vecresMPI(two,npoinLocal,a%level(1,1),a%level(1,3),rilev,zelev,a%kfl_MPIComType,a%MPIcomm,a%MPIroot,a%MPIrank,a%MPIsize)

      if (a%MPIrank == a%MPIroot) then
         write(a%lun_conve,101) a%istep,0,a%ctime,rilev
         if(rilev<=a%sstol.and.a%kfl_conbc==1) then
            a%kfl_stead = 1
            write(a%lun_conve,102) a%istep
         end if
      endif
      CALL MPI_BCAST(a%kfl_stead, 1, MPI_INTEGER4, a%MPIroot, a%MPIcomm, ierr)

   end select
   !
   ! Formats
   !
   100 format('$ ','       Time','      Inner','       Current','   LevelSet',/,&
            & '$ ','       step','  iteration','          time','      residual')
   101 format(4x,i9,2x,i9,10(2x,e12.6))
   102 format('$ >>>  LEVELSET IS STATIONARY AT TIME STEP ',i5)

end subroutine


module type_SphericalUnitVectors

    use spherepack_precision, only: &
        wp, & ! working precision
        ip ! integer precision

    use type_SphericalGrid, only: &
        SphericalGrid

    use type_TrigonometricTable, only: &
        TrigonometricTable

    use type_Vector3D, only: &
        Vector => Vector3D, &
        assignment(=), &
        operator(*)

    ! Explicit typing only
    implicit none

    ! Everything is private unless stated otherwise
    private
    public :: SphericalUnitVectors

    type, public :: SphericalUnitVectors
        ! Type components
        logical,                   public :: initialized = .false.
        integer(ip),               public :: NUMBER_OF_LONGITUDES = 0
        integer(ip),               public :: NUMBER_OF_LATITUDES = 0
        type(Vector), allocatable, public :: radial(:, :)
        type(Vector), allocatable, public :: polar(:, :)
        type(Vector), allocatable, public :: azimuthal(:, :)
    contains
        ! Type-bound procedures
        procedure, public :: create => create_spherical_unit_vectors
        procedure, public :: destroy => destroy_spherical_unit_vectors
        procedure, public :: get_spherical_angle_components
        procedure, public :: get_vector_function
    end type SphericalUnitVectors

    ! Declare user-defined constructor
    interface SphericalUnitVectors
        module procedure spherical_unit_vectors_constructor
    end interface

contains

    function spherical_unit_vectors_constructor(grid) &
        result (return_value)

        ! Dummy arguments
        class(SphericalGrid), intent(inout) :: grid
        type(SphericalUnitVectors)          :: return_value

        call return_value%create(grid)

    end function spherical_unit_vectors_constructor

    subroutine create_spherical_unit_vectors(self, grid)

        ! Dummy arguments
        class(SphericalUnitVectors), intent(inout) :: self
        class(SphericalGrid),        intent(inout) :: grid

        ! Local variables
        integer(ip)                  :: i, j ! Counters
        type(TrigonometricTable) :: trig_func

        ! Check if object is usable
        call self%destroy()

        ! Check if polymorphic argument is usable
        if (.not.grid%initialized) then
            error stop 'Object of class(SphericalUnitVectors): '&
                //'uninitialized polymorphic argument of class(SphericalGrid) '&
                //'in create_spherical_unit_vectors'
        end if

        associate (&
            nlat => grid%NUMBER_OF_LATITUDES, &
            nlon => grid%NUMBER_OF_LONGITUDES &
           )

            ! Set constants
            self%NUMBER_OF_LATITUDES = nlat
            self%NUMBER_OF_LONGITUDES = nlon

            !  Allocate memory
            allocate(self%radial(nlat, nlon))
            allocate(self%polar(nlat, nlon))
            allocate(self%azimuthal(nlat, nlon))

            ! Compute required trigonometric functions
            trig_func = TrigonometricTable(grid)

            ! Compute spherical unit vectors
            associate (&
                r => self%radial, &
                theta => self%polar, &
                phi => self%azimuthal, &
                sint => trig_func%sint, &
                cost => trig_func%cost, &
                sinp => trig_func%sinp, &
                cosp => trig_func%cosp &
               )

                do j = 1, nlon
                    do i = 1, nlat

                        ! set radial unit vector
                        r(i, j) = &
                            Vector(&
                            sint(i) * cosp(j), &
                            sint(i) * sinp(j), &
                            cost(i) &
                           )

                        ! set polar unit vector
                        theta(i, j) = &
                            Vector( &
                            cost(i) * cosp(j), &
                            cost(i) * sinp(j), &
                            -sint(i) &
                           )

                        ! set azimuthal unit vector
                        phi(i, j) = &
                            Vector( &
                            -sinp(j), &
                            cosp(j), &
                            0.0_wp &
                           )
                    end do
                end do
            end associate
        end associate

        ! Set flag
        self%initialized = .true.

    end subroutine create_spherical_unit_vectors

    subroutine destroy_spherical_unit_vectors(self)

        ! Dummy arguments
        class(SphericalUnitVectors), intent(inout) :: self

        ! Check flag
        if (.not.self%initialized) return

        ! Release memory
        if (allocated(self%radial)) deallocate (self%radial)
        if (allocated(self%polar)) deallocate (self%polar)
        if (allocated(self%azimuthal)) deallocate (self%azimuthal)

        ! Reset constants
        self%NUMBER_OF_LONGITUDES = 0
        self%NUMBER_OF_LATITUDES = 0

        ! Reset initialization flag
        self%initialized = .false.

    end subroutine destroy_spherical_unit_vectors

    subroutine get_spherical_angle_components(self, &
        vector_function, polar_component, azimuthal_component)

        ! Dummy arguments
        class(SphericalUnitVectors), intent(inout) :: self
        real(wp),                    intent(in)    :: vector_function(:, :, :)
        real(wp),                    intent(out)   :: polar_component(:, :)
        real(wp),                    intent(out)   :: azimuthal_component(:, :)

        ! Local variables
        integer(ip)  :: k, l ! Counters
        type(Vector) :: vector_field ! To cast array to vector

        ! Check if object is usable
        if (.not.self%initialized) then
            error stop 'Object of class(SphericalUnitVectors): '&
                //'uninitialized object in get_spherical_angle_components'
        end if

        ! Calculate the spherical angle components
        associate (&
            nlat => self%NUMBER_OF_LATITUDES, &
            nlon => self%NUMBER_OF_LONGITUDES &
           )
            do l = 1, nlon
                do k = 1, nlat
                    ! Cast array to vector
                    vector_field = vector_function(:, k, l)
                    associate (&
                        theta => self%polar(k, l), &
                        phi => self%azimuthal(k, l) &
                       )
                        ! set the theta component
                        polar_component(k, l) = theta.dot.vector_field
                        ! set the azimuthal_component
                        azimuthal_component(k, l) = phi.dot.vector_field
                    end associate
                end do
            end do
        end associate

    end subroutine get_spherical_angle_components

    subroutine get_vector_function(self, &
        radial_component, polar_component, azimuthal_component, vector_function)

        ! Dummy arguments
        class(SphericalUnitVectors), intent(inout) :: self
        real(wp),                    intent(in)    :: radial_component(:, :)
        real(wp),                    intent(in)    :: polar_component(:, :)
        real(wp),                    intent(in)    :: azimuthal_component(:, :)
        real(wp),                    intent(out)   :: vector_function(:, :, :)

        ! Local variables
        integer(ip)  :: k, l ! Counters

        ! Check if object is usable
        if (.not.self%initialized) then
            error stop 'TYPE(SphericalUnitVectors): '&
                //'uninitialized object in GET_VECTOR_FUNCTION'
        end if

        associate (&
            nlat => self%NUMBER_OF_LATITUDES, &
            nlon => self%NUMBER_OF_LONGITUDES &
           )
            do l = 1, nlon
                do k = 1, nlat
                    associate (&
                        r => self%radial(k, l), &
                        theta => self%polar(k, l), &
                        phi => self%azimuthal(k, l) &
                       )

                        !  Calculate the spherical angle components
                        vector_function(:, k, l) = &
                            r * radial_component(k, l) &
                            + theta * polar_component(k, l) &
                            + phi * azimuthal_component(k, l)

                    end associate
                end do
            end do
        end associate

    end subroutine get_vector_function

end module type_SphericalUnitVectors


program main
  implicit none
  real  :: year
  real  :: c, r, anchor, dd

! Initialize the functions
  interface
     function leapy(yr)
       logical           :: leapy
       real, intent(in)  :: yr
     end function leapy
  end interface

  interface
     subroutine dday(yr, c, r, anchor, dd)
       real, intent(in)   :: yr
       real, intent(out)  :: c, r, anchor, dd
     end subroutine dday
  end interface

  read *, year

  print *, leapy(year)

  call dday(year, c, r, anchor, dd)

  print *, c, r, anchor, dd

end program main


	subroutine lubksb0(A, n, np, indx, b)
!***********************************************************************
!  Copyright, 1993, 2004,  The  Regents of the University of California.
!  This program was prepared by the Regents of the University of
!  California at Los Alamos National Laboratory (the University) under
!  contract No. W-7405-ENG-36 with the U.S. Department of Energy (DOE).
!  All rights in the program are reserved by the DOE and the University.
!  Permission is granted to the public to copy and use this software
!  without charge, provided that this Notice and any statement of
!  authorship are reproduced on all copies. Neither the U.S. Government
!  nor the University makes any warranty, express or implied, or
!  assumes any liability or responsibility for the use of this software.
C***********************************************************************
CD1
CD1  PURPOSE
CD1
CD1  To solve the set of n linear equations A*x = b.
CD1
C***********************************************************************
CD2
CD2  REVISION HISTORY
CD2
CD2 $Log:   /pvcs.config/fehm90/src/lubksb0.f_a  $
!D2
!D2    Rev 2.5   06 Jan 2004 10:43:28   pvcs
!D2 FEHM Version 2.21, STN 10086-2.21-00, Qualified October 2003
!D2
!D2    Rev 2.4   29 Jan 2003 09:10:00   pvcs
!D2 FEHM Version 2.20, STN 10086-2.20-00
!D2
!D2    Rev 2.3   14 Nov 2001 13:10:52   pvcs
!D2 FEHM Version 2.12, STN 10086-2.12-00
!D2
!D2    Rev 2.2   06 Jun 2001 13:25:14   pvcs
!D2 FEHM Version 2.11, STN 10086-2.11-00
!D2
!D2    Rev 2.1   30 Nov 2000 12:04:58   pvcs
!D2 FEHM Version 2.10, STN 10086-2.10-00
!D2
!D2    Rev 2.0   Fri May 07 14:43:22 1999   pvcs
!D2 FEHM Version 2.0, SC-194 (Fortran 90)
CD2
CD2    Rev 1.4   Fri Feb 16 10:04:28 1996   zvd
CD2 Modified requirement.
CD2
CD2    Rev 1.3   Thu Jan 18 09:53:50 1996   zvd
CD2 Modifications to prolog
CD2
CD2    Rev 1.2   Wed Jan 10 14:17:34 1996   hend
CD2 Added Prolog
CD2
CD2    Rev 1.1   03/18/94 15:57:46   gaz
CD2 Added solve_new and cleaned up memory management.
CD2
CD2    Rev 1.0   01/20/94 10:25:28   pvcs
CD2 original version in process of being certified
CD2
C***********************************************************************
CD3
CD3  REQUIREMENTS TRACEABILITY
CD3
CD3  Not Applicable. This is a general utility routine used in the code
CD3  as needed for solving a matrix.
CD3
C***********************************************************************
CD4
CD4  SPECIAL COMMENTS AND REFERENCES
CD4
CD4  Taken from Numerical Recipes, section 2.3.
CD4
CD4  Solves the set of n linear equations A*x=b.  Here A is input, not
CD4  as the matrix A but rather as its LU decomposition, determined by
CD4  routine ludcmp. indx is input as the permutation vector returned by
CD4  ludcmp. b is input as the right-hnd side vector b, and returns with
CD4  the solution vector x.  A, n, np, and indx are not modified by this
CD4  routine and can be left in place for successive calls with differnt
CD4  right-hand sides b. This routine takes into account the possibility
CD4  that b will begin with many zero elements, so it is efficient for
CD4  use in matrix inversion.
CD4
C***********************************************************************
CD5
CD5 INTERFACES
CD5
CD5 Formal Calling Parameters
CD5
CD5   Identifier      Type     Use  Description
CD5
CD5   A               REAL*8   I    LU decomposition of A
CD5   b               REAL*8   I/O  Vector, input right-hand side, output
CD5                                   solution vector
CD5   indx            INT      I    Permutation vector returned by ludcmp0
CD5   n               INT      I    Number of elements in b
CD5   np              INT      I    Number of elements in A
CD5
CD5 Interface Tables
CD5
CD5   None
CD5
CD5 Files
CD5
CD5   None
CD5
C***********************************************************************
CD6
CD6 GLOBAL OBJECTS
CD6
CD6 Global Constants
CD6
CD6   None
CD6
CD6 Global Types
CD6
CD6   None
CD6
CD6 Global Variables
CD6
CD6   None
CD6
CD6 Global Subprograms
CD6
CD6   None
CD6
C***********************************************************************
CD7
CD7 LOCAL IDENTIFIERS
CD7
CD7 Local Constants
CD7
CD7   None
CD7
CD7 Local Types
CD7
CD7   None
CD7
CD7 Local variables
CD7
CD7   Identifier      Type     Description
CD7
CD7   i               INT      Loop index
CD7   ii              INT      Index of first nonvanishing element of b
CD7   j               INT      Loop index
CD7   ll              INT      Element index
CD7   sum             REAL*8   Modified element value
CD7
CD7 Local Subprograms
CD7
CD7   None
CD7
C***********************************************************************
CPS
CPS PSEUDOCODE
CPS
CPS BEGIN lubksb0
CPS
CPS   set the index for the first nonzero element of b to 0
CPS   FOR each element of b [do forward substitution]
CPS       set the index, initial sum, initial value of b
CPS       IF the nonzero element index is set
CPS          FOR each element from the first nonzero b to the previous b
CPS              compute the new sum
CPS          END FOR
CPS       ELSE IF the sum (element value) is not zero
CPS          set the index for the first nonzero element of b
CPS       END IF
CPS       set value of b for this element to sum
CPS   END FOR
CPS
CPS   FOR each element of b [do backward substitution]
CPS       set the initial sum
CPS       IF this isn't the last element
CPS          FOR each element from the previous element to the last
CPS              compute the new sum
CPS          END FOR
CPS       END IF
CPS       set value of b for this element to sum over diagonal A of
CPS        that element
CPS   END FOR
CPS
CPS END lubksb0
CPS
C***********************************************************************

 	implicit none

	integer np,n,i,ii,indx(n),j,ll
        real*8 A(np,np),b(n),sum

c  When ii is set to a positive value, it will become the index of the
c  first nonvanishing element of b.  We now do the forward substitution,
c  equation 2.3.6.  The only new wrinkle is to unscramble the
c  permutation as we go.
	ii = 0
	do i = 1, n
	   ll = indx(i)
	   sum = b(ll)
	   b(ll) = b(i)
	   if (ii .ne. 0) then
	      do j = ii, i-1
		 sum = sum - A(i,j)*b(j)
	      enddo
	   else if (sum .ne. 0) then
c  A nonzero element was encountered, so from now on we will have to do
c  the sums in the loop above.
	      ii = i
	   end if
	   b(i) = sum
	enddo

c  Now we do the backsubstitution, equation 2.3.7.
	do i = n, 1, -1
	   sum = b(i)
	   if (i .lt. n) then
	      do j = i+1, n
		 sum = sum - A(i,j)*b(j)
	      enddo
	   end if
c  Store a component of the solution vector x.
	   b(i) = sum/A(i,i)
	enddo

	return
	end


module Adder

use Types_mod
contains
function adder_func(a, b) result(out)

    implicit none

    !Inputs
    real(kind=SP), intent(in) :: a
    real(kind=SP), intent(in) :: b

    !Outputs
    real(kind=SP) :: out

    out = a + b

end function adder_func

end module Adder


c=======================================================================
c
c     Workspace for APT methods
c
c     GWCALAPT : CALAPT workspace
c     GWAPTVOL : APTVOL workspace
c     GWAPTCST : APTCST workspace
c
c=======================================================================
c
c     GWCALAPT, get the size of the workspace needed by  CALAPT
c
c-----------------------------------------------------------------------
      SUBROUTINE gwcalapt ( ndate, nasset, nfact, siwork, sdwork )
c-----------------------------------------------------------------------
c     INPUT
c            ndate  : number of dates                            integer
c            nasset : size of assets                             integer
c            nfact  : number of factors                          integer
c
c     OUTPUT
c            siwork : size of integer workspace                  integer
c            sdwork : size of double precision workspace         integer
c-----------------------------------------------------------------------
      IMPLICIT NONE
      INTEGER ndate, nasset, nfact, siwork, sdwork
      siwork = nfact+1
      sdwork = (nfact + 1)*( ndate + 2*nasset + nfact + 2 )
      RETURN
      END
c=======================================================================
c
c     GWAPTVOL, get the size of the workspace needed by  APTVOL
c
c-----------------------------------------------------------------------
      SUBROUTINE gwaptvol ( ndate, nasset, nfact, siwork, sdwork )
c-----------------------------------------------------------------------
c     INPUT
c            ndate  : number of dates                            integer
c            nasset : size of assets                             integer
c            nfact  : number of factors                          integer
c
c     OUTPUT
c            siwork : size of integer workspace                  integer
c            sdwork : size of double precision workspace         integer
c
c-----------------------------------------------------------------------
      IMPLICIT NONE
      INTEGER ndate, nasset, nfact, siwork, sdwork
      siwork = nfact+1
      sdwork = (nfact+1)*( ndate + 2*nasset + nfact + 2 ) + ndate*nasset
      RETURN
      END
c=======================================================================
c
c     GWAPTCST, get the size of the workspace needed by  APTCST
c
c-----------------------------------------------------------------------
      SUBROUTINE gwaptcst ( ndate, nfact, neq, nin, siwork, sdwork )
c-----------------------------------------------------------------------
c     INPUT
c            ndate  : number of dates                            integer
c            nfact  : number of factors                          integer
c            neq    : number of equality constraints             integer
c            nin    : number of inequality constraints           integer
c
c     OUTPUT
c            siwork : size of integer workspace                  integer
c            sdwork : size of double precision workspace         integer
c-----------------------------------------------------------------------
      IMPLICIT NONE
      INTEGER ndate, nfact, neq, nin, siwork, sdwork, p
      p = nfact + 1
      siwork = 3*p + 2*nin + neq + 1
      sdwork = p*(2*p + ndate + 9) + 3*nin + neq
      RETURN
      END


C
C************************* CGSOLV ***********************************
C
      subroutine cgsolv(nequ,nterm,ndir,niaux,nraux,iparm,noddir,
     1                  ia,ja,iaux,tolcg,aux,sysmat,rhs,sol)
c
c
c  driver for symmetric conjugate gradient method
c  (cfr. Gambolati 1994, page 148)
c
c  uses the symmetric CSR storage scheme for the sparse matrix
c
c  Exchange parameters
c  *******************
c
c  Integer scalars
c  ***************
c
c nequ         = dimension of system matrix
c nterm        = # of nonzero elements in system matrix
c ndir         = # of equations to exclude in the calculation of the
c                residual
c niaux        = dimension of iaux >= ntermp+nequ+1
c nraux        = dimension of aux >= ntermp+5*nequ
c                N.B. ntermp >= nterm is used when the preconditioner
c                has a number of nonzero elements larger than
c                the system matrix (e.g. AINV preconditioner)
c                Usually ntermp=nterm.
c
c Integer arrays
c **************
c
c iparm(10)    = array of control parameters (see local integer scalars)
c noddir(ndir) = equation numbers to be excluded in the calculation of
c                the residual
c ia(nequ+1)   = pointer to diagonal element of row for system matrix
cxcx ja(nterm)    = column index for system matrix
c ja(N1*N)     = column index for system matrix
c iaux(niaux)  = scratch array
c
c
c Real*8 scalars
c **************
c
c tolcg        = tolerance for relative residual
c
c
c Real*8 arrays
c *************
c
c aux(nraux)   = scratch array
c sysmat(nterm)= coefficient matrix
c rhs(nequ)    = right hand side
c sol(nequ)    = solution vector (xk)
c
c
c Local variables
c ***************
c
c Integer scalars
c ***************
c
c ntermp       = # of nonzero elements in preconditioning matrix
c iout         = output of # iterations, and different residuals, etc.
c                (<-iparm(1))
c iprt         = 0 does not print the convergence profile
c                  (n. of iteration, PCG relative residual norm)
c              > 0 prints the convergence profile in unit "iprt"
c                (<-iparm(2))
c iprec        = 0   no preconditioning
c                1   D^-1 preconditioning
c                2   D^-1-norm preconditioning
c                3   Choleski preconditioning
c                    (for compatibility with the nonsymmetric solver
c                     krysol we accept also 30,31,32)
c                4   AINV preconditioning
c                if iprec=3  ntprec=nterm
c                        =0-2 ntprec is not used
c                (<-iparm(3))
c         NB --> if iprec<0 then reuse previously calculated
c                preconditioner
c info         = 0 normal exit (convergence achieved)
c              = 1 imax reached
c                (<-iparm(4))
c iter         = final number of iteration to convergence
c                (<-iparm(5))
c imax         = maximum # of iterations
c                (<-iparm(6))
c isol         = 0 --> SOL      is the initial solution for pcg
c              = 1 --> PREC*RHS is the initial solution for pcg
c                (<-iparm(7))
c iexit        = 0 --> exit on absolute residual
c              = 1 --> exit on |r_k|/|b|
c                (<-iparm(10))
c
c Real*8 scalars
c **************
c
c bnorm        = norm of right hand side
c resini       = norm of initial residual (|b-Ax_0|/|b|)
c resnorm      = norm of residual at final iteration (|r_k|/|b|)
c resreal      = norm of real residual at final iteration (|b-Ax_k|/|b|)
c
      implicit none

      integer  nequ,nterm,ndir,niaux,nraux
cxcx  integer  iparm(10),noddir(ndir),ia(nequ+1),ja(nterm),iaux(niaux)
      integer  iparm(10),noddir(*),ia(*),ja(*),iaux(*)

      integer  ntermp,iout,iprt,iexit,iprec,info,iter,imax,isol
      integer  indiap,indjap,ires,ipk,iscr1,indprec

      integer  i,j

      real*8   tolcg

cxcx  real*8   aux(nraux),sysmat(nterm),rhs(nequ),sol(nequ)
      real*8   aux(*),sysmat(*),rhs(*),sol(*)
      real*8   bnorm,resini,resnorm,resreal

      real*8   normres

      iout = iparm(1)
      iprt = iparm(2)
      iprec = iparm(3)
      if(iprec.ge.30) iprec=iprec/10
      imax = iparm(6)
      isol = iparm(7)
      iexit = iparm(10)
c
c  set pointers to integer scratch array
c
      indiap = 1
      indjap = nequ + 2
c
c  set pointers to real scratch array
c
      ires = 1
      ipk = ires + nequ
      iscr1 = ipk + nequ
      indprec = iscr1 + 3*nequ
c
c  evaluates preconditioner
c
      if (iprec.ge.0)  then
         if(iprec.eq.1) then
            do i=1,nequ
               j=indprec+i-1
               aux(j)=1.0d0/sysmat(ia(i))
            end do
            ntermp=nequ
         else if (iprec.eq.3) then
            call kersh(iout,nequ,nterm,ia,ja,sysmat,aux(indprec))
            do i=1,nequ+1
               iaux(i) = ia(i)
            end do
            do i=1,nterm
               iaux(indjap+i-1) = ja(i)
            end do
            ntermp = nterm
         else
            write(iout,*) ' Wrong iprec value, iprec=',iprec
            stop
         end if
      else
         iprec=-iprec
      end if
      if (isol.ne.0) then
         call precond(iprec,nequ,ntermp,iaux(indiap),iaux(indjap),
     1                aux(indprec),rhs,aux(1),sol)
      end if

c
c  calls the pcg routine
c
      call pcg(iprt,iexit,iprec,imax,nequ,nterm,ntermp,ndir,info,
     1         iter,noddir,ia,ja,iaux(indiap),iaux(indjap),
     2         tolcg,bnorm,resini,resnorm,sysmat,aux(indprec),
     3         rhs,sol,aux(iscr1),aux(ires),aux(ipk))

      iparm(4) = info
      iparm(5) = iter
c
c  calculates real residual
c
      call ressym2(nequ,nterm,ndir,ia,ja,noddir,sysmat,rhs,sol,
     1     aux(ires))
      if (iexit.eq.0) then
         resreal=normres(nequ,ndir,noddir,aux(ires),aux(ipk))
      else
         resreal=normres(nequ,ndir,noddir,aux(ires),aux(ipk))/bnorm
      end if

c
c  prints convergence detailes
c
      write(iout,'(2i5,3(1pe12.5),a)')
     1 info,iter,resini,resnorm,resreal,' <<symmetric solver>>'

      return
      end


module positions
    use parameters
    use cudafor

    implicit none

    public position
contains
    attributes(global) subroutine position(x,y,z,fx,fy,fz,randv,pbc)
        real, intent(in) :: fx(:), fy(:), fz(:)
        real, intent(inout) :: x(:), y(:), z(:)
        real, intent(inout) :: randv(:)
        integer, intent(in), value :: pbc
        ! Local variables
        integer :: i, s, idx

        idx = blockDim%x * (blockIdx%x - 1) + threadIdx%x
        s = blockDim%x * gridDim%x

        do i = idx, np, s
            x(i) = x(i) + (fx(i) * deltat / ktemp) + randv(i)
            y(i) = y(i) + (fy(i) * deltat / ktemp) + randv(i+np)
            z(i) = z(i) + (fz(i) * deltat / ktemp) + randv(i+(2*np))

            if ( pbc == 1 ) then
                x(i) = x(i) - boxl*nint(x(i)/boxl)
                y(i) = y(i) - boxl*nint(y(i)/boxl)
                z(i) = z(i) - boxl*nint(z(i)/boxl)
            end if
        end do
    end subroutine position
end module


! @@name:	target_data.3f
! @@type:	F-free
! @@compilable:	yes
! @@linkable:	no
! @@expect:	success
subroutine gramSchmidt(Q,rows,cols)
integer             ::   rows,cols,  i,k
double precision    :: Q(rows,cols), tmp
      !$omp target data map(Q)
      do k=1,cols
         tmp = 0.0d0
        !$omp target
           !$omp parallel do reduction(+:tmp)
           do i=1,rows
              tmp = tmp + (Q(i,k) * Q(i,k))
           end do
        !$omp end target
          tmp = 1.0d0/sqrt(tmp)
        !$omp target
           !$omp parallel do
           do i=1,rows
               Q(i,k) = Q(i,k)*tmp
           enddo
        !$omp end target
      end do
      !$omp end target data
end subroutine


      SUBROUTINE PHO_SAMP1D(Imode,X_inp,F_inp,F_int,N_dim,X_out)
C***********************************************************************
C
C     Monte Carlo sampling from arbitrary 1d distribution
C     (linear interpolation to improve reproduction of initial function)
C
C     input: Imode          -1  initialization
C                            1  sampling (after initialization)
C            X_inp(N_dim)   array with x values
C            F_inp(N_dim)   array with function values
C            F_int(N_dim)   array with integral
C
C     output:  X_out        sampled value (Imode=1)
C
C                                                 (R.E. 10/99)
C
C***********************************************************************
      IMPLICIT NONE
      SAVE

C  input/output channels
      INCLUDE 'inc/poinou'

      INTEGER Imode , N_dim
      DOUBLE PRECISION X_inp , F_inp , F_int , X_out
      DIMENSION X_inp(N_dim) , F_inp(N_dim) , F_int(N_dim)

C  local variables
      INTEGER i
      DOUBLE PRECISION dum , xi , a , b

C  external functions
      DOUBLE PRECISION DT_RNDM
      EXTERNAL DT_RNDM

      IF ( Imode.EQ.-1 ) THEN

C  initialization

         F_int(1) = 0.D0
         DO i = 2 , N_dim
            F_int(i) = F_int(i-1) + 0.5D0*(F_inp(i)+F_inp(i-1))
     &                 *(X_inp(i)-X_inp(i-1))
         END DO

      ELSE IF ( Imode.EQ.1 ) THEN

C  sample from previously calculated integral

         xi = DT_RNDM(dum)*F_int(N_dim)

         DO i = 2 , N_dim
            IF ( xi.LT.F_int(i) ) THEN
               a = (F_inp(i)-F_inp(i-1))/(X_inp(i)-X_inp(i-1))
               b = F_inp(i) - a*X_inp(i)
               xi = xi - F_int(i-1) + 0.5D0*a*X_inp(i-1)
     &              **2 + b*X_inp(i-1)
               X_out = (SQRT(b**2+2.D0*a*xi)-b)/a
               RETURN
            END IF
         END DO
         X_out = X_inp(N_dim)

      ELSE

C  invalid option Imode

         IF ( LPRi.GT.4 ) WRITE (LO,'(1x,a,i6)')
     &         'PHO_SAMP1D: invalid option Imode: ' , Imode
         X_out = 0.D0

      END IF

      END SUBROUTINE