!*****************************************************************************************
!>
!  Speed test for 1d-6d tensor product b-spline interpolation (subroutine version).
!
!### Results
!  ![Plot of results](https://raw.githubusercontent.com/jacobwilliams/bspline-fortran/master/src/tests/results/speed_test.png)

    program bspline_speed_test

    use bspline_module
    use bspline_kinds_module, only: wp, ip
    use pyplot_module
    use, intrinsic :: iso_fortran_env

    implicit none

    integer(ip),parameter :: nx = 8   !number of points
    integer(ip),parameter :: ny = 8
    integer(ip),parameter :: nz = 8
    integer(ip),parameter :: nq = 8
    integer(ip),parameter :: nr = 8
    integer(ip),parameter :: ns = 8

    integer(ip),parameter :: kx = 4    !order
    integer(ip),parameter :: ky = 4
    integer(ip),parameter :: kz = 4
    integer(ip),parameter :: kq = 4
    integer(ip),parameter :: kr = 4
    integer(ip),parameter :: ks = 4

    integer(ip),parameter :: n_cases = nx*ny*nz*nq*nr*ns

    real(wp) :: x(nx),y(ny),z(nz),q(nq),r(nr),s(ns)
    real(wp),dimension(nx) :: fcn_1d, bcoef_1d
    real(wp),dimension(nx,ny) :: fcn_2d, bcoef_2d
    real(wp),dimension(nx,ny,nz) :: fcn_3d, bcoef_3d
    real(wp),dimension(nx,ny,nz,nq) :: fcn_4d, bcoef_4d
    real(wp),dimension(nx,ny,nz,nq,nr) :: fcn_5d, bcoef_5d
    real(wp),dimension(nx,ny,nz,nq,nr,ns) :: fcn_6d, bcoef_6d
    real(wp) :: tx(nx+kx),ty(ny+ky),tz(nz+kz),tq(nq+kq),tr(nr+kr),ts(ns+ks)

    real(wp),dimension(3*kx)                     :: w1_1d
    real(wp),dimension(ky)                       :: w1_2d
    real(wp),dimension(3*max(kx,ky))             :: w2_2d
    real(wp),dimension(ky,kz)                    :: w1_3d
    real(wp),dimension(kz)                       :: w2_3d
    real(wp),dimension(3*max(kx,ky,kz))          :: w3_3d
    real(wp),dimension(ky,kz,kq)                 :: w1_4d
    real(wp),dimension(kz,kq)                    :: w2_4d
    real(wp),dimension(kq)                       :: w3_4d
    real(wp),dimension(3*max(kx,ky,kz,kq))       :: w4_4d
    real(wp),dimension(ky,kz,kq,kr)              :: w1_5d
    real(wp),dimension(kz,kq,kr)                 :: w2_5d
    real(wp),dimension(kq,kr)                    :: w3_5d
    real(wp),dimension(kr)                       :: w4_5d
    real(wp),dimension(3*max(kx,ky,kz,kq,kr))    :: w5_5d
    real(wp),dimension(ky,kz,kq,kr,ks)           :: w1_6d
    real(wp),dimension(kz,kq,kr,ks)              :: w2_6d
    real(wp),dimension(kq,kr,ks)                 :: w3_6d
    real(wp),dimension(kr,ks)                    :: w4_6d
    real(wp),dimension(ks)                       :: w5_6d
    real(wp),dimension(3*max(kx,ky,kz,kq,kr,ks)) :: w6_6d

    real(wp) :: val,sumval
    integer(ip)  :: i,j,k,l,m,n,idx,idy,idz,idq,idr,ids,iknot,iflag
    real :: tstart, tend
    type(pyplot) :: plt
    real(wp),dimension(6) :: cases_per_sec
    integer(ip) :: inbvx,inbvy,inbvz,inbvq,inbvr,inbvs,iloy,iloz,iloq,ilor,ilos
    integer :: istat  !! pyplot-fortran status flag

    inbvx = 1
    inbvy = 1
    inbvz = 1
    inbvq = 1
    inbvr = 1
    inbvs = 1
    iloy  = 1
    iloz  = 1
    iloq  = 1
    ilor  = 1
    ilos  = 1

    idx = 0
    idy = 0
    idz = 0
    idq = 0
    idr = 0
    ids = 0

    x = [ (real(i,wp), i=1,nx) ]
    y = [ (real(i,wp), i=1,ny) ]
    z = [ (real(i,wp), i=1,nz) ]
    q = [ (real(i,wp), i=1,nq) ]
    r = [ (real(i,wp), i=1,nr) ]
    s = [ (real(i,wp), i=1,ns) ]

    !evaluate the functions:

    do i=1,nx
        fcn_1d(i) = f1(x(i))
        do j=1,ny
            fcn_2d(i,j) = f2(x(i),y(j))
            do k=1,nz
                fcn_3d(i,j,k) = f3(x(i),y(j),z(k))
                do l=1,nq
                    fcn_4d(i,j,k,l) = f4(x(i),y(j),z(k),q(l))
                    do m=1,nr
                        fcn_5d(i,j,k,l,m) = f5(x(i),y(j),z(k),q(l),r(m))
                        do n=1,ns
                            fcn_6d(i,j,k,l,m,n) = f6(x(i),y(j),z(k),q(l),r(m),s(n))
                        end do
                    end do
                end do
            end do
        end do
    end do

    iknot = 0 !auto-compute the knots

    !initialize:

    call db1ink(x,nx,fcn_1d,kx,iknot,tx,bcoef_1d,iflag)
        if (iflag/=0) then
            write(*,*) 'iflag=',iflag
            error stop 'error initializing 1d spline.'
        end if
    call db2ink(x,nx,y,ny,fcn_2d,kx,ky,iknot,tx,ty,bcoef_2d,iflag)
        if (iflag/=0) then
            write(*,*) 'iflag=',iflag
            error stop 'error initializing 2d spline.'
        end if
    call db3ink(x,nx,y,ny,z,nz,fcn_3d,kx,ky,kz,iknot,tx,ty,tz,bcoef_3d,iflag)
        if (iflag/=0) then
            write(*,*) 'iflag=',iflag
            error stop 'error initializing 3d spline.'
        end if
    call db4ink(x,nx,y,ny,z,nz,q,nq,fcn_4d,kx,ky,kz,kq,iknot,tx,ty,tz,tq,bcoef_4d,iflag)
        if (iflag/=0) then
            write(*,*) 'iflag=',iflag
            error stop 'error initializing 4d spline.'
        end if
    call db5ink(x,nx,y,ny,z,nz,q,nq,r,nr,fcn_5d,kx,ky,kz,kq,kr,iknot,tx,ty,tz,tq,tr,bcoef_5d,iflag)
        if (iflag/=0) then
            write(*,*) 'iflag=',iflag
            error stop 'error initializing 5d spline.'
        end if
    call db6ink(x,nx,y,ny,z,nz,q,nq,r,nr,s,ns,fcn_6d,kx,ky,kz,kq,kr,ks,iknot,tx,ty,tz,tq,tr,ts,bcoef_6d,iflag)
        if (iflag/=0) then
            write(*,*) 'iflag=',iflag
            error stop 'error initializing 6d spline.'
        end if

    ! evaluate the interpolants:
    sumval = 0.0_wp
    call cpu_time(tstart)
    do i=1,nx
        do j=1,ny
           do k=1,nz
                do l=1,nq
                    do m=1,nr
                        do n=1,ns
                            call db1val(x(i),idx,tx,nx,kx,bcoef_1d,val,iflag,inbvx,w1_1d)
                            sumval = sumval + val
                        end do
                    end do
                end do
            end do
        end do
    end do
    call cpu_time(tend)
    write(*,*) ''
    write(*,*) '1D'
    write(*,*) 'result         :', sumval
    write(*,*) 'number of cases:', n_cases
    write(*,*) 'cases/sec      :', n_cases/(tend-tstart)
    cases_per_sec(1) = n_cases/(tend-tstart)

    sumval = 0.0_wp
    call cpu_time(tstart)
    do i=1,nx
        do j=1,ny
           do k=1,nz
                do l=1,nq
                    do m=1,nr
                        do n=1,ns
                            call db2val(x(i),y(j),idx,idy,tx,ty,&
                                        nx,ny,kx,ky,bcoef_2d,val,iflag,&
                                        inbvx,inbvy,iloy,w1_2d,w2_2d)
                           sumval = sumval + val
                       end do
                   end do
               end do
           end do
       end do
    end do
    call cpu_time(tend)
    write(*,*) ''
    write(*,*) '2D'
    write(*,*) 'result         :', sumval
    write(*,*) 'number of cases:', n_cases
    write(*,*) 'cases/sec      :', n_cases/(tend-tstart)
    cases_per_sec(2) = n_cases/(tend-tstart)

    sumval = 0.0_wp
    call cpu_time(tstart)
    do i=1,nx
        do j=1,ny
           do k=1,nz
                do l=1,nq
                    do m=1,nr
                        do n=1,ns
                            call db3val(x(i),y(j),z(k),idx,idy,idz,tx,ty,tz,&
                                        nx,ny,nz,kx,ky,kz,bcoef_3d,val,iflag,&
                                        inbvx,inbvy,inbvz,iloy,iloz,w1_3d,w2_3d,w3_3d)
                            sumval = sumval + val
                        end do
                    end do
                end do
            end do
        end do
    end do
    call cpu_time(tend)
    write(*,*) ''
    write(*,*) '3D'
    write(*,*) 'result         :', sumval
    write(*,*) 'number of cases:', n_cases
    write(*,*) 'cases/sec      :', n_cases/(tend-tstart)
    cases_per_sec(3) = n_cases/(tend-tstart)

    sumval = 0.0_wp
    call cpu_time(tstart)
    do i=1,nx
        do j=1,ny
           do k=1,nz
                do l=1,nq
                    do m=1,nr
                        do n=1,ns
                            call db4val(x(i),y(j),z(k),q(l),idx,idy,idz,idq,tx,ty,tz,tq,&
                                        nx,ny,nz,nq,kx,ky,kz,kq,bcoef_4d,val,iflag,&
                                        inbvx,inbvy,inbvz,inbvq,iloy,iloz,iloq,&
                                        w1_4d,w2_4d,w3_4d,w4_4d)
                            sumval = sumval + val
                        end do
                    end do
                end do
            end do
        end do
    end do
    call cpu_time(tend)
    write(*,*) ''
    write(*,*) '4D'
    write(*,*) 'result         :', sumval
    write(*,*) 'number of cases:', n_cases
    write(*,*) 'cases/sec      :', n_cases/(tend-tstart)
    cases_per_sec(4) = n_cases/(tend-tstart)

    sumval = 0.0_wp
    call cpu_time(tstart)
    do i=1,nx
        do j=1,ny
           do k=1,nz
                do l=1,nq
                    do m=1,nr
                        do n=1,ns
                            call db5val(x(i),y(j),z(k),q(l),r(m),idx,idy,idz,idq,idr,tx,ty,tz,tq,tr,&
                                        nx,ny,nz,nq,nr,kx,ky,kz,kq,kr,bcoef_5d,val,iflag,&
                                        inbvx,inbvy,inbvz,inbvq,inbvr,iloy,iloz,iloq,ilor,&
                                        w1_5d,w2_5d,w3_5d,w4_5d,w5_5d)
                            sumval = sumval + val
                        end do
                    end do
                end do
            end do
        end do
    end do
    call cpu_time(tend)
    write(*,*) ''
    write(*,*) '5D'
    write(*,*) 'result         :', sumval
    write(*,*) 'number of cases:', n_cases
    write(*,*) 'cases/sec      :', n_cases/(tend-tstart)
    cases_per_sec(5) = n_cases/(tend-tstart)

    sumval = 0.0_wp
    call cpu_time(tstart)
    do i=1,nx
        do j=1,ny
           do k=1,nz
                do l=1,nq
                    do m=1,nr
                        do n=1,ns
                            call db6val(x(i),y(j),z(k),q(l),r(m),s(n),idx,idy,idz,idq,idr,ids,&
                                        tx,ty,tz,tq,tr,ts,&
                                        nx,ny,nz,nq,nr,ns,kx,ky,kz,kq,kr,ks,bcoef_6d,val,iflag,&
                                        inbvx,inbvy,inbvz,inbvq,inbvr,inbvs,iloy,iloz,iloq,ilor,ilos,&
                                        w1_6d,w2_6d,w3_6d,w4_6d,w5_6d,w6_6d)
                            sumval = sumval + val
                        end do
                    end do
                end do
            end do
        end do
    end do
    call cpu_time(tend)
    write(*,*) ''
    write(*,*) '6D'
    write(*,*) 'result         :', sumval
    write(*,*) 'number of cases:', n_cases
    write(*,*) 'cases/sec      :', n_cases/(tend-tstart)
    cases_per_sec(6) = n_cases/(tend-tstart)

    !plot results in bar chart:
    call plt%initialize(grid=.false.,xlabel='Dimension',ylabel='Cases Per Second',&
                        title='Speed Test (Subroutine Interface)',legend=.false.,&
                        font_size = 20,&
                        axes_labelsize = 20,&
                        xtick_labelsize = 20,&
                        ytick_labelsize = 20)
    call plt%add_bar(x=real([1,2,3,4,5,6],real64),height=real(cases_per_sec,real64),&
                        label='Speed test runs',&
                        yscale='log',align='center',color='r',istat=istat)
    call plt%savefig('speed_test.png',istat=istat)

    contains

        function f1(x) result(f) !! 1d test function
        implicit none
        real(wp),intent(in) :: x
        real(wp) :: f
        f = x**1.1_wp + x**1.2_wp + x**1.3_wp + x**1.4_wp + x**1.5_wp + x**1.6_wp
        end function f1

        function f2(x,y) result(f) !! 2d test function
        implicit none
        real(wp),intent(in) :: x,y
        real(wp) :: f
        f = x**1.1_wp + y**1.2_wp + x**1.3_wp + y**1.4_wp + x**1.5_wp + y**1.6_wp
        end function f2

        function f3 (x,y,z) result(f) !! 3d test function
        implicit none
        real(wp),intent(in) :: x,y,z
        real(wp) :: f
        f = x**1.1_wp + y**1.2_wp + z**1.3_wp + x**1.4_wp + y**1.5_wp + z**1.6_wp
        end function f3

        function f4 (x,y,z,q) result(f) !! 4d test function
        implicit none
        real(wp),intent(in) :: x,y,z,q
        real(wp) :: f
        f = x**1.1_wp + y**1.2_wp + z**1.3_wp + q**1.4_wp + x**1.5_wp + y**1.6_wp
        end function f4

        function f5 (x,y,z,q,r) result(f) !! 5d test function
        implicit none
        real(wp),intent(in) :: x,y,z,q,r
        real(wp) :: f
        f = x**1.1_wp + y**1.2_wp + z**1.3_wp + q**1.4_wp + r**1.5_wp + x**1.6_wp
        end function f5

        function f6 (x,y,z,q,r,s) result(f) !! 6d test function
        implicit none
        real(wp),intent(in) :: x,y,z,q,r,s
        real(wp) :: f
        f = x**1.1_wp + y**1.2_wp + z**1.3_wp + q**1.4_wp + r**1.5_wp + s**1.6_wp
        end function f6

    end program bspline_speed_test


      FUNCTION amotry(p,y,psum,mp,np,ndim,funk,ihi,fac)
      INTEGER ihi,mp,ndim,np,NMAX
      REAL amotry,fac,p(mp,np),psum(np),y(mp),funk
      PARAMETER (NMAX=20)
      EXTERNAL funk
CU    USES funk
      INTEGER j
      REAL fac1,fac2,ytry,ptry(NMAX)
      fac1=(1.-fac)/ndim
      fac2=fac1-fac
      do 11 j=1,ndim
        ptry(j)=psum(j)*fac1-p(ihi,j)*fac2
11    continue
      ytry=funk(ptry)
      if (ytry.lt.y(ihi)) then
        y(ihi)=ytry
        do 12 j=1,ndim
          psum(j)=psum(j)-p(ihi,j)+ptry(j)
          p(ihi,j)=ptry(j)
12      continue
      endif
      amotry=ytry
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/amotry.for


module gasdevf90
  implicit none

contains
  real(8) function gasdev() result(gval)
    use mtmod
    implicit none
    real(8) :: FAC, R, V1, V2, X
    real(8), save :: GSET
    integer, save :: ISET = 0

    IF (ISET.EQ.0) THEN
       R = 99
       do while( R .ge. 1.0d0 )
          V1= 2.0d0*grnd() - 1.0d0
          V2= 2.0d0*grnd() - 1.0d0
          R = V1**2 + V2**2
       end do
       FAC = SQRT( -2.0d0*LOG(R)/R )
       GSET   = V1*FAC
       gval = V2*FAC
       ISET=1
    ELSE
       gval=GSET
       ISET=0
    ENDIF

    RETURN
  END function gasdev

end module gasdevf90


MODULE d_optimal_design
IMPLICIT NONE

! Latest revision - 4 March 1998

INTEGER, PARAMETER :: dp = SELECTED_REAL_KIND(14, 60)

CONTAINS

SUBROUTINE dopt(x, dim1, ncand, kin, n, nblock, in, blksiz, k, rstart, nrbar, &
                d, rbar, picked, lndet, xx, tol, zpz, wk, ifault)

!     Heuristic algorithm to pick N rows of X out of NCAND to maximize
!     the determinant of X'X, using the Fedorov exchange algorithm.

!     Code written by Alan Miller and Nam Nguyen from CSIRO, Melbourne,
!     Australia and published in Applied Statistics as AS 295.

!     This version differs from the official version in the following
!     ways:
!     1. When an existing design is augmented, the design points of the
!        existing design are NOT used as candidates for augmentation.
!     2. Some Fortran 90 features are used.

INTEGER, INTENT(IN)       :: dim1, ncand, kin, n, nblock, in(:),       &
                             blksiz(:), k, nrbar
INTEGER, INTENT(OUT)      :: picked(:), ifault
REAL, INTENT(IN)          :: x(:,:)
REAL (dp), INTENT(IN OUT) :: d(:), rbar(:), lndet, xx(:), tol(:), zpz(:,:), &
                             wk(:)
LOGICAL, INTENT(IN)       :: rstart

!     Local variables

INTEGER         :: i, j, nin, point, case, nb, block, l, pos, best, first,  &
                   last, cand, lastin, lstout, drop, rempos, bl, rpos,      &
                   last1, last2, first1, first2, pos1, pos2, block1,        &
                   block2, case1, case2, posi, posj, bestb1, bestb2,        &
                   bestp1, bestp2, rank, mxrank, inc, navail
REAL (dp)       :: temp, dmax, total
REAL            :: rand
LOGICAL         :: change
REAL (dp), PARAMETER :: one = 1.D0, minus1 = -1.D0, eps = EPSILON(one),   &
                        above1 = 1.0001D0, small = 1.D-04, hundrd = 100.D0
! REAL (dp)  :: det

!     Argument checks

ifault = 0
IF (dim1 < ncand) ifault = 1
IF (k > n) ifault = ifault + 2
IF (nrbar < k* (k-1)/2) ifault = ifault + 4
IF (k /= kin+nblock) ifault = ifault + 8
IF (nblock > 1) THEN
  l = 0
  DO block = 1, nblock
    l = l + blksiz(block)
  END DO
  IF (n /= l) ifault = ifault + 16
ELSE
  IF (n /= blksiz(1)) ifault = ifault + 16
END IF

!     NB = max(1, Nblock) so that we can force it to go through DO-loops
!     once.   NIN = no. of design points forced into the design.

nb = MAX(1, nblock)
nin = 0
DO i = 1, nb
  IF (in(i) < 0) GO TO 30
  IF (in(i) > 0) THEN
    IF (in(i) > blksiz(i)) GO TO 30
    nin = nin + in(i)
  END IF
END DO
navail = ncand - nin
IF (nin <= n) GO TO 40
30 ifault = ifault + 32

40 IF (ifault /= 0) RETURN
CALL clear(k, nrbar, d, rbar, ifault)

!     Set up an array of tolerances.

tol(1:k) = eps
block = 1
DO case = 1, ncand
  CALL getx(x, kin, nblock, block, xx, case)
  DO i = 1, k
    tol(i) = tol(i) + ABS(xx(i))
  END DO
END DO
temp = REAL(n) * eps / ncand
DO i = 1, k
  IF (i <= nblock) THEN
    tol(i) = eps
  ELSE
    tol(i) = tol(i) * temp
  END IF
END DO

!     Form initial Cholesky factorization.

pos = 1
DO block = 1, nb
  IF (rstart) THEN
    last1 = (in(block) + blksiz(block))/2
    inc = SQRT(REAL(ncand, KIND=dp) + small)
  END IF
  DO i = 1, blksiz(block)
    IF (rstart .AND. i > in(block)) THEN
      CALL RANDOM_NUMBER(rand)
      point = nin + 1 + navail * rand

!     If I <= LAST1, use a random point, otherwise find the candidate which
!     maximizes the rank, and then maximizes the subspace determinant for
!     that rank.

      IF (i > last1) THEN
        mxrank = 0
        lndet = -hundrd
        DO cand = nin+1, ncand, inc
          CALL getx(x, kin, nblock, block, xx, point)
          CALL modtr2(k, xx, d, rbar, tol, rank, total)
          IF (rank < mxrank) GO TO 90
          IF (rank == mxrank .AND. total < lndet) GO TO 90
          best = point
          mxrank = rank
          lndet = total * above1
          90 point = point + inc
          IF (point > ncand) point = point - navail
        END DO
        point = best
      END IF
      picked(pos) = point
    ELSE

!     Case in which a full design has been input, or points are to be
!     forced into the design.

      point = picked(pos)
    END IF

!     Augment the Cholesky factorization.

    CALL getx(x, kin, nblock, block, xx, point)
    CALL modtri(k, one, xx, d, rbar, tol)
    pos = pos + 1
  END DO
END DO

!     Adjust factorization in case of singular matrix.

CALL singm(k, nrbar, d, rbar, tol, wk, ifault)

!     If rank of input design < K, try replacing points.

IF (ifault == 0) GO TO 280

!     Find first row of Cholesky factorization with a zero multiplier.

180 DO pos = 1, k
  IF (d(pos) < tol(pos)) GO TO 200
END DO
GO TO 280

!     Find linear relationship between variable in position POS and the
!     previous variables.

200 l = pos - 1
DO i = 1, pos-1
  wk(i) = rbar(l)
  l = l + k - i - 1
END DO
CALL regcf(k, nrbar, d, rbar, wk, tol, wk, pos-1, ifault)

!     Find a candidate point which does not satisfy this linear
!     relationship.   Use a random start.

bl = 1
CALL RANDOM_NUMBER(rand)
case = nin + 1 + navail * rand
DO cand = 1, ncand
  CALL getx(x, kin, nblock, bl, xx, case)
  total = xx(pos) - DOT_PRODUCT( wk(1:pos-1), xx(1:pos-1) )
  IF (ABS(total) > hundrd * tol(pos)) GO TO 240
  case = case + 1
  IF (case > ncand) case = nin + 1
END DO

!     Failed to find any candidate point which would make the design
!     of higher rank.

ifault = -1
RETURN

!     Before adding the point, find one which it can replace without
!     lowering the rank.

240 bl = 0
temp = one - small
pos = in(1) + 1
DO block = 1, nb
  DO j = in(block)+1, blksiz(block)
    l = picked(pos)
    CALL getx(x, kin, nblock, block, xx, l)
    CALL bksub2(rbar, k, xx, wk)
    total = DOT_PRODUCT( wk(1:k), wk(1:k)/d(1:k) )
    IF (total < temp) THEN
      temp = total
      rempos = pos
      bl = block
    END IF
    pos = pos + 1
  END DO
  IF (block < nblock) pos = pos + in(block+1)
END DO

!     If BL = 0 it means that any point removed from the existing design
!     would reduce the rank.

IF (bl == 0) THEN
  ifault = -1
  RETURN
END IF

!     Add candidate CASE in block BL, then delete the design point
!     already in that position.

CALL getx(x, kin, nblock, bl, xx, case)
CALL modtri(k, one, xx, d, rbar, tol)
l = picked(rempos)
CALL getx(x, kin, nblock, bl, xx, l)
CALL modtri(k, minus1, xx, d, rbar, tol)
picked(rempos) = case
!     write(*, 8000) case, l, bl
!8000 format(' Replacing candidate ', i4, ' with candidate ', i4,
!    +    ' in block', i3)
GO TO 180
!----------------------------------------------------------------------

!     Design is now of full rank.

!     Calculate z'z for all candidate points.
!     z is the solution of R'z = x, so that z'z = x'.inv(X'X).x
!     WK holds sqrt(D) times vector z on return from BKSUB2.

