!--------------------------------------------------------------------------------
! Copyright (c) 2016 Peter Grünberg Institut, Forschungszentrum Jülich, Germany
! This file is part of FLEUR and available as free software under the conditions
! of the MIT license as expressed in the LICENSE file in more detail.
!--------------------------------------------------------------------------------

MODULE m_brzone2
USE m_juDFT

CONTAINS
SUBROUTINE brzone2(rcmt,nsym,idrot,mface,nbsz,nv48,&
                   cpoint,xvec,ncorn,nedge,nface,fnorm,fdist)

   USE m_constants, ONLY : pimach

   IMPLICIT NONE

   ! This subroutine constructs the irreducible wedge of the Brillouin
   ! zone (IBZ). The subroutine signature is based on an earlier
   ! implementation of this functionality in brzone but the algorithm
   ! is a new design as the old routine featured too many bugs. Some
   ! code parts are taken from the old routine.
   !
   !                                  GM, 2016

   INTEGER, INTENT (IN) :: mface,nbsz,nv48
   INTEGER, INTENT (IN) :: nsym               ! number of symmetry elements
   REAL,    INTENT (IN) :: rcmt(3,3)          ! reciprocal lattice basis (2\pi/a.u.)
   REAL,    INTENT (IN) :: idrot(3,3,48)      ! rotation matrices in cartesian repr.

   INTEGER, INTENT (OUT) :: ncorn,nedge,nface ! number of corners, faces and edges of the IBZ
   REAL,    INTENT (OUT) :: fnorm(3,mface)    ! normal vector of the planes bordering the IBZ
   REAL,    INTENT (OUT) :: fdist(mface)      ! distance vector of the planes bordering the IBZ
   REAL,    INTENT (OUT) :: cpoint(3,mface)   ! cartesian coordinates of corner points of IBZ
   REAL,    INTENT (OUT) :: xvec(3)           ! arbitrary vector lying in the IBZ

   INTEGER               :: info, n1, n2, n3, n4, i, j, k, iPlane, iSym
   INTEGER               :: nPlanes, nOuterPlanes, nCorners, numIBZPlanes
   INTEGER               :: maxNumPlanes, nUniqueCorners, duplicateNum
   INTEGER               :: nbszLocal, planesDim
   INTEGER               :: ipiv(3), krecip(3)
   INTEGER               :: cornerPlanes(3,5000)
   INTEGER               :: numCornerPlanes(5000)
   LOGICAL               :: foundDuplicate, filterOut
   LOGICAL               :: isIBZCorner(5000)
   REAL, PARAMETER       :: eps09 = 1.0e-9
   REAL                  :: pi, maxRecDist, vecDist, recScale, scalarProduct
   REAL                  :: norm, normalScalarProduct
   REAL                  :: denominator, vec1Fac, vec2Fac, edgeDist
   REAL                  :: testVec(3), sk(3), vecA(3), vecB(3), vecC(3)
   REAL                  :: corners(3,5000)
   REAL                  :: planesMatrix(3,3), solutions(3,1)
   REAL                  :: equationSystem(3,3)
   REAL                  :: edgeDirec(3), edgeDistVec(3), distVec(3)

   INTEGER, ALLOCATABLE  :: cornerPlaneList(:,:)
   INTEGER, ALLOCATABLE  :: planeCorners(:,:)
   INTEGER, ALLOCATABLE  :: nPlaneCorners(:)
   LOGICAL, ALLOCATABLE  :: isIBZPlane(:)

   REAL, ALLOCATABLE     :: dvec(:,:), ddist(:)

   ! Subroutine plan:
   ! 0. Initializations
   ! 1. Construct all possible boundary planes. Remove duplicate planes.
   ! 1.1. Construct boundary planes according to bisections of the lines
   !      to reciprocal lattice vectors <> 0.
   ! 1.2. Construct all boundary planes due to symmetry relations.
   ! 2. Determine all corner points of the IBZ.
   ! 2.1. Determine all possibly relevant intersection points of each
   !      subset of 3 boundary planes, respectively.
   ! 2.2. Select those points that are on the correct side of all of the
   !      possible boundary planes.
   ! 3. Remove all boundary planes that don't feature at least three
   !    corner points. These planes are not part of the real boundary.
   !    Also remove the corner points associated with each removed plane.
   ! 4. Construct edges from the set of boundary points. Actually
   !    only the number of edges is needed. I might use the Euler
   !    characteristic to calculate it.

   ! 0. Initializations

   pi = pimach()

   maxRecDist = 0.0
   DO n1 = -1,1,2
      DO n2 = -1,1,2
         DO n3 = -1,1,2
            testVec(:) = n1*rcmt(:,1)+n2*rcmt(:,2)+n3*rcmt(:,3)
            vecDist = SQRT(testVec(1)**2.0+testVec(2)**2.0+testVec(3)**2.0)
            IF(vecDist.GT.maxRecDist) maxRecDist = vecDist
         END DO
      END DO
   END DO
   maxRecDist = 1.01 * maxRecDist

   ! We ignore the nbsz (the number of neighboring cells to consider) passed to the routine
   ! and calculate an own nbszLocal on the basis of maxRecDist.

   nbszLocal = 1
   DO i = -1, 1, 2
      DO j = -1, 1, 2
         DO k = -1, 1, 2
            solutions(1,1) = i * maxRecDist
            solutions(2,1) = j * maxRecDist
            solutions(3,1) = k * maxRecDist
            equationSystem(:,:) = rcmt(:,:)
            ipiv = 0
            info = 0
            CALL DGETRF(3,3, equationSystem,3,ipiv,info)
            CALL DGETRS('N',3,1,equationSystem,3,ipiv,solutions,3,info)
            ! I assume that info == 0: The reciprocal lattice vectors should be linearly independent.
            if(nbszLocal.LT.ABS(solutions(1,1))) nbszLocal = CEILING(ABS(solutions(1,1)))
            if(nbszLocal.LT.ABS(solutions(2,1))) nbszLocal = CEILING(ABS(solutions(2,1)))
            if(nbszLocal.LT.ABS(solutions(3,1))) nbszLocal = CEILING(ABS(solutions(3,1)))
         END DO
      END DO
   END DO

   maxRecDist = 0.5 * maxRecDist

   ! 1. Construct all possible boundary planes. Remove duplicate planes.
   ! 1.1. Construct boundary planes according to bisections of the lines
   !      to reciprocal lattice vectors <> 0

   ! 1.1.1. Construct an arbitrary point xvec in the IBZ
   ! 1.1.1.1. Determine a "scale" for the point on the basis of the
   !          reciprocal lattice basis.
   DO i = 1,3
      sk(i) = 0.0
      DO j = 1,3
         sk(i)=sk(i)+rcmt(j,i)**2
      END DO
   END DO
   recScale = SQRT(MIN(sk(1),sk(2),sk(3)))*0.1

   ! 1.1.1.2. Use the scale to construct the arbitrary point xvec.
   xvec(1) = recScale
   xvec(2) = recScale/SQRT(pi)
   xvec(3) = recScale/pi

   ! loops over all neighboring lattice vectors. The size of the
   ! neigborhood is defined by nbszLocal.

   planesDim = 2*nbszLocal+1
   planesDim = planesDim*planesDim*planesDim + nsym
   ALLOCATE(dvec(3,planesDim), ddist(planesDim))
   ALLOCATE(nPlaneCorners(planesDim),isIBZPlane(planesDim))

   nPlanes = 0
   DO n1 = -nbszLocal, nbszLocal
      krecip(1) = n1
      DO n2 = -nbszLocal, nbszLocal
         krecip(2) = n2
         DO n3 = -nbszLocal, nbszLocal
            IF ( .NOT.(n1.EQ.0.AND.n2.EQ.0.AND.n3.EQ.0) ) THEN
               krecip(3) = n3
               nPlanes = nPlanes + 1

               ! determine distance vector dvec to the selected reciprocal
               ! lattice vector
               DO i = 1,3
                  dvec(i,nPlanes) = 0.0
                  DO j = 1,3
                     dvec(i,nPlanes) = dvec(i,nPlanes) + rcmt(i,j)*krecip(j)
                  END DO
               END DO

               ! Determine the norm of the distance vector
               norm = 0.0
               DO i = 1,3
                  norm = norm + dvec(i,nPlanes)**2
               END DO
               norm = SQRT(norm)

               ! Deterime the distance for the intersection
               ddist(nPlanes) = 0.5*norm

               IF(ddist(nPlanes).GT.maxRecDist) THEN
                  nPlanes = nPlanes - 1
                  CYCLE
               END IF

               ! Normalize distance vector. Note that this is now the
               ! normal to the intersecting plane. The intersecting plane
               ! is defined by the normal and the distance.
               ! Note: With the current sign the vector points outwards.
               dvec(:,nPlanes) = dvec(:,nPlanes) / norm

               ! Remove plane again if it is a duplicate of another plane
               ! or only use the plane nearer to the origin if it is parallel
               ! to another plane and has the same direction of the normal.
               foundDuplicate = .FALSE.
               DO iPlane = 1, nPlanes-1
                  IF (ABS(dvec(1,iPlane)-dvec(1,nPlanes)).GT.eps09) CYCLE
                  IF (ABS(dvec(2,iPlane)-dvec(2,nPlanes)).GT.eps09) CYCLE
                  IF (ABS(dvec(3,iPlane)-dvec(3,nPlanes)).GT.eps09) CYCLE
                  IF (ddist(nPlanes).LT.ddist(iPlane)) ddist(iPlane) = ddist(nPlanes)
                  foundDuplicate = .TRUE.
                  EXIT
               END DO
               IF(foundDuplicate) nPlanes = nPlanes - 1
            END IF
         END DO
      END DO
   END DO

   nOuterPlanes = nPlanes

   ! 1.2. Construct all boundary planes due to symmetry relations.
   !      These are the planes bisecting the line connecting xvec
   !      with an element of the star of xvec ( <> xvec )
   !      Note that the loop over the symmetry operations actually
   !      is a loop over all group element of all the relevant
   !      symmetry groups. These are not only the generating elements
   !      and therefore it is not neccessary to invert the matrices to
   !      also obtain boundary planes on the other side.

   DO iSym = 2, nsym ! leave out identity symmetry operation
      nPlanes = nPlanes + 1

      ! The origin is part of all the planes determined in this way.
      ! -> distance = 0.
      ddist(nPlanes) = 0.0

      ! Determine the vector connecting xvec and its image after performing
      ! the symmetry operation.
      DO i = 1,3
         dvec(i,nPlanes)=-xvec(i)
         DO j=1,3
            dvec(i,nPlanes) = dvec(i,nPlanes) + idrot(i,j,iSym)*xvec(j)
         END DO
      END DO

      ! Normalize the vector (it is normal to the possible boundary plane).
      ! Note that the vector points away from xvec.
      norm = 0.0
      DO i = 1,3
         norm = norm + dvec(i,nPlanes)**2
      END DO
      norm = SQRT(norm)
      dvec(:,nPlanes)=dvec(:,nPlanes) / norm

      ! Remove plane again if it is a duplicate of another plane
      foundDuplicate = .FALSE.
      DO iPlane = nOuterPlanes+1, nPlanes-1
         IF (ABS(ddist(iPlane)-ddist(nPlanes)).GT.eps09) CYCLE
         IF (ABS(dvec(1,iPlane)-dvec(1,nPlanes)).GT.eps09) CYCLE
         IF (ABS(dvec(2,iPlane)-dvec(2,nPlanes)).GT.eps09) CYCLE
         IF (ABS(dvec(3,iPlane)-dvec(3,nPlanes)).GT.eps09) CYCLE
         foundDuplicate = .TRUE.
         EXIT
      END DO
      IF(foundDuplicate) nPlanes = nPlanes - 1
   END DO

   ! 2. Determine all corner points of the IBZ.
   ! 2.1. Determine all possibly relevant intersection points of each
   !      subset of 3 boundary planes, respectively.

   nCorners = 0
   nPlaneCorners = 0
   DO n1 = 1, nOuterPlanes
      DO n2 = n1+1, nPlanes
         ! 2.1.1. Calculate intersection edge between planes n1 and n2
         !        and only consider those n1 and n2 where the cuttig
         !        edge is not too far from the origin. (This would mean
         !        that the crossing point of these two planes and a third
         !        plane is never relevant for the construction of the
         !        IBZ.)

         ! The direction of the intersection edge is the cross product
         ! of the normals on planes n1 and n2:
         edgeDirec(1) = dvec(2,n1)*dvec(3,n2) - dvec(3,n1)*dvec(2,n2)
         edgeDirec(2) = dvec(3,n1)*dvec(1,n2) - dvec(1,n1)*dvec(3,n2)
         edgeDirec(3) = dvec(1,n1)*dvec(2,n2) - dvec(2,n1)*dvec(1,n2)

         ! Ignore parallel planes
         IF ((ABS(edgeDirec(1)).LT.eps09).AND.&
             (ABS(edgeDirec(2)).LT.eps09).AND.&
             (ABS(edgeDirec(3)).LT.eps09)) CYCLE

         ! The distance vector of the intersection edge to the origin is given
         ! by (since dvec is normalized):
         !
         !       ddist(n1) - ddist(n2) * <dvec(:,n1)|dvec(:,n2)>              ddist(n2) - ddist(n1) * <dvec(:,n1)|dvec(:,n2)>
         ! d_e = ----------------------------------------------- dvec(:,n1) + ----------------------------------------------- dvec(:,n2)
         !              1.0 - <dvec(:,n1)|dvec(:,n2)>^2                               1.0 - <dvec(:,n1)|dvec(:,n2)>^2

         normalScalarProduct = dvec(1,n1)*dvec(1,n2)+&
                               dvec(2,n1)*dvec(2,n2)+&
                               dvec(3,n1)*dvec(3,n2)
         denominator = 1.0 - (normalScalarProduct**2.0)
         vec1Fac = ddist(n1) - ddist(n2) * normalScalarProduct
         vec2Fac = ddist(n2) - ddist(n1) * normalScalarProduct
         edgeDistVec(:) = vec1Fac*dvec(:,n1) + vec2Fac*dvec(:,n2)
         edgeDistVec(:) = edgeDistVec(:) / denominator
         edgeDist = SQRT(edgeDistVec(1)**2.0+edgeDistVec(2)**2.0+edgeDistVec(3)**2.0)

         ! Ignore planes if intersection edge is too far from origin
         ! (is this criterion ok?)
         IF (edgeDist.GT.maxRecDist) CYCLE

         ! Now calculate the possibly relevant crossing points
         innerPlaneLoop: DO n3 = n2+1, nPlanes
            ! Set up system of linear equations to determine the crossing point
            DO i = 1, 3
               planesMatrix(1,i) = dvec(i,n1)
               planesMatrix(2,i) = dvec(i,n2)
               planesMatrix(3,i) = dvec(i,n3)
            END DO
            solutions(1,1) = ddist(n1)
            solutions(2,1) = ddist(n2)
            solutions(3,1) = ddist(n3)
            ! Solve system of linear equations and cycle if no solution is found
            ! or solution is not in relevant region.
            ipiv = 0
            info = 0
            CALL DGETRF(3,3, planesMatrix,3,ipiv,info)
            IF(info.NE.0) CYCLE
            CALL DGETRS('N',3,1,planesMatrix,3,ipiv,solutions,3,info)
            IF(info.NE.0) CYCLE
            vecDist = SQRT(solutions(1,1)**2.0+solutions(2,1)**2.0+solutions(3,1)**2.0)
            IF(vecDist.GT.maxRecDist) CYCLE

   ! 2.2. Select those points that are on the correct side of all of the
   !      possible boundary planes.

            DO n4 = 1, nPlanes
               vecDist = dvec(1,n4)*solutions(1,1)+dvec(2,n4)*solutions(2,1)+dvec(3,n4)*solutions(3,1) - ddist(n4)
               IF(vecDist*10..GT.eps09) THEN
                  CYCLE innerPlaneLoop
               END IF
            END DO

            !Add new crossing point to list and associate it with the 3 planes

            nCorners = nCorners + 1
            corners(:,nCorners) = solutions(:,1)
            cornerPlanes(1,nCorners) = n1
            cornerPlanes(2,nCorners) = n2
            cornerPlanes(3,nCorners) = n3
            nPlaneCorners(n1) = nPlaneCorners(n1) + 1
            nPlaneCorners(n2) = nPlaneCorners(n2) + 1
            nPlaneCorners(n3) = nPlaneCorners(n3) + 1
         END DO innerPlaneLoop
      END DO
   END DO

   ! 3. Remove all boundary planes that don't feature at least three
   !    corner points. These planes are not part of the real boundary.
   !    Also remove the corner points associated with each removed plane.
   !    ...and also those planes and corners that are irrelevant due to
   !    other reasons.

   ! Corners might be present multiple times (for different triples of planes).
   ! Remove duplicates. Collect planes intersecting at the respective point.
   maxNumPlanes = MAX(nPlanes-nOuterPlanes,3*nCorners) ! Just a save guess for the array dimension in the next line
   ALLOCATE(cornerPlaneList(maxNumPlanes,nCorners+1)) ! The +1 is because of the origin that is added later.
   cornerPlaneList = 0
   numCornerPlanes = 0

   nUniqueCorners = 0
   DO i = 1, nCorners
      duplicateNum = -1
      compareLoop: DO j = 1, i-1
         distVec(:) = corners(:,i) - corners(:,j)
         vecDist = SQRT(distVec(1)**2+distVec(2)**2+distVec(3)**2)
         IF (vecDist.LT.eps09) THEN
            duplicateNum = j
            EXIT compareLoop
         END IF
      END DO compareLoop
      IF (duplicateNum.EQ.-1) THEN
         nUniqueCorners = nUniqueCorners + 1
         numCornerPlanes(nUniqueCorners) = 3
         corners(:,nUniqueCorners) = corners(:,i)
         cornerPlaneList(1,nUniqueCorners) = cornerPlanes(1,i)
         cornerPlaneList(2,nUniqueCorners) = cornerPlanes(2,i)
         cornerPlaneList(3,nUniqueCorners) = cornerPlanes(3,i)
      ELSE
         DO j = 1, 3
            foundDuplicate = .FALSE.
            DO k = 1, numCornerPlanes(duplicateNum)
               IF (cornerPlaneList(k,duplicateNum).EQ.cornerPlanes(j,i)) THEN
                  foundDuplicate = .TRUE.
               END IF
            END DO
            IF (.NOT.foundDuplicate) THEN
               numCornerPlanes(duplicateNum) = numCornerPlanes(duplicateNum) + 1
               cornerPlaneList(numCornerPlanes(duplicateNum),duplicateNum) = cornerPlanes(j,i)
            END IF
         END DO
      END IF
   END DO
   nCorners = nUniqueCorners

   ! Add origin to corner points
   IF ((nPlanes-nOuterPlanes).GE.3) THEN ! The origin is only a corner if at least 3 planes feature this point
      nCorners = nCorners + 1
      corners(:,nCorners) = 0.0
      DO i = nOuterPlanes + 1, nPlanes
         cornerPlaneList(i-nOuterPlanes,nCorners) = i
      END DO
      numCornerPlanes(nCorners) = nPlanes - nOuterPlanes
   END IF

   ! Filter out "corners" found for sets of planes that do not meet in a single
   ! point but have a common intersection edge.
   ! (This absurd case actually occured so it has to be treated. LAPACK might
   ! find a "corner point" in this situation. Also the origin can be such a point.)
   nUniqueCorners = 0
   DO i = 1, nCorners
      filterOut = .FALSE.
      IF(numCornerPlanes(i).GE.3) THEN
         filterOut = .TRUE.
         vecA(:) = dvec(:,cornerPlaneList(1,i))
         vecB(:) = dvec(:,cornerPlaneList(2,i))
         cornerPlaneLoop: DO n3 = 3, numCornerPlanes(i)
            vecC(:) = dvec(:,cornerPlaneList(n3,i))
            testVec(1) = vecA(2)*vecB(3)-vecA(3)*vecB(2)
            testVec(2) = vecA(3)*vecB(1)-vecA(1)*vecB(3)
            testVec(3) = vecA(1)*vecB(2)-vecA(2)*vecB(1)
            scalarProduct = testVec(1)*vecC(1) + testVec(2)*vecC(2) + testVec(3)*vecC(3)
            IF (ABS(scalarProduct).GT.eps09) THEN
               filterOut = .FALSE.
               EXIT cornerPlaneLoop
            END IF
         END DO cornerPlaneLoop
      END IF
      IF(filterOut) THEN
         CYCLE
      END IF
      nUniqueCorners = nUniqueCorners + 1
      IF(nUniqueCorners.NE.i) THEN
         numCornerPlanes(nUniqueCorners) = numCornerPlanes(i)
         DO j = 1, numCornerPlanes(i)
            cornerPlaneList(j,nUniqueCorners) = cornerPlaneList(j,i)
         END DO
         corners(:,nUniqueCorners) = corners(:,i)
      END IF
   END DO
   nCorners = nUniqueCorners

   ! Count the number of corners for each plane
   nPlaneCorners = 0
   ALLOCATE(planeCorners(nPlanes,nCorners))
   planeCorners = 0
   DO i = 1, nCorners
      DO j = 1, numCornerPlanes(i)
         nPlaneCorners(cornerPlaneList(j,i)) = nPlaneCorners(cornerPlaneList(j,i)) + 1
         planeCorners(cornerPlaneList(j,i),nPlaneCorners(cornerPlaneList(j,i))) = i
      END DO
   END DO

   ! Remove irrelevant planes:
   nface = 0
   isIBZPlane(:) = .TRUE.
   DO n1 = 1, nPlanes
      IF (nPlaneCorners(n1).LE.2) THEN
         isIBZPlane(n1) = .FALSE.
         CYCLE
      END IF
