%% GrabCut segmentation demo
% Interactive foreground extraction using the GrabCut algorithm.
%
% This program demonstrates GrabCut segmentation: select an object in a
% region and then grabcut will attempt to segment it out.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/grabcut.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/grabcut.py>
% * <https://docs.opencv.org/3.2.0/d8/d83/tutorial_py_grabcut.html>
%

%% Theory
%
% GrabCut algorithm was designed by Carsten Rother, Vladimir Kolmogorov and
% Andrew Blake from Microsoft Research Cambridge, UK. in their paper:
%
% * "GrabCut": interactive foreground extraction using iterated graph cuts
%   <http://dl.acm.org/citation.cfm?id=1015720>
%
% An algorithm was needed for foreground extraction with minimal user
% interaction, and the result was *GrabCut*.
%
% How it works from user point of view? Initially user draws a rectangle
% around the foreground region (foreground region should be completely inside
% the rectangle). Then algorithm segments it iteratively to get the best
% result. Done. But in some cases, the segmentation won't be fine, like, it
% may have marked some foreground region as background and vice versa. In that
% case, user need to do fine touch-ups. Just give some strokes on the images
% where some faulty results are there. Strokes basically says: "Hey, this
% region should be foreground, you marked it background, correct it in next
% iteration", or its opposite for background. Then in the next iteration, you
% get better results.
%
% See the image below. First player and football is enclosed in a blue
% rectangle. Then some final touchups with white strokes (denoting foreground)
% and black strokes (denoting background) is made. And we get a nice result.
%
% <<https://docs.opencv.org/3.2.0/grabcut_output1.jpg>>
%
% So what happens in background ?
%
% * User inputs the rectangle. Everything outside this rectangle will be taken
%   as sure background (That is the reason it is mentioned before that your
%   rectangle should include all the objects). Everything inside rectangle is
%   unknown. Similarly any user input specifying foreground and background are
%   considered as hard-labelling which means they won't change in the process.
% * Computer does an initial labelling depeding on the data we gave. It labels
%   the foreground and background pixels (or it hard-labels)
% * Now a Gaussian Mixture Model (GMM) is used to model the foreground and
%   background.
% * Depending on the data we gave, GMM learns and create new pixel
%   distribution. That is, the unknown pixels are labelled either probable
%   foreground or probable background depending on its relation with the other
%   hard-labelled pixels in terms of color statistics (It is just like
%   clustering).
% * A graph is built from this pixel distribution. Nodes in the graphs are
%   pixels. Additional two nodes are added, *Source node* and *Sink node*.
%   Every foreground pixel is connected to source node and every background
%   pixel is connected to sink node.
% * The weights of edges connecting pixels to source node/end node are defined
%   by the probability of a pixel being foreground/background. The weights
%   between the pixels are defined by the edge information or pixel
%   similarity. If there is a large difference in pixel color, the edge
%   between them will get a low weight.
% * Then a mincut algorithm is used to segment the graph. It cuts the graph
%   into two separating source node and sink node with minimum cost function.
%   The cost function is the sum of all weights of the edges that are cut.
%   After the cut, all the pixels connected to source node become foreground
%   and those connected to sink node become background.
% * The process is continued until the classification converges.
%
% It is illustrated in below image
% (Image Courtesy: <http://www.cs.ru.ac.za/research/g02m1682/>)
%
% <<https://docs.opencv.org/3.2.0/grabcut_scheme.jpg>>
%

%% Code
% This is an interactive tool using grabcut. You can also watch this
% <http://www.youtube.com/watch?v=kAwxLTDDAwU youtube video> on how to use it.
%

function varargout = grabcut_demo_gui(im)
    % load an image
    if nargin < 1
        src = imread(fullfile(mexopencv.root(),'test','fruits.jpg'));
    elseif isempty(im)
        fmts = imformats();
        filtspec = strjoin(strcat('*.', [fmts.ext]), ';');
        [fn,fp] = uigetfile(filtspec, 'Select an image');
        if fp==0, error('No file selected'); end
        src = imread(fullfile(fp,fn));
    elseif ischar(im)
        src = imread(im);
    else
        src = im;
    end

    % we expect an 8-bit RGB image
    validateattributes(src, {'uint8'}, ...
        {'ndims',3, 'size',[nan nan 3], 'nonempty'});

    % initialize app state, and create the UI
    app = initApp(src);
    h = buildGUI(src, app);

    % hook event handlers
    opts = {'Interruptible','off', 'BusyAction','cancel'};
    set(h.pop, 'Callback',@onChange);
    set(h.btn(1), 'Callback',@onHelp);
    set(h.btn(2), 'Callback',@onReset);
    set(h.btn(3), 'Callback',@onNext, opts{:});
    set(h.fig, 'WindowKeyPressFcn',@onType, ...
        'WindowButtonDownFcn',@onMouseDown, opts{:});

    % return graphics handles
    if nargout > 0, varargout{1} = h; end

    % ========== Event Handlers ==========

    function onHelp(~,~)
        %ONHELP  Display usage help dialog

        helpdlg({
            'This program demonstrates GrabCut segmentation:'
            'select an object in a region and then grabcut'
            'will attempt to segment it out.'
            ''
            'Select a rectangular area around the object you'
            'want to segment.'
            ''
            'Then press "next" to segment the object (once or a few times).'
            ''
            'For finer touch-ups, set the mode and draw lines on the areas you'
            'want, to mark them as foreground/background (sure or probable),'
            'then press "next" again.'
            ''
            'Hot keys:'
            'ESC - quit the program'
            'r - restore the original image'
            'n - next iteration'
            'left mouse button - First set rectangle, then set pixels'
            '    as either BGD/FGD/PR_BGD/PR_FGD depending on selected'
            '    mode in dropdown menu.'
        });
    end

    function onReset(~,~)
        %ONRESET  Event handler for reset button

        app.mask(:) = 0;
        app.bgdModel(:) = 0;
        app.fgdModel(:) = 0;
        app.rect = zeros(0,4);
        app.rectxy = zeros(0,2);
        app.pts = repmat({zeros(0,2)}, [1 4]);
        app.iterCount = 0;
        app.isInitialized = false;

        set(h.txt, 'String','Iter =  0');
        set(h.img, 'CData',app.img0);
        set(h.rect, 'XData',NaN, 'YData',NaN);
        set(h.line(:), 'XData',NaN, 'YData',NaN);
        drawnow;
    end

    function onNext(~,~)
        %ONNEXT  Event handler for next button

        if app.isInitialized
            % set pixels in GC mask using drawing points
            if any(~cellfun(@isempty, app.pts))
                setLblsInMask();
            end
            % continue using current mask
            tic
            [app.mask, app.bgdModel, app.fgdModel] = cv.grabCut(...
                app.img0, app.mask, 'Mode','Eval', 'IterCount',1, ...
                'BgdModel',app.bgdModel, 'FgdModel',app.fgdModel);
            toc
        elseif any(~cellfun(@isempty, app.pts))
            % set foreground pixels in GC mask using rectangle
            setRectInMask();
            % set pixels in GC mask using drawing points
            setLblsInMask();
            % init using mask
            tic
            [app.mask, app.bgdModel, app.fgdModel] = cv.grabCut(...
                app.img0, app.mask, 'Mode','InitWithMask', 'IterCount',1);
            toc
        elseif ~isempty(app.rect)
            % init using rectangle
            rect = app.rect - [1 1 0 0];
            tic
            [app.mask, app.bgdModel, app.fgdModel] = cv.grabCut(...
                app.img0, rect, 'Mode','InitWithRect', 'IterCount',1);
            toc
        else
            disp('First select object to segment by drawing a rectangle');
            return;
        end

        % mark mask as initialized, and increment counter
        app.isInitialized = true;
        app.iterCount = app.iterCount + 1;

        % show result
        showImage();
    end

    function onType(~,e)
        %ONTYPE  Event handler for key press on figure

        % handle keys
        switch e.Key
            case {'q', 'escape'}
                close(h.fig);

            case 'h'
                onHelp([],[]);

            case 'r'
                onReset([],[]);

            case 'n'
                onNext([],[]);

            case {'add', 'subtract'}
                % adjust brush thickness
                if strcmp(e.Character, '+')
                    app.thick = min(app.thick + 2, 40);
                elseif strcmp(e.Character, '-')
                    app.thick = max(app.thick - 2, 1);
                end
                set(h.line(:), 'MarkerSize',app.thick*5.4);

            case {'1', '2', '3', '4'}
                % set brush value
                app.currIdx = str2double(e.Key);
                set(h.pop, 'Value',app.currIdx);
        end
    end

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

        % change current GC mask drawing value: BGD/FGD/PR_BGD/PR_FGD
        app.currIdx = get(h.pop, 'Value');
    end

    function onMouseDown(~,~)
        %ONMOUSEDOWN  Event handler for mouse down on figure

        % ignore anything but left mouse clicks
        if ~strcmp(get(h.fig,'SelectionType'), 'normal')
            return;
        end

        % one of two phases: drawing rectangle, or free-drawing of points
        if isempty(app.rect)
            % select and draw rectangle
            select_rectangle();
            if isempty(app.rect), return; end
            set(h.rect, 'XData',app.rectxy(:,1), 'YData',app.rectxy(:,2));
        else
            % attach event handlers, and change mouse pointer
            set(h.fig, 'Pointer','circle', ...
                'WindowButtonMotionFcn',@onMouseMove, ...
                'WindowButtonUpFcn',@onMouseUp);
        end
    end

    function onMouseMove(~,~)
        %ONMOUSEMOVE  Event handler for mouse move on figure

        % get current point and append it
        app.pts{app.currIdx}(end+1,:) = getCurrentPoint();

        % update corresponding graphic line
        set(h.line(app.currIdx), ...
            'XData',app.pts{app.currIdx}(:,1), ...
            'YData',app.pts{app.currIdx}(:,2));
    end

    function onMouseUp(~,~)
        %ONMOUSEUP  Event handler for mouse up on figure

        % detach event handlers, and restore mouse pointer
        set(h.fig, 'Pointer','arrow', ...
            'WindowButtonMotionFcn','', ...
            'WindowButtonUpFcn','');
    end

    % ========== Helper Functions ==========

    function showImage()
        out = app.img0;
        if app.isInitialized
            % zero-out background pixels
            if true
                binMask = repmat(app.mask == 0 | app.mask == 2, [1 1 3]);
                out(binMask) = 0;
            else
                binMask = (app.mask == 1 | app.mask == 3);
                out = cv.bitwise_and(out, out, 'Mask',binMask);
            end
        end
        set(h.img, 'CData',out);
        set(h.txt, 'String',sprintf('Iter = %2d',app.iterCount));
        drawnow;
    end

    function setRectInMask()
        % convert rectangle to binary mask
        rect_mask = poly2mask(app.rectxy(:,1), app.rectxy(:,2), ...
            app.sz(1), app.sz(2));

        % set foreground pixels in GC mask using rectangle
        app.mask(:) = 0;          % BGD
        app.mask(rect_mask) = 3;  % PR_FGD
    end

    function setLblsInMask()
        % set pixels in GC mask from drawing points: BGD, FGD, PR_BGD, PR_FGD
        for i=1:4
            app.mask = cv.circle(app.mask, app.pts{i}-1, app.thick, ...
                'Color',uint8(i-1), 'Thickness','Filled');
        end

        % clear drawing points after being processed
        app.pts = repmat({zeros(0,2)}, [1 4]);
        set(h.line(:), 'XData',NaN, 'YData',NaN);
    end

    function select_rectangle()
        %TODO: consider IMRECT from image_toolbox
        % create rubberband box to prompt user for a rectangle
        p1 = getCurrentPoint(); % retrieve mouse location before dragging
        rbbox;                  % ignore its output (figure coordinates)
        pause(0.005);           % CP might not get updated if selection was too fast
        p2 = getCurrentPoint(); % retrieve mouse location after dragging

        % form rectangle from two points: [x y w h]
        tl = min([p1;p2]);   % top-left corner
        br = max([p1;p2]);   % bottom-right corner
        if all((br-tl) > 1)  % ignore small rectangles
            app.rect = [tl br-tl];
            app.rectxy = [tl; tl+[app.rect(3) 0]; br; tl+[0 app.rect(4)]; tl];
        end
    end

    function p = getCurrentPoint()
        % retrieve current mouse location
        p = get(h.ax, 'CurrentPoint');
        p = p(1,1:2);

        % clamp to within image coordinates
        p = max(p, [1 1]);
        p = min(p, [app.sz(2) app.sz(1)]);
    end
end

% ========== Initializer functions ==========

function app = initApp(img)
    %INITAPP  Initialize app state

    app = struct();
    app.img0 = img;              % original image
    app.sz = size(img);          % image size
    app.mask = zeros(size(img,1), size(img,2), 'uint8'); % GC mask
    app.bgdModel = zeros(1,64);  % GC background model
    app.fgdModel = zeros(1,64);  % GC foreground model
    app.currIdx = 1;             % drawing value (BGD/FGD/PR_BGD/PR_FGD)
    app.thick = 5;               % drawing thickness
    app.pts = repmat({zeros(0,2)}, [1 4]); % drawing points of each brush
    app.rect = zeros(0, 4);      % rectangle [x,y,w,h]
    app.rectxy = zeros(0,2);     % rectangle points [TL;TR;BR;BL;TL]
    app.iterCount = 0;           % iterations counter
    app.isInitialized = false;   % whethet GC mask is initialized
end

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

    % parameters
    sz = size(img);
    sz(2) = max(sz(2), 350);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.fig = figure('Name','GrabCut Demo', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+29]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, ...
        'Units','pixels', 'Position',[1 30 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(img, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(img);
    end
    %axis(h.ax, 'on');
    h.btn(1) = uicontrol('Parent',h.fig, 'Style','pushbutton', ...
        'Position',[5 5 60 20], 'String','Help');
    h.btn(2) = uicontrol('Parent',h.fig, 'Style','pushbutton', ...
        'Position',[70 5 60 20], 'String','Reset');
    h.btn(3) = uicontrol('Parent',h.fig, 'Style','pushbutton', ...
        'Position',[135 5 60 20], 'String','Next');
    h.txt = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[200 5 60 20], 'String','Iter =  0');
    h.pop = uicontrol('Parent',h.fig, 'Style','popupmenu', ...
        'Position',[260 5 80 20], 'String',{'BGD','FGD','PR_BGD','PR_FGD'});

    % initialize lines (drawing and rectangle selection)
    clr = 'kwgrb';  % 'rbcmg'
    for i=1:4
        h.line(i) = line(NaN, NaN, 'Color',clr(i), 'Parent',h.ax, ...
            'LineStyle','none', 'Marker','.', 'MarkerSize',app.thick*5.4);
    end
    h.rect = line(NaN, NaN, 'Color',clr(5), 'Parent',h.ax, 'LineWidth',2);
end


function node_order = tet_mesh_node_order ( tetra_order, tetra_num, ...
  tetra_node, node_num )

%*****************************************************************************80
%
%% TET_MESH_NODE_ORDER: determines the order of nodes.
%
%  Discussion:
%
%    The order of a node is the number of tetrahedrons that use that node
%    as a vertex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TETRA_ORDER, the order of the tetrahedrons.
%
%    Input, integer TETRA_NUM, the number of tetrahedrons.
%
%    Input, integer TETRA_NODE(TETRA_ORDER,TETRA_NUM), the nodes
%    that make up the tetrahedrons.
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Output, integer NODE_ORDER(NODE_NUM), the order of each node.
%
  node_order(1:node_num) = 0;

  for tetra = 1 : tetra_num
    for i = 1 : tetra_order
      node = tetra_node(i,tetra);
      if ( node < 1 | node_num < node )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'TET_MESH_NODE_ORDER - Fatal error!\n' );
        fprintf ( 1, '  Illegal entry in TETRA_NODE.\n' );
        error ( 'TET_MESH_NODE_ORDER - Fatal error!' );
      else
        node_order(node) = node_order(node) + 1;
      end
    end
  end

  return
end


classdef PoseNode < handle
    %POSENODE A class for pose node

    properties (Access = public)
        id    % Id of this pose node
        pose  % Pose of this pose node
    end  % properties public

    properties (Dependent = true)
        x    % X coordinate
        y    % Y coordinate
        yaw  % Yaw angle
        rt   % Transformation local to global
    end  % properties dependent

    methods

        function obj = PoseNode(id, pose)
            % Constructor of PoseNode
            obj.id   = id;
            obj.pose = pose(:);
        end

        function plot(obj)
            % Plot all pose nodes position
            x = [obj.x];
            y = [obj.y];
            plot(x, y, 'b');
        end

        function x = get.x(obj)
            x = obj.pose(1);
        end

        function y = get.y(obj)
            y = obj.pose(2);
        end

        function yaw = get.yaw(obj)
            yaw = obj.pose(3);
        end

        function rt = get.rt(obj)
            R = [cos(obj.yaw) -sin(obj.yaw);
                 sin(obj.yaw)  cos(obj.yaw)];
            rt = [R [obj.x; obj.y]; 0 0 1];
        end

    end  % methods public

end  % classdef


%this is the code for AAAI 2016 paper: Top-N Recommender System via Matrix
%Completion by zhao kang,chong peng,qiang cheng.

%[hr,arhr,X] = mc_logdet(data,mu,rho)
function [X] = demo(data,mu,rho,toler,maxiter)
  [m,n]=size(data);
  M=data;
  id=find(M==0);
  ID=ones(m,n);
  ID(id)=0;
  [X] = MC_LogDet_v3(M,ID,mu,rho,toler,maxiter);
end


% CONDSTAT - accumulate surrogate data for comparing two data conditions
%
% Usage:
%     >> [diffres, accres, res1, res2] = condstat(formula, naccu, alpha, ...
%                                              bootside, condboot, arg1, arg2 ...);
%
% Inputs:
%    formula  - [string or cell array of strings] formula(s) to compute a given measure.
%               Takes arguments 'arg1', 'arg2' ... and X as inputs. e.g.,
%        'sum(arg1(:,:,X),3) ./ sqrt(sum(arg2(:,:,X))) ./ sqrt(sum(arg3(:,:,X)))'
%    naccu    - [integer] number of accumulations of surrogate data. e.g., 200
%    alpha    - [float] significance level (0<alpha<0.5)
%    bootside - ['both'|'upper'] side of the surrogate distribution to
%               consider for significance. This parameter affect the size
%               of the last dimension of accumulation array 'accres' (size
%               is 2 for 'both' and 1 for 'upper').
%    condboot - ['abs'|'angle'|'complex'|''] When comparing two conditions, compare
%               either absolute vales ('abs'), angles ('angles') or complex values
%               ('complex'). Either '' or 'complex' leave the formula unchanged;
%               'abs' takes its norm before subtraction, and 'angle' normalizes
%               each value (to norm 1) before taking the difference.
%    arg1     - [cell_array] of two 1D,2D,or 3D matrices of values to compare.
%               The last dimension of the array is shuffled to accumulate data, the
%               other dimensions must be the same size across matrices.
%               e.g. size(arg1{1})=[100 200 500], size(arg1{2})=[100 200 395]
%    arg2     - same as arg1, note that it is compared only to itself, and has
%               nothing to do with arg1 besides using the same formula, alpha, etc.
% ...argn     - may call n number of argument pairs
%
% Outputs:
%    diffres  - difference array for the actual (non-shuffled) data, if more than one
%               arg pair is called, format is a cell array of matrices.
%    accres  -  [cell array of 3D numerical arrays] for shuffled data, one per formula.
%    res1     - result for first condition
%    res2     - result for second condition
%
% Authors: Arnaud Delorme & Scott Makeig
%          CNL/Salk Institute 1998-2001; SCCN/INC/UCSD, La Jolla, 2002-
%
% See also: TIMEF, CROSSF

% Copyright (C) 2002  Arnaud Delorme, Lars Kai Hansen & Scott Makeig, SCCN/INC/UCSD
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [diffres, accres, res1, res2] = condstat(formula, naccu, alpha, bootside, condboot, varargin);

if nargin < 6
	help condstat;
	return;
end

if ~ischar(formula) && ~iscell(formula)
	error('The first argument must be a string formula or cell array of string');
end
if ischar(formula)
	formula = { formula };
end
if ischar(bootside)
	bootside = { bootside };
end
for index = 1:length(bootside)
	if ~strcmpi(bootside{index}, 'both') && ~strcmpi(bootside{index}, 'upper')
		error('Bootside must be either ''both'' or ''upper''');
	end
end
if ischar(condboot)
	condboot = { condboot };
end
for index = 1:length(condboot)
	if isempty(condboot{index}), condboot{index} = 'complex'; end
end

for index = 1:length(varargin)
	if ~iscell(varargin) || length(varargin{index}) ~=2
		error('Except for the first arguments, all other arguments given to the function must be cell arrays of two numerical array');
	end
end

% accumulate coherence images (all arrays [nb_points x timesout x trials])
% ---------------------------
for index=1:length(varargin)
	tmpvar1 = varargin{index}{1};
	tmpvar2 = varargin{index}{2};
	if index == 1   % Shouldn't this be recalculated for arg2, etc.? TF 2007.06.04
		cond1trials = size(tmpvar1,ndims(tmpvar1));
		cond2trials = size(tmpvar2,ndims(tmpvar2));
		for tmpi = 1:length(formula)
			accres{tmpi} = zeros(size(tmpvar1,1), size(tmpvar1,2), naccu);
		end
	end

	if ndims(tmpvar1) == 2
		eval( [ 'arg' int2str(index) '=zeros(size(tmpvar1,1), cond1trials+cond2trials);' ] );
		eval( [ 'arg' int2str(index) '(:,1:cond1trials)=tmpvar1;' ] );
		eval( [ 'arg' int2str(index) '(:,cond1trials+1:end)=tmpvar2;' ] );
	else
		eval( [ 'arg' int2str(index) '=zeros(size(tmpvar1,1), size(tmpvar1,2), cond1trials+cond2trials);' ] );
		eval( [ 'arg' int2str(index) '(:,:,1:cond1trials)=tmpvar1;' ] );
		eval( [ 'arg' int2str(index) '(:,:,cond1trials+1:end)=tmpvar2;' ] );
	end
end

fprintf('Accumulating permutation statistics:');
alltrials = [1:cond1trials+cond2trials];

% processing formulas
% -------------------
formula1 = [];
formula2 = [];
for index = 1:length(formula)
	% updating formula
	% ----------------
	switch lower(condboot{ min(length(condboot), index) })
	 case 'abs', formula{index} = [ 'abs(' formula{index} ')' ];
	 %case 'angle', formula{index} = [ 'exp(j*angle(' formula{index} '))' ];
	 case 'angle', formula{index} = [ 'angle(' formula{index} ')/(2*pi)' ];
	 case 'complex', ;
	 otherwise, condboot, error('condstat argument must be either ''abs'', ''angle'', ''complex'' or empty');
	end

	% computing difference (non-shuffled)
	% -----------------------------------
	X = 1:cond1trials;
	eval( [ 'res1{index} = ' formula{index} ';'] );
	X = cond1trials+1:cond1trials+cond2trials;
	eval( [ 'res2{index} = ' formula{index}  ';'] );
	diffres{index} = res1{index}-res2{index};

	% build formula to execute
	% ------------------------
	arrayname = [ 'accres{' int2str(index) '}' ];
	if ndims(tmpvar1) == 2 % 2 dimensions
		formula1 = [ formula1 arrayname '(:,index) = ' formula{index} ';'];
		formula2 = [ formula2 arrayname '(:,index) = ' arrayname '(:,index) - ' formula{index}  ';'];
	else % 3 dimensions
		formula1 = [ formula1 arrayname '(:,:,index) = ' formula{index}  ';'];
		formula2 = [ formula2 arrayname '(:,:,index) = ' arrayname '(:,:,index) - ' formula{index}  ';'];
	end
end

% accumulating (shuffling)
% -----------------------
for index=1:naccu
	if rem(index,10) == 0,  fprintf(' %d',index); end
	if rem(index,120) == 0, fprintf('\n'); end

	alltrials = shuffle(alltrials);
	X = alltrials(1:cond1trials);
	eval( formula1 );
	X = alltrials(cond1trials+1:end);
	eval( formula2 );
end

% significance level
% ------------------
for index= 1:length(formula)
	accarray = accres{index};

    % size = nb_points*naccu
    % size = nb_points*naccu*times
	if ~isreal(accarray)    % might want to introduce a warning here: a complex
                            % result may not be desirable, and a single complex value
                            % in accarray could turn this from a 2-tail to 1-tail
                            % bootstrap test, leading to false positives. Hard to
                            % think of a meaningful warning though...
                            % TF 2007.06.04
		accarray = sqrt(accarray .* conj(accarray)); % faster than abs()
    end

    % compute bootstrap significance level
    i = round(naccu*alpha);
    switch ndims(accarray)
	 case 3
	     accarray = sort(accarray,3); % always sort on naccu (when 3D, naccu is the second dim)
         if strcmpi(bootside{min(length(bootside), index)}, 'upper')
			 accarray = accarray(:,:,floor(naccu-i/2+1));
	     else
			 accarray = accarray(:,:,[end:-1:1]);
			 accarraytmp(:,:,2) = accarray(:,:,ceil(i/2));
			 accarraytmp(:,:,1) = accarray(:,:,floor(naccu-i/2+1));
			 accarray = accarraytmp;
		 end

	 case 2
	     accarray = sort(accarray,2); % always sort on naccu (when 3D, naccu is the second dim)
         if strcmpi(bootside{min(length(bootside), index)}, 'upper')
			 accarray = accarray(:,floor(naccu-i/2+1));
	     else
			 accarraytmp(:,2) = accarray(:,ceil(i/2));
			 accarraytmp(:,1) = accarray(:,floor(naccu-i/2+1));
			 accarray = accarraytmp;
		 end
	 case 1
	     accarray = sort(accarray,1); % always sort on naccu (when 3D, naccu is the second dim)
         if strcmpi(bootside{min(length(bootside), index)}, 'upper')
			 accarray = accarray(floor(naccu-i/2+1));
	     else
			 accarraytmp(2) = accarray(ceil(i/2));
			 accarraytmp(1) = accarray(floor(naccu-i/2+1));
			 accarray = accarraytmp;
		 end
    end
    accres{index} = accarray;
end

if length(res1) == 1
	res1 = res1{1};
	res2 = res2{1};
	diffres = diffres{1};
	accres = accres{1};
end
fprintf('\n');
return;

% writing a function
% ------------------
% $$$ fid = fopen('tmpfunc.m', 'w');
% $$$ fprintf(fid, 'function [accres] = tmpfunc(alltrials, cond1trials, naccu,');
% $$$ for index=1:length(varargin)
% $$$ 	fprintf(fid, 'arg%d', index);
% $$$ 	if index ~=length(varargin), fprintf(fid,','); end
% $$$ end
% $$$ fprintf(fid, ')\n');
% $$$ commandstr = [ 'for index=1:naccu, ' ]
% $$$ % 			   'if rem(index,10) == 0,  disp(index); end;' ];
% $$$ % 			   'if rem(index,10) == 0,  fprintf('' %d'',index); end;' ...
% $$$ %	           'if rem(index,120) == 0, fprintf(''\n''); end;' ];
% $$$ commandstr = [ 	commandstr 'shuffle(alltrials);' ];
% $$$ commandstr = [ 	commandstr 'X = alltrials(1:cond1trials);' ];
% $$$ if ndims(tmpvar1) == 2 % 2 dimensions
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = ' formula ';'];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);'];
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = accres(:,index)-' formula ';end;'];
% $$$ else
% $$$ 	commandstr = [ 	commandstr 'res1 = ' formula ';' 10];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);' 10];
% $$$ 	commandstr = [ 	commandstr 'res2 = ' formula ';' 10];
% $$$ 	commandstr = [ 	commandstr 'accres(:,:,index) = res1 - res2; end;'];
% $$$ end;
% $$$ fprintf(fid, commandstr);
% $$$ fclose(fid);
% $$$ profile on;
% $$$ accres = tmpfunc(alltrials, cond1trials, naccu, arg1);
% $$$ profile report;
% $$$ profile off;
% $$$ return;

