# +----------------------------------------------------+
# |                  Lekce 10 - demo 2                 |
# +----------------------------------------------------+
#  Linearni algebra - numpy
#   diagonalizace Jacobiho metodou

import numpy as np
#import scipy.linalg as la
import matplotlib.pyplot as plt

#Testik na matici jejiz diagonalizaci zname
N=50
q=np.linalg.qr(np.random.rand(N,N))[0]  #konstrukce nahodne ortogonalni matice pomoci qr-faktorizace
d=np.random.rand(N)
d.sort()                                #zavolani tridici metody
a=np.dot(np.dot(q,np.diag(d)),q.transpose())

#Mapovani hodnot matice na integery pro zobrazovani
def ZobrFce(x):
  if (x==0):
    h=-15
  else:
    h=np.log10(abs(x))
  if (h<-15):
    h=-15
  if (h>0):
    h=0
  return h

#Zobrazeni matice v logskale
def ZobrMat(a):
  N=a.shape[0]
  z=np.zeros([N+2,N+2],dtype=np.int)-15
  for i in range(N):
    for j in range(N):
      z[i+1,j+1]=ZobrFce(a[i,j])
  plt.contourf(z[::-1,:])
  #alternativa: plt.spy(a)
  plt.show()

ZobrMat(a)

#Pomocna procedura: Givensova rotace - modifikuje matici
#  -> nesmi byt i=j (netestuji)
#  -> tohle je specialni pripad, kdy phi je voleno, aby a[i,j] vynulovalo
def Givens(a,p,q):
  n=a.shape[0]
  if (abs(a[p,q])>1e-20):
    theta=0.5*(a[q,q]-a[p,p])/a[p,q]
    t=1.0/(abs(theta)+np.sqrt(1.0+theta**2))
    if (theta<0.0):
      t=-t
    c=1.0/np.sqrt(1+t**2)
    s=t*c
    tau=s/(1+c)
    a[p,p], a[q,q] = a[p,p]-t*a[p,q], a[q,q]+t*a[p,q]
    a[p,q], a[q,p] = 0.0, 0.0
    for r in range(n):
      if ((r!=p)and(r!=q)):
        a[r,p], a[r,q] = a[r,p]-s*(a[r,q]+tau*a[r,p]), a[r,q]+s*(a[r,p]-tau*a[r,q]) 
        a[q,r]=a[r,q]
        a[p,r]=a[r,p]
  
#Diagonalizace Jacobiho metodou - vlastni implementace pro demo
#  rutina modifikuje puvodni matici az do diagonalni, nevraci transf q
def JacobiDiagMax(a):
  n=a.shape[0]
  nsweep=(n*(n-1))//2
  max_val=10.0
  s=0
  ng=0
  while (max_val>1e-15):   # Cyklus pres Givensovy rotace
    ng=ng+1
    mm=np.argmax(abs(np.triu(a,1))) #nalezeni souradnice maxima nad diagonalou
    i=mm//n #celocis. deleni
    j=mm%n  #zbytek
    max_val=abs(a[i,j])
    Givens(a,i,j)
    if (ng%nsweep==0):
      s=s+1
      print(s,max_val)
      ZobrMat(a)
  return np.diag(a).copy()

d3=JacobiDiagMax(a)
d3.sort()
print('max. rozdil vl. cisel: ',max(abs(d3-d)))