! ifort:
! ifort multidimensional-scaling_gelsy.f90 $MKLROOT/lib/intel64/libmkl_lapack95_lp64.a 
!   -mkl -init=snan -C -warn -fpe0 -traceback
! gfortran:
! yum install lapack blas, und lapack95 von netlib downloaden, make.inc von linux-redhat nehmen,
! und lapack95_modules in /usr/local/include sowie lapack95.a in /usr/local/lib64 kopieren.
! gfortran -O -I/usr/local/include multidimensional-scaling_gelsy.f90 -L/usr/local/lib64 -ltrlan -llapack -lblas
! bzw mit https://software.intel.com/en-us/node/528569 MKL-komptible interfaces

!PROGRAM mds
! generate test data for solve_mds
!IMPLICIT NONE
!REAL, ALLOCATABLE :: distance(:,:),city(:,:),result(:,:),select(:),saveresult(:,:)
!INTEGER, ALLOCATABLE :: purpose(:,:)
!REAL :: error,forget,phi
!INTEGER :: idim,n,i,j,jdim

!PRINT*,'enter n,idim,forget,jdim'
!READ*,n,idim,forget,jdim
!PRINT*,'n,idim,forget,jdim=',n,idim,forget,jdim

!ALLOCATE(distance(n,n),purpose(n,n),city(idim,n),select(n),result(jdim,n))
! create example data from vectors with dimension idim
!CALL RANDOM_NUMBER(city)
!city=100*city
!DO j=2,idim
!  city(j,:)=city(j,:)/j**2
!END DO
!CALL wpdb(city,n,idim,'city.pdb')
!city=-city ! this does not change the scalar products
!CALL wpdb(city,n,idim,'city-inv.pdb')

! generate matrix of scalar products:
!DO i=1,n
!  DO j=i+1,n
!    call RANDOM_NUMBER(error)
!    error=error*10-5
!    distance(j,i)=DOT_PRODUCT(city(:,i),city(:,j)) +error 
!    distance(i,j)=distance(j,i)
!  END DO
!END DO

! declare some matrix elements as unknown
!purpose=0             ! at first, all matrix elements declared known and useful
!DO i=1,n
!  purpose(i,i)=-1     ! diagonal is always unknown
!  CALL RANDOM_NUMBER(select(i+1:))
!  DO j=i+1,n
!    IF (select(j)>forget) CYCLE
!    purpose(j,i)=-1   ! unknown matrix elements
!    purpose(i,j)=purpose(j,i)
!  END DO
!END DO

!phi=0.
!result=0.
!PRINT*,'finding',n,jdim,'-dimensional vectors from',COUNT(purpose==0)/2,' values'
!CALL solve_mds(n,jdim,distance,purpose,result,phi)
!
! align result with x,y,z axes
!CALL inertia_align(n,jdim,result)
! make the first result vector have positive components in all directions
!DO i=1,jdim
!  IF (result(i,1)<0) result(i,:)=-result(i,:) ! this conserves scalar products
!END DO
!
!CALL wpdb(result,n,jdim,'result.pdb')
!PRINT*,'jdim,phi=',jdim,phi
!ALLOCATE(saveresult(jdim,n))
!saveresult=result
!DEALLOCATE(result)
!ALLOCATE(result(jdim+1,n))
!result=0
!result(:jdim,:)=saveresult
!phi=SQRT(MAXVAL(ABS(distance)))/10
!
!CALL solve_mds(n,jdim+1,distance,purpose,result,phi)
!CALL wpdb(result,n,jdim+1,'result1.pdb')
!PRINT*,'jdim,phi=',jdim+1,phi
!DEALLOCATE(saveresult)
!ALLOCATE(saveresult(jdim+1,n))
!saveresult=result
!DEALLOCATE(result)
!ALLOCATE(result(jdim+2,n))
!result=0
!result(:jdim+1,:)=saveresult
!phi=SQRT(MAXVAL(ABS(distance)))/10
!
!CALL solve_mds(n,jdim+2,distance,purpose,result,phi)
!CALL wpdb(result,n,jdim+2,'result2.pdb')
!PRINT*,'jdim,phi=',jdim+2,phi
!
!END