280 DO block = 1, nb
  DO case = nin+1, ncand
    CALL getx(x, kin, nblock, block, xx, case)
    CALL bksub2(rbar, k, xx, wk)
    temp = DOT_PRODUCT( wk(1:k), wk(1:k)/d(1:k) )
    zpz(case, block) = temp
  END DO
END DO
!     det = one
!     do 600 i = 1, k
! 600 det = det * d(i)
!     write(*, 9020) det
!9020 format(' Det = ', g14.6)
!----------------------------------------------------------------------

!     Start of Fedorov exchange algorithm

lastin = 0
lstout = 0
320 change = .false.
last = 0
DO block = 1, nb
  first = last + 1 + in(block)
  last = last + blksiz(block)
  dmax = small
  best = 0

!     Start at a random position within the block.
!     I = no. of point being considered for deletion.

  CALL RANDOM_NUMBER(rand)
  pos = first + (blksiz(block) - in(block)) * rand
  DO case = in(block)+1, blksiz(block)
    pos = pos + 1
    IF (pos > last) pos = first
    i = picked(pos)
    IF (i == lastin) CYCLE
    CALL getx(x, kin, nblock, block, xx, i)
    CALL bksub2(rbar, k, xx, wk)
    CALL bksub1(rbar, k, wk, wk, tol, d)

