!
! -----------------------------------
! Module of POS optimization routines
! -----------------------------------
!    module util_random is requied
!    module util_sort   is requied
!    module util_qmc    is requied
!
!----------------------------------------------------------------------
module opti_pso
  implicit none
  private

  !============= PUBLIC ROUTINES =============
  public :: do_pso       ! simple routine with standard control parameters
  public :: do_psof      ! simple routine with configuration file
  public :: pso          ! primitive routine
  public :: pso_cbdummy  ! dummy callback function

  !============= PUBLIC PARAMETERS =============
  !----- control flags -----
  integer, public, parameter :: pso_maximize = 1  ! maximize object function
  integer, public, parameter :: pso_minimize = 0  ! minimize object function
  integer, public, parameter :: f_quiet   = 1     ! restrain messages
  integer, public, parameter :: f_initp   = 2     ! use initial parameters
  integer, public, parameter :: f_lrand   = 4     ! local random search algorithm
  integer, public, parameter :: f_vlimit  = 8     ! velocity limit algorithm
  integer, public, parameter :: f_vrand   = 16    ! velocity perturbation algorithm
  integer, public, parameter :: f_gcenter = 32    ! evaluate center of gravity
  integer, public, parameter :: f_gjump   = 64    ! move to center of gravity
  integer, public, parameter :: f_qmcinit = 128   ! initialize by Quasi-Monte Carlo

  !============= PRIVATE VARIABLES =============
  integer, parameter :: max_nparam = 110      ! maximum number of parameter
  real               :: scale_a(max_nparam)   ! parameter scaling factor A
  real               :: scale_b(max_nparam)   ! parameter scaling factor B
  real, parameter    :: constrained = -1.E37  ! evaluation value of constrained particle
  real, parameter    :: void = -1.E38         ! evaluation value of undefined particle

  type type_p
    real :: x(max_nparam)          ! current position
    real :: v(max_nparam)          ! current velocity
    real :: ev                     ! current evaluation value
    real :: evbest                 ! best evaluation value of the particle
    real :: xbest(max_nparam)      ! best position
    integer :: constrained         ! number of constrained condition
  end type type_p

contains


!======================================================================
! PSO Simplified Wrapper Routine (with standard control parameters)
!======================================================================
subroutine do_pso(n, xmin, xmax, xinit, np, it, ir, maximize, evfunc, x, r)
  integer, intent(in)    :: n             ! number of parameter
  real,    intent(in)    :: xmin(1:n)     ! lower bounds
  real,    intent(in)    :: xmax(1:n)     ! upper bounds
  real,    intent(in)    :: xinit(1:n)    ! initial parameters
  integer, intent(in)    :: np            ! population size (number of particle)
  integer, intent(in)    :: it            ! maximum iteration count (<=0:unlimitted)
  integer, intent(in)    :: ir            ! interval of message printing (<=0:no mesage)
  integer, intent(in)    :: maximize      ! maximize-minimize flag (>0:max, <=0:min)
  real,    intent(out)   :: x(1:n)        ! optimum parameter set
  real,    intent(out)   :: r             ! optimum evaluation value
  interface
    !-------- evaluation function -------
    integer function evfunc(x, r)  ! return number of constrained condition
      real, intent(in)  :: x(:)    ! parameter set
      real, intent(out) :: r       ! evaluation value
    end function evfunc
  end interface

  !====== Local variables =======
  real    :: w, c1, c2, c3, vmax, vrand
  integer :: ie, flag

  !====== Setup control parameters =======
  w = 0.9            ! inertia parameter (0.9 is recommended)
  c1 = 2.0           ! self intention parameter (2.0 is recommended)
  c2 = 2.0           ! swarm intention parameter (2.0 is recommended)
  c3 = 1.e-5         ! random search parameter (very small value is recommended)
  vmax = 0.5         ! limit of vector length (0.5-1.0 is recommended)
  vrand = 0.15       ! velocity perturbation parameter (0.0-0.1 is recommended)
  ie = 0             ! maximum evaluation count (<=0 means unlimitted)

  !====== Setup control flags =======
  flag = 0           ! control flag
