!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!  Structured Markov Chains Solver      [  SMCSolver  ]       !
!  Dario Bini, Beatrice Meini, Sergio Steffe'                 !
!  bini@dm.unipi.it, meini@dm.unipi.it, steffe@dm.unipi.it    !
!  Dipartimento di Matematica "Leonida Tonelli"               !
!  Largo Pontecorvo 5                                         !
!  56127 Pisa                                                 !
!  Italy                                                      !
!  Version 1.1 - Oct 2006                                     !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
!
!
  subroutine drft(am1, a0, a1, v, drift)
    ! compute the drift and the left dominant eigenvector v of Am1+A0+A1, where
    ! Am1,A0,A1 are nonnegative and their sum is stochastic
    use smc_int, only: gth
    use smc_tools
    use ponte_f_f, only: debug,verb,timing,finish,wout
    implicit none
    ! arguments
    real(dp),intent(in),dimension(:,:) :: am1,a0,a1
    real(dp),intent(out),dimension(:)  :: v
    real(dp),intent(out)               :: drift
    ! local variables
    real(dp),allocatable,dimension(:)  :: p
    real(dp),dimension(:,:),allocatable :: a
    integer :: m
    m=size(a0,1)
    if (allocated(a)) deallocate(a) 
    if (allocated(p)) deallocate(p) 
    allocate(a(m,m),p(m),stat=info)
    if (info/=0) return
    a=am1+a0+a1
    call gth(a,v)
    a=a0+2*a1
    p=sum(a,dim=2);
    drift=dot_product(p,v)-1.d0;
    if (allocated(a)) deallocate(a)
    if (allocated(p)) deallocate(p)
  end subroutine drft

  subroutine gth(a,p)
    ! Compute the probability invariant vector p of the stochastic matrix A
    ! by using the gth trick  
    use smc_tools
    use ponte_f_f, only: debug,verb,timing,finish,wout
    use f95_lapack, only: la_gesv
    implicit none
    ! arguments
    real(dp),intent(in),dimension(:,:) :: a
    real(dp),intent(out),dimension(:)  :: p
    ! local variables
    real(dp),dimension(:,:),allocatable :: aa
    real(dp) :: m, s
    integer  :: n, i, k, ii, j
!
    n=size(a,1)
    if (allocated(aa)) deallocate(aa) 
    allocate(aa(n,n),stat=info)
    if (info/=0) return
    aa=-a
    do i=1,n
       aa(i,i)=aa(i,i)+1.0d0
    end do
    do k=1,n-1
       do ii=k+1,n
          if (abs(aa(k,k))<epsilon(1.d0)) then
             if(verb)then
                write(wout,*)"Zero diagonal entry in gth. La_gesv used"
                call print_it
             end if
             goto 50
          end if
          m=aa(ii,k)/aa(k,k)
          aa(ii,k)=m
          do j=k+1,n
             if (ii /= j) aa(ii,j)=aa(ii,j)-m*aa(k,j)
          end do
       end do
       ! diagonal adjustment
       do ii=k+1,n
          aa(ii,ii)=-sum(aa(ii,k+1:ii-1))-sum(aa(ii,ii+1:n));
       end do
    end do
    p(n)=1
    do ii=n-1,1,-1
       s=0
       do j=ii+1,n
          s=s+aa(j,ii)*p(j);
       end do
       p(ii)=-s
    end do
    p=p/sum(p)
    return
50 continue
    aa=-a
    do i=1,n
       aa(i,i)=aa(i,i)+1.0d0
    end do
    aa =transpose(aa)
    aa(n,:)=1.d0
    p=0.d0
    p(n)=1.d0
    call la_gesv(aa,p)
  end subroutine gth


