! Metoda Jacobi
! Triunghiul superior - stocat in a(n*(n+1)/2)
! Full search
! n - in "work"
subroutine Jacobi(kodVP, kodI, iter, amax)
use work

implicit real(8) (a-h,o-z)

integer p, q
real(8) aip(n), aiq(n)		! temporar pt. off-diagonal
integer width


n1 =n*(n+1)/2	! dim. lui a
aip =0.
aiq =0.

! Search for maximum element in a(n*(n+1)/2);
iter =0
p =1
q =2

100 continue
!if(iter >=lnit) RETURN

amax =0.

k_Do: do k =1, n1
call iloc(k, i, j)
! Avoids diagonals:
if(i==j) cycle

! Element <tol:
!if(abs(a(k)) <tol) cycle k_Do

if(dabs(a(k)) >amax) then
amax =dabs(a(k))
p =i
q =j
endif
enddo k_Do

! Print A and p, q, amax:
if(kodI /=0) then
iwi =width(iter); iwp =width(p); iwq =width(q);
write(1, '(a, I<iwi>)') ' Iteration: ', iter

write(1, '(a)') ' Matrix A:'
do i =1,n
j1 =i; j2 =min(i+n-1, n); nj =j2-j1+1
write(1, '(<nj>g24.16)') (a(loca(i,j)), j= j1,j2)
enddo

write(1, '(a2, I<iwp>, a1, I<iwq>, a1, 1pg24.16)') '(', p, ',', q, ')', amax
endif


if(amax <= tol) then
RETURN
else
iter =iter +1		
endif

! When exiting the cycle k_Do: (p,q) are detemined.
! And: q > p!
apq =a(loca(p,q))
app =a(loca(p,p))
aqq =a(loca(q,q))

! Compute r, sina, cosa
r =dsqrt( 4*apq**2 +(app-aqq)**2 )
sina =dsqrt( (1-(app-aqq)/r)/2 )
if(sina /=0) then
cosa =apq/r/sina
else
cosa =1.
endif

! With sina, cosa, element(p,q)=0.
! Compute new 'a's:
app1 =(app +aqq +r)/2.
aqq1 =(app +aqq -r)/2.


! Col. "p": 1p,2p,...(p-1,p); (p+1,p),... (q,p),...,(n,p)
! i=p: skip
do i=1, n	
if(i <p) then		! i <p <q : 1,...,p-1: vor fi in (i,p)
aip1 =a(loca(i,p))
aiq1 =a(loca(i,q))
aip(i) =aip1*cosa +aiq1*sina

elseif(i==p) then	! skip (p,p)
cycle

else				! i >p: p+1, ..., q,... n: vor fi in (p,i)
if(i ==q) cycle		! skip (q,p)
aip1 =a(loca(p,i))	! A(i,p)=A(p,i)
	if(i<q) then
	aiq1 =a(loca(i,q))	
	else
	aiq1 =a(loca(q,i))
	endif
aip(i) =aip1*cosa +aiq1*sina
endif
enddo


! Col. "q": 1q,2q,...,(p,q),...,(q-1,q); (q,q+1),...(q,n)
! i=q: skip
do i=1, n
if(i <q) then
if(i ==p) cycle		! skip: (p,q)
	if(i<p) then
	aip1 =a(loca(i,p))
	else
	aip1 =a(loca(p,i))
	endif
aiq1 =a(loca(i,q))
aiq(i) =-aip1*sina +aiq1*cosa

elseif(i ==q) then	! skip: (q,q)
cycle

else			! i >q>p
aip1 =a(loca(p,i))
aiq1 =a(loca(q,i))
aiq(i) =-aip1*sina +aiq1*cosa
endif
enddo

! Assign diagonals: Elements (p,q) ... q>p; (p,p), (q,q)
a(loca(p,q)) =0.
a(loca(p,p)) =app1
a(loca(q,q)) =aqq1

! Assign off-diagonals - except (p,q):
! (i,p) or (p,i):
do i=1, p-1
a(loca(i,p)) =aip(i)
enddo

do i=p+1,n
if(i==q) cycle
a(loca(p,i)) =aip(i)
enddo

! (i,q) or (q,i):
do i=1, q-1
if(i==p) cycle
a(loca(i,q)) =aiq(i)
enddo

do i=q+1,n
a(loca(q,i)) =aiq(i)
enddo

! Compute N~X:
if(kodVP /=0) then

! Compute X=X*N
! Only columns "p" and "q" in X, are modified:
! Store original entries of X - columns p & q:
aip(:) =x(:,p)
aiq(:) =x(:,q)

do i=1,n

x(i,p) =aip(i)*cosa +aiq(i)*sina

x(i,q) =-aip(i)*sina +aiq(i)*cosa
enddo

endif


GOTO 100

END subroutine Jacobi
