function [f,G,H] = self_collision_barrier(V,E,tol)
  % SELF_COLLISION_BARRIER Compute the a barrier function (and its derivatives)
  % for point-edge collisions of a given *strictly feasible** line-complex in
  % 2D. See "Bijective Parameterization with Free Boundaries" [Smith and
  % Schaefer 2015] or for a similar barrier "Incremental Potential Contact:
  % Intersection- and Inversion-free, Large-Deformation Dynamics" [Li et al.
  % 2020].
  %
  % [f,G,H] = self_collision_barrier(V,E,tol)
  %
  % Inputs:
  %   V  #V by 2 list of input vertex positions
  %   E  #E by 2 list of edge indices into rows of V
  %   tol  distance at which barrier term becomes positive {1e-3}
  % Outputs:
  %   f  scalar total objective value
  %   G  #V*2 gradient
  %   H  #V*2 by #V*2 sparse Hessian matrix
  %
  % Note: this function uses a special symbolic library trick which generates
  % files self_collision_barrier_cap_sym.m and self_collision_barrier_line_sym.m
  % if they don't already exist. If your change the symbolic-math part of this
  % file, you must delete these automatically generated files.
  %

  function [sqrD,T] = point_segment_squared_distance(P,A,B)
    PA = P-A;
    BA = B-A;
    T = min(max(sum(PA.*BA,2)./sum(BA.^2,2),0),1);
    % vector to closest point
    PC = PA-BA.*T;
    sqrD = sum(PC.^2,2);
  end

  function [f,G,H] = self_collision_barrier_cap(V,E,IJ)
    % vertex i colliding with edge jk

    n = size(V,1);
    % [x x x y y y]
    IJ = [IJ  n+IJ];
    X = V(IJ);

    %% Sanity check
    %p = X(:,1:2:end);
    %c = X(:,2:2:end);
    %pc = p-c;
    %d = sqrt(sum(pc.^2,2));
    %if ~isempty(d) && max(d) > tol && nargout > 1
    %  error
    %end

    % Writing the symbol
    path = mfilename('fullpath');
    aux = [path '_cap_sym.m'];
    % should also check date...
    if ~exist(aux,'file')
      % From here, only touch X
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      sX = sym('X',[1 4]);
      stol = sym('tol',[1 1]);
      sp = sX(1:2:end);
      sc = sX(2:2:end);
      spc = sp-sc;
      sd = sqrt(sum(spc.^2,2));
      sf = barrier(sd,stol);

      hess = @(sf,sX) cell2sym(arrayfun(@(g) gradient(g,sX),gradient(sf,sX),'UniformOutput',false));

      aux_handle = ...
        matlabFunction(sf,gradient(sf,sX),hess(sf,sX),'vars',{sX,stol},'File',aux);
    else
      aux_name = [mfilename('func') '_cap_sym'];
      aux_handle = str2func(aux_name);
    end

    faux=[];gaux = [];Haux = [];
    switch nargout
    case {0,1}
      [faux] = aux_handle(X,tol);
    case 2
      [faux,gaux] = aux_handle(X,tol);
    case 3
      [faux,gaux,Haux] = aux_handle(X,tol);
    end

    % unnecessary indirection?
    f_fun = @(X) faux;
    dfdX_fun = @(X) gaux;
    d2fdX2_fun = @(X) Haux;
    f = sum(f_fun(X));
    if nargout<=1
      return;
    end
    dfdX = dfdX_fun(X);
    G = full_sparse(IJ,ones(size(IJ)),reshape(dfdX,size(IJ)),2*n,1);
    if nargout<=2
      return;
    end
    d2fdX2 = double(d2fdX2_fun(X));

    d2fdX2 = reshape(d2fdX2,[],4*4);
    if psd_project
      d2fdX2 = psd_project_rows(d2fdX2);
    end

    HI = repmat(IJ,[1 1 size(IJ,2)]);
    HJ = permute(repmat(IJ,[1 1 size(IJ,2)]),[1 3 2]);
    H = fast_sparse(HI(:),HJ(:),d2fdX2(:),2*n,2*n);
  end

  function [f,G,H] = self_collision_barrier_line(V,E,IJK)

    n = size(V,1);
    % [x x x y y y]
    IJK = [IJK n+[IJK]];
    X = V(IJK);

    %% Sanity check
    %p = X(:,1:3:end);
    %a = X(:,2:3:end);
    %b = X(:,3:3:end);
    %pa = p-a;
    %ba = b-a;
    %t = sum(pa.*ba,2)./sum(ba.^2,2);
    %pc = pa - ba.*t;
    %d = sqrt(sum(pc.^2,2));
    %if ~isempty(d) && max(d) > tol && nargout > 1
    %  error
    %end

    % Writing the symbol
    path = mfilename('fullpath');
    aux = [path '_line_sym.m'];
    % should also check date...
    if ~exist(aux,'file')
      % From here, only touch X
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      sX = sym('X',[1 6]);
      stol = sym('tol',[1 1]);
      sp = sX(1:3:end);
      sa = sX(2:3:end);
      sb = sX(3:3:end);
      spa = sp-sa;
      sba = sb-sa;
      st = sum(spa.*sba,2)./sum(sba.^2,2);
      spc = spa - sba.*st;
      sd = sqrt(sum(spc.^2,2));
      sf = barrier(sd,stol);

      hess = @(sf,sX) cell2sym(arrayfun(@(g) gradient(g,sX),gradient(sf,sX),'UniformOutput',false));
      aux_handle = ...
        matlabFunction(sf,gradient(sf,sX),hess(sf,sX),'vars',{sX,stol},'File',aux);
    else
      aux_name = [mfilename('func') '_line_sym'];
      aux_handle = str2func(aux_name);
    end

    faux=[];gaux = [];Haux = [];
    switch nargout
    case {0,1}
      [faux] = aux_handle(X,tol);
    case 2
      [faux,gaux] = aux_handle(X,tol);
    case 3
      [faux,gaux,Haux] = aux_handle(X,tol);
    end

    % unnecessary indirection?
    f_fun = @(X) faux;
    dfdX_fun = @(X) gaux;
    d2fdX2_fun = @(X) Haux;
    f = sum(f_fun(X));
    if nargout<=1
      return;
    end
    dfdX = dfdX_fun(X);
    G = full_sparse(IJK,ones(size(IJK)),reshape(dfdX,size(IJK)),2*n,1);
    if nargout<=2
      return;
    end
    d2fdX2 = double(d2fdX2_fun(X));

    d2fdX2 = reshape(d2fdX2,[],6*6);
    if psd_project
      d2fdX2 = psd_project_rows(d2fdX2);
    end

    HI = repmat(IJK,[1 1 size(IJK,2)]);
    HJ = permute(repmat(IJK,[1 1 size(IJK,2)]),[1 3 2]);
    H = fast_sparse(HI(:),HJ(:),d2fdX2(:),2*n,2*n);

  end

  psd_project = true;
  % The max is not needed because we're handling that explicitly
  %
  % Smith and Schaefer
  %barrier = @(d,tol) (tol./d - 1).^2;
  % IPC
  barrier = @(D,tol) -(D-tol).^2.*log(D./tol);

  b = unique(E);
  [b1,b2] = box_each_element(V,b);
  [E1,E2] = box_each_element(V,E);
  I = box_intersect(b1-tol,b2+tol,E1-tol,E2+tol);
  I = I(~any(b(I(:,1))==E(I(:,2),:),2),:);
  BC = barycenter(V,E);

  [sqrD,T] = point_segment_squared_distance(V(b(I(:,1)),:),V(E(I(:,2),1),:),V(E(I(:,2),2),:));

  keep = find(sqrD<tol^2);

  I = I(keep,:);
  T = T(keep);

  %BC = barycenter(V,E);
  %plot_edges(V,E,'-ok');
  %hold on;
  %sct(V(b(I(:,1)),:),'or','LineWidth',2);
  %I(:,2)
  %size(E)
  %sct(BC(I(:,2),:),'og','LineWidth',2);
  %hold off;

  cap = T==0 | T==1;
  IJKline = [b(I(~cap,1)) E(I(~cap,2),:)];

  sqrD = sqrD(keep);

  Icap = I(cap,:);
  Tcap = T(cap);
  Jcap = E(Icap(:,2),1);
  Jcap(Tcap==1) = E(Icap(Tcap==1,2),2);
  IJcap = unique(sort([b(Icap(:,1)) Jcap],2),'rows');

  switch nargout
  case {0,1}
    [fline] = self_collision_barrier_line(V,E,IJKline);
    [fcap] = self_collision_barrier_cap(V,E,IJcap);
    f = fline + fcap;
  case 2
    [fline,Gline] = self_collision_barrier_line(V,E,IJKline);
    [fcap,Gcap] = self_collision_barrier_cap(V,E,IJcap);
    f = fline + fcap;
    G = Gline + Gcap;
  case 3
    [fline,Gline,Hline] = self_collision_barrier_line(V,E,IJKline);
    [fcap,Gcap,Hcap] = self_collision_barrier_cap(V,E,IJcap);
    f = fline + fcap;
    G = Gline + Gcap;
    H = Hline + Hcap;
  end

end


clear all; close all;
I=imread('circbw.tif');
se=strel('disk', 3);
J=imdilate(I, se);
a1=bwarea(I)
a2=bwarea(J)
(a2-a1)/a1
figure;
subplot(121);  imshow(I);
subplot(122);  imshow(J);


