!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!     
SUBROUTINE GenInv(N,A,InvA,NReal)
!!!!!!!!!!!!!!!!
  !     
  !     This subroutines calculates the generalized inverse of a matrix
  !     It first diagonalize the matrix A, then inverse all non-zero
  !     eigenvalues, and forms the  InvA matrix using these new eigenvalues
  !     
  !     Input:
  !     N : dimension of A
  !     NReal :Actual dimension of A
  !     A(N,N) : Initial Matrix, stored in A(Nreal,Nreal)
  !     
  !     Output:
  !     InvA(N,N) : Inversed Matrix, stored in a (Nreal,NReal) matrix
  !     
!!!!!!!!!!!!!!!!!!!!!!!

  Use Vartypes
  IMPLICIT NONE

  INTEGER(KINT), INTENT(IN) :: N,Nreal
  REAL(KREAL), INTENT(IN) :: A(NReal,NReal)
  REAL(KREAL), INTENT(OUT) :: InvA(NReal,NReal)
  !     

  INTEGER(KINT) :: I,J,K
  REAL(KREAL), ALLOCATABLE :: EigVec(:,:) ! (Nreal,Nreal)
  REAL(KREAL), ALLOCATABLE :: EigVal(:) ! (Nreal)
  REAL(KREAL), ALLOCATABLE :: ATmp(:,:) ! (NReal,Nreal)
  REAL(KREAL) :: ss
  !     
  REAL(KREAL), PARAMETER :: eps=1e-12

  ALLOCATE(EigVec(Nreal,Nreal), EigVal(Nreal),ATmp(NReal,NReal))
! A will be destroyed in Jacobi so we save it
  ATmp=A
  CALL JAcobi(ATmp,N,EigVal,EigVec,NReal)

  DO I=1,N
     IF (abs(EigVal(I)).GT.eps) EigVal(I)=1.d0/EigVal(I)
  END DO

  InvA=0.d0
  do k = 1, n
     do j = 1, n
        ss = eigval(k) * eigvec(j,k)
        do i = 1, j
            InvA(i,j) = InvA(i,j) + ss * eigvec(i,k)
         end do
      end do
   end do
   do j = 1, n
      do i = 1, j-1
         InvA(j,i) = InvA(i,j)
      end do
   end do


  DEALLOCATE(EigVec, EigVal,ATmp)
END SUBROUTINE GenInv

!============================================================
!
!     ++  Matrix diagonalization Using jacobi
!  Works only for symmetric matrices
!     EigvenVectors  : V(i,i)
!     Eigenvalues : D(i)
! PFL 30/05/03
! This versioin uses packed matrices.
! it unpacks them before calling Jacobi !
! we have  AP(i + (j-1)*j/2) = A(i,j) for 1<=i<=j;
!
!============================================================
!
      SUBROUTINE JacPacked(N,AP,D,V,nreal)

      Use Vartypes
      IMPLICIT NONE

      INTEGER(KINT), INTENT(IN) :: N,NREAL
      INTEGER(KINT), PARAMETER :: Max_it=500
      REAL(KREAL) :: AP(N*(N+1)/2)
      REAL(KREAL), ALLOCATABLE ::  A(:,:)
      REAL(KREAL) :: V(Nreal,Nreal),D(Nreal)
      INTEGER(KINT) :: i,j,nn

      allocate(A(nreal,nreal))
      nn=n*(n+1)/2
!      WRITE(*,*) 'Jacpa 0'
!      WRITE(*,'(I3,10(1X,F15.6))') n,(AP(i),i=1,min(nn,5))
 !     WRITE(*,*) 'Jacpa 0'
      do j=1,n
         do i=1,j
!            WRITE(*,*) i,j
            A(i,J)=AP(i + (j-1)*j/2)
            A(j,I)=A(i,J)
         end do
      end do
!      do j=1,n
!         WRITE(*,'(10(1X,F15.6))') (A(i,J),i=1,min(n,5))
!      end do

