subroutine CilmPlusRhoH(cilm, gridin, lmax, nmax, mass, d, rho, gridtype, w, &
                        zero, plx, n, dref, exitstatus)
!------------------------------------------------------------------------------
!
!   This routine will compute the potential coefficients associated
!   with the input gridded relief using the method of Wieczorek and
!   Phillips (1998). The input grid must contain the degree-0 term
!   and the computed coefficients will be referenced to the corresonding
!   degree-0 radius. Note that the array plx is optional, and should not
!   be precomputed when memory is an issue (i.e., lmax>360). In contrast
!   to CilmPlus, takes into account lateral variations in density by
!   inputing both a relief grid (GRIDIN) and density grid (RHO).
!
!   Calling Parameters
!
!       IN
!           gridin      Input grid to be transformed to spherical harmonics.
!           lmax        Maximum spherical harmonic degree to compute. For
!                       gridtype 3 and 4, this must be less than or equal
!                       to N/2 - 1.
!           nmax        Order of expansion.
!           mass        Mass of planet.
!           rho         Input grid of the density with same dimensions as
!                       GRIDIN.
!           gridtype    1 = Gauss-Legendre quadrature grid corresponding
!                       to LMAX.
!                       2 = N by N Driscoll and Healy grid corresponding
!                       to LMAX.
!                       3 = N by 2N Driscoll and Healy grid corresponding
!                       to LMAX.
!                       (4 = 2D Cartesian using MakeGrid2D is not implemented).
!
!       OUT
!           cilm        Output spherical harmonic coefficients with
!                       dimensions (2, lmax+1, lmax+1).
!           d           The radius that the coefficients are referenced
!                       to. This parameter corresponds to the degree zero term
!                       of the data.
!
!       OPTIONAL
!           w           Gauss-Legendre points used in the integrations of
!                       dimension lmax+1.
!           zero        Array of dimension lmax+1 that contains the latitudinal
!                       gridpoints used in the Gauss-Legendre quadrature
!                       integration scheme. Only needed when PLX is not
!                       included (determined from a call to SHGLQ).
!           plx         Input array of Associated Legendre Polnomials computed
!                       at the Gauss points (determined from a call to
!                       SHGLQ). If this is not included, then the optional
!                       array zero MUST be inlcuded.
!           N           Number of latitude points in the Driscoll and Healy
!                       grid. Required for Gridtype 2 and 3.
!           dref        The reference radius used to be used when calculating
!                       the spherical harmonic coefficients. If not specified,
!                       this will be set equal to the mean radius of GRIDIN.
!
!       OPTIONAL (OUT)
!           exitstatus  If present, instead of executing a STOP when an error
!                       is encountered, the variable exitstatus will be
!                       returned describing the error.
!                       0 = No errors;
!                       1 = Improper dimensions of input array;
!                       2 = Improper bounds for input variable;
!                       3 = Error allocating memory;
!                       4 = File IO error.
!
!   All units assumed to be SI.
!
!   Copyright (c) 2005-2019, SHTOOLS
!   All rights reserved.
!
!------------------------------------------------------------------------------
    use SHTOOLS, only: SHExpandGLQ, SHExpandDH
    use ftypes

    implicit none

    real(dp), intent(in) :: gridin(:,:), mass, rho(:,:)
    real(dp), intent(in), optional :: w(:), zero(:), plx(:,:), dref
    real(dp), intent(out) :: cilm(:,:,:), d
    integer(int32), intent(in) :: lmax, nmax, gridtype
    integer(int32), intent(in), optional :: n
    integer(int32), intent(out), optional :: exitstatus
    real(dp) :: prod, pi, scalef
    real(dp), allocatable :: cilmn(:,:,:), grid(:,:)
    integer(int32) :: j, l, k, nlat, nlong, astat(2), lmax_dh

    if (present(exitstatus)) exitstatus = 0

    pi = acos(-1.0_dp)

    if (size(cilm(:,1,1)) < 2 .or. size(cilm(1,:,1)) < lmax+1 .or. &
        size(cilm(1,1,:)) < lmax+1) then
        print*, "Error --- CilmPlusRhoH"
        print*, "CILM must be dimensioned as (2, LMAX+1, LMAX+1) where " // &
                "LMAX is ",  lmax
        print*, "Input dimension is ", size(cilm(:,1,1)), size(cilm(1,:,1)), &
                                       size(cilm(1,1,:))
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    end if

    if (gridtype == 4) then
        print*, "Error --- CilmPlusRhoH"
        print*, "GRIDTYPE 4 (Cartesian obtained from MakeGrid2D) is " // &
                "not allowed."
        if (present(exitstatus)) then
            exitstatus = 2
            return
        else
            stop
        end if

    else if (gridtype == 1) then
        if (present(n)) then
            print*, "Error --- CilmPlusRhoH"
            print*, "N can not be present when using GLQ grids."
            if (present(exitstatus)) then
                exitstatus = 5
                return
            else
                stop
            end if

        end if

        if (present(w) .and. present(zero) .and. present(plx)) then
            print*, "Error --- CilmPlusRhoH"
            print*, "For GLQ grids, either W and ZERO or W and PLX " // &
                    "must be present, but not all three."
            if (present(exitstatus)) then
                exitstatus = 5
                return
            else
                stop
            end if

        else if (present(w) .and. present(zero)) then
            if (size(w) < lmax + 1) then
                print*, "Error --- CilmPlusRhoH"
                print*, "W must be dimensioned as (LMAX+1) where LMAX is ", lmax
                print*, "Input dimension is ", size(w)
                if (present(exitstatus)) then
                    exitstatus = 1
                    return
                else
                    stop
                end if

            end if

            if (size(zero) < lmax + 1) then
                print*, "Error --- CilmPlusRhoH"
                print*, "ZERO must be dimensioned as (LMAX+1) " // &
                        "where LMAX is ", lmax
                print*, "Input dimension is ", size(zero)
                if (present(exitstatus)) then
                    exitstatus = 1
                    return
                else
                    stop
                end if

            end if

        else if (present(plx) .and. present(w)) then
            if (size(w) < lmax + 1) then
                print*, "Error --- CilmPlusRhoH"
                print*, "W must be dimensioned as (LMAX+1) where LMAX is ", lmax
                print*, "Input dimension is ", size(w)
                if (present(exitstatus)) then
                    exitstatus = 1
                    return
                else
                    stop
                end if

            end if

            if (size(plx(:,1)) < lmax+1 .or. &
                size(plx(1,:)) < (lmax+1)*(lmax+2)/2) then
                print*, "Error --- CilmPlusRhoH"
                print*, "PLX must be dimensioned as (LMAX+1, " // &
                        "(LMAX+1)*(LMAX+2)/2) where LMAX is ", lmax
                print*, "Input dimension is ", size(plx(:,1)), size(plx(1,:))
                if (present(exitstatus)) then
                    exitstatus = 1
                    return
                else
                    stop
                end if

            end if

        else
            print*, "Error --- CilmPlusRhoH"
            print*, "For GLQ grids, either W and ZERO or W and " // &
                    "PLX must be present"
            if (present(exitstatus)) then
                exitstatus = 5
                return
            else
                stop
            end if

        end if

    else if (gridtype == 2) then
        if (present(w) .or. present(zero) .or. present(plx)) then
            print*, "Error --- CilmPlusRhoH"
            print*, "W, ZERO and PLX can not be present for " // &
                    "Driscoll-Healy grids."
            if (present(exitstatus)) then
                exitstatus = 5
                return
            else
                stop
            end if

        else if (.not. present(N)) then
            print*, "Error --- CilmPlusRhoH"
            print*, "N must be present when GRIDTYPE is 2 or 3."
            if (present(exitstatus)) then
                exitstatus = 5
                return
            else
                stop
            end if

        end if

    else if (gridtype == 3) then
        if (present(w) .or. present(zero) .or. present(plx)) then
            print*, "Error --- CilmPlusRhoH"
            print*, "W, ZERO and PLX can not be present for " // &
                    "Driscoll-Healy grids."
            if (present(exitstatus)) then
                exitstatus = 5
                return
            else
                stop
            end if

        else if (.not. present(N)) then
            print*, "Error --- CilmPlusRhoH"
            print*, "N must be present when GRIDTYPE is 2 or 3."
            if (present(exitstatus)) then
                exitstatus = 5
                return
            else
                stop
            end if

        end if

    else
        print*, "Error --- CilmPlusRhoH"
        print*, "GRIDTYPE must be 2 (GLQ), 3 (NxN) or 4 (Nx2N)"
        print*, "Input value is ", gridtype
        if (present(exitstatus)) then
            exitstatus = 2
            return
        else
            stop
        end if

    end if

    if (gridtype == 1) then
        nlat = lmax + 1
        nlong = 2 * lmax + 1

    else if (gridtype == 2) then
        nlat = N
        nlong = N
        lmax_dh = N / 2 - 1

        if (lmax > lmax_dh) then
            print*, "Error --- CilmPlusRhoH"
            print*, "For Driscoll-Healy grids, LMAX must be less " // &
                    "than or equal to N/2 -1, where N is ", N
            print*, "Input value of LMAX is ", lmax
            if (present(exitstatus)) then
                exitstatus = 2
                return
            else
                stop
            end if

        end if

    else if (gridtype == 3) then
        nlat = N
        nlong = 2 * N
        lmax_dh = N/2 - 1

        if (lmax > lmax_dh) then
            print*, "Error --- CilmPlusRhoH"
            print*, "For Driscoll-Healy grids, LMAX must be less " // &
                    "than or equal to N/2 -1, where N is ", N
            print*, "Input value of LMAX is ", lmax
            if (present(exitstatus)) then
                exitstatus = 2
                return
            else
                stop
            end if

        end if

    end if

    if (size(gridin(1,:)) < nlong .or. size(gridin(:,1)) < nlat .or. &
        size(rho(1,:)) < nlong .or. size(rho(:,1)) < nlat) then
        print*, "Error --- CilmPlusRhoH"
        if (gridtype == 2) then
            print*, "GRIDIN and RHO must be dimensioned as " // &
                    "(LMAX+1, 2*LMAX+1) where LMAX is ", lmax
            print*, "Input dimension of GRIDIN is ", size(gridin(1,:)), &
                                                     size(gridin(:,1))
            print*, "Input dimension of RHO is ", size(rho(1,:)), size(rho(:,1))
            if (present(exitstatus)) then
                exitstatus = 1
                return
            else
                stop
            end if

        else if (gridtype == 3) then
            print*, "GRIDIN and RHO must be dimensioned as " // &
                    "(N, N) where N is ", n
            print*, "Input dimension of GRIDIN is ", size(gridin(1,:)), &
                                                     size(gridin(:,1))
            print*, "Input dimension of RHO is ", size(rho(1,:)), size(rho(:,1))
            if (present(exitstatus)) then
                exitstatus = 1
                return
            else
                stop
            end if

        else if (gridtype == 4) then
            print*, "GRIDIN and RHO must be dimensioned as " // &
                    "(N, 2N) where N is ", n
            print*, "Input dimension of GRIDIN is ", size(gridin(1,:)), &
                                                     size(gridin(:,1))
            print*, "Input dimension of RHO is ", size(gridin(1,:)), &
                                                  size(gridin(:,1))
            if (present(exitstatus)) then
                exitstatus = 2
                return
            else
                stop
            end if

        end if

    end if

    allocate (cilmn(2, lmax+1, lmax+1), stat=astat(1))
    allocate (grid(nlat, nlong), stat=astat(2))
    if (astat(1) /= 0 .or. astat(2) /= 0) then
        print*, "Error --- CilmPlusRhoH"
        print*, "Problem allocating arrays CILMN and GRID", astat(1), astat(2)
        if (present(exitstatus)) then
            exitstatus = 3
            return
        else
            stop
        end if

    end if

    !--------------------------------------------------------------------------
    !
    !   Do the expansion.
    !
    !--------------------------------------------------------------------------
    grid(1:nlat, 1:nlong) = gridin(1:nlat,1:nlong)
    cilm = 0.0_dp
    cilmn = 0.0_dp

    ! Determine reference radius, if necessary
    if (present(dref)) then
        d = dref

    else
        select case (gridtype)
            case (1)
                if (present(plx)) then
                    if (present(exitstatus)) then
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, plx = plx, &
                                         norm = 1, csphase = 1, lmax_calc = 0, &
                                         exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, plx = plx, &
                                         norm = 1, csphase = 1, lmax_calc = 0)
                    end if
                else
                    if (present(exitstatus)) then
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, zero=zero, &
                                         norm = 1, csphase = 1, lmax_calc = 0, &
                                         exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, zero=zero, &
                                         norm = 1, csphase = 1, lmax_calc = 0)
                    end if
                end if

            case (2)
                if (present(exitstatus)) then
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 1, csphase = 1, &
                                    lmax_calc = 0, exitstatus = exitstatus)
                    if (exitstatus /= 0) return
                else
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 1, csphase = 1, &
                                    lmax_calc = 0)
                end if

            case (3)
                if (present(exitstatus)) then
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 2, csphase = 1, &
                                    lmax_calc = 0, exitstatus = exitstatus)
                    if (exitstatus /= 0) return
                else
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 2, csphase = 1, &
                                    lmax_calc = 0)
                end if

        end select

        d = cilmn(1,1,1)   ! use mean radius of relief for reference sphere

    end if

    ! Do k = 1 terms first
    grid(1:nlat, 1:nlong) = (gridin(1:nlat,1:nlong)-d) &
                            * rho(1:nlat,1:nlong) / d

    select case (gridtype)
        case (1)
            if (present(plx)) then
                if (present(exitstatus)) then
                    call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                     grid(1:nlat,1:nlong), w, plx = plx, &
                                     norm = 1, csphase = 1, &
                                     exitstatus = exitstatus)
                    if (exitstatus /= 0) return
                else
                    call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                     grid(1:nlat,1:nlong), w, plx = plx, &
                                     norm = 1, csphase = 1)
                end if
            else
                if (present(exitstatus)) then
                    call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                     grid(1:nlat,1:nlong), w, zero=zero, &
                                     norm = 1, csphase = 1, &
                                     exitstatus = exitstatus)
                    if (exitstatus /= 0) return
                else
                    call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                     grid(1:nlat,1:nlong), w, zero=zero, &
                                     norm = 1, csphase = 1)
                end if
            end if

        case (2)
            if (present(exitstatus)) then
                call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                norm = 1, sampling = 1, csphase = 1, &
                                lmax_calc = lmax, exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                norm = 1, sampling = 1, csphase = 1, &
                                lmax_calc = lmax)
            end if

        case (3)
            if (present(exitstatus)) then
                call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                norm = 1, sampling = 2, csphase = 1, &
                                lmax_calc = lmax, exitstatus = exitstatus)
                if (exitstatus /= 0) return
            else
                call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                norm = 1, sampling = 2, csphase = 1, &
                                lmax_calc = lmax)
            end if

    end select

    do l = 0, lmax
        cilm(1:2,l+1,1:l+1) = 4.0_dp * pi * (d**3) * cilmn(1:2,l+1,1:l+1) &
                              / mass / dble(2*l+1)
    end do

    scalef = maxval(abs(gridin(1:nlat,1:nlong) - d))

    do k = 2, nmax
        grid(1:nlat,1:nlong) = rho(1:nlat,1:nlong) &
                               * ((gridin(1:nlat,1:nlong) - d) / scalef)**k
        select case (gridtype)
            case (1)
                if (present(plx)) then
                    if (present(exitstatus)) then
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, plx=plx, &
                                         norm = 1, csphase = 1, &
                                         exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, plx=plx, &
                                         norm = 1, csphase = 1)
                    end if
                else
                    if (present(exitstatus)) then
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, zero=zero, &
                                         norm = 1, csphase = 1, &
                                         exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call SHExpandGLQ(cilmn(1:2,1:lmax+1,1:lmax+1), lmax, &
                                         grid(1:nlat,1:nlong), w, zero=zero, &
                                         norm = 1, csphase = 1)
                    end if
                end if

            case(2)
                if (present(exitstatus)) then
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 1, csphase = 1, &
                                    lmax_calc = lmax, exitstatus = exitstatus)
                    if (exitstatus /= 0) return
                else
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 1, csphase = 1, &
                                    lmax_calc = lmax)
                end if

            case(3)
                if (present(exitstatus)) then
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 2, csphase = 1, &
                                    lmax_calc = lmax, exitstatus = exitstatus)
                    if (exitstatus /= 0) return
                else
                    call SHExpandDH(grid(1:nlat,1:nlong), n, &
                                    cilmn(1:2,1:lmax+1,1:lmax+1), lmax_dh, &
                                    norm = 1, sampling = 2, csphase = 1, &
                                    lmax_calc = lmax)
                end if

        end select

        do l = 0, lmax
            prod = 4.0_dp * pi * (d**3) / mass * (scalef / d)**k

            do j = 1, k
                prod = prod * dble(l+4-j)
            end do

            prod = prod / (dble(2*l+1) * dble(l+3) * dble(fact(k)))

            cilm(1:2,l+1,1:l+1) = cilm(1:2,l+1,1:l+1) &
                                  + cilmn(1:2,l+1,1:l+1) * prod
        end do

    end do

    deallocate(cilmn)
    deallocate(grid)

    CONTAINS

        function fact(i)
        !----------------------------------------------------------------------
        !
        !   This function computes the factorial of an integer.
        !
        !----------------------------------------------------------------------
            implicit none
            integer(int32) :: i, j
            real(dp) :: fact

            if (i == 0) then
                fact = 1.0_dp
            else if (i < 0) then
                print*, "Argument to FACT must be positive"
                if (present(exitstatus)) then
                    exitstatus = 2
                    return
                else
                    stop
                end if

            else
                fact = 1.0_dp

                do j = 1, i
                    fact = fact * j
                end do

            end if

        end function fact

