module posterization
  implicit none

contains
  pure function make_histogram(layer, maximum) result(hist)
    implicit none
    integer, intent(in) :: layer(:, :)
    integer, intent(in) :: maximum
    integer :: hist(0:maximum), i
    hist = 0
    do concurrent(i=0:maximum)
      block
        hist(i) = count(layer == i)
      end block
    end do
  end function make_histogram

  pure function to_binary(img, threshold) result(rst)
    implicit none
    integer, intent(in) :: img(:, :, :)
    integer, intent(in) :: threshold
    integer, allocatable :: rst(:, :, :)

    rst = img
    where (img(1, :, :) >= threshold)
      rst(1, :, :) = 1
    else where
      rst(1, :, :) = 0
    end where
  end function to_binary

  pure function otsu(img, maximum) result(rst)
    implicit none
    integer, intent(in) :: img(:, :, :)
    integer, intent(in) :: maximum
    integer, allocatable :: rst(:, :, :), hist(:)
    integer :: i, th, best_th, rescale_rate
    real :: ave_class1, ave_class2, nums(0:maximum), var, best_var, &
            n_pix_class1, n_pix_class2

    hist = make_histogram(img(1, :, :), maximum)
    rescale_rate = size(img)**0.5
    ! numsとthresold_to_varを配列の宣言時に代入するのはエラーになる
    nums = [(real(i), i=0, maximum)]
    best_th = 0
    best_var = -1.0

    do concurrent(th=0:maximum)
      block
        n_pix_class1 = sum(hist(:th))/rescale_rate
        n_pix_class2 = sum(hist(th + 1:))/rescale_rate

        if (n_pix_class1 == 0) then
          ave_class1 = 0
        else
          ave_class1 = sum(hist(:th)*nums(:th))/n_pix_class1/rescale_rate
        end if

        if (n_pix_class2 == 0) then
          ave_class2 = 0
        else
          ave_class2 = sum(hist(th + 1:)*nums(th + 1:))/n_pix_class2/rescale_rate
        end if

        var = n_pix_class1*n_pix_class2*((ave_class1 - ave_class2)**2)

        if (var > best_var) then
          best_th = th
          best_var = var
        end if
      end block
    end do

    rst = to_binary(img, best_th)
  end function otsu

  pure function adaptive_threshold(img, maximum, kernel_size) result(rst)
    implicit none
    integer, intent(in) :: img(:, :, :)
    integer, intent(in) :: maximum
    integer, intent(in), optional :: kernel_size
    integer, allocatable :: rst(:, :, :), thresholds(:, :), row(:), column(:)
    integer :: height, width, h, w, img_shape(3), s, dh, dw, i, k_size

    if (present(kernel_size)) then
      k_size = kernel_size
    else
      k_size = 15
    end if

    img_shape = shape(img)
    height = img_shape(2)
    width = img_shape(3)
    allocate (thresholds(height, width))
    allocate (rst(1, height, width))

    do concurrent(w=1:width, h=1:height)
      block
        row = [(i, i=max(1, w - k_size/2), min(w + k_size/2, width))]
        column = [(i, i=max(1, h - k_size/2), min(h + k_size/2, height))]
        s = 0
        do dw = 1, size(row)
          do dh = 1, size(column)
            s = s + img(1, column(dh), row(dw))
          end do
        end do
        thresholds(h, w) = nint(s/real(size(row)*size(column)))
        deallocate (row)
        deallocate (column)
      end block
    end do

    where (img(1, :, :) <= thresholds(:, :))
      rst(1, :, :) = 0
    else where
      rst(1, :, :) = 1
    end where
  end function adaptive_threshold

  pure function quantize(img, n_split, maximum) result(rst)
    implicit none
    integer, intent(in) :: img(:, :, :)
    integer, intent(in) :: n_split
    integer, intent(in), optional :: maximum
    integer, allocatable :: rst(:, :, :)
    integer :: m

    if (present(maximum)) then
      m = maximum
    else
      m = 255
    end if

    rst = img/(m/n_split + 1)
  end function quantize

  pure function dither(img, maximum) result(rst)
    implicit none
    integer, intent(in) :: img(:, :, :)
    integer, intent(in) :: maximum
    integer, allocatable :: rst(:, :, :)
    integer :: dither_mtx(4, 4)
    integer :: height, width, h, w, h_range(2), w_range(2), dh, dw, img_shape(3)
    integer, allocatable :: targets(:, :)

    dither_mtx = reshape([0, 12, 3, 15, &
                          8, 4, 11, 7, &
                          2, 14, 1, 13, &
                          10, 6, 9, 5]*((maximum + 1)/16), shape(dither_mtx))
    img_shape = shape(img)
    height = img_shape(2)
    width = img_shape(3)
    allocate (rst(1, height, width))

    do w = 0, ceiling(width/4.0)
      do h = 0, ceiling(height/4.0)
        h_range = [h*4 + 1, min(height, (h + 1)*4)]
        w_range = [w*4 + 1, min(width, (w + 1)*4)]

        targets = img(1, h_range(1):h_range(2), w_range(1):w_range(2))
        do concurrent(dh=1:h_range(2) - h_range(1) + 1, dw=1:w_range(2) - w_range(1) + 1)
          block
            if (dither_mtx(dh, dw) > targets(dh, dw)) then
              targets(dh, dw) = 0
            else
              targets(dh, dw) = 1
            end if
          end block
        end do
        rst(1, h_range(1):h_range(2), w_range(1):w_range(2)) = targets
      end do
    end do

  end function dither

  pure function error_diffusion(img, maximum) result(rst)
    implicit none
    integer, intent(in) :: img(:, :, :)
    integer, intent(in) :: maximum
    integer, allocatable :: rst(:, :, :)
    integer :: height, width, h, w, img_shape(3), quant_error, replace_coords(4, 2), dh, dw, old, new, i
    real :: dither_rates(4)

    dither_rates = [7, 3, 5, 1]/16.0

    img_shape = shape(img)
    rst = img(1:1, :, :)

    do w = 1, img_shape(3)
      do h = 1, img_shape(2)
        old = rst(1, h, w)
        if (rst(1, h, w) >= (nint(maximum/2.0))) then
          rst(1, h, w) = 1
        else
          rst(1, h, w) = 0
        end if
        new = rst(1, h, w)*maximum
        quant_error = old - new
        ! TODO
        ! 列優先でループが回るので拡散を縦方向にしているのを修正する
        replace_coords = reshape([[h + 1, h - 1, h, h + 1], [w, w + 1, w + 1, w + 1]], shape(replace_coords))
        do concurrent(i=1:4)
          block
            dh = replace_coords(i, 1)
            dw = replace_coords(i, 2)
            if (dh > 0 .and. dh <= img_shape(2) .and. dw > 0 .and. dw <= img_shape(3)) then
              rst(1, dh, dw) = rst(1, dh, dw) + nint(dither_rates(i)*quant_error)
            end if
          end block
        end do
      end do
    end do
  end function error_diffusion

  pure function rgb_to_gray(img) result(rst)
    implicit none
    integer, intent(in) :: img(:, :, :)
    integer, allocatable :: rst(:, :, :)
    integer :: h, w, img_shape(3)
    img_shape = shape(img)
    allocate (rst(1, img_shape(2), img_shape(3)))

    do concurrent(h=1:img_shape(2), w=1:img_shape(3))
      rst(1, h, w) = 2989*img(1, h, w) + 5870*img(2, h, w) + 1140*img(3, h, w)
    end do
    rst = nint(rst/10000.0)
  end function rgb_to_gray
end module posterization


program nbody_parallel
implicit none
    integer(kind = 8) :: nsteps
    integer(kind = 8) :: nparticles

    real(kind = 8), dimension(:), allocatable :: m
    real(kind = 8), dimension(:), allocatable :: x, y, z
    real(kind = 8), dimension(:), allocatable :: x_tmp, y_tmp, z_tmp
    real(kind = 8), dimension(:), allocatable :: vx, vy, vz

    real(kind = 8) :: G
    real(kind = 8) :: dt
    real(kind = 8) :: dx, dy, dz
    real(kind = 8) :: r, a
    real(kind = 8) :: softening

    integer(kind = 8) :: n, i, j

    character(len = 1024) :: arg
    character(len = 1024) :: param_file

    integer :: ios

    do i = 1, 5
        call get_command_argument(int(i), arg)
        if (len_trim(arg) == 0) then
            print *, "Please specify arguments correctly." // achar(10)
            print *, "Usage: nbody <nsteps> <dt> <G> <softening> " &
                // "<PARAMETERS_FILE>" // achar(10)
            print *, "PARAMETERS_FILE format:"
            print *, " nparticles (on the first line)"
            print *, " m x y z vx vy vz (on each line after the first line)"
            stop
        end if

        select case (i)
        case (1)
            read(arg, *) nsteps
        case (2)
            read(arg, *) dt
        case (3)
            read(arg, *) G
        case (4)
            read(arg, *) softening
        case (5)
            read(arg, "(a)") param_file
        end select
    end do

    open(1, file = param_file, iostat = ios, status = "old")
    if (ios /= 0) then
        print *, "Can't open the PARAMETERS_FILE:", param_file
        stop
    end if

    read(1, *) nparticles
    allocate(m(0:nparticles - 1))
    allocate(x(0:nparticles - 1))
    allocate(y(0:nparticles - 1))
    allocate(z(0:nparticles - 1))
    allocate(x_tmp(0:nparticles - 1))
    allocate(y_tmp(0:nparticles - 1))
    allocate(z_tmp(0:nparticles - 1))
    allocate(vx(0:nparticles - 1))
    allocate(vy(0:nparticles - 1))
    allocate(vz(0:nparticles - 1))

    print *, "Initial parameters:"
    do i = 0, nparticles - 1
        read(1, *) m(i), x(i), y(i), z(i), vx(i), vy(i), vz(i)
        print *, i, ": m =", m(i), "x =", x(i), "y =", y(i), "z =", z(i), &
            "vx =", vx(i), "vy =", vy(i), "vz =", vz(i)
    end do

    close(1)

    print *, "n =", 0_8
    do i = 0, nparticles - 1
        print *, i, ":", x(i), y(i), z(i)
    end do

    !$omp parallel private(n, j, dx, dy, dz, r, a)
    do n = 1, nsteps
        !$omp do
        do i = 0, nparticles - 1
            x_tmp(i) = x(i) + vx(i) * dt
            y_tmp(i) = y(i) + vy(i) * dt
            z_tmp(i) = z(i) + vz(i) * dt

            do j = 0, nparticles - 1
                dx = x(j) - x(i)
                dy = y(j) - y(i)
                dz = z(j) - z(i)

                r = sqrt(dx ** 2 + dy ** 2 + dz ** 2 + softening)
                a = G * m(j) / r ** 3 * dt

                vx(i) = vx(i) + a * dx
                vy(i) = vy(i) + a * dy
                vz(i) = vz(i) + a * dz
            end do
        end do

        !$omp do
        do i = 0, nparticles - 1
            x(i) = x_tmp(i)
            y(i) = y_tmp(i)
            z(i) = z_tmp(i)
        end do

        !$omp master
        print *, "n =", n
        do i = 0, nparticles - 1
            print *, i, ":", x(i), y(i), z(i)
        end do
        !$omp end master
    end do
    !$omp end parallel

    deallocate(m)
    deallocate(x)
    deallocate(y)
    deallocate(z)
    deallocate(x_tmp)
    deallocate(y_tmp)
    deallocate(z_tmp)
    deallocate(vx)
    deallocate(vy)
    deallocate(vz)
end program


*
* average3 - Computes the average of three numbers
*
*  Written by: C. Severance 16Mar92
*
* Declare the variables:
*
      REAL A,B,C,TOT,AVE
*
* Prompt the user for the three variables
*
      PRINT *,'Enter the first number -'
      READ *,A
      PRINT *,'Enter the second number -'
      READ *,B
      PRINT *,'Enter the third number -'
      READ *,C
*
* Print the numbers out
*
      PRINT *,'The numbers are -',A,B,C
*
* Calculate the total and average
*
      TOT = A + B + C
      AVE = TOT / 3.0
*
* Print the values out and end the program
*
      PRINT *,'Total - ',TOT
      PRINT *,'Average - ',AVE
      END


!-----------------------------------------------------------------------
! Saturation specific humidity
!-----------------------------------------------------------------------
subroutine QSAT(water,P,T,Qs)

use, intrinsic :: iso_fortran_env, only: dp=>real64

use Constants, only : &
  eps,               &! Ratio of molecular weights of water and dry air
  e0,                &! Saturation vapour pressure at Tm (Pa)
  Tm                  ! Melting point (K)

implicit none

logical, intent(in) :: &
  water               ! Saturation wrt water if TRUE

real*8, intent(in) :: &
  P,                 &! Air pressure (Pa)
  T                   ! Temperature (K)

real*8, intent(out) :: &
  Qs                  ! Saturation specific humidity

real*8 :: &
  Tc,                &! Temperature (C)
  es                  ! Saturation vapour pressure (Pa)

Tc = T - Tm
if (Tc > 0 .or. water) then
  es = e0*exp(17.5043_dp*Tc / (241.3_dp + Tc))
else
  es = e0*exp(22.4422_dp*Tc / (272.186_dp + Tc))
end if
Qs = eps*es / P

end subroutine QSAT


program threebody
	real(kind=4), parameter :: PI = 4*atan(1.), G = 4.498279e-8
	integer,parameter :: npar = 1000,dim = 3
	real(kind=4),dimension(2) :: M
	real(kind=4), dimension(npar,dim) :: x,v
	integer :: nt, dsnap, np1,np2
	real :: t,dt
	! Chunk to be divided between threads
	integer,parameter :: chunk = 100

!	Input Data
	call input_data(x,v,np1,np2,M)
	v(3:np1+2,2)	=	v(3:np1+2,2) + v(1,2)
	v(np1+3:np1+np2+2,2) =	v(np1+3:np1+np2+2,2) + v(2,2)