!! flag = flag + f_quiet   ! restrain messages
   flag = flag + f_initp   ! use initial parameters
   flag = flag + f_lrand   ! local random search algorithm
   flag = flag + f_vlimit  ! velocity limit algorithm
   flag = flag + f_vrand   ! velocity perturbation algorithm
   flag = flag + f_gcenter ! evaluate center of gravity
!! flag = flag + f_gjump   ! move to center of gravity
!! flag = flag + f_qmcinit ! initialize by Quasi-Monte Carlo

  !====== Optimize =======
  call pso(n, xmin, xmax, xinit, np, w, c1, c2, c3, vmax, vrand, &
&              it, ie, ir, 0, flag, maximize, evfunc, pso_cbdummy, x, r)

end subroutine do_pso

!======================================================================
! PSO Simplified Wrapper Routine (with configuration file)
!======================================================================
subroutine do_psof(n, xmin, xmax, xinit, maximize, evfunc, pfname, x, r)
  integer, intent(in)    :: n             ! number of parameter
  real,    intent(in)    :: xmin(1:n)     ! lower bounds
  real,    intent(in)    :: xmax(1:n)     ! upper bounds
  real,    intent(in)    :: xinit(1:n)    ! initial parameters
  integer, intent(in)    :: maximize      ! maximize-minimize flag (>0:max, <=0:min)
  character(len=*)       :: pfname        ! configuration file name
  real,    intent(out)   :: x(1:n)        ! optimum parameter set
  real,    intent(out)   :: r             ! optimum evaluation value
  interface
    !-------- evaluation function -------
    integer function evfunc(x, r)  ! return number of constrained condition
      real, intent(in)  :: x(:)    ! parameter set
      real, intent(out) :: r       ! evaluation value
    end function evfunc
  end interface

  integer :: i, nn
  integer :: np, it, ie, ir, flag
  real    :: w, c1, c2, c3, vmax, vrand
  integer :: flag_quiet, flag_initp, flag_vlimit, flag_lrand, flag_vrand, &
&            flag_gcenter, flag_gjump, flag_qmcinit
  namelist /opti_pso/ np,w,c1,c2,c3,vmax,vrand,it,ie,ir, &
&            flag_quiet, flag_initp, flag_vlimit, flag_lrand, flag_vrand, &
&            flag_gcenter, flag_gjump, flag_qmcinit

  open(1, file=pfname, status='old')
  read(1, opti_pso)
  close(1)

  flag = flag_quiet
  flag = flag + flag_initp   * 2**1
  flag = flag + flag_vlimit  * 2**2
  flag = flag + flag_lrand   * 2**3
  flag = flag + flag_vrand   * 2**4
  flag = flag + flag_gcenter * 2**5
  flag = flag + flag_gjump   * 2**6
  flag = flag + flag_qmcinit * 2**7

  nn = 0
  do i = 1, n
    if(xmin(i) /= xmax(i)) then
      nn = nn + 1
    end if
  end do
  c3   = c3   * sqrt(real(nn))
  vmax = vmax * sqrt(real(nn))

  call pso(n, xmin, xmax, xinit, np, w, c1, c2, c3, vmax, vrand, &
&              it, ie, ir, 0, flag, maximize, evfunc, pso_cbdummy, x, r)

end subroutine do_psof