!      WRITE(*,*) 'plane ', n1
!      WRITE(*,'(4f20.13)') dvec(:,n1), ddist(n1)
!      WRITE(*,*) 'corners:'
!      DO i = 1, nPlaneCorners(n1)
!         WRITE(*,'(i5,3f20.13)') planeCorners(n1,i), corners(:,planeCorners(n1,i))
!      END DO
      nface = nface + 1
   END DO

   ! Remove irrelevant corners:
   ncorn = 0
   isIBZCorner(:) = .TRUE.
!   WRITE(*,*) 'IBZ corners:'
   DO i = 1, nCorners
      numIBZPlanes = 0
      DO j = 1, numCornerPlanes(i)
         IF(isIBZPlane(cornerPlaneList(j,i))) THEN
            numIBZPlanes = numIBZPlanes + 1
         END IF
      END DO
      IF(numIBZPlanes.LE.2) isIBZCorner(i) = .FALSE.
      IF(.NOT.isIBZCorner(i)) CYCLE
      ncorn = ncorn + 1
!      WRITE(*,'(i5,3f20.13)') i, corners(:,i)
   END DO

   DEALLOCATE(cornerPlaneList)

   ! 4. Construct edges from the set of boundary points. Actually
   !    only the number of edges is needed.

   ! Count number of edges based on number of corners for each face
   ! Each face has the same number of edges as it has corners.
   ! Counting the number of edges in this way counts each edge twice.
   nedge = 0
   DO n1 = 1, nPlanes
      IF(.NOT.isIBZPlane(n1)) CYCLE
      nedge = nedge + nPlaneCorners(n1)
   END DO
   nedge = nedge / 2

   ! 5. Fill the output arrays.

   IF((mface.LT.ncorn).OR.(mface.LT.nface)) THEN
      WRITE(*,*) "mface: ", mface
      WRITE(*,*) "ncorn: ", ncorn
      WRITE(*,*) "nface: ", nface
      WRITE(*,*) "mface has to be larger or equal to nface and ncorn."
      WRITE(*,*) "Its value is hardcoded. Adapt it."
      CALL juDFT_error("mface was chosen too small.",calledby ="brzone2")
   END IF

   cpoint = 0.0
   j = 0
   DO i = 1, nCorners
      IF (.NOT.isIBZCorner(i)) CYCLE
      j = j + 1
      cpoint(:,j) = corners(:,i)
   END DO

   fdist = 0.0
   fnorm = 0.0
   j = 0
   DO i = 1, nPlanes
      IF(.NOT.isIBZPlane(i)) CYCLE
      j = j + 1
      fdist(j) = ddist(i)
      fnorm(:,j) = dvec(:,i)
   END DO

!   WRITE(*,*) 'ncorn', ncorn
!   WRITE(*,*) 'nedge', nedge
!   WRITE(*,*) 'nface', nface
!   WRITE(*,*) 'faces:'
!   DO i  =1,nface
!      WRITE(*,'(4f20.13)') fnorm(:,i), fdist(i)
!   END DO
!   WRITE(*,*) 'coners:'
!   DO i = 1,ncorn
!      WRITE(*,'(3f20.13)') cpoint(:,i)
!   END DO

   ! 5.1. Check Euler characteristic

   IF ((ncorn + nface - nedge).NE.2) THEN
      WRITE(*,*) "ncorn: ", ncorn
      WRITE(*,*) "nface: ", nface
      WRITE(*,*) "nedge: ", nedge
      WRITE(*,*) "corners: "
      DO i = 1, ncorn
         WRITE(*,'(3f20.13)') cpoint(:,i)
      END DO
      WRITE(*,*) "faces: "
      DO i = 1, nface
         WRITE(*,'(4f20.13)') fnorm(:,i), fdist(i)
      END DO
      CALL juDFT_error("Brillouin zone does not fulfill Euler characteristic.",calledby ="brzone2")
   END IF

   DEALLOCATE (planeCorners,dvec,ddist,nPlaneCorners,isIBZPlane)