function [Psi, Astats, Kstats] = sampleHMMhypers( Psi, algParams )
% Sample hyperparameters (alpha,kappa) that control HMM transition distr.
% Uses a Metropolis-Hastings random walk proposal centered on current state
% INPUT:
%   Psi : input Markov Chain state
% OUTPUT
%   Psi : new Markov chain state
% DETAILS:
%  Given hyperparams alpha (non-sticky) and kappa (sticky) params,
%   each sequence draws transition probabilities as follows:
%     eta(jj,:) ~ Gamma( alpha, alpha, ... alpha+kappa, alpha, alpha )
%     pi(jj,:) = eta(jj,:)/sum( eta(jj,:)

%------------------------ Unpack
F = Psi.F;
[N,K] = size(F);
alpha0 = Psi.TransM.prior.alpha;
kappa0 = Psi.TransM.prior.kappa;
TransM   = Psi.TransM;

% Hyperparameters for prior on kappa:
a_kappa = TransM.prior.a_kappa;
b_kappa = TransM.prior.b_kappa;
% Variance of gamma proposal:
var_kappa = algParams.HMM.var_kappa;

% Hyperparameters for prior on alpha:
a_alpha = TransM.prior.a_alpha;
b_alpha = TransM.prior.b_alpha;
% Variance of gamma proposal:
var_alpha = algParams.HMM.var_alpha;

% Build sufficient stats
%    Ki( ii ) : # total features on for object ii
%    Skk( ii ) : sum( log(   diag entries of Pz(ii)  )  )
%    Sall( ii )    : sum( log(   all entries of Pz(ii)  ) )
Ki = zeros(1,N);
Skk = zeros(1,N);
Sall = zeros(1,N);
for ii=1:N
    Ki(ii) = sum(F(ii,:));
    pi_ii = TransM.pi( ii );

    Skk( ii ) = sum( log( diag( pi_ii ) ) );
    Sall( ii ) = sum( log(  pi_ii(:) ) );
end

Astats.nAccept = 0;
Astats.nTotal   = algParams.HMM.Niter;

Kstats.nAccept = 0;
Kstats.nTotal   = algParams.HMM.Niter;

for nn=1:algParams.HMM.Niter

    %%%%%%% Sample kappa given alpha %%%%%%%

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to previous kappa value:
    aa_kappa0 = (kappa0^2)/var_kappa;
    bb_kappa0 = kappa0/var_kappa;

    % Sample a proposed kappa:
    kappaP = randgamma(aa_kappa0) / bb_kappa0;

    % Determine log-likelihood of transition distributions given previous kappa
    % value and proposed kappa value:
    log_diff_Z = sum( ...
              Ki .* ( gammaln(alpha0*Ki+kappaP) - gammaln(alpha0*Ki+kappa0) )...
            - Ki .* ( gammaln(alpha0+kappaP) - gammaln(alpha0+kappa0)) ...
                            + (kappaP-kappa0)*Skk ...
                    );
    % Add in prior probability of previous and proposed kappa values:
    log_diff_Prior = (a_kappa-1)*(log(kappaP)-log(kappa0))-(kappaP-kappa0)*b_kappa;

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to proposed kappa value:
    aa_kappaP = (kappaP^2)/var_kappa;

    log_diff_Q  = (gammaln(aa_kappa0) - gammaln(aa_kappaP))...
                  + (aa_kappaP-aa_kappa0-1)*log(kappa0) - (aa_kappa0-aa_kappaP-1)*log(kappaP)...
                  + (aa_kappa0-aa_kappaP)*log(var_kappa);

    % Log accept-reject ratio:
    log_rho = log_diff_Z + log_diff_Prior + log_diff_Q;

    if isinf(log_rho)
        log_rho = -Inf;
    end
    rho = exp(log_rho);

    if rand < rho
        kappa0 = kappaP;
        Kstats.nAccept = Kstats.nAccept + 1;
    end
    % otherwise, just keep kappa0 to current value

    %%%%%%% Sample alpha given kappa %%%%%%%

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to previous alpha value:
    aa_alpha0 = (alpha0^2)/var_alpha;
    bb_alpha0 = alpha0/var_alpha;

    % Sample a proposed alpha:
    alphaP = randgamma(aa_alpha0) / bb_alpha0;

    % Determine log-likelihood of transition distributions given previous alpha
    % value and proposed alpha value:
    log_diff_Z = sum( ...
                          Ki .* ( gammaln( alphaP*Ki + kappa0 ) - gammaln( alpha0*Ki + kappa0 )) ...
                        - Ki .* ( gammaln( alphaP+kappa0 )      - gammaln( alpha0+kappa0 ) ) ...
                        - Ki .* (Ki-1) .* ( gammaln( alphaP ) - gammaln( alpha0 )  ) ...
                        + ( alphaP - alpha0 ) .* Sall ...
                      );

    % Add in prior probability of previous and proposed alpha values:
    log_diff_Prior = (a_alpha-1)*(log(alphaP)-log(alpha0))-(alphaP-alpha0)*b_alpha;

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to proposed kappa value:
    aa_alphaP = (alphaP^2)/var_alpha;
    %bb_alpha = alphaP/var_alpha; % seems to be unused

    log_diff_Q = (gammaln(aa_alpha0) - gammaln(aa_alphaP))...
        + (aa_alphaP-aa_alpha0-1)*log(alpha0) - (aa_alpha0-aa_alphaP-1)*log(alphaP)...
        + (aa_alpha0-aa_alphaP)*log(var_alpha);

    % Log accept-reject ratio:
    log_rho = log_diff_Z + log_diff_Prior + log_diff_Q;

    if isinf(log_rho)
        log_rho = -Inf;
    end
    rho = exp(log_rho);

    if rand < rho
        alpha0 = alphaP;
        Astats.nAccept = Astats.nAccept + 1;
    end
    % otherwise, just keep previous value of alpha0

end % end iterative loop over params

% -------------------------------  Repack
Psi.TransM.prior.alpha = alpha0;
Psi.TransM.prior.kappa = kappa0;


function score = IGD(Population,optimum)
% <min> <multi/many> <real/integer/label/binary/permutation> <large/none> <constrained/none> <expensive/none> <multimodal/none> <sparse/none> <dynamic/none> <robust/none>
% Inverted generational distance

%------------------------------- Reference --------------------------------
% C. A. Coello Coello and N. C. Cortes, Solving multiobjective optimization
% problems using an artificial immune system, Genetic Programming and
% Evolvable Machines, 2005, 6(2): 163-190.
%------------------------------- 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".
%--------------------------------------------------------------------------

    PopObj = Population.best.objs;
    if size(PopObj,2) ~= size(optimum,2)
        score = nan;
    else
        score = mean(min(pdist2(optimum,PopObj),[],2));
    end
end


function [windows,window_radii,windowi2radiusi,nradii] = SetWindowParameters(...
  windows,window_offsets,...
  window_radii,min_window_radius,...
  max_window_radius,nwindow_radii)

if isempty(windows),

  % use radii and offsets if windows not input
  if isempty(window_radii),
    % use min, max, n if radii not input
    window_radii = unique(round(logspace(log10(min_window_radius+1),log10(max_window_radius+1),nwindow_radii)))-1;
  end
  if isempty(window_radii),
    error('window_radii is empty.');
  end
  if isempty(window_offsets),
    error('window_offsets is empty.');
  end

  % take all combinations of radii and offsets
  [all_radii,all_offsets] = meshgrid(window_radii,window_offsets);
  all_radii = all_radii(:);
  all_offsets = all_offsets(:);

  % offsets are fractions of radii
  all_offsets = round(all_offsets.*(max(all_radii,1)));

  % window is the pair of radius, offset
  windows = [all_radii,all_offsets];

  % make sure these are unique -- rounding to nearest frame might make them
  % not unique
  windows = unique(windows,'rows');

  % window i for frame t is
  % [t-windows(i,1)+windows(i,2), t+windows(i,1)+windows(i,2)]

end

if isempty(windows),
  error('windows is empty.');
end
if size(windows,2) ~= 2,
  error('windows must be nwindows x 2.');
end

[window_radii,~,windowi2radiusi] = unique(windows(:,1));
nradii = numel(window_radii);


% Test file for @chebfun/all.m.

function pass = test_all(pref)

if (nargin < 1)
    pref = chebfunpref();
end

% Test scalar valued chebfuns.
f = chebfun(@(x) sin(x), [-1 -0.5 0 0.5 1], pref);
pass(1) = ~all(f);

f = chebfun(@(x) sin(x - 0.1), [-1 -0.5 0 0.5 1], pref);
pass(2) = ~all(f);

f = chebfun(@(x) exp(2*pi*1i*x), [-1 -0.5 0 0.5 1], pref);
pass(3) = all(f);

% Test array-valued chebfun.
f = chebfun(@(x) [sin(x) sin(x - 0.1) exp(2*pi*1i*x)], [-1 -0.5 0 0.5 1], pref);
pass(4) = isequal(all(f), logical([0 0 1]));

% Test on SINGFUN:
f = chebfun(@(x) sin(x)./(x+1), 'exps', [-1 0]);
pass(5) = ~all(f);

% Test for function defined on unbounded domain:

% Blowing-up functions on [-inf inf]:

% Set the domain:
dom = [-Inf Inf];

op = @(x) x.^2.*(1-exp(-x.^2))+3;
f = chebfun(op, dom, 'exps', [2 2]);
pass(6) = all(f);

end


function calc_McGrid_plot(mCatalog, fBinning)
% function calc_McGrid_plot(mCatalog, fBinning);
% --------------------------------------------
% Determine Mc using maximum likelihood score
% Fitting non-cumulative frequency magnitude distribution above and below Mc:
% below Mc with an exponential function: M = c*exp(d*M)
% above: Gutenberg-Richter law
% Normalized version !!!!
% Incoming variables:
% mCatalog   : EQ catalog
% fBinning   : Binning interval, usually 0.1
%
% Outgoing variables:
%
% J. Woessner: woessner@seismo.ifg.ethz.ch
% last update: 15.11.02

% Initialize
vProbability = [];
vMc = [];
vABValue =[];
vX_res = [];
vNCumTmp = [];
mDataPred = [];

% Determine exact time period
fPeriod1 = max(mCatalog(:,3)) - min(mCatalog(:,3));

% Determine max. and min. magnitude
fMinMag = floor(min(mCatalog(:,6)));
if fMinMag > 0
  fMinMag = 0;
end
fMaxMag = ceil(10 * max(mCatalog(:,6))) / 10;

% Calculate FMD for original catalog
%  [vFMD, vNonCFMD] = calc_FMD(mCatalog);
% % Time normalization
% vFMD(2,:) = ceil(vFMD(2,:)./fPeriod1);
% vNonCFMD(2,:) = ceil(vNonCFMD(2,:)./fPeriod1);
%  vNonCFMD = fliplr(vNonCFMD);
for fMc = 0.8:0.1:3.5
    % Calculate FMD for original catalog
    [vFMD, vNonCFMD] = calc_FMD(mCatalog);
    vNonCFMD = fliplr(vNonCFMD);
    [nIndexLo, fMagHi, vSel, vMagnitudes] = fMagToFitBValue(mCatalog, vFMD, fMc);
    [fMeanMag, fBValue, fStdDev, fAValue] =  calc_bmemag(mCatalog(vSel,:), fBinning);
    % Normalize FMDs
    vFMD(2,:) = ceil(vFMD(2,:)./fPeriod1);
    vNonCFMD(2,:) = ceil(vNonCFMD(2,:)./fPeriod1);
    % Compute quantity of earthquakes by power law
    vMstep = [fMinMag:0.1:fMaxMag];
    vNCum = 10.^(fAValue-fBValue.*vMstep); % Cumulative number
    % Compute non-cumulative numbers vN
    fNCumTmp = 10^(fAValue-fBValue*(fMaxMag+0.1));
    vNCumTmp  = [vNCum fNCumTmp];
    vN = abs(diff(vNCumTmp));
    % Normalizea-value
    fAValue = fAValue./fPeriod1;
    % Normlize vN
    vN = vN./fPeriod1;
    % Data selection above and below Mc
    mData = [vN' vNonCFMD']
    vSel = (mData(:,2) >= fMc);
    mDataTest = mData(~vSel,:)
    mDataTmp = mData(vSel,:)
    % Curve fitting: Non cumulative part below Mc
    options = optimset;
    options = optimset('Display','iter','Tolfun',1e-6,'TolX',0.0001);
    [vX, resnorm, resid, exitflag, output, lambda, jacobian]=lsqcurvefit(@calc_expdecay2,[0 1], mDataTest(:,2), mDataTest(:,3));
    mDataTest(:,1) = vX(1).*exp(vX(2).*mDataTest(:,2))-1;

     %% Set predicted data together
    mDataPred = [mDataTest; mDataTmp]
    vProb_ = calc_log10poisspdf(vNonCFMD(2,:)', ceil(mDataPred(:,1)));

    % Sum the probabilities
    fProbability = (-1) * sum(vProb_);
    vProbability = [vProbability; fProbability];
    vMc = [vMc; fMc];
    vABValue = [vABValue; fAValue fBValue];

    %% Confidence interval
    [vPred,delta] = nlpredci(@calc_expdecay2,mDataTest(:,2),vX, resid, jacobian);

    figure_w_normalized_uicontrolunits(200)
    subplot(3,1,1)
    %plot(vFactor, exp(vFactor)-1);
    plot(mDataTest(:,2), mDataTest(:,1),'-r', mDataTest(:,2), mDataTest(:,3), '*')
    hold on;
    plot(vNonCFMD(1,:)', vNonCFMD(2,:),'g^')
    %figure_w_normalized_uicontrolunits(300)
    subplot(3,1,2)
    semilogy(vNonCFMD(1,:)', vNonCFMD(2,:)', '^', vNonCFMD(1,:)', vN, '*', vNonCFMD(1,:)',mDataPred(:,1),'o')
    subplot(3,1,3)
    %figure_w_normalized_uicontrolunits(310)
    plot(mDataTest(:,2),vPred)
    hold on
    plot(mDataTest(:,2), mDataTest(:,3),'+r')
    plot(mDataTest(:,2),vPred + delta,'--g')
    plot(mDataTest(:,2),vPred - delta,'--g')
    hold off;
    if (fProbability == min(vProbability))
        vPredBest = vPred;
        vDeltaBest = delta;
        mDat = mDataTest;
        vNBest = vN;
        fMcBest = fMc;
        mDatPredBest = mDataPred;
        vX_res = [vX resnorm exitflag];
    end
    % Clear variables
    vNCumTmp = [];
    mModelDat = [];
    vNCum = [];
    vSel = [];
    mDataTest = [];
    mDataPred = [];
end
figure_w_normalized_uicontrolunits(400)
plot(vMc, vProbability,'*');
figure_w_normalized_uicontrolunits(410)
plot(mDat(:,2),vPredBest)
hold on;
plot(mDat(:,2), mDat(:,3),'+r')
plot(mDat(:,2),vPredBest + vDeltaBest,'--g')
plot(mDat(:,2),vPredBest - vDeltaBest,'--g')
hold off;
figure_w_normalized_uicontrolunits(420)
semilogy(vNonCFMD(1,:)', vNonCFMD(2,:)', '^', vNonCFMD(1,:)', vNBest, '*', vNonCFMD(1,:)',mDatPredBest(:,1),'o')
sTitlestr = ['N = ' num2str(vX(1)) ' * exp( ' num2str(vX(2)) ' * M) at ' num2str(fMcBest)];
title(sTitlestr)


function C = mrdivide(A,B)
%MRDIVIDE     Long division A / B
%

% written  12/30/98     S.M. Rump
% modified 10/22/99     S.M. Rump  improvement of error if 1/B exact in few digits
% modfied  02/09/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 01/29/10     S.M. Rump  midpoint corrected, thanks to Nozomu Matsuda and Nobito Yamamoto
% modified 06/09/10     S.M. Rump  division w/o error term, thanks to Nozomu Matsuda
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  INTLAB_LONG_BETA = getappdata(0,'INTLAB_LONG_BETA');
  INTLAB_LONG_LOGBETA = getappdata(0,'INTLAB_LONG_LOGBETA');
  INTLAB_LONG_ERROR = getappdata(0,'INTLAB_LONG_ERROR');
  INTLAB_LONG_PRECISION = getappdata(0,'INTLAB_LONG_PRECISION');

  A = long(A);

  if isa(B,'double') & all( B==round(B) ) & all( abs(B)<INTLAB_LONG_BETA )

    % check zero denominator
    if any( B==0 )
      error('long division by zero')
    end

    % Division by integer
    C = A;
    C.sign = C.sign .* sign(B);
    B = abs(B);

    % Compute mantissa
    precA = size(A.mantissa,2);
    C.mantissa(:,1) = floor( A.mantissa(:,1)./B );
    for i=2:precA
      A.mantissa(:,i) = A.mantissa(:,i) + ...
         ( A.mantissa(:,i-1) - B.*C.mantissa(:,i-1) )*INTLAB_LONG_BETA ;
      C.mantissa(:,i) = floor( A.mantissa(:,i)./B );
    end

    % If remainder nonzero, continue division
    rem = A.mantissa(:,precA) - B.*C.mantissa(:,precA);
    if any( rem )
      C.mantissa = ...
         [ C.mantissa zeros(size(C.mantissa,1),INTLAB_LONG_PRECISION-precA+1) ];
      i = precA;
      while any( rem ) & ( i<INTLAB_LONG_PRECISION+1 )
        i = i+1;
        num = rem*INTLAB_LONG_BETA;
        C.mantissa(:,i) = floor( num./B );
        rem = num - B.*C.mantissa(:,i);
      end
    end

    % Omit trailing zeros
    if i<INTLAB_LONG_PRECISION
      C.mantissa = C.mantissa(:,1:i);
    end

    % Compute error
    if INTLAB_LONG_ERROR
      precC = size(C.mantissa,2);
      C.error = errorupdate( -B , C.error , 0 , 1 , rem~=0 , C.exponent-precC );
    end

    % Omit leading zeros
    C = omitleadingzeros(C);

    % Normalize result
    C = normalize(C);
    if INTLAB_LONG_ERROR
      C.error = errornormalize(C.error);
    end

  else   % long division

    % denominator with one extra digit precision
    INTLAB_LONG_PRECISION = INTLAB_LONG_PRECISION + 1;
    B = long(B);
    if INTLAB_LONG_ERROR
      Bmid = mid(B);
      % denominator offset rad(B)^2/mid(B) in double
      N = long2intval(rad(B)).^2./long2intval(Bmid);
      C = addlongerror(Bmid - mid(N),rad(N));     % to be inverted
    else
      Bmid = B;
      C = Bmid;
    end
    precC = size(C.mantissa,2);

    % check zero denominators
    if any( C.mantissa(:,1)==0 )
      error('long division by zero')
    end

    % approximation of inverse for Newton iteration, approx. 52 bits accuracy
    Cinv = C;
    Cinv.exponent = 0;
    Cinv = long(1./long2dble(Cinv));    % inverse of mid(C)
    Cinv.exponent = Cinv.exponent - C.exponent;

    % approximate inverse without error term
    err = longinit('errorterm',0);
    longinit('WithoutErrorTerm',0);

    while 1
      Res = 2 - Cinv*C;
      % check Res==1
      if all( Res.mantissa(:,1)==1 ) & all(all( Res.mantissa(:,2:end)==0 ))
        break
      end
      Cinv = Cinv * Res;
    end

    % reset precision and error term, Cinv~1/C
    longinit(err,0);
    INTLAB_LONG_PRECISION = INTLAB_LONG_PRECISION - 1;

    if INTLAB_LONG_ERROR
      % calculate error
      % error by inversion:  Res_/C  with
      %   Res_ < beta^(1-precRes),   1/C >= 1/C_1*beta^(1-C.exponent)
      % even Res < beta^(-INTLAB_LONG_PRECISION) because Newton iteration
      %   stopped with Res==1 in precision INTLAB_LONG_PRECISION+1; this
      %   is important if 1/C is exactly representable in fewer than
      %   INTLAB_LONG_PRECISION+1 beta-digits (thanks to Kurt Zehetleitner)
      precCinv = size(Cinv.mantissa,2);
      Cmant = C.mantissa(:,1);
      if precC>1
        Cmant = Cmant + C.mantissa(:,2)/INTLAB_LONG_BETA;
      end
      Cinv.error = ...
           errorupdate( -Cmant , 1 , 1-C.exponent-INTLAB_LONG_PRECISION );

      % error introduced by B:  B.error/(sqr(B)-sqr(B.error))
      Bmant = B.mantissa(:,1);
      if size(B.mantissa,2)>1
        Bmant = Bmant + B.mantissa(:,2)/INTLAB_LONG_BETA;
      end
      setround(1)
      N = ( B.error.mant .* INTLAB_LONG_BETA.^(B.error.exp-B.exponent) ) .^ 2;
      setround(-1)
      N = ( Bmant/INTLAB_LONG_BETA ) .^ 2 - N;
      % true denominator  N*beta^(2*B.exponent)

      % check zero denominator (including error)
      if any( N<=0 )
        error('long division by zero')
      end

      % first part:   B.error/(sqr(B)-sqr(B.error))
      Cinv.error = errorupdate( 1 , Cinv.error , 0 , ...
                               -N , B.error , -2*B.exponent );
      Cinv.error = errornormalize(Cinv.error);
    end

    C = A*Cinv;

  end

  setround(rndold)


function [ x ] = psit_fp( X,h,n,image)
% function [ x ] = psit_fp( X,h,n,image)
% PSI_FP maps pixels to coefficients
% Input:
%       X       : pixel values
%       h   : wavelet transform filter coefficients
%       n   : signal size
%       image  : 1 or 0 based on whether or not x is a square image
%Output:
%       X   : the coefficient values associated with the pixel values X
if image==1
    X=reshape(X,[sqrt(n),sqrt(n)]);
end
x=mdwt(X,h,log2(sqrt(n)));
x=x(:);
end


function [pt_errs_phys, pts_moved_pix, TRE_phys, TREstd_phys] = DIR_movepoints_v2(pts_mov_pix, ...
                        pts_fix_pix, spc_orig, spc_disp, D)
    pts_moved_pix = pts_fix_pix;
    D_pix = conv_3d_T_from_phys_to_pix(D, spc_disp);
    for i = 1 : size(pts_mov_pix, 1)
        x = fl( pts_fix_pix(i, :) - 1).*spc_orig(:)./spc_disp + 1;
        d1 = lerp_eval(x, D_pix(:,:,:,1));
        d2 = lerp_eval(x, D_pix(:,:,:,2));
        d3 = lerp_eval(x, D_pix(:,:,:,3));

        pts_moved_pix(i, :) =  pts_moved_pix(i,:) + [d1, d2, d3] .*spc_disp./spc_orig;
    end
    pts_moved_pix = round(pts_moved_pix);
    koef = repmat(spc_orig, [size(pts_moved_pix, 1), 1]);
    pt_errs_phys = sqrt( sum((  (pts_moved_pix - pts_mov_pix).*koef  ).^2, 2) );
    TRE_phys = mean(pt_errs_phys);
    TREstd_phys = std(sqrt( sum((  (pts_moved_pix - pts_mov_pix).*koef  ).^2, 2) ));
end


% this is an experimentaal spiral sequence

fov=256e-3; Nx=96; Ny=Nx;  % Define FOV and resolution
sliceThickness=3e-3;             % slice thinckness
Nslices=1;
Oversampling=2; % by looking at the periphery of the spiral I would say it needs to be at least 2
phi=pi/2; % orientation of the readout e.g. for interleaving

% Set system limits
sys = mr.opts('MaxGrad',30,'GradUnit','mT/m',...
    'MaxSlew',120,'SlewUnit','T/m/s',...
    'rfRingdownTime', 30e-6, 'rfDeadtime', 100e-6, 'adcDeadTime', 10e-6);
seq=mr.Sequence(sys);          % Create a new sequence object
warning('OFF', 'mr:restoreShape'); % restore shape is not compatible with spirals and will throw a warning from each plot() or calcKspace() call

% Create fat-sat pulse
% (in Siemens interpreter from January 2019 duration is limited to 8.192 ms, and although product EPI uses 10.24 ms, 8 ms seems to be sufficient)
B0=2.89; % 1.5 2.89 3.0
sat_ppm=-3.45;
sat_freq=sat_ppm*1e-6*B0*sys.gamma;
rf_fs = mr.makeGaussPulse(110*pi/180,'system',sys,'Duration',8e-3,...
    'bandwidth',abs(sat_freq),'freqOffset',sat_freq);
gz_fs = mr.makeTrapezoid('z',sys,'delay',mr.calcDuration(rf_fs),'Area',1/1e-4); % spoil up to 0.1mm

% Create 90 degree slice selection pulse and gradient
[rf, gz] = mr.makeSincPulse(pi/2,'system',sys,'Duration',3e-3,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4);

% define k-space parameters
deltak=1/fov;
kRadius = round(Nx/2);
kSamples=round(2*pi*kRadius)*Oversampling;
readoutTime = 4.2e-4;

% calculate a raw Archimedian spiral trajectory
clear ka;
ka(kRadius*kSamples+1)=1i; % init as complex
for c=0:kRadius*kSamples
    r=deltak*c/kSamples;
    a=mod(c,kSamples)*2*pi/kSamples;
    ka(c+1)=r*exp(1i*a);
end
ka=[real(ka); imag(ka)];
% calculate gradients and slew rates
[ga, sa]=mr.traj2grad(ka);

% limit analysis
safety_magrin=0.94; % we need that  otherwise we just about violate the slew rate due to the rounding errors
dt_gcomp=abs(ga)/(sys.maxGrad*safety_magrin)*sys.gradRasterTime;
dt_gabs=abs(ga(1,:)+1i*ga(2,:))/(sys.maxGrad*safety_magrin)*sys.gradRasterTime;
dt_scomp=sqrt(abs(sa)/(sys.maxSlew*safety_magrin))*sys.gradRasterTime;
dt_sabs=sqrt(abs(sa(1,:)+1i*sa(2,:))/(sys.maxSlew*safety_magrin))*sys.gradRasterTime;

figure;plot([dt_gabs; max(dt_gcomp); dt_sabs; max(dt_scomp)]');title('time stepping defined by gradient and slew-rate');

dt_smooth=max([dt_gabs;dt_sabs]);
dt_rough=max([dt_gcomp;dt_scomp]);

% apply the lower limit not to lose the trajectory detail
dt_min=4*sys.gradRasterTime/kSamples; % we want at least 4 points per revolution
dt_smooth0=dt_smooth;
dt_rough0=dt_rough;
dt_smooth(dt_smooth<dt_min)=dt_min;
dt_rough(dt_rough<dt_min)=dt_min;

figure;plot([dt_smooth0; dt_smooth; dt_rough0; dt_rough]');title('combined time stepping');

t_smooth=[0 cumsum(dt_smooth,2)];
t_rough=[0 cumsum(dt_rough,2)];

kopt_smooth=interp1(t_smooth, ka', (0:floor(t_smooth(end)/sys.gradRasterTime))*sys.gradRasterTime)';
kopt_rough=interp1(t_rough, ka', (0:floor(t_rough(end)/sys.gradRasterTime))*sys.gradRasterTime)';

% analyze what we've got
fprintf('duration orig %d us\n', round(1e6*sys.gradRasterTime*length(ka)));
fprintf('duration smooth %d us\n', round(1e6*sys.gradRasterTime*length(kopt_smooth)));
fprintf('duration rough %d us\n', round(1e6*sys.gradRasterTime*length(kopt_rough)));

[gos, sos]=mr.traj2grad(kopt_smooth);
[gor, sor]=mr.traj2grad(kopt_rough);

figure;plot([gos;abs(gos(1,:)+1i*gos(2,:))]');title('gradient with smooth (abs) constraint')
figure;plot([gor;abs(gor(1,:)+1i*gor(2,:))]');title('gradient with rough (component) constraint')

figure;plot([sos;abs(sos(1,:)+1i*sos(2,:))]');title('slew rate with smooth (abs) constraint')
figure;plot([sor;abs(sor(1,:)+1i*sor(2,:))]');title('slew rate with rough (component) constraint')

% Define gradients and ADC events
spiral_grad_shape=gos;
% Create 90 degree slice selection pulse and gradient
[rf, gz] = mr.makeSincPulse(pi/2,'system',sys,'Duration',3e-3,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4);
gzReph = mr.makeTrapezoid('z',sys,'Area',-gz.area/2);

% calculate ADC
% round-down dwell time to 10 ns
adcTime = sys.gradRasterTime*size(spiral_grad_shape,2);
% actually it is trickier than that: the (Siemens) interpreter sequence
% per default will try to split the trajectory into segments <=1000 samples
% and every of these segments will have to have duration aligned to the
% gradient raster time
adcSamplesPerSegment=1000; % you may need to play with this number to fill the entire trajectory
adcSamplesDesired=kRadius*kSamples;
adcSegments=round(adcSamplesDesired/adcSamplesPerSegment);
adcSamples=adcSegments*adcSamplesPerSegment;
adcDwell=round(adcTime/adcSamples/100e-9)*100e-9; % on Siemens adcDwell needs to be aligned to 100ns (if my memory serves me right)
adcSegmentDuration=adcSamplesPerSegment*adcDwell; % with the 100 samples above and the 100ns alignment we automatically fullfill the segment alignment requirement
if mod(adcSegmentDuration, sys.gradRasterTime)>eps
    error('ADC segmentation model results in incorrect segment duration');
end
% update segment count
adcSegments=floor(adcTime/adcSegmentDuration);
adcSamples=adcSegments*adcSamplesPerSegment;
adc = mr.makeAdc(adcSamples,'Dwell',adcDwell,'Delay',mr.calcDuration(gzReph));%lims.adcDeadTime);

% extend spiral_grad_shape by repeating the last sample
% this is needed to accomodate for the ADC tuning delay
spiral_grad_shape = [spiral_grad_shape spiral_grad_shape(:,end)];

% readout grad
gx = mr.makeArbitraryGrad('x',spiral_grad_shape(1,:),'Delay',mr.calcDuration(gzReph));
gy = mr.makeArbitraryGrad('y',spiral_grad_shape(2,:),'Delay',mr.calcDuration(gzReph));

% spoilers
gz_spoil=mr.makeTrapezoid('z',sys,'Area',deltak*Nx*4);
gx_spoil=mr.makeExtendedTrapezoid('x','times',[0 mr.calcDuration(gz_spoil)],'amplitudes',[spiral_grad_shape(1,end),0]); %todo: make a really good spoiler
gy_spoil=mr.makeExtendedTrapezoid('y','times',[0 mr.calcDuration(gz_spoil)],'amplitudes',[spiral_grad_shape(2,end),0]); %todo: make a really good spoiler

% because of the ADC alignment requirements the sampling window possibly
% extends past the end of the trajectory (these points will have to be
% discarded in the reconstruction, which is no problem). However, the
% ramp-down parts and the Z-spoiler now have to be added to the readout
% block otherwise there will be a gap inbetween
% gz_spoil.delay=mr.calcDuration(gx);
% gx_spoil.delay=gz_spoil.delay;
% gy_spoil.delay=gz_spoil.delay;
% gx_combined=mr.addGradients([gx,gx_spoil], lims);
% gy_combined=mr.addGradients([gy,gy_spoil], lims);
% gz_combined=mr.addGradients([gzReph,gz_spoil], lims);

% Define sequence blocks
for s=1:Nslices
    seq.addBlock(rf_fs,gz_fs); % fat-sat
    rf.freqOffset=gz.amplitude*sliceThickness*(s-1-(Nslices-1)/2);
    seq.addBlock(rf,gz);
    seq.addBlock(mr.rotate('z',phi,gzReph,gx,gy,adc));
    seq.addBlock(mr.rotate('z',phi,gx_spoil,gy_spoil,gz_spoil));
    %seq.addBlock(gx_combined,gy_combined,gz_combined,adc);
end

% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end

%
seq.setDefinition('FOV', [fov fov sliceThickness]);
seq.setDefinition('Name', 'spiral');
seq.setDefinition('MaxAdcSegmentLength', adcSamplesPerSegment);

seq.write('spiral.seq');   % Output sequence for scanner

% the sequence is ready, so let's see what we got
seq.plot();             % Plot sequence waveforms

%% k-space trajectory calculation
[ktraj_adc, t_adc, ktraj, t_ktraj, t_excitation, t_refocusing] = seq.calculateKspacePP();

% plot k-spaces
figure; plot(t_ktraj, ktraj'); title('k-space components as functions of time'); % plot the entire k-space trajectory
figure; plot(ktraj(1,:),ktraj(2,:),'b'); % a 2D plot
hold;plot(ktraj_adc(1,:),ktraj_adc(2,:),'r.'); title('2D k-space');

% seq.install('siemens');


function im_ext = bound_extension(im,By,Bx,type);

% im_ext = bound_extension(im,B,type);
%
% Extend an image for avoiding boundary artifacts,
%
%   By, Bx:    widths of the added stripes.
%   type:   'mirror'        Mirror extension
%           'mirror_nr':    Mirror without repeating the last pixel
%           'circular':     fft2-like
%           'zeros'

% Javier Portilla, Universidad de Granada, Jan 2004

[Ny,Nx,Nc] = size(im);

im_ext = zeros(Ny+2*By,Nx+2*Bx,Nc);
im_ext(By+1:Ny+By,Bx+1:Nx+Bx,:) = im;

if strcmp(type,'mirror'),

    im_ext(1:By,:,:) = im_ext(2*By:-1:By+1,:,:);
    im_ext(:,1:Bx,:) = im_ext(:,2*Bx:-1:Bx+1,:);
    im_ext(Ny+1+By:Ny+2*By,:,:) = im_ext(Ny+By:-1:Ny+1,:,:);
    im_ext(:,Nx+1+Bx:Nx+2*Bx,:) = im_ext(:,Nx+Bx:-1:Nx+1,:);
    im_ext(1:By,1:Bx,:) = im_ext(2*By:-1:By+1,2*Bx:-1:Bx+1,:);
    im_ext(Ny+1+By:Ny+2*By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(Ny+By:-1:Ny+1,Nx+Bx:-1:Nx+1,:);
    im_ext(1:By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(2*By:-1:By+1,Nx+Bx:-1:Nx+1,:);
    im_ext(Ny+1+By:Ny+2*By,1:Bx,:) = im_ext(Ny+By:-1:Ny+1,2*Bx:-1:Bx+1,:);

elseif strcmp(type,'mirror_nr'),

    im_ext(1:By,:,:) = im_ext(2*By+1:-1:By+2,:,:);
    im_ext(:,1:Bx,:) = im_ext(:,2*Bx+1:-1:Bx+2,:);
    im_ext(Ny+1+By:Ny+2*By,:,:) = im_ext(Ny+By-1:-1:Ny,:,:);
    im_ext(:,Nx+1+Bx:Nx+2*Bx,:) = im_ext(:,Nx+Bx-1:-1:Nx,:);
    im_ext(1:By,1:Bx,:) = im_ext(2*By+1:-1:By+2,2*Bx+1:-1:Bx+2,:);
    im_ext(Ny+1+By:Ny+2*By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(Ny+By-1:-1:Ny,Nx+Bx-1:-1:Nx,:);
    im_ext(1:By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(2*By+1:-1:By+2,Nx+Bx-1:-1:Nx,:);
    im_ext(Ny+1+By:Ny+2*By,1:Bx,:) = im_ext(Ny+By-1:-1:Ny,2*Bx+1:-1:Bx+2,:);

elseif strcmp(type,'circular'),

    im_ext(1:By,:,:) =  im_ext(Ny+1:Ny+By,:,:);
    im_ext(:,1:Bx,:) = im_ext(:,Nx+1:Nx+Bx,:);
    im_ext(Ny+1+By:Ny+2*By,:,:) = im_ext(By+1:2*By,:,:);
    im_ext(:,Nx+1+Bx:Nx+2*Bx,:) = im_ext(:,Bx+1:2*Bx,:);
    im_ext(1:By,1:Bx,:) = im_ext(Ny+1:Ny+By,Nx+1:Nx+Bx,:);
    im_ext(Ny+1+By:Ny+2*By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(By+1:2*By,Bx+1:2*Bx,:);
    im_ext(1:By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(Ny+1:Ny+By,Bx+1:2*Bx,:);
    im_ext(Ny+1+By:Ny+2*By,1:Bx,:) = im_ext(By+1:2*By,Nx+1:Nx+Bx,:);

end


function [NEIGH,posInfo] = comp_filterbankneighbors(a,M,N,do_real)

%this function is called by filterbankconstphase

chanStart = [0;cumsum(N)];

NEIGH = zeros(6,chanStart(end));

%Horizontal neighbors
for kk = 1:M
  NEIGH(1,chanStart(kk)+1) = chanStart(kk)+2;
  NEIGH(1,chanStart(kk+1)) = chanStart(kk+1)-1;
  NEIGH(1:2,chanStart(kk)+(2:N(kk)-1)) = chanStart(kk)+[(1:N(kk)-2);(3:N(kk))];
end

%Vertical neighbors
%Set time distance limit
LIM = .8;

%One channel higher
for kk = 1:M-1
  aTemp = a(kk)/a(kk+1);
  POSlow = chanStart(kk+1)+min(max(0,ceil(((0:N(kk)-1)-LIM)*aTemp)),N(kk+1)-1);
  POShigh = chanStart(kk+1)+max(0,min(floor(((0:N(kk)-1)+LIM)*aTemp),N(kk+1)-1));

%   for ll = 1:N(kk)
%     tmpIdx = (POSlow(ll):POShigh(ll))+1;
%     NEIGH((5:4+numel(tmpIdx)),chanStart(kk)+ll) = tmpIdx.';
%   end

NEIGH(5,chanStart(kk)+(1:N(kk))) = POSlow + 1;
NEIGH(6,chanStart(kk)+(1:N(kk))) = POShigh + 1;
end
if ~do_real
    aTemp = a(M)/a(1);
    POSlow = chanStart(1)+min(max(0,ceil(((0:N(M)-1)-LIM)*aTemp)),N(1)-1);
    POShigh = chanStart(1)+max(0,min(floor(((0:N(M)-1)+LIM)*aTemp),N(1)-1));

    NEIGH(5,chanStart(M)+(1:N(M))) = POSlow + 1;
    NEIGH(6,chanStart(M)+(1:N(M))) = POShigh + 1;
end
NEIGH(6,NEIGH(6,:)==NEIGH(5,:)) = 0;

%One channel lower
for kk = 2:M
  aTemp = a(kk)/a(kk-1);
  POSlow = chanStart(kk-1)+min(max(0,ceil(((0:N(kk)-1)-LIM)*aTemp))',N(kk-1)-1);
  POShigh = chanStart(kk-1)+max(0,min(floor(((0:N(kk)-1)+LIM)*aTemp),N(kk-1)-1)');

%   for ll = 1:N(kk)
%     tmpIdx = (POSlow(ll):POShigh(ll))+1;
%     NEIGH((3:2+numel(tmpIdx)),chanStart(kk)+ll) = tmpIdx.';
%   end
NEIGH(3,chanStart(kk)+(1:N(kk))) = POSlow + 1;
NEIGH(4,chanStart(kk)+(1:N(kk))) = POShigh + 1;
end
if ~do_real
    aTemp = a(1)/a(M);
    POSlow = chanStart(M)+min(max(0,ceil(((0:N(1)-1)-LIM)*aTemp)),N(M)-1);
    POShigh = chanStart(M)+max(0,min(floor(((0:N(1)-1)+LIM)*aTemp),N(M)-1));

    NEIGH(3,chanStart(1)+(1:N(1))) = POSlow + 1;
    NEIGH(4,chanStart(1)+(1:N(1))) = POShigh + 1;
end
NEIGH(4,NEIGH(4,:)==NEIGH(3,:)) = 0;

posInfo = zeros(chanStart(end),2);
for kk = 1:M
    posInfo(chanStart(kk)+(1:N(kk)),:) = [(kk-1)*ones(N(kk),1),(0:N(kk)-1)'.*a(kk)];
end
posInfo = posInfo.';


%% Copyright (C) 2016, 2019, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty
%% of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See
%% the GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public
%% License along with this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun polylog (@var{s}, @var{z})
%% Numerical polylogarithm function
%%
%% Evaluates the polylogarithm of order @var{s} and argument @var{z},
%% in double precision.  Both inputs can be arrays but their sizes
%% must be either the same or scalar.
%%
%% Example:
%% @example
%% @group
%% polylog (2, -4)
%%   @result{} ans = -2.3699
%% @end group
%% @end example
%%
%% @strong{Note} this function may be slow for large numbers of inputs.
%% This is because it is not a native double-precision implementation
%% but rather the numerical evaluation of the Python @code{mpmath} function
%% @code{polylog}.
%%
%% @seealso{@@sym/polylog}
%% @end defun

function y = polylog (s, x)
  if (nargin ~= 2)
    print_usage ();
  end

  if (isequal (size (s), size (x)) || isscalar(s))
    y = zeros (size (x));
  elseif (isscalar (x))
    y = zeros (size( s));
  else
    error ('polylog: inputs S and X must have compatible sizes')
  end

  cmd = { 'Ls = _ins[0]'
          'Lx = _ins[1]'
          'if len(Ls) == 1 and len(Lx) != 1:'
          '    Ls = Ls*len(Lx)'
          'if len(Ls) != 1 and len(Lx) == 1:'
          '    Lx = Lx*len(Ls)'
          'c = [complex(polylog(s, x)) for s,x in zip(Ls, Lx)]'
          'return c,' };
  c = pycall_sympy__ (cmd, num2cell (s(:)), num2cell (x(:)));
  for i = 1:numel (c)
    y(i) = c{i};
  end
end

%!error polylog (1)
%!error polylog (1, 2, 3)

%!error <sizes> polylog ([1 2], [1 2 3])
%!error <sizes> polylog ([1 2], [1; 2])

%!test
%! y = sym(11)/10;
%! t = sym(2);
%! x = 1.1;
%! s = 2;
%! A = polylog (s, x);
%! B = double (polylog (t, y));
%! assert (A, B, -eps);

%!test
%! % maple
%! A = 2.3201804233130983964 - 3.4513922952232026614*1i;
%! B = polylog (2, 3);
%! assert (A, B, -eps)

%!test
%! % maple, complex inputs
%! A = -11.381456201167411758 + 6.2696695219721651947*1i;
%! B = polylog (1+2i, 3+4i);
%! assert (A, B, -eps);

%!test
%! % maple, matrix inputs
%! A1 = 0.47961557317612748431 - 0.52788287823025778869*1i;
%! A2 = -0.0049750526563452645369 - 0.024579343612396884851*1i;
%! B = polylog ([-1-2i -3], [30+40i 40i]);
%! assert ([A1 A2], B, -eps);

%!test
%! % x matrix, s scalar
%! y = [1 2 sym(pi); exp(sym(1)) 5 6];
%! t = sym(2);
%! x = double (y);
%! s = 2;
%! A = polylog (s, x);
%! B = double (polylog (t, y));
%! assert (A, B, -eps);

%!test
%! % s matrix, x scalar
%! t = [1 2 sym(pi); exp(sym(1)) 5 6];
%! y = sym(2);
%! s = double (t);
%! x = 2;
%! A = polylog (s, x);
%! B = double (polylog (t, y));
%! assert (A, B, -eps);


function hf = r8_hyper_2f1 ( a, b, c, x )

%*****************************************************************************80
%
%% R8_HYPER_2F1 evaluates the hypergeometric function F(A,B,C,X).
%
%  Discussion:
%
%    A minor bug was corrected.  The HW variable, used in several places as
%    the "old" value of a quantity being iteratively improved, was not
%    being initialized.  JVB, 11 February 2008.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Shanjie Zhang, Jianming Jin.
%    MATLAB version by John Burkardt.
%
%    The F77 original version of this routine is copyrighted by
%    Shanjie Zhang and Jianming Jin.  However, they give permission to
%    incorporate this routine into a user program provided that the copyright
%    is acknowledged.
%
%  Reference:
%
%    Shanjie Zhang, Jianming Jin,
%    Computation of Special Functions,
%    Wiley, 1996,
%    ISBN: 0-471-11963-6,
%    LC: QA351.C45
%
%  Parameters:
%
%    Input, real A, B, C, X, the arguments of the function.
%    C must not be equal to a nonpositive integer.
%    X < 1.
%
%    Output, real HF, the value of the function.
%
  el = 0.5772156649015329;

  l0 = ( c == floor ( c ) ) && ( c < 0.0 );
  l1 = ( 1.0 - x < 1.0E-15 ) && ( c - a - b <= 0.0 );
  l2 = ( a == floor ( a ) ) && ( a < 0.0 );
  l3 = ( b == floor ( b ) ) && ( b < 0.0 );
  l4 = ( c - a == floor ( c - a ) ) && ( c - a <= 0.0 );
  l5 = ( c - b == floor ( c - b ) ) && ( c - b <= 0.0 );

  if ( l0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_HYPER_2F1 - Fatal error!\n' );
    fprintf ( 1, '  The hypergeometric series is divergent.\n' );
    fprintf ( 1, '  C is integral and negative.\n' );
    fprintf ( 1, '  C = %f\n', c );
    error ( 'R8_HYPER_F1 - Fatal error!' );
  end

  if ( l1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_HYPER_2F1 - Fatal error!\n' );
    fprintf ( 1, '  The hypergeometric series is divergent.\n' );
    fprintf ( 1, '  1 = X < 0, C - A - B <= 0.\n' );
    fprintf ( 1, '  A = %f\n', a );
    fprintf ( 1, '  B = %f\n', b );
    fprintf ( 1, '  C = %f\n', c );
    fprintf ( 1, '  X = %f\n', x );
    error ( 'R8_HYPER_F1 - Fatal error!' );
  end

  if ( 0.95 < x )
    eps = 1.0E-08;
  else
    eps = 1.0E-15;
  end

  if ( x == 0.0 || a == 0.0 || b == 0.0 )

    hf = 1.0;
    return

  elseif ( 1.0 - x == eps && 0.0 < c - a - b )

    gc = gamma ( c );
    gcab = gamma ( c - a - b );
    gca = gamma ( c - a );
    gcb = gamma ( c - b );
    hf = gc * gcab / ( gca * gcb );
    return

  elseif ( 1.0 + x <= eps && abs ( c - a + b - 1.0 ) <= eps )

    g0 = sqrt ( pi ) * 2.0^( - a );
    g1 = gamma ( c );
    g2 = gamma ( 1.0 + a / 2.0 - b );
    g3 = gamma ( 0.5 + 0.5 * a );
    hf = g0 * g1 / ( g2 * g3 );
    return

  elseif ( l2 || l3 )

    if ( l2 )
      nm = floor ( abs ( a ) );
    end

    if ( l3 )
      nm = floor ( abs ( b ) );
    end

    hf = 1.0;
    r = 1.0;

    for k = 1 : nm
      r = r * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
        / ( k * ( c + k - 1.0 ) ) * x;
      hf = hf + r;
    end

    return

  elseif ( l4 || l5 )

    if ( l4 )
      nm = floor ( abs ( c - a ) );
    end

    if ( l5 )
      nm = floor ( abs ( c - b ) );
    end

    hf = 1.0;
    r  = 1.0;
    for k = 1 : nm
      r = r * ( c - a + k - 1.0 ) * ( c - b + k - 1.0 ) ...
        / ( k * ( c + k - 1.0 ) ) * x;
      hf = hf + r;
    end
    hf = ( 1.0 - x )^( c - a - b ) * hf;
    return

  end

  aa = a;
  bb = b;
  x1 = x;

  if ( x < 0.0 )
    x = x / ( x - 1.0 );
    if ( a < c && b < a && 0.0 < b )
      a = bb;
      b = aa;
    end
    b = c - b;
  end

  if ( 0.75 <= x )

    gm = 0.0;

    if ( abs ( c - a - b - floor ( c - a - b ) ) < 1.0E-15 )

      m = floor ( c - a - b );
      ga = gamma ( a );
      gb = gamma ( b );
      gc = gamma ( c );
      gam = gamma ( a + m );
      gbm = gamma ( b + m );

      pa = r8_psi ( a );
      pb = r8_psi ( b );

      if ( m ~= 0 )
        gm = 1.0;
      end

      for j = 1 : abs ( m ) - 1
        gm = gm * j;
      end

      rm = 1.0;
      for j = 1 : abs ( m )
        rm = rm * j;
      end

      f0 = 1.0;
      r0 = 1.0;
      r1 = 1.0;
      sp0 = 0.0;
      sp = 0.0;

      if ( 0 <= m )

        c0 = gm * gc / ( gam * gbm );
        c1 = - gc * ( x - 1.0 )^m / ( ga * gb * rm );

        for k = 1 : m - 1
          r0 = r0 * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
            / ( k * ( k - m ) ) * ( 1.0 - x );
          f0 = f0 + r0;
        end

        for k = 1 : m
          sp0 = sp0 + 1.0 / ( a + k - 1.0 ) ...
            + 1.0 / ( b + k - 1.0 ) - 1.0 / k;
        end

        f1 = pa + pb + sp0 + 2.0 * el + log ( 1.0 - x );
        hw = f1;

        for k = 1 : 250

          sp = sp + ( 1.0 - a ) / ( k * ( a + k - 1.0 ) ) ...
            + ( 1.0 - b ) / ( k * ( b + k - 1.0 ) );

          sm = 0.0;
          for j = 1 : m
            sm = sm + ( 1.0 - a ) ...
              / ( ( j + k ) * ( a + j + k - 1.0 ) ) ...
              + 1.0 / ( b + j + k - 1.0 );
          end

          rp = pa + pb + 2.0 * el + sp + sm + log ( 1.0 - x );

          r1 = r1 * ( a + m + k - 1.0 ) * ( b + m + k - 1.0 ) ...
            / ( k * ( m + k ) ) * ( 1.0 - x );

          f1 = f1 + r1 * rp;

          if ( abs ( f1 - hw ) < abs ( f1 ) * eps )
            break
          end

          hw = f1;

        end

        hf = f0 * c0 + f1 * c1;

      elseif ( m < 0 )

        m = - m;
        c0 = gm * gc / ( ga * gb * ( 1.0 - x )^m );
        c1 = - ( - 1 )^m * gc / ( gam * gbm * rm );

        for k = 1 : m - 1
          r0 = r0 * ( a - m + k - 1.0 ) * ( b - m + k - 1.0 ) ...
            / ( k * ( k - m ) ) * ( 1.0 - x );
          f0 = f0 + r0;
        end

        for k = 1 : m
          sp0 = sp0 + 1.0 / k;
        end

        f1 = pa + pb - sp0 + 2.0 * el + log ( 1.0 - x );
        hw = f1;

        for k = 1 : 250

          sp = sp + ( 1.0 - a ) ...
            / ( k * ( a + k - 1.0 ) ) ...
            + ( 1.0 - b ) / ( k * ( b + k - 1.0 ) );

          sm = 0.0;
          for j = 1 : m
            sm = sm + 1.0 / ( j + k );
          end

          rp = pa + pb + 2.0 * el + sp - sm + log ( 1.0 - x );

          r1 = r1 * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
            / ( k * ( m + k ) ) * ( 1.0 - x );

          f1 = f1 + r1 * rp;

          if ( abs ( f1 - hw ) < abs ( f1 ) * eps )
            break
          end

          hw = f1;

        end

        hf = f0 * c0 + f1 * c1;

      end

    else

      ga = gamma ( a );
      gb = gamma ( b );
      gc = gamma ( c );
      gca = gamma ( c - a );
      gcb = gamma ( c - b );
      gcab = gamma ( c - a - b );
      gabc = gamma ( a + b - c );
      c0 = gc * gcab / ( gca * gcb );
      c1 = gc * gabc / ( ga * gb ) * ( 1.0 - x )^( c - a - b );
      hf = 0.0;
      hw = hf;
      r0 = c0;
      r1 = c1;

      for k = 1 : 250

        r0 = r0 * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
          / ( k * ( a + b - c + k ) ) * ( 1.0 - x );

        r1 = r1 * ( c - a + k - 1.0 ) * ( c - b + k - 1.0 ) ...
          / ( k * ( c - a - b + k ) ) * ( 1.0 - x );

        hf = hf + r0 + r1;

        if ( abs ( hf - hw ) < abs ( hf ) * eps )
          break
        end

        hw = hf;

      end

      hf = hf + c0 + c1;

    end

  else

    a0 = 1.0;

    if ( a < c && c < 2.0 * a && b < c && c < 2.0 * b )

      a0 = ( 1.0 - x )^( c - a - b );
      a = c - a;
      b = c - b;

    end

    hf = 1.0;
    hw = hf;
    r = 1.0;

    for k = 1 : 250

      r = r * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
        / ( k * ( c + k - 1.0 ) ) * x;

      hf = hf + r;

      if ( abs ( hf - hw ) <= abs ( hf ) * eps )
        break
      end

      hw = hf;

    end

    hf = a0 * hf;

  end

  if ( x1 < 0.0 )
    x = x1;
    c0 = 1.0 / ( 1.0 - x )^aa;
    hf = c0 * hf;
  end

  if ( 120 < k )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_HYPER_2F1 - Warning!\n' );
    fprintf ( 1, '  A large number of iterations were needed.\n' );
    fprintf ( 1, '  The accuracy of the results should be checked.\n' );
  end

  return
end


 function [x, mask0] = mri_phase_denoise(yi, varargin)
%function [x, mask0] = mri_phase_denoise(yi, [options]) <- recommended usage
%|function [x] = mri_phase_denoise(yi, l2b, niter, chat, wthresh) <- old way
%|
%| in
%|	yi	[(N)]	noisy complex image: y = mag .* exp(1i * x)
%|			can be any size: 1D, 2D, 3D, ...
%|
%| options
%|	l2b		log_2(beta), regularization parameter
%|	order		regularization order (default: 1, for historical
%|				reasons, but 2 is probably preferable)
%|	niter		# of iterations
%|	chat		1 to show pictures
%|	wthresh		fraction of magnitude maximum to include in fitting
%|	init		initial image for iterations
%|	isave		which iterations to save.  (default: last)
%|	'pl'	1|0	1 for new PL method (recommend), 0 for old way (default)
%| out
%|	x	[(N)]	cleaned up phase estimate (radians)
%|	mask0	[(N)]	logical: 1 for high-magnitude pixels
%|
%| Example of weighted "denoising" of MRI phase images.
%| This is a "simple" way to estimate good field inhomogeneity maps
%| from the usual approach of two readouts with a short delay.
%| It also smoothly interpolates over regions with signal voids.
%|
%| Caution: the sign of the field map estimated here is the opposite (negative)
%| of the sign of the field map needed for input to the Gmri object.
%|
%| Copyright 1999, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if ischar(yi)
	[x, mask0] = mri_phase_denoise_test(yi, varargin{:});
	if ~nargout, clear x mask0, end
return
end

% defaults
arg.chat = 0;
arg.l2b = -5;
arg.order = 1;
arg.niter = 150;
arg.isave = [];
arg.wthresh = 0.4;
arg.fmax = 0.05; % fraction of max threshold for trimmed median for wi_ml
arg.init = [];
arg.clim = []; % limits for phase display
arg.pl = false; % PL
arg.wi_ml = false; % use wi based on ML instead of threshold

% backword compatible for old argument list: l2b, niter, chat, wthresh
if length(varargin) && isnumeric(varargin{1})
	arg.l2b = varargin{1};
	if length(varargin) >= 2, arg.niter = varargin{2}; end
	if length(varargin) >= 3, arg.chat = varargin{3}; end
	if length(varargin) >= 4, arg.wthresh = varargin{4}; end
else
	arg = vararg_pair(arg, varargin);
end
if isempty(arg.isave), arg.isave = arg.niter; end

mag = abs(yi);
yi = angle(yi);
if arg.chat
	im plc 2 3
	im(1, mag, 'magnitude'), cbar
	im(2, yi, 'raw phase map', arg.clim), cbar
end

dim_yi = size(yi);

%
% specify weights: this needs more work to be automatic!
%
mask0 = mag > arg.wthresh * max(mag(:)); % ignore pixels with "too small" magnitude
if arg.chat
	im(3, mask0, 'weights'), cbar
	im(4, mask0 .* yi, 'masked phase', arg.clim), cbar
end

%mean(mag(:))
%median(mag(:))
%clf, hist(mag(:), 100), pause
%median(mag(mag(:) > 0.05 * max(mag(:))))

if arg.wi_ml
	wi = mri_phase_wi_ml(mag, arg.fmax);
else
	wi = mask0;
end
%W = diag_sp(wi(:));
W = Gdiag(wi);

%
% initial phase image
%
if isempty(arg.init)
	arg.init = yi;
	arg.init(mask0 == 0) = mean(yi(mask0 == 0));
end
if arg.chat
	im(5, arg.init, 'Initial phase', arg.clim), cbar
end

%G = diag_sp(ones(prod(dim_yi),1));
G = Gdiag(ones([dim_yi 1]));

%
% regularizer
%
if arg.order ~= 2, warn('order=2 recommended'), end
mask1 = true(size(yi)); % estimate / extrapolate to *all* pixels
R = Reg1(mask1, 'beta', 2^arg.l2b, 'order', arg.order);
%R = Robject(mask1, 'beta', 2^arg.l2b, 'order', arg.order);
%	'type_denom', 'matlab', ...

if 0 % old way
	[C, wjk] = C2sparse('tight', mask1, 8); % todo: cut?  do not use!
	C = spdiag(sqrt(wjk), 'nowarn') * C; % caution: missing prior to 2005-11-28
	C = sqrt(2^arg.l2b) * C;
end

% report expected blur (at image center)
if 1
	qpwls_psf(G, R, 1, mask1, W);
end

%
% run qpwls algorithm for regularized fitting
%
xinit = arg.init(mask1);
if arg.pl
	med = median(mag(mag(:) > 0.05 * max(mag(:))));
	data = {yi(:), (mag(:)/med).^2}; handle = @phase_dercurv; % PL
%profile on % todo!
	x = pl_pcg_qs_ls(xinit, G, data, handle, R, ...
		'niter', arg.niter, 'isave', arg.isave);
%profile report
	x = embed(x, mask1);
else
	warn 'recommend using "pl" option.  use wls for historical only'
%	data = {yi(:), wi(:)}; handle = @wls_dercurv; % qpwls
	x = qpwls_pcg1(xinit, G, W, yi(:), R.C, ...
		'niter', arg.niter, 'isave', arg.isave);
	x = embed(x, mask1);
end

if 0 % old way
	x = qpwls_pcg(x, G, W, yi(:), 0, R.C, 1, arg.niter);
	x = reshape(x, [dim_yi arg.niter]);
	x = x(:,:,end);
end

if arg.chat
	if ndims(yi) == 2
		im(6, x(:,:,end), 'QPWLS-CG phase', arg.clim), cbar
	else % 3d
		im(6, x(:,:,:,end), 'QPWLS-CG phase', arg.clim), cbar
	end
end

%
% mri_phase_wi_ml()
% wi based on ML estimation
% trick: normalize by median of non-background so that beta is "universal"
%
function wi = mri_phase_wi_ml(mag, fmax)
med = median(mag(mag(:) > fmax * max(mag(:))));
wi = (mag / med).^2;

%
% phase_dercurv()
% wi * (1 - cos(yi - li))
%
function [deriv, curv] = phase_dercurv(data, li, varargin)
yi = data{1};
wi = data{2};
deriv = wi .* sin(li - yi);
curv = wi;

%
% built-in test/example
%
function [xq, mask0] = mri_phase_denoise_test(type, varargin)

% read data
yi = ir_get_data(fullfile('mri','2001-phase-data','phfit.mat'));
clim = [-0.5 1.5];

%if nargout
%	order = 1;
%else
%	order = 2;
%end
[xq mask0] = mri_phase_denoise(yi, ...
	'clim', clim, 'init', [], 'chat', 1, varargin{:});
%	'init', 5*randn(size(yi)));
%	'init', 5*ones(size(yi)));
xq = xq(:,:,end);
if im
	title 'QPWLS-CG phase (simple wi)'
end

cpu etic
xpl = mri_phase_denoise(yi, 'pl', 1, varargin{:});
cpu etoc 'PL time'
im(4, xpl, 'PL-CG phase', clim), cbar

cpu etic
xml_qpwls = mri_phase_denoise(yi, 'wi_ml', 1, varargin{:});
cpu etoc 'PWLS time'
im(5, xml_qpwls, 'QPWLS-CG (ML wj)', clim), cbar

max_percent_diff(xpl, xml_qpwls)
%max_percent_diff(xpl, xq)
nrms(xml_qpwls, xpl)
nrms(xq, xpl)
%im(4, xpl-xq), cbar

% ir_savefig fig_mr_phase_pl


function D = all_pairs_distances(V,U)
  % ALL_PAIRS_DISTANCES compute distances between each point i in V and point j
  % in U
  %
  % This is obsolete: use pdist2 instead
  %
  % D = all_pairs_distances(V,U)
  %
  % Inputs:
  %   V  #V by dim list of points
  %   U  #U by dim list of points
  % Outputs:
  %   D  #V by #U matrix of distances, where D(i,j) gives the distance between
  %     V(i,:) and U(j,:)
  %

  warning('obsolete. Call `pdist2` directly instead');

  assert(size(V,2) == size(U,2));

  %% Super-slow for loops method
  %% Elapsed time is 128.969203 seconds.
  %D = zeros(size(V,1),size(U,1));
  %for i = 1:size(V,1)
  %  for j = 1:size(U,1)
  %    D(i,j) = sqrt(sum((V(i,:)-U(j,:)).^2,2));
  %  end
  %end

  %% Slow, especially if hitting memory paging
  %% Elapsed time is 2.519262 seconds.
  %D = ...
  %  squeeze(sqrt(sum((repmat(V,[1 1 size(U,1)])- ...
  %  permute(repmat(U,[1 1 size(V,1)]),[3 2 1])).^2,2)));

  %% Faster single for loop, single repmat method, handles memory paging better
  %% Elapsed time is 1.160419 seconds.
  %D = zeros(size(V,1),size(U,1));
  %for i = 1:size(V,1)
  %  D(i,:) = ...
  %    sqrt(sum((repmat(V(i,:),[size(U,1) 1]) - U).^2,2))';
  %end

  if size(V,1) > 1000 || size(U,1) > 1000
    % Fast single for loop, single bsxfun method, but matlab can handle both for
    % loops in bsxfun. This seems to be a nice balance between hitting a memory
    % wall for big input and still being fast for medium and slow input
    % Elapsed time is 0.863138 seconds.
    D = zeros(size(V,1),size(U,1));
    for i = 1:size(V,1)
      D(i,:) = ...
        sqrt(sum(bsxfun(@minus,V(i,:),U).^2,2));
    end
  else
    % Fastest
    % Elapsed time is 0.653082 seconds.
    D = permute(sqrt(sum(bsxfun(@minus,V,permute(U,[3 2 1])).^2,2)),[1 3 2]);
  end
end


function [out] = melt_3(p1,p2,T,S1,S2,St,dt,varargin)
%melt_3

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Glacier melt provided no snow is stored on the ice layer
% Constraints:  f <= S1/dt
% @(Inputs):    p1   - degree-day factor [mm/oC/d]
%               p2   - temperature threshold for snowmelt [oC]
%               T    - current temperature [oC]
%               S1   - current storage in glacier [mm]
%               S2   - current storage in snowpack [mm]
%               St   - storage in S2 threshold below which glacier melt occurs [mm]
%               dt   - time step size [d]
%               varargin(1) - smoothing variable r (default 0.01)
%               varargin(2) - smoothing variable e (default 5.00)

if size(varargin,2) == 0
    out = min(max(p1*(T-p2),0),S1/dt).*smoothThreshold_storage_logistic(S2,St);
elseif size(varargin,2) == 1
    out = min(max(p1*(T-p2),0),S1/dt).*smoothThreshold_storage_logistic(S2,St,varargin(1));
elseif size(varargin,2) == 2
    out = min(max(p1*(T-p2),0),S1/dt).*smoothThreshold_storage_logistic(S2,St,varargin(1),varargin(2));
end

end


function [nyear,ncoef]=decode_coeff_pointer(np);
%USAGE:  [nyear,ncoef]=decode_coeff_pointer(np);
%
% Decode igrf10syn gh pointer to extract year number and ncoef
% 1<=ncoef<=120 for nyear=1:19
% 1<=ncoef<=195 for nyear=20:24
%
% np=(nyear-1)*120+ncoef for np<=np1_max
% np=np1_max+(nyear-19)*195+ncoef; for nyear=20:24
%
% year=1995+(nyear-1)*5;
%
np1_max=2280;
if np<=np1_max
    nyear=floor(np/120)+1;
    ncoef=np-(nyear-1)*120;
    if ncoef==0
        ncoef=120;
        nyear=nyear+1;
    end
elseif np<=3255
   np2=np-np1_max;
   nyear=floor(np2/195)+1;
   ncoef=np2-(nyear-1)*195;
   if ncoef==0
       ncoef=195;
       nyear=nyear+1;
   end
   nyear=19+nyear;
else
    error('np>3255!')
end
return


%------------------------------ PolyMesher -------------------------------%
% Ref: C Talischi, GH Paulino, A Pereira, IFM Menezes, "PolyMesher: A     %
%      general-purpose mesh generator for polygonal elements written in   %
%      Matlab," Struct Multidisc Optim, DOI 10.1007/s00158-011-0706-z     %
%-------------------------------------------------------------------------%
function [x] = MichellDomain(Demand,Arg)
  BdBox = [0 5 -2 2];
  switch(Demand)
    case('Dist');  x = DistFnc(Arg,BdBox);
    case('BC');    x = BndryCnds(Arg{:},BdBox);
    case('BdBox'); x = BdBox;
    case('PFix');  x = FixedPoints(BdBox);
  end
%----------------------------------------------- COMPUTE DISTANCE FUNCTIONS
function Dist = DistFnc(P,BdBox)
  d1 = dRectangle(P,BdBox(1),BdBox(2),BdBox(3),BdBox(4));
  d2 = dCircle(P,0,0,BdBox(4)/2);
  Dist = dDiff(d1,d2);
%---------------------------------------------- SPECIFY BOUNDARY CONDITIONS
function [x] = BndryCnds(Node,Element,BdBox)
  eps = 0.1*sqrt((BdBox(2)-BdBox(1))*(BdBox(4)-BdBox(3))/size(Node,1));
  CircleNodes = find(abs(sqrt(Node(:,1).^2+Node(:,2).^2)-1.0)<eps);
  Supp = ones(size(CircleNodes,1),3);
  Supp(:,1) = CircleNodes;
  MidRightFace = sqrt((Node(:,1)-BdBox(2)).^2+...
                      (Node(:,2)-(BdBox(3)+BdBox(4))/2).^2);
  [foo,MidRightFace] = sort(MidRightFace);
  Load = [MidRightFace(1),0,-1];
  x = {Supp,Load};
%----------------------------------------------------- SPECIFY FIXED POINTS
function [PFix] = FixedPoints(BdBox)
  PFix = [5 0];
%-------------------------------------------------------------------------%


function [Fnew, obj, M,k,x,u,n,deg,linears,nonlinears,vecConstraints,isinequality,ulong] = momentmodel(F,obj,k,keepnonlinears)

if nargin < 2
    obj = [];
end
if nargin < 3
    k = [];
end
if nargin < 4
    keepnonlinears = 0;
end

vecConstraints = [];
sdpConstraints = [];
isinequality = [];
binaries = [];
xvars = [];
Fnew = ([]);
for i = 1:length(F)
    if is(F(i),'elementwise')
        X = sdpvar(F(i));
        vecConstraints = [vecConstraints;X(:)];
        isinequality = [isinequality ones(1,prod(size(X)))];
        xvars = [xvars depends(X(:))];
    elseif is(F(i),'equality')
        X = sdpvar(F(i));
        if is(X,'symmetric')
            X = X(find(triu(ones(length(X)))));
        end
        vecConstraints = [vecConstraints;-X(:)];
        isinequality = [isinequality zeros(1,prod(size(X)))];
        xvars = [xvars depends(X(:))];
    elseif is(F(i),'sdp')
        sdpConstraints{end+1} = sdpvar(F(i));
        xvars = [xvars depends(F(i))];
    elseif is(F(i),'binary')
        binaries = [binaries getvariables(F(i))];
    else
        Fnew = Fnew+F(i); % Should only be SOCP constraints
    end
end

% Recover the involved variables
x = recover(unique([depends(obj) xvars]));
n = length(x);

% Check degrees of constraints
deg = [];
for i = 1:length(vecConstraints)
    deg(end+1) = degree(vecConstraints(i));
end
for i = 1:length(sdpConstraints)
    deg(end+1) = degree(sdpConstraints{i});
end
if isempty(deg)
    deg = 0;
end

% Create lowest possible relaxation if k=[]
d = ceil((max(degree(obj),max(deg)))/2);
k_min = d;
if isempty(k)
    k = k_min;
else
    if k<k_min
        error('Higher order relaxation needed')
    end
end

% Generate monomials of order k
u{k} = monolist(x,k);
ulong{k} = monolist(x,2*k);

% Largest moment matrix. NOTE SHIFT M{k+1} = M_k.
M{k+1}=u{k}*u{k}';
% Moment matrices easily generated with this trick
% The matrices will NOT be rank-1 since the products
% generate the relaxed variables

% ... and lower degree localization matrices
M{1} = 1;
for i = 1:1:k-1;
    n_i = round(factorial(n+k-i)/(factorial(n)*factorial(k-i)));
    M{k-i+1} = M{k+1}(1:n_i,1:n_i);
end

% Lasserres relaxation (Lasserre, SIAM J. OPTIM, 11(3) 796-817)
Fmoments = (M{k+1}>=0);
for i = 1:length(vecConstraints)
    if isinequality(i)
        v_k = floor((degree(vecConstraints(i))+1)/2);
        Localizer = vecConstraints(i)*M{k-v_k+1};
        if isa(vecConstraints(i),'double')
            if vecConstraints(i)<0
                error('Problem is trivially infeasible due to negative constant')
            else
                continue
            end
        end
        Fmoments = Fmoments+(Localizer>=0);
    else
        if isa(vecConstraints(i),'double')
            if vecConstraints(i)~=0
                error('Problem is trivially infeasible due to non-zero constant in equality constraints')
            else
                continue
            end
        end
        Localizer = vecConstraints(i)*monolist(x,2*k-degree(vecConstraints(i)));
        Fmoments = Fmoments+(Localizer==0);
    end
end
for i = 1:length(sdpConstraints)
    v_k = floor((degree(sdpConstraints{i})+1)/2);
    Fmoments = Fmoments+(kron(M{k-v_k+1},sdpConstraints{i})>=0);
end

% Add them all
Fnew = Fnew + Fmoments;

% Get all binary and reduce problem
binaries = union(binaries,yalmip('binvariables'));
if ~isempty(binaries)
    if isa(obj,'sdpvar')
        obj = eliminateBinary(obj,binaries);
    end
    for i = 1:length(Fmoments)
        Fnew(i) = eliminateBinary(Fnew(i),binaries);
    end
    for i = 2:1:k+1;
        M{i} = eliminateBinary(M{i},binaries);
    end
end

vars = getvariables(Fnew);
for i = 1:length(M)
    vars = [vars getvariables(M{i})];
end
vars = unique([vars getvariables(obj)]);
[mt,variabletype] = yalmip('monomtable');
nonlinears = vars(find(variabletype(vars)));
newLinear = sdpvar(length(nonlinears),1);

if isa(obj,'sdpvar')
    obj = variablereplace(obj,nonlinears,getvariables(newLinear));
end
for i = 1:length(M)
    if isa(M{i},'sdpvar')
        M{i} = variablereplace(M{i},nonlinears,getvariables(newLinear));
    end
end
linears = getvariables(newLinear);
Fnew = variablereplace(Fnew,nonlinears,getvariables(newLinear));
linears = recover(linears);
nonlinears = recover(nonlinears);

end


%SOLVEP3P  Finds an object pose from 3 3D-2D point correspondences
%
%     [rvecs, tvecs, solutions] = cv.solveP3P(objectPoints, imagePoints, cameraMatrix)
%     [...] = cv.solveP3P(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __objectPoints__ Array of object points in the object coordinate space,
%   1xNx3/Nx1x3 or Nx3 array, where `N=3` is the number of points, or cell
%   array of length `N=3` of 3-element vectors can be also passed here
%   `{[x1,y1,z1], [x2,y2,z2], [x3,y3,z3]}`.
% * __imagePoints__ Array of corresponding image points, 1xNx2/Nx1x2 or Nx2
%   array, where `N=3` is the number of points, or cell array of length `N=3`
%   of 2-element vectors can be also passed here `{[x1,y1], [x2,y2], [x3,y3]}`.
% * __cameraMatrix__ Input camera matrix `A = [fx 0 cx; 0 fy cy; 0 0 1]`.
%
% ## Output
% * __rvecs__ Output rotation vectors (see cv.Rodrigues) that, together with
%   `tvecs`, brings points from the model coordinate system to the camera
%   coordinate system. A P3P problem has up to 4 solutions.
% * __tvecs__ Output translation vectors.
% * __solutions__ number of solutions.
%
% ## Options
% * __DistCoeffs__ Input vector of distortion coefficients
%   `[k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,taux,tauy]` of 4, 5, 8, 12 or 14
%   elements. If the vector is empty, the zero distortion coefficients are
%   assumed. default empty.
% * __Method__ Method for solving the P3P problem. One of the following:
%   * __P3P__ (default) Method is based on the paper [gao2003complete].
%   * __AP3P__ Method is based on the paper [Ke17].
%
% The function estimates the object pose given 3 object points, their
% corresponding image projections, as well as the camera matrix and the
% distortion coefficients.
%
% ## References
% [gao2003complete]:
% > X.S. Gao, X.R. Hou, J. Tang, H.F. Chang; "Complete Solution
% > Classification for the Perspective-Three-Point Problem",
% > IEEE Trans. on PAMI, vol. 25, No. 8, p. 930-943, August 2003.
%
% [Ke17]:
% > T. Ke, S. Roumeliotis; "An Efficient Algebraic Solution to the
% > Perspective-Three-Point Problem", IEEE Conference on Computer Vision and
% > Pattern Recognition (CVPR), 2017
% > [PDF](https://arxiv.org/pdf/1701.08237.pdf)
%
% See also: cv.solvePnP
%


% fig2i HungarianCV matrix plot

i_fish = 6;
[cIX1,gIX1] = LoadCluster_Direct(i_fish,4,2);
[cIX2,gIX2] = LoadCluster_Direct(i_fish,5,2);

% [score,im1] = HungarianCV(cIX1,cIX2,gIX1,gIX2,isPlotFig);
[score,im1] = HungarianCV(cIX2,cIX1,gIX2,gIX1);

%%
figure('Position',[500,300,250,250]);
% subplot(1,2,1)
% imagesc(-im1)
% colormap(bluewhitered)
% axis equal; axis tight;axis xy
%
% subplot(1,2,2)
imagesc(-log(im1))
% colormap(bluewhitered)
axis equal; axis tight;%axis xy
colormap('gray')
ylabel('clusters (1st half)')
xlabel('clusters (2nd half)')
title('Cross-Val: # of cells overlap')
text(size(im1,1)/4,size(im1,2)/20,'score = 0.69');


clear
clc

ObjectiveFunction = @my_first_SA;   % Function handle to the objective function
X0 = [1 1];   % Starting point
lb = [-2 -2];     % Lower bound
ub = [2 2];       % Upper bound

options = saoptimset('MaxIter',500,'StallIterLim',500,'TolFun',1e-100,'AnnealingFcn',@annealingfast,'InitialTemperature',100,'TemperatureFcn',@temperatureexp,'ReannealInterval',500,'PlotFcns',{@saplotbestx, @saplotbestf, @saplotx, @saplotf,@saplottemperature});

[x,fval] = simulannealbnd(ObjectiveFunction,X0,lb,ub,options);


classdef L2normalization < dagnn.ElementWise
    properties (Transient)
        numInputs
        SIZE_
    end

    methods
        function outputs = forward(obj, inputs, params)
            obj.numInputs = numel(inputs);
            assert(obj.numInputs == 1);

            X = inputs{1};
            obj.SIZE_ = size(X);
            if length(obj.SIZE_) < 3
                obj.SIZE_ = [obj.SIZE_, 1, 1];
            elseif length(obj.SIZE_) ==3
                obj.SIZE_ = [obj.SIZE_ 1];
            elseif length(obj.SIZE_) > 4
                error('length(SIZE_) > 4');
            end

            X = permute(X, [3,1,2,4]);
            X = reshape(X, [size(X,1), prod(obj.SIZE_)/obj.SIZE_(3)] );
            sumX = sqrt(sum(X.^2,1));
            %sumX = sumX + (sumX==0);
            sumX = sumX + 0.00001;
            X = bsxfun(@rdivide, X, sumX);

            X = reshape(X, [size(X,1), obj.SIZE_(1:2), obj.SIZE_(4)]);
            X = permute(X, [2,3,1,4]);
            outputs{1} = X;
        end

        function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
            derInputs = cell(1, numel(inputs));

            dzdy = derOutputs{1};
            X = inputs{1};
%             obj.SIZE_ = size(X);
%             if length(obj.SIZE_) < 3
%                 obj.SIZE_ = [obj.SIZE_, 1, 1];
%             elseif length(obj.SIZE_) ==3
%                 obj.SIZE_ = [obj.SIZE_ 1];
%             elseif length(obj.SIZE_) > 4
%                 error('length(SIZE_) > 4');
%             end

            X = permute(X, [3,1,2,4]);
            X = reshape(X, [size(X,1), prod(obj.SIZE_)/obj.SIZE_(3)] );
            dzdy = permute(dzdy, [3,1,2,4]);
            dzdy = reshape(dzdy, [size(dzdy,1), prod(obj.SIZE_)/obj.SIZE_(3)] );

            lambda = 1./(sqrt(sum(X.^2, 1)) + 1e-10);
            dzdx = bsxfun(@times, lambda, dzdy) - bsxfun(@times, X, (lambda.^3) .* sum(X.*dzdy, 1));

            dzdx = reshape(dzdx, [size(dzdx,1), obj.SIZE_(1:2), obj.SIZE_(4)]);
            dzdx = permute(dzdx, [2,3,1,4]);

            derInputs{1} = dzdx;
            derParams = {} ;
        end

        function obj = Scale(varargin)
            obj.load(varargin) ;
        end
    end
end


function bessel_y1_spherical_values_test ( )

%*****************************************************************************80
%
%% BESSEL_Y1_SPHERICAL_VALUES_TEST tests BESSEL_Y1_SPHERICAL_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_Y1_SPHERICAL_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_Y1_SPHERICAL_VALUES stores values of\n' );
  fprintf ( 1, '  the y1 spherical Bessel function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X            FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_y1_spherical_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


%% Colour Filter
%% This function can be used for separating or filter out Red components,
%% Green components and Blue Components of colors from the color images.
%% Function C = colorfilter(img,color)
%% input    img = Color image (The input image should be a color image)
%%          color = which color want to be filtered out from the color image
%%                  it may be 'R' / 'r' / 'G' / 'g' / 'B' / 'b'
%%  Example: C = colorfilter(img,'r');
%%      Posted date   : 26 - 06 - 2008
%%      Modified date : 08 - 07 - 2008
%%
%% Developed By : K.Kannan & Jeny Rajan
%%                  Medical Imaging Research Group (MIRG), NeST, Trivandrum.
%%
function C = colorfilter(img,color)

[row col plane] = size(img);
img = double(img);
C = zeros(row,col,plane);
if plane ~= 3
    disp('Input should be a color image');
    return;
end
factor = max(img(:)) * 0.2;
switch color
    case {'R','r'}
        for i = 1:row
            for j = 1:col
                if (img(i,j,1) > factor && img(i,j,1) == max([img(i,j,1) img(i,j,2) img(i,j,3)]))
                    C(i,j,1:3) = img(i,j,1:3);
                else
                    C(i,j,1:3) = (img(i,j,1) * 0.3) + (img(i,j,2) * 0.59) + (img(i,j,3) * 0.11);
                end
            end
        end
    case {'G','g'}
        for i = 1:row
            for j = 1:col
                if (img(i,j,2) > factor && img(i,j,2) == max([img(i,j,1) img(i,j,2) img(i,j,3)]))
                    C(i,j,1:3) = img(i,j,1:3);
                else
                    C(i,j,1:3) = (img(i,j,1) * 0.3) + (img(i,j,2) * 0.59) + (img(i,j,3) * 0.11);
                end
            end
        end
    case {'B','b'}
        for i = 1:row
            for j = 1:col
                if (img(i,j,3) > factor && img(i,j,3) == max([img(i,j,1) img(i,j,2) img(i,j,3)]))
                    C(i,j,1:3) = img(i,j,1:3);
                else
                    C(i,j,1:3) = (img(i,j,1) * 0.3) + (img(i,j,2) * 0.59) + (img(i,j,3) * 0.11);
                end
            end
        end
    otherwise
        disp('unknown method');
end
C = uint8(C);
figure,imshow(uint8(img),[]);
figure,imshow(uint8(C),[]);


  function ob = Godwt1(mask, varargin)
%|function ob = Godwt1(mask, varargin)
%| Construct Godwt1 object that computes orthonormal discrete wavelet
%| decomposition of a signal with dimensions [(N)].
%| This is useful for sparsity regularization (aka compressed sensing).
%| (1D or 2D wavelets only)
%|
%| in
%|	'mask'	logical [(Nd)]	image-domain mask, often true(nx,ny)
%|
%| options
%|	'level'	int	decomposition level (default 1)
%|	'wname'	char	wavelet name. default: 'haar'
%|
%| out
%|	ob	[*N np]	fatrix  object, where np = sum(mask(:))
%|
%|
%| Copyright 2012-05-17, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(mask, 'test'), Godwt1_test, return, end

arg.mask = mask;
arg.level = 1;
arg.wname = 'haar';
arg.abs = false;
arg = vararg_pair(arg, varargin);

if isempty(arg.mask), fail 'must provide a mask', end

% transform dimension
idim = size(arg.mask);
if idim(end) == 1
	idim = idim(1:end-1);
end

switch numel(idim)
case 1
	forw = @(arg, x) ir_odwt1(x, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs);
	back = @(arg, y) fatrix2_maskit(arg.mask, ir_odwt1(y, 'adj', 1, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs));
	does_many = true;
case 2
	forw = @(arg, x) ir_odwt2(x, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs);
	back = @(arg, y) fatrix2_maskit(arg.mask, ir_odwt2(y, 'adj', 1, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs));
	does_many = false;
otherwise
        fail('dim %d unsupported', numel(idim))
end

% build fatrix2 object
arg.idim = idim;
ob = fatrix2('idim', idim, 'odim', idim, 'arg', arg, ...
	'abs', @Godwt1_abs, 'meth', {'codes', @Godwt1_codes, '()'}, ...
	'forw', forw, 'back', back, 'does_many', does_many);

% Godwt1_abs()
function ob = Godwt1_abs(ob)
ob.arg.abs = true;

% Godwt1_codes()
function codes = Godwt1_codes(arg)

switch numel(arg.idim)
case 1
	[dummy codes] = ir_odwt1(zeros([arg.idim 1]), ...
		'level', arg.level, 'wname', arg.wname);
case 2
	[dummy codes] = ir_odwt2(zeros(arg.idim), ...
		'level', arg.level, 'wname', arg.wname);
otherwise
        fail('dim %d unsupported', numel(idim))
end

% Godwt1_test()
function Godwt1_test

if 1 % 1d
	mask = true(8*3,1);
	mask(1:3) = false;
	level = 3;
	U = Godwt1(mask, 'level', level);
%	abs(U) % todo

	if im
		im plc 1 2
		im(1, full(U))
		im(2, U.codes)
		drawnow
	end

	fatrix2_tests(U, 'complex', 1) % check complex data
	test_adjoint(U, 'complex', 1, 'tolre', 1e-9);
end

if 1 % 2d
	mask = true(8*3,16);
	mask(1:3) = false;
	level = 3;
	wname = 'haar';
	wname = 'sym2';
	U = Godwt1(mask, 'level', level, 'wname', wname);

	x = ellipse_im(size(mask));
	if im
		im plc 1 2
		im(1, x)
		im(2, U * x)
		drawnow
	end

	fatrix2_tests(U, 'complex', 1) % check complex data
	test_adjoint(U, 'complex', 1, 'tolre', 1e-9, 'big', 1);
end


function [ a, det ] = dpodi ( a, lda, n, job )

%*****************************************************************************80
%
%% DPODI computes the determinant and inverse of a certain matrix.
%
%  Discussion:
%
%    The matrix is real symmetric positive definite.
%    DPODI uses the factors computed by DPOCO, DPOFA or DQRDC.
%
%    A division by zero will occur if the input factor contains
%    a zero on the diagonal and the inverse is requested.
%    It will not occur if the subroutines are called correctly
%    and if DPOCO or DPOFA has set INFO == 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the output A from DPOCO or DPOFA, or the output
%    X from DQRDC.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix A.
%
%    Input, integer JOB, specifies the task.
%    11, both determinant and inverse.
%    01, inverse only.
%    10, determinant only.
%
%    Output, real A(LDA,N), if DPOCO or DPOFA was used to factor A then
%    DPODI produces the upper half of inverse(A).  If DQRDC was used to
%    decompose X then DPODI produces the upper half of inverse(X'*X)
%    where X' is the transpose.  Elements of A below the diagonal are
%    unchanged.  If the units digit of JOB is zero, A is unchanged.
%
%    Output, real DET(2), the determinant of A or of X'*X
%    if requested.
%      determinant = DET(1) * 10.0**DET(2)
%    with 1.0 <= DET(1) < 10.0 or DET(1) == 0.0.
%

%
%  Compute the determinant.
%
  if ( job / 10 ~= 0 )

    det(1) = 1.0;
    det(2) = 0.0;
    s = 10.0;

    for i = 1 : n

      det(1) = a(i,i) * a(i,i) * det(1);

      if ( det(1) == 0.0 )
        break
      end

      while ( det(1) < 1.0 )
        det(1) = s * det(1);
        det(2) = det(2) - 1.0;
      end

      while ( s <= det(1) )
        det(1) = det(1) / s;
        det(2) = det(2) + 1.0;
      end

    end

  end
%
%  Compute inverse(R).
%
  if ( mod ( job, 10 ) ~= 0 )

    for k = 1 : n

      a(k,k) = 1.0 / a(k,k);
      t = -a(k,k);
      a(1:k-1,k) = dscal ( k-1, t, a(1:k-1,k), 1 );

      for j = k+1 : n
        t = a(k,j);
        a(k,j) = 0.0;
        a(1:k,j) = daxpy ( k, t, a(1:k,k), 1, a(1:k,j), 1 );
      end

    end
%
%  Form inverse(R) * (inverse(R))'.
%
    for j = 1 : n
      for k = 1 : j-1
        t = a(k,j);
        a(1:k,k) = daxpy ( k, t, a(1:k,j), 1, a(1:k,k), 1 );
      end
      t = a(j,j);
      a(1:j,j) = dscal ( j, t, a(1:j,j), 1 );
    end

  end

  return
end


dt = 0.02;
sim_t = 20;
x0 = [0;5;0];

params.v = 1; % velocity
params.u_max = 3; % max yaw rate (left)
params.u_min = -3; % min yaw rate (right)

% Obstacle position
params.xo = 5;
params.yo = 4;
% Obstacle radius
params.d = 2;
params.cbf_gamma0 = 1;
% Desired target point
params.xd = 12;
params.yd = 0;

params.clf.rate = 0.5;
params.weight.slack = 10;

params.cbf.rate = 1;

dubins = DubinsCar(params);

odeFun = @dubins.dynamics;
controller = @dubins.ctrlCbfClfQp;
odeSolver = @ode45;

total_k = ceil(sim_t / dt);
x = x0;
t = 0;
% initialize traces.
xs = zeros(total_k, dubins.xdim);
ts = zeros(total_k, 1);
us = zeros(total_k-1, 1);
Vs = zeros(total_k-1, 1);
hs = zeros(total_k-1, 1);
xs(1, :) = x0';
ts(1) = t;
for k = 1:total_k-1
    t
    % Determine control input.
    % dV_hat: analytic Vdot based on model.
    [u, slack, h, V] = controller(x);
    us(k, :) = u';
    hs(k) = h;
    Vs(k) = V;

    % Run one time step propagation.
    [ts_temp, xs_temp] = odeSolver(@(t, s) odeFun(t, s, u), [t t+dt], x);
    x = xs_temp(end, :)';

    xs(k+1, :) = x';
    ts(k+1) = ts_temp(end);
    t = t + dt;
end

plot_results(ts, xs, us, hs, [params.xo;params.yo], params.d)

function plot_results(t, xs, us, hs, p_o, r_o)

figure
subplot(3,1,1)
plot(t, xs(:,1))
xlabel('t')
ylabel('x [m]')

subplot(3,1,2)
plot(t, xs(:,2))
xlabel('t')
ylabel('y [m]')

subplot(3,1,3)
plot(t, xs(:,3))
xlabel('t')
ylabel('theta [rad]')

figure
plot(t(1:end-1), us)
xlabel('t')
ylabel('u [rad/s]')

lim_min = min(min(xs(:, 1)), min(xs(:, 2)));
lim_max = max(max(xs(:, 1)), max(xs(:, 2)));
lim_min = min([lim_min, p_o(1)-r_o, p_o(2)-r_o]);
lim_max = max([lim_max, p_o(1)+r_o, p_o(2)+r_o]);

figure
plot(xs(:, 1), xs(:, 2));
draw_circle(p_o, r_o);

xlim([lim_min, lim_max]);
ylim([lim_min, lim_max]);
xlabel('x [m]')
ylabel('y [m]')

figure
plot(t(1:end-1), hs)
xlabel('t')
ylabel('cbf h(s)');

end

function h = draw_circle(center,r)
hold on
th = 0:pi/50:2*pi;
xunit = r * cos(th) + center(1);
yunit = r * sin(th) + center(2);
h = plot(xunit, yunit);
hold off

end


function [ x,y ] = scd_scheme_deltaDeltaTE_constraints( TE, Treadout, T_RF180, deltaMin )
% tri_Dd( TE, Treadout, T_RF180, deltaMin )
% display 2D space of feasible DELTA delta for the set of parameters in input
%
% Constraints :
% DELTA + delta + Tro/2 <= TE
% DELTA >= delta + T_RF180

DELTAMin = deltaMin + T_RF180;

x1 = deltaMin;
y1 = DELTAMin;

x2 = 0.5*(TE - Treadout - T_RF180);
y2 = -x2 + TE - Treadout;

x3 = deltaMin;
y3 = -deltaMin + TE - Treadout;

x = [x1 x2 x3];
y = [y1 y2 y3];

% fill(x, y, 'r')
% xlabel('\delta (in ms)')
% ylabel('\Delta (in ms)')
% title({'2D space of feasible (\Delta,\delta) combinations' ; ['TE : ', num2str(TE),' ms', '   ','Treadout : ', num2str(Treadout),' ms', '   ']});

end


function [cx, cy, w, h] = getAxisAlignedBB(region)
% GETAXISALIGNEDBB extracts an axis aligned bbox from the ground truth REGION with same area as the rotated one
    cx = mean(region(1:2:end));
    cy = mean(region(2:2:end));
    x1 = min(region(1:2:end));
    x2 = max(region(1:2:end));
    y1 = min(region(2:2:end));
    y2 = max(region(2:2:end));
    A1 = norm(region(1:2) - region(3:4)) * norm(region(3:4) - region(5:6));
    A2 = (x2 - x1) * (y2 - y1);
    s = sqrt(A1/A2);
    w = s * (x2 - x1) + 1;
    h = s * (y2 - y1) + 1;
end


%  INTERNAL FUNCTION: Projection of covariance matrix such the eigenvalues
%  are sandwiched.
%
%  ::
%
%     vcov = project(vcov0,e_min,e_max);
%
%  Args:
%
%     - **vcov0** [matrix]: initial covariance matrix
%     - **e_min** [[]|{sqrt(eps)}]: scalar such that the minimum eigenvalue of vcov
%       is greater than or equal to "e_min".
%     - **e_max** [[]|{1/e_min}]: scalar such that maximum eigenvalue of vcov
%       is less than or equal to "e_max"
%
%  Returns:
%     :
%
%     - **vcov** [matrix]: updated covariance matrix
%
%


% robust_kmeans() - an extension of Matlab kmeans() that removes outlier
%        components from all clusters.
%        This is a helper function called from pop_clust().

function  [IDX,C,sumd,D,outliers] = robust_kmeans(data,N,STD,MAXiter,method)
% data - pre-clustering data matrix.
% N - number of wanted clusters.

if nargin < 5
    method = 'kmeans';
end;

flag  = 1;
not_outliers = 1:size(data,1);
old_outliers = [];
if strcmpi(method, 'kmeans')
    [IDX,C,sumd,D] = kmeans(data,N,'replicates',30,'emptyaction','drop'); % Cluster using K-means algorithm
else
    [IDX,C,sumd,D] = kmeanscluster(data,N); % Cluster using K-means algorithm
end;
if STD >= 2 % STD for returned outlier
    rSTD = STD -1;
else
    rSTD = STD;
end
loop = 0;

while flag
     loop =  loop + 1;
	std_all = [];
    ref_D = 0;
	for k = 1:N
        tmp = ['cls' num2str(k) ' = find(IDX=='  num2str(k) ')''; ' ]; %find the component indices belonging to each cluster (cls1 = ...).
        eval(tmp);
        tmp = ['std' num2str(k) ' = std(D(cls'  num2str(k) ' ,' num2str(k) ')); ' ]; %compute the std of each cluster
        eval(tmp);
        std_all = [std_all ['std' num2str(k)  '  ']];
        tmp = [ 'ref_D = ' num2str(ref_D) ' + mean(D(cls'  num2str(k) ' ,' num2str(k) '));' ];
        eval(tmp);
	end
	std_all = [ '[ ' std_all ' ]' ];
    std_all = eval(std_all);

	% Find the outliers
    % Outlier definition - its distance from its cluster center is bigger
    % than STD times the std of the cluster, as long as the distance is bigger
    % than the mean distance times STD (avoid problems where all points turn to be outliers).
	outliers = [];
    ref_D = ref_D/N;
	for k = 1:N
        tmp = ['cls' num2str(k) '(find(D(find(IDX=='  num2str(k) ')'' , ' num2str(k) ') > ' num2str(STD)  '*std' num2str(k) ')); ' ];
        optionalO = eval(tmp);
        Oind = find(D(optionalO,k) >  ref_D*STD);
        outliers = [outliers optionalO(Oind)];
	end
    if isempty(outliers) | (loop == MAXiter)
        flag = 0;
    end
    l = length(old_outliers);
    returned_outliers = [];

    for k = 1:l
        tmp = sum((C-ones(N,1)*data(old_outliers(k),:)).^2,2)'; % Find the distance of each former outlier to the current cluster
        if isempty(find(tmp <= std_all*rSTD))  %Check if the outlier is still an outlier (far from each cluster center more than STD-1 times its std).
            returned_outliers = [returned_outliers old_outliers(k)];
        end;
    end
    outliers = not_outliers(outliers);
    outliers = [outliers returned_outliers ];
	tmp = ones(1,size(data,1));
	tmp(outliers) = 0;
	not_outliers = (find(tmp==1));

    if strcmpi(method, 'kmeans')
        [IDX,C,sumd,D] = kmeans(data(not_outliers,:),N,'replicates',30,'emptyaction','drop');
    else
        [IDX,C,sumd,D] = kmeanscluster(data(not_outliers,:),N);
    end;
    old_outliers = outliers;
    old_IDX = zeros(size(data,1),1);
    old_IDX(sort(not_outliers)) = IDX;

end

IDX = old_IDX;


function C_hs = hdr_covariance(X,C,nh,nScans);
%
% C_hs = hdr_covariance(X,C,nh,nScans);
%
% Event-Related GLM tools:
% Compute the voxel-independent factor of the
% covariance matrix of a general linear model.
% This is taken from eq. (16) in the Greve
% theory paper (FS-FAST).
%
% original code by gb, 11/04:
% updated by ras, 05/05
C_hs = zeros(size(X,2));

for s = 1:nScans
    C_hs = C_hs + ((X(:,:,s)')*(C(:,:,s)^(-1))*X(:,:,s));
end

C_hs = C_hs^(-1);

return


function amanatidesWooAlgorithm(origin, direction, grid3D, verbose)
% A fast and simple voxel traversal algorithm through a 3D space partition (grid)
% proposed by J. Amanatides and A. Woo (1987).
%
% Input:
%    origin.
%    direction.
%    grid3D: grid dimensions (nx, ny, nz, minBound, maxBound).
% Author:
%    Jesús P. Mena-Chalco.

    if (verbose)
        figure;
        hold on;
        text(origin(1), origin(2), origin(3), 'origin');
        plot3(origin(1), origin(2), origin(3), 'k.', 'MarkerSize', 15);
        quiver3(origin(1), origin(2), origin(3), direction(1), direction(2), direction(3), 30);

        vmin = grid3D.minBound';
        vmax = grid3D.maxBound';
        BoxVertices = [vmax(1) vmin(2) vmin(3); vmax(1) vmax(2) vmin(3); vmin(1) vmax(2) vmin(3); vmin(1) vmax(2) vmax(3); vmin(1) vmin(2) vmax(3); vmax(1) vmin(2) vmax(3); vmin; vmax ];
        BoxFaces = [1 2 3 7; 1 2 8 6; 1 6 5 7; 7 5 4 3; 2 8 4 3; 8 6 5 4];
        h = patch('Vertices',BoxVertices,'Faces',BoxFaces,'FaceColor','yellow');
        set(h, 'FaceAlpha', 0.1);

        view(60,30);
        axis tight;
        xlabel('x');
        ylabel('y');
        zlabel('z');
        grid on;
    end;

    [flag, tmin] = rayBoxIntersection(origin, direction, grid3D.minBound, grid3D.maxBound);

    if (flag==0)
        disp('\n The ray does not intersect the grid');
    else
        if (tmin<0)
            tmin = 0;
        end;

        start   = origin + tmin*direction;
        boxSize = grid3D.maxBound-grid3D.minBound;

        if (verbose)
            plot3(start(1), start(2), start(3), 'r.', 'MarkerSize', 15);
        end;

        x = floor( ((start(1)-grid3D.minBound(1))/boxSize(1))*grid3D.nx )+1;
        y = floor( ((start(2)-grid3D.minBound(2))/boxSize(2))*grid3D.ny )+1;
        z = floor( ((start(3)-grid3D.minBound(3))/boxSize(3))*grid3D.nz )+1;

        if (x==(grid3D.nx+1));  x=x-1;  end;
        if (y==(grid3D.ny+1));  y=y-1;  end;
        if (z==(grid3D.nz+1));  z=z-1;  end;

        if (direction(1)>=0)
            tVoxelX = (x)/grid3D.nx;
            stepX = 1;
        else
            tVoxelX = (x-1)/grid3D.nx;
            stepX = -1;
        end;

        if (direction(2)>=0)
            tVoxelY = (y)/grid3D.ny;
            stepY = 1;
        else
            tVoxelY = (y-1)/grid3D.ny;
            stepY = -1;
        end;

        if (direction(3)>=0)
            tVoxelZ = (z)/grid3D.nz;
            stepZ = 1;
        else
            tVoxelZ = (z-1)/grid3D.nz;
            stepZ = -1;
        end;

        voxelMaxX  = grid3D.minBound(1) + tVoxelX*boxSize(1);
        voxelMaxY  = grid3D.minBound(2) + tVoxelY*boxSize(2);
        voxelMaxZ  = grid3D.minBound(3) + tVoxelZ*boxSize(3);

        tMaxX      = tmin + (voxelMaxX-start(1))/direction(1);
        tMaxY      = tmin + (voxelMaxY-start(2))/direction(2);
        tMaxZ      = tmin + (voxelMaxZ-start(3))/direction(3);

        voxelSizeX = boxSize(1)/grid3D.nx;
        voxelSizeY = boxSize(2)/grid3D.ny;
        voxelSizeZ = boxSize(3)/grid3D.nz;

        tDeltaX    = voxelSizeX/abs(direction(1));
        tDeltaY    = voxelSizeY/abs(direction(2));
        tDeltaZ    = voxelSizeZ/abs(direction(3));

        while ( (x<=grid3D.nx)&&(x>=1) && (y<=grid3D.ny)&&(y>=1) && (z<=grid3D.nz)&&(z>=1) )

            if (verbose)
                fprintf('\nIntersection: voxel = [%d %d %d]', [x y z]);

                t1 = [(x-1)/grid3D.nx, (y-1)/grid3D.ny, (z-1)/grid3D.nz ]';
                t2 = [  (x)/grid3D.nx,  (y)/grid3D.ny,    (z)/grid3D.nz ]';

                vmin = (grid3D.minBound + t1.*boxSize)';
                vmax = (grid3D.minBound + t2.*boxSize)';

                smallBoxVertices = [vmax(1) vmin(2) vmin(3); vmax(1) vmax(2) vmin(3); vmin(1) vmax(2) vmin(3); vmin(1) vmax(2) vmax(3); vmin(1) vmin(2) vmax(3); vmax(1) vmin(2) vmax(3); vmin; vmax ];
                smallBoxFaces    = [1 2 3 7; 1 2 8 6; 1 6 5 7; 7 5 4 3; 2 8 4 3; 8 6 5 4];

                h = patch('Vertices', smallBoxVertices, 'Faces', smallBoxFaces, 'FaceColor', 'blue', 'EdgeColor', 'white');
                set(h,'FaceAlpha',0.2);
            end;

            % ---------------------------------------------------------- %
            % check if voxel [x,y,z] contains any intersection with the ray
            %
            %   if ( intersection )
            %       break;
            %   end;
            % ---------------------------------------------------------- %

            if (tMaxX < tMaxY)
                if (tMaxX < tMaxZ)
                    x = x + stepX;
                    tMaxX = tMaxX + tDeltaX;
                else
                    z = z + stepZ;
                    tMaxZ = tMaxZ + tDeltaZ;
                end;
            else
                if (tMaxY < tMaxZ)
                    y = y + stepY;
                    tMaxY = tMaxY + tDeltaY;
                else
                    z = z + stepZ;
                    tMaxZ = tMaxZ + tDeltaZ;
                end;
            end;
        end;
     end;
end


%#  NLP written by GAMS Convert at 06/20/02 11:29:54
%#
%#  Equation counts
%#     Total       E       G       L       N       X
%#         3       2       1       0       0       0
%#
%#  Variable counts
%#                 x       b       i     s1s     s2s      sc      si
%#     Total    cont  binary integer    sos1    sos2   scont    sint
%#         5       5       0       0       0       0       0       0
%#  FX     0       0       0       0       0       0       0       0
%#
%#  Nonzero counts
%#     Total   const      NL     DLL
%#        12       6       6       0
%#
%#  Reformualtion has removed 1 variable and 1 equation
%
%
%var x1 := 50, >= 50, <= 200;
%var x2 := 37.5, >= 37.5, <= 150;
%var x3 := 45, >= 45, <= 180;
%var x4;
%
%minimize obj: 0.00533*x1^2 + 11.669*x1 + 0.00889*x2^2 + 10.333*x2 + 0.00741*x3^
%              2 + 10.833*x3 + 653.1;
%
%subject to
%
%e2:  - (0.01*(0.0676*x1*x1 + 0.00953*x1*x2 - 0.00507*x1*x3 + 0.00953*x2*x1 +
%    0.0521*x2*x2 + 0.00901*x2*x3 - 0.00507*x3*x1 + 0.00901*x3*x2 + 0.0294*x3*x3
%    ) - 0.000766*x1 - 3.42e-5*x2 + 0.000189*x3) + x4 = 0.040357;
%
%e3:    x1 + x2 + x3 - x4 >= 210;

function test_ipopt

  auxdata = {} ;

  options.lb = [ 50, 37.5, 45, -Inf ] ;  % Lower bound on the variables.
  options.ub = [ 200, 150, 180, Inf ] ;  % Upper bound on the variables.

  % The constraint functions are bounded to zero
  options.cl = [ 0, 0 ]; %  constraints
  options.cu = [ 0, Inf ];

  % Set up the auxiliary data.
  options.auxdata = auxdata ;

  % Set the IPOPT options.
  options.ipopt.jac_d_constant   = 'no';
  options.ipopt.hessian_constant = 'no';
  options.ipopt.mu_strategy      = 'adaptive';
  options.ipopt.max_iter         = 400;
  options.ipopt.tol              = 1e-10;

  % The callback functions.
  funcs.objective         = @objective;
  funcs.constraints       = @constraints;
  funcs.gradient          = @gradient;
  funcs.jacobian          = @jacobian;
  funcs.jacobianstructure = @jacobianstructure;
  if true
    funcs.hessian           = @hessian;
    funcs.hessianstructure  = @hessianstructure;
    options.ipopt.derivative_test = 'second-order';
  else
    options.ipopt.hessian_approximation      = 'limited-memory';
    %options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
    %options.ipopt.limited_memory_update_type = 'sr1' ;
    options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
  end

  % Run IPOPT.
  x0 = [50, 37.5, 45, 0] ;

  tic
  [x, info] = ipopt_auxdata(x0,funcs,options);
  elapsed = toc ;

  info;

  x

end

%%
% map the indices with the corresponding index in the spase matrix
function f = objective(x,auxdata)
  f = 0.00533*x(1)^2 + 11.669*x(1) + ...
      0.00889*x(2)^2 + 10.333*x(2) + ...
      0.00741*x(3)^2 + 10.833*x(3) + 653.1 ;
end

%%
% map the indices with the corresponding index in the spase matrix
function g = gradient(x,auxdata)
  g = [ 0.01066*x(1) + 11.669, 0.01778*x(2) + 10.333, 0.01482*x(3) + 10.833, 0 ] ;
end

function f = constraints(x,auxdata)
  f = zeros(2,1) ;
  f(1) = - (0.01*(0.0676*x(1)^2 + 0.00953*x(1)*x(2) - 0.00507*x(1)*x(3) + ...
                  0.00953*x(2)*x(1) + 0.0521*x(2)^2 + 0.00901*x(2)*x(3) - ...
                  0.00507*x(3)*x(1) + 0.00901*x(3)*x(2) + 0.0294*x(3)*x(3) ) ...
         - 0.000766*x(1) - 3.42e-5*x(2) + 0.000189*x(3)) + x(4) - 0.040357 ; % = 0
  f(2) = x(1) + x(2) + x(3) - x(4) - 210 ; % >= 0
end

function jac = jacobian(x,auxdata)
  jac = [ -0.001352*x(1) - 0.0001906*x(2) + 0.0001014*x(3) + 0.000766, ...
          -0.0001906*x(1) - 0.001042*x(2) - 0.0001802*x(3) + 0.0000342, ...
           0.0001014*x(1) - 0.0001802*x(2) - 0.000588*x(3) - 0.000189, ...
           1 ; 1, 1, 1, -1 ] ;
  jac = sparse(jac) ;
end

function jac = jacobianstructure(auxdata)
  jac = sparse(ones(2,4)) ;
end

function H = hessian(x, sigma, lambda, auxdata)
  H1 = [ 0.01066 0       0       0 ; ...
         0       0.01778 0       0 ; ...
         0       0       0.01482 0 ; ...
         0       0       0       0 ] ;
  H2 = [ -0.1352e-2,          0,         0, 0 ; ...
         -0.1906e-3, -0.1042e-2,         0, 0 ; ...
          0.0001014, -0.1802e-3, -0.588e-3, 0 ; ...
          0,                  0,         0, 0 ] ;
  H = sparse(sigma*H1 + lambda(1)*H2) ;
end

function H = hessianstructure(auxdata)
  H = sparse([ 1 0 0 0 ; 1 1 0 0 ; 1 1 1 0 ; 1 1 1 1 ]) ;
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR:  Landmark Based Registration, TPS
%
% - load data (see setup2DhandData)
% - setup  viewer (viewImage2D), interpolator (splineInter),
% - setup landmarks (LM)
% - run TPS
%==============================================================================

clear, close all, help(mfilename)

%% setup hand data
setup2DhandData

if FAIRinput('','set new landmarks ? ',0),
  [LM,fig] = getLandmarks(dataT,dataR,omega,m);
  close(fig);
end;

omegaT = omega(1,:);
omegaR = omega(end,:);
xT = getCellCenteredGrid(omegaT,m);
xR = getCellCenteredGrid(omegaR,m);
Tc = imgModel(dataT,omegaT,xT);
Rc = imgModel(dataR,omegaR,xR);

%% visualize data
FAIRfigure(1,'figname',mfilename); clf;
subplot(1,3,1); viewImage(Tc,omegaT,m); hold on;
ph = plotLM(LM(:,1:2),'numbering','on','color','r');
set(ph,'linewidth',2,'markersize',20);
title(sprintf('%s','T&LM'),'fontsize',20);

subplot(1,3,2); viewImage(Rc,omegaR,m); hold on;
ph = plotLM(LM(:,3:4),'numbering','on','color','g','marker','+');
set(ph,'linewidth',2,'markersize',20);
title(sprintf('%s','R&LM'),'fontsize',20);

%% run TPS registration
[yc,LM] = LMreg('TPS',LM(:,1:4),xR);
TLM = imgModel(dataT,omegaT,yc);

subplot(1,3,3); cla; viewImage(TLM,omegaR,m); hold on;
ph = plotLM(LM(:,3:4),'numbering','off','color','g','marker','+');
qh = plotLM(LM(:,7:8),'numbering','off','color','m','marker','x');
rh = plot(LM(:,[3,7])',LM(:,[4,8])','m-','linewidth',3);
set([ph;qh;rh],'linewidth',2,'markersize',20);
title(sprintf('%s','T(y^{TPS})&LM'),'fontsize',20);
%==============================================================================


function [range_h,range_w,reg_window]=init_regwindow(sz,target_sz,params)
        reg_scale =target_sz;
        use_sz = sz;
        reg_window = ones(use_sz) * params.reg_window_max;
        range = zeros(numel(reg_scale), 2);

        % determine the target center and range in the regularization windows
        for j = 1:numel(reg_scale)
            range(j,:) = [0, reg_scale(j) - 1] - floor(reg_scale(j) / 2);
        end
        center = floor((use_sz + 1)/ 2) + mod(use_sz + 1,2);
        range_h = (center(1)+ range(1,1)) : (center(1) + range(1,2));
        range_w = (center(2)+ range(2,1)) : (center(2) + range(2,2));
        reg_window(range_h, range_w) = params.reg_window_min;