!======================================================================
! PSO (primitive routine)
!======================================================================
subroutine pso(n, xmin, xmax, xinit, np, w, c1, c2, c3, vmax, vrand, &
&              it, ie, ir, ic, flag, maximize, evfunc, cbfunc, &
&                                                  xopti, ropti)
  use util_random, only : init_grand
  integer, intent(in)    :: n             ! number of parameter
  real,    intent(in)    :: xmin(1:n)     ! lower bounds
  real,    intent(in)    :: xmax(1:n)     ! upper bounds
  real,    intent(in)    :: xinit(1:n)    ! initial parameters
  integer, intent(in)    :: np            ! population size (number of particle)
  real,    intent(in)    :: w             ! inertia parameter
  real,    intent(in)    :: c1            ! self intention parameter
  real,    intent(in)    :: c2            ! swarm intention parameter
  real,    intent(in)    :: c3            ! random search parameter (f_lrand)
  real,    intent(in)    :: vmax          ! limit of vector (f_vlimit)
  real,    intent(in)    :: vrand         ! velocity parametebtion parameter (f_vrand)
  integer, intent(in)    :: it            ! maximum iteration count (<=0:unlimitted)
  integer, intent(in)    :: ie            ! maximum evaluation count (<=0:unlimitted)
  integer, intent(in)    :: ir            ! interval of message printing (<=0:nomessage)
  integer, intent(in)    :: ic            ! interval of callback function (<=0:no call)
  integer, intent(in)    :: flag          ! control flag
  integer, intent(in)    :: maximize      ! maximize-minimize flag (>0:max, <=0:min)
  real,    intent(out)   :: xopti(1:n)    ! optimum parameter set
  real,    intent(out)   :: ropti         ! optimum evaluation value
  interface
    !-------- evaluation function -------
    integer function evfunc(x, r)  ! return number of constrained condition
      real, intent(in)  :: x(:)    ! parameter set
      real, intent(out) :: r       ! evaluation value
    end function evfunc
    !-------- callback function -------
    integer function cbfunc(it, iev, ev, x) ! return continuation flag(>=0:cont.,<0:quit)
      integer, intent(in) :: it    ! current iteration counter
      integer, intent(in) :: iev   ! current evaluation counter
      real,    intent(in) :: ev    ! current evaluation value
      real,    intent(in) :: x(:)  ! current parameter value
    end function cbfunc
  end interface


  !===== Local variables =====
  type(type_p) :: p(1:np)          ! particles
  integer      :: ip(1:np)         ! index of particles
  real         :: gebest           ! swarm best evaluation value
  real         :: gxbest(1:n)      ! swarm best position
  type(type_p) :: g                ! gravity center of good particles
  integer      :: evcount          ! counter of evaluation
  integer      :: sign             ! sign of evaluation value
  integer      :: best, worst      ! index of best/worst particle
  logical      :: exit_loop        ! exit flag
  integer      :: i, j             ! loop counter


  !===== Print PSO parameters =====
  if(flag_off(flag, f_quiet)) then
    call print_param(n, np, w, c1, c2, c3, vmax, it, ie, flag)
    print *, 'PSO --- cycle / best / mean / constrained(%) / evaluation ---'
  end if

  !===== Check arguments =====
  call check_param(n, xmin, xmax, xinit, np, w, c1, c2, c3, vmax, it, flag)

  !===== Initialize scaling function =====
  call init_scaling(n, xmin, xmax)

  !===== Initialize random number generator =====
  call init_grand

  !===== Initialize sign of evaluation =====
  if(maximize > 0) then
    sign = 1
  else
    sign = -1
  end if

  !===== Initialize particles =====
  evcount = 0
  call init_particles(n, xinit, np, flag, p)
  gebest = void


  !***********************************************************
  !*********************** Main Loop *************************
  i = 0
  do

    !===== Evaluate all particles =====
    !$omp parallel do
    do j=1, np
      p(j)%ev = evaluate(n, p(j)%x, sign, evfunc, evcount)
      if(abs(p(j)%ev) > p(j)%evbest) then
        p(j)%evbest = p(j)%ev
        p(j)%xbest(1:n) = p(j)%x(1:n)
      end if
    end do
    !$omp end parallel do

    !===== Sort particles by evaluation value =====
    call psort(np, p, ip)

    !===== Update swarm best position =====
    best = ip(1)
    if(p(best)%ev > gebest) then
      gxbest(1:n) = p(best)%x(1:n)
      gebest      = p(best)%ev
    end if

    !===== Calculate gravity center of good particles =====
    if(flag_on(flag, f_gcenter) .or. flag_on(flag, f_gjump)) then
      g%x(1:n)  = gcenter(n, 3, p, ip)
      g%ev = evaluate(n, g%x, sign, evfunc, evcount)
    end if

    !===== Update swarm best position by gravity center =====
    if(flag_on(flag, f_gcenter)) then
      if(g%ev > gebest) then
        gxbest(1:n) = g%x(1:n)
        gebest      = g%ev
      end if
    end if

    !===== Move worst particle to gravity center =====
    if(flag_on(flag, f_gjump)) then
      worst = ip(np)
      p(worst)%v(1:n)  = g%x(1:n) - p(worst)%x(1:n)
      p(worst)%x(1:n)  = g%x(1:n)
      p(worst)%ev      = g%ev
      if(p(worst)%ev > p(worst)%evbest) then
        p(worst)%xbest(1:n) = p(worst)%x(1:n)
        p(worst)%evbest     = p(worst)%ev
      end if
    end if

    !===== Check exit conditions =====
    if((it > 0 .and. i >= it) .or. (ie > 0 .and. evcount >= ie)) then
      exit_loop = .true.
    else
      exit_loop = .false.
    end if

    !===== Call callback function =====
    if((ic > 0) .and. (mod(i, ic) == 0 .or. exit_loop)) then
      if(cbfunc(i, evcount, gebest, unscaling(n, gxbest(1:n))) < 0) then
        exit_loop = .true.
      end if
    end if

    !===== Print intermediate conditions =====
    if(flag_off(flag, f_quiet) .and. (ir > 0) &
&                              .and. (mod(i, ir) == 0 .or. exit_loop)) then
      print *, 'PSO ---', i, gebest * sign, evmean(np, p) * sign, &
