function Offspring = Operator(Problem,Particle,Pbest,Gbest)
% Particle swarm optimization in SMPSO

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Parameter setting
    ParticleDec = Particle.decs;
    PbestDec    = Pbest.decs;
    GbestDec    = Gbest.decs;
    [N,D]       = size(ParticleDec);
    ParticleVel = Particle.adds(zeros(N,D));

    %% Particle swarm optimization
    W  = repmat(unifrnd(0.1,0.5,N,1),1,D);
    r1 = repmat(rand(N,1),1,D);
    r2 = repmat(rand(N,1),1,D);
    C1 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    C2 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    OffVel = W.*ParticleVel + C1.*r1.*(PbestDec-ParticleDec) + C2.*r2.*(GbestDec-ParticleDec);
    phi    = max(4,C1+C2);
    OffVel = OffVel.*2./abs(2-phi-sqrt(phi.^2-4*phi));
    delta  = repmat((Problem.upper-Problem.lower)/2,N,1);
    OffVel = max(min(OffVel,delta),-delta);
    OffDec = ParticleDec + OffVel;

    %% Deterministic back
    Lower  = repmat(Problem.lower,N,1);
    Upper  = repmat(Problem.upper,N,1);
    repair = OffDec < Lower | OffDec > Upper;
    OffVel(repair) = 0.001*OffVel(repair);
    OffDec = max(min(OffDec,Upper),Lower);

    %% Polynomial mutation
    disM  = 20;
    Site1 = repmat(rand(N,1)<0.15,1,D);
    Site2 = rand(N,D) < 1/D;
    mu    = rand(N,D);
    temp  = Site1 & Site2 & mu<=0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                   (1-(OffDec(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp  = Site1 & Site2 & mu>0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                   (1-(Upper(temp)-OffDec(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
    Offspring = Problem.Evaluation(OffDec,OffVel);
end


function [association] = ft_nonlinearassociation(cfg, data)

% NONLINEARASSOCIATION calculate the association coefficient as a
% function of delay.
%
% In order to estimate the amount of association between all possible
% pairs of MEG sensors the nonlinear association analysis is used.
% It was first developed for EEG data analysis by Pijn and co-workers
% (Lopes da Silva, et al. 1989; Pijn, et al. 1990). The basic principle
% is similar to that of coherence and (cross) correlation, with the
% exception that this nonlinear association method can be applied
% independent of the type of relationship (linear or nonlinear) in
% the data.
%
% The method is based on the idea that if two signals x and y are
% correlated, a nonlinear regression curve can be calculated that
% represents their relationship. In practice, that regression curve
% is estimated by creating a scatterplot of y versus x, dividing the
% data in segments and describing each segment with a linear regression
% curve. The estimated correlation ratio h2, which gives the reduction
% in variance of y as a result of predicting its values according to
% the regression curve, can be calculated as follows:
%
% h^2 = (sum(Yi - mean(Y))^2 - sum(Yi - f(Xi))^2) / sum(Yi - mean(Y))^2
%
% With the sum going over N samples and f(Xi) the estimated value of
% Yi according to the regression line. The h2 coefficient has values
% between 0 (y is completely independent of x) and 1 (y is completely
% determined by x). In the case of a linear relationship between x
% and y, h2 is equal to the well known Pearson correlation coefficient
% (r2). As is the case with cross-correlation, it is possible to
% estimate h2 as a function of time shift () between the signals. The
% h2 is then iteratively calculated for different values of , by
% shifting the signals in comparison to each other, and the value for
% which the maximal h2 is reached can be used as an estimate of the
% time lag between both signals. In deciding what epoch length to use
% in the association analysis, a trade-off has to be made between
% successfully determining the correct delay and h2-value (for which
% large epoch lengths are necessary) and a small enough time-resolution
% (for which small epoch lengths are necessary).
%
% Use as
%   [association] = ft_nonlinearassociation(cfg, data)
%
% The input data should be organised in a structure as obtained from
% the PREPROCESSING function.
%
% The configuration should contain
%   cfg.channel    = Nx1 cell-array with selection of channels (default = 'all'), see CHANNELSELECTION for details
%   cfg.keeptrials = 'yes' or 'no', process the individual trials or the concatenated data (default = 'no')
%   cfg.trials     = 'all' or a selection given as a 1xN vector (default = 'all')
%   cfg.fsample    = 1200
%   cfg.maxdelay   = 32/cfg.fsample
%   cfg.delaystep  = 2/cfg.fsample
%   cfg.nr_bins    = 7
%   cfg.offset     = 0
%   cfg.order      = 'Hxy'
%   cfg.timwin     = 0.2
%   cfg.toi        = []
%
% References
% - Lopes da Silva F, Pijn JP, Boeijinga P. (1989): Interdependence of
% EEG signals: linear vs. nonlinear associations and the significance
% of time delays and phase shifts. Brain Topogr 2(1-2):9-18.
% - Pijn JP, Vijn PC, Lopes da Silva FH, Van Ende Boas W, Blanes W.
% (1990): Localization of epileptogenic foci using a new signal
% analytical approach. Neurophysiol Clin 20(1):1-11.

% Copyright (C) 2007, Inge Westmijse

ft_defaults

% set the defaults
if ~isfield(cfg, 'trials'),        cfg.trials  = 'all';             end
if ~isfield(cfg, 'keeptrials'),    cfg.keeptrials  = 'no';          end
if ~isfield(cfg, 'channel'),       cfg.channel = 'all';             end
if ~isfield(cfg, 'toi'),           cfg.toi = [];                    end
if ~isfield(cfg, 'timwin'),        cfg.timwin = 0.2;                end
if ~isfield(cfg, 'fsample'),       cfg.fsample = 1200;              end
if ~isfield(cfg, 'delaystep'),     cfg.delaystep = 2/cfg.fsample;   end
if ~isfield(cfg, 'maxdelay'),      cfg.maxdelay = 32/cfg.fsample;   end
if ~isfield(cfg, 'offset'),        cfg.offset = 0;                  end
if ~isfield(cfg, 'nr_bins'),       cfg.nr_bins = 7;                 end
if ~isfield(cfg, 'order'),         cfg.order = 'Hxy';               end
if ~isfield(cfg, 'feedback'),      cfg.feedback = 'textbar';        end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% do some bookkeeping on the data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% select trials of interest
if ~strcmp(cfg.trials, 'all')
  if islogical(cfg.trials),  cfg.trials=find(cfg.trials);  end
  fprintf('selecting %d trials\n', length(cfg.trials));
  data.trial  = data.trial(cfg.trials);
  data.time   = data.time(cfg.trials);
end
Ntrials  = numel(data.trial);

% select channels of interest
cfg.channel = channelselection(cfg.channel, data.label);
chansel = match_str(data.label, cfg.channel);
fprintf('selecting %d channels\n', length(chansel));
for trial=1:Ntrials
  data.trial{trial} = data.trial{trial}(chansel,:);
end
data.label = data.label(chansel);
Nchans     = length(chansel);

% determine the size of each trial, they can be variable length
Nsamples = zeros(1,Ntrials);
for trial=1:Ntrials
  Nsamples(trial) = size(data.trial{trial},2);
end

if strcmp(cfg.keeptrials, 'no')
  % concatenate all the data into a 2D matrix
  fprintf('concatenating data');
  dat = zeros(Nchans, sum(Nsamples));
  for trial=1:Ntrials
    fprintf('.');
    begsample = sum(Nsamples(1:(trial-1))) + 1;
    endsample = sum(Nsamples(1:trial));
    dat(:,begsample:endsample) = data.trial{trial};
  end
  fprintf('\n');
  fprintf('concatenated data matrix size %dx%d\n', size(dat,1), size(dat,2));
  time = [1/cfg.fsample : size(dat,1)/cfg.fsample : size(dat,1)];
  data.trial = {dat};
  data.time  = {time};
  Ntrials    = 1;
else
  % replace the time axis, since shifted time-axes over trials are not supported
  for i = 1:Ntrials
    data.time{i} = [1/cfg.fsample : 1/cfg.fsample : size(data.trial{i},2) / cfg.fsample];
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% prepare all data selection
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% item 1: channel combinations
numchannelcmb = Nchans*(Nchans-1)/2;
channelcmb = zeros(numchannelcmb, 2);
k = 1;

if (strcmp(cfg.order, 'Hxy'))
  for i=1:Nchans
    for j=(i+1):Nchans
      channelcmb(k, :) = [i j];
      k = k+1;
    end
  end
elseif (strcmp(cfg.order, 'Hyx'))
  for i = Nchans:-1:1
    for j = Nchans : -1:(i+1)
      channelcmb(k,:) = [i j];
      k = k+1;
    end
  end
end
numchannelcmb = size(channelcmb,1);

% item 2: time windows
% this requires
%    cfg.toi = [t1 t2 t3 t4]    center time of each window
%    cfg.timwin = 0.2           in seconds
% TODO implement "time, offset, fsample"

for j = 1:Ntrials
  time = data.time{j};
  numtimwin = size(cfg.toi,2); % initial guess, not all might fit in this trial
  timwin = zeros(numtimwin,2);
  sel = zeros(numtimwin,1);
  for i=1:numtimwin
    timwin(i,1) = cfg.toi(i) - cfg.timwin/2;  % begin of time window
    timwin(i,2) = cfg.toi(i) + cfg.timwin/2;  % end of time window
    sel(i) = timwin(i,1)>=time(1) && timwin(i,2)<=time(end);  % does it fit in?
  end
  timwin        = timwin(find(sel==1),:);
  toi_mat{j}    = cfg.toi(find(sel == 1));                        % update the configuration
  timwin_mat{j} = round((timwin - cfg.offset) * cfg.fsample);     % convert to samples
end

timwin = timwin_mat;
cfg.toi = toi_mat;

% item 3: delays within each timewindow
% this requires
%   cfg.delaystep
%   cfg.maxdelay
delay = -cfg.maxdelay:cfg.delaystep:cfg.maxdelay;
numdelay = length(delay);
% convert to samples
delay = round(delay * cfg.fsample);

if strcmp(cfg.keeptrials, 'yes')
  for trllop=1:Ntrials
    association.trial(trllop) = Do_Association_Calculation( trllop , Ntrials , cfg , timwin , numchannelcmb , numdelay , data , channelcmb , delay );
  end % for each trial
else
  trllop = 1;
  association = Do_Association_Calculation( trllop , Ntrials , cfg , timwin , numchannelcmb , numdelay , data , channelcmb , delay );
end

% add the version details of this function call to the configuration
try
  % get the full name of the function
  cfg.version.name = mfilename('fullpath');
catch
  % required for compatibility with Matlab versions prior to release 13 (6.5)
  [st, i] = dbstack;
  cfg.version.name = st(i);
end
cfg.version.id   = '$Id$';
% remember the configuration details of the input data
try, cfg.previous = data.cfg; end
% remember the exact configuration details in the output
association.cfg = cfg;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION that does the  computation for each trial
function [association] = Do_Association_Calculation( trllop , Ntrials , cfg , timwin , numchannelcmb , numdelay , data , channelcmb , delay )

fprintf('\nprocessing trial %d from %d\n', trllop, Ntrials);

association = [];
numtimwin = size(timwin{trllop},1);
h2 = zeros(numchannelcmb, numtimwin, numdelay);

l = 0;
maxl = numchannelcmb*numtimwin*numdelay;
progress('init', cfg.feedback, 'Computing nonlinear association for each delay');

for i=1:numchannelcmb
  dat1_chan = data.trial{trllop}(channelcmb(i,1),:);
  dat2_chan = data.trial{trllop}(channelcmb(i,2),:);
  progress(l/maxl,'Computing nonlinear association %d from %d', l, maxl);

  for j=1:numtimwin
    dat1_timwin = dat1_chan(timwin{trllop}(j,1):timwin{trllop}(j,2));
    dat2_timwin = dat2_chan(timwin{trllop}(j,1):timwin{trllop}(j,2));

    for k=1:numdelay
      if delay(k)==0
        % select the complete (unshifted) window
        dat1_delay = dat1_timwin;
        dat2_delay = dat2_timwin;
      elseif delay(k)<0
        % channel 1 is shifted w.r.t. channel 2
        dat1_delay = dat1_timwin((abs(delay(k))+1):end);
        dat2_delay = dat2_timwin(1:(end-abs(delay(k))));
      elseif delay(k)>0
        % channel 2 is shifted w.r.t. channel 1
        dat1_delay = dat1_timwin(1:(end-delay(k)));
        dat2_delay = dat2_timwin((delay(k)+1):end);
      end

      % remove the mean of each snippet of data
      dat1_delay = dat1_delay - mean(dat1_delay);
      dat2_delay = dat2_delay - mean(dat2_delay);

      % do the computation

      [sorted_data1,id] = sort(dat1_delay);
      sorted_data2     = dat2_delay(id);

      data_is = sorted_data1;
      data_js = sorted_data2;

      % divided data_i in bins
      [H,mp_i] = hist(data_is,cfg.nr_bins);

      % Divide data_i and data_j in bins
      bp = 1;
      gem_j = zeros (cfg.nr_bins,1);
      for s = 1:cfg.nr_bins
        gem_j(s) = mean(data_js(bp:bp+H(s)-1));
        bp = bp + H(s);
      end

      % Calculation of line segment and variance per bin
      p=1;
      sp = 1;

      clear unex_var tot_var;

      for u = 1:cfg.nr_bins
        data_is_bin = data_is(sp:sp+H(u)-1)';
        data_js_bin = data_js(sp:sp+H(u)-1)';

        if u == 1
          fx1 = gem_j(u) + (gem_j(u+1) - gem_j(u))/(mp_i(u+1) - mp_i(u))*(data_is_bin(1:round(length(data_is_bin)/2)) - mp_i(u));
        else
          fx1 = gem_j(u-1) + (gem_j(u) - gem_j(u-1))/(mp_i(u) - mp_i(u-1))*(data_is_bin(1:round(length(data_is_bin)/2)) - mp_i(u-1));
        end
        if u == cfg.nr_bins
          fx2 = gem_j(u-1) + (gem_j(u) - gem_j(u-1))/(mp_i(u) - mp_i(u-1))*(data_is_bin(round(length(data_is_bin)/2)+1:end) - mp_i(u-1));
        else
          fx2 = gem_j(u) + (gem_j(u+1) - gem_j(u))/(mp_i(u+1) - mp_i(u))*(data_is_bin(round(length(data_is_bin)/2)+1:end) - mp_i(u));
        end

        % calculation unexplained variance
        ftot = [fx1; fx2];
        unex_var(p:p+length(data_js_bin)-1,:) = (data_js_bin - ftot).^2;

        % calculation total variance
        tot_var(p:p+length(data_js_bin)-1,:) = (data_js_bin - mean(data_js)).^2;

        p = p+length(data_is_bin);
        sp = sp + H(u);
      end

      % Calculation of association coefficient h2
      h2(i,j,k) = ((sum(tot_var) - sum(unex_var)) / sum(tot_var))*100;
      l=l+1;
    end
  end
end

progress('close');

% collect the results
if (size(h2, 1) ~= numchannelcmb)
  error('number of channel combinations is not right');
end
if (size(h2, 2) ~= numtimwin)
  error('number of timewindows does not match input');
end
if (size(h2, 3) ~= numdelay)
  error('number of delays does not match input');
end
h2 = reshape(h2, numchannelcmb*numtimwin, numdelay);
[m, indx] = max(h2, [], 2);
h2 = reshape(m, numchannelcmb, numtimwin);
d = (delay(indx)./cfg.fsample)*1000; % convert to miliseconds (to compare with original method)
d = reshape(d, numchannelcmb, numtimwin);

% FIXME this does not work: one is not supposed to rely on data.cfg.trl, use data.time please!
% association.time = cfg.toi{trllop} + data.cfg.trl(trllop,1);

association.h2 = h2;
association.delay = d;

return % from Do_Association_Calculation

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION that computes the mean over all values without checking the dimensions
% this function is approximately 2x faster on 1 dimensional data than the matlab version
function y = mean(x)
y = sum(x(:))./numel(x);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION that bins the elements of Y into equally spaced containers
% this function is approximately 2x faster on 1 dimensional data than the matlab version
function [nn, x] = hist(y, x)
miny = min(y);
maxy = max(y);
binwidth = (maxy - miny) ./ x;
xx = miny:binwidth:maxy;
x  = xx(1:(end-1)) + binwidth/2;
% Shift bins so the interval is ( ] instead of [ ).
xx = full(real(xx)); y = full(real(y)); % For compatibility
bins = xx + eps(xx);
nn = histc(y',[-inf bins],1);
% Combine first bin with 2nd bin and last bin with next to last bin
nn(2,:) = nn(2,:)+nn(1,:);
nn(end-1,:) = nn(end-1,:)+nn(end,:);
nn = nn(2:end-1,:);


classdef IMMOEA_F3 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing IM-MOEA

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, K. Narukawa, and B. Sendhoff, A multiobjective
% evolutionary algorithm using Gaussian process-based inverse modeling,
% IEEE Transactions on Evolutionary Computation, 2015, 19(6): 838-856.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = (1+5*repmat(2:obj.D,size(X,1),1)/obj.D).*X(:,2:obj.D) - repmat(X(:,1),1,obj.D-1);
            g = 1 + 9*mean(t.^2,2);
            PopObj(:,1) = 1 - exp(-4*X(:,1)).*sin(6*pi*X(:,1)).^6;
            PopObj(:,2) = g.*(1-(PopObj(:,1)./g).^2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            minf1  = min(1-exp(-4*(0:1e-6:1)).*(sin(6*pi*(0:1e-6:1))).^6);
            R(:,1) = linspace(minf1,1,N)';
            R(:,2) = 1 - R(:,1).^2;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function handle = draw_waypoints(fig_handle, wpt_list, radius, handle, mode)
% DRAW_WAYPOINTS - plot waypoints

    if wpt_list(1,4)==-9999 % check to see if Dubins paths
        XX = [wpt_list(:,1)];
        YY = [wpt_list(:,2)];
        ZZ = [wpt_list(:,3)];
    else
        XX = [];
        YY = [];
        ZZ = [];
        for i=2:size(wpt_list,1)
            dubinspath = compute_dubins_param(wpt_list(i-1,:),wpt_list(i,:),radius);
            [tmpX,tmpY,tmpZ] = points_along_dubins_path(dubinspath,0.1);
            XX = [XX; tmpX];
            YY = [YY; tmpY];
            ZZ = [ZZ; tmpZ];
        end
%         ZZ = wpt_list(i,3)*ones(size(XX));
    end

    if isempty(handle)
        handle = plot3(fig_handle.Children, YY, XX, -ZZ, 'b');
    else
        set(handle,'XData', YY, 'YData', XX, 'ZData', -ZZ);
        drawnow
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X,Y,Z] = points_along_dubins_path(dubinspath,Del)
% Points Along Dubins Path -
%   Find points along Dubin's path separted by Del (to be used in
%   collision detection)

  % points along start circle
  th1 = mod(atan2(dubinspath.ps(2)-dubinspath.cs(2),dubinspath.ps(1)-dubinspath.cs(1)),2*pi);
  th2 = mod(atan2(dubinspath.w1(2)-dubinspath.cs(2),dubinspath.w1(1)-dubinspath.cs(1)),2*pi);
  if dubinspath.lams>0
      if th1>=th2
        th = [th1:Del:2*pi,0:Del:th2];
      else
        th = [th1:Del:th2];
      end
  else
      if th1<=th2
        th = [th1:-Del:0,2*pi:-Del:th2];
      else
        th = [th1:-Del:th2];
      end
  end
  X = [];
  Y = [];
  Z = [];
  for i=1:length(th)
    X = [X; dubinspath.cs(1)+dubinspath.R*cos(th(i))];
    Y = [Y; dubinspath.cs(2)+dubinspath.R*sin(th(i))];
    Z = [Z; dubinspath.cs(3)];
  end

  % points along straight line
  sig = 0;
  while sig<=1
      X = [X; (1-sig)*dubinspath.w1(1) + sig*dubinspath.w2(1)];
      Y = [Y; (1-sig)*dubinspath.w1(2) + sig*dubinspath.w2(2)];
      Z = [Z; (1-sig)*dubinspath.w1(3) + sig*dubinspath.w2(3)];
      sig = sig + Del;
  end

  % points along end circle
  th2 = mod(atan2(dubinspath.pe(2)-dubinspath.ce(2),dubinspath.pe(1)-dubinspath.ce(1)),2*pi);
  th1 = mod(atan2(dubinspath.w2(2)-dubinspath.ce(2),dubinspath.w2(1)-dubinspath.ce(1)),2*pi);
  if dubinspath.lame>0
      if th1>=th2
        th = [th1:Del:2*pi,0:Del:th2];
      else
        th = [th1:Del:th2];
      end
  else
      if th1<=th2
        th = [th1:-Del:0,2*pi:-Del:th2];
      else
        th = [th1:-Del:th2];
      end
  end
  for i=1:length(th)
    X = [X; dubinspath.ce(1)+dubinspath.R*cos(th(i))];
    Y = [Y; dubinspath.ce(2)+dubinspath.R*sin(th(i))];
    Z = [Z; dubinspath.ce(3)];
  end
end


function [C,nc] = graph_coloring(A,nc)
  % GRAPH_COLORING Color a graph given an adjacency matrix. This heuristic is
  % designed to efficiently find a conservative coloring (i.e., using too many
  % colors). It does *not* attempt to find the minimal coloring. It is best used
  % when the provided nc is a few more than the optimal coloring. For example,
  % planar graphs can be colored with 4 colors, (slow) heuristics for finding the
  % optimal coloring will often find 6 colors, but this function will very
  % quickly find a 7-coloring (nc=7). On such a graph, if you passed nc=4, this
  % function will likely waste time attemping to find a 4-coloring, 5-coloring,
  % 6-coloring, and then very quickly find a 7-coloring. It is designed to give
  % up searching for parsimonious colorings and increase the number of colors
  % (throwing a warning).
  %
  % [C,nc] = graph_coloring(A,nc)
  %
  % Inputs:
  %   A  #V by #V adjacency matrix
  %   nc  desired number of colors (for planar/triangle meshes, 7 is a fast
  %     choice; for tetrahedral meshes, 13 appears to often be fast)
  % Outputs:
  %   C  #V list of color ids into (1:nc)
  %   nc  effective number of colors >= input nc
  %
  % Examples:
  %   C = graph_coloring(facet_adjacency_matrix(F),9);
  %   tsurf(F,V,'CData',C);
  %   colormap(cbrewer('Set1',max(C)));

  n = size(A,1);
  [FI,FJ] = find(triu(A,1));
  % always marking the higher valence vertex as bad definitely makes things
  % worse 50x.
  %
  % if nc is much greater than optimal, always marking the lower valence vertex
  % doesn't much effect. But for nc close to optimal, this seems to make a very
  % big difference (300x)
  val = sum(A,2);
  swap = val(FI)<val(FJ);
  EI = FI.*swap + FJ.*~swap;
  EJ = FJ.*swap + FI.*~swap;

  onc = nc;
  % worst case just increase number of colors
  max_outer = 10;
  for outer = 1:max_outer
    % Might as well retry a few times for this nc
    for retry = 1:10
      C = ceil(nc*rand(n,1));
      for iter = 1:2*ceil(sqrt(size(A,1)))
        bad = unique(EI(C(EI)==C(EJ)));
        if numel(bad) == 0
          break;
        end
        C(bad) = nan;
        C(bad) = ceil(nc*rand(numel(bad),1));
      end
      if numel(bad) == 0
        break;
      end
    end
    if numel(bad) == 0
      break;
    end
    nc = nc+1;
    if nc>max_outer
      error('Failed to converge.\n');
    end
  end
  if nc ~= onc
    warning('Had to increasing number of colors')
  end

  %V2C = sparse(1:n,C,1,n,nc);
  % find all edges with the same color
  %A*V2C;
end


function varargout = pnes_master(fcn, x0, opt)
%PNES_MASTER  Parameterized Nonlinear Equation Solver wrapper function.
%   [X, F, EXITFLAG, OUTPUT, JAC] = PNES_MASTER(FCN, X0, OPT)
%   [X, F, EXITFLAG, OUTPUT, JAC] = PNES_MASTER(PROBLEM)
%   A common wrapper function for numerical continuation methods for
%   solving parameterized nonlinear equations. Traces the solutions of
%   a parameterized nonlinear equation f(x) = 0, beginning from a starting
%   point x0, where f(x) has dimension n and x has dimension n+1.
%
%   In the current implementation, the last element of x is taken to be
%   the parameter lambda, where lambda = 0 corresponds to the base solution.
%
%   Inputs:
%       FCN : handle to function that evaluates the function f(x) to
%           be solved and its Jacobian, J(x). Calling syntax for this
%           function is:
%               f = FCN(x)
%               [f, J] = FCN(x)
%           For a parameterized function, f is n x 1, x is (n+1) x 1,
%           and J is the n x (n+1) matrix of partial derivatives of
%           f (rows) w.r.t. x (cols).
%       X0 : starting value, x0, of vector x ((n+1) x 1)
%       OPT : optional options structure with the following fields,
%           all of which are also optional (default values shown in
%           parentheses)
%           alg ('DEFAULT') : determines which solver to use
%               'DEFAULT' : automatic, currently there is only one
%               solver implementation, a predictor/corrector method
%           verbose (0) - controls level of progress output displayed
%               0 = no progress output
%               1-5 = increasing levels of progress output
%           nleqs_opt - options struct for NLEQS_MASTER used for
%               corrector stage (see NLEQS_MASTER for details), default
%               sets nleqs_opt.verbose to 0, otherwise to 2 if OPT.verbose > 4
%           solve_base (1) : 0/1 flag that determines whether or not to
%               run a corrector stage for initial solution point, x0
%           parameterization (3) - choice of parameterization
%               1 - natural
%               2 - arc len
%               3 - pseudo arc len
%           stop_at ('NOSE') - determines stopping criterion
%               'NOSE'     - stop when limit or nose point is reached
%               'FULL'     - trace full continuation curve
%               <lam_stop> - stop upon reaching specified target lambda value
%           max_it (2000) - maximum number of continuation steps
%           step (0.05) - continuation step size
%           adapt_step (0) - toggle adaptive step size feature
%               0 - adaptive step size disabled
%               1 - adaptive step size enabled
%           adapt_step_damping (0.7) - damping factor for adaptive step sizing
%           adapt_step_tol (1e-3) - tolerance for adaptive step sizing
%           adapt_step_ws (1) - scale factor for default initial step size
%               when warm-starting with adaptive step size enabled
%           step_min (1e-4) - minimum allowed step size
%           step_max (0.2) - maximum allowed step size
%           default_event_tol (1e-3) - default tolerance for event functions
%           target_lam_tol (0) - tolerance for target lambda detection, 0 means
%               use the value of default_event_tol
%           nose_tol (0) - tolerance for nose point detection, 0 means use
%               the value of default_event_tol
%           events (<empty>) - cell array of specs for user-defined event
%               functions, passed as MY_EVENTS arg to PNE_REGISTER_EVENTS
%               (see PNE_REGISTER_EVENTS for details).
%           callbacks (<empty>) - cell array of specs for user-defined callback
%               functions, to be passed as MY_CBACKS arg to
%               PNE_REGISTER_CALLBACKS (see PNE_REGISTER_CALLBACKS for details).
%           output_fcn (<empty>) - handle to custom output function, called by
%               PNE_CALLBACK_DEFAULT
%           plot - options for plotting of continuation curve by
%               PNE_CALLBACK_DEFAULT
%               .level (0) - control plotting of continuation curve
%                   0 - do not plot continuation curve
%                   1 - plot when completed
%                   2 - plot incrementally at each continuation step
%                   3 - same as 2, with 'pause' at each continuation step
%               .idx (<empty>) - index of quantity to plot, passed to yfcn()
%               .idx_default (<empty>) - function to provide default value
%                   for idx, if none provided
%               .xname ('lam') - name of output field holding values that
%                   determine horizontal coordinates of plot
%               .yname ('x') - name of output field holding values that
%                   determine vertical coordinates of plot
%               .xfcn (<empty>) - handle to function that maps a value from
%                   the field of the OUTPUT indicated by value of plot.xname
%                   to a horizontal coordinate for plotting
%               .yfcn (<empty>) - handle to function that maps a value from
%                   the field of the OUTPUT indicated by value of plot.yname
%                   and an index to be applied to that value into a vertical
%                   coordinate for plotting
%               .xlabel ('\lambda') - label for horizontal axis
%               .ylabel ('Variable Value') - label for vertical axis
%               .title ('Value of Variable %d') - plot title used for plot of
%                   single variable, can use %d as placeholder for var index
%               .title2 ('Value of Multiple Variables') - plot title used for
%                   plot of multiple variables
%               .legend ('Variable %d') - legend label, %d can be used as
%                   placeholder for variable index
%           warmstart (<empty>) - struct containing warm-start state, see
%               warmstart field in OUTPUT below for details of expected
%               fields
%       PROBLEM : The inputs can alternatively be supplied in a single
%           PROBLEM struct with fields corresponding to the input arguments
%           described above: fcn, x0, opt
%
%   Outputs (all optional, except X):
%       X : solution vector x
%       F : final function value, f(x)
%       EXITFLAG : exit flag
%           1 = succeeded
%           0 = failed
%       OUTPUT : output struct with the following fields:
%           corrector - output return value from NLEQS_MASTER from final
%               corrector run (see NLEQS_MASTER for details)
%           iterations - N, total number of continuation steps performed
%           events - struct array of size NE of events detected with fields:
%               k - continuation step at which event was located
%               name - name of detected event
%               idx - index(es) of critical elements in corresponding event
%                   function
%               msg - descriptive text detailing the event
%           done_msg - message describing cause of continuation termination
%           steps - (N+1) row vector of stepsizes taken at each continuation
%               step
%           lam_hat - (N+1) row vector of lambda values from prediction steps
%           lam - (N+1) row vector of lambda values from correction steps
%           max_lam - maximum value of parameter lambda (from OUTPUT.lam)
%           warmstart - optional output with information needed for
%               warm-starting an updated continuation problem, with fields:
%               cont_steps - current value of continuation step counter
%               direction - +1 or -1, for tracing of curve in same or
%                   opposite direction, respectively
%               dir_from_jac_eigs - 0/1 flag to indicate whether to use
%                   the sign of the smallest eigenvalue of the Jacobian to
%                   determine the initial direction
%               x - current solution vector
%               z - current tangent vector
%               xp - previous step solution vector
%               zp - previous step tangent vector
%               parm - function handle for current parameterization function
%               default_parm - function handle for default parameterization fcn
%               default_step - default step size
%               events - current event log, same as OUTPUT.events
%               cbs - struct containing user state information for callbacks
%                   see PNES_CALLBACK_DEFAULT for more details
%           (others) - depends on OPT.output_fcn, by default (i.e. with no
%               explicitly provided output function) includes fields:
%                   x_hat - NX x (N+1) matrix of solution values from
%                       prediction steps
%                   x - NX x (N+1) matrix of solution values from correction
%                       steps
%       JAC : final Jacobian matrix, J(x)
%
%   Calling syntax options:
%       [x, f, exitflag, output, jac] = pnes_master(fcn, x0);
%       [x, f, exitflag, output, jac] = pnes_master(fcn, x0, opt);
%       x = pnes_master(problem);
%               where problem is a struct with fields: fcn, x0, opt
%               where opt is optional
%       x = pnes_master(...);
%       [x, f] = pnes_master(...);
%       [x, f, exitflag] = pnes_master(...);
%       [x, f, exitflag, output] = pnes_master(...);
%       [x, f, exitflag, output, jac] = pnes_master(...);
%
%   Example: (based on https://www.chilimath.com/lessons/advanced-algebra/systems-non-linear-equations/)
%       function [f, J] = f1p(x)
%           f = [  x(1)   + x(2) + 6*x(3) - 1;
%                 -x(1)^2 + x(2)          + 5   ];
%           if nargout > 1
%               J = [1 1 6; -2*x(1) 1 0];
%           end
%       end
%       problem = struct( ...
%           'fcn',  @(x)f1p(x), ...
%           'x0',   [-1; 0; 0], ...
%           'opt',  struct('verbose', 2, 'adapt_step', 1, 'step_max', 10) ...
%       );
%       [x, f, exitflag, output, jac] = pnes_master(problem);
%
%   See also PNE_CALLBACK_DEFAULT, PNE_REGISTER_CALLBACKS, PNE_REGISTER_EVENTS

%   MP-Opt-Model
%   Copyright (c) 2013-2021, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell,
%   Shrirang Abhyankar, Argonne National Laboratory,
%   and Alexander Flueck, IIT
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

%%----- input argument handling  -----
%% gather inputs
if nargin == 1 && isstruct(fcn) %% problem struct
    p = fcn;
    fcn = p.fcn;
    x0 = p.x0;
    if isfield(p, 'opt'),   opt = p.opt;    else,   opt = struct(); end
else                            %% individual args
    if nargin < 3
        opt = struct();
    end
end

%% default options
if isfield(opt, 'verbose') && opt.verbose > 4
    nleqs_opt_verbose = 2;
else
    nleqs_opt_verbose = 0;
end
dopts = struct( ...
    'alg',              'DEFAULT', ...  %% algorithm
    'verbose',          0, ...
    'nleqs_opt',        struct('verbose', nleqs_opt_verbose), ...
    'solve_base',       1, ...          %% run corrector for initial point
    'parameterization', 3, ...          %% 1 - natural, 2 - arc len, 3 - pseudo arc len
    'stop_at',          'NOSE', ...     %% 'NOSE', 'FULL', <lam_stop>
    'max_it',           2000, ...       %% maximum number of continuation steps
    'step',             0.05, ...       %% continuation step size
    'step_min',         1e-4, ...       %% minimum allowed step size
    'step_max',         0.2, ...        %% maximum allowed step size
    'adapt_step',       0, ...          %% 0/1 toggle, adaptive step size
    'adapt_step_ws',    1, ...          %% warm start inital step size scale factor
    'adapt_step_damping', 0.7, ...      %% adaptive step sizing damping factor
    'adapt_step_tol',   1e-3, ...       %% adaptive step sizing tolerance
    'default_event_tol',1e-3, ...       %% default event function tolerance
    'target_lam_tol',   0, ...          %% event function tolerance for TARGET_LAM event
    'nose_tol',         0, ...          %% event function tolerance for NOSE event
    'events',           {{}}, ...       %% user-defined event detection functions
    'callbacks',        {{}}, ...       %% user-defined callback functions
    'output_fcn',       [], ...         %% custom output fcn, default callback
    'warmstart',        [], ...         %% struct containing warm-start state
    'plot',             struct( ...     %% used by pne_callback_default() for plotting
        'level',        0, ...          %% 0 - no plot, 1 - final, 2 - steps, 3 - steps w/pause
        'idx',          [], ...         %% index of quantity to plot, passed to yfcn()
        'idx_default',  [], ...         %% fcn to provide default value for idx, if none provided
        'xfcn',         [], ...         %% fcn to compute x-coord from data
        'yfcn',         [], ...         %% fcn to compute x-coord from data, idx
        'title',        'Value of Variable %d', ... %% plot title for single var plot
        'title2',       'Value of Multiple Variables', ...  %% plot title for multiple var plot
        'xname',        'lam', ...      %% name of output field holding x vals
        'yname',        'x', ...        %% name of output field holding y vals
        'xlabel',       '\lambda', ...              %% horizontal axis label
        'ylabel',       'Variable Value', ...       %% vertical axis label
        'legend',       'Variable %d' ...           %% legend label
    ) ...
);
opt = nested_struct_copy(dopts, opt);
%% use opt.default_event_tol for NOSE and TARGET_LAM events, unless specified
if opt.target_lam_tol == 0
    opt.target_lam_tol = opt.default_event_tol;
end
if opt.nose_tol == 0
    opt.nose_tol = opt.default_event_tol;
end
if opt.max_it == 0      %% zero means use the default
    opt.max_it == dopts.max_it;
end

%% initialize
warmstarted = ~isempty(opt.warmstart);
s = struct( ...         %% container struct for various variables, flags
    'done',     0, ...      %% flag indicating continuation has terminated
    'done_msg', '', ...     %% termination message
    'warmstart',[], ...     %% warm start state to return when done (to pass
                            ...%% to subsequent warm-started call to PNES_MASTER)
    'rollback', 0, ...      %% flag to indicate a step must be rolled back
    'events',    [], ...    %% struct array for detected events
    'results',  []  );      %% results struct

%% register event and callback functions
switch opt.stop_at
    case 'NOSE'
        my_events = {{'NOSE', @pne_event_nose, opt.nose_tol}, opt.events{:}};
        my_cbacks = {{@pne_callback_nose, 51}, opt.callbacks{:}};
    case 'FULL'
        my_events = {{'TARGET_LAM', @pne_event_target_lam, opt.target_lam_tol}, opt.events{:}};
        my_cbacks = {{@pne_callback_target_lam, 50}, opt.callbacks{:}};
    otherwise   %% numeric, stop at target lam or nose point, whichever is 1st
        my_events = {{'TARGET_LAM', @pne_event_target_lam, opt.target_lam_tol}, ...
                     {'NOSE', @pne_event_nose, opt.nose_tol}, ...
                        opt.events{:}};
        my_cbacks = {{@pne_callback_target_lam, 50}, ...
                     {@pne_callback_nose, 51}, ...
                        opt.callbacks{:}};
end
my_cbacks{end+1} = {@pne_callback_default, 0};
reg_ev = pne_register_events(my_events, opt);   %% registered event functions
reg_cb = pne_register_callbacks(my_cbacks);     %% registered callback functions
nef = length(reg_ev);   %% number of registered event functions
ncb = length(reg_cb);   %% number of registered callback functions

%% initialize continuation step counter
if warmstarted
    cont_steps = opt.warmstart.cont_steps + 1;
    if opt.verbose
        fprintf('... CONTINUATION RESUMED\n');
    end
else
    cont_steps = 0;
    if opt.verbose
        v = mpomver('all');
        fprintf('\nMP-Opt-Model Version %s, %s', v.Version, v.Date);
        fprintf(' -- Predictor/Corrector Continuation Method\n');
    end
end

%% solve corrector step for base point
if opt.solve_base && ~warmstarted
    cfcn = @(xx)pne_corrector_fcn(xx, fcn, @pne_pfcn_natural, x0, 0, []);
    [x, f, exitflag, out] = nleqs_master(cfcn, x0, opt.nleqs_opt);
    if exitflag
        if opt.verbose > 1
            fprintf('step %3d  :                          lambda = %6.3f, %2d corrector steps\n', cont_steps, x0(end), out.iterations);
        end
    else
        s.done = 1;
        s.done_msg = sprintf('base solution did not converge in %d iterations', out.iterations);
        if opt.verbose
            fprintf('%s\n', s.done_msg);
        end
    end
else
    x = x0;     %% ignored for warmstart, overwritten by warmstart.cx
end

%% initialize numerical continuation
if ~s.done
    locating = 0;   %% flag to indicate that an event interval was detected,
                    %% but the event has not yet been located
    rb_cnt_ef = 0;  %% counter for rollback steps triggered by event function intervals
    rb_cnt_cb = 0;  %% counter for rollback steps triggered directly by callbacks

    if warmstarted
        manual_direction_switch = 0;    %% set to 1 to manually prompt for
                                        %% direction change upon warmstart
        ws = opt.warmstart;
        x = ws.x;           %% starting value solution vector
        z = ws.z;           %% starting value of tangent vector
        step = 0;           %% re-solve current point
        parm = ws.parm;
        direction = ws.direction;
        default_parm = ws.default_parm;
        default_step = ws.default_step;
        cbs = ws.cbs;
        event_log = ws.events;
        if manual_direction_switch
            %% decide whether to switch directions
            reply = input('Switch directions? Y/N [N]:','s');
            if strcmp(upper(reply), 'Y')
                direction = -direction;
            end
        elseif isfield(ws, 'dir_from_jac_eigs') && ws.dir_from_jac_eigs
            %% attempt to determine direction from smalles Jacobian eigenvalue
            [~, J] = fcn(x);
            eigs_opt.tol = 1e-3;
            eigs_opt.maxit = 2*length(x);
            direction = sign(z(end) * ...
                        min(real(eigs(J(:,1:end-1), 1, 'sr', eigs_opt))));
        end

        if opt.adapt_step   %% hey, maybe slow down, things might have changed
            default_step = default_step * opt.adapt_step_ws;
        end
    else
        %% initialize parameterization function
        switch opt.parameterization
            case 1
                parm = @pne_pfcn_natural;           %% NAT
            case 2
                parm = @pne_pfcn_arc_len;           %% ARC
            case 3
                parm = @pne_pfcn_pseudo_arc_len;    %% PAL
            otherwise
                error('pnes_master: OPT.parameterization (= %d) must be 1, 2, or 3', opt.parameterization);
        end

        %% finish initializing tangent vector
        direction = 1;
        z0 = zeros(length(x), 1); z0(end) = direction;  %% +ve lambda direction
        z = pne_tangent(x, x, z0, fcn, parm, direction);

        step = opt.step;
        default_step = step;
        default_parm = parm;
        cbs = [];
        event_log = [];
    end

    %% initialize state struct for current continuation step
    cx = struct( ...        %% current state
        'x_hat',        x, ...      %% predicted solution value
        'x',            x, ...      %% corrected solution value
        'z',            z, ...      %% normalized tangent vector
        'default_step', default_step, ...   %% default step size
        'default_parm', default_parm, ...   %% default parameterization
        'this_step', [], ...        %% step size for this step only
        'this_parm', [], ...        %% parameterization for this step only
        'step', step, ...           %% current step size
        'parm', parm, ...           %% current parameterization
        'events', event_log, ...    %% event log
        'cbs', cbs, ...             %% user-defined callback state
        'efv', [] ...               %% event function values
    );

    %% initialize event function values
    cx.efv = cell(nef, 1);
    for k = 1:nef
        cx.efv{k} = reg_ev(k).fcn(cx, opt);
    end

    if warmstarted  %% no need to initialize callbacks
        %% initialize state for previous continuation step
        px = cx;
        px.x = ws.xp;   %% use warm start value for solution value
        px.z = ws.zp;   %% use warm start value for tangent
    else
        %% invoke callbacks - "initialize" context
        for k = 1:ncb
            [nx, cx, s] = reg_cb(k).fcn(cont_steps, cx, cx, cx, s, opt);
        end
        cont_steps = cont_steps + 1;

        %% check for case with base and target the same
        %% evaluate function at lambda = 0 (base)
        if opt.solve_base
            fb = f(1:end-1);
        else
            fb = fcn(x);
            exitflag = 1;
        end

        %% evaluate function at lambda = 1 (target)
        xt = x;
        xt(end) = 1;
        ft = fcn(xt);
        if norm(fb - ft, Inf) < 1e-12
            s.done = 1;
            s.done_msg = 'base and target functions are identical';
        end

        %% initialize state for previous continuation step
        px = cx;
    end
end

%%-----  run numerical continuation  -----
while ~s.done
    %% initialize next candidate with current state
    nx = cx;

    %% predictor step
    nx.x_hat = cx.x + cx.step * cx.z;

    %% corrector step
    cfcn = @(xx)pne_corrector_fcn(xx, fcn, cx.parm, cx.x, cx.step, cx.z);
    [nx.x, f, exitflag, out] = nleqs_master(cfcn, nx.x_hat, opt.nleqs_opt);
    if ~exitflag        %% corrector failed
        s.done = 1;
        s.done_msg = sprintf('Corrector did not converge in %d iterations.', out.iterations);
        if opt.verbose
            fprintf('step %3d  : %s stepsize = %-9.3g lambda = %6.3f  corrector did not converge in %d iterations\n', cont_steps, pne_ptag(cx.parm), cx.step, nx.x(end), out.iterations);
        end
        cont_steps = max(cont_steps - 1, 1);    %% go back to last step, but not to 0
        break;
    end

    %% compute new tangent direction, based on current or prev state: tx
    if nx.step == 0     %% if this is a re-do step, cx and nx are the same
        tx = px;            %% so use px as the previous state
    else                %% otherwise
        tx = cx;            %% use cx as the previous state
    end
    nx.z = pne_tangent(nx.x, tx.x, tx.z, fcn, nx.parm, direction);
    direction = 1;      %% continue in same direction

    %% detect events
    for k = 1:nef
        nx.efv{k} = reg_ev(k).fcn(nx, opt); %% update event function values
    end
    [s.rollback, s.events, nx.efv] = ...
        pne_detect_events(reg_ev, nx.efv, cx.efv, nx.step);

    %% adjust step-size to locate event function zero, if necessary
    if s.rollback               %% current step overshot
        %% roll back & initialize next step size based on rollback and previous
        rx = nx;                    %% save state we're rolling back from
        rx_evnts = s.events;        %% and critical event info
        cx.this_step = s.events.step_scale * rx.step;
        cx.this_parm = rx.parm;     %% keep same parameterization as last step
        locating = 1;               %% enter "locating" mode (or stay in it)
        rb_cnt_ef = rb_cnt_ef + 1;  %% increment rollback counter for ef intervals
        if rb_cnt_ef > 26
            s.done = 1;
            s.done_msg = sprintf('Could not locate %s event!', s.events.name);
        end
        if opt.verbose > 3
            loc_msg = sprintf('OVERSHOOT  : f = [%g, <<%g>>], step <-- %.4g', ...
                        cx.efv{s.events.eidx}(s.events.idx(1)), ...
                        rx.efv{s.events.eidx}(s.events.idx(1)), cx.this_step);
        end
    elseif locating
        if s.events(1).zero      %% found the zero!
            %% step size will be reset to previously used default step size
            locating = 0;           %% exit "locating" mode
            rb_cnt_ef = 0;          %% reset rollback counter for ef intervals
            if opt.verbose > 3
                loc_msg = sprintf('ZERO!      : f = %g, step <-- %.4g', ...
                    nx.efv{rx_evnts.eidx}(rx_evnts.idx(1)), nx.default_step);
            end
        else                    %% prev rollback undershot
            %% initialize next step size based on critical event function
            %% values from prev rollback step and current step
            rx_efv = rx.efv{rx_evnts.eidx}(rx_evnts.idx(1));
            cx_efv = nx.efv{rx_evnts.eidx}(rx_evnts.idx(1));
            step_scale = cx_efv / (cx_efv - rx_efv);
            nx.this_step = step_scale * (rx.step - nx.step);
            rb_cnt_ef = 0;          %% reset rollback counter for ef intervals
            if opt.verbose > 3
                loc_msg = sprintf('UNDERSHOOT : f [<<%g>>, %g], step <-- %.4g', ...
                    cx_efv, rx_efv, nx.this_step);
            end
        end
    else                    %% normal step, not locating anything
        loc_msg = '';
    end

    %% invoke callbacks - "iterations" context
    rb = s.rollback;
    for k = 1:ncb
        [nx, cx, s] = reg_cb(k).fcn(cont_steps, nx, cx, px, s, opt);
    end
    if ~rb && s.rollback    %% rollback triggered by callback (vs event function interval)
        rb_cnt_cb = rb_cnt_cb + 1;  %% increment rollback counter for callbacks
        if rb_cnt_cb > 26
            s.done = 1;
            s.done_msg = 'Too many rollback steps triggered by callbacks!';
        end
    else
        rb_cnt_cb = 0;              %% reset rollback counter for callbacks
    end

    %% print iteration information
    if opt.verbose > 1
        %% set label for rollback step counter
        if rb_cnt_ef
            sub_step = char('a' + rb_cnt_ef - 1);
        elseif rb_cnt_cb
            sub_step = char('A' + rb_cnt_cb - 1);
        else
            sub_step = ' ';
        end

        fprintf('step %3d%s : %s stepsize = %-9.3g lambda = %6.3f', cont_steps, sub_step, pne_ptag(cx.parm), cx.step, nx.x(end));
        if opt.verbose < 5
            fprintf('  %2d corrector steps', out.iterations);
        end
        if s.rollback
            fprintf(' ^ ROLLBACK\n');
        else
            fprintf('\n');
        end
        if opt.verbose > 3 && ~isempty(loc_msg)
            fprintf('    LOCATING -- %s\n', loc_msg);
        end
    end

    %% log events
    for k = 1:length(s.events)
        if s.events(k).log
            e = struct( 'k', cont_steps, ...
                        'name', s.events(k).name, ...
                        'idx', s.events(k).idx, ...
                        'msg', s.events(k).msg   );
            if isempty(nx.events)
                nx.events = e;
            else
                nx.events(end+1) = e;
            end
        end
        if (opt.verbose > 2 && s.events(k).log) || ...
                (opt.verbose > 3 && s.events(k).eidx)
            fprintf('    %s\n', s.events(k).msg);
        end
    end

    %% adapt stepsize if requested and not terminating, locating a zero
    %% or warm starting
    if opt.adapt_step && ~s.done && ~locating && ~s.events(1).zero && nx.step ~= 0
        pred_error = norm(nx.x - nx.x_hat, Inf);

        %% new nominal step size is current size * tol/err, but we reduce
        %% the change from the current size by a damping factor and limit
        %% increases to a factor of 2
        step_scale = min(2, 1 + opt.adapt_step_damping * ...
                        (opt.adapt_step_tol/pred_error - 1));
        nx.default_step = nx.step * step_scale;

        %% limit step-size
        if nx.default_step > opt.step_max
            nx.default_step = opt.step_max;
        end
        if nx.default_step < opt.step_min
            nx.default_step = opt.step_min;
        end
    end

    %% if this is a normal step
    if ~s.rollback
        px = cx;    %% save current state before update
        cx = nx;    %% update current state to next candidate
        if ~s.done
            if cont_steps >= opt.max_it
                s.done = 1;
                s.done_msg = sprintf('Reached maximun number of continuation steps (opt.max_it = %d)', opt.max_it);
            else
                cont_steps = cont_steps + 1;
            end
        end
    end

    %% set step size and parameterization, from one-time or defaults
    if isempty(cx.this_step)
        cx.step = cx.default_step;
    else
        cx.step = cx.this_step;
        cx.this_step = [];      %% disable for next time
    end
    if isempty(cx.this_parm)
        cx.parm = cx.default_parm;
    else
        cx.parm = cx.this_parm;
        cx.this_parm = [];      %% disable for next time
    end
end     %% while ~s.done

%% invoke callbacks - "final" context
s.results = struct();   %% initialize results struct
for k = 1:ncb
    [nx, cx, s] = reg_cb(k).fcn(-cont_steps, nx, cx, px, s, opt);
end
output = s.results;
output.done_msg = s.done_msg;
output.events = cx.events;  %% copy eventlog to results
output.corrector = out;     %% output from last corrector run

%% prepare to exit
if isempty(s.warmstart)
    if opt.verbose
        fprintf('CONTINUATION TERMINATION: %s\n', s.done_msg);
    end
else
    %% save warmstart values
    ws = s.warmstart;
    ws.cont_steps = cont_steps;
    ws.direction = direction;

    %% from state at current step
    ws.x = cx.x;            %% state from current step
    ws.z = cx.z;            %% tangent vector from current step
    ws.parm = cx.parm;
    ws.default_parm = cx.default_parm;
    ws.default_step = cx.default_step;
    ws.events = cx.events;
    ws.cbs = cx.cbs;

    %% from state at previous step
    ws.xp = px.x;           %% state from previous step
    ws.zp = px.z;           %% tangent vector from previous step

    output.warmstart = ws;

    if opt.verbose
        fprintf('%s : CONTINUATION SUSPENDED ...\n', s.done_msg);
    end
end

%% output arguments
if nargout > 4
    [f, J] = fcn(cx.x);
elseif nargout > 1
    f = fcn(cx.x);
end
varargout{1} = cx.x;
if nargout > 1
    varargout{2} = f;
    if nargout > 2
        varargout{3} = exitflag;
        if nargout > 3
            varargout{4} = output;
            if nargout > 4
                varargout{5} = J;
            end
        end
    end
end

%%-----  pne_corrector_fcn  -----
%% fcn(x) combined with parameterization constraint
function [fp, dfp] = pne_corrector_fcn(x, fcn, parm, cx_x, step, z)
if nargout < 2
    fp = [ fcn(x); parm(x, cx_x, step, z) ];
else
    [f, df] = fcn(x);
    [p, dp] = parm(x, cx_x, step, z);
    fp = [f; p];
    dfp = [df; dp];
end

%%-----  pne_tangent  -----
%% find normalized tangent vector
function z = pne_tangent(x, xp, zp, fcn, parm, direction)
[f, df] = fcn(x);
[p, dp] = parm(x, xp, 0, zp);
rhs = [ zeros(length(f), 1); direction ];
z = [df; dp] \ rhs;
z = z / norm(z);    %% normalize it

%%-----  pne_ptag  -----
%% return 3-letter string to indicate parameterization scheme
%% NAT - natural, ARC - arc length, PAL - pseudo arc length
function ptag = pne_ptag(parm)
switch func2str(parm)
    case 'pne_pfcn_natural'
        ptag = 'NAT';
    case 'pne_pfcn_arc_len'
        ptag = 'ARC';
    case 'pne_pfcn_pseudo_arc_len'
        ptag = 'PAL';
end


function numLS=leapSecsOnDay(year,month,day)
%%LEAPSECSONDAY Returns the number of leap seconds in a day for dates from
%               1972 onward. Prior to 1972, non-integer leap second numbers
%               were used, meaning that the time of day matters. From 1972
%               onward, one could have a positive or negative leap second,
%               meaning that the last minute of the day in coordinated
%               universal time (UTC) might contain 59 or 61 seconds.
%
%INPUTS: year    An integer  year in the Gregorian calendar under UTC
%                time. year>= 1960 when UTC started.
%       month    An integer month in the Gregorian calendar under UTC
%                time. 1<=month<=12
%        day     An integer day in the Gregorian calendar under UTC
%                time. Days count from 1.
%
%OUTPUTS: numLS  The number of leap seconds on the given day. This can be
%                0, -1, or 1.
%
%This function calls the cumLeapSec function for the day in question and
%for one day later to see what the difference is. The function cumLeapSec
%relies on iaudat in the International Astronomical Union's (IAU)
%Standard's of Fundamental Astronomy library. Note that leap second
%information is not available for future dates as they can not be
%accurately predicted.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(year<1972)
        error('Dates before 1972 have a more complicated leap second methodology than is used here');
    end

%Determine the number of leap on the date.
    numSec=cumLeapSec(year,month,day);
    [Jul1,Jul2]=Cal2UTC(year,month,day,0,0,0);
    %Advance the UTC day count by one day.
    Jul1=Jul1+1;
    [year,month,day]=UTC2Cal(Jul1,Jul2,true);
    %Determine the number of leap seconds on day later.
    numSecNextDay=cumLeapSec(year,month,day);

    numLS=numSecNextDay-numSec;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function rnrb = nrbreverse(nrb)
%
% Function Name:
%
%   nrbreverse - Reverse the evaluation direction of a NURBS curve or surface.
%
% Calling Sequence:
%
%   rnrb = nrbreverse(nrb);
%
% Parameters:
%
%   nrb		: NURBS data structure, see nrbmak.
%
%   rnrb		: Reversed NURBS.
%
% Description:
%
%   Utility function to reverse the evaluation direction of a NURBS
%   curve or surface.

%  D.M. Spink
%  Copyright (c) 2000.

if nargin ~= 1
  error('Incorrect number of input arguments');
end

if iscell(nrb.knots)

  % reverse a NURBS surface
  coefs = nrb.coefs(:,:,end:-1:1);
  rnrb = nrbmak(coefs(:,end:-1:1,:), {1.0-fliplr(nrb.knots{1}),...
                1.0-fliplr(nrb.knots{2})});

else

  % reverse a NURBS curve
  rnrb = nrbmak(fliplr(nrb.coefs), 1.0-fliplr(nrb.knots));

end


function mpc = case15da
%CASE15DA  Power flow data for 15 bus distribution system from Das, et al
%   Please see CASEFORMAT for details on the case file format.
%
%   Data from ...
%       Das D, Kothari DP, Kalam A (1995) Simple and efficient method for load
%       flow solution of radial distribution networks. Int J Electr Power
%       Energy Syst 17:335-346. doi: 10.1016/0142-0615(95)00050-0
%       URL: https://doi.org/10.1016/0142-0615(95)00050-0

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 1;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [ %% (Pd and Qd are specified in kW & kVAr here, converted to MW & MVAr below)
	1	3	0	0	0	0	1	1	0	11	1	1	1;
	2	1	44.1	44.991	0	0	1	1	0	11	1	1.1	0.9;
	3	1	70	71.4143	0	0	1	1	0	11	1	1.1	0.9;
	4	1	140	142.8286	0	0	1	1	0	11	1	1.1	0.9;
	5	1	44.1	44.991	0	0	1	1	0	11	1	1.1	0.9;
	6	1	140	142.8286	0	0	1	1	0	11	1	1.1	0.9;
	7	1	140	142.8286	0	0	1	1	0	11	1	1.1	0.9;
	8	1	70	71.4143	0	0	1	1	0	11	1	1.1	0.9;
	9	1	70	71.4143	0	0	1	1	0	11	1	1.1	0.9;
	10	1	44.1	44.991	0	0	1	1	0	11	1	1.1	0.9;
	11	1	140	142.8286	0	0	1	1	0	11	1	1.1	0.9;
	12	1	70	71.4143	0	0	1	1	0	11	1	1.1	0.9;
	13	1	44.1	44.991	0	0	1	1	0	11	1	1.1	0.9;
	14	1	70	71.4143	0	0	1	1	0	11	1	1.1	0.9;
	15	1	140	142.8286	0	0	1	1	0	11	1	1.1	0.9;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	0	0	10	-10	1	100	1	10	0	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [  %% (r and x specified in ohms here, converted to p.u. below)
	1	2	1.35309	1.32349	0	0	0	0	0	0	1	-360	360;
	2	3	1.17024	1.14464	0	0	0	0	0	0	1	-360	360;
	3	4	0.84111	0.82271	0	0	0	0	0	0	1	-360	360;
	4	5	1.52348	1.0276	0	0	0	0	0	0	1	-360	360;
	2	9	2.01317	1.3579	0	0	0	0	0	0	1	-360	360;
	9	10	1.68671	1.1377	0	0	0	0	0	0	1	-360	360;
	2	6	2.55727	1.7249	0	0	0	0	0	0	1	-360	360;
	6	7	1.0882	0.734	0	0	0	0	0	0	1	-360	360;
	6	8	1.25143	0.8441	0	0	0	0	0	0	1	-360	360;
	3	11	1.79553	1.2111	0	0	0	0	0	0	1	-360	360;
	11	12	2.44845	1.6515	0	0	0	0	0	0	1	-360	360;
	12	13	2.01317	1.3579	0	0	0	0	0	0	1	-360	360;
	4	14	2.23081	1.5047	0	0	0	0	0	0	1	-360	360;
	4	15	1.19702	0.8074	0	0	0	0	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	0	0	3	0	20	0;
];

%% convert branch impedances from Ohms to p.u.
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;
Vbase = mpc.bus(1, BASE_KV) * 1e3;      %% in Volts
Sbase = mpc.baseMVA * 1e6;              %% in VA
mpc.branch(:, [BR_R BR_X]) = mpc.branch(:, [BR_R BR_X]) / (Vbase^2 / Sbase);

%% convert loads from kW to MW
mpc.bus(:, [PD, QD]) = mpc.bus(:, [PD, QD]) / 1e3;


%nav formulas on a (non rotating flat earth)
%vtyp==0->n frame vel, vtyp==1->b frame vel
function [Cbn_new, Vx_new, pos_new]=strapdown_pln_dcm(Cbn, Vx, pos, a, w, g, dt, vtyp)

%%%Update attitude
%Part I:Body frame update
rot=w*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_a=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);

Cbn_new=Cbn*mx_a;

%%Update Velocity and position
if (vtyp==0) %vel in n frame
    vel_inc=(Cbn*(a*dt))+[0;0;g]*dt;
    Vx_new=Vx+vel_inc;

    pos_new=pos+Vx*dt;
elseif (vtyp==1) %vel in b frame
    vel_inc1=(a+(Cbn'*[0;0;g]))*dt;
    vel_inc2=(cross(Vx,w))*dt;
    Vx_new=Vx+vel_inc1+vel_inc2;

    pos_new=pos+Cbn*Vx*dt;
end


%*****************************************************************************
% DSDP5:  Dual Scaling Algorithm for Positive Semidefinite Programming
% Copyright (c) 2002 by
% S. J. Benson, Y. Ye
% Last modified: 20 January 2004
%*****************************************************************************
%
% Converts data from DSDP4 format to DSDP5 format, calls solver, and
% and converts solution to DSDP4 format.
%
%
% > DSDP(A,C,b) attempts to solve the positive semidefinite program
%      MINIMIZE trace(C*X) SUCH THAT trace(A_i*X) = b_i, i=1,...,m and X >= 0
%      using a dual scaling algorithm.  For a problem with p blocks and m
%      constraints, A is a p x m cell array and C is a p x 1 cell array.
%      One block may contain LP variables, and the cells corresponding to
%      this block should be a one dimensional array.  All other cells
%      must contain a square, symmetric, real valued matrix.  The third
%      argument b is a dense column vector of length m.
%
% > DSDP(A,C,b,OPTIONS,y0) specifies an initial dual vector y0.
%
% > [STAT,y,X] = DSDP() returns a structure containing relevant statistics,
%                and approximate dual and primal solutions.
%
%*****************************************************************************

function [STAT,y,XX] = dsdp4(A,C,b,OPTIONS,y0);

  p=length(C);
  m=length(b);

  AC=cell(p,3);
  for j=1:p,
     [n1,n2]=size(C{j});
     if (n1==1 | n2==1)
       AAC=sparse(n1*n2,m+1);
       for i=1:m, AAC=[AAC sparse(A{j,i})']; end;
       AAC=[AAC sparse(C{j})'];
       AC{j,1}='LP';
       AC{j,2}=length(C{j});
       AC{j,3}=AAC;
     else
       AAC=sparse(n1*(n1+1)/2,m+1);
       for i=1:m, AAC=[AAC dvec(A{j,i})]; end;
       AAC=[AAC dvec(C{j})];
       AC{j,1}='SDP';
       AC{j,2}=size(C{j},1);
       AC{j,3}=AAC;
     end;
  end;

  [STAT,y,X]=dsdp(b,AC,OPTIONS,y0);

  XX=cell(p,1);
  for j=1:p,
     [n1,n2]=size(C{j})
     if (n1==1 | n2==1)
       XX{j}=X{j};
     else
       XX{j}=dmat(X{j});
     end;
  end;

return;


function [gx,dgdx,dgdp] = g_classif0(x,P,u,in)
% logistic-like observation function for VBA_classification
% function [gx,dgdx,dgdp] = g_classif0(x,P,u,in)
% IN:
%   - x: [useless]
%   - P: classification weights
%   - u: [useless]
%   - in: contains design matrix (in.X) and sparsify flag (in.sparse)
% OUT:
%   - gx: E[y|P]
%   - dgdx: [useless]
%   - dgdp: gradient of E[y|P] wrt P.

if in.sparse
    [sP, dsdP] = VBA_sparsifyPrior(P);
else
    sP = P;
end
gx = sss(in.X'*sP);
dgdx = [];
dgdp = diag(gx.*(1-gx))*in.X';
dgdp = dgdp';
if in.sparse % for exploiting the analytical gradients from g_GLM
    dgdp = dsdP*dgdp;
end

function sx = sss(x)
sx = 1./(1+exp(-x));
sx(sx < 1e-8) = 1e-8;
sx(sx > 1-1e-8) = 1-1e-8;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  MATLAB Code for                                                  %
%                                                                   %
%  Multi-Objective Particle Swarm Optimization (MOPSO)              %
%  Version 1.0 - Feb. 2011                                          %
%                                                                   %
%  According to:                                                    %
%  Carlos A. Coello Coello et al.,                                  %
%  "Handling Multiple Objectives with Particle Swarm Optimization," %
%  IEEE Transactions on Evolutionary Computation, Vol. 8, No. 3,    %
%  pp. 256-279, June 2004.                                          %
%                                                                   %
%  Developed Using MATLAB R2009b (Version 7.9)                      %
%                                                                   %
%  Programmed By: S. Mostapha Kalami Heris                          %
%                                                                   %
%         e-Mail: sm.kalami@gmail.com                               %
%                 kalami@ee.kntu.ac.ir                              %
%                                                                   %
%       Homepage: http://www.kalami.ir                              %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Index SubIndex]=GetGridIndex(particle,G)

    c=particle.Cost;

    nobj=numel(c);
    ngrid=numel(G(1).Upper);

    str=['sub2ind(' mat2str(ones(1,nobj)*ngrid)];

    SubIndex=zeros(1,nobj);
    for j=1:nobj

        U=G(j).Upper;

        i=find(c(j)<U,1,'first');

        SubIndex(j)=i;

        str=[str ',' num2str(i)];
    end

    str=[str ');'];

    Index=eval(str);

end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
% problem 7 - function that computes the convolution of x[n] and h[n]

function [y,n] = convd(x,n1,h,n2)
a = n1(1)+n2(1);
b = n1(end) + n2(end);
n =a:b;
y = conv(x,h);
stem(n,y);
legend('y[n]')


classdef IMMOEA_F2 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing IM-MOEA

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, K. Narukawa, and B. Sendhoff, A multiobjective
% evolutionary algorithm using Gaussian process-based inverse modeling,
% IEEE Transactions on Evolutionary Computation, 2015, 19(6): 838-856.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = (1+5*repmat(2:obj.D,size(X,1),1)/obj.D).*X(:,2:obj.D) - repmat(X(:,1),1,obj.D-1);
            g = 1 + 9*mean(t.^2,2);
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-(PopObj(:,1)./g).^2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^2;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function [stan,rtfr,hat] = tfrrstan(x,t,N,G,h,trace);
%TFRRSTAN Reassigned Stankovic distribution.
%	[TFR,RTFR,HAT] = TFRRSTAN(X,T,N,H,TRACE)
%	computes the Stankovic distribution and its reassigned version.
%
%	X     : analysed signal.
%	T     : the time instant(s)      (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%       G     : frequency averaging window
%	h     : stft window              (default : Hamming(N/4)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR,  : time-frequency representation and its reassigned
%	RTFR    version. When called without output arguments,
%	        TFRRSTAN runs TFRQVIEW.
%	HAT   : Complex matrix of the reassignment vectors.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); t=1:2:128;
%	 G=tftb_window(9,'hanning'); h=tftb_window(61,'hanning'); tfrrstan(sig,t,128,G,h,1);
%
%	See also  all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, August, September 1997.
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (nargin <= 2),
 N=xrow;
end;

hlength=floor(N/4);
hlength=hlength+1-rem(hlength,2);

if (nargin == 1),
 t=1:xrow; G=[0.25; 0.5; 0.25]; h = tftb_window(hlength); trace=0;
elseif (nargin == 2)|(nargin == 3),
 G=[0.25; 0.5; 0.25]; h = tftb_window(hlength); trace=0;
elseif (nargin == 4),
 h = tftb_window(hlength); trace = 0;
elseif (nargin == 5),
 trace = 0;
end;

[Grow,Gcol]=size(G); LG=(Grow-1)/2;
if (Gcol~=1)|(rem(Grow,2)==0),
 error('G must be a smoothing window with odd length');
end;

if (N<0),
 error('N must be greater than zero');
end;
[trow,tcol] = size(t);
if (xcol~=1),
 error('X must have only one column');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2;
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

if (tcol==1),
 Dt=1;
else
 Deltat=t(2:tcol)-t(1:tcol-1);
 Mini=min(Deltat); Maxi=max(Deltat);
 if (Mini~=Maxi),
  error('The time instants must be regularly sampled.');
 else
  Dt=Mini;
 end;
 clear Deltat Mini Maxi;
end;

stan = zeros(N,tcol);
rtfr = zeros(N,tcol);
hat  = zeros(N,tcol);

if trace, disp('Stankovic distribution (with reassignement)'); end;
Ex=mean(abs(x(min(t):max(t))).^2); Threshold=1.0e-3*Ex;
Dh=dwindow(h); Th=h.*[-Lh:Lh]';
for icol=1:tcol,
 if trace, disprog(icol,tcol,10); end;
 ti= t(icol);
 tau=-min([round(N/2)-1,Lh,ti-1]):min([round(N/2)-1,Lh,xrow-ti]);
 indices= rem(N+tau,N)+1;
 tfr= zeros(N,3);
 tfr(indices,1)=x(ti+tau).*conj( h(Lh+1-tau));
 tfr(indices,2)=x(ti+tau).*conj(Th(Lh+1-tau));
 tfr(indices,3)=x(ti+tau).*conj(Dh(Lh+1-tau));
 tfr=fft(tfr);
 stan(:,icol)=G(LG+1) * abs(tfr(:,1)).^2;

 for jcol=1:N,
  stanTh=G(LG+1)*tfr(jcol,1)*conj(tfr(jcol,2));
  stanDh=G(LG+1)*tfr(jcol,1)*conj(tfr(jcol,3));
  for kstan=1:min(N/2-1,LG),
   stanbefore=rem(rem(jcol-kstan-1,N)+N,N)+1;
   stanafter =rem(rem(jcol+kstan-1,N)+N,N)+1;
   stan(jcol,icol)= stan(jcol,icol) ...
                  + G(LG+1-kstan)*tfr(stanbefore,1)*conj(tfr(stanafter ,1)) ...
                  + G(LG+1+kstan)*tfr(stanafter ,1)*conj(tfr(stanbefore,1));
   stanTh= stanTh + G(LG+1-kstan)*tfr(stanbefore,1)*conj(tfr(stanafter ,2)) ...
                  + G(LG+1+kstan)*tfr(stanafter ,1)*conj(tfr(stanbefore,2));
   stanDh= stanDh + G(LG+1-kstan)*tfr(stanbefore,1)*conj(tfr(stanafter ,3)) ...
                  + G(LG+1+kstan)*tfr(stanafter ,1)*conj(tfr(stanbefore,3));
  end;
  stan(jcol,icol)=real(stan(jcol,icol));
  if abs(stan(jcol,icol))>Threshold,
   icolhat = round(icol - real(stanTh/stan(jcol,icol))/Dt);
   jcolhat = round(jcol - N*imag(stanDh/stan(jcol,icol)/(2.0*pi)));

   jcolhat= rem(rem(jcolhat-1,N)+N,N)+1;
   icolhat= min(max(icolhat,1),tcol);

   rtfr(jcolhat,icolhat)=rtfr(jcolhat,icolhat) + stan(jcol,icol) ;
   hat(jcol,icol)= jcolhat + j * icolhat;
   %fprintf('%12.3f %12.3f , %12.3f %12.3f \n',jcol,icol,jcolhat,icolhat);
  else
   rtfr(jcol,icol)=rtfr(jcol,icol) + stan(jcol,icol);
   hat(jcol,icol)= jcol + j * icol;
  end;
 end;

end ;

if trace, fprintf('\n'); end;

if (nargout==0),
 TFTBcontinue=1;
 while (TFTBcontinue==1),
  choice=menu ('Choose the representation:',...
               'stop',...
               'Stankovic distribution',...
               'reassigned Stankovic distribution');
  if (choice==1), TFTBcontinue=0;
  elseif (choice==2),
   tfrqview(stan,x,t,'type1');
  elseif (choice==3),
   tfrqview(rtfr,x,t,'type1');
  end;
 end;
end;


function [c, frec, info] = multidgtrealmp(f,dicts,varargin)
%MULTIDGTREALMP  Matching Pursuit Decomposition with Multi-Gabor Dictionary
%   Usage:  c = multidgtrealmp(f,dicts)
%           c = multidgtrealmp(f,dicts,errdb,maxit)
%           [c,frec,info] = multidgtrealmp(...)
%
%   Input parameters:
%       f        : Input signal
%       dicts    : Dictionaries. Format {g1,a1,M1,g2,a2,M2,...}
%       errdb    : Target normalized approximation error in dB
%       maxit    : Maximum number of iterations.
%   Output parameters:
%       c        : Sparse representation
%       frec     : Reconstructed signal
%       info     : Struct with additional output paramerets
%
%   `multidgtrealmp(f,{g1,a1,M1,g2,a2,M2,...,gW,aW,MW})` returns sparse
%   representation of a signal in `W` Gabor dictionaries using the
%   fast matching pursuit algorithm. `gw` is a Gabor window defined
%   as in |dgt| and |dgtreal|, `aw` is a hop factor, `Mw` is the number of
%   frequency channels. All `aw` and `Mw` must be divisible by `min(a1,...,aW)`.
%   The algorithm will try to reach -40 dB relative approximation error
%   in at most `numel(f)` iterations.
%   The function returns a cell-array with elements storing coefficients
%   for individual Gabor systems such that they can be directly used in
%   |idgtreal|.
%
%   `multidgtrealmp(f,dicts,errdb,maxit)` tries to reach normalized
%   approximation error *errdb* dB in at most *maxit* iterations.
%
%   `[c,frec,info] = multidgtrealmp(...)` in addition returns the
%   aproximation *frec* and a struct `info` with the following fields:
%
%     .iter     Number of iterations done.
%
%     .atoms    Number of atoms selected.
%
%     .relres   Final approximation error. If resets are enabled, this is a
%               vector additionally containing the approximation error at
%               every reset.
%
%     .g        Cell array of numeric windows used in the multi-dictionary
%
%     .a        Array of hop factors for indivitual dictionaries
%
%     .M        Array of numbers of channels for individual dictionaries
%
%     .synthetize  Anonymous function which can be used to synthetize from
%                  the (modified) coefficients as
%                  `frec = sum(info.synthetize(c),dim)`
%                  where `dim=2` if the input *f* was a column vector and
%                  `dim=1` if it was a row vector.
%
%   The normalized approximation error is computed as
%   `errdb=20*log10(norm(f-frec)/norm(f))`.
%
%   The function takes the following optional parameters at the end of
%   the line of input arguments:
%
%     'kenrnthr',kt     Kernel threshold. Must be in range ]0,1].
%                       Default is 1e-4.
%
%     'timeinv'         Use the time invariant phase convention. The
%                       default is 'freqinv'.
%
%     'pedanticsearch'  Be pedantic about the energy of pairs of
%                       conjugated atoms in the selection step.
%                       Disabled by default.
%
%     'algorithm',alg   Algorithm to use. Available:
%                       'mp'(default),'selfprojmp','cyclicmp'
%
%     'reset'           Reset the decomposition until stopping criteria
%                       are met. The reset means a complete synthesis and
%                       re-analysis. Available:
%                       'noreset' (default), 'reset', 'quickreset'
%                       Option 'quickreset' applies a heuristic to stop the
%                       algorithm more quickly when the targer error is
%                       reached.
%
%   Reset conditions can be controlled by the following key-value pairs:
%
%     'resetit',it      Reset the decomposition every `it` iteration.
%                       Must be a positive integer.
%
%     'reseterrdb',err  Reset the decomposition when the error estimate
%                       in dB decreases below `err`. Must be negative.
%                       Default is `10*log10(1.1*kv.kernthr)`.
%
%   The computational routine is only available in C. Use |ltfatmex| to
%   to compile it.
%
%   Algorithms
%   ----------
%
%   By default, the function uses the fast MP using approximate update
%   in the coefficient domain as described in::
%
%   "Z. Prusa, N. Holighaus, P. Balazs: Fast Matching Pursuit with Multi-Gabor Dictionaries"
%
%   The kernel threshold limits the minimum approximation error which
%   can be reached. For example the default threshold 1e-4 in general
%   allows achieving at least -40 dB.
%
%   Note that this algorithm internally computes the error as
%   `errdb = 10*log10(err(k)/norm(f)^2)`, with `err(0) = norm(f)^2` and
%   `err(k) = err(k-1) - abs(cselected(k))^2`. Here, `cselected(k)` is the
%   Gabor coefficient selected at the k-th MP step. This formula is exact
%   only if the kernel threshold equals 0. Otherwise it serves as a cheap
%   estimate. The exact error is only computed when this function is run
%   with resets enabled and only in between resets.
%
%   Examples
%   --------
%
%   The following example shows the decomposition in 3 dictionaries and
%   plots contributions from the individual dictionaries and the residual.:::
%
%       [f,fs] = gspi;
%       [c, frec, info] = multidgtrealmp(f,...
%       {'blackman',128,512,'blackman',512,2048,'blackman',2048,8192});
%       frecd = info.synthetize(c);
%       figure(1);
%       xvals = (0:numel(f)-1)/fs;
%       subplot(4,1,1); plot(xvals,frecd(:,1));ylim([-0.5,0.5]);
%       subplot(4,1,2); plot(xvals,frecd(:,2));ylim([-0.5,0.5]);
%       subplot(4,1,3); plot(xvals,frecd(:,3));ylim([-0.5,0.5]);
%       subplot(4,1,4); plot(xvals,f-frec);ylim([-0.5,0.5]); xlabel('Time (s)');
%
%   See also: dgtreal idgtreal
%
%   References: ltfatnote052 mazh93 stchr10 rerose17

%AUTHOR: Zdenek Prusa

thismfile = upper(mfilename);
complainif_notenoughargs(nargin,2,thismfile);
% Define initial value for flags and key/value pairs.
definput.keyvals.errdb=-40;
definput.keyvals.maxit=[];
definput.keyvals.resetit=0;
definput.keyvals.reseterrdb = 0;
%definput.keyvals.iterstep=[];
definput.keyvals.kernthr = 1e-4;
%definput.flags.print={'quiet','print'};
definput.flags.algversion={'fast','slow'};
definput.flags.algorithm={'mp','selfprojmp','cyclicmp'};
definput.flags.search={'plainsearch','pedanticsearch'};
definput.flags.phaseconv={'freqinv','timeinv'};
definput.flags.reset={'noreset','reset','quickreset'};

[flags,kv]=ltfatarghelper({'errdb','maxit'},definput,varargin);

if exist('comp_multidgtrealmp','file') ~= 3 && flags.do_fast
    error(['%s: MEX/OCT file is missing. Either compile the MEX/OCT ',...
           'interfaces or re-run the function with ''slow'''], thismfile);
end

if flags.do_slow
    error('%s: ''slow'' is not supported yet.',thismfile)
end

if flags.do_reset || flags.do_quickreset % Check reseterrdb or resetit
   if kv.reseterrdb == 0
        if kv.resetit == 0 % Set both to conservative default values depending on kernthr
            kv.reseterrdb = 10*log10(1.1*kv.kernthr);
            kv.resetit = round(1000/sqrt(kv.kernthr));
        else % If only resetit is given, ignore reseterrdb
            kv.reseterrdb = -flintmax;
        end
   elseif kv.resetit == 0 % If only reseterrdb is given, ignore resetit
        kv.resetit = flintmax;
   end
   if kv.reseterrdb >= 0
       error('%s: Reset error tolerance must be negative.',thismfile);
   end
   if kv.resetit < 0 || kv.resetit ~= round(kv.resetit)
       error('%s: Number of iterations before reset must be a positive integer.',thismfile);
   end
end

%% ----- step 1 : Verify f and determine its length -------
% Change f to correct shape.
[f,~,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,[],[],upper(mfilename));

if W>1
    error('%s: Input signal can be single channel only.',upper(mfilename));
end

if kv.errdb > 0
    error('%s: Target error must be lower than 0 dB.',upper(mfilename));
end

if ~(kv.kernthr > 0 && kv.kernthr <= 1)
    error('%s: Kernel threshold must be in range ]0,1].',upper(mfilename));
end

if ~iscell(dicts), error('%s: dicts must be cell',thismfile); end
if rem(numel(dicts),3) ~= 0 || ~all(cellfun(@(x)isscalar(x), dicts([2:3:end,3:3:end])))
    error('%s: bad format of dicts. Check {g1,a1,M1,g2,a2,M2,...,gW,aW,MW}',...
        thismfile);
end

dictno = numel(dicts)/3;
gin = dicts(1:3:end);
a = cell2mat(dicts(2:3:end));
M = cell2mat(dicts(3:3:end));

if any(rem(M,a) ~= 0) || any(M./a<2)
    error(['%s: Only integer oversampling greater than 1 is allowed ',...
           'i.e. M/a must be an integer>=2.'],...
    upper(mfilename));
end

if dictno > 1
    asort  = sort(a);
    Msort  = sort(M);
    if any(rem(asort(2:end),asort(1:end-1)) ~= 0)
        error('%s: all au and av must be divisible by min(au,av)',thismfile);
    end
    if any(rem(Msort(2:end),Msort(1:end-1)) ~= 0)
        error('%s: all Mu and Mv must be divisible by min(Mu,Mv)',thismfile);
    end
end

info.a = a;
info.M = M;
info.iter = 0;
info.relres = [];
fnorm = norm(f);

L = filterbanklength(Ls,[a(:);M(:)]);
if isempty(kv.maxit), kv.maxit = L; end

info.g = cell(dictno,1);
for dIdx = 1:dictno
    info.g{dIdx} = setnorm(gabwin(gin{dIdx},a(dIdx),M(dIdx),L),'2');
end

for dIdx = 1:dictno
    condnum = gabframebounds(info.g{dIdx},a(dIdx),M(dIdx));
    if condnum > 1e3
        error('%s: Dictionary %d is badly conditioned.',dIdx,upper(mfilename));
    end
end

% Initial residuum
fpad = postpad(f,L);

% Determine reset/no reset mode and run MP decomposition
if flags.do_noreset
    [c,info.atoms,info.iter,info.status] = ...
        comp_multidgtrealmp(fpad,info.g,a,M,flags.do_timeinv,...
                            kv.kernthr,kv.errdb,kv.maxit,kv.maxit,...
                            flags.do_pedanticsearch, flags.algorithm );
else
    c = cell(1,dictno);
    for dIdx = 1:dictno
        c{dIdx} = zeros(floor(M(dIdx)/2) + 1, L/a(dIdx),class(f));
    end

    remainingit = kv.maxit;
    currerrdb = kv.errdb;

    info.resets = -1;
    reseterrdb = kv.reseterrdb;

    while remainingit > 0 && currerrdb < 0
        info.resets = info.resets + 1;

        % Adjust number of iterations
        currit = min([remainingit,kv.resetit]);
        if flags.do_quickreset % Adjust reset error condition
            reseterrdb = max([1.2*currerrdb,kv.reseterrdb]);
        end

        [ctmp,info.atoms,info.iter,info.status] = ...
            comp_multidgtrealmp(fpad,info.g,a,M,flags.do_timeinv,...
                                kv.kernthr,reseterrdb,currit,currit,...
                                flags.do_pedanticsearch, flags.algorithm );

        c = cellfun(@(cEl,ctmpEl) cEl + ctmpEl, c(:)',ctmp(:)','UniformOutput', 0);

        remainingit = remainingit - info.iter;

        %The following means that some other stopping criterion has been reached
        if info.status ~=0 && info.status ~=3 && currit ~= info.iter, break; end

        if remainingit > 0
            % Recompute residuum
            frec = sum(cell2mat(cellfun(@(cEl,gEl,aEl,MEl) idgtreal(cEl,gEl,aEl,MEl,flags.phaseconv),...
                   c(:)',info.g(:)',num2cell(a(:))',num2cell(M(:))','UniformOutput',0)),2);

            fpad = postpad(f,L) - frec;
            relres = norm(fpad(:)) / fnorm;
            % Since we are changing the residual, we must adjust the target error
            currerrdb = kv.errdb - 20*log10(relres);
            if currerrdb < 0
                info.relres(info.resets+1) = relres;
            end
            if info.resets >= 1 && info.relres(end) > info.relres(end-1)
                info.status = 6;
                break;
            end
        end
    end

    %Fix the info
    info.atoms = sum(cellfun(@(cEl) numel(find( abs(cEl) > 0 )), c));
    info.iter = kv.maxit - remainingit;
end

if nargout>1
  permutedsize2 = permutedsize; permutedsize2(2) = dictno;
  info.synthetize = @(c) ...
      assert_sigreshape_post(...
      postpad(cell2mat(cellfun(@(cEl,gEl,aEl,MEl) idgtreal(cEl,gEl,aEl,MEl,flags.phaseconv),...
      c(:)',info.g(:)',num2cell(a(:))',num2cell(M(:))','UniformOutput',0)),Ls),...
      dim,permutedsize2,order);
  dim2 = 2;
  if dim == 2, dim2 = 1; end
  frec = sum(info.synthetize(c),dim2);
  if fnorm == 0
    info.relres(end+1) = 0;
  else
    info.relres(end+1) = norm(frec(:)-f(:))/fnorm;
  end

  % Fix the status
  if info.relres(end) <= 10^(kv.errdb/20)
      info.status = 0;
  elseif info.iter == kv.maxit
      info.status = 2;
  end

  status_str = {...
  'Target error reached',...
  'Maximum number of atoms reached',...
  'Maximum number of iterations reached',...
  'Stalled (abs. norm. error estimate became negative)',...
  'Selected coefficient tolerance reached',...
  'All zeros',...
  'Stalled (Residual has increased since last reset. Try to reduce resetit.)'...
  };
  info.message = status_str{1 + info.status};
end