module optvor

  use iso_c_binding, only: c_ptr,c_f_pointer
  use VorCells_Polygons, only: Polygon,VoronoiCell,voronoi_cell,VorCellInterConvPol, &
       VorCellInterConvPol_Collinear,voronoi_cell_destroy,polygon_destroy

  implicit none
  
  type :: pdata_type
     integer, parameter :: meritfunction = 2
     integer :: npols
     real(kind=8) :: c(2),rmax,volA,xmin,xmax,ymin,ymax,drawscale
     integer, allocatable :: nvpols(:),poledges(:,:)
     real(kind=8), allocatable :: xpol(:,:),ypol(:,:)
  end type pdata_type
  
  public :: evalfg,proj,approxg,drawsol,InA,compscale,compvols

contains

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

  logical function InA(p,pdataptr)
    
    implicit none

    ! SCALAR ARGUMENTS
    type(c_ptr), optional, intent(in) :: pdataptr
    
    ! ARRAY ARGUMENTS
    real(kind=8), intent(in) :: p(2)

    ! LOCAL SCALARS
    integer :: allocerr,j,maxnv
    type(pdata_type), pointer :: pdata

    ! LOCAL ARRAYS
    real(kind=8), allocatable :: vertices(:,:)
    
    call c_f_pointer(pdataptr,pdata)
      
    maxnv = maxval( pdata%nvpols(1:pdata%npols) )
      
    allocate(vertices(2,maxnv),stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'InA: Allocation error.'
       stop
    end if

    j = 1
    InA = .false.
    do while ( j .le. pdata%npols .and. .not. InA )
       vertices(1,1:pdata%nvpols(j)-1) = pdata%xpol(j,1:pdata%nvpols(j)-1)
       vertices(2,1:pdata%nvpols(j)-1) = pdata%ypol(j,1:pdata%nvpols(j)-1)
       call polygon_contains_point_2d_convex(pdata%nvpols(j)-1,vertices,p,inA)
       j = j + 1
    end do
        
    deallocate(vertices,stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'InA: Deallocation error.'
       stop
    end if
    
  end function InA

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

  subroutine evalfg(n,x,f,g,flag,pdataptr)
	
    implicit none
  
    ! SCALAR ARGUMENTS
    integer, intent(in) :: n
    integer, intent(out) :: flag
    real(kind=8), intent(out) :: f
    type(c_ptr), optional, intent(in) :: pdataptr
	  
    ! ARRAY ARGUMENTS
    real(kind=8), intent(in) :: x(n)
    real(kind=8), intent(out) :: g(n)

    ! PARAMETERS
    logical :: debug = .false.
    
    ! LOCAL SCALARS
    integer :: allocerr,ed,edprev,ednext,i,ierror,ind,j,k,maxnv,nsites,status,ntriag
    real(kind=8) :: mu,vol,volparc,xi
    type(pdata_type), pointer :: pdata
    type(VoronoiCell) :: Vi
    type(Polygon) :: Pj
	    
    ! LOCAL ARRAYS
    integer :: indices(n/2),til(3,2*(n/2)),tnbr(3,2*(n/2))
    real(kind=8) :: sites(2,n/2),vorxy(2,2*(n/2)),M(2,2),normal(2)
    type(Polygon), allocatable :: Wi(:)
		
    flag = 0
    
    call c_f_pointer(pdataptr,pdata)

    nsites = n / 2
    indices(1:nsites) = (/ (i,i=1,nsites) /)
    sites(1:2,1:nsites) = reshape( x(1:n), (/ 2, nsites /) )
	    
    if ( nsites .le. 1 ) then
       write(*,*) 'In evalfg: nsites must be at least 2!'
       flag = - 1
       return
    end if
  
    ! Compute Delaunay triangulation

    call dtris2(nsites,sites,indices,ntriag,til,tnbr,ierror)
    
    if ( ierror .ne. 0 .and. ierror .ne. 225 ) then
       write(*,*) 'In evalfg: dtris2 returned an error different from 0 and 225: ',ierror
       flag = - 1
       return
    end if
  
    ! Compute vertices of the Voronoi diagram
          
    if ( ntriag .gt. 2 * nsites ) then
       write(*,*) 'In evalfg: There is something wrong. According to the dtris2, '
       write(*,*) 'documentation the number of triangles in the Delaunay trangulation '
       write(*,*) 'cannot be larger than twice the number of sites!'
       flag = - 1
       return
    end if
  
    do k = 1,ntriag
       call triangle_circumcenter_2d(sites(1:2,til(1:3,k)),vorxy(1:2,k))
       if ( isnan(vorxy(1,k)) .or. isnan(vorxy(1,k)) ) then
          write(*,*) 'In evalfg: triangle_circumcenter_2d returned NaN.'
          write(*,*) 'Triangle vertices: ',sites(1:2,til(1:3,k))
          flag = - 1
          return
       end if
    end do

    f = 0.0d0

    g(1:n) = 0.0d0
    
    maxnv = maxval( pdata%nvpols(1:pdata%npols) )
      
    allocate(Wi(pdata%npols),Pj%v(2,maxnv),Pj%e(maxnv-1),stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'In evalfg: Allocation error.'
       flag = - 1
       return
    end if

    do i = 1,nsites
       if ( debug ) then
          write(*,*)
          write(*,*) 'SITE i = ',i
          write(*,*) 'Generator: ',sites(1:2,i)
          write(*,*)
       end if
       
       call voronoi_cell(i,nsites,sites,ntriag,til,tnbr,vorxy,Vi,status)
       if ( status .ne. 0 ) then
          write(*,*) 'In evalfg: Error in voronoi_cell, status = ',status
          flag = - 1
          return
       end if

       vol = 0.0d0
       do j = 1,pdata%npols
          Pj%n = pdata%nvpols(j)
          Pj%deg = .false.
	            
          Pj%v(1,1:Pj%n) = pdata%xpol(j,1:Pj%n)
          Pj%v(2,1:Pj%n) = pdata%ypol(j,1:Pj%n)
          Pj%e(1:Pj%n-1) = pdata%poledges(j,1:Pj%n-1)
          
          if ( nsites .ge. 3 .and. ierror .eq. 0 ) then
             call VorCellInterConvPol(sites(1:2,i),Pj,Vi,Wi(j))
          else ! if ( ( nsites .ge. 3 .and. ierror .eq. 225 ) .or. nsites .eq. 2 ) then
             call VorCellInterConvPol_Collinear(i,nsites,sites,Pj,Wi(j))
          end if
	            
          if ( .not. Wi(j)%deg ) then
!!$             volparc = 0.0d0
!!$             do ed = 1,Wi(j)%n - 1
!!$                volparc = volparc + Wi(j)%v(1,ed) * Wi(j)%v(2,ed+1) - Wi(j)%v(1,ed+1) * Wi(j)%v(2,ed)
!!$             end do
!!$             vol = vol + 0.5d0 * volparc
             call polygon_area_2d(Wi(j)%n-1,Wi(j)%v(1:2,1:Wi(j)%n-1),volparc)
             vol = vol + volparc
          end if
       end do
       
       f = f + ( nsites * vol / pdata%volA - 1.0d0 ) ** 2

       xi = 2.0d0 * ( nsites * vol / pdata%volA - 1.0d0 ) * nsites / pdata%volA

       if ( debug ) then
          write(*,*)
          write(*,*) '\xi_i = ',xi
          write(*,*)
       end if
       
       do j = 1,pdata%npols
          if ( debug ) then
             if ( .not. Wi(j)%deg ) then
                write(*,*)
                write(*,*) 'POLYGON Wij with i = ',i,' and j = ',j
                write(*,*) 'Number of vertices: ',Wi(j)%n - 1
                write(*,*)
             else
                write(*,*)
                write(*,*) 'POLYGON Wij with i = ',i,' and j = ',j,' IS EMPTY.'
                write(*,*)
             end if
          end if
       
          if ( .not. Wi(j)%deg ) then
             do ed = 1,Wi(j)%n - 1
                if ( debug ) then
                   write(*,*)
                   write(*,*) '===================='
                   write(*,*) 'Processing edge ed = ',ed
                   write(*,*) 'Its inner vertex, named ',ed,' as well, corresponds to the point ',Wi(j)%v(1:2,ed)
                   write(*,*) '===================='
                   write(*,*)
                end if
                
                if ( debug ) then
                   if ( ed .eq. 1 ) then
                      write(*,*) 'Previous: ',Wi(j)%n-1,' next: ',ed+1
                      write(*,*) 'They correspond to the points ',Wi(j)%v(1:2,Wi(j)%n-1),' and ',Wi(j)%v(1:2,ed+1),' respectively.'
                   else if ( ed .lt. Wi(j)%n - 1 ) then
                      write(*,*) 'Previous: ',ed-1,' next: ',ed+1
                      write(*,*) 'They correspond to the points ',Wi(j)%v(1:2,ed-1),' and ',Wi(j)%v(1:2,ed+1),' respectively.'
                   else ! if ( ed .eq. Wi(j)%n-1 ) then
                      write(*,*) 'Previous: ',ed-1,' next: ',1
                      write(*,*) 'They correspond to the points ',Wi(j)%v(1:2,ed-1),' and ',Wi(j)%v(1:2,1),' respectively.'
                   end if
                end if
                
                ! Computing the outward unit normal vector to the edge ed
                call line_exp_normal_2d(Wi(j)%v(1:2,ed+1),Wi(j)%v(1:2,ed),normal)
                   
                if ( debug ) then
                   write(*,*) 'Outward unit normal vector to the edge ed: ',normal(1:2)
                end if
                
                mu = 0.5d0 * norm2( Wi(j)%v(1:2,ed) - Wi(j)%v(1:2,ed+1) )
                   
                if ( debug ) then
                   write(*,*) 'mu (sized of edge ed, i.e. distance between vertices ed and ednext) = ',mu
                end if
                
                ! Processing edge (v_E,w_E)
                   
                if ( Wi(j)%e(ed) .lt. 0 ) then
                   
                   ! Starting point v_E

                   if ( debug ) then
                      write(*,*)
                      write(*,*) 'Processing v_E'
                      write(*,*)
                   end if

                   if ( ed .eq. 1) then
                      edprev = Wi(j)%n - 1
                   else
                      edprev = ed - 1
                   end if
                  
                   if ( Wi(j)%e(edprev) .lt. 0 ) then
                      if ( debug ) then
                         write(*,*) 'We are in the case |P(v_E)|=3. ', &
                              'Neighbour cells are: ',abs(Wi(j)%e(edprev)),' and ',abs(Wi(j)%e(ed))
                      end if
                
                      call matrixint(sites(1:2,abs(Wi(j)%e(edprev))),sites(1:2,abs(Wi(j)%e(ed))),sites(1:2,i), &
                           Wi(j)%v(1:2,ed),M,flag)
                      if ( flag .ne. 0 ) return
                      
                      ind = 2 * i - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                   
                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                
                      call matrixint(sites(1:2,i),sites(1:2,abs(Wi(j)%e(edprev))),sites(1:2,abs(Wi(j)%e(ed))), &
                           Wi(j)%v(1:2,ed),M,flag)
                      if ( flag .ne. 0 ) return

                      ind = 2 * abs(Wi(j)%e(ed)) - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                   
                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                
                      call matrixint(sites(1:2,abs(Wi(j)%e(ed))),sites(1:2,i),sites(1:2,abs(Wi(j)%e(edprev))), &
                           Wi(j)%v(1:2,ed),M,flag)
                      if ( flag .ne. 0 ) return

                      ind = 2 * abs(Wi(j)%e(edprev)) - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )

                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if

                   else
                      if ( debug ) then
                         write(*,*) 'We are in the case |P(v_E)|=2. Neighbour cell is: ',abs(Wi(j)%e(ed))
                      end if
                
                      call matrixbd(sites(1:2,abs(Wi(j)%e(ed))),sites(1:2,i),Wi(j)%v(1:2,ed),Wi(j)%v(1:2,edprev),M,flag)
                      if ( flag .ne. 0 ) return

                      ind = 2 * i - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                   
                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                
                      call matrixbd(sites(1:2,i),sites(1:2,abs(Wi(j)%e(ed))),Wi(j)%v(1:2,ed),Wi(j)%v(1:2,edprev),M,flag)
                      if ( flag .ne. 0 ) return
                      
                      ind = 2 * abs(Wi(j)%e(ed)) - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                   
                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                   end if
                   
                   ! Ending point w_E
                  
                   if ( debug ) then
                      write(*,*)
                      write(*,*) 'Processing w_E'
                      write(*,*)
                   end if
                
                   if ( ed .eq. Wi(j)%n - 1 ) then
                      ednext = 1
                   else
                      ednext = ed + 1
                   end if
                  
                   if ( Wi(j)%e(ednext) .lt. 0 ) then
                      if ( debug ) then
                         write(*,*) 'We are in the case |P(w_E)|=3. ', &
                              'Neighbour cells are: ',abs(Wi(j)%e(ed)),' and ',abs(Wi(j)%e(ednext))
                      end if
                
                      call matrixint(sites(1:2,abs(Wi(j)%e(ed))),sites(1:2,abs(Wi(j)%e(ednext))),sites(1:2,i), &
                           Wi(j)%v(1:2,ed+1),M,flag)
                      if ( flag .ne. 0 ) return
                   
                      ind = 2 * i - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )

                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                   
                      call matrixint(sites(1:2,i),sites(1:2,abs(Wi(j)%e(ed))),sites(1:2,abs(Wi(j)%e(ednext))), &
                           Wi(j)%v(1:2,ed+1),M,flag)
                      if ( flag .ne. 0 ) return
                 
                      ind = 2 * abs(Wi(j)%e(ednext)) - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )

                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                   
                      call matrixint(sites(1:2,abs(Wi(j)%e(ednext))),sites(1:2,i),sites(1:2,abs(Wi(j)%e(ed))), &
                           Wi(j)%v(1:2,ed+1),M,flag)
                      if ( flag .ne. 0 ) return
                 
                      ind = 2 * abs(Wi(j)%e(ed)) - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )

                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if

                   else 
                      if ( debug ) then
                         write(*,*) 'We are in the case |P(w_E)|=2. Neighbour cell is: ',abs(Wi(j)%e(ed))
                      end if
                
                      call matrixbd(sites(1:2,abs(Wi(j)%e(ed))),sites(1:2,i),Wi(j)%v(1:2,ed+1),Wi(j)%v(1:2,ednext+1),M,flag)
                      if ( flag .ne. 0 ) return

                      ind = 2 * i - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                   
                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                   
                      call matrixbd(sites(1:2,i),sites(1:2,abs(Wi(j)%e(ed))),Wi(j)%v(1:2,ed+1),Wi(j)%v(1:2,ednext+1),M,flag)
                      if ( flag .ne. 0 ) return
                   
                      ind = 2 * abs(Wi(j)%e(ed)) - 1
                      g(ind:ind+1) = g(ind:ind+1) + xi * mu * matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                   
                      if ( debug ) then
                         write(*,*) 'To gradient positions: ',ind,ind+1
                         write(*,*) 'we add: ',matmul( transpose( M(1:2,1:2) ), normal(1:2) )
                         write(*,*) 'multiplied by \xi * mu = ',xi * mu
                      end if
                   end if

                else
                   if ( debug ) then
                      write(*,*) 'This is a fixed edge.'
                      write(*,*) 'So, its vertices v_E and w_E are ignored.'
                   end if
                end if
             end do
          end if
       end do

       do j = 1,pdata%npols
          call polygon_destroy(Wi(j))
       end do
       
       call voronoi_cell_destroy(Vi)
    end do

    f = f / nsites
    
    g(1:n) = g(1:n) / nsites
    
    deallocate(Wi,Pj%v,Pj%e,stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'Deallocation error.'
       flag = - 1
       return
    end if
    
  end subroutine evalfg

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

  subroutine matrixint(x,y,z,v,M,flag)
    
    ! It computes the matrix used in the calculation of the gradient
    ! of a volume functional, when v belongs to T_int.
        
    implicit none

    ! SCALAR ARGUMENTS
    integer, intent(inout) :: flag
    
    ! ARRAY ARGUMENTS
    real(kind=8), intent(in) :: x(2),y(2),z(2),v(2)
    real(kind=8), intent(out) :: M(2,2)
    
    ! LOCAL SCALARS
    real(kind=8) :: det,invdet

    det = ( y(1) - x(1) ) * ( z(2) - x(2) ) - ( y(2) - x(2) ) * ( z(1) - x(1) )
        
    if ( det .eq. 0.0d0 ) then
       write(*,*) 'In matrixint: Division by zero in the calculation of matrix M.'
       flag = - 1
       return
    else if ( abs(det) .lt. 1.0d-14 ) then
       write(*,*) 'In matrixint: Warning: Determinant is too small.'
    end if
		
    invdet = 1.0d0 / det
        
    M(1,1) = invdet * ( y(2) - x(2) ) * ( v(1) - z(1) )
    M(1,2) = invdet * ( y(2) - x(2) ) * ( v(2) - z(2) )
    M(2,1) = invdet * ( x(1) - y(1) ) * ( v(1) - z(1) )
    M(2,2) = invdet * ( x(1) - y(1) ) * ( v(2) - z(2) )
    
  end subroutine matrixint
    
  ! ******************************************************************
  ! ******************************************************************

  subroutine matrixbd(y,x,ref,p,M,flag)
    
    ! It computes the matrix used in the calculation of the gradient
    ! of a volume functional, when ref belongs to T_bd.
        
    implicit none
        
    ! SCALAR ARGUMENTS
    integer, intent(inout) :: flag
    
    ! ARRAY ARGUMENTS
    real(kind=8), intent(in) :: y(2),x(2),ref(2),p(2)
    real(kind=8), intent(out) :: M(2,2)
        
    ! LOCAL SCALARS
    real(kind=8) :: a,b,c,det,invdet
        
    ! LOCAL ARRAYS
    real(kind=8) ::gradphi(2)
        
    call line_exp2imp_2d(ref,p,a,b,c)
        
    gradphi(1:2) = (/ a, b /)
		
    det = ( y(1) - x(1) ) * gradphi(2) - ( y(2) - x(2) ) * gradphi(1)
		
    if ( det .eq. 0.0d0 ) then
       write(*,*) 'In matrixbd: Division by zero in the calculation of matrix M.'
       flag = - 1
       return
    else if ( abs(det) .lt. 1.0d-14 ) then
       write(*,*) 'In matrixbd: Warning: Determinant is too small.'
    end if
		
    invdet = 1.0d0 / det
        
    M(1,1) =   invdet * gradphi(2) * ( ref(1) - x(1) )
    M(1,2) =   invdet * gradphi(2) * ( ref(2) - x(2) )
    M(2,1) = - invdet * gradphi(1) * ( ref(1) - x(1) )
    M(2,2) = - invdet * gradphi(1) * ( ref(2) - x(2) )
    
  end subroutine matrixbd
    
  ! ******************************************************************
  ! ******************************************************************

  subroutine proj(n,x,flag,pdataptr)
	
    implicit none
  
    ! SCALAR ARGUMENTS
    integer, intent(in) :: n
    integer, intent(out) :: flag
    type(c_ptr), optional, intent(in) :: pdataptr
	  
    ! ARRAY ARGUMENTS
    real(kind=8), intent(inout) :: x(n)

    ! LOCAL SCALARS
    integer :: i,nsites
    real(kind=8) :: tmp
    type(pdata_type), pointer :: pdata

    call c_f_pointer(pdataptr,pdata)

    flag = 0

    nsites = n / 2

    do i = 1,nsites
       tmp = norm2( x(2*i-1:2*i) - pdata%c(1:2) )
       if ( tmp .gt. pdata%rmax ) then
          x(2*i-1:2*i) = pdata%c(1:2) + pdata%rmax * ( x(2*i-1:2*i) - pdata%c(1:2) ) / tmp
       end if
    end do
    
  end subroutine proj
    
  ! ******************************************************************
  ! ******************************************************************

  subroutine approxg(n,x,g,flag,pdataptr)
	
    implicit none
  
    ! SCALAR ARGUMENTS
    integer, intent(in) :: n
    integer, intent(out) :: flag
    type(c_ptr), optional, intent(in) :: pdataptr
	  
    ! ARRAY ARGUMENTS
    real(kind=8), intent(inout) :: x(n)
    real(kind=8), intent(out) :: g(n)

    ! LOCAL SCALARS
    integer :: i
    real(kind=8), parameter :: h = 1.0d-06
    real(kind=8) :: fplus,fminus,tmp

    ! LOCAL ARRAYS
    real(kind=8) :: gtmp(n)

    do i = 1,n
       tmp = x(i)
       x(i) = tmp + h
       call evalfg(n,x,fplus,gtmp,flag,pdataptr)
       x(i) = tmp - h
       call evalfg(n,x,fminus,gtmp,flag,pdataptr)
       g(i) = ( fplus - fminus ) / ( 2.0d0 * h )
       x(i) = tmp
    end do
      
  end subroutine approxg
    
  ! ******************************************************************
  ! ******************************************************************

  subroutine compscale(nsites,scale,area,pdataptr)

    implicit none
  
    ! SCALAR ARGUMENTS
    integer, intent(in) :: nsites
    real(kind=8), intent(out) :: area,scale
    type(c_ptr), optional, intent(in) :: pdataptr

    ! PARAMETERS
    real(kind=8), parameter :: eps = 0.1d0
  ! real(kind=8), parameter :: eps = 1.0d0
    
    ! LOCAL SCALARS
    real(kind=8) :: a,b,phia,phib,tphi,tscale,tarea
    type(pdata_type), pointer :: pdata
    
    call c_f_pointer(pdataptr,pdata)

    tscale = 1.0d0

    call comparea(tscale,tarea,pdataptr)

    tphi = tarea / nsites - 1.0d0

    a = tscale
    phia = tphi
    
    write(*,*) 'scale factor (a) = ',tscale,' ideal area (a) = ',tarea / nsites
    
    if ( abs( tphi ) .le. eps ) then
       scale = tscale
       area = tarea
       return
    end if

    if ( tphi .lt. 0.0d0 ) then
       do while ( tphi .lt. 0.0d0 )
          tscale = 2.0d0 * tscale
          call comparea(tscale,tarea,pdataptr)
          tphi = tarea / nsites - 1.0d0
       end do
       
       b = tscale
       phib = tphi
       
    else
       do while ( tphi .gt. 0.0d0 )
          tscale = 0.5d0 * tscale
          call comparea(tscale,tarea,pdataptr)
          tphi = tarea / nsites - 1.0d0
       end do
       
       b = tscale
       phib = tphi
    end if

    write(*,*) 'scale factor (b) = ',tscale,' ideal area (b) = ',tarea / nsites
    
    if ( abs( tphi ) .le. eps ) then
       scale = tscale
       area = tarea
       return
    end if

10  continue
    
    tscale = 0.5d0 * ( a + b )
    
    call comparea(tscale,tarea,pdataptr)

    tphi = tarea / nsites - 1.0d0

    write(*,*) 'scale factor (c) = ',tscale,' ideal area (c) = ',tarea / nsites
    
    if ( abs( tphi ) .le. eps ) then
       scale = tscale
       area = tarea
       return
    end if

    if ( sign(1.0d0,tphi) .eq. sign(1.0d0,phia) ) then
       a = tscale
       phia = tphi
    else
       b = tscale
       phib = tphi
    end if

    go to 10
    
  end subroutine compscale
	  
  ! ******************************************************************
  ! ******************************************************************

  subroutine comparea(scale,area,pdataptr)
	
    implicit none
  
    ! SCALAR ARGUMENTS
    real(kind=8), intent(in) :: scale
    real(kind=8), intent(out) :: area
    type(c_ptr), optional, intent(in) :: pdataptr
	  
    ! LOCAL SCALARS
    type(pdata_type), pointer :: pdata
    
    ! LOCAL SCALARS
    integer :: allocerr,j,maxnv
    real(kind=8) :: areaPj
    
    ! LOCAL ARRAYS
    real(kind=8), allocatable :: poltmp(:,:)
    
    call c_f_pointer(pdataptr,pdata)

    maxnv = maxval( pdata%nvpols(1:pdata%npols) )
  
    allocate(poltmp(2,maxnv),stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'Allocation error.'
       stop
    end if
        
    area = 0.0d0
    
    do j = 1,pdata%npols
       poltmp(1,1:pdata%nvpols(j)) = scale * pdata%xpol(j,1:pdata%nvpols(j))
       poltmp(2,1:pdata%nvpols(j)) = scale * pdata%ypol(j,1:pdata%nvpols(j))
     
       call polygon_area_2d(pdata%nvpols(j)-1,poltmp(1:2,1:pdata%nvpols(j)-1),areaPj)
        
       area = area + areaPj
    end do
	
    deallocate(poltmp,stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'Deallocation error.'
       stop
    end if

   end subroutine comparea
  
  ! ******************************************************************
  ! ******************************************************************

  subroutine compvols(n,x,vol,pdataptr)
	
    implicit none
  
    ! SCALAR ARGUMENTS
    integer, intent(in) :: n
    type(c_ptr), optional, intent(in) :: pdataptr
	  
    ! ARRAY ARGUMENTS
    real(kind=8), intent(in) :: x(n)
    real(kind=8), intent(out) :: vol(n/2)

    ! LOCAL SCALARS
    integer :: allocerr,i,ierror,j,k,maxnv,nsites,status,ntriag
    real(kind=8) :: volparc
    type(pdata_type), pointer :: pdata
    type(VoronoiCell) :: Vi
    type(Polygon) :: Wij,Pj
    
    ! LOCAL ARRAYS
    integer :: indices(n/2),til(3,2*(n/2)),tnbr(3,2*(n/2))
    real(kind=8) :: sites(2,n/2),vorxy(2,2*(n/2))

    call c_f_pointer(pdataptr,pdata)

    nsites = n / 2
    indices(1:nsites) = (/ (i,i=1,nsites) /)
    sites(1:2,1:nsites) = reshape( x(1:n), (/ 2, nsites /) )
  
    if ( nsites .le. 1 ) then
       write(*,*) 'In drawsol: nsites must be at least 2!'
       stop
    end if
  
    ! Compute Delaunay triangulation

    call dtris2(nsites,sites,indices,ntriag,til,tnbr,ierror)

    if ( ierror .ne. 0 .and. ierror .ne. 225 ) then
       write(*,*) 'In drawsol: dtris2 returned an error different from 0 and 225: ',ierror
       stop
    end if
  
    ! Compute vertices of the Voronoi diagram
          
    if ( ntriag .gt. 2 * nsites ) then
       write(*,*) 'In drawsol: There is something wrong. According to the dtris2, '
       write(*,*) 'documentation the number of triangles in the Delaunay trangulation '
       write(*,*) 'cannot be larger than twice the number of sites!'
       stop
    end if
  
    do k = 1,ntriag
       call triangle_circumcenter_2d(sites(1:2,til(1:3,k)),vorxy(1:2,k))
       if ( isnan(vorxy(1,k)) .or. isnan(vorxy(1,k)) ) then
          write(*,*) 'In drawsol: triangle_circumcenter_2d returned NaN.'
          write(*,*) 'Triangle vertices: ',sites(1:2,til(1:3,k))
          stop
       end if
    end do

    maxnv = maxval( pdata%nvpols(1:pdata%npols) )
      
    allocate(Pj%v(2,maxnv),Pj%e(maxnv-1),stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'In drawsol: Allocation error.'
       stop
    end if
      
    do i = 1,nsites
       call voronoi_cell(i,nsites,sites,ntriag,til,tnbr,vorxy,Vi,status)
       if ( status .ne. 0 ) then
          write(*,*) 'In drawsol: Error in voronoi_cell, status = ',status
          stop
       end if

       vol(i) = 0.0d0
       do j = 1,pdata%npols
          Pj%n = pdata%nvpols(j)
          Pj%deg = .false.       

          Pj%v(1,1:Pj%n) = pdata%xpol(j,1:Pj%n)
          Pj%v(2,1:Pj%n) = pdata%ypol(j,1:Pj%n)
          Pj%e(1:Pj%n-1)  = pdata%poledges(j,1:Pj%n-1)

          if ( nsites .ge. 3 .and. ierror .eq. 0 ) then
             call VorCellInterConvPol(sites(1:2,i),Pj,Vi,Wij)
          else ! if ( ( nsites .ge. 3 .and. ierror .eq. 225 ) .or. nsites .eq. 2 ) then
             call VorCellInterConvPol_Collinear(i,nsites,sites,Pj,Wij)
          end if

          if ( .not. Wij%deg ) then
             call polygon_area_2d( Wij%n - 1, Wij%v(1:2,1:Wij%n - 1), volparc )
             vol(i) = vol(i) + volparc
          end if

          call polygon_destroy(Wij)
       end do
    end do

  end subroutine compvols
  
  ! ******************************************************************
  ! ******************************************************************

  subroutine drawsol(n,x,filename,pdataptr)
	
    implicit none
  
    ! SCALAR ARGUMENTS
    integer, intent(in) :: n
    type(c_ptr), optional, intent(in) :: pdataptr
	  
    ! ARRAY ARGUMENTS
    real(kind=8), intent(in) :: x(n)
    character(len=80) :: filename

    ! LOCAL SCALARS
    integer :: allocerr,i,ierror,j,k,maxnv,nsites,status,ntriag,v
    real(kind=8) :: volparc
    type(pdata_type), pointer :: pdata
    type(VoronoiCell) :: Vi
    type(Polygon) :: Wij,Pj,square,squacurcell
    
    ! LOCAL ARRAYS
    integer :: indices(n/2),til(3,2*(n/2)),tnbr(3,2*(n/2))
    real(kind=8) :: sites(2,n/2),vol(n/2),vorxy(2,2*(n/2))

    call c_f_pointer(pdataptr,pdata)

    nsites = n / 2
    indices(1:nsites) = (/ (i,i=1,nsites) /)
    sites(1:2,1:nsites) = reshape( x(1:n), (/ 2, nsites /) )
  
    if ( nsites .le. 1 ) then
       write(*,*) 'In drawsol: nsites must be at least 2!'
       stop
    end if
  
    ! Compute Delaunay triangulation

    call dtris2(nsites,sites,indices,ntriag,til,tnbr,ierror)

    if ( ierror .ne. 0 .and. ierror .ne. 225 ) then
       write(*,*) 'In drawsol: dtris2 returned an error different from 0 and 225: ',ierror
       stop
    end if
  
    ! Compute vertices of the Voronoi diagram
          
    if ( ntriag .gt. 2 * nsites ) then
       write(*,*) 'In drawsol: There is something wrong. According to the dtris2, '
       write(*,*) 'documentation the number of triangles in the Delaunay trangulation '
       write(*,*) 'cannot be larger than twice the number of sites!'
       stop
    end if
  
    do k = 1,ntriag
       call triangle_circumcenter_2d(sites(1:2,til(1:3,k)),vorxy(1:2,k))
       if ( isnan(vorxy(1,k)) .or. isnan(vorxy(1,k)) ) then
          write(*,*) 'In drawsol: triangle_circumcenter_2d returned NaN.'
          write(*,*) 'Triangle vertices: ',sites(1:2,til(1:3,k))
          stop
       end if
    end do

    maxnv = maxval( pdata%nvpols(1:pdata%npols) )
      
    allocate(Pj%v(2,maxnv),Pj%e(maxnv-1),stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'In drawsol: Allocation error.'
       stop
    end if
      
    open(unit=10,file=trim(filename))                           

  ! write(10,10) nsites
    write(10,10) 1,pdata%drawscale

    ! Filling small cells of Voronoi diagram
    
    do i = 1,nsites
       call voronoi_cell(i,nsites,sites,ntriag,til,tnbr,vorxy,Vi,status)
       if ( status .ne. 0 ) then
          write(*,*) 'In drawsol: Error in voronoi_cell, status = ',status
          stop
       end if

       vol(i) = 0.0d0
       do j = 1,pdata%npols
          Pj%n = pdata%nvpols(j)
          Pj%deg = .false.       

          Pj%v(1,1:Pj%n) = pdata%xpol(j,1:Pj%n)
          Pj%v(2,1:Pj%n) = pdata%ypol(j,1:Pj%n)
          Pj%e(1:Pj%n-1)  = pdata%poledges(j,1:Pj%n-1)

          if ( nsites .ge. 3 .and. ierror .eq. 0 ) then
             call VorCellInterConvPol(sites(1:2,i),Pj,Vi,Wij)
          else ! if ( ( nsites .ge. 3 .and. ierror .eq. 225 ) .or. nsites .eq. 2 ) then
             call VorCellInterConvPol_Collinear(i,nsites,sites,Pj,Wij)
          end if

          if ( .not. Wij%deg ) then
             call polygon_area_2d( Wij%n - 1, Wij%v(1:2,1:Wij%n - 1), volparc )
             vol(i) = vol(i) + volparc
          end if

          call polygon_destroy(Wij)
       end do

       if ( vol(i) .lt. 0.5d0 * pdata%volA / nsites ) then
          do j = 1,pdata%npols
             Pj%n = pdata%nvpols(j)
             Pj%deg = .false.         
             
             Pj%v(1,1:Pj%n) = pdata%xpol(j,1:Pj%n)
             Pj%v(2,1:Pj%n) = pdata%ypol(j,1:Pj%n)
             Pj%e(1:Pj%n-1)  = pdata%poledges(j,1:Pj%n-1)

             if ( nsites .ge. 3 .and. ierror .eq. 0 ) then
                call VorCellInterConvPol(sites(1:2,i),Pj,Vi,Wij)
             else ! if ( ( nsites .ge. 3 .and. ierror .eq. 225 ) .or. nsites .eq. 2 ) then
                call VorCellInterConvPol_Collinear(i,nsites,sites,Pj,Wij)
             end if

             if ( .not. Wij%deg ) then
                do v = 1,Wij%n - 1
                   if ( v .eq. 1 ) then
                      write(10,31) Wij%v(1:2,v),Wij%v(1:2,v+1)
                   else if ( v .gt. 1 .and. v .lt. Wij%n - 1 ) then 
                      write(10,32) Wij%v(1:2,v),Wij%v(1:2,v+1)
                   else
                      write(10,33) Wij%v(1:2,v)
                   end if
                end do
             end if

             call polygon_destroy(Wij)
          end do
       end if
       
       call voronoi_cell_destroy(Vi)         
    end do

    ! Drawing Delaunay triangulation
    
    do k = 1,ntriag
       do v = 1,3
          write(10,20) sites(1:2,til(v,k)),sites(1:2,til(mod(v,3)+1,k))
       end do
    end do

    ! Frame
    
    square%n = 5
    square%deg = .false.
    
    allocate(square%v(2,square%n),square%e(square%n-1),stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'In drawsol: Allocation error.'
       stop
    end if
    
    square%v(1,1:5) = (/ pdata%xmin, pdata%xmax, pdata%xmax, pdata%xmin, pdata%xmin /)
    square%v(2,1:5) = (/ pdata%ymin, pdata%ymin, pdata%ymax, pdata%ymax, pdata%ymin /)
    square%e(1:4) = (/ 0, 0, 0, 0 /)  
        
    ! Drawing Voronoi diagram
    
    do i = 1,nsites
       call voronoi_cell(i,nsites,sites,ntriag,til,tnbr,vorxy,Vi,status)
       if ( status .ne. 0 ) then
          write(*,*) 'In drawsol: Error in voronoi_cell, status = ',status
          stop
       end if

       if ( nsites .ge. 3 .and. ierror .eq. 0 ) then
          call VorCellInterConvPol(sites(1:2,i),square,Vi,squacurcell)
       else ! if ( ( nsites .ge. 3 .and. ierror .eq. 225 ) .or. nsites .eq. 2 ) then
          call VorCellInterConvPol_Collinear(i,nsites,sites,square,squacurcell)
       end if

       if ( .not. squacurcell%deg ) then
          do v = 1,squacurcell%n - 1
             write(10,50) squacurcell%v(1:2,v),squacurcell%v(1:2,v+1)
          end do
       end if

       call polygon_destroy(squacurcell)

       do j = 1,pdata%npols
          Pj%n = pdata%nvpols(j)
          Pj%deg = .false.

          Pj%v(1,1:Pj%n) = pdata%xpol(j,1:Pj%n)
          Pj%v(2,1:Pj%n) = pdata%ypol(j,1:Pj%n)
          Pj%e(1:Pj%n-1)  = pdata%poledges(j,1:Pj%n-1)

          if ( nsites .ge. 3 .and. ierror .eq. 0 ) then
             call VorCellInterConvPol(sites(1:2,i),Pj,Vi,Wij)
          else ! if ( ( nsites .ge. 3 .and. ierror .eq. 225 ) .or. nsites .eq. 2 ) then
             call VorCellInterConvPol_Collinear(i,nsites,sites,Pj,Wij)
          end if

          if ( .not. Wij%deg ) then
             do v = 1,Wij%n - 1
                if ( Wij%e(v) .lt. 1 ) then
                   write(10,40) Wij%v(1:2,v),Wij%v(1:2,v+1)
                end if
             end do
          end if

          call polygon_destroy(Wij)
       end do

       ! Points
       write(10,60) sites(1:2,i)
!!$       if ( nsites .le. 10 ) then
!!$          write(10,65) i,sites(1:2,i)            
!!$       end if
       
       call voronoi_cell_destroy(Vi)
    end do
      
    deallocate(square%v,square%e,stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'In drawsol: Deallocation error.'
       stop
    end if
        
    ! Drawing A
    
    do j = 1,pdata%npols
       Pj%n = pdata%nvpols(j)
       Pj%deg = .false.          

       Pj%v(1,1:Pj%n) = pdata%xpol(j,1:Pj%n)
       Pj%v(2,1:Pj%n) = pdata%ypol(j,1:Pj%n)
       Pj%e(1:Pj%n-1)  = pdata%poledges(j,1:Pj%n-1)

       do k = 1,Pj%n - 1
          if ( Pj%e(k) .eq. 0 ) then
             write(10,30) Pj%v(1:2,k),Pj%v(1:2,k+1)
          end if
       end do
    end do

    ! Close figure adding a few statistics
    
    write(10,*) '% Area of A = ',pdata%volA
    write(10,*) '% Number of polygons that define A = ',pdata%npols
    write(10,*) '% Number of sites = ',nsites
    write(10,*) '% Value of f = ',sum( ( vol(1:nsites) * nsites / pdata%volA - 1.0d0 ) ** 2 ) / nsites
    write(10,*) '% Ideal area = ',pdata%volA / nsites
    write(10,*) '% Number of cells with area smaller than sqrt(eps) approx. 1e-08: ', &
         count( vol(1:nsites) .lt. sqrt( epsilon( 1.0d0 ) ) )
    write(10,*) '% Number of cells with area smaller than half of ideal area: ', &
         count( vol(1:nsites) .lt. 0.5d0 * pdata%volA / nsites )
    write(10,*) '% Discarding null cells, largest relative error with respect to ideal area: ', &
         maxval( abs( vol(1:nsites) * nsites / pdata%volA - 1.0d0 ), &
         vol(1:nsites) .ge. sqrt( epsilon( 1.0d0 ) ) )
    
    write(10,70)
    
    close(10)

    deallocate(Pj%v,Pj%e,stat=allocerr)
    if ( allocerr .ne. 0 ) then
       write(*,*) 'In drawsol: Deallocation error.'
       stop
    end if

    ! NON-EXECUTABLE STATEMENTS
    
10  format('prologues := 3;',/, &
         'outputtemplate := "%j.mps";',/, &
         'input mpcolornames;',/, &
         'beginfig(',i4,');',/, &
         'u = ',f20.10,'cm;')
20  format('draw ( ',f20.10,'u,',f20.10,'u)--(',f20.10,'u,',f20.10,'u) withcolor green+0.6white;')
30  format('draw ( ',f20.10,'u,',f20.10,'u)--(',f20.10,'u,',f20.10,'u) withcolor black;')
31  format('fill ((',f20.10,'u,',f20.10,'u)--(',f20.10,'u,',f20.10,'u)')
32  format('    --(',f20.10,'u,',f20.10,'u)--(',f20.10,'u,',f20.10,'u)')
33  format('    --(',f20.10,'u,',f20.10,'u)--cycle) withcolor red+0.2white;')              
40  format('draw ( ',f20.10,'u,',f20.10,'u)--(',f20.10,'u,',f20.10,'u) withcolor 0.2green+red;')        
50  format('draw ( ',f20.10,'u,',f20.10,'u)--(',f20.10,'u,',f20.10,'u) withcolor 0.9green+0.9red;')
60  format('drawdot(',f20.10,'u,',f20.10,'u);') 
!!$65  format('label(btex ',i2,' etex, (',f20.10,'u,',f20.10,'u));') 
70  format('endfig;',/,'end;')
    
  end subroutine drawsol
    
end module optvor
