Module m_unista
Use m_uniinv
Private
Integer, Parameter :: kdp = selected_real_kind(15)
public :: unista
private :: kdp
private :: R_unista, I_unista, D_unista
interface unista
  module procedure d_unista, r_unista, i_unista
end interface unista
contains

Subroutine D_unista (XDONT, NUNI)
!   UNISTA = (Stable unique) Removes duplicates from an array,
!            leaving unique entries in the order of their first
!            appearance in the initial set.
!  Michel Olagnon - Feb. 2000
! __________________________________________________________
! __________________________________________________________
      Real (kind=kdp), Dimension (:), Intent (InOut) :: XDONT
      Integer, Intent (Out) :: NUNI
! __________________________________________________________
!
      Integer, Dimension (Size(XDONT)) :: IWRKT
      Logical, Dimension (Size(XDONT)) :: IFMPTYT
      Integer :: ICRS
! __________________________________________________________
      Call UNIINV (XDONT, IWRKT)
      IFMPTYT = .True.
      NUNI = 0
      Do ICRS = 1, Size(XDONT)
         If (IFMPTYT(IWRKT(ICRS))) Then
            IFMPTYT(IWRKT(ICRS)) = .False.
            NUNI = NUNI + 1
            XDONT (NUNI) = XDONT (ICRS)
         End If
      End Do
      Return
!
End Subroutine D_unista

Subroutine R_unista (XDONT, NUNI)
!   UNISTA = (Stable unique) Removes duplicates from an array,
!            leaving unique entries in the order of their first
!            appearance in the initial set.
!  Michel Olagnon - Feb. 2000
! __________________________________________________________
! _________________________________________________________
      Real, Dimension (:), Intent (InOut) :: XDONT
      Integer, Intent (Out) :: NUNI
! __________________________________________________________
!
      Integer, Dimension (Size(XDONT)) :: IWRKT
      Logical, Dimension (Size(XDONT)) :: IFMPTYT
      Integer :: ICRS
! __________________________________________________________
      Call UNIINV (XDONT, IWRKT)
      IFMPTYT = .True.
      NUNI = 0
      Do ICRS = 1, Size(XDONT)
         If (IFMPTYT(IWRKT(ICRS))) Then
            IFMPTYT(IWRKT(ICRS)) = .False.
            NUNI = NUNI + 1
            XDONT (NUNI) = XDONT (ICRS)
         End If
      End Do
      Return
!
End Subroutine R_unista
Subroutine I_unista (XDONT, NUNI)
!   UNISTA = (Stable unique) Removes duplicates from an array,
!            leaving unique entries in the order of their first
!            appearance in the initial set.
!  Michel Olagnon - Feb. 2000
! __________________________________________________________
! __________________________________________________________
      Integer, Dimension (:), Intent (InOut)  :: XDONT
      Integer, Intent (Out) :: NUNI
! __________________________________________________________
!
      Integer, Dimension (Size(XDONT)) :: IWRKT
      Logical, Dimension (Size(XDONT)) :: IFMPTYT
      Integer :: ICRS
! __________________________________________________________
      Call UNIINV (XDONT, IWRKT)
      IFMPTYT = .True.
      NUNI = 0
      Do ICRS = 1, Size(XDONT)
         If (IFMPTYT(IWRKT(ICRS))) Then
            IFMPTYT(IWRKT(ICRS)) = .False.
            NUNI = NUNI + 1
            XDONT (NUNI) = XDONT (ICRS)
         End If
      End Do
      Return
!
End Subroutine I_unista
end module m_unista


      subroutine setprob()

      implicit none

      double precision pi, pi2
      common /compi/ pi, pi2

      integer example
      common /example_comm/ example

      integer mapping
      common /mapping_comm/ mapping

      integer initial_condition
      common /initchoice_comm/ initial_condition

      double precision alpha
      common /fivepatch_comm/ alpha

      double precision x0, y0
      common /bilinear_comm/ x0, y0

      double precision velocity(2)
      common /velocity_comm/ velocity

      pi = 4.d0*atan(1.d0)
      pi2 = 2*pi

      open(10,file='setprob.data')
      read(10,*) example
      read(10,*) mapping
      read(10,*) initial_condition
      read(10,*) alpha
      read(10,*) x0
      read(10,*) y0
      read(10,*) velocity(1)
      read(10,*) velocity(2)
      close(10)

      end


!==============================================================================!
  subroutine Field_Mod_Grad_Pressure_Correction(flow, pp)
!------------------------------------------------------------------------------!
!   Calculates gradient of pressure correction.                                !
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  type(Field_Type) :: flow
  type(Var_Type)   :: pp
!-----------------------------------[Locals]-----------------------------------!
  type(Grid_Type), pointer :: grid
  integer                  :: s, c, c1, c2
!==============================================================================!

  ! Take aliases
  grid => flow % pnt_grid

  call Grid_Mod_Exchange_Cells_Real(grid, pp % n)

  !---------------------------------!
  !   No correction at boundaries   !
  !---------------------------------!

  do s = 1, grid % n_faces
    c1 = grid % faces_c(1,s)
    c2 = grid % faces_c(2,s)
    if(c2 < 0) then
      if(Grid_Mod_Bnd_Cond_Type(grid,c2) .ne. PRESSURE) then
        pp % n(c2) = 0.0
      end if
    end if
  end do

  call Field_Mod_Grad_Component(flow, pp % n, 1, pp % x)  ! dp/dx
  call Field_Mod_Grad_Component(flow, pp % n, 2, pp % y)  ! dp/dy
  call Field_Mod_Grad_Component(flow, pp % n, 3, pp % z)  ! dp/dz

  end subroutine


      SUBROUTINE GAUSS2 (A,N,N2)
C
      COMPLEX A(20,1)
      DOUBLE COMPLEX DA(20,30)
C
      DO 5 I = 1, N
      DO 5 J = 1, N2
      DA(I,J) = A(I,J)
5     CONTINUE
      DO 100 I=1,N
      K=I+1
      DO 10 J=K,N2
   10 DA(I,J)=DA(I,J)/DA(I,I)
      DO 30 M=1,N
      IF(M.EQ.I) GO TO 30
      DO 20 L=K,N2
   20 DA(M,L)=DA(M,L)-DA(M,I)*DA(I,L)
   30 CONTINUE
  100 CONTINUE
      DO 150 I = 1, N
      DO 150 J = 1, N2
      A(I,J) = DA(I,J)
  150 CONTINUE
      RETURN
      END


program main
  implicit none
  integer i,ni,nj
  integer,dimension (128) :: iseed
  real(8) diff,norm
  real(8),allocatable,dimension(:) :: xi,ui,ud,xj,gj
  ni = 10000
  nj = 20000
  allocate( xi(3*ni),ui(3*ni),ud(3*ni),xj(3*nj),gj(3*nj) )
  do i = 1,128
     iseed(i) = 0
  enddo
  call random_seed(put=iseed)
  call random_number(xi)
  call random_number(xj)
  call random_number(gj)
  do i = 1,nj
     gj(3*i-2) = (gj(3*i-2) - 0.5) / ni
     gj(3*i-1) = (gj(3*i-1) - 0.5) / ni
     gj(3*i-0) = (gj(3*i-0) - 0.5) / ni
  enddo
  do i = 1,ni
     ui(3*i-2) = 0
     ui(3*i-1) = 0
     ui(3*i-0) = 0
     ud(3*i-2) = 0
     ud(3*i-1) = 0
     ud(3*i-0) = 0
  enddo
  call fmm_init()
  call fmm_biot_savart(ni,xi,ui,nj,xj,gj)
  call direct_biot_savart(ni,xi,ud,nj,xj,gj)
  diff = 0
  norm = 0
  do i = 1,ni
     diff = diff + (ui(3*i-2) - ud(3*i-2)) ** 2
     diff = diff + (ui(3*i-1) - ud(3*i-1)) ** 2
     diff = diff + (ui(3*i-0) - ud(3*i-0)) ** 2
     norm = norm + ud(3*i-2) ** 2
     norm = norm + ud(3*i-1) ** 2
     norm = norm + ud(3*i-0) ** 2
  enddo
  print '(a,es12.5)',"error         :",sqrt(diff/norm)
  call fmm_finalize()
  deallocate( xi,ui,ud,xj,gj )
end program main


#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! z_upr1fact_startchase
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine initializes one iteration of Francis' singleshift
! algorithm for a factored unitary plus rank one (upr1fact) matrix.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! INPUT VARIABLES:
!
!  VEC             LOGICAL
!                    .TRUE. update schurvectors
!                    .FALSE. no schurvectors
!
!  N               INTEGER
!                    dimension of matrix
!
!  P               LOGICAL array of dimension (N-2)
!                    array of position flags for Q
!
!  Q               REAL(8) array of dimension (3*(N-1))
!                    array of generators for givens rotations
!
!  D               REAL(8) arrays of dimension (2*N)
!                    array of generators for complex diagonal matrix
!
!  C,B             REAL(8) arrays of dimension (3*N)
!                    array of generators for upper-triangular part
!
!  M               INTEGER
!                    leading dimesnion of V
!
!  V               COMPLEX(8) array of dimension (M,N)
!                    right schur vectors
!                    if VEC = .FALSE. unused
!                    if VEC = .TRUE. update V to store right schurvectors
!
! OUTPUT VARIABLES:
!
!  G               REAL(8) array of dimension 3
!                    generators for bulge core transformation
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine z_upr1fact_startchase(VEC,N,P,Q,D,C,B,M,V,ITCNT,G)

  implicit none

  ! input variables
  logical, intent(in) :: VEC
  integer, intent(in) :: M, N, ITCNT
  logical, intent(in) :: P(N-2)
  real(8), intent(inout) :: Q(3*(N-1)), D(2*N), C(3*N), B(3*N), G(3)
  complex(8), intent(inout) :: V(M,N)

  ! compute variables
  integer :: ir1, ir2, id1, id2
  logical :: tp(2)
  real(8) :: Ginv(3)
  real(8) :: tq(6), td(6), tc(9), tb(9)
  complex(8) :: shift, A(2,2)

  ! compute shift
  ! random shift
  if ((mod(ITCNT,20).EQ.0).AND.(ITCNT.GT.0)) then
    call random_number(G(1))
    call random_number(G(2))
    shift = cmplx(G(1),G(2),kind=8)

  ! wilkinson shift
  else

    ! special case N = 2
    if (N.LT.3) then

      ! pad with identity
      tp = .FALSE.
      tq = 0d0; tq(1) = 1d0; tq(4:6) = Q
      td = 0d0; td(1) = 1d0; td(3:6) = D
      tc = 0d0; tc(3) = 1d0; tc(4:9) = C
      tb = 0d0; tb(3) = -1d0; tb(4:9) = B

    ! general case
    else

      ! store in temp arrays
      if (N.EQ.3) then
        tp(1) = .FALSE.
        tp(2) = P(N-2)
      else
        tp = P((N-3):(N-2))
      end if
      ir2 = 3*N; ir1 = ir2-8
      id2 = 2*N; id1 = id2-5
      tq = Q((ir1):(ir2-3))
      td = D(id1:id2)
      tc = C(ir1:ir2)
      tb = B(ir1:ir2)

    end if

    ! compute wilkinson shift
    call z_upr1fact_singleshift(tp,tq,td,tc,tb,shift)

  end if

!print*,""
!print*," shift =",shift
!print*,""

  ! build bulge
  call z_upr1fact_buildbulge(P(1),Q(1:3),D(1:4),C(1:6),B(1:6),shift,G)

  ! set Ginv
  Ginv(1) = G(1)
  Ginv(2) = -G(2)
  Ginv(3) = -G(3)

  ! update V
  if (VEC) then

    A(1,1) = cmplx(G(1),G(2),kind=8)
    A(2,1) = cmplx(G(3),0d0,kind=8)
    A(1,2) = -A(2,1)
    A(2,2) = conjg(A(1,1))

    V(:,1:2) = matmul(V(:,1:2),A)

  end if

  ! initialize turnover
  ! hess
  if (.NOT.P(1)) then

    ! fuse Ginv and Q, Ginv is now a diagonal rotation
    call z_rot3_fusion(.FALSE.,Ginv,Q(1:3))

    ! pass G through triangular part
    call z_upr1utri_rot3swap(.FALSE.,D(1:4),C(1:6),B(1:6),G)

    ! move Ginv to the other side and update V
    if (VEC) then

      V(:,1) = V(:,1)*cmplx(Ginv(1),Ginv(2),kind=8)
      V(:,2) = V(:,2)*cmplx(Ginv(1),-Ginv(2),kind=8)

    end if

    ! Ginv scales the columns of the upper-triangular part
    call z_upr1utri_unimodscale(.FALSE.,D(1:2),C(1:3),B(1:3), &
                                cmplx(Ginv(1),Ginv(2),kind=8))
    call z_upr1utri_unimodscale(.FALSE.,D(3:4),C(4:6),B(4:6), &
                                cmplx(Ginv(1),-Ginv(2),kind=8))

  ! inverse hess
  else

    ! pass G through triangular part
    call z_upr1utri_rot3swap(.FALSE.,D(1:4),C(1:6),B(1:6),G)

    ! fuse Q and G, G is now a diagonal rotation
    call z_rot3_fusion(.TRUE.,Q(1:3),G)

    ! G scales the rows of the upper-triangular part
    call z_upr1utri_unimodscale(.TRUE.,D(1:2),C(1:3),B(1:3), &
                                cmplx(G(1),G(2),kind=8))
    call z_upr1utri_unimodscale(.TRUE.,D(3:4),C(4:6),B(4:6), &
                                cmplx(G(1),-G(2),kind=8))

    ! replace G with Ginv as misfit
    G = Ginv

  end if

end subroutine z_upr1fact_startchase


      module mba2d_module

      contains

C ==========================================================
      Subroutine mbaNodal(
C ==========================================================
c group (M)
     &      nP, MaxP, nF, MaxF, nE, MaxE, nPv,
     &      XYP, IPF, IPE, IPV,
     &      CrvFunction, ParCrv, iFnc,
     &      nEStar,
c group (D)
     &      nFv, nEv, IFV, IEV, lbE,
     &      flagAuto, status,
c group (Q)
     &      MaxSkipE, MaxQItr,
     &      Metric, Quality, rQuality,
c group (W)
     &      MaxWr, MaxWi, rW, iW,
     &      iPrint, iERR)
C ==========================================================
      include 'lintrp.fd'
      include 'status.fd'
