! Main program for structured eigenvalue computation.
!
! This is a preliminary implementation
! of the method described in the draft
! "Implicit QR with compression",
! by P. Boito, Y. Eidelman, L. Gemignani
! and I. Gohberg.
!
! The proposed method is a variant of
! structured implicit QR, where the
! unitary factors V and F are not explicitly
! used; instead, the generators are
! updated directly and then compressed.
!
! Single shift only for now.
!
! Lapack needs to be installed in order to evaluate
! accuracy of computed eigenvalues.
! The Fortran routine ssort.f is also needed 
! (it can easily be found on the Internet). 
!
! Last update by Paola on Jan 4, 2011.

program main

implicit none

complex(kind(1d0)), allocatable :: myeig(:),poly(:),eig(:),A(:,:)
complex(kind(1d0)), allocatable :: work(:),vL(:,:),vR(:,:) 
real(kind(1d0)) :: w1,w2,our_time,lapack_time,maxdiffeR,maxdiffeI
real(kind(1d0)), allocatable :: rwork(:),dist1(:),dist2(:) 
real(kind(1d0)), allocatable :: eR1(:),eR2(:),eI1(:),eI2(:),seR1(:),seR2(:),seI1(:),seI2(:),diffeR(:),diffeI(:)
integer :: i,j,n,lwork,info
integer :: clock_start,clock_end,clock_rate 

write(*,"(a)",advance="no") "Degree = "
read(*,*) n

allocate(poly(n),myeig(n))
call init_random_seed()


!********************************************************
! Comment/uncomment the following to select a polynomial.
!********************************************************

!*******************************************************
! Monic polynomial with random coefficients
!*******************************************************

 !do i=1,n
 !call random_number(w1)
 !call random_number(w2)
 !poly(i)=(2.d0,0.d0)*w1+(0.d0,2.d0)*w2-(1.d0,1.d0)
 !enddo

!*******************************************************
! Load polynomial from file
!*******************************************************

 !open(unit=300,file="mypoly.txt")
 !do i=1,n
 !  write(300,*) poly(i)
 !end do
 !close(300)

!******************************************************
! Cyclotomic polynomial (roots of 1)
!******************************************************

!poly(1:n-1)=0.d0
!poly(n)=(-1.d0,0.d0)

!******************************************************
! Cyclotomic polynomial (roots of i)
!******************************************************

poly(1:n-1)=0.d0
poly(n)=(0.d0,-1.d0)



 CALL SYSTEM_CLOCK(COUNT_RATE=clock_rate) 
 CALL SYSTEM_CLOCK(COUNT=clock_start) 

 call qrwc(n,poly,myeig)

 CALL SYSTEM_CLOCK(COUNT=clock_end)  
 our_time=(REAL(clock_end - clock_start)/clock_rate) 
 write(*,*)"Our time = ",our_time 

! Apply the Lapack routine ZGEEV to the companion matrix A
! associated with POLY and compute running time

allocate(vL(1,n),vR(1,n),A(n,n),eig(n),dist1(n),dist2(n))    

  A=0.d0 
  do i=1,n-1 
     A(i+1,i)=(1.d0,0.d0) 
  enddo 
do i=1,n
  A(1,i)=-poly(i) 
enddo   

  LWORK = max(1,3*n+100) 
  allocate (WORK(LWORK)) 
  allocate(rwork(2*n)) 
  CALL SYSTEM_CLOCK(COUNT=clock_start)
  call ZGEEV('n','n',n,A,n,eig,vL,1,vR,1,WORK,LWORK,RWORK,INFO) 

  CALL SYSTEM_CLOCK(COUNT=clock_end)  
  lapack_time=(REAL(clock_end - clock_start)/clock_rate) 
  write(*,*)"Lapack time = ",lapack_time


! Compare eigenvalues:
  !
  ! comparison by minimum distance  

  compare_outer:do j=1,n 
     compare_inner:do i=1,n 
        dist2(i)=abs(myeig(j)-eig(i)) 
     end do compare_inner 
     dist1(j)=minval(dist2) 
!write(*,*) dist1(j)
  end do compare_outer 
  write(*,*)"distance = ",maxval(dist1) 
   

  ! comparison by sorting
  ! (warning: this might sometimes be unreliable, due to complex conjugate
  ! eigenvalues)
  allocate(eR1(n),eR2(n),eI1(n),eI2(n),seR1(n),seR2(n),seI1(n),seI2(n),diffeR(n),diffeI(n))

  eR1=real(myeig)
  eI1=aimag(myeig)
  eR2=real(eig)
  eI2=aimag(eig)

  call sort_eig(n,eR1,eI1,seR1,seI1)
  call sort_eig(n,eR2,eI2,seR2,seI2)

  diffeR = abs(seR2 - seR1)
  diffeI = abs(seI2 - seI1)
  maxdiffeR = maxval(abs(diffeR))
  maxdiffeI = maxval(abs(diffeI))

  write(*,*)"distance (by sorting) = ",maxdiffeR, maxdiffeI
  !write(*,*)"abs. distance (by sorting) = ",sqrt(maxdiffeR**2 + maxdiffeI**2)


end program main

!
! Structured eigenvalue computation:
! for use in main program.
! 

subroutine qrwc(n,poly,myeig)

implicit none

complex(kind(1d0)), intent(in) :: poly(n)
complex(kind(1d0)), intent(out) :: myeig(n)
complex(kind(1d0)) :: F(3,3,n-1),V(2,2,n-1),p(n),q(n),GG(2,2)
complex(kind(1d0)) :: g(n,2),h(2,n),sigma(n-1),gamma(n,2),alpha,auxvec(2)
complex(kind(1d0)) :: b11(n-1),b12(n-1),b21(n-1),b22(n-1)
complex(kind(1d0)) :: bnew11(n-1),bnew12(n-1),bnew21(n-1),bnew22(n-1)
complex(kind(1d0)) :: gnew(n,2),hnew(2,n),sigma_one(n-1),p_one(n),q_one(n)
complex(kind(1d0)) :: diagonals(n),subdiag(n-1),MM(2,3),auxmat(2,3),auxmat6(2,3),y(2)
complex(kind(1d0)) :: c_xold,c_gnew(2),c_Snew(3,2),c_z(2),c_xold_new,c_gnew_new(2),c_Snew_new(3,2),c_z_new(2)  
real :: threshold,w1,w2
integer, intent(in) :: n
integer :: iterations(n)
integer :: i,j,k,defl_size,m,id_flag,id_index,av_it 

threshold= epsilon(1.d0)
 c_xold=0.d0
 c_gnew=0.d0
 c_Snew=0.d0
 c_z=0.d0
 c_xold_new=0.d0
 c_gnew_new=0.d0
 c_Snew_new=0.d0
 c_z_new=0.d0
id_flag=0
id_index=n-4

do i=1,n
iterations(i)=0
myeig(i)=0.d0
p(i)=0.d0
g(i,1)=0.d0
g(i,2)=0.d0
h(1,i)=0.d0
h(2,i)=0.d0
enddo
p(1)=1.d0

do i=1,n-3
F(1,1,i)=0.d0
F(2,2,i)=0.d0
F(3,1,i)=0.d0
F(1,3,i)=0.d0
F(2,3,i)=0.d0
F(3,2,i)=0.d0
F(1,2,i)=1.d0
F(2,1,i)=1.d0
F(3,3,i)=1.d0
enddo
F(1,1,n-2)=0.d0
F(1,2,n-2)=0.d0
F(2,2,n-2)=0.d0
F(2,3,n-2)=0.d0
F(3,1,n-2)=0.d0
F(3,3,n-2)=0.d0
F(1,3,n-2)=1.d0
F(2,1,n-2)=1.d0
F(3,2,n-2)=1.d0
F(1,2,n-1)=0.d0
F(1,3,n-1)=0.d0
F(2,1,n-1)=0.d0
F(2,3,n-1)=0.d0
F(3,1,n-1)=0.d0
F(3,2,n-1)=0.d0
F(1,1,n-1)=1.d0
F(2,2,n-1)=1.d0
F(3,3,n-1)=1.d0

do i=1,n-1
V(1,1,i)=1.d0
V(2,2,i)=1.d0
V(1,2,i)=0.d0
V(2,1,i)=0.d0

sigma(i)=0.d0
gamma(i,1)=0.d0
gamma(i,2)=0.d0
enddo