subroutine bgth(v,a,b)
  ! Solve the system Ax=b where A is an M-matrix and v=Ae, e=(1,...,1)
  ! by using the generalization of the GTH trick
  ! Input:  A, v and b
  ! Output: b
    use smc_tools
    use ponte_f_f, only: debug,verb,timing,finish,wout
    implicit none
    real(dp),intent(in),dimension(:,:) :: a
    real(dp),intent(inout),dimension(:,:) :: b
    real(dp),intent(inout),dimension(:)   :: v
    ! local variables
    real(dp),dimension(:), allocatable :: alfa
    real(dp),dimension(:,:),allocatable:: p, y
    integer:: m, n, i, j, k
    real(dp) :: ak
    n=size(a,1)
    m=size(b,2)
    if (allocated(alfa)) deallocate(alfa)
    if (allocated(p)) deallocate(p)
    allocate(alfa(n),p(n,n),stat=info)
    if (info/=0) return
    p=-a
    ! LU factorization
    do k=1,n-1
       alfa(k)=v(k)+sum(p(k,k+1:n))
       ak=1.d0/alfa(k)
       Do i=k+1,n
          Do j=k+1,n
             If(i/=j)then
                p(i,j)=p(i,j)+p(i,k)*p(k,j)*ak
             End If
          End Do
       End Do
       Do j=k+1,n
          v(j)=v(j)+v(k)*p(j,k)*ak
       End Do
    End do
    alfa(n)=v(n)
    alfa=1.d0/alfa
    ! Solving LY=B
    allocate(y(n,m),stat=info)
    ak=alfa(1)
    y(1,:)=b(1,:)*ak
    Do k=2,n
       ak=alfa(k)
       y(k,:)=(b(k,:)+matmul(p(k,1:k-1),y(1:k-1,:)))*ak
    End Do
    ! Solving UB=Y
    b(n,:)=y(n,:)
    Do k=n-1,1,-1
       ak=alfa(k)
       b(k,:)=y(k,:)+Matmul(p(k,k+1:n),b(k+1:n,:))*ak
    End Do
    if (allocated(alfa)) deallocate(alfa)
    if (allocated(p)) deallocate(p)
  end subroutine bgth

  subroutine crqbd(Am1, A0, A1,  G, R, U, doshift, dogth,  drift, nerror, niteraz)
! compute the minimal solutions of the equations
!   Am1 + A0 G + A1 G^2   = G
!   A1  + R A0 + R^2 Am1  = R
!   A0 + Am1(I-U)^(-1) A1 = U
!   by using (shifted) cyclic reduction
! Input variables
!   Am1, A0, A1
!   doshift:  if .true. the shift acceleration is performed
!   dogth:    if .true. the diagonal adjustment technique is applied
!   verb: (global)   if .true. an error estimate is printed at each step
!   debug (global)   if .true. some printings are performed on the console
! Output variables:
!   G, R, U (optional)
!   drift (optional)
    use smc_tools
    use smc_int, only: drft, bgth
    USE f95_lapack, ONLY: la_getrf, la_getri, la_gesv
    use ponte_f_f, only: debug,verb,timing,finish,wout,fdrift
    implicit none
    ! arguments
    real(dp),intent(in),dimension(:,:) :: Am1, A0, A1
    logical, intent(in), optional  :: doshift, dogth
    real(dp),intent(out), dimension(:,:) :: G
    real(dp),intent(out),optional, dimension(:,:) ::  R, U
    real(dp),intent(out),optional ::drift
    integer,optional,intent(in) :: nerror
    integer, optional,intent(in) :: niteraz
    ! parameters
    !real(dp),parameter :: eps=epsilon(1.d0)
    !integer, parameter :: nitmax=50  
    ! local variables
    integer :: m, i, k, nitmax
    real(dp),dimension(:,:),allocatable :: tam1, ta0, ta1, tha, aux, auxbig, gg
    real(dp),dimension(:),allocatable   :: v,w,vgth
    real(dp) :: f, err, ldrift, eps
    logical :: ldoshift,  ldogth
    integer,dimension(:),allocatable:: ipiv