END SUBROUTINE brzone2

END MODULE m_brzone2


      double complex function fa(j1,j2,j3,j4,j5,j6,za,zb)
      implicit none
      include 'constants.f'
      include 'sprods_com.f'
      include 'zprods_decl.f'
      integer j1,j2,j3,j4,j5,j6
      integer i1,i2,i3,i4
      double complex Lsm1_2mht,z2,BigT,I3m,flipbit

      double precision t134,t234,del3,del12

C---statement function
      z2(i1,i2,i3,i4)=za(i1,i2)*zb(i2,i4)+za(i1,i3)*zb(i3,i4)

c     Taken from Eq.(2.14)
      t134=s(j1,j3)+s(j1,j4)+s(j3,j4)
      t234=s(j2,j3)+s(j2,j4)+s(j3,j4)
      del12=s(j1,j2)-s(j3,j4)-s(j5,j6)
      del3=s(j1,j2)**2+s(j3,j4)**2+s(j5,j6)**2
     . -2d0*(s(j1,j2)*s(j3,j4)+s(j1,j2)*s(j5,j6)+s(j3,j4)*s(j5,j6))

      fa=(za(j1,j3)**2*zb(j2,j5)**2
     . /(za(j3,j4)*zb(j5,j6)*t134*(z2(j1,j5,j6,j2)))
     . -(z2(j2,j5,j6,j4)*z2(j6,j2,j5,j1))**2
     . /(zb(j3,j4)*za(j5,j6)*t134*(z2(j2,j5,j6,j1)**3)))
     . *Lsm1_2mht(s(j1,j2),t134,s(j3,j4),s(j5,j6))

      fa=fa+flipbit(j1,j2,j3,j4,j5,j6,za,zb)
     .     -flipbit(j2,j1,j5,j6,j3,j4,zb,za)

      fa=fa
     . +BigT(j1,j2,j3,j4,j5,j6,za,zb)*I3m(s(j1,j2),s(j3,j4),s(j5,j6))

      fa=fa+0.5d0*(t234*del12+2d0*s(j3,j4)*s(j5,j6))
     . /(z2(j2,j5,j6,j1)*del3)
     . *(zb(j4,j5)**2/(zb(j3,j4)*zb(j5,j6))
     .  +za(j3,j6)**2/(za(j3,j4)*za(j5,j6)))
      fa=fa+za(j3,j6)*zb(j4,j5)*(t134-t234)/(z2(j2,j5,j6,j1)*del3)
     . -0.5d0*z2(j6,j2,j5,j4)**2
     . /(zb(j3,j4)*za(j5,j6)*t134*z2(j2,j5,j6,j1))
      return
      end

      double complex function flipbit(j1,j2,j3,j4,j5,j6,za,zb)
      implicit none
      include 'constants.f'
      include 'sprods_com.f'
      include 'zprods_decl.f'
      integer j1,j2,j3,j4,j5,j6
      integer i1,i2,i3,i4
      double complex L0,L1,Lnrat,z2,L34_12,fb

      double precision t134

      z2(i1,i2,i3,i4)=za(i1,i2)*zb(i2,i4)+za(i1,i3)*zb(i3,i4)
      t134=s(j1,j3)+s(j1,j4)+s(j3,j4)

      fb=0.5d0*(za(j6,j1)*zb(j1,j4))**2*t134
     ./(zb(j3,j4)*za(j5,j6)*z2(j2,j5,j6,j1)*t134**2)*L1(-s(j3,j4),-t134)

      fb=fb+2d0*za(j6,j1)*zb(j1,j4)*z2(j6,j2,j5,j4)*L0(-t134,-s(j3,j4))
     ./(zb(j3,j4)*za(j5,j6)*z2(j2,j5,j6,j1)*s(j3,j4))

      fb=fb-za(j1,j6)*za(j2,j6)*zb(j1,j4)**2*t134*L0(-t134,-s(j3,j4))
     ./(zb(j3,j4)*za(j5,j6)*z2(j2,j5,j6,j1)**2*s(j3,j4))

      fb=fb-0.5d0*za(j2,j6)*zb(j1,j4)*z2(j6,j2,j5,j4)
     .*(Lnrat(-t134,-s(j3,j4))+Lnrat(-s(j1,j2),-s(j3,j4)))
     ./(zb(j3,j4)*za(j5,j6)*z2(j2,j5,j6,j1)**2)

      fb=fb-0.75d0*z2(j6,j2,j5,j4)**2
     ./(zb(j3,j4)*za(j5,j6)*t134*z2(j2,j5,j6,j1))
     .*(Lnrat(-t134,-s(j3,j4))+Lnrat(-s(j1,j2),-s(j3,j4)))

      fb=fb+L34_12(j1,j2,j3,j4,j5,j6,za,zb)*lnrat(-s(j3,j4),-s(j1,j2))
      flipbit=fb
      return
      end


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

!--------------------------------------------------------------------------
! EMsoft:filters.f90
!--------------------------------------------------------------------------
!
! MODULE: filters
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief various useful filters
!
!> @date 01/21/16 MDG 1.0 original
!> @date 02/02/16 MDG 1.1 added Hough Transform
!> @date 01/09/18 MDG 1.2 added getADPmap
!> @date 09/14/18 MDG 1.3 added fftw_cleanup() calls to eliminate momory leaks
!> @date 11/16/19 MDG 1.4 added Gaussian beam broadening filter for EBSD
!--------------------------------------------------------------------------

module filters

use local

IMPLICIT NONE

contains

!--------------------------------------------------------------------------
!
! SUBROUTINE: applyGaussianBeamSpread
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  apply a 2D Gaussian beam broadening kernel to the planes of a 3D array
!
!> @param ipar array dimensions
!> @param fpar step sizes
!> @param Vxyz counts array; will contain convolved array on exit
!> @param w FWHM of Gaussian beam
!
!> @date 11/16/19 MDG 1.0 original
!--------------------------------------------------------------------------
recursive subroutine applyGaussianBeamSpread(ipar, fpar, Vxyz, w, verbose)
!DEC$ ATTRIBUTES DLLEXPORT :: applyGaussianBeamSpread

use constants

IMPLICIT NONE

integer(kind=irg),INTENT(IN)                :: ipar(3)
real(kind=sgl),INTENT(IN)                   :: fpar(2)
integer(kind=irg),INTENT(INOUT)             :: Vxyz(ipar(1),ipar(2),ipar(3))
real(kind=dbl),INTENT(IN)                   :: w
logical,OPTIONAL,INTENT(IN)                 :: verbose

real(kind=dbl)                              :: sigma, pre, x, y, bd
integer(kind=irg)                           :: smax, ix, iy
real(kind=dbl),allocatable                  :: gkernel(:,:), accum(:,:,:), V(:,:,:)

! convert w to sigma
sigma = w / (2.D0*sqrt(2.D0*log(2.D0)))
pre = 1.D0/(sigma**2*2.0*cPi)

! get the largest distance from the origin for the convolution kernel
smax =  ceiling(2.5758*sigma) + 1  ! determined by requiring that kernel contains at least 98.5% of the Gaussian intensity

if (present(verbose)) then
  if (verbose.eqv..TRUE.) write (*,*) 'w, sigma, pre, smax : ',w, sigma, pre, smax
end if

! convert sigma to effective exponential argument
sigma = 1.D0/(2.D0 * sigma*sigma)

! generate the Gaussian kernel
allocate(gkernel(-smax:smax,-smax:smax))
do ix = -smax, smax
  x = (fpar(1) * ix)**2
  do iy = -smax, smax
    y = (fpar(2) * iy)**2
    gkernel(ix,iy) = exp( -sigma * (x+y) )
  end do
end do
gkernel = pre * gkernel

if (present(verbose)) then
  if (verbose.eqv..TRUE.) then
    do ix = -smax, smax
      write (*,*) gkernel(ix,-smax:smax)
    end do
    write (*,*) 'sum of all kernel elements : ', sum(gkernel) * fpar(1) * fpar(2)
  end if
end if

! generate the accumulator array accum; the nint version of this will be returned in Vxyz
allocate(accum(ipar(1),ipar(2),ipar(3)))
accum = 0.D0
bd = 0.D0

if (present(verbose)) then
  if (verbose.eqv..TRUE.) write(*,*) shape(Vxyz), shape(accum)
end if

! loop over x and y range and add the weighted eoshift() array to the accumulator
! we begin by eoshifting the input array to row -smax
allocate(V(ipar(1),ipar(2),ipar(3)))
V = dble(Vxyz)
V = eoshift(V, shift=-smax, boundary=bd, dim=1)
do ix = 1,2*smax+1
! loop over all the column shifts
  do iy = -smax, smax
    accum = accum + gkernel(ix-1-smax,iy) * eoshift(V, shift = iy, boundary = bd, dim=2)
  end do
! go to the next row
  if (ix.lt.2*smax+1) V = eoshift(V, shift=1, boundary=bd, dim=1)
end do

! and copy the accumulator array into the input array as nearest integers
Vxyz = nint( accum * fpar(1) * fpar(2) )
deallocate(V, accum, gkernel)

end subroutine applyGaussianBeamSpread

!--------------------------------------------------------------------------
!
! FUNCTION: applyPoissonNoise
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  apply a Poisson random deviate to an image
!
!> @param nx x dimension
!> @param ny y dimension
!> @param im image array; must have values in range [1..256]
!
!> @date 03/23/18 MDG 1.0 original
!--------------------------------------------------------------------------
recursive function applyPoissonNoise(image, nx, ny, idum) result(noisy)
!DEC$ ATTRIBUTES DLLEXPORT :: applyPoissonNoise

use noise

IMPLICIT NONE

integer, parameter              :: K4B=selected_int_kind(9)

integer(kind=irg),INTENT(IN)    :: nx
integer(kind=irg),INTENT(IN)    :: ny
real(kind=sgl),INTENT(IN)       :: image(nx, ny)
integer(K4B),INTENT(INOUT)      :: idum
!f2py intent(in,out) ::  idum
real(kind=sgl)                  :: noisy(nx, ny)

integer(kind=irg)               :: i, j
real(kind=sgl)                  :: av, mult, invmult

do i=1, nx
  do j=1, ny
    noisy(i,j) = POIDEV(image(i,j),idum)
  end do
end do

end function applyPoissonNoise

!--------------------------------------------------------------------------
!
! FUNCTION: image_histogram
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute the histogram of an image
!
!> @param nx x dimension
!> @param ny y dimension
!> @param im image array; must have values in range [1..256]
!
!> @date 01/23/16 MDG 1.0 original
!--------------------------------------------------------------------------
recursive function image_histogram( nx, ny, im ) result(h)
!DEC$ ATTRIBUTES DLLEXPORT :: image_histogram

IMPLICIT NONE

integer(kind=irg),INTENT(IN)    :: nx
integer(kind=irg),INTENT(IN)    :: ny
integer(kind=irg),INTENT(IN)    :: im(nx, ny)
integer(kind=irg)               :: h(256)

integer(kind=irg),parameter     :: nh = 256
integer(kind=irg)               :: i, j

! initialize parameters
h = 0

do i=1,nx
  do j=1,ny
    h(im(i,j)) = h(im(i,j))+1
  end do
end do

end function image_histogram

!--------------------------------------------------------------------------
!
! FUNCTION: image_jointhistogram
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute the joint histogram of two images
!
!> @param nx x dimension
!> @param ny y dimension
!> @param im1 image array; must have values in range [1..256]
!> @param im2 image array; must have values in range [1..256]
!
!> @date 04/23/17 MDG 1.0 original
!--------------------------------------------------------------------------
recursive function image_jointhistogram( nx, ny, im1, im2 ) result(h)
!DEC$ ATTRIBUTES DLLEXPORT :: image_jointhistogram

IMPLICIT NONE

integer(kind=irg),INTENT(IN)    :: nx
integer(kind=irg),INTENT(IN)    :: ny
integer(kind=irg),INTENT(IN)    :: im1(nx, ny)
integer(kind=irg),INTENT(IN)    :: im2(nx, ny)
integer(kind=irg)               :: h(256,256)