SUBROUTINE solve_mds(n,idim,skalarprod,property,result,phi)
! if phi>0 on input, then starting values of trials will be calculated as
! result + phi * random_number 
! else they will be calculated as
! SQRT(MAXVAL(ABS(skalarprod))) * random_number
IMPLICIT NONE
INTEGER, INTENT(IN) :: n,idim
REAL, INTENT(IN) :: skalarprod(n,n)
INTEGER, INTENT(IN) :: property(n,n)
REAL, INTENT(INOUT) :: result(idim,n)
REAL, INTENT(INOUT)   :: phi
! local variables
INTEGER, ALLOCATABLE :: purpose(:,:)
REAL, ALLOCATABLE :: bestresult(:,:),start(:,:)
REAL :: error,rmult,omega,bestomega,bestphi,odfactor
INTEGER i,j,nfree,nset
LOGICAL found

 ALLOCATE(purpose(n,n),bestresult(idim,n),start(idim,n))
 purpose=property
! modify purpose: set 10% of known matrix elements aside
 nfree=0
 nset=0
 DO i=1,n-1
   DO j=i+1,n
     IF (purpose(j,i)==0) THEN
       nset=nset+1
       CALL RANDOM_NUMBER(error)
       IF (error<0.1) THEN   ! set aside as "free"
         nfree=nfree+1
         purpose(j,i)=1
         purpose(i,j)=purpose(j,i)  ! symmetric
       END IF
     END IF
   END DO
 END DO
 nset=nset-nfree
! how should starting values be calculated?
 IF (phi>0.) THEN
   rmult=phi
   start=result
 ELSE
! estimate the magnitude of the elements of result
   rmult=SQRT(MAXVAL(ABS(skalarprod),MASK=property==0))
   PRINT*,'rmult=',rmult
!do i=1,n
!do j=1,n
!if (property(j,i)==0.AND.ABS(skalarprod(j,i))>1) print*,i,j,property(j,i),skalarprod(j,i)
!end do
!end do
   start=0.
 END IF
! try to obtain a solution 
 bestomega=huge(bestomega)/2
 bestphi=huge(bestphi)/2
 found=.FALSE.
 odfactor=COUNT(purpose==0)/2./n/idim
 print*,'will do ',INT(1000/odfactor)+1,' trials'
!$OMP PARALLEL DO DEFAULT(none) PRIVATE(i,result,phi,omega) &
!$OMP SHARED(n,nfree,nset,bestphi,bestresult,purpose,bestomega,idim,skalarprod,rmult,found,odfactor,start) 
 DO i=1,INT(1000/odfactor)+1
   IF (.NOT.found) THEN   ! only way before OpenMP 4.0 to cancel iterations; CYCLE and EXIT disallowed
     CALL RANDOM_NUMBER(result)
     result=start + rmult*result ! if solution is in the first quadrant - all elements positive !!!
     CALL mds_scalarproduct_iter(n,idim,skalarprod,result,purpose,phi,omega)
!$omp critical
     IF (omega+phi<bestomega+bestphi) THEN
       bestomega=omega
       bestphi=phi
       bestresult=result
       PRINT*,'solution improved!',i,omega/MAX(1,nfree),phi/nset,idim
       IF (bestphi/SUM(skalarprod**2,MASK=purpose==0)<1.e-5) found=.TRUE.
     END IF
!$omp end critical
   END IF
 END DO
 PRINT*,'relative error of matrix element estimates=', &
               bestphi/SUM(skalarprod**2,MASK=purpose==0),           &
               bestomega/MAX(1.,SUM(skalarprod**2,MASK=purpose==1))
 result=bestresult   ! return this value to caller
 phi=bestphi+bestomega  ! return this value to caller
! align result with x,y,z axes
! CALL inertia_align(n,idim,result)
! make the first result vector have positive components in all directions
! DO i=1,idim
!   IF (result(i,1)<0) result(i,:)=-result(i,:) ! this conserves scalar products
! END DO
END SUBROUTINE solve_mds