!10 FORMAT(1X,"*",\)
10 FORMAT("*")
    m=size(a0,1)
    if (debug) write(*,*) "crqbd:  Dimension AM1 =",size(am1,1), size(am1,2)
    if (debug) write(*,*) "crqbd:  Dimension A0 =",size(a0,1), size(a0,2)
    if (debug) write(*,*) "crqbd:  Dimension A1 =",size(a1,1), size(a1,2) 
!   defaults parameters if missing
    if ( .not. present(nerror)) then
          eps=epsilon(1.d0) 
       else 
          eps=epsilon(1.d0) * 10.0d0 ** nerror
    endif
    if ( .not. present(niteraz)) then
          nitmax = 50
       else
          nitmax = niteraz
    endif 
    if (debug) write(*,*) "crqbd: eps=",eps," nitmax=",nitmax
!   from here nitmax and eps are the values of the parameters
 
    if (allocated(tam1)) deallocate(tam1)
    if (allocated(ta0)) deallocate(ta0)
    if (allocated(ta1)) deallocate(ta1)
    if (allocated(tha)) deallocate(tha)
    if (allocated(auxbig)) deallocate(auxbig)
    if (allocated(aux)) deallocate(aux)
    if (allocated(gg)) deallocate(gg)
    if (allocated(v)) deallocate(v)
    if (allocated(ipiv)) deallocate(ipiv)
    if (allocated(w)) deallocate(w)
    if (allocated(vgth)) deallocate(vgth)
    allocate(tam1(m,m),ta0(m,m),ta1(m,m),tha(m,m),auxbig(m,2*m),aux(m,m), &
      gg(m,m),stat=info)
    if (info/=0) return
    allocate(v(m),ipiv(m),w(m),vgth(m),stat=info)
    if (info/=0) return
    ! -1- initialize
    ldoshift=.true.;ldogth=.false.
    if(present(doshift))then
       ldoshift=doshift
       if(present(dogth))then
          ldogth=dogth
          if(dogth.and.doshift)then
             if(debug) write(*,*)"crqbd: Warning: doshift and dogth cannot be both .true."
             ldogth=.false.
          end if
       else
          ldoshift=.true.
          ldogth=.false.
       end if
    end if
    call drft(Am1,A0,A1,v,ldrift)
    fdrift=ldrift
    if(present(drift))drift=ldrift
    if(present(dogth).and. .not.present(doshift))then
       ldogth=dogth
       ldoshift=.not. dogth
    end if

    tam1=-Am1; ta0=-A0; ta1=-A1
    do i=1,m
       ta0(i,i)=1+ta0(i,i)
    end do
    !-2- shift
    if(ldoshift)then
       f=1.d0/m
       if(ldrift<0) then  
          v=sum(tam1,dim=2)
          do i=1,m
             tam1(i,:)=tam1(i,:)-v(i)*f
          end do
          v=sum(ta1,dim=2)
          do i=1,m
             ta0(i,:)=ta0(i,:)+v(i)*f
          end do
       else
          w=matmul(transpose(a1),v)
          do i=1,m
             ta1(:,i)=ta1(:,i)+w(i)
          end do
          w=matmul(transpose(am1),v)
          do i=1,m
             ta0(:,i)=ta0(:,i)-w(i)
          end do
       end if
    end if
    tha=ta0

    !-3- cr iteration
    do k=1,nitmax
       auxbig(1:m,1:m)=tam1
       auxbig(1:m,m+1:2*m)=ta1
       aux=ta0
       if(ldogth)then
          vgth = -sum(tam1+ta1,dim=2)
          call bgth(vgth,aux,auxbig)
       else
          call la_gesv(aux,auxbig)
       end if
       aux=matmul(ta1,auxbig(1:m,1:m))
       ta0=ta0-aux-matmul(tam1,auxbig(1:m,m+1:2*m))
       tha=tha-aux
       tam1=-matmul(tam1,auxbig(1:m,1:m))
       ta1=-matmul(ta1,auxbig(1:m,m+1:2*m))
       !-3.1- check stop 
       if(ldoshift)then
          err = maxval(sum(abs(ta1),dim=1))*maxval(sum(abs(tam1),dim=1))
       else
          if(ldrift<0) then
             err = maxval(-sum(ta1,dim=1))
          else
             err = maxval(-sum(tam1,dim=1))
          end if
       end if
       if(verb)then
          write(wout,*)"--iter=",k,"  Check  ",err
          call print_it
          else
          write(wout,10)
          call print_it_nolf
       end if
       if(err<eps)exit
    end do

 