C ==========================================================
C This routine adapts the mesh to the discrete metric given
C at mesh nodes.
C ==========================================================
C  VARIABLES & PARAMETER:
C
C     nP, MaxP  - the real and maximal number of points (P)
C     nF, MaxF  - the real and maximal number of boundary and inner
C     edges (F)
C     nE, MaxE  - the real and maximal number of triangles (E)
C     nPv       - the number of fixed points (V)
C
C     XYP(2, MaxP) - the Cartesian coordinates of mesh points
C     IPE(3, MaxE) - connectivity list of triangles
C     IPF(4, MaxF) - column 1 &  2 - connectivity list of boundary and
C     inner edges
C                    column 3      - number in the list of
C                    parametrizations:
C
C                                   =0     - this edge is the linear
C                                   segment
C                                   =n > 0 - ParCrv(*, n) gives a
C                                   parametrization
C                                            of this edge and iFnc(n)
C                                            gives
C                                            a function number for
C                                            computing the
C                                            Cartesian coordinates (see
C                                            calCrv())
C                    column 4      - boundary identificator
C                                    (example: unit square has 4
C                                    boundaries which
C                                     may have or not different
C                                     identificators.
C                                     In order to automatically
C                                     recognize corners
C                                     of the square, boundaries have to
C                                     have
C                                     different colors. It is not
C                                     required if
C                                     the corner points are in the list
C                                     of fixed
C                                     points. See colors.fd for more
C                                     details.)
C
C     IPV(nPv)     - list of fixed points
C
C
C     CrvFunction  - user-created routine that computes the
C                    Cartesian coordinates of a point xyc from
C                    its parametric coordinate tc:
C
C                    Subroutine CrvFunction(tc, xyc, iFnc)
C
C                      tc     - parametric coordinate of point xyc
C                      xyc(2) - Cartesian coordinate of the same point
C                      iFnc   - the function number associated with a
C                               curved edge
C
C                      On input :  tc, iFnc
C                      On output:  xyc(2)
C
C     ParCrv(2, MaxF) - linear parametrizations of curvilinear edges
C                       column 1 - parameter for the starting point
C                       column 2 - parameter for the terminal point
C
C                       parameters for the inner points are computed by
C                       the
C                       linear interpolation between two given numbers
C
C                       the Cartesian coordinates are computed by
C                       user-given formulas defined inside CrvFunction.
C
C     iFnc(MaxF)      - function number for computing the Cartesian
C     coordinates
C
C     nEstar - the desired number of triangles
C
C
C     nFv       - number of fixed edges
C     nEv       - number of fixed triangles
C     IFV(nFv)  - list of fixed edges
C     IEV(nEv)  - list of fixed triangles
C
C     lbE(MaxE) - element indentificator (a positive number)
C
C     flagAuto  - flag controling the mesh generation:
C                 TRUE  - automatic recovering of missing mesh elements
C                 FALSE - rigorous checking of input data
C
C     status    - advanced control of mesh generation:
C                 0 or negative - no additional control
C                 positive      - enforce some of the mesh properties
C                      The detailed description of available properties
C                      is in file status.fd. Variable status is equal
C                      to the sum of positive numbers corresponding
C                      to the desired properties. Here is the list of
C                      user-controled properties:
C
C                      1  - the final mesh will not have boundary
C                      elements;
C                      4  - the algorithm will not change boundary
C                      edges;
C                      8  - the missing material interfaces and
C                      boundary;
C                           edges created by the code will be removed
C                           from the final mesh;
C                      16 - the algorithm will not change boundary
C                      points.
C
C                      If only two first properties are required, set
C                      status = 5. Do not use tne number since status
C                      is input/output parameter.
C
C
C     MaxSkipE  - the maximal number of skipped triangles
C     MaxQItr   - the maximal number of local grid modifications
C
C     Metric(3, nP) - real array containing the metric defined at
C                     mesh points. The metric is a 2x2 positive
C                     definite symmetric tensor:
C
C                            M11   M12
C                   Metric =
C                            M12   M22
C
C                   Each column of this array stores the upper
C                   triangular entries in the following order:
C                   M11, M22, and M12.
C
C     Quality   - the requested quality for the final grid
C                 (a positive number between 0 and 1)
C     rQuality  - the reached quality for the final grid
C
C     MaxWr     - the maximal space for real arrays
C     MaxWi     - the maximal space for integer arrays
C
C     rW(MaxWr)  - the real  working array. On output:
C                  rW(1) - mesh generation time
C                  rW(2) - rQuality
C                  rW(3) - average size of triangles (with respect to
C                          the given metric) [hStar]
C
C     iW(MaxWi)  - the integer working array. On output:
C                  iW(1 : nP) - colors of mesh points as
C                               described in file color.fd
C                  iW(nP + 1) - the number of performed iterations
C
C                  If memory allocation unsufficient, the output
C                  is different:
C                  iW(1) - the required integer memory allocation
C                  iW(2) - the required real  memory allocation
C
C
C     iPrint - nonnegative number 10 * A + B:
C
C              B  - the level of output information:
C                   0 - nothing
C                   9 - maximum
C
C              A - the output chanel number (between 0 and 99):
C                  A > 0 - the output goes to file aniMPI.log
C                  A = 0 - screen output is used
c
C     iERR - the error code:
C             0 - a correct termination
C          1000 - the quality has not been reached
C
C ================================================================
C
C  Note:
C       Input parameters:  MaxP, MaxF, MaxE, nPv,
C                          IPV, IFV, IEV, lbE, flagAuto,
C                          nEStar, MaxSkipE, MaxQItr,
C                          Quality, MaxWr, MaxWi, iPrint
C
C       Input / output:    nP, nF, nE, XYP, IPF, IPE,
C                          ParCrv, iFnc, Sol, status, rW, iW
C
C       Output parameters: rQuality, iERR
C
C ================================================================
C
C  A possible choice for input parameters:
C       MaxP > nP
C       MaxF > nF
C       MaxE > nE
C       nPv >= 0
C       nFv >= 0
C       nEv >= 0
C       MaxSkipE = 100
C       MaxQItr = 15000
C       Quality = 0.6
C       MaxWr > 4 * MaxP + 10 * nP + MaxF + MaxE (approximate estimate)
C       MaxWi > 6 * MaxP + 10 * nP + 19 * MaxF + 11 * MaxE + 12 * nE
C       (approximately)
C
C *** Authors: K. Lipnikov (lipnikov@hotmail.com)
C              Y. Vassilevski (vasilevs@dodo.inm.ras.ru)
C ==========================================================
C group (M)
      Integer MaxP, MaxF, MaxE
      real  XYP(2, *)
      Integer IPE(3, *), IPF(4, *), IPV(*)

      EXTERNAL CrvFunction
      real   ParCrv(2, *)
      Integer  iFnc(*)

C group (D)
      Integer nFv, nEv
      Integer IFV(*), IEV(*), lbE(*)

      Logical flagAuto
      Integer status

C group (Q)
      Integer MaxSkipE, MaxQItr
      real  Metric(3, *)
      real  Quality, rQuality
      Logical flagAnalytic

C group (W)
      real  rW(*)
      Integer iW(*)

C group (Local variables)
      real  hStar

      Integer  MetricFunction_ani
      EXTERNAL MetricFunction_ani

C ==========================================================
C group (Common blocks)
      Integer iDomBnd, iMatBnd
      Common /aniBND/ iDomBnd, iMatBnd

      real  refXYP(2), scaXYP(2)
      Common /rescale/refXYP, scaXYP

C ==========================================================
      iERR = 0

      Do i = 1, 2
         refXYP(i) = 0D0
         scaXYP(i) = 1D0
      End do

c ... refine initial mesh when nE is very small
c ... it increases robustness of the code
      Do while(nE < nEStar / 15 .AND. nE.LE.500 .AND. nEv+nFv.EQ.0)
         iIFE = 1
         iiW  = iIFE + 3 * nE
         nWi  = iiW  + 3 * nE + nP
         If(nWi.GT.MaxWi) goto 100

         If(iPrint.GE.1) Write(*,5001) nP, nE

         Call uniformRefinement(
     &        nP, MaxP, nF, MaxF, nE, MaxE,
     &        XYP, IPF, IPE, lbE,
     &        CrvFunction, ParCrv, iFnc, iW(iIFE),
     &        Metric, 3, iW(iiW), MaxWi)
      End do

 100  miLINTRP = 10 * nP + 3 * nE + 6
      mrLINTRP =  4 * nP + MaxH + 4

      inEt = 1
      inStept = inEt + MaxF
      inL2t = inStept + 4 * MaxF
      iLFnc = inL2t + MaxF
      iILt  = iLFnc + MaxF
      iL1Et = iILt + MaxF
      iL2Et = iL1Et + 2 * MaxF
      iIHolP = iL2Et + 2 * MaxF
      iIHolF = iIHolP + MaxP
      iIHolE = iIHolF + MaxF
      iICP = iIHolE + MaxE
      iIEP = iICP + MaxP
      iIFE = iIEP + MaxP
      iIEE = iIFE + 3 * MaxE
      iL1E = iIEE + 3 * MaxE
      iL2E = iL1E + 2 * MaxE
      iIPEw = iL2E + 2 * MaxE
      iiSE  = iIPEw + 3 * nE
      iIEPw = iiSE + miLINTRP
c ... we need twice less memory for backReferences
      inEPw = iIEPw + max(6 * nE, 4 * MaxF)
      nWi   = inEPw + max(3 * MaxP, 2 * MaxF)

      itE = 1
      idG = itE + MaxF
      iqE = idG + MaxP
      iHesPw = iqE + MaxE
      iXYPw = iHesPw + 3 * nP
      irSE = iXYPw + 2 * nP
      nWr  = irSE + max(mrLINTRP, max(nE, MaxF))

      iW(1) = nWi
      iW(2) = nWr
      If(nWi.GT.MaxWi) Then
         iERR = 1001
         goto 1000
      End if

      If(nWr.GT.MaxWr) Then
         iERR = 1002
         goto 1000
      End if

      Do n = 1, nWr
         rW(n) = 0D0
      End do

      Do n = 1, nWi
         iW(n) = 0
      End do

c ... scale geometry to unit cube
      Call scale2Square(nP, XYP, .TRUE.)

c ... print Ani2D header
      If(iPrint.GE.1) Write(*, 5004) Quality, nEStar, MaxQItr

c ... set up default status
      Call setStatus(flagAuto, status, iPrint)

c ... call the main module
      flagAnalytic = .FALSE.
      Call ani2(
c group (M)
     &      nP, MaxP, nF, MaxF, nE, MaxE, nPv,
     &      XYP, IPF, IPE, IPV,
     &      CrvFunction, ParCrv, iFnc,
     &      nEStar, hStar,
     &      iW(iICP), iW(iIEP),
     &      iW(iIFE), iW(iIEE),
     &      iW(iL1E), iW(iL2E),
     &      iW(iIHolP), iW(iIHolF), iW(iIHolE),
     &      iW(iIEPw), iW(inEPw),
     &      iW(iIPEw), iW(iiSE),
c group (Dev)
     &      nFv, nEv, IFV, IEV, lbE,
     &      flagAuto, status,
c group (CRV)
     &      iW(iL1Et), iW(iL2Et), rW(itE),
     &      iW(inL2t), iW(inStept), iW(inEt),
     &      iW(iLFnc), iW(iILt),
c group (Q)
     &      MaxSkipE, MaxQItr, nQItr,
     &      Metric, Quality, rQuality,
     &      rW(idG), rW(iqE), rW(iXYPw), rW(iHesPw), rW(irSE),
     &      MetricFunction_ani, flagAnalytic,
c group (ERR)
     &      iPrint, iERR)

c ... rescale geometry back
      Call scale2Square(nP, XYP, .FALSE.)

c ... returning sadditional information
      Do n = 1, nP
         iW(n) = iW(iICP + n - 1)
      End do
      iW(nP + 1) = nQItr

      rW(1) = 0D0
      rW(2) = rQuality
      rW(3) = hStar

 1000 If(iERR.EQ.0 .OR. iERR.EQ.1000) Return
      Call errMes(iERR, 'mbaMetric',
     &            'See error.f for error description')

      Return

 5001 Format('Refining mesh:', I6, ' pts  and', I7, ' elements')

 5004 Format(/,
     &    'STONE FLOWER! (1997-2007), version 2.0', /,
     &    'Target: Quality', F5.2, ' with', I8,
     &    ' triangles for at most', I8, ' iterations')
      End subroutine

      end module mba2d_module


program sargo_inb
! compute inbreeding as in sargolzaei et al. 2005
! works from a reordered and renumbered pedigree
! computes inbreeding coefficient for all animals
implicit none
integer, parameter:: unped=15
integer:: n, i, k, maxgen, npar, c, nb_anim, nex
integer, allocatable:: ped(:,:), psigen(:), ispar(:), bgen(:)
integer, allocatable:: xrf(:), ped2(:,:), isex(:), ispar2(:)
real:: tot_inb, t1, t2, t11, t22
real, allocatable:: dd(:), inb(:), A22(:,:), dd2(:)
character*40:: fped

print *, 'name of pedigree?'
read (*,*) fped
call cpu_time(t1)
fped=trim(adjustl(fped))
open(unped, file=fped)
n=count_lines(unped)
allocate(ped(n,3),psigen(n),dd(n),inb(n),ispar(n),isex(n))
print *, '  reading pedigree and computing pseudo-generations ...'
do i=1,n
        read(unped,*) ped(i,1), ped(i,2), ped(i,3)
enddo
psigen=pseudo_gen(ped,n)
print *, '             ... done'
maxgen=maxval(psigen)+1
print '(a,i9)', ' # of animals in pedigree:', n
print '(a,i3)', ' # of pseudo-generations (including founders):', maxgen
allocate(bgen(maxgen))
bgen=bounds_gen(psigen,n,maxgen)
dd=1.d0; inb=0.d0
print *, ' working on first generations ...'
! animals from generation 0 : inb = 0
! animals from generation 1 : inb = 0, but dd can be <1
do i=1, n
        if (psigen(i)==0) cycle
        if (psigen(i)>1) exit
        if (ped(i,2)>0.and.ped(i,3)>0) dd(i)=0.5
        if (ped(i,2)>0.and.ped(i,3)==0) dd(i)=0.75
        if (ped(i,2)==0.and.ped(i,3)>0) dd(i)=0.75
