function [lossValue, gradients, predictions] = vl_structuredNetwork_pairwiseModel(net, x, gradients, labels, dzdy, varargin)
%vl_structuredNetwork_pairwiseModel implements the evaluation of the structured network together with its gradient comuptation

opts = struct;
opts.conserveMemory = true ;
opts.sync = true ;
opts.disableDropout = false ;
opts.backPropagateType = 'all'; % 'all', 'unaryAndPairwise', 'onlyUnary', 'onlyPairwise'
opts.computeMaxMarginals = false;
opts.disableDropoutFeatureExtractor = false;
opts.lossNormalization = 1;
opts = vl_argparse(opts, varargin);

if (nargin <= 4) || isempty(dzdy)
    doder = false ;
else
    doder = true ;
end

%% preparation
numImages = length(labels);
numNodes = zeros(numImages, 1);
numEdges = zeros(numImages, 1);
for iImage = 1 : numImages
    numNodes( iImage ) = length( labels{iImage}.candidateBatchIds );
    numEdges( iImage ) = size(labels{ iImage }.clusteredEdges.bbIds, 1);
end
numCandidates = sum(numNodes);
if size(x, 4) ~= numCandidates
    error('vl_structuredNetwork_pairwiseModel:badSizeInput', 'Size of data input "x" is incompatible with "labels"');
end

n = numel(net.layers) ;
if doder
    if isempty(gradients)
        gradients = cell( length(net.layers), 1);
    end
    for i = 1 : n
        if isfield( net.layers{i}, 'weights' )
            for j = 1 : length( net.layers{i}.weights )
                gradients{i}.dzdw{j} = zeros( size(net.layers{i}.weights{j}), 'like', net.layers{i}.weights{j} );
            end
        end
    end
end

%% forward pass
% forward pass on the feature extraction network
disableFeatureExtractorDropout = opts.disableDropout || opts.disableDropoutFeatureExtractor;
numLayers_features = length(net.featureExtractor.layers);
res_features = initRes( numLayers_features );
res_features = vl_simplenn_pairwiseModel_forwardPass(net.featureExtractor, x, net.layers, res_features, ...
    'conserveMemory', opts.conserveMemory, ...
    'sync', opts.sync, ...
    'disableDropout', disableFeatureExtractorDropout);

extractedFeatures = res_features(end).x;
numFeatures = size( extractedFeatures, 3 );

% forward pass on the unary potential network
numLayers_unary = length(net.unaryNetwork.layers);
res_unary = initRes( numLayers_unary );
res_unary = vl_simplenn_pairwiseModel_forwardPass( net.unaryNetwork, extractedFeatures, net.layers, res_unary, ...
    'conserveMemory', opts.conserveMemory, ...
    'sync', opts.sync, ...
    'disableDropout', opts.disableDropout);

% forward pass on the pairwise potential network
x_pairwise = zeros( 1, 1, numFeatures * 2, sum(numEdges), 'like', x);
startId = 0;
batchId_inEdge = zeros( sum(numEdges), 2 );
for iImage = 1 : numImages
    curIds = startId + (1 : numEdges(iImage));
    curCandidateBatchIds = labels{ iImage }.candidateBatchIds( labels{ iImage }.clusteredEdges.bbIds );
    x_pairwise(:,:,1:numFeatures,curIds) = extractedFeatures(:,:,:, curCandidateBatchIds(:,1) );
    x_pairwise(:,:,numFeatures + 1 : 2 * numFeatures,curIds) = extractedFeatures(:,:,:, curCandidateBatchIds(:,2) );

    batchId_inEdge( curIds, :) = curCandidateBatchIds;

    startId = startId + numEdges(iImage);
end

numLayers_pairwise = length(net.pairwiseNetwork.layers);
res_pairwise = initRes( numLayers_pairwise );
res_pairwise = vl_simplenn_pairwiseModel_forwardPass( net.pairwiseNetwork, x_pairwise, net.layers, res_pairwise, ...
    'conserveMemory', opts.conserveMemory, ...
    'sync', opts.sync, ...
    'disableDropout', opts.disableDropout);

%% loss evaluation
unaryPotentials = gather(res_unary(end).x);
pairwisePotentials = gather(res_pairwise(end).x);

switch net.lossLayer.type
    case 'logisticScoresCompact'
        [lossValue, unaryDerivative, pairwiseDerivative, predictions] = vl_logisticScoreLoss_pairwiseCompactModel( unaryPotentials, pairwisePotentials, labels, dzdy, opts.computeMaxMarginals);
    case 'svmStructCompact'
        [lossValue, unaryDerivative, pairwiseDerivative, predictions] = vl_svmStructLoss_pairwiseCompactModel( unaryPotentials, pairwisePotentials, labels, dzdy, opts.computeMaxMarginals, opts.lossNormalization);
    otherwise
        error( ['Unknown structured loss: ', net.lossLayer.type] );
end

%% backward pass
if doder
    if isequal( opts.backPropagateType, 'all' ) || isequal( opts.backPropagateType, 'onlyUnary' ) || isequal( opts.backPropagateType, 'unaryAndPairwise' )
        % backward pass on the unary potential network
        [res_unary, gradients] = vl_simplenn_pairwiseModel_backwardPass( net.unaryNetwork, extractedFeatures, net.layers, res_unary, gradients, unaryDerivative, ...
            'conserveMemory', opts.conserveMemory, ...
            'sync', opts.sync, ...
            'disableDropout', opts.disableDropout );
    end

    if isequal( opts.backPropagateType, 'all' )
        % backward pass of the unary signal through the feature extracting network
        % start accumulating data to backprop through the feature extraction network:
        featureBackPropSignal = res_unary(1).dzdx;
    end

    if isequal( opts.backPropagateType, 'all' ) || isequal( opts.backPropagateType, 'onlyPairwise' ) || isequal( opts.backPropagateType, 'unaryAndPairwise' )
        % backward pass through pairwise potentials network
        [res_pairwise, gradients] = vl_simplenn_pairwiseModel_backwardPass( net.pairwiseNetwork, x_pairwise, net.layers, res_pairwise, gradients, pairwiseDerivative, ...
            'conserveMemory', opts.conserveMemory, ...
            'sync', opts.sync, ...
            'disableDropout', opts.disableDropout );
    end

    % prepare data for passing backward pairwise signal
    if isequal( opts.backPropagateType, 'all' )
        I = eye( numCandidates, 'like', x );
        ids1 = I(batchId_inEdge(:,1), :);
        ids2 = I(batchId_inEdge(:,2), :);

        data1 = res_pairwise(1).dzdx(:,:, 1 : numFeatures, :);
        data2 = res_pairwise(1).dzdx(:,:, numFeatures + 1 : 2 * numFeatures, :);

        data1 = reshape(data1, [], sum( numEdges ));
        data2 = reshape(data2, [], sum( numEdges ));

        featureBackPropSignal = featureBackPropSignal ...
            + reshape( data1 * ids1, size(featureBackPropSignal) ) ;
        featureBackPropSignal = featureBackPropSignal ...
            + reshape( data2 * ids2, size(featureBackPropSignal) ) ;

%         % SLOW VERSION
%         edgeOffset = 0;
%         for iImage = 1 : numImages
%             for iEdge = 1 : numEdges( iImage )
%                 instance1 = labels{iImage}.clusteredEdges.bbIds(iEdge, 1);
%                 instance1 = labels{iImage}.candidateBatchIds( instance1 );
%
%                 featureBackPropSignal(:,:,:,instance1) = featureBackPropSignal(:,:,:,instance1) ...
%                     + res_pairwise(1).dzdx(:,:, 1 : numFeatures, edgeOffset + iEdge);
%
%                 instance2 = labels{iImage}.clusteredEdges.bbIds(iEdge, 2);
%                 instance2 = labels{iImage}.candidateBatchIds( instance2 );
%
%                 featureBackPropSignal(:,:,:,instance2) = featureBackPropSignal(:,:,:,instance2) ...
%                     + res_pairwise(1).dzdx(:,:, numFeatures + 1 : 2 * numFeatures, edgeOffset + iEdge);
%             end
%             edgeOffset = edgeOffset + numEdges( iImage ) ;
%         end

        % backward pass of the pairwise signal through the feature extracting network
        [res_features, gradients] = vl_simplenn_pairwiseModel_backwardPass( net.featureExtractor, x, net.layers, res_features, gradients, featureBackPropSignal, ...
            'conserveMemory', opts.conserveMemory, ...
            'sync', opts.sync, ...
            'disableDropout', disableFeatureExtractorDropout );
    end
end

end

function res = initRes(n)
res = struct(...
    'x', cell(1,n+1), ...
    'dzdx', cell(1,n+1), ...
    'dzdw', cell(1,n+1), ...
    'aux', cell(1,n+1), ...
    'time', num2cell(zeros(1,n+1)), ...
    'backwardTime', num2cell(zeros(1,n+1))) ;
end


function [ClusterID,EventType,AlgoInfo] =  clusterSLIDERmag(ShortCat,MainMag,BackTime,ForeTime,MinRad,MagRadScale,MagRadConst)

	% Example [mCatclus] =  clusterSTEPmag(mCatalog, 1.95, 1.95, 1984, 1, 1);

	% code to cluster catalog with spatial windows according to STEP forecasting model
	% with sliding windows of t1 backwards in time and t2 forward in time
	% starting with the largest earthquake in the catalog

	%
	% Input parameters:
	%   mCatalog        Earthquake catalog
	%   Mainmag         minimum mainmag magnitude
	%   Mc              Completeness magnitude
	%   startyear
	%   t1              time before an earthquake in days
	%   t2              time window following an earthquake in days
	%
	% Output parameters:
	%   fBValue         mCatclus: The clustered catalog for further anaysis
	%
	% Annemarie Christophersen, 31. January 2008

	% Code written for catalogue under zmap, thus the input
	% catalogue is in the variable a, where
	% column 1: longitude
	% column 2: latitude
	% column 3: year (decimal year, including seconds)
	% column 4: month
	% column 5: day
	% column 6: magnitude
	% column 7: depth
	% column 8: hour
	% column 9: minute
	% column 10: seconds
	% column 11-23 not important for clustering and cluster analysis
	% column 24: SCSN flag for event type (l=local, r=regional, q=quarry)

	% variables used
	% mc completeness magnitude
	% twindow duration in time in which to look for related events

	%Converted for MapSeis by DE 21.6.2012

	%may make this switchable if needed
	Rounding=true;

	%init work arrays
	numEvents = length(ShortCat);
	WorkArray=zeros(numEvents,4);
	WorkArray(:,1)= (1:numEvents)'; %introduce column 11 with row number

	%init output
	ClusterID=nan(numEvents,1);
	%EventType=ones(numEvents,1);
	EventType = categorical(ones(numEvents,1),...
		[0 1 2 3],...
        {'unclassified','single event','mainshock','aftershock'});
	InitEvent=false(numEvents,1);
	AlgoInfo=[];

	%InitEvent is currently not directly supported by mapseis, but will be
	%added to AlgoInfo, so it can be used by specialiced algorithms

	clusterno = 1;

	%Dtafter = t2/365; %30 days in decimal years
	%Dtbefore = t1/365; %2 days in decimal years
	clusterno = 1;

	while any(WorkArray(:,2)==0)
		notClustered=WorkArray(:,2)==0;

		[maxmag IDX]=max(ShortCat(notClustered,5)); %the magnitude of the largest earthquake not yet clustered
		maxmag=maxmag(1); %could have mor than one maximum afterall
		NrInGame=WorkArray(notClustered,1);
		ActualIndex=NrInGame(IDX(1));

	   	lino = ActualIndex;
	   	WorkArray(lino,2)=clusterno; %write cluster number into column 12
	   	searchradius=max(MinRad, 10^(MagRadScale*maxmag-MagRadConst)); %calculate search radius

		%get data of the new event
		tref = ShortCat(lino,4); %set reference time
		latref = ShortCat(lino,2); %set reference latitude
		lonref = ShortCat(lino,1); %set reference longitude

		%search for earthquakes before, extend by sliding time windows of t1
		eventsDtbefore=(ShortCat(:,4) > tref-BackTime & ShortCat(:,4) < tref);
		eventsbefore = sum(eventsDtbefore);
		linobefore = lino-eventsbefore;

		%%That did not work like it was intended
		%%--------------------------------------

		% for i=linobefore:lino
			% if (b(i,12) == 0) % don't bother if event already clustered
			    % edist = deg2km(distance(latref,lonref,ShortCat(i,2),ShortCat(i,1))); %calculate distance to mainshock
			    % if (edist <= searchradius)
				% WorkArray(i,2)=clusterno;
				% %make this event new tref and search for events before
				% tref=ShortCat(i,4);
				% eventsDtbefore=(ShortCat(:,4) > tref-BackTime & ShortCat(:,4) < tref);
				% eventsbefore = length(b(eventsDtbefore,1));
				% linobefore = max(1, i-eventsbefore);
				% i=linobefore; %is this the right way to restart loop at linobefore?
			    % end
			% end
		% end %this could now have an earlier linebefore and will loop up to lino
		% % where lino remains the number of mainshoc
		% % now look for later earthquakes

		%old version whould not work, for "borders" once set, will stay as the are;
		i=linobefore;
		while i<=lino
			if WorkArray(i,2)==0
				edist = deg2km(distance(latref,lonref,ShortCat(i,2),ShortCat(i,1))); %calculate distance to mainshock
				if (edist <= searchradius)
					WorkArray(i,2)=clusterno;
					%make this event new tref and search for events before
					tref=ShortCat(i,4);
					eventsDtbefore=(ShortCat(:,4) > tref-BackTime & ShortCat(:,4) < tref);
					eventsbefore = sum(eventsDtbefore);
					linobefore = max(1, i-eventsbefore);
					i=linobefore; %Now it does restart

				else
					i=i+1;
				end
			else
				i=i+1;
			end

		end

		tref=ShortCat(lino,4); %reset reference time to mainshock

		while (lino < numEvents+1) && ShortCat(lino,4) < (tref+ForeTime)
			if WorkArray(lino,2) == 0
				edist = deg2km(distance(latref,lonref,ShortCat(lino,2),ShortCat(lino,1)));
				if (edist <= searchradius)
					WorkArray(lino,2)=clusterno;

					%Not needed already cutted
					%if (ShortCat(lino,6) > Mc)
					tref=ShortCat(lino,4); %set reference time to new earthquake in cluster

				end
				lino=lino+1;
			end
		end

		clusterno=clusterno+1;

	end

	%Output clustered matrix
	% column 1: longitude
	% column 2: latitude
	% column 3: year (decimal year, including seconds)
	% column 4: month
	% column 5: day
	% column 6: magnitude
	% column 7: depth
	% column 8: hour
	% column 9: minute
	% column 10: seconds
	% column 11: line number
	% column 12: cluster number
	% column 13: mainshock with its cluster number
	% column 14: initiating event cluster number

	if Rounding
		ShortCat(:,5)=round(ShortCat(:,5)*10)/10;%  round magnitudes to 0.1
	end

	%may not be needed anymore -> it is needed
	WorkArray(:,1)= 1:length(ShortCat)'; %introduce column 1 with row number

	%WorkArray(WorkArray(:,2)==0,2)=nan;
	UsedCluster=unique(WorkArray(~isnan(WorkArray(:,2))&WorkArray(:,2)~=0,2));
	clusterno=1;

	disp(numel(UsedCluster));

	for i=UsedCluster'
		inCluster = WorkArray(:,2)==i;

		if sum(inCluster) >1
			%A real cluster

			%First mark all as Events of the cluster and as Aftershocks
			ClusterID(inCluster)=clusterno;
			EventType(inCluster)='aftershock';

			%find the first of the largest events label it as mainshock
			[MaxMag IDX] = max(ShortCat(inCluster,5));
			availIndex=WorkArray(inCluster,1);
			MrMaxMag=availIndex(IDX(1));
			EventType(MrMaxMag)='mainshock';

			%Mark initial event
			[MinTime IDX] = min(ShortCat(inCluster,4));
			availIndex=WorkArray(inCluster,1);
			MrInit=availIndex(IDX(1));
			InitEvent(MrInit)=true;

			clusterno=clusterno+1;

		elseif sum(inCluster)==1
			%only one event
			ClusterID(inCluster)=nan;
			EventType(inCluster)='single event';

			%clusterno=clusterno+1;

		end

	end

	%And set all not assign clusters to NaN and 1
	notInClust=WorkArray(:,2)==0;
	ClusterID(notInClust)=nan;
	EventType(notInClust)='single event'

	%And finally the AlgoInfo
	ParamInput=struct(	'MainMag',MainMag,...
				'BackTime',BackTime,...
				'ForeTime',ForeTime,...
				'MinRad',MinRad,...
				'MagRadScale',MagRadScale,...
				'MagRadConst',MagRadConst);

	AlgoInfo.Type='SLIDER-Magnitude-mapseis-v1';
	AlgoInfo.UsedConfig=ParamInput;
	AlgoInfo.CalculationDate=date;
	AlgoInfo.InitialEvents=InitEvent;
	AlgoInfo.WorkArray=WorkArray;