integer(kind=irg),parameter     :: nh = 256
integer(kind=irg)               :: i, j

! initialize parameters
h = 0

do i=1,nx
  do j=1,ny
    h(im1(i,j),im2(i,j)) = h(im1(i,j),im2(i,j))+1
  end do
end do

end function image_jointhistogram

!--------------------------------------------------------------------------
!
! FUNCTION: cumul_histogram
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute the cumulative histogram of an image
!
!> @param nx x dimension
!> @param ny y dimension
!> @param im image array; must have values in range [1..256]
!
!> @date 01/23/16 MDG 1.0 original
!--------------------------------------------------------------------------
recursive function cumul_histogram( nx, ny, im ) result(h)
!DEC$ ATTRIBUTES DLLEXPORT :: cumul_histogram

IMPLICIT NONE

integer(kind=irg),INTENT(IN)    :: nx
integer(kind=irg),INTENT(IN)    :: ny
integer(kind=irg),INTENT(IN)    :: im(nx, ny)
integer(kind=irg)               :: h(256)

integer(kind=irg),parameter     :: nh = 256
integer(kind=irg)               :: i, j, low, high, hst(nh), np, nploc(1)

np = nx*ny
hst = 0

! get the regular histogram
h = image_histogram( nx, ny, im )

! if all the intensities equal 0, then we need to return a zero cumulative
! histogram.
if (h(1).eq.np) then
  h = 0
  return
end if

! we need to allow for the possibility that the image is flat, so that
! all intensity is in a single bin.  In that case, the cumulative histogram
! will be a step function and the usual noramlization to [1..256] will
! not work
if (maxval(h).eq.np) then
  nploc = maxloc(h)
  hst(1:nploc(1)-1) = h(1)
  hst(nploc(1):nh) = 256
  h = hst
else
! convert h to a cumulative histogram hst
  hst = 0
  hst(1) = h(1)
  do i=2,nh
    hst(i) = h(i) + hst(i-1)
  end do

! and rescale to [1..256]
  h = int(255.0*((float(hst)-float(hst(1)))/float(hst(nh)-hst(1)))) + 1
end if

end function cumul_histogram

!--------------------------------------------------------------------------
!
! FUNCTION: image_entropy
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute the Shannon entropy of an image
!
!> @param h a 256 element intensity histogram
!
!> @date 04/23/17 MDG 1.0 original
!--------------------------------------------------------------------------
recursive function image_entropy( h ) result(e)
!DEC$ ATTRIBUTES DLLEXPORT :: image_entropy

IMPLICIT NONE

integer(kind=irg),INTENT(IN)    :: h(256)
real(kind=sgl)                  :: e

integer(kind=irg),parameter     :: nh = 256
integer(kind=irg)               :: i, j
real(kind=sgl)                  :: hnorm(256)

! initialize parameters
hnorm = float(h)

! normalize the histogram
hnorm = hnorm / sum(hnorm)

! add up the natural logarithm factors for the non-zero bins
e = 0.0

do i=1,nh
  if (h(i).ne.0) e = e - hnorm(i) * log(hnorm(i))
end do

end function image_entropy

!--------------------------------------------------------------------------
!
! FUNCTION: image_jointentropy
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute the Shannon joint entropy of two images
!
!> @param h a 256x256 element intensity joint histogram
!
!> @date 04/23/17 MDG 1.0 original
!--------------------------------------------------------------------------
recursive function image_jointentropy( h ) result(e)
!DEC$ ATTRIBUTES DLLEXPORT :: image_jointentropy

IMPLICIT NONE

integer(kind=irg),INTENT(IN)    :: h(256,256)
real(kind=sgl)                  :: e

integer(kind=irg),parameter     :: nh = 256
integer(kind=irg)               :: i, j
real(kind=sgl)                  :: hnorm(256,256)

! initialize parameters
hnorm = float(h)

! normalize the histogram
hnorm = hnorm / sum(hnorm)

! add up the natural logarithm factors for the non-zero bins
e = 0.0

do i=1,nh
 do j=1,nh
  if (h(i,j).ne.0) e = e - hnorm(i,j) * log(hnorm(i,j))
 end do
end do

end function image_jointentropy

!--------------------------------------------------------------------------
!
! FUNCTION: image_mutualinformation
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute the mutual information of two images
!
!> @param nx x dimension
!> @param ny y dimension
!> @param im image array; must have values in range [1..256]
!
!> @date 04/23/17 MDG 1.0 original
!--------------------------------------------------------------------------
recursive function image_mutualinformation( nx, ny, im1, im2 ) result(mi)
!DEC$ ATTRIBUTES DLLEXPORT :: image_mutualinformation

IMPLICIT NONE

integer(kind=irg),INTENT(IN)    :: nx
integer(kind=irg),INTENT(IN)    :: ny
integer(kind=irg),INTENT(IN)    :: im1(nx, ny)
integer(kind=irg),INTENT(IN)    :: im2(nx, ny)
real(kind=sgl)                  :: mi

real(kind=sgl)                  :: e1, e2, je

! get the individual and joint histograms
e1 = image_entropy(image_histogram( nx, ny, im1))
e2 = image_entropy(image_histogram( nx, ny, im2))
je = image_jointentropy(image_jointhistogram( nx, ny, im1, im2))

! compute the mutual information
mi = e1 + e2 - je

end function image_mutualinformation

!--------------------------------------------------------------------------
!
! FUNCTION: adhisteq
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  adaptive histogram equalization
!
!> @details This algorithm is based on the original paper by Pizer et al.,
!> "Adaptive Histogram Equalization and its Variations", Computer Vision,
!> Graphics, and Image Processing, 39:355-368, 1987.
!
!> @param nr number of subregions to split the image into [10 works well]
!> @param dimx x dimension
!> @param dimy y dimension
!> @param im image array; must have integer values in range [0..255]
!
!> @date 01/23/16 MDG 1.0 original
!> @date 01/27/16 MDG 1.1 correction of off-by-one error in final array copy
!--------------------------------------------------------------------------
recursive function adhisteq( nr, dimx, dimy, im, verbose ) result(output)
!DEC$ ATTRIBUTES DLLEXPORT :: adhisteq

IMPLICIT NONE

integer(kind=irg),INTENT(IN)    :: nr
integer(kind=irg),INTENT(IN)    :: dimx
integer(kind=irg),INTENT(IN)    :: dimy
integer(kind=irg),INTENT(IN)    :: im(dimx,dimy)
logical,INTENT(IN),OPTIONAL     :: verbose
integer(kind=irg)               :: output(dimx,dimy)

integer(kind=irg)               :: ts, hts, ntx, nty, i, j, i1, i2, ir, ic, istop, jstop, ix0, iy0, Tvalx, Tvaly
integer(kind=irg),parameter     :: nh=256
integer(kind=irg),allocatable   :: subim(:,:)
real(kind=sgl),allocatable      :: tmp(:), tintx(:,:), tinty(:,:), LL(:,:), LR(:,:), UL(:,:), UR(:,:)
integer(kind=irg),allocatable   :: chistarr(:,:,:)

if (PRESENT(verbose)) write(*,*) minval(im),maxval(im)

output = 0

! determine integer parameters
ts = int(maxval( (/ dimx, dimy /)) / nr)        ! size of a single tile for histogram computation
hts = maxval( (/ ts/2, 1 /) )                   ! half the tile size
ntx = (dimx-1)/hts                              ! number of tiles along x
nty = (dimy-1)/hts                              ! number of tiles along y

! allocate temporary array to create tile interpolation matrices
allocate(tmp(hts))
tmp = (/ (dble(i), i=0,hts-1) /) / dble(hts)

allocate(tintx(hts,hts), tinty(hts,hts))
do j=1,hts
  tintx(1:hts,j) = tmp(1:hts)
end do
tinty = transpose(tintx)
deallocate(tmp)

! define the array that will hold two rows of cumulative histograms
allocate(chistarr(nh,ntx,2))
chistarr = 0

do ir = 1, nty+1
  do i = 1, ntx
    chistarr(1:256,i,1) = chistarr(1:256,i,2)  ! copy the histograms from the next row
  end do

! get the cumulative histograms for this row of tiles
  if (ir.lt.nty+1)  then
    do ic = 1, ntx
! set the sub image array limits
      ix0 = (ic-1)*hts+1
      istop = minval( (/ix0+ts-1, dimx /) )
      Tvalx = ts
      if (istop.eq.dimx) Tvalx = istop-ix0 + 1
      iy0 = (ir-1)*hts+1
      jstop = minval( (/iy0+ts-1, dimy /) )
      Tvaly = ts
      if (jstop.eq.dimy) Tvaly = jstop-iy0 + 1
!if (PRESENT(verbose)) write(*,*) ix0, istop, iy0, jstop, Tvalx, Tvaly

! extract the sub image and compute its cumulative histogram (histogram starts at bin 1)
      allocate(subim(Tvalx,Tvaly))
      subim = 0
      subim(1:Tvalx,1:Tvaly) = im(ix0:istop, iy0:jstop)  + 1
!if (PRESENT(verbose)) write (*,*) 'subim : ',minval(subim),maxval(subim)
      chistarr(1:nh,ic,2) = cumul_histogram( Tvalx, Tvaly, subim )
      deallocate(subim)
    end do
  end if
!if (PRESENT(verbose)) write(*,*) minval(chistarr),maxval(chistarr)
!stop

  if (ir.eq.1) then  ! this is done only the first time through...
     do i = 1, ntx
       chistarr(1:256,i,1) = chistarr(1:256,i,2)  ! copy the histograms from the next row
     end do
  end if

! and here is the actual interpolation part
  do ic = 1, ntx+1
! set the sub image array limits (different from above!)
    ix0 = (ic-1)*hts+1
    istop = minval( (/ix0+hts-1, dimx /) )
    Tvalx = hts
    if (istop.eq.dimx) Tvalx = istop-ix0 + 1
    iy0 = (ir-1)*hts+1
    jstop = minval( (/iy0+hts-1, dimy /) )
    Tvaly = hts
    if (jstop.eq.dimy) Tvaly = jstop-iy0 + 1

! allocate the sub image and the four interpolation arrays
    allocate(subim(Tvalx,Tvaly),LL(Tvalx,Tvaly),LR(Tvalx,Tvaly),UL(Tvalx,Tvaly),UR(Tvalx,Tvaly))

! get the sub image
    subim(1:Tvalx,1:Tvaly) = im(ix0:istop, iy0:jstop) + 1

! set the coordinates to be used in the histogram array
    i1 = maxval( (/ ic-1, 1 /) )
    i2 = minval( (/ ic, ntx /) )

! and extract the cumulative histogram values for the intensities in the sub image,
! for each of the four interpolation arrays
    do i = 1, Tvalx
      do j = 1, Tvaly
        LL(i,j) = float(chistarr(subim(i,j),i1,1))
        LR(i,j) = float(chistarr(subim(i,j),i2,1))
        UL(i,j) = float(chistarr(subim(i,j),i1,2))
        UR(i,j) = float(chistarr(subim(i,j),i2,2))
      end do
    end do

! perform the interpolation along x
    LL = LL + (LR-LL) * tintx(1:Tvalx,1:Tvaly)
    UL = UL + (UR-UL) * tintx(1:Tvalx,1:Tvaly)
! and interpolate along y; store the result in the output array
    output(ix0:istop,iy0:jstop) = int(LL + (UL-LL) * tinty(1:Tvalx,1:Tvaly))

! deallocate the arrays
    deallocate(subim, LL, LR, UL, UR)
  end do
end do

deallocate(tintx, tinty, chistarr)

end function adhisteq

!--------------------------------------------------------------------------
!
! SUBROUTINE: getADPmap
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute Average Dot Product map, reading patterns from file unit iunit (expected to be open already)
!
!> @param iunit input file unit
!> @param nexpt number of experimental patterns
!> @param L number of pixels per pattern
!> @param wd ROI-width
!> @param ht ROI-ht
!> @param dpmap output ADP map
!
!> @date 01/09/18 MDG 1.0 original
!--------------------------------------------------------------------------
recursive subroutine getADPmap(iunit, nexpt, L, wd, ht, dpmap)
!DEC$ ATTRIBUTES DLLEXPORT :: getADPmap

integer(kind=irg),INTENT(IN)        :: iunit
integer(kind=irg),INTENT(IN)        :: nexpt
integer(kind=irg),INTENT(IN)        :: L
integer(kind=irg),INTENT(IN)        :: wd
integer(kind=irg),INTENT(IN)        :: ht
real(kind=sgl),INTENT(OUT)          :: dpmap(nexpt)

