function [datout, tim, Fnew] = ft_preproc_resample(dat, Fold, Fnew, method)

% FT_PREPROC_RESAMPLE resamples all channels in the data matrix
%
% Use as
%   dat = ft_preproc_resample(dat, Fold, Fnew, method)
% where
%   dat    = matrix with the input data (Nchans X Nsamples)
%   Fold   = scalar, original sampling frequency in Hz
%   Fnew   = scalar, desired sampling frequency in Hz
%   method = string, can be 'resample', 'decimate', 'downsample', 'fft'
%
% The resample method applies an anti-aliasing (lowpass) FIR filter to
% the data during the resampling process, and compensates for the filter's
% delay. For the other two methods you should apply an anti-aliassing
% filter prior to calling this function.
%
% If the data contains NaNs, these are ignored for the computation, but
% retained in the output.
%
% See also PREPROC, FT_PREPROC_LOWPASSFILTER

% Copyright (C) 2006-2012, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

[nchans, nsamples] = size(dat);

if nargout>1
  tim = 1:nsamples;
  tim = ft_preproc_resample(tim, Fold, Fnew, method);
end

if Fold==Fnew
  ft_warning('requested sampling rate after resampling is the same as the sampling rate of the data, no resampling is performed');
  datout = dat;
  return
end

% resample and decimate require double formatted input
if ~strcmp(method, 'downsample')
  typ = class(dat);
  dat = cast(dat, 'double');
end

% preprocessing fails on channels that contain NaN
nanchan = any(isnan(dat),2);
if any(nanchan)
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
  if ft_platform_supports('matlabversion', '2020a', inf)
    % temporarily replace with zero, this is not needed for older MATLAB versions
    dat(nanchan,:) = 0;
  end
end

