function [pE, pC, x] = tapas_rdcm_spm_dcm_fmri_priors(A, B, C, D)
% Returns the priors for a two-state DCM for fMRI.
% FORMAT:[pE,pC,x] = spm_dcm_fmri_priors(A,B,C,D,options)
%
%   options.two_state:  (0 or 1) one or two states per region
%   options.endogenous: (0 or 1) exogenous or endogenous fluctuations
%
% INPUT:
%    A,B,C,D - constraints on connections (1 - present, 0 - absent)
%
% OUTPUT:
%    pE     - prior expectations (connections and hemodynamic)
%    pC     - prior covariances  (connections and hemodynamic)
%    x      - prior (initial) states
%__________________________________________________________________________
%
% References for state equations:
% 1. Marreiros AC, Kiebel SJ, Friston KJ. Dynamic causal modelling for
%    fMRI: a two-state model.
%    Neuroimage. 2008 Jan 1;39(1):269-78.
%
% 2. Stephan KE, Kasper L, Harrison LM, Daunizeau J, den Ouden HE,
%    Breakspear M, Friston KJ. Nonlinear dynamic causal models for fMRI.
%    Neuroimage 42:649-662, 2008.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_fmri_priors.m 4146 2010-12-23 21:01:39Z karl $

% number of regions
%--------------------------------------------------------------------------
n = length(A);

% check options and D (for nonlinear coupling)
%--------------------------------------------------------------------------
try, options.two_state;  catch, options.two_state  = 0; end
try, options.endogenous; catch, options.endogenous = 0; end
try, D;                  catch, D = zeros(n,n,0);       end

% prior (initial) states and shrinkage priors on A for endogenous DCMs
%--------------------------------------------------------------------------
if options.two_state,  x = sparse(n,6); else, x = sparse(n,5); end
if options.endogenous, a = 128;         else, a = 8;           end

% connectivity priors
%==========================================================================
if options.two_state

    % enforce optimisation of intrinsic (I to E) connections
    %----------------------------------------------------------------------
    A     = (A + eye(n,n)) > 0;

    % prior expectations and variances
    %----------------------------------------------------------------------
    pE.A  =  A*32 - 32;
    pE.B  =  B*0;
    pE.C  =  C*0;
    pE.D  =  D*0;

    % prior covariances
    %----------------------------------------------------------------------
    pC.A  =  A/4;
    pC.B  =  B/4;
    pC.C  =  C*4;
    pC.D  =  D/4;

else

    % enforce self-inhibition
    %----------------------------------------------------------------------
    A     =  A > 0;
    A     =  A - diag(diag(A));

    % prior expectations
    %----------------------------------------------------------------------
    pE.A  =  A/(64*n) - eye(n,n)/2;
    pE.B  =  B*0;
    pE.C  =  C*0;
    pE.D  =  D*0;

    % prior covariances
    %----------------------------------------------------------------------
    pC.A  =  A*a/n + eye(n,n)/(8*n);
    pC.B  =  B;
    pC.C  =  C;
    pC.D  =  D;

end

% and add hemodynamic priors
%==========================================================================
pE.transit = sparse(n,1);
pE.decay   = sparse(n,1);
pE.epsilon = sparse(1,1);

pC.transit = sparse(n,1) + exp(-6);
pC.decay   = sparse(n,1) + exp(-6);
pC.epsilon = sparse(1,1) + exp(-6);

return


function c = sinh(a)
% SINH for adiff objects.

c = adiff( sinh(a.x), rowmult(cosh(a.x), a.dx), a.root);


function err = nmf_eucl_dist(X,Y)

err = sum(sum((X-Y).^2));


function A = area(grains,varargin)
% calculates the area of a list of grains
%
% Input
%  grains - @grain2d
%
% Output
%  A  - list of areas (in measurement units)
%

A = zeros(length(grains.poly),1);

poly = grains.poly;
V = grains.V;

for ig = 1:length(poly)
  A(ig) = polySgnArea(V(poly{ig},1),V(poly{ig},2));
end


function retval=statglmeval(action,fn,varargin)
%STATGLMEVAL  Evaluate or test link function in proper environment
%   STATGLMEVAL('eval',FN,ARGS,...) evaluates the function FN in an
%   environment in which certain functions such as LOGIT and
%   D_LOGIT are defined.  This allows the function FN to be either
%   a user-defined function or one of the pre-defined functions
%   provided in the GLMFIT function, without contaminating the name
%   space with those pre-defined functions.
%
%   STATGLMEVAL('testlink',FN) test for the existence of FN as a
%   function handle, inline function, or text string containing the
%   name of a function in an M-file.

%   Author:  Tom Lane, 3-6-2000
%   Copyright 1993-2002 The MathWorks, Inc.
%   $Revision: 1.3 $  $Date: 2002/02/04 19:25:46 $

switch(action)
 case 'eval'
   retval = feval(fn,varargin{:});

 case 'testlink'
   c = class(fn);
   fnclass = class(@logit);
   if (isequal(c,fnclass) | isequal(c,'inline') | ...
       (isequal(c,'char') & ~isempty(which(fn))))
      retval = 1;
   else
      retval = 0;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%
% The following functions are related to the link function that
% links the distribution parameter mu with the linear combination
% eta of predictor variables.  For example, the logit link defines
%
%        eta = log(mu/(1-mu))

%%%% functions for identity link
function a=identity(b)
a=b;

function a=d_identity(b)
a = ones(size(b));

function b=i_identity(a)
b=a;

%%%% functions for logit link
function a=logit(p)
a = log(p ./ (1-p));

function a=d_logit(p)
a = 1 ./ max(eps, (p .* (1-p)));

function p=i_logit(a)
p = 1 ./ (1 + exp(-a));

%%%% functions for probit link
function a=probit(p)
a = norminv(p);

function a=d_probit(p)
a = 1 ./ max(eps, normpdf(norminv(p)));

function p=i_probit(a)
p = normcdf(a);

%%%% functions for complementary log log link
function a=comploglog(p)
a = log(-log(1-max(eps,p)));

function a=d_comploglog(p)
a = 1 ./ -(max(eps,1-p) .* log(1-max(eps,p)));

function p=i_comploglog(a)
p = 1 - exp(-exp(a));

%%%% functions for log log link
function a=logloglink(p)
a = log(-log(max(eps,p)));

function a=d_logloglink(p)
a = 1 ./ (max(eps, p) .* log(max(eps,p)));

function p=i_logloglink(a)
p = exp(-exp(a));

%%%% functions for log link
function a=d_log(b);
a = 1 ./ max(eps,b);

function b=i_log(a);
b = exp(a);

%%%% functions for reciprocal link
function a=reciprocal(b)
a = 1 ./ max(eps, b);

function a=d_reciprocal(b);
a = -1 ./ max(eps,b).^2;

function b=i_reciprocal(a);
b = 1 ./ max(eps, a);

%%%% functions for power link
function a=power(b,p)
if (p==0)
   a = log(max(eps,b));
else
   a = max(eps,b) .^ p;
end

function a=d_power(b,p);
if (p==0)
   a = 1 ./ max(eps,b);
else
   a = p * max(eps,b).^(p-1);
end

function b=i_power(a,p);
if (p==0)
   b = exp(a);
else
   b = max(eps,a) .^ (1/p);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The following functions define the variance

function a=normalvariance(b)
a = ones(size(b));

function a=poissonvariance(b)
a = b;

function a=binomialvariance(p,N)
a = p .* (1-p) ./ N;

function a=gammavariance(b)
a = b.^2;

function a=inversegaussianvariance(b)
a = b.^3;