end subroutine CilmPlusRhoH


      subroutine dinverse(a,n,d)
c********************************************
c  Finds inverse of A from cholesky decomposition A=LL'
c  Input: A & d from DCHOLSL
c  Output: A - inverse of covariance matrix
c============================================
      integer n,i,j
      real*8 a(n,n),d(n),sum
      intent(in) d,n
c********************************************
c  Lower diagonal inverse of L in a
      do i=1,n
       a(i,i)=1.d0/d(i)
       do j=i+1,n
        sum=0.d0
        do k=i,j-1
         sum=sum-a(j,k)*a(k,i)
        enddo
        a(j,i)=sum/d(j)
       enddo
      enddo
c------
c  Inverse of LL'
      do i=1,n
       do j=i+1,n
        sum=0.d0
        do k=j,n
         sum=sum+a(k,i)*a(k,j)
        enddo
        a(i,j)=sum
       enddo
c------
c  Diagonal
       sum=0.d0
       do k=i,n
        sum=sum+a(k,i)*a(k,i)
       enddo
        a(i,i)=sum
      enddo
c------
      do i=1,n
       do j=i+1,n
        a(j,i)=a(i,j)
       enddo
      enddo
c----------
      return
      end


c     ##########################################################
c     # 2d LP solver based on Simplex algorithm                #
c     #        for directional limiting of cutcells            #
c     # This solver is based on the allineq form of Simplex    #
c     ##########################################################

      subroutine Simplex_allineq(row_A,A, b, c,
     &     x0,W,output_debug,x,LP_success,my_iters)

c :::::::::::::::::::::: Simplex_allineq :::::::::::::::::::::::::
c
c  INPUT:
c     * row_A: no of rows of matrix A || row_A = number of constraints
c     * A: constraint matrix of LP: A(row_A,2)
c     * b: RHS of LP
c     * c: vector in objective in LP
c     * x0: admissible starting point for LP
c     * W: working set for Simplex method
c     * output_debug: logical whether we should output or not
c
c  OUTPUT:
c     * x: solution of LP
c     * LP_success: was the Simplex algorithm successful?
c     * my_iters: number of iterations needed
c
c :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

c     ::::::::::::::::::::::::::::
c     Declare variables
c     ::::::::::::::::::::::::::::

      implicit none  ! force yourself to properly declare all var

c     Input
      integer row_A
      double precision A(row_A,2), b(row_A), c(2)
      double precision x0(2)
      integer W(2)
      logical output_debug
      integer my_fort

c     Output
      double precision x(2)
      logical LP_success
      integer my_iters ! number of iterations needed

c     Local variables: Optimization variables
c      double precision matr_B(row_A,row_A), B_trans(row_A,row_A) ! columns of A corresponding to index_B
c      double precision matr_N(row_A,2) ! columns of A corresponding to index_N
c      double precision x_B(row_A), c_B(row_A)  ! components of x,c in set B
c      double precision x_N(2), c_N(2)  ! components of x,c in set N
c      double precision pi_vec(row_A)  ! vector pi of Lagrange multipliers
c      double precision s_N(2)  ! Lagrange multiplier s for set N

c     Local variables: other stuff
      integer i,j,k, shift_int, index_min_alpha
      double precision A_k(2,2)
      double precision det, lambda(2),min_lambda
      double precision e_q(2),p_k(2)
      double precision gamma,alpha,test
      integer min_index_W,size_D_k
c      integer ind_perm(row_A)  ! permutation array coming back from solving matrix
c      integer INFO ! was subroutine of LAPACK successful?
c      double precision product, max_step_dir, min_comp ! help variables
c      integer index_N_to_B ! index in set N chosen to go to set B
c      integer index_B_to_N ! index in set B chosen to go to set N (ie limits step length)
c      double precision rhs_for_dgesv(row_A) ! help vector
c      double precision step_dir(row_A) ! help vector
c      integer min_comp_index_B, min_index_s_N
c      double precision test_admiss(row_A),max_admiss ! test whether B*x_B-b = 0

 902     format (2(E12.2))
 901     format (A,'\t',F6.3)
 908     format (A,'\t',8(F6.3,'\t'))
 910     format (A,'\t',10(F6.3,'\t'))
 912     format (A,'\t',12(F6.3,'\t'))
 918     format (A,'\t',8(E12.3,'\t'))

c     ::::::::::::::::::::::::::::
c     Print given data
c     ::::::::::::::::::::::::::::

      if (output_debug) then
         write(*,*) "###"
         write(*,*) "Subroutine Simplex_allineq"
         write(*,*) "###"
      endif

      if (output_debug) then
         write (*,*) "%%%%%%%"
         write (*,*) "Given data for LP"
         write (*,*) "%%%%%%%"
         write(*,*) "number of constraints", row_A
         write(*,*) "matrix A"
         do 10 i = 1, row_A
            write(*,902) (A(i,j),j=1,2)
 10      continue
c
         if (row_A .EQ. 10) then
             write(*,910) "b",b
         elseif (row_A .EQ. 12) then
             write(*,912) "b",b
         endif
         write(*,*) "x0:", x0(1), x0(2)
         write(*,*) "W:", W
         write(*,*) "c:", c(1), c(2)
      endif   ! end of  if (output_debug)

c     #########################
c     Beginning of loop
c     #########################

      do 555 k = 1, 20

          if (output_debug) then
              write(*,*) "%%%%%%%%"
              write(*,*) "Start iteration ",k
              write(*,*) "%%%%%%%%"
              write(*,*) "x:", x0(1), x0(2)
              write(*,*) "W:", W
              write(*,*) "---"
          endif

c      Step 1 of Algorithm -----------------------------
c      solve for Lagrange multiplier lambda: A_k^T lambda = c
          A_k(1,1) = A(W(1),1)
          A_k(1,2) = A(W(1),2)
          A_k(2,1) = A(W(2),1)
          A_k(2,2) = A(W(2),2)
          det = A_k(1,1)*A_k(2,2)-A_k(1,2)*A_k(2,1)
          ! added mjb for robustness
          if (det .eq. 0.0) then
             LP_Success = .false.
             return
          endif
          lambda(1) = (1.0d0/det)*(A_k(2,2)*c(1)-A_k(2,1)*c(2))
          lambda(2) = (1.0d0/det)*(-A_k(1,2)*c(1)+A_k(1,1)*c(2))
          if (output_debug) then
             write(*,*) "lambda", lambda
          endif

c     Step 2 of Algorithm --------------------------------
c     Test whether lambda >= 0
         min_lambda = min(lambda(1),lambda(2))
         if (min_lambda .GT. -1.E-12) then
c            write(*,*) "\n ###################### \n"
c            write(*,*) "Have found the optimal x\n"
            x(1) = x0(1)
            x(2) = x0(2)
c
c            if ((x(1) > 1.00005) .AND. (x(2) > 1.00005)) then
c               write(*,*) "both slope > 1", x(1), x(2)
c            endif
            LP_success = .TRUE.
c          write(*,*) "LP_success", LP_success
c            write(25,*) "No-steps", k-1
c            write(25,*) "x", x
            my_iters = k-1
            return
         endif

c         Step 3 of Algorithm -----------------------------------
c         select q with (lambda)_q < 0 as index to leave the working set
c         have 2 choices
c         our choice for now: go with the most negative one
          if (lambda(1).GE.lambda(2)) then
            min_index_W = 2
          else
            min_index_W = 1
          endif
          if (output_debug) then
            write(*,*) "Index which leaves W",W(min_index_W)
          endif
c         or go with the first negative index
c         if (lambda(1) < -1.E-12)
c             min_index_W = 1
c         else
c             min_index_W = 2
c         endif

c         Step 4 of Algorithm ---------------------------------
c         Calculate step direction by solving A_k p_k = e_q
          if (min_index_W .EQ. 1) then
             e_q = (/ 1.0d0, 0.0d0 /)
          else
             e_q = (/ 0.0d0, 1.0d0 /)
          endif
          p_k(1) = (1.0d0/det)*(A_k(2,2)*e_q(1)-A_k(1,2)*e_q(2))
          p_k(2) = (1.0d0/det)*(-A_k(2,1)*e_q(1)+A_k(1,1)*e_q(2))
          if (output_debug) then
             write(*,*) "p_k", p_k
          endif

c         Step 5 of Algorithm ----------------------------------
c         Calculate step length
          size_D_k = 0
          alpha = 100000000.0d0
          index_min_alpha = row_A + 10
          do 20 i=1,row_A
             test = A(i,1)*p_k(1) + A(i,2)*p_k(2)
             if (test .LT. -1.E-12) then
                gamma = -(A(i,1)*x0(1) + A(i,2)*x0(2) - b(i))/test
                if (gamma .LT. (alpha-1.E-12)) then