!   Simulation Parameters
    nt = 5000
    dt = 0.1
    t = 0
	dsnap = 20

!   Time-Marching Loop
	do n = 1,nt
		if (modulo(n-1,dsnap)==0) then
			call output(x,v,t)
		endif

		!$OMP PARALLEL SHARED(x,v) PRIVATE(i,a,r)
  		!$OMP DO SCHEDULE(STATIC,CHUNK)
		do i=1,2+np1+np2
			call leapfrog(x,v,dt)
		enddo
		!$OMP ENDDO
		!$OMP END PARALLEL

		t = t+dt
	end do

	call output(x,v,t) ! Output last values

contains
!***********************************************************************
subroutine input_data(x,v,np1,np2,M)
! Initialize Position and Velocity values
	real(kind=4), dimension(npar,dim), intent(out) :: x,v
	real(kind=4), dimension(2),intent(out) :: M
	integer, intent(out) :: np1,np2
	integer :: k

	open(312,FILE='body.dat',STATUS="OLD",FORM="FORMATTED",ACTION="READ")
	read(312,*) x(1,:),v(1,:),M(1)
	read(312,*) x(2,:),v(2,:),M(2)
	close(312)

	open(312,FILE='A.dat',STATUS="OLD",FORM="FORMATTED",ACTION="READ")
	read(312,*) np1
	do k=1,np1
		read(312,*) x(k+2,:),v(k+2,:)
	enddo
	close(312)

	open(312,FILE='B.dat',STATUS="OLD",FORM="FORMATTED",ACTION="READ")
		read(312,*) np2
	do k = 1,np2
		read(312,*) x(k+2+np1,:), v(k+2+np1,:)
	enddo
	close(312)

endsubroutine input_data
!***********************************************************************
subroutine leapfrog (x,v,dt)
! Leapfrog using Position-Verlet algorithm
    real(kind=4), dimension(npar,dim),intent(inout) :: x,v
    real(kind=4), intent(in) :: dt
    real(kind=4),dimension(dim) :: a

	x(i,:) = x(i,:) + 0.5 * dt * v(i,:)
	call accel(x,a)
	v(i,:) = v(i,:) + dt * G * a
	x(i,:) = x(i,:) + 0.5 * dt * v(i,:)

endsubroutine leapfrog
!***********************************************************************
subroutine accel(x,a)
! Calculate Gravitation Acceleration based on position
    real(kind=4), dimension(npar,dim),intent(in) :: x
    real(kind=4), dimension(dim),intent(out) :: a
	real(kind=4) :: r
	integer :: j

	! Acceleration
	a = (/0.,0.,0./)
	do j = 1,2
		if (i/=j) then
			r =	 norm2(x(i,:)-x(j,:))
			a = a - M(j)*(x(i,:)-x(j,:))/(r**3)
		endif
	enddo

endsubroutine accel
!***********************************************************************
subroutine output(x,v,t)
    real(kind=4),dimension(npar,dim),intent(in) :: x,v
    real(kind=4),intent(in) :: t
    integer :: k, f_handle = 314
    character(len=12) :: f_name, num_sfx

    write(num_sfx,"(I5)") n
	f_name = 'mc_'//trim(adjustl(num_sfx))//'.dat'
    open(f_handle,FILE=trim(f_name),FORM="formatted")
    do k = 1,np1+np2+2
		write(f_handle,FMT="(6E20.6)") x(k,:),v(k,:)
    enddo
	close(f_handle)

endsubroutine output
!***********************************************************************
endprogram threebody


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

program ex_7_16g
    implicit none

    integer                 :: x, y, Out = 0, In = 0, i = 0, maxCol = 0, min = 0
    integer, allocatable    :: B(:,:)
    character(*), parameter :: output_file = "output.txt", &
                               input_file = "../data/input.txt"

    open (file=input_file, newunit=In)
        read(In, *) x, y
        allocate(B(x,y))
        read(In, *) (B(i,:), i = 1, y)
    close (In)

    maxCol = maxloc(sum(B, 1), 1)
    min = minval(B(:,maxCol))

    open (file=output_file, newunit=Out)
        write(Out, *) min
    close (Out)
end program ex_7_16g


!Program for moving mesh in 1D FDM
!Romit Maulik
!Refer introductory chapter of Adaptive Moving Mesh Methods - Springer
!Burgers equation with Dirichlet BCs

program mm1d
implicit none

integer :: nx,i,t,nt,isol,iform
real*8,dimension(:),allocatable :: u,x,dxdt,utemp
real*8 :: dt,xl,dx,pi,x0

common/nlform/iform

pi = 4.0d0*datan(1.0d0)
dt = 1d-3
nt = nint(1d3)
nx = 41
xl = 2.0d0*pi
dx = xl/dfloat(nx)
isol = 2			!Euler FWD or RK3
iform = 1			![1] - Central or [2] - QUICK for Nonlinear term

allocate(u(1:nx))  !Not periodic BC
allocate(x(1:nx))

!Setting up initial condition
x0 = -pi
do i = 1,nx
  x(i) = x0+dx
  u(i) = -dsin(x(i))
  x0 = x0+dx
end do

open(8,file='Initial_Plot.plt')
write(8,*) 'variables ="x","f(x)"'
write(8,*)'zone f=point i=',nx,''
	do i=1,nx
	write(8,*) x(i),u(i)
	end do
close(8)

!Calculating mesh speed - dxdt
allocate(dxdt(1:nx))
allocate(utemp(1:nx))

if (isol==1) then !forward Euler

open(8,file='MM_Plot.plt')
do t = 1,nt

call meshspeed(u,x,dxdt,nx)

call vel_update(u,x,dxdt,nx,utemp)

!New mesh distribution
do i = 1,nx
  x(i) = x(i)+dxdt(i)*dt
end do

!New velocity distribution
do i = 1,nx
  u(i) = u(i)+utemp(i)*dt
end do

!if (mod(t,nt/10)==0) then

write(8,*)'variables ="xnew","f(x)"'
write(8,*)'zone f=point i=',nx,',t="case',t,'"'
	do i=1,nx
	write(8,*) x(i),u(i)
	end do

!end if

end do

close(8)

else if (isol==2) then!Rk3

open(8,file='MM_Plot.plt')
call rk3(u,x,dxdt,nx,nt,dt)
close(8)

end if

end

!--------------------------------------------------------------------------------------------
!--------------------------------------------------------------------------------------------
!subroutine for rk3 time integration
!--------------------------------------------------------------------------------------------
!--------------------------------------------------------------------------------------------
subroutine rk3(u,x,dxdt,nx,nt,dt)
implicit none

integer :: nx,t,nt,i
real*8 :: dt
real*8,dimension(1:nx) :: u,x,dxdt
real*8,dimension(1:nx) :: u1,u2,utemp,x1,x2

do i = 1,nx
u1(i) = u(i)
u2(i) = u(i)
utemp(i) = u(i)
x1(i) = x(i)
x2(i) = x(i)
end do

do t = 1,nt

	!Stage 1
	call meshspeed(u,x,dxdt,nx)

    call vel_update(u,x,dxdt,nx,utemp)

	do i = 1,nx
    u1(i) = u(i) + dt*utemp(i)
	end do

	!New stage mesh distribution
    do i = 1,nx
  	x1(i) = x(i)+dxdt(i)*dt
    end do

	!Stage 2
	call meshspeed(u1,x1,dxdt,nx)

    call vel_update(u1,x1,dxdt,nx,utemp)

	do i = 1,nx
    u2(i) = 0.75d0*u(i) + 0.25d0*u1(i) + 0.25*utemp(i)*dt

	x2(i) = 0.75d0*x(i) + 0.25d0*x1(i) + 0.25*dt*dxdt(i)
    end do

    !stage 3

	call meshspeed(u2,x2,dxdt,nx)

    call vel_update(u2,x2,dxdt,nx,utemp)

	do i = 1,nx
    u(i) = 1.0d0/3.0d0*u(i) + 2.0d0/3.0d0*u2(i) + 2.0d0/3.0d0*utemp(i)*dt

    x(i) = 1.0d0/3.0d0*x(i) + 2.0d0/3.0d0*x2(i) + 2.0d0/3.0d0*dxdt(i)*dt
    end do

!if (mod(t,nt/10)==0) then

write(8,*)'variables ="xnew","f(x)"'
write(8,*)'zone f=point i=',nx,',t="case',t,'"'
	do i=1,nx
	write(8,*) x(i),u(i)
	end do

!end if

end do

return
end

!--------------------------------------------------------------------------------------------
!--------------------------------------------------------------------------------------------
!subroutine to calculate meshspeed call meshspeed(u,x,dxdt,nx)
!--------------------------------------------------------------------------------------------
!--------------------------------------------------------------------------------------------
subroutine meshspeed(u,x,dxdt,nx)
implicit none

integer :: i,j,nx
real*8,dimension(1:nx) :: dxdt,u,x
real*8,dimension(:),allocatable :: uxx,rho
real*8 :: alphah,sum,tau,dxi

allocate(uxx(1:nx))

uxx(1) 	= 	2.0d0*((x(2)-x(1))*(u(3)-u(1))-(x(3)-x(1))*(u(2)-u(1)))/((x(3)-x(1))*(x(2)-x(1))*(x(3)-x(2)))
uxx(nx) = 	2.0d0*((x(nx-1)-x(nx))*(u(nx-2)-u(nx))-(x(nx-2)-x(nx))*(u(nx-1)-u(nx)))&
			&/((x(nx-2)-x(nx))*(x(nx-1)-x(nx))*(x(nx-2)-x(nx-1)))

do i = 2,nx-1
  uxx(i) = 2.0d0/(x(i+1)-x(i-1))*((u(i+1)-u(i))/(x(i+1)-x(i))-(u(i)-u(i-1))/(x(i)-x(i-1)))
end do

sum = 0.0d0

do j = 2,nx
    sum = sum + (0.5d0*(x(j)-x(j-1))*(dabs(uxx(j))**(2.0d0/3.0d0)+dabs(uxx(j-1))**(2.0d0/3.0d0)))**3
end do

alphah = max(1,sum)

allocate(rho(1:nx))

do i = 1,nx
  rho(i) = (1+1/alphah*dabs(uxx(i))**2)**(1.0d0/3.0d0)
end do

!Smoothing rho

do i = 2,nx-1
  rho(i) = 0.25d0*rho(i-1)+0.5d0*rho(i)+0.25d0*rho(i+1)
end do

rho(1) = 0.5d0*rho(1)+0.5d0*rho(2)
rho(nx) = 0.5d0*rho(nx-1)+0.5d0*rho(nx)

dxdt(1) = 0.0d0
dxdt(nx) = 0.0d0

tau = 0.01d0
dxi = 1.0d0/dfloat(nx-1)

do i = 2,nx-1
  dxdt(i) = 1.0d0/(rho(i)*tau*dxi**2)*((rho(i+1)+rho(i))/2.0d0*(x(i+1)-x(i))-(rho(i)+rho(i-1))/2.0d0*(x(i)-x(i-1)))
end do

deallocate(rho)

return
end

!--------------------------------------------------------------------------------------------
!--------------------------------------------------------------------------------------------
!subroutine to calculate new velocty
!--------------------------------------------------------------------------------------------
!--------------------------------------------------------------------------------------------
subroutine vel_update(u,x,dxdt,nx,v)
implicit none

integer :: nx,i,iform
real*8,dimension(1:nx) :: u,x,dxdt,v
real*8 :: eps,ta,tb,tc,td
real*8,dimension(:),allocatable :: rhs,nl

common/nlform/iform

!eps = 0.0001d0
eps = 0.0d0

allocate(rhs(1:nx))
allocate(nl(1:nx))

if (iform==1) then

do i = 2,nx-1
  ta = (u(i+1)-u(i))/(x(i+1)-x(i))
  tb = (u(i)-u(i-1))/(x(i)-x(i-1))
  tc = 0.5d0*(u(i+1)**2-u(i-1)**2)/(x(i+1)-x(i-1))
  rhs(i) = 2*eps/(x(i+1)-x(i-1))*(ta-tb)-tc
end do

do i = 2,nx-1
  nl(i) = -(u(i+1)-u(i-1))/(x(i+1)-x(i-1))*dxdt(i)
end do

do i = 2,nx-1
  v(i) = rhs(i)- nl(i)
end do

v(1) = 0.0d0
v(nx) = 0.0d0

else if (iform==2) then

do i = 2,nx-1
	if (u(i)>0.0d0) then
		ta = (u(i+1)-u(i))/(x(i+1)-x(i))
		tb = (u(i)-u(i-1))/(x(i)-x(i-1))
        td = (0.75d0*u(i-1)+3.0d0/8.0d0*u(i)-1.0/8.0d0*u(i-2))**2
        td = td - (0.75d0*u(i)+3.0d0/8.0d0*u(i+1)-1.0/8.0d0*u(i-1))**2!Ref Wikipedia East face - West Face
		tc = 0.5d0*(2.0d0*td)/(x(i+1)-x(i-1))			!Multiply 2 due to cell face approx
		rhs(i) = 2*eps/(x(i+1)-x(i-1))*(ta-tb)+tc
	else
		ta = (u(i+1)-u(i))/(x(i+1)-x(i))
		tb = (u(i)-u(i-1))/(x(i)-x(i-1))
        td = (0.75d0*u(i)+3.0d0/8.0d0*u(i-1)-1.0/8.0d0*u(i+1))**2
        td = td - (0.75d0*u(i+1)+3.0d0/8.0d0*u(i)-1.0/8.0d0*u(i+2))**2
		tc = 0.5d0*(2.0d0*td)/(x(i+1)-x(i-1))			!Multiply 2 due to cell face approx
		rhs(i) = 2*eps/(x(i+1)-x(i-1))*(ta-tb)+tc
    end if
end do

do i = 2,nx-1
  nl(i) = -(u(i+1)-u(i-1))/(x(i+1)-x(i-1))*dxdt(i)
