!-----------------------------------------------------------------------
! CUDA Fortran GPU Tutorial: Transpose
! written by Viktor K. Decyk, UCLA
      program example2
      use transpose
      use gpuflib2
      implicit none
! nx, ny = size of array
! mx, my = data block size
      integer, parameter :: nx = 512, ny = 512, mx = 16, my = 16
! nblock = block size on GPU
      integer :: nblock = 64
      integer :: j, k, irc
      real :: eps, epsmax
! timing data
      double precision :: dtime
      integer, dimension(4) :: itime
! data for Fortran Host
      real, dimension(nx,ny) :: b2
      real, dimension(ny,nx) :: a2, c2
! data for GPU
      real, device, dimension(:,:), allocatable :: g_a2, g_b2
!
! set up GPU
      irc = 0
      call setgbsize(nblock)
      call init_cu(0,irc)
      if (irc /= 0) then
         write (*,*) 'CUDA initialization error!'
         stop
      endif
!
! allocate 2d data on GPU
      allocate(g_a2(ny,nx),g_b2(nx,ny))
!
! initialize 2d data on Host
      do k = 1, ny
      do j = 1, nx
         b2(j,k) = real(j + nx*(k-1))
      enddo
      enddo
      a2 = 0.0
! copy data to GPU
      g_a2 = a2
      g_b2 = b2
!
! measure overhead time by running empty kernel
      call dtimer(dtime,itime,-1)
      call empty_kernel()
      call dtimer(dtime,itime,1)
      write (*,*) 'Fortran empty kernel time=', real(dtime)
!
! segmented 2d transpose on host with block size mx, my
      call dtimer(dtime,itime,-1)
!     call transpose0(a2,b2)
      call transpose2(a2,b2,mx,my)
      call dtimer(dtime,itime,1)
      write (*,*) 'Fortran 2d transpose time=', real(dtime)
!
! 2d transpose on GPU with block size mx, mx
      call dtimer(dtime,itime,-1)
      call gpu_transpose2(g_a2,g_b2,mx)
      call dtimer(dtime,itime,1)
      write (*,*) 'GPU 2d transpose time=', real(dtime)
!
! copy data from GPU
      c2 = g_a2
!
! Check for correctness: compare a2 and g_a2
      epsmax = 0.0
      do j = 1, nx
         do k = 1, ny
            eps = abs(a2(k,j)-c2(k,j))
            if (eps > epsmax) epsmax = eps
         enddo
      enddo
      write (*,*) '2d transpose maximum difference = ', epsmax
!
!
! deallocate memory on GPU
      deallocate(g_a2,g_b2)
! close down GPU
      call end_cu()
!
      end program


!> Module calculating thermodynamic potentials and differentials
!! for solid H2O.
!!
!! Ref:
!! A New Equation of State for H$_2$O Ice Ih
!! Rainer Feistel and Wolfgang Wagner
!! Journal of Physical and Chemical Reference Data (J. Phys. Chem. Ref. Data)
!! Pages: 1021-1047
!! January 2006
!! Volume(Number): 35(2)
!! doi: http://dx.doi.org/10.1063/1.2183324
!!
module h2o_gibbs
  implicit none
  private
  save
  !
  real, parameter :: Ttriple = 273.160  !< K
  real, parameter :: Ptriple = 611.657  !< Pa
  real, parameter :: P0_H2O = 101325.0 !< Pa
  real, parameter :: T0_H2O = 273.152519  !< K
  real, parameter :: mw_H2O = 18.01528e-3 !< Molar weight in kg/mol
  !
  real, parameter :: g00  = -632020.233449497     !< J/kg
  real, parameter :: g01  = 0.655022213658955     !< J/kg
  real, parameter :: g02  = -1.893699293261e-8    !< J/kg
  real, parameter :: g03  = 3.39746123271053e-15  !< J/kg
  real, parameter :: g04  = -5.56464869058991e-22 !< J/kg
  real, dimension(5) :: g0_vec  = (/ g00, g01, g02, g03, g04 /)
  real :: s0  = -3327.33756492168 !189.13 !< J/kg/K ()

  !> J/kg/K
  complex*16, parameter :: t1  = (3.68017112855051e-02, 5.10878114959572e-02)
  !> J/kg/K
  complex*16, parameter :: t2  = (0.337315741065416, 0.335449415919309)
  !> J/kg/K
  complex*16, dimension(2), parameter :: t_vec  = (/ t1, t2 /)
  !> J/kg/K
  complex*16, parameter :: r1  = (44.7050716285388, 65.6876847463481)
  !> J/kg/K
  complex*16, parameter :: r20 = (-72.597457432922, -78.100842711287)
  !> J/kg/K
  complex*16, parameter :: r21 = (-5.57107698030123e-05, 4.64578634580806e-05)
  !> J/kg/K
  complex*16, parameter :: r22 = (2.34801409215913e-11, -2.85651142904972e-11)
  !> J/kg/K
  complex*16, dimension(3), parameter :: r2_vec  = (/ r20, r21, r22 /)

  !Melting enthalpy at (T0_H2O, P0_H2O)
  !Giauque and Stout 1936: 333.4220 kJ/kg and 333.4920 kJ/kg
  !Osborne 1939: 333.54(20) kJ/kg
  !Haida et al. 1974: 333.41 kJ/kg
  !Model: 333.426516 kJ/kg
  real, parameter :: dSmelt = mw_H2O*333.426516*1.0e3/T0_H2O !< J/mol/K - (Sliq - Ssolid)

  public :: sh2o_gibbs, sh2o_dgdp, sh2o_d2gdp2, sh2o_dgdT
  public :: sh2o_d2gdT2, sh2o_d2gdtdp
  public :: dSmelt, T0_H2O, P0_H2O, sho2_init

  public :: sh2o_specvol, sh2o_heat_capacity
  public :: sh2o_entropy, sh2o_enthalpy, sh2o_energy, sh2o_helmholtz

contains

  !> Set reference potentials (H2O)
  !! Input: Liquid entropy and Gibbs energy evaluated at (T0_H2O,P0_H2O)
  subroutine sho2_init(sl,gl)
    implicit none
    real, intent(in) :: sl !< Entropy - J/mol/K
    real, intent(in) :: gl !< Gibbs energy - J/mol
    ! Locals
    real :: ss_current, gs_current, ss

    ! Set entropy reference
    ss_current = sh2o_entropy(T0_H2O,P0_H2O)
    ss = sl - dSmelt
    s0 = s0 + (ss - ss_current)/mw_H2O

    ! Set Gibbs energy reference
    gs_current = sh2o_gibbs(T0_H2O,P0_H2O)
    g0_vec(1) = g0_vec(1) + (gl-gs_current)/mw_H2O

  end subroutine sho2_init

  !> Gibbs free energy for ice (H2O) (J/mol)
  function sh2o_gibbs(T,P) result(g)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: g !< J/mol
    real :: g0
    complex*16, dimension(2) :: r
    complex*16 :: c
    integer :: k
    real :: tau
    g0 = sh2o_g0(P)
    r(1) = r1
    r(2) = sh2o_r2(P)
    tau = T/Ttriple
    c = 0.0
    do k=1,2
       c = c + r(k)*( &
       (t_vec(k) - tau)*log(t_vec(k) - tau) &
       + (t_vec(k) + tau)*log(t_vec(k) + tau) &
       - 2.0*t_vec(k)*log(t_vec(k)) - tau**2/t_vec(k) &
       )
    enddo
    g = g0 - s0*T + Ttriple*REAL(c)
    g = g * mw_H2O ! per kg -> per mol
  end function sh2o_gibbs

  !> Gibbs free energy for ice (H2O) -
  !! Differential wrpt. temperature at constant pressure (J/mol/K)
  function sh2o_dgdT(T,P) result(dgdT)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: dgdT !< J/mol/K
    complex*16, dimension(2) :: r
    complex*16 :: c
    real :: tau
    integer :: k
    r(1) = r1
    r(2) = sh2o_r2(P)
    tau = T/Ttriple
    c = 0.0
    do k=1,2
       c = c + r(k)*( &
       -log(t_vec(k) - tau) &
       + log(t_vec(k) + tau) &
       - 2.0*tau/t_vec(k) &
       )
    enddo
    dgdT = - s0 + REAL(c)
    dgdT = dgdT * mw_H2O ! per kg -> per mol
  end function sh2o_dgdT

  !> Gibbs free energy for ice (H2O) -
  !! Second differential wrpt. temperature at constant pressure (J/mol/K)
  function sh2o_d2gdT2(T,P) result(d2gdT2)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: d2gdT2 !< J/mol/K2
    complex*16, dimension(2) :: r
    complex*16 :: c
    integer :: k
    real :: tau
    r(1) = r1
    r(2) = sh2o_r2(P)
    tau = T/Ttriple
    c = 0.0
    do k=1,2
       c = c + r(k)*( &
       1.0/(t_vec(k) - tau) &
       + 1.0/(t_vec(k) + tau) &
       - 2.0/t_vec(k) &
       )
    enddo
    d2gdT2 = REAL(c)/Ttriple
    d2gdT2 = d2gdT2 * mw_H2O ! per kg -> per mol
  end function sh2o_d2gdT2

  !> Gibbs free energy for ice (H2O) -
  !! Second differential wrpt. temperature and pressure (m3/mol/K)
  function sh2o_d2gdTdP(T,P) result(d2gdTdP)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: d2gdTdP !< m3/mol/K
    complex*16 :: r2p
    complex*16 :: c
    real :: tau
    r2p = sh2o_r2p(P)
    tau = T/Ttriple
    c = r2p*( &
         -log(t_vec(2) - tau) &
         + log(t_vec(2) + tau) &
         - 2.0*tau/t_vec(2) &
         )
    d2gdTdP = REAL(c)
    d2gdTdP = d2gdTdP * mw_H2O ! per kg -> per mol
  end function sh2o_d2gdTdP

  !> Gibbs free energy for ice (H2O) -
  !! Differential wrpt. pressure at constant temperature (m3/mol)
  function sh2o_dgdP(T,P) result(dgdP)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: dgdP !< m3/mol
    real :: dg0dP
    complex*16 :: r2p
    complex*16 :: c
    real :: tau
    dg0dP = sh2o_dg0dP(P)
    r2p = sh2o_r2p(P)
    tau = T/Ttriple
    c = r2p*( &
         (t_vec(2) - tau)*log(t_vec(2) - tau) &
         + (t_vec(2) + tau)*log(t_vec(2) + tau) &
         - 2.0*t_vec(2)*log(t_vec(2)) - tau**2/t_vec(2) &
         )
    dgdP = dg0dP + Ttriple*REAL(c)
    dgdP = dgdP * mw_H2O ! per kg -> per mol
  end function sh2o_dgdP

  !> Gibbs free energy for ice (H2O) -
  !! Second differential wrpt. pressure at constant temperature (m3/mol/Pa)
  function sh2o_d2gdP2(T,P) result(d2gdP2)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: d2gdP2 !< m3/mol/Pa
    real :: d2g0dP2
    complex*16 :: r2pp
    complex*16 :: c
    real :: tau
    d2g0dP2 = sh2o_d2g0dP2(P)
    r2pp = 2.0*r22/Ptriple**2
    tau = T/Ttriple
    c = r2pp*( &
         (t_vec(2) - tau)*log(t_vec(2) - tau) &
         + (t_vec(2) + tau)*log(t_vec(2) + tau) &
         - 2.0*t_vec(2)*log(t_vec(2)) - tau**2/t_vec(2) &
         )
    d2gdP2 = d2g0dP2 + Ttriple*REAL(c)
    d2gdP2 = d2gdP2 * mw_H2O ! per kg -> per mol
  end function sh2o_d2gdP2

  !> Utility function for Gibbs free energy of ice (H2O) (J/kg)
  function sh2o_g0(P) result(g0)
    implicit none
    real, intent(in) :: P !< Pa
    real :: g0 !< J/kg
    real :: pi, pi0, dpi
    integer :: k
    pi0 = P0_H2O/Ptriple
    pi = P/Ptriple
    dpi = pi - pi0
    g0 = 0.0
    do k=1,5
       g0 = g0 + g0_vec(k)*dpi**(k-1)
    enddo
  end function sh2o_g0

  !> Utility function for Gibbs free energy of ice (H2O) -
  !! Differential wrpt. pressure (m3/kg)
  function sh2o_dg0dP(P) result(dg0dP)
    implicit none
    real, intent(in) :: P !< Pa
    real :: dg0dP !< m3/kg
    real :: pi, pi0, dpi
    integer :: k
    pi0 = P0_H2O/Ptriple
    pi = P/Ptriple
    dpi = pi - pi0
    dg0dP = 0.0
    do k=1,4
       dg0dP = dg0dP + g0_vec(k+1)*k*dpi**(k-1)/Ptriple
    enddo
  end function sh2o_dg0dP

  !> Utility function for Gibbs free energy of ice (H2O) -
  !! Second differential wrpt. pressure (m3/kg/Pa)
  function sh2o_d2g0dP2(P) result(d2g0dP2)
    implicit none
    real, intent(in) :: P !< Pa
    real :: d2g0dP2 !< m3/kg/Pa
    real :: pi, pi0, dpi
    integer :: k
    pi0 = P0_H2O/Ptriple
    pi = P/Ptriple
    dpi = pi - pi0
    d2g0dP2 = 0.0
    do k=2,4
       d2g0dP2 = d2g0dP2 + g0_vec(k+1)*k*(k-1)*dpi**(k-2)/Ptriple**2
    enddo
  end function sh2o_d2g0dP2

  !> Utility function (J/kg/K)
  function sh2o_r2(P) result(r2)
    implicit none
    real, intent(in) :: P !< Pa
    complex*16 :: r2 !< J/kg/K
    real :: pi, pi0, dpi
    integer :: k
    pi0 = P0_H2O/Ptriple
    pi = P/Ptriple
    dpi = pi - pi0
    r2 = cmplx(0.0,00)
    do k=1,3
       r2 = r2 + r2_vec(k)*dpi**(k-1)
    enddo
  end function sh2o_r2

  !> Utility function
  !! Differential wrpt. pressure (m3/kg/K)
  function sh2o_r2p(P) result(r2p)
    implicit none
    real, intent(in) :: P !< Pa
    complex*16 :: r2p !< m3/kg/K
    real :: pi, pi0, dpi
    integer :: k
    pi0 = P0_H2O/Ptriple
    pi = P/Ptriple
    dpi = pi - pi0
    r2p = cmplx(0.0,00)
    do k=1,2
       r2p = r2p + r2_vec(k+1)*k*dpi**(k-1)/Ptriple
    enddo
  end function sh2o_r2p

  !> Specific volume for ice (H2O) (m3/mol)
  function sh2o_specvol(T,P) result(vol)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: vol !< m3/mol
    vol = sh2o_dgdP(T,P)
  end function sh2o_specvol

  !> Ice (H2O) entropy (J/mol/K)
  function sh2o_entropy(T,P) result(s)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: s !< J/mol/K
    s = -sh2o_dgdT(T,P)
  end function sh2o_entropy

  !> Ice (H2O) enthalpy (J/mol)
  function sh2o_enthalpy(T,P) result(h)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: h ! J/mol
    h = sh2o_gibbs(T,P) + T*sh2o_entropy(T,P)
  end function sh2o_enthalpy

  !> Ice (H2O) energy (J/mol)
  function sh2o_energy(T,P) result(e)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: e !< J/mol
    e = sh2o_enthalpy(T,P)-P*sh2o_specvol(T,P)
  end function sh2o_energy

  !> Ice (H2O) helmholtz energy (J/mol)
  function sh2o_helmholtz(T,P) result(a)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: a !< J/mol
    a = sh2o_gibbs(T,P)-P*sh2o_specvol(T,P)
  end function sh2o_helmholtz

  !> Ice (H2O) heat capacity at constant pressure (J/mol/K)
  function sh2o_heat_capacity(T,P) result(cp)
    implicit none
    real, intent(in) :: T !< K
    real, intent(in) :: P !< Pa
    real :: cp ! J/mol/K
    cp = -T*sh2o_d2gdt2(T,P)
    cp = cp * mw_H2O ! per kg -> per mol
  end function sh2o_heat_capacity

  ! function sh2o_sound_velocity(T,P) result(c)
  !   implicit none
  !   real, intent(in) :: T !< K
  !   real, intent(in) :: P !< Pa
  !   real :: c
  !   ! Locals
  !   real :: dsdt, dsdp, dvdp, dtdp
  !   dsdt = -sh2o_d2gdT2(T,P)
  !   dsdp = -sh2o_d2gdTdp(T,P)
  !   dvdp = sh2o_d2gdP2(T,P)
  !   dtdp = sh2o_d2gdTdP(T,P)
  !   c = 0.0 ! MH TODO - implement
  ! end function sh2o_sound_velocity