integer(kind=irg)                   :: ii, iii, jj
real(kind=sgl)                      :: lstore(L,wd), pstore(L,wd), lp(L), cp(L), imageexpt(L), dp

dpmap= 0.0
pstore = 0.0
lstore = 0.0
lp = 0.0
cp = 0.0

do iii = 1,nexpt
    read(iunit,rec=iii) imageexpt
    ii = mod(iii,wd)
    if (ii.eq.0) ii = wd
    jj = iii/wd+1
! do we need to copy pstore into lstore ?
    if ((ii.eq.1).and.(jj.gt.1)) lstore = pstore
! determine to which dpmap entries we need to add the dot product
    if (ii.eq.1) then
      cp(1:L) = imageexpt(1:L)
      pstore(1:L,ii) = cp(1:L)
    else
      lp = cp
      cp(1:L) = imageexpt(1:L)
      pstore(1:L,ii) = cp(1:L)
      dp = sum(lp(1:L)*cp(1:L))
      dpmap(iii-1) = dpmap(iii-1) + dp
      dpmap(iii) = dpmap(iii) + dp
    end if
    if (jj.gt.1) then
      dp = sum(lstore(1:L,ii)*cp(1:L))
      dpmap(iii-wd+1) = dpmap(iii-wd+1) + dp
      dpmap(iii) = dpmap(iii) + dp
    end if
end do

! correct the dot product map values depending on inside, edge, or corner pixels
! divide by 4
dpmap = dpmap*0.25

! correct the straight segments
dpmap(2:wd-1) = dpmap(2:wd-1) * 4.0/3.0
dpmap(nexpt-wd+2:nexpt-1) = dpmap(nexpt-wd+2:nexpt-1) * 4.0/3.0
do jj=1,ht-2
  dpmap(wd*jj+1) = dpmap(wd*jj+1) * 4.0/3.0
end do
do jj=2,ht-1
  dpmap(wd*jj) = dpmap(wd*jj) * 4.0/3.0
end do

! and the corners
dpmap(1) = dpmap(1) * 4.0
dpmap(wd) = dpmap(wd) * 2.0
dpmap(nexpt) = dpmap(nexpt) * 2.0
dpmap(nexpt-wd+1) = dpmap(nexpt-wd+1) * 4.0/3.0

end subroutine getADPmap

!--------------------------------------------------------------------------
!
! SUBROUTINE: getADPmapRAM
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  compute Average Dot Product map, reading patterns from file unit iunit
!
!> @param epatterns experimental patterns after preprocessing (held in RAM!)
!> @param nexpt number of experimental patterns
!> @param L number of pixels per pattern
!> @param wd ROI-width
!> @param ht ROI-ht
!> @param dpmap output ADP map
!
!> @date 01/09/18 MDG 1.0 original
!--------------------------------------------------------------------------
recursive subroutine getADPmapRAM(epatterns, nexpt, cs, L, wd, ht, dpmap)
!DEC$ ATTRIBUTES DLLEXPORT :: getADPmapRAM

integer(kind=irg),INTENT(IN)        :: nexpt
integer(kind=irg),INTENT(IN)        :: cs
real(kind=sgl),INTENT(IN)           :: epatterns(cs,nexpt)
integer(kind=irg),INTENT(IN)        :: L
integer(kind=irg),INTENT(IN)        :: wd
integer(kind=irg),INTENT(IN)        :: ht
real(kind=sgl),INTENT(OUT)          :: dpmap(nexpt)

integer(kind=irg)                   :: ii, iii, jj
real(kind=sgl)                      :: lstore(L,wd), pstore(L,wd), lp(L), cp(L), imageexpt(L), dp

dpmap= 0.0
pstore = 0.0
lstore = 0.0
lp = 0.0
cp = 0.0

do iii = 1,nexpt
    imageexpt = epatterns(1:L,iii)
    ii = mod(iii,wd)
    if (ii.eq.0) ii = wd
    jj = iii/wd+1
! do we need to copy pstore into lstore ?
    if ((ii.eq.1).and.(jj.gt.1)) lstore = pstore
! determine to which dpmap entries we need to add the dot product
    if (ii.eq.1) then
      cp(1:L) = imageexpt(1:L)
      pstore(1:L,ii) = cp(1:L)
    else
      lp = cp
      cp(1:L) = imageexpt(1:L)
      pstore(1:L,ii) = cp(1:L)
      dp = sum(lp(1:L)*cp(1:L))
      dpmap(iii-1) = dpmap(iii-1) + dp
      dpmap(iii) = dpmap(iii) + dp
    end if
    if (jj.gt.1) then
      dp = sum(lstore(1:L,ii)*cp(1:L))
      dpmap(iii-wd+1) = dpmap(iii-wd+1) + dp
      dpmap(iii) = dpmap(iii) + dp
    end if
end do

! correct the dot product map values depending on inside, edge, or corner pixels
! divide by 4
dpmap = dpmap*0.25

! correct the straight segments
dpmap(2:wd-1) = dpmap(2:wd-1) * 4.0/3.0
dpmap(nexpt-wd+2:nexpt-1) = dpmap(nexpt-wd+2:nexpt-1) * 4.0/3.0
do jj=1,ht-2
  dpmap(wd*jj+1) = dpmap(wd*jj+1) * 4.0/3.0
end do
do jj=2,ht-1
  dpmap(wd*jj) = dpmap(wd*jj) * 4.0/3.0
end do

! and the corners
dpmap(1) = dpmap(1) * 4.0
dpmap(wd) = dpmap(wd) * 2.0
dpmap(nexpt) = dpmap(nexpt) * 2.0
dpmap(nexpt-wd+1) = dpmap(nexpt-wd+1) * 4.0/3.0

end subroutine getADPmapRAM

!--------------------------------------------------------------------------
!
! SUBROUTINE: CalcHoughLUT
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  create a look-up table for a square Hough transform
!
!> @param dimx x dimension
!> @param LUT look-up table, allocated in calling program
!
!> @date 02/02/16 MDG 1.0 original
!--------------------------------------------------------------------------
recursive subroutine CalcHoughLUT( dimx, LUT )
!DEC$ ATTRIBUTES DLLEXPORT :: CalcHoughLUT

use constants

IMPLICIT NONE

integer(kind=irg),INTENT(IN)            :: dimx
real(kind=sgl),INTENT(OUT)              :: LUT(dimx*dimx,dimx)

real(kind=sgl)                          :: ct(dimx), st(dimx), pre, line(dimx), d2, x, y
integer(kind=irg)                       :: i, j, k, icnt

d2 = float(dimx/2)+0.5

! trigonometric look-up tables
ct = 0.0
st = 0.0
pre = sngl(cPi)/float(dimx)
line = (/ (i,i=0,dimx-1) /) * pre
ct = cos(line)
st = sin(line)

! and create the look up table
LUT = 0.0
icnt = 1
do j = 1,dimx
  y = float(j)-d2
  do i = 1,dimx
    x = float(i)-d2
    LUT(icnt,:) = nint(x*ct(:)+y*st(:))
    icnt = icnt + 1
  end do
end do

LUT = LUT + dimx/2 + 1

! this is here just so the ped indexing program can be tested out.
! the out of bounds error in the main chunk of the program should be corrected

do i = 1,dimx*dimx
    do j = 1,dimx
        if (LUT(i,j) .gt. dimx) LUT(i,j) = dimx
        if (LUT(i,j) .lt. 1.0) LUT(i,j) = 1.0
    end do
end do

end subroutine CalcHoughLUT

!--------------------------------------------------------------------------
!
! SUBROUTINE: HoughTransform
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  Perform a square Hough transform
!
!> @param dimx x dimension
!> @param LUT look-up table from CalcHoughLUT
!> @param im image array (must be mean-subtracted)
!> @param HT resulting Hough transform array
!
!> @date 02/02/16 MDG 1.0 original
!--------------------------------------------------------------------------
recursive subroutine HoughTransform( dimx, LUT, im, HT )
!DEC$ ATTRIBUTES DLLEXPORT :: HoughTransform

IMPLICIT NONE

integer(kind=irg),INTENT(IN)            :: dimx
real(kind=sgl),INTENT(IN)               :: LUT(dimx*dimx,dimx)
real(kind=sgl),INTENT(IN)               :: im(dimx*dimx)
real(kind=sgl),INTENT(OUT)              :: HT(dimx,dimx)

integer(kind=irg)                       :: i, j, k, d2

HT = 0.0
d2 = dimx/2

do j = 1,dimx
  do i = 1, dimx*dimx
    k = LUT(i,j)
    HT(k,j) = HT(k,j) + im(i)
  end do
end do

HT = transpose(HT)

end subroutine HoughTransform

!--------------------------------------------------------------------------
!
! SUBROUTINE: HiPassFilter
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  Perform a high pass filter
!
!> @param rdata real data to be transformed
!> @param dims dimensions of rdata array
!> @param w width of Gaussian profile
!> @param init (optional) initialize without computing anything
!> @param destroy (optional) destroy fft plans
!
!> @date 02/02/16 MDG 1.0 original
!> @date 06/03/16 MDG 1.1 modified mask to inverted Gaussian profile; added init optional parameter
!--------------------------------------------------------------------------
recursive function HiPassFilter(rdata,dims,w,init,destroy) result(fdata)
!DEC$ ATTRIBUTES DLLEXPORT :: HiPassFilter

use FFTW3mod

IMPLICIT NONE

integer(kind=irg),INTENT(IN)            :: dims(2)
real(kind=dbl),INTENT(IN)               :: w
real(kind=dbl),INTENT(IN)               :: rdata(dims(1),dims(2))
logical,INTENT(IN),OPTIONAL             :: init
logical,INTENT(IN),OPTIONAL             :: destroy
real(kind=dbl)                          :: fdata(dims(1),dims(2))

complex(kind=dbl),SAVE,allocatable      :: hpmask(:,:)
complex(kind=dbl)                       :: cone = cmplx(1.D0,0.D0), czero = cmplx(0.D0,0.D0)
integer(kind=irg)                       :: i, j, k, ii, jj
real(kind=dbl)                          :: x, y, val

! fftw variables
type(C_PTR),SAVE                        :: planf, planb
complex(C_DOUBLE_COMPLEX),SAVE,allocatable :: inp(:,:), outp(:,:)

! are we just destroying the fftw plans ?
if (present(destroy)) then
  if (destroy) then
    deallocate(hpmask, inp, outp)
    call fftw_destroy_plan(planf)
    call fftw_destroy_plan(planb)
    fdata = 0.D0
    return
  end if
end if

! if init=.TRUE. then initialize the hpmask variable and the fftw plans
if (present(init)) then
  if (init) then
! allocate arrays
    allocate(hpmask(dims(1),dims(2)), inp(dims(1),dims(2)), outp(dims(1),dims(2)))

! generate the complex inverted Gaussian mask; w = 0.05 produces good results (usually)
  do i=1,dims(1)/2
    x = dble(i)**2
    do j=1,dims(2)/2
      y = dble(j)**2
      if ((x+y).lt.30.D0) then
        val = 1.D0-dexp(-w*(x+y))
        hpmask(i,j) = cmplx(val, 0.D0)
        hpmask(dims(1)+1-i,j) = cmplx(val, 0.D0)
        hpmask(i,dims(2)+1-j) = cmplx(val, 0.D0)
        hpmask(dims(1)+1-i,dims(2)+1-j) = cmplx(val, 0.D0)
        fdata(i,j) = val
        fdata(dims(1)+1-i,j) = val
        fdata(i,dims(2)+1-j) = val
        fdata(dims(1)+1-i,dims(2)+1-j) = val
      end if
    end do
  end do

! then we set up the fftw plans for forward and reverse transforms
    planf = fftw_plan_dft_2d(dims(2),dims(1),inp,outp, FFTW_FORWARD, FFTW_ESTIMATE)
    planb = fftw_plan_dft_2d(dims(2),dims(1),inp,outp, FFTW_BACKWARD, FFTW_ESTIMATE)

! and return
    return
  end if
end if

! apply the hi-pass mask to rdata
do j=1,dims(1)
 do k=1,dims(2)
  inp(j,k) = cmplx(rdata(j,k),0.D0)
 end do
end do
call fftw_execute_dft(planf, inp, outp)
inp = outp * hpmask
call fftw_execute_dft(planb, inp, outp)
fdata(1:dims(1),1:dims(2)) = real(outp)

! call fftw_cleanup()

end function HiPassFilter