!     Cycle through the candidates for exchange, using a random start.
!     J = no. of point being considered for addition.

    CALL RANDOM_NUMBER(rand)
    j = nin + 1 + navail * rand
    DO cand = nin+1, ncand
      j = j + 1
      IF (j > ncand) j = nin+1
      IF (j == i .OR. j == lstout) CYCLE
!... The Cauchy-Schwarz test.
      temp = zpz(j,block) - zpz(i,block)
      IF (temp < dmax) CYCLE
!
      CALL getx(x, kin, nblock, block, xx, j)
      total = DOT_PRODUCT( xx(1:k), wk(1:k) )
      temp = temp + total**2 - zpz(i,block)*zpz(j,block)
      IF (temp > dmax) THEN
        dmax = temp * above1
        best = j
        rempos = pos
        drop = i
      END IF
    END DO
  END DO

!     Exchange points BEST & DROP in position REMPOS, if the determinant
!     is increased.

  IF (best /= 0) THEN
    change = .true.
    IF (nb == 1) THEN
      lastin = best
      lstout = drop
    END IF

!     Add the new point, BEST, first to avoid ill-conditioning.
!     Update z'z.

    CALL getx(x, kin, nblock, block, xx, best)
    CALL bksub2(rbar, k, xx, wk)
    CALL bksub1(rbar, k, wk, wk, tol, d)
    CALL modtri(k, one, xx, d, rbar, tol)
    temp = one + zpz(best,block)
    DO bl = 1, nb
      DO case = 1, ncand
        CALL getx(x, kin, nblock, bl, xx, case)
        total = DOT_PRODUCT( xx(1:k), wk(1:k) )
        zpz(case,bl) = zpz(case,bl) - total**2 / temp
      END DO
    END DO