enddo
print *, '             ... done'
print *, '---------------------------------------------------------------------'
print '(a)', '  PSEUDO-GEN   #ANIMALS   #PARENTS #EXTRACTED AVG_INBREEDING TIME[MIN]'
! animals from generation 2 to max: inb can be /= 0, dd can be <1
do k=3, maxgen
        ! pseudogeneration (=k-1)/ nb parents / nb offspring/ time /
        ! avg(inb)
        ! find the parents of generation k-1
        ! (those parents are in generation 0 (k=1)) to k-2
        call cpu_time(t11)
        allocate(xrf(bgen(k-1)))
        ispar=0; tot_inb=0.d0; nb_anim=0
        do i=1, n
                if (psigen(i)>k-1) exit
                if (psigen(i)<k-1) cycle
                if (ped(i,2)>0) ispar(ped(i,2))=1
                if (ped(i,3)>0) ispar(ped(i,3))=1
        enddo
        npar=sum(ispar)
        ! get cross-references for the parents
        c=1
        do i=1, bgen(k-1)
                if (ispar(i)==1) then
                        xrf(i)=c
                        c=c+1
                endif
        enddo
        ! extract the pedigree
        isex=ispar
        call extraction1(ped,isex,n)
        nex=sum(isex)
        allocate(ped2(nex,3),ispar2(nex))
        call extraction2(ped,isex,ispar,n,ped2,ispar2,nex)
        ! compute A22 between parents
        allocate(dd2(nex),A22(npar,npar))
        dd2=pack(dd,isex==1)
        call colleau3(ped2,ispar2,dd2,nex,A22,npar)
        ! get the inbreeding and dd for animals of generation k-1
        do i=1, n
                if (psigen(i)<k-1) cycle
                if (psigen(i)>k-1) exit
                if (ped(i,2)>0.and.ped(i,3)>0) then
                        inb(i)=.5*A22(xrf(ped(i,2)),xrf(ped(i,3)))
                        dd(i)=.5-.25*(inb(ped(i,2))+inb(ped(i,3)))
                elseif (ped(i,2)>0.and.ped(i,3)==0) then
                        dd(i)=.75-.25*inb(ped(i,2))
                elseif (ped(i,2)==0.and.ped(i,3)>0) then
                        dd(i)=.75-.25*inb(ped(i,3))
                endif
                tot_inb=tot_inb+inb(i)
                nb_anim=nb_anim+1
        enddo
        deallocate(ispar2,xrf,ped2,A22,dd2)
        call cpu_time(t22)
        print '(4i11,f12.4,f12.2)', k-1, nb_anim,  npar, nex, tot_inb/real(nb_anim), (t22-t11)/60
enddo
print *, '----------------------------------------------------------------------'
print *, ' DISTRIBUTION OF INBREEDING'
print '(i10,a,i10,a)', count(inb==0), ' non-inbred animals within', count(psigen<=1), ' in G0 and G1'
print *, '     #ANIMALS  FROM[%]  TO[%]'
do i=1, int(maxval(inb)*100)+1
        print '(3x,3i10)', count(inb>(real(i-1))/100.and.inb<=real(i)/100), i-1,i
enddo
print *, '----------------------------------------------------------------------'

