!*******************************************************************************
!>  @file metrics.f90
!>  @brief Contains module @ref metrics.
!
!  Note separating the Doxygen comment block here so detailed decription is
!  found in the Module not the file.
!
!>  Module for reading in VMEC input and computing metric elements on the SIESTA
!>  (sqrt-flux) radial grid. Computes and stores the real-space metric elements,
!>  Jacobian based on square root of fluc splined vmec coordinate system.
!*******************************************************************************

      MODULE metrics
      USE stel_kinds
      USE stel_constants
      USE island_params
      USE shared_data, ONLY: lasym, r1_i, z1_i, ru_i, zu_i, rv_i, zv_i,  &
                             lverbose, jsupvdotA, nsp
      USE read_wout_mod, ns_vmec=>ns, mpol_vmec=>mpol, ntor_vmec=>ntor,  &
          rmnc_vmec=>rmnc, zmns_vmec=>zmns, lmns_vmec=>lmns,             &
          xm_vmec=>xm, xn_vmec=>xn, chipf_vmec=>chipf,                   &  ! MRC 4/1/2016
          rmns_vmec=>rmns, zmnc_vmec=>zmnc, lmnc_vmec=>lmnc,             &  ! MRC 12/1/2016
          phipf_vmec=>phipf, presf_vmec=>presf, nfp_vmec=>nfp,           &
          wb_vmec=>wb, wp_vmec=>wp, gamma_vmec=>gamma,                   &
          volume_vmec=>volume, raxis_vmec=>raxis, lasym_vmec=>lasym,     &
          iasym_vmec=>iasym
      USE descriptor_mod, ONLY: iam
      USE timer_mod
      USE utilities, ONLY: to_full_mesh
      USE siesta_namelist, ONLY: l_vessel

      IMPLICIT NONE

!*******************************************************************************
!  metrics module variables
!*******************************************************************************
!>  Jacobian on half grid
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: sqrtg
!>  Lower metric tensor half grid. e_s . e_s
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gss
!>  Lower metric tensor half grid. e_s . e_u
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gsu
!>  Lower metric tensor half grid. e_s . e_v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gsv
!>  Lower metric tensor half grid. e_u . e_u
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: guu
!>  Lower metric tensor half grid. e_u . e_v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: guv
!>  Lower metric tensor half grid. e_v . e_v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gvv
!>  Upper metric tensor half grid. e^s . e^s
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: hss
!>  Upper metric tensor. e^s . e^u
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: hsu
!>  Upper metric tensor. e^s . e^v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: hsv
!>  Upper metric tensor. e^u . e^u
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: huu
!>  Upper metric tensor. e^u . e^v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: huv
!>  Upper metric tensor. e^v . e^v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: hvv
!>  Lower metric tensor full grid. e_s . e_s
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gssf
!>  Lower metric tensor full grid. e_s . e_u
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gsuf
!>  Lower metric tensor full grid. e_s . e_v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gsvf
!>  Lower metric tensor full grid. e_u . e_u
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: guuf
!>  Lower metric tensor full grid. e_u . e_v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: guvf
!>  Lower metric tensor full grid. e_v . e_v
      REAL(dp), DIMENSION(:,:), ALLOCATABLE :: gvvf

!>  Maximum of the grid R inside the last closed flux surface.
      REAL(dp)                              :: rmax
!>  Minimum of the grid R inside the last closed flux surface.
      REAL(dp)                              :: rmin
!>  Maximum of the grid Z inside the last closed flux surface.
      REAL(dp)                              :: zmax
!>  Minimum of the grid Z inside the last closed flux surface.
      REAL(dp)                              :: zmin

!>  Start timer
      REAL(dp)                              :: skston
!>  Stop timer
      REAL(dp)                              :: skstoff

      CONTAINS

!*******************************************************************************
!  UTILITY SUBROUTINES
!*******************************************************************************
!-------------------------------------------------------------------------------
!>  @brief Initialize the metric elements.
!>
!>  Loads values on to the SIESTA meshes. This involves splining and
!>  interpolating the VMEC quantities from the VMEC radial grid to the SIESTA
!>  radial grid.
!-------------------------------------------------------------------------------
      SUBROUTINE init_metric_elements()
      USE timer_mod, ONLY: init_timers
      USE shared_data, ONLY: torflux, polflux
      USE island_params, hs=>hs_i, ns=>ns_i

      IMPLICIT NONE

!  Start of executable code
!  Compute half-mesh lower and upper metric elements and jacobian.
      CALL half_mesh_metrics(r1_i, ru_i, rv_i, z1_i, zu_i, zv_i)
!  Convert half grid elements to the full grid.
      CALL full_mesh_metrics

      END SUBROUTINE init_metric_elements

!-------------------------------------------------------------------------------
!>  @brief Set grid sizes.
!>
!>  The real space grid is determined from the number of toroidal and poloidal
!>  modes.
!>
!>  @param[in] mpol_in Number of SIESTA poloidal modes.
!>  @param[in] ntor_in Number of SIESTA toroidal modes.
!-------------------------------------------------------------------------------
      SUBROUTINE set_grid_sizes(mpol_in, ntor_in)
      USE island_params
      USE shared_data, ONLY: mblk_size, ndims, lasym

      IMPLICIT NONE

!  Declare Arguments
      INTEGER, INTENT(IN) :: mpol_in
      INTEGER, INTENT(IN) :: ntor_in

!  Start of executable code
      ndims = 3
      IF (lasym) THEN
         ndims = 2*ndims
      END IF

      mpol_i = mpol_in
      ntor_i = ntor_in

!  Set number of points == number of modes for now! (May want mid-points for
!  flux conservation)
      nu_i = mpol_i + 2
      nv_i = 2*ntor_i + 2

!USE 3/2 (ORSZAG) RULE FOR ANTI-ALIASING OF EVOLUTION EQNS
!Suppresses RADIAL grid separation in pressure
      nu_i = 3*nu_i/2
!SPH051617      nu_i = nu_i+MOD(nu_i,2)
      IF (lasym) THEN
         nu_i = 2*nu_i
      ELSE IF (MOD(nu_i, 2) .ne. 1) THEN
         nu_i = nu_i + 1
      END IF

      nv_i = 3*nv_i/2
      nv_i = nv_i + MOD(nv_i, 2)

      IF (ntor_i .EQ. 0) THEN
         nv_i = 1
      END IF
      nuv_i = nu_i*nv_i
      mnmax_i = (mpol_i + 1)*(2*ntor_i + 1) ! Added RS. Contains total number of modes.

      mblk_size = ndims*mnmax_i
      CALL ASSERT(mblk_size .NE. 0, 'mblk_size = 0 in set_grid_sizes')

      END SUBROUTINE

!-------------------------------------------------------------------------------
!>  @brief Surface average a quantity.
!>
!>  @param[out] average Surface average
!>  @param[in]  q3d     3D quantity is real space.
!>  @param[in]  nsmin   Minimum radial index.
!>  @param[in]  nsmax   Maximum radial index.
!-------------------------------------------------------------------------------
      SUBROUTINE SurfAvg(average, q3d, nsmin, nsmax)
      USE island_params, ONLY: fourier_context

      IMPLICIT NONE

!  Declare Arguments
      REAL(dp), DIMENSION(nsmin:nsmax), INTENT(out)          :: average
      REAL(dp), DIMENSION(nu_i,nv_i,nsmin:nsmax), INTENT(IN) :: q3d
      INTEGER, INTENT(IN)                                    :: nsmin
      INTEGER, INTENT(IN)                                    :: nsmax

!  Local variables
      INTEGER                                                :: l
      INTEGER                                                :: js
      REAL(dp), ALLOCATABLE, DIMENSION(:,:,:)                :: wint

!  Start of executable code
      ALLOCATE(wint(nu_i,nv_i,nsmin:nsmax))
      DO l = 1, nu_i
         wint(l,:,:) = fourier_context%cosmui(l,0)
      END DO
      DO js = nsmin, nsmax
         average(js) = SUM(q3d(:,:,js)*wint(:,:,js))
      END DO
      DEALLOCATE(wint)

      END SUBROUTINE

!-------------------------------------------------------------------------------
!>  @brief Load the R, Z and lambda arrays from VMEC.
!>
!>  R, Z and Lambda are recast onto the SIESTA grid.
!>
!>  @param[out] istat Error status.
!-------------------------------------------------------------------------------
      SUBROUTINE LoadGrid(istat)
      USE vmec_info

      IMPLICIT NONE

!  Declare Arguments
      INTEGER, INTENT(out) :: istat

!  Start executable code.
      ALLOCATE (r1_i(nu_i, nv_i, ns_i), z1_i(nu_i, nv_i, ns_i),         &
                ru_i(nu_i, nv_i, ns_i), zu_i(nu_i, nv_i, ns_i),         &
                rv_i(nu_i, nv_i, ns_i), zv_i(nu_i, nv_i, ns_i),         &
                stat = istat)

      CALL rz_to_ijsp(rmnc_i, zmns_i, .false.)
      IF (lasym) THEN
         CALL rz_to_ijsp(rmns_i, zmnc_i, .true.)
      END IF

      rmax = MAXVAL(r1_i)
      rmin = MINVAL(r1_i)
      zmax = MAXVAL(z1_i)
      zmin = MINVAL(z1_i)
      IF (.NOT.lasym) THEN
         zmin = MIN(zmin, -zmax) !  ONLY TOP HALF
      END IF

      END SUBROUTINE

!-------------------------------------------------------------------------------
!>  @brief Transform from fourier R Z to real space qauntities.
!>
!>  Computes R, Z, dr/du, dr/dv, dz/du and dz/dv from the fourier
!>  representation.
!>
!>  @param[in] rmn   Radial fourier amplitudes.
!>  @param[in] zmn   Vertical fourier amplitudes.
!>  @param[in] lasym A symmetric flag.
!-------------------------------------------------------------------------------
      SUBROUTINE rz_to_ijsp(rmn, zmn, lasym)
      USE island_params, ONLY: fourier_context
      USE fourier, ONLY: f_cos, f_sin, f_none, f_sum, f_du, f_dv

      IMPLICIT NONE

!  Declare Arguments
      REAL(dp), DIMENSION(:,:,:), INTENT(in) :: rmn
      REAL(dp), DIMENSION(:,:,:), INTENT(in) :: zmn
      LOGICAL, INTENT(in)                    :: lasym

!  Local variables
      INTEGER                                :: sum
      INTEGER                                :: m
      INTEGER                                :: n
      INTEGER                                :: rmode
      INTEGER                                :: zmode

!  Start executable code.
      IF (lasym) THEN
         sum = f_sum
         rmode = f_sin
         zmode = f_cos
      ELSE
         sum = f_none
         rmode = f_cos
         zmode = f_sin
      END IF
      m = IOR(sum, f_du)
      n = IOR(sum, f_dv)

      CALL fourier_context%toijsp(rmn, r1_i, sum, rmode)
      CALL fourier_context%toijsp(rmn, ru_i, m,   rmode)
      CALL fourier_context%toijsp(rmn, rv_i, n,   rmode)

      CALL fourier_context%toijsp(zmn, z1_i, sum, zmode)
      CALL fourier_context%toijsp(zmn, zu_i, m,   zmode)
      CALL fourier_context%toijsp(zmn, zv_i, n,   zmode)

      END SUBROUTINE

!-------------------------------------------------------------------------------
!>  @brief Compute the metric elements on the half mesh.
!>
!>  @param[in] r1_i R on the full mesh.
!>  @param[in] ru_i dRdu on the full mesh.
!>  @param[in] rv_i dRdv on the full mesh.
!>  @param[in] z1_i Z on the full mesh.
!>  @param[in] zu_i dZdu on the full mesh.
!>  @param[in] zv_i dZdv on the full mesh.
!-------------------------------------------------------------------------------
      SUBROUTINE half_mesh_metrics(r1_i, ru_i, rv_i, z1_i, zu_i, zv_i)
      USE island_params, nuv=>nuv_i, nu=>nu_i, nv=>nv_i!, ns=>ns_i

      IMPLICIT NONE

!  Declare Arguments
      REAL(dp), DIMENSION(nuv,ns_i), INTENT(in) :: r1_i
      REAL(dp), DIMENSION(nuv,ns_i), INTENT(in) :: ru_i
      REAL(dp), DIMENSION(nuv,ns_i), INTENT(in) :: rv_i
      REAL(dp), DIMENSION(nuv,ns_i), INTENT(in) :: z1_i
      REAL(dp), DIMENSION(nuv,ns_i), INTENT(in) :: zu_i
      REAL(dp), DIMENSION(nuv,ns_i), INTENT(in) :: zv_i

!  Local variables
      INTEGER                                   :: js
      INTEGER                                   :: istat
      INTEGER                                   :: imin(2)
      INTEGER                                   :: imax(2)
      REAL(dp)                                  :: mintest
      REAL(dp)                                  :: maxtest
      REAL(dp), DIMENSION(nuv)                  :: r12
      REAL(dp), DIMENSION(nuv)                  :: rs12
      REAL(dp), DIMENSION(nuv)                  :: ru12
      REAL(dp), DIMENSION(nuv)                  :: rv12
      REAL(dp), DIMENSION(nuv)                  :: zs12
      REAL(dp), DIMENSION(nuv)                  :: zu12
      REAL(dp), DIMENSION(nuv)                  :: zv12

!  Local parameters.
      REAL(dp), PARAMETER                     :: p5 = 1.d0/2.d0
      REAL(dp), PARAMETER                     :: zero = 0

!  Start executable code.

!  ALLOCATE METRIC ELEMENT ARRAYS
      ALLOCATE(sqrtg(nuv,ns_i),                                                &
               gss(nuv,ns_i), gsu(nuv,ns_i), gsv(nuv,ns_i),                    &
               guu(nuv,ns_i), guv(nuv,ns_i), gvv(nuv,ns_i), stat=istat)