!     Remove the point DROP, and update z'z.

    CALL getx(x, kin, nblock, block, xx, drop)
    CALL bksub2(rbar, k, xx, wk)
    CALL bksub1(rbar, k, wk, wk, tol, d)
    CALL modtri(k, minus1, xx, d, rbar, tol)
    temp = one - zpz(drop,block)
    DO bl = 1, nb
      DO case = 1, ncand
        CALL getx(x, kin, nblock, bl, xx, case)
        total = DOT_PRODUCT( xx(1:k), wk(1:k) )
        zpz(case,bl) = zpz(case,bl) + total**2 / temp
      END DO
    END DO

!         det = one
!         do 620 i = 1, k
! 620     det = det * d(i)
!         write(*, 9010) block, best, drop, dmax, det
!9010     format(' block: ', i3, '  Add : ', i4, '  Drop: ', i4,
!    +           '  dmax: ', f9.3, '  Det: ', g14.6)
    picked(rempos) = best
  END IF
END DO

!     Repeat until there is no further improvement

IF (change) GO TO 320
!---------------------------------------------------------------------

!     If there is more than one block, try swapping treatments between
!     blocks.   This is the Cook & Nachtsheim (1989) algorithm.

IF (nblock <= 1) GO TO 500
!...  RPOS is the position of the first element in RBAR after the rows
!     for the block constants.
rpos = nblock * k - nblock * (nblock + 1)/2 + 1

430 last1 = 0
!...  POS1 & POS2 will hold the positions of the start of the means of
!     the X-variables in the two blocks being considered in RBAR.
pos1 = nblock
dmax = small
change = .false.
DO block1 = 1, nblock-1
  first1 = last1 + 1 + in(block1)
  last1 = last1 + blksiz(block1)
  last2 = last1
  pos2 = pos1 + k - 1 - block1
  DO block2 = block1+1, nblock
    first2 = last2 + 1 + in(block2)
    last2 = last2 + blksiz(block2)
    DO case1 = in(block1)+1, blksiz(block1)
      posi = first1 - 1 + case1
      i = picked(posi)
      CALL getx(x, kin, nblock, block, xx, i)
      zpz(1:kin,1) = xx(nblock+1:nblock+kin)
      DO case2 = in(block2)+1, blksiz(block2)
        posj = first2 - 1 + case2
        j = picked(posj)
        IF (i == j) CYCLE
        CALL getx(x, kin, nblock, block, xx, j)
        zpz(1:kin,2) = xx(nblock+1:nblock+kin)
!...  Pass the orthogonal factorization to DELTA with the top nblock
!     rows removed, i.e. without that part relating to the blocks.
        CALL delta(kin, zpz(:,1), zpz(:,2), rbar(pos1:), rbar(pos2:),   &
             blksiz(block1), blksiz(block2), d(nblock+1:), rbar(rpos:), temp)
        IF (temp > dmax) THEN
          dmax = temp * above1
          bestb1 = block1
          bestb2 = block2
          bestp1 = posi
          bestp2 = posj
          change = .true.
        END IF
      END DO
    END DO
    pos2 = pos2 + k - 1 - block2
  END DO
  pos1 = pos1 + k - 1 - block1
END DO

!     If CHANGE = .TRUE. then make the swap, otherwise the search ends.

IF (change) THEN
  i = picked(bestp1)
  j = picked(bestp2)
  CALL getx(x, kin, nblock, bestb2, xx, i)
  CALL modtri(k, one, xx, d, rbar, tol)
  CALL getx(x, kin, nblock, bestb1, xx, j)
  CALL modtri(k, one, xx, d, rbar, tol)
  CALL getx(x, kin, nblock, bestb1, xx, i)
  CALL modtri(k, minus1, xx, d, rbar, tol)
  CALL getx(x, kin, nblock, bestb2, xx, j)
  CALL modtri(k, minus1, xx, d, rbar, tol)
  picked(bestp1) = j
  picked(bestp2) = i
!       det = one
!       do 610 i = 1, k
! 610   det = det * d(i)
!       write(*, 4000) dmax, det
!4000   format(' DMAX = ', f9.4, 5x, 'New det. = ', g13.5)
  GO TO 430
END IF

!     Calculate log of determinant.

500 lndet = SUM( LOG(d(1:k)) )

RETURN
END SUBROUTINE dopt

SUBROUTINE modtri(np, weight, xrow, d, rbar, tol)

!     ALGORITHM AS75.1 with minor modifications

!     Modify a triangular (Cholesky) decomposition.
!     Calling this routine updates d and rbar by adding another design
!     point with weight = WEIGHT, which may be negative.

!     *** WARNING  Array XROW is overwritten.

INTEGER, INTENT(IN)       :: np
REAL (dp), INTENT(IN)     :: weight, tol(:)
REAL (dp), INTENT(IN OUT) :: xrow(:), d(:), rbar(:)

!     Local variables

INTEGER              :: i, k, nextr
REAL (dp)            :: w, xi, di, wxi, dpi, cbar, sbar, xk
REAL (dp), PARAMETER :: zero = 0.D0

w = weight
nextr = 1
DO i = 1, np

!     Skip unnecessary transformations.   Test on exact zeroes must be
!     used or stability can be destroyed.

  IF (w == zero) RETURN

  xi = xrow(i)
  IF (ABS(xi) < tol(i)) THEN
    nextr = nextr + np - i
    CYCLE
  END IF

  di = d(i)
  wxi = w*xi
  dpi = di + wxi*xi

!     Test for new singularity.

  IF (dpi < tol(i)) THEN
    dpi = zero
    cbar = zero
    sbar = zero
    w = zero
  ELSE
    cbar = di/dpi
    sbar = wxi/dpi
    w = cbar*w
  END IF

  d(i) = dpi
  DO k = i + 1, np
    xk = xrow(k)
    xrow(k) = xk - xi*rbar(nextr)
    rbar(nextr) = cbar*rbar(nextr) + sbar*xk
    nextr = nextr + 1
  END DO
END DO

RETURN
END SUBROUTINE modtri

SUBROUTINE modtr2(np, xrow, d, rbar, tol, rank, lndet)

!     ALGORITHM AS75.1 with modifications.
!     Calculate the effect of an update of a QR factorization upon the
!     rank and determinant, without changing D or RBAR.

!     *** WARNING  Array XROW is overwritten.

INTEGER, INTENT(IN)       :: np
INTEGER, INTENT(OUT)      :: rank
REAL (dp), INTENT(IN)     :: tol(:)
REAL (dp), INTENT(IN OUT) :: xrow(:), d(:), rbar(:)
REAL (dp), INTENT(OUT)    :: lndet

!     Local variables

INTEGER              :: i, k, nextr, j
REAL (dp)            :: w, xi, di, wxi, dpi, cbar, xk
REAL (dp), PARAMETER :: zero = 0.0_dp, one = 1.0_dp

w = one
rank = 0
lndet = zero
nextr = 1
DO i = 1, np

!     Skip unnecessary transformations.   Test on exact zeroes must be
!     used or stability can be destroyed.

  IF (w == zero) THEN
    DO j = i, np
      IF (d(j) > tol(j)) THEN
        rank = rank + 1
        lndet = lndet + LOG(d(j))
      END IF
    END DO
    RETURN
  END IF

  xi = xrow(i)
  IF (ABS(xi) < tol(i)) THEN
    IF (d(i) > tol(i)) THEN
      rank = rank + 1
      lndet = lndet + LOG(d(i))
    END IF
    nextr = nextr + np - i
    CYCLE
  END IF

  di = d(i)
  wxi = w*xi
  dpi = di + wxi*xi

!     Test for new singularity.

  IF (dpi < tol(i)) THEN
    dpi = zero
    cbar = zero
    w = zero
  ELSE
    cbar = di/dpi
    w = cbar*w
    lndet = lndet + LOG(dpi)
    rank = rank + 1
  END IF

  DO k = i + 1, np
    xk = xrow(k)
    xrow(k) = xk - xi*rbar(nextr)
    nextr = nextr + 1
  END DO
