!! R757 forall-assignment-stmt
!    is assignment-stmt
!    or pointer-assignment-stmt

! NOTE 7.57

integer, parameter :: N=10
integer :: i, j, Y(10,10)
integer :: A(10,10), X(10), Z(10,10)

13 FORALL (I=1:N) A(I,I) = X(I)

forall (I = 1:N, J = 1:N)  Z(I,J) = 1.0 / REAL (I+J-1)

FORALL (I=1:N, J=1:N, Y(I,J) /= 0 .AND. I /= J)  Z(I,J) = 1.0 / Y(I,J)

END


program allperm
    implicit none
    integer :: n, i
    integer, allocatable :: a(:)
    read *, n
    allocate(a(n))
    a = [ (i, i = 1, n) ]
    call perm(1)
    deallocate(a)
contains
    recursive subroutine perm(i)
        integer :: i, j, t
        if (i == n) then
            print *, a
        else
            do j = i, n
                t = a(i)
                a(i) = a(j)
                a(j) = t
                call perm(i + 1)
                t = a(i)
                a(i) = a(j)
                a(j) = t
            end do
        end if
    end subroutine
end program


! -*- coding: utf-8 -*-
module mod_sub
  implicit none
contains
!--------------------------------------------
  function dist2(x, y) result(res)
    implicit none

    real(8), intent(in) :: x(:)
    real(8), intent(in) :: y(:)
    real(8) :: res

    res = (x(1) - y(1))**2d0 + (x(2) - y(2))**2d0

    return

  end function dist2
!--------------------------------------------
  subroutine grouping(xn, mu, gk)
    implicit none

    real(8),intent(in) :: xn(:, :)
    real(8),intent(in) :: mu(:, :)
    integer,intent(out) :: gk(:)
    real(8) :: now
    real(8) :: tmp
    integer :: i
    integer :: j

    do i = 1, size(xn(1, :))
       now = 0d0
       do j = 1, size(mu(1, :))
          tmp = dist2(xn(:, i), mu(:, j))
          if(now .eq. 0d0) then
             now = tmp
             gk(i) = 1
          elseif(tmp .le. now) then
             now = tmp
             gk(i) = j
          end if
       end do
    end do

  end subroutine grouping
!--------------------------------------------
  subroutine mean_group(xn, mu, gk, numk)
    implicit none

    real(8),intent(in) :: xn(:, :)
    real(8),intent(out) :: mu(:, :)
    integer,intent(in) :: gk(:)
    integer,intent(out) :: numk(:)
    real(8) :: now
    real(8) :: tmp
    integer :: i
    integer :: j

    numk = 0
    do j = 1, size(mu(1, :))
       mu(:, j) = (/0d0, 0d0/)
       do i = 1, size(xn(1, :))
          if(gk(i) .eq. j) then
             numk(j) = numk(j) + 1
             mu(:, j) = mu(:, j) + xn(:, i)
          end if
       end do
       mu(:, j) = mu(:, j)/numk(j)
    end do

  end subroutine mean_group
!--------------------------------------------
end module mod_sub


      PROGRAM LA_SGBSV_EXAMPLE

!  -- LAPACK95 EXAMPLE DRIVER ROUTINE (VERSION 1.0) --
!     UNI-C, DENMARK
!     DECEMBER, 1999

!  .. "Use Statements"
      USE LA_PRECISION, ONLY: WP => SP
      USE F95_LAPACK, ONLY: LA_GBSV
!  .. "Implicit Statement" ..
      IMPLICIT NONE
!  .. "Local Scalars" ..
      INTEGER :: K, KL, KU, I, J, N, NRHS, INFO
!  .. "Local Arrays" ..
      INTEGER, ALLOCATABLE :: IPIV(:)
      REAL(WP), ALLOCATABLE :: AB(:,:), B(:,:)
!  .. "Executable Statements" ..
       WRITE (*,*) 'SGBSV Example Program Results.'
       N = 6; KL = 2; KU = 1; NRHS = 2
       ALLOCATE ( AB(2*KL+KU+1,N), B(N,NRHS), IPIV(N))

       OPEN(UNIT=21,FILE='gbsv.ma',STATUS='UNKNOWN')
       DO I=KL+1,2*KL+KU+1
       DO J=1,N
          READ(21,'(F2.0)') AB(I,J)
       ENDDO
       ENDDO
       CLOSE(21)

       WRITE(*,*) 'The matrix AB:'
       DO I=1,N; WRITE(*,"(5(I3,1X,1X),I3,1X)") INT(AB(I,:));
       ENDDO

       DO I = 1, NRHS
       DO J = 1, N
       DO K = MAX(1,J-KL),MIN(J+KU,N); B(J,I) = AB(KL+KU+1+J-K,K)+B(J,I);
       ENDDO
       ENDDO;
       B(:,I) = B(:,I)*I;
       ENDDO

       WRITE(*,*) 'The RHS matrix B:'
       DO I=1,N; WRITE(*,"(1(I3,1X),I3,1X)") INT(B(I,:)); ENDDO
       WRITE(*,*) 'CALL LA_GBSV( AB, B, 2, IPIV, INFO)'

       CALL LA_GBSV( AB, B, 2, IPIV, INFO )

       WRITE(*,*)'AB on exit: '
       DO I=1,2*KL+KU+1; WRITE(*,"(5(E12.6,1X),E12.6,1X)") AB(I,:); ENDDO
       WRITE(*,*)'B on exit: '
       DO I=1,N; WRITE(*,"(1(E12.6,1X),E12.6,1X)") B(I,:); ENDDO
       WRITE(*,*)'IPIV on exit: ', IPIV
       WRITE(*,*)'INFO on exit: ', INFO

       END PROGRAM LA_SGBSV_EXAMPLE


FUNCTION golden_sp(ax,bx,cx,func,tol,xmin)
  use mo_kind
  IMPLICIT NONE
  REAL(SP), INTENT(IN)      :: ax,bx,cx,tol
  REAL(SP), INTENT(OUT)     :: xmin
  REAL(SP)                  :: golden_sp
  INTERFACE
     FUNCTION func(x)
       use mo_kind
       IMPLICIT NONE
       REAL(SP), INTENT(IN) :: x
       REAL(SP)             :: func
     END FUNCTION func
  END INTERFACE
  REAL(SP), PARAMETER       :: R=0.61803399_sp,C=1.0_sp-R
  REAL(SP)                  :: f1,f2,x0,x1,x2,x3
  x0=ax
  x3=cx
  if (abs(cx-bx) > abs(bx-ax)) then
     x1=bx
     x2=bx+C*(cx-bx)
  else
     x2=bx
     x1=bx-C*(bx-ax)
  end if
  f1=func(x1)
  f2=func(x2)
  do
     if (abs(x3-x0) <= tol*(abs(x1)+abs(x2))) exit
     if (f2 < f1) then
        call shft3(x0,x1,x2,R*x2+C*x3)
        call shft2(f1,f2,func(x2))
     else
        call shft3(x3,x2,x1,R*x1+C*x0)
        call shft2(f2,f1,func(x1))
     end if
  end do
  if (f1 < f2) then
     golden_sp=f1
     xmin=x1
  else
     golden_sp=f2
     xmin=x2
  end if
CONTAINS
  !BL
  SUBROUTINE shft2(a,b,c)
    REAL(SP), INTENT(OUT)   :: a
    REAL(SP), INTENT(INOUT) :: b
    REAL(SP), INTENT(IN)    :: c
    a=b
    b=c
  END SUBROUTINE shft2
  !BL
  SUBROUTINE shft3(a,b,c,d)
    REAL(SP), INTENT(OUT)   :: a
    REAL(SP), INTENT(INOUT) :: b,c
    REAL(SP), INTENT(IN)    :: d
    a=b
    b=c
    c=d
  END SUBROUTINE shft3
END FUNCTION golden_sp

FUNCTION golden_dp(ax,bx,cx,func,tol,xmin)
  use mo_kind
  IMPLICIT NONE
  REAL(DP), INTENT(IN)      :: ax,bx,cx,tol
  REAL(DP), INTENT(OUT)     :: xmin
  REAL(DP)                  :: golden_dp
  INTERFACE
     FUNCTION func(x)
       use mo_kind
       IMPLICIT NONE
       REAL(DP), INTENT(IN) :: x
       REAL(DP)             :: func
     END FUNCTION func
  END INTERFACE
  REAL(DP), PARAMETER       :: R=0.61803399_dp,C=1.0_dp-R
  REAL(DP)                  :: f1,f2,x0,x1,x2,x3
  x0=ax
  x3=cx
  if (abs(cx-bx) > abs(bx-ax)) then
     x1=bx
     x2=bx+C*(cx-bx)
  else
     x2=bx
     x1=bx-C*(bx-ax)
  end if
  f1=func(x1)
  f2=func(x2)
  do
     if (abs(x3-x0) <= tol*(abs(x1)+abs(x2))) exit
     if (f2 < f1) then
        call shft3(x0,x1,x2,R*x2+C*x3)
        call shft2(f1,f2,func(x2))
     else
        call shft3(x3,x2,x1,R*x1+C*x0)
        call shft2(f2,f1,func(x1))
     end if
  end do
  if (f1 < f2) then
     golden_dp=f1
     xmin=x1
  else
     golden_dp=f2
     xmin=x2
  end if
CONTAINS
  !BL
  SUBROUTINE shft2(a,b,c)
    REAL(DP), INTENT(OUT)   :: a
    REAL(DP), INTENT(INOUT) :: b
    REAL(DP), INTENT(IN)    :: c
    a=b
    b=c
  END SUBROUTINE shft2
  !BL
  SUBROUTINE shft3(a,b,c,d)
    REAL(DP), INTENT(OUT)   :: a
    REAL(DP), INTENT(INOUT) :: b,c
    REAL(DP), INTENT(IN)    :: d
    a=b
    b=c
    c=d
  END SUBROUTINE shft3
END FUNCTION golden_dp


module diffusion
  implicit none

contains

  double precision function diffusion3d(nx, ny, nz, dx, dy, dz, dt, kappa, f, fn)

    integer,intent(in) :: nx, ny, nz
    real(KIND=4),intent(in) :: dx, dy, dz, dt, kappa
    real(KIND=4),intent(in),dimension(:,:,:) :: f
    real(KIND=4),intent(out),dimension(:,:,:) :: fn
    real(KIND=4) :: ce,cw,cn,cs,ct,cb,cc
    integer :: w,e,n,s,b,t
    integer :: i,j,k

    ce = kappa*dt/(dx*dx)
    cw = ce
    cn = kappa*dt/(dy*dy)
    cs = cn
    ct = kappa*dt/(dz*dz)
    cb = ct

    cc = 1.0 - (ce + cw + cn + cs + ct + cb)

!$acc kernels  !!present(f,fn)
!$acc loop independent
    do k = 1, nz
!$acc loop independent
       do j = 1, ny
!$acc loop independent
          do i = 1, nx

             w = -1; e = 1; n = -1; s = 1; b = -1; t = 1;
             if(i == 1)  w = 0
             if(i == nx) e = 0
             if(j == 1)  n = 0
             if(j == ny) s = 0
             if(k == 1)  b = 0
             if(k == nz) t = 0
             fn(i,j,k) = cc * f(i,j,k) + cw * f(i+w,j,k) &
                  + ce * f(i+e,j,k) + cs * f(i,j+s,k) + cn * f(i,j+n,k) &
                  + cb * f(i,j,k+b) + ct * f(i,j,k+t)

          end do
       end do
    end do
!$acc end kernels

    diffusion3d = dble(nx*ny*nz)*13.0

  end function diffusion3d

  subroutine init(nx, ny, nz, dx, dy, dz, f)

    integer,intent(in) :: nx, ny, nz
    real(KIND=4),intent(in) :: dx, dy, dz
    real(KIND=4),intent(out),dimension(:,:,:) :: f
    real(KIND=4) :: kx,ky,kz,x,y,z,pi
    integer :: i,j,k

    pi = acos(-1.0)
    kx = 2.0*pi
    ky = kx
    kz = kx

    do k = 1, nz
       do j = 1, ny
          do i = 1, nx

             x = dx*(real(i-1) + 0.5)
             y = dy*(real(j-1) + 0.5)
             z = dz*(real(k-1) + 0.5)

             f(i,j,k) = 0.125*(1.0 - cos(kx*x))*(1.0 - cos(ky*y))*(1.0 - cos(kz*z))

          end do
       end do
    end do

  end subroutine init

  double precision function accuracy(time, nx, ny, nz, dx, dy, dz, kappa, f)
    double precision,intent(out) :: time
    integer,intent(in) :: nx, ny, nz
    real(KIND=4),intent(in) :: dx, dy, dz, kappa
    real(KIND=4),intent(out),dimension(:,:,:) :: f
    real(KIND=4) :: kx,ky,kz,ax,ay,az,x,y,z,pi,f0
    double precision :: ferr
    integer :: i,j,k

    pi = acos(-1.0)
    kx = 2.0*pi
    ky = kx
    kz = kx

    ax = exp(-kappa*time*(kx*kx))
    ay = exp(-kappa*time*(ky*ky))
    az = exp(-kappa*time*(kz*kz))

    ferr = 0.d0

    do k = 1, nz
       do j = 1, ny
          do i = 1, nx

             x = dx*(real(i-1) + 0.5)
             y = dy*(real(j-1) + 0.5)
             z = dz*(real(k-1) + 0.5)

             f0 = 0.125*(1.0 - ax*cos(kx*x)) * (1.0 - ay*cos(ky*y)) * (1.0 - az*cos(kz*z))

             ferr = ferr + (f(i,j,k) - f0)*(f(i,j,k) - f0);
          end do
       end do
    end do

    accuracy = sqrt(ferr/dble(nx*ny*nz))

  end function accuracy

end module diffusion


    program demo_anint
    use, intrinsic :: iso_fortran_env, only : real_kinds, &
    & real32, real64, real128
    implicit none
    real(kind=real32) :: x4
    real(kind=real64) :: x8

       x4 = 1.234E0_real32
       x8 = 4.321_real64
       print *, anint(x4), dnint(x8)
       x8 = anint(x4,kind=real64)
       print *, x8
       print *
       ! elemental
       print *,anint([ &
        & -2.7,  -2.5, -2.2, -2.0, -1.5, -1.0, -0.5, &
        &  0.0, &
        & +0.5,  +1.0, +1.5, +2.0, +2.2, +2.5, +2.7  ])

    end program demo_anint


