!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!  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 2.1 - June  2009                                   !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
!
!
subroutine qbdpi(Am1, A0, A1, Bm1, B0, B1, R, maxnc, epspi, pi0,pi,pinc)
  use smc_tools
  USE f95_lapack
  use smc_int, only:gth
  implicit none
  
  integer :: maxnc,pinc
  real(dp),dimension(:,:) :: Am1, A0, A1, Bm1, B0,  R
  real(dp),dimension(:,:), optional :: B1
  real(dp),dimension(:,:)::pi
  real(dp),dimension(:):: pi0
  real(dp):: epspi,sumpi,alpha
  real(dp),dimension(:,:),allocatable::aux,aux1,aux2
  real(dp),dimension(:),allocatable::x,y,v
   integer :: n,i,count,m
10 FORMAT("*")
   n=size(A0,1)
   m=size(B0,1)
   if(allocated(aux))deallocate(aux)
   if(allocated(aux1))deallocate(aux1)
   if(allocated(aux2))deallocate(aux2)
   if(allocated(x))deallocate(x)
   if(allocated(y))deallocate(y)
   if(allocated(v))deallocate(v)
   allocate(aux(m,m),aux1(n,n),aux2(m+n,m+n),x(m),y(n),v(m+n),stat=info)
   if (info/=0) then
        if(debug) write(*,*) "qbdpi: info=",info
        info=1000
        return
   endif
   aux1=-r
   do i=1,n
      aux1(i,i)=aux1(i,i)+1.d0
   end do
   y=1.d0
   call la_gesv(aux1,y)
   aux1=transpose(R)
   if(.not. present(B1)) then
      aux=B0+matmul(R,Bm1)
      call gth(aux,x)
!if(debug) write(*,*)"alfa=",(1.d0/dot_product(x,y))
!if(debug) write(*,*)"x=",x
      x=x*(1.d0/dot_product(x,y))
      pi0=x
      sumpi=sum(x)
      if(verb)then
         write(wout,*)"--component=",0,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
         if((count /= 0) .and. (mod(count,100)==0)) then 
            call print_it 
         else 
            call print_it_nolf
         end if
      end if
      count=1
      pi(:,1)=matmul(aux1,pi0)
      sumpi=sumpi+sum(pi(:,1))
      if(verb)then
         write(wout,*)"--component=",count,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
         if((count /= 0) .and. (mod(count,100)==0)) then 
            call print_it 
         else 
            call print_it_nolf
         end if
      end if
      do while (sumpi<1.d0-epspi .and. count <maxnc)
         count=count+1
         pi(:,count)=matmul(aux1,pi(:,count-1))
         sumpi=sumpi+sum(pi(:,count))
         if(verb)then
            write(wout,*)"--component=",count,"accumulated mass=",sumpi
            call print_it
         else
            write(wout,10)
            if((count /= 0) .and. (mod(count,100)==0)) then 
               call print_it 
            else 
               call print_it_nolf
            end if
         end if
      end do
      if(verb) then 
      else
         write(wout,*)" "
         call print_it
      end if
   else
      aux2(1:m,1:m)=b0;      aux2(1:m,m+1:m+n)=b1
      aux2(m+1:m+n,1:m)=bm1; aux2(m+1:m+n,m+1:m+n)=a0+matmul(r,am1)
      call gth(aux2,v)
!! verificare alfa  
      alpha=sum(v(1:m))+dot_product(v(m+1:m+n),y)
      alpha=1.d0/alpha