!      WRITE(*,*) 'Jacpa 1'
      call Jacobi(A,n,D,V,Nreal)
!      WRITE(*,*) 'Jacpa 2'
!      DO i=1,n
!         WRITE(*,'(1X,I3,10(1X,F15.6))') i,D(i),(V(j,i),j=1,min(n,5))
!      end do
      deallocate(a)

    end SUBROUTINE JacPacked



!     
!============================================================
!     
!     ++  Matrix diagonalization Using jacobi
!  Works only for symmetric matrices
!     EigvenVectors  : V
!     Eigenvalues : D
!     
!============================================================
!     
SUBROUTINE JACOBI(A,N,D,V,Nreal)

!!!!!!!!!!!!!!!!
  !     
  !     Input:
  !     N      :  Dimension of A
  !     NReal  : Actual dimensions of A, D and V.
  !
  !     Input/output:
  !     A(N,N) : Matrix to be diagonalized, store in a (Nreal,Nreal) matrix
  !              Destroyed in output.
  !     Output:
  !     V(N,N) : Eigenvectors, stored in V(NReal, NReal)
  !     D(N)   : Eigenvalues, stored in D(NReal)
  !     

  Use Vartypes

  IMPLICIT NONE
  INTEGER(KINT), parameter :: max_it=500
  REAL(KREAL), ALLOCATABLE ::  B(:),Z(:)

  INTEGER(KINT) :: N,NReal
  REAL(KREAL) :: A(NReal,NReal)
  REAL(KREAL) :: V(Nreal,Nreal),D(Nreal)

  INTEGER(KINT) :: I, J,IP, IQ, NROT
  REAL(KREAL) :: SM, H, Tresh, G, T, Theta, C, S, Tau

  allocate(B(N),Z(N))

  DO  IP=1,N
     DO IQ=1,N
        V(IP,IQ)=0.
     END DO
     V(IP,IP)=1.
  END DO
  DO  IP=1,N
     B(IP)=A(IP,IP)
     D(IP)=B(IP)
     Z(IP)=0.
  END DO
  NROT=0
  DO  I=1,max_it
     SM=0.
     DO  IP=1,N-1
        DO IQ=IP+1,N
           SM=SM+ABS(A(IP,IQ))
        END DO
     END DO
     IF(SM.EQ.0.) GOTO 100
     IF(I.LT.4)THEN
        TRESH=0.2*SM/N**2
     ELSE
        TRESH=0.
     ENDIF
     DO  IP=1,N-1
        DO  IQ=IP+1,N
           G=100.*ABS(A(IP,IQ))
           IF((I.GT.4).AND.(ABS(D(IP))+G.EQ.ABS(D(IP))) &
                .AND.(ABS(D(IQ))+G.EQ.ABS(D(IQ))))THEN
              A(IP,IQ)=0.
           ELSE IF(ABS(A(IP,IQ)).GT.TRESH)THEN
              H=D(IQ)-D(IP)
              IF(ABS(H)+G.EQ.ABS(H))THEN
                 T=A(IP,IQ)/H
              ELSE
                 THETA=0.5*H/A(IP,IQ)
                 T=1./(ABS(THETA)+SQRT(1.+THETA**2))
                 IF(THETA.LT.0.)T=-T
              ENDIF
              C=1./SQRT(1+T**2)
              S=T*C
              TAU=S/(1.+C)
              H=T*A(IP,IQ)
              Z(IP)=Z(IP)-H
              Z(IQ)=Z(IQ)+H
              D(IP)=D(IP)-H
              D(IQ)=D(IQ)+H
              A(IP,IQ)=0.
              DO J=1,IP-1
                 G=A(J,IP)
                 H=A(J,IQ)
                 A(J,IP)=G-S*(H+G*TAU)
                 A(J,IQ)=H+S*(G-H*TAU)
              END DO
              DO  J=IP+1,IQ-1
                 G=A(IP,J)
                 H=A(J,IQ)
                 A(IP,J)=G-S*(H+G*TAU)
                 A(J,IQ)=H+S*(G-H*TAU)
              END DO
              DO  J=IQ+1,N
                 G=A(IP,J)
                 H=A(IQ,J)
                 A(IP,J)=G-S*(H+G*TAU)
                 A(IQ,J)=H+S*(G-H*TAU)
              END DO
              DO  J=1,N
                 G=V(J,IP)
                 H=V(J,IQ)
                 V(J,IP)=G-S*(H+G*TAU)
                 V(J,IQ)=H+S*(G-H*TAU)
              END DO
              NROT=NROT+1
           ENDIF
        END DO
     END DO
     DO  IP=1,N
        B(IP)=B(IP)+Z(IP)
        D(IP)=B(IP)
        Z(IP)=0.
     END DO
  END DO
  write(6,*) max_it,' iterations should never happen'
  STOP