program problem_13

    use aoc_utilities

    implicit none

    logical :: status_ok, read_grid
    integer :: iunit, istep, i, j, k, n_lines, n_nodes, x, y, maxy, maxx
    character(len=:),allocatable :: line
    integer,dimension(:),allocatable :: xvec, yvec, foldxy
    integer,dimension(:,:),allocatable :: grid
    type(string),dimension(:),allocatable :: vals
    character(len=1),dimension(:),allocatable :: fold
    character(len=1) :: f

    open(newunit=iunit,file='inputs/day13.txt', status='OLD')
    n_lines = number_of_lines_in_file(iunit)

    read_grid = .true.
    allocate(xvec(0)); allocate(yvec(0))
    allocate(foldxy(0))
    do i = 1, n_lines
        call read_line_from_file(iunit,line,status_ok)
        if (line=='') then
            read_grid = .false.
            cycle
        end if
        if (read_grid) then
            ! read grid points
            call split(line,',',vals)
            x = vals(1)%to_int()
            y = vals(2)%to_int()
            xvec = [xvec, x]
            yvec = [yvec, y]
        else
            ! read folds
            call split(line(12:),'=',vals)
            f = vals(1)%str
            if (allocated(fold)) then ! for some reason we have to do this with gfortran
                fold = [fold, f]
            else
                fold = [f]
            end if
            foldxy = [foldxy, vals(2)%to_int()]
        end if
    end do

    allocate( grid(0:maxval(yvec), 0:maxval(xvec)) )
    grid = 0
    do i = 1, size(xvec)
        grid( yvec(i), xvec(i) ) = 1
    end do
    maxy = ubound(grid,1) ! initialize : will be updated for each fold
    maxx = ubound(grid,2)

    !   2
    ! 00|000  -> x
    ! 11|111
    ! 22|221
    !y

    ! or :

    !    3
    ! 000|00  -> x
    ! 111|11
    ! 222|21
    !y

    do i = 1, size(fold)
        !write(*,*) 'fold along ', fold(i), foldxy(i)
        select case (fold(i))
        case('x')
            do x = 1, min(foldxy(i), maxx-foldxy(i))
                do y = 0, maxy
                    if (grid(y,foldxy(i)+x)==1 .or. grid(y, foldxy(i)-x)==1) grid(y,foldxy(i)-x) = 1
                end do
            end do
            maxx = foldxy(i) - 1
        case('y')
            do y = 1, min(foldxy(i), maxy-foldxy(i))
                do x = 0, maxx
                    if (grid(foldxy(i)+y,x)==1 .or. grid(foldxy(i)-y, x)==1) grid(foldxy(i)-y,x) = 1
                end do
            end do
            maxy = foldxy(i) - 1
        end select
        if (i==1) write(*,*) '13a: number visible = ', count(grid(0:maxy,0:maxx)==1)
    end do

    write(*,*) '13b: '
    do k = 0, maxy
        do j = 0, maxx
            if (grid(k,j)==0) then
                write(*,'(A1)',advance='NO') ' '
            else
                write(*,'(A1)',advance='NO') '#'
            end if
        end do
        write(*,*) ''
    end do

end program problem_13


! { dg-do  run }
! { dg-options "-finline-matmul-limit=0" }
! Stress-test the matmul blocking code with sizes close to or
! equal to powers ot two.

program main
  implicit none
  integer, dimension(*), parameter :: nn = &
       & [2,3,4,5, 7,8,9, 15,16,17, 31,32,33, 63,64,65, &
       127 ,228,129,  255,256,257];
  integer, parameter :: s = size(nn)
  real, dimension(:,:),allocatable :: a, b, c
  integer :: i1, i2, i3
  integer :: nx, ny, count
  real :: sm

  sm = 0.0
  do i1=1, s
     nx = nn(i1)
     do i2=1,s
        ny = nn(i2)
        do i3=1,s
           count = nn(i3)
           allocate (a(nx,ny), b(ny,count), c(nx,count))
           call random_number(a)
           call random_number(b)
           c = matmul(a,b)
           sm = sm + sum(c)
           deallocate(a,b,c)
        end do
     end do
  end do

end program main


! main program for Fortran 2018 RPN calculator

!---- (historical notes) -----------
!  Programmer:   David G. Simpson
!                NASA Goddard Space Flight Center
!                Greenbelt, Maryland  20771
!  Date:         December 28, 2005
!-----------------------------------

PROGRAM RPN
use, intrinsic:: iso_fortran_env, only: stdout=>output_unit, stdin=>input_unit
USE GLOBAL
use funcs, only:  isrational, isreal, iscomplex, toUpper
use stackops, only: printx, push_stack
use evals, only: eval
IMPLICIT NONE

real(wp), PARAMETER :: PI = 4._wp * atan(1._wp)
real(wp), PARAMETER :: TWOPI = 2*pi
INTEGER :: IDX, IERR, DEL, PTR, RN, RD
real(wp) :: X
COMPLEX(wp) :: CX
CHARACTER(300) :: LINE, SUBSTR
CHARACTER(100) :: NUMSTR
LOGICAL :: NUM_FLAG = .false.

print *, 'Fortran 2018  RPN Calculator, Version '//VERSION

!     Initialize data.

call init_stack()

DEL = IACHAR('a') - IACHAR('A')                                               ! find ASCII position diff between 'A' and 'a'

STACK = 0._wp                                                                 ! clear the REAL stack
REG = 0._wp                                                                   ! clear the REAL registers
LASTX = 0._wp                                                                 ! clear the REAL LAST X register

NN = 0._wp                                                                    ! clear the REAL summation registers
SUMX = 0._wp
SUMX2 = 0._wp
SUMY = 0._wp
SUMY2 = 0._wp
SUMXY = 0._wp

CSTACK = (0._wp,0._wp)                                                        ! clear the COMPLEX stack
CREG = (0._wp,0._wp)                                                          ! clear the COMPLEX registers
CLASTX = (0._wp,0._wp)                                                        ! clear the COMPLEX LAST X register

CNN = (0._wp,0._wp)                                                           ! clear the COMPLEX summation registers
CSUMX = (0._wp,0._wp)
CSUMX2 = (0._wp,0._wp)
CSUMY = (0._wp,0._wp)
CSUMY2 = (0._wp,0._wp)
CSUMXY = (0._wp,0._wp)

RNSTACK = 0; RDSTACK = 1                                                      ! clear the RATIONAL stack
RNREG = 0; RDREG = 1                                                          ! clear the RATIONAL registers
RNLASTX = 0; RDLASTX = 1                                                      ! clear the RATIONAL LAST X register

RNNN = 0; RDNN = 1                                                            ! clear the RATIONAL summation registers
RNSUMX = 0; RDSUMX = 1
RNSUMX2 = 0; RDSUMX2 = 1
RNSUMY = 0; RDSUMY = 1
RNSUMY2 = 0; RDSUMY2 = 1
RNSUMXY = 0; RDSUMXY = 1

ANGLE_MODE = INITIAL_ANGLE_MODE

SELECT CASE (ANGLE_MODE)
   CASE (1)                                                                   ! deg
      ANGLE_FACTOR = PI/180._wp
   CASE (2)                                                                   ! rad
      ANGLE_FACTOR = 1._wp
   CASE (3)                                                                   ! grad
      ANGLE_FACTOR = PI/200._wp
   CASE (4)                                                                   ! rev
      ANGLE_FACTOR = TWOPI
END SELECT

DISP_MODE = INITIAL_DISP_MODE                                                 ! set modes
DISP_DIGITS = INITIAL_DISP_DIGITS
DOMAIN_MODE = INITIAL_DOMAIN_MODE
BASE_MODE = INITIAL_BASE_MODE
FRACTION_MODE = INITIAL_FRACTION_MODE

FRACTOL = INITIAL_FRACTOL                                                     ! set decimal-to-fraction tolerance

!     call random_init()   ! Fortran 2018 + the following line
CALL RANDOM_SEED()                                                           ! init random number generator

! -----  Main loop.

DO                                                                            ! loop once for each input line
   WRITE(stdout,'(A)', ADVANCE='NO') '  ? '
   READ (stdin,'(A132)', iostat=ierr) LINE
   if (ierr<0) exit  ! Ctrl D was pressed

!     Convert the input line to all uppercase, removing leftmost blanks

   LINE = toUpper(ADJUSTL(LINE))

!     Search for QUIT 'Q'

   IF (TRIM(LINE) == 'Q') exit

   PTR = 1

!     Loop for each element in the input line.

   DO
      IDX = INDEX(LINE(PTR:), ' ') + PTR - 1                                  ! look for the next space..
      IF (IDX .EQ. 0) IDX = LEN(LINE(PTR:))                                   ! ..or end of line
      SUBSTR = LINE(PTR:IDX-1)                                                ! get the current substring

      SELECT CASE (DOMAIN_MODE)
         CASE (1)
            NUM_FLAG = ISREAL(SUBSTR, X)                                     ! convert to a real number, if possible
         CASE (2)
            NUM_FLAG = ISCOMPLEX (SUBSTR, CX)                                 ! convert to a complex number, if possible
         CASE (3)
            NUM_FLAG = ISRATIONAL (SUBSTR, RN, RD)                            ! convert to a rational number, if possible
      END SELECT

      IF (NUM_FLAG) THEN                                                      ! if a number, then put it on the stack
         SELECT CASE (DOMAIN_MODE)
            CASE (1)
               CALL PUSH_STACK (X)                                            ! push real number onto real stack
            CASE (2)
               CALL push_stack(CX)                                          ! push complex number onto complex stack
            CASE (3)
               CALL push_stack(RN, RD)                                      ! push rational number onto rational stack
         END SELECT
      ELSE                                                                    ! else it's an operator
         CALL EVAL (SUBSTR)                                                   ! evaluate operator
      END IF

      PTR = IDX + 1                                                           ! update line pointer
      IF (LEN_TRIM(LINE(PTR:)) .EQ. 0) EXIT                                   ! exit if at end of line
   END DO

!     Print X register.

   SELECT CASE (DOMAIN_MODE)
      CASE (1)
         CALL PRINTX(STACK(1), NUMSTR)                                        ! format REAL X
      CASE (2)
         CALL PRINTX(CSTACK(1), NUMSTR)                                      ! format COMPLEX X
      CASE (3)
         CALL PRINTX(RNSTACK(1), RDSTACK(1), NUMSTR)                         ! format RATIONAL X
   END SELECT

   print '(3X,A)', TRIM(NUMSTR)                                  ! print X

END DO

! -- end program by printing last value (helping automatic self test cases)
print *,new_line('')
SELECT CASE (DOMAIN_MODE)
CASE (1)
  CALL PRINTX(STACK(1), NUMSTR)
CASE (2)
  CALL PRINTX(CSTACK(1), NUMSTR)
CASE (3)
  CALL PRINTX(RNSTACK(1), RDSTACK(1), NUMSTR)
END SELECT

print '(3X,A)', TRIM(NUMSTR)

END PROGRAM RPN


!->Module LIB_SOLV. Salvatore Maraniello. 04Sep2014
!
!->Description.-
!
!  This module contains tools for the solution of non-linear systems
!
!->Subroutines:
! - residual_check: checks Newtonm iteration convergence via residual
! - error_check: checks convergence making an estimation of the error
! - delta_check: original convergence check, based on the maximum residual value
!   and the maximum incremental value
! - solv_set_vec_rows_zero: (analogous to lib_sparse-sparse_set_rows_to_zero)
!      Given a vector/matrix, sets rows specified in rows_list to zero.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

module lib_solv
    use, intrinsic          :: iso_c_binding
  implicit none

 contains

! subroutine residual_check
! ------------------------------------------------------------------------------
    ! Checks the convergence of a Newton iteration using the criteria
    !
    ! Res < Tr Ro + Ta
    !
    ! where Res and Ro are the current and initial residual. The check is such
    ! that:
    ! 1. if the relative residual Res/Res0 < Tr, the test is always passed
    ! 2. if the relative residual is Res/Res0 > Tr, the tolerance for the
    !    absolute residual is increased of a factor as much higher as closer
    !    the relative residual gets to the relative tolerance Tr.
    !    The test is in fact passed if:
    !    Res < Ta / ( 1 - Tr/(Res/Res0) )
    ! 3. In the event that Res/Res0 ~ 1 (e.g. initial guess very close to
    !    solution) the test is passed if Res < Ta
    ! 4. If v is dimensional, Ta should be a dimensional quantity as well
    !
    !
    ! Remarks:
    ! 1. Res0 is calculated only if Iter = 1. Otherwise the input value is used.
    ! --------------------------------------------------------------------------
  subroutine residual_check(Iter,v,Res,Res0,passed,Tr,Ta,print_info)

    ! input:
    integer, intent(in)           :: Iter        ! iteration number
    real(8), intent(in)           :: v(:)
    ! in/out:
    real(8), intent(inout)        :: Res0        ! Residual at iteration 1.
    ! optional
    real(8),             optional :: Tr, Ta      ! relative and absolute tolerance
    logical(c_bool),             optional :: print_info  ! print update or not
    ! output:
    logical, intent(out)          :: passed      ! outcome of convergence test
    real(8), intent(out)          :: Res ! absolute residual

    ! default values for optional input
    if ( present(Tr).eqv. .false.) then
        Tr = 1e-6
    end if
    if ( present(Ta).eqv. .false.) then
        Ta = 1e-6
    end if
    if ( present(print_info).eqv. .false.) then
        print_info = .true.
    end if

    ! current residual
    Res = vector_norm(v)      ! sqrt( dot_product(v,v)/dble(NumDof) )

    ! initial residual: shared variable
    ! The .le. condition allows to recompute Res0 for quantities for which
    ! the initial Ro may be too close to zero.
    ! if (Iter .eq. 1) then
    if (Iter .eq. 1) then
        Res0 = Res ! sqrt( dot_product(v,v)/dble(NumDof) )
    end if

    ! check convergence
    passed = .false.
    if ( Res < Tr*Res0 + Ta ) then
        passed = .true.
    end if

    !if (print_info .eqv. .true.) write (*,'(2X,A,1PE10.3,A,1PE10.3,$)') &
    !&                           'Res=',Res, ' ResRel=',ResRel
    ! if (print_info .eqv. .true.) write (*,'(2X,1PE10.3,2X,1PE10.3,$)') Res, Res/Res0

  end subroutine residual_check

