 ! Main program for structured eigenvalue computation.
!
! This program finds the roots of a given real polynomial
! by computing the eigenvalues of the associated
! companion pencil with O(n^2) complexity. 
! The unitary + rank one and quasiseparable
! structures of the pencil are used.
!
! This is an implementation
! of the method described in the paper
! "A real QZ algorithm for structured companion pencils",
! by P. Boito, Y. Eidelman and L. Gemignani.
!
! Double shift is implemented in this version
! (the polynomial needs to have real coefficients). 
! A mixed shift strategy is used: either single or double
! shift, depending on the sign of the discriminant
! of the trailing 2x2 submatrix.  
!
! Do not use on polynomials of degree <10.
!
! Lapack needs to be installed. 
! The Fortran routine ssort.f is also needed 
! (it can easily be found on the Internet) if comparison
! by sorting is required. 
!
! The computed eigenvalues are stored in array myeig.
! The program displays timings for fest QZ and DGEEV,
! the error on myeig w.r.t to DGEEV and the average
! number of iterations per eigenvalue.
!
! Please note: we are working here with well-behaved polynomials.
! Therefore: 
! - the distance between eigenvalues computed
!   by fast QZ and by Lapack is evaluated in double precision,
! - eigenvalues are "naively" computed explicitly.
! Also, deflation on B is not implemented for now.
!
! Stopping criterion: subdiagonal entry smaller than tol*eps,
! where tol is currently set to sqrt(n).
!
! The "givens" subroutine follows the algorithm given
! in Golub and Van Loan, "Matrix Computations". 
!
! Last update on August 15, 2016. 
! Please send comments or bug reports to
! paola.boito@unilim.fr


program main

implicit none

integer :: n,i,j,k,lwork,info,lapacksize
integer :: clock_start,clock_end,clock_rate
real(kind(1d0)) :: w1,w2,w3,our_time,maxdiffeR,maxdiffeI,lapack_time,normpoly
real(kind(1d0)) :: numiter
real(kind(1d0)), allocatable :: rwork(:),dist1(:),dist2(:) 
real(kind(1d0)), allocatable :: eR1(:),eR2(:),eI1(:),eI2(:),seR1(:),seR2(:),seI1(:),seI2(:),diffeR(:),diffeI(:)
real(kind(1d0)), allocatable :: work(:),vl(:,:),vr(:,:),alphaL(:),betaL(:),alphaR(:),alphaI(:),beta(:)
real(kind(1d0)),allocatable :: Am(:,:),Bm(:,:),poly(:)
complex(kind(1d0)), allocatable :: eig(:),myeig(:)


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

allocate(poly(n+1),myeig(n)) 

allocate(vL(1,n),vR(1,n),Am(n,n),Bm(n,n),alphaR(n),alphaI(n),beta(n))    
allocate(eig(n),dist1(n),dist2(n))

LWORK = 10*n+100
  allocate(WORK(LWORK)) 

call random_seed

! Define polynomial coefficients.
! (Comment/uncomment as needed).
!
!
!*******************************************  
! TEST 1: Random coefficients 
!*******************************************
!   
!
!   
do i=1,n+1 
   call random_number(w1) 
   poly(i)=(2.d0)*w1-1.d0
enddo 
!
!
!********************************************
! TEST 2: Cyclotomic polynomial  (roots of 1)
!********************************************
!
!poly(1)=1.d0
!do k=2,n
!poly(k)=0.d0
!enddo
!poly(n+1)=-1.d0
!
!********************************************  
! TEST 3: From file 
!********************************************
!
!open(unit=300,file="mypoly.txt")
!do i=1,n+1
!  write(300,*) poly(i)
!end do
!close(300)
!
!****************************************  
! TEST 4: All coefficients = 1
!****************************************
!
!do i=1,n+1
!   poly(i)=1.d0
!enddo
!
!****************************************

! Normalize polynomial:
normpoly=0.d0
do i=1,n+1
   normpoly=normpoly+poly(i)*poly(i)
enddo
poly=poly/sqrt(normpoly)   

! Apply fast_companion_QZ_ds.f90 to poly and compute running time


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

  call fast_companion_QZ_ds(n,poly,myeig,numiter)

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

! Apply the Lapack routine DGGEV to the companion pencil (Am,Bm)
! associated with POLY and compute running time


Am=0.d0 
do i=1,n-1 
   Am(i+1,i)=1.d0 
enddo 
do i=1,n
   Am(1,i)=-poly(i+1) 
enddo   
Bm=0.d0
Bm(1,1)=poly(1)
do i=2,n
   Bm(i,i)=1.d0
enddo   

  
  CALL SYSTEM_CLOCK(COUNT=clock_start)

  call DGGEV('n','n',n,Am,n,Bm,n,alphaR,alphaI,beta,vL,1,vR,1,WORK,LWORK,INFO) 
 
  CALL SYSTEM_CLOCK(COUNT=clock_end)  
  lapack_time=(REAL(clock_end - clock_start)/clock_rate) 
  write(*,*)"Lapack time = ",lapack_time

! Compare eigenvalues:

do j=1,n
   eig(j)=(alphaR(j)*(1.d0,0.d0)+(0.d0,1.d0)*alphaI(j))/beta(j)
enddo
!write(*,*)"info=",info
!write(*,*)"myeig=",myeig
!write(*,*)"Lapack eig=",eig

  
  ! 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) 
  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)

!write(*,*)"myeig=",myeig
!write(*,*)"Lapack eig=",eig

end program main



!***********************************************************


subroutine fast_companion_QZ_ds(n,poly,myeig,numiter)

! Fast eigenvalue computation.

implicit none

! Input/output variables: 

integer, intent(in) :: n
real(kind(1d0)), intent(in) :: poly(n+1)
real(kind(1d0)), intent(out) :: numiter
complex(kind(1d0)), intent(out) :: myeig(n)

! Variables for quasiseparable representation of A and B 
! and main loop: 

integer :: i,j,k,kin,kfin,numiter_global,numiter_local,max_local_iter,Bdefl,kin_flag,dflag,conv_flag
real(kind(1d0)) :: tol,eps,ttt,w1rand,w2rand,normpoly,safmin
real(kind(1d0)) :: adiag(n),bdiag(n)
real(kind(1d0)) :: sigmaa(n-1),gv(n,2),hv(2,n),bv(2,2,n-1),z(n),w(n)
real(kind(1d0)) :: db(n),gu(n-1),hu(n),bu(n-1),p(n),q(n)
real(kind(1d0)) :: xx(2),G(2,2),tkin,tkfin,tj,tjm1,yy(2)
real(kind(1d0)) :: sigmaanew(n-1),gvnew(n,2),hvnew(2,n),bvnew(2,2,n-1)
real(kind(1d0)) :: gunew(n-1),hunew(n),bunew(n-1),Qin(2,2),p1(n),q1(n)


! Variables from structured QZ subroutine:


real(kind(1d0)) :: du(n),du1tilde(n+1),gu1tilde(n+1,2),hu1tilde(2,n+1),bu1tilde(2,2,n+1),w1(n),z1(n)
real(kind(1d0)) :: dv1tilde(n+1),gv1tilde(n+1,3),hv1tilde(3,n+2),bv1tilde(3,3,n+1),sigmav(n-1)
real(kind(1d0)) :: db1(n),du1(n),gu1(n,2),hu1(2,n),bu1(2,2,n),dv1(n),gv1(n,3),hv1(3,n),bv1(3,3,n)
real(kind(1d0)) :: aa(2,2),bb(2,2),a44,b44,wr1,wr2,wi,scale1,scale2
real(kind(1d0)) :: ascale,bscale,a11,a12,a21,a22,a32,a33,a34,a43,b11,b12,b22,b33,b34
real(kind(1d0)) :: Q3(3,3),initialQ(3,3),a3(3),sv3(3),start_vector(3),auxvec(2)
real(kind(1d0)) :: ad11,ad21,ad12,ad22,u12,ad11l,ad21l,ad22l,ad12l,ad32l,u12l 
real(kind(1d0)) :: AAA(n,n),BBB(n,n)

! External functions

double precision dlamch
external dlamch

eps=epsilon(1d0)
tol=sqrt(n*1.d0)
numiter_global=0
numiter_local=0
max_local_iter=13


! Compute initial quasiseparable generators

call companion_generators(n,poly,sigmaa,gv,hv,bv,z,w,db,gu,hu,bu,p,q)

!call recover_A(AAA,sigmaa,gv,hv,bv,z,w,n)  
!call recover_B(BBB,db,gu,hu,bu,p,q,n)
!write(*,*),AAA
!write(*,*),BBB


SAFMIN = DLAMCH('S')

kfin=n
conv_flag=0
do while (conv_flag==0)
   ! Check deflation on B 
   Bdefl=1
   do while (Bdefl==1)
      k=kfin-1
      do while (k>0 .AND. abs(db(k))>tol*eps)
         k=k-1
      enddo   
      if (k>0) then
         write(*,*) "Deflation on B is needed." 
         ! deflation on B currently not implemented
         return
      else
      Bdefl=0   
      end if
   enddo
   
   ! Check deflation on A and choose kfin
   dflag=1
   do while (dflag==1)
    if (abs(sigmaa(kfin-2))<tol*eps) then
           ! Deflation of a 2x2 block
           ! write(*,*)"Double deflation."
           aa(1,1) = (gv(kfin-1,1)*hv(1,kfin-1)+gv(kfin-1,2)*hv(2,kfin-1)-z(kfin-1)*w(kfin-1))
           auxvec=matmul(bv(:,:,kfin-1),hv(:,kfin))
           aa(1,2) = gv(kfin-1,1)*auxvec(1)+gv(kfin-1,2)*auxvec(2)-z(kfin-1)*w(kfin)
           aa(2,1) = sigmaa(kfin-1)
           aa(2,2) = gv(kfin,1)*hv(1,kfin)+gv(kfin,2)*hv(2,kfin)-z(kfin)*w(kfin)
           bb(1,1) = db(kfin-1)
           bb(2,2) = db(kfin)
           bb(1,2) = gu(kfin-1)*hu(kfin)-p(kfin-1)*q(kfin)
           bb(2,1) = 0.d0          
        
           call DLAG2(aa,2,bb,2,SAFMIN,scale1,scale2,wr1,wr2,wi)
                
           myeig(kfin-1)=(wr1*(1.d0,0.d0)-wi*(0.d0,1.d0))/scale1
           myeig(kfin)=(wr2*(1.d0,0.d0)+wi*(0.d0,1.d0))/scale2
           kfin=kfin-2
           numiter_local=0
        
       else if (abs(sigmaa(kfin-1))<tol*eps) then
           ! Deflation of a 1x1 block
           ! write(*,*)"Single deflation."
           a44 = (gv(kfin,1)*hv(1,kfin)+gv(kfin,2)*hv(2,kfin)-z(kfin)*w(kfin))
           b44 = db(kfin)
           myeig(kfin)=(a44/b44)*(1.d0,0.d0)
           kfin=kfin-1
           numiter_local=0
       else
           dflag=0    
       end if
       if (kfin<=2) then
           conv_flag=1
           dflag=0
       end if
    end do ! all deflations done   

      
   if (conv_flag==0) then 
    

   ! Decide whether to apply single or double shift. 
    aa(1,1) = (gv(kfin-1,1)*hv(1,kfin-1)+gv(kfin-1,2)*hv(2,kfin-1)-z(kfin-1)*w(kfin-1))
    auxvec=matmul(bv(:,:,kfin-1),hv(:,kfin))
    aa(1,2) = gv(kfin-1,1)*auxvec(1)+gv(kfin-1,2)*auxvec(2)-z(kfin-1)*w(kfin)
    aa(2,1) = sigmaa(kfin-1)
    aa(2,2) = (gv(kfin,1)*hv(1,kfin)+gv(kfin,2)*hv(2,kfin)-z(kfin)*w(kfin))
    bb(1,1) = db(kfin-1)
    bb(2,2) = db(kfin)
    bb(1,2) = gu(kfin-1)*hu(kfin)-p(kfin-1)*q(kfin)
    bb(2,1) = 0.d0
    
    call DLAG2(aa,2,bb,2,SAFMIN,scale1,scale2,wr1,wr2,wi)

     if (wi==0.d0) then

       !************************
       ! single shift QZ
       !************************
       
       ! write(*,*)"Single shift."
    
       ! Choose kin with single subdiag criterion
       kin=kfin-1
       tkin=gv(kin,1)*hv(1,kin)+gv(kin,2)*hv(2,kin)
       ttt=abs(tkin-z(kin)*(w(kin)))
       ! do while (kin>1 .AND. abs(sigmaa(kin-1))>eps*ttt)
       do while (kin>1 .AND. abs(sigmaa(kin-1))>eps*tol) ! choose criterion
        kin=kin-1
       enddo
   
       !if (kin>0) then
         ! Do the "magic trick" on A, increase kin if necessary
         j=kfin-1
         if (numiter_local<max_local_iter) then
           tj=gv(j,1)*hv(1,j)+gv(j,2)*hv(2,j)
           tkfin=gv(kfin,1)*hv(1,kfin)+gv(kfin,2)*hv(2,kfin)
           xx(1)=db(kfin)*(tj-z(j)*w(j))-(tkfin-z(kfin)*w(kfin))*db(j)
           xx(2)=db(kfin)*sigmaa(j)
         else
           write(*,*) "Exceptional shift"   
           call random_number(w1rand) 
           xx(1)=(2.d0)*w1rand-1.d0  
           call random_number(w2rand) 
           xx(2)=(2.d0)*w2rand-1.d0 
         end if   
         call givens(xx,G)
         if (j>1) then ! do the "magic trick", otherwise we already know kin=1
           yy(1)=sigmaa(j-1)*G(1,1)
           yy(2)=sigmaa(j-1)*G(2,1)
           do while (j>kin .AND. abs(yy(2))>eps*tol)
             j=j-1
             if (numiter_local<max_local_iter) then
               ! Choose shift
               tj=gv(j,1)*hv(1,j)+gv(j,2)*hv(2,j)
               tkfin=gv(kfin,1)*hv(1,kfin)+gv(kfin,2)*hv(2,kfin)
               xx(1)=db(kfin)*(tj-z(j)*w(j))-(tkfin-z(kfin)*w(kfin))*db(j)
               xx(2)=db(kfin)*sigmaa(j)
               call givens(xx,G)            
             end if
             if (j>1) then
               yy=sigmaa(j-1)*G(:,1)
             end if
           enddo
         
           ! Update kin
           kin=j
         else      
           kin=1
         end if
         ! Reinitialize counter after exceptional shift
         if (numiter_local>=max_local_iter) then
            numiter_local=0
         end if

         ! Call single shift QZ subroutine
         call structured_QZ_ss(sigmaa,gv,hv,bv,gu,hu,bu,db,p,q,z,w,sigmaanew,&
&gvnew,hvnew,bvnew,gunew,hunew,bunew,db1,p1,q1,z1,w1,n,kin,kfin,G)    
    
         else
           !************************
           ! double shift QZ
           !************************
           
          !  write(*,*)"Double shift."
           kin=kfin-1
           kin_flag=0
           do while ((kin_flag==0).and.(kin>1))
             kin=kin-1
             !ascale=1/norm(A); % to be updated later if necessary
             !bscale=1/norm(B);
             ascale=1.d0
             bscale=1.d0;
             a11 = (gv(kin,1)*hv(1,kin)+gv(kin,2)*hv(2,kin)-z(kin)*w(kin))
             a21 = sigmaa(kin)
             a32 = sigmaa(kin+1)
             auxvec=matmul(bv(:,:,kin),hv(:,kin+1))
             a12 = (gv(kin,1)*auxvec(1)+gv(kin,2)*auxvec(2)-z(kin)*w(kin+1))
             a22 = (gv(kin+1,1)*hv(1,kin+1)+gv(kin+1,2)*hv(2,kin+1)-z(kin+1)*w(kin+1))
             a33 = (gv(kfin-1,1)*hv(1,kfin-1)+gv(kfin-1,2)*hv(2,kfin-1)-z(kfin-1)*w(kfin-1))
             auxvec=matmul(bv(:,:,kfin-1),hv(:,kfin))
             a34 = gv(kfin-1,1)*auxvec(1)+gv(kfin-1,2)*auxvec(2)-z(kfin-1)*w(kfin)
             a43 = sigmaa(kfin-1)
             a44 = (gv(kfin,1)*hv(1,kfin)+gv(kfin,2)*hv(2,kfin)-z(kfin)*w(kfin))
             b11 = db(kin)
             b22 = db(kin+1)
       	     b33 = db(kfin-1)
             b44 = db(kfin)
             b12 = gu(kin)*hu(kin+1)-p(kin)*q(kin+1)
             b34 = gu(kfin-1)*hu(kfin)-p(kfin-1)*q(kfin)
             ad11 = ascale*a33/(bscale*b33)
             ad21 = ascale*a43/(bscale*b33)
             ad12 = ascale*a34/(bscale*b44)
             ad22 = ascale*a44/(bscale*b44)
             u12 = b34/b44
             ad11l = ascale*a11/(bscale*b11)
             ad21l = ascale*a21/(bscale*b11)
             ad12l = ascale*a12/(bscale*b22)
             ad22l = ascale*a22/(bscale*b22)
             ad32l = ascale*a32/(bscale*b22)
             u12l = b12/b22
             a3(1) = (ad11-ad11l)*(ad22-ad11l)-ad12*ad21+ad21*u12*ad11l+(ad12l-ad11l*u12l)*ad21l
             a3(2) = ((ad22l-ad11l)-ad21l*u12l-(ad11-ad11l)-(ad22-ad11l)+ad21*u12)*ad21l
             a3(3) = ad32l*ad21l     
            
             call hh3(a3,Q3)     
             initialQ=transpose(Q3)
       
             if (kin>1) then
                sv3(1)=sigmaa(kin-1)
                sv3(2)=0.d0
                sv3(3)=0.d0
                start_vector=matmul(transpose(initialQ),sv3)
                !if (sqrt(start_vector(2)*start_vector(2)+start_vector(3)*start_vector(3))<eps*normpoly) then
                if (abs(start_vector(2))+abs(start_vector(3))<eps*tol) then

                   kin_flag=1
                  
                end if
             end if
           end do
    
           ! implement exceptional shift
           if (numiter_local>max_local_iter) then
              write(*,*)"Exceptional shift."
              numiter_local=0
              call random_number(a3(1)) 
              call random_number(a3(2))
              call random_number(a3(3))  
              call hh3(a3,Q3)     
              initialQ=transpose(Q3)
              
              ! apply magic trick to find kin
              kin=kfin-1
              kin_flag=0
              do while ((kin_flag==0).and.(kin>1))
                 kin=kin-1
                 if (kin>1) then
                    sv3(1)=sigmaa(kin-1)
                    sv3(2)=0.d0
                    sv3(3)=0.d0
                    start_vector=matmul(transpose(initialQ),sv3)
                    if (start_vector(2)*start_vector(2)+start_vector(3)*start_vector(3)<eps*tol) then
                        kin_flag=1
                    end if
                 end if
              end do
           end if ! end if-then for exceptional shift
    
           ! call double shift subroutine
   
           call structured_QZ_ds(sigmaa,gv,hv,bv,z,w,db,gu,hu,bu,p,q,sigmaanew,gvnew,hvnew,bvnew,z1,w1,db1,&
&gunew,hunew,bunew,p1,q1,n,kin,kfin,initialQ)    
           
           end if                       
   
           ! update generators
           gv=gvnew
           hv=hvnew
           bv=bvnew
           sigmaa=sigmaanew
           z=z1
           w=w1
        
           gu=gunew
           hu=hunew
           bu=bunew
           db=db1
           p=p1
           q=q1
   
    
        numiter_global=numiter_global+1
        numiter_local=numiter_local+1
        
     end if ! if conv_flag==0   
           