100 DEALLOCATE(B,Z)
  RETURN
END SUBROUTINE JACOBI
!
!============================================================
!c
!c ++  Sort Eigenvectors in ascending eigenvalues order
!c
!c============================================================
!c
SUBROUTINE trie(nb_niv,ene,psi,nreal)

  !
  ! Input:
  !   Nb_niv      :  Dimension of Psi
  !   NReal  : Actual dimensions of Psi, Ene
  ! Input/output:
  !   Psi(N,N) : Eigvenvectors, changed in output. Stored in a (Nreal,Nreal) matrix
  !   Ene(N)   : Eigenvalues, changed in output. Stored in Ene(NReal)
  !   
!!!!!!!!!!!!!!!!

  Use VarTypes
  IMPLICIT NONE

  integer(KINT) :: i,j,k,nb_niv,max_niv, nreal
  real(KREAL) :: ene(nreal),psi(nreal,nreal)
  real(KREAL) :: a


!!!!!!!!!!!!!!!!


  DO i=1,nb_niv
     DO j=i+1,nb_niv
        IF (ene(i) .GT. ene(j)) THEN
           !              permutation
           a=ene(i)
           ene(i)=ene(j)
           ene(j)=a

           DO k=1,nb_niv
              a=psi(k,i)
              psi(k,i)=psi(k,j)
              psi(k,j)=a
           END DO
        END IF
     END DO
  END DO

END SUBROUTINE trie

!============================================================
!c
!c ++  Sort Eigenvectors in ascending eigenvalues order
!c
!c============================================================
!c
SUBROUTINE SortEigenSys(N,Eigval,Eigvec)


  !
  ! Input/output:
  !   N : dimension of the system
  !   EigVec(N,N) : Eigvenvectors, changed in output. Stored in a (N,N) matrix
  !   EigVal(N)   : Eigenvalues, changed in output. Stored in a (n) vector
  !   
 ! Process: 
 ! We use first a ranking, then a working array to reorder the eigenvalues and eigenvectors
 
!!!!!!!!!!!!!!!!

  Use VarTypes
  use m_mrgrnk
  IMPLICIT NONE


  INTEGER(KINT), INTENT(IN) :: N
  REAL(KREAL), INTENT(OUT) :: EigVal(N), Eigvec(N,N)

  integer(KINT) :: i,j,k

  INTEGER(KINT), ALLOCATABLE :: Rank(:) !N
  REAL(KREAL), ALLOCATABLE :: EigValT(:) !n
  REAL(KREAL), ALLOCATABLE :: EigVecT(:,:) !n,n

!!!!!!!!!!!!!!!!
  ALLOCATE(Rank(n),EigValT(n),EigVecT(n,n))
  CALL MrgRnk(EigVal,Rank)

  DO I=1,N
     EigValT(I)=EigVal(Rank(I))
     EigVecT(I,1:N)=EigVec(Rank(I),1:N)
  END DO
  EigVal=EigValT
  EigVec=EigVecT

  DEALLOCATE(Rank,EigValT,EigVecT)


END SUBROUTINE SortEigenSys