! subroutine error_check
! ------------------------------------------------------------------------------
    ! Check the convergence of a Newton iteration using an approximation of the
    ! error. The check is
    ! passed if:
    !
    ! E_i < Ta
    !
    ! where E is an approximation of the error at the i-th iteration and Ta is
    ! the tolerance level (note that the eq. above is dimensional, therefore Ta
    ! is an absolute tolerance). The error is approximated as:
    !
    ! E_i ~ |dx_{i-1}|**2 / |dx_i|
    !
    ! where:
    ! dx_i: is the delta step in the solution x at iteriation i
    ! |.| : norm computed as per vector_norm function
    !
    ! Remarks:
    ! 1. The approximation holds for superlinear methods only.
    ! 2. The methods passes in output the norm dx_i and required in input dx_{i-1}
    ! 3. The first check is possible only at the 2nd iteration
    !
    ! --------------------------------------------------------------------------
    subroutine error_check(Iter,dx,DX_old,DX_now,Er,passed,Ta,print_info)

    ! input:
    integer, intent(in)   :: Iter        ! iteration number
    real(8), intent(in)   :: dx(:)       ! solution delta
    real(8), intent(in)   :: DX_old      ! norm of dx at previous iteration
    ! optional
    real(8), optional :: Ta               ! tolerance level
    logical(c_bool), optional :: print_info      ! print update or not
    ! output:
    real(8), intent(out)   :: DX_now     ! norm of dx at current iteration
    real(8), intent(out)   :: Er         ! Error at current iteration
    logical, intent(out)   :: passed     ! outcome of convergence test

    ! default values for optional input
    if ( present(Ta).eqv. .false.) then
        Ta = 1e-9
    end if
    if ( present(print_info).eqv. .false.) then
        print_info = .true.
    end if

    ! vector norm
    DX_now = vector_norm(dx)

    ! check convergence
    passed = .false.
    if (Iter > 1) then
        Er = DX_old**2 / DX_now
        if ( Er < Ta ) then
            passed = .true.
        end if
        ! if (print_info .eqv. .true.) write (*,'(2X,1PE10.3,$)') Er
    else
        ! if (print_info .eqv. .true.) write (*,'(2X,9X,$)')
    end if

    end subroutine error_check

! subroutine delta_check
! ------------------------------------------------------------------------------
    ! Original convergence check used for non-linear solvers of cbem3_solv.
    ! The chekc is passed if:
    !
    ! delta = max(abs(r)) + max(abs(dx)) < T
    !
    ! where ideally:
    ! 1. r and dx are residual and delta x vectors (where x is the  solution).
    !    Ideally, r_vec and dx_vec would be non-dimensional, but the original
    !    check does not take care of that.
    ! 2. T tolerance level
    ! --------------------------------------------------------------------------
  subroutine delta_check(r,dx,delta,passed,T,print_info)

    ! input
    real(8), intent(in)  :: r(:), dx(:) ! residual and delta solution vectors
    real(8), optional    :: T           ! tolerance
    logical(c_bool), optional    :: print_info  ! print on screenupdate or not
    ! output
    logical, intent(out) :: passed
    real(8), intent(out) :: delta

    ! default values for optional input
    if ( present(T).eqv. .false.) then
        T = 1e-5
    end if
    if ( present(print_info).eqv. .false.) then
        print_info = .true.
    end if

    ! check convergence
    passed = .false.
    delta = maxval(abs(r))+maxval(abs(dx))
    if (  delta < T ) then
        passed = .true.
    end if

    !if (print_info .eqv. .true.) write (*,'(2X,A,1PE10.3,A,1PE10.3,$)') &
    ! &                           'DeltaF=',maxval(abs(Qglobal)), ' DeltaX=',maxval(abs(DeltaX))
    ! if (print_info .eqv. .true.) write (*,'(2X,1PE10.3,2X,1PE10.3,$)') maxval(abs(r)), maxval(abs(dx))

  end subroutine delta_check

! function vector_norm
! ------------------------------------------------------------------------------
  ! Computes the vector norm n such that:
  ! n**2 = int_0^1{v**2}dx
  ! ----------------------------------------------------------------------------
  function vector_norm(v)

    ! input:
    real(8), intent(in)  :: v(:)  ! vector
    ! output:
    real(8) :: vector_norm        ! norm of the vector
    ! internal
    integer  :: N                 ! size of v

    N=size(v)
    vector_norm = sqrt( dot_product(v,v)/dble(N) )

  end function vector_norm

! subroutine separate_dofs(v,avec,bvec,va,vb)
! ------------------------------------------------------------------------------
  ! Given a vector v, the routine separates the elements of v into 2 different
  ! arrays, va and vb. avec and bvec are arrays of arbitrary length and such
  ! that:
  ! va( nn*(len(avec)-1) + avec(ii) ) = v( nn*(len(avec)+len(bvec))-1 + avec(ii))
  ! with nn = 1 .... len(v)/( len(avec)+len(bvec) )
  !
  ! e.g.
  ! v: displacement solution
  ! va: solution related to the translational dof
  ! vb: solution related to rotational dof
  ! avec: (/1,2,3/) - given the dof associated to the node nn, the translational dof are the first 3
  ! bvec: (/4,5,6/) - given the dof associated to the node nn, the rotational dof are the last 3
  !
  ! remark:
  ! - output vectors va and vb need to be allocated outside the subroutine
  ! ----------------------------------------------------------------------------
  subroutine separate_dofs(v,avec,bvec,va,vb)

    ! input
    real(8), intent(in) :: v(:)             ! input vector
    integer, intent(in) :: avec(:), bvec(:) ! positions vectors
    ! output
    real(8), intent(inout) :: va(:), vb(:)
    ! internal
    integer :: Na, Nb, Ntot ! length of avec, bvec and repetition pattern in v
    integer :: kk, ii       ! counters

    Na = size(avec)
    Nb = size(bvec)
    Ntot = Na + Nb

     kk=0
     do while (kk < size(v)/Ntot) ! Divide Displ from Rotations

        do ii=1,Na
            va(Na*kk+ii)=v(Ntot*kk+avec(ii))
        end do

        do ii=1,Nb
            vb(Nb*kk+ii)=v(Ntot*kk+bvec(ii))
        end do

        kk = kk+1
     end do

  end subroutine separate_dofs

! subroutine solv_set_vec_rows_zero
! ------------------------------------------------------------------------------
  ! Given a vector v, the routine sets to zero the values on the rows specified
  ! in rows_list
  !-----------------------------------------------------------------------------

 subroutine solv_set_vec_rows_zero (rows_list,v)

  integer,     intent(in)   :: rows_list(:)! rows for which all entries of sparse matrix will be set to zero
  real(8),     intent(inout) :: v(:)

  integer:: nn, rr

  do nn=1,size(rows_list)
    rr = rows_list(nn)
    v(rr)=0.d0
  end do

  return
 end subroutine solv_set_vec_rows_zero

! subroutine print_mat
! ------------------------------------------------------------------------------
  ! Print entried of rank 2 array (for debugging).
  !-----------------------------------------------------------------------------

 subroutine print_mat(mat,Imax,Jmax)
  real(8), intent(in) :: mat(:,:)
  integer :: ii, jj, Imax, Jmax

  do ii=1,Imax
    do jj=1,Jmax
    !   write (*,'(2X,F5.2,$)') mat(ii,jj)
    end do
    print *, ' '
  end do

 end subroutine print_mat

end module lib_solv


!*==isamax.f90  processed by SPAG 7.51RB at 20:37 on  3 Mar 2022
!> \brief \b ISAMAX
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!  Definition:
!  ===========
!
!       INTEGER FUNCTION ISAMAX(N,SX,INCX)
!
!       .. Scalar Arguments ..
!       INTEGER INCX,N
!       ..
!       .. Array Arguments ..
!       REAL SX(*)
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!>    ISAMAX finds the index of the first element having maximum absolute value.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>         number of elements in input vector(s)
!> \endverbatim
!>
!> \param[in] SX
!> \verbatim
!>          SX is REAL array, dimension ( 1 + ( N - 1 )*abs( INCX ) )
!> \endverbatim
!>
!> \param[in] INCX
!> \verbatim
!>          INCX is INTEGER
!>         storage spacing between elements of SX
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date November 2017
!
!> \ingroup aux_blas
!
!> \par Further Details:
!  =====================
!>
!> \verbatim
!>
!>     jack dongarra, linpack, 3/11/78.
!>     modified 3/93 to return if incx .le. 0.
!>     modified 12/3/93, array(1) declarations changed to array(*)
!> \endverbatim
!>
!  =====================================================================
      INTEGER FUNCTION ISAMAX(N,Sx,Incx)
      IMPLICIT NONE
!*--ISAMAX75
!
!  -- Reference BLAS level1 routine (version 3.8.0) --
!  -- Reference BLAS is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     November 2017
!
!     .. Scalar Arguments ..
      INTEGER Incx , N
!     ..
!     .. Array Arguments ..
      REAL Sx(*)
!     ..
!
!  =====================================================================
!
!     .. Local Scalars ..
      REAL smax
      INTEGER i , ix
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC ABS
!     ..
      ISAMAX = 0
      IF ( N<1 .OR. Incx<=0 ) RETURN
      ISAMAX = 1
      IF ( N==1 ) RETURN
      IF ( Incx==1 ) THEN
!
!        code for increment equal to 1
!
         smax = ABS(Sx(1))
         DO i = 2 , N
            IF ( ABS(Sx(i))>smax ) THEN
               ISAMAX = i
               smax = ABS(Sx(i))
            ENDIF
         ENDDO
      ELSE
!
!        code for increment not equal to 1
!
         ix = 1
         smax = ABS(Sx(1))
         ix = ix + Incx
         DO i = 2 , N
            IF ( ABS(Sx(ix))>smax ) THEN
               ISAMAX = i
               smax = ABS(Sx(ix))
            ENDIF
            ix = ix + Incx
         ENDDO
      ENDIF
      END FUNCTION ISAMAX


!---------------------------------------------------!
! Copyright (c) 2017 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
module global_variables
  implicit none
! Mathematical parameters
!  real(8),parameter :: pi=3.14159265358979323846d0
  real(8),parameter :: pi=4d0*atan(1d0)
  complex(8),parameter :: zI=(0d0,1d0)

! Control parameter
  character(8) :: calc_mode

! Parameters of model
!! Harmonic oscillator
  real(8) :: Ip_HO = 24.6d0/27.211d0
  real(8) :: alpha_HO, beta_HO
!! Jellium model
  real(8) :: K_JL, C_JL, Q_JL, R0_JL

! Multi-trajectory
  integer :: Ntraj
  real(8) :: x_n(3),p_n(3)
  real(8),allocatable :: x_l(:,:),p_l(:,:),x_ave(:,:),p_ave(:,:)

! Time propagation
  integer :: Nt
  real(8) :: dt, Tprop

! Laser parameters
  real(8) :: Up,E0,omega0,Tpulse
  real(8),allocatable :: Et(:,:)

! MPI
  include 'mpif.h'
  integer :: Myrank,Nprocs,ierr

end module global_variables