! compute the solutions
    if(k>= nitmax) then 
           write(wout,*)"Reached the maximum number of iterations in CR"
           call print_it
           endif
    aux=tha
    call la_getrf(aux,ipiv)
    call la_getri(aux,ipiv)
    gg=matmul(aux,am1)
    aux=a0+matmul(a1,gg)
    if(present(u)) u=aux
    
    g=gg
    if(present(R))  then
       aux=-aux
       do i=1,m
          aux(i,i)=aux(i,i)+1
       end do
       call la_getrf(aux,ipiv)
       call la_getri(aux,ipiv)
       r=matmul(a1,aux)
    end if
    if(verb)then
!       write(wout,*)"drift=",ldrift
!        call print_it
       write(wout,*)"Flags: doshift=",ldoshift,"dogth=",ldogth
       call print_it
       write(wout,*)"G-residual=",maxval(abs(matmul(matmul(a1,g)+a0,g)+am1-g))
       call print_it
       if(present(R)) then
           write(wout,*) "R-residual=", &
                              maxval(abs(matmul(r,matmul(r,am1)+a0)+a1-r))
            call print_it
        endif
       if(present(u)) then
          ta0=u
          ta1=-u
          do i=1,m
             ta1(i,i)=1.d0+ta1(i,i)
          end do
          call la_getrf(ta1,ipiv)
          call la_getri(ta1,ipiv)
          ta1=ta0-matmul(a1,matmul(ta1,am1))-a0
          write(wout,*)"U-residual=",maxval(abs(ta1))
          call print_it
       end if
    else
! if not verbose, print something anyway ....
        write(wout,*)" "
        call print_it
        write(wout,*)"G-residual=",maxval(abs(matmul(matmul(a1,g)+a0,g)+am1-g))
        call print_it
        if(present(R)) then
            write(wout,*) "R-residual=", &
                            maxval(abs(matmul(r,matmul(r,am1)+a0)+a1-r))
            call print_it
         end if
        if(present(u)) then
          ta0=u
          ta1=-u
          do i=1,m
             ta1(i,i)=1.d0+ta1(i,i)
          end do
          call la_getrf(ta1,ipiv)
          call la_getri(ta1,ipiv)
          ta1=ta0-matmul(a1,matmul(ta1,am1))-a0
          write(wout,*)"U-residual=",maxval(abs(ta1))
          call print_it
        end if
    end if
    if (allocated(tam1)) deallocate(tam1)
    if (allocated(ta0)) deallocate(ta0)
    if (allocated(ta1)) deallocate(ta1)
    if (allocated(tha)) deallocate(tha)
    if (allocated(auxbig)) deallocate(auxbig)
    if (allocated(aux)) deallocate(aux)
    if (allocated(gg)) deallocate(gg)
    if (allocated(v)) deallocate(v)
    if (allocated(ipiv)) deallocate(ipiv)
    if (allocated(w)) deallocate(w)
    if (allocated(vgth)) deallocate(vgth)
if(debug) write(*,*) "crqbd: info=",info
  end subroutine crqbd



  subroutine lrqbd(Am1, A0, A1, G, R, U, doshift, dogth, drift, nerror, niteraz)