end do

do i = 2,nx-1
  v(i) = rhs(i)- nl(i)
end do

v(1) = 0.0d0
v(nx) = 0.0d0

end if

deallocate(rhs,nl)

return
end


C$Procedure      DVCRSS ( Derivative of Vector cross product )

      SUBROUTINE DVCRSS ( S1, S2, SOUT )

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

      DOUBLE PRECISION    S1   ( 6 )
      DOUBLE PRECISION    S2   ( 6 )
      DOUBLE PRECISION    SOUT ( 6 )

C$ Brief_I/O
C
C     VARIABLE  I/O  DESCRIPTION
C     --------  ---  --------------------------------------------------
C     S1        I   Left hand state for cross product and derivative.
C     S2        I   Right hand state for cross product and derivative.
C     SOUT      O   State associated with cross product of positions.
C
C$ Detailed_Input
C
C     S1       This may be any state vector.  Typically, this
C              might represent the apparent state of a planet or the
C              Sun, which defines the orientation of axes of
C              some coordinate system.
C
C     S2       A state vector.
C
C$ Detailed_Output
C
C     SOUT     This variable represents the state associated with the
C              cross product of the position components of S1 and S2.
C              In other words, if S1 = (P1,V1) and S2 = (P2,V2) then
C              SOUT is ( P1xP2, d/dt{ P1xP2 } ).
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     Error free.
C
C     1) If S1 and S2 are large in magnitude (taken together,
C        their magnitude surpasses the limit allowed by the
C        computer) then it may be possible to generate a
C        floating point overflow from an intermediate
C        computation even though the actual cross product and
C        derivative may be well within the range of double
C        precision numbers.
C
C        DVCRSS does NOT check the magnitude of S1 or S2 to
C        insure that overflow will not occur.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     DVCRSS calculates the three-dimensional cross product of two
C     vectors and the derivative of that cross product according to
C     the definition.
C
C$ Examples
C
C            S1                    S2                   SOUT
C     -----------------------------------------------------------------
C     (0, 1, 0, 1, 0, 0)  ( 1,  0,  0, 1, 0, 0)  (0, 0, -1, 0,  0, -1 )
C     (5, 5, 5, 1, 0, 0)  (-1, -1, -1, 2, 0, 0)  (0, 0,  0, 0, 11,-11 )
C
C$ Restrictions
C
C     None.
C
C$ Literature_References
C
C     None.
C
C$ Author_and_Institution
C
C     W.L. Taber      (JPL)
C
C$ Version
C
C-    SPICELIB Version 1.0.1, 22-APR-2010 (NJB)
C
C        Header correction: assertions that the output
C        can overwrite the input have been removed.
C
C-    SPICELIB Version 1.0.0, 15-JUN-1995 (WLT)
C
C
C-&

C$ Index_Entries
C
C     Compute the derivative of a cross product
C
C-&
C

C
C     Local Variables
C
      DOUBLE PRECISION      VTEMP  ( 3 )
      DOUBLE PRECISION      DVTMP1 ( 3 )
      DOUBLE PRECISION      DVTMP2 ( 3 )

C
C     Calculate the cross product of S1 and S2, store it in VTEMP.
C
      CALL VCRSS ( S1(1), S2(1), VTEMP  )

C
C     Calculate the two components of the derivative of S1 x S2.
C
      CALL VCRSS ( S1(4), S2(1), DVTMP1 )
      CALL VCRSS ( S1(1), S2(4), DVTMP2 )

C
C     Put all of the pieces into SOUT.
C
      CALL VEQU  ( VTEMP,          SOUT(1) )
      CALL VADD  ( DVTMP1, DVTMP2, SOUT(4) )

      RETURN
      END


!     Last change:  PND   6 Sep 2007    5:56 pm

module parameters
        implicit none
        REAL, PARAMETER  	:: b = 0.99, d = 0.025, a = 0.36
        REAL, PARAMETER  	:: klb = 0.01, inc = 0.025, kub = 45.0
        INTEGER, PARAMETER 	:: length_grid_k = (kub-klb)/inc+1
        INTEGER, PARAMETER:: no_states = 2.
        REAL, PARAMETER:: stay_good_prob = 0.977, stay_bad_prob = 0.926   ! for the transitions
        REAL, PARAMETER 	:: toler   = 1.e-4						! Numerical tolerance
end module

! ============================================================================================
! ============================================================================================

PROGRAM  HW2NonStochastic
        REAL 			:: total, dist
        REAL, DIMENSION(2)  		:: elapsed

        call solution
        call etime(elapsed, total)

        PRINT*,'--------------------------------------------------'
        PRINT*,'total time elapsed =',elapsed, total
        PRINT*,'--------------------------------------------------'

END PROGRAM HW2NonStochastic

! ============================================================================================
subroutine solution
        USE parameters
        IMPLICIT  NONE

        REAL, dimension(:), allocatable		:: Kgrid, g_k
        REAL, dimension(:,:), allocatable:: value, value_new
        INTEGER, DIMENSION(:,:),allocatable :: PFUNC
        REAL     :: Zgrid(no_states) = (/1.25, 0.2/)
        REAL  :: Pi(no_states, no_states)
        REAL, dimension(:,:,:), allocatable                :: vtmp
        INTEGER:: iter, index_k, index_kp, index_z
        REAL:: diffg, diffb, diff, k, kp, c, z

        INTEGER:: i = 1

        allocate(value(length_grid_k, no_states))
        allocate(value_new(length_grid_k, no_states))
        allocate(PFUNC(length_grid_k, no_states))
        allocate(vtmp(length_grid_k, length_grid_k, no_states))
        allocate(Kgrid(length_grid_k))
        allocate(g_k, mold = Kgrid)
        do while (i <= length_grid_k)   !do loop for assigning capital grid K
        Kgrid(i) = klb + (i-1)*inc
        !write(*,*) i, Kgrid(i)
        i = i+1
        end do

        Pi(1, :) = (/stay_good_prob, 1-stay_bad_prob/)
        Pi(2, :) = (/1-stay_good_prob, stay_bad_prob/)
        iter = 1
        diff = 1000.d0
        value(:,1)= 0.*Kgrid		!Initial Value guess
        value(:, 2) = 0.*Kgrid
        do while (diff >= toler)
        !$omp parallel do default(shared) private(index_k, index_z, k, vtmp, z, kp, c)
        do index_k = 1, length_grid_k				! Capital grid
        k = Kgrid(index_k)
        vtmp(index_k, :,:) = -1.0e-16
        do index_z = 1, no_states
        z = Zgrid(index_z)
        do index_kp = 1, length_grid_k
        kp = Kgrid(index_kp)
        c = k**a+(1.-d)*k-kp

        if (c > 0.) then
                vtmp(index_k, index_kp, index_z) = log(c)+b*(Pi(index_z, 1)*value(index_kp, 1)+Pi(index_z, 2)*value(index_kp, 2))
        endif

        enddo
        value_new(index_k, index_z) = MAXVAL(vtmp(index_k, :,index_z))
        PFUNC(index_k, index_z) = MAXLOC(vtmp(index_k, :, index_z), 1)
        g_k(index_k) 	   = Kgrid(MAXLOC(vtmp(index_k, :, index_z), 1))
        enddo
        enddo
        !$omp end parallel do
        diffg  = maxval(abs(value_new(:,1)-value(:,1)))/ABS(value_new(length_grid_k, 1))
        diffb  = maxval(abs(value_new(:,2)-value(:,2)))/ABS(value_new(length_grid_k, 2))
        diff = max(abs(diffg), abs(diffb))
        value = value_new
        iter = iter+1

        enddo

        print *, ' '
        print *, 'Successfully converged with sup_norm ', diff, 'with number of iteration:', iter
        !print *, g_k

        !CALL vcDrawCurve(d, Kgrid, g_k, length_grid_k)

        open (UNIT = 1, FILE='VFUNC',STATUS='replace')
        do index_k = 1, length_grid_k
        WRITE(UNIT = 1, FMT=*) value(index_k, :)
        end do
        close (UNIT = 1)

        open (UNIT = 2, FILE='AGRID',STATUS='replace')
        do index_k = 1, length_grid_k
        WRITE(UNIT = 2, FMT=*) Kgrid(index_k)
        end do
        close (UNIT = 2)

        open (UNIT = 3, FILE='PFUNC',STATUS='replace')
        do index_k = 1, length_grid_k
        WRITE(UNIT = 3, FMT=*) PFUNC(index_k,:)
        end do
        close (UNIT = 3)

end subroutine


      subroutine setupq ( x, dx, y, npoint, v, qty )
c  from  * a practical guide to splines *  by c. de boor
c  to be called in  s m o o t h
c  put	delx = x(.+1) - x(.)  into  v(.,4),
c  put	the three bands of  q-transp*d	into  v(.,1-3), and
c  put the three bands of  (d*q)-transp*(d*q)  at and above the diagonal
c     into  v(.,5-7) .
c     here,  q is  the tridiagonal matrix of order (npoint-2,npoint)
c  with general row  1/delx(i) , -1/delx(i) - 1/delx(i+1) , 1/delx(i+1)
c  and	 d  is the diagonal matrix  with general row  dx(i) .
      integer npoint,	i,npm1
      real dx(npoint),qty(npoint),v(npoint,7),x(npoint),y(npoint),
     *							  diff,   prev
      npm1 = npoint - 1
      v(1,4) = x(2) - x(1)
      do 11 i=2,npm1
	 v(i,4) = x(i+1) - x(i)
	 v(i,1) = dx(i-1)/v(i-1,4)
	 v(i,2) = - dx(i)/v(i,4) - dx(i)/v(i-1,4)
   11	 v(i,3) = dx(i+1)/v(i,4)
      v(npoint,1) = 0.
      do 12 i=2,npm1
   12	 v(i,5) = v(i,1)**2 + v(i,2)**2 + v(i,3)**2
      if (npm1 .lt. 3)			go to 14
      do 13 i=3,npm1
   13	 v(i-1,6) = v(i-1,2)*v(i,1) + v(i-1,3)*v(i,2)
   14 v(npm1,6) = 0.
      if (npm1 .lt. 4)			go to 16
      do 15 i=4,npm1
   15	 v(i-2,7) = v(i-2,3)*v(i,1)
   16 v(npm1-1,7) = 0.
      v(npm1,7) = 0.
construct  q-transp. * y  in  qty.
      prev = (y(2) - y(1))/v(1,4)
      do 21 i=2,npm1
	 diff = (y(i+1)-y(i))/v(i,4)
	 qty(i) = diff - prev
   21	 prev = diff
					return
      end


!
!*******************************************************************************
!
subroutine Legendre_expand(jx_max,xval,Ang_Dist,L_max,Ang_L)
!
!*******************************************************************************
!
!  Discussion:
!
!  FORTRAN 90 subroutine to find the coefficients for an expaansion into
!  Legendre polynomials
!
!  Ang_Dist(x) = Sum Ang_L(L)*poly(L,1,alpha, beta,x)
!
!  Ang_L(L) = int(-1,1) Ang_Dist(x)*poly(L,1,alpha,beta,x)*dx*(2L+1)/2.
!
!  Note, input array is interpolated to be put onto the grid for the
!  Gauss-Legendre integration points
!
!   Dependencies:
!
!     Modules:
!
!        None
!
!     Subroutines:
!
!        None
!
!     External functions:
!
!        real(kind=8) :: Poly
!        real(kind=8) :: interp
!
!     MPI routines:
!
!        None
!
!  Licensing:
!
!    SPDX-License-Identifier: MIT
!
!  Date:
!
!    11 May 2021
!
!  Author:
!
!      Erich Ormand, LLNL
!
!*******************************************************************************
!
   implicit none
   integer(kind=4), intent(in) :: jx_max
   real(kind=8), intent(in) :: xval(jx_max),Ang_Dist(jx_max)
   integer(kind=4), intent(in) :: L_max
   real(kind=8), intent(out) :: Ang_L(0:L_max)
!------------------------------------------------------
   integer(kind=4) :: L, LL_max
   real(kind=8) :: sum, value
   real(kind=8) :: alpha, beta
   real(kind=8) :: xnorm
!   integer(kind=4) :: jx
   integer(kind=4) :: n
   integer(kind=4) :: n_gleg
   real(kind=8), allocatable :: x_gleg(:), w_gleg(:)
!-------------   External function calls
   real(kind=8) :: Poly
   real(kind=8) :: interp
!------------------------------------------------------
!
!----   Set up data for Legendre integration
!----   Can't have more polynomials than number of data points in Ang_Dist
!----   The array Ang_Dist is assumed to be on a fixed grid
!----   with values xval(jx) and fixed spacing dx with dx = xval(2) - xval(1)
!----   First number of computed coefficients is LL_max
!----   set LL_max = L_max, but if L_max > jx_max, adjust
!----   Then the same for number of Gauss-Legendre points. Must be <= jx_max
!----   In general, preferably less
!
   LL_max = L_max
   if(L_max > jx_max)LL_max = jx_max - 1
   n_gleg = jx_max

   Ang_L(0) = 0.5d0
   if(LL_max == 0)return    !   All done, just one value and must be 0.5

   if(.not.allocated(x_gleg))allocate(x_gleg(n_gleg))
   if(.not.allocated(w_gleg))allocate(w_gleg(n_gleg))

   alpha = 0.0d0
   beta = 0.0d0
   call gauss_quad(n_gleg, 1, alpha, beta, x_gleg, w_gleg)

   xnorm = 0.0d0
   do n = 1, n_gleg
      value = interp(x_gleg(n),jx_max,xval,Ang_Dist)
      xnorm = xnorm + value*w_gleg(n)
   end do

   Ang_L(0:LL_max) = 0.0d0

   do L = 0, L_max
      sum = 0.0d0
      do n = 1, n_gleg
         value = interp(x_gleg(n),jx_max,xval,Ang_Dist)
         sum = sum + value*w_gleg(n)*poly(L,1,alpha,beta,x_gleg(n))
      end do
      Ang_L(L) = sum*0.5d0*(2.0d0*real(L,kind=8)+1.0d0)/xnorm
   end do
   if(allocated(x_gleg))deallocate(x_gleg)
   if(allocated(w_gleg))deallocate(w_gleg)
   return