enddo

! last block
if (kfin==1) then
   myeig(1)=(gv(1,1)*hv(1,1)+gv(1,2)*hv(2,1)-z(1)*w(1))/db(1)
else if (kfin==2) then
   aa(1,1) = (gv(1,1)*hv(1,1)+gv(1,2)*hv(2,1)-z(1)*w(1))
   aa(2,1) = sigmaa(1)
   auxvec=matmul(bv(:,:,1),hv(:,2))
   aa(1,2) = gv(1,1)*auxvec(1)+gv(1,2)*auxvec(2)-z(1)*w(2)
   aa(2,2) = gv(2,1)*hv(1,2)+gv(2,2)*hv(2,2)-z(2)*w(2)
   bb(1,1) = db(1)
   bb(2,2) = db(2)
   bb(1,2) = gu(1)*hu(2)-p(1)*q(2)
   bb(2,1) = 0.d0
        
   call DLAG2(aa,2,bb,2,SAFMIN,scale1,scale2,wr1,wr2,wi)
        
   myeig(kfin-1)=(wr1*(1.d0,0.d0)-wi*(0.d0,1.d0))/scale1
   myeig(kfin)=(wr2*(1.d0,0.d0)+wi*(0.d0,1.d0))/scale2
else
   write(*,*) "Something is wrong...kfin=",kfin
endif   

!write(*,*)"Average number of iterations per eigenvalue:",numiter_global*1.d0/n
numiter=numiter_global*1.d0/n
return

end subroutine fast_companion_QZ_ds








!*************************************
!*************************************    
     

subroutine structured_QZ_ss(sigmaa,gv,hv,bv,gu,hu,bu,db,p,q,z,w,sigmaanew,gvnew,hvnew,bvnew,gunew,&
& hunew,bunew,db1,p1,q1,z1,w1,n,kin,kfin,G)      
      
! Perform structured QZ from kin to kfin  
   
! Recall that:
! - sigma,gv,hv,bv,z,w are parameters for the matrix A,
! - d,gu,hu,bu,p,q are parameters for the matrix B,
! - Qin is the initial Givens matrix (encoding the shift).

implicit none

integer, intent(in) :: n,kin,kfin
integer :: i,j,k

real(kind(1d0)), intent(in) :: sigmaa(n-1),gv(n,2),hv(2,n),bv(2,2,n-1),z(n),w(n)
real(kind(1d0)), intent(in) :: db(n),gu(n-1),hu(n),bu(n-1),p(n),q(n),G(2,2)

real(kind(1d0)), intent(out) :: sigmaanew(n-1),gvnew(n,2),hvnew(2,n),bvnew(2,2,n-1),w1(n),z1(n)
real(kind(1d0)), intent(out) :: gunew(n-1),hunew(n),bunew(n-1),p1(n),q1(n)

real(kind(1d0)) :: xx(2),tkin,tkfin,tj,tjm1,yy(2)
real(kind(1d0)) :: du(n),du1tilde(n+1),gu1tilde(n+1,2),hu1tilde(2,n+1),bu1tilde(2,2,n+1),Qin(2,2)
real(kind(1d0)) :: dv1tilde(n+1),gv1tilde(n+1,3),hv1tilde(3,n+2),bv1tilde(3,3,n+1),sigmav(n-1)
real(kind(1d0)) :: db1(n),du1(n),gu1(n-1,2),hu1(2,n),bu1(2,2,n),dv1(n),gv1(n,3),hv1(3,n),bv1(3,3,n)
real(kind(1d0)) :: QQ(2,2),epsi,auxmat(2,3),GGamma(2,4),fa,fv,phia(2),phiv(2),auxvec(2),chi
real(kind(1d0)) :: gamma,c,theta,fu,phiu,fb,phi,vareps,PPhi(2,2),auxmat2(2,2),Omega(2,2),ZZ(2,2)
real(kind(1d0)) :: newQ(2,2),auxmat24(2,4),auxmat32(3,2),Gammarow(2),auxmat22(2,2),htilde1,betav(3)
real(kind(1d0)) :: bs(2,2,n-1),gs(n,2),hs(2,n),Sold(2,3),Snew(2,3),yold,ynew,zvec(2),VV(2,2),AA(3,2)
real(kind(1d0)) :: auxvec3(3),FF(3,3),WW(2,2),xnum,yvec(2),znum,Effe(2,2),Zmat(2,2),CC(3,4),Gammap(2,4)
real(kind(1d0)) :: Lambda(2,3),betau(2),hutilde,hvtilde1,GG(2,2),auxmat34(3,4)




Qin=G

! In order to speed up the program:
! if kfin is large do the loop until the end,
! otherwise stop early
!*******************************************
if (kfin>n-8) then
!*******************************************

do j=1,n-1
   sigmav(j)=sigmaa(j)+z(j+1)*(w(j))
enddo   
epsi=gv(1,1)*hv(1,1)+gv(1,2)*hv(2,1)-z(1)*(w(1))
if (kin==1) then
    QQ=Qin
else
    QQ(1,1)=1.d0
    QQ(2,2)=1.d0
    QQ(1,2)=0.d0
    QQ(2,1)=0.d0
endif

! *** (1) ****

GGamma=0.d0
auxmat(1,1)=gv(1,1)*hv(1,1)+gv(1,2)*hv(2,1)
auxmat(1,2)=gv(1,1)*bv(1,1,1)+gv(1,2)*bv(2,1,1)
auxmat(1,3)=gv(1,1)*bv(1,2,1)+gv(1,2)*bv(2,2,1)
auxmat(2,1)=sigmav(1)
auxmat(2,2)=gv(2,1)
auxmat(2,3)=gv(2,2)
GGamma(:,1:3)=matmul(QQ,auxmat)

gv1tilde(2,:)=GGamma(1,1:3)
fv=GGamma(2,1)
phiv=GGamma(2,2:3)
auxvec(1)=QQ(1,1)*z(1)+QQ(1,2)*z(2)
auxvec(2)=QQ(2,1)*z(1)+QQ(2,2)*z(2)

z1(1)=auxvec(1)
chi=auxvec(2)
fa=fv-chi*(w(1))
phia=phiv


! *** (2) ****

gamma=w(1)
c=p(1)
theta=q(1)
du(1)=db(1)+p(1)*(q(1))
fu=du(1)
phiu=gu(1)
fb=db(1)
phi=gu(1)

! *** (3) ****

do k=1,n-2
   ! *** (3a) ****
   vareps=phi*hu(k+1)-c*(q(k+1))
   epsi=phia(1)*hv(1,k+1)+phia(2)*hv(2,k+1)-chi*(w(k+1))
   du(k+1)=db(k+1)+p(k+1)*(q(k+1))
   auxmat2(1,1)=fb
   auxmat2(1,2)=vareps
   auxmat2(2,1)=0.d0
   auxmat2(2,2)=db(k+1)
   PPhi=matmul(QQ,auxmat2)
  
   
   
   ! *** (3b) ****
   if (k>=kin) then
      xx(1)=(PPhi(2,1))
      xx(2)=(PPhi(2,2))
      call givens(xx,GG)
      ZZ(1,1)=(GG(2,1))
      ZZ(1,2)=(GG(1,1))
      ZZ(2,1)=(GG(2,2))
      ZZ(2,2)=(GG(1,2))
   else
      ZZ(1,1)=1.d0
      ZZ(1,2)=0.d0
      ZZ(2,1)=0.d0
      ZZ(2,2)=1.d0
   endif
   auxmat2(1,1)=fa
   auxmat2(1,2)=epsi
   auxmat2(2,1)=0.d0
   auxmat2(2,2)=sigmaa(k+1)
   
   Omega=matmul(auxmat2,ZZ)   
     
   
   ! *** (3c) ****
   if (k>=kin .AND. k<=kfin-2) then
      xx=Omega(:,1)
      call givens(xx,newQ)
      sigmaanew(k)=newQ(1,1)*xx(1)+newQ(1,2)*xx(2)
   else
      if (k+1==kin) then
         newQ=Qin
      else
         newQ(1,1)=1.d0
         newQ(1,2)=0.d0
         newQ(2,1)=0.d0
         newQ(2,2)=1.d0                     
      end if
      sigmaanew(k)=newQ(1,1)*Omega(1,1)+newQ(1,2)*Omega(2,1)
   endif
   
   
     
   
   ! *** (3d) ****
   auxmat24(1,1)=fv
   auxmat24(1,2)=phiv(1)*hv(1,k+1)+phiv(2)*hv(2,k+1)
   auxmat24(1,3)=phiv(1)*bv(1,1,k+1)+phiv(2)*bv(2,1,k+1)
   auxmat24(1,4)=phiv(1)*bv(1,2,k+1)+phiv(2)*bv(2,2,k+1)
   auxmat24(2,1)=z(k+2)*(gamma)
   auxmat24(2,2)=sigmav(k+1)
   auxmat24(2,3)=gv(k+2,1)
   auxmat24(2,4)=gv(k+2,2)
   Gammap=matmul(newQ,auxmat24)
   GGamma=Gammap
   auxmat2(1,1)=Gammap(1,1)*ZZ(1,1)+Gammap(1,2)*ZZ(2,1)
   auxmat2(1,2)=Gammap(1,1)*ZZ(1,2)+Gammap(1,2)*ZZ(2,2)
   auxmat2(2,1)=Gammap(2,1)*ZZ(1,1)+Gammap(2,2)*ZZ(2,1)
   auxmat2(2,2)=Gammap(2,1)*ZZ(1,2)+Gammap(2,2)*ZZ(2,2)
   
   GGamma(1,1)=auxmat2(1,1)
   GGamma(1,2)=auxmat2(1,2)
   GGamma(2,1)=auxmat2(2,1)
   GGamma(2,2)=auxmat2(2,2)
   auxmat34(1,1)=1.d0
   auxmat34(1,2)=0.d0
   auxmat34(1,3)=0.d0
   auxmat34(1,4)=0.d0
   auxmat34(2,1)=0.d0
   auxmat34(3,1)=0.d0
   auxmat34(2:3,2)=hv(:,k+1)
   auxmat34(2:3,3:4)=bv(:,:,k+1)
   CC=auxmat34
   auxmat32=matmul(auxmat34(:,1:2),ZZ)
   CC(1,1)=auxmat32(1,1)
   CC(1,2)=auxmat32(1,2)
   CC(2,1)=auxmat32(2,1)
   CC(2,2)=auxmat32(2,2)
   CC(3,1)=auxmat32(3,1)
   CC(3,2)=auxmat32(3,2)
   dv1tilde(k+2)=GGamma(1,1)
   gv1tilde(k+2,:)=GGamma(1,2:4)
   betav=GGamma(2,2:4)
   fv=betav(1)
   phiv=betav(2:3)
   hv1tilde(1,k+2)=CC(1,1)
   hv1tilde(2,k+2)=CC(2,1)
   hv1tilde(3,k+2)=CC(3,1)
   bv1tilde(1,1,k+2)=CC(1,2)
   bv1tilde(1,2,k+2)=CC(1,3)
   bv1tilde(1,3,k+2)=CC(1,4)
   bv1tilde(2,1,k+2)=CC(2,2)
   bv1tilde(2,2,k+2)=CC(2,3)
   bv1tilde(2,3,k+2)=CC(2,4)
   bv1tilde(3,1,k+2)=CC(3,2)
   bv1tilde(3,2,k+2)=CC(3,3)
   bv1tilde(3,3,k+2)=CC(3,4)
   ! *** (3e) ****
   auxmat(1,1)=fu
   auxmat(1,2)=phiu*hu(k+1)
   auxmat(1,3)=phiu*bu(k+1)
   auxmat(2,1)=p(k+1)*(theta)
   auxmat(2,2)=du(k+1)
   auxmat(2,3)=gu(k+1)
   auxmat2=matmul(auxmat(:,1:2),ZZ)
   auxmat(1,1)=auxmat2(1,1)
   auxmat(1,2)=auxmat2(1,2)
   auxmat(2,1)=auxmat2(2,1)
   auxmat(2,2)=auxmat2(2,2)
   Lambda=matmul(QQ,auxmat)
   auxmat(1,1)=1.d0
   auxmat(1,2)=0.d0
   auxmat(1,3)=0.d0
   auxmat(2,1)=0.d0
   auxmat(2,2)=hu(k+1)
   auxmat(2,3)=bu(k+1)
   auxmat2=matmul(auxmat(:,1:2),ZZ)
   auxmat(1,1)=auxmat2(1,1)
   auxmat(1,2)=auxmat2(1,2)
   auxmat(2,1)=auxmat2(2,1)
   auxmat(2,2)=auxmat2(2,2)
   du1tilde(k)=Lambda(1,1)
   gu1tilde(k,1)=Lambda(1,2)
   gu1tilde(k,2)=Lambda(1,3)
   betau=Lambda(2,2:3)
   fu=betau(1)
   phiu=betau(2)
   hu1tilde(1,k+1)=auxmat(1,1)
   hu1tilde(2,k+1)=auxmat(2,1)
   bu1tilde(1,1,k+1)=auxmat(1,2)
   bu1tilde(1,2,k+1)=auxmat(1,3)
   bu1tilde(2,1,k+1)=auxmat(2,2)
   bu1tilde(2,2,k+1)=auxmat(2,3)
   ! *** (3f) ****
   auxvec(1)=newQ(1,1)*chi+newQ(1,2)*z(k+2)
   auxvec(2)=newQ(2,1)*chi+newQ(2,2)*z(k+2)
   z1(k+1)=auxvec(1)
   chi=auxvec(2)
   auxvec(1)=(ZZ(1,1))*gamma+(ZZ(2,1))*w(k+1)
   auxvec(2)=(ZZ(1,2))*gamma+(ZZ(2,2))*w(k+1)   
   w1(k)=auxvec(1)
   gamma=auxvec(2)
   ! *** (3g) ****
   auxvec(1)=(ZZ(1,1))*theta+(ZZ(2,1))*q(k+1)
   auxvec(2)=(ZZ(1,2))*theta+(ZZ(2,2))*q(k+1)
   q1(k)=auxvec(1)
   theta=auxvec(2)
   auxvec(1)=QQ(1,1)*c+QQ(1,2)*p(k+1)
   auxvec(2)=QQ(2,1)*c+QQ(2,2)*p(k+1)
   p1(k)=auxvec(1)
   c=auxvec(2)
   ! *** (3h) ****
   fa=fv-chi*(gamma)
   phia=phiv
   fb=fu-c*(theta)
   phi=phiu
   QQ=newQ  
   