END DO

RETURN
END SUBROUTINE modtr2

SUBROUTINE getx(x, kin, nblock, block, xx, case)

!     Copy one case from X to XX.

INTEGER, INTENT(IN)    :: kin, nblock, block, case
REAL (dp), INTENT(OUT) :: xx(:)
REAL, INTENT(IN)       :: x(:,:)

!     Local variables.

INTEGER              :: i
REAL (dp), PARAMETER :: zero = 0.0_dp, one = 1.0_dp

DO i = 1, nblock
  IF (i /= block) THEN
    xx(i) = zero
  ELSE
    xx(i) = one
  END IF
END DO
xx(nblock+1:nblock+kin) = x(case, 1:kin)

RETURN
END SUBROUTINE getx

SUBROUTINE bksub1(rbar, k, rhs, soln, tol, d)

!     Solves  D R y = z for y (SOLN), where z = RHS.
!     RBAR is an upper-triangular matrix with implicit 1's on it's
!     diagonal, stored by rows.

INTEGER, INTENT(IN)    :: k
REAL (dp), INTENT(IN)  :: rbar(:), rhs(:), tol(:), d(:)
REAL (dp), INTENT(OUT) :: soln(:)

!     Local variables

INTEGER              :: row, col, pos
REAL (dp)            :: temp
REAL (dp), PARAMETER :: zero = 0.0_dp

pos = k * (k - 1) / 2
DO row = k, 1, -1
  IF (d(row) > tol(row)) THEN
    temp = rhs(row) / d(row)
    DO col = k, row+1, -1
      temp = temp - rbar(pos) * soln(col)
      pos = pos - 1
    END DO
    soln(row) = temp
  ELSE
    pos = pos - k + row
    soln(row) = zero
  END IF
END DO

RETURN
END SUBROUTINE bksub1

SUBROUTINE singm(np, nrbar, d, rbar, tol, work, ifault)

!     Modified from:
!     ALGORITHM AS274.5  APPL. STATIST. (1992) VOL.41, NO.2

!     Checks for singularities, and adjusts orthogonal reductions produced by
!     AS75.1.

!     Auxiliary routine called: MODTRI

INTEGER, INTENT(IN)       :: np, nrbar
INTEGER, INTENT(OUT)      :: ifault
REAL (dp), INTENT(IN OUT) :: d(:), rbar(:), work(:)
REAL (dp), INTENT(IN)     :: tol(:)

!     Local variables

REAL (dp)            :: temp
REAL (dp), PARAMETER :: zero = 0.0_dp
INTEGER              :: col, pos, row, np2, pos2

!     Check input parameters

ifault = 0
IF (np <= 0) ifault = 1
IF (nrbar < np*(np-1)/2) ifault = ifault + 2
IF (ifault /= 0) RETURN

work(1:np) = SQRT(d(1:np))

DO col = 1, np

!     Set elements within RBAR to zero if they are less than TOL(COL) in
!     absolute value after being scaled by the square root of their row
!     multiplier.

  temp = tol(col)
  pos = col - 1
  DO row = 1, col-1
    IF (ABS(rbar(pos)) * work(row) < temp) rbar(pos) = zero
    pos = pos + np - row - 1
  END DO

!     If diagonal element is near zero, set it to zero, and use MODTRI
!     to augment the projections in the lower rows of the factorization.

  IF (work(col) <= temp) THEN
    ifault = ifault - 1
    IF (col < np) THEN
      np2 = np - col
      pos2 = pos + np - col + 1
      IF (np2 > 1) THEN
        CALL modtri(np2, d(col), rbar(pos+1:), d(col+1:), rbar(pos2:), tol)
      ELSE
        CALL modtri(1, d(col), rbar(pos+1:), d(col+1:), rbar, tol)
      END IF
      rbar(pos+1:pos2-1) = zero
    END IF
    d(col) = zero
  END IF
END DO

RETURN
END SUBROUTINE singm

SUBROUTINE xxtr(np, d, rbar, nreq, trace, rinv)

!     Calculate the trace of the inverse of X'X (= R'R).

INTEGER, INTENT(IN)    :: np, nreq
REAL (dp), INTENT(IN)  :: d(:), rbar(:)
REAL (dp), INTENT(OUT) :: trace, rinv(:)

!     Local variables

INTEGER              :: pos, row, col
REAL (dp), PARAMETER :: one = 1.D0, zero = 0.D0

!     Get the inverse of R

CALL inv(np, rbar, nreq, rinv)

!     Trace = the sum of the diagonal elements of RINV * (1/D) * (RINV)'

trace = zero
pos = 1
DO row = 1, nreq
  trace = trace + one / d(row)
  DO col = row+1, nreq
    trace = trace + rinv(pos)**2 / d(col)
    pos = pos + 1
  END DO
END DO

RETURN
END SUBROUTINE xxtr

SUBROUTINE inv(np, rbar, nreq, rinv)

!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2

!     Invert first NREQ rows and columns of Cholesky factorization
!     produced by AS 75.1.

INTEGER, INTENT(IN)    :: np, nreq
REAL (dp), INTENT(IN)  :: rbar(:)
REAL (dp), INTENT(OUT) :: rinv(:)

!     Local variables.

INTEGER              :: pos, row, col, start, k, pos1, pos2
REAL (dp)            :: sum
REAL (dp), PARAMETER :: zero = 0.0_dp

!     Invert RBAR ignoring row multipliers, from the bottom up.

pos = nreq * (nreq-1)/2
DO row = nreq-1, 1, -1
  start = (row-1) * (np+np-row)/2 + 1
  DO col = nreq, row+1, -1
    pos1 = start
    pos2 = pos
    sum = zero
    DO k = row+1, col-1
      pos2 = pos2 + nreq - k
      sum = sum - rbar(pos1) * rinv(pos2)
      pos1 = pos1 + 1
    END DO
    rinv(pos) = sum - rbar(pos1)
    pos = pos - 1
  END DO
END DO

RETURN
END SUBROUTINE inv

SUBROUTINE clear(np, nrbar, d, rbar, ier)

!     Modified version of:
!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2

!     Sets arrays to zero prior to calling INCLUD

INTEGER, INTENT(IN)    :: np, nrbar
INTEGER, INTENT(OUT)   :: ier
REAL (dp), INTENT(OUT) :: d(:), rbar(:)

!     Local variable

REAL (dp), PARAMETER :: zero = 0.0_dp

!     Some checks.

ier = 0
IF (np < 1) ier = 1
IF (nrbar < np*(np-1)/2) ier = ier + 2
IF (ier /= 0) RETURN

d(1:np) = zero
rbar(1:nrbar) = zero

RETURN
END SUBROUTINE clear

SUBROUTINE regcf(np, nrbar, d, rbar, thetab, tol, beta, nreq, ier)

!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2

!     Modified version of AS75.4 to calculate regression coefficients for the
!     first NREQ variables, given an orthogonal reduction from AS75.1.

INTEGER, INTENT(IN)       :: np, nrbar, nreq
INTEGER, INTENT(OUT)      :: ier
REAL (dp), INTENT(IN)     :: rbar(:), thetab(:), tol(:)
REAL (dp), INTENT(IN OUT) :: d(:)
REAL (dp), INTENT(OUT)    :: beta(:)

!     Local variables

INTEGER              :: i, j, nextr
REAL (dp), PARAMETER :: zero = 0.0_dp

!     Some checks.

ier = 0
IF (np < 1) ier = 1
IF (nrbar < np*(np-1)/2) ier = ier + 2
IF (nreq < 1 .OR. nreq > np) ier = ier + 4
IF (ier /= 0) RETURN

DO i = nreq, 1, -1
  IF (SQRT(d(i)) < tol(i)) THEN
    beta(i) = zero
    d(i) = zero
    CYCLE
  END IF
  beta(i) = thetab(i)
  nextr = (i-1) * (np+np-i)/2 + 1
  DO j = i+1, nreq
    beta(i) = beta(i) - rbar(nextr) * beta(j)
    nextr = nextr + 1
  END DO
END DO

RETURN
END SUBROUTINE regcf

SUBROUTINE bksub2(rbar, k, rhs, soln)

!     Solves  R'(sqrt(D).z) = x where (sqrt(D).z) = SOLN and x = RHS.
!     RBAR is an upper-triangular matrix with implicit 1's on it's diagonal,
!     stored by rows.

INTEGER, INTENT(IN)    :: k
REAL (dp), INTENT(IN)  :: rbar(:), rhs(:)
REAL (dp), INTENT(OUT) :: soln(:)

!     Local variables

INTEGER   :: row, col, pos
REAL (dp) :: temp

soln(1) = rhs(1)
DO row = 2, k
  temp = rhs(row)
  pos = row - 1
  DO col = 1, row-1
    temp = temp - rbar(pos) * soln(col)
    pos = pos + k - col - 1
  END DO
  soln(row) = temp