q=poly
q(n)=poly(n)+1.d0

do i=1,n-2
h(1,i)=F(1,1,i)
h(2,i)=F(2,1,i)
b11(i)=F(1,2,i)
b12(i)=F(1,3,i)
b21(i)=F(2,2,i)
b22(i)=F(2,3,i)
enddo

!h(:,n-1)=F(1:2,1,n-1)
h(1,n-1)=F(1,1,n-1)
h(2,n-1)=F(2,1,n-1)
!b(1:2,1,n-1)=F(1:2,2,n-1)
b11(n-1)=F(1,2,n-1)
b21(n-1)=F(2,2,n-1)
!b(1:2,2,n-1)=0.d0
b12(n-1)=0.d0
b22(n-1)=0.d0

!h(:,n)=[1.d0; 0.d0]
h(1,n)=1.d0
h(2,n)=0.d0

!gamma(1,:)=[0.d0, 1.d0]
gamma(1,1)=0.d0
gamma(1,2)=1.d0

!g(1,:)=[1.d0, 0.d0]
g(1,1)=1.d0
g(1,2)=0.d0

do i=1,n-2
    auxmat(1,3)=0.d0
    auxmat(2,1)=0.d0
    auxmat(2,2)=0.d0
    !auxmat(1,1:2)=gamma(i,:)
auxmat(1,1)=gamma(i,1)
auxmat(1,2)=gamma(i,2)

    auxmat(2,3)=1.d0
    !auxmat6=auxmat*F(:,:,i)
! auxmat6=matmul(auxmat,F(:,:,i))
auxmat6(1,1)=auxmat(1,1)*F(1,1,i)+auxmat(1,2)*F(2,1,i)+auxmat(1,3)*F(3,1,i)
auxmat6(1,2)=auxmat(1,1)*F(1,2,i)+auxmat(1,2)*F(2,2,i)+auxmat(1,3)*F(3,2,i)
auxmat6(1,3)=auxmat(1,1)*F(1,3,i)+auxmat(1,2)*F(2,3,i)+auxmat(1,3)*F(3,3,i)
auxmat6(2,1)=auxmat(2,1)*F(1,1,i)+auxmat(2,2)*F(2,1,i)+auxmat(2,3)*F(3,1,i)
auxmat6(2,2)=auxmat(2,1)*F(1,2,i)+auxmat(2,2)*F(2,2,i)+auxmat(2,3)*F(3,2,i)
auxmat6(2,3)=auxmat(2,1)*F(1,3,i)+auxmat(2,2)*F(2,3,i)+auxmat(2,3)*F(3,3,i)

MM(1,1)=V(1,1,i+1)*auxmat6(1,1)+V(1,2,i+1)*auxmat6(2,1)
MM(1,2)=V(1,1,i+1)*auxmat6(1,2)+V(1,2,i+1)*auxmat6(2,2)
MM(1,3)=V(1,1,i+1)*auxmat6(1,3)+V(1,2,i+1)*auxmat6(2,3)
MM(2,1)=V(2,1,i+1)*auxmat6(1,1)+V(2,2,i+1)*auxmat6(2,1)
MM(2,2)=V(2,1,i+1)*auxmat6(1,2)+V(2,2,i+1)*auxmat6(2,2)
MM(2,3)=V(2,1,i+1)*auxmat6(1,3)+V(2,2,i+1)*auxmat6(2,3)
   !MM=V(:,:,i+1)*auxmat6
!    MM=V(:,:,i+1)*[gamma(i,:), 0; 0, 0, 1]*F(:,:,i)
    sigma(i)=MM(1,1)
    !g(i+1,:)=MM(1,2:3)
g(i+1,1)=MM(1,2)
g(i+1,2)=MM(1,3)

!    gamma(i+1,:)=MM(2,2:3)
 gamma(i+1,1)=MM(2,2)
 gamma(i+1,2)=MM(2,3)
end do

sigma(n-1)=gamma(n-1,1)*h(1,n-1)+gamma(n-1,2)*h(2,n-1)
g(n,1)=gamma(n-1,1)*b11(n-1)+gamma(n-1,2)*b21(n-1)
g(n,2)=gamma(n-1,1)*b12(n-1)+gamma(n-1,2)*b22(n-1)

! Ok, let's go

defl_size=n

mainloop:do while (defl_size>1.d0)

! Compute diagonal and subdiagonal entries
do j=1,n
diagonals(j)=g(j,1)*h(1,j)+g(j,2)*h(2,j)-p(j)*q(j)
enddo
do j=1,defl_size-1
subdiag(j)=sigma(j)-p(j+1)*q(j)
enddo

!write(*,*)"Last subdiagonal=",subdiag(defl_size-1)

! Look for first (from the bottom) small subdiagonal entry
m=defl_size-1
findm:do while ((m>0).AND.(abs(subdiag(m)))>threshold*(abs(diagonals(m))+abs(diagonals(m+1))))
!findm:do while ((m>0).AND.(abs(subdiag(m))>threshold))
    m=m-1
end do findm

! Has an eigenvalue been found?
eigfound: if (m==defl_size-1) then
 myeig(defl_size)=diagonals(defl_size)
!write(*,*)"iterations",iterations(defl_size)
 defl_size=defl_size-1
! update c_variables (for faster compression)
        if (defl_size<n-2) then
        c_xold=c_xold_new
        c_Snew=c_Snew_new
        c_gnew=c_gnew_new
        c_z=c_z_new
        end if

if (id_index>defl_size+2) then
id_flag=id_index-defl_size-2
!write(*,*)"id_flag,id_index,defl_size=",id_flag,id_index,defl_size
else
id_flag=0
endif

if (id_flag<0) then
write (*,*)"sth wrong with id_flag"
endif
else
! we'll have to perform QR then!
alpha=diagonals(defl_size)
iterations(defl_size)=iterations(defl_size)+1
random_shift: if (iterations(defl_size)>15) then 
write(*,*) "Random shift..." 
!write(*,*) "defl_size is",defl_size
call random_number(w1)
call random_number(w2)
alpha=(1.d0,0.d0)*w1+(0.d0,1.d0)*w2
iterations(defl_size)=0
end if random_shift
m=m+1

! check for "magic trick"
k=defl_size-1
magic: do while (k>m)
!auxvec=[diagonals(k)-alpha;subdiag(k)]
auxvec(1)=diagonals(k)-alpha
auxvec(2)=subdiag(k)

 call givens(auxvec,GG)
!y=[subdiag(k-1);0]
!y=G*y
y(1)=GG(1,1)*subdiag(k-1)
y(2)=GG(2,1)*subdiag(k-1)
  
if (abs(y(2))<threshold) then
!write(*,*)"magic trick, k=",k
       exit magic
end if
k=k-1
end do magic
m=k
! perform one sweep of QR
 call bulge_chasing_ss(g,h,b11,b12,b21,b22,sigma,p,q,m,defl_size,alpha,n,gnew,hnew,bnew11,bnew12,bnew21,bnew22,&
&sigma_one,p_one,q_one,c_xold_new,c_Snew_new,c_gnew_new,c_z_new,c_xold,c_Snew,c_gnew,c_z,id_flag,id_index)

! update variables
g=gnew
h=hnew
!b=bnew
b11=bnew11
b12=bnew12
b21=bnew21
b22=bnew22
sigma=sigma_one
p=p_one
q=q_one

end if eigfound

end do mainloop

myeig(1)=diagonals(1)

!enddo
! If iteration number is needed: (random shift not counted)
av_it=0.d0
do j=1,n
av_it=av_it+iterations(j)
enddo
write(*,*)"Average iterations per eigenvalue:",av_it/(1.d0*n)

end subroutine qrwc

! For use in qrwc.f90.
!
! Performs one sweep of structured implicit QR
! (with compression)
! on the submatrix m:defl_size.
!
! Input:
! g,h,b: quasiseparable generators
! sigma: subdiagonal entries
! p,q: vectors for rank one correction
! m: starting index
! defl_size: what it says on the tin
! alpha: shift
! n: polynomial degree and companion matrix size
!
! Output:
! gnew,hnew,bnew: q.s. generators for updated matrix
! sigma_one: subdiagonal entries of updated matrix
! p_one, q_one: vectors for updated rank one correction.
!