SUBROUTINE solve_mds_neu(n,idim,amat,purpose,result)
! 10.7.17 KD determine result vectors
      USE lapack95, ONLY: syevr  ! ifort/MKL
      IMPLICIT NONE
      INTEGER, INTENT(IN) :: n,idim
      REAL, INTENT(IN)    :: amat(n,n)
      INTEGER, INTENT(IN) :: purpose(n,n)
      REAL, INTENT(INOUT) :: result(idim,n)
      INTEGER :: i,j,k,m,indexworst,taken_care,taken_care_total=0,previous,info !it_num,num_rot
      REAL, ALLOCATABLE   :: x(:),y(:)
      INTEGER, ALLOCATABLE :: nwrong(:),nright(:)
      REAL x1,y1
      REAL(8), ALLOCATABLE :: a8(:,:),v(:,:),d(:)

      ALLOCATE(x(n),y(n),nwrong(n),nright(n),a8(n,n),v(n,n),d(n)) 

! -------- find approximate absolute values for the x -------------------
!
      x=0
      DO i=1,n
        m=0
        DO j=1,n
!          IF (amat(j,i)>2) CYCLE    ! skip unknowns
          IF (purpose(j,i)/=0) CYCLE    ! skip unknowns
          x(i) = x(i) + ABS(amat(j,i))
          m = m + 1
        END DO
        IF (m>0) THEN 
          x(i) = x(i)/m
        ELSE
          STOP 'x(i) undefined'
        END IF
      END DO
! scale these x estimates by taking into account the average of the x that they
! are multiplied with
      x = x/SQRT(SUM(x)/n)
! prevent x from being >1
      x = MIN(1.,x)
      
! -------------------find the sign of the x ----------------------------------
! 
! for each x, count number of positive and negative matrix elements 
      nright=0
      nwrong=0
      previous=0
      DO i=1,n
        DO j=1,n
!          IF (amat(j,i)>2) CYCLE    ! skip unknowns
          IF (purpose(j,i)/=0) CYCLE    ! skip unknowns
          IF (amat(j,i)>=0) THEN
            nright(i)=nright(i)+1
          ELSE
            nwrong(i)=nwrong(i)+1
          END IF
        END DO
      END DO
      
! find x(i) where sign flipping yields the largest number of corrected signs
      DO k=1,n                      ! n trials
        taken_care=-1
        indexworst=0
        DO i=1,n                    ! find x with highest nwrong-nright
          IF (nwrong(i)-nright(i)>taken_care) THEN  ! this is a candidate
            taken_care=nwrong(i)-nright(i)
            indexworst=i
          END IF
        END DO
        IF (indexworst==0.OR.indexworst==previous) EXIT
        x(indexworst)=-x(indexworst)
        previous=indexworst
! bookkeeping:
! a) exchange nright and nwrong for this x
        j=nright(indexworst)
        nright(indexworst)=nwrong(indexworst)
        nwrong(indexworst)=j
! b) adjust nright and nwrong of those x that interact with indexworst
        DO i=1,n
!          IF (amat(i,indexworst)>2) CYCLE    ! skip unknowns
          IF (purpose(i,indexworst)/=0) CYCLE    ! skip unknowns
          IF (x(i)*x(indexworst)*amat(i,indexworst)<0) THEN
            j=1
          ELSE
            j=-1
          END IF
          nright(i) = nright(i) - j
          nwrong(i) = nwrong(i) + j 
        END DO
        taken_care_total=taken_care_total + taken_care
      END DO
      print*,'taken_care_total, # negatives =',taken_care_total*2,COUNT(amat<0.AND.purpose==0)

! -------------- iteratively improve result -------------
      DO k=1,3   ! 3 iterations
      DO i=1,n
        x1=0
        y1=0
        DO j=1,n
!          IF (amat(j,i)>2) CYCLE
          IF (purpose(j,i)/=0) CYCLE    ! skip unknowns
          x1 = x1 + x(j)*amat(j,i)
          y1 = y1 + x(j)**2
        END DO
        x(i)=x1/y1
      END DO
      END DO

