function [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,optname,x0,LB,UB,varargin)
% RMSEARCH: Randomly multistarted overlay to several optimizers
% usage: [xfinal,ffinal,exitflag,xstart] = RMSEARCH(fun,optname,x0,LB,UB)
% usage: [xfinal,ffinal,exitflag,xstart] = RMSEARCH(fun,optname,x0,LB,UB,prop1,val1,prop2,val2,...)
%
% This tool is useful when little information is available
% for good starting values for an optimization, or when many
% local solutions exist. It is also useful to map out the various
% basins of attaction of each local solution.
%
% RMSEARCH is normally used for bounded optimization problems,
% but with care can be used when some variables are unbounded.
% Whenever possible I'd still recommend the use of both lower
% and upper bounds if you can possibly do so. Restriction of
% the solution space is an important way to improve the
% performance of any optimization.
%
%
% arguments: (Input)
%  fun - name of a function, inline function, or a function handle
%
%  optname - character string - the name of the
%        optimizer to be used. Valid options are
%
%        'fminsearchbnd', 'fminsearchcon', 'fmincon'
%        'fminbnd', 'fzero', 'lsqnonlin', 'fminsearch'
%
%        No default is allowed, but the function name may
%        be shortened as long as the shortening is unambiguous.
%        Thus 'fminb' or 'fz' are acceptable, but 'fm' is not.
%
%        Note that fzero and fminbnd, since they work in 1-d
%        will search EVERY bracketed interval they find that
%        contains either a root or a minimizer (as appropriate.)
%
%        Remember to set the options structure properly for
%        fmincon, turning off the largescale optimizer as
%        appropriate. Otherwise you will see many warning
%        messages generated, one for each call to the optimizer.
%
%  x0 -  starting value. Used to determine the dimension and
%        shape of the parameter vector. Also used to determine
%        the distribution of the random starts when the bounds
%        are incompletely specified.
%
%        No default is allowed.
%
%        If both lower and upper bounds are supplied for a
%        parameter, then the distribution for the multi-starts
%        is chosen to be uniform between those bounds. If no
%        upper bound or no lower bound, then the distribution
%        follows a two parameter exponential distribution, with
%        mean at x0(i). If both upper and lower bounds are
%        missing (i.e., they are +/- inf), then that parameter
%        will be normally distribued with mean at x0(i).
%
%  LB -  Lower bounds for the parameters, as used by fmincon,
%        fminsearchbnd, lsqnonlin, etc.
%
%        LB must be the same size as x0, and LB(i)<=UB(i)
%
%        When no lower bound exists for a variable, use -inf
%
%  UB -  Upper bounds for the parameters, as used by fmincon,
%        fminsearchbnd, lsqnonlin, etc.
%
%        UB must be the same size as x0, and LB(i)<=UB(i)
%
%        When no upper bound exists for a variable, use +inf
%
%
% Additional inputs must be in the form of property/value pairs.
%   Properties are character strings. They may be shortened
%   to the extent that they are unambiguous. Properties are
%   not case sensitive. Valid property names are:
%
%    'Plot', 'Nonlcon', 'FractionUsed', 'InitialSample',
%    'OverSample', 'A', 'B' 'Options'
%
%   All properties have default values, chosen as intelligently
%   as I could manage. Values that are character strings may
%   also be unambiguously shortened. The legal values for each
%   property are:
%
%   'Plot' - specifies if a plot is generated {'on', 'off'}
%         A plot can only be generated for 1 or 2 dimensional
%         problems. A warning is generated for more than 2
%         variables if plot is turned on.
%
%         DEFAULT: 'off'
%
%   'Nonlcon' - Nonlinear constraint function or function handle
%         Only used for 'fmincon' or 'fminsearchcon'. Other
%         optimizers will cause an error if this is provided.
%
%         DEFAULT: ''
%
%   'FractionUsed' - Fractional amount of the total sample that
%         are used as multiple starts for the optimizer. It must
%         lie in the half open interval (0,1].
%
%         DEFAULT: 1  (100% of the samples are used as starting
%         points.
%
%         Note: FractionUsed is ignored for fminbnd and fzero.
%
%   'InitialSample' - Size of the total initial random sample
%         generated.
%
%         DEFAULT: 100  (100 points are generated)
%
%   'OverSample' - Over-sampling ratio. If linear inequality or
%         or nonlinear inequality constraints are involved, a
%         simple sampling strategy within the bounds may fail
%         to produce as many feasible points as are requested.
%
%         Thus if 100 points are requested in the initial sample,
%         a 20 to 1 oversampling ratio will generate a total of
%         2000 initial samples that are all within the simple bound
%         constraints. All of these points are compared to the
%         inequality constraints, rejecting all of those that fail.
%         The first InitialSample set of points are retained, only
%         these points are considered as starting points for an
%         optimization.
%
%         DEFAULT: 20  (a 20 to 1 over-sampling ratio)
%
%   'A', 'B' - Linear Inequality constraint matrix (A) and right
%         hand side vector (B), used only for 'fmincon' or
%         'fminsearchcon'. Other optimizers will cause an error
%         if these are provided.
%
%   'Options' - options structure for the specified optimizer.
%
%        default: optimset('optname')
%
%        For fminsearchbnd or fminsearchcon, the default is
%        optimset('fminsearch')
%
% arguments: (Output)
%  xfinal - final solutions found for each xstart point. Each
%         row of xfinal is one solution.
%
%  ffinal - objective function at xfinal.
%
%         For lsqnonlin or lsqcurvefit, ffinal is the sum
%         of squares.
%
%  exitflag - exitflags for each "solution" found
%
%  xstart - starting value used for each point in xfinal
%
%
% Example usage:
%  Find all zeros of besselj(2,x) betweeen x=0 and x=50. Note
%  that x0 is actually ignored in this case. Generate a plot of
%  the sample points and the solutions found.
%
%  fun = @(x) besselj(2,x);
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,'fzero',...
%       10,0,50,'initialsample',500,'plot','on')
%
% Example usage:
%  Find local minimizers of besselj(2,x) betweeen x=0 and x=200
%  Generate a plot of the sample points and the solutions found.
%
%  fun = @(x) besselj(2,x);
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,'fminbnd',...
%       10,0,200,'initialsample',500,'plot','on')
%
% Example usage:
%  Find local minimizers of the peaks function. Generate
%  a plot of the sample points and the solutions found. Use
%  fminsearchbnd as the optimizer.
%
%  fun = inline('peaks(x(1),x(2))','x');
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,'fminsearchbnd',...
%      [0 0],[-5 -5],[5 5],'initialsample',100,'plot','on')
%
% Example usage:
%  Find the local minimizers of the peaks function within a circle
%  of radius 2 around the origin. Generate a plot of the sample points
%  and the solutions found. Use fmincon as the optimizer, setting the
%  appropriate options for its use. Note the trick with deal for nonlcon
%  inside an anonymous function.
%
%  fun = inline('peaks(x(1),x(2))','x');
%  opts = optimset('fmincon');
%  opts.LargeScale = 'off';
%  opts.Display = 'none';
%  nonlcon = @(x) deal(norm(x) - 2,[]);
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun, ...
%     'fmincon',[0 0],[-2 -2],[2 2],'initialsample',100, ...
%     'plot','on','options',opts,'nonlcon',nonlcon)
%
%
% See also: fminsearchbnd, fmincon, fminsearchcon, fzero, fminbnd, lsqnonlin
%
%
% Author: John D'Errico
% E-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 1/1/07

% set defaults
par.Plot = 'off';
par.Nonlcon = '';
par.FractionUsed = 1;
par.InitialSample = 100;
par.OverSample = 20;
par.A = [];
par.B = [];
par.OptName = optname;
par.Options = [];

na = length(varargin);
if (rem(na,2)==1)
  error 'Property/value pairs must come as PAIRS of arguments.'
elseif na>0
  par = parse_pv_pairs(par,varargin);
end

% shape of x0, dimension of the problem
if (nargin<3) || isempty(x0)
  error 'x0 must be provided.'
else
  par.Nx0 = size(x0);
  x0 = x0(:);
  par.Dim = prod(par.Nx0);
end

% checks for validity of parameters, any problems
par = check_params(par);

% Was fun a string, or an inline/anonymous function?
if (nargin<1)
  help rmsearch
  return
elseif isempty(fun)
  error 'fun was not supplied.'
elseif ischar(fun)
  % a character function name
  fun = str2func(fun);
end

% Check the bounds
if (nargin<4) || isempty(LB)
  LB = repmat(-inf,par.Dim,1);
else
  LB = LB(:);
end
if (nargin<5) || isempty(UB)
  UB = repmat(inf,par.Dim,1);
else
  UB = UB(:);
end
% are they the same size as X0?
if length(LB)~=par.Dim
  error 'Lower bound array was inconsistent in size with x0'
end
if length(UB)~=par.Dim
  error 'Upper bound array was inconsistent in size with x0'
end

% Were there any unusable inequalities provided?
if ~ismember(par.OptName,{'fmincon','fminsearchcon'})
  if par.UseLC
    error(['Linear inequalities provided are not usable by ',par,OptName])
  elseif par.UseNLC
    error(['Nonlinear inequalities provided are not usable by ',par,OptName])
  end
end

% are any of the bounds inconsistent?
if any(LB>UB)
  error 'It is required that: LB <= UB'
end

% Choose the distributions used to sample
% from for each variable.
LBinf=isinf(LB);
UBinf=isinf(UB);
for i = 1:par.Dim
  if LBinf(i) && UBinf(i)
    % both are unbounded, use a normal
    vardist(i).dist = 'normal';
    vardist(i).mean = x0(i);
    vardist(i).sd = 1;
  elseif ~LBinf(i) && UBinf(i)
    % only lower bound, use an exponential
    vardist(i).dist = 'lower';
    vardist(i).lambda = x0(i) - LB(i);
    if vardist(i).lambda<=0
      vardist(i).lambda = 0.5;
    else
      vardist(i).lambda = 0.5 ./(vardist(i).lambda);
    end
    vardist(i).shift = LB(i);
  elseif LBinf(i) && ~UBinf(i)
    % only upper bound, use an exponential
    vardist(i).dist = 'upper';
    vardist(i).lambda = UB(i) - x0(i);
    if vardist(i).lambda <= 0
      vardist(i).lambda = 0.5;
    else
      vardist(i).lambda = 0.5 ./(vardist(i).lambda);
    end
    vardist(i).shift = UB(i);
  else
    % dual bounds, use a uniform
    vardist(i).dist = 'uniform';
    vardist(i).a = LB(i);
    vardist(i).b = UB(i);
  end
end

% Generate the intial random samples. Choose enough
% of them in case there are constraints to worry about.
if par.UseLC || par.UseNLC
  % oversampling for rejection
  xinitial = rand(par.OverSample*par.InitialSample,par.Dim);
else
  % No need to oversample
  xinitial = rand(par.InitialSample,par.Dim);
end

% generate a sample with the specified distributions
for i = 1:par.Dim
  switch vardist(i).dist
    case 'uniform'
      % uniform between two bounds
      xinitial(:,i) = vardist(i).a + ...
        (vardist(i).b - vardist(i).a)*xinitial(:,i);
    case 'normal'
      % unbounded - use a normal
      xinitial(:,i) = vardist(i).mean + ...
        vardist(i).sd*randn(par.InitialSample,1);
    case 'lower'
      % lower bound, use an exponential
      xinitial(:,i) = vardist(i).shift - ...
        log(xinitial(:,i))/vardist(i).lambda;
    case 'upper'
      % upper bound, use an exponential
      xinitial(:,i) = vardist(i).shift + ...
        log(xinitial(:,i))/vardist(i).lambda;
  end
end

% if 1-d problem, fminbnd and fzero will both do best
% if any bounds are included in the sample
if (par.Dim == 1)
  if ~isinf(LB) && ~isinf(UB)
    xinitial = [LB;UB;xinitial];
  elseif  (~isinf(LB) && isinf(UB))
    xinitial = [LB;xinitial];
  elseif (isinf(LB) && ~isinf(UB))
    xinitial = [UB;xinitial];
  end
end