!--------------------------------------------------------------------------
!
! SUBROUTINE: init_HiPassFilter
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  Perform a high pass filter
!
!> @param rdata real data to be transformed
!> @param dims dimensions of rdata array
!> @param w width of Gaussian profile
!> @param init (optional) initialize without computing anything
!
!> @date 02/02/16 MDG 1.0 original
!> @date 06/03/16 MDG 1.1 modified mask to inverted Gaussian profile; added init optional parameter
!> @date 01/11/18 MDG 1.2 split routine from original to allow for OpenMP access
!> @date 07/14/20 MDG 1.3 commented out fftw_cleanup; possibly caused issue with ifort compiler
!--------------------------------------------------------------------------
recursive subroutine init_HiPassFilter(w, dims, hpmask, inp, outp, planf, planb)
!DEC$ ATTRIBUTES DLLEXPORT :: init_HiPassFilter

use FFTW3mod

IMPLICIT NONE

real(kind=dbl),INTENT(IN)               :: w
integer(kind=irg),INTENT(IN)            :: dims(2)
complex(kind=dbl),INTENT(INOUT)         :: hpmask(dims(1),dims(2))
!f2py intent(in,out) ::  hpmask
complex(C_DOUBLE_COMPLEX),pointer,INTENT(INOUT) :: inp(:,:), outp(:,:)
!f2py intent(in,out) ::  inp
type(C_PTR),INTENT(INOUT)               :: planf, planb
!f2py intent(in,out) ::  planf, planb

integer(kind=irg)                       :: i, j
real(kind=dbl)                          :: x, y, v2
complex(kind=dbl)                       :: val

hpmask = cmplx(1.D0,0.D0)

! generate the complex inverted Gaussian mask; w = 0.05 produces good results (usually)
do i=1,dims(1)/2
  x = dble(i)**2
  do j=1,dims(2)/2
    y = dble(j)**2
    v2 = w * ( x+y )
    if (v2.lt.30.D0) then
      val = cmplx(1.D0-dexp(-v2), 0.D0)
      hpmask(i,j) = val
      hpmask(dims(1)+1-i,j) = val
      hpmask(i,dims(2)+1-j) = val
      hpmask(dims(1)+1-i,dims(2)+1-j) = val
    end if
  end do
end do

! then we set up the fftw plans for forward and reverse transforms
planf = fftw_plan_dft_2d(dims(2),dims(1),inp,outp, FFTW_FORWARD, FFTW_ESTIMATE)
planb = fftw_plan_dft_2d(dims(2),dims(1),inp,outp, FFTW_BACKWARD, FFTW_ESTIMATE)

! call fftw_cleanup()

end subroutine init_HiPassFilter

!--------------------------------------------------------------------------
!
! SUBROUTINE: applyHiPassFilter
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  Perform a high pass filter
!
!> @param rdata real data to be transformed
!> @param dims dimensions of rdata array
!> @param w width of Gaussian profile
!
!> @date 02/02/16 MDG 1.0 original
!> @date 06/03/16 MDG 1.1 modified mask to inverted Gaussian profile
!> @date 01/11/18 MDG 1.2 split routine from original to allow for OpenMP access
!> @date 07/14/20 MDG 1.3 commented out fftw_cleanup
!--------------------------------------------------------------------------
recursive function applyHiPassFilter(rdata, dims, w, hpmask, inp, outp, planf, planb) result(fdata)
!DEC$ ATTRIBUTES DLLEXPORT :: applyHiPassFilter

use FFTW3mod

IMPLICIT NONE

integer(kind=irg),INTENT(IN)            :: dims(2)
real(kind=dbl),INTENT(IN)               :: w
real(kind=dbl),INTENT(IN)               :: rdata(dims(1),dims(2))
complex(kind=dbl),INTENT(IN)            :: hpmask(dims(1),dims(2))
complex(C_DOUBLE_COMPLEX),pointer,INTENT(INOUT) :: inp(:,:), outp(:,:)
!f2py intent(in,out) ::  inp
type(C_PTR),INTENT(IN)                  :: planf, planb
real(kind=dbl)                          :: fdata(dims(1),dims(2))

integer(kind=irg)                       :: j, k

! apply the hi-pass mask to rdata
do j=1,dims(1)
 do k=1,dims(2)
  inp(j,k) = cmplx(rdata(j,k),0.D0)
 end do
end do
call fftw_execute_dft(planf, inp, outp)
inp = outp * hpmask
call fftw_execute_dft(planb, inp, outp)
fdata(1:dims(1),1:dims(2)) = real(outp)

! call fftw_cleanup()

end function applyHiPassFilter

!--------------------------------------------------------------------------
!
! SUBROUTINE: HiPassFilterC
!
!> @author Marc De Graef, Carnegie Mellon University
!
!> @brief  Perform a high pass filter, callable from C
!
!> @param rdata real data to be transformed
!> @param dims dimensions of rdata array
!> @param w width of Gaussian profile
!> @param init initialize without computing anything
!> @param destroy destroy fft plans
!> @param fdata output data
!
!> @date 05/17/17 MDG 1.0 original, taken from regular routine above
!--------------------------------------------------------------------------
recursive subroutine HiPassFilterC(rdata,dims,w,init,destroy,fdata) bind(c, name='HiPassFilterC')
!DEC$ ATTRIBUTES DLLEXPORT :: HiPassFilterC

use FFTW3mod
use,INTRINSIC :: ISO_C_BINDING

IMPLICIT NONE

integer(c_int32_t),INTENT(IN)           :: dims(2)
real(C_DOUBLE),INTENT(IN)               :: w
real(C_DOUBLE),INTENT(IN)               :: rdata(dims(1),dims(2))
logical(C_BOOL),INTENT(IN)              :: init
logical(C_BOOL),INTENT(IN)              :: destroy
real(C_DOUBLE)                          :: fdata(dims(1),dims(2))

complex(C_DOUBLE_COMPLEX),SAVE,allocatable      :: hpmask(:,:)
complex(C_DOUBLE_COMPLEX)                       :: cone = cmplx(1.D0,0.D0), czero = cmplx(0.D0,0.D0)
integer(c_int32_t)                      :: i, j, k, ii, jj
real(C_DOUBLE)                          :: x, y, val

! fftw variables
type(C_PTR),SAVE                        :: planf, planb
complex(C_DOUBLE_COMPLEX),SAVE,allocatable :: inp(:,:), outp(:,:)

! are we just destroying the fftw plans ?
if (destroy.eqv..TRUE.) then
    deallocate(hpmask, inp, outp)
    call fftw_destroy_plan(planf)
    call fftw_destroy_plan(planb)
    fdata = 0.D0
end if

! if init=.TRUE. then initialize the hpmask variable and the fftw plans
if (init.eqv..TRUE.) then
! allocate arrays
    allocate(hpmask(dims(1),dims(2)), inp(dims(1),dims(2)), outp(dims(1),dims(2)))

! generate the complex inverted Gaussian mask; w = 0.05 produces good results (usually)
    do i=1,dims(1)/2
      x = float(i)
      do j=1,dims(2)/2
        y = float(j)
        val = 1.D0-dexp(-w*(x*x+y*y))
        hpmask(i,j) = cmplx(val, 0.D0)
        hpmask(dims(1)+1-i,j) = cmplx(val, 0.D0)
        hpmask(i,dims(2)+1-j) = cmplx(val, 0.D0)
        hpmask(dims(1)+1-i,dims(2)+1-j) = cmplx(val, 0.D0)
        fdata(i,j) = val
        fdata(dims(1)+1-i,j) = val
        fdata(i,dims(2)+1-j) = val
        fdata(dims(1)+1-i,dims(2)+1-j) = val
      end do
   end do

! then we set up the fftw plans for forward and reverse transforms
    planf = fftw_plan_dft_2d(dims(2),dims(1),inp,outp, FFTW_FORWARD, FFTW_ESTIMATE)
    planb = fftw_plan_dft_2d(dims(2),dims(1),inp,outp, FFTW_BACKWARD, FFTW_ESTIMATE)
end if

! apply the hi-pass mask to rdata
if ((destroy.eqv..FALSE.).and.(init.eqv..FALSE.)) then
  do j=1,dims(1)
   do k=1,dims(2)
    inp(j,k) = cmplx(rdata(j,k),0.D0)
   end do
  end do
  call fftw_execute_dft(planf, inp, outp)
  inp = outp * hpmask
  call fftw_execute_dft(planb, inp, outp)
  fdata(1:dims(1),1:dims(2)) = real(outp)
endif

! call fftw_cleanup()

end subroutine HiPassFilterC

!--------------------------------------------------------------------------
!
! SUBROUTINE: ButterflyMask9x9
!
!> @author Saransh Singh, Carnegie Mellon University
!
!> @brief apply 9x9 butterfly mask to the hough image
!
!> @param input input image
!> @param output output image
!> @param dims dimension of these images
!
!> @date 12/06/16 SS 1.0 original
!--------------------------------------------------------------------------
recursive subroutine ButterflyMask9x9(input, output, dims)
!DEC$ ATTRIBUTES DLLEXPORT :: ButterflyMask9x9

use constants

IMPLICIT NONE

integer(kind=irg),INTENT(IN)           :: dims
real(kind=sgl),INTENT(IN)              :: input(dims, dims)
real(kind=sgl),INTENT(OUT)             :: output(dims, dims)

integer(kind=irg)                      :: ii, jj, kk, ll
real(kind=sgl),allocatable             :: inputpadded(:,:)

allocate(inputpadded(-3:dims+4,-3:dims+4))
inputpadded = 0.0
inputpadded(1:dims,1:dims) = input(1:dims,1:dims)

!inputpadded(-3:0,:1:dims) = input(dims-3:dims,1:dims)
!inputpadded(dims+1:dims+4,1:dims) = input(1:4,1:dims)

!inputpadded(1:dims,-3:0) = inputpadded(1:dims,dims-3:dims)
!inputpadded(1:dims,dims+1:dims+4) = inputpadded(1:dims,1:4)

output = 0.0

do ii = 1,dims
    do jj = 1,dims
        do kk = -4,4
            do ll = -4,4
                output(ii,jj) = output(ii,jj) + inputpadded(ii+kk,jj+ll)*Butterfly9x9((kk+4)*9+ll+5)
            end do
        end do
    end do
end do

deallocate(inputpadded)

end subroutine ButterflyMask9x9

!--------------------------------------------------------------------------
!
! SUBROUTINE: InversionDivision
!
!> @author Saransh Singh, Carnegie Mellon University
!
!> @brief Inversion division operation to enhance contrast of hough image
!
!> @param input input image; must be between 0-1
!> @param output output image
!> @param dims dimension of these image
!
!> @date 12/06/16 SS 1.0 original
!--------------------------------------------------------------------------
recursive subroutine InversionDivision(input, output, dims)
!DEC$ ATTRIBUTES DLLEXPORT :: InversionDivision

IMPLICIT NONE

integer(kind=irg),INTENT(IN)           :: dims
real(kind=sgl),INTENT(IN)              :: input(dims, dims)
real(kind=sgl),INTENT(OUT)             :: output(dims, dims)

integer(kind=irg)                      :: ii, jj
real(kind=sgl),allocatable             :: inverse(:,:)
real(kind=sgl)                         :: ma, mi

allocate(inverse(dims, dims))

inverse = (1.0 - Input)

do ii = 1,dims
    do jj = 1,dims
        if(inverse(ii,jj) .ne. 0.0) then
            output(ii,jj) = input(ii,jj)/inverse(ii,jj)
        else
            output(ii,jj) = 1.0
        end if
    end do
end do

deallocate(inverse)

ma = maxval(output)
mi = minval(output)

output = (output - mi)/(ma - mi)

end subroutine InversionDivision

!--------------------------------------------------------------------------
!
! SUBROUTINE: HannWindow
!
!> @author Chaoyi Zhu, Carnegie Mellon University
!
!> @brief Hann windowing function for pattern region of interest
!
!> @param roi_size: size of the ROI
!
!> @date 03/18/2021 CZ 1.0 original
!--------------------------------------------------------------------------
recursive subroutine HannWindow(roi_size, window)
!DEC$ ATTRIBUTES DLLEXPORT :: HannWindow

use constants

IMPLICIT NONE

integer(kind=irg),INTENT(IN)           :: roi_size
real(kind=sgl),INTENT(INOUT)           :: window(roi_size, roi_size)
integer(kind=irg)                      :: i, j

do i=1,roi_size
    do j=1,roi_size
        window(i,j)=cos((cPi*(i-roi_size/2)/roi_size))*cos((cPi*(roi_size/2-j)/roi_size))
    end do
end do
end subroutine HannWindow