c               Make sure that you don't take an index in W which is already there
c               Contribution for indices in W should be 0 -- but by numerical error it
c               could happen that the error is bigger than the tested 1.E-12
                   if ((i .ne. W(1)) .AND. (i .ne. W(2))) then
                      size_D_k = size_D_k + 1
                      alpha = gamma
                      index_min_alpha = i
                   endif
                endif
             endif
 20       continue
          if (size_D_k .EQ. 0) then
             write(*,*)  "\n LP unbounded !!!!!!!!!!!!\n"
             LP_success = .FALSE.
             return
          endif
          if (index_min_alpha .GT. row_A) then
             write(*,*)  "\n Problem with index_min_alpha\n"
             LP_success = .FALSE.
             return
          endif
          if (output_debug) then
             write(*,*) "Index which enters W",index_min_alpha
          endif
          ! update x0
          x0(1) = x0(1) + alpha*p_k(1)
          x0(2) = x0(2) + alpha*p_k(2)
          if (output_debug) then
             write(*,*) "new iterate x0", x0
          endif

c         Step 6 of Algorithm ----------------------------------
c         Update working set
c         get rid of the index in position "min_index_W" and put
c         index_min_alpha there
          W(min_index_W) = index_min_alpha

c         write(*,*) "After ",k," steps:", "Phi_x, Phi_y =",x0(1),x0(2)

 555  continue    ! end of main loop

c     #########################
c     End of loop
c     #########################

            x(1) = x0(1)
            x(2) = x0(2)

            my_iters = k-1

      return
      end


      PROGRAM LA_SGELSS_EXAMPLE

!  -- LAPACK95 EXAMPLE DRIVER ROUTINE (VERSION 1.0) --
!     UNI-C, DENMARK
!     DECEMBER, 1999
!
!  .. "Use Statements" ..
      USE LA_PRECISION, ONLY: WP => SP
      USE F95_LAPACK, ONLY: LA_GELSS, LA_LANGE
!  .. "Implicit Statement" ..
      IMPLICIT NONE
!  .. "Local Scalars" .
      REAL(WP) ::  R1, R2, R3
      INTEGER :: RANK, I, J, INFO, M, N, NRHS
!  .. "Local Arrays" ..
      REAL(WP), ALLOCATABLE :: A(:,:), AA(:,:), B(:,:), BB(:,:), S(:)
!  .. "Executable Statements" ..
      WRITE (*,*) 'LA_GELSS Example Program Results'
      M=6; N = 4; NRHS = 3
      WRITE(*,'(5H N = , I4, 9H; NRHS = , I4)') N, NRHS
      ALLOCATE( A(M,N), AA(M,N), B(M,NRHS), BB(M,NRHS), S(N) )

      OPEN(UNIT=21,FILE='gelss.ma',STATUS='UNKNOWN')
      DO I=1,M
         DO J=1,N
            READ(21,*) A(I,J)
         ENDDO
      ENDDO
      CLOSE(21)

      OPEN(UNIT=22,FILE='gelss.mb',STATUS='UNKNOWN')
      DO I=1,M
         DO J=1,NRHS
            READ(22,*) B(I,J)
         ENDDO
      ENDDO
      CLOSE(22)

      WRITE(*,*)'Matrix A :'
      DO I=1,M;
         WRITE(*,"(24(F9.5))") A(I,:);
      ENDDO

      WRITE(*,*)'Matrix B :'
      DO I=1,M;
         WRITE(*,"(3(F9.5))") B(I,:);
      ENDDO

      WRITE(*,*) 'CALL LA_GELSS( A, B, RANK, S, 0.00001_WP, INFO )'
      CALL LA_GELSS( A, B, RANK, S, 0.00001_WP, INFO=INFO )

      WRITE(*,*) ' A on exit : '
      DO I=1,M;
         WRITE(*,"(4(E14.6))") A(I,:);
      ENDDO

      WRITE(*,*) ' B on exit : '
      DO I=1,M;
         WRITE(*,"(3(E14.6))") B(I,:);
      ENDDO

      WRITE(*,*) ' S on exit : '
      DO I=1,N;
         WRITE(*,"(E14.6)") S(I);
      ENDDO

      WRITE(*,*) 'RANK = ', RANK
      WRITE(*,*)'INFO = ',INFO

      R1=LA_LANGE(B(N+1:M,1),NORM='F')
      R2=LA_LANGE(B(N+1:M,2),NORM='F')
      R3=LA_LANGE(B(N+1:M,3),NORM='F')
      WRITE(*,*)'The residual r = ',R1,R2,R3

      END PROGRAM LA_SGELSS_EXAMPLE


SUBROUTINE RK3(NEQ,X0,XE,N,Y0,YN,N1,WORK)
!*********************************************************************
!     SUBROUTINE RK NUMERICALLY INTEGRATES A SYSTEM OF NEQ           *
!     FIRST ORDER ORDINARY DIFFERENTIAL EQUATIONS OF THE FORM        *
!             DY(I)/DX = F(X, Y(1),..., Y(NEQ)),                     *
!     BY THE CLASSICAL RUNGE-KUTTA FORMULA.                          *
!                                                                    *
!     PARAMETERS                                                     *
!  === INPUT ===                                                     *
!     (1) NEQ: NUMBER OF EQUATIONS TO BE INTEGRATED                  *
!     (2) FUNC: SUBROUTINE FUNC(X,Y,F) TO EVALUATE DERIVATIVES       *
!                F(I)=DY(I)/DX                                       *
!     (3) X0: INITIAL VALUE OF INDEPENDENT VARIABLE                  *
!     (4) XE: OUTPUT POINT AT WHICH THE SOLUTION IS DESIRED          *
!     (5) N: NUMBER OF DIVISIONS                                     *
!        THE INTERVAL (X0, XE) IS DIVIDED INTO N SUBINTERVALS        *
!        WITH THE LENGTH (XE-X0)/N AND IN EACH SUBINTERVAL           *
!        THE CLASSICAL RUNGE-KUTTA FORMULA IS USED.                  *
!     (6) Y0(I) (I=1,..,NEQ): INITIAL VALUE AT X0                    *
!  === OUTPUT ===                                                    *
!     (7) YN(I) (I=1,..,NEQ): APPROXIMATE SOLUTION AT XE             *
!  === OTHER ===                                                     *
!     (8) WORK(): TWO-DIMENTIONAL ARRAY (SIZE=(NEQ,2)) TO BE         *
!                 USED INSIDE RK                                     *
!     COPYRIGHT: M. SUGIHARA, NOVEMBER 15, 1989, V. 1                *
! modified: Kensuke Konishi 2018 in Paris 2020 in Taiwan
!*********************************************************************
    use parameters
    implicit none
    !EXTERNAL FUNC
    INTEGER:: NEQ,N,I,J,N1
    double precision:: X0,XE
    complex(dp):: Y0(NEQ),YN(NEQ),WORK(N1,2)
    double precision:: H
    H = (XE - X0) / DBLE(N)
    do I = 1,N
        CALL RKSTEP(NEQ,X0,H,Y0,YN,WORK(1,1),WORK(1,2))
        X0 = X0 + H
        Y0(1:neq) = YN(1:neq)
    enddo
    X0 = XE
    RETURN
    END

SUBROUTINE RKSTEP(NEQ,X,H,Y0,YN,AK,W)
    use parameters
    implicit none

    INTEGER,intent(in):: NEQ
    double precision:: X,H
    complex(dp):: Y0(NEQ),YN(NEQ),AK(NEQ),W(NEQ)

    integer:: i
    double precision:: A2,A3,B2,B3,C1,C2,C3,C4
    PARAMETER(A2 = 0.5D0, A3 = A2)
    PARAMETER(B2 = 0.5D0, B3 = B2)
    PARAMETER(C1 = 1.D0/6.D0, C2 = 1.D0/3.D0, C3 = C2, C4 = C1)
    !EXTERNAL FUNC
    CALL eqmotion1(X,Y0,AK)

    YN(1:NEQ) = Y0(1:NEQ) + DCMPLX( H * C1 ) * AK(1:NEQ)

    W(1:NEQ) = Y0(1:NEQ) + DCMPLX( H * B2 ) * AK(1:NEQ)

    CALL eqmotion1(X + A2 * H,W,AK)

    YN(1:NEQ) = YN(1:NEQ) + DCMPLX( H * C2 ) * AK(1:NEQ)

    W(1:NEQ) = Y0(1:NEQ) + DCMPLX( H * B3 ) * AK(1:NEQ)

    CALL eqmotion1(X + A3 * H,W,AK)
    YN(1:NEQ) = YN(1:NEQ) + DCMPLX( H * C3 ) * AK(1:NEQ)

    W(1:NEQ) = Y0(1:NEQ) + DCMPLX( H ) * AK(1:NEQ)
    CALL eqmotion1(X + H,W,AK)

    YN(1:NEQ) = YN(1:NEQ) + DCMPLX( H * C4 ) * AK(1:NEQ)

    RETURN
    END


module inv
  implicit none

  integer, parameter :: DP=8

contains

!<subroutine>

subroutine invert(Da,Df,Dx,ndim,ipar)

  !<description>

  ! This subroutine performs the direct inversion of a NxN system.
  !
  ! If the parameter ipar=0, then only factorization of matrix A is performed.
  ! For ipar=1, the vector x is calculated using the factorized matrix A.
  ! For ipar=2, LAPACK routine DGESV is used to solve the dense linear system Ax=f.
  ! In addition, for NDIM=2,3,4 explicit formulas are employed to replace the
  ! more expensive LAPACK routine.
  !

  !</description>

  !<input>

  real(DP), dimension(ndim), intent(IN) :: Df
  integer, intent(IN) :: ndim,ipar

  !</input>

  !<inputoutput>

  real(DP), dimension(ndim,ndim), intent(INOUT) :: Da

  !</inputoutput>

  !<output>

  real(DP), dimension(ndim), intent(OUT) :: Dx

  !</output>