% evaluating a command
% --------------------
% $$$ commandstr = [ 'for index=1:naccu, ' ...
% $$$ 			   'if rem(index,10) == 0,  fprintf('' %d'',index); end;' ...
% $$$ 	           'if rem(index,120) == 0, fprintf(''\n''); end;' ];
% $$$ commandstr = [ 	commandstr 'shuffle(alltrials);' ];
% $$$ commandstr = [ 	commandstr 'X = alltrials(1:cond1trials);' ];
% $$$ if ndims(tmpvar1) == 2 % 2 dimensions
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = ' formula ';'];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);'];
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = accres(:,index)-' formula ';end;'];
% $$$ else
% $$$ 	commandstr = [ 	commandstr 'accres(:,:,index) = ' formula ';'];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);'];
% $$$ 	commandstr = [ 	commandstr 'accres(:,:,index) = accres(:,:,index)-' formula ';end;'];
% $$$ end;
% $$$ eval(commandstr);
% $$$ return;
% $$$


function render3DTraces(mtx,colors);
% render3DTraces(mtx,[colors]);
%
% Using plot3, render the columns in a 2D matrix mtx
% as a set of traces in a 3D graph (in current axes).
%
% colors: optional cell specifying the color to use for
% each column (string -- see help plot -- or 3-vector).
%
% 07/04 ras.
if ~exist('colors','var')
    colOrder = 'rbgcky';
    for i = 1:size(mtx,2)
        colors{i} = colOrder(mod(i,length(colOrder))+1);
    end