END DO

RETURN
END SUBROUTINE bksub2

SUBROUTINE delta(k, xj, xl, xbari, xbark, ni, nk, d, rbar, fn_val)

!     Calculate the delta function for the swap of case J in block I
!     with case L in block K.   Uses the method of Cook & Nachtsheim.

INTEGER, INTENT(IN)    :: k, ni, nk
REAL (dp), INTENT(IN ) :: xj(:), xl(:), xbari(:), xbark(:), d(:), rbar(:)
REAL (dp), INTENT(OUT) :: fn_val

!     Local variables

INTEGER              :: i
REAL (dp)            :: const, temp, e11, e12, e21, e22
REAL (dp)            :: z(k,3), a(k), b(k), diff(k)
REAL (dp), PARAMETER :: one = 1.0_dp, two = 2.0_dp

!     Calculate vectors DIFF, A & B.

const = two - one/ni - one/nk
DO i = 1, k
  temp = xj(i) - xl(i)
  diff(i) = -temp
  a(i) = temp - xbari(i) + xbark(i)
  b(i) = a(i) - const * temp
END DO

!     Calculate the z-vectors by back-substitution.
!     Z1 for A, Z2 for B and Z3 for DIFF.
!     N.B. The solutions returned from BKSUB2 have the I-th element
!     multiplied by sqrt(D(I)).

CALL bksub2(rbar, k, a, z(:,1))
CALL bksub2(rbar, k, b, z(:,2))
CALL bksub2(rbar, k, diff, z(:,3))

!     Calculate the elements E11, E12, E21 & E22 as dot-products of the
!     appropriate z-vectors.

e11 = DOT_PRODUCT( z(:,3), z(:,1)/d )
e12 = DOT_PRODUCT( z(:,3), z(:,3)/d )
e21 = DOT_PRODUCT( z(:,2), z(:,1)/d )
e22 = DOT_PRODUCT( z(:,2), z(:,3)/d )

!     Return the determinant of the matrix:   E11+1    E12
!     (minus 1)                                E21    E22+1

fn_val = (e11+one) * (e22+one) - e12 * e21 - one

RETURN
END SUBROUTINE delta

END MODULE d_optimal_design


c=======================================================================
c
c     GENMUL
c
c     Generate an observation from the multinomial distribution
c
c-----------------------------------------------------------------------
c
c-----------------------------------------------------------------------
      SUBROUTINE genmul(n,p,ncat,ix)
c-----------------------------------------------------------------------
c
c     INPUT :
c      N : number of events that will be classified into one of
c           the categories 1..NCAT                               integer
c      P : Vector of probabilities.  P(i) is the probability that
c           an event will be classified into category i.  Thus, P(i)
c           must be [0,1]. Only the first NCAT-1 P(i) must be defined
c           since P(NCAT) is 1.0 minus the sum of the first
c           NCAT-1 P(i).                                 DOUBLE (NCAT-1)
c      NCAT : number of categories.  Length of P and IX           integer
c
c     OUTPUT :
c      IX : Observation from multinomial distribution.  All IX(i)
c            will be nonnegative and their sum will be N.   INTEGER (NCAT)
c
c     Method
c
c     Algorithm from page 559 of Devroye, Luc 1986
c     Non-Uniform Random Variate Generation. Springer-Verlag, New York
c
c------------------------------------------------------------------------
c
c     scalar arguments
      INTEGER n,ncat
c
c     array arguments
      DOUBLE PRECISION p(*)
      INTEGER ix(*)
c
c     local scalars
      DOUBLE PRECISION prob,sum
      INTEGER i,icat,ntot
c
c     external functions
      INTEGER ignbin
      EXTERNAL ignbin
c
c     intrinsic functions
      INTRINSIC abs
c
c     executable statements
c     check arguments
c     see Rand.c
c     initialize variables
      ntot = n
      sum = 1.0
      DO 20,i = 1,ncat
          ix(i) = 0
   20 CONTINUE
c
c     generate the observation
      DO 30,icat = 1,ncat - 1
          prob = p(icat)/sum
          ix(icat) = ignbin(ntot,prob)
          ntot = ntot - ix(icat)
          IF (ntot.LE.0) RETURN
          sum = sum - p(icat)
   30 CONTINUE
      ix(ncat) = ntot
c
      RETURN
      END


subroutine regcoil_read_bnorm()

  use regcoil_variables, only: load_bnorm, bnorm_filename, ntheta_plasma, nzeta_plasma, &
       Bnormal_from_plasma_current, theta_plasma, zeta_plasma, nfp, curpol, verbose, &
       nbf, bfn, bfm, bfs, bfc
  use safe_open_mod
  use stel_constants
  use stel_kinds

  implicit none

  integer :: iunit, i, mm, nn, itheta, izeta, index, iflag, num_modes_added
  real(dp) :: bf
  integer :: tic, toc, countrate

  call system_clock(tic,countrate)

  if (allocated(Bnormal_from_plasma_current)) deallocate(Bnormal_from_plasma_current)
  allocate(Bnormal_from_plasma_current(ntheta_plasma,nzeta_plasma),stat=iflag)
  if (iflag .ne. 0) stop 'regcoil_read_bnorm Allocation error!'

  Bnormal_from_plasma_current = 0

  if (.not. load_bnorm) then
     if (verbose) print *,"Not reading a bnorm file, so Bnormal_from_plasma_current arrays will all be 0."
     return
  end if

  ! using data from FOCUS file ; by czhu
  if ( nbf > 0 ) then
     if (verbose) print *, "Using Bn coefficients in FOCUS file."

     do izeta = 1,nzeta_plasma
        do itheta = 1,ntheta_plasma
           do i = 1, nbf
              Bnormal_from_plasma_current(itheta,izeta) = Bnormal_from_plasma_current(itheta,izeta) + &
                   bfc(i)*cos(bfm(i)*theta_plasma(itheta) - bfn(i)*zeta_plasma(izeta)) + &
                   bfs(i)*sin(bfm(i)*theta_plasma(itheta) - bfn(i)*zeta_plasma(izeta))
              ! In FOCUS, the angle is using mu-nv. Both cosine and sin terms are retained.
              ! For more information, please check https://princetonuniversity.github.io/FOCUS/rdsurf.pdf.
           enddo
        end do
     end do
     return
  endif

  if (verbose) print *,"Loading B_normal on the plasma surface due to plasma current from file ",trim(bnorm_filename)

  call safe_open(iunit, i, trim(bnorm_filename), 'old', 'formatted')
  if (i .ne. 0 ) then
     stop 'Unable to open bnorm_filename.'
  end if

  num_modes_added = 0
  do
     read(iunit,*,iostat = i) mm, nn, bf
     if (i .ne. 0) exit
     num_modes_added = num_modes_added+1
     !print *,"  Adding a mode with m=",mm,",  n=",nn
     do izeta = 1,nzeta_plasma
        do itheta = 1,ntheta_plasma
           Bnormal_from_plasma_current(itheta,izeta) = Bnormal_from_plasma_current(itheta,izeta) + &
                bf*sin(mm*theta_plasma(itheta) + nn*nfp*zeta_plasma(izeta))

           ! To see that it should be (mu+nv) rather than (mu-nv) in the above line, you can examine
           ! BNORM/Sources/bn_fouri.f (where the arrays in the bnorm files are computed)
           ! or
           ! either NESCOIL/Sources/bnfld.f (where bnorm files are read)
        end do
     end do
  end do

  close(iunit)

  ! BNORM scales B_n by curpol=(2*pi/nfp)*bsubv(m=0,n=0)
  ! where bsubv is the extrapolation to the last full mesh point of
  ! bsubvmnc.  Let's undo this scaling now.
  Bnormal_from_plasma_current = Bnormal_from_plasma_current * curpol

!!$  ! I'll use an explicit loop here so there is no ambiguity about the order of theta vs zeta in the 1D vector:
!!$  do izeta = 1,nzeta_plasma
!!$     do itheta = 1,ntheta_plasma
!!$        Bnormal_from_plasma_current_1D((izeta-1)*ntheta_plasma+itheta) = Bnormal_from_plasma_current(itheta,izeta)
!!$     end do
!!$  end do
!!$  ! The syntax in the next line might be faster? But I should check the order.
!!$  !Bnormal_from_plasma_current_1D = reshape(Bnormal_from_plasma_current, (/ntheta_plasma*nzeta_plasma/))

  call system_clock(toc)
  if (num_modes_added>0) then
     if (verbose) print *,"Number of modes read from bnorm file:",num_modes_added
  else
     print *,"WARNING!!! No modes found in the bnorm file."
  end if
  if (verbose) print *,"Done reading B_normal on the plasma surface due to plasma current."
  if (verbose) print *,"Took ",real(toc-tic)/countrate," sec."

end subroutine  regcoil_read_bnorm