subroutine bulge_chasing_ss(g,h,b11,b12,b21,b22,sigma,p,q,m,defl_size,alpha,n,gnew,hnew,bnew11,bnew12,bnew21,bnew22,&
&sigma_one,p_one,q_one,c_xold_new,c_Snew_new,c_gnew_new,c_z_new,c_xold,c_Snew,c_gnew,c_z,id_flag,id_index)

implicit none

integer, intent(in) :: m,n,defl_size

complex(kind(1d0)), intent(in) :: g(n,2),h(2,n),sigma(n-1),p(n),q(n),alpha
complex(kind(1d0)), intent(in) :: b11(n-1),b12(n-1),b21(n-1),b22(n-1)
complex(kind(1d0)), intent(in) :: c_xold,c_gnew(2),c_Snew(3,2),c_z(2)
complex(kind(1d0)), intent(out) :: gnew(n,2),hnew(2,n),sigma_one(n-1),p_one(n),q_one(n)
complex(kind(1d0)), intent(out) :: bnew11(n-1),bnew12(n-1),bnew21(n-1),bnew22(n-1)
complex(kind(1d0)), intent(out) :: c_xold_new,c_gnew_new(2),c_Snew_new(3,2),c_z_new(2) 

integer :: i,j,k,fake_n,id_flag,id_index

complex(kind(1d0)) :: x(2),Y(2),xx(2),auxx(2),pM(2),qM(2),Y1(2),Y2(2),F(3,3)
complex(kind(1d0)) :: GG(2,2),Gt(2,2),A(2,3),aux(2,3),aux2(2,2),BB(2,2),G1(2,2),G2(2,2)
complex(kind(1d0)) :: hU(3,n),gU(n,3),V(2,2)
complex(kind(1d0)) :: bU11(n-1),bU12(n-1),bU13(n-1),bU21(n-1),bU22(n-1),bU23(n-1),bU31(n-1),bU32(n-1),bU33(n-1)
complex(kind(1d0)) :: SN(3),Sold(3,2),Snew(3,2),auxvec(2),auxvec2(3),z(2),finalB(2),xold,xnew
complex(kind(1d0)) :: auxmat(3,3),auxmat2(3,3),auxmat3(3,3),auxmat4(2,3),auxmat5(2,2),auxb(2,2)
complex(kind(1d0)) :: Saux(3,2),Saux2(3,2),Saux3(3,2)


! Initialization (to zero):
do i=1,n-1
sigma_one(i)=0.d0
enddo

do i=1,n
p_one(i)=0.d0
q_one(i)=0.d0
enddo

