!=======================================================================
! StochOptim provides user friendly functions to solve optimization
! problems using stochastic algorithms.
!
! Created by
!     Keurfon Luu <keurfon.luu@mines-paristech.fr>
!     MINES ParisTech - Centre de Géosciences
!     PSL - Research University
!=======================================================================

module stochoptim

  use forlab, only: IPRE, RPRE, progress_perc, num2str, randu, randn, &
    randi, randperm, norm, argsort, zeros, ones, eye, prctile, mean, &
    median, diag, signum, argsort, triu, eig, savebin
#ifdef do_mpi
  use forlab, only: mpi_rpre
  use mpi
#endif

  implicit none

  private
  public :: optifunc, EvolutionaryKeywords, MonteCarloKeywords, &
    Evolutionary, MonteCarlo

  abstract interface
    real(kind = RPRE) function optifunc(x)
      use forlab, only : RPRE
      import
      real(kind = RPRE), dimension(:), intent(in) :: x
    end function optifunc
  end interface

  type EvolutionaryKeywords
    integer(kind = IPRE) :: popsize = 10, max_iter = 100, verbose = 0
    real(kind = RPRE) :: eps1 = 1d-8, eps2 = 1d-8, w = 0.7298, c1 = 1.49618, &
      c2 = 1.49618, gamma = 1., F = 0.5, CR = 0.1, sigma = 0.5, mu_perc = 0.5
    logical :: constrain = .true., snap = .false.
    character(len = 5) :: solver, strategy
  contains
    procedure, private, pass :: print_EvolutionaryKeywords
    generic, public :: print => print_EvolutionaryKeywords
  end type EvolutionaryKeywords

  type MonteCarloKeywords
    integer(kind = IPRE) :: max_iter = 1000, verbose = 0
    real(kind = RPRE) :: perc
    logical :: constrain = .true.
    real(kind = RPRE), dimension(:), allocatable :: stepsize, xstart
    character(len = 4) :: sampler
  contains
    procedure, private, pass :: print_MonteCarloKeywords
    generic, public :: print => print_MonteCarloKeywords
  end type MonteCarloKeywords

  type Evolutionary
    procedure(optifunc), pointer, nopass :: func
    real(kind = RPRE), dimension(:), allocatable :: lower, upper
    integer(kind = IPRE) :: n_dim, popsize = 10, max_iter = 100, verbose = 0
    real(kind = RPRE) :: eps1 = 1e-8, eps2 = 1e-8
    logical :: constrain = .true., snap = .false.
    character(len = 5) :: solver, strategy

    real(kind = RPRE), dimension(:), allocatable :: xopt
    real(kind = RPRE) :: gfit
    integer(kind = IPRE) :: iflag, n_iter, n_eval, n_restart
    real(kind = RPRE), dimension(:,:,:), allocatable :: models
    real(kind = RPRE), dimension(:,:), allocatable :: energy, means
    real(kind = RPRE), dimension(:), allocatable :: fitness, mu_scale, std_scale
    real(kind = RPRE) :: w, c1, c2, gamma, F, CR, sigma, mu_perc

    integer(kind = IPRE) :: mpi_rank = 0, mpi_size = 1, mpi_ierr
  contains
    procedure, private, pass :: print_Evolutionary_parameters, print_Evolutionary_result, &
      print_Evolutionary, flag, standardize1_evolutionary, standardize2_evolutionary, &
      unstandardize1_evolutionary, unstandardize2_evolutionary, init_models_evolutionary, &
      eval_models, constrain_de, constrain_cpso, de_mutation, de, cpso, cmaes, &
      save_models_evolutionary
    generic, private :: init_models => init_models_evolutionary
    generic, private :: standardize => standardize1_evolutionary, standardize2_evolutionary
    generic, private :: unstandardize => unstandardize1_evolutionary, unstandardize2_evolutionary
    generic, public :: print_parameters => print_Evolutionary_parameters
    generic, public :: print_result => print_Evolutionary_result
    generic, public :: print => print_Evolutionary
    generic, public :: save_models => save_models_evolutionary
    procedure, public, pass :: optimize
  end type Evolutionary

  type MonteCarlo
    procedure(optifunc), pointer, nopass :: func
    real(kind = RPRE), dimension(:), allocatable :: lower, upper
    integer(kind = IPRE) :: n_dim, n_dim_per_iter, max_iter = 1000, verbose = 0
    logical :: constrain = .true.
    character(len = 4) :: sampler

    real(kind = RPRE), dimension(:), allocatable :: xopt
    real(kind = RPRE) :: gfit, acceptance_ratio, perc
    real(kind = RPRE), dimension(:,:), allocatable :: models
    real(kind = RPRE), dimension(:), allocatable :: energy
    real(kind = RPRE), dimension(:), allocatable :: mu_scale, std_scale
  contains
    procedure, private, pass :: print_MonteCarlo_parameters, print_MonteCarlo_result, &
      print_MonteCarlo, standardize_montecarlo, unstandardize1_montecarlo, unstandardize2_montecarlo, &
      init_models_montecarlo, in_search_space, mcmc, save_models_montecarlo
    generic, private :: init_models => init_models_montecarlo
    generic, private :: standardize => standardize_montecarlo
    generic, private :: unstandardize => unstandardize1_montecarlo, unstandardize2_montecarlo
    generic, public :: print_parameters => print_MonteCarlo_parameters
    generic, public :: print_result => print_MonteCarlo_result
    generic, public :: print => print_MonteCarlo
    generic, public :: save_models => save_models_montecarlo
    procedure, public, pass :: sample
  end type MonteCarlo

  interface EvolutionaryKeywords
    module procedure init_EvolutionaryKeywords
  end interface EvolutionaryKeywords

  interface MonteCarloKeywords
    module procedure init_MonteCarloKeywords
  end interface MonteCarloKeywords

  interface Evolutionary
    module procedure init_Evolutionary
  end interface Evolutionary

  interface MonteCarlo
    module procedure init_MonteCarlo
  end interface MonteCarlo

