!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!  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 is(A, method, eps, maxiter, G, drift, err)
  use smc_tools
  use smc_int
  use fi_int, only: gresidual
  use is_int, only: drft_mg1
  USE f95_lapack
!use la_precision
use f77_lapack
  use ponte_f_f, only: debug,verb,timing,finish,wout,fdrift
  implicit none
  REAL(dp), DIMENSION(:,:,:):: a
  REAL(dp), DIMENSION(:,:) :: g
  REAL(dp) :: eps,err,drift
  integer :: method, maxiter
  integer :: nverbose,n,nb, lwork,nm

  integer ::i,j,iter,sdim
  INTEGER, ALLOCATABLE,  DIMENSION(:) :: ipiv,ipivf
  REAL(dp), DIMENSION(:),allocatable :: w,x0,y0,x,work,xi
  !complex(dp), DIMENSION(:),allocatable :: xi
  REAL(dp), DIMENSION(:,:),allocatable :: h,s,h0,h1,ftmp,f ,maux
!  integer, DIMENSION(:,:),allocatable ::maux

  REAL(dp)::alpha,gamma,check,binomial
  logical,external ::  sel
 10 FORMAT("*")
 nverbose=1

if(debug) write(*,*) "SUBROUTINE is: beginning"
   n=size(a,1)
   nb=size(a,3)
   if (allocated(F)) deallocate (F)
   if (allocated(H)) deallocate (H)
   if (allocated(maux)) deallocate (maux)
   if (allocated(s)) deallocate (s)
   if (allocated(ipiv)) deallocate (ipiv)
   if (allocated(ipivf)) deallocate (ipivf)
   if (allocated(x0)) deallocate (x0)
   if (allocated(y0)) deallocate (y0)
  if (allocated(h0)) deallocate (h0)
  if (allocated(h1)) deallocate (h1)
   if (allocated(x)) deallocate (x)
   if (allocated(xi)) deallocate (xi)
   if (allocated(ftmp)) deallocate (ftmp)
   if (allocated(w)) deallocate (w)
   if (allocated(work)) deallocate (work)
nm=n*(nb-1) 
  lwork=3*nm+1

  allocate(F(n*(nb-1),n*(nb-1)),H(n,n*nb),maux(0:nb-1,0:nb-1),s(n,n),&
        ipiv(n),ipivf(n*nb-n),x0(n),y0(n),x(n*(nb-1)),xi(n*(nb-1)),h0(n,n),&
        h1(n,n), Ftmp(n*(nb-1),n*(nb-1)),w(n),work(lwork),stat=info)
   if (info/=0) return

! precompute coefficients
   do i=0,nb-1
      maux(nb-1,i)=binomial(nb-1,i)
      maux(i,0)=1.d0
   end do
   do i=nb-2,0,-1
      do j=1,nb-1
         maux(i,j)=maux(i+1,j)-maux(i+1,j-1)-maux(i,j-1)
      end do
   end do


! compute matrix coefficients 
   do j=0,nb-1
      s=0.d0
      do i=0,nb-1
         s=s+a(:,:,i+1)*maux(i,j)
      end do
      s=-s
      do i=1,n
         s(i,i)=s(i,i)+maux(1,j)
      end do
      h(:,j*n+1:(j+1)*n)=s
   end do

   call la_getrf(s,ipiv)
   call la_getri(s,ipiv)
   do j=0,nb-2
      h(:,j*n+1:(j+1)*n)=matmul(s, h(:,j*n+1:(j+1)*n))
   end do
! construct F
   F=0.d0
   do i=1,n*(nb-2)
      f(i,n+i)=1.d0
   end do
   do i=1,nb-1
      f((nb-2)*n+1:(nb-1)*n,(i-1)*n+1:i*n)=-h(:,(i-1)*n+1:i*n)
   end do


! rank one correction
   y0=1.d0 !!!!!!!!!!!!!!
   x0=0
   x0(n)=1.d0
   s=transpose(h(:,1:n))
   s(n,:)=1.d0
   call la_gesv(s,x0)
   drift= drft_mg1(a)
   fdrift=drift
   gamma=sign(1.d0,drift)/dot_product(x0,matmul(h(:,n+1:2*n),y0))
   y0=y0*(1.d0*gamma) !!!!!!!!!!!
   do i=1,nb-2
      w=matmul(transpose(h(:,i*n+1:(i+1)*n)),x0)
      do j=1,n
         f(1:n,(i-1)*n+j)=w(j)*y0+ f(1:n,(i-1)*n+j)
      end do
   end do

   do j=1,n
      f(1:n,(nb-2)*n+j)=y0*x0(j)+ f(1:n,(nb-2)*n+j)
   end do