! compute the minimal solutions of the equations
!   Am1 + A0 G + A1 G^2   = G
!   A1  + R A0 + R^2 Am1  = R
!   A0 + Am1(I-U)^(-1) A1 = U
!   by using (shifted) logarithmic reduction
! Input variables
!   Am1, A0, A1
!   doshift: if .true. the shift acceleration is performed
!   dogth:   if .true. the gth trick is applied
!   verb: (global) if .true. an error estimate is printed at each step
! Output variables:
!   G, R, U (optional)
!   drift (optional)
    use smc_tools
    use smc_int, only: drft, bgth
    USE f95_lapack, ONLY: la_getrf, la_getri, la_gesv
    use ponte_f_f, only: debug,verb,timing,finish,wout,fdrift
    implicit none
    ! arguments
    real(dp),dimension(:,:),intent(in) :: Am1, A0, A1
    real(dp),dimension(:,:),intent(out),optional :: G, R, U
    real(dp),intent(out),optional ::drift
    logical, optional,intent(in) :: doshift, dogth
    integer,optional,intent(in) :: nerror
    integer, optional,intent(in) :: niteraz
    ! parameters
    !real(dp),parameter :: eps=epsilon(1.d0)
    !integer, parameter :: nitmax=50  
    ! local variables
    integer :: m, i, k, nitmax
    real(dp),dimension(:,:),allocatable :: tam1, ta0, ta1, aux, auxbig
    real(dp),dimension(:,:),allocatable :: ww, uu,v1, vm1
    real(dp) :: f, err, ldrift, eps
    logical :: ldoshift, ldogth
    real(dp),dimension(:),allocatable :: v,w,vgth
    integer,dimension(:),allocatable:: ipiv
!10 FORMAT(1X,"*",\)
10 FORMAT("*")
    m=size(a0,1)
    if (debug) write(*,*) "lrqbd:  Dimension AM1 =",size(am1,1), size(am1,2)
    if (debug) write(*,*) "lrqbd:  Dimension A0 =",size(a0,1), size(a0,2)
    if (debug) write(*,*) "lrqbd:  Dimension A1 =",size(a1,1), size(a1,2)
!   defaults parameters if missing
    if ( .not. present(nerror)) then 
          eps=epsilon(1.d0) 
       else 
          eps=epsilon(1.d0) * 10.0d0 ** nerror
    endif
    if ( .not. present(niteraz)) then
          nitmax = 50
       else
          nitmax = niteraz
    endif
   if (debug) write(*,*) "lrqbd: eps=",eps," nitmax=",nitmax
!   from here nitmax and eps are the values of the parameters
    if (debug) write(*,*) "lrqbd:  Iteration max =",nitmax,"  epsilon =",eps
    if (allocated(tam1)) deallocate(tam1)
    if (allocated(ta0)) deallocate(ta0)
    if (allocated(ta1)) deallocate(ta1)
    if (allocated(auxbig)) deallocate(auxbig)
    if (allocated(aux)) deallocate(aux)
    if (allocated(v)) deallocate(v)
    if (allocated(ipiv)) deallocate(ipiv)
    if (allocated(w)) deallocate(w)
    if (allocated(vgth)) deallocate(vgth)
    if (allocated(uu)) deallocate(uu)
    if (allocated(v1)) deallocate(v1)
    if (allocated(vm1)) deallocate(vm1)
    if (allocated(ww)) deallocate(ww)
    allocate(tam1(m,m),ta0(m,m),ta1(m,m),auxbig(m,2*m),&
       aux(m,m),stat=info)
    if (info/=0) return
    allocate(v(m),ipiv(m),w(m),vgth(m),uu(m,m),v1(m,m),vm1(m,m),&
       ww(m,m),stat=info)
    if (info/=0) return

   ! -1- initialize
  ldoshift=.true.;ldogth=.false.

    if(present(doshift))then
       ldoshift=doshift
       if(present(dogth))then
          ldogth=dogth
          if(dogth.and.doshift)then
             if(debug) write(*,*)"lrqbd:  Warning: doshift and dogth cannot be both .true."
             ldogth=.false.
          end if
       else
          ldoshift=.true.
          ldogth=.false.
       end if
    end if
    call drft(Am1,A0,A1,v,ldrift)
    fdrift=ldrift
    if(present(drift))drift=ldrift
    if(present(dogth).and. .not.present(doshift))then
       ldogth=dogth
       ldoshift=.not. dogth
    end if

    tam1=-Am1; ta0=-A0; ta1=-A1
    do i=1,m
       ta0(i,i)=1+ta0(i,i)
    end do
    !-2- shift
    if(ldoshift)then
       f=1.d0/m
       if(ldrift<0) then  
          v=sum(tam1,dim=2)
          do i=1,m
             tam1(i,:)=tam1(i,:)-v(i)*f
          end do
          v=sum(ta1,dim=2)
          do i=1,m
             ta0(i,:)=ta0(i,:)+v(i)*f
          end do
       else
          w=matmul(transpose(a1),v)
          do i=1,m
             ta1(:,i)=ta1(:,i)+w(i)
          end do
          w=matmul(transpose(am1),v)
          do i=1,m
             ta0(:,i)=ta0(:,i)-w(i)
          end do
       end if
    end if