program test_hankel2
  use types, only: dp
  use constants, only: i_
  use utils, only: assert
  use special, only: hankel2
  implicit none

  ! check a few value against the ones computed with SciPy:
  integer :: i
  real(dp), parameter :: eps = 1d-12
  real(dp), parameter :: ordersReal(7) = [0.0_dp,  1.0_dp,  2.0_dp,  3.3_dp,  7.0_dp,  -1.5_dp,  -4.0_dp]
  real(dp), parameter :: xReal(7) = [1.1_dp,  3.0_dp,  10.0_dp,  2.1_dp,  5.5_dp,  1.1_dp,  2.0_dp]
  real(dp), parameter :: xImag(7) = [0.1_dp,  -0.3_dp,  2.5_dp,  -1.8_dp,  2.0_dp,  0.5_dp,  1.5_dp]
  complex(dp), parameter :: correctResultsRealArg(7) =[(0.719622018527511_dp, -0.162163202926887_dp),&
       (0.339058958525936_dp, -0.324674424791800_dp), (0.254630313685121_dp, 0.005868082442209_dp),&
       (0.101980666751263_dp, 1.267804061151291_dp), (0.086601225791613_dp, 0.874921069456376_dp),&
       (-0.991692967169576_dp, 0.271278757000343_dp), (0.033995719807567_dp, 2.765943226330601_dp)]

  complex(dp), parameter :: correctResultsComplexArg(7) = [(0.790792692816186_dp, -0.213286580016703_dp),&
       (0.265992551112011_dp, -0.228390788709935_dp), (2.907712660238902_dp, -0.270368699569746_dp),&
       (-0.284672444442432_dp, 0.000042806070897_dp), (0.298090132076709_dp, 0.410825383335137_dp),&
       (-0.990558382646087_dp, 0.671547450313583_dp), (0.630157438174331_dp, -0.261220122940811_dp)]

  ! test real argument with real order:
  do i = 1,7
     call assert(abs(hankel2(ordersReal(i), xReal(i)) - correctResultsRealArg(i)) < eps)
  end do

  ! test real argument with integer order:
  ! test those to machine precision
  call assert(abs(hankel2(0, xReal(1)) - hankel2FromDef(0, xReal(1))) <= epsilon(1.0_dp))
  call assert(abs(hankel2(1, xReal(2)) - hankel2FromDef(1, xReal(2))) <= epsilon(1.0_dp))
  call assert(abs(hankel2(2, xReal(3)) - hankel2FromDef(2, xReal(3))) <= epsilon(1.0_dp))
  call assert(abs(hankel2(3, xReal(4)) - hankel2FromDef(3, xReal(4))) <= epsilon(1.0_dp))
  call assert(abs(hankel2(4, xReal(5)) - hankel2FromDef(4, xReal(5))) <= epsilon(1.0_dp))

  ! test complex data with real order:
  do i = 1,7
     call assert(abs(hankel2(ordersReal(i), xReal(i)+i_*xImag(i)) - correctResultsComplexArg(i)) < eps)
  end do

contains

  function hankel2FromDef(order, x) result(z)
    integer, intent(in) :: order
    real(dp), intent(in) :: x
    complex(dp) :: z

    z = bessel_jn(order, x) - i_*bessel_yn(order, x)
  end function hankel2FromDef

end program test_hankel2


      FUNCTION OPVF03(R,IR,A,IA,P,B,X,W,ALFA,ND,N,MIF,MDF)
C
C***********************************************************************
C                                                                      *
C                                                                      *
C      Copyright:       Eduardo Casas Renteria                         *
C                       Cecilia Pola Mendez                            *
C                                                                      *
C       Departamento de Matematicas,Estadistica y Computacion          *
C       -----------------------------------------------------          *
C                       UNIVERSIDAD DE CANTABRIA                       *
C                       ------------------------                       *
C                              AGOSTO  1987                            *
C                                                                      *
C***********************************************************************
C
C     OBJETIVO:
C        Este subprograma calcula el valor del funcional de  OPTR03  en
C     un punto.
C
C     LISTA DE LLAMADA:
C
C        R      Matriz de dimension  (IR,N)  que contiene la matriz del
C               termino cuadratico.
C
C        A      Matriz de dimension  (IA,MIF+MDF). Referencia en OPTR03
C
C        P      Vector de dimension  N que contiene los coeficientes del
C               termino lineal del funcional.
C
C        B      Vector de dimension  MIF+MDF.Referencia en  OPTR03.
C
C        X      Vector de dimension  N  que contiene  el punto donde se
C               calculara el valor del funcional.
C
C        W      Vector de trabajo de dimension  ND+2*N-1.
C
C        ALFA   (Referencia en  OPTR03).
C
C        ND     Variable de trabajo. (Referencia en  OPTR03).
C
C
C        Esta subprograma trabaja en doble precision via una sentencia
C     "implicit":
C                Implicit double precision (a-h,o-z)
C
C     SUBPROGRAMAS AUXILIARES:  ddot,dadd,dscal
C     FUNCIONES FORTRAN INTRINSECAS:  abs,mod
C
      implicit double precision(a-h,o-z)
      dimension r(ir,*),a(ia,*),p(*),b(*),x(*),w(*)
      ni=nd+n-1
      do 290 i=1,n
         i1=i+1
         ii=ni+i
         w(ii)=ddot(i,r(i,1),ir,x,1)
         if(i.lt.n) w(ii)=w(ii)+ddot(n-i,r(i1,i),1,x(i1),1)
290   continue
      s1=0.5d0
      call dscal(n,s1,w(ni+1),1)
      call dadd(n,p,1,w(ni+1),1)
      opvf03=ddot(n,x,1,w(ni+1),1)
      if(alfa.ne.1.d0) opvf03=opvf03*alfa
      do 300 i=1,mif
300   opvf03=opvf03+abs(ddot(n,a(1,i),1,x,1)-b(i))
      do 310 i=mif+1,mif+mdf
         s1=ddot(n,a(1,i),1,x,1)-b(i)
         if(s1.gt.0.d0) opvf03=opvf03+s1
310   continue
      end


! Erick Juarez
! CPSC 479 Sec 1
! HOMEWORK 6 - 4/20/20
! tested using GNU Fortran (GCC) 4.8.5
program fortran_matrix
implicit none

! Initialize 3 matrices with 4 rows & 4 columns
integer, dimension(4, 4) :: A, B, C
integer:: i, j

A = 4 ! set values of A to 4
do i = 1, 4 ! set values of B(i,j) = i + j + 1
    do j = 1, 4
        B(i, j) = i + j + 1
    end do
end do

C = A + B ! set values of C

! Change values of 8 to 16 in C
where(C == 8)
        C = 16
end where

! print results
do i = 1,4
    print '(a, i2, a, i2, 3i5)', 'A (row', i, '): ', A(i, :)
end do

do i = 1,4
    print '(a, i2, a, i2, 3i5)', 'B (row', i, '): ', B(i, :)
end do

do i = 1,4
    print '(a, i2, a, i2, 3i5)', 'C (row', i, '): ', C(i, :)
end do

end


      program main
c
c ... array declarations.
c
      implicit double precision (a-h, o-z)
      parameter (nxx = 20)
      parameter (nlen = nxx*nxx)
      parameter (nwk = 10*nlen)
      parameter (inwk = 3*nlen)
      dimension coef(nlen,3), rhs(nlen), u(nlen), wksp(nwk), ubar(1),
     a     rparm(16)
      integer jcoef(3), iwksp(inwk), iparm(25), p(1), ip(1)
      external cg, bic2
c
      ndim = nlen
      mdim = 3
      nw = nwk
      inw = inwk
      zeta = 1.0d-5
      nout = 6
c
c ... generate coef, jcoef, and rhs.
c
      nx = nxx
      ny = nx
      n = nx*ny
      h = 1.0d0/dble(nx + 1)
      maxnz = 3
      do i = 1,n
         coef(i,1) = 6.0d0
         coef(i,2) = -1.0d0
         coef(i,3) = -2.0d0
         rhs(i) = 0.0d0
      enddo
      k = 0
      do j = 1,ny
         y = dble(j)*h
         do i = 1,nx
            x = dble(i)*h
            k = k + 1
            if (j .eq. 1) then
               rhs(k) = rhs(k) + 2.0d0
            endif
            if (j .eq. ny) then
               rhs(k) = rhs(k) + 2.0d0*(1.0d0 + x)
               coef(k,3) = 0.0d0
            endif
            if (i .eq. 1) then
               rhs(k) = rhs(k) + 1.0d0
            endif
            if (i .eq. nx) then
               rhs(k) = rhs(k) + 1.0d0 + y
               coef(k,2) = 0.0d0
            endif
         enddo
      enddo
      jcoef(1) = 0
      jcoef(2) = 1
      jcoef(3) = nx
      call dfault (iparm,rparm)
c
c ... now, reset some default values.
c
      iparm(3) = 3
      iparm(4) = nout
      iparm(18) = 1
      iparm(19) = nx
      rparm(1) = zeta
c
c ... generate an initial guess for u and call nspcg.
c
      call vfill (n,u,0.0d0)