!</subroutine>

  ! local variables
  real(DP), dimension(ndim,ndim) :: Db
  real(DP), dimension(ndim) :: Dpiv
  integer, dimension(ndim) :: Kindx,Kindy

  real(DP) :: dpivot,daux
  integer :: idim1,idim2,idim3,ix,iy,indx,indy,info

  select case (ipar)
  case (0)
     ! Perform factorization of matrix Da

     ! Initialization
     Kindx=0;  Kindy=0

     do idim1=1,ndim

        ! Determine pivotal element
        dpivot=0

        do iy=1,ndim
           if (Kindy(iy) /= 0) cycle

           do ix=1,ndim
              if (Kindx(ix) /= 0) cycle

              if (abs(Da(ix,iy)) .le. abs(dpivot)) cycle
              dpivot=Da(ix,iy);  indx=ix;  indy=iy
           end do
        end do

        ! Return if pivotal element is zero
        if (abs(dpivot) .le. 0._DP) return

        Kindx(indx)=indy;  Kindy(indy)=indx;  Da(indx,indy)=1._DP/dpivot

        do idim2=1,ndim
           if (idim2 == indy) cycle
           Da(1:indx-1,   idim2)=Da(1:indx-1,   idim2)-Da(1:indx-1,   indy)*Da(indx,idim2)/dpivot
           Da(indx+1:ndim,idim2)=Da(indx+1:ndim,idim2)-Da(indx+1:ndim,indy)*Da(indx,idim2)/dpivot
        end do

        do ix=1,ndim
           if (ix /= indx) Da(ix,indy)=Da(ix,indy)/dpivot
        end do

        do iy=1,ndim
           if (iy /= indy) Da(indx,iy)=-Da(indx,iy)/dpivot
        end do
     end do

     do ix=1,ndim
        if (Kindx(ix) == ix) cycle

        do iy=1,ndim
           if (Kindx(iy) == ix) exit
        end do

        do idim1=1,ndim
           daux=Da(ix,idim1);  Da(ix,idim1)=Da(iy,idim1);  Da(iy,idim1)=daux
        end do

        Kindx(iy)=Kindx(ix);  Kindx(ix)=ix
     end do

     do ix=1,ndim
        if (Kindy(ix) == ix) cycle

        do iy=1,ndim
           if (Kindy(iy) == ix) exit
        end do

        do idim1=1,ndim
           daux=Da(idim1,ix);  Da(idim1,ix)=Da(idim1,iy);  Da(idim1,iy)=daux
        end do

        Kindy(iy)=Kindy(ix);  Kindy(ix)=ix
     end do

  case (1)
     ! Perform inversion of Da to solve the Da * Dx = Df
     do idim1=1,ndim
        Dx(idim1)=0
        do idim2=1,ndim
           Dx(idim1)=Dx(idim1)+Da(idim1,idim2)*Df(idim2)
        end do
     end do

  case (2)
     ! Solve the dense linear system Ax=f calling LAPACK routine

     select case(ndim)
     case (2)
        ! Explicit formula for 2x2 system
        Db(1,1)= Da(2,2)
        Db(2,1)=-Da(2,1)
        Db(1,2)=-Da(1,2)
        Db(2,2)= Da(1,1)
        daux=Da(1,1)*Da(2,2)-Da(1,2)*Da(2,1)
        Dx=matmul(Db,Df)/daux

     case (3)
        ! Explicit formula for 3x3 system
        Db(1,1)=Da(2,2)*Da(3,3)-Da(2,3)*Da(3,2)
        Db(2,1)=Da(2,3)*Da(3,1)-Da(2,1)*Da(3,3)
        Db(3,1)=Da(2,1)*Da(3,2)-Da(2,2)*Da(3,1)
        Db(1,2)=Da(1,3)*Da(3,2)-Da(1,2)*Da(3,3)
        Db(2,2)=Da(1,1)*Da(3,3)-Da(1,3)*Da(3,1)
        Db(3,2)=Da(1,2)*Da(3,1)-Da(1,1)*Da(3,2)
        Db(1,3)=Da(1,2)*Da(2,3)-Da(1,3)*Da(2,2)
        Db(2,3)=Da(1,3)*Da(2,1)-Da(1,1)*Da(2,3)
        Db(3,3)=Da(1,1)*Da(2,2)-Da(1,2)*Da(2,1)
        daux=Da(1,1)*Da(2,2)*Da(3,3)+Da(2,1)*Da(3,2)*Da(1,3)+ &
             Da(3,1)*Da(1,2)*Da(2,3)-Da(1,1)*Da(3,2)*Da(2,3)- &
             Da(3,1)*Da(2,2)*Da(1,3)-Da(2,1)*Da(1,2)*Da(3,3)
        Dx=matmul(Db,Df)/daux

     case (4)
        ! Explicit formula for 4x4 system
        Db(1,1)=Da(2,2)*Da(3,3)*Da(4,4)+Da(2,3)*Da(3,4)*Da(4,2)+Da(2,4)*Da(3,2)*Da(4,3)- &
             Da(2,2)*Da(3,4)*Da(4,3)-Da(2,3)*Da(3,2)*Da(4,4)-Da(2,4)*Da(3,3)*Da(4,2)
        Db(2,1)=Da(2,1)*Da(3,4)*Da(4,3)+Da(2,3)*Da(3,1)*Da(4,4)+Da(2,4)*Da(3,3)*Da(4,1)- &
             Da(2,1)*Da(3,3)*Da(4,4)-Da(2,3)*Da(3,4)*Da(4,1)-Da(2,4)*Da(3,1)*Da(4,3)
        Db(3,1)=Da(2,1)*Da(3,2)*Da(4,4)+Da(2,2)*Da(3,4)*Da(4,1)+Da(2,4)*Da(3,1)*Da(4,2)- &
             Da(2,1)*Da(3,4)*Da(4,2)-Da(2,2)*Da(3,1)*Da(4,4)-Da(2,4)*Da(3,2)*Da(4,1)
        Db(4,1)=Da(2,1)*Da(3,3)*Da(4,2)+Da(2,2)*Da(3,1)*Da(4,3)+Da(2,3)*Da(3,2)*Da(4,1)- &
             Da(2,1)*Da(3,2)*Da(4,3)-Da(2,2)*Da(3,3)*Da(4,1)-Da(2,3)*Da(3,1)*Da(4,2)
        Db(1,2)=Da(1,2)*Da(3,4)*Da(4,3)+Da(1,3)*Da(3,2)*Da(4,4)+Da(1,4)*Da(3,3)*Da(4,2)- &
             Da(1,2)*Da(3,3)*Da(4,4)-Da(1,3)*Da(3,4)*Da(4,2)-Da(1,4)*Da(3,2)*Da(4,3)
        Db(2,2)=Da(1,1)*Da(3,3)*Da(4,4)+Da(1,3)*Da(3,4)*Da(4,1)+Da(1,4)*Da(3,1)*Da(4,3)- &
             Da(1,1)*Da(3,4)*Da(4,3)-Da(1,3)*Da(3,1)*Da(4,4)-Da(1,4)*Da(3,3)*Da(4,1)
        Db(3,2)=Da(1,1)*Da(3,4)*Da(4,2)+Da(1,2)*Da(3,1)*Da(4,4)+Da(1,4)*Da(3,2)*Da(4,1)- &
             Da(1,1)*Da(3,2)*Da(4,4)-Da(1,2)*Da(3,4)*Da(4,1)-Da(1,4)*Da(3,1)*Da(4,2)
        Db(4,2)=Da(1,1)*Da(3,2)*Da(4,3)+Da(1,2)*Da(3,3)*Da(4,1)+Da(1,3)*Da(3,1)*Da(4,2)- &
             Da(1,1)*Da(3,3)*Da(4,2)-Da(1,2)*Da(3,1)*Da(4,3)-Da(1,3)*Da(3,2)*Da(4,1)
        Db(1,3)=Da(1,2)*Da(2,3)*Da(4,4)+Da(1,3)*Da(2,4)*Da(4,2)+Da(1,4)*Da(2,2)*Da(4,3)- &
             Da(1,2)*Da(2,4)*Da(4,3)-Da(1,3)*Da(2,2)*Da(4,4)-Da(1,4)*Da(2,3)*Da(4,2)
        Db(2,3)=Da(1,1)*Da(2,4)*Da(4,3)+Da(1,3)*Da(2,1)*Da(4,4)+Da(1,4)*Da(2,3)*Da(4,1)- &
             Da(1,1)*Da(2,3)*Da(4,4)-Da(1,3)*Da(2,4)*Da(4,1)-Da(1,4)*Da(2,1)*Da(4,3)
        Db(3,3)=Da(1,1)*Da(2,2)*Da(4,4)+Da(1,2)*Da(2,4)*Da(4,1)+Da(1,4)*Da(2,1)*Da(4,2)- &
             Da(1,1)*Da(2,4)*Da(4,2)-Da(1,2)*Da(2,1)*Da(4,4)-Da(1,4)*Da(2,2)*Da(4,1)
        Db(4,3)=Da(1,1)*Da(2,3)*Da(4,2)+Da(1,2)*Da(2,1)*Da(4,3)+Da(1,3)*Da(2,2)*Da(4,1)- &
             Da(1,1)*Da(2,2)*Da(4,3)-Da(1,2)*Da(2,3)*Da(4,1)-Da(1,3)*Da(2,1)*Da(4,2)
        Db(1,4)=Da(1,2)*Da(2,4)*Da(3,3)+Da(1,3)*Da(2,2)*Da(3,4)+Da(1,4)*Da(2,3)*Da(3,2)- &
             Da(1,2)*Da(2,3)*Da(3,4)-Da(1,3)*Da(2,4)*Da(3,2)-Da(1,4)*Da(2,2)*Da(3,3)
        Db(2,4)=Da(1,1)*Da(2,3)*Da(3,4)+Da(1,3)*Da(2,4)*Da(3,1)+Da(1,4)*Da(2,1)*Da(3,3)- &
             Da(1,1)*Da(2,4)*Da(3,3)-Da(1,3)*Da(2,1)*Da(3,4)-Da(1,4)*Da(2,3)*Da(3,1)
        Db(3,4)=Da(1,1)*Da(2,4)*Da(3,2)+Da(1,2)*Da(2,1)*Da(3,4)+Da(1,4)*Da(2,2)*Da(3,1)- &
             Da(1,1)*Da(2,2)*Da(3,4)-Da(1,2)*Da(2,4)*Da(3,1)-Da(1,4)*Da(2,1)*Da(3,2)
        Db(4,4)=Da(1,1)*Da(2,2)*Da(3,3)+Da(1,2)*Da(2,3)*Da(3,1)+Da(1,3)*Da(2,1)*Da(3,2)- &
             Da(1,1)*Da(2,3)*Da(3,2)-Da(1,2)*Da(2,1)*Da(3,3)-Da(1,3)*Da(2,2)*Da(3,1)
        daux=Da(1,1)*Da(2,2)*Da(3,3)*Da(4,4)+Da(1,1)*Da(2,3)*Da(3,4)*Da(4,2)+Da(1,1)*Da(2,4)*Da(3,2)*Da(4,3)+ &
             Da(1,2)*Da(2,1)*Da(3,4)*Da(4,3)+Da(1,2)*Da(2,3)*Da(3,1)*Da(4,4)+Da(1,2)*Da(2,4)*Da(3,3)*Da(4,1)+ &
             Da(1,3)*Da(2,1)*Da(3,2)*Da(4,4)+Da(1,3)*Da(2,2)*Da(3,4)*Da(4,1)+Da(1,3)*Da(2,4)*Da(3,1)*Da(4,2)+ &
             Da(1,4)*Da(2,1)*Da(3,3)*Da(4,2)+Da(1,4)*Da(2,2)*Da(3,1)*Da(4,3)+Da(1,4)*Da(2,3)*Da(3,2)*Da(4,1)- &
             Da(1,1)*Da(2,2)*Da(3,4)*Da(4,3)-Da(1,1)*Da(2,3)*Da(3,2)*Da(4,4)-Da(1,1)*Da(2,4)*Da(3,3)*Da(4,2)- &
             Da(1,2)*Da(2,1)*Da(3,3)*Da(4,4)-Da(1,2)*Da(2,3)*Da(3,4)*Da(4,1)-Da(1,2)*Da(2,4)*Da(3,1)*Da(4,3)- &
             Da(1,3)*Da(2,1)*Da(3,4)*Da(4,2)-Da(1,3)*Da(2,2)*Da(3,1)*Da(4,4)-Da(1,3)*Da(2,4)*Da(3,2)*Da(4,1)- &
             Da(1,4)*Da(2,1)*Da(3,2)*Da(4,3)-Da(1,4)*Da(2,2)*Da(3,3)*Da(4,1)-Da(1,4)*Da(2,3)*Da(3,1)*Da(4,2)
        Dx=matmul(Db,Df)/daux

     case DEFAULT
        ! Use LAPACK routine for general NxN system, where N>4
        Dpiv=0; Dx=Df
        call DGESV(ndim,1,Da,ndim,Dpiv,Dx,ndim,info)

     end select
  end select
end subroutine invert

end module inv


C ******************** CFLPECNUMBER ****************************************
C  to calculate the CFL number  CFL = Deltat * sup(surface/volume) * sup(dF/du)
C  where F is the function of the advection equation u_t + div(F(u))=0
C  In this case F is a linear function F(u)= v*u
C  and to calculate the Peclet number Pec = CFL/gamma where gamma is the
C  diffusion number gamma=Deltat*sup(|D|/volume)
C  supdiffus is equal to sup (|D|/volume)
C  **********************************************************************
C
      subroutine cflpecnumber(NTETRA,DELTATADV,VOLUR,SURFAR,
     1                      VX,VY,VZ,SUPDIFFUS,cflinp,CFLNUMB,PECNUMB,
     2                      CFLTETRA,NADV,nadvfl,DELTAT)

      implicit none
C
      integer ntetra
      integer j, cfltetra, nadv,nadvfl
      real*8 deltat,deltatadv
      real*8 volur(ntetra),surfar(ntetra)
      real*8 vx(ntetra),vy(ntetra),vz(ntetra)
      real*8 cflinp,cflnumb,pecnumb,supdiffus
      real*8 supcfl,supcflt

      cfltetra=1
      supcfl= surfar(1)*volur(1)*
     1        dsqrt(vx(1)**2+vy(1)**2+vz(1)**2)
c     write(999,*)surfar(1),volur(1)
      do j=2,ntetra
         supcflt= surfar(j)*volur(j)*
     1            dsqrt(vx(j)**2+vy(j)**2+vz(j)**2)
         if (supcflt.gt.supcfl) then
            supcfl=supcflt
            cfltetra=j
         end if
c     write(999,*)surfar(j),volur(j)
      end do
      if (nadvfl.eq.1) then
         deltatadv=deltat/float(nadv)
         cflnumb=deltatadv*supcfl
      else if(nadvfl.eq.0) then
         deltatadv= cflinp/supcfl
         nadv=max(int(deltat/deltatadv)+1,2)
         deltatadv= deltat/nadv
         cflnumb=deltatadv*supcfl
      else
         write(6,*) 'wrong value for nadvfl ',nadvfl
         stop
      end if
      if (supdiffus.eq.0.0d0) then
         pecnumb=1.e+20
      else
         pecnumb=cflnumb/(supdiffus*deltatadv)
      end if

      return
      end


! Program to compare timings between bml wrapper to dgemms
! and dgemms standalone.

program main

  use bml

  implicit none
  integer, parameter :: dp = kind(1.0d0)
  character(20)          ::  bml_type
  integer                ::  hdim, i, j, mdim
  real(8)                ::  mls, mlsI
  real(dp)               ::  alpha, beta, numthresh
  real(dp), allocatable  ::  aux(:,:), diff(:,:), h(:,:), trace(:)
  real(dp), allocatable  ::  x2(:,:)
  type(bml_matrix_t)     ::  hmat_bml, x2_bml

  write(*,*)''
  write(*,*)'Example BML 4: Construct a random matrix;'
  write(*,*)'multiply the matrix by itself and compare times to dgemm'
  write(*,*)''

  hdim = 4000
  mdim = 4000
  bml_type = "dense"
  numthresh = 0.0_dp
  alpha = 1.0_dp
  beta = 0.0_dp

  allocate(h(hdim,hdim))
  allocate(aux(hdim,hdim))
  allocate(diff(hdim,hdim))
  allocate(x2(hdim,hdim))

  H=0.0_dp

  call bml_random_matrix(bml_type, bml_element_real, dp, hdim, mdim, hmat_bml)
  call bml_export_to_dense(hmat_bml,H)
  call bml_copy_new(hmat_bml,x2_bml)

  !With BML wrapper to dgemm
  mlsI = mls()
  call bml_multiply_x2(hmat_bml, x2_bml, numthresh, trace)
  write(*,*)"Time for x2 bml dense=", mls()-mlsI

  call bml_export_to_dense(x2_bml,x2)

  !With dgemm
  mlsI = mls()
  call MMult(alpha,h,h,beta,aux,'N','N',hdim)
  write(*,*)"Time for x2 dgemm=", mls()-mlsI

  !Fnorm to see the difference
  diff = aux - x2
  call bml_import_from_dense(bml_type,diff,hmat_bml,numthresh,mdim)

  write(*,*)"Fnorm difference=", bml_fnorm(hmat_bml)

end program main

