C
C	$Id$
C
      SUBROUTINE CSTRMESH (N,X,Y,Z, LIST,LPTR,LEND,LNEW,NEAR,
     +                     NEXT,DIST,IER)
      INTEGER N, LIST(*), LPTR(*), LEND(N), LNEW, NEAR(N),
     +        NEXT(N), IER
      DOUBLE PRECISION X(N), Y(N), Z(N), DIST(N)
C
C***********************************************************
C
C                                              From STRIPACK
C                                            Robert J. Renka
C                                  Dept. of Computer Science
C                                       Univ. of North Texas
C                                           renka@cs.unt.edu
C                                                   07/08/99
C
C   This subroutine creates a Delaunay triangulation of a
C set of N arbitrarily distributed points, referred to as
C nodes, on the surface of the unit sphere.  The Delaunay
C triangulation is defined as a set of (spherical) triangles
C with the following five properties:
C
C  1)  The triangle vertices are nodes.
C  2)  No triangle contains a node other than its vertices.
C  3)  The interiors of the triangles are pairwise disjoint.
C  4)  The union of triangles is the convex hull of the set
C        of nodes (the smallest convex set that contains
C        the nodes).  If the nodes are not contained in a
C        single hemisphere, their convex hull is the en-
C        tire sphere and there are no boundary nodes.
C        Otherwise, there are at least three boundary nodes.
C  5)  The interior of the circumcircle of each triangle
C        contains no node.
C
C The first four properties define a triangulation, and the
C last property results in a triangulation which is as close
C as possible to equiangular in a certain sense and which is
C uniquely defined unless four or more nodes lie in a common
C plane.  This property makes the triangulation well-suited
C for solving closest-point problems and for triangle-based
C interpolation.
C
C   The algorithm has expected time complexity O(N*log(N))
C for most nodal distributions.
C
C   Spherical coordinates (latitude and longitude) may be
C converted to Cartesian coordinates by Subroutine CSTRANSD.
C
C   The following is a list of the software package modules
C which a user may wish to call directly:
C
C  CSADDNOD - Updates the triangulation by appending a new
C             node.
C
C  CSAREAS  - Returns the area of a spherical triangle.
C
C  CSBNODES - Returns an array containing the indexes of the
C             boundary nodes (if any) in counterclockwise
C             order.  Counts of boundary nodes, triangles,
C             and arcs are also returned.
C
C  CSCIRCUM - Returns the circumcenter of a spherical trian-
C             gle.
C
C  CSCRLIST - Returns the set of triangle circumcenters
C             (Voronoi vertices) and circumradii associated
C             with a triangulation.
C
C  CSDELARC - Deletes a boundary arc from a triangulation.
C
C  CSDELNOD - Updates the triangulation with a nodal deletion.
C
C  CSEDGE   - Forces an arbitrary pair of nodes to be connec-
C             ted by an arc in the triangulation.
C
C  CSGETNP  - Determines the ordered sequence of L closest
C             nodes to a given node, along with the associ-
C             ated distances.
C
C  CSINSIDE - Locates a point relative to a polygon on the
C             surface of the sphere.
C
C  CSINTRSC - Returns the point of intersection between a
C             pair of great circle arcs.
C
C  CSJRAND  - Generates a uniformly distributed pseudo-random
C             integer.
C
C  CSLEFT   - Locates a point relative to a great circle.
C
C  CSNEARND - Returns the index of the nearest node to an
C             arbitrary point, along with its squared
C             distance.
C
C  CSSCOORD - Converts a point from Cartesian coordinates to
C             spherical coordinates.
C
C  CSSTORE  - Forces a value to be stored in main memory so
C             that the precision of floating point numbers
C             in memory locations rather than registers is
C             computed.
C
C  CSTRANSD - Transforms spherical coordinates into Cartesian
C             coordinates on the unit sphere for input to
C             Subroutine CSTRMESH.
C
C  CSTRLIST - Converts the triangulation data structure to a
C             triangle list more suitable for use in a fin-
C             ite element code.
C
C  CSTRLPRT - Prints the triangle list created by Subroutine
C             CSTRLIST.
C
C  CSTRMESH - Creates a Delaunay triangulation of a set of
C             nodes.
C
C  CSTRPRNT - Prints the triangulation data structure and,
C             optionally, the nodal coordinates.
C
C On input:
C
C       N = Number of nodes in the triangulation.  N .GE. 3.
C
C       X,Y,Z = Arrays of length N containing the Cartesian
C               coordinates of distinct nodes.  (X(K),Y(K),
C               Z(K)) is referred to as node K, and K is re-
C               ferred to as a nodal index.  It is required
C               that X(K)**2 + Y(K)**2 + Z(K)**2 = 1 for all
C               K.  The first three nodes must not be col-
C               linear (lie on a common great circle).
C
C The above parameters are not altered by this routine.
C
C       LIST,LPTR = Arrays of length at least 6N-12.
C
C       LEND = Array of length at least N.
C
C       NEAR,NEXT,DIST = Work space arrays of length at
C                        least N.  The space is used to
C                        efficiently determine the nearest
C                        triangulation node to each un-
C                        processed node for use by CSADDNOD.
C
C On output:
C
C       LIST = Set of nodal indexes which, along with LPTR,
C              LEND, and LNEW, define the triangulation as a
C              set of N adjacency lists -- counterclockwise-
C              ordered sequences of neighboring nodes such
C              that the first and last neighbors of a bound-
C              ary node are boundary nodes (the first neigh-
C              bor of an interior node is arbitrary).  In
C              order to distinguish between interior and
C              boundary nodes, the last neighbor of each
C              boundary node is represented by the negative
C              of its index.
C
C       LPTR = Set of pointers (LIST indexes) in one-to-one
C              correspondence with the elements of LIST.
C              LIST(LPTR(I)) indexes the node which follows
C              LIST(I) in cyclical counterclockwise order
C              (the first neighbor follows the last neigh-
C              bor).
C
C       LEND = Set of pointers to adjacency lists.  LEND(K)
C              points to the last neighbor of node K for
C              K = 1,...,N.  Thus, LIST(LEND(K)) < 0 if and
C              only if K is a boundary node.
C
C       LNEW = Pointer to the first empty location in LIST
C              and LPTR (list length plus one).  LIST, LPTR,
C              LEND, and LNEW are not altered if IER < 0,
C              and are incomplete if IER > 0.
C
C       NEAR,NEXT,DIST = Garbage.
C
C       IER = Error indicator:
C             IER =  0 if no errors were encountered.
C             IER = -1 if N < 3 on input.
C             IER = -2 if the first three nodes are
C                      collinear.
C             IER =  L if nodes L and M coincide for some
C                      M > L.  The data structure represents
C                      a triangulation of nodes 1 to M-1 in
C                      this case.
C
C Modules required by CSTRMESH:  CSADDNOD, CSBDYADD, CSCOVSPH,
C                                CSINSERT, CSINTADD, CSJRAND,
C                                CSLEFT, CSLSTPTR, CSSTORE, CSSWAP,
C                                CSSWPTST, CSTRFIND
C
C Intrinsic function called by CSTRMESH:  ABS
C
C***********************************************************
C
      INTEGER I, I0, J, K, LP, LPL, NEXTI, NN
      LOGICAL CSLEFT
      DOUBLE PRECISION D, D1, D2, D3
C
C Local parameters:
C
C D =        (Negative cosine of) distance from node K to
C              node I
C D1,D2,D3 = Distances from node K to nodes 1, 2, and 3,
C              respectively
C I,J =      Nodal indexes
C I0 =       Index of the node preceding I in a sequence of
C              unprocessed nodes:  I = NEXT(I0)
C K =        Index of node to be added and DO-loop index:
C              K > 3
C LP =       LIST index (pointer) of a neighbor of K
C LPL =      Pointer to the last neighbor of K
C NEXTI =    NEXT(I)
C NN =       Local copy of N
C
      NN = N
      IF (NN .LT. 3) THEN
        IER = -1
        RETURN
      ENDIF
C
C Store the first triangle in the linked list.
C
      IF ( .NOT. CSLEFT (X(1),Y(1),Z(1),X(2),Y(2),Z(2),
     .                 X(3),Y(3),Z(3)) ) THEN
C
C   The first triangle is (3,2,1) = (2,1,3) = (1,3,2).
C
        LIST(1) = 3
        LPTR(1) = 2
        LIST(2) = -2
        LPTR(2) = 1
        LEND(1) = 2
C
        LIST(3) = 1
        LPTR(3) = 4
        LIST(4) = -3
        LPTR(4) = 3
        LEND(2) = 4
C
        LIST(5) = 2
        LPTR(5) = 6
        LIST(6) = -1
        LPTR(6) = 5
        LEND(3) = 6
C
      ELSEIF ( .NOT. CSLEFT(X(2),Y(2),Z(2),X(1),Y(1),Z(1),
     .                    X(3),Y(3),Z(3)) )
     .       THEN
C
C   The first triangle is (1,2,3):  3 Strictly Left 1->2,
C     i.e., node 3 lies in the left hemisphere defined by
C     arc 1->2.
C
        LIST(1) = 2
        LPTR(1) = 2
        LIST(2) = -3
        LPTR(2) = 1
        LEND(1) = 2
C
        LIST(3) = 3
        LPTR(3) = 4
        LIST(4) = -1
        LPTR(4) = 3
        LEND(2) = 4
C
        LIST(5) = 1
        LPTR(5) = 6
        LIST(6) = -2
        LPTR(6) = 5
        LEND(3) = 6
C
      ELSE
C
C   The first three nodes are collinear.
C
        IER = -2
        RETURN
      ENDIF
C
C Initialize LNEW and test for N = 3.
C
      LNEW = 7
      IF (NN .EQ. 3) THEN
        IER = 0
        RETURN
      ENDIF
C
C A nearest-node data structure (NEAR, NEXT, and DIST) is
C   used to obtain an expected-time (N*log(N)) incremental
C   algorithm by enabling constant search time for locating
C   each new node in the triangulation.
C
C For each unprocessed node K, NEAR(K) is the index of the
C   triangulation node closest to K (used as the starting
C   point for the search in Subroutine CSTRFIND) and DIST(K)
C   is an increasing function of the arc length (angular
C   distance) between nodes K and NEAR(K):  -Cos(a) for arc
C   length a.
C
C Since it is necessary to efficiently find the subset of
C   unprocessed nodes associated with each triangulation
C   node J (those that have J as their NEAR entries), the
C   subsets are stored in NEAR and NEXT as follows:  for
C   each node J in the triangulation, I = NEAR(J) is the
C   first unprocessed node in J's set (with I = 0 if the
C   set is empty), L = NEXT(I) (if I > 0) is the second,
C   NEXT(L) (if L > 0) is the third, etc.  The nodes in each
C   set are initially ordered by increasing indexes (which
C   maximizes efficiency) but that ordering is not main-
C   tained as the data structure is updated.
C
C Initialize the data structure for the single triangle.
C
      NEAR(1) = 0
      NEAR(2) = 0
      NEAR(3) = 0
      DO 1 K = NN,4,-1
        D1 = -(X(K)*X(1) + Y(K)*Y(1) + Z(K)*Z(1))
        D2 = -(X(K)*X(2) + Y(K)*Y(2) + Z(K)*Z(2))
        D3 = -(X(K)*X(3) + Y(K)*Y(3) + Z(K)*Z(3))
        IF (D1 .LE. D2  .AND.  D1 .LE. D3) THEN
          NEAR(K) = 1
          DIST(K) = D1
          NEXT(K) = NEAR(1)
          NEAR(1) = K
        ELSEIF (D2 .LE. D1  .AND.  D2 .LE. D3) THEN
          NEAR(K) = 2
          DIST(K) = D2
          NEXT(K) = NEAR(2)
          NEAR(2) = K
        ELSE
          NEAR(K) = 3
          DIST(K) = D3
          NEXT(K) = NEAR(3)
          NEAR(3) = K
        ENDIF
    1   CONTINUE
C
C Add the remaining nodes
C
      DO 6 K = 4,NN
        CALL CSADDNOD (NEAR(K),K,X,Y,Z, LIST,LPTR,LEND,
     .               LNEW, IER)
        IF (IER .NE. 0) RETURN
C
C Remove K from the set of unprocessed nodes associated
C   with NEAR(K).
C
        I = NEAR(K)
        IF (NEAR(I) .EQ. K) THEN
          NEAR(I) = NEXT(K)
        ELSE
          I = NEAR(I)
    2     I0 = I
            I = NEXT(I0)
            IF (I .NE. K) GO TO 2
          NEXT(I0) = NEXT(K)
        ENDIF
        NEAR(K) = 0
C
C Loop on neighbors J of node K.
C
        LPL = LEND(K)
        LP = LPL
    3   LP = LPTR(LP)
          J = ABS(LIST(LP))