contains

  function init_EvolutionaryKeywords(popsize, max_iter, eps1, eps2, constrain, snap, solver, &
                                     w, c1, c2, gamma, F, CR, strategy, sigma, mu_perc, verbose) result(evo_kws)
    type(EvolutionaryKeywords) :: evo_kws
    integer(kind = IPRE), intent(in), optional :: popsize, max_iter, verbose
    real(kind = RPRE), intent(in), optional :: eps1, eps2, w, c1, c2, gamma, &
      F, CR, sigma, mu_perc
    logical, intent(in), optional :: constrain, snap
    character(len = *), intent(in), optional :: solver, strategy

    if ( present(popsize) ) evo_kws % popsize = popsize
    if ( present(max_iter) ) evo_kws % max_iter = max_iter
    if ( present(eps1) ) evo_kws % eps1 = eps1
    if ( present(eps2) ) evo_kws % eps2 = eps2
    if ( present(constrain) ) evo_kws % constrain = constrain
    if ( present(snap) ) evo_kws % snap = snap
    if ( present(w) ) evo_kws % w = w
    if ( present(c1) ) evo_kws % c1 = c1
    if ( present(c2) ) evo_kws % c2 = c2
    if ( present(gamma) ) evo_kws % gamma = gamma
    if ( present(F) ) evo_kws % F = F
    if ( present(CR) ) evo_kws % CR = CR
    if ( present(sigma) ) evo_kws % sigma = sigma
    if ( present(mu_perc) ) evo_kws % mu_perc = mu_perc
    if ( present(verbose) ) evo_kws % verbose = verbose
    if ( present(solver) ) then
      evo_kws % solver = trim(solver)
    else
      evo_kws % solver = "cpso"
    end if
    if ( present(strategy) ) then
      evo_kws % strategy = trim(strategy)
    else
      evo_kws % strategy = "rand1"
    end if
    return
  end function init_EvolutionaryKeywords

  subroutine print_EvolutionaryKeywords(self)
    class(EvolutionaryKeywords), intent(inout) :: self
    character(len = 18) :: attributes

    attributes = "solver:"
    print *, adjustr(attributes) // " '" // trim(self % solver) // "'"
    attributes = "parameters:"
    select case(trim(self % solver))
    case("pso")
      print *, adjustr(attributes) &
        // " w = " // num2str(self % w, "(F5.2)") &
        // ", c1 = " // num2str(self % c1, "(F5.2)") &
        // ", c2 = " // num2str(self % c2, "(F5.2)")
    case("cpso")
      print *, adjustr(attributes) &
        // " w = " // num2str(self % w, "(F5.2)") &
        // ", c1 = " // num2str(self % c1, "(F5.2)") &
        // ", c2 = " // num2str(self % c2, "(F5.2)") &
        // ", gamma = " // num2str(self % gamma, "(F5.2)")
    case("de")
      print *, adjustr(attributes) &
        // " F = " // num2str(self % F, "(F5.2)") &
        // ", CR = " // num2str(self % CR, "(F5.2)") &
        // ", strategy = '" // trim(self % strategy) // "'"
    case("cmaes")
      print *, adjustr(attributes) &
        // " sigma = " // num2str(self % sigma, "(F5.2)") &
        // ", mu_perc = " // num2str(self % mu_perc, "(F5.2)")
    end select
    attributes = "popsize:"
    print *, adjustr(attributes) // " " // num2str(self % popsize)
    attributes = "max_iter:"
    print *, adjustr(attributes) // " " // num2str(self % max_iter)
    return
  end subroutine print_EvolutionaryKeywords

  function init_MonteCarloKeywords(max_iter, perc, constrain, sampler, &
                                   stepsize, xstart, verbose) result(mc_kws)
    type(MonteCarloKeywords) :: mc_kws

    integer(kind = IPRE), intent(in), optional :: max_iter, verbose
    real(kind = RPRE), intent(in), optional :: perc
    logical, intent(in), optional :: constrain
    character(len = *), intent(in), optional :: sampler
    real(kind = RPRE), dimension(:), intent(in), optional :: stepsize, xstart

    if ( present(max_iter) ) mc_kws % max_iter = max_iter
    if ( present(perc) ) mc_kws % perc = perc
    if ( present(constrain) ) mc_kws % constrain = constrain
    if ( present(stepsize) ) mc_kws % stepsize = stepsize
    if ( present(xstart) ) mc_kws % xstart = xstart
    if ( present(verbose) ) mc_kws % verbose = verbose
    if ( present(sampler) ) then
      mc_kws % sampler = trim(sampler)
    else
      mc_kws % sampler = "mcmc"
    end if
    return
  end function init_MonteCarloKeywords

  subroutine print_MonteCarloKeywords(self)
    class(MonteCarloKeywords), intent(inout) :: self
    character(len = 18) :: attributes

    attributes = "sampler:"
    print *, adjustr(attributes) // " '" // trim(self % sampler) // "'"
    attributes = "max_iter:"
    print *, adjustr(attributes) // " " // num2str(self % max_iter)
    return
  end subroutine print_MonteCarloKeywords

  function init_Evolutionary(func, lower, upper, popsize, max_iter, eps1, eps2, &
                             constrain, snap, verbose) result(ea)
    type(Evolutionary) :: ea
    procedure(optifunc) :: func
    real(kind = RPRE), dimension(:), intent(in) :: lower, upper
    integer(kind = IPRE), intent(in), optional :: popsize, max_iter, verbose
    real(kind = RPRE), intent(in), optional :: eps1, eps2
    logical, intent(in), optional :: constrain, snap

    ea % func => func
    if ( size(lower) .ne. size(upper) ) then
      print *, "Error: lower and upper must have the same length"
      stop
    else if ( any(lower .gt. upper) ) then
      print *, "Error: upper must be greater than lower"
      stop
    else
      ea % lower = lower
      ea % upper = upper
      ea % n_dim = size(lower)
    end if

    if ( present(popsize) ) then
      if ( popsize .lt. 2 ) then
        print *, "Error: popsize must be an integer > 1"
        stop
      else
        ea % popsize = popsize
      end if
    end if

    if ( present(max_iter) ) then
      if ( max_iter .lt. 1 ) then
        print *, "Error: max_iter must be a positive integer"
        stop
      else
        ea % max_iter = max_iter
      end if
    end if

    if ( present(verbose) ) then
      if ( verbose .ne. 0 .and. verbose .ne. 1 ) then
        print *, "Error: verbose must be either 0 or 1"
        stop
      else
        ea % verbose = verbose
      end if
    end if

    if ( present(eps1) ) then
      if ( eps1 .lt. 0. ) then
        print *, "Error: eps1 must be positive"
        stop
      else
        ea % eps1 = eps1
      end if
    end if

    if ( present(eps2) ) ea % eps2 = eps2
    if ( present(constrain) ) ea % constrain = constrain
    if ( present(snap) ) ea % snap = snap
    return
  end function init_Evolutionary

  subroutine print_Evolutionary_parameters(self)
    class(Evolutionary), intent(inout) :: self
    character(len = 18) :: attributes
    type(EvolutionaryKeywords) :: evo_kws

    if ( self % mpi_rank .eq. 0 ) then
      if ( allocated(self % xopt) ) then
        evo_kws = EvolutionaryKeywords(&
          solver = self % solver, &
          popsize = self % popsize, &
          max_iter = self % max_iter, &
          w = self % w, &
          c1 = self % c1, &
          c2 = self % c2, &
          gamma = self % gamma, &
          F = self % F, &
          CR = self % CR, &
          strategy = self % strategy, &
          sigma = self % sigma, &
          mu_perc = self % mu_perc)
        call evo_kws % print()
        attributes = "n_dim:"
        print *, adjustr(attributes) // " " // num2str(self % n_dim)
      end if
    end if
    return
  end subroutine print_Evolutionary_parameters

  subroutine print_Evolutionary_result(self)
    class(Evolutionary), intent(inout) :: self
    integer(kind = IPRE) :: i
    character(len = 18) :: attributes

    if ( self % mpi_rank .eq. 0 ) then
      if ( allocated(self % xopt) ) then
        attributes = "solution:"
        print *, adjustr(attributes)
        do i = 1, self % n_dim
          print *, char(9), char(9), char(9), self % xopt(i)
        end do
        attributes = "fitness:"
        print *, adjustr(attributes) // " " // num2str(self % gfit)
        attributes = "n_iter:"
        print *, adjustr(attributes) // " " // num2str(self % n_iter)
        attributes = "n_eval:"
        print *, adjustr(attributes) // " " // num2str(self % n_eval)
        if ( trim(self % solver) .eq. "cpso" ) then
          attributes = "n_restart:"
          print *, adjustr(attributes) // " " // num2str(self % n_restart)
        end if
        attributes = "flag:"
        print *, adjustr(attributes) // " " // self % flag()
      end if
    end if
    return
  end subroutine print_Evolutionary_result

  subroutine print_Evolutionary(self)
    class(Evolutionary), intent(inout) :: self

    call self % print_parameters()
    call self % print_result()
    return
  end subroutine print_Evolutionary

  function init_MonteCarlo(func, lower, upper, max_iter, perc, constrain, verbose) result (mc)
    type(MonteCarlo) :: mc
    procedure(optifunc) :: func
    real(kind = RPRE), dimension(:), intent(in) :: lower, upper
    integer(kind = IPRE), intent(in), optional :: max_iter, verbose
    real(kind = RPRE), intent(in), optional :: perc
    logical, intent(in), optional :: constrain

    mc % func => func
    if ( size(lower) .ne. size(upper) ) then
      print *, "Error: lower and upper must have the same length"
      stop
    else if ( any(lower .gt. upper) ) then
      print *, "Error: upper must be greater than lower"
      stop
    else
      mc % lower = lower
      mc % upper = upper
      mc % n_dim = size(lower)
    end if

    if ( present(max_iter) ) then
      if ( max_iter .lt. 1 ) then
        print *, "Error: max_iter must be a positive integer"
        stop
      else
        mc % max_iter = max_iter
      end if
    end if

    if ( present(perc) ) then
      if ( perc .lt. 0. .or. perc .gt. 1. ) then
        print *, "Error: perc must be a scalar in [ 0, 1 ]"
        stop
      else
        mc % n_dim_per_iter = max(1, int(perc * mc % n_dim))
      end if
    else
      mc % n_dim_per_iter = mc % n_dim
    end if

    if ( present(verbose) ) then
      if ( verbose .ne. 0 .and. verbose .ne. 1 ) then
        print *, "Error: verbose must be either 0 or 1"
        stop
      else
        mc % verbose = verbose
      end if
    end if

    if ( present(constrain) ) mc % constrain = constrain
    return
  end function init_MonteCarlo

  subroutine print_MonteCarlo_parameters(self)
    class(MonteCarlo), intent(inout) :: self
    character(len = 18) :: attributes
    type(MonteCarloKeywords) :: mc_kws

    if ( allocated(self % xopt) ) then
      mc_kws = MonteCarloKeywords(&
        sampler = self % sampler, &
        max_iter = self % max_iter)
      call mc_kws % print()
      attributes = "n_dim:"
      print *, adjustr(attributes) // " " // num2str(self % n_dim)
    end if
    return
  end subroutine print_MonteCarlo_parameters

  subroutine print_MonteCarlo_result(self)
    class(MonteCarlo), intent(inout) :: self
    integer(kind = IPRE) :: i
    character(len = 18) :: attributes

    if ( allocated(self % xopt) ) then
      attributes = "solution:"
      print *, adjustr(attributes)
      do i = 1, self % n_dim
        print *, char(9), char(9), char(9), self % xopt(i)
      end do
      attributes = "fitness:"
      print *, adjustr(attributes) // " " // num2str(self % gfit)
      attributes = "acceptance_ratio:"
      print *, adjustr(attributes) // " " // num2str(self % acceptance_ratio, "(F6.2)")
    end if
    return
  end subroutine print_MonteCarlo_result

  subroutine print_MonteCarlo(self)
    class(MonteCarlo), intent(inout) :: self

    call self % print_parameters()
    call self % print_result()
    return
  end subroutine print_MonteCarlo

  function flag(self) result(str)
    character(len = :), allocatable :: str
    class(Evolutionary), intent(inout) :: self

    select case(self % iflag)
    case(-1)
      str = "maximum number of iterations is reached"
    case(0)
      str = "best individual position changes less than eps1 (" // num2str(self % eps1) // ")"
    case(1)
      str = "fitness is lower than threshold eps2 (" // num2str(self % eps2) // ")"
    case(2)
      str = "NoEffectAxis"
    case(3)
      str = "NoEffectCoord"
    case(4)
      str = "ConditionCov"
    case(5)
      str = "EqualFunValues"
    case(6)
      str = "TolXUp"
    case(7)
      str = "TolFun"
    case(8)
      str = "TolX"
    end select
    return
  end function flag

  subroutine optimize(self, solver, xstart, w, c1, c2, gamma, &
                      F, CR, strategy, sigma, mu_perc)
    class(Evolutionary), intent(inout) :: self
    character(len = *), intent(in), optional :: solver, strategy
    real(kind = RPRE), intent(in), optional :: w, c1, c2, gamma, F, CR, sigma, mu_perc
    real(kind = RPRE), dimension(:,:), intent(in), optional :: xstart
    real(kind = RPRE) :: opt_w = 0.7298, opt_c1 = 1.49618, opt_c2 = 1.49618, opt_gamma = 1., &
      opt_F = 0.5, opt_CR = 0.1, opt_sigma = 0.5, opt_mu_perc = 0.5
    real(kind = RPRE), dimension(:,:), allocatable :: opt_xstart
    character(len = :), allocatable :: opt_solver, opt_strategy
    integer(kind = 4) :: master = 0, num_procs, rank, ierr

    ! Check inputs
    if ( present(solver) ) then
      if ( trim(solver) .ne. "pso" .and. trim(solver) .ne. "cpso" &
        .and. trim(solver) .ne. "de" .and. trim(solver) .ne. "cmaes" ) then
        print *, "Error: solver must be either 'cpso', 'pso', 'de' or 'cmaes'"
        stop
      else
        opt_solver = trim(solver)
      end if
    else
      opt_solver = "cpso"
    end if
    if ( present(xstart) ) then
      select case(opt_solver)
      case("cmaes")
        if ( size(xstart, 1) .ne. 1 &
          .or. size(xstart, 2) .ne. self % n_dim ) then
          print *, "Error: xstart must be of shape [ 1, " // num2str(self % n_dim) &
            // " ] for '" // opt_solver // "'"
          stop
        else
          opt_xstart = xstart
        end if
      case default
        if ( size(xstart, 1) .ne. self % popsize &
          .or. size(xstart, 2) .ne. self % n_dim ) then
          print *, "Error: xstart must be of shape [ " // num2str(self % popsize) &
            // ", " // num2str(self % n_dim) // " ] for '" // opt_solver // "'"
          stop
        else
          opt_xstart = xstart
        end if
      end select
    end if
    if ( present(w) ) then
      if ( w .lt. 0. .or. w .gt. 1. ) then
        print *, "Error: w must be a scalar in [ 0, 1 ]"
        stop
      else
        opt_w = w
      end if
    end if
    if ( present(c1) ) then
      if ( ( c1 .lt. 0. ) .or. ( c1 .gt. 4. ) ) then
        print *, "Error: c1 must be a scalar in [ 0, 4 ]"
        stop
      else
        opt_c1 = c1
      end if
    end if
    if ( present(c2) ) then
      if ( c2 .lt. 0. .or. c2 .gt. 4. ) then
        print *, "Error: c2 must be a scalar in [ 0, 4 ]"
        stop
      else
        opt_c2 = c2
      end if
    end if
    if ( present(gamma) ) then
      if ( gamma .lt. 0. .or. gamma .gt. 2. ) then
        print *, "Error: gamma must be a scalar in [ 0, 2 ]"
        stop
      else
        opt_gamma = gamma
      end if
    end if
    if ( present(CR) ) then
      if ( ( CR .lt. 0. ) .or. ( CR .gt. 1. ) ) then
        print *, "Error: CR must be a scalar in [ 0, 1 ]"
        stop
      else
        opt_CR = CR
      end if
    end if
    if ( present(F) ) then
      if ( F .lt. 0. .or. F .gt. 2. ) then
        print *, "Error: F must be a scalar in [ 0, 2 ]"
        stop
      else
        opt_F = F
      end if
    end if
    if ( present(strategy) ) then
      if ( trim(strategy) .ne. "rand1" .and. trim(strategy) .ne. "rand2" &
        .and. trim(strategy) .ne. "best1" .and. trim(strategy) .ne. "best2" ) then
        print *, "Error: solver must be either 'rand1', 'rand2', 'best1' or 'best2'"
        stop
      else
        opt_strategy = trim(strategy)
      end if
    else
      opt_strategy = "rand1"
    end if
    if ( present(sigma) ) then
      if ( sigma .le. 0. ) then
        print *, "Error: sigma must be positive"
        stop
      else
        opt_sigma = sigma
      end if
    end if
    if ( present(mu_perc) ) then
      if ( mu_perc .le. 1. / self % popsize .or. mu_perc .gt. 1. ) then
        print *, "Error: mu_perc must be a scalar in ] " // num2str(1. / self % popsize) // ", 1 ]"
        stop
      else
        opt_mu_perc = mu_perc
      end if
    end if

    ! Check population size
    select case(opt_solver)
    case("de")
      if ( self % popsize .le. 3 .and. ( opt_strategy .eq. "rand1" .or. opt_strategy .eq. "rand2" ) ) then
        print *, "Error: popsize cannot be lower than 4 for DE '" // opt_strategy // "' strategy"
        stop
      else if ( self % popsize .le. 4 .and. opt_strategy .eq. "best2" ) then
        print *, "Error: popsize cannot be lower than 5 for DE '" // opt_strategy // "' strategy"
        stop
      else if ( self % popsize .le. 5 .and. opt_strategy .eq. "rand2" ) then
        print *, "Error: popsize cannot be lower than 6 for DE '" // opt_strategy // "' strategy"
        stop
      end if
    case("cmaes")
      if ( self % popsize .le. 3 ) then
        print *, "Error: popsize cannot be lower than 4 for CMA-ES"
        stop
      end if
    end select

    ! Initialize
    self % solver = opt_solver
    self % n_iter = 0
    self % n_eval = 0
    self % n_restart = 0
    self % mu_scale = 0.5 * (self % upper + self % lower)
    self % std_scale = 0.5 * (self % upper - self % lower)
#ifdef do_mpi
    call mpi_comm_size(mpi_comm_world, self % mpi_size, self % mpi_ierr)
    call mpi_comm_rank(mpi_comm_world, self % mpi_rank, self % mpi_ierr)
#endif

    ! Solve
    select case(opt_solver)
    case("pso")
      self % w = opt_w
      self % c1 = opt_c1
      self % c2 = opt_c2
      call self % cpso(w = opt_w, c1 = opt_c1, c2 = opt_c2, gamma = real(0., RPRE), xstart = opt_xstart)
    case("cpso")
      self % w = opt_w
      self % c1 = opt_c1
      self % c2 = opt_c2
      self % gamma = opt_gamma
      call self % cpso(w = opt_w, c1 = opt_c1, c2 = opt_c2, gamma = opt_gamma, xstart = opt_xstart)
    case("de")
      self % CR = opt_CR
      self % F = opt_F
      self % strategy = opt_strategy
      call self % de(CR = opt_CR, F = opt_F, strategy = opt_strategy, xstart = opt_xstart)
    case("cmaes")
      self % sigma = opt_sigma
      self % mu_perc = opt_mu_perc
      call self % cmaes(sigma = opt_sigma, mu_perc = opt_mu_perc, xstart = opt_xstart)
    end select
    return
  end subroutine optimize

  function standardize1_evolutionary(self, models) result(out)
    real(kind = RPRE), dimension(:), allocatable :: out
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:), intent(in) :: models
    real(kind = RPRE), dimension(:), allocatable :: std_scale
    logical, dimension(:), allocatable :: mask

    mask = self % std_scale .ne. 0.
    std_scale = merge(self % std_scale, real(1., RPRE), mask)
    out = ( models - self % mu_scale ) / std_scale
    return
  end function standardize1_evolutionary

  function standardize2_evolutionary(self, models) result(out)
    real(kind = RPRE), dimension(:,:), allocatable :: out
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:,:), intent(in) :: models
    real(kind = RPRE), dimension(:), allocatable :: std_scale
    logical, dimension(:), allocatable :: mask

    mask = self % std_scale .ne. 0.
    std_scale = merge(self % std_scale, real(1., RPRE), mask)
    out = ( models - spread(self % mu_scale, 1, self % popsize) ) &
          / spread(std_scale, 1, self % popsize)
    return
  end function standardize2_evolutionary

  function unstandardize1_evolutionary(self, models) result(out)
    real(kind = RPRE), dimension(:), allocatable :: out
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:), intent(in) :: models
    out = models * self % std_scale + self % mu_scale
    return
  end function unstandardize1_evolutionary

  function unstandardize2_evolutionary(self, models) result(out)
    real(kind = RPRE), dimension(:,:), allocatable :: out
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:,:), intent(in) :: models
    out = models * spread(self % std_scale, 1, self % popsize) &
          + spread(self % mu_scale, 1, self % popsize)
    return
  end function unstandardize2_evolutionary

  subroutine init_models_evolutionary(self)
    class(Evolutionary), intent(inout) :: self
    if ( .not. allocated(self % models) ) then
      allocate(self % models(self % popsize, self % n_dim, self % max_iter))
      allocate(self % energy(self % popsize, self % max_iter))
      allocate(self % fitness(self % max_iter))
    else
      self % models = 0.
      self % energy = 0.
      self % fitness = 0.
    end if
    return
  end subroutine init_models_evolutionary

  function eval_models(self, models) result(fit)
    real(kind = RPRE), dimension(:), allocatable :: fit
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:), allocatable :: fit_mpi
    real(kind = RPRE), dimension(:,:), intent(in) :: models
    integer(kind = IPRE) :: i, n

    n = size(models, 1)