subroutine MMult(alpha,a,b,beta,c,ta,tb,hdim)
  implicit none
  integer, parameter :: dp = kind(1.0d0)
  character(1), intent(in)  ::  ta, tb
  integer, intent(in)       ::  hdim
  real(dp), intent(inout)   ::  a(hdim, hdim), alpha, b(hdim,hdim), beta
  real(dp), intent(inout)   ::  c(hdim, hdim)

  call dgemm(ta, tb, hdim, hdim, hdim, alpha, &
                a, hdim, b, hdim, beta, c, hdim)

end subroutine MMult

function mls()
  integer, parameter :: dp = kind(1.0d0)
  real(dp) :: mls
  integer :: timevector(8)

  mls = 0.0_dp
  call date_and_time(values=timevector)
  mls=timevector(5)*60.0_dp*60.0_dp*1000.0_dp + timevector(6)*60.0_dp*1000.0_dp &
         + timevector(7)*1000.0_dp + timevector(8)

end function mls


program P1407
    implicit none
    integer(8) :: a, b
    integer(8) :: sum
    integer :: shift = 0
    read *, a, b
    a = a - 1
    sum = b - a
    a = rshift(a, 1)
    b = rshift(b, 1)
    do while (b > 0)
        sum = sum + lshift(b - a, shift)
        a = rshift(a, 1)
        b = rshift(b, 1)
        shift = shift + 1
    end do
    write (*, '(I0)') sum
end program P1407


module module1
implicit none

contains

function myadd(a, b)
real(8) myadd
real(8) a, b
myadd=a + b
end function myadd

subroutine mytrig(x, sinx, cosx, tanx)
real(8) x
real(8) sinx, cosx, tanx
sinx = sin(x)
cosx = cos(x)
tanx = tan(x)
end subroutine mytrig

end module module1


! 2017 / 09 /10  version  1.1
! -----------------------------------------------------------------------------
! The ELEMENTAL attribute allows for declaring procedures that operate element
! by element(rank insensitive code) and can be applied to arrays of any dimensions(up to 16D so far).
! CAUTION :
! In a simplified model, we leave off implicit none, private, public
! -----------------------------------------------------------------------------
module mod1
   contains

      elemental function contract(a,b) result(c)
         integer, intent(in) :: a, b
         integer             :: c
         c = a - b
      end function

      elemental subroutine elesub1(a, b, c)
         integer, intent(in) :: a, b
         integer, intent(out) :: c
         c = a - b
      end subroutine elesub1

      subroutine elesub2(a, b, c)
         integer, intent(in) :: a, b
         integer, intent(out) :: c
         c = a - b
      end subroutine elesub2
end module mod1

program prog1
   use mod1
   integer :: i, j, k, ia(3), ib(3), ic(3)
   i = 0
   j = -1
   ia = [1, 2, 3]
   ib = [4, 5, 6]

   write (*,*) contract(i, j)
   write (*,*) contract(i, ia)
   write (*,*) contract(ib, i)
   write (*,*) contract(ia, ib)

   call elesub1(i, j, k)
   write(*,*) k
   call elesub1(ia, j, ic)
   write(*,*) ic
   call elesub1(ia, ib, ic)
   write(*,*) ic

   ! call elesub2(ia, j, ic)
   !               1
   ! Error: Rank mismatch in argument ‘a’ at (1) (scalar and rank-1)

   !write(*,*) ic
end program prog1


! 5.14б в учебнике
! Найти отрицательные индексы в массиве

program ex_5_14b
    implicit none
    integer                 :: arrayLength, Out = 0, In = 0, i = 0
    integer, allocatable    :: X(:), Indexes(:), NegativeIndexes(:)
    logical, allocatable    :: mask(:)
    character(*), parameter :: output_file = "output.txt", &
                               input_file  = "../data/input.txt"

    open (file=input_file, newunit=In)
        read(In, *) arrayLength
        allocate(X(arrayLength))
        read(In, *) X(:)
    close (In)

    Indexes = [(i, i = 1, arrayLength)]
    mask = X < 0

    NegativeIndexes = pack(Indexes, mask)

    open (file=output_file, newunit=Out)
        write(Out,*) 'Negative indexes'
        write(Out,*) NegativeIndexes
    close (Out)

end program ex_5_14b


C     LAST UPDATE 07/08/95
C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C
      REAL FUNCTION VGTINT(A1,A3,A4)
      IMPLICIT NONE
C
C Purpose: Y(X;A) is a Voigtian. The amplitude. centre and widths of
C          the Voigtian are stored in consecutive locations of A:
C          A(1) = H,  A(2) = E,  A(3) = G, A(4) = P
C          This function returns the integrated area under the curve.
C
C-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-
C Parameters:
C
      DOUBLE PRECISION TINY,FACTOR
      PARAMETER(TINY=1.0D-10,FACTOR=1.66510922231D0)
C
C Arguments:
C
      DOUBLE PRECISION A1,A3,A4
C
C Local variables:
C
      INTEGER I
      DOUBLE PRECISION  PI,PMA,PMA2,RM,RN,RA,RB,RC,SUM,W,P
      DOUBLE PRECISION  ALPHA(4),BETA(4),GAMMA(4),DELTA(4)
C
C Common block:
C
      DOUBLE PRECISION VCOM
      COMMON /VGTCOM/ VCOM
      SAVE /VGTCOM/
C
C Data:
C
      DATA PI    /3.1415926536D0/
      DATA ALPHA /-1.2150D0,-1.3509D0,-1.2150D0,-1.3509D0/,
     &     BETA  / 1.2359D0, 0.3786D0,-1.2359D0,-0.3786D0/,
     &     GAMMA /-0.3085D0, 0.5906D0,-0.3085D0, 0.5906D0/,
     &     DELTA / 0.0210D0,-1.1858D0,-0.0210D0, 1.1858D0/
C
C-----------------------------------------------------------------------
      IF(A3.LT.TINY)A3 = TINY
      W = A3/FACTOR
      P = A4/2.0D0
      SUM = 0.0D0
      RC = 1.0D0
      DO 10 I=1,4
         PMA = P - ALPHA(I)
         PMA2 = PMA*PMA
         RB = -BETA(I)
         RM = DELTA(I)
         RN = GAMMA(I)*PMA + DELTA(I)*RB
         RA = PMA2 + RB*RB
         SUM = SUM + (RN*RC-RM*RB)/(RC*SQRT(RA*RC-RB*RB))
 10   CONTINUE
      SUM = SUM*PI*A1*W/VCOM
      VGTINT = SNGL(SUM)
C
      RETURN
      END


program test_diffusion1d
!! Need program statement for FORD
!! Solve a time-dependent heat equation in 1D.  See GEMINI-docs repo for
!! a description of the specific problem solved here

use phys_consts, only : wp,pi
use PDEparabolic, only : backEuler1D,TRBDF21D
use h5fortran, only : hdf5_file

implicit none (type, external)

type(hdf5_file) :: h5f
integer, parameter :: npts=256,lt=20*5

character(:), allocatable :: outfile
character(1024) :: argv

character(4) :: ic

real(wp), dimension(npts) :: v1,dx1i
real(wp), dimension(-1:npts+2) :: x1,TsEuler,TsBDF2,Tstrue
real(wp), dimension(npts) :: lambda,A,B,C,D,E
real(wp), dimension(npts+1) :: x1i
real(wp), dimension(0:npts+2) :: dx1
integer :: lx1,it,ix1,ierr
real(wp) :: t=0,dt
real(wp) :: Tsminx1,Tsmaxx1

real(wp), dimension(npts) :: errorEuler,errorBDF2
real(wp), dimension(npts,3) :: abc
real(wp), dimension(npts) :: y

!! create a grid for the calculation
x1=[ (real(ix1-1,wp)/real(npts-1,wp), ix1=-1,npts+2) ]
lx1=npts   !exclude ghost cells in count
dx1=x1(0:lx1+2)-x1(-1:lx1+1)
x1i(1:lx1+1)=0.5*(x1(0:lx1)+x1(1:lx1+1))
dx1i=x1i(2:lx1+1)-x1i(1:lx1)

call get_command_argument(1, argv, status=ierr)
if(ierr/=0) error stop 'please specify input filename'
outfile = trim(argv)

!! write the time, space length and spatial grid to a file
! open(newunit=u,file=outfile,status='replace')
! write(u,*) lt
! write(u,*) lx1
! call writearray(u,x1)
call h5f%open(outfile, action='w')
call h5f%write('/lt', lt, ierr)
call h5f%write('/lx1', lx1, ierr)
call h5f%write('/x1', x1, ierr)

!! initial conditions
TsEuler(-1:lx1+2)=sin(2*pi*x1(-1:lx1+2))+sin(8*pi*x1(-1:lx1+2))
TsBDF2=TsEuler
lambda(:)=1     !thermal conductivity

!! typical diffusion time, make our time step a fraction of this
dt=0.05/8**2/pi**2/maxval(lambda)