! LR inizialization
    aux=ta0
    call la_getrf(aux,ipiv)
    call la_getri(aux,ipiv)
    v1=-matmul(aux,ta1)
    vm1=-matmul(aux,tam1)
    ww=-matmul(vm1,v1)-matmul(v1,vm1)
    uu=0.d0
    do i=1,m
       ww(i,i)=ww(i,i)+1.d0
       uu(i,i)=1.d0
    end do
    g=vm1
    
! LR cycle
    do k=1,nitmax
       uu=matmul(uu,v1)
       aux=matmul(vm1,vm1)
       if(ldogth) vgth=sum(aux,dim=2)
       auxbig(1:m,1:m)=aux
       aux=matmul(v1,v1)
       if (ldogth) vgth=vgth+sum(aux,dim=2)
       auxbig(1:m,m+1:2*m)=aux
       aux=ww
       if(ldogth)then
          call bgth(vgth,aux,auxbig)
       else
          call la_gesv(aux,auxbig)
       end if
       vm1=auxbig(1:m,1:m)
       v1=auxbig(1:m,m+1:2*m)
       ww=matmul(vm1,v1)+matmul(v1,vm1)
       ww=-ww
       do i=1,m
          ww(i,i)=ww(i,i)+1.d0
       end do
       g = g + matmul(uu,vm1)
       err = min (maxval( sum(abs(v1),dim=2)), maxval(sum(abs(vm1),dim=2)))

       if(verb)then
          write(wout,*)"--iter=",k,"  Check  ",err
          call print_it
          else
          write(wout,10)
          call print_it_nolf
       end if

       if(err<eps)exit
    end do
! compute the solutions
    if(k>= nitmax) then 
          write(wout,*)"Reached the maximum number of iterations in LR"
          call print_it
     endif
    if (ldoshift.and. drift<0) g=g+f 
    if(present(u)) u=a0+matmul(a1,g)
    if(present(r)) then
       aux=-a0-matmul(a1,g)
       do i=1,m
          aux(i,i)=aux(i,i)+1.d0
       end do
       call la_getrf(aux,ipiv)
       call la_getri(aux,ipiv)
       r=matmul(a1,aux)
    end if
    if(verb)then
!       write(wout,*)"drift=",ldrift
!       call print_it
       write(wout,*)"Flags:  doshift=",ldoshift,"dogth=",ldogth
       call print_it
       write(wout,*)"G-residual=",maxval(abs(matmul(matmul(a1,g)+a0,g)+am1-g))
       call print_it
       if(present(R)) then
              write(wout,*) "R-residual=", &
                  maxval(abs(matmul(r,matmul(r,am1)+a0)+a1-r)) 
               call print_it
       endif
       if(present(u)) then
          aux=-u
          do i=1,m
             aux(i,i)=1.d0+aux(i,i)
          end do
          call la_getrf(aux,ipiv)
          call la_getri(aux,ipiv)
          ta1=u-matmul(a1,matmul(aux,am1))-a0
          write(wout,*)"U-residual=",maxval(abs(ta1))
          call print_it
       end if
   else 