end module h2o_gibbs


      SUBROUTINE TG01HY( COMPQ, COMPZ, L, N, M, P, N1, LBE, A, LDA,
     $                   E, LDE, B, LDB, C, LDC, Q, LDQ, Z, LDZ, NR,
     $                   NRBLCK, RTAU, TOL, IWORK, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     Given the descriptor system (A-lambda*E,B,C) with the system
C     matrices A, E and B of the form
C
C            ( A1 X1 )        ( E1 Y1 )        ( B1 )
C        A = (       ) ,  E = (       ) ,  B = (    ) ,
C            ( 0  X2 )        ( 0  Y2 )        ( 0  )
C
C     where
C          - B is an L-by-M matrix, with B1 an N1-by-M  submatrix
C          - A is an L-by-N matrix, with A1 an N1-by-N1 submatrix
C          - E is an L-by-N matrix, with E1 an N1-by-N1 submatrix
C              with LBE nonzero sub-diagonals,
C     this routine reduces the pair (A1-lambda*E1,B1) to the form
C
C     Qc'*[ B1 A1-lambda*E1 ]*diag(I,Zc) =
C
C                              ( Bc Ac-lambda*Ec      *         )
C                              (                                ) ,
C                              ( 0     0         Anc-lambda*Enc )
C
C     where:
C     1) the pencil ( Bc Ac-lambda*Ec ) has full row rank NR for
C        all finite lambda and is in a staircase form with
C                           _      _          _        _
C                         ( A1,0   A1,1  ...  A1,k-1   A1,k  )
C                         (        _          _        _     )
C             ( Bc Ac ) = (  0     A2,1  ...  A2,k-1   A2,k  ) ,  (1)
C                         (              ...  _        _     )
C                         (  0       0   ...  Ak,k-1   Ak,k  )
C
C                           _          _        _
C                         ( E1,1  ...  E1,k-1   E1,k  )
C                         (            _        _     )
C               Ec      = (   0   ...  E2,k-1   E2,k  ) ,         (2)
C                         (       ...           _     )
C                         (   0   ...    0      Ek,k  )
C               _
C         where Ai,i-1 is an rtau(i)-by-rtau(i-1) full row rank
C                                       _
C         matrix (with rtau(0) = M) and Ei,i is an rtau(i)-by-rtau(i)
C         upper triangular matrix.
C
C      2) the pencil Anc-lambda*Enc is regular of order N1-NR with Enc
C         upper triangular; this pencil contains the uncontrollable
C         finite eigenvalues of the pencil (A1-lambda*E1).
C
C     The transformations are applied to the whole matrices A, E, B
C     and C. The left and/or right orthogonal transformations Qc and Zc
C     performed to reduce the pencil S(lambda) can be optionally
C     accumulated in the matrices Q and Z, respectively.
C
C     The reduced order descriptor system (Ac-lambda*Ec,Bc,Cc) has no
C     uncontrollable finite eigenvalues and has the same
C     transfer-function matrix as the original system (A-lambda*E,B,C).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     COMPQ   CHARACTER*1
C             = 'N':  do not compute Q;
C             = 'I':  Q is initialized to the unit matrix, and the
C                     orthogonal matrix Q is returned;
C             = 'U':  Q must contain an orthogonal matrix Q1 on entry,
C                     and the product Q1*Q is returned.
C
C     COMPZ   CHARACTER*1
C             = 'N':  do not compute Z;
C             = 'I':  Z is initialized to the unit matrix, and the
C                     orthogonal matrix Z is returned;
C             = 'U':  Z must contain an orthogonal matrix Z1 on entry,
C                     and the product Z1*Z is returned.
C
C     Input/Output Parameters
C
C     L       (input) INTEGER
C             The number of descriptor state equations; also the number
C             of rows of matrices A, E and B.  L >= 0.
C
C     N       (input) INTEGER
C             The dimension of the descriptor state vector; also the
C             number of columns of matrices A, E and C.  N >= 0.
C
C     M       (input) INTEGER
C             The dimension of descriptor system input vector; also the
C             number of columns of matrix B.  M >= 0.
C
C     P       (input) INTEGER
C             The dimension of descriptor system output; also the
C             number of rows of matrix C.  P >= 0.
C
C     N1      (input) INTEGER
C             The order of subsystem (A1-lambda*E1,B1,C1) to be reduced.
C             MIN(L,N) >= N1 >= 0.
C
C     LBE     (input) INTEGER
C             The number of nonzero sub-diagonals of submatrix E1.
C             MAX(0,N1-1) >= LBE >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading L-by-N part of this array must
C             contain the L-by-N state matrix A in the partitioned
C             form
C                      ( A1 X1 )
C                  A = (       ) ,
C                      ( 0  X2 )
C
C             where A1 is N1-by-N1.
C             On exit, the leading L-by-N part of this array contains
C             the transformed state matrix,
C
C                                          ( Ac  *   * )
C                       Qc'*A*diag(Zc,I) = ( 0  Anc  * ) ,
C                                          ( 0   0   * )
C
C             where Ac is NR-by-NR and Anc is (N1-NR)-by-(N1-NR).
C             The matrix ( Bc Ac ) is in the controllability
C             staircase form (1).
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,L).
C
C     E       (input/output) DOUBLE PRECISION array, dimension (LDE,N)
C             On entry, the leading L-by-N part of this array must
C             contain the L-by-N descriptor matrix E in the partitioned
C             form
C                      ( E1 Y1 )
C                  E = (       ) ,
C                      ( 0  Y2 )
C
C             where E1 is N1-by-N1 matrix with LBE nonzero
C             sub-diagonals.
C             On exit, the leading L-by-N part of this array contains
C             the transformed descriptor matrix
C
C                                          ( Ec  *   * )
C                       Qc'*E*diag(Zc,I) = ( 0  Enc  * ) ,
C                                          ( 0   0   * )
C
C             where Ec is NR-by-NR and Enc is (N1-NR)-by-(N1-NR).
C             Both Ec and Enc are upper triangular.
C
C     LDE     INTEGER
C             The leading dimension of array E.  LDE >= MAX(1,L).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M)
C             On entry, the leading L-by-M part of this array must
C             contain the L-by-M input matrix B in the partitioned
C             form
C                      ( B1 )
C                  B = (    ) ,
C                      ( 0  )
C
C             where B1 is N1-by-M.
C             On exit, the leading L-by-M part of this array contains
C             the transformed input matrix
C
C                               ( Bc )
C                       Qc'*B = (    ) ,
C                               ( 0  )
C
C             where Bc is NR-by-M.
C             The matrix ( Bc Ac ) is in the controllability
C             staircase form (1).
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,L).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the state/output matrix C.
C             On exit, the leading P-by-N part of this array contains
C             the transformed matrix C*diag(Zc,I).
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,P).
C
C     Q       (input/output) DOUBLE PRECISION array, dimension (LDQ,L)
C             If COMPQ = 'N': Q is not referenced.
C             If COMPQ = 'I': on entry, Q need not be set;
C                             on exit, the leading L-by-L part of this
C                             array contains the orthogonal matrix Q,
C                             where Q' is the product of transformations
C                             which are applied to A, E, and B on
C                             the left.
C             If COMPQ = 'U': on entry, the leading L-by-L part of this
C                             array must contain an orthogonal matrix
C                             Q1;
C                             on exit, the leading L-by-L part of this
C                             array contains the orthogonal matrix
C                             Q1*Q.
C
C     LDQ     INTEGER
C             The leading dimension of array Q.
C             LDQ >= 1,        if COMPQ = 'N';
C             LDQ >= MAX(1,L), if COMPQ = 'U' or 'I'.
C
C     Z       (input/output) DOUBLE PRECISION array, dimension (LDZ,N)
C             If COMPZ = 'N': Z is not referenced.
C             If COMPZ = 'I': on entry, Z need not be set;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix Z,
C                             which is the product of transformations
C                             applied to A, E, and C on the right.
C             If COMPZ = 'U': on entry, the leading N-by-N part of this
C                             array must contain an orthogonal matrix
C                             Z1;
C                             on exit, the leading N-by-N part of this
C                             array contains the orthogonal matrix
C                             Z1*Z.
C
C     LDZ     INTEGER
C             The leading dimension of array Z.
C             LDZ >= 1,        if COMPZ = 'N';
C             LDZ >= MAX(1,N), if COMPZ = 'U' or 'I'.
C
C     NR      (output) INTEGER
C             The order of the reduced matrices Ac and Ec, and the
C             number of rows of the reduced matrix Bc; also the order of
C             the controllable part of the pair (B, A-lambda*E).
C
C     NRBLCK  (output) INTEGER                      _
C             The number k, of full row rank blocks Ai,i in the
C             staircase form of the pencil (Bc Ac-lambda*Ec) (see (1)
C             and (2)).
C
C     RTAU    (output) INTEGER array, dimension (N1)
C             RTAU(i), for i = 1, ..., NRBLCK, is the row dimension of
C                                     _
C             the full row rank block Ai,i-1 in the staircase form (1).
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used in rank determinations when
C             transforming (A-lambda*E, B). If the user sets TOL > 0,
C             then the given value of TOL is used as a lower bound for
C             reciprocal condition numbers in rank determinations; a
C             (sub)matrix whose estimated condition number is less than
C             1/TOL is considered to be of full rank.  If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance,
C             defined by  TOLDEF = L*N*EPS,  is used instead, where
C             EPS is the machine precision (see LAPACK Library routine
C             DLAMCH).  TOL < 1.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (M)
C
C     DWORK   DOUBLE PRECISION array, dimension LDWORK
C             On exit, if  INFO = 0,  DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.
C             LDWORK >= MAX(1,N,L,2*(M+N1-1))
C             For good performance, LDWORK should be generally larger.
C
C             If LDWORK = -1, then a workspace query is assumed;
C             the routine only calculates the optimal size of the
C             DWORK array, returns this value as the first entry of
C             the DWORK array, and no error message related to LDWORK
C             is issued by XERBLA.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The subroutine is based on the reduction algorithm of [1].
C     If suitable, it uses block algorithms for the reduction of the
C     matrix E and for the corresponding updates of the matrices A, B,
C     and Q. Moreover, for large systems, the row transformations are
C     applied on panels of columns of the matrices A, B, and E.
C
C     REFERENCES
C
C     [1] A. Varga
C         Computation of Irreducible Generalized State-Space
C         Realizations.
C         Kybernetika, vol. 26, pp. 89-106, 1990.
C
C     NUMERICAL ASPECTS
C
C     The algorithm is numerically backward stable and requires
C     0( N*N1**2 )  floating point operations.
C
C     FURTHER COMMENTS
C
C     If INFO > 0 on entry, that value is used as block size for the
C     block algorithms. Otherwise, the block size is chosen internally.
C
C     CONTRIBUTOR
C
C     V. Sima, Research Institute for Informatics, Bucharest, Jan. 2012.
C     Based on SLICOT Library routine TG01HX, by A. Varga.
C
C     REVISIONS
C
C     V. Sima, Feb. 2012, April 2012, June 2012.
C
C     KEYWORDS
C
C     Controllability, minimal realization, orthogonal canonical form,
C     orthogonal transformation.
C
C     ******************************************************************
C
C     .. Parameters ..
      INTEGER            IMAX, IMIN
      PARAMETER          ( IMAX = 1, IMIN = 2 )
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D0, ZERO = 0.0D0 )
C     .. Scalar Arguments ..
      CHARACTER          COMPQ, COMPZ
      INTEGER            INFO, L, LBE, LDA, LDB, LDC, LDE, LDQ, LDWORK,
     $                   LDZ, M, N, N1, NR, NRBLCK, P
      DOUBLE PRECISION   TOL