! *** Parts 1 and 2: compute the Q's and update generators ***
! (new uncompressed generators are computed along with the Q's)
!
! (The Givens matrices are not actually stored).
!
! ****** 1.1 ******

x(1)=g(1,1)*h(1,1)+g(1,2)*h(2,1)-p(1)*q(1)-alpha
x(2)=sigma(1)-p(2)*q(1)
call givens(x,GG)
if (m>1) then   ! in case qr starts later
  GG(1,1)=1.d0
  GG(1,2)=0.d0
  GG(2,1)=0.d0
  GG(2,2)=1.d0
end if
  
Gt(1,1)=conjg(GG(1,1))
Gt(1,2)=conjg(GG(2,1))
Gt(2,1)=conjg(GG(1,2))
Gt(2,2)=conjg(GG(2,2))

aux(1,1)=g(1,1)*h(1,1)+g(1,2)*h(2,1)
aux(1,2)=g(1,1)*b11(1)+g(1,2)*b21(1)
aux(1,3)=g(1,1)*b12(1)+g(1,2)*b22(1)
aux(2,1)=sigma(1)
aux(2,2)=g(2,1)
aux(2,3)=g(2,2)

!A=GG*aux
A(1,1)=GG(1,1)*aux(1,1)+GG(1,2)*aux(2,1)
A(1,2)=GG(1,1)*aux(1,2)+GG(1,2)*aux(2,2)
A(1,3)=GG(1,1)*aux(1,3)+GG(1,2)*aux(2,3)
A(2,1)=GG(2,1)*aux(1,1)+GG(2,2)*aux(2,1)
A(2,2)=GG(2,1)*aux(1,2)+GG(2,2)*aux(2,2)
A(2,3)=GG(2,1)*aux(1,3)+GG(2,2)*aux(2,3)

aux2(1,1)=A(2,1)
aux2(1,2)=A(2,2)*h(1,2)+A(2,3)*h(2,2)

aux2(2,1)=p(3)*q(1)
aux2(2,2)=sigma(2)
!BB=matmul(aux2,Gt)
BB(1,1)=aux2(1,1)*Gt(1,1)+aux2(1,2)*Gt(2,1)
BB(1,2)=aux2(1,1)*Gt(1,2)+aux2(1,2)*Gt(2,2)
BB(2,1)=aux2(2,1)*Gt(1,1)+aux2(2,2)*Gt(2,1)
BB(2,2)=aux2(2,1)*Gt(1,2)+aux2(2,2)*Gt(2,2)

pM(1)=GG(1,1)*p(1)+GG(1,2)*p(2)
pM(2)=GG(2,1)*p(1)+GG(2,2)*p(2)
qM(1)=q(1)*Gt(1,1)+q(2)*Gt(2,1)
qM(2)=q(1)*Gt(1,2)+q(2)*Gt(2,2)
!gU(1,:)=A(1,:)
gU(1,1)=A(1,1)
gU(1,2)=A(1,2)
gU(1,3)=A(1,3)

!hU(:,1)=[Gt(1,1);h(:,2)*Gt(2,1)]
hU(1,1)=Gt(1,1)
!hU(2:3,1)=h(:,2)*Gt(2,1)
hU(2,1)=h(1,2)*Gt(2,1)
hU(3,1)=h(2,2)*Gt(2,1)

!bU(:,:,1)=[Gt(1,2), 0, 0; h(:,2)*Gt(2,2), b(:,:,2)]
bU11(1)=Gt(1,2)
bU12(1)=0.d0
bU13(1)=0.d0
!bU(2:3,1)=h(:,2)*Gt(2,2)
bU21(1)=h(1,2)*Gt(2,2)
bU31(1)=h(2,2)*Gt(2,2)

!bU(2:3,2:3,1)=b(:,:,2)
bU22(1)=b11(2)
bU23(1)=b12(2)
bU32(1)=b21(2)
bU33(1)=b22(2)

p_one(1)=pM(1)
q_one(1)=qM(1)

! ****** 1.2 ******

if (defl_size>n-4-id_flag) then
    ! update as usual
do k=2,n-2
!   x=[B(1,1)-pM(2)*qM(1); B(2,1)-p(k+1)*qM(1)]
x(1)=BB(1,1)-pM(2)*qM(1)
x(2)=BB(2,1)-p(k+1)*qM(1)

  call givens(x,GG)
   !Y=GG*x
   Y(1)=GG(1,1)*x(1)+GG(1,2)*x(2)
   Y(2)=GG(2,1)*x(1)+GG(2,2)*x(2)
   
   if (k>=defl_size) then
       GG(1,1)=1.d0
       GG(1,2)=0.d0
       GG(2,1)=0.d0
       GG(2,2)=1.d0
       Y=x
   end if

   if (k<m) then
       GG(1,1)=1.d0
       GG(1,2)=0.d0
       GG(2,1)=0.d0
       GG(2,2)=1.d0
       Y=x
   end if
   
   if (k==m) then
     !  xx=[g(k,:)*h(:,k)-p(k)*q(k)-alpha;sigma(k)-p(k+1)*q(k)]
     !xx(1)=g(k,:)*h(:,k)-p(k)*q(k)-alpha
     xx(1)=g(k,1)*h(1,k)+g(k,2)*h(2,k)-p(k)*q(k)-alpha
     xx(2)=sigma(k)-p(k+1)*q(k)

       call givens(xx,GG)
       !Y=GG*x
        Y(1)=GG(1,1)*x(1)+GG(1,2)*x(2)
        Y(2)=GG(2,1)*x(1)+GG(2,2)*x(2)
       
   end if

! Q(:,:,k)=G';
Gt(1,1)=conjg(GG(1,1))
Gt(1,2)=conjg(GG(2,1))
Gt(2,1)=conjg(GG(1,2))
Gt(2,2)=conjg(GG(2,2))

!    A=G*[B(1,2), A(2,2:3)*b(:,:,k); B(2,2), g(k+1,:)]
    auxmat4(1,1)=BB(1,2)
!    auxmat4(1,2:3)=A(2,2:3)*b(:,:,k)
    auxmat4(1,2)=A(2,2)*b11(k)+A(2,3)*b21(k)
    auxmat4(1,3)=A(2,2)*b12(k)+A(2,3)*b22(k)    

    auxmat4(2,1)=BB(2,2)
    !auxmat4(2,2:3)=g(k+1,:)
auxmat4(2,2)=g(k+1,1)
auxmat4(2,3)=g(k+1,2)

    !A=GG*auxmat4
    !A=matmul(GG,auxmat4)
A(1,1)=GG(1,1)*auxmat4(1,1)+GG(1,2)*auxmat4(2,1)
A(1,2)=GG(1,1)*auxmat4(1,2)+GG(1,2)*auxmat4(2,2)
A(1,3)=GG(1,1)*auxmat4(1,3)+GG(1,2)*auxmat4(2,3)
A(2,1)=GG(2,1)*auxmat4(1,1)+GG(2,2)*auxmat4(2,1)
A(2,2)=GG(2,1)*auxmat4(1,2)+GG(2,2)*auxmat4(2,2)
A(2,3)=GG(2,1)*auxmat4(1,3)+GG(2,2)*auxmat4(2,3)

!    B=[A(2,1), A(2,2:3)*h(:,k+1); p(k+2)*qM(2), sigma(k+1)]*Gt
    auxmat5(1,1)=A(2,1)
!    auxmat5(1,2)=A(2,2:3)*h(:,k+1)
    auxmat5(1,2)=A(2,2)*h(1,k+1)+A(2,3)*h(2,k+1)

    auxmat5(2,1)=p(k+2)*qM(2)
    auxmat5(2,2)=sigma(k+1)
    !BB=auxmat5*Gt
    !BB=matmul(auxmat5,Gt)
BB(1,1)=auxmat5(1,1)*Gt(1,1)+auxmat5(1,2)*Gt(2,1)
BB(1,2)=auxmat5(1,1)*Gt(1,2)+auxmat5(1,2)*Gt(2,2)
BB(2,1)=auxmat5(2,1)*Gt(1,1)+auxmat5(2,2)*Gt(2,1)
BB(2,2)=auxmat5(2,1)*Gt(1,2)+auxmat5(2,2)*Gt(2,2)

!    pM=G*[pM(2); p(k+1)]
    auxx(1)=pM(2)
    auxx(2)=p(k+1)
    !pM=GG*auxx
     pM(1)=GG(1,1)*auxx(1)+GG(1,2)*auxx(2)
     pM(2)=GG(2,1)*auxx(1)+GG(2,2)*auxx(2)

    sigma_one(k-1)=Y(1)+pM(1)*qM(1)  ! order is important
    !qM=[qM(2), q(k+1)]*Gt
    qM(1)=qM(2)*Gt(1,1)+q(k+1)*Gt(2,1)
    qM(2)=qM(2)*Gt(1,2)+q(k+1)*Gt(2,2)

    !gU(k,:)=A(1,:)
gU(k,1)=A(1,1)
gU(k,2)=A(1,2)
gU(k,3)=A(1,3)

!    hU(:,k)=[Gt(1,1);h(:,k+1)*Gt(2,1)]
    hU(1,k)=Gt(1,1)
!    hU(2:3,k)=h(:,k+1)*Gt(2,1)
 hU(2,k)=h(1,k+1)*Gt(2,1)
 hU(3,k)=h(2,k+1)*Gt(2,1)

!    bU(:,:,k)=[Gt(1,2), 0.d0, 0.d0; h(:,k+1)*Gt(2,2), b(:,:,k+1)]
    bU11(k)=Gt(1,2)
    bU12(k)=0.d0
    bU13(k)=0.d0
    !bU(2:3,1,k)=h(:,k+1)*Gt(2,2)
    bU21(k)=h(1,k+1)*Gt(2,2)
    bU31(k)=h(2,k+1)*Gt(2,2)
    !bU(2:3,2:3,k)=b(:,:,k+1)
    bU22(k)=b11(k+1)
    bU23(k)=b12(k+1)
    bU32(k)=b21(k+1)
    bU33(k)=b22(k+1)

    p_one(k)=pM(1)
    q_one(k)=qM(1)
end do

! ****** 1.3 ******

! x=[B(1,1)-pM(2)*qM(1); B(2,1)-p(n)*qM(1)]
x(1)=BB(1,1)-pM(2)*qM(1)
x(2)=BB(2,1)-p(n)*qM(1)

 call givens(x,GG)
!Y=GG*x
 Y(1)=GG(1,1)*x(1)+GG(1,2)*x(2)
 Y(2)=GG(2,1)*x(1)+GG(2,2)*x(2)
  
 if (n-1>=defl_size) then
       GG(1,1)=1.d0
       GG(1,2)=0.d0
       GG(2,1)=0.d0
       GG(2,2)=1.d0
       Y=x
end if

if ((n-1)==m) then
!       xx=[g(n-1,:)*h(:,n-1)-p(n-1)*q(n-1)-alpha;sigma(n-1)-p(n)*q(n-1)]
       !xx(1)=g(n-1,:)*h(:,n-1)-p(n-1)*q(n-1)-alpha
       xx(1)=g(n-1,1)*h(1,n-1)+g(n-1,2)*h(2,n-1)-p(n-1)*q(n-1)-alpha  
       
       xx(2)=sigma(n-1)-p(n)*q(n-1)

       call givens(xx,GG)
       !Y=GG*x  
       !Y=GG*xx
!******************
! Hic sunt leones!
!******************
!        Y(1)=GG(1,1)*xx(1)+GG(1,2)*xx(2)
!        Y(2)=GG(2,1)*xx(1)+GG(2,2)*xx(2)
         Y(1)=GG(1,1)*x(1)+GG(1,2)*x(2)
         Y(2)=GG(2,1)*x(1)+GG(2,2)*x(2)
end if

!Q(:,:,n-1)=G'
Gt(1,1)=conjg(GG(1,1))
Gt(1,2)=conjg(GG(2,1))
Gt(2,1)=conjg(GG(1,2))
Gt(2,2)=conjg(GG(2,2))

!A=GG*[B(1,2), A(2,2:3)*b(:,:,n-1); B(2,2), g(n,:)]
aux(1,1)=BB(1,2)
!aux(1,2:3)=A(2,2:3)*b(:,:,n-1)
aux(1,2)=A(2,2)*b11(n-1)+A(2,3)*b21(n-1)
aux(1,3)=A(2,2)*b12(n-1)+A(2,3)*b22(n-1)

aux(2,1)=BB(2,2)
!aux(2,2:3)=g(n,:)
aux(2,2)=g(n,1)
aux(2,3)=g(n,2)

!A=GG*aux
!A=matmul(GG,aux)
A(1,1)=GG(1,1)*aux(1,1)+GG(1,2)*aux(2,1)
A(1,2)=GG(1,1)*aux(1,2)+GG(1,2)*aux(2,2)
A(1,3)=GG(1,1)*aux(1,3)+GG(1,2)*aux(2,3)
A(2,1)=GG(2,1)*aux(1,1)+GG(2,2)*aux(2,1)
A(2,2)=GG(2,1)*aux(1,2)+GG(2,2)*aux(2,2)
A(2,3)=GG(2,1)*aux(1,3)+GG(2,2)*aux(2,3)

!B=[A(2,1), A(2,2:3)*h(:,n)]*Gt   ! here only one row
auxvec(1)=A(2,1)
!auxvec(2)=A(2,2:3)*h(:,n)
auxvec(2)=A(2,2)*h(1,n)+A(2,3)*h(2,n)

finalB(1)=auxvec(1)*Gt(1,1)+auxvec(2)*Gt(2,1)
finalB(2)=auxvec(1)*Gt(1,2)+auxvec(2)*Gt(2,2)

!pM=GG*[pM(2); p(n)]
pM(1)=GG(1,1)*pM(2)+GG(1,2)*p(n)
pM(2)=GG(2,1)*pM(2)+GG(2,2)*p(n)

sigma_one(n-2)=Y(1)+pM(1)*qM(1)
!qM=[qM(2), q(n)]*Gt
qM(1)=qM(2)*Gt(1,1)+q(n)*Gt(2,1)
qM(2)=qM(2)*Gt(1,2)+q(n)*Gt(2,2)

sigma_one(n-1)=finalB(1)
p_one(n-1)=pM(1)
q_one(n-1)=qM(1)
!gU(n-1,:)=A(1,:)
gU(n-1,1)=A(1,1)
gU(n-1,2)=A(1,2)
gU(n-1,3)=A(1,3)

!hU(:,n-1)=[Q(1,1,n-1);h(:,n)*Q(2,1,n-1)]
hU(1,n-1)=Gt(1,1)
!hU(2:3,n-1)=h(:,n)*Gt(2,1)
hU(2,n-1)=h(1,n)*Gt(2,1)
hU(3,n-1)=h(2,n)*Gt(2,1)

!bU(:,:,n-1)=[Q(1,2,n-1),0,0; h(:,n)*Q(2,2,n-1),eye(2)]

!bU(1,:,n-1)=[Gt(1,2),0.d0,0.d0]
bU11(n-1)=Gt(1,2)
bU12(n-1)=0.d0
bU13(n-1)=0.d0

!bU(2:3,1,n-1)=h(:,n)*Gt(2,2)
bU21(n-1)=h(1,n)*Gt(2,2)
bU31(n-1)=h(2,n)*Gt(2,2)

bU22(n-1)=1.d0
bU33(n-1)=1.d0
bU23(n-1)=0.d0
bU32(n-1)=0.d0

! ****** 2 ******
! (last part of generator update)
p_one(n)=pM(2)
q_one(n)=qM(2)
!gU(n,:)=[B(2),0,0]
gU(n,1)=finalB(2)
gU(n,2)=0.d0
gU(n,3)=0.d0

!hU(:,n)=[1;0;0]
hU(1,n)=1.d0
hU(2,n)=0.d0
hU(3,n)=0.d0

else
  ! stop update process early
do k=2,defl_size+2+id_flag
!   x=[B(1,1)-pM(2)*qM(1); B(2,1)-p(k+1)*qM(1)]
x(1)=BB(1,1)-pM(2)*qM(1)
x(2)=BB(2,1)-p(k+1)*qM(1)

  call givens(x,GG)
   !Y=GG*x
   Y(1)=GG(1,1)*x(1)+GG(1,2)*x(2)
   Y(2)=GG(2,1)*x(1)+GG(2,2)*x(2)
   
   if (k>=defl_size) then
       GG(1,1)=1.d0
       GG(1,2)=0.d0
       GG(2,1)=0.d0
       GG(2,2)=1.d0
       Y=x
   end if

   if (k<m) then
       GG(1,1)=1.d0
       GG(1,2)=0.d0
       GG(2,1)=0.d0
       GG(2,2)=1.d0
       Y=x
   end if
   
   if (k==m) then
     !  xx=[g(k,:)*h(:,k)-p(k)*q(k)-alpha;sigma(k)-p(k+1)*q(k)]
     !xx(1)=g(k,:)*h(:,k)-p(k)*q(k)-alpha
     xx(1)=g(k,1)*h(1,k)+g(k,2)*h(2,k)-p(k)*q(k)-alpha
     xx(2)=sigma(k)-p(k+1)*q(k)

       call givens(xx,GG)
       !Y=GG*x
        Y(1)=GG(1,1)*x(1)+GG(1,2)*x(2)
        Y(2)=GG(2,1)*x(1)+GG(2,2)*x(2)
       
   end if

! Q(:,:,k)=G';
Gt(1,1)=conjg(GG(1,1))
Gt(1,2)=conjg(GG(2,1))
Gt(2,1)=conjg(GG(1,2))
Gt(2,2)=conjg(GG(2,2))

!    A=G*[B(1,2), A(2,2:3)*b(:,:,k); B(2,2), g(k+1,:)]
    auxmat4(1,1)=BB(1,2)
!    auxmat4(1,2:3)=A(2,2:3)*b(:,:,k)
    auxmat4(1,2)=A(2,2)*b11(k)+A(2,3)*b21(k)
    auxmat4(1,3)=A(2,2)*b12(k)+A(2,3)*b22(k)    

    auxmat4(2,1)=BB(2,2)
!    auxmat4(2,2:3)=g(k+1,:)
 auxmat4(2,2)=g(k+1,1)
 auxmat4(2,3)=g(k+1,2)

    !A=GG*auxmat4
!    A=matmul(GG,auxmat4)
A(1,1)=GG(1,1)*auxmat4(1,1)+GG(1,2)*auxmat4(2,1)
A(1,2)=GG(1,1)*auxmat4(1,2)+GG(1,2)*auxmat4(2,2)
A(1,3)=GG(1,1)*auxmat4(1,3)+GG(1,2)*auxmat4(2,3)
A(2,1)=GG(2,1)*auxmat4(1,1)+GG(2,2)*auxmat4(2,1)
A(2,2)=GG(2,1)*auxmat4(1,2)+GG(2,2)*auxmat4(2,2)
A(2,3)=GG(2,1)*auxmat4(1,3)+GG(2,2)*auxmat4(2,3)

!    B=[A(2,1), A(2,2:3)*h(:,k+1); p(k+2)*qM(2), sigma(k+1)]*Gt
    auxmat5(1,1)=A(2,1)
!    auxmat5(1,2)=A(2,2:3)*h(:,k+1)
    auxmat5(1,2)=A(2,2)*h(1,k+1)+A(2,3)*h(2,k+1)

    auxmat5(2,1)=p(k+2)*qM(2)
    auxmat5(2,2)=sigma(k+1)
    !BB=auxmat5*Gt
    !BB=matmul(auxmat5,Gt)
BB(1,1)=auxmat5(1,1)*Gt(1,1)+auxmat5(1,2)*Gt(2,1)
BB(1,2)=auxmat5(1,1)*Gt(1,2)+auxmat5(1,2)*Gt(2,2)
BB(2,1)=auxmat5(2,1)*Gt(1,1)+auxmat5(2,2)*Gt(2,1)
BB(2,2)=auxmat5(2,1)*Gt(1,2)+auxmat5(2,2)*Gt(2,2)

!    pM=G*[pM(2); p(k+1)]
    auxx(1)=pM(2)
    auxx(2)=p(k+1)
    !pM=GG*auxx
     pM(1)=GG(1,1)*auxx(1)+GG(1,2)*auxx(2)
     pM(2)=GG(2,1)*auxx(1)+GG(2,2)*auxx(2)

    sigma_one(k-1)=Y(1)+pM(1)*qM(1)  ! order is important
    !qM=[qM(2), q(k+1)]*Gt
    qM(1)=qM(2)*Gt(1,1)+q(k+1)*Gt(2,1)
    qM(2)=qM(2)*Gt(1,2)+q(k+1)*Gt(2,2)

   ! gU(k,:)=A(1,:)
gU(k,1)=A(1,1)
gU(k,2)=A(1,2)
gU(k,3)=A(1,3)

!    hU(:,k)=[Gt(1,1);h(:,k+1)*Gt(2,1)]
    hU(1,k)=Gt(1,1)
   ! hU(2:3,k)=h(:,k+1)*Gt(2,1)
hU(2,k)=h(1,k+1)*Gt(2,1)
hU(3,k)=h(2,k+1)*Gt(2,1)

!    bU(:,:,k)=[Gt(1,2), 0.d0, 0.d0; h(:,k+1)*Gt(2,2), b(:,:,k+1)]
    bU11(k)=Gt(1,2)
    bU12(k)=0.d0
    bU13(k)=0.d0
    !bU(2:3,1,k)=h(:,k+1)*Gt(2,2)
    bU21(k)=h(1,k+1)*Gt(2,2)
    bU31(k)=h(2,k+1)*Gt(2,2)    
!    bU(2:3,2:3,k)=b(:,:,k+1)
    bU22(k)=b11(k+1)
    bU23(k)=b12(k+1)
    bU32(k)=b21(k+1)
    bU33(k)=b22(k+1)

    p_one(k)=pM(1)
    q_one(k)=qM(1)
end do

end if

! ****** 3 ******
! (compression according to Theorem 2.1)

!do j=1,n
!!hnew(:,j)=[0.d0,0.d0]
!hnew(1,j)=0.d0
!hnew(2,j)=0.d0
!!gnew(j,:)=[0.d0;0.d0]
!gnew(j,1)=0.d0
!gnew(j,2)=0.d0
!enddo
!
!do j=1,n-1
!!bnew(:,:,j)=[0.d0,0.d0;0.d0,0.d0]
!bnew(:,:,j)=0.d0
!enddo

hnew=h
gnew=g
!bnew=b
bnew11=b11
bnew12=b12
bnew21=b21
bnew22=b22

if (defl_size>n-3-id_flag) then

!Sold=hU(:,n)
SN(1)=hU(1,n)
SN(2)=hU(2,n)
SN(3)=hU(3,n)

hnew(1,n)=1.d0
!gnew(n,1)=gU(n,:)*SN
gnew(n,1)=gU(n,1)*SN(1)+gU(n,2)*SN(2)+gU(n,3)*SN(3)
!hnew(:,n-1)=[1.d0;0.d0]
hnew(1,n-1)=1.d0
hnew(2,n-1)=0.d0

!bnew(:,:,n-1)=[0.d0,0.d0;1.d0,0.d0]
bnew11(n-1)=0.d0
bnew12(n-1)=0.d0
bnew21(n-1)=1.d0
bnew22(n-1)=0.d0

!Snew=[hU(:,n-1), bU(:,:,n-1)*Sold]
!Snew(:,1)=hU(:,n-1)
Snew(1,1)=hU(1,n-1)
Snew(2,1)=hU(2,n-1)
Snew(3,1)=hU(3,n-1)

!Snew(:,2)=bU(:,:,n-1)*SN
Snew(1,2)=bU11(n-1)*SN(1)+bU12(n-1)*SN(2)+bU13(n-1)*SN(3)
Snew(2,2)=bU21(n-1)*SN(1)+bU22(n-1)*SN(2)+bU23(n-1)*SN(3)
Snew(3,2)=bU31(n-1)*SN(1)+bU32(n-1)*SN(2)+bU33(n-1)*SN(3)

!gnew(n-1,:)=gU(n-1,:)*Snew
gnew(n-1,1)=gU(n-1,1)*Snew(1,1)+gU(n-1,2)*Snew(2,1)+gU(n-1,3)*Snew(3,1)
gnew(n-1,2)=gU(n-1,1)*Snew(1,2)+gU(n-1,2)*Snew(2,2)+gU(n-1,3)*Snew(3,2)

!z=[sigma_one(n-1), gnew(n,1)]
z(1)=sigma_one(n-1)
z(2)=gnew(n,1)

xold=p_one(n)

fake_n=n  
! update for later
 c_xold_new=xold
    !c_gnew_new=gnew(fake_n-1,:)
 c_gnew_new(1)=gnew(fake_n-1,1)
 c_gnew_new(2)=gnew(fake_n-1,2)
 c_z_new=z
 c_Snew_new=Snew
 id_index=fake_n-1

else
fake_n=defl_size+2+id_flag
xold=c_xold
!gnew(fake_n-1,:)=c_gnew
gnew(fake_n-1,1)=c_gnew(1)
gnew(fake_n-1,2)=c_gnew(2)
z=c_z
Snew=c_Snew
 id_index=fake_n-1
end if

! do compression loop separately once to update starting values
k=fake_n-1
 ![V,Y]=planerot([p_one(k);xold]);  % here V = V_k'
!xx=[p_one(k);xold]
xx(1)=p_one(k)
xx(2)=xold

call givens(xx,V)
!Y=V*xx
!xnew=Y(1)
xnew=V(1,1)*xx(1)+V(1,2)*xx(2)

!A=V*[sigma_one(k-1), gnew(k,:); xold*q_one(k-1), z]
aux(1,1)=sigma_one(k-1)
aux(1,2)=gnew(k,1)
aux(1,3)=gnew(k,2)
aux(2,1)=xold*q_one(k-1)
aux(2,2)=z(1)
aux(2,3)=z(2)
!A=V*aux
!A=matmul(V,aux)
A(1,1)=V(1,1)*aux(1,1)+V(1,2)*aux(2,1)
A(1,2)=V(1,1)*aux(1,2)+V(1,2)*aux(2,2)
A(1,3)=V(1,1)*aux(1,3)+V(1,2)*aux(2,3)
A(2,1)=V(2,1)*aux(1,1)+V(2,2)*aux(2,1)
A(2,2)=V(2,1)*aux(1,2)+V(2,2)*aux(2,2)
A(2,3)=V(2,1)*aux(1,3)+V(2,2)*aux(2,3)

xold=xnew
  ! auxvec=A(2,:)';
!auxvec2=conjg(A(2,:))
auxvec2(1)=conjg(A(2,1))
auxvec2(2)=conjg(A(2,2))
auxvec2(3)=conjg(A(2,3))

!   [G1,Y1]=planerot(auxvec(2:3));
!xx=auxvec2(2:3)
xx(1)=auxvec2(2)
xx(2)=auxvec2(3)

call givens(xx,G1)
!Y1=G1*xx
Y1(1)=G1(1,1)*xx(1)+G1(1,2)*xx(2)
Y1(2)=G1(2,1)*xx(1)+G1(2,2)*xx(2)

!   [G2,Y2]=planerot([auxvec(1);Y1(1)]);
!xx=[auxvec(1);Y1(1)]
xx(1)=auxvec2(1)
xx(2)=Y1(1)

call givens(xx,G2)
!Y2=G2*xx
Y2(1)=G2(1,1)*xx(1)+G2(1,2)*xx(2)
Y2(2)=G2(2,1)*xx(1)+G2(2,2)*xx(2)

!   F=[G2,[0;0];0,0,1]*[1,0,0;[0;0],G1];  
!   F=flipud(F);   % here F=F_k-1 (not *)
auxmat=0.d0
!auxmat(1:2,1:2)=G2
auxmat(1,1)=G2(1,1)
auxmat(1,2)=G2(1,2)
auxmat(2,1)=G2(2,1)
auxmat(2,2)=G2(2,2)

auxmat(3,3)=1.d0
auxmat2=0.d0
!auxmat2(2:3,2:3)=G1
auxmat2(2,2)=G1(1,1)
auxmat2(2,3)=G1(1,2)
auxmat2(3,2)=G1(2,1)
auxmat2(3,3)=G1(2,2)

auxmat2(1,1)=1.d0
!auxmat3=matmul(auxmat,auxmat2)
auxmat3(1,1)=auxmat(1,1)*auxmat2(1,1)+auxmat(1,2)*auxmat2(2,1)+auxmat(1,3)*auxmat2(3,1) 
auxmat3(1,2)=auxmat(1,1)*auxmat2(1,2)+auxmat(1,2)*auxmat2(2,2)+auxmat(1,3)*auxmat2(3,2) 
auxmat3(1,3)=auxmat(1,1)*auxmat2(1,3)+auxmat(1,2)*auxmat2(2,3)+auxmat(1,3)*auxmat2(3,3) 
auxmat3(2,1)=auxmat(2,1)*auxmat2(1,1)+auxmat(2,2)*auxmat2(2,1)+auxmat(2,3)*auxmat2(3,1) 
auxmat3(2,2)=auxmat(2,1)*auxmat2(1,2)+auxmat(2,2)*auxmat2(2,2)+auxmat(2,3)*auxmat2(3,2) 
auxmat3(2,3)=auxmat(2,1)*auxmat2(1,3)+auxmat(2,2)*auxmat2(2,3)+auxmat(2,3)*auxmat2(3,3) 
auxmat3(3,1)=auxmat(3,1)*auxmat2(1,1)+auxmat(3,2)*auxmat2(2,1)+auxmat(3,3)*auxmat2(3,1) 
auxmat3(3,2)=auxmat(3,1)*auxmat2(1,2)+auxmat(3,2)*auxmat2(2,2)+auxmat(3,3)*auxmat2(3,2) 
auxmat3(3,3)=auxmat(3,1)*auxmat2(1,3)+auxmat(3,2)*auxmat2(2,3)+auxmat(3,3)*auxmat2(3,3) 

!F(1,:)=auxmat3(3,:)
F(1,1)=auxmat3(3,1)
F(1,2)=auxmat3(3,2)
F(1,3)=auxmat3(3,3)

!F(2,:)=auxmat3(2,:)
F(2,1)=auxmat3(2,1)
F(2,2)=auxmat3(2,2)
F(2,3)=auxmat3(2,3)

!F(3,:)=auxmat(1,:)
F(3,1)=auxmat(1,1)
F(3,2)=auxmat(1,2)
F(3,3)=auxmat(1,3)

  ! hnew(:,k-1)=F(1:2,1)
hnew(1,k-1)=F(1,1)
hnew(2,k-1)=F(2,1)

   !bnew(:,:,k-1)=F(1:2,2:3)
   bnew11(k-1)=F(1,2)
   bnew12(k-1)=F(1,3)
   bnew21(k-1)=F(2,2)
   bnew22(k-1)=F(2,3)

   Sold=Snew
! Snew=hU(:,k-1)*(hnew(:,k-1))'+bU(:,:,k-1)*Sold*bnew(:,:,k-1)';
!   z=A(1,1)*hnew(:,k-1)'+A(1,2:3)*bnew(:,:,k-1)';
auxb(1,1)=conjg(bnew11(k-1))
auxb(2,2)=conjg(bnew22(k-1))
auxb(1,2)=conjg(bnew21(k-1))
auxb(2,1)=conjg(bnew12(k-1))
!Snew=hU(:,k-1)*conjg(hnew(:,k-1))+bU(:,:,k-1)*Sold*auxb
!Saux=matmul(Sold,auxb)
Saux(1,1)=Sold(1,1)*auxb(1,1)+Sold(1,2)*auxb(2,1)
Saux(1,2)=Sold(1,1)*auxb(1,2)+Sold(1,2)*auxb(2,2)
Saux(2,1)=Sold(2,1)*auxb(1,1)+Sold(2,2)*auxb(2,1)
Saux(2,2)=Sold(2,1)*auxb(1,2)+Sold(2,2)*auxb(2,2)
Saux(3,1)=Sold(3,1)*auxb(1,1)+Sold(3,2)*auxb(2,1)
Saux(3,2)=Sold(3,1)*auxb(1,2)+Sold(3,2)*auxb(2,2)

Saux2(1,1)=hU(1,k-1)*conjg(hnew(1,k-1))
Saux2(2,1)=hU(2,k-1)*conjg(hnew(1,k-1))
Saux2(3,1)=hU(3,k-1)*conjg(hnew(1,k-1))
Saux2(1,2)=hU(1,k-1)*conjg(hnew(2,k-1))
Saux2(2,2)=hU(2,k-1)*conjg(hnew(2,k-1))
Saux2(3,2)=hU(3,k-1)*conjg(hnew(2,k-1))
!Saux3=matmul(bU(:,:,k-1),Saux)
Saux3(1,1)=bU11(k-1)*Saux(1,1)+bU12(k-1)*Saux(2,1)+bU13(k-1)*Saux(3,1)
Saux3(2,1)=bU21(k-1)*Saux(1,1)+bU22(k-1)*Saux(2,1)+bU23(k-1)*Saux(3,1)
Saux3(3,1)=bU31(k-1)*Saux(1,1)+bU32(k-1)*Saux(2,1)+bU33(k-1)*Saux(3,1)
Saux3(1,2)=bU11(k-1)*Saux(1,2)+bU12(k-1)*Saux(2,2)+bU13(k-1)*Saux(3,2)
Saux3(2,2)=bU21(k-1)*Saux(1,2)+bU22(k-1)*Saux(2,2)+bU23(k-1)*Saux(3,2)
Saux3(3,2)=bU31(k-1)*Saux(1,2)+bU32(k-1)*Saux(2,2)+bU33(k-1)*Saux(3,2)

Snew=Saux2+Saux3

!z=A(1,1)*conjg(hnew(:,k-1))+A(1,2:3)*auxb
z(1)=A(1,1)*conjg(hnew(1,k-1))+A(1,2)*auxb(1,1)+A(1,3)*auxb(2,1)
z(2)=A(1,1)*conjg(hnew(2,k-1))+A(1,2)*auxb(1,2)+A(1,3)*auxb(2,2)

!gnew(k-1,:)=gU(k-1,:)*Snew
gnew(k-1,1)=gU(k-1,1)*Snew(1,1)+gU(k-1,2)*Snew(2,1)+gU(k-1,3)*Snew(3,1)
gnew(k-1,2)=gU(k-1,1)*Snew(1,2)+gU(k-1,2)*Snew(2,2)+gU(k-1,3)*Snew(3,2)

! update starting values
 c_xold_new=xold
 !c_gnew_new=gnew(k-1,:);
 c_gnew_new(1)=gnew(k-1,1)
 c_gnew_new(2)=gnew(k-1,2)
 c_z_new=z
 c_Snew_new=Snew


! do the rest of the loop now
do k=fake_n-2,2,-1
   ![V,Y]=planerot([p_one(k);xold]);  % here V = V_k'
!xx=[p_one(k);xold]
xx(1)=p_one(k)
xx(2)=xold

call givens(xx,V)
!Y=V*xx
!xnew=Y(1)
xnew=V(1,1)*xx(1)+V(1,2)*xx(2)

!A=V*[sigma_one(k-1), gnew(k,:); xold*q_one(k-1), z]
aux(1,1)=sigma_one(k-1)
aux(1,2)=gnew(k,1)
aux(1,3)=gnew(k,2)
aux(2,1)=xold*q_one(k-1)
aux(2,2)=z(1)
aux(2,3)=z(2)
!A=V*aux
!A=matmul(V,aux)
A(1,1)=V(1,1)*aux(1,1)+V(1,2)*aux(2,1)
A(1,2)=V(1,1)*aux(1,2)+V(1,2)*aux(2,2)
A(1,3)=V(1,1)*aux(1,3)+V(1,2)*aux(2,3)
A(2,1)=V(2,1)*aux(1,1)+V(2,2)*aux(2,1)
A(2,2)=V(2,1)*aux(1,2)+V(2,2)*aux(2,2)
A(2,3)=V(2,1)*aux(1,3)+V(2,2)*aux(2,3)

xold=xnew
  ! auxvec=A(2,:)';
!auxvec2=conjg(A(2,:))
auxvec2(1)=conjg(A(2,1))
auxvec2(2)=conjg(A(2,2))
auxvec2(3)=conjg(A(2,3))

!   [G1,Y1]=planerot(auxvec(2:3));
!xx=auxvec2(2:3)
xx(1)=auxvec2(2)
xx(2)=auxvec2(3)

call givens(xx,G1)
!Y1=G1*xx
Y1(1)=G1(1,1)*xx(1)+G1(1,2)*xx(2)
Y1(2)=G1(2,1)*xx(1)+G1(2,2)*xx(2)

!   [G2,Y2]=planerot([auxvec(1);Y1(1)]);
!xx=[auxvec(1);Y1(1)]
xx(1)=auxvec2(1)
xx(2)=Y1(1)

call givens(xx,G2)
!Y2=G2*xx
Y2(1)=G2(1,1)*xx(1)+G2(1,2)*xx(2)
Y2(2)=G2(2,1)*xx(1)+G2(2,2)*xx(2)

!   F=[G2,[0;0];0,0,1]*[1,0,0;[0;0],G1];  
!   F=flipud(F);   % here F=F_k-1 (not *)
auxmat=0.d0
!auxmat(1:2,1:2)=G2
auxmat(1,1)=G2(1,1)
auxmat(1,2)=G2(1,2)
auxmat(2,1)=G2(2,1)
auxmat(2,2)=G2(2,2)

auxmat(3,3)=1.d0
auxmat2=0.d0
!auxmat2(2:3,2:3)=G1
auxmat2(2,2)=G1(1,1)
auxmat2(2,3)=G1(1,2)
auxmat2(3,2)=G1(2,1)
auxmat2(3,3)=G1(2,2)

auxmat2(1,1)=1.d0
!auxmat3=matmul(auxmat,auxmat2)
auxmat3(1,1)=auxmat(1,1)*auxmat2(1,1)+auxmat(1,2)*auxmat2(2,1)+auxmat(1,3)*auxmat2(3,1) 
auxmat3(1,2)=auxmat(1,1)*auxmat2(1,2)+auxmat(1,2)*auxmat2(2,2)+auxmat(1,3)*auxmat2(3,2) 
auxmat3(1,3)=auxmat(1,1)*auxmat2(1,3)+auxmat(1,2)*auxmat2(2,3)+auxmat(1,3)*auxmat2(3,3) 
auxmat3(2,1)=auxmat(2,1)*auxmat2(1,1)+auxmat(2,2)*auxmat2(2,1)+auxmat(2,3)*auxmat2(3,1) 
auxmat3(2,2)=auxmat(2,1)*auxmat2(1,2)+auxmat(2,2)*auxmat2(2,2)+auxmat(2,3)*auxmat2(3,2) 
auxmat3(2,3)=auxmat(2,1)*auxmat2(1,3)+auxmat(2,2)*auxmat2(2,3)+auxmat(2,3)*auxmat2(3,3) 
auxmat3(3,1)=auxmat(3,1)*auxmat2(1,1)+auxmat(3,2)*auxmat2(2,1)+auxmat(3,3)*auxmat2(3,1) 
auxmat3(3,2)=auxmat(3,1)*auxmat2(1,2)+auxmat(3,2)*auxmat2(2,2)+auxmat(3,3)*auxmat2(3,2) 
auxmat3(3,3)=auxmat(3,1)*auxmat2(1,3)+auxmat(3,2)*auxmat2(2,3)+auxmat(3,3)*auxmat2(3,3) 

!F(1,:)=auxmat3(3,:)
F(1,1)=auxmat3(3,1)
F(1,2)=auxmat3(3,2)
F(1,3)=auxmat3(3,3)

!F(2,:)=auxmat3(2,:)
F(2,1)=auxmat3(2,1)
F(2,2)=auxmat3(2,2)
F(2,3)=auxmat3(2,3)

!F(3,:)=auxmat(1,:)
F(3,1)=auxmat(1,1)
F(3,2)=auxmat(1,2)
F(3,3)=auxmat(1,3)

  ! hnew(:,k-1)=F(1:2,1)
 hnew(1,k-1)=F(1,1)
 hnew(2,k-1)=F(2,1)

   !bnew(:,:,k-1)=F(1:2,2:3)
bnew11(k-1)=F(1,2)
bnew12(k-1)=F(1,3)
bnew21(k-1)=F(2,2)
bnew22(k-1)=F(2,3)

   Sold=Snew
! Snew=hU(:,k-1)*(hnew(:,k-1))'+bU(:,:,k-1)*Sold*bnew(:,:,k-1)';
!   z=A(1,1)*hnew(:,k-1)'+A(1,2:3)*bnew(:,:,k-1)';
auxb(1,1)=conjg(bnew11(k-1))
auxb(2,2)=conjg(bnew22(k-1))
auxb(1,2)=conjg(bnew21(k-1))
auxb(2,1)=conjg(bnew12(k-1))
!Snew=hU(:,k-1)*conjg(hnew(:,k-1))+bU(:,:,k-1)*Sold*auxb
!Saux=matmul(Sold,auxb)
Saux(1,1)=Sold(1,1)*auxb(1,1)+Sold(1,2)*auxb(2,1)
Saux(1,2)=Sold(1,1)*auxb(1,2)+Sold(1,2)*auxb(2,2)
Saux(2,1)=Sold(2,1)*auxb(1,1)+Sold(2,2)*auxb(2,1)
Saux(2,2)=Sold(2,1)*auxb(1,2)+Sold(2,2)*auxb(2,2)
Saux(3,1)=Sold(3,1)*auxb(1,1)+Sold(3,2)*auxb(2,1)
Saux(3,2)=Sold(3,1)*auxb(1,2)+Sold(3,2)*auxb(2,2)

Saux2(1,1)=hU(1,k-1)*conjg(hnew(1,k-1))
Saux2(2,1)=hU(2,k-1)*conjg(hnew(1,k-1))
Saux2(3,1)=hU(3,k-1)*conjg(hnew(1,k-1))
Saux2(1,2)=hU(1,k-1)*conjg(hnew(2,k-1))
Saux2(2,2)=hU(2,k-1)*conjg(hnew(2,k-1))
Saux2(3,2)=hU(3,k-1)*conjg(hnew(2,k-1))
!Saux3=matmul(bU(:,:,k-1),Saux)
Saux3(1,1)=bU11(k-1)*Saux(1,1)+bU12(k-1)*Saux(2,1)+bU13(k-1)*Saux(3,1)
Saux3(2,1)=bU21(k-1)*Saux(1,1)+bU22(k-1)*Saux(2,1)+bU23(k-1)*Saux(3,1)
Saux3(3,1)=bU31(k-1)*Saux(1,1)+bU32(k-1)*Saux(2,1)+bU33(k-1)*Saux(3,1)
Saux3(1,2)=bU11(k-1)*Saux(1,2)+bU12(k-1)*Saux(2,2)+bU13(k-1)*Saux(3,2)
Saux3(2,2)=bU21(k-1)*Saux(1,2)+bU22(k-1)*Saux(2,2)+bU23(k-1)*Saux(3,2)
Saux3(3,2)=bU31(k-1)*Saux(1,2)+bU32(k-1)*Saux(2,2)+bU33(k-1)*Saux(3,2)

Snew=Saux2+Saux3

!z=A(1,1)*conjg(hnew(:,k-1))+A(1,2:3)*auxb
z(1)=A(1,1)*conjg(hnew(1,k-1))+A(1,2)*auxb(1,1)+A(1,3)*auxb(2,1)
z(2)=A(1,1)*conjg(hnew(2,k-1))+A(1,2)*auxb(1,2)+A(1,3)*auxb(2,2)

!gnew(k-1,:)=gU(k-1,:)*Snew
gnew(k-1,1)=gU(k-1,1)*Snew(1,1)+gU(k-1,2)*Snew(2,1)+gU(k-1,3)*Snew(3,1)
gnew(k-1,2)=gU(k-1,1)*Snew(1,2)+gU(k-1,2)*Snew(2,2)+gU(k-1,3)*Snew(3,2)
enddo

end subroutine bulge_chasing_ss


!********************************************************
subroutine givens(x,G)
! computes 2x2 Givens matrix such that the second
! entry of G*x is zero
implicit none
complex(kind(1d0)), intent(in) :: x(2)
complex(kind(1d0)), intent(out) :: G(2,2)
real(kind(1d0)) :: s,w1,w2

w1=abs(x(1))
w2=abs(x(2))
G(1,1)=1.d0
G(2,2)=1.d0
G(1,2)=0.d0
G(2,1)=0.d0
if(max(w1,w2)<3.d-16)return
!if(max(w1,w2)<epsilon(1.d0))return

s=1/sqrt(w1*w1+w2*w2)
G(1,1)=conjg(s*x(1))
G(1,2)=conjg(s*x(2))
G(2,1)=-s*x(2)
G(2,2)=s*x(1)
return
end subroutine givens



subroutine	sort_eig(n,eR,eI,seR,seI)
  !!
  !!	sort the sequence according to real part first, if two complex numbers
  !!		have the same real part, put the one with bigger imaginary part in
  !!		front
  !!

  implicit none
  integer				n
  double precision	eR(n),eI(n),seR(n),seI(n),tmpR,tmpI
  integer				i,IY(n),KFLAG,tmpindx

  do i = 1,n
     IY(i) = i
  enddo

  seR = eR
  call ssort(seR,iy,n,kflag)

  do i = 1,n
     seI(i) = eI(iy(i))
  enddo

  tmpR = seR(1)
  tmpI = seI(1)
  do i = 2,n
     if (tmpR == seR(i)) then
        if (tmpI < seI(i)) then
           seI(i-1) = seI(i)
           seI(i)	= tmpI
           tmpindx	= IY(i-1)
           IY(i-1)	= IY(i)
           IY(i) = tmpindx
        endif
     else
        tmpR = seR(i)
        tmpI = seI(i)
     endif
  enddo

  return

end subroutine sort_eig


SUBROUTINE init_random_seed()
            INTEGER :: i, n, clock
            INTEGER, DIMENSION(:), ALLOCATABLE :: seed
          
            CALL RANDOM_SEED(size = n)
            ALLOCATE(seed(n))
          
            CALL SYSTEM_CLOCK(COUNT=clock)
          
            seed = clock + 37 * (/ (i - 1, i = 1, n) /)
            CALL RANDOM_SEED(PUT = seed)
          
            DEALLOCATE(seed)
          END SUBROUTINE