! if not verbose, print something anyway ....
       write(wout,*)" "
       call print_it
       write(wout,*)"G-residual=",maxval(abs(matmul(matmul(a1,g)+a0,g)+am1-g))
       call print_it
       if(present(R)) then
            write(wout,*) "R-residual=", &
                            maxval(abs(matmul(r,matmul(r,am1)+a0)+a1-r))
            call print_it
       end if
       if(present(u)) then
          aux=-u
          do i=1,m
             aux(i,i)=1.d0+aux(i,i)
          end do
          call la_getrf(aux,ipiv)
          call la_getri(aux,ipiv)
          ta1=u-matmul(a1,matmul(aux,am1))-a0
          write(wout,*)"U-residual=",maxval(abs(ta1))
          call print_it
       end if
    endif
    if (allocated(tam1)) deallocate(tam1)
    if (allocated(ta0)) deallocate(ta0)
    if (allocated(ta1)) deallocate(ta1)
    if (allocated(auxbig)) deallocate(auxbig)
    if (allocated(aux)) deallocate(aux)
    if (allocated(v)) deallocate(v)
    if (allocated(ipiv)) deallocate(ipiv)
    if (allocated(w)) deallocate(w)
    if (allocated(vgth)) deallocate(vgth)
    if (allocated(uu)) deallocate(uu)
    if (allocated(v1)) deallocate(v1)
    if (allocated(vm1)) deallocate(vm1)
    if (allocated(ww)) deallocate(ww)
  end subroutine lrqbd
!=========================================================================!
!                          SUBROUTINE SHIFT                               !
!=========================================================================!
! On input: a
! on output: shifted coefficients a1, drift
SUBROUTINE shift(a,a1,drift)
  use smc_tools
  use smc_int, only: gth
  IMPLICIT NONE
  REAL(dp), DIMENSION(:,:,:):: a
  REAL(dp), DIMENSION(:,:) :: a1

  REAL(dp), DIMENSION(:,:), ALLOCATABLE   :: s,aux,aux1
  REAL(dp), DIMENSION(:), ALLOCATABLE     :: p,v,w
  REAL(dp)                                :: drift,im
  INTEGER                                 :: m,n,i,j
  n=size(a,3)
  m=size(a,1)
  im=1.d0/m
  if (allocated(s)) deallocate(s) 
  if (allocated(p)) deallocate(p) 
  if (allocated(v)) deallocate(v) 
  if (allocated(aux)) deallocate(aux) 
  if (allocated(w)) deallocate(w) 
  if (allocated(aux1)) deallocate(aux1) 
  allocate(s(m,m),p(m),v(m),aux(m,n),aux1(n,m),w(m),stat=info)
  if (info/=0) return
  a1=a(:,:,1)
  s=sum(a,dim=3)
  call gth(s,w)
  s=0.d0
  do i=2,n
     s=s+(i-1)*a(:,:,i)
  end do
  p=sum(s,dim=2)
  drift=dot_product(p,w)-1.d0
  if(drift<0)then ! positive recurrent
     do i=1,n
        aux(:,i)=sum(a(:,:,i),dim=2)
     end do
     v=0.d0
     do i=n-1,2,-1
        v=v+aux(:,i+1)
        p=v*im
        do j=1,m
           a(:,j,i)=a(:,j,i)+p
        end do
     end do
     p=sum(a(:,:,1),dim=2)*im
     do j=1,m
        a(:,j,1)=a(:,j,1)-p
     end do
  else
    do i=1,n
        aux1(i,:)=matmul(w,a(:,:,i))
     end do
     v=0
     do i=n,3,-1
        v=v+aux1(i,:)
        do j=1,m
           a(j,:,i)=a(j,:,i)-v
        end do
     end do
     p=aux1(1,:)
     do j=1,m
        a(j,:,2)=a(j,:,2)+p
     end do
  end if
end SUBROUTINE shift