!--------------------------------------------------------------------------
!
! SUBROUTINE: init_BandPassFilter
!
!> @author Chaoyi Zhu, Carnegie Mellon University
!
!> @brief  Initialize high pass filter and low pass filter with predefined cut-off frequencies
!
!> @param rdata real data to be transformed
!> @param dims dimensions of rdata array
!> @param high_pass cut-off frequency for high pass filter
!> @param low_pass cut-off frequency for low pass filter
!
!> @date 03/18/21 CZ 1.0 original
!--------------------------------------------------------------------------
recursive subroutine init_BandPassFilter(dims, high_pass, low_pass, hpmask_shifted, &
lpmask_shifted, inp, outp, planf, planb)
!DEC$ ATTRIBUTES DLLEXPORT :: init_BandPassFilter

use FFTW3mod

IMPLICIT NONE

real(kind=sgl),INTENT(IN)               :: high_pass, low_pass
integer(kind=irg),INTENT(IN)            :: dims(2)
real(kind=sgl),INTENT(INOUT)            :: hpmask_shifted(dims(1),dims(2)), lpmask_shifted(dims(1),dims(2))
!f2py intent(in,out) ::  hpmask_shifted, lpmask_shifted
complex(C_DOUBLE_COMPLEX),pointer,INTENT(INOUT) :: inp(:,:), outp(:,:)
!f2py intent(in,out) ::  inp, outp
type(C_PTR),INTENT(INOUT)               :: planf, planb
!f2py intent(in,out) ::  planf, planb

integer(kind=irg)                       :: i, j
real(kind=sgl)                          :: grid(dims(1))
real(kind=sgl),dimension(dims(1),dims(2))    :: X_grid, Y_grid, D
complex(kind=sgl)                       :: val
real(kind=sgl)                          :: hpmask(dims(1),dims(2)), lpmask(dims(1),dims(2))

! generate the meshgrid in the frequency space
grid(1:dims(1))=(/(-1.0 + (i-1)*(2.0/(dims(1)-1)), i=1,dims(1))/)
call meshgrid2d(grid, grid, X_grid, Y_grid)

! distance from the zero frequency to a cut-off frequency
do i=1,dims(1)
  do j=1,dims(2)
    D(i,j)=0.5*sqrt(X_grid(i,j)**2+Y_grid(i,j)**2)
  end do
end do
! generate the mask for high pass filter
do i = 1,dims(1)
  do j = 1,dims(2)
    if (abs(D(i,j)).ge.high_pass+high_pass/4) then
      hpmask(i,j) = 1.0
    else if ((abs(D(i,j)).ge.high_pass-high_pass/4).AND.(abs(D(i,j)).le.high_pass+high_pass/4)) then
      hpmask(i,j) = (abs(D(i,j))-(high_pass-high_pass/4))/(high_pass/2)
    else
      hpmask(i,j) = 0.0
    end if
  end do
end do

hpmask_shifted = 0.0
call ifftshift(dims, hpmask, hpmask_shifted)

! generate the mask for low pass filter
do i = 1,dims(1)
  do j = 1,dims(2)
    if (abs(D(i,j)).le.low_pass-low_pass/4) then
      lpmask(i,j) = 1.0
    else if ((abs(D(i,j)).ge.low_pass-low_pass/4).AND.(abs(D(i,j)).le.low_pass+low_pass/4)) then
      lpmask(i,j) = 1.0-(abs(D(i,j))-(low_pass-low_pass/4))/(low_pass/2)
    else
      lpmask(i,j) = 0.0
    end if
  end do
end do

lpmask_shifted = 0.0
call ifftshift(dims, lpmask, lpmask_shifted)

! then we set up the fftw plans for forward and reverse transforms
planf = fftw_plan_dft_2d(dims(2), dims(1), inp, outp, FFTW_FORWARD, FFTW_ESTIMATE)
planb = fftw_plan_dft_2d(dims(2), dims(1), inp, outp, FFTW_BACKWARD, FFTW_ESTIMATE)

end subroutine init_BandPassFilter

!--------------------------------------------------------------------------
!
! SUBROUTINE: applyBandPassFilter
!
!> @author Chaoyi Zhu, Carnegie Mellon University
!
!> @brief  Apply high pass filter and low pass filter with predefined cut-off frequencies
!
!> @param rdata real data to be transformed
!> @param dims dimensions of rdata array
!> @param w width of Gaussian profile
!
!> @date 03/18/21 CZ 1.0 original
!--------------------------------------------------------------------------
recursive function applyBandPassFilter(rdata, dims, hpmask, lpmask, inp, outp, planf, planb) result(fdata)
!DEC$ ATTRIBUTES DLLEXPORT :: applyBandPassFilter

use FFTW3mod

IMPLICIT NONE

integer(kind=irg),INTENT(IN)            :: dims(2)
real(kind=dbl),INTENT(IN)               :: rdata(dims(1),dims(2)), lpmask(dims(1),dims(2)), hpmask(dims(1),dims(2))
complex(C_DOUBLE_COMPLEX),pointer,INTENT(INOUT) :: inp(:,:), outp(:,:)
!f2py intent(in,out) ::  inp, outp
type(C_PTR),INTENT(IN)                  :: planf, planb
real(kind=dbl)                          :: fdata(dims(1),dims(2))
integer(kind=irg)                       :: j, k
complex(kind=dbl)                       :: hpmask_complex(dims(1),dims(2)), lpmask_complex(dims(1),dims(2))

! apply the hi-pass mask to rdata
do j=1,dims(1)
 do k=1,dims(2)
  inp(j,k) = cmplx(rdata(j,k),0.D0)
  hpmask_complex(j,k) = cmplx(hpmask(j,k),0.D0)
  lpmask_complex(j,k) = cmplx(lpmask(j,k),0.D0)
 end do
end do

call fftw_execute_dft(planf, inp, outp)
inp = outp * hpmask * lpmask

call fftw_execute_dft(planb, inp, outp)
fdata(1:dims(1),1:dims(2)) = real(outp)

end function applyBandPassFilter

subroutine meshgrid2d(xgv, ygv, X, Y)
  implicit none
  real(kind=sgl),intent(in)   :: xgv(:), ygv(:)
  real(kind=sgl),intent(out)  :: X(:,:), Y(:,:)
  integer(kind=irg)           :: sX, sY, i

  sX = size(xgv)
  sY = size(ygv)
  X(:,:) = spread( xgv, 1, sY )
  Y(:,:) = spread( ygv, 2, sX )
end subroutine

subroutine ifftshift(dims, X, Y)
  implicit none
  integer(kind=irg),intent(in)                    :: dims(2)
  real(kind=sgl),intent(in)                       :: X(dims(1),dims(2))
  real(kind=sgl),intent(out)                      :: Y(dims(1),dims(2))
  ! shift the quadrants
  if (mod(dims(1),2).eq.0) then
    Y(dims(1)/2+1:dims(1),1:dims(2)/2)=X(1:dims(1)/2,dims(2)/2+1:dims(2))
    Y(1:dims(1)/2,dims(2)/2+1:dims(2))=X(dims(1)/2+1:dims(1),1:dims(2)/2)
    Y(1:dims(1)/2,1:dims(2)/2)=X(dims(1)/2+1:dims(1),dims(2)/2+1:dims(2))
    Y(dims(1)/2+1:dims(1),dims(2)/2+1:dims(2))=X(1:dims(1)/2,1:dims(2)/2)
  else
    Y(dims(1)/2+2:dims(1),1:dims(2)/2)=X(1:dims(1)/2,dims(2)/2+2:dims(2))
    Y(1:dims(1)/2,dims(2)/2+2:dims(2))=X(dims(1)/2+2:dims(1),1:dims(2)/2)
    Y(1:dims(1)/2,1:dims(2)/2)=X(dims(1)/2+1:dims(1),dims(2)/2+1:dims(2))
    Y(dims(1)/2+2:dims(1),dims(2)/2+2:dims(2))=X(1:dims(1)/2,1:dims(2)/2)
  endif
end subroutine

end module filters


!==========================================================================
elemental function gsw_entropy_part (sa, t, p)
!==========================================================================
!
! entropy minus the terms that are a function of only SA
!
! sa     : Absolute Salinity                               [g/kg]
! t      : in-situ temperature                             [deg C]
! p      : sea pressure                                    [dbar]
!
! gsw_entropy_part : entropy part
!--------------------------------------------------------------------------

use gsw_mod_teos10_constants, only : gsw_sfac

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, t, p

real (r8) :: gsw_entropy_part

real (r8) :: x2, x, y, z, g03, g08

x2 = gsw_sfac*sa
x = sqrt(x2)
y = t*0.025_r8
z = p*1e-4_r8

g03 = z*(-270.983805184062_r8 + &
    z*(776.153611613101_r8 + z*(-196.51255088122_r8 + (28.9796526294175_r8 - 2.13290083518327_r8*z)*z))) + &
    y*(-24715.571866078_r8 + z*(2910.0729080936_r8 + &
    z*(-1513.116771538718_r8 + z*(546.959324647056_r8 + z*(-111.1208127634436_r8 + 8.68841343834394_r8*z)))) + &
    y*(2210.2236124548363_r8 + z*(-2017.52334943521_r8 + &
    z*(1498.081172457456_r8 + z*(-718.6359919632359_r8 + (146.4037555781616_r8 - 4.9892131862671505_r8*z)*z))) + &
    y*(-592.743745734632_r8 + z*(1591.873781627888_r8 + &
    z*(-1207.261522487504_r8 + (608.785486935364_r8 - 105.4993508931208_r8*z)*z)) + &
    y*(290.12956292128547_r8 + z*(-973.091553087975_r8 + &
    z*(602.603274510125_r8 + z*(-276.361526170076_r8 + 32.40953340386105_r8*z))) + &
    y*(-113.90630790850321_r8 + y*(21.35571525415769_r8 - 67.41756835751434_r8*z) + &
    z*(381.06836198507096_r8 + z*(-133.7383902842754_r8 + 49.023632509086724_r8*z)))))))

g08 = x2*(z*(729.116529735046_r8 + &
    z*(-343.956902961561_r8 + z*(124.687671116248_r8 + z*(-31.656964386073_r8 + 7.04658803315449_r8*z)))) + &
    x*( x*(y*(-137.1145018408982_r8 + y*(148.10030845687618_r8 + y*(-68.5590309679152_r8 + 12.4848504784754_r8*y))) - &
    22.6683558512829_r8*z) + z*(-175.292041186547_r8 + (83.1923927801819_r8 - 29.483064349429_r8*z)*z) + &
    y*(-86.1329351956084_r8 + z*(766.116132004952_r8 + z*(-108.3834525034224_r8 + 51.2796974779828_r8*z)) + &
    y*(-30.0682112585625_r8 - 1380.9597954037708_r8*z + y*(3.50240264723578_r8 + 938.26075044542_r8*z)))) + &
    y*(1760.062705994408_r8 + y*(-675.802947790203_r8 + &
    y*(365.7041791005036_r8 + y*(-108.30162043765552_r8 + 12.78101825083098_r8*y) + &
    z*(-1190.914967948748_r8 + (298.904564555024_r8 - 145.9491676006352_r8*z)*z)) + &
    z*(2082.7344423998043_r8 + z*(-614.668925894709_r8 + (340.685093521782_r8 - 33.3848202979239_r8*z)*z))) + &
    z*(-1721.528607567954_r8 + z*(674.819060538734_r8 + &
    z*(-356.629112415276_r8 + (88.4080716616_r8 - 15.84003094423364_r8*z)*z)))))

gsw_entropy_part = -(g03 + g08)*0.025_r8

return
end function

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


*
*     ------------------------------------------------------------
*        Z Z
*     ------------------------------------------------------------
*
      Double precision FUNCTION ZZ(i1,i2,i3,i4,K)
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      Parameter (NOD=220)
      POINTER(IQP,P(NOD,1)),(IQN,N(1)),(IQL,L(1)),(IQAZ,AZ(1)),
     :       (IQMAX,MAX(1))
      COMMON/NEL/IQP,IQN,IQL,IQAZ,IQMAX,IQ(7)
      C= 2*k*(k+1)
      ZZ = - C * (TK(I1,I2,I3,I4,K+1) - TK(I1,I2,I3,I4,K-1))
*
      C1= L(i1)*(L(i1)+1) - L(i3)*(L(i3)+1) - k*(k+1)
      ZZ = ZZ - C1 * (UK(I1,I2,I3,I4,K+1) - UK(I1,I2,I3,I4,K-1))
*
      C2= L(i2)*(L(i2)+1) - L(i4)*(L(i4)+1) - k*(k+1)
      ZZ = ZZ - C2 * (UK(I2,I1,I4,I3,K+1) - UK(I2,I1,I4,I3,K-1))
