  function [B Bgx Bgy Bgz] = makeB(ig, kg, varargin)
%|function [B Bgx Bgy Bgz] = makeB(ig, kg, [?])
%|
%| Create a Fatrix B operation of warping based on cubic B-spline
%|
%| in:
%|	'ig'		image geometry
%|	'kg'		knot geometry
%|
%| out:
%|	B		fatrix operation for warping
%|	Bgx		fatrix operation for warping
%|	Bgy		fatrix operation for warping
%|	Bgz		fatrix operation for warping
%|
%| Copyright August 2006, Se Young Chun and Jeff Fessler, University of Michigan

if nargin < 2, ir_usage, end

argB.power = 0;
argB.ig = ig;
argB.kg = kg;
[argB.kernelx argB.kernelgx] = makeKernel(3, kg.mx);
[argB.kernely argB.kernelgy] = makeKernel(3, kg.my);
if (kg.is3 == 1)
	[argB.kernelz argB.kernelgz] = makeKernel(3, kg.mz);
end

if (nargin == 2)
	B = Fatrix([ig.np kg.np], argB, 'forw', @makeB_forward, ...
		'back', @makeB_transpose, 'power', @makeB_power);
	Bgx = Fatrix([ig.np kg.np], argB, 'forw', @makeBgx_forward, ...
		'back', @makeBgx_transpose);
	Bgy = Fatrix([ig.np kg.np], argB, 'forw', @makeBgy_forward, ...
		'back', @makeBgy_transpose);
	if (kg.is3 == 1)
		Bgz = Fatrix([ig.np kg.np], argB, 'forw', @makeBgz_forward, ...
			'back', @makeBgz_transpose);
	end

else
	if (kg.is3 == 1)
		argB.WarpCell = varargin(1:3);
		Bgz = Fatrix([ig.np kg.np],argB,'forw',@makeBgz_forwardwarp, ...
			'back', @makeBgz_transposewarp);
	else
		argB.WarpCell = varargin(1:2);
	end
	B = Fatrix([ig.np kg.np], argB, 'forw', @makeB_forwardwarp, ...
		'back', @makeB_transposewarp);
	Bgx = Fatrix([ig.np kg.np], argB, 'forw', @makeBgx_forwardwarp, ...
		'back', @makeBgx_transposewarp);
	Bgy = Fatrix([ig.np kg.np], argB, 'forw', @makeBgy_forwardwarp, ...
		'back', @makeBgy_transposewarp);
end

%%%
function DFM = makeB_forward(argB, alpha)
alpha = double(reshape(alpha, argB.kg.dim));
if (argB.kg.is3 == 0) % 2D case
	DFM = BsplCo2ValZeroFilt(alpha, [argB.ig.nx, argB.ig.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], {argB.kernelx, argB.kernely});
else
	DFM = BsplCo2ValZeroFilt(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], ...
		{argB.kernelx, argB.kernely, argB.kernelz});
end
DFM = single(DFM(:));

%%%
function ALP = makeB_transpose(argB, imgco)
imgco = double(reshape(imgco, argB.ig.dim));
if (argB.kg.is3 == 0) % 2D case
	ALP = BsplCo2ValTranZeroFilt(imgco, [argB.kg.nx argB.kg.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], {argB.kernelx, argB.kernely});
else
	ALP = BsplCo2ValTranZeroFilt(imgco,[argB.kg.nx argB.kg.ny argB.kg.nz],...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], ...
		{argB.kernelx, argB.kernely, argB.kernelz});
end
ALP = single(ALP(:));

%%%
function DFM = makeB_forwardwarp(argB, alpha)
alpha = single(reshape(alpha, argB.kg.dim));
if (argB.kg.is3 == 0) % 2D case
        DFM = BsplCo2ValZero(alpha, [argB.ig.nx, argB.ig.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], argB.WarpCell);