!if(debug) write(*,*)"alfa=",alpha
!if(debug) write(*,*)v
      v=v*alpha
      pi0=v(1:m)
      sumpi=sum(pi0)
      if(verb)then
         write(wout,*)"--component=",0,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
         if((count /= 0) .and. (mod(count,100)==0)) then 
            call print_it 
         else 
            call print_it_nolf
         end if
      end if

      pi(:,1)=v(m+1:m+n)
      sumpi=sumpi+sum(pi(:,1))
      count=1

      if(verb)then
         write(wout,*)"--component=",count,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
         if((count /= 0) .and. (mod(count,100)==0)) then 
            call print_it 
         else 
            call print_it_nolf
         end if
      end if

      do while (sumpi<1.d0-epspi .and. count <maxnc)
         count=count+1
         pi(:,count)=matmul(aux1,pi(:,count-1))
         sumpi=sumpi+sum(pi(:,count))
         if(verb)then
            write(wout,*)"--component=",count,"accumulated mass=",sumpi
            call print_it
         else
            write(wout,10)
            if((count /= 0) .and. (mod(count,100)==0)) then 
               call print_it 
            else 
               call print_it_nolf
            end if
         end if
      end do
      if(verb) then
      else
         write(wout,*)" "
         call print_it
      end if
      
   end if
   pinc=count
   if(allocated(aux))deallocate(aux)
   if(allocated(aux1))deallocate(aux1)
   if(allocated(aux2))deallocate(aux2)
   if(allocated(x))deallocate(x)
   if(allocated(y))deallocate(y)
   if(allocated(v))deallocate(v)
 end subroutine qbdpi



! Normalization of pi: once pi0 has been computed such that p0 e=1,
! set pi0=pi0 * alpha where 
! alpha=1/(1+pi0 sum_{i>=1}B_i^* (I-sum_{i>=0} A_i^*)^{-1} e)
! this formula works even when the size of B0 is different from that of A0

 subroutine mg1pi(A, B0, B, Bm1, G, maxnc, epspi, pi0, pi,pinc)
   use smc_tools
   USE f95_lapack
   use smc_int, only:gth
   implicit none
   
   integer :: maxnc, pinc
   real(dp),dimension(:,:,:) :: A, B
   real(dp),dimension(:,:), optional :: Bm1
   real(dp),dimension(:,:)::pi,G,B0
   real(dp),dimension(:):: pi0
   real(dp):: epspi
   
   real(dp)::sumpi, alpha
   real(dp),dimension(:,:,:),allocatable:: Bstar, Astar
   real(dp),dimension(:,:),allocatable::Astari,B0star,SumA,SumB
   real(dp),dimension(:),allocatable::x,y
   integer,dimension(:),allocatable::ipiv
   integer :: n,nbb, nba, i,count,m

   n=size(A,1)
   nba=size(A,3)
   m=size(B,1)
   nbb=size(B,3)
   if(allocated(Bstar))deallocate(Bstar)
   if(allocated(Astar))deallocate(Astar)
   if(allocated(Astari))deallocate(Astari)
   if(allocated(B0star))deallocate(B0star)
   if(allocated(SumA))deallocate(SumA)
   if(allocated(SumB))deallocate(SumB)
   allocate(Bstar(m,n,nbb),Astar(n,n,nba),Astari(n,n),B0star(m,n),SumA(n,n),SumB(m,n),stat=info)
   if (info/=0) then
        if(debug) write(*,*) "mg1pi: info=",info
        info=1000
        return
   endif
   if(allocated(y))deallocate(y)
   if(allocated(x))deallocate(x)
   if(allocated(ipiv))deallocate(ipiv)
   allocate(y(n),ipiv(n),x(m),stat=info)
   if (info/=0) then
        if(debug) write(*,*) "mg1pi: info=",info
        info=1000
        return
   endif
   
10 FORMAT("*")

if(debug) write(*,*) "mg1_pi: dim A =",n,"dim B =",m," blocks A =",nba,"blocks B =",nbb

   ! compute B*, A*
   Bstar(:,:,nbb)=B(:,:,nbb)
   do i=nbb-1,1,-1
      Bstar(:,:,i)=matmul(Bstar(:,:,i+1),G)+B(:,:,i)
   end do
   Astar(:,:,nba)=A(:,:,nba)
   do i=nba-1,2,-1
      Astar(:,:,i)=matmul(Astar(:,:,i+1),G)+A(:,:,i)
   end do
   
   Astari=-Astar(:,:,2)
   do i=1,n
      Astari(i,i)=Astari(i,i)+1.d0
   end do
   call la_getrf(Astari,ipiv)
   call la_getri(Astari,ipiv,info)
  if (info/=0) then
        if(debug) write(*,*) "mg1_pi:la_gestri info=",info
        info=2000
        return
  endif

   ! compute B_0^*
   if(.not. present(Bm1)) then
      B0star=B0+matmul(Bstar(:,:,1),G)
   else
      B0star=B0+matmul(matmul(Bstar(:,:,1),Astari),Bm1)
   endif
   ! compute pi0
   call gth(B0star,pi0)
   
