function [XU] = Associate(Population,W,N)
% Associate each solution with a neuron

%------------------------------- 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 Chao He

     A  = 1 : N;
     U  = 1 : N;
     XU = zeros(1,N);
     for i = 1 : N
         x = randi(length(A));
         [~,u] = min(pdist2(Population(A(x)).dec,W(U,:)));
         XU(U(u)) = A(x);
         A(x)     = [];
         U(u)     = [];
     end
end


function [FcellZ, FcellNeuZ] = extractZSignals(ops, zpos, Fcell, FcellNeu, subpixel)

Nk = size(Fcell{1},1);

F = [];
Fneu = [];
for k = 1:numel(Fcell)
    F = [F Fcell{k}];
    Fneu = [Fneu FcellNeu{k}];
end

% compute each cell's profile in Z
zspread  = 10;
iZ = [-zspread : 1/subpixel : zspread];
zind = zpos * subpixel + find(iZ==0);
zF = zeros(Nk, numel(iZ));
zFneu = zeros(Nk, numel(iZ));
for j = 1:numel(iZ)
    zF(:,j)    = mean(F(:, zind == j), 2);
    zFneu(:,j) = mean(Fneu(:, zind == j), 2);
end

if sum(zind==1) == 0
    jm = min(zind);
    zF(:, 1:jm-1) = repmat(zF(:, jm), 1, jm-1);
    zFneu(:, 1:jm-1) = repmat(zFneu(:, jm), 1, jm-1);
end

if sum(zind==numel(iZ)) == 0
    jm = max(zind);
    zF(:, jm+1:end) = repmat(zF(:, jm), 1, numel(iZ) - jm);
    zFneu(:, jm+1:end) = repmat(zFneu(:, jm), 1, numel(iZ) - jm);
end

zF= fixnangaps(zF);
zFneu= fixnangaps(zFneu);

zF = my_conv2(zF, 0.5, 2);
zFneu = my_conv2(zFneu, 0.5, 2);
plot(zF(1,:));

%%
Fz      = zF(:, zind);
Fzneu    = zFneu(:, zind);

csumNframes = [0 cumsum(ops.Nframes)];
FcellZ       = cell(1, length(ops.Nframes));
FcellNeuZ    = cell(1, length(ops.Nframes));
for i = 1:length(ops.Nframes)
    FcellZ{i}     = Fz(:, csumNframes(i) + (1:ops.Nframes(i)));
    FcellNeuZ{i}  = Fzneu(:, csumNframes(i) + (1:ops.Nframes(i)));
end