% Do we need to reject any samples?
if par.UseLC
  k = ((repmat(par.B,1,size(xinitial,1)) - par.A*xinitial') < 0);
  k = any(k,1);
  xinitial(k,:) = [];
end
if size(xinitial,1)<1
  error 'Linear inequalities too strict. No feasible points were identified.'
end
if par.UseNLC
  % test any nonlinear constraints
  nx = size(xinitial,1);
  k = true(nx,1);
  for i = 1:nx
    [cineq,ceq] = par.Nonlcon(xinitial(i,:)); %#ok
    k(i) = all(cineq>0);
  end
  % reject those that failed
  xinitial(k,:) = [];
end
if size(xinitial,1)<1
  error 'Nonlinear constraints too strict. No feasible points were identified.'
end

% Some points got through, but not as many as requested
xsize = size(xinitial,1);
if xsize < par.InitialSample
  warning('RMSEARCH:FeasiblePoints',['Only ',num2str(xsize),' feasible points were found, ', ...
    num2str(par.InitialSample),' were requested. Insufficiently Oversampled.'])
end
% keep as many samples as we need if oversampled
xinitial = xinitial(1:min(xsize,par.InitialSample),:);
xsize = size(xinitial,1);

% Evaluate each sample through our objective function.
finitial = zeros(xsize,1);
for i = 1:xsize
  if ~strcmp(par.OptName,'lsqnonlin')
    % A simple optimizer:
    % 'fminbnd', 'fzero', 'fmincon', 'fminsearchbnd',
    % 'fminsearchcon','fminsearch'
    finitial(i) = fun(reshape(xinitial(i,:),par.Nx0));
  else
    % lsqnonlin, so we need to form the sum of squares
    f = fun(reshape(xinitial(i,:),par.Nx0));
    finitial(i) = sum(f.^2);
  end
end

% At which of these sample points will we
% start the indicated optimizer? Was it a 1-d
% problem? If so, then both fzero and fminbnd
% use a bracketed interval.
xfinal = zeros(size(xinitial));
ffinal = zeros(size(finitial));
if (par.Dim==1) && ismember(par.OptName,{'fzero', 'fminbnd'})
  switch par.OptName
    case 'fzero'
      % fzero - sort on x first
      [xinitial,tags] = sort(xinitial);
      finitial = finitial(tags);

      % find zero crossings in f
      ind = 1:(xsize-1);
      k = find((sign(finitial(ind)) .* sign(finitial(ind+1))) <= 0);

      % any crossings found?
      if isempty(k)
        error 'No zero crossings found in this sample for fzero'
      end

      xfinal = zeros(length(k),1);
      ffinal = xfinal;
      exitflag = xfinal;
      xstart = xfinal;
      fstart = xfinal;
      % loop over the candidate intervals
      for i = 1:length(k)
        [xfinal(i),ffinal(i),exitflag(i)] = fzero(fun, ...
          [xinitial(k(i)),xinitial(k(i)+1)],par.Options);

        % store the better of the two points in each bracket
        if abs(finitial(k(i))) <= abs(finitial(k(i)+1))
          xstart(i) = xinitial(k(i));
          fstart(i) = finitial(k(i));
        else
          xstart(i) = xinitial(k(i)+1);
          fstart(i) = finitial(k(i)+1);
        end
      end

    case 'fminbnd'
      % fminbnd - sort on x first
      [xinitial,tags] = sort(xinitial);
      finitial = finitial(tags);

      % find local minima in f
      ind = 2:(xsize-1);
      k = 1 + find((finitial(ind) <= finitial(ind-1)) & ...
        (finitial(ind) <= finitial(ind+1)));

      % do we look in the first interval?
      if finitial(1) < finitial(2)
        k = union(2,k);
      end
      % or the last one?
      if finitial(end) < finitial(end-1)
        k = union(xsize-1,k);
      end

      % any local minima found?
      xfinal = zeros(length(k),1);
      ffinal = xfinal;
      exitflag = xfinal;
      xstart = xfinal;
      fstart = xfinal;
      % loop over the candidate intervals
      for i = 1:length(k)
        [xfinal(i),ffinal(i),exitflag(i)] = fminbnd(fun, ...
          xinitial(k(i)-1),xinitial(k(i)+1),par.Options);

        % store in xstart
        xstart(i) = xinitial(k(i));
        fstart(i) = finitial(k(i));
      end
  end
else
  % it must be a multivariable minimization, or only one
  % variable, but not with a bracketed optimizer.

  % sort on f first in increasing order
  [finitial,tags] = sort(finitial);
  xinitial = xinitial(tags,:);

  if isempty(par.FractionUsed) || (par.FractionUsed==1)
    % use all the points as starting values
    nk = xsize;
  else
    nk = min(xsize,ceil(xsize*par.FractionUsed));
  end

  % any local minima found?
  xfinal = zeros(nk,par.Dim);
  ffinal = zeros(nk,1);
  exitflag = ffinal;
  xstart = xfinal;
  fstart = ffinal;
  % loop over the candidates chosen
  for i = 1:nk
    % store the start points in xstart and
    % fstart for plotting later
    xstart(i,:) = xinitial(i,:);
    fstart(i) = finitial(i);

    % but which optimizer?
    switch par.OptName
      case 'lsqnonlin'
        [xfinal(i,:),ffinal(i),residual,exitflag(i)] = ...
          lsqnonlin(fun,xinitial(i,:),LB,UB,par.Options); %#ok
      case 'fminsearchbnd'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fminsearchbnd(fun,xinitial(i,:),LB,UB,par.Options);
      case 'fminsearchcon'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fminsearchcon(fun,xinitial(i,:),LB,UB, ...
          par.A,par.B,par.Nonlcon,par.Options);
      case 'fminsearch'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fminsearch(fun,xinitial(i,:),par.Options);
      case 'fmincon'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fmincon(fun,xinitial(i,:),par.A,par.B, ...
          [],[],LB,UB,par.Nonlcon,par.Options);
    end
  end
end

% do we plot the results?
if strcmp(par.Plot,'on') && par.Dim <= 2
  switch par.Dim
    case 1
      % its a 1-d problem
      figure
      plot([xstart';xfinal'],[fstart';ffinal'],'g-')
      hold on
      plot(xfinal,ffinal,'ro')
      plot(xinitial,finitial,'b+')
      plot([xinitial(1),xinitial(end)],[0 0],'k:')
      hold off
      xlabel 'x'
      ylabel 'fun(x)'
      title(['Random multi-started search: ',par.OptName])

    case 2
      % a 2-d problem
      figure
      C = finitial-min(finitial);
      C = C/max(C);
      colormap hsv
      scatter3(xinitial(:,1),xinitial(:,2),finitial,50,C,'+')
      hold on
      plot3([xstart(:,1)';xfinal(:,1)'], ...
        [xstart(:,2)';xfinal(:,2)'],[fstart';ffinal'],'g-')
      plot3(xfinal(:,1),xfinal(:,2),ffinal,'ro')
      hold off
      xlabel 'x'
      ylabel 'y'
      zlabel 'fun([x,y])'
      title(['Random multi-started search: ',par.OptName])

  end
end

% ============================================
%     end of mainline rmsearch
% ============================================

% ============================================
% subfunction - check_params
% ============================================
function par = check_params(par)
% check the parameters for acceptability
%
% Defaults
% par.Plot = 'off'
% par.Nonlcon = '';
% par.FractionUsed = 1;
% par.InitialSample = 100;
% par.OptName = optname;

% Nonlcon == '' by default
if ~isempty(par.Nonlcon)
  if ischar(par.Nonlcon)
    par.Nonlcon = str2func(par.Nonlcon);
  end
  par.UseNLC = true;
else
  par.UseNLC = false;
end

% FractionUsed == 1 by default
if isempty(par.FractionUsed)
  par.FractionUsed = 1;
elseif (length(par.FractionUsed)>1) || (par.FractionUsed<=0) || (par.FractionUsed>1)
  error 'FractionUsed must be scalar, in the interval (0,1]'
end

% InitialSample == 100 by default
if isempty(par.InitialSample)
  par.InitialSample = 100;
elseif (length(par.InitialSample)>1) || (par.InitialSample<=0)
  error 'InitialSample must be positive and scalar'
else
  par.InitialSample = ceil(par.InitialSample);
end

% OverSample == 20 by default
if isempty(par.OverSample)
  par.OverSample = 20;
elseif (length(par.OverSample)>1) || (par.OverSample<1)
  error 'OverSample must be positive and scalar, >= 1'
end

% OptName is char
valid = {'fmincon', 'fminsearchcon', 'fminsearchbnd', ...
  'fminbnd', 'fzero', 'lsqnonlin', 'fminsearch'};
if isempty(par.OptName) || ~ischar(par.OptName)
  error 'Invalid OptName: Must be non-empty character'
end
ind = strmatch(par.OptName,valid,'exact');
if (length(ind)==1)
  par.OptName = valid{ind};
else
  ind = strmatch(par.OptName,valid);
  if isempty(ind) || (length(ind)>1)
    error(['Invalid OptName: ',par.OptName])
  else
    par.OptName = valid{ind};
  end
end
% 2+ dimensions for a 1-d optimizer?
if (par.Dim>1) && ismember(par.OptName,{'fminbnd', 'fzero'})
  error '1-d optimizer specified, but more than 1 variable to optimize'
end

% Options is a struct
if isempty(par.Options)
  switch par.OptName
    case 'fmincon'
      par.options = optimset('fmincon');
    case 'fzero'
      par.options = optimset('fzero');
    case 'fminbnd'
      par.options = optimset('fminbnd');
    case 'lsqnonlin'
      par.options = optimset('lsqnonlin');
    case {'fminsearch', 'fminsearchbnd', 'fminsearchcon'}
      par.options = optimset('fminbnd');
  end
elseif ~isstruct(par.Options)
  error 'If provided, Options must be a struct generated by optimset'
end

% Plot is char
valid = {'on', 'off'};
if isempty(par.Plot)
  error 'Invalid Plot: Must be character or empty'
end
ind = find(strncmpi(par.Plot,valid,length(par.Plot)));
if (length(ind)==1)
  par.Plot = valid{ind};
else
  error(['Invalid Plot: ',par.Plot])
end
if strcmp(par.Plot,'on') && (par.Dim>3)
  % No plots available for 4 or more variables
  Warning 'Sorry, plots are not generated for 4 or more variables.'
  par.Plot = 'off';
end

% A & B are both empty by default
if ~isempty(par.A) && (size(par.A,2)~=par.Dim)
  error 'A must have the same number of columns as the # of variables'
end
if ~isempty(par.B) && (size(par.A,1)~=size(par.B,1))
  error 'B must have the same number of rows as A'
end
if isempty(par.A)
  par.UseLC = false;
else
  par.UseLC = true;
end

% ============================================
% Included subfunction - parse_pv_pairs
% ============================================
function params=parse_pv_pairs(params,pv_pairs)
% parse_pv_pairs: parses sets of property value pairs, allows defaults
% usage: params=parse_pv_pairs(default_params,pv_pairs)
%
% arguments: (input)
%  default_params - structure, with one field for every potential
%             property/value pair. Each field will contain the default
%             value for that property. If no default is supplied for a
%             given property, then that field must be empty.
%
%  pv_array - cell array of property/value pairs.
%             Case is ignored when comparing properties to the list
%             of field names. Also, any unambiguous shortening of a
%             field/property name is allowed.
%
% arguments: (output)
%  params   - parameter struct that reflects any updated property/value
%             pairs in the pv_array.
%
% Example usage:
% First, set default values for the parameters. Assume we
% have four parameters that we wish to use optionally in
% the function examplefun.
%
%  - 'viscosity', which will have a default value of 1
%  - 'volume', which will default to 1
%  - 'pie' - which will have default value 3.141592653589793
%  - 'description' - a text field, left empty by default
%
% The first argument to examplefun is one which will always be
% supplied.
%
%   function examplefun(dummyarg1,varargin)
%   params.Viscosity = 1;
%   params.Volume = 1;
%   params.Pie = 3.141592653589793
%
%   params.Description = '';
%   params=parse_pv_pairs(params,varargin);
%   params
%
% Use examplefun, overriding the defaults for 'pie', 'viscosity'
% and 'description'. The 'volume' parameter is left at its default.
%
%   examplefun(rand(10),'vis',10,'pie',3,'Description','Hello world')
%
% params =
%     Viscosity: 10
%        Volume: 1
%           Pie: 3
%   Description: 'Hello world'
%
% Note that capitalization was ignored, and the property 'viscosity'
% was truncated as supplied. Also note that the order the pairs were
% supplied was arbitrary.

npv = length(pv_pairs);
n = npv/2;

if n~=floor(n)
  error 'Property/value pairs must come in PAIRS.'
end
if n<=0
  % just return the defaults
  return
end

if ~isstruct(params)
  error 'No structure for defaults was supplied'
end

% there was at least one pv pair. process any supplied
propnames = fieldnames(params);
lpropnames = lower(propnames);
for i=1:n
  p_i = lower(pv_pairs{2*i-1});
  v_i = pv_pairs{2*i};

  ind = strmatch(p_i,lpropnames,'exact');
  if isempty(ind)
    ind = find(strncmp(p_i,lpropnames,length(p_i)));
    if isempty(ind)
      error(['No matching property found for: ',pv_pairs{2*i-1}])
    elseif length(ind)>1
      error(['Ambiguous property name: ',pv_pairs{2*i-1}])
    end
  end
  p_i = propnames{ind};

  % override the corresponding default in params
  params = setfield(params,p_i,v_i); %#ok

end

% end % parse_pv_pairs


function [init_factors, init_factors_opts] = generate_init_factors(V, rank, options)
% Initialization algorithm
%
% Created by H.Kasai on May 21, 2019
%
% Change log:
%
%
%   May  21, 2019 (Hiroyuki Kasai): Created the initial version
%
%   Oct. 14, 2020 (H.Huangi): Added 'LPinit'
%
%   June. 20, 2022 (Hiroyuki Kasai): Modified to handle the case where one
%                                   factorization matrix is input, and
%                                   the other is not.

    % V = WH + R

    m = size(V, 1);
    n = size(V, 2);

    init_factors_opts = [];

    % initialize
    generate_wh_init = true;
    generate_r_init = true;

    %% generate W and H
    if isfield(options, 'x_init')
        if isfield(options.x_init, 'W') && isfield(options.x_init, 'H')
            init_factors.W = options.x_init.W;
            init_factors.H = options.x_init.H;
            generate_wh_init = false;
        elseif isfield(options.x_init, 'W') && ~isfield(options.x_init, 'H')
            init_factors.W = options.x_init.W;
            init_factors.H = rand(rank, n);
            generate_wh_init = false;
        elseif ~isfield(options.x_init, 'W') && isfield(options.x_init, 'H')
            init_factors.W = rand(m, rank);
            init_factors.H = options.x_init.H;
            generate_wh_init = false;
        end

        if isfield(options.x_init, 'R')
            init_factors.R = options.x_init.R;
            generate_r_init = false;
        end
    end

    if isfield(options, 'special_init_factors')

        if generate_wh_init || generate_r_init
            [init_factors, init_factors_opts] = options.special_init_factors(V, rank, generate_wh_init, init_factors_opts, options);
        end

    else

        if generate_wh_init

            if ~isfield(options, 'init_alg')
                alg = 'random';
            else
                alg = options.init_alg;
            end

            switch(alg)

                case 'LPinit'

                    H = LPinitSemiNMF(V, rank);
                    W = V * pinv(H);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'random'

                    W = rand(m, rank);
                    H = rand(rank, n);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'ones'

                    W = ones(m, rank);
                    H = ones(rank, n);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'semi_random' % for SemiNMF

                    H = rand(rank, n);
                    W = V / H; % V * inv(H)

                    init_factors.W = W;
                    init_factors.H = H;

                case 'symm'

                    W = ones(m, rank);

                    init_factors.W = W;
                    init_factors.H = W';

                case 'symm_mean'

                    % make sure that entries of H fall into the interval [0, 2*sqrt(m/k)],
                    % where 'm' is the average of all entries of V.
                    % See https://github.com/dakuang/symnmf

                    W = 2 * full(sqrt(mean(mean(V)) / rank)) * rand(m, rank);

                    init_factors.W = W;
                    init_factors.H = W';

                case 'NNDSVD'

                    [W, H] = NNDSVD(abs(V), rank, 0);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'kmeans'

                    [label, center] = litekmeans(V',rank, 'maxIter', 10);
                    center = max(0,center);
                    W = center';
                    WTW = W'*W;
                    WTW = max(WTW, WTW');
                    WTX = W'*V;
                    H = max(0, WTW\WTX);

                    init_factors.W = W;
                    init_factors.H = H;

                case {'prob_expectation', 'prob_random'}

                    T = tn_vector();

                    if strcmp(alg, 'prob_random')
                        E = exponential_dist();
                    end

                    exp_U   = options.exp_U;
                    exp_V   = options.exp_V;
                    var_U   = options.var_U;
                    var_V   = options.var_V;
                    mu_U    = options.mu_U;
                    mu_V    = options.mu_V;
                    tau_U   = options.tau_U;
                    tau_V   = options.tau_V;

                    for k = 1 : rank
                        for i = 1 : m
                            tau_U(i, k) = 1;

                            if options.ard
                                hyperparam = options.exp_lambdak(k);
                            else
                                hyperparam = options.hyperparams.lambdaU(i, k);
                            end

                            if strcmp(alg, 'prob_random')
                                mu_U(i, k) = E.exponential_draw(hyperparam);
                            else
                                mu_U(i, k) = 1.0/hyperparam;
                            end
                        end
                    end

                    for k = 1 : rank
                        for j = 1 : n
                            tau_V(j, k) = 1;

                            if options.ard
                                hyperparam = options.exp_lambdak(k);
                            else
                                hyperparam = options.hyperparams.lambdaV(j, k);
                            end

                            if strcmp(alg, 'prob_random')
                                mu_V(j, k) = E.exponential_draw(hyperparam);
                            else
                                mu_V(j, k) = 1.0/hyperparam;
                            end
                        end
                    end

                    for k = 1 : rank
                        [exp_U, var_U] = update_exp(T, exp_U, var_U, mu_U, tau_U, k);
                    end

                    for k = 1 : rank
                        [exp_V, var_V] = update_exp(T, exp_V, var_V, mu_V, tau_V, k);
                    end

                    init_factors.W = exp_U;
                    init_factors.H = exp_V';
                    init_factors_opts.var_W = var_U;
                    init_factors_opts.var_H = var_V';
                    init_factors_opts.mu_W  = mu_U;
                    init_factors_opts.mu_H  = mu_V';
                    init_factors_opts.tau_W = tau_U;
                    init_factors_opts.tau_H = tau_V';

                otherwise

                    % do random initialization

                    W = rand(m, rank);
                    H = rand(rank, n);

                    init_factors.W = W;
                    init_factors.H = H;

            end

        end

        if isfield(options, 'norm_w')
            if options.norm_w ~= 0
                % normalize W
                init_factors.W = normalize_W(init_factors.W, options.norm_w);
            end
        end

        if isfield(options, 'norm_h')
            if options.norm_h ~= 0
                % normalize H
                init_factors.H = normalize_H(init_factors.H, options.norm_h);
            end
        end

        %% generate R
        if generate_r_init
            if isfield(options, 'x_init_robust')
                if options.x_init_robust
                    if isfield(options, 'x_init')
                        if isfield(options.x_init, 'R')
                            init_factors.R = options.x_init.R;
                            generate_r_init = false;
                        end
                    end

                    if generate_r_init
                        init_factors.R = rand(m, n);
                    end
                else
                    init_factors.R = zeros(m, n);
                end
            else
                init_factors.R = zeros(m, n);
            end
        end

    end
end

function [exp_A, var_A] = update_exp(T, exp_A, var_A, mu_A, tau_A, k)

    exp_A(:,k) = T.expectation(mu_A(:,k), tau_A(:,k));
    var_A(:,k) = T.variance(mu_A(:,k), tau_A(:,k));

end


function [Sl1, Sl2, Jac] = deformation_field_3d_analysis(Tr, pixel_resolution)
% calculates sliding amount jacobian and other parameters of transformation
% Tr should be given in physical units (mm)!

dT = cell(3,3);
sz = size(Tr);
[m1,m2,m3] = ndgrid(1:sz(1), 1:sz(2), 1:sz(3));
mm{1} = m1;
mm{2} = m2;
mm{3} = m3;
for i1 = 1 : 3
    for i2 = 1: 3
        tmp = Tr(:,:,:,i1)+mm{i1}*pixel_resolution(i1);
        dT{i1,i2} = DGradient(tmp, [], i2) / pixel_resolution(i2);
    end
end

% EV = zeros([3, sz(1), sz(2), sz(3)]);
Sl1 = zeros([sz(1), sz(2), sz(3)]);
Sl2 = zeros([sz(1), sz(2), sz(3)]);
Jac = zeros([sz(1), sz(2), sz(3)]);
for i3 = 1 : sz(3)
for i2 = 1 : sz(2)
for i1 = 1 : sz(1)

    mat = [dT{1,1}(i1,i2,i3), dT{1,2}(i1,i2,i3), dT{1,3}(i1,i2,i3);...
           dT{2,1}(i1,i2,i3), dT{2,2}(i1,i2,i3), dT{2,3}(i1,i2,i3);...
           dT{3,1}(i1,i2,i3), dT{3,2}(i1,i2,i3), dT{3,3}(i1,i2,i3); ];

    mat_sym = mat'*mat;

    v = eig(mat_sym);
    v = sort(v);
    v = sqrt(abs(v));
    Sl1(i1,i2,i3) = (v(3)-v(1))/2;
    Sl2(i1,i2,i3) = v(3)/(v(2) + v(1));
    Jac(i1,i2,i3) = det(mat);
end
end
end

end


function dimacs = computedimacs(b,c,A,xin,y,s,K);
% COMPUTEDIMACS
%
% min <C,X> s.t     AX = b, X > 0
% max b'y   s.t S-C+A'y =0, S > 0

% If no primal exist, fake till later
if isempty(xin)
    x = c*0;
else
    x = xin;
end

if isempty(s)
    s = c-A'*y;
end

xres = inf;
sres = inf;

% Not officially defined in DIMACS
if K.f>0
    sres = -min(norm(s(1:K.f),inf));
end

% Errors in linear cone
if K.l>0
    xres = min(x(1+K.f:K.f+K.l));
    sres = min(s(1+K.f:K.f+K.l));
end

% Errors in quadratic cone
if any(K.q)
    top = K.f+K.l;
    for i = 1:length(K.q)
        X = x(1+top:top+K.q(i));
        S = s(1+top:top+K.q(i));
        xres = min(xres,X(1)-norm(X(2:end)));
        sres = min(sres,S(1)-norm(S(2:end)));
        top = top + K.q(i);
    end
end

% Errors in semidefinite cone
if any(K.s)
    top = K.f+K.l+K.q+K.r;
    for i = 1:length(K.s)
        X = reshape(x(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        S = reshape(s(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        xres = min(xres,min(eig(full(X))));
        sres = min(sres,min(eig(full(S))));
        top = top + K.s(i)^2;
    end
end

err1 = norm(b-A*x)/(1 + norm(b,inf));
err2 = max(0,-xres)/(1 + norm(b,inf));
err3 = conenorm(s-(c-A'*y),K)/(1+norm(c,inf));
%err3 = norm(s-(c-A'*y))/(1+norm(c,inf)); % Used by some solvers
err4 = max(0,-sres)/(1+max(abs(c)));
err5 = (c'*x-b'*y)/(1+abs(c'*x)+abs(b'*y));
err6 = x'*(c-A'*y)/(1+abs(c'*x)+abs(b'*y));

% No primal was computed
if isempty(xin)
    err1 = nan;
    err2 = nan;
    err5 = nan;
    err6 = nan;
end
dimacs = [err1 err2 err3 err4 err5 err6];

function t = conenorm(s,K)

% Implementation of the norm described on
% http://plato.asu.edu/dimacs/node3.html

t = 0;

if K.f + K.l>0
    t = t + norm(s(1:K.f+K.l));
end

top = 1+K.f+K.l;
if any(K.q)
    for i = 1:length(K.q)
        t = t + norm(s(top:top+K.q(i)-1));
        top  = top + K.q(i);
    end
end

if any(K.s)
    for i = 1:length(K.s)
        S = reshape(s(top:top+K.s(i)^2-1),K.s(i),K.s(i));
        t = t + norm(S,'fro');
        top  = top + K.s(i)^2;
    end
end


%   SCRIPT TO COMPUTE THE TORQUES AT EACH JOINT FOR DIFFERENT MOTION STATES OF
%   THE ARM.
%

function motor_selection

close all
global robot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETERS SECTION
%   Feel free to change the values of q, maximum_speeds and
%   maximum_accels.
%
%   This script tries to allow the student to test any mechanism
%   at the worst case. In this sense, q should be adjusted
%   as the pose where each joint would (statically) be needing a
%   higher torque.
%   The maximum_speeds and maximum_acceleration define a trapezoidal
%   speed profile. This trapezoidal speed is used by most machines
%   to command changes in speed in any of their joints.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

echo on
%LOAD ANY ROBOT YOU WOULD LIKE TO ANALYZE
%EXPERIMENTA CAMBIANDO EL ROBOT Y VIENDO LOS RESULTADOS!
robot = load_robot('ABB', 'IRB2600');
echo off

% robot pose: experiment by changing the pose while observing the different
%             torques at each joint
q=[0 pi/2 -pi/2 0 0 0]; %rad

%Velocidad maxima en cada articulacion en rad/s.
% robot.velmax = [deg2rad(175); %Axis 1, rad/s
%                 deg2rad(175); %Axis 2, rad/s
%                 deg2rad(175); %Axis 3, rad/s
%                 deg2rad(360); %Axis 4, rad/s
%                 deg2rad(360); %Axis 5, rad/s
%                 deg2rad(500)];%Axis 6, rad/s

maximum_speeds=[3.5 3.5 3.5 6.5 6.5 8.5];%rad/second

%maximum acceleration/deceleration for each joint

maximum_accels=maximum_speeds*2; %rad/second^2
%maximum_accels=[5 5 6 7 8 9];

% time of the trapezoidal profile that the joint moves at maximum speed
time_at_constant_speed=0.4; %seconds

%load robot parameters. Just uncomment this line

drawrobot3d(robot, q)

%START BY COMPUTING TORQUES AT G
%    robot.motors.G = [1 1 1 1 1 1 ]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  FIRST, COMPUTE TRAPEZOIDAL PROFILES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%compute acceleration plus deceleration times for every joint
time_acc = 2*maximum_speeds./maximum_accels+time_at_constant_speed;

%compute the total time for the slowest joint
total_time=max(time_acc);

% Trapezoidal speed profiles for each joint
[input_speeds, input_accels, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FINALLY, COMPUTE TORQUES FOR EACH MOTION STATE.
% Please note that we consider that the robot is placed at a fixed position and consider
% different motion situations when we change the acceleration and speed at
% each joint. For each motion state, the inverse dynamic model returns the
% torques at each joint that would bring the robot to that motion.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
compute_inverse_dynamics(q, input_speeds, input_accels, time);

  fprintf('\n\nOBSERVE THE PLOTS AND NOTE DOWN THE PEAK TORQUE, NOMINAL TORQUE AND MOTOR SPEEDS')
fprintf('\nNOW COMPUTE THE TORQUES FOR 5 DIFFERENT SELECTED MOTIONS STATES')
fprintf('\nPRESS ANY KEY TO CONTINUE...')

pause

% NOW COMPUTE THE WORST CASE CONSIDERING ONLY THE SPEED AND ACCELERATION AT
% 5 MOTIONS STATES
input_speeds = [zeros(6,1) maximum_speeds' maximum_speeds' maximum_speeds' zeros(6,1) ];
input_accels = [maximum_accels' maximum_accels' zeros(6,1) -maximum_accels' -maximum_accels' ];

compute_inverse_dynamics(q, input_speeds, input_accels, [1:5]);

%Computes a trapezoidal speed profile for every joint given maximum
%permitted accelerations and maximum joint speeds
function [input_speeds, input_accelerations, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time)

delta_time=0.01;

%build time vector: twice acceleration time plus time at constant speed
time = 0:delta_time:total_time;

input_speeds=[];
input_accelerations=[];

for j=1:length(maximum_speeds),
    vel_row=[];
    acc_row=[];
    for i=1:length(time),
        [vel acc] = compute_values(time(i), maximum_speeds(j), maximum_accels(j), total_time);
        vel_row = [vel_row vel];
        acc_row = [acc_row acc];
    end
    input_speeds = [input_speeds; vel_row];
    input_accelerations = [input_accelerations; acc_row];
end

%returns the values of velocity and speed corresponding to a given time
function [vel acc]=compute_values(time_i, vel_max, acc_max, total_time)

tacc = vel_max/acc_max;
tdec = total_time-tacc;

if time_i < tacc
    vel = time_i.*acc_max;
    acc = acc_max;
    return;
elseif (time_i >= tacc) & (time_i < tdec)
    vel = vel_max;
    acc = 0;
    return;
else % time_i> tdec
    vel = vel_max-(time_i-tdec)*acc_max;
    acc = -acc_max;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   COMPUTE THE INVERSE DYNAMICS FOR EACH MOTION STATE
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function compute_inverse_dynamics(q, input_speeds, input_accels, time)
global robot

%adjust_view(robot)
torques=[];
for j=1:length(time),
    fprintf('\nComputing time %d out of %d', j, length(time));
    % compute the torque to bring the robot instantaneously to this motion
    % state. change M=1  to add the effects of a 1kg mass load at the end effector
    M=20;
    %please note that the force due to the load acts on the z axis of
    tau=inversedynamic(robot, q, input_speeds(:,j), input_accels(:,j), [0  0 -9.81]', [ 0  0 -M 0 0 0 ]');
    torques=[torques tau];
end

%plot trapezoidal profiles
figure, hold, xlabel('time (s)'), ylabel('Input reference speeds (rad/s)')
plot(time, input_speeds(1,:), time, input_speeds(2,:), time, input_speeds(3,:),...
        time, input_speeds(4,:), time, input_speeds(5,:), time, input_speeds(6,:));
legend('Speed for joint 1 (qd1)','Speed for joint 2 (qd2)','Speed for joint 3 (qd3)',...
   'Speed for joint 4 (qd4)','Speed for joint 5 (qd5)','Speed for joint 6 (qd6)' )
%plot trapezoidal profiles, acceleration
figure, hold, xlabel('time (s)'), ylabel('Input reference acceleration (rad/s)')
plot(time, input_accels(1,:), time, input_accels(2,:), time, input_accels(3,:),...
        time, input_accels(4,:), time, input_accels(5,:), time, input_accels(6,:));
legend('Acceleration for joint 1 (qd1)','Acceleration for joint 2 (qd2)','Acceleration for joint 3 (qd3)',...
   'Acceleration for joint 4 (qd4)','Acceleration for joint 5 (qd5)','Acceleration for joint 6 (qd6)' )

% plot results. First, torques at each joint
figure, hold, xlabel('time (s)'), ylabel('Join Torques (N m)')
plot(time, torques(1,:), time, torques(2,:), time, torques(3,:),...
        time, torques(4,:), time, torques(5,:), time, torques(6,:));
legend('Torque for joint 1 ','Torque  for joint 2 ','Torque  for joint 3 ',...
   'Torque  for joint 4','Torque  for joint 5 ','Torque  for joint 6 ' )

%plot torques at each motor
figure, hold, xlabel('time (s)'), ylabel('Motor Torques (N m)')
plot(time, torques(1,:)/robot.motors.G(1), time, torques(2,:)/robot.motors.G(2), time, torques(3,:)/robot.motors.G(3),...
        time, torques(4,:)/robot.motors.G(4), time, torques(5,:)/robot.motors.G(5), time, torques(6,:)/robot.motors.G(6));
legend('Torque at motor 1 ','Torque at motor 2 ','Torque at motor 3 ',...
   'Torque at motor 4 ','Torque at motor 5 ','Torque at motor 6 ' )

%plot power needed by the motor at each time step, without considering the
%losses at the gears
figure, hold, xlabel('time (s)'), ylabel('Power needed by each motor (W)')
plot(time, torques(1,:).*input_speeds(1,:), time, torques(2,:).*input_speeds(2,:), time, torques(3,:).*input_speeds(3,:),...
        time, torques(4,:).*input_speeds(4,:), time, torques(5,:).*input_speeds(5,:), time, torques(6,:).*input_speeds(6,:));
legend('Power: motor 1','Power: motor 2','Power: motor 3',...
   'Power: motor 4','Power: motor 5','Power: motor 6' )

%plot motor speed in rpm for each motor
figure, hold, xlabel('time (s)'), ylabel('Speed in r.p.m of every motor (rev/min)')
plot(time, robot.motors.G(1)*input_speeds(1,:)*30/pi, time, robot.motors.G(2)*input_speeds(2,:)*30/pi, time, robot.motors.G(3)*input_speeds(3,:)*30/pi,...
        time, robot.motors.G(4)*input_speeds(4,:)*30/pi, time, robot.motors.G(5)*input_speeds(5,:)*30/pi, time, robot.motors.G(6)*input_speeds(6,:)*30/pi);
legend('Speed at motor 1 (qd1*G)','Speed at motor 2 (qd2*G)','Speed at motor 3 (qd3*G)',...
   'Speed at motor 4 (qd4*G)','Speed at motor 5 (qd5*G)','Speed at motor 6 (qd6*G)' )

%Now present results:
fprintf('\nMAIN RESULTS (referred to each motor): ')
fprintf('\n------------------------------------------------------------------------------------ ')
fprintf('\n                         Joint 1 - Joint 2 - Joint 3 - Joint 4  - Joint 5 - Joint 6: ')
fprintf('\nPeak Torque (N�m):        %.3f     %.3f     %.3f     %.3f      %.3f    %.3f ', max(abs(torques(1,:)/robot.motors.G(1))), max(abs(torques(2,:)/robot.motors.G(2))) , max(abs(torques(3,:)/robot.motors.G(3))) , max(abs(torques(4,:)/robot.motors.G(4))) , max(abs(torques(5,:)/robot.motors.G(5))) , max(abs(torques(6,:)/robot.motors.G(6))))
fprintf('\nNominal Torque (N�m):     %.3f     %.3f     %.3f     %.3f      %.3f    %.3f  ', abs(torques(1,round(length(torques)/2))/robot.motors.G(1)), abs(torques(2,round(length(torques)/2))/robot.motors.G(2)), abs(torques(3,round(length(torques)/2))/robot.motors.G(3))...
    , abs(torques(4,round(length(torques)/2))/robot.motors.G(4)), abs(torques(5,round(length(torques)/2))/robot.motors.G(5)), abs(torques(6,round(length(torques)/2))/robot.motors.G(6)))
fprintf('\nMax motor speed (r.p.m.):   %.1f     %.1f     %.1f     %.1f      %.1f    %.1f  ', max(abs(robot.motors.G(1)*input_speeds(1,:))*30/pi), max(abs(robot.motors.G(2)*input_speeds(2,:))*30/pi), max(abs(robot.motors.G(3)*input_speeds(3,:))*30/pi)...
    ,max(abs(robot.motors.G(4)*input_speeds(4,:))*30/pi), max(abs(robot.motors.G(5)*input_speeds(5,:))*30/pi), max(abs(robot.motors.G(6)*input_speeds(6,:))*30/pi))
fprintf('\n------------------------------------------------------------------------------------ ')


function [ undx, xdnu ] = r8col_tol_undex ( m, n, a, unique_num, tol )

%*****************************************************************************80
%
%% R8COL_TOL_UNDEX indexes tolerably unique entries of an R8COL.
%
%  Discussion:
%
%    An R8COL is an M x N array of R8 values, regarded as N columns
%    each of M R8 values.
%
%    The goal of this routine is to determine a vector UNDX,
%    which points to the unique elements of A, in sorted order,
%    and a vector XDNU, which identifies, for each entry of A, the index of
%    the unique sorted element of A.
%
%    This is all done with index vectors, so that the elements of
%    A are never moved.
%
%    The first step of the algorithm requires the indexed sorting
%    of A, which creates arrays INDX and XDNI.  (If all the entries
%    of A are unique, then these arrays are the same as UNDX and XDNU.)
%
%    We then use INDX to examine the entries of A in sorted order,
%    noting the unique entries, creating the entries of XDNU and
%    UNDX as we go.
%
%    Once this process has been completed, the object A could be
%    replaced by a compressed object XU, containing the unique entries
%    of A in sorted order, using the formula
%
%      XU(1:UNIQUE_NUM) = A(UNDX(1:UNIQUE_NUM)).
%
%    We could then, if we wished, reconstruct the entire vector A, or
%    any element of it, by index, as follows:
%
%      A(I) = XU(XDNU(I)).
%
%    We could then replace A by the combination of XU and XDNU.
%
%    Later, when we need the I-th entry of A, we can locate it as
%    the XDNU(I)-th entry of XU.
%
%    Here is an example of a vector A, the sort and inverse sort
%    index vectors, and the unique sort and inverse unique sort vectors
%    and the compressed unique sorted vector.
%
%      I    A   Indx  Xdni      XU   Undx  Xdnu
%    ----+-----+-----+-----+--------+-----+-----+
%      1 : 11.     1     1 :    11.     1     1
%      2 : 22.     3     5 :    22.     2     2
%      3 : 11.     6     2 :    33.     4     1
%      4 : 33.     9     8 :    55.     5     3
%      5 : 55.     2     9 :                  4
%      6 : 11.     7     3 :                  1
%      7 : 22.     8     6 :                  2
%      8 : 22.     4     7 :                  2
%      9 : 11.     5     4 :                  1
%
%    INDX(2) = 3 means that sorted item(2) is A(3).
%    XDNI(2) = 5 means that A(2) is sorted item(5).
%
%    UNDX(3) = 4 means that unique sorted item(3) is at A(4).
%    XDNU(8) = 2 means that A(8) is at unique sorted item(2).
%
%    XU(XDNU(I))) = A(I).
%    XU(I)        = A(UNDX(I)).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the dimension of the data values.
%
%    Input, integer N, the number of data values.
%
%    Input, real A(M,N), the data values.
%
%    Input, integer UNIQUE_NUM, the number of unique values in A.
%    This value is only required for languages in which the size of
%    UNDX must be known in advance.
%
%    Input, real TOL, a tolerance for equality.
%
%    Output, integer UNDX(UNIQUE_NUM), the UNDX vector.
%
%    Output, integer XDNU(N), the XDNU vector.
%
  undx = zeros ( unique_num, 1 );
  xdnu = zeros ( n, 1 );
%
%  Implicitly sort the array.
%
  indx = r8col_sort_heap_index_a ( m, n, a );
%
%  Consider entry I = 1.
%  It is unique, so set the number of unique items to K.
%  Set the K-th unique item to I.
%  Set the representative of item I to the K-th unique item.
%
  i = 1;
  k = 1;
  undx(k) = indx(i);
  xdnu(indx(i)) = k;
%
%  Consider entry I.
%
%  If it is unique, increase the unique count K, set the
%  K-th unique item to I, and set the representative of I to K.
%
%  If it is not unique, set the representative of item I to a
%  previously determined unique item that is close to it.
%
  for i = 2 : n

    unique = 1;

    for j = 1 : k
      diff = max ( abs ( a(1:m,indx(i)) - a(1:m,undx(j)) ) );
      if ( diff <= tol )
        unique = 0;
        xdnu(indx(i)) = j;
        break
      end
    end

    if ( unique )
      k = k + 1;
      undx(k) = indx(i);
      xdnu(indx(i)) = k;
    end

  end

  return
end


function z = cpf_tangent(V, lam, Ybus, Sbusb, Sbust, pv, pq, ...
                            zprv, Vprv, lamprv, parameterization, direction)
%CPF_TANGENT  Computes normalized tangent predictor for continuation power flow
%   Z = CPF_TANGENT(V, LAM, YBUS, SBUSB, SBUST, PV, PQ, ...
%                                 ZPRV, VPRV, LAMPRV, PARAMETERIZATION, DIRECTION)
%
%   Computes a normalized tangent predictor for the continuation power flow.
%
%   Inputs:
%       V : complex bus voltage vector at current solution
%       LAM : scalar lambda value at current solution
%       YBUS : complex bus admittance matrix
%       SBUSB : handle of function returning nb x 1 vector of complex
%               base case injections in p.u. and derivatives w.r.t. |V|
%       SBUST : handle of function returning nb x 1 vector of complex
%               target case injections in p.u. and derivatives w.r.t. |V|
%       PV : vector of indices of PV buses
%       PQ : vector of indices of PQ buses
%       ZPRV : normalized tangent prediction vector from previous step
%       VPRV : complex bus voltage vector at previous solution
%       LAMPRV : scalar lambda value at previous solution
%       PARAMETERIZATION : value of cpf.parameterization option.
%       DIRECTION: continuation direction (+1 for postive lambda
%                  increase, -1 otherwise)
%
%   Outputs:
%       Z : the normalized tangent prediction vector

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Shrirang Abhyankar, Argonne National Laboratory
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% sizes
nb = length(V);
npv = length(pv);
npq = length(pq);

Vm = abs(V);

%% compute Jacobian for the power flow equations
[dSbus_dVa, dSbus_dVm] = dSbus_dV(Ybus, V);
[dummy, neg_dSdb_dVm] = Sbusb(Vm);
[dummy, neg_dSdt_dVm] = Sbust(Vm);
dSbus_dVm = dSbus_dVm - neg_dSdb_dVm - lam * (neg_dSdt_dVm - neg_dSdb_dVm);

j11 = real(dSbus_dVa([pv; pq], [pv; pq]));
j12 = real(dSbus_dVm([pv; pq], pq));
j21 = imag(dSbus_dVa(pq, [pv; pq]));
j22 = imag(dSbus_dVm(pq, pq));

J = [   j11 j12;
        j21 j22;    ];

Sxf = Sbust(Vm) - Sbusb(Vm);    %% "transfer" at current voltage level
dF_dlam = -[real(Sxf([pv; pq])); imag(Sxf(pq))];
[dP_dV, dP_dlam] = cpf_p_jac(parameterization, zprv, V, lam, Vprv, lamprv, pv, pq);

%% linear operator for computing the tangent predictor
J = [   J   dF_dlam;
      dP_dV dP_dlam ];

% J = [ J, dF_dlam;
%       z([pv; pq; nb+pq; 2*nb+1])'];

%% compute normalized tangent predictor
z = zeros(size(zprv));
s = zeros(npv+2*npq+1, 1);
s(end,1) = sign(direction);
z([pv; pq; nb+pq; 2*nb+1]) = J\s;   %% tangent vector
z = z/norm(z);                      %% normalize tangent predictor


function [u,eqn,info] = fracLapP2P2(node,elem,pde,option)
%% FRACLAPP2P2 solves fractional Laplacian equation using P2-P2 element
%
% [u,eqn,info] = fracLapP1P1(node,elem,pde,option) solves the fractional
% Laplacian equation
%
%  (-\Delta^^s u = f in \Omega with u = 0 on \partial \Omega.

%% Options
if ~exist('option','var'), option = []; end
if ~isfield(option,'plotflag'), option.plotflag = 0; end

%% Parameters
global s;
alpha = 1-2*s;
if s == 0.5
    gamma = 1;
else
    gamma = 5/(2*s)+0.01; % a different grading factor
end

%% Mesh and data structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1); NE = size(edge,1);
My = round(pde.L*sqrt(NT/2));
y = gradmap(0,pde.L,gamma,My);
Ny = length(y); % number of vertices in y-direction
NTy = Ny - 1;   % number of elements in y-direction
NTtotal = NT*NTy;
Nxdof = (N + NE); % dof in x  is number of vertices plus edges
Nydof = (Ny + NTy); % dof in y direction is number of vertices plus elements
Ndof = Nxdof*Nydof;

tic;
%% Stiffness matrix and mass matrix in the extended direction
% quantities in y direction
hy = diff(y);
a = zeros(NTy,5);
for i = 1:5
    a(:,i) = diff(y.^(alpha+i)/(alpha+i));
end
y1 = y(1:end-1);
y2 = y(2:end);
y12 = y1 + y2;
y1y2 = y1.*y2;
ym = y12/2;
% stiffness matrix in y direction
Ay = zeros(NTy,3,3);
Ay(:,1,1) = a(:,1)./hy.^2;
Ay(:,1,2) = -Ay(:,1,1);
Ay(:,2,1) = -Ay(:,1,1);
Ay(:,2,2) = Ay(:,1,1);
Ay(:,1,3) = 8*(a(:,2) - ym.*a(:,1))./hy.^3;
Ay(:,3,1) = Ay(:,1,3);
Ay(:,2,3) = -Ay(:,1,3);
Ay(:,3,2) = Ay(:,2,3);
Ay(:,3,3) = 64*(a(:,3) - 2*ym.*a(:,2) + ym.^2.*a(:,1))./hy.^4;
% mass matrix in y direction
My = zeros(NTy,3,3);
My(:,1,1) = (a(:,3) - 2*y1.*a(:,2) + y1.^2.*a(:,1))./hy.^2;
My(:,1,2) = (-a(:,3) + (y1+y2).*a(:,2) - y1.*y2.*a(:,1))./hy.^2;
My(:,2,1) = My(:,1,2);
My(:,2,2) = (a(:,3) - 2*y2.*a(:,2) + y2.^2.*a(:,1))./hy.^2;
My(:,1,3) = 4*(a(:,4) - (y12+y2).*a(:,3) + (y2.*y12 + y1y2).*a(:,2) ...
             - y2.*y1y2.*a(:,1))./hy.^3;
My(:,3,1) = My(:,1,3);
My(:,2,3) = 4*(-a(:,4) + (y12+y1).*a(:,3) - (y1.*y12 + y1y2).*a(:,2) ...
             + y1.*y1y2.*a(:,1))./hy.^3;
My(:,3,2) = My(:,2,3);
My(:,3,3) = 16*(a(:,5) - 2*y12.*a(:,4) + (y12.^2 + 2*y1y2).*a(:,3)...
               -2*y12.*y1.*y2.*a(:,2) + y1y2.^2.*a(:,1))./hy.^4;

%% Stiffness matrix and mass matrix in the original direction
[Dlambda,area] = gradbasis(node,elem);
% Compute a piecewise constant diffusion coefficient
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                                 % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)       % d is a function
    [lambda,weight] = quadpts(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
        K = K + weight(p)*pde.d(pxy);
   end
end
if ~isempty(pde.d) % build the coefficients into the scaled area
    areaK = K.*area;
else
    areaK = area;
end
At = zeros(NT,6,6);
Mt = zeros(NT,6,6);
% stiffness matrix
[lambda, w] = quadpts(2);
nQuad = size(lambda,1);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    for i = 1:6
        for j = 1:6
            At(:,i,j) = At(:,i,j) + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*areaK;
        end
    end
end
% mass matrix
[lambda, w] = quadpts(4);
nQuad = size(lambda,1);
phi(:,6) = 4*lambda(:,1).*lambda(:,2);
phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
phi(:,4) = 4*lambda(:,2).*lambda(:,3);
phi(:,5) = 4*lambda(:,3).*lambda(:,1);
for i = 1:6
    for j = 1:6
        for p = 1:nQuad
            Mt(:,i,j) = Mt(:,i,j) + w(p)*phi(p,i).*phi(p,j);
        end
        Mt(:,i,j) = Mt(:,i,j).*areaK;
    end
end
clear phi Dphip lambda Dlambda

%% Assemble stiffness matrix
dofMap = repmat(1:Nxdof,Nydof,1) + repmat((0:Nydof-1)'*Nxdof,1,Nxdof);
ii = zeros(18*18*NTtotal,1);
jj = zeros(18*18*NTtotal,1);
sA = zeros(18*18*NTtotal,1);
index = 0;
for m = 1:3
    for n = 1:3
        for i = 1:6
            for j = 1:6
                if m < 3
                    ii(index+1:index+NTtotal) = dofMap(m:Ny+m-2,elem2dof(:,i));
                else % m = 3: middle points of elements in y direction
                    ii(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem2dof(:,i));
                end
                if n < 3
                    jj(index+1:index+NTtotal) = dofMap(n:Ny+n-2,elem2dof(:,j));
                else % n = 3: middle points of elements in y direction
                    jj(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem2dof(:,j));
                end
                sA(index+1:index+NTtotal) = kron(At(:,i,j),My(:,m,n)) + ...
                                            kron(Mt(:,i,j),Ay(:,m,n));
                index = index + NTtotal;
            end
        end
    end
end
A = sparse(ii,jj,sA,Ndof,Ndof);
clear ii jj sA At Mt Ay My

%% Assemble the right hand side
b = zeros(Ndof,1);
u = zeros(Ndof,1);

%% Set up boundary conditions
% bdDof is found in dofP2
lateralbd = dofMap(:,bdDof);
topbd = dofMap(Ny,:);

% Modify the matrix to include the Dirichlet boundary condition
bdidx = zeros(Ndof,1);
bdidx(lateralbd) = 1;
bdidx(topbd) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
A = T*A*T + Tbd;

% Compute boundary integral over the original domain
if ~isfield(option,'fquadorder')
    option.fquadorder = 6;
end
[lambdaf,weightf] = quadpts(option.fquadorder);
phif(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
phif(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
phif(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
phif(:,4) = 4*lambda(:,2).*lambda(:,3);
phif(:,5) = 4*lambda(:,3).*lambda(:,1);
phif(:,6) = 4*lambda(:,1).*lambda(:,2);
nQuadf = size(lambdaf,1);
ft = zeros(NT,6);
for p = 1:nQuadf
    % quadrature points in the x-y coordinate
    pxy = lambdaf(p,1)*node(elem(:,1),:) ...
        + lambdaf(p,2)*node(elem(:,2),:) ...
        + lambdaf(p,3)*node(elem(:,3),:);
    fp = pde.f(pxy);
    for i = 1:6
        ft(:,i) = ft(:,i) + weightf(p)*phif(p,i)*fp;
    end
end
ft = ds*ft.*repmat(area,1,6);
b = b + accumarray(elem2dof(:),ft(:),[Ndof,1]);
% Neumann edfts are considered as open set. So the corner points should be
% set as Dirichlet boundary condition!
b(lateralbd) = 0;
clear ft

%% Record assembling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
freeNode = find(bdidx==0);
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for larft size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeNode) = A(freeNode,freeNode)\b(freeNode);
        residual = norm(b - A*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        option.x0 = u;
        option.solver = 'VCYCLE';
        option.freeDof = freeDof;
        [u,info] = mgfracLapP2P2(A,b,elem,option);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(A(freeNode,freeNode),b(freeNode),option);
end

%% Compute error using boundary integral
if isfield(pde,'exactu')
    err = zeros(NT,1);
    [lambda,weight] = quadpts(7);
    phi(:,6) = 4*lambda(:,2).*lambda(:,1);
    phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
    phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
    phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
    phi(:,4) = 4*lambda(:,2).*lambda(:,3);
    phi(:,5) = 4*lambda(:,1).*lambda(:,3);
    nQuad = size(lambda,1);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
             + lambda(p,2)*node(elem(:,2),:) ...
             + lambda(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        uhp = u(elem2dof(:,1)).*phi(p,1) + ...
              u(elem2dof(:,2)).*phi(p,2) + ...
              u(elem2dof(:,3)).*phi(p,3) + ...
              u(elem2dof(:,4)).*phi(p,4) + ...
              u(elem2dof(:,5)).*phi(p,5) + ...
              u(elem2dof(:,6)).*phi(p,6);
        up = pde.exactu(pxy);
        err = err + weight(p)*fp.*(up - uhp);
    end
else
    err = 0;
end
err = ds*sum(err.*area);
err = sqrt(err);
info.errH1 = err;

%% Output information
eqn = struct('A',A,'b',b,'freeNode',freeNode,'edge',edge);
info.assembleTime = assembleTime;


function vAberr=aberCorr(vOrig,obsVel,sunDist)
%%ABERCORR Rotate vectors for stellar aberration, approximately including
%          the effects of the gravitational potential of the Sun if the
%          distances between the observers and the Sun are provided.
%
%INPUTS: vOrig A 3XN set of N vectors pointing from the observers to the
%              objects being observed without corruption due to
%              aberration. The units of the vector do not matter.
%              Normally, one would expect vOrig to be composed of unit
%              vectors as aberration affects the pointing direction of the
%              vectors.
%       obsVel The 3XN matrix of the velocity of each observer with
%              respect to the stellar coordinate system origin in meters
%              per second. For example, if using the barycentric celestial
%              reference system (BCRS), they are the set of velocity
%              vectors of the observers with respect to the barycenter.
%      sunDist An optional 3XN vector of the scalar distances in meters
%              from the Sun to the observer. If this parameter is
%              included, an approximate correction due to the
%              gravitational potential of the Sun is included.
%
%OUTPUTS: vAberr The 3XN set of N vectors rotated to deal with aberration
%                effects.
%
%If all parameters are provided, the function is essentially a wrapper for
%the function iauAb in the International Astronomical Union's Standards of
%Fundamental Astronomy library with an adjustment so that the vector in
%question need not have unit magnitude. If the distance to the Sun is
%omitted, then the standard special relativistic aberration correction
%without any gravitational effects is applied. The standard special
%relativistic aberration correction is described in Chapter 7 of [1]
%
%Note that if the vectors provided are meant to be apparent distances,
%rather than just unit vectors representing directions, the transformation
%does not adjust the magnitudes of the vectors to account for special
%relativistic contraction of space due to the motion of the observer with
%respect to the origin. The magnitudes of the output vectors equal the
%magnitudes of the input vectors.
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%vAberr=aberCorr(vOrig,obsVel,sunDist);
%or
%vAberr=aberCorr(vOrig,obsVel);
%
%REFERENCES:
%[1] S. E. Urban and K. P.Seidelmann, Eds.,Explanatory Supplement to the
%    Astronomical Almanac, 3rd ed. Mill Valley, CA: University Science
%    Books, 2013.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

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 fhat = convSO3(fhat1,fhat2)
%

% old sizes
s1 = size(fhat1);
s2 = size(fhat2);

% get bandwidth
L = min(dim2deg(s1(1)),dim2deg(s2(1)));

% new size
l=length(s2)-length(s1);
s = max([s1(2:end),ones(1,l);s2(2:end),ones(1,-l)]);

% compute Fourier coefficients of the convolution
fhat = zeros([deg2dim(L+1),s]);
if prod(s) == 1 %simple SO3Fun
  for l = 0:L
    ind = deg2dim(l)+1:deg2dim(l+1);
    fhat(ind) = reshape(fhat2(ind),2*l+1,2*l+1) * ...
      reshape(fhat1(ind),2*l+1,2*l+1) ./ sqrt(2*l+1);
  end
else % vector valued SO3Fun
  for l = 0:L
    ind = deg2dim(l)+1:deg2dim(l+1);
    fhat_l = pagemtimes( full(reshape(fhat2(ind,:),[2*l+1,2*l+1,s2(2:end)])) , ...
      full(reshape(fhat1(ind,:),[2*l+1,2*l+1,s1(2:end)])) ) ./ sqrt(2*l+1);
    fhat(ind,:) = reshape(fhat_l,[],prod(s));
  end
end

end


function [sys,x0,str,ts]=NL_PID_fhan_c(t,x,u,flag,r0,h0,r1,h1,h,c,r2,h2)
switch flag,
    case 0,
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2,
        sys=mdlUpdates(x,u,r0,r1,h0,h1,h);
    case 3,
        sys=mdlOutputs(x,c,r2,h2);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9},
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=5;
    sizes.NumOutputs=2;
    sizes.NumInputs=2;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdates(x,u,r0,r1,h0,h1,h)
    fh0=fhan(x(1)-u(1),x(2),r0,h0);
    x(1)=x(1)+h*x(2);
    x(2)=x(2)+h*fh0;
    fh1=fhan(x(3)-u(2),x(4),r1,h1);
    x(3)=x(3)+h*x(4);
    x(4)=x(4)+h*fh1;
    x(5)=x(5)+h*(x(1)-x(3));
    sys=x;
function sys=mdlOutputs(x,c,r2,h2)
    sys(1)=-fhan(x(1)-x(3),c*(x(2)-x(4)),r2,h2);
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;
function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end


%% 2's complement of binary no;

function twos_comp = twos_complement_of_binary(bin)
c=0;
temp=bin;
twos_comp=0;
while(temp>0 && rem(temp,10)==0)
    twos_comp=twos_comp*10 + rem(temp,10);
    temp=fix(temp/10);
    c=c+1;
end
if(temp>0)
    twos_comp=twos_comp*10 + rem(temp,10);
    temp=fix(temp/10);
    c=c+1;
end
while(temp>0)
    if(rem(temp,10)==1)
        twos_comp=twos_comp*10 + 0;
    else
        twos_comp=twos_comp*10 + 1;
    end
    temp=fix(temp/10);
    c=c+1;
end

temp=twos_comp;
twos_comp=0;
while(c>0)      %reversing the order;
    twos_comp=twos_comp*10 + rem(temp,10);
    temp=fix(temp/10);
    c=c-1;
end
end


function Keff = electrolyteConductivity(ce,T,param,batterySection)
% electrolyteConductivity  Evaluates the conductivity coefficients for the
% electrolyte phase. The measurement unit is [S/m]
%
%   Keff = electrolyteConductivity(ce,T,param) evaluates
%   the conductivity coefficients for the anode, separator and cathode electrolyte phase of the
%   battery. You can modify the script to meet your particular needs.
%
%   The conductivity coefficients can be evaluated in isothermal case
%   (param.TemperatureEnabled=0) or adiabatic case
%   (param.TemperatureEnabled=1 or 2).
%
%   You can modify the way that the conductivity coefficients are computed, as
%   function of electrolyte concentration and temperature. The main script
%   will pass also the param array.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

if(param.TemperatureEnabled>=1)
    switch(batterySection)
        case'p'
            Keff = param.eps_p^param.brugg_p *(1e-4*ce.*((-10.5+0.668*1e-3*ce+0.494*1e-6*ce.^2) +...
                (0.074  -1.78*1e-5*ce -8.86*1e-10*ce.^2).*T + (-6.96*1e-5+2.8*1e-8*ce).*T.^2).^2);
        case 's'
            Keff = param.eps_s^param.brugg_s *(1e-4*ce.*((-10.5+0.668*1e-3*ce+0.494*1e-6*ce.^2) +...
                (0.074  -1.78*1e-5*ce -8.86*1e-10*ce.^2).*T + (-6.96*1e-5+2.8*1e-8*ce).*T.^2).^2);
        case 'n'
            Keff = param.eps_n^param.brugg_n *(1e-4*ce.*((-10.5+0.668*1e-3*ce+0.494*1e-6*ce.^2) +...
                (0.074  -1.78*1e-5*ce -8.86*1e-10*ce.^2).*T + (-6.96*1e-5+2.8*1e-8*ce).*T.^2).^2);
    end
else
        switch(batterySection)
        case'p'
            Keff = param.eps_p^param.brugg_p *(4.1253*1e-2 + 5.007*1e-4*ce - 4.7212*1e-7*ce.^2 +1.5094*1e-10*ce.^3 -1.6018*1e-14*ce.^4);
        case 's'
            Keff = param.eps_s^param.brugg_s *(4.1253*1e-2 + 5.007*1e-4*ce - 4.7212*1e-7*ce.^2 +1.5094*1e-10*ce.^3 -1.6018*1e-14*ce.^4);
        case 'n'
            Keff = param.eps_n^param.brugg_n *(4.1253*1e-2 + 5.007*1e-4*ce - 4.7212*1e-7*ce.^2 +1.5094*1e-10*ce.^3 -1.6018*1e-14*ce.^4);
        end
end


function Sigma = UpdateCMA(X,Sigma,gen)
% Update the CMA model

%------------------------------- 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".
%--------------------------------------------------------------------------

    n = size(X,2);

    %% Calculate the CMA parameters
    mu    = 4 + floor(3*log(n));
    mu1   = floor(mu/2);
    w     = log((mu+1)/2) - log(1:mu1);
    w     = w./sum(w);
    mueff = 1./sum(w.^2);
    cs    = (mueff+2)./(n+mueff+5);
    ds    = 1 + 2*max(0,sqrt((mueff-1)./(n+1))-1) + cs;
    cc    = (4+mueff/n)./(n+4+2*mueff/n);
    c1    = 2./((n+1.3).^2+mueff);
    cmu   = min(1-c1,2*(mueff-2+1/mueff)./((n+2).^2+mueff)); % Modified
    ENI   = sqrt(n)*(1-1/4/n+1/21/n^2);

    %% Update the CMA model
    y           = (X(1:mu1,:)-repmat(Sigma.x,mu1,1))/Sigma.sigma;
    yw          = w*y;
    Sigma.x     = Sigma.x + Sigma.sigma*yw;
    Sigma.ps    = (1-cs)*Sigma.ps + sqrt(cs*(2-cs)*mueff)*Sigma.C^(-1/2)*yw';
    hs          = norm(Sigma.ps)./sqrt(1-(1-cs).^(2*(gen+1))) < (1.4+2/(n+1))*ENI;
    deltahs     = 1 - hs; % Modified
    Sigma.pc    = (1-cc)*Sigma.pc + hs*sqrt(cc*(2-cc)*mueff)*yw;
    Sigma.sigma = Sigma.sigma*exp(cs/ds*(norm(Sigma.ps)/ENI-1));
    Sigma.C     = (1-c1-cmu)*Sigma.C + c1*(Sigma.pc'*Sigma.pc+deltahs*Sigma.C) + cmu*y'*diag(w)*y;
    Sigma.C     = triu(Sigma.C) + triu(Sigma.C,1)'; % Enforce symmetry

    %% Reset the CMA model if possible
    [B,D] = eig(Sigma.C);
    diagD = diag(D);
    diagC = diag(Sigma.C);
    ConditionCov  = max(diagD) > 1e14*min(diagD);
    NoEffectCoord = any(Sigma.x==Sigma.x+0.2*Sigma.sigma*sqrt(diagC)');
    NoEffectAxis  = all(Sigma.x==Sigma.x+0.1*Sigma.sigma*sqrt(diagD(mod(gen,n)+1))*B(:,mod(gen,n)+1)');
    TolXUp        = any(Sigma.sigma*sqrt(diagC)>1e4);
    if ConditionCov || NoEffectCoord || NoEffectAxis || TolXUp
        Sigma = struct('s',[],'x',[],'sigma',0.5,'C',eye(n),'pc',0,'ps',0);
    end
end


function vesselness_wrapper = PTKComputeVesselnessFromHessianeigenvalues(hessian_eigs_wrapper, voxel_size)
    % PTKComputeVesselnessFromHessianeigenvalues. Vesselness filter for detecting blood vessels
    %
    %     PTKComputeVesselnessFromHessianeigenvalues computes a mutiscale
    %     vesselness filter based on Frangi et al., 1998. "Multiscale Vessel
    %     Enhancement Filtering". The filter returns a value at each point which
    %     in some sense representes the probability of that point belonging to a
    %     blood vessel.
    %
    %     This function takes in a PTKWraper object which can either contain a nx6
    %     matrix containing the 3 Hessian matrix eigenvalues for each of n
    %     points, or it can be an ixjxkx3 matrix representing the 3 Hessian
    %     matrix eigenvalues for an image of dimension ixjxk.
    %
    %     The output is a single vesselness value for each input point.
    %
    %     See the PTKVesselness plugin for example usage.
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    % lam1 = smallest eigenvalue, lam3 = largest eigenvalue

    vesselness_wrapper = CoreWrapper;

    % The input matrix could be a linear set of points, or an image matrix
    if ndims(hessian_eigs_wrapper.RawImage) == 2
        lam1 = hessian_eigs_wrapper.RawImage(:, 1); % smallest
        lam2 = hessian_eigs_wrapper.RawImage(:, 2);
        lam3 = hessian_eigs_wrapper.RawImage(:, 3); % biggest
    else
        lam1 = hessian_eigs_wrapper.RawImage(:,:,:,1); % smallest
        lam2 = hessian_eigs_wrapper.RawImage(:,:,:,2);
        lam3 = hessian_eigs_wrapper.RawImage(:,:,:,3); % biggest
    end

    term_1 = abs(lam2./lam3); % Ra
    alpha = 0.5;
    term_1 = 1 - exp((-term_1.^2)./(2*alpha^2));

    term_2 = abs(lam1)./sqrt(abs(lam2.*lam3)); % Rb
    beta = 0.5;
    term_2 = exp((-term_2.^2)./(2*beta^2));

    term_3 = sqrt(lam1.^2 + lam2.^2 + lam3.^2); % S

    % Frangi et al. choose a noise threshold based on the Hessian mean.
    % However, we find a fixed experimentally-chosen threshold works better, but
    % this must be scaled by the voxel size
    multiple_matrix = voxel_size'*voxel_size;
    c_scaling = 1/mean(multiple_matrix(:));
    c = 400*c_scaling;

    term_3 = 1 - exp((-term_3.^2)./(2*c^2));

    check_signs = (lam2 <= 0) & (lam3 <= 0);

    vesselness_wrapper.RawImage = term_1.*term_2.*term_3.*check_signs;
end


function Wmat = bst_shepards(destLoc, srcLoc, nbNeighbors, excludeParam, expDistance)
% BST_SHEPARDS: 3D nearest-neighbor interpolation using Shepard's weighting.
%
% USAGE:  Wmat = bst_shepards(destLoc, srcLoc, nbNeighbors=8, excludeParam=0, expDistance=2)
%
% INPUT:
%    - srcLoc       : Nx3 array of original locations, or tesselation structure (Faces,Vertices,VertConn)
%    - destLoc      : NNx3 array of locations onto original data will be interpolated, or tesselation structure (Faces,Vertices,VertConn)
%    - nbNeighbors  : Number of nearest neighbors to be considered in the interpolation (default is 8)
%    - excludeParam : If > 0, the source points that are two far away from the destination surface are ignored.
%                     Excluded points #i that have: (minDist(i) > mean(minDist) + excludeParam * std(minDist))
%                     where minDist represents the minimal distance between each source point and the destination surface
%                     If < 0, exclude the vertices that are further from the absolute distance excludeParam  (in millimeters)
%    - expDistance  : Distance exponent (if higher, influence of a value decreases faster)
%
% OUTPUT:
%    - Wmat : Interpolation matrix

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2010-2017

%% ===== PARSE INPUTS =====
% Check number of arguments
if (nargin < 2)
    error('Usage: Wmat = bst_shepards(destLoc, srcLoc, nbNeighbors, excludeParam, expDistance)');
end
% Check matrices orientation
if ((size(destLoc, 2) ~= 3) || (size(srcLoc, 2) ~= 3)) && ((size(destLoc, 2) ~= 2) || (size(srcLoc, 2) ~= 2))
    error('destLoc and srcLoc must have 2 or 3 columns.');
end
% Argument: Number of neighbors for interpolation
if (nargin < 3) || isempty(nbNeighbors)
    nbNeighbors = 8;
end
% Argument: excludeParam
if (nargin < 4) || isempty(excludeParam)
    excludeParam = 0;
end
% Argument: expDistance
if (nargin < 5) || isempty(expDistance)
    expDistance = 2;
end

%% ===== SHEPARDS INTERPOLATION =====
% Allocate interpolation matrix
nDest = size(destLoc,1);
nSrc  = size(srcLoc,1);
% Maximum number of neighbors = number of electrodes
if (nbNeighbors > nSrc)
    nbNeighbors = nSrc;
end

% Find nearest neighbors
[I,dist] = bst_nearest(srcLoc, destLoc, nbNeighbors, 1);
% Square the distance matrix
dist = dist .^ 2;
% Eliminate zeros in distance matrix for stability
dist(dist == 0) = eps;

% One neighbor
if (nbNeighbors == 1)
    Wmat = sparse(1:nDest, I(:)', ones(1,nDest), nDest, nSrc);

% More complicated cases
elseif (nbNeighbors > 1)
    % Interpolation weights from Shepards method
    W = (bst_bsxfun(@minus, dist(:,nbNeighbors), dist) ./ bst_bsxfun(@times, dist(:,nbNeighbors), dist)) .^ expDistance;
    sumW = sum(W(:,1:nbNeighbors-1),2);
    % Correct zero values: points overlap exactly => take only the first point
    iZeroW = find(sumW == 0);
    if ~isempty(iZeroW)
        sumW(iZeroW) = 1;
        W(iZeroW, 1:nbNeighbors-1) = ones(length(iZeroW),1) * [1,zeros(1,nbNeighbors-2)];
    end
    W = W(:,1:nbNeighbors-1) ./ (sumW * ones(1,nbNeighbors-1));
    % Create sparse matrix with those weights
    i = repmat((1:nDest)', nbNeighbors-1, 1);
    j = reshape(I(:, 1:nbNeighbors-1), [], 1);
    Wmat = sparse(i, j, W(:), nDest, nSrc);
end

%% ===== IGNORE VERTICES TOO FAR AWAY =====
% Set to zero the weights of the vertices that are too far away from the sources
% EEG: Distance relative to the mean distance between sensors
if (excludeParam > 0)
    % Find vertices that are too far from their nearest neighbors
    iTooFarVertices = (dist(:,1) > mean(dist(:,1)) + excludeParam * std(dist(:,1)));
    % Remove them from the interpolation matrix
    Wmat(iTooFarVertices, :) = 0;
% SEEG/ECOG: Absolute distance
elseif (excludeParam < 0)
    % Find vertices that are too far from their nearest neighbors (in millimeters)
    iTooFarVertices = (sqrt(dist(:,1)) >  abs(excludeParam));
    % Remove them from the interpolation matrix
    Wmat(iTooFarVertices, :) = 0;
end


function [AF,BF]=framebounds(F,varargin);
%FRAMEBOUNDS  Frame bounds
%   Usage: fcond=framebounds(F);
%          [A,B]=framebounds(F);
%          [...]=framebounds(F,Ls);
%
%   `framebounds(F)` calculates the ratio $B/A$ of the frame bounds of the
%   frame given by *F*. The length of the system the frame bounds are
%   calculated for is given by `L=framelength(F,1)`.
%
%   `framebounds(F,Ls)` additionally specifies a signal length for which
%   the frame should work. The actual length used is `L=framelength(F,Ls)`.
%
%   `[A,B]=framebounds(F)` returns the frame bounds *A* and *B* instead of
%   just their ratio.
%
%
%   'framebounds` accepts the following optional parameters:
%
%     'fac'        Use a factorization algorithm. The function will throw
%                  an error if no algorithm is available.
%
%     'iter'       Call `eigs` to use an iterative algorithm.
%
%     'full'       Call `eig` to solve the full problem.
%
%     'auto'       Choose the `fac` method if possible, otherwise
%                  use the `full` method for small problems and the
%                  `iter` method for larger problems.
%                  This is the default.
%
%     'crossover',c
%                  Set the problem size for which the 'auto' method
%                  switches between `full` and `iter`. Default is 200.
%
%   The following parameters specifically related to the `iter` method:
%
%     'tol',t      Stop if relative residual error of eighs is less than the
%                  specified tolerance. Default is 1e-9
%
%     'maxit',n    Do at most n iterations in eigs. Default is 100.
%
%     'pcgtol',t   Stop if relative residual error of pcg is less than the
%                  specified tolerance. Default is 1e-6
%
%     'pcgmaxit',n Do at most n iterations in pcg. Default is 150.
%
%     'p',p        The number of Lanzcos basis vectors to use.  More vectors
%                  will result in faster convergence, but a larger amount of
%                  memory.  The optimal value of `p` is problem dependent and
%                  should be less than *L*.  The default value chosen
%                  automatically by eigs.
%
%     'print'      Display the progress.
%
%     'quiet'      Don't print anything, this is the default.
%
%   See also: frame, framered

complainif_notenoughargs(nargin,1,'FRAMEBOUNDS');
complainif_notvalidframeobj(F,'FRAMEBOUNDS');

% We handle the container frames first
  if strcmp(F.type,'fusion')
      AF=0;
      BF=0;
      for ii=1:F.Nframes
          [A,B]=framebounds(F.frames{ii},varargin{:});
          AF=AF+(A*F.w(ii)).^2;
          BF=BF+(B*F.w(ii)).^2;
      end;
      AF=sqrt(AF);
      BF=sqrt(BF);

      return;
  end;

  if strcmp(F.type,'tensor')
    AF=1;
    BF=1;
    for ii=1:F.Nframes
      [A,B]=framebounds(F.frames{ii},varargin{:});
      AF=AF*A;
      BF=BF*B;
    end;

    return;
  end;

  definput.keyvals.Ls=1;
  definput.keyvals.maxit=100;
  definput.keyvals.tol=1e-9;
  definput.keyvals.pcgmaxit=150;
  definput.keyvals.pcgtol=1e-6;
  definput.keyvals.crossover=200;
  definput.keyvals.p=[];
  definput.flags.print={'quiet','print'};
  definput.flags.method={'auto','fac','iter','full'};

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

  F=frameaccel(F,kv.Ls);
  L=F.L;

  % Default values, works for the pure frequency transforms.
  AF=1;
  BF=1;

  % Simple heuristic: If F.g is defined, the frame uses windows.
  if isfield(F,'g')
      if isempty(F.g)
          error('%s: No analysis frame is defined.', upper(mfilename));
      end;
      g=F.g;
      op    = @frana;
      opadj = @frsyn;
  end;

  F_isfac = isfield(F,'isfac') && F.isfac;

  if flags.do_fac && ~F_isfac
    error('%s: The type of frame has no factorization algorithm.',upper(mfilename));
  end;

  if (flags.do_auto && F_isfac) || flags.do_fac
    switch(F.type)
     case 'gen'
      V=svd(g);
      AF=min(V)^2;
      BF=max(V)^2;
     case {'dgt','dgtreal'}
      [AF,BF]=gabframebounds(g,F.a,F.M,L);
     case {'dwilt','wmdct'}
      [AF,BF]=wilbounds(g,F.M,L);
     case {'filterbank','ufilterbank'}
      [AF,BF]=filterbankbounds(g,F.a,L);
     case {'filterbankreal','ufilterbankreal'}
      [AF,BF]=filterbankrealbounds(g,F.a,L);
     case 'fwt'
      [AF,BF]=wfbtbounds({g,F.J,'dwt'},L);
     case 'wfbt'
      [AF,BF]=wfbtbounds(g,L);
     case 'ufwt'
      [AF,BF]=wfbtbounds({g,F.J,'dwt'},L,F.flags.scaling);
     case 'uwfbt'
      [AF,BF]=wfbtbounds(g,L,F.flags.scaling);
     case 'wpfbt'
      [AF,BF]=wpfbtbounds(g,L,F.flags.interscaling);
     case 'uwpfbt'
      [AF,BF]=wpfbtbounds(g,L,F.flags.interscaling,F.flags.scaling);
    end;
  end;

  if (flags.do_auto && ~F_isfac && F.L>kv.crossover) || flags.do_iter

    if flags.do_print
      opts.disp=1;
    else
      opts.disp=0;
    end;
    opts.isreal = F.realinput;
    opts.maxit  = kv.maxit;
    opts.tol    = kv.tol;
    opts.issym  = 0;
    if ~isempty(kv.p)
       opts.p      = kv.p;
    end

    pcgopts.maxit = kv.pcgmaxit;
    pcgopts.tol = kv.pcgtol;

    % Upper frame bound
    frameop = @(x) F.frsyn(F.frana(x));
    BF = real(eigs(frameop,L,1,'LM',opts));

    % Lower frame bound
    frameop2 = @(x) F.frsyn(F.frana(x));
    invfrop = @(x) pcgwrapper(frameop2,x,pcgopts.tol,pcgopts.maxit);

    % Test convergence of pcg
    test = randn(L,1);
    if ~F.realinput, test = test +1i*randn(L,1); end
    [~,flag] = invfrop(test);

    % If PCG converges, estimate the smallest eigenvalue, otherwise assume
    % AF = 0;
    if ~flag
        AF = real(eigs(invfrop,L,1,'SM',opts));
    else
        AF = 0;
    end

  end;

  if (flags.do_auto && ~F_isfac && F.L<=kv.crossover) || flags.do_full
    % Compute thee transform matrix.
    bigM=opadj(F,op(F,eye(L)));

    D=eig(bigM);

    % Clean the eigenvalues, we know they are real
    D=real(D);
    AF=min(D);
    BF=max(D);
  end;

  if nargout<2
    % Avoid the potential warning about division by zero.
    if AF==0
      AF=Inf;
    else
      AF=BF/AF;
    end;
  end;

end

% In order to mute pprinting out pcg progress
function [y,flag]=pcgwrapper(varargin)
[y,flag,dummyrelres,dummyiter] = pcg(varargin{:});
end

% The function has been written in this way, because Octave (at the time
% of writing) does not accept additional parameters at the end of the
% line of input arguments for eigs
function y=afun(x,F_in,op_in,opadj_in)
  persistent F;
  persistent op;
  persistent opadj;

  if nargin>1
    F     = F_in;
    op    = op_in;
    opadj = opadj_in;
  else
    y=opadj(F,op(F,x));
  end;

end


 function [err, sn, kernel] = nufft1_error(om, N1, J1, K1, kernel, sn)
%function [err, sn, kernel] = nufft1_error(om, N1, J1, K1, kernel, sn)
%
% Compute worst-case error for each input frequency for 1D NUFFT
% using specified (inline) `ad hoc' kernel function (e.g., gaussian).
% This is worst-case for a unit-norm signal of length N1.
% in
%	om	[M,1]	digital frequency omega in radians
%	N1		signal length
%	J1		# of neighbors used per frequency location
%	K1		FFT size (should be > N1)
%	kernel		inline kernel function, args (k,J)
%				(or choose from built-in examples - see code!)
%	sn		optional scaling factors (otherwise do-no-harm)
% out
%	err	[M,1]	worst-case error over unit-norm signals
%	sn	[N,1]	scaling factors
%
% examples for kernel:
% linear:		inline('(1 - abs(k/(J/2))) .* (abs(k) < J/2)', 'k', 'J')
% truncated diric:	inline('sinc(k) .* (abs(k) < J/2)', 'k', 'J')
%
% Copyright 2001-12-7, Jeff Fessler, The University of Michigan

% if no arguments, give an example, comparing triangular kernel to min-max
if nargin < 4
	help(mfilename)
	N = 2^7; K = 2*N; gam = 2*pi/K;
	Jlist = [2:10]';
	om = gam * linspace(0,1,101);

	err.linear	= zeros(size(Jlist));
	err.minmaxu	= zeros(size(Jlist));
	err.minmax2	= zeros(size(Jlist));
	err.minmaxo	= zeros(size(Jlist));
	err.minmaxk	= zeros(size(Jlist));	% kaiser sn's
	err.gauss_zn	= zeros(size(Jlist));
	err.gauss_ft	= zeros(size(Jlist));
	err.kaiser	= zeros(size(Jlist));

	for ii=1:length(Jlist)
		J = Jlist(ii);
		printf('J=%d', J)
		err.linear(ii) = ...
			max(nufft1_error(om, N, J, K, 'linear'));
		err.minmaxu(ii) = ...
			max(nufft1_error(om, N, J, K, 'minmax,uniform'));
		err.minmax2(ii) = ...
			max(nufft1_error(om, N, J, K, 'minmax,best,L=2'));
		err.minmaxo(ii) = ...
			max(nufft1_error(om, N, J, K, 'minmax,best'));
		err.gauss_zn(ii) = ...
			max(nufft1_error(om, N, J, K, 'gauss'));
		err.gauss_ft(ii) = ...
			max(nufft1_error(om, N, J, K, 'gauss', 'ft'));
		[tmp, sn] = nufft1_error(om, N, J, K, 'kaiser', 'ft');
		err.kaiser(ii) = max(tmp);
		err.minmaxk(ii) = ...
			max(nufft1_err_mm(om, N, J, K, 'qr', sn));
	end
	clf, semilogy(Jlist, err.linear, 'g-x', ...
		Jlist, err.minmaxu, 'c-+', ...
		Jlist, err.gauss_zn, 'b-*', ...
		Jlist, err.gauss_ft, 'b-o', ...
		Jlist, err.minmax2, 'r-^', ...
		Jlist, err.kaiser, 'm->', ...
		Jlist, err.minmaxo, 'y-<', ...
		Jlist, err.minmaxk, 'w-o'), axis tight
	xlabel J, ylabel 'worst-case error'
	legend('linear', 'min-max, uniform', ...
		'gaussian (zn)', 'gaussian (FT)', ...
		'min-max, best L=2', 'kaiser', 'min-max, optimized', ...
		'min-max, kaiser s', 3)
	clear err
return
end

%
% kernel selection
%
if ischar(kernel)

	% min-max interpolators (uniform, best, etc., see nufft1_err_mm.m)
	if strncmp(kernel, 'minmax,', 7)
		type = kernel(8:end);	% uniform or best or best,L=2 etc.
		[err, sn] = nufft1_err_mm(om, N1, J1, K1, 'qr', type);
		return

	% cos^3-tapered dirichlet
	elseif streq(kernel, 'cos3diric')
		kernel = 'diric(2*pi*k/J, J) .* cos((2*pi*k/J)/2).^3';

	% Dirichlet (truncated)
	elseif streq(kernel, 'diric')
		kernel = 'nufft_diric(k,%d,%d,1) .* (abs(k) < J/2)';
		kernel = sprintf(kernel, N1, N1);

	% gaussian (truncated) with previously numerically-optimized width
	elseif streq(kernel, 'gauss')
		if isvar('sn') & ischar(sn) & streq(sn, 'ft')
			stype = 'ft';
		else
			stype = 'zn';
		end
		[dummy, kernel, kernel_ft] = nufft_best_gauss(J1, K1/N1, stype);
		kernel_ft = inline(kernel_ft, 't');

	% kaiser-bessel with previously numerically-optimized shape
	elseif streq(kernel, 'kaiser')
		[kernel, kb_a, kb_m] = kaiser_bessel('string', J1, 'best', 0, K1/N1);
		kernel_ft = kaiser_bessel_ft('inline', J1, kb_a, kb_m, 1);

	% linear interpolation via triangular function (possibly "wide"!)
	elseif streq(kernel, 'linear')
		kernel = '(1 - abs(k/(J/2))) .* (abs(k) < J/2)';

	else
		error(sprintf('unknown kernel "%s"', kernel))
	end

	kernel = inline(kernel, 'k', 'J');

elseif ~streq('inline', class(kernel))
	error 'need inline kernel'
end

gam = 2*pi/K1;

if 0
	%	plot interpolator
	k = linspace(-J/2-1,J/2+1,101);
	clf, subplot(221), plot(k, kernel(k, J))
	xlabel k, ylabel kernel(k), axis tight, grid
end

%
% Compute scaling factors using the "do no harm" strategy.
% This may not be optimal; analytical FT could also be reasonable.
%
if ~isvar('sn')
	sn = 1 ./ nufft_interp_zn(0, N1, J1, K1, kernel);	% [N]
%	sn = 1 ./ mean(nufft_interp_zn([0 1/2], N1, J1, K1, kernel), 2); % alt

elseif isa(sn, 'inline')
	n = [0:(N1-1)]'-(N1-1)/2;
	sn = 1 ./ sn(n/K1);		% [N]

% trick to use Gaussian FT scaling factors
elseif ischar(sn) & streq(sn, 'ft') & isvar('kernel_ft')
	n = [0:(N1-1)]'-(N1-1)/2;
	sn = 1 ./ kernel_ft(n/K1);		% [N]
	if 0 & J1 > 2
		sn_zn = 1 ./ nufft_interp_zn(0, N1, J1, K1, kernel);	% [N]
		clf, plot(n, [sn reale(sn_zn)])
		keyboard
	end

% trick to use Kaiser-Bessel FT scaling factors
elseif isstruct(sn) & streq(sn.type, 'kaiser')
	n = [0:(N1-1)]'-(N1-1)/2;
	sn = 1 ./ kaiser_bessel_ft(n/K1, J1, sn.alpha, sn.m, 1);

else
	error 'unsupport scaling factors type'
end

%
% interpolator worst-case error for each frequency (scaled by 1/sqrt(N)),
% from equations (46)-(47) in Fessler&Sutton NUFFT paper, T-SP, Feb. 2003
%
zn = nufft_interp_zn(om/gam, N1, J1, K1, kernel);		% [N,M]
err = sqrt(mean(abs(spdiag(sn) * zn - 1).^2, 1))';	% [M]


% SOLVE_MAXWELL_EIG_MIXED1: Solve the Maxwell eigenvalue problem with a mixed formulation, and a B-spline discretization.
%
% Example to solve the problem
%
%    curl (1/mu(x) curl (u)) = lambda (epsilon(x) u)   in Omega = F((0,1)^n)
%          div (epsilon(x) u) = 0                       in Omega
%       (1/mu(x) curl(u)) x n = 0                       on Gamma_N
%                       u x n = 0                       on Gamma_D
%
% with the variational mixed formulation
%
%    \int (1/mu(x) curl(u) curl(v)) + \int (epsilon(x) v grad(p))
%                = lambda \int (epsilon(x) u v),   \forall v \in H_0(curl),
%                                     \int (epsilon(x) u grad(q)) = 0,
%                                                  \forall q \in H^1_0.
%
% USAGE:
%
%  [geometry, msh, space, sp_mul, eigv, eigf] =
%                  solve_maxwell_eig_mixed1 (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - nmnn_sides:   sides with Neumann boundary condition (may be empty)
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - c_elec_perm:  electric permittivity (epsilon in the equation)
%    - c_magn_perm:  magnetic permeability (mu in the equation)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:     degree of the spline functions.
%    - regularity: continuity of the spline functions.
%    - nsub:       number of subelements with respect to the geometry mesh
%                   (nsub=1 leaves the mesh unchanged)
%    - nquad:      number of points for Gaussian quadrature rule
%
% OUTPUT:
%
%  geometry: geometry structure (see geo_load)
%  msh:      mesh object that defines the quadrature rule (see msh_cartesian)
%  space:    space object that defines the discrete functions (see sp_vector)
%  sp_mul:   space object for the multiplier (see sp_scalar)
%  eigv:     the computed eigenvalues
%  eigf:     degrees of freedom of the associated eigenfunctions
%
% See also EX_MAXWELL_EIG_MIXED1_SQUARE for an example
%
% Copyright (C) 2010, 2011, 2015 Rafael Vazquez
%
%    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 3 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, see <http://www.gnu.org/licenses/>.

function [geometry, msh, space, sp_mul, eigv, eigf] = ...
              solve_maxwell_eig_mixed1 (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

% Construct geometry structure
geometry = geo_load (geo_name);

[knots, zeta] = kntrefine (geometry.nurbs.knots, nsub-1, degree, regularity);
[knots_hcurl, degree_hcurl] = knt_derham (knots, degree, 'Hcurl');

% Construct msh structure
rule     = msh_gauss_nodes (nquad);
[qn, qw] = msh_set_quad_nodes (zeta, rule);
msh      = msh_cartesian (zeta, qn, qw, geometry);

% Construct the space structures for the field and the Lagrange multiplier
scalar_spaces = cell (msh.ndim, 1);
for idim = 1:msh.ndim
  scalar_spaces{idim} = sp_bspline (knots_hcurl{idim}, degree_hcurl{idim}, msh);
end
space = sp_vector (scalar_spaces, msh, 'curl-preserving');
sp_mul = sp_bspline (knots, degree, msh);

% Assemble the matrices
if (msh.rdim == 2)
  invmu = @(x,y) 1./c_magn_perm (x,y);
elseif (msh.rdim == 3)
  invmu = @(x,y,z) 1./c_magn_perm (x,y,z);
end

stiff_mat = op_curlu_curlv_tp (space, space, msh, invmu);
mass_mat  = op_u_v_tp (space, space, msh, c_elec_perm);
saddle_mat = op_v_gradp_tp (space, sp_mul, msh, c_elec_perm);

% Apply homogeneous Dirichlet boundary conditions
drchlt_dofs = []; drchlt_dofs_mul = [];
for iside = 1:numel (drchlt_sides)
  drchlt_dofs = union (drchlt_dofs, space.boundary(drchlt_sides(iside)).dofs);
  drchlt_dofs_mul = union (drchlt_dofs_mul, sp_mul.boundary(drchlt_sides(iside)).dofs);
end
if (isempty (drchlt_dofs_mul))
  drchlt_dofs_mul = sp_mul.ndof;
end

int_dofs = setdiff (1:space.ndof, drchlt_dofs);
int_dofs_mul = setdiff (1:sp_mul.ndof, drchlt_dofs_mul);

% Solve the eigenvalue problem
stiff_mat  = stiff_mat (int_dofs, int_dofs);
mass_mat   = mass_mat (int_dofs, int_dofs);
saddle_mat = saddle_mat (int_dofs_mul, int_dofs);

A = [stiff_mat, saddle_mat.'; ...
     saddle_mat, sparse(numel(int_dofs_mul),numel(int_dofs_mul))];
M = [mass_mat, sparse(numel(int_dofs), numel(int_dofs_mul)); ...
     sparse(numel(int_dofs_mul), numel(int_dofs)+numel(int_dofs_mul))];

eigf = zeros (space.ndof + sp_mul.ndof, numel(int_dofs) + numel(int_dofs_mul));
[eigf([int_dofs space.ndof+int_dofs_mul], :), eigv] = eig (full(A), full(M));
eigv = diag (eigv);

end

%!demo
%! ex_maxwell_eig_mixed1_square

%!demo
%! ex_maxwell_eig_mixed1_Lshaped

%!demo
%! ex_maxwell_eig_mixed1_cube

%!demo
%! ex_maxwell_eig_mixed1_thick_L