print *, ' saving results ...'
! save results
open(30,file=fped(:len_trim(fped))//'.inb')
do i=1, n
        write(30,'(3i12,2f9.4,i4)') ped(i,1), ped(i,2), ped(i,3), inb(i), dd(i), psigen(i)
enddo
print *, '             ... done'
call cpu_time(t2)
print '(a,f10.2,a)', ' computations achieved in ', (t2-t1)/60,' min.'
print *, ' file *.inb contains pedigree, inbreeding, D-values & pseudo-generations'
close(30)

contains

function count_lines(un_file) result(n)
implicit none
integer:: un_file, n, k
character*1:: junk
rewind(un_file); n=0
do
        read(un_file,*,iostat=k) junk
        if (k/=0) exit
        n=n+1
enddo
rewind(un_file)
end function count_lines

function pseudo_gen(ped,n) result(psi)
implicit none
integer:: n, ped(n,3), psi(n), psip, psid
do i=1, n
        if (ped(i,2)==0.and.ped(i,3)==0) then
                psi(i)=0
        else
                psip=0; psid=0
                if (ped(i,2)>0) psip=psi(ped(i,2))
                if (ped(i,3)>0) psid=psi(ped(i,3))
                psi(i)=maxval((/psip, psid/))+1
        endif
enddo
end function

function bounds_gen(psigen,n,m) result(bgen)
! m = maxgen
implicit none
integer:: n, m, psigen(n), bgen(m), i
do i=2,n
        if (psigen(i-1)/=psigen(i)) bgen(psigen(i-1)+1)=i-1
enddo
bgen(m)=n
end function bounds_gen

subroutine extraction1(ped,isex,n)
implicit none
integer:: i, n, ped(n,3), isex(n)
do i=n,1,-1
        if (isex(i)==0) cycle
        if (ped(i,2)>0) isex(ped(i,2))=1
        if (ped(i,3)>0) isex(ped(i,3))=1
enddo
end subroutine extraction1

subroutine extraction2(ped,isex,ispar,n,ped2,ispar2,nex)
implicit none
integer:: n, nex, ped(n,3), isex(n), ispar(n), ped2(nex,3), ispar2(nex)
integer:: i, c
integer, allocatable:: xrf(:)
allocate(xrf(n))
ispar2=0; ped2=0; xrf=0; c=0
do i=1,n
        if (isex(i)==0) cycle
        c=c+1
        xrf(i)=c
        ped2(c,1)=c
        if (ped(i,2)>0) ped2(c,2)=xrf(ped(i,2))
        if (ped(i,3)>0) ped2(c,3)=xrf(ped(i,3))
        if (ispar(i)==1) ispar2(c)=1
enddo
end subroutine extraction2

subroutine colleau3(ped,isgen,d,n,A22,ng)
implicit none
integer:: i, k, n, ng, c, isgen(n), ped(n,3)
integer, allocatable:: inds(:)
real:: vs, vd, x, d(n), A22(ng,ng)
real, allocatable:: q(:), r(:), u(:), v(:)
allocate(inds(ng), u(n), q(n), r(n), v(n))
c=1
do i=1,n
        if (isgen(i)==1) then
                inds(c)=i
                c=c+1
        endif
enddo
! computation of A22
do k=1,ng
        q=0; v=0; r=0
        r(inds(k))=1
        do i=n, 1, -1
                q(i)=q(i)+r(i)
                if (ped(i,2)>0) q(ped(i,2))=q(ped(i,2))+q(i)/2
                if (ped(i,3)>0) q(ped(i,3))=q(ped(i,3))+q(i)/2
        enddo
        do i=1,n
                vs=0.0;vd=0.0
                if (ped(i,2)>0) vs=v(ped(i,2))
                if (ped(i,3)>0) vd=v(ped(i,3))
                v(i)=q(i)*d(i)+(vs+vd)/2
        enddo
        A22(:,k)=v(inds)
enddo
!A22=v(inds,:)
deallocate(v)

end subroutine colleau3

end program


!*==dtgsja.f90  processed by SPAG 7.51RB at 20:08 on  3 Mar 2022
!> \brief \b DTGSJA
!
!  =========== DOCUMENTATION ===========
!
! Online html documentation available at
!            http://www.netlib.org/lapack/explore-html/
!
!> \htmlonly
!> Download DTGSJA + dependencies
!>
!> [TGZ]
!>
!> [ZIP]
!>
!> [TXT]
!> \endhtmlonly
!
!  Definition:
!  ===========
!
!       SUBROUTINE DTGSJA( JOBU, JOBV, JOBQ, M, P, N, K, L, A, LDA, B,
!                          LDB, TOLA, TOLB, ALPHA, BETA, U, LDU, V, LDV,
!                          Q, LDQ, WORK, NCYCLE, INFO )
!
!       .. Scalar Arguments ..
!       CHARACTER          JOBQ, JOBU, JOBV
!       INTEGER            INFO, K, L, LDA, LDB, LDQ, LDU, LDV, M, N,
!      $                   NCYCLE, P
!       DOUBLE PRECISION   TOLA, TOLB
!       ..
!       .. Array Arguments ..
!       DOUBLE PRECISION   A( LDA, * ), ALPHA( * ), B( LDB, * ),
!      $                   BETA( * ), Q( LDQ, * ), U( LDU, * ),
!      $                   V( LDV, * ), WORK( * )
!       ..
!
!
!> \par Purpose:
!  =============
!>
!> \verbatim
!>
!> DTGSJA computes the generalized singular value decomposition (GSVD)
!> of two real upper triangular (or trapezoidal) matrices A and B.
!>
!> On entry, it is assumed that matrices A and B have the following
!> forms, which may be obtained by the preprocessing subroutine DGGSVP
!> from a general M-by-N matrix A and P-by-N matrix B:
!>
!>              N-K-L  K    L
!>    A =    K ( 0    A12  A13 ) if M-K-L >= 0;
!>           L ( 0     0   A23 )
!>       M-K-L ( 0     0    0  )
!>
!>            N-K-L  K    L
!>    A =  K ( 0    A12  A13 ) if M-K-L < 0;
!>       M-K ( 0     0   A23 )
!>
!>            N-K-L  K    L
!>    B =  L ( 0     0   B13 )
!>       P-L ( 0     0    0  )
!>
!> where the K-by-K matrix A12 and L-by-L matrix B13 are nonsingular
!> upper triangular; A23 is L-by-L upper triangular if M-K-L >= 0,
!> otherwise A23 is (M-K)-by-L upper trapezoidal.
!>
!> On exit,
!>
!>        U**T *A*Q = D1*( 0 R ),    V**T *B*Q = D2*( 0 R ),
!>
!> where U, V and Q are orthogonal matrices.
!> R is a nonsingular upper triangular matrix, and D1 and D2 are
!> ``diagonal'' matrices, which are of the following structures:
!>
!> If M-K-L >= 0,
!>
!>                     K  L
!>        D1 =     K ( I  0 )
!>                 L ( 0  C )
!>             M-K-L ( 0  0 )
!>
!>                   K  L
!>        D2 = L   ( 0  S )
!>             P-L ( 0  0 )
!>
!>                N-K-L  K    L
!>   ( 0 R ) = K (  0   R11  R12 ) K
!>             L (  0    0   R22 ) L
!>
!> where
!>
!>   C = diag( ALPHA(K+1), ... , ALPHA(K+L) ),
!>   S = diag( BETA(K+1),  ... , BETA(K+L) ),
!>   C**2 + S**2 = I.
!>
!>   R is stored in A(1:K+L,N-K-L+1:N) on exit.
!>
!> If M-K-L < 0,
!>
!>                K M-K K+L-M
!>     D1 =   K ( I  0    0   )
!>          M-K ( 0  C    0   )
!>
!>                  K M-K K+L-M
!>     D2 =   M-K ( 0  S    0   )
!>          K+L-M ( 0  0    I   )
!>            P-L ( 0  0    0   )
!>
!>                N-K-L  K   M-K  K+L-M
!> ( 0 R ) =    K ( 0    R11  R12  R13  )
!>           M-K ( 0     0   R22  R23  )
!>         K+L-M ( 0     0    0   R33  )
!>
!> where
!> C = diag( ALPHA(K+1), ... , ALPHA(M) ),
!> S = diag( BETA(K+1),  ... , BETA(M) ),
!> C**2 + S**2 = I.
!>
!> R = ( R11 R12 R13 ) is stored in A(1:M, N-K-L+1:N) and R33 is stored
!>     (  0  R22 R23 )
!> in B(M-K+1:L,N+M-K-L+1:N) on exit.
!>
!> The computation of the orthogonal transformation matrices U, V or Q
!> is optional.  These matrices may either be formed explicitly, or they
!> may be postmultiplied into input matrices U1, V1, or Q1.
!> \endverbatim
!
!  Arguments:
!  ==========
!
!> \param[in] JOBU
!> \verbatim
!>          JOBU is CHARACTER*1
!>          = 'U':  U must contain an orthogonal matrix U1 on entry, and
!>                  the product U1*U is returned;
!>          = 'I':  U is initialized to the unit matrix, and the
!>                  orthogonal matrix U is returned;
!>          = 'N':  U is not computed.
!> \endverbatim
!>
!> \param[in] JOBV
!> \verbatim
!>          JOBV is CHARACTER*1
!>          = 'V':  V must contain an orthogonal matrix V1 on entry, and
!>                  the product V1*V is returned;
!>          = 'I':  V is initialized to the unit matrix, and the
!>                  orthogonal matrix V is returned;
!>          = 'N':  V is not computed.
!> \endverbatim
!>
!> \param[in] JOBQ
!> \verbatim
!>          JOBQ is CHARACTER*1
!>          = 'Q':  Q must contain an orthogonal matrix Q1 on entry, and
!>                  the product Q1*Q is returned;
!>          = 'I':  Q is initialized to the unit matrix, and the
!>                  orthogonal matrix Q is returned;
!>          = 'N':  Q is not computed.
!> \endverbatim
!>
!> \param[in] M
!> \verbatim
!>          M is INTEGER
!>          The number of rows of the matrix A.  M >= 0.
!> \endverbatim
!>
!> \param[in] P
!> \verbatim
!>          P is INTEGER
!>          The number of rows of the matrix B.  P >= 0.
!> \endverbatim
!>
!> \param[in] N
!> \verbatim
!>          N is INTEGER
!>          The number of columns of the matrices A and B.  N >= 0.
!> \endverbatim
!>
!> \param[in] K
!> \verbatim
!>          K is INTEGER
!> \endverbatim
!>
!> \param[in] L
!> \verbatim
!>          L is INTEGER
!>
!>          K and L specify the subblocks in the input matrices A and B:
!>          A23 = A(K+1:MIN(K+L,M),N-L+1:N) and B13 = B(1:L,N-L+1:N)
!>          of A and B, whose GSVD is going to be computed by DTGSJA.
!>          See Further Details.
!> \endverbatim
!>
!> \param[in,out] A
!> \verbatim
!>          A is DOUBLE PRECISION array, dimension (LDA,N)
!>          On entry, the M-by-N matrix A.
!>          On exit, A(N-K+1:N,1:MIN(K+L,M) ) contains the triangular
!>          matrix R or part of R.  See Purpose for details.
!> \endverbatim
!>
!> \param[in] LDA
!> \verbatim
!>          LDA is INTEGER
!>          The leading dimension of the array A. LDA >= max(1,M).
!> \endverbatim
!>
!> \param[in,out] B
!> \verbatim
!>          B is DOUBLE PRECISION array, dimension (LDB,N)
!>          On entry, the P-by-N matrix B.
!>          On exit, if necessary, B(M-K+1:L,N+M-K-L+1:N) contains
!>          a part of R.  See Purpose for details.
!> \endverbatim
!>
!> \param[in] LDB
!> \verbatim
!>          LDB is INTEGER
!>          The leading dimension of the array B. LDB >= max(1,P).
!> \endverbatim
!>
!> \param[in] TOLA
!> \verbatim
!>          TOLA is DOUBLE PRECISION
!> \endverbatim
!>
!> \param[in] TOLB
!> \verbatim
!>          TOLB is DOUBLE PRECISION
!>
!>          TOLA and TOLB are the convergence criteria for the Jacobi-
!>          Kogbetliantz iteration procedure. Generally, they are the
!>          same as used in the preprocessing step, say
!>              TOLA = max(M,N)*norm(A)*MAZHEPS,
!>              TOLB = max(P,N)*norm(B)*MAZHEPS.
!> \endverbatim
!>
!> \param[out] ALPHA
!> \verbatim
!>          ALPHA is DOUBLE PRECISION array, dimension (N)
!> \endverbatim
!>
!> \param[out] BETA
!> \verbatim
!>          BETA is DOUBLE PRECISION array, dimension (N)
!>
!>          On exit, ALPHA and BETA contain the generalized singular
!>          value pairs of A and B;
!>            ALPHA(1:K) = 1,
!>            BETA(1:K)  = 0,
!>          and if M-K-L >= 0,
!>            ALPHA(K+1:K+L) = diag(C),
!>            BETA(K+1:K+L)  = diag(S),
!>          or if M-K-L < 0,
!>            ALPHA(K+1:M)= C, ALPHA(M+1:K+L)= 0
!>            BETA(K+1:M) = S, BETA(M+1:K+L) = 1.
!>          Furthermore, if K+L < N,
!>            ALPHA(K+L+1:N) = 0 and
!>            BETA(K+L+1:N)  = 0.
!> \endverbatim
!>
!> \param[in,out] U
!> \verbatim
!>          U is DOUBLE PRECISION array, dimension (LDU,M)
!>          On entry, if JOBU = 'U', U must contain a matrix U1 (usually
!>          the orthogonal matrix returned by DGGSVP).
!>          On exit,
!>          if JOBU = 'I', U contains the orthogonal matrix U;
!>          if JOBU = 'U', U contains the product U1*U.
!>          If JOBU = 'N', U is not referenced.
!> \endverbatim
!>
!> \param[in] LDU
!> \verbatim
!>          LDU is INTEGER
!>          The leading dimension of the array U. LDU >= max(1,M) if
!>          JOBU = 'U'; LDU >= 1 otherwise.
!> \endverbatim
!>
!> \param[in,out] V
!> \verbatim
!>          V is DOUBLE PRECISION array, dimension (LDV,P)
!>          On entry, if JOBV = 'V', V must contain a matrix V1 (usually
!>          the orthogonal matrix returned by DGGSVP).
!>          On exit,
!>          if JOBV = 'I', V contains the orthogonal matrix V;
!>          if JOBV = 'V', V contains the product V1*V.
!>          If JOBV = 'N', V is not referenced.
!> \endverbatim
!>
!> \param[in] LDV
!> \verbatim
!>          LDV is INTEGER
!>          The leading dimension of the array V. LDV >= max(1,P) if
!>          JOBV = 'V'; LDV >= 1 otherwise.
!> \endverbatim
!>
!> \param[in,out] Q
!> \verbatim
!>          Q is DOUBLE PRECISION array, dimension (LDQ,N)
!>          On entry, if JOBQ = 'Q', Q must contain a matrix Q1 (usually
!>          the orthogonal matrix returned by DGGSVP).
!>          On exit,
!>          if JOBQ = 'I', Q contains the orthogonal matrix Q;
!>          if JOBQ = 'Q', Q contains the product Q1*Q.
!>          If JOBQ = 'N', Q is not referenced.
!> \endverbatim
!>
!> \param[in] LDQ
!> \verbatim
!>          LDQ is INTEGER
!>          The leading dimension of the array Q. LDQ >= max(1,N) if
!>          JOBQ = 'Q'; LDQ >= 1 otherwise.
!> \endverbatim
!>
!> \param[out] WORK
!> \verbatim
!>          WORK is DOUBLE PRECISION array, dimension (2*N)
!> \endverbatim
!>
!> \param[out] NCYCLE
!> \verbatim
!>          NCYCLE is INTEGER
!>          The number of cycles required for convergence.
!> \endverbatim
!>
!> \param[out] INFO
!> \verbatim
!>          INFO is INTEGER
!>          = 0:  successful exit
!>          < 0:  if INFO = -i, the i-th argument had an illegal value.
!>          = 1:  the procedure does not converge after MAXIT cycles.
!> \endverbatim
!>
!> \verbatim
!>  Internal Parameters
!>  ===================
!>
!>  MAXIT   INTEGER
!>          MAXIT specifies the total loops that the iterative procedure
!>          may take. If after MAXIT cycles, the routine fails to
!>          converge, we return INFO = 1.
!> \endverbatim
!
!  Authors:
!  ========
!
!> \author Univ. of Tennessee
!> \author Univ. of California Berkeley
!> \author Univ. of Colorado Denver
!> \author NAG Ltd.
!
!> \date December 2016
!
!> \ingroup doubleOTHERcomputational
!
!> \par Further Details:
!  =====================
!>
!> \verbatim
!>
!>  DTGSJA essentially uses a variant of Kogbetliantz algorithm to reduce
!>  min(L,M-K)-by-L triangular (or trapezoidal) matrix A23 and L-by-L
!>  matrix B13 to the form:
!>
!>           U1**T *A13*Q1 = C1*R1; V1**T *B13*Q1 = S1*R1,
!>
!>  where U1, V1 and Q1 are orthogonal matrix, and Z**T is the transpose
!>  of Z.  C1 and S1 are diagonal matrices satisfying
!>
!>                C1**2 + S1**2 = I,
!>
!>  and R1 is an L-by-L nonsingular upper triangular matrix.
!> \endverbatim
!>
!  =====================================================================
      SUBROUTINE DTGSJA(Jobu,Jobv,Jobq,M,P,N,K,L,A,Lda,B,Ldb,Tola,Tolb, &
     &                  Alpha,Beta,U,Ldu,V,Ldv,Q,Ldq,Work,Ncycle,Info)
      IMPLICIT NONE
!*--DTGSJA381
!
!  -- LAPACK computational routine (version 3.7.0) --
!  -- LAPACK is a software package provided by Univ. of Tennessee,    --
!  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
!     December 2016
!
!     .. Scalar Arguments ..
      CHARACTER Jobq , Jobu , Jobv
      INTEGER Info , K , L , Lda , Ldb , Ldq , Ldu , Ldv , M , N ,      &
     &        Ncycle , P
      DOUBLE PRECISION Tola , Tolb
!     ..
!     .. Array Arguments ..
      DOUBLE PRECISION A(Lda,*) , Alpha(*) , B(Ldb,*) , Beta(*) ,       &
     &                 Q(Ldq,*) , U(Ldu,*) , V(Ldv,*) , Work(*)
!     ..
!
!  =====================================================================
!
!     .. Parameters ..
      INTEGER MAXIT
      PARAMETER (MAXIT=40)
      DOUBLE PRECISION ZERO , ONE
      PARAMETER (ZERO=0.0D+0,ONE=1.0D+0)
!     ..
!     .. Local Scalars ..
!
      LOGICAL initq , initu , initv , upper , wantq , wantu , wantv
      INTEGER i , j , kcycle
      DOUBLE PRECISION a1 , a2 , a3 , b1 , b2 , b3 , csq , csu , csv ,  &
     &                 error , gamma , rwk , snq , snu , snv , ssmin
!     ..
!     .. External Functions ..
      LOGICAL LSAME
      EXTERNAL LSAME
!     ..
!     .. External Subroutines ..
      EXTERNAL DCOPY , DLAGS2 , DLAPLL , DLARTG , DLASET , DROT ,       &
     &         DSCAL , XERBLA
!     ..
!     .. Intrinsic Functions ..
      INTRINSIC ABS , MAX , MIN
!     ..
!     .. Executable Statements ..
!
!     Decode and test the input parameters
!
      initu = LSAME(Jobu,'I')
      wantu = initu .OR. LSAME(Jobu,'U')
!
      initv = LSAME(Jobv,'I')
      wantv = initv .OR. LSAME(Jobv,'V')
!
      initq = LSAME(Jobq,'I')
      wantq = initq .OR. LSAME(Jobq,'Q')
!
      Info = 0
      IF ( .NOT.(initu .OR. wantu .OR. LSAME(Jobu,'N')) ) THEN
         Info = -1
      ELSEIF ( .NOT.(initv .OR. wantv .OR. LSAME(Jobv,'N')) ) THEN
         Info = -2
      ELSEIF ( .NOT.(initq .OR. wantq .OR. LSAME(Jobq,'N')) ) THEN
         Info = -3
      ELSEIF ( M<0 ) THEN
         Info = -4
      ELSEIF ( P<0 ) THEN
         Info = -5
      ELSEIF ( N<0 ) THEN
         Info = -6
      ELSEIF ( Lda<MAX(1,M) ) THEN
         Info = -10
      ELSEIF ( Ldb<MAX(1,P) ) THEN
         Info = -12
      ELSEIF ( Ldu<1 .OR. (wantu .AND. Ldu<M) ) THEN
         Info = -18
      ELSEIF ( Ldv<1 .OR. (wantv .AND. Ldv<P) ) THEN
         Info = -20
      ELSEIF ( Ldq<1 .OR. (wantq .AND. Ldq<N) ) THEN
         Info = -22
      ENDIF
      IF ( Info/=0 ) THEN
         CALL XERBLA('DTGSJA',-Info)
         RETURN
      ENDIF
!
!     Initialize U, V and Q, if necessary
!
      IF ( initu ) CALL DLASET('Full',M,M,ZERO,ONE,U,Ldu)
      IF ( initv ) CALL DLASET('Full',P,P,ZERO,ONE,V,Ldv)
      IF ( initq ) CALL DLASET('Full',N,N,ZERO,ONE,Q,Ldq)
!
!     Loop until convergence
!
      upper = .FALSE.
      DO kcycle = 1 , MAXIT
!
         upper = .NOT.upper
!
         DO i = 1 , L - 1
            DO j = i + 1 , L
!
               a1 = ZERO
               a2 = ZERO
               a3 = ZERO
               IF ( K+i<=M ) a1 = A(K+i,N-L+i)
               IF ( K+j<=M ) a3 = A(K+j,N-L+j)
!
               b1 = B(i,N-L+i)
               b3 = B(j,N-L+j)
!
               IF ( upper ) THEN
                  IF ( K+i<=M ) a2 = A(K+i,N-L+j)
                  b2 = B(i,N-L+j)
               ELSE
                  IF ( K+j<=M ) a2 = A(K+j,N-L+i)
                  b2 = B(j,N-L+i)
               ENDIF
!
               CALL DLAGS2(upper,a1,a2,a3,b1,b2,b3,csu,snu,csv,snv,csq, &
     &                     snq)
!
!              Update (K+I)-th and (K+J)-th rows of matrix A: U**T *A
!
               IF ( K+j<=M ) CALL DROT(L,A(K+j,N-L+1),Lda,A(K+i,N-L+1), &
     &                                 Lda,csu,snu)
!
!              Update I-th and J-th rows of matrix B: V**T *B
!
               CALL DROT(L,B(j,N-L+1),Ldb,B(i,N-L+1),Ldb,csv,snv)
!
!              Update (N-L+I)-th and (N-L+J)-th columns of matrices
!              A and B: A*Q and B*Q
!
               CALL DROT(MIN(K+L,M),A(1,N-L+j),1,A(1,N-L+i),1,csq,snq)
!
               CALL DROT(L,B(1,N-L+j),1,B(1,N-L+i),1,csq,snq)
!
               IF ( upper ) THEN
                  IF ( K+i<=M ) A(K+i,N-L+j) = ZERO
                  B(i,N-L+j) = ZERO
               ELSE
                  IF ( K+j<=M ) A(K+j,N-L+i) = ZERO
                  B(j,N-L+i) = ZERO
               ENDIF
!
!              Update orthogonal matrices U, V, Q, if desired.
!
               IF ( wantu .AND. K+j<=M )                                &
     &              CALL DROT(M,U(1,K+j),1,U(1,K+i),1,csu,snu)
!
               IF ( wantv ) CALL DROT(P,V(1,j),1,V(1,i),1,csv,snv)
!
               IF ( wantq ) CALL DROT(N,Q(1,N-L+j),1,Q(1,N-L+i),1,csq,  &
     &                                snq)
!
            ENDDO
         ENDDO
!
         IF ( .NOT.upper ) THEN
!
!           The matrices A13 and B13 were lower triangular at the start
!           of the cycle, and are now upper triangular.
!
!           Convergence test: test the parallelism of the corresponding
!           rows of A and B.
!
            error = ZERO
            DO i = 1 , MIN(L,M-K)
               CALL DCOPY(L-i+1,A(K+i,N-L+i),Lda,Work,1)
               CALL DCOPY(L-i+1,B(i,N-L+i),Ldb,Work(L+1),1)
               CALL DLAPLL(L-i+1,Work,1,Work(L+1),1,ssmin)
               error = MAX(error,ssmin)
            ENDDO
!
            IF ( ABS(error)<=MIN(Tola,Tolb) ) GOTO 100
         ENDIF
!
!        End of cycle loop
!
      ENDDO
!
!     The algorithm has not converged after MAXIT cycles.
!
      Info = 1
      GOTO 200
!
!
!     If ERROR <= MIN(TOLA,TOLB), then the algorithm has converged.
!     Compute the generalized singular value pairs (ALPHA, BETA), and
!     set the triangular matrix R to array A.
!
 100  DO i = 1 , K
         Alpha(i) = ONE
         Beta(i) = ZERO
      ENDDO
!
      DO i = 1 , MIN(L,M-K)
!
         a1 = A(K+i,N-L+i)
         b1 = B(i,N-L+i)
!
         IF ( a1/=ZERO ) THEN
            gamma = b1/a1
!
!           change sign if necessary
!
            IF ( gamma<ZERO ) THEN
               CALL DSCAL(L-i+1,-ONE,B(i,N-L+i),Ldb)
               IF ( wantv ) CALL DSCAL(P,-ONE,V(1,i),1)
            ENDIF
!
            CALL DLARTG(ABS(gamma),ONE,Beta(K+i),Alpha(K+i),rwk)
!
            IF ( Alpha(K+i)>=Beta(K+i) ) THEN
               CALL DSCAL(L-i+1,ONE/Alpha(K+i),A(K+i,N-L+i),Lda)
            ELSE
               CALL DSCAL(L-i+1,ONE/Beta(K+i),B(i,N-L+i),Ldb)
               CALL DCOPY(L-i+1,B(i,N-L+i),Ldb,A(K+i,N-L+i),Lda)
            ENDIF
!
         ELSE
!
            Alpha(K+i) = ZERO
            Beta(K+i) = ONE
            CALL DCOPY(L-i+1,B(i,N-L+i),Ldb,A(K+i,N-L+i),Lda)
!
         ENDIF
!
      ENDDO
!
!     Post-assignment
!
      DO i = M + 1 , K + L
         Alpha(i) = ZERO
         Beta(i) = ONE
      ENDDO
!
      IF ( K+L<N ) THEN
         DO i = K + L + 1 , N
            Alpha(i) = ZERO
            Beta(i) = ZERO
         ENDDO
      ENDIF
!
 200  Ncycle = kcycle
!
!     End of DTGSJA
!
      END SUBROUTINE DTGSJA


module Wave_Packet_Simulation

    use iso_fortran_env, only : REAL64
    use Wavefunc_DataStorage
    use omp_lib
    implicit none

    contains

    ! Simple wave packet centered at (0,0) with momentum px, py and width sigma
    pure function start_wave_packet(x, y, px, py, sigma, hbar, pi) result (psi_at_point)
        REAL(kind=REAL64), intent(in) :: x, y, px, py, sigma, hbar, pi
        COMPLEX(kind=REAL64) :: psi_at_point

        REAL(kind=REAL64) :: coeff_val
        COMPLEX(kind=REAL64) :: exp_val

        !coeff_val = sqrt(sigma *  sqrt(pi))
        !exp_val = (px * x + py * y) * (0, 1.0)/hbar - (x * x + y * y)/(2 * sigma * sigma)

        !psi_at_point = coeff_val * exp(exp_val)

        if (x .eq. 0 .and. y .eq. 0) then
            psi_at_point = 1
        else
            psi_at_point = 0
        end if

    end function start_wave_packet

    ! The first step iteration, since we don't have access to t-s at that point
    pure function first_iteration_at_point(xind, yind, s, d, hbar, m, curV, curPsi) result(psi_next)
        Integer, intent(in) :: xind, yind
        Real(kind=REAL64), intent(in) :: s, d, hbar, m, curV
        Complex(kind=REAL64), intent(in), dimension(:, :) :: curPsi

        Complex(kind=REAL64) :: psi_next

        Complex(kind=REAL64) :: offPsi, offPsi_coeff, psiV_coeff, psi_coeff, curPsiPoint
        Integer, dimension(2) :: realitySize

        psi_coeff = 1 - (0, -2) *hbar * s/(m * d * d)
        psiV_coeff = (0, -1) * s /hbar
        offPsi_coeff = (0, 0.5) * hbar * s/(m * d * d)

        curPsiPoint = curPsi(xind, yind)

        ! We default the value of Psi to 0 at the borders, since we are assuming
        ! a particle in a finite box.
        realitySize = shape(curPsi)
        if (xind .gt. 1 .and. xind .lt. realitySize(1)) then
            offPsi = curPsi(xind + 1, yind) + curPsi(xind - 1, yind)
        else if(xind .lt. realitySize(1)) then
            offPsi = curPsi(xind + 1, yind) + 0
        else
            offPsi = curPsi(xind - 1, yind) + 0
        end if

        ! Same for y
        if (yind .gt. 1 .and. yind .lt. realitySize(2)) then
            offPsi = offPsi + curPsi(xind, yind + 1) + curPsi(xind, yind - 1)
        else if(yind .lt. realitySize(2)) then
            offPsi = offPsi + curPsi(xind, yind + 1) + 0
        else
            offPsi = offPsi + curPsi(xind, yind - 1) + 0
        end if

        psi_next = curPsiPoint * psi_coeff + curPsiPoint * curV * psiV_coeff + offPsi * offPsi_coeff

    end function first_iteration_at_point

    ! Get the coefficient for the \Psi(x, y, t) term
    pure function calc_psi_coeff(m, hbar, s, d) result(retval)
        REAL(kind=REAL64), intent(in) :: m, hbar, s, d
        COMPLEX(kind=REAL64) :: retval

        retval = (-4.0, 1.0) * s * hbar / (m * d * d)

    end function calc_psi_coeff

    ! Get the coefficient for the \V(x, y, t) * \Psi(x, y, t) term
    pure function calc_psiV_coeff(m, hbar, s, d) result(retval)
        REAL(kind=REAL64), intent(in) :: m, hbar, s, d
        COMPLEX(kind=REAL64) :: retval

        retval = (-2.0, 1.0) * s/hbar

    end function calc_psiV_coeff

    ! Get the coefficient for the gradient components summation term
    pure function calc_psiOff_coeff(m, hbar, s, d) result(retval)
        REAL(kind=REAL64), intent(in) :: m, hbar, s, d
        COMPLEX(kind=REAL64) :: retval

        retval = hbar * s * (0.0, 1.0) / (m * d * d)
    end function calc_psiOff_coeff

    ! General function to iterate the wave at a point
    pure function iterate_psi_at_point(xind, yind, psi_coeff, psiV_coeff, psiOff_coeff, curV, psi_prev, psi_cur) result(psi_next)
        INTEGER, intent(in) :: xind, yind
        REAL(kind=REAL64), intent(in) :: curV
        COMPLEX(kind=REAL64), intent(in) :: psi_coeff, psiV_coeff, psiOff_coeff
        Complex(kind=REAL64), dimension(:, :), intent(in) :: psi_prev, psi_cur
        COMPLEX(kind=REAL64) :: psi_next

        COMPLEX(kind=REAL64) :: offPsi
        Integer, dimension(2) :: realitySize

        ! We default the value of Psi to 0 at the borders, since we are assuming
        ! a particle in a finite box.
        realitySize = shape(psi_cur)
        if (xind .gt. 1 .and. xind .lt. realitySize(1)) then
            offPsi = psi_cur(xind + 1, yind) + psi_cur(xind - 1, yind)
        else if(xind .lt. realitySize(1)) then
            offPsi = psi_cur(xind + 1, yind) + 0
        else
            offPsi = psi_cur(xind - 1, yind) + 0
        end if

        ! Same for y
        if (yind .gt. 1 .and. yind .lt. realitySize(2)) then
            offPsi = offPsi + psi_cur(xind, yind + 1) + psi_cur(xind, yind - 1)
        else if(yind .lt. realitySize(2)) then
            offPsi = offPsi + psi_cur(xind, yind + 1) + 0
        else
            offPsi = offPsi + psi_cur(xind, yind - 1) + 0
        end if

        psi_next = psi_prev(xind, yind) + (psi_coeff + curV * psiV_coeff) * psi_cur(xind, yind) + psiOff_coeff * offPsi

    end function iterate_psi_at_point

    ! Normalizes the wave function data, so sum(abs(matrix)**2) = 1.
    subroutine normalize_matrix(matrix)
        COMPLEX(kind=REAL64), intent(inout), dimension(:, :) :: matrix

        INTEGER, dimension(2) :: dims
        REAL(kind=REAL64) :: sum_squares
        INTEGER :: xind, yind

        dims = SHAPE(matrix)
        sum_squares = 0.0

        ! Get the normalization coefficient at that time
        ! $OMP parallel do default(shared) private(xind, yind) reduction (+:sum_squares)
        do xind = 1, dims(1)
            do concurrent (yind = 1:dims(2))
                if (matrix(xind, yind) .ne. 0) then
                    sum_squares = sum_squares + 1D100 * custom_abs(matrix(xind, yind))**2
                end if
            end do
        end do
        ! $OMP end parallel do

        sum_squares = sum_squares /1D100

        ! Short Circuit on a bad universe
        if (sum_squares .eq. 0) then
            Print *, "Bad universe. "
            return
        else if(abs(sum_squares - 1) .lt. 0.0001) then
            Print *, "SPICY! Sumsq = ", sum_squares
        end if

        ! Get the normalization coefficient at that time

        ! $OMP parallel do default(shared) private(xind, yind) shared(sum_squares, matrix)
        do xind = 1, dims(1)
            do concurrent (yind = 1:dims(2))
                if (matrix(xind, yind) .ne. 0) then
                    matrix(xind, yind) = matrix(xind, yind) / sqrt(sum_squares)
                end if
            end do
        end do
        ! $OMP end parallel do

    end subroutine

end module Wave_Packet_Simulation

program main

    use Wave_Packet_Simulation

    implicit none

    ! Constants of reality
    REAL(kind=REAL64), parameter :: PI  = 4.D0 * DATAN(1.D0) ! Gurantees max accuracy
    REAL(kind=REAL64), parameter :: hbar = 1.054571800D-34!J * s

    ! Constants of this simulation

    ! Data storage variables
    type(DataStore) ::  storage
    Integer, parameter :: timesize = 200, points_per_time = 100000, storage_sz = points_per_time * timesize
    Integer, parameter :: max_points = 400000
    Integer, parameter :: xsize = 1000, ysize = 1000 ! Universe size
    COMPLEX(kind=REAL64), dimension(:, :), allocatable, target :: universe_a, universe_b, universe_c ! The closest time slices
    COMPLEX(kind=REAL64), dimension(:, :), pointer :: prev_data, curr_data, next_data, tmp_ptr ! Pointers to each slice to more efficiently rotate

    ! Universe bounds
    REAL(kind=REAL64), parameter :: xmin = -1.0, xmax = 1.0, ymin = -1.0, ymax = 1.0
    REAL(kind=REAL64), parameter :: tmax = 1.0
    REAL(kind=REAL64), parameter :: min_percent = 1D-6

    REAL(kind=REAL64), parameter :: s = (tmax)/timesize ! time step, seconds
    REAL(kind=REAL64), parameter :: d = MAX((xmax - xmin)/xsize, (ymax -ymin)/ysize) ! distance step, meters; equal to equal to (xmax - xmin)/xsize

    REAL(kind=REAL64), parameter :: px = 1.0, py = 0 ! initial momentum, m/s
    REAL(kind=REAL64), parameter :: sigma = 4D-4 ! the width in meters?

    REAL(kind=REAL64), parameter :: m = 9.10938356D-31 ! Mass of particle (electron here) in kg

    Integer, parameter :: fd = 1 ! Output CSV file descriptor

    ! Local variables
    Integer :: xind, yind, tind ! Iteration indices
    REAL(kind=REAL64) :: x, y, t ! Temp variables for converting from the index
    REAL(kind=REAL64) :: curV ! Temp variable for the potential at a point
    COMPLEX(kind=REAL64) :: psi_coeff, psiV_coeff, psi_off_coeff ! Coefficients for the iterator
    type(TimeNode), pointer :: data_idx

    storage%size = 0
    Nullify(storage%head)
    Nullify(storage%tail)

    allocate(universe_a(xsize, ysize))
    allocate(universe_b(xsize, ysize))
    allocate(universe_c(xsize, ysize))

    Print *, "Hello!"
    Print *, "Going from t=0 to t=", timesize * s, " with steps of ", s, &
             " over a region of [",xmin, ", ", xmin + d*xsize, "] x [", ymin, ", ", ymin + d*ysize, &
             "] with steps of ", d

    tmp_ptr => universe_a
    prev_data => universe_a ! The universe at time t+s
    curr_data => universe_b ! The universe at time t
    next_data => universe_c ! The universe at time t+s

    !$omp parallel do default(shared) private(x, y, xind, yind)
    do xind = 1,xsize
        x = (xind - 1) * d + xmin
        do concurrent (yind = 1:ysize)
            y = (yind - 1) * d + ymin
            prev_data(xind, yind) = start_wave_packet(x, y, px, py, sigma, hbar, pi)
        end do
    end do
    !$omp end parallel do

    call normalize_matrix(prev_data)
    call store_data(storage, prev_data, 0D0, d, xmin, ymin, max_points, min_percent)

    Print *, "Finished t = ", 0, " step with ", storage%size, " total points."

    !$omp parallel do default(shared) private(x, y, xind, yind, curV)
    do xind = 1,xsize
        x = (xind - 1) * d + xmin
        do concurrent (yind = 1:ysize)
            y = (yind - 1) * d + ymin
            curV = V(x, y, s)
            curr_data(xind, yind) = first_iteration_at_point(xind, yind, s, d, hbar, m, curV, prev_data)
        end do
    end do
    !$omp end parallel do

    call normalize_matrix(curr_data)

    call store_data(storage, curr_data, s, d, xmin, ymin, max_points, min_percent)

    Print *, "Finished t = ", 1, " step with ", storage%size, " total points."

    psi_coeff = calc_psi_coeff(m, hbar, s, d)
    psiV_coeff = calc_psiV_coeff(m, hbar, s, d)
    psi_off_coeff = calc_psiOff_coeff(m, hbar, s, d)

    tind = 3
    t = s
    do while(storage%size .le. storage_sz .and. t .lt. tmax)
        t = (tind - 1) * s

        !$omp parallel do default(shared) private(x, y, xind, yind, curV)
        do xind = 1,xsize
            x = (xind - 1) * d + xmin
            do concurrent (yind = 1:ysize)
                y = (yind - 1) * d + ymin
                curV = V(x, y, t)
                next_data(xind, yind) = iterate_psi_at_point( &
                    xind, yind, psi_coeff, psiV_coeff, psi_off_coeff, curV,  &
                    prev_data, curr_data &
                )
            end do
        end do
        !$omp end parallel do

        call normalize_matrix(next_data)
        call store_data(storage, next_data, t, d, xmin, ymin, max_points, min_percent)
        tmp_ptr => prev_data
        prev_data => curr_data
        curr_data => next_data
        next_data => tmp_ptr
        if(MOD(tind-1, 100) .eq. 0) then
            Print *, "Finished t = ", tind-1, " step with ", storage%size, " total data points."
        end if
        tind = tind + 1
    end do

    ! Write the data to a disk
    ! To save space we only write the non-zero data, not the entire
    ! universe at every time point

    open(fd, file="Simulationdata.csv")

    data_idx => storage%head
    do while(ASSOCIATED(data_idx))

        do xind = 1, size(data_idx%data)
            Write (fd, *) data_idx%t, ", ", &
                        data_idx%data(xind)%x, ", ", &
                        data_idx%data(xind)%y, ", ", &
                        real(data_idx%data(xind)%value), ",", &
                        aimag(data_idx%data(xind)%value)
        end do
        data_idx => data_idx%next
    end do

    deallocate(universe_a)
    deallocate(universe_b)
    deallocate(universe_c)

    contains

    ! The potential function
    pure function V(x, y, t) result(retval)
        REAL(kind=REAL64), intent(in) :: x, y, t
        REAL(kind=REAL64) :: retval

        ! BIG potential at the edges of the box
        if (t .ge. 0 .and. (x .gt. 1 .or. y .gt. 1 .or. x .lt. -1 .or. y .lt. -1)) then
            retval = 9D99
        else

            ! Right now we have no potential inside
            retval = 0
        end if

    end function V

end program main


      double precision function virtgamgam(s,t,u)
      implicit none
      include 'epinv.f'
      include 'constants.f'
      include 'scale.f'
      include 'qcdcouple.f'
      include 'nflav.f'
      integer h1,h2,h3,h4
      double precision temp,s,t,u
      double complex m1(2,2,2,2),m2fin(2,2,2,2),lnrat,
     & I1ggtogamgam,xlog,FL(2,2,2,2),FSL(2,2,2,2)

      xlog=lnrat(musq,-s)
c--- This is taken from hep-ph/0109078 Eq.(2.11); note however that the log
c--- proportional to the beta-function coefficient is added in Eq. (2.11)
c--- and subtracted again in Eq. (4.5), therefore we omit it here.
      I1ggtogamgam=
     & -xn*((epinv**2+epinv*xlog+0.5d0*xlog**2)
     & +(11d0/6d0-dfloat(nflav)/(3d0*xn))*epinv)

      temp=0d0
      call M1fill(s,t,u,m1)

c--- testing the 1-loop matrix element contribution up to Order(ep^2)
c      call oneloopep(s,t,u,IxM1ep)

c--- pure 2-loop contribution
      call twoloop(s,t,u,FL,FSL)

c--- Compare m1 with m1ep
c      do h1=1,2
c      do h2=1,2
c      do h3=1,2
c      do h4=1,2
c      write(6,*) h1,h2,h3,h4,m1(h1,h2,h3,h4)*I1ggtogamgam,
c     &           IxM1ep(h1,h2,h3,h4)
c      enddo
c      enddo
c      enddo
c      enddo
c      pause

      do h1=1,2
      do h2=1,2
      do h3=1,2
      do h4=1,2
      m2fin(h1,h2,h3,h4)=xn*FL(h1,h2,h3,h4)-FSL(h1,h2,h3,h4)/xn
      temp=temp+2d0*ason2pi*dble(dconjg(m1(h1,h2,h3,h4))
     & *(m1(h1,h2,h3,h4)*I1ggtogamgam+m2fin(h1,h2,h3,h4)))
c      temp=temp+2d0*ason2pi*dble(dconjg(m1(h1,h2,h3,h4))
c     & *(IxM1ep(h1,h2,h3,h4)+m2fin(h1,h2,h3,h4)))
      enddo
      enddo
      enddo
      enddo

      virtgamgam=temp

      return
      end


      subroutine qdrift(trans,cod,al,coup)
      use mathfun, only:sqrtl
      implicit none
      real*8 ,intent(out):: trans(4,5)
      real*8 ,intent(inout):: cod(6)
      real*8 ,intent(in):: al
      real*8 pxi,pyi,pxisq,pyisq,pzi,ale,alz
      logical*4 ,intent(out):: coup
      pxi=cod(2)
      pyi=cod(4)
      pxisq=pxi**2
      pyisq=pyi**2
      pzi=sqrtl(1.d0-pxisq-pyisq)
      ale=al/pzi
      alz=ale/pzi**2
      cod(1)=cod(1)+pxi*ale
      cod(3)=cod(3)+pyi*ale
      trans(1,1)=1.d0
      trans(1,2)=ale+pxisq*alz
      trans(1,3)=0.d0
      trans(1,4)=pxi*pyi*alz
      trans(1,5)=0.d0
      trans(2,1)=0.d0
      trans(2,2)=1.d0
      trans(2,3)=0.d0
      trans(2,4)=0.d0
      trans(2,5)=0.d0
      trans(3,1)=0.d0
      trans(3,2)=trans(1,4)
      trans(3,3)=1.d0
      trans(3,4)=ale+pyisq*alz
      trans(3,5)=0.d0
      trans(4,1)=0.d0
      trans(4,2)=0.d0
      trans(4,3)=0.d0
      trans(4,4)=1.d0
      trans(4,5)=0.d0
      coup=.true.
      return
      end

      subroutine qtest(trans,cod,al,angle,coup)
      implicit none
      real*8 trans(4,5),cod(6),al
      real*8 angle,rho
      logical coup

      rho    = al/angle

      cod(1)=cod(1)*cos(angle)+cod(2)*rho*cos(angle)
     $ +rho*(1-cos(angle))*cod(6)
      cod(2)=-sin(angle)/rho*cod(1)+cod(2)*cos(angle)
     $ +sin(angle)*cod(6)
      cod(3)=cod(3)+cod(4)*al
      cod(4)=cod(4)

      write(*,*)'qtest ',al,angle,rho,cod

      trans(1,1)=cos(angle)
      trans(1,2)=rho*sin(angle)
      trans(1,3)=0.d0
      trans(1,4)=0.d0
      trans(1,5)=rho*(1-cos(angle))
      trans(2,1)=-sin(angle)/rho
      trans(2,2)=cos(angle)
      trans(2,3)=0.d0
      trans(2,4)=0.d0
      trans(2,5)=sin(angle)
      trans(3,1)=0.d0
      trans(3,2)=0.d0
      trans(3,3)=1.d0
      trans(3,4)=al
      trans(3,5)=0.d0
      trans(4,1)=0.d0
      trans(4,2)=0.d0
      trans(4,3)=0.d0
      trans(4,4)=1.d0
      trans(4,5)=0.d0
      coup=.true.
      return
      end


      COMPLEX*16 FUNCTION C0IR_Wdec(tHla2,M12,M22,P2)
*     ------------------------------------------
* All cases of C0IR:
*
      IMPLICIT NONE
      INTEGER*4 Lim
      REAL*8 tHla2,P2,DRM12,DRM22,D2,DREAL
      COMPLEX*16 M12,M22,SQR,P2P,P2M,X1,X2,OMX1,OMX2,F1
      COMPLEX*16 LOG,SQRT,DCMPLX,XSPENZ_Wdec
      PARAMETER (D2=1.64493406684822618D0)
*
      DRM12=DREAL(M12)
      DRM22=DREAL(M22)
*
* Limiting cases
*
      IF(DRM12.LE.1D-2.AND.DRM22.LE.1D-2) THEN
* M1.EQ.M2
       IF(DRM12.EQ.DRM22) THEN
        C0IR_Wdec=1D0/P2*(LOG(M12/tHla2)*LOG((P2-DCMPLX(0D0,1D-20))/M12)
     &      +1D0/2D0*(LOG((P2-DCMPLX(0D0,1D-20))/M12))**2-D2)
        RETURN
* M1.NE.M2
       ELSE
        C0IR_Wdec=1D0/2/P2*(
     &      +(LOG(P2/M12)+LOG(P2/M22))*LOG((P2-DCMPLX(0D0,1D-20))/tHla2)
     &      -.5D0*LOG(M12/P2)**2-.5D0*LOG(M22/P2)**2-2D0*D2)
        RETURN
       ENDIF
      ENDIF
*
* Special case for W->\nu\lept decay
*
      IF(DABS(P2).LT.1D-10) THEN
        C0IR_Wdec=1D0/4/DREAL(M12-M22)*DLOG(DRM12/DRM22)
     &       *DLOG(DRM12*DRM22/tHla2**2)
        RETURN
      ENDIF
*
* General, exact case of C0IR
*
      SQR=SQRT(P2**2+2D0*P2*(M12+M22)+(M12-M22)**2)
      P2P=P2-M12+M22+SQR
      P2M=P2-M12+M22-SQR
      IF(DABS(DREAL(P2P)).GE.DABS(DREAL(P2M))) THEN
        X1=P2P/2D0/P2
        X2=-2D0*M12/P2P
      ELSE
        X2=P2M/2D0/P2
        X1=-2D0*M12/P2M
      ENDIF
      P2P=P2+M12-M22+SQR
      P2M=P2+M12-M22-SQR
      IF(DABS(DREAL(P2P)).GE.DABS(DREAL(P2M))) THEN
        OMX2=P2P/2D0/P2
        OMX1=-2D0*M22/P2P
      ELSE
        OMX1=P2M/2D0/P2
        OMX2=-2D0*M22/P2M
      ENDIF
*
      F1=1D0/SQR*(LOG(OMX2/(-X2))-LOG(OMX1/(-X1)))
*
      C0IR_Wdec=1D0/2*F1*LOG((P2-DCMPLX(0D0,1D-20))/tHla2)
     &    +1D0/2/SQR*(
     & +.5D0*LOG( OMX2*(X1-X2))**2
     & -.5D0*LOG(  -X2*(X1-X2))**2
     & -.5D0*LOG(-OMX1*(X1-X2))**2
     & +.5D0*LOG(   X1*(X1-X2))**2
     & -XSPENZ_Wdec(OMX2/(X1-X2))+XSPENZ_Wdec((-X2)/(X1-X2))
     & +XSPENZ_Wdec(OMX1/(X2-X1))-XSPENZ_Wdec((-X1)/(X2-X1))
     &               )
*
      RETURN
      END


c
c
c     ###################################################
c     ##  COPYRIGHT (C)  1990  by  Jay William Ponder  ##
c     ##              All Rights Reserved              ##
c     ###################################################
c
c     ################################################################
c     ##                                                            ##
c     ##  subroutine ebond2  --  atom-by-atom bond stretch Hessian  ##
c     ##                                                            ##
c     ################################################################
c
c
c     "ebond2" calculates second derivatives of the bond
c     stretching energy for a single atom at a time
c
c
      subroutine ebond2 (i)
      implicit none
      include 'sizes.i'
      include 'atmlst.i'
      include 'atoms.i'
      include 'bndpot.i'
      include 'bond.i'
      include 'bound.i'
      include 'couple.i'
      include 'group.i'
      include 'hessn.i'
      integer i,j,k,ia,ib
      real*8 ideal,force,fgrp
      real*8 xab,yab,zab
      real*8 rab,rab2
      real*8 expterm,bde
      real*8 dt,dt2,term
      real*8 termx,termy,termz
      real*8 de,deddt,d2eddt2
      real*8 d2e(3,3)
      logical proceed
c
c
c     compute the Hessian elements of the bond stretch energy
c
      ia = i
      do k = 1, n12(ia)
         j = bndlist(k,ia)
         if (ibnd(1,j) .eq. ia) then
            ib = ibnd(2,j)
         else
            ib = ibnd(1,j)
         end if
         ideal = bl(j)
         force = bk(j)
c
c     decide whether to compute the current interaction
c
         proceed = .true.
         if (use_group)  call groups (proceed,fgrp,ia,ib,0,0,0,0)
c
c     compute the value of the bond length deviation
c
         if (proceed) then
            xab = x(ia) - x(ib)
            yab = y(ia) - y(ib)
            zab = z(ia) - z(ib)
            if (use_polymer)  call image (xab,yab,zab)
            rab2 = xab*xab + yab*yab + zab*zab
            rab = sqrt(rab2)
            dt = rab - ideal
c
c     harmonic potential uses Taylor expansion of Morse potential
c     through the fourth power of the bond length deviation
c
            if (bndtyp .eq. 'HARMONIC') then
               dt2 = dt * dt
               deddt = 2.0d0 * bndunit * force * dt
     &                    * (1.0d0+1.5d0*cbnd*dt+2.0d0*qbnd*dt2)
               d2eddt2 = 2.0d0 * bndunit * force
     &                      * (1.0d0+3.0d0*cbnd*dt+6.0d0*qbnd*dt2)
c
c     Morse potential uses energy = BDE * (1 - e**(-alpha*dt))**2)
c     with the approximations alpha = sqrt(ForceConst/BDE) = -2
c     and BDE = Bond Dissociation Energy = ForceConst/alpha**2
c
            else if (bndtyp .eq. 'MORSE') then
               expterm = exp(-2.0d0*dt)
               bde = 0.25d0 * bndunit * force
               deddt = 4.0d0 * bde * (1.0d0-expterm) * expterm
               d2eddt2 = -8.0d0 * bde * (1.0d0-2.0d0*expterm) * expterm
            end if