! compute SumA=(I-sum{i>=0}A_i^*)^{-1} and SumB=sum_{i>=1}B_i^*
   SumA=sum(Astar(:,:,2:nba),dim=3)
   SumB=sum(Bstar(:,:,:),dim=3)
   SumA=-SumA
   do i=1,n
      SumA(i,i)=1.d0+SumA(i,i)
   end do
   call la_getrf(SumA,ipiv)
   call la_getri(SumA,ipiv,info)
   if (info/=0) then
        if(debug) write(*,*) "mg1_pi:la_gestri info=",info
        info=2000
        return
    endif

! compute the normalization factor and normalize pi0
! alpha=1/(1+pi0 sum_{i>=1}B_i^* (I-sum_{i>=0} A_i^*)^{-1} e)

! Verificare alfa
  y=sum(SumA,dim=2)
  x=matmul(SumB,y)
  alpha=1.d0/(1.d0+dot_product(pi0,x))
  pi0=pi0*alpha

! Ramaswami formula
! transpose B^* and A^*   
!   do i=1,nbb
!      Bstar(:,:,i)=transpose(Bstar(:,:,i))
!   end do
!   do i=2,nba
!      Astar(:,:,i)=transpose(Astar(:,:,i))
!   end do
!   astari=transpose(astari)

   sumpi=sum(pi0)
   count=0
   if(verb)then
      write(wout,*)"--component=",count,"accumulated mass=",sumpi
      call print_it
   else
      write(wout,10)
        if((count /= 0) .and. (mod(count,100)==0)) then
               call print_it
        else
               call print_it_nolf
        end if
   end if
   do while (sumpi<1.d0-epspi .and. count <maxnc)
      count=count+1
      y=0.d0
      if(count<=nbb) y=matmul(pi0,Bstar(:,:,count))   !matmul(Bstar(:,:,count),pi0)
      do i=max(1,count-nba+2),count-1
! if(debug) write(*,*)"count-i+2=",count-i+2, "nba=",nba,"count=",count,"i=",i
!         y=y+matmul(Astar(:,:,count-i+2),pi(:,i))
         y=y+matmul(Pi(:,i),Astar(:,:,count-i+2))
      end do
!      pi(:,count)=matmul(astari,y)
      pi(:,count)=matmul(y,astari)
      sumpi=sumpi+sum(pi(:,count))
      if(verb)then
         write(wout,*)"--component=",count,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
            if((count /= 0) .and. (mod(count,100)==0)) then
               call print_it
            else
               call print_it_nolf
            end if
      end if
   end do
              if(verb) then
             else
             write(wout,*)" "
             call print_it
             end if

   pinc=count
   if(allocated(Bstar))deallocate(Bstar)
   if(allocated(Astar))deallocate(Astar)
   if(allocated(Astari))deallocate(Astari)
   if(allocated(B0star))deallocate(B0star)
   if(allocated(SumA))deallocate(SumA)
   if(allocated(SumB))deallocate(SumB)
   if(allocated(y))deallocate(y)
   if(allocated(x))deallocate(x)
   if(allocated(ipiv))deallocate(ipiv)
if(debug)  write(*,*) "mg1_pi: exiting with pinc=",pinc
end subroutine mg1pi


subroutine gm1pi(A, B, B0, Bm1, R, maxnc, epspi, pi0, pi, pinc)
   use smc_tools
   USE f95_lapack
   use smc_int, only:gth
   implicit none
   
   integer :: maxnc, pinc
   real(dp),dimension(:,:,:) :: A, B
   real(dp),dimension(:,:), optional :: Bm1
   real(dp),dimension(:,:)::pi,R,B0
   real(dp),dimension(:):: pi0
   real(dp):: epspi
   
   real(dp)::sumpi, alpha
   real(dp),dimension(:,:),allocatable::B0star,auxn,auxm
   real(dp),dimension(:),allocatable::x,y
   integer,dimension(:),allocatable::ipiv
   integer :: n,nbb, nba, i,count,m