% Copyright (C) 1993-2013, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for MATLAB (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin

% Create a white square
im = testpattern('squares', 256, 256, 128);
% and rotate it
im = irotate(im, -0.3);
idisp(im)

% find the edges
edges = icanny(im);
idisp(edges)

% compute the Hough transform of the edge image
h = Hough(edges)
about h
% where we have a Hough object

% We can view the Hough accumulator array
figure(2); h.show()
% where each point represents a line and the bright spots represent
% dominant lines in the scene

% We can find those dominant lines
lines = h.lines();
about lines
% which is an array of LineFeature objects

% Each LineFeature object has a number of properties
lines(1)
% which we can access individually by
lines(1).theta
lines(1).rho

% If we display these
lines
% we see that

axis([-1.4 -1.1 -190 -110])

% We can avoid this problem by using non-local minima suppression
h = Hough(edges, 'suppress', 5)
% in this case with a radius of 5 Hough cells

% Repeating the process above we see fewer lines
lines = h.lines()

figure(1); idisp(im)
h.plot('b')
im = iread('church.png', 'grey', 'double');
idisp(im)
edges = icanny(im);
idisp(edges)
h = Hough(edges, 'suppress', 5)
h.show()
h.lines()
lines = h.lines()
idisp(im)
lines(1:10).plot();


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defop  Method   @@sym {vertcat} {(@var{x}, @var{y}, @dots{})}
%% @defopx Operator @@sym {[@var{x}; @var{y}; @dots{}]} {}
%% Vertically concatentate symbolic arrays.
%%
%% Example:
%% @example
%% @group
%% A = sym([1 2])
%%   @result{} A = (sym) [1  2]  (1×2 matrix)
%% [A; A; 2*A]
%%   @result{} (sym 3×2 matrix)
%%       ⎡1  2⎤
%%       ⎢    ⎥
%%       ⎢1  2⎥
%%       ⎢    ⎥
%%       ⎣2  4⎦
%% @end group
%% @end example
%% @seealso{@@sym/horzcat, @@sym/cat}
%% @end defop

function h = vertcat(varargin)

  % special case for 0x0 but other empties should be checked for
  % compatibilty
  cmd = {
          '_proc = []'
          'for i in _ins:'
          '    if i is None or not i.is_Matrix:'
          '        _proc.append(sp.Matrix([[i]]))'
          '    else:'
          '        if i.shape == (0, 0):'
          '            pass'
          '        else:'
          '            _proc.append(i)'
          'return sp.MatrixBase.vstack(*_proc),'
          };

  for i = 1:nargin
    varargin{i} = sym(varargin{i});
  end
  h = pycall_sympy__ (cmd, varargin{:});

end

%!test
%! % basic
%! syms x
%! A = [x; x];
%! B = vertcat(x, x);
%! C = vertcat(x, x, x);
%! assert (isa (A, 'sym'))
%! assert (isa (B, 'sym'))
%! assert (isa (C, 'sym'))
%! assert (isequal (size(A), [2 1]))
%! assert (isequal (size(B), [2 1]))
%! assert (isequal (size(C), [3 1]))

%!test
%! % basic, part 2
%! syms x
%! A = [x; 1];
%! B = [1; x];
%! C = [1; 2; x];
%! assert (isa (A, 'sym'))
%! assert (isa (B, 'sym'))
%! assert (isa (C, 'sym'))
%! assert (isequal (size(A), [2 1]))
%! assert (isequal (size(B), [2 1]))
%! assert (isequal (size(C), [3 1]))

%!test
%! % column vectors
%! a = [sym(1); 2];
%! b = [sym(3); 4];
%! assert (isequal ( [a;b] , [1; 2; 3; 4]  ))
%! assert (isequal ( [a;b;a] , [1; 2; 3; 4; 1; 2]  ))

%!test
%! % row vectors
%! a = [sym(1) 2];
%! b = [sym(3) 4];
%! assert (isequal ( [a;b] , [1 2; 3 4]  ))
%! assert (isequal ( [a;b;a] , [1 2; 3 4; 1 2]  ))

%!test
%! % row vector, other row
%! a = [sym(1) 2];
%! assert (isequal ( [a; [sym(3) 4]] , [1 2; 3 4]  ))

%!test
%! % empty vectors
%! v = [sym(1) sym(2)];
%! a = [v; []];
%! assert (isequal (a, v))
%! a = [[]; v; []];
%! assert (isequal (a, v))
%! a = [v; []; []];
%! assert (isequal (a, v))

%!xtest
%! % FIXME: is this Octave bug? worth worrying about
%! syms x
%! a = [x; [] []];
%! assert (isequal (a, x))

%!test
%! % more empty vectors
%! v = [sym(1) sym(2)];
%! q = sym(ones(0, 2));
%! assert (isequal ([v; q], v))

%!error <ShapeError>
%! v = [sym(1) sym(2)];
%! q = sym(ones(0, 3));
%! w = vertcat(v, q);

%!test
%! % Octave 3.6 bug: should pass on 3.8.1 and matlab
%! a = [sym(1) 2];
%! assert (isequal ( [a; [3 4]] , [1 2; 3 4]  ))
%! assert (isequal ( [a; sym(3) 4] , [1 2; 3 4]  ))
%! % more examples
%! syms x
%! [x [x x]; x x x];
%! [[x x] x; x x x];
%! [[x x] x; [x x] x];
%! [x x x; [x x] x];

%!test
%! % issue #700
%! A = sym ([1 2]);
%! B = simplify (A);
%! assert (isequal ([B; A], [A; B]))


function [clout] = mask_princomp(clusters,varargin)
% :Usage:
% ::
%
%     [clusters] = mask_princomp(clusters,[behavioral score vector],[corr flag],[plotflag],[saveflag])
%
% This function is just like cluster_princomp, except that it works on the SET
% of activations in all clusters, rather than within each cluster.
%
% clusters is structure of clusters from tor_extract_rois.m
%
% behavioral vector is row vector of behavioral or other scores to correlate
%
% corr flag:  1 = work on correlations among voxels, 2 = work on covariance
%
% plotflag:   1 = yes, 0 = no.  plots.
%
% try this to test the program on random data:
% ::
%
%    cl(1).all_data = randn(23,30);cl(1).numVox = 30;cl = cluster_princomp(cl,EXPT.behavior,1,1);
%    cl(1).all_data(:,1:10) = cl(1).all_data(:,1:10) + 10; cl = cluster_princomp(cl,EXPT.behavior,1,1);
%    cl(1).all_data(:,25:30) = cl(1).all_data(:,25:30) + repmat((EXPT.behavior .* 3)',1,6);
%    cl(1).all_data(:,21:24) = cl(1).all_data(:,21:24) + repmat((1:23)',1,4);
%    cl = cluster_princomp(cl,EXPT.behavior,1,1);
%
% mean-center everything now:
% ::
%
%    cl.PCA = []; cl.all_data - cl.all_data - repmat(mean(cl.all_data),size(cl.all_data,1),1);
%    cl = cluster_princomp(cl,EXPT.behavior,1,1);
%
% add another correlated group:
% ::
%
%    cl.all_data(:,1:5) = cl.all_data(:,1:5) + repmat(rand(23,1)*5,1,5);
%    cl = cluster_princomp(cl,EXPT.behavior,1,1);
%
% if component scores are used and correlated with behavior, this means that the subjects
% tend to show the behavioral effect who also show the pattern associated with comp. x.
% this may mean high on a number of voxels, or high on some and low on others.
% the weights may be used to interpret what the components mean, and this can be done
% graphically.
%
% t-tests on component scores have ambiguous interpretations, because a high t-score
% may indicate negative values or close-to-zero values on some voxels.
% a component could have the interpretation, "high on this component means high on V1
% and low on V2."
%
% classifying voxels is done using cluster analysis (hierarchical, centroid linkage)
% on the voxels (observations) using the PCA weights (eigenvectors) as variables.
% This lets the clustering algorithm work in the reduced variable space with dimensionality
% equal to the number of components.
% The max number of clusters is restricted based on the gradient of the eigenvalues in the PCA
% maxclusters = 1 + the number of eigenvalues with gradient at least 20% of the initial drop
% from 1 to 2 eigenvalues.
%
% Requires clustering library in Matlab.
% Robust option also uses the robust PCA algorithm RAPCA,
% created by:
% Hubert, M., Rousseeuw, P.J., Verboven, S. (2002),
%  "A fast method for robust principal components with applications to chemometrics", by Mia Hubert, Peter J. Rousseeuw,
%  Chemometrics and Intelligent Laboratory Systems, 60, 101-111.
%

corrflag = 1; plotflag = 1; robustflag = 1; saveflag = 0;
if length(varargin) > 1, corrflag = varargin{2};, end
if length(varargin) > 2, plotflag = varargin{3};, end
if length(varargin) > 3, saveflag = varargin{4};, end

% get all the voxels and data together

CLU = clusters2CLU(clusters);
CLU.all_data = cat(2,clusters.all_data);

clear clusters;
clusters(1) = CLU;
i = 1;
subclusters{1} = []; clusters(i).PCA = [];

disp('Output saved in mask_princomp.out')
diary mask_princomp.out

a = CLU.all_data;

    if size(a,2) > 2    % must have 3 voxels to try clustering

    % -------------------------------------------------------------------------------
    % * All the real work is done here.  Compute pc's and clustering
    % -------------------------------------------------------------------------------

    if ~robustflag
        [clusters(i).PCA.pcomps,clusters(i).PCA.weights,clusters(i).PCA.eigval,clusters(i).PCA.class] = pc(a,corrflag);

        % automatically pick number of clusters, based on gradient in eigenvalues
        g = abs(gradient(clusters(i).PCA.eigval));
        maxclusters = sum(g > g(1).*.2) + 1;
    else

        % pick number of clusters by hand
        out = rapca(a);
        clusters(i).PCA.pcomps = out.T;
        clusters(i).PCA.weights = out.P;
        clusters(i).PCA.eigval = out.L;
        maxclusters = length(out.L);

        if saveflag,
            try mkdir mask_pca_results, catch, end
            saveas(gcf,'mask_pca_results/robust_pca_diagnostic','fig'), close
            saveas(gcf,'mask_pca_results/robust_eigenvalues','fig'), close
        end

    end

    clusters(i).PCA.class = docluster(a,maxclusters,plotflag);

    grps = unique(clusters(i).PCA.class(clusters(i).PCA.class~=0)); % values are component of origin
    for j = 1:length(grps),
        clusters(i).PCA.avgs(:,j) = mean(a(:,find(clusters(i).PCA.class==grps(j))),2);,
        freq(j) = sum(clusters(i).PCA.class == grps(j));
    end

    disp([clusters(i).title ', ' num2str(clusters(i).numVox) ' voxels: ' num2str(size(clusters(i).PCA.pcomps,2)) ' components'])
    fprintf(1,'\tMean\tEigval\tcorrel\t')

    % -------------------------------------------------------------------------------
    % * display each component and correlation with behavior
    % -------------------------------------------------------------------------------

    for j = 1:size(clusters(i).PCA.pcomps,2)

        %[H,P,CI,STATS] = TTEST(clusters(i).PCA.pcomps(:,j),0,.05,0);
        % skip the t-test.  t-tests on component scores don't make a lot of sense.
        fprintf(1,'\n\t%3.3f\t%3.3f\t',mean(clusters(i).PCA.pcomps(:,j)),clusters(i).PCA.eigval(j))

        if length(varargin) > 0
            if ~isempty(varargin{1})
                co = corrcoef(clusters(i).PCA.pcomps(:,j),varargin{1});
                co = co(1,2);
                fprintf(1,'%3.3f\t',co)
            end
        end

    end
    fprintf(1,'\n')

    % -------------------------------------------------------------------------------
    % * display classification info
    % -------------------------------------------------------------------------------
    disp(['Classified into ' num2str(max(clusters(i).PCA.class)) ' groups:'])
    fprintf(1,'\tClass\tVoxels\tMean\tt\tp\tcorrect. p\tcorrel\t')

    % for each component, test mean value and correlation with behavior
    for j = 1:length(grps)

        [H,P,CI,STATS] = TTEST(clusters(i).PCA.avgs(:,j),0,.05,0);
        fprintf(1,'\n\t%3.0f\t%3.0f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t',j,freq(j),mean(clusters(i).PCA.avgs(:,j)),STATS.tstat,P,P .* size(clusters(i).PCA.avgs,2))

        if length(varargin) > 0
            if ~isempty(varargin{1})
                co = corrcoef(clusters(i).PCA.avgs(:,j),varargin{1});
                co = co(1,2);
                fprintf(1,'%3.3f\t',co)
            end
        end

    end
    fprintf(1,'\n')

    % -------------------------------------------------------------------------------
    % * Plot, if requested
    % -------------------------------------------------------------------------------

    if plotflag,
        figure('Color','w'), subplot(1,3,1), imagesc(a), title(['Cl ' num2str(i) ': Data']), xlabel('Voxels'),ylabel('Subjects')
        subplot(1,3,2), imagesc(clusters(i).PCA.weights'), title(['Weights (eigenvectors)']), xlabel('Voxels'),ylabel('Eigenvectors')
        subplot(1,3,3), imagesc(clusters(i).PCA.pcomps), title(['Component scores (predictions)']), xlabel('Voxels'),ylabel('Subjects')

        if saveflag,
            try mkdir mask_pca_results, catch, end
            disp('Images saved in mask_pca_results folder')
            saveas(gcf,'mask_pca_results/PCA1','fig'),
        end

        a = [clusters(i).PCA.class' a']; a=sortrows(a,1); a = a(:,2:end)';
        figure;subplot 131; imagesc(a),title(['Cl ' num2str(i) ':Data sorted by class']), xlabel('Class'),ylabel('Subjects'),
        xlab = [sort(clusters(i).PCA.class(clusters(i).PCA.class~=0)) clusters(i).PCA.class(clusters(i).PCA.class==0)];
        set(gca,'XTick',1:length(clusters(i).PCA.class)); set(gca,'XTickLabel',xlab)
        subplot 132; imagesc(clusters(i).PCA.avgs),title('Class averages'), xlabel('Class'),ylabel('Subjects'),
        subplot 133; if length(varargin) > 0, if ~isempty(varargin{1}), imagesc(varargin{1}'), title('Behavior'),end,end

        if saveflag, saveas(gcf,'mask_pca_results/PCA_with_clustering','fig'),end
    end

    else
        disp(['Cluster ' num2str(i) ' has less than 3 voxels.'])
        clusters(i).PCA.class = ones(1,clusters(i).numVox);
        clusters(i).PCA.avgs = clusters(i).timeseries;
        grps = 1;
        if ~isfield(clusters,'correl'), clusters(1).correl = [];, end
    end

    diary off

    % -------------------------------------------------------------------------------
    % * separate into subclusters, based on class membership
    % -------------------------------------------------------------------------------

    for j = 1:length(grps)

        wh = find(clusters(i).PCA.class == grps(j));

        if length(varargin) > 0
            if ~isempty(varargin{1})
                co = corrcoef(clusters(i).PCA.avgs(:,j),varargin{1});
                subc(j).correl = co(1,2);
            end
        end

        CLUcomp = clusters(1);
        CLUcomp.XYZmm = CLUcomp.XYZmm(:,wh);
        CLUcomp.XYZ = CLUcomp.XYZ(:,wh);
        CLUcomp.Z = CLUcomp.Z(:,wh);
        CLUcomp.all_data = CLUcomp.all_data(:,wh);
        CLUcomp.numVox = size(CLUcomp.all_data,2);

        clout{j} = tor_extract_rois([],CLUcomp,CLUcomp);
        if plotflag
            montage_clusters([],clout{j})
            if saveflag, saveas(gcf,['mask_pca_results/montage_subset' num2str(j)],'fig'),end
        end

    end

end

return

function [b,v,d,class] = pc(a,corrflag)
% a is original matrix, b is principal components, v is eigenvectors
% (weights on columns, which = weights on voxels)
% class is classification of voxels into groups based on component loadings

if corrflag, [v,d]=eig(corrcoef(a));, else, [v,d]=eig(cov(a));,end
b = (pinv(v) * a')' ./ repmat((diag(d)').^.5,size(a,1),1);
% i made this up: think of rptating each subject's scores (in cols of a')
% by the rotation matrix pinv(v), and normalizing by the sqrt of the eigenvalues
% pinv(v) and v are rotation matrices because det = 1, no shearing or dilation
%
% this appears to work to give scores as well
% both methods (above,below) are scaled versions of the splus factor scores
% the problem is that doing it two different ways in splus flips the signs
% of some components and not others (gui vs cmd line).

%X = a; R = corrcoef(a); A = v * (d^.5); B = inv(R) * A;
%scores = X * B;
% X is data, A is factor loading matrix, B is factor score coeff matrix
% this method, from the text, and the one giving b above produce identical results

A = v * (d^.5);

b = fliplr(b); v = fliplr(v); A = fliplr(A); %scores = fliplr(scores);

num = min(10,sum(diag(d) >= 1));
b = b(:,1:num); v = v(:,1:num); A = A(:,1:num);
origd = diag(d);
d = diag(d)'; d= fliplr(d); d = d(1:num);

if num == 0, warning('No eigenvalues above 1!');, origd, class = [];

else
    % classify each voxel into a group based on loading
    % use A, which re-introduces the comp variance, because we
    % want relationships with more variance to count more.
    % This just doesn't work so hot.  See docluster, below.

    %wh = A' == repmat(max(A'),size(A,2),1);
    %for i = 1:size(wh,2), tmp = find(wh(:,i));, class(i) = tmp(1); end
    %class(max(A') < .3) = 0;

end

%figure;plot(b,'r'),hold on;plot(a,'k'), hold on; plot(mean(a,2),'g--'),legend({'eig' 'orig' 'avg'})

return

function class = docluster(a,maxclusters,doplot)

    if size(a,2) > 300,
        Y = pdist_by_parts(a');
    else
        Y = pdist(a','euclid');     % transpose so the voxels are observations, eigenvectors the variables
    end
    Z = linkage(Y,'centroid');
    class = cluster(Z,maxclusters)';

    if doplot,
        dendrogram(Z,0); title('Dendrogram for clustering')
    end

return

function pd =  pdist_by_parts(x)

disp(['Computing distances between observations'])

indx = 1;
vindx = 2;

for i = 1:size(x,1)
    indx = 1;   % faster this way
    for j = vindx:size(x,1)
        pd{i}(indx) = (sum((x(i,:) - x(j,:)).^2)).^.5;
        indx = indx+1;
    end
    vindx = vindx+1;
    if mod(i,100)==0,fprintf(1,'.'),end
end

pd = cat(2,pd{:});
fprintf(1,'done\n')

return


function [Coor, EffIdx, ind] = CvtPtsToVoxelFun( data, minVal, Size, gridRes )
Coor = [];
EffIdx = [];
ind = [];
Dim = size(data, 1);
for i = 1 : 1 : Dim
    Coor(i, :) = ceil(( data(i, :) - minVal(i) )/gridRes);
end
if Dim == 3
    EffIdx = find( Coor(1, :) >= 1 & Coor(1, :) <= Size(1) & ...
        Coor(2, :) >= 1 & Coor(2, :) <= Size(2) & ...
        Coor(3, :) >= 1 & Coor(3, :) <= Size(3) );
    ind = sub2ind(Size, Coor(1, EffIdx), Coor(2, EffIdx), Coor(3, EffIdx));
end
if Dim == 2
    EffIdx = find( Coor(1, :) >= 1 & Coor(1, :) <= Size(1) & ...
        Coor(2, :) >= 1 & Coor(2, :) <= Size(2) );
    ind = sub2ind(Size, Coor(1, EffIdx), Coor(2, EffIdx));
end
end


function [ row, col ] = i4row_find_pair_wrap ( m, n, a, item1, item2 )

%*****************************************************************************80
%
%% I4ROW_FIND_PAIR_WRAP searches rows of an I4ROW for a pair of items.
%
%  Discussion:
%
%    The items must occur consecutively, with ITEM1 occurring
%    first.  However, wrapping is allowed.  That is, if ITEM1
%    occurs in the last column, and ITEM2 in the first, this
%    is also regarded as a match.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), the table to search.
%
%    Input, integer ITEM1, ITEM2, the values to search for.
%
%    Output, integer ROW, COL, the row and column indices
%    of the first occurrence of the value ITEM1 followed immediately
%    by ITEM2.  The search is conducted by rows.  If the pair of
%    items is not found, then ROW = COL = -1.  If COL = N,
%    the ITEM1 occurs in column N and ITEM2 occurs in column 1.
%
  for i = 1 : m
    for j = 1 : n

      if ( a(i,j) == item1 )

        if ( j < n )
          jp1 = j + 1;
        else
          jp1 = 1;
        end

        if ( a(i,jp1) == item2 )
          row = i;
          col = j;
          return
        end

      end

    end
  end

  row = -1;
  col = -1;

  return
end


function [GAmp,GTime]=GzAreaTrapezoid(p) % unit m-1
% Trapezoid with prescribed area & time span & maximum slew rate
% note: don't support ramp sampling

global VCtl
global VObj

tStart=p.tStart; % start time
tEnd=p.tEnd; % end time
Area=abs(p.Area);   % trapezoid area m-1
Duplicates=max(1,p.Duplicates);
DupSpacing=max(0,p.DupSpacing);

Area  =Area/(VObj.Gyro/(2*pi));
GzAmp =Area /(tEnd - tStart);
tRamp=max(VCtl.MinUpdRate,GzAmp/VCtl.MaxSlewRate);   % ramp time

[GAmp,GTime]=StdTrap(tStart-tRamp, ...
                     tEnd+tRamp,   ...
                     tStart,               ...
                     tEnd,                 ...
                     GzAmp*sign(p.Area),2,2,2);

[GTime,m,n]=unique(GTime);
GAmp=GAmp(m);

% Create Duplicates
if Duplicates~=1 & DupSpacing ~=0
    GAmp=repmat(GAmp,[1 Duplicates]);
    TimeOffset = repmat(0:DupSpacing:(Duplicates-1)*DupSpacing,[length(GTime) 1]);
    GTime=repmat(GTime,[1 Duplicates]) + (TimeOffset(:))';
end

end


% RES = upConv(IM, FILT, EDGES, STEP, START, STOP, RES)
%
% Upsample matrix IM, followed by convolution with matrix FILT.  These
% arguments should be 1D or 2D matrices, and IM must be larger (in
% both dimensions) than FILT.  The origin of filt
% is assumed to be floor(size(filt)/2)+1.
%
% EDGES is a string determining boundary handling:
%    'circular' - Circular convolution
%    'reflect1' - Reflect about the edge pixels
%    'reflect2' - Reflect, doubling the edge pixels
%    'repeat'   - Repeat the edge pixels
%    'zero'     - Assume values of zero outside image boundary
%    'extend'   - Reflect and invert
%    'dont-compute' - Zero output when filter overhangs OUTPUT boundaries
%
% Upsampling factors are determined by STEP (optional, default=[1 1]),
% a 2-vector [y,x].
%
% The window over which the convolution occurs is specfied by START
% (optional, default=[1,1], and STOP (optional, default =
% step .* (size(IM) + floor((start-1)./step))).
%
% RES is an optional result matrix.  The convolution result will be
% destructively added into this matrix.  If this argument is passed, the
% result matrix will not be returned. DO NOT USE THIS ARGUMENT IF
% YOU DO NOT UNDERSTAND WHAT THIS MEANS!!
%
% NOTE: this operation corresponds to multiplication of a signal
% vector by a matrix whose columns contain copies of the time-reversed
% (or space-reversed) FILT shifted by multiples of STEP.  See corrDn.m
% for the operation corresponding to the transpose of this matrix.

% Eero Simoncelli, 6/96.  revised 2/97.

function result = upConv(im,filt,edges,step,start,stop,res)

%% THIS CODE IS NOT ACTUALLY USED! (MEX FILE IS CALLED INSTEAD)

fprintf(1,'WARNING: You should compile the MEX version of "upConv.c",\n         found in the MEX subdirectory of matlabPyrTools, and put it in your matlab path.  It is MUCH faster, and provides more boundary-handling options.\n');

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('edges') == 1)
  if (strcmp(edges,'reflect1') ~= 1)
    warning('Using REFLECT1 edge-handling (use MEX code for other options).');
  end
end

if (exist('step') ~= 1)
  step = [1,1];
end

if (exist('start') ~= 1)
  start = [1,1];
end

% A multiple of step
if (exist('stop') ~= 1)
  stop = step .* (floor((start-ones(size(start)))./step)+size(im))
end

if ( ceil((stop(1)+1-start(1)) / step(1)) ~= size(im,1) )
  error('Bad Y result dimension');
end
if ( ceil((stop(2)+1-start(2)) / step(2)) ~= size(im,2) )
  error('Bad X result dimension');
end

if (exist('res') ~= 1)
  res = zeros(stop-start+1);
end

%------------------------------------------------------------

tmp = zeros(size(res));
tmp(start(1):step(1):stop(1),start(2):step(2):stop(2)) = im;

result = rconv2(tmp,filt) + res;


function [ beta ] =upadte_beta( H,W,K,grt)
    Hc=H*W;
    He=abs(Hc).^2;
    %% update beta
    beta=zeros(1,K);
    for k0=1:K
         tmp=Hc(k0,:);
         tmpe=He(k0,:);
         beta(k0)=sqrt(grt(k0))*tmp(k0)/(sum(tmpe)+1);
    end
end


function [ centers, mincenter, mindist, q2, quality ] = kmeans_fast ( ...
  data, initcenters, method )

%*****************************************************************************80
%
%% KMEANS_FAST carries out the fast KMEANS algorithm of Charles Elkan.
%
%  Discussion:
%
%    Note that if the input quantity INITCENTERS is a scalar, it is taken
%    to be the number of clusters desired, but if it is a KxM array, it is]
%    taken to be the M-dimensional coordinates of K points, to be used as
%    initial guesses for the cluster centers.
%
%  Modified:
%
%    04 September 2013
%
%  Author:
%
%    Charles Elkan
%
%  Reference:
%
%    Charles Elkan,
%    Using the Triangle Inequality to Accelerate k-Means,
%    Proceedings of the Twentieth International Conference on Machine Learning
%    (ICML-2003),
%    Washington DC, 2003.
%
%  Parameters:
%
%    Input, real DATA(N,M), the M-dimensional coordinates of N points.
%
%    Input, integer INITCENTERS, the number K of clusters desired.
%
%    Input, real INITCENTERS(K,M), the M-dimensional coordinates of K
%    points, which the program will use as starting guesses for the
%    cluster centers.
%
%    Input, integer METHOD, selects the algorithm to be used.
%    * 0, unoptimized, using n by k matrix of distances O(nk) space;
%    * 1, vectorized, using only O(n+k) space;
%    * 2, like 1, in addition using distance inequalities (default).
%
%    Output, real CENTERS(K,M), the M-dimensional coordinates of the K
%    cluster centers.
%
%    Output, integer MINCENTER(N,1), the index, for each data point, of
%    the cluster to which it belongs.
%
%    Output, real MINDIST(N,1), an upper bound of the distance of each point to
%    the nearest center.
%
%    Output, real Q2, the mean of UDIST^2.
%
%    Output, real QUALITY, the mean of UDIST.

tic
if nargin < 3 method = 2; end
[n,dim] = size(data);

if max(size(initcenters)) == 1
    k = initcenters;
    [centers, mincenter, mindist, lower, computed] = anchors(mean(data),k,data);
    total = computed;
    skipestep = 1;
else
    centers = initcenters;
    mincenter = zeros(n,1);
    total = 0;
    skipestep = 0;
    [k,dim2] = size(centers);
    if dim ~= dim2 error('dim(data) ~= dim(centers)'); end;
end

nchanged = n;
iteration = 0;
oldmincenter = zeros(n,1);

while nchanged > 0
    % do one E step, then one M step
    computed = 0;

    if method == 0 & ~skipestep
        for i = 1:n
            for j = 1:k
                distmat(i,j) = calcdist(data(i,:),centers(j,:));
            end
        end
        [mindist,mincenter] = min(distmat,[],2);
        computed = k*n;

    elseif (method == 1 | (method == 2 & iteration == 0)) & ~skipestep
        mindist = Inf*ones(n,1);
        lower = zeros(n,k);
        for j = 1:k
           jdist = calcdist(data,centers(j,:));
           lower(:,j) = jdist;
           track = find(jdist < mindist);
           mindist(track) = jdist(track);
           mincenter(track) = j;
        end
        computed = k*n;

    elseif method == 2 & ~skipestep
        computed = 0;
%
% for each center, nndist is half the distance to the nearest center
% if d(x,center) < nndist then x cannot belong to any other center
% mindist is an upper bound on the distance of each point to its nearest center
%
        nndist = min(centdist,[],2);
% the following usually is not faster
%        ldist = min(lower,[],2);
%        mobile = find(mindist > max(nndist(mincenter),ldist));
        mobile = find(mindist > nndist(mincenter));

% recompute distances for point i and center j
%       only if j can possibly be the new nearest center
% for speed, the first check has been optimized by modifying centdist
% swapping the order of the checks is slower for data with natural clusters

        mdm = mindist(mobile);
        mcm = mincenter(mobile);

        for j = 1:k
% the following is incorrect: for j = unique(mcm)'
            track = find(mdm > centdist(mcm,j));
            if isempty(track) continue; end
            alt = find(mdm(track) > lower(mobile(track),j));
            if isempty(alt) continue; end
            track1 = mobile(track(alt));
%
% calculate exact distances to the mincenter
% recalculate separately for each jj to avoid copying too much of data
% redo may be empty, but we don't need to check this.
%
            redo = find(~recalculated(track1));
            redo = track1(redo);
            c = mincenter(redo);
            computed = computed + size(redo,1);
            for jj = unique(c)'
                rp = redo(find(c == jj));
                udist = calcdist(data(rp,:),centers(jj,:));
                lower(rp,jj) = udist;
                mindist(rp) = udist;
            end
            recalculated(redo) = 1;

            track2 = find(mindist(track1) > centdist(mincenter(track1),j));
            track1 = track1(track2);
            if isempty(track1) continue; end

            % calculate exact distances to center j
            track4 = find(lower(track1,j) < mindist(track1));
            if isempty(track4) continue; end
            track5 = track1(track4);
            jdist = calcdist(data(track5,:),centers(j,:));
            computed = computed + size(track5,1);
            lower(track5,j) = jdist;

            % find which points really are assigned to center j
            track2 = find(jdist < mindist(track5));
            track3 = track5(track2);
            mindist(track3) = jdist(track2);
            mincenter(track3) = j;
        end % for j=1:k
    end % if method

    oldcenters = centers;
%
% M step: recalculate the means for each cluster
% if a cluster is empty, its mean is left unchanged
% we minimize computations for clusters with little changed membership
%
    diff = find(mincenter ~= oldmincenter);
    diffj = unique([mincenter(diff);oldmincenter(diff)])';
    diffj = diffj(find(diffj > 0));

    if size(diff,1) < n/3 & iteration > 0
         for j = diffj
            plus = find(mincenter(diff) == j);
            minus = find(oldmincenter(diff) == j);
            oldpop = pop(j);
            pop(j) = pop(j) + size(plus,1) - size(minus,1);
            if pop(j) == 0 continue; end
            centers(j,:) = (centers(j,:)*oldpop + sum(data(diff(plus),:),1) - sum(data(diff(minus),:),1))/pop(j);
        end
    else
        for j = diffj
            track = find(mincenter == j);
            pop(j) = size(track,1);
            if pop(j) == 0 continue; end
% it's correct to have mean(data(track,:),1) but this can make answer worse!
            centers(j,:) = mean(data(track,:),1);
        end
    end

    if method == 2
        for j = diffj
            offset = calcdist(centers(j,:),oldcenters(j,:));
            computed = computed + 1;
            if offset == 0 continue; end
            track = find(mincenter == j);
            mindist(track) = mindist(track) + offset;
            lower(:,j) = max(lower(:,j) - offset,0);
        end
%
% compute distance between each pair of centers
% modify centdist to make "find" using it faster.
%
        recalculated = zeros(n,1);
        realdist = alldist(centers);
        centdist = 0.5*realdist + diag(Inf*ones(k,1));
        computed = computed + k + k*(k-1)/2;
    end

    nchanged = size(diff,1) + skipestep;
    iteration = iteration+1;
    skipestep = 0;
    oldmincenter = mincenter;

%   difference = max(max(abs(oldcenters - centers)));
%   [iteration toc nchanged computed size(diffj,2)]
    fprintf ( 1, '%4d  %g  %d  %d\n', iteration, toc, nchanged, computed );
%   [iteration toc nchanged computed]
    total = total + computed;
end % while nchanged > 0

  udist = calcdist(data,centers(mincenter,:));
  quality = mean(udist);
  q2 = mean(udist.^2);
  %[iteration toc quality q2 total]
  fprintf ( 1, '  %4d  %g  %g  %g  %d\n', iteration, toc, quality, q2, total );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'KMEANS_FAST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


%%
% Copyright (c) 2019-present, Mahmoud Afifi
%
% This source code is licensed under the license found in the
% LICENSE file in the root directory of this source tree.
%
% Please, cite the following paper if you use this code:
%
% Mahmoud Afifi and Michael S. Brown. What else can fool deep learning?
% Addressing color constancy errors on deep neural network performance.
% ICCV, 2019
%
% Email: mafifi@eecs.yorku.ca | m.3afifi@gmail.com
%%

classdef PCAFeature
    properties
        weights
        bias
    end
    methods
        function feature = encode(obj,hist)
		% Generates a compacted feature of a given histogram tensor.
            feature = (reshape(hist,1,[]) - obj.bias') *obj.weights;
        end
    end
end


 function xs = eql_sps_os(x, Gb, yi, ci, ri, R, ...
		niter, pixmax, curv, relax0, chat)
%function xs = eql_sps_os(x, Gb, yi, ci, ri, R, ...
%		niter, pixmax, curv, relax0, chat)
%
% E-QPL-SPS-OS algorithm for emission Poisson problem
% (ordered subsets separable paraboloidal surrogates)
% in
%	x	[np,1]		initial estimate
%	Gb	[nd,np]		Gblock object (see eql_osps_test.m)
%	yi,ci,ri [nb,na]	see em_fbp.m (for model too)
%	R			penalty structure (see Robject.m)
%					(or empty for ML case)
%	niter			# iterations
%	pixmax			upper constraint for pixel values
%	curv		'oc' for erdogan's optimal curvatures
%			'pc' for erdogan's fast precomputed curvatures,
%				which usually provides faster convergence,
%				but can be nonmonotone.
%	relax0	[1] or [2]	relax0 or (relax0, relax_rate)
% out
%	xs [np,niter]	updated image vectors each iteration
%
% Copyright Mar 2000, Jeff Fessler, The University of Michigan

if nargin < 3, ir_usage, end

Gb = block_ob(Gb, 'ensure'); % make it a block object (if not already)
nblock = block_ob(Gb, 'n');
starts = subset_start(nblock);

if ~isvar('ci') || isempty(ci)
	ci = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end

if ~isvar('R'), R = []; end
if ~isvar('niter')	| isempty(niter),	niter = 1;	end
if ~isvar('pixmax')	| isempty(pixmax),	pixmax = inf;	end
if ~isvar('curv')	| isempty(curv),	curv = 'oc';	end
if ~isvar('chat')	| isempty(chat),	chat = false;	end

if ~isvar('relax0')	| isempty(relax0),	relax0 = 1;	end
if length(relax0) == 1
	relax_rate = 0;
elseif length(relax0) == 2
	relax_rate = relax0(2);
	relax0 = relax0(1);
else
	error relax
end

eml_check(yi, ci, ri, 'os', nblock);

[nb, na] = size(yi);

gi = sum(Gb')';		% g_i = sum_j g_ij
gi = reshape(gi, nb, na);

%
% precomputed curvatures
%
if streq(curv, 'pc')
	ni = ci.^2 ./ max(yi,1);	% precomputed
	ni = eml_curvature(yi, ci, ri, [], [], 'pc');

	% efficient single denominator consistent with aspire
	denom = Gb' * col(gi .* ni);
elseif ~streq(curv, 'oc')
	error 'curv not implemented'
end

%
% loop over iterations
%
xs = zeros(numel(x), niter);
x = max(x,0);
x = min(x,pixmax);
xs(:,1) = x;

for iter = 2:niter
	if chat, printf('E-QL-SPS-OS iteration %d', iter-1), end

	relax = relax0 / (1 + relax_rate * (iter-2));

	%
	% loop over subsets
	%
	for iset=1:nblock
		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Gb{iblock} * x;			% l=G*x "line integrals"
		li = reshape(li, nb, length(ia));
		yb = ci(:,ia) .* li + ri(:,ia);		% predicted meas. means

		% fix: need to be careful here with 0/0 -> 0
		dothi = ci(:,ia) .* (yi(:,ia) ./ yb - 1);
		grad = Gb{iblock}' * dothi(:);

		if streq(curv, 'oc')
			% optimal curvatures (for monotone increase)
			ni = eml_curvature(yi(:,ia), ci(:,ia), ri(:,ia), ...
					li, yb, 'oc');
			denom = Gb{iblock}' * col(gi(:,ia) .* ni);
			denom = nblock * denom;
		end

		if isempty(R)
			num = nblock * grad;
			den = denom;
		else
			num = nblock * grad - R.cgrad(R, x);
			den = denom + R.denom(R, x);
		end

		x = x + relax * num ./ den;	% relaxed update
		x = max(x,0);			% lower bound
		x = min(x,pixmax);		% upper bound
	end

	if chat, printf('Range %g %g', min(x), max(x)), end
	xs(:,iter) = x;
end


function [VM, hrf, fit, e, param] = Det_Logit(V0,t,tc,Run)
%
% [VM, h, fit, e, param] = Det_Logit_allstim(V0,t,tc,Run)
%
% Estimate inverse logit (IL) HRF model
% Creates fitted curve - 3 logistic functions to be summed together - from parameter estimates
%
% INPUT: V0, t, tc, Run
% Run = stick function
% tc = time course
% t = vector of time points
% V0 = initial value for the parameter vector
%
% By Martin Lindquist, Christian Waugh and Tor Wager
% Created by Martin Lindquist on 10/02/09
% Last edited: 05/26/10 (ML)

numstim = length(Run);
len = length(Run{1});

% LB = [0.05, 1, 0, 0.05, 4, 0, 10];      % Lower bounds for parameters
% UB = [10, 15, 10, 10, 15, 5, 50];           % Upper bounds for parameters
% LB = repmat(LB, 1, numstim);
% UB = repmat(UB, 1, numstim);

% Remove intercept

b0 = pinv(ones(length(tc),1))*tc;
tc = tc - b0;

% Find optimal values

options = optimset('MaxFunEvals',10000000,'Maxiter',10000000,'TolX',1e-8,'TolFun',1e-8,'Display','off');

%VM = fminsearchbnd(@cost_allstim, V0, LB,UB,options,t,tc,Run);
VM = fminsearch(@msq_logit,V0,options,Run,t,tc);

% Use optimal values to fit hemodynamic response functions
hrf =zeros(length(t),numstim);
fitt = zeros(len,numstim);
param = zeros(3,numstim);

for g = 1:numstim
    hrf(:,g) = il_hdmf_tw2(t,VM(((g-1)*7+1):(g*7)));                   % Calculate HRF estimate (fit, given theta)
    param(:,g) = get_parameters2(hrf(:,g),t(end));
    fits(:,g) = conv(Run{g}, hrf(:,g));
    fitt(:,g) = fits(1:len,g);
end

fit = sum(fitt,2);
e = tc-fit;
fit = fit + b0;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   SUBFUNCTIONS
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function m=msq_logit(V,Run, t, tc)

numstim = length(Run);
len = length(Run{1});
h = zeros(length(t),numstim);
yhatt =zeros(len,numstim);

for k = 1:numstim
    h(:,k) = il_hdmf_tw2(t,V(((k-1)*7+1):(k*7)));           % Get IL model corresponding to parameters V
    yhat(:,k) = conv(Run{k}, h(:,k));                     % Convolve IL model with stick function
    yhatt(:,k) = yhat(1:len,k);
end

yhat2 = sum(yhatt,2); %Sum models together to get overall estimate

m = sum((tc-yhat2).^2);              % Calculate cost function

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [h,base] = il_hdmf_tw2(t,V)
% inverse logit -- creates fitted curve from parameter estimates
%
% t = vector of time points
% V = parameters

% 3 logistic functions to be summed together
base = zeros(length(t),3);
A1 = V(1);
T1 = V(2);
d1 = V(3);
A2 = V(4);
T2 = V(5);
A3 = V(6);
T3 = V(7);
d2 = -d1*(ilogit(A1*(1-T1)) - ilogit(A3*(1-T3)))/(ilogit(A2*(1-T2)) + ilogit(A3*(1-T3)));
d3 = abs(d2)-abs(d1);

base(:,1)= d1*ilogit(A1*(t-T1))';
base(:,2)= d2*ilogit(A2*(t-T2))';
base(:,3)= d3*ilogit(A3*(t-T3))';
h = sum(base,2)';

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [L] = ilogit(t)
L = exp(t)./(1+exp(t));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% set system limits
system = mr.opts('MaxGrad', 10, 'GradUnit', 'mT/m', ...
    'MaxSlew', 50, 'SlewUnit', 'T/m/s', 'rfRingdownTime', 20e-6, ...
    'rfDeadTime', 100e-6, 'adcDeadTime', 10e-6);

% high-level sequence parameters
alpha=90;
sliceThickness=50e-3;
Nx = 2048;
Nrep = 1;
TE=30e-3;
TR = 6000e-3;

rf_90_duration=2.6e-3; % excitation pulse duration
rf_180_duration= 4.6e-3; % refocusing pulse duration

% create and populate the sequence
seq = mr.Sequence(system);              % Create a new sequence object

% Create excitation pulse and gradient
rf_90= mr.makeSLRpulse(pi/2,'duration',rf_90_duration,'timeBwProduct',7.88,'dwell',rf_90_duration/500,'passbandRipple',1,'stopbandRipple',1e-2,'filterType','ms','system',system);

%Create refocusing pulse and gradient
rf_180_1 = mr.makeAdiabaticPulse('wurst','duration',rf_180_duration,'bandwidth',6000,'dwell',rf_180_duration/500,'n_fac',20,'use','refocusing','system',system);

timebwproduct_90=8;   % "experimental" value for the used for SLR excitation
timebwproduct_180=22; % "experimental" value for the used refocusing pulse

grad_amplitude_90 = (timebwproduct_90/2.6e-3)/sliceThickness; %BW/thickness
grad_amplitude_180 = (timebwproduct_180/4.6e-3)/sliceThickness; %BW/thickness

% we cannot use mr.calcDuration(rf_90) and mr.calcDuration(rf_180_1) here because this already includes delays and RF dead times
gx = mr.makeTrapezoid('x','flatTime',rf_90_duration,'amplitude',grad_amplitude_90,'system',system);
gy = mr.makeTrapezoid('y','flatTime',rf_180_duration,'amplitude',grad_amplitude_180,'system',system);
gz = mr.makeTrapezoid('z','flatTime',rf_180_duration,'amplitude',grad_amplitude_180,'system',system);
gz_2 = mr.makeTrapezoid('z','flatTime',rf_180_duration,'amplitude',grad_amplitude_180,'system',system);

% refocusing area for the 90-degree pulse (needs to be remembered in the "refocusing budget")
area_90_toRefocus=gx.amplitude*(0.5*gx.fallTime+gx.flatTime-mr.calcRfCenter(rf_90));

% gradient spoiling
spoilMoment=10/sliceThickness;
gzSpoil=mr.makeTrapezoid('z','area',spoilMoment,'system',system);
gxSpoil=mr.makeTrapezoid('x','duration',2*mr.calcDuration(gzSpoil),'area',spoilMoment,'system',system);
gzSpoil_2=mr.makeTrapezoid('z','duration',2*mr.calcDuration(gzSpoil),'area',spoilMoment,'system',system);

% split x gradient at the end of the plato and calculate the rf_90 delay
gx_parts=mr.splitGradientAt(gx,mr.calcDuration(gx)-gx.fallTime+system.rfRingdownTime);
[gx_p1,rf_90,~]=mr.align('right',gx_parts(1),rf_90,mr.makeDelay(system.rfDeadTime+rf_90_duration+system.rfRingdownTime));
gx_parts(2).delay=0; % set the delay to 0 because it will be used in a separate block

% split y gradient at the end of the plato and calculate the rf_180_1 delay
gy_parts=mr.splitGradientAt(gy,mr.calcDuration(gy)-gy.fallTime+system.rfRingdownTime);
rf_180_1.delay=max(mr.calcDuration(gy_parts(1))-rf_180_duration-system.rfRingdownTime, mr.calcDuration(gzSpoil));
assert(rf_180_1.delay>=system.rfDeadTime);
gy_p1=gy_parts(1);
gy_p1.delay=rf_180_1.delay+rf_180_duration+system.rfRingdownTime-mr.calcDuration(gy_p1);
gy_parts(2).delay=0; % set the delay to 0 because it will be used in a separate block

% combine y gradient with delays to a new gradient the rf_180_2 delay
rf_180_2=rf_180_1;
rf_180_2.delay=max(mr.calcDuration(gxSpoil),mr.calcDuration(gzSpoil_2));
gy_tmp=mr.splitGradientAt(gy,mr.calcDuration(gy)-gy.fallTime);
gy_tmp(1).delay=rf_180_2.delay+rf_180_duration-mr.calcDuration(gy_tmp(1));
gySpoil=mr.makeExtendedTrapezoidArea('y',gy.amplitude,0,spoilMoment+0.5*gy.amplitude*gy.fallTime,system);
gySpoil.delay=mr.calcDuration(gy_tmp(1));
gy_comb=mr.addGradients({gy_parts(2),gy_tmp(1),gySpoil},'system',system);
gy_comb_parts=mr.splitGradientAt(gy_comb,rf_180_2.delay+rf_180_duration+system.rfRingdownTime);%
gy_comb_parts(2).delay=0;

%
gxSpoil_2=mr.makeTrapezoid('x','area',spoilMoment,'system',system);
rf_180_3=rf_180_1;
rf_180_3.delay=mr.calcDuration(gxSpoil_2);
gz.delay=rf_180_3.delay-gz.riseTime;

%Additional Spoiler gradients
gzSpoil_semiFinal=mr.makeExtendedTrapezoidArea('z',0,gz.amplitude,spoilMoment,system);
gxSpoil_semiFinal=mr.makeTrapezoid('x','area',spoilMoment+area_90_toRefocus,'system',system);
gySpoil_semiFinal=mr.makeTrapezoid('y','area',2*spoilMoment,'system',system);
[gxSpoil_semiFinal,gySpoil_semiFinal,gzSpoil_semiFinal]=mr.align('right',gxSpoil_semiFinal,gySpoil_semiFinal,gzSpoil_semiFinal);

rf_180_4=rf_180_1;
rf_180_4.delay=0;
gz_temp=mr.splitGradientAt(gz_2,gz_2.riseTime);
gz_temp(2).delay=0;
gz_parts=mr.splitGradientAt(gz_temp(2),rf_180_4.delay+rf_180_duration);
gz_parts(1).delay=mr.calcDuration(gzSpoil_semiFinal);
rf_180_4.delay=mr.calcDuration(gzSpoil_semiFinal);

gzSpoil_Final=mr.makeExtendedTrapezoidArea('z',gz.amplitude,0,spoilMoment,system);
gxSpoil_Final=mr.makeTrapezoid('x','area',spoilMoment,'system',system);
gySpoil_Final=mr.makeTrapezoid('y','area',spoilMoment,'system',system);

gzSpoil_Final.delay=rf_180_4.delay+rf_180_duration;
gz_comb=mr.addGradients({gzSpoil_semiFinal,gz_parts(1),gzSpoil_Final},'system',system);

gxSpoil_Final.delay=rf_180_4.delay+rf_180_duration;
gySpoil_Final.delay=rf_180_4.delay+rf_180_duration;

gxSpoil_combi=mr.addGradients({gxSpoil_semiFinal,gxSpoil_Final},'system',system);
gySpoil_combi=mr.addGradients({gySpoil_semiFinal,gySpoil_Final},'system',system);

%timing calculation
lTime1=(rf_90_duration+rf_180_duration)/2+system.rfRingdownTime+mr.calcDuration(gzSpoil);
lTime2=(rf_180_duration+rf_180_duration)/2+system.rfRingdownTime+mr.calcDuration(gxSpoil);
lTime3=(rf_180_duration+rf_180_duration)/2+system.rfRingdownTime+mr.calcDuration(gxSpoil_2);

lTime4=TE/2-lTime2;
lTime5=TE/2-lTime1-lTime3;

%Define ADC events
adc = mr.makeAdc(Nx, 'Dwell', 2e-4, 'system', system);
delayTE1=lTime4-(rf_180_duration+gz.fallTime+mr.calcDuration(gySpoil_semiFinal));
delayTE2=lTime5-(rf_180_duration/2+mr.calcDuration(gySpoil_Final)-gySpoil_Final.delay)-adc.dwell/2;
adc.delay=delayTE2;

% Loop over repetitions and define sequence blocks
for i=1:Nrep
    seq.addBlock(rf_90, gx_p1);
    seq.addBlock(gzSpoil,gx_parts(2),rf_180_1,gy_p1);
    seq.addBlock(gxSpoil,gzSpoil_2,rf_180_2, gy_comb_parts(1));
    seq.addBlock(gxSpoil_2,gy_comb_parts(2),rf_180_3, gz);
    seq.addBlock(mr.makeDelay(delayTE1));
    seq.addBlock(gxSpoil_combi,gySpoil_combi,gz_comb,rf_180_4);
    seq.addBlock(adc,mr.makeDelay(mr.calcDuration(adc)+system.adcDeadTime));

    % this is realy a lazy way of defining the TR delay
    if i==1
        delayTR = TR- seq.duration();
        assert(delayTR>0);
    end

    seq.addBlock(mr.makeDelay(delayTR))
end

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

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end
seq.setDefinition('FOV', [sliceThickness sliceThickness sliceThickness]);
seq.setDefinition('Name', 'semiLaser');

seq.write('semiLASER.seq');       % Write to pulseq file

%% do some visualizations

seq.plot('timeDisp','us','showBlocks',1,'timeRange',[0 TE*1.2]);
%seq.plot();             % Plot sequence waveforms

%% trajectory calculation
% [ktraj_adc, ktraj, t_excitation, t_refocusing, t_adc] = seq.calculateKspace();
% t_ktraj=(1:(size(ktraj,2)))*system.gradRasterTime;
[ktraj_adc, t_adc, ktraj, t_ktraj, t_excitation, t_refocusing] = seq.calculateKspacePP();
[ktraj_adc_ofs, t_adc_ofs, ktraj_ofs, t_ktraj_ofs] = seq.calculateKspacePP('gradient_offset',[1.0 .5 -1.0]*1e3); % this will help us to verify the correct echo positions

% plot "k-spaces" (gradient moments)
figure; plot(t_ktraj, ktraj'); % plot the entire k-space trajectory
hold on; plot(t_adc,ktraj_adc(1,:),'.'); % and sampling points on the kx-axis
plot(t_ktraj_ofs, ktraj_ofs');
axis([0 TE*2 -250 250]);
title('gradient moments without and with background gradients');

%% additional timing checks
% Karl Landheer et al define tau(1:5) and require tau_1+tau_2+tau_3 = tau_2+tau4
tau=diff([t_excitation t_refocusing t_adc(1)]);

if abs(sum(tau)-TE)>5e-5 % we tolerate an error of 1/2 grad rasters
    warning('TE calculation seems to be wrong, check timing!');
end

if abs(sum(tau([1 3 5]))-sum(tau([2 4])))>5e-5 % we tolerate an error of 1/2 grad rasters
    warning('spin echo condition is not fulfilled, check timing!');
end


function [TR, TT, ER, maxD, t] = icp(q,p,varargin)
% this is modified version of the original version
%
% Perform the Iterative Closest Point algorithm on three dimensional point
% clouds.
%
% [TR, TT] = icp(q,p)   returns the rotation matrix TR and translation
% vector TT that minimizes the distances from (TR * p + TT) to q.
% p is a 3xm matrix and q is a 3xn matrix.
%
% [TR, TT] = icp(q,p,k)   forces the algorithm to make k iterations
% exactly. The default is 10 iterations.
%
% [TR, TT, ER] = icp(q,p,k)   also returns the RMS of errors for k
% iterations in a (k+1)x1 vector. ER(0) is the initial error.
%
% [TR, TT, ER, t] = icp(q,p,k)   also returns the calculation times per
% iteration in a (k+1)x1 vector. t(0) is the time consumed for preprocessing.
%
% Additional settings may be provided in a parameter list:
%
% Boundary
%       {[]} | 1x? vector
%       If EdgeRejection is set, a vector can be provided that indexes into
%       q and specifies which points of q are on the boundary.
%
% EdgeRejection
%       {false} | true
%       If EdgeRejection is true, point matches to edge vertices of q are
%       ignored. Requires that boundary points of q are specified using
%       Boundary or that a triangulation matrix for q is provided.
%
% Extrapolation
%       {false} | true
%       If Extrapolation is true, the iteration direction will be evaluated
%       and extrapolated if possible using the method outlined by
%       Besl and McKay 1992.
%
% Matching
%       bruteForce | Delaunay | {kDtree}
%       Specifies how point matching should be done.
%       bruteForce is usually the slowest and kDtree is the fastest.
%       Note that the kDtree option is depends on the Statistics Toolbox
%       v. 7.3 or higher.
%
% Minimize
%       {point} | plane | lmaPoint
%       Defines whether point to point or point to plane minimization
%       should be performed. point is based on the SVD approach and is
%       usually the fastest. plane will often yield higher accuracy. It
%       uses linearized angles and requires surface normals for all points
%       in q. Calculation of surface normals requires substantial pre
%       proccessing.
%       The option lmaPoint does point to point minimization using the non
%       linear least squares Levenberg Marquardt algorithm. Results are
%       generally the same as in points, but computation time may differ.
%
% Normals
%       {[]} | n x 3 matrix
%       A matrix of normals for the n points in q might be provided.
%       Normals of q are used for point to plane minimization.
%       Else normals will be found through a PCA of the 4 nearest
%       neighbors.
%
% ReturnAll
%       {false} | true
%       Determines whether R and T should be returned for all iterations
%       or only for the last one. If this option is set to true, R will be
%       a 3x3x(k+1) matrix and T will be a 3x1x(k+1) matrix.
%
% Triangulation
%       {[]} | ? x 3 matrix
%       A triangulation matrix for the points in q can be provided,
%       enabling EdgeRejection. The elements should index into q, defining
%       point triples that act together as triangles.
%
% Verbose
%       {false} | true
%       Enables extrapolation output in the Command Window.
%
% Weight
%       {@(match)ones(1,m)} | Function handle
%       For point or plane minimization, a function handle to a weighting
%       function can be provided. The weighting function will be called
%       with one argument, a 1xm vector that specifies point pairs by
%       indexing into q. The weighting function should return a 1xm vector
%       of weights for every point pair.
%
% WorstRejection
%       {0} | scalar in ]0; 1[
%       Reject a given percentage of the worst point pairs, based on their
%       Euclidean distance.
%
% Martin Kjer and Jakob Wilm, Technical University of Denmark, 2012

% Use the inputParser class to validate input arguments.
inp = inputParser;

inp.addRequired('q', @(x)isreal(x) && size(x,1) == 3);
inp.addRequired('p', @(x)isreal(x) && size(x,1) == 3);

inp.addOptional('iter', 10, @(x)x > 0 && x < 10^5);

inp.addParamValue('Boundary', [], @(x)size(x,1) == 1);

inp.addParamValue('EdgeRejection', false, @(x)islogical(x));

inp.addParamValue('Extrapolation', false, @(x)islogical(x));

validMatching = {'bruteForce','Delaunay','kDtree'};
inp.addParamValue('Matching', 'kDtree', @(x)any(strcmpi(x,validMatching)));

validMinimize = {'point','plane','lmapoint'};
inp.addParamValue('Minimize', 'point', @(x)any(strcmpi(x,validMinimize)));

inp.addParamValue('Normals', [], @(x)isreal(x) && size(x,1) == 3);

inp.addParamValue('NormalsData', [], @(x)isreal(x) && size(x,1) == 3);

inp.addParamValue('ReturnAll', false, @(x)islogical(x));

inp.addParamValue('Triangulation', [], @(x)isreal(x) && size(x,2) == 3);

inp.addParamValue('Verbose', false, @(x)islogical(x));

inp.addParamValue('Weight', @(x)ones(1,length(x)), @(x)isa(x,'function_handle'));

inp.addParamValue('WorstRejection', 0, @(x)isscalar(x) && x > 0 && x < 1);

inp.addParamValue('SmartRejection', 0, @(x)isscalar(x) && x > 0);

inp.parse(q,p,varargin{:});
arg = inp.Results;
clear('inp');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Actual implementation

% Allocate vector for RMS of errors in every iteration.
t = zeros(arg.iter+1,1);

% Start timer
tic;

Np = size(p,2);

% Transformed data point cloud
pt = p;

% Allocate vector for RMS of errors in every iteration.
ER = zeros(arg.iter+1,1);
maxD = zeros(arg.iter,1);

% Initialize temporary transform vector and matrix.
T = zeros(3,1);
R = eye(3,3);

% Initialize total transform vector(s) and rotation matric(es).
TT = zeros(3,1, arg.iter+1);
TR = repmat(eye(3,3), [1,1, arg.iter+1]);

% If Minimize == 'plane', normals are needed
if (strcmp(arg.Minimize, 'plane') && isempty(arg.Normals))
    arg.Normals = lsqnormest(q,4);
end

% If Matching == 'Delaunay', a triangulation is needed
if strcmp(arg.Matching, 'Delaunay')
    DT = DelaunayTri(transpose(q));
end

% If Matching == 'kDtree', a kD tree should be built (req. Stat. TB >= 7.3)
if strcmp(arg.Matching, 'kDtree')
    kdOBJ = KDTreeSearcher(transpose(q));
end

% If edge vertices should be rejected, find edge vertices
if arg.EdgeRejection
    if isempty(arg.Boundary)
        bdr = find_bound(q, arg.Triangulation);
    else
        bdr = arg.Boundary;
    end
end

if arg.Extrapolation
    % Initialize total transform vector (quaternion ; translation vec.)
    qq = [ones(1,arg.iter+1);zeros(6,arg.iter+1)];
    % Allocate vector for direction change and change angle.
    dq = zeros(7,arg.iter+1);
    theta = zeros(1,arg.iter+1);
end

t(1) = toc;

% Go into main iteration loop
for k=1:arg.iter

    % Do matching
    switch arg.Matching
        case 'bruteForce'
            [match mindist] = match_bruteForce(q,pt);
        case 'Delaunay'
            [match mindist] = match_Delaunay(q,pt,DT);
        case 'kDtree'
            [match mindist] = match_kDtree(q,pt,kdOBJ);
    end

    % If matches to edge vertices should be rejected
    if arg.EdgeRejection
        p_idx = not(ismember(match, bdr));
        q_idx = match(p_idx);
        mindist = mindist(p_idx);
    else
        p_idx = true(1, Np);
        q_idx = match;
    end

    if k==1 && arg.SmartRejection
        arg.WorstRejection = sum(mindist > (median(mindist)* arg.SmartRejection))/ length(mindist);
        fprintf('ICP: Median=%f Threshold=%f WorstRejection=%f\n', median(mindist),median(mindist)* arg.SmartRejection,arg.WorstRejection);
        % vis
        %{
        figure
        hist(mindist,0:0.01/10:max(mindist)); hold on;
        plot(median(mindist),0,'g*');
        plot(median(mindist)* arg.SmartRejection,0,'r*');
        %}
    end

    % If worst matches should be rejected
    if arg.WorstRejection
        edge = round((1-arg.WorstRejection)*sum(p_idx));
        pairs = find(p_idx);
        [~, idx] = sort(mindist);
        p_idx(pairs(idx(edge:end))) = false;
        q_idx = match(p_idx);
        mindist = mindist(p_idx);
    end

    maxD(k) = max(mindist);

    if k == 1
        ER(k) = sqrt(sum(mindist.^2)/length(mindist));
    end

    switch arg.Minimize
        case 'point'
            % Determine weight vector
            weights = arg.Weight(match);
            [R,T] = eq_point(q(:,q_idx),pt(:,p_idx), weights(p_idx));
        case 'plane'
            weights = arg.Weight(match);
            [R,T] = eq_plane(q(:,q_idx),pt(:,p_idx),arg.Normals(:,q_idx),weights(p_idx));
        case 'lmaPoint'
            [R,T] = eq_lmaPoint(q(:,q_idx),pt(:,p_idx));
    end

    % Add to the total transformation
    TR(:,:,k+1) = R*TR(:,:,k);
    TT(:,:,k+1) = R*TT(:,:,k)+T;

    % Apply last transformation
    pt = TR(:,:,k+1) * p + repmat(TT(:,:,k+1), 1, Np);

    % Root mean of objective function
    ER(k+1) = rms_error(q(:,q_idx), pt(:,p_idx));

    % If Extrapolation, we might be able to move quicker
    if arg.Extrapolation
        qq(:,k+1) = [rmat2quat(TR(:,:,k+1));TT(:,:,k+1)];
        dq(:,k+1) = qq(:,k+1) - qq(:,k);
        theta(k+1) = (180/pi)*acos(dot(dq(:,k),dq(:,k+1))/(norm(dq(:,k))*norm(dq(:,k+1))));
        if arg.Verbose
            disp(['Direction change ' num2str(theta(k+1)) ' degree in iteration ' num2str(k)]);
        end
        if k>2 && theta(k+1) < 10 && theta(k) < 10
            d = [ER(k+1), ER(k), ER(k-1)];
            v = [0, -norm(dq(:,k+1)), -norm(dq(:,k))-norm(dq(:,k+1))];
            vmax = 25 * norm(dq(:,k+1));
            dv = extrapolate(v,d,vmax);
            if dv ~= 0
                q_mark = qq(:,k+1) + dv * dq(:,k+1)/norm(dq(:,k+1));
                q_mark(1:4) = q_mark(1:4)/norm(q_mark(1:4));
                qq(:,k+1) = q_mark;
                TR(:,:,k+1) = quat2rmat(qq(1:4,k+1));
                TT(:,:,k+1) = qq(5:7,k+1);
                % Reapply total transformation
                pt = TR(:,:,k+1) * p + repmat(TT(:,:,k+1), 1, Np);
                % Recalculate root mean of objective function
                % Note this is costly and only for fun!
                switch arg.Matching
                    case 'bruteForce'
                        [~, mindist] = match_bruteForce(q,pt);
                    case 'Delaunay'
                        [~, mindist] = match_Delaunay(q,pt,DT);
                    case 'kDtree'
                        [~, mindist] = match_kDtree(q,pt,kdOBJ);
                end
                ER(k+1) = sqrt(sum(mindist.^2)/length(mindist));
            end
        end
    end
    t(k+1) = toc;
end

if not(arg.ReturnAll)
    TR = TR(:,:,end);
    TT = TT(:,:,end);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [match mindist] = match_bruteForce(q, p)
    m = size(p,2);
    n = size(q,2);
    match = zeros(1,m);
    mindist = zeros(1,m);
    for ki=1:m
        d=zeros(1,n);
        for ti=1:3
            d=d+(q(ti,:)-p(ti,ki)).^2;
        end
        [mindist(ki),match(ki)]=min(d);
    end

    mindist = sqrt(mindist);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [match mindist] = match_Delaunay(q, p, DT)
	match = transpose(nearestNeighbor(DT, transpose(p)));
	mindist = sqrt(sum((p-q(:,match)).^2,1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [match mindist] = match_kDtree(~, p, kdOBJ)
	[match mindist] = knnsearch(kdOBJ,transpose(p));
    match = transpose(match);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [R,T] = eq_point(q,p,weights)

m = size(p,2);
n = size(q,2);

% normalize weights
weights = weights ./ sum(weights);

% find data centroid and deviations from centroid
q_bar = q * transpose(weights);
q_mark = q - repmat(q_bar, 1, n);
% Apply weights
q_mark = q_mark .* repmat(weights, 3, 1);

% find data centroid and deviations from centroid
p_bar = p * transpose(weights);
p_mark = p - repmat(p_bar, 1, m);
% Apply weights
%p_mark = p_mark .* repmat(weights, 3, 1);

N = p_mark*transpose(q_mark); % taking points of q in matched order

[U,~,V] = svd(N); % singular value decomposition

R = V*diag([1 1 det(U*V')])*transpose(U);

T = q_bar - R*p_bar;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [R,T] = eq_plane(q,p,n,weights)

n = n .* repmat(weights,3,1);

c = cross(p,n);

cn = vertcat(c,n);

C = cn*transpose(cn);

b = - [sum(sum((p-q).*repmat(cn(1,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(2,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(3,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(4,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(5,:),3,1).*n));
       sum(sum((p-q).*repmat(cn(6,:),3,1).*n))];

X = C\b;

cx = cos(X(1)); cy = cos(X(2)); cz = cos(X(3));
sx = sin(X(1)); sy = sin(X(2)); sz = sin(X(3));

R = [cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz;
     cy*sz cx*cz+sx*sy*sz cx*sy*sz-cz*sx;
     -sy cy*sx cx*cy];

T = X(4:6);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [R,T] = eq_lmaPoint(q,p)

Rx = @(a)[1     0       0;
          0     cos(a)  -sin(a);
          0     sin(a)  cos(a)];

Ry = @(b)[cos(b)    0   sin(b);
          0         1   0;
          -sin(b)   0   cos(b)];

Rz = @(g)[cos(g)    -sin(g) 0;
          sin(g)    cos(g)  0;
          0         0       1];

Rot = @(x)Rx(x(1))*Ry(x(2))*Rz(x(3));

myfun = @(x,xdata)Rot(x(1:3))*xdata+repmat(x(4:6),1,length(xdata));

options = optimset('Algorithm', 'levenberg-marquardt');
x = lsqcurvefit(myfun, zeros(6,1), p, q, [], [], options);

R = Rot(x(1:3));
T = x(4:6);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Extrapolation in quaternion space. Details are found in:
%
% Besl, P., & McKay, N. (1992). A method for registration of 3-D shapes.
% IEEE Transactions on pattern analysis and machine intelligence, 239?256.

function [dv] = extrapolate(v,d,vmax)

p1 = polyfit(v,d,1); % linear fit
p2 = polyfit(v,d,2); % parabolic fit
v1 = -p1(2)/p1(1); % linear zero crossing
v2 = -p2(2)/(2*p2(1)); % polynomial top point

if issorted([0 v2 v1 vmax]) || issorted([0 v2 vmax v1])
    disp('Parabolic update!');
    dv = v2;
elseif issorted([0 v1 v2 vmax]) || issorted([0 v1 vmax v2])...
        || (v2 < 0 && issorted([0 v1 vmax]))
    disp('Line based update!');
    dv = v1;
elseif v1 > vmax && v2 > vmax
    disp('Maximum update!');
    dv = vmax;
else
    disp('No extrapolation!');
    dv = 0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Determine the RMS error between two point equally sized point clouds with
% point correspondance.
% ER = rms_error(p1,p2) where p1 and p2 are 3xn matrices.

function ER = rms_error(p1,p2)
dsq = sum(power(p1 - p2, 2),1);
ER = sqrt(mean(dsq));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Converts (orthogonal) rotation matrices R to (unit) quaternion
% representations
%
% Input: A 3x3xn matrix of rotation matrices
% Output: A 4xn matrix of n corresponding quaternions
%
% http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion

function quaternion = rmat2quat(R)

Qxx = R(1,1,:);
Qxy = R(1,2,:);
Qxz = R(1,3,:);
Qyx = R(2,1,:);
Qyy = R(2,2,:);
Qyz = R(2,3,:);
Qzx = R(3,1,:);
Qzy = R(3,2,:);
Qzz = R(3,3,:);

w = 0.5 * sqrt(1+Qxx+Qyy+Qzz);
x = 0.5 * sign(Qzy-Qyz) .* sqrt(1+Qxx-Qyy-Qzz);
y = 0.5 * sign(Qxz-Qzx) .* sqrt(1-Qxx+Qyy-Qzz);
z = 0.5 * sign(Qyx-Qxy) .* sqrt(1-Qxx-Qyy+Qzz);

quaternion = reshape([w;x;y;z],4,[]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Converts (unit) quaternion representations to (orthogonal) rotation matrices R
%
% Input: A 4xn matrix of n quaternions
% Output: A 3x3xn matrix of corresponding rotation matrices
%
% http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#From_a_quaternion_to_an_orthogonal_matrix

function R = quat2rmat(quaternion)
q0(1,1,:) = quaternion(1,:);
qx(1,1,:) = quaternion(2,:);
qy(1,1,:) = quaternion(3,:);
qz(1,1,:) = quaternion(4,:);

R = [q0.^2+qx.^2-qy.^2-qz.^2 2*qx.*qy-2*q0.*qz 2*qx.*qz+2*q0.*qy;
     2*qx.*qy+2*q0.*qz q0.^2-qx.^2+qy.^2-qz.^2 2*qy.*qz-2*q0.*qx;
     2*qx.*qz-2*q0.*qy 2*qy.*qz+2*q0.*qx q0.^2-qx.^2-qy.^2+qz.^2];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Least squares normal estimation from point clouds using PCA
%
% H. Hoppe, T. DeRose, T. Duchamp, J. McDonald, and W. Stuetzle.
% Surface reconstruction from unorganized points.
% In Proceedings of ACM Siggraph, pages 71:78, 1992.
%
% p should be a matrix containing the horizontally concatenated column
% vectors with points. k is a scalar indicating how many neighbors the
% normal estimation is based upon.
%
% Note that for large point sets, the function performs significantly
% faster if Statistics Toolbox >= v. 7.3 is installed.
%
% Jakob Wilm 2010

function n = lsqnormest(p, k)
m = size(p,2);
n = zeros(3,m);

v = ver('stats');
if str2double(v.Version) >= 7.5
    neighbors = transpose(knnsearch(transpose(p), transpose(p), 'k', k+1));
else
    neighbors = k_nearest_neighbors(p, p, k+1);
end

for i = 1:m
    x = p(:,neighbors(2:end, i));
    p_bar = 1/k * sum(x,2);

    P = (x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k)); %spd matrix P
    %P = 2*cov(x);

    [V,D] = eig(P);

    [~, idx] = min(diag(D)); % choses the smallest eigenvalue

    n(:,i) = V(:,idx);   % returns the corresponding eigenvector
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Program to find the k - nearest neighbors (kNN) within a set of points.
% Distance metric used: Euclidean distance
%
% Note that this function makes repetitive use of min(), which seems to be
% more efficient than sort() for k < 30.

function [neighborIds neighborDistances] = k_nearest_neighbors(dataMatrix, queryMatrix, k)

numDataPoints = size(dataMatrix,2);
numQueryPoints = size(queryMatrix,2);

neighborIds = zeros(k,numQueryPoints);
neighborDistances = zeros(k,numQueryPoints);

D = size(dataMatrix, 1); %dimensionality of points

for i=1:numQueryPoints
    d=zeros(1,numDataPoints);
    for t=1:D % this is to avoid slow repmat()
        d=d+(dataMatrix(t,:)-queryMatrix(t,i)).^2;
    end
    for j=1:k
        [s,t] = min(d);
        neighborIds(j,i)=t;
        neighborDistances(j,i)=sqrt(s);
        d(t) = NaN; % remove found number from d
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Boundary point determination. Given a set of 3D points and a
% corresponding triangle representation, returns those point indices that
% define the border/edge of the surface.

function bound = find_bound(pts, poly)

%Correcting polygon indices and converting datatype
poly = double(poly);
pts = double(pts);

%Calculating freeboundary points:
TR = TriRep(poly, pts(1,:)', pts(2,:)', pts(3,:)');
FF = freeBoundary(TR);

%Output
bound = FF(:,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function stroud_test19 ( )

%*****************************************************************************80
%
%% TEST19 tests CUBE_UNIT_3D, QMULT_3D, RECTANGLE_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  a1 = -1.0;
  b1 = +1.0;

  a(1) = -1.0;
  a(2) = -1.0;
  a(3) = -1.0;
  b(1) = 1.0;
  b(2) = 1.0;
  b(3) = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST19\n' );
  fprintf ( 1, '  CUBE_UNIT_3D approximates integrals\n' );
  fprintf ( 1, '    in the unit cube in 3D.\n' );
  fprintf ( 1, '  QMULT_3D approximates triple integrals.\n' );
  fprintf ( 1, '  RECTANGLE_3D approximates integrals\n' );
  fprintf ( 1, '    in a rectangular block.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '    F(X)    CUBE_UNIT_3D  QMULT_3D      RECTANGLE_3D\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    result1 = cube_unit_3d ( 'function_3d' );
    result2 = qmult_3d ( 'function_3d', a1, b1, 'fu18', 'fl18', 'fu28', 'fl28' );
    result3 = rectangle_3d ( 'function_3d', a, b );

    fname = function_3d_name ( i );

    fprintf ( 1, '  %s  %12f  %12f  %12f\n', ...
      fname, result1, result2, result3 );

  end

  return
end


function [s,S1,S2] = makeSegment(p1,p2)

% MAKESEGMENT  Make a segment out of two endpoints
%   MAKESEGMENT(E1,E2) makes a segment out of the two endpoints E1 and E2
%   by stacking both endpoints [E1;E2];
%
%   [s,S1,S2] = MAKESEGMENT(E1,E2) returns the Jacobians of the segments
%   wrt the endpoints.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

s = [p1(:);p2(:)];

if nargout>1

    n  = numel(p1);
    S1 = [eye(n);zeros(n)];
    S2 = [zeros(n);eye(n)];

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 net = gp(nin, covar_fn, prior)
%GP	Create a Gaussian Process.
%
%	Description
%
%	NET = GP(NIN, COVARFN) takes the number of inputs NIN  for a Gaussian
%	Process model with a single output, together with a string COVARFN
%	which specifies the type of the covariance function, and returns a
%	data structure NET. The parameters are set to zero.
%
%	The fields in NET are
%	  type = 'gp'
%	  nin = number of inputs
%	  nout = number of outputs: always 1
%	  nwts = total number of weights and covariance function parameters
%	  bias = logarithm of constant offset in covariance function
%	  noise = logarithm of output noise variance
%	  inweights = logarithm of inverse length scale for each input
%	  covarfn = string describing the covariance function:
%	      'sqexp'
%	      'ratquad'
%	  fpar = covariance function specific parameters (1 for squared exponential,
%	   2 for rational quadratic)
%	  trin = training input data (initially empty)
%	  trtargets = training target data (initially empty)
%
%	NET = GP(NIN, COVARFN, PRIOR) sets a Gaussian prior on the parameters
%	of the model. PRIOR must contain the fields PR_MEAN and PR_VARIANCE.
%	If PR_MEAN is a scalar, then the Gaussian is assumed to be isotropic
%	and the additional fields NET.PR_MEAN and PR_VARIANCE are set.
%	Otherwise,  the Gaussian prior has a mean defined by a column vector
%	of parameters PRIOR.PR_MEAN and covariance defined by a column vector
%	of parameters PRIOR.PR_VARIANCE. Each element of PRMEAN corresponds
%	to a separate group of parameters, which need not be mutually
%	exclusive. The membership of the groups is defined by the matrix
%	PRIOR.INDEX in which the columns correspond to the elements of
%	PRMEAN. Each column has one element for each weight in the matrix, in
%	the order defined by the function GPPAK, and each element is 1 or 0
%	according to whether the parameter is a member of the corresponding
%	group or not.  The additional field NET.INDEX is set in this case.
%
%	See also
%	GPPAK, GPUNPAK, GPFWD, GPERR, GPCOVAR, GPGRAD
%

%	Copyright (c) Ian T Nabney (1996-2001)

net.type = 'gp';
net.nin = nin;
net.nout = 1;  % Only do single output GP

% Store log parameters
net.bias = 0;
net.min_noise = sqrt(eps);  % Prevent output noise collapsing completely
net.noise = 0;
net.inweights = zeros(1,nin);  % Weights on inputs in covariance function

covarfns = {'sqexp', 'ratquad'};

if sum(strcmp(covar_fn, covarfns)) == 0
  error('Undefined activation function. Exiting.');
else
  net.covar_fn = covar_fn;
end

switch covar_fn

  case 'sqexp'		% Squared exponential
    net.fpar = zeros(1,1);  % One function specific parameter

  case 'ratquad' 	% Rational quadratic
    net.fpar = zeros(1, 2); % Two function specific parameters

  otherwise
    error(['Unknown covariance function ', covar_fn]);
end

net.nwts = 2 + nin + length(net.fpar);

if nargin >= 3
  if size(prior.pr_mean) == [1 1]
    net.pr_mean = prior.pr_mean;
    net.pr_var = prior.pr_var;
  else
    net.pr_mean = prior.pr_mean;
    net.pr_var = prior.pr_var;
    net.index = prior.index;
  end
end

% Store training data as needed for gpfwd
net.tr_in = [];
net.tr_targets = [];


function [output_canvas] = image_blending_average(warped_img1,warped_img2)
    w1 = imfill(im2bw(uint8(warped_img1), 0),'holes');
    w2 = imfill(im2bw(uint8(warped_img2), 0),'holes');

    w1 = mat2gray(w1);
    w2 = mat2gray(w2);

    warped_img1 = double(warped_img1);
    warped_img2 = double(warped_img2);
    output_canvas(:,:,1) = ((warped_img1(:,:,1).*w1)+(warped_img2(:,:,1).*w2))./(w1+w2);
    output_canvas(:,:,2) = ((warped_img1(:,:,2).*w1)+(warped_img2(:,:,2).*w2))./(w1+w2);
    output_canvas(:,:,3) = ((warped_img1(:,:,3).*w1)+(warped_img2(:,:,3).*w2))./(w1+w2);
    output_canvas = uint8(output_canvas);
end


function Hilbert_EWT(ewt,t,f,sub,color)

%==================================================================
% function Hilbert_EWT(ewt,t,f,sub,color)
%
% This function display the time-frequency plane by applying
% the Hilbert transform on each EWT component.
%
% TO RUN THIS FUNCTION, YOU MUST HAVE FLANDRIN'S EMD TOOLBOX
%
% Inputs:
%   - ewt: output of the EWT transform
%   - t: the time vector corresponding to your input signal
%   - f: the input signal
%   - sub: permits to restrict the time-frequency plane to
%          frequencies from [0;pi/sub]. This value must be
%          >=1 (1 meaning the function display the entire TF-plane
%   - color: display the TF plane in grayscale if this parameter is
%            set to 0, otherwise the display is in color (useful to
%            generate figures for your papers!)
%
% Author: Jerome Gilles
% Institution: UCLA - Department of Mathematics
% Year: 2013
% Version: 1.0
%==================================================================

M=length(ewt);
ewtM=zeros(M,length(ewt{1}));
for i=1:M
   b=ewt{i};
   ewtM(i,:)=b';
end

if sub<1
    sub=1;
end

[Ae,fe,tte]=hhspectrum(ewtM);
[ime,tte,ffe]=toimage(Ae,fe);

disp_hhs2(ime,t,[],0,sub,color);
if color == 0
    subplot(6,1,1);plot(t,f,'black');
else
    subplot(6,1,1);plot(t,f);
end
axis tight


function MountainCarPlot( x,a,steps )
subplot(2,1,2);
set(gco,'BackingStore','off')  % for realtime inverse kinematics
set(gco,'Units','data')
xplot =-1.6:0.05:0.6;
yplot =sin(3*xplot);
%Mountain
h = area(xplot,yplot,-1.1);
set(h,'FaceColor',[.1 .7 .1])
hold on
% Car  [1 .7 .1]
plot([x(1)-0.075 x(1)+0.075] ,[sin(3*(x(1)-0.075))+0.2  sin(3*(x(1)+0.075))+0.2 ],'-','LineWidth',10,'Color',[1 .7 .1]);
% wheels
plot(x(1)-0.05,sin(3*(x(1)-0.05))+0.06,'ok','markersize',12,'MarkerFaceColor',[.5 .5 .5]);
plot(x(1)+0.05,sin(3*(x(1)+0.05))+0.06,'ok','markersize',12,'MarkerFaceColor',[.5 .5 .5]);

%Goal
plot(0.45,sin(3*0.5)+0.1,'-pk','markersize',15,'MarkerFaceColor',[1 .7 .1]);
% direction of the force
if (a<0)
      plot(x(1)-0.08-0.05,sin(3*(x(1)-0.05))+0.2,'<k','MarkerFaceColor','g','markersize',10);
elseif (a>0)
      plot(x(1)+0.08+0.05,sin(3*(x(1)+0.05))+0.2,'>k','MarkerFaceColor','g','markersize',10);
end

%ctitle(strcat ('Step: ',int2str(steps)));
%-----------------------
axis([-1.6 0.6 -1.1 1.5]);
drawnow
hold off


function [l, L_rf, L_sf, L_sk, L_hm, L_beta] = ...
    retroProjPlkLinFromPinHoleOnRob(Rf, Sf, Sk, hm, beta)

% RETROPROJPLKLINFROMPINHOLEONROB Retro-project Plucker line from pinhole on robot.
%
%   L = RETROPROJPLKLINFROMPINHOLEONROB(RF, SF, SK, HML, beta) gives the
%   retroprojected Plucker line L in World Frame from an observed
%   homogeneous line HM. RF and SF are Robot and Sensor Frames, SK is the
%   camera calibration parameters vector, HML is the detected homogeneous 2D
%   line and BETA is the non-measurable prior. L is a 6-vector :
%     L = [nx ny nz vx vy vz]'
%   with the 6 Plucker coordinates of a line in 3D space.
%
%   [L, L_rf, L_sf, L_k, L_hm, L_n] = ... returns the
%   Jacobians wrt RF.x, SF.x, SK, SC, HML and N.
%
%   See also INVPINHOLEIDP, FROMFRAMEIDP.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

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

if nargout == 1

    % L in Sensor Frame
    ls = invPinHolePlucker(Sk,hm,beta) ;

    % L in world frame
    lr = fromFramePlucker(Sf, ls) ;
    l  = fromFramePlucker(Rf, lr) ;

else

    % L in Sensor Frame
    [ls, LS_sk, LS_hm, LS_beta] = invPinHolePlucker(Sk,hm,beta) ;

    % L in world frame
    [lr, LR_sf, LR_ls] = fromFramePlucker(Sf, ls) ;
    [l,  L_rf,  L_lr]  = fromFramePlucker(Rf, lr) ;

    % chain rule for Jacobians
    L_sf   = L_lr*LR_sf ;
    L_ls   = L_lr*LR_ls ;
    L_sk   = L_ls*LS_sk ;
    L_hm   = L_ls*LS_hm ;
    L_beta = L_ls*LS_beta ;

end

return

%% test
syms rx ry rz ra rb rc rd sx sy sz sa sb sc sd u0 v0 au av h1 h2 h3 b1 b2 real

Rf.x = [rx;ry;rz;ra;rb;rc;rd];
Sf.x = [sx;sy;sz;sa;sb;sc;sd] ;

Rf   = updateFrame(Rf);
Sf   = updateFrame(Sf);
Sk   = [u0;v0;au;av];
hm   = [h1 h2 h3]';
beta = [b1 b2]';

%% test jacobian
[l,L_r,L_s,L_k,L_hm,L_beta] = retroProjPlkLinFromPinHoleOnRob(Rf,Sf,Sk,hm,beta)

%% down here tha Jac test - WARNING! IT TAKES AGES TO COMPUTE !!
simplify(L_r - jacobian(l,Rf.x))
simplify(L_s - jacobian(l,Sf.x))
% simplify(L_k - jacobian(l,Sk))
% simplify(L_hm - jacobian(l,hm))

%% numerical test
Rf.x = epose2qpose([0 0 0 deg2rad([0 0 0])]');
Sf.x = epose2qpose([0 0 0 deg2rad([90 0 90])]');
Rf   = updateFrame(Rf);
Sf   = updateFrame(Sf);
Sk   = [100 100 100 100]';
hm   = [1 0 0]';
beta = [1 0]';

[l,L_r,L_s,L_k,L_hm,L_beta]  = retroProjPlkLinFromPinHoleOnRob(Rf,Sf,Sk,hm,beta)
[l2,L2_s,L2_k,L2_hm,L2_beta] = retroProjectPlucker(Sf,Sk,hm,beta)

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


classdef ShFunc_Compliance < ShFunWithElasticPdes

    properties (Access = private)
        compliance
        fieldToPrint
        adjointProblem
        gradientGauss
    end

    methods (Access = public)

        function obj = ShFunc_Compliance(cParams)
            cParams.filterParams.quadratureOrder = 'LINEAR';
            obj.init(cParams);
            fileName = cParams.femSettings.fileName;
            obj.createEquilibriumProblem(fileName);
            obj.createOrientationUpdater();
        end

        function fP = addPrintableVariables(obj)
            phy = obj.getPdesVariablesToPrint();
            fP{1}.value = phy{1};
            fP{2}.value = obj.compliance/obj.value0;
            fP{3}.value = obj.designVariable.alpha;
            fP{4}.value = abs(obj.designVariable.alpha);
            fP{5}.value = permute(obj.gradientGauss,[3 1 2]);
            fP = obj.addHomogVariables(fP);
        end

        function v = getVariablesToPlot(obj)
            v{1} = obj.value*obj.value0;
        end

        function t = getTitlesToPlot(obj)
            t{1} = 'Compliance non scaled';
        end

        function fP = createPrintVariables(obj)
            types = {'Elasticity','ScalarGauss','VectorGauss'...
                        'VectorGauss','VectorGauss'};
            names = {'Primal','ComplianceGauss','AlphaGauss',...
                        'AlphaAbsGauss','GradientGauss'};
            fP = obj.obtainPrintVariables(types,names);
            fP = obj.addHomogPrintVariablesNames(fP);
        end

        function [fun, funNames] = getFunsToPlot(obj)
            mesh = obj.designVariable.mesh;
            phy = obj.physicalProblem;
            strain = phy.strainFun{1}; % !!!
            stress = phy.stressFun{1}; % !!!
            displ  = phy.uFun{1}; % !!!
            compl  = obj.compliance/obj.value0;

            quad = Quadrature.set(mesh.type);
            quad.computeQuadrature('LINEAR');

            aa.mesh       = mesh;
            aa.quadrature = quad;
            aa.fValues    = permute(compl, [3 2 1]);
            complFun = FGaussDiscontinuousFunction(aa);

            bb.mesh    = mesh;
            bb.fValues = obj.designVariable.alpha';
            alphaFun = P0Function(bb);

            fun      = {complFun, strain, stress, displ};
            funNames = {'compliance', 'strain', 'stress', 'u'};

            cc.mesh     = mesh;
            cc.filename = 'shfunc_compliance';
            cc.fun      = fun;
            cc.funNames = funNames;
%             pvPst = ParaviewPostprocessor(cc);
%             pvPst.print();
%             fp = FunctionPrinter(cc);
%             fp.print();
        end
    end

    methods (Access = protected)

        function solveState(obj)
            obj.physicalProblem.setC(obj.homogenizedVariablesComputer.C) % (:,:,7200,4); cmat
%             obj.physicalProblem.computeVariables();
            obj.physicalProblem.solve();
        end

        function solveAdjoint(obj)
            obj.adjointProblem = obj.physicalProblem;
        end

        function computeFunctionValue(obj)
            phy = obj.physicalProblem;
            dvolum = phy.getDvolume()';
            stress = phy.variables.stress;
            strain = phy.variables.strain;
            ngaus  = size(strain,1);
            nelem  = size(strain,3);

            c = zeros(nelem,ngaus);
            for igaus = 1:ngaus
                stressG = squeeze(stress(igaus,:,:));
                strainG  = squeeze(strain(igaus,:,:));
                e = stressG.*strainG;
                c(:,igaus) = c(:,igaus) + sum(e)';
            end
            obj.compliance = c;
            int = c.*dvolum;
            obj.value = sum(int(:));
        end

        function computeGradientValue(obj)
            obj.computeGradientInGauss();
            obj.gradient = obj.gradientGauss;
        end

        function computeGradientInGauss(obj)
            phy = obj.physicalProblem;
            ep    = phy.variables.strain;
            ngaus  = size(ep,1);
            nstre  = size(ep,2);
            nelem  = size(ep,3);
            g = zeros(nelem,ngaus,obj.nVariables);
            for igaus = 1:ngaus
                for istre = 1:nstre
                    for jstre = 1:nstre
                        eu_i = squeeze(ep(igaus,istre,:));
                        ep_j = squeeze(ep(igaus,jstre,:));
                        for ivar = 1:obj.nVariables
                            dCij = squeeze(obj.homogenizedVariablesComputer.dC(istre,jstre,ivar,:,igaus));
                            g(:,igaus,ivar) = g(:,igaus,ivar) + (-eu_i.*dCij.*ep_j);
                        end
                    end
                end
            end
            obj.gradientGauss = g;
        end

        function f = getPdesVariablesToPrint(obj)
            f{1} = obj.getPdeVariableToPrint(obj.physicalProblem);
        end

    end

end


function rec=iEWT2D_Ridgelet(ewt,mfb)

%==========================================================================
% function rec=iEWT2D_Ridgelet(ewt,mfb)
%
% This function performs the inverse 2D Empirical Ridgelet of ewt
% accordingly to the filter bank mfb .
%
% TO RUN THIS FUNCTION YOU NEED TO HAVE THE MATLAB POLARLAB TOOLBOX OF
% MICHAEL ELAD: http://www.cs.technion.ac.il/~elad/Various/PolarLab.zip
%
% Inputs:
%   -ewt: cell containing the 2D EWT components
%   -mfb: filter bank used during the EWT
%
% Output:
%   -rec: reconstructed image
%
% Author: Jerome Gilles - Giang Tran
% Institution: UCLA - Department of Mathematics
% Year: 2013
% Version: 1.0
%==========================================================================

PseudoFFT=zeros(size(ewt{1}));

% We perform the reconstruction of the Pseudo-Polar FFT domain by performing
% the adjoint operator of the EWT1D
for k=1:length(ewt)
    PseudoFFT=PseudoFFT+fftshift(fft(ewt{k}).*mfb{k},1);
end

rec=real(IPPFFT(PseudoFFT,1e-6));


function [Estimation_X] = RocEKF_propagate(Estimation_X, OdometryFromThis2Next, Sigma_ODO )

% retrieve v and w
v = OdometryFromThis2Next(1:3);
w = OdometryFromThis2Next(4:6);

NumberOfLandmarks = size(Estimation_X.landmarks, 2);
Jrw = jaco_r(-w);
ExpMinusM = so3_exp(-w);

temp = repmat({Estimation_X.orientation}, 2,1 );
adRobot = blkdiag(temp{:});
 adRobot(4:6,1:3)=skew(Estimation_X.position)*Estimation_X.orientation;

B2=zeros(3*NumberOfLandmarks ,  6 );
if NumberOfLandmarks>0
    for i=1:NumberOfLandmarks
    %B2( 3*i-2:3*i  , :  )= ExpMinusM*[  skew( Estimation_X.landmarks(1:3,i) -v  )*Jrw , -eye(3) ];
    B2( 3*i-2:3*i  , :  )= ExpMinusM*[  skew( Estimation_X.landmarks(1:3,i)   ) , -eye(3) ];
    %B2=B2*[eye(3) zeros(3,3); -skew(v) eye(3) ];
    end
end

%B1=-[Jrw  zeros(3,3); skew(v)*Jrw eye(3)];
B1=[Estimation_X.orientation  zeros(3,3); -skew( Estimation_X.orientation*v ) Estimation_X.orientation];
adA= [  B1;  B2    ];
%adA= [  -adRobot* B1; -B2    ];

odoCov=diag([w.^2;v.^2])*Sigma_ODO^2;

temp = repmat({ ExpMinusM  }, 2+NumberOfLandmarks,1 );
AA = blkdiag(temp{:});
AA(1:6,1:6)=eye(6);

% final update the covariance
Estimation_X.cov = AA*Estimation_X.cov*AA'+ adA*odoCov*adA';

% update position and orientation
Estimation_X.position = Estimation_X.position+Estimation_X.orientation*v;
Estimation_X.orientation = Estimation_X.orientation*so3_exp(w);

% update the local coordinates of landmarks
if NumberOfLandmarks>0

landmarks=Estimation_X.landmarks(1:3,:);
vv=repmat(v, 1, NumberOfLandmarks);
landmarks=ExpMinusM*(landmarks - vv  );
Estimation_X.landmarks(1:3,:)=landmarks;

end

end


function f = testFile(d1,d2,d3,d4,d5,d6,dth1,dth2,dth3,dth4,dth5,dth6,g,l1,l2,l3,l4,l5,m1,m2,m3,m4,m5,m6,th1,th2,th3,th4,th5,th6)
%TESTFILE
%    F = TESTFILE(D1,D2,D3,D4,D5,D6,DTH1,DTH2,DTH3,DTH4,DTH5,DTH6,G,L1,L2,L3,L4,L5,M1,M2,M3,M4,M5,M6,TH1,TH2,TH3,TH4,TH5,TH6)

%    This function was generated by the Symbolic Math Toolbox version 6.0.
%    04-Nov-2014 12:56:21

t2 = cos(th1);
t3 = dth2.^2;
t4 = th1-th2;
t5 = sin(t4);
t6 = dth3.^2;
t7 = th1-th3;
t8 = sin(t7);
t9 = dth4.^2;
t10 = th1-th4;
t11 = sin(t10);
t12 = dth5.^2;
t13 = th1-th5;
t14 = sin(t13);
t15 = cos(th2);
t16 = dth6.^2;
t17 = dth1.^2;
t18 = th2-th3;
t19 = sin(t18);
t20 = th2-th4;
t21 = sin(t20);
t22 = th2-th5;
t23 = sin(t22);
t24 = cos(th3);
t25 = th3-th4;
t26 = sin(t25);
t27 = th3-th5;
t28 = sin(t27);
t29 = cos(th4);
t30 = th4-th5;
t31 = sin(t30);
t32 = cos(th5);
t33 = th1-th6;
t34 = sin(t33);
t35 = th2-th6;
t36 = sin(t35);
t37 = th3-th6;
t38 = sin(t37);
t39 = th4-th6;
t40 = sin(t39);
t41 = th5-th6;
t42 = sin(t41);
f = [-d1.*g.*m1.*t2-g.*l1.*m2.*t2-g.*l1.*m3.*t2-g.*l1.*m4.*t2-g.*l1.*m5.*t2-g.*l1.*m6.*t2-d2.*l1.*m2.*t3.*t5-d3.*l1.*m3.*t6.*t8-d4.*l1.*m4.*t9.*t11-d5.*l1.*m5.*t12.*t14-d6.*l1.*m6.*t16.*t34-l1.*l2.*m3.*t3.*t5-l1.*l2.*m4.*t3.*t5-l1.*l2.*m5.*t3.*t5-l1.*l2.*m6.*t3.*t5-l1.*l3.*m4.*t6.*t8-l1.*l3.*m5.*t6.*t8-l1.*l3.*m6.*t6.*t8-l1.*l4.*m5.*t9.*t11-l1.*l4.*m6.*t9.*t11-l1.*l5.*m6.*t12.*t14;-d2.*g.*m2.*t15-g.*l2.*m3.*t15-g.*l2.*m4.*t15-g.*l2.*m5.*t15-g.*l2.*m6.*t15+d2.*l1.*m2.*t5.*t17-d3.*l2.*m3.*t6.*t19-d4.*l2.*m4.*t9.*t21-d5.*l2.*m5.*t12.*t23-d6.*l2.*m6.*t16.*t36+l1.*l2.*m3.*t5.*t17+l1.*l2.*m4.*t5.*t17+l1.*l2.*m5.*t5.*t17+l1.*l2.*m6.*t5.*t17-l2.*l3.*m4.*t6.*t19-l2.*l3.*m5.*t6.*t19-l2.*l3.*m6.*t6.*t19-l2.*l4.*m5.*t9.*t21-l2.*l4.*m6.*t9.*t21-l2.*l5.*m6.*t12.*t23;-d3.*g.*m3.*t24-g.*l3.*m4.*t24-g.*l3.*m5.*t24-g.*l3.*m6.*t24+d3.*l2.*m3.*t3.*t19+d3.*l1.*m3.*t8.*t17-d4.*l3.*m4.*t9.*t26-d5.*l3.*m5.*t12.*t28-d6.*l3.*m6.*t16.*t38+l2.*l3.*m4.*t3.*t19+l2.*l3.*m5.*t3.*t19+l1.*l3.*m4.*t8.*t17+l2.*l3.*m6.*t3.*t19+l1.*l3.*m5.*t8.*t17+l1.*l3.*m6.*t8.*t17-l3.*l4.*m5.*t9.*t26-l3.*l4.*m6.*t9.*t26-l3.*l5.*m6.*t12.*t28;-d4.*g.*m4.*t29-g.*l4.*m5.*t29-g.*l4.*m6.*t29+d4.*l2.*m4.*t3.*t21+d4.*l1.*m4.*t11.*t17+d4.*l3.*m4.*t6.*t26-d5.*l4.*m5.*t12.*t31-d6.*l4.*m6.*t16.*t40+l2.*l4.*m5.*t3.*t21+l2.*l4.*m6.*t3.*t21+l1.*l4.*m5.*t11.*t17+l1.*l4.*m6.*t11.*t17+l3.*l4.*m5.*t6.*t26+l3.*l4.*m6.*t6.*t26-l4.*l5.*m6.*t12.*t31;-d5.*g.*m5.*t32-g.*l5.*m6.*t32+d5.*l2.*m5.*t3.*t23+d5.*l1.*m5.*t14.*t17+d5.*l3.*m5.*t6.*t28+d5.*l4.*m5.*t9.*t31-d6.*l5.*m6.*t16.*t42+l2.*l5.*m6.*t3.*t23+l1.*l5.*m6.*t14.*t17+l3.*l5.*m6.*t6.*t28+l4.*l5.*m6.*t9.*t31;d6.*m6.*(-g.*cos(th6)+l2.*t3.*t36+l3.*t6.*t38+l1.*t17.*t34+l4.*t9.*t40+l5.*t12.*t42)];


function lr = LikelihoodR(ys,ar0,ma0,var0,ar1,ma1,var1)

%LIKELIHOODR Likelihood ratio between estimated ARMA models
%   lr = likelihoodR(y,ar0,ma0,var0,ar1,ma1,var1)
%   is the Likelihood ratio of the ARMA model ar0, ma0, var0
%   with respect to the ARMA model ar1, ma1, var1.
%
%   y can also be a matrix containing several segments of equal
%   length (segments in colums). The segments are considered to be
%   independent.
%
%   See also: LIKELIHOODR_MOD, KLINDEX_HAT.

% S. de Waele, March 2003.

lr = sum(KLIndex_hat(ys,ar0,ma0,var0)) - sum(KLIndex_hat(ys,ar1,ma1,var1));


function rr = ACF(rel_data, wins, up)
%ACF calculates the autocorrelation function, and finds the RR.
%
%	            ACF(rel_data, wins, up)
%
%	Inputs:
%       rel_data    .t  vector of times
%                   .v  vector of resp Sig values
%                   .fs sampling freq
%       up              universal parameters structure
%       wins        .t  vector of start times
%
%	Outputs:
%       rr          .t  vector of times of estimated RRs
%                   .v  vector of estimated RRs
%                   .f  vector of freqs of power spectrum
%                   .p  vector of powers of power spectrum
%

%% Setup

downsample_freq = up.paramSet.fft_resample_freq;    % it would be worth changing this - it changes the answer (try 1 Hz e.g.)
true_fs = rel_data.fs;

%% Cycle through windows
rr.t = mean([wins.t_start(:)' ; wins.t_end(:)']); rr.t = rr.t(:);
rr.v = nan(length(rr.t),1);

for win_no = 1 : length(wins.t_start)

    % extract relevant data
    rel_els = find(rel_data.t >= wins.t_start(win_no) & rel_data.t < wins.t_end(win_no));
    data.v = rel_data.v(rel_els);
    data.t = rel_data.t(rel_els);

    good_els = ~isnan(data.v);
    data.v = data.v(good_els);
    data.t = data.t(good_els);

    % Downsample
    data.filt.t = downsample(data.t, true_fs/downsample_freq);
    data.filt.v = decimate(data.v, true_fs/downsample_freq);
    data.filt.v = detrend(data.filt.v);

    data.filt.v = data.filt.v(:);
    data.filt.t = data.filt.t(:);

    % Find the cross correlation

    [r,lags] = xcorr(data.filt.v);

    % find the fft of the cross correlation

    % Find FFT
    WINLENGTH = length(r);      % data.filt is a column vector, non?
    NFFT = 2^nextpow2(WINLENGTH);           % Size of the FFT
    HAMMWIN = hamming(WINLENGTH);          % Gots me my Hamming window.  Mmm... Hamm.
    HAMMWIN = HAMMWIN(:);
    f_nyq = downsample_freq/2;                           % Nyquist frequency.  "It's a Nyquist thing."
    FREQS = f_nyq.*linspace(0, 1, NFFT/2+1);            % Array of correspondent FFT bin frequencies, in BR (RPM)
    % NB: the 60 stays regardless of window length because this is used to calculate the freq in bpm

    WINDATA = detrend(r);                      % Remove the LSE straight line from the data
    % used to be: WINDATA = detrend(data.filt.v, 'constant');                      % Remove the LSE straight line from the data
    WINDATA = WINDATA .* HAMMWIN;                           % Apply the Hamm
    myFFT = fft(WINDATA, NFFT);                             % Congratulations, madame - it's an FFT
    myFFT = myFFT(1 : NFFT/2 + 1);                          % Discrard the upper half, which reflects the lower half
    myFFT = 2.*abs(myFFT/NFFT);                             % Single-sided amplitude spectrum
    psdx = (1/(downsample_freq*NFFT)) * abs(myFFT).^2;
    psdx(2:end-1) = 2*psdx(2:end-1);

    data.power = 10*log10(psdx); data.power = data.power(:);
    data.freqs = FREQS; data.freqs = data.freqs(:);

    %% Find spectral peak
    [rr.v(win_no), rr.f{win_no}, rr.p{win_no}] = find_spectral_peak(data, up);

end

end


function ind = randompoint(prob, n)
%RANDOMNEW to generate n new point randomly from the mop problem given.

if (nargin==1)
    n=1;
end

randarray = rand(prob.pd, n);
lowend = prob.domain(:,1);
span = prob.domain(:,2)-lowend;
point = randarray.*(span(:,ones(1, n)))+ lowend(:,ones(1,n));
cellpoints = num2cell(point, 1);

indiv = struct('parameter',[],'objective',[], 'estimation', []);
ind = repmat(indiv, 1, n);
[ind.parameter] = cellpoints{:};

% estimation = struct('obj', NaN ,'std', NaN);
% [ind.estimation] = deal(repmat(estimation, prob.od, 1));
end


% Test file for chebfun constructor (periodic).

function pass = test_constructor_basic_periodic(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

%% Test construction of some basic functions:

% Some basic test functions:
FF = {@(x) exp(sin(pi*x)), @(x) exp([sin(pi*x), cos(pi*x)]), @(x) exp([sin(pi*x), cos(pi*x), -cos(pi*x).^2])};

for j = 1:numel(FF);
    % Initialise k:
    k = 0;

    % Pick the test function:
    F = FF{j};

    % Test on [-1 1]:
    f = chebfun(F, [-1, 1], pref, 'periodic');
    g = chebfun(F, [-1, 1], pref, 'trig');
    xx = linspace(-1, 1);
    err = norm(feval(f, xx) - F(xx), inf);
    pass(j, k+1) = (err < 10*eps*vscale(f)) && (norm(f-g) < pref.chebfuneps);
    pass(j, k+2) = err < 50*pref.chebfuneps;
    k = k + 2;

    % Test on [-1 1] (no domain passed):
    f = chebfun(F, pref, 'periodic');
    xx = linspace(-1, 1);
    err = norm(feval(f, xx) - F(xx), inf);
    pass(j, k+1) = err < 10*eps*vscale(f);
    pass(j, k+2) = err < 500*pref.chebfuneps;
    k = k + 2;

    % Test on [0 10000]:
    f = chebfun(F, [-100, 100], pref, 'periodic');
    g = chebfun(F, [-100, 100], pref, 'trig');
    xx = linspace(-100, 100);
    err = norm(feval(f, xx) - F(xx), inf);
    pass(j, k+1) = err < 1e3*eps*vscale(f);
    pass(j, k+2) = (err < 100*hscale(f)*pref.chebfuneps) && (norm(f-g) <...
        100*pref.chebfuneps);
    k = k + 2;

end

end


function val=soft_threshoda(w,alpha,mu)
%soft_threshoda
%
val=sign(w).*max(abs(w)-alpha*mu,0)/(1+alpha*mu);
end


function aa = fitnesstra2(pop,para)
 global angles
[pops,numvar]=size(pop);
for i=1:pops
    time=pop(i,5)+pop(i,6);
    kk=trajt(para,pop(i,:));
    angles= kk(1:2,[5,7,10,13,15,17,20,23,25,27,30,33,35]);
    tt=torque(kk);
    ft=ftorque(tt);
    poo=kk(1:2,:);
    fq=sum(sum(abs(diff(poo'))));
    pos=forkin(poo);xx=pos(1,:);yy=pos(2,:);
    x=diff(xx);
    y=diff(yy);
    dis=sqrt(x.^2+y.^2);
    fdis=sum(dis);
    a1=2.3;
    a2=1.8;
    a3=2;
    a4=1;
    f=a1*ft+a2*fq+a3*fdis+a4*time;
    fob=fobstacle2(xx,yy);
    aa(i)=fob/f;
end