end subroutine Legendre_expand


      subroutine graeff(a,b)
c
c     --  Graeffe Root-Squaring Method   --
c     --  to Find Roots of a Polynomial  --
c
      b(1)=a(11)**2
      b(2)=2.0*a(9)*a(11)-a(10)**2
      b(3)=2.0*a(7)*a(11)-2.0*a(8)*a(10)+a(9)**2
      b(4)=2.0*a(5)*a(11)-2.0*a(6)*a(10)+2.0*a(7)*a(9)-a(8)**2
      b(5)=2.0*a(3)*a(11)-2.0*a(4)*a(10)+2.0*a(5)*a(9)-2.0*a(6)*a(8)+a(7
     . )**2
      b(6)=2.0*a(1)*a(11)-2.0*a(2)*a(10)+2.0*a(3)*a(9)-2.0*a(4)*a(8)+2.0
     . *a(5)*a(7)-a(6)**2
      b(7)=2.0*a(1)*a(9)-2.0*a(2)*a(8)+2.0*a(3)*a(7)-2.0*a(4)*a(6)+a(5)
     . **2
      b(8)=2.0*a(1)*a(7)-2.0*a(2)*a(6)+2.0*a(3)*a(5)-a(4)**2
      b(9)=2.0*a(1)*a(5)-2.0*a(2)*a(4)+a(3)**2
      b(10)=2.0*a(1)*a(3)-a(2)**2
      b(11)=a(1)**2
      return
      end


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

	z2 = z1
	c = dcabs(z2)
	c = dsqrt(c)
	b = datan2(b, a)/2.0d0
	a = c*dcos(b)
	b = c*dsin(b)
	dcsqrt = z2
	return
	end


*DECK ZSQRT
      SUBROUTINE ZSQRT (AR, AI, BR, BI)
C***BEGIN PROLOGUE  ZSQRT
C***SUBSIDIARY
C***PURPOSE  Subsidiary to ZBESH, ZBESI, ZBESJ, ZBESK, ZBESY, ZAIRY and
C            ZBIRY
C***LIBRARY   SLATEC
C***TYPE      ALL (ZSQRT-A)
C***AUTHOR  Amos, D. E., (SNL)
C***DESCRIPTION
C
C     DOUBLE PRECISION COMPLEX SQUARE ROOT, B=CSQRT(A)
C
C***SEE ALSO  ZAIRY, ZBESH, ZBESI, ZBESJ, ZBESK, ZBESY, ZBIRY
C***ROUTINES CALLED  ZABS
C***REVISION HISTORY  (YYMMDD)
C   830501  DATE WRITTEN
C   910415  Prologue converted to Version 4.0 format.  (BAB)
C***END PROLOGUE  ZSQRT
      DOUBLE PRECISION AR, AI, BR, BI, ZM, DTHETA, DPI, DRT
      DOUBLE PRECISION ZABS
      EXTERNAL ZABS
      DATA DRT , DPI / 7.071067811865475244008443621D-1,
     1                 3.141592653589793238462643383D+0/
C***FIRST EXECUTABLE STATEMENT  ZSQRT
      ZM = ZABS(AR,AI)
      ZM = SQRT(ZM)
      IF (AR.EQ.0.0D+0) GO TO 10
      IF (AI.EQ.0.0D+0) GO TO 20
      DTHETA = DATAN(AI/AR)
      IF (DTHETA.LE.0.0D+0) GO TO 40
      IF (AR.LT.0.0D+0) DTHETA = DTHETA - DPI
      GO TO 50
   10 IF (AI.GT.0.0D+0) GO TO 60
      IF (AI.LT.0.0D+0) GO TO 70
      BR = 0.0D+0
      BI = 0.0D+0
      RETURN
   20 IF (AR.GT.0.0D+0) GO TO 30
      BR = 0.0D+0
      BI = SQRT(ABS(AR))
      RETURN
   30 BR = SQRT(AR)
      BI = 0.0D+0
      RETURN
   40 IF (AR.LT.0.0D+0) DTHETA = DTHETA + DPI
   50 DTHETA = DTHETA*0.5D+0
      BR = ZM*COS(DTHETA)
      BI = ZM*SIN(DTHETA)
      RETURN
   60 BR = ZM*DRT
      BI = ZM*DRT
      RETURN
   70 BR = ZM*DRT
      BI = -ZM*DRT
      RETURN
      END


program ddeabm_example

use ddeabm_module
use ddeabm_kinds

implicit none

real(wp),parameter :: mu = 398600.436233_wp !! Earth gravitational parameter (km^3/s^2)
integer,parameter  :: n = 6                 !! number of state variables

type(ddeabm_class) :: s
real(wp),dimension(n) :: x0,x
real(wp) :: t0,tf,t
integer :: idid

call s%initialize(n,maxnum=10000,df=twobody,rtol=[1.0e-12_wp],atol=[1.0e-12_wp])

!initial conditions:
x0 = [10000.0_wp,10000.0_wp,10000.0_wp,&   !initial state [r,v] (km,km/s)
        1.0_wp,2.0_wp,3.0_wp]
t0 = 0.0_wp       !initial time (sec)
tf = 1000.0_wp    !final time (sec)

write(*,'(A/,*(F15.6/))') 'Initial time: ',t0
write(*,'(A/,*(F15.6/))') 'Initial state:',x0
t = t0
x = x0
call s%integrate(t,x,tf,idid=idid)
write(*,'(A/,*(F15.6/))') 'Final time: ',t
write(*,'(A/,*(F15.6/))') 'Final state:',x

contains

    subroutine twobody(me,t,x,xdot)

        !! derivative routine for two-body orbit propagation

        implicit none

        class(ddeabm_class),intent(inout) :: me
        real(wp),intent(in)               :: t
        real(wp),dimension(:),intent(in)  :: x
        real(wp),dimension(:),intent(out) :: xdot

        real(wp),dimension(3) :: r,v,a_grav
        real(wp) :: rmag

        r = x(1:3)
        v = x(4:6)
        rmag = norm2(r)
        a_grav = -mu/rmag**3 * r ! acceleration due to gravity

        xdot(1:3) = v
        xdot(4:6) = a_grav

    end subroutine twobody

end program ddeabm_example


      PROGRAM xhunt
C     driver for routine hunt
      INTEGER N
      PARAMETER(N=100)
      INTEGER i,j,ji
      REAL x,xx(N)
C     create array to be searched
      do 11 i=1,N
        xx(i)=exp(i/20.0)-74.0
11    continue
      write(*,*) 'Result of:    j=0 indicates x too small'
      write(*,*) '            j=100 indicates x too large'
      write(*,'(t7,a7,t17,a5,t25,a1,t32,a5,t42,a7)') 'locate:',
     *     'guess','j','xx(j)','xx(j+1)'
C     perform test
      do 12 i=1,19
        x=-100.0+200.0*i/20.0
C     trial parameter
        ji=5*i
        j=ji
C     begin search
        call hunt(xx,N,x,j)
        if (j.eq.0) then
          write(*,'(1x,f12.6,2i6,a12,f12.6)') x,ji,j,
     *         'lower lim',xx(j+1)
        else if (j.eq.N) then
          write(*,'(1x,f12.6,2i6,f12.6,a12)') x,ji,j,
     *         xx(j),'upper lim'
        else
          write(*,'(1x,f12.6,2i6,2f12.6)') x,ji,j,xx(j),xx(j+1)
        endif
12    continue
      END

NR-Functions/Numerical Recipes- Example & Functions/Examples/xhunt.for


      SUBROUTINE ZPOLZ(A,NDEG,Z,H,IERR)