10 FORMAT("*")
   n=size(A,1)
   nba=size(A,3)
   m=size(B,1)
   nbb=size(B,3)
   if(allocated(B0star))deallocate(B0star)
   if(allocated(x))deallocate(x)
   if(allocated(y))deallocate(y)
   if(allocated(ipiv))deallocate(ipiv)
   if(allocated(auxn))deallocate(auxn)
   if(allocated(auxm))deallocate(auxm)
   allocate(B0star(m,n),auxn(n,n),auxm(m,m),x(n),y(n),ipiv(n),stat=info)
   if (info/=0) then
        if(debug) write(*,*) "gm1pi: info=",info
        info=1000
        return
   endif
   b0star=b(:,:,nbb)
   do i=nbb-1,1,-1
      b0star=matmul(r,b0star)+b(:,:,i)
   end do
   auxn=-r
   do i=1,n
      auxn(i,i)=auxn(i,i)+1.d0
   end do
   y=1.d0
   call la_gesv(auxn,y)
   if(.not.present(Bm1))then
      auxn=B0+matmul(R,B0star)
      call gth(auxn,x)
      x=x*(1.d0/dot_product(x,y))
      pi0=x
      sumpi=sum(x)
      if(verb)then
         write(wout,*)"--component=",0,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
          if((count /= 0) .and. (mod(count,100)==0)) then
               call print_it
          else
               call print_it_nolf
          end if
      end if
      count=1
      auxn=transpose(r)
      pi(:,1)=matmul(auxn,pi0)
      sumpi=sumpi+sum(pi(:,1))
      if(verb)then
         write(wout,*)"--component=",count,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
           if((count /= 0) .and. (mod(count,100)==0)) then
               call print_it
           else
               call print_it_nolf
           end if
      end if
      do while (sumpi<1.d0-epspi .and. count <maxnc)
         count=count+1
         pi(:,count)=matmul(auxn,pi(:,count-1))
         sumpi=sumpi+sum(pi(:,count))
         if(verb)then
            write(wout,*)"--component=",count,"accumulated mass=",sumpi
            call print_it
         else
            write(wout,10)
            if((count /= 0) .and. (mod(count,100)==0)) then
               call print_it
           else
               call print_it_nolf
           end if
         end if
      end do
                if(verb) then
             else
             write(wout,*)" "
             call print_it
             end if

   else        
      auxn=A(:,:,nba)
      do i=nba-1,2,-1
         auxn=matmul(r,auxn)+a(:,:,i)
      end do
      auxn=-auxn
      do i=1,n
         auxn(i,i)=auxn(i,i)+1.d0
      end do
      call la_getrf(auxn,ipiv)
      call la_getri(auxn,ipiv)
      auxm=B0+matmul(matmul(Bm1,Auxn),B0star)
      call gth(auxm,pi0)
      pi(:,1)=matmul(matmul(transpose(auxn),transpose(Bm1)),pi0)
      alpha=sum(pi0)+dot_product(pi(:,1),y)
      alpha=1.d0/alpha
      pi0=pi0*alpha
      pi(:,1)=pi(:,1)*alpha
      sumpi=sum(pi0)+sum(pi(:,1))
      count=1
      if(verb)then
         write(wout,*)"--component=",count,"accumulated mass=",sumpi
         call print_it
      else
         write(wout,10)
           if((count /= 0) .and. (mod(count,100)==0)) then
               call print_it
           else
               call print_it_nolf
           end if
      end if
      auxn=transpose(r)
      do while (sumpi<1.d0-epspi .and. count <maxnc)
         count=count+1
         pi(:,count)=matmul(auxn,pi(:,count-1))
         sumpi=sumpi+sum(pi(:,count))
         if(verb)then
            write(wout,*)"--component=",count,"accumulated mass=",sumpi
            call print_it
         else
            write(wout,10)
            if((count /= 0) .and. (mod(count,100)==0)) then
               call print_it
            else
               call print_it_nolf
            end if
         end if
      end do
                if(verb) then
             else
             write(wout,*)" "
             call print_it
             end if

   end if
   pinc=count
   if(allocated(B0star))deallocate(B0star)
   if(allocated(x))deallocate(x)
   if(allocated(y))deallocate(y)
   if(allocated(ipiv))deallocate(ipiv)
   if(allocated(auxn))deallocate(auxn)
   if(allocated(auxm))deallocate(auxm)
 end subroutine gm1pi
 