! start iterations
   if(method/=2)then
      do iter=1,maxiter
         ftmp=f
         call la_getrf(f,ipivf)
         if(method==1)then
            alpha=1.d0
            do i=1,n
               alpha=alpha*f(i,i)
            end do
            alpha=abs(alpha)**(1.d0/(n*(nb-1)))
            alpha=1.d0/(1.d0+alpha)
         end if
         call la_getri(f,ipivf)
         if(method==0)then
            ftmp=(f+ftmp)*0.5d0
         else
            ftmp=alpha*ftmp+(1.d0-alpha)*f
         end if
         check=maxval(sum(abs(f-ftmp),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)
                  call print_it_nolf
                  if(mod(iter,100)==0) then
                          call print_it
                          else
                          call print_it_nolf
                          end if
               endif
         
            endif
         end if
         f=ftmp
         if(check<eps)exit
      end do
      if (verb) then
      else
      write(wout,*)" "
      call print_it
      end if
      if (iter>=maxiter) then
         if (verb) then
         write(wout,*)"Reached max number of iterations in IS"
         call print_it
         else
         write(wout,*)" "
         call print_it
         write(wout,*)"Reached max number of iterations in IS"
         call print_it
         endif
      endif

      do i=1,n*(nb-1)
         f(i,i)=f(i,i)-1.d0
      end do
      nm=n*(nb-1)
      ftmp=f

      goto 1000
! the interfaces of the following calls have bugs
!   M, N, A, LDA, JPVT, TAU, WORK, INFO
      call la_GEQP3( nm, nm, ftmp, nm, ipivf, x, WORK, LWORK, INFO )
      if(info/=0)then
        write(*,*)"error in la_geqp3 called by is"
         write(wout,*)"error in la_geqp3 called by is"
         return
      endif
     call la_ORGQR( nm, nm, nm, ftmp, nm, x, WORK, LWORK, INFO )
      if(info/=0)then
         write(wout,*)"error in la_geqp3 called by is"
         return
      endif
 
 1000 continue

      call la_gesvd(f,x,ftmp)
   else
!   schur decomposition
      ftmp=f
      call la_gees(f,x,xi,ftmp,sel,sdim,info)
     if(info/=0)then
         write(wout,*)"error in la_gees called by is"
         call print_it
         return
      end if
   end if

! compute G
  h1=ftmp(1:n,1:n)
  s=ftmp(n+1:n+n,1:n)
  h0=h1-s
  CALL la_getrf(h0,ipiv)
  CALL la_getri(h0,ipiv,info)
     if(info/=0)then
         write(wout,*)"error in la_getri called by is"
         call print_it
         return
      end if
  g=matmul(h1+s,h0)
  call gresidual(a,g,err)
 ! write(wout,*)"G-residual=",err
 ! call print_it
 ! write(wout,*)"drift=",drift
 ! call print_it
end SUBROUTINE is

function drft_mg1(a)
   use smc_tools
  use smc_int, only: gth

  implicit none
  real(dp),dimension(:,:,:)::a
  real(dp)::drft_mg1
  !
  real(dp),dimension(:,:),allocatable::s
  real(dp),dimension(:),allocatable:: w,p
  integer:: i,m,n
   m=size(a,1)
  n=size(a,3)


  if (allocated(s)) deallocate(s)
  if (allocated(p)) deallocate(p)
    if (allocated(w)) deallocate(w)
    allocate(s(m,m),p(m),w(m),stat=info)
  if (info/=0) return
  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)
  drft_mg1=dot_product(p,w)-1.d0
end function drft_mg1


logical function sel(xr,xi)
use smc_tools
implicit none
real(dp)::xr,xi
if(xr<0) then
   sel=.true.
else
   sel=.false.
end if
end function sel


!function binomial(n,k)
!  implicit none
!  integer ::  n,k , i
!  real(kind(0.d0)):: rn,rk,rb,binomial
!  rb=1
!  rn=n
!  rk=k
!  do i=0,k-1
!     rb=((rn-i)/(rk-i))*rb
!  end do
!  binomial=rb
!end function binomial
