module bmandmix

  use bmqr, only: qr,extractQ,extractR,qrupdate,qrsolve
  use bmproblem, only: pdata_type,evalr
  use iso_c_binding, only: c_ptr

  implicit none
  
  public :: andmix

contains

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

  subroutine andmix(n,x,res,epsf,maxit,nhlim,iter,fcnt,istop,pdataptr)

    implicit none
    
    ! SCALAR ARGUMENTS
    integer, intent(in) :: maxit,n,nhlim
    integer, intent(out) :: fcnt,istop,iter
    real(kind=8), intent(in) :: epsf
    type(c_ptr), optional, intent(in) :: pdataptr

    ! ARRAY ARGUMENTS
    real(kind=8), intent(inout) :: x(n)
    real(kind=8), intent(out) :: res(n)

    ! PARAMETERS
    logical, parameter :: tabline = .false.
    integer, parameter :: iprint = 1

    ! LOCAL SCALARS
    integer :: jtbr,ncol,nh,rank
    real(kind=8) :: beta,f
    
    ! LOCAL ARRAYS
    integer :: perm(nhlim-1)
    real(kind=8) :: col(n),resstar(n),xhist(n,nhlim),rhist(n,nhlim), &
         q(n,nhlim-1),r(nhlim-1,nhlim-1),gam(nhlim-1),qrsol(nhlim-1)

    nh = 0
    iter = 0
    fcnt = 0
    
    write(*,*) 'Enter beta: '
    read(*,*) beta

!!$    open(50,file='andmix.out')
    
100 continue

    call evalr(n,x,res,pdataptr)
    fcnt = fcnt + 1

    f = norm2( res(1:n) )

!!$    write(50,9010) iter,fcnt,f
    
    if ( iprint .ge. 0 ) then
     ! write(*,*) 'iter = ',iter,' ||F||_2 = ',f,' x = ',x(1:n),' res = ',res(1:n)
       write(*,*) 'iter = ',iter,' ||F||_2 = ',f
    end if

    if ( tabline ) then
       open(10,file='tabline-interrupted.txt')
       write(10,9000) n,9,f,iter,fcnt
       close(10)
    end if
    
    if ( f .le. epsf ) then
       if ( iprint .ge. 0 ) then
          write(*,*) 'success!'
       end if
       istop = 0
       go to 200
    end if

    if ( iter .ge. maxit ) then
       if ( iprint .ge. 0 ) then
          write(*,*) 'maximum number of iterations reached!'
       end if
       istop = 1
       go to 200
    end if

    iter = iter + 1

    if ( iprint .ge. 1 ) then
       write(*,*) 'Including xk in history.'
    end if
       
    if ( nh + 1 .le. min( nhlim, n + 1 ) ) then
       if ( iprint .ge. 1 ) then
          write(*,*) 'There is space to include xk without removing any other point ', &
               '(nh = ',nh,' min( nhlim, n + 1 ) = ',min( nhlim, n + 1 ),').'
       end if
       
       nh = nh + 1
       jtbr = 0
       
    else
       if ( iprint .ge. 1 ) then
          write(*,*) 'History is full. Oldest point is being removed ', &
               '(nh = ',nh,' min( nhlim, n + 1 ) = ',min( nhlim, n + 1 ),').'
       end if
       
       xhist(1:n,1:nh-1) = xhist(1:n,2:nh)
       rhist(1:n,1:nh-1) = rhist(1:n,2:nh)
       jtbr = 1
    end if

    xhist(1:n,nh) = x(1:n)
    rhist(1:n,nh) = res(1:n)

    resstar(1:n) = res(1:n)
    
    if ( nh .gt. 1 ) then
       col(1:n) = rhist(1:n,nh) - rhist(1:n,nh-1)

       if ( nh .eq. 2 ) then
          ncol = 1
          call qr(n,ncol,n,col,gam,perm,rank)
          call extractQ(n,ncol,n,col,gam,rank,n,Q)
          call extractR(ncol,n,col,rank,nhlim-1,R)
       else
          call qrupdate(n,ncol,rank,nhlim-1,jtbr,n,Q,nhlim-1,R,perm,col)
       end if

       if ( iprint .ge. 1 ) then
          write(*,*) 'Number of columns = ',ncol,' rank = ',rank
       end if

       if ( ncol .ne. nh - 1 ) then
          write(*,*) 'ERROR in subroutine andmix: ncol is not equal to nh - 1! ncol = ',ncol,' nh = ',nh
          stop
       end if
       
       if ( rank .gt. 0 ) then
          call qrsolve(n,ncol,rank,n,Q,nhlim-1,R,perm,res,qrsol)
          resstar(1:n) = resstar(1:n) - matmul( rhist(1:n,2:nh) - rhist(1:n,1:nh-1), qrsol(1:nh-1) )
          x(1:n)       = x(1:n)       - matmul( xhist(1:n,2:nh) - xhist(1:n,1:nh-1), qrsol(1:nh-1) )
       else
          if ( iprint .ge. 1 ) then
             write(*,*) 'Yk has null rank!'
          end if
       end if
    end if

    x(1:n) = x(1:n) - beta * resstar(1:n)
    
    go to 100

200 continue

!!$    close(50)
    
9000 format(1X,I6,1X,I1,1X,1P,D7.1,2(1X,I10))
9010 format(1X,I6,1X,I6,1X,1P,E12.6)

  end subroutine andmix
  
end module bmandmix