end


function [B_hat,mapping] = train_linear_classifier(X,labels,w)
%TRAIN_LINEAR_CLASSIFIER

 [B_hat,mapping] = linear_classifier('train',X,labels,w,'classification',[]);

end


function varargout = spy(X)
%SPY (overloaded)

 if isa(X,'blkvar')
    X = sdpvar(X);
 end

Z = reshape(sum(abs(X.basis),2),X.dim(1),X.dim(2));
Z = Z~=0;
if nargout==0
    spy(Z)
else
    varargout{1}=Z;
end


% pdfbtrainthmt.m
% written by: Duncan Po
% Date: August 24, 2002
% Using the EM algorithm, train a model for the provided data
% Usage: model = pdfbtrainthmt(tree, levndir, mD, ns,zeromean)
%        [model, stateprob] = pdfbtrainthmt(tree, levndir, mD, ns,zeromean)
%        model = pdfbtrainthmt(tree, levndir, mD, initmodel)
%        [model, stateprob] = pdfbtrainthmt(tree, levndir, mD, initmodel)
% Inputs:   tree        - data in tree structure
%           levndir     - the number of subbands in each level (e.g. [2 2 3 3])
%           mD          - convergence value
%           ns          - number of states in the desired model
%           zeromean    - 'yes' for zero mean model and 'no' for non-zeromean model
%           initmodel   - an initial model can be provided to speed up the training
% Output:   model       - the model generated
%           stateprob   - state probabilities

function [model, stateprob] = pdfbtrainthmt(tree, levndir, mD, ns,zeromean)

if nargout == 2
    needstateprob = 1;
else
    needstateprob = 0;
end;

nlevel = length(tree);

if nargin == 4
    initmodel.nstates = ns.nstates;
    realns = initmodel.nstates;
    initmodel.nlevels = ns.nlevels;
    initmodel.zeromean = ns.zeromean;
    initmodel.rootprob = zeros(1,realns);
    for l = 1:realns
        initmodel.rootprob(l) = ns.rootprob(l);
    end;
    for l = 1:nlevel-1
        for k = 1:2.^(levndir(l+1)-levndir(1))
            initmodel.transprob{l}{k} = zeros(realns);
            for m = 1:realns
                for n = 1:realns
                    initmodel.transprob{l}{k}(m,n) = ns.transprob{l}{k}(m,n);
                end;
            end;
        end;
    end;
    for l = 1:nlevel
        for k = 1:2.^(levndir(l)-levndir(1))
            if strcmp(ns.zeromean, 'yes') == 0
                initmodel.mean{l}{k} = zeros(1,realns);
                for m = 1:realns
                    initmodel.mean{l}{k}(m) = ns.mean{l}{k}(m);
                end;
            end;
            initmodel.stdv{l}{k} = zeros(1, realns);
            for m = 1:realns
                initmodel.stdv{l}{k}(m) = ns.stdv{l}{k}(m);
            end;
        end;
    end;
else
    model.nstates = -2;
    model.nlevels = -1;
    model.zeromean = 0;
    model.rootprob = zeros(1,ns);
    for l = 1:nlevel-1
        for k = 1:2.^(levndir(l+1)-levndir(1))
            model.transprob{l}{k} = zeros(ns);
        end;
    end;
    for l = 1:nlevel
        for k = 1:2.^(levndir(l)-levndir(1))
            if strcmp(zeromean, 'yes') == 0
                model.mean{l}{k} = zeros(1,ns);
            end;
            model.stdv{l}{k} = zeros(1, ns);
        end;
    end;
end;

if ~exist('initmodel', 'var')
    if needstateprob == 0
        pdfbtrain_thmt(ns, nlevel, levndir, zeromean, tree, mD, model);
    else
        for l = 1:nlevel
            numofel = length(tree{l});
            stateprob{l} = zeros(numofel, ns);
        end;
        pdfbtrain_thmt(ns, nlevel, levndir, zeromean, tree, mD, model, stateprob);
    end;
else
    model = initmodel;
    if needstateprob == 0
        pdfbprotrain_thmt(ns, nlevel, levndir, tree ,mD, model);
    else
        ns = initmodel.nstates;
        for l = 1:nlevel
            numofel = length(tree{l});
            stateprob{l} = ones(numofel, ns);
        end;
        pdfbprotrain_thmt(ns, nlevel, levndir, tree, mD, model, stateprob);
    end;
end;


function x_opt = plotInternalModel(internalmodel,x,n,localindex,color,opts)
% Code used by both lmi/plot and optimizer/plot

if isempty(internalmodel.binary_variables)
    [x_opt{1},errorstatus] = generateBoundary(internalmodel,x,n,localindex);
else
    if strcmp(internalmodel.solver.tag,'BNB')
        internalmodel.solver = internalmodel.solver.lower;
    end
    nBin = length(internalmodel.binary_variables);
    p = extractLP(internalmodel);
    p = extractOnly(p,internalmodel.binary_variables);

    internalmodel.F_struc = [zeros(nBin,size(internalmodel.F_struc,2));internalmodel.F_struc];
    I = eye(nBin);
    internalmodel.F_struc(1:nBin,1+internalmodel.binary_variables) = I;
    internalmodel.K.f = internalmodel.K.f + length(internalmodel.binary_variables);
    errorstatus = 1;
    x_opt = {};
    errorstatus = zeros(1,2^nBin);
    for i = 0:2^nBin-1;
        comb = dec2decbin(i,nBin);
        if checkfeasiblefast(p,comb(:),1e-6)
            internalmodel.F_struc(1:nBin,1) = -comb(:);
            [x_temp,wrong] = generateBoundary(internalmodel,x,n,localindex);
            if ~wrong
                errorstatus(i+1) = 0;
                x_opt{end+1} = x_temp;
            end
        else
            errorstatus(i+1)=0;
        end
    end
end

if all(errorstatus)
    if nargout==0
        plot(0);
    end
elseif nargout == 0
    for i = 1:length(x_opt)
        try
            plotSet(x_opt{i},color(1+rem(i-1,size(color,1)),:),opts);
        catch
        end
    end
end

if nargout > 0
    varargout{1} = x_opt;
end

function [xout,errorstatus] = solvefordirection(c,internalmodel,uv)
internalmodel.c = 0*internalmodel.c;
internalmodel.c(uv) = c;
sol  = feval(internalmodel.solver.call,internalmodel);
xout = sol.Primal;
xout = xout(uv(:));
errorstatus = sol.problem;

function p = plotSet(x_opt,color,options)
if size(x_opt,1)==1
    p = line(x_opt,[0 0],'color',color);
    set(p,'LineStyle',options.plot.wirestyle);
    set(p,'LineStyle',options.plot.wirestyle);
    set(p,'LineWidth',options.plot.linewidth);
    set(p,'EdgeColor',options.plot.edgecolor);
    set(p,'Facealpha',options.plot.shade);
elseif size(x_opt,1)==2
    p = patch(x_opt(1,:),x_opt(2,:),color);
    set(p,'LineStyle',options.plot.wirestyle);
    set(p,'LineWidth',options.plot.linewidth);
    set(p,'EdgeColor',options.plot.edgecolor);
    set(p,'Facealpha',options.plot.shade);
