!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!  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.2 - Nov 2006                                     !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
!
!
SUBROUTINE fi(A, doshift, method, eps, maxit, x0, G, drift, err)
  use smc_tools
  use smc_int
  use fi_int, only: fi_n, fi_t, fi_u, gresidual
  use is_int, only: drft_mg1
  use ponte_f_f, only: debug,verb,wout,fdrift
  implicit none
  REAL(dp), DIMENSION(:,:,:):: a
  REAL(dp), DIMENSION(:,:) :: g, x0
  REAL(dp) :: eps,err,drift
  integer :: method, maxit
  logical :: doshift
  integer :: nverbose,n,nb
REAL(dp), DIMENSION(:,:,:),allocatable :: a1
  nverbose=10

   n=size(a,1)
   nb=size(a,3)
   if (allocated(a1)) deallocate (a1)
   allocate(a1(n,n,nb))


! method 1: natural
!        2: traditional
!        3: u-based

   a1=a
   If(doshift) then
      call shift(a1,g,drift)
   else
      drift= drft_mg1(a)
   end If
   fdrift=drift
  if(method==1)then
     call fi_n(a1,x0,eps,maxit,g,nverbose)
     elseif(method==2)then
     call fi_t(a1,x0,eps,maxit,g,nverbose)
     elseif(method==3)then
     call fi_u(a1,x0,eps,maxit,g,nverbose)
  end if
  if(doshift)then
     if(drift<=0) g=g+1.0d0/n
  end if
  call gresidual(a,g,err)
end SUBROUTINE fi
! natural fixed point iteration
SUBROUTINE fi_n(a,x0,eps,maxit,g,nverbose)
  use smc_tools
  use ponte_f_f, only: debug,verb,wout
  IMPLICIT NONE
  REAL(dp), DIMENSION(:,:,:):: a
  REAL(dp), DIMENSION(:,:) :: g, x0
  REAL(dp) :: eps,check
  integer :: n,na,nverbose,maxit,j,iter
  REAL(dp), DIMENSION(:,:),allocatable    :: y
10 FORMAT("*")
  n=size(g,1)
  na=size(a,3)
  if (allocated(y)) deallocate(y)
  ALLOCATE(y(n,n),stat=info)
  if(info/=0)return
  y=x0;
  do iter=1,maxit
     g=a(:,:,na)
     do j=na-1,1,-1
        g = matmul(g,y)+ a(:,:,j)
     end do
     check=maxval(sum(abs(g-y),dim=2))
     if(nverbose>0)then
        if(mod(iter,nverbose)==0) then 
             if (verb) then
             write(wout,*)"iter=",iter,"check=",check
             call print_it  
              else
             write(wout,10)
             if((iter /= 0) .and. (mod(iter,100)==0)) then
               call print_it
             else
               call print_it_nolf
              end if
             endif
        endif
     end if
     if(check<eps)exit
     y=g
  end do
  if (verb) then
  else
   write(wout,*)" "
   call print_it
  endif
  if (iter>=maxit) then
            write(wout,*)"Reached max number of iterations in FI"
            call print_it
            endif
end SUBROUTINE fi_n

! traditional fixed point iteration
SUBROUTINE fi_t(a,x0,eps,maxit,g,nverbose)
  use smc_tools
  USE f95_lapack
  use ponte_f_f, only: debug,verb,wout
  IMPLICIT NONE
  REAL(dp), DIMENSION(:,:,:):: a
  REAL(dp), DIMENSION(:,:) :: g, x0
  REAL(dp) :: eps,check,res
  integer :: n,na,nverbose,maxit,j,iter
  integer,dimension(:),allocatable::ipiv
  REAL(dp), DIMENSION(:,:),allocatable    :: y,f