!  COMPUTE ALL ON THE HALF MESH
      sqrtg(:,1) = 0

      DO js = 2, ns_i
         r12  = p5*(r1_i(:,js) + r1_i(:,js - 1))
         rs12 =    (r1_i(:,js) - r1_i(:,js - 1))*ohs_i
         ru12 = p5*(ru_i(:,js) + ru_i(:,js - 1))
         rv12 = p5*(rv_i(:,js) + rv_i(:,js - 1))
         zs12 =    (z1_i(:,js) - z1_i(:,js - 1))*ohs_i
         zu12 = p5*(zu_i(:,js) + zu_i(:,js - 1))
         zv12 = p5*(zv_i(:,js) + zv_i(:,js - 1))
         gss(:,js) = rs12*rs12 + zs12*zs12
         gsu(:,js) = rs12*ru12 + zs12*zu12
         gsv(:,js) = rs12*rv12 + zs12*zv12
         guu(:,js) = ru12*ru12 + zu12*zu12
         guv(:,js) = ru12*rv12 + zu12*zv12
         gvv(:,js) = rv12*rv12 + r12*r12 + zv12*zv12
         sqrtg(:,js) = r12*(ru12*zs12 - rs12*zu12)
      END DO

      sqrtg(:,1) = sqrtg(:,2)

!NEED THESE FOR CURRENT CALCULATION AT ORIGIN
      gss(:,1) = gss(:,2)
      gsu(:,1) = gsu(:,2)
      gsv(:,1) = gsv(:,2)
      guu(:,1) = guu(:,2)
      guv(:,1) = guv(:,2)
      gvv(:,1) = gvv(:,2)

      mintest = MINVAL(sqrtg(:,2:))
      maxtest = MAXVAL(sqrtg(:,2:))

      IF (mintest*maxtest .LE. zero) THEN
         imin = MINLOC(sqrtg(:,2:))
         imax = MAXLOC(sqrtg(:,2:))
         IF (iam .EQ. 0) THEN
            WRITE(*,1000) mintest, imin(2), maxtest, imax(2)
         END IF
         CALL ASSERT(mintest*maxtest.GT.zero,                                  &
                     ' Jacobian changed sign in half_mesh_metrics')
      END IF

1000  FORMAT(' MIN-G: ',1p,e12.4,' AT JS: ',i5,                                &
             ' MAX-G: ',1p,e12.4,' AT JS: ',i5)

      END SUBROUTINE

!-------------------------------------------------------------------------------
!>  @brief Gets the full grid metric elements.
!>
!>  This subroutine gets the lower metric elements on the full mesh. Preserves
!>  positive definiteness of metric tensor.
!-------------------------------------------------------------------------------
      SUBROUTINE full_mesh_metrics
      USE island_params, nuv=>nuv_i

      IMPLICIT NONE

!  Local variables
      INTEGER                                :: istat
      INTEGER                                :: js
      REAL (dp), DIMENSION(:), ALLOCATABLE   :: det
      REAL (dp), DIMENSION(:,:), ALLOCATABLE :: temph

!  Start executable code.
      ALLOCATE(gssf(nuv,ns_i), guuf(nuv,ns_i), gvvf(nuv,ns_i),                 &
               gsuf(nuv,ns_i), gsvf(nuv,ns_i), guvf(nuv,ns_i),                 &
               hss(nuv,ns_i),  huu(nuv,ns_i),  hvv(nuv,ns_i),                  &
               hsu(nuv,ns_i),  hsv(nuv,ns_i),  huv(nuv,ns_i), stat=istat)

      CALL to_full_mesh(gss, gssf)
      CALL to_full_mesh(guu, guuf)
      CALL to_full_mesh(gvv, gvvf)
      CALL to_full_mesh(gsu, gsuf)
      CALL to_full_mesh(gsv, gsvf)
      CALL to_full_mesh(guv, guvf)

      ALLOCATE(det(nuv))

!  Compute upper metric elements (inverse of lower matrix) on full mesh.
!  NOTE: DET == 1/det|Gij| == 1/sqrtg(full)**2
      DO js = 1, ns_i
         det(:) = gsvf(:,js)*gsvf(:,js)*guuf(:,js)                             &
                - 2.0*gsuf(:,js)*gsvf(:,js)*guvf(:,js)                         &
                + gssf(:,js)*guvf(:,js)*guvf(:,js)                             &
                + gsuf(:,js)*gsuf(:,js)*gvvf(:,js)                             &
                - gssf(:,js)*guuf(:,js)*gvvf(:,js)

          hss(:,js) = (guvf(:,js)*guvf(:,js) - guuf(:,js)*gvvf(:,js))/det
          hsu(:,js) = (gsuf(:,js)*gvvf(:,js) - gsvf(:,js)*guvf(:,js))/det
          hsv(:,js) = (gsvf(:,js)*guuf(:,js) - gsuf(:,js)*guvf(:,js))/det
          huu(:,js) = (gsvf(:,js)*gsvf(:,js) - gssf(:,js)*gvvf(:,js))/det
          huv(:,js) = (gssf(:,js)*guvf(:,js) - gsuf(:,js)*gsvf(:,js))/det
          hvv(:,js) = (gsuf(:,js)*gsuf(:,js) - gssf(:,js)*guuf(:,js))/det
      END DO

      guuf(:,1) = 0
      gsuf(:,1) = 0
      guvf(:,1) = 0

      hss(:,1) = 0
      hsu(:,1) = 0
      hsv(:,1) = 0
      huv(:,1) = 0
      hvv(:,1) = 0

      DEALLOCATE(det)

      CALL check_metrics

1000  FORMAT('Determinant |gijf| <= 0 at js = ',i4)

      END SUBROUTINE full_mesh_metrics

!-------------------------------------------------------------------------------
!>  @ brief Test to check that we computed the upper metric elements correctly.
!>
!>  Check that the metric elements conform to the following relation to machine
!>  precission.
!>
!>    hij*gjk = delta_ik                                                     (1)
!>
!>  Note the repeated index implys a sum.
!-------------------------------------------------------------------------------
      SUBROUTINE check_metrics

      IMPLICIT NONE

!  Local Parameters
      REAL (dp), PARAMETER :: tolarance = 1.0E15

!  Start executable code.
      CALL ASSERT(ALL(ABS(hss*gssf + hsu*gsuf + hsv*gsvf - 1) .lt. tolarance), &
                      's Diagonal metric element failed.')
      CALL ASSERT(ALL(ABS(hsu*gsuf + huu*guuf + huv*guvf - 1) .lt. tolarance), &
                      'u Diagonal metric element failed.')
      CALL ASSERT(ALL(ABS(hsv*gsvf + huv*guvf + hvv*gvvf - 1) .lt. tolarance), &
                      'v Diagonal metric element failed.')
      CALL ASSERT(ALL(ABS(hss*gsuf + hsu*guuf + hsv*guvf) .lt. tolarance),     &
                      'su Off diagonal metric element failed.')
      CALL ASSERT(ALL(ABS(hss*gsvf + hsu*guvf + hsv*gvvf) .lt. tolarance),     &
                      'sv Off diagonal metric element failed.')
      CALL ASSERT(ALL(ABS(hsu*gsvf + huu*guvf + huv*gvvf) .lt. tolarance),     &
                      'uv Off diagonal metric element failed.')

      END SUBROUTINE

!-------------------------------------------------------------------------------
!>  @brief Converts to contravariant component full grid.
!>
!>  Computes contravariant (upper) from covariant (lower)  components on the
!>  full mesh.
!>
!>    xsupsij = hss*xsubsij + hsu*xsupuij + hsv*xsubvij
!>    xsupuij = hsu*xsubsij + huu*xsupuij + huv*xsubvij
!>    xsupvij = hsv*xsubsij + huv*xsupuij + hvv*xsubvij
!>
!>  @param[in]  xsubsij Covariant s component.
!>  @param[in]  xsubuij Covariant u component.
!>  @param[in]  xsubvij Covariant v component.
!>  @param[out] xsupsij Contravariant s component.
!>  @param[out] xsupuij Contravariant u component.
!>  @param[out] xsupvij Contravariant v component.
!>  @param[in]  nsmin   Min radial index.
!>  @param[in]  nsmax   Max radial index.
!-------------------------------------------------------------------------------
      SUBROUTINE toupper(xsubsij, xsubuij, xsubvij,                            &
                         xsupsij, xsupuij, xsupvij, nsmin, nsmax)
      USE stel_kinds
      USE island_params, nuv=>nuv_i

      IMPLICIT NONE

!  Declare Arguments
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(in)  :: xsubsij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(in)  :: xsubuij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(in)  :: xsubvij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(out) :: xsupsij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(out) :: xsupuij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(out) :: xsupvij
      INTEGER, INTENT(in)                               :: nsmin
      INTEGER, INTENT(in)                               :: nsmax

!  Local variables
        INTEGER                                         :: js

!  Start executable code.
      DO js = nsmin, nsmax
         xsupsij(:,js) = hss(:,js)*xsubsij(:,js)                               &
                       + hsu(:,js)*xsubuij(:,js)                               &
                       + hsv(:,js)*xsubvij(:,js)
         xsupuij(:,js) = hsu(:,js)*xsubsij(:,js)                               &
                       + huu(:,js)*xsubuij(:,js)                               &
                       + huv(:,js)*xsubvij(:,js)
         xsupvij(:,js) = hsv(:,js)*xsubsij(:,js)                               &
                       + huv(:,js)*xsubuij(:,js)                               &
                       + hvv(:,js)*xsubvij(:,js)
      END DO

      END SUBROUTINE

!-------------------------------------------------------------------------------
!>  @brief Converts to covariant component half grid.
!>
!>  Computes covariant (lower) from contravariant (upper) magnetic field
!>  components on the half mesh.
!>
!>    xsubsij = gss*xsupsij + gsu*xsupuij + gsv*xsupvij
!>    xsubuij = gsu*xsupsij + guu*xsupuij + guv*xsupvij
!>    xsubvij = gsv*xsupsij + guv*xsupuij + gvv*xsupvij
!>
!>  @param[in]  xsupsij Contravariant s component.
!>  @param[in]  xsupuij Contravariant u component.
!>  @param[in]  xsupvij Contravariant v component.
!>  @param[out] xsubsij Covariant s component.
!>  @param[out] xsubuij Covariant u component.
!>  @param[out] xsubvij Covariant v component.
!>  @param[in]  nsmin   Min radial index.
!>  @param[in]  nsmax   Max radial index.
!-------------------------------------------------------------------------------
      SUBROUTINE tolowerh(xsupsij, xsupuij, xsupvij,                      &
                          xsubsij, xsubuij, xsubvij, nsmin, nsmax)
      USE stel_kinds
      USE island_params, ONLY: nuv=>nuv_i

      IMPLICIT NONE

!  Declare Arguments
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(in)  :: xsupsij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(in)  :: xsupuij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(in)  :: xsupvij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(out) :: xsubsij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(out) :: xsubuij
      REAL(dp), DIMENSION(nuv,nsmin:nsmax), INTENT(out) :: xsubvij
      INTEGER, INTENT(IN)                               :: nsmin
      INTEGER, INTENT(IN)                               :: nsmax

!  Start executable code.
      xsubsij = gss(:,nsmin:nsmax)*xsupsij                               &
              + gsu(:,nsmin:nsmax)*xsupuij                               &
              + gsv(:,nsmin:nsmax)*xsupvij

      xsubuij = gsu(:,nsmin:nsmax)*xsupsij                               &
              + guu(:,nsmin:nsmax)*xsupuij                               &
              + guv(:,nsmin:nsmax)*xsupvij

      xsubvij = gsv(:,nsmin:nsmax)*xsupsij                               &
              + guv(:,nsmin:nsmax)*xsupuij                               &
              + gvv(:,nsmin:nsmax)*xsupvij

      END SUBROUTINE tolowerh

!-------------------------------------------------------------------------------
!>  @brief Converts to covariant component full grid.
!>
!>  Computes covariant (lower) from contravariant (upper) magnetic field
!>  components on the full mesh.
!>
!>    xsubsij = gssf*xsupsij + gsuf*xsupuij + gsvf*xsupvij
!>    xsubuij = gsuf*xsupsij + guuf*xsupuij + guvf*xsupvij
!>    xsubvij = gsvf*xsupsij + guvf*xsupuij + gvvf*xsupvij
!>
!>  @param[in]  xsupsij Contravariant s component.
!>  @param[in]  xsupuij Contravariant u component.
!>  @param[in]  xsupvij Contravariant v component.
!>  @param[out] xsubsij Covariant s component.
!>  @param[out] xsubuij Covariant u component.
!>  @param[out] xsubvij Covariant v component.
!>  @param[in]  nsmin   Min radial index.
!>  @param[in]  nsmax   Max radial index.
!-------------------------------------------------------------------------------
      SUBROUTINE tolowerf(xsupsij, xsupuij, xsupvij,                     &
                          xsubsij, xsubuij, xsubvij, nsmin, nsmax)
      USE stel_kinds
      USE island_params, ONLY: ns=>ns_i, nuv=>nuv_i

      IMPLICIT NONE

!  Declare Arguments
      REAL(dp), DIMENSION(nuv,ns), INTENT(in)  :: xsupsij  !  MRC: Why ns and not nsmin:nsmax here?
      REAL(dp), DIMENSION(nuv,ns), INTENT(in)  :: xsupuij
      REAL(dp), DIMENSION(nuv,ns), INTENT(in)  :: xsupvij
      REAL(dp), DIMENSION(nuv,ns), INTENT(out) :: xsubsij
      REAL(dp), DIMENSION(nuv,ns), INTENT(out) :: xsubuij
      REAL(dp), DIMENSION(nuv,ns), INTENT(out) :: xsubvij
      INTEGER, INTENT(in)                        :: nsmin
      INTEGER, INTENT(in)                        :: nsmax

