program Multigrid 

  real, dimension(:,:), allocatable :: Ah,A2h
  real, dimension(:), allocatable :: x,xold,xh, rb,dh,d2h,eh,e2h,e2hold
  real :: u0, u1, rstep, precision, fac, pi
  integer :: idt1, ils1, isol, npoin, ndeg, npoin2, niter, niter2 

  pi = 2 * asin(1.)

  
  idt1 = 11

  ils1 = 12

  isol = 13

  open(idt1,file='mg.data',status='OLD',blank='ZERO')
  open(ils1,file='res.out',status='UNKNOWN')
  open(isol,file='sol.gnu',status='UNKNOWN')

  read(idt1, * ) ndeg
  read(idt1, * ) niter
  read(idt1, * ) niter2
  read(idt1, * ) ncycle
  read(idt1, * ) list
  read(idt1, * ) precision

  npoin = ndeg-1

  npoin2 = (npoin-1)/2 

  allocate (Ah(npoin,npoin),A2h(npoin2,npoin2),x(npoin) )
  allocate( xold(npoin),rb(npoin),dh(npoin),d2h(npoin2), xh(1:npoin) )
  allocate( eh(npoin),e2h(npoin2),e2hold(npoin2)  )
  

  !     -u'' = f (x)
  !     arising of matrix Ah

  !     boundary conditions
  u0 = 0.
  u1 = 0.           
  rstep = 1.0/ndeg
      

  print*,'npoin=',npoin,'    h=',rstep,'  niter=',niter,'/',niter2 
  print*,'precision=',precision

  write(ils1,*)'npoin=',npoin,'    h=',rstep,'  niter=',niter,'/',niter2 
  write(ils1,*)'precision=',precision


  ! matrixes from the FD discretization
  ! step h
  fac = 1./rstep/rstep
  fac2 = 1./rstep/rstep/4
  
  Ah(:,:)=0.

  do  k=1,npoin-1
     Ah(k,k)   =  2.0 * fac
     Ah(k+1,k) = -1.0 * fac
     Ah(k,k+1) = -1.0 * fac
  end do
  
  Ah(npoin,npoin) = 2.0 * fac

  ! step 2h
  A2h(:,:)=0.
  do  k=1,npoin2-1
     A2h(k,k)   =  2.0 * fac2
     A2h(k+1,k) = -1.0 * fac2
     A2h(k,k+1) = -1.0 * fac2
  end do
  A2h(npoin2,npoin2) = 2.0* fac2
      

  ! setting of RHS
  do  k=1,npoin
     xx = 1.0*k/ndeg
     rb(k) = f(xx, 1)
  end do

  ! terms from the left-hand side
  rb(1) = rb(1) + u0 * fac   
  rb(npoin) = rb(npoin)+u1 * fac


  !do k=1,npoin
  !    write(*,'(a8, i5,es12.4,a1, 300es12.4)' )' A:',k, rb(k), '|', Ah(k, :)
  ! enddo
  ! print*
  ! do k=1,npoin2
  !    write(*,'(a8, i5, 300es12.4)' )' A2:',k, A2h(k, :)
  ! enddo
  ! stop "d3y38h3"

  
  ! setting of the initial approximation, linear interpolation of the BC
  do  k=1,npoin
     !x(k) = u0 + (u1-u0)*k/ndeg
     x(k) = u0 + (u1-u0)*k/ndeg + sin( 20 * pi * ( 1. * k/ndeg) )
  enddo

  ! direct solver == approximate solution with elimination of the algebraic errors
  call DIRECT_SOLVE_TRIDIAG( npoin, Ah(1:npoin, 1:npoin), rb(1:npoin), xh(1:npoin) )
  call WriteSol(10 , 0, npoin, xh, xh, u0, u1)
  

  ! only damped Jacobi
  icit = 0
  do k=1, 6
     if(k == 1) call WriteSol(100 , 0, npoin, x, xh, u0, u1)
  
     call JACOBI_TL(ils1, npoin, 1000, icit, &
          Ah(1:npoin, 1:npoin), rb(1:npoin), x(1:npoin), &
          rstep,list,iprec, precision)
  
     call WriteSol(100 + k, 0, npoin, x, xh, u0, u1)
  enddo
  
  !stop "3d333"

  ! setting of the initial approximation, linear interpolation of the BC
  do  k=1,npoin
     x(k) = u0 + (u1-u0)*k/ndeg
     !x(k) = u0 + (u1-u0)*k/ndeg + sin( 20 * pi * ( 1. * k/ndeg) )
  enddo

  ! TWO-GRID MULTIGRID
  icit = 0
  do incy=1,ncycle

     ! pre smoothing
     if( incy == 1) then
        iprec = 0
        call JACOBI_TL(ils1, npoin, niter, icit, &
             Ah(1:npoin, 1:npoin), rb(1:npoin), x(1:npoin), &
             rstep,list,iprec, precision)
        if ( iprec .eq. 1) goto 900
     endif
     
     !      computing of reziduum
     if((niter2 .eq. 0) .or. (incy .eq. ncycle))goto 900
     call REZIDUUM(npoin,Ah,x,rb,dh)
     call RESTRICTION(npoin, npoin2, dh(1:npoin), d2h(1:npoin2))
     !call RESTRICTION(npoin, npoin2, eh(1:npoin), e2h(1:npoin2))



     ! solution of the coarser grid: damped Jacobi method
     iprec = -1
     e2h(:) = 0.
     call JACOBI_TL(ils1, npoin2, niter2, icit, &
          A2h(1:npoin2,1:npoin2), d2h(1:npoin2), e2h(1:npoin2), &
          2*rstep, list, iprec, precision)

     ! solution of the coarser grid: direct solved
     !call DIRECT_SOLVE_TRIDIAG(npoin2, A2h(1:npoin2,1:npoin2), d2h(1:npoin2), e2h(1:npoin2))

     
     call PROLONGATION(npoin, npoin2, u0, u1, e2h(1:npoin2), eh(1:npoin))
 
     ! correction of the solution using eh
     x(1:npoin) = x(1:npoin)-eh(1:npoin)

     ! post-smoothing
     iprec = 0
     call JACOBI_TL(ils1, npoin, niter, icit, &
          Ah(1:npoin, 1:npoin), rb(1:npoin), x(1:npoin), &
          rstep,list,iprec, precision)

     call WriteSol(200+incy , incy,  npoin, x, xh, u0, u1)
     
  end do  !do incy = 1,ncycle
 
 900  continue

  ! final solution
  call WriteSol(isol, 99, npoin, x, xh, u0, u1)
  