function [V,F,TV,TF,obj] = unbake_normal_map(V,F,TV,TF,nim,varargin)
  % UNBAKE_NORMAL_MAP Given a uv-mapped (TV,TF) 3D surface (V,F) mesh and a
  % corresponding normal map image (nim), recover a high-resolution 3D geometry
  % that "explains" the normal map. Resolution (and runtime) is controlled by
  % the resolution of the normal map image (hint: start with a downsampled
  % image).
  %
  % [V,F,TV,TF,obj] = unbake_normal_map(V,F,TV,TF,nim,varargin)
  %
  % Inputs:
  %   V  #V by 3 list of 3D vertex positions
  %   F  #F by 3 list of triangle indices into V
  %   TV  #TV by 3 list of 2D uv vertex positions
  %   TF  #F by 3 list of triangle indices into TV
  %   nim  #nim by #nim by 3 image of *object-space* normals (in range [0,1])
  % Outupts:
  %   V  #V by 3 list of 3D vertex positions
  %   F  #F by 3 list of triangle indices into V
  %   TV  #TV by 3 list of 2D uv vertex positions
  %   TF  #F by 3 list of triangle indices into TV
  %
  % See also: normal_to_displacement_map, tangent_to_object_normal_map
  %

  function [sel,V,F,TV,TF] = upsample_helper(V,F,TV,TF)
    mean_area = mean(doublearea(TV,TF));
    pixel_area = 1./(size(nim,1)*size(nim,2));
    dblA = doublearea(TV,TF);
    sel = find(dblA>pixel_area);
    if isempty(sel)
      return;
    end
    % Selected edges on 3D mesh (V,F)
    [uE,~,EMAP] = unique(sort(reshape(F(:,[2 3 1 3 1 2]),[],2),2),'rows');
    uEM = sparse(EMAP,1,repmat(sparse(sel,1,1,size(F,1),1),3,1),size(uE,1),1)>0;
    MF = full(reshape(uEM(EMAP),[],3));
    % Selected edges on texture mesh (TV,TF)
    [uE,~,EMAP] = unique(sort(reshape(TF(:,[2 3 1 3 1 2]),[],2),2),'rows');
    uEM = sparse(EMAP,1,repmat(sparse(sel,1,1,size(TF,1),1),3,1),size(uE,1),1)>0;
    MTF = full(reshape(uEM(EMAP),[],3));
    M = MF | MTF;
    [TV,TF] = upsample(TV,TF,'OnlySelected',M);
    mprev = size(F,1);
    [V,F] = upsample(V,F,'OnlySelected',M);
    if ~quiet
      fprintf('#F: %g -> %g\n',mprev,size(F,1));
    end
  end

  max_iters = 1000;
  quiet = true;
  quiet = false;
  max_step_size_seen = -inf;
  method = 'lbfgs';
  progressive_upsampling = true;
  fix_symmetry = false;

  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'FixSymmetry','Method','Quiet','Progressive'}, ...
    {'fix_symmetry','method','quiet','progressive_upsampling'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  % Progressive Upsampling is better: similar amount of time for a far better
  % solution.
  if ~progressive_upsampling
    while true
      [sel,V,F,TV,TF] = upsample_helper(V,F,TV,TF);
      if isempty(sel)
        break;
      end
    end
  end

  cross2 = @(a,b,c) ...
    [a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
     a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
     a(:,1).*b(:,2)-a(:,2).*b(:,1)];
  nrmlev = @(p1,p2,p3) cross2(p2-p1,p3-p1);
  unrml = @(V,F) normalizerow(nrmlev(V(F(:,1),:),V(F(:,2),:),V(F(:,3),:)));

  while true
    % Compute normal averaged over all texture-space pixels landing in each
    % triangle: ~/uniform quadrature in texture space
    [X,Y] = meshgrid(linspace(0,1,size(nim,2)),linspace(1,0,size(nim,1)));
    I = in_element_aabb(TV(:,1:2),TF,[X(:) Y(:)]);
    Nnim = reshape(nim,[],3);
    TNsum = sparse( ...
      repmat(I(I>0),1,3),repmat(1:3,sum(I>0),1),Nnim(I>0,:),size(TF,1),3);
    TNcount =  sparse(I(I>0),1,1,size(TF,1),1);
    TN = normalizerow(full(TNsum./TNcount)*2-1);
    % Some triangles might not have received any samples
    N = unrml(V,F);
    TN(any(isnan(TN),2),:) = N(any(isnan(TN),2),:);
    A = diag(sparse(doublearea(V,F)));
    if fix_symmetry
      % flip if aggregate normal points in opposite direction from initial
      % normal
      D = A*sum(TN.*N,2);
      [C,CF] = connected_components(TF);
      CD = accumarray(CF',D)./accumarray(CF',diag(A));
      %tsurf(TF,[TV(:,1:2) C'],'CData',1*(CD(CF)<0.8));
      %T = normals(TV(:,1:2)*eye(2,3),TF);
      %tsurf(TF,[TV(:,1:2) C'],'CData',1*(T(:,3)>0));
      flipped = (CD(CF)<0.8);
      tsurf(TF(flipped,:),[TV(:,1:2) C'],'FaceVertexCData',N(flipped,:)*0.5+0.5);
      hold on;
      surf(X,Y,0*X,falpha(1,0),'FaceColor','texturemap','CData',nim);
      hold off;
      %TN(D<0,:) = -TN(D<0,:);
      error
    end
    ATN = A*TN;

    vec = @(X) X(:);
    Asqr = @(X) sum(sum(X.*(A*X)));
    obj_fun = @(V) Asqr(unrml(V,F)-TN);
    grad_fun = @(V) normal_gradient(V,F,vec(A*unrml(V,F))-vec(ATN));

    r3 = @(X) reshape(X,[],3);
    % This is awful...
    %options = optimoptions('fminunc','SpecifyObjectiveGradient',true,'Display','iter');
    %[V,obj1] = fminunc(@(V) deal(obj_fun(r3(V)),grad_fun(r3(V))),vec(V),options);

    switch method
    case 'lbfgs'
      % This thing actually seems to work
      [V,obj1] = fminlbfgs( ...
        @(V) fun_and_grad(V,@(V) obj_fun(r3(V)),@(V) grad_fun(r3(V))),vec(V), ...
        struct('Display','final','GradObj','on','TolFun',1e-6,'TolX',1e-5));
    case 'gd'
      %% My gradient descent is faster
      %[V,obj1] = fminlbfgs( ...
      %  @(V) fun_and_grad(V,@(V) obj_fun(r3(V)),@(V) grad_fun(r3(V))),vec(V), ...
      %  struct('HessUpdate','steepdesc','Display','final','GradObj','on','TolX',5e-5));
      [V,obj1] = fmingd( ...
        @(V) fun_and_grad(V,@(V) obj_fun(r3(V)),@(V) grad_fun(r3(V))),vec(V));
    end
    V = r3(V);

    %obj1 = obj_fun(V);
    %prev_step = 0;
    %step_size = 1;
    %prev_V = V;
    %for iter = 1:max_iters
    %  %method = 'gradient-descent';
    %  %method = 'stochastic-gradient-descent';
    %  %method = 'momentum';
    %  method = 'nesterov';
    %  %method = 'gauss-newton';
    %  %method = 'levenberg-marquardt';
    %  switch method
    %  case {'gradient-descent','gauss-newton','levenberg-marquardt','momentum'}
    %    % Recompute current normals and gradients
    %    dNdVT = normal_gradient(V,F);
    %    grad = dNdVT*(vec(A*unrml(V,F))-vec(ATN));
    %    switch method
    %    case {'gauss-newton','levenberg-marquardt'}
    %      max_step_size = 100;
    %      % This works really well for small meshes but takes forever/diverges for
    %      % big meshes
    %      K = (dNdVT*A*dNdVT');
    %      switch method
    %      case 'levenberg-marquardt'
    %        K = (dNdVT*A*dNdVT');
    %        T = diag(diag(K));
    %        lambda = 1;
    %        K = K + lambda*T;
    %      end
    %      % This works really well for small meshes but takes forever/diverges for
    %      % big meshes
    %      step = reshape(K\(-grad),[],3);
    %    case 'gradient-descent'
    %      % Gradient descent
    %      step = -reshape(grad,[],3);
    %      max_step_size = 1;
    %    case 'momentum'
    %      % Fairly sensitive to mu
    %      mu = 0.5;
    %      step = mu*prev_step + (1-mu)*(-reshape(grad,[],3));
    %      % works better if I use the non line search step here...
    %      prev_step = step;
    %      max_step_size = 1;
    %    end
    %  case 'nesterov'
    %    % following AQP paper
    %    eta = 10;
    %    theta = (1-sqrt(1/eta))/(1+sqrt(1/eta));
    %    V = (1+theta)*V - theta*prev_V;
    %    dNdVT = normal_gradient(V,F);
    %    grad = dNdVT*(vec(A*unrml(V,F))-vec(ATN));
    %    step = -reshape(grad,[],3);
    %    max_step_size = 1;
    %  case 'stochastic-gradient-descent'
    %    % not impressed with this... maybe it'll be worthwhile if to keep things
    %    % in memory
    %    %
    %    % Especially doesn't seem to play well with the "stalling" criteria.
    %    batch_size = min(size(F,1),10000);
    %    P = randperm(size(F,1));
    %    P = P(1:batch_size);
    %    dNdVT = normal_gradient(V,F(P,:));
    %    grad = dNdVT*(vec(A(P,P)*unrml(V,F(P,:)))-vec(ATN(P,:)));
    %    step = -reshape(grad,[],3);
    %    max_step_size = 1;
    %  end
    %  prev_V = V;
    %  [step_size,V,obj,step] = line_search(obj_fun,@(V) V,V,step,max_step_size);
    %  max_step_size_seen = max(max_step_size_seen,step_size);
    %  V = reshape(V,[],3);
    %  if step_size == 0
    %    warning('step size == 0');
    %    break;
    %  end
    %
    %  obj0 = obj1;
    %  obj1 = obj_fun(V);
    %  if ~quiet
    %    fprintf('  iter: % 5d, obj: %g\n',iter,obj1);
    %  end
    %  if obj0 - obj1 < 1e-5
    %    break
    %  end
    %end
    %if ~quiet
    %  fprintf('converged after %d iterations to %g\n',iter,obj1);
    %end

    if ~quiet
      tsurf(F,V,fsoft,'FaceVertexCData',repmat(orange,size(F,1),1),falpha(1,0.1));
      view(2);axis equal;camlight;
      %apply_ambient_occlusion([],'AddLights',false,'Factor',1);
      drawnow;
    end

    if progressive_upsampling
      [sel,V,F,TV,TF] = upsample_helper(V,F,TV,TF);
      if isempty(sel)
        break;
      end
    else
      % only one iteration needed: we upsampled everything at the beginning
      break;
    end
  end

  obj = obj_fun(V);
  if ~quiet
    fprintf('Final objective: %g\n',obj);
  end

  if ~quiet
    fprintf('max_step_size_seen: %g\n',max_step_size_seen);
  end

end


function a = i4mat_perm ( n, a, p )

%*****************************************************************************80
%
%% I4MAT_PERM permutes the rows and columns of a square I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    A Nijenhuis, H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer A(N,N), the matrix to be permuted.
%
%    Input, integer P(N), the permutation.  P(I) is the new number of
%    row and column I.
%
%    Output, integer A(N,N), the permuted matrix.
%
  ierror = perm1_check ( n, p );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_PERM - Fatal error!\n' );
    fprintf ( 1, '  PERM1_CHECK says permutation is illegal.\n' );
    error ( 'I4MAT_PERM - Fatal error!' );
  end

  p = perm_cycle ( n, p, is, nc, 1 );

  for i = 1 : n

    i1 = -p(i);

    if ( 0 < i1 )

      lc = 0;

      while ( 1 )

        i1 = p(i1);
        lc = lc + 1;

        if ( i1 <= 0 )
          break
        end

      end

      i1 = i;

      for j = 1 : n

        if ( p(j) <= 0 )

          j2 = j;
          k = lc;

          while ( 1 )

            j1 = j2;
            it = a(i1,j1);

            while ( 1 )

              i1 = abs ( p(i1) );
              j1 = abs ( p(j1) );

              temp = it;
              it = a(i1,j1);
              a(i1,j1) = it;

              if ( j1 ~= j2 )
                continue
              end

              k = k - 1;

              if ( i1 == i )
                break
              end

            end

            j2 = abs ( p(j2) );

            if ( k == 0 )
              break
            end

          end

        end

      end

    end

  end
%
%  Restore the positive signs of the data.
%
  p(1:n) = abs ( p(1:n) );

  return
end


%% Convex Hull demo
% We learn how to get hull contours and draw them.
%
% In this sample you will learn how to use the OpenCV function
% <matlab:doc('cv.convexHull') cv.convexHull>.
%
% Sources:
%
% * <https://docs.opencv.org/3.4.0/d7/d1d/tutorial_hull.html>
% * <https://github.com/opencv/opencv/blob/3.4.0/samples/cpp/tutorial_code/ShapeDescriptors/hull_demo.cpp>
%

function varargout = hull_demo_gui(im)
    % load source image
    if nargin < 1
        src = cv.imread(fullfile(mexopencv.root(),'test','stuff.jpg'));
    elseif ischar(im)
        src = imread(im);
    else
        src = im;
    end

    % Convert image to gray and blur it
    if size(src,3) == 3
        src = cv.cvtColor(src, 'RGB2GRAY');
    end
    src = cv.blur(src, 'KSize',[5 5]);

    % create the UI
    h = buildGUI(src);
    if nargout > 0, varargout{1} = h; end
end

function onChange(~,~,h)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    thresh = round(get(h.slid, 'Value'));
    set(h.txt, 'String',sprintf('Threshold: %3d',thresh));

    % Detect edges using Threshold
    threshold_output = cv.threshold(h.src, thresh, ...
        'MaxValue',255, 'Type','Binary');

    % Find contours
    contours = cv.findContours(threshold_output, ...
        'Mode','Tree', 'Method','Simple');

    % Find the convex hull object for each contour
    hull = cell(size(contours));
    for i=1:numel(contours)
        hull{i} = cv.convexHull(contours{i}, 'Clockwise',false);
    end

    % Draw contours + hull results
    drawing = zeros([size(threshold_output) 3], 'uint8');
    for i=1:numel(contours)
        clr = randi([0 255], [1 3], 'uint8');
        drawing = cv.drawContours(drawing, contours, ...
            'ContourIdx',i-1, 'MaxLevel',0, ...
            'Color',clr, 'Thickness',1, 'LineType',8);
        drawing = cv.drawContours(drawing, hull, ...
            'ContourIdx',i-1, 'MaxLevel',0, ...
            'Color',clr, 'Thickness',2, 'LineType',8);
    end

    % show result
    set(h.img, 'CData',drawing);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    thresh = 100;
    max_thresh = 255;
    sz = size(img);
    sz(2) = max(sz(2), 250);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Convex Hull Demo', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+29]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 30 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(img, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(img);
    end
    h.txt = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 5 130 20], 'String',sprintf('Threshold: %3d',thresh));
    h.slid = uicontrol('Parent',h.fig, 'Style','slider', 'Value',thresh, ...
        'Min',0, 'Max',max_thresh, 'SliderStep',[1 10]./(max_thresh-0), ...
        'Position',[135 5 sz(2)-135-5 20]);

    % hook event handlers, and trigger default start
    set(h.slid, 'Callback',{@onChange,h}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h);
end


function [ KRlm ] = AMICO_RotateKernel( K, AUX, idx_IN, idx_OUT, isIsotropic )

	if ( isIsotropic == false )
		% fit SH and rotate kernel to 181*181 directions
		KRlm = zeros( size(AUX.fit,1), 181, 181, 'single' );
		for ox = 1:181
		for oy = 1:181
			Ylm_rot = AUX.Ylm_rot{ ox, oy };
			for s = 1 : numel(idx_IN)
				Klm = AUX.fit * K( idx_IN{s} );		% fit SH of shell to rotate
				Rlm = zeros( size(Klm) );
				idx = 1;
				for l = 0 : 2 : AUX.lmax
					const = sqrt(4.0*pi/(2.0*l+1.0)) * Klm( (l*l + l + 2.0)/2.0 );
					for m = -l : l
						Rlm(idx) = const * Ylm_rot(idx);
						idx = idx+1;
					end
				end
				KRlm( idx_OUT{s}, ox, oy ) = single( Rlm );
			end
		end
		end
	else
		% simply fit SH
		KRlm = zeros( size(AUX.fit,1), 1, 1, 'single' );
		Ylm_rot = AUX.Ylm_rot{ 1, 1 };
		for s = 1:numel(idx_IN)
			KRlm( idx_OUT{s}, 1, 1 ) = single( AUX.fit * K( idx_IN{s} ) );
		end
end


function S = globMatrixVIFE3DStiff(fun,mesh,fem1,fem2)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initializaiton
if strcmp(fem1.type,'P1')||strcmp(fem1.type,'DGP1')||strcmp(fem1.type,'CR')
    feEvalBas1 = @evalP1Bas3D;
elseif strcmp(fem1.type,'P2')||strcmp(fem1.type,'DGP2')
    feEvalBas1 = @evalP2Bas3D;
end

if strcmp(fem2.type,'P1')||strcmp(fem2.type,'DGP1')||strcmp(fem2.type,'CR')
    feEvalBas2 = @evalP1Bas3D;
elseif strcmp(fem2.type,'P2')||strcmp(fem2.type,'DGP2')
    feEvalBas2 = @evalP2Bas3D;
end

%% 1. Matrix on noninterface elements
dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
ntID = find(mesh.tLoc > 0); ntN = length(ntID);
AN = fem1.area(ntID);
gxN = fem1.gx(ntID,:); gyN = fem1.gy(ntID,:); gzN = fem1.gz(ntID,:); gw = fem1.gw;
XN = zeros(nloc*ntN, 1);

coefN = feval(fun,gxN,gyN,gzN);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);
for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, [1,0,0]);
    Ibasy{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, [0,1,0]);
    Ibasz{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, [0,0,1]);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, [1,0,0]);
    Jbasy{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, [0,1,0]);
    Jbasz{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, [0,0,1]);
end

IN = reshape(repmat(fem1.t(ntID,:),4,1),nloc*ntN,1);
JN = repmat(reshape(fem2.t(ntID,:),dof2*ntN,1),4,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XN(ind+1:ind+ntN) = AN.*(sum(((Ibasx{i}.*(coefN.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coefN.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coefN.*Jbasz{j})).*gw'),2));
        ind = ind + ntN;
    end
end
ID = find(XN~=0);
Ndof = size([mesh.p;mesh.eIntP],1);
S = sparse(IN(ID),JN(ID),XN(ID),Ndof,Ndof);


function y = atanh(x)
%ATANH        Implements  atanh(x)  for intervals
%
%   y = atanh(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, sparse input,
%                                  major revision, improved accuracy
% modified 09/02/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 12/04/05     S.M. Rump  'realstdfctsexcptnignore' added and some
%                                     improvements, tocmplx replaced by cintval
% modified 09/06/07     S.M. Rump  approximate std fcts removed, exceptional arguments
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/18/08     S.M. Rump  StdFctsException ignore/NaN
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,atanh(full(sx)),m,n);
    return
  end

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

  if x.complex
    y = log( 2./(1-x) - 1 )/2;
    if rndold
      setround(rndold)
    end
    return
  end

  % input x real and full
  % real range of definition:  [-1,1]
  INTLAB_STDFCTS_EXCPTN = getappdata(0,'INTLAB_STDFCTS_EXCPTN');
  indexneg = ( x.inf<-1 );               % (partially) exceptional indices
  indexpos = ( x.sup>1 );                % (partially) exceptional indices
  if ~isempty(find(indexneg)) | ~isempty(find(indexpos)) % handle input out-of-range
    if INTLAB_STDFCTS_EXCPTN<=1   % out-of-range input handled as complex
      if INTLAB_STDFCTS_EXCPTN==1
        warning('ATANH: Real interval input out of range changed to be complex')
      end
      y = x;
      index = indexneg | indexpos;
      %VVVV  y(index) = atanh(cintval(x(index)));
      s.type = '()'; s.subs = {index}; y = subsasgn(y,s,atanh(cintval(subsref(x,s))));
      %AAAA  Matlab bug fix
      index = ~index;
      if any(index(:))
        %VVVV  y(index) = atanh(x(index));
        s.type = '()'; s.subs = {index}; y = subsasgn(y,s,atanh(subsref(x,s)));
        %AAAA  Matlab bug fix
      end
      if rndold
        setround(rndold)
      end
      return
    end
    setappdata(0,'INTLAB_STDFCTS_EXCPTN_',1);
    if INTLAB_STDFCTS_EXCPTN==3    % ignore input out of range (ignore-mode)
      x.inf(indexneg) = -1;               % completely exceptional indices treated below
      x.sup(indexpos) = 1;
      index = ( x.sup<-1 ) | ( x.inf>1 ); % completely exceptional indices
    end
  else
    index = [];                           % make sure Index is not undefined
  end

  % input x real and full
  y = x;
  wng = warning;
  warning off

  % treat non-exceptional arguments
  xinf = x.inf(:);
  xsup = x.sup(:);

  IndexInfPos = ( xinf>=0 );
  len1 = sum(IndexInfPos);
  IndexSupNeg = ( xsup<=0 );
  len2 = sum(IndexSupNeg);

  Y = atanh_pos( [ xinf(IndexInfPos) ; -xsup(IndexSupNeg) ] , -1 );
  y.inf(IndexInfPos) = Y(1:len1);
  y.sup(IndexSupNeg) = -Y( len1+1 : end );

  IndexInfNeg = ( xinf<0 );
  len1 = sum(IndexInfNeg);
  IndexSupPos = ( xsup>0 );
  len2 = sum(IndexSupPos);

  Y = atanh_pos( [ -xinf(IndexInfNeg) ; xsup(IndexSupPos) ] , 1 );
  y.inf(IndexInfNeg) = -Y(1:len1);
  y.sup(IndexSupPos) = Y( len1+1 : len1+len2 );

  y.inf(xinf==-1) = -inf;
  y.sup(xsup==-1) = -inf;
  y.inf(xinf==1) = inf;
  y.sup(xsup==1) = inf;

  if INTLAB_STDFCTS_EXCPTN==3      % ignore input out of range (ignore-mode)
    if ~isempty(find(index))              % completely exceptional arguments to NaN
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  else                                    % any input out of range to NaN (NaN-mode)
    index = indexneg | indexpos;
    if ~isempty(find(index))              % exceptional arguments to NaN
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  end

  setround(rndold)
  warning(wng)


% This file is part of the project NILM-Eval (https://github.com/beckel/nilm-eval).
% Licence: GPL 2.0 (http://www.gnu.org/licenses/gpl-2.0.html)
% Copyright: ETH Zurich, 2014
% Author: Romano Cicchetti

function HMMs = generateHMMsFromSnippets(snippets)

    % generate HMMs using snippets

    HMMs = struct;
    numOfSnippets = length(snippets.mean);
    HMMs.mean = cell(numOfSnippets,1);
    HMMs.std = cell(numOfSnippets,1);
    HMMs.transition = cell(numOfSnippets,1);
    snippetsNumOfStates = cellfun('length', snippets.mean) + 1;
    for i = 1:length(snippets.mean)
        numOfStates = snippetsNumOfStates(i);
        HMMs.mean{i} = [cell2mat(snippets.mean(i)); 0];
        HMMs.std{i} = [cell2mat(snippets.std(i))'; 1];
        transition = zeros(numOfStates);
        for state = 1:numOfStates
            if state == numOfStates
               transition(state, state) = 1;
            else
               duration = cell2mat(snippets.duration(i));
               transition(state, state) = 1 - 1/duration(state);
               transition(state, mod(state, numOfStates) + 1) = 1/duration(state);
            end
        end
        HMMs.transition{i} = transition;
    end

end


% acsobiro() - A. Chickocki's robust Second-Order Blind Identification (SOBI)
%              by joint diagonalization of the time-delayed covariance matrices.
%              NOTE: THIS CODE ASSUMES TEMPORALLY CORRELATED SIGNALS.
%              Thus, the estimated time-delayed covariance matrices
%              for at least some time delays must be nonsingular.
%
% Usage:  >>   [H] = acsobiro(X);
%         >> [H,S] = acsobiro(X,n,p);
% Inputs:
%         X - data matrix of dimension [m,N] where
%                    m is the number of sensors
%                    N is the number of samples
%         n - number of sources {Default: n=m}
%         p - number of correlation matrices to be diagonalized {default: 100}
%             For noisy data, use at least 100 time delays.
% Outputs:
%         H - matrix of dimension [m,n] an estimate of the *mixing* matrix
%         S - matrix of dimension [n,N] an estimate of the source activities
%             where  >> X [m,N] = H [m,n] * S [n,N]
%
% Authors: Implemented and improved by A. Cichocki on the basis of
%          the classical SOBI algorithm of Belouchrani and publications of:
%            A. Belouchrani et al., F. Cardoso et al.,
%            S. Choi, S. Cruces, S. Amari, and P. Georgiev
%          For references: see function body
%
% Note: Extended by Arnaud Delorme and Scott Makeig to process data epochs
%       (computes the correlation matrix respecting epoch boundaries).

% REFERENCES:
%  A. Belouchrani, K. Abed-Meraim, J.-F. Cardoso, and E. Moulines, ``Second-order
%  blind separation of temporally correlated sources,'' in Proc. Int. Conf. on
%  Digital Sig. Proc., (Cyprus), pp. 346--351, 1993.
%
%  A. Belouchrani, and A. Cichocki,
%  Robust whitening procedure in blind source separation context,
%  Electronics Letters, Vol. 36, No. 24, 2000, pp. 2050-2053.
%
%  A. Cichocki and S. Amari,
%  Adaptive Blind Signal and Image Processing, Wiley,  2003.

function [H,S,D]=acsobiro(X,n,p),

[m,N,ntrials]=size(X);
if nargin<1 | nargin > 3

  help acsobiro

elseif nargin==1,

 DEFAULT_LAGS = 100;
 n=m; % source detection (hum...)
 p=min(DEFAULT_LAGS,ceil(N/3)); % number of time delayed correlation matrices to be diagonalized
                                % Note: For noisy data, use at least p=100.
elseif nargin==2,

 p=min(DEFAULT_LAGS,ceil(N/3)); % number of correlation matrices to be diagonalized

end;

X(:,:)=X(:,:)-(mean(X(:,:)')'*ones(1,N*ntrials));        % Remove data means

for t = 1:ntrials
    if t == 1
        Rxx=(X(:,1:N-1,t)*X(:,2:N,t)')/(N-1)/ntrials; % Estimate the sample covariance matrix
                                          % for the time delay p=1, to reduce influence
                                          % of white noise.
    else
        Rxx=Rxx+(X(:,1:N-1,t)*X(:,2:N,t)')/(N-1)/ntrials; % Estimate the sample covariance matrix
                                          % for the time delay p=1, to reduce influence
                                          % of white noise.
    end;
end;

[Ux,Dx,Vx]=svd(Rxx);
        Dx=diag(Dx);

if n<m, % under assumption of additive white noise and when the number
         % of sources is known, or can be estimated a priori
  Dx=Dx-real((mean(Dx(n+1:m))));
  Q= diag(real(sqrt(1./Dx(1:n))))*Ux(:,1:n)';
else    % under assumption of no additive noise and when the
        % number of sources is unknown
   n=max(find(Dx>1e-99)); % detect the number of sources
   fprintf('acsobiro(): Estimated number of sources is %d\n',n);
   Q= diag(real(sqrt(1./Dx(1:n))))*Ux(:,1:n)';
end;
Xb = zeros(size(X));
Xb(:,:)=Q*X(:,:); % prewhitened data

% Estimate the time delayed covariance matrices:
 k=1;
 pn=p*n; % for convenience
 for u=1:m:pn,
   k=k+1;
   for t = 1:ntrials
       if t == 1
           Rxp=Xb(:,k:N,t)*Xb(:,1:N-k+1,t)'/(N-k+1)/ntrials;
       else
           Rxp=Rxp+Xb(:,k:N,t)*Xb(:,1:N-k+1,t)'/(N-k+1)/ntrials;
       end;
   end;
   M(:,u:u+m-1)=norm(Rxp,'fro')*Rxp;  % Frobenius norm =
 end;                                  % sqrt(sum(diag(Rxp'*Rxp)))

% Approximate joint diagonalization:
eps=1/sqrt(N)/100; encore=1; U=eye(n);
while encore, encore=0;
 for p=1:n-1,
  for q=p+1:n,
    % Givens rotations:
    g=[ M(p,p:n:pn)-M(q,q:n:pn)  ;
        M(p,q:n:pn)+M(q,p:n:pn)  ;
        i*(M(q,p:n:pn)-M(p,q:n:pn))];
   [Ucp,D] = eig(real(g*g')); [la,K]=sort(diag(D));
   angles=Ucp(:,K(3));angles=sign(angles(1))*angles;
   c=sqrt(0.5+angles(1)/2);
   sr=0.5*(angles(2)-j*angles(3))/c; sc=conj(sr);
   asr = abs(sr)>eps ;
   encore=encore | asr ;
   if asr , % Update the M and U matrices:
     colp=M(:,p:n:pn);
     colq=M(:,q:n:pn);
     M(:,p:n:pn)=c*colp+sr*colq;
     M(:,q:n:pn)=c*colq-sc*colp;
     rowp=M(p,:);
     rowq=M(q,:);
     M(p,:)=c*rowp+sc*rowq;
     M(q,:)=c*rowq-sr*rowp;
     temp=U(:,p);
     U(:,p)=c*U(:,p)+sr*U(:,q);
     U(:,q)=c*U(:,q)-sc*temp;
   end  %% if
  end  %% q loop
 end  %% p loop
end  %% while

% Estimate the mixing matrix H
H= pinv(Q)*U(1:n,1:n);

% Estimate the source activities S
if nargout>1
  S=[];
  W=U(1:n,1:n)'*Q;
  S= W*X(:,:);
end


function dx = l2lossBackward(x,r,p )

dx = 2*p*(x-r);

dx = dx / (size(x,1)*size(x,2));

end


Ie=imread('background05_empty.jpg');
%imshow(Ie);

I=imread('background05.jpg');
% imshow(I);
dI=double(rgb2gray(Ie))-double(rgb2gray(I));

%imshow(dI,[min(dI(:))  max(dI(:))]);

tr=0.45*max(max(abs(dI(:))));
% tr=0.1*max(abs(dI(:)));

bw=abs(dI)>tr;

%imshow(bw);

bw(226,426:428)=false;
bw(227,328)=false;
% imshow(bw);

L = bwlabel(bw,4);
map=rand(max(L(:))+1,3);
%imshow(L+1,map);

ns=max(L(:)); % number of initial segments

% find rectangles of initial segments:
six=zeros(ns,1);
siy=zeros(ns,1); % positions
six2=zeros(ns,1);
siy2=zeros(ns,1); % positions
for c=1:ns
    bwt=(L==c);
    [yt xt]=find(bwt);
    x1=min(xt);
    x2=max(xt);
    y1=min(yt);
    y2=max(yt);
    six(c)=x1;
    siy(c)=y1;
    six2(c)=x2;
    siy2(c)=y2;
end

[sy sx tmp3]=size(Ie);

%in_rect=false(sy,sx,ns);
in_rect=false(sy*sx,ns);
waitbar(0,'in rect');
for yc=1:sy
    for xc=1:sx
        for c=1:ns
            if (siy(c)<=yc)&&(yc<=siy2(c))&&(six(c)<=xc)&&(xc<=six2(c))
                %in_rect(yc,xc,c)=true;
                in_rect(yc+(xc-1)*sy,c)=true;
            end
        end
    end
    waitbar(yc/sy);
end

in_rect_u=unique(in_rect,'rows'); % unicue pixels class
% in_rect_u(class_counter,:) - what segments used

nsr0=size(in_rect_u,1); % new number of segments
nsr=nsr0-1;

sixr=zeros(nsr,1);
siyr=zeros(nsr,1); % positions
six2r=zeros(nsr,1);
siy2r=zeros(nsr,1); % positions

% find limits:
%in_rect(pixel_number,segment_number)
%for cr=1:nsr
tic
waitbar(0,'new limits');
for cr1=1:nsr % exlude big
    cr=cr1+1;
    in_rect_u_t=in_rect_u(cr,:); % in_rect_u_t(segment_number)
    sgu=find(in_rect_u_t); % what segments used

    rect=false(sy,sx);

    for yc=1:sy
        for xc=1:sx
            %if all(in_rect_u_t==())
            pn=yc+(xc-1)*sy; % pixel number
            %in_rect(pn,:) - in wat seggments curent pixel
            if all(in_rect_u_t==in_rect(pn,:))
                rect(yc,xc)=true;
            end
        end
    end

    [py px]=find(rect);
    siyr(cr1)=min(py);
    siy2r(cr1)=max(py);
    sixr(cr1)=min(px);
    six2r(cr1)=max(px);
    waitbar(cr1/nsr);
end

sir=cell(nsr,1); % small images
sibwr=cell(nsr,1); % small images, masks
for cr1=1:nsr % exlude big
    cr=cr1+1;
    y1=siyr(cr1);
    y2=siy2r(cr1);
    x1=sixr(cr1);
    x2=six2r(cr1);
    rect=false(sy,sx);
    rect(y1:y2,x1:x2)=true;
    bwt=false(sy,sx);
    bwt(rect)=bw(rect);
    It=trim_with_mask(I,bwt);
    sir{cr1}=It(y1:y2,x1:x2,:);
    sibwr{cr1}=bwt(y1:y2,x1:x2);
end

% rename back:
six=sixr;
siy=siyr;
six2=six2r;
siy2=siy2r;

si=sir;
sibw=sibwr;

save('segments','si','sibw','six','siy','six2','siy2');


function r = times(a,b)
%TIMES        Gradient multiplication  a .* b
%

% written  10/16/98     S.M. Rump
% modified 11/30/98     S.M. Rump  array/scalar operations
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    improved performance
%                                    complete redesign
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 12/05/05     S.M. Rump  improved performance (thanks to J. Kubitz)
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% 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

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  if ~isa(a,'gradient')
    m = prod(size(a));
    if m==1                     % non-gradient scalar .* gradient
      r.x = a * b.x;
      r.dx = a * b.dx;
    else                        % non-gradient array .* gradient
      n = prod(size(b.x));
      if n==1                   % non-gradient array .* gradient scalar
        r.x = a * b.x;
        ax = a(:);
        if issparse(b.dx)
          ax = sparse(ax);
        end
        r.dx = ax * b.dx;
      else                      % non-gradient array .* gradient array
        if ~isequal(size(a),size(b))
          error('gradient .* : dimensions not compatible')
        end
        r.x = a .* b.x;
        if issparse(b.dx)
          [ib,jb,sb] = find(b.dx);
          r.dx = sparse(ib,jb,reshape(a(ib),size(sb)).*sb,n,N);
        else
          r.dx = b.dx .* repmat(a(:),1,N);
        end
      end
    end
  elseif ~isa(b,'gradient')      % gradient times non-gradient
    r = b .* a;
    if rndold
      setround(rndold)
    end
    return
  else                          % both factors gradient
    m = prod(size(a.x));
    n = prod(size(b.x));
    sparse_ = issparse(a.dx) | issparse(b.dx);
    if m==1                     % scalar gradient .* gradient
      if n==1                   % scalar gradient .* scalar gradient
        r.x = a.x * b.x;
        r.dx = a.dx * b.x + a.x * b.dx;
      else                      % scalar gradient .* array gradient
        r.x = a.x * b.x;
        if sparse_
          r.dx = sparse(b.x(:)) * a.dx + b.dx * a.x;
        else
          r.dx = b.x(:) * a.dx + b.dx * a.x;
        end
      end
    else                        % array gradient .* gradient
      if n==1                   % array gradient .* scalar gradient
        r.x = a.x * b.x;
        if sparse_
          r.dx = sparse(a.x(:)) * b.dx + a.dx * b.x;
        else
          r.dx = a.x(:) * b.dx + a.dx * b.x;
        end
      else                      % array gradient .* array gradient
        if ~isequal(size(a),size(b))
          error('dimensions not compatible for gradient .*');
        end
        r.x = a.x .* b.x;
        if issparse(a.dx) | issparse(b.dx)
          r.dx = sparse(1:n,1:n,b.x(:))*a.dx + sparse(1:n,1:n,a.x(:))*b.dx;
        else
          r.dx = repmat(b.x(:),1,N) .* a.dx + b.dx .* repmat(a.x(:),1,N);
        end
      end
    end
  end

  r = class(r,'gradient');

  if rndold
    setround(rndold)
  end


%% fnc_GetIndex: give the index of the element in the vector that
%%               corresponds to the set of inputs
%
% Usage:
%   i = fnc_GetIndex(C)
%
% Inputs:
%    C                array of input indexes
%
% Output:
%     i               index in the vector that contains all the variances
%
% ------------------------------------------------------------------------
% See also
%
% Author : Flavio Cannavo'
% e-mail: flavio(dot)cannavo(at)gmail(dot)com
% Release: 1.0
% Date   : 29-01-2011
%
% History:
% 1.0  29-01-2011  First release.
%%

function i = fnc_GetIndex(C)

i = sum(2.^(C-1));


function SO3VF = cross(SO3VF1, SO3VF2, varargin)
% pointwise cross product
%
% Syntax
%   SO3VF = cross(SO3VF1, SO3VF2)
%   SO3VF = cross(v, SO3VF2)
%   SO3VF = cross(SO3VF1, v)
%
% Input
%   SO3VF1, SO3VF2 - @SO3VectorField
%   v - @vector3d
%
% Output
%   SO3VF - @SO3VectorField
%

if isa(SO3VF2, 'vector3d') && length(SO3VF2)==1
  SO3VF = SO3VectorFieldHandle(@(rot) cross(SO3VF1.eval(rot),SO3VF2),SO3VF1.CS,SO3VF1.SS);
  if SO3VF2.antipodal
    SO3VF = abs(SO3VF);
  end
  return
end

if isa(SO3VF1, 'vector3d')
  SO3VF = -cross(SO3VF2,SO3VF1);
  return
end

ensureCompatibleSymmetries(SO3VF1,SO3VF2)
SO3VF = SO3VectorFieldHandle(@(rot) cross(SO3VF1.eval(rot),SO3VF2.eval(rot)),SO3VF1.CS,SO3VF1.SS);

end


function [ end_index, min_index ] = getTrough( time_slice, begin_index, threshold )
%GETTROUGH Get trough for cyclonic eddy
%   Detailed explanation goes here

end_index = begin_index;
n = length(time_slice);
while end_index < n && time_slice(end_index) >= time_slice(end_index+1) + threshold
    end_index = end_index + 1;
end

min_index = end_index;

while end_index < n && (time_slice(end_index) - time_slice(end_index+1)) <= threshold
    end_index = end_index + 1;
end

end


function [theta,P,res]=JPLAY(X,Y,G,L,k,d,sigma,alfa,beta,gama,rho,maxiter,eta,epsilon)

% Joint & Progressive Learning Strategy (J-PLAY)

% Usage:
%       [theta,P] = JPLAY(X,Y,G,L,k,d,sigma,alfa,beta,gama,maxiter,eta)

%% Input:
%       X      -Input data: d*N (d: dimension, N: the number of sample)
%       Y      -Label matrix: c*N (c: the number of class),
%                             e.g. if class==1 then y=[1,0,0,...,0]'
%                                  if class==2 then y=[0,1,0,...,0]'
%       G      -Adjacency matrix: N*N
%       L      -Laplacian matrix: N*N
%       k      -The number of nearest neighbor
%       d      -Reduced dimension
%     sigma    -Gaussian kernel parameter used in constructing graph
%     alfa     -Reconstruction loss parameter (default: 1)
%     beta     -Manifold regularization parameter (default: 0.1)
%     gama     -Ridge regression parameter  (default: 0.1)
%    maxiter   -Maximum number of iterations (default: 1000)
%     eta      -Eta in AutoRULe (default: 0.1)

%% Output:
%     theta    -A series of coupled-projection matrices
%       P      -Property-labeled projection matrix

% (c) Danfeng Hong, Remote Sensing Technology Institute (IMF), German Aerospace Center (DLR), Germany.
%                   Singnal Processing in Earth Oberservation (SiPEO), Technical University of Munich (TUM), Germany.
%     Naoto Yokoya, Remote Sensing Technology Institute (IMF), German Aerospace Center (DLR), Germany.
%                   Singnal Processing in Earth Oberservation (SiPEO), Technical University of Munich (TUM), Germany.
%                   Research Center for Advanced Science and Technology, The Univerisity of Tokyo, Japan.
%     danfeng.hong@dlr.de
%     naoto.yokoya@dlr.de; yokoya@sal.rcast.u-tokyo.ac.jp

if nargin < 2
    error('Please specify label information!')
end;

if nargin < 3
    error('Please construct adjacency matrix!')
end;

if nargin < 4
    error('Please construct Laplacian matrix!')
end;

if nargin < 5
    error('Please specify the number of nearest neighbor!')
end;

if nargin < 6
    error('Please specify subspace dimensions!')
end;

if nargin < 7
    error('Please specify Gaussian kernel parameter used in constructing graph!')
end;

if nargin < 8 || isempty(alfa)
    alfa=1;
end;

if nargin < 9 || isempty(beta)
    beta=0.1;
end;

if nargin < 10 || isempty(gama)
    gama=0.1;
end;

if nargin < 11 || isempty(maxiter)
    maxiter=1000;
end;

if nargin < 12 || isempty(eta)
    eta=0.1;
end;

%% Parameters Setting
% epsilon = 1e-3; % Tolerance error 1e-3Houston2018
iter=1;
stop = false;
res=zeros(1,maxiter); % Residuals
Num=length(d); % The number of layers

%% JPL: Initialization-step
theta=cell(1,1);
X0=X;
for i=1:Num
       theta0=DR_LPP(X0,k,d(i),sigma,G);
        theta_init=AutoRULe(theta0'*X0,X0,L,eta,theta0',maxiter);
        X0=theta_init*X0;
        theta{1,i}=theta_init;
end

%% JPL: Fine-tuning parameters
while ~stop && iter < maxiter+1

    %% Solve W
    D=X;
    for i=1:Num
        D=theta{1,i}*D;
    end
    P=(alfa*(Y*D'))/(alfa*(D*D')+gama*eye(size(D*D')));

    %% Solve the group of theta
    for j=1:Num

        %give W
        Pi=P;
        if j<Num
            for z=Num:-1:j+1
                Pi=Pi*theta{1,z};
            end
        end

        %give H
        H=X;
        for m=1:j
            H=theta{1,m}*H;
        end

        %give X
        Xi=X;
%         XWi=XW;
        if j>1
           for n=1:j-1
               Xi=theta{1,n}*Xi;
%                XWi=theta{1,n}*XWi;
           end
        end

        theta{1,j}=Theta_ADMM(Y,Pi,H,Xi,L,alfa,beta,theta{1,j},rho,maxiter);
    end

    %% Compute the vaules of objection function
     ErrorTerm=X;
%      ErrorTerm1=XW;
     ManifoldTerm=0;
%      ManifoldTerm1=0;
     ReconstructionTerm=0;
     for r=1:Num
         ReconstructionTerm=ReconstructionTerm+(norm(ErrorTerm-theta{1,r}'*theta{1,r}*ErrorTerm,'fro')^2);
         ErrorTerm=theta{1,r}*ErrorTerm;
%          ErrorTerm1=theta{1,r}*ErrorTerm1;
         ManifoldTerm=ManifoldTerm+trace(ErrorTerm*L*ErrorTerm');
%          ManifoldTerm1=ManifoldTerm1+trace(ErrorTerm1*L1*ErrorTerm1');
     end
     res(1,iter)=0.5*alfa*(norm(Y-P*ErrorTerm,'fro')^2)+0.5*beta*(ManifoldTerm)+0.5*gama*(norm(P,'fro')^2)+0.5*(ReconstructionTerm);

    %% Check the convergence condition
    if iter>1
       r_Obj=abs(res(1,iter)-res(1,iter-1))/res(1,iter-1);
       if r_Obj<epsilon
            stop = true;
            fprintf(' i = %f,res_Obj= %f\n',iter,r_Obj);
            break;
       end

       if mod(iter,10) == 1
           fprintf(' i = %f,res_Obj= %f\n',iter,r_Obj);
       end
    end

    iter=iter+1;
end
end


function [B,R] = FindRotation(X, bit)

UX = randn(size(X,1),bit);
UX = sign(UX);
UX(UX<=0)=0;
UX = normalize(UX);

% optimization
for i=1:1
    % find rotation
    C = UX' * X;
    [UB,~,UA] = svds(double(C),bit);
    %[UB,~,UA] = lansvd(double(C),bit,'L');

    R = UA * UB';

    % find B
    Z = X*R;
    UX = ones(size(Z)).*0;
    for j=1:size(Z,1)
        [b] = findBestBinary(Z(j,:));
        UX(j,:) = b./sqrt(sum(b));
    end
end

B = UX;


function [ data ] = sortWalls( data )
%SORTWALLS re-order the walls based on their orientation
% and delete the surround relations with walls

wallind = strmatch('wall',data.labellist,'exact');
wallfront = data.obblist([4,6],wallind);
wallidx = 1:size(wallfront,2);

% 1st floor
delta = wallfront - repmat([1;0],1,size(wallfront,2));
delta = delta.*delta;
delta = delta(1,:)+delta(2,:);
[~,I] = min(delta);
fid1 = wallidx(I);
ind = find([1:size(wallfront,2)]~=I);
wallfront = wallfront(:,ind);
wallidx = wallidx(ind);

% 2st floor
delta = wallfront - repmat([0;1],1,size(wallfront,2));
delta = delta.*delta;
delta = delta(1,:)+delta(2,:);
[~,I] = min(delta);
fid2 = wallidx(I);
ind = find([1:size(wallfront,2)]~=I);
wallfront = wallfront(:,ind);
wallidx = wallidx(ind);

% 3st floor
delta = wallfront - repmat([-1;0],1,size(wallfront,2));
delta = delta.*delta;
delta = delta(1,:)+delta(2,:);
[~,I] = min(delta);
fid3 = wallidx(I);
ind = find([1:size(wallfront,2)]~=I);
wallfront = wallfront(:,ind);
wallidx = wallidx(ind);

% 4st floor
delta = wallfront - repmat([0;-1],1,size(wallfront,2));
delta = delta.*delta;
delta = delta(1,:)+delta(2,:);
[~,I] = min(delta);
fid4 = wallidx(I);
ind = find([1:size(wallfront,2)]~=I);
wallfront = wallfront(:,ind);
wallidx = wallidx(ind);

wallorder = [fid1,fid2,fid3,fid4];
walllist = data.obblist(:,wallind);
data.walllist = walllist(:,wallorder);
data.obblist(:,wallind) = walllist(:,wallorder);

% update the Rmat and Dmat
wallRmat = data.Rmat(:,wallind);
wallRmat = wallRmat(:,wallorder);
data.Rmat(:,wallind) = wallRmat;
data.Rmat(wallind,:) = wallRmat';

wallDmat = data.Dmat(:,wallind);
wallDmat = wallDmat(:,wallorder);
data.Dmat(:,wallind) = wallDmat;
data.Dmat(wallind,:) = wallDmat';
%
% %% all the walls should be supported by the floor
% floorid = strmatch('floor',data.labellist,'exact');
% data.Rmat(wallind,floorid) = -2;
% data.Rmat(floorid,wallind) = -1;

%% delete the surround relations with walls
% nsur = {};
% for i = 1:length(data.sur)
%     list = data.sur{i};
%     ind = intersect(list,wallind);
%     if(length(ind)==0)
%         nsur{length(nsur)+1} = list;
%     end
% end
% data.sur = nsur;

%% floor is not supported by any object
% ind = find(data.Rmat(:,floorid)==-1|data.Rmat(:,floorid)==-3);
% data.Rmat(ind,floorid) = -2;
% data.Rmat(floorid,ind) = -1;

end


% 5g ldpc encoding
% input: s, bit sequence of dimension K * 1
% output: encoded_bits, bit sequence
% reference: 3GPP TS 38.212 section 5.3.2
% author: Xiao, Shaoning 萧少宁
% license: MIT

function [encoded_bits, H, Z_c, encoded_bits_original] = ldpc_encode(s, base_graph_index)

K = length(s);

encoded_bits = zeros(3*K, 1);

if base_graph_index == 1
    a = 4;
    b = 22;
    c = 26;
    d = 42;
    e = 46;
    z = K/b;
    Z_c = z;
    N = 66 * Z_c;
    z = K/b;
    set_index = lifting_size_table_lookup(z);
    load parity_check_matrices_protocol_1
    BG = parity_check_matrices_protocol_1(:, :, set_index); %#ok<NODEF>
elseif base_graph_index == 2
    a = 4;
    b = 10;
    c = 14;
    d = 38;
    e = 42;
    z = K/b;
    Z_c = z;
    N = 50 * Z_c;
    set_index = lifting_size_table_lookup(z);
    load parity_check_matrices_protocol_2
    BG = parity_check_matrices_protocol_2(:, :, set_index); %#ok<NODEF>
else
  error('wrong base graph index in ldpc encoding.');
end

BG(BG ~= -1) = mod(BG(BG ~= -1), Z_c);

for k = (2*Z_c):(K-1)
  if s(k+1) ~= -1
    encoded_bits(k-2*Z_c+1) = s(k+1);
  else
    s(k+1) = 0;
    encoded_bits(k-2*Z_c+1) = -1;
  end
end

% set_index = lifting_size_table_lookup(Z_c);
% BG = parity_check_matrices_protocol(:, :, set_index);

A_prime = BG(1:a, 1:b);
B_prime = BG(1:a, (b+1):c);
C_prime = BG((a+1):e, 1:b);
D_prime = BG((a+1):e, (b+1):c);

z = Z_c;

A = spalloc(a*z, b*z, nnz(A_prime + ones(size(A_prime))));

for row_index = 1:a
    for column_index = 1:b
        if A_prime(row_index, column_index) ~= -1
            A((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(A_prime(row_index, column_index), z)+1):z, 1:mod(A_prime(row_index, column_index), z)], ones(1, z), z, z);
        end
    end
end

B = spalloc(a*z, a*z, nnz(B_prime + ones(size(B_prime))));

for row_index = 1:a
    for column_index = 1:a
        if B_prime(row_index, column_index) ~= -1
            B((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(B_prime(row_index, column_index), z)+1):z, 1:mod(B_prime(row_index, column_index), z)], ones(1, z), z, z);
        end
    end
end

C = spalloc(d*z, b*z, nnz(C_prime + ones(size(C_prime))));

for row_index = 1:d
    for column_index = 1:b
        if C_prime(row_index, column_index) ~= -1
            C((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(C_prime(row_index, column_index), z)+1):z, 1:mod(C_prime(row_index, column_index), z)], ones(1, z), z, z);
        end
    end
end

D = spalloc(d*z, a*z, nnz(D_prime + ones(size(D_prime))));

for row_index = 1:d
    for column_index = 1:a
        if D_prime(row_index, column_index) ~= -1
            D((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(D_prime(row_index, column_index), z)+1):z, 1:mod(D_prime(row_index, column_index), z)], ones(1, z), z, z);
        else
            D((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = spalloc(z, z, 0);
        end
    end
end

B_inv = spalloc(a*z, a*z, 20*z);

if (base_graph_index == 1) && (set_index ~= 7)
    B_inv(1:z, 1:z)             = speye(z);
    B_inv(1:z, 1+z:2*z)         = speye(z);
    B_inv(1:z, 1+2*z:3*z)       = speye(z);
    B_inv(1:z, 1+3*z:4*z)       = speye(z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
elseif (base_graph_index == 2) && ((set_index ~= 4) && (set_index ~= 8))
    B_inv(1:z, 1:z)             = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+z:2*z)         = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+2*z:3*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+3*z:4*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
elseif (base_graph_index == 1) && (z == 208)
    B_inv(1:z, 1:z)             = sparse(circshift(eye(208), 105));
    B_inv(1:z, 1+z:2*z)         = sparse(circshift(eye(208), 105));
    B_inv(1:z, 1+2*z:3*z)       = sparse(circshift(eye(208), 105));
    B_inv(1:z, 1+3*z:4*z)       = sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1:z)         = speye(208) + sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1:z)       = sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1+z:2*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1+2*z:3*z) = speye(208) + sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1+3*z:4*z) = speye(208) + sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1:z)       = sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(208) + sparse(circshift(eye(208), 105));
elseif (base_graph_index == 1) && ((z ~= 208) && (set_index == 7))
    B_inv(1:z, 1:z)             = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+z:2*z)         = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+2*z:3*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+3*z:4*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
elseif (base_graph_index == 2) && ((set_index == 4) || (set_index == 8))
    B_inv(1:z, 1:z)             = speye(z);
    B_inv(1:z, 1+z:2*z)         = speye(z);
    B_inv(1:z, 1+2*z:3*z)       = speye(z);
    B_inv(1:z, 1+3*z:4*z)       = speye(z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
end

s = s(:);

p_1 = mod(B_inv * (A * s), 2);
p_2 = mod(C * s + D * p_1, 2);

w = [p_1; p_2];

for k = K:(N+2*Z_c-1)
   encoded_bits(k-2*Z_c+1) = w(k-K+1);
end

H = [A, B, spalloc(a*z, d*z, 0); C, D, speye(d*z)];

encoded_bits_original = [s; w];

% mod(H * [s; p_1; p_2]) = 0

clear A
clear B
clear C
clear D
clear B_inv
clear A_prime
clear B_prime
clear C_prime
clear D_prime

end


function [L,S,G,error,time]=GreBackground(X,rank,tau,power,isize,tol,k)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Background Modeling by Greedy Semi-Soft GoDec (GreBsmo)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%isize: image size of each frame, for example, 1024x768
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Tianyi Zhou, 2013, All rights reserved.

[L,S,error,time]=GreGoDec(X,rank,tau,tol,power,k);
G=X-L-S;

figure;
for i=1:min([200,size(X,2)])
    subplot(2,2,1);imagesc(reshape(X(:,i),isize));colormap(gray);axis image;axis off;title('X(Sample)');
    subplot(2,2,2);imagesc(reshape(L(:,i),isize));colormap(gray);axis image;axis off;title('L(Low-rank)');
    %subplot(1,4,3);imagesc(reshape(X(:,i).*(~~S(:,i)),isize));colormap(gray);axis image;axis off;title('S(Sparse)');
    subplot(2,2,4);imagesc(reshape(S(:,i),isize));colormap(gray);axis image;axis off;title('S(Sparse)');
    subplot(2,2,3);imagesc(reshape(G(:,i),isize));colormap(gray);axis image;axis off;title('G(Noise)');
    pause(0.1);
    if i<10
        print('-djpeg', '-r250', sprintf('%s%s','00',num2str(i)));
    elseif i<100
        print('-djpeg', '-r250', sprintf('%s%s','0',num2str(i)));
    else
        print('-djpeg', '-r250', sprintf('%s',num2str(i)));
    end
end


function [ftAllNew,transMdl] = ftTrans_mida(ftAll,domainFtAll,target,...
	maLabeled,param)
%Maximum Independence Domain Adaptation (MIDA)
%
%	A feaure-level transfer learning (domain adaptation) algorithm which
% augments the features then learns a domain-invariant subspace.
% Application scope:
%	+ two or multiple discrete domains
%	+ continuous distributional change
%	+ labeled or unlabeled or partially labeled source domain
%	+ labeled or unlabeled or partially labeled target domain
%	+ label type: classification or regression
%	There is no need to distinguish which domain a sample is from. The domain
% information is contained in the variable domainFtAll, which contains the
% domain features of all samples.
%	Domain features indicate the background of samples. For example, if
% the samples are from md discrete domains, then domainFtAll can be a
% n-by-md matrix, domainFtAll(i,j)=1 if sample i is from the j'th domain, 0
% else. If the samples have a time order and their distribution changes
% continuously along with time, then domainFtAll can be n-by-1 and
% domainFtAll(i) is the "time" of sample i. Multiple background info can be
% integrated into domainFtAll in the similar way. See the ref for details.
%
% ftAll:	All samples in all domains. n-by-m matrix, n is the number of
%	samples, m is the dimension of features.
% domainFtAll:	Domain features of all samples. n-by-md matrix, md is the
%	dimension of domain features.
% target:	When some samples in any domain are labeled, their labels can
%	be provided in this variable to enhance the discriminative power of the
%	the learned features. ntr-by-1 matrix, ntr is the number of labeled
%	samples. Classification problems should use class indices as labels,
%	i.e. target(i)=j if the i'th labeled sample is from the j'th class.
% maLabeled:	Mask for the labeled samples. n-by-1 matrix,
%	maLabeled(i)=true if sample i is labeled, false else.
%
% param: Struct of hyper-parameters, please see the first cell of this
%	program ("default parameters") for details. You can set parameter p to
%	x by setting param.p = x. For parameters that are not set, default
%	values will be used.

% ftAllNew:	All samples in the learned subspace.
% transMdl:	A struct containing the model, transMdl.W is the projection
%	matrix.

% ref: Ke Yan, Lu Kou, and David Zhang, "Domain Adaptation via Maximum
%	Independence of Domain Features," http://arxiv.org/abs/1603.04535
% Copyright 2016 Ke YAN, Tsinghua Univ. http://yanke23.com , xjed09@gmail.com

%% default parameters
isRegress = 0; % 0 for classification problem, 1 for regression
kerName = 'lin'; % kernel name, see the next cell ("kernels")
kerSigma = 10; % kernel parameter, see the next cell ("kernels")
bSmida = true; % 0 for MIDA if no label information is considered for all
	% samples, 1 for semisupervised MIDA (SMIDA) if some labels are
	% considered.
doSample = false; % when there are too many unlabeled data, eigenvalue
	% decomposition can be slow. Setting this variable to true makes the
	% code to sample some unlabeled data.
nSmpRatio = 1; % if doSample=true, the number of unlabeled data to sample
	% will be ceil(ntr*nSmpRatio)
mu = 1; % the weight of the variance term, see the ref
m = 30; % the dimension of the subspace
gamma = .1; % the weight of the supervised term, only useful in SMIDA, see
	% the ref
ftAugType = 1; % feature augmentation, 0: no aug; 1: aug with domainFt;
	% 2: frustratingly easy aug, only for discrete domains, see the ref

defParam % set user-defined hyper-parameters

%% kernels
nm = @(X,p)repmat(sum(X.^2,2),1,p);
linKer = @(X1,X2)X1*X2';
rbfKer = @(X1,X2)exp(-(nm(X1,size(X2,1))+nm(X2,size(X1,1))'-2*X1*X2')/2/kerSigma^2);
lapKer = @(X1,X2)exp(-pdist2(X1,X2)/kerSigma);
polyKer = @(X1,X2)(1+kerSigma*X1*X2').^2;
if strcmpi(kerName,'lin'), kerFun = linKer; % linear kernel
elseif strcmpi(kerName,'poly'), kerFun = polyKer; % polynomial kernel
elseif strcmpi(kerName,'rbf'), kerFun = rbfKer;
elseif strcmpi(kerName,'lap'), kerFun = lapKer; % Laplacian kernel
else error('unknown kernel'); end

%% sort samples
ntr = nnz(maLabeled);
nAll = size(ftAll,1);
Df = double(domainFtAll);
% Df = zscore(domainFtAll); % bad
if doSample && nSmpRatio<inf % sample some unlabeled data
	rng(0)
	nUnlabeledUsed = min(nAll-ntr,ceil(ntr*nSmpRatio));
	id = randperm(nAll-ntr,nUnlabeledUsed); % rand sel
	idTest = find(~maLabeled);
	ftUsed = [ftAll(maLabeled,:);ftAll(idTest(id),:)];
	dfUsed = [Df(maLabeled,:);Df(idTest(id),:)];
	nUsed = ntr+nUnlabeledUsed;
else
	ftUsed = [ftAll(maLabeled,:);ftAll(~maLabeled,:)];
	dfUsed = [Df(maLabeled,:);Df(~maLabeled,:)];
	nUsed = nAll;
end
Kd = linKer(dfUsed,dfUsed);

%% feature augmentation
if ftAugType==0
	ftUsedAug = ftUsed;
elseif ftAugType==1
	ftUsedAug = [ftUsed,dfUsed];
elseif ftAugType==2
	nFt = size(ftUsed,2);
	nDomain = size(dfUsed,2);
	ftUsedAug = zeros(nUsed,nFt*(nDomain+1));
	ftUsedAug(:,1:nFt) = ftUsed;
	for p = 1:nDomain
		ftUsedAug(dfUsed(:,p)==1,nFt*p+1:nFt*(p+1)) = ftUsed(dfUsed(:,p)==1,:);
	end
end

%% compute
Kx = kerFun(ftUsedAug,ftUsedAug);
H = eye(nUsed)-ones(nUsed)/(nUsed);

if ~bSmida % MIDA

	A = Kx*(mu*H-H*Kd*H)*Kx;

else % SMIDA

	target = double(target);
	if isRegress==0 && all(target==floor(target)) % classification
		Ky = repmat(target,1,ntr)==repmat(target,1,ntr)';
% 		Ky = (Kyy-.5)*2; % no need
	else % regression
		target = zscore(target);
		Ky = target*target'; % linear ker
	end

	Kyy_tilde = zeros(nUsed);
	Kyy_tilde(1:ntr,1:ntr) = Ky;

	A = Kx*(H*(-Kd+gamma*Kyy_tilde)*H+mu*H)*Kx;
end

A = (A+A')/2; % to compensate float num error
[V,D] = eig(A);
[D,I] = sort(diag(real(D)),'descend');
transMdl.W = real(V(:,I(1:m)));

%% project the samples
if ftAugType==0
	ftAllAug = ftAll;
elseif ftAugType==1
	ftAllAug = [ftAll,Df];
elseif ftAugType==2
	ftAllAug = zeros(nUsed,nFt*(nDomain+1));
	ftAllAug(:,1:nFt) = ftAll;
	for p = 1:nDomain
		ftAllAug(Df(:,p)==1,nFt*p+1:nFt*(p+1)) = ftAll(Df(:,p)==1,:);
	end
end

KxAll = kerFun(ftAllAug,ftUsedAug);
ftAllNew = KxAll*transMdl.W;

end


function laguerre_general_values_test ( )

%*****************************************************************************80
%
%% LAGUERRE_GENERAL_VALUES_TEST demonstrates the use of LAGUERRE_GENERAL_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGUERRE_GENERAL_VALUES_TEST:\n' );
  fprintf ( 1, '  LAGUERRE_GENERAL_VALUES stores values of\n' );
  fprintf ( 1, '  the generalized Laguerre function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N     A    X             L(N,A)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, a, x, fx ] = laguerre_general_values ( n_data );

    if ( n_data == 0 )
      break
    end

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

  end

  return
end


% Reconstruction of 2D Cartesian Pulseq data
% provides an example on how data reordering can be detected from the MR
% sequence with almost no additional prior knowledge
%
% it loads Matlab .mat files with the rawdata in the format
%     adclen x channels x readouts
% if Matlab .mat file not available it attempt to load Siemens .dat (which needs mapVBVD in the path)
% but first it seeks the accompanying .seq file with the same name to interpret
%     the data

%% Load the latest file from the specified directory
path='../IceNIH_RawSend/'; % directory to be scanned for data files
%path='/data/Dropbox/ismrm2021pulseq_liveDemo/dataLive/Vienna_7T_Siemens'; % directory to be scanned for data files

pattern='*.seq';
D=dir([path filesep pattern]);
[~,I]=sort([D(:).datenum]);
seq_file_path=[path filesep D(I(end-0)).name]; % use end-1 to reconstruct the second-last data set, etc...
                                                % or replace I(end-0) with I(1) to process the first dataset, I(2) for the second, etc...
%seq_file_path='../interpreters/siemens/data_example/gre_example.seq'

% keep basic filename without the extension
[p,n,e] = fileparts(seq_file_path);
basic_file_path=fullfile(p,n);

% try loading Matlab data
data_file_path=[basic_file_path '.mat'];
if isfile(data_file_path)
    fprintf(['loading `' data_file_path '´ ...\n']);
    data_unsorted = load(data_file_path);
    if isstruct(data_unsorted)
        fn=fieldnames(data_unsorted);
        assert(length(fn)==1); % we only expect a single variable
        data_unsorted=data_unsorted.(fn{1});
    end
else
    % revert to Siemens .dat file
    data_file_path=[basic_file_path '.dat'];
    fprintf(['loading `' data_file_path '´ ...\n']);
    twix_obj = mapVBVD(data_file_path);
    if iscell(twix_obj)
        data_unsorted = twix_obj{end}.image.unsorted();
    else
        data_unsorted = twix_obj.image.unsorted();
    end
end
[adc_len,channels,readouts]=size(data_unsorted);

%% Load sequence from file
fprintf(['loading `' seq_file_path '´ ...\n']);
seq = mr.Sequence();              % Create a new sequence object
seq.read(seq_file_path,'detectRFuse');
[ktraj_adc, t_adc, ktraj, t_ktraj, t_excitation, t_refocusing] = seq.calculateKspacePP();
figure; plot(ktraj(1,:),ktraj(2,:),'b',...
             ktraj_adc(1,:),ktraj_adc(2,:),'r.'); % a 2D plot
axis('equal'); title('2D kx/ky k-space trajectory');

figure; plot(ktraj(1,:),ktraj(3,:),'b',...
             ktraj_adc(1,:),ktraj_adc(3,:),'r.'); % another 2D plot
axis('equal'); title('2D kx/kz k-space trajectory');

%% Analyze the trajectory data (ktraj_adc)
fprintf('analyzing the k-space trajectory ...\n');
k_extent=max(abs(ktraj_adc),[],2);
k_scale=max(k_extent);
k_threshold=k_scale/5000;

% detect unused dimensions and delete them
if any(k_extent<k_threshold)
    ktraj_adc(k_extent<k_threshold,:)=[]; % delete rows
    k_extent(k_extent<k_threshold)=[];
end

% detect dK, k-space reordering and repetitions (or slices, etc)
kt_sorted=sort(ktraj_adc,2);
dk_all=kt_sorted(:,2:end)-kt_sorted(:,1:(end-1));
dk_all(dk_all<k_threshold)=NaN;
dk_min=min(dk_all,[],2);
dk_max=max(dk_all,[],2);
dk_all(dk_all-dk_min(:,ones(1,size(dk_all,2)))>k_threshold)=NaN;
dk_all_cnt=sum(isfinite(dk_all),2);
dk_all(~isfinite(dk_all))=0;
dk=sum(dk_all,2)./dk_all_cnt;
dk(~isfinite(dk))=0;
[~,k0_ind]=min(sum(ktraj_adc.^2,1));
kindex=round((ktraj_adc-ktraj_adc(:,k0_ind*ones(1,size(ktraj_adc,2))))./dk(:,ones(1,size(ktraj_adc,2))));
kindex(~isfinite(kindex))=0;
kindex_min=min(kindex,[],2);
kindex_mat=kindex-kindex_min(:,ones(1,size(ktraj_adc,2)))+1;
kindex_end=max(kindex_mat,[],2);
sampler=zeros(kindex_end');
repeat=zeros(1,size(ktraj_adc,2));
for i=1:size(kindex_mat,2)
    if (size(kindex_mat,1)==3)
        ind=sub2ind(kindex_end,kindex_mat(1,i),kindex_mat(2,i),kindex_mat(3,i));
    else
        ind=sub2ind(kindex_end,kindex_mat(1,i),kindex_mat(2,i));
    end
    repeat(i)=sampler(ind);
    sampler(ind)=repeat(i)+1;
end
if (max(repeat(:))>0)
    kindex=[kindex;(repeat+1)];
    kindex_mat=[kindex_mat;(repeat+1)];
    kindex_end=max(kindex_mat,[],2);
end
%figure; plot(kindex(1,:),kindex(2,:),'.-');

%% sort the k-space data into the data matrix
% the incoming data order is [kx coils acquisitions]
data_coils_last = permute(data_unsorted, [1, 3, 2]);
data_coils_last = reshape(data_coils_last, [adc_len*readouts, channels]);

data=zeros([kindex_end' channels]);
if (size(kindex,1)==3)
    for i=1:size(kindex,2)
        data(kindex_mat(1,i),kindex_mat(2,i),kindex_mat(3,i),:)=data_coils_last(i,:);
    end
else
    for i=1:size(kindex,2)
        data(kindex_mat(1,i),kindex_mat(2,i),:)=data_coils_last(i,:);
    end
end

if size(kindex,1)==3
    nImages=size(data,3);
else
    nImages=1;
    data=reshape(data, [size(data,1) size(data,2) 1 size(data,3)]); % we need a dummy images/slices dimension
end

%figure; imab(log(abs(data))); title('k-space data');

%% Reconstruct coil images

images = zeros(size(data));
%figure;

for ii = 1:channels
    images(:,:,:,ii) = fftshift(fft2(fftshift(data(:,:,:,ii)))); % 1.4.0. does not need inversion of the read direction
    %for ni = 1:nImages
        %tmp = abs(images(:,:,ni,ii));
        %tmp = tmp./max(tmp(:));
        %imwrite(tmp, ['img_coil_' num2str(ii) '.png'])
    %end
end

% Phase images (possibly channel-by-channel and echo-by-echo)
%figure;imab(angle(images));colormap('jet');
%figure;imab(abs(images));colormap('gray');

%% Image display with optional sum of squares combination
% figure;
% if nCoils>1
%     sos=abs(sum(images.*conj(images),ndims(images))).^(1/2);
%     sos=sos./max(sos(:));
%     imab(sos);
%     %imwrite(sos, ['img_combined.png']
% else
%     imab(abs(images));
% end
% colormap('gray');

%% 3D image now

images3D=fftshift(fft(fftshift(images,3),[],3),3);
if channels>1
    sos3D=abs(sum(images3D.*conj(images3D),ndims(images3D))).^(1/2);
    im3D=sos3D./max(sos3D(:));
else
    im3D=abs(images3D);
end

figure;imab(im3D);colormap('gray');title('all reconstructed image(s)');
figure;imab(im3D(:,:,end/2+1));colormap('gray'); title('central partition');


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function optval = PW_Vega_CallPut(S,K,C,r,sigma,T)
    % S = NSim x 1 matrix of simulated prices
    % K = Strike price
    % C = 1 -> Call; C = 0 -> Put

    if(C==1)
        Indicator = (S(:,end)>K);

    else
        Indicator = (S(:,end) <= K);
    end
    Sval = S(:,end) .* ((log(S(:,end)./S(1,1)) - (r+0.5*sigma^2)*T)/sigma);
    optval =exp(-r*T) * mean(Indicator .*Sval );
end


% This is a test script which demonstrates the usage of the "MofN_TrackInitiatorX" class.
% =========================================================================>

% Load the ground truth data
load('multiple-robot-tracking.mat');

% Plot settings
ShowPlots = 0;              % Set to 0 to hide plots
numTrueTracks = 3;

% Model parameter shortcuts
lambdaV = 10; % Expected number of clutter measurements over entire surveillance region
V = 10^2;     % Volume of surveillance region (10x10 2D-grid)
V_bounds = [0 10 0 10]; % [x_min x_max y_min y_max]
P_D = 1;    % Probability of detection
timestep_duration = duration(0,0,1);

%% Models
transition_model = ConstantVelocityX('VelocityErrVariance', 0.0001,...
                                     'NumDims', 2,...
                                     'TimestepDuration', timestep_duration);
measurement_model = LinearGaussianX('NumMeasDims', 2,...
                                    'NumStateDims', 4,...
                                    'MeasurementErrVariance', 0.02,...
                                    'Mapping', [1 3]);
clutter_model = PoissonRateUniformPositionX('ClutterRate',lambdaV,...
                                            'Limits',[V_bounds(1:2);...
                                                      V_bounds(3:4)]);
detection_model = ConstantDetectionProbabilityX('DetectionProbability',P_D);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,measurement_model,'Clutter',clutter_model, 'Detection', detection_model);

%% Generate DataList
meas_simulator = MultiTargetMeasurementSimulatorX('Model',model);
% meas_simulator.DetectionProbability = 1;
DataList = meas_simulator.simulate(GroundTruthStateSequence);
N = numel(DataList);

%% Base Filter
obs_covar= measurement_model.covar();
PriorState = GaussianStateX(zeros(4,1), transition_model.covar() + blkdiag(obs_covar(1,1), 0, obs_covar(2,2),0));
base_filter = KalmanFilterX('Model', model, 'StatePrior', PriorState);

%% Data Associator
config.ClutterModel = clutter_model;
config.DetectionModel = detection_model;
config.Clusterer = NaiveClustererX();
config.Gater = EllipsoidalGaterX(2,'GateLevel',10)';
config.DetectionProbability = P_D;
jpdaf = JointProbabilisticDataAssocX(config);

%% Track Initiator

% Initiate Data Associator
config_pdaf.ClutterModel = clutter_model;
% config.Clusterer = NaiveClustererX();
config_pdaf.Gater = EllipsoidalGaterX(2,'GateLevel',5);
config_pdaf.DetectionProbability = P_D;
pdaf = ProbabilisticDataAssocX(config_pdaf);

% Initiate Tag Generator
tag_gen = RandSampleTagGeneratorX(1:10000);

% Prepare initiator parameters
config_ti.TagGenerator = tag_gen;
config_ti.InitFilter = base_filter;
config_ti.DataAssociator = pdaf;
config_ti.ConfirmThreshold = [8,10];
config_ti.DeleteThreshold = [5,10];
CovarThreshold = 4*PriorState.Covar;
config_ti.CustomDeleteConditionFcn = ...
    @(x,t) t.Filter.StatePosterior.Covar(1,1)>CovarThreshold(1,1) ...
           || t.Filter.StatePosterior.Covar(3,3)>CovarThreshold(3,3);

% Create the track initiator
myti = MofN_TrackInitiatorX(config_ti);

%% START OF SIMULATION
%  ===================>

% Create figure windows
if(ShowPlots)
    img = imread('maze.png');

    % set the range of the axes
    % The image will be stretched to this.
    min_x = 0;
    max_x = 10;
    min_y = 0;
    max_y = 10;

    % make data to plot - just a line.
    x = min_x:max_x;
    y = (6/8)*x;

    figure('units','normalized','outerposition',[0 0 .5 1])
    ax(1) = gca;
end

TrackList = [];
for k=2:N
    fprintf('Iteration = %d/%d\n================>\n',k,N);

    %% Extract DataList at time k
    MeasurementList = DataList(k);
    timestamp_km1 = DataList(k-1).Timestamp;
    timestamp_k = MeasurementList.Timestamp;
    dt = timestamp_k - timestamp_km1;
    transition_model.TimestepDuration = dt;
    fprintf('Timestamp = %s\n================>\n',timestamp_k);

    %% Process JPDAF
    jpdaf.MeasurementList = MeasurementList;
    jpdaf.TrackList = TrackList;
    jpdaf.predictTracks();
    jpdaf.associate();
    jpdaf.updateTracks();

    %% Perform Track initiation
    [TrackList, TentativeTrackList] = myti.initiateTracks(jpdaf.TrackList, MeasurementList, jpdaf.AssocWeightsMatrix);

    %% Plot update step results
    if(ShowPlots)

        cla(ax(1));
        %imagesc(ax(1),[min_x max_x], [min_y max_y], flipud(img));
        hold on;
        if(exist('data_plot','var'))
            delete(data_plot);
        end
        data_inv = measurement_model.finv(MeasurementList.Vectors);
        data_plot = plot(ax(1), data_inv(1,:), data_inv(3,:),'k*','MarkerSize', 10);

        % Plot confirmed tracks
        for j=1:numel(TrackList)
            means = [TrackList{j}.Trajectory.Mean];
            h2 = plot(ax(1), means(1,:),means(3,:),'-','LineWidth',1);
            h2 = plotgaussellipse(TrackList{j}.Filter.StatePosterior.Mean([1 3]),...
                                  TrackList{j}.Filter.StatePosterior.Covar([1 3],[1 3]),...
                                  'Color','r',...
                                  'Axis',ax(1));
        end

        % Plot tentative tracks
        for j=1:numel(TentativeTrackList)
            h2 = plot(ax(1), TentativeTrackList{j}.Filter.StatePosterior.Mean(1),TentativeTrackList{j}.Filter.StatePosterior.Mean(3),'.','LineWidth',1);
            if j==2
                set(get(get(h2,'Annotation'),'LegendInformation'),'IconDisplayStyle','off');
            end
            h2 = plotgaussellipse(TentativeTrackList{j}.Filter.StatePosterior.Mean([1 3]),...
                                  TentativeTrackList{j}.Filter.StatePosterior.Covar([1 3],[1 3]),...
                                  'Color','g',...
                                  'Axis',ax(1));
        end

        % set the y-axis back to normal.
        set(ax(1),'ydir','normal');
        str = sprintf('Robot positions (Update)');
        title(ax(1),str)
        xlabel('X position (m)')
        ylabel('Y position (m)')
        axis(ax(1),V_bounds)
        pause(0.01)
    end
end


% -------------------------------------------------------------------------------------------------
function rect  = get_axis_aligned_rect(region)
%GETAXISALIGNEDRECT computes axis-aligned rect with same area as the rotated one (REGION)
% -------------------------------------------------------------------------------------------------
if size(region,2) == 8
    cx = mean(region(:,1:2:end),2);
    cy = mean(region(:,2:2:end),2);
    x1 = min(region(:,1:2:end),[],2);
    x2 = max(region(:,1:2:end),[],2);
    y1 = min(region(:,2:2:end),[],2);
    y2 = max(region(:,2:2:end),[],2);
    x1y1x2y2 = region(:,1:2) - region(:,3:4);
    x2y2x3y3 = region(:,3:4) - region(:,5:6);
    A1 = sqrt(sum(x1y1x2y2.*x1y1x2y2,2)).* sqrt(sum(x2y2x3y3.*x2y2x3y3,2));
    A2 = (x2 - x1) .* (y2 - y1);
    s = sqrt(A1./A2);
    w = s .* (x2 - x1) + 1;
    h = s .* (y2 - y1) + 1;
    rect = [cx-w/2,cy-h/2,w,h];
else
    rect = region;
end
end


function [ahm, AHM_rf, AHM_sf, AHM_sk, AHM_sc, AHM_u, AHM_rho] = ...
    retroProjAhmPntFromOmniCamOnRob(Rf, Sf, Sk, Sc, u, n)

% RETROPROJAHMPNTFROMOMNICAMONROB Retro-project ahm from omnicam on robot.
%
%   AHM = RETROPROJAHMPNTINTOOMNICAMONROB(RF, SF, SK, SC, U, N) gives the
%   retroprojected AHM in World Frame from an observed pixel U. RF and SF
%   are Robot and Sensor Frames, Sk and Sc are camera calibration and
%   distortion correction parameters. U is the pixel coordinate and N is
%   the non-observable inverse depth. AHM is a 7-vector :
%     AHM = [X Y Z U V W IDepth]'
%
%   [AHM, AHM_rf, AHM_sf, AHM_k, AHM_c, AHM_u, AHM_n] = ... returns the
%   Jacobians wrt RF.x, SF.x, SK, SC, U and N.
%
%   See also FROMFRAMEAHM.
%

%   Copyright 2012 Grigory Abuladze @ ASL-vision
%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% Frame World -> Robot  :  Rf
% Frame Robot -> Sensor :  Sf

  % AHM in Sensor Frame
  [ahms, AHMS_u, AHMS_rho, AHMS_sk, AHMS_sc] = invOmniCamAhm(u, n, Sk, Sc) ;

  [ahmr, AHMR_sf, AHMR_ahms] = fromFrameAhm(Sf,ahms);
  [ahm , AHM_rf , AHM_ahmr]  = fromFrameAhm(Rf,ahmr);

  AHM_ahms = AHM_ahmr*AHMR_ahms;
  AHM_sk   = AHM_ahms*AHMS_sk ;
  AHM_sf   = AHM_ahmr*AHMR_sf;

  AHM_sc  = AHM_ahms*AHMS_sc ;

  AHM_u   = AHM_ahms*AHMS_u ;
  AHM_rho = AHM_ahms*AHMS_rho ;

end

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function [data_rec] = gtmmapimputate(net, data)

%GTMEXPIMPUTE Impute missing data using GTM MAP estimates
%
%	Description
%	 DATA_REC = GTMEXPIMPUTE(NET, DATA) takes a GTM structure NET, and
%	imputes the missing values in DATA according to MAP estimates of hidden
% variables given observed data.
%
%	See also
%	GTMEXPIMPUTE, GTM, GTMEM, GTMLMEAN, GMLMODE, GMMPROB

% Copyright (c) Tommi Vatanen (2012)

% Check for consistency
errstring = consist(net, 'gtm', data);
if ~isempty(errstring)
  error(errstring);
end

data_rec = data;
missing = isnan(data_rec);

%
net.gmmnet.centres = rbffwd(net.rbfnet, net.X);
R = gmmpost(net.gmmnet, data);

R_tmp = zeros(size(R));
R_max = repmat(max(R,[],2), [1 size(R,2)]);
R_tmp(R==R_max) = 1;

% pick up map reference vectors
rec = R_tmp*net.gmmnet.centres;
data_rec(missing) = rec(missing);


%% load an RGBD sequence
data = loadSUN3Dv2('2014-05-01_19-32-43_260595134347');

%% demo to show how to use a frame
frameID = 11;

%% load the frame data
image = readImage(data,frameID);
depth = readDepth(data,frameID); % use this to read the denoised depth map that is closet to the image

IR = readIR(data,frameID);

%% get 3D point cloud from the depth map
XYZcamera(:,:,1)=data.camera.D.X .* depth;
XYZcamera(:,:,2)=data.camera.D.Y .* depth;
XYZcamera(:,:,3)=depth .* (~isnan(data.camera.D.X) & ~isnan(data.camera.D.Y));
XYZcamera(:,:,4)=depth>0 & ~isnan(data.camera.D.X) & ~isnan(data.camera.D.Y);
valid = logical(XYZcamera(:,:,4));
valid = valid(:)';
XYZ = reshape(XYZcamera,[],4)';
XYZ = XYZ(1:3,valid);

points2ply('demo_all_points.ply', XYZ);

%% get the color for each 3D point by tranforming and projecting each 3D point
XYZrgb = transformPointCloud(XYZ,data.camera.D2RGB.Rt);
xyRGB = 1+project_points2(XYZrgb,data.camera.RGB.fc,data.camera.RGB.cc,data.camera.RGB.kc,data.camera.RGB.alpha_c);
uvRGB = round(xyRGB);
valid = 1<= uvRGB(1,:) & uvRGB(1,:) <= data.camera.RGB.width & 1<= uvRGB(2,:) & uvRGB(2,:) <= data.camera.RGB.height;
XYZ = XYZ(1:3,valid);
ind = sub2ind([data.camera.RGB.height data.camera.RGB.width],uvRGB(2,valid),uvRGB(1,valid));
RGB =[image(ind); image(ind+data.camera.RGB.height*data.camera.RGB.width); image(ind+data.camera.RGB.height*data.camera.RGB.width*2)];
points2ply('demo_points_with_color.ply', XYZ, RGB);

%% project the 3D points to the undistort color image
xyzRGBnormal = data.camera.RGB.K2render * XYZrgb;
xyzRGBnormal(1,:) = 1+ xyzRGBnormal(1,:) ./ xyzRGBnormal(3,:);
xyzRGBnormal(2,:) = 1+ xyzRGBnormal(2,:) ./ xyzRGBnormal(3,:);

%% undistort the RGB image into an pin hole camera with data.camera.RGB.K2render as intrinsics
undistortImage = undistort_image(im2double(image), data.camera.RGB.KK, data.camera.RGB.kc, data.camera.RGB.width, data.camera.RGB.height, data.camera.RGB.K2render);

%% undistort the depth map, transform the depth, and project it using OpenGL to the undistorted image with data.camera.RGB.K2render as intrincis
% manually find one
%[~,imageDepth] = WarpDepthMatlab(XYZcamera,data.camera.RGB.K2render, data.camera.D2RGB.Rt, data.camera.RGB.width, data.camera.RGB.height);
% or better to use this one
imageDepth = depth4RGB(data, frameID); % use this to read a depth map that is a combination of the closest two depth map
% another useful function to do the same thing is
% frames = getRGBDframe(sequenceName,frameID);

%% undistort the IR image. Typicaly, you don't need this
undistortIR = undistort_image(im2double(IR), data.camera.D.KK, data.camera.D.kc, data.camera.D.width, data.camera.D.height);

%% undistore the depth map to a pixel camera, and do NOT align it with the RGB image
[~,undistortDepth] = WarpDepthMatlab(XYZcamera,data.camera.D.KK, [eye(3) zeros(3,1)], data.camera.D.width, data.camera.D.height);

%% visualization
figure
subplot(2,3,1);
imagesc(depth);
axis equal; axis tight; axis off;
title('raw depth');

subplot(2,3,2);
imshow(image);
hold on;
plot(xyRGB(1,:),xyRGB(2,:),'.')
axis equal;
axis tight
title('raw image');

subplot(2,3,3);
imshow(undistortImage);
hold on;
plot(xyzRGBnormal(1,:),xyzRGBnormal(2,:),'.')
axis equal;
axis tight
title('undistorted image');

subplot(2,3,4);
imagesc(undistortDepth);
axis equal; axis tight; axis off;
title('undistorted depth');

subplot(2,3,5);
imshow(undistortIR);
title('undistorted IR');

subplot(2,3,6);
imagesc(imageDepth);
axis equal; axis tight; axis off;
title('depth on undistorted image');


function [time_finish, time_table_pen] = cal_time_data(cp_ptp, ts, time_wait)
% Finish Time and Pen Time Table Calculation
%
% [Inputs]
%	cp_ptp			 : CP + PTP trajectory
%   ts				 : sample rate [msec]
%	time_wait		 : time to wait [msec]
% [Outputs]
%	time_finish		 : time to finish tracking trajectory [msec]
%	time_table_pen	 : time table to manipulate pen [msec]

% time_table_pen
num_pen = length(cp_ptp) - 1;
time_table_pen = zeros(1, num_pen);
noe = 0;
for n = 1:num_pen
	noe = noe + max(size(cp_ptp{n}));
	time_table_pen(n) = noe * ts;
end

% time_finish
noe = noe + max(size(cp_ptp{end}));
time_finish = noe * ts + time_wait;


%% Example
% real time control of the KUKA iiwa 7 R 800
% Moving EEF of the robot in Z direction using a sinisoidal function

% An example script, it is used to show how to use the different
% functions of the KUKA Sunrise matlab toolbox

% First run the following script in Matlab
% Then start the client on the KUKA iiwa controller

% Mohammad SAFEEA, 24th of April 2018

close all,clear all;clc;

warning('off')

ip='172.31.1.147'; % The IP of the controller
% start a connection with the server
global t_Kuka;
t_Kuka=net_establishConnection( ip );

if ~exist('t_Kuka','var') || isempty(t_Kuka) || strcmp(t_Kuka.Status,'closed')
  warning('Connection could not be establised, script aborted');
  return;
end
% Joints positions
jPos={0,0,0,-pi/2,0,pi/2,0};
%setBlueOn(t); % turn on blue light
relVel=0.15;
movePTPJointSpace( t_Kuka , jPos, relVel); % move to initial configuration
%% Get Cartesian position of EEF
fprintf('Cartesian position')
eefpos=getEEFPos( t_Kuka );
eefposDist=eefpos;
disp(eefpos)
%% Start direct servo in Cartesian space
realTime_startDirectServoCartesian(t_Kuka);
disp('Starting direct servo in Cartesian space');
w=2; % motion constants, frequency rad/sec
A=75; % motion constants, amplitude of motion (mm)
tic;
deltaT=0;
counter=0;
initiationFlag=0;
disp('Enter control loop, stream EEF positions')
try
    %% Control loop
    while(deltaT<(6*pi/w))
        if(initiationFlag==0)
            initiationFlag=1;
            t_0=toc;
            t0=t_0;
        else
            time=toc;
            deltaT=time-t0;
            %%%%%%%%%%%%%%%%%%%%%%%%
            %% Perform trajectory calculation here
            eefposDist{3}=eefpos{3}-A*(1-cos(w*deltaT));
            %%%%%%%%%%%%%%%%%%%%%%%%
            % Send EEF position to robot
            if(toc-t_0>0.003)
                counter=counter+1;
                sendEEfPositionf( t_Kuka ,eefposDist);
                t_0=toc;
            end
        end
    end
    tstart=t0;
    tend=time;
    rate=counter/(tend-tstart);
    %% Stop the direct servo motion
    realTime_stopDirectServoCartesian( t_Kuka );
    fprintf('\nThe rate of update per second is: \n');
    disp(rate);
    fprintf('\n')
    pause(2);
    %% turn off light
    %setBlueOff(t);
    %% turn off the server
    net_turnOffServer( t_Kuka );
    fclose(t_Kuka);
    warning('on')
catch
    %% turn off the server
    net_turnOffServer( t_Kuka );
end


% Codes for CVPR-15 work `Face Alignment by Coarse-to-Fine Shape Searching'
% Any question please contact Shizhan Zhu: zhshzhutah2@gmail.com
% Released on July 25, 2015

function T = getTransViaRotateGivenCenter(theta_vector,center,rotatorLength)
%T = getTransViaRotateGivenCenter(theta_vector,win_size)
%   T: m*1 t_concord
%   theta_vector: m*1 in degree! Make sure abs(theta)<180
%   center: 1*2 indicates the x-y coordinates of rotation center
%   rotatorLength: scalar, length of the ratator. Set this variable to
%       avoid numerical problems.

warning('This function might get non-affine tform');
assert(size(theta_vector,1)==1 || size(theta_vector,2)==1);
assert(size(center,1)==1 || size(theta_vector,2)==1);
assert(length(center)==2);
assert(length(rotatorLength)==1);
theta_vector = theta_vector(:);
assert(all(abs(theta_vector) < 180));

pose_src = zeros(length(theta_vector),4);
pose_src(:,[1 3]) = repmat(center(:)',length(theta_vector),1);
pose_src(:,2) = center(1) + rotatorLength * cosd(theta_vector);
pose_src(:,4) = center(2) + rotatorLength * sind(theta_vector);
pose_dst = [center(1) center(1)+rotatorLength center(2) center(2)];

T = getTransToSpecific(pose_src,pose_dst);

end


function [BigPopulation,tempPara] = GenerateBigPopulation(PV,groups,Archive)

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

% This function is written by Huangke Chen

   cDecs  = Archive.decs;
   PVDecs = cDecs(:,PV);

   BigPopulation = cell(1,size(PVDecs,1));	% initialize the big population to contain all the population
   cPopSize      = 16;                      % the population size of each population

   para.lambda = cPopSize;                                      % population size, offspring number
   tempPara    = repmat(para,size(PVDecs,1),length(groups));	% Initialize the parameters for CMA-ES

   for ci = 1: size(PVDecs,1)
       % Construct the population
       BigPopulation{ci} = repmat(cDecs(ci,:),cPopSize,1);

       % Set the CMA-ES parameters
       for g = 1 : length(groups)	% Set the parameters for each variable group
           DVindex = groups{g};     % the variable index of this group
           N = length(DVindex);     % variable number

           % set the CMA-ES parameters for each group
           tempPara(ci,g).N = N;
           mu      = cPopSize/2;                % number of parents/points for recombination
           weights = log(mu+1/2) - log(1:mu)';	% muXone array for weighted recombination
           tempPara(ci,g).mu = floor(mu);

           tempPara(ci,g).weights = weights/sum(weights);	% normalize recombination weights array
           mueff = sum(weights)^2/sum(weights.^2);          % variance-effectiveness of sum w_i x_i
           tempPara(ci,g).mueff = mueff;

           tempPara(ci,g).cc    = (4+mueff/N)/(N+4+2*mueff/N);                                  % time constant for cumulation for C
           tempPara(ci,g).cs    = (mueff+2)/(N+mueff+5);                                        % t-const for cumulation for sigma control
           tempPara(ci,g).c1    = 2/((N+1.3)^2+mueff);                                          % learning rate for rank-one update of C
           tempPara(ci,g).cmu   = min(1-tempPara(ci,g).c1,2*(mueff-2+1/mueff)/((N+2)^2+mueff));	% and for rank-mu update
           tempPara(ci,g).damps = 1 + 2*max(0,sqrt((mueff-1)/(N+1))-1) + tempPara(ci, g).cs;    % damping for sigma
           tempPara(ci,g).chiN  = N^0.5*(1-1/(4*N)+1/(21*N^2));                                 % expectation of ||N(0,I)|| == norm(randn(N,1))

           % variable parameters
           paraDecs = BigPopulation{ci};
           tempPara(ci,g).xmean = mean(paraDecs(:,DVindex))';
           tempPara(ci,g).sigma = 0.1;          % coordinate wise standard deviation (step size)
           tempPara(ci,g).pc    = zeros(N,1);	% evolution paths for C and sigma
           tempPara(ci,g).ps    = zeros(N,1);	% evolution paths for C and sigma
           B = eye(N,N);	% B defines the coordinate system
           tempPara(ci,g).B = B;
           D = ones(N,1);	% diagonal D defines the scaling
           tempPara(ci,g).D = D;
           tempPara(ci,g).C = B*diag(D.^2)*B';	% covariance matrix C
           tempPara(ci,g).eigeneval = 0;
           tempPara(ci,g).counteval = 0;
       end
   end
end


function [ml] = in32ml(in3)
% Convert volume from cubic inches to milliliters.
% Chad Greene 2012
ml = in3*16.387064;


function r8_round2_test ( )

%*****************************************************************************80
%
%% R8_ROUND2_TEST tests R8_ROUND2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  x = pi;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_ROUND2_TEST\n' );
  fprintf ( 1, '  R8_ROUND2 rounds a number to a\n' );
  fprintf ( 1, '  specified number of base 2 digits.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Test effect on PI:\n' );
  fprintf ( 1, '  X = %f\n', x );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  NPLACE  XROUND\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 20
    nplace = i;
    xround = r8_round2 ( nplace, x );
    fprintf ( 1, '  %8d  %f\n', i, xround );
  end

  return
end