enddo

   

! *** (4) ****

vareps=phi*hu(n)-c*(q(n))
du(n)=db(n)+p(n)*(q(n))
auxmat2(1,1)=fb
auxmat2(1,2)=vareps
auxmat2(2,1)=0.d0
auxmat2(2,2)=db(n)
PPhi=matmul(QQ,auxmat2)


! *** (5) ****

xx(1)=(PPhi(2,1))
xx(2)=(PPhi(2,2))
call givens(xx,GG)
ZZ(1,1)=(GG(2,1))
ZZ(1,2)=(GG(1,1))
ZZ(2,1)=(GG(2,2))
ZZ(2,2)=(GG(1,2))

! *** (6) ****

auxvec(1)=fv
auxvec(2)=phiv(1)*hv(1,n)+phiv(2)*hv(2,n)
Gammarow(1)=auxvec(1)*ZZ(1,1)+auxvec(2)*ZZ(2,1)
Gammarow(2)=auxvec(1)*ZZ(1,2)+auxvec(2)*ZZ(2,2)
auxmat32(1,1)=1.d0
auxmat32(1,2)=0.d0
auxmat32(2,1)=0.d0
auxmat32(2,2)=hv(1,n)
auxmat32(3,1)=0.d0
auxmat32(3,2)=hv(2,n)
CC(:,1:2)=matmul(auxmat32,ZZ)
hvtilde1=1.d0
dv1tilde(n+1)=Gammarow(1)
gv1tilde(n+1,1)=Gammarow(2)
gv1tilde(n+1,2)=0.d0
gv1tilde(n+1,3)=0.d0
hv1tilde(1,n+1)=CC(1,1)
hv1tilde(2,n+1)=CC(2,1)
hv1tilde(3,n+1)=CC(3,1)
bv1tilde(1,1,n+1)=CC(1,2)
bv1tilde(2,1,n+1)=CC(2,2)
bv1tilde(3,1,n+1)=CC(3,2)
bv1tilde(1,2,n+1)=0.d0
bv1tilde(2,2,n+1)=0.d0
bv1tilde(3,2,n+1)=0.d0
bv1tilde(1,3,n+1)=0.d0
bv1tilde(2,3,n+1)=0.d0
bv1tilde(3,3,n+1)=0.d0




! *** (7) ****

auxmat2(1,1)=fu
auxmat2(1,2)=phiu*hu(n)
auxmat2(2,1)=p(n)*(theta)
auxmat2(2,2)=du(n)
auxmat22=matmul(auxmat2,ZZ)
Lambda(:,1:2)=matmul(QQ,auxmat22)
auxmat2(1,1)=1.d0
auxmat2(1,2)=0.d0
auxmat2(2,1)=0.d0
auxmat2(2,2)=hu(n)
auxmat22=matmul(auxmat2,ZZ)
hutilde=1.d0
du1tilde(n-1)=Lambda(1,1)
gu1tilde(n-1,1)=Lambda(1,2)
gu1tilde(n-1,2)=0.d0
du1tilde(N)=Lambda(2,2)
hu1tilde(1,n)=auxmat22(1,1)
hu1tilde(2,n)=auxmat22(2,1)
bu1tilde(1,1,n)=auxmat22(1,2)
bu1tilde(2,1,n)=auxmat22(2,2)
bu1tilde(1,2,n)=0.d0
bu1tilde(2,2,n)=0.d0
hv1tilde(1,n+2)=1.d0
hv1tilde(2,n+2)=0.d0
hv1tilde(3,n+2)=0.d0
hu1tilde(1,n+1)=1.d0
hu1tilde(2,n+1)=0.d0

! *** (8) ****

w1(n-1)=(ZZ(1,1))*gamma+(ZZ(2,1))*w(n)
w1(n)=(ZZ(1,2))*gamma+(ZZ(2,2))*w(n)
p1(n-1)=QQ(1,1)*c+QQ(1,2)*p(n)
p1(n)=QQ(2,1)*c+QQ(2,2)*p(n)
q1(n-1)=(ZZ(1,1))*theta+(ZZ(2,1))*q(n)
q1(n)=(ZZ(1,2))*theta+(ZZ(2,2))*q(n)
z1(n)=chi
sigmaanew(n-1)=dv1tilde(n+1)-z1(n)*(w1(n-1))


! *** (9) ****

do j=1,n
   db1(j)=du1tilde(j)-p1(j)*(q1(j))
enddo

! *** (10) ****

do j=1,n
   gv1(j,1)=gv1tilde(j+1,1)
   gv1(j,2)=gv1tilde(j+1,2)
   gv1(j,3)=gv1tilde(j+1,3)
   hv1(1,j)=hv1tilde(1,j+2)
   hv1(2,j)=hv1tilde(2,j+2)
   hv1(3,j)=hv1tilde(3,j+2)
enddo
do j=1,n-1
   bv1(1,1,j)=bv1tilde(1,1,j+2)
   bv1(1,2,j)=bv1tilde(1,2,j+2)
   bv1(1,3,j)=bv1tilde(1,3,j+2)
   bv1(2,1,j)=bv1tilde(2,1,j+2)
   bv1(2,2,j)=bv1tilde(2,2,j+2)
   bv1(2,3,j)=bv1tilde(2,3,j+2)
   bv1(3,1,j)=bv1tilde(3,1,j+2)
   bv1(3,2,j)=bv1tilde(3,2,j+2)
   bv1(3,3,j)=bv1tilde(3,3,j+2)
enddo
do j=2,n
   dv1(j)=dv1tilde(j+1)
enddo
do j=1,n-1
   gu1(j,1)=gu1tilde(j,1)
   gu1(j,2)=gu1tilde(j,2)
enddo
do j=1,n
   hu1(1,j)=hu1tilde(1,j+1)
   hu1(2,j)=hu1tilde(2,j+1)
   du1(j)=du1tilde(j)
enddo
do j=2,n-1
   bu1(1,1,j)=bu1tilde(1,1,j+1)
   bu1(1,2,j)=bu1tilde(1,2,j+1)
   bu1(2,1,j)=bu1tilde(2,1,j+1)
   bu1(2,2,j)=bu1tilde(2,2,j+1)
enddo
! Now we have redundant generators for the updated A and B



! *** (11) ****
! Compression of V

Sold(1,1)=gv1(1,1)
Sold(1,2)=gv1(1,2)
Sold(1,3)=gv1(1,3)
gs(1,1)=1.d0
gs(1,2)=0.d0
hs(1,1)=Sold(1,1)*hv1(1,1)+Sold(1,2)*hv1(2,1)+Sold(1,3)*hv1(3,1)
hs(2,1)=0.d0

gs(2,1)=0.d0
gs(2,2)=1.d0
bs(1,1,1)=1.d0
bs(1,2,1)=0.d0
bs(2,1,1)=0.d0
bs(2,2,1)=0.d0
Snew(1,1)=Sold(1,1)*bv1(1,1,1)+Sold(1,2)*bv1(2,1,1)+Sold(1,3)*bv1(3,1,1)
Snew(1,2)=Sold(1,1)*bv1(1,2,1)+Sold(1,2)*bv1(2,2,1)+Sold(1,3)*bv1(3,2,1)
Snew(1,3)=Sold(1,1)*bv1(1,3,1)+Sold(1,2)*bv1(2,3,1)+Sold(1,3)*bv1(3,3,1)
Snew(2,1)=gv1(2,1)
Snew(2,2)=gv1(2,2)
Snew(2,3)=gv1(2,3)
hs(1,2)=Snew(1,1)*hv1(1,2)+Snew(1,2)*hv1(2,2)+Snew(1,3)*hv1(3,2)
hs(2,2)=Snew(2,1)*hv1(1,2)+Snew(2,2)*hv1(2,2)+Snew(2,3)*hv1(3,2)

zvec(1)=gs(1,1)*hs(1,1)+gs(1,2)*hs(2,1)
zvec(2)=dv1(2)
yold=w1(1)


do k=2,n-1
   auxvec(1)=(yold)
   auxvec(2)=(w1(k))
   call givensr(auxvec,VV)
   ynew=auxvec(1)*VV(1,2)+auxvec(2)*VV(2,2)
   auxmat32(1,1)=zvec(1)
   auxmat32(2,1)=zvec(2)
   auxmat32(1,2)=Snew(1,1)*hv1(1,k)+Snew(1,2)*hv1(2,k)+Snew(1,3)*hv1(3,k)
   auxmat32(2,2)=Snew(2,1)*hv1(1,k)+Snew(2,2)*hv1(2,k)+Snew(2,3)*hv1(3,k)
   auxmat32(3,1)=z1(k+1)*(yold)
   auxmat32(3,2)=dv1(k+1)
   AA=matmul(auxmat32,VV)
   yold=ynew
   auxvec3=AA(:,1)
   call hh3(auxvec3,FF)
   Sold=Snew
   
   auxmat(1,1)=Sold(1,1)*bv1(1,1,k)+Sold(1,2)*bv1(2,1,k)+Sold(1,3)*bv1(3,1,k)
   auxmat(1,2)=Sold(1,1)*bv1(1,2,k)+Sold(1,2)*bv1(2,2,k)+Sold(1,3)*bv1(3,2,k)
   auxmat(1,3)=Sold(1,1)*bv1(1,3,k)+Sold(1,2)*bv1(2,3,k)+Sold(1,3)*bv1(3,3,k)
   auxmat(2,1)=Sold(2,1)*bv1(1,1,k)+Sold(2,2)*bv1(2,1,k)+Sold(2,3)*bv1(3,1,k)
   auxmat(2,2)=Sold(2,1)*bv1(1,2,k)+Sold(2,2)*bv1(2,2,k)+Sold(2,3)*bv1(3,2,k)
   auxmat(2,3)=Sold(2,1)*bv1(1,3,k)+Sold(2,2)*bv1(2,3,k)+Sold(2,3)*bv1(3,3,k)
 
   zvec(1)=FF(2,1)*AA(1,2)+FF(2,2)*AA(2,2)+FF(2,3)*AA(3,2)       ! Bb(2,1)
   zvec(2)=FF(3,1)*AA(1,2)+FF(3,2)*AA(2,2)+FF(3,3)*AA(3,2)       ! Bb(3,1)  
   bs(1,1,k)=(FF(2,1))
   bs(1,2,k)=(FF(3,1))
   bs(2,1,k)=(FF(2,2))
   bs(2,2,k)=(FF(3,2))
   gs(k+1,1)=(FF(2,3))
   gs(k+1,2)=(FF(3,3))
   Snew(1,1)=(gs(k+1,1))*gv1(k+1,1)+(bs(1,1,k))*auxmat(1,1)+(bs(2,1,k))*auxmat(2,1)
   Snew(1,2)=(gs(k+1,1))*gv1(k+1,2)+(bs(1,1,k))*auxmat(1,2)+(bs(2,1,k))*auxmat(2,2)
   Snew(1,3)=(gs(k+1,1))*gv1(k+1,3)+(bs(1,1,k))*auxmat(1,3)+(bs(2,1,k))*auxmat(2,3)
   Snew(2,1)=(gs(k+1,2))*gv1(k+1,1)+(bs(1,2,k))*auxmat(1,1)+(bs(2,2,k))*auxmat(2,1)
   Snew(2,2)=(gs(k+1,2))*gv1(k+1,2)+(bs(1,2,k))*auxmat(1,2)+(bs(2,2,k))*auxmat(2,2)
   Snew(2,3)=(gs(k+1,2))*gv1(k+1,3)+(bs(1,2,k))*auxmat(1,3)+(bs(2,2,k))*auxmat(2,3)
   hs(1,k+1)=Snew(1,1)*hv1(1,k+1)+Snew(1,2)*hv1(2,k+1)+Snew(1,3)*hv1(3,k+1)
   hs(2,k+1)=Snew(2,1)*hv1(1,k+1)+Snew(2,2)*hv1(2,k+1)+Snew(2,3)*hv1(3,k+1)

enddo


gvnew=gs
hvnew=hs
bvnew=bs


! *** (12) ****
! Compression of U

xnum=q1(1)
yvec=gu1(1,:)
znum=du1(1)
hunew(1)=0.d0
hunew(2)=gu1(1,1)*hu1(1,2)+gu1(1,2)*hu1(2,2)
gunew(1)=1.d0
do k=2,n-1
  xx(1)=(xnum)
  xx(2)=(q1(k))
  call givensr(xx,WW)
  auxmat2(1,1)=znum
  auxmat2(1,2)=hunew(k)
  auxmat2(2,1)=p1(k)*(xnum)
  auxmat2(2,2)=du1(k)
  Zmat=matmul(auxmat2,WW)
  xnum=xx(1)*WW(1,2)+xx(2)*WW(2,2)
  xx=Zmat(:,1)
  call givens(xx,auxmat22)
 
  bunew(k)=(auxmat22(2,1))
  gunew(k)=(auxmat22(2,2))
  auxvec=yvec
  yvec(1)=(gunew(k))*gu1(k,1)+(bunew(k))*(auxvec(1)*bu1(1,1,k)+auxvec(2)*bu1(2,1,k))
  yvec(2)=(gunew(k))*gu1(k,2)+(bunew(k))*(auxvec(1)*bu1(1,2,k)+auxvec(2)*bu1(2,2,k))
  znum=(gunew(k))*Zmat(2,2)+(bunew(k))*Zmat(1,2)
  hunew(k+1)=yvec(1)*hu1(1,k+1)+yvec(2)*hu1(2,k+1)
enddo