end program Multigrid


subroutine WriteSol(isol, icyc, npoin, x, xh, u0, u1)
  integer, intent(in) :: isol  ! index of the output file
  integer, intent(in) :: npoin ! size of the problem
  integer, intent(in) :: icyc  !index of cycle
  real, dimension(1:npoin), intent(in) :: x    ! actual approximate solution
  real, dimension(1:npoin), intent(in) :: xh   ! accurate approximate solution
  real, intent(in) :: u0, u1  ! BC
  integer :: i, ndeg
  real :: xx, fx,  sumx 
  
  ndeg = npoin + 1

  sumx = 0.
  
  fx = f(0., 2)
  !write(isol, *) 0., u0, fx, u0 - fx, 1E-20, 1E-20

  do i=1, npoin
     xx = 1.0*i/ndeg
     write(isol, *) xx, x(i), f(xx, 2), x(i) - f(xx, 2), abs(x(i) - xh(i)), abs( x(i) - f(xx, 2))
     sumx = sumx + ( x(i) - f(xx, 2))**2
  enddo

  fx = f(1., 2)
  !write(isol, *) 1., u1, fx, u1 - fx, 1E-20, 1E-20

  sumx = sqrt( sumx)

  print*
  write(*,'(a8,2i5, a5, 300es12.4)') 'error', icyc, npoin, '   ', sumx
  print*,'----------------------------------'
  
end subroutine WriteSol