C     .. Array Arguments ..
      INTEGER            IWORK( * ), RTAU( * )
      DOUBLE PRECISION   A( LDA, * ), B( LDB, * ), C( LDC, * ),
     $                   DWORK( * ), E( LDE, * ), Q( LDQ, * ),
     $                   Z( LDZ, * )
C     .. Local Scalars ..
      LOGICAL            ILQ, ILZ, LQUERY, ONECOL, OPB, UB, UPDB, WITHC
      INTEGER            I, IB, IC, ICOL, ICOMPQ, ICOMPZ, IR, IROT,
     $                   IROW, ISMAX, ISMIN, J, JB, K, KB, LB, MAXWRK,
     $                   MINWRK, MN, NB, NF, NI, NR1, NX, RANK, SR,
     $                   TAUIM1
      DOUBLE PRECISION   C1, C2, CO, RCOND, S1, S2, SI, SMAX, SMAXPR,
     $                   SMIN, SMINPR, SVLMAX, SVMA, SVMR, T, TOLZ, TT
C     .. External Functions ..
      LOGICAL            LSAME
      INTEGER            IDAMAX, ILAENV
      DOUBLE PRECISION   DLAMCH, DLANGE, DLAPY2, DNRM2
      EXTERNAL           DLAMCH, DLANGE, DLAPY2, DNRM2, IDAMAX, ILAENV,
     $                   LSAME
C     .. External Subroutines ..
      EXTERNAL           DGEQRF, DLACPY, DLAIC1, DLARF, DLARFG, DLARTG,
     $                   DLASET, DORGQR, DORMQR, DROT, DSWAP, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC          ABS, DBLE, INT, MAX, MIN, MOD, SQRT
C
C     .. Executable Statements ..
C
C     Decode COMPQ.
C
      IF( LSAME( COMPQ, 'N' ) ) THEN
         ILQ = .FALSE.
         ICOMPQ = 1
      ELSE IF( LSAME( COMPQ, 'U' ) ) THEN
         ILQ = .TRUE.
         ICOMPQ = 2
      ELSE IF( LSAME( COMPQ, 'I' ) ) THEN
         ILQ = .TRUE.
         ICOMPQ = 3
      ELSE
         ICOMPQ = 0
      END IF
C
C     Decode COMPZ.
C
      IF( LSAME( COMPZ, 'N' ) ) THEN
         ILZ = .FALSE.
         ICOMPZ = 1
      ELSE IF( LSAME( COMPZ, 'U' ) ) THEN
         ILZ = .TRUE.
         ICOMPZ = 2
      ELSE IF( LSAME( COMPZ, 'I' ) ) THEN
         ILZ = .TRUE.
         ICOMPZ = 3
      ELSE
         ICOMPZ = 0
      END IF