&                               infeasible(np, p), evcount
    end if

    !===== Exit loop =====
    if(exit_loop) exit

    !===== Move particles =====
    do j = 1, np
      call  p_move(n, gxbest, w, c1, c2, c3, vmax, vrand, flag, p(j))
    end do

    i = i + 1
  end do
  !*********************** Main Loop *************************
  !***********************************************************


  !===== Unscaling parameters =====
  xopti = unscaling(n, gxbest)
  ropti = gebest * sign

  !===== Print exit message =====
  if(flag_off(flag, f_quiet)) then
    print *, 'PSO --- finish ---'
    print *
  end if

end subroutine pso

!----------------------------------------------------------------------
! Move Particle
!----------------------------------------------------------------------
subroutine p_move(n, gxbest, w, c1, c2, c3, vmax, vrand, flag, p)
  use util_random, only : grand, vngrand
  integer,      intent(in)    :: n             ! number of parameter
  real,         intent(in)    :: gxbest(1:n)   ! swarm best position
  real,         intent(in)    :: w             ! inertia parameter
  real,         intent(in)    :: c1            ! self intention parameter
  real,         intent(in)    :: c2            ! swarm intention parameter
  real,         intent(in)    :: c3            ! random search parameter
  real,         intent(in)    :: vmax          ! limit of vector
  real,         intent(in)    :: vrand         ! velocity parametebtion parameter
  integer,      intent(in)    :: flag          ! control flag
  type(type_p), intent(inout) :: p             ! current particle
  real    :: r1, r2, r3
  real    :: x(1:n), v(1:n), v0(1:n), v1(1:n), v2(1:n), v3(1:n)
  real    :: vv

  !===== Prep. random number =====
  r1 = grand()
  r2 = grand()
  r3 = 1.0
  v3(1:n) = 0.0

  !---- Velocity random expansion (optional) ----
  if(flag_on(flag, f_vrand)) then
    r3 = (1.0 - vrand) + grand() * vrand
  end if

  !---- Local random search (optional) ----
  if(flag_on(flag, f_lrand)) then
    v3(1:n) = c3 * vngrand(n)
  end if

  !===== Calculate velocity vector =====
  v0(1:n) = w * p%v(1:n)
  v1(1:n) = c1 * r1 * (p%xbest(1:n) - p%x(1:n))
  v2(1:n) = c2 * r2 * (gxbest(1:n)  - p%x(1:n))
  v(1:n)  = v0(1:n) + v1(1:n) + v2(1:n)

  !===== Limit velocity length (optional) =====
  if(flag_on(flag, f_vlimit)) then
    vv = vabs(n, v)
    if(vv > vmax) then
      v(1:n) = v(1:n) * vmax / vv
    end if
  end if

  !===== Update velocity vector =====
  v(1:n) = r3 * v(1:n) + v3(1:n)

  !===== perturbation =====
  vv = vabs(n, v)
  if(vv == 0.0) then
    v(1:n) = c3 * vngrand(n)
  end if

  !===== Update position =====
  x(1:n)  = p%x(1:n) + v(1:n)

  !===== Reflection =====
  call reflect(n, x, v)

  !===== Update particle =====
  p%x(1:n) = x(1:n)
  p%v(1:n) = v(1:n)