subroutine DIRECT_SOLVE_TRIDIAG( npoin, ra, rb, x)
  integer, intent(in) :: npoin ! size of the problem
  real, dimension(1:npoin, 1:npoin), intent(in) :: ra
  real, dimension(1:npoin), intent(in) :: rb
  real, dimension(1:npoin), intent(inout) :: x
  real, dimension(:), allocatable :: eta, xi, a, b, c
  integer :: i

  allocate( a(1:npoin), b(1:npoin), c(1:npoin), eta(1:npoin),xi(1:npoin) )

  ! assembling
  do i=1,npoin-1
     a(i) = ra(i,i)
     c(i) = ra(i, i+1)
     b(i) = ra(i+1, i)
  end do
  a(npoin) = ra(npoin, npoin)
     
  eta(1)= a(1)
  xi(1) = rb(1)

  do i = 2, npoin
     eta(i) = a(i)  - b(i-1)  / eta(i-1) * c(i-1) 
     xi(i)  = rb(i) - b( i-1) / eta(i-1) * xi(i-1) 
  end do

  x(npoin) = xi(npoin) / eta(npoin)
  do i = npoin-1, 1, -1

     x(i) =  ( xi(i) - c(i) * x(i+1) ) /eta(i) 

  end do

  deallocate(eta, xi, a, b, c)
  
end subroutine DIRECT_SOLVE_TRIDIAG
  

subroutine JACOBI_TL(ils1, npoin, niter, icit, ra, rb, x, &
     rstep, list, iprec, precision)
  integer, intent(in) :: ils1  ! index of the output file
  integer, intent(in) :: npoin ! size of the problem
  integer, intent(in) :: niter ! maximal number of iterations
  integer, intent(inout) :: icit ! total number of steps
  real, intent(in) :: rstep ! size of the step
  integer, intent(in) :: list ! output aftep "list" iters
  integer, intent(inout) :: iprec ! number of converged solvers
  real, intent(in) :: precision !  prescribed accuracy
  real, dimension(1:npoin, 1:npoin), intent(in) :: ra
  real, dimension(1:npoin), intent(in) :: rb
  real, dimension(1:npoin), intent(inout) :: x
  real, dimension(:), allocatable :: xold
  integer :: k, l
  real :: sum, err, erre, fac

  allocate(xold (1:npoin) )
  
  fac = rstep*rstep/4

  do iter=1,niter
     icit = icit + 1
     
     xold(1:npoin)=x(1:npoin)
               

     !do  k=1,npoin
     !   sum = dot_product( ra(k, 1:npoin), xold(1:npoin) )
     !   x(k) = xold(k) + rstep*rstep/4 * ( rb(k) - sum )
     !end do

     x(1:npoin) = xold(1:npoin) &
          + fac * ( rb(1:npoin) - matmul( ra(1:npoin, 1:npoin), xold(1:npoin) ) )

     !write(*,'(a8,i5, 300es12.4)') 'x_i', iter, x(1:npoin)
     
     err = norm2( x(:) - xold(:) ) / ( max(norm2( x(:) ) ,1E-15) )

     if (list*int((iter-1)/list).eq.(iter-1) .or. iter == 1 .or. iter == niter) then
        write(*,'(a8,i8, 2i5, 300es12.4)') 'estim', icit, npoin, iter, err !, norm2( x(:) - xold(:) ) 
     endif
     !write(99,'(a8,i5, 300es12.4)') '   ', iter, err !, norm2( x(:) - xold(:) ) 
     
     !if (list*int((iter-1)/list).eq.(iter-1)) then
     !   write(*,'(a15, 2i8, es12.4)') 'iter(s), error', icit,iter, err
     !   write(ils1,*)icit,'   ',iter,'   ',err
     !endif

     ! iprec == 0 only on the fine grid
     if( iprec >= 0 .and.  err .le. precision) then
        iprec = 1
        return
     endif

  end do ! iter = 1, niter

  deallocate(xold)

end subroutine JACOBI_TL