c
c     scale the interaction based on its group membership
c
            if (use_group) then
               deddt = deddt * fgrp
               d2eddt2 = d2eddt2 * fgrp
            end if
c
c     set the chain rule terms for the Hessian elements
c
            if (rab2 .eq. 0.0d0) then
               de = 0.0d0
               term = 0.0d0
            else
               de = deddt / rab
               term = (d2eddt2-de) / rab2
            end if
            termx = term * xab
            termy = term * yab
            termz = term * zab
            d2e(1,1) = termx*xab + de
            d2e(1,2) = termx*yab
            d2e(1,3) = termx*zab
            d2e(2,1) = d2e(1,2)
            d2e(2,2) = termy*yab + de
            d2e(2,3) = termy*zab
            d2e(3,1) = d2e(1,3)
            d2e(3,2) = d2e(2,3)
            d2e(3,3) = termz*zab + de
c
c     increment diagonal and non-diagonal Hessian elements
c
            do j = 1, 3
               hessx(j,ia) = hessx(j,ia) + d2e(1,j)
               hessy(j,ia) = hessy(j,ia) + d2e(2,j)
               hessz(j,ia) = hessz(j,ia) + d2e(3,j)
               hessx(j,ib) = hessx(j,ib) - d2e(1,j)
               hessy(j,ib) = hessy(j,ib) - d2e(2,j)
               hessz(j,ib) = hessz(j,ib) - d2e(3,j)
            end do
         end if
      end do
      return
      end