end subroutine p_move

!----------------------------------------------------------------------
! Reflection
!----------------------------------------------------------------------
subroutine reflect(n, x, v)
  integer, intent(in)    :: n
  real,    intent(inout) :: x(1:n), v(1:n)
  integer :: i
  logical :: retry
  retry = .true.
  do while(retry)
    retry = .false.
    do i = 1, n
      if(x(i) < 0.0) then
        x(i) = -1.0 * x(i)
        !x(i) = 0.0
        v(i) = -1.0 * v(i)
        retry = .true.
      else if(x(i) > 1.0) then
        x(i) = 2.0 - 1.0 * x(i)
        !x(i) = 1.0
        v(i) = -1.0 * v(i)
        retry = .true.
      end if
    end do
  end do
end subroutine reflect

!----------------------------------------------------------------------
! Calculate Vector Length
!----------------------------------------------------------------------
function vabs(n, v) result(r)
  integer, intent(in) :: n
  real,    intent(in) :: v(:)
  real                :: r
  integer :: i
  r = 0.0
  do i = 1, n
    r = r + v(i) ** 2
  end do
  r = sqrt(r)
end function vabs

!----------------------------------------------------------------------
! Check Arguments
!----------------------------------------------------------------------
subroutine check_param(n, xmin, xmax, xinit, np, w, c1, c2, c3, vmax, it, flag)
  integer, intent(in)  :: n, np, it, flag
  real,    intent(in)  :: xmin(1:n) ,xmax(1:n), xinit(1:n)
  real,    intent(in)  :: w, c1, c2, c3, vmax
  integer :: i, err

  err = 0

  if(n > max_nparam) then
    print *, 'pso: error: n > max_nparam', n
    err = err + 1
  end if
  if(np < 1) then
    print *, 'pso: error: np < 1', np
    err = err + 1
  end if
  if(w < 0.0) then
    print *, 'pso: error: w < 0', w
    err = err + 1
  end if
  if(c1 < 0.0) then
    print *, 'pso: error: c1 < 0', c1
    err = err + 1
  end if
  if(c2 < 0.0) then
    print *, 'pso: error: c2 < 0', c2
    err = err + 1
  end if
  if(c3 < 0.0) then
    print *, 'pso: error: c3 < 0', c3
    err = err + 1
  end if
  if(vmax <= 0.0) then
    print *, 'pso: error: Rmax <= 0', vmax
    err = err + 1
  end if
  if(it < 0) then
    print *, 'pso: error: it < 0', it
    err = err + 1
  end if

  do i=1, n
    if(xmax(i) < xmin(i)) then
      print *, 'pso: error: xmax < xmin', i
      err = err + 1
    end if
    if(flag_on(flag, f_initp)) then
      if(xinit(i) < xmin(i)) then
        print *, 'pso: error: xinit < xmin', i, xinit(i), xmin(i)
        err = err + 1
      endif
      if(xinit(i) > xmax(i)) then
        print *, 'pso: error: xinit > xmax', i, xinit(i), xmax(i)
        err = err + 1
      endif
    end if
  end do

  if(err > 0) then
    stop
  end if
end subroutine check_param