else
        DFM = BsplCo2ValZero(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
end
DFM = DFM(:);

%%%
function ALP = makeB_transposewarp(argB, imgco)
imgco = single(reshape(imgco, argB.ig.dim));
if (argB.kg.is3 == 0) % 2D case
	ALP = BsplCo2ValTranZero(imgco, [argB.kg.nx, argB.kg.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], argB.WarpCell);
else
	ALP = BsplCo2ValTranZero(imgco, [argB.kg.nx, argB.kg.ny argB.kg.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
end
ALP = ALP(:);

%%%
function DFM = makeBgx_forward(argB, alpha)
alpha = double(reshape(alpha, argB.kg.dim));
if (argB.kg.is3 == 0) % 2D case
	DFM = BsplCo2ValZeroFilt(alpha, [argB.ig.nx, argB.ig.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], {argB.kernelgx, argB.kernely});
else
	DFM = BsplCo2ValZeroFilt(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], ...
		{argB.kernelgx, argB.kernely, argB.kernelz});
end
DFM = single(DFM(:));

%%%
function ALP = makeBgx_transpose(argB, imgco)
imgco = double(reshape(imgco, argB.ig.dim));
if (argB.kg.is3 == 0) % 2D case
	ALP = BsplCo2ValTranZeroFilt(imgco, [argB.kg.nx argB.kg.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], ...
		{fliplr(argB.kernelgx), fliplr(argB.kernely)});
else
	ALP = BsplCo2ValTranZeroFilt(imgco,[argB.kg.nx argB.kg.ny argB.kg.nz],...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], ...
		{fliplr(argB.kernelgx), fliplr(argB.kernely), ...
		fliplr(argB.kernelz)});
end
ALP = single(ALP(:));

%%%
function DFM = makeBgx_forwardwarp(argB, alpha)
alpha = single(reshape(alpha, argB.kg.dim));
if (argB.kg.is3 == 0) % 2D case
        DFM = BsplCo2GdXZero(alpha, [argB.ig.nx, argB.ig.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], argB.WarpCell);
else
        DFM = BsplCo2GdXZero(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
end
DFM = DFM(:);

%%%
function ALP = makeBgx_transposewarp(argB, imgco)
imgco = single(reshape(imgco, argB.ig.dim));
if (argB.kg.is3 == 0) % 2D case
	ALP = BsplCo2GdXTranZero(imgco, [argB.kg.nx, argB.kg.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], argB.WarpCell);
else
	ALP = BsplCo2GdXTranZero(imgco, [argB.kg.nx, argB.kg.ny argB.kg.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
end
ALP = ALP(:);

%%%
function DFM = makeBgy_forward(argB, alpha)
alpha = double(reshape(alpha, argB.kg.dim));
if (argB.kg.is3 == 0) % 2D case
	DFM = BsplCo2ValZeroFilt(alpha, [argB.ig.nx, argB.ig.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], {argB.kernelx, argB.kernelgy});
else
	DFM = BsplCo2ValZeroFilt(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], ...
		{argB.kernelx, argB.kernelgy, argB.kernelz});
end
DFM = single(DFM(:));

%%%
function ALP = makeBgy_transpose(argB, imgco)
imgco = double(reshape(imgco, argB.ig.dim));
if (argB.kg.is3 == 0) % 2D case
	ALP = BsplCo2ValTranZeroFilt(imgco, [argB.kg.nx argB.kg.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], ...
		{fliplr(argB.kernelx), fliplr(argB.kernelgy)});
else
	ALP = BsplCo2ValTranZeroFilt(imgco,[argB.kg.nx argB.kg.ny argB.kg.nz],...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], ...
		{fliplr(argB.kernelx), fliplr(argB.kernelgy), ...
		fliplr(argB.kernelz)});
end
ALP = single(ALP(:));

%%%
function DFM = makeBgy_forwardwarp(argB, alpha)
alpha = single(reshape(alpha, argB.kg.dim));
if (argB.kg.is3 == 0) % 2D case
        DFM = BsplCo2GdYZero(alpha, [argB.ig.nx, argB.ig.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], argB.WarpCell);
else
        DFM = BsplCo2GdYZero(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
end
DFM = DFM(:);

%%%
function ALP = makeBgy_transposewarp(argB, imgco)
imgco = single(reshape(imgco, argB.ig.dim));
if (argB.kg.is3 == 0) % 2D case
	ALP = BsplCo2GdYTranZero(imgco, [argB.kg.nx, argB.kg.ny], ...
		[argB.kg.offset_x argB.kg.offset_y], ...
		[argB.kg.mx argB.kg.my], argB.WarpCell);
else
	ALP = BsplCo2GdYTranZero(imgco, [argB.kg.nx, argB.kg.ny argB.kg.nz], ...
		[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
		[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
end
ALP = ALP(:);

%%%
function DFM = makeBgz_forward(argB, alpha)
alpha = double(reshape(alpha, argB.kg.dim));
DFM = BsplCo2ValZeroFilt(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
	[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
	[argB.kg.mx argB.kg.my argB.kg.mz], ...
	{argB.kernelx, argB.kernely, argB.kernelgz});
DFM = single(DFM(:));

%%%
function ALP = makeBgz_transpose(argB, imgco)
imgco = double(reshape(imgco, argB.ig.dim));
ALP = BsplCo2ValTranZeroFilt(imgco,[argB.kg.nx argB.kg.ny argB.kg.nz],...
	[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
	[argB.kg.mx argB.kg.my argB.kg.mz], ...
	{fliplr(argB.kernelx), fliplr(argB.kernely), ...
	fliplr(argB.kernelgz)});
ALP = single(ALP(:));

%%%
function DFM = makeBgz_forwardwarp(argB, alpha)
alpha = single(reshape(alpha, argB.kg.dim));
DFM = BsplCo2GdZZero(alpha, [argB.ig.nx, argB.ig.ny argB.ig.nz], ...
	[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
	[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
DFM = DFM(:);

%%%
function ALP = makeBgz_transposewarp(argB, imgco)
imgco = single(reshape(imgco, argB.ig.dim));
ALP = BsplCo2GdZTranZero(imgco, [argB.kg.nx, argB.kg.ny argB.kg.nz], ...
	[argB.kg.offset_x argB.kg.offset_y argB.kg.offset_z], ...
	[argB.kg.mx argB.kg.my argB.kg.mz], argB.WarpCell);
ALP = ALP(:);

%%%
function ob = makeB_power(ob, sup)

if (sup == 1)

elseif ((sup == 2) && (ob.arg.power == 0))
	ob.arg.power = 1;
else
	error('only squares of each element are supported');
end
ob = ob;


% trainTestSurfaceContextScript
%
% Assumes you have created:
%   imsegs  :  data structure that contains information on all training
%              and testing images
%   trainind:  the indices of the training images
%   testind:   the indices of the testing images

basedir = '???'; % set this appropriately
imdir = [basedir '/images/all_images'];
datadir = [basedir '/data'];
resultsdir = [basedir '/results'];

DO_TRAIN = 1;
DO_TEST = 1;

if DO_TRAIN

    nsegments = [10 20 30 40 50 60 80 100];

    % Computes the superpixel features
    if ~exist('spfeatures')
        spfeatures = mcmcGetAllSuperpixelData(imdir, imsegs);
        save([datadir '/superpixelData.mat'], 'spfeatures');
    end

    % Trains the superpixel classifiers
    if ~exist('vclassifierSP')
        [vclassifierSP, hclassifierSP] = ...
            mcmcTrainSuperpixelClassifier(spfeatures(trainind), imsegs(trainind));
        save([datadir '/superpixelClassifier.mat'], 'vclassifierSP', 'hclassifierSP');
    end

    % Computes the same-label features
    if ~exist('efeatures')
        [efeatures, adjlist] = mcmcGetAllEdgeData(spfeatures, imsegs);
        save([datadir '/edgeData.mat'], 'efeatures', 'adjlist');
    end

    % Trains the same-label classifier
    if ~exist('eclassifier')
        eclassifier = mcmcTrainEdgeClassifier(efeatures(trainind), ...
            adjlist(trainind), imsegs(trainind));
        ecal = calibrateEdgeClassifier(efeatures(trainind), adjlist(trainind), ...
            imsegs(trainind), eclassifier, 1);
        ecal = ecal{1};
        save([datadir '/edgeClassifier.mat'], 'eclassifier', 'ecal');
    end

    % Computes the multiple segmentations, the segment features, and the
    % ground truth labels for each segment
    if ~exist('labdata')
        % gather data
        for f = 1:numel(imsegs)

            disp([num2str(f) ': ' imsegs(f).imname])

            [pvSP{f}, phSP{f}, pE{f}] = mcmcInitialize(spfeatures{f}, efeatures{f}, ...
                adjlist{f}, imsegs(f), vclassifierSP, hclassifierSP, eclassifier, ecal, 'none');
            smaps{f} = generateMultipleSegmentations2(pE{f}, adjlist{f}, imsegs(f).nseg, nsegments);

            im = im2double(imread([imdir '/' imsegs(f).imname]));
            imdata = mcmcComputeImageData(im, imsegs(f));

            for k = 1:numel(nsegments)
                labdata{f, k} = mcmcGetSegmentFeatures(imsegs(f), spfeatures{f}, imdata, smaps{f}(:, k), (1:max(smaps{f}(:, k))));
                [mclab{f, k}, mcprc{f, k}, allprc{f, k}, trainw{f, k}] = segmentation2labels(imsegs(f), smaps{f}(:, k));
                unilabel{f, k} = mclab{f, k}.*(mcprc{f, k}>0.95);
                seglabel{f,k} =  1*(mcprc{f, k}>0.95) + (-1)*(mcprc{f, k}<0.95);
            end
        end
        save([datadir '/allData.mat'], 'smaps', 'labdata', 'segdata', 'mclab', 'mcprc', 'allprc', 'seglabel', 'unilabel', 'trainw', 'pvSP', 'phSP', 'pE');
    end

    % Trains the segment classifiers
    if ~exist('vclassifier')
        sclassifier = mcmcTrainSegmentationClassifier2(labdata(trainind, :), seglabel(trainind{k}, :), trainw(trainind, :));
        [vclassifier, hclassifier] = ...
            mcmcTrainSegmentClassifier2(labdata(trainind, :), unilabel(trainind, :), trainw(trainind, :), 50000);
    end

    save([datadir '/allClassifiers.mat'], 'vclassifier', 'hclassifier', 'sclassifier', 'eclassifier', 'vclassifierSP', 'hclassifierSP');

end

if DO_TEST

    % Computes the label confidences for each superpixel in the test images
    % and gives the final accuracy.
    % pg{image number}(superpixel number, [000 left center right porous solid sky])
    % gives the superpixel label confidences
    [vacc, hacc, vcm, hcm, pg] = ...
        testMultipleSegmentationsCV2(imsegs(testind), labdata(testind, :), ...
        labdata(testind, :), smaps(testind), ...
        vclassifier, hclassifier, sclassifier, pvSP(testind), phSP(testind), 1);
    save([datadir '/results.mat'], 'vacc', 'hacc', 'vcm', 'hcm', 'pg');

    % Computes and writes the labeled images
    for f = 1:testind
        im = im2double(imread([imdir '/' imsegs(f).imname]));
        [pv, ph] = splitpg(pg{f});
        lim = APPgetLabeledImage2(im, imsegs(f), pv, ph);
        imwrite(im, [resultsdir '/' imsegs(f).imname]);
        imwrite(lim, [resultsdir '/' strtok(imsegs(f).imname, '.') '.l.jpg']);
    end
end


function [vertices, faces] = removeInvalidBorderFaces(varargin)
%REMOVEINVALIDBORDERFACES Remove faces whose edges are connected to 3, 3, and 1 faces.
%
%   [V2, F2] = removeInvalidBorderFaces(V, F)
%
%   Example
%   removeInvalidBorderFaces
%
%   See also
%     isManifoldMesh, collapseEdgesWithManyFaces

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2019-01-31, using Matlab 9.5.0.944444 (R2018b)
% Copyright 2019-2022 INRA - Cepia Software Platform

vertices = varargin{1};
faces = varargin{2};

% compute edge to vertex array
if nargin == 3
    edges = faces;
    faces = varargin{3};
else
    % compute edge to vertex array
    edges = meshEdges(faces);
end

% compute face to edge indices array
% as a nFaces-by-3 array (each face connected to exactly three edges)
faceEdgeInds = meshFaceEdges(vertices, edges, faces);

% compute number of faces incident each edge
edgeFaces = trimeshEdgeFaces(faces);
edgeFaceDegrees = sum(edgeFaces > 0, 2);

% for each face, concatenate the face degree of each edge
faceEdgeDegrees = zeros(size(faces, 1), 3);
for iFace = 1:size(faces, 1)
    edgeInds = faceEdgeInds{iFace};
    faceEdgeDegrees(iFace, :) = edgeFaceDegrees(edgeInds);
end

% remove faces containing edges connected to 1 face and edges connected to
% 3 faces
inds = sum(faceEdgeDegrees == 1, 2) > 0 & sum(faceEdgeDegrees == 3, 2);
% inds = sum(ismember(faceEdgeDegrees, [1 3 4]), 2) == 3;
faces(inds, :) = [];


% ------------------------------------------------------------------------
%  Copyright (C)
%  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
%  University of California Berkeley (UCB) - USA
%
%  Jordi Pont-Tuset <jordi.pont@upc.edu>
%  Pablo Arbelaez <arbelaez@berkeley.edu>
%  June 2014
% ------------------------------------------------------------------------
% This file is part of the MCG package presented in:
%    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
%    "Multiscale Combinatorial Grouping,"
%    Computer Vision and Pattern Recognition (CVPR) 2014.
% Please consider citing the paper if you use this code.
% ------------------------------------------------------------------------

function stats = get_single_hier_stats(params,hier_id,gt_set)

if nargin<3
    gt_set = params.gt_set_pareto;
    res_file = params.files.pareto_singles{hier_id};
else
    res_file = strrep(params.files.pareto_singles{hier_id},[params.gt_set_pareto '_pareto_single'],[gt_set '_pareto_single']);
end

% Sampled number of candidates from each hierarchy
n_cands = [10:10:100,200:100:1000,2000:1000:10000];

for ii=1:params.n_r_cand
    n_max_cands(ii) = max(n_cands); %#ok<*AGROW>
end

% Are the results already computed?
if exist(res_file, 'file')
    load(res_file)
    disp(['Loaded: ' res_file '.'])
    recompute = 0;
else
    disp(['RECOMPUTING: ' res_file '.'])
    recompute = 1;
end

if recompute
    % Load which images to consider from the database (train, val, etc.)
    im_ids = database_ids(params.database,gt_set);

    for ll=1:params.n_r_cand
        % Store number of regions and gt_set
        stats(ll).n_cands = n_cands;
        stats(ll).gt_set = gt_set;
        stats(ll).num_objects = 0;
        stats(ll).obj_classes = [];
    end

    % Sweep all images
    num_images = length(im_ids);
    for ii = 1:num_images

        % Read the UCM as a hierarchy
        hier = ucm2hier(fullfile(params.hier_dirs{hier_id}, [im_ids{ii} '.mat']));
        n_regs = hier.ms_struct(end).parent;

        % Get all pairs of neighboring leave regions
        [~, idx_neighbors] = seg2gridbmap(hier.leaves_part);
        K = max(idx_neighbors.matrix_max(:)) + 1;
        neigh_pairs = unique(idx_neighbors.matrix_min+K*idx_neighbors.matrix_max);
        neigh_pairs(neigh_pairs==0) = [];
        neigh_pairs_min = mod(neigh_pairs,K);
        neigh_pairs_max = (neigh_pairs-neigh_pairs_min)/K;

        if isrow(neigh_pairs_min)
            neigh_pairs_min = neigh_pairs_min';
        end
        if isrow(neigh_pairs_max)
            neigh_pairs_max = neigh_pairs_max';
        end

        % Get the 'n_cands' top candidates from each hierarchy

        % Singletons
        if n_regs<=n_max_cands(1)
            all_cands{1} = (n_regs:-1:1)';
        else
            all_cands{1} = (n_regs:-1:(n_regs-n_max_cands(1))+1)';
        end

        % Pairs, triplets, etc.
        all_cands(2:params.n_r_cand) = ...
        mex_get_tree_cands(double(hier.leaves_part)-1, double(hier.ms_matrix)-1,...
                             neigh_pairs_min-1, neigh_pairs_max-1,...
                             n_max_cands(2:end));

        % Load ground truth
        curr_gt = get_ground_truth( params.database, im_ids{ii} );

        for ll=1:params.n_r_cand
            % Evaluate the candidates
            [jaccards,inters,false_pos,false_neg,true_areas,obj_classes] = eval_cands(hier,all_cands{ll},curr_gt); %#ok<ASGLU>

            % Get best candidate at different number of candidates
            for jj=1:length(stats(ll).n_cands)
                curr_n_regs = min(stats(ll).n_cands(jj), size(inters,2));
                to_consider = 1:curr_n_regs;
                stats(ll).all_n_masks(ii,jj) = length(to_consider);
                if (stats(ll).all_n_masks(ii,jj)>0)
                    for kk=1:size(inters,1)
                        [stats(ll).max_J(stats(ll).num_objects+kk,jj), which_one] = max(jaccards(kk,to_consider));
                        stats(ll).max_indicator(stats(ll).num_objects+kk,jj) = to_consider(which_one);
                        stats(ll).max_fp(stats(ll).num_objects+kk,jj)        = false_pos(kk,to_consider(which_one));
                        stats(ll).max_fn(stats(ll).num_objects+kk,jj)        = false_neg(kk,to_consider(which_one));
                        stats(ll).max_inters(stats(ll).num_objects+kk,jj)    = inters(kk,to_consider(which_one));
                    end
                end
            end
            stats(ll).obj_classes = [stats(ll).obj_classes; obj_classes(1:size(inters,1))'];
            stats(ll).num_objects = stats(ll).num_objects + size(jaccards,1);
        end
    end

    for ll=1:params.n_r_cand
        stats(ll).jaccard_object = mean(stats(ll).max_J);
        stats(ll).mean_n_masks   = mean(stats(ll).all_n_masks);

        % ----- Compute jaccard at pixel level (J_p) ----
        class_ids = unique(stats(ll).obj_classes);

        % Compute per-class statistics and then mean
        for ii=1:length(class_ids)
            curr_class = class_ids(ii);
            stats(ll).per_class_results{ii}.num_objects = sum(stats(ll).obj_classes==curr_class);

            stats(ll).per_class_results{ii}.max_fp     = stats(ll).max_fp(logical(stats(ll).obj_classes==curr_class),:);
            stats(ll).per_class_results{ii}.max_fn     = stats(ll).max_fn(logical(stats(ll).obj_classes==curr_class),:);
            stats(ll).per_class_results{ii}.max_inters = stats(ll).max_inters(logical(stats(ll).obj_classes==curr_class),:);
            stats(ll).per_class_results{ii}.max_J      = stats(ll).max_J(logical(stats(ll).obj_classes==curr_class),:);
            stats(ll).per_class_results{ii}.meanmax    = mean(stats(ll).per_class_results{ii}.max_J);

            stats(ll).per_class_results{ii}.global_fp      = sum(stats(ll).per_class_results{ii}.max_fp,1);
            stats(ll).per_class_results{ii}.global_fn      = sum(stats(ll).per_class_results{ii}.max_fn,1);
            stats(ll).per_class_results{ii}.global_inters  = sum(stats(ll).per_class_results{ii}.max_inters,1);

            % Compute per-class total inters, fp, fn
            stats(ll).per_class_results{ii}.global_J = ...
                 stats(ll).per_class_results{ii}.global_inters ./...
                (stats(ll).per_class_results{ii}.global_inters+...
                 stats(ll).per_class_results{ii}.global_fp    +...
                 stats(ll).per_class_results{ii}.global_fn);
        end

        % Compute global mean
        tmp = [];
        for ii=1:length(class_ids)
            tmp = [tmp; stats(ll).per_class_results{ii}.global_J];
        end
        stats(ll).jaccard_class = mean(tmp);
    end

    % Store
    save(res_file,'stats');
end


function [lineextimg c] = orient_from_lines(lines, vp, imgwidth, imgheight)

%%
ls = sample_line(lines);
linesamples = cat(1,ls(:).sample);
linesampleclass = cat(1,ls(:).lineclass);

%%
% lineextimg = cell(3,3);
% for i=1:9, lineextimg{i} = zeros(imgheight,imgwidth); end
lineextimg = cell(3,3,2);
for i=1:18, lineextimg{i} = zeros(imgheight,imgwidth); end

%%
% poly = extend_line(line, vp{1}, stoppinglines_sample, imgwidth, imgheight);
for i = 1:length(lines)
    lc = lines(i).lineclass;
    if lc~=0
    for extdir = setdiff(1:3, lc)
        targetdir = setdiff(1:3, [lc extdir]);

%         poly = extend_line_old(lines(i), vp{extdir}, linesamples(linesampleclass==targetdir,:), imgwidth, imgheight);
%         lineextimg{lc,extdir} = lineextimg{lc,extdir} + poly2mask(poly(:,1), poly(:,2), imgheight, imgwidth);

        poly = extend_line(lines(i), vp{extdir}, 1, linesamples(linesampleclass==targetdir,:), imgwidth, imgheight);
        lineextimg{lc,extdir,1} = lineextimg{lc,extdir,1} + poly2mask(poly(:,1), poly(:,2), imgheight, imgwidth);

        poly = extend_line(lines(i), vp{extdir}, -1, linesamples(linesampleclass==targetdir,:), imgwidth, imgheight);
        lineextimg{lc,extdir,2} = lineextimg{lc,extdir,2} + poly2mask(poly(:,1), poly(:,2), imgheight, imgwidth);
    end
    end
end

%%
function poly = extend_line(line, vp, towards_or_away, stoppinglines_sample, imgwidth, imgheight)
% towards_or_away: 1 or -1

p1 = line.point1;
p2 = line.point2;

curp1 = p1; curp2 = p2;
move_amount = 128;
while move_amount>=1
    [newp1 newp2 atvp] = move_line_towards_vp(curp1, curp2, vp, towards_or_away * move_amount);

    failed = 0;
    if atvp==1
%         move_amount = 0; % exit now.
        failed = 1;

    elseif (newp1(1)>imgwidth || newp1(1)<1 || newp1(2)>imgheight || newp1(2)<1) && ...
       (newp2(1)>imgwidth || newp2(1)<1 || newp2(2)>imgheight || newp2(2)<1)
        failed = 1;

    else
        isstop = inpolygon(stoppinglines_sample(:,1), stoppinglines_sample(:,2), ...
            [p1(1) p2(1) newp2(1) newp1(1) p1(1)], [p1(2) p2(2) newp2(2) newp1(2) p1(2)]);

        if any(isstop)
            failed = 1;
        end
    end

    if failed
        move_amount = move_amount/2;
    else
        curp1 = newp1;
        curp2 = newp2;
    end
end
% poly = [curp1(:)'; curp2(:)'];
poly = [p1(:)'; p2(:)'; curp2(:)'; curp1(:)'];

% curp1 = p1; curp2 = p2;
% move_amount = 32;
% while move_amount>=1
%     [newp1 newp2 atvp] = move_line_towards_vp(curp1, curp2, vp, -move_amount);
%
%     failed = 0;
%     if atvp==1
%         move_amount = 0; % exit now.
%
%     elseif (newp1(1)>imgwidth || newp1(1)<1 || newp1(2)>imgheight || newp1(2)<1) && ...
%        (newp2(1)>imgwidth || newp2(1)<1 || newp2(2)>imgheight || newp2(2)<1)
%         failed = 1;
%
%     else
%         isstop = inpolygon(stoppinglines_sample(:,1), stoppinglines_sample(:,2), ...
%             [p1(1) p2(1) newp2(1) newp1(1) p1(1)], [p1(2) p2(2) newp2(2) newp1(2) p1(2)]);
%
%         if any(isstop)
%             failed = 1;
%         end
%     end
%
%     if failed
%         move_amount = move_amount/2;
%     else
%         curp1 = newp1;
%         curp2 = newp2;
%     end
% end
% poly = [poly; curp2(:)'; curp1(:)'];
% poly = [poly; poly(1,:)];

%%

%%
function [newp1 newp2 atvp] = move_line_towards_vp(linep1, linep2, vp, amount)

% d = dist_line_to_point(linep1, linep2, vp);
% r = amount / d;
n1 = norm(vp-linep1);
n2 = norm(vp-linep2);
dir1 = (vp - linep1) / n1;
dir2 = (vp - linep2) / n2;
ratio21 = n2 / n1;

% if n1>amount && n2<amount
%     fprintf('check');
% end

if n1 < amount
    newp1 = linep1;
    newp2 = linep2;
    atvp = 1;
else
    newp1 = linep1 + dir1 * amount;
    newp2 = linep2 + dir2 * amount * ratio21;
    atvp = 0;
end

%%
function [newp1 newp2 atvp] = move_line_towards_vp_old(linep1, linep2, vp, amount)

d = dist_line_to_point(linep1, linep2, vp);
r = amount / d;
if r > 1
    newp1 = vp;
    newp2 = vp;
    atvp = 1;
else
    newp1 = linep1 + (vp-linep1)*r;
    newp2 = linep2 + (vp-linep2)*r;
    atvp = 0;
end


function data = simulate_slideNeg(setup)

%This function simulates the stick sliding to the left

%Set up the integration algorithm
Tspan = setup.Tspan;
IC = [  setup.IC.th;
    setup.IC.x;
    setup.IC.dth;
    setup.IC.dx];
eventFunc = @(t,z)events_slideNeg(t,z,setup.P);
options = odeset(...
    'RelTol',setup.tol,...
    'AbsTol',setup.tol,...
    'Vectorized','on',...
    'MaxStep',setup.odeMaxStep,...
    'Events',eventFunc);
userfun = @(t,z)dynamics_slideNeg(t,z,setup.P);

%Run simulation
sol = feval(setup.solver,userfun,Tspan,IC,options);

%Format for post processing
tspan = [sol.x(1),sol.x(end)];
nTime = ceil(setup.dataFreq*diff(tspan));
t = linspace(tspan(1),tspan(2),nTime);
Z = deval(sol,t);
[~, C, E] = dynamics_slideNeg(t,Z,setup.P);

%Store in a nice format for plotting
data.time = t;
data.state.th = Z(1,:);
data.state.x = Z(2,:);
data.state.dth = Z(3,:);
data.state.dx = Z(4,:);
data.state.y = zeros(1,nTime) + setup.IC.y;
data.state.dy = zeros(1,nTime);
data.contact.h = C(1,:);
data.contact.v = C(2,:);
data.energy.potential = E(1,:);
data.energy.kinetic = E(2,:);
data.P = setup.P;

%Get transitions for finite state machine
data.phase = 'SLIDE_NEG';
if isempty(sol.ie)
    data.exit = 'TIMEOUT';
elseif sol.xe(end) ~= sol.x(end);
    %An event was triggered, but it was non-terminal
    data.exit = 'TIMEOUT';
else
    switch sol.ie(end)
        case 1
            data.exit = 'FALL_NEG';
        case 2
            data.exit = 'FALL_POS';
        case 3
            data.exit = 'STUCK';
        case 4
            data.exit = 'LIFT';
        case 5
            data.exit = 'LIFT';
        otherwise
            error('Invalid exit condition in simulate flight')
    end
end

end


function [xPosV, yPosV, beamlet_delta_x, beamlet_delta_y, beamGeometry]=...
planCToPB(planC, beamNumber, bResx, bResy);

indexS=planC{end};
% assume the words Isocenter Coordinates are enclosed by quotes
% look for the end quotes to denote the begining of the actual numbers
s=planC{indexS.beamGeometry}(beamNumber).file{1};
ind=max(strfind(s, '"'));
isocenter=str2num(s(ind+1:end));
indiceJaws = 2;

% Note: In the RTOG files export by Pinnacle, the isocenter is defined at
% the first three field of
% planC{indexS.beamGeometry}(beamNumber).file
% Not the the first field only.
if (length(isocenter) == 1)
isocenter(1)=str2num(s(ind+1:end));
isocenter(2)=str2num(planC{indexS.beamGeometry}(beamNumber).file{2});
isocenter(3)=str2num(planC{indexS.beamGeometry}(beamNumber).file{3});
indiceJaws = 4;
end

gantryAngle=planC{indexS.beamGeometry}(beamNumber).gantryAngle;
isoDistance=planC{indexS.beamGeometry}(beamNumber).nominalIsocenterDistance;
beamEnergy = planC{indexS.beamGeometry}(beamNumber).beamEnergyMeV;

% if planC{indexS.beamGeometry}(beamNumber).file has only
% 3 entries, then this is a square field defined by collimator
  s=planC{indexS.beamGeometry}(beamNumber).file{indiceJaws};
  ind=max(strfind(s, '"'));
  xjaws=str2num(s(ind+1:end));
  if(length(xjaws)==1),
      xjaws = [xjaws/2 xjaws/2];
  end

  s=planC{indexS.beamGeometry}(beamNumber).file{indiceJaws+1};
  ind=max(strfind(s, '"'));
  yjaws=str2num(s(ind+1:end));
  if(length(yjaws)==1),
      yjaws = [yjaws/2 yjaws/2];
  end

  xjaws(1)=-xjaws(1);
  % by symmetry with the below(line 45),
  % I think I should swap the positive and negative y jaws) ??
  yjaws(3)=yjaws(1);
  yjaws(1)=-yjaws(2);
  yjaws(2)=yjaws(3);
  yjaws(3)=[];

if(length(planC{indexS.beamGeometry}(beamNumber).file)<7),
  % no MLC or block shapes
  % field is formed by the collimator jaws
  MLC = 0;
%   for i=1:yjaws(2)-yjaws(1),
%      x1(i)=xjaws(1);
%      xend(i)=xjaws(2);
%      vertices_y(i)=yjaws(1)-1+i;
%      vertices_ywidth(i) = 1;
%    end
   beamletInput.miny=yjaws(1);
  beamletInput.maxy=yjaws(2);
%   input = [];
%   input(:,1) = [x1'; xend'];
%   input(:,2) = [vertices_y'; vertices_y'];
  beamletInput.xjaws=xjaws;
  beamletInput.yjaws=yjaws;

else
  MLC = 1;
  s=planC{indexS.beamGeometry}(beamNumber).file{7};
  ind=max(strfind(s, '"'));
  numPairs=str2num(s(ind+1:end));
  for i=1:numPairs,
    input(i, :)=str2num(planC{indexS.beamGeometry}(beamNumber).file{7+i});
  end

  % +Y towards head, -Y towards feet (looking from BEV)
  input(:, 2)=-1*input(:,2);

 % now, calculate the intersect between the jaws and the MLC/block
  % taking into account the collimator rotation
  % jaws and collimator are both rotated EXACTLY the same way, so this is
  % just stupid!

  midPointx = xjaws(2) - (xjaws(2) - xjaws(1))/2;
  midPointy = yjaws(2) - (yjaws(2) - yjaws(1))/2;

 ind = find(input(:,1)>=midPointx & input(:,1)>xjaws(2));
  if(~isempty(ind)),
    input(ind, 1) = xjaws(2);
  end
  ind = find(input(:,1)<midPointx & input(:,1)<xjaws(1));
  if(~isempty(ind)),
    input(ind, 1) = xjaws(1);
  end

  ind = find(input(:,2)>=midPointy & input(:,2)>yjaws(2));
  if(~isempty(ind)),
    input(ind, 2) = yjaws(2);
  end
  ind = find(input(:,2)<midPointy & input(:,2)<yjaws(1));
  if(~isempty(ind)),
    input(ind, 2) = yjaws(1);
  end
end

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

  % input is list of 2xn points that form the corners of the polygon
  % want to sort this into rectangles, with lower-left corners (x, y) and
  % width and height (w, h)

  % assume plan is MLC

  beamGeometry.isoDistance = isoDistance;
  beamGeometry.beamEnergy = beamEnergy;
  beamGeometry.isocenter=isocenter;
  beamGeometry.gantryAngle=gantryAngle;
  beamGeometry.collimatorAngle=planC{indexS.beamGeometry}(beamNumber).collimatorAngle;
  beamGeometry.couchAngle=planC{indexS.beamGeometry}(beamNumber).couchAngle;

  beamN=planC{indexS.beamGeometry}(beamNumber);
  if(MLC==0),
      % different function if the field shaping is done just by the
      % collimator jaws
      nBeamletx = ceil(diff(beamletInput.xjaws)/bResx)+1;
      nBeamlety = ceil(diff(beamletInput.yjaws)/bResy)+1;
      xvals = linspace(beamletInput.xjaws(1), beamletInput.xjaws(2), nBeamletx);
      yvals = linspace(beamletInput.yjaws(1), beamletInput.yjaws(2), nBeamlety);
      for i=1:length(yvals)-1,
          beamlets{i} = xvals;
          beamletsy{i} = yvals(i);
          beamletsDeltay{i} = yvals(i+1) - yvals(i);
      end

  else
      [x1, xend, vertices_y, vertices_ywidth]=cornersMLC(input);
      % redo setting y-vertices based on jaws
      if(vertices_y(1)<yjaws(1))
          vertices_y(1) = yjaws(1);
          vertices_ywidth(1) = vertices_y(2) -vertices_y(1);
      end
      if(vertices_y(end)+vertices_ywidth(end)>yjaws(2))
          vertices_ywidth(end) = yjaws(2) - vertices_y(end);
      end

      beamletInput.input=input;
      beamletInput.miny=min(input(:,2));
      beamletInput.maxy=max(input(:,2));
      beamletInput.x1=x1;
      beamletInput.xend=xend;
      beamletInput.vertices_y=vertices_y;
      beamletInput.vertices_ywidth=vertices_ywidth;
      beamletInput.xjaws=xjaws;
      beamletInput.yjaws=yjaws;

      numBeamlets=ceil((xend-x1)/bResx)+1;
      %numBeamlets_y=ceil(length(vertices_y)/bResy);

      numcopies = ceil(vertices_ywidth/bResy);
      % still need to deal with "fractional" number of copies

      for i=1:length(x1),
          beamlets{i}=linspace(x1(i), xend(i), numBeamlets(i));
      end

      cntr=1;

      beamletsOrig = beamlets;
      beamlets = [];

      for i=1:length(beamletsOrig),
          if(numcopies(i)==1),
              y=vertices_y(i);
              beamletsy{cntr} = y;
              ynext = vertices_y(i)+vertices_ywidth(i);
              beamletsDeltay{cntr} = ynext - y;
              beamlets{cntr} = beamletsOrig{i};
              cntr = cntr+1;
          else
              for k=1:numcopies(i)
                  y=vertices_y(i) + bResy*(k-1);
                  beamletsy{cntr} = y;
                  ynext=y+bResy;
                  beamletsDeltay{cntr} = ynext -y;
                  beamlets = insertCellEntry(beamlets, cntr, beamletsOrig{i});
                  cntr=cntr+1;
              end
          end
      end

  end

  xPosV = [];
  yPosV = [];
  beamlet_delta_x = [];
  beamlet_delta_y = [];

  for i = 1:length(beamlets)
      n = length(beamlets{i})-1;
      xPosV = [xPosV beamlets{i}(1:n)];
      beamlet_delta_x = [beamlet_delta_x diff(beamlets{i})];
      yPosV = [yPosV ones(1, n)*beamletsy{i}];
      beamlet_delta_y = [beamlet_delta_y ones(1, n)*beamletsDeltay{i}];
  end

  % need to move the position to the center of the voxel?
  xPosV = xPosV + beamlet_delta_x/2;
  yPosV = yPosV + beamlet_delta_y/2;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%
  function [x1, xend, vertices_y, vertices_ywidth]=cornersMLC(input);

  input(:,1) = round(input(:,1)*10)/10;
  % assume 0.5 cm leaf width as the narrowest
  input(:,2) = round(input(:,2)*2)/2;

  input = checkInputPoints(input);
  miny=min(input(:,2));
  maxy=max(input(:,2));

  yvalues=sort(unique(input(:,2)));

  % Have to deal with the case that the 1st and/or last MLC positions
  % are caused by collimator jaws not MLC

  % still assuming 1 cm leaf widths, so

  %if((mod(miny, 1)~=0) | mod(maxy, 1)~=0)

  % miny1=floor(miny);
  % maxy1=ceil(maxy);
  % input(input(:,2)==miny, 2)=miny1;
  % input(input(:,2)==maxy, 2)=maxy1;
  %
  % miny=miny1;
  % maxy=maxy1;

  % numRect=maxy-miny+1;
  %
  % for i=1:numRect
  %     vertices{i}=sort(input(input(:,2)==i+miny-1, 1));
  %     vertices_y(i)=i+miny-1;
  % end

  numRect=length(yvalues);

  for i=1:numRect
      vertices{i}=sort(input(input(:,2)==yvalues(i), 1));
      vertices_y(i)=yvalues(i);
  end

  if(length(vertices{1}>2)),
      vertices{1} = unique(vertices{1});
  end
  if(length(vertices{end}>2)),
      vertices{end} = unique(vertices{end});
  end

  for i = 2:length(vertices)-1,
      if(length(vertices{i})>4)
          vertices{i} = unique(vertices{i});
      end
  end

  % for i = 2:length(vertices)-1,
  %     if(length(vertices{i}==2)),
  %         % either left or right side MLC positions not given because leaf
  %         % same as leaf above and below
  %

  x1(1)=vertices{1}(1);
  xend(1)=vertices{1}(end);

  %find the points in each line which are also in the next line
  % because the vertices are sorted, 1:2 will be left, and 3:4 will be right
  % vertices

  % june30, 2005, if it's a straight vertical line, there may be only 2
  % vertices!

  for i=2:numRect-1
      %     if(length(vertices{i} == 2)),
      %         x1(i) = vertices{i}(1);
      %         xend(i) = vertices{i}(2);
      %     else

      t=vertices{i}(1:2);
      if(t(1)==t(2)),
          x1(i)=t(1);
      else
          x1(i)=t(t~=x1(i-1));
      end
      t=vertices{i}(3:4);
      if(t(1)==t(2)),
          xend(i)=t(1);
      else
          xend(i)=t(t~=xend(i-1));
      end
      % end

  end

  vertices_ywidth = diff(vertices_y);
  vertices_y=vertices_y(1:end-1);


function hra = computeHourAngle(ut, long, bodyInfo)
%     if(bodyInfo.propTypeEnum == BodyPropagationTypeEnum.TwoBody)
%         chain = bodyInfo.getOrbitElemsChain();
%         rVectBodyToSun = -1.0 * getPositOfBodyWRTSun_alg(ut, chain{:});
%
% 	elseif(bodyInfo.propTypeEnum == BodyPropagationTypeEnum.Numerical)
%
%     else
%
%     end
    [rVectBodyToSun, ~] = getPositOfBodyWRTSun(ut, bodyInfo, bodyInfo.celBodyData);
    rVectBodyToSun = -1.0 * rVectBodyToSun;

    rVectBodyToSun = reshape(rVectBodyToSun, 3,numel(ut));
    rVectBodyToSunNorm = sqrt(sum(rVectBodyToSun.^2,1));

    inputs = bodyInfo.getFixedFrameFromInertialFrameInputsCache();
    rVectSunECEF = getFixedFrameVectFromInertialVect_alg(ut, rVectBodyToSun, inputs{:});

    rECEF = getrVectEcefFromLatLongAlt(zeros(size(long)), long, zeros(size(long)), bodyInfo);

    planarRvectEcef = [rECEF(1,:); rECEF(2,:); zeros(1,size(rECEF,2))];
    planarRvectSunEcef = [rVectSunECEF(1,:); rVectSunECEF(2,:); zeros(1,size(rVectSunECEF,2))];

    hra = angleNegPiToPi(dang(planarRvectEcef,planarRvectSunEcef));
    hra(rVectBodyToSunNorm == 0) = 1.0;
    hra = reshape(hra,size(ut));
end


%%*******************************************************************
%% HKMrhsfun: compute the right-hand side vector of the
%%            Schur complement equation for the  HKM direction.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

function [rhs,EinvRc,hRd] = HKMrhsfun(blk,At,par,X,Z,rp,Rd,sigmu,hRd,dX,dZ)

m = length(rp);
if (nargin > 8)
    corrector = 1;
else
    corrector = 0;
    hRd = zeros(m,1);
end
hEinvRc = zeros(m,1);
EinvRc = cell(size(blk,1),1);
rhsfree = [];
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    n = sum(pblk{2});
    if strcmp(pblk{1},'l')
        if iscell(sigmu)
            EinvRc{p} = sigmu{p}./Z{p} -X{p};
        else
            EinvRc{p} = sigmu./Z{p} -X{p};
        end
        Rq = sparse(n,1);
        if (corrector) && (norm(par.parbarrier{p})==0)
            Rq = dX{p}.*dZ{p}./Z{p};
        else
            tmp  = par.dd{p}.*Rd{p};
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = mexMatvec(At{p,1},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'q')
        if iscell(sigmu)
            EinvRc{p} = qops(pblk,sigmu{p},par.Zinv{p},3) -X{p};
        else
            EinvRc{p} = sigmu*par.Zinv{p} -X{p};
        end
        Rq = sparse(n,1);
        if (corrector) && (norm(par.parbarrier{p})==0)
            ff{p} = qops(pblk,1./par.gamz{p},Z{p},3); %#ok
            hdx = qops(pblk,par.gamz{p},ff{p},5,dX{p});
            hdz = qops(pblk,par.gamz{p},ff{p},6,dZ{p});
            hdxdz = Arrow(pblk,hdx,hdz);
            Rq = qops(pblk,par.gamz{p},ff{p},6,hdxdz);
        else
            tmp = par.dd{p}.*Rd{p} ...
                + qops(pblk,qops(pblk,Rd{p},par.Zinv{p},1),X{p},3) ...
                + qops(pblk,qops(pblk,Rd{p},X{p},1),par.Zinv{p},3);
            tmp2 = mexMatvec(At{p,1},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = mexMatvec(At{p,1},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'s')
        if iscell(sigmu)
            %%ss = [0,cumsum(pblk{2})];
            %%sigmuvec = zeros(n,1);
            %%for k = 1:length(pblk{2});
            %%   sigmuvec(ss(k)+1:ss(k+1)) = sigmu{p}(k)*ones(pblk{2}(k),1);
            %%end
            sigmuvec = mexexpand(pblk{2},sigmu{p});
            EinvRc{p} = par.Zinv{p}*spdiags(sigmuvec,0,n,n) -X{p};
        else
            EinvRc{p} = sigmu*par.Zinv{p} -X{p};
        end
        Rq = sparse(n,n);
        if (corrector) && (norm(par.parbarrier{p})==0)
            Rq = Prod3(pblk,dX{p},dZ{p},par.Zinv{p},0);
            Rq = 0.5*(Rq+Rq');
        else
            tmp = Prod3(pblk,X{p},Rd{p},par.Zinv{p},0,par.nzlistAy{p});
            tmp = 0.5*(tmp+tmp');
            tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),{tmp});
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'u')
        rhsfree = [rhsfree; Rd{p}]; %#ok
    end
end
%%
rhs = rp + hRd - hEinvRc;
rhs = full([rhs; rhsfree]);
%%*******************************************************************


function [params] = sr_calcSRC(params)
% function [params] = sr_calcZ(params)
% -------------------------------------
% Calculation of seismic rate changes. This function calculates the rates
% for all grid points together.
%
% Input parameters:
% please see function ~/zmap/src/thomas/seismicrates/get_parameter.m
%
% Output parameters:
%   Same as input parameters including but additionally
%   params.mResult_    z,probability of z, beta, probability of beta, and
%                      resolution. This data will be stored into
%                      params.mResult{n}, where n is the number of runs
%                      (see sr_startZ.m)
%   params.mVar        List of all combinations of parameters used for
%                      searching the parameter space ([N, Tw, Tbin]).
%   params.m1          Max. value for z ([z,Resolution,N,Tw,Tbin])
%   params.m2          Max. value for p(z) ([z,Resolution,N,Tw,Tbin])
%   params.m3          Max. value for beta ([z,Resolution,N,Tw,Tbin])
%   params.m4          Max. value for p(beta) ([z,Resolution,N,Tw,Tbin])
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Th. van Stiphout vanstiphout@sed.ethz.ch
% last update: 16.08.2005
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

report_this_filefun(mfilename('fullpath'));

% Initialize
bChk=false;    % for debugging the code

% Determine time period of catalog
params.fTminCat = min(params.mCatalog(:,3));
params.fTmaxCat = max(params.mCatalog(:,3));
% Adjust to decimal years
% fTimePeriod =params.fTimePeriod/365;
% mCat=params.mCatalog(:,3);
mLoc=[params.mCatalog(:,1) params.mCatalog(:,2) params.mCatalog(:,7) ];
% Init result matrix
% mValueGrid_ = [];
%  Selection criteria for subcatalog (between StartTime and
% TimeCut + WindowLength

% Create Indices of the catalog for each Node and select quakes in time
[params.caNodeIndices, params.vResolution] = ex_CreateIndexCatalog(params.mCatalog,...
    params.mPolygon, params.bMap, 0, size(params.mCatalog,1),...
    params.fRadius, params.fSizeRectHorizontal, params.fSizeRectDepth);

% prepare input parameter matrix  [ vN   vTw   vTbin ]
mVar(:,1)=repmat(params.vN,size(params.vTw,1)*size(params.vTbin,1),1);
mVar(:,2)=reshape(repmat(params.vTw',size(params.vN,1)*size(params.vTbin,1),1),...
    size(params.vTw,1)*size(params.vN,1)*size(params.vTbin,1),1);
mVar(:,3)=repmat(reshape(repmat(params.vTbin',size(params.vN,1),1),...
    size(params.vTbin,1)*size(params.vN,1),1),size(params.vTw,1),1);

% loop over all input parameters
for i=1:size(mVar,1)

    % get input parameters
    fTstart=params.fTstart;
    fT=params.fT;
    fTw=mVar(i,2);
    nN=mVar(i,1);
    nTbin=mVar(i,3);
    mNumDeclus_=params.mNumDeclus(:,end);
%     sum(params.mNumDeclus  )
    % vSelMag=(params.mCatalog(:,6)>=params.fMc)

    % create vectors for time periods
    vSelR0_=(params.mCatalog(:,3) > fTstart);
    vSelR1_=(params.mCatalog(:,3) <= fT);
    vSelR2_=(params.mCatalog(:,3) >= fT-fTw);
    vSelR3_=(params.mCatalog(:,3)  <= fT-fTw);

    % combine time periods with decluster info's
    mSelD0_=repmat(vSelR0_,1,size(mNumDeclus_,2)) & mNumDeclus_;
    mSelD1_=repmat(vSelR1_,1,size(mNumDeclus_,2)) & mNumDeclus_;
    mSelD2_=repmat(vSelR2_,1,size(mNumDeclus_,2)) & mNumDeclus_;
    mSelD3_=repmat(vSelR3_,1,size(mNumDeclus_,2)) & mNumDeclus_;

    % only events during certain period
    mSelD00=( mSelD0_ & mSelD1_);   % whole period
    mSelD10=( mSelD0_ & mSelD3_);   % 1st period
    mSelD20=( mSelD1_ & mSelD2_);   % 2nd period

    % events in whole period  (us it for calculating probabilities of z and beta)
    mCat=params.mCatalog(mSelD00,3);

    % repeat matrix for multiplication with grid node distance-ranking
    aSelD02=repmat(mSelD00,1,size(params.mPolygon,1));
    % if gridding is based on radius
    if params.nGriddingMode==1
        for pp=1:size(params.mPolygon,1)
            vNaN(:,pp)=params.vResolution{pp}>params.fRadius;
%             params.caNodeIndices{pp}(vNaN(:,pp))=NaN;
            vResolutionN_(pp)=sum(not(vNaN(:,pp)));
        end
        nN=max(vResolutionN_);
    end
    % transform cells of node indices to matrix (column wise)
    caNodeIndices_=cell2mat(params.caNodeIndices');   % distance-ranking of each event from each grid node

    % create help matrices
    tmp=zeros(size(aSelD02));
    tmp1=(0:1:size(aSelD02,2)-1)*size(params.mCatalog,1);
    tmp2=repmat(tmp1,size(aSelD02,1),1);  %

    % sort rankings for each grid node
    [X1, I1] =sort(caNodeIndices_);
    % do reverse "sort ranking" for each grid node
    [X2, I2] =sort(I1);

    I1=I1+tmp2;I2=I2+tmp2;
    bSelD02=aSelD02(I2); % resort 1-0 matrix
    cSelD02=cumsum(bSelD02);
    dSelD02=logical(bSelD02.*(cSelD02<=nN)); % only the first nN events
    mSelRes=dSelD02;  % copy for the first nN events
    dSelD02=dSelD02(I1); % transform back  / nN Events over whole catalog per grid node
    % transformation back of NaN's-list for event that are not in a certain params.fRadius
    if params.nGriddingMode==1
        vNaN1=vNaN(I1);
    end
    %     eSelD02=bSelD02(I1); % transform back
    %     Checkpoint
    if bChk
        ii=ceil(rand(1)*length(params.mPolygon));
        figure;plot(mLoc(:,1),mLoc(:,2),'.c');
        hold on;plot(params.mPolygon(ii,1),params.mPolygon(ii,2),'ok');
        %  hold on;plot(mLoc(I2(1:nN,ii)-(ii-1)*size(mLoc,1),1),mLoc(I2(1:nN,ii)-(ii-1)*size(mLoc,1),2),'dr')
    end

    % reshape resolution matrix
    vResolution_=cell2mat(params.vResolution');

    % prepare catalog (origin times) - copy them column-wise for each
    % grid point
    mCat0=params.mCatalog(:,3);
    mCat0=(repmat(mCat0,1,size(dSelD02,2)));

    if bChk
        % and for latitude and longitude aswell
        mLon0=params.mCatalog(:,1);
        mLon0=(repmat(mLon0,1,size(dSelD02,2)));
        mLat0=params.mCatalog(:,2);
        mLat0=(repmat(mLat0,1,size(dSelD02,2)));
    end

    % for gridding mode 1 (radius)
    if params.nGriddingMode==1
        adder=repmat(0:size(caNodeIndices_,1):size(caNodeIndices_,1)*(size(caNodeIndices_,2)-1),size(caNodeIndices_,1),1);
        caNodeIndices2_=caNodeIndices_+adder;
        caNodeIndices3_=caNodeIndices2_(~isnan(caNodeIndices2_));
        caNodeIndices4_=zeros(size(caNodeIndices_));
        caNodeIndices4_(caNodeIndices3_)=1;
        caNodeIndices4_=logical(caNodeIndices4_);
%         hold on;plot(params.mCatalog(caNodeIndices4_(:,ii),1),...
%                                  params.mCatalog(caNodeIndices4_(:,ii),2),'sr')
         mCat0(not(caNodeIndices4_))=NaN;
         mLon0(not(caNodeIndices4_))=NaN;
         mLat0(not(caNodeIndices4_))=NaN;

         clear caNodeIndices2_ caNodeIndices3_
    end

    % prepare matrices for subset of catalog
    gSelD02=ones(size(dSelD02))*NaN;
    mCat00=gSelD02; % whole catalog
    mCat10=gSelD02; % 1st period
    mCat20=gSelD02; % 2nd period
    mLon00=gSelD02; mLon00_=mLon00;
    mLat00=gSelD02;    mLat00_=mLat00;
    % put the times in the NaN's matrix
    gSelD02(dSelD02)=(mCat0(dSelD02));

    % whole catalog
    mSelD00=repmat(mSelD00,1,size(dSelD02,2));
    mSelD01=(dSelD02&mSelD00);
    mCat00(mSelD01)=(mCat0(mSelD01));
    if params.nGriddingMode==1
        mCat00(vNaN1)=NaN;
    end
    % Checkpoint
    if bChk
        % figure;histogram(mCat00(:,1),100)
        % Checkpoint plot location
        mLon00=mLon00_;mLat00=mLat00_;
        mLon00(mSelD01)=(mLon0(mSelD01));
        mLat00(mSelD01)=(mLat0(mSelD01));
%         hold on;plot(mLon00(:,ii),mLat00(:,ii),'xk','MarkerSize',10);
        hold on;plot(mLon00(not(vNaN1(:,ii)),ii),mLat00(not(vNaN1(:,ii)),ii),'xk','MarkerSize',10);
    end

    % 1st period
    mSelD10=repmat(mSelD10,1,size(dSelD02,2));
    mSelD11=(dSelD02&mSelD10);
    mCat10(mSelD11)=(mCat0(mSelD11));
    if params.nGriddingMode==1
        mCat10(vNaN1)=NaN;
    end
    % Checkpoint
    if bChk
        % figure;histogram(mCat10(:,1),100)
        % Checkpoint plot location
        mLon10=mLon00_;mLat10=mLat00_;
        mLon10(mSelD11)=(mLon0(mSelD11));
        mLat10(mSelD11)=(mLat0(mSelD11));
%         hold on;plot(mLon10(:,ii),mLat10(:,ii),'dr','MarkerSize',10);
        hold on;plot(mLon10(not(vNaN1(:,ii)),ii),mLat10(not(vNaN1(:,ii)),ii),'dr','MarkerSize',10);
    end

    % 2nd period
    mSelD20=repmat(mSelD20,1,size(dSelD02,2));
    mSelD21=(dSelD02&mSelD20);
    mCat20(mSelD21)=(mCat0(mSelD21));
    if params.nGriddingMode==1
        mCat20(vNaN1)=NaN;
    end
    % Checkpoint
    if bChk
        % figure;histogram(mCat20(:,1),100)
        % Checkpoint plot location
        mLon20=mLon00_;mLat20=mLat00_;
        mLon20(mSelD21)=(mLon0(mSelD21));
        mLat20(mSelD21)=(mLat0(mSelD21));
%         hold on;plot(mLon20(:,ii),mLat20(:,ii),'sb','MarkerSize',10);
        hold on;plot(mLon20(not(vNaN1(:,ii)),ii),mLat20(not(vNaN1(:,ii)),ii),'sb','MarkerSize',10);
    end

    %  lta
    [mLTA, mLTAprob] =calc_zlta(mCat,mCat00,mCat20,params.fTstart, fT,...
        fTw,nTbin, nN);
    % mBeta
    [mBeta, mBetaprob] =calc_beta(mCat,mCat00,mCat20,params.fTstart, fT,...
        fTw,nTbin, nN);
    %      mBeta(:,kk)=calc_beta(mCat10,mCat20,params.fStartTime,...
    %          params.fTimeCut,params.fTwLength,params.fTimeSteps);

    if params.nGriddingMode==1
        % Resolution by No.of Events in a certain distance from grid node,
        % defined by params.fRadius .
        mResolution=vResolutionN_';
    else
        % Resolution by N-th event in radii-vector
        mResolution=max(vResolution_.*mSelRes)';
    end
    % preallocation of mResult_
    if i==1
        mResult_=ones(size(params.mPolygon,1),5,params.nMCS).*NaN;
    end
    mResult_(:,:,i)=[mLTA mLTAprob mBeta mBetaprob double(mResolution)];

    sString=sprintf('End loop %d with N=  %d  Tw=  %d   Tbin=  %d   ',i,...
        mVar(i,1),mVar(i,2),mVar(i,3));
    disp(sString);
end % end loop variation over input parameter

% params.mResult_=mResult_;
% saving parameter space
% params.mVar=mVar;

% Maxima search
[mMaxValue, mMaxIndices]=max(mResult_,[],3);
% ask for parameter settings (parameter settings for max for each  grid
% point)
% nMax=2; % (i.e. 1: z, 2:prob(z), 3:beta, 4:prob(beta),5:radius)
mParamax1=mVar(mMaxIndices(:,1),1:3);
mParamax2=mVar(mMaxIndices(:,2),1:3);
mParamax3=mVar(mMaxIndices(:,3),1:3);
mParamax4=mVar(mMaxIndices(:,4),1:3);

% plotting preliminary results / data
if bChk
    % plot results for grid points with contour lines that indicate z,
    % prob(z), beta, prob(beta)
    %     figure;pcolor( params.mPolygon(:,1), params.mPolygon(:,2), mResult_(:,1))
    figure;
    XX_=repmat(params.vN,1,size(params.vTw,1));
    YY_=repmat(params.vTw,1,size(params.vN,1))';
    for ii_=1:10
        ngp_=ceil(rand(1,1)*1170);
        for kk_=1:4
            subplot(2,2,kk_);
            contour(XX_,YY_,reshape(squeeze(mResult_(ngp_,kk_,:)),...
                size(params.vN,1),size(params.vTw,1)));
            colorbar;
        end
        pause
    end
    %     pcolor(reshape(squeeze(mResult_(1,1,:)),size(params.vN,1),size(params.vTw,1)));
end

% Prepare mResult_
% vSel=find(mResult_(:,5) > params.fMaxRadius);
% mResult_(vSel,1:4)=NaN';

% Pre Result matrix (result for 1
params.m1=[mMaxValue(:,1) double(mResolution) mParamax1];
params.m2=[mMaxValue(:,2) double(mResolution) mParamax2];
params.m3=[mMaxValue(:,3) double(mResolution) mParamax3];
params.m4=[mMaxValue(:,4) double(mResolution) mParamax4];
% figure;pcolor(reshape(params.mPreResult(:,1),39,30));
% colorbar


classdef GenericLinearTangentSteeringModel < AbstractSteeringModel
    %GenericLinearTangentSteeringModel Summary of this class goes here
    %   Detailed explanation goes here

    properties
        gammaAngleModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);
        betaAngleModel(1,1) LinearTangentModel = LinearTangentModel(0,0,0,0,0);
        alphaAngleModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);

        gammaContinuity(1,1) logical = false;
        betaContinuity(1,1) logical = false;
        alphaContinuity(1,1) logical = false;

        refFrame AbstractReferenceFrame
        controlFrame AbstractControlFrame
    end

    methods
        function dcm = getBody2InertialDcmAtTime(obj, ut, rVect, vVect, bodyInfo)
            gammaAng = obj.gammaAngleModel.getValueAtTime(ut);
            betaAng = obj.betaAngleModel.getValueAtTime(ut);
            alphaAng = obj.alphaAngleModel.getValueAtTime(ut);

%             elemSet = CartesianElementSet(ut, rVect(:), vVect(:), bodyInfo.getBodyCenteredInertialFrame());
%             if(not(isempty(obj.refFrame.getOriginBody())))
%                 elemSet = elemSet.convertToFrame(obj.refFrame);
%             end

            dcm = real(obj.controlFrame.computeDcmToInertialFrame(ut, rVect, vVect, bodyInfo, gammaAng, betaAng, alphaAng, obj.refFrame));
        end

        function [angleModel, continuity] = getAngleNModel(obj, n)
            angleModel = PolynominalModel.empty(1,0);

            switch n
                case 1
                    angleModel = obj.gammaAngleModel;
                    continuity = obj.gammaContinuity;
                case 2
                    angleModel = obj.betaAngleModel;
                    continuity = obj.betaContinuity;
                case 3
                    angleModel = obj.alphaAngleModel;
                    continuity = obj.alphaContinuity;
            end
        end

        function t0 = getT0(obj)
            t0 = obj.alphaAngleModel.t0;
        end

        function setT0(obj, newT0)
            obj.gammaAngleModel.t0 = newT0;
            obj.betaAngleModel.t0 = newT0;
            obj.alphaAngleModel.t0 = newT0;
        end

        function setConstTerms(obj, gammaConst, alphaConst)
            obj.gammaAngleModel.constTerm = gammaConst;
            obj.alphaAngleModel.constTerm = alphaConst;
        end

        function setLinearTerms(obj, gammaRate, alphaRate)
            obj.gammaAngleModel.linearTerm = gammaRate;
            obj.alphaAngleModel.linearTerm = alphaRate;
        end

        function setAccelTerms(obj, gammaRateRate, alphaRateRate)
            obj.gammaAngleModel.accelTerm = gammaRateRate;
            obj.alphaAngleModel.accelTerm = alphaRateRate;
        end

        function setLinearTangentTerms(obj, a, a_dot, b, b_dot)
            obj.betaAngleModel.a = a;
            obj.betaAngleModel.a_dot = a_dot;
            obj.betaAngleModel.b = b;
            obj.betaAngleModel.b_dot = b_dot;
        end

        function setTimeOffsets(obj, timeOffset)
            obj.alphaAngleModel.tOffset = timeOffset;
            obj.betaAngleModel.tOffset = timeOffset;
            obj.gammaAngleModel.tOffset = timeOffset;
        end

        function [angle1Cont, angle2Cont, angle3Cont] = getContinuityTerms(obj)
            angle1Cont = obj.gammaContinuity;
            angle2Cont = obj.betaContinuity;
            angle3Cont = obj.alphaContinuity;
        end

        function setContinuityTerms(obj, angle1Cont, angle2Cont, angle3Cont)
            obj.gammaContinuity = angle1Cont;
            obj.betaContinuity = angle2Cont;
            obj.alphaContinuity = angle3Cont;
        end

        function setConstsFromDcmAndContinuitySettings(obj, dcm, ut, rVect, vVect, bodyInfo)
            if(obj.gammaContinuity || obj.betaContinuity || obj.alphaContinuity)
                elemSet = CartesianElementSet(ut, rVect(:), vVect(:), bodyInfo.getBodyCenteredInertialFrame());

%                 if(not(isempty(obj.refFrame.getOriginBody())))
%                     elemSet = elemSet.convertToFrame(obj.refFrame);
%                 end

                [gammaAngle, betaAngle, alphaAngle] = obj.controlFrame.getAnglesFromInertialBodyAxes(LaunchVehicleAttitudeState(dcm), elemSet.time, elemSet.rVect(:), elemSet.vVect(:), elemSet.frame.getOriginBody(), obj.refFrame);

                if(obj.gammaContinuity)
                    obj.gammaAngleModel.constTerm = gammaAngle;
                end

                if(obj.betaContinuity)
                    obj.betaAngleModel.setBForContinuity(betaAngle);
                end

                if(obj.alphaContinuity)
                    obj.alphaAngleModel.constTerm = alphaAngle;
                end
            end
        end

        function setInitialAttitudeFromState(obj, stateLogEntry, tOffsetDelta)
            t0 = stateLogEntry.time;
            obj.setT0(t0);

            obj.gammaAngleModel.tOffset = obj.gammaAngleModel.tOffset + tOffsetDelta;
            obj.betaAngleModel.tOffset = obj.betaAngleModel.tOffset + tOffsetDelta;
            obj.alphaAngleModel.tOffset = obj.alphaAngleModel.tOffset + tOffsetDelta;
        end

        function [angle1Name, angle2Name, angle3Name] = getAngleNames(obj)
            angleNames = obj.controlFrame.getControlFrameEnum().angleNames;
            angle1Name = angleNames{1};
            angle2Name = angleNames{2};
            angle3Name = angleNames{3};
        end

        function newSteeringModel = deepCopy(obj)
            newSteeringModel = GenericPolySteeringModel(obj.gammaAngleModel.deepCopy(), obj.betaAngleModel.deepCopy(), obj.alphaAngleModel.deepCopy());
            newSteeringModel.gammaContinuity = obj.gammaContinuity;
            newSteeringModel.betaContinuity = obj.betaContinuity;
            newSteeringModel.alphaContinuity = obj.alphaContinuity;
            newSteeringModel.refFrame = obj.refFrame;
            newSteeringModel.controlFrame = obj.controlFrame;
        end

        function optVar = getNewOptVar(obj)
            optVar = SetGenericLinearTangentSteeringModelActionOptimVar(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function tf = usesRefFrame(~)
            tf = true;
        end

        function refFrame = getRefFrame(obj)
            refFrame = obj.refFrame;
        end

        function setRefFrame(obj, refFrame)
            obj.refFrame = refFrame;
        end

        function tf = usesControlFrame(~)
            tf = true;
        end

        function cFrame = getControlFrame(obj)
            cFrame = obj.controlFrame;
        end

        function setControlFrame(obj, cFrame)
            obj.controlFrame = cFrame;
        end

        function tf = requiresReInitAfterSoIChange(obj)
            tf = false;
        end

        function enum = getSteeringModelTypeEnum(~)
            enum = SteerModelTypeEnum.LinearTangentAngles;
        end
    end

    methods(Access=private)
        function obj = GenericLinearTangentSteeringModel(gammaAngleModel, betaAngleModel, alphaAngleModel)
            obj.gammaAngleModel = gammaAngleModel;
            obj.betaAngleModel = betaAngleModel;
            obj.alphaAngleModel = alphaAngleModel;
        end
    end

    methods(Static)
        function model = getDefaultSteeringModel()
            gammaAngleModel = PolynominalModel(0,0,0,0);
            betaAngleModel = LinearTangentModel(0,0,0,0,0);
            alphaAngleModel = PolynominalModel(0,0,0,0);

            model = GenericLinearTangentSteeringModel(gammaAngleModel, betaAngleModel, alphaAngleModel);
        end

        function typeStr = getTypeNameStr()
            typeStr = SteeringModelEnum.GenericLinTan.nameStr;
        end
    end
end


function net = vgg16_intialization_down5(warpsize,cdim,ndim, RoIRoD)

net = load('data/pretrained_models/imagenet-vgg-verydeep-16.mat');
net = vl_simplenn_tidy(net);
net.layers = net.layers(1:31);

% Convert to DagNN.
net = dagnn.DagNN.fromSimpleNN(net, 'canonicalNames', true) ;

% Dimension reduction
net.addLayer('dimred', convBlocknoPad(1,1,512,cdim), {'x31'}, {'xRed'}, {'dimred_filter','dimred_bias'});
net = initLayerParams(net);

% RoIAlign
if strcmp(RoIRoD,'RoIRoD') || strcmp(RoIRoD,'RoIOnly')
    roiSize = [warpsize,warpsize];
    net.addLayer('roialign', dagnn.RoiAlign('subdivisions',roiSize,'transformation',1/32), {'xRed','rois'}, 'xRoi');
    net.addLayer('roipool', dagnn.Pooling('method', 'avg', 'poolSize', [2 2], 'stride', 1), {'xRoi'}, {'pool_xRoi'}, {});
end

% RoDAlign
if strcmp(RoIRoD,'RoIRoD') || strcmp(RoIRoD,'RoDOnly')
    rodSize = [warpsize+1,warpsize+1];
    net.addLayer('rodalign', dagnn.RodAlign('subdivisions',rodSize,'transformation',1/32), {'xRed','rois'}, 'xRod');
    net.addLayer('rodpool', dagnn.Pooling('method', 'avg', 'poolSize', [2 2], 'stride', 1), {'xRod'}, {'pool_xRod'}, {});
end

switch RoIRoD
    case 'RoIRoD'
        net.addLayer('concat', dagnn.Concat('dim', 3), {'pool_xRoi', 'pool_xRod'}, {'xConc'}, {});
        net.addLayer('fc1', convBlocknoPad(warpsize,warpsize,cdim*2,ndim), {'xConc'}, {'fc1'}, {'fc1_filter', 'fc1_bias'});
    case 'RoIOnly'
        net.addLayer('fc1', convBlocknoPad(warpsize,warpsize,cdim,ndim), {'pool_xRoi'}, {'fc1'}, {'fc1_filter', 'fc1_bias'});
    case 'RoDOnly'
        net.addLayer('fc1', convBlocknoPad(warpsize,warpsize,cdim,ndim), {'pool_xRod'}, {'fc1'}, {'fc1_filter', 'fc1_bias'});
    otherwise
        error('Unknown RoIRoD choice.');
end
net = initLayerParams(net);
net.addLayer('relu_fc1', dagnn.ReLU(), {'fc1'}, {'relu_fc1'}, {});
net.addLayer('fc2', convBlocknoPad(1,1,ndim,ndim), {'relu_fc1'}, {'fc2'}, {'fc2_filter', 'fc2_bias'});
net = initLayerParams(net);
net.addLayer('relu_fc2', dagnn.ReLU(), {'fc2'}, {'relu_fc2'}, {});

net.addLayer('dropout',dagnn.DropOut('rate',0.5),'relu_fc2','dropout',{});

net.addLayer('predcls',convBlocknoPad(1,1,ndim,1), 'dropout','predcls',{'predcls_filter','predcls_bias'});
net = initLayerParams(net);

% net.print({'input', [256 256 3]}, 'all', true);
net.addLayer('losscls',dagnn.RegressionLoss('lossType','Huber'), {'predcls','label'}, 'losscls',{});

% Meta parameters
net.meta.trainOpts.learningRate = [logspace(-5,-4,5) logspace(-4,-4,35)];
net.meta.trainOpts.weightDecay = 0.0001;
net.meta.trainOpts.batchSize = 1 ;
net.meta.trainOpts.solver = @solver.adam ;
net.meta.trainOpts.numEpochs = numel(net.meta.trainOpts.learningRate) ;
net.meta.normalization.averageImage = reshape([122.7717 102.9801 115.9465],[1 1 3]);

end

function convObj = convBlocknoPad(fh,fw,fc,k)
    convObj = dagnn.Conv('size', [fh fw fc k], 'hasBias', true);
end

function net = initLayerParams(net)
    p = net.getParamIndex(net.layers(end).params) ;
    params = net.layers(end).block.initParams();
    [net.params(p).value] = deal(params{:}) ;
end


function pred = ml_predict(trials, model)
% Make predictions for some data, using some (previously learned) model.
% Prediction = ml_predict(Trials, Model)
%
% This function makes predictions for some given data and a supplied model, as learned using
% ml_train. It dispatches to the appropriate ml_predict* function, depending on what was used to
% train the model.
%
% For each of the N supplied trials, one prediction is produced in the output, which may be in a
% number of different formats, depending on the parameters of the model (which in turn depends on
% the learning function used, its parameters, and the form of the target variables in the training
% data).
%
% The most common format of the prediction is discrete probability distributions per trial, which is
% the output produced by most classifiers. In this case, structed as a 3-element cell array {'disc',
% Probabilities, Classes}, the distributions are returned as a [#Trials x #Classes] matrix in the
% Probabilities entry, which contains the probability for each of a set of classes, per trial. The
% set of classes is associated with set of target values (e.g. -1,+1, or 3,7,5) by the Classes cell
% entry, which is size [#Classes x 1], and is sorted in the same order as in the probability
% distributions (in ascending order of target values), so that Prediction{2}*Prediction{3} would
% give a probability-weighted sum of the possible target values). This format can be mapped into the
% "usual" format of most likely target value per trial via the expression
% Prediction{3}(argmax(Prediction{2}')).
%
% For regression, the most common format is the [NxD] (usually D=1) format of one point estimate per
% trial, since only very few methods can give a posterior probability distribution right now. Other
% formats are primarily provided for more advanced predictors which can give regression outputs as a
% parametric (or even non-parametric) probability distribution per trial, as {distrib,NxP}, or in an
% entirely custom format, as {'struct',{N}}.
%
% In:
%   Trials   : data, [NxF] array (N = number of training instances,F = number of feature dimensions)
%              or in special cases a [UxVxN] array (N... number of training instances, U,V ...
%              number of rows/columns of the feature matrices)
%              If not otherwise possible, it is also allowed to pass trials in any custom format to
%              ml_predict, given that the prediction function in question can handle these data.
%
%   Model    : the model to be used for prediction (output of ml_train);
%              when empty, the actual features are returned (passed through).
%
% Out:
%   Prediction : predictions of the model, one for each trial
%                format depends on the chosen Model:
%                  * [Nx1] : classification or one-dimensional regression outputs (point estimates)
%                  * [NxD] : d-dimensional regression outputs (point estimates)
%                  * {'disc', [NxC], [Cx1]}: discrete probabilities for C classes in probabilistic
%                    classification, the last array determines the assigment from class indices (in
%                    NxC) to the output labels, such that [NxC]*[Cx1] is the expected output
%                  * {distrib, NxP}: posterior probability distributions in probabilistic
%                    regression, with distrib being one of:
%                    'bino','chi2','exp','ev','f','gam','gev','gp','geo','hyge','logn','nbin','ncf',
%                    'nct','ncx2','norm','poiss','rayl','t','unif','unid','wbl' with the
%                    appropriate interpretation under pdf(), and P the number of parameters to pdf()
%                  * {distrib, {N}}: posterior probability distributions, with distrib being either
%                    'mvn', for mvnpdf(), or any function handle that can accept parameter sets of
%                    the form distrib(X,Prediction{2}{i}{:});
%                  * {'struct', {N}}: structured predictions
%
% See also:
%   ml_train
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2010-04-03

if isempty(model)
    pred = trials;
else
    pred = feval(['ml_predict' model.args.arg_selection],trials,model.model);

    % post-process predictions
    if iscell(pred) && length(pred) == 3 && strcmp(pred{1},'disc')
        % sort discrete distributions
        [classes,inds] = sort(pred{3});
        pred = {pred{1} pred{2}(:,inds) classes};
    end
end


function bnet = mk_map_hhmm(varargin)

% p is the prob of a successful move (defines the reliability of motors)
p = 1;
obs_model = 'unique';

for i=1:2:length(varargin)
  switch varargin{i},
   case 'p', p = varargin{i+1};
   case 'obs_model', obs_model = varargin{i+1};
  end
end

q = 1-p;
unique_obs = strcmp(obs_model, 'unique');

% assign numbers to the nodes in topological order
U = 1; A = 2; C = 3; F = 4;
if unique_obs
  onodes = 5;
else
  N = 5; E = 6; S = 7; W = 8; % north, east, south, west
  onodes = [N E S W];
end

% create graph structure

ss = 4 + length(onodes); % slice size
intra = zeros(ss,ss);
intra(U,F)=1;
intra(A,[C F onodes])=1;
intra(C,[F onodes])=1;

inter = zeros(ss,ss);
inter(U,[A C])=1;
inter(A,[A C])=1;
inter(F,[A C])=1;
inter(C,C)=1;

% node sizes
ns = zeros(1,ss);
ns(U) = 2; % left/right
ns(A) = 2;
ns(C) = 3;
ns(F) = 2;
if unique_obs
  ns(onodes) = 5; % we will assign each state a unique symbol
else
  ns(onodes) = 2;
end
l = 1; r = 2; % left/right
L = 1; R = 2;

% Make the DBN
bnet = mk_dbn(intra, inter, ns, 'observed', onodes);
eclass = bnet.equiv_class;

% Define CPDs for slice 1
% We clamp all the CPDs that are not tied,
% since we cannot learn them from a single sequence.

% uniform probs over actions (the input could be chosen from a policy)
bnet.CPD{eclass(U,1)} = tabular_CPD(bnet, U, 'CPT', mk_stochastic(ones(ns(U),1)), ...
				    'adjustable', 0);

% uniform probs over starting abstract state
bnet.CPD{eclass(A,1)} = tabular_CPD(bnet, A, 'CPT', mk_stochastic(ones(ns(A),1)), ...
				    'adjustable', 0);

% Uniform probs over starting concrete state, modulo the fact
% that corridor 2 is only of length 2.
CPT = zeros(ns(A), ns(C)); % CPT(i,j) = P(C starts in j | A=i)
CPT(1, :) = [1/3 1/3 1/3];
CPT(2, :) = [1/2 1/2 0];
bnet.CPD{eclass(C,1)} = tabular_CPD(bnet, C, 'CPT', CPT, 'adjustable', 0);

% Termination probs
CPT = zeros(ns(U), ns(A), ns(C), ns(F));
CPT(r,1,1,:) = [1 0];
CPT(r,1,2,:) = [1 0];
CPT(r,1,3,:) = [q p];
CPT(r,2,1,:) = [1 0];
CPT(r,2,2,:) = [q p];
CPT(l,1,1,:) = [q p];
CPT(l,1,2,:) = [1 0];
CPT(l,1,3,:) = [1 0];
CPT(l,2,1,:) = [q p];
CPT(l,2,2,:) = [1 0];

bnet.CPD{eclass(F,1)} = tabular_CPD(bnet, F, 'CPT', CPT);

% Observation model
if unique_obs
  CPT = zeros(ns(A), ns(C), 5);
  CPT(1,1,1)=1;  % Theo state 4
  CPT(1,2,2)=1;  % Theo state 5
  CPT(1,3,3)=1; % Theo state 6
  CPT(2,1,4)=1; % Theo state 9
  CPT(2,2,5)=1; % Theo state 10
  %CPT(2,3,:) undefined
  O = onodes(1);
  bnet.CPD{eclass(O,1)} = tabular_CPD(bnet, O, 'CPT', CPT);
else
  % north/east/south/west can see wall (1) or opening (2)
  CPT = zeros(ns(A), ns(C), 2);
  CPT(:,:,1) = q;
  CPT(:,:,2) = p;
  bnet.CPD{eclass(W,1)} = tabular_CPD(bnet, W, 'CPT', CPT);
  bnet.CPD{eclass(E,1)} = tabular_CPD(bnet, E, 'CPT', CPT);
  CPT = zeros(ns(A), ns(C), 2);
  CPT(:,:,1) = p;
  CPT(:,:,2) = q;
  bnet.CPD{eclass(S,1)} = tabular_CPD(bnet, S, 'CPT', CPT);
  bnet.CPD{eclass(N,1)} = tabular_CPD(bnet, N, 'CPT', CPT);
end

% Define the CPDs for slice 2

% Abstract

% Since the top level never resets, the starting distribution is irrelevant:
% A2 will be determined by sampling from transmat(A1,:).
% But the code requires we specify it anyway; we make it all 0s, a dummy value.
startprob = zeros(ns(U), ns(A));

transmat = zeros(ns(U), ns(A), ns(A));
transmat(R,1,:) = [q p];
transmat(R,2,:) = [0 1];
transmat(L,1,:) = [1 0];
transmat(L,2,:) = [p q];

% Qps are the parents we condition the parameters on, in this case just
% the past action.
bnet.CPD{eclass(A,2)} = hhmm2Q_CPD(bnet, A+ss, 'Fbelow', F, ...
				  'startprob', startprob, 'transprob', transmat);

% Concrete

transmat = zeros(ns(C), ns(U), ns(A), ns(C));
transmat(1,r,1,:) = [q p 0.0];
transmat(2,r,1,:) = [0.0 q p];
transmat(3,r,1,:) = [0.0 0.0 1.0];
transmat(1,r,2,:) = [q p 0.0];
transmat(2,r,2,:) = [0.0 1.0 0.0];
%
transmat(1,l,1,:) = [1.0 0.0 0.0];
transmat(2,l,1,:) = [p q 0.0];
transmat(3,l,1,:) = [0.0 p q];
transmat(1,l,2,:) = [1.0 0.0 0.0];
transmat(2,l,2,:) = [p q 0.0];

% Add a new dimension for A(t-1), by copying old vals,
% so the matrix is the same size as startprob

transmat = reshape(transmat, [ns(C) ns(U) ns(A) 1 ns(C)]);
transmat = repmat(transmat, [1 1 1 ns(A) 1]);

% startprob(C(t-1), U(t-1), A(t-1), A(t), C(t))
startprob = zeros(ns(C), ns(U), ns(A), ns(A), ns(C));
startprob(1,L,1,1,:) = [1.0 0.0 0.0];
startprob(3,R,1,2,:) = [1.0 0.0 0.0];
startprob(3,R,1,1,:) = [0.0 0.0 1.0];
%
startprob(1,L,2,1,:) = [0.0 0.0 010];
startprob(2,L,2,1,:) = [1.0 0.0 0.0];
startprob(2,R,2,2,:) = [0.0 1.0 0.0];

% want transmat(U,A,C,At,Ct), ie. in topo order
transmat = permute(transmat, [2 3 1 4 5]);
startprob  = permute(startprob, [2 3 1 4 5]);
bnet.CPD{eclass(C,2)} = hhmm2Q_CPD(bnet, C+ss, 'Fself', F, ...
				  'startprob', startprob, 'transprob', transmat);


function [ know, x ] = p19_sol ( n )

%*****************************************************************************80
%
%% P19_SOL returns the solution for problem 19.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 March 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the problem.  This value
%    is only needed for those problems with variable N.
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X(N), the solution, if known.
%
  know = 1;

  x = [ 1.0, 1.0 ]';

  return
end


function [sttm] = tt_sparse_for_mv(ttm, cuttol)
%Sparsify the TT matrix in TT1.0 matrix format
%   [STTM]=TT_SPARSE_FOR_MV(TTM,[CUTTOL]) Computes the sparse version of the
%   matrix in the TT1.0 matrix format. May work for certain operators
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

if (nargin<2)||(isempty(cuttol))
    cuttol = 1e-14;
end;

d=size(ttm,1);

sttm=cell(d+2,1);
sttm{d+1}=zeros(d-1,1);
sttm{d+2}=zeros(d,1);

n=size(ttm{1},1);
m=size(ttm{1},2);
rm1=size(ttm{1},3);
sttm{d+1}(1)=rm1;
sttm{d+2}(1)=n;
if (~issparse(ttm{1}))
    sttm{1}=reshape(permute(ttm{1},[1,3,2]),[n*rm1,m]);
    sttm{1}(abs(sttm{1})/max(max(abs(sttm{1})))<cuttol)=0;
    sttm{1}=sparse(sttm{1});
else
    sttm{1} = ttm{1}; % let it be rank-1 first
end;

if (d>1)
    n=size(ttm{d},1);
    m=size(ttm{d},2);
    rm1=size(ttm{d},3);
    sttm{d+1}(d-1)=rm1;
    sttm{d+2}(d)=n;
    if (~issparse(ttm{1}))
        sttm{d}=reshape(permute(ttm{d},[1,3,2]),[n*rm1,m]);
        sttm{d}(abs(sttm{d})/max(max(abs(sttm{d})))<cuttol)=0;
        sttm{d}=sparse(sttm{d});
    else
        sttm{d} = ttm{d}; % let it be rank-1 first
    end;
end;

for i=2:d-1
    n=size(ttm{i},1);
    m=size(ttm{i},2);
    rm1=size(ttm{i},3);
    rm2=size(ttm{i},4);
    sttm{d+1}(i)=rm2;
    sttm{d+2}(i)=n;
    if (~issparse(ttm{1}))
        sttm{i}=reshape(permute(ttm{i},[1,4,3,2]),[n*rm2*rm1,m]);
        sttm{i}(abs(sttm{i})/max(max(abs(sttm{i})))<cuttol)=0;
        sttm{i}=sparse(sttm{i});
    else
        sttm{i} = ttm{i}; % let it be rank-1 first
    end;
end

end


function percentile = prctile(this, percentile, varargin)
% Computes given percentile in data (e.g. 50 for median)
%
%   Y = MrImage()
%   percentile = ...
%       Y.prctile(percentile, 'ParameterName1', 'ParameterValue1', ...)
%
% This is a method of class MrImage.
%
% IN
%   percentile  percent for percentile computation; default: 50 (median)
%   varargin    parameterName/Value pairs for selection of volumes/slices
%
% OUT
%
% EXAMPLE
%   Y.prctile(50, 'z', 1, 't', 3:100, ...,
%           'x', 55:75)
%
%   See also MrImage MrImage.hist

% Author:   Lars Kasper
% Created:  2015-08-23
% Copyright (C) 2015 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

if nargin < 2
    percentile = 50;
end

if nargin < 3
    imgSelect = this;
else
    imgSelect = this.select(varargin{:});
end

percentile = prctile(imgSelect.data(:), percentile);


function [Gx_org,Gy_org,Gz_org]=compute_gradients(img_pca,ins_msk,Subject,Figures,Streamlines)

% Compute eigenmap's gradient magnitude

% INPUT
% img_pca is the eigenmap image in which gradients are computed
% ins_msk is a mask. Only gradients for voxels in the mask are shown in
% figure
% Subject: prefix of output
% Figures: 0->suppress figures; 1->print figures
% slices
% Streamlines:1-> write out vector file; 0->do not write out vector file

% OUTPUT
% Local gradient orientations for each voxel

Erode=1; %erosion kernel
N=size(img_pca);
ind_ins=find(~~ins_msk);

% Compute gradient
% updated script to clean up edges
img_new=zeros(N);
img_new(ind_ins)=img_pca(ind_ins);
img_pca=img_new; %Remove gradients computed in boundary voxels added by dilation

[Gy,Gx,Gz,mag]=compute_grads_local(img_pca);
% %Swap order of Gx and Gy because the first output of the function is the
% %gradient along direction of increasing column subscripts.
% %With this re-ordering
% %Gx is gradient along rows    (1st dimension)
% %Gy is gradient along columns (2nd dimension)
% %Gz is gradeint in 3rd dimensions

img_mag=mag;

Gx_org=Gx; Gy_org=Gy; Gz_org=Gz;

%Crop for a better visulization
Gx=Gx.*~~ins_msk;
Gy=Gy.*~~ins_msk;
Gz=Gz.*~~ins_msk;

%Bounding box
tmp=squeeze(sum(ins_msk,1));
[y,z]=find(tmp);
max_y=max(y); min_y=min(y);
max_z=max(z); min_z=min(z);
tmp=squeeze(sum(ins_msk,2));
[x,z]=find(tmp);
max_x=max(x); min_x=min(x);
Nx=max_x-min_x+1;
Ny=max_y-min_y+1;
Nz=max_z-min_z+1;

%Write out img_pca and img_mag after cropping
if ~isempty(Subject)
    fprintf('Write out %s\n',[Subject,'magnitude.nii'])
    mat2nii(img_mag(min_x:max_x,min_y:max_y,min_z:max_z),[Subject,'magnitude.nii']);

    fprintf('Write out %s\n',[Subject,'eigenvector.nii'])
    mat2nii(img_pca(min_x:max_x,min_y:max_y,min_z:max_z),[Subject,'eigenvector.nii']);
end

if Streamlines
    u=repmat([1:Nx]',1,Ny); u=repmat(u,1,1,Nz);
    v=repmat([1:Ny],Nx,1); v=repmat(v,1,1,Nz);
    w=zeros(Nx,Ny,Nz);
    for i=1:Nz
        w(:,:,i)=ones(Nx,Ny)*i;
    end
    slice_msk=imerode(~~squeeze(ins_msk(min_x:max_x,min_y:max_y,min_z:max_z)),ones(Erode,Erode));
    gx=squeeze(Gx(min_x:max_x,min_y:max_y,min_z:max_z)).*slice_msk;
    gy=squeeze(Gy(min_x:max_x,min_y:max_y,min_z:max_z)).*slice_msk;
    gz=squeeze(Gz(min_x:max_x,min_y:max_y,min_z:max_z)).*slice_msk;

    save ([Subject,'VectorFile.mat'],'gy','gx','gz','v','u','w','Gx_org','Gy_org','Gz_org');
end

if Figures
    Mag=0; % Mag=0:print out eigenmap slices;
           % Mag=1:print out gradient magnitude slices

    % Colormap consistent with Trackvis
    mycolormap=dlmread('trackvis_jet.txt');
    mycolormap(1,:)=1; % White background

    % Rescale according to the value in the magnitude map
    img_pca(ind_ins)=img_pca(ind_ins)/0.03*63+2;
    img_mag(ind_ins)=img_mag(ind_ins)/0.03*63+2;

%     %Use default colormap
%     mycolormap=parula(256); mycolormap(1,:)=1; % White background
%     img_pca(ind_ins)=img_pca(ind_ins)/max(img_pca(ind_ins))*255+2;
%     img_mag(ind_ins)=img_mag(ind_ins)/max(img_mag(ind_ins))*255+2;
%     %Rescale to 256 colors:
%     %Bin 1->2 is white
%     %Bin 2->3 is first color
%     %Bin 256->257 is last color

    %Sagittal
    for i=1:N(1)
        sz(i)=sum(sum(~~ins_msk(i,:,:)));
    end
    [~,ind_srt]=sort(sz,'descend'); %find slices with greatest coverage

    Slices=ind_srt(1:4);
    hf=figure; hf.Position=[100,300,2000,300]; hf.Color='w';
    for i=1:length(Slices)
        subplot(1,length(Slices),i)
        if Mag
            %1st dimension of image is placed on rows
            %2nd dimension of image is placed on columns
            im=image(squeeze(img_mag(Slices(i),min_y:max_y,min_z:max_z)));
        else
            im=image(squeeze(img_pca(Slices(i),min_y:max_y,min_z:max_z)));
        end
        slice_msk=imerode(~~squeeze(ins_msk(Slices(i),min_y:max_y,min_z:max_z)),ones(Erode,Erode));
        colormap(mycolormap);
        hold on;
        gx=squeeze(Gx(Slices(i),min_y:max_y,min_z:max_z)).*slice_msk;
        gy=squeeze(Gy(Slices(i),min_y:max_y,min_z:max_z)).*slice_msk;
        gz=squeeze(Gz(Slices(i),min_y:max_y,min_z:max_z)).*slice_msk;

        % Even the magnitude for figure
        mag=sqrt(gy.^2+gx.^2+gz.^2);
        gy=gy./mag; gx=gx./mag; gz=gz./mag;

        u=repmat([1:Ny]',1,Nz); v=repmat([1:Nz],Ny,1); w=ones(Ny,Nz);
        q=quiver3(v,u,w,gz,gy,gx);

        %First input is gradient in direction of increasing columns
        %Second input is gradient in direction of increasing rows
        q.LineWidth=0.5;
        q.Color=[192,192,192]/255;
        q.AutoScaleFactor=2;
        view(-90, 90);
        axis off;
        ax=gca; ax.XLabel.Visible='on'; ax.YLabel.Visible='on';
        axis equal;
    end

    % Coronal
    for i=1:N(2)
        sz(i)=sum(sum(~~ins_msk(:,i,:)));
    end
    [~,ind_srt]=sort(sz,'descend'); %find slices with greatest coverage
    Slices=ind_srt(1:4);
    hf=figure; hf.Position=[100,800,2000,300]; hf.Color='w';
    for i=1:length(Slices)
        subplot(1,length(Slices),i)
        if Mag
            %1st dimension of image is placed on rows
            %2nd dimension of image is placed on columns
            tmp=squeeze(img_mag(min_x:max_x,Slices(i),min_z:max_z));
            im=image(tmp);
        else
            tmp=squeeze(img_pca(min_x:max_x,Slices(i),min_z:max_z));
            im=image(tmp);
        end
        slice_msk=imerode(~~squeeze(ins_msk(min_x:max_x,Slices(i),min_z:max_z)),ones(Erode,Erode));
        colormap(mycolormap);
        hold on;
        gx=squeeze(Gx(min_x:max_x,Slices(i),min_z:max_z)).*slice_msk;
        gy=squeeze(Gy(min_x:max_x,Slices(i),min_z:max_z)).*slice_msk;
        gz=squeeze(Gz(min_x:max_x,Slices(i),min_z:max_z)).*slice_msk;

        % Even the magnitude
        mag=sqrt(gy.^2+gx.^2+gz.^2);
        gy=gy./mag; gx=gx./mag; gz=gz./mag;

        u=repmat([1:Nx]',1,Nz); v=repmat([1:Nz],Nx,1); w=ones(Nx,Nz);
        q=quiver3(v,u,w,gz,gx,gy);

        %First input is gradient in direction of increasing columns
        %Second input is gradient in direction of increasing rows
        q.LineWidth=0.5;
        q.Color=[192,192,192]/255;
        q.AutoScaleFactor=2;
        view(-90, 90);
        axis off;
        ax=gca; ax.XLabel.Visible='on'; ax.YLabel.Visible='on';
        axis equal;
    end

    %Axial
    for i=1:N(3)
        sz(i)=sum(sum(~~ins_msk(:,:,i)));
    end
    [~,ind_srt]=sort(sz,'descend'); %find slices with greatest coverage
    Slices=ind_srt(1:4);
    hf=figure; hf.Position=[100,1200,2000,300]; hf.Color='w';
    for i=1:length(Slices)
        subplot(1,length(Slices),i)
        if Mag
            %1st dimension of image is placed on rows
            %2nd dimension of image is placed on columns
            tmp=squeeze(img_mag(min_x:max_x,min_y:max_y,Slices(i)));
            im=image(tmp);
        else
            tmp=squeeze(img_pca(min_x:max_x,min_y:max_y,Slices(i)));
            im=image(tmp);
        end
        slice_msk=imerode(~~squeeze(ins_msk(min_x:max_x,min_y:max_y,Slices(i))),ones(Erode,Erode));
        colormap(mycolormap);
        hold on;
        gx=squeeze(Gx(min_x:max_x,min_y:max_y,Slices(i))).*slice_msk;
        gy=squeeze(Gy(min_x:max_x,min_y:max_y,Slices(i))).*slice_msk;
        gz=squeeze(Gz(min_x:max_x,min_y:max_y,Slices(i))).*slice_msk;

        % Even the magnitude
        mag=sqrt(gy.^2+gx.^2+gz.^2);
        gy=gy./mag; gx=gx./mag; gz=gz./mag;

        u=repmat([1:Nx]',1,Ny); v=repmat([1:Ny],Nx,1); w=ones(Nx,Ny);

        q=quiver3(v,u,w,gy,gx,gz);

        %First input is gradient in direction of increasing columns
        %Second input is gradient in direction of increasing rows
        q.LineWidth=0.5;
        q.Color=[192,192,192]/255;
        q.AutoScaleFactor=2;

        view(-90, 90);
        axis off;
        ax=gca; ax.XLabel.Visible='on'; ax.YLabel.Visible='on';
        axis equal;
    end
end


function [feature_pos_out, transformed_points_out, scale_out, error] = em2_affine_imu(...
    events, ...
    feature_pos, ...
    template_points, ...
    template_weights, ...
    scale_in, ...
    flow, ...
    R_in, ...
    cinfo, ...
    params, ...
    fig)
%EM2_AFFINE_IMU Estimates a scale and translationbetween a set of events
%and a set of template points, given camera rotation.
%
% EM2_AFFINE_IMU estimates an scale and translation (s, b) between a set of
% events with known optical flow, and a set of 2D template points, given the
% camera rotation between the events and the template. This is an
% implementation of the method outlined in:
% Alex Zihao Zhu, Nikolay Atanasov, and Kostas Daniilidis.
% "Event-Based Visual Inertial Odometry." CVPR 2017.
%
% Syntax:  [feature_pos_out, transformed_points_out, scale_out, error] = EM2_AFFINE_IMU(...
%            events, ...
%            feature_pos, ...
%            template_points, ...
%            template_weights, ...
%            scale_in, ...
%            flow, ...
%            R_in, ...
%            cinfo, ...
%            params, ...
%            fig)
%
% Inputs:
%    events           - 4xN, each column is (x,y,t,p).
%    feature_pos      - 2x1, pixel position of the feature.
%    template_points  - 2xM, set of points to align the events with.
%    template_weights - 1xM, weights for each template point.
%    scale_in         - 1x1, initialization for the scale factor.
%    flow_init        - 2x1, flow for the event window.
%    R_in             - 3x3, 3D camera rotation between the template and events.
%    cinfo            - camera info struct. Structure is defined by the ROS message:
%                       http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
%    params           - parameters, defined in get_params().
%    fig              - figure handle for plotting.
%
% Outputs:
%    feature_pos_out        - 2x1, feature pos after alignment, feature_pos - b.
%    transformed_points_out - 2xN, input events in the feature window
%                               shifted by the flow A[x; y] + dt * flow + b.
%    scale_out              - 1x1, estimate for the scale factor.
%    error                  - Error code, defined in TrackingErrors.
%
% See also GET_PARAMS
%
% Author: Alex Zihao Zhu, University of Pennsylvania
% Email: alexzhu(at)seas.upenn.edu
% Copyright 2018 University of Pennsylvania
% Alex Zihao Zhu, Nikolay Atanasov, Kostas Daniilidis
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS, CONTRIBUTORS, AND THE
% TRUSTEES OF THE UNIVERSITY OF PENNSYLVANIA "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, CONTRIBUTORS OR THE TRUSTEES OF
% THE UNIVERSITY OF PENNSYLVANIA 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.

%% Initialization
% Estimated canonical events
transformed_points_out = [];
% New estimated patch position if estimated
feature_pos_out = [nan; nan];
errs = zeros(params.em2_params.max_iters, 1);
num_iter = 0;
scatter_plot_handle = [];
scale = scale_in;
scale_out = scale;
pos_shift = [0;0];

event_window = [];

error = TrackingErrors.ICPFailed;

% Transform the input events based on the 3D camera rotation between the
% input events and template points.
px = cinfo.K(3);
py = cinfo.K(6);
fx = cinfo.K(1);
fy = cinfo.K(5);

pos_u = [(feature_pos-[px;py])./[fx;fy]; 1];
pos_ur = R_in*pos_u;
pos_ur = pos_ur(1:2)/pos_ur(3);
pos_ur = pos_ur.*[fx;fy]+[px;py];

events_xy= events(1:2, :)+bsxfun(@times, flow, events(3, 1) - events(3, :));
events_xy = bsxfun(@rdivide, bsxfun(@minus, events_xy(1:2, :), [px;py]), [fx;fy]);
events_xy = [events_xy; ones(1, size(events_xy, 2))];
events_xy = R_in*events_xy;
events_xy = bsxfun(@rdivide, events_xy(1:2, :), events_xy(3, :));
events_xy = bsxfun(@plus, bsxfun(@times, events_xy, [fx;fy]), [px;py]);
rotated_points = bsxfun(@minus, events_xy, pos_ur);

if isempty(template_points)
    return
end

kdtree = KDTreeSearcher(...
    template_points' / (sqrt(2) * params.em2_params.sigma), ...
    'Distance', 'euclidean');

while true
    if num_iter > params.em2_params.max_iters
        error = TrackingErrors.ICPMaxIters;
        return
    end

    transformed_points= bsxfun(@plus, scale*rotated_points, pos_shift);

    if isempty(event_window)
        event_window = ...
            transformed_points(1, :) >= -params.window_size/2 & ...
            transformed_points(2, :) >= -params.window_size/2 & ...
            transformed_points(1, :) <= params.window_size/2 & ...
            transformed_points(2, :) <= params.window_size/2;

        n_in_window = sum(event_window);
        if n_in_window < params.min_events_for_em
            error = TrackingErrors.ICPFailed;
            return
        end

        rotated_points = rotated_points(:, event_window);
        transformed_points = transformed_points(:, event_window);

        weights = zeros(size(template_points, 2), size(transformed_points, 2));
    end

    [neighbors_cell, distances_cell] = rangesearch(...
        kdtree, ...
        transformed_points' / (sqrt(2) * params.em2_params.sigma), ...
        params.em2_params.max_distance);

    distancesstacked = cell2mat(cellfun(...
        @transpose, distances_cell, 'UniformOutput', false))';

    if isempty(distancesstacked)
        error = TrackingErrors.ICPFailed;
        return
    end

    % NOTE: 'length' is not the same as @length
    num_neighbors = cellfun('length', neighbors_cell);
    template_correspondences = cell2mat(cellfun(...
        @transpose, neighbors_cell, 'UniformOutput', false))';

    transformed_correspondences = repelem(1:size(transformed_points, 2), num_neighbors);

    weightsstacked = exp(-distancesstacked);

    valid_inds = sub2indc(...
        transformed_correspondences, ...
        template_correspondences, ...
        size(weights));

    % It's cheaper to multiply to 0 to reset the weights matrix than to
    % reinitialize it using zeros.
    weights = weights * 0;
    weights(valid_inds) = weightsstacked;

    if ~isempty(template_weights)
        weights = bsxfun(@times, template_weights, weights);
    end

    weight_sum = sum(weights, 1);
    valid_weights = (weight_sum > 0);
    weights = bsxfun(@rdivide, weights, weight_sum + 1e-10);

    weightsstacked = weights(valid_inds);

    %% Simultaneously minimize over scale and translation.
    weighted_template_barycenters = template_points*weights;
    weighted_template_barycenters = weighted_template_barycenters(:, valid_weights);

    valid_rotated_points = rotated_points(:, valid_weights);

    rotated_mean = mean(valid_rotated_points, 2);
    template_mean = mean(weighted_template_barycenters, 2);

    v_rotated_points_centered = bsxfun(@minus, valid_rotated_points, rotated_mean);
    w_template_b_centered = bsxfun(@minus, weighted_template_barycenters, template_mean);

    covariance = v_rotated_points_centered * w_template_b_centered';
    [U, S, V] = svd(covariance);
    if det(U*V') < 0
        S(2, 2) = -S(2, 2);
    end

    variance = sum(sum(v_rotated_points_centered.^2));
    new_scale = trace(S) / variance;

    pos_shift = template_mean - new_scale*rotated_mean;
    scale = new_scale;
    %
    %     scale = sqrt(sum(sum(...
    %         v_rotated_points_centered.*w_template_b_centered, 2)) / ...
    %         sum(sum(v_rotated_points_centered.^2)));

    if scale < 0.1
        error = TrackingErrors.ICPFailed;
        return
    end

    %% Calculate errors, plot debug information.
    err = weightsstacked*distancesstacked'/size(valid_rotated_points, 2);
    errs(num_iter+1) = err;

    if params.debug
        set(0, 'CurrentFigure', fig)
        subplot(2,1,1)
        plot(errs(errs > 0), 'b')
        title('Change in error (convergence criterion)')
        xlim([0 params.em2_params.max_iters])
        subplot(2,1,2)
        if (~isempty(scatter_plot_handle))
            delete(scatter_plot_handle)
        end
        scatter_plot_handle = scatter(transformed_points(1, :), transformed_points(2, :),'r.');
        hold on
        scatter(template_points(1, :), template_points(2, :),template_weights*10,'b.');
        hold off
        axis equal
        axis([-params.window_size/2-5, ...
            params.window_size/2+5, ...
            -params.window_size/2-5, ...
            params.window_size/2+5])
        axis ij

        title('EM2 with IMU Rotation')
        pause(0.01)
    end

    if num_iter
        derr = err-errs(num_iter);
        if derr > -params.em2_params.min_err && derr <= 0
            break;
        end
    end

    num_iter = num_iter + 1;
end

scale_out = scale;
% Update the feature position based on the shift.
feature_pos_out = feature_pos-pos_shift;

% Compute the template events to be used in later iterations.
dt = events(3, end)-events(3, 1);
transformed_points = scale * ...
    (bsxfun(@minus, events(1:2, :), feature_pos_out + flow * dt) + ...
    bsxfun(@times, flow, (events(3, end) - events(3, :))));

% Make the window size a bit bigger for the template.
window_size = round(params.window_size * 1.5);

event_window = ...
    transformed_points(1, :) >= -window_size/2 & ...
    transformed_points(2, :) >= -window_size/2 & ...
    transformed_points(1, :) <= window_size/2 & ...
    transformed_points(2, :) <= window_size/2;

transformed_points_out = transformed_points(:, event_window);

if params.debug
    pause(0.5)
end

% Measure how many transformed points are considered 'outliers' when
% matched against the templates. Outliers are defined as points without any
% template points within 2 pixels. Note that this function still requires
% more work to be properly robust.
[~, distances_cell] = knnsearch(...
    kdtree, ...
    transformed_points_out'/(sqrt(2)*params.em2_params.sigma), ...
    'K', 1);
distances_cell = distances_cell * 2 * params.em2_params.sigma^2;
percent_outliers = sum(distances_cell > 2.0) / length(distances_cell);

if percent_outliers > params.em2_params.max_outlier_thresh
    error = TrackingErrors.ICPError;
    feature_pos_out = [nan; nan];
    return;
end

end


function [L, V] = model_sort(m, i, L, V)
% Topological sort of the nonterminal symbols in m's grammar.
%   [L, V] = model_sort(m, i, L, V)
%
% Return values
%   L   Symbols visited in post order
%   V   (internal use) Symbol visitation status
%
% Arguments
%   m   Object model
%   i   (internal use) Current symbol
%   L   (internal use) Symbols visited thus far in post order
%   V   (internal use) Symbol visitation status thus far

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2009-2012 Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

% initialize depth-first search at start symbol
if nargin < 2
  i = m.start;
  L = [];
  V = zeros(m.numsymbols, 1);
end

% check for cycle containing symbol i
if V(i) == 1
  error('Cycle detected in grammar!');
end

% mark symbol i as pre-visit
V(i) = 1;
for r = 1:length(m.rules{i})
  for s = m.rules{i}(r).rhs
    % recurse if s is a nonterminal and not already visited
    if m.symbols(s).type == 'N' && V(s) < 2
      [L, V] = model_sort(m, s, L, V);
    end
  end
end
% mark symbol i as post-visit
V(i) = 2;
L = [L i];


function C = cache_test(A,B)
  % CACHE_TEST Dummy program that computes C = A+B, but demonstrates how to use
  % md5 caching of function results on input parameters
  %
  % C = cache_test(A,B)
  %

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Check for cached result, do NOT edit variables until cache is checked,
  % your function code comes later. See below
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % get a list of current variables in this scope, this is the input "state"
  variables = who;
  % get a temporary file's name
  tmpf = [tempname('.') '.mat'];
  % save the "state" to file, so we can get a md5 checksum
  save(tmpf,'-regexp',sprintf('^%s$|',variables{:}),'-ascii');
  % get md5 checksum on input "state", we append .cache.mat to the check sum
  % because we'll use the checksum as the cache file name
  [s,cache_name] = system(['md5sum ' tmpf ' | awk ''{printf "."$1".cache.mat"}''']);
  % clean up
  delete(tmpf);
  clear s tmpf variables;

  % If the checksum cache file exists then we've seen this input "state"
  % before, load in cached output "state"
  if(exist(cache_name,'file'))
    fprintf('Cache exists. Using cache...\n');
    % use cache
    load(cache_name);
  % Otherwise this is the first time we've seen this input "state", so we
  % execute the function as usual and save the output "state" to the cache
  else
    fprintf('First time. Creating cache...\n');

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Your function code goes here
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    C = A+B;

    % get list of variables present in this scope at finish of function code,
    % this is the output "state"
    variables = who;
    % save output "state" to file, using md5 checksum cache file name
    save(cache_name,'-regexp',sprintf('^%s$|',variables{:}));
  end
end