switch method
  case 'resample'
    [fold, fnew] = rat(Fold./Fnew); %account for non-integer fs
    Fnew         = Fold.*(fnew./fold); %get new fs exact

    % the actual implementation resamples along columns
    datout = resample(dat', fnew, fold)';

  case 'decimate'
    fac         = round(Fold/Fnew);
    % this only works one channel at the time
    nresampled  = ceil(nsamples/fac);
    datout      = zeros(nchans, nresampled);
    for i=1:nchans
      datout(i,:) = decimate(dat(i,:), fac);
    end

  case 'downsample'
    fac = round(Fold/Fnew);
    % the actual implementation resamples along columns
    datout = downsample(dat', fac)';

  case 'fft'
    % Code written for SPM by Jean Daunizeau
    fac         = Fnew/Fold;
    nresampled  = floor(nsamples*fac);
    fac         = nresampled/nsamples;
    datfft      = fftshift(fft(dat,[],2),2);
    middle = floor(size(datfft,2)./2)+1;
    if fac>1 % upsample
      npad = floor((nresampled-nsamples)./2);

      if nsamples/2 == floor(nsamples/2)
        datfft(:,1) = []; % throw away non symmetric DFT coef
      end

      datfft  = [zeros(size(datfft,1),npad), datfft,zeros(size(datfft,1),npad)];
    else % downsample
      ncut    = floor(nresampled./2);
      datfft  = datfft(:,middle-ncut:middle+ncut);
    end
    datout      = fac*ifft(ifftshift(datfft,2),[],2);
  otherwise
    ft_error('unsupported resampling method');
end

if any(nanchan) && ft_platform_supports('matlabversion', '2020a', inf)
  % replace the zeros back to nan, this is not needed for older MATLAB versions
  datout(nanchan,:) = nan;
end

if ~strcmp(method, 'downsample')
  % convert back into the original input format
  datout = cast(datout, typ);
end


% fixedtrx2mtrax(inmatname,outmatname)
% inmatname: name of a matfile created by fixerrorsgui
% outmatname: name of a matfile to save results to
% results will be saved in the format returned by mtrax
function fixedtrx2mtrax(inmatname,outmatname)

load(inmatname);
nflies = length(trx);
nframes = 0;
for fly = 1:nflies,
  nframes = max(nframes,trx(fly).endframe);
end
x_pos = nan(nflies,nframes);
y_pos = nan(nflies,nframes);
angle = nan(nflies,nframes);
maj_ax = nan(nflies,nframes);
min_ax = nan(nflies,nframes);
identity = repmat((1:nflies)',[1,nframes]);
for fly = 1:nflies,
  x_pos(fly,trx(fly).firstframe:trx(fly).endframe) = trx(fly).x;
  y_pos(fly,trx(fly).firstframe:trx(fly).endframe) = trx(fly).y;
  angle(fly,trx(fly).firstframe:trx(fly).endframe) = trx(fly).theta;
  maj_ax(fly,trx(fly).firstframe:trx(fly).endframe) = trx(fly).a;
  min_ax(fly,trx(fly).firstframe:trx(fly).endframe) = trx(fly).b;
end
keep = ~isnan(x_pos);
x_pos = x_pos(keep)'-1;
y_pos = y_pos(keep)'-1;
angle = angle(keep)';
maj_ax = maj_ax(keep)';
min_ax = min_ax(keep)';
identity = identity(keep)' - 1;
ntargets = sum(double(keep),1);

save(outmatname,'x_pos','y_pos','angle','maj_ax','min_ax','identity','ntargets');


%--- help for splanar.rediff ---
%
%  differentiate - differentiates vectors of splanar objects
%
%  ::
%
%
%    derivs=splanar.differentiate(eqtns,nwrt)
%    derivs=splanar.differentiate(eqtns,nwrt,order)
%    derivs=splanar.differentiate(eqtns,nwrt,order,verbose)
%
%  Args:
%
%     - **eqtns** [splanar|cell array]: vector or cell array of splanar objects
%
%     - **nwrt** [integer]: number of variables for which differentiation is
%       taken
%
%     - **order** [integer|{1}]: order of differentiation
%
%     - **alien_list** [empty|cellstr|char]: list of alien functions
%       (functions that RISE does not recognize and that are to be
%       differentiated by the user himself).
%
%     - **verbose** [true|{false}]: displays information about the process e.g.
%       the amount of time it takes to differentiate each order
%
%  Returns:
%     :
%
%     - **derivs** [structure]: each element in the structure contains:
%       - **size** [2-colum vector]: number of rows and number of columns of
%         the compacted derivatives i.e. unique columns
%       - **derivatives** [vector of splanar]: derivatives
%       - **nwrt** [integer]: number of variables for which differentiation is
%         taken
%       - **order** [integer]: order of differentiation of the current
%         structure
%       - **nnz_derivs** [integer]: number of non-zero derivatives
%       - **partitions** [vector]: vector that help reconstruct the
%         expanded/grand derivative matrix.
%       - **map** []: empty field that will be used in a later stage
%
%  Note:
%
%  Example:
%
%     See also:
%


function Avect   = sparsevec(blk,Acell)
%% Implement vec of a cell of sparse matrices
%% This is much faster than calling vec(Acell{i})
%% The generated Avect is typically used for CDCS

n           = blk{1,2};
m           = length(Acell);
nsq         = n^2;

ssi         = [];
ssj         = [];
ssdata      = [];
% fprintf('vec a cell of sparse matrices, progress ...')
for i = 1:m
    % if rem(i,10000) == 1
    %     fprintf('%d/%d ',i,m);
    % end
    Ai              = Acell{i};
    [ii,jj,val]     = find(Ai);

    si              = n * (jj-1) + ii;
    sj              = ones(length(si),1) * i;
    sdata           = val;

    ssi             = [ssi;si(:)];
    ssj             = [ssj;sj(:)];
    ssdata          = [ssdata;sdata(:)];
end
Avect               = sparse(ssi,ssj,ssdata,nsq,m);
% fprintf('Done.\n')
end


function h=m_ruler(posx,posy,varargin)
% M_RULER Draws a distance scalebar for a map
%   M_RULER([X1 X2],Y1) draws a horizontal scale bar between the
%   normalized coordinates (X1,Y1) and (X2,Y1) where both X/Y are
%   in the range 0 to 1.
%   M_RULER(X1,[Y1 Y2]) draws a vertical scalebar
%   M_RULER(...,NINTS) draws the scalebar with NINTS intervals
%   if NINTS is a scalar (default 4).  Distances of each interval are
%   chosen to be 'nice'. If NINTS is a vector it is understood to be
%   the distances to be used (in meters)
%
%   M_RULER(....,'parameter','value',...) lets you specify
%   extra parameter/value pairs in the usual handle-graphics way.
%   'color' and 'fontsize' are probably the most useful, 'tickdir'
%   'in' and 'out' chooses between different styles.
%
%   Probably BEST to call this AFTER M_GRID otherwise placement might
%   seem a bit odd.
%
% WARNING - the scalebar is probably not useful for any global
%           (i.e. whole-world) or even a significant-part-of-the-globe
%           map, but I won't stop you using it. Caveat user!

% R. Pawlowicz rich@eos.ubc.ca  8/Nov/2006
% 7/Dec/11 - Octave 3.2.3 compatibility
%   Nov/17 - some changes in look for new matlab graphics
%   Jan/18 - patch colours must be double not logical! (no longer the same)

global MAP_PROJECTION

if isempty(MAP_PROJECTION)
   disp('No Map Projection initialized - call M_PROJ first!');
   return;
end

nints=4;
fixticks=0;

if length(varargin)>0
 if isnumeric(varargin{1})
   nints=varargin{1};
   varargin=varargin(2:end);
   fixticks=1;
 end
end

gcolor='k';
glinestyle='-';
glinewidth=3;
gfontsize=get(gca,'fontsize');
gfontname=get(gca,'fontname');
gticklen=get(gca,'ticklength'); gticklen=gticklen(1);
gtickdir=get(gca,'tickdir');

if MAP_PROJECTION.newgraphics
 gticklen=gticklen*3;
end

% Parse parameter list for options. I really should do some
% error checking here, but...

k=1;
while k<=length(varargin)
  switch lower(varargin{k}(1:3))
    case 'col'
      gcolor=varargin{k+1};
    case 'lin'
      switch lower(varargin{k}(1:5))
         case 'linew'
           glinewidth=varargin{k+1};
         case 'lines'
           glinestyle=varargin{k+1};
      end
    case 'fon'
       switch lower(varargin{k}(1:5))
         case 'fonts'
           gfontsize=varargin{k+1};
         case 'fontn'
           gfontname=varargin{k+1};
       end
    case 'tic'
       switch lower(varargin{k}(1:5))
         case 'tickl'
           gticklen=varargin{k+1}(1);
         case 'tickd'
           gtickdir=varargin{k+1};
       end
    case {'get','usa'}
      disp('      ''ticklength'',value');
      disp('      ''tickdir'',( ''in'' | ''out'' )');
      disp('      ''color'',colorspec');
      disp('      ''linewidth'', value');
      disp('      ''linestyle'', ( linespec | ''none'' )');
      disp('      ''fontsize'',value');
      disp('      ''fontname'',name');
       return;
    case 'set'
      disp(['      ticklength = ' num2str(gticklen)]);
      disp(['      tickdir = ' gtickdir]);
      disp(['      color = ' gcolor]);
      disp(['      linewidth = ' num2str(glinewidth)]);
      disp(['      linestyle = ' glinestyle]);
      disp(['      fontsize = ' num2str(gfontsize)]);
      disp(['      fontname = ' gfontname]);
       return;
      otherwise
       disp([' Unknown option: ' varargin{k}]);
  end
  k=k+2;
end

% Need earth radius, in m.
erad=6378137; %m (from WGS-84)

if  ( length(posx)==2 && length(posy)==1)
  posy=[posy posy];
  horiz=1;
elseif  ( length(posx)==1 && length(posy)==2)
  posx=[posx posx];
  horiz=0;
end

xlm=get(gca,'xlim');
ylm=get(gca,'ylim');

% Get into screen coords
posx=xlm(1) + posx*diff(xlm);
posy=ylm(1) + posy*diff(ylm);

if diff(xlm)>10 % we are probably already in meters, i.e. UTM
   scfac=1;
else
  scfac=erad;
end

distance=(diff(posx)+diff(posy))*scfac;

niceints=[ 1       2               5         	     ...
           10      20      25      50       75       ...
	       100     200     250     500      750      ...
	       1000    2000    4000    5000              ...
	       10000   20000   25000   50000    75000    ...
	       100000  200000  250000  500000   750000   ...
	       1000000 2000000 2500000 50000000 7500000  ];

if length(nints)==1

  exactint=distance/(nints);
  [dun,I]=min(abs(niceints-exactint));

  if ~fixticks
    nints=fix(distance/niceints(I));
  end
  dist=[0:nints]*niceints(I);

else
  dist=(nints-nints(1));
  nints=length(dist)-1;
end

if max(log10(dist(2:end)))>=3
   numfac=1000;
   units=' km';
else
   numfac=1;
   units= ' m';
end

if horiz

  if strcmp(gtickdir,'in')

    line(posx(1)+[0 dist(end)/scfac],posy(1)+[0 0],'color',gcolor,'linewidth',glinewidth,'linestyle',glinestyle,...
	 'clipping','off','tag','m_ruler_x');

    XX=posx(1)+[dist;dist]/scfac;
    YY=posy(1)+diff(ylm)*gticklen*[-1;1]*ones(1,nints+1);
    if MAP_PROJECTION.IsOctave
      for k=1:size(XX,2)
        line(XX(:,k),YY(:,k),...
	 'color',gcolor,'linewidth',glinewidth/3,'linestyle',glinestyle,...
	 'clipping','off','tag','m_ruler_y');
      end
    else
      line(XX,YY,...
	 'color',gcolor,'linewidth',glinewidth/3,'linestyle',glinestyle,...
	 'clipping','off','tag','m_ruler_y');
    end
  else

    patch(posx(1)+[dist(1:end-1);dist(1:end-1);dist(2:end);dist(2:end)]/scfac,...
           posy(1)+diff(ylm)*gticklen*[-1;1;1;-1]*ones(1,nints),...
	   repmat(MAP_PROJECTION.LARGVAL  ,4,nints),...
           reshape(double(rem(rem(0:nints*3-1,nints),2)==0),1,nints,3),...
	 'linewidth',glinewidth/3,'linestyle',glinestyle,...
         'clipping','off','tag','m_ruler');

  end

  if nints>1
    for k=1:nints+1
      text(posx(1)+dist(k)/scfac,posy(1)-diff(ylm)*gticklen,num2str(dist(k)/numfac), ...
          'fontsize',gfontsize,'fontname',gfontname,...
         'verticalalignment','top','horizontalalignment','center','tag','m_ruler_label');
    end
  %  text(posx(1)+dist(nints+1)/scfac,posy(1)-diff(ylm)*gticklen*2,sprintf('%d km',dist(end)/numfac),...
  %        'fontsize',gfontsize,'fontname',gfontname,...
  %        'verticalalignment','top','horizontalalignment','center','tag','m_ruler_label');
    text(posx(1)+diff(posx)/2,posy(1)+diff(ylm)*gticklen,units,...
          'fontsize',gfontsize,'fontname',gfontname,...
          'verticalalignment','bottom','horizontalalignment','center','tag','m_ruler_label');
  else
    text(posx(1)+mean(dist)/scfac,posy(1)-diff(ylm)*gticklen*2,[num2str(dist(end)/numfac) units],...
          'fontsize',gfontsize,'fontname',gfontname,...
          'verticalalignment','top','horizontalalignment','center','tag','m_ruler_label');
  end

else

  if strcmp(gtickdir,'in')

    line(posx(1)+[0 0],posy(1)+[0 dist(end)/scfac],'color',gcolor,'linewidth',glinewidth,'linestyle',glinestyle,...
	 'clipping','off','tag','m_ruler_x');

    XX=posx(1)+diff(xlm)*gticklen*[-1;1]*ones(1,nints+1);
    YY=posy(1)+[dist;dist]/scfac;

    if MAP_PROJECTION.IsOctave
      for k=1:size(XX,2)
       line(XX(:,k),YY(:,k),...
	 'color',gcolor,'linewidth',glinewidth/3,'linestyle',glinestyle,...
	 'clipping','off','tag','m_ruler_y');
      end
    else
       line(XX,YY,...
	 'color',gcolor,'linewidth',glinewidth/3,'linestyle',glinestyle,...
	 'clipping','off','tag','m_ruler_y');
    end
  else

    patch(posx(1)+diff(xlm)*gticklen*[-1;1;1;-1]*ones(1,nints),...
          posy(1)+[dist(1:end-1);dist(1:end-1);dist(2:end);dist(2:end)]/scfac,...
	   repmat(MAP_PROJECTION.LARGVAL  ,4,nints),...
           reshape(double(rem(rem(0:nints*3-1,nints),2)==0),1,nints,3),...
	 'linewidth',glinewidth/3,'linestyle',glinestyle,...
         'clipping','off','tag','m_ruler');

  end

  if nints>1
    for k=1:nints
      text(posx(1)+diff(xlm)*gticklen*2,posy(1)+dist(k)/scfac,num2str(dist(k)/numfac), ...
          'fontsize',gfontsize,'fontname',gfontname,...
         'verticalalignment','middle','horizontalalignment','left','tag','m_ruler_label');
    end
    text(posx(1)+diff(xlm)*gticklen*2,posy(1)+dist(nints+1)/scfac,[num2str(dist(end)/numfac) units],...
          'fontsize',gfontsize,'fontname',gfontname,...
          'verticalalignment','middle','horizontalalignment','left','tag','m_ruler_label');
  else
    text(posx(1)+diff(xlm)*gticklen*2,posy(1)+mean(dist)/scfac,[num2str(dist(end)/numfac) units],...
          'fontsize',gfontsize,'fontname',gfontname,...
          'verticalalignment','middle','horizontalalignment','left','tag','m_ruler_label');
  end
end

if nargout==0
    clear h
end


function [f,g] = autoGrad(x,useComplex,funObj,varargin)
% [f,g] = autoGrad(x,useComplex,funObj,varargin)
%
% Numerically compute gradient of objective function from function values

p = length(x);
mu = 1e-150;

if useComplex % Use Complex Differentials
    diff = zeros(p,1);
    for j = 1:p
        e_j = zeros(p,1);
        e_j(j) = 1;
        diff(j,1) = funObj(x + mu*i*e_j,varargin{:});
    end

    f = mean(real(diff));
    g = imag(diff)/mu;
else % Use Finite Differencing
    f = funObj(x,varargin{:});
    mu = 2*sqrt(1e-12)*(1+norm(x))/norm(p);
    for j = 1:p
        e_j = zeros(p,1);
        e_j(j) = 1;
        diff(j,1) = funObj(x + mu*e_j,varargin{:});
    end
    g = (diff-f)/mu;
end

if 0 % DEBUG CODE
    [fReal gReal] = funObj(x,varargin{:});
    [fReal f]
    [gReal g]
    pause;
end


function ori = discreteSample(odf,npoints,varargin)
% draw a random sample
%
%

% preallocate orientations
q = quaternion.id(npoints,1);

% which component
if numel(odf.weights) == 1
  icmp = ones(size(q));
else
  icmp = discretesample(odf.weights,npoints);
end

% compute discrete sample for each component seperately
for ic = 1:length(odf.components)
  q(icmp == ic) = discreteSample(odf.components{ic},sum(icmp==ic),varargin{:});
end

% take random symmetrically equivalent samples
qcs = reshape(quaternion(odf.CS.properGroup),[],1);
qss = reshape(quaternion(odf.SS.properGroup),[],1);
ics = discretesample(length(qcs),npoints,1);
iss = discretesample(length(qss),npoints,1);

ori = orientation(qss(iss(:)) .* q .* qcs(ics(:)),odf.CS,odf.SS);

% the antipodal case
if  odf.antipodal
  ori.antipodal =true;
  doInv = 1==discretesample(2,npoints,1);
  ori(doInv)=inv(ori(doInv));
end


function [M0,M1,L,M2] = spm_mfa_bi_multi(S,C)
% Bilinear form for multiple Gibb's ensembles
% FORMAT [M0,M1,L,M2] = spm_mfa_bi_multi(S,C)
%--------------------------------------------------------------------------
% S(s)   - MFA system specification structure for s ensembles
% Required fields:
%     S(i).M:  [1x1 struct]    - dynamic model structure
%     S(i).J0: [n x n double]  - Jacobian
%     S(i).J1: {1 x M.m cell}  - dJ0/du
%     S(i).L : [l x n double]  - d<y>/dp
%     S(i).u:  [n x m double]  - probability modes
%     S(i).v:  [m x n double]  - v*u = 1
%     S(i).X:  [n x d double]  - evaluation points of state [d]-space
%     S(i).x:  {1 x d cell}    - range of state [d]-space
%     S(i).p0: [n x 1 sparse]  - expansion point
%
% C{s x s cell} - coupling cell = dP/d<y> (change in parameters of S(i).M with
%                                   mean outputs of S(j).M - [p x l double])
%
% M0  [ns + 1 x ns + 1double]  - 1st order Bilinear matrix dq/dt;
% M1  {M.m x s}                - 2nd order Bilinear matrix dM0/du
% M2  {s x s}                  - 2nd order Bilinear matrix dM0/dC
% L   [1s x ns + 1 double]     - output matrix          <y> = L*q;
%
% Transformed probability states:  q = [1; v*(p(X) - p0)];
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mfa_bi_multi.m 4936 2012-09-18 19:47:55Z karl $

% indices
%--------------------------------------------------------------------------
s     = length(S);                      % number of ensembles
m     = length(S(1).J1);                % number of inputs
I     = {(1:size(S(1).v,1)) + 1};       % ensemble indices
for i = 2:s
    I{i} = (1:size(S(i).v,1)) + I{i - 1}(end);
end
q     = I{s}(end);
M0    = sparse(q,q);
M1    = {};
L     = sparse(0,q);

% compute M0 = df/dx. M1 = d(df/dx)/du amd L = dy/dx
%--------------------------------------------------------------------------
for i = 1:s

    % Ist order bilinear operator M0
    %======================================================================
    M0(I{i},1)       = S(i).v*S(i).J0*S(i).p0;
    M0(I{i},I{i})    = S(i).v*S(i).J0*S(i).u;

    % 2nd order bilinear operators M1{i} - dM0/du
    %======================================================================
    for j = 1:m
        M            = sparse(q,q);
        M(I{i},1)    = S(i).v*S(i).J1{j}*S(i).p0;
        M(I{i},I{i}) = S(i).v*S(i).J1{j}*S(i).u;
        M1{m,i}      = M;
    end

    % output matrix <y>
    %======================================================================
    j         = (1:size(S(i).L,1)) + size(L,1);
    L(j,1)    = S(i).L*S(i).p0;
    L(j,I{i}) = S(i).L*S(i).u;

end

% return unless coupling matrices are required
%--------------------------------------------------------------------------
if nargout < 4
    return
end

% 2nd order bilinear operators M2{i} - dM0/dC
%==========================================================================
M2    = cell(s,s);
dP    = 1e-1;
for i = 1:s

    % target ensemble df/dP = dJ/dP*p0
    %----------------------------------------------------------------------
    M     = S(i).M;
    p     = length(M.pE);
    for k = 1:p
        M.pE      = S(i).M.pE;
        M.pE(k)   = M.pE(k) + dP;
        dJdP      = (spm_mfa(M,S(i).x) - S(i).J0)/dP;
        dfdP(:,k) = S(i).v*dJdP*S(i).p0;
    end

    for j = 1:s

        % source ensemble dP/dx = dP/d<y>*d<y>/dx = C*L
        %------------------------------------------------------------------
        dfdx      = sparse(q,q);
        if ~isempty(C{i,j})
            dPdx            = C{i,j}*S(j).L*S(j).u;
            dfdx(I{i},I{j}) = dfdP*dPdx;
        end

        % M2 = df/dx = df/dP*dP/dx
        %------------------------------------------------------------------
        M2{i,j} = dfdx;

    end
end


function [h,filtertype] = gsp_jtv_filter_array(G,g,filtertype)
%GSP_JTV_FILTER_ARRAY Convert ts/js filters to -array filters
%   Usage: [h,filtertype] = gsp_jtv_filter_array(G,g, filtertype)
%
%   Input parameters:
%         G          : Graph
%         g          : Cell array of time-vertex filters
%         filtertype : Filter domain (ts,js)
%   Output parameters:
%         h          : Cell array of graph filterbank
%         filtertype : Filter domain (ts-array,js-array)
%
%   Convert ts/js filters to -array filters
%

% Author :  Francesco Grassi, Nathanael Perraudin
% Date : September 2016

if ~iscell(g)
   g = {g};
end

if ~gsp_check_filtertype(filtertype,{'ts','js'})
    error('Invalid filtertype.');
end

T  = G.jtv.T;
Nf = numel(g);

switch filtertype
    case 'ts'
        v = gsp_jtv_ta(G);
    case 'js'
        v = gsp_jtv_fa(G);
end

h  = cell(Nf,T);
for n=1:Nf
    for ii = 1:T
        h{n,ii} = @(x) g{n}(x,v(ii));
    end
end

filtertype = [filtertype '-array'];

end


function [yesno,diagnostic] = isfeasible(OptimizationProblem,Options)
%FEASIBLE  Check feasibility by solving feasibility problem
%
%   YESNO = isfeasible(P[,Options])
%
%   Example
%
%    The following code creates an optimization problem, and then checks
%    feasibility
%
%    x = sdpvar(1);P = optproblem(x >= 0, x^2);isfeasible(P)

OptimizationProblem.Options.verbose = 0;
if nargin < 2
    diagnostics = solvesdp(OptimizationProblem.Constraints,[],OptimizationProblem.Options);
    yesno = diagnostics.problem ~=1;
else
    Options.verbose = 0;
    diagnostics = solvesdp(OptimizationProblem.Constraints,[],OptimizationProblem.Objective,Options);
    yesno = diagnostics.problem ~=1;
end


function varargout = barvalues(h,precision)
% BARVALUES  Display bar values ontop of bars in bar or histogram plot.
% SYNTAX:
% barvalues;                  - operates on currnet axes.
% barvalues(h);               - operates on h.
% barvalues(_,precision);     - additionaly, specifies the precision of
%                               displayed values. or format.
% t = barvalues(_,_);         - returns the handles to the value text objects.
%
% h - handle to axes or bar (operates on specified object only)
%     or figure (operates on all child axes).
%
% precision - Decimal precision to display (0-10),
%             or 'formatSpec' as in num2str. (default:'% .0f')
%
% t - handles to the text objects.
%
%   For more information about 'formatSpec':
%   See also NUM2STR.

%Author: Elimelech Schreiber, 11/2017
% ver 2.1    - updated 04/2018

t=[];

if nargin>1 && ~isempty(precision) % Parse precision
    if isnumeric(precision) && precision >=0 && precision <=10
        precision =['% .',int2str(precision),'f'];
    elseif ~ischar(precision) && ~isstring(precision)
        error('Precision format unsupported.');
    end
else
    precision ='% .0f';
end

if nargin<1 || isempty(h)   % parse h (handle)
    h =gca;
elseif isaType(h,'figure')
   B = findobj(h,'type','bar','-or','type','Histogram'); % apply to multiple axes in figure.
   for b =B'
           t = [t; {barvalues(b,precision)}]; % Return array of text objects
                                              % for each bar plot.
   end
    if nargout>0
        varargout{1}=t;
    end
    return;
end
if isaType(h,'axes')
    h  = findobj(h,'type','bar','-or','type','Histogram','-or','type','patch');
    if isempty(h)
        return; % silently. to support multiple axes in figure.
    end
end
h = h(isaType(h,'bar') | isaType(h,'patch') | isaType(h,'histogram'));
if isempty(h)
    error('Cannot find bar plot.');
end
if size(h,1)>size(h,2)
    h=h';
end
for hn = h
    axes(ancestor(hn,'axes')); % make intended axes curent.
    if isaType(hn,'histogram')
        t =[t;  histvalues(hn,precision)];
        continue;
    end
    if isfield(hn,'XOffset')&&~isempty(hn.XOffset)
        XOffset = hn.XOffset;
    else
        XOffset = 0;
    end
    if isfield(hn,'YOffset')&&~isempty(hn.YOffset)
        YOffset = hn.YOffset;
    else
        YOffset = 0;
    end
    xData = hn.XData +XOffset;
    yData = hn.YData +YOffset;
    if size(xData,1)==4
      xData=mean(xData);
      yData=yData(2,:);
    end
    t = [t;  text(xData,yData,...    %position
       arrayfun(@(x)num2str(x,precision),yData,'UniformOutput' ,false),...    %text to display
        'HorizontalAlignment','center','VerticalAlignment','bottom')];
end
if nargout>0
    varargout{1}=t;
end

function flag =isaType(h,type)
try
    flag =strcmpi(get(h, 'type'), type);
catch
    flag =false;
end

function flag = isfield(h,fld)
flag =true;
try
    get(h,fld);
catch
    flag =false;
end

function t =histvalues(h,precision)
   hn=h;
    axes(ancestor(hn,'axes')); % make intended axes curent.
%     if isfield(hn,'XOffset')&&~isempty(hn.XOffset), XOffset = hn.XOffset; else XOffset = 0; end
%     if isfield(hn,'YOffset')&&~isempty(hn.YOffset), YOffset = hn.YOffset; else YOffset = 0; end
    xData = (hn.BinEdges(1:end-1) + hn.BinEdges(2:end))/2; yData = hn.Values;

    t = text(xData,yData,...    %position
       arrayfun(@(x)num2str(x,precision),yData,'UniformOutput' ,false),...    %text to display
        'HorizontalAlignment','center','VerticalAlignment','bottom');


function a = r8vec_to_r8cb ( m, n, ml, mu, x )

%*****************************************************************************80
%
%% R8VEC_TO_R8CB copies an R8VEC into a R8CB matrix.
%
%  Discussion:
%
%    In C++ and FORTRAN, this routine is not really needed.  In MATLAB,
%    a data item carries its dimensionality implicitly, and so cannot be
%    regarded sometimes as a vector and sometimes as an array.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%
%    Input, real X((ML+MU+1)*N), the vector to be copied into the array.
%
%    Output, real A(ML+MU+1,N), the array.
%
  for j = 1 : n
    for i = 1 : ml + mu + 1

      if ( 1 <= i + j - mu - 1 & i + j - mu - 1 <= m )
        a(i,j) = x(i+(ml+mu+1)*(j-1));
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function label_vec  = convert_matlabel_to_vec(label_mat, num)
    label_vec = zeros(1, num);
    for i=1:num
        nonzero_idx = find(label_mat(:,i)>0);
        if length(nonzero_idx) > 1
            nonzero_idx = min(nonzero_idx);
        end
        label_vec(1, i) = nonzero_idx;
    end
end


function testLandmarks

FV.vertices= double(reshape(BFM.shapeMU,3,length(BFM.shapeMU)/3)');
FV.faces = BFM.faces;

figure;
subplot(1,3,1); plotLandmarks(FV.vertices*roty(-30),faces,idx);
subplot(1,3,2); plotLandmarks(FV.vertices,faces,idx)
subplot(1,3,3); plotLandmarks(FV.vertices*roty(30),faces,idx)
function plotLandmarks(vertices,faces,idx)
patch('Vertices',vertices,'Faces',faces,'FaceColor', [1 1 1], 'EdgeColor', 'none', 'FaceLighting', 'phong'); axis equal; axis off; light;
plot3(vertices(idx,1),vertices(idx,2),vertices(idx,3),'.');
text(vertices(idx,1),vertices(idx,2),vertices(idx,3)*2,cellstr(num2str((1:length(idx))')));


function test1 (nmat)
%TEST1 test for BTF
% Requires CSparse and UFget
% Example:
%   test1
% See also btf, maxtrans, strongcomp, dmperm, UFget,
%   test1, test2, test3, test4, test5.

% Copyright 2007, Timothy A. Davis, University of Florida

index = UFget ;
% f = find (index.sprank < min (index.nrows, index.ncols)) ;
f = 1:length (index.nrows) ;

% too much time:
skip = [1514 1297 1876 1301] ;

f = setdiff (f, skip) ;

[ignore i] = sort (index.nnz (f)) ;
f = f (i) ;

if (nargin < 1)
    nmat = 1000 ;
end
nmat = min (nmat, length (f)) ;
f = f (1:nmat) ;

T0 = zeros (nmat,1) ;
T1 = zeros (nmat,1) ;
Anz = zeros (nmat,1) ;
figure (1) ;
MN = zeros (nmat, 2) ;
Nzdiag = zeros (nmat,1) ;

% warmup
p = maxtrans (sparse (1)) ;         %#ok
p = cs_dmperm (sparse (1)) ;        %#ok
a = cs_transpose (sparse (1)) ;     %#ok

h = waitbar (0, 'BTF test 1 of 6') ;

try

    for k = 1:nmat

        Prob = UFget (f (k), index) ;
        A = Prob.A ;
        clear Prob
        t = 0 ;

        waitbar (k/nmat, h) ;

        r = full (sum (spones (A), 2)) ;
        c = full (sum (spones (A))) ;
        m2 = length (find (r > 0)) ;
        n2 = length (find (c > 0)) ;

        if (m2 < n2)
            tic
            A = cs_transpose (A) ;
            t = toc ;
        end

        Nzdiag (k) = nnz (diag (A)) ;

        [m n] = size (A) ;
        Anz (k) = nnz (A) ;
        MN (k,:) = [m n] ;

        tic
        q = maxtrans (A) ;
        t0 = toc ;
        s0 = sum (q > 0) ;
        T0 (k) = max (1e-9, t0) ;

        tic
        p = cs_dmperm (A) ;
        t1 = toc ;
        s1 = sum (p > 0) ;
        T1 (k) = max (1e-9, t1) ;

        fprintf (...
            '%4d maxtrans %10.6f %10.6f  cs_dmperm %10.6f m/n %8.2f', ...
            f(k), t, t0, t1, m/n) ;
        if (t1 ~= 0)
            fprintf (' rel: %8.4f', t0 / t1) ;
        end
        fprintf ('\n') ;
        if (s0 ~= s1)
            error ('!') ;
        end

        if (s0 == n & m == n)                                               %#ok
            B = A (:, q) ;
            subplot (2,2,1) ;
            cspy (B) ;
            if (nnz (diag (B)) ~= n)
                error ('?')
            end
            clear B
        else
            cspy (0) ;
        end

        maxnz = nnz (A) ;

        zfree  = find (MN (1:k,1) == MN (1:k,2) & Nzdiag (1:k) == MN(1:k,1)) ;
        square = find (MN (1:k,1) == MN (1:k,2) & Nzdiag (1:k) ~= MN(1:k,1)) ;
        tall   = find (MN (1:k,1) >  MN (1:k,2)) ;
        squat  = find (MN (1:k,1) <  MN (1:k,2)) ;

        subplot (2,2,2) ;
        loglog (Anz (square), T0 (square) ./ T1 (square), ...
            'o', [1 maxnz], [1 1], 'r-') ;
        title ('square') ;
        subplot (2,2,3) ;
        loglog (Anz (tall), T0 (tall) ./ T1 (tall), ...
            'o', [1 maxnz], [1 1], 'r-') ;
        title ('tall') ;
        subplot (2,2,4) ;
        title ('square, intially zero-free') ;
        loglog (Anz (zfree), T0 (zfree) ./ T1 (zfree), ...
            'o', [1 maxnz], [1 1], 'r-') ;
        title ('square, zero-free diag') ;

        drawnow

    end

catch
    % out-of-memory is OK, other errors are not
    disp (lasterr) ;
    if (isempty (strfind (lasterr, 'Out of memory')))
        error (lasterr) ;                                                   %#ok
    else
        fprintf ('test terminated early, but otherwise OK\n') ;
    end
end

close (h) ;


function [hd_record,hd_estimates]=panel3hd(beta_gibbs,D_record,strshocks_record,It,Bu,Yi,Xi,N,n,m,p,k,T,HDband)

% initiate the cells storing the results
hd_record={};
hd_estimates={};
% because historical decomposition has to be computed for ach unit, loop over units
for ii=1:N
% run the Gibbs sampler for historical decomposition
hd_record(:,:,ii)=bear.hdecomp(beta_gibbs(:,:,ii),D_record(:,:,ii),strshocks_record(:,:,ii),It,Bu,Yi(:,:,ii),Xi(:,:,ii),n,m,p,k,T);
% then obtain point esimates and credibility intervals
hd_estimates(:,:,ii)=bear.hdestimates(hd_record(:,:,ii),n,T,HDband);
end


function [out_val] = per_graph(in_vec)
% per_graph.m - draws PER graph
global numPkts PER_snr PER_exp PER_bitPayload;

% PER graph settings (actual values are user-selected (GUI))
%%%%% numPkts = 200;
%%%%% PER_snr = 29:33;

% Input parameters
Clk        = in_vec(1);
bitsPerBlk = in_vec(2:9);
bmode      = in_vec(10);
dbits      = in_vec(11:end);
txbits     = dbits(1:end/2);
rxbits     = dbits(end/2+1:end);

% Viterbi trace-back depth (hard-coded for now)
link_delay = 128; %%34;

% Reset PER graph data
if (Clk==0)  PER_exp = zeros(size(PER_snr));  end

% Count number of error packets
if (floor(Clk/numPkts)<length(PER_snr))
    idx = floor(Clk/numPkts)+1;
    % Compare tx, rx bits
    txb = txbits( 1 : PER_bitPayload-link_delay );
    rxb = rxbits( link_delay+1 : PER_bitPayload );

    if (1) % for PER
        if ~all(txb==rxb)  PER_exp(idx) = PER_exp(idx) + 1;  end
    else   % for BER
        PER_exp(idx) = PER_exp(idx) + sum(xor(txb,rxb))/(PER_bitPayload-link_delay);
    end

    % Display PER information
    disp( ['Pkt:' num2str(mod(Clk,numPkts)+1) ', Idx=' num2str(idx) ...
           ', PER_list: ' num2str(PER_exp)] );

    % save to file (every tenth packet)
    if (mod(Clk,10)==9)
        fid = fopen('..\per_results.txt','w');
        fprintf(fid, 'Pkt: %d, Idx=%d,', mod(Clk,numPkts)+1, idx);
        fprintf(fid, 'SNR_list: [%s]\n', num2str(PER_snr));
        fprintf(fid, 'Pkt: %d, Idx=%d,', mod(Clk,numPkts)+1, idx);
        fprintf(fid, 'PER_list: [%s]\n', num2str(PER_exp));
        fclose(fid);
    end
end

if (Clk==length(PER_snr)*numPkts)
    PER_fig = figure;
    % Draw PER Curves
    PER_exp = PER_exp/numPkts;
    semilogy(PER_snr, PER_exp, 'g');
    %axis([0 25 1/1000 1]);
    axis([0 50 1/1000 1]);

    % Label graph...
    title('PER Curve');
    xlabel('SNR (dB)');
    ylabel('PER');
    legend('Simulation Result');
    grid on;

    % Display end message
    disp('PER test complete.');
end

% Output percentage done
out_val = min([100 100*Clk/(length(PER_snr)*numPkts)]);


function Lmk = lmkJacobians(Lmk)

% LMKJACOBIANS Compute Jacobians for projection onto the manifold.
%   LMKJACOBIANS computes all the Jacobians of the landmarks in structure
%   array Lmk for the projection of the landmark errors into the landmark
%   manifolds. The computed Jacobians are stored in Lmk.state.M. Its
%   expression depends on the landmark type.
%
%   Landmark types are specifyed by Lmk.type. In the case of Euclidean
%   points 'eucPnt', details follow.
%
%   The landmark's position manifold is defined trivially, so that
%       dp = [dpx dpy dpz]'
%   and the composition is just a sum,
%       p+ = p + dp.
%   In such case, the Jacobian
%       M = d p+ / d dp
%   is the identity matrix. To save some computations, this function
%   returns just M = 1 (scalar 'one').
%
%   For details on the Jacobians of other landmark parametrizations, please
%   refer to the code by editing the file lmkJacobians.m.
%
%   See also FRMJACOBIANS.

% Copyright 2015-     Joan Sola @ IRI-UPC-CSIC.

for lmk = [Lmk([Lmk.used]).lmk]
    switch Lmk(lmk).type
        case 'eucPnt'
            Lmk(lmk).state.M = 1; % trivial Jac
        case 'hmgPnt'
            [~,~,H_dh] = composeHmgPnt(Lmk(lmk).state.x, zeros(3,1));
            Lmk(lmk).state.M = H_dh;
        otherwise
            error('??? Unknown landmark type ''%s'' or Jacobian not implemented.',Lmk.type)
    end
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


classdef MOMFEAII < ALGORITHM
% <multi> <real/integer/label/binary/permutation> <constrained/none> <multitask>
% Multi-objective multifactorial evolutionary algorithm II

%------------------------------- Reference --------------------------------
% K. K. Bali, A. Gupta, Y. Ong, and P. S. Tan, Cognizant multitasking in
% multiobjective multifactorial evolution: MO-MFEA-II, IEEE Transactions on
% Cybernetics, 2021, 51(4): 1784-1796.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Initialize population
            Population    = Problem.Initialization();
            SubPopulation = Divide(Population,length(Problem.SubD));

            %% Optimization
            while Algorithm.NotTerminated([SubPopulation{:}])
                RMP                  = learnRMP(Problem,SubPopulation);
                [SubPopulation,Rank] = Sort(Problem,SubPopulation);
                Population           = [SubPopulation{:}];
                ParentPool           = Population(TournamentSelection(2,length(Population),[Rank{:}]));
                SubOffspring         = CreateOff(Problem,ParentPool,SubPopulation,RMP);
                for i = 1 : length(Problem.SubD)
                    SubPopulation{i} = EnviSelect([SubPopulation{i},SubOffspring{i}],length(SubPopulation{i}));
                end
            end
        end
    end
end


function plotOptStates(model, nlp, gait, indices)

    if nargin < 5
        indices = 1:length(model.Joints);
    else
        if isempty(indices), return; end
    end

    joint_names = {model.Joints.Name};

    if length(gait)==1 && isa(nlp,'TrajectoryOptimization')

        t = gait.tspan;

        x_opt = gait.states.x;
        x_lb  = [nlp.OptVarTable.x.LowerBound];
        x_ub  = [nlp.OptVarTable.x.UpperBound];

        dx_opt = gait.states.dx;
        dx_lb  = [nlp.OptVarTable.dx.LowerBound];
        dx_ub  = [nlp.OptVarTable.dx.UpperBound];

        ddx_opt = gait.states.ddx;
        ddx_lb  = [nlp.OptVarTable.ddx.LowerBound];
        ddx_ub  = [nlp.OptVarTable.ddx.UpperBound];
    else
        cont_domain_idx = find(cellfun(@(x)isa(x,'ContinuousDynamics'),{nlp.Phase.Plant}));

        t = [];
        x_opt = []; x_lb = [];  x_ub = [];
        dx_opt = []; dx_lb = [];  dx_ub = [];
        ddx_opt = []; ddx_lb = [];  ddx_ub = [];

        for j=cont_domain_idx
            t = [t,gait(j).tspan];

            x_opt = [x_opt,gait(j).states.x];
            x_lb  = [x_lb,[nlp.Phase(j).OptVarTable.x.LowerBound]];
            x_ub  = [x_ub,[nlp.Phase(j).OptVarTable.x.UpperBound]];

            dx_opt = [dx_opt,gait(j).states.dx];
            dx_lb  = [dx_lb,[nlp.Phase(j).OptVarTable.dx.LowerBound]];
            dx_ub  = [dx_ub,[nlp.Phase(j).OptVarTable.dx.UpperBound]];

            ddx_opt = [ddx_opt,gait(j).states.ddx];
            ddx_lb  = [ddx_lb,[nlp.Phase(j).OptVarTable.ddx.LowerBound]];
            ddx_ub  = [ddx_ub,[nlp.Phase(j).OptVarTable.ddx.UpperBound]];
        end
    end

    ax = [];
    for i=indices
        f = figure;clf;
        set(f, 'WindowStyle', 'docked');
        ax = [ax, subplot(3, 1, 1)]; %#ok<*AGROW>
        hold on;
        plot(t, x_opt(i,:), 'b');
        plot(t, x_lb(i,:), 'r--');
        plot(t, x_ub(i,:), 'g--');

        title('Joint Displacement');
        legend('q', 'lb', 'ub');

        ax = [ax, subplot(3, 1, 2)];
        hold on;
        plot(t, dx_opt(i,:), 'b');
        plot(t, dx_lb(i,:), 'r--');
        plot(t, dx_ub(i,:), 'g--');
        title('Joint Velocity');
        legend('dq', 'lb', 'ub');

        ax = [ax, subplot(3, 1, 3)];
        hold on;
        plot(t, ddx_opt(i,:), 'b');
        plot(t, ddx_lb(i,:), 'r--');
        plot(t, ddx_ub(i,:), 'g--');

        title('Joint Acceleration');
        legend('ddq', 'lb', 'ub');

        f.Name = [joint_names{i},'_state'];
    end

    linkaxes(ax, 'x');

end


function plasma_matrix_test ( n )

%*****************************************************************************80
%
%% PLASMA_MATRIX_TEST tests PLASMA_MATRIX.
%
%  Discussion:
%
%    This program shows how a MATLAB sparse matrix (and possibly a right
%    hand side vector) can be stored into a Harwell-Boeing file,
%    and later retrieved.
%
%    Harwell-Boeing files are useful as a means of storing sparse matrices,
%    especially when data is created with one program and needed by another.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 January 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PLASMA_MATRIX_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Create a large sparse plasma matrix and right hand side.\n' );
  fprintf ( 1, '  Store the matrix and right hand side in a Harwell-Boeing file.\n' );
  fprintf ( 1, '  Then retrieve the information.\n' );

  plasma_matrix_test01 ( 5 );
  plasma_matrix_test01 ( 100 );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PLASMA_MATRIX_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function z = mldivide( x, y )

%   Disciplined convex/geomtric programming information for MLDIVIDE:
%      The MLDIVIDE operation X\Y can be employed with X as a scalar. In
%      that case, it is equivalent to the LDIVIDE operation X.\Y, and
%      must obey the same rules as outlined in the help for CVX/LDIVIDE.
%
%      When X is a matrix, the MRDIVIDE operation X\Y is equivalent to
%      inv(X)*Y for both DCP and DGP purposes. The inv() operation is
%      not supported for non-constant expressions, so X must be both
%      constant and nonsingular. The resulting matrix multiplication
%      must obey the same rules as outlined in the help for CVX/MTIMES.

sz = size( x );
if all( sz == 1 ),
    z = ldivide( x, y, '\' );
elseif length( sz ) > 2,
    cvx_throw( 'Inputs must be 2-D, or at least one input must be scalar.' );
elseif sz( 1 ) ~= sz( 2 ),
    cvx_throw( 'Non-square matrix divisors are not supported in CVX.' );
elseif ~cvx_isconstant( x ),
    cvx_throw( 'Matrix divisors must be constant.' );
else
    z = mtimes( cvx_constant( x ), y, '\' );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function D = spm_eeg_erp_correction(S)
% Applies corrections to ERPs or single trials as in DCM-ERP
% This can be used to make a sensor level analysis or source reconstruction
% consistent with DCM.
% FORMAT D = spm_eeg_erp_correction(S)
%
% S        - optional input struct
% (optional) fields of S:
% S.D        - MEEG object or filename of M/EEG mat-file with epoched data
% S.detrend  - detrending order (0 for no detrending)
% S.hanning  - apply Hanning window (true or false)
%
% Output:
% D        - MEEG object (also written on disk)
%__________________________________________________________________________
% Copyright (C) 2010 Wellcome Trust Centre for Neuroimaging

% Melanie Boly
% $Id: spm_eeg_erp_correction.m 6907 2016-10-21 09:41:59Z vladimir $

SVNrev = '$Rev: 6907 $';

%-Startup
%--------------------------------------------------------------------------
spm('FnBanner', mfilename, SVNrev);
spm('FigName','Correct ERPs'); spm('Pointer','Arrow');

if nargin == 0
    S = [];
end

%-Get MEEG object
%--------------------------------------------------------------------------
try
    D = S.D;
catch
    [D, sts] = spm_select(1, 'mat', 'Select M/EEG mat file');
    if ~sts, D = []; return; end
    S.D = D;
end

D = spm_eeg_load(D);

if isequal(D.type, 'continuous')
    error('Corrections can only be applied to epoched or averaged datasets.');
end

%-Get parameters
%--------------------------------------------------------------------------
if ~isfield(S, 'detrend')
    S.detrend = spm_input('Detrend order', '+1', 'n', '2', 1);
end

if ~isfield(S, 'hanning')
    S.hanning = spm_input('Apply Hanning?','+1','yes|no',[1 0], 0);
end

Ns = D.nsamples;

%-Confounds - DCT:
%--------------------------------------------------------------------------
if S.detrend == 0
    X0 = sparse(Ns, 1);
else
    X0 = spm_dctmtx(Ns, S.detrend);
end
R      = speye(Ns) - X0*X0';

%-Hanning
%--------------------------------------------------------------------------
if S.hanning
    R  = R*diag(spm_hanning(Ns))*R;
end

Dnew = clone(D, ['C' fname(D)]);

spm_progress_bar('Init', D.ntrials, 'Trials filtered'); drawnow;
if D.ntrials > 100, Ibar = floor(linspace(1, D.ntrials,100));
else Ibar = [1:D.ntrials]; end

spm('Pointer','Watch');

%-Adjust data
%--------------------------------------------------------------------------
for i = 1:D.ntrials
    Dnew(Dnew.indchantype('MEEG', 'GOOD'),:,i) = (R*spm_squeeze(D(D.indchantype('MEEG', 'GOOD'),:,i), 3)')';

    if ismember(i, Ibar), spm_progress_bar('Set', i); end
end

spm_progress_bar('Clear');

D = Dnew;

D = D.history(mfilename, S);

save(D);

%-Cleanup
%--------------------------------------------------------------------------
spm('FigName','Correct ERPs: done'); spm('Pointer','Arrow');


classdef KosDragCoeffientModel < AbstractDragCoefficientModel
    %kOSDragCoefficientModel Summary of this class goes here
    %   Detailed explanation goes here

    properties
        dataFile(1,:) char

        machNum(:,1) double
        aoa(:,1) double
        sideslip(:,1) double
        data double

        giDragCube
        giOtherDrag
    end

    properties(Access=private)
        giRnCorr
    end

    properties(Constant)
        enum = DragCoefficientModelEnum.kOSModel
    end

    methods
        function obj = KosDragCoeffientModel(dataFile)
            obj.dataFile = dataFile;

            obj.dataFile = dataFile;

            if(isfile(dataFile))
                obj.createGriddedInterpFromFile();
            else
                obj.machNum = [0; 1];
                obj.aoa = [0; deg2rad(1)];
                obj.sideslip = [0; deg2rad(1)];

                rows = combvec(obj.machNum', obj.aoa', obj.sideslip')';
                rows(:,4:5) = 0;
                obj.data = rows;

                obj.createGriddedInterpFromData();
            end

            %see Profile.ks in the Project-Atmospheric-Drag for details of
            %this correction factor
            %https://github.com/Ren0k/Project-Atmospheric-Drag/blob/main/CCAT/DragProfile/LIB/Profile.ks
            rnCorrData = [0.00, 4.0000;
                            0.0001 ,3.0000;
                            0.01   ,2.0000;
                            0.10   ,1.2000;
                            1.00   ,1.0000;
                            100.0  ,1.0000;
                            200.0  ,0.8200;
                            500.0  ,0.8600;
                            1000.0 ,0.9000;
                            10000  ,0.9500];
            obj.giRnCorr = griddedInterpolant(rnCorrData(:,1), rnCorrData(:,2), 'pchip', 'nearest');
        end

        function createGriddedInterpFromFile(obj)
            obj.data = readmatrix(obj.dataFile);
            obj.data(:,2:3) = deg2rad(obj.data(:,2:3)); %convert aoa and sideslip to radian from deg

            obj.data = sortrows(obj.data, [3 2 1]);
            obj.machNum = unique(obj.data(:,1));
            obj.aoa = unique(obj.data(:,2));
            obj.sideslip = unique(obj.data(:,3));

            obj.createGriddedInterpFromData();
        end

        function createGriddedInterpFromData(obj)
            %drag cube Model
            c = obj.data(:,4);
            V = reshape(c, [length(obj.machNum), length(obj.aoa), length(obj.sideslip)]);

            gridVecs = {obj.machNum,obj.aoa,obj.sideslip};

            obj.giDragCube = griddedInterpolant(gridVecs,V, "linear", "nearest");

            %other drag Model
            c = obj.data(:,5);
            V = reshape(c, [length(obj.machNum), length(obj.aoa), length(obj.sideslip)]);

            gridVecs = {obj.machNum,obj.aoa,obj.sideslip};

            obj.giOtherDrag = griddedInterpolant(gridVecs,V, "linear", "nearest");
        end

        function clearData(obj)
            obj.machNum = [];
            obj.aoa = [];
            obj.sideslip = [];
            obj.data = [];
            obj.giDragCube = [];
        end

        function CdA = getDragCoeff(obj, ut, rVect, vVect, bodyInfo, mass, altitude, pressure, density, vVectECEFMag, totalAoA, aoa, sideslip)
            arguments
                obj(1,1) KosDragCoeffientModel
                ut(1,1) double
                rVect(3,1) double
                vVect(3,1) double
                bodyInfo(1,1) KSPTOT_BodyInfo
                mass(1,1) double
                altitude(1,1) double
                pressure(1,1) double
                density(1,1) double
                vVectECEFMag(1,1) double
                totalAoA(1,1) double = 0;
                aoa(1,1) double = 0;
                sideslip(1,1) double = 0;
            end

            if(density > 0)
                sideslip = angleNegPiToPi(sideslip);

                %see useProfile.ks in the Project-Atmospheric-Drag for details
                %of this algorithm
                %https://github.com/Ren0k/Project-Atmospheric-Drag/blob/main/CCAT/useProfile.ks
                reynoldsNumber = density*vVectECEFMag;
                reynoldsCorrection = obj.giRnCorr(reynoldsNumber);

                pressurePa = pressure*1000;
                speedSound = sqrt(1.4 * pressurePa / density); %m/s
                speedMS = vVectECEFMag*1000;                   %m/s
                thisMachNum = speedMS / speedSound;

                dragCubeCdA = obj.giDragCube(thisMachNum, aoa, sideslip);
                otherDragCdA = obj.giOtherDrag(thisMachNum, aoa, sideslip);

                CdA = ((dragCubeCdA*reynoldsCorrection)+otherDragCdA);
            else
                CdA = 0;
            end
        end

        function tf = usesTotalAoA(obj)
            tf = false;
        end

        function tf = usesAoaAndSideslip(obj)
            tf = true;
        end

        function useTf = openEditDialog(obj, lvdData)
            out = AppDesignerGUIOutput({false});
            lvd_EditKosDragPropertiesGUI_App(obj, lvdData, out);
            useTf = out.output{1};
        end

        function plotDragEnvelope(obj, hAx, machNum)
            arguments
                obj(1,1) KosDragCoeffientModel
                hAx = [];
                machNum(1,1) double = 0;
            end

            if(isempty(obj.machNum) || isempty(obj.aoa) || isempty(obj.sideslip))
                warning('No kOS drag coefficient data loaded: cannot plot.');
                return;
            end

            if(isempty(hAx))
                hAx = axes(figure());
            end

            dragCubeData = obj.data(:,4);

            maxCdA = max(dragCubeData);
            minCdA = min(dragCubeData);
            levels = linspace(minCdA, maxCdA, 50);

            warning("off",'MATLAB:contour:ConstantData');

            allVect = combvec(obj.aoa(:)', obj.sideslip(:)')';
            allVect = [machNum*ones(height(allVect), 1), allVect];
            cd = obj.giDragCube(allVect(:,1), allVect(:,2), allVect(:,3));

            [~,hContour] = contour(hAx, rad2deg(obj.aoa), rad2deg(obj.sideslip), reshape(cd, [length(obj.aoa), length(obj.sideslip)]), levels, 'Fill','on');
            xlabel(hAx, 'Angle of Attack [deg]');
            ylabel(hAx, 'Sideslip Angle [deg]');
            title(hAx, sprintf('Cd*A for Mach Number = %0.3f', machNum));
            grid(hAx, 'on');

            hContour.DataTipTemplate.DataTipRows(1).Label = 'AoA (deg)';
            hContour.DataTipTemplate.DataTipRows(2).Label = 'Sideslip (deg)';
            hContour.DataTipTemplate.DataTipRows(3).Label = 'Cd*A (m^2)';

            hContour.DataTipTemplate.DataTipRows(1).Format = '%0.3f';
            hContour.DataTipTemplate.DataTipRows(2).Format = '%0.3f';
            hContour.DataTipTemplate.DataTipRows(3).Format = '%0.3f';

            warning("on",'MATLAB:contour:ConstantData');

            hC = colorbar(hAx);
            hC.Label.String = 'Cd*A [m^2]';
        end
    end
end


function Pout = scale_imgs_by_csf(hP)
% Takes a string matrix of image file names
% finds the mean and std of the CSF space
% specified in a mask (hard-coded)
% and standardizes images by these values
%
% :Usage:
% ::
%
%     Pout = scale_imgs_by_csf(hP)
%
% Writes SC* images (SCaled)
%
% assumes images are spatially normalized.
% uses a canonical CSF mask!
%
% ..
%    tor wager
% ..

mP = which('canonical_ventricles.img');
[tmp,mP] = reslice_imgs(deblank(hP(1,:)),mP,0);

M = spm_general_hist(hP,mP,'canonical_ventricles');
disp(' ')
disp(['Means are: ' num2str(M(:,1)')])
disp(['Var    is: ' num2str(M(:,2)')])

for i = 1:size(hP,1)

	V = spm_vol(deblank(hP(i,:)));
	v = spm_read_vols(V);

	v = v - M(i,1);
	v = v ./ M(i,2);

	[d,f,e] = fileparts(V.fname);
	V.fname = [d filesep 'SC' f e];

	if i == 1, Pout = V.fname;, else, Pout = str2mat(Pout,V.fname);, end
	spm_write_vol(V,v);
	disp(['Written ' V.fname])

end


function[box,flag] = getBox2(theta,thetaDot,x,xDot)
theta = wrapTo180(rad2deg(theta));
thetaDot = rad2deg(thetaDot);
flag = 0;
if(theta>=-180&&theta<-150)
    thetaBucket = 1;
    flag = -1;
elseif(theta>=-150&&theta<-120)
    thetaBucket = 2;
    flag = -1;
elseif(theta>=-120&&theta<-90)
    thetaBucket = 3;
    flag = -1;
elseif(theta>=-90&&theta<-60)
    thetaBucket = 4;
    flag = -1;
elseif(theta>=-60&&theta<-30)
    thetaBucket = 5;
    flag = -1;
elseif(theta>=-30&&theta<-24)
    thetaBucket = 6;
    flag = -1;
elseif(theta>=-24&&theta<-18)
    thetaBucket = 7;
    flag = -1;
elseif(theta>=-18&&theta<-12)
    thetaBucket = 8;
    flag = -1;
elseif(theta>=-12&&theta<-6)
    thetaBucket = 9;
elseif(theta>=-6&&theta<-1)
    thetaBucket = 10;
elseif(theta>=-1&&theta<0)
    thetaBucket = 11;
elseif(theta>=0&&theta<1)
    thetaBucket  =12;
elseif(theta>=1&&theta<6)
    thetaBucket = 13;
elseif(theta>=6&&theta<12)
    thetaBucket = 14;
elseif(theta>=12&&theta<18)
    thetaBucket = 15;
    flag = -1;
elseif(theta>=18&&theta<24)
    thetaBucket = 16;
    flag = -1;
elseif(theta>=24&&theta<30)
    thetaBucket = 17;
    flag = -1;
elseif(theta>=30&&theta<60)
    thetaBucket = 18;
    flag = -1;
elseif(theta>=60&&theta<90)
    thetaBucket = 19;
    flag = -1;
elseif(theta>=90&&theta<120)
    thetaBucket = 20;
    flag = -1;
elseif(theta>=120&&theta<150)
    thetaBucket = 21;
    flag = -1;
elseif(theta>=150&&theta<=180)
    thetaBucket = 22;
    flag = -1;
end
if (x < -2.4 || x > 2.4)
    flag = -1;
    xBucket = 1;
end
if (x<-0.8&&x>=-2.4)
	xBucket = 1;
elseif (x<=0.8&&x>=-0.8)
	xBucket = 2;
elseif (x<=2.4&&x>0.8)
	xBucket = 3;
end

if (xDot<-0.5)
	xDotBucket = 1;
elseif (xDot>=-0.5&&xDot<=0.5)
	xDotBucket = 2;
else
	xDotBucket = 3;
end

if (thetaDot<-50)
	thetaDotBucket = 1;
elseif (thetaDot>=-50&&thetaDot<=50)
	thetaDotBucket = 2;
else
	thetaDotBucket = 3;
end

box = sub2ind([22,3,3,3],thetaBucket,thetaDotBucket,xBucket,xDotBucket);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Eugenio Alcala Baselga
% Date: 02/06/2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ u_opt, u_predicted, x_predicted, x_err] = NLMPC_Kinematic_Computation...
    ( controller, Hp, KC, oldu, u_predicted, x_predicted, x_Real,...
    kin_states, kin_inputs, x_Ref, y_Ref, theta_Ref, vel_Ref, omega_Ref,...
    longitud_vect, car)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%\\ Vehicle kinematic control:
%   controller: object provided by the MPC design
%   Hp: Prediction horizon
%   KC: discrete kinematic time
%   oldu: previous control action
%   u_predicted: vector of predicted control actions in the last iteration
%   x_predicted: vector of predicted states in the last iteration
%   x_Real: vector of measured vehicle states [x y theta v alpha w]
%   kin_states: number of kinematic states: 3
%   kin_inputs: number of kinematic control actions: 2
%   x_Ref: x reference (planner)
%   y_Ref: y reference (planner)
%   theta_Ref: theta reference (planner)
%   vel_Ref: velocity reference (planner)
%   omega_Ref: angular velocity reference (planner)
%   longitud_vect: length of vectors for simulation
%   car: vehicle object
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if KC==1
    % Initializing vectors:
    theta_cos           = ones(1,Hp+1);
    theta_sin           = zeros(1,Hp+1);
    theta_err_cos       = ones(1,Hp);
    U_OPT_VECTOR        = zeros(kin_inputs,Hp,longitud_vect);
    X_OPT_VECTOR        = zeros(kin_states,Hp,longitud_vect);
end

%%%% Constraints %%%%
% Control action constraints:
    % CA
        u_min_ini   = vehicle.u_min_ini;
        u_max_ini   = vehicle.u_max_ini;
        u_min       = vehicle.u_min;
        u_max       = vehicle.u_max;
    % derivative of CA
        du_min_ini  = vehicle.du_min_ini;
        du_max_ini  = vehicle.du_max_ini;
        du_min      = vehicle.du_min;
        du_max      = vehicle.du_max;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Terminal Set LMIs
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %[ PP, WW ] = TerminalSet_Continuous_Knemtic_Cmputtion();
    %[ PP, WW ] = TerminalSet_Discrete_Knemtic_Cmputtion(Ts_Kcontrol);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Errors de verdad (no forecasted from the LPV model):
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    x_err(1)    = (x_Ref(1)-x_Real(1))*cos(x_Real(3)) + (y_Ref(1)-x_Real(2))*sin(x_Real(3));
    x_err(2)    = -(x_Ref(1)-x_Real(1))*sin(x_Real(3)) + (y_Ref(1)-x_Real(2))*cos(x_Real(3));
    x_err(3)    = theta_Ref(1) - x_Real(3);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Moving Horizon Constraints:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if KC>1
        for j=1:Hp+1
            theta_cos(j) = cos(theta_Ref(1));
            theta_sin(j) = sin(theta_Ref(1));
            if(j>1)
                theta_err_cos(j-1) = cos(x_predicted(3,1)); % cos (theta_ERROR forecasted)
            end
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Computing PREDICTIONS of KIN-DYN LPV Vehicle Model. Note that
        % this A and B matrices are in continuous time due to the
        % discretization is carried out inside the controller object.
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [Ac_pred, Bc_pred, Bref] = KinemError_LPV_Model_predicted(KC,Hp,x_predicted(3,:),...
            u_predicted(2,:), vel_Ref );
        for i=1:Hp
            Ac_pred(:,:,i) = Ac_pred(:,:,1);
            Bc_pred(:,:,i) = Bc_pred(:,:,1);
        end
    else
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Computing KIN-DYN LPV Vehicle Model for First iteration. Note that
        % this A and B matrices are in continuous time due to the
        % discretization is carried out inside the controller object.
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [Ac_pred, Bc_pred, Bref] = KinemError_LPV_Model(KC,Hp,x_err(3),...
            x_Real(6), vel_Ref(1) );
    end

    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Optimization stage:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%     if(KC > car.t_ini)
        inputs = {x_err', oldu, vel_Ref, omega_Ref,...
            theta_cos, theta_sin, u_min, u_max, du_min, du_max};
%     else
%         inputs = {x_err', oldu, vel_Ref, omega_Ref,...
%             theta_cos, theta_sin, u_min_ini, u_max_ini,...
%             du_min_ini, du_max_ini};
%     end

    [solutions,diagnostics] = controller{inputs};

    if diagnostics == 1
        u_opt = oldu;
        U_OPT_VECTOR(:,:,KC) = U_OPT_VECTOR(:,:,KC-1);
        X_OPT_VECTOR(:,:,KC) = X_OPT_VECTOR(:,:,KC-1);
    else
        U_OPT_VECTOR(:,:,KC) = solutions{1};
        u_opt = U_OPT_VECTOR(:,1,KC);
        X_OPT_VECTOR(:,:,KC) = solutions{2}; % Kinematic errors
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Predictions obtained from the solver:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    x_predicted = X_OPT_VECTOR(:,:,KC);
    u_predicted = U_OPT_VECTOR(:,:,KC);

end


function [F,FF] = icml2011_mohsst5_parse_results(filename, N, burnin)

F_mean = 0;
FF = 0;

for n=(burnin+1):N
  fprintf('%d/%d\n', n, N);
  name = sprintf('%s_F%d',filename,n);
  load(name);
  F_mean = (F + (n-burnin-1)*F_mean) / (n-burnin);
  FF = (F.*F + (n-burnin-1)*FF) / (n-burnin);
end

F = F_mean;


classdef MOCell < ALGORITHM
% <multi> <real/integer/label/binary/permutation> <constrained/none>
% Cellular genetic algorithm

%------------------------------- Reference --------------------------------
% A. J. Nebro, J. J. Durillo, F. Luna, B. Dorronsoro, and E. Alba, MOCell:
% A cellular genetic algorithm for multiobjective optimization,
% International Journal of Intelligent Systems, 2009, 24(7): 726-746.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Neighborhood of each cell
            cSize    = floor(sqrt(Problem.N));
            n1       = reshape(1:cSize^2,cSize,cSize);
            n2       = n1([end,1:end-1],[end,1:end-1]);
            n3       = n1([end,1:end-1],:);
            n4       = n1([end,1:end-1],[2:end,1]);
            n5       = n1(:,[end,1:end-1]);
            n6       = n1(:,[2:end,1]);
            n7       = n1([2:end,1],[end,1:end-1]);
            n8       = n1([2:end,1],:);
            n9       = n1([2:end,1],[2:end,1]);
            Neighbor = [n1(:),n2(:),n3(:),n4(:),n5(:),n6(:),n7(:),n8(:),n9(:)];

            %% Generate random population
            Population = Problem.Initialization(size(Neighbor,1));
            Archive    = [];

            %% Optimization
            while Algorithm.NotTerminated(Archive)
                % Update the fitness of solutions in the population
                FrontNo  = NDSort(Population.objs,Population.cons,inf);
                CrowdDis = CrowdingDistance(Population.objs,FrontNo);

                % Generate one offspring for each cell
                Offspring(1:length(Population)) = SOLUTION();
                for i = 1 : length(Population)
                    parents      = Neighbor(i,TournamentSelection(2,2,FrontNo(Neighbor(i,:)),-CrowdDis(Neighbor(i,:))));
                    Offspring(i) = OperatorGAhalf(Problem,Population(parents));
                end

                % Replace the old population
                ViolationP = sum(max(0,Population.cons),2);
                ViolationO = sum(max(0,Offspring.cons),2);
                Replace    = ViolationO < ViolationP | ViolationO == ViolationP & any(Population.objs>Offspring.objs,2);
                Population(Replace) = Offspring(Replace);

                % Update the archive
                Archive = [Archive,Offspring];
                Archive = Archive(NDSort(Archive.objs,Archive.cons,1)==1);
                if length(Archive) > Problem.N
                    [~,rank] = sort(CrowdingDistance(Archive.objs,ones(1,length(Archive))),'descend');
                    Archive  = Archive(rank(1:Problem.N));
                end

                % Feedback
                nRep = min(min(20,length(Population)),length(Archive));
                Population(randperm(length(Population),nRep)) = Archive(randperm(length(Archive),nRep));
            end
        end
    end
end


function doomsday_test02 ( )

%*****************************************************************************80
%
%% DOOMSDAY_TEST02 tests DOOMSDAY against a number of known values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 May 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'DOOMSDAY_TEST02\n' );
  fprintf ( 1, '  WEEKDAY_VALUES supplies a list of dates and weekdays.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  YYYY  MM  DD  Weekday    Weekday\n' );
  fprintf ( 1, '                Tabulated  Computed\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, y, m, d, w1 ] = weekday_values ( n_data );

    if ( n_data <= 0 )
      break;
    end
%
%  The transition from Julian to Gregorian calendars occurred in 1582
%  (for some people).  The data in "WEEKDAY_VALUES" before the transition
%  is stored in Julian format, which DOOMSDAY_GREGORIAN can't handle.
%  So let's just refuse to handle 1582 or earlier%
%
    if ( y <= 1582 )
      continue;
    end

    w2 = doomsday_gregorian ( y, m, d );

    s1 = weekday_to_name_common ( w1 );
    s2 = weekday_to_name_common ( w2 );

    fprintf ( 1, '  %4d  %2d  %2d  %10s  %10s\n', y, m, d, s1, s2 );

  end

  return
end


function [results] = tapas_ti_example()
%% This script simulates data from different models and checks where it comes
% from.
%
% Input
%   mi          Model to be used for inversion
%   session     Session of the experiment (either 1 or 2)
%   block       Block of the experiment, either 3, 4 or 5
%   sbj         Subject
% Output
%
% aponteeduardo@gmail.com
% copyright (C) 2015
%

DIM_THETA = sooner_gamma_ndims();

ptheta = tapas_sooner_ptheta();
ptheta.p0(13:15) = [log(1.0) log(0.5) log(0.5)];
htheta = tapas_sooner_htheta();

pars = struct();
pars.T = linspace(0.0001, 1, 16).^5;
pars.nburnin = 5000;
pars.niter = 5000;
pars.mc3it = 8;
pars.verbose = 1;
pars.samples = 1;

data = load(mat_fname());
data = data.table;
[y, u] = prepare_data_db(data);
[y, u] = select_data(y, u, 3, 1);

[pt, tfe]  = tapas_ti_estimate(y, u, ptheta, htheta, pars);
results.pt = pt;
results.fe = tfe;
results.y = y;

end % gamma_inversion

function [y, u] = select_data(y, u, block, session)
%% Selects a chuck of the data

block = block * session;

y.t = y.t(u.b == block);
y.a = y.a(u.b == block);
y.i = y.i(u.b == block);

u.tt = u.tt(u.b == block);

end % select_data

function [fname] = mat_fname()
%% Generates the name of the simulation

% Get current location
f = mfilename('fullpath');
[tdir, ~, ~] = fileparts(f);

fname = fullfile(tdir, 'data', 'tapas_sooner.mat');

end %

function [y, u] = prepare_data_db(data)
%% Generates the data ready to be feeded into the inversion method
%
% Input
%
%   data    Matrix as generated by the python script.
%
% Output
%   y       Subjects time, actions, and trial validity
%   u       Trial type and block
%

% aponteeduardo@gmail.com
% copyright (C) 2015
%

y = struct('t', [], 'a', [], 'i', []);
u = struct('tt', [], 'b', []);

nt = size(data, 1);

% Tenths of second!
y.t = data(:, 5)/100;
y.a = data(:, 6);
y.i = zeros(nt, 1);
y.i(isnan(y.t)) = 1;

u.tt = data(:, 4);
u.b =  data(:, 1) .* data(:, 2);

% Because of an inconsistency

for i = 1:nt
    if y.a(i) == 0
        y.a(i) = 1;
    else
        y.a(i) = 0;
    end
    if u.tt(i) == 1
        u.tt(i) = 0;
    else
        u.tt(i) = 1;
    end
end

end % prepare_data_db


function [newW] = update(eta, Y, W)
%UPDATE gradient value W = W + delW

newW = zeros(size(W));
newW = W + gradient(eta, Y, W);

end


function h = mtimes(f, g)
%*   BALLFUN multiplication.
%   a*F and F*a multiplies the BALLFUN F by the scalar a.
%
% See also TIMES.

% Copyright 2018 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

fIsBallfun = isa(f, 'ballfun');
gIsBallfun = isa(g, 'ballfun');
fIsBallfunv = isa(g, 'ballfunv');

if (fIsBallfun && isnumeric(g))
    h = ballfun(g*f.coeffs, 'coeffs');
elseif (isnumeric(f) && gIsBallfun)
    h = ballfun(f*g.coeffs, 'coeffs');
elseif (fIsBallfun && gIsBallfun)
    h = times(f,g);
elseif fIsBallfunv
    h = mtimes(g, f);
else
    error('BALLFUN:mtimes:unknown', ...
          ['Undefined function ''mtimes'' for input arguments of type ' ...
           '%s and %s.'], class(f), class(g));
end
end


classdef BinaryDescriptorMatcher < handle
    %BINARYDESCRIPTORMATCHER  BinaryDescriptor matcher class
    %
    % Furnishes all functionalities for querying a dataset provided by user or
    % internal to class (that user must, anyway, populate) on the model of
    % Descriptor Matchers.
    %
    % Once descriptors have been extracted from an image (both they represent
    % lines and points), it becomes interesting to be able to match a
    % descriptor with another one extracted from a different image and
    % representing the same line or point, seen from a differente perspective
    % or on a different scale. In reaching such goal, the main headache is
    % designing an efficient search algorithm to associate a query descriptor
    % to one extracted from a dataset. In the following, a matching modality
    % based on *Multi-Index Hashing (MiHashing)* will be described.
    %
    % ### Multi-Index Hashing
    %
    % The theory described in this section is based on [MIH]. Given a dataset
    % populated with binary codes, each code is indexed `m` times into `m`
    % different hash tables, according to `m` substrings it has been divided
    % into. Thus, given a query code, all the entries close to it at least in
    % one substring are returned by search as *neighbor candidates*. Returned
    % entries are then checked for validity by verifying that their full codes
    % are not distant (in Hamming space) more than `r` bits from query code.
    % In details, each binary code `h` composed of `b` bits is divided into
    % `m` disjoint substrings `h^(1), ..., h^(m)`, each with length
    % `floor(b/m)` or `ceil(b/m)` bits. Formally, when two codes `h` and `g`
    % differ by at the most `r` bits, in at the least one of their `m`
    % substrings they differ by at the most `floor(r/m)` bits. In particular,
    % when `||h-g||_H <= r` (where `||.||_H` is the Hamming norm), there must
    % exist a substring `k` (with `1 <= k <= m`) such that:
    %
    %     || h^(k) - g^(k) ||_H <= floor(r/m)
    %
    % That means that if Hamming distance between each of the `m` substring is
    % strictly greater than `floor(r/m)`, then `||h-g||_H` must be larger that
    % `r` and that is a contradiction. If the codes in dataset are divided
    % into `m` substrings, then `m` tables will be built. Given a query `q`
    % with substrings `{q^(i)}_[i=1..m]`, `i`-th hash table is searched for
    % entries distant at the most `floor(r/m)` from `q^(i)` and a set of
    % candidates `N_i(q)` is obtained. The union of sets `N(q) = U_i {N_i(q)}`
    % is a superset of the `r`-neighbors of `q`. Then, last step of algorithm
    % is computing the Hamming distance between `q` and each element in `N(q)`,
    % deleting the codes that are distant more that `r` from `q`.
    %
    % ## References
    % [MIH]:
    % > Mohammad Norouzi, Ali Punjani, and David J Fleet. "Fast search in
    % > hamming space with Multi-Index Hashing". In Computer Vision and
    % > Pattern Recognition (CVPR), 2012 IEEE Conference on, pages 3108-3115.
    % > IEEE, 2012.
    %
    % See also: cv.BinaryDescriptor, cv.DescriptorMatcher, cv.drawLineMatches
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    methods
        function this = BinaryDescriptorMatcher()
            %BINARYDESCRIPTORMATCHER  Constructor
            %
            %     matcher = cv.BinaryDescriptorMatcher()
            %
            % The BinaryDescriptorMatcher constructed is able to store and
            % manage 256-bits long entries.
            %
            % See also: cv.BinaryDescriptorMatcher
            %
            this.id = BinaryDescriptorMatcher_(0, 'new');
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     matcher.delete()
            %
            % See also: cv.BinaryDescriptorMatcher
            %
            if isempty(this.id), return; end
            BinaryDescriptorMatcher_(this.id, 'delete');
        end
    end

    %% Algorithm
    methods
        function clear(this)
            %CLEAR  Clear dataset and internal data
            %
            %     matcher.clear()
            %
            % See also: cv.BinaryDescriptorMatcher.empty
            %
            BinaryDescriptorMatcher_(this.id, 'clear');
        end

        function status = empty(this)
            %EMPTY  Returns true if there are no train descriptors in the collection
            %
            %     status = matcher.empty()
            %
            % ## Output
            % * __status__ boolean status
            %
            % See also: cv.BinaryDescriptorMatcher.clear
            %
            status = BinaryDescriptorMatcher_(this.id, 'empty');
        end

        function save(this, filename)
            %SAVE  Saves the algorithm parameters to a file
            %
            %     matcher.save(filename)
            %
            % ## Input
            % * __filename__ Name of the file to save to.
            %
            % This method stores the algorithm parameters in the specified
            % XML or YAML file.
            %
            % See also: cv.BinaryDescriptorMatcher.load
            %
            BinaryDescriptorMatcher_(this.id, 'save', filename);
        end

        function load(this, fname_or_str, varargin)
            %LOAD  Loads algorithm from a file or a string
            %
            %     matcher.load(fname)
            %     matcher.load(str, 'FromString',true)
            %     matcher.load(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __fname__ Name of the file to read.
            % * __str__ String containing the serialized model you want to
            %   load.
            %
            % ## Options
            % * __ObjName__ The optional name of the node to read (if empty,
            %   the first top-level node will be used). default empty
            % * __FromString__ Logical flag to indicate whether the input is a
            %   filename or a string containing the serialized model.
            %   default false
            %
            % This method reads algorithm parameters from the specified XML or
            % YAML file (either from disk or serialized string). The previous
            % algorithm state is discarded.
            %
            % See also: cv.BinaryDescriptorMatcher.save
            %
            BinaryDescriptorMatcher_(this.id, 'load', fname_or_str, varargin{:});
        end

        function name = getDefaultName(this)
            %GETDEFAULTNAME  Returns the algorithm string identifier
            %
            %     name = matcher.getDefaultName()
            %
            % ## Output
            % * __name__ This string is used as top level XML/YML node tag
            %   when the object is saved to a file or string.
            %
            % See also: cv.BinaryDescriptorMatcher.save, cv.BinaryDescriptorMatcher.load
            %
            name = BinaryDescriptorMatcher_(this.id, 'getDefaultName');
        end
    end

    %% BinaryDescriptorMatcher
    methods
        function add(this, descriptors)
            %ADD  Store locally new descriptors to be inserted in dataset, without updating dataset
            %
            %     matcher.add(descriptors)
            %
            % ## Input
            % * __descriptors__ cell array of matrices containing descriptors
            %   to be inserted into dataset. Each matrix `descriptors{i}`
            %   should contain descriptors relative to lines extracted from
            %   i-th image.
            %
            % See also: cv.BinaryDescriptorMatcher.clear
            %
            BinaryDescriptorMatcher_(this.id, 'add', descriptors);
        end

        function train(this)
            %TRAIN  Update dataset by inserting into it all descriptors that were stored locally by add function
            %
            %     matcher.train()
            %
            % NOTE: Every time this function is invoked, current dataset is
            % deleted and locally stored descriptors are inserted into
            % dataset. The locally stored copy of just inserted descriptors is
            % then removed.
            %
            % See also: cv.BinaryDescriptorMatcher.add
            %
            BinaryDescriptorMatcher_(this.id, 'train');
        end

        function matches = match(this, queryDescriptors, varargin)
            %MATCH  For every input query descriptor, retrieve the best matching one from a dataset provided from user or from the one internal to class
            %
            %     matches = matcher.match(queryDescriptors, trainDescriptors)
            %     matches = matcher.match(queryDescriptors)
            %     [...] = matcher.match(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __queryDescriptors__ query descriptors.
            % * __trainDescriptors__ dataset of descriptors furnished by user.
            %
            % ## Output
            % * __matches__ vector to host retrieved matches.
            %
            % ## Options
            % * __Mask__ mask to select which input descriptors must be
            %   matched to one in dataset. In the second variant, a vector of
            %   masks (the i-th mask in vector indicates whether each input
            %   query can be matched with descriptors in dataset relative to
            %   i-th image). Not set by default.
            %
            % For every input descriptor, find the best matching one:
            %
            % - in the first variant, for a pair of images
            % - in the second variant, from one image to a set
            %
            % See also: cv.BinaryDescriptorMatcher.knnMatch,
            %  cv.BinaryDescriptorMatcher.radiusMatch
            %
            matches = BinaryDescriptorMatcher_(this.id, 'match', queryDescriptors, varargin{:});
        end

        function matches = knnMatch(this, queryDescriptors, varargin)
            %KNNMATCH  For every input query descriptor, retrieve the best k matching ones from a dataset provided from user or from the one internal to class
            %
            %     matches = matcher.knnMatch(queryDescriptors, trainDescriptors, k)
            %     matches = matcher.knnMatch(queryDescriptors, k)
            %     [...] = matcher.knnMatch(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __queryDescriptors__ query descriptors.
            % * __trainDescriptors__ dataset of descriptors furnished by user.
            % * __k__ number of the closest descriptors to be returned for
            %   every input query.
            %
            % ## Output
            % * __matches__ vector to host retrieved matches.
            %
            % ## Options
            % * __Mask__ mask to select which input descriptors must be
            %   matched to ones in dataset. A vector of masks in the second
            %   variant (the i-th mask in vector indicates whether each input
            %   query can be matched with descriptors in dataset relative to
            %   i-th image). Not set by default.
            % * __CompactResult__ flag to obtain a compact result (if true, a
            %   vector that doesn't contain any matches for a given query is
            %   not inserted in final result). default false
            %
            % For every input descriptor, find the best k matching descriptors:
            %
            % - in the first variant, for a pair of images
            % - in the second variant, from one image to a set
            %
            % See also: cv.BinaryDescriptorMatcher.match,
            %  cv.BinaryDescriptorMatcher.radiusMatch
            %
            matches = BinaryDescriptorMatcher_(this.id, 'knnMatch', ...
                queryDescriptors, varargin{:});
        end

        function matches = radiusMatch(this, queryDescriptors, varargin)
            %RADIUSMATCH  For every input query descriptor, retrieve, from a dataset provided from user or from the one internal to class, all the descriptors that are not further than maxDist from input query
            %
            %     matches = matcher.radiusMatch(queryDescriptors, trainDescriptors, maxDistance)
            %     matches = matcher.radiusMatch(queryDescriptors, maxDistance)
            %     [...] = matcher.radiusMatch(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __queryDescriptors__ query descriptors.
            % * __trainDescriptors__ dataset of descriptors furnished by user.
            % * __maxDistance__ search radius.
            %
            % ## Output
            % * __matches__ vector to host retrieved matches.
            %
            % ## Options
            % * __Mask__ mask to select which input descriptors must be
            %   matched to ones in dataset. A vector of masks in the second
            %   variant (the i-th mask in vector indicates whether each input
            %   query can be matched with descriptors in dataset relative to
            %   i-th image). Not set by default.
            % * __CompactResult__ flag to obtain a compact result (if true, a
            %   vector that doesn't contain any matches for a given query is
            %   not inserted in final result). default false
            %
            % For every input desciptor, find all the ones falling in a
            % certaing matching radius:
            %
            % - in the first variant, for a pair of images
            % - in the second variant, from one image to a set
            %
            % See also: cv.BinaryDescriptorMatcher.match,
            %  cv.BinaryDescriptorMatcher.knnMatch
            %
            matches = BinaryDescriptorMatcher_(this.id, 'radiusMatch',...
                queryDescriptors, varargin{:});
        end
    end

end


function [A,varargout] = selectMinbyRow(A,varargin)
% find minimum in each row

s = size(A);
[A,ind] = min(A,[],2);
ind = sub2ind(s,1:s(1),ind.');

for i = 1:nargout-1
  varargout{i} = varargin{i}(ind.');
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
% problem 8- periodic signal
% in one period x[n]=[0.5,1,1]

 x=[0.5 1 1];

 xp=repmat(x,1,40);

 stem(10:length(xp)+9,xp);


% bug_3s.m
% explore an imperfect adjoint that used to exist in 3s

if 0 % rotmex bug, fixed by AY on 2006-3-8:
	im(rotmex(ones(200, 'single'), 40.97561, int32(3)))
end

if ~isvar('G')
	nx = 12;
	ny = 12;
	nz = 8;
	na = 6;

%	mask = []; % for 3s object!
	f.sys_type = sprintf('3s@1,1,1,360,0,%d%s@%s@%s@-%d,%d,%d', ...
			1, ',none', '-', '-', nx, nz, na)

	G = Gtomo3(f.sys_type, [], nx, ny, nz);

	ig = image_geom('nx', nx, 'ny', ny, 'nz', nz, 'dx', 1);
	ig.mask = G.arg.mask;
prompt
end

if ~isvar('A')
	[A Aa] = test_adjoint(G);
	im(131, A), im(132, Aa'), im(133, Aa'-A)
end

jj = find(sum(abs(Aa'-A)) > 0.1);
if isempty(jj), printm 'no bug now!', return, end
jj = jj(4)

a1 = reshape(A(:,jj), nx, nz, na, []);
a2 = reshape(Aa(jj,:)', nx, nz, na, []);

im pl 3 1
im(1, a1, 'A ej'), cbar
im(2, a2, 'A'''' ej'), cbar
im(3, a2-a1, 'diff'), cbar


function p = myismember(a,A)
% MYISMEMBER Is 'a' an element of a set of positive integers? (much faster than built-in ismember)
% p = myismember(a,A)

%if isempty(A) | a < min(A) | a > max(A)  % slow

if length(A)==0
  p = 0;
elseif a < min(A)
  p = 0;
elseif a > max(A)
  p = 0;
else
  bits = zeros(1, max(A));
  bits(A) = 1;
  p = bits(a);
end
p = logical(p);


function test_nmtf(varargin)
% function test_nmtf()
%
% demonstration file for NMFLibrary.
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on June 14, 2022

    if nargin < 1
        clc;
        clear;
        close all;
        rng('default')

        m = 500;
        n = 100;
        V = rand(m,n);
        rank = 20;
        options = [];
        options.verbose = 2;
        options.max_epoch = 100;
        health_check_mode = false;
    else
        V = varargin{1};
        rank = varargin{2};
        options = varargin{3};
        health_check_mode = true;
    end

    %% perform factroization
    [x, info] = sep_symm_nmtf(V, rank, options);

end


function [fig] = plot_trajectories_offline(pos_history, N, ...
                colors, fontsize, map)
% plot_trajectpries_offline - Plot trajectories of the agents.

x0 = 10;
y0 = 10;
width = 400;
height = 400;

fig = figure('Name','Offline swarm trajectories','NumberTitle','off');
hold on;
grid on;
box on;

% Plot environment
if ~isempty(map)
    fig = draw_cylinders(fig,map);
end

% Plot trajetcories
for agent = 1:N
    hold on;

    if ~isempty(colors)
        plot3(pos_history(:,(agent-1)*3+2), pos_history(:,(agent-1)*3+1), ...
            - pos_history(:,(agent-1)*3+3), 'Color', colors(:,agent));
    else
        plot3(pos_history(:,(agent-1)*3+2), pos_history(:,(agent-1)*3+1), ...
            - pos_history(:,(agent-1)*3+3));
    end

end

xlabel('Y Position [m]','fontsize',fontsize);
ylabel('X Position [m]','fontsize',fontsize);
zlabel('Z Position [m]','fontsize',fontsize);
view(2);

set(fig,'units','pixels','position',[x0,y0,width,height]);
set(fig,'PaperPositionMode','auto');

end


function m = size(t,idx)
%SIZE Size of ktensor.
%
%   D = SIZE(T) returns the size of the tensor.
%
%   I = SIZE(T,DIM) returns the size of the dimension specified by
%   the scalar DIM.
%
%   See also KTENSOR, KTENSOR/NDIMS.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if isempty(t.lambda)
    m = [];
end

if exist('idx','var')
    m = size(t.u{idx}, 1);
else
    for i = 1 : ndims(t)
	m(i) = size(t.u{i}, 1);
    end
end


function ord = plot_vpath (vpath,T,continuous,order_trials,behaviour,cm)

if nargin < 2 || isempty(T), T = size(vpath,1); end
if nargin < 3, continuous = length(T)==1 || any(T(1)~=T); end % show it as continuous data?
if nargin < 4, order_trials = false; end % order the trials?
if nargin < 5, behaviour = []; end % behaviour with respect to which order the trials
if nargin < 6, cm = colormap; end % colormap

Gamma = vpath_to_stc(vpath);
ord = plot_Gamma (Gamma,T,continuous,order_trials,behaviour,cm);

end


function scalhand(H,scale)
    %
    %    scalhand(Handle,scalefactor)
    %
    %    Scale object Handle by amount scalefactor
    %
    %    See also ROTATE, TRNSLATE
    %
    %    Richard G. Cobb    3/96
    %
    origin=findcntr(H);
    %
    xold=get(H,'Xdata');
    xnew=scale*(xold-origin(1))+origin(1);
    yold=get(H,'Ydata');
    ynew=scale*(yold-origin(2))+origin(2);

    zold=get(H,'Zdata');
    znew=scale*(zold-origin(3))+origin(3);

    set(H,'Xdata',xnew,'Ydata',ynew,'Zdata',znew)


%                    runBenchmarkTransmissionMatrix.m
%
% Benchmark algorthms using real/empirical data.  The measurement matrix is
% a transmission matrix, and the measurements were acquired using an
% optical aparatus. The data acquisition is described in "Coherent
% Inverse Scattering via Transmission Matrices: Efficient Phase Retrieval
% Algorithms and a Public Dataset."
%    This script will reconstruct images using various algorithms, and
% report the image quality of the results.
%
% See the header of transmissionMatrixExperiment.m, or the paper cited
% above for more details.
%
% This script does the following:
% 1. Set up parameters and create a list of
% algorithm structs.
% 2. Invoke the general benchmark function benchmarkTransmissionMatrix.
%
% The benchmark program compares the performance of specified algorithms on
% reconstructing an image.  By default, this script reconstructs a 16x16
% image.  However, it can also reconstruct a 40x40 or 64x64 image by
% changing the options below.  Note, the runtime and memory requirements
% are very high when reconstructing larger images.
%
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

%clc
%clear
%close all

%% 1.Set up parameters
imageSize = 64;         % Side-length of image to reconstruct. Valid choices: 16,40,64
datasetSelection = 5;   % Which dataset to select measurements from.  Valid choices: 1-5
residualConstant = 0.3;

% Create a list of algorithms structs
wf = struct('initMethod','spectral','algorithm','wirtflow');
twf = struct('algorithm','twf');
rwf = struct('algorithm','rwf');
ampflow = struct('algorithm','amplitudeflow');
taf = struct('initMethod','orthogonal','algorithm','taf');
raf = struct('initMethod','weighted','algorithm','raf');
fienup = struct('algorithm','fienup');
gs = struct('algorithm','gerchbergsaxton');
cd = struct('algorithm','coordinatedescent','maxIters',50000);
kac = struct('algorithm','kaczmarz','maxIters',1000);
pmax = struct( 'algorithm','phasemax','maxIters',1000);
plamp = struct('algorithm', 'phaselamp','maxIters',1000);
scgm = struct('algorithm','sketchycgm');
plift = struct('algorithm','phaselift','initMethod','orthogonal');

% Grab your pick of algorithms.
algorithms = {plift};

%% 2. Run benchmark
benchmarkTransmissionMatrix(imageSize, datasetSelection, residualConstant, algorithms)