C
C Loop on elements I in the sequence of unprocessed nodes
C   associated with J:  K is a candidate for replacing J
C   as the nearest triangulation node to I.  The next value
C   of I in the sequence, NEXT(I), must be saved before I
C   is moved because it is altered by adding I to K's set.
C
          I = NEAR(J)
    4     IF (I .EQ. 0) GO TO 5
          NEXTI = NEXT(I)
C
C Test for the distance from I to K less than the distance
C   from I to J.
C
          D = -(X(I)*X(K) + Y(I)*Y(K) + Z(I)*Z(K))
          IF (D .LT. DIST(I)) THEN
C
C Replace J by K as the nearest triangulation node to I:
C   update NEAR(I) and DIST(I), and remove I from J's set
C   of unprocessed nodes and add it to K's set.
C
            NEAR(I) = K
            DIST(I) = D
            IF (I .EQ. NEAR(J)) THEN
              NEAR(J) = NEXTI
            ELSE
              NEXT(I0) = NEXTI
            ENDIF
            NEXT(I) = NEAR(K)
            NEAR(K) = I
          ELSE
            I0 = I
          ENDIF
C
C Bottom of loop on I.
C
          I = NEXTI
          GO TO 4
C
C Bottom of loop on neighbors J.
C
    5     IF (LP .NE. LPL) GO TO 3
    6   CONTINUE
      RETURN
      END


  subroutine backward_loop( L, N, KU, LB,Bj,Cj,Pj,Sj, NRHS, INFO, LR,LU,UDR, UER, LL)
  Use lapackinterface
  IMPLICIT NONE
!  PURPOSE backward iterative loop
!  Copyright (c) 2021   Anthony M de Beus
   INTEGER, PARAMETER :: wp = KIND(0.0D0) ! working precision

!  .. Scalar Arguments ..
   Integer, Intent(IN) ::  L, LB, LR, LU, KU, N, NRHS  ! L is starting place, LB=size(B,2)=size(C/P/J,3)
                                                       ! LU+1=size(UD,3)=size(UE,2) index LR arrays
   INTEGER, INTENT(OUT) :: INFO,LL                     ! LL is number of steps
!  .. Array Arguments ..

   REAL(wp),INTENT(IN) :: Bj(2*KU,LB,NRHS)
   REAL(wp),INTENT(IN) :: Cj(2*KU,2*KU,LB)
   REAL(wp),INTENT(IN) :: Pj(2*KU,2*KU,LB)
   REAL(wp),INTENT(IN) :: Sj(2*KU,2*KU,LB)
   REAL(wp),INTENT(INOUT) :: UDR(2*KU,2*KU,LR:LU) ! ud is my set of matrices Aj
   REAL(wp),INTENT(INOUT) :: UER(2*KU,LR:LU,NRHS)      ! ue is my vectors vj

!  .. Work space ..
   REAL(wp) :: A(2*KU,2*KU),AA(2*KU,2*KU),CC(2*KU,NRHS),EE(2*KU,2*KU+NRHS) ! working copies
   INTEGER ::  i,j,k,kk,hh,p,ii,jj,ipiv(2*KU)

   p=mod(N,2*KU)

!! BACKWARD
!  ALL BUT THE LAST EQUATION, GENERATE UDR & UER  ! (Cj+Pj*A(:,:,j+1))*A(:,:,j)=-Sj
   jj=(N-p)/(2*KU)+1  !index of number of arrays
    do j=(N-p)/2,L+KU,-KU ! j not used; just a counter
    jj=jj-1
    call DGEMM('N','N',2*KU,2*KU,2*KU,1.0_wp,Pj(:,:,jj),2*KU,UDR(:,:,jj+1),2*KU,0.0_wp,AA,2*KU)
    A=Cj(:,:,jj)+AA
!    A=Cj(:,:,jj)+matmul(Pj(:,:,jj),UDR(:,:,jj+1))
!   concatenate Aj and vj solutions onto EE
     EE(:,1:2*KU)=-Sj(:,:,jj)
     do hh=1,NRHS
      call DGEMV('N',2*KU,2*KU,1.0_wp,Pj(:,:,jj),2*KU,UER(:,jj+1,hh),1,0.0_wp,CC(:,hh),1)
      EE(:,2*KU+hh)=Bj(:,jj,hh)-CC(:,hh)
!      EE(:,2*KU+hh)=Bj(:,jj,hh)-matmul(Pj(:,:,jj),UER(:,jj+1,hh))
     end do
!    compute next UD,UE using factored A
    call DGESV(2*KU,2*KU+NRHS,A,2*KU,IPIV,EE,2*KU,INFO) ! overwrites EE into solution
!    call GaussJordan( 2*KU, 2*KU+NRHS ,A ,2*KU , EE, 2*KU, INFO )   ! overwrites EE into solution
    if (info /= 0) then
     CALL XERBLA( 'DGETRS/DCBSV ', INFO )
!     RETURN
    else
     UDR(:,:,jj)=EE(:,1:2*KU)
    do hh=1,NRHS
     UER(:,jj,hh)=EE(:,2*KU+hh)
    end do
    endif
   end do
   LL=jj

  end subroutine backward_loop


program phaml_master
use phaml
implicit none
type(phaml_solution_type) :: soln
real(my_real) :: err1, err2
call phaml_create(soln,nproc=4,system_size=2)
call phaml_solve_pde(soln,                   &
                     max_vert=300,           &
                     mg_cycles=5,            &
                     print_grid_when=PHASES, &
                     print_grid_who=MASTER  ,&
                     print_error_when=PHASES    , &
                     print_error_what=ENERGY_LINF_ERR, &
                     print_errest_what=ENERGY_LINF_ERREST, &
                     print_error_who=MASTER)
call phaml_query(soln,linf_error=err1,comp=1)
call phaml_query(soln,linf_error=err2,comp=2)
write(6,"(A,SS,1P,2E19.12E2)") "Linf errors from query ",err1,err2
call phaml_destroy(soln)
end program phaml_master


      FUNCTION DGAMMLN(XX)

!PURPOSE: ln(gamma(xx)), taken from Numerical Recipes, p156-157

      INCLUDE       '../INCLDS/COPYRIGT.FOR'

!PSEUDOCODE:

!CALLING ARGUMENTS:

      REAL*8        DGAMMLN              !ln(gamma(xx)) with xx>0
      REAL*4        XX                  !argument must be >0

!INCLUDE STATEMENTS:

!LOCAL VARIABLES:

      REAL*8        COF(6),STP,HALF,ONE,FPF,X,TMP,SER

!SUBROUTINES CALLED:

!FUNCTION DEFINITIONS:

!DATA STATEMENTS:

      DATA      COF,STP/76.18009172947146D0,-86.50532032941677D0,
     1  24.01409824083091D0,-1.231739572450155D0,0.1208650973866D-2,
     1  -0.5395239384953D-5,2.5066282746310005D0/
      DATA      HALF,ONE,FPF/0.5D0,1.000000000190015D0,5.5D0/

!CODE:

      X = XX-ONE
      TMP = X+FPF
      TMP = (X+HALF)*DLOG(TMP)-TMP
      SER = ONE
      DO J=1,6
        X = X+ONE
        SER = SER+COF(J)/X
      END DO
      DGAMMLN = TMP+DLOG(STP*SER)
      RETURN
      END


c=======================================================================
c
c     subroutine COREXP
c
c     Empirical Correlation matrix and mean vector
c
c                        cov(i,j)
c     corr(i,j) = -----------------------,
c                 sqrt[cov(i,i)*cov(j,j)]
c
c     where cov is exponentially weighted covariance matrix
c
c                       n
c                       ---
c     cov(i,j) = (1-L)  \    L^(k-1)*[X(k,i) - rho(i)] * [X(k,j) - rho(j)]
c                       /
c                       ---
c                       k=1
c
c                  n
c              1  ---
c     rho(j) = -  \   X(i,j) for j = 1,...,p
c              n  /
c                 ---
c                 i=1
c
c     L : exponential parameter
c
c-----------------------------------------------------------------------
      SUBROUTINE corexp ( n, p, X, lambda, dwork, corr, info)
c-----------------------------------------------------------------------
c
c     INPUT
c            n      : number of values (n > 1)                   integer
c            p      : number of asset(s) (n >= 1)                integer
c            X      : value(s) (n*p)                              double
c            lambda : exponential parameter                       double
c
c     WORKSPACE
c            dwork  : vector ( p*(n + p + 1) )                    double
c
c     OUTPUT
c            corr   : exp. weighted correlation matrix (p*p)      double
c            info   : diagnostic argument                        integer
c
c     CALL
c            COVEXP : empiric exp. weighted covariance matrix
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     arguments i/o
      INTEGER n, p, info
      DOUBLE PRECISION lambda, X(n, *), corr(p,*)
c
c     workspaces
      DOUBLE PRECISION dwork(*)
c
c     local variables
      INTEGER pdwork, pdw, pdcov, pdsqr, i, j, kii, kij
      DOUBLE PRECISION var, eps
      PARAMETER ( eps = 1.E-30 )
c
c     external subroutines
      EXTERNAL covexp
c
c     intrinsic functions
      INTRINSIC SQRT
c
c-----------------------------------------------------------------------
c
c     initialization
      info = 0
c
c     pointers for double precision work space  : dwork
c     -------------------------------------------------
      pdwork = 1
      pdw  = pdwork
c     pdw   : pointer for COVEXP, so ( p*(n+1) ) more
      pdcov = pdw + ( p*(n + 1) )
c     pdcov   : pointer for cov. matrix, so ( p*p ) more
      pdsqr = pdwork + ( p*p )
c     psqr   : pointer for square root of diagonal, so ( p ) more
c
c     Total size of dwork array = p*(n + p + 1)
c
c-----------------------------------------------------------------------
c
c     empirical exponential weighted covariance matrix
      CALL covexp ( n, p, X, lambda, dwork(pdw), dwork(pdcov), info)
      IF (info .LT. 0) RETURN
c
c     standard deviation:  square root of diag(cov)
      DO i = 1,p
         kii = pdcov + (i - 1)*(p + 1)
         var = dwork(kii)
         IF ( var .GT. eps ) THEN
            dwork(pdsqr+i-1)= SQRT(var)
         ELSE
            info = -5
            RETURN
         ENDIF
      ENDDO
c
c     empirical correlation matrix
      corr(1,1) = 1.0
      DO j = 2,p
         DO i = 1,j
            kij       = pdcov + (i - 1)*p + (j - 1)
            corr(i,j) = dwork(kij)/( dwork(pdsqr+i-1)*dwork(pdsqr+j-1) )
            IF ( i .NE. j ) corr(j, i) = corr(i, j)
         ENDDO
         corr(j, j) = 1.0
      ENDDO
      RETURN
      END


program rzf

double precision pi, pi_squared_over_6, zeta_2, zeta
integer*8 N
pi = 3.1415926535897
pi_squared_over_6 = pi*pi / 6.0
N = 16000000000_8
zeta_2 = zeta(N,2_8)

print *,"    zeta(2) = ",zeta_2
print *,"pi^2 / 6.0  = ",pi_squared_over_6
print *,"      error = ",pi_squared_over_6 - zeta_2

end

double precision function zeta(N,a)
integer*8 N,a,k
double precision sum

sum = 0.0
do k=N-1,1,-1
   sum = sum + 1.0d0/(dble(k)**a)
enddo
zeta = sum

end function


      double complex function Fsc2(j1,j2,j3,j4,j5,j6,za,zb)
      implicit none
      include 'constants.f'
      include 'zprods_decl.f'
      include 'sprods_com.f'
      integer j1,j2,j3,j4,j5,j6
      double complex L1
      double precision t
      Fsc2=
     .za(j3,j5)**2/(2d0*za(j1,j2)*za(j2,j3)*za(j5,j6)*zb(j3,j4))+
     .zb(j2,j6)**2/(2d0*za(j1,j2)*zb(j2,j3)*zb(j3,j4)*zb(j5,j6))-
     .(za(j1,j3)*((L1(-t(j1,j2,j3),-s(j2,j3))*za(j2,j3)**2*zb(j1,j2)**2*
     .(za(j1,j2)*zb(j2,j6)+za(j1,j3)*zb(j3,j6))**2)/s(j2,j3)**2+
     .L1(-s(j5,j6),-t(j1,j2,j3))*za(j3,j4)**2*zb(j4,j6)**2))/
     .(2d0*za(j1,j2)*za(j2,j3)*(za(j1,j2)*zb(j2,j4)+za(j1,j3)*zb(j3,j4))
     .*
     .zb(j5,j6)*t(j1,j2,j3))-(zb(j2,j4)*
     .(L1(-s(j5,j6),-t(j2,j3,j4))*za(j1,j5)**2*zb(j1,j2)**2+
     .(L1(-t(j2,j3,j4),-s(j2,j3))*za(j3,j4)**2*zb(j2,j3)**2*
     .(-(za(j2,j5)*zb(j2,j4))-za(j3,j5)*zb(j3,j4))**2)/s(j2,j3)**2))/
     .(2d0*za(j5,j6)*zb(j2,j3)*zb(j3,j4)*
     .(za(j1,j2)*zb(j2,j4)+za(j1,j3)*zb(j3,j4))*t(j2,j3,j4))
      return
      end