subroutine GAUSS_SEIDEL_TL(ils1, npoin, niter, icit, ra, rb, x, &
     rstep, list, iprec, precision)
  integer, intent(in) :: ils1  ! index of the output file
  integer, intent(in) :: npoin ! size of the problem
  integer, intent(in) :: niter ! maximal number of iterations
  integer, intent(inout) :: icit ! total number of steps
  real, intent(in) :: rstep ! size of the step
  integer, intent(in) :: list ! output aftep "list" iters
  integer, intent(inout) :: iprec ! number of converged solvers
  real, intent(in) :: precision !  prescribed accuracy
  real, dimension(1:npoin, 1:npoin), intent(in) :: ra
  real, dimension(1:npoin), intent(in) :: rb
  real, dimension(1:npoin), intent(inout) :: x
  real, dimension(:), allocatable :: xold
  integer :: k, l, j
  real :: sum, err, erre, fac

  allocate(xold (1:npoin) )
  
  fac = rstep*rstep/4

  do iter=1,niter
     icit = icit + 1
     
     xold(1:npoin)=x(1:npoin)
               

     !do  k=1,npoin
     !   sum = dot_product( ra(k, 1:npoin), xold(1:npoin) )
     !   x(k) = xold(k) + rstep*rstep/4 * ( rb(k) - sum )
     !end do

     do j=1,npoin
        !x(j) = xold(j) + fac * ( rb(j) - dot_product( ra(j, 1:npoin), xold(1:npoin) ) )
        x(j) = x(j) + fac * ( rb(j) - dot_product( ra(j, 1:npoin), x(1:npoin) ) )
     enddo
     
     !write(*,'(a8,i5, 300es12.4)') 'x_i', iter, x(1:npoin)
     
     err = norm2( x(:) - xold(:) ) / ( max(norm2( x(:) ) ,1E-15) )

     !if (list*int((iter-1)/list).eq.(iter-1)) then
        write(*,'(a8,3i5, 300es12.4)') 'estim', icit, npoin, iter, err !, norm2( x(:) - xold(:) ) 
     !endif
     !write(99,'(a8,i5, 300es12.4)') '   ', iter, err !, norm2( x(:) - xold(:) ) 
     
     !if (list*int((iter-1)/list).eq.(iter-1)) then
     !   write(*,'(a15, 2i8, es12.4)') 'iter(s), error', icit,iter, err
     !   write(ils1,*)icit,'   ',iter,'   ',err
     !endif

     ! iprec == 0 only on the fine grid
     if( iprec >= 0 .and.  err .le. precision) then
        iprec = 1
        return
     endif

  end do ! iter = 1, niter

  deallocate(xold)

end subroutine GAUSS_SEIDEL_TL


subroutine RESTRICTION(npoin,npoin2,dh,d2h)
  integer, intent(in) :: npoin, npoin2
  real, dimension(1:npoin), intent(in) :: dh      ! vector to be restricted
  real, dimension(1:npoin2), intent(out) :: d2h   ! restrcited vector 
    
  do  k=1,npoin2
     d2h(k)= (dh(2*k-1) + 2.*dh(2*k) + dh(2*k+1))/4
     !d2h(k)= dh(2*k)
  end do

end subroutine RESTRICTION


subroutine PROLONGATION(npoin, npoin2, u0, u1, d2h, dh)
  integer, intent(in) :: npoin, npoin2
  real, dimension(1:npoin2), intent(in) :: d2h   ! vector to be prolongated
  real, dimension(1:npoin), intent(out) :: dh    ! prolongated vector
  real, intent(in) :: u0, u1
  integer :: k

  !    using boundary condition
  dh(1) = (u0+d2h(1))/2
  dh(npoin) = (u1+d2h(npoin2))/2  

  do k=2,npoin-1
     if(2*int(k/2) .eq. k)then
        dh(k) = d2h(k/2)
     else
        dh(k) = (d2h((k+1)/2)+d2h((k-1)/2))/2
     endif
  end do

end subroutine PROLONGATION


subroutine REZIDUUM(npoin, Ah, x, b, dh)
  integer, intent(in) :: npoin
  real, dimension(1:npoin,1:npoin), intent(in) :: Ah 
  real,dimension(1:npoin), intent(in) :: b, x 
  real,dimension(1:npoin), intent(inout) :: dh

  dh(1:npoin) = matmul( Ah(1:npoin, 1:npoin), x(1:npoin) ) - b(1:npoin) 

end subroutine REZIDUUM
   
! RHS function (ityp == 1) and the exact solution (ityp == 2)
function f(x, ityp)
  real :: f
  real, intent(in) :: x
  integer, intent(in) :: ityp
  real :: pi
  
  pi = 2 * asin(1.)

  if(ityp == 1) then
     f = 2.
     !f = x
     !f = 4 * pi*pi * sin( 2* pi *x) 
  elseif(ityp == 2) then
     f = x * (1-x)
     !f = -x**3 / 6. + x / 6
     !f = sin( 2* pi *x) 
  else
     stop "bad ityp"
  endif
  
end function f