end

cla;
hold on;

X = 1:size(mtx,1);

for i = 1:size(mtx,2)
    Y = i*ones(1,size(mtx,1));
    htmp = plot3(X,Y,mtx(:,i)');
    set(htmp,'LineWidth',1.5,'Color',colors{i});
end

axis auto;
view(-8,60);
grid on;
hold off;

return


function [meanCoherehce,meanAmps,stdCos,semCos] = meanCos(view)
%
% [meanCos,meanAmps] = vectorMeans(view)
%
% Calculates mean correlations and amplitudes (ignoring phase)
% for all scans, for pixels that are in the currently selected
% ROI.
%
% djh 7/98

nscans = numScans(view);

% Get selpts from current ROI
ROIcoords = getCurROIcoords(view);

% Compute vector mean for each scan
meanCoherehce = zeros(1,nscans);
meanAmps = zeros(1,nscans);
stdCos=zeros(1,nscans);

for scanNum = 1:nscans
  [meanCoVM,meanAmp,stdCo,semCo] = meanCo(view,scanNum,ROIcoords);
  meanCoherehce(scanNum) = meanCoVM;
  meanAmps(scanNum) = meanAmp;
  stdCos(scanNum) = stdCo;
  semCos(scanNum) = semCo;

end


function g = getGroupSuperNonOverlap(I, param)

    N = size(I,1) * size(I, 2);

    g = sparse(zeros(N,1));
    g = diag(g);

    imlab = vl_xyz2lab(vl_rgb2xyz(I)) ;
    imlab = single(imlab);
    slicParam = param.superpixel.slicParam;

    groupCount = 0;

    segments = vl_slic(imlab, slicParam(1,1),  slicParam(1,2)) ;
    segments = segments(:);

    for j = 0:segments(N)
        g(:, groupCount+j+1) = segments==j;
    end
    groupCount  = groupCount+ segments(N)+1;

    % croping G
    g =g(:, 1:groupCount);

end


function disparity_in_pixels = disparity_in_pixels_cityscapes(input_disparity)

disparity_in_pixels = (double(input_disparity) - 1) / 256;

end


function [] = show_plot(gen, pop, varargin)
%   This function plots the front, stolen from KeLi and Zhanghu's code.
%   gen = generation count
%   pop = population at generation gen
%   optional parameters:
%       is_text = will show values for each point on the plot, boolean
%       xlist = list of variables to be plotted for the pareto set
%       flist = list of objectives to plot, when nobj > 3

    global nreal ;
    global nobj ;

    p = inputParser;
    addRequired(p, 'gen', @isnumeric);
    addRequired(p, 'pop', @ismatrix);
    addOptional(p, 'is_text', false, @islogical);
    addOptional(p, 'xlist', [], @(x) isvector(x) || isempty(x));
    addOptional(p, 'flist', [], @(x) isvector(x) || isempty(x));
    addOptional(p, 'xlim_', [], @(x) isvector(x) || isempty(x));
    addOptional(p, 'ylim_', [], @(x) isvector(x) || isempty(x));
    addOptional(p, 'zlim_', [], @(x) isvector(x) || isempty(x));
    parse(p, gen, pop, varargin{:});

    if(nobj == 2)
        f = p.Results.pop(:, nreal + [1 2]);
    elseif(isempty(p.Results.flist))
        f = p.Results.pop(:, nreal + [1 2 3]);
        flist = [1 2 3] ;
    else
        f = p.Results.pop(:, nreal + p.Results.flist);
        flist = p.Results.flist ;
    end

    if(~isempty(p.Results.xlist))
        xlist = p.Results.xlist ;
        x = p.Results.pop(:, nreal + xlist);
    end

    fstr = sprintf('objective space, gen = %d', p.Results.gen);
    xstr = sprintf('variable space, gen = %d', p.Results.gen);

    if(p.Results.gen == 1)
        if(~isempty(p.Results.xlist))
            cpos = get(gcf, 'Position');
            set(gcf,'Position',[cpos(1) cpos(2) 1000 400]);
        end
    end

    hold off;
    if(~isempty(p.Results.xlist))
        subplot(1,2,1);
    end

    % plot objectives
    if nobj == 2
        plot(f(:,1), f(:,2), 'ro', 'MarkerSize', 4);
        xlabel('f1', 'FontSize', 6);
        ylabel('f2', 'FontSize', 6);
        if(~isempty(p.Results.xlim_)); xlim(p.Results.xlim_); end;
        if(~isempty(p.Results.ylim_)); ylim(p.Results.ylim_); end;
        if(p.Results.is_text)
            strValues = strtrim(cellstr(num2str([f(:,1) f(:,2)],'(%.4f,%.4f)')));
            text(f(:,1), f(:,2), strValues, 'VerticalAlignment', 'bottom');
        end
    else
        plot3(f(:,1), f(:,2), f(:,3), ...
                            'ro', 'MarkerSize', 4);
        xlabel(sprintf('f%d', flist(1)), 'FontSize', 6);
        ylabel(sprintf('f%d', flist(2)), 'FontSize', 6);
        zlabel(sprintf('f%d', flist(3)), 'FontSize', 6);
        if(~isempty(p.Results.xlim_)); xlim(p.Results.xlim_); end;
        if(~isempty(p.Results.ylim_)); ylim(p.Results.ylim_); end;
        if(~isempty(p.Results.zlim_)); zlim(p.Results.zlim_); end;
        if(p.Results.is_text)
            strValues = strtrim(cellstr(num2str([f(:,1) f(:,2) f(:,3)],...
                                            '(%.4f,%.4f,%.4f)')));
            text(f(:,1), f(:,2), f(:,3), ...
                            strValues, 'VerticalAlignment', 'bottom');
        end
    end
    title(fstr, 'FontSize', 8);
    box on;
    drawnow;

    % plot variables
    if(~isempty(p.Results.xlist))
        subplot(1,2,2);
        if(length(xlist) == 2)
            plot(x(:,1), x(:,2), 'ro', 'MarkerSize', 4);
            xlabel('x1', 'FontSize', 6);
            ylabel('x2', 'FontSize', 6);
            if(p.Results.is_text)
                strValues = strtrim(cellstr(num2str([x(:,1) x(:,2)],...
                                            '(%.4f,%.4f)')));
                text(x(:,1), x(:,2), strValues, ...
                                'VerticalAlignment', 'bottom');
            end
        else
            plot3(x(:,1), x(:,2), x(:,3), ...
                            'ro', 'MarkerSize', 4);
            xlabel(sprintf('x%d', xlist(1)), 'FontSize', 6);
            ylabel(sprintf('x%d', xlist(2)), 'FontSize', 6);
            zlabel(sprintf('x%d', xlist(3)), 'FontSize', 6);
            if(p.Results.is_text)
                strValues = strtrim(cellstr(...
                        num2str([x(:,1) x(:,2) x(:,3)],...
                                '(%.4f,%.4f,%.4f)')));
                text(x(:,1), x(:,2), x(:,3), ...
                                strValues, 'VerticalAlignment', 'bottom');
            end
        end
        title(xstr, 'FontSize', 8);
        box on;
        drawnow;
    end


classdef CMatrixTest < matlab.unittest.TestCase
   properties (TestParameter)
      type = {@ddouble}
      lhsSize = {[0, 0], [1, 1], [30, 20], [30, 1], [1 20]}
      rhsSize = {[0, 0], [1, 1], [30, 20], [30, 1], [1 20]}
      lhsMode = struct('dense', 0, 'sparse', 1);
      rhsMode = struct('dense', 0, 'sparse', 1);
   end

   methods (Static)
      function [okay, A1, B1] = generateMatrices(lhsSize, rhsSize, lhsMode, rhsMode)
         Am = lhsSize(1); An = lhsSize(2); Bm = rhsSize(1); Bn = rhsSize(2);
         A1 = randn(Am, An); B1 = randn(Bm, Bn);
         okay = false;

         if lhsMode == 1, A1 = sparse(A1); end
         if rhsMode == 1, B1 = sparse(B1); end

         % check compatibility
         m = Am; n = An;
         if (Bm ~= 1), m = Bm; end
         if (Bn ~= 1), n = Bn; end
         if (Am ~= 1 && Am ~= m), return; end
         if (Bm ~= 1 && Bm ~= m), return; end
         if (An ~= 1 && An ~= n), return; end
         if (Bn ~= 1 && Bn ~= n), return; end

         okay = true;
      end
   end

   methods (Test)
      function comparisons(testCase, type, lhsSize, rhsSize, lhsMode, rhsMode)
         [okay, A1, B1] = CMatrixTest.generateMatrices(lhsSize, rhsSize, lhsMode, rhsMode);
         if (~okay), return; end

         % lt, gt, le, ge
         A2 = type(A1); B2 = type(B1);
         testCase.verifyEqual(A2 < B2, A1 < B1)
         testCase.verifyEqual(A2 > B2, A1 > B1)
         testCase.verifyEqual(A2 <= B2, A1 <= B1)
         testCase.verifyEqual(A2 >= B2, A1 >= B1)

         % ne, eq, and, or, not
         A1 = round(A1); B1 = round(B1);
         A2 = type(A1); B2 = type(B1);
         testCase.verifyEqual(A2 ~= B2, A1 ~= B1)
         testCase.verifyEqual(A2 == B2, A1 == B1)
         testCase.verifyEqual(A2 & B2, A1 & B1)
         testCase.verifyEqual(A2 | B2, A1 | B1)
         testCase.verifyEqual(~A2, ~A1)
      end

      function arithmetic(testCase, type, lhsSize, rhsSize, lhsMode, rhsMode)
         [okay, A1, B1] = CMatrixTest.generateMatrices(lhsSize, rhsSize, lhsMode, rhsMode);
         Ceps = double(eps(type(1))) + eps;
         if (~okay), return; end

         % plus, minus, times, rdivide, ldivide
         A2 = type(A1); B2 = type(B1);
         testCase.verifyEqual(double(A2 + B2), A1 + B1, 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2 - B2), A1 - B1, 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2 .* B2), A1 .* B1, 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2 ./ B2), A1 ./ B1, 'AbsTol', Ceps*1e4, 'RelTol', Ceps*1e4)
         testCase.verifyEqual(double(A2 .\ B2), A1 .\ B1, 'AbsTol', Ceps*1e4, 'RelTol', Ceps*1e4)

         % max, min
         testCase.verifyEqual(double(max(A2,B2)), max(A1,B1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(min(A2,B2)), min(A1,B1), 'AbsTol', Ceps*1e4)

         % horzcat, vertcat
         if size(A2,1) == size(B2,1)
            testCase.verifyEqual(double([A2 B2]), [A1 B1], 'AbsTol', Ceps*1e4)
            testCase.verifyEqual(double([A2 B2 A2 B2]), [A1 B1 A1 B1], 'AbsTol', Ceps*1e4)
         end

         if size(A2,2) == size(B2,2)
            testCase.verifyEqual(double([A2;B2]), [A1;B1], 'AbsTol', Ceps*1e4)
            testCase.verifyEqual(double([A2;B2;A2;B2]), [A1;B1;A1;B1], 'AbsTol', Ceps*1e4)
         end

         % times, rdivide, ldivide
         if size(A1,2) == size(B2',1)
            testCase.verifyEqual(double(A2 * B2'), A1 * B1', 'AbsTol', Ceps*1e6)
         end

         if size(A1,1) == size(B2,1)
            testCase.verifyEqual(double(A2 \ B2), A1 \ B1, 'AbsTol', Ceps*1e6)
         end

         if size(A1,2) == size(B2,2)
            testCase.verifyEqual(double(A2 / B2), A1 / B1, 'AbsTol', Ceps*1e6)
         end
      end

      function unaryOperations(testCase, type, lhsSize, lhsMode)
         A1 = randn(lhsSize);
         if lhsMode == 1, A1 = sparse(A1); end
         Ceps = double(eps(type(1))) + eps;

         % size, length, numel
         A2 = type(A1);
         testCase.verifyEqual(size(A2), size(A1))
         testCase.verifyEqual(size(A2, 1), size(A1, 1))
         testCase.verifyEqual(size(A2, 2), size(A1, 2))
         [a1, b1] = size(A1);
         [a2, b2] = size(A2);
         testCase.verifyEqual(a2, a1)
         testCase.verifyEqual(b2, b1)
         testCase.verifyEqual(length(A2), length(A1))
         testCase.verifyEqual(numel(A2), numel(A1))

         % isscalar, isvector, ismatrix, isempty, isrow, iscolumn, issymmetric
         testCase.verifyEqual(isscalar(A2), isscalar(A1))
         testCase.verifyEqual(isvector(A2), isvector(A1))
         testCase.verifyEqual(ismatrix(A2), ismatrix(A1))
         testCase.verifyEqual(isempty(A2), isempty(A1))
         testCase.verifyEqual(isrow(A2), isrow(A1))
         testCase.verifyEqual(iscolumn(A2), iscolumn(A1))
         testCase.verifyEqual(issymmetric(A2), issymmetric(A1))

         if (size(A1,1) == size(A1,2))
            A1 = A1 + A1'; A2 = type(A1);
            testCase.verifyEqual(issymmetric(A2), issymmetric(A1))
         end

         % uminus, uplus, abs, transpose, ctranspose, sqrt, all, any, nnz, diag
         testCase.verifyEqual(double(-A2), -A1, 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(+A2), +A1, 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(abs(A2)), abs(A1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(transpose(A2)), transpose(A1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(ctranspose(A2)), ctranspose(A1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(sqrt(abs(A2)+1)), sqrt(abs(A1)+1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(all(A2), all(A1))
         testCase.verifyEqual(all(A2,1), all(A1,1))
         testCase.verifyEqual(all(A2,2), all(A1,2))
         testCase.verifyEqual(any(A2), any(A1))
         testCase.verifyEqual(any(A2,1), any(A1,1))
         testCase.verifyEqual(any(A2,2), any(A1,2))
         testCase.verifyEqual(nnz(A2), nnz(A1))
         testCase.verifyEqual(double(diag(A2)), diag(A1), 'AbsTol', Ceps*1e4)

         % sum, prod, max, min, norm
         testCase.verifyEqual(double(sum(A2)), sum(A1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(sum(A2,1)), sum(A1,1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(sum(A2,2)), sum(A1,2), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(sum(A2,'all')), sum(A1,'all'), 'AbsTol', Ceps*1e4)

         testCase.verifyEqual(double(prod(A2)), prod(A1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(prod(A2,1)), prod(A1,1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(prod(A2,2)), prod(A1,2), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(prod(A2,'all')), prod(A1,'all'), 'AbsTol', Ceps*1e4)

         testCase.verifyEqual(double(max(A2)), max(A1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(max(A2,[],1)), max(A1,[],1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(max(A2,[],2)), max(A1,[],2), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(max(A2,[],'all')), max(A1,[],'all'), 'AbsTol', Ceps*1e4)

         testCase.verifyEqual(double(min(A2)), min(A1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(min(A2,[],1)), min(A1,[],1), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(min(A2,[],2)), min(A1,[],2), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(min(A2,[],'all')), min(A1,[],'all'), 'AbsTol', Ceps*1e4)

         if isvector(A1)
            testCase.verifyEqual(double(norm(A2)), norm(A1), 'AbsTol', Ceps*1e4)
         end

         % find
         testCase.verifyEqual(find(A2), find(A1))
         [i2,j2] = find(A2); [i1,j1] = find(A1);
         testCase.verifyEqual(i2, i1)
         testCase.verifyEqual(j2, j1)
         [i2,j2,v2] = find(A2); [i1,j1,v1] = find(A1);
         testCase.verifyEqual(i2, i1)
         testCase.verifyEqual(j2, j1)
         testCase.verifyEqual(double(v2), v1, 'AbsTol', Ceps*1e4)

         % chol
         H1 = A1 * A1' + speye(size(A1,1));
         testCase.verifyEqual(double(chol(type(H1))), chol(H1), 'AbsTol', Ceps*1e6)
      end

      function externalFunc(testCase, type, lhsSize)
         Am = lhsSize(1); An = lhsSize(2);
         Ceps = double(eps(type(1))) + eps;
         typename = class(type(1.0));

         % ones, zeros, eye, rand, randn, randi, sparse
         A1 = ones(Am, An, typename); A2 = ones(Am, An);
         testCase.verifyTrue(all(A1 == A2, 'all'))
         A1 = zeros(Am, An, typename); A2 = zeros(Am, An);
         testCase.verifyTrue(all(A1 == A2, 'all'))
         A1 = eye(Am, An, typename); A2 = eye(Am, An);
         testCase.verifyTrue(all(A1 == A2, 'all'))
         rng(1); A1 = rand(Am, An, typename); rng(1); A2 = rand(Am, An);
         testCase.verifyTrue(all(A1 == A2, 'all'))
         rng(1); A1 = randn(Am, An, typename); rng(1); A2 = randn(Am, An);
         testCase.verifyTrue(all(A1 == A2, 'all'))
         rng(1); A1 = randi(10, Am, An, typename); rng(1); A2 = randi(10, Am, An);
         testCase.verifyTrue(all(A1 == A2, 'all'))
         A1 = full(sprand(Am, An, 0.5)); A2 = type(A1);
         testCase.verifyEqual(double(sparse(A2)), sparse(A1), 'AbsTol', 1e4*Ceps);
         testCase.verifyEqual(double(full(A2)), full(A1), 'AbsTol', 1e4*Ceps);
         A1 = (sprand(Am, An, 0.5)); A2 = type(A1);
         testCase.verifyEqual(double(sparse(A2)), sparse(A1), 'AbsTol', 1e4*Ceps);
         testCase.verifyEqual(double(full(A2)), full(A1), 'AbsTol', 1e4*Ceps);
         Carr = type(1:Am);
         testCase.verifyEqual(double(sparse(1:Am, 1:Am, Carr)), sparse(1:Am, 1:Am, 1:Am));
         testCase.verifyEqual(double(sparse(1:Am, 1:Am, Carr, Am, Am)), sparse(1:Am, 1:Am, 1:Am, Am, Am));
         testCase.verifyEqual(double(sparse(1:Am, 1:Am, Carr, Am, Am, Am)), sparse(1:Am, 1:Am, 1:Am, Am, Am, Am));

         % .x
         b = A2.x;
         A2.x = b;
      end

      function otherTests(testCase, type, lhsMode)
         A1 = sprandn(5, 6, 0.6);
         if lhsMode == 0, A1 = full(A1); end
         A2 = type(A1);
         Ceps = double(eps(type(1))) + eps;

         % subsref
         testCase.verifyEqual(double(A2(1:3,:)), A1(1:3,:), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2(:,2:4)), A1(:,2:4), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2(1:3,2:4)), A1(1:3,2:4), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2(1:3,2:end)), A1(1:3,2:end), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2(5:end)), A1(5:end), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2(:)), A1(:), 'AbsTol', Ceps*1e4)
         testCase.verifyEqual(double(A2(2:2:8)), A1(2:2:8), 'AbsTol', Ceps*1e4)

         % subsasgn
         A1(1:3,:) = 5; A2(1:3,:) = 5;
         testCase.verifyEqual(double(A2),A1, 'AbsTol', Ceps*1e4)
         A1(5:end) = 5; A2(5:end) = 5;
         testCase.verifyEqual(double(A2),A1, 'AbsTol', Ceps*1e4)
         A1(:,2:4) = []; A2(:,2:4) = [];
         testCase.verifyEqual(double(A2),A1, 'AbsTol', Ceps*1e4)

         disp(A2);

         % dissect, amd, symbfact
         A1 = sprandn(6, 6, 0.6);
         A1 = A1 * A1';
         A2 = type(A1);
         testCase.verifyEqual(dissect(A2),dissect(A1))
         testCase.verifyEqual(amd(A2),amd(A1))
         testCase.verifyEqual(symbfact(A2),symbfact(A1))
         [r21, r22] = etree(A2);
         [r11, r12] = etree(A1);
         testCase.verifyEqual(r21,r11)
         testCase.verifyEqual(r22,r12)

         % chol
         try
            A = randn(5,5);
            A = A * A';
            A = sparse(A) * NaN;
            Z = chol(type(A));
            Z'\randn(5,1);
         end

         A = sprandn(30,30,0.3);
         H = type(A * A');
         R = chol(H);
         D = H - R' * R;
         testCase.verifyLessThan(double(norm(D(:))), Ceps*1e4)

         % solves for triangular matrix
         A = type(tril(sprandn(30,30,0.3) + speye(30)));
         b = randn(30,1);
         x = A\b;
         testCase.verifyLessThan(double(norm(A*x-b)), Ceps*1e4)

         A = type(triu(sprandn(30,30,0.3) + speye(30)));
         b = randn(30,1);
         x = A\b;
         testCase.verifyLessThan(double(norm(A*x-b)), Ceps*1e4)

         A = type(tril(randn(30,30) + eye(30)));
         b = randn(30,1);
         x = A\b;
         testCase.verifyLessThan(double(norm(A*x-b)), Ceps*1e4)

         A = type(triu(randn(30,30) + eye(30)));
         b = randn(30,1);
         x = A\b;
         testCase.verifyLessThan(double(norm(A*x-b)), Ceps*1e4)
      end

      function cholTests(testCase)
         load('..\..\Problem\LPnetlib\lp_80bau3b.mat')

         A = Problem.A;
         p = colamd(A');
         A = A(p,:);
         w = rand(size(A,2),1);
         R = chol((A*(w.*A')));
         x = rand(size(A,1),1);
         z = R\(R'\x);
         d = full(diag(R));

         o = AdaptiveChol(A);
         o.factorize(diag(sparse(w)));
         z2 = o.solve(x);
         testCase.verifyEqual(double(z), z2, 'AbsTol', eps*1e4)

         d2 = o.diagonal();
         testCase.verifyEqual(double(d), d2, 'AbsTol', eps*1e4)

         ls = o.leverageScore(100);
         testCase.verifyTrue(all(ls<1.5));

         try
            o.factorize(diag(sparse(rand(12,1))));
         end
      end
   end
end


function eventLog = ma_executeCoast_goto_tru(truTarget, initialState, eventNum, considerSoITransitions, soiSkipIds, refBody, massLoss, orbitDecay, celBodyData)
%ma_executeCoast_goto_tru Summary of this function goes here
%   Detailed explanation goes here
    bodyID = initialState(8);

    bodyInfo = getBodyInfoByNumber(bodyID, celBodyData);
    gmu = bodyInfo.gm;
    rVect = initialState(2:4)';
    vVect = initialState(5:7)';

    [sma, ecc, ~, ~, ~, truINI] = getKeplerFromState(rVect,vVect,gmu);
    meanMotion = computeMeanMotion(sma, gmu);

    if(isempty(refBody))
        refBody.id = -1;
    end

    if((abs(truTarget-truINI) < 1E-7 || abs(abs(truTarget-truINI)-2*pi) < 1E-7) && refBody.id==bodyInfo.id)
        eventLog = initialState;
        eventLog(:,13) = eventNum;
        return;
    end

    if(ecc < 1.0)
        if(abs(truTarget-2*pi) < 1E-7)
            truTarget = 2*pi;
        else
            truTarget = AngleZero2Pi(truTarget);
        end

        if(abs(truINI-2*pi) < 1E-7)
            if(truINI - 2*pi >= 0 || truINI==2*pi)
                truINI = 0;
            else
                truINI = 0;
            end
        else
            truINI = AngleZero2Pi(truINI);
        end
    else
        truINI = angleNegPiToPi(truINI);
        truTarget = angleNegPiToPi(truTarget);
    end

    if(abs(AngleZero2Pi(truTarget)-AngleZero2Pi(truINI)) <1E-6)
        truTarget = truINI;
    end

    if(ecc >= 1.0)
        parentBodyInfo = bodyInfo.getParBodyInfo(celBodyData);
        if(~isempty(parentBodyInfo))
            rSOI = getSOIRadius(bodyInfo, parentBodyInfo);
        else
            rSOI = Inf;
        end
        iniOrbit = [sma, ecc];
        iniHyTruMax = AngleZero2Pi(computeTrueAFromRadiusEcc(rSOI, iniOrbit(1), iniOrbit(2)));
        meanMotion = computeMeanMotion(iniOrbit(1), bodyInfo.gm);
        lbTA = -iniHyTruMax;
        ubTA = iniHyTruMax;

        bool1 = (ecc >= 1.0 && (truTarget)<(truINI));
        bool2 = (ecc >= 1.0 && truTarget>ubTA);
        bool3 = (ecc >= 1.0 && truTarget<lbTA);

        if(bool1 || bool2 || bool3)
            soITrans = findSoITransitions(initialState, Inf, soiSkipIds, massLoss, orbitDecay, celBodyData);
            if(~isempty(soITrans))
                SoITransEventLog = ma_executeCoast_goto_soi_trans(initialState, eventNum, Inf, soiSkipIds, massLoss, orbitDecay, celBodyData, soITrans);
                goToUTEventLog = ma_executeCoast_goto_tru(truTarget, SoITransEventLog(end,:), eventNum, true, soiSkipIds, refBody, massLoss, orbitDecay, celBodyData);
                eventLog = [SoITransEventLog; goToUTEventLog];
                return;
            else
                warnStr = ['Cannot go to target true anomaly (', num2str(rad2deg(truTarget)), ' deg).  Could not find an SoI transition, skipping.'];
                addToExecutionWarnings(warnStr, eventNum, bodyID, celBodyData);
                eventLog = initialState;
                eventLog(:,13) = eventNum;
                return;
            end
        end
    end

    if(considerSoITransitions)
        if(refBody.id == bodyID || refBody.id == -1)
            dt = getDtToTru(truTarget, sma, ecc, truINI, gmu);
            utTru = initialState(1) + dt;
        else
            dt = NaN;
            utTru = Inf;
        end

        if(not(isfinite(utTru)) || dt > 0)
            soITrans = findSoITransitions(initialState, utTru, soiSkipIds, massLoss, orbitDecay, celBodyData);
            SoITransEventLog = [];
            if(~isempty(soITrans) && min(soITrans(:,2)) < utTru)
                SoITransEventLog = ma_executeCoast_goto_soi_trans(initialState, eventNum, utTru, soiSkipIds, massLoss, orbitDecay, celBodyData, soITrans);
                goToUTEventLog = ma_executeCoast_goto_tru(truTarget, SoITransEventLog(end,:), eventNum, true, soiSkipIds, refBody, massLoss, orbitDecay, celBodyData);
            else
                goToUTEventLog = ma_executeCoast_goto_tru(truTarget, initialState, eventNum, false, soiSkipIds, refBody, massLoss, orbitDecay, celBodyData);
            end
            eventLog = [SoITransEventLog; goToUTEventLog];
        else
            eventLog = initialState;
        end

        return;
    end

    if(truTarget >= truINI)
        meanINI = computeMeanFromTrueAnom(truINI, ecc);
        if(abs(truTarget) < 1E-8)
            meanTarget = 0;
        elseif(abs(truTarget-2*pi) < 1E-8)
            meanTarget = 2*pi;
            if(meanINI < 0)
                meanINI = meanINI + 2*pi;
            end
        else
            meanTarget = computeMeanFromTrueAnom(truTarget, ecc);
        end

        dM = meanTarget-meanINI;
        dt = dM/meanMotion;

        eventLog = ma_executeCoast_goto_dt(dt, initialState, eventNum, considerSoITransitions, soiSkipIds, massLoss, orbitDecay, celBodyData);
    else
        eventLog1 = ma_executeCoast_goto_tru(2*pi, initialState, eventNum, considerSoITransitions, soiSkipIds, refBody, massLoss, orbitDecay, celBodyData);

        bodyID = eventLog1(end,8);
        bodyInfo = getBodyInfoByNumber(bodyID, celBodyData);
        gmu = bodyInfo.gm;
        [sma, ecc, inc, raan, arg, ~] = getKeplerFromState(eventLog1(end,2:4),eventLog1(end,5:7),gmu);

        if(truTarget>=1E-6)
            newTruINI = 1E-6;
            [rVect,vVect]=getStatefromKepler(sma, ecc, inc, raan, arg, newTruINI, gmu); %set true anomaly to 0 to avoid having issues w/ infinite recursion
            eventLog1(end,2:4) = reshape(rVect,1,3);
            eventLog1(end,5:7) = reshape(vVect,1,3);

            eventLog2 = ma_executeCoast_goto_tru(truTarget, eventLog1(end,:), eventNum, considerSoITransitions, soiSkipIds, refBody, massLoss, orbitDecay, celBodyData);
        else
            eventLog2 = [];
        end

        eventLog = [eventLog1; eventLog2];
    end
end


function g = ndsimKernGradient(kern, t1, varargin)

% NDSIMKERNGRADIENT Gradient of SIM kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% single input motif
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG t : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
%
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG t1 : the input locations associated with the rows of the
% kernel matrix.
% ARG t2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% That is, this function computes sum(sum(deriv(K,a).*partial)) for
% each scalar parameter a, where deriv(K,a) is the matrix-valued
% derivative of the kernel K (computed between times t and t2) with
% respect to the parameter a.
%
% SEEALSO simKernParamInit, kernGradient, simKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Jaakko Peltonen, 2011

% KERN

if length(varargin)<2
  t2 = t1;
  covGrad = varargin{1};
else
  t2 = varargin{1};
  covGrad = varargin{2};
end

sigma = sqrt(2/kern.inverseWidth);
if isfield(kern, 'isNegativeS') && (kern.isNegativeS == true)
    variancemultiplier = (kern.sensitivity*kern.sensitivity);
else
    variancemultiplier = kern.variance;
end

dim1 = size(t1, 1);
dim2 = size(t2, 1);
t1Mat = t1(:, ones(1, dim2));
t2Mat = t2(:, ones(1, dim1))';
diffT = (t1Mat - t2Mat);

k1a = sqrt(pi)/2*( t1Mat.*erf(t1Mat/sigma) + t2Mat.*erf(t2Mat/sigma) - diffT.*erf(diffT/sigma) );
k1b = sigma/2*( exp(-(t1Mat/sigma).^2) + exp(-(t2Mat/sigma).^2) - exp(-(diffT/sigma).^2) - 1 );
K = (k1a+k1b)*sigma*variancemultiplier;

% k = ndsimKernCompute(kern, t1, t2);
% deriv_variancemultiplier = sum(sum((k/variancemultiplier).*covGrad));
% deriv_sigma = k/sigma + variancemultiplier*sigma/2*(exp(-(t1Mat/sigma).^2)+exp(-(t2Mat/sigma).^2)-exp(-(diffT/sigma).^2)-1);
% deriv_sigma = sum(sum(deriv_sigma.*covGrad));
% % sigma=sqrt(2/inversewidth)
% % --> df/dinversewidth = (df/dsigma)*(dsigma/dinversewidth)
% % = (df/dsigma)*((-1/2)*sqrt(2)*(inversewidth^(-3/2)))
% % = (df/dsigma)*(-1/2*sigma/inversewidth)
% deriv_inversewidth=deriv_sigma*(-0.5*sigma/kern.inverseWidth);

deriv_variancemultiplier = sum(sum(sigma*(k1a+k1b).*covGrad));
deriv_sigma = (k1a+2*k1b)*variancemultiplier;
% deriv_sigma = (k1a+k1b)*variancemultiplier ...
%     + variancemultiplier*sigma/2*(exp(-(t1Mat/sigma).^2)+exp(-(t2Mat/sigma).^2)-exp(-(diffT/sigma).^2)-1) ...
%     + variancemultiplier/sigma*(-(t1Mat.^2).*exp(-(t1Mat/sigma).^2)-(t2Mat.^2).*exp(-(t2Mat/sigma).^2)+(diffT.^2).*exp(-(diffT/sigma).^2)) ...
%    + variancemultiplier/sigma*(-(t1Mat.^2).*exp(-(t1Mat/sigma).^2)-(t2Mat.^2).*exp(-(t2Mat/sigma).^2)+(diffT.^2).*exp(-(diffT/sigma).^2)) ;

deriv_sigma = sum(sum(deriv_sigma.*covGrad));
deriv_inversewidth=deriv_sigma*(-0.5*sigma/kern.inverseWidth);

if isfield(kern, 'isNegativeS') && (kern.isNegativeS == true)
  % variancemultiplier=sensitivity^2
  % --> df/dsensitivity = (df/dvarmult)*(dvarmult/dsensitivity)
  % = (df/dvarmult)*(2*sensitivity)
  deriv_sensitivity=deriv_variancemultiplier*2*kern.sensitivity;
  g = [deriv_inversewidth deriv_sensitivity];
else
  g = [deriv_inversewidth deriv_variancemultiplier];
end

% gaussianinitial currently unsupported


% absolute change in area
function [data,units] = compute_absdarea(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);
  data{i} = abs(trx(fly).darea);
end
units = parseunits('mm^2/s');


function Z = xor(X,Y)
%XOR Logical EXCLUSIVE OR for tensors.
%
%   See also TENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

Z = tenfun(@xor,X,Y);


%DISPERROR Display error matrix with information on classifiers and datasets
%
%	DISPERROR(DATA,CLASSF,ERROR,STD,FID)
%
% INPUT
%   DATA     Cell array of M datasets or dataset names (strings)
%   CLASSF   Cell array of N mappings or mapping names (strings)
%   ERROR    M*N matrix of (average) error estimates
%   STD      M*N matrix of standard devations on ERROR (optional)
%   FID      File in which results are written (default: 1)
% OUTPUT
%
% DESCRIPTION
% Displays the matrix ERROR matrix with error estimates for N
% classifiers related to M datasets. This routine is called by TESTC
% and CROSVALL to display results.
%
% EXAMPLE
% testsets  = {gendath gendatb gendatd(100,5)}
% trainsets = {gendath gendatb gendatd(100,5)}
% classifiers = {nmc fisherc qdc svc}
% testc(testsets,prmap(trainsets,classifiers))
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, TESTC, PRCROSSVAL

% $Id: disperror.m,v 1.3 2007/06/05 12:43:35 duin Exp $

function disperror (data,classf,err,stdev,fid)

	if nargin < 5, fid = 1; end
	% Check arguments.
	if (nargin > 3) & (any(size(err) ~= size(stdev)))
		error('size of matrix with standard deviations should match matrix with errors')
	end
	if (~iscell(classf)) | (~isstr(classf{1}) & ~ismapping(classf{1}))
		error('cell array of mappings or mapping names expected')
	end
	if (~iscell(data)) | (~isstr(data{1}) & ~isdataset(data{1}))
		error('cell array of datasets or datasets names expected')
	end

	[m,n] = size(err);
	if (length(data) ~= m)
		error('size of dataset cell array should equal number of rows in error matrix');
	end

	if (length(classf) ~= n)
		error('size of classifier cell array should equal number of columns in error matrix');
	end

	% If datasets are supplied, extract their names.
	for j = 1:m
		if (isdataset(data{j}))
			data{j} = getname(data{j});
		end
	end

	% If classifiers are supplied, extract their names.
	for j = 1:n
		if (ismapping(classf{j}))
			classf{j} = getname(classf{j});
		end
  end

  if n >=  m

    if m == 1
      fprintf(fid, ' %s \n\n',data{1});
    else
      fprintf(fid,'\n');
      for j = 1:m
        fprintf(fid,'\n  data_%i : %20s',j,data{j});
      end
      fprintf(fid,'\n\n                      ');
      for j = 1:m
        fprintf(fid,'  data_%i',j);
      end
      fprintf(fid,'\n\n');
    end

    for i = 1:n
      fprintf(fid,'  %-22s',classf{i});
      fprintf(fid,'  %5.3f',err(:,i)');
      if (nargin > 3)
        fprintf(fid,' (%5.3f)',stdev(:,i)');
        fprintf(fid,'\n');
      end
      fprintf(fid,'\n');
    end

  else

    if (n == 1)
      fprintf(fid,' %s \n\n',classf{1});
    else
      fprintf(fid,'\n');
      for i = 1:n
        fprintf(fid,'\n  clsf_%i : %s',i,classf{i});
      end
      fprintf(fid,'\n\n                      ');
      for i = 1:n
        fprintf(fid,'  clsf_%i',i);
      end
      fprintf(fid,'\n\n');
    end

    for j = 1:m
      fprintf(fid,'  %s',data{j});
      fprintf(fid,' %7.3f',err(j,:));
      if (nargin > 3)
        fprintf(fid,'\n                      ');
        fprintf(fid,' %7.3f',stdev(j,:));
        fprintf(fid,'\n');
      end
      fprintf(fid,'\n');
    end

  end
  fprintf(fid,'\n');

return


function b = yalmipbandwidth(S)

if isa(S,'sdpvar')
    S = spy(S);
end
[i,j] = find(triu((S)));
b = max(abs(i-j));


function Z = xor(X,Y)
%XOR Logical EXCLUSIVE OR for tensors.
%
%   See also TENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

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

Z = tenfun(@xor,X,Y);


%  This function takes a problem in SeDuMi MATLAB format and writes it out
%  in SDPpack format.
%
%  Usage:
%
%  writesdp(fname,A,b,c,K)
%
%      fname           Name of SDPpack file, in quotes
%      A,b,c,K         Problem in SeDuMi form
%
%  Notes:
%
%     Problems with complex data are not allowed.
%
%     Rotated cone constraints are not supported.
%
%     Nonsymmetric A.s and C.s matrices are symmetrized with A=(A+A')/2
%     a warning is given when this happens.
%
%     Floating point numbers are written out with 18 decimal digits for
%     accuracy.
%
%  Please contact the author (Brian Borchers, borchers@nmt.edu) with any
%  questions or bug reports.
%
function writesdp(fname,A,b,c,K)

%From: 	Brian Borchers[SMTP:borchers@nmt.edu]
%Sent: 	Wednesday, December 08, 1999 12:33 PM
%To: 	J.Sturm@KE.UNIMAAS.NL
%
%Here's a MATLAB routine that will take a problem in SeDuMi's MATLAB format
%and write it out in SDPpack format.

%
%  First, check for complex numbers in A, b, or c.
%
if (isreal(A) ~= 1),
  disp('A is not real!');
  return;
end;
if (isreal(b) ~= 1),
  disp('b is not real!');
  return;
end;
if (isreal(c) ~= 1),
  disp('c is not real!');
  return;
end;
%
%  Check for any rotated cone constraints.
%
if (isfield(K,'r') && (~isempty(K.r)) && (K.r ~= 0)),
  disp('rotated cone constraints are not yet supported.');
  return;
end;
%
%  Get the size data.
%
if (isfield(K,'l')),
  nlin=K.l;
  sizelin=nlin;
  if (isempty(sizelin)),
    sizelin=0;
    nlin=0;
  end;
else
  nlin=0;
  sizelin=0;
end;

if (isfield(K,'s')),
  nsdpblocks=length(K.s);
  %sizesdp=sum((K.s).^2);
  %if (isempty(sizesdp)),
    %sizesdp=0;
    %nsdpblocks=0;
  %end;
else
  %sizesdp=0;
  nsdpblocks=0;
end;

if (isfield(K,'q')),
  nqblocks=length(K.q);
  sizeq=sum(K.q);
  if (isempty(sizeq)),
    sizeq=0;
    nqblocks=0;
  end;
else
  nqblocks=0;
  sizeq=0;
end;

m=length(b);

%
%  Open up the file for writing.
%
fid=fopen(fname,'w');
%
%  Print out m, the number of constraints.
%
fprintf(fid,'%d \n',m);
%
%  Next, b, with one entry per line.
%
fprintf(fid,'%.18e\n',full(b));
%
%  Next, the semidefinite part.
%
if (nsdpblocks == 0),
  fprintf(fid,'0\n');
else
%
%  Print out the number of semidefinite blocks.
%
  fprintf(fid,'%d\n',nsdpblocks);
%
%  For each block, print out its size.
%
  fprintf(fid,'%d\n',full(K.s));
%
%  Next, the cost matrix C.s.
%
%
%  First, calculate where in c things start.
%
  base=sizelin+sizeq+1;
%
%  Next, work through the blocks.
%
  for i=1:nsdpblocks,
    fprintf(fid,'1\n');
    work=c(base:base+K.s(i)^2-1);
    if nnz(work ~= work'),
    if (work ~= work'),
      disp('Non symmetric C.s matrix!');
      work=(work+work')/2;
    end;
    work=triu(work);
    [II,JJ,V]=find(work);
    cnt=length(II);
    fprintf(fid,'%d\n',cnt);
    if (cnt ~= 0),
      fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
    end;
%
%  Next, update to the next base.
%
    base=base+K.s(i)^2;
  end;
%
%  Now, loop through the constraints, one at a time.
%
  for cn=1:m,
%
%  Print out the SDP part of constraint cn.
%
    base=sizelin+sizeq+1;
    for i=1:nsdpblocks,
      fprintf(fid,'1\n');
      if nnz(work ~= work'),
      work=reshape(work,K.s(i),K.s(i));
      if (work ~= work'),
        disp('Non symmetric A.s matrix!');
        work=(work+work')/2;
      end;
      work=triu(work);

      [II,JJ,V]=find(work);
      cnt=length(II);
      fprintf(fid,'%d\n',cnt);
      if (cnt ~= 0),
        fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
      end;
%
%  Next, update to the next base.
%
      base=base+K.s(i)^2;
    end;
%
% Done with constraint cn
%
  end;
%
% Done with SDP part.
%
end;
%
% Next, handle the Quadratic part.
%
%
% Describe the Q blocks.
%
if (nqblocks == 0),
  fprintf(fid,'0\n');
else
  fprintf(fid,'%d\n',nqblocks);
  fprintf(fid,'%d\n',full(K.q));
%
%  Find C.q.
%
  base=sizelin+1;
  cq=c(base:base+sizeq-1);
%
% Print out the C.q coefficients.
%
  fprintf(fid,'%.18e\n',full(cq));
%
%  Next, the constraint matrix A.q.
%
  Aq=A(:,base:base+sizeq-1);
%
%  Print out the count of nonzeros.
%
  [II,JJ,V]=find(Aq);
  cnt=length(II);
  fprintf(fid,'1\n');
  fprintf(fid,'%d\n',cnt);
  if (cnt ~= 0),
    fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
  end;
%
% End of handling quadratic part.
%
end;
%
%
% Finally, handle the linear part.
%
if (nlin == 0),
  fprintf(fid,'0\n');
else
%
% Print out the number of linear variables.
%
  fprintf(fid,'%d\n',nlin);
%
% Print out C.l
%
  fprintf(fid,'%.18e\n',full(c(1:nlin)));
%
%  Print out the A matrix.
%
  Al=A(:,1:nlin);
  [II,JJ,V]=find(Al);
  cnt=length(II);
  fprintf(fid,'1\n');
  fprintf(fid,'%d\n',cnt);
  if (cnt ~= 0),
    fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
  end;
end;


function varargout = functionPoints(disc)
%FUNCTIONPOINTS   Points at which functions are discretized.

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

pointsFun = @(n) trigtech.trigpts(n);
[varargout{1:nargout}] = valsDiscretization.points(disc, pointsFun);

end


function value = i4_max ( a, b )

%*****************************************************************************80
%
%% I4_MAX returns the maximum of two I4's.
%
%  Discussion:
%
%    An I4 is an integer value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, B, values to compare.
%
%    Output, real VALUE, the maximum of A and B.
%
  a = floor ( a );
  b = floor ( b );

  if ( a < b )
    value = b;
  else
    value = a;
  end

  return
end


%   This subroutine selcts the Ni closest earthquakes
%   around a userdefined point
%

report_this_filefun(mfilename('fullpath'));

new = a;
ni  = input('Please input  number of events ni:')
%ni = 100
axes(h6)
[xa0,ya0]  = ginput(1);

l = sqrt(((a.Longitude-xa0)*cos(pi/180*ya0)*111).^2 + ((a.Latitude-ya0)*111).^2) ;
[s,is] = sort(l);
new = a(is(:,1),:) ;
plos1 = plot(new(1:ni,1),new(1:ni,2),'xw','EraseMode','back');
%plos1 = plot(new(1:ni,1),new(1:ni,2),'xw')

figure_w_normalized_uicontrolunits(2)
clf
h3 = gcf;
newt = new(1:ni,:);
[st,ist] = sort(newt);
newt2 = newt(ist(:,3),:);

newt2(:,9) = newt2.Date + newt2.Date.Month/12 + newt2.Date.Day/365;
[st,ist] = sort(newt2);
newt3 = newt2(ist(:,9),:);

%figure_w_normalized_uicontrolunits(3)
%clf
%plode = plot(newt3(:,9),-newt3(:,8),'o')
%axis([81 92.6 -20 0])
%grid
%xlabel('Time')
%ylabel('Depth in km')

timeplot


% This script is used to evluation 3D face reconstruction using NME metric,
% The script contains the process of 3D face mesh reconstruction from the
% 3DMM parameters, and the process of Iterative Closest Point (ICP).
clear all;
close all;
addpath(genpath('./visualize'));
addpath(genpath('./3D_ICP-master'));
aa = textread('./keypoints.txt', '%s');
bb = dir('./results/AFLW-2000-3D_grdth/*.mat');
load('Model_Expression.mat');
load('Model_Shape.mat');
load('BaseSample.mat');

std_size = 120;
vertex_mean = reshape(mu_shape, 3, length(mu_shape)/3);
mu_id = mu_shape + mu_exp;

base_ind = keypoints;
base_ind1 = [(3 * base_ind - 2); (3 * base_ind -1); (3 * base_ind)];
base_ind1 = base_ind1(:);
mu_base = mu_id(base_ind1);
w_base = w_shape(base_ind1,:);
w_exp_base = w_exp(base_ind1,:);

pts = zeros(204, 1);
for i = 1:204
    xx = aa(i);
    xx = xx{1};
    pts(i) = str2num(xx(1:end-1));
end
pts = reshape(pts, 3, 68) + 1;
index = pts(3,:)/3;

LL = 40;
nme_list_2DASSL = zeros(2, LL);
nme_list_3DDFA = zeros(2, LL);
smpNum = 50;
for ii = 1:LL
    ii
    imgName = bb(ii).name;
    img = imread(strcat('./results/AFLW-2000-3D_grdth/', strrep(imgName,'mat', 'jpg')));

    [height, width, nChannels] = size(img);
    info = load(strcat('./results/AFLW-2000-3D_grdth/', imgName));
    pt3d_68 = info.pt3d_68;
    Pose_Para = info.Pose_Para;
    pts1 = pt3d_68;

    % Reconstruct 3D point of base point
    project_base_point = pt3d_68;
    bbox = [min(project_base_point(1,:)), min(project_base_point(2,:)), max(project_base_point(1,:)), max(project_base_point(2,:))];

    center = [(bbox(1)+bbox(3))/2, (bbox(2)+bbox(4))/2];
    radius = max(bbox(3)-bbox(1), bbox(4)-bbox(2)) / 2;
    bbox = [center(1) - radius, center(2) - radius, center(1) + radius, center(2) + radius];

    bbox = double(bbox);
    widthb = vertex_mean(1,keypoints(17)) - vertex_mean(1,keypoints(1));
    heightb = vertex_mean(2, keypoints(20)) - vertex_mean(2, keypoints(9));

    mean_x = vertex_mean(1, keypoints(31));
    mean_y = vertex_mean(2, keypoints(31));

    f0 = ((bbox(3) - bbox(1)) / widthb + (bbox(4) - bbox(2)) / heightb) / 2;
    t3d0(1) = (bbox(3) + bbox(1))/2 - f0*mean_x;
    temp = height + 1 - (bbox(2) + bbox(4))/2;
    t3d0(2) = temp - f0 * mean_y;
    t3d0(3) = 0;

    Pose_Para0 = [Pose_Para(1:3), t3d0(1), t3d0(2), t3d0(3), f0];
%    Shape_Para0 = zeros(size(Shape_Para));
%    Exp_Para0 = zeros(size(Exp_Para));
    Shape_Para = info.Shape_Para;
    Tex_Para = info.Tex_Para;
    Pose_Para = info.Pose_Para;

    [phi, gamma, theta, t3d, f] = ParaMap_Pose(Pose_Para);
    R = RotationMatrix(phi, gamma, theta);

    alpha_shape = Shape_Para;
    alpha_tex = Tex_Para;
    alpha_exp = Exp_Para;
    express = w_exp * alpha_exp; express = reshape(express, 3, length(express)/3);
    shape = mu_shape + w_shape * alpha_shape; shape = reshape(shape, 3, length(shape)/3);
    tex = mu_tex + w_tex * alpha_tex; tex = reshape(tex, 3, length(tex)/3);
    vertex = shape + express;
    grdVertex = f * R * vertex + repmat(t3d, 1, size(vertex, 2));
%     grdVertex(1,:) = grdVertex(1,:) - min(grdVertex(1,:));
%     grdVertex(2,:) = grdVertex(2,:) - min(grdVertex(2,:));
    grdVertex(3,:) = grdVertex(3,:) - min(grdVertex(3,:));

%     figure
%     imshow(uint8(img), [])
%     hold on

    minx = min(grdVertex(1,:)); miny = min(grdVertex(2,:));
    maxx = max(grdVertex(1,:)); maxy = max(grdVertex(2,:));
    llen = sqrt((maxx-minx)*(maxy-miny));

    grdVertex(3,:,:) = grdVertex(3,:,:) - min(grdVertex(3,:,:));
    grdVertex(2,:,:) = 1-grdVertex(2,:,:)+450;
%     pcshow(grdVertex')
%     view(2)

    vertex1 = load(strcat('./results/2DASL_results/', imgName));
    vertex1 = vertex1.vertex;
    vertex1(3,:) =  vertex1(3,:) - min(vertex1(3,:)) + 120;
%     vertex1(1,:) =  vertex1(1,:) - min(vertex1(1,:));
%     vertex1(2,:) =  vertex1(2,:) - min(vertex1(2,:));
    vertex1(3,:) =  vertex1(3,:) - min(vertex1(3,:));
%     pcshow(vertex1')
%     view(2)

    grdVertex_ori = grdVertex(:,1:smpNum:53215);
    vertex1 = vertex1(:,1:smpNum:53215);
%     plot_3d_2(grdVertex_ori', vertex1', -90); % ��ʾ����ǰ�����㼯
    [grdVertex1, vertex1] = icp_process_xgtu(grdVertex_ori', vertex1');
%     plot_3d_2(grdVertex1, vertex1, -90);
    dis1 = (grdVertex1' - vertex1');
    dis1 = sqrt(sum(dis1.^2));
    dis1 = mean(dis1)/llen;

    vertex2 = load(strcat('./results/3DDFA_results/', imgName));
    vertex2 = vertex2.vertex;
    vertex2(3,:,:) =  vertex2(3,:,:) - min(vertex2(3,:,:)) + 120;
    vertex2(3,:) =  vertex2(3,:) - min(vertex2(3,:));
%     pcshow(vertex2')
%     view(2)

    vertex2 = vertex2(:,1:smpNum:53215)
%     plot_3d_2(grdVertex_ori', vertex2', -90); % ��ʾ����ǰ�����㼯
    [grdVertex2, vertex2] = icp_process_xgtu(grdVertex_ori', vertex2');
%     plot_3d_2(grdVertex2, vertex2, -90);
    dis2 = (grdVertex2' - vertex2');
    dis2 = sqrt(sum(dis2.^2));
    dis2 = mean(dis2)/llen;

    nme_list_2DASSL(1, ii) = dis1;
    nme_list_3DDFA(1, ii) = dis2;
end

% save('nme_list', 'nme_list');
dis_2DASSL = nme_list_2DASSL(1,:);
dis_3DDFA = nme_list_3DDFA(1,:);

[s_dis_2DASSL, index] = sort(dis_2DASSL);
[s_dis_3DDFA, index] = sort(dis_3DDFA);

s_dis_2DASSL = s_dis_2DASSL(1:LL);
s_dis_3DDFA = s_dis_3DDFA(1:LL);

x_len = 0:1:length(s_dis_2DASSL);
x_len = x_len(1:length(s_dis_2DASSL));
plot(s_dis_2DASSL*100, x_len, 'g', 'linewidth',2);
hold on
plot(s_dis_3DDFA*100, x_len, 'r', 'linewidth',2);

axis([0 5 0 LL])
set(gca,'XLim',[0 5]);%
set(gca,'YLim',[0 LL]);%

grid on
grid minor

ax = gca;
ax.GridColor = [0 .5 .5];
ax.GridLineStyle = '--';
ax.GridAlpha = 0.5;
ax.Layer = 'top';

h = legend('2DASSL', '3DDFA', 'Location','southeast')
%set(h,'Orientation','horizon', 'Fontsize',12)
set(h,'Fontsize',12)

xlabel('NME normalized by bounding box', 'fontsize', 12)
ylabel('Number of images', 'fontsize',12)


roi = [1 1  480 720];
%Region of interest
maxNumObj = 200; % Maximum number of objects to track
alarmCount = 75; % Max no of frames an object can remain stationary before alarm is raised
maxConsecutiveMiss = 7; %Min frames an object's centroid changes after which it is not tracked

% System object for reading the video with each frame of the type 'Single'
hVideoSrc = vision.VideoFileReader;
hVideoSrc.Filename = input('Enter the video to run:- ','s');
hVideoSrc.VideoOutputDataType = 'single';

% Offsets for drawing bounding boxes in original input video
PtsOffset = int32(repmat([roi(1), roi(2), 0, 0],[maxNumObj 1]));

% Converts RGB image to YCbCr
hColorConv = vision.ColorSpaceConverter('Conversion', 'RGB to YCbCr');

% Does background subtraction on 2 images
hAutothreshold = vision.Autothresholder('ThresholdScaleFactor', 1.0);

% Removes noise and really small blobs
hClosing = vision.MorphologicalClose('Neighborhood', strel('square',10));

% Finds the blobs in the segmented images, properties of blobs also
% specified
hBlob = vision.BlobAnalysis('MaximumCount', maxNumObj, 'ExcludeBorderBlobs', true);
hBlob.MinimumBlobArea = 100;
hBlob.MaximumBlobArea = 5000;

% Creating system objects for players with their locations
pos = [10 300 roi(3)+25 roi(4)+25];
hAbandonedObjects = vision.VideoPlayer('Name', 'Abandoned Objects', 'Position', pos);
pos(1) = 46+roi(3); % move the next viewer to the right
hAllObjects = vision.VideoPlayer('Name', 'All Objects', 'Position', pos);
pos = [80+2*roi(3) 300 roi(3)-roi(1)+25 roi(4)-roi(2)+25];
hThresholdDisplay = vision.VideoPlayer('Name', 'Threshold', 'Position', pos);
mPlayer1=vision.DeployableVideoPlayer('Location',[10,100]);
mPlayer2=vision.DeployableVideoPlayer('Location',[20,110]);
mPlayer3=vision.DeployableVideoPlayer('Location',[20,110]);
mPlayer4=vision.DeployableVideoPlayer('Location',[20,110]);
mPlayer5=vision.DeployableVideoPlayer('Location',[20,110]);

firsttime = true; % Initialisation for storing background
allBlobList=[]; % Used for tracking blobs, stores all information about all blobs
frame_no=0; % Current frame no

% Loop runs till all the video frames are completed
while ~isDone(hVideoSrc)
    frame_no=frame_no+1;
    Im = step(hVideoSrc); % Stores current frame in Im
    OutIm = Im(roi(2):end, roi(1):end, :); % Selects the region of interest from the original video
    YCbCr = step(hColorConv, OutIm); % Gives the YCbCr image of frame
    CbCr = complex(YCbCr(:,:,2), YCbCr(:,:,3)); % CbCr has the color components of YCbCr
    step(mPlayer2,YCbCr); % Dis[plays YCbCr image

    % Stores background
    if firsttime
        firsttime = false;
        BkgY = YCbCr(:,:,1);
        BkgCbCr = CbCr;
    end

    SegY = step(hAutothreshold, abs(YCbCr(:,:,1)-BkgY)); % Background subtraction on the luminosity part
    SegCbCr = abs(CbCr-BkgCbCr) > 0.05; % Background subtraction on chroma parts
    step(mPlayer3,SegY); % Luminosity subtracted image

    % Fill in small gaps in the detected objects and clubs the separated
    % image
    Segmented = step(hClosing, SegY | SegCbCr);
    step(mPlayer1,Segmented); % Foreground

    % Perform blob analysis
    [Area, Centroid, BBox] = step(hBlob, Segmented);
    %[x y hitCount latest_detected_frame blob_number starting_frame misscount]
    %[^ ^ ^         ^                      ^           ^              ^]
    for blob=1:size(Centroid,1)%Traverses through the list of Centroids of all the blobs
        %To map x and y to the largest multiple of 5 less than it
        %eg->(103,107)=(100,105)
        roundX=Centroid(blob,1)-mod(Centroid(blob,1),5);
        roundY=Centroid(blob,2)-mod(Centroid(blob,2),5);
        found=false;%to check if the given blob centoid already was there or not
        for x=1:size(allBlobList,1)
            if(allBlobList(x,1)==roundX && allBlobList(x,2)==roundY)%if the centroid was found
                allBlobList(x,3)=allBlobList(x,3)+1;%increasing the hit count
                found=true;
                allBlobList(x,4)=frame_no;%storing the latest detected frame no.
                allBlobList(x,5)=blob;%storing the blob number of the centroid of that frame
                allBlobList(x,7)=0;%since the blob as detected, miss count will be zero
            end
        end
        if(found==false)%else we need to add the centroid to our list of centroids
            allBlobList=[allBlobList;[roundX roundY 1 frame_no blob frame_no 0 ]];
        end
    end
    BlobCount = size(BBox,1);%to get the total no. o blobs detected in that frame

    BBoxOffset = BBox + int32(repmat([roi(1) roi(2) 0 0],[BlobCount 1]));
    Imr = insertShape(Im,'Rectangle',BBoxOffset,'Color','green');%inserting green rectangles for that detected blobs
    myImr=insertMarker(Imr,Centroid);%inserting marker at the centroid of each blob
    rowToRemove=[];%list of all the rows to be  removed since there miss count exceeded the maxMissCount
    for blob=1:size(allBlobList,1)
        %check for the blob to be abandoned
        if(allBlobList(blob,3)>alarmCount && allBlobList(blob,7)<=maxConsecutiveMiss && frame_no==allBlobList(blob,4) )
            %allBlobList(blob,7)=0;
            myImr=insertShape(myImr, 'FilledRectangle', BBox(allBlobList(blob,5), :), 'color','red', 'Opacity', 0.5);
            sound(randn(4096,1),8192);
        %increment the miss count
        else
            allBlobList(blob,7)=allBlobList(blob,7)+1;
            if(allBlobList(blob,7)>maxConsecutiveMiss)%if misscount>maxConsicutiveMiss delete it
                rowToRemove=[rowToRemove;blob];
            end
        end
    end
    %deleting the rows for misscount>maxConsicutiveMiss
    for row=1:size(rowToRemove,1)
        try
        allBlobList(row,:)=[];
        end
    end
    step(hAllObjects, myImr);
    % Display the segmented video
    SegBBox = PtsOffset;
end
release(hVideoSrc);


function [ a, b ] = p04_lim ( )

%*****************************************************************************80
%
%% P04_LIM returns the integration limits for problem 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A(2), B(2), the lower and upper limits of integration.
%
  a(1:2) = -1.0;
  b(1:2) =  1.0;

  return
end


function p = addMonomialCuts(p)

if any(p.originalModel.variabletype==3)
    singleMonomial =  sum(p.originalModel.monomtable |  p.originalModel.monomtable,2)==1;
    p_cut = emptyNumericalModel;
    monomials = find(p.originalModel.variabletype==3);
    for i = 1:length(monomials)
        monom_index = monomials(i);
        monom = p.originalModel.monomtable(monomials(i),:);
        monom_variable = find(monom);
        if length(monom_variable)==1
            n = monom(monom_variable);
            L = p.lb(monom_variable);
            U = p.ub(monom_variable);
            if ~isinf(L) && ~isinf(U)
                if even(n)
                    M = (L+U)/2;
                    if L <= 0 && U >= 0 && M~=0
                        if M < 0
                            [Ax,Ay,b,K] = convexhullConvex(L,M,0,U,L^n,M^n,0,U^n,n*L^(n-1),n*M^(n-1),0,n*U^(n-1));
                        else
                            [Ax,Ay,b,K] = convexhullConvex(L,0,M,U,L^n,0,M^n,U^n,n*L^(n-1),0,n*M^(n-1),n*U^(n-1));
                        end
                    else
                        [Ax,Ay,b,K] = convexhullConvex(L,M,U,L^n,M^n,U^n,n*L^(n-1),n*M^(n-1),n*U^(n-1));
                    end
                    p_cut.F_struc(end+1:end+length(b),1) = b;
                    p_cut.F_struc(end-length(b)+1:end,1+monom_variable) = -Ax;
                    p_cut.F_struc(end-length(b)+1:end,1+monom_index) = -Ay;
                    p_cut.K.l = p_cut.K.l+length(b);
                else
                    if p.lb(monom_variable)<0 &  p.ub(monom_variable)>0 & ~isinf(p.lb(monom_variable)) & ~isinf(p.ub(monom_variable))

                        % Line between lower bound and tangent intersection
                        r = zeros(1,n+1);r(1)=n-1;r(2)=-L*n;r(end)=L^n;
                        r = roots(r);
                        %r = r(min(find(r==real(r))));
                        r = max(r(imag(r)==0));
                        if r >= U
                            r = U;
                            fprim = (U^n-L^n)/(U-L);
                        else
                            fprim = n*r^(n-1);
                        end
                        p_cut.F_struc(end+1,1) = -L^n+L*fprim;
                        p_cut.F_struc(end,1+monom_index)=1;
                        p_cut.F_struc(end,1+monom_variable)=-fprim;
                        p_cut.K.l = p_cut.K.l+1;

                        % Line between upper bound and tangent intersection
                        r = zeros(1,n+1);r(1)=n-1;r(2)=-U*n;r(end)=U^n;
                        r = roots(r);
                        r = min(r(imag(r)==0));
                        if r <= L
                            r = L;
                            fprim = (U^n-L^n)/(U-L);
                        else
                            fprim = n*r^(n-1);
                        end
                        p_cut.F_struc(end+1,1) = U^n-U*fprim;
                        p_cut.F_struc(end,1+monom_index)=-1;
                        p_cut.F_struc(end,1+monom_variable)=fprim;
                        p_cut.K.l = p_cut.K.l+1;
                    end
                end
            end
        end
    end
    p = mergeNumericalModels(p,p_cut);
end


function [rsun,rmoon,gmst]=sunmoonpos(tutc,erpv)

tut=timeadd(tutc,erpv(3));

%calculate sun and moon position in ECI
[rsun,rmoon]=sunmoonpos_eci(tut);

%calculate the transition matrix from ECI to ECEF
[U,gmst]=eci2ecef(tutc,erpv);

%calculate sun and moon position in ECEF
rsun=U*rsun;
rmoon=U*rmoon;

return


%% ----------------------------
% Input: Work_mode: Mode of working condition 1 --> BBDST, 2 --> constant current
%                   SOC_est_init: The initial value of estimated SOC
%% ----------------------------
function main(Work_mode, SoC_est_init)
    if nargin == 0  % Set parameter by default
        Work_mode = 1;
        SoC_est_init = 1;
    elseif nargin == 1
        SoC_est_init = 1;
    end
    if Work_mode == 1
        sim BBDST_workingcondition;
        I = -(current.data)' * 1.5 / 50;
    elseif Work_mode == 2
        N = 60001;
        I = 1.5 * ones(1, N);
        I(ceil(N / 5) : ceil(N * 3 / 9)) = 0;
        I(ceil(N * 5 / 9) : ceil(N * 4 / 5)) = 0;
    else
        disp("Input error!");
        disp("Work_mode: Mode of working condition");
        disp("           1 --> BBDST, 2 --> constant current ");
        disp("SOC_est_init : The initial value of estimated SOC");
        return;
    end
    tic;  % start time
    [avr_err_EKF, std_err_EKF, avr_err_UKF, std_err_UKF] = EKF_UKF_Thev(SoC_est_init, I);
    toc;  % end time
    fprintf('Initial SOC : %f\nWorking Mode: %d\n', SoC_est_init, Work_mode);
    fprintf("avr_err_EKF --> %f\n", avr_err_EKF);
    fprintf("standard_err_EKF --> %f\n", std_err_EKF);
    fprintf("avr_err_UKF --> %f\n", avr_err_UKF);
    fprintf("standard_err_UKF --> %f\n", std_err_UKF);
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs, non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

%RL = springs_info(:,4); % resting-length vector

% Contraction Frequency
freq = 1.0;

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(159:end,4) = abs( cos(freq*current_time*pi) );

%NOTE: not 2*pi*ft b/c of abs()


function isMatchAll = eval_match_parts_gt(endpointsAll, annolist, sc, evlType, factor, nPartsEval)

fprintf('eval_match_parts_gt()\n');

if ischar(annolist)
    annolist = loadannotations(annolist);
end

if (nargin < 5)
    factor = 0.5;
end

if (nargin < 4)
    evlType = 1;
end

if (nargin < 5)
    nPartsEval = 10;
end

if (nPartsEval > 10)
    [~, parts] = util_get_parts_spatial();
else
    [~, parts] = util_get_parts();
end

if (evlType == 2)
    mean_part_length = eval_compute_mean_part_length(annolist,parts);
end

isMatchAll = nan(length(annolist),length(parts));

for imgidx = 1:length(annolist)
    fprintf('.');
    rect = annolist(imgidx).annorect(1);
    points = rect.annopoints.point;
    endpoints = endpointsAll{imgidx};

    if (isempty(endpoints))
        for pidx = 1:length(parts)
            p1 = util_get_annopoint_by_id(points,parts(pidx).xaxis(2));
            p2 = util_get_annopoint_by_id(points,parts(pidx).xaxis(1));
            if (~isempty(p1) && ~isempty(p2))
                isMatchAll(imgidx,pidx) = 0;
            end
        end
    else
        for pidx = 1:length(parts)
            p1 = util_get_annopoint_by_id(points,parts(pidx).xaxis(2));
            p2 = util_get_annopoint_by_id(points,parts(pidx).xaxis(1));

            detBottom = [endpoints(pidx,1) endpoints(pidx,2)];
            detTop    = [endpoints(pidx,3) endpoints(pidx,4)];

            if (~isempty(p1) && ~isempty(p2))
                gtBottom = sc*[p1.x p1.y];
                gtTop    = sc*[p2.x p2.y];

                distBottom = norm(detBottom - gtBottom);
                distTop    = norm(detTop   - gtTop);

                if (evlType == 1) % standard pcp
                    % use part size
                    gtScale = norm(gtBottom - gtTop);
                elseif (evlType == 2) % pcp with fixed thresh
                    % use mean part length
                    gtScale = sc*mean_part_length(pidx);
                end

                bIsGTmatch = is_gt_match_pcp(distBottom, distTop, factor, gtScale);
                isMatchAll(imgidx,pidx) = bIsGTmatch;
            end
        end
    end
    if (~mod(imgidx, 100))
        fprintf(' %d/%d\n',imgidx,length(annolist));
    end
end
fprintf('\ndone\n');

    function res = is_gt_match_pcp(distBottom, distTop, factor, gtLen)
        res = distBottom <= gtLen*factor && distTop <= gtLen*factor;
    end

end


function IND = determine_search_location(A,method,params)

% determine the search location for updating each spatial component
% INPUTS:
% A:        current estimate of spatial component ( d x nr sparse matrix)
% method:   method to be used of determining search locations
%     available methods:
%       'ellipse': draw an ellipse centered at the center of mass with
%           axes the first two principal components and expand it to
%           determine the search location  (default)
%       'dilate' : grow the spatial footprint by dilating the current
%           footprint
%      if a different argument is passed then search location covers the whole field of view
% params:   hyper-parameter struct for the various methods (see default settings below for description)
%
% OUTPUT:
% IND:      binary d x nr matrix. IND(i,j) = 1 if pixel i is included in the search location of component j
%
% Written by Eftychios A. Pnevmatikakis, Simons Foundation
%   with input from Weijian Yang, Columbia University

if nargin < 3
    params = [];
end

if nargin < 2 || isempty(method)
    method = 'ellipse';         % default method
end

[d,nr] = size(A);

if ~isfield(params,'d1'); d1 = sqrt(d); params.d1 = d1; else d1 = params.d1; end          % # of rows
if ~isfield(params,'d2'); d2 = sqrt(d); params.d2 = d2; else d2 = params.d2; end          % # of columns
if ~isfield(params,'d3'); d3 = 1; params.d3 = d3; else d3 = params.d3; end
if ~isfield(params,'min_size') || isempty(params.min_size); min_size = 3; else min_size = params.min_size; end    % minimum size of ellipse axis
if ~isfield(params,'max_size') || isempty(params.max_size); max_size = 8; else max_size = params.max_size; end    % maximum size of ellipse axis
if ~isfield(params,'dist')  || isempty(params.dist); dist = 3; else dist = params.dist; end                              % expansion factor of ellipse
if ~isfield(params,'bSiz')  || isempty(params.dist); bSiz = 4; else bSiz = params.bSiz; end                              % expansion factor of ellipse
if ~isfield(params,'se')  || isempty(params.se);  % morphological element (for 'dilate')
    if d3 == 1; expandCore = strel('disk',bSiz,0); else expandCore = strel(ones(bSiz,bSiz,2)); end
else
    expandCore = params.se;
end

if strcmpi(method,'ellipse'); method = 'ellipse';
elseif strcmpi(method,'dilate'); method = 'dilate';
else fprintf('Method not recongnized. Search location equals the entire field of view. \n');
end

IND = false(d,nr);
% find all zero components
ind_empty = (sum(A, 1)==0);
if any(ind_empty)
    A(1, ind_empty) = 1;
end
switch method
    case 'ellipse'
        Coor.x = kron(ones(d2*d3,1),(1:d1)');
        Coor.y = kron(ones(d3,1),kron((1:d2)',ones(d1,1)));
        Coor.z = kron((1:d3)',ones(d1*d2,1));
        if ~(dist==Inf)             % determine search area for each neuron
           %cm = zeros(nr,2);        % vector for center of mass
           cm = com(A,d1,d2,d3);
           if d3 == 1
               cm = [cm,ones(nr,1)];
           end
           Vr = cell(nr,1);
           IND = zeros(d,nr);       % indicator for distance
           %cm(:,1) = Coor.x'*A(:,1:nr)./sum(A(:,1:nr));
           %cm(:,2) = Coor.y'*A(:,1:nr)./sum(A(:,1:nr));          % center of mass for each components
           parfor i = 1:nr            % calculation of variance for each component and construction of ellipses
               if d3 == 1
                   Vr{i} = ([Coor.x - cm(i,1), Coor.y - cm(i,2)]'*spdiags(A(:,i),0,d,d)*[Coor.x - cm(i,1), Coor.y - cm(i,2)])/sum(A(:,i));
                   [V,D] = eig(Vr{i});
                   cor = [Coor.x - cm(i,1),Coor.y - cm(i,2)];
               else
                   Vr{i} = ([Coor.x - cm(i,1), Coor.y - cm(i,2), Coor.z - cm(i,3)]'*spdiags(A(:,i),0,d,d)*[Coor.x - cm(i,1), Coor.y - cm(i,2), Coor.z - cm(i,3)])/sum(A(:,i));
                   [V,D] = eig(Vr{i});
                   cor = [Coor.x - cm(i,1),Coor.y - cm(i,2),Coor.z - cm(i,3)];
               end
               d11 = min(max_size^2,max(min_size^2,D(1,1)));
               d22 = min(max_size^2,max(min_size^2,D(2,2)));
               if d3 == 1
                   IND(:,i) = sqrt((cor*V(:,1)).^2/d11 + (cor*V(:,2)).^2/d22)<=dist;
               else
                   d33 = min((max_size/2)^2,max((min_size/2)^2,D(3,3)));
                   IND(:,i) = sqrt((cor*V(:,1)).^2/d11 + (cor*V(:,2)).^2/d22 + (cor*V(:,3)).^2/d33)<=dist;       % search indeces for each component
               end
           end
        else
            IND = true(d,nr);
        end
    case 'dilate'
        A = threshold_components(A,params);
        parfor i = 1:nr
            A_temp = imdilate(reshape(full(A(:,i)),d1,d2,d3),expandCore);
            IND(:,i) = A_temp(:)>0;
        end
    otherwise
        IND = true(d,nr);
end
if any(ind_empty)
    IND(:, ind_empty) = false;
end


%
% Clear tiny coefficients, tiny real parts, and tiny imaginary parts
% from a polynomial or a cell array of polynomials
%
% Syntax:  (pclear is the shortened alias of PolynomialClear)
%          >> g = PolynomialClear(f)
%          >> g = PolynomialClear(f,tol)
%
%   Input:   f --- (string/numeric/cell array) polynomial (array)
%          tol --- (numeric, optional)         threshold for being tiny
%
%  Output:   g --- (string/numeric/cell array) cleared polynomial (array)
%
%  Example:
%
%   >> PolynomialClear('(3+2e-13*i) + 2.1e-15*x*y+(1e-14+2*i)*x^3',1e-10)
%
%   ans =
%
%   3 + (0+2i)*x^3


function [gradp,varlist] = gradient(sp)
% sympoly/gradient: gradient vector of a sympoly
% usage: [gradp,varlist] = gradient(sp);
%
% arguments: (input)
%     sp - scalar sympoly object
%
% arguments: (output)
%  gradp - sympoly (row vector) object containing the gradient vector
%
%  varlist - cell array of variable names used for the gradient

if numel(sp)>1
  error 'Gradient only works for scalar sympoly objects.'
end

varlist = setdiff(sp.Var,{''});
nvar=length(varlist);

if nvar==0
  % sympoly had no variables, i.e., it was a constant
  gradp=sympoly(0);
else
  % loop over the variables
  gradp = sympoly(zeros(1,nvar));
  for i=1:nvar
    gradp(i)=diff(sp,1,varlist{i});
  end
end