! prepare matrix by filling in missing values
      a8=amat
      k=0
      DO i=1,n
        DO j=i,n
          IF (purpose(j,i)/=0) THEN    ! fill in unknowns
            a8(j,i)=x(i)*x(j)
            a8(i,j)=a8(j,i)
          ELSE
            k=k+1                      ! count known values
          END IF
        END DO
      END DO
      
! ----------- get eigenvalues and -vectors  --
! CALL jacobi_eigenvalue ( n, a8, 100, v, d, it_num, num_rot )
! for SYEVR, see https://software.intel.com/de-de/node/469188
      CALL syevr(a=a8,w=d,z=v,il=n-idim+1,info=info)   ! d=eigenvalues v=eigenvectors 
! print*,'it_num,num_rot=',it_num,num_rot
 IF (info/=0) print*,'info=',info
 print '(a,10f10.3)','    syevr eigenvalues=',d(:idim)
 d(:idim-1)=d(:idim-1)*(n*n+n)/(2.*k)  ! scale up all eigenvalues except highest
 print '(a,10f10.3)','new syevr eigenvalues=',d(:idim)
 DO j=1,idim
   DO i=1,n
     result(j,i)= SQRT(MAX(0.,d(idim+1-j)))*v(i,idim+1-j) ! scale Eigenvectors by sqrt(Eigenvalue)
!     result(j,i)=SQRT(MAX(0.,d(n+1-j))) * v(i,n+1-j)
   END DO
 END DO

! improve the solution by only using given values, not estimated ones
 CALL mds_scalarproduct_iter_neu(n,idim,amat,result,purpose)

END SUBROUTINE solve_mds_neu

SUBROUTINE inertia_align(n,idim,xvec)
! routine to align n vectors of dimension idim with the axes
USE lapack95, ONLY: syevr  ! ifort/MKL
IMPLICIT NONE
INTEGER, INTENT(IN) :: n,idim
REAL, INTENT(INOUT) :: xvec(idim,n)
! local variables
REAL, ALLOCATABLE :: tensor(:,:),eigenvalues(:),eigenvectors(:,:)
INTEGER i,j,k,l

 ALLOCATE(tensor(idim,idim),eigenvalues(idim),eigenvectors(idim,idim))
! Trägheitstensor:  https://de.wikipedia.org/wiki/Tr%C3%A4gheitstensor
 tensor=0
 DO k=1,n
   DO i=1,idim
     DO j=1,idim
       IF (i==j) THEN   ! diagonal elements
         DO l=1,idim
           IF (l/=i) tensor(j,i)=tensor(j,i)+xvec(l,k)**2
         END DO
       ELSE             ! off-diagonal elements
         tensor(j,i)=tensor(j,i)-xvec(j,k)*xvec(i,k)
       END IF
     END DO
   END DO
 END DO
 CALL SYEVR(tensor,w=eigenvalues,z=eigenvectors,info=i) ! https://software.intel.com/en-us/node/469188
 IF (i/=0) THEN
   PRINT*,'SYEVR problem, info=',i
 END IF
 PRINT*,'Eigenvalues:',eigenvalues
 PRINT*,'Eigenvektoren:'
 DO i=1,idim
   PRINT*,eigenvectors(:,i)
 END DO
! rotate:
! DO i=idim,1,-1            ! reverse order: highest eigenvalue first
!   tensor(:,idim-i+1)=eigenvectors(:,i)
! END DO
 tensor=TRANSPOSE(eigenvectors)
 xvec = MATMUL(tensor,xvec)
! diese 4 Zeilen machen Mist und sind nicht nötig:
! CALL SYEVR(tensor,w=eigenvalues,info=i)
! PRINT*,'Eigenvalues of rotation matrix:',eigenvalues
! PRINT*,'Determinant of rotation matrix:',PRODUCT(eigenvalues)
! IF (PRODUCT(eigenvalues)<0) xvec(1,:)=-xvec(1,:) 
END SUBROUTINE inertia_align

SUBROUTINE wpdb(xyz,n,idim,fn,segid)
      implicit none
      integer i,idim,n,unit
      real xyz(idim,n)
      character*(*) fn
      character segid*1