c     program DRSNLSFU
c>> 2001-05-24 DRSNLSFU Krogh Minor change for making .f90 version.
c>> 1994-11-02 DRSNLSFU Krogh  Changes to use M77CON
c>> 1994-09-14 DRSNLSFU CLL Set IV(OUTLEV) = 0 for comparing output.
c>> 1992-02-03 CLL @ JPL
c>> 1990-07-02 CLL @ JPL
c>> 1990-06-27 CLL @ JPL
c>> 1990-06-14 CLL @ JPL
c>> 1990-04-05 CLL @ JPL
c>> 1990-03-29 CLL @ JPL
c     Demo driver for SNLSFU. A variant of the nonlinear LS code NL2SOL.
c     SNLSFU solves the "separable" problem.
c     SNLSFU requires function values only.
c     Note:  The expressions below set LIV and LV larger than
c     necessary because the precise formulas cannot be written in a
c     Fortran parameter statement.
c     ------------------------------------------------------------------
c--S replaces "?": DR?NLSFU, ?NLSFU, ?CALCA, ?IVSET
c     ------------------------------------------------------------------
      external SCALCA
      integer  ITERM,IVAR,LIV,LV,MA,MB,MDATA,MDIR,MLEN,NA,NB,NDATA
      parameter(MDATA = 30, MA = 2, MB = 5, MDIR = 4)
      parameter(MLEN = (MB+MA)*(MDATA+MB+MA+1))
      parameter(LIV = 122 + 2*MDIR + 4*MA + 2*MB + MB+1 + 6*MA)
      parameter(LV = 105 + 2*MDATA*(MB+3) +
     *               MLEN + (MB*(MB+3))/2 + MA*(2*MA + 18))
      integer IND(MB+1,MA), IV(LIV)
      integer F, COVPRT, OUTLEV, SOLPRT, STATPR, X0PRT
      parameter(F=10)
      parameter(COVPRT=14, OUTLEV=19, SOLPRT=22, STATPR=23, X0PRT=24)
      real             ALF(MA), BET(MB), DOF, V(LV), YDATA(MDATA)
      data ((IND(ITERM,IVAR),IVAR = 1,2),ITERM = 1,6)/
     *     0, 0,
     *     1, 0,
     *     1, 0,
     *     0, 1,
     *     0, 1,
     *     0, 0/
      data YDATA /
     *     1.700641e0, 1.793512e0, 1.838309e0, 1.838416e0, 1.792204e0,
     *     1.700501e0, 1.579804e0, 1.426268e0, 1.260724e0, 1.084901e0,
     *     0.917094e0, 0.761920e0, 0.627304e0, 0.522146e0, 0.446645e0,
     *     0.404920e0, 0.392033e0, 0.409622e0, 0.453045e0, 0.510765e0,
     *     0.584554e0, 0.663109e0, 0.747613e0, 0.829439e0, 0.908496e0,
     *     0.983178e0, 1.051046e0, 1.114072e0, 1.171746e0, 1.227823e0/