#ifdef do_mpi
    allocate(fit(n), fit_mpi(n))
    fit = 0.
    fit_mpi = 0.
    call mpi_barrier(mpi_comm_world, self % mpi_ierr)
    call mpi_bcast(models, n * self % n_dim, mpi_rpre(), 0, mpi_comm_world, self % mpi_ierr)
    do i = self % mpi_rank+1, n, self % mpi_size
      fit_mpi(i) = self % func(self % unstandardize(models(i,:)))
    end do
    call mpi_barrier(mpi_comm_world, self % mpi_ierr)
    call mpi_allreduce(fit_mpi, fit, n, mpi_rpre(), mpi_sum, mpi_comm_world, self % mpi_ierr)
#else
    fit = [ ( self % func(self % unstandardize(models(i,:))), i = 1, n ) ]
#endif
    return
  end function eval_models

  function constrain_de(self, models) result(out)
    real(kind = RPRE), dimension(:,:), allocatable :: out
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:,:), intent(in) :: models

    out = merge(randu(self % popsize, self % n_dim), models, models .lt. -1. .or. models .gt. 1.)
    return
  end function constrain_de

  function constrain_cpso(self, models, models_old) result(out)
    real(kind = RPRE), dimension(:,:), allocatable :: out
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:,:), intent(in) :: models, models_old
    integer(kind = IPRE) :: i
    real(kind = RPRE) :: beta_l, beta_u, beta
    logical, dimension(:), allocatable :: maskl, masku

    out = models
    do i = 1, self % popsize
      maskl = models(i,:) .lt. -1.
      masku = models(i,:) .gt. 1.
      if ( any(maskl) .and. any(masku) ) then
        beta_l = minval( ( models_old(i,:) + 1. ) / ( models_old(i,:) - models(i,:) ), mask = maskl )
        beta_u = minval( ( models_old(i,:) - 1. ) / ( models_old(i,:) - models(i,:) ), mask = masku )
        beta = min(beta_l, beta_u)
        out(i,:) = models_old(i,:) + beta * ( models(i,:) - models_old(i,:) )
      else if ( any(maskl) .and. .not. any(masku) ) then
        beta = minval( ( models_old(i,:) + 1. ) / ( models_old(i,:) - models(i,:) ), mask = maskl )
        out(i,:) = models_old(i,:) + beta * ( models(i,:) - models_old(i,:) )
      else if ( .not. any(maskl) .and. any(masku) ) then
        beta = minval( ( models_old(i,:) - 1. ) / ( models_old(i,:) - models(i,:) ), mask = masku )
        out(i,:) = models_old(i,:) + beta * ( models(i,:) - models_old(i,:) )
      end if
    end do
    return
  end function constrain_cpso

  function de_mutation(self, X, F, gbest, strategy) result(V)
    real(kind = RPRE), dimension(:,:), allocatable :: V
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), dimension(:,:), intent(in) :: X
    real(kind = RPRE), intent(in) :: F
    real(kind = RPRE), dimension(:), intent(in) :: gbest
    character(len = *), intent(in) :: strategy
    integer(kind = IPRE), dimension(:,:), allocatable :: idx
    real(kind = RPRE), dimension(:,:), allocatable :: X1, X2, X3, X4, X5

    select case(trim(strategy))
    case("rand1")
      idx = make_idx(self % popsize, 3)
      X1 = X(idx(:,1),:)
      X2 = X(idx(:,2),:)
      X3 = X(idx(:,3),:)
      V = X1 + F * ( X2 - X3 )
    case("rand2")
      idx = make_idx(self % popsize, 5)
      X1 = X(idx(:,1),:)
      X2 = X(idx(:,2),:)
      X3 = X(idx(:,3),:)
      X4 = X(idx(:,4),:)
      X5 = X(idx(:,5),:)
      V = X1 + F * ( X2 + X3 - X4 - X5 )
    case("best1")
      idx = make_idx(self % popsize, 2)
      X1 = X(idx(:,1),:)
      X2 = X(idx(:,2),:)
      V = spread(gbest, 1, self % popsize) + F * ( X1 - X2 )
    case("best2")
      idx = make_idx(self % popsize, 4)
      X1 = X(idx(:,1),:)
      X2 = X(idx(:,2),:)
      X3 = X(idx(:,3),:)
      X4 = X(idx(:,4),:)
      V = spread(gbest, 1, self % popsize) + F * ( X1 + X2 - X3 - X4 )
    end select
    return
  contains
    function make_idx(popsize, n) result(idx)
      integer(kind = IPRE), dimension(:,:), allocatable :: idx
      integer(kind = IPRE), intent(in) :: popsize, n
      integer(kind = IPRE) :: i
      integer(kind = IPRE), dimension(:), allocatable :: tmp

      allocate(idx(popsize, n))
      do i = 1, popsize
        tmp = randperm(popsize, n+1)
        tmp = pack(tmp, tmp .ne. i)
        idx(i,:) = tmp(:n)
      end do
      return
    end function make_idx
  end function de_mutation

  subroutine de(self, CR, F, strategy, xstart)
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), intent(in) :: CR, F
    character(len = *), intent(in) :: strategy
    real(kind = RPRE), dimension(:,:), allocatable, intent(in) :: xstart
    integer(kind = IPRE) :: i, it, gbidx
    integer(kind = IPRE), dimension(:), allocatable :: irand
    real(kind = RPRE) :: gfit
    real(kind = RPRE), dimension(:), allocatable :: pfit, pbestfit, gbest, xopt
    real(kind = RPRE), dimension(:,:), allocatable :: X, V, U, r1
    logical :: converge
    logical, dimension(:), allocatable :: mask1
    logical, dimension(:,:), allocatable :: mask2

    ! Verbosity
    if ( self % verbose .eq. 1 ) call progress_perc(0, self % max_iter, " Processing: ")

    ! Population initial positions
    if ( allocated(xstart) ) then
      X = self % standardize(xstart)
    else
      X = 2. * randu(self % popsize, self % n_dim) - 1.
    end if

    ! Compute fitness
    pfit = self % eval_models(X)
    pbestfit = pfit
    self % n_eval = self % popsize
    if ( self % verbose .eq. 1 ) call progress_perc(1, self % max_iter, " Processing: ")

    ! Initialize best individual
    gbidx = minloc(pbestfit, dim = 1)
    gfit = pbestfit(gbidx)
    gbest = X(gbidx,:)

    if ( self % snap ) then
      call self % init_models()
      self % models(:,:,1) = self % unstandardize(X)
      self % energy(:,1) = pbestfit
      self % fitness(1) = gfit
    end if

    ! Iterate until one of the termination criterion is satisfied
    it = 1
    converge = .false.
    allocate(mask2(self % popsize, self % n_dim))
    do while ( .not. converge )
      it = it + 1
      r1 = randu(self % popsize, self % n_dim)

      ! Mutation
      V = self % de_mutation(X, F, gbest, strategy)

      ! Recombination
      mask2 = .false.
      irand = randi(self % n_dim, self % popsize)
      do i = 1, self % popsize
        mask2(i,irand(i)) = .true.
      end do
      mask2 = mask2 .or. r1 .le. CR
      U = merge(V, X, mask2)
      if ( self % constrain ) U = self % constrain_de(U)

      ! Selection
      pfit = self % eval_models(U)
      self % n_eval = self % n_eval + self % popsize
      mask1 = pfit .lt. pbestfit
      pbestfit = merge(pfit, pbestfit, mask1)
      X = merge(U, X, spread(mask1, 2, self % n_dim))

      ! Update best individual
      gbidx = minloc(pbestfit, dim = 1)

      ! Stop if best individual position changes less than eps1
      if ( norm(gbest - X(gbidx,:)) .lt. self % eps1 &
        .and. pbestfit(gbidx) .lt. self % eps2 ) then
        converge = .true.
        xopt = self % unstandardize(X(gbidx,:))
        gfit = pbestfit(gbidx)
        self % iflag = 0

      ! Stop if fitness is less than eps2
      else if ( pbestfit(gbidx) .lt. self % eps2 ) then
        converge = .true.
        xopt = self % unstandardize(X(gbidx,:))
        gfit = pbestfit(gbidx)
        self % iflag = 1

      ! Stop if maximum iteration is reached
      else if ( it .ge. self % max_iter ) then
        converge = .true.
        xopt = self % unstandardize(X(gbidx,:))
        gfit = pbestfit(gbidx)
        self % iflag = -1

      ! Otherwise, update best individual
      else
        gbest = X(gbidx,:)
        gfit = pbestfit(gbidx)
      end if

      ! Save models and energy
      if ( self % snap ) then
        self % models(:,:,it) = self % unstandardize(X)
        self % energy(:,it) = pfit
        self % fitness(it) = gfit
      end if

      if ( self % verbose .eq. 1 ) call progress_perc(it, self % max_iter, " Processing: ")
    end do

    self % xopt = xopt
    self % gfit = gfit
    self % n_iter = it
    if ( self % snap ) then
      self % models = self % models(:,:,:it)
      self % energy = self % energy(:,:it)
      self % fitness = self % fitness(:it)
    end if
    if ( self % verbose .eq. 1 ) then
      call progress_perc(self % max_iter, self % max_iter, " Processing: ")
      print *
    end if
    return
  end subroutine de

  subroutine cpso(self, w, c1, c2, gamma, xstart)
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), intent(in) :: w, c1, c2, gamma
    real(kind = RPRE), dimension(:,:), allocatable, intent(in) :: xstart
    integer(kind = IPRE) :: i, it, gbidx, nw
    integer(kind = IPRE), dimension(:), allocatable :: idx
    real(kind = RPRE) :: gfit, delta, inorm, swarm_radius
    real(kind = RPRE), dimension(:), allocatable :: pfit, pbestfit, gbest, xopt
    real(kind = RPRE), dimension(:,:), allocatable :: X, pbest, V, r1, r2
    logical :: converge
    logical, dimension(:), allocatable :: mask

    ! Verbosity
    if ( self % verbose .eq. 1 ) call progress_perc(0, self % max_iter, " Processing: ")

    ! Particles initial positions
    if ( allocated(xstart) ) then
      X = self % standardize(xstart)
    else
      X = 2. * randu(self % popsize, self % n_dim) - 1.
    end if
    pbest = X

    ! Initialize particle velocity
    allocate(V(self % popsize, self % n_dim))
    V = 0.

    ! Compute fitness
    pfit = self % eval_models(X)
    pbestfit = pfit
    self % n_eval = self % popsize
    if ( self % verbose .eq. 1 ) call progress_perc(1, self % max_iter, " Processing: ")

    ! Initialize best individual
    gbidx = minloc(pbestfit, dim = 1)
    gfit = pbestfit(gbidx)
    gbest = X(gbidx,:)

    if ( self % snap ) then
      call self % init_models()
      self % models(:,:,1) = self % unstandardize(X)
      self % energy(:,1) = pbestfit
      self % fitness(1) = gfit
    end if

    ! Swarm maximum radius
    delta = log(1. + 0.003 * self % popsize) / max(0.2, log(0.01*real(self % max_iter)))

    ! Iterate until one of the termination criterion is satisfied
    it = 1
    converge = .false.
    do while ( .not. converge )
      it = it + 1
      r1 = randu(self % popsize, self % n_dim)
      r2 = randu(self % popsize, self % n_dim)

      ! Mutation
      V = w * V + c1 * r1 * (pbest - X) + c2 * r2 * (spread(gbest, 1, self % popsize) - X)
      if ( self % constrain ) then
        X = self % constrain_cpso(X + V, X)
      else
        X = X + V
      end if

      ! Selection
      pfit = self % eval_models(X)
      self % n_eval = self % n_eval + self % popsize
      mask = pfit .lt. pbestfit
      pbestfit = merge(pfit, pbestfit, mask)
      pbest = merge(X, pbest, spread(mask, 2, self % n_dim))

      ! Update best individual
      gbidx = minloc(pbestfit, dim = 1)

      ! Stop if best individual position changes less than eps1
      if ( norm(gbest - pbest(gbidx,:)) .lt. self % eps1 &
        .and. pbestfit(gbidx) .lt. self % eps2 ) then
        converge = .true.
        xopt = self % unstandardize(pbest(gbidx,:))
        gfit = pbestfit(gbidx)
        self % iflag = 0

      ! Stop if fitness is less than eps2
      else if ( pbestfit(gbidx) .lt. self % eps2 ) then
        converge = .true.
        xopt = self % unstandardize(pbest(gbidx,:))
        gfit = pbestfit(gbidx)
        self % iflag = 1

      ! Stop if maximum iteration is reached
      else if ( it .ge. self % max_iter ) then
        converge = .true.
        xopt = self % unstandardize(pbest(gbidx,:))
        gfit = pbestfit(gbidx)
        self % iflag = -1

      ! Otherwise, update best individual
      else
        gbest = pbest(gbidx,:)
        gfit = pbestfit(gbidx)
      end if

      ! Save models and energy
      if ( self % snap ) then
        self % models(:,:,it) = self % unstandardize(X)
        self % energy(:,it) = pfit
        self % fitness(it) = gfit
      end if

      ! Competitive PSO algorithm
      if ( .not. converge .and. gamma .gt. 0. ) then
        swarm_radius = maxval([ ( norm(X(i,:) - gbest), i = 1, self % popsize ) ])
        swarm_radius = swarm_radius / sqrt(4.*self % n_dim)

        ! Reset positions if swarm size if lower than threshold
        if ( swarm_radius .lt. delta ) then
          inorm = real(it, RPRE) / real(self % max_iter, RPRE)
          nw = int((self % popsize - 1.) / (1. + exp(1./0.09*(inorm-gamma+0.5))))

          ! Reset positions, velocities and personal bests
          if ( nw .gt. 0 ) then
            self % n_restart = self % n_restart + 1
            idx = argsort(pbestfit, 2)
            idx = idx(:nw)
            V(idx,:) = 0.
            do i = 1, nw
              X(idx(i),:) = merge(2.*randu(self % n_dim)-1., real(0., RPRE), self % std_scale .ne. 0.)
            end do
            pbest(idx,:) = X(idx,:)
            pbestfit(idx) = 1e30
          end if
        end if
      end if

      if ( self % verbose .eq. 1 ) call progress_perc(it, self % max_iter, " Processing: ")
    end do

    self % xopt = xopt
    self % gfit = gfit
    self % n_iter = it
    if ( self % snap ) then
      self % models = self % models(:,:,:it)
      self % energy = self % energy(:,:it)
      self % fitness = self % fitness(:it)
    end if
    if ( self % verbose .eq. 1 ) then
      call progress_perc(self % max_iter, self % max_iter, " Processing: ")
      print *
    end if
    return
  end subroutine cpso

  subroutine cmaes(self, sigma, mu_perc, xstart)
    class(Evolutionary), intent(inout) :: self
    real(kind = RPRE), intent(in) :: sigma, mu_perc
    real(kind = RPRE), dimension(:,:), allocatable, intent(in) :: xstart
    integer(kind = IPRE) :: i, k, it, mu, eigeneval, ilim
    real(kind = RPRE) :: mueff, cc, cs, c1, cmu, damps, chind, hsig, perc(2), &
      delta, insigma
    integer(kind = IPRE), dimension(:), allocatable :: arindex
    real(kind = RPRE), dimension(:), allocatable :: xmean, xold, weights, &
      pc, ps, D, arfitness, arbestfitness, bnd_weights, bnd_scale, dfithist, tx
    real(kind = RPRE), dimension(:,:), allocatable :: B, C, invsqrtC, arx, arxvalid, &
      artmp, pctmp
    logical :: validfitval, iniphase, converge
    logical, dimension(:), allocatable :: ti, idx

    ! Initialize saved outputs
    if ( self % snap ) then
      call self % init_models()
      allocate(self % means(self % max_iter, self % n_dim))
    end if

    ! Population initial positions
    if ( allocated(xstart) ) then
      xmean = self % standardize([ xstart ])
    else
      xmean = 2. * randu(self % n_dim) - 1.
    end if
    allocate(xold(self % n_dim))

    ! Number of parents
    mu = int(mu_perc * self % popsize)

    ! Strategy parameter setting: Selection
    weights = log(mu + 0.5) - log([ ( real(i, RPRE), i = 1, mu ) ])
    weights = weights / sum(weights)
    mueff = sum(weights)**2 / sum(weights**2)

    ! Strategy parameter setting: Adaptation
    cc = ( 4. + mueff / self % n_dim ) / ( self % n_dim + 4. + 2. * mueff / self % n_dim )
    cs = ( mueff + 2. ) / ( self % n_dim + mueff + 5. )
    c1 = 2. / ( ( self % n_dim + 1.3 )**2 + mueff )
    cmu = min(1. - c1, 2. * ( mueff - 2. + 1. / mueff ) / ( ( self % n_dim + 2. )**2 + mueff ) )
    damps = 1. + 2. * max(0., sqrt( ( mueff - 1. ) / ( self % n_dim + 1. ) ) - 1.) + cs

    ! Initialize dynamic (internal) strategy parameters and constants
    arx = zeros(self % popsize, self % n_dim)
    pc = zeros(self % n_dim)
    ps = zeros(self % n_dim)
    B = eye(self % n_dim, self % n_dim)
    D = ones(self % n_dim)
    C = eye(self % n_dim, self % n_dim)
    invsqrtC = eye(self % n_dim, self % n_dim)
    chind = sqrt(real(self % n_dim, RPRE)) * ( 1. - 1. / ( 4. * self % n_dim ) + 1. / ( 21. * self % n_dim**2 ) )

    ! Initialize boundaries weights
    bnd_weights = zeros(self % n_dim)
    bnd_scale = zeros(self % n_dim)
    dfithist = [ real(1., RPRE) ]

    ! (opt_mu, opt_lambda)-CMA-ES
    it = 0
    eigeneval = 0
    arbestfitness = zeros(self % max_iter)
    ilim = int(10 + 30 * self % n_dim / self % popsize)
    insigma = sigma
    validfitval = .false.
    iniphase = .true.
    converge = .false.

    do while ( .not. converge )
      it = it + 1

      ! Generate lambda offspring
      do k = 1, self % popsize
        arx(k,:) = xmean + insigma * matmul(B, D*randn(self % n_dim))
      end do
      arxvalid = arx

      ! Evaluate Fitness
      if ( self % constrain ) then
        ! Clip to boundaries
        arxvalid = merge(real(-1., RPRE), arxvalid, arxvalid .lt. -1.)
        arxvalid = merge(real(1., RPRE), arxvalid, arxvalid .gt. 1.)
        arfitness = self % eval_models(arxvalid)

        ! Get delta fitness values
        perc = prctile(arfitness, [ 25, 75 ])
        delta = ( perc(2) - perc(1) ) / real(self % n_dim, RPRE) / mean(diag(C)) / insigma**2

        ! Catch non-sensible values
        if ( delta .eq. 0. ) then
          delta = minval(dfithist, mask = dfithist .gt. 0.)
        else if ( .not. validfitval ) then
          dfithist = zeros(0)
          validfitval = .true.
        end if

        ! Store delta fitness values
        if ( size(dfithist) .lt. 20. + (3.*self % n_dim) / self % popsize ) then
          dfithist = [ dfithist, delta ]
        else
          dfithist = [ dfithist(2:), delta ]
        end if

        ! Corrected mean
        ti = xmean .lt. -1. .or. xmean .gt. 1.
        tx = merge(xmean, real(-1., RPRE), xmean .ge. -1.)
        tx = merge(xmean, real(1., RPRE), xmean .le. 1.)

        ! Set initial weights
        if ( iniphase ) then
          if ( any(ti) ) then
            bnd_weights = 2.0002 * median(dfithist)
            if ( validfitval .and. it .gt. 2 ) iniphase = .false.
          end if
        end if

        if ( any(ti) ) then
          tx = xmean - tx
          idx = ti .and. abs(tx) .gt. 3. * max(1., sqrt(real(self % n_dim, RPRE))/mueff) &
                * insigma * sqrt(diag(C))
          idx = idx .and. signum(tx) .eq. signum(xmean - xold)
          where ( idx )
            bnd_weights = bnd_weights * 1.2**(min(1., mueff/10./real(self % n_dim, RPRE)))
          end where
        end if

        ! Calculate scaling biased to unity, product is one
        bnd_scale = exp( 0.9 * ( log(diag(C)) - mean(log(diag(C))) ) )

        ! Assigned penalized fitness
        arfitness = self % eval_models(arxvalid) &
                    + matmul((arxvalid - arx)**2, bnd_weights / bnd_scale)
      else
        arfitness = self % eval_models(arxvalid)
      end if
      self % n_eval = self % n_eval + self % popsize
      if ( self % snap ) then
        self % models(:,:,it) = self % unstandardize(arx)
        self % energy(:,it) = arfitness
        self % fitness(it) = minval(arfitness)
        self % means(it,:) = self % unstandardize(xmean)
      end if

      ! Sort by fitness and compute weighted mean into xmean
      arindex = argsort(arfitness)
      xold = xmean
      xmean = matmul(weights, arx(arindex(:mu),:))

      ! Save best Fitness
      arbestfitness(it) = arfitness(arindex(1))

      ! Cumulation
      ps = ( 1. - cs ) * ps &
           + sqrt( cs * ( 2. - cs ) * mueff ) * matmul(invsqrtC, xmean - xold) / insigma
      if ( norm(ps) / sqrt( 1. - ( 1. - cs )**(2.*real(self % n_eval, RPRE) / real(self % popsize, RPRE))) / chind &
        .lt. 1.4 + 2. / ( real(self % n_dim) + 1. ) ) then
        hsig = 1.
        pc = ( 1. - cc ) * pc &
             + sqrt( cc * ( 2. - cc ) * mueff ) * (xmean - xold) / insigma
      else
        hsig = 0.
        pc = ( 1. - cc ) * pc
      end if

      ! Adapt covariance matrix C
      artmp = ( arx(arindex(:mu),:) - spread(xold, 1, mu) ) / insigma
      pctmp = reshape(pc, shape = [ size(pc), 1 ])
      if ( hsig .eq. 1. ) then
        C = ( 1. - c1 - cmu ) * C &
            + c1 * matmul(pctmp, transpose(pctmp)) &
            + cmu * matmul(matmul(transpose(artmp), diag(weights)), artmp)
      else
        C = ( 1. - c1 - cmu ) * C &
            + c1 * ( matmul(pctmp, transpose(pctmp)) + cc * ( 2. - cc ) * C ) &
            + cmu * matmul(matmul(transpose(artmp), diag(weights)), artmp)
      end if

      ! Adapt step size sigma
      insigma = insigma * exp( ( cs / damps ) * ( norm(ps) / chind - 1. ) )

      ! Diagonalization of C
      if ( self % n_eval - eigeneval .gt. real(self % popsize, RPRE) / ( c1 + cmu ) / self % n_dim / 10. ) then
        eigeneval =  self % n_eval
        C = triu(C) + transpose(triu(C, 1))
        call eig(C, B, D)
        D = sqrt(D)
        invsqrtC = matmul(matmul(B, diag(real(1./D, RPRE))), transpose(B))
      end if

      ! Stop if maximum number of iteration is reached
      if ( it .ge. self % max_iter ) then
        converge = .true.
        self % iflag = -1
      end if

      ! Stop if mean position changes less than eps1
      if ( .not. converge .and. norm(xold - xmean) .le. self % eps1 ) then
        converge = .true.
        self % iflag = 0
      end if

      ! Stop if fitness is less than eps2
      if ( .not. converge .and. arfitness(arindex(1)) .le. self % eps2 ) then
        converge = .true.
        self % iflag = 1
      end if

      ! NoEffectAxis: stop if numerical precision problem
      i = floor( mod( real(it), real(self % n_dim) ) ) + 1
      if ( .not. converge .and. all( 0.1 * insigma * B(:,i) * D(i) .lt. 1e-10 ) ) then
        converge = .true.
        self % iflag = 2
      end if

      ! NoEffectCoord: stop if too low coordinate axis deviations
      if ( .not. converge .and. any( 0.2 * insigma * sqrt(diag(C)) .lt. 1e-10 ) ) then
        converge = .true.
        self % iflag = 3
      end if

      ! ConditionCov: stop if the condition number exceeds 1e14
      if ( .not. converge .and. maxval(D) .gt. 1e7 * minval(D) ) then
        converge = .true.
        self % iflag = 4
      end if

      ! EqualFunValues: stop if the range of fitness values is zero
      if ( .not. converge .and. it .ge. ilim ) then
        if ( maxval(arbestfitness(it-ilim+1:it)) &
             - minval(arbestfitness(it-ilim+1:it)) .lt. 1e-10 ) then
          converge = .true.
          self % iflag = 5
        end if
      end if

      ! TolXUp: stop if x-changes larger than 1e3 times initial sigma
      if ( .not. converge .and. any( insigma * sqrt(diag(C)) .gt. 1e3 * sigma ) ) then
        converge = .true.
        self % iflag = 6
      end if

      ! TolFun: stop if fun-changes smaller than 1e-12
      if ( .not. converge .and.  it .gt. 2 &
           .and. maxval( [ arfitness, arbestfitness ] ) &
                 - minval( [ arfitness, arbestfitness ] ) .lt. 1e-12 ) then
        converge = .true.
        self % iflag = 7
      end if

      ! TolX: stop if x-changes smaller than 1e-11 times initial sigma
      if ( .not. converge .and. all( insigma * ( max( abs(pc), sqrt(diag(C)) ) ) .lt. 1e-11 * sigma ) ) then
        converge = .true.
        self % iflag = 8
      end if

      if ( self % verbose .eq. 1 ) call progress_perc(it, self % max_iter, " Processing: ")
    end do

    self % xopt = self % unstandardize(arx(arindex(1),:))
    self % gfit = arfitness(arindex(1))
    self % n_iter = it
    if ( self % snap ) then
      self % models = self % models(:,:,:it)
      self % energy = self % energy(:,:it)
      self % fitness = self % fitness(:it)
      self % means = self % means(:it,:)
    end if
    if ( self % verbose .eq. 1 ) then
      call progress_perc(self % max_iter, self % max_iter, " Processing: ")
      print *
    end if
    return
  end subroutine cmaes

  subroutine save_models_evolutionary(self, prefix)
    class(Evolutionary), intent(inout) :: self
    character(len = *), intent(in), optional :: prefix
    character(len = :), allocatable :: opt_prefix, str_popsize, str_n_dim, &
      str_n_iter, models_filename, energy_filename, fitness_filename, means_filename

    if ( self % mpi_rank .eq. 0 ) then
      if ( present(prefix) ) then
        opt_prefix = trim(prefix)
      else
        opt_prefix = "./"
      end if

      if ( self % snap ) then
        str_popsize = num2str(self % popsize)
        str_n_dim = num2str(self % n_dim)
        str_n_iter = num2str(self % n_iter)
        models_filename = "models_" // str_popsize // "_" // str_n_dim &
                          // "_" // str_n_iter // ".bin"
        energy_filename = "energy_" // str_popsize // "_" // str_n_iter // ".bin"
        fitness_filename = "fitness_" // str_n_iter // ".bin"
        call savebin(opt_prefix // models_filename, self % models)
        call savebin(opt_prefix // energy_filename, self % energy)
        call savebin(opt_prefix // fitness_filename, self % fitness)
        if ( self % solver .eq. "cmaes" ) then
          means_filename = "means_" // str_n_iter // "_" // str_n_dim // ".bin"
          call savebin(opt_prefix // means_filename, self % means)
        end if
      else
        print *, "Warning: cannot save models when snap is false"
      end if
    end if
    return
  end subroutine save_models_evolutionary

  subroutine sample(self, sampler, stepsize, xstart)
    class(MonteCarlo), intent(inout) :: self
    character(len = *), intent(in), optional :: sampler
    real(kind = RPRE), dimension(:), intent(in), optional :: stepsize, xstart
    real(kind = RPRE), dimension(:), allocatable :: opt_stepsize, opt_xstart
    character(len = :), allocatable :: opt_sampler
    integer(kind = 4) :: master = 0, num_procs, rank, ierr

    ! Check inputs
    if ( present(sampler) ) then
      if ( trim(sampler) .ne. "mcmc" ) then
        print *, "Error: solver must be 'mcmc'"
        stop
      else
        opt_sampler = trim(sampler)
      end if
    else
      opt_sampler = "mcmc"
    end if
    if ( present(stepsize) ) then
      if ( size(stepsize) .ne. self % n_dim ) then
        print *, "Error: stepsize must be a 1-D array of length " // num2str(self % n_dim)
        stop
      end if
      if ( any(stepsize .le. 0.) ) then
        print *, "Error: elements in stepsize must be positive"
        stop
      else
        opt_stepsize = stepsize
      end if
    else
      allocate(opt_stepsize(self % n_dim))
      opt_stepsize = 0.1
    end if
    if ( present(xstart) ) then
      if ( size(xstart) .ne. self % n_dim ) then
        print *, "Error: xstart must be a 1-D array of length " // num2str(self % n_dim)
        stop
      else
        opt_xstart = self % standardize(xstart)
      end if
    else
      opt_xstart = 2. * randu(self % n_dim) - 1.
    end if

    ! Initialize
    self % sampler = opt_sampler
    self % mu_scale = 0.5 * (self % upper + self % lower)
    self % std_scale = 0.5 * (self % upper - self % lower)

    ! Sample
    select case(opt_sampler)
    case("mcmc")
      call self % mcmc(stepsize = opt_stepsize, xstart = opt_xstart)
    end select
    return
  end subroutine sample

  function standardize_montecarlo(self, models) result(out)
    real(kind = RPRE), dimension(:), allocatable :: out
    class(MonteCarlo), intent(inout) :: self
    real(kind = RPRE), dimension(:), intent(in) :: models
    real(kind = RPRE), dimension(:), allocatable :: std_scale
    logical, dimension(:), allocatable :: mask

    out = ( models - self % mu_scale ) / std_scale
    return
  end function standardize_montecarlo

  function unstandardize1_montecarlo(self, models) result(out)
    real(kind = RPRE), dimension(:), allocatable :: out
    class(MonteCarlo), intent(inout) :: self
    real(kind = RPRE), dimension(:), intent(in) :: models
    out = models * self % std_scale + self % mu_scale
    return
  end function unstandardize1_montecarlo

  function unstandardize2_montecarlo(self, models) result(out)
    real(kind = RPRE), dimension(:,:), allocatable :: out
    class(MonteCarlo), intent(inout) :: self
    real(kind = RPRE), dimension(:,:), intent(in) :: models
    out = models * spread(self % std_scale, 1, self % max_iter) &
          + spread(self % mu_scale, 1, self % max_iter)
    return
  end function unstandardize2_montecarlo

  subroutine init_models_montecarlo(self)
    class(MonteCarlo), intent(inout) :: self
    if ( allocated(self % models) ) deallocate(self % models, self % energy)
    allocate(self % models(self % max_iter, self % n_dim))
    allocate(self % energy(self % max_iter))
    return
  end subroutine init_models_montecarlo

  function in_search_space(self, x) result(out)
    logical :: out
    class(MonteCarlo), intent(inout) :: self
    real(kind = RPRE), dimension(:), intent(in) :: x

    if ( self % constrain ) then
      out = all( x .le. 1. ) .and. all( x .ge. -1. )
    else
      out = .true.
    end if
    return
  end function in_search_space

  subroutine mcmc(self, stepsize, xstart)
    class(MonteCarlo), intent(inout) :: self
    real(kind = RPRE), dimension(:), intent(in) :: stepsize, xstart
    integer(kind = IPRE) :: i, j, jmax, rejected, idx
    real(kind = RPRE) :: log_alpha

    ! Verbosity
    if ( self % verbose .eq. 1 ) call progress_perc(0, self % max_iter, " Processing: ")

    ! Initialize models
    call self % init_models()
    self % models(1,:) = xstart
    self % energy(1) = self % func(self % unstandardize(self % models(1,:)))
    if ( self % verbose .eq. 1 ) call progress_perc(1, self % max_iter, " Processing: ")

    ! Metropolis-Hastings algorithm
    rejected = 0
    i = 1
    do while ( i .lt. self % max_iter )
      do j = 1, self % n_dim, self % n_dim_per_iter
        i = i + 1
        jmax = min(self % n_dim, j + self % n_dim_per_iter - 1)
        self % models(i,:) = self % models(i-1,:)
        self % models(i,j:jmax) = self % models(i,j:jmax) + randn(jmax-j+1) * stepsize(j:jmax)
        self % energy(i) = self % func(self % unstandardize(self % models(i,:)))

        log_alpha = min(real(0., RPRE), self % energy(i-1) - self % energy(i))
        if ( log_alpha .lt. log(randu()) &
          .or. .not. self % in_search_space(self % models(i,:)) ) then
          rejected = rejected + 1
          self % models(i,:) = self % models(i-1,:)
          self % energy(i) = self % energy(i-1)
        end if

        if ( i .eq. self % max_iter ) exit
      end do
      if ( self % verbose .eq. 1 ) call progress_perc(i, self % max_iter, " Processing: ")
    end do

    idx = minloc(self % energy, dim = 1)
    self % models = self % unstandardize(self % models)
    self % xopt = self % models(idx,:)
    self % gfit = self % energy(idx)
    self % acceptance_ratio = 1. - real(rejected) / real(self % max_iter)
    if ( self % verbose .eq. 1 ) print *
    return
  end subroutine mcmc

  subroutine save_models_montecarlo(self, prefix)
    class(MonteCarlo), intent(inout) :: self
    character(len = *), intent(in), optional :: prefix
    character(len = :), allocatable :: opt_prefix, str_max_iter, str_n_dim, &
      models_filename, energy_filename

    if ( present(prefix) ) then
      opt_prefix = trim(prefix)
    else
      opt_prefix = "./"
    end if

    str_max_iter = num2str(self % max_iter)
    str_n_dim = num2str(self % n_dim)
    models_filename = "models_" // str_max_iter // "_" // str_n_dim // ".bin"
    energy_filename = "energy_" // str_max_iter // ".bin"
    call savebin(opt_prefix // models_filename, self % models)
    call savebin(opt_prefix // energy_filename, self % energy)
    return
  end subroutine save_models_montecarlo

end module stochoptim


submodule (Prng_Class) sm_rngWeibull
!! Generate random number from a weibull distribution.

  ! Original Version
  ! Author: Alan Miller

  ! Updated version:
  ! Version 2.00, 11 November 2016
  ! Increased precision to double
  !
  ! Updated again
  ! Added overloaded operations for single number, nD arrays
  !     Author: Leon Foks

implicit none

contains

  !====================================================================!
  module procedure rngWeibull_d1_Prng!(this, res, lambda, k)
    !! Overloaded Type bound procedure Prng%rngWeibull()
  !====================================================================!
  ! Class(Prng), intent(inout) :: this
    !! Prnge Class
  ! real(r64), intent(inout) :: res(:)
    !! Draw from Weibull distribution
  ! real(r64), intent(in) :: lambda
    !! Scale of the distribution
  ! real(r64), intent(in) :: k
    !! Shape of the distribution
  if (lambda <= 0.d0) call eMsg('Prng%rngExponential: lambda must be > 0')
  if (dabs(k) < 1.d-16) call Emsg('Prng%rngWeibull : scale is very small, increase it.')
  call rngExponential_unscaled_d1(this, res)
  res =  (res / lambda)**(1.d0 / k)
  end procedure
  !====================================================================!
  !====================================================================!
  module procedure rngWeibull_d1D_Prng!(this, res, lambda, k)
    !! Overloaded Type bound procedure Prng%rngWeibull()
  !====================================================================!
  ! Class(Prng), intent(inout) :: this
    !! Prnge Class
  ! real(r64), intent(inout) :: res(:)
    !! Draw from Weibull distribution
  ! real(r64), intent(in) :: lambda
    !! Scale of the distribution
  ! real(r64), intent(in) :: k
    !! Shape of the distribution
  integer(i32) :: i, N
  real(r64) :: wk, wk2, wk3
  if (lambda <= 0.d0) call eMsg('Prng%rngWeibull: lambda must be > 0')
  if (dabs(k) < 1.d-16) call Emsg('Prng%rngWeibull : scale is very small, increase it.')

  wk2 = 1.d0 / lambda
  wk3 = 1.d0 / k
  N = size(res)
  do i = 1, N
    call rngExponential_unscaled_d1(this, wk)
    res(i) =  (wk * wk2)**(wk3)
  end do
  end procedure
  !====================================================================!
  !====================================================================!
  module procedure rngWeibull_d2D_Prng!(this, res, lambda, k)
    !! Overloaded Type bound procedure Prng%rngWeibull()
  !====================================================================!
  ! class(Prng) :: this
  ! real(r64) :: res(:, :)
  ! real(r64) :: lambda
  integer(i32) :: i, iSub(2)
  integer(i32) :: n, nSub(2)
  real(r64) :: wk, wk2, wk3

  if (lambda <= 0.d0) call eMsg('Prng%rngWeibull: lambda must be > 0')
  if (dabs(k) < 1.d-16) call Emsg('Prng%rngWeibull : scale is very small, increase it.')

  wk2 = 1.d0 / lambda
  wk3 = 1.d0 / k

  nSub = shape(res)
  n = size(res)
  do i = 1, n
    iSub = ind2sub(i, nSub)
    call rngExponential_unscaled_d1(this, wk)
    res(iSub(1), iSub(2)) =  (wk * wk2)**(wk3)
  enddo
  end procedure
  !====================================================================!
  !====================================================================!
  module procedure rngWeibull_d3D_Prng!(this, res, lambda, k)
    !! Overloaded Type bound procedure Prng%rngWeibull()
  !====================================================================!
  ! class(Prng) :: this
  ! real(r64) :: res(:,:,:)
  ! real(r64) :: lambda
  integer(i32) :: i, iSub(3)
  integer(i32) :: n, nSub(3)
  real(r64) :: wk, wk2, wk3

  if (lambda <= 0.d0) call eMsg('Prng%rngWeibull: lambda must be > 0')
  if (dabs(k) < 1.d-16) call Emsg('Prng%rngWeibull : scale is very small, increase it.')

  wk2 = 1.d0 / lambda
  wk3 = 1.d0 / k

  nSub = shape(res)
  n = size(res)
  do i = 1, n
    iSub = ind2sub(i, nSub)
    call rngExponential_unscaled_d1(this, wk)
    res(iSub(1), iSub(2), iSub(3)) =  (wk * wk2)**(wk3)
  enddo
  end procedure
  !====================================================================!
end submodule


chapter x. example 2. plotting the pol,s which make up a b-spline
c  from  * a practical guide to splines *  by c. de boor
calls  bsplvb
c
      integer ia,left
      real biatx(4),t(11),values(4),x
c		  knot sequence set here....
      data t / 4*0.,1.,3.,4.,4*6. /
      do 20 ia=1,40
	 x = float(ia)*.2 - 1.
	 do 10 left=4,7
	    call bsplvb ( t, 4, 1, x, left, biatx )
c
c	    according to  bsplvb  listing,  biatx(.) now contains value
c	    at	x  of polynomial which agrees on the interval  (t(left),
c	    t(left+1) )  with the b-spline  b(left-4 + . ,4,t) . hence,
c	    biatx(8-left)  now contains value of that polynomial for
c	    b(left-4 +(8-left) ,4,t) = b(4,4,t) . since this b-spline
c	    has support  (t(4),t(8)), it makes sense to run  left = 4,
c	    ...,7, storing the resulting values in  values(1),...,
c	    values(4)  for later printing.
c
   10	    values(left-3) = biatx(8-left)
   20	 print 620, x, values
  620 format(f10.1,4f20.8)
					stop
      end


C++***********************************************************************
C Program XYFIT1
C To compute the linear transformation between two sets of X,Y positions
C and output the 6 coefficients in a file (directly compatible with ALIGN1).
C
C SYNTAX:
C     RUNS XYFIT1 input_list1 input_list2 coeff_file option
C
C Example:
C     RUNS XYFIT1 test1.STA test2.STA test1_2.XYF 1
C
C Nota1: the input lists are simply positions x,y in an ASCII file, as
C        generated by STARFIT1, or CENTROID1
C Nota2: Possible options are
C           1 = shift only
C           2 = shift and rotation
C           3 = shift, rotation and magnification
C           4 = full 6-coefficient fit
C
C JLP
C Version 19-07-90
C--***********************************************************************
	PROGRAM XYFIT1
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*METHOD
*       obtain the input x,y position lists. assign workspace and
*       extract the x,y positions and identifiers. obtain parameters
*       controlling the fitting, then call fitlst to obtain the
*       fit and print the results.
*
*STARLINK PARAMETERS
*       ILEVEL
*               INTERACTION LEVEL: CONTROLS PRINTING OF RESULTS
*       XA, YA
*               INPUT LIST OF X, Y POSITIONS
*       XB, YB
*               SECOND INPUT LIST OF X, Y POSITIONS
*       NOSPACE/ERROR/
*               ACCESSED IF DYNAMIC STORAGE SPACE IS NOT AVAILABLE
*       FITTYPE
*               CONTROLS NUMBER OF DEGREES OF FREEDOM IN FIT
*       NSIGMA
*               NUMBER OF STANDARD DEVIATIONS AT WHICH ABERRANT POINTS
*               ARE REJECTED
*       MXREJECT
*               MAX NUMBER OF REJECTION ITERATIONS
*       TRCOEFFS
*               OUTPUT PARAMETER, GIVES 6 COEFFICIENTS DEFINING THE
*               TRANSFORMATION
*
*WRITTEN BY
*       R.F. WARREN-SMITH
*-----------------------------------------------------------------------
*
*
	PARAMETER (IDIM1=1000)
	REAL*4 XA(IDIM1),YA(IDIM1),XB(IDIM1),YB(IDIM1)
	INTEGER*4 IDIAMA(IDIM1),IDIAMB(IDIM1)
	CHARACTER NAMEA*40,NAMEB*40,NAMEOUT*40
	LOGICAL VALID(IDIM1)
	REAL*4 C(6)

10	FORMAT(A)

	CALL JLP_BEGIN
	PRINT 7
7	FORMAT(' Program XYFIT1   Version of 12-04-90',/,
     1	' To compute the linear transformation',/,
     1	' from two input lists of coordinates of the same objects')

*
* OBTAIN INTERACTION LEVEL  (3=talkative, 1=quiet)
*
	ilevel=3
        open(4,file='xyfit1.dat',status='new')

C	call getpar('ILEVEL','INTEGER',1,1.0,3.0,.true.,ilevel,rval,ierr)

* Input of the two lists :

C Read the first list XA, YA :
15	PRINT *,' NAME OF THE FIRST LIST :'
	READ(5,10) NAMEA
	OPEN(1,FILE=NAMEA,STATUS='OLD',ERR=15)
	I=0
94	IF(I.GE.IDIM1) GOTO 17
	   I=I+1
	   READ(1,*,END=17)XA(I),YA(I),IDIAMA(I)
        GOTO 94
17	NPTSA=I-1
	CLOSE(1)
	PRINT *,NPTSA,' POINTS RECEIVED'

C Read the second list XB, YB :
16	PRINT *,' NAME OF THE SECOND LIST :'
	READ(5,10) NAMEB
	OPEN(2,FILE=NAMEB,STATUS='OLD',ERR=16)
	I=0
95	IF(I.GE.IDIM1) GOTO 21
	   I=I+1
	   READ(2,*,END=21)XB(I),YB(I),IDIAMB(I)
        GOTO 95
21	NPTSB=I-1
	CLOSE(2)
	PRINT *,NPTSB,' POINTS RECEIVED'

C Exit if one of the lists is empty :
	NPTS=MIN(NPTSA,NPTSB)
	IF(NPTS.EQ.0)THEN
	 PRINT *,' EMPTY INPUT LIST'
	 STOP
	ENDIF

23	PRINT *,' NAME OF THE OUTPUT FILE :'
	READ(5,10) NAMEOUT
	OPEN(3,FILE=NAMEOUT,STATUS='NEW',ERR=23)
*
* OBTAIN TYPE OF FIT (IFIT), NO. OF SIGMA REJECTION THRESHOLD (GAMMA)
* AND MAX NO. OF REJECTED POINTS (MAXIT)
*
	      PRINT *,' TYPE OF FIT : 1 SHIFT ONLY, 2 SHIFT AND ROTATION'
	      PRINT *,' 3 SHIFT, ROTATION, MAGNI.  4 FULL 6 COEFFICIENTS ?'
	      READ(5,*) IFIT
	      PRINT *,'      FITTYPE =',IFIT
	      gamma=1.0e10
CC	      PRINT *,' NO. OF SIGMA REJECTION'
C	      call getpar('NSIGMA','REAL',1,0.0,1.0e10,.true.,ival,gamma
C     1	,ierr)
CC	      READ(5,*) GAMMA
C	      PRINT *,'      NSIGMA =',GAMMA
	      maxit=NPTS
CC	      PRINT *,' MAX NO. OF REJECTED POINTS'
C	      call getpar('MXREJECT','INTEGER',1,0.0,1.0e7,.true.,maxit
C     1	,rval,ierr)
CC	      READ(5,*) MAXIT
C	      PRINT *,'      MAXREJECT =',MAXIT

*
* IF NO MATCHES ARE FOUND BETWEEN THE TWO SETS OF POSITIONS, GIVE
* MESSAGE AND ABORT
*
C Tolerance for the positions : 1.0 pixel
	TOLER=1.
	CALL MATCH_LIST(XA,YA,IDIAMA,NPTSA,XB,YB,IDIAMB,
     1	NPTSB,TOLER,NMATCH,IERRM)

	      if(ierrm.ne.0.or.nmatch.lt.2) then
	 	 PRINT *,' THE TWO LISTS DO NOT MATCH'
	         go to 99
	      endif

*
* CALL FITLST TO PERFORM THE FITTING AND DISPLAY THE RESULTS
*
	      call fitlst(XA,YA,XB,YB,NMATCH
     1	,maxit,gamma,ifit,ilevel,C,VALID)

*
* WRITE TRANSFORMATION COEFFICIENTS TO THE OUTPUT FILE
	      WRITE(3,*)(C(K),K=1,6)

*
* FREE DATA AREAS
*
99	CLOSE(3)
	CLOSE(4)
	CALL JLP_END
	STOP
	END

C@*********************************************************************
C FITLST
C@*********************************************************************
	SUBROUTINE FITLST(XA,YA,XB,YB,NPTS,maxit
     1	,gamma,ifit,ilevel,c,valid)

*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*PURPOSE
*       TO CONTROL THE FITTING OF A LINEAR TRANSFORMATION BETWEEN
*       TWO SETS OF X,Y POSITIONS AND TO DISPLAY THE RESULTS
*
*METHOD
*       CALL XYFITR TO PERFORM
*       THE FITTING, THEN DISPLAY A TABLE OF RESULTS.
*
*ARGUMENTS
*       XA, YA
*               INPUT LIST OF X, Y POSITIONS
*       XB, YB
*               SECOND INPUT LIST OF X, Y POSITIONS
*       MAXIT (IN)
*       INTEGER
*               THE MAXIMUM NUMBER OF REJECTION ITERATIONS TO BE
*               PERFORMED BY THE FITTING ROUTINE XYFITR
*       GAMMA (IN)
*       REAL
*               THE NUMBER OF STANDARD DEVIATIONS AT WHICH ABERRANT
*               POINTS ARE REJECTED IN XYFITR
*       IFIT (IN)
*       INTEGER
*               AN INTEGER DEFINING THE TYPE OF LINEAR TRANSFORMATION
*               CALCULATED BY XYFITR
*       ILEVEL (IN)
*       INTEGER
*               INTERACTION LEVEL, CONTROLLING THE PRINTING OF RESULTS
*       C (OUT)
*       REAL(6)
*               RETURNS THE 6 TRANSFORMATION COEFFICIENTS
*       XB,YB (WORKSPACE)
*       REAL(NPTS)
*               STORAGE FOR X,Y POSITIONS
*       VALID (WORKSPACE)
*       LOGICAL(*)
*               USED TO HOLD FLAGS INDICATING WHICH POINTS WERE REJECTED
*               DURING FITTING. SHOULD BE AT LEAST NPTS
*               ELEMENTS LONG
*
*CALLS
*       THIS PACKAGE:
*               XYFITR
*
*NOTES
*       USES BYTE ARRAYS
*
*WRITTEN BY
*       R.F. WARREN-SMITH
*-----------------------------------------------------------------------
      real xa(NPTS),ya(NPTS),xb(NPTS),yb(NPTS),c(6)
      logical valid(*)
      character prbuf*80,fitmsg(4)*80

*
* SET UP INFORMATION MESSAGES TO INDICATE THE FIT USED
*
      data fitmsg(1)/'   TRANSFORMATION IS A SHIFT OF ORIGIN ONLY'/
     : ,fitmsg(2)/'   TRANSFORMATION IS A SHIFT AND ROTATION'/,
     : fitmsg(3)/
     : '   TRANSFORMATION IS A SHIFT,ROTATION AND MAGNIFICATION'/,
     : fitmsg(4)/'   TRANSFORMATION IS A FULL 6 PARAMETER FIT'/

*
* SET ALL MATCHED POSITIONS AS VALID, THEN CALL XYFITR TO PERFORM
* THE FITTING

      do 1 i=1,npts
         valid(i)=.true.
1     continue

      call xyfitr(xa,ya,xb,yb,valid,npts,maxit,gamma,ifit,c,ierr)

*
* PRINT NUMBER OF MATCHED POSITIONS
*

      if(ilevel.ge.2) then
         write(6,10)npts
10       format(3x,i8,' POSITION(S) MATCHED BETWEEN INPUT LISTS')
      endif

*
* IF TABLE OF RESULTS IS NEEDED, PRINT HEADINGS
*

      if(ilevel.ge.3) then
         write(6,11)
         write(4,11)
11       format(3x,'TRANSFORMED A POSITION',12x,'B POSITION',/,
     1	3x,'----------------------',12x,'----------',/,
     1	2x,2(' X COORD.      Y COORD.     '),3x,'ALIGNMENT ERROR',/,
     1	2x,2(' --------      --------     '),3x,'---------------')
      endif

*
* CALCULATE RMS ERROR AND COUNT NUMBER OF POSITIONS REJECTED
*

      if(ilevel.ge.2) then
         nrej=0
         sigma=0.0

         do 66 i=1,npts

*
* CALCULATE TRANSFORMED 'A' POSITIONS AND ERROR
*
            xd=c(1)+c(2)*xa(i)+c(3)*ya(i)
            yd=c(4)+c(5)*xa(i)+c(6)*ya(i)
            errsq=(xd-xb(i))**2+(yd-yb(i))**2
            err=sqrt(errsq)

            if(valid(i)) then

*
* INCREASE SUM OF ERRORS AND PRINT RESULT IF REQUIRED
*
               sigma=sigma+errsq

               if(ilevel.ge.3) write(prbuf,15)xd,yd,xb(i),yb(i),err,
     :         'OK'

            else

*
* COUNT ONE REJECTED POINT AND PRINT RESULT IF REQUIRED
*
               nrej=nrej+1

               if(ilevel.ge.3) write(prbuf,15)xd,yd,xb(i),yb(i),err,
     :         'REJ'
            endif

15          format(1x,4(1x,ss,g13.6),4x,ss,g12.4,2x,a3)

66       continue

*
* CALCULATE RMS ERROR AND PRINT NO. OF REJECTED POINTS AND RMS ERROR
*

         if(nrej.lt.npts) then
            sigma=sqrt(sigma/(npts-nrej))
          else
            sigma=0.0
         endif

         write(6,16) nrej,sigma
         write(4,16) nrej,sigma
16       format(3x,i8,' POSITION(S) REJECTED',/,
     1	3x,'RMS ALIGNMENT ERROR=',ss,g11.4)

*
* PRINT MESSAGE SHOWING TYPE OF FIT USED
*
	 write(6,'(A)') fitmsg(ifit)
         write(4,'(A)') fitmsg(ifit)

*
* FINALLY PRINT THE TRANSFORMATION COEFFICIENTS
*
         write(6,18)
         write(4,18)
18       format(3x,'TRANSFORMATION COEFFICIENTS:')

         do 44 j=1,4,3
* JLP99:
*           write(6,19)((l,c(l)),l=j,j+2)
*           write(4,19)((l,c(l)),l=j,j+2)
            write(6,19)j,c(j),j+1,c(j+1),j+2,c(j+2)
            write(4,19)j,c(j),j+1,c(j+1),j+2,c(j+2)
19          format(10x,3('C(',i1,')=',ss,g13.6,2x))
44       continue

      endif

99    return

      end

C@**********************************************************************
C XYFITR
C@**********************************************************************

      subroutine xyfitr(xa,ya,xb,yb,ok,n,maxit,gamma,ifit,c,ierr)

*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*PURPOSE
*       TO FIND A LEAST-SQUARES LINEAR TRANSFORMATION BETWEEN TWO SETS
*       OF X,Y POSITIONS, REJECTING ERRONEOUS DATA.
*
*METHOD
*       CALL LINTRN TO OBTAIN A LEAST-SQUARES FIT. CALCULATE THE RMS
*       MIS-ALIGNMENT AND THE MOST MIS-ALIGNED DATA POINT. REJECT THE
*       WORST POINT IF IT EXCEEDS A THRESHOLD BASED ON THE RMS ERROR.
*       REPEAT FOR A PRESET NUMBER OF ITERATIONS, OR UNTIL NO FURTHER
*       POINTS ARE REJECTED
*
*ARGUMENTS
*       XA,YA (IN)
*       REAL(N)
*               THE FIRST SET OF POSITIONS TO BE TRANSFORMED
*       XB,YB (IN)
*       REAL(N)
*               THE SECOND SET OF REFERENCE POSITIONS
*       OK (IN/OUT)
*       LOGICAL(N)
*               FLAGS TO INDICATE WHICH DATA POINTS HAVE NOT BEEN
*               REJECTED
*       N (IN)
*       INTEGER
*               NUMBER OF DATA POINTS
*       MAXIT (IN)
*       INTEGER
*               MAX. NUMBER OF REJECTION ITERATIONS
*       GAMMA (IN)
*       REAL
*               NUMBER OF STANDARD DEVIATIONS AT WHICH ABERRANT POINTS
*               ARE REJECTED
*       IFIT (IN/OUT)
*       INTEGER
*               FLAG WHICH INDICATES THE TYPE OF LINEAR TRANSFORMATION
*               USED... SERVES THE SAME PURPOSE AS IN LINTRN
*       C (OUT)
*       REAL(6)
*               THE COEFFICIENTS DEFINING THE TRANSFORMATION
*       IERR (OUT)
*       INTEGER
*               ERROR FLAG: ZERO FOR SUCCESS
*
*CALLS
*       THIS PACKAGE:
*               LINTRN
*
*WRITTEN BY
*       R.F. WARREN-SMITH
*-----------------------------------------------------------------------
*
*
      real xa(n),ya(n),xb(n),yb(n),c(6)
      logical ok(n),reject

*
* INITIALLISE ITERATION COUNT AND LOOP WHILE POINTS WERE REJECTED LAST
* ITERATION
*
      iter=0
      reject=.true.

63    if(reject) then

*
* CALL LINTRN TO FIND THE LINEAR TRANSFORMATION BETWEEN THE 2 SETS
* OF X,Y POSITIONS
*
         call lintrn(xa,ya,xb,yb,ok,n,c,ifit,ierr)

*
* QUIT IF ERROR, OR MAX ITERATIONS EXCEEDED
*

         if((ierr.eq.0).and.(iter.lt.maxit)) then

*
* COUNT 1 ITERATION AND INITIALLISE REJECTED POINTS FLAG
*
            iter=iter+1
            reject=.false.

*
* INITIALLISE COUNTERS FOR FINDING STANDARD DEVIATION AND MAXIMUM
* MIS-ALIGNMENT
*
            varsum=0.0
            ngood=0
            errmax=-1.0

*
* SCAN ALL POINTS NOT REJECTED SO FAR
*

            do 2 i=1,n

               if(ok(i)) then

*
* CALCULATE THE MIS-ALIGNMENT
*
                  xd=c(1)+c(2)*xa(i)+c(3)*ya(i)
                  yd=c(4)+c(5)*xa(i)+c(6)*ya(i)
                  errsq=(xb(i)-xd)**2+(yb(i)-yd)**2
                  varsum=varsum+errsq
                  ngood=ngood+1

*
* FIND THE MAXIMUM MIS-ALIGNMENT
*

                  if(errsq.gt.errmax) then
                     errmax=errsq
                     imax=i
                  endif

               endif

2           continue

*
* FIND THE RMS MIS-ALIGNMENT AND REJECT THE WORST POINT IF IT IS MORE
* THAN GAMMA STD. DEVS. OUT
*

            if(ngood.gt.0) then
               var=varsum/ngood

               if((var.gt.0.0).and.(errmax.gt.var*(gamma**2))) then
                  ok(imax)=.false.
                  reject=.true.
               endif

            endif

            go to 63

         endif

      endif

      return

      end

C***************************************************************************
C Subroutine MATCH_LIST to match the two input lists of positions
C JLP 18/08/87
C
C At present work only for shifts (not for rotations nor expansions)
C
C XA,YA (IN/OUT) : List A of positions
C
C XB,YB (IN/OUT) : List B of positions
C
C
C CXA, CXB : Arrays XA and XB sorted according to the diameter of the objects
C CYA, CYB : Arrays YA and YB sorted according to the diameter of the objects
C DISTA : Relative distances of list A to a given object of list A
C DISTB : Relative distances of list B to a given object of list B
C***************************************************************************

	SUBROUTINE MATCH_LIST(XA,YA,IDIAMA,NPTSA,XB,YB,IDIAMB,
     1	NPTSB,TOLER,NMATCH,ISTAT)
	PARAMETER (IDIM1=1000)
	REAL*4 XA(NPTSA),YA(NPTSA),XB(NPTSB),YB(NPTSB)
	REAL*4 CXA(IDIM1),CYA(IDIM1),CXB(IDIM1),CYB(IDIM1)
	REAL*4 DISTA(10),DISTB(10)
	INTEGER*4 IDIAMA(NPTSA),IDIAMB(NPTSB)
	INTEGER*4 INDEXA(IDIM1),INDEXB(IDIM1),KMATCH(10,10)

	ISTAT=0

C*******************
C First looking for the 10 largest objects

	NPTS=MIN(NPTSA,NPTSB)
	IF(NPTS.GT.1)THEN
C Sorts the array IDIAMA in descending order with INDEXA(I)=initial index
	   CALL SORT_INT4(IDIAMA,INDEXA,NPTSA,'D')
C	   NWORK=IDIM1
C NEW NAG: CALL M01DBF(IDIAMA,1,NPTSA,'D',INDEXA,IFAIL)
	   DO I=1,NPTSA
	     CXA(I)=XA(INDEXA(I))
	     CYA(I)=YA(INDEXA(I))
	   END DO

C Sorts the array IDIAMB in descending order with INDEXB(I)=initial index
C	   NWORK=IDIM1
C NEW NAG : CALL M01DBF(IDIAMB,1,NPTSB,'D',INDEXB,IFAIL)
	   CALL SORT_INT4(IDIAMB,INDEXB,NPTSB,'D')
	   DO I=1,NPTSB
	     CXB(I)=XB(INDEXB(I))
	     CYB(I)=YB(INDEXB(I))
	   END DO
	ELSE
	   PRINT *,' TOO FEW POINTS TO FIT ANYTHING !!!'
	   ISTAT=1
	   RETURN
	ENDIF

	NPTS=MIN(10,NPTS)

C************************
C Compute a cross-correlation matrix of the two lists :
C Loop on all the objects of list A to look for the best agreement with list B
	DO IA=1,NPTS

C Computing the distances relative to the object IA :
	  DO KA=1,NPTS
	   DISTA(KA)=SQRT((CXA(KA)-CXA(IA))**2
     1	+ (CYA(KA)-CYA(IA))**2)
	  END DO

C For each position of IB, looking for the best agreement in list B :
	    DO IB=1,NPTS

C Computing the distances relative to the object IB :
	       DO KB=1,NPTS
	         DISTB(KB)=SQRT((CXB(KB)-CXB(IB))**2
     1	 + (CYB(KB)-CYB(IB))**2)
	       END DO

C Looking for similar distances with array A :
	     KMATCH(IB,IA)=0
	      DO KA=1,NPTS
	       DO KB=1,NPTS
	        WORK=ABS(DISTB(KB)-DISTA(KA))
	        IF(WORK.LT.TOLER)KMATCH(IB,IA)=KMATCH(IB,IA)+1
	       END DO
	      END DO

C Go to another position in list B
	  END DO

C Go to another position in list A
	END DO

C *****************
C Looking for the maximum of the correlation array KMATCH :
	IAMAX=1
	IBMAX=1
	KMAX=0
	DO IA=1,NPTS
	  DO IB=1,NPTS
	    IF(KMATCH(IB,IA).GT.KMAX)THEN
	     IAMAX=IA
	     IBMAX=IB
	     KMAX=KMATCH(IB,IA)
	    ENDIF
	  END DO
	END DO

C********************
C Return if no correlation found between the two lists :
	IF(KMAX.EQ.0)THEN
	 PRINT *,' NO CORRELATION FOUND BETWEEN THE TWO LISTS'
	 ISTAT=1
	 RETURN
	ENDIF

C********************
C If correlation found
C get a first approximation of DELTAX and DELTAY :
	DELTAX=CXA(IAMAX)-CXB(IBMAX)
	DELTAY=CYA(IAMAX)-CYB(IBMAX)

C********************
C and sort the two lists in the same order :
	NMATCH=0
	 DO IA=1,NPTSA
	   DO IB=1,NPTSB
	    WORK1=ABS(CXA(IA)-CXB(IB)-DELTAX)
	    WORK2=ABS(CYA(IA)-CYB(IB)-DELTAY)
	    WORK3=AMAX1(WORK1,WORK2)
	     IF(WORK3.LT.TOLER)THEN
	      NMATCH=NMATCH+1
	      XA(NMATCH)=CXA(IA)
	      XB(NMATCH)=CXB(IB)
	      YA(NMATCH)=CYA(IA)
	      YB(NMATCH)=CYB(IB)
	      GO TO 2
	     ENDIF
	   END DO
2	 END DO
	IF(NMATCH.EQ.0)ISTAT=2

	PRINT *,' NMATCH=',NMATCH

	RETURN
	END
C**********************************************************************
      subroutine lintrn(xa,ya,xb,yb,ok,n,c,ifit,ierr)
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*PURPOSE
*       TO OBTAIN A LINEAR TRANSFORMATION BETWEEN 2 SETS OF X,Y
*       POSITIONS WITH LEAST SQUARED ERROR.
*
*METHOD
*       SET UP THE NORMAL EQUATIONS CORRESPONDING TO THE TYPE OF FIT
*       REQUIRED. SOLVE NORMAL EQUATIONS TO GIVE THE TRANSFORMATION.
*       IF SUCCESSFUL, EXIT. OTHERWISE REDUCE THE NUMBER OF DEGREES
*       OF FREEDOM IN THE FIT AND REPEAT.
*
*ARGUMENTS
*       XA,YA (IN)
*       REAL(N)
*               THE FIRST SET OF X,Y POSITIONS WHICH ARE TO BE
*               TRANSFORMED
*       XB,YB (IN)
*       REAL(N)
*               THE SECOND (REFERENCE) SET OF POSITIONS
*       OK (IN)
*       LOGICAL(N)
*               A SET OF LOGICAL FLAGS INDICATING WHICH POSITIONS ARE
*               TO BE USED
*       N (IN)
*       INTEGER
*               THE SIZE OF ARRAYS XA,YA,XB,YB,OK
*       C (OUT)
*       REAL(6)
*               THE SIX COEFFICIENTS DEFINING THE TRANSFORMATION
*       IFIT (IN/OUT)
*       INTEGER
*               THE TYPE OF FIT REQUIRED:
*               1:SHIFT OF ORIGIN
*               2:SHIFT AND ROTATION
*               3:SHIFT, ROTATION AND MAGNIFICATION
*               4:FULL 6 PARAMETER TRANSFORMATION
*               IFIT MAY BE REDUCED ON OUTPUT IF THE SPECIFIED FIT
*               WAS NOT OBTAINED
*       IERR (OUT)
*       INTEGER
*               ERROR FLAG: ZERO FOR SUCCESS
*
*CALLS
*       NAG LIBRARY:
*               F04AEF
*
*WRITTEN BY
*       R.F. WARREN-SMITH
*-----------------------------------------------------------------------
*
*
      real xa(n),ya(n),xb(n),yb(n),c(6)
      double precision a(4,4),b(4,2),ans(4,2),wkspce(4),aa(4,4),bb(4,2)
      logical ok(n)

*
* CHECK VALIDITY OF ARGUMENTS
*
      ierr=0

      if(n.lt.1) then
         ierr=1

      else

*
* IS AT LEAST 1 POSITION VALID?
*
         sw=0.0

         do 10 i=1,n

            if(ok(i)) sw=sw+1.0
10       continue

         if(sw.le.0.0) then
            ierr=2

         else

*
* OK...SET TYPE OF FIT REQUIRED BETWEEN 1 AND 4
*
            ifit=min(max(1,ifit),4)

*
* CHECK THAT THE FIT DOES NOT HAVE TOO MANY DEGREES OF FREEDOM FOR
* THE NUMBER OF DATA POINTS AVAILABLE
*
            npts=nint(sw)

            if(npts.le.2) ifit=min(ifit,3)

            if(npts.le.1) ifit=1

*
* INITIALLISE SUMS FOR NORMAL EQUATIONS
*
            swx=0.0
            swy=0.0
            swxy=0.0
            swx2=0.0
            swy2=0.0
            swxd=0.0
            swyd=0.0
            swxxd=0.0
            swyyd=0.0
            swxyd=0.0
            swyxd=0.0

*
* FORM SUMS, SETTING WEIGHT TO ZERO FOR INVALID POSITIONS
*

            do 20 i=1,n

               if(ok(i)) then
                  w=1.0

               else
                  w=0.0
               endif

               wx=w*xa(i)
               wy=w*ya(i)
               swx=swx+wx
               swy=swy+wy
               swxd=swxd+w*xb(i)
               swyd=swyd+w*yb(i)

*
* IF FIT ONLY REQUIRES A SHIFT OF ORIGIN, FURTHER SUMS ARE NOT
* REQUIRED
*

               if(ifit.ne.1) then
                  swxy=swxy+wx*ya(i)
                  swx2=swx2+wx*xa(i)
                  swy2=swy2+wy*ya(i)
                  swxxd=swxxd+wx*xb(i)
                  swxyd=swxyd+wx*yb(i)
                  swyxd=swyxd+wy*xb(i)
                  swyyd=swyyd+wy*yb(i)
               endif

20          continue

*
* ITERATE UP TO 4 TIMES, REDUCING IFIT BY 1 EACH TIME
*
            ifit=ifit+1

            do 60 itry=1,4
               ifit=ifit-1

*
* SHIFT OF ORIGIN ONLY: EQUATIONS SIMPLY SOLVED
* --------------------
*

               if(ifit.eq.1) then
                  c(1)=(swxd-swx)/sw
                  c(2)=1.0
                  c(3)=0.0
                  c(4)=(swyd-swy)/sw
                  c(5)=0.0
                  c(6)=1.0
                  ifail=0

*
* SHIFT OF ORIGIN AND ROTATION
* ----------------------------

               else if(ifit.eq.2) then

*
* CALCULATE THE CENTROIDS OF EACH SET OF POSITIONS
*
                  xd0=swxd/sw
                  yd0=swyd/sw
                  x0=swx/sw
                  y0=swy/sw

*
* INITIALLISE STORAGE FOR NEW SUMS
*
                  swyxd0=0.0
                  swxyd0=0.0
                  swxxd0=0.0
                  swyyd0=0.0

*
* FORM NEW SUMS, USING THE DEVIATIONS FROM THE CENTROIDS
*

                  do 146 i=1,n

                     if(ok(i)) then
                        swyxd0=swyxd0+(ya(i)-y0)*(xb(i)-xd0)
                        swxyd0=swxyd0+(xa(i)-x0)*(yb(i)-yd0)
                        swxxd0=swxxd0+(xa(i)-x0)*(xb(i)-xd0)
                        swyyd0=swyyd0+(ya(i)-y0)*(yb(i)-yd0)
                     endif

146               continue

*
* IF THE ROTATION ANGLE IS NOT DEFINED, IFAIL=1
*
                  top=swyxd0-swxyd0
                  bot=swyyd0+swxxd0

                  if(top.eq.0.0.and.bot.eq.0.0) then
                     ifail=1

                  else

*
* OTHERWISE CALCULATE THE ROTATION ANGLE ABOUT THE CENTROIDS
* AND ASSIGN THE RESULTS TO THE TRANSFORM COEFFICIENTS
*
                     theta=atan2(top,bot)
                     c(1)=xd0-(x0*cos(theta)+y0*sin(theta))
                     c(2)=cos(theta)
                     c(3)=sin(theta)
                     c(4)=yd0-(-x0*sin(theta)+y0*cos(theta))
                     c(5)=-sin(theta)
                     c(6)=cos(theta)
                     ifail=0
                  endif

*
* SHIFT, ROTATION AND MAGNIFICATION: SET UP NORMAL EQUATIONS
* ---------------------------------
*

               else if(ifit.eq.3) then
                  a(1,1)=sw
                  a(1,2)=swx
                  a(1,3)=swy
                  a(1,4)=0.0d0
                  a(2,1)=swx
                  a(2,2)=swx2+swy2
                  a(2,3)=0.0d0
                  a(2,4)=swy
                  a(3,1)=swy
                  a(3,2)=0.0d0
                  a(3,3)=swx2+swy2
                  a(3,4)=-swx
                  a(4,1)=0.0d0
                  a(4,2)=swy
                  a(4,3)=-swx
                  a(4,4)=sw
                  b(1,1)=swxd
                  b(2,1)=swxxd+swyyd
                  b(3,1)=swyxd-swxyd
                  b(4,1)=swyd

*
* CALL NAG ROUTINE F04AEF TO SOLVE THE LINEAR NORMAL EQUATIONS
*
                  ifail=1
                  call f04aef(a,4,b,4,4,1,ans,4,wkspce,aa,4,bb,4,ifail)

*
* IF SUCCESSFUL, ASSIGN RESULT TO THE TRANSFORMATION COEFFICIENTS
*

                  if(ifail.eq.0) then
                     c(1)=ans(1,1)
                     c(2)=ans(2,1)
                     c(3)=ans(3,1)
                     c(4)=ans(4,1)
                     c(5)=-ans(3,1)
                     c(6)=ans(2,1)
                  endif

*
* FULL FIT REQUIRED: SET UP NORMAL EQUATIONS
* -----------------
*

               else if(ifit.eq.4) then
                  a(1,1)=sw
                  a(1,2)=swx
                  a(1,3)=swy
                  a(2,1)=swx
                  a(2,2)=swx2
                  a(2,3)=swxy
                  a(3,1)=swy
                  a(3,2)=swxy
                  a(3,3)=swy2
                  b(1,1)=swxd
                  b(2,1)=swxxd
                  b(3,1)=swyxd
                  b(1,2)=swyd
                  b(2,2)=swxyd
                  b(3,2)=swyyd

*
* CALL NAG ROUTINE F04AEF TO SOLVE LINEAR NORMAL EQUATIONS
*
                  ifail=1
                  call f04aef(a,4,b,4,3,2,ans,4,wkspce,aa,4,bb,4,ifail)

*
* IF SUCCESSFUL, ASSIGN RESULTS TO TRANSFORMATION COEFFICIENTS
*

                  if(ifail.eq.0) then
                     c(1)=ans(1,1)
                     c(2)=ans(2,1)
                     c(3)=ans(3,1)
                     c(4)=ans(1,2)
                     c(5)=ans(2,2)
                     c(6)=ans(3,2)
                  endif

               endif

*
* IF A FIT WAS SUCCESSFULLY OBTAINED THIS TIME, EXIT FROM ITERATION
* LOOP. OTHERWISE TRY AGAIN WITH IFIT REDUCED BY 1
*

               if(ifail.eq.0) go to 70
60          continue

70          continue
         endif

      endif

      return

      end
C**************************************************************
      include 'jlpsub:sort_set.for'


CS    REAL FUNCTION ANORM(ARG)
      DOUBLE PRECISION FUNCTION ANORM(ARG)
C------------------------------------------------------------------
C
C This function evaluates the normal distribution function:
C
C                              / x
C                     1       |       -t*t/2
C          P(x) = ----------- |      e       dt
C                 sqrt(2 pi)  |
C                             /-oo
C
C   The main computation evaluates near-minimax approximations
C   derived from those in "Rational Chebyshev approximations for
C   the error function" by W. J. Cody, Math. Comp., 1969, 631-637.
C   This transportable program uses rational functions that
C   theoretically approximate the normal distribution function to
C   at least 18 significant decimal digits.  The accuracy achieved
C   depends on the arithmetic system, the compiler, the intrinsic
C   functions, and proper selection of the machine-dependent
C   constants.
C
C*******************************************************************
C*******************************************************************
C
C Explanation of machine-dependent constants.  Let
C
C   XMIN  = the smallest positive floating-point number.
C
C Then the following machine-dependent constants must be declared
C   in DATA statements.  IEEE values are provided as a default.
C
C   EPS   = argument below which anorm(x) may be represented by
C           0.5  and above which  x*x  will not underflow.
C           A conservative value is the largest machine number X
C           such that   1.0 + X = 1.0   to machine precision.
C   XLOW  = the most negative argument for which ANORM does not
C           vanish.  This is the negative of the solution to
C                    W(x) * (1-1/x**2) = XMIN,
C           where W(x) = exp(-x*x/2)/[x*sqrt(2*pi)].
C   XUPPR = positive argument beyond which anorm = 1.0.  A
C           conservative value is the solution to the equation
C                    exp(-x*x/2) = EPS,
C           i.e., XUPPR = sqrt[-2 ln(eps)].
C
C   Approximate values for some important machines are:
C
C                          XMIN        EPS        XLOW    XUPPR
C
C  CDC 7600      (S.P.)  3.13E-294   7.11E-15   -36.641   8.072
C  CRAY-1        (S.P.)  4.58E-246   7.11E-157 -106.521  26.816
C  IEEE (IBM/XT,
C    SUN, etc.)  (S.P.)  1.18E-38    5.96E-8    -12.949   5.768
C  IEEE (IBM/XT,
C    SUN, etc.)  (D.P.)  2.23D-308   1.11D-16   -37.519   8.572
C  IBM 195       (D.P.)  5.40D-79    1.39D-17   -18.781   8.811
C  VAX D-Format  (D.P.)  2.94D-39    1.39D-17   -13.055   8.811
C  VAX G-Format  (D.P.)  5.56D-309   1.11D-16   -37.556   8.572
C
C*******************************************************************
C*******************************************************************
C
C Error returns
C
C  The program returns  ANORM = 0     for  ARG .LE. XLOW.
C
C
C Intrinsic functions required are:
C
C     ABS, AINT, EXP
C
C
C  Author: W. J. Cody
C          Mathematics and Computer Science Division
C          Argonne National Laboratory
C          Argonne, IL 60439
C
C  Latest modification: March 15, 1992
C
C------------------------------------------------------------------
      INTEGER I
CS    REAL
      DOUBLE PRECISION
     1     A,ARG,B,C,D,DEL,EPS,HALF,P,ONE,Q,RESULT,SIXTEN,
     2     SQRPI,THRSH,ROOT32,X,XLOW,XDEN,XNUM,Y,XSQ,XUPPR,ZERO
      DIMENSION A(5),B(4),C(9),D(8),P(6),Q(5)
C------------------------------------------------------------------
C  Mathematical constants
C
C  SQRPI = 1 / sqrt(2*pi), ROOT32 = sqrt(32), and
C  THRSH is the argument for which anorm = 0.75.
C------------------------------------------------------------------
CS    DATA ONE,HALF,ZERO,SIXTEN/1.0E0,0.5E0,0.0E0,1.60E1/,
CS   1     SQRPI/3.9894228040143267794E-1/,THRSH/0.66291E0/,
CS   2     ROOT32/5.656854248E0/
      DATA ONE,HALF,ZERO,SIXTEN/1.0D0,0.5D0,0.0D0,1.60D1/,
     1     SQRPI/3.9894228040143267794D-1/,THRSH/0.66291D0/,
     2     ROOT32/5.656854248D0/
C------------------------------------------------------------------
C  Machine-dependent constants
C------------------------------------------------------------------
CS    DATA EPS/5.96E-8/,XLOW/-12.949E0/,XUPPR/5.768E0/
      DATA EPS/1.11D-16/,XLOW/-37.519D0/,XUPPR/8.572D0/
C------------------------------------------------------------------
C  Coefficients for approximation in first interval
C------------------------------------------------------------------
CS    DATA A/2.2352520354606839287E00,1.6102823106855587881E02,
CS   1       1.0676894854603709582E03,1.8154981253343561249E04,
CS   2       6.5682337918207449113E-2/
CS    DATA B/4.7202581904688241870E01,9.7609855173777669322E02,
CS   1       1.0260932208618978205E04,4.5507789335026729956E04/
      DATA A/2.2352520354606839287D00,1.6102823106855587881D02,
     1       1.0676894854603709582D03,1.8154981253343561249D04,
     2       6.5682337918207449113D-2/
      DATA B/4.7202581904688241870D01,9.7609855173777669322D02,
     1       1.0260932208618978205D04,4.5507789335026729956D04/
C------------------------------------------------------------------
C  Coefficients for approximation in second interval
C------------------------------------------------------------------
CS    DATA C/3.9894151208813466764E-1,8.8831497943883759412E00,
CS   1       9.3506656132177855979E01,5.9727027639480026226E02,
CS   2       2.4945375852903726711E03,6.8481904505362823326E03,
CS   3       1.1602651437647350124E04,9.8427148383839780218E03,
CS   4       1.0765576773720192317E-8/
CS    DATA D/2.2266688044328115691E01,2.3538790178262499861E02,
CS   1       1.5193775994075548050E03,6.4855582982667607550E03,
CS   2       1.8615571640885098091E04,3.4900952721145977266E04,
CS   3       3.8912003286093271411E04,1.9685429676859990727E04/
      DATA C/3.9894151208813466764D-1,8.8831497943883759412D00,
     1       9.3506656132177855979D01,5.9727027639480026226D02,
     2       2.4945375852903726711D03,6.8481904505362823326D03,
     3       1.1602651437647350124D04,9.8427148383839780218D03,
     4       1.0765576773720192317D-8/
      DATA D/2.2266688044328115691D01,2.3538790178262499861D02,
     1       1.5193775994075548050D03,6.4855582982667607550D03,
     2       1.8615571640885098091D04,3.4900952721145977266D04,
     3       3.8912003286093271411D04,1.9685429676859990727D04/
C------------------------------------------------------------------
C  Coefficients for approximation in third interval
C------------------------------------------------------------------
CS    DATA P/2.1589853405795699E-1,1.274011611602473639E-1,
CS   1       2.2235277870649807E-2,1.421619193227893466E-3,
CS   2       2.9112874951168792E-5,2.307344176494017303E-2/
CS    DATA Q/1.28426009614491121E00,4.68238212480865118E-1,
CS   1       6.59881378689285515E-2,3.78239633202758244E-3,
CS   2       7.29751555083966205E-5/
      DATA P/2.1589853405795699D-1,1.274011611602473639D-1,
     1       2.2235277870649807D-2,1.421619193227893466D-3,
     2       2.9112874951168792D-5,2.307344176494017303D-2/
      DATA Q/1.28426009614491121D00,4.68238212480865118D-1,
     1       6.59881378689285515D-2,3.78239633202758244D-3,
     2       7.29751555083966205D-5/
C------------------------------------------------------------------
      X = ARG
      Y = ABS(X)
      IF (Y .LE. THRSH) THEN
C------------------------------------------------------------------
C  Evaluate  anorm  for  |X| <= 0.66291
C------------------------------------------------------------------
            XSQ = ZERO
            IF (Y .GT. EPS) XSQ = X * X
            XNUM = A(5)*XSQ
            XDEN = XSQ
            DO 20 I = 1, 3
               XNUM = (XNUM + A(I)) * XSQ
               XDEN = (XDEN + B(I)) * XSQ
   20       CONTINUE
            RESULT = X * (XNUM + A(4)) / (XDEN + B(4))
            RESULT = HALF + RESULT
C------------------------------------------------------------------
C  Evaluate  anorm  for 0.66291 <= |X| <= sqrt(32)
C------------------------------------------------------------------
         ELSE IF (Y .LE. ROOT32) THEN
            XNUM = C(9)*Y
            XDEN = Y
            DO 120 I = 1, 7
               XNUM = (XNUM + C(I)) * Y
               XDEN = (XDEN + D(I)) * Y
  120       CONTINUE
            RESULT = (XNUM + C(8)) / (XDEN + D(8))
            XSQ = AINT(Y*SIXTEN)/SIXTEN
            DEL = (Y-XSQ)*(Y+XSQ)
            RESULT = EXP(-XSQ*XSQ*HALF)*EXP(-DEL*HALF)*RESULT
            IF (X .GT. ZERO) RESULT = ONE - RESULT
C------------------------------------------------------------------
C  Evaluate  anorm  for |X| > sqrt(32)
C------------------------------------------------------------------
         ELSE
            RESULT = ZERO
            IF ((X .GE. XLOW) .AND. (X .LT. XUPPR)) THEN
               XSQ = ONE / (X * X)
               XNUM = P(6)*XSQ
               XDEN = XSQ
               DO 240 I = 1, 4
                  XNUM = (XNUM + P(I)) * XSQ
                  XDEN = (XDEN + Q(I)) * XSQ
  240          CONTINUE
               RESULT = XSQ *(XNUM + P(5)) / (XDEN + Q(5))
               RESULT = (SQRPI -  RESULT) / Y
               XSQ = AINT(X*SIXTEN)/SIXTEN
               DEL = (X-XSQ)*(X+XSQ)
               RESULT = EXP(-XSQ*XSQ*HALF)*EXP(-DEL*HALF)*RESULT
            END IF
            IF (X .GT. ZERO) RESULT = ONE - RESULT
      END IF
C------------------------------------------------------------------
C  Fix up for negative argument, erf, etc.
C------------------------------------------------------------------
      ANORM = RESULT
C---------- Last card of ANORM ----------
      END