!----------------------------------------------------------------------
! Print PSO Parameters
!----------------------------------------------------------------------
subroutine print_param(n, np, w, c1, c2, c3, vmax, it, ie, flag)
  integer, intent(in) :: n, np, it, ie, flag
  real,    intent(in) :: w, c1, c2, c3, vmax
  print *
  print *, '****** PSO parameters ******'
  print *, 'n    :', n
  print *, 'np   :', np
  print *, 'w    :', w
  print *, 'c1   :', c1
  print *, 'c2   :', c2
  print *, 'c3   :', c3
  print *, 'Vmax :', vmax
  print *, 'it   :', it
  print *, 'ie   :', ie
  print *, 'flag_quiet   :', chk_flag(flag, f_quiet)
  print *, 'flag_initp   :', chk_flag(flag, f_initp)
  print *, 'flag_vlimit  :', chk_flag(flag, f_vlimit)
  print *, 'flag_lrand   :', chk_flag(flag, f_lrand)
  print *, 'flag_vrand   :', chk_flag(flag, f_vrand)
  print *, 'flag_gcenter :', chk_flag(flag, f_gcenter)
  print *, 'flag_gjump   :', chk_flag(flag, f_gjump)
  print *, 'flag_qmcinit :', chk_flag(flag, f_qmcinit)
  print *
end subroutine print_param

!----------------------------------------------------------------------
! Initialize Particles
!----------------------------------------------------------------------
subroutine init_particles(n, xinit, np, flag, p)
  use util_random, only : vgrand
  use util_qmc,    only : hammersley
  integer,      intent(in)    :: n             ! number of parameter
  real,         intent(in)    :: xinit(1:n)    ! initial parameters
  integer,      intent(in)    :: np            ! number of particle
  integer,      intent(in)    :: flag          ! control flag
  type(type_p), intent(out)   :: p(1:np)       ! particles
  integer :: i, j
  integer :: na, ip(1:n)
  real    :: x(1:np * n)

  if(flag_on(flag, f_qmcinit)) then
    na = 0                           ! number of effective dimension
    do j=1, n
      if(scale_a(j) > 0.0) then      ! not fixed parameter
        na = na + 1
        ip(j) = na
      end if
    end do
    call hammersley(np, na, x)
    do i=1, np
      do j=1, n
        if(scale_a(j) > 0.0) then
          p(i)%x(j) = x((i-1)*na + ip(j))
        else
          p(i)%x(j) = 0.0
        end if
      end do
    end do
  else
    do i = 1, np
      p(i)%x(1:n) = vgrand(n)
    end do
  end if

  if(flag_on(flag, f_initp)) then
    p(1)%x(1:n) = scaling(n, xinit)
    p(1)%v(1:n) = 0.0
    p(1)%evbest = void
    p(1)%xbest  = p(1)%x
  end if

  do i = 1, np
    p(i)%v(1:n) = 0.0
    p(i)%evbest = void
    p(i)%xbest(1:n) = p(i)%x(1:n)
  end do

end subroutine init_particles

!----------------------------------------------------------------------
! Call Evaluation Function
!----------------------------------------------------------------------
function evaluate(n, x, sign, evfunc, evcount) result(r)
  integer, intent(in)    :: n
  real,    intent(in)    :: x(:)
  integer, intent(in)    :: sign
  integer, intent(inout) :: evcount
  real                   :: r
  interface
    integer function evfunc(x, r)
      real, intent(in)  :: x(:)
      real, intent(out) :: r
    end function evfunc
  end interface
  if(evfunc(unscaling(n, x), r) == 0) then
    r = r * sign
  else
    r = constrained
  end if
  evcount = evcount + 1
end function evaluate

!----------------------------------------------------------------------
! Sort Particles by Evaluation
!----------------------------------------------------------------------
subroutine psort(np, p, ip)
  use util_sort, only : qsort
  integer,      intent(in)  :: np
  type(type_p), intent(in)  :: p(1:np)
  integer,      intent(out) :: ip(1:np)
  real    :: x(1:np)
  x(1:np) = p(1:np)%ev
  call qsort(x, ip, -1)
end subroutine psort

!----------------------------------------------------------------------
! Calculate Gravity Center of Particles
!----------------------------------------------------------------------
function gcenter(n, np, p, ip) result(x)
  integer,      intent(in) :: n           ! number of parameter
  integer,      intent(in) :: np          ! number of particle
  type(type_p), intent(in) :: p(:)        ! particles
  integer,      intent(in) :: ip(:)       ! index of particles
  real                     :: x(1:n)
  integer :: i
  x(1:n) = 0.0
  do i = 1, np
    x(1:n) = x(1:n) + p(ip(i))%x(1:n)
  end do
  x(1:n) = x(1:n) / real(np)
