function output = callfiltersd(model)

options = model.options.filtersd;

model = yalmip2nonlinearsolver(model);

% Make sure the callback returns a dense Jacobian
model.dense = 1;

if ~model.derivative_available
    disp('Derivate-free call to filterSD not yet implemented')
    error('Derivate-free call to filterSD not yet implemented')
end
if model.options.savedebug
    save filtersddebug model
end
showprogress('Calling filterSD',model.options.showprogress);

cu = [ repmat(0,length(model.bnonlinineq),1);
    repmat(0,length(model.bnonlineq),1);
    repmat(0,length(model.b),1);
    repmat(0,length(model.beq),1)];

cl = [ repmat(-inf,length(model.bnonlinineq),1);
    repmat(0,length(model.bnonlineq),1);
    repmat(-inf,length(model.b),1);
    repmat(0,length(model.beq),1)];

if isempty(cu)
    Flow = [];
    Fupp = [];
end

% These are needed to avoid recomputation due to ipopts double call to get
% f and df, and g and dg
global latest_x_f
global latest_x_g
global latest_df
global latest_f
global latest_G
global latest_g
global latest_xevaled
global latest_x_xevaled
latest_G = [];
latest_g = [];
latest_x_f = [];
latest_x_g = [];
latest_xevaled = [];
latest_x_xevaled = [];

funcs.objective = @(x)ipopt_callback_f(x,model);
funcs.gradient = @(x)ipopt_callback_df(x,model);
if ~isempty(cu)
    funcs.constraints = @(x)ipopt_callback_g(x,model);
    funcs.jacobian  = @(x)ipopt_callback_dg(x,model);
else
    funcs.constraints = [];
    funcs.jacobian = [];
end

lb  = model.lb(:)';
ub = model.ub(:)';

if ~model.options.usex0
    model.x0 = (lb+ub)/2;
    model.x0(isinf(ub)) = lb(isinf(ub))+1;
    model.x0(isinf(lb)) = ub(isinf(lb))-1;
    model.x0(isinf(model.x0)) = 0;
end

options.display = model.options.verbose;
solvertime = tic;
[xout,fval,exitflag,stats,lambda] = filtersd(funcs.objective, funcs.gradient, model.x0, lb, ub, funcs.constraints, funcs.jacobian, cl, cu, options);
solvertime = toc(solvertime);

% Duals currently not supported
lambda = [];

x = RecoverNonlinearSolverSolution(model,xout);

switch exitflag
    case {0}
        problem = 0;
    case {1}
        problem = 2;
    case {2,3}
        problem = 1;
    case {5}
        problem = 3;
    case {4,9}
        problem = 4;
    case 105
        problem = 16;
    otherwise
        problem = -1;
end

% Internal format for duals
D_struc = [];

% Save all data sent to solver?
if model.options.savesolverinput
    solverinput.model = model;
else
    solverinput = [];
end

% Save all data from the solver?
if model.options.savesolveroutput
    solveroutput.x = xout;
    solveroutput.exitflag = exitflag;
    solveroutput.stats = stats;
else
    solveroutput = [];
end

% Standard interface
output = createoutput(x,D_struc,[],problem,'filterSD',solverinput,solveroutput,solvertime);