c     ------------------------------------------------------------------
      NDATA = MDATA
      NA = MA
      NB = MB
      ALF(1) =    5.0e0
      ALF(2) =   10.0e0
      IV(1) = 0

      print'(1x,a)',
     * 'Program DRSNLSFU.. Demo driver for SNLSFU.',
     * '   A variant of NL2SOL.',
     * '   SNLSFU handles the Separable problem.',
     * '   SNLSFU requires function values but not the Jacobian.',
     * ' ',
     * 'Sample problem is a nonlinear curve fit to data.',
     * 'Model function is B1 + B2 * cos(A1*t) + B3 * sin(A1*t) +',
     * '                       B4 * cos(A2*t) + B5 * sin(A2*t) + Noise',
     * 'Data generated using',
     * '(A1, A2, B1, ..., B5) = (6, 9, 1, 0.5, 0.4, 0.2, 0.1)',
     * 'and Gaussian noise with mean 0 and',
     * 'sample standard deviation 0.001',
     * ' '

      call SIVSET(1, IV, LIV, LV, V)
      IV( X0PRT) = 1
      IV(OUTLEV) = 0
      IV(STATPR) = 1
      IV(SOLPRT) = 1
      IV(COVPRT) = 1

      call SNLSFU(NDATA, NA, NB, ALF, BET, YDATA, SCALCA,
     *         IND, NB+1, IV, LIV, LV, V)

      DOF = max(NDATA - NA - NB, 1)
      print'(1x/1x,a,g12.4)',
     *    'SIGFAC: sqrt((2 * V(F))/DOF) =',
     *             sqrt(2.0e0 * V(F)/DOF)
      stop
      end
c     ==================================================================
      subroutine SCALCA(NDATA, NA, NB, ALF, NCOUNT, PHI)
c     Test case for separable nonlinear least squares computation.
c     Computes NDATA x NB matrix PHI as a function of the
c     nonlinear parameters ALF().
c     For J .le. NB the (I,J) term of PHI is the coefficient of the
c     linear coefficient B(J) in row I of the model.
c     In this example the model does not have a term that is not
c     multiplied by a linear coefficient.  If such a term is present
c     then PHI must have an (NB+1)st column to hold this term.
c     ------------------------------------------------------------------
      integer I, NA, NB, NCOUNT, NDATA
      real             ALF(NA)
      real             DEL, PHI(NDATA,NB), T
c     ------------------------------------------------------------------
      T = 0.0E0
      DEL = 1.0E0 / 29.0E0
      do 10 I = 1,NDATA
         PHI(I,1) = 1.0E0
         PHI(I,2) = cos(ALF(1)*T)
         PHI(I,3) = sin(ALF(1)*T)
         PHI(I,4) = cos(ALF(2)*T)
         PHI(I,5) = sin(ALF(2)*T)
         T = T + DEL
   10 continue
      return
      end


program bin2hdf5

    ! Created by Manuel A. Diaz, ENSMA 2020

    use HDF5 ! This module contains all necessary modules

    IMPLICIT NONE

    !-------- initialize variables -------------
    character(len=30) :: input_file0
    character(len=30) :: input_file1='xp.dat'
    character(len=30) :: input_file2='yp.dat'
    character(len=30) :: input_file3='zp.dat'
    character(len=30) :: output_file
    character(len=30) :: xdmf_file
    character(len=10) :: Nx_char
    character(len=10) :: Ny_char
    character(len=10) :: Nz_char
    character(len=3)  :: data_extension="h5"
    character(len=3)  :: xdmf_extension="xmf"

    integer,  parameter   :: fp=8             ! float data presicion
    integer               :: pointPosition    ! '.' location
    integer               :: Nx, Ny, Nz       ! Array dimensions
    real(fp), allocatable :: x(:), y(:), z(:) ! Axis points arrays
    real(fp), allocatable :: p(:,:,:)         ! Buffer for array
    integer               :: buff_lenght      ! Buffer length
    integer               :: i, j, k          ! dummy indexes

    integer(HSIZE_T), dimension(3) :: dims    ! Dataset dimensions
    integer(HID_T)        :: input_file_id=1  ! File identifier
    integer(HID_T)        :: output_file_id   ! File identifier
    integer(HID_T)        :: dset_id          ! Dataset identifier
    integer(HID_T)        :: dspace_id        ! Dataspace identifier
    integer               :: error            ! Error flag

    !--------  Parse arguments from command -------------
    if ( command_argument_count() .NE. 4 ) then
        print*, "Mode of use: ./bin2hdf5_fields_serial.run [*.bin] [Nx] [Ny] [Nz]"; stop
    else
        call get_command_argument(1,input_file0)
        call get_command_argument(2,Nx_char); read(Nx_char,*) Nx
        call get_command_argument(3,Ny_char); read(Ny_char,*) Ny
        call get_command_argument(4,Nz_char); read(Nz_char,*) Nz
        print*, "Attemping to read ",Nx,"Nx",Ny,"Ny",Nz,"Nz array."
    end if

    !-------- Set the name of the output files ----------
    pointPosition = scan(trim(input_file0),".", BACK=.true.)
    if (pointPosition>0) then
        output_file = input_file0(1:pointPosition)//data_extension
          xdmf_file = input_file0(1:pointPosition)//xdmf_extension
    end if

    !-------- Allocate space for the array -------------
    allocate( x(Nx) )
    allocate( y(Ny) )
    allocate( z(Nz) )
    allocate( p(Nx,Ny,Nz) )

    !-------- Read the input data -------------
    Inquire( iolength = buff_lenght ) p
    open (input_file_id,FILE=trim(input_file0), form='unformatted', status='old', access='direct',recl=buff_lenght)
    read (input_file_id,rec=1) p
    close(input_file_id)
    open (input_file_id,file=trim(input_file1), form='formatted', status='old', action='read')
    do i = 1, Nx
        read(input_file_id,*) x(i)
    end do
    close(input_file_id)
    open (input_file_id,file=trim(input_file2), form='formatted', status='old', action='read')
    do j = 1, Ny
        read(input_file_id,*) y(j)
    end do
    close(input_file_id)
    open (input_file_id,file=trim(input_file3), form='formatted', status='old', action='read')
    do k = 1, Nz
        read(input_file_id,*) z(k)
    end do
    close(input_file_id)

    !-------- Write the data to HDF5 -------------
    ! Write Fields
    dims(1) = Nx
    dims(2) = Ny
    dims(3) = Nz
    !
    CALL h5open_f(error) ! Initialize FORTRAN interface of HDF5.
    CALL h5fcreate_f (output_file, H5F_ACC_TRUNC_F, output_file_id, error) ! Create a new file.
    ! p
    CALL h5screate_simple_f(3, dims, dspace_id, error) ! Create the dataspace.
    CALL h5dcreate_f(output_file_id, "p", H5T_NATIVE_DOUBLE, dspace_id, dset_id, error)
    CALL h5dwrite_f(dset_id, H5T_NATIVE_DOUBLE, p, dims, error) ! Write the dataset.
    CALL h5dclose_f(dset_id, error) ! End access to the dataset and release resources used by it.
    CALL h5sclose_f(dspace_id, error) ! Terminate access to the data space.

    CALL h5fclose_f(output_file_id, error) ! Close the file.
    CALL h5close_f(error) ! Close FORTRAN interface.

    ! Write geometry file
    CALL writeGeometry_h5_serial(x,y,z,Nx,Ny,Nz)

    ! if everything goes well then report that:
    print*, "translation *.bin to *.h5 successful :)"
    deallocate(x)
    deallocate(y)
    deallocate(z)
    deallocate(p)

    !-------- Write the associated XDMF file -------------
    call writeFields3D_xmf_dp(xdmf_file,Nx,Ny,Nz,0.0_fp)

    end program bin2hdf5


module test
	Contains
	PURE subroutine test_f(nn)
	 implicit none
	   integer, intent (in) :: nn
	   Real arr(nn,nn)
	   integer i,j,x,y,n
	   INTEGER , PARAMETER :: runs=10000
	   x = nn
	   y = nn
	   DO n = 1 , runs
		  !DIR$ PARALLEL  ALWAYS
		  FORALL(i = 1:x,j = 1:y)
			 arr(j,i) = REAL(i)*REAL(j) + 2
		  END FORALL
		  arr(1,1) = arr(1,2)
	   END DO

	end
 end module test

PROGRAM timer2
	use test
	IMPLICIT NONE
	REAL :: t1,t2,rate
	real sys_clock(4), cpu_time_(4)
	INTEGER :: c1,c2,cr,cm,i,j,n,s
	INTEGER :: x, y, count
	INTEGER , PARAMETER :: runs=10000
	REAL, dimension (:,:), allocatable :: array
  	!dir$ attributes align:64 ::array
	! First initialize the system_clock
	CALL system_clock(count_rate=cr)
	CALL system_clock(count_max=cm)
	rate = REAL(cr)
	WRITE(*,*) "system_clock rate ",rate

	open(1, file = 'times.txt')

	do count = 100,1500,100
		x = count
		y = count

		allocate ( array(x,y) )
		CALL CPU_TIME(t1)
		CALL SYSTEM_CLOCK(c1)

		call test_f(count)

		CALL CPU_TIME(t2)
		CALL SYSTEM_CLOCK(c2)

		sys_clock(1) = (c2 - c1)/rate
		cpu_time_(1) = (t2-t1)

	CALL CPU_TIME(t1)
	CALL SYSTEM_CLOCK(c1)
	DO n = 1 , runs

	!DIR$ PARALLEL  ALWAYS
	   do i = 1,x
		do j = 1,y
		  array(j,i) = REAL(i)*REAL(j) + 2
		end do
		end do
	   array(1,1) = array(1,2)
	END DO
	CALL CPU_TIME(t2)
	CALL SYSTEM_CLOCK(c2)

	sys_clock(2) = (c2 - c1)/rate
	cpu_time_(2) = (t2-t1)

	CALL CPU_TIME(t1)
	CALL SYSTEM_CLOCK(c1)
	DO n = 1 , runs
	   !DIR$ 	NOPARALLEL
		do i = 1,x
			do j = 1,y
			  array(j,i) = REAL(i)*REAL(j) + 2
			end do
		end do
	   array(1,1) = array(1,2)
	END DO
	CALL CPU_TIME(t2)
	CALL SYSTEM_CLOCK(c2)

	sys_clock(3) = (c2 - c1)/rate
	cpu_time_(3) = (t2-t1)

	CALL CPU_TIME(t1)
	CALL SYSTEM_CLOCK(c1)
	DO n = 1 , runs
	   !DIR$ 	NOPARALLEL
	   FORALL(i = 1:x,j = 1:y)
		  array(j,i) = REAL(i)*REAL(j) + 2
	   END FORALL
	   array(1,1) = array(1,2)
	END DO
	CALL CPU_TIME(t2)
	CALL SYSTEM_CLOCK(c2)

	sys_clock(4) = (c2 - c1)/rate
	cpu_time_(4) = (t2-t1)

	print *, x, x*y, '         PARALLEL', '       NOPARALLEL', '     FORALL'
	WRITE(*,*) "system_clock : ",sys_clock
	WRITE(*,*) "cpu_time     : ",cpu_time_
	print *,'-----------------'
	write(1,"(5F10.5)")  sys_clock(3)/sys_clock(2)
	deallocate (array)
	end do
	close(1)
  END PROGRAM timer2


!======================MGM's hit3d Postprocessing script===================
!Function:       Save u, v & w in my (organized .dat, ascii & tecplot) formats
!Description:    Save the resutls of hit3d (u, v & w) in proper organized manner of .dat, ascii & tecplot formats
!Details:        save data
!Date created:   07/06/2018
!Date edited:    07/09/2018; cleaned-up

program MgmTurbNetTools

    implicit none

    integer:: brooks_step

    write (*, *) "printing relevant information for first 10 timesteps"

    do brooks_step = 1, 1
        !write(*,*) "========================== step=", brooks_step*100, " =================="
        call data_step(brooks_step*100)
    end do

!============SUBROUTINES & FUNCTIONS=========================
CONTAINS
! Subroutine for clock start
    SUBROUTINE tick(t)
        INTEGER, INTENT(OUT) :: t

        CALL system_clock(t)
    END SUBROUTINE tick
! Function to return end time
    REAL FUNCTION tock(t)
        INTEGER, INTENT(IN) :: t
        INTEGER :: now, clock_rate

        call system_clock(now, clock_rate)
        tock = real(now - t)/real(clock_rate)
    END FUNCTION tock
! Function to perform cross product of 2 vectors
    FUNCTION cross(a, b, n)
        INTEGER, INTENT(IN) :: n
        REAL, DIMENSION(n, 3) :: cross
        REAL, INTENT(IN), DIMENSION(n, 3) :: a, b

        cross(:, 1) = a(:, 2)*b(:, 3) - a(:, 3)*b(:, 2)
        cross(:, 2) = a(:, 3)*b(:, 1) - a(:, 1)*b(:, 3)
        cross(:, 3) = a(:, 1)*b(:, 2) - a(:, 2)*b(:, 1)
    END FUNCTION cross

!===========================
! Subroutine for gradient of a property in 3D domain
    SUBROUTINE gradient3D(m, n, o, f, hx, hy, hz, dfdx, dfdy, dfdz)
        INTEGER, INTENT(IN) :: m, n, o
        REAL*4, DIMENSION(m, n, o), INTENT(IN) :: f
        REAL, INTENT(IN) :: hx, hy, hz
        REAL, DIMENSION(m, n, o), INTENT(OUT) :: dfdx, dfdy, dfdz
        INTEGER :: i, j, k

        dfdx = 0.0; dfdy = 0.0; dfdz = 0.0;
!dfdx
        do k = 1, o
            do j = 1, n
!        forward difference at start point
                dfdx(1, j, k) = (f(2, j, k) - f(1, j, k))/hx;