!  Local variables
      INTEGER                                    :: js
      INTEGER                                    :: js1

!  Start executable code.
      DO js = nsmin, nsmax
         js1 = js - nsmin + 1
         xsubsij(:,js1) = gssf(:,js)*xsupsij(:,js1)                      &
                        + gsuf(:,js)*xsupuij(:,js1)                      &
                        + gsvf(:,js)*xsupvij(:,js1)

         xsubuij(:,js1) = gsuf(:,js)*xsupsij(:,js1)                      &
                        + guuf(:,js)*xsupuij(:,js1)                      &
                        + guvf(:,js)*xsupvij(:,js1)

         xsubvij(:,js1) = gsvf(:,js)*xsupsij(:,js1)                      &
                        + guvf(:,js)*xsupuij(:,js1)                      &
                        + gvvf(:,js)*xsupvij(:,js1)
      END DO

!  FIXME: CHECK THIS
      IF (nsmin .eq. 1) THEN
         xsubuij(:,1) = 0
      END IF

      END SUBROUTINE tolowerf

!-------------------------------------------------------------------------------
!>  @brief Deallocate memory containing metric elements on the half mesh.
!>
!>  Also deallocates the grid variables.
!>
!>  @note
!>  sqrtg is deallocated in @ref init_bcovar and stored in @ref jacob variable
!-------------------------------------------------------------------------------
      SUBROUTINE cleanup_metric_elements
      USE island_params, ONLY: fourier_context
      USE vmec_info

      IMPLICIT NONE

!  Local variables
      INTEGER :: istat

!  Start executable code.
      DEALLOCATE(gss, gsu, gsv, guu, guv, gvv,                           &
                 hss, hsu, hsv, huu, huv, hvv,                           &
                 phipf_i, chipf_i, presf_i, stat=istat)

      DEALLOCATE (r1_i, z1_i, ru_i, zu_i, rv_i, zv_i)

      DEALLOCATE(fourier_context)
      CALL vmec_info_destruct_island

      END SUBROUTINE cleanup_metric_elements

!-------------------------------------------------------------------------------
!>  @brief Deallocate memory containing metric elements on the full mesh.
!-------------------------------------------------------------------------------
      SUBROUTINE dealloc_full_lower_metrics
      USE stel_kinds

      IMPLICIT NONE

!  Local variables
      INTEGER :: istat

!  Start executable code.
      DEALLOCATE(gssf, guuf, gvvf, gsuf, gsvf, guvf, stat = istat)
!      IF (istat .NE. 0) CALL ErrorAbort('Problem dealloc. in LOWER_METRIC_FULL')

      END SUBROUTINE dealloc_full_lower_metrics

      END MODULE


      SUBROUTINE PCC_EXP(NR,ICORE,AC,BC,CC,R,CDC)

c mmga   M.M.G. Alemany
c mmga
c mmga

      IMPLICIT DOUBLE PRECISION (A-H,O-Z)

      PARAMETER (NN = 5)

      DIMENSION R(NR),CDC(NR),
     .          DR_K(-NN:NN),DDR_K(-NN:NN),
     .          CDC_SCA(ICORE-NN:ICORE+NN)

      IF ( (ICORE - NN).LT.1 .OR. (ICORE + NN).GT.NR ) THEN
        WRITE(6,2017)
        CALL EXT(830)
      ENDIF

      DO 2000   I = ICORE-NN,ICORE+NN
        CDC_SCA(I)= LOG(CDC(I)) - 2.D0 * LOG(R(I))
 2000 CONTINUE

      IN1 =-NN
      IN2 = NN

      DO 2011 IN = IN1,IN2
        IF (IN.EQ.0) THEN
          DR_K(IN) = 0.D0
          DO 2012 JN = IN1,IN2
            IF (JN.NE.0) DR_K(IN) = DR_K(IN) + 1.D0/(0 - JN)
 2012     CONTINUE
        ELSE
          F1 = 1.D0
          F2 = 1.D0
          DO 2014 JN = IN1,IN2
            IF (JN.NE.IN .AND. JN.NE.0) F1 = F1 * (0  - JN)
            IF (JN.NE.IN)               F2 = F2 * (IN - JN)
 2014     CONTINUE
          DR_K(IN) = F1 / F2
        ENDIF
 2011 CONTINUE

      DR = 0.D0
      DO 2016 IN = IN1,IN2
        DR = DR + DR_K(IN) * R(ICORE+IN)
 2016 CONTINUE

      CDCP = 0.D0
      DO 2001 IN = IN1,IN2
        CDCP = CDCP + DR_K(IN) * CDC_SCA(ICORE+IN)
 2001 CONTINUE
      CDCP = CDCP / DR

      DO 2002 IN = IN1,IN2
        DDR_K(IN)= 0.D0
        IF (IN.EQ.0) THEN
          DO 2003 JN = IN1,IN2
            IF (JN.NE.0) THEN
              DO 2004 KN = JN+1,IN2
                IF (KN.NE.0) DDR_K(IN) = DDR_K(IN) +
     .                                   2.D0/((0 - JN)*(0 - KN))
 2004         CONTINUE
            ENDIF
 2003     CONTINUE
        ELSE
          F2 = 1.D0
          DO 2005 JN = IN1,IN2
            IF (JN.NE.IN) THEN
              F2 = F2 * (IN - JN)
              DO 2006 KN = JN+1,IN2
                IF (KN.NE.IN) THEN
                  F1 = 2.D0
                  DO 2007 LN = IN1,IN2
                    IF (LN.NE.IN .AND. LN.NE.JN .AND. LN.NE.KN)
     .                  F1 = F1 * (0 - LN)
 2007             CONTINUE
                  DDR_K(IN) = DDR_K(IN) + F1
                ENDIF
 2006         CONTINUE
            ENDIF
 2005     CONTINUE
          DDR_K(IN) = DDR_K(IN) / F2
        ENDIF
 2002 CONTINUE

      DDR   = 0.D0
      DDCDC = 0.D0
      DO 2010 IN = IN1,IN2
        DDR   = DDR   + DDR_K(IN) * R(ICORE+IN)
        DDCDC = DDCDC + DDR_K(IN) * CDC_SCA(ICORE+IN)
 2010 CONTINUE

      CDCPP = (DDCDC - DDR * CDCP) / DR**2

      CC = R(ICORE) * CDCPP - CDCP
      CC = CC / (8.D0 * R(ICORE)**3)

      BC = CDCP - 4.D0 * CC * R(ICORE)**3
      BC = BC / (2.D0 * R(ICORE))

      AC = CDC_SCA(ICORE) - BC*R(ICORE)**2 - CC*R(ICORE)**4

      DO 2009 I = 1,ICORE
          CDC(I)= R(I)*R(I) *
     .            EXP( (CC*R(I)*R(I) + BC) * R(I)*R(I) + AC )
 2009 CONTINUE

 2017 FORMAT(//,' error in pseud2 - ',/,
     . ' derivatives at r(icore) not calculated')

      RETURN

      END


  subroutine adder1( &
     ngeomax,    maxstr,     imuv,        &
     imu0,       mmax,       nsmax,       &
     ns_elas,    m,          gmu,         &
     R1m_A,      T1m_A,      R1mStar_A,   &
     T1mStar_A,  dtau_A,     R1m_B,       &
     T1m_B,      R1mStar_B,  T1mStar_B,   &
     dtau_B)

!----------------------------------------------------------------------
! Subroutine:
!   adder1
!
! Purpose:
!   Adding two layers for single scattering only
!   (neglect multiple scattering between layer A and B)
!   Layer A on top, layer B at bottom
!   After adding: Rm_B, Tm_B, RmStar_B, TmStar_B contain
!   reflectance, transmittance of layers A and B  combined
!
! Input:
!
!   Reflectance and transmittance of top layer
!     R1m_A
!     T1m_A
!     R1mStar_A
!     T1mStar_A
!     dtau_A
!
!   Reflectance and transmitance of bottom layer
!     R1m_B
!     T1m_B
!     R1mStar_B
!     T1mStar_B
!     dtau_B
!
! Output:
!
!   Reflectance and transmittance of combined layer AB
!   with optical thickness dtau_A + dtau_B
!     R1m_B
!     T1m_B
!     R1mStar_B
!     T1mStar_B
!
!------------------------------------------------------------------

  implicit none

  integer,parameter::dp=selected_real_kind(8) !double precision

  integer,intent(in):: &
     ngeomax,          &
     maxstr,           &
     imuv(ngeomax),    &
     imu0(ngeomax),    &
     mmax,             &
     m,                & !Fourier number
     nsmax,            & !dimension of internal arrays
     ns_elas             !index of elastic wavenr

  integer:: &
     ns1, &
     ns2, & !wavenumber indices [internal grid]
     i1,  &
     i2,  & !quad angle indices
     iv

! input
  real(kind=dp):: &
     R1m_A(0:nsmax,0:nsmax,ngeomax),       &
     T1m_A(0:nsmax,0:nsmax,ngeomax),       &
     R1mStar_A(0:nsmax,0:nsmax,ngeomax),   &
     T1mStar_A(0:nsmax,0:nsmax,ngeomax),   &
     dtau_A(0:nsmax),               &
     R1m_B(0:nsmax,0:nsmax,ngeomax),       &
     T1m_B(0:nsmax,0:nsmax,ngeomax),       &
     R1mStar_B(0:nsmax,0:nsmax,ngeomax),   &
     T1mStar_B(0:nsmax,0:nsmax,ngeomax),   &
     dtau_B(0:nsmax),               &
     gmu(maxstr)    !Gaussian quadrature angles

  real(kind=dp):: &
     uv, &
     u0

! output
  real(kind=dp):: &
     R1mTot(0:nsmax,0:nsmax,ngeomax),     &
     T1mTot(0:nsmax,0:nsmax,ngeomax),     &
     R1mTotStar(0:nsmax,0:nsmax,ngeomax), &
     T1mTotStar(0:nsmax,0:nsmax,ngeomax)

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

  do iv=1,ngeomax

     uv = gmu(imuv(iv))
     u0 = gmu(imu0(iv))