function res = stackSlice(img, dir, slice)
%STACKSLICE Extract a planar slice from a 3D image
%
%   SLICE = stackSlice(IMG, DIR, INDEX)
%   IMG is either a 3D or a 4D (3D+color) image.
%   DIR is 1, 2 or 3, or 'x', 'y', 'z'
%   INDEX is the slice index, between 1 and the number of voxels in the DIR
%   direction (using physical indexing).
%
%   If the input image has size NY-by-NX-by-NZ (gray scale image), or
%   NY-by-NX-by-3-by-NZ (color image), then the resulting slice has size:
%   - NZ-by-NY(-by-3) if DIR is 1 or DIR is 'x'
%   - NX-by-NZ(-by-3) if DIR is 2 or DIR is 'y'
%   - NY-by-NX(-by-3) if DIR is 3 or DIR is 'z'
%
%   The functions returns a planar image with the same type as the
%   original.
%
%
%   Example
%   % Display 3 slices of a MRI head
%     img = analyze75read(analyze75info('brainMRI.hdr'));
%     figure(1); clf; hold on;
%     for i=1:3
%         subplot(3, 1, i);
%         imshow(stackSlice(img, 'x', 20+20*i)');
%         set(gca, 'ydir', 'normal')
%     end
%
%   See also
%     imStacks, imMiddleSlice

%   ------
%   author: David Legland, david.legland@inra.fr
%   INRA - Cepia Software Platform
%   Created: 2007-08-14,    using Matlab 7.4.0.287 (R2007a)
%   http://www.pfl-cepia.inra.fr/index.php?page=slicer

%   HISTORY
%   2010-12-02 code cleanup, fix dimension order of output slices
%   2011-04-26 use xyz convention for slice index

% image size
dim = size(img);

% convert to an index between 1 and 3, in xyz order
dir = parseAxisIndex(dir);

% check dimension of input array
nd = length(dim);
if nd < 3 || nd > 4
    error('Input array should have dimension 3 or 4');
end

% Extract either a grayscale or a color slice
if nd == 3
    % gray-scale image
    switch dir
        case 1
            % X-slice, result is a YZ slice with size NZ-by-NY
            res = squeeze(img(:, slice, :))';
        case 2
            % Y-slice, result is a ZX slice with size NX-by-NZ
            res = squeeze(img(slice, :, :));
        case 3
            % Z-slice, result is a XY slice with size NY-by-NX
            res = img(:, :, slice);
        otherwise
            error('Direction should be comprised between 1 and 3');
    end

else
    % color images: keep channel dim as third dimension, and put slice
    % dimension as last dimension.
    switch dir
        case 1
            % X-slice, result is a YZ color slice with size NZ-by-NY-by-3
            res = squeeze(permute(img(:, slice, :, :), [4 1 3 2]));
        case 2
            % Y-slice, result is a ZX color slice with size NX-by-NZ-by-3
            res = squeeze(permute(img(slice, :, :,:), [2 4 3 1]));
        case 3
            % Z-slice, result is a XY color slice with size NY-by-NX-by-3
            res = img(:, :, :, slice);
        otherwise
            error('Direction should be comprised between 1 and 3');
    end
end


function ho=addmarkers(hi,n)
% ADDMARKERS    Add a pre-defined number of markers to a plot instead of
% one at each datapoint which is default in Matlab.
%
% Usage:
% ADDMARKERS(HI) adds 5 markers to the input handle HI, J=1..length(HI)
% ADDMARKERS(HI,N) adds N markers to the input handle HI, J=1..length(HI)
% HO=ADDMARKERS(...) returns the output handle HO for future usage such as
% changing colors, markers, etc.
%
% Hint: use the output handle when adding legends, e.g.
% LEGEND(HO,'legend 1',...,'legend N')
%
% The markers will be added in the following order:
%
% o     Circle
% s     Square
% d     Diamond
% x     Cross
% +     Plus sign
% *     Asterisk
% .     Point
% ^     Upward-pointing triangle
% v     Downward-pointing triangle
% >     Right-pointing triangle
% <     Left-pointing triangle
% p     Five-pointed star (pentagram)
% h     Six-pointed start (hexagram)
%
% By Matthijs Klomp at Saab Automobile AB, 2009-10-11
% E-mail: matthijs.klomp@gm.com
%
if nargin<2, n=5; end % default number of markers
if nargin<1, error('Supply an input handle as input argument.'), end
figure(get(get(hi(1),'parent'),'parent'))% plot in the figure of the handle
subplot(get(hi(1),'parent'))            % plot in the subplot of the handle
hold on                                 % do not overwrite the current plot
markers = {'o','s','d','^','v','x','+','*','.','>','<','p','h'};
ho = hi;                                % initialize output handle
for i = 1:length(hi)
    x = get(hi(i),'xdata');             % get the independent variable
    y = get(hi(i),'ydata');             % get the dependent variable data
    s = linspace(1,n,length(x));        % sampling independent variable
    sn = [1 (2:n-1)+randn(1,n-2)/n n];  % add some noise to avoid overlap
    xrs = interp1(s,x,sn,'nearest');    % downsample to n datapoints
    yrs = interp1(s,y,sn,'nearest');    % downsample to n datapoints
    plot(xrs,yrs,...                    % Plot the markers
        'Marker',markers{i},'LineStyle','None','Color',get(hi(i),'Color'));
    ho(i) = plot([0 1],[1 1]*NaN,...    % Create the output handle
        'Marker',markers{i},...
        'LineStyle',get(hi(i),'Linestyle'),...
        'Color',get(hi(i),'Color'));
end
if nargout==0, clear ho, end


%This directory contains the files which can be used for various ways of
%visualizing data and analyses.
%
%PLOT_CHANNEL - Plot the classwise averages of one channel of 1D (time OR
% frequency) or 2D data (time x frequency)
%PLOT_SCOREMATRIX - Visualizes score matrix of e.g. r-values
%
%GRID_PLOT - Classwise averaged epochs in a grid layout
%GRID_ADDBARS - Add a colorbar to the bottom of each subplot of a grid plot
%GRID_MARKBYBOX - Draw error or percentiles box inside grid plots
%GRID_MARKINTERVAL - Draw time interval inside grid plots as colored patches
%GRID_MARKRANGE -  Mark range on y axis  inside grid plots as horizontal lines
%GRID_MARKTIMEBOX - Draw time intervals inside grid plots with vertical lines
%GRID_MARKTIMEPOINT -  Draw a time point inside grid plots as a vertical line
%
%MNT_ADAPTMONTAGE - Adapts an electrode montage to another electrode set
%MNT_RESTRICTMONTAGE - Restrict an electrode montage to a subset of channels
%MNT_RESTRICTNIRSMONTAGE - restrict a NIRS montage to a subset of channels
%MNT_GETNIRSMONTAGE - Get montage with NIRS channel positions
%MNT_SCALPTOGRID - Montage for grid plot with boxes at scalp locations
%MNT_SETELECTRODEPOSITIONS - Electrode positions of standard named channels
%MNT_SETGRID - Define a new eletrode grid layout for an electrode montage
%
%PLOT_SCALP - Display a vector as scalp topography
%PLOT_SCALPEVOLUTION - Draws scalp topographies for specified intervals
%PLOT_SCALPEVOLUTIONPLUSCHANNEL - Display evolution of scalp topographies
%PLOT_SCALPPATTERNSPLUSCHANNEL - Display Classwise topographies and one channel
%PLOT_SCALPLOADING - Visualize single channel loadings of a weight vector
%PLOT_SCALPOUTLINE - Schematic sketch of the scalp
%PLOT_SCALPPATTERN - Display average trials as scalp topography
%
%PLOT_CSPANALYSIS - Show CSP filters and patterns as topographies
%
%IMAGE_LOCALCOLORMAP - Display image without using the figure's colormap
%
%PLOT_BARWITHANTENNA - Bar plot with antennas, e.g., to show the SEM


% Test file for singfun/plus.m
% This has been adapted from bndfun test for plus.
function pass = test_plus(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Generate a few random points to use as test values.
seedRNG(666);
x = 2 * rand(100, 1) - 1;

% A random number to use as an arbitrary additive constant.
alpha = -0.194758928283640 + 0.075474485412665i;

%%
% Check operation in the case of empty arguments.
f = singfun();
data.exponents = [-1, 0];
g = singfun(@(x) 1./(1+x), data, pref);
pass(1) = (isempty(f + f) && isempty(f + g) && isempty(g + f));

% Addition of smooth SINGFUNs should not return a SINGFUN
f = singfun(@(x) sin(x));
g = singfun(@(x) cos(x));
pass(2) = ~isa(f+g, 'singfun');

% SMOOTHFUN + SINGFUN
f = smoothfun.constructor(@(x) sin(x));
g = singfun(@(x) cos(x));
pass(3) = isa(f + g, 'smoothfun') && isa(g + f, 'smoothfun');
%%
% Check addition with scalars.

fh = @(x) 1./((1+x).*(1-x));
data.exponents = [-1, -1];
f = singfun(fh, data, pref);
pass(4:5) = test_add_function_to_scalar(f, fh, alpha, x);

%%
% Check addition of two singfun objects.

fh = @(x) zeros(size(x));
data.exponents = [];
data.singType = {'none', 'none'};
f = singfun(fh, data, pref);
pass(6:7) = test_add_function_to_function(f, fh, f, fh, x);

fh = @(x) sin(pi*x)./(1-x);
f = singfun(fh, [], pref);

gh = @(x) cos(pi*x)./(1-x);
g = singfun(gh, [], pref);
pass(8:9) = test_add_function_to_function(f, fh, g, gh, x);

gh = @(x) cos(1e2*x);
g = singfun(gh, [], pref);
pass(10:11) = test_add_function_to_function(f, fh, g, gh, x);

gh = @(t) sinh(t*exp(2*pi*1i/6));
g = singfun(gh, [], pref);
pass(12:13) = test_add_function_to_function(f, fh, g, gh, x);

%%
% Check that direct construction and PLUS give comparable results.

f = singfun(@(x) x, [], pref);
g = singfun(@(x) cos(x) - 1, [], pref);
h1 = f + g;
h2 = singfun(@(x) x + cos(x) - 1, [], pref);
tol = 10*max(get(h1, 'vscale')*eps, ...
    get(h2, 'vscale')*eps);
pass(14) = norm(feval(h1, x) - feval(h2, x), inf) < tol;

% Check that plus handles "small" results appropriately in the case of a
% non-integer exponent difference.
warnState = warning('off', 'CHEBFUN:SINGFUN:plus:exponentDiff');

pref_fixed = pref;
pref_fixed.fixedLength = 256;
op = @(x) ((x + 1)/2).^pi;
data.exponents = [pi - 3, 0];
f = singfun(op, data, pref);
g = singfun(op, [], pref_fixed);
h = f - g;
pass(15) = get(h, 'ishappy') && (length(h) < 1024);

warning(warnState);

end

% Test the addition of a SINGFUN F, specified by Fh, to a scalar C using
% a grid of points X in [a  b] for testing samples.
function result = test_add_function_to_scalar(f, fh, c, x)
    g1 = f + c;
    g2 = c + f;
    result(1) = isequal(g1, g2);
    g_exact = @(x) fh(x) + c;
    result(2) = norm(feval(g1, x) - g_exact(x), inf) < 1e3*eps;
end

% Test the addition of two SINGFUN objects F and G, specified by FH and
% GH, using a grid of points X in [-1  1] for testing samples.
function result = test_add_function_to_function(f, fh, g, gh, x)
    h1 = f + g;
    h2 = g + f;
    result(1) = isequal(h1, h2);
    h_exact = @(x) fh(x) + gh(x);
    result(2) = norm(feval(h1, x) - h_exact(x), inf) <= 2e3*eps;
end


% surrogdistrib - Build surrogate distribution
%
% surrog = surrogdistrib(data, varargin);
%
% Inputs:
%  data   - [cell] data arrays for which to compute a surrogate
%           distribution.
%
% Optional inputs:
%  'method'  - ['bootstrap'|'perm'] use either 'bootstrap' or 'permutation'
%              method. Bootstrap performs draws with replacement and
%              permutation performs draws without replacement. Default
%              is 'perm'.
%  'pairing' - ['on'|'off'] pair the data arrays.
%  'naccu'   - [integer] number of surrogate. Default is 1.
%  'precomp' - cell array containing precomputed value for speeding up
%              mulitple calls
%
% Output:
%  surrog  - surrogate distribution
%  precomp - cell array containing precomputed value for speeding up
%            mulitple calls
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 2005-

% Copyright (C) Arnaud Delorme
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [res precomp ] = surrogdistrib(data, varargin)

if nargin < 1
    help surrogdistrib,
    return;
end;

if ~strcmpi(varargin{1}, 'precomp')
    opt = finputcheck(varargin, { 'naccu'      'integer'   [1 Inf]             1;
                                  'method'     'string'    { 'perm','permutation','bootstrap' }  'perm';
                                  'pairing'    'string'    { 'on','off' }      'on';
                                  'precomp'    'cell'      {} {} }, 'surrogdistrib');
    if isstr(opt), error(opt); end;
    if strcmpi(opt.method, 'permutation'), opt.method = 'perm'; end;
    if strcmpi(opt.method, 'bootstrap'), bootflag = 1;
    else                                 bootflag = 0;
    end;
    if strcmpi(opt.pairing, 'on')
         pairflag = 1;
    else pairflag = 0;
    end;
else
    opt.precomp = varargin{2};
end;

% concatenate data
% ----------------
if isempty(opt.precomp)
    [ datavals datalen datadims ] = concatdata( data );
    precomp = { datavals datalen datadims bootflag pairflag opt.naccu};
else
    precomp   = opt.precomp;
    datavals  = precomp{1};
    datalen   = precomp{2};
    datadims  = precomp{3};
    bootflag  = precomp{4};
    pairflag  = precomp{5};
    opt.naccu = precomp{6};
end;

% compute surrogate distribution
% ------------------------------
if opt.naccu > 1
    res = supersurrogate( datavals, datalen, datadims, bootflag, pairflag, opt.naccu);
else
    res = surrogate( datavals, datalen, datadims, bootflag, pairflag);
end;

function res = supersurrogate(dat, lens, dims, bootstrapflag, pairedflag, naccu); % for increased speed only shuffle half the indices

    % recompute indices in set and target cell indices
    % ------------------------------------------------
    ncond = length(lens)-1;
    nsubj = lens(2);
    if bootstrapflag
        if pairedflag
             indswap  = mod( repmat([1:lens(end)],[naccu 1]) + ceil(rand(naccu,lens(end))*length(lens))*lens(2)-1, lens(end) )+1;
        else indswap  = ceil(rand(naccu,lens(end))*lens(end));
        end;
    else
        if pairedflag
            [tmp idx] = sort(rand(naccu,nsubj,ncond),3);
            indswap   = ((idx)-1)*nsubj + repmat( repmat([1:nsubj], [naccu 1 1]),[1 1 ncond]);
            indswap   = reshape(indswap, [naccu lens(end)]);
        else
            [tmp indswap] = sort(rand(naccu, lens(end)),2);
        end;
    end;

    for i = 1:length(lens)-1
        if myndims(dat) == 1
             res{i} = reshape(dat(indswap(:,lens(i)+1:lens(i+1))), naccu, lens(i+1)-lens(i));
        else res{i} = reshape(dat(:,indswap(:,lens(i)+1:lens(i+1))), size(dat,1), naccu, lens(i+1)-lens(i));
        end;
    end;
    res = reshape(res, dims);

function res = surrogate(dataconcat, lens, dims, bootstrapflag, pairedflag); % for increased speed only shuffle half the indices

    % recompute indices in set and target cell indices
    % ------------------------------------------------
    if bootstrapflag
        if pairedflag
             indswap  = mod( [1:lens(end)]+ ceil(rand(1,lens(end))*length(lens))*lens(2)-1, lens(end) )+1;
        else indswap  = ceil(rand(1,lens(end))*lens(end));
        end;
    else
        if pairedflag
            indswap  = [1:lens(end)];
            indswap  = reshape(indswap, [lens(2) length(lens)-1]);
            for i = 1:size(indswap,1) % shuffle each row
                [tmp idx] = sort(rand(1,size(indswap,2)));
                indswap(i,:) = indswap(i,idx);
            end;
            indswap  = reshape(indswap, [1 lens(2)*(length(lens)-1)]);
        else
            oriindices = [1:lens(end)]; % just shuffle indices
            [tmp idx] = sort(rand(1,length(oriindices)));
            indswap   = oriindices(idx);
        end;
    end;

    res = {};
    for i = 1:length(lens)-1
        if myndims(dataconcat) == 1
             res{i} = dataconcat(indswap(lens(i)+1:lens(i+1)));
        else res{i} = dataconcat(:,indswap(lens(i)+1:lens(i+1)));
        end;
    end;
    res = reshape(res, dims);

function val = myndims(a)
    if ndims(a) > 2
        val = ndims(a);
    else
        if size(a,1) == 1,
            val = 2;
        elseif size(a,2) == 1,
            val = 1;
        else
            val = 2;
        end;
    end;


function MOVC = PQ_InitMOVC (Nchan, Np)
MOVC.MDiff.Mt1B = zeros (Nchan, Np);
MOVC.MDiff.Mt2B = zeros (Nchan, Np);
MOVC.MDiff.Wt   = zeros (Nchan, Np);

MOVC.NLoud.NL   = zeros (Nchan, Np);

MOVC.Loud.NRef  = zeros (Nchan, Np);
MOVC.Loud.NTest = zeros (Nchan, Np);

MOVC.BW.BWRef  = zeros (Nchan, Np);
MOVC.BW.BWTest = zeros (Nchan, Np);

MOVC.NMR.NMRavg = zeros (Nchan, Np);
MOVC.NMR.NMRmax = zeros (Nchan, Np);

MOVC.PD.Pc = zeros (1, Np);
MOVC.PD.Qc = zeros (1, Np);

MOVC.EHS.EHS = zeros (Nchan, Np);


function  generateProposalsForImages()
%boxes have this format: [xmin ymin xmax ymax]
addpath(genpath('../'));
addpath(genpath('../../../utils/'));
config=createConfig();

imageLoc=config.path.input;
outputLoc=config.path.output;
%extension inclues .
imageExt=config.opts.imageExt;
images=dir([imageLoc '*' imageExt]);

for i=1:length(images)
	imName=images(i).name;
	image=imread([imageLoc imName]);
	[ranked_regions superpixels image_data]=generate_proposals(image);
	boxes=zeros(length(ranked_regions),4);
	for j=1:length(ranked_regions)
		boxes(j,:)=mask2box(ismember(superpixels, ranked_regions{j}));
	end

	if(isfield(config.opts,'numProposals'))
        numProposals=config.opts.numProposals;

        if(size(boxes,1)>=numProposals)
                boxes=boxes(1:numProposals,:);
                ranked_regions=ranked_regions(1:numProposals);
        else
                fprintf('Only %d proposals were generated for image:%s\n',size(boxes,1),imName);
        end
end
	%mask2box outputs  [ymin xmin ymax xmax]
	boxes=[boxes(:,2) boxes(:,1) boxes(:,4) boxes(:,3)];
	proposals.boxes=boxes;
	proposals.scores = [size(boxes,1):-1:1]`;
	proposals.regions.ranked_regions=ranked_regions;
	proposals.regions.superpixels=superpixels;
	proposals.regions.image_data=image_data;

	proposalFileName=strrep(imName,imageExt,'.mat');
	save([outputLoc proposalFileName],'proposals');
end


function [nodes, error, C] = removeUnconnected(nodes,error,C)
% This function removes unconnected (dead) nodes from the DCS structure.

NumOfNodes = size(nodes,2);

i = 1;
while i<=NumOfNodes
          if (any(C(i,:))==0)

              C = [C(1:i-1,:); C(i+1:NumOfNodes,:);];
              C = [C(:,1:i-1)  C(:,i+1:NumOfNodes);];

              nodes = [nodes(:,1:i-1) nodes(:,i+1:NumOfNodes);];
                 error = [error(1,1:i-1) error(1,i+1:NumOfNodes);];

               NumOfNodes = NumOfNodes - 1;
               i = i-1;
          end
           i = i+1;
 end


function [Nbegcol,Nlencol,Nrowndx] = lindosparse(cJacobian);

Nbegcol = [];
Nrowndx = [];
Nlencol = [];
top = 0;
for i = 1:size(cJacobian,2)
    [ii,jj,kk] = find(cJacobian(:,i));
    if isempty(ii)
        Nbegcol = [Nbegcol top];
        Nlencol = [Nlencol 0];
    else
        Nbegcol = [Nbegcol top];
        Nrowndx = [Nrowndx ii(:)'-1];
        Nlencol = [Nlencol length(ii)];
        top = top + length(ii);
    end
end
if  isempty(Nrowndx)
    Nrowndx = [];
end
Nbegcol = [Nbegcol sum(Nlencol)];


function box = mergeBoxes(box1, box2)
%MERGEBOXES Merge two boxes, by computing their greatest extent
%
%   BOX = mergeBoxes(BOX1, BOX2);
%
%   Example
%   box1 = [5 20 5 30];
%   box2 = [0 15 0 15];
%   mergeBoxes(box1, box2)
%   ans =
%       0 20 0 30
%
%
%   See also
%   boxes2d, drawBox, intersectBoxes
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% unify sizes of data
if size(box1,1) == 1
    box1 = repmat(box1, size(box2,1), 1);
elseif size(box2, 1) == 1
    box2 = repmat(box2, size(box1,1), 1);
elseif size(box1,1) ~= size(box2,1)
    error('Bad size for inputs');
end

% compute extreme coords
mini = min(box1(:,[1 3]), box2(:,[1 3]));
maxi = max(box1(:,[2 4]), box2(:,[2 4]));

% concatenate result into a new box structure
box = [mini(:,1) maxi(:,1) mini(:,2) maxi(:,2)];


function out = FieldMap_applyvdm(job)
% Apply VDM and reslice images
% FORMAT FieldMap_applyvdm(job)
% job.data(sessnum).scans   - images for session/run sessnum
% job.data(sessnum).vdmfile - VDM file for session/run sessnum
% job.roptions.rinterp - interpolation method
% job.roptions.wrap    - perform warp around in specified dimensions
% job.roptions.mask    - perform masking
% job.roptions.which(1) - reslice images in time series only
% job.roptions.which(2) - reslice images in time series and mean
% job.roptions.prefix   - prefix for vdm applied files
% job.roptions.pedir    - phase encode direction (i.e. aplly vdm file along
% this dimension
%__________________________________________________________________________
%
% A VDM (voxel displacement map) created using the FieldMap toolbox
% can be used to resample and reslice realigned images to the original
% subdirectory with the same (prefixed) filename.
%
% Voxels in the images will be shifted according to the values in the VDM
% file along the direction specified by job.roptions.pedir (i.e. this is
% usually the phase encode direction) and resliced to the space of the
% first image in the time series.
%
% Inputs:
% A job structure containing fields for the input data and the processing
% options. The input data contains the series of images conforming to
% SPM data format (see 'Data Format'), the relative displacement of the images
% is stored in their header and a VDM which has (probably) been created
% using the FieldMap toolbox and matched to the first image in the time
% series (this can also be done via the FieldMap toolbox).
%
% Outputs:
% The resampled and resliced images resliced to the same subdirectory with a prefix.
%__________________________________________________________________________
% Copyright (C) 2011-2014 Wellcome Trust Centre for Neuroimaging

% Chloe Hutton
% $Id: FieldMap_applyvdm.m 6258 2014-11-07 18:15:40Z guillaume $

tiny = 5e-2;

% assemble roptions
%--------------------------------------------------------------------------
flags.interp    = job.roptions.rinterp;
flags.wrap      = job.roptions.wrap;
flags.mask      = job.roptions.mask;
flags.which     = job.roptions.which(1);
flags.mean      = job.roptions.which(2);
flags.prefix    = job.roptions.prefix;
flags.pedir     = job.roptions.pedir;
hold = [repmat(flags.interp,1,3) flags.wrap];

% Determine dimension along which to apply vdm
applydim=flags.pedir;
if applydim~=1 &&  applydim~=2 && applydim~=3
    applydim=2;
end

% Gather up data into ds structure which holds images and vdm file
%--------------------------------------------------------------------------
P = {};
for i = 1:numel(job.data)
    P{i} = strvcat(job.data(i).scans{:});
    ds(i).P=spm_vol(P{i});
    if ~isempty(job.data(i).vdmfile)
        sfP{i} = job.data(i).vdmfile{1};
        ds(i).sfP=spm_vol(sfP{i});
    else
        sfP{i} = [];
    end
    ds(i).hold = [1 1 1 0 1 0];
end

ntot = 0;
for i=1:length(ds)
   ntot = ntot + length(ds(i).P);
end

% Set up  x y z for resampling
%--------------------------------------------------------------------------
[x,y,z] = ndgrid(1:ds(1).P(1).dim(1),1:ds(1).P(1).dim(2),1:ds(1).P(1).dim(3));
xyz = [x(:) y(:) z(:) ones(prod(ds(1).P(1).dim(1:3)),1)]; clear x y z;

% Create mask if required (usually default and required to create mean)
%--------------------------------------------------------------------------
if flags.mask || flags.mean,
    spm_progress_bar('Init',ntot,'Computing available voxels',...
        'volumes completed');

    if flags.mean
        Count    = zeros(prod(ds(1).P(1).dim(1:3)),1);
        Integral = zeros(prod(ds(1).P(1).dim(1:3)),1);
    end

    % if flags.mask
    msk = zeros(prod(ds(1).P(1).dim(1:3)),1);
    % end

    % To create mean, read each session specific vdmfile in
    % to the space of the first image of first session
    tv = 1;
    for s=1:length(ds)
        T = ds(s).sfP.mat\ds(1).P(1).mat;
        txyz = xyz * T';
        c = spm_bsplinc(ds(s).sfP,ds(1).hold);
        ds(s).sfield = spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),ds(1).hold);
        ds(s).sfield = ds(s).sfield(:);
        clear c txyz;
        sess_msk = zeros(prod(ds(s).P(1).dim(1:3)),1);
        % Read in each images in space of first image of first session
        for i = 1:numel(ds(s).P)
            T = inv(ds(s).P(i).mat) * ds(1).P(1).mat;
            txyz = xyz * T';
            txyz(:,applydim) = txyz(:,applydim) + ds(s).sfield;
            tmp       = false(size(txyz,1),1);
            if ~flags.wrap(1), tmp = tmp | txyz(:,1) < (1-tiny) | txyz(:,1) > (ds(s).P(i).dim(1)+tiny); end
            if ~flags.wrap(2), tmp = tmp | txyz(:,2) < (1-tiny) | txyz(:,2) > (ds(s).P(i).dim(2)+tiny); end
            if ~flags.wrap(3), tmp = tmp | txyz(:,3) < (1-tiny) | txyz(:,3) > (ds(s).P(i).dim(3)+tiny); end
            sess_msk = sess_msk + real(tmp);
            spm_progress_bar('Set',tv);
            tv = tv+1;
        end
        msk = msk + sess_msk;
        if flags.mean, Count = Count + repmat(length(ds(s).P),prod(ds(s).P(1).dim(1:3)),1) - sess_msk; end

        %
        % Include static field in estmation of mask.
        %
        if isfield(ds(s),'sfP') && ~isempty(ds(s).sfP)
            T = inv(ds(1).sfP.mat) * ds(1).P(1).mat;
            txyz = xyz * T';
            tmp  = false(size(txyz,1),1);
            if ~flags.wrap(1), tmp = tmp | txyz(:,1) < (1-tiny) | txyz(:,1) > (ds(1).sfP.dim(1)+tiny); end
            if ~flags.wrap(2), tmp = tmp | txyz(:,2) < (1-tiny) | txyz(:,2) > (ds(1).sfP.dim(2)+tiny); end
            if ~flags.wrap(3), tmp = tmp | txyz(:,3) < (1-tiny) | txyz(:,3) > (ds(1).sfP.dim(3)+tiny); end
            msk = msk + real(tmp);
        end

        if isfield(ds(1),'sfield') && ~isempty(ds(1).sfield)
            ds(1).sfield = [];
        end
    end
    if flags.mask, msk = find(msk ~= 0); end
end

% Apply fieldmap to all files, looping through sessions
%--------------------------------------------------------------------------
spm_progress_bar('Init',ntot,'Reslicing','volumes completed');
tv = 1;
for s = 1:numel(ds)

    % Get transformation between distortion field and first image
    T = ds(s).sfP.mat\ds(1).P(1).mat;
    txyz = xyz * T';
    c = spm_bsplinc(ds(s).sfP,ds(1).hold);
    ds(s).sfield = spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),ds(1).hold);
    ds(s).sfield = ds(s).sfield(:);

    % Read in each images in space of first image of first session
    for i = 1:numel(ds(s).P)
        T = inv(ds(s).P(i).mat) * ds(1).P(1).mat;
        txyz = xyz * T';
        txyz(:,applydim) = txyz(:,applydim) + ds(s).sfield;
        c = spm_bsplinc(ds(s).P(i),hold);
        ima = spm_bsplins(c,txyz(:,1),txyz(:,2),txyz(:,3),hold);
        %
        % Write out resliced images
        %
        if flags.which
            PO         = ds(s).P(i);
            [pth,nm,xt,vr] = spm_fileparts(deblank(PO.fname));
            PO.fname       = fullfile(pth,[flags.prefix nm xt vr]);
            PO.mat     = ds(1).P(1).mat;
            PO.descrip = sprintf('spm - applied vdm');
            ivol       = ima;
            if flags.mask
                ivol(msk) = NaN;
            end
            ivol = reshape(ivol,PO.dim(1:3));
            PO   = spm_create_vol(PO);
            spm_write_vol(PO,ivol);
            if nargout > 0
                  out.sess(s).rfiles{i} = PO.fname;
            end
        end
        %
        % Build up mean image if so required.
        %
        if flags.mean
            Integral = Integral + nan2zero(ima);
        end
        spm_progress_bar('Set',tv);
        tv = tv+1;
    end
    if isfield(ds(s),'sfield') && ~isempty(ds(s).sfield)
        ds(s).sfield = [];
    end
end

% Write mean image
%--------------------------------------------------------------------------
if flags.mean
   % Write integral image (16 bit signed)
   %-----------------------------------------------------------------------
   sw = warning('off','MATLAB:divideByZero');
   Integral   = Integral./Count;
   warning(sw);
   PO         = ds(1).P(1);
   [pth,nm,xt,vr] = spm_fileparts(deblank(ds(1).P(1).fname));
   PO.fname       = fullfile(pth,['mean' flags.prefix nm xt vr]);
   PO.pinfo   = [max(max(max(Integral)))/32767 0 0]';
   PO.descrip = 'spm - mean applied vdm image';
   PO.dt      = [spm_type('int16') spm_platform('bigend')];
   ivol = reshape(Integral,PO.dim);
   spm_write_vol(PO,ivol);
end

if nargout > 0
   out.rmean{1} = PO.fname;
end

spm_figure('Clear','Interactive');

%==========================================================================
function vo = nan2zero(vi)
vo = vi;
vo(~isfinite(vo)) = 0;
return;


function elementalWeightMatrix = getElementalWeightMatrix(isotopeAbundance)
% Get the symbols for all chemical elements and their atomic weights
%
% USAGE:
%    elementalWeightMatrix = getElementalWeightMatrix(isotopeAbundance)
%
% OPTIONAL INPUTS:
%    isotopeAbundance:         * false to return standard atomic weights (default)
%                              * true to return the weights of naturally predominant isotopes for biological
%                                elements and standard weights for other elements.
%                              * `m` x 3 cell arrray with user defined isotope abundance:
%                                isotopeAbundance{i, 1} = 'Atomic_Symbol';
%                                isotopeAbundance{i, 2} = Mass_Number;
%                                isotopeAbundance{i, 3} = abundance;
%                                (where sum of abundances of all isotopes of an element must be one)
%    printWarning:             true to print warning if there are unknown elements in isotopeAbundance
%
% OUTPUT:
%    elementalWeightMatrix:    #elements x 2 cell array, the 1st coloumn being the symbols
%                              of chemial elements and the 2nd their standard atomic weights

if nargin == 0 || isempty(isotopeAbundance)
    isotopeAbundance = false;
end

atomicWeights = parse_Atomic_Weights_and_Isotopic_Compositions_for_All_Elements;
[symbols, ia] = unique([atomicWeights.data.AtomicSymbol]);
% remove XYZ (non-existing element) from the symbol list
elementRemove = strcmp(symbols, 'D') | strcmp(symbols, 'T') | strncmp(symbols, 'XYZ', 3);
[symbols, ia] = deal(columnVector(symbols(~elementRemove)), ia(~elementRemove));
% identify elements without standard weight
elementWoMW = arrayfun(@(x) isempty(x.StandardAtomicWeight), atomicWeights.data(ia));
% elements with standard weight come first
symbols = [symbols(~elementWoMW); symbols(elementWoMW)];
elementWeights = [[atomicWeights.data(ia(~elementWoMW)).StandardAtomicWeight], NaN(1, sum(elementWoMW))]';

if ~isscalar(isotopeAbundance) || isotopeAbundance

    % biological elements as defined in getMolecularMass.m
    %allBiologicalElements = {'C', 'O', 'P', 'N', 'S', 'H', 'Mg', 'Na', 'K', 'Cl', 'Ca', 'Zn', 'Fe', 'Cu', 'Mo', 'I'};
    if isscalar(isotopeAbundance)
        % use the defaulted most naturally predominant isotope of each biological element
        isotopeAbundance = {...
            'C', 12, 1; ...  % Carbon, all as C12
            'H', 1, 1; ...  % Hydrogen, all as H1
            'O', 16, 1; ...  % Oxygen, all as O16
            'N', 14, 1; ...  % Nitrogen, all as N14
            'S', 32, 1; ...  % Sulphur, all as S32 (94% naturally S32)
            'Mg', 24, 1; ...  % Magnesium, all as Mg24, (79 % naturally Mg24)
            'K', 39, 1; ...  % Potassium, all as K39, (93% naturally K39)
            'Cl', 35, 1; ...  % Chlorine, all as Cl35 (75.78% naturally Cl35)
            'Ca', 40, 1; ...  % Calcium, all as Ca40 (96% naturally Ca40)
            'Zn', 64, 1; ...  % Zinc, all as Zn64 (48% naturally Zn64)
            'Cu', 63, 1; ...  % Copper, all as Cu63 (69% naturally Cu63)
            'Mo', 98, 1; ...  % Molybdenum, all as Mo98 (24% naturally Mo98)
            'P', 31, 1; ...  % Phosphorus, all as P31
            'Na', 23, 1; ...  % Sodium, all as Na23
            'Fe', 56, 1; ...  % Iron, all as Fe56
            'I', 127, 1; ...  % Iodine, all as I127
            };
    end

    % get the list of all isotopes
    [isotopes, isoMassNums, isoWeights] = deal([atomicWeights.data.AtomicSymbol], ...
        [atomicWeights.data.MassNumber], [atomicWeights.data.RelativeAtomicMass]);
    % treat 'D' and 'T' as the same element as 'H'
    isotopes(strcmp(isotopes, 'D') | strcmp(isotopes, 'T')) = {'H'};
    isotopeAbundance(strcmp(isotopeAbundance(:, 1), 'D') | ...
        strcmp(isotopeAbundance(:, 1), 'T')) = {'H'};

    [eleWtIsoAb, ~, ib] = unique(isotopeAbundance(:, 1));
    [ynE, idE] = ismember(eleWtIsoAb, symbols);

    % sanity check
    if size(isotopeAbundance, 2) ~= 3
        error('Incorrect size. IsotopeAbundance must be a n-by-3 cell array.')
    elseif ~all(ynE)
        % unknown elements in isotope abundance profile
        error('%s in the supplied isotope abundance profile is/are not standard chemical elements.', strjoin(eleWtIsoAb(~ynE), ', '))
    end

    massNumbers = cell2mat(isotopeAbundance(:, 2));
    abundances = cell2mat(isotopeAbundance(:, 3));
    if any(massNumbers <= 0)  % positive masses
        error('Non-positive isotope mass in row(s) %s.', strjoin(cellstr(num2str(find(massNumbers <= 0))), ', '))
    elseif any(abundances < 0 | abundances > 1)  % 0 <= abundnace <= 1
        error('Invalid isotope abundance in row(s) %s. Must lie between 0 and 1.', ...
            strjoin(cellstr(num2str(find(abundances < 0 | abundances > 1))), ', '))
    end

    % get the order of the supplied isotopeAbundance in the atomic weight data
    eleWtIsoAbIDs = zeros(size(isotopeAbundance, 1), 1);
    for j = 1:size(isotopeAbundance, 1)
        f = strcmp(isotopeAbundance{j, 1}, isotopes(:)) & isoMassNums(:) == isotopeAbundance{j, 2};
        if sum(f) == 1
            eleWtIsoAbIDs(j) = find(f);
        end
    end
    % error if the isotopes cannot be matched
    if ~all(eleWtIsoAbIDs)
        notMatched = find(eleWtIsoAbIDs == 0);
        errMsg = '';
        for j = 1:numel(notMatched)
            errMsg = [errMsg, ['\n#' num2str(notMatched(j)) '\t' ...
                isotopeAbundance{notMatched(j), 1} '\t' num2str(isotopeAbundance{notMatched(j), 2})]];
        end
        error(['%s', errMsg], 'The following isotopes could not be identified:')
    end

    % get the corresponding relative atomic mass
    relativeMasses = columnVector(isoWeights(eleWtIsoAbIDs));

    % get the user-defined atomic masses
    sumIsoEq1 = false(numel(eleWtIsoAb), 1);
    averageWeight = zeros(numel(eleWtIsoAb), 1);
    for j = 1:numel(eleWtIsoAb)
        sumIsoEq1(j) = abs(sum(abundances(ib == j)) - 1) < 1e-7;
        averageWeight(j) = relativeMasses(ib == j)' * abundances(ib == j);
    end
    if any(~sumIsoEq1)  % sum(abundance) = 1
        error('Invalid isotope abundances for element(s) %s. The sum of isotope abundances for each element must be equal to 1.', ...
            strjoin(eleWtIsoAb(~sumIsoEq1), ', '))
    end

    % update the list of weights
    elementWeights(idE) = averageWeight;
end

elementalWeightMatrix = [symbols, num2cell(elementWeights)];


function [baseMVA, bus, gen, branch, areas, gencost] = c30
% CASE_IEEE30    Power flow data for IEEE 30 bus test case.
%   Please see 'help caseformat' for details on the case file format.
%   This data was converted from IEEE Common Data Format
%   (ieee30cdf.txt) on 20-Sep-2004 by cdf2matp, rev. 1.11
%   See end of file for warnings generated during conversion.
%
%   Converted from IEEE CDF file from:
%       http://www.ee.washington.edu/research/pstca/
%
%  08/20/93 UW ARCHIVE           100.0  1961 W IEEE 30 Bus Test Case

%   MATPOWER
%   $Id: case_ieee30.m,v 1.2 2004/09/21 01:45:05 ray Exp $

%%-----  Power Flow Data  -----%%
%% system MVA base
baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
bus = [
	1	3	0	0	0	0	1	1.06	0	132	1	1.06	0.94;
	2	2	21.7	12.7	0	0	1	1.043	-5.48	132	1	1.06	0.94;
	3	1	2.4	1.2	0	0	1	1.021	-7.96	132	1	1.06	0.94;
	4	1	7.6	1.6	0	0	1	1.012	-9.62	132	1	1.06	0.94;
	5	2	94.2	19	0	0	1	1.01	-14.37	132	1	1.06	0.94;
	6	1	0	0	0	0	1	1.01	-11.34	132	1	1.06	0.94;
	7	1	22.8	10.9	0	0	1	1.002	-13.12	132	1	1.06	0.94;
	8	2	30	30	0	0	1	1.01	-12.1	132	1	1.06	0.94;
	9	1	0	0	0	0	1	1.051	-14.38	1	1	1.06	0.94;
	10	1	5.8	2	0	19	1	1.045	-15.97	33	1	1.06	0.94;
	11	2	0	0	0	0	1	1.082	-14.39	11	1	1.06	0.94;
	12	1	11.2	7.5	0	0	1	1.057	-15.24	33	1	1.06	0.94;
	13	2	0	0	0	0	1	1.071	-15.24	11	1	1.06	0.94;
	14	1	6.2	1.6	0	0	1	1.042	-16.13	33	1	1.06	0.94;
	15	1	8.2	2.5	0	0	1	1.038	-16.22	33	1	1.06	0.94;
	16	1	3.5	1.8	0	0	1	1.045	-15.83	33	1	1.06	0.94;
	17	1	9	5.8	0	0	1	1.04	-16.14	33	1	1.06	0.94;
	18	1	3.2	0.9	0	0	1	1.028	-16.82	33	1	1.06	0.94;
	19	1	9.5	3.4	0	0	1	1.026	-17	33	1	1.06	0.94;
	20	1	2.2	0.7	0	0	1	1.03	-16.8	33	1	1.06	0.94;
	21	1	17.5	11.2	0	0	1	1.033	-16.42	33	1	1.06	0.94;
	22	1	0	0	0	0	1	1.033	-16.41	33	1	1.06	0.94;
	23	1	3.2	1.6	0	0	1	1.027	-16.61	33	1	1.06	0.94;
	24	1	8.7	6.7	0	4.3	1	1.021	-16.78	33	1	1.06	0.94;
	25	1	0	0	0	0	1	1.017	-16.35	33	1	1.06	0.94;
	26	1	3.5	2.3	0	0	1	1	-16.77	33	1	1.06	0.94;
	27	1	0	0	0	0	1	1.023	-15.82	33	1	1.06	0.94;
	28	1	0	0	0	0	1	1.007	-11.97	132	1	1.06	0.94;
	29	1	2.4	0.9	0	0	1	1.003	-17.06	33	1	1.06	0.94;
	30	1	10.6	1.9	0	0	1	0.992	-17.94	33	1	1.06	0.94;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin
gen = [
	1	260.2	-16.1	10	0	1.06	100	1	360.2	0;
	2	40	50	50	-40	1.045	100	1	140	0;
	5	0	37	40	-40	1.01	100	1	100	0;
	8	0	37.3	40	-10	1.01	100	1	100	0;
	11	0	16.2	24	-6	1.082	100	1	100	0;
	13	0	10.6	24	-6	1.071	100	1	100	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status
branch = [
	1	2	0.0192	0.0575	0.0528	9900	0	0	0	0	1;
	1	3	0.0452	0.1652	0.0408	9900	0	0	0	0	1;
	2	4	0.057	0.1737	0.0368	9900	0	0	0	0	1;
	3	4	0.0132	0.0379	0.0084	9900	0	0	0	0	1;
	2	5	0.0472	0.1983	0.0418	9900	0	0	0	0	1;
	2	6	0.0581	0.1763	0.0374	9900	0	0	0	0	1;
	4	6	0.0119	0.0414	0.009	9900	0	0	0	0	1;
	5	7	0.046	0.116	0.0204	9900	0	0	0	0	1;
	6	7	0.0267	0.082	0.017	9900	0	0	0	0	1;
	6	8	0.012	0.042	0.009	9900	0	0	0	0	1;
	6	9	0	0.208	0	9900	0	0	0.978	0	1;
	6	10	0	0.556	0	9900	0	0	0.969	0	1;
	9	11	0	0.208	0	9900	0	0	0	0	1;
	9	10	0	0.11	0	9900	0	0	0	0	1;
	4	12	0	0.256	0	9900	0	0	0.932	0	1;
	12	13	0	0.14	0	9900	0	0	0	0	1;
	12	14	0.1231	0.2559	0	9900	0	0	0	0	1;
	12	15	0.0662	0.1304	0	9900	0	0	0	0	1;
	12	16	0.0945	0.1987	0	9900	0	0	0	0	1;
	14	15	0.221	0.1997	0	9900	0	0	0	0	1;
	16	17	0.0524	0.1923	0	9900	0	0	0	0	1;
	15	18	0.1073	0.2185	0	9900	0	0	0	0	1;
	18	19	0.0639	0.1292	0	9900	0	0	0	0	1;
	19	20	0.034	0.068	0	9900	0	0	0	0	1;
	10	20	0.0936	0.209	0	9900	0	0	0	0	1;
	10	17	0.0324	0.0845	0	9900	0	0	0	0	1;
	10	21	0.0348	0.0749	0	9900	0	0	0	0	1;
	10	22	0.0727	0.1499	0	9900	0	0	0	0	1;
	21	22	0.0116	0.0236	0	9900	0	0	0	0	1;
	15	23	0.1	0.202	0	9900	0	0	0	0	1;
	22	24	0.115	0.179	0	9900	0	0	0	0	1;
	23	24	0.132	0.27	0	9900	0	0	0	0	1;
	24	25	0.1885	0.3292	0	9900	0	0	0	0	1;
	25	26	0.2544	0.38	0	9900	0	0	0	0	1;
	25	27	0.1093	0.2087	0	9900	0	0	0	0	1;
	28	27	0	0.396	0	9900	0	0	0.968	0	1;
	27	29	0.2198	0.4153	0	9900	0	0	0	0	1;
	27	30	0.3202	0.6027	0	9900	0	0	0	0	1;
	29	30	0.2399	0.4533	0	9900	0	0	0	0	1;
	8	28	0.0636	0.2	0.0428	9900	0	0	0	0	1;
	6	28	0.0169	0.0599	0.013	9900	0	0	0	0	1;
];
mm=length(branch(:,1));
for i=1:mm
    if branch(i,9)==0;
        branch(i,9)=1;
    else
    end
end
%%-----  OPF Data  -----%%
%% area data
areas = [
	1	1;
];

%% generator cost data
%	1	startup	shutdown	n	x0	y0	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
gencost = [
	2	0	0	3	.5*0.02	2	0;
	2	0	0	3	.5*0.0175	1.75	0;
	2	0	0	3	.5*0.0625	1	0;
	2	0	0	3	.5*0.00834	3.25	0;
	2	0	0	3	.5*0.025	3	0;
	2	0	0	3	.5*0.025	3	0;
];

return;

% Warnings from cdf2matp conversion:
%
% ***** Qmax = Qmin at generator at bus    1 (Qmax set to Qmin + 10)
% ***** area data conversion not yet implemented (creating dummy area data)
% ***** MVA limit of branch 1 - 2 not given, set to 9900
% ***** MVA limit of branch 1 - 3 not given, set to 9900
% ***** MVA limit of branch 2 - 4 not given, set to 9900
% ***** MVA limit of branch 3 - 4 not given, set to 9900
% ***** MVA limit of branch 2 - 5 not given, set to 9900
% ***** MVA limit of branch 2 - 6 not given, set to 9900
% ***** MVA limit of branch 4 - 6 not given, set to 9900
% ***** MVA limit of branch 5 - 7 not given, set to 9900
% ***** MVA limit of branch 6 - 7 not given, set to 9900
% ***** MVA limit of branch 6 - 8 not given, set to 9900
% ***** MVA limit of branch 6 - 9 not given, set to 9900
% ***** MVA limit of branch 6 - 10 not given, set to 9900
% ***** MVA limit of branch 9 - 11 not given, set to 9900
% ***** MVA limit of branch 9 - 10 not given, set to 9900
% ***** MVA limit of branch 4 - 12 not given, set to 9900
% ***** MVA limit of branch 12 - 13 not given, set to 9900
% ***** MVA limit of branch 12 - 14 not given, set to 9900
% ***** MVA limit of branch 12 - 15 not given, set to 9900
% ***** MVA limit of branch 12 - 16 not given, set to 9900
% ***** MVA limit of branch 14 - 15 not given, set to 9900
% ***** MVA limit of branch 16 - 17 not given, set to 9900
% ***** MVA limit of branch 15 - 18 not given, set to 9900
% ***** MVA limit of branch 18 - 19 not given, set to 9900
% ***** MVA limit of branch 19 - 20 not given, set to 9900
% ***** MVA limit of branch 10 - 20 not given, set to 9900
% ***** MVA limit of branch 10 - 17 not given, set to 9900
% ***** MVA limit of branch 10 - 21 not given, set to 9900
% ***** MVA limit of branch 10 - 22 not given, set to 9900
% ***** MVA limit of branch 21 - 22 not given, set to 9900
% ***** MVA limit of branch 15 - 23 not given, set to 9900
% ***** MVA limit of branch 22 - 24 not given, set to 9900
% ***** MVA limit of branch 23 - 24 not given, set to 9900
% ***** MVA limit of branch 24 - 25 not given, set to 9900
% ***** MVA limit of branch 25 - 26 not given, set to 9900
% ***** MVA limit of branch 25 - 27 not given, set to 9900
% ***** MVA limit of branch 28 - 27 not given, set to 9900
% ***** MVA limit of branch 27 - 29 not given, set to 9900
% ***** MVA limit of branch 27 - 30 not given, set to 9900
% ***** MVA limit of branch 29 - 30 not given, set to 9900
% ***** MVA limit of branch 8 - 28 not given, set to 9900
% ***** MVA limit of branch 6 - 28 not given, set to 9900


function varargout=sort(varargin)
%SORT (overloaded)
%
% [t,loc] = sort(x)
%
% The variable t will be the sorted version of x.
%
% SORT is implemented in the nonlinear operator framework using a big-M
% model.

switch class(varargin{1})

    case 'double'
        error('Overloaded SDPVAR/SORT CALLED WITH DOUBLE. Report error')

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.

        if nargin > 1 | min(size(varargin{1}))>1
            error('SDPVAR/SORT only supports simple 1-D sorting'),
        end

        x = varargin{1};
        data.D = binvar(length(x),length(x),'full');
        data.V = sdpvar(length(x),length(x),'full');
        y = [];

        for i = 1:length(x)
            data.i = i;
            data.isthisloc = 0;
            y = [y;yalmip('addextendedvariable',mfilename,x,data)];%i,P,V)];
        end
        loc = [];
        for i = 1:length(x)
            data.i = i;
            data.isthisloc = 1;
            loc = [loc;yalmip('addextendedvariable',mfilename,x,data)];
        end
        varargout{1} = y;
        varargout{2} = loc;

    case 'char' % YALMIP send 'graph' when it wants the epigraph or hypograph
        switch varargin{1}
            case {'milp','graph'}
                % Description using epigraphs
                t = varargin{2};
                X = varargin{3};
                data = varargin{4};
                %i = varargin{4};
                %D = varargin{5};
                %V = varargin{6};

                % Call external to allow subsrefs in classs
                [F,vars] = sort_internal(t,X,data);%i,D,V);

                varargout{1} = F;
                varargout{2} = struct('convexity','milp','monotonicity','milp','definiteness','milp');
                varargout{3} = X;

                % Currently a hack, gneral feature coming soon...
                varargout{2}.models = vars;

            otherwise
                error('SDPVAR/SORT called with CHAR argument?');
        end
    otherwise
        error('Strange type on first argument in SDPVAR/SORT');
end


%% import_STL_txt
% Below is a demonstration of the features of the |import_STL_txt| function

%% Syntax
% |[stlStruct] = import_STL_txt(fileName);|

%% Description
% Use |import_STL_txt| to import .txt type (as apposed to binary) STL
% files. The function supports multi-solid STL.

%% Examples

clear; close all; clc;

%%
% Plot settings
faceAlpha=0.5;
fontSize=25;

%% Import STL file as patch data

%Set main folder
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
pathName=fullfile(defaultFolder,'data','STL');
fileName=fullfile(pathName,'femur.stl');
[stlStruct] = import_STL_txt(fileName);

F=stlStruct.solidFaces{1};
V=stlStruct.solidVertices{1};

%%
% Merging nodes example
[F,V]=mergeVertices(F,V);

%%
% Plotting the model

cFigure;
title('Imported patch data from STL','fontSize',fontSize);
gpatch(F,V,'g');
axisGeom;
camlight('headlight');
lighting phong; axis off;
drawnow;

%% Importing a multi-solid STL file as patch data

%Set main folder
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
pathName=fullfile(defaultFolder,'data','STL');
fileName=fullfile(pathName,'stanford_bunny_multi.stl');
[stlStruct] = import_STL_txt(fileName);

%%
% Plotting the models
pColors=autumn(numel(stlStruct.solidNames));

cFigure;
title('Imported patch data from multi-solid STL','fontSize',fontSize);
for q=1:1:numel(stlStruct.solidNames)
    F=stlStruct.solidFaces{q};
    V=stlStruct.solidVertices{q};
    gpatch(F,V,pColors(q,:),'k',faceAlpha);
end
axisGeom;
camlight('headlight');
lighting phong; axis off;
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [rf rootIndex] = buildForest( rf_model )
%(treemap, nodestatus, nodeclass, bestvar, xbestsplit, ndbigtree)
    % extract all the fields
    treemap = double(rf_model.treemap);
    nodestatus = double(rf_model.nodestatus);
    nodeclass = double(rf_model.nodeclass);
    bestvar = double(rf_model.bestvar);
    xbestsplit = double(rf_model.xbestsplit);
    ndbigtree = double(rf_model.ndbigtree);

    [nrnodes ntree] = size(nodestatus);
    nnodes = sum(ndbigtree(1:ntree));
    rf = cell(1, nnodes);
    rootIndex = zeros(1, ntree);
    maskIndex = 0 * nodestatus;

    % first create nodes
    index = 1;
    for n = 1 : ntree
        rootIndex(n) = index;
        for jx = 1 : nrnodes
            if nodestatus(jx, n) ~= 0
                rf{index} = RFTreeNode( nodestatus(jx,n) == -1, nodeclass(jx, n),...
                    bestvar(jx, n), xbestsplit(jx, n) );
                maskIndex(jx, n) = index;
                index = index + 1;
            end
        end
    end

    assert( nnodes == index - 1 );

    % then create classification trees by linking the nodes
    for n = 1 : ntree
        for jx = 1 : nrnodes
            if nodestatus(jx, n) ~= 0
                nodeIndex = maskIndex(jx, n);
                treemat = treemap(:, 2*n-1 : 2*n);
                treemat = treemat(:);

                if treemat(2*jx-1) > 0
                    leftChildIndex = maskIndex( treemat(2*jx-1), n );
                    % rf{nodeIndex}.insertLeftChild( rf{leftChildIndex} );
                    rf{nodeIndex}.leftChild = leftChildIndex;
                end

                if treemat(2*jx) > 0
                    rightChildIndex = maskIndex( treemat(2*jx), n );
                    % rf{nodeIndex}.insertRightChild( rf{rightChildIndex} );
                    rf{nodeIndex}.rightChild = rightChildIndex;
                end
            end
        end
    end
end

function buildTree(nodes, r, treemat)

end

%     r = RFTreeNode;
%     r.bestVar = 'r';
%     n1 = RFTreeNode;
%     n1.bestVar = 'n1';
%     n2 = RFTreeNode;
%     n2.bestVar = 'n2';
%     n3 = RFTreeNode;
%     n3.bestVar = 'n3';
%     n3.isTerminal = true;
%     n4 = RFTreeNode;
%     n4.bestVar = 'n4';
%     n5 = RFTreeNode;
%     n5.bestVar = 'n5';
%     n5.isTerminal = true;
%     n6 = RFTreeNode;
%     n6.bestVar = 'n6';
%     n6.isTerminal = true;
%     n7 = RFTreeNode;
%     n7.bestVar = 'n7';
%     n7.isTerminal = true;
%     n8 = RFTreeNode;
%     n8.bestVar = 'n8';
%     n8.isTerminal = true;
%
%     r.insertLeftChild( n1 );
%     r.insertRightChild( n2 );
%     n1.insertLeftChild( n3 );
%     n1.insertRightChild( n4 );
%     n4.insertLeftChild( n7 );
%     n4.insertRightChild( n8 );
%     n2.insertLeftChild( n5 );
%     n2.insertRightChild( n6 );
%
% %     rf = cell(1, 8);
% %     rf{1} = n1;
% %     rf{2} = n2;
% %     rf{3} = n3;
% %     rf{4} = n4;
% %     rf{5} = n5;
% %     rf{6} = n6;
% %     rf{7} = n7;
% %     rf{8} = n8;
% %     rf{9} = r;
%
%     % rf = zeros(1, 8);
%     rf(1) = r;
%     rf(2) = n1;
%     rf(3) = n2;
%     rf(4) = n3;
%     rf(5) = n4;
%     rf(6) = n5;
%     rf(7) = n6;
%     rf(8) = n7;
%     rf(9) = n8;


%% RRT for Planar Redundant Manipulator
%
%

% Do not change anything in rrt.m,
% rrt_star.m and rrt_star_fn.m
%

%

%% Getting started
% Create *main_redundant_rrt_star.m* file. You can create m-file with any other name, that is
% perfectly okay, this will not affect to the solution of a path/motion
% planning problem. All the sources could be found in examples/ directory
% of the distribution.
%

%% Step 1: Choosing the map
% Firstly, we should define what map to use. It is done by defining map
% structure the sample code is goes after.
%
%   map = struct('name', 'bench_redundant_3.mat', 'start_point', [0 0], 'goal_point', [35 35]);

%%
%
% * *name field* defines the file name in maps/ directory
% * *start_point* field defines where the initial point of the problem
% * *goal_point* field define where is the goal point on the map
%

%% Step 2: Setting maximum number of iterations
%
%   max_iter = 20e3;
%
% * *max_iter* variable defines how much iteration should be done to solve
% the path planning problem.

%% Step 3: Do we have to benchmark
%
%   is_benchmark = false;
%
% * *is_benchmark* enables benchmarking. For more details please read the
% sources of rrt.m, rrt_star.m and rrt_star_fn.m

%% Setting random seed
%
%   rand_seed = 40;
%
% * *rand_seed* variable is a random seed for random number generator, it
% is used for sampling nodes. It is usually used for benchmarking. We set
% the same map, however the random seed is different for the same map. You
% can use *now* if you don't care about random seed.
%
%   rand_seed = now;
%

%% Choosing the class (model) we want
%
%   variant = 'FNRedundantManipulator';
%
% * *variant* defines from what class we should instantiate the object. In
% other words it defines what model we choose for application of RRT.
%
% *FNSimple2D* is a name of a class which contains all necessary methods
% and fields in order to represent simple 2D Mobile Robot model.

%% RRT*
%
%   rrt_star(map, max_iter, is_benchmark, rand_seed, variant);
%
% Line above runs RRT with given parameters. In addition, *rrt* function
% returns the class object with a certain solution.
%

%% Sources of *main_redundant_rrt_star.m*
% Press
% <matlab:edit('examples/main_redundant_rrt_star.m') here>
% to play with example code.
%
%   % RRT for Redundant manipulator example.
%   % by Olzhas Adiyatov
%   % 08/28/2013
%
%   map = struct('name', 'bench_redundant_3.mat', 'start_point', [0 0], 'goal_point', [35 35]);
%   max_iter = 20e3;
%   is_benchmark = false;
%   rand_seed = 40;
%   variant = 'FNRedundantManipulator';
%   result = rrt_star(map, max_iter, is_benchmark, rand_seed, variant);
%


function candoit = canGetHessian(problem)
% Checks whether the Hessian can be computed for a problem structure.
%
% function candoit = canGetHessian(problem)
%
% Returns true if the Hessian of the cost function can be computed given
% the problem description, false otherwise.
%
% See also: canGetCost canGetDirectionalDerivative canGetGradient

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    candoit = isfield(problem, 'hess') || ...
              (isfield(problem, 'ehess') && canGetEuclideanGradient(problem));

    % Display an extra warning message to the user in anticipation of
    % common mistakes.
    if ~candoit && ...
           (isfield(problem, 'ehess') && ~canGetEuclideanGradient(problem))
        warning('manopt:canGetHessian', ...
               ['If the Hessian is supplied as a Euclidean Hessian (ehess),\n' ...
                'then the Euclidean gradient must also be supplied (egrad).']);
    end

end


function YLim= visutil_selectYLim(h, varargin)

props = {'Policy',          'auto'          '!CHAR(auto tightest tight)';
         'TightenBorder'     0.03        	'DOUBLE';
         'Symmetrize'       0               '!BOOL';
         'SetLim'           1               '!BOOL';
         };

if nargin==0,
  YLim= props; return
end

opt= opt_proplistToStruct(varargin{:});
[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props);
misc_checkType(h,'!GRAPHICS');

switch(opt.Policy),
 case 'auto',
  YLim= get(h, 'YLim');
 case 'tightest',
  visutil_backaxes(h);
  axis('tight');
  YLim= get(h, 'YLim');
 case 'tight',
  visutil_backaxes(h);
  axis('tight');
  yl= get(h, 'YLim');
  % add border not to make it too tight:
  yl= yl + [-1 1]*opt.TightenBorder*diff(yl);
  % determine nicer limits
  dig= floor(log10(diff(yl)));
  if diff(yl)>1,
    dig= max(1, dig);
  end
  YLim= [util_trunc(yl(1),-dig+1,'floor') util_trunc(yl(2),-dig+1,'ceil')];
end

if opt.Symmetrize,
  ma= max(abs(YLim));
  YLim= [-ma ma];
end

if opt.SetLim,
  set(h, 'YLim',YLim);
end

if nargout==0,
  clear YLim;
end


function piece_processing
global ai ps st hp pc paus frs datas
global highpass lowpass numerator_hp numerator_lp
if st
    stop(ai);
    delete(ai);
else
    data = getdata(ai,ps);
    %datas(1:ps)=datas(ps+1:2*ps);
    datas(1:2*ps)=datas(1*ps+1:3*ps);
    %datas(ps+1:2*ps)=data;
    datas(2*ps+1:3*ps)=data;
    pc=pc+1;
    if (mod(pc,frs)==0)&&(~paus)
        [tmp ii20]=max(data); % syncrhinize to maximum
        %datas1=datas(ii20:ps+ii20-1);
        if (~highpass)&&(~lowpass)
            datas1=datas(ps+ii20:2*ps+ii20-1);
            %datas1=datas(ps+ii20-0:2*ps+ii20-1);
            %datas1(0+1:end)
        end
        if highpass
            datas10=datas(ps+ii20-300:2*ps+ii20-1);
            datas10=filter(numerator_hp,1,datas10);
            datas1=datas10(301:end);
        end
        if lowpass
            datas10=datas(ps+ii20-300:2*ps+ii20-1);
            datas10=filter(numerator_lp,1,datas10);
            datas1=datas10(301:end);
        end
        set(hp,'YData',datas1);
        drawnow;
    end
end


function [Rob,Lmk,Trj,Frm,Fac] = addKeyFrame(Rob,Lmk,Trj,Frm,Fac,factorRob, factorType)

% ADDKEYFRAME Add key frame to trajectory.
%   [Rob,Lmk,Trj,Frm,Fac] = ADDKEYFRAME(Rob,Lmk,Trj,Frm,Fac,factorRob,factorType)
%   adds a new frame Frm at the head of trajectory Trj. This new frame has
%   the pose of Rob. If the trajectory is full, the oldest frame is
%   deleted, and the graph is updated with the removal of all the necessary
%   pointers, factors, and, eventually, also the landmarks that have become
%   orphelin of any factor. It also creates the necessary factors linking
%   the new frame to the graph, and reserves space in the Map for the error
%   states of the new frame.
%
%   See also PRINTGRAPH, CHECKGRAPHINTEGRITY, ADDFRMTOTRJ.

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

% Add frame to trajectory
[Lmk,Trj,Frm,Fac] = addFrmToTrj(...
    Lmk,...
    Trj,...
    Frm,...
    Fac);

% Update new frame with Rob info
[Rob, Frm(Trj.head)] = rob2frm(...
    Rob,...
    Frm(Trj.head));

% Create motion factor
fac = find([Fac.used] == false, 1, 'first');

switch factorType

    case 'absolute'
        [Frm(Trj.head), Fac(fac)] = makeAbsFactor(...
            Frm(Trj.head), ...
            Fac(fac), ...
            Rob);

    case 'motion'
        head_old = mod(Trj.head - 2, Trj.maxLength) + 1;
        [Frm(head_old), Frm(Trj.head), Fac(fac)] = makeMotionFactor(...
            Frm(head_old), ...
            Frm(Trj.head), ...
            Fac(fac), ...
            factorRob);

    otherwise
        error('??? Unknown or invalid factor type ''%s''.', facType)
end

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function r = plus(p,q)
% this function adds the x, y, and z values of the structures that I use in
% the FVtool.
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Copyright (c) 2012-2016 Ali Akbar Eftekhari
% See the license file

if (isa(p, 'FaceVariable')&&isa(q, 'FaceVariable'))
    r=p;
    r.xvalue = p.xvalue+q.xvalue;
    r.yvalue = p.yvalue+q.yvalue;
    r.zvalue = p.zvalue+q.zvalue;
elseif isa(p, 'FaceVariable')
    r=p;
    r.xvalue = p.xvalue+q;
    r.yvalue = p.yvalue+q;
    r.zvalue = p.zvalue+q;
else
    r=q;
    r.xvalue = p+q.xvalue;
    r.yvalue = p+q.yvalue;
    r.zvalue = p+q.zvalue;
end


function m=min2(u)

if ndims(u) > 2
   error('Input image must be grayscale.')
   return
end
if strcmp(class(u),'double')
   y = u;
else
   y = double(u);
end

m=min(min(u));


%============================================================================
% Copyright (C) 2015, Heikki Hyyti
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the "Software"),
% to deal in the Software without restriction, including without limitation
% the rights to use, copy, modify, merge, publish, distribute, sublicense,
% and/or sell copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
% DEALINGS IN THE SOFTWARE.
%============================================================================

disp(' ');
disp('If you use the algorithm in any scientific context, please cite:');
disp('Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm');
disp('for Low-Cost MEMS IMUs," International Journal of Navigation and Observation,');
disp('vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814');
disp(' ');

clear all;
close all;
pause(0.1);

%% Configuration!
% select to test biases using Acceleration test (true) or Rotation test (false)
ACCtest = true;

% use c/compile.m before setting this to true (tested only with linux)
use_C_versions = false;

% fetch GPL licensed algortihms in matlab and C from here before use:
% http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
% Add the C code to c folder and matlab functions in this folder
% (please add also the quaternion library used by Madgwick).
% I apologize this mess. The Madgwick's GPS licenced code is separated from
% this MIT licenced product to avoid infecting this code with GPL licence.
use_comparisonAlgorithms = false;

if (ACCtest)
    disp('Calculating and plotting IMU bias test for Acceleration data');
else
    disp('Calculating and plotting IMU bias test for Rotation data');
end
data = load('allData.mat');

if (~exist('figures', 'dir')); mkdir('figures'); end;

ILtime = data.InertiaLink.tv_sec + (data.InertiaLink.tv_msec/1000);
SFtime = data.SparkFun6DOF.tv_sec + (data.SparkFun6DOF.tv_usec / 1000000);
Ktime = data.Kuka.tv_sec + (data.Kuka.tv_usec/1000000);

firstCommonTime = max([ILtime(1), SFtime(1), Ktime(1)]);
lastCommonTime = min([ILtime(end), SFtime(end), Ktime(end)]);
commonDuration = lastCommonTime - firstCommonTime;

ILtime = ILtime - firstCommonTime;
SFtime = SFtime - firstCommonTime;
Ktime = Ktime - firstCommonTime;

if (ACCtest)
    %ACC-test sequence: Linear motion with different accelerations
    StartIdx = find(ILtime > 468, 1, 'first');
    StopIdx = find(ILtime > 559, 1, 'first');
else
    %IMU-test sequence: free 6D motion
    StartIdx = find(ILtime > 564, 1, 'first');
    StopIdx = find(ILtime > 770, 1, 'first');
end

testName = 'Rotation test';
if (ACCtest)
    testName = 'Acceleration test';
end

markerSize = 8;

figPos = [105 105 645 660];

plotPos = [0.1 0.15 0.70 0.70];

plot21Pos = [0.1 0.53 0.86 0.40];
plot22Pos = plot21Pos + [0 -0.47 0 0];

plot1Pos = [0.1 0.69 0.86 0.25];
plot2Pos = plot1Pos + [0 -0.31 0 0];
plot3Pos = plot2Pos + [0 -0.31 0 0];

plot41Pos = [0.1 0.75 0.86 0.19];
plot42Pos = plot41Pos + [0 -0.23 0 0];
plot43Pos = plot42Pos + [0 -0.23 0 0];
plot44Pos = plot43Pos + [0 -0.23 0 0];

%% getting reference trajectory
R11 = data.Kuka.data_msrCartPos01;
R12 = data.Kuka.data_msrCartPos02;
R13 = data.Kuka.data_msrCartPos03;
x = data.Kuka.data_msrCartPos04;
R21 = data.Kuka.data_msrCartPos05;
R22 = data.Kuka.data_msrCartPos06;
R23 = data.Kuka.data_msrCartPos07;
y = data.Kuka.data_msrCartPos08;
R31 = data.Kuka.data_msrCartPos09;
R32 = data.Kuka.data_msrCartPos10;
R33 = data.Kuka.data_msrCartPos11;
z = data.Kuka.data_msrCartPos12;

[yaw, pitch, roll] = yawpitchroll(R11, R21, R31, R32, R33);
yaw = un_modulo(yaw, 2*pi); %remove 2pi jumps and make yaw continuous
yaw = yaw - yaw(1); %starting yaw from zero (same as IMU estimates)

% compute reference angular velocities using the KUKA robot
% (derivate between positions)
gyro = zeros(length(R11),3);
dt = median(Ktime(2:end)-Ktime(1:end-1)); % the most frequently used discretation time
for i = 2:length(R11)
    R_last = [R11(i-1), R12(i-1), R13(i-1); ...
              R21(i-1), R22(i-1), R23(i-1); ...
              R31(i-1), R32(i-1), R33(i-1)];
    R_cur = [R11(i), R12(i), R13(i); ...
             R21(i), R22(i), R23(i); ...
             R31(i), R32(i), R33(i)];

    Wh = (R_last'*R_cur - eye(3));
    W = 1/dt * Wh;
    gyro(i,1) = (W(3,2) - W(2,3))/2;
    gyro(i,2) = (W(1,3) - W(3,1))/2;
    gyro(i,3) = (W(2,1) - W(1,2))/2;
end

%% calibrate IMU data
imusWithKukaCalibration;

if (~use_comparisonAlgorithms)
    disp('To get reference algorithms, download their implementations from here:');
    disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/');
    disp(' ');
end

%% start bias test computation (compute everything with test region using different biases)
% test and computation is done only for InertiaLink data
testBiases = ((0:0.5:7)*pi/180)';
%testBiases = ((0:0.5:1)*pi/180)';

yaw_DCM_RMSE = zeros(size(testBiases));
yaw_e1_RMSE = zeros(size(testBiases));
yaw_e2_RMSE = zeros(size(testBiases));

pitch_DCM_RMSE = zeros(size(testBiases));
pitch_e1_RMSE = zeros(size(testBiases));
pitch_e2_RMSE = zeros(size(testBiases));

roll_DCM_RMSE = zeros(size(testBiases));
roll_e1_RMSE = zeros(size(testBiases));
roll_e2_RMSE = zeros(size(testBiases));

acc2 = [acc2_x(StartIdx:StopIdx), acc2_y(StartIdx:StopIdx), acc2_z(StartIdx:StopIdx)];
gyro2_noBias = [w2_x(StartIdx:StopIdx), w2_y(StartIdx:StopIdx), w2_z(StartIdx:StopIdx)];
time = ILtime(StartIdx:StopIdx);
deltaT = [0; time(2:end) - time(1:end-1)];

%resample Kuka reference data to InertiaLink time
warning('off','interpolation:interpolation:noextrap');
yaw_in_IL = resample(timeseries(yaw, Ktime),time);
yaw_in_IL = yaw_in_IL.Data;
yaw_in_IL = yaw_in_IL - yaw_in_IL(1); %starting yaw from zero (same as IMU estimates)
pitch_in_IL = resample(timeseries(pitch, Ktime),time);
pitch_in_IL = pitch_in_IL.Data;
roll_in_IL = resample(timeseries(roll, Ktime),time);
roll_in_IL = roll_in_IL.Data;
warning('on','interpolation:interpolation:noextrap');

disp('Starting to compute RMSEs using different biases');

%check if matlabpool is opened
scheduler = findResource();
if (matlabpool('size') < scheduler.ClusterSize)
    if (matlabpool('size') > 0)
        matlabpool close;
    end

    matlabpool(scheduler.ClusterSize);
end

warning('off','MATLAB:mir_warning_maybe_uninitialized_temporary');
parfor i = 2:(length(testBiases))
    disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']);

    % add constant bias for testing purposes
    addedGyroBias = testBiases(i);
    gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias));

    if (use_C_versions)
        [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
            DCM_IMU_C(gyro2, acc2, deltaT);

        if (use_comparisonAlgorithms)
            quaternion1 = Madgwick_IMU_C(gyro2, acc2);
            quaternion2 = Mahony_IMU_C(gyro2, acc2);
        else
            quaternion1 = nan(size(gyro2,1),4);
            quaternion2 = nan(size(gyro2,1),4);
        end
    else
        DCM = DCM_IMU();

        if (use_comparisonAlgorithms)
            addpath('quaternion_library');      % include quaternion library

            IMU1t = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
            IMU2t = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);

            quaternion1 = zeros(length(time), 4);
            quaternion2 = zeros(length(time), 4);
        else
            quaternion1 = nan(length(time), 4);
            quaternion2 = nan(length(time), 4);
        end

        x_hist2 = zeros(length(time), 6);
        ypr_hist2 = zeros(length(time), 3);
        P_diag_hist2 = zeros(length(time), 6);
        for t = 1:length(time)
            DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t));	% gyroscope units must be radians
            x_hist2(t, :) = DCM.state';
            ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll];
            P_diag_hist2(t, :) = diag(DCM.P)';

            if (use_comparisonAlgorithms)
                IMU1t.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion1(t, :) = IMU1t.Quaternion;

                IMU2t.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion2(t, :) = IMU2t.Quaternion;
            end
        end
    end

    % Plot algorithm output as Euler angles
    % The first and third Euler angles in the sequence (phi and psi) become
    % unreliable when the middle angles of the sequence (theta) approaches ~90
    % degrees. This problem commonly referred to as Gimbal Lock.
    % See: http://en.wikipedia.org/wiki/Gimbal_lock

    % use conjugate for sensor frame relative to Earth and convert to degrees.
    if (use_comparisonAlgorithms)
        euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi);
        euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi);
    else
        euler1 = nan(size(quaternion1,1),3);
        euler2 = nan(size(quaternion2,1),3);
    end

    % compute errors

    %yaw pitch roll errors (only InertiaLink data)
    ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous
    ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM

    euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous
    euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick

    euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous
    euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony

    %yaw
    yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi;
    yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2));
    yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi;
    yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2));
    yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi;
    yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2));

    %pitch
    pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi;
    pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2));
    pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi;
    pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2));
    pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi;
    pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2));

    %roll
    roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi;
    roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2));
    roll_e1_error = euler1(:,1) - roll_in_IL*180/pi;
    roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2));
    roll_e2_error = euler2(:,1) - roll_in_IL*180/pi;
    roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2));
end
warning('on','MATLAB:mir_warning_maybe_uninitialized_temporary');
%% plotting

% compute same for smallest and some other index (for separate plotting)
figIdx = 1;
j_idx = [1 3];
for j = 1:2
    i = j_idx(j);

    disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']);

    % add constant bias for testing purposes
    addedGyroBias = testBiases(i);
    gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias));

    if (use_C_versions)
        [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
            DCM_IMU_C(gyro2, acc2, deltaT);

        if (use_comparisonAlgorithms)
            quaternion1 = Madgwick_IMU_C(gyro2, acc2);
            quaternion2 = Mahony_IMU_C(gyro2, acc2);
        else
            quaternion1 = nan(size(gyro2,1),4);
            quaternion2 = nan(size(gyro2,1),4);
        end
    else
        DCM = DCM_IMU();

        if (use_comparisonAlgorithms)
            addpath('quaternion_library');      % include quaternion library

            IMU1 = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
            IMU2 = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);

            quaternion1 = zeros(length(time), 4);
            quaternion2 = zeros(length(time), 4);
        else
            quaternion1 = nan(length(time), 4);
            quaternion2 = nan(length(time), 4);
        end

        x_hist2 = zeros(length(time), 6);
        ypr_hist2 = zeros(length(time), 3);
        P_diag_hist2 = zeros(length(time), 6);
        for t = 1:length(time)
            DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t));	% gyroscope units must be radians
            x_hist2(t, :) = DCM.state';
            ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll];
            P_diag_hist2(t, :) = diag(DCM.P)';

            if (use_comparisonAlgorithms)
                IMU1.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion1(t, :) = IMU1.Quaternion;

                IMU2.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
                quaternion2(t, :) = IMU2.Quaternion;
            end
        end
    end

    % Plot algorithm output as Euler angles
    % The first and third Euler angles in the sequence (phi and psi) become
    % unreliable when the middle angles of the sequence (theta) approaches ~90
    % degrees. This problem commonly referred to as Gimbal Lock.
    % See: http://en.wikipedia.org/wiki/Gimbal_lock

    % use conjugate for sensor frame relative to Earth and convert to degrees.
    if (use_comparisonAlgorithms)
        euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi);
        euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi);
    else
        euler1 = nan(size(quaternion1,1),3);
        euler2 = nan(size(quaternion2,1),3);
    end

    % compute errors

    % yaw pitch roll errors (only InertiaLink data)
    ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous
    ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM

    euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous
    euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick

    euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous
    euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony

    %yaw
    yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi;
    yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2));
    yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi;
    yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2));
    yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi;
    yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2));

    %pitch
    pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi;
    pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2));
    pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi;
    pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2));
    pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi;
    pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2));

    %roll
    roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi;
    roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2));
    roll_e1_error = euler1(:,1) - roll_in_IL*180/pi;
    roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2));
    roll_e2_error = euler2(:,1) - roll_in_IL*180/pi;
    roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2));

    % plotting
    % Yaw, pitch and roll plot
    figure(figIdx); clf;
    subplot(3,1,1);
    yaw_dcm = ypr_hist2(:,1)*180/pi;
    pitch_dcm = ypr_hist2(:,2)*180/pi;
    roll_dcm = ypr_hist2(:,3)*180/pi;

    plot(time, yaw_dcm, 'b',  time, euler1(:,3), 'g--', time, euler2(:,3), 'r:', time, yaw_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
    legend('DCM', 'Madgwick', 'Mahony', 'Reference', 'Location', legendLocation([yaw_dcm euler1(:,3) euler2(:,3)]));

    ylabel('yaw (deg)', 'FontSize', 12, 'FontName', 'Times');
    title(['Euler angles with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,2);
    plot(time, pitch_dcm, 'b',  time, euler1(:,2), 'g--', time, euler2(:,2), 'r:', time, pitch_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
    ylabel('pitch (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,3);
    plot(time, roll_dcm, 'b',  time, euler1(:,1), 'g--', time, euler2(:,1), 'r:', time, roll_in_IL*180/pi, 'k--', 'LineWidth', 1);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('roll (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    % error plot
    figure(figIdx); clf;

    subplot(3,1,1);
    plot(time, yaw_DCM_error, 'b', time, yaw_e1_error, 'g--', time, yaw_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
    legend('DCM', 'Madgwick', 'Mahony', 'Location', legendLocation([yaw_DCM_error yaw_e1_error yaw_e2_error]));
    ylabel('yaw error (deg)', 'FontSize', 12, 'FontName', 'Times');
    title(['Errors to the reference measurement with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,2);
    plot(time, pitch_DCM_error, 'b', time, pitch_e1_error, 'g--', time, pitch_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
    ylabel('pitch error (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    subplot(3,1,3);
    plot(time, roll_DCM_error, 'b',  time, roll_e1_error, 'g--', time, roll_e2_error, 'r:', 'LineWidth', 1);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('roll error (deg)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)]);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    % bias plot
    figure(figIdx); clf;

    x_sigma = sqrt(P_diag_hist2(:,4));
    y_sigma = sqrt(P_diag_hist2(:,5));
    z_sigma = sqrt(P_diag_hist2(:,6));

    bias_x = x_hist2(:,4)*180/pi;
    bias_y = x_hist2(:,5)*180/pi;
    bias_z = x_hist2(:,6)*180/pi;

    plotLims = 1.25*[mean(x_sigma(1:floor(end/3))), mean(y_sigma(1:floor(end/3))), mean(z_sigma(1:floor(end/3)))];

    subplot(3,1,1);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_x, 'b', time, (addedGyroBias-x_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+x_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    h = legend('Reference', 'Estimate', '1-sigma');
    set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');

    ylabel('x_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    title('Bias estimates and 1-sigma distances of standard deviations', 'FontSize', 14, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(1) addedGyroBias+plotLims(1)]*180/pi);

    subplot(3,1,2);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_y, 'b', time, (addedGyroBias-y_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+y_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');

    ylabel('y_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(2) addedGyroBias+plotLims(2)]*180/pi);

    subplot(3,1,3);
    plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
        time, bias_z, 'b', time, (addedGyroBias-z_sigma)*180/pi, 'b-.', ...
        time, (addedGyroBias+z_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
    set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');

    xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
    ylabel('z_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
    set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(3) addedGyroBias+plotLims(3)]*180/pi);

    set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
    set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
    set(gcf,'PaperPositionMode','auto');
    saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
    print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])
    figIdx = figIdx + 1;

    pause(0.2);
end

%% statistics plot
figure(figIdx); clf;

subplot(3,1,1);
semilogy(testBiases*180/pi, yaw_DCM_RMSE, 'b*-', testBiases*180/pi, yaw_e1_RMSE, 'g^-', testBiases*180/pi, yaw_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
legend('DCM', 'Madgwick', 'Mahony', 'Location', 'SouthEast');
ylabel('yaw RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');
title(['RMSEs as a function of gyroscope bias (' testName ')'], 'FontSize', 14, 'FontName', 'Times');

subplot(3,1,2);
semilogy(testBiases*180/pi, pitch_DCM_RMSE, 'b*-', testBiases*180/pi, pitch_e1_RMSE, 'g^-', testBiases*180/pi, pitch_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');

subplot(3,1,3);
semilogy(testBiases*180/pi, roll_DCM_RMSE, 'b*-', testBiases*180/pi, roll_e1_RMSE, 'g^-', testBiases*180/pi, roll_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('Constant gyroscope bias (deg/s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('roll RMSE (deg)', 'FontSize', 12, 'FontName', 'Times');

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps'])


% RESON_dyProg_mat.m
% Function to carry out dynamic programming componenent of SE_VQ
%
% Octave compatible
%
% Description
% Function to carry out dynamic programming method described in Ney (1989)
% and used previously in the ESPS GCI detection algorithm. The method
% considers target costs and transition costs which are accumulated in
% order to select the `cheapest' path, considering previous context
%
% Inputs
%  rel_amp         : [samples] [MxNcand] Relative residual amplitudes
%  GCI_N           : [samples] [MxNcand] matrix containing M by Ncandidate GCI locations
%  F0mean          : [Hz]      [1x1] Mean fundamental frequency
%  x               : [samples] [Nx1] Speech signal
%  fs              : [Hz]      [1x1] sampling frequency
%  trans_wgt       : [integer] [1x1] Weight of transition cost
%  relAmp_wgt      : [integer] [1x1] Weight of target cost
%
% Outputs
%  GCI_opt         : [samples] [Mx1] Optimal GCI path
%
% Example
%  GCI_opt =
%  RESON_dyProg_mat(GCI_relAmp,GCI_N,F0_mean,x,fs,trans_wgt,relAmp_wgt)
%
% References
%  [1] Kane, J., Gobl, C., (2013) `Evaluation of glottal closure instant
%       detection in a range of voice qualities', Speech Communication
%       55(2), pp. 295-314.
%
% Copyright (c) 2013 Trinity College Dublin
%
% License
%  This code is a part of the Voice Analysis Toolkit with the following
%  licence:
%  The software product (and any modifications thereof) is distributed under
%  a dual licence: an open source license for individual, non-commercial
%  purposes, and a commercial license. The opensource licence under which
%  the product is distributed is GNU GPL v2. For individual users, this
%  licence suits their use as these are not distributing proprietary
%  modifications, additions to, or derivatives of the product and don't
%  require legal protection of a commercial licence. For commercial users,
%  where open source does not meet their requirements, we offer commercial
%  licensing of the product. A commercial license permits customers to
%  modify, add or produce derivative products without the obligation of
%  making the subsequent code open source. For more information regarding
%  our commercial licence, please contact john.whelan@tcd.ie
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  John Kane kanejo@tcd.ie
%
% $Id <info set by the versioning system> $

function GCI_opt = RESON_dyProg_mat(GCI_relAmp,GCI_N,F0_mean,x,fs,trans_wgt,relAmp_wgt)

%% Initial settings
plots=0;
cost = (GCI_relAmp.*relAmp_wgt)';
ncands=size(GCI_N,1);
nframe=size(GCI_N,2);
GCI_N=GCI_N';
prev=zeros(nframe,ncands);      % traceback pointer
pulseLen = round(fs/F0_mean);
GCI_opt=zeros(1,nframe);

%% Do processing
for n=1:nframe

    if n>1
        costm=zeros(ncands,ncands);         % transition cost matrix: rows (previous), cols (current)

        for c=1:ncands
            % Transitions TO states in current frame
            start=GCI_N(n,c)-round(pulseLen/2);
            stop=GCI_N(n,c)+round(pulseLen/2);
            if stop > length(x)
                stop=length(x);
            end

            if start<1
                start=1;
            end
            pulse_cur = x(start:stop);

            for p=1:ncands
                % Transitions FROM states in previous frame
                start=GCI_N(n-1,p)-round(pulseLen/2);
                stop=GCI_N(n-1,p)+round(pulseLen/2);
                if start<1
                    start=1;
                end
                if stop > length(x)
                    stop=length(x);
                end
                if start<1
                    start=1;
                end

                pulse_prev = x(start:stop);

                if isempty(pulse_cur) ||isnan(pulse_cur(1)) || ...
                        isnan(pulse_prev(1))
                    costm(p,c)=0;
                else
                    if length(pulse_cur)~=length(pulse_prev)
                        cor_cur=0;
                    else
                        cor_cur=corrcoef(pulse_cur(:),pulse_prev(:));
			if length(cor_cur)==1
			   cor_cur=0;
			else cor_cur=cor_cur(2);
			end
                    end
                    costm(p,c) = (1-abs(cor_cur))*trans_wgt; % transition cost
                end
            end
        end

        costm=costm+repmat(cost(n-1,1:ncands)',1,ncands);  % add in cumulative costs
        [costi,previ]=min(costm,[],1);
        cost(n,1:ncands)=cost(n,1:ncands)+costi;
        prev(n,1:ncands)=previ;
    end

end

%% Do traceback
best=zeros(n,1);
[cbest,best(n)]=min(cost(n,1:ncands));
for i=n:-1:2
     best(i-1)=prev(i,best(i));
end

for n=1:nframe
    GCI_opt(n) = GCI_N(n,best(n));
end

%% Do plots
if plots
    GCI_norm=zeros(nframe,ncands);
    GCI_opt_norm=zeros(nframe,ncands);
    for n=1:nframe
        GCI_norm(n,:) = GCI_N(n,:)-GCI_N(n,1);
        GCI_opt_norm(n) = GCI_opt(n)-GCI_N(n,1);
    end
    subplot(211), plot(x), hold on, stem(GCI_N(:,1),ones(1,length(GCI_N(:,1)))*-.1,'r')
        stem(GCI_opt,ones(1,length(GCI_opt))*-.1,'k')
    subplot(212), plot(GCI_opt,GCI_norm,'rx'), ylim([-20 20]),
        hold on, plot(GCI_opt,GCI_opt_norm)
end


clc;clear;
close all;
Angle = [90,180];                               %Angle,Options:90,135,180,270 plus:0,45,225,315
if(exist('SetupStruc','var'))
    [s,sOri,Unmix_s,~] = readData(Angle,ISM_setup);
else
    [s,sOri,Unmix_s,SetupStruc] = readData(Angle,ISM_setup);    % 's' is the muli-channel mixture signal,size:Length*7,
end                                            % 'sOri' is the each single singal of the 1st channel corresponding to the angle,size:Length*N
method = {'DSB'                1;
          'DSB_Mask'           0;
          'MVDR'               0;
          'MVDR_ESB'           0;
          'MVDR_AESB'          0;
          'MVDR_PCA'           0;
          'MVDR_Search'        0;
          'MVDR_AESB_Search'   0;
          'LCMV'               0;
          'LCMV_ESB'           0;
          'LCMV_AESB'          0;
          'LCMV_Search'        0;
          'ICA_funda'          0;
          'ICA_initial'        0;
          'ICA_Sawada'         0;
          'FastICA_HO_Sawada'  0;
          'IVA'                0;
          'IVA_woDR'           0;
          'AuxIVA'             0;
          'OverIVA'            0;
          'OverILRMA'          0;
          'ILRMA_woDR'         0;
          'ILRMA'              0;
          'ILRMA_PF'           0;
          'FastMNMF1'          0;
          'FastMNMF2'          0;
          'FastFCA_AS'         0; %%%% remaining to be written
          'maxSNR'             0;
          %%%%%%% Dereverberation
          'WPE'                0;
          %%%%%%% Compound method
          'cGMM_maxSNR'        0;
          'ALL'                0
          };
SetupStruc.unS = Unmix_s;
[Re,SetupStruc] = Process(method,s,SetupStruc);
[Me] = Cal_metrics(Re,Unmix_s,sOri,SetupStruc);


function s = getAlgorithmsHeaderSizeWordAligned(this)

% Assume a word is 4 bytes
s = getAlgorithmsHeaderSize(this);
s = ceil(s/4)*4;


%
%   Copyright (C) 2016  Starsky Wong <sununs11@gmail.com>
%
%   Note: The SIFT algorithm is patented in the United States and cannot be
%   used in commercial products without a license from the University of
%   British Columbia.  For more information, refer to the file LICENSE
%   that accompanied this distribution.

function [feat] = hist2Descr(feat,descr,descr_mag_thr)
% Function: Convert histogram to descriptor
descr = descr/norm(descr);
descr = min(descr_mag_thr,descr);
descr = descr/norm(descr);
feat.descr = descr;
end


% sim_onepulse_shaped.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% out = sim_onepulse_shaped(n,sw,Bfield,linewidth,sys,RF,tp,phCyc,dfdx,G)
%
% DESCRIPTION:
% %This function simulates the effect of a frequency selective or slice
% selective excitation, followed immediately by the acquisition window.
% This is mainly an exercise to see if I can get slice selective
% excitation working.
%
% Note that when simulating a frequency selective pulse, it is okay to
% specify only 8 arguments (no gradient needs to be specified).  If the
% 9th argument, G, is specified and is non-zero, then a slice selective
% pulse is assumed.
%
% INPUTS:
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% RF        = RF pulse definition structure (obtain using 'io_loadRFwaveform.m')
% tp        = RF pulse duration in [ms]
% phCyc     = Phase of excitation rf pulse in [degrees].
% dfdx      = if simulating a frequency selective pulse, this argument
%             should be the frequency offset [Hz].  If simulating a slice
%             selective pulse, this argument should be the position offset [cm].
% G         = gradient strength for slice-selective pulse [G/cm];
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using pulse-acquire
%             sequence.

function out = sim_onepulse_shaped(n,sw,Bfield,linewidth,sys,RF,tp,phCyc,dfdx,G)

if nargin>9 && G~=0
    simType='g';
else
    simType='f';
end

%Set water to centre
centreFreq=4.65;
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H,d]=sim_Hamiltonian(sys,Bfield);

%BEGIN PULSE SEQUENCE************
if simType=='g'
    d=sim_shapedRF(d,H,RF,tp,90,90+phCyc,dfdx,G);   %slice selective excitation
elseif simType=='f'
    d=sim_shapedRF(d,H,RF,tp,90,90+phCyc,dfdx);     %frequency selective excitation
end
[out,dout]=sim_readout(d,H,n,sw,linewidth,90);      %Readout along y (90 degree phase);
%END PULSE SEQUENCE**************

%Correct the ppm scale:
out.ppm=out.ppm-(4.65-centreFreq);

%Fill in structure header fields:
out.seq='onepulse';
out.te=tp/2;
out.sim='shaped';

%Additional fields for compatibility with FID-A processing tools.
out.sz=size(out.specs);
out.date=date;
out.dims.t=1;
out.dims.coils=0;
out.dims.averages=0;
out.dims.subSpecs=0;
out.dims.extras=0;
out.averages=1;
out.rawAverages=1;
out.subspecs=1;
out.rawSubspecs=1;
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.subtracted=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;


%get_file_info2.m
%
%Replacement for get_file_info.m:
%   Numerous efficiency fixes, smaller filesize, easier readability,
%   and structural/organizational changes to keep returned info together

function [file_info] = get_file_info2(file, handles, override)
    [file_info.path, file_info.name, file_info.ext] = fileparts(file);

    try
        image0    = imread(file,2);
        file_info.type = 'multipage';
        int_prec=0;
    catch
        image0    = imread(file);
        file_info.type = 'singlepage';
    end

    class_type    = class(image0);
    file_info.bitdepth = log2(double(intmax(class_type))-double(intmin(class_type)));
    file_info.width    = size(image0,2);
    file_info.height   = size(image0,1);

    if ~isnan(str2double(get(handles.prec_edit,'String')))
        int_prec = str2double(get(handles.prec_edit,'String'));
        file_info.prec = ['%',num2str(int_prec),'.',num2str(int_prec),'d'];
    else
        length_name = length(file_info.name);
        for x = length_name:-1:1
            if isnan(str2double(file_info.name(x)))
                int_prec  = length_name-x;
                file_info.prec = ['%',num2str(int_prec),'.',num2str(int_prec),'d'];
                break
            end
        end
    end

    file_info.part_name = file_info.name(1:length(file_info.name)-int_prec);

    if get(handles.custom_frame,'Value')
        file_info.start = str2double(get(handles.from_edit,'String'));
        file_info.stop  = str2double(get(handles.to_edit,'String'));
    elseif ~get(handles.custom_frame,'Value')
        if strcmp(file_info.type,'multipage')
            file_info.start = 1;

            if ~exist('override','var') %Allow function inputs to override autodetect of end frame (saves on time)
                file_imf = imfinfo(file);
                file_info.stop  = length(file_imf);
            end
        elseif strcmp(file_info.type,'singlepage')
            file_info.start = 1;

            if ~exist('override','var') %Allow function inputs to override autodetect of end frame (saves on time)
                index = 1;
                while exist([file_info.path,'\',file_info.part_name,num2str(index,file_info.prec),file_info.ext],'file')
                    index = index + 1;
                end
                file_info.stop = index - 1;
            end
        end
    end

    if strcmp(file_info.type,'multipage') %Override prec and partial name for multipage images
        file_info.part_name = file_info.name;
        file_info.prec  = '%0.0d';
    end