!! time iterations
do it=1,lt
  write(ic, '(I4.4)') it
  !boundary values
  Tsminx1=0.0
  Tsmaxx1=0.0

  !solve using two different numerical schemes
  A(:)=0
  B(:)=0
  C(:)=1
  D(:)=lambda(:)
  E(:)=0.0
  TsEuler(1:lx1)=backEuler1D(TsEuler(1:lx1),A,B,C,D,E,Tsminx1,Tsmaxx1,dt,dx1,dx1i,coeffs=abc,rhs=y)
  TsBDF2(1:lx1)=TRBDF21D(TsBDF2(1:lx1),A,B,C,D,E,Tsminx1,Tsmaxx1,dt,dx1,dx1i)
  t=t+dt

  !compute analytical solution to compare
  Tstrue(1:lx1) = exp(-4 *pi**2*lambda*t)*sin(2 *pi*x1(1:lx1))+exp(-64 *pi**2*lambda*t)*sin(8 *pi*x1(1:lx1))

  !! output
  ! write(u,*) t
  ! call writearray(u,TsEuler(1:lx1))
  ! call writearray(u,TsBDF2(1:lx1))
  ! call writearray(u,Tstrue(1:lx1))
  call h5f%write('/t'//ic, t, ierr)
  call h5f%write('/TsEuler'//ic, TsEuler(1:lx1), ierr)
  call h5f%write('/TsBDF2'//ic, TsBDF2(1:lx1), ierr)
  call h5f%write('/TsTrue'//ic, Tstrue(1:lx1), ierr)
  call h5f%write('/coeffs'//ic,abc,ierr)
  call h5f%write('/rhs'//ic,y,ierr)

  !check the validity of the numerical solutions at this time step
  errorEuler(1:lx1)=TsEuler(1:lx1)-Tstrue(1:lx1)
  errorBDF2(1:lx1)=TsBDF2(1:lx1)-Tstrue(1:lx1)
  if (mod(it,5) == 0) then
    print*, 'At time step:  ',it,' max error:  ',maxval(abs(errorEuler)),maxval(abs(errorBDF2))
  end if
  if (maxval(abs(errorEuler)) > 0.05_wp) then    !more that 5% error at a point means something really bad happened...
    print*, 'Time step:  ',it,dt
    error stop 'Excessive error (large max diff) in backward Euler solution, check time step maybe???'
  end if
  if (maxval(abs(errorBDF2)) > 0.05_wp) then
    print*, 'Time step:  ',it,dt
    error stop 'Excessive error (large max diff) in TRBDF2 solution, check time step maybe???'
  end if
end do

! close(u)
call h5f%close()

contains

! subroutine writearray(u,array)
! integer, intent(in) :: u
! real(wp), dimension(:), intent(in) :: array

! integer :: k

! do k=1,size(array)
!   write(u,*) array(k)
! end do
! end subroutine writearray

!
!  subroutine write2Darray(u,array)
!    integer, intent(in) :: u
!    real(wp), dimension(:,:), intent(in) :: array
!
!    integer :: k1,k2
!
!    do k1=1,size(array,1)
!      write(u,'(f8.0)') (array(k1,k2), k2=1,size(array,2))
!    end do
!  end subroutine write2Darray
!

end program


module precision
  !
  !*Brief Description:* Defines the precision of real parameters.
  !
  !*LICENSE:*
  !
  !
  !
  !*Full Description:*
  ! Defines the precision of real, via a parameter 'dp'.
  implicit none

  !Module parameters
  integer, parameter :: dp=kind(0.d0) ! for double precision

  real(dp),parameter :: zero_tol = 1.0e-12_dp
  real(dp),parameter :: loose_tol = 1.0e-6_dp

  !Module types

  !Module variables

  !Interfaces
  private
  public dp
  public zero_tol
  public loose_tol

end module precision


!> @ingroup insertion_sort_gen
!>
!> @brief
!> Run an insertion sort on a vector of double precision values.
!>
!> @details
!> Run the specified on a vector of double precision values to sort them in the specified order.The vector of intergers map is permuted in the same way that the vector vec is permuted to be sorted. It is important that map be at least as long as vec.
!>
!> @param vec
!> The vector double precision values to be sorted.
!>
!> @param map
!> Vector of intergers which is permuted in the same way that vec is permuted. If map is a vector of values 1 to N, where N is the length of vec, than, map is the map from vec's original value to vec's final value after the sort.
!>
!> @param ord
!> A logical function which describes the ordering used. The function must take two double precision arguments and return true if and only if the first argument must proceed the second argument in the order.
!>
!> @warning
!> The argument types and counts of ord are not checked within this subroutine.
!>
!> @warning
!> The length of map is not checked within this subroutine. If the length of vec is larger than the length of map, than this subroutine may attempt to access elements outside of map.
!>
!> @author Arin R. Bratt
!> @date 11/07/2014
subroutine insertion_sort_gen_map_dble(vec,map,ord)
  ! Arguments
  double precision, intent(inout) :: vec(:)
  integer, intent(inout) :: map(:)
  procedure(logical) :: ord

  ! Variables
  double precision :: double_precision_value
  double precision :: swap_value
  integer :: i
  integer :: j

  ! Sort to the first element
  if (ord(vec(size(vec,1)),vec(1))) then
     double_precision_value = vec(1)
  else
     double_precision_value = vec(size(vec,1))
     vec(size(vec,1)) = vec(1)
  end if

  do i = size(vec,1) - 1,2,-1
     if (ord(double_precision_value,vec(i))) then
        ! Swap double precision values double_precision_value and vec(i).
        swap_value = double_precision_value
        double_precision_value = vec(i)
        vec(i) = swap_value
     end if
  end do

  ! Run regualr insertion sort
  do j = 3,size(vec,1)
     swap_value = vec(j)
     i = j - 1
     if (ord(vec(i),swap_value)) then
        vec(j) = vec(i)
        i = i - 1
        do
           if (.not. ord(vec(i),swap_value)) exit
           vec(i + 1) = vec(i)
           i = i - 1
        end do
        vec(i + 1) = swap_value
     end if
  end do
end subroutine insertion_sort_gen_map_dble


*----------------------------------------------------------------------*
      integer function ipermfac(ilist,len,ordered)
*----------------------------------------------------------------------*
*     obtain number of equivalent permutations of list ilist
*     i.e. for each n-tuple of equivalent elements on ilist, we
*     get a factor of n!
*     if ordered==.true., the list is in ordered sequence and
*     equivalent entries come subsequently
*     ... intended for small lists ...
*----------------------------------------------------------------------*

      implicit none

      include 'stdunit.h'

      integer, parameter ::
     &     ntest = 00

      logical, intent(in) ::
     &     ordered
      integer, intent(in) ::
     &     len, ilist(len)

      logical ::
     &     succ
      integer ::
     &     idx, ihlp, iscr(len), ilast, nn
      integer, external ::
     &     ifac

      ! copy to scratch (do not change input)
      iscr(1:len) = ilist(1:len)
      ! bubble-sort (len is small, remember?) into ordered list
      ! OK, I am ashamed -- I will use insertion sort next time ....
      if (.not.ordered) then
        succ = .false.
        do while (.not.succ)
          succ = .true.
          do idx = 1, len-1
            if (iscr(idx).gt.iscr(idx+1)) then
              succ = .false.
              ihlp = iscr(idx)
              iscr(idx) = iscr(idx+1)
              iscr(idx+1) = ihlp
            end if
          end do
        end do
      end if

      ilast = iscr(1)
      nn = 1
      ipermfac = 1
      do idx = 2, len
        if (iscr(idx).eq.ilast) then
          nn = nn+1
        else
          ilast = iscr(idx)
          if (nn.gt.1) ipermfac = ipermfac*ifac(nn)
          nn = 1
        end if
      end do
      if (nn.gt.1) ipermfac = ipermfac*ifac(nn)

      if (ntest.ge.100) then
        write(lulog,*) '-------------------'
        write(lulog,*) ' ipermfac speaking'
        write(lulog,*) '-------------------'
        write(lulog,*) ' list:      ',ilist(1:len)
        if (.not.ordered)
     &       write(lulog,*) ' list(reo): ',iscr(1:len)
        write(lulog,*) ' ipermfac = ',ipermfac
      end if

      return
      end


      SUBROUTINE SB02MD( DICO, HINV, UPLO, SCAL, SORT, N, A, LDA, G,
     $                   LDG, Q, LDQ, RCOND, WR, WI, S, LDS, U, LDU,
     $                   IWORK, DWORK, LDWORK, BWORK, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To solve for X either the continuous-time algebraic Riccati
C     equation
C                              -1
C        Q + A'*X + X*A - X*B*R  B'*X = 0                            (1)
C
C     or the discrete-time algebraic Riccati equation
C                                        -1
C        X = A'*X*A - A'*X*B*(R + B'*X*B)  B'*X*A + Q                (2)
C
C     where A, B, Q and R are N-by-N, N-by-M, N-by-N and M-by-M matrices
C     respectively, with Q symmetric and R symmetric nonsingular; X is
C     an N-by-N symmetric matrix.
C                       -1
C     The matrix G = B*R  B' must be provided on input, instead of B and
C     R, that is, for instance, the continuous-time equation
C
C        Q + A'*X + X*A - X*G*X = 0                                  (3)
C
C     is solved, where G is an N-by-N symmetric matrix. SLICOT Library
C     routine SB02MT should be used to compute G, given B and R. SB02MT
C     also enables to solve Riccati equations corresponding to optimal
C     problems with coupling terms.
C
C     The routine also returns the computed values of the closed-loop
C     spectrum of the optimal system, i.e., the stable eigenvalues
C     lambda(1),...,lambda(N) of the corresponding Hamiltonian or
C     symplectic matrix associated to the optimal problem.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     DICO    CHARACTER*1
C             Specifies the type of Riccati equation to be solved as
C             follows:
C             = 'C':  Equation (3), continuous-time case;
C             = 'D':  Equation (2), discrete-time case.
C
C     HINV    CHARACTER*1
C             If DICO = 'D', specifies which symplectic matrix is to be
C             constructed, as follows:
C             = 'D':  The matrix H in (5) (see METHOD) is constructed;
C             = 'I':  The inverse of the matrix H in (5) is constructed.
C             HINV is not used if DICO = 'C'.
C
C     UPLO    CHARACTER*1
C             Specifies which triangle of the matrices G and Q is
C             stored, as follows:
C             = 'U':  Upper triangle is stored;
C             = 'L':  Lower triangle is stored.
C
C     SCAL    CHARACTER*1
C             Specifies whether or not a scaling strategy should be
C             used, as follows:
C             = 'G':  General scaling should be used;
C             = 'N':  No scaling should be used.
C
C     SORT    CHARACTER*1
C             Specifies which eigenvalues should be obtained in the top
C             of the Schur form, as follows:
C             = 'S':  Stable   eigenvalues come first;
C             = 'U':  Unstable eigenvalues come first.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrices A, Q, G and X.  N >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the coefficient matrix A of the equation.
C             On exit, if DICO = 'D', and INFO = 0 or INFO > 1, the
C                                                                    -1
C             leading N-by-N part of this array contains the matrix A  .
C             Otherwise, the array A is unchanged on exit.
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     G       (input) DOUBLE PRECISION array, dimension (LDG,N)
C             The leading N-by-N upper triangular part (if UPLO = 'U')
C             or lower triangular part (if UPLO = 'L') of this array
C             must contain the upper triangular part or lower triangular
C             part, respectively, of the symmetric matrix G. The stricly
C             lower triangular part (if UPLO = 'U') or stricly upper
C             triangular part (if UPLO = 'L') is not referenced.
C
C     LDG     INTEGER
C             The leading dimension of array G.  LDG >= MAX(1,N).
C
C     Q       (input/output) DOUBLE PRECISION array, dimension (LDQ,N)
C             On entry, the leading N-by-N upper triangular part (if
C             UPLO = 'U') or lower triangular part (if UPLO = 'L') of
C             this array must contain the upper triangular part or lower
C             triangular part, respectively, of the symmetric matrix Q.
C             The stricly lower triangular part (if UPLO = 'U') or
C             stricly upper triangular part (if UPLO = 'L') is not used.
C             On exit, if INFO = 0, the leading N-by-N part of this
C             array contains the solution matrix X of the problem.
C
C     LDQ     INTEGER
C             The leading dimension of array N.  LDQ >= MAX(1,N).
C
C     RCOND   (output) DOUBLE PRECISION
C             An estimate of the reciprocal of the condition number (in
C             the 1-norm) of the N-th order system of algebraic
C             equations from which the solution matrix X is obtained.
C
C     WR      (output) DOUBLE PRECISION array, dimension (2*N)
C     WI      (output) DOUBLE PRECISION array, dimension (2*N)
C             If INFO = 0 or INFO = 5, these arrays contain the real and
C             imaginary parts, respectively, of the eigenvalues of the
C             2N-by-2N matrix S, ordered as specified by SORT (except
C             for the case HINV = 'D', when the order is opposite to
C             that specified by SORT). The leading N elements of these
C             arrays contain the closed-loop spectrum of the system
C                           -1
C             matrix A - B*R  *B'*X, if DICO = 'C', or of the matrix
C                               -1
C             A - B*(R + B'*X*B)  B'*X*A, if DICO = 'D'. Specifically,
C                lambda(k) = WR(k) + j*WI(k), for k = 1,2,...,N.
C
C     S       (output) DOUBLE PRECISION array, dimension (LDS,2*N)
C             If INFO = 0 or INFO = 5, the leading 2N-by-2N part of this
C             array contains the ordered real Schur form S of the
C             Hamiltonian or symplectic matrix H. That is,
C
C                    (S   S  )
C                    ( 11  12)
C                S = (       ),
C                    (0   S  )
C                    (     22)
C
C             where S  , S   and S   are N-by-N matrices.
C                    11   12      22
C
C     LDS     INTEGER
C             The leading dimension of array S.  LDS >= MAX(1,2*N).
C
C     U       (output) DOUBLE PRECISION array, dimension (LDU,2*N)
C             If INFO = 0 or INFO = 5, the leading 2N-by-2N part of this
C             array contains the transformation matrix U which reduces
C             the Hamiltonian or symplectic matrix H to the ordered real
C             Schur form S. That is,
C
C                    (U   U  )
C                    ( 11  12)
C                U = (       ),
C                    (U   U  )
C                    ( 21  22)
C
C             where U  , U  , U   and U   are N-by-N matrices.
C                    11   12   21      22
C
C     LDU     INTEGER
C             The leading dimension of array U.  LDU >= MAX(1,2*N).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (2*N)
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK and DWORK(2) returns the scaling factor used
C             (set to 1 if SCAL = 'N'), also set if INFO = 5;
C             if DICO = 'D', DWORK(3) returns the reciprocal condition
C             number of the given matrix  A.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX(2,6*N) if DICO = 'C';
C             LDWORK >= MAX(3,6*N) if DICO = 'D'.
C             For optimum performance LDWORK should be larger.
C
C     BWORK   LOGICAL array, dimension (2*N)
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  if matrix A is (numerically) singular in discrete-
C                   time case;
C             = 2:  if the Hamiltonian or symplectic matrix H cannot be
C                   reduced to real Schur form;
C             = 3:  if the real Schur form of the Hamiltonian or
C                   symplectic matrix H cannot be appropriately ordered;
C             = 4:  if the Hamiltonian or symplectic matrix H has less
C                   than N stable eigenvalues;
C             = 5:  if the N-th order system of linear algebraic
C                   equations, from which the solution matrix X would
C                   be obtained, is singular to working precision.
C
C     METHOD
C
C     The method used is the Schur vector approach proposed by Laub.
C     It is assumed that [A,B] is a stabilizable pair (where for (3) B
C     is any matrix such that B*B' = G with rank(B) = rank(G)), and
C     [E,A] is a detectable pair, where E is any matrix such that
C     E*E' = Q with rank(E) = rank(Q). Under these assumptions, any of
C     the algebraic Riccati equations (1)-(3) is known to have a unique
C     non-negative definite solution. See [2].
C     Now consider the 2N-by-2N Hamiltonian or symplectic matrix
C
C                 ( A   -G )
C            H =  (        ),                                    (4)
C                 (-Q   -A'),
C
C     for continuous-time equation, and
C                    -1        -1
C                 ( A         A  *G   )
C            H =  (   -1          -1  ),                         (5)
C                 (Q*A    A' + Q*A  *G)
C                                                            -1
C     for discrete-time equation, respectively, where G = B*R  *B'.
C     The assumptions guarantee that H in (4) has no pure imaginary
C     eigenvalues, and H in (5) has no eigenvalues on the unit circle.
C     If Y is an N-by-N matrix then there exists an orthogonal matrix U
C     such that U'*Y*U is an upper quasi-triangular matrix. Moreover, U
C     can be chosen so that the 2-by-2 and 1-by-1 diagonal blocks
C     (corresponding to the complex conjugate eigenvalues and real
C     eigenvalues respectively) appear in any desired order. This is the
C     ordered real Schur form. Thus, we can find an orthogonal
C     similarity transformation U which puts (4) or (5) in ordered real
C     Schur form
C
C            U'*H*U = S = (S(1,1)  S(1,2))
C                         (  0     S(2,2))
C
C     where S(i,j) is an N-by-N matrix and the eigenvalues of S(1,1)
C     have negative real parts in case of (4), or moduli greater than
C     one in case of (5). If U is conformably partitioned into four
C     N-by-N blocks
C
C               U = (U(1,1)  U(1,2))
C                   (U(2,1)  U(2,2))
C
C     with respect to the assumptions we then have
C     (a) U(1,1) is invertible and X = U(2,1)*inv(U(1,1)) solves (1),
C         (2), or (3) with X = X' and non-negative definite;
C     (b) the eigenvalues of S(1,1) (if DICO = 'C') or S(2,2) (if
C         DICO = 'D') are equal to the eigenvalues of optimal system
C         (the 'closed-loop' spectrum).
C
C     [A,B] is stabilizable if there exists a matrix F such that (A-BF)
C     is stable. [E,A] is detectable if [A',E'] is stabilizable.
C
C     REFERENCES
C
C     [1] Laub, A.J.
C         A Schur Method for Solving Algebraic Riccati equations.
C         IEEE Trans. Auto. Contr., AC-24, pp. 913-921, 1979.
C
C     [2] Wonham, W.M.
C         On a matrix Riccati equation of stochastic control.
C         SIAM J. Contr., 6, pp. 681-697, 1968.
C
C     [3] Sima, V.
C         Algorithms for Linear-Quadratic Optimization.
C         Pure and Applied Mathematics: A Series of Monographs and
C         Textbooks, vol. 200, Marcel Dekker, Inc., New York, 1996.
C
C     NUMERICAL ASPECTS
C                               3
C     The algorithm requires 0(N ) operations.
C
C     FURTHER COMMENTS
C
C     To obtain a stabilizing solution of the algebraic Riccati
C     equation for DICO = 'D', set SORT = 'U', if HINV = 'D', or set
C     SORT = 'S', if HINV = 'I'.
C
C     The routine can also compute the anti-stabilizing solutions of
C     the algebraic Riccati equations, by specifying
C         SORT = 'U' if DICO = 'D' and HINV = 'I', or DICO = 'C', or
C         SORT = 'S' if DICO = 'D' and HINV = 'D'.
C
C     Usually, the combinations HINV = 'D' and SORT = 'U', or HINV = 'I'
C     and SORT = 'U', will be faster then the other combinations [3].
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Aug. 1997.
C     Supersedes Release 2.0 routine SB02AD by Control Systems Research
C     Group, Kingston Polytechnic, United Kingdom, March 1982.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, Dec. 2002.
C
C     KEYWORDS
C
C     Algebraic Riccati equation, closed loop system, continuous-time
C     system, discrete-time system, optimal regulator, Schur form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, HALF, ONE
      PARAMETER         ( ZERO = 0.0D0, HALF = 0.5D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         DICO, HINV, SCAL, SORT, UPLO
      INTEGER           INFO, LDA, LDG, LDQ, LDS, LDU, LDWORK, N
      DOUBLE PRECISION  RCOND
C     .. Array Arguments ..
      LOGICAL           BWORK(*)
      INTEGER           IWORK(*)
      DOUBLE PRECISION  A(LDA,*), DWORK(*), G(LDG,*), Q(LDQ,*),
     $                  S(LDS,*), U(LDU,*), WR(*), WI(*)
C     .. Local Scalars ..
      LOGICAL           DISCR, LHINV, LSCAL, LSORT, LUPLO
      INTEGER           I, IERR, ISCL, N2, NP1, NROT
      DOUBLE PRECISION  GNORM, QNORM, RCONDA, UNORM, WRKOPT
C     .. External Functions ..
      LOGICAL           LSAME, SB02MR, SB02MS, SB02MV, SB02MW
      DOUBLE PRECISION  DLAMCH, DLANGE, DLANSY
      EXTERNAL          DLAMCH, DLANGE, DLANSY, LSAME, SB02MR, SB02MS,
     $                  SB02MV, SB02MW
C     .. External Subroutines ..
      EXTERNAL          DAXPY, DCOPY, DGECON, DGEES, DGETRF, DGETRS,
     $                  DLACPY, DLASCL, DLASET, DSCAL, DSWAP, SB02MU,
     $                  XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX
C     .. Executable Statements ..
C
      INFO = 0
      N2  = N + N
      NP1 = N + 1
      DISCR = LSAME( DICO, 'D' )
      LSCAL = LSAME( SCAL, 'G' )
      LSORT = LSAME( SORT, 'S' )
      LUPLO = LSAME( UPLO, 'U' )
      IF ( DISCR ) LHINV = LSAME( HINV, 'D' )
C
C     Test the input scalar arguments.
C
      IF( .NOT.DISCR .AND. .NOT.LSAME( DICO, 'C' ) ) THEN
         INFO = -1
      ELSE IF( DISCR ) THEN
         IF( .NOT.LHINV .AND. .NOT.LSAME( HINV, 'I' ) )
     $      INFO = -2
      END IF
      IF( .NOT.LUPLO .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -3
      ELSE IF( .NOT.LSCAL .AND. .NOT.LSAME( SCAL, 'N' ) ) THEN
         INFO = -4
      ELSE IF( .NOT.LSORT .AND. .NOT.LSAME( SORT, 'U' ) ) THEN
         INFO = -5
      ELSE IF( N.LT.0 ) THEN
         INFO = -6
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -8
      ELSE IF( LDG.LT.MAX( 1, N ) ) THEN
         INFO = -10
      ELSE IF( LDQ.LT.MAX( 1, N ) ) THEN
         INFO = -12
      ELSE IF( LDS.LT.MAX( 1, N2 ) ) THEN
         INFO = -17
      ELSE IF( LDU.LT.MAX( 1, N2 ) ) THEN
         INFO = -19
      ELSE IF( ( .NOT.DISCR .AND. LDWORK.LT.MAX( 2, 6*N ) ) .OR.
     $         (      DISCR .AND. LDWORK.LT.MAX( 3, 6*N ) ) ) THEN
         INFO = -22
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'SB02MD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( N.EQ.0 ) THEN
         RCOND = ONE
         DWORK(1) = ONE
         DWORK(2) = ONE
         IF ( DISCR ) DWORK(3) = ONE
         RETURN
      END IF
C
      IF ( LSCAL ) THEN
C
C        Compute the norms of the matrices Q and G.
C
         QNORM = DLANSY( '1-norm', UPLO, N, Q, LDQ, DWORK )
         GNORM = DLANSY( '1-norm', UPLO, N, G, LDG, DWORK )
      END IF
C
C     Initialise the Hamiltonian or symplectic matrix associated with
C     the problem.
C     Workspace:  need   1          if DICO = 'C';
C                        max(2,4*N) if DICO = 'D';
C                 prefer larger if DICO = 'D'.
C
      CALL SB02MU( DICO, HINV, UPLO, N, A, LDA, G, LDG, Q, LDQ, S, LDS,
     $             IWORK, DWORK, LDWORK, INFO )
      IF ( INFO.NE.0 ) THEN
         INFO = 1
         RETURN
      END IF
C
      WRKOPT = DWORK(1)
      IF ( DISCR ) RCONDA = DWORK(2)
C
      ISCL = 0
      IF ( LSCAL ) THEN
C
C        Scale the Hamiltonian or symplectic matrix.
C
         IF( QNORM.GT.GNORM .AND. GNORM.GT.ZERO ) THEN
            CALL DLASCL( 'G', 0, 0, QNORM, GNORM, N, N, S(NP1,1), N2,
     $                   IERR )
            CALL DLASCL( 'G', 0, 0, GNORM, QNORM, N, N, S(1,NP1), N2,
     $                   IERR )
            ISCL = 1
         END IF
      END IF
C
C     Find the ordered Schur factorization of S,   S = U*H*U'.
C     Workspace:  need   6*N;
C                 prefer larger.
C
      IF ( .NOT.DISCR ) THEN
         IF ( LSORT ) THEN
            CALL DGEES( 'Vectors', 'Sorted', SB02MV, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         ELSE
            CALL DGEES( 'Vectors', 'Sorted', SB02MR, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         END IF
      ELSE
         IF ( LSORT ) THEN
            CALL DGEES( 'Vectors', 'Sorted', SB02MW, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         ELSE
            CALL DGEES( 'Vectors', 'Sorted', SB02MS, N2, S, LDS, NROT,
     $                  WR, WI, U, LDU, DWORK, LDWORK, BWORK, INFO )
         END IF
         IF ( LHINV ) THEN
            CALL DSWAP( N, WR, 1, WR(NP1), 1 )
            CALL DSWAP( N, WI, 1, WI(NP1), 1 )
         END IF
      END IF
      IF ( INFO.GT.N2 ) THEN
         INFO = 3
      ELSE IF ( INFO.GT.0 ) THEN
         INFO = 2
      ELSE IF ( NROT.NE.N ) THEN
         INFO = 4
      END IF
      IF ( INFO.NE.0 )
     $   RETURN
C
      WRKOPT = MAX( WRKOPT, DWORK(1) )
C
C     Check if U(1,1) is singular.  Use the (2,1) block of S as a
C     workspace for factoring U(1,1).
C
      UNORM = DLANGE( '1-norm', N, N, U, LDU, DWORK )
C
      CALL DLACPY( 'Full', N, N, U, LDU, S(NP1,1), LDS )
      CALL DGETRF( N, N, S(NP1,1), LDS, IWORK, INFO )
C
      IF ( INFO.GT.0 ) THEN
C
C        Singular matrix.  Set INFO and RCOND for error return.
C
         INFO  = 5
         RCOND = ZERO
         GO TO 100
      END IF
C
C     Estimate the reciprocal condition of U(1,1).
C     Workspace: 6*N.
C
      CALL DGECON( '1-norm', N, S(NP1,1), LDS, UNORM, RCOND,
     $             DWORK, IWORK(NP1), INFO )
C
      IF ( RCOND.LT.DLAMCH( 'Epsilon' ) ) THEN
C
C        Nearly singular matrix.  Set INFO for error return.
C
         INFO = 5
         RETURN
      END IF
C
C     Transpose U(2,1) in Q and compute the solution.
C
      DO 60 I = 1, N
         CALL DCOPY( N, U(NP1,I), 1, Q(I,1), LDQ )
   60 CONTINUE
C
      CALL DGETRS( 'Transpose', N, N, S(NP1,1), LDS, IWORK, Q, LDQ,
     $             INFO )
C
C     Set S(2,1) to zero.
C
      CALL DLASET( 'Full', N, N, ZERO, ZERO, S(NP1,1), LDS )
C
C     Make sure the solution matrix X is symmetric.
C
      DO 80 I = 1, N - 1
         CALL DAXPY( N-I, ONE, Q(I,I+1), LDQ, Q(I+1,I), 1 )
         CALL DSCAL( N-I, HALF, Q(I+1,I), 1 )
         CALL DCOPY( N-I, Q(I+1,I), 1, Q(I,I+1), LDQ )
   80 CONTINUE
C
      IF( LSCAL ) THEN
C
C        Undo scaling for the solution matrix.
C
         IF( ISCL.EQ.1 )
     $      CALL DLASCL( 'G', 0, 0, GNORM, QNORM, N, N, Q, LDQ, IERR )
      END IF
C
C     Set the optimal workspace, the scaling factor, and reciprocal
C     condition number (if any).
C
      DWORK(1) = WRKOPT
  100 CONTINUE
      IF( ISCL.EQ.1 ) THEN
         DWORK(2) = QNORM / GNORM
      ELSE
         DWORK(2) = ONE
      END IF
      IF ( DISCR ) DWORK(3) = RCONDA
C
      RETURN
C *** Last line of SB02MD ***
      END


      subroutine tok_fraction(ft_tok,irho)
c--
c  This calculates the fraction passing and the fraction trapped
c  for a tokamak using the Lin-Liu Miller approximation with Houlberg
c  weighting factors.  (Phys. Plas. 2, 1966 (1995).
c--
C-----------------------------------------------
C   M o d u l e s
C-----------------------------------------------
      use parambs
      use vmec0
      implicit none
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      integer irho
      real(rprec) ft_tok
C-----------------------------------------------
C   L o c a l   P a r a m e t e r s
C-----------------------------------------------
      real(rprec), parameter :: one = 1
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      integer i
      real(rprec) fub, flb, bavg, b2av, sum_gsqrt_tok
      real(rprec) bmax
C-----------------------------------------------

      real(rprec), dimension(:), allocatable ::
     1    bfield_tok,  gsqrt_tok, b2obm_tok, one_b2
      allocate(bfield_tok(nthetah),  gsqrt_tok(nthetah),
     1   b2obm_tok(nthetah), one_b2(nthetah), stat = i)
      if(i .ne. 0) stop 'allocation error of tokamak fields'

c  make symetric copies of field quantites--average over zeta to extract
c  symmetric component.

      do i =1,nthetah
         bfield_tok(i) = sum(bfield(i,:nzetah))/nzetah
      enddo

c  renormalize to max tok field equivalent

      bmax = maxval(bfield_tok)
      bfield_tok = bfield_tok/bmax
      where(bfield_tok .gt. one) bfield_tok = one

c  calculate 1/b**2, b**2

      one_b2 = one/bfield_tok**2
      b2obm_tok = bfield_tok**2

c  jacobian only includes 1/b**2 component and is normalized to bmax tok
c  integrate over jacobian to obtain normalization for averages
      sum_gsqrt_tok= sum(one_b2)

c  find average b, b**2

      bavg = sum(bfield_tok*one_b2)/sum_gsqrt_tok
      b2av = sum(b2obm_tok*one_b2)/sum_gsqrt_tok

c  find upper bound

      fub = one-(one-sqrt(one-bavg)*(one+0.5_dp*bavg))*b2av/bavg**2

c  find lower bound

c  minus <1/b**2>

      flb = - sum(one/b2obm_tok**2)/sum_gsqrt_tok

c  plus <sqrt(1-b)/b**2>

      flb = flb + sum(sqrt(one-bfield_tok)
     1    /b2obm_tok**2)/sum_gsqrt_tok

c  plus <sqrt(1-b)/b)>/2

      flb = flb + 0.5_dp*sum(sqrt(one-bfield_tok)
     1    /bfield_tok/b2obm_tok)/sum_gsqrt_tok

c  1+(previous stuff)*<b**2>

      flb = one + flb*b2av

c finally combine upper and lower limits with Houlberg factors
      ft_tok = 0.25_dp*flb + 0.75_dp*fub

      deallocate(bfield_tok, gsqrt_tok, b2obm_tok, one_b2)

      return
      end subroutine tok_fraction


	FUNCTION XPADC(F)
C
C	THIS FUNCTION RETURNS THE GAIN OF THE DC CHANNEL OF THE WIND X PREAMP
C	NOT TESTED YET
C
	COMPLEX CGAIN,Y1,Z1,Y2,Z2
	DATA R1,C1,R2,CINP /5.E8,22.E-12,5.E8,4.51E-12/
	DATA R3,C3 /2.2E+03,1.E-9/
	DATA TWOPI /6.2831853/
C
	W = TWOPI*F
	CGAIN = 1.
C	DIVISION OF SIGNAL BEFORE FOLLOWER
	Y1 = CMPLX(1./R1,W*C1)
	Y2 = CMPLX(1./R2,W*CINP)
	Z1 = 1./Y1
	Z2 = 1./Y2
	CGAIN = CGAIN*Z2/(Z1+Z2)
C	EFFECT OF OUTPUT RC
	WC3 = 1./(W*C3)
	CGAIN = CGAIN*CMPLX(0.,WC3)/CMPLX(R3,WC3)
	XPADC = CABS(CGAIN)
	RETURN
	END


! =========================================================================
elemental function gsw_pt_from_entropy (sa, entropy)
! =========================================================================
!
!  Calculates potential temperature with reference pressure p_ref = 0 dbar
!  and with entropy as an input variable.
!
!  SA       =  Absolute Salinity                                   [ g/kg ]
!  entropy  =  specific entropy                                   [ deg C ]
!
!  pt   =  potential temperature                                  [ deg C ]
!          with reference sea pressure (p_ref) = 0 dbar.
!  Note. The reference sea pressure of the output, pt, is zero dbar.
!--------------------------------------------------------------------------

use gsw_mod_teos10_constants, only : gsw_cp0, gsw_sso, gsw_t0

use gsw_mod_toolbox, only : gsw_entropy_from_pt, gsw_gibbs_pt0_pt0

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, entropy

real (r8) :: gsw_pt_from_entropy

integer :: number_of_iterations
real (r8) :: c, dentropy, dentropy_dt, ent_sa, part1, part2, pt, ptm
real (r8) :: pt_old

! Find the initial value of pt
part1 = 1.0_r8 - sa/gsw_sso
part2 = 1.0_r8 - 0.05_r8*part1
ent_sa = (gsw_cp0/gsw_t0)*part1*(1.0_r8 - 1.01_r8*part1)
c = (entropy - ent_sa)*(part2/gsw_cp0)
pt = gsw_t0*(exp(c) - 1.0_r8)
dentropy_dt = gsw_cp0/((gsw_t0 + pt)*part2)

do number_of_iterations = 1, 2
    pt_old = pt
    dentropy = gsw_entropy_from_pt(sa,pt_old) - entropy
    pt = pt_old - dentropy/dentropy_dt
    ptm = 0.5_r8*(pt + pt_old)
    dentropy_dt = -gsw_gibbs_pt0_pt0(sa,ptm)
    pt = pt_old - dentropy/dentropy_dt
end do

! Maximum error of 2.2x10^-6 degrees C for one iteration.
! Maximum error is 1.4x10^-14 degrees C for two iterations
! (two iterations is the default, "for Number_of_iterations = 1:2").

gsw_pt_from_entropy = pt

return
end function

!--------------------------------------------------------------------------


      SUBROUTINE airy(x,ai,bi,aip,bip)
      REAL ai,aip,bi,bip,x
CU    USES bessik,bessjy
      REAL absx,ri,rip,rj,rjp,rk,rkp,rootx,ry,ryp,z,PI,THIRD,TWOTHR,
     *ONOVRT
      PARAMETER (PI=3.1415927,THIRD=1./3.,TWOTHR=2.*THIRD,
     *ONOVRT=.57735027)
      absx=abs(x)
      rootx=sqrt(absx)
      z=TWOTHR*absx*rootx
      if(x.gt.0.)then
        call bessik(z,THIRD,ri,rk,rip,rkp)
        ai=rootx*ONOVRT*rk/PI
        bi=rootx*(rk/PI+2.*ONOVRT*ri)
        call bessik(z,TWOTHR,ri,rk,rip,rkp)
        aip=-x*ONOVRT*rk/PI
        bip=x*(rk/PI+2.*ONOVRT*ri)
      else if(x.lt.0.)then
        call bessjy(z,THIRD,rj,ry,rjp,ryp)
        ai=.5*rootx*(rj-ONOVRT*ry)
        bi=-.5*rootx*(ry+ONOVRT*rj)
        call bessjy(z,TWOTHR,rj,ry,rjp,ryp)
        aip=.5*absx*(ONOVRT*ry+rj)
        bip=.5*absx*(ONOVRT*rj-ry)
      else
        ai=.35502805
        bi=ai/ONOVRT
        aip=-.25881940
        bip=-aip/ONOVRT
      endif
      return
      END


      SUBROUTINE TEST8(L,N,M,A,B,C)
      INTEGER L,N,M
      REAL A(L,M), B(M,N), C(L,M)

      DO 10 I = 1, L
         DO 20 J = 1, N
            C(I,J) = 0.0
            DO 30 K = 1, M
               C(I,J) = C(I,J) + A(I,K) * B(K,J)
 30         CONTINUE
 20      CONTINUE
 10   CONTINUE
      END


!*==zrotg.f90  processed by SPAG 7.51RB at 20:37 on  3 Mar 2022
!> \brief \b ZROTG
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!  Definition:
!  ===========
!
!       SUBROUTINE ZROTG(CA,CB,C,S)
!
!       .. Scalar Arguments ..
!       COMPLEX*16 CA,CB,S
!       DOUBLE PRECISION C
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!>    ZROTG determines a double complex Givens rotation.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in,out] CA
!> \verbatim
!>          CA is COMPLEX*16
!> \endverbatim
!>
!> \param[in] CB
!> \verbatim
!>          CB is COMPLEX*16
!> \endverbatim
!>
!> \param[out] C
!> \verbatim
!>          C is DOUBLE PRECISION
!> \endverbatim
!>
!> \param[out] S
!> \verbatim
!>          S is COMPLEX*16
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date November 2017
!
!> \ingroup complex16_blas_level1
!
!  =====================================================================
      SUBROUTINE ZROTG(Ca,Cb,C,S)
      IMPLICIT NONE
!*--ZROTG66
!
!  -- Reference BLAS level1 routine (version 3.8.0) --
!  -- Reference BLAS is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     November 2017
!
!     .. Scalar Arguments ..
      COMPLEX*16 Ca , Cb , S
      DOUBLE PRECISION C
!     ..
!
!  =====================================================================
!
!     .. Local Scalars ..
      COMPLEX*16 alpha
      DOUBLE PRECISION norm , scale
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC CDABS , DCMPLX , DCONJG , DSQRT
!     ..
      IF ( CDABS(Ca)==0.0D0 ) THEN
         C = 0.0D0
         S = (1.0D0,0.0D0)
         Ca = Cb
      ELSE
         scale = CDABS(Ca) + CDABS(Cb)
         norm = scale*DSQRT((CDABS(Ca/DCMPLX(scale,0.0D0)))             &
     &          **2+(CDABS(Cb/DCMPLX(scale,0.0D0)))**2)
         alpha = Ca/CDABS(Ca)
         C = CDABS(Ca)/norm
         S = alpha*DCONJG(Cb)/norm
         Ca = alpha*norm
      ENDIF
      END SUBROUTINE ZROTG


program array_init_test
    implicit none
    integer, parameter :: m = 4, n = 5, p = m*n/2
    integer :: i, j
    integer, dimension(m, n) :: a = reshape( [ (i, i = 1, m*n) ], [m, n])
    real, dimension(m, n) :: b
    integer, dimension(p) :: c
    character(len=10) :: fmt_str

    print '(A)', '# initialized at declaration'
    write (fmt_str, '(A, I1, A)') '(', n, 'I3)'
    do i = 1, size(a, 1)
        print fmt_str, a(i, :)
    end do

    print '(A)', '# initialized in program'
    b = reshape([ ((1.0/(i*n + j), i = 1, m), j=1, n) ], [m, n])
    write (fmt_str, '(A, I1, A)') '(', n, 'F7.3)'
    do i = 1, size(b, 1)
        print fmt_str, b(i, :)
    end do

    print '(A)', '# transposed and reshaped'
    c = reshape(transpose(a(2:3, :)), [p])
    write (fmt_str, '(A, I2, A)') '(', p, 'I3)'
    print fmt_str, c(:)

end program array_init_test


! Program to generate a lattice of spin-1/2 particles and study its
! Ferromagnetic properties

program ising_3d
    implicit none

    integer:: i,j,k,L,p,a,b,c,d,f,g,niter,time,mm,nn,oo,N
    real*8:: r,E,M,mag,Ei,Ef,dE,u,h

    real*8:: T, J_ising=1.0 !assigning value to relevant parameters: k_B, J_ising

    integer, dimension(:,:,:), allocatable :: spin
    integer:: seed
    ! character(len=30):: charac_a, charac_b ! charac_b stores the name dump_pos

    seed=44859
    ! charac_b = "store_config"

    print*, "Welcome to this program! This program simulates a 3D ising model with all spins iniitally in the random state"
    print*, "Enter the value of KbT in units of J_ising"
    read*, T
    print*, "Enter the number of lattice points in one dimension for (LxLxL)"
    read*,L
    print*, "Enter the number of iterations"
    read*, niter

    allocate(spin(L,L,L))
    E=0.0d0 ! Instantaneous Energy of the lattice
    M=0.0d0 ! Instantaneous magnetization of the lattice
    N = L*L*L ! Total number of spins in lattice

    ! Initialize your lattice
    open(unit=71, file="initial_ising_3D.dat")
    p=0
    do i=1,L
        do j=1,L
            do k=1,L
             call RANDOM_NUMBER(r)
                ! spin(k,j,i) = -1
                if (r < 0.5) then
                    spin(i,j,k) = -1
                else
                    spin(i,j,k)=1
                end if
            ! Write down this configuration
            !write(71, fmt="(5g10.8)") float(i), float(j), float(p), float(spin(j,i))
            write(71,*) float(i), float(j), float(k), float(p), float(spin(i,j,k))
            end do
        end do
    end do

    close(71)

    ! Calculate Initial magnetization and energy
    do i=1,L
        do j=1,L
            do k=1,L
            a=i+1; b=i-1; c=j+1; d=j-1; f=k+1; g=k-1 !identifying the 4 neighbors of spin(j,i)

            ! Setting PBCs (Periodic Boundary conditions)
            if (i==L)a=1
            if (i==1)b=L
            if (j==L)c=1
            if (j==1)d=L
            if (k==L)f=1
            if (k==1)g=L

            E=E-J_ising*float((spin(i,j,k))*(spin(a,j,k)+spin(b,j,k)+spin(i,c,k)+spin(i,d,k)+spin(i,j,f)+spin(i,j,g)))
            M=M+spin(i,j,k)
            end do
        end do
    end do

    ! Calculate instantaneous magnetization per spin
    mag = M/float(N)
    E=E*0.5d0
    print*, "Initial energy E, E per spin is: ",E, E/float(N)
    print*, "Initial magentization M, M per spin is: ", M,M/float(N)

    ! INITIALIZATION COMPLETE
    ! -----------------------------
    ! EVOLVE TO REACH EQUILIBRIUM

    open(unit=10, file="ising_3d_T_L20_random.dat")
    do time=i,niter ! loop over number of MCS
        do mm=1,L
            do nn=1,L
                do oo=1,L
                call random_number(r); i=int(r*float(L))+1 !choosing a lattice site
                call random_number(r); j=int(r*float(L))+1
                call random_number(r); k=int(r*float(L))+1

                a=i+1;b=i-1;c=j+1;d=j-1;f=k+1;g=k-1 !identify the neighbors of spin(j,i)

                if(i==L)a=1; if(i==1)b=L; if(j==L)c=1; if(j==1)d=L; if(k==L)f=1; if(k==1)g=L !PBC
                ! BEFORE TRAIL FLIP
                Ei = -J_ising*float((spin(i,j,k))*(spin(a,j,k)+spin(b,j,k)+spin(i,c,k)+spin(i,d,k)+spin(i,j,f)+spin(i,j,g)))

                spin(i,j,k) =-spin(i,j,k) !TRIAL FLIP

                ! AFTER TRAIL FLIP
                Ef = -J_ising*float((spin(i,j,k))*(spin(a,j,k)+spin(b,j,k)+spin(i,c,k)+spin(i,d,k)+spin(i,j,f)+spin(i,j,g)))
                dE = Ef-Ei

                if(dE <=0.0) then
                    E=E+dE
                    M=M+(2.0*float(spin(i,j,k)))
                else
                    u=exp(-dE/(T))
                    call random_number(h)
                    if (h<u) then
                        E=E+dE
                        M=M+(2.0*float(spin(i,j,k))) ! Instantaneous mag. Of entire lattice
                    else
                        spin(i,j,k) = -spin(i,j,k) ! Trial flip not accepted; E & M not updated
                    end if
                end if
                end do
            end do
        end do
    write(10,*) time, M/float(N), E/float(N) ! Writing down mag and E/float(N) with no of iterations
    end do
    close(10)

end program ising_3d