! 102   format('ATOM',i7,'  CA  SER ',a1,i4,t31,3f8.3,t60,f6.2,t55,f6.2)
 102  format('HETATM',i5,'  O   HOH ',a1,i4,t31,3f8.3,t61,f6.2,t55,f6.2)
 
      open(newunit=unit,file=fn)
      write(unit,'(a)') 'CRYST1  100.000  100.000  100.000  90.00  90.00  90.00 P 1'
      do i=1,n
        IF (idim==1) THEN
          write(unit,102) i,segid,i,xyz(1,i),0.,0.,100.,1.
        ELSE IF (idim==2) THEN
          write(unit,102) i,segid,i,xyz(1,i),xyz(2,i),0.,100.,1.
        ELSE IF (idim==3) THEN
          write(unit,102) i,segid,i,xyz(1,i),xyz(2,i),xyz(3,i),100.,1.
        ELSE IF (idim==4) THEN
          write(unit,102) i,segid,i,xyz(1,i),xyz(2,i),xyz(3,i),100+xyz(4,i),1.
        ELSE 
          write(unit,102) i,segid,i,xyz(1,i),xyz(2,i),xyz(3,i),xyz(4,i),(100+xyz(5,i))/100.
        END IF
      end do
      PRINT*,'wrote ',fn(:LEN_TRIM(fn))
      CLOSE(unit)
END SUBROUTINE wpdb


SUBROUTINE mds_scalarproduct_iter(n,idim,skalarprod,result,purpose,phi,omega)
! Given a n * n matrix "skalarprod", find idim-dimensional vectors "result" whose 
! scalar products matches the matrix elements in the least-squares sense
! The program returns the best solution identified by the lowest "free" bestomega (or if that is
! not available because no purpose>0 is defined, identified by the lowest bestphi)
! The iterations are stopped if they do not improve bestomega (or bestphi).
!
! purpose<0  means: unknown matrix element, should be skipped
! purpose=0  means: fit this matrix element and report average squared error in bestphi
! purpose>0  means: calculate matrix element from result; report average squared error in bestomega
USE lapack95, ONLY: gelsy  ! ifort/MKL
!USE blas95, ONLY: dot ! ifort/MKL
!USE F95_LAPACK , ONLY: LA_GELSY
IMPLICIT none
INTEGER, INTENT(IN) :: n,idim
REAL, INTENT(IN) :: skalarprod(n,n)
INTEGER, INTENT(IN) :: purpose(n,n)
REAL, INTENT(INOUT) :: result(idim,n)
REAL, INTENT(OUT) :: phi,omega
! local variables
REAL, ALLOCATABLE :: temp(:,:),b(:)
REAL bestphi,bestomega,xx
INTEGER i,j,k,l,irank,info,lstop
!INTEGER nphi,nomega
LOGICAL, PARAMETER :: debug=.false.

ALLOCATE(temp(n,idim),b(n))
lstop=-1
bestphi=huge(bestphi)
bestomega=huge(bestomega)
DO l=1,20   ! max 20 iterations
! calculate the error function
  phi=0.
  omega=0.    ! free set !
!  nphi=0
!  nomega=0
  DO j=1,n-1
    DO k=j+1,n
      IF (purpose(j,k)==0) THEN
        phi=phi    +(DOT_PRODUCT(result(:,j),result(:,k))-skalarprod(j,k))**2
!        nphi=nphi+1
      ELSE IF (purpose(j,k)>0) THEN
        omega=omega+(DOT_PRODUCT(result(:,j),result(:,k))-skalarprod(j,k))**2
!        nomega=nomega+1
      END IF
    END DO
  END DO
  IF (debug) PRINT*,l,omega,phi
! find out whether to stop
  IF (omega>0) THEN    ! there are "free" data
    IF (omega<bestomega) THEN
      bestomega=omega
      lstop=-1
    ELSE
! exit two iterations later; in many cases this will make omega increase a lot
! so will signal to the calling routine that this is not a good solution
      IF (lstop<0) lstop=l+2  
    END IF
  ELSE
    IF (phi<bestphi) THEN
      bestphi=phi
    ELSE
      EXIT
    END IF
  END IF
  IF (l==lstop.or.l==20) EXIT
  DO i=1,n