!--------------------------------------------------------------------------
!    R1mtot  = R1m_A  + exp(-dtau_A(nu)/mu) R1m_B  exp(-dtau_A(nu')/mu')
!    R1mtot* = R1m*_B + exp(-dtau_B(nu)/mu) R1m*_A exp(-dtau_B(nu')/mu')
!
!    T1mtot  = exp(-dtau_B(nu)/mu) T1m_A  + T1m_B  exp(-dtau_A(nu')/mu')
!    T1mtot* = exp(-dtau_A(nu)/mu) T1m*_B + T1m*_A exp(-dtau_B(nu')/mu')
!--------------------------------------------------------------------------

     do ns2=0,nsmax
        do ns1=0,nsmax

           R1mTot(ns1,ns2,iv) = R1m_A(ns1,ns2,iv) + &
                                 exp(-dtau_A(ns1)/uv) * &
                                  R1m_B(ns1,ns2,iv) * &
                                   exp(-dtau_A(ns2)/u0)

           R1mTotStar(ns1,ns2,iv) = R1mStar_B(ns1,ns2,iv) + &
                                     exp(-dtau_B(ns1)/uv) * &
                                      R1mStar_A(ns1,ns2,iv) * &
                                       exp(-dtau_B(ns2)/u0)

           T1mTot(ns1,ns2,iv) = exp(-dtau_B(ns1)/uv) * &
                                 T1m_A(ns1,ns2,iv) + &
                                  T1m_B(ns1,ns2,iv) * &
                                   exp(-dtau_A(ns2)/u0)

           T1mTotStar(ns1,ns2,iv) = exp(-dtau_A(ns1)/uv) * &
                                     T1mStar_B(ns1,ns2,iv) + &
                                      T1mStar_A(ns1,ns2,iv) * &
                                       exp(-dtau_B(ns2)/u0)

           R1m_B(ns1,ns2,iv) = R1mTot(ns1,ns2,iv)
           T1m_B(ns1,ns2,iv) = T1mTot(ns1,ns2,iv)

           R1mStar_B(ns1,ns2,iv) = R1mTotStar(ns1,ns2,iv)
           T1mStar_B(ns1,ns2,iv) = T1mTotStar(ns1,ns2,iv)

      enddo
    enddo

  enddo

  return
  end subroutine adder1


Program Pancake
    Integer, Dimension(1:1010) :: P
    Integer :: Ti, T, D, Di, M, Mi, Bs, Mb
    read (*, *) T
    do Ti=1, T
        read (*, *) D
        read (*, *) P(1:D)
        M = maxval(P(1:D))
        Mb = M
        do Mi=1, M
            Bs = 0
            do Di=1, D
                if (mod(P(Di), Mi) == 0) then
                    Bs = Bs + (P(Di) / Mi)
                else
                    Bs = Bs + 1 + (P(Di) / Mi)
                end if
            end do
            Di = Bs + Mi - D
            if (Di < Mb) then
                Mb = Di
            end if
        end do
        write (*, "(A6, I0, A2, I0)") "Case #", Ti, ": ", Mb
    end do
End Program


!  *********************************************************************
!  *                                                                   *
!  *                           function dcvaa1                         *
!  *                                                                   *
!  *********************************************************************
!  Double Precision Version 1.4
!  Written by Gordon A. Fenton, DalTech, Jun 9, 2000
!  Latest Update: Apr 8, 2003
!
!  PURPOSE  returns the covariance between two 1-D local averages of equal
!           length. Used by LAS1G.
!
!  This function evaluates the covariance between two local averages in
!  1-dimensional space. The local averages are assumed to be of equal
!  size, Dx, and separated in space by the lag Tx=C1*Dx
!
!  The covariance is obtained by a 16-pt Gaussian quadrature of a
!  1-dimensional integral of the externally provided point covariance
!  function of the process
!
!                   1    C1*Dx                   (C1+1)Dx
!  Cov[X_a, X_b] = --- [ int{x - (C1-1)Dx}cov(x)dx+int{(C1+1)Dx - x}cov(x) dx ]
!                  Dx^2 (C1-1)Dx                   C1*Dx
!
!  Where X_a and X_b are the local averages of the two areas, each of size
!  Dx and separated center-to-center by lag C1*Dx.
!
!  The covariance function is referenced herein using a call of the form
!
!          V = cov( X )
!
!  where X is the lag distances between the points in the field.
!
!  Arguments to this function are as follows
!
!    cov       external covariance function provided by the user.
!
!    Dx        x-dimension of each local average. (input)
!
!    C1        x-direction distance between local average centers is C1*Dx.
!              (input)
!
!  REVISION HISTORY:
!  1.1	eliminated lvarfn - now choose integration method to minimize
!	errors. (Feb 27/01)
!  1.2	now choose Gauss Quadrature except when intervals overlap. (Apr 5/01)
!  1.3	now use Gauss Quadrature for everything (Apr 16/01)
!  1.4	now use 16-pt Gauss quadrature for increased accuracy (Apr 8/02)
!---------------------------------------------------------------------------
      real*8 function dcvaa1( cov, Dx, C1 )
      parameter (NG = 16)
      implicit real*8 (a-h,o-z)
      dimension w(NG), z(NG)

      external cov

      data zero/0.d0/, quart/0.25d0/, half/0.5d0/, one/1.d0/
      data two/2.d0/

!			these are for NG = 3
!     data w/0.555555555555556d0,.888888888888889d0,.555555555555556d0/
!     data z/-.774596669241483d0,.000000000000000d0,.774596669241483d0/
!			and these are for NG = 5
!     data w/ .236926885056189d0,.478628670499366d0,.568888888888889d0,
!    >        .478628670499366d0,.236926885056189d0/
!     data z/-.906179845938664d0,-.538469310105683d0,.000000000000000d0,
!    >        .538469310105683d0, .906179845938664d0/
!			these are for NG = 16
      data w/0.027152459411754094852d0, 0.062253523938647892863d0,  &
             0.095158511682492784810d0, 0.124628971255533872052d0,  &
             0.149595988816576732081d0, 0.169156519395002538189d0,  &
             0.182603415044923588867d0, 0.189450610455068496285d0,  &
             0.189450610455068496285d0, 0.182603415044923588867d0,  &
             0.169156519395002538189d0, 0.149595988816576732081d0,  &
             0.124628971255533872052d0, 0.095158511682492784810d0,  &
             0.062253523938647892863d0, 0.027152459411754094852d0/
      data z/-.989400934991649932596d0, -.944575023073232576078d0,  &
             -.865631202387831743880d0, -.755404408355003033895d0,  &
             -.617876244402643748447d0, -.458016777657227386342d0,  &
             -.281603550779258913230d0, -.095012509837637440185d0,  &
             0.095012509837637440185d0, 0.281603550779258913230d0,  &
             0.458016777657227386342d0, 0.617876244402643748447d0,  &
             0.755404408355003033895d0, 0.865631202387831743880d0,  &
             0.944575023073232576078d0, 0.989400934991649932596d0/

      r1  = half*Dx
!				if intervals overlap, can use this
!     if( C1 .eq. zero ) then
!        d1 = zero
!        do 10 i = 1, NG
!           xi = r1*(one + z(i))
!           d1 = d1 + w(i)*(one-z(i))*cov(xi)
! 10     continue
!        dcvaa1 = half*d1
!        return
!     endif
!				however, this formulation works well
!				for both overlapping and non-overlapping
!				intervals
      s1  = two*C1 - one
      s2  = two*C1 + one

      d1  = zero
      do 20 i = 1, NG
         xi1 = r1*(z(i) + s1)
         xi2 = r1*(z(i) + s2)
         d1  = d1 + w(i)*( (one+z(i))*cov(xi1) + (one-z(i))*cov(xi2) )
  20  continue
      dcvaa1 = quart*d1

      return
      end


module Elastic_sol_PGD

use Structure
use bar
use Matrix_op
use gnuplot_fortran
use Time_matrices

implicit none

  type :: Init_sol

    double precision, dimension(:,:), allocatable :: U
    double precision, dimension(:,:,:,:), allocatable :: sigma,eps
    double precision, dimension(:,:), allocatable :: Mss,Kss,ChKss

  end type Init_sol

  type(Init_sol) :: Elast

contains

subroutine solve_init_PGD(I,Sop,Top)

    type(Input), intent(in) :: I
    type(Space_op), intent(in) :: Sop
    type(Time_op), intent(inout) :: Top

    double precision, dimension(I%n,I%n) :: M ! Total mass matrix
    double precision, dimension(I%n,I%n) :: K ! Total stifness matrix
    double precision, dimension(I%ns,I%ns) :: Mss ! Free dofs mass matrix
    double precision, dimension(I%ns,I%ns) :: Kss ! Free dofs stifness matrix
    double precision, dimension(I%ns,I%ns) :: ChKss ! Free dofs stifness matrix

    double precision, dimension(I%ns,I%ns) :: invKss ! inverse of Kss
    double precision, dimension(I%ns,2) :: Kud ! stiffness matrix related to fix dofs
    double precision, dimension(I%ns,I%Tngpt) :: Fud,x
    double precision, dimension(I%n,I%Tngpt) :: U   ! Displacement
    double precision, dimension(2,I%Tngpt) :: Ud   ! imposed Displacement
    double precision, dimension(I%ns,I%Tngpt) :: Us ! free dofs displacement
    double precision, dimension(I%Tngpt) :: vect
    double precision, dimension(I%Tngps,I%Tngpt) :: eps,sigma
    integer, dimension(4) :: siz

    integer :: ielm
    integer :: ns, nrhs, info, lda, ldb   ! dimension of the matrix that must be inverted.
    integer, allocatable, dimension(:) :: ipiv

    ! PGD variable definitions:
    double precision, dimension(I%ns,I%Tngpt) :: F
    double precision :: error_PGD_lim, error_PGD, Fud_norm, a, b, snorm, stagn_lim, stagn
    integer ::  PGDm, iter_pgd, max_iter

    double precision, dimension(I%ns,1) :: Si,Fi
    double precision, dimension(I%Tngpt,1) :: Ti, Ta
    double precision, dimension(:,:) , allocatable :: Sl,Tl,S,T

    ! Assembly:
    do ielm = 1, I%Nx
       M( Sop%Idvs(:,ielm) , Sop%Idvs(:,ielm)  ) = M( Sop%Idvs(:,ielm) , Sop%Idvs(:,ielm) ) + Sop%Melm
       K( Sop%Idvs(:,ielm) , Sop%Idvs(:,ielm)  ) = K( Sop%Idvs(:,ielm) , Sop%Idvs(:,ielm) ) + Sop%Kelm
    end do

    ! Imposed displacement:
    U(1,:) = 0 ! Fix condition.
    U(I%n,:) = I%ud ! displacement condition.

    ! Dofs free and imposed of the matrices used:
    Kss=K(Sop%dof_free,Sop%dof_free)
    Kud=K(Sop%dof_free,Sop%dof_imposed)

    ChKss = cholesky(Kss) ! Cholesky decomposition of Kss

    Ud = U(Sop%dof_imposed,:)
    Fud=matmul(Kud,Ud) ! Force due to the imposition of displacements.

    !! Classic solution (code Seba):

    if (.false.) then

      print*, "Classic resolution of elastic problem."

      !! (code Blas Lapack):
      ns = size(Kss,1)
      nrhs = size(Fud,2)
      !print*, ns,nrhs
      lda = ns; ldb = ns
      allocate(ipiv(ns))
      x = -Fud
      call dgesv(ns,nrhs,Kss,lda,ipiv,x,ldb,info)
      Us = x

    else

      ! PGD resolution:

      print*, "PGD resolution of elastic problem."

      ns = size(Kss,1)
      nrhs = 1
      lda = ns; ldb = ns
      allocate(ipiv(ns)); !ipiv = 0

      error_PGD = 100
      error_PGD_lim = 0.01
      stagn_lim = 0.1

      max_iter = 6
      Fud_norm = norm2(Fud)

      ! S, T;  PGD functions related to the displacement solution.
      PGDm = 0 ! PGD mode
      do while ( error_PGD_lim <= error_PGD )

        PGDm = PGDm + 1 ! PGD mode

        ! Update of the external force:
        if (PGDm .eq. 1) then
          F = -Fud
        else
          F = -Fud - matmul( matmul(Kss,S), transpose(T) )
        end if
        ! end update of the external force:

        Si = 0; Ti = reshape(Top%vect_gpt, (/I%Tngpt,1/) ); Ta = Ti ! Initialization.

        iter_pgd = 0
        stagn = 100
        do while ( stagn_lim <= stagn .and. max_iter > iter_pgd )

          iter_pgd = iter_pgd + 1
          !print*, "Iter PGD", iter_pgd

          ! Space problem:
          a = sum( (Ti**2)*Top%IntT )
          Fi = matmul(F,Ti*Top%IntT)
          call dgesv(ns,nrhs,Kss,lda,ipiv,Fi,ldb,info)
          Si = Fi/a

          ! Temporal problem:
          b = sum(matmul( matmul(transpose(Si),Kss), Si ))
          Ti = transpose( matmul( transpose(Si) , F ) )/b

          ! Stagnation of the PGD:
          snorm = norm2(Si)
          Si = Si/snorm; Ti = Ti*snorm
          stagn = abs( norm2(Ti-Ta)/norm2(Ta) )
          Ta = Ti

          !print*, "STAGN" ,stagn

        end do ! end while PGD calculation

        ! Update of the PGD and external excitation:

        if (PGDm .eq. 1) then

          allocate(Sl(ns,PGDm)); allocate(Tl(I%Tngpt,PGDm))
          allocate(S(ns,PGDm)); allocate(T(I%Tngpt,PGDm))

          Sl = Si; Tl = Ti
          S = Si; T = Ti

        else

          deallocate(Sl); deallocate(Tl)
          allocate(Sl(ns,PGDm-1)); allocate(Tl(I%Tngpt,PGDm-1))
          Sl = S; Tl = T

          deallocate(S); deallocate(T)
          allocate(S(ns,PGDm)); allocate(T(I%Tngpt,PGDm))
          S(:,1:PGDm-1) = Sl; T(:,1:PGDm-1) = Tl
          S(:,PGDm) = Si(:,1); T(:,PGDm) = Ti(:,1)

        end if

        ! PGD indicator:
        error_PGD = 100*norm2( matmul( matmul( Kss,S ), transpose(T) )  + Fud)/Fud_norm ! [%]
        print*, "Error PGD", error_PGD

      end do ! end while PGD process

      Us = matmul( S,transpose(T) )

    end if ! end solution choices

    U(Sop%dof_free,:) = Us
    eps = matmul(Sop%B,U)
    sigma = reshape(matmul(Sop%He, reshape( eps, (/Sop%comp, size(eps)/Sop%comp/))), (/size(eps,1),I%Tngpt/) )

    ! Assignation of the final solution:
    siz = (/Sop%comp,I%ngps,I%Nx,I%Tngpt/)
    Elast%U = U
    allocate(Elast%eps(siz(1),siz(2),siz(3),siz(4)))
    allocate(Elast%sigma(siz(1),siz(2),siz(3),siz(4)))

    Elast%eps = reshape(eps, siz )    !matmul(Sop%B,Elast%U) ! Sop%B*Elast%U
    Elast%sigma = reshape(sigma, siz )  !matmul(Sop%He,Elast%eps)  ! Sop%He*Elast%eps

    Elast%Mss = Mss ! mass matrix
    Elast%Kss = Kss ! stiffness matrix
    Elast%ChKss = ChKss ! stiffness matrix

    print*, "ELASTIC SOLUTION DONE, ERROR:", 100*norm2( matmul(Kss,Us) + Fud )/norm2(Fud) , "[%]."

  end subroutine solve_init_PGD

end module Elastic_sol_PGD


module voronoi

  use sortlib, only: argsort

  implicit none

  public  :: voro_wsc, convex_hull, circumcircle, circumsphere
  private :: voro_wscpoints, det3

  integer,                                       private :: npoints
  double precision, dimension(:,:), allocatable, private :: point
  double precision, dimension(:),   allocatable, private :: dist

contains

  !--------------------------------------------------------------------!
  !                                                                    !
  !                         Wigner-Seitz cell                          !
  !                                                                    !
  ! The following procedures implement the computation of the Wigner-  !
  ! Seitz cell in three dimensions.  In general they can be used to    !
  ! compute a single voronoi cell around the origin of a periodic grid !
  ! defined by three lattice/bais vectors (e.g. also Brillouin zone).  !
  !--------------------------------------------------------------------!

  subroutine voro_wsc(avec, wscpnt, npnts)

    implicit none

    integer,                              intent(inout) :: npnts
    double precision, dimension(3,3),     intent(in)    :: avec
    double precision, dimension(3,npnts), intent(out)   :: wscpnt

    double precision, dimension(3), parameter :: O = (/0.0d0, 0.0d0, 0.0d0/)
    double precision :: x0, y0, z0, r, r2, dx, dy, dz, dist2
    integer          :: ipt1, ipt2, ipt3, ipt4
    logical          :: accept
    double precision :: rmax

    if (.not. allocated(point)) then
       call voro_wscpoints(avec)
    end if

    if (npnts < npoints) then
       npnts = -npoints
       return
    end if

    rmax = sqrt(dist(npoints))

    npnts = 0
    do ipt1 = 1, npoints
    do ipt2 = ipt1+1, npoints
    do ipt3 = ipt2+1, npoints

       call circumsphere( point(1:3,ipt3), &
                          point(1:3,ipt2), &
                          point(1:3,ipt1), &
                          O, x0, y0, z0, r )
       if ((r < 0.0d0) .or. (r > rmax)) cycle

       ! check if we already have this point:
       accept = .true.
       check1 : do ipt4 = 1, npnts
          dx = wscpnt(1,ipt4) - x0
          dy = wscpnt(2,ipt4) - y0
          dz = wscpnt(3,ipt4) - z0
          dist2 = dx*dx + dy*dy + dz*dz
          if (dist2 < 1.0d-10) then
             accept = .false.
             exit check1
          end if
       end do check1
       if (.not. accept) cycle

       ! check if other sites are within the sphere:
       r2 = 0.99999d0*r*r
       check2 : do ipt4 = 1, npoints
          dx = point(1,ipt4) - x0
          dy = point(2,ipt4) - y0
          dz = point(3,ipt4) - z0
          dist2 = dx*dx + dy*dy + dz*dz
          if (dist2 < r2) then
             accept = .false.
             exit check2
          end if
       end do check2

       if (accept) then
          npnts = npnts + 1
          wscpnt(1, npnts) = x0
          wscpnt(2, npnts) = y0
          wscpnt(3, npnts) = z0
       end if

    end do
    end do
    end do

    if (allocated(point)) then
       deallocate(point, dist)
    end if

  end subroutine voro_wsc

  !--------------------------------------------------------------------!
  ! determine all points that might be necessary to compute the        !
  ! Wigner-Seitz cell of the grid defined by the grid vectors avec     !
  !--------------------------------------------------------------------!

  subroutine voro_wscpoints(avec)

    implicit none

    !------------------------------------------------------------------!
    ! avec(i,j)  : i-th component of the j-th lattice vector           !
    !------------------------------------------------------------------!

    double precision, dimension(3,3),           intent(in) :: avec

    integer,                                    parameter  :: memunit = 256

    integer                                                :: bufsize
    double precision, dimension(:,:), allocatable, target  :: buf1, buf2
    double precision, dimension(:,:),              pointer :: buffer
    integer,          dimension(:),   allocatable          :: idx1
    double precision, dimension(3)                         :: vec1
    double precision                                       :: r2, short, long
    integer                                                :: ia, ib, ic, ip
    logical                                                :: lasta, lastb, lastc

    !-----------------!
    ! compute cut-off !
    !-----------------!

    vec1(:) = avec(:,1) + avec(:,2) + avec(:,3)
    short = sum(vec1*vec1)
    long  = short
    do ia = -1, 1
       do ib = -1, 1
          do ic = -1, 1

             if ((ia == 0) .and. (ib == 0) .and. (ic == 0)) cycle

             vec1(:) = dble(ia)*avec(1:3,1) &
                  + dble(ib)*avec(1:3,2) &
                  + dble(ic)*avec(1:3,3)
             r2 = sum(vec1*vec1)
             if (r2 < short) then
                short = r2
             else if (r2 > long) then
                long = r2
             end if

          end do
       end do
    end do

    !-------------!
    ! open buffer !
    !-------------!

    bufsize = memunit
    allocate(buf1(4,bufsize))
    buffer => buf1

    !------------------------------------!
    ! look for points within the cut-off !
    !------------------------------------!

    npoints = 0
    ia = 0
    iado : do
       lasta = .false.
       ib = 0
       ibdo : do
          lastb = .false.
          ic = 0
          icdo : do
             lastc = .false.

             if ((ia==0) .and. (ib==0) .and. (ic==0)) then
                ic = ic + 1
                cycle
             end if

             if (bufsize <= (npoints + 8)) then
                bufsize = bufsize + memunit
                if (allocated(buf1)) then
                   allocate(buf2(4,bufsize))
                   buf2(:,1:npoints) =  buf1(:,1:npoints)
                   buffer => buf2
                   deallocate(buf1)
                else
                   allocate(buf1(4,bufsize))
                   buf1(:,1:npoints) =  buf2(:,1:npoints)
                   buffer => buf1
                   deallocate(buf2)
                end if
             end if

             vec1(:) = dble(ia)*avec(1:3,1) &
                     + dble(ib)*avec(1:3,2) &
                     + dble(ic)*avec(1:3,3)
             r2 = sum(vec1*vec1)
             if (r2 <= long) then
                lastc = .true.
                npoints = npoints + 1
                buffer(1:3,npoints) = vec1(1:3)
                buffer(4,npoints)   = r2
                npoints = npoints + 1
                buffer(1:3,npoints) = -vec1(1:3)
                buffer(4,npoints)   = r2
             end if

             if (ia /= 0) then
                vec1(:) = dble(-ia)*avec(1:3,1) &
                        + dble(ib)*avec(1:3,2) &
                        + dble(ic)*avec(1:3,3)
                r2 = sum(vec1*vec1)
                if (r2 <= long) then
                   lastc = .true.
                   npoints = npoints + 1
                   buffer(1:3,npoints) = vec1(1:3)
                   buffer(4,npoints)   = r2
                   npoints = npoints + 1
                   buffer(1:3,npoints) = -vec1(1:3)
                   buffer(4,npoints)   = r2
                end if
             end if

             if (ib /= 0) then
                vec1(:) = dble(ia)*avec(1:3,1) &
                        + dble(-ib)*avec(1:3,2) &
                        + dble(ic)*avec(1:3,3)
                r2 = sum(vec1*vec1)
                if (r2 <= long) then
                   lastc = .true.
                   npoints = npoints + 1
                   buffer(1:3,npoints) = vec1(1:3)
                   buffer(4,npoints)   = r2
                   npoints = npoints + 1
                   buffer(1:3,npoints) = -vec1(1:3)
                   buffer(4,npoints)   = r2
                end if
             end if

             if (ic /= 0) then
                vec1(:) = dble(ia)*avec(1:3,1) &
                        + dble(ib)*avec(1:3,2) &
                        + dble(-ic)*avec(1:3,3)
                r2 = sum(vec1*vec1)
                if (r2 <= long) then
                   lastc = .true.
                   npoints = npoints + 1
                   buffer(1:3,npoints) = vec1(1:3)
                   buffer(4,npoints)   = r2
                   npoints = npoints + 1
                   buffer(1:3,npoints) = -vec1(1:3)
                   buffer(4,npoints)   = r2
                end if
             end if

             if (.not. lastc) exit icdo
             lastb = .true.
             ic = ic + 1
          end do icdo
          if (.not. lastb) exit ibdo
          lasta = .true.
          ib = ib + 1
       end do ibdo
       if (.not. lasta) exit iado
       ia = ia + 1
    end do iado

    !-----------------------------------------!
    ! sort points by distance from the origin !
    !-----------------------------------------!

    allocate(point(3,npoints), dist(npoints), idx1(npoints))
    call argsort(buffer(4,1:npoints),idx1)
    do ip = 1, npoints
       point(1:3,ip) = buffer(1:3,idx1(ip))
       dist(ip)      = buffer(4,idx1(ip))
    end do
    if (allocated(buf1)) then
       deallocate(buf1, idx1)
    else
       deallocate(buf2, idx1)
    end if

  end subroutine voro_wscpoints

  !====================================================================!
  !                                                                    !
  !                       Auxilliary procedures                        !
  !                                                                    !
  ! These procedures are used in some of the above routines.  Since    !
  ! they are quite general and might be useful for other applications  !
  ! the following routines are public (at least the main once).        !
  !                                                                    !
  !====================================================================!

  !--------------------------------------------------------------------!
  !                         convex hull in 3d                          !
  !                                                                    !
  ! All nodes of the voronoi cell are points on the convex hull.  This !
  ! general routine also works if points in the node-set are inside of !
  ! the convex hull.                                                   !
  !                                                                    !
  ! Note: this implementation is absolutely inefficient and should be  !
  !       used for small point sets only !                             !
  !--------------------------------------------------------------------!

  subroutine convex_hull(point, npoints, edge, nedges)

    implicit none

    !------------------------------------------------------------------!
    ! point(1:3,i)       : i-th point in the point set                 !
    ! npoints            : total number of points                      !
    ! edge(i,j)          : i-th vertex (i=1,2) of the j-th edge        !
    ! nedges             : in : size of the edge array                 !
    !                      out: total number of edges                  !
    !                                                                  !
    ! If the size of the edge array is unsufficient, the optimal size  !
    ! will be returned as -nedges.                                     !
    !                                                                  !
    !------------------------------------------------------------------!

    integer,                                intent(in)    :: npoints
    integer,                                intent(inout) :: nedges
    double precision, dimension(3,npoints), intent(in)    :: point
    integer,          dimension(2,nedges),  intent(out)   :: edge

    logical, dimension(npoints,npoints)  :: connect
    integer                              :: ncon

    integer                              :: ip1, ip2, ip3, ip4
    double precision, dimension(3)       :: r1, r2, r3, r4
    double precision                     :: V, V0, c
    double precision, dimension(3)       :: r1_x_r2, r1_x_r3
    logical                              :: valid
    integer, dimension(:,:), allocatable :: deledge
    integer                              :: i, ndel

    connect(:,:) = .false.
    ncon         = 0

    ! determine all edges on the convex hull:
    do ip1 = 1, npoints
       do ip2 = ip1+1, npoints
          do ip3 = ip2+1, npoints

             r1(:) = point(:,ip2) - point(:,ip1)
             r2(:) = point(:,ip3) - point(:,ip1)

             V0 = 0.0d0

             valid = .true.
             do ip4 = 1, npoints
                if (any(ip4 == (/ip1,ip2,ip3/))) cycle
                r4 = point(:,ip4) - point(:,ip1)
                V  = det3(r1,r2,r4)
                if (V0 == 0.0d0) V0 = V
                if (V*V0 < 0.0d0) then
                   valid = .false.
                   exit
                end if
             end do

             if (valid) then
                if (.not. connect(ip1, ip2)) then
                   connect(ip1, ip2) = .true.
                   connect(ip2, ip1) = .true.
                   ncon = ncon + 1
                end if
                if (.not. connect(ip1, ip3)) then
                   connect(ip1, ip3) = .true.
                   connect(ip3, ip1) = .true.
                   ncon = ncon + 1
                end if
                if (.not. connect(ip2, ip3)) then
                   connect(ip2, ip3) = .true.
                   connect(ip3, ip2) = .true.
                   ncon = ncon + 1
                end if
             end if

          end do
       end do
    end do

    ! remove redundant edges:
    allocate(deledge(2,ncon))
    ndel = 0
    do ip1 = 1, npoints
       do ip2 = ip1+1, npoints
          if (.not. connect(ip1,ip2)) cycle
          P3 : do ip3 = 1, npoints
             if (any(ip3 == (/ip1, ip2/))) cycle
             if (.not. connect(ip1,ip2)) exit
             if (.not. ( connect(ip1,ip3) &
                 .and.   connect(ip2,ip3) ) ) cycle

             do ip4 = 1, npoints
                if (any(ip4 == (/ip1, ip2, ip3/))) cycle
                if (.not. ( connect(ip1,ip4) &
                    .and.   connect(ip2,ip4) ) ) cycle

                ! At this point we have to triangles (p1,p2,p3) and
                ! (p1,p2,p4). Now check, if p3 and p4 lie in one plane:

                r1(:) = point(:,ip2) - point(:,ip1)
                r2(:) = point(:,ip3) - point(:,ip1)
                r3(:) = point(:,ip4) - point(:,ip1)
                V = det3(r1,r2,r3)
                if (abs(V) < 1.0d-10) then

                   ! now check, if p3 and p4 lie on different sides of
                   ! the (p1,p2) vector:

                   r1_x_r2(1) = r1(2)*r2(3) - r1(3)*r2(2)
                   r1_x_r2(2) = r1(3)*r2(1) - r1(1)*r2(3)
                   r1_x_r2(3) = r1(1)*r2(2) - r1(2)*r2(1)

                   r1_x_r3(1) = r1(2)*r3(3) - r1(3)*r3(2)
                   r1_x_r3(2) = r1(3)*r3(1) - r1(1)*r3(3)
                   r1_x_r3(3) = r1(1)*r3(2) - r1(2)*r3(1)

                   if (r1_x_r2(1) /= 0.0d0) then
                      c = r1_x_r2(1)/r1_x_r3(1)
                   else if (r1_x_r2(2) /= 0.0d0) then
                      c = r1_x_r2(2)/r1_x_r3(2)
                   else if (r1_x_r2(3) /= 0.0d0) then
                      c = r1_x_r2(3)/r1_x_r3(3)
                   else
                      write(0,*) "Warning: check point set in `convex_hull()'."
                      c = 0.0d0
                   end if

                   if (c < 0.0d0) then
                      ndel            = ndel + 1
                      deledge(1,ndel) = ip1
                      deledge(2,ndel) = ip2
                      exit P3
                   end if
                end if
             end do

          end do P3
       end do
    end do
    do i = 1, ndel
       if (connect(deledge(1,i),deledge(2,i))) then
          connect(deledge(1,i),deledge(2,i)) = .false.
          connect(deledge(2,i),deledge(1,i)) = .false.
          ncon = ncon - 1
       end if
    end do
    deallocate(deledge)

    ! return results:
    if (nedges < ncon) then
       nedges = -ncon
    else
       nedges = ncon
       ncon   = 0
       do ip1 = 1, npoints
       do ip2 = ip1+1, npoints
          if (connect(ip1,ip2)) then
             ncon = ncon + 1
             edge(1,ncon) = ip1
             edge(2,ncon) = ip2
          end if
       end do
       end do
    end if

  end subroutine convex_hull

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

  function det3(P1, P2, P3) result(d)

    implicit none

    double precision, dimension(3), intent(in) :: P1, P2, P3
    double precision :: d

    d = P1(1)*P2(2)*P3(3) + P1(3)*P2(1)*P3(2) + P1(2)*P2(3)*P3(1) &
      - P1(3)*P2(2)*P3(1) - P1(1)*P2(3)*P3(2) - P1(2)*P2(1)*P3(3)

  end function det3

  !--------------------------------------------------------------------!
  !                    circumcircle / circumsphere                     !
  !                                                                    !
  ! The nodes of the voronoi cell are the centers of circumcircles in  !
  ! 2d, and the centers of circumspheres in 3d, of adjacent points.    !
  !--------------------------------------------------------------------!

  subroutine circumcircle(p1, p2, p3, x0, y0, r)

    implicit none

    double precision, dimension(2), intent(in)  :: p1, p2, p3
    double precision,               intent(out) :: x0, y0, r

    double precision :: xy1, xy2, xy3, d21, d31, d32, dx, dy
    double precision :: f

    f  = p1(2)*(p3(1)-p2(1)) + p2(2)*(p1(1)-p3(1)) + p3(2)*(p2(1)-p1(1))
    if (abs(f) <= 1.0d-15) then
       ! the points don't lie on a circle (since they are all on one line)
       ! --> return negative radius
       r = -1.0d0
       return
    end if

    f  = 0.5d0 / f

    xy1 = p1(1)*p1(1) + p1(2)*p1(2)
    xy2 = p2(1)*p2(1) + p2(2)*p2(2)
    xy3 = p3(1)*p3(1) + p3(2)*p3(2)

    d21 = xy2 - xy1
    d31 = xy3 - xy1
    d32 = xy3 - xy2

    x0 =  f*(p1(2)*d32 - p2(2)*d31 + p3(2)*d21)
    y0 = -f*(p1(1)*d32 - p2(1)*d31 + p3(1)*d21)

    dx = p1(1) - x0
    dy = p1(2) - y0
    r  = sqrt(dx*dx + dy*dy)

  end subroutine circumcircle

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

  subroutine circumsphere(p1,p2,p3,p4,x0,y0,z0,r)

    implicit none

    double precision, dimension(3), intent(in)  :: p1, p2, p3, p4
    double precision,               intent(out) :: x0, y0, z0, r

    double precision :: xyz1, xyz2, xyz3, xyz4
    double precision :: d21, d31, d41, d32, d42, d43
    double precision :: dx, dy, dz
    double precision :: f

    f = p1(3)*(p2(2)*(p4(1)-p3(1)) - p3(2)*(p4(1)-p2(1)) + p4(2)*(p3(1)-p2(1))) &
      - p2(3)*(p1(2)*(p4(1)-p3(1)) - p3(2)*(p4(1)-p1(1)) + p4(2)*(p3(1)-p1(1))) &
      + p3(3)*(p1(2)*(p4(1)-p2(1)) - p2(2)*(p4(1)-p1(1)) + p4(2)*(p2(1)-p1(1))) &
      - p4(3)*(p1(2)*(p3(1)-p2(1)) - p2(2)*(p3(1)-p1(1)) + p3(2)*(p2(1)-p1(1)))

    if (abs(f) <= 1.0d-15) then
       ! the points don't lie on a circle (since they are all on one line)
       ! --> return negative radius
       r = -1.0d0
       return
    end if

    f = 0.5d0 / f

    xyz1 = p1(1)*p1(1) + p1(2)*p1(2) + p1(3)*p1(3)
    xyz2 = p2(1)*p2(1) + p2(2)*p2(2) + p2(3)*p2(3)
    xyz3 = p3(1)*p3(1) + p3(2)*p3(2) + p3(3)*p3(3)
    xyz4 = p4(1)*p4(1) + p4(2)*p4(2) + p4(3)*p4(3)

    d21  = xyz2 - xyz1
    d31  = xyz3 - xyz1
    d41  = xyz4 - xyz1
    d32  = xyz3 - xyz2
    d42  = xyz4 - xyz2
    d43  = xyz4 - xyz3

    x0   =  f*( p1(3)*(p2(2)*d43 - p3(2)*d42 + p4(2)*d32) &
              - p2(3)*(p1(2)*d43 - p3(2)*d41 + p4(2)*d31) &
              + p3(3)*(p1(2)*d42 - p2(2)*d41 + p4(2)*d21) &
              - p4(3)*(p1(2)*d32 - p2(2)*d31 + p3(2)*d21) )

    y0   = -f*( p1(3)*(p2(1)*d43 - p3(1)*d42 + p4(1)*d32) &
              - p2(3)*(p1(1)*d43 - p3(1)*d41 + p4(1)*d31) &
              + p3(3)*(p1(1)*d42 - p2(1)*d41 + p4(1)*d21) &
              - p4(3)*(p1(1)*d32 - p2(1)*d31 + p3(1)*d21) )

    z0   =  f*( p1(2)*(p2(1)*d43 - p3(1)*d42 + p4(1)*d32) &
              - p2(2)*(p1(1)*d43 - p3(1)*d41 + p4(1)*d31) &
              + p3(2)*(p1(1)*d42 - p2(1)*d41 + p4(1)*d21) &
              - p4(2)*(p1(1)*d32 - p2(1)*d31 + p3(1)*d21) )

    dx = p1(1) - x0
    dy = p1(2) - y0
    dz = p1(3) - z0
    r = sqrt(dx*dx + dy*dy + dz*dz)

  end subroutine circumsphere

end module voronoi


!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
!   FEM Navier Stokes fluid solver
!     compressible, implicit
! Latest update: Jack Yang, 09/02/2014
!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
if (myid == 0) then
    write(*,*) "*** Solving Fluids: calculate fluid field ***"
endif
!...store the variables from the previous time step,dold=d
dold = d
qvold = qv
!----------------------------------------------------
call formid(id,rng,ien)
call formd_time(d,rng,ien)
!call formd(d,rng,ien)
if (myid == 0) write(*,*) "********** Applied Time Changing BC ***************"
!===========================================================================
! Applying natural(pressure) BC
if (edge_inflow .ne. 0) then
    if (myid == 0) then
        write (*,*) '********** Apply natural BC ***********'
        if (ptotflag == 1) write(*,*) '======== Total Pressure B.C. is used ==========='
    endif
    if (nsd == 2) then
        if (ptotflag == 0) then
            call nature_pre(x,dold,ien,rng,bc4el,ne_inflow,edge_inflow,pin,res_bc)
        else
            call nature_totpre(x,dold,ien,rng,bc4el,ne_inflow,edge_inflow,pin,res_bc,pin_s)
            call nature_pre(x,dold,ien,rng,bc4el,ne_inflow,edge_inflow,pin_s,res_bc)
        endif

        f_fluids(1,:) =  f_fluids(1,:) + res_bc(1,:)
    elseif (nsd == 3) then
        if (ptotflag == 0) then
            call nature_pre_3d(x,dold,ien,rng,bc4el,ne_inflow,edge_inflow,pin,res_bc)
        else
            call nature_totpre_3d(x,dold,ien,rng,bc4el,ne_inflow,edge_inflow,pin,res_bc,pin_s)
            call nature_pre_3d(x,dold,ien,rng,bc4el,ne_inflow,edge_inflow,pin_s,res_bc)
        endif
        f_fluids(1:nsd,:) =  f_fluids(1:nsd,:) + res_bc(1:nsd,:)
    endif
endif
!===========================================================================
include "applySpecBodyForce.fi"
!===========================================================================
do iit=1,nit
    p(:,:) = 0.0d0
    w(:,:) = 0.0d0
    time_com=mpi_wtime()
    call block(x,d,dold,p,w,hg,ien,f_fluids,rng,f_stress,ne_intlocal,ien_intlocal,node_local,nn_local,&
               fden,fvis,I_fluid)
    time_com=mpi_wtime()-time_com
    !if (myid == 0) write(*,*) 'Time for evaluate block', time_com

    time_com=mpi_wtime()
    call communicate_respml_ad_sub(p,ndf,nn,send_address,ad_length)
    call communicate_respml_ad_sub(w,ndf,nn,send_address,ad_length)
    time_com=mpi_wtime()-time_com
    !if (myid == 0) write(*,*) 'Time for communication twice', time_com

    !...apply boundary conditions, calculate residual with corrected BC, i.e. where id=0 -> p=0
    time_com=mpi_wtime()
    call setidpml_pa(p,ndf,nn,nn_PML,id,node_local,nn_local)
    time_com=mpi_wtime()-time_com
    !if (myid == 0) write(*,*) 'Time for set BC', time_com

    !...calculate the normalized error, res_l
    call getnormpml_pa(p,ndf,nn,node_local,nn_local,res_l)
    res_l= sqrt(res_l)
    !...calculate the increment(delta_d) for each degree of freedom, dg
    dg(:,:) = 0.0d0
    call gmres(x,d,dold,w,p,dg,hg,ien,f_fluids,id, &
               ne_intlocal,ien_intlocal,node_local,nn_local, &
               global_com,nn_global_com,local_com,nn_local_com,send_address,ad_length,&
               fden,fvis,I_fluid,rng)
    !...calculate the normalized increment, del_l
    call getnormpml_pa(dg,ndf,nn,node_local,nn_local,del_l)
    del_l = sqrt(del_l)
    !...update the calculated variables, d=d+dg
    call mpi_barrier(mpi_comm_world,ierror)
    call updatePML(p, d, dg, ndf)

    if (myid == 0) then
        !...output the errors in each iteration
        !write(6,102) iit,res_l,del_l
        !write(7,102) iit,res_l,del_l
        write(*,1023) iit, res_l, del_l
    endif
enddo ! end of loop over iterations

if (myid ==0) then
    write(*,'("maximum fluid velocity component = ",f13.6)') maxval( d(1:nsd,1:nn) )
    write(*,'("maximum PML qv component = ",f13.6)') maxval( qv(1:nsd,1:nn) )
    call blockcvoutput(x,d,dold,p,hg,ien,f_fluids,ne_intlocal,ien_intlocal,node_local,nn_local,fden,fvis,I_fluid,I_fluid_old,rng,&
                       node_sbc,solid_coor_curr)
endif


C$Procedure      DNEARP ( Derivative of near point )

      SUBROUTINE DNEARP ( STATE, A, B, C, DNEAR, DALT, FOUND )

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

      IMPLICIT NONE
      DOUBLE PRECISION      STATE ( 6 )
      DOUBLE PRECISION      A
      DOUBLE PRECISION      B
      DOUBLE PRECISION      C
      DOUBLE PRECISION      DNEAR ( 6 )
      DOUBLE PRECISION      DALT  ( 2 )
      LOGICAL               FOUND

C$ Brief_I/O
C
C     VARIABLE  I/O  DESCRIPTION
C     --------  ---  --------------------------------------------------
C     STATE      I   State of an object in body-fixed coordinates.
C     A          I   Length of semi-axis parallel to x-axis.
C     B          I   Length of semi-axis parallel to y-axis.
C     C          I   Length on semi-axis parallel to z-axis.
C     DNEAR      O   State of the nearest point on the ellipsoid.
C     DALT       O   Altitude and derivative of altitude.
C     FOUND      O   Tells whether DNEAR is degenerate.
C
C$ Detailed_Input
C
C     STATE      is a 6-vector giving the position and velocity of
C                some object in the body-fixed coordinates of the
C                ellipsoid.
C
C                In body-fixed coordinates, the semi-axes of the
C                ellipsoid are aligned with the x, y, and z-axes of the
C                coordinate system.
C
C     A          is the length of the semi-axis of the ellipsoid
C                that is parallel to the x-axis of the body-fixed
C                coordinate system.
C
C     B          is the length of the semi-axis of the ellipsoid
C                that is parallel to the y-axis of the body-fixed
C                coordinate system.
C
C     C          is the length of the semi-axis of the ellipsoid
C                that is parallel to the z-axis of the body-fixed
C                coordinate system.
C
C$ Detailed_Output
C
C
C     DNEAR      is the 6-vector giving the position and velocity
C                in body-fixed coordinates of the point on the
C                ellipsoid, closest to the object whose position
C                and velocity are represented by STATE.
C
C                While the position component of DNEAR is always
C                meaningful, the velocity component of DNEAR will be
C                meaningless if FOUND if .FALSE.  (See the discussion
C                of the meaning of FOUND below.)
C
C
C     DALT       is an array of two double precision numbers.  The
C                first gives the altitude of STATE with respect to
C                the ellipsoid.  The second gives the rate of
C                change of the altitude.
C
C                Note that the rate of change of altitude is meaningful
C                if and only if FOUND is .TRUE.  (See the discussion of
C                the meaning of FOUND below.)
C
C     FOUND      is a logical flag indicating whether or not the
C                velocity portion of DNEAR is meaningful.
C                If the velocity portion of DNEAR is meaningful
C                FOUND will be returned with a value of .TRUE.
C                Under very rare circumstance the velocity of the
C                near point is undefined.  Under these circumstances
C                FOUND will be returned with the value .FALSE.
C
C                FOUND can be .FALSE. only for states whose position
C                components are inside the ellipsoid and then only at
C                points on a special surface contained inside the
C                ellipsoid called the focal set of the ellipsoid.
C
C                A point in the interior is on this special surface
C                only if there are two or more points on the ellipsoid
C                that are closest to it.  The origin is such a point
C                and the only such point if the ellipsoid is a
C                sphere.  For non-spheroidal ellipsoids the focal
C                set contains small portions of the planes of
C                symmetry of the ellipsoid.
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C
C     1) If the axes are non-positive, a routine in the call tree
C        of this routine will diagnose the error.
C
C     2) If an object is passing through the interior of an ellipsoid
C        there are points at which there is more than 1 point on
C        the ellipsoid that is closest to the object.  At these
C        points the velocity of the near point is undefined. (See
C        the description of the output variable FOUND).
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     If an object is moving relative to some triaxial body along
C     a trajectory C(t) then there is a companion trajectory N(t)
C     that gives the point on the ellipsoid that is closest to
C     C(t) as a function of t.  The instantaneous position and
C     velocity of C(t) (STATE) are sufficient to compute the
C     instantaneous position and velocity of N(t) (DNEAR).
C
C     This routine computes DNEAR from STATE.  In addition it returns
C     the altitude and rate of change of altitude.
C
C     Note that this routine can compute DNEAR for STATES outside,
C     on, or inside the ellipsoid.  However, the velocity of DNEAR
C     and derivative of altitude do not exist for a "small" set
C     of STATES  in the interior of the ellipsoid. See the
C     discussion of FOUND above for a description of this set of
C     points.
C
C$ Examples
C
C     Example 1.  Speed of a ground track.
C     =======================================
C
C     Suppose you wish to compute the velocity of the ground track
C     of a satellite as it passes over a location on the earth
C     and that the moment of passage (ET) has been previously
C     determined.  (We assume that the spacecraft is close enough
C     to the surface that light time corrections do not matter.)
C
C     We let
C
C        BODY    be the idcode for the body
C        FRAME   be the string representing the body's body-fixed frame
C        SCID    be the idcode of the spacecraft
C
C     First get the axes of the body.
C
C        CALL BODVCD ( BODY, 'RADII', 3, DIM, ABC  )
C
C        A = ABC(1)
C        B = ABC(2)
C        C = ABC(3)
C
C        CALL SPKEZ  ( SCID,  ET,   FRAME,   'NONE', BODY, STATE, LT )
C        CALL DNEARP ( STATE, A, B, C, DNEAR, DALT )
C
C     DNEAR contains the state of the subspacecraft point.
C
C
C     Example 2. Doppler shift of an altimeter.
C     =========================================
C
C     Suppose you wish to compute the one-way doppler shift of a radar
C     altimeter mounted on board a spacecraft as it passes
C     over some region.  Moreover, assume that for your
C     purposes it is sufficient to neglect effects of atmosphere,
C     topography and antenna pattern for the sake of this
C     computation.  We use the same notation as in the previous example.
C
C     First get the axes of the body.
C
C        CALL BODVCD ( BODY, 'RADII', 3, DIM, ABC  )
C
C        A = ABC(1)
C        B = ABC(2)
C        C = ABC(3)
C
C        CALL SPKEZ  ( SCID,  ET,   FRAME,   'NONE', BODY, STATE, LT )
C        CALL DNEARP ( STATE, A, B, C, DNEAR, DALT )
C
C
C     The change in frequency is given by multiplying SHIFT times the
C     carrier frequency
C
C        SHIFT = ( DALT(2) / CLIGHT() )
C
C$ Restrictions
C
C     None.
C
C$ Literature_References
C
C     None.
C
C$ Author_and_Institution
C
C     N.J. Bachman    (JPL)
C     W.L. Taber      (JPL)
C
C$ Version
C
C-    SPICELIB Version 1.1.2, 26-JUN-2008 (NJB)
C
C        Corrected spelling error in abstract; re-wrote
C        abstract text.
C
C-    SPICELIB Version 1.1.1, 24-OCT-2005 (NJB)
C
C        Header update:  changed references to BODVAR to references
C        to BODVCD.
C
C-    SPICELIB Version 1.1.0, 05-MAR-1998 (WLT)
C
C        In the previous version of the routine FOUND could be
C        returned without being set to TRUE when the velocity
C        of the near point and rate of change of altitude
C        could be determined.  This error has been corrected.
C
C-    SPICELIB Version 1.0.0, 15-JUN-1995 (WLT)
C
C
C-&

C$ Index_Entries
C
C     Velocity of the nearest point on an ellipsoid
C     Rate of change of the altitude over an ellipsoid
C     Derivative of altitude over an ellipoid
C     Velocity of a ground track
C
C-&
C
C     Spicelib functions
C
      LOGICAL               RETURN
      LOGICAL               FAILED

      DOUBLE PRECISION      VDOT
      DOUBLE PRECISION      VTMV

C
C     Local Variables
C
      DOUBLE PRECISION      DENOM
      DOUBLE PRECISION      DTERM  ( 3 )
      DOUBLE PRECISION      GRAD   ( 3 )
      DOUBLE PRECISION      L
      DOUBLE PRECISION      LENGTH
      DOUBLE PRECISION      LPRIME
      DOUBLE PRECISION      NORML  ( 3 )
      DOUBLE PRECISION      TEMP   ( 3 )
      DOUBLE PRECISION      ZENITH ( 3 )

      DOUBLE PRECISION      GRADM  ( 3, 3 )
      DOUBLE PRECISION      M      ( 3, 3 )

      INTEGER               I
C
C     Saved Variables
C
      SAVE                  GRADM
      SAVE                  M

C
C     Initial Values
C
      DATA   GRADM / 1.0D0, 0.0D0, 0.0D0,
     .               0.0D0, 1.0D0, 0.0D0,
     .               0.0D0, 0.0D0, 1.0D0  /

      DATA   M     / 1.0D0, 0.0D0, 0.0D0,
     .               0.0D0, 1.0D0, 0.0D0,
     .               0.0D0, 0.0D0, 1.0D0  /
C
C     Standard SPICE error handling.
C
      IF ( RETURN() ) THEN
         RETURN
      END IF

      CALL CHKIN ( 'DNEARP' )
C
C     Until we have reason to believe otherwise, we set FOUND to TRUE.
C
      FOUND = .TRUE.

C
C     First we need to compute the near point.
C
      CALL NEARPT ( STATE, A, B, C, DNEAR, DALT )

C
C     Make sure nothing went bump in the dark innards of NEARPT.
C
      IF ( FAILED() ) THEN
         FOUND = .FALSE.
         CALL CHKOUT ( 'DNEARP' )
         RETURN
      END IF

C
C     Now for the work of this routine.  We need to compute the
C     velocity component of DNEAR.
C
C     In all of the discussions below we let <,> stand for the
C     dot product.
C
C     Let P be the position (first three components) of STATE
C     and let N be the position (first three components) of DNEAR.
C
C     The surface of the ellipsoid is described as the level set
C     f(x,y,z) = 1 for the function f defined by
C
C         f(x,y,z) = x**2/a**2 + y**2/b**2 + z**2/c**2
C
C     Let GRAD be the "half" gradiant of f. Then for some L
C
C
C           N + L * GRAD = P                         ( 1 )
C
C
C     So that
C                < P - N, GRAD >
C           L =  --------------
C                < GRAD , GRAD >
C
C                          GRAD
C             =  < P - N, ------ >  /  | GRAD |
C                         |GRAD|
C
C     Since GRAD is computed at a point on the level set f(x,y,z) = 1
C     we don't have to worry about the magnitude of |GRAD| being
C     so small that underflow can occur.
C
C     Note that the half gradiant of f  can be computed by simple
C     vector multiplication
C
C                       [ 1/A**2    0       0    ] [ x ]
C        GRAD(x,y,z)  = |   0     1/B**2    0    | | y |
C                       [   0       0     1/C**2 ] [ z ]
C
C     We call the matrix above GRADM.  The correct off
C     diagonal values have been established in the data statement
C     following the declaration section of this routine.
C

      GRADM(1,1) = 1.0D0/(A*A)
      GRADM(2,2) = 1.0D0/(B*B)
      GRADM(3,3) = 1.0D0/(C*C)

      CALL VSUB  ( STATE,  DNEAR, ZENITH )

      CALL MXV   ( GRADM,  DNEAR,    GRAD   )
      CALL UNORM ( GRAD,   NORML,    LENGTH )

      L  = VDOT  ( ZENITH, NORML ) / LENGTH

C
C     We can rewrite equation (1) as
C
C        P = N + L * GRADM * N
C
C     from this it follows that
C
C        P' =  N' + L' * GRADM * N
C                 + L  * GRADM * N'
C
C           = ( IDENT + L*GRADM ) * N'   + L' * GRADM * N
C
C           = ( IDENT + L*GRADM ) * N'   + L' * GRAD
C
C     where IDENT is the 3x3 identity matrix.
C
C     Let M be the inverse of the matrix IDENT + L*GRADM. (Provided
C     of course that all of the diagonal entries are non-zero).
C
C     If we multiply both sides of the equation above by M
C     we have
C
C
C        M*P'  = N'  + L'* M * GRAD                      ( 2 )
C
C
C     Recall now that N' is orthogonal to GRAD (N' lies in the
C     tangent plane to the ellipsoid at N and GRAD is normal
C     to this tangent plane).  Thus
C
C        < GRAD, M*P' > = L' < GRAD, M * GRAD >
C
C     and
C
C                 < GRAD, M*P'   >
C        L'   =   -----------------
C                 < GRAD, M*GRAD >
C
C
C             =   VTMV ( GRAD, M, P' ) / VTMV ( GRAD, M, GRAD )
C
C     Let's pause now to compute M and L'.
C
C        This is where things could go bad.  M might not exist (which
C        indicates STATE is on the focal set of the ellipsoid).  In
C        addition it is conceivable that VTMV ( GRAD, M, GRAD ) is
C        zero.  This turns out not to be possible.  However, the
C        demonstration of this fact requires delving into the details
C        of how N was computed by NEARPT.  Rather than spending a
C        lot of time explaining the details we will make an
C        unnecessary but inexpensive check that we don't divide by
C        zero when computing L'.
C
      DO I = 1, 3
         DTERM(I) = 1.0D0 + L*GRADM(I,I)
      END DO

      DO I = 1, 3

         IF ( DTERM(I) .NE. 0.0D0 ) THEN
            M(I,I) = 1.0D0 / DTERM(I)
         ELSE
            FOUND = .FALSE.
            CALL CHKOUT ( 'DNEARP' )
            RETURN
         END IF

      END DO

      DENOM = VTMV ( GRAD, M, GRAD )

      IF ( DENOM .EQ. 0.0D0 ) THEN
         FOUND = .FALSE.
         CALL CHKOUT ( 'DNEARP' )
         RETURN
      END IF

      LPRIME = VTMV( GRAD, M, STATE(4) ) / DENOM

C
C     Now that we have L' we can easily compute N'. Rewriting
C     equation (2) from above we have.
C
C        N'  = M * ( P' - L'*GRAD )
C
      CALL VLCOM ( 1.0D0, STATE(4), -LPRIME, GRAD, TEMP     )
      CALL MXV   ( M,     TEMP,                    DNEAR(4) )

C
C     Only one thing left to do.  Compute the derivative
C     of the altitude ALT.  Recall that
C
C                              GRAD
C        ALT     = < P  -  N, ------ >
C                             |GRAD|
C
C                             GRAD
C        dALT/dt = < P' - N', ------ >
C                             |GRAD|
C
C                                        GRAD
C                 + < P  -  N, Deriv of{------} >
C                                       |GRAD|
C
C     The second term is zero.  To see this note that P - N is parallel
C     to GRAD.  Moreover, since GRAD/|GRAD| is a unit vector its
C     derivative is necessarily orthogonal to it.  Hence it is
C     orthogonal to GRAD and P-N.
C
C     Thus
C                              GRAD
C        dALT/dt = < P' - N', ------ >
C                             |GRAD|
C
C     But as we discussed earlier N' is orthogonal to GRAD.  Thus
C
C                          GRAD
C        dALT/dt = < P' , ------ >
C                         |GRAD|
C
C     We've already computed GRAD/|GRAD| (NORML). Hence
C
C        dALT/dt = < P', NORML >
C
      DALT(2) = VDOT ( STATE(4), NORML )

      CALL CHKOUT ( 'DNEARP' )
      RETURN
      END


      SUBROUTINE FVRS1C (Z,W1,OMEGA,NF)
C----------------------------------------------------------------------
      COMPLEX Z(3,NF),P,Z1,PY1A,PY1B,PZ1A,PZ1B,PY2A,PY2B,PZ2A,PZ2B
C
      DIMENSION W1(NF)
C
      COMMON /CONDAS/ PI,TWOPI,RADEG,DEGRA,S4PIQ
      COMMON /BLANK / DUM(5),IXT,IXP,IYT,IYP,IZT,IZP,DUM1(3)
C----------------------------------------------------------------------
      LL=1
      DO 400 KKK=1,NF
      IF(W1(KKK).EQ.0.0)GO TO 30
      A=1.0
      IF(W1(KKK)-OMEGA.LT.0.0)A=-1.0
      B=1.0
      IF(W1(KKK)+OMEGA.LT.0.0)B=-1.0
C
C     COMPUTE BASE(FI)(3X3)  IF W.NE.0--MODFRL=TRUE
C
C     ZERO OUT MATRIX
C
      KK=LL+2
      DO 1 I=1,3
      DO 1 J=LL,KK
  1   Z(I,J)=(0.0,0.0)
      F=W1(KKK)/TWOPI
      IF(IXT.EQ.-1) GO TO 9
      CALL TAB(IXT,F,XO)
      IF(IXP.EQ.-1)GO TO 4
      CALL TAB(IXP,F,PHI)
      RAD=PHI*DEGRA
      Z1=CMPLX(0.0,RAD)
      P=CEXP(Z1)
      GO TO 5
 4    P=(1.0,0.0)
 5    Z(1,LL+1)=XO*P
 9    IF(IYT.EQ.-1)GO TO 11
      CALL TAB(IYT,F,YO)
      GO TO 12
 11   YO=0.0
 12   IF(IZT.EQ.-1)GO TO 13
      CALL TAB(IZT,F,ZO)
      GO TO 14
 13   ZO=0.0
 14   IF(IYP.EQ.-1)GO TO 15
      CALL TAB(IYP,F,PHI)
      GO TO 16
 15   PHI=0.0
 16   RAD=PHI*DEGRA
      Z1=CMPLX(0.0,RAD)
      PY1A=CEXP(A*Z1)
      PY1B=CEXP(B*Z1)
      Z1=CMPLX(0.0,RAD-0.5*PI*A)
      PY2A=CEXP(A*Z1)
      Z1=CMPLX(0.0,RAD-0.5*PI*B)
      PY2B=CEXP(B*Z1)
      IF(IZP.EQ.-1)GO TO 17
      CALL TAB(IZP,F,PHI)
      GO TO 18
 17   PHI=0.0
 18   RAD=PHI*DEGRA
      Z1=CMPLX(0.0,RAD)
      PZ1A=CEXP(A*Z1)
      PZ1B=CEXP(B*Z1)
      Z1=CMPLX(0.0,RAD-0.5*PI*A)
      PZ2A=CEXP(A*Z1)
      Z1=CMPLX(0.0,RAD-0.5*PI*B)
      PZ2B=CEXP(B*Z1)
      Z(2,LL)=(YO*PY1A-A*ZO*PZ2A)*0.5
      Z(3,LL)=(A*YO*PY2A+ZO*PZ1A)*0.5
      Z(2,LL+2)=(YO*PY1B+B*ZO*PZ2B)*0.5
      Z(3,LL+2)=(-B*YO*PY2B+ZO*PZ1B)*0.5
      LL=LL+3
      GO TO 400
 30   CONTINUE
C
C     COMPUTE BASE(FI)(3X2) IF W1=0.0, FOR MODFRL=TRUE
C
      A=1.0
      IF(OMEGA.LT.0.0)A=-1.0
C------ZERO OUT MATRIX(3X2)
      KK=LL+1
      DO 32 I=1,3
      DO 32 J=LL,KK
 32   Z(I,J)=(0.0,0.0)
      F=W1(KKK)/TWOPI
      IF(IXT.EQ.-1)GO TO 90
      CALL TAB(IXT,F,XO)
      IF(IXP.EQ.-1)GO TO 40
      CALL TAB(IXP,F,PHI)
      RAD=PHI*DEGRA
      Z1=CMPLX(0.0,RAD)
      P=CEXP(Z1)
      GO TO 50
 40   P=(1.0,0.0)
 50   Z(1,LL)=XO*P
      GO TO 100
 90   Z(1,LL)=(0.0,0.0)
 100  IF(IYT.EQ.-1)GO TO 190
      CALL TAB(IYT,F,YO)
      IF(IYP.EQ.-1) GO TO 140
      CALL TAB(IYP,F,PHI)
      RAD=PHI*DEGRA
      CY=COS(RAD)
      GO TO 150
 140  CY=1.0
 150  YY=YO*CY
      GO TO 200
 190  YY=0.0
 200  IF(IZT.EQ.-1)GO TO 290
      CALL TAB(IZT,F,ZO)
      IF(IZP.EQ.-1) GO TO 240
      CALL TAB(IZP,F,PHI)
      RAD=PHI*DEGRA
      CZ=COS(RAD)
      GO TO 250
 240  CZ=1.0
 250  ZZ=ZO*CZ
      GO TO 300
 290  ZZ=0.0
 300  Z(2,KK)=YY-A*CMPLX(0.0,ZZ)
      Z(3,KK)=ZZ+A*CMPLX(0.0,YY)
      LL=LL+2
 400  CONTINUE
      RETURN
      END


!-----------------------------------------------------------------------
!     Function:      fieldlines_find_hc
!     Authors:       S. Lazerson (lazerson@pppl.gov)
!     Date:          01/03/2013
!     Description:   This subroutine finds the homoclinic tangles given
!                    an initial guess for the x-points.
!                         fieldlines_find_hc
!                                  |
!                                E04CCA
!                                  |
!                               faxis_nag
!                                  |
!                              follow_single
!                                  |
!                              ODE Solvers
!                                  |
!                                fblin's
!
!-----------------------------------------------------------------------
      SUBROUTINE fieldlines_find_hc
!-----------------------------------------------------------------------
!     Libraries
!-----------------------------------------------------------------------
      USE stel_kinds, ONLY: rprec
      USE fieldlines_grid
      USE fieldlines_lines
      USE fieldlines_runtime
      IMPLICIT NONE
!-----------------------------------------------------------------------
!     Local Variables
!-----------------------------------------------------------------------
      INTEGER     :: i, j, num_hc, dex
      REAL(rprec) :: r0,z0,r1,z1,rerr,zerr,alpha
      REAL(rprec) :: detq,traq,l1r,l2r,l1i,l2i
      REAL(rprec) :: q4(4),q_map(4),evecr(4),eveci(4)
      REAL(rprec) :: vec1(4),vec2(4)
      REAL(rprec) :: ident(4) = (/1,0,0,1/)
      COMPLEX(8)  :: sqrtval
!-----------------------------------------------------------------------
!     External Functions
!          E04CBF               NAG mimizier
!          E04CBK               NAG Dummy function
!-----------------------------------------------------------------------
      EXTERNAL E04FCF, monit_find_axis, faxis_nag, E04FDZ
!-----------------------------------------------------------------------
!     Begin Subroutine
!-----------------------------------------------------------------------
      alpha = 0.75
      num_hc = COUNT(r_hc > 0) ! Number of homoclines
      j=1
      r_start = -1
      z_start = -1
      phi_start = 0.0
      phi_end   = 0.0
      dex = 0
      WRITE(6,'(A)')      '===========HOMOCLINES=========='
      WRITE(6,'(A, I3)')  '   # OF HOMOCLINES  = ',num_hc
      WRITE(6,'(A, I4)')  '   LINES PER HOMOC. = ',2*num_hcp+1
      WRITE(6,'(A, F8.5)')'   LENGTH OF LINE   = ',2*delta_hc
      DO i = 1, num_hc
         WRITE(6,'(A)')    '-------------------------------'
         WRITE(6,'(A, I3.3)')         '   WORKING ON HOMOCLINE: ',i
         r0 = r_hc(i)
         z0 = z_hc(i)
         WRITE(6,'(A,F8.5,A,F8.5,A)') '      XPOINT0[R,Z]   = [',r0,',',z0,']'
         j=1
         DO
            r1=r0
            z1=z0
            CALL follow_single(r1,0.0,z1,pi2,q4)
            rerr = r0-r1
            zerr = z0-z1
            IF (sqrt(rerr*rerr+zerr*zerr) < (follow_tol*10)) EXIT
            q_map(1) = q4(1)-1.0
            q_map(2) = q4(2)
            q_map(3) = q4(3)
            q_map(4) = q4(4)-1.0
            r0   = r0 + alpha*(q_map(4) * rerr - q_map(2) * zerr) / (q_map(1)*q_map(4)-q_map(2)*q_map(3))
            z0   = z0 + alpha*(-q_map(3) * rerr + q_map(1) * zerr) / (q_map(1)*q_map(4)-q_map(2)*q_map(3))
            j= j+1
            IF (j > 1000) EXIT
         END DO
         IF (j > 1000) WRITE(6,'(A)') 'WARNING: NITER > 1000'
         r0 = r1
         z0 = z1
         WRITE(6,'(A,F8.5,A,F8.5,A)') '      XPOINTF[R,Z]   = [',r0,',',z0,']'
         CALL FLUSH(6)
         ! Now return eigenvalues of q
         ! From http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/index.html
         detq = (q4(1)*q4(4)-q4(2)*q4(3))
         traq = q4(1)+q4(4)
         sqrtval = sqrt(0.25*traq*traq-detq)
         l1r   = 0.5*traq + REAL(sqrtval)
         l1i   =          + AIMAG(sqrtval)
         l2r   = 0.5*traq - REAL(sqrtval)
         l2i   =          - AIMAG(sqrtval)
         WRITE(6,'(A,F8.5,SP,F8.5,A)') '      EIGN. VAL 1    =  ',l1r,l1i,'i '
         WRITE(6,'(A,F8.5,SP,F8.5,A)') '      EIGN. VAL 2    =  ',l2r,l2i,'i '
         ! Now return the eigenvectors
         vec1 = q4-l1r*ident
         vec2 = q4-l2r*ident
         evecr(1) = vec2(2)
         evecr(3) = vec2(4)
         evecr(2) = vec1(1)
         evecr(4) = vec1(3)
         vec1 = q4-l1i*ident
         vec2 = q4-l2i*ident
         eveci(1) = vec2(2)
         eveci(3) = vec2(4)
         eveci(2) = vec1(1)
         eveci(4) = vec1(3)
         WRITE(6,'(A,F8.5,A,F8.5,A)') '      EIGN. VEC 1    = [',evecr(1),',',evecr(3),']'
         WRITE(6,'(A,F8.5,A,F8.5,A)') '      EIGN. VEC 2    = [',evecr(2),',',evecr(4),']'
         WRITE(6,'(A,F8.5,A,F8.5,A)') '      EIGN. VEC 1i   = [',eveci(1),',',eveci(3),']'
         WRITE(6,'(A,F8.5,A,F8.5,A)') '      EIGN. VEC 2i   = [',eveci(2),',',eveci(4),']'
         CALL FLUSH(6)
         DO j = -num_hcp,num_hcp
            dex = dex + 1
            !dex = j + num_hcp + 1 + (i-1)*(2*num_hcp+1)
            r_start(dex) = r0 + evecr(1)*j*delta_hc/num_hcp
            z_start(dex) = z0 + evecr(3)*j*delta_hc/num_hcp
            phi_start(dex) = 0.0
            phi_end(dex)   = pi2*25
         END DO
         nlines = dex
         DO j = -num_hcp,num_hcp
            dex = dex + 1
            !dex = j + num_hcp + 1 + (i-1)*(2*num_hcp+1)+nlines
            r_start(dex) = r0 + evecr(2)*j*delta_hc/num_hcp
            z_start(dex) = z0 + evecr(4)*j*delta_hc/num_hcp
            phi_start(dex) = 0.0
            phi_end(dex)   = -pi2*25
         END DO
         nlines = dex
      END DO
      RETURN
!-----------------------------------------------------------------------
!     End Subroutine
!-----------------------------------------------------------------------
      END SUBROUTINE fieldlines_find_hc


*
* island.f subroutine to calculate path integral around island.
* Started 7/5/2
* var ds version 10/5/5
*
      subroutine island(ubloc,erisl1,isl,indj)
c for exact curl form, psi is required (locally called pex)
c     subroutine island(ubloc,pex,erisl,isl,indj)
#include "ocean.cmn"

      integer i, k, lpi, ipi, jpi, isl, indj

      real ubloc(2,0:maxi+1,0:maxj),erisl1,cor,tv1,tv2
c     real pex(0:maxi,0:maxj)

      erisl1 = 0.0
      do i=1,npi(isl)
         lpi = lpisl(i,isl)
         ipi = ipisl(i,isl)
         jpi = jpisl(i,isl)
         if(abs(lpi).eq.1)then
            cor = - s(jpi)*0.25*(ubloc(2,ipi,jpi) + ubloc(2,ipi+1,jpi)
     &    + ubloc(2,ipi,jpi-1) + ubloc(2,ipi+1,jpi-1))
c for exact curl form use corex
c           corex = - s(jpi)*0.25*(pex(ipi+1,jpi) + pex(ipi+1,jpi-1)
c    &    - pex(ipi-1,jpi) - pex(ipi-1,jpi-1))*rh(1,ipi,jpi)
         else
            cor = sv(jpi)*0.25*(ubloc(1,ipi-1,jpi) + ubloc(1,ipi,jpi)
     &    + ubloc(1,ipi-1,jpi+1) + ubloc(1,ipi,jpi+1))
c           corex = - sv(jpi)*0.25*(pex(ipi,jpi+1) + pex(ipi-1,jpi+1)
c    &    - pex(ipi,jpi-1) - pex(ipi-1,jpi-1))*rh(2,ipi,jpi)
         endif
c        erisl1 = erisl1 + sign(1,lpi)*((drag(abs(lpi),ipi,jpi)
c    &      *ubloc(abs(lpi),ipi,jpi)
         erisl1 = erisl1 + sign(1,lpi)*(drag(abs(lpi),ipi,jpi)
     &      *ubloc(abs(lpi),ipi,jpi) + cor
     &      - indj*tau(abs(lpi),ipi,jpi)*rh(abs(lpi),ipi,jpi))
     &      *(c(jpi)*dphi*(2.0 - abs(lpi))
     &      + rcv(jpi)*dsv(jpi)*(abs(lpi) - 1.0))
c    &      + rcv(jpi)*dsv(jpi)*(abs(lpi) - 1.0)) + corex)
c
c calc tricky bits and add to source term for path integral round
c islands, all sums have at least one element
c
         if(indj.eq.1)then
            if(abs(lpi).eq.1)then
               tv1 = 0.0
               do k=ku(1,ipi,jpi),mk(ipi+1,jpi)
                  tv1 = tv1 + bp(ipi+1,jpi,k)*dz(k)
               enddo
               do k=ku(1,ipi,jpi),mk(ipi,jpi)
                  tv1 = tv1 - bp(ipi,jpi,k)*dz(k)
               enddo
               erisl1 = erisl1 + (sbp(ipi+1,jpi) - sbp(ipi,jpi) + tv1)
     &            *sign(1,lpi)*rh(1,ipi,jpi)
            else
               tv2 = 0.0
               do k=ku(2,ipi,jpi),mk(ipi,jpi+1)
                  tv2 = tv2 + bp(ipi,jpi+1,k)*dz(k)
               enddo
               do k=ku(2,ipi,jpi),mk(ipi,jpi)
                  tv2 = tv2 - bp(ipi,jpi,k)*dz(k)
               enddo
               erisl1 = erisl1 + (sbp(ipi,jpi+1) - sbp(ipi,jpi) + tv2)
     &            *sign(1,lpi)*rh(2,ipi,jpi)
            endif
         endif
      enddo
c     print*,'erisl1 ',erisl1
      end