10 FORMAT("*")
  n=size(g,1)
  na=size(a,3)
  if (allocated(y)) deallocate(y)
  ALLOCATE(y(n,n),stat=info)
  if(info/=0)return
  if (allocated(f)) deallocate(f)
  ALLOCATE(f(n,n),stat=info)
  if(info/=0)return
  if (allocated(ipiv)) deallocate(ipiv)
  ALLOCATE(ipiv(n),stat=info)
  if(info/=0)return

  f=-a(:,:,2)
  do j=1,n
     f(j,j)=f(j,j)+1.d0
  end do
  call la_getrf(f,ipiv)
  call la_getri(f,ipiv)
  y=x0;
  do iter=1,maxit
     g=a(:,:,na)
     do j=na-1,3,-1
        g = matmul(g,y)+ a(:,:,j)
     end do
     g=matmul(g,y)
     g=matmul(g,y)+a(:,:,1)
     g=matmul(f,g)
     check=maxval(sum(abs(g-y),dim=2))
     if(nverbose>0)then
        if(mod(iter,nverbose)==0)then
           if (verb) then
           write(wout,*)"iter=",iter,"check=",check
           call print_it
           else
           write(wout,10)
             if((iter /= 0) .and. (mod(iter,100)==0)) then
               call print_it
             else
               call print_it_nolf
              end if
           endif
        end if
     end if
     if(check<eps)exit
     y=g
  end do
  if (verb) then
  else
  write(wout,*)" "
  call print_it
  end if
  if (iter>=maxit) then
         write(wout,*)"Reached max number of iterations in FI"
         call print_it
         endif
end SUBROUTINE fi_t


! U-based fixed point iteration
SUBROUTINE fi_u(a,x0,eps,maxit,g,nverbose)
  use smc_tools
  USE f95_lapack
  use ponte_f_f, only: debug,verb,wout
  IMPLICIT NONE
  REAL(dp), DIMENSION(:,:,:):: a
  REAL(dp), DIMENSION(:,:) :: g, x0
  REAL(dp) :: eps,check,res
  integer :: n,na,nverbose,maxit,j,iter,i
  REAL(dp), DIMENSION(:,:),allocatable    :: y,f
10 FORMAT("*")
  n=size(g,1)
  na=size(a,3)
  if (allocated(y)) deallocate(y)
  ALLOCATE(y(n,n),stat=info)
  if(info/=0)return
  if (allocated(f)) deallocate(f)
  ALLOCATE(f(n,n),stat=info)
  if(info/=0)return
  y=x0;
  do iter=1,maxit
     f=a(:,:,na)
     do j=na-1,2,-1
        f = matmul(f,y)+ a(:,:,j)
     end do
     f=-f
     do i=1,n
        f(i,i)=f(i,i)+1.d0
     end do
     g=a(:,:,1)
     call la_gesv(f,g)
     check=maxval(sum(abs(g-y),dim=2))
     if(nverbose>0)then
        if(mod(iter,nverbose)==0)then
           if (verb) then
           write(wout,*)"iter=",iter,"check=",check
           call print_it
            else
           write(wout,10)
             if((iter /= 0) .and. (mod(iter,100)==0)) then
               call print_it
             else
               call print_it_nolf
              end if
           endif
        end if
     end if
if(debug) write(*,*)"eps=",eps," check=",check," iter=",iter," maxit=",maxit
     if(check<eps)exit

     y=g
  end do
  if (verb) then
  else
  write(wout,*)" "
  call print_it
  end if
  if (iter>=maxit) then 
           write(wout,*)"Reached max number of iterations in FI"
           call print_it
  endif
end SUBROUTINE fi_u

subroutine gresidual(a,g,res)
  use smc_tools
  IMPLICIT NONE
  REAL(dp), DIMENSION(:,:,:):: a
  REAL(dp), DIMENSION(:,:) :: g
  REAL(dp) :: res
  integer :: na,j,n
  REAL(dp), DIMENSION(:,:),allocatable    :: y
  n=size(g,1)
  na=size(a,3)
  if (allocated(y)) deallocate(y)
  ALLOCATE(y(n,n),stat=info)
  if(info/=0)return
  
   y=a(:,:,na)
   do j=na-1,1,-1
      y = matmul(y,g)+ a(:,:,j)
   end do
   res=maxval(sum(abs(g-y),dim=2))

 end SUBROUTINE gresidual