Subroutine gaussian_marsaglia(n, avg, sdv, dist, limits)
  ! Generates a gaussian distribution of n points

  ! Arguments
  Integer, intent(IN) :: n                                     ! Number of points
  Double Precision, intent(IN) :: avg                          ! mu
  Double Precision, intent(IN) :: sdv                          ! sigma
  Double Precision, intent(IN) :: limits(2)                    ! Assumes limit(1) is less than zero

  ! Output
  Double Precision, intent(OUT) :: dist(n)

  ! Locals
  Double Precision :: v1(n), v2(n), s(n), z(n)
  Double Precision :: a, b

  Double Precision, parameter :: zero=0.d0, half=0.5d0, one=1.d0, two=2.d0
  ! -------------------------------------------------------------------- !

  v1 = zero
  v2 = zero
  s = zero

  Do I = 1,n
    Do While (s(I) >= one .OR. s(I) == zero)
      Call RANDOM_NUMBER(a)
      v1(i) = two*a - one
      Call RANDOM_NUMBER(b)
      v2(i) = two*b - one
      s(i) = v1(i)**two + v2(i)**two
    End Do
    s(i) = SQRT((-two*LOG(s(i)))/s(i))
    z(i) = v1(i)*s(i)
    dist(i) = avg + z(i)*sdv
  End Do

  If (.NOT. (limits(1) == zero .AND. limits(2) == zero)) Then
    Do I = 1,n
      If (dist(I) .GT. limits(2)) Then
        dist(I) = dist(I) - two*(limits(2))
      Else If (dist(I) .LT. limits(1)) Then
        dist(I) = dist(I) - two*(limits(1))
      End If
    End Do
  End If

  Return
End Subroutine gaussian_marsaglia

Subroutine lognormal(n, avg, sdv, dist)
  ! Generates a gaussian distribution of n points

  ! Arguments
  Integer, intent(IN) :: n                                     ! Number of points
  Double Precision, intent(IN) :: avg                          ! mu
  Double Precision, intent(IN) :: sdv                          ! sigma

  ! Output
  Double Precision, intent(OUT) :: dist(n)

  ! Locals
  Double Precision :: normal(n)

  Double Precision, parameter :: zero=0.d0, half=0.5d0, one=1.d0, two=2.d0
  ! -------------------------------------------------------------------- !

  Call gaussian_marsaglia(n, LOG10(avg), sdv, normal, (/zero, zero/))
  Do I = 1,n
    dist(i) = EXP(avg+sdv*normal(i))
  End Do

  Return
End Subroutine lognormal

Subroutine vexternaldb(lOp, i_Array, niArray, r_Array, nrArray)

  Use parameters_Mod

  Implicit Double Precision (a-h, o-z)
  Integer, parameter :: j_sys_Dimension = 2, n_vec_Length = 136, maxblk = n_vec_Length

  Include 'mpif.h'

  ! Arguments
  Dimension i_Array(niArray), r_Array(nrArray)

  ! Contents of i_Array
  Integer, parameter :: i_int_nTotalNodes = 1, i_int_nTotalElements = 2, i_int_kStep = 3, i_int_kInc = 4, i_int_iStatus = 5, i_int_lWriteRestart = 6

  ! Possible values for the lOp argument
  Integer, parameter :: j_int_StartAnalysis = 0, j_int_StartStep = 1, j_int_SetupIncrement = 2, j_int_StartIncrement = 3, j_int_EndIncrement = 4, j_int_EndStep = 5, j_int_EndAnalysis = 6

  ! Possible values for i_Array(i_int_iStatus)
  Integer, parameter :: j_int_Continue = 0, j_int_TerminateStep = 1, j_int_TerminateAnalysis = 2

  ! Contents of r_Array
  Integer, parameter :: i_flt_TotalTime = 1, i_flt_StepTime = 2, i_flt_dTime = 3

  ! Locals
  Integer :: kStep                                     ! Current step number
  Integer :: increment                                      ! Current increment number
  Integer :: analysis_status
  Double Precision :: randomNumbers1(10000), randomNumbers2(10000)
  Double Precision :: randomNumbers3(10000), randomNumbers4(10000)
  Double Precision :: limits(2)
  Integer :: n
  Integer :: values(8)
  Type(parameters) :: p
  Double Precision, Parameter :: zero=0.d0, one=1.d0

  Dimension INTV(1), REALV(1)    ! For Abaqus warning messages
  Character(len=8) CHARV(1)      ! For Abaqus warning messages

  ! Fatigue
  Integer :: fatigue_parameters(2), increment_old
  Double Precision :: cycles_per_increment(1), cycles_per_increment_old
  Double Precision :: cycles, cycles_old
  ! fatigue_parameters(1) is an integer flag for whether the current analysis step is a fatigue step (1 = .TRUE.)
  ! fatigue_parameters(2) describes the current rate of fatigue damage propagation (0 = slow, 1 = in-range, 2 = fast)
  Pointer(ptr_fatigue_int, fatigue_parameters)  ! pointer for fatigue parameters integer array
  Pointer(ptr_fatigue_dbl, cycles_per_increment)  ! pointer for fatigue parameters real array
  Logical :: update_inc2cycles_log  ! flag to update the inc2cycles log

  ! MPI
  Integer :: ABA_COMM_WORLD  ! communicator that Abaqus defines for its worker processes
  Integer :: process_rank  ! Process number or rank
  Integer :: ierr  ! Error status

  ! Job names, file names, and unit numbers
  Character(len=256) :: outputDir, jobName, inc2cycles_filename
  Integer :: lenOutputDir, lenJobName
  Integer, parameter :: inc2cycles_file_unit = 15

  ! Common
  Common /analysis_termination/ analysis_status
  Common randomNumbers1, randomNumbers2, randomNumbers3, randomNumbers4

  Interface UserUtilities

    Function GETCOMMUNICATOR() result(communicator)
      ! Utility function GETCOMMUNICATOR can be called from any Abaqus user subroutine. Returns a communicator (type INTEGER) that
      ! Abaqus defines for its worker processes, similar to MPI_COMM_WORLD. The communicator thus obtained can be used for
      ! subsequent MPI communication routines. In a nonparallel run, communicators do not exist and GET_COMMUNICATOR() returns 0.
      ! See 'Obtaining parallel processes information' in the Abaqus documentation for more details.
      Integer(kind=4) :: communicator
    End Function GETCOMMUNICATOR

  End Interface UserUtilities

  Interface
    ! See 'Allocatable arrays' in the Abaqus documentation for more details on the following functions.
    Function SMAIntArrayCreate(ID, SIZE, INITVAL)
      ! Create or resize a global integer array.
      Integer(kind=8) :: SMAIntArrayCreate  ! Returns an address that can be associated with a Fortran pointer
      Integer(kind=4) :: ID       ! Arbitrary integer chosen by the user, used later to access this array
      Integer(kind=4) :: SIZE     ! max value is INT_MAX ( 2,147,483,647 )
      Integer(kind=4) :: INITVAL  ! initialization value for each element in the array
    End Function SMAIntArrayCreate

    Function SMAIntArrayAccess(ID)
      ! Access an existing global integer array.
      Integer(kind=8) :: SMAIntArrayAccess  ! Returns an address that can be associated with a Fortran pointer
      Integer(kind=4) :: ID  ! Array ID
    End Function SMAIntArrayAccess

    Function SMARealArrayCreateDP(ID, SIZE, INITVAL)
      ! Create or resize a global real array.
      Integer(kind=8) :: SMARealArrayCreateDP  ! Returns a pointer to the newly allocated array
      Integer(kind=4), intent(IN) :: ID        ! Arbitrary integer chosen by the user, used later to locate this array
      Integer(kind=4), intent(IN) :: SIZE      ! max value is INT_MAX ( 2,147,483,647 )
      Double Precision, intent(IN) :: INITVAL  ! (optional) initial value for each element of the array
    End Function SMARealArrayCreateDP

    Function SMARealArrayAccess(ID)
      ! Access an existing global real array.
      Integer(kind=8) :: SMARealArrayAccess  ! Returns an address that can be associated with a Fortran pointer
      Integer(kind=4) :: ID   ! Array ID
    End Function SMARealArrayAccess

  End Interface
  ! -------------------------------------------------------------------- !

  kStep = i_Array(i_int_kStep)
  increment = i_Array(i_int_kInc)

  ! Start of the analysis
  If (lOp == j_int_StartAnalysis) Then

    ! Initialize common analysis_status variable
    analysis_status = 1

    ! Load the CompDam solution parameters
    p = loadParameters(.TRUE.)

    ! Global integer array for fatigue parameters
    ptr_fatigue_int = SMAIntArrayCreate(1, 2, 0)  ! create the pointer for fatigue parameters array
    ptr_fatigue_dbl = SMARealArrayCreateDP(2, 1, p%cycles_per_increment_init)  ! create the pointer for fatigue parameters array

    ! Initialize random numbers for fiber misalignment here
    If (p%fkt_random_seed) Then
      Call DATE_AND_TIME(VALUES=values)
      Call RANDOM_SEED (PUT=(/values(8)/))
    End If
    Call RANDOM_NUMBER(randomNumbers1)
    Call RANDOM_NUMBER(randomNumbers2)

    ! Get a Lognormal distribution (polar)
    n = 10000
    Call lognormal(n, p%fkt_init_misalignment_polar_shape, p%fkt_init_misalignment_polar_scale, randomNumbers3)

    ! Get a Gaussian distribution (azi)
    limits = (/-180.d0, 180.d0/)
    Call gaussian_marsaglia(n, p%fkt_init_misalignment_azi_mu, p%fkt_init_misalignment_azi_sigma, randomNumbers4, limits)

  ! Start of a step
  Else If (lOp == j_int_StartStep) Then

    p = loadParameters()  ! Load the CompDam solution parameters
    ptr_fatigue_int = SMAIntArrayAccess(1)  ! Access the pointer for fatigue parameters

    ! Check to see if this is a fatigue step
    FatigueStepSetup: If (kStep == p%fatigue_step) Then  ! This is a fatigue step

      fatigue_parameters(1) = 1

      ! Create a log file for the fatigue cycles per solution increment
      Call VGETRANK(process_rank)
      If (process_rank == 0) Then
        ptr_fatigue_dbl = SMARealArrayAccess(2)
        Call VGETJOBNAME(jobName, lenJobName)
        Call VGETOUTDIR(outputDir, lenOutputDir)
        inc2cycles_filename = trim(outputDir) // '/' // trim(jobName) // '_inc2cycles.log'
        open(inc2cycles_file_unit, file=inc2cycles_filename, status='replace', action='write')
        write(inc2cycles_file_unit,"(A9, A22, A22)") 'increment', 'cycles_per_increment', 'cycles'
        write(inc2cycles_file_unit,"(I9, ES22.12E2, ES22.12E2)") 0, cycles_per_increment(1), zero
        close(inc2cycles_file_unit)
      End If

    Else FatigueStepSetup  ! This is not a fatigue step

      fatigue_parameters(1) = 0

    End If FatigueStepSetup

  ! End of an increment
  Else If (lOp == j_int_EndIncrement) Then

    ! If analysis_status has been changed, cleanly terminate the analysis
    If (analysis_status == 0) i_Array(i_int_iStatus) = j_int_TerminateAnalysis

    ptr_fatigue_int = SMAIntArrayAccess(1)  ! Access the pointer for fatigue_parameters

    ! Check the rate of fatigue damage progression and adjust solution parameters if necessary
    FatigueIncrement: If (fatigue_parameters(1) == 1) Then

      ptr_fatigue_dbl = SMARealArrayAccess(2)

      ! Synchronize fatigue damage progression parameter across processes
      ABA_COMM_WORLD = GETCOMMUNICATOR()
      If (ABA_COMM_WORLD /= 0) Then
        Call MPI_Allreduce(MPI_IN_PLACE, fatigue_parameters(2), 1, MPI_INTEGER, MPI_MAX, ABA_COMM_WORLD, ierr)
      End If

      FatigueParameterUpdate: If (fatigue_parameters(2) == 1) Then
        Continue
      Else FatigueParameterUpdate
        p = loadParameters()  ! Load the CompDam solution parameters
        update_inc2cycles_log = .FALSE.
        ! If fatigue damage is progressing too slowly...
        If (fatigue_parameters(2) == 0 .AND. cycles_per_increment(1) < p%cycles_per_increment_max) Then
            ! ...increase the number of cycles represented by a solution increment.
          cycles_per_increment(1) = MIN(cycles_per_increment(1)*(one + p%cycles_per_increment_mod), p%cycles_per_increment_max)
          update_inc2cycles_log = .TRUE.
        ! If fatigue damage is progressing too quickly...
        Else If (fatigue_parameters(2) == 2 .AND. cycles_per_increment(1) > p%cycles_per_increment_min) Then
          ! ...decrease the number of cycles represented by a solution increment.
          cycles_per_increment(1) = MAX(cycles_per_increment(1)/(one + p%cycles_per_increment_mod), p%cycles_per_increment_min)
          update_inc2cycles_log = .TRUE.
        End If

        FatigueLog: If (update_inc2cycles_log) Then
          ! Update the inc2cycles log file
          Call VGETRANK(process_rank)
          If (process_rank == 0) Then
            Call VGETJOBNAME(jobName, lenJobName)
            Call VGETOUTDIR(outputDir, lenOutputDir)
            inc2cycles_filename = trim(outputDir) // '/' // trim(jobName) // '_inc2cycles.log'
            open(inc2cycles_file_unit, file=inc2cycles_filename, status='old', action='READWRITE', position='append')
            backspace(inc2cycles_file_unit)
            read(inc2cycles_file_unit,"(I9, ES22.12E2, ES22.12E2)") increment_old, cycles_per_increment_old, cycles_old
            cycles = cycles_old + (increment - increment_old)*cycles_per_increment_old
            write(inc2cycles_file_unit,"(I9, ES22.12E2, ES22.12E2)") increment, cycles_per_increment(1), cycles
            close(inc2cycles_file_unit)
          End If
        End If FatigueLog

      End If FatigueParameterUpdate

      ! Reset fatigue damage progression variable, i.e., fatigue_parameters(2)
      fatigue_parameters(2) = 0

    End If FatigueIncrement

  End If

  Return