*
      C= C1*C2/2
      C1= C*(k-2)/k/(k+k-1)
      C2= C*(k+3)/(k+1)/(k+k+3)
      ZZ = ZZ - C1 * (SN(I1,I2,I3,I4,K-2) + SN(I2,I1,I4,I3,K-2) )
     :        + C2 * (SN(I1,I2,I3,I4,K)   + SN(I2,I1,I4,I3,K) )
      RETURN
      END


!ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
! This subroutine do interpolation for velocity (ibuf==1) or make distribution for FSI force (ibuf==2)
! input:
! data_solids: source (velocity) for 1 & result (FSI force) for 2
! data_fluids: result (velocity) for 2 & source (FSI force) for 1
! nn_fluids: number of nodes in fluid domain
! nn_solids: number of nodes in solid domain
! nsd: spatial degree freedom
! ibuf: control parameter
! ne: number of elements in fluid domain
! nen: number of nodes per element in fluid domain
! ne_solid: number of elements in solid domain
! nen_solid: number of nodes per element in solid domain
! xyz_solid: xyz coordinates for solid points
! ien_solid_m(ne_solid,nen_solid) !conectivity matrix for solid from hypo
! ien_solid(nen_solid,ne_solid) ! connectivity matrix for FEM interpolation
! xyz_fluid(nsd, nn_fluids) !xyz coordinates for fluid points
! ien(nen,ne) !conectivity matrix for fluid
! infdomain: influence domain
! output:
! data_solids for ibuf==1
! data_fluids for ibuf==2
! Xingshi Wang 2008
!ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
subroutine data_exchange_FEM(data_solids,nn_solids,data_fluids,nn_fluids,dv,nsd,ibuf,ne,nen,&
                              ne_solid,nen_solid,xyz_solid,ien_solid_m,xyz_fluid,ien,infdomain,pre_f,pre_inter)
   use mpi_variables ! call mpi variable module
  implicit none
  include 'mpif.h'
  integer,intent(in) :: ibuf
  integer nsd
  integer ne
  integer nen
  integer nen_solid
  integer ne_solid

 !...solids variables
  integer,intent(in)   :: nn_solids
  real(8),intent(inout) :: data_solids(nsd,nn_solids)
  real(8) xyz_solid(nsd,nn_solids) ! xyz coordinates for solid points
  integer ien_solid_m(ne_solid,nen_solid) !conectivity matrix for solid from hypo
  integer ien_solid(nen_solid,ne_solid) ! connectivity matrix for FEM interpolation

 !...fluids variables
  integer,intent(in)   :: nn_fluids
  real(8),intent(inout) :: data_fluids(nsd,nn_fluids)
  real(8),intent(in)    :: dv(nn_fluids)
  real(8) xyz_fluid(nsd, nn_fluids) !xyz coordinates for fluid points
  integer ien(nen,ne) !conectivity matrix for fluid

 !... added variables to get the pressure at interface
  real(8) pre_f(nn_fluids)
  real(8) pre_inter(nn_solids)
!===========================================================
  integer infdomain(nn_solids) ! influence domain matrix

!********************************
! distribution loop variables

  integer index_el(nen)
  real(8) xyz_el(nsd,nen)
  !distribution coefficients for force
  real(8) dis_solid(nen)

!***********************************

 ! local varialbes
  integer inn ! loop variable for global nodals
  real(8) x(nsd) ! searching point for getinf_el_3d
  integer finf ! index of the searched element for getinf_el_3d
  integer maxconn ! constant for getinf_el_3d, the maximun number we can bear for the step of search
  integer inf_index(nen) ! the global indexes of the nodals formed the element we got

  integer ii ! loop variable for the nodals in the element
  real(8) inf_xyz(nsd,nen) ! xyz coordinates for the influence nodals

  real(8) sh(nen) ! shape function for each element in fluid domain

  ! check the sum of force both in fluid and solid
  real(8) force_solid(2)
  real(8) force_fluid(2)

  integer isd  ! loop variable for nsd
  integer ie  ! loop varible for ne
  do ii=1,nen_solid
     do ie=1,ne_solid
        ien_solid(ii,ie)=ien_solid_m(ie,ii)
     end do
  end do
  pre_inter(:)=0.0
  if (ibuf ==1) then  !velocity interpolation
	if (myid ==0) then
        write(*,*) '***Velocity interpolation using FEM method***'
	end if
        data_solids(:,:)=0

!        open (unit=40, file='interface.dat', status='unknown')
!        open (unit=41, file='exchange_solid.dat', status='unknown')
!        open (unit=42, file='sh.dat', status='unknown')
           do inn=1,nn_solids

           x(1:nsd)=xyz_solid(1:nsd,inn)

           do ii=1,nen
              inf_index(ii)=ien(ii,infdomain(inn))
              inf_xyz(1:nsd,ii)=xyz_fluid(1:nsd,inf_index(ii))
           end do
!           write(40,*) inf_xyz(1,:), inf_xyz(2,:)
           call sh_exchange(x,inf_xyz,nsd,nen,sh)

          do ii=1,nen
             data_solids(1:nsd,inn) = data_solids(1:nsd,inn) +&
             data_fluids(1:nsd,inf_index(ii)) * sh(ii)
             pre_inter(inn) = pre_inter(inn)+pre_f(inf_index(ii))*sh(ii)

          end do
!         write(42,*) sh(:)
!         write(41,*) data_solids(1,inn), data_solids(2,inn)
        end do

     else if (ibuf ==2) then !force distribution
 !       open(unit=43, file='dis_coe.dat', status='unknown')
 !       open(unit=44, file='force_fluid.dat', status='unknown')
 !       open(unit=45, file='force_solid.dat', status='unknown')
	if (myid ==0) then
         write(*,*) '*** Distributing Forces onto Fluid using FEM interpolation***'
	end if
        ! pre-step pick the element near the solid domain
        data_fluids(:,:)=0
! check if we lost any information when we do the distribution when needed
!        force_solid(:)=0
!        force_fluid(:)=0

!        write(*,*) 'I am in the inverse change'

        do inn=1, nn_solids

           do ii=1,nen
              index_el(ii)=ien(ii,infdomain(inn))
              do isd=1,nsd
               xyz_el(isd,ii)=xyz_fluid(isd,index_el(ii))
              end do
           end do
           x(:)=xyz_solid(:,inn)
           dis_solid(:)=0
           call sh_exchange(x,xyz_el,nsd,nen,dis_solid)
           do ii =1,nen

!if (myid ==0 ) then
!if (index_el(ii) == 33) then
!write(*,*) dis_solid(:)
!end if
!end if

           data_fluids(:,index_el(ii))=data_fluids(:,index_el(ii))+data_solids(:,inn)*dis_solid(ii)
           end do
        end do
        do inn=1,nn_solids
     !      do isd=1,nsd
        !      if (data_solids(isd,inn)>0) then
      !           force_solid(isd)=force_solid(isd)+data_solids(isd,inn)
        !      end if
     !      end do
        end do
      !    write(*,*) 'sum of fsi for solid', force_solid(1) , force_solid(2)
        do inn=1,nn_fluids
 !          write(44,*) data_fluids(1,inn), data_fluids(2,inn)
       !    do isd=1,nsd
       !    if (data_fluids(isd,inn)>0) then
       !       force_fluid(isd)=force_fluid(isd)+data_fluids(isd,inn)
       !    end if
       !   end do
        end do
        ! write(*,*) 'sum of fsi for fluid', force_fluid(1) , force_fluid(2)
        ! write(*,*) 'difference of force= ', force_solid(1)-force_fluid(1)

     end if
!return
end subroutine
! cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
! calculate the shape function for both 2-D and 3-D case
! 3-D case has not been finished yet
! The shape function here also can be used as the cooeffiecints for the force distribution
! Except triangle, other shape function need be fixed manually,
! since the digital error can not be avoided
! Xingshi Wang 2008
! cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
subroutine sh_exchange(x,xe,nsd,nen,sh)
integer nen
integer nsd
real(8) x(nsd)
real(8) xe(nsd,nen) ! xyz coordinates of the element nodals
real(8) sh(nen)
real(8) sh1(nen)
real(8) me(nen,nen)
real(8) me1(nen,nen)
real(8) invme(nen,nen)
real(8) det
real(8) xp(nen)
integer indx(nen)
integer i ! loop variable
integer j
! Lapack variables
integer info
integer ipiv(nen)
real(8) work(nen)
        do i=1,nen
        sh1(i)=0
        do j=1,nen
        me(i,j)=0
        end do
        end do

        if (nsd .eq. 2) then  ! 2-D case
           if(nen .eq. 3) then ! 2-D triangle
           do i=1,nen
           me1(i,1)=1.0
           me1(i,2:(nsd+1))=xe(1:nsd,i)
           end do
           call determinant(me1,nen,nen,det)
           !write(*,*)' det'
           sh(1)=(xe(1,2)*xe(2,3)-xe(1,3)*xe(2,2)+&
           (xe(2,2)-xe(2,3))*x(1)+&
           (xe(1,3)-xe(1,2))*x(2))/(det)

           sh(2)=(xe(1,3)*xe(2,1)-xe(1,1)*xe(2,3)+&
           (xe(2,3)-xe(2,1))*x(1)+&
           (xe(1,1)-xe(1,3))*x(2))/(det)

           sh(3)=(xe(1,1)*xe(2,2)-xe(1,2)*xe(2,1)+&
           (xe(2,1)-xe(2,2))*x(1)+&
           (xe(1,2)-xe(1,1))*x(2))/(det)

           sh(3)=1-sh(1)-sh(2)

           else if (nen .eq. 4) then ! 2-D quadrilateral

           do i=1,nen
           me(1,i)=1.0
           me(2,i)=xe(1,i)
           me(3,i)=xe(2,i)
           me(4,i)=xe(1,i)*xe(2,i)
           end do
           xp(1)=1.0d0
           xp(2)=x(1)
           xp(3)=x(2)
           xp(4)=x(1)*x(2)

!           call MIGS(me,nen,invme,indx) ! get inverse
		call DGETRF(nen,nen,me,nen,ipiv,info)
		call DGETRI(nen,me,nen,ipiv,work,nen,info)
           do j=1,nen
           do i=1,nen
           sh1(j)=sh1(j)+xp(i)*me(j,i)
           sh(j)=sh1(j)
           end do
           end do
!write(*,*) 'in data_exchange', xp(:)
!                call DGESV(nen,nen,me,nen,ipiv,xp,nen,info)
!		sh(:)=xp(:)
           end if
        else
           if (nen .eq. 4) then ! 3-D tet case
		   call shx_tets(x,xe,nsd,nen,sh)
		   end if

        end if
        do i=1,nen
          if (sh(i)<0) then
            call fix_sh(sh,nen)

        write(*,*) '****errors in shape function, running fix munually***'
          do j=1,nen
!         write(*,*) 'xe=', xe(1,j), xe(2,j)
         write(*,*)'sh= ', sh(j)
!         write(*,*) 'xp= ', xp(j)
         write(*,*) 'sum of shape function should be 1', sum(sh)
          end do
          go to 1000
          end if

        end do
1000    continue
return
end subroutine

!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
! Manually fix the digital error in shape function
! Right now assume we only have one negetive shapefunction
! Take the two smallest number (one is the negetive value)
! sum(these two terms)=1-sum(rest terms)
! further assume that these two equal to each other
! then fix them
! input:
! nen: number of nodes per element
! sh: shape function of this element
! output:
! sh: the fixed shape funtion
! (all larger tham zero and less than one; sum of them equals one)
! Xingshi Wang 2008
!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
subroutine fix_sh(sh,nen)
real(8) sh(nen)
integer nen

integer i
real(8) tempsh(nen)
real(8) fixsh1
real(8) fixsh2
real(8) tempsum
integer index1
integer index2

   fixsh1=1.0
   fixsh2=1.0
   tempsum=0

   tempsh(:)=sh(:)
   do i=1,nen
      if (tempsh(i)< fixsh1) then
      fixsh1=tempsh(i)
      index1=i
      end if
   end do
   do i=1,nen
      if (tempsh(i)< fixsh2 .and. tempsh(i)> fixsh1) then
      fixsh2=tempsh(i)
      index2=i
      end if
   end do

   do i=1,nen
   if ((i .ne. index1) .and. (i .ne. index2)) then
   tempsum=tempsum+tempsh(i)
   end if
   end do
   tempsum=1-tempsum
   fixsh1=tempsum/2.0
   fixsh2=tempsum/2.0
   sh(index1)=fixsh1
   sh(index2)=fixsh2

   return

end