! solve overdetermined system of eqns A * x = skalarprod, and iterate this
    k=0
    DO j=1,n
      IF (purpose(i,j)<0) CYCLE
      IF (i==j) CYCLE
      k=k+1
      temp(k,:)=result(:,j)
      b(k)=skalarprod(i,j)
    END DO 
    IF (k>=idim) THEN
! for gelsy, see https://software.intel.com/en-us/node/469162
      CALL gelsy(a=temp(1:k,:),b=b(:k),rank=irank,info=info)  !,rank=,jpvt=,rcond=1.e-5,info=info) 
      IF (irank>0.AND.info==0) THEN
        result(:,i)=b(:idim)    
! KD 19.10.15 limit length of result to < 1 (correlation coefficient!!)
xx=DOT_PRODUCT(b(:idim),b(:idim))
IF (xx>1) result(:,i)=b(:idim)/SQRT(xx)  
!        IF (irank<idim) PRINT*,'underdetermined i,irank:',i,irank
!      ELSE
!   MKL prints an error msg by itself!     PRINT*,'undetermined i,irank,info:',i,irank,info
      END IF
    ELSE
      IF (l==1) PRINT*,'cannot determine item',i
      result(:,i)=0
    END IF
  END DO
END DO
IF (debug) PRINT*,'stopped at l=',l
END SUBROUTINE mds_scalarproduct_iter

SUBROUTINE mds_scalarproduct_iter_neu(n,idim,skalarprod,result,purpose)
! Given a n * n matrix "skalarprod", return idim-dimensional vectors "result" whose 
! scalar products matches the matrix elements in the least-squares sense
! The program returns the best solution identified by the lowest bestphi
!
! purpose<0  means: unknown matrix element, should be skipped
! purpose=0  means: fit this matrix element and report average squared error in bestphi
USE lapack95, ONLY: gelsy  ! ifort/MKL
!USE blas95, ONLY: dot ! ifort/MKL
!USE F95_LAPACK , ONLY: LA_GELSY
USE qsort_m, ONLY: myqsort
IMPLICIT none
INTEGER, INTENT(IN) :: n,idim
REAL, INTENT(IN) :: skalarprod(n,n)
INTEGER, INTENT(IN) :: purpose(n,n)
REAL, INTENT(INOUT) :: result(idim,n)
! local variables
REAL(8), ALLOCATABLE :: temp(:,:),b(:),result8(:,:)
REAL(8) phi,bestphi,xx
INTEGER i,j,k,l,irank,info,nphi
LOGICAL, PARAMETER :: debug=.TRUE.

ALLOCATE(temp(n,idim),b(n),result8(idim,n))
!DO i=1,n
!print '(7f9.3)',skalarprod(i,:)
!END DO
!DO i=1,n
!print '(7f9.3)',result(:,i)
!END DO

! make sure vectors are not longer than 1
l=0
DO i=1,n
  xx=DOT_PRODUCT(result(:,i),result(:,i))
  IF (xx>1) THEN
    result(:,i)=result(:,i)/SQRT(xx)
    l=l+1
  END IF
END DO
IF (l>0) PRINT*,l,' vectors were shortened'
result8=result
bestphi=huge(bestphi)
DO l=1,20   ! max 20 iterations
! calculate the error function
  phi=0.
  nphi=0
  DO j=1,n-1
    DO k=j+1,n
      IF (purpose(j,k)==0) THEN
        xx = (DOT_PRODUCT(result8(:,j),result8(:,k))-skalarprod(j,k))**2
        phi=phi + xx
        nphi=nphi+1
      END IF
    END DO
  END DO
  IF (debug) PRINT*,'l,phi,phi/n=',l,phi,phi/nphi
! find out whether to stop
  IF (phi<bestphi) THEN
    bestphi=phi
    result=result8
  ELSE
    EXIT
  END IF
  IF (l==20) EXIT
  DO i=1,n