End Subroutine vexternaldb


      subroutine splcon(xlat,xlon,nver,verlat,verlon,verrad,
     #                  ncon,icon,con)
      dimension verlat(1)
      dimension verlon(1)
      dimension verrad(1)
      dimension icon(1)
      dimension con(1)
c
      real*8 dd
      real*8 rn
      real*8 dr
      real*8 xrad
      real*8 ver8
      real*8 xla8
c
      xrad=3.14159265358979/180.d0
      ncon=0
      do iver=1,nver
        if(xlat.gt.verlat(iver)-2.*verrad(iver)) then
          if(xlat.lt.verlat(iver)+2.*verrad(iver)) then
            ver8=xrad*(verlat(iver))
            xla8=xrad*(xlat)
            dd=sin(ver8)*sin(xla8)
            dd=dd+cos(ver8)*cos(xla8)*
     #         cos(xrad*(xlon-verlon(iver)))
            dd=acos(dd)/xrad
            if(dd.gt.(verrad(iver))*2.d0) then
            else
              ncon=ncon+1
              icon(ncon)=iver
              rn=dd/(verrad(iver))
              dr=rn-1.d0
              if(rn.le.1.d0) then
                con(ncon)=(0.75d0*rn-1.5d0)*(rn**2)+1.d0
              else if(rn.gt.1.d0) then
                con(ncon)=((-0.25d0*dr+0.75d0)*dr-0.75d0)*dr+0.25d0
              else
                con(ncon)=0.
              endif
            endif
          endif
        endif
      enddo
      return
      end


!>
program p_qho

  use m_io
  use m_parameters , only : PI
  use m_schrodinger , only : shooting_bisection, numerov_cooley

  implicit none

  double precision , parameter :: omega = 1.0d0
  integer , parameter :: n_wf = 4
  integer , parameter :: n_delta = 8

  integer :: n_x
  double precision :: step_size
  double precision , allocatable :: wf(:, :, :)
  double precision :: energies(n_wf, n_delta+1, 2)
  integer :: iterations(n_wf, n_delta, 2)

  double precision , allocatable :: x_grid(:)
  double precision , allocatable :: v_grid(:)
  integer :: status = 0
  integer :: ii, jj, kk, nn

  write (*, *) "qho"

  do jj = 1, n_delta
    ! set n_x and step_size
    n_x = 2 ** (jj + 9)
    step_size = 10.0d0/dble(n_x)

    ! allocate grids and wavefunctions
    allocate(x_grid(n_x))
    allocate(v_grid(n_x))
    allocate(wf(n_x, n_wf, 3))

    ! initialise x_grid
    do ii = 1, n_x
      x_grid(ii) = step_size*(dble(ii) - dble(n_x + 1)/2.0d0)
    end do

    ! initialise v_grid
    do ii = 1, n_x
      v_grid(ii) = 0.5d0*(omega ** 2)*(x_grid(ii) ** 2)
    end do

    ! initialise default wf
    wf(:, :, :) = 0.0d0

    ! calculate wavefunctions and energies using shooting_bisection
    do nn = 1, n_wf
      call shooting_bisection(n_x, step_size, x_grid, omega, v_grid, nn-1, &
          wf(:, nn, 1), energies(nn, jj, 1), iterations(nn, jj, 1), status)

      ! terminate subroutine if shooting_bisection failed
      if (status /= 0) then
        write (*, *) "shooting_bisection() failed"
        write (*, *) "exiting qho"
        call exit(status)
      end if
    end do

    ! calculate wavefunctions and energies using numerov_cooley
    do nn = 1, n_wf
      call numerov_cooley(n_x, step_size, x_grid, omega, v_grid, nn-1, &
          wf(:, nn, 2), energies(nn, jj, 2), iterations(nn, jj, 2), status)

      ! terminate subroutine if numerov_cooley failed
      if (status /= 0) then
        write (*, *) "numerov_cooley() failed"
        write (*, *) "exiting qho"
        call exit(status)
      end if
    end do

    ! calculate analytic wavefunctions of first 4 states
    do nn = 1, min(n_wf, 4)
      do kk = 1, n_x
        wf(kk, nn, 3) = hermite(nn, sqrt(omega)*x_grid(kk))
      end do
    end do

    ! ! display
    ! write (*, *) "wavefunctions:"
    ! do nn = 1, min(n_wf, 4)
    !   write (*, *) "n = ", int_trim(nn)
    !   write (*, *) "shooting_bisection:"
    !   call display_graph(n_x, x_grid, wf(:, nn, 1))
    !   write (*, *) "numerov_cooley:"
    !   call display_graph(n_x, x_grid, wf(:, nn, 2))
    !   write (*, *) "analytic:"
    !   call display_graph(n_x, x_grid, wf(:, nn, 3))
    !   write (*, *)
    ! end do

    ! write functions to file
    call write_wf(n_x, x_grid, n_wf, wf)

    ! deallocate grids
    deallocate(x_grid)
    deallocate(v_grid)
    deallocate(wf)
  end do

  ! calculate analytic energies of first 4 states
  do nn = 1, min(n_wf, 4)
    energies(nn, n_delta+1, 1) = omega*(dble(nn)-0.5d0)
    energies(nn, n_delta+1, 2) = omega*(dble(nn)-0.5d0)
  end do

  ! write energies to file
  call write_energies(n_wf, n_delta, energies)

  ! write iterations to file
  call write_iterations(n_wf, n_delta, iterations)

  ! ! display
  ! write (*, *) "energies:"
  ! write (*, *) "shooting_bisection:"
  ! call display_matrix(n_wf, n_delta, energies(:, :, 1))
  ! write (*, *) "numerov_cooley:"
  ! call display_matrix(n_wf, n_delta, energies(:, :, 2))
  ! write (*, *) "analytic:"
  ! call display_matrix(n_wf, n_delta, energies(:, :, 3))

  ! write (*, *) "iterations:"
  ! write (*, *) "shooting_bisection:"
  ! call display_matrix(n_wf, n_delta, 1.0d0*iterations(:, :, 1))
  ! write (*, *) "numerov_cooley:"
  ! call display_matrix(n_wf, n_delta, 1.0d0*iterations(:, :, 2))

  write (*, *) "end qho"

contains

  ! hermite functions up to n=4
  function hermite (n, x) result (f)
    integer , intent(in) :: n
    double precision , intent(in) :: x
    double precision :: f
    double precision :: a

    f = 0.0d0
    a = (1.0d0/sqrt(sqrt(PI)))*exp(-((x ** 2)/2.0d0))

    select case (n)
    case (1)
      f = a
    case (2)
      f = a*sqrt(2.0d0)*x
    case (3)
      f = a*(((2.0d0*x*x) - 1.0d0)/(sqrt(2.0d0)))
    case (4)
      f = a*(((2.0d0*x*x*x) - (3.0d0*x))/(sqrt(3.0d0)))
    end select

  end function hermite

  ! write wavefunctions to file
  subroutine write_wf (n_x, x_grid, n_wf, wf)
    integer , intent(in) :: n_x
    double precision , intent(in) :: x_grid(n_x)
    integer , intent(in) :: n_wf
    double precision , intent(in) :: wf(n_x, n_wf, 3)
    character(len=100) :: basename

    ! construct basename
    write (basename, *) "output/qho/wf.n_x-", int_trim(n_x), "."

    ! write shooting_bisection
    call write_functions(n_x, x_grid, n_wf, wf(:, :, 1), &
        trim(adjustl(basename))//"sb.txt")

    ! write numerov_cooley
    call write_functions(n_x, x_grid, n_wf, wf(:, :, 2), &
        trim(adjustl(basename))//"nc.txt")

    ! write analytic
    call write_functions(n_x, x_grid, n_wf, wf(:, :, 3), &
        trim(adjustl(basename))//"an.txt")

  end subroutine write_wf

  ! write energies to file
  subroutine write_energies (n_wf, n_delta, energies)
    integer , intent(in) :: n_wf
    integer , intent(in) :: n_delta
    double precision , intent(in) :: energies(n_wf, n_delta+1, 3)
    character(len=100) :: basename

    ! construct basename
    write (basename, *) "output/qho/en."

    ! write shooting_bisection
    call write_matrix(n_wf, n_delta+1, energies(:, :, 1), &
        trim(adjustl(basename))//"sb.txt")

    ! write numerov_cooley
    call write_matrix(n_wf, n_delta+1, energies(:, :, 2), &
        trim(adjustl(basename))//"nc.txt")

  end subroutine write_energies

  ! write iterations to file
  subroutine write_iterations (n_wf, n_delta, iterations)
    integer , intent(in) :: n_wf
    integer , intent(in) :: n_delta
    integer , intent(in) :: iterations(n_wf, n_delta, 2)
    character(len=100) :: basename

    ! construct basename
    write (basename, *) "output/qho/it."

    ! write shooting_bisection
    call write_matrix_int(n_wf, n_delta, iterations(:, :, 1), &
        trim(adjustl(basename))//"sb.txt")

    ! write numerov_cooley
    call write_matrix_int(n_wf, n_delta, iterations(:, :, 2), &
        trim(adjustl(basename))//"nc.txt")

  end subroutine write_iterations

end program p_qho


c=======================================================================
c
c     Statistic Utilies
c
c-----------------------------------------------------------------------
c
c        VARFAC  : explain variance by p factors
c        COVIND  : covariance of assets/index
c        COVR    : covariance matrix from centered returns
c        COVE    : exponentially weighted covariance matrix from centered returns
c        VARIAN  : variance
c        HVARIAN : variance on an history of assets
c        VOLAT   : volatility (standard deviation)
c        HVOLAT  : volatility on an history of assets
c        SEMVAR  : semi-variance ( variance(v), v(i) < mean(v(i)) )
c        SEMVOL  : semi-volatility
c        COVCOR  : conversion covariance  -> correlation
c        CORCOV  : conversion correlation -> covariance
c
c=======================================================================
c
c     subroutine VARFAC
c
c     explain variance by p factors
c
c-----------------------------------------------------------------------
      SUBROUTINE VARFAC ( N, eig, nfact, var, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N      : dimension of the eigenvalues vector             integer
c       eig(N) : sorted eigenvalues vector (N)                    double
c       nfact  : number of factors                               integer
c
c     OUTPUT
c       var    : variance of nbfact factors                       double
c       info   : diagnostic argument                             integer
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o parameters
      INTEGER N, nfact, info
      DOUBLE PRECISION var, eig(*)
c
c     local variables
      DOUBLE PRECISION vart, EPS
      PARAMETER ( EPS = 1.E-30 )
c
c     initialization
      info  = 0
c
c     total variance (sum of the eigenvalues)
      CALL SEV ( N, eig, vart )
      IF ( ABS(vart) .LT. EPS ) THEN
         info = -1
         RETURN
      ENDIF
c
c     explained variance
      CALL SEV ( nfact, eig, var)
      var = var / vart
      RETURN
      END
c
c=======================================================================
c
c     subroutine COVIND
c
c     covariance between assets/index
c
c-----------------------------------------------------------------------
      SUBROUTINE COVIND ( p, n, X, Y, meanY, meanX, cov, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       p     : number of assets                                 integer
c       n     : number of returns                                integer
c       X     : assets matrix of returns (n*p)                    double
c       Y     : index vector of returns (n)                       double
c
c     OUTPUT
c       meanY : mean return of index                              double
c       meanX : mean returns vector (n)                           double
c       cov   : covariance vector (n)                             double
c       info  : diagnostic argument                              integer
c
c     CALL
c       MV    : mean of a vector
c       MCM   : mean of each column of a vectorized full matrix
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER p, n, info
      DOUBLE PRECISION meanY, meanX(*), X(n,*), Y(*), cov(*)
c
c     local variables
      INTEGER i, j
      DOUBLE PRECISION sum, tmp
c
c     initialization
      info = 0
c
c     test if ndate < 2
      IF ( n .LT. 2 ) THEN
         info = -2
         RETURN
      ENDIF
c
c     mean of index and assets
      CALL MV ( n, Y, meanY )
      CALL MCM ( n, p, X, meanX )
c
c     cov = E(X)*E(Y) - E(X*Y)
      tmp = 1./FLOAT(n - 1)
      DO i = 1,p
         sum = 0.
         DO j = 1,n
            sum = sum + X(j,i) * Y(j)
         ENDDO
         cov(i) = tmp * ( sum - n*meanY*meanX(i) )
      ENDDO
      RETURN
      END
c
c=======================================================================
c
c     subroutine COVR
c
c     covariance matrix from centered returns
c
c     cov(M,M) = (1/(N-1)) * X(N,M)'X(N,M) where X are centered returns
c
c-----------------------------------------------------------------------
      SUBROUTINE COVR ( N, M, X, cov, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N        : number of rows                                integer
c       M        : number of columns                             integer
c       X(N*M)   : cebtered returns matrix (N*M)                  double
c
c     OUTPUT
c       cov(M*M) : covariance matrix (M*M)                        double
c       info     : diagnostic argument                           integer
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, M, info
      DOUBLE PRECISION alpha, X(N,*), cov(M,*)
c
c     initialization
      info = 0
c
c     test if N < 2
      if ( N .LT. 2 ) THEN
         info = -2
         RETURN
      ENDIF
c
c     matrix product, cov(M,M) <- X'(N,M)*X(N,M)
      CALL PRMM ( N, M, X, cov )
c
c     scales a matrix by a constant, cov(M,M) <- alpha*cov(M,M)
      alpha = 1./FLOAT(N-1)
      CALL PMX2 ( M, M, cov, alpha)
      RETURN
      END
c=======================================================================
c
c     subroutine COVE
c
c     exponential weighted covariance matrix from centered returns
c
c-----------------------------------------------------------------------
      SUBROUTINE COVE ( N, M, X, lambda, cov, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N        : number of rows                                integer
c       M        : number of columns                             integer
c       X(N*M)   : centered returns matrix (N*M)                  double
c       lambda   : decay factor                                   double
c
c     OUTPUT
c       cov(M*M) : exponential covariance matrix (M*M)            double
c       info     : diagnostic argument                           integer
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, M, info
      DOUBLE PRECISION lambda, X(N,*), cov(M,*)
c
c     local variables
      INTEGER i, j, imat
      DOUBLE PRECISION sum, tmp, EPS
      PARAMETER( EPS = 1.E-4)
c
c     initialization
      info = 0
c
c     test N > 2
      IF ( N .LT. 2 ) THEN
         info = -2
         RETURN
      ENDIF
c
c     case lambda > 0.9999
      IF (lambda .GT. (1.-EPS)) THEN
         CALL COVR ( N, M, X, cov, info )
         RETURN
      ENDIF
c
      IF (lambda .LT. 1.E-15) THEN
         tmp = 1.0
      ELSE
        tmp = 0.0
        DO i = 1,N
           tmp = tmp + (lambda**(i-1))
        ENDDO
        tmp = 1./(tmp - 1.)
      ENDIF
      DO j = 1,M
         DO i = 1,j
            sum = 0.0
            DO imat = 1,N
               sum = sum + (lambda**(imat-1))*(X(imat,i)*X(imat,j))
            ENDDO
            cov(i,j) = tmp * sum
            IF ( i .NE. j ) cov(j,i) = cov(i,j)
         ENDDO
      ENDDO
      RETURN
      END
c
c=======================================================================
c
c     subroutine VARIAN
c
c     Equally Weighted (Empirical) Variance
c
c-----------------------------------------------------------------------
      SUBROUTINE VARIAN ( N, X, var, info )
c-----------------------------------------------------------------------
c
c     INPUT
c            N    : number of values                             integer
c            X(N) : input data, values (N)                        double
c
c     OUTPUT
c            var  : variance                                      double
c            info : diagnostic argument                          integer
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, info
      DOUBLE PRECISION var, X(*)
c
c     local variables
      INTEGER i
      DOUBLE PRECISION sum, sum2
c
c     initialization
      info = 0
      var  = 0.0
      sum  = 0.0
      sum2 = 0.0
c
c     test if N < 2
      IF ( N .LT. 2 ) THEN
         info = -2
         RETURN
      ENDIF
c
c     square sum and sum of square
      DO i = 1,N
         sum = sum + X(i)
         sum2 = sum2 + X(i)*X(i)
      ENDDO
c
c     variance
      var = ( sum2 - (sum*sum/N) ) / ( N - 1 )
      RETURN
      END
c
c=======================================================================
c
c     subroutine HVARIAN
c
c     Variance of P assets
c
c-----------------------------------------------------------------------
      SUBROUTINE HVARIAN ( N, P, X, var, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N      : number of values (N > 1)                        integer
c       P      : number of assets (P > 0)                        integer
c       X(N*P) : input data, values (N*P)                         double
c     OUTPUT
c       var(P) : variance of each asset, vector (P)               double
c       info   : diagnostic argument                             integer
c
c     CALL
c       VARIAN : variance
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, P, info
      DOUBLE PRECISION var(*), X(N,*)
c
c     local variables
      INTEGER i
c
c     initialization
      info = 0
c
      DO i = 1,P
         CALL VARIAN ( N, X(1,i), var(i), info )
      ENDDO
      RETURN
      END
c
c=======================================================================
c
c     subroutine VOLAT
c
c     Volatility (standard deviation)
c
c-----------------------------------------------------------------------
      SUBROUTINE VOLAT ( N, X, vol, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N      : number of values (N > 1)                        integer
c       X(N)   : input data, values (N)                           double
c
c     OUTPUT
c       vol    : volatility                                       double
c       info   : diagnostic argument                             integer
c
c     CALL
c       VARIAN : variance
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, info
      DOUBLE PRECISION vol, X(*)
c
c     local variables
      DOUBLE PRECISION var, EPS
      PARAMETER ( EPS = 1.E-30)
c
c     initialization
      info = 0
      vol  = 0.0
c
c     variance
      CALL VARIAN ( N, X, var, info )
      IF (info .LT. 0) RETURN
c
c     test if variance is too small
      IF (var .LT. EPS) THEN
        vol = 0.0
        RETURN
      ELSE
c
c       volatility (standard deviation)
        vol = SQRT(var)
      ENDIF
      RETURN
      END
c
c=======================================================================
c
c     subroutine HVOLAT
c
c     Volatility of P assets
c
c-----------------------------------------------------------------------
      SUBROUTINE HVOLAT ( N, P, X, vol, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N      : number of dates (N > 1)                         integer
c       P      : number of assets (P > 0)                        integer
c       X(N*P) : input data, values (N*P)                         double
c
c     OUTPUT
c       vol(P) : volatility of each asset, vector (P)             double
c       info   : diagnostic argument                             integer
c
c     CALL
c       VOLAT  : volatility
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, P, info
      DOUBLE PRECISION vol(*), X(N,*)
c
c     local variables
      INTEGER i
c
c     initialization
      info = 0
c
      DO i = 1,P
         CALL VOLAT ( N, X(1,i), vol(i), info )
      ENDDO
      RETURN
      END
c
c=======================================================================
c
c     subroutine SEMVAR
c
c     Semi-variance ( variance(v), for v(i) < mean(v(i)) )
c
c-----------------------------------------------------------------------
      SUBROUTINE SEMVAR ( N, X, svar, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N    : number of values (N > 1)                          integer
c       X(N) : input data, values (N)                             double
c
c     OUTPUT
c       svar : semi-variance                                      double
c       info : diagnostic argument                               integer
c
c     CALL
c       MV   : mean of a vector
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, info
      DOUBLE PRECISION svar, X(*)
c
c     local variables
      INTEGER i
      DOUBLE PRECISION mu, sum, sum2, tmpx
c
c     initialization
      info = 0
      svar = 0.0
      sum  = 0.
      sum2 = 0.
      tmpx = 0.
c
c     test if N > 1
      IF ( N .LT. 2. ) THEN
         info = -2
         RETURN
      ENDIF
c
c     mean of X
      CALL MV ( N, X, mu )
c
      DO i = 1,N
         tmpx = MIN(X(i) - mu, 0.0)
         sum  = sum + tmpx
         sum2 = sum2 + (tmpx * tmpx)
      ENDDO
c
c     semi-variance of X
      svar = ( sum2 - (sum * sum / N) ) / ( N - 1. )
      RETURN
      END
c
c=======================================================================
c
c     subroutine SEMVOL
c
c     Semi-volatility ( volatility(v), v(i) < mean(v(i)) )
c
c-----------------------------------------------------------------------
      SUBROUTINE SEMVOL ( N, X, svol, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N      : number of values (N > 1)                        integer
c       X(N)   : input data, values (N)                           double
c
c     OUTPUT
c       svol   : semi-volatility                                  double
c       info   : diagnostic argument                             integer
c
c     CALL
c       SEMVAR : semi-variance ( variance(x), x(i) < mean(x(i)) )
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, info
      DOUBLE PRECISION svol, X(*)
c
c     local variables
      DOUBLE PRECISION svar
c
c     initialization
      info = 0
      svol = 0.0
c
c     semi-variance
      CALL SEMVAR ( N, X, svar, info )
      IF (info .LT. 0) RETURN
c
c     semi-volatility (semi-standard deviation)
      svol = SQRT(svar)
      RETURN
      END
c
c=======================================================================
c
c     subroutine COVCOR
c
c     covariance matrix --> correlation matrix
c
c-----------------------------------------------------------------------
      SUBROUTINE COVCOR ( N, cov, corr, vol, info )
c-----------------------------------------------------------------------
c
c     INPUT
c       N         : size of the covariace matrix                 integer
c       cov(N*N)  : covariance matrix (N*N)                       double
c
c     OUTPUT
c       corr(N*N) : correlation, matrix (N*N)                     double
c       vol(N)    : volatilities, vector (N)                      double
c       info      : diagnostic argument                          integer
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N, info
      DOUBLE PRECISION cov(N,*), corr(N,*), vol(*)
c
c     local variables
      INTEGER i, j
      DOUBLE PRECISION vcov, EPS
      PARAMETER ( EPS = 1.E-30 )
c
c     initialization
      info = 0
c
c     extracting volatilities
      DO i = 1,N
         vcov = cov(i,i)
         IF ( vcov .GT. EPS ) THEN
            vol(i)= SQRT(vcov)
         ELSE
            info = -1
            RETURN
         ENDIF
      ENDDO
c
c     building the correlation matrix
      DO j = 1,N
         DO i = 1,j
            IF ( i .NE. j ) THEN
               corr(i,j) = cov(i,j)/( vol(i)*vol(j) )
               corr(j,i) = corr(i,j)
            ELSE
               corr(i,i) = 1.
            ENDIF
         ENDDO
      ENDDO
      RETURN
      END
c
c=======================================================================
c
c     subroutine CORCOV
c
c     correlation matrix --> covariance matrix
c
c-----------------------------------------------------------------------
      SUBROUTINE CORCOV ( N, corr, vol, cov )
c-----------------------------------------------------------------------
c
c     INPUT
c       N         : size of the covariace matrix                 integer
c       corr(N*N) : correlation matrix (N*N)                      double
c       vol(N)    : volatilities vector (N)                       double
c
c     OUTPUT
c       cov(N*N)  : covariance matrix (N*N)                       double
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     i/o arguments
      INTEGER N
      DOUBLE PRECISION corr(N,*), cov(N,*), vol(*)
c
c     local variables
      INTEGER i, j
c
      DO j = 1,N
         DO i = 1,j
            cov(i,j) = corr(i,j) * vol(i) * vol(j)
            IF ( i .NE. j ) cov(j,i) = cov(i,j)
         ENDDO
      ENDDO
      RETURN
      END


      FUNC(X) = X * X - 5.              ! function whose root is sought
      TOLX = 1.E-6                      ! tolerance for the search
      X = -3.                           ! initial guess
      FOLD = FUNC(X)                    ! initial function
      DX = 6.                           ! initial step
      ITER = 0                          ! initialize count
 10   CONTINUE
        ITER = ITER + 1                 ! increment iteration count
        X = X + DX                      ! step X
        PRINT *, ITER, X, SQRT(5.)-X    ! output current values
        IF ((FOLD*FUNC(X)).LT.0) THEN
          X = X - DX                    ! if sign changes, back up and
          DX = DX / 2                   !   halve the step
        END IF
        IF(ITER.GE.100) THEN            ! terminate if iteration is
          PRINT *,'Iteration too many!' !   too large
          STOP
        END IF
      IF (ABS(DX).GT.TOLX) GOTO 10
      STOP
      END


!==============================================================================!
  subroutine Residual_Vector(ni, r, b, a, x)
!------------------------------------------------------------------------------!
!   Calculates residual vector {r} = {b} - [A]{x}                              !
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  integer           :: ni
  real              :: r(:)  ! this might be only for inner cells
  real              :: b(:)  ! this might be only for inner cells
  type(Matrix_Type) :: a
  real              :: x(:)  ! this may incude buffer cells
!-----------------------------------[Locals]-----------------------------------!
  integer  :: i, j, k
!==============================================================================!

  !----------------!
  !   r = b - Ax   !
  !----------------!
  ! Why not callig this: call exchange(x) ???
  do i = 1, ni
    r(i) = b(i)
    do j = a % row(i), a % row(i+1) - 1
      k = a % col(j)
      r(i) = r(i) - a % val(j) * x(k)
    end do
  end do

  end subroutine


*DECK DTRDI
      SUBROUTINE DTRDI (T, LDT, N, DET, JOB, INFO)
C***BEGIN PROLOGUE  DTRDI
C***PURPOSE  Compute the determinant and inverse of a triangular matrix.
C***LIBRARY   SLATEC (LINPACK)
C***CATEGORY  D2A3, D3A3
C***TYPE      DOUBLE PRECISION (STRDI-S, DTRDI-D, CTRDI-C)
C***KEYWORDS  DETERMINANT, INVERSE, LINEAR ALGEBRA, LINPACK,
C             TRIANGULAR MATRIX
C***AUTHOR  Moler, C. B., (U. of New Mexico)
C***DESCRIPTION
C
C     DTRDI computes the determinant and inverse of a double precision
C     triangular matrix.
C
C     On Entry
C
C        T       DOUBLE PRECISION(LDT,N)
C                T contains the triangular matrix.  The zero
C                elements of the matrix are not referenced, and
C                the corresponding elements of the array can be
C                used to store other information.
C
C        LDT     INTEGER
C                LDT is the leading dimension of the array T.
C
C        N       INTEGER
C                N is the order of the system.
C
C        JOB     INTEGER
C                = 010       no det, inverse of lower triangular.
C                = 011       no det, inverse of upper triangular.
C                = 100       det, no inverse.
C                = 110       det, inverse of lower triangular.
C                = 111       det, inverse of upper triangular.
C
C     On Return
C
C        T       inverse of original matrix if requested.
C                Otherwise unchanged.
C
C        DET     DOUBLE PRECISION(2)
C                determinant of original matrix if requested.
C                Otherwise not referenced.
C                DETERMINANT = DET(1) * 10.0**DET(2)
C                with  1.0 .LE. ABS(DET(1)) .LT. 10.0
C                or  DET(1) .EQ. 0.0 .
C
C        INFO    INTEGER
C                INFO contains zero if the system is nonsingular
C                and the inverse is requested.
C                Otherwise INFO contains the index of
C                a zero diagonal element of T.
C
C
C***REFERENCES  J. J. Dongarra, J. R. Bunch, C. B. Moler, and G. W.
C                 Stewart, LINPACK Users' Guide, SIAM, 1979.
C***ROUTINES CALLED  DAXPY, DSCAL
C***REVISION HISTORY  (YYMMDD)
C   780814  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890831  Modified array declarations.  (WRB)
C   890831  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900326  Removed duplicate information from DESCRIPTION section.
C           (WRB)
C   920501  Reformatted the REFERENCES section.  (WRB)
C***END PROLOGUE  DTRDI
      INTEGER LDT,N,JOB,INFO
      DOUBLE PRECISION T(LDT,*),DET(2)
C
      DOUBLE PRECISION TEMP
      DOUBLE PRECISION TEN
      INTEGER I,J,K,KB,KM1,KP1
C***FIRST EXECUTABLE STATEMENT  DTRDI
C
C        COMPUTE DETERMINANT
C
         IF (JOB/100 .EQ. 0) GO TO 70
            DET(1) = 1.0D0
            DET(2) = 0.0D0
            TEN = 10.0D0
            DO 50 I = 1, N
               DET(1) = T(I,I)*DET(1)
               IF (DET(1) .EQ. 0.0D0) GO TO 60
   10          IF (ABS(DET(1)) .GE. 1.0D0) GO TO 20
                  DET(1) = TEN*DET(1)
                  DET(2) = DET(2) - 1.0D0
               GO TO 10
   20          CONTINUE
   30          IF (ABS(DET(1)) .LT. TEN) GO TO 40
                  DET(1) = DET(1)/TEN
                  DET(2) = DET(2) + 1.0D0
               GO TO 30
   40          CONTINUE
   50       CONTINUE
   60       CONTINUE
   70    CONTINUE
C
C        COMPUTE INVERSE OF UPPER TRIANGULAR
C
         IF (MOD(JOB/10,10) .EQ. 0) GO TO 170
            IF (MOD(JOB,10) .EQ. 0) GO TO 120
                  DO 100 K = 1, N
                     INFO = K
                     IF (T(K,K) .EQ. 0.0D0) GO TO 110
                     T(K,K) = 1.0D0/T(K,K)
                     TEMP = -T(K,K)
                     CALL DSCAL(K-1,TEMP,T(1,K),1)
                     KP1 = K + 1
                     IF (N .LT. KP1) GO TO 90
                     DO 80 J = KP1, N
                        TEMP = T(K,J)
                        T(K,J) = 0.0D0
                        CALL DAXPY(K,TEMP,T(1,K),1,T(1,J),1)
   80                CONTINUE
   90                CONTINUE
  100             CONTINUE
                  INFO = 0
  110          CONTINUE
            GO TO 160
  120       CONTINUE
C
C              COMPUTE INVERSE OF LOWER TRIANGULAR
C
               DO 150 KB = 1, N
                  K = N + 1 - KB
                  INFO = K
                  IF (T(K,K) .EQ. 0.0D0) GO TO 180
                  T(K,K) = 1.0D0/T(K,K)
                  TEMP = -T(K,K)
                  IF (K .NE. N) CALL DSCAL(N-K,TEMP,T(K+1,K),1)
                  KM1 = K - 1
                  IF (KM1 .LT. 1) GO TO 140
                  DO 130 J = 1, KM1
                     TEMP = T(K,J)
                     T(K,J) = 0.0D0
                     CALL DAXPY(N-K+1,TEMP,T(K,K),1,T(K,J),1)
  130             CONTINUE
  140             CONTINUE
  150          CONTINUE
               INFO = 0
  160       CONTINUE
  170    CONTINUE
  180 CONTINUE
      RETURN
      END


program test_cond_orth_band
  use mod_orrb
  implicit none
  integer(kind=int32) :: na, lbw, ubw
  type(error_info) :: error
  real(kind=dp), parameter :: tol=1.0e-15_dp, tol1=10*tol
  character(len=*), parameter :: fmt_sv="(A15, '; SV: ',ES8.2, ', Res1: ', ES8.2, ', Res2: ', ES8.2, ', Res3: ', ES8.2, ', ', A10)"
  character(len=40) :: test_name

  real(kind=dp), dimension(:), allocatable :: u_d, v_d, res_d, res_d1
  real(kind=dp) :: sigma_d, tmp
  real(kind=dp), allocatable, dimension(:,:) :: a_d
  complex(kind=dp), allocatable, dimension(:,:) :: a_z

  complex(kind=dp), dimension(:), allocatable :: u_z, v_z, res_z, res_z1
  real(kind=dp) :: sigma_z

  type(d_ub), allocatable :: ub_d
  type(z_ub), allocatable :: ub_z

  call initialize_errors
  print *
  print *, "--------------------------------"
  print *
  print *, "Real UB Condition Estimation Tests"
  print *

  na=50; lbw=0; ubw=5
  allocate(u_d(na),v_d(na),res_d(na),res_d1(na))
  ub_d=d_random_ub(na,lbw,ubw,error=error)
  a_d=general(ub_d, error)
  sigma_d=d_ub_min_sv(ub_d,u_d,v_d,res_d,tol,1000,error)
  tmp=norm2(res_d)
  res_d=matmul(a_d,v_d)-sigma_d*u_d
  res_d1=matmul(u_d,a_d)-sigma_d*v_d
  test_name='UB Min SV'
  call output_result(test_name,sigma_d,tmp,norm2(res_d),norm2(res_d1),tol1,error)
  deallocate(u_d,v_d,res_d,res_d1)

  na=10; lbw=3; ubw=5
  allocate(u_d(na),v_d(na),res_d(na),res_d1(na))
  ub_d=d_random_ub(na,lbw,ubw,error=error)
  a_d=general(ub_d, error)
  sigma_d=d_ub_max_sv(ub_d,u_d,v_d,res_d,tol,1000,error)
  tmp=norm2(res_d)
  res_d=matmul(a_d,v_d)-sigma_d*u_d
  res_d1=matmul(u_d,a_d)-sigma_d*v_d
  test_name='UB Max SV'
  call output_result(test_name,sigma_d,tmp,norm2(res_d),norm2(res_d1),tol1,error)
  deallocate(u_d,v_d,res_d,res_d1)

  print *
  print *, "--------------------------------"
  print *
  print *, "Complex UB Condition Estimation Tests"
  print *

  na=50; lbw=0; ubw=5
  allocate(u_z(na),v_z(na),res_z(na),res_z1(na))
  ub_z=z_random_ub(na,lbw,ubw,error=error)
  a_z=general(ub_z, error)
  sigma_z=z_ub_min_sv(ub_z,u_z,v_z,res_z,tol,100,error)
  tmp=norm2(res_z)
  res_z=matmul(a_z,v_z)-sigma_z*u_z
  res_z1=matmul(conjg(u_z),a_z)-sigma_z*conjg(v_z)
  test_name='UB Min SV'
  call output_result(test_name,sigma_z,tmp,norm2(res_z),norm2(res_z1),tol1,error)
  deallocate(u_z,v_z,res_z,res_z1)

  na=10; lbw=3; ubw=5
  allocate(u_z(na),v_z(na),res_z(na),res_z1(na))
  ub_z=z_random_ub(na,lbw,ubw,error=error)
  a_z=general(ub_z, error)
  sigma_z=z_ub_max_sv(ub_z,u_z,v_z,res_z,tol,1000,error)
  tmp=norm2(res_z)
  res_z=matmul(a_z,v_z)-sigma_z*u_z
  res_z1=matmul(conjg(u_z),a_z)-sigma_z*conjg(v_z)
  test_name='UB Max SV'
  call output_result(test_name,sigma_z,tmp,norm2(res_z),norm2(res_z1),tol1,error)
  deallocate(u_z,v_z,res_z,res_z1)

contains

  subroutine output_result(name,sv,res1,res2,res3,bnd,error)
    character(len=*) :: name
    real(kind=dp) :: bnd, sv, res1, res2, res3
    type(error_info) :: error
    character(len=10) :: test_result

    if (error%code > 0) then
       print *, "Calling error in test: ", name
    else
       if (res1 < bnd .and. res2 < bnd .and. res3 < bnd) then
          test_result="PASSED"
       else
          test_result="    FAILED"
       end if
       write (*,fmt_sv) name, sv, res1, res2, res3, test_result
    end if
  end subroutine output_result

end program test_cond_orth_band


*
* aveetc - Program which demonstrates the read loop for processing
* an unknown number of input lines
*
* This program reads any number of values from input until end of file
* is reached.  The count, total, and average of the values is printed.
*
* Written by: Charles Severance 10Mar92
*
      INTEGER COUNT
      REAL VALUE,SUM,AVERAGE
*
* Set the sum and count to zero at the beginning
*
      SUM = 0.0
      COUNT = 0.0
*
* Top of the read loop.  Read the values.  If end of file is encountered
* we go to line 20.  In the read statement the first asterisk is where
* to read from and the second is the FORMAT of the input.
*
* NOTE that on UNIX systems like briggs1 End of file can be entered
* by pressing CTRL-D when the program is reading from the terminal.
*
10    CONTINUE
      PRINT *,'Enter a value or End of File -'
      READ (*,*,END=20) VALUE
*
* Accumulate the running total in TOTAL and COUNT in count
*
      TOTAL = TOTAL + VALUE
      COUNT = COUNT + 1
*
      PRINT *,'Value = ', VALUE
      PRINT *,'Running total = ',TOTAL
      PRINT *,'Count = ', COUNT
      GOTO 10
*
* Loop exit when end of file is reached - calculate average and print out
*
   20 CONTINUE
      PRINT *,'We got End of FILE!!'
      AVERAGE = TOTAL/COUNT
      PRINT *,'Average = ', AVERAGE
      END


! { dg-do run }
program main
  implicit none
  integer, parameter :: n=5, m=3
  character(kind=4,len=5), dimension(n) :: a
  character(kind=4,len=5), dimension(n,m) :: b
  character(kind=4,len=5) :: res
  integer, dimension(n,m) :: v
  real, dimension(n,m) :: r
  integer :: i,j
  logical, dimension(n,m) :: mask
  character(kind=4,len=5), dimension(:,:), allocatable :: empty
  integer(kind=4), dimension(5) :: kmin = [-1, -1, -1, -1, -1]
  character(kind=4,len=5) :: all_full
  logical :: smask

  all_full = transfer(kmin,all_full)
  write (unit=a,fmt='(I5.5)') (21-i*i+6*i,i=1,n)
  res = minval(a)
  if (res /= 4_'00026') STOP 1
  do
     call random_number(r)
     v = int(r * 100)
     if (count(v<30) > 1) exit
  end do
  write (unit=b,fmt='(I5.5)') v
  write (unit=res,fmt='(I5.5)') minval(v)
  if (res /= minval(b)) STOP 2
  smask = .true.
  if (res /= minval(b, smask)) STOP 3
  smask = .false.
  if (all_full /= minval(b, smask)) STOP 4

  mask = v < 30
  write (unit=res,fmt='(I5.5)') minval(v,mask)
  if (res /= minval(b, mask)) STOP 5
  mask = .false.
  if (minval(b, mask) /= all_full) STOP 6
  allocate (empty(0:3,0))
  res = minval(empty)
  if (res /= all_full) STOP 7
end program main


program square_ex

  use mathability, only: square
  implicit none

  integer, parameter :: n = 5
  integer :: i
  integer :: x(n) = (/(i, i = 1, n)/)
  integer :: r(n)

  r = square(x)

  write(*,*) "Square array example"
  write(*,*) "Numbers: ", x
  write(*,*) "Their squares: ", r

end program square_ex


! ###################################################################
! Copyright (c) 2013-2015, Marc De Graef/Carnegie Mellon University
! All rights reserved.
!
! Redistribution and use in source and binary forms, with or without modification, are
! permitted provided that the following conditions are met:
!
!     - Redistributions of source code must retain the above copyright notice, this list
!        of conditions and the following disclaimer.
!     - Redistributions in binary form must reproduce the above copyright notice, this
!        list of conditions and the following disclaimer in the documentation and/or
!        other materials provided with the distribution.
!     - Neither the names of Marc De Graef, Carnegie Mellon University nor the names
!        of its contributors may be used to endorse or promote products derived from
!        this software without specific prior written permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
! AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
! IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
! ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
! LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
! DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
! SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
! CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
! OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
! USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
! ###################################################################

!--------------------------------------------------------------------------
! EMsoft:Lambert.f90
!--------------------------------------------------------------------------
!
! MODULE: Lambert
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief everything that has to do with the modified Lambert projections
!
!> @details This module contains a number of projection functions for the modified
!> Lambert projection between square lattice and 2D hemisphere, hexagonal lattice
!> and 2D hemisphere, as well as the more complex mapping between a 3D cubic grid
!> and the unit quaternion hemisphere with positive scalar component.  In addition, there
!> are some other projections, such as the stereographic one.  Each function is named
!> by the projection, the dimensionality of the starting grid, and the forward or inverse
!> character.  For each function, there is also a single precision and a double precision
!> version, but we use the interface formalism to have only a single call.  The Forward
!> mapping is taken to be the one from the simple grid to the curved grid.  Since the module
!> deals with various grids, we also add a few functions/subroutines that apply symmetry
!> operations on those grids.
!>
!> To use any of these routines, one must first call the InitLambertParameters routine
!> to initialize a bunch of constants.
!
!> @date 7/10/13   MDG 1.0 original
!> @date 7/12/13   MDG 1.1 added forward cube to ball to quaternion mappings
!> @date 8/01/13   MDG 1.2 added standard Lambert projection
!> @date 8/12/13   MDG 1.3 added inverse Lambert projections for Ball to Cube
!> @date 9/20/13   MDG 1.4 added ApplyLaueSymmetry
!> @date 08/29/15  MDG 1.5 small changes to hexagonal mapping routines; coordinate swap inside routines
!> @date 09/19/15  MDG 1.6 split off some routines for 3Drotations module
!--------------------------------------------------------------------------
module Lambert

use local
use constants
use quaternions

IMPLICIT NONE

!------------
! public functions and subroutines
!------------

! mappings from the 3D cubic grid to the 3D spherical grid
public :: LambertCubeToBall
interface LambertCubeToBall
        module procedure Lambert3DCubeForwardSingle
        module procedure Lambert3DCubeForwardDouble
end interface

public :: LambertBallToCube
interface LambertBallToCube
        module procedure Lambert3DCubeInverseSingle
        module procedure Lambert3DCubeInverseDouble
end interface

! auxiliary private functions for the cube to sphere mappings
private :: GetPyramidSingle
private :: GetPyramidDouble

contains

!--------------------------------------------------------------------------
! the functions below deal with the cubic grid to the 3D ball, and then from
! the 3D ball to the unit quaternion hemisphere projection
!
! all derivations and equations can be found in
!
! D. Rosca, A. Morawiec, and M. De Graef. “A new method of constructing a grid
! in the space of 3D rotations and its applications to texture analysis”.
! Modeling and Simulations in Materials Science and Engineering 22, 075013 (2014).
!
!--------------------------------------------------------------------------

!--------------------------------------------------------------------------
!
! FUNCTION: Lambert3DCubeForwardSingle
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief map from 3D cubic grid to 3D ball
!
!> @param xyzin 3D coordinates to be considered (single precision)
!> @param ierr error flag 0 = OK, 1 =
!
!> @date 7/12/13    MDG 1.0 original
!> @date 7/7/14  MDG 2.0 correction to cube edge length check
!--------------------------------------------------------------------------
recursive function Lambert3DCubeForwardSingle(xyzin,ierr) result(res)

IMPLICIT NONE

real(kind=sgl),INTENT(IN)       :: xyzin(3)
integer(kind=irg),INTENT(INOUT):: ierr
real(kind=sgl)                  :: res(3)

real(kind=sgl)                  :: XYZ(3), sXYZ(3), T1, T2, c, s, q, LamXYZ(3)
integer(kind=irg)               :: p

ierr = 0
if (maxval(abs(xyzin)).gt.LPs%ap/2) then
  res = (/ 0.0, 0.0, 0.0 /)
  ierr = 1
  return
end if

! determine which pyramid pair the point lies in and copy coordinates in correct order (see paper)
p = GetPyramidSingle(xyzin)
select case (p)
 case (1,2)
  sXYZ = xyzin
 case (3,4)
  sXYZ = (/ xyzin(2), xyzin(3), xyzin(1) /)
 case (5,6)
  sXYZ = (/ xyzin(3), xyzin(1), xyzin(2) /)
end select

! scale by grid parameter ratio sc
XYZ = LPs%sc * sXYZ

! transform to the sphere grid via the curved square, and intercept the zero point
if (maxval(abs(XYZ)).eq.0.0) then
  LamXYZ = (/ 0.0, 0.0, 0.0 /)
else
! intercept all the points along the z-axis
  if (maxval(abs(XYZ(1:2))).eq.0.0) then
    LamXYZ = (/ 0.0, 0.0, sngl(LPs%pref) * XYZ(3) /)
  else  ! this is a general grid point
    if (abs(XYZ(2)).le.abs(XYZ(1))) then
      q = LPs%pi12 * XYZ(2)/XYZ(1)
      c = cos(q)
      s = sin(q)
      q = LPs%prek * XYZ(1) / sqrt(LPs%r2-c)
      T1 = (LPs%r2*c - 1.0) * q
      T2 = LPs%r2 * s * q
    else
      q = LPs%pi12 * XYZ(1)/XYZ(2)
      c = cos(q)
      s = sin(q)
      q = LPs%prek * XYZ(2) / sqrt(LPs%r2-c)
      T1 = LPs%r2 * s * q
      T2 = (LPs%r2*c - 1.0) * q
    end if

! transform to sphere grid (inverse Lambert)
! [note that there is no need to worry about dividing by zero, since XYZ(3) can not become zero]
    c = T1**2+T2**2
    s = LPs%Pi * c/(24.0*XYZ(3)**2)
    c = LPs%sPi * c / LPs%r24 / XYZ(3)
    q = sqrt( 1.0 - s )
    LamXYZ = (/ T1 * q, T2 * q, sngl(LPs%pref) * XYZ(3) - c /)
  end if
end if

! reverse the coordinates back to the regular order according to the original pyramid number
select case (p)
 case (1,2)
  res = LamXYZ
 case (3,4)
  res = (/ LamXYZ(3), LamXYZ(1), LamXYZ(2) /)
 case (5,6)
  res = (/ LamXYZ(2), LamXYZ(3), LamXYZ(1) /)
end select

end function Lambert3DCubeForwardSingle

!--------------------------------------------------------------------------
!
! FUNCTION: Lambert3DCubeForwardDouble
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief map from 3D cubic grid to 3D ball
!
!> @param xyzin 3D coordinates to be considered (double precision)
!> @param ierr error flag 0 = OK, 1 = outside of unit cube
!
!> @date 7/12/13    MDG 1.0 original
!> @date 7/7/14  MDG 2.0 correction to cube edge length check
!--------------------------------------------------------------------------
recursive function Lambert3DCubeForwardDouble(xyzin,ierr) result(res)

IMPLICIT NONE

real(kind=dbl),INTENT(IN)       :: xyzin(3)
integer(kind=irg),INTENT(INOUT):: ierr
real(kind=dbl)                  :: res(3)

real(kind=dbl)                  :: XYZ(3), sXYZ(3), T1, T2, c, s, q, LamXYZ(3), eps
integer(kind=irg)               :: p

eps = 1.0D-8

ierr = 0
!if (maxval(dabs(xyzin)).gt.LPs%ap/2.D0) then
if (maxval(dabs(xyzin)).gt.(LPs%ap/2.D0+eps)) then
  res = (/ 0.D0, 0.D0, 0.D0 /)
  ierr = 1
  return
end if

! determine which pyramid pair the point lies in and copy coordinates in correct order (see paper)
p = GetPyramidDouble(xyzin)
select case (p)
 case (1,2)
  sXYZ = xyzin
 case (3,4)
  sXYZ = (/ xyzin(2), xyzin(3), xyzin(1) /)
 case (5,6)
  sXYZ = (/ xyzin(3), xyzin(1), xyzin(2) /)
end select

! scale by grid parameter ratio sc
XYZ = LPs%sc * sXYZ

! transform to the sphere grid via the curved square, and intercept the zero point
if (maxval(dabs(XYZ)).eq.0.D0) then
  LamXYZ = (/ 0.D0, 0.D0, 0.D0 /)
else
! intercept all the points along the z-axis
  if (maxval(dabs(XYZ(1:2))).eq.0.D0) then
    LamXYZ = (/ 0.D0, 0.D0, LPs%pref * XYZ(3) /)
  else  ! this is a general grid point
    if (dabs(XYZ(2)).le.dabs(XYZ(1))) then
      q = LPs%pi12 * XYZ(2)/XYZ(1)
      c = dcos(q)
      s = dsin(q)
      q = LPs%prek * XYZ(1) / dsqrt(LPs%r2-c)
      T1 = (LPs%r2*c - 1.D0) * q
      T2 = LPs%r2 * s * q
    else
      q = LPs%pi12 * XYZ(1)/XYZ(2)
      c = dcos(q)
      s = dsin(q)
      q = LPs%prek * XYZ(2) / dsqrt(LPs%r2-c)
      T1 = LPs%r2 * s * q
      T2 = (LPs%r2*c - 1.D0) * q
    end if

! transform to sphere grid (inverse Lambert)
! [note that there is no need to worry about dividing by zero, since XYZ(3) can not become zero]
    c = T1**2+T2**2
    s = LPs%Pi * c/(24.D0*XYZ(3)**2)
    c = LPs%sPi * c / LPs%r24 / XYZ(3)
    q = dsqrt( 1.0 - s )
    LamXYZ = (/ T1 * q, T2 * q, LPs%pref * XYZ(3) - c /)
  end if
end if

! reverse the coordinates back to the regular order according to the original pyramid number
select case (p)
 case (1,2)
  res = LamXYZ
 case (3,4)
  res = (/ LamXYZ(3), LamXYZ(1), LamXYZ(2) /)
 case (5,6)
  res = (/ LamXYZ(2), LamXYZ(3), LamXYZ(1) /)
end select

end function Lambert3DCubeForwardDouble

!--------------------------------------------------------------------------
!
! FUNCTION: Lambert3DCubeInverseSingle
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief map from 3D ball to 3D cubic grid
!
!> @param xyz 3D coordinates to be considered (single precision)
!> @param ierr error flag 0 = OK, 1 =
!
!> @date 7/12/13    MDG 1.0 original
!> @date 8/12/14    MDG 1.1 fixed acos() problem
!--------------------------------------------------------------------------
recursive function Lambert3DCubeInverseSingle(xyz,ierr) result(res)

IMPLICIT NONE

real(kind=sgl),INTENT(IN)       :: xyz(3)
integer(kind=irg),INTENT(INOUT):: ierr
real(kind=sgl)                  :: res(3)

real(kind=sgl)                  :: rs, xyz3(3), xyz2(3), qxy, q2xy, sq2xy, q, ac, T1inv, T2inv, &
                                   xyz1(3), sx, sy, qx2y, sqx2y, tt

integer(kind=irg)               :: p

ierr = 0

rs = sqrt(sum(xyz*xyz))
if (rs.gt.LPs%R1) then
  res = (/ 0.0,0.0,0.0 /)
  ierr = 1
  return
end if

if (maxval(abs(xyz)).eq.0.0) then
  res = (/ 0.0,0.0,0.0 /)
  return
end if

! determine pyramid
p = GetPyramidSingle(xyz)
select case (p)
 case (1,2)
  xyz3 = xyz
 case (3,4)
  xyz3 = (/ xyz(2), xyz(3), xyz(1) /)
 case (5,6)
  xyz3 = (/ xyz(3), xyz(1), xyz(2) /)
end select

! inverse M_3
q = sqrt( 2.0*rs/(rs+abs(xyz3(3))) )
xyz2 = (/ xyz3(1) * q, xyz3(2) * q, (abs(xyz3(3))/xyz3(3)) * rs / sngl(LPs%pref) /)

! inverse M_2
qxy = xyz2(1)*xyz2(1)+xyz2(2)*xyz2(2)
sx = 1.0
if (xyz2(1).ne.0.0)  sx = abs(xyz2(1))/xyz2(1)
sy = 1.0
if (xyz2(2).ne.0.0)  sy = abs(xyz2(2))/xyz2(2)

if (qxy.ne.0.0) then
 if (abs(xyz2(2)).le.abs(xyz2(1))) then
  q2xy = qxy + xyz2(1)*xyz2(1)
  sq2xy = sqrt(q2xy)
  q = (LPs%beta/LPs%r2/LPs%R1) * sqrt(q2xy*qxy/(q2xy-abs(xyz2(1))*sq2xy))
  tt = (xyz2(2)*xyz2(2)+abs(xyz2(1))*sq2xy)/LPs%r2/qxy
  if (tt.gt.1.0) tt = 1.0
  if (tt.lt.-1.0) tt = -1.0
  ac = acos(tt)
  T1inv = q * sx
  T2inv = q * sy * ac/LPs%pi12
 else
  qx2y = qxy + xyz2(2)*xyz2(2)
  sqx2y = sqrt(qx2y)
  q = (LPs%beta/LPs%r2/LPs%R1) * sqrt(qx2y*qxy/(qx2y-abs(xyz2(2))*sqx2y))
  tt = (xyz2(1)*xyz2(1)+abs(xyz2(2))*sqx2y)/LPs%r2/qxy
  if (tt.gt.1.0) tt = 1.0
  if (tt.lt.-1.0) tt = -1.0
  ac = acos(tt)
  T1inv = q * sx * ac/LPs%pi12
  T2inv = q * sy
 end if
else
 T1inv = 0.0
 T2inv = 0.0
end if

xyz1 = (/ T1inv, T2inv, xyz2(3) /)

! inverse M_1
xyz1 = xyz1 / LPs%sc

! reverst the coordinates back to the regular order according to the original pyramid number
select case (p)
 case (1,2)
  res = xyz1
 case (3,4)
  res = (/ xyz1(3), xyz1(1), xyz1(2) /)
 case (5,6)
  res = (/ xyz1(2), xyz1(3), xyz1(1) /)
end select

end function Lambert3DCubeInverseSingle

!--------------------------------------------------------------------------
!
! FUNCTION: Lambert3DCubeInverseDouble
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief map from 3D ball to 3D cubic grid
!
!> @param xyz 3D coordinates to be considered (double precision)
!> @param ierr error flag 0 = OK, 1 =
!
!> @date 7/12/13    MDG 1.0 original
!> @date 8/12/14    MDG 1.1 fixed dacos() problem
!--------------------------------------------------------------------------
recursive function Lambert3DCubeInverseDouble(xyz,ierr) result(res)

IMPLICIT NONE

real(kind=dbl),INTENT(IN)       :: xyz(3)
integer(kind=irg),INTENT(INOUT):: ierr
real(kind=dbl)                  :: res(3)

real(kind=dbl)                  :: rs, xyz3(3), xyz2(3), qxy, q2xy, sq2xy, q, ac, T1inv, T2inv, &
                                   xyz1(3), sx, sy, qx2y, sqx2y, tt
integer(kind=irg)               :: p

ierr = 0

rs = dsqrt(sum(xyz*xyz))
if (rs.gt.LPs%R1) then
  res = (/ 0.D0,0.D0,0.D0 /)
  ierr = 1
  return
end if

if (maxval(dabs(xyz)).eq.0.D0) then
  res = (/ 0.D0,0.D0,0.D0 /)
  return
end if

! determine pyramid
p = GetPyramidDouble(xyz)
select case (p)
 case (1,2)
  xyz3 = xyz
 case (3,4)
  xyz3 = (/ xyz(2), xyz(3), xyz(1) /)
 case (5,6)
  xyz3 = (/ xyz(3), xyz(1), xyz(2) /)
end select

! inverse M_3
q = dsqrt( 2.D0*rs/(rs+dabs(xyz3(3))) )
xyz2 = (/ xyz3(1) * q, xyz3(2) * q, (dabs(xyz3(3))/xyz3(3)) * rs / LPs%pref /)

! inverse M_2
qxy = xyz2(1)*xyz2(1)+xyz2(2)*xyz2(2)
sx = 1.D0
if (xyz2(1).ne.0.D0)  sx = dabs(xyz2(1))/xyz2(1)
sy = 1.D0
if (xyz2(2).ne.0.D0)  sy = dabs(xyz2(2))/xyz2(2)

if (qxy.ne.0.D0) then
 if (dabs(xyz2(2)).le.dabs(xyz2(1))) then
  q2xy = qxy + xyz2(1)*xyz2(1)
  sq2xy = dsqrt(q2xy)
  q = (LPs%beta/LPs%r2/LPs%R1) * dsqrt(q2xy*qxy/(q2xy-dabs(xyz2(1))*sq2xy))
  tt = (xyz2(2)*xyz2(2)+dabs(xyz2(1))*sq2xy)/LPs%r2/qxy
  if (tt.gt.1.D0) tt = 1.D0
  if (tt.lt.-1.D0) tt = -1.D0
  ac = dacos(tt)
  T1inv = q * sx
  T2inv = q * sy * ac/LPs%pi12
 else
  qx2y = qxy + xyz2(2)*xyz2(2)
  sqx2y = dsqrt(qx2y)
  q = (LPs%beta/LPs%r2/LPs%R1) * dsqrt(qx2y*qxy/(qx2y-dabs(xyz2(2))*sqx2y))
  tt = (xyz2(1)*xyz2(1)+dabs(xyz2(2))*sqx2y)/LPs%r2/qxy
  if (tt.gt.1.D0) tt = 1.D0
  if (tt.lt.-1.D0) tt = -1.D0
  ac = dacos(tt)
  T1inv = q * sx * ac/LPs%pi12
  T2inv = q * sy
 end if
else
  T1inv = 0.D0
  T2inv = 0.D0
end if
xyz1 = (/ T1inv, T2inv, xyz2(3) /)

! inverse M_1
xyz1 = xyz1 / LPs%sc

! reverst the coordinates back to the regular order according to the original pyramid number
select case (p)
 case (1,2)
  res = xyz1
 case (3,4)
  res = (/ xyz1(3), xyz1(1), xyz1(2) /)
 case (5,6)
  res = (/ xyz1(2), xyz1(3), xyz1(1) /)
end select

end function Lambert3DCubeInverseDouble

!--------------------------------------------------------------------------
!
! FUNCTION: GetPyramidSingle
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief determine to which pyramid a point in a cubic grid belongs
!
!> @param xyz 3D coordinates to be considered (single precision)
!
!> @date 11/21/12    MDG 1.0 original
!--------------------------------------------------------------------------
recursive function GetPyramidSingle(xyz) result(res)

IMPLICIT NONE

real(kind=sgl),INTENT(IN)       :: xyz(3)
integer(kind=irg)               :: res, p

logical                         :: next

next = .TRUE.
if ((abs(xyz(1)).le.xyz(3)).and.(abs(xyz(2)).le.xyz(3))) then
  p = 1                         ! pyramid 1
  next = .FALSE.
end if
if (next) then
 if ((abs(xyz(1)).le.-xyz(3)).and.(abs(xyz(2)).le.-xyz(3))) then
  p = 2                         ! pyramid 2
  next = .FALSE.
 end if
end if

if (next) then
 if ((abs(xyz(3)).le.xyz(1)).and.(abs(xyz(2)).le.xyz(1))) then
  p = 3                         ! pyramid 3
  next = .FALSE.
 end if
end if
if (next) then
 if ((abs(xyz(3)).le.-xyz(1)).and.(abs(xyz(2)).le.-xyz(1))) then
  p = 4                         ! pyramid 4
  next = .FALSE.
 end if
end if

if (next) then
 if ((abs(xyz(1)).le.xyz(2)).and.(abs(xyz(3)).le.xyz(2))) then
  p = 5                         ! pyramid 5
  next = .FALSE.
 end if
end if
if (next) then
 if ((abs(xyz(1)).le.-xyz(2)).and.(abs(xyz(3)).le.-xyz(2))) then
  p = 6                         ! pyramid 6
  next = .FALSE.
 end if
end if
res = p

end function GetPyramidSingle

!--------------------------------------------------------------------------
!
! FUNCTION: GetPyramidDouble
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief determine to which pyramid a point in a cubic grid belongs
!
!> @param xyz 3D coordinates to be considered (double precision)
!
!> @date 11/21/12    MDG 1.0 original
!--------------------------------------------------------------------------
recursive function GetPyramidDouble(xyz) result(res)

IMPLICIT NONE

real(kind=dbl),INTENT(IN)       :: xyz(3)
integer(kind=irg)               :: res, p

logical                         :: next

next = .TRUE.
if ((dabs(xyz(1)).le.xyz(3)).and.(dabs(xyz(2)).le.xyz(3))) then
  p = 1                         ! pyramid 1
  next = .FALSE.
end if
if (next) then
 if ((dabs(xyz(1)).le.-xyz(3)).and.(dabs(xyz(2)).le.-xyz(3))) then
  p = 2                         ! pyramid 2
  next = .FALSE.
 end if
end if

if (next) then
 if ((dabs(xyz(3)).le.xyz(1)).and.(dabs(xyz(2)).le.xyz(1))) then
  p = 3                         ! pyramid 3
  next = .FALSE.
 end if
end if
if (next) then
 if ((dabs(xyz(3)).le.-xyz(1)).and.(dabs(xyz(2)).le.-xyz(1))) then
  p = 4                         ! pyramid 4
  next = .FALSE.
 end if
end if

if (next) then
 if ((dabs(xyz(1)).le.xyz(2)).and.(dabs(xyz(3)).le.xyz(2))) then
  p = 5                         ! pyramid 5
  next = .FALSE.
 end if
end if
if (next) then
 if ((dabs(xyz(1)).le.-xyz(2)).and.(dabs(xyz(3)).le.-xyz(2))) then
  p = 6                         ! pyramid 6
  next = .FALSE.
 end if
end if

res = p

end function GetPyramidDouble

end module Lambert