c
      call nspcg (bic2,cg,ndim,mdim,n,maxnz,coef,jcoef,p,ip,
     a            u,ubar,rhs,wksp,iwksp,nw,inw,iparm,rparm,ier)
      write (nout,35) nw, inw
 35   format (/1x,'nw  = ',i8/
     a         1x,'inw = ',i8)
      stop
      end


SUBROUTINE UpdateGhostLayerXCurvilinear(var,S,NNx,NNy,CompGrid,alpha,beta,GhostGridX,GhostGridY)
! By Allan P. Engsig-Karup.
USE Precision
USE DataTypes
IMPLICIT NONE
TYPE (Level_def) :: CompGrid
INTEGER :: NNx, NNy, rank, alpha, beta, i,j, GhostGridX, GhostGridY, diffb
REAL(KIND=long), DIMENSION(NNx,NNy) :: var, S
REAL(KIND=long), DIMENSION(2*alpha+1) :: Stencile, Stenciln
REAL(KIND=long) :: ex, ey, nx, ny, tmp, diage
INTEGER, DIMENSION(2*beta+1) :: idx
IF (alpha/=beta) THEN
	print*,'ERROR: alpha/=beta. (UpdateGhostLayerXCurvilinear)'
	STOP
END IF
rank = 2*alpha+1
DO i = -beta, beta
	idx(i+beta+1) = i
END DO
! two boundaries
i=1
DO j = 1+GhostGridY, NNy-GhostGridY
	if (j-1<beta+1) then
		diffb = (beta+1)-(j-1)  ! substract one to ensure that it is only dependent on one ghost point.
	else if (j+1>NNy-beta) then
		diffb = (NNy-beta)-(j+1)  ! substract one to ensure that it is only dependent on one ghost point.
	else
		diffb = 0
	end if

	! use geometric factors for wall boundary
	ex = CompGrid%CurvilinearStuff%ex(i+GhostGridX,j)
!	ey = CompGrid%CurvilinearStuff%ey(i+GhostGridX,j)
	nx = CompGrid%CurvilinearStuff%nx(i+GhostGridX,j)
!	ny = CompGrid%CurvilinearStuff%ny(i+GhostGridX,j)

	Stencile = CompGrid%CurvilinearStuff%DiffStencils%StencilG(1:rank,1+GhostGridX,1)
	Stenciln = CompGrid%CurvilinearStuff%DiffStencils%StencilG(1:rank,1+beta-diffb,1)

	diage = ex*Stencile(1)
	tmp = ex * DOT_PRODUCT(Stencile(2:rank),var(2:rank,j) ) + nx * DOT_PRODUCT( Stenciln,var(i+GhostGridX,j+idx+diffb) )

    var(i,j) = (S(i+GhostGridX,j)- tmp) / diage

END DO
i = NNx
DO j = 1+GhostGridY, NNy-GhostGridY
	if (j-1<beta+1) then
		diffb = (beta+1)-(j-1)
	else if (j+1>NNy-beta) then
		diffb = (NNy-beta)-(j+1)
	else
		diffb = 0
	end if

	! use geometric factors for wall boundary
	ex = CompGrid%CurvilinearStuff%ex(i-GhostGridX,j)
!	ey = CompGrid%CurvilinearStuff%ey(i-GhostGridX,j)
	nx = CompGrid%CurvilinearStuff%nx(i-GhostGridX,j)
!	ny = CompGrid%CurvilinearStuff%ny(i-GhostGridX,j)

	Stencile = CompGrid%CurvilinearStuff%DiffStencils%StencilG(1:rank,rank-GhostGridX,1)
	Stenciln = CompGrid%CurvilinearStuff%DiffStencils%StencilG(1:rank,1+beta-diffb,1)

	diage = ex*Stencile(rank)

	tmp = ex * DOT_PRODUCT(Stencile(1:rank-1),var(i-rank+1:i-1,j) ) + nx * DOT_PRODUCT( Stenciln,var(i-GhostGridX,j+idx+diffb) )

    var(i,j) = (S(i-GhostGridX,j)- tmp) / diage
END DO
END SUBROUTINE UpdateGhostLayerXCurvilinear


! =============================================================================
!                       Test parcel initialisation in 3D
!
!         This unit test checks the parcel initialisation from fields.
! =============================================================================
program test_parcel_init_3d
    use unit_test
    use constants, only : pi, zero, one, two, four, f12, f13, f23, f32
    use parcel_container
    use parcel_init, only : gen_parcel_scalar_attr, unit_test_parcel_init_alloc, init_timer
    use parcel_interpl, only : par2grid, par2grid_timer
    use parcel_ellipsoid, only : get_abc
    use fields, only : tbuoyg, field_default
    use parameters, only : update_parameters, dx, ncell, nx, ny, nz, lower, vcell
    use timer
    implicit none

    double precision  :: xg, yg, zg, facx, facy, facz, argx, argy, argz, v0
    integer :: i, ix, iy, iz, mx, my, mz
    double precision :: rms, rmserr, error
    double precision, allocatable :: workg(:, :, :)
    double precision :: tol = 1.0d-9

     !Number of parcels per grid box = nbgx*nbgz
    integer, parameter :: nbgx = 2, nbgy = nbgx, nbgz = nbgx

    !Fractions of grid cell used for placing parcels:
    double precision, parameter :: dxf = one / dble(nbgx), &
                                   dyf = one / dble(nbgy), &
                                   dzf = one / dble(nbgz)

    nx = 64
    ny = 64
    nz = 32
    lower = (/-four, -four, -two/)
    extent = (/8.0d0, 8.0d0, four/)

    call register_timer('parcel init', init_timer)
    call register_timer('par2grid', par2grid_timer)

    call update_parameters

    call field_default

    !Maximum number of parcels:

    !Total number of parcels:
    n_parcels = nbgx * nbgy * nbgz * ncell
    call parcel_alloc(n_parcels)

    !--------------------------------------------------------
    ! Define a gridded field "tbuoyg" (this can be arbitrary):
    facx = two * pi / extent(1)
    facy = two * pi / extent(2)
    facz = one / extent(3)
    do ix = 0, nx-1
        xg = dx(1) * dble(ix)
        argx = facx * xg
        do iy = 0, ny-1
            yg = dx(2) * dble(iy)
            argy = facy * yg
            do iz = 0, nz
                zg = dx(3) * dble(iz)
                argz = facz * zg
                tbuoyg(iz, iy, ix) = dexp(argz) * dsin(argx - argy + one) / &
                                     ((one + f12 * dcos(argx)) * (one + f13 * dcos(argy)))
            enddo
        enddo
    enddo

    !---------------------------------------------------------
    !Initialise parcel volume positions and volume fractions:
    v0 = dxf * dyf * dzf * vcell
    i = 0
    do iz = 0, nz-1
        do iy = 0, ny-1
            do ix = 0, nx-1
                do mz = 1, nbgz
                    do my = 1, nbgy
                        do mx = 1, nbgx
                            i = i + 1
                            parcels%position(1, i) = lower(1) + dx(1) * (dble(ix) + dxf * (dble(mx) - f12))
                            parcels%position(2, i) = lower(2) + dx(2) * (dble(iy) + dyf * (dble(my) - f12))
                            parcels%position(3, i) = lower(3) + dx(3) * (dble(iz) + dzf * (dble(mz) - f12))
                            parcels%volume(i) = v0
                            parcels%B(:, i) = zero
                            parcels%B(1, i) = get_abc(v0) ** f23
                            parcels%B(4, i) = parcels%B(1, i)
                        enddo
                    enddo
                enddo
            enddo
        enddo
    enddo

    ! Prepare for "gen_parcel_scalar_attr"
    call unit_test_parcel_init_alloc

    !---------------------------
    ! Generate parcel attribute:
    call gen_parcel_scalar_attr(tbuoyg, tol, parcels%buoyancy)

    !
    ! check result
    !
    error = zero

    ! Copy buoyg since it is overwritten in par2grid after:
    allocate(workg(-1:nz+1, 0:ny-1, 0:nx-1))
    workg = tbuoyg

    call par2grid

    ! Compute max and rms errors:

    ! Rms value of original field
    rms = dsqrt((f12 * sum(workg(0, :, :) ** 2 + workg(nz, :, :) ** 2) + &
                       sum(workg(1:nz-1, :, :) ** 2)) / dble(ncell))

    workg = tbuoyg - workg

    ! Rms error in reconstruction
    rmserr = dsqrt((f12 * sum(workg(0, :, :) ** 2 + workg(nz, :, :) ** 2) + &
                          sum(workg(1:nz-1, :, :) ** 2)) / dble(ncell))

    error = max(error, rmserr)

    ! Relative rms error
    error = max(error, rmserr / rms)

    ! Maximum error
    error = max(error, maxval(dabs(workg(0:nz, :, :))))

    call print_result_dp('Test parcel initialisation 3D', error, atol=two * tol)

    call parcel_dealloc

    deallocate(workg)

end program test_parcel_init_3d