! Transform GM1 into an MG1
subroutine gm1tomg1(A, AA, v)
  use smc_tools
  use smc_int, only: gth
  use ponte_f_f, only: debug,verb,timing,finish,wout

  implicit none
  real(dp),dimension(:,:,:):: A,AA
  real(dp),dimension(:):: v
  integer :: nba,n,i
  real(dp),dimension(:),allocatable:: vi
 real(dp),dimension(:,:),allocatable:: SumA

  nba=size(a,3)
  n=size(a,1)
  if(allocated(SumA))deallocate(SumA)
  if(allocated(vi)) deallocate(vi)
  allocate(SumA(n,n),vi(n),stat=info)
  if(info/=0)then
     write(wout,*)"in gm1tomg1, info=",info
     call print_it
     return
  end if
  SumA=sum(A(:,:,:),dim=3)
  call gth(SumA,v)
  vi=1.d0/v
  do i=1,nba
     AA(:,:,i)=transpose(A(:,:,i))
  end do
  do i=1,n
     AA(:,i,:)=AA(:,i,:)*v(i)
     AA(i,:,:)=AA(i,:,:)*vi(i)
  end do
end subroutine gm1tomg1



subroutine rresidual(A,r,norm1)
  use smc_tools
!  use ponte_f_f, only: debug,verb,timing,finish,wout

  implicit none
  real(dp),dimension(:,:,:):: A
  real(dp),dimension(:,:):: R
  real(dp)::norm1
  real(dp),dimension(:,:),allocatable :: aux
  integer :: nba, m,i
  m=size(a,1)
  nba=size(a,3)
  if(allocated(aux)) deallocate(aux)
  allocate(aux(m,m),stat=info)
  if(info/=0) then
     return
  end if
  aux=a(:,:,nba)
  do i=nba-1,1,-1
     aux=a(:,:,i)+matmul(R,aux)
  end do
  aux=aux-r
  norm1=maxval(sum(abs(aux),dim=1))
end subroutine rresidual


subroutine utor(A1,u,r)
use smc_tools
use ponte_f_f, only: wout
USE f95_lapack, ONLY: la_gesv
implicit none 
integer :: n,i
real(dp),dimension(:,:) :: a1,u,r
real(dp),dimension(:,:), allocatable :: aux
n=size(A1,1)
if(allocated(aux))deallocate(aux)
allocate(aux(n,n),stat=info)
if(info/=0)then
write(wout,*)"In UTOR: info=",info
call print_it
return
end if
R=transpose(A1)
aux=-U
do i=1,n
aux(i,i)=aux(i,i)+1.d0
end do
aux=transpose(aux)
call la_gesv(aux,r)
r=transpose(r)
end subroutine utor

subroutine gtou(A0,A1,G,U)
use smc_tools
implicit none 
real(dp),dimension(:,:) :: a0,a1,g,u
U=A0+matmul(A1,G)
end subroutine gtou


subroutine qbdrres(Am1,A0,A1,r,res)
use smc_tools
implicit none 
real(dp),dimension(:,:) :: am1,a0,a1,r
real(dp)::res
res=  maxval(abs(matmul(r,matmul(r,am1)+a0)+a1-r))
end subroutine qbdrres

subroutine qbdures(Am1,A0,A1,u,res)
use smc_tools
use ponte_f_f, only : wout
USE f95_lapack, ONLY: la_getri, la_getrf
implicit none 
real(dp),dimension(:,:) :: am1,a0,a1,u
real(dp),dimension(:,:),allocatable :: aux
integer,dimension(:),allocatable:: ipiv
real(dp)::res
integer :: m,i
m=size(am1,1)
if (allocated(aux))deallocate(aux)
if(allocated(ipiv))deallocate(ipiv)
allocate(aux(m,m),ipiv(m),stat=info)
if(info/=0)then
write(wout,*)"qbdu_res: info=",info
call print_it
return
end if
aux=-u
do i=1,m
aux(i,i)=aux(i,i)+1.d0
end do
call la_getrf(aux,ipiv)
call la_getri(aux,ipiv)
res=  maxval(abs(u-matmul(a1,matmul(aux,am1))-a0))
end subroutine qbdures