end function gcenter

!----------------------------------------------------------------------
! Calculate Mean Evaluation Value
!----------------------------------------------------------------------
function evmean(np, p) result(r)
  integer,      intent(in) :: np
  type(type_p), intent(in) :: p(1:np)
  real                     :: x, r
  integer :: i, j
  r = 0.0
  j = 0
  do i = 1, np
    x = p(i)%ev
    if(x > constrained) then
      r = r + x
      j = j + 1
    end if
  end do
  if(j > 0) then
    r = r / real(j)
  else
    r = constrained
  end if
end function evmean

!----------------------------------------------------------------------
! Calculate Ratio of Restricted Particles
!----------------------------------------------------------------------
function infeasible(np, p) result(r)
  integer,      intent(in) :: np
  type(type_p), intent(in) :: p(1:np)
  integer                  :: r
  integer :: i, j
  j = 0
  do i = 1, np
    if(p(i)%ev <= constrained) then
      j = j + 1
    end if
  end do
  r = int(real(j) / real(np) * 100.0)
end function infeasible

!----------------------------------------------------------------------
! Initialize Parameter Scaling Functions
!----------------------------------------------------------------------
subroutine init_scaling(n, xmin, xmax)
  integer, intent(in) :: n
  real,    intent(in) :: xmin(1:n), xmax(1:n)
  scale_a(1:n) = xmax(1:n) - xmin(1:n)
  scale_b(1:n) = xmin(1:n)
end subroutine init_scaling

!----------------------------------------------------------------------
! Parameter Scaling (normalize)
!----------------------------------------------------------------------
function scaling(n, x) result(px)
  integer, intent(in) :: n
  real,    intent(in) :: x(1:n)
  real                :: px(1:n)
  px(1:n) = 0.0
  where(scale_a /= 0.0) px(1:n) = (x(1:n) - scale_b(1:n)) / scale_a(1:n)
end function scaling

!----------------------------------------------------------------------
! Parameter Unscaling (denormalize)
!----------------------------------------------------------------------
function unscaling(n, px) result(x)
  integer, intent(in) :: n
  real,    intent(in) :: px(1:n)
  real                :: x(1:n)
  x(1:n) = px(1:n) * scale_a(1:n) + scale_b(1:n)
end function unscaling

!----------------------------------------------------------------------
! Check Flag
!----------------------------------------------------------------------
function chk_flag(flag, mask) result(r)
  integer, intent(in) :: flag, mask
  integer             :: r
  if(mask <= 0) then
    print *, 'error: invalid mask for chk_flag'
    stop
  end if
  r = mod(flag / mask, 2)
end function chk_flag

!----------------------------------------------------------------------
! Check Flag is ON
!----------------------------------------------------------------------
function flag_on(flag, mask) result(r)
  integer, intent(in) :: flag, mask
  logical             :: r
  if(chk_flag(flag, mask) > 0) then
    r = .true.
  else
    r = .false.
  endif
end function flag_on

!----------------------------------------------------------------------
! Check Flag is OFF
!----------------------------------------------------------------------
function flag_off(flag, mask) result(r)
  integer, intent(in) :: flag, mask
  logical             :: r
  r = .not. flag_on(flag, mask)
end function flag_off

!----------------------------------------------------------------------
! Dummy (sample) callback function
!----------------------------------------------------------------------
function pso_cbdummy(it, iev, ev, x) result(r)
  integer, intent(in) :: it    ! iteration counter
  integer, intent(in) :: iev   ! current evaluation counter
  real,    intent(in) :: ev    ! current evaluation value
  real,    intent(in) :: x(:)  ! current parameter value
  integer             :: r     ! continuation flag (>=0: cont., <0: quit)
  real, parameter :: eps = 1.e-5
  print *, 'PSO ---', it, ev, iev, x(:)
  if (ev < eps) then
    r = -1
  else
    r = 1
  end if
end function pso_cbdummy


end module opti_pso