c Copyright (c) 1996 California Institute of Technology, Pasadena, CA.
c ALL RIGHTS RESERVED.
c Based on Government Sponsored Research NAS7-03001.
c>> 1996-04-27 ZPOLZ  Krogh Changes to use .C. and C%%.
c>> 1996-03-30 ZPOLZ  Krogh Added external statement, removed "!..."
C>> 1995-01-18 ZPOLZ  Krogh More M77CON for conversion to C.
C>> 1995-11-17 ZPOLZ  Krogh Added M77CON statements for conversion to C
C>> 1995-11-17 ZPOLZ  Krogh Converted SFTRAN to Fortran 77.
c>> 1992-05-11 CLL IERR was not being set when N = 0 or 1. Fixed this.
C>> 1989-10-20 CLL Delcared all variables.
C>> 1987-02-25 ZPOLZ  Lawson  Initial code.
c--Z Replaces "?": ?POLZ, ?QUO
C--D (Type) Replaces "?": ?COMQR
c++ Default NO_COMPLEX = .C. | (.N. == 'D')
c++ Default COMPLEX = ~NO_COMPLEX
C     ------------------------------------------------------------------
C
C In the discussion below, the notation A([*,],k} should be interpreted
c as the complex number A(k) if A is declared complex, and should be
c interpreted as the complex number A(1,k) + i * A(2,k) if A is not
c declared to be of type complex.  Similar statements apply for Z(k).
C     Given complex coefficients A([*,[1),...,A([*,]NDEG+1) this
C     subr computes the NDEG roots of the polynomial
C                 A([*,]1)*X**NDEG + ... + A([*,]NDEG+1)
C     storing the roots as complex numbers in the array Z( ).
C     Require NDEG .ge. 1 and A([*,]1) .ne. 0.
C
C     ------------------------------------------------------------------
C
C     Argument Definitions
C     --------------------
C
C     A( )     (In) Contains the complex coefficients of a polynomial
C              high order coefficient first, with A([*,]1).ne.0. The
C              real and imaginary parts of the Jth coefficient must
C              be provided in A([*],J). The contents of this array will
C              not be modified by the subroutine.
C
C     NDEG     (In) Degree of the polynomial.
C
C     Z( )     (Out) Contains the polynomial roots stored as complex
C              numbers.  The real and imaginary parts of the Jth root
C              will be stored in Z([*,]J).
C
C     H( )     (Scratch) Array of work space.
C
C     IERR     (Out) Error flag. Set by the subroutine to 0 on normal
C              termination. Set to -1 if A([*,]1)=0. Set to -2 if NDEG
C              .le. 0. Set to  J > 0 if the iteration count limit
C              has been exceeded and roots 1 through J have not been
C              determined.
C
C     ------------------------------------------------------------------
C     C.L.Lawson & S.Y.Chan, JPL, June 3,1986.
C     ------------------------------------------------------------------
      EXTERNAL I1MACH
      INTEGER I, IERR, I1MACH, J, NDEG, N
      DOUBLE PRECISION B2, C, C95, F, G, H(NDEG,NDEG,2)
      DOUBLE PRECISION ONE, R, BASE, S, ZERO
      LOGICAL MORE,FIRST
      PARAMETER(ZERO=0.D0, ONE=1.D0, C95=.95D0)
c++ CODE for COMPLEX is inactive
C      COMPLEX A(NDEG+1),TEMP,Z(NDEG)
c++ CODE for NO_COMPLEX is active
      DOUBLE PRECISION A(2,NDEG+1), TEMP(2), Z(2,NDEG)
c++ END
C
      SAVE FIRST,BASE,B2
C
      DATA FIRST /.TRUE./
C     ------------------------------------------------------------------
C
      IF (FIRST) THEN
C
C     Set BASE = machine dependent parameter specifying the base
C                 of the machine floating point representation.
C
        FIRST = .FALSE.
        BASE = I1MACH(10)
        B2 = BASE * BASE
      END IF
C
      IF (NDEG .LE. 0) THEN
        IERR = -2
        CALL ERMSG('ZPOLZ',IERR,0,'NDEG .LE. 0','.')
        RETURN
      END IF
C
c++ CODE for COMPLEX is inactive
c      IF (A(1) .EQ. CMPLX(ZERO, ZERO)) THEN
c++ CODE for NO_COMPLEX is active
      IF (A(1,1) .EQ. ZERO .AND. A(2,1) .EQ. ZERO) THEN
c++ END
        IERR = -1
        CALL ERMSG('ZPOLZ',IERR,0,'A(*,1) .EQ. ZERO','.')
        RETURN
      END IF
C
      N = NDEG
      IERR = 0
C
C     Build first row of companion matrix.
c
      DO 20 I = 2,N+1
c++ CODE for COMPLEX is inactive
C        TEMP = -(A(I)/A(1))
C        H(1,I-1,1) = REAL(TEMP)
C        H(1,I-1,2) = AIMAG(TEMP)
c++ CODE for NO_COMPLEX is active
        CALL ZQUO(A(1,I),A(1,1),TEMP)
        H(1,I-1,1) = -TEMP(1)
        H(1,I-1,2) = -TEMP(2)
c++ END
   20 CONTINUE
C
C     Extract any exact zero roots and set N = degree of
C     remaining polynomial.
C
      DO 30 J = NDEG,1,-1
c++ CODE for COMPLEX is inactive
C        IF (H(1,J,1).NE.ZERO .OR. H(1,J,2).NE.ZERO) go to 40
C        Z(J) = ZERO
c++ CODE for NO_COMPLEX is active
        IF (H(1,J,1).NE.ZERO .OR. H(1,J,2).NE.ZERO) GO TO 40
        Z(1,J) = ZERO
        Z(2,J) = ZERO
c++ END
        N = N - 1
   30 CONTINUE
   40 CONTINUE
C
C     Special for N = 0 or 1.
C
      IF (N .EQ. 0) RETURN
      IF (N .EQ. 1) THEN
c++ CODE for COMPLEX is inactive
C        Z(1) = CMPLX(H(1,1,1),H(1,1,2))
c++ CODE for NO_COMPLEX is active
        Z(1,1) = H(1,1,1)
        Z(2,1) = H(1,1,2)
c++ END
        RETURN
      END IF
C
C     Build rows 2 thru N of the companion matrix.
C
      DO 80 I = 2,N
        DO 60 J = 1,N
          IF (J .EQ. I-1) THEN
             H(I,J,1) = ONE
             H(I,J,2) = ZERO
          ELSE
            H(I,J,1) = ZERO
            H(I,J,2) = ZERO
          END IF
   60   CONTINUE
   80 CONTINUE
C
C ***************** BALANCE THE MATRIX ***********************
C
C     This is an adaption of the EISPACK subroutine BALANC to
C     the special case of a complex companion matrix. The EISPACK
C     BALANCE is a translation of the ALGOL procedure BALANCE,
C     NUM. MATH. 13, 293-304(1969) by Parlett and Reinsch.
C     HANDBOOK FOR AUTO. COMP., VOL.II-LINEAR ALGEBRA, 315-326(1971).
C
C     ********** ITERATIVE LOOP FOR NORM REDUCTION **********
  100 CONTINUE
        MORE = .FALSE.
        DO 200 I = 1, N
C     Compute R = sum of magnitudes in row I skipping diagonal.
C             C = sum of magnitudes in col I skipping diagonal.
          IF (I .EQ. 1) THEN
            R = ABS(H(1,2,1)) + ABS(H(1,2,2))
            DO 120 J = 3,N
              R = R + ABS(H(1,J,1)) + ABS(H(1,J,2))
  120       CONTINUE
            C = ABS(H(2,1,1)) + ABS(H(2,1,2))
          ELSE
            R = ABS(H(I,I-1,1)) + ABS(H(I,I-1,2))
            C = ABS(H(1,I,1)) + ABS(H(1,I,2))
            IF (I .NE. N) THEN
              C = C + ABS(H(I+1,I,1)) + ABS(H(I+1,I,2))
            END IF
          END IF
C
C     Determine column scale factor, F.
C
          G = R / BASE
          F = ONE
          S = C + R
  140     IF (C .LT. G) THEN
            F = F * BASE
            C = C * B2
            GO TO 140
          END IF
          G = R * BASE
  160     IF (C .GE. G) THEN
            F = F / BASE
            C = C / B2
            GO TO 160
          END IF
C
C     Will the factor F have a significant effect ?
C
          IF ((C + R) / F .LT. C95 * S) THEN
C
C           Yes, so do the scaling.
C
            G = ONE / F
            MORE = .TRUE.
C
C     Scale Row I
C
            IF (I .EQ. 1) THEN
              DO 180 J = 1,N
                H(1,J,1) = H(1,J,1)*G
                H(1,J,2) = H(1,J,2)*G
  180         CONTINUE
            ELSE
              H(I,I-1,1) = H(I,I-1,1)*G
              H(I,I-1,2) = H(I,I-1,2)*G
            END IF
C
C     Scale Column I
C
            H(1,I,1) = H(1,I,1) * F
            H(1,I,2) = H(1,I,2) * F
            IF (I .NE. N) THEN
              H(I+1,I,1) = H(I+1,I,1) * F
              H(I+1,I,2) = H(I+1,I,2) * F
            END IF
C
          END IF
  200   CONTINUE
      IF (MORE) GO TO 100
C
      CALL DCOMQR(NDEG,N,1,N,H(1,1,1),H(1,1,2),Z,IERR)
C
      IF (IERR .NE. 0) THEN
        CALL ERMSG('ZPOLZ',IERR,0,'Convergence failure','.')
      END IF
      RETURN
      END


MODULE FUN_xyROTATE
!!#### PURPOSE
!! Rotate a point in polar coordinates about the origin in R^2.

!!#### EXTERNAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_Rsp,KIND_Rdp              !!((01-A-KND_IntrinsicTypes.f90))

!!#### EXTERNAL PARAMETERS
USE PAR_Constants_Rsp,ONLY: c_0_Rsp => c_0 , c_1_Rsp => c_1 !!((02-A-PAR_Constants_Rsp.f90))
USE PAR_Constants_Rdp,ONLY: c_0_Rdp => c_0 , c_1_Rdp => c_1 !!((02-A-PAR_Constants_Rdp.f90))

!!#### EXTERNAL PROCEDURES
USE FUN_xyNORM                                              !!((03-A-FUN_xyNORM.f90))
USE FUN_xyDOT                                               !!((03-A-FUN_xyDOT.f90))

!!#### DEFAULT IMPLICIT
IMPLICIT NONE

!!#### DEFAULT ACCESS
PRIVATE

!!#### PROCEDURE OVERLOADING
INTERFACE xyROTATE_V
 MODULE PROCEDURE xyROTATE_V_Rsp
 MODULE PROCEDURE xyROTATE_V_Rdp
END INTERFACE

INTERFACE xyROTATE_U
 MODULE PROCEDURE xyROTATE_U_Rsp
 MODULE PROCEDURE xyROTATE_U_Rdp
END INTERFACE

INTERFACE xyROTATE_Px
 MODULE PROCEDURE xyROTATE_Px_Rsp
 MODULE PROCEDURE xyROTATE_Px_Rdp
END INTERFACE

!!#### PUBLIC ACCESS LIST
PUBLIC :: xyROTATE_V
PUBLIC :: xyROTATE_U
PUBLIC :: xyROTATE_Px

CONTAINS

PURE FUNCTION xyROTATE_V_Rsp( V , dt ) RESULT( xyV )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp               !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rsp
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rsp
INCLUDE "05-B-FUN_xyROTATE_V.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyROTATE_V.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyROTATE_V_Rdp( V , dt ) RESULT( xyV )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp               !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rdp
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rdp
INCLUDE "05-B-FUN_xyROTATE_V.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyROTATE_V.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyROTATE_U_Rsp( U , dt ) RESULT( xyU )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp               !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rsp
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rsp
INCLUDE "05-B-FUN_xyROTATE_U.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyROTATE_U.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyROTATE_U_Rdp( U , dt ) RESULT( xyU )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp               !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rdp
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rdp
INCLUDE "05-B-FUN_xyROTATE_U.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyROTATE_U.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyROTATE_Px_Rsp( N , Px , dt ) RESULT( xyPx )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp               !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rsp
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rsp
INCLUDE "05-B-FUN_xyROTATE_Px.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyROTATE_Px.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyROTATE_Px_Rdp( N , Px , dt ) RESULT( xyPx )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp               !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rdp
REAL(KIND_R),PARAMETER :: c_1 = c_1_Rdp
INCLUDE "05-B-FUN_xyROTATE_Px.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyROTATE_Px.f90.bdy"
!!--end--
END FUNCTION

END MODULE


subroutine seq_saxpy(x, y, n, a)
  real :: a, x(n), y(n)
  integer :: n, i

  do i = 1, n
    y(i) = a * x(i) + y(i)
  enddo
end subroutine seq_saxpy

subroutine par_saxpy(x, y, n, a)
  real :: a, x(n), y(n)
  integer :: n, i

  do concurrent (i = 1: n)
    y(i) = a * x(i) + y(i)
  enddo
end subroutine par_saxpy

subroutine acc_saxpy(x, y, n, a)
  real :: a, x(n), y(n)
  integer :: n, i

  !$acc parallel loop copy(x, y)
  do i = 1, n
    y(i) = a * x(i) + y(i)
  enddo
end subroutine acc_saxpy

program main
    implicit none
    integer i, n
    real, allocatable :: x(:), y(:)
    real start, finish

    n = 1000000000
    allocate(x(n), y(n))

    !$acc kernels
    x(:) = 1
    y(:) = 2
    !$acc end kernels

    call cpu_time(start)
    call acc_saxpy(x, y, n, 0.5)
    call cpu_time(finish)
    print '("OpenACC saxpy time = ",f6.3," seconds.")', (finish - start)

    call cpu_time(start)
    call par_saxpy(x, y, n, 0.5)
    call cpu_time(finish)
    print '("Parallel saxpy time = ",f6.3," seconds.")', (finish - start)

    call cpu_time(start)
    call seq_saxpy(x, y, n, 0.5)
    call cpu_time(finish)
    print '("Sequential saxpy time = ",f6.3," seconds.")', (finish - start)

    deallocate(x, y)
end program main


      subroutine func1(n, x, res)
        use, intrinsic :: iso_fortran_env, only: int64, real64
        implicit none
        integer(int64), intent(in) :: n
        real(real64), intent(in) :: x(n)
        real(real64), intent(out) :: res
Cf2py   intent(hide) :: n
        res = sum(x)
      end


module feutils
! Various Finite Element (FE) utilities
! Based on code written by John E. Pask, LLNL.
use types
use utils, only: stop_error
use quadrature, only: gauss_pts, gauss_wts, lobatto_wts, lobatto_pts
use solvers, only: solve_sym
implicit none
private
public get_nodes, define_connect, get_quad_pts, define_connect_n, &
    define_connect_np, get_parent_quad_pts_wts, &
    fe2quad, c2fullc, c2fullc2, fe_evaluate, calc_proj_error, &
    get_parent_nodes, phih, dphih, quad_gauss, quad_lobatto

integer, parameter :: quad_gauss = 1, quad_lobatto = 2

contains

subroutine get_parent_quad_pts_wts(qtype, Nq, xiq, wtq)
! Quadrature points and weights
integer, intent(in) :: qtype      ! type of quadrature: 1= Gauss, 2= Lobatto
integer, intent(in) :: Nq         ! number or quadrature points/weights
real(dp), intent(out) :: xiq(:)   ! quadrature points
real(dp), intent(out) :: wtq(:)   ! quadrature weights
select case(qtype)
    case(1)  ! Gauss
        xiq = gauss_pts(Nq)
        wtq = gauss_wts(Nq)
    case(2)  ! Lobatto
        xiq = lobatto_pts(Nq)
        wtq = lobatto_wts(Nq)
    case default
        call stop_error("Invalid quadrature type.")
end select
end subroutine

subroutine get_nodes(xe,xin,xn)
! generates basis nodes xn via affine mapping of parent nodes xin to elements xe
real(dp), intent(in) :: xe(:)     ! elements: xe(i/i+1) = coord of left/right boundary of ith element
real(dp), intent(in) :: xin(:)    ! parent basis nodes: xin(i) = coord of ith parent basis fn node
real(dp), intent(out) :: xn(:)    ! basis nodes: xn(i) = coordinate of ith basis fn node
real(dp) :: xa,xb                ! left and right element boundaries
integer Ne,p,i,j
Ne=size(xe)-1
p=size(xin)-1
xn(1)=xe(1) ! left-most node
do i=1,Ne   ! for each element ...
    xa=xe(i); xb=xe(i+1)
    xn(i*p+1)=xb   ! right-end node
    do j=2,p
        xn((i-1)*p+j)=(xin(j)+1)/2*(xb-xa)+xa ! internal nodes: affine mapping
    end do
end do
end subroutine

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

subroutine define_connect(bca,bcb,Ne,p,in,ib)
! constructs connectivity matrices in and ib defining local-global node and basis
! correspondence, respectively
integer, intent(in):: bca        ! boundary condition at x=a: 1=Dirichlet, 2=Neumann,
   ! 3=periodic, 4=antiperiodic. If bca=3 or 4, bcb must be set correspondingly
integer, intent(in):: bcb        ! boundary condition at x=b: 1=Dirichlet, 2=Neumann,
   ! 3=periodic, 4=antiperiodic. If bcb = 3 or 4, bca must be set correspondingly
integer, intent(in):: Ne         ! number of elements
integer, intent(in):: p          ! order of FE/SE basis
integer, intent(out):: in(:,:)   ! nodal connectivity: in(i,j) = index of basis node
                                 ! corresponding to local node i of element j
integer, intent(out):: ib(:,:)  ! basis connectivity: ib(i,j) = index of basis function
   ! associated with local basis function i of element j. 0 = no associated basis fn.
   ! -1 = associated with antiperiodic basis fn, with negative multiplier
integer i,e
! check boundary condition consistency
if ((bca>2 .or. bcb>2) .and. bcb/=bca) then
   write(*,'(1x,a,i0,a,i0)') "Error: bca = ", bca, " /= bcb = ", bcb
   call stop_error("stop")
end if
! construct nodal connectivity matrix
do e=1,Ne
   do i=1,p+1
      in(i,e)=(e-1)*p+i
   end do
end do
! construct basis connectivity matrix
   ! construct for natural BCs
ib=in
   ! modify for other BCs
if (bca==1) ib=ib-1        ! Dirichlet at x=a -> omit basis fn at node 1
select case(bcb)           ! x=b....
   case(1); ib(p+1,Ne)=0     ! Dirichlet
   case(3); ib(p+1,Ne)=1     ! periodic
   case(4); ib(p+1,Ne)=-1    ! antiperiodic
end select
end subroutine

subroutine define_connect_n(bca, bcb, Ne, p, Nm, in, ib)
! constructs connectivity matrices in and ib defining local-global node and
! basis correspondence, respectively. Works for arbitrary number of meshes
integer, intent(in):: bca(:)        ! bca(m) boundary condition at x=a for mesh
    ! m: 1=Dirichlet, 2=Neumann
integer, intent(in):: bcb(:)        ! bca(m) boundary condition at x=b for mesh
    ! m: 1=Dirichlet, 2=Neumann
integer, intent(in):: Ne         ! number of elements
integer, intent(in):: p          ! order of FE/SE basis
integer, intent(in):: Nm         ! number of meshes
integer, intent(out):: in(:,:,:)   ! nodal connectivity: in(i,j,k) = index of
    ! basis node corresponding to local node i of element j of mesh k
integer, intent(out):: ib(:,:,:)  ! basis connectivity: ib(i,j,k) = index of
    ! basis function associated with local basis function i of element j of
    ! mesh k.  0 = no associated basis fn
integer :: i, e, m, dofs
! construct nodal connectivity matrix
dofs = 0
do m = 1, Nm
    do e = 1, Ne
        do i = 1, p+1
            dofs = dofs + 1
            in(i, e, m) = dofs
        end do
        dofs = dofs - 1 ! connect the shape functions across the element
    end do
    dofs = dofs + 1 ! don't connect the shape functions across meshes
end do
! construct basis connectivity matrix
dofs = 0
do m = 1, Nm
    do e = 1, Ne
        do i = 1, p+1
            if (i == 1   .and. e == 1  .and. bca(m) == 1) then
                ib(i, e, m) = 0 ! Dirichlet
                cycle
            end if
            if (i == p+1 .and. e == Ne .and. bcb(m) == 1) then
                ib(i, e, m) = 0 ! Dirichlet
                cycle
            end if
            dofs = dofs + 1
            ib(i, e, m) = dofs
        end do
        dofs = dofs - 1 ! connect the shape functions across the element
    end do
    dofs = dofs + 1 ! don't connect the shape functions across meshes
end do
end subroutine

subroutine define_connect_np(bca, bcb, Ne, p, Nm, in, ib)
! constructs connectivity matrices in and ib defining local-global node and
! basis correspondence, respectively. Works for arbitrary number of meshes
integer, intent(in):: bca(:)        ! bca(m) boundary condition at x=a for mesh
    ! m: 1=Dirichlet, 2=Neumann
integer, intent(in):: bcb(:)        ! bca(m) boundary condition at x=b for mesh
    ! m: 1=Dirichlet, 2=Neumann
integer, intent(in):: Ne         ! number of elements
integer, intent(in):: p(:)          ! order of FE/SE basis
integer, intent(in):: Nm         ! number of meshes
integer, intent(out):: in(:,:,:)   ! nodal connectivity: in(i,j,k) = index of
    ! basis node corresponding to local node i of element j of mesh k
integer, intent(out):: ib(:,:,:)  ! basis connectivity: ib(i,j,k) = index of
    ! basis function associated with local basis function i of element j of
    ! mesh k.  0 = no associated basis fn
integer :: i, e, m, dofs
! Initialize the arrays to -1
in = -1
ib = -1
! construct nodal connectivity matrix
dofs = 0
do m = 1, Nm
    do e = 1, Ne
        do i = 1, p(m)+1
            dofs = dofs + 1
            in(i, e, m) = dofs
        end do
        dofs = dofs - 1 ! connect the shape functions across the element
    end do
    dofs = dofs + 1 ! don't connect the shape functions across meshes
end do
! construct basis connectivity matrix
dofs = 0
do m = 1, Nm
    do e = 1, Ne
        do i = 1, p(m)+1
            if (i == 1   .and. e == 1  .and. bca(m) == 1) then
                ib(i, e, m) = 0 ! Dirichlet
                cycle
            end if
            if (i == p(m)+1 .and. e == Ne .and. bcb(m) == 1) then
                ib(i, e, m) = 0 ! Dirichlet
                cycle
            end if
            dofs = dofs + 1
            ib(i, e, m) = dofs
        end do
        dofs = dofs - 1 ! connect the shape functions across the element
    end do
    dofs = dofs + 1 ! don't connect the shape functions across meshes
end do
end subroutine

subroutine get_quad_pts(xe,xiq,xq)
! generates quadrature points xq via affine mapping of parent quad points xiq to elements xe
real(dp), intent(in):: xe(:)     ! elements: xe(i/i+1) = coord of left/right boundary of ith element
real(dp), intent(in):: xiq(:)    ! parent quad pts
real(dp), intent(out):: xq(:,:)  ! quad pts: xq(i,j) = coordinate of ith point in jth element
real(dp) xa,xb                ! left and right element boundaries
integer ie
do ie=1,size(xe)-1
   xa=xe(ie); xb=xe(ie+1)
   xq(:,ie)=(xb-xa)/2*xiq+(xb+xa)/2 ! affine transformation
end do
end subroutine

subroutine fe2quad(xe, xin, xiq, in, fullu, uq)
! transforms fullu from FE-coefficient to quadrature-grid representation.
! fullu is a full FE coefficient vector, having values for all nodes in the mesh,
! including domain-boundary nodes.
real(dp), intent(in) :: xe(:)     ! elements: xe(i/i+1) = coord of left/right boundary of ith element
real(dp), intent(in) :: xin(:)    ! parent basis nodes: xin(i) = coordinate of ith parent basis node
real(dp), intent(in) :: xiq(:)    ! quadrature points
integer, intent(in) :: in(:,:)    ! nodal connectivity: in(i,j) = index of basis node
                                 ! corresponding to local node i of element j
real(dp), intent(in) :: fullu(:)  ! FE coefficient representatin of fullu: full vector, including
   ! values for domain-boundary nodes / basis functions
real(dp), intent(out) :: uq(:,:)  ! quadrature-grid representatin of fullu
   ! uq(i,j) = value at ith quadrature point of jth element
real(dp) :: phihq(size(xiq), size(xin)) ! parent basis fn values at quadrature points:
   ! phihq(i,j) = value of jth function at ith quadrature point
integer :: ie, iln, iq                ! element, local node, quad point indices

! tabulate parent basis at quadrature points
do iln = 1, size(xin)
    do iq = 1, size(xiq)
        phihq(iq, iln) = phih(xin, iln, xiq(iq))
    end do
end do
! evaluate at quad points in each element
do ie = 1, size(xe)-1
    uq(:, ie) = 0
    do iln = 1, size(xin)
        uq(:, ie) = uq(:, ie) + fullu(in(iln, ie)) * phihq(:, iln)
    end do
end do
end subroutine

subroutine c2fullc(in, ib, c, fullc)
! Converts FE coefficient vector to full coefficient vector
integer, intent(in) :: in(:,:,:)   ! nodal connectivity: in(i,j) = index of basis
   ! node
                                 ! corresponding to local node i of element j
integer, intent(in) :: ib(:,:,:)   ! basis connectivity: ib(i,j) = index of basis
   ! function associated with local basis function i of element j. 0 = no
   ! associated basis fn.
real(dp), intent(in) :: c(:) ! coefficient vector with regards to ib
real(dp), intent(out) :: fullc(:) ! full coefficients vector with regards to in
integer :: m, e, i
do m = 1, size(in, 3)
    do e = 1, size(in, 2)
        do i = 1, size(in, 1)
            if (in(i, e, m) == -1) cycle
            if (ib(i, e, m) == 0) then
                fullc(in(i, e, m)) = 0 ! Dirichlet
            else
                fullc(in(i, e, m)) = c(ib(i, e, m))
            end if
        end do
    end do
end do
end subroutine

subroutine c2fullc2(in, ib, c, fullc)
! Converts FE coefficient vector to full coefficient vector
! It puts 0 for Dirichlet boundary conditions (ib==0), otherwise it just copies
! the coefficients.
integer, intent(in) :: in(:,:)   ! nodal connectivity: in(i,j) = index of basis
   ! node
                                 ! corresponding to local node i of element j
integer, intent(in) :: ib(:,:)   ! basis connectivity: ib(i,j) = index of basis
   ! function associated with local basis function i of element j. 0 = no
   ! associated basis fn.
real(dp), intent(in) :: c(:) ! coefficient vector with regards to ib
real(dp), intent(out) :: fullc(:) ! full coefficients vector with regards to in
integer :: e, i
do e = 1, size(in, 2)
    do i = 1, size(in, 1)
        if (ib(i, e) == 0) then
            fullc(in(i, e)) = 0 ! Dirichlet
        else
            fullc(in(i, e)) = c(ib(i, e))
        end if
    end do
end do
end subroutine

subroutine fe_evaluate(xe, xin, xn, in, fullu, xout, yout)
! Evaluates the FE solution (fullu) on a grid.
! fullu is a full FE coefficient vector, having values for all nodes in the mesh,
! including domain-boundary nodes.
real(dp), intent(in):: xe(:)     ! elements: xe(i/i+1) = coord of left/right boundary of ith element
real(dp), intent(in):: xin(:)    ! parent basis nodes: xin(i) = coordinate of ith parent basis node
real(dp), intent(in):: xn(:)     ! basis nodes: xn(i) = coordinate of ith basis node
integer, intent(in):: in(:,:)    ! nodal connectivity: in(i,j) = index of basis node
                                 ! corresponding to local node i of element j
real(dp), intent(in):: fullu(:, :)  ! FE coefficient representatin of fullu: full vector, including
   ! values for domain-boundary nodes / basis functions
real(dp), intent(in):: xout(:)   ! grid points to evaluate the solution at
real(dp), intent(out):: yout(:, :)  ! values of the solution at 'xout'
real(dp) :: phihx(size(xin)) ! parent basis fn values at a point
integer :: p, n, e, i, j
real(dp) :: x, xa, xb, xi

p = size(xin) - 1
e = 1
do i = 1, size(xout)
    ! get x and element containing x
    x = xout(i)
    call getElement(xn, p, x, e)
    ! get parent coordinates xi
    xa = xe(e)
    xb = xe(e+1)
    xi = (x-xa)/(xb-xa)*2-1
    ! get parent basis values at xi
    do n= 1, p+1
        phihx(n) = phih(xin, n, xi)
    end do
    ! get eigenfunction value at x
    do j = 1, size(fullu, 2)
        yout(i, j) = sum(fullu(in(:,e), j) * phihx)
    end do
end do
end subroutine

subroutine getElement(xn,p,x,e)
! returns element containing point x, at or after element e
real(dp), intent(in):: xn(:)  ! basis nodes: xn(i) = coordinate of ith basis node
integer, intent(in):: p       ! order of FE/SE basis
real(dp), intent(in):: x      ! point x
integer, intent(inout):: e    ! input: index of element to start search
                              ! output: index of element containing x, at or after element e
integer Ne                 ! number of elements
Ne=(size(xn)-1)/p
if (e<1 .or. e>Ne) e=1
do
   if (x>=xn((e-1)*p+1) .and. x<=xn(e*p+1)) exit
   e=e+1
   if (e>Ne) call stop_error("getElement error: x not in mesh.")
end do
end subroutine

function calc_proj_error(p, xe, xiq, wtq, uq) result(error)
! Calculates the L2 error of the projected solution on the mesh "p"
integer, intent(in) :: p
real(dp), intent(in):: xe(:)     ! elements: xe(i/i+1) = coord of left/right boundary of ith element
real(dp), intent(in):: xiq(:), wtq(:)    ! quadrature points and weights
real(dp), intent(in):: uq(:,:)  ! FE coefficient representatin of fullu: full vector, including
real(dp) :: error(size(xe)-1) ! errors for each element
   ! values for domain-boundary nodes / basis functions
real(dp) phihq(size(xiq),p+1) ! parent basis fn values at quadrature points:
   ! phihq(i,j) = value of jth function at ith quadrature point
integer :: ie,iln,iq                 ! element, local node, quad point indices
integer :: i, j, Ne, dofs
real(dp) :: xin(p+1), al, be, jac, xa, xb, norm
real(dp), allocatable :: A(:, :), f(:), x(:), xn(:)
integer, allocatable :: in(:, :), ib(:, :)
real(dp) :: projq(size(uq, 1), size(uq, 2))

if (.not. p >= 1) call stop_error("p >= 1 required")

! define parent basis
call get_parent_nodes(2, p, xin)

Ne = size(xe) - 1

! define basis
allocate(xn(Ne*p+1))
call get_nodes(xe, xin, xn)

allocate(in(p+1,Ne),ib(p+1,Ne))
call define_connect(2, 2, Ne, p, in, ib)

! tabulate parent basis at quadrature points
do iln = 1, p+1
   do iq = 1, size(xiq)
      phihq(iq,iln) = phih(xin,iln,xiq(iq))
   end do
end do

dofs = maxval(in)
!print *, "DOFS=", dofs
allocate(A(dofs, dofs), f(dofs), x(dofs))

A = 0
f = 0
do ie = 1, Ne ! sum over elements
    xa=xe(ie); xb=xe(ie+1)
    al=(xb-xa)/2; be=(xb+xa)/2
    jac=al
    do i = 1, p+1
        do j = 1, p+1
            A(in(i,ie), in(j,ie)) = A(in(i,ie), in(j,ie)) + &
                sum(wtq*phihq(:, i)*phihq(:, j)*jac)
        end do
        f(in(i,ie)) = f(in(i,ie)) + sum(wtq*phihq(:, i)*uq(:, ie)*jac)
    end do
end do
! Solve A x = f
x = solve_sym(A, f)
call fe2quad(xe,xin,xiq,in,x,projq)
norm = 0
do ie = 1, Ne ! sum over elements
    xa=xe(ie); xb=xe(ie+1)
    al=(xb-xa)/2; be=(xb+xa)/2
    jac=al
    !print *, "el =", ie, sqrt(sum(wtq * abs(uq(:, ie)-projq(:, ie))**2 * jac))
    error(ie) = sum(wtq * abs(uq(:, ie)-projq(:, ie))**2 * jac)
    norm = norm + sum(wtq * abs(uq(:, ie))**2 * jac)
end do
norm = sqrt(norm)
error = sqrt(error)/norm
!open(newunit=ff, file="proj.dat", status="replace")
!write(ff, *) xn
!write(ff, *) x
!close(ff)
end function

subroutine get_parent_nodes(btype, p, xin)
integer, intent(in):: btype    ! type of basis: 1=uniform node, 2=Lobatto node
integer, intent(in):: p        ! order of FE/SE basis
! parent basis nodes: xin(i) = coordinate of ith parent basis node:
real(dp), intent(out):: xin(:)
integer i
if (p < 1) call stop_error("Error: p < 1.")
if (size(xin) /= p+1) call stop_error("Error: size(xin) /= p+1")
select case (btype)
    case(1)  ! uniform
        do i = 1, p+1
            xin(i) = real(i-1, dp)/p*2-1
        end do
    case(2)  ! Lobatto
        xin=lobatto_pts(p+1)
    case default
        call stop_error("Error: invalid basis type.")
end select
end subroutine

real(dp) pure function phih(xin, n, xi)
! "phi hat": nth Lagrange polynomial with nodes xin at point xi.
! Value equals to 1 for xi=xin(n) and zero for all other nodes.
real(dp), intent(in) :: xin(:) ! polynomial nodes
integer, intent(in) :: n       ! polynomial index
real(dp), intent(in) :: xi     ! point at which to evaluate phih
integer :: i
! compute nth polynomial: 1 at node n, 0 at all others
phih = 1
do i = 1, size(xin)
    if (i == n) cycle
    phih = phih * (xi-xin(i))/(xin(n)-xin(i))
end do
end function

real(dp) pure function dphih(xin, n, xi)
! "d phi hat": derivative of nth Lagrange polynomial with nodes 'xin' at point
! 'xi'.
real(dp), intent(in) :: xin(:) ! polynomial nodes
integer, intent(in) :: n       ! polynomial index
real(dp), intent(in) :: xi     ! point at which to evaluate dphih
real(dp) :: term
real(dp) :: tmp(size(xin))
integer :: i, j
do i = 1, size(xin)
    if (i==n) cycle
    tmp(i) = (xi-xin(i))/(xin(n)-xin(i))
end do
! compute derivative of nth polynomial
dphih = 0
do j = 1, size(xin)
    if (j == n) cycle
    term = 1 / (xin(n)-xin(j))
    do i = 1, size(xin)
        if (i==n .or. i==j) cycle
        term = term * tmp(i)
    end do
    dphih = dphih + term
end do
end function

real(dp) pure function hermite_basis_1(xin, n, xi) result(h)
! nth Hermite basis polynomial (first kind) with nodes 'xin' at point 'xi'.
! Value equals to 1 for xi=xin(n) and zero for all other nodes. Derivatives are
! zero for all nodes.
real(dp), intent(in) :: xin(:) ! polynomial nodes
integer, intent(in) :: n       ! polynomial index
real(dp), intent(in) :: xi     ! point at which to evaluate phih
h = (1-2*dphih(xin, n, xin(n))*(xi - xin(n))) * phih(xin, n, xi)**2
end function

real(dp) pure function hermite_basis_2(xin, n, xi) result(h)
! nth Hermite basis polynomial (second kind) with nodes 'xin' at point 'xi'.
! Value is zero for all nodes. Derivative equals to 1 for xi=xin(n) and zero
! for all other nodes.
real(dp), intent(in) :: xin(:) ! polynomial nodes
integer, intent(in) :: n       ! polynomial index
real(dp), intent(in) :: xi     ! point at which to evaluate phih
h = (xi - xin(n)) * phih(xin, n, xi)**2
end function

end module


!
! Copyright (C) 2001-2010 Quantum ESPRESSO group
! This file is distributed under the terms of the
! GNU General Public License. See the file `License'
! in the root directory of the present distribution,
! or http://www.gnu.org/copyleft/gpl.txt .
!
!
!-----------------------------------------------------------------------
SUBROUTINE multable (nsym, s, table)
  !-----------------------------------------------------------------------
  !
  !  Checks that {S} is a group and calculates multiplication table
  !
  IMPLICIT NONE
  !
  INTEGER, INTENT(IN) :: nsym, s(3,3,nsym)
  ! nsym = number of symmetry operations
  ! s    = rotation matrix (in crystal axis, represented by integers)
  INTEGER, INTENT(OUT) :: table (48, 48)
  ! multiplication table:  S(n)*S(m) = S (table(n,m) )
  !
  INTEGER :: isym, jsym, ksym, ss (3, 3)
  LOGICAL :: found, smn
  !
  DO isym = 1, nsym
     DO jsym = 1, nsym
        !
        ss = MATMUL (s(:,:,jsym),s(:,:,isym))
        !
        !     here we check that the input matrices really form a group
        !     and we set the multiplication table
        !
        found = .false.
        DO ksym = 1, nsym
           smn =  ALL ( s(:,:,ksym) == ss(:,:) )
           IF (smn) THEN
              IF (found) CALL errore ('multable', 'Not a group', 1)
              found = .true.
              table (jsym, isym) = ksym
           END IF
        END DO
        IF ( .NOT.found) CALL errore ('multable', ' Not a group', 2)
     END DO
  END DO
  RETURN
  !
END SUBROUTINE multable


submodule (stdlib_linalg) stdlib_linalg_diag

  implicit none

contains

      module function diag_rdp(v) result(res)
        real(dp), intent(in) :: v(:)
        real(dp) :: res(size(v),size(v))
        integer :: i
        res = 0
        do i = 1, size(v)
          res(i,i) = v(i)
        end do
      end function diag_rdp
      module function diag_cdp(v) result(res)
        complex(dp), intent(in) :: v(:)
        complex(dp) :: res(size(v),size(v))
        integer :: i
        res = 0
        do i = 1, size(v)
          res(i,i) = v(i)
        end do
      end function diag_cdp
      module function diag_iint32(v) result(res)
        integer(int32), intent(in) :: v(:)
        integer(int32) :: res(size(v),size(v))
        integer :: i
        res = 0
        do i = 1, size(v)
          res(i,i) = v(i)
        end do
      end function diag_iint32

      module function diag_rdp_k(v,k) result(res)
        real(dp), intent(in) :: v(:)
        integer, intent(in) :: k
        real(dp) :: res(size(v)+abs(k),size(v)+abs(k))
        integer :: i, sz
        sz = size(v)
        res = 0
        if (k > 0) then
          do i = 1, sz
              res(i,k+i) = v(i)
          end do
        else if (k < 0) then
          do i = 1, sz
              res(i+abs(k),i) = v(i)
          end do
        else
          do i = 1, sz
              res(i,i) = v(i)
          end do
        end if
      end function diag_rdp_k
      module function diag_cdp_k(v,k) result(res)
        complex(dp), intent(in) :: v(:)
        integer, intent(in) :: k
        complex(dp) :: res(size(v)+abs(k),size(v)+abs(k))
        integer :: i, sz
        sz = size(v)
        res = 0
        if (k > 0) then
          do i = 1, sz
              res(i,k+i) = v(i)
          end do
        else if (k < 0) then
          do i = 1, sz
              res(i+abs(k),i) = v(i)
          end do
        else
          do i = 1, sz
              res(i,i) = v(i)
          end do
        end if
      end function diag_cdp_k
      module function diag_iint32_k(v,k) result(res)
        integer(int32), intent(in) :: v(:)
        integer, intent(in) :: k
        integer(int32) :: res(size(v)+abs(k),size(v)+abs(k))
        integer :: i, sz
        sz = size(v)
        res = 0
        if (k > 0) then
          do i = 1, sz
              res(i,k+i) = v(i)
          end do
        else if (k < 0) then
          do i = 1, sz
              res(i+abs(k),i) = v(i)
          end do
        else
          do i = 1, sz
              res(i,i) = v(i)
          end do
        end if
      end function diag_iint32_k

      module function diag_rdp_mat(A) result(res)
        real(dp), intent(in) :: A(:,:)
        real(dp) :: res(minval(shape(A)))
        integer :: i
        do i = 1, minval(shape(A))
          res(i) = A(i,i)
        end do
      end function diag_rdp_mat
      module function diag_cdp_mat(A) result(res)
        complex(dp), intent(in) :: A(:,:)
        complex(dp) :: res(minval(shape(A)))
        integer :: i
        do i = 1, minval(shape(A))
          res(i) = A(i,i)
        end do
      end function diag_cdp_mat
      module function diag_iint32_mat(A) result(res)
        integer(int32), intent(in) :: A(:,:)
        integer(int32) :: res(minval(shape(A)))
        integer :: i
        do i = 1, minval(shape(A))
          res(i) = A(i,i)
        end do
      end function diag_iint32_mat

      module function diag_rdp_mat_k(A,k) result(res)
        real(dp), intent(in) :: A(:,:)
        integer, intent(in) :: k
        real(dp) :: res(minval(shape(A))-abs(k))
        integer :: i, sz
        sz = minval(shape(A))-abs(k)
        if (k > 0) then
          do i = 1, sz
              res(i) = A(i,k+i)
          end do
        else if (k < 0) then
          do i = 1, sz
              res(i) = A(i+abs(k),i)
          end do
        else
          do i = 1, sz
              res(i) = A(i,i)
          end do
        end if
      end function diag_rdp_mat_k
      module function diag_cdp_mat_k(A,k) result(res)
        complex(dp), intent(in) :: A(:,:)
        integer, intent(in) :: k
        complex(dp) :: res(minval(shape(A))-abs(k))
        integer :: i, sz
        sz = minval(shape(A))-abs(k)
        if (k > 0) then
          do i = 1, sz
              res(i) = A(i,k+i)
          end do
        else if (k < 0) then
          do i = 1, sz
              res(i) = A(i+abs(k),i)
          end do
        else
          do i = 1, sz
              res(i) = A(i,i)
          end do
        end if
      end function diag_cdp_mat_k
      module function diag_iint32_mat_k(A,k) result(res)
        integer(int32), intent(in) :: A(:,:)
        integer, intent(in) :: k
        integer(int32) :: res(minval(shape(A))-abs(k))
        integer :: i, sz
        sz = minval(shape(A))-abs(k)
        if (k > 0) then
          do i = 1, sz
              res(i) = A(i,k+i)
          end do
        else if (k < 0) then
          do i = 1, sz
              res(i) = A(i+abs(k),i)
          end do
        else
          do i = 1, sz
              res(i) = A(i,i)
          end do
        end if
      end function diag_iint32_mat_k

end submodule


subroutine distance_table(pos, lbox, natom, ndim, dtable)
  integer, intent(in) :: natom, ndim
  real*8, intent(in) :: pos(natom, ndim), lbox
  real*8, intent(out) :: dtable(natom, natom)
  integer i, j
  real*8 drij(ndim)
  do i=1,natom
    do j=i+1,natom
      drij = pos(i, :) - pos(j, :)
      drij = drij - lbox*nint(drij/lbox)
      dtable(i, j) = norm2(drij)
      !write (*, '(4f8.4)') drij, dtable(i, j)
    end do
    !stop
  end do
end subroutine


      subroutine stat_2d (s, nx_s, nx_e, ny_s, ny_e)

!  subroutine to stat a  3-d array with ferret

!       character field_name(20)

        integer nx_s,nx_e,ny_s,ny_e,i,j
        real s(nx_s:nx_e,ny_s:ny_e)

        real sum,save,std

          sum = 0.0

       do j = ny_s+1,ny_e-1
       do i = nx_s+1,nx_e-1
          sum = s(i,j) + sum
       enddo;enddo

          save = sum / real(nx_e- nx_s-1) / real(ny_e- ny_s-1)

          sum = 0.0

       do j = ny_s+1,ny_e-1
       do i = nx_s+1,nx_e-1

          sum = (s(i,j)-save)**2 + sum

       enddo;enddo

          std = sqrt( sum / ( real(nx_e- nx_s-1) * real(ny_e- ny_s-1) -1 ))
          write(6,*) 'save,std:',real(save),real(std)

        return
        end


*DECK CPBDI
      SUBROUTINE CPBDI (ABD, LDA, N, M, DET)
C***BEGIN PROLOGUE  CPBDI
C***PURPOSE  Compute the determinant of a complex Hermitian positive
C            definite band matrix using the factors computed by CPBCO or
C            CPBFA.
C***LIBRARY   SLATEC (LINPACK)
C***CATEGORY  D3D2
C***TYPE      COMPLEX (SPBDI-S, DPBDI-D, CPBDI-C)
C***KEYWORDS  BANDED, DETERMINANT, INVERSE, LINEAR ALGEBRA, LINPACK,
C             MATRIX, POSITIVE DEFINITE
C***AUTHOR  Moler, C. B., (U. of New Mexico)
C***DESCRIPTION
C
C     CPBDI computes the determinant
C     of a complex Hermitian positive definite band matrix
C     using the factors computed by CPBCO or CPBFA.
C     If the inverse is needed, use CPBSL  N  times.
C
C     On Entry
C
C        ABD     COMPLEX(LDA, N)
C                the output from CPBCO or CPBFA.
C
C        LDA     INTEGER
C                the leading dimension of the array  ABD .
C
C        N       INTEGER
C                the order of the matrix  A .
C
C        M       INTEGER
C                the number of diagonals above the main diagonal.
C
C     On Return
C
C        DET     REAL(2)
C                determinant of original matrix in the form
C                determinant = DET(1) * 10.0**DET(2)
C                with  1.0 .LE. DET(1) .LT. 10.0
C                or  DET(1) .EQ. 0.0 .
C
C***REFERENCES  J. J. Dongarra, J. R. Bunch, C. B. Moler, and G. W.
C                 Stewart, LINPACK Users' Guide, SIAM, 1979.
C***ROUTINES CALLED  (NONE)
C***REVISION HISTORY  (YYMMDD)
C   780814  DATE WRITTEN
C   890831  Modified array declarations.  (WRB)
C   890831  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900326  Removed duplicate information from DESCRIPTION section.
C           (WRB)
C   920501  Reformatted the REFERENCES section.  (WRB)
C***END PROLOGUE  CPBDI
      INTEGER LDA,N,M
      COMPLEX ABD(LDA,*)
      REAL DET(2)
C
      REAL S
      INTEGER I
C***FIRST EXECUTABLE STATEMENT  CPBDI
C
C     COMPUTE DETERMINANT
C
      DET(1) = 1.0E0
      DET(2) = 0.0E0
      S = 10.0E0
      DO 50 I = 1, N
         DET(1) = REAL(ABD(M+1,I))**2*DET(1)
         IF (DET(1) .EQ. 0.0E0) GO TO 60
   10    IF (DET(1) .GE. 1.0E0) GO TO 20
            DET(1) = S*DET(1)
            DET(2) = DET(2) - 1.0E0
         GO TO 10
   20    CONTINUE
   30    IF (DET(1) .LT. S) GO TO 40
            DET(1) = DET(1)/S
            DET(2) = DET(2) + 1.0E0
         GO TO 30
   40    CONTINUE
   50 CONTINUE
   60 CONTINUE
      RETURN
      END


      double precision function  fdisc(blockno,xc,yc,zc)
      implicit none

      integer blockno
      double precision xc,yc, zc

      double precision r

      r = sqrt((xc-0.5d0)**2 + (yc-1.d0)**2 + (zc-0.5)**2)

      fdisc = r-0.25d0
      end


      program computation of function using do statement
      write(*,1)
1     format(3x,'x',11x,'f(x)')
      do 50 x=0.0, 1.0, 0.1
      fx=sqrt(X)*cos(2*X+5.0)
      write(*,20)x,fx
50    continue
20    format(1x,f3.1,9x,f6.4)
      pause
      stop
      end


20     PRINT *, ' Enter x, l (l .lt. 0 to stop)'
       READ *, X,L

       IF (L .LT. 0) THEN
           STOP
       ELSE IF (L .EQ. 0) THEN
           PL=0.
       ELSE IF (L .EQ. 1) THEN
           PL=X
       ELSE
           PM=1.
           PZ=X
           DO 10 IL=1,L-1
              PP=((2*IL+1)*X*PZ-IL*PM)/(IL+1)
              PM=PZ
              PZ=PP
10         CONTINUE
           PL=PZ
       END IF

       PRINT *,X,L,PL
       GOTO 20
       END


!Josh Adams combined these routines to solve the incomplete Beta
!function for the calculation of the gNFW potential. The integral
!needing solution is from Zhao'96, Eq A2 as B(a,b,x)=
!int(dt*t^(a-1)*(1-t)^(b-1),0,x). I have taken routines from numrec,
!but removed some unnecessary normalization. We also want the complete
!beta function, which can be simply related to the gamma function.
!To avoid conflicts with the
!original routines, I've prefixed zh_ to all the routines herein.
      FUNCTION zh_betai(a,b,x)
      use numeric_kinds
      Real(kind=dp):: zh_betai,a,b,x,zh_beta
!U    USES betacf,gammln
      Real(kind=dp):: bt,zh_betacf,zh_gammln
      if(x.lt.0.0_dp.or.x.gt.1.0_dp) print*, 'bad argument x in zh_betai'
      if(x.eq.0.0_dp)then
        bt=0.0_dp
      else if(x.eq.1.0_dp)then
        bt=zh_beta(a,b)
        return
      else
        bt=x**a*(1.0_dp-x)**b
      endif
      if(x.lt.(a+1.0_dp)/(a+b+2.0_dp).or.b.le.0.0_dp)then
!Can't use the inverse relation with b=0 b/c beta_comp will be infinite.
        zh_betai=bt*zh_betacf(a,b,x)/a
        return
      else
        zh_betai=zh_beta(a,b)-bt*zh_betacf(b,a,1.0_dp-x)/b
        return
      endif
      END
!---------------------------------------------------------------
      FUNCTION zh_beta(z,w)
      use numeric_kinds
      Real(kind=dp):: zh_beta,w,z
!U    USES gammln
      Real(kind=dp):: zh_gammln
      zh_beta=exp(zh_gammln(z)+zh_gammln(w)-zh_gammln(z+w))
      return
      END
!---------------------------------------------------------------
      FUNCTION zh_betacf(a,b,x)
      use numeric_kinds
      INTEGER(kind=i4b):: MAXIT
      Real(kind=dp):: zh_betacf,a,b,x,EPS,FPMIN
      PARAMETER (MAXIT=500,EPS=3.e-7_dp,FPMIN=1.e-30_dp)
      INTEGER(kind=i4b):: m,m2
      Real(kind=dp):: aa,c,d,del,h,qab,qam,qap
      qab=a+b
      qap=a+1.0_dp
      qam=a-1.0_dp
      c=1.0_dp
      d=1.0_dp-qab*x/qap
      if(abs(d).lt.FPMIN)d=FPMIN
      d=1.0_dp/d
      h=d
      do 11 m=1,MAXIT
        m2=2*m
        aa=m*(b-m)*x/((qam+m2)*(a+m2))
        d=1.0_dp+aa*d
        if(abs(d).lt.FPMIN)d=FPMIN
        c=1.0_dp+aa/c
        if(abs(c).lt.FPMIN)c=FPMIN
        d=1.0_dp/d
        h=h*d*c
        aa=-(a+m)*(qab+m)*x/((a+m2)*(qap+m2))
        d=1.0_dp+aa*d
        if(abs(d).lt.FPMIN)d=FPMIN
        c=1.0_dp+aa/c
        if(abs(c).lt.FPMIN)c=FPMIN
        d=1.0_dp/d
        del=d*c
        h=h*del
        if(abs(del-1.0_dp).lt.EPS)goto 1
11    continue
      print*, 'a or b too big, or MAXIT too small in zh_betacf', a, b, &
x, abs(del-1.0_dp)
1     zh_betacf=h
      return
      END
!---------------------------------------------------------------
      FUNCTION zh_gammln(xx)
      use numeric_kinds
      Real(kind=dp):: zh_gammln,xx
      INTEGER(kind=i4b):: j
      Real(kind=dp):: ser,stp,tmp,x,y,cof(6)
      SAVE cof,stp
      DATA cof,stp/76.18009172947146_dp,-86.50532032941677_dp,&
      24.01409824083091_dp,-1.231739572450155_dp,.1208650973866179e-2_dp,&
      -.5395239384953e-5_dp,2.5066282746310005_dp/
      x=xx
      y=x
      tmp=x+5.5_dp
      tmp=(x+0.5_dp)*log(tmp)-tmp
      ser=1.000000000190015_dp
      do 11 j=1,6
        y=y+1.0_dp
        ser=ser+cof(j)/y
11    continue
      zh_gammln=tmp+log(stp*ser/x)
      return
      END
!---------------------------------------------------------------


program outcomes

  integer, dimension(1:10) :: deck
  integer :: d,n,p
  integer, external :: partitions

  deck = (/ (4, i = 1,10) /)
  deck(10) = 16

  d = 0

  do i = 1, 10
     ! Dealer showing
     deck(i) = deck(i)-1

     p = 0
     do j = 1, 10
        deck(j) = deck(j)-1
        n = partitions(deck, j)
        deck(j) = deck(j)+1
        p = p+n
     end do

     print *,"Dealer showing ",i," partitions =",p
     d = d+p
     deck(i) = deck(i)+1
  end do

  print *,"Total partitions =",d

end program outcomes

recursive integer function partitions(cards,subtotal) result(m)
  integer :: total
  integer, dimension(1:10) :: cards
  integer :: subtotal

  m = 0
  ! Hit
  do i = 1, 10
     if (cards(i)>0) then
        total = subtotal+i
        if (total < 21) then
           ! Stand
           m = m+1
           ! Hit again
           cards(i) = cards(i)-1
           m = m+partitions(cards, total)
           cards(i) = cards(i)+1
        else if (total==21) then
           ! Stand; hit again is an automatic bust
           m = m+1
           exit
        end if
     end if
  end do
end function partitions


subroutine bondseek(n,positions,r0,r1,bn)

    implicit none

    integer*4, intent(in)       :: n
    real*8,    intent(in)       :: r0,r1
    real*8,    intent(in)       :: positions(n,3)

    integer*4 :: i,j,k
    integer*4, intent(out) :: bn

    real*8 :: xi,yi,zi,xj,yj,zj,dx,dy,dz,mdl

    bn = 0

    do i = 1, (n - 1)
        xi = positions(i,1)
        yi = positions(i,2)
        zi = positions(i,3)
        do j = 1, n
            if (i .ge. j) cycle
            xj = positions(j,1)
            yj = positions(j,2)
            zj = positions(j,3)

            dx = abs(xj - xi)
            dy = abs(yj - yi)
            dz = abs(zj - zi)

            mdl = sqrt(dx * dx + dy * dy + dz * dz)

            if (mdl .ge. r0 .and. mdl .le. r1) bn = bn + 1

        end do
    end do
    return
end subroutine bondseek


program test1

  use multigrid

  implicit none

  integer, parameter :: num=selected_real_kind(p=15)
  real(num) :: pi = 4.0_num * ATAN(1.0_num)

  integer :: nx, ny, ix, iy

  real(num) :: dx, dy, L2, x_min, x_max, y_min, y_max

  real(num), dimension(:), allocatable :: xc, yc

  real(num), dimension(:,:), allocatable :: utrue, vtrue
  real(num), dimension(:,:), allocatable :: upol, vpol
  real(num), dimension(:,:), allocatable :: divu, phi

  type(mg_input) :: input

  ! setup a test problem

  nx = 256
  ny = nx
  x_min = 0.0_num
  x_max = 1.0_num
  y_min = x_min
  y_max = x_max

  print *,'Test1: Projection test, periodic, constant coeffs Poission'
  print *,'This tests the ability of MG to solve L(phi) = div(U)'
  print *,'in order to clean divergence from a polluted velocity field'

  allocate(xc(-1:nx+2))
  allocate(yc(-1:ny+2))

  dx = (x_max - x_min) / real(nx,num)
  xc(-1) = x_min - 3.0_num * dx / 2.0_num
  do ix = 0,nx+2
    xc(ix) = xc(ix-1) + dx
  enddo

  dy = (y_max - y_min) / real(ny,num)
  yc(-1) = y_min - 3.0_num * dy / 2.0_num
  do iy = 0,ny+2
    yc(iy) = yc(iy-1) + dy
  enddo

  allocate(phi(-1:nx+2,-1:ny+2)) ! again, redundant ghosts but for comparison to CCAPS
  phi = 0.0_num

  allocate(utrue(0:nx+1,0:ny+1)) !no need for ghosts in the test case
  allocate(vtrue(0:nx+1,0:ny+1))
  allocate(upol(0:nx+1,0:ny+1))
  allocate(vpol(0:nx+1,0:ny+1))
  allocate(divu(1:nx,1:ny))

  do iy = 0, ny+1
  do ix = 0, nx+1
    utrue(ix,iy) = - sin(1.0_num * pi * xc(ix))**2 &
      & * sin(2.0_num * pi * yc(iy))
    vtrue(ix,iy) =   sin(1.0_num * pi * yc(iy))**2 &
      & * sin(2.0_num * pi * xc(ix))

    upol(ix,iy) = utrue(ix,iy) - &
      & 0.2_num * pi * cos(2.0_num * pi * yc(iy)) &
      & * sin(2.0_num * pi * xc(ix))
    vpol(ix,iy) = vtrue(ix,iy) - &
      & 0.2_num * pi * sin(2.0_num * pi * yc(iy)) &
      & * cos(2.0_num * pi * xc(ix))
  enddo
  enddo

  do iy = 1, ny
  do ix = 1, nx
    divu(ix,iy) = (upol(ix+1,iy) - upol(ix-1,iy)) / 2.0_num / dx + &
            & (vpol(ix,iy+1) - vpol(ix,iy-1)) / 2.0_num / dy
  enddo
  enddo

  print *, 'max divu before cleaning',maxval(divu)
  L2 = sqrt(sum(abs(utrue(1:nx,1:ny)-upol(1:nx,1:ny))**2) / real(nx*ny,num))
  print *, 'L2 (utrue vs upol) before cleaning',L2
  L2 = sqrt(sum(abs(vtrue(1:nx,1:ny)-vpol(1:nx,1:ny))**2) / real(nx*ny,num))
  print *, 'L2 (vtrue vs vpol) before cleaning',L2

  ! create the input state object

  input = mg_input(tol = 1e-12_num, nx=nx, ny = ny, dx=dx, dy=dy, f = divu, phi = phi, &
            & bc_xmin = 'periodic', bc_ymin='periodic', bc_xmax='periodic', bc_ymax = 'periodic')

  call mg_interface(input)

  phi = input%phi

!STOP

  ! solve for phi
!  nlevels = -1 ! auto
!  call mg_interface(f = divu, phi=phi, tol = 1e-12_num, &
!                  & nx = nx, ny = ny, dx = dx, dy = dy, &
!                  &  nlevels = nlevels, &
!  & bc_xmin_in = periodic, bc_ymin_in = periodic, bc_xmax_in = periodic, bc_ymax_in=periodic)

  ! correct it

  do iy = 1, ny
  do ix = 1, nx
    upol(ix,iy) = upol(ix,iy) - 0.5_num*(phi(ix+1,iy)-phi(ix-1,iy))/dx
    vpol(ix,iy) = vpol(ix,iy) - 0.5_num*(phi(ix,iy+1)-phi(ix,iy-1))/dy
  enddo
  enddo

  ! apply periodic bc to the polluted vels and then re calc divu
  upol(0,:) = upol(nx,:)
  upol(nx+1,:) = upol(1,:)
  upol(:,0) = upol(:,ny)
  upol(:,ny+1) = upol(:,1)
  vpol(0,:) = vpol(nx,:)
  vpol(nx+1,:) = vpol(1,:)
  vpol(:,0) = vpol(:,ny)
  vpol(:,ny+1) = vpol(:,1)

  do iy = 1, ny
  do ix = 1, nx
    divu(ix,iy) = (upol(ix+1,iy) - upol(ix-1,iy)) / 2.0_num / dx + &
            & (vpol(ix,iy+1) - vpol(ix,iy-1)) / 2.0_num / dy
  enddo
  enddo

  print *, 'max divu after cleaning',maxval(divu)
  L2 = sqrt(sum(abs(utrue(1:nx,1:ny)-upol(1:nx,1:ny))**2) / real(nx*ny,num))
  print *, 'L2 (utrue vs upol) after cleaning',L2
  L2 = sqrt(sum(abs(vtrue(1:nx,1:ny)-vpol(1:nx,1:ny))**2) / real(nx*ny,num))
  print *, 'L2 (vtrue vs vpol) after cleaning',L2

end program test1