!**********************************
! if kfin is small, stop loop early
! (update of generators will be incomplete,
! checks on whole matrices won't be useful here)
else
!**********************************

do j=1,n-1
   sigmav(j)=sigmaa(j)+z(j+1)*(w(j))
enddo   
epsi=gv(1,1)*hv(1,1)+gv(1,2)*hv(2,1)-z(1)*(w(1))
if (kin==1) then
    QQ=Qin
else
    QQ(1,1)=1.d0
    QQ(2,2)=1.d0
    QQ(1,2)=0.d0
    QQ(2,1)=0.d0
endif

! *** (1) ****

GGamma=0.d0
auxmat(1,1)=gv(1,1)*hv(1,1)+gv(1,2)*hv(2,1)
auxmat(1,2)=gv(1,1)*bv(1,1,1)+gv(1,2)*bv(2,1,1)
auxmat(1,3)=gv(1,1)*bv(1,2,1)+gv(1,2)*bv(2,2,1)
auxmat(2,1)=sigmav(1)
auxmat(2,2)=gv(2,1)
auxmat(2,3)=gv(2,2)
GGamma(:,1:3)=matmul(QQ,auxmat)

gv1tilde(2,:)=GGamma(1,1:3)
fv=GGamma(2,1)
phiv=GGamma(2,2:3)
auxvec(1)=QQ(1,1)*z(1)+QQ(1,2)*z(2)
auxvec(2)=QQ(2,1)*z(1)+QQ(2,2)*z(2)

z1(1)=auxvec(1)
chi=auxvec(2)
fa=fv-chi*(w(1))
phia=phiv


! *** (2) ****

gamma=w(1)
c=p(1)
theta=q(1)
du(1)=db(1)+p(1)*(q(1))
fu=du(1)
phiu=gu(1)
fb=db(1)
phi=gu(1)

! *** (3) ****

do k=1,kfin+3
   ! *** (3a) ****
   vareps=phi*hu(k+1)-c*(q(k+1))
   epsi=phia(1)*hv(1,k+1)+phia(2)*hv(2,k+1)-chi*(w(k+1))
   du(k+1)=db(k+1)+p(k+1)*(q(k+1))
   auxmat2(1,1)=fb
   auxmat2(1,2)=vareps
   auxmat2(2,1)=0.d0
   auxmat2(2,2)=db(k+1)
   PPhi=matmul(QQ,auxmat2)
  
   
   
   ! *** (3b) ****
   if (k>=kin) then
      xx(1)=(PPhi(2,1))
      xx(2)=(PPhi(2,2))
      call givens(xx,GG)
      ZZ(1,1)=(GG(2,1))
      ZZ(1,2)=(GG(1,1))
      ZZ(2,1)=(GG(2,2))
      ZZ(2,2)=(GG(1,2))
   else
      ZZ(1,1)=1.d0
      ZZ(1,2)=0.d0
      ZZ(2,1)=0.d0
      ZZ(2,2)=1.d0
   endif
   auxmat2(1,1)=fa
   auxmat2(1,2)=epsi
   auxmat2(2,1)=0.d0
   auxmat2(2,2)=sigmaa(k+1)
   
   Omega=matmul(auxmat2,ZZ)   
   
     
   ! *** (3c) ****
   if (k>=kin .AND. k<=kfin-2) then
      xx=Omega(:,1)
      call givens(xx,newQ)
      sigmaanew(k)=newQ(1,1)*xx(1)+newQ(1,2)*xx(2)
   else
      if (k+1==kin) then
         newQ=Qin
      else
         newQ(1,1)=1.d0
         newQ(1,2)=0.d0
         newQ(2,1)=0.d0
         newQ(2,2)=1.d0                     
      end if
      sigmaanew(k)=newQ(1,1)*Omega(1,1)+newQ(1,2)*Omega(2,1)
   endif
      
   
   
   ! *** (3d) ****
   auxmat24(1,1)=fv
   auxmat24(1,2)=phiv(1)*hv(1,k+1)+phiv(2)*hv(2,k+1)
   auxmat24(1,3)=phiv(1)*bv(1,1,k+1)+phiv(2)*bv(2,1,k+1)
   auxmat24(1,4)=phiv(1)*bv(1,2,k+1)+phiv(2)*bv(2,2,k+1)
   auxmat24(2,1)=z(k+2)*(gamma)
   auxmat24(2,2)=sigmav(k+1)
   auxmat24(2,3)=gv(k+2,1)
   auxmat24(2,4)=gv(k+2,2)
   Gammap=matmul(newQ,auxmat24)
   GGamma=Gammap
   
   auxmat2(1,1)=Gammap(1,1)*ZZ(1,1)+Gammap(1,2)*ZZ(2,1)
   auxmat2(1,2)=Gammap(1,1)*ZZ(1,2)+Gammap(1,2)*ZZ(2,2)
   auxmat2(2,1)=Gammap(2,1)*ZZ(1,1)+Gammap(2,2)*ZZ(2,1)
   auxmat2(2,2)=Gammap(2,1)*ZZ(1,2)+Gammap(2,2)*ZZ(2,2)
   
   GGamma(1,1)=auxmat2(1,1)
   GGamma(1,2)=auxmat2(1,2)
   GGamma(2,1)=auxmat2(2,1)
   GGamma(2,2)=auxmat2(2,2)
   auxmat34(1,1)=1.d0
   auxmat34(1,2)=0.d0
   auxmat34(1,3)=0.d0
   auxmat34(1,4)=0.d0
   auxmat34(2,1)=0.d0
   auxmat34(3,1)=0.d0
   auxmat34(2:3,2)=hv(:,k+1)
   auxmat34(2:3,3:4)=bv(:,:,k+1)
   CC=auxmat34
   auxmat32=matmul(auxmat34(:,1:2),ZZ)
   CC(1,1)=auxmat32(1,1)
   CC(1,2)=auxmat32(1,2)
   CC(2,1)=auxmat32(2,1)
   CC(2,2)=auxmat32(2,2)
   CC(3,1)=auxmat32(3,1)
   CC(3,2)=auxmat32(3,2)
   dv1tilde(k+2)=GGamma(1,1)
   gv1tilde(k+2,:)=GGamma(1,2:4)
   betav=GGamma(2,2:4)
   fv=betav(1)
   phiv=betav(2:3)
   hv1tilde(1,k+2)=CC(1,1)
   hv1tilde(2,k+2)=CC(2,1)
   hv1tilde(3,k+2)=CC(3,1)
   bv1tilde(1,1,k+2)=CC(1,2)
   bv1tilde(1,2,k+2)=CC(1,3)
   bv1tilde(1,3,k+2)=CC(1,4)
   bv1tilde(2,1,k+2)=CC(2,2)
   bv1tilde(2,2,k+2)=CC(2,3)
   bv1tilde(2,3,k+2)=CC(2,4)
   bv1tilde(3,1,k+2)=CC(3,2)
   bv1tilde(3,2,k+2)=CC(3,3)
   bv1tilde(3,3,k+2)=CC(3,4)
   ! *** (3e) ****
   auxmat(1,1)=fu
   auxmat(1,2)=phiu*hu(k+1)
   auxmat(1,3)=phiu*bu(k+1)
   auxmat(2,1)=p(k+1)*(theta)
   auxmat(2,2)=du(k+1)
   auxmat(2,3)=gu(k+1)
   auxmat2=matmul(auxmat(:,1:2),ZZ)
   auxmat(1,1)=auxmat2(1,1)
   auxmat(1,2)=auxmat2(1,2)
   auxmat(2,1)=auxmat2(2,1)
   auxmat(2,2)=auxmat2(2,2)
   Lambda=matmul(QQ,auxmat)
   auxmat(1,1)=1.d0
   auxmat(1,2)=0.d0
   auxmat(1,3)=0.d0
   auxmat(2,1)=0.d0
   auxmat(2,2)=hu(k+1)
   auxmat(2,3)=bu(k+1)
   auxmat2=matmul(auxmat(:,1:2),ZZ)
   auxmat(1,1)=auxmat2(1,1)
   auxmat(1,2)=auxmat2(1,2)
   auxmat(2,1)=auxmat2(2,1)
   auxmat(2,2)=auxmat2(2,2)
   du1tilde(k)=Lambda(1,1)
   gu1tilde(k,1)=Lambda(1,2)
   gu1tilde(k,2)=Lambda(1,3)
   betau=Lambda(2,2:3)
   fu=betau(1)
   phiu=betau(2)
   hu1tilde(1,k+1)=auxmat(1,1)
   hu1tilde(2,k+1)=auxmat(2,1)
   bu1tilde(1,1,k+1)=auxmat(1,2)
   bu1tilde(1,2,k+1)=auxmat(1,3)
   bu1tilde(2,1,k+1)=auxmat(2,2)
   bu1tilde(2,2,k+1)=auxmat(2,3)
   ! *** (3f) ****
   auxvec(1)=newQ(1,1)*chi+newQ(1,2)*z(k+2)
   auxvec(2)=newQ(2,1)*chi+newQ(2,2)*z(k+2)
   z1(k+1)=auxvec(1)
   chi=auxvec(2)
   auxvec(1)=(ZZ(1,1))*gamma+(ZZ(2,1))*w(k+1)
   auxvec(2)=(ZZ(1,2))*gamma+(ZZ(2,2))*w(k+1)   
   w1(k)=auxvec(1)
   gamma=auxvec(2)
   ! *** (3g) ****
   auxvec(1)=(ZZ(1,1))*theta+(ZZ(2,1))*q(k+1)
   auxvec(2)=(ZZ(1,2))*theta+(ZZ(2,2))*q(k+1)
   q1(k)=auxvec(1)
   theta=auxvec(2)
   auxvec(1)=QQ(1,1)*c+QQ(1,2)*p(k+1)
   auxvec(2)=QQ(2,1)*c+QQ(2,2)*p(k+1)
   p1(k)=auxvec(1)
   c=auxvec(2)
   ! *** (3h) ****
   fa=fv-chi*(gamma)
   phia=phiv
   fb=fu-c*(theta)
   phi=phiu
   QQ=newQ  
   
enddo

   

! *** (4) ****

vareps=phi*hu(n)-c*(q(n))
du(n)=db(n)+p(n)*(q(n))
auxmat2(1,1)=fb
auxmat2(1,2)=vareps
auxmat2(2,1)=0.d0
auxmat2(2,2)=db(n)
PPhi=matmul(QQ,auxmat2)


! *** (5) ****

xx(1)=(PPhi(2,1))
xx(2)=(PPhi(2,2))
call givens(xx,GG)
ZZ(1,1)=(GG(2,1))
ZZ(1,2)=(GG(1,1))
ZZ(2,1)=(GG(2,2))
ZZ(2,2)=(GG(1,2))

! *** (6) ****
! empty, no need for last updates


! *** (7) ****
! empty, no need for last updates

! *** (8) ****
! empty, no need for last updates

! *** (9) ****

do j=1,kfin+3
   db1(j)=du1tilde(j)-p1(j)*(q1(j))
enddo

! *** (10) ****

do j=1,kfin+3
   gv1(j,1)=gv1tilde(j+1,1)
   gv1(j,2)=gv1tilde(j+1,2)
   gv1(j,3)=gv1tilde(j+1,3)
   hv1(1,j)=hv1tilde(1,j+2)
   hv1(2,j)=hv1tilde(2,j+2)
   hv1(3,j)=hv1tilde(3,j+2)
enddo
do j=1,kfin+3
   bv1(1,1,j)=bv1tilde(1,1,j+2)
   bv1(1,2,j)=bv1tilde(1,2,j+2)
   bv1(1,3,j)=bv1tilde(1,3,j+2)
   bv1(2,1,j)=bv1tilde(2,1,j+2)
   bv1(2,2,j)=bv1tilde(2,2,j+2)
   bv1(2,3,j)=bv1tilde(2,3,j+2)
   bv1(3,1,j)=bv1tilde(3,1,j+2)
   bv1(3,2,j)=bv1tilde(3,2,j+2)
   bv1(3,3,j)=bv1tilde(3,3,j+2)
enddo
do j=2,kfin+3
   dv1(j)=dv1tilde(j+1)
enddo
do j=1,kfin+3
   gu1(j,1)=gu1tilde(j,1)
   gu1(j,2)=gu1tilde(j,2)
   hu1(1,j)=hu1tilde(1,j+1)
   hu1(2,j)=hu1tilde(2,j+1)
   du1(j)=du1tilde(j)
enddo
do j=2,kfin+3
   bu1(1,1,j)=bu1tilde(1,1,j+1)
   bu1(1,2,j)=bu1tilde(1,2,j+1)
   bu1(2,1,j)=bu1tilde(2,1,j+1)
   bu1(2,2,j)=bu1tilde(2,2,j+1)
enddo
! Now we have redundant generators for the updated A and B



! *** (11) ****
! Compression of V

Sold(1,1)=gv1(1,1)
Sold(1,2)=gv1(1,2)
Sold(1,3)=gv1(1,3)
gs(1,1)=1.d0
gs(1,2)=0.d0
hs(1,1)=Sold(1,1)*hv1(1,1)+Sold(1,2)*hv1(2,1)+Sold(1,3)*hv1(3,1)
hs(2,1)=0.d0

gs(2,1)=0.d0
gs(2,2)=1.d0
bs(1,1,1)=1.d0
bs(1,2,1)=0.d0
bs(2,1,1)=0.d0
bs(2,2,1)=0.d0
Snew(1,1)=Sold(1,1)*bv1(1,1,1)+Sold(1,2)*bv1(2,1,1)+Sold(1,3)*bv1(3,1,1)
Snew(1,2)=Sold(1,1)*bv1(1,2,1)+Sold(1,2)*bv1(2,2,1)+Sold(1,3)*bv1(3,2,1)
Snew(1,3)=Sold(1,1)*bv1(1,3,1)+Sold(1,2)*bv1(2,3,1)+Sold(1,3)*bv1(3,3,1)
Snew(2,1)=gv1(2,1)
Snew(2,2)=gv1(2,2)
Snew(2,3)=gv1(2,3)
hs(1,2)=Snew(1,1)*hv1(1,2)+Snew(1,2)*hv1(2,2)+Snew(1,3)*hv1(3,2)
hs(2,2)=Snew(2,1)*hv1(1,2)+Snew(2,2)*hv1(2,2)+Snew(2,3)*hv1(3,2)

zvec(1)=gs(1,1)*hs(1,1)+gs(1,2)*hs(2,1)
zvec(2)=dv1(2)
yold=w1(1)


do k=2,kfin+3
   auxvec(1)=(yold)
   auxvec(2)=(w1(k))
   call givensr(auxvec,VV)
  
   ynew=auxvec(1)*VV(1,2)+auxvec(2)*VV(2,2)
   auxmat32(1,1)=zvec(1)
   auxmat32(2,1)=zvec(2)
   auxmat32(1,2)=Snew(1,1)*hv1(1,k)+Snew(1,2)*hv1(2,k)+Snew(1,3)*hv1(3,k)
   auxmat32(2,2)=Snew(2,1)*hv1(1,k)+Snew(2,2)*hv1(2,k)+Snew(2,3)*hv1(3,k)
   auxmat32(3,1)=z1(k+1)*(yold)
   auxmat32(3,2)=dv1(k+1)
   AA=matmul(auxmat32,VV)
   yold=ynew
   auxvec3=AA(:,1)
   call hh3(auxvec3,FF)
   Sold=Snew
   
   auxmat(1,1)=Sold(1,1)*bv1(1,1,k)+Sold(1,2)*bv1(2,1,k)+Sold(1,3)*bv1(3,1,k)
   auxmat(1,2)=Sold(1,1)*bv1(1,2,k)+Sold(1,2)*bv1(2,2,k)+Sold(1,3)*bv1(3,2,k)
   auxmat(1,3)=Sold(1,1)*bv1(1,3,k)+Sold(1,2)*bv1(2,3,k)+Sold(1,3)*bv1(3,3,k)
   auxmat(2,1)=Sold(2,1)*bv1(1,1,k)+Sold(2,2)*bv1(2,1,k)+Sold(2,3)*bv1(3,1,k)
   auxmat(2,2)=Sold(2,1)*bv1(1,2,k)+Sold(2,2)*bv1(2,2,k)+Sold(2,3)*bv1(3,2,k)
   auxmat(2,3)=Sold(2,1)*bv1(1,3,k)+Sold(2,2)*bv1(2,3,k)+Sold(2,3)*bv1(3,3,k)
 
   zvec(1)=FF(2,1)*AA(1,2)+FF(2,2)*AA(2,2)+FF(2,3)*AA(3,2)       ! Bb(2,1)
   zvec(2)=FF(3,1)*AA(1,2)+FF(3,2)*AA(2,2)+FF(3,3)*AA(3,2)       ! Bb(3,1)  
   bs(1,1,k)=(FF(2,1))
   bs(1,2,k)=(FF(3,1))
   bs(2,1,k)=(FF(2,2))
   bs(2,2,k)=(FF(3,2))
   gs(k+1,1)=(FF(2,3))
   gs(k+1,2)=(FF(3,3))
   Snew(1,1)=(gs(k+1,1))*gv1(k+1,1)+(bs(1,1,k))*auxmat(1,1)+(bs(2,1,k))*auxmat(2,1)
   Snew(1,2)=(gs(k+1,1))*gv1(k+1,2)+(bs(1,1,k))*auxmat(1,2)+(bs(2,1,k))*auxmat(2,2)
   Snew(1,3)=(gs(k+1,1))*gv1(k+1,3)+(bs(1,1,k))*auxmat(1,3)+(bs(2,1,k))*auxmat(2,3)
   Snew(2,1)=(gs(k+1,2))*gv1(k+1,1)+(bs(1,2,k))*auxmat(1,1)+(bs(2,2,k))*auxmat(2,1)
   Snew(2,2)=(gs(k+1,2))*gv1(k+1,2)+(bs(1,2,k))*auxmat(1,2)+(bs(2,2,k))*auxmat(2,2)
   Snew(2,3)=(gs(k+1,2))*gv1(k+1,3)+(bs(1,2,k))*auxmat(1,3)+(bs(2,2,k))*auxmat(2,3)
   hs(1,k+1)=Snew(1,1)*hv1(1,k+1)+Snew(1,2)*hv1(2,k+1)+Snew(1,3)*hv1(3,k+1)
   hs(2,k+1)=Snew(2,1)*hv1(1,k+1)+Snew(2,2)*hv1(2,k+1)+Snew(2,3)*hv1(3,k+1)

enddo


gvnew=gs
hvnew=hs
bvnew=bs


! *** (12) ****
! Compression of U

xnum=q1(1)
yvec=gu1(1,:)
znum=du1(1)
hunew(1)=0.d0
hunew(2)=gu1(1,1)*hu1(1,2)+gu1(1,2)*hu1(2,2)
gunew(1)=1.d0
do k=2,kfin+3
  xx(1)=(xnum)
  xx(2)=(q1(k))
  call givensr(xx,WW)
  auxmat2(1,1)=znum
  auxmat2(1,2)=hunew(k)
  auxmat2(2,1)=p1(k)*(xnum)
  auxmat2(2,2)=du1(k)
  Zmat=matmul(auxmat2,WW)
  xnum=xx(1)*WW(1,2)+xx(2)*WW(2,2)
  xx=Zmat(:,1)
  call givens(xx,auxmat22)
  
  bunew(k)=(auxmat22(2,1))
  gunew(k)=(auxmat22(2,2))
  auxvec=yvec
  yvec(1)=(gunew(k))*gu1(k,1)+(bunew(k))*(auxvec(1)*bu1(1,1,k)+auxvec(2)*bu1(2,1,k))
  yvec(2)=(gunew(k))*gu1(k,2)+(bunew(k))*(auxvec(1)*bu1(1,2,k)+auxvec(2)*bu1(2,2,k))
  znum=(gunew(k))*Zmat(2,2)+(bunew(k))*Zmat(1,2)
  hunew(k+1)=yvec(1)*hu1(1,k+1)+yvec(2)*hu1(2,k+1)
enddo



!***************
endif
!***************


!*************************************
! end of structured QZ sweep
!*************************************
end subroutine structured_QZ_ss




! **********************************************************
! Structured double shift QZ sweep.
! **********************************************************
subroutine structured_QZ_ds(sigmaa,gv,hv,bv,z,w,db,gu,hu,bu,p,q,sigmaa1,gvnew,hvnew,bvnew,z1,w1,db1,gunew,&
& hunew,bunew,p1,q1,n,kin,kfin,initialQ)   

implicit none

integer, intent(in) :: n,kin,kfin
integer :: i,j,k

real(kind(1d0)), intent(in) :: sigmaa(n-1),gv(n,2),hv(2,n),bv(2,2,n-1),z(n),w(n)
real(kind(1d0)), intent(in) :: db(n),gu(n-1),hu(n),bu(n-1),p(n),q(n),initialQ(3,3)

real(kind(1d0)), intent(out) :: sigmaa1(n-1),gvnew(n,2),hvnew(2,n),bvnew(2,2,n-1),w1(n),z1(n)
real(kind(1d0)), intent(out) :: gunew(n-1),hunew(n),bunew(n-1),p1(n),q1(n)

real(kind(1d0)) :: db1(n),du1(n),dv1(n),gv1(n,4),bv1(4,4,n-1),hv1(4,N)
real(kind(1d0)) :: gu1(n-1,3),bu1(3,3,n-1),hu1(3,n),sigmav(n-1),du(n)
real(kind(1d0)) :: QQ(3,3),VV(3,4),auxm(3,4),auxv(3),chi(2),gam(2),aux3(3),aux2(2)
real(kind(1d0)) :: aux22(2,2),c(2),theta2(2),fu(2,2),fb(2,2),phib(2),phiu(2)
real(kind(1d0)) :: epsilona(2),epsilonb(2),Phi(3,3),ZZ(3,3),eye(3,3),aux44(4,4)
real(kind(1d0)) :: Utilde(3,4),Vtilde(3,5),fa(2,2),fv(2,2),phiv(2,2),phia(2,2),epsilon1
real(kind(1d0)) :: epsilonb_nm1(2),epsilona_n(2),Q_nm1(2,2),betav_np1(2),Phi_nm1(2,2)
real(kind(1d0)) :: aux23(2,3),Vtilden(2,3),zvec(2),yold,ynew,aux33(3,3),betau(2,3),betav(2,4)
real(kind(1d0)) :: bs(2,2,N-1),gs(N,2),hs(2,N),Snew(2,4),Sold(2,4),aux35(3,5),Omega_nm2(2,3)
real(kind(1d0)) :: AA(3,2),aux24(2,4),FF(3,3),auxmat2(2,2),Omega(3,3),aux55(5,5),Utilden(3,3)
real(kind(1d0)) :: xnum,yvec(3),znum,xx(2),WW(2,2),Zmat(2,2),auxvec(3),auxmat32(3,2)
real(kind(1d0)) :: auxm23(2,3),Z_nm1(2,2),auxmat22(2,2),AAA(n,n)

! eye = 3x3 identity matrix
eye=0.d0
eye(1,1)=1.d0
eye(2,2)=1.d0
eye(3,3)=1.d0

if (kin>1) then
    QQ=eye
end if
if (kin==1) then
    QQ=initialQ
end if

! initialize variables so as not to pass undefined values
sigmaa1=0.d0
gs=0.d0   ! gvnew will take gs values, same for hvnew and hs, bvnew and bs
hs=0.d0
bs=0.d0
w1=0.d0
z1=0.d0
gunew=0.d0
hunew=0.d0
bunew=0.d0
p1=0.d0
q1=0.d0


sigmav(1)=sigmaa(1)+z(2)*w(1)
sigmav(2)=sigmaa(2)+z(3)*w(2)

VV(1,1)=gv(1,1)*hv(1,1)+gv(1,2)*hv(2,1)
aux2=matmul(bv(:,:,1),hv(:,2))
VV(1,2)=gv(1,1)*aux2(1)+gv(1,2)*aux2(2)
aux2=matmul(bv(:,:,1),bv(:,1,2))
VV(1,3)=gv(1,1)*aux2(1)+gv(1,2)*aux2(2)
aux2=matmul(bv(:,:,1),bv(:,2,2))
VV(1,4)=gv(1,1)*aux2(1)+gv(1,2)*aux2(2)
VV(2,1)=sigmav(1)
VV(2,2)=gv(2,1)*hv(1,2)+gv(2,2)*hv(2,2)
VV(2,3)=gv(2,1)*bv(1,1,2)+gv(2,2)*bv(2,1,2)
VV(2,4)=gv(2,1)*bv(1,2,2)+gv(2,2)*bv(2,2,2)
VV(3,1)=z(3)*w(1)
VV(3,2)=sigmav(2)
VV(3,3)=gv(3,1)
VV(3,4)=gv(3,2)

auxm=matmul(transpose(QQ),VV)
gv1(1,1)=auxm(1,1)
gv1(1,2)=auxm(1,2)
gv1(1,3)=auxm(1,3)
gv1(1,4)=auxm(1,4)

fv=auxm(2:3,1:2)
phiv=auxm(2:3,3:4)

auxv=matmul(transpose(QQ),z(1:3))
z1(1)=auxv(1)
chi=auxv(2:3)
gam=w(1:2)
aux22(1,1)=chi(1)*gam(1)
aux22(1,2)=chi(1)*gam(2)
aux22(2,1)=chi(2)*gam(1)
aux22(2,2)=chi(2)*gam(2)
fa=fv-aux22
phia=phiv

c=p(1:2)
theta2=q(1:2)
fu(1,1)=db(1)+p(1)*q(1)
fu(1,2)=gu(1)*hu(2)
fu(2,1)=p(2)*q(1)
fu(2,2)=db(2)+p(2)*q(2)
phiu(1)=gu(1)*bu(2)
phiu(2)=gu(2)
epsilon1=gu(1)*hu(2)-p(1)*q(2)
fb(1,1)=db(1)
fb(1,2)=epsilon1
fb(2,1)=0.d0
fb(2,2)=db(2)
phib=phiu



! In order to speed up the program:
! if kfin is large do the loop until the end,
! otherwise stop early
!*******************************************
if (kfin>n-8) then
!*******************************************




! *********************
! Chasing the Bulge
! *********************

do k=1,n-3  
! (a)
    epsilonb=phib*hu(k+2)-c*q(k+2)
    aux33(1,1)=fb(1,1)
    aux33(1,2)=fb(1,2)
    aux33(1,3)=epsilonb(1)
    aux33(2,1)=fb(2,1)
    aux33(2,2)=fb(2,2)
    aux33(2,3)=epsilonb(2)
    aux33(3,1)=0.d0
    aux33(3,2)=0.d0
    aux33(3,3)=db(k+2)
    Phi=matmul(transpose(QQ),aux33)
    call hh23(Phi(2:3,:),ZZ)
    if (kin>k) then 
        ZZ=eye
    end if
    ! this line inserted to avoid cancellation in db
    db1(k)=Phi(1,1)*ZZ(1,1)+Phi(1,2)*ZZ(2,1)+Phi(1,3)*ZZ(3,1)

    ! (c) (before (b) because of updated Q and Z)
    du(k+2)=db(k+2)+p(k+2)*q(k+2)
    Utilde(1,1)=fu(1,1)
    Utilde(1,2)=fu(1,2)
    Utilde(2,1)=fu(2,1)
    Utilde(2,2)=fu(2,2)
    Utilde(1,3)=phiu(1)*hu(k+2)
    Utilde(2,3)=phiu(2)*hu(k+2)
    Utilde(1,4)=phiu(1)*bu(k+2)
    Utilde(2,4)=phiu(2)*bu(k+2)
    Utilde(3,1)=p(k+2)*theta2(1)
    Utilde(3,2)=p(k+2)*theta2(2)
    Utilde(3,3)=du(k+2)
    Utilde(3,4)=gu(k+2)

    aux44(1,1)=ZZ(1,1)
    aux44(1,2)=ZZ(1,2)
    aux44(1,3)=ZZ(1,3)
    aux44(2,1)=ZZ(2,1)
    aux44(2,2)=ZZ(2,2)
    aux44(2,3)=ZZ(2,3)
    aux44(3,1)=ZZ(3,1)
    aux44(3,2)=ZZ(3,2)
    aux44(3,3)=ZZ(3,3)
    aux44(1,4)=0.d0
    aux44(2,4)=0.d0
    aux44(3,4)=0.d0
    aux44(4,1)=0.d0
    aux44(4,2)=0.d0
    aux44(4,3)=0.d0
    aux44(4,4)=1.d0

    auxm=matmul(transpose(QQ),matmul(Utilde,aux44))
    du1(k)=auxm(1,1)
    gu1(k,1)=auxm(1,2)
    gu1(k,2)=auxm(1,3)
    gu1(k,3)=auxm(1,4) 
    betau=auxm(2:3,2:4)
    fu=betau(:,1:2)
    phiu=betau(:,3)

  
    hu1(1,k)=ZZ(1,1)
    hu1(2,k)=ZZ(2,1)
    hu1(3,k)=hu(k+2)*ZZ(3,1)
    bu1(1,1,k)=ZZ(1,2)
    bu1(1,2,k)=ZZ(1,3)
    bu1(1,3,k)=0.d0
    bu1(2,1,k)=ZZ(2,2)
    bu1(2,2,k)=ZZ(2,3)
    bu1(2,3,k)=0.d0
    bu1(3,1,k)=hu(k+2)*ZZ(3,2)
    bu1(3,2,k)=hu(k+2)*ZZ(3,3)
    bu1(3,3,k)=bu(k+2)

  
    auxv(1)=c(1)
    auxv(2)=c(2)
    auxv(3)=p(k+2)
    p1(k)=QQ(1,1)*auxv(1)+QQ(2,1)*auxv(2)+QQ(3,1)*auxv(3)
    c(1)=QQ(1,2)*auxv(1)+QQ(2,2)*auxv(2)+QQ(3,2)*auxv(3)
    c(2)=QQ(1,3)*auxv(1)+QQ(2,3)*auxv(2)+QQ(3,3)*auxv(3)
    auxv(1)=theta2(1)
    auxv(2)=theta2(2)
    auxv(3)=q(k+2)
    q1(k)=ZZ(1,1)*auxv(1)+ZZ(2,1)*auxv(2)+ZZ(3,1)*auxv(3)
    theta2(1)=ZZ(1,2)*auxv(1)+ZZ(2,2)*auxv(2)+ZZ(3,2)*auxv(3)
    theta2(2)=ZZ(1,3)*auxv(1)+ZZ(2,3)*auxv(2)+ZZ(3,3)*auxv(3)

  
    fb(1,1)=fu(1,1)-c(1)*theta2(1)
    fb(1,2)=fu(1,2)-c(1)*theta2(2)
    fb(2,1)=fu(2,1)-c(2)*theta2(1)
    fb(2,2)=fu(2,2)-c(2)*theta2(2)
    phib=phiu

    ! (b)
    epsilona=matmul(phia,hv(:,k+2))-chi*w(k+2)
    aux33(1,1)=fa(1,1)
    aux33(1,2)=fa(1,2)
    aux33(2,1)=fa(2,1)
    aux33(2,2)=fa(2,2)
    aux33(1,3)=epsilona(1)
    aux33(2,3)=epsilona(2)
    aux33(3,1)=0.d0
    aux33(3,2)=0.d0
    aux33(3,3)=sigmaa(k+2)
    Omega=matmul(aux33,ZZ)

! inserted these special cases so that deflated form is preserved
    if (k==kfin-2) then
        call givens(Omega(1:2,1),aux22) 
        QQ(1,1)=aux22(1,1)
        QQ(1,2)=aux22(1,2)
        QQ(2,1)=aux22(2,1)
        QQ(2,2)=aux22(2,2)
        QQ(1,3)=0.d0
        QQ(2,3)=0.d0
        QQ(3,1)=0.d0 
        QQ(3,2)=0.d0
        QQ(3,3)=1.d0           
    else if (k>=kfin-1) then
        QQ=eye
    else
        call hh3(Omega(:,1),QQ)
    end if
    
    QQ=transpose(QQ)
    
    if (kin>k+1) then
        QQ=eye
    end if
    if (kin==k+1) then
        QQ=initialQ
    end if
    
   
    sigmaa1(k)=QQ(1,1)*Omega(1,1)+QQ(2,1)*Omega(2,1)+QQ(3,1)*Omega(3,1)

    ! (d)
    sigmav(k+2)=sigmaa(k+2)+z(k+3)*w(k+2)
    Vtilde(1,1)=fv(1,1)
    Vtilde(1,2)=fv(1,2)
    Vtilde(2,1)=fv(2,1)
    Vtilde(2,2)=fv(2,2)
    Vtilde(1,3)=phiv(1,1)*hv(1,k+2)+phiv(1,2)*hv(2,k+2)
    Vtilde(2,3)=phiv(2,1)*hv(1,k+2)+phiv(2,2)*hv(2,k+2)
    Vtilde(1,4)=phiv(1,1)*bv(1,1,k+2)+phiv(1,2)*bv(2,1,k+2)
    Vtilde(1,5)=phiv(1,1)*bv(1,2,k+2)+phiv(1,2)*bv(2,2,k+2)
    Vtilde(2,4)=phiv(2,1)*bv(1,1,k+2)+phiv(2,2)*bv(2,1,k+2)
    Vtilde(2,5)=phiv(2,1)*bv(1,2,k+2)+phiv(2,2)*bv(2,2,k+2)
    Vtilde(3,1)=z(k+3)*gam(1)
    Vtilde(3,2)=z(k+3)*gam(2)
    Vtilde(3,3)=sigmav(k+2)
    Vtilde(3,4)=gv(k+3,1)
    Vtilde(3,5)=gv(k+3,2)
   
    aux55(1,1)=ZZ(1,1)
    aux55(1,2)=ZZ(1,2)
    aux55(1,3)=ZZ(1,3)
    aux55(2,1)=ZZ(2,1)
    aux55(2,2)=ZZ(2,2)
    aux55(2,3)=ZZ(2,3)
    aux55(3,1)=ZZ(3,1)
    aux55(3,2)=ZZ(3,2)
    aux55(3,3)=ZZ(3,3)
    aux55(1,4)=0.d0
    aux55(1,5)=0.d0
    aux55(2,4)=0.d0
    aux55(2,5)=0.d0
    aux55(3,4)=0.d0
    aux55(3,5)=0.d0
    aux55(4,1)=0.d0
    aux55(5,1)=0.d0
    aux55(4,2)=0.d0
    aux55(5,2)=0.d0
    aux55(4,3)=0.d0
    aux55(5,3)=0.d0
    aux55(4,5)=0.d0
    aux55(5,4)=0.d0
    aux55(4,4)=1.d0
    aux55(5,5)=1.d0
    aux35=matmul(transpose(QQ),matmul(Vtilde,aux55))
    dv1(k+1)=aux35(1,1)
    gv1(k+1,:)=aux35(1,2:5)
    betav=aux35(2:3,2:5)
    fv=betav(:,1:2)
    phiv=betav(:,3:4)
   
    hv1(1,k)=ZZ(1,1)
    hv1(2,k)=ZZ(2,1)
    hv1(3,k)=hv(1,k+2)*ZZ(3,1)
    hv1(4,k)=hv(2,k+2)*ZZ(3,1)
    bv1(1,1,k)=ZZ(1,2)
    bv1(1,2,k)=ZZ(1,3)
    bv1(1,3,k)=0.d0
    bv1(1,4,k)=0.d0
    bv1(2,1,k)=ZZ(2,2)
    bv1(2,2,k)=ZZ(2,3)
    bv1(2,3,k)=0.d0
    bv1(2,4,k)=0.d0
    bv1(3,1,k)=hv(1,k+2)*ZZ(3,2)
    bv1(4,1,k)=hv(2,k+2)*ZZ(3,2)
    bv1(3,2,k)=hv(1,k+2)*ZZ(3,3)
    bv1(4,2,k)=hv(2,k+2)*ZZ(3,3)
    bv1(3:4,3:4,k)=bv(:,:,k+2)

    auxv(1)=chi(1)
    auxv(2)=chi(2)
    auxv(3)=z(k+3)
    aux3=matmul(transpose(QQ),auxv)
    z1(k+1)=aux3(1)
    chi=aux3(2:3)

    auxv(1)=gam(1)
    auxv(2)=gam(2)
    auxv(3)=w(k+2)
    aux3=matmul(transpose(ZZ),auxv)
    w1(k)=aux3(1)
    gam=aux3(2:3)    
    
    aux22(1,1)=chi(1)*gam(1)
    aux22(1,2)=chi(1)*gam(2)
    aux22(2,1)=chi(2)*gam(1)
    aux22(2,2)=chi(2)*gam(2)
    fa=fv-aux22
    phia=phiv

enddo

! Last steps

! 1.3.1
epsilonb_nm1=phib*hu(n)-c*q(n)
aux33(1,1)=fb(1,1)
aux33(1,2)=fb(1,2)
aux33(2,1)=fb(2,1)
aux33(2,2)=fb(2,2)
aux33(1,3)=epsilonb_nm1(1)
aux33(2,3)=epsilonb_nm1(2)
aux33(3,1)=0.d0
aux33(3,2)=0.d0
aux33(3,3)=db(n)
Phi=matmul(transpose(QQ),aux33)
call hh23(Phi(2:3,:),ZZ)

if (kin>n-2) then
    ZZ=eye
end if
! this line added later to prevent cancellation in diagonal of B
db1(N-2)=Phi(1,1)*ZZ(1,1)+Phi(1,2)*ZZ(2,1)+Phi(1,3)*ZZ(3,1)

! 1.3.2
epsilona_n=matmul(phia,hv(:,n))-chi*w(n)
aux23(1,1)=fa(1,1)
aux23(1,2)=fa(1,2)
aux23(2,1)=fa(2,1)
aux23(2,2)=fa(2,2)
aux23(1,3)=epsilona_n(1)
aux23(2,3)=epsilona_n(2)  ! will also be used later
Omega_nm2=matmul(aux23,ZZ)
aux2(1)=Omega_nm2(1,1)
aux2(2)=Omega_nm2(2,1)

call givens(aux2,Q_nm1)  ! matrix stays in transpose form
sigmaa1(n-2)=Q_nm1(1,1)*aux2(1)+Q_nm1(1,2)*aux2(2)

Vtilden(1,1)=fv(1,1)
Vtilden(2,1)=fv(2,1)
Vtilden(1,2)=fv(1,2)
Vtilden(2,2)=fv(2,2)
Vtilden(1,3)=phiv(1,1)*hv(1,n)+phiv(1,2)*hv(2,n)
Vtilden(2,3)=phiv(2,1)*hv(1,n)+phiv(2,2)*hv(2,n)
aux23=matmul(Q_nm1,matmul(Vtilden,ZZ))
dv1(n-1)=aux23(1,1)
gv1(n-1,1)=aux23(1,2)
gv1(n-1,2)=aux23(1,3)
gv1(n-1,3)=0.d0
gv1(n-1,4)=0.d0

betav_np1(1)=aux23(2,2)
betav_np1(2)=aux23(2,3)

! 1.3.3
du(n)=db(n)+p(n)*q(n)
Utilden(1,1)=fu(1,1)
Utilden(1,2)=fu(1,2)
Utilden(2,1)=fu(2,1)
Utilden(2,2)=fu(2,2)
Utilden(1,3)=phiu(1)*hu(n)
Utilden(2,3)=phiu(2)*hu(n)
Utilden(3,1)=p(n)*theta2(1)
Utilden(3,2)=p(n)*theta2(2)
Utilden(3,3)=du(n)

aux33=matmul(transpose(QQ),matmul(Utilden,ZZ))
du1(n-2)=aux33(1,1)
gu1(n-2,1)=aux33(1,2)
gu1(n-2,2)=aux33(1,3)
gu1(n-2,3)=0.d0
fu=aux33(2:3,2:3)

hu1(1,n-2)=ZZ(1,1)
hu1(2,n-2)=ZZ(2,1)
hu1(3,n-2)=hu(n)*ZZ(3,1)
bu1(1,1,n-2)=ZZ(1,2)
bu1(2,1,n-2)=ZZ(2,2)
bu1(3,1,n-2)=hu(n)*ZZ(3,2)
bu1(1,2,n-2)=ZZ(1,3)
bu1(2,2,n-2)=ZZ(2,3)
bu1(3,2,n-2)=hu(n)*ZZ(3,3)
bu1(1,3,n-2)=0.d0
bu1(2,3,n-2)=0.d0
bu1(3,3,n-2)=0.d0

aux2=matmul(Q_nm1,chi)
auxv(1)=gam(1)
auxv(2)=gam(2)
auxv(3)=w(n)
auxv=matmul(transpose(ZZ),auxv)

z1(n-1)=aux2(1)
z1(n)=aux2(2)
w1(n-2)=auxv(1)
gam=auxv(2:3)

auxv(1)=c(1)
auxv(2)=c(2)
auxv(3)=p(n)
auxv=matmul(transpose(QQ),auxv)
p1(n-2)=auxv(1)
c=auxv(2:3)

auxv(1)=theta2(1)
auxv(2)=theta2(2)
auxv(3)=q(n)
auxv=matmul(transpose(ZZ),auxv)
q1(n-2)=auxv(1)
theta2=auxv(2:3)


fb(1,1)=fu(1,1)-c(1)*theta2(1)
fb(1,2)=fu(1,2)-c(1)*theta2(2)
fb(2,1)=fu(2,1)-c(2)*theta2(1)
fb(2,2)=fu(2,2)-c(2)*theta2(2)


hv1(1,n-2)=ZZ(1,1)
hv1(2,n-2)=ZZ(2,1)
hv1(3,n-2)=hv(1,n)*ZZ(3,1)
hv1(4,n-2)=hv(2,n)*ZZ(3,1)
bv1(1,1,n-2)=ZZ(1,2)
bv1(2,1,n-2)=ZZ(2,2)
bv1(3,1,n-2)=hv(1,n)*ZZ(3,2)
bv1(4,1,n-2)=hv(2,n)*ZZ(3,2)
bv1(1,2,n-2)=ZZ(1,3)
bv1(2,2,n-2)=ZZ(2,3)
bv1(3,2,n-2)=hv(1,n)*ZZ(3,3)
bv1(4,2,n-2)=hv(2,n)*ZZ(3,3)
bv1(:,3:4,n-2)=0.d0

! 1.4
Phi_nm1=matmul(Q_Nm1,fb)

aux2(1)=Phi_nm1(2,1)
aux2(2)=Phi_nm1(2,2)
call givens(aux2,Z_nm1)  ! Z_nm1 not yet flipped
Z_nm1=transpose(Z_nm1)
! added later to prevent cancellation in diagonal of B

db1(n-1)=Phi_nm1(1,1)*Z_nm1(1,2)+Phi_nm1(1,2)*Z_nm1(2,2)   !Psi_Nm1(1,1);
db1(n)=Phi_nm1(2,1)*Z_nm1(1,1)+Phi_nm1(2,2)*Z_nm1(2,1)    !Psi_Nm1(2,2);

hu1(1,n-1)=Z_nm1(1,2)
hu1(2,n-1)=Z_nm1(2,2)
hu1(3,n-1)=0.d0
bu1(:,:,n-1)=0.d0
bu1(1,1,n-1)=Z_nm1(1,1)
bu1(2,1,n-1)=Z_nm1(2,1)
hu1(1,n)=1.d0
hu1(2,n)=0.d0
hu1(3,n)=0.d0


hv1(1,n-1)=Z_nm1(1,2)
hv1(2,n-1)=Z_nm1(2,2)
hv1(3,n-1)=0.d0
hv1(4,n-1)=0.d0
bv1(:,:,n-1)=0.d0
bv1(1,1,n-1)=Z_nm1(1,1)
bv1(2,1,n-1)=Z_nm1(2,1)
hv1(1,n)=1.d0
hv1(2,n)=0.d0
hv1(3,n)=0.d0
hv1(4,n)=0.d0

aux23(1,1)=Omega_nm2(1,1)
aux23(1,2)=Omega_nm2(1,2)*Z_nm1(1,2)+Omega_nm2(1,3)*Z_nm1(2,2)
aux23(1,3)=Omega_nm2(1,2)*Z_nm1(1,1)+Omega_nm2(1,3)*Z_nm1(2,1)
aux23(2,1)=Omega_nm2(2,1)
aux23(2,2)=Omega_nm2(2,2)*Z_nm1(1,2)+Omega_nm2(2,3)*Z_nm1(2,2)
aux23(2,3)=Omega_nm2(2,2)*Z_nm1(1,1)+Omega_nm2(2,3)*Z_nm1(2,1)
sigmaa1(n-1)=Q_nm1(2,1)*aux23(1,2)+Q_nm1(2,2)*aux23(2,2)

dv1(n)=betav_np1(1)*Z_Nm1(1,2)+betav_np1(2)*Z_Nm1(2,2)

gv1(n,1)=betav_np1(1)*Z_Nm1(1,1)+betav_np1(2)*Z_Nm1(2,1)
gv1(n,2)=0.d0
gv1(n,3)=0.d0
gv1(n,4)=0.d0

aux22=matmul(Q_nm1,fu)
du1(n-1)=aux22(1,1)*Z_nm1(1,2)+aux22(1,2)*Z_nm1(2,2)
du1(n)=aux22(2,1)*Z_nm1(1,1)+aux22(2,2)*Z_nm1(2,1)
gu1(n-1,1)=aux22(1,1)*Z_nm1(1,1)+aux22(1,2)*Z_nm1(2,1)
gu1(n-1,2)=0.d0
gu1(n-1,3)=0.d0


w1(n-1)=Z_nm1(1,2)*gam(1)+Z_nm1(2,2)*gam(2)
w1(n)=Z_nm1(1,1)*gam(1)+Z_nm1(2,1)*gam(2)


aux2=matmul(Q_nm1,c)
p1(N-1)=aux2(1)
p1(N)=aux2(2)

q1(n-1)=Z_nm1(1,2)*theta2(1)+Z_nm1(2,2)*theta2(2)
q1(n)=Z_nm1(1,1)*theta2(1)+Z_nm1(2,1)*theta2(2)

! ****************************
! Compression
! ****************************
!  compression of V

gs(1,1)=1.d0
gs(1,2)=0.d0
! Sold=first line of gv1
hs(1,1)=gv1(1,1)*hv1(1,1)+gv1(1,2)*hv1(2,1)+gv1(1,3)*hv1(3,1)+gv1(1,4)*hv1(4,1)
hs(2,1)=0.d0

gs(2,1)=0.d0
gs(2,2)=1.d0
bs(1,1,1)=1.d0
bs(1,2,1)=0.d0
bs(2,1,1)=0.d0
bs(2,2,1)=0.d0
Snew(1,1)=gv1(1,1)*bv1(1,1,1)+gv1(1,2)*bv1(2,1,1)+gv1(1,3)*bv1(3,1,1)+gv1(1,4)*bv1(4,1,1)
Snew(1,2)=gv1(1,1)*bv1(1,2,1)+gv1(1,2)*bv1(2,2,1)+gv1(1,3)*bv1(3,2,1)+gv1(1,4)*bv1(4,2,1)
Snew(1,3)=gv1(1,1)*bv1(1,3,1)+gv1(1,2)*bv1(2,3,1)+gv1(1,3)*bv1(3,3,1)+gv1(1,4)*bv1(4,3,1)
Snew(1,4)=gv1(1,1)*bv1(1,4,1)+gv1(1,2)*bv1(2,4,1)+gv1(1,3)*bv1(3,4,1)+gv1(1,4)*bv1(4,4,1)
Snew(2,1)=gv1(2,1)
Snew(2,2)=gv1(2,2)
Snew(2,3)=gv1(2,3)
Snew(2,4)=gv1(2,4)
hs(1,2)=Snew(1,1)*hv1(1,2)+Snew(1,2)*hv1(2,2)+Snew(1,3)*hv1(3,2)+Snew(1,4)*hv1(4,2)
hs(2,2)=Snew(2,1)*hv1(1,2)+Snew(2,2)*hv1(2,2)+Snew(2,3)*hv1(3,2)+Snew(2,4)*hv1(4,2)
zvec(1)=gs(1,1)*hs(1,1)+gs(1,2)*hs(2,1)
zvec(2)=dv1(2)
yold=w1(1)


do k=2,n-1
   aux2(1)=yold
   aux2(2)=w1(k)
   call givensr(aux2,WW)
   ynew=aux2(1)*WW(1,2)+aux2(2)*WW(2,2)
   auxmat32(1,1)=zvec(1)
   auxmat32(2,1)=zvec(2)
   auxmat32(1,2)=Snew(1,1)*hv1(1,k)+Snew(1,2)*hv1(2,k)+Snew(1,3)*hv1(3,k)+Snew(1,4)*hv1(4,k)
   auxmat32(2,2)=Snew(2,1)*hv1(1,k)+Snew(2,2)*hv1(2,k)+Snew(2,3)*hv1(3,k)+Snew(2,4)*hv1(4,k)
   auxmat32(3,1)=z1(k+1)*yold
   auxmat32(3,2)=dv1(k+1)
   AA=matmul(auxmat32,WW)
   yold=ynew
   auxv=AA(:,1)
   call hh3(auxv,FF)
   Sold=Snew
   
   aux24(1,1)=Sold(1,1)*bv1(1,1,k)+Sold(1,2)*bv1(2,1,k)+Sold(1,3)*bv1(3,1,k)+Sold(1,4)*bv1(4,1,k)
   aux24(1,2)=Sold(1,1)*bv1(1,2,k)+Sold(1,2)*bv1(2,2,k)+Sold(1,3)*bv1(3,2,k)+Sold(1,4)*bv1(4,2,k)
   aux24(1,3)=Sold(1,1)*bv1(1,3,k)+Sold(1,2)*bv1(2,3,k)+Sold(1,3)*bv1(3,3,k)+Sold(1,4)*bv1(4,3,k)
   aux24(1,4)=Sold(1,1)*bv1(1,4,k)+Sold(1,2)*bv1(2,4,k)+Sold(1,3)*bv1(3,4,k)+Sold(1,4)*bv1(4,4,k)
   aux24(2,1)=Sold(2,1)*bv1(1,1,k)+Sold(2,2)*bv1(2,1,k)+Sold(2,3)*bv1(3,1,k)+Sold(2,4)*bv1(4,1,k)
   aux24(2,2)=Sold(2,1)*bv1(1,2,k)+Sold(2,2)*bv1(2,2,k)+Sold(2,3)*bv1(3,2,k)+Sold(2,4)*bv1(4,2,k)
   aux24(2,3)=Sold(2,1)*bv1(1,3,k)+Sold(2,2)*bv1(2,3,k)+Sold(2,3)*bv1(3,3,k)+Sold(2,4)*bv1(4,3,k)
   aux24(2,4)=Sold(2,1)*bv1(1,4,k)+Sold(2,2)*bv1(2,4,k)+Sold(2,3)*bv1(3,4,k)+Sold(2,4)*bv1(4,4,k)
 
   zvec(1)=FF(2,1)*AA(1,2)+FF(2,2)*AA(2,2)+FF(2,3)*AA(3,2)       ! Bb(2,1)
   zvec(2)=FF(3,1)*AA(1,2)+FF(3,2)*AA(2,2)+FF(3,3)*AA(3,2)       ! Bb(3,1)  
   bs(1,1,k)=(FF(2,1))
   bs(1,2,k)=(FF(3,1))
   bs(2,1,k)=(FF(2,2))
   bs(2,2,k)=(FF(3,2))
   gs(k+1,1)=(FF(2,3))
   gs(k+1,2)=(FF(3,3))
   Snew(1,1)=(gs(k+1,1))*gv1(k+1,1)+(bs(1,1,k))*aux24(1,1)+(bs(2,1,k))*aux24(2,1)
   Snew(1,2)=(gs(k+1,1))*gv1(k+1,2)+(bs(1,1,k))*aux24(1,2)+(bs(2,1,k))*aux24(2,2)
   Snew(1,3)=(gs(k+1,1))*gv1(k+1,3)+(bs(1,1,k))*aux24(1,3)+(bs(2,1,k))*aux24(2,3)
   Snew(1,4)=(gs(k+1,1))*gv1(k+1,4)+(bs(1,1,k))*aux24(1,4)+(bs(2,1,k))*aux24(2,4)
   Snew(2,1)=(gs(k+1,2))*gv1(k+1,1)+(bs(1,2,k))*aux24(1,1)+(bs(2,2,k))*aux24(2,1)
   Snew(2,2)=(gs(k+1,2))*gv1(k+1,2)+(bs(1,2,k))*aux24(1,2)+(bs(2,2,k))*aux24(2,2)
   Snew(2,3)=(gs(k+1,2))*gv1(k+1,3)+(bs(1,2,k))*aux24(1,3)+(bs(2,2,k))*aux24(2,3)
   Snew(2,4)=(gs(k+1,2))*gv1(k+1,4)+(bs(1,2,k))*aux24(1,4)+(bs(2,2,k))*aux24(2,4)
   hs(1,k+1)=Snew(1,1)*hv1(1,k+1)+Snew(1,2)*hv1(2,k+1)+Snew(1,3)*hv1(3,k+1)+Snew(1,4)*hv1(4,k+1)
   hs(2,k+1)=Snew(2,1)*hv1(1,k+1)+Snew(2,2)*hv1(2,k+1)+Snew(2,3)*hv1(3,k+1)+Snew(2,4)*hv1(4,k+1)

enddo


gvnew=gs
hvnew=hs
bvnew=bs

! Compression of U

bunew(1)=0.d0
xnum=q1(1)
yvec=gu1(1,:)
znum=du1(1)
hunew(1)=0.d0
hunew(2)=gu1(1,1)*hu1(1,2)+gu1(1,2)*hu1(2,2)+gu1(1,3)*hu1(3,2)
gunew(1)=1.d0
do k=2,n-1
  xx(1)=xnum
  xx(2)=q1(k)
  call givensr(xx,WW)
  auxmat2(1,1)=znum
  auxmat2(1,2)=hunew(k)
  auxmat2(2,1)=p1(k)*(xnum)
  auxmat2(2,2)=du1(k)
  Zmat=matmul(auxmat2,WW)
  xnum=xx(1)*WW(1,2)+xx(2)*WW(2,2)
  xx=Zmat(:,1)
  call givens(xx,auxmat22)
 
  bunew(k)=(auxmat22(2,1))
  gunew(k)=(auxmat22(2,2))
  auxvec=yvec
  yvec(1)=(gunew(k))*gu1(k,1)+(bunew(k))*(auxvec(1)*bu1(1,1,k)+auxvec(2)*bu1(2,1,k)+auxvec(3)*bu1(3,1,k))
  yvec(2)=(gunew(k))*gu1(k,2)+(bunew(k))*(auxvec(1)*bu1(1,2,k)+auxvec(2)*bu1(2,2,k)+auxvec(3)*bu1(3,2,k))
  yvec(3)=(gunew(k))*gu1(k,3)+(bunew(k))*(auxvec(1)*bu1(1,3,k)+auxvec(2)*bu1(2,3,k)+auxvec(3)*bu1(3,3,k))
  znum=(gunew(k))*Zmat(2,2)+(bunew(k))*Zmat(1,2)
  hunew(k+1)=yvec(1)*hu1(1,k+1)+yvec(2)*hu1(2,k+1)+yvec(3)*hu1(3,k+1)
enddo


!**********************************
! if kfin is small, stop loop early
! (update of generators will be incomplete)
else
!**********************************



! *********************
! Chasing the Bulge
! *********************

do k=1,kfin+4 
! (a)
    epsilonb=phib*hu(k+2)-c*q(k+2)
    aux33(1,1)=fb(1,1)
    aux33(1,2)=fb(1,2)
    aux33(1,3)=epsilonb(1)
    aux33(2,1)=fb(2,1)
    aux33(2,2)=fb(2,2)
    aux33(2,3)=epsilonb(2)
    aux33(3,1)=0.d0
    aux33(3,2)=0.d0
    aux33(3,3)=db(k+2)
    Phi=matmul(transpose(QQ),aux33)
    call hh23(Phi(2:3,:),ZZ)
    if (kin>k) then 
        ZZ=eye
    end if
    ! this line inserted to avoid cancellation in db
    db1(k)=Phi(1,1)*ZZ(1,1)+Phi(1,2)*ZZ(2,1)+Phi(1,3)*ZZ(3,1)

    ! (c) (before (b) because of updated Q and Z)
    du(k+2)=db(k+2)+p(k+2)*q(k+2)
    
    Utilde(1,1)=fu(1,1)
    Utilde(1,2)=fu(1,2)
    Utilde(2,1)=fu(2,1)
    Utilde(2,2)=fu(2,2)
    Utilde(1,3)=phiu(1)*hu(k+2)
    Utilde(2,3)=phiu(2)*hu(k+2)
    Utilde(1,4)=phiu(1)*bu(k+2)
    Utilde(2,4)=phiu(2)*bu(k+2)
    Utilde(3,1)=p(k+2)*theta2(1)
    Utilde(3,2)=p(k+2)*theta2(2)
    Utilde(3,3)=du(k+2)
    Utilde(3,4)=gu(k+2)

    aux44(1,1)=ZZ(1,1)
    aux44(1,2)=ZZ(1,2)
    aux44(1,3)=ZZ(1,3)
    aux44(2,1)=ZZ(2,1)
    aux44(2,2)=ZZ(2,2)
    aux44(2,3)=ZZ(2,3)
    aux44(3,1)=ZZ(3,1)
    aux44(3,2)=ZZ(3,2)
    aux44(3,3)=ZZ(3,3)
    aux44(1,4)=0.d0
    aux44(2,4)=0.d0
    aux44(3,4)=0.d0
    aux44(4,1)=0.d0
    aux44(4,2)=0.d0
    aux44(4,3)=0.d0
    aux44(4,4)=1.d0

    auxm=matmul(transpose(QQ),matmul(Utilde,aux44))
    du1(k)=auxm(1,1)
  
    gu1(k,1)=auxm(1,2)
    gu1(k,2)=auxm(1,3)
    gu1(k,3)=auxm(1,4) 
    betau=auxm(2:3,2:4)
    fu=betau(:,1:2)
    phiu=betau(:,3)

 
    hu1(1,k)=ZZ(1,1)
    hu1(2,k)=ZZ(2,1)
    hu1(3,k)=hu(k+2)*ZZ(3,1)
    bu1(1,1,k)=ZZ(1,2)
    bu1(1,2,k)=ZZ(1,3)
    bu1(1,3,k)=0.d0
    bu1(2,1,k)=ZZ(2,2)
    bu1(2,2,k)=ZZ(2,3)
    bu1(2,3,k)=0.d0
    bu1(3,1,k)=hu(k+2)*ZZ(3,2)
    bu1(3,2,k)=hu(k+2)*ZZ(3,3)
    bu1(3,3,k)=bu(k+2)

    auxv(1)=c(1)
    auxv(2)=c(2)
    auxv(3)=p(k+2)
    p1(k)=QQ(1,1)*auxv(1)+QQ(2,1)*auxv(2)+QQ(3,1)*auxv(3)
    c(1)=QQ(1,2)*auxv(1)+QQ(2,2)*auxv(2)+QQ(3,2)*auxv(3)
    c(2)=QQ(1,3)*auxv(1)+QQ(2,3)*auxv(2)+QQ(3,3)*auxv(3)
    auxv(1)=theta2(1)
    auxv(2)=theta2(2)
    auxv(3)=q(k+2)
    q1(k)=ZZ(1,1)*auxv(1)+ZZ(2,1)*auxv(2)+ZZ(3,1)*auxv(3)
    theta2(1)=ZZ(1,2)*auxv(1)+ZZ(2,2)*auxv(2)+ZZ(3,2)*auxv(3)
    theta2(2)=ZZ(1,3)*auxv(1)+ZZ(2,3)*auxv(2)+ZZ(3,3)*auxv(3)

    fb(1,1)=fu(1,1)-c(1)*theta2(1)
    fb(1,2)=fu(1,2)-c(1)*theta2(2)
    fb(2,1)=fu(2,1)-c(2)*theta2(1)
    fb(2,2)=fu(2,2)-c(2)*theta2(2)
    phib=phiu

    ! (b)
    epsilona=matmul(phia,hv(:,k+2))-chi*w(k+2)
    aux33(1,1)=fa(1,1)
    aux33(1,2)=fa(1,2)
    aux33(2,1)=fa(2,1)
    aux33(2,2)=fa(2,2)
    aux33(1,3)=epsilona(1)
    aux33(2,3)=epsilona(2)
    aux33(3,1)=0.d0
    aux33(3,2)=0.d0
    aux33(3,3)=sigmaa(k+2)
    Omega=matmul(aux33,ZZ)

! inserted these special cases so that deflated form is preserved
    if (k==kfin-2) then
        call givens(Omega(1:2,1),aux22) 
        QQ(1,1)=aux22(1,1)
        QQ(1,2)=aux22(1,2)
        QQ(2,1)=aux22(2,1)
        QQ(2,2)=aux22(2,2)
        QQ(1,3)=0.d0
        QQ(2,3)=0.d0
        QQ(3,1)=0.d0 
        QQ(3,2)=0.d0
        QQ(3,3)=1.d0           
    else if (k>=kfin-1) then
        QQ=eye
    else
        call hh3(Omega(:,1),QQ)
    end if
    
    QQ=transpose(QQ)
    
    if (kin>k+1) then
        QQ=eye
    end if
    if (kin==k+1) then
        QQ=initialQ
    end if
    
  
    sigmaa1(k)=QQ(1,1)*Omega(1,1)+QQ(2,1)*Omega(2,1)+QQ(3,1)*Omega(3,1)

    ! (d)
    sigmav(k+2)=sigmaa(k+2)+z(k+3)*w(k+2)
    Vtilde(1,1)=fv(1,1)
    Vtilde(1,2)=fv(1,2)
    Vtilde(2,1)=fv(2,1)
    Vtilde(2,2)=fv(2,2)
    Vtilde(1,3)=phiv(1,1)*hv(1,k+2)+phiv(1,2)*hv(2,k+2)
    Vtilde(2,3)=phiv(2,1)*hv(1,k+2)+phiv(2,2)*hv(2,k+2)
    Vtilde(1,4)=phiv(1,1)*bv(1,1,k+2)+phiv(1,2)*bv(2,1,k+2)
    Vtilde(1,5)=phiv(1,1)*bv(1,2,k+2)+phiv(1,2)*bv(2,2,k+2)
    Vtilde(2,4)=phiv(2,1)*bv(1,1,k+2)+phiv(2,2)*bv(2,1,k+2)
    Vtilde(2,5)=phiv(2,1)*bv(1,2,k+2)+phiv(2,2)*bv(2,2,k+2)
    Vtilde(3,1)=z(k+3)*gam(1)
    Vtilde(3,2)=z(k+3)*gam(2)
    Vtilde(3,3)=sigmav(k+2)
    Vtilde(3,4)=gv(k+3,1)
    Vtilde(3,5)=gv(k+3,2)
   
    aux55(1,1)=ZZ(1,1)
    aux55(1,2)=ZZ(1,2)
    aux55(1,3)=ZZ(1,3)
    aux55(2,1)=ZZ(2,1)
    aux55(2,2)=ZZ(2,2)
    aux55(2,3)=ZZ(2,3)
    aux55(3,1)=ZZ(3,1)
    aux55(3,2)=ZZ(3,2)
    aux55(3,3)=ZZ(3,3)
    aux55(1,4)=0.d0
    aux55(1,5)=0.d0
    aux55(2,4)=0.d0
    aux55(2,5)=0.d0
    aux55(3,4)=0.d0
    aux55(3,5)=0.d0
    aux55(4,1)=0.d0
    aux55(5,1)=0.d0
    aux55(4,2)=0.d0
    aux55(5,2)=0.d0
    aux55(4,3)=0.d0
    aux55(5,3)=0.d0
    aux55(4,5)=0.d0
    aux55(5,4)=0.d0
    aux55(4,4)=1.d0
    aux55(5,5)=1.d0
    aux35=matmul(transpose(QQ),matmul(Vtilde,aux55))
    dv1(k+1)=aux35(1,1)
    gv1(k+1,:)=aux35(1,2:5)
    betav=aux35(2:3,2:5)
    fv=betav(:,1:2)
    phiv=betav(:,3:4)
   
    hv1(1,k)=ZZ(1,1)
    hv1(2,k)=ZZ(2,1)
    hv1(3,k)=hv(1,k+2)*ZZ(3,1)
    hv1(4,k)=hv(2,k+2)*ZZ(3,1)
    bv1(1,1,k)=ZZ(1,2)
    bv1(1,2,k)=ZZ(1,3)
    bv1(1,3,k)=0.d0
    bv1(1,4,k)=0.d0
    bv1(2,1,k)=ZZ(2,2)
    bv1(2,2,k)=ZZ(2,3)
    bv1(2,3,k)=0.d0
    bv1(2,4,k)=0.d0
    bv1(3,1,k)=hv(1,k+2)*ZZ(3,2)
    bv1(4,1,k)=hv(2,k+2)*ZZ(3,2)
    bv1(3,2,k)=hv(1,k+2)*ZZ(3,3)
    bv1(4,2,k)=hv(2,k+2)*ZZ(3,3)
    bv1(3:4,3:4,k)=bv(:,:,k+2)

    auxv(1)=chi(1)
    auxv(2)=chi(2)
    auxv(3)=z(k+3)
    aux3=matmul(transpose(QQ),auxv)
    z1(k+1)=aux3(1)
    chi=aux3(2:3)

    auxv(1)=gam(1)
    auxv(2)=gam(2)
    auxv(3)=w(k+2)
    aux3=matmul(transpose(ZZ),auxv)
    w1(k)=aux3(1)
    gam=aux3(2:3)    
    
    aux22(1,1)=chi(1)*gam(1)
    aux22(1,2)=chi(1)*gam(2)
    aux22(2,1)=chi(2)*gam(1)
    aux22(2,2)=chi(2)*gam(2)
    fa=fv-aux22
    phia=phiv

enddo

! Last steps not needed



! ****************************
! Compression
! ****************************
!  compression of V

gs(1,1)=1.d0
gs(1,2)=0.d0
! Sold=first line of gv1
hs(1,1)=gv1(1,1)*hv1(1,1)+gv1(1,2)*hv1(2,1)+gv1(1,3)*hv1(3,1)+gv1(1,4)*hv1(4,1)
hs(2,1)=0.d0

gs(2,1)=0.d0
gs(2,2)=1.d0
bs(1,1,1)=1.d0
bs(1,2,1)=0.d0
bs(2,1,1)=0.d0
bs(2,2,1)=0.d0
Snew(1,1)=gv1(1,1)*bv1(1,1,1)+gv1(1,2)*bv1(2,1,1)+gv1(1,3)*bv1(3,1,1)+gv1(1,4)*bv1(4,1,1)
Snew(1,2)=gv1(1,1)*bv1(1,2,1)+gv1(1,2)*bv1(2,2,1)+gv1(1,3)*bv1(3,2,1)+gv1(1,4)*bv1(4,2,1)
Snew(1,3)=gv1(1,1)*bv1(1,3,1)+gv1(1,2)*bv1(2,3,1)+gv1(1,3)*bv1(3,3,1)+gv1(1,4)*bv1(4,3,1)
Snew(1,4)=gv1(1,1)*bv1(1,4,1)+gv1(1,2)*bv1(2,4,1)+gv1(1,3)*bv1(3,4,1)+gv1(1,4)*bv1(4,4,1)
Snew(2,1)=gv1(2,1)
Snew(2,2)=gv1(2,2)
Snew(2,3)=gv1(2,3)
Snew(2,4)=gv1(2,4)
hs(1,2)=Snew(1,1)*hv1(1,2)+Snew(1,2)*hv1(2,2)+Snew(1,3)*hv1(3,2)+Snew(1,4)*hv1(4,2)
hs(2,2)=Snew(2,1)*hv1(1,2)+Snew(2,2)*hv1(2,2)+Snew(2,3)*hv1(3,2)+Snew(2,4)*hv1(4,2)
zvec(1)=gs(1,1)*hs(1,1)+gs(1,2)*hs(2,1)
zvec(2)=dv1(2)
yold=w1(1)

do k=2,kfin+3
   aux2(1)=yold
   aux2(2)=w1(k)
   call givensr(aux2,WW)
   ynew=aux2(1)*WW(1,2)+aux2(2)*WW(2,2)
   auxmat32(1,1)=zvec(1)
   auxmat32(2,1)=zvec(2)
   auxmat32(1,2)=Snew(1,1)*hv1(1,k)+Snew(1,2)*hv1(2,k)+Snew(1,3)*hv1(3,k)+Snew(1,4)*hv1(4,k)
   auxmat32(2,2)=Snew(2,1)*hv1(1,k)+Snew(2,2)*hv1(2,k)+Snew(2,3)*hv1(3,k)+Snew(2,4)*hv1(4,k)
   auxmat32(3,1)=z1(k+1)*yold
   auxmat32(3,2)=dv1(k+1)
   AA=matmul(auxmat32,WW)
   yold=ynew
   auxv=AA(:,1)
   call hh3(auxv,FF)
   Sold=Snew
   
   aux24(1,1)=Sold(1,1)*bv1(1,1,k)+Sold(1,2)*bv1(2,1,k)+Sold(1,3)*bv1(3,1,k)+Sold(1,4)*bv1(4,1,k)
   aux24(1,2)=Sold(1,1)*bv1(1,2,k)+Sold(1,2)*bv1(2,2,k)+Sold(1,3)*bv1(3,2,k)+Sold(1,4)*bv1(4,2,k)
   aux24(1,3)=Sold(1,1)*bv1(1,3,k)+Sold(1,2)*bv1(2,3,k)+Sold(1,3)*bv1(3,3,k)+Sold(1,4)*bv1(4,3,k)
   aux24(1,4)=Sold(1,1)*bv1(1,4,k)+Sold(1,2)*bv1(2,4,k)+Sold(1,3)*bv1(3,4,k)+Sold(1,4)*bv1(4,4,k)
   aux24(2,1)=Sold(2,1)*bv1(1,1,k)+Sold(2,2)*bv1(2,1,k)+Sold(2,3)*bv1(3,1,k)+Sold(2,4)*bv1(4,1,k)
   aux24(2,2)=Sold(2,1)*bv1(1,2,k)+Sold(2,2)*bv1(2,2,k)+Sold(2,3)*bv1(3,2,k)+Sold(2,4)*bv1(4,2,k)
   aux24(2,3)=Sold(2,1)*bv1(1,3,k)+Sold(2,2)*bv1(2,3,k)+Sold(2,3)*bv1(3,3,k)+Sold(2,4)*bv1(4,3,k)
   aux24(2,4)=Sold(2,1)*bv1(1,4,k)+Sold(2,2)*bv1(2,4,k)+Sold(2,3)*bv1(3,4,k)+Sold(2,4)*bv1(4,4,k)
 
   zvec(1)=FF(2,1)*AA(1,2)+FF(2,2)*AA(2,2)+FF(2,3)*AA(3,2)       ! Bb(2,1)
   zvec(2)=FF(3,1)*AA(1,2)+FF(3,2)*AA(2,2)+FF(3,3)*AA(3,2)       ! Bb(3,1)  
   bs(1,1,k)=(FF(2,1))
   bs(1,2,k)=(FF(3,1))
   bs(2,1,k)=(FF(2,2))
   bs(2,2,k)=(FF(3,2))
   gs(k+1,1)=(FF(2,3))
   gs(k+1,2)=(FF(3,3))
   Snew(1,1)=(gs(k+1,1))*gv1(k+1,1)+(bs(1,1,k))*aux24(1,1)+(bs(2,1,k))*aux24(2,1)
   Snew(1,2)=(gs(k+1,1))*gv1(k+1,2)+(bs(1,1,k))*aux24(1,2)+(bs(2,1,k))*aux24(2,2)
   Snew(1,3)=(gs(k+1,1))*gv1(k+1,3)+(bs(1,1,k))*aux24(1,3)+(bs(2,1,k))*aux24(2,3)
   Snew(1,4)=(gs(k+1,1))*gv1(k+1,4)+(bs(1,1,k))*aux24(1,4)+(bs(2,1,k))*aux24(2,4)
   Snew(2,1)=(gs(k+1,2))*gv1(k+1,1)+(bs(1,2,k))*aux24(1,1)+(bs(2,2,k))*aux24(2,1)
   Snew(2,2)=(gs(k+1,2))*gv1(k+1,2)+(bs(1,2,k))*aux24(1,2)+(bs(2,2,k))*aux24(2,2)
   Snew(2,3)=(gs(k+1,2))*gv1(k+1,3)+(bs(1,2,k))*aux24(1,3)+(bs(2,2,k))*aux24(2,3)
   Snew(2,4)=(gs(k+1,2))*gv1(k+1,4)+(bs(1,2,k))*aux24(1,4)+(bs(2,2,k))*aux24(2,4)
   hs(1,k+1)=Snew(1,1)*hv1(1,k+1)+Snew(1,2)*hv1(2,k+1)+Snew(1,3)*hv1(3,k+1)+Snew(1,4)*hv1(4,k+1)
   hs(2,k+1)=Snew(2,1)*hv1(1,k+1)+Snew(2,2)*hv1(2,k+1)+Snew(2,3)*hv1(3,k+1)+Snew(2,4)*hv1(4,k+1)

enddo


gvnew=gs
hvnew=hs
bvnew=bs


! Compression of U

bunew(1)=0.d0
xnum=q1(1)
yvec=gu1(1,:)
znum=du1(1)
hunew(1)=0.d0
hunew(2)=gu1(1,1)*hu1(1,2)+gu1(1,2)*hu1(2,2)+gu1(1,3)*hu1(3,2)
gunew(1)=1.d0
do k=2,kfin+3
  xx(1)=xnum
  xx(2)=q1(k)
  call givensr(xx,WW)
  auxmat2(1,1)=znum
  auxmat2(1,2)=hunew(k)
  auxmat2(2,1)=p1(k)*(xnum)
  auxmat2(2,2)=du1(k)
  Zmat=matmul(auxmat2,WW)
  xnum=xx(1)*WW(1,2)+xx(2)*WW(2,2)
  xx=Zmat(:,1)
  call givens(xx,auxmat22)
 
  bunew(k)=(auxmat22(2,1))
  gunew(k)=(auxmat22(2,2))
  auxvec=yvec
  yvec(1)=(gunew(k))*gu1(k,1)+(bunew(k))*(auxvec(1)*bu1(1,1,k)+auxvec(2)*bu1(2,1,k)+auxvec(3)*bu1(3,1,k))
  yvec(2)=(gunew(k))*gu1(k,2)+(bunew(k))*(auxvec(1)*bu1(1,2,k)+auxvec(2)*bu1(2,2,k)+auxvec(3)*bu1(3,2,k))
  yvec(3)=(gunew(k))*gu1(k,3)+(bunew(k))*(auxvec(1)*bu1(1,3,k)+auxvec(2)*bu1(2,3,k)+auxvec(3)*bu1(3,3,k))
  znum=(gunew(k))*Zmat(2,2)+(bunew(k))*Zmat(1,2)
  hunew(k+1)=yvec(1)*hu1(1,k+1)+yvec(2)*hu1(2,k+1)+yvec(3)*hu1(3,k+1)
enddo

end if ! if-then for truncated main loops


end subroutine structured_QZ_ds
!************************************************************



!*************************************************************

subroutine recover_A(A,sigmaa,gv,hv,bv,z,w,n)

! Recovers the matrix A from its quasiseparable generators.

implicit none

integer, intent(in) :: n
real(kind(1d0)), intent(in) :: sigmaa(n-1),gv(n,2),hv(2,n),bv(2,2,n-1),z(n),w(n)
real(kind(1d0)), intent(out) :: A(n,n)

integer :: j,k
real(kind(1d0)) :: x(2),auxx(2)

A=0.d0

! Fill subdiagonal:
do k=1,n-1
   A(k+1,k)=sigmaa(k)
enddo

! Fill upper triangular part:
do j=1,n-1
   x=gv(j,:)
   do k=j,n-1
      A(j,k)=x(1)*hv(1,k)+x(2)*hv(2,k)-z(j)*(w(k))
      auxx=x
      x(1)=auxx(1)*bv(1,1,k)+auxx(2)*bv(2,1,k)
      x(2)=auxx(1)*bv(1,2,k)+auxx(2)*bv(2,2,k)
   enddo
   A(j,n)=x(1)*hv(1,n)+x(2)*hv(2,n)-z(j)*(w(n))
enddo
A(n,n)=gv(n,1)*hv(1,n)+gv(n,2)*hv(2,n)-z(n)*(w(n))

return

end subroutine recover_A


!************************************************************


subroutine recover_A_red(A,sigmaa,gv,hv,bv,z,w,n)

! Recovers the matrix A from its redundant (order 4) quasiseparable generators.

implicit none

integer, intent(in) :: n
real(kind(1d0)), intent(in) :: sigmaa(n-1),gv(n,4),hv(4,n),bv(4,4,n),z(n),w(n)
real(kind(1d0)), intent(out) :: A(n,n)

integer :: j,k
real(kind(1d0)) :: x(4),auxx(4)

A=0.d0

! Fill subdiagonal:
do k=1,n-1
   A(k+1,k)=sigmaa(k)
enddo

! Fill upper triangular part:
do j=1,n-1
   x=gv(j,:)
   do k=j,n-1
      A(j,k)=x(1)*hv(1,k)+x(2)*hv(2,k)+x(3)*hv(3,k)+x(4)*hv(4,k)-z(j)*(w(k))
      auxx=x
      x(1)=auxx(1)*bv(1,1,k)+auxx(2)*bv(2,1,k)+auxx(3)*bv(3,1,k)+auxx(4)*bv(4,1,k)
      x(2)=auxx(1)*bv(1,2,k)+auxx(2)*bv(2,2,k)+auxx(3)*bv(3,2,k)+auxx(4)*bv(4,2,k)
      x(3)=auxx(1)*bv(1,3,k)+auxx(2)*bv(2,3,k)+auxx(3)*bv(3,3,k)+auxx(4)*bv(4,3,k)
      x(4)=auxx(1)*bv(1,4,k)+auxx(2)*bv(2,4,k)+auxx(3)*bv(3,4,k)+auxx(4)*bv(4,4,k)
   enddo
   A(j,n)=x(1)*hv(1,n)+x(2)*hv(2,n)+x(3)*hv(3,n)+x(4)*hv(4,n)-z(j)*(w(n))
enddo
A(n,n)=gv(n,1)*hv(1,n)+gv(n,2)*hv(2,n)+gv(n,3)*hv(3,n)+gv(n,4)*hv(4,n)-z(n)*(w(n))

return

end subroutine recover_A_red


!************************************************************


!************************************************************

subroutine recover_B(B,db,gu,hu,bu,p,q,n)

! Recovers the matrix B from its quasiseparable generators.

implicit none

integer, intent(in) :: n
real(kind(1d0)), intent(in) :: db(n),gu(n-1),hu(n),bu(n-1),p(n),q(n)
real(kind(1d0)), intent(out) :: B(n,n)

integer :: j,k
real(kind(1d0)) :: x

B=0.d0

! Fill diagonal:
do j=1,n
   B(j,j)=db(j)
enddo

! Fill upper triangular part:
do j=1,n-2
   x=gu(j)
   do k=j+1,n-1
      B(j,k)=x*hu(k)-p(j)*(q(k))
      x=x*bu(k)
   enddo
   B(j,n)=x*hu(n)-p(j)*(q(n))
enddo
B(n-1,n)=gu(n-1)*hu(n)-p(n-1)*(q(n))
  

return

end subroutine recover_B

!************************************************************


!************************************************************

subroutine hh3(x,H)

! Computes 3x3 Householder matrix that reduces the vector x.

implicit none

real(kind(1d0)), intent(in) :: x(3)
real(kind(1d0)), intent(out) :: H(3,3)
real(kind(1d0)) :: xx(2),yy(2),GG1(2,2),GG2(2,2)

xx(1)=x(2)
xx(2)=x(3)
call givens(xx,GG1)
yy(1)=x(1)
yy(2)=GG1(1,1)*xx(1)+GG1(1,2)*xx(2)
call givens(yy,GG2)
H(1,1)=GG2(1,1)
H(1,2)=GG2(1,2)*GG1(1,1)
H(1,3)=GG2(1,2)*GG1(1,2)
H(2,1)=GG2(2,1)
H(2,2)=GG2(2,2)*GG1(1,1)
H(2,3)=GG2(2,2)*GG1(1,2)
H(3,1)=0.d0
H(3,2)=GG1(2,1)
H(3,3)=GG1(2,2)

return

end subroutine hh3

!************************************************************


!************************************************************


subroutine companion_generators(n,poly,sigmaa,gv,hv,bv,z,w,db,gu,hu,bu,p,q)

! Computes quasiseparable generators for the companion pencil
! associated with the polynomial poly.

implicit none

integer, intent(in) :: n
real(kind(1d0)), intent(in) :: poly(n+1)
real(kind(1d0)), intent(out) :: sigmaa(n-1),gv(n,2),hv(2,n),bv(2,2,n-1),z(n),w(n)
real(kind(1d0)), intent(out) :: db(n),gu(n-1),hu(n),bu(n-1),p(n),q(n)
integer :: j

 
! Compute generators for A. 
do j=1,n-1
   sigmaa(j)=1.d0
   bv(1,1,j)=1.d0
   bv(1,2,j)=0.d0
   bv(2,1,j)=0.d0
   bv(2,2,j)=1.d0
enddo
gv=0.d0
gv(1,1)=1.d0
hv=0.d0
hv(1,n)=1.d0
z=0.d0
z(1)=1.d0
w=(poly(2:n+1))
w(n)=w(n)+1

! Compute generators for B.
db=1.d0
db(1)=poly(1)
gu=0.d0
hu=0.d0
bu=0.d0
p=0.d0
p(1)=1.d0
q=0.d0
q(1)=1.d0-(poly(1))

return

end subroutine companion_generators

!**************************************************


! ************************************************* 

subroutine givens(x,G)

! Computes a Givens matrix G such that
! G*x has the second entry equal to zero. 
! Follows algorithm in Golub & Van Loan,
! 4th edition, page 240 (alg. 5.1.3).
 

implicit none

real(kind(1d0)), intent(in) :: x(2)
real(kind(1d0)), intent(out) :: G(2,2)
real(kind(1d0)) :: c,s,t,a,b

a=x(1)
b=x(2)

if (b == 0.d0) then
   c=1.d0
   s=0.d0
else
   if (abs(b)>abs(a)) then
      t=-a/b
      s=1/sqrt(1+t*t)
      c=s*t
   else
      t=-b/a
      c=1/sqrt(1+t*t)
      s=c*t
   end if
end if

G(1,1)=c
G(1,2)=-s
G(2,1)=s
G(2,2)=c

end subroutine givens

!**********************************************************


!**********************************************************

subroutine givensr(y,H)

! Computes 2x2 Givens matrix that acts on the right
! of row vector y and puts a zero in the leftmost entry.

implicit none
real(kind(1d0)), intent(in) :: y(2)
real(kind(1d0)), intent(out) :: H(2,2)
real(kind(1d0)) :: G(2,2) 

call givens(y,G)
H(1,1)=G(2,1)
H(1,2)=G(1,1)
H(2,1)=G(2,2)
H(2,2)=G(1,2)

return

end subroutine givensr


!*********************************************************

!**********************************************************

subroutine hh23(A,Z)

! Computes a 3x3 orthogonal matrix Z such that 
! A*Z=[0 x x; 0 0 x], where A is 2x3.

implicit none
real(kind(1d0)), intent(in) :: A(2,3)
real(kind(1d0)), intent(out) :: Z(3,3)
real(kind(1d0)) :: y(2),Z0(2,2),H0(3,3),H1(3,3),H2(3,3),B(2,3)

y(1)=A(2,2)
y(2)=A(2,1)
call givens(y,Z0)
H0(1,1)=Z0(2,2)
H0(1,2)=Z0(1,2)
H0(1,3)=0.d0
H0(2,1)=Z0(2,1)
H0(2,2)=Z0(1,1)
H0(2,3)=0.d0
H0(3,1)=0.d0
H0(3,2)=0.d0
H0(3,3)=1.d0
B=matmul(A,H0)
y(1)=B(2,3)
y(2)=B(2,2)
call givens(y,Z0)
H1(1,1)=1.d0
H1(1,2)=0.d0
H1(1,3)=0.d0
H1(2,1)=0.d0
H1(2,2)=Z0(2,2)
H1(2,3)=Z0(1,2)
H1(3,1)=0.d0
H1(3,2)=Z0(2,1)
H1(3,3)=Z0(1,1)
B=matmul(B,H1)
y(1)=B(1,2)
y(2)=B(1,1)
call givens(y,Z0)
H2(1,1)=Z0(2,2)
H2(1,2)=Z0(1,2)
H2(1,3)=0.d0
H2(2,1)=Z0(2,1)
H2(2,2)=Z0(1,1)
H2(2,3)=0.d0
H2(3,1)=0.d0
H2(3,2)=0.d0
H2(3,3)=1.d0

Z=matmul(H0,matmul(H1,H2))
return

end subroutine hh23


!*********************************************************




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