C
C     Test the input scalar parameters.
C
      NB   = INFO
      INFO = 0
      IF( ICOMPQ.LE.0 ) THEN
         INFO = -1
      ELSE IF( ICOMPZ.LE.0 ) THEN
         INFO = -2
      ELSE IF( L.LT.0 ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( M.LT.0 ) THEN
         INFO = -5
      ELSE IF( P.LT.0 ) THEN
         INFO = -6
      ELSE IF( N1.LT.0 .OR. N1.GT.MIN( L, N ) ) THEN
         INFO = -7
      ELSE IF( LBE.LT.0 .OR. LBE.GT.MAX( 0, N1-1 ) ) THEN
         INFO = -8
      ELSE IF( LDA.LT.MAX( 1, L ) ) THEN
         INFO = -10
      ELSE IF( LDE.LT.MAX( 1, L ) ) THEN
         INFO = -12
      ELSE IF( LDB.LT.MAX( 1, L ) ) THEN
         INFO = -14
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -16
      ELSE IF( ( ILQ .AND. LDQ.LT.L ) .OR. LDQ.LT.1 ) THEN
         INFO = -18
      ELSE IF( ( ILZ .AND. LDZ.LT.N ) .OR. LDZ.LT.1 ) THEN
         INFO = -20
      ELSE IF( TOL.GE.ONE ) THEN
         INFO = -24
      ELSE
         MINWRK = MAX( 1, N, L, 2*( M + N1 - 1 ) )
         MAXWRK = MINWRK
         LQUERY = LDWORK.EQ.-1
         IF( LQUERY .AND. LBE.GT.0 ) THEN
C
C           Compute optimal workspace.
C
            CALL DGEQRF( N1, N1, E, LDE, DWORK, DWORK, -1, INFO )
            MAXWRK = MAX( MAXWRK, INT( DWORK(1) ) )
            CALL DORMQR( 'Left', 'Transpose', N1, N, N1, E, LDE, DWORK,
     $                   A, LDA, DWORK, -1, INFO )
            MAXWRK = MAX( MAXWRK, INT( DWORK(1) ) )
            CALL DORMQR( 'Left', 'Transpose', N1, M, N1, E, LDE, DWORK,
     $                   B, LDB, DWORK, -1, INFO )
            MAXWRK = MAX( MAXWRK, INT( DWORK(1) ) )
            IF( ILQ ) THEN
               CALL DORGQR( L, N1, N1, Q, LDQ, DWORK, DWORK, -1, INFO )
               MAXWRK = MAX( MAXWRK, INT( DWORK(1) ) )
            END IF
         ELSE IF( LQUERY ) THEN
            MAXWRK = MINWRK
         ELSE IF( LDWORK.LT.MINWRK ) THEN
            INFO = -27
         END IF
      END IF
C
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'TG01HY', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = MAXWRK
         RETURN
      END IF
C
C     Initialize Q and Z if necessary.
C
      IF( ICOMPQ.EQ.3 )
     $   CALL DLASET( 'Full', L, L, ZERO, ONE, Q, LDQ )
      IF( ICOMPZ.EQ.3 )
     $   CALL DLASET( 'Full', N, N, ZERO, ONE, Z, LDZ )
C
C     Initialize output variables.
C
      NR = 0
      NRBLCK = 0
C
C     Quick return if possible.
C
      IF( M.EQ.0 .OR. N1.EQ.0 ) THEN
         DWORK(1) = ONE
         RETURN
      END IF
C
      TOLZ   = DLAMCH( 'Precision' )
      WITHC  = P.GT.0
      SVLMAX = DLANGE( 'F', L, M, B, LDB, DWORK )
      RCOND  = TOL
      IF ( RCOND.LE.ZERO ) THEN
C
C        Use the default tolerance in controllability determination.
C
         RCOND = DBLE( L*N )*TOLZ
      END IF
      TOLZ = SQRT( TOLZ )
C
      IF ( SVLMAX.LT.RCOND )
     $   SVLMAX = ONE
      SVMR = SVLMAX*RCOND
      SVMA = MAX( ONE, DLANGE( 'F', L, N, A, LDA, DWORK ) )*RCOND
      IF( SVMA.GT.SVMR*TOLZ )
     $    SVMA = DLAPY2( SVMR, SVMA )
C
C     Reduce E to upper triangular form if necessary.
C     Check if block algorithms should be used.
C
      IF( NB.LE.0 ) THEN
         CALL DGEQRF( N1, N1, E, LDE, DWORK, DWORK, -1, INFO )
         NB = INT( DWORK(1)/N1 )
      END IF
      NX = ILAENV( 3, 'DGEQRF', ' ', N1, N1, -1, -1 )
C
      IF( LBE.GT.0 ) THEN
         NI = NB
         IF( LDWORK.LT.N1*NB )
     $      NB = INT( LDWORK/N1 )
C
         IF( LBE.LT.NX/2 .OR. NB.LT.NX .OR. N1.LT.NX ) THEN
            DO 10 I = 1, N1-1
C
C              Generate elementary reflector H(i) to annihilate
C              E(i+1:i+lbe,i).
C
               K = MIN( LBE, N1-I ) + 1
               CALL DLARFG( K, E(I,I), E(I+1,I), 1, TT )
               T = E(I,I)
               E(I,I) = ONE
C
C              Apply H(i) to E(i:n1,i+1:n) from the left.
C
               CALL DLARF( 'Left', K, N-I, E(I,I), 1, TT,
     $                     E(I,I+1), LDE, DWORK )
C
C              Apply H(i) to A(i:n1,1:n) from the left.
C
               CALL DLARF( 'Left', K, N, E(I,I), 1, TT,
     $                     A(I,1), LDA, DWORK )
C
C              Apply H(i) to B(i:n1,1:m) from the left.
C
               CALL DLARF( 'Left', K, M, E(I,I), 1, TT,
     $                     B(I,1), LDB, DWORK )
               IF( ILQ ) THEN
C
C                 Apply H(i) to Q(1:l,i:n1) from the right.
C
                  CALL DLARF( 'Right', L, K, E(I,I), 1, TT,
     $                        Q(1,I), LDQ, DWORK )
               END IF
               E(I,I) = T
   10       CONTINUE
            MAXWRK = MINWRK
         ELSE
            CALL DGEQRF( N1, N1, E, LDE, DWORK, DWORK(N1+1), LDWORK-N1,
     $                   INFO )
            MAXWRK = MAX( MINWRK, INT( DWORK(N1+1) )+N1 )
            CALL DORMQR( 'Left', 'Transpose', N1, N-N1, N1, E, LDE,
     $                   DWORK, E(1,N1+1), LDE, DWORK(N1+1), LDWORK-N1,
     $                   INFO )
            CALL DORMQR( 'Left', 'Transpose', N1, N, N1, E, LDE, DWORK,
     $                   A, LDA, DWORK(N1+1), LDWORK-N1, INFO )
            MAXWRK = MAX( MAXWRK, INT( DWORK(N1+1) )+N1 )
            CALL DORMQR( 'Left', 'Transpose', N1, M, N1, E, LDE, DWORK,
     $                   B, LDB, DWORK(N1+1), LDWORK-N1, INFO )
            MAXWRK = MAX( MAXWRK, INT( DWORK(N1+1) )+N1 )
            IF( ILQ ) THEN
               CALL DLACPY( 'Lower', N1, N1, E, LDE, Q, LDQ )
               CALL DORGQR( L, N1, N1, Q, LDQ, DWORK, DWORK(N1+1),
     $                      LDWORK-N1, INFO )
               MAXWRK = MAX( MAXWRK, INT( DWORK(N1+1) )+N1 )
            END IF
         END IF
         IF( N1.GT.1 )
     $      CALL DLASET( 'Lower', N1-1, N1-1, ZERO, ZERO, E(2,1), LDE )
         NB = NI
      END IF
C
      ISMIN  = 1
      ISMAX  = ISMIN + M
      IROT   = 2*( M + N1 ) - 3
      TAUIM1 = M
      IC = -M
      NF = N1
      UB = NB.LE.2
C
   20 CONTINUE
      NRBLCK = NRBLCK + 1
      RANK   = 0
      IF( NF.GT.0 ) THEN
C
C        IROW will point to the current pivot line in B,
C        ICOL+1 will point to the first active columns of A.
C
         ICOL = IC + TAUIM1
         NI   = N  - ICOL
         IROW = NR
         NR1  = NR + 1
         IF( NR.GT.0 ) THEN
            CALL DLACPY( 'Full', NF, TAUIM1, A(NR1,IC+1), LDA,
     $                   B(NR1,1), LDB )
            SVMR = SVMA
         END IF
         ONECOL = TAUIM1.EQ.1
C
C        Perform QR-decomposition with column pivoting on the current B
C        while keeping E upper triangular.
C        The current B is at first iteration B and for subsequent
C        iterations the NF-by-TAUIM1 matrix delimited by rows
C        NR + 1 to N1 and columns IC + 1 to IC + TAUIM1 of A.
C        The rank of current B is computed in RANK.
C
         IF( ONECOL ) THEN
            MN = 1
         ELSE
            MN = MIN( NF, TAUIM1 )
C
C           Compute column norms.
C
            DO 30 J = 1, TAUIM1
               DWORK(J) = DNRM2( NF, B(NR1,J), 1 )
               DWORK(M+J) = DWORK(J)
               IWORK(J) = J
   30       CONTINUE
         END IF
C
   40    CONTINUE
         IF( RANK.LT.MN ) THEN
            J    = RANK + 1
            IROW = IROW + 1
            UPDB = TAUIM1-J.GT.0
C
C           Pivot if necessary.
C
            IF( UPDB ) THEN
               K = ( J - 1 ) + IDAMAX( TAUIM1-J+1, DWORK(J), 1 )
               IF( K.NE.J ) THEN
                  CALL DSWAP( NF, B(NR1,J), 1, B(NR1,K), 1 )
                  I = IWORK(K)
                  IWORK(K) = IWORK(J)
                  IWORK(J) = I
                  DWORK(K) = DWORK(J)
                  DWORK(M+K) = DWORK(M+J)
               END IF
            END IF
C
C           Zero elements below the current diagonal element of B.
C           Apply transformations on block rows and columns of A, B,
C           and E.
C
            IR = IROT
            IB = N1 - 1
            K  = 0
C
            DO 50 I = IB, IROW, -1
C
C              Rotate rows I and I+1 to zero B(I+1,J).
C
               K = K + 1
               T = B(I,J)
               CALL DLARTG( T, B(I+1,J), CO, SI, B(I,J) )
               B(I+1,J) = ZERO
               DWORK(IR)   = CO
               DWORK(IR+1) = SI
               IR = IR - 2
               IF( UB ) THEN
                  CALL DROT( N-I+1, E(I,I), LDE, E(I+1,I), LDE, CO, SI )
               ELSE
                  CALL DROT( MIN(N-I,K)+1, E(I,I), LDE, E(I+1,I), LDE,
     $                       CO, SI )
               END IF
               IF( K.EQ.NB )
     $             K = 0
               IF( UPDB ) THEN
                  OPB = TAUIM1-J.LT.NX .OR. UB .OR.
     $                ( TAUIM1-J )*LDB.LE.NB*N1
                  IF( OPB )
     $               CALL DROT( TAUIM1-J, B(I,J+1), LDB, B(I+1,J+1),
     $                          LDB, CO, SI )
               ELSE
                  OPB = .FALSE.
               END IF
               IF( NI.LT.NX .OR. UB )
     $            CALL DROT( NI, A(I,ICOL+1), LDA, A(I+1,ICOL+1), LDA,
     $                       CO, SI )
               IF( ILQ )
     $            CALL DROT( L, Q(1,I), 1, Q(1,I+1), 1, CO, SI )
C
C              Rotate columns I, I+1 to zero E(I+1,I).
C
               T = E(I+1,I+1)
               CALL DLARTG( T, E(I+1,I), CO, SI, E(I+1,I+1) )
               E(I+1,I) = ZERO
               CALL DROT(  I, E(1,I+1), 1, E(1,I), 1, CO, SI )
               CALL DROT( N1, A(1,I+1), 1, A(1,I), 1, CO, SI )
               IF( ILZ )
     $            CALL DROT( N, Z(1,I+1), 1, Z(1,I), 1, CO, SI )
               IF( WITHC )
     $            CALL DROT( P, C(1,I+1), 1, C(1,I), 1, CO, SI )
   50       CONTINUE
C
            IF( .NOT.UB ) THEN
               IF( NI.GE.NX ) THEN
C
C                 Apply all row rotations to column panels of A.
C
                  DO 70 KB = N-NB+1, ICOL+1, -NB
                     IR = IROT
                     DO 60 I = IB, IROW, -1
                        CALL DROT( NB, A(I,KB), LDA, A(I+1,KB), LDA,
     $                             DWORK(IR), DWORK(IR+1) )
                        IR = IR - 2
   60                CONTINUE
   70             CONTINUE
C
                  LB = MOD( NI, NB )
C
                  IF( LB.GT.0 ) THEN
                     IR = IROT
                     DO 80 I = IB, IROW, -1
                        CALL DROT( LB, A(I,ICOL+1), LDA, A(I+1,ICOL+1),
     $                             LDA, DWORK(IR), DWORK(IR+1) )
                        IR = IR - 2
   80                CONTINUE
                  END IF
               END IF
C
               IF( .NOT.OPB ) THEN
C
C                 Apply all row rotations to column panels of B.
C
                  DO 100 KB = TAUIM1-NB+1, J+1, -NB
                     IR = IROT
                     DO 90 I = IB, IROW, -1
                        CALL DROT( NB, B(I,KB), LDB, B(I+1,KB), LDB,
     $                             DWORK(IR), DWORK(IR+1) )
                        IR = IR - 2
   90                CONTINUE
  100             CONTINUE
C
                  LB = MOD( TAUIM1-J, NB )
C
                  IF( LB.GT.0 ) THEN
                     IR = IROT
                     DO 110 I = IB, IROW, -1
                        CALL DROT( LB, B(I,J+1), LDB, B(I+1,J+1), LDB,
     $                             DWORK(IR), DWORK(IR+1) )
                        IR = IR - 2
  110                CONTINUE
                  END IF
               END IF
C
C              Apply all row rotations to column panels of E.
C              Start with the rightmost, possibly thinner, panel.
C
               IF( N.GT.N1 ) THEN
                  LB = MOD( N-N1, NB )
                  JB = N - LB + 1
                  K  = ( N - LB - N1 )/NB
C
                  IF( LB.GT.0 ) THEN
                     IR = IROT
                     DO 120 I = IB, IROW, -1
                        CALL DROT( LB, E(I,JB), LDE, E(I+1,JB), LDE,
     $                             DWORK(IR), DWORK(IR+1) )
                        IR = IR - 2
  120                CONTINUE
                  END IF
C
                  DO 140 KB = JB-NB, N-LB-K*NB, -NB
                     IR = IROT
                     DO 130 I = IB, IROW, -1
                        CALL DROT( NB, E(I,KB), LDE, E(I+1,KB), LDE,
     $                             DWORK(IR), DWORK(IR+1) )
                        IR = IR - 2
  130                CONTINUE
  140             CONTINUE
C
               END IF
C
               SR = IROT
               IB = IB - NB
               LB = MOD( N1 - IROW + 1, NB )
               IF( LB.EQ.0 ) THEN
                   LB = NB
               ELSE IF( LB.EQ.1 ) THEN
                   LB = 2
               END IF
C
               DO 170 KB = N1-NB+1, IROW+LB, -NB
                  SR = SR - 2*NB
                  IR = SR
                  DO 160 I = IB, IROW, -1
                     CALL DROT( NB, E(I,KB), LDE, E(I+1,KB), LDE,
     $                          DWORK(IR), DWORK(IR+1) )
                     IR = IR - 2
  160             CONTINUE
                  IB = IB - NB
  170          CONTINUE
            END IF
C
            IF( RANK.EQ.0 ) THEN
C
C              Initialize; exit if matrix is zero (RANK = 0).
C              Short pass if the current B has one column.
C
               SMAX = ABS( B(NR1,1) )
               IF ( SMAX.LE.SVMR ) THEN
                  GO TO 200
               ELSE IF ( ONECOL ) THEN
                  RANK = RANK + 1
                  GO TO 200
               END IF
               SMIN   = SMAX
               SMAXPR = SMAX
               SMINPR = SMIN
               C1 = ONE
               C2 = ONE
            ELSE
C
C              One step of incremental condition estimation.
C
               CALL DLAIC1( IMIN, RANK, DWORK(ISMIN), SMIN,
     $                      B(NR1,J), B(IROW,J), SMINPR, S1, C1 )
               CALL DLAIC1( IMAX, RANK, DWORK(ISMAX), SMAX,
     $                      B(NR1,J), B(IROW,J), SMAXPR, S2, C2 )
            END IF
C
C           Check the rank; finish the loop if rank loss occurs.
C
            IF( SVMR.LE.SMAXPR ) THEN
               IF( SMAXPR*RCOND.LE.SMINPR ) THEN
C
C                 Finish the loop if last row.
C
                  IF( IROW.EQ.N1 ) THEN
                     RANK = RANK + 1
                     GO TO 200
                  END IF
C
C                 Update partial column norms.
C
                  DO 180 I = J + 1, TAUIM1
                     IF( DWORK(I).NE.ZERO ) THEN
                        T  = ABS( B(IROW,I) )/DWORK(I)
                        T  = MAX( ( ONE + T )*( ONE - T ), ZERO )
                        TT = T*( DWORK(I)/DWORK(M+I) )**2
                        IF( TT.GT.TOLZ ) THEN
                           DWORK(I) = DWORK(I)*SQRT( T )
                        ELSE
                           DWORK(I) = DNRM2( NF-J, B(IROW+1,I), 1 )
                           DWORK(M+I) = DWORK(I)
                        END IF
                     END IF
  180             CONTINUE
C
                  DO 190 I = 1, RANK
                     DWORK(ISMIN+I-1) = S1*DWORK(ISMIN+I-1)
                     DWORK(ISMAX+I-1) = S2*DWORK(ISMAX+I-1)
  190             CONTINUE
C
                  DWORK(ISMIN+RANK) = C1
                  DWORK(ISMAX+RANK) = C2
                  SMIN = SMINPR
                  SMAX = SMAXPR
                  RANK = RANK + 1
                  GO TO 40
               END IF
            END IF
         END IF
      END IF
C
  200 CONTINUE
C
      IF( RANK.GT.0 ) THEN
         RTAU(NRBLCK) = RANK
C
C        Back permute interchanged columns.
C
         IF( .NOT.ONECOL ) THEN
            DO 220 J = 1, TAUIM1
               IF( IWORK(J).GT.0 ) THEN
                  K = IWORK(J)
                  IWORK(J) = -K
  210             CONTINUE
                  IF( K.NE.J ) THEN
                     CALL DSWAP( RANK, B(NR1,J), 1, B(NR1,K), 1 )
                     IWORK(K) = -IWORK(K)
                     K = -IWORK(K)
                     GO TO 210
                  END IF
               END IF
  220       CONTINUE
         END IF
C
         IF( NR.GT.0 )
     $      CALL DLACPY( 'Full', NF, TAUIM1, B(NR1,1), LDB,
     $                   A(NR1,IC+1), LDA )
         NR = NR + RANK
         NF = NF - RANK
         IC = ICOL
         TAUIM1 = RANK
         GO TO 20
      ELSE
         NRBLCK = NRBLCK - 1
      END IF
C
      IF( NRBLCK.GT.0 )
     $   RANK = RTAU(1)
      IF( RANK.LT.N1 )
     $   CALL DLASET( 'Full', N1-RANK, M, ZERO, ZERO, B(RANK+1,1), LDB )
C
      DWORK(1) = MAXWRK
      RETURN
C *** Last line of TG01HY ***
      END


c
c
c     ###################################################
c     ##  COPYRIGHT (C)  2003  by  Jay William Ponder  ##
c     ##              All Rights Reserved              ##
c     ###################################################
c
c     ##################################################################
c     ##                                                              ##
c     ##  subroutine cspline  --  periodic interpolating cube spline  ##
c     ##                                                              ##
c     ##################################################################
c
c
c     "cspline" computes the coefficients for a periodic interpolating
c     cubic spline
c
c     literature reference:
c
c     G. Engeln-Mullges and F. Uhlig, Numerical Algorithms with Fortran,
c     Springer Verlag, 1996, Section 10.1.2  [see routine "isplpe"]
c
c
      subroutine cspline (n,xn,fn,b,c,d,h,du,dm,rc,rs)
      implicit none
      include 'iounit.i'
      integer i,n,iflag
      real*8 eps,average
      real*8 temp1,temp2
      real*8 xn(0:*)
      real*8 fn(0:*)
      real*8 b(0:*)
      real*8 c(0:*)
      real*8 d(0:*)
      real*8 h(0:*)
      real*8 du(0:*)
      real*8 dm(0:*)
      real*8 rc(0:*)
      real*8 rs(0:*)
c
c
c     check the periodicity of fn, and for subsequent call
c
      eps = 0.000001d0
      if (abs(fn(n)-fn(0)) .gt. eps) then
         write (iout,10)  fn(0),fn(n)
   10    format (' CSPLINE  --  Warning, Non-Periodic Input',
     &             ' Values',2f12.5)
      end if
      average = 0.5d0 * (fn(0) + fn(n))
      fn(0) = average
      fn(n) = average
c
c     get auxiliary variables and matrix elements on first call
c
      do i = 0, n-1
         h(i) = xn(i+1) - xn(i)
      end do
      h(n) = h(0)
      do i = 1, n-1
         du(i) = h(i)
      end do
      du(n) = h(0)
      do i = 1, n
         dm(i) = 2.0d0 * (h(i-1)+h(i))
      end do
c
c     compute the right hand side
c
      temp1 = (fn(1)-fn(0)) / h(0)
      do i = 1, n-1, 1
         temp2 = (fn(i+1)-fn(i)) / h(i)
         rs(i)  = 3.0d0 * (temp2-temp1)
         temp1 = temp2
      end do
      rs(n) = 3.0d0 * ((fn(1)-fn(0))/h(0)-temp1)
c
c     solve the linear system with factorization
c
      call cytsy (n,dm,du,rc,rs,c,iflag)
      if (iflag .ne. 1)  return
c
c     compute remaining spline coefficients
c
      c(0) = c(n)
      do i = 0, n-1
         b(i) = (fn(i+1)-fn(i))/h(i) - h(i)/3.0d0*(c(i+1)+2.0d0*c(i))
         d(i) = (c(i+1)-c(i)) / (3.0d0*h(i))
      end do
      b(n) = (fn(1)-fn(n))/h(n) - h(n)/3.0d0*(c(1)+2.0d0*c(n))
      return
      end
c
c
c     #############################################################
c     ##                                                         ##
c     ##  subroutine cytsy  --  solve cyclic tridiagonal system  ##
c     ##                                                         ##
c     #############################################################
c
c
c     "cytsy" solves a system of linear equations for a cyclically
c     tridiagonal, symmetric, positive definite matrix
c
c     literature reference:
c
c     G. Engeln-Mullges and F. Uhlig, Numerical Algorithms with Fortran,
c     Springer Verlag, 1996, Section 4.11.2
c
c
      subroutine cytsy (n,dm,du,cr,rs,x,iflag)
      implicit none
      integer n,iflag
      real*8 dm(0:*)
      real*8 du(0:*)
      real*8 cr(0:*)
      real*8 rs(0:*)
      real*8 x(0:*)
c
c
c     factorization of the input matrix
c
      iflag = -2
      if (n .lt. 3)  return
      call cytsyp (n,dm,du,cr,iflag)
c
c     update and back substitute as necessary
c
      if (iflag .eq. 1)  call cytsys (n,dm,du,cr,rs,x)
      return
      end
c
c
c     #################################################################
c     ##                                                             ##
c     ##  subroutine cytsyp  --  tridiagonal Cholesky factorization  ##
c     ##                                                             ##
c     #################################################################
c
c
c     "cytsyp" finds the Cholesky factors of a cyclically tridiagonal
c     symmetric, positive definite matrix given by two vectors
c
c     literature reference:
c
c     G. Engeln-Mullges and F. Uhlig, Numerical Algorithms with Fortran,
c     Springer Verlag, 1996, Section 4.11.2
c
c
      subroutine cytsyp (n,dm,du,cr,iflag)
      implicit none
      integer i,n,iflag
      real*8 eps,row,d
      real*8 temp1,temp2
      real*8 dm(0:*)
      real*8 du(0:*)
      real*8 cr(0:*)
c
c
c     set error bound and test for condition n greater than 2
c
      eps = 0.00000001d0
      iflag = -2
      if (n .lt. 3)  return
c
c     checking to see if matrix is positive definite
c
      row = abs(dm(1)) + abs(du(1)) + abs(du(n))
      if (row .eq. 0.0d0) then
         iflag = 0
         return
      end if
      d = 1.0d0 / row
      if (dm(1) .lt. 0.0d0) then
         iflag = -1
         return
      else if (abs(dm(1))*d .le. eps) then
         iflag = 0
         return
      end if
c
c     factoring a while checking for a positive definite and strong
c     nonsingular matrix a
c
      temp1 = du(1)
      du(1) = du(1) / dm(1)
      cr(1) = du(n) / dm(1)
      do i = 2, n-1
         row = abs(dm(i)) + abs(du(i)) + abs(temp1)
         if (row .eq. 0.0d0) then
            iflag = 0
            return
         end if
         d = 1.0d0 / row
         dm(i) = dm(i) - temp1*du(i-1)
         if (dm(i) .lt. 0.0d0) then
            iflag = -1
            return
         else if (abs(dm(i))*d .le. eps) then
            iflag = 0
            return
         end if
         if (i .lt. (n-1)) then
            cr(i) = -temp1 * cr(i-1) / dm(i)
            temp1 = du(i)
            du(i) = du(i) / dm(i)
         else
            temp2 = du(i)
            du(i) = (du(i) - temp1*cr(i-1)) / dm(i)
         end if
      end do
      row = abs(du(n)) + abs(dm(n)) + abs(temp2)
      if (row .eq. 0.0d0) then
         iflag = 0
         return
      end if
      d = 1.0d0 / row
      dm(n) = dm(n) - dm(n-1)*du(n-1)*du(n-1)
      temp1 = 0.0d0
      do i = 1, n-2
         temp1 = temp1 + dm(i)*cr(i)*cr(i)
      end do
      dm(n) = dm(n) - temp1
      if (dm(n) .lt. 0) then
         iflag = -1
         return
      else if (abs(dm(n))*d .le. eps) then
         iflag = 0
         return
      end if
      iflag = 1
      return
      end
c
c
c     ################################################################
c     ##                                                            ##
c     ##  subroutine cytsys  --  tridiagonal solution from factors  ##
c     ##                                                            ##
c     ################################################################
c
c
c     "cytsys" solves a cyclically tridiagonal linear system
c     given the Cholesky factors
c
c     literature reference:
c
c     G. Engeln-Mullges and F. Uhlig, Numerical Algorithms with Fortran,
c     Springer Verlag, 1996, Section 4.11.2
c
c
      subroutine cytsys (n,dm,du,cr,rs,x)
      implicit none
      integer i,n
      real*8 sum,temp
      real*8 dm(0:*)
      real*8 du(0:*)
      real*8 cr(0:*)
      real*8 rs(0:*)
      real*8 x(0:*)
c
c
c     updating phase
c
      temp = rs(1)
      rs(1) = temp / dm(1)
      sum = cr(1) * temp
      do i = 2, n-1
         temp = rs(i) - du(i-1)*temp
         rs(i) = temp / dm(i)
         if (i .ne. (n-1))  sum = sum + cr(i)*temp
      end do
      temp = rs(n) - du(n-1)*temp
      temp = temp - sum
      rs(n) = temp / dm(n)
c
c     back substitution phase
c
      x(n) = rs(n)
      x(n-1) = rs(n-1) - du(n-1)*x(n)
      do i = n-2, 1, -1
         x(i) = rs(i) - du(i)*x(i+1) - cr(i)*x(n)
      end do
      return
      end


Program oscilador
  implicit none
  double precision, parameter::ti=0.0 , tf=10.0
  integer, parameter::N=1000
  double precision, parameter::x0=6.6d0, v0=0.0
  doubleprecision::t, x, y, f, g, h
  doubleprecision::k1, k2, k3, k4, l1, l2, l3, l4
  integer :: i
  open(01, file="Datos.dat", status="unknown")
  h=(tf-ti)/N
  x=x0
  y=v0
  do i=0,1000
     t=ti+i*h
     k1=h*f(t, x, y)
     l1=h*g(t, x, y)
     k2=h*f(t+h*0.5d0, x+k1*0.5d0, y+L1*0.5d0)
     l2=h*g(t+h*0.5d0, x+k1*0.5d0, y+L1*0.5d0)
     k3=h*f(t+h*0.5d0, x+k2*0.5d0, y+L2*0.5d0 )
     l3=h*g(t+h*0.5d0, x+k2*0.5d0, y+L2*0.5d0 )
     k4=h*f(t+h , x+k3, y+L3 )
     l4=h*g(t+h , x+k3, y+L3 )
     x=x+(k1+k2+k2+k3+k3+k4)/6.0d0;
     y=y+(L1+L2+L2+L3+L3+L4)/6.0d0;
     write(01, *) t, x, y
  end do
end Program oscilador

function g(t, x, y)
  double precision::g, t, x, y, a, c
  !g=-x + t*x*y*0.0
  a=-9.932e-6
  c=1.575e-6
  g=a*sin(x)+c*sin(2*x)
end function g

function f(t, x, y)
  double precision::f, t, x, y
  f=0.0
end function f


c     Calculate LU decomposition of a matrix
c     Author: Pearu Peterson, March 2002
c
c     prefixes: d,z,s,c   (double,complex double,float,complex float)
c     suffixes: _c,_r     (column major order,row major order)

      subroutine dlu_c(p,l,u,a,m,n,k,piv,info,permute_l,m1)
      integer m,n,piv(k),i,j,k,permute_l,m1,info
      double precision l(m,k),u(k,n),a(m,n)
      double precision p(m1,m1)

cf2py intent(in,copy) :: a
cf2py intent(out) :: info
cf2py integer intent(hide,cache),depend(k),dimension(k) :: piv
cf2py integer intent(hide),depend(a) :: m = shape(a,0)
cf2py integer intent(hide),depend(a) :: n = shape(a,1)
cf2py integer intent(hide),depend(m,n) :: k = (m<n?m:n)
cf2py intent(out) :: p,l,u
cf2py integer optional,intent(in):: permute_l = 0
cf2py integer intent(hide),depend(permute_l,m) :: m1 = (permute_l?1:m)
cf2py depend(m1) :: p

cf2py callprotoargument double*,double*,double*,double*,int*,int*,int*,int*,int*,int*,int*

      external dgetrf,dlaswp
      call dgetrf(m,n,a,m,piv,info)
      if (info.lt.0) then
         return
      endif
      do 20 j=1,k
         l(j,j) = 1d0
         do 10, i=j+1,m
            l(i,j) = a(i,j)
 10      continue
 20   continue
      do 40 j=1,k
         do 30, i=1,j
            u(i,j) = a(i,j)
 30      continue
 40   continue
      do 60 j=k+1,n
         do 50, i=1,k
            u(i,j) = a(i,j)
 50      continue
 60   continue
      if (permute_l.ne.0) then
         call dlaswp(k,l,m,1,k,piv,-1)
      else
         do 65 i=1,m
            p(i,i)=1d0
 65       continue
         call dlaswp(m,p,m,1,k,piv,-1)
      endif
      end

      subroutine zlu_c(p,l,u,a,m,n,k,piv,info,permute_l,m1)
      integer m,n,piv(k),i,j,k,permute_l,m1,info
      complex*16 l(m,k),u(k,n),a(m,n)
      double precision p(m1,m1)

cf2py intent(in,copy) :: a
cf2py intent(out) :: info
cf2py integer intent(hide,cache),depend(k),dimension(k) :: piv
cf2py integer intent(hide),depend(a) :: m = shape(a,0)
cf2py integer intent(hide),depend(a) :: n = shape(a,1)
cf2py integer intent(hide),depend(m,n) :: k = (m<n?m:n)
cf2py intent(out) :: p,l,u
cf2py integer optional,intent(in):: permute_l = 0
cf2py integer intent(hide),depend(permute_l,m) :: m1 = (permute_l?1:m)
cf2py depend(m1) :: p

cf2py callprotoargument double*,complex_double*,complex_double*,complex_double*,int*,int*,int*,int*,int*,int*,int*

      external zgetrf,zlaswp,dlaswp
      call zgetrf(m,n,a,m,piv,info)
      if (info.lt.0) then
         return
      endif
      do 20 j=1,k
         l(j,j) = 1d0
         do 10, i=j+1,m
            l(i,j) = a(i,j)
 10      continue
 20   continue
      do 40 j=1,k
         do 30, i=1,j
            u(i,j) = a(i,j)
 30      continue
 40   continue
      do 60 j=k+1,n
         do 50, i=1,k
            u(i,j) = a(i,j)
 50      continue
 60   continue
      if (permute_l.ne.0) then
         call zlaswp(k,l,m,1,k,piv,-1)
      else
         do 65 i=1,m
            p(i,i)=1d0
 65       continue
         call dlaswp(m,p,m,1,k,piv,-1)
      endif
      end

      subroutine slu_c(p,l,u,a,m,n,k,piv,info,permute_l,m1)
      integer m,n,piv(k),i,j,k,permute_l,m1,info
      real l(m,k),u(k,n),a(m,n)
      real p(m1,m1)

cf2py intent(in,copy) :: a
cf2py intent(out) :: info
cf2py integer intent(hide,cache),depend(k),dimension(k) :: piv
cf2py integer intent(hide),depend(a) :: m = shape(a,0)
cf2py integer intent(hide),depend(a) :: n = shape(a,1)
cf2py integer intent(hide),depend(m,n) :: k = (m<n?m:n)
cf2py intent(out) :: p,l,u
cf2py integer optional,intent(in):: permute_l = 0
cf2py integer intent(hide),depend(permute_l,m) :: m1 = (permute_l?1:m)
cf2py depend(m1) :: p

cf2py callprotoargument float*,float*,float*,float*,int*,int*,int*,int*,int*,int*,int*

      external sgetrf,slaswp
      call sgetrf(m,n,a,m,piv,info)
      if (info.lt.0) then
         return
      endif
      do 20 j=1,k
         l(j,j) = 1d0
         do 10, i=j+1,m
            l(i,j) = a(i,j)
 10      continue
 20   continue
      do 40 j=1,k
         do 30, i=1,j
            u(i,j) = a(i,j)
 30      continue
 40   continue
      do 60 j=k+1,n
         do 50, i=1,k
            u(i,j) = a(i,j)
 50      continue
 60   continue
      if (permute_l.ne.0) then
         call slaswp(k,l,m,1,k,piv,-1)
      else
         do 65 i=1,m
            p(i,i)=1e0
 65       continue
         call slaswp(m,p,m,1,k,piv,-1)
      endif
      end

      subroutine clu_c(p,l,u,a,m,n,k,piv,info,permute_l,m1)
      integer m,n,piv(k),i,j,k,permute_l,m1,info
      complex l(m,k),u(k,n),a(m,n)
      real p(m1,m1)

cf2py intent(in,copy) :: a
cf2py intent(out) :: info
cf2py integer intent(hide,cache),depend(k),dimension(k) :: piv
cf2py integer intent(hide),depend(a) :: m = shape(a,0)
cf2py integer intent(hide),depend(a) :: n = shape(a,1)
cf2py integer intent(hide),depend(m,n) :: k = (m<n?m:n)
cf2py intent(out) :: p,l,u
cf2py integer optional,intent(in):: permute_l = 0
cf2py integer intent(hide),depend(permute_l,m) :: m1 = (permute_l?1:m)
cf2py depend(m1) :: p

cf2py callprotoargument float*,complex_float*,complex_float*,complex_float*,int*,int*,int*,int*,int*,int*,int*

      external cgetrf,claswp,slaswp
      call cgetrf(m,n,a,m,piv,info)
      if (info.lt.0) then
         return
      endif
      do 20 j=1,k
         l(j,j) = 1d0
         do 10, i=j+1,m
            l(i,j) = a(i,j)
 10      continue
 20   continue
      do 40 j=1,k
         do 30, i=1,j
            u(i,j) = a(i,j)
 30      continue
 40   continue
      do 60 j=k+1,n
         do 50, i=1,k
            u(i,j) = a(i,j)
 50      continue
 60   continue
      if (permute_l.ne.0) then
         call claswp(k,l,m,1,k,piv,-1)
      else
         do 65 i=1,m
            p(i,i)=1e0
 65       continue
         call slaswp(m,p,m,1,k,piv,-1)
      endif
      end


program main
    use FEMDomainClass
    implicit none

    type(FEMDomain_) :: domain
    type(Random_) :: random
    real(real64),allocatable :: StressTensor(:,:,:),symtensor(:,:),tensor(:,:)
    integer(int32) :: i

    call domain%create(meshtype="rectangular2D",x_num=10, y_num=10)

    ! create StressTensorField
    StressTensor = random%cube(domain%nn(),2,2 )
    do i=1,domain%nn()
        symtensor = random%matrix(2,2)
        symtensor = symtensor + transpose(symtensor)
        StressTensor(i,1:2,1:2) = symtensor(1:2,1:2)
    enddo

    !call eigen_2d(symtensor,tensor)
    !call print(tensor)
    !return

    call domain%addlayer(name="CauchyStress",tensor=StressTensor)
    call domain%showLayer()
    call domain%msh(name="mesh")
    print *, domain%PhysicalField(1)%name
    call domain%msh(name="eigens",fieldname="CauchyStress")

end program main


*> \brief \b DSTECH
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       SUBROUTINE DSTECH( N, A, B, EIG, TOL, WORK, INFO )
*
*       .. Scalar Arguments ..
*       INTEGER            INFO, N
*       DOUBLE PRECISION   TOL
*       ..
*       .. Array Arguments ..
*       DOUBLE PRECISION   A( * ), B( * ), EIG( * ), WORK( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*>    Let T be the tridiagonal matrix with diagonal entries A(1) ,...,
*>    A(N) and offdiagonal entries B(1) ,..., B(N-1)).  DSTECH checks to
*>    see if EIG(1) ,..., EIG(N) are indeed accurate eigenvalues of T.
*>    It does this by expanding each EIG(I) into an interval
*>    [SVD(I) - EPS, SVD(I) + EPS], merging overlapping intervals if
*>    any, and using Sturm sequences to count and verify whether each
*>    resulting interval has the correct number of eigenvalues (using
*>    DSTECT).  Here EPS = TOL*MAZHEPS*MAXEIG, where MACHEPS is the
*>    machine precision and MAXEIG is the absolute value of the largest
*>    eigenvalue. If each interval contains the correct number of
*>    eigenvalues, INFO = 0 is returned, otherwise INFO is the index of
*>    the first eigenvalue in the first bad interval.
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The dimension of the tridiagonal matrix T.
*> \endverbatim
*>
*> \param[in] A
*> \verbatim
*>          A is DOUBLE PRECISION array, dimension (N)
*>          The diagonal entries of the tridiagonal matrix T.
*> \endverbatim
*>
*> \param[in] B
*> \verbatim
*>          B is DOUBLE PRECISION array, dimension (N-1)
*>          The offdiagonal entries of the tridiagonal matrix T.
*> \endverbatim
*>
*> \param[in] EIG
*> \verbatim
*>          EIG is DOUBLE PRECISION array, dimension (N)
*>          The purported eigenvalues to be checked.
*> \endverbatim
*>
*> \param[in] TOL
*> \verbatim
*>          TOL is DOUBLE PRECISION
*>          Error tolerance for checking, a multiple of the
*>          machine precision.
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is DOUBLE PRECISION array, dimension (N)
*> \endverbatim
*>
*> \param[out] INFO
*> \verbatim
*>          INFO is INTEGER
*>          0  if the eigenvalues are all correct (to within
*>             1 +- TOL*MAZHEPS*MAXEIG)
*>          >0 if the interval containing the INFO-th eigenvalue
*>             contains the incorrect number of eigenvalues.
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date December 2016
*
*> \ingroup double_eig
*
*  =====================================================================
      SUBROUTINE DSTECH( N, A, B, EIG, TOL, WORK, INFO )
*
*  -- LAPACK test routine (version 3.7.0) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     December 2016
*
*     .. Scalar Arguments ..
      INTEGER            INFO, N
      DOUBLE PRECISION   TOL
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( * ), B( * ), EIG( * ), WORK( * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ZERO
      PARAMETER          ( ZERO = 0.0D0 )
*     ..
*     .. Local Scalars ..
      INTEGER            BPNT, COUNT, I, ISUB, J, NUML, NUMU, TPNT
      DOUBLE PRECISION   EMIN, EPS, LOWER, MX, TUPPR, UNFLEP, UPPER
*     ..
*     .. External Functions ..
      DOUBLE PRECISION   DLAMCH
      EXTERNAL           DLAMCH
*     ..
*     .. External Subroutines ..
      EXTERNAL           DSTECT
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, MAX
*     ..
*     .. Executable Statements ..
*
*     Check input parameters
*
      INFO = 0
      IF( N.EQ.0 )
     $   RETURN
      IF( N.LT.0 ) THEN
         INFO = -1
         RETURN
      END IF
      IF( TOL.LT.ZERO ) THEN
         INFO = -5
         RETURN
      END IF
*
*     Get machine constants
*
      EPS = DLAMCH( 'Epsilon' )*DLAMCH( 'Base' )
      UNFLEP = DLAMCH( 'Safe minimum' ) / EPS
      EPS = TOL*EPS
*
*     Compute maximum absolute eigenvalue, error tolerance
*
      MX = ABS( EIG( 1 ) )
      DO 10 I = 2, N
         MX = MAX( MX, ABS( EIG( I ) ) )
   10 CONTINUE
      EPS = MAX( EPS*MX, UNFLEP )
*
*     Sort eigenvalues from EIG into WORK
*
      DO 20 I = 1, N
         WORK( I ) = EIG( I )
   20 CONTINUE
      DO 40 I = 1, N - 1
         ISUB = 1
         EMIN = WORK( 1 )
         DO 30 J = 2, N + 1 - I
            IF( WORK( J ).LT.EMIN ) THEN
               ISUB = J
               EMIN = WORK( J )
            END IF
   30    CONTINUE
         IF( ISUB.NE.N+1-I ) THEN
            WORK( ISUB ) = WORK( N+1-I )
            WORK( N+1-I ) = EMIN
         END IF
   40 CONTINUE
*
*     TPNT points to singular value at right endpoint of interval
*     BPNT points to singular value at left  endpoint of interval
*
      TPNT = 1
      BPNT = 1
*
*     Begin loop over all intervals
*
   50 CONTINUE
      UPPER = WORK( TPNT ) + EPS
      LOWER = WORK( BPNT ) - EPS
*
*     Begin loop merging overlapping intervals
*
   60 CONTINUE
      IF( BPNT.EQ.N )
     $   GO TO 70
      TUPPR = WORK( BPNT+1 ) + EPS
      IF( TUPPR.LT.LOWER )
     $   GO TO 70
*
*     Merge
*
      BPNT = BPNT + 1
      LOWER = WORK( BPNT ) - EPS
      GO TO 60
   70 CONTINUE
*
*     Count singular values in interval [ LOWER, UPPER ]
*
      CALL DSTECT( N, A, B, LOWER, NUML )
      CALL DSTECT( N, A, B, UPPER, NUMU )
      COUNT = NUMU - NUML
      IF( COUNT.NE.BPNT-TPNT+1 ) THEN
*
*        Wrong number of singular values in interval
*
         INFO = TPNT
         GO TO 80
      END IF
      TPNT = BPNT + 1
      BPNT = TPNT
      IF( TPNT.LE.N )
     $   GO TO 50
   80 CONTINUE
      RETURN
*
*     End of DSTECH
*
      END


FUNCTION CSNDSET(SND,SDEN,IOPT)
  !
  ! CHANGE RECORD
  ! **  CALCULATES HINDERED SETTLING CORRECTION FOR CLASS NS NONCOHESIVE
  ! **  SEDIMENT

  IMPLICIT NONE

  INTEGER :: IOPT
  REAL :: CSNDSET,SND,SDEN,ROPT

  ROPT=FLOAT(IOPT)
  CSNDSET=(1.-SDEN*SND)**ROPT

END FUNCTION


program csfcal
       use prep, only : i16b, rk, Ldes, Lzdes, Sdes_t2, econfigs, nconfig, nshell, neact, &
            !parameters and variables
           & read_input, checkSmin, checkSmax, checkILmax,checkLmin, Lmax, Lmin, &
               & Smin_t2, Smax_t2, ARRAY_SHORT_LENGTH
            !subroutines and functions
       use detgen, only : assign_shell
       use projection, only : initlists, Proj_L, Proj_S, getallsigns, collect_csf, normalizetable, initlists_fromlists
       use checkcsfs, only: sortBasisCoefTable, Lsq_multiple, Ssq_multiple
       use gramschmidt, only : orth

       implicit none
       call main()

       contains

       subroutine main
           integer :: totdets, minLp, maxLp, innum, num1, num, i, nbasis, ncsf, iconf,&
               & iend, istart, min2Sp, max2Sp
           integer(i16b), allocatable :: detlist(:, :), inbasis(:, :), &
               &basislist1(:, :),basislist(:, :), allbasis(:, :), allbasis1(:, :)
           real(rk), allocatable :: incoefs(:), coeflist1(:), coeflist(:), coeftable(:, :), Q(:, :), R(:, :)
           integer, allocatable :: ineposes(:, :, :), eposlist1(:, :, :), eposlist(:, :, :)
           logical, allocatable :: iniszeros(:), iszerolist1(:), iszerolist(:)
           character(10) :: line

           call read_input()
           call checkSmin()
           call checkSmax()
           call checkILmax()
           call checkLmin()
          !call checkLmin() !Lmin not that simple
           totdets = 0
           nbasis = 0
           ncsf = 0
           iend = 6
           do iconf = 4, 4
               istart = iend + 1
               iend = istart + nshell(iconf) - 1
               write(*, *) '%%%%%%%%%%%%%%%%%%%%%%  Assign_shell for Config', iconf, '  %%%%%%%%%%%%%%%%%%%'

               !write(*,'(''paused, type [enter] to continue'')')
               !read(*, *) line

               totdets = 0
               call assign_shell(0_i16b, 0_i16b, 0, 0, 0, 0,&
                  & Lzdes, Sdes_t2, econfigs(istart:iend, 1:3), 0, totdets, detlist)
               write(*, *) totdets
               do i = 1, totdets
                   write(*, '(2B16)') detlist(i, 1:2)
               enddo
               minLp = Lmin(econfigs(istart:iend, 1:3))
               maxLp = Lmax(econfigs(istart:iend, 1:3))
               min2Sp = Smin_t2(neact)
               max2Sp = Smax_t2(econfigs(istart:iend, 1:3))
               ncsf = 0 !Each configs has a csftable,n
                        !so set size to 0 before filling for current config
               do i = 1, totdets
                   !PAUSE 2
                    !write(*,'(''paused, type [enter] to continue'')')
                    !      read (*,*) line

                   write(*, *) 'xxxxxxxxxxxxxxxxxxxxx  Projection of det #', i,'  xxxxxxxxxxxxxxxxxxxx'
                   call initlists(detlist(i, 1:2), inbasis, incoefs, ineposes, iniszeros, innum)
                   !inbasis... always initialized to arrays with only one det
                   call Proj_L(minLp, maxLp, Ldes, &
                   & inbasis, incoefs, ineposes, iniszeros, innum, &
                   & basislist1, coeflist1, eposlist1, iszerolist1, num1)

                   call Proj_S(min2Sp, max2Sp, Sdes_t2, &
                       &basislist1, coeflist1, eposlist1, iszerolist1, num1, &
                       &basislist, coeflist, eposlist, iszerolist, num)

                   !PAUSE 4
                   ! write(*,'(''paused, type [enter] to continue'')')
                   !       read (*,*) line

                   call getallsigns(basislist, coeflist, eposlist, iszerolist, num)
                   call collect_csf(basislist, coeflist, iszerolist, num, &
                     & allbasis, coeftable, nbasis, ncsf)
               enddo !for each det

               call sortBasisCoefTable(allbasis, coeftable, nbasis, ncsf)

               call normalizetable(coeftable, nbasis, ncsf)

               write(*, *) nbasis, ncsf
               do i = 1, nbasis
                 write(*, *) coeftable(i, 1:ncsf)
               enddo
               if(nbasis.ne.ncsf) then
                   write(*, *) 'coeftable not square'
                   exit
               endif

               !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
               !All correct up to this point
               !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

              ! if(.not.allocated(Q)) then
              !     allocate(Q(ARRAY_SHORT_LENGTH, ARRAY_SHORT_LENGTH))
              ! endif
              ! if(.not.allocated(R)) then
              !     allocate(R(ARRAY_SHORT_LENGTH, ARRAY_SHORT_LENGTH))
              ! endif

              ! call orth(coeftable, Q, R, 8, ncsf)
              ! do i = 1, nbasis
              !    write(*, *) coeftable(i, 1:ncsf)
              !  enddo

!TODO Need to change a method for orth

!!!!test_3
!test_manual_table
!Test the rest using results from Mathematica
               ! coeftable(1, 2) = coeftable(2, 2)
               ! coeftable(2, 2) = coeftable(1, 1)

!!!!test_4
!orig_table
                do i = 1, nbasis
                   write(*, *) coeftable(i, 1:ncsf)
                 enddo

               call initlists_fromlists(allbasis, coeftable(1:nbasis, 1), nbasis,  incoefs, ineposes, iniszeros)

               write(*, *) '################# Apply Lsq on csf1 ########################'

               num1 = 0
               innum = nbasis

               call Lsq_multiple(allbasis,  incoefs, ineposes, iniszeros, innum, &
                   &  basislist1, coeflist1, eposlist1, iszerolist1, num1)
               call getallsigns(basislist1, coeflist1, eposlist1, iszerolist1, num1)
               ncsf = 0
               nbasis = 0
               call collect_csf(basislist1, coeflist1, iszerolist1, num1, &
                   & allbasis1, coeftable, nbasis, ncsf)
               call sortBasisCoefTable(allbasis1, coeftable, nbasis, ncsf)
                num1 = 0
                innum = nbasis

                write(*, *) '################# Apply Ssq on csf1 ########################'
                num1 = 0
                innum = nbasis

                call Ssq_multiple(allbasis,  incoefs, ineposes, iniszeros, innum, &
                    &  basislist1, coeflist1, eposlist1, iszerolist1, num1)
                call getallsigns(basislist1, coeflist1, eposlist1, iszerolist1, num1)
                ncsf = 0
                nbasis = 0
                call collect_csf(basislist1, coeflist1, iszerolist1, num1, &
                    & allbasis1, coeftable, nbasis, ncsf)
                call sortBasisCoefTable(allbasis1, coeftable, nbasis, ncsf)

                call initlists_fromlists(allbasis, coeftable(1:nbasis, 2), nbasis,  incoefs,       ineposes, iniszeros)

                write(*, *) '################# Apply Lsq on csf2 ########################'

                num1 = 0
                innum = nbasis

                call Lsq_multiple(allbasis, incoefs, ineposes, iniszeros, innum, &
                    &  basislist1, coeflist1, eposlist1, iszerolist1, num1)
                call getallsigns(basislist1, coeflist1, eposlist1, iszerolist1, num1)
                ncsf = 0
                nbasis = 0
                call collect_csf(basislist1, coeflist1, iszerolist1, num1, &
                    & allbasis1, coeftable, nbasis, ncsf)
                call sortBasisCoefTable(allbasis1, coeftable, nbasis, ncsf)

                 write(*, *) '################# Apply Ssq on csf2 ########################'
                 num1 = 0
                 innum = nbasis

                 call Ssq_multiple(allbasis,  incoefs, ineposes, iniszeros, innum, &
                     &  basislist1, coeflist1, eposlist1, iszerolist1, num1)
                 call getallsigns(basislist1, coeflist1, eposlist1, iszerolist1, num1)
                 ncsf = 0
                 nbasis = 0
                 call collect_csf(basislist1, coeflist1, iszerolist1, num1, &
                     & allbasis1, coeftable, nbasis, ncsf)
                 call sortBasisCoefTable(allbasis1, coeftable, nbasis, ncsf)

           enddo !for each config

           deallocate(econfigs)
           deallocate(detlist)
           deallocate(inbasis)
           deallocate(basislist)
           deallocate(basislist1)
           deallocate(allbasis)
           deallocate(allbasis1)
           deallocate(incoefs)
           deallocate(coeflist1)
           deallocate(coeftable)
           deallocate(ineposes)
           deallocate(iniszeros)
           deallocate(iszerolist1)
           deallocate(iszerolist)

       end subroutine main
 end program csfcal


      subroutine dbesyg (x1, alpha, n, y, nz,w,ierr)
c     Author Serge Steer, Copyright INRIA, 2005
c     extends dbesy for the case where alpha is negative
c     x is assumed to be >0 (if negative bessely(alpha,x) is complex)
      double precision x1,alpha,y(n),w(n)
      integer n,nz,ierr
c
      double precision a,b,pi,inf,x,a1,eps
      integer ier1,ier2
      double precision dlamch
      data pi /3.14159265358979324D0/

      inf=dlamch('o')*2.0d0
      eps=dlamch('p')
      x=x1
      ier2=0
      if (x.ne.x.or.alpha.ne.alpha) then
c     .  NaN case
         call dset(n,inf-inf,y,1)
         ierr=4
      else if (x .eq. 0.0d0) then
         ierr=2
         call dset(n,-inf,y,1)
      elseif (alpha .ge. 0.0d0) then
         call dbesy(x,alpha,n,y,ierr)
         if (ierr.eq.2) call dset(n,inf,y,1)
      else if (alpha .eq. dint(alpha)) then
c     .  alpha <0 and integer,
c     .  transform to positive value of alpha
         if(alpha-1+n.ge.0) then
c     .     0 is between alpha and alpha+n
            a1=0.0d0
            nn=min(n,int(-alpha))
         else
            a1=-(alpha-1+n)
            nn=n
         endif
         call dbesy(x,a1,n,w,ierr)
         if (ierr.eq.2) then
            call dset(n,inf,y,1)
         else
           if(n.gt.nn) then
c     .        0 is between alpha and alpha+n
               call dcopy(n-nn,w,1,y(nn+1),1)
               call dcopy(nn,w(2),-1,y,1)
            else
c     .        alpha and alpha+n are negative
               call dcopy(nn,w,-1,y,1)
            endif
         endif
         i0=mod(int(abs(alpha))+1,2)
         call dscal((nn-i0+1)/2,-1.0d0,y(1+i0),2)
      else
c     .  first alpha is negative non integer, x should be positive (with
C     .  x negative the result is complex. CHECKED
c     .  transform to positive value of alpha
         if(alpha-1.0d0+n.ge.0.0d0) then
c     .     0 is between alpha and alpha+n
            nn=int(-alpha)+1
         else
            nn=n
         endif
c     .  compute for negative value of alpha+k, transform problem for
c     .  a1:a1+(nn-1) with a1 positive  a1+k =abs(alpha+nn-k)
         a1=-(alpha-1.0d0+nn)

         call dbesj(x,a1,nn,y,nz1,ierr)
         call dbesy(x,a1,nn,w,ier)
         ierr=max(ierr,ier)
         if (ierr.eq.0) then
            a=sin(a1*pi)
            b=cos(a1*pi)
c     .     to avoid numerical errors if a is near 0 (or b near 0)
c     .     and y is big (or w is big)
            if(abs(abs(a)-1.0d0).lt.eps)  b=0.0d0
            if(abs(abs(b)-1.0d0).lt.eps)  a=0.0d0
            call dscal(nn,b,w,1)
            call daxpy(nn,a,y,1,w,1)
         elseif (ierr.eq.2) then
            call dset(nn,inf,w,1)
         elseif (ierr.eq.4) then
            call dset(nn,inf-inf,w,1)
         endif

c     .  change sign to take into account that cos((a1+k)*pi) and
C     .  sin((a1+k)*pi) changes sign with k
         if (nn.ge.2) call dscal(nn/2,-1.0d0,w(2),2)
c     .  store the result in the correct order
         call dcopy(nn,w,-1,y,1)
c     .  compute for positive value of alpha+k is any
         if (n.gt.nn) then
            call dbesy(x,1.0d0-a1,n-nn,y(nn+1),ier)
            if (ierr.eq.2) call dset(n-nn,inf,y(nn+1),1)
            ierr=max(ierr,ier)
         endif
      endif

      end

      subroutine dbesyv (x,nx,alpha,na,kode,y,w,ierr)
c     Author Serge Steer, Copyright INRIA, 2005
c     compute bessely function for x and alpha given by vectors
c     w : working array of size 2*na (used only if nz>0 and alpha contains negative
C         values
      double precision x(nx),alpha(na),y(*),w(*)
      integer kode, nx,na,ier
      double precision e,dlamch,w1,eps
      eps=dlamch('p')
      ierr=0
      if (na.lt.0) then
c     .  element wise case x and alpha are supposed to have the same size
         do i=1,nx
            call  dbesyg (abs(x(i)), alpha(i),1,y(i), nz, w1,ier)
            ierr=max(ierr,ier)
         enddo
      elseif (na.eq.1) then
c     .  element wise case x and alpha are supposed to have the same size
         do i=1,nx
            call  dbesyg (abs(x(i)), alpha(1),1,y(i), nz, w1,ier)
            ierr=max(ierr,ier)
         enddo
      else
c     .  compute bessely(x(i),y(j)), i=1,nx,j=1,na
         j0=1
 05      n=0
 10      n=n+1
         j=j0+n
         if (j.le.na.and.abs((1+alpha(j-1))-alpha(j)).le.eps) then
            goto 10
         endif
         do i=1,nx
            call dbesyg(abs(x(i)),alpha(j0),n, w, nz, w(na+1),ier)
            ierr=max(ierr,ier)
            call dcopy(n,w,1,y(i+(j0-1)*nx),nx)
         enddo
         j0=j
         if (j0.le.na) goto 05
      endif
      end


! ::: -----------------------------------------------------------
! ::: This routine will tag high error cells based on overdensity
! :::
! ::: INPUTS/OUTPUTS:
! :::
! ::: tag      <=  integer tag array
! ::: lo,hi     => index extent of tag array
! ::: set       => integer value to tag cell for refinement
! ::: clear     => integer value to untag cell
! ::: den       => density array
! ::: nc        => number of components in density array
! ::: domlo,hi  => index extent of problem domain
! ::: delta     => cell spacing
! ::: xlo       => physical location of lower left hand
! :::              corner of tag array
! ::: problo    => phys loc of lower left corner of prob domain
! ::: time      => problem evolution time
! ::: level     => refinement level of this array
! ::: -----------------------------------------------------------
      subroutine tag_overdensity(tag,tagl1,tagl2,tagl3,tagh1,tagh2,tagh3, &
                                 set,clear, &
                                 den,denl1,denl2,denl3,denh1,denh2,denh3, &
                                 lo,hi,nc,domlo,domhi,delta,level,avg_den)

      use probdata_module
      use comoving_module, only : comoving_h, comoving_OmAx
      implicit none

      integer set, clear, nc, level
      integer tagl1,tagl2,tagl3,tagh1,tagh2,tagh3
      integer denl1,denl2,denl3,denh1,denh2,denh3
      integer lo(3), hi(3), domlo(3), domhi(3)
      integer tag(tagl1:tagh1,tagl2:tagh2,tagl3:tagh3)
      double precision den(denl1:denh1,denl2:denh2,denl3:denh3,nc)
      double precision delta(3), avg_den, critdens

      double precision :: over_den

      integer i, j, k

      over_den = 2.0d0**(3*(level+1)) * avg_den
      ! critdens = 2.775d11 * 0.7d0**2 * comoving_OmAx !3 * (100)**2 *0.7d0**2 *  comoving_OmAx / ( 8 * PI * Gconst)
      ! over_den = 2.0d0**(3*(level+1))*critdens
!     Tag on regions of overdensity
      do k = lo(3), hi(3)
         do j = lo(2), hi(2)
            do i = lo(1), hi(1)
               if ( den(i,j,k,1) .gt. over_den ) then
                  tag(i,j,k) = set
               endif
            enddo
         enddo
      enddo

      end subroutine tag_overdensity

      subroutine tag_axvel(tag,tagl1,tagl2,tagl3,tagh1,tagh2,tagh3, &
                              set,clear, &
                              vel,vell1,vell2,vell3,velh1,velh2,velh3, &
                              lo,hi,nc,domlo,domhi,delta,xlo,problo,time,level)

      use probdata_module
      use fdm_params_module, only : critvalue

      implicit none

      integer set, clear, nc, level
      integer tagl1,tagl2,tagl3,tagh1,tagh2,tagh3
      integer vell1,vell2,vell3,velh1,velh2,velh3
      integer lo(3), hi(3), domlo(3), domhi(3)
      integer tag(tagl1:tagh1,tagl2:tagh2,tagl3:tagh3)
      double precision vel(vell1:velh1,vell2:velh2,vell3:velh3,nc)
      double precision delta(3), avg_den

      double precision xlo(3), problo(3),time
      integer i, j, k

      do k = lo(3), hi(3)
         do j = lo(2), hi(2)
            do i = lo(1), hi(1)
               if ( vel(i,j,k,1) .gt. critvalue ) then
                  tag(i,j,k) = set
               endif
            enddo
         enddo
      enddo

      end subroutine tag_axvel

      subroutine tag_center(tag,tagl1,tagl2,tagl3,tagh1,tagh2,tagh3, &
                            set,clear, &
                            den,denl1,denl2,denl3,denh1,denh2,denh3, &
                            lo,hi,nc,domlo,domhi,delta,level,avg_den)

      use probdata_module
      use comoving_module, only : comoving_h, comoving_OmAx
      implicit none

      integer set, clear, nc, level
      integer tagl1,tagl2,tagl3,tagh1,tagh2,tagh3
      integer denl1,denl2,denl3,denh1,denh2,denh3
      integer lo(3), hi(3), domlo(3), domhi(3)
      integer tag(tagl1:tagh1,tagl2:tagh2,tagl3:tagh3)
      double precision den(denl1:denh1,denl2:denh2,denl3:denh3,nc)
      double precision delta(3), avg_den, critdens

      integer i, j, k
!       double precision low(3), high(3)
      double precision r

!       do i = 1,3
!          low(i)  = (1.d0-0.5d0**(level+1)) * 0.5d0 * domhi(i) !*0.25d0
!          high(i) = (1.d0+0.5d0**(level+1)) * 0.5d0 * domhi(i) !*0.75d0
!       enddo

!       do k = lo(3), hi(3)
!          do j = lo(2), hi(2)
!             do i = lo(1), hi(1)
!                if ( i .lt. high(1) .and. i .gt. low(1) ) then
!                   if ( j .lt. high(2) .and. j .gt. low(2) ) then
!                      if ( k .lt. high(3) .and. k .gt. low(3) ) then
!                         tag(i,j,k) = set
!                      endif
!                   endif
!                endif
!             enddo
!          enddo
!       enddo

      r = (5-level)/16.d0*domhi(1)

      do k = lo(3), hi(3)
         do j = lo(2), hi(2)
            do i = lo(1), hi(1)
               if ( dsqrt( (i-0.5d0*domhi(1))**2 + (j-0.5d0*domhi(2))**2 + (k-0.5d0*domhi(3))**2 ) .lt. r ) then
                  tag(i,j,k) = set
               endif
            enddo
         enddo
      enddo

      end subroutine tag_center

! ::: -----------------------------------------------------------
! ::: This routine will tag high error cells based on overdensity
! :::
! ::: INPUTS/OUTPUTS:
! :::
! ::: tag      <=  integer tag array
! ::: lo,hi     => index extent of tag array
! ::: set       => integer value to tag cell for refinement
! ::: clear     => integer value to untag cell
! ::: den       => density array
! ::: nc        => number of components in density array
! ::: domlo,hi  => index extent of problem domain
! ::: delta     => cell spacing
! ::: xlo       => physical location of lower left hand
! :::              corner of tag array
! ::: problo    => phys loc of lower left corner of prob domain
! ::: time      => problem evolution time
! ::: level     => refinement level of this array
! ::: -----------------------------------------------------------
      subroutine tag_lohner(tag,tagl1,tagl2,tagl3,tagh1,tagh2,tagh3, &
                            set,clear, &
                            err,errl1,errl2,errl3,errh1,errh2,errh3, &
                            lo,hi,nc,domlo,domhi,delta,level)

      use probdata_module
      implicit none

      integer set, clear, nc, level
      integer tagl1,tagl2,tagl3,tagh1,tagh2,tagh3
      integer errl1,errl2,errl3,errh1,errh2,errh3
      integer lo(3), hi(3), domlo(3), domhi(3)
      integer tag(tagl1:tagh1,tagl2:tagh2,tagl3:tagh3)
      double precision err(errl1:errh1,errl2:errh2,errl3:errh3,nc)
      double precision delta(3), avg_den, critdens, maxi

      integer i, j, k

      maxi=0.0
!     Tag on regions where the Lohner criterion is not met.
      do k = lo(3), hi(3)
         do j = lo(2), hi(2)
            do i = lo(1), hi(1)
               if ( err(i,j,k,1) .gt. 0.1 ) then
                  tag(i,j,k) = set
               endif
            enddo
         enddo
      enddo

      end subroutine tag_lohner


      SUBROUTINE LSFUN2(M,N,XC,FVECC,FJACC,LJC)

      PARAMETER ( MMAX = 1000 )
      REAL X(MMAX),Y(MMAX)
      COMMON / COMXY / X,Y

      INTEGER M,N,LJC
      DOUBLE PRECISION XC(N),FVECC(M),FJACC(LJC,N)

      INTEGER I

      DO I = 1,M
          FVECC(I) = (XC(1)*DEXP(-((X(I)-XC(2))**2D0)/2D0/XC(3)/XC(3)))
     *               - Y(I)
          FJACC(I,1) = DEXP(-((X(I)-XC(2))**2D0)/2D0/XC(3)/XC(3))
          FJACC(I,2) = XC(1)*DEXP(-((X(I)-XC(2))**2D0)/2D0/XC(3)/XC(3))
     *                 * (X(I)-XC(2)) / XC(3) / XC(3)
          FJACC(I,3) = XC(1)*DEXP(-((X(I)-XC(2))**2D0)/2D0/XC(3)/XC(3))
     *                 * ((X(I)-XC(2))**2D0) / (XC(3)**3D0)
      ENDDO
      RETURN
      END


use FEMDomainClass
use LinearSolverClass
implicit none
type(FEMDomain_) :: domain1, domain2
type(LinearSolver_) :: solver
type(ShapeFunction_) :: sf
integer(int32),allocatable :: domainIDs1(:),domainIDs2(:),domainIDs12(:),InterConnect(:)
real(real64) :: position(3)
real(real64),parameter :: penalty=100.0d0
integer(int32) :: ElementID, i,NodeID
integer(int32),allocatable :: FixBoundary(:)
real(real64),allocatable :: A_ij(:,:), x_i(:), b_i(:) ! A x = b

! A simple contact solver

call domain1%create(meshtype="Cube3D",y_num=3,z_num=3)
call domain1%resize(x=2.0d0,y=2.0d0,z=2.0d0)
call domain1%msh("domain")
call domain2%create(meshtype="Cube3D",y_num=3,z_num=3)
call domain2%resize(x=2.0d0,y=2.0d0,z=2.0d0)
call domain2%move(x=1.9d0)
call domain2%msh("domain2")

! initialize solver
call solver%init(NumberOfNode=(/ domain1%nn(), domain2%nn() /), DOF = 3 )

! Domain#1
! create Elemental Matrices and Vectors
print *, "Domain1: started."
allocate(DomainIDs1(domain1%nne()*domain1%nd() ) )
DomainIDs1(:) = 1
do ElementID=1, domain1%ne()
    ! For 1st element, create stiffness matrix
    A_ij = domain1%StiffnessMatrix(ElementID=ElementID, E=1000.0d0, v=0.40d0)
    b_i  = domain1%MassVector(&
        ElementID=ElementID,&
        DOF=domain1%nd() ,&
        Density=0.30d0,&
        Accel=(/0.0d0, 0.0d0, 0.0d0/)&
        )
    ! assemble them
    call solver%assemble(&
        connectivity=domain1%connectivity(ElementID=ElementID),&
        DOF=domain1%nd() ,&
        eMatrix=A_ij,&
        DomainIDs=DomainIDs1)
    call solver%assemble(&
        connectivity=domain1%connectivity(ElementID=ElementID),&
        DOF=domain1%nd(),&
        eVector=b_i,&
        DomainIDs=DomainIDs1)
enddo
! Domain#2
print *, "Domain2: started."
! create Elemental Matrices and Vectors
allocate(DomainIDs2(domain2%nne()*domain2%nd() ) )
DomainIDs2(:) = 2
do ElementID=1, domain2%ne()
    ! For 1st element, create stiffness matrix
    A_ij = domain2%StiffnessMatrix(ElementID=ElementID, E=1000.0d0, v=0.40d0)
    b_i  = domain2%MassVector(&
        ElementID=ElementID,&
        DOF=domain2%nd() ,&
        Density=0.30d0,&
        Accel=(/0.0d0, 0.0d0, 0.0d0/)&
        )
    ! assemble them
    call solver%assemble(&
        connectivity=domain2%connectivity(ElementID=ElementID),&
        DOF=domain2%nd() ,&
        eMatrix=A_ij,&
        DomainIDs=DomainIDs2)
    call solver%assemble(&
        connectivity=domain2%connectivity(ElementID=ElementID),&
        DOF=domain2%nd(),&
        eVector=b_i,&
        DomainIDs=DomainIDs2)
enddo
! Interface
print *, "Interface: started."
! create Elemental Matrices and Vectors
allocate(DomainIDs12(domain2%nne()+1 ) )
allocate(InterConnect(domain2%nne()+1 ) )
DomainIDs12(1) = 1
DomainIDs12(2:) = 2
do NodeID=1, domain2%nn()
    ! For 1st element, create stiffness matrix
    ! set global coordinate
    position(:) = domain1%mesh%nodcoord(NodeID,:)
    InterConnect(1) = NodeID
    if( domain2%mesh%nearestElementID(x=position(1),y=position(2),z=position(3))<=0 )then
        cycle
    endif
    InterConnect(2:) = domain2%connectivity(domain2%mesh%nearestElementID(x=position(1),y=position(2),z=position(3) ))
    A_ij = penalty*domain2%connectMatrix(position,DOF=domain2%nd() )
    ! assemble them
    call solver%assemble(&
        connectivity=InterConnect,&
        DOF=domain2%nd() ,&
        eMatrix=A_ij,&
        DomainIDs=DomainIDs12)
enddo

! fix deformation >> Dirichlet Boundary
FixBoundary = domain1%select(x_max=0.10d0)
do i=1,size(FixBoundary)
    call solver%fix(nodeid=FixBoundary(i), EntryID=1, entryvalue=0.0d0, DOF=domain2%nd() ,row_DomainID=1)
    call solver%fix(nodeid=FixBoundary(i), EntryID=2, entryvalue=0.0d0, DOF=domain2%nd() ,row_DomainID=1)
    call solver%fix(nodeid=FixBoundary(i), EntryID=3, entryvalue=0.0d0, DOF=domain2%nd() ,row_DomainID=1)
enddo
deallocate(FixBoundary)
FixBoundary = domain2%select(x_min=maxval(domain2%mesh%nodcoord(:,1) )-0.10d0 )
do i=1,size(FixBoundary)
    call solver%fix(nodeid=FixBoundary(i), EntryID=1, entryvalue=0.0d0, DOF=domain2%nd() ,row_DomainID=2)
    call solver%fix(nodeid=FixBoundary(i), EntryID=2, entryvalue=0.50d0, DOF=domain2%nd() ,row_DomainID=2)
    call solver%fix(nodeid=FixBoundary(i), EntryID=3, entryvalue=0.00d0, DOF=domain2%nd() ,row_DomainID=2)
enddo
! solve > get displacement
call solver%solve("BiCGSTAB")
domain1%mesh%nodcoord(:,:) = domain1%mesh%nodcoord(:,:) +&
    reshape( solver%x(1:domain1%nd()*domain1%nn() ),domain1%nn(),domain1%nd() )
domain2%mesh%nodcoord(:,:) = domain2%mesh%nodcoord(:,:) +&
    reshape( solver%x(domain1%nn()*domain1%nd()+1:),domain2%nn(),domain2%nd() )
call domain1%msh("domain1_result")
call domain2%msh("domain2_result")
end


program problem87
  implicit none
  integer, parameter        :: m=664579,limit=50000000
  real*8, parameter         :: alimit=sqrt(dble(limit)),blimit=dble(limit)**(1.0/3.0),climit=dble(limit)**(1.0/4.0)
  integer                   :: a,b,c,temp
  integer, dimension(limit) :: found
  integer, dimension(m)     :: primes

  ! Read primes
  open(33,file='./data/problem70.dat')
  read(33,*) primes

  ! Initialize
  do a=1,limit
     found(a)=0
  end do

  print *, alimit,blimit,climit

  do a=1,huge(1)
     if (primes(a)>alimit) exit
     do b=1,huge(1)
        if (primes(b)>blimit) exit
        do c=1,huge(1)
           if (primes(c)>climit) exit
           temp=primes(a)**2 + primes(b)**3 + primes(c)**4
           if (temp<=limit) then
              found(temp)=1
           end if
        end do
     end do
  end do

  print *, sum(found)

contains

end program problem87


      subroutine runCY_0000li(k,l,i1,Xtwiddle,Gtwiddle,Shat6zz,N0)
      implicit none
C---  Expression for Eq. 5.58b
C---  Calculates C0000li, requires C0000ll
C---  Small terms of order Xtwiddle(0,k)*C00iii,Xtwiddle(0,0)*C00iiii
C---  Denominator Gtwiddle(k,l)
      include 'pvCnames.f'
      include 'pvCv.f'
      include 'Carraydef.f'
      include 'Carrays.f'
      integer ep,N0,k,l,i1,np
      parameter(np=2)
      double precision Xtwiddle(0:np,0:np),Gtwiddle(np,np)
      double complex Shat6zz(np,z3max,-2:0)

      if ((i1 .eq. l) .or. (i1 .eq. 0)) then
      return
      endif

      do ep=-2,0

      Cv(czzzzii(z2(l,i1))+N0,ep)=
     . (-2d0*Gtwiddle(k,i1)*Cv(czzzzii(z2(l,l))+N0,ep)
     . +Gtwiddle(k,1)*Shat6zz(1,z3(l,l,i1),ep)
     . +Gtwiddle(k,2)*Shat6zz(2,z3(l,l,i1),ep)
     . +Xtwiddle(0,k)*Cv(czziii(z3(l,l,i1))+N0,ep)
     . -Xtwiddle(0,0)*Cv(czziiii(z4(k,l,l,i1))+N0,ep)
     . )/(4d0*Gtwiddle(k,l))

      enddo

      return
      end


************************************************************************
*
*     Subroutine OUTSUMMC                Called by: OUTEQN, OUTEQN2
*
*     print summary stats to compare estimates with observations
*
*     local vars
*     ----------
*     XMIN      minimum value
*     P10       10th percentile
*     P25       25th percentile
*     P50       50th percentile
*     P75       75th percentile
*     P90       90th percentile
*     P95       95th percentile
*     P99       99th percentile
*     XMAX      maximum value
*
*     w/ E and O suffix for estimates and observations
*
************************************************************************
      SUBROUTINE OUTSUMMC(NOBSC,EST,OBS,LDOUT)
*
*     subroutine arguments
*
      INTEGER*4 NOBSC,LDOUT
      DOUBLE PRECISION EST(*),OBS(*)
*
*     local vars
*
      DOUBLE PRECISION XMINE,P10E,P25E,P50E,P75E,P90E,P95E,P99E,XMAXE,
     &                 XMINO,P10O,P25O,P50O,P75O,P90O,P95O,P99O,XMAXO
*
*     format statements
*
 1000 FORMAT(1X,A4,3X,8(1PE8.2,1X))
 1100 FORMAT(1X,A7,8(F8.2,1X))
 1200 FORMAT(/,1X,'Est/Obs > 1 indicates overestimation; Est/Obs ',
     &       '< 1 indicates underestimation')
*
*     calculate and print summary stats for estimates, observations, and
*     the ratio of estimated to observed.
*
      CALL PCTL(EST,NOBSC,XMINE,P10E,P25E,P50E,P75E,P90E,P95E,P99E,
     &          XMAXE)
      WRITE(LDOUT,1000) 'Est.',XMINE,P25E,P50E,P75E,P90E,P95E,P99E,XMAXE
      CALL PCTL(OBS,NOBSC,XMINO,P10O,P25O,P50O,P75O,P90O,P95O,P99O,
     &          XMAXO)
      WRITE(LDOUT,1000) 'Obs.',XMINO,P25O,P50O,P75O,P90O,P95O,P99O,XMAXO

      WRITE(LDOUT,1100) 'Est/Obs',XMINE/XMINO,P25E/P25O,P50E/P50O,
     &                  P75E/P75O,P90E/P90O,P95E/P95O,P99E/P99O,
     &                  XMAXE/XMAXO
      WRITE(LDOUT,1200)

      RETURN
      END


!##############################################################################
! MODULE globals
!
! This code is published under the GNU General Public License v3
!                         (https://www.gnu.org/licenses/gpl-3.0.en.html)
!
! Authors: Hans Fehr and Fabian Kindermann
!          contact@ce-fortran.com
!
! #VC# VERSION: 1.0  (23 January 2018)
!
!##############################################################################
module globals

    implicit none

    ! model parameters
    real*8, parameter :: gamma = 0.5d0
    real*8, parameter :: egam = 1d0-1d0/gamma
    real*8, parameter :: beta = 0.95d0
    real*8, parameter :: a0 = 100d0

    ! numerical parameters
    real*8, parameter :: sig = 1d-6
    integer, parameter :: itermax = 2000

    ! time path of consumption and resource
    integer, parameter :: TT = 200
    real*8 :: c_t(0:TT), a_t(0:TT)

    ! value and policy function
    integer, parameter :: NA = 1000
    real*8 :: a(0:NA), c(0:NA), V(0:NA)

    ! variables to numerically determine value and policy function
    real*8 :: V_new(0:NA), coeff_V(NA+3), coeff_c(NA+3)
    real*8 :: con_lev, x_in, fret

    ! variables to communicate with function
    integer :: ia_com

contains

    ! the function that should be minimized
    function utility(x_in)

        use toolbox

        implicit none
        real*8, intent(in) :: x_in
        real*8 :: utility, cons, vplus

        ! calculate consumption
        cons = max(a(ia_com) - x_in, 1d-10)

        ! calculate future utility
        vplus = max(spline_eval(x_in, coeff_V, 0d0, a0), 1d-10)**egam/egam

        ! get utility function
        utility = - (cons**egam/egam + beta*vplus)

    end function

end module