! solve overdetermined system of eqns A * x = skalarprod, and iterate this
    k=0
    DO j=1,n
      IF (purpose(i,j)<0) CYCLE
      IF (i==j) CYCLE
      k=k+1
      temp(k,:)=result8(:,j)
      b(k)=skalarprod(i,j)
    END DO 
    IF (k>=idim) THEN
! for gelsy, see https://software.intel.com/en-us/node/469162
      CALL gelsy(a=temp(1:k,:),b=b(:k),rank=irank,info=info)  !,rank=,jpvt=,rcond=1.e-5,info=info) 
      IF (irank>0.AND.info==0) THEN
        result8(:,i)=b(:idim)    
! KD 19.10.15 limit length of result8 to <= 1 (correlation coefficient!!)
        xx=DOT_PRODUCT(b(:idim),b(:idim))
        IF (xx>1) result8(:,i)=b(:idim)/SQRT(xx)  
      ELSE
!   MKL prints an error msg by itself!     
        PRINT*,'undetermined i,irank,info:',i,irank,info
      END IF
    ELSE
      IF (l==1) PRINT*,'cannot determine item',i
      result8(:,i)=0
    END IF
  END DO
END DO
IF (debug) PRINT*,'stopped iterations at l=',l
!DO i=1,n
!print '(7f9.3)',result(:,i)
!END DO
END SUBROUTINE mds_scalarproduct_iter_neu

subroutine jacobi_eigenvalue ( n, a, it_max, v, d, it_num, rot_num )
! https://people.sc.fsu.edu/~jburkardt/f_src/jacobi_eigenvalue/jacobi_eigenvalue.f90

!*****************************************************************************80
!
!! JACOBI_EIGENVALUE carries out the Jacobi eigenvalue iteration.
!
!  Discussion:
!
!    This function computes the eigenvalues and eigenvectors of a
!    real symmetric matrix, using Rutishauser's modfications of the classical
!    Jacobi rotation method with threshold pivoting. 
!
!  Licensing:
!
!    This code is distributed under the GNU LGPL license.
!
!  Modified:
!
!    17 September 2013
!
!  Author:
!
!    FORTRAN90 version by John Burkardt
!
!  Parameters:
!
!    Input, integer ( kind = 4 ) N, the order of the matrix.
!
!    Input, real ( kind = 8 ) A(N,N), the matrix, which must be square, real,
!    and symmetric.
!
!    Input, integer ( kind = 4 ) IT_MAX, the maximum number of iterations.
!
!    Output, real ( kind = 8 ) V(N,N), the matrix of eigenvectors.
!
!    Output, real ( kind = 8 ) D(N), the eigenvalues, in descending order.
!
!    Output, integer ( kind = 4 ) IT_NUM, the total number of iterations.
!
!    Output, integer ( kind = 4 ) ROT_NUM, the total number of rotations.
!
  implicit none

  integer ( kind = 4 ),INTENT(IN) :: n

  real ( kind = 8 ),INTENT(INOUT) :: a(n,n)
  real ( kind = 8 ) bw(n)
  real ( kind = 8 ) c
  real ( kind = 8 ),INTENT(OUT) :: d(n)
  real ( kind = 8 ) g
  real ( kind = 8 ) gapq
  real ( kind = 8 ) h
  integer ( kind = 4 ) i
  integer ( kind = 4 ),INTENT(IN) :: it_max
  integer ( kind = 4 ),INTENT(OUT) :: it_num
  integer ( kind = 4 ) j
  integer ( kind = 4 ) k
  integer ( kind = 4 ) l
  integer ( kind = 4 ) m
  integer ( kind = 4 ) p
  integer ( kind = 4 ) q
  integer ( kind = 4 ),INTENT(OUT) :: rot_num
  real ( kind = 8 ) s
  real ( kind = 8 ) t
  real ( kind = 8 ) tau
  real ( kind = 8 ) term
  real ( kind = 8 ) termp
  real ( kind = 8 ) termq
  real ( kind = 8 ) theta
  real ( kind = 8 ) thresh
  real ( kind = 8 ),INTENT(OUT) :: v(n,n)
  real ( kind = 8 ) w(n)
  real ( kind = 8 ) zw(n)

  do j = 1, n
    do i = 1, n
      v(i,j) = 0.0D+00
    end do
    v(j,j) = 1.0D+00
  end do

  do i = 1, n
    d(i) = a(i,i)
  end do

  bw(1:n) = d(1:n)
  zw(1:n) = 0.0D+00
  it_num = 0
  rot_num = 0

  do while ( it_num < it_max )

    it_num = it_num + 1