else
    try
        K = convhulln(x_opt');
        p = patch('Vertices', x_opt','Faces',K,'FaceColor', color);
    catch
         p = fill3(x_opt(1,:),x_opt(2,:),x_opt(3,:),1);
    end
    set(p,'LineStyle',options.plot.wirestyle);
    set(p,'LineWidth',options.plot.linewidth);
    set(p,'EdgeColor',options.plot.edgecolor);
    set(p,'Facealpha',options.plot.shade);
    lighting gouraud;
    view(3);
    camlight('headlight','infinite');
    camlight('headlight','infinite');
    camlight('right','local');
    camlight('left','local');
end

function [x_opt,errorstatus] = generateBoundary(internalmodel,x,n,localindex);

x_opt = [];
phi = [];
errorstatus = 0;
waitbar_created = 0;
t0 = clock;
waitbar_starts_at = 2;
lastdraw = clock;
try % Try to ensure that we close h
    if length(x)==2
        mu = 0.5;
    else
        mu=1;
    end
    n_ = n;
    n = ceil(mu*n);
   % h = waitbar(0,['Please wait, solving ' num2str(n_) ' problems using ' internalmodel.solver.tag]);
    angles = (0:(n))*2*pi/n;
    if length(x)==2
        c = [cos(angles);sin(angles)];
    elseif length(x) == 1
        c = [-1 1];n = 2;
    else
        c = randn(3,n);
    end
    i=1;
    while i<=n & errorstatus ~=1
        [xi,errorstatus] = solvefordirection(c(:,i),internalmodel,localindex(:));
        if errorstatus == 2
            disp('Discovered unbounded direction. You should add bounds on variables')
        elseif errorstatus == 12
            [xi,errorstatus] = solvefordirection(0*c(:,i),internalmodel,localindex(:));
            if errorstatus == 0
                errorstatus = 2;
                disp('Discovered unbounded direction. You should add bounds on variables')
            end
        end
        x_opt = [x_opt xi];
        if ~waitbar_created
            if etime(clock,t0)>waitbar_starts_at;
             h = waitbar(0,['Please wait, solving ' num2str(n_) ' problems using ' internalmodel.solver.tag]);
             waitbar_created = 1;
            end
        end
        if waitbar_created & etime(clock,lastdraw)>1/10
            waitbar(i/n_,h)
            lastdraw = clock;
        end
        i=i+1;
    end

    if errorstatus==0 & length(x)==2
        % Close the set
        x_opt = [x_opt x_opt(:,1)];

        % Add points adaptively
        pick = 1;
        n = floor((1-mu)*n_);
        for i = 1:1:n
            for j= 1:(size(x_opt,2)-1)
                d = x_opt(:,j)-x_opt(:,j+1);
                distance(j,1) = d'*d;
            end
            [dist,pos]=sort(-distance);
            % Select insertion point
            phii=(angles(pos(pick))+angles(pos(pick)+1))/2;
            xi = solvefordirection([cos(phii);sin(phii)],internalmodel,localindex);
            d1=xi-x_opt(:,pos(pick));
            d2=xi-x_opt(:,pos(pick)+1);
            if d1'*d1<1e-3 | d2'*d2<1e-3
                pick = pick+1;
            else
                angles = [angles(1:pos(pick)) phii  angles((pos(pick))+1:end)];
                x_opt = [x_opt(:,1:pos(pick)) xi  x_opt(:,(pos(pick))+1:end)];
            end
            if ~waitbar_created
            if etime(clock,t0)>waitbar_starts_at;
                h = waitbar(0,['Please wait, solving ' num2str(n_) ' problems using ' internalmodel.solver.tag]);
                waitbar_created = 1;
            end
            end
            if waitbar_created
            waitbar((ceil(n_*mu)+i)/n_,h);
            end
        end
    end
    if waitbar_created
    close(h);
    end
catch
    if waitbar_created
    close(h);
    end
end

function pLP = extractLP(p)
pLP = p;
pLP.F_struc = pLP.F_struc(1:p.K.f+p.K.l,:);
pLP.K.q = 0;
pLP.K.s = 0;

function pRed = extractOnly(p,these)
pRed = p;
p.F_struc(:,1+these) = 0;
removeEQ = find(any(p.F_struc(1:pRed.K.f,2:end),2));
removeLP = find(any(p.F_struc(1+pRed.K.f:end,2:end),2));
pRed.F_struc(pRed.K.f+removeLP,:)=[];
pRed.F_struc(removeEQ,:)=[];
pRed.K.f = pRed.K.f - length(removeEQ);
pRed.K.l = pRed.K.l - length(removeLP);
pRed.F_struc = pRed.F_struc(:,[1 1+these]);
pRed.lb = pRed.lb(these);
pRed.ub = pRed.ub(these);


function imdata = mcmcComputeImageData(im, imsegs)

[imh, imw] = size(imsegs.segimage);
imdata.yim = 1-repmat([(0:imh-1)/(imh-1)]', 1, imw);
imdata.xim = repmat([(0:imw-1)/(imw-1)], imh, 1);

[gx, gy] = gradient(im);
imdata.gradim = sqrt(sum(gx.^2 + gy.^2, 3));

minEdgeLen = sqrt(imh^2+imw^2)*0.02;
[vpdata.lines, vpdata.spinfo] = ...
    APPgetLargeConnectedEdges(rgb2gray(im), minEdgeLen, imsegs);
[vpdata.v, vpdata.vars, vpdata.p, vpdata.hpos] = ...
    APPestimateVp(vpdata.lines, [imh imw], 0);

imdata.vpdata = vpdata;

% get pixels in each superpixe
stats = regionprops(imsegs.segimage, 'PixelIdxList');
imdata.pixlist = {stats.PixelIdxList};

% get boundary pixels for each superpixel
[tx, ty] = gradient(double(imsegs.segimage));
segedgeim = (tx~=0) | (ty ~=0);
stats = regionprops(double(imsegs.segimage).*segedgeim, 'PixelIdxList');
imdata.bndpixlist = {stats.PixelIdxList};
imdata.bndnpix = zeros(size(imdata.bndpixlist));
for s = 1:imsegs.nseg
    try
        imdata.bndnpix(s) = numel(imdata.bndpixlist{s});
    catch
        imdata.bndnpix(s) = 0;
    end
end

% get a random subset of pixels and boundary pixels to speed computation of
% some features
for s = 1:imsegs.nseg
    npix = imsegs.npixels(s);
    nbndpix = imdata.bndnpix(s);
    imdata.nsmpix(s) = min(npix, 1000);
    imdata.nsmbndpix(s) = floor(nbndpix/4);
    if npix>1000
        rind = randperm(npix);
        imdata.smpixlist{s} = imdata.pixlist{s}(rind(1:1000));
    else
        imdata.smpixlist(s) = imdata.pixlist(s);
    end
    rind = randperm(nbndpix);
    try
        imdata.smbndpixlist{s} = imdata.bndpixlist{s}(rind(1:imdata.nsmbndpix(s)));
    catch
        imdata.smbndpixlist{s} = [];
    end
end


function output = callmosek(model)

% Retrieve needed data
options = model.options;
F_struc = model.F_struc;
c       = model.c;
Q       = model.Q;
K       = model.K;
x0      = model.x0;
integer_variables = model.integer_variables;
binary_variables = model.binary_variables;
extended_variables = model.extended_variables;
ub      = model.ub;
lb      = model.lb;
mt      = model.monomtable;

% *********************************
% What type of variables do we have
% *********************************
model.linear_variables = find((sum(abs(mt),2)==1) & (any(mt==1,2)));
model.nonlinear_variables = setdiff((1:size(mt,1))',model.linear_variables);
model.sigmonial_variables = find(any(0>mt,2) | any(mt-fix(mt),2));

% Some meta-solver thinks we handle binaries
if ~isempty(model.binary_variables)
    integer_variables = union(model.integer_variables, model.binary_variables);
    if isempty(lb)
        lb = repmat(-inf,1,length(model.c));
    end
    lb(model.binary_variables) = max(lb(model.binary_variables),0);
    if isempty(ub)
        ub = repmat(inf,1,length(model.c));
    end
    ub(model.binary_variables) = min(ub(model.binary_variables),1);
    model.lb = lb;
    model.ub = ub;
    model.integer_variables = integer_variables;
end

% Some meta solvers might construct model with empty cones
if any(model.K.s) && any(model.K.s == 0)
    model.K.s(model.K.s==0)=[];
end
if any(model.K.q) && any(model.K.q == 0)
    model.K.q(model.K.q==0)=[];
end

if ~isempty(model.sigmonial_variables) | isequal(model.solver.version,'GEOMETRIC')
    [x,D_struc,problem,r,res,solvertime,prob] = call_mosek_geometric(model);
else
    [x,D_struc,problem,r,res,solvertime,prob] = call_mosek_lpqpsocpsdp(model);
end

% YALMIP has introduced internal variables for socp/exp cones etc
if length(x) > 0 && length(x) ~= length(model.c)
    x = x(1:length(model.c));
end

infostr = yalmiperror(problem,'MOSEK');

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

% Save all data from the solver?
if options.savesolveroutput
    solveroutput.r = r;
    solveroutput.res = res;

else
    solveroutput = [];
end

% Standard interface
output = createOutputStructure(x,D_struc,[],problem,infostr,solverinput,solveroutput,solvertime);

function [x,D_struc,problem,r,res,solvertime,prob] = call_mosek_lpqpsocpsdp(model);

[model,output] = normalizeExponentialCone(model);
if output.problem
    problem = output.problem;
    x = [];
    D_struc = [];
    r = [];
    res = [];
    solvertime = 0;
    prob = [];
else
    if nnz(model.Q)==0 && isempty(model.integer_variables) && isempty(model.x0)
        % Standard cone problem which we can model by sending our standard dual
        % and then recover solution via Moseks dual
        [x,D_struc,problem,r,res,solvertime,prob] = call_mosek_dual(model);
    else
        % Integer conic program
        % Quadratic objective
        % Exponential cones
        [x,D_struc,problem,r,res,solvertime,prob] = call_mosek_primal(model);
    end
end


% ----------------------------------------------------------------------------
%
%         Convert Allen CCF indices to Franklin-Paxinos labels
%
%   Based on data from Chon et al. Enhanced and unified anatomical labeling
%   for a common mouse brain atlas (2020).
%
% ----------------------------------------------------------------------------

%% Start with coordinates within the Allen CCF mouse brain atlas
% in the form [AP1, DV1, ML1
%              AP2, DV2, ML2]

% This example is of points along a neuropixels probe track through cortex, SC, PAG
brain_points =    [893   189   475
                   890    57   454
                   891   114   468
                   922   156   482
                   923   114   472
                   920   215   492
                   920   268   505
                   954   199   479
                   948   256   491
                   943   312   508
                   938   369   525
                   932   421   537
                   928   471   551
                   951   281   507
                   944   342   522
                   937   400   538
                   931   450   551];

% directory of reference files
annotation_volume_location = 'C:\Drive\Histology\annotation_volume_10um_by_index.npy'; % from the allen inst (see readme)
structure_tree_location = 'C:\Drive\Histology\structure_tree_safe_2017.csv'; % located in github repo
CCF_to_FP_location =  'C:\Drive\Histology\CCF_to_FP.csv'; % located in github repo
FP_table_location = 'C:\Drive\Histology\FP_table_Chon_2020.csv'; % located in github repo
chon_images_loc = 'C:\Drive\Histology\Suppl_File1_Labels'; % from chon et al (supplementary data 4, https://www.nature.com/articles/s41467-019-13057-w)

% generate values for pixel-to-coordinate transformation
bregma = allenCCFbregma(); % estimated bregma position in reference data space
atlas_resolution = 0.010; % pixels to mm

% should the brain image be dark or light
black_brain = true;
brain_points_color = [.5 .5 1];

%% load the reference brain annotations
if ~exist('av','var') || ~exist('st','var')
    disp('loading reference atlas...')
    av = readNPY(annotation_volume_location);
    st = loadStructureTree(structure_tree_location);
end
if ~exist('CCFtoFPtable','var') || ~exist('FPtable','var')
    disp('loading CCF-FP lookup tables...')
    CCFtoFPtable = loadCCFtoFP(CCF_to_FP_location);
    FPtable = loadFPtable(FP_table_location);
end

% initialize array of region annotations
annotation_CCF = cell(size(brain_points,1),3);
annotation_FP = cell(size(brain_points,1),3);

%% process data

% loop through every point to get ROI locations and region annotations
for point = 1:size(brain_points,1)

    % find the annotation, name, and acronym of the current point from
    % Allen CCF data
    ann = av(brain_points(point,1),brain_points(point,2),brain_points(point,3));
    name = st.safe_name{ann};
    acr = st.acronym{ann};

    annotation_CCF{point,1} = ann;
    annotation_CCF{point,2} = name;
    annotation_CCF{point,3} = acr;

    % find the annotation, name, and acronym of the current ROI pixel
    % using Chon et al data synthesizing CCF and Franklin-Paxinos
    [ann_FP, name_FP, acr_FP] = CCF_to_FP(brain_points(point,1), brain_points(point,2), brain_points(point,3), ...
                                          CCFtoFPtable, FPtable, chon_images_loc);

    annotation_FP{point,1} = ann_FP;
    annotation_FP{point,2} = name_FP;
    annotation_FP{point,3} = acr_FP;

end

% get coordinates relative to bregm\
ap = -(brain_points(:,1)-bregma(1))*atlas_resolution;
dv = (brain_points(:,2)-bregma(2))*atlas_resolution;
ml = (brain_points(:,3)-bregma(3))*atlas_resolution;

% generate table
data_table = table(annotation_CCF(:,2),annotation_CCF(:,3), annotation_FP(:,2),annotation_FP(:,3),...
                        ap,dv,ml, annotation_CCF(:,1), annotation_FP(:,1),...
         'VariableNames', {'CCF_name', 'CCF_abbrv', 'FP_name', 'FP_abbrv', 'AP_location', 'DV_location', 'ML_location', 'CCF_index', 'FP_index'});

%% display results

% plot points on the wire frame brain
fwireframe = plotBrainGrid([], [], [], black_brain); hold on;
fwireframe.InvertHardcopy = 'off';
figure(fwireframe); hold on
hp = plot3(brain_points(:,1), brain_points(:,3), brain_points(:,2), '.','linewidth',2, 'color',brain_points_color,'markers',10);

% display table
disp(data_table)


% Test file for ShortTimeFourierTransformFrameComputer computer
%
% Copyright (c) 2018 Department of Computer Science,
%                    University of Toronto, Canada,
%                    Vector Institute, Canada
%
% License
% This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Yingxue Wang <yingxue@cs.toronto.edu>
%  Sean Robertson <sdrobert@cs.toronto.edu>
%

classdef test_compute < matlab.unittest.TestCase
    properties
        computer
    end

    properties (TestParameter)
        bank = {GaborFilterBank(MelScaling())}
        frame_length_ms = {25}
        frame_shift_ms = {10}
        frame_style = {'causal'} %{'causal', 'centered'}
        include_energy = {true}  %{true, false}
        pad_to_nearest_power_of_two = {true} % {true, false}
        use_log = {true} %{true, false}
        use_power = {true} %{true, false}
        buff_len = {0, 1, 2 ^ 8, 2^10} %{ 0, 1, 2 ^ 8, 2 ^ 10} % empty buffer', 'length 1 buffer', 'medium buffer', 'large buffer'
    end

    methods (Test)
        % Test validility of the function
        function test_validate(testCase,...
                bank, ...
                frame_length_ms, ...
                frame_shift_ms, ...
                frame_style, ...
                include_energy, ...
                pad_to_nearest_power_of_two, ...
                use_log, ...
                use_power)

            testCase.computer = ShortTimeFourierTransformFrameComputer(...
                                        bank, ...
                                        frame_length_ms, ...
                                        frame_shift_ms, ...
                                        frame_style, ...
                                        include_energy, ...
                                        pad_to_nearest_power_of_two, ...
                                        use_log, ...
                                        use_power);
            testCase.verifyTrue(testCase.computer.isvalid);
        end

        function test_framewise_matches_full(testCase,...
                bank, ...
                frame_length_ms, ...
                frame_shift_ms, ...
                frame_style, ...
                include_energy, ...
                pad_to_nearest_power_of_two, ...
                use_log, ...
                use_power,...
                buff_len)
            % This test might fail due to minor numerical error

            % Create buffer
            buff = rand(buff_len,1);

            % Create Computer
            testCase.computer = ShortTimeFourierTransformFrameComputer(...
                                        bank, ...
                                        frame_length_ms, ...
                                        frame_shift_ms, ...
                                        frame_style, ...
                                        include_energy, ...
                                        pad_to_nearest_power_of_two, ...
                                        use_log, ...
                                        use_power);

            feats_full = testCase.computer.compute_full(buff);
            feats_framewise = Util.frame_by_frame_calculation(testCase.computer, buff);
            testCase.verifyEqual(feats_full, feats_framewise, 'RelTol', 1);
        end

        function test_zero_samples_generate_zero_features(testCase,...
                bank, ...
                frame_length_ms, ...
                frame_shift_ms, ...
                frame_style, ...
                include_energy, ...
                pad_to_nearest_power_of_two, ...
                use_log, ...
                use_power)
            % Create Computer
            testCase.computer = ShortTimeFourierTransformFrameComputer(...
                                        bank, ...
                                        frame_length_ms, ...
                                        frame_shift_ms, ...
                                        frame_style, ...
                                        include_energy, ...
                                        pad_to_nearest_power_of_two, ...
                                        use_log, ...
                                        use_power);

            testCase.verifyEqual(size(testCase.computer.compute_full(double.empty(0))), [0 testCase.computer.num_coeffs]);
            testCase.verifyEqual(size(testCase.computer.compute_chunk(double.empty(0))), [0  testCase.computer.num_coeffs]);
            testCase.verifyEqual(size(testCase.computer.finalize()), [0 testCase.computer.num_coeffs]);
        end

        function test_finalize_twice_generates_no_coefficients(testCase,...
                bank, ...
                frame_length_ms, ...
                frame_shift_ms, ...
                frame_style, ...
                include_energy, ...
                pad_to_nearest_power_of_two, ...
                use_log, ...
                use_power)
            % Create Computer
            testCase.computer = ShortTimeFourierTransformFrameComputer(...
                                        bank, ...
                                        frame_length_ms, ...
                                        frame_shift_ms, ...
                                        frame_style, ...
                                        include_energy, ...
                                        pad_to_nearest_power_of_two, ...
                                        use_log, ...
                                        use_power);
            buff = rand(testCase.computer.frame_length * 2, 1);
            coeffs = [...
                testCase.computer.compute_chunk(buff); ...
                testCase.computer.finalize()...
                ];

            shape_coeffs = size(coeffs);
            testCase.verifyGreaterThanOrEqual(shape_coeffs(1), 1);
            shape_finalize = size(testCase.computer.finalize());
            testCase.verifyEqual(shape_finalize, [0 testCase.computer.num_coeffs]);
        end

        function test_repeated_calls_generate_same_results(testCase,...
                bank, ...
                frame_length_ms, ...
                frame_shift_ms, ...
                frame_style, ...
                include_energy, ...
                pad_to_nearest_power_of_two, ...
                use_log, ...
                use_power, ...
                buff_len)

            % Create buffer
            buff = rand(buff_len,1);

            % Create Computer
            testCase.computer = ShortTimeFourierTransformFrameComputer(...
                                        bank, ...
                                        frame_length_ms, ...
                                        frame_shift_ms, ...
                                        frame_style, ...
                                        include_energy, ...
                                        pad_to_nearest_power_of_two, ...
                                        use_log, ...
                                        use_power);
            testCase.verifyEqual(testCase.computer.compute_full(buff), ...
                testCase.computer.compute_full(buff));
            testCase.verifyEqual(...
                Util.frame_by_frame_calculation(testCase.computer, buff), ...
                Util.frame_by_frame_calculation(testCase.computer, buff));
        end

    end
end


%  This scriptfile ask for several input parameters that can be setup
%  at the beginning of each session. The default values are the
%  extrema in the catalog
%
%a = org;        % resets the main catalogue "a" to initial state

%special version of the inpu script for Mapseis, it does not open a
%parameter selection window is the catalog should be already filtered.

report_this_filefun(mfilename('fullpath'));

%  default values
t0b = min(a.Date);
teb = max(a.Date);
tdiff = (teb - t0b)*365;

if ~exist('par1','var')
    %  if tdiff>10                 %select bin length respective to time in catalog
    %     par1 = ceil(tdiff/100);
    %  elseif tdiff<=10 & tdiff>1
    %     par1 = 0.1;
    %  elseif tdiff<=1
    %     par1 = 0.01;
    %  end
    par1 = 30;
end

minmag = max(a.Magnitude) -0.2;
dep1 = 0.3*max(a.Depth);
dep2 = 0.6*max(a.Depth);
dep3 = max(a.Depth);
minti = min(a.Date);
maxti  = max(a.Date);
minma = min(a.Magnitude);
maxma = max(a.Magnitude);
mindep = min(a.Depth);
maxdep = max(a.Depth);

think;
sele_sub;


% this script tests NGLDM features between CERR and pyradiomics.
%
% RKP, 03/22/2018

%% load image
gldmParamFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','tests_for_cerr','test_ngldm_radiomics_extraction_settings.json');
cerrFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','data_for_cerr_tests','CERR_plans','head_neck_ex1_20may03.mat.bz2');

planC = loadPlanC(cerrFileName,tempdir);
indexS = planC{end};

paramS = getRadiomicsParamTemplate(gldmParamFileName);
strNum = getMatchingIndex(paramS.structuresC{1},{planC{indexS.structures}.structureName});
scanNum = getStructureAssociatedScan(strNum,planC);

%% NGLDM features CERR

ngldmS = calcGlobalRadiomicsFeatures...
            (scanNum, strNum, paramS, planC);
ngldmS = ngldmS.Original.ngldmFeatS;

cerrNgldmV = [ngldmS.lde, ngldmS.hde, ngldmS.lgce, ngldmS.hgce, ...
    ngldmS.ldlge, ngldmS.ldhge, ngldmS.hdlge, ngldmS.hdhge, ...
    ngldmS.gln, ngldmS.glnNorm, ngldmS.dcn, ngldmS.dcnNorm,...
    ngldmS.dcp, ngldmS.glv, ngldmS.dcv, ngldmS.entropy, ngldmS.energy];

% %% Calculate features using pyradiomics
%
% testM = single(planC{indexS.scan}(scanNum).scanArray) - ...
%     single(planC{indexS.scan}(scanNum).scanInfo(1).CTOffset);
% mask3M = zeros(size(testM),'logical');
% [rasterSegments, planC, isError] = getRasterSegments(strNum,planC);
% [maskBoundBox3M, uniqueSlices] = rasterToMask(rasterSegments, scanNum, planC);
% mask3M(:,:,uniqueSlices) = maskBoundBox3M;
%
% scanType = 'original';
%
% dx = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dy = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dz = mode(diff([planC{indexS.scan}(scanNum).scanInfo(:).zValue]));
% pixelSize = [dx dy dz]*10;
%
% teststruct = PyradWrapper(testM, mask3M, pixelSize, scanType, dirString);
%
% %teststruct = PyradWrapper(testM, mask3M, scanType);
%
% pyradNgldmNamC = {'SmallDependenceEmphasis', 'LargeDependenceEmphasis',...
%     'LowGrayLevelCountEmphasis', 'HighGrayLevelCountEmphasis',  'SmallDependenceLowGrayLevelEmphasis', ...
%     'SmallDependenceHighGrayLevelEmphasis', 'LargeDependenceLowGrayLevelEmphasis', ...
%     'LargeDependenceHighGrayLevelEmphasis', 'GrayLevelNonUniformity', 'GrayLevelNonUniformityNorm', ...
%     'DependenceNonUniformity', 'DependenceNonUniformityNormalized', ...
%     'DependencePercentage', 'GrayLevelVariance', 'DependenceVariance', ...
%     'DependenceEntropy', 'DependenceEnergy'};
%
%
% pyradNgldmNamC = strcat(['original', '_gldm_'],pyradNgldmNamC);
%
% pyRadNgldmV = [];
% for i = 1:length(pyradNgldmNamC)
%     if isfield(teststruct,pyradNgldmNamC{i})
%         pyRadNgldmV(i) = teststruct.(pyradNgldmNamC{i});
%     else
%         pyRadNgldmV(i) = NaN;
%     end
% end
%
% %% Compare
% ngldmDiffV = (cerrNgldmV - pyRadNgldmV) ./ cerrNgldmV * 100

%% Compare with previously calculated pyradiomics features
saved_pyRadNgldmV = [0.177896163786327,80.7514028586554,NaN,NaN,0.00117423243903225,380.447741305586,3.84549682517831,136232.842773954,1250.78930651138,NaN,634.668395976707,0.0671962303839817,NaN,122.726429080550,26.6319748251348,7.44897904303236,NaN];
ngldmDiffV = (cerrNgldmV - saved_pyRadNgldmV) ./ cerrNgldmV * 100


% Use the VAD stream (the second stream) to select speech frames in the first stream
% Assume the input data is a matrix of DxT size.
function output = SynchronizeStreamsVAD(data_in, action)
data = data_in{1};
vad = data_in{2};
[D,nFr] = size(data);
precision = class(gather(data_in{1}(1,1,1)));

words = ExtractWordsFromString_v2(action);
action = words{1};

% first
switch lower(action)
    case 'concatenation'
        [n1,n2,n3] = size(data);
        if n3>1
            nFr = n3;
        else
            nFr = n2;   % this would be erronous if the input is a tensor with just one frame. But this should be almost impossible as we will not use vad in this case.
        end
        vad(nFr+1:end) = [];
        if n3>1
            output = data(:,:,vad==1);
        else
            output = data(:,vad==1);
        end

    case 'segmentation'
        seglen = str2num(words{2});
        segshift = str2num(words{3});
        vad_seg = label2seg(vad);
        output_tmp = {};
        for j=1:length(vad_seg.label)
            if vad_seg.label(j)==0; continue; end
            if vad_seg.stop(j)-vad_seg.start(j) < seglen; continue; end
            start = max(1,vad_seg.start(j));
            stop = min(vad_seg.stop(j), nFr);
            output_tmp{end+1} = DivideSent2Segments(data(:,start:stop), seglen, segshift, 0);
        end
        % move data into a tensor
        nSeg = [];
        for j=1:length(output_tmp)
            nSeg(j) = size(output_tmp{j}, 3);
        end
        if length(nSeg) == 0
            pause(0.1);
        end
        output = zeros(D, seglen, sum(nSeg), precision);
        for j=1:length(output_tmp)
            output(:,:, sum(nSeg(1:j-1))+1 : sum(nSeg(1:j)) ) = output_tmp{j};
        end

    otherwise
        fprintf('SynchronizeStreamsVAD: Error: unknown action %s\n', action);
end

end


function repeatability_generate_images()
  load('data/pascal_voc07_test_annotations.mat', 'impos');
  config = get_config();
  rep_config = repeatability_get_config();

  homographies = [];
  fields = fieldnames(rep_config);
  for field_i = 1:numel(fields)
    homographies.(fields{field_i}) = zeros(numel(impos), 3, 3);
  end

  for im_i=1:numel(impos)
    tic_toc_print('image %d/%d\n', im_i, numel(impos));
    orig_img_id = impos(im_i).im;
    im = imread(sprintf(config.pascal_images, orig_img_id));

    fields = fieldnames(rep_config);
    for field_i = 1:numel(fields)
      %tic_toc_print('running %s\n', fields{field_i});

      sub_config = rep_config.(fields{field_i});
      for i = 1:numel(sub_config.params), param = sub_config.params(i);
        % skip parameters that are not plotted
        if isfield(sub_config, 'display_points')
          if ~ismember(i, sub_config.display_points)
            continue;
          end
        end
        [transformed_im, H] = sub_config.func(im, param);
        homographies.(fields{field_i})(im_i,:,:) = H;

        img_id = sprintf(sub_config.img_id, orig_img_id, i);
        imwrite(transformed_im, sprintf(config.transformed_pascal_images, img_id));
      end
    end
  end

  save('data/pascal_voc07_test_repeatability_homographies.mat', 'homographies');
end


function Data=ImgRead(imgpath,suffixname,nline,bkformat,machinefmt)
%Read data stack from a specified directory.
%   Usage:
%       Data=ImgRead(imgpath,suffixname,nline,bkformat,machinefmt);
%
%
%   Inputs:
%   - imgpath:      The path of image set
%   - suffixname:   The suffix of all files in image set, e.g., mli
%   - nline:        The image height
%   - bkformat:     See freadbkj.m for details
%   - machinefmt:   See freadbkj.m for details
%
%   Outputs:
%   - Data.datastack:   A height by width by page matrix where each page
%                       corresponds to a 2D image
%   - Data.filename:    The file name list.
%
%   Examples:
%   To read a batch of intensity series in float32 format with Big-endian
%   ordering machinefmt, which has a height of 200 lines,use:
%   Data=ImgRead('/home/user/INSAR/COHEST/MLI','mli',200,'float32');
%
%   For complex differential interferogram with height 1800, use:
%   Data=ImgRead('/home/user/INSAR/COHEST/DIFF','diff',1800,'cpxfloat32');
%
%
%   Mi JIANG, Hohai University/The Hong Kong Polytechnic University,

if nargin < 5
    machinefmt='b'; % b - GAMMA software, for example
end

if nargin < 4
    bkformat='float32'; %for *mli,*cc file
end

if nargin < 3
    help ImgRead
    return;
end

if isempty(strmatch(imgpath(end),filesep))
    imgpath=[imgpath,filesep];
end

tag_files = dir([imgpath,'*',suffixname]);
img_num = length(tag_files);
disp(['The number of the ', suffixname,' images:' num2str(img_num)]);

for ii=1:img_num
    tic;
    Data.datastack(:,:,ii)=single(freadbkj([imgpath,tag_files(ii).name],nline,bkformat,machinefmt));
    temp=regexp(tag_files(ii).name,'\d+','match');
    if length(temp)==1     %mli
        Data.filename(ii,1)=str2double(temp{1});
    elseif length(temp)==2 %intf
        Data.filename(ii,1)=str2double(temp{1});
        Data.filename(ii,2)=str2double(temp{2});
    else
        error('The format of file name should be: <yyyymmdd> or <yyyymmdd_yyyymmdd>.')
    end
    time=toc;
    fprintf('Reading Img %d / %d, time = %.0f sec\n',ii,img_num,time);
end


%  When you load any ANALYZE or NIfTI file with 'load_nii.m', and view
%  it with 'view_nii.m', you may find that the image is L-R flipped.
%  This is because of the confusion of radiological and neurological
%  convention in the medical image before NIfTI format is adopted. You
%  can find more details from:
%
%  http://www.rotman-baycrest.on.ca/~jimmy/UseANALYZE.htm
%
%  Sometime, people even want to convert RAS (standard orientation) back
%  to LAS orientation to satisfy the legend programs or processes. This
%  program is only written for those purpose. So PLEASE BE VERY CAUTIOUS
%  WHEN USING THIS 'FLIP_LR.M' PROGRAM.
%
%  With 'flip_lr.m', you can convert any ANALYZE or NIfTI (no matter
%  3D or 4D) file to a flipped NIfTI file. This is implemented simply
%  by flipping the affine matrix in the NIfTI header. Since the L-R
%  orientation is determined there, so the image will be flipped.
%
%  Usage: flip_lr(original_fn, flipped_fn, [old_RGB],[tolerance],[preferredForm])
%
%  original_fn  -  filename of the original ANALYZE or NIfTI (3D or 4D) file
%
%  flipped_fn  -  filename of the L-R flipped NIfTI file
%
%  old_RGB (optional)  -  a scale number to tell difference of new RGB24
%	from old RGB24. New RGB24 uses RGB triple sequentially for each
%	voxel, like [R1 G1 B1 R2 G2 B2 ...]. Analyze 6.0 from AnalyzeDirect
%	uses old RGB24, in a way like [R1 R2 ... G1 G2 ... B1 B2 ...] for
%	each slices. If the image that you view is garbled, try to set
%	old_RGB variable to 1 and try again, because it could be in
%	old RGB24. It will be set to 0, if it is default or empty.
%
%  tolerance (optional) - distortion allowed for non-orthogonal rotation
%	or shearing in NIfTI affine matrix. It will be set to 0.1 (10%),
%	if it is default or empty.
%
%  preferredForm (optional)  -  selects which transformation from voxels
%	to RAS coordinates; values are s,q,S,Q.  Lower case s,q indicate
%	"prefer sform or qform, but use others if preferred not present".
%	Upper case indicate the program is forced to use the specificied
%	tranform or fail loading.  'preferredForm' will be 's', if it is
%	default or empty.	- Jeff Gunter
%
%  Example: flip_lr('avg152T1_LR_nifti.nii', 'flipped_lr.nii');
%           flip_lr('avg152T1_RL_nifti.nii', 'flipped_rl.nii');
%
%  You will find that 'avg152T1_LR_nifti.nii' and 'avg152T1_RL_nifti.nii'
%  are the same, and 'flipped_lr.nii' and 'flipped_rl.nii' are also the
%  the same, but they are L-R flipped from 'avg152T1_*'.
%
%  NIFTI data format can be found on: http://nifti.nimh.nih.gov
%
%  - Jimmy Shen (jimmy@rotman-baycrest.on.ca)
%
function flip_lr(original_fn, flipped_fn, old_RGB, tolerance, preferredForm)

   if ~exist('original_fn','var') | ~exist('flipped_fn','var')
      error('Usage: flip_lr(original_fn, flipped_fn, [old_RGB],[tolerance])');
   end

   if ~exist('old_RGB','var') | isempty(old_RGB)
      old_RGB = 0;
   end

   if ~exist('tolerance','var') | isempty(tolerance)
      tolerance = 0.1;
   end

   if ~exist('preferredForm','var') | isempty(preferredForm)
      preferredForm= 's';				% Jeff
   end

   nii = load_nii(original_fn, [], [], [], [], old_RGB, tolerance, preferredForm);
   M = diag(nii.hdr.dime.pixdim(2:5));
   M(1:3,4) = -M(1:3,1:3)*(nii.hdr.hist.originator(1:3)-1)';
   M(1,:) = -1*M(1,:);
   nii.hdr.hist.sform_code = 1;
   nii.hdr.hist.srow_x = M(1,:);
   nii.hdr.hist.srow_y = M(2,:);
   nii.hdr.hist.srow_z = M(3,:);
   save_nii(nii, flipped_fn);

   return;					% flip_lr


function display(rot,varargin)
% standart output

displayClass(rot,inputname(1),varargin{:});
if length(rot)~=1, disp(['  size: ' size2str(rot)]); end

if length(rot) <= 19 && ~isempty(rot)

  Euler(rot);

elseif ~getMTEXpref('generatingHelpMode')  && ~isempty(rot)

  disp(' ')
  s = setappdata(0,'data2beDisplayed',rot);
  disp(['  show Euler angles'])
  disp(' ')

else

  disp(' ')

end


function im=lpcar2im(ar,np)
%LPCAR2IM Convert AR coefs to impulse response IM=(AR,NP)

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcar2im.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(ar);
if nargin<2 np=p1-1; end
im=zeros(nf,np+1);
x=[1 zeros(1,np)];
for k=1:nf
  im(k,:)=filter(1,ar(k,:),x);
end


function ringMaskM = getSurfaceRing(structNum,radius,planC)
%function mask3M = getSurfaceRing(structNum,radius,planC)
%
%This function returns surface ring of radius "radius" for structNum
%
%APA, 05/19/2020
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('planC','var')
    global planC
end

% Get Surface mask
% mask3M = getStructSurface(structNum,planC);

% Get surface mask of contracted unionStrNum
xyDownsampleIndex = 1; % no downsampling
contractedStrM = getSurfaceContract(structNum,radius/2,xyDownsampleIndex,planC);
expandedStrM = getSurfaceExpand(structNum,radius/2,xyDownsampleIndex,planC);

ringMaskM = expandedStrM - contractedStrM;


function savgenas() % autogenerated function wrapper
% Matlab script to write output from genas to a file.
% writes two files: one for results for magnitudes and below
% another for magnitudes and above.
%
 % turned into function by Celso G Reyes 2017

ZG=ZmapGlobal.Data; % used by get_zmap_globals

report_this_filefun(mfilename('fullpath'));

figure(mess);
clf ;
set(mess,'Name','Messages');
set(gca,'visible','off');
set(mess,'pos',[ 0.02  0.9 0.3 0.2])
format short

[tbin,zmag,zval] = find(ZABO);           % deal with sparse matrix results
xtz = t0b + (tbin*days(ZG.bin_days));
zmag = minmg+(zmag-1)*magstep;
[~,l] = sort(xtz);                     % sort in time
xtz = xtz(l);
zmag = zmag(l);
zval = zval(l);
tbin = tbin(l);
Z = [tbin'; xtz'; zmag'; zval'];

[newmatfile, newpath] = uiputfile(hodi  , 'Above -Save As'); %Syntax change Matlab Version 7, no window positioning on macs
fid = fopen(fullfile(newpath,newmatfile),'w');
fprintf(fid,'%3.0f %4.2f  %3.2f+  %6.4f\n',Z);

[tbin,zmag,zval] = find(ZBEL);
xtz = t0b + (tbin*days(ZG.bin_days));
zmag = minmg+(zmag-1)*magstep;
[xx,l] = sort(xtz);                     % sort in time
xtz = xtz(l);
zmag = zmag(l);
zval = zval(l);
tbin = tbin(l);
Z = [tbin'; xtz'; zmag'; zval'];

[newmatfile, newpath] = uiputfile(hodi, 'Below -Save As'); %Syntax change Matlab Version 7, no window positioning on macs
fid = fopen(fullfile(newpath,newmatfile),'w');
fprintf(fid,'%3.0f %4.2f  %3.2f-  %6.4f\n',Z);

disp('Output was saved in files \newline \newlinebelow.out and above.out,\newline \newlinePlease rename them if desired. ');

pause(5.0);
zmap_message_center();

end


% Test routine for mtimesx, multi-dimensional speed and equality to MATLAB
%******************************************************************************
%
%  MATLAB (R) is a trademark of The Mathworks (R) Corporation
%
%  Function:    mtimesx_test_nd
%  Filename:    mtimesx_test_nd.m
%  Programmer:  James Tursa
%  Version:     1.40
%  Date:        October 4, 2010
%  Copyright:   (c) 2009,2010 by James Tursa, All Rights Reserved
%
%  This code uses the BSD License:
%
%  Redistribution and use in source and binary forms, with or without
%  modification, are permitted provided that the following conditions are
%  met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
%  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
%  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
%  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
%  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
%  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
%  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
%  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
%  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
%  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
%  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
%  POSSIBILITY OF SUCH DAMAGE.
%
%  Syntax:
%
%    A = mtimesx_test_nd    % default n=4 is used
%    A = mtimesx_test_nd(n)
%
%    where n = number of repetitions (should be 4 <= n <= 100)
%
%  Output:
%
%    Prints out speed and equality test results.
%    A = cell array with tabled results.
%
%  2010/Oct/04 --> 1.40, Added OpenMP support for custom code
%                        Expanded sparse * single and sparse * nD support
%
%--------------------------------------------------------------------------

function Cr = mtimesx_test_nd(n)
mtimesx; % load the mex routine into memory
if( nargin == 0 )
    n = 4;
else
    n = floor(n);
    if( ~(n >= 4 && n <= 100) )
        n = 4;
    end
end
cn = sprintf('%g',n);

disp(' ');
disp('MTIMESX multi-dimensional equality and speed tests');
disp('--------------------------------------------------');
disp(' ');
disp('(M x K) * ( K x N) equality tests, SPEED mode, M,K,N <= 4');
trans = 'NGTC';
cmpx = {'real ','cmpx '};
mtimesx('speed');
smallok = true;
for m=1:4
    for k=1:4
        for n=1:4
            for transa=1:4
                if( transa <= 2 )
                    ma = m;
                    ka = k;
                else
                    ma = k;
                    ka = m;
                end
                for transb=1:4
                    if( transb <= 2 )
                        kb = k;
                        nb = n;
                    else
                        kb = n;
                        nb = k;
                    end
                    for cmplxa=1:2
                        if( cmplxa == 1 )
                            A = floor(rand(ma,ka)*100+1);
                        else
                            A = floor(rand(ma,ka)*100+1) + floor(rand(ma,ka)*100+1)*1i;
                        end
                        for cmplxb=1:2
                            if( cmplxb == 1 )
                                B = floor(rand(kb,nb)*100+1);
                            else
                                B = floor(rand(kb,nb)*100+1) + floor(rand(kb,nb)*100+1)*1i;
                            end
                            Cm = mtimesx_sparse(A,trans(transa),B,trans(transb));
                            Cx = mtimesx(A,trans(transa),B,trans(transb));
                            if( isequal(Cm,Cx) )
                                disp(['(' cmpx{cmplxa} num2str(m) ' x ' num2str(k) ')' trans(transa) ...
                                        ' * (' cmpx{cmplxb} num2str(k) ' x ' num2str(n) ')' trans(transb) '  EQUAL']);
                            else
                                disp(['(' cmpx{cmplxa} num2str(m) ' x ' num2str(k) ')' trans(transa) ...
                                        ' * (' cmpx{cmplxb} num2str(k) ' x ' num2str(n) ')' trans(transb) '  NOT EQUAL']);
                                smallok = false;
                            end
                        end
                    end
                end
            end
        end
    end
end

if( mtimesx('openmp') )
disp(' ');
disp('(M x K) * ( K x N) equality tests, SPEEDOMP mode, M,K,N <= 4');
mtimesx('speedomp');
smallokomp = true;
for m=1:4
    for k=1:4
        for n=1:4
            for transa=1:4
                if( transa <= 2 )
                    ma = m;
                    ka = k;
                else
                    ma = k;
                    ka = m;
                end
                for transb=1:4
                    if( transb <= 2 )
                        kb = k;
                        nb = n;
                    else
                        kb = n;
                        nb = k;
                    end
                    for cmplxa=1:2
                        if( cmplxa == 1 )
                            A = floor(rand(ma,ka)*100+1);
                        else
                            A = floor(rand(ma,ka)*100+1) + floor(rand(ma,ka)*100+1)*1i;
                        end
                        A = reshape(repmat(A,1000,1),ma,ka,1000);
                        for cmplxb=1:2
                            if( cmplxb == 1 )
                                B = floor(rand(kb,nb)*100+1);
                            else
                                B = floor(rand(kb,nb)*100+1) + floor(rand(kb,nb)*100+1)*1i;
                            end
                            B = reshape(repmat(B,1000,1),kb,nb,1000);
                            Cm = mtimesx_sparse(A(:,:,1),trans(transa),B(:,:,1),trans(transb));
                            Cx = mtimesx(A,trans(transa),B,trans(transb));
                            if( isequal(Cm,Cx(:,:,1)) )
                                disp(['(' cmpx{cmplxa} num2str(m) ' x ' num2str(k) ')' trans(transa) ...
                                        ' * (' cmpx{cmplxb} num2str(k) ' x ' num2str(n) ')' trans(transb) '  EQUAL']);
                            else
                                disp(['(' cmpx{cmplxa} num2str(m) ' x ' num2str(k) ')' trans(transa) ...
                                        ' * (' cmpx{cmplxb} num2str(k) ' x ' num2str(n) ')' trans(transb) '  NOT EQUAL']);
                                smallokomp = false;
                            end
                        end
                    end
                end
            end
        end
    end
end
end

disp(' ');
if( smallok )
    disp('All small matrix multiplies are OK in SPEED mode');
else
    disp('ERROR --> One or more of the small matrix multiplies was not equal in SPEED mode');
end
if( mtimesx('openmp') )
if( smallokomp )
    disp('All small matrix multiplies are OK in SPEEDOMP mode');
else
    disp('ERROR --> One or more of the small matrix multiplies was not equal in SPEEDOMP mode');
end
end

disp(' ');
disp(['mtimesx multi-dimensional test routine using ' cn ' repetitions']);

if( mtimesx('OPENMP') )
    topm = 6;
else
    topm = 4;
end
Cr = cell(6,topm+1);
Cr{1,1} = 'All operands real';

for m=2:topm+1
if( m == 2 )
    mtimesx('BLAS');
elseif( m == 3 )
    mtimesx('LOOPS');
elseif( m == 4 )
    mtimesx('MATLAB');
elseif( m == 5 )
    mtimesx('SPEED');
elseif( m == 6 )
    mtimesx('LOOPSOMP');
else
    mtimesx('SPEEDOMP');
end
Cr{1,m} = mtimesx;

disp(' ');
disp('--------------------------------------------------------------');
disp('--------------------------------------------------------------');
disp(' ');
disp(['MTIMESX mode: ' mtimesx]);
disp(' ');
disp('(real 3x5x1x4x3x2x1x8) * (real 5x7x3x1x3x2x5) example');
Cr{2,1} = '(3x5xND) *(5x7xND)';
A = rand(3,5,1,4,3,2,1,8);
B = rand(5,7,3,1,3,2,5);
% mtimes
tm = zeros(1,n);
for k=1:n
clear Cm
A(1) = 2*A(1);
B(1) = 2*B(1);
tic
Cm = zeros(3,7,3,4,3,2,5,8);
for k1=1:3
    for k2=1:4
        for k3=1:3
            for k4=1:2
                for k5=1:5
                    for k6=1:8
                        Cm(:,:,k1,k2,k3,k4,k5,k6) = A(:,:,1,k2,k3,k4,1,k6) * B(:,:,k1,1,k3,k4,k5);
                    end
                end
            end
        end
    end
end
tm(k) = toc;
end
% mtimesx
tx = zeros(1,n);
for k=1:n
clear Cx
tic
Cx = mtimesx(A,B);
tx(k) = toc;
end
% results
tm = median(tm);
tx = median(tx);
if( tx < tm )
    faster = sprintf('%7.1f',100*(tm)/tx-100);
    slower = '';
else
    faster = sprintf('%7.1f',-(100*(tx)/tm-100));
    slower = ' (i.e., slower)';
end
Cr{2,m} = faster;
disp(' ');
disp(['mtimes  Elapsed time ' num2str(tm) ' seconds.']);
disp(['MTIMESX Elapsed time ' num2str(tx) ' seconds.']);
disp(['MTIMESX ' mtimesx ' mode is ' faster '% faster than MATLAB mtimes' slower])
if( isequal(Cx,Cm) )
    disp(['MTIMESX ' mtimesx ' mode result matches mtimes:  EQUAL'])
else
    dx = max(abs(Cx(:)-Cm(:)));
    disp(['MTIMESX ' mtimesx ' mode result does not match mtimes:  NOT EQUAL , max diff = ' num2str(dx)])
end

disp(' ');
disp('--------------------------------------------------------------');
disp('(real 3x3x1000000) * (real 3x3x1000000) example');
Cr{3,1} = '(3x3xN) *(3x3xN)';
A = rand(3,3,1000000);
B = rand(3,3,1000000);
% mtimes
tm = zeros(1,n);
for k=1:n
clear Cm
A(1) = 2*A(1);
B(1) = 2*B(1);
tic
Cm = zeros(3,3,1000000);
for k1=1:1000000
    Cm(:,:,k1) = A(:,:,k1) * B(:,:,k1);
end
tm(k) = toc;
end
% mtimesx
tx = zeros(1,n);
for k=1:n
clear Cx
tic
Cx = mtimesx(A,B);
tx(k) = toc;
end
% results
tm = median(tm);
tx = median(tx);
if( tx < tm )
    faster = sprintf('%7.1f',100*(tm)/tx-100);
    slower = '';
else
    faster = sprintf('%7.1f',-(100*(tx)/tm-100));
    slower = ' (i.e., slower)';
end
Cr{3,m} = faster;
disp(' ');
disp(['mtimes  Elapsed time ' num2str(tm) ' seconds.']);
disp(['MTIMESX Elapsed time ' num2str(tx) ' seconds.']);
disp(['MTIMESX ' mtimesx ' mode is ' faster '% faster than MATLAB mtimes' slower])
if( isequal(Cx,Cm) )
    disp(['MTIMESX ' mtimesx ' mode result matches mtimes:  EQUAL'])
else
    dx = max(abs(Cx(:)-Cm(:)));
    disp(['MTIMESX ' mtimesx ' mode result does not match mtimes:  NOT EQUAL , max diff = ' num2str(dx)])
end

disp(' ');
disp('--------------------------------------------------------------');
disp('(real 2x2x2000000) * (real 2x2x2000000) example');
Cr{4,1} = '(2x2xN) *(2x2xN)';
A = rand(2,2,2000000);
B = rand(2,2,2000000);
% mtimes
tm = zeros(1,n);
for k=1:n
clear Cm
A(1) = 2*A(1);
B(1) = 2*B(1);
tic
Cm = zeros(2,2,2000000);
for k1=1:2000000
    Cm(:,:,k1) = A(:,:,k1) * B(:,:,k1);
end
tm(k) = toc;
end
% mtimesx
tx = zeros(1,n);
for k=1:n
clear Cx
tic
Cx = mtimesx(A,B);
tx(k) = toc;
end
% results
tm = median(tm);
tx = median(tx);
if( tx < tm )
    faster = sprintf('%7.1f',100*(tm)/tx-100);
    slower = '';
else
    faster = sprintf('%7.1f',-(100*(tx)/tm-100));
    slower = ' (i.e., slower)';
end
Cr{4,m} = faster;
disp(' ');
disp(['mtimes  Elapsed time ' num2str(tm) ' seconds.']);
disp(['MTIMESX Elapsed time ' num2str(tx) ' seconds.']);
disp(['MTIMESX ' mtimesx ' mode is ' faster '% faster than MATLAB mtimes' slower])
if( isequal(Cx,Cm) )
    disp(['MTIMESX ' mtimesx ' mode result matches mtimes:  EQUAL'])
else
    dx = max(abs(Cx(:)-Cm(:)));
    disp(['MTIMESX ' mtimesx ' mode result does not match mtimes:  NOT EQUAL , max diff = ' num2str(dx)])
end

disp(' ');
disp('--------------------------------------------------------------');
disp('(real 2x2x2000000) * (real 1x1x2000000) example');
Cr{5,1} = '(2x2xN) *(1x1xN)';
A = rand(2,2,2000000);
B = rand(1,1,2000000);
% mtimes
tm = zeros(1,n);
for k=1:n
clear Cm
A(1) = 2*A(1);
B(1) = 2*B(1);
tic
Cm = zeros(2,2,2000000);
for k1=1:2000000
    Cm(:,:,k1) = A(:,:,k1) * B(:,:,k1);
end
tm(k) = toc;
end
% mtimesx
tx = zeros(1,n);
for k=1:n
clear Cx
tic
Cx = mtimesx(A,B);
tx(k) = toc;
end
% results
tm = median(tm);
tx = median(tx);
if( tx < tm )
    faster = sprintf('%7.1f',100*(tm)/tx-100);
    slower = '';
else
    faster = sprintf('%7.1f',-(100*(tx)/tm-100));
    slower = ' (i.e., slower)';
end
Cr{5,m} = faster;
disp(' ');
disp(['mtimes  Elapsed time ' num2str(tm) ' seconds.']);
disp(['MTIMESX Elapsed time ' num2str(tx) ' seconds.']);
disp(['MTIMESX ' mtimesx ' mode is ' faster '% faster than MATLAB mtimes' slower])
if( isequal(Cx,Cm) )
    disp(['MTIMESX ' mtimesx ' mode result matches mtimes:  EQUAL'])
else
    dx = max(abs(Cx(:)-Cm(:)));
    disp(['MTIMESX ' mtimesx ' mode result does not match mtimes:  NOT EQUAL , max diff = ' num2str(dx)])
end

try
bsxfun(@times,1,1);
Cr{6,1} = 'above vs bsxfun';
A = rand(2,2,2000000);
B = rand(1,1,2000000);
% bsxfun
tm = zeros(1,n);
for k=1:n
clear Cm
A(1) = 2*A(1);
B(1) = 2*B(1);
tic
Cm = bsxfun(@times,A,B);
tm(k) = toc;
end
% mtimesx
tx = zeros(1,n);
for k=1:n
clear Cx
tic
Cx = mtimesx(A,B);
tx(k) = toc;
end
% results
tm = median(tm);
tx = median(tx);
if( tx < tm )
    faster = sprintf('%7.1f',100*(tm)/tx-100);
    slower = '';
else
    faster = sprintf('%7.1f',-(100*(tx)/tm-100));
    slower = ' (i.e., slower)';
end
Cr{6,m} = faster;
disp(' ');
disp(['bsxfun  Elapsed time ' num2str(tm) ' seconds.']);
disp(['MTIMESX Elapsed time ' num2str(tx) ' seconds.']);
disp(['MTIMESX ' mtimesx ' mode is ' faster '% faster than MATLAB bsxfun with @times' slower])
if( isequal(Cx,Cm) )
    disp(['MTIMESX ' mtimesx ' mode result matches bsxfun with @times:  EQUAL'])
else
    dx = max(abs(Cx(:)-Cm(:)));
    disp(['MTIMESX ' mtimesx ' mode result does not match bsxfun with @times:  NOT EQUAL , max diff = ' num2str(dx)])
end
catch
    disp('Could not perform comparison with bsxfun, possibly because your version of');
    disp('MATLAB does not have it. You can download a substitute for bsxfun from the');
    disp('FEX here: http://www.mathworks.com/matlabcentral/fileexchange/23005-bsxfun-substitute');
end

end

disp(' ');
disp('Percent Faster Results Table');
disp(' ');
disp(Cr);

disp(' ');
disp('Done');
disp(' ');

end


function l = dim2deg(dim)
% dimension to harmonic degree of Wiegner D functions

l = 0;
while deg2dim(l+2) <= dim, l = l + 1;end


function[result]=Forward(docbatch,parameter,isTraining)
    [result]=Forward_Source_Word(docbatch,parameter,isTraining);
    % forward calculation at word level for each sentence
    [result]=Forward_Source_Sen(result,docbatch,parameter,isTraining);
    % forward calculation at sentence level for each document
    if isTraining==1
        [result]=Forward_Target(result,docbatch,parameter,isTraining);
    end
end

function[result]=Forward_Source_Word(docbatch,parameter,isTraining)
    % forward calculation at word level for each sentence
    sourceBatch=docbatch.source_smallBatch;
    result.source_sen_vector=[];
    for i=1:length(sourceBatch)
        batch=sourceBatch{i};
        T=batch.max_length;
        h_t_source_word=cell(parameter.layer_num,T);
        % store h_t
        result.c_t_source_word{i}=cell(parameter.layer_num,T);
        % store c_t
        result.lstms_source_word{i} = cell(parameter.layer_num,T);
        % store gate values for lstm units
        N=size(batch.Word,1);
        zeroState=zeroMatrix([parameter.hidden,N]);
        for ll=1:parameter.layer_num
            for tt=1:T
                h_t_source_word{ll,tt}=zeroState;
                result.c_t_source_word{i}{ll,tt}=zeroState;
            end
        end
        for t=1:T
            for ll=1:parameter.layer_num
                W=parameter.Word_S{ll};
                % W for word level composition in source
                if t==1
                    h_t_1=zeroState;
                    c_t_1=zeroState;
                else
                    c_t_1=result.c_t_source_word{i}{ll, t-1};
                    h_t_1=h_t_source_word{ll, t-1};
                end
                if ll==1
                    x_t=parameter.vect(:,batch.Word(:,t));
                else
                    x_t=h_t_source_word{ll-1,t};
                end
                x_t(:,batch.Delete{t})=0;
                h_t_1(:,batch.Delete{t})=0;
                c_t_1(:,batch.Delete{t})=0;
                % set postion that do not have words to zero
                [result.lstms_source_word{i}{ll, t},h_t_source_word{ll, t},result.c_t_source_word{i}{ll, t}]=lstmUnit(W,parameter,x_t,[],h_t_1,c_t_1,ll,t,isTraining);
                % compute lstm unit
                if t==T && ll==parameter.layer_num
                    result.source_sen_vector=[result.source_sen_vector,h_t_source_word{ll,t}];
                    % store vector embeddings for each source sentence
                    clear h_t_source_word;
                end
            end
        end
    end
    clear x_t;
    clear h_t_1;
    clear c_t_1;
end

function[result]=Forward_Source_Sen(result,docbatch,parameter,isTraining)
    % forward calculation at sentence level for each document
    T=docbatch.max_source_sen;
    h_t_source_sen=cell(parameter.layer_num,T);
    result.c_t_source_sen=cell(parameter.layer_num,T);
    result.lstms_source_sen=cell(parameter.layer_num,T);
    result.source_sen=cell(parameter.layer_num,1);
    result.source_each_sen=gpuArray();

    N=size(docbatch.source_sen_matrix,1);
    zeroState=zeroMatrix([parameter.hidden,N]);
    for ll=1:parameter.layer_num
        for tt=1:T
            h_t_source_sen{ll,tt}=zeroState;
            result.c_t_source_sen{ll,tt}=zeroState;
        end
    end
    for t=1:T
        for ll=1:parameter.layer_num
            W=parameter.Sen_S{ll};
            % sentence-level compostion
            if t==1
                h_t_1=zeroState;
                c_t_1 =zeroState;
            else
                c_t_1 =result.c_t_source_sen{ll, t-1};
                h_t_1 =h_t_source_sen{ll, t-1};
            end
            if ll==1
                x_t=result.source_sen_vector(:,docbatch.source_sen_matrix(:,t));
                % compute sentence-level embedding
            else
                x_t=h_t_source_sen{ll-1,t};
            end
            x_t(:,docbatch.source_delete{t})=0;
            h_t_1(:,docbatch.source_delete{t})=0;
            c_t_1(:,docbatch.source_delete{t})=0;
            % set values for deleted postion to 0
            [result.lstms_source_sen{ll, t},h_t_source_sen{ll, t},result.c_t_source_sen{ll, t}]=lstmUnit(W,parameter,x_t,[],h_t_1,c_t_1,ll,t,isTraining);
            % lstm unit calculation at sentence level
            if t==T
                result.source_sen{ll,1}=h_t_source_sen{ll, t};
                % store vector embeddings for documents
            end
            if ll==parameter.layer_num
                result.source_each_sen=[result.source_each_sen,h_t_source_sen{parameter.layer_num,t}];
            end
        end
    end
    clear result.source_sen_vector;
    clear h_t_source_sen;
    clear x_t;
    clear h_t_1;
    clear c_t_1;
end

function[result]=Forward_Target(result,docbatch,parameter,isTraining)
    % forward for target documents
    T=docbatch.max_target_sen;
    result.h_t_target_sen=cell(parameter.layer_num,T);
    % sentence level embeddings
    result.c_t_target_sen=cell(parameter.layer_num,T);
    result.lstms_target_sen=cell(parameter.layer_num,T);
    N=size(docbatch.target_sen_matrix,1);
    zeroState=zeroMatrix([parameter.hidden,N]);

    for ll=1:parameter.layer_num
        for tt=1:T
            result.h_t_target_sen{ll,tt}=zeroState;
            result.c_t_target_sen{ll,tt}=zeroState;
        end
    end
    result.Target_sen={};
    for sen_tt=1:T
        for ll=1:parameter.layer_num
            W=parameter.Sen_T{ll};
            % sentnece compositions for target
            if sen_tt==1
                % if sentence index is 1, h_t_1 and c_t_1 are from outputs from source sentences
                h_t_1=result.source_sen{ll,1};
                dim=size(result.c_t_source_sen);
                c_t_1=result.c_t_source_sen{ll,dim(2)};
            else
                % otherwise, c_t_1 are lstm outputs from last time step
                c_t_1 =result.c_t_target_sen{ll, sen_tt-1};
                h_t_1 =result.h_t_target_sen{ll, sen_tt-1};
            end
            if ll==1
                Word_List=docbatch.target_word{sen_tt}.Word;
                Word_Delete=docbatch.target_word{sen_tt}.Delete;
                if sen_tt==1
                    M1=result.source_sen(:,1);
                    dim=size(result.c_t_source_sen);
                    M2=result.c_t_source_sen(:,dim(2));
                else
                    M1=result.h_t_target_sen(:,sen_tt-1);
                    M2=result.c_t_target_sen(:,sen_tt-1);
                end
                result=Forward_Target_Word(result,M1,M2,Word_List,Word_Delete,docbatch,parameter,isTraining,sen_tt);
                x_t=result.Target_sen{sen_tt}.h_t_target_word{parameter.layer_num,size(Word_List,2)};
                result=Attention(docbatch,result,M1{parameter.layer_num,1},parameter,sen_tt);
                % compute attention values
                m_t=result.sum_vector{sen_tt};
            else
                x_t=result.h_t_target_sen{ll-1,sen_tt};
                m_t=[];
            end
            x_t(:,docbatch.target_delete{sen_tt})=0;
            h_t_1(:,docbatch.target_delete{sen_tt})=0;
            c_t_1(:,docbatch.target_delete{sen_tt})=0;
            % set deleted postions to 0 value
            [result.lstms_target_sen{ll,sen_tt},result.h_t_target_sen{ll,sen_tt},result.c_t_target_sen{ll,sen_tt}]=lstmUnit(W,parameter,x_t,m_t,h_t_1,c_t_1,ll,sen_tt,isTraining);
            % lstm unit calculation
        end
    end
    clear target_sen_vector;
    clear x_t;
    clear h_t_1;
    clear c_t_1;
end

function[result]=Forward_Target_Word(result,h_t_sen,c_t_sen,Word_List,Word_Delete,docbatch,parameter,isTraining,sen_tt)
    % obtain sentence level embeddings for target sentences
    N=size(Word_List,1);
    T=size(Word_List,2);
    target_sen.h_t_target_word=cell(parameter.layer_num,T);
    target_sen.c_t_target_word=cell(parameter.layer_num,T);
    target_sen.lstms=cell(parameter.layer_num,T);
    zeroState=zeroMatrix([parameter.hidden,N]);

    for ll=1:parameter.layer_num
        for tt=1:T
            target_sen.h_t_target_word{ll,tt}=zeroState;
            target_sen.c_t_target_word{ll,tt}=zeroState;
        end
    end
    for t=1:T
        for ll=1:parameter.layer_num
            W=parameter.Word_T{ll};
            if t==1
                h_t_1=h_t_sen{ll,1};
                c_t_1=c_t_sen{ll,1};
            else
                c_t_1 =target_sen.c_t_target_word{ll, t-1};
                h_t_1 =target_sen.h_t_target_word{ll, t-1};
            end
            if ll==1
                x_t=parameter.vect(:,Word_List(:,t));
                % if ll=1, x_t are correspondent word embeddings
            else
                x_t=target_sen.h_t_target_word{ll-1,t};
                % else x_t are outputs from previous layer
            end
            x_t(:,Word_Delete{t})=0;
            h_t_1(:,Word_Delete{t})=0;
            c_t_1(:,Word_Delete{t})=0;
            % set deleted positions to 0
            [target_sen.lstms{ll, t},target_sen.h_t_target_word{ll, t},target_sen.c_t_target_word{ll, t}]=lstmUnit(W,parameter,x_t,[],h_t_1,c_t_1,ll,t,isTraining);
            % lstm unit calculations
            if t~=1
                target_sen.h_t_target_word{ll, t}(:,Word_Delete{t})=target_sen.h_t_target_word{ll,t-1}(:,Word_Delete{t});
                 % sentence representations stay the same for deleted positions
            end
        end
    end
    result.Target_sen{sen_tt}=target_sen;
    clear h_t_1;
    clear c_t_1;
    clear x_t;
end


% This small example illustrates how to use the remote API
% synchronous mode. The synchronous mode needs to be
% pre-enabled on the server side. You would do this by
% starting the server (e.g. in a child script) with:
%
% simRemoteApi.start(19999,1300,false,true)
%
% But in this example we try to connect on port
% 19997 where there should be a continuous remote API
% server service already running and pre-enabled for
% synchronous mode.
%
% IMPORTANT: for each successful call to simxStart, there
% should be a corresponding call to simxFinish at the end!

function irb140_abb_path_planning()
    global configuration

    % init basic data. Robots and number of collision objects.
    coppelia = [];
    coppelia.n_collision_objects = 0;
    coppelia.robots{1}.n_joints = 6;
    coppelia.robots{1}.end_effector.n_joints = 2;
    coppelia.dt = 50/1000; % default 50 ms
    %should match ARTE delta_time
    configuration.delta_time = 50/1000;
    %configuration.delta_time = 0.01;
    %coppelia = coppelia_start(coppelia);

    pick_and_place(coppelia)
    %coppelia_stop(coppelia);
end

function pick_and_place(coppelia)
%load in arte
global robot
robot = load_robot('ABB', 'IRB140');
robot_number = 1;
% initial position
Q = [0 0 0 0 0 0]';
robot.q = Q;
robot.qd = [0 0 0 0 0 0]';
lin_vel = 0.5; % m/s
ang_vel = 0.8; % rad/s

% %Initial point
% T1 = [-1 0 0 0.52;
%        0 1 0  0;
%        0 0 -1 0.4;
%        0 0 0 1];
% % Final point
% T2 = [1 0 0 0.5;
%        0 1 0  0.5;
%        0 0 1 0.5;
%        0 0 0 1];
% % Final point
% T3 = [-1 0 0 0.5;
%        0 1 0  -0.5;
%        0 0 -1 0.5;
%        0 0 0 1];
q2 = [0.5 0.5 0.5 0.5 0.5 0.5];
qd2 = [0 0 0 0 0 0];
% end speed is 20 % of max speed for all joints
[q, qdd]=AbsJPath(robot, q2, qd2, 20);
move_robot(coppelia, 1, q, qdd)

% go to pre pick point, then open, then to pick point
%MoveQ(coppelia, robot_number, T1)
%open_gripper(coppelia, robot_number);
%coppelia_wait(coppelia, 10)
%MoveL(coppelia, robot_number, T2, lin_vel, ang_vel)
%MoveL(coppelia, robot_number, T3, lin_vel, ang_vel)
%MoveL(coppelia, robot_number, T1, lin_vel, ang_vel)

% % now close gripper
% close_gripper(coppelia, robot_number);
% MoveQ(coppelia, robot_number, T1)
%
%
% %release
% MoveQ(coppelia, robot_number, T3)
% open_gripper(coppelia, robot_number);
% % over the release point
% MoveQ(coppelia, robot_number, T4)
% %start point again
% MoveAbsQ(coppelia, robot_number, Q)
end

% % Linear interpolation move to Q
% function MoveAbsQ(coppelia, robot_number, Q)
%     global robot
%     q0 = robot.q;
%     %q1 = [0.0 0.0 0.0 0.0 0.0 0.0]';
%     q_path = linear_q_path(q0, Q);
%     setjointtargetpositions(coppelia,robot_number, q_path);
%     %coppelia_wait(coppelia, 10)
%     robot.q = Q;
% end

% % MoveL
% % MoveQ
% function MoveQ(coppelia, robot_number, T)
% %drawrobot3d(robot, q0)
%     global robot
%     %start joint coordinates, as saved before
%     q0 = robot.q;
%
%     qinv = inversekinematic(robot, T);
%     q1 = qinv(:,1);
%     q_path = linear_q_path(q0, q1);
%     setjointtargetpositions(coppelia, robot_number, q_path)
%     %coppelia_wait(coppelia, 10)
%     robot.q = q1;
% end

function MoveL(coppelia, robot_number, Tf, lin_vel, ang_vel)

    global robot
    %start joint coordinates, as saved before
    q0 = robot.q;
    T0 = directkinematic(robot, q0);
    p0 = T0(1:3, 4);
    pf = Tf(1:3, 4);

    qinv = inversekinematic(robot, Tf);
    qf = qinv(:,1);
    Q0 = T2quaternion(T0);
    Qf = T2quaternion(Tf);

    v0 = abs(lin_vel)*compute_speed(p0, pf);
    w0 = abs(ang_vel)*compute_ang_speed(Q0, Qf);
    Vref = [v0' w0']';
    qs = [robot.q];
    qds = [robot.qd];
    q = q0;
    while 1
        Ti = directkinematic(robot, q);
        pi = Ti(1:3, 4);
        e = norm(pf - pi);
        if e < 0.05
            break
        end
        J = manipulator_jacobian(robot, q);
        qd = inv(J)*Vref;
        q = q + qd*coppelia.dt;
        qs = [qs q];
        qds = [qds qd];
    end
    %q_path = linear_q_path(q0, q1);
    move_robot(coppelia, robot_number, qs, qds);
    %coppelia_wait(coppelia, 10)
    robot.q = q;
    robot.qd = qd;
end

function q_path = linear_q_path(q0, q1)
q_path=[];
% find max difference
dq = abs(q0-q1);
[y, i] = max(dq);
n = floor(y/0.1 + 1);
for i=1:length(q0)
    q_path = [q_path; linspace(q0(i), q1(i), n)];
end
%q_path
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%En base a la posici�n y orientaci�n final, calcular cu�les deben ser las
%velocidades...
% Esto es diferente a calcular la velocidades cuando ya hay contacto y se
% trata de un problema de control... pero es parecido
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [v] = compute_speed(Pi, Pf)
%compute a constant linear speed till target
v = (Pf-Pi);
v = v(:)/norm(v);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%En base a la posici�n y orientaci�n final, calcular cu�les deben ser las
%velocidades...
% Esto es diferente a calcular la velocidades cuando ya hay contacto y se
% trata de un problema de control... pero es parecido
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [w] = compute_ang_speed(Qi, Qf)
%compute a constant angular speed till target
%asume the movement is performed in 1 second
w = angular_w_between_quaternions(Qi, Qf, 1);
w = w(:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute angular speed w that moves Q0 into Q1 in time total_time.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function w = angular_w_between_quaternions(Q0, Q1, total_time)
%global robot
%below this number, the axis is considered as [1 0 0]
%this is to avoid numerical errors
%this is the actual error allowed for w
%epsilon_len = robot.parameters.epsilonQ;
epsilon_len = 0.0001;
%Let's first find quaternion q so q*q0=q1 it is q=q1/q0
%For unit length quaternions, you can use q=q1*Conj(q0)
Q = qprod(Q1, qconj(Q0));

%To find rotation velocity that turns by q during time Dt you need to
%convert quaternion to axis angle using something like this:
len=sqrt(Q(2)^2 + Q(3)^2 + Q(4)^2);

if len > epsilon_len
    angle=2*atan2(len, Q(1));
    axis=[Q(2) Q(3) Q(4)]./len;
else
    angle=0;
    axis=[1 0 0];
end
w=axis*angle/total_time;
end


classdef RotatedRect
    %ROTATEDRECT  The class represents rotated (i.e. not up-right) rectangles on a plane
    %
    % Each rectangle is specified by the center point (mass center), length of
    % each side (represented by `[width,height]`) and the rotation angle in
    % degrees.
    %
    % The sample `RotatedRect_demo.m` demonstrates how to use RotatedRect.
    %
    % See also: cv.CamShift, cv.fitEllipse, cv.minAreaRect
    %

    methods (Static)
        function rrect = from3points(pt1, pt2, pt3)
            %FROM3POINTS  Create a rotated rectangle from 3 points
            %
            %     rrect = cv.RotatedRect.from3points(pt1, pt2, pt3)
            %
            % ## Input
            % * __pt1__, __pt2__, __pt3__ Any 3 end points `[x,y]` of the
            %   rotated rectangle. They must be given in order (either
            %   clockwise or anticlockwise). By definition, the two sides
            %   formed by these three points must be perpendicular.
            %
            % ## Output
            % * __rrect__ output rotated rectangle. A structure with the
            %   following fields:
            %   * __center__ The rectangle mass center `[x,y]`.
            %   * __size__ Width and height of the rectangle `[w,h]`.
            %   * __angle__ The rotation angle in a clockwise direction. When
            %     the angle is 0, 90, 180, 270 etc., the rectangle becomes an
            %     up-right rectangle.
            %
            rrect = RotatedRect_('from3points', pt1, pt2, pt3);
        end

        function pts = points(rrect)
            %POINTS  Returns 4 vertices of the rectangle
            %
            %     pts = cv.RotatedRect.points(rrect)
            %
            % ## Input
            % * __rrect__ rotated rectangle. A structure with the following
            %   fields:
            %   * __center__ The rectangle mass center `[x,y]`.
            %   * __size__ Width and height of the rectangle `[w,h]`.
            %   * __angle__ The rotation angle in a clockwise direction. When
            %     the angle is 0, 90, 180, 270 etc., the rectangle becomes an
            %     up-right rectangle.
            %
            % ## Output
            % * __pts__ 4-by-2 points matrix of the rectangle vertices
            %   `[x1 y1; x2 y2; x3 y3; x4 y4]`. The order is bottom-left,
            %   top-left, top-right, bottom-right.
            %
            % See also: cv.boxPoints, bbox2points
            %
            pts = RotatedRect_('points', rrect);
        end

        function rect = boundingRect(rrect)
            %BOUNDINGRECT  Returns the minimal up-right integer rectangle containing the rotated rectangle
            %
            %     rect = cv.RotatedRect.boundingRect(rrect)
            %
            % ## Input
            % * __rrect__ rotated rectangle. A structure with the following
            %   fields:
            %   * __center__ The rectangle mass center `[x,y]`.
            %   * __size__ Width and height of the rectangle `[w,h]`.
            %   * __angle__ The rotation angle in a clockwise direction. When
            %     the angle is 0, 90, 180, 270 etc., the rectangle becomes an
            %     up-right rectangle.
            %
            % ## Output
            % * __rect__ bounding rectangle, a 1-by-4 vector `[x, y, w, h]`
            %
            % See also: cv.RotatedRect.boundingRect2f
            %
            rect = RotatedRect_('boundingRect', rrect);
        end

        function rect = boundingRect2f(rrect)
            %BOUNDINGRECT  returns the minimal (exact) floating point rectangle containing the rotated rectangle
            %
            %     rect = cv.RotatedRect.boundingRect2f(rrect)
            %
            % ## Input
            % * __rrect__ rotated rectangle. A structure with the following
            %   fields:
            %   * __center__ The rectangle mass center `[x,y]`.
            %   * __size__ Width and height of the rectangle `[w,h]`.
            %   * __angle__ The rotation angle in a clockwise direction. When
            %     the angle is 0, 90, 180, 270 etc., the rectangle becomes an
            %     up-right rectangle.
            %
            % ## Output
            % * __rect__ bounding rectangle, a 1-by-4 vector `[x, y, w, h]`
            %
            % Not intended for use with images.
            %
            % See also: cv.RotatedRect.boundingRect
            %
            rect = RotatedRect_('boundingRect2f', rrect);
        end
    end

end


%PARSC Parse classifier
%
% 	PARSC(W)
%
% Displays the type and, for combining classifiers, the structure of the
% mapping W.
%
% See also MAPPINGS

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: parsc.m,v 1.2 2006/03/08 22:06:58 duin Exp $

function parsc(w,space)

		% If W is not a mapping, do not generate an error but simply return.

	if (~isa(w,'prmapping')) return; end
	if (nargin == 1)
		space = '';
	end

	% Display W's type.

	display(w,space);

	% If W is a combining classifier, recursively call PARSC to plot the
	% combined classier.

	pars = getdata(w);
	if (iscell(pars))
		space = [space '  '];
		for i=1:length(pars)
			parsc(pars{i},space)
		end
	end

return


%% DEMO_import_FEB_export_INP
% Below is a demonstration of how import a FEB file and subsequently export
% the geometry into an INP file.

%%

clear; close all; clc;

%%
% Plot settings
fontSize=15;
faceAlpha1=0.5;
faceAlpha2=0.5;
edgeColor=0.25*ones(1,3);
edgeWidth=1.5;
markerSize1=50;

%% Importing .feb file

%Set main folders
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
pathName_FEB=fullfile(defaultFolder,'data','FEB'); %Where to load the FEB file
pathName_INP=fullfile(defaultFolder,'data','INP'); %Where to export the INP file

febFileNamePart='example_HEX_QUAD.feb';
febFileName=fullfile(pathName_FEB,febFileNamePart);
[febXML,nodeStruct,elementCell]=import_FEB(febFileName);

%% Plotting model

% Plotting the example model surfaces
hf1=cFigure;
title('Visualizing model','FontSize',fontSize);
xlabel('X','FontSize',fontSize);ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

uniqueMaterialIndices=[];
for q=1:1:numel(elementCell)
    uniqueMaterialIndices=unique([uniqueMaterialIndices(:); elementCell{q}.E_mat(:)]);
    switch elementCell{q}.E_type
        case {'tri3', 'quad4'}
            F=elementCell{q}.E;
            V=nodeStruct.N;
            C=elementCell{q}.E_mat;
       case {'hex8', 'tet4'}
            [F,C]=element2faces(elementCell{q}.E,elementCell{q}.E_mat); %Creates faces and colors (e.g. stress) for patch based plotting
    end
    hp=patch('Faces',F,'Vertices',V,'EdgeColor','k','FaceColor','flat','Cdata',C,'FaceAlpha',0.8);
end

colormap(jet(numel(uniqueMaterialIndices))); hc=colorbar; caxis([min(uniqueMaterialIndices)-0.5 max(uniqueMaterialIndices)+0.5]);
axis equal; view(3); axis tight;  grid on; set(gca,'FontSize',fontSize);
camlight('headlight');
drawnow;

%% EXPORTING INP FILES FOR EACH ELEMENT TYPE

%You can change this example to do this for material type instead. Just use
%the material indices to select the elements from the lists. However the
%export_INP function can only handle 1 element type at a time at the moment

for q=1:1:numel(elementCell)

    inpFileNamepart=[febFileNamePart(1:end-4),'_',num2str(q),'.inp']; %filename for inp file
    inpFileName=fullfile(pathName_INP,inpFileNamepart);

    elementStruct=elementCell{q};
    %Setting appropriate element type line for ABAQUS. CHECK THESE!
    switch elementStruct.E_type
        case 'tri3'
            elementStruct.E_type='*ELEMENT, TYPE=STRI3, ELSET=PART-DEFAULT_1_EB1';
        case 'quad4'
            elementStruct.E_type='*ELEMENT, TYPE=S4R, ELSET=PART-DEFAULT_1_EB1';
        case 'tet4'
            elementStruct.E_type='*ELEMENT, TYPE=C3D4, ELSET=PART-DEFAULT_1_EB1';
        case 'hex8'
            elementStruct.E_type='*ELEMENT, TYPE=C3D8R, ELSET=PART-DEFAULT_1_EB1';
    end
    export_INP(elementStruct,nodeStruct,inpFileName);
end

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