!        central difference at middle region
                dfdx(2:m - 1, j, k) = (f(3:m, j, k) - f(1:m - 2, j, k))/(2.*hx);
!        backward difference at end point
                dfdx(m, j, k) = (f(m, j, k) - f(m - 1, j, k))/hx;
            end do
        end do

!dfdy
        do k = 1, o
            do i = 1, m
!        forward difference at start point
                dfdy(i, 1, k) = (f(i, 2, k) - f(i, 1, k))/hy;
!        central difference at middle region
                dfdy(i, 2:n - 1, k) = (f(i, 3:n, k) - f(i, 1:n - 2, k))/(2.*hy);
!        backward difference at end point
                dfdy(i, n, k) = (f(i, n, k) - f(i, n - 1, k))/hy;
            end do
        end do

!dfdz
        do j = 1, n
            do i = 1, m
!        forward difference at start point
                dfdz(i, j, 1) = (f(i, j, 2) - f(i, j, 1))/hz;
!        central difference at middle region
                dfdz(i, j, 2:o - 1) = (f(i, j, 3:o) - f(i, j, 1:o - 2))/(2.*hz);
!        backward difference at end point
                dfdz(i, j, o) = (f(i, j, o) - f(i, j, o - 1))/hz;
            end do
        end do

    END SUBROUTINE gradient3D

    subroutine data_step(itr)
        !========================
        integer*4 :: nx, ny, nz, npoints
        integer*4 :: sizes(3)
        real :: dx, dy, dz
        real, parameter :: pi = 3.141592653589793
        real, dimension(:), allocatable :: xx_v, yy_v, zz_v, XX, YY, ZZ
        logical :: readforce
        INTEGER :: tclock
        REAL(4)    :: time, ttime
        character(6) :: boxchar
        character(2) :: boxchar1
        character*80 :: fname
        !real, dimension(:,:,:), allocatable :: u, v, w
        real*4, allocatable :: u(:, :, :), v(:, :, :), w(:, :, :)!, phi(:,:,:)
        real, dimension(:, :, :), allocatable :: dUdX, dUdY, dUdZ, dVdX, dVdY, dVdZ, dWdX, dWdY, dWdZ, omgmag, strmag
        real, dimension(:, :, :), allocatable :: OmgX, OmgY, OmgZ, omg1, omg2, omg3, gam1, gam2, gam3, sig1, sig2, sig3, qcrit
        real, dimension(:), allocatable :: u_vec, v_vec, w_vec, OmgX_vec, OmgY_vec, OmgZ_vec, qcrit_vec, omgmag_vec, strmag_vec
        real*4 :: brooks_helicity
        !real, dimension(:), allocatable :: phi_vec

        real*4 :: a, b, c, energy

        integer, parameter :: RP = 4 ! Number of bytes for reals (single precision)

        real(RP), dimension(:, :), allocatable :: points    ! input

        ! loop iterator
        integer :: i, j, k, l, p, q, r, boxnum, itr

        !============Load data====================================
        !itr = 1000
        !write(*,*) 'Enter iteration number = '
        !read(*,*) itr
        write (boxchar, "(I6.6)") itr

        fname = "output/velocity/u."//boxchar
        !write(*,*) "opening file", fname
        open (unit=100, file=fname, form="unformatted", access='stream')
        read (100) sizes
        nx = sizes(1); ny = sizes(2); nz = sizes(3);
        npoints = nx*ny*nz;
        !print *, 'Sizes: ', nx, ny, nz
        allocate (u(nx, ny, nz)); allocate (v(nx, ny, nz)); allocate (w(nx, ny, nz))
        read (100) (((u(i, j, k), i=1, nx), j=1, ny), k=1, nz)   ! +(fact*(p-1))
        close (100)

        fname = "output/velocity/v."//boxchar
        open (unit=100, file=fname, form="unformatted", access='stream')
        read (100)
        read (100) (((v(i, j, k), i=1, nx), j=1, ny), k=1, nz)
        close (100)
        !print *, 'v(2,1,1) = ', v(2,1,1)

        fname = "output/velocity/w."//boxchar
        open (unit=100, file=fname, form="unformatted", access='stream')
        read (100)
        read (100) (((w(i, j, k), i=1, nx), j=1, ny), k=1, nz)
        close (100)

        !fname = "output/sc"//boxchar1//"."//boxchar
        !write(*,*) "fname for scalars is", fname
        !allocate(phi(nx,ny,nz));
        !open(unit=100,file=fname,form="unformatted",access='stream')
        !read(100)
        !read(100) ( ( (phi(i,j,k),i=1,nx) ,j=1,ny) ,k= 1,nz )
        !close(100)

        !print *, 'Data loaded...'

        != ====

        !=========Post-process data=================================
        allocate (xx_v(nx)); allocate (yy_v(ny)); allocate (zz_v(nz));
        allocate (XX(npoints)); allocate (YY(npoints)); allocate (ZZ(npoints));
        allocate (points(3, npoints));
        allocate (u_vec(npoints)); allocate (v_vec(npoints)); allocate (w_vec(npoints));
        !allocate(phi_vec(npoints));

        dx = (2.*pi)/nx; dy = dx; dz = dx;
        !Vorticity calculation
        allocate (dUdX(nx, ny, nz)); allocate (dUdY(nx, ny, nz)); allocate (dUdZ(nx, ny, nz));
        allocate (dVdX(nx, ny, nz)); allocate (dVdY(nx, ny, nz)); allocate (dVdZ(nx, ny, nz));
        allocate (dWdX(nx, ny, nz)); allocate (dWdY(nx, ny, nz)); allocate (dWdZ(nx, ny, nz));
        allocate (OmgX(nx, ny, nz)); allocate (OmgY(nx, ny, nz)); allocate (OmgZ(nx, ny, nz));
        allocate (omg1(nx, ny, nz)); allocate (omg2(nx, ny, nz)); allocate (omg3(nx, ny, nz));
        allocate (gam1(nx, ny, nz)); allocate (gam2(nx, ny, nz)); allocate (gam3(nx, ny, nz));
        allocate (sig1(nx, ny, nz)); allocate (sig2(nx, ny, nz)); allocate (sig3(nx, ny, nz));
        allocate (qcrit(nx, ny, nz)); allocate (omgmag(nx, ny, nz)); allocate (strmag(nx, ny, nz));
        CALL gradient3D(nx, ny, nz, u, dx, dy, dz, dUdX, dUdY, dUdZ)
        CALL gradient3D(nx, ny, nz, v, dx, dy, dz, dVdX, dVdY, dVdZ)
        CALL gradient3D(nx, ny, nz, w, dx, dy, dz, dWdX, dWdY, dWdZ)

        allocate (OmgX_vec(npoints)); allocate (OmgY_vec(npoints)); allocate (OmgZ_vec(npoints));
        allocate (qcrit_vec(npoints)); allocate (omgmag_vec(npoints)); allocate (strmag_vec(npoints));
        energy = 0.

        do i = 1, nx
            do j = 1, ny
                do k = 1, nz
                    a = u(i, j, k)
                    b = v(i, j, k)
                    c = w(i, j, k)
                    !write(*,*) a*a + b*b + c*c
                    energy = energy + ((a**2 + b**2 + c**2)*dx*dy*dz*0.5)

                    !write(*,*) energy

                end do
            end do
        end do

        !write(*,*) "energy is ", energy

        OmgX = dUdY - dVdX
        OmgY = dUdZ - dWdX
        OmgZ = dVdZ - dWdY

        !Q-criterion
        omg1 = -0.5*(dUdY - dVdX)
        omg2 = -0.5*(dUdZ - dWdX)
        omg3 = -0.5*(dVdZ - dWdY)
        gam1 = 0.5*(dVdX + dUdY)
        gam2 = 0.5*(dWdX + dUdZ)
        gam3 = 0.5*(dWdY + dVdZ)
        sig1 = dUdX
        sig2 = dVdY
        sig3 = dWdZ
        qcrit = (omg1**2 + omg2**2 + omg3**2) - ((gam1**2 + gam2**2 + gam3**2) + 0.5*(sig1**2 + sig2**2 + sig3**2))
        omgmag = omg1**2 + omg2**2 + omg3**2
        strmag = (gam1**2 + gam2**2 + gam3**2) + 0.5*(sig1**2 + sig2**2 + sig3**2)
        !print *, 'Vorticity & Q-criterion calculated...'

        !Mesh generation
        xx_v(1) = 0.0; yy_v(1) = 0.0; zz_v(1) = 0.0;
        do i = 2, nx
            xx_v(i) = xx_v(i - 1) + dx
            yy_v(i) = yy_v(i - 1) + dy
            zz_v(i) = zz_v(i - 1) + dz
        end do

        !write(*,*) "outputting dUdX before other variables"
        !do i = 1, nx
        !    write(*,*) dUdX(i,1,1), dUdX(i,1,1), dUdX(i,1,1)
        !end do

        write (*, *) "moving on to other variables"

        brooks_helicity = 0.
        l = 1
        do k = 1, nz
            do j = 1, ny
                do i = 1, nx
                    if (k == 1 .and. j == 1) then
                        write (*, *) u(i, j, k), v(i, j, k), w(i, j, k), OmgX(i, j, k), OmgY(i, j, k), OmgZ(i, j, k)
                    end if

                    XX(l) = xx_v(i)
                    YY(l) = yy_v(j)
                    ZZ(l) = zz_v(k)
                    u_vec(l) = u(i, j, k)
                    v_vec(l) = v(i, j, k)
                    w_vec(l) = w(i, j, k)
                    !phi_vec(l) = phi(i,j,k)
                    OmgX_vec(l) = OmgX(i, j, k)
                    OmgY_vec(l) = OmgY(i, j, k)
                    OmgZ_vec(l) = OmgZ(i, j, k)
                    qcrit_vec(l) = qcrit(i, j, k)
                    omgmag_vec(l) = omgmag(i, j, k)
                    strmag_vec(l) = strmag(i, j, k)

                    brooks_helicity = brooks_helicity + (u_vec(l)*OmgX_vec(l) + v_vec(l)*OmgY_vec(l) + w_vec(l)*OmgZ_vec(l))

                    l = l + 1
                end do
            end do
        end do
        points(1, :) = XX
        points(2, :) = YY
        points(3, :) = ZZ
        !print *, 'Cartesian mesh generated...'

        !write(*,*) "MGM helicity calculation is before resize", brooks_helicity
        brooks_helicity = brooks_helicity*dx*dy*dz

        write (*, *) brooks_helicity

    !!===============Write data (Binary)========================
        !inquire(file="post/veldata_"//boxchar//".dat",exist=readforce)
        !if (readforce) THEN
        !    open(unit=100,file="post/veldata_"//boxchar//".dat",&
        !        action="write",form="unformatted",status="replace")
        !else
        !    open(unit=100,file="post/veldata_"//boxchar//".dat",&
        !        action="write",form="unformatted",status="new")
        !end if
        !
        !write(100) points
        !write(100) u_vec
        !write(100) v_vec
        !write(100) w_vec
        !write(100) OmgX_vec
        !write(100) OmgY_vec
        !write(100) OmgZ_vec
        !write(100) qcrit_vec
        !write(100) omgmag_vec
        !write(100) strmag_vec
    !!write(100) phi_vec
        !close(100)
        !write(*,*) 'Data written (binary)'
        !
    !!===============Write data (ASCII)========================
    !!inquire(file="post/veldata_"//boxchar//"ascii.dat",exist=readforce)
    !!if (readforce) THEN
    !!    open(unit=100,file="post/veldata_"//boxchar//"ascii.dat",&
    !!        action="write",form="formatted",status="replace")
    !!else
    !!    open(unit=100,file="post/veldata_"//boxchar//"ascii.dat",&
    !!        action="write",form="formatted",status="new")
    !!end if
    !!do i = 1,npoints
    !!    write(100,*) points(1,i), points(2,i), points(3,i),&
    !!    u_vec(i), v_vec(i), w_vec(i)
    !!end do
    !!close(100)
    !!write(*,*) 'Data written (ascii)'
        !
        !
    !!==================TECPLOT FILE==================
        !inquire(file="post/figs/veldata_"//boxchar//"plt.dat",exist=readforce)
        !
        !if (readforce) THEN
        !    open(unit=100,file="post/figs/veldata_"//boxchar//"plt.dat",&
        !    action="write",form="formatted",status="replace")
        !else
        !    open(unit=100,file="post/figs/veldata_"//boxchar//"plt.dat",&
        !    action="write",form="formatted",status="new")
        !end if
        !
        !WRITE(100,*) 'TITLE     = "3D Turbulent flow field (hit3d)"'
        !WRITE(100,*) 'VARIABLES = "X"'
        !WRITE(100,*) '"Y"'
        !WRITE(100,*) '"Z"'
        !WRITE(100,*) '"U"'
        !WRITE(100,*) '"V"'
        !WRITE(100,*) '"W"'
        !WRITE(100,*) '"VortX"'
        !WRITE(100,*) '"VortY"'
        !WRITE(100,*) '"VortZ"'
        !WRITE(100,*) '"Q"'
        !WRITE(100,*) '"Str"'
    !!WRITE(100,*) '"Phi"'
        !WRITE(100,*) 'ZONE T="Rectangular zone"'
        !WRITE(100,*) 'I=',nx,', J=',ny,', K=',nz, ', ZONETYPE=Ordered'
        !WRITE(100,*) 'DATAPACKING=POINT'
        !WRITE(100,*) 'DT=(SINGLE SINGLE SINGLE)'
        !
        !do i = 1,npoints
        !    write(100,*) points(1,i), points(2,i), points(3,i),&
        !    u_vec(i), v_vec(i), w_vec(i),&
        !    OmgX_vec(i), OmgY_vec(i), OmgZ_vec(i), qcrit_vec(i), strmag_vec(i)!,&
        !    !phi_vec(i)
        !end do
        !close(100)
        !write(*,*) 'Figure plotted (TECPLOT)'
    end subroutine data_step

end program MgmTurbNetTools


Counter = {get:()->Nat, inc:()->()};
newCounter = \_:().
  let x = ref 1 in
	{get = λ _:(). !x,
	 inc = λ _:(). x:=succ(!x)
    } as Counter;
inc3 = λc:Counter. (c.inc (); c.inc (); c.inc ());

c = newCounter ();
c.get ();
{c0=c.get (),
 c1=(inc3 c; c.get ())
 };

ResetCounter = {get:()->Nat, inc:()->(), reset:()->()};
newResetCounter = \_:().
  let x = ref 1 in
	{get = \_:(). !x,
	 inc = \_:(). x:=succ(!x),
     reset =\_:(). x:=1
    } as ResetCounter;
rc = newResetCounter ();
(inc3 rc; rc.reset (); inc3 rc; rc.get ());