subroutine navier_stokes()
!updates the vorticity according to the vorticity transport equation:
!
! dw/dt + u dw/dx + v dw/dy = nabla^2 w
!
! The maximum velocity (that the vorticity is advected by) is capped to 1
!
!Also the maximum vorticity is capped at maxvort

    use vars
    use parallel

    implicit none

    integer :: i, j
    real :: uu, vv, v2
    real :: r
    real :: vmax

    real :: deltax, deltay
    real :: dwdx, dwdy

    vmax=1.
    vmax=vmax*vmax

    !$OMP DO PRIVATE(uu,vv,v2,r,i, deltax, deltay, dwdx, dwdy)
    do j=1,ny
        do i=1,nx
            if (mask(i,j) .eq. 0) then
                !advection

                uu=u(i,j)
                vv=v(i,j)
                v2=uu*uu+vv*vv

                if (v2 .gt. vmax) then
                    v2 = sqrt(vmax/v2)
                    uu=uu*v2
                    vv=vv*v2
                endif

                !Time depenent vorticity evolution: (dw/dt = -u*dw/dx -v*dw/dy + 1/Re * (laplacian(w))

                ! weight the derivative according to the velocity (stronger weight is given to the
                ! derivitive that is being advected into the gridcell)
                deltax = uu*dt/dx
                dwdx = (1-deltax)*(vort(i+1,j)-vort(i,j)) + (1+deltax)*(vort(i,j)-vort(i-1,j))

                deltay = vv*dt/dy
                dwdy = (1-deltay)*(vort(i,j+1)-vort(i,j)) + (1+deltay)*(vort(i,j)-vort(i,j-1))

                dw(i,j) = 0.5*(-uu*dwdx - vv*dwdy)

    !             dw(i,j) = -uu*(vort(i+1,j)-vort(i-1,j))/2./dx - vv*(vort(i,j+1)-vort(i,j-1))/2./dy

                !diffusion

                r=r0

                dw(i,j)= dw(i,j) + 1./r *( (vort(i+1,j) - 2.*vort(i,j) + vort(i-1,j) )/dx/dx &
                    + (vort(i,j+1) - 2.*vort(i,j) + vort(i,j-1))/dy/dy )

            endif
        enddo
    enddo

    !$OMP END DO

    !$OMP DO PRIVATE(j)
    do j=1,ny
        vort(:,j)=vort(:,j) + dw(:,j)*dt
    enddo
    !$OMP END DO

    !limit maximum vorticity
    !$OMP DO PRIVATE(i,j)
    do j=1,ny
        do i=1,nx
            if (vort(i,j) .gt. maxvort) vort(i,j) = maxvort
            if (vort(i,j) .lt. -maxvort) vort(i,j) = -maxvort
        enddo
    enddo
    !$OMP END DO

    !$OMP SINGLE
    call haloswap(vort)
    !$OMP END SINGLE

end subroutine


      SUBROUTINE DTR312(A1,A2,A3,C)
C
      IMPLICIT REAL*8(A-H,O-Z)
C
      DIMENSION C(3,3)
C
      CGAM=DCOS(A1)
      SGAM=DSIN(A1)
      CALP=DCOS(A2)
      SALP=DSIN(A2)
      CBET=DCOS(A3)
      SBET=DSIN(A3)
C
      SASB=SALP*SBET
      SACB=SALP*CBET
C
      C(1,1)=CBET*CGAM-SASB*SGAM
      C(2,1)=CBET*SGAM+SASB*CGAM
      C(3,1)=-CALP*SBET
      C(1,2)=-CALP*SGAM
      C(2,2)=CALP*CGAM
      C(3,2)=SALP
      C(1,3)=SBET*CGAM+SACB*SGAM
      C(2,3)=SBET*SGAM-SACB*CGAM
      C(3,3)=CALP*CBET
C
      RETURN
C
      END


      SUBROUTINE dsvbksb(u,w,v,m,n,mp,np,b,x)
      INTEGER m,mp,n,np,NMAX
      DOUBLE PRECISION b(mp),u(mp,np),v(np,np),w(np),x(np)
      PARAMETER (NMAX=500)
      INTEGER i,j,jj
      DOUBLE PRECISION s,tmp(NMAX)
      do 12 j=1,n
        s=0.0d0
        if(w(j).ne.0.0d0)then
          do 11 i=1,m
            s=s+u(i,j)*b(i)
11        continue
          s=s/w(j)
        endif
        tmp(j)=s
12    continue
      do 14 j=1,n
        s=0.0d0
        do 13 jj=1,n
          s=s+v(j,jj)*tmp(jj)
13      continue
        x(j)=s
14    continue
      return
      END

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


C     LAST UPDATE 16/10/96
C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
C
      INTEGER FUNCTION NMIN(L,U,T,NSTRT,NSTCK,NMAX)
      IMPLICIT NONE
C
C Purpose: Find the minimum bessel function order (unsigned) which
C          will contribute to a layer line.
C
C Calls   0:
C Called by:
C
C-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-
C Arguments:
C
      INTEGER L,U,T,NSTRT,NSTCK,NMAX
C
C Local variables:
C
      INTEGER MMAX,M,N
C
C-----------------------------------------------------------------------
      IF(MOD(NSTCK*L,U).EQ.0)THEN
         NMIN = 0
         RETURN
      ENDIF
      NMIN = NMAX + 1
      MMAX = (ABS(NSTCK*L)+ABS(T)*NMAX)/ABS(U) + 1
      DO 10 M=-MMAX,MMAX
         N = ABS(L)*NSTCK - ABS(U)*M
         IF(MOD(N,T).EQ.0)THEN
            N = ABS(N/T)
            IF(MOD(N,NSTRT).EQ.0)NMIN = MIN(NMIN,N)
         ENDIF
 10   CONTINUE
      IF(NMIN.GT.NMAX)NMIN = -1
      RETURN
      END


module TRANSFORM
! Description:
!    Contains subroutines to transform between the state vectors of the
!    regularized formulations and the position and velocity in Cartesian
!    coordinates.
!
! References:
! [1] Stiefel E.L., and Scheifele G., "Linear and Regular Celestial Mechanics",
!     Springer-Verlag Berlin Heidelberg New York, Berlin, Germany. 1971.
! [2] Baù G., Bombardelli C., Peláez J., and Lorenzini E., "Non-singular orbital
!     elements for special perturbations in the two-body problem", MNRAS 454,
!     pp. 2890-2908. 2015.
! [3] Baù G., Amato D., Bombardelli C., and Milani A., "New orbital elements for
!     accurate propagation in the Solar System", Proceedings of the 6th
!     International Conference on Astrodynamics Tools and Techniques, European
!     Space Agency, Darmstadt, Germany. March 2016.
! [4] Roa J., and Peláez J., "Orbit propagation in Minkowskian geometry", CMDA
!     123(1), pp. 13-43. 2015.
!
! Author:
!    Davide Amato
!    Space Dynamics Group - Technical University of Madrid
!    d.amato@upm.es
!
! ==============================================================================

use KINDS, only: ik,dk,qk
implicit none

contains

function DKSMAT(u)
! Description:
!    Computes the KS-matrix from the K-S state vector.
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments
real(dk),intent(in)  ::  u(:)                ! K-S state vector
real(dk)             ::  DKSMAT(1:4,1:4)     ! K-S-matrix

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

DKSMAT = RESHAPE([ u(1),  u(2),  u(3),  u(4)&
                &,-u(2),  u(1),  u(4), -u(3)&
                &,-u(3), -u(4),  u(1),  u(2)&
                &, u(4), -u(3),  u(2), -u(1)],[4,4])

end function DKSMAT

subroutine DKS2CART(u,x,xdot)
! Description:
!    Computes the Cartesian state (dimensionless) from the KS state vector.
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments
real(dk),intent(in)   ::  u(:)                  ! KS (extended) state vector
real(dk),intent(out)  ::  x(:),xdot(:)          ! Position and velocity, ND
! Locals
real(dk)              ::  r                     ! Radius magnitude, ND

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

! Position
x(1) = u(1)**2_ik - u(2)**2_ik - u(3)**2_ik + u(4)**2_ik
x(2) = 2._dk*(u(1)*u(2) - u(3)*u(4))
x(3) = 2._dk*(u(1)*u(3) + u(2)*u(4))
r    = u(1)**2_ik + u(2)**2_ik + u(3)**2_ik + u(4)**2_ik

! Velocity
xdot(1) = 2._dk*(u(1)*u(5) - u(2)*u(6) - u(3)*u(7) + u(4)*u(8))/r
xdot(2) = 2._dk*(u(2)*u(5) + u(1)*u(6) - u(4)*u(7) - u(3)*u(8))/r
xdot(3) = 2._dk*(u(3)*u(5) + u(4)*u(6) + u(1)*u(7) + u(2)*u(8))/r

! If x, xdot are in R^4 their fourth component is null.
if (size(x,1) > 3)    x(4:) = 0._dk
if (size(xdot,1) > 3) xdot(4:) = 0._dk

end subroutine DKS2CART

function DTO_ENERGY(z)
! Description:
!    Transforms the third element of the Dromo state vector from the inverse of the specific
!    angular momentum to the total energy.
!
! ==============================================================================

! VARIABLES
implicit none
! Function definition
real(dk)              ::  DTO_ENERGY(1:8)
! Arguments
real(dk),intent(in)   ::  z(1:8)

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

DTO_ENERGY(:) = z(:)
DTO_ENERGY(3) = .5_dk*(z(1)**2_ik + z(2)**2_ik - z(3)**2_ik)
end function DTO_ENERGY

function DTO_MOMENTUM(z)
! Description:
!    Transforms the third element of the Dromo state vector from the total energy
!    to the inverse of the specific angular momentum.
!
! ==============================================================================

! VARIABLES
implicit none
! Function definition
real(dk)             ::  DTO_MOMENTUM(1:8)
! Arguments
real(dk),intent(in)  ::  z(1:8)

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

DTO_MOMENTUM(:) = z(:)
DTO_MOMENTUM(3) = sqrt(z(1)**2_ik + z(2)**2_ik - 2._dk*z(3))
end function DTO_MOMENTUM

subroutine DDROMO2CART(z,phi,phi0,RV,VV,pot,propagateEnergy)
! Description:
!    Transforms from Dromo state vector to Cartesian coordinates. Optionally, it
!    takes into account the potential deriving from Earth's oblateness.
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments IN
real(dk),intent(in)   ::  z(1:8)            ! Dromo(P) state vector
real(dk),intent(in)   ::  phi               ! Dromo independent variable
real(dk),intent(in)   ::  phi0              ! Dromo independent variable - initial value
logical,intent(in)    ::  propagateEnergy   ! .TRUE. if z(3) is the total energy.
real(dk),intent(in)   ::  pot               ! Potential
! Arguments OUT
real(dk),intent(out)  ::  RV(1:3), VV(1:3)  ! Cartesian position and velocity.
! Locals
real(dk)              ::  r, u, s, lam, sdp, cdp
real(dk)              ::  z3               ! Inverse of specific angular momentum.

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

! ==============================================================================
! 01. TOTAL ENERGY / SPECIFIC ANGULAR MOMENTUM
! ==============================================================================

if (propagateEnergy) then
    z3 = sqrt(z(1)**2_ik + z(2)**2_ik - 2._dk*z(3))
else
    z3 = z(3)
end if

! ==============================================================================
! 02. COMPUTATION OF AUXILIARIES
! ==============================================================================

sdp = sin(phi-phi0); cdp = cos(phi-phi0)
s = z3 + z(1)*cos(phi) + z(2)*sin(phi)
r = 1._dk/(z3*s)
u = z(1)*sin(phi) - z(2)*cos(phi)

! ==============================================================================
! 03. COMPUTATION OF STATE
! ==============================================================================

lam = sqrt(s**2_ik - 2._dk*pot)

RV(1) =    r*( ( 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik )*cdp &
&+ 2._dk*( z(4)*z(5) - z(6)*z(7) )*sdp )
RV(2) =    r*( ( 1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik )*sdp &
&+ 2._dk*( z(4)*z(5) + z(6)*z(7) )*cdp )
RV(3) = 2._dk*r*( ( z(5)*z(6) + z(4)*z(7) )*sdp + ( z(4)*z(6) - z(5)*z(7) )*cdp )

VV(1) = ( 2._dk*lam*( z(4)*z(5) - z(6)*z(7) ) + u*( 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik ) )*cdp + &
      & ( 2._dk*u*( z(4)*z(5) - z(6)*z(7) ) -&
      & lam*( 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik ) )*sdp
VV(2) = ( lam*( 1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik ) + 2._dk*u*( z(4)*z(5) + z(6)*z(7) ) )*cdp + &
      & ( u*( 1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik ) -&
      & 2._dk*lam*( z(4)*z(5) + z(6)*z(7) ) )*sdp
VV(3) = 2._dk*( lam*( z(5)*z(6) + z(4)*z(7) ) + u*( z(4)*z(6) - z(5)*z(7) ) )*cdp + &
      & 2._dk*( u*( z(5)*z(6) + z(4)*z(7) ) - lam*( z(4)*z(6) - z(5)*z(7) ) )*sdp
end subroutine DDROMO2CART

function DINERT2ORB(vI,z,phi,phi0)
! Description:
!    Transforms a vector vI from orbital to inertial axes through a rotation
!    matrix obtained from Dromo(P) elements.
!
! ==============================================================================

! VARIABLES
implicit none

! Arguments IN
real(dk),intent(in)          ::  vI(1:3)
real(dk),intent(in)          ::  z(:)
real(dk),intent(in)          ::  phi, phi0
! Function definition
real(dk)                     ::  DINERT2ORB(1:3)
! Locals
real(dk)                     ::  Q0T(1:3,1:3), MphiT(1:3,1:3), QIR(1:3,1:3)
real(dk)                     ::  dphi

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

dphi  = phi - phi0

! Compute rotation matrix QIR (Inertial -> Orbital)
MphiT = reshape( (/ cos(dphi), -sin(dphi), 0._dk, sin(dphi), cos(dphi), 0._dk,&
& 0._dk, 0._dk, 1._dk /), (/ 3, 3 /) )
Q0T   = reshape( (/ 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik,&
& 2._dk*z(4)*z(5) - 2._dk*z(6)*z(7), 2._dk*z(4)*z(6) + 2._dk*z(5)*z(7),&
& 2._dk*z(4)*z(5) + 2._dk*z(6)*z(7),&
& 1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik, 2._dk*z(5)*z(6) - 2._dk*z(4)*z(7),&
& 2._dk*z(4)*z(6) - 2._dk*z(5)*z(7), 2._dk*z(5)*z(6) + 2._dk*z(4)*z(7), 1._dk -&
& 2._dk*z(4)**2_ik - 2._dk*z(5)**2_ik /),&
& (/ 3, 3 /) )
QIR   = matmul(MphiT, Q0T)

! Rotate VI into orbital axes
DINERT2ORB = matmul(QIR,vI)
end function DINERT2ORB

function DINERT2ORB_EDROMO(vI,z,cnu,snu)
! Description:
!    Transforms a vector vI from inertial to orbital axes through a rotation
!    matrix obtained from EDromo(P) elements.
!
! ==============================================================================

! VARIABLES
implicit none

! Arguments
real(dk),intent(in)  ::  vI(1:3)
real(dk),intent(in)  ::  z(:)
real(dk),intent(in)  ::  cnu,snu
real(dk)             ::  DINERT2ORB_EDROMO(1:3)
! Locals
real(dk)         ::  R1(1:3,1:3)    ! Inertial -> Intermediate rotation matrix
real(dk)         ::  R2(1:3,1:3)    ! Intermediate -> Orbital rotation matrix
real(dk)         ::  RTOT(1:3,1:3)  ! Inertial -> Orbital rotation matrix (= R2*R1)
! Auxiliary variables
real(dk)  ::  z42,z52,z62,z4z5,z6z7,z4z6,z5z7,z5z6,z4z7

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

z42 = z(4)**2
z52 = z(5)**2
z62 = z(6)**2
z4z5 = z(4)*z(5)
z6z7 = z(6)*z(7)
z4z6 = z(4)*z(6)
z5z7 = z(5)*z(7)
z5z6 = z(5)*z(6)
z4z7 = z(4)*z(7)

R1 = 2._dk*reshape([ .5_dk - z52 - z62, z4z5 + z6z7, z4z6 - z5z7,&
            &  z4z5 - z6z7, .5_dk - z42 - z62, z5z6 + z4z7, &
            &  z4z6 + z5z7, z5z6 - z4z7, .5_dk - z42 - z52 ], [3_ik,3_ik])
R1 = transpose(R1)
R2    = reshape([ cnu, -snu, 0._dk,&
              &   snu,  cnu, 0._dk,&
              &   0._dk,    0._dk, 1._dk ],[3_ik,3_ik])
RTOT = matmul(R2,R1)

DINERT2ORB_EDROMO = matmul(RTOT,vI)

end function DINERT2ORB_EDROMO

subroutine DEDROMO2CART(z,phi,Upot,r_vec,v_vec)
! Description:
!    Transforms from the EDromo state vector "z", fictitious time "phi" and
!    potential "Upot" to Cartesian position and velocity "r_vec", "v_vec".
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments IN
real(dk),intent(in)   ::  z(1:8),phi,Upot
! Arguments OUT
real(dk),intent(out)  ::  r_vec(1:3),v_vec(1:3)

! Auxiliaries
real(dk)    ::  sph,cph
real(dk)    ::  rho,rmag,zeta,emme
real(dk)    ::  cnu,snu
real(dk)    ::  x_vec(1:3),y_vec(1:3)
real(dk)    ::  i_vec(1:3),j_vec(1:3)
real(dk)    ::  v_rad,v_tan

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

! ==============================================================================
! 01. COMPUTE AUXILIARY QUANTITIES
! ==============================================================================

! Store trig functions
sph = sin(phi)
cph = cos(phi)

rho   = 1._dk - z(1)*cph - z(2)*sph
rmag  = z(3)*rho
zeta  = z(1)*sph - z(2)*cph
emme  = sqrt(1._dk - z(1)**2_ik - z(2)**2_ik)

cnu = (cph - z(1) + (zeta*z(2))/(emme + 1._dk))/rho
snu = (sph - z(2) - (zeta*z(1))/(emme + 1._dk))/rho

! ==============================================================================
! 02. COMPUTE POSITION IN INERTIAL FRAME
! ==============================================================================

! Intermediate frame unit vectors
x_vec = 2._dk*[ .5_dk - z(5)**2_ik - z(6)**2_ik,  &
           &  z(4)*z(5) + z(6)*z(7),  &
           &  z(4)*z(6) - z(5)*z(7)]
y_vec = 2._dk*[  z(4)*z(5) - z(6)*z(7),  &
           & .5_dk - z(4)**2_ik - z(6)**2_ik,  &
           &  z(5)*z(6) + z(4)*z(7)]

! Position in inertial frame
r_vec = rmag*(x_vec*cnu + y_vec*snu)

! ==============================================================================
! 03. COMPUTE VELOCITY IN INERTIAL FRAME
! ==============================================================================

! Radial and tangential unit vectors in the inertial frame
i_vec = x_vec*cnu + y_vec*snu
j_vec = y_vec*cnu - x_vec*snu

! Radial and tangential components
v_rad = zeta/(sqrt(z(3))*rho)
v_tan = sqrt((1._dk - z(1)**2_ik - z(2)**2_ik)/(z(3)*rho**2_ik) - 2._dk*Upot)

! Velocity in the inertial frame
v_vec = v_rad*i_vec + v_tan*j_vec

end subroutine DEDROMO2CART

function DEDROMO_TE2TIME(z,phi,flag_time)
! Description:
!    Gets the value of physical time from the EDromo state vector "z" and
!    fictitious time "phi".
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments IN
real(dk),intent(in)     ::  z(1:8),phi
integer(ik),intent(in)  ::  flag_time
! Function definition
real(dk)                ::  DEDROMO_TE2TIME

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

if ( flag_time == 0_ik ) then
    ! Physical time
    DEDROMO_TE2TIME = z(8)
else if  ( flag_time == 1_ik ) then
    ! Constant time element
    DEDROMO_TE2TIME = z(8) - z(3)**1.5_dk*(z(1)*sin(phi) - z(2)*cos(phi) - phi)
else if  ( flag_time == 2_ik ) then
    ! Linear time element
    DEDROMO_TE2TIME = z(8) - z(3)**1.5_dk*(z(1)*sin(phi) - z(2)*cos(phi))
end if

end function DEDROMO_TE2TIME

function DGDROMO_TE2TIME(z,phi,flag_time)
! Description:
!    Computes value of physical time from the GDromo state vector "z" and
!    fictitious time "phi".
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments IN
real(dk),intent(in)     ::  z(1:8),phi
integer(ik),intent(in)  ::  flag_time       ! Flag indicating the type of time element.
                                            ! = 0: physical time
                                            ! = 1: constant time element
                                            ! = 2: linear time element
! Locals
real(dk)  ::  zeta                          ! Auxiliary variable
! Function definition
real(dk)  ::  DGDROMO_TE2TIME

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

zeta = z(1)*sinh(phi) + z(2)*cosh(phi)

! Time/Time Element
if ( flag_time == 0_ik ) then
	! Physical time
    DGDROMO_TE2TIME = z(8)
else if  ( flag_time == 1_ik ) then
    ! Constant time element
    DGDROMO_TE2TIME = z(8) + z(3)**1.5_dk*(zeta - phi)
else if  ( flag_time == 2_ik ) then
    ! Linear time element
    DGDROMO_TE2TIME = z(8) + z(3)**1.5_dk*zeta
end if

end function DGDROMO_TE2TIME

subroutine DGDROMO2CART(z,phi,pot,rV,vV)
! Description:
!    Computes Cartesian position and velocity from the GDromo state vector "z",
!    independent variable "phi" and potential "pot".
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments IN
real(dk),intent(in)   ::  z(1:8),phi,pot
! Arguments OUT
real(dk),intent(out)  ::  rV(1:3),vV(1:3)
! Locals
real(dk)              ::  sph,cph
real(dk)              ::  ecg
real(dk)              ::  gamma,aux1,aux2
real(dk)              ::  rmag,cnu,snu,zeta,rho,emme

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

! Store hyperbolic function
sph = sinh(phi)
cph = cosh(phi)

! Orbital radius
rho = z(1)*cph + z(2)*sph - 1._dk
rmag = z(3)*rho

! Generalized eccentricity
ecg = sqrt(z(1)**2_ik - z(2)**2_ik)

! Compute auxiliary variables
emme  = sqrt(ecg**2_ik-1._dk)
zeta  = z(1)*sph + z(2)*cph
gamma = sqrt(z(1)**2_ik + z(2)**2_ik)
aux1  = emme*zeta/(rho*ecg*gamma)
aux2  = (emme**2_ik/rho - 1._dk)/(ecg*gamma)

! cosine and sine of the angle of rotation nu between the LVLH and the
! intermediate frames
cnu = z(1)*aux2 - z(2)*aux1
snu = z(1)*aux1 + z(2)*aux2

call DGDROMO2CART_CORE(z,rmag,cnu,snu,zeta,rho,emme,rV,vV,pot)

end subroutine DGDROMO2CART

function DINERT2ORB_GDROMO(vI,z,cnu,snu)
! Description:
!    Transforms a vector vI from orbital to inertial axes through a rotation
!    matrix obtained fromo GDromo(P) elements.
!
! ==============================================================================

! VARIABLES
implicit none

! Arguments
real(dk),intent(in)  ::  vI(1:3)
real(dk),intent(in)  ::  z(:)
real(dk),intent(in)  ::  cnu,snu
real(dk)  ::  DINERT2ORB_GDROMO(1:3)
! Locals
real(dk)  ::  Q0T(1:3,1:3), MnuT(1:3,1:3), QIR(1:3,1:3)

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

! Compute rotation matrix QIR (Inertial -> Orbital)
MnuT = reshape( (/ cnu, -snu, 0._dk,&
                 & snu,  cnu, 0._dk,&
                 & 0._dk, 0._dk, 1._dk /), (/ 3_ik, 3_ik /) )
Q0T   = reshape( (/ 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik,&
                &   2._dk*z(4)*z(5) - 2._dk*z(6)*z(7),&
                &   2._dk*z(4)*z(6) + 2._dk*z(5)*z(7),&
                &   2._dk*z(4)*z(5) + 2._dk*z(6)*z(7),&
                &   1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik,&
                &   2._dk*z(5)*z(6) - 2._dk*z(4)*z(7),&
                &   2._dk*z(4)*z(6) - 2._dk*z(5)*z(7),&
                &   2._dk*z(5)*z(6) + 2._dk*z(4)*z(7),&
                &   1._dk - 2._dk*z(4)**2_ik - 2._dk*z(5)**2_ik /), &
                & (/ 3_ik, 3_ik /) )
QIR   = matmul(MnuT, Q0T)

! Rotate vI into orbital axes
DINERT2ORB_GDROMO = matmul(QIR,vI)

end function DINERT2ORB_GDROMO

subroutine DGDROMO2CART_CORE(z,rmag,cnu,snu,zeta,rho,emme,RV,VV,pot)
! Description:
!    Transforms from GDromo state vector to Cartesian coordinates.
!    Optionally, it takes into account the potential.
!
! ==============================================================================

implicit none

! Arguments
real(dk),intent(in)   ::  z(1:8)                ! GDromo state vector
real(dk),intent(in)   ::  rmag                  ! Radius vector magnitude
real(dk),intent(in)   ::  pot                   ! Potential
real(dk),intent(in)   ::  zeta,rho,emme         ! Auxiliary quantities
real(dk),intent(in)   ::  cnu,snu               ! cosine and sine of the angle of rotation nu between the LVLH and the
                                                ! intermediate frames
real(dk),intent(out)  ::  RV(1:3), VV(1:3)      ! Cartesian position and velocity
! Locals
real(dk)  ::  u,lam

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

! Position vector
RV(1) =  rmag*( ( 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik )*cnu +&
& 2._dk*( z(4)*z(5) - z(6)*z(7) )*snu )
RV(2) =  rmag*( ( 1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik )*snu +&
& 2._dk*( z(4)*z(5) + z(6)*z(7) )*cnu )
RV(3) = 2._dk*rmag*( ( z(5)*z(6) + z(4)*z(7) )*snu +&
& ( z(4)*z(6) - z(5)*z(7) )*cnu )

! Velocity vector
! Radial velocity
u = zeta/(rho*sqrt(z(3)))
! Transverse velocity
lam = sqrt(emme**2_ik/(z(3)*rho**2_ik) - 2._dk*pot)

VV(1) = ( 2._dk*lam*( z(4)*z(5) - z(6)*z(7) ) + u*( 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik ) )*cnu + &
          	& ( 2._dk*u*( z(4)*z(5) - z(6)*z(7) ) - lam*( 1._dk - 2._dk*z(5)**2_ik - 2._dk*z(6)**2_ik ) )*snu
VV(2) = ( lam*( 1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik ) + 2._dk*u*( z(4)*z(5) + z(6)*z(7) ) )*cnu + &
          	& ( u*( 1._dk - 2._dk*z(4)**2_ik - 2._dk*z(6)**2_ik ) - 2._dk*lam*( z(4)*z(5) + z(6)*z(7) ) )*snu
VV(3) = 2._dk*( lam*( z(5)*z(6) + z(4)*z(7) ) + u*( z(4)*z(6) - z(5)*z(7) ) )*cnu + &
          	& 2._dk*( u*( z(5)*z(6) + z(4)*z(7) ) - lam*( z(4)*z(6) - z(5)*z(7) ) )*snu

end subroutine DGDROMO2CART_CORE

end module TRANSFORM


c=======================================================================
c
c     subroutine EMPCOVEXP
c
c     Empirical Exponentially Weighted Covariance matrix and mean
c
c                         n
c                        ---
c     cov(i,j) = (1 - L) \   L^(k-1)*[X(k,i) - rho(i)] * [X(k,j) - rho(j)]
c                        /
c                        ---
c                        k=1
c
c-----------------------------------------------------------------------
      SUBROUTINE empcovexp ( n, p, lambda, X, rho, XR, cov, info )
c-----------------------------------------------------------------------
c
c     INPUT
c            n     : number of value(s) (n > 1)                  integer
c            p     : number of asset(s) (p >= 1)                 integer
c            lambda: exponetial parameter                        integer
c            X     : matrix of values (n*p)                       double
c            rho   : mean vector (n)                              double
c
c     OUTPUT
c            XR    : relative [X(.,j) - rho(j)] (n*p)            double
c            cov   : empirical covariance matrix (p*p)           double
c            info  : diagnostic argument                        integer
c
c     CALL
c        cove      : covariance matrix from centred values : X - E(X)
c                   (input matrix n*m, result : full symetric matrix m*m)
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     arguments i/o
      INTEGER n, p, info
      DOUBLE PRECISION lambda, X(n,*), rho(*), cov(p,*), XR(n,*)
c
c     local variables
      INTEGER i, j
      DOUBLE PRECISION mean
c
c     external subroutines
      EXTERNAL cove
c
c-----------------------------------------------------------------------
c
c     initialization
      info = 0
c
c     relative return(s)
      DO j = 1,p
         mean = rho(j)
         DO i = 1,n
            XR(i, j) = X(i,j) - mean
         ENDDO
      ENDDO
c
c     computation of sum on k of XR(k,i) * XR(k,j)
      CALL cove ( n, p, XR, lambda, cov, info )
c
      RETURN
      END


      real function unifor(slope,z,n,q)
c
c     + + + PURPOSE + + +
c
c     Function UNIFOR computes the normal depth of flow.
c
c     Called from: SR FSLPAR
c     Author(s): Ascough II, R. van der Zweep, V. Lopes
c     Reference in User Guide:
c
c     Version:
c     Date recoded:
c     Recoded by: Jim Ascough II
c
c     + + + KEYWORDS + + +
c
c     + + + PARAMETERS + + +
c
c     + + + ARGUMENT DECLARATIONS + + +
c
      real slope, z, n, q
c
c     + + + ARGUMENT DEFINITIONS + + +
c
c     slope -
c     z     -
c     n     -
c     q     -
c
c     + + + COMMON BLOCKS + + +
c
c     + + + LOCAL VARIABLES + + +
c
      real c1
c
c     + + + LOCAL DEFINITIONS + + +
c
c     c1 -
c
c     + + + SAVES + + +
c
c     + + + SUBROUTINES CALLED + + +
c
c     + + + DATA INITIALIZATIONS + + +
c
c     + + + END SPECIFICATIONS + + +
c
c
      c1 = (z**2.5/(2.0*sqrt(z**2+1.0))) ** (2.0/3.0)
      unifor = (q*n/(c1*1.49*sqrt(slope))) ** 0.375
c
      return
      end


! Copyright (C) 2015-2017, Simone Marsili
! All rights reserved.
! License: BSD 3 clause

module cost
  use kinds
  implicit none
  private
  public :: compute_gradient
  public :: compute_cost

contains

  subroutine compute_gradient(fdata, fmodel, lambda, prm, grd)
    real(kflt), intent(in)      :: fdata(:)
    real(kflt), intent(in)      :: fmodel(:)
    real(kflt), intent(in)      :: lambda
    real(kflt), intent(in)      :: prm(:)
    real(kflt), intent(out)     :: grd(:)

    ! the gradient is: F - f + l * p
    grd = fmodel - fdata + lambda * prm

  end subroutine compute_gradient

  subroutine compute_cost(fdata, prm, lambda, edata, ereg)
    real(kflt), intent(in)  :: fdata(:)
    real(kflt), intent(in)  :: prm(:)
    real(kflt), intent(in)  :: lambda
    real(kflt), intent(out) :: edata,ereg

    edata = -sum(fdata * prm)

    ereg = 0.5_kflt * lambda * sum(prm**2)

  end subroutine compute_cost

end module cost


! Test program for using customized calling subroutine, use
! the Rosenbrock function from driver 1, added constants

!
! CHANGES
! 21.12.2018 - Miha Polajnar
! Initial version
!
program custom1
  use iso_fortran_env, only: rk => real64
  use l_bfgs_b_ag, only: l_bfgs_b, optim_prob_t, lbfgsb_opt_t
  implicit none
  integer, parameter :: n = 25
  integer :: i
  type :: rconst_t
    real(rk) :: a, b
  end type
  type(optim_prob_t) :: optim_prob
  type(lbfgsb_opt_t) :: lbfgsb_opt
  type(rconst_t) :: rconst
  !
  lbfgsb_opt = lbfgsb_opt_t(iprint=1)
  rconst = rconst_t(a=4._rk,b=8._rk)
  !
  optim_prob%name = 'Rosenbrock'
  optim_prob%n = n
  optim_prob%x = [(3._rk,i=1,n)]
  optim_prob%nbd=[(2,i=1,n)]
  allocate(optim_prob%lb(n),optim_prob%ub(n))
  do i = 1, n, 2
    optim_prob%lb(i)   = 1.0_rk
    optim_prob%ub(i)   = 1.0e2_rk
  end do
  do i =2, n, 2
    optim_prob%lb(i)   = -1.0e2_rk
    optim_prob%ub(i)   =  1.0e2_rk
  end do
  optim_prob%func => rosenbrock_func
  optim_prob%grad => grad_rosenbrock_func
  allocate(optim_prob%dat,source=rconst)
  ! Run the algorithm
  call l_bfgs_b(optim_prob,lbfgsb_opt)
contains
  !
  pure function rosenbrock_func(x,dat) result(res)
    real(rk), intent(in) :: x(:)
    class(*), intent(in) :: dat
    real(rk) :: res
    integer :: i
    res = .25_rk * (x(1) - 1._rk)**2
    do i = 2, size(x)
      res = res + (x(i) - x(i - 1)**2)**2
    end do
    select type(dat)
    type is (rconst_t)
      res = dat%a * res
    class default
      error stop 'Rosenbrock function, incorrect type.'
    end select
  end function
  !
  pure subroutine grad_rosenbrock_func(x,dat,grad)
    real(rk), intent(in) :: x(:)
    class(*), intent(in) :: dat
    real(rk), intent(out) :: grad(:)
    integer :: i, n
    real(rk) :: t1, t2
    n = size(x)
    t1 = x(2) - x(1)**2
    grad(1) = 2._rk * (x(1) - 1._rk) - 1.6e1_rk * x(1) * t1
    do i = 2, n - 1
       t2   = t1
       t1   = x(i+1) - x(i)**2
       grad(i) = 8._rk * t2 - 1.6e1_rk * x(i) * t1
    end do
    select type (dat)
    type is (rconst_t)
      grad(n) = dat%b * t1
    class default
      error stop 'Rosenbrock function gradient, incorrect type.'
    end select
  end subroutine
  !
end program


MODULE sim
  USE iso_Fortran_env, ONLY: wp => real64
  IMPLICIT NONE

  REAL(wp) :: piwp = 4.0 * ATAN(1.0)
CONTAINS

  SUBROUTINE fixSeed()
    IMPLICIT NONE
    INTEGER, ALLOCATABLE, DIMENSION(:) :: seed
    INTEGER :: n
    CALL RANDOM_SEED(size = n)
    ALLOCATE(seed(n))
    seed = 1989
    CALL RANDOM_SEED(put = seed)
  END SUBROUTINE fixSeed

  SUBROUTINE simMarkov(sz, piMat, newIx, oldIx)
    IMPLICIT NONE
    INTEGER, INTENT(IN) :: sz, oldIx
    REAL(wp), DIMENSION(sz, sz), INTENT(IN) :: piMat
    INTEGER, INTENT(OUT) :: newIx
!    REAL(wp) :: rno, ssum
!    INTEGER :: ix

!    CALL simUniform(rno)
    CALL simDiscrete(sz, piMat(oldIx, :), newIx)

!    ssum = 0
!    DO ix = 1,sz
!      ssum = ssum + piMat(oldIx, ix)
!      IF ( rno < ssum .AND. ssum - piMat(oldIx, ix) <= rno ) THEN
!        newIx = ix
!        EXIT
!      END IF
!    END DO
  END SUBROUTINE simMarkov

  SUBROUTINE simDiscrete(sz, pmf, drawIx)
    IMPLICIT NONE
    INTEGER, INTENT(IN) :: sz
    REAL(wp), DIMENSION(sz), INTENT(IN) :: pmf
    INTEGER, INTENT(OUT) :: drawIx
    REAL(wp) :: rno, ssum
    INTEGER :: ix

    CALL simUniform(rno)

    ssum = 0
    DO ix = 1,sz
      ssum = ssum + pmf(ix)
      IF ( rno < ssum .AND. rno >= ssum - pmf(ix) ) THEN
        drawIx = ix
        EXIT
      END IF
    END DO
  END SUBROUTINE simDiscrete

  SUBROUTINE simStdNormal(val)
    IMPLICIT NONE
    REAL(wp), INTENT(OUT) :: val
    REAL(wp) :: v1, v2

    CALL RANDOM_NUMBER(v1)
    CALL RANDOM_NUMBER(v2)
    val = sqrt ( - 2.0 * log ( v1 ) ) * cos ( 2.0 * piwp * v2 )
  END SUBROUTINE simStdNormal

  SUBROUTINE simUniform(val)
    IMPLICIT NONE
    REAL(wp), INTENT(OUT) :: val

    CALL RANDOM_NUMBER(val)
  END SUBROUTINE simUniform

  SUBROUTINE simDiscreteUniform(sz, retIx)
    IMPLICIT NONE
    INTEGER, INTENT(IN) :: sz
    INTEGER, INTENT(OUT) :: retIx
    REAL(wp) :: rno

    CALL simUniform(rno)
    retIx = FLOOR(sz * rno) + 1
  END SUBROUTINE simDiscreteUniform

END MODULE sim


PROGRAM COCKTAIL
  IMPLICIT NONE

  INTEGER :: intArray(10) = (/ 4, 9, 3, -2, 0, 7, -5, 1, 6, 8 /)

  WRITE(*,"(A,10I5)") "Unsorted array:", intArray
  CALL Cocktail_sort(intArray)
  WRITE(*,"(A,10I5)") "Sorted array  :", intArray

CONTAINS

  SUBROUTINE Cocktail_sort(a)
    INTEGER, INTENT(IN OUT) :: a(:)
    INTEGER :: i, bottom, top, temp
    LOGICAL :: swapped

    bottom = 1
    top = SIZE(a) - 1
    DO WHILE (bottom < top )
       swapped = .FALSE.
       DO i = bottom, top
          IF (array(i) > array(i+1)) THEN
              temp = array(i)
              array(i) = array(i+1)
              array(i+1) = temp
              swapped = .TRUE.
          END IF
       END DO
       IF (.NOT. swapped) EXIT
       DO i = top, bottom + 1, -1
          IF (array(i) < array(i-1)) THEN
              temp = array(i)
              array(i) = array(i-1)
              array(i-1) = temp
              swapped = .TRUE.
          END IF
       END DO
       IF (.NOT. swapped) EXIT
       bottom = bottom + 1
       top = top - 1
    END DO
  END SUBROUTINE Cocktail_sort

END PROGRAM COCKTAIL


module malg
use, intrinsic :: iso_fortran_env, only : sp=>REAL32,dp=>REAL64, INT64, stdout=>output_unit

use perf, only : sysclock2ms

implicit none

private

integer, parameter :: wp=dp

public :: double_gemm, single_gemm, double_matmul, single_matmul, wp

contains

Real(wp) Function double_gemm(N,Nrun)

external :: dgemm
integer, intent(in) :: N,Nrun

real(dp), dimension(N,N) :: A, B, D
integer :: k
integer(int64) :: tic,toc,tmin=huge(0_int64)  ! MUST BE _int64!!!

D(:,:) = 0. ! cannot initialize automatic array directly

call random_init(.false., .false.)

print *,'priming double-prec. DGEMM loop'
! recommended to call once before loop per Intel manual
call dgemm('N','N',N,N,N,1._dp,A,N,B,N,0._dp,D,N)

do k = 1, Nrun
!refilling arrays with random numbers to be sure a clever compiler doesn't workaround
  call random_number(A)
  call random_number(B)

  call system_clock(tic)
  call dgemm('N','N',N,N,N,1._dp,A,N,B,N,0._dp,D,N) !ifort 14 10% faster than gfortran 5
  call system_clock(toc)

  if (toc-tic < tmin) tmin=toc-tic

  if (modulo(k,1) == 0) then
    write(stdout,'(F6.1,A1)', advance='no') real(k,wp)/Nrun*100,'% done'
    flush(stdout)
  endif
end do

double_gemm = sysclock2ms(tmin) / 1000.

end function double_gemm

Real(wp) function single_gemm(N,Nrun)

external :: sgemm
integer, intent(in) :: N,Nrun
real(sp),dimension(N,N) :: A, B, D
integer :: k
integer(int64) :: tic,toc, tmin=huge(0_int64)

D(:,:) = 0.

call random_init(.false., .false.)

print *,'priming single-prec. SGEMM loop'
! recommended to call once before loop per Intel manual
call sgemm('N','N',N,N,N,1._sp,A,N,B,N,0._sp,D,N)

do k = 1, Nrun
!refilling arrays with random numbers to be sure a clever compiler doesn't workaround
  call random_number(A)
  call random_number(B)

  call system_clock(tic)
  call sgemm('N','N',N,N,N,1._sp,A,N,B,N,0._sp,D,N)
  call system_clock(toc)

  if (toc-tic < tmin) tmin=toc-tic

  if (modulo(k,1) == 0) then
    write(stdout,'(F6.1,A1)', advance='no') real(k,wp)/Nrun*100,'% done'
    flush(stdout)
  endif
end do

single_gemm = sysclock2ms(tmin) / 1000.

end function single_gemm

Real(wp) Function double_matmul(N,Nrun)

integer, intent(in) :: N,Nrun

real(dp), dimension(N,N) :: A, B, D
integer :: k
integer(int64) :: tic,toc,tmin=huge(0_int64)  ! MUST BE _int64!!!

D(:,:) = 0. ! cannot initialize automatic array directly

call random_init(.false., .false.)

print *,'priming double-prec. matmul loop'
! recommended to call once before loop per Intel manual
D = matmul(A,B)

do k = 1, Nrun
!refilling arrays with random numbers to be sure a clever compiler doesn't workaround
  call random_number(A)
  call random_number(B)

  call system_clock(tic)
  D = matmul(A,B)
  call system_clock(toc)

  if (toc-tic < tmin) tmin=toc-tic

  if (modulo(k,1) == 0) then
    write(stdout,'(F6.1,A1)', advance='no') real(k,wp)/Nrun*100,'% done'
    flush(stdout)
  endif
end do

double_matmul = sysclock2ms(tmin) / 1000.

end function double_matmul

Real(wp) Function single_matmul(N,Nrun)

integer, intent(in) :: N,Nrun

real(sp), dimension(N,N) :: A, B, D
integer :: k
integer(int64) :: tic,toc,tmin=huge(0_int64)  ! MUST BE _int64!!!

D(:,:) = 0. ! cannot initialize automatic array directly

call random_init(.false., .false.)

print *,'priming single-prec. matmul loop'
! recommended to call once before loop per Intel manual
D = matmul(A,B)

do k = 1, Nrun
!refilling arrays with random numbers to be sure a clever compiler doesn't workaround
  call random_number(A)
  call random_number(B)

  call system_clock(tic)
  D = matmul(A,B)
  call system_clock(toc)

  if (toc-tic < tmin) tmin=toc-tic

  if (modulo(k,1) == 0) then
    write(stdout,'(F6.1,A1)', advance='no') real(k,wp)/Nrun*100,'% done'
    flush(stdout)
  endif
end do

single_matmul = sysclock2ms(tmin) / 1000.

end function single_matmul

end module malg

Program run_matmul

use malg
!use, intrinsic:: iso_fortran_env, only: compiler_version, compiler_options

implicit none

integer :: N, Nrun, ios
character(16) :: argv
real(wp) :: tdmatmul,tsmatmul,tdblint, tsglint

!print *,compiler_version()
!print *,compiler_options()

N=1000
call get_command_argument(1,argv,status=ios)
if(ios==0) read(argv,*) N

Nrun=3
call get_command_argument(2,argv,status=ios)
if(ios==0) read(argv,*) Nrun

tdmatmul = double_gemm(N,Nrun)
tsmatmul = single_gemm(N,Nrun)
tdblint = double_matmul(N,Nrun)
tsglint = single_matmul(N,Nrun)

print *, tsmatmul, 'single SGEMM (sec)'
print *, tdmatmul, 'double DGEMM (sec)'
print *, tsglint, 'single intrinsic (sec)'
print *, tdblint, 'double intrinsic (sec)'

end program


* ThreeMom.f
* this file is part of FormCalc
* last modified 15 Jun 04 th

* ThreeMom computes the length of \vec p_b in the frame in which
* \vec p_a + \vec p_b vanishes.  With sqrtS = sqrt((p_a + p_b)^2),
*
* ThreeMom(sqrtS, ma, mb) = sqrt(lambda(sqrtS, ma**2, mb**2))/(2*sqrtS),
*
* where lambda is the Kallen function.

	double precision function ThreeMom(sqrtS, ma, mb)
	implicit none
	double precision sqrtS, ma, mb

	ThreeMom = sqrt(.25D0*(sqrtS - (ma**2 - mb**2)/sqrtS)**2 -
     &    mb**2)
	end


#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! z_rfr3_turnover
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine computes a square root free turnover.
!
! | cg        -s |                | c       -s | | -conj(rho)     0 |
! |  s  conj(cg) | | u       -v | | s  conj(c) | |          0  -rho |
!                  | v  conj(u) |
!
!
!                                =
!
!                                     | u       -v |
! | c       -s | | -conj(rho)     0 | | v  conj(u) | | cg        -s |
! | s  conj(c) | |          0  -rho |                |  s  conj(cg) |
!
! Due to the symmetry it is not necessary to store the values c, s
! or g explicitly. The following variables take their place:
!
! CC = |c|^2
! SS = s^2
!  W = gc^2/|c|^2
! VV = v^2
!
! The input W, CC, SS, U, VV and RHO must satisfy the following:
!
!    CC + SS = 1
!        |W| = 1
! |U|^2 + VV = 1
!      |RHO| = 1
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! INPUT VARIABLES:
!
!  W               COMPLEX(8)
!                    unimodular complex number
!
!  CC, SS          REAL(8)
!                    moduli squared of cosine and sine
!
!  U               COMPLEX(8)
!                    complex component of Givens rotation
!
!  VV              REAL(8)
!                    real component squared of Givens rotation
!
!  RHO             COMPLEX(8)
!                    unimodular shift
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine z_rfr3_turnover(W,CC,SS,U,VV,RHO)

  implicit none

  ! input variables
  real(8), intent(inout) :: CC, SS, VV
  complex(8), intent(inout) :: W, U
  complex(8), intent(in) :: RHO

  ! compute variables
  real(8) :: nn, zz, xx
  complex(8) :: z, uold

  ! store old SS and U
  xx = SS
  uold = U

  ! z and zz
  z = U + W
  zz = dble(z)**2 + aimag(z)**2

  ! new U
  U = (CC*z - U)

  ! new W, CC and SS
  CC = CC*zz
  if ( CC.EQ.0d0 ) then
    nn = VV
    SS = 1d0
    W = cmplx(1d0,0d0,kind=8)
  else
    nn = VV + CC
    CC = CC/nn
    SS = VV/nn
    W = -RHO*(z*(VV/zz) + uold)
  end if

  ! new U
  U = -conjg(RHO)*U

  ! new VV
  VV = xx*nn

  ! ensure normality
  xx = dble(U)**2 + aimag(U)**2 + VV
  U = 5d-1*U*(3d0 - xx)
  VV = VV*(2d0-xx)
  xx = dble(W)**2 + aimag(W)**2
  W = 5d-1*W*(3d0 - xx)

end subroutine z_rfr3_turnover


	SUBROUTINE SNDWND ( storm, crdrot, nparms, nlevel, hdata, iret )
C************************************************************************
C* SNDWND								*
C*									*
C* This routine will calculate the winds relative to the storm/wave	*
C* motion and coordinate rotation.					*
C*									*
C* SNDWND ( STORM, CRDROT, NPARMS, NLEVEL, HDATA, IRET )		*
C*									*
C* Input parameters:							*
C*	STROM		CHAR*		Storm speed and direction	*
C*	CRDROT		CHAR*		Coordinate rotation		*
C*	NPARMS		INTEGER		Number of parameters		*
C*	NLEVLE		INTEGER		Number of levels		*
C*									*
C* Input/Output parameters:						*
C*	HDATA (LLMXLV)	REAL		Interpolated sounding data	*
C*									*
C* Output parameters:							*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal			*
C**									*
C* Log:									*
C* S. Jacobs/SSAI	 4/92						*
C* J. Whistler/SSAI	 4/93		Cleaned up header		*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
	INCLUDE		'sndiag.prm'
C*
	CHARACTER*(*)	storm, crdrot
	REAL		hdata(*)
C*
	REAL		rarr(2)
C*
	INCLUDE		'ERMISS.FNC'
C------------------------------------------------------------------------
	iret = 0
C
	CALL ST_RLST ( storm, '/', 0., 2, rarr, isnum, ier )
	stsped = rarr(1)
	stdrct = rarr(2)
	ust = stsped * ( -SIN(stdrct*DTR) )
	vst = stsped * ( -COS(stdrct*DTR) )
C
	CALL ST_CRNM ( crdrot, rotang, ier )
	IF  ( ier .ne. 0 )  rotang = 0.
	rotang = 270. - rotang
C
	DO  i = 1, nlevel
	    IF  ( ( .not. ERMISS(hdata((i-1)*nparms+IUWND)) ) .and.
     +		  ( .not. ERMISS(hdata((i-1)*nparms+IVWND)) ) )  THEN
		ur = hdata((i-1)*nparms+IUWND) - ust
		vr = hdata((i-1)*nparms+IVWND) - vst
		hdata((i-1)*nparms+IUWND) = ur * COS(rotang*DTR) +
     +					    vr * SIN(rotang*DTR)
		hdata((i-1)*nparms+IVWND) = vr * COS(rotang*DTR) -
     +					    ur * SIN(rotang*DTR)
	    END IF
	END DO
C*
	RETURN
	END


module quaternion_mod
    use,intrinsic :: iso_fortran_env
    implicit none
    private
    real(real64),parameter:: dzero=0d+0, done=1d+1, dhalf=5d-1
    integer(int32),parameter:: izero=0, ione=1, n=4
    type quaternion_type
        real(real64):: val(4)
    contains
        procedure:: incremental_matrix
        procedure:: coordinate_inv_rotation_matrix
        procedure:: coordinate_rotation_matrix
        procedure:: absolute_value
        procedure:: constraint
    end type

    public:: quaternion_type
    public:: from_zxz_euler_angle
    public:: mul
contains
    function from_zxz_euler_angle(phi, theta, psi) result(quaternion)
        ! z, x', z" -> phi, theta, psi
        real(real64),intent(in):: phi, theta, psi
        real(real64):: sinterm, costerm, plusterm, diffterm
        type(quaternion_type):: quaternion

        sinterm = sin(theta*dhalf)
        costerm = cos(theta*dhalf)
        plusterm = (psi+phi)*dhalf
        diffterm = (psi-phi)*dhalf
        quaternion%val(:) = [sinterm*sin(diffterm), sinterm*cos(diffterm), costerm*sin(plusterm), costerm*cos(plusterm)]
        call quaternion%constraint()
    end function

    function mul(q,p) result(ret)
        type(quaternion_type),intent(in):: q,p
        type(quaternion_type):: ret
        real(real64):: v1,v2,v3,v4

        associate(q1=>q%val(1), q2=>q%val(2),q3=>q%val(3),q4=>q%val(4),&
        p1=>p%val(1), p2=>p%val(2),p3=>p%val(3),p4=>p%val(4))
            v1 = q4*p1 - q3*p2 + q2*p3 + q1*p4
            v2 = q3*p1 + q4*p2 - q1*p3 + q2*p4
            v3 = - q2*p1 - q1*p2 + q4*p3 + q3*p4
            v4 = - q1*p1 - q2*p2 - q3*p3 + q4*p4
        end associate
        ret = quaternion_type([v1,v2,v3,v4])
        call ret%constraint()
    end function

    function inv(q) result(ret)
        type(quaternion_type),intent(in):: q
        type(quaternion_type):: ret

        ret = q
        ret%val(2) = -ret%val(2)
        ret%val(3) = -ret%val(3)
        ret%val(4) = -ret%val(4)
    end function

    function incremental_matrix(q) result(ret)
        class(quaternion_type),intent(in):: q
        real(real64):: ret(4,4)

        associate(xi=>q%val(1), eta=>q%val(2), zeta=>q%val(3), chi=>q%val(4))
            ret(:,1) = [-zeta, chi,  xi,  -eta]
            ret(:,2) = [-chi, -zeta,  eta,  xi]
            ret(:,3) = [ eta, -xi, chi,  -zeta]
            ret(:,4) = [ xi,  eta,  zeta,  chi]
        end associate
    end function

    function coordinate_rotation_matrix(q) result(ret)
        class(quaternion_type),intent(in):: q
        real(real64):: ret(3,3),t11, t12, t13, t14, t22, t23, t24, t33, t34, t44

        associate(xi=>q%val(1), eta=>q%val(2), zeta=>q%val(3), chi=>q%val(4))
            t11 = xi*xi;     t12 = xi*eta;   t13 = xi*zeta; t14 = xi*chi
            t22 = eta*eta;   t23 = eta*zeta; t24 = eta*chi
            t33 = zeta*zeta; t34 = zeta*chi
            t44 = chi*chi
        end associate
        ret(:,1) = [-t11+t22-t33+t44, -2d0*(t12+t34), 2d0*(t23-t14)]
        ret(:,2) = [ 2d0*(t34-t12), t11-t22-t33+t44, -2d0*(t13+t24)]
        ret(:,3) = [ 2d0*(t23+t14), 2d0*(t24-t13), -t11-t22+t33+t44]
    end function

    function coordinate_inv_rotation_matrix(q) result(ret)
        class(quaternion_type),intent(in):: q
        real(real64):: ret(3,3),t11, t12, t13, t14, t22, t23, t24, t33, t34, t44

        associate(xi=>q%val(1), eta=>q%val(2), zeta=>q%val(3), chi=>q%val(4))
            t11 = xi*xi;     t12 = xi*eta;   t13 = xi*zeta; t14 = xi*chi
            t22 = eta*eta;   t23 = eta*zeta; t24 = eta*chi
            t33 = zeta*zeta; t34 = zeta*chi
            t44 = chi*chi
        end associate
        ret(:,1) = [-t11+t22-t33+t44, 2d0*(t34-t12), 2d0*(t23+t14)]
        ret(:,2) = [-2d0*(t34+t12), t11-t22-t33+t44, 2d0*(t24-t13)]
        ret(:,3) = [2d0*(t23-t14), -2d0*(t13+t24), -t11-t22+t33+t44]
    end function

    function absolute_value(q) result(ret)
        class(quaternion_type),intent(in):: q
        real(real64):: ret
        integer(int32):: i

        ret = dzero
        do i=ione,n
            ret=ret+q%val(i)*q%val(i)
        end do
    end function

    subroutine constraint(q)
        class(quaternion_type),intent(inout):: q

        q%val(:) = q%val(:)/sqrt(q%absolute_value())
    end subroutine
end module

program main
    use,intrinsic :: iso_fortran_env
    use quaternion_mod
    implicit none
    real(real64),parameter:: pi=acos(-1d0)
    real(real64):: euler(3) = [pi/3d0, pi/4d0, pi/2d0]
    real(real64):: p(3) = [1d0, 2d0, 3d0]
    type(quaternion_type):: q,q1,q2,q3

    q = from_zxz_euler_angle(euler(1), euler(2), euler(3))
    q1 = from_zxz_euler_angle(euler(1), 0d0, 0d0)
    q2 = from_zxz_euler_angle(0d0, euler(2), 0d0)
    q3 = from_zxz_euler_angle(0d0, 0d0, euler(3))
    print*, 'qaternion: ', q1%val
    print*, 'qaternion: ', q2%val
    print*, 'qaternion: ', q3%val
    print*, 'qaternion: ', q%val
    print*, 'qaternion: ', mul(mul(q1,q2),q3)
    print*, 'rotation: ', matmul(q%coordinate_rotation_matrix(), p)
    print*, 'inv_rotation: ', matmul(q%coordinate_inv_rotation_matrix(), p)
end program main


      REAL FUNCTION DBUBF(RLAT)
C
C SPECIFIES FUNCTIONAL FORM OF 1ST DERIVATIVE OF BIG U BAR (BUBFNC)
C
C CALLED BY: ANLYTC, FORCE
C CALLS:
C
C REVISIONS:
C 7-10-92 CHANGE TO CCM CODING CONVENTIONS (R. JAKOB)
C
C---- Common Blocks ----------------------------------------------------
C
C     Case 4 variables
      INCLUDE 'case4.i'
C
C------------ Arguments ------------------------------------------------
C
C     Input
C
C LATITUDE
      REAL RLAT
C
C------ Local Variables ------------------------------------------------
C
      REAL RMU,COSLAT
C
C----- Executable Statements -------------------------------------------
C
      RMU    = SIN(RLAT)
      COSLAT = COS(RLAT)
      DBUBF  = 2.0*SU0*(2.0*RMU*COSLAT)**(NPWR-1)*
     $         (REAL(NPWR)-REAL(2*NPWR+1)*RMU*RMU)
C
      RETURN
      END


!==============================================================================!
  real function Root_Mean_Square(ni, r)
!------------------------------------------------------------------------------!
!   Calculates root means square of vector r without normalization.            !
!   This non-normalized variant seems to be better option for ACM.             !
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  integer :: ni
  real    :: r(:)  ! this may be only in inner cells
!-----------------------------------[Locals]-----------------------------------!
  real    :: rms
  integer :: i
!==============================================================================!

  ! Compute rms normalizing it with main diagonal in the system matrix
  rms = 0.0
  do i = 1, ni
    rms = rms + r(i)**2
  end do
  call Comm_Mod_Global_Sum_Real(rms)
  rms = sqrt(rms)

  Root_Mean_Square = rms

  end function


c  f90-intrinsic-mathematical.f
c
c Test Fortran 90 intrinsic mathematical functions - Section 13.10.3 and
c 13.13
c     David Billinghurst <David.Billinghurst@riotinto.com>
c
c Notes:
c  * g77 does not fully comply with F90.  Noncompliances noted in comments.
c  * Section 13.12: Specific names for intrinsic functions tested in
c intrinsic77.f

      logical fail
      common /flags/ fail
      fail = .false.

c     ACOS - Section 13.13.3
      call c_r(ACOS(0.54030231),1.0,'ACOS(real)')
      call c_d(ACOS(0.54030231d0),1.d0,'ACOS(double)')

c     ASIN - Section 13.13.12
      call c_r(ASIN(0.84147098),1.0,'ASIN(real)')
      call c_d(ASIN(0.84147098d0),1.d0,'ASIN(double)')

c     ATAN - Section 13.13.14
      call c_r(ATAN(1.5574077),1.0,'ATAN(real)')
      call c_d(ATAN(1.5574077d0),1.d0,'ATAN(double)')

c     ATAN2 - Section 13.13.15
      call c_r(ATAN2(1.5574077,1.),1.0,'ATAN2(real)')
      call c_d(ATAN2(1.5574077d0,1.d0),1.d0,'ATAN2(double)')

c     COS - Section 13.13.22
      call c_r(COS(1.0),0.54030231,'COS(real)')
      call c_d(COS(1.d0),0.54030231d0,'COS(double)')
      call c_c(COS((1.,0.)),(0.54030231,0.),'COS(complex)')
      call c_z(COS((1.d0,0.d0)),(0.54030231d0,0.d0),
     $     'COS(double complex)')

c     COSH - Section 13.13.23
      call c_r(COSH(1.0),1.5430806,'COSH(real)')
      call c_d(COSH(1.d0),1.5430806d0,'COSH(double)')

c     EXP - Section 13.13.34
      call c_r(EXP(1.0),2.7182818,'EXP(real)')
      call c_d(EXP(1.d0),2.7182818d0,'EXP(double)')
      call c_c(EXP((1.,0.)),(2.7182818,0.),'EXP(complex)')
      call c_z(EXP((1.d0,0.d0)),(2.7182818d0,0.d0),
     $     'EXP(double complex)')

c     LOG - Section 13.13.59
      call c_r(LOG(10.0),2.3025851,'LOG(real)')
      call c_d(LOG(10.d0),2.3025851d0,'LOG(double)')
      call c_c(LOG((10.,0.)),(2.3025851,0.),'LOG(complex)')
      call c_z(LOG((10.d0,0.)),(2.3025851d0,0.d0),
     $     'LOG(double complex)')

c     LOG10 - Section 13.13.60
      call c_r(LOG10(10.0),1.0,'LOG10(real)')
      call c_d(LOG10(10.d0),1.d0,'LOG10(double)')

c     SIN - Section 13.13.97
      call c_r(SIN(1.0),0.84147098,'SIN(real)')
      call c_d(SIN(1.d0),0.84147098d0,'SIN(double)')
      call c_c(SIN((1.,0.)),(0.84147098,0.),'SIN(complex)')
      call c_z(SIN((1.d0,0.d0)),(0.84147098d0,0.d0),
     $     'SIN(double complex)')

c     SINH - Section 13.13.98
      call c_r(SINH(1.0),1.175201,'SINH(real)')
      call c_d(SINH(1.d0),1.175201d0,'SINH(double)')

c     SQRT - Section 13.13.102
      call c_r(SQRT(4.0),2.0,'SQRT(real)')
      call c_d(SQRT(4.d0),2.d0,'SQRT(double)')
      call c_c(SQRT((4.,0.)),(2.,0.),'SQRT(complex)')
      call c_z(SQRT((4.d0,0.)),(2.d0,0.),
     $     'SQRT(double complex)')

c     TAN - Section 13.13.105
      call c_r(TAN(1.0),1.5574077,'TAN(real)')
      call c_d(TAN(1.d0),1.5574077d0,'TAN(double)')

c     TANH - Section 13.13.106
      call c_r(TANH(1.0),0.76159416,'TANH(real)')
      call c_d(TANH(1.d0),0.76159416d0,'TANH(double)')

      if ( fail ) call abort()
      end

      subroutine failure(label)
c     Report failure and set flag
      character*(*) label
      logical fail
      common /flags/ fail
      write(6,'(a,a,a)') 'Test ',label,' FAILED'
      fail = .true.
      end

      subroutine c_r(a,b,label)
c     Check if REAL a equals b, and fail otherwise
      real a, b
      character*(*) label
      if ( abs(a-b) .gt. 1.0e-5 ) then
         call failure(label)
         write(6,*) 'Got ',a,' expected ', b
      end if
      end

      subroutine c_d(a,b,label)
c     Check if DOUBLE PRECISION a equals b, and fail otherwise
      double precision a, b
      character*(*) label
      if ( abs(a-b) .gt. 1.0d-5 ) then
         call failure(label)
         write(6,*) 'Got ',a,' expected ', b
      end if
      end

      subroutine c_c(a,b,label)
c     Check if COMPLEX a equals b, and fail otherwise
      complex a, b
      character*(*) label
      if ( abs(a-b) .gt. 1.0e-5 ) then
         call failure(label)
         write(6,*) 'Got ',a,' expected ', b
      end if
      end

      subroutine c_z(a,b,label)
c     Check if COMPLEX a equals b, and fail otherwise
      double complex a, b
      character*(*) label
      if ( abs(a-b) .gt. 1.0d-5 ) then
         call failure(label)
         write(6,*) 'Got ',a,' expected ', b
      end if
      end


!=====================================================
SUBROUTINE eva2d_fd(nx,ny,ix,iy,dx,dy,spl,spval)

! Evaluates the first derivatives of 2-dimensional cubic spline of function f(x,y)
!
! Input:  nx, ny              number of values in x and y
!         ix, iy              pointer into the spline array spl
!         dx, dy              distance from x(ix) and y(iy)
!         spl                 array with spline data
! Output: spval(2)            evaluated function value
!                             spval(1) = df/dx
!                             spval(2) = df/dy

  USE neo_precision

  IMPLICIT NONE

  INTEGER,                             INTENT(in)  :: nx, ny, ix, iy
  REAL(kind=dp),                       INTENT(in)  :: dx, dy
  REAL(kind=dp), DIMENSION(4,4,nx,ny), INTENT(in)  :: spl
  REAL(kind=dp), DIMENSION(2),         INTENT(out) :: spval

  INTEGER                                          :: i,j
  REAL(kind=dp)                                    :: muli, mulj

  spval = 0

! df/dx
  DO i=2,4
     IF (i == 2) THEN
        muli = 1
     ELSE
        muli = dx**(i-2)
     END IF
     muli = muli * (i-1)
     DO j=1,4
       IF (j == 1) THEN
           mulj = 1
        ELSE
           mulj = dy**(j-1)
        END IF
        spval(1) = spval(1) + spl(i,j,ix,iy) * muli * mulj
     END DO
  END DO

! df/dy
  DO i=1,4
     IF (i == 1) THEN
        muli = 1
     ELSE
        muli = dx**(i-1)
     END IF
     DO j=2,4
        IF (j == 2) THEN
           mulj = 1
        ELSE
           mulj = dy**(j-2)
        END IF
        mulj = mulj * (j-1)
        spval(2) = spval(2) + spl(i,j,ix,iy) * muli * mulj
     END DO
  END DO

  RETURN
END SUBROUTINE eva2d_fd


module fqs_utility

    use fqs_types,         only: wp
    use fqs_constants,     only: pi
    use fqs_vector,        only: vect_t
    use fqs_quaternion,    only: quat_t
    use fqs_utility_basic, only: check_division_scalar

    implicit none

    private

    public deg2rad
    public rad2deg
    public linspace_fcn
    public linspace_sub
    public check_division

    interface check_division
        procedure :: check_division_scalar
        procedure :: check_division_vect
        procedure :: check_division_quat
    end interface check_division

contains

    elemental function deg2rad(ang_deg) result(ang_rad)
        real(wp), intent(in) :: ang_deg
        real(wp)             :: ang_rad
        ang_rad = (pi/180.0_wp)*ang_deg
    end function deg2rad

    elemental function rad2deg(ang_rad) result(ang_deg)
        real(wp), intent(in) :: ang_rad
        real(wp)             :: ang_deg
        ang_deg = (180.0_wp/pi)*ang_rad
    end function rad2deg

    function linspace_fcn(a,b,n,endpoint) result(samples)
        real(wp), intent(in)           :: a          ! start point
        real(wp), intent(in)           :: b          ! stop point
        integer,  intent(in)           :: n          ! number of samples
        logical,  intent(in), optional :: endpoint   ! flag true/false
        real(wp), allocatable          :: samples(:) ! array of samples

        logical  :: endpoint_

        endpoint_ = .true.
        if (present(endpoint)) then
            endpoint_ = endpoint
        end if

        allocate(samples(n))
        call linspace_sub(a,b,samples,endpoint)
    end function linspace_fcn

    subroutine linspace_sub(a,b,samples,endpoint)
        real(wp), intent(in)           :: a           ! start point
        real(wp), intent(in)           :: b           ! stop point
        real(wp), intent(inout)        :: samples(:)  ! array for samples
        logical,  intent(in), optional :: endpoint    ! include endpoint?

        integer   :: n
        integer   :: i
        logical   :: endpoint_
        real(wp)  :: step

        endpoint_ = .true.
        if (present(endpoint)) then
            endpoint_ = endpoint
        end if

        n = size(samples)
        if (endpoint_) then
            step = (b - a)/real(n-1,wp)
        else
            step = (b - a)/real(n,wp)
        end if

        do i = 1, n
            samples(i) = a + (i-1)*step
        end do
    end subroutine linspace_sub

    elemental function check_division_vect(num, den) result(ok)
        type(vect_t), intent(in) :: num
        real(wp),     intent(in) :: den
        logical                  :: ok
        ok = check_division(num % x, den)
        ok = ok .and. check_division(num % y, den)
        ok = ok .and. check_division(num % z, den)
    end function check_division_vect

    elemental function check_division_quat(num, den) result(ok)
        type(quat_t), intent(in) :: num
        real(wp),     intent(in) :: den
        logical                  :: ok
        ok = check_division(num % w, den)
        ok = ok .and. check_division(num % x, den)
        ok = ok .and. check_division(num % y, den)
        ok = ok .and. check_division(num % z, den)
    end function check_division_quat

end module fqs_utility


! test sln of 2d diffusion equation
! under Crank-Nicholson discritisation
! and MMG

program test5

  use multigrid

  implicit none

  integer, parameter :: num = selected_real_kind(p=15)
  integer :: out_unit = 10

  integer :: nx = 256
  integer :: ny = 256

  real(num) :: x_min = 0.0_num
  real(num) :: x_max = 1.0_num
  real(num) :: y_min = 0.0_num
  real(num) :: y_max = 1.0_num

  real(num) :: time = 0.0_num
  real(num) :: t_end = 0.02_num

  real(num) :: k = 1.0_num
  real(num) :: phi2 = 2.0_num !max val
  real(num) :: phi1 = 1.0_num !baseline

  real(num) :: CFL = 2.0_num

  real(num) :: tol = 1e-12_num

  integer :: tstep = 0
  integer :: rstep = 0
  integer :: ix, iy
  real(num) :: dx, dy
  real(num) :: r,xx,yy

  real(num) :: dt
  real(num), dimension(:), allocatable :: x, y
  real(num), dimension(:,:), allocatable :: phi, phi_an, f

  real(num) :: L_phi
  real(num) :: alpha, beta

  real(num) :: L2

  type(mg_input) :: input

  real(num) :: start
  real(num) :: finish

  allocate(x(0:nx+1))  ! cell center (cc) - counts from 1 to nx
  allocate(y(0:ny+1))  ! cell center (cc) - counts from 1 to ny
                        ! ONE GUARD CELL FOR THIS PROBLEM
  dx = (x_max - x_min) / REAL(nx,num)
  do ix = 1, nx+1
    x(ix) = x(ix-1) + dx
  end do

  dy = (y_max - y_min) / REAL(ny,num)
  y(0) = y_min - dy/2.0_num
  do iy = 1, ny+1
  y(iy) = y(iy-1) + dy
  end do

  if (dy /= dx) then
    print *, 'Warning: dy /= dx set. Has not yet being generalised.'
    print *, 'Terminating'
    STOP
  end if

  allocate(phi(-1:nx+2,-1:ny+2))
  allocate(phi_an(-1:nx+2,-2:ny+2))

  do ix = 0, nx + 1
  do iy = 0, ny + 1
    xx = x(ix) - 0.5_num
    yy = y(iy) - 0.5_num
    r = sqrt(xx**2 + yy**2)
    phi(ix,iy) = (phi2-phi1) * exp(-0.25_num * r**2 / k / 0.001_num) + phi1
  enddo
  enddo

!  GOTO 1 ! uncomment to just dump initial condiiton and check it, bypassing the loop

  allocate(f(1:nx,1:ny))

  ! evolution loop

  time = 0.0_num
  call cpu_time(start)
  do
    tstep = tstep + 1
    if (time > t_end) exit

    dt = CFL * dx**2 / k

    alpha = 1.0_num
    beta = dt * k / 2.0_num

    do ix = 1, nx
    do iy = 1, ny

      ! function based on laplacian of phi at the previous time level
      ! - dont update during relaxation

      L_phi = (phi(ix+1,iy) - 2.0_num*phi(ix,iy) + phi(ix-1,iy)) / dx**2 + &
        & (phi(ix,iy+1) - 2.0_num*phi(ix,iy) + phi(ix,iy-1)) / dy**2

      f(ix,iy) = phi(ix,iy) + 0.5_num * dt * k * L_phi

    enddo
    enddo

    input = mg_input(tol = tol, nx=nx, ny = ny, dx=dx, dy=dy, f = f, phi = phi, &
            & bc_xmin = 'zero_gradient', bc_ymin='zero_gradient', &
            & bc_xmax='zero_gradient', bc_ymax = 'zero_gradient', &
!            & deallocate_after = .true., &
            & quiet = .true., &
            & const_helmholtz = .true., ch_alpha = alpha, ch_beta = beta)
    call mg_interface(input)
    phi = input%phi

    print *,'time', time,'tstep',tstep, 'done'

    time = time + dt
  enddo

1 CONTINUE

  call cpu_time(finish)
  print '(" test 5 total cpu_time: ",f20.3," seconds.")',finish-start

  !the analytical solution
  do ix = 1,nx
  do iy = 1,ny
    xx = x(ix) - 0.5_num
    yy = y(iy) - 0.5_num
    r = sqrt(xx**2 + yy**2)
    phi_an(ix,iy) = (phi2-phi1) * (0.001_num / (time+0.001_num)) &
      & * exp(-0.25_num * r**2 / k / (0.001_num + time)) + phi1
  enddo
  enddo

  !calculate the L2 norm against the analytical solution

  L2 = 0.0_num

  do ix = 1, nx
  do iy = 1, ny
    L2 = L2 + abs(phi_an(ix,iy)-phi(ix,iy))**2
  enddo
  enddo
  L2 = sqrt( L2 / REAL(nx*ny,num))

  print *,'L2 error norm agains analtical solution is',L2

  call execute_command_line("rm -rf *.dat test5_*.png")
    !dont want to stream to existing files

  open(out_unit, file="x.dat", access="stream")
  write(out_unit) x(1:nx)
  close(out_unit)

  open(out_unit, file="y.dat", access="stream")
  write(out_unit) y(1:ny)
  close(out_unit)

  open(out_unit, file="phi.dat", access="stream")
  write(out_unit) phi(1:nx,1:ny)
  close(out_unit)

  open(out_unit, file="phi_an.dat", access="stream")
  write(out_unit) phi_an(1:nx,1:ny)
  close(out_unit)

  call execute_command_line("python test5_plot.py")
  call execute_command_line("rm -rf *.dat")

end program test5