!
!  The convergence threshold is based on the size of the elements in
!  the strict upper triangle of the matrix.
!
    thresh = 0.0D+00
    do j = 1, n
      do i = 1, j - 1
        thresh = thresh + a(i,j) ** 2
      end do
    end do

    thresh = sqrt ( thresh ) / real ( 4 * n, kind = 8 )

    if ( thresh == 0.0D+00 ) then
      exit 
    end if

    do p = 1, n
      do q = p + 1, n

        gapq = 10.0D+00 * abs ( a(p,q) )
        termp = gapq + abs ( d(p) )
        termq = gapq + abs ( d(q) )
!
!  Annihilate tiny offdiagonal elements.
!
        if ( 4 < it_num .and. &
             termp == abs ( d(p) ) .and. &
             termq == abs ( d(q) ) ) then

          a(p,q) = 0.0D+00
!
!  Otherwise, apply a rotation.
!
        else if ( thresh <= abs ( a(p,q) ) ) then

          h = d(q) - d(p)
          term = abs ( h ) + gapq

          if ( term == abs ( h ) ) then
            t = a(p,q) / h
          else
            theta = 0.5D+00 * h / a(p,q)
            t = 1.0D+00 / ( abs ( theta ) + sqrt ( 1.0D+00 + theta * theta ) )
            if ( theta < 0.0D+00 ) then 
              t = - t
            end if
          end if

          c = 1.0D+00 / sqrt ( 1.0D+00 + t * t )
          s = t * c
          tau = s / ( 1.0D+00 + c )
          h = t * a(p,q)
!
!  Accumulate corrections to diagonal elements.
!
          zw(p) = zw(p) - h                  
          zw(q) = zw(q) + h
          d(p) = d(p) - h
          d(q) = d(q) + h

          a(p,q) = 0.0D+00
!
!  Rotate, using information from the upper triangle of A only.
!
          do j = 1, p - 1
            g = a(j,p)
            h = a(j,q)
            a(j,p) = g - s * ( h + g * tau )
            a(j,q) = h + s * ( g - h * tau )
          end do

          do j = p + 1, q - 1
            g = a(p,j)
            h = a(j,q)
            a(p,j) = g - s * ( h + g * tau )
            a(j,q) = h + s * ( g - h * tau )
          end do

!DIR$ SIMD
          do j = q + 1, n
            g = a(p,j)
            h = a(q,j)
            a(p,j) = g - s * ( h + g * tau )
            a(q,j) = h + s * ( g - h * tau )
          end do
!
!  Accumulate information in the eigenvector matrix.
!
          do j = 1, n
            g = v(j,p)
            h = v(j,q)
            v(j,p) = g - s * ( h + g * tau )
            v(j,q) = h + s * ( g - h * tau )
          end do

          rot_num = rot_num + 1

        end if

      end do
    end do

    bw(1:n) = bw(1:n) + zw(1:n)
    d(1:n) = bw(1:n)
    zw(1:n) = 0.0D+00

  end do
!
!  Restore upper triangle of input matrix.
!
  do j = 1, n
    do i = 1, j - 1
      a(i,j) = a(j,i)
    end do
  end do
!
!  Ascending sort the eigenvalues and eigenvectors.
!
  do k = 1, n - 1

    m = k

    do l = k + 1, n
      if ( d(l) < d(m) ) then
        m = l
      end if
    end do

    if ( m /= k ) then

      t    = d(m)
      d(m) = d(k)
      d(k) = t

      w(1:n)   = v(1:n,m)
      v(1:n,m) = v(1:n,k)
      v(1:n,k) = w(1:n)

    end if

  end do

  return
end
