function feature3M = calcFeatureImpact(scanNum, structNum, quantizeS, ...
    patchSizeV, featureFun, featureName, planC, varargin)
% function feature3M = calcFeatureImpact(scanNum, structNum, quantizeS,...
% patchSizeV, featureFun, featureName, planC, varargin)
%
% This function computes the feature Impact at each voxel.
%
% Method:
% 1> Omit the voxel and its neighborhood.
% 2> Compute the feature with the passed featureFun and the varargin.
% Repeat 1 and 2 for each voxel in the region of interest
% 3> Calc. percentage difference between features calculated by omitting
% vs. using all voxels
%
% Example:
%
% In order to compute the impact map for 1st order kurtosis,
% scanNum = 1;
% structNum = 6;
% patchRadiusV = [2 2 0];
% funcHandle = @radiomics_first_order_stats;
% featureName = 'kurtosis';
% quantizeS.quantize = 1;
% quantizeS.nL = 32;
% quantizeS.xmin = [];
% quantizeS.xmax = [];
% quantizeS.binwidth = [];
% feature3M = calcFeatureImpact(scanNum, structNum, quantizeS, patchRadiusV,...
%   funcHandle, featureName, planC, structNum);
%
% Also, refer to call_calcFeatureImpact.m for an example to calculate
% feature impact for RLM features
%
% APA, 5/18/2017
% AI,  8/21/2018 Updates to inputs, featureFun signature

indexS = planC{end};

% Get uniformized structure Mask
struct3M = getUniformStr(structNum,planC);

% Get uniformized scan mask in HU
scan3M = getUniformizedCTScan(1, scanNum, planC);
% Convert to HU if image is of type CT
if ~isempty(planC{indexS.scan}(scanNum).scanInfo(1).CTOffset)
    scan3M = double(scan3M) - planC{indexS.scan}(scanNum).scanInfo(1).CTOffset;
end

fullScanSiz = size(scan3M);

% Crop scan within the structures bounding box
[minr, maxr, minc, maxc, mins, maxs] = compute_boundingbox(struct3M);
struct3M = struct3M(minr:maxr,minc:maxc,mins:maxs);
scan3M = scan3M(minr:maxr,minc:maxc,mins:maxs);
scan3M(struct3M==0)     = NaN;

% Initialize the feature3M matrix
feature3M = zeros(fullScanSiz);

featureCropped3M = zeros(size(scan3M));

% Get indices of non-NaN voxels
calcIndM = struct3M == 1;

% % Grid resolution
slcWindow = 2 * patchSizeV(3) + 1;
rowWindow = 2 * patchSizeV(1) + 1;
colWindow = 2 * patchSizeV(2) + 1;

% Build distance matrices
numColsPad = floor(colWindow/2);
numRowsPad = floor(rowWindow/2);
numSlcsPad = floor(slcWindow/2);

% Get number of voxels per slice
[numRows, numCols, numSlices] = size(scan3M);
numVoxels = numRows*numCols;

% Pad q, so that sliding window works also for the edge voxels
if exist('padarray.m','file')
    %scanArrayTmp3M = padarray(scanArray3M,[numRowsPad numColsPad
    %numSlcsPad],NaN,'both'); % aa commented
    q = padarray(scan3M,[numRowsPad numColsPad numSlcsPad],NaN,'both');
    structPadded3M = padarray(uint32(struct3M),[numRowsPad numColsPad numSlcsPad],NaN,'both');
    calcIndM = padarray(calcIndM,[0 0 numSlcsPad],0,'both');
else
    %scanArrayTmp3M = padarray(scanArray3M,[numRowsPad numColsPad
    %numSlcsPad],NaN,'both'); % aa commented
    q = padarray_oct(scan3M,[numRowsPad numColsPad numSlcsPad],NaN,'both');
    structPadded3M = padarray_oct(uint32(struct3M),[numRowsPad numColsPad numSlcsPad],NaN,'both');
    calcIndM = padarray_oct(calcIndM,[0 0 numSlcsPad],0,'both');
end
structPadded3M = logical(structPadded3M);

% Create indices for 2D blocks
[m,n,~] = size(q);
m = uint32(m);
n = uint32(n);
colWindow = uint32(colWindow);
rowWindow = uint32(rowWindow);
slcWindow = uint32(slcWindow);

% Index calculation adapted from
% http://stackoverflow.com/questions/25449279/efficient-implementation-of-im2col-and-col2im

%// Start indices for each block
start_ind = reshape(bsxfun(@plus,[1:m-rowWindow+1]',[0:n-colWindow]*m),[],1); %//'

%// Row indices
lin_row = permute(bsxfun(@plus,start_ind,[0:rowWindow-1])',[1 3 2]);  %//'

%// Get linear indices based on row and col indices and get desired output
indM = reshape(bsxfun(@plus,lin_row,(0:colWindow-1)*m),rowWindow*colWindow,[]);

% Feature computed using all voxels
quantized3M = q;
if ~isempty(quantizeS) && quantizeS.quantize
    quantized3M = imquantize_cerr(quantized3M,quantizeS.nL,quantizeS.xmin,quantizeS.xmax,quantizeS.binwidth);
end
featureEntireStruct = feval(featureFun,quantized3M,varargin{:});

if ~isempty(featureName) && isstruct(featureEntireStruct)
    [f1,f2] = strtok(featureName,'.');
    fieldC = {f1};
    while ~isempty(f2)
        [f1,f2] = strtok(f2,'.');
        fieldC{end+1} = f1;
    end
    for i = 1:length(fieldC)
        featureEntireStruct = featureEntireStruct.(fieldC{i});
    end
end

% Iterate over slices. Compute cooccurance for all patches per slice
for slcNum = (1+numSlcsPad):(numSlices+numSlcsPad)
    disp(['--- Feature Impact Calculation for Slice # ', num2str(slcNum), ' ----'])

    calcSlcIndV = calcIndM(:,:,slcNum);
    calcSlcIndV = calcSlcIndV(:);
    numCalcVoxs = sum(calcSlcIndV);
    indSlcM = indM(:,calcSlcIndV);
    slcV = slcNum-patchSizeV(3):slcNum+patchSizeV(3);
    featureV = zeros(numCalcVoxs,1);

    for vox = 1:size(indSlcM,2)
        scanTmp3M = quantized3M;
        for slcNumTmp = 1:length(slcV)
            slc = slcV(slcNumTmp);
            scanSlcM = scanTmp3M(:,:,slc);
            scanSlcM(indSlcM(:,vox)) = NaN;
            scanTmp3M(:,:,slc) = scanSlcM;
        end
        featureVox = feval(featureFun,scanTmp3M,varargin{:});
        for i = 1:length(fieldC)
            featureVox = featureVox.(fieldC{i});
        end
        featureV(vox) = featureVox;
    end

    indScanTmp3M = zeros(size(calcIndM),'logical');
    indScanTmp3M(:,:,slcNum) = calcIndM(:,:,slcNum);
    featuresC{slcNum} = featureV;
    calcIndC{slcNum} = indScanTmp3M;

end

for slcNum = (1+numSlcsPad):(numSlices+numSlcsPad)
    featureCropped3M(calcIndC{slcNum}) = featuresC{slcNum};
end

featureCropped3M(calcIndM) = ...
    (featureCropped3M(calcIndM) - featureEntireStruct) / featureEntireStruct * 100;
feature3M(minr:maxr,minc:maxc,mins:maxs) = featureCropped3M;

% Write feature impact as a dose in CERR

% Create Texture Scans
[xVals, yVals, zVals] = getUniformScanXYZVals(planC{indexS.scan}(scanNum));
deltaXYZv(1) = abs(xVals(2)-xVals(1));
deltaXYZv(2) = abs(yVals(2)-yVals(1));
deltaXYZv(3) = abs(zVals(2)-zVals(1));
uniqueSlicesV = mins:maxs;
zV = zVals(uniqueSlicesV);
regParamsS.horizontalGridInterval = deltaXYZv(1);
regParamsS.verticalGridInterval   = -deltaXYZv(2); %(-)ve for dose
regParamsS.coord1OFFirstPoint   = xVals(minc);
regParamsS.coord2OFFirstPoint   = yVals(minr); % for dose
regParamsS.zValues  = zV;
regParamsS.sliceThickness = [planC{indexS.scan}(scanNum).scanInfo(uniqueSlicesV).sliceThickness];
assocScanUID = planC{indexS.structures}(structNum).assocScanUID;
dose2CERR(featureCropped3M,[], featureName, [featureName,'_impact'],...
    [featureName,'_impact'],'non CT',regParamsS,'no',assocScanUID)


function optimParam = tuneParam(LPProblem,contFunctName,timelimit,nrepeat,printLevel)
% Optimizes cplex parameters to make model resolution faster.
% Particularly interetsing for large-scale MILP models and repeated runs of
% optimisation.
% While, it won't optimize memory space nor model constraints for numerical
% infeasibilities, tuneParam will provide the optimal set of solver
% parameters for feasbile models. It requires IBM ILOG cplex (for now).
%
% USAGE:
%
%    optimalParameters = tuneParam(LPProblem,contFunctName,timelimit,nrepeat,printLevel);
%
% INPUT:
%         LPProblem:     MILP as COBRA LP problem structure
%         contFunctName: Parameters structure containing the name and value.
%                        A set of routine parameters will be added by the solver
%                        but won't be reported.
%         timelimit:     default is 10000 second
%         nrepeat:       number of row/column permutation of the original
%                        problem, reports robust results.
%                        sets the CPX_PARAM_TUNINGREPEAT parameter
%                        High values of nrepeat would require consequent
%                        memory and swap.
%         printLevel:    0/(1)/2/3
%
% OUTPUT:
%         optimParam: structure of optimal parameter values directly usable as
%                     contFunctName argument in solveCobraLP function
%
% .. Author: Marouen Ben Guebila 24/07/2017

if ~changeCobraSolver('ibm_cplex')
    error('This function requires IBM ILOG CPLEX');
end

if ~exist('printLevel','var')
    printLevel = 1;
end

if exist('timelimit','var')
    contFunctName.tune.timelimit = timelimit;
end
if exist('nrepeat','var')
    contFunctName.tune.repeat = nrepeat;
end
if exist('printLevel','var')
    contFunctName.tune.display = printLevel;
end

%read parameters
if isstruct(contFunctName)
    cpxControl=contFunctName;
else
    if ~isempty(contFunctName)
        %calls a user specified function to create a CPLEX control structure
        %specific to the users problem. A TEMPLATE for one such function is
        %CPLEXParamSet
        cpxControl=eval(contFunctName);
    else
        cpxControl=[];
    end
end

if ~isfield(LPProblem,'A')
    if ~isfield(LPProblem,'S')
            error('Equality constraint matrix must either be a field denoted A or S.')
    end
    LPProblem.A=LPProblem.S;
end

if ~isfield(LPProblem,'csense')
    nMet=size(LPProblem.A);
    if printLevel>0
        fprintf('%s\n','Assuming equality constraints, i.e. S*v=b');
    end
    %assuming equality constraints
    LPProblem.csense(1:nMet,1)='E';
end

if ~isfield(LPProblem,'osense')
    %assuming maximisation
    LPProblem.osense=-1;
    if printLevel>0
        fprintf('%s\n','Assuming maximisation of objective');
    end
end

if size(LPProblem.A,2)~=length(LPProblem.c)
    error('dimensions of A & c are inconsistent');
end

if size(LPProblem.A,2)~=length(LPProblem.lb) || size(LPProblem.A,2)~=length(LPProblem.ub)
    error('dimensions of A & bounds are inconsistent');
end

%get data
[c,x_L,x_U,b,csense,osense] = deal(LPProblem.c,LPProblem.lb,LPProblem.ub,...
    LPProblem.b,LPProblem.csense,LPProblem.osense);
%modify objective to correspond to osense
c=full(c*osense);

%cplex expects it dense
b=full(b);

%complex ibm ilog cplex interface
if ~isempty(csense)
    %set up constant vectors for CPLEX
    b_L(csense == 'E',1) = b(csense == 'E');
    b_U(csense == 'E',1) = b(csense == 'E');
    b_L(csense == 'G',1) = b(csense == 'G');
    b_U(csense == 'G',1) = Inf;
    b_L(csense == 'L',1) = -Inf;
    b_U(csense == 'L',1) = b(csense == 'L');
end

% Initialize the CPLEX object
try
    ILOGcplex = Cplex('fba');
catch ME
    error('CPLEX not installed or licence server not up')
end

ILOGcplex.Model.sense = 'minimize';

% Now populate the problem with the data
ILOGcplex.Model.obj   = c;
ILOGcplex.Model.lb    = x_L;
ILOGcplex.Model.ub    = x_U;
ILOGcplex.Model.A     = LPProblem.A;
ILOGcplex.Model.lhs   = b_L;
ILOGcplex.Model.rhs   = b_U;

%loop through parameters
ILOGcplex = setCplexParam(ILOGcplex, cpxControl, 1);
optimParam=cpxControl;

%Call parameter tuner
if ~ILOGcplex.tuneParam()
    fprintf('Optimal parameters found. \n');
    [paramList, paramPath] = getParamList(cpxControl, 1);
     for i=1:length(paramPath)
         try
             eval(['optimParam.' paramPath{i} '=ILOGcplex.Param.' paramPath{i} '.Cur;']);
         catch ME
             fprintf(['Parameter ' paramPath{i} ' was not found. \n']);
         end
     end
else
    fprintf('Optimisation failed. \n')
end

end


function f=burger(u,der)
% if second arg. absent u^2/2, else u.
if nargin<2
	f=0.5*u.^2;
else
	f=u;
end;


function exact = p14_exact ( )

%*****************************************************************************80
%
%% P14_EXACT returns the estimated integral for problem 14.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the estimated value of the integral.
%
  exact = 1.0634618101722400407;

  return
end


function res = exp_same(a,b)
% Check whether two expressions are structurally identical.
% Result = exp_same(Expression-A, Expression-B)
%
% Differs from isequal() in the following ways:
%  a) ignores the values associated with impure expressions
%  b) treats @x as equal to exp_symbol('x'); therefore, @x is a shortcut notation for exp_symbol('x').
%
% See also:
%   exp_match
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2010-04-19

if ~exp_beginfun('symbolic') return; end

res = utl_same(a,b);

exp_endfun;


function g = netderiv(w, net, x)
%NETDERIV Evaluate derivatives of network outputs by weights generically.
%
%	Description
%
%	G = NETDERIV(W, NET, X) takes a weight vector W and a network data
%	structure NET, together with the matrix X of input vectors, and
%	returns the gradient of the outputs with respect to the weights
%	evaluated at W.
%
%	See also
%	NETEVFWD, NETOPT
%

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

fstr = [net.type, 'deriv'];
net = netunpak(net, w);
g = feval(fstr, net, x);


% Gcone_test.m
% test the Gcone object
% Copyright 2008-1-1, Jeff Fessler, University of Michigan

printm 'todo: check user_source_zs vs hct2 with file_source_z'

pn = jf_protected_names;

f.sf1 = {'sf1', ...
	'sf1,p:ts,b:st', 'sf1,p:st,b:st', ...
	'sf1,p:ts,b:ts', 'sf1,p:st,b:ts'};
f.sf2 = {'sf2', ...
	'sf2,p:ts,b:st', 'sf2,p:st,b:st', ...
	'sf2,p:ts,b:ts', 'sf2,p:st,b:ts'};
systypes = { ... % lots of variations on system models!
	'rf1', ... % RR model
	f.sf1{:}, f.sf2{:}, ... % SF variations
	'sf1', 'sf2', 'sf3', 'sf4', 'sf5' ...
};
%systypes = {'rf1', 'sf1', 'dd2'}
if 0 && exist('dd_ge1_mex') == 3 % UM only
	systypes{end+1} = 'dd1';
	systypes{end+1} = 'dd2'; % todo
end
% systypes = {'nn1', 'pd1'};
%systypes = {'dd2'}; % todo! fails adjoint test!?
%systypes = {'sf1', 'sf2'};
%systypes = f.sf2;
nsys = numel(systypes);

%f.class = 'Fatrix';
f.class = 'fatrix2';

% todo: test both ways for fatrix2, not for Fatrix
%is_zxy = false;
is_zxy = true;
if is_zxy
	to_zxy = @(x) permute(x, [3 1 2]);
else
	to_zxy = @(x) x;
end

% small systems for basic tests
if 1 && (0 || ~isvar('A1')), printm 'setup small'
	f.down = 16;

	dfs_list = [inf inf 0]; % parallel flat arc
	dsd_list = [inf 949.075 949.075]; % parallel flat arc

	if 0 % arc only
		dfs_list = [0];
		dsd_list = [949.075];
	end

	for kk = 1:numel(dfs_list)

		cgs = ct_geom('ge2', 'nt', 320, ...
			'source_z0', -20, 'pitch', 0.5, ... % test helix
			'dfs', dfs_list(kk), ... % arc or flat
			'dsd', dsd_list(kk), ... % fan or parallel beam
			'down', f.down);

		for ii=1:nsys
			systype = systypes{ii};
			printm('testing type %s dfs=%g dsd=%g', ...
				systype, cgs.dfs, cgs.dsd)

			if streq(systype, 'dd1') || streq(systype, 'dd2')
				f.dy = 1; % DD requires square pixels
				if isinf(cgs.dsd) % no parallel for DD
					continue
				end
			else
				f.dy = 0.7; % stress test SF with non-square
			%	f.dy = 'dx';
			end

			igs = image_geom('nx', 512, 'ny', 480, 'nz', 416, ...
				'dx', 1, 'dy', f.dy, 'dz', 0.5, ...
				'offset_x', 2.9, 'offset_y', 3.5, ...
				'offset_z', -3.4, ...
				'mask', 'all-but-edge', ... % trick: for hct2
				'down', f.down);

			if im, cgs.plot(igs); end

			args = {cgs, igs, 'type', systype, 'class', f.class};
			if streq(systype, 'sf4') || streq(systype, 'sf5')
				args = {args{:}, 'mexarg', {single(0)}};
			end

			args = {args{:}, 'zxy', is_zxy};

			A1 = Gcone(args{:}, 'nthread', 1);
			Ac = Gcone(args{:});

			if 1 % adjoint
				tester_tomo2(A1, to_zxy(igs.mask), 'G2', Ac)
				test_adjoint(A1, 'big', 1, 'tol', 5e-5)
				test_adjoint(Ac, 'big', 1, 'tol', 5e-5)
			end

			if streq(systype, 'dd1') || streq(systype, 'dd2')
				continue % don't bother hct2 test for DD
			end

			% hereafter is hct2 test
			Ah = Gcone(args{:}, 'use_hct2', 1);

			if pn.has_hct2
				if streq(systype, 'nn1') || streq(systype, 'pd1')
					thresh = 3e-2; % big due to rounding in nn1
				else
					thresh = 4e-6;
				end

				if 1 % mex vs hct2
					xs = single(igs.mask);
					xs(round(end/3), round(2*end/3), round(end/4)) = 10;
					xs = to_zxy(xs);
					t1 = A1 * xs;
					t2 = Ah * xs;
					equivs(t1, t2, 'thresh', thresh)

					b1 = A1' * t1;
					b2 = Ah' * t1;
%					im plc 1 3, im(1, b1), im(2, b2), im(3, b2-b1)
					equivs(b1, b2, 'thresh', thresh)
				end
				if 0 % block
					B1 = Gblock(A1, 2);
					Bh = Gblock(Ah, 2);
					t1 = B1{2} * xs;
					t2 = Bh{2} * xs;
					equivs(t1, t2, 'thresh', thresh)
				end

				tester_tomo2(Ah, to_zxy(igs.mask), ...
					'equiv_thresh', thresh) % paces
				test_adjoint(Ah, 'big', 1, 'tol', 5e-5)
			end
		end % systype
	end % dfs
end % small

if 0, printm 'rf1' % test rf1 vs dd2 small
	A0 = Gcone(cgs, igs, 'type', 'rf1');
	Ad = Gcone(cgs, igs, 'type', 'dd2');
%	x0 = single(igs.mask);
	x0 = igs.circ;
	im(x0)
	cpu etic
	yd = Ad * x0;
	cpu etoc dd
	if 1
		cpu etic
		y0 = A0 * x0;
		cpu etoc rf1
	end
	im plc 2 2
	im(yd), im(y0), im(y0-yd)
return
end

if ~isvar('x0'), printm 'x0 big'
	f.down = 4;
	igb = image_geom('nx', 512, 'ny', 480, 'nz', 416, ...
		'dx', 1, 'dz', 0.5, ...
		'dy', 0.7, ... % stress test
		'offset_x', 12.9, 'offset_y', 3.5, 'offset_z', -3.4, ...
		'down', f.down);
	ell = [3*igb.dx 5*igb.dx -2*igb.dz ...
		igb.dx*igb.nx/3 igb.dy*igb.ny/4 igb.zfov/4 ...
		0 0 10];
	x0 = ellipsoid_im(igb, ell, 'oversample', 2);
%	clf, im(x0), return
%prompt
end

% big systems for accuracy tests
if 0 || ~isvar('Ab'), printm 'setup big'
	cgb = ct_geom('ge1', 'nt', 320, ...
		'source_z0', -40, 'pitch', 0.5, ... % test helix
	...%	'dfs', inf, ... % flat detector
	...%	'dsd', inf, 'dfs', inf, ... % parallel beam
		'down', f.down);

	clear Ab Ah
	for ii=1:nsys
		systype = systypes{ii};
		if streq(systype, 'dd', 2) && isinf(cgb.dsd)
			Ab{ii} = [];
			continue
		end
		Ab{ii} = Gcone(cgb, igb, 'type', systype, ...
				'zxy', is_zxy, 'class', f.class);
	end
end

if ~isvar('ya'), printm 'analytical projections'
	ya = ellipsoid_proj(cgb, ell, 'oversample', 2);
%	im clf, im(ya)
%prompt
end

if 0 || ~isvar('yb'), printm 'discrete projections'
	nrmse = @(x,y) norm(y(:)-x(:)) / norm(x(:)) * 100;
	for ii=1:nsys
		if ~isempty(Ab{ii})
			cpu etic
			yb{ii} = Ab{ii} * to_zxy(x0);
			f.time(ii) = cpu('etoc');
			printm('%s: time %g nrmse %g %%', ...
				systypes{ii}, f.time(ii), nrmse(ya, yb{ii}))
		else
			f.time(ii) = 0;
			yb{ii} = cgb.zeros;
		end
	end
end

if 0 % toggle analytical vs big discrete
	ii = 1;
	im_toggle(ya(:,end/2,:), yb{ii}(:,end/2,:), ...
		yb{ii}(:,end/2,:) - ya(:,end/2,:))
	im(ya(:,end/2,:) - yb{ii}(:,end/2,:))
return
end

if 0, printm 'look at error in worst views'
	ilist = 1:nsys;
	ilist = [1 2 7];
	im('plc', 2, numel(ilist))
	for jj=1:numel(ilist)
		ii = ilist(jj);
		err = yb{ii} - ya;
		tmp = reshape(err, [], cgb.na);
		tmp = sum(tmp.^2); % error in each view
		ia = imax(tmp); % worst view
		im(jj, err(:,:,ia)), cbar h
		titlef('%s ia=%d', systypes{ii}, ia)
		im('subplot', jj+numel(ilist))
		plot(tmp), axis tight
	end
return
end

if 1, printm 'projection profiles'
	it = cgb.nt;
	it = round(cgb.nt/2); % middle
	it = it + [-2 0 2];
	ia = imin(abs(cgb.ad - 45)); % closest to 45
%	ia = ceil(ia/2);
	pro = @(y) col(y(:,it,ia));
	arg = [];
	for ii=1:numel(systypes)
		arg = [arg pro(yb{ii})];
	end
	if im
		clf, plot([arg pro(ya)])
		text(10, 200, sprintf('ia=%d', ia))
		text(10, 400, sprintf('ang=%g', cgb.ad(ia)))
		ir_legend({systypes{:}, 'true'})
		axisy(0, 1.2 * max(col(pro(ya))))
		grid
	end
%prompt
return
end

if 0 % dd1 vs dd2 - they match well
	i_dd1 = strmatch('dd1', systypes);
	i_dd2 = strmatch('dd2', systypes);
	im clf, im(yb{i_dd1} - yb{i_dd2}), cbar
	equivs(yb{i_dd1}, yb{i_dd2}, 'thresh', 2e-5)
return
end

if 1, printm 'show projections and differences'
	im('plc', 2, 1+nsys)
	im(1, x0)
	ia = round([1 cgb.na/4 cgb.na/2 cgb.na]);
	im(nsys+2, ya(:,:,ia)), axis normal

	for ii=1:nsys
		tmp = yb{ii};
		im(ii+1, tmp(:,:,ia)), axis normal
		xlabel(systypes{ii})
		im(ii+2+nsys, tmp(:,:,ia)-ya(:,:,ia)), axis normal
	end
prompt
end

if 1, printm 'show back-projections and pairwise differences'
	im('plc', nsys, nsys)
	iz = round([1 igb.nz/4 igb.nz/2 igb.nz]);
	iz = 1:2:igb.nz;
	clear yt
	for ii=1:nsys
%		tmp = cgb.ones;
		tmp = cgb.zeros; tmp(:,:,20) = 1;
		yt{ii} = Ab{ii}' * tmp;
	end
	for ii=1:nsys
		for jj=1:nsys
			if ii == jj
				im('subplot', (ii-1)*nsys + jj)
				im(yt{ii}(:,:,iz))
				xlabel(systypes{ii})
			elseif ii > jj
				im('subplot', (ii-1)*nsys + jj)
				im(yt{ii}(:,:,iz) - yt{jj}(:,:,iz))
				xlabelf([systypes{ii} ' - ' systypes{jj}])
			end
		end
	end
end


function []=uw_sb_unwrap_space_time(day,ifgday_ix,unwrap_method,time_win,la_flag,bperp,n_trial_wraps,prefilt_win,scf_flag,temp,n_temp_wraps,max_bperp_for_temp_est)
%UW_SB_UNWRAP_SPACE_TIME smooth and unwrap phase diffs between neighboring data points in time
%
%   Andy Hooper, June 2007
%
%   ======================================================================
%   07/2008 AH: Allow isolated images, not included in any interferogram
%   01/2010 AH: Allow for non-positive definite inverse var/covar matrix
%   01/2012 AH: New option la_flag that estimates LA error for each arc
%   02/2012 AH: New method 3D_NEW
%   02/2012 AH: New method 3D_FULL
%   03/2012 AH: version updated for subsets of full network
%   10/2012 AH: Bug corrected in 3D_FULL
%   11/2012 AH: Add temperature option and scf_flag
%   09/2013 DB: Fix compatibility issue with matlab 2009
%   10/2013 DB: Fix introduced bug on them variable
%   09/2014 DB: Fix the order in which isolated dates are removed. Start
%               from big to small.
%   02/2014 AH: Add predefined ph_uw option
%   ======================================================================
%
%
disp('Unwrapping in time-space...')

uw=load('uw_grid');
ui=load('uw_interp');

n_ifg=uw.n_ifg;
n_ps=uw.n_ps;
nzix=uw.nzix;
ij=uw.ij;

if isempty(uw.ph_uw_predef)
    predef_flag='n';
else
    predef_flag='y';
end

n_image=size(day,1);
master_ix=find(day==0);
[nrow,ncol]=size(ui.Z);

day_pos_ix=find(day>0);
[tempdummy,I]=min(day(day_pos_ix));
dph_space=((uw.ph(ui.edgs(:,3),:).*conj(uw.ph(ui.edgs(:,2),:))));
if predef_flag=='y'
    dph_space_uw=uw.ph_uw_predef(ui.edgs(:,3),:)-uw.ph_uw_predef(ui.edgs(:,2),:);
    predef_ix=~isnan(dph_space_uw);
    dph_space_uw=dph_space_uw(predef_ix);
else
    predef_ix=[];
end
clear uw tempdummy

dph_space=dph_space./abs(dph_space);

ifreq_ij=[];
jfreq_ij=[];

G=zeros(n_ifg,n_image);
for i=1:n_ifg
    G(i,ifgday_ix(i,1))=-1;
    G(i,ifgday_ix(i,2))=1;
end

nzc_ix=sum(abs(G))~=0; % non-zero column index
day=day(nzc_ix);
n_image=size(day,1);
G=G(:,nzc_ix);
zc_ix=find(nzc_ix==0);
zc_ix = sort(zc_ix,'descend');              % fix to make sure the order goes from largest to smallest
for i=1:length(zc_ix)
    ifgday_ix(ifgday_ix>zc_ix(i))=ifgday_ix(ifgday_ix>zc_ix(i))-1;
end
n=size(G,2);

if ~isempty(temp)
    temp_flag='y';
else
    temp_flag='n';
end

if strcmpi(temp_flag,'y')
    fprintf('   Estimating temperature correlation (elapsed time=%ds)\n',round(toc))
    ix=abs(bperp)<max_bperp_for_temp_est;
    temp_sub=temp(ix);
    temp_range=max(temp)-min(temp);
    temp_range_sub=max(temp_sub)-min(temp_sub);
    dph_sub=dph_space(:,ix); % only ifgs using ith image
    n_temp_wraps=n_temp_wraps*(temp_range_sub/temp_range);

    trial_mult=[-ceil(8*n_temp_wraps):ceil(8*n_temp_wraps)];
    n_trials=length(trial_mult);
    trial_phase=temp_sub/temp_range_sub*pi/4;
    trial_phase_mat=exp(-j*trial_phase*trial_mult);
    Kt=zeros(ui.n_edge,1,'single');
    coh=zeros(ui.n_edge,1,'single');
    for i=1:ui.n_edge
        cpxphase=dph_sub(i,:).';
        cpxphase_mat=repmat(cpxphase,1,n_trials);
        phaser=trial_phase_mat.*cpxphase_mat;
        phaser_sum=sum(phaser);
        coh_trial=abs(phaser_sum)/sum(abs(cpxphase));
        [coh_max,coh_max_ix]=max(coh_trial);
        falling_ix=find(diff(coh_trial(1:coh_max_ix))<0); % segemnts prior to peak where falling
        if ~isempty(falling_ix)
            peak_start_ix=falling_ix(end)+1;
        else
            peak_start_ix=1;
        end
        rising_ix=find(diff(coh_trial(coh_max_ix:end))>0); % segemnts after peak where rising
        if ~isempty(rising_ix)
            peak_end_ix=rising_ix(1)+coh_max_ix-1;
        else
            peak_end_ix=n_trials;
        end
        coh_trial(peak_start_ix:peak_end_ix)=0;
        if coh_max-max(coh_trial)>0.1 % diff between peak and next peak at least 0.1
            K0=pi/4/temp_range_sub*trial_mult(coh_max_ix);
            resphase=cpxphase.*exp(-1i*(K0*temp_sub)); % subtract approximate fit
            offset_phase=sum(resphase);
            resphase=angle(resphase*conj(offset_phase)); % subtract offset, take angle (unweighted)
            weighting=abs(cpxphase);
            mopt=double(weighting.*temp_sub)\double(weighting.*resphase);
            Kt(i)=K0+mopt;
            phase_residual=cpxphase.*exp(-1i*(Kt(i)*temp_sub));
            mean_phase_residual=sum(phase_residual);
            coh(i)=abs(mean_phase_residual)/sum(abs(phase_residual));
        end
    end

    clear cpxphase_mat trial_phase_mat phaser
    Kt(coh<0.31)=0; % not to be trusted;
    dph_space=dph_space.*exp(-1i*Kt*temp');
    if predef_flag=='y'
        dph_temp=Kt*temp';
        dph_space_uw=dph_space_uw-dph_temp(predef_ix);
        clear dph_temp
    end
    dph_sub=dph_sub.*exp(-1i*Kt*temp_sub');

end

if strcmpi(la_flag,'y')

    fprintf('   Estimating look angle error (elapsed time=%ds)\n',round(toc))

    bperp_range=max(bperp)-min(bperp);
    ix=find(abs(diff(ifgday_ix,[],2))==1);

    if length(ix)>=length(day)-1 % if almost full chain is present
        fprintf('   using sequential daisy chain of interferograms\n')
        dph_sub=dph_space(:,ix); % only ifgs using ith image
        bperp_sub=bperp(ix);
        bperp_range_sub=max(bperp_sub)-min(bperp_sub);
        n_trial_wraps=n_trial_wraps*(bperp_range_sub/bperp_range);
    else
        ifgs_per_image=sum(abs(G));
        [max_ifgs_per_image,max_ix]=max(ifgs_per_image);
        if max_ifgs_per_image>=length(day)-2; % can create chain from (almost) complete single master series
            fprintf('   Using sequential daisy chain of interferograms\n')
            ix=G(:,max_ix)~=0;
            gsub=G(ix,max_ix);
            sign_ix=-sign(single(gsub'));
            dph_sub=dph_space(:,ix); % only ifgs using chosen master image
            bperp_sub=[bperp(ix)];
            bperp_sub(sign_ix==-1)=-bperp_sub(sign_ix==-1);
            bperp_sub=[bperp_sub;0]; % add master
            sign_ix=repmat(sign_ix,ui.n_edge,1);
            dph_sub(sign_ix==-1)=conj(dph_sub(sign_ix==-1)); % flip sign if necessary to make ith image master
            dph_sub=[dph_sub,mean(abs(dph_sub),2)]; % add zero phase master
            slave_ix=sum(ifgday_ix(ix,:),2)-max_ix;
            day_sub=day([slave_ix;max_ix]); % extract days for subset
            [day_sub,sort_ix]=sort(day_sub); % sort ascending day
            dph_sub=dph_sub(:,sort_ix); % sort ascending day
            bperp_sub=bperp_sub(sort_ix);
            bperp_sub=diff(bperp_sub);
            bperp_range_sub=max(bperp_sub)-min(bperp_sub);
            n_trial_wraps=n_trial_wraps*(bperp_range_sub/bperp_range);
            n_sub=length(day_sub);
            dph_sub=dph_sub(:,[2:end]).*conj(dph_sub(:,1:end-1)); % sequential dph, to reduce influence of defo
            dph_sub=dph_sub./abs(dph_sub); % normalise
        else % just use all available
            dph_sub=dph_space;
            bperp_sub=bperp;
            bperp_range_sub=bperp_range;
        end
    end

    trial_mult=[-ceil(8*n_trial_wraps):ceil(8*n_trial_wraps)];
    n_trials=length(trial_mult);
    trial_phase=bperp_sub/bperp_range_sub*pi/4;
    trial_phase_mat=exp(-j*trial_phase*trial_mult);
    K=zeros(ui.n_edge,1,'single');
    coh=zeros(ui.n_edge,1,'single');
    for i=1:ui.n_edge
        cpxphase=dph_sub(i,:).';
        cpxphase_mat=repmat(cpxphase,1,n_trials);
        phaser=trial_phase_mat.*cpxphase_mat;
        phaser_sum=sum(phaser);
        coh_trial=abs(phaser_sum)/sum(abs(cpxphase));
        [coh_max,coh_max_ix]=max(coh_trial);
        falling_ix=find(diff(coh_trial(1:coh_max_ix))<0); % segemnts prior to peak where falling
        if ~isempty(falling_ix)
            peak_start_ix=falling_ix(end)+1;
        else
            peak_start_ix=1;
        end
        rising_ix=find(diff(coh_trial(coh_max_ix:end))>0); % segemnts after peak where rising
        if ~isempty(rising_ix)
            peak_end_ix=rising_ix(1)+coh_max_ix-1;
        else
            peak_end_ix=n_trials;
        end
        coh_trial(peak_start_ix:peak_end_ix)=0;
        if coh_max-max(coh_trial)>0.1 % diff between peak and next peak at least 0.1
            K0=pi/4/bperp_range_sub*trial_mult(coh_max_ix);
            resphase=cpxphase.*exp(-1i*(K0*bperp_sub)); % subtract approximate fit
            offset_phase=sum(resphase);
            resphase=angle(resphase*conj(offset_phase)); % subtract offset, take angle (unweighted)
            weighting=abs(cpxphase);
            mopt=double(weighting.*bperp_sub)\double(weighting.*resphase);
            K(i)=K0+mopt;
            phase_residual=cpxphase.*exp(-1i*(K(i)*bperp_sub));
            mean_phase_residual=sum(phase_residual);
            coh(i)=abs(mean_phase_residual)/sum(abs(phase_residual));
        end
    end

    clear cpxphase_mat trial_phase_mat phaser dph_sub
    K(coh<0.31)=0; % not to be trusted;
    if strcmpi(temp_flag,'y')
        dph_space(K==0,:)=dph_space(K==0,:).*exp(1i*Kt(K==0)*temp'); % add back temp correction if not able to estimate DEM reliably
        Kt(K==0)=0;
        K(Kt==0)=0;
    end
    dph_space=dph_space.*exp(-1i*K*bperp');
    if predef_flag=='y'
        dph_scla=K*bperp';
        dph_space_uw=dph_space_uw-dph_scla(predef_ix);
        clear dph_scla
    end

end

spread=sparse(zeros(ui.n_edge,n_ifg));

if strcmpi(unwrap_method,'2D')
    dph_space_uw=angle(dph_space);
    if strcmpi(la_flag,'y')
        dph_space_uw=dph_space_uw+K*bperp';   % equal to dph_space + integer cycles
    end
    if strcmpi(temp_flag,'y')
        dph_space_uw=dph_space_uw+Kt*temp';   % equal to dph_space + integer cycles
    end
    dph_noise=[];
    save('uw_space_time','dph_space_uw','spread','dph_noise');
elseif strcmpi(unwrap_method,'3D_NO_DEF')
    dph_noise=angle(dph_space);
    dph_space_uw=angle(dph_space);
    if strcmpi(la_flag,'y')
        dph_space_uw=dph_space_uw+K*bperp';   % equal to dph_space + integer cycles
    end
    if strcmpi(temp_flag,'y')
        dph_space_uw=dph_space_uw+Kt*temp';   % equal to dph_space + integer cycles
    end
    save('uw_space_time','dph_space_uw','dph_noise','spread');
else
    fprintf('   Smoothing in time (elapsed time=%ds)\n',round(toc))

    if strcmpi(unwrap_method,'3D_FULL')

       dph_smooth_ifg=nan(size(dph_space),'single');

       for i=1:n_image
         ix=G(:,i)~=0;
         if sum(ix)>=n_image-2 % allow only 1 missing ifg for each image as master
           gsub=G(ix,i);
           dph_sub=dph_space(:,ix); % only ifgs using ith image
           sign_ix=repmat(-sign(single(gsub')),ui.n_edge,1);
           dph_sub(sign_ix==-1)=conj(dph_sub(sign_ix==-1)); % flip sign if necessary to make ith image master
           slave_ix=sum(ifgday_ix(ix,:),2)-i;
           day_sub=day(slave_ix); % extract days for subset
           [day_sub,sort_ix]=sort(day_sub); % sort ascending day
           dph_sub=dph_sub(:,sort_ix); % sort ascending day
           dph_sub_angle=angle(dph_sub);
           n_sub=length(day_sub);
           dph_smooth=zeros(ui.n_edge,n_sub,'single');
           for i1=1:n_sub
               time_diff=(day_sub(i1)-day_sub)';
               weight_factor=exp(-(time_diff.^2)/2/time_win^2);
               weight_factor=weight_factor/sum(weight_factor);

               dph_mean=sum(dph_sub.*repmat(weight_factor,ui.n_edge,1),2);
               %dph_mean_adj=angle(dph_sub.*repmat(conj(dph_mean),1,n_sub)); % subtract weighted mean
               dph_mean_adj=mod(dph_sub_angle-repmat(angle(dph_mean),1,n_sub)+pi,2*pi)-pi;
               GG=[ones(n_sub,1),time_diff'];
               if size(GG,1)>1
                   m=lscov(GG,double(dph_mean_adj)',weight_factor);
               else
                   m=zeros(size(GG,1),ui.n_edge);
               end
               %dph_mean_adj=mod(dph_mean_adj-(GG*m)'+pi,2*pi)-pi; % subtract first estimate
               %m2=lscov(GG,double(dph_mean_adj)',weight_factor);
               %dph_smooth(:,i1)=dph_mean.*exp(1i*(m(1,:)'+m2(1,:)')); % add back weighted mean
               dph_smooth(:,i1)=dph_mean.*exp(1i*(m(1,:)')); % add back weighted mean
            end
            dph_smooth_sub=cumsum([angle(dph_smooth(:,1)),angle(dph_smooth(:,2:end).*conj(dph_smooth(:,1:end-1)))],2);
            close_master_ix=find(slave_ix-i>0);
            if isempty(close_master_ix)
                close_master_ix=n_sub;
            else
                close_master_ix=close_master_ix(1);
                if close_master_ix>1
                    close_master_ix=[close_master_ix-1;close_master_ix];
                end
            end
            dph_close_master=mean(dph_smooth_sub(:,close_master_ix),2);
            dph_smooth_sub=dph_smooth_sub-repmat(dph_close_master-angle(exp(j*dph_close_master)),1,n_sub);
            dph_smooth_sub=dph_smooth_sub.*sign_ix;
            already_sub_ix=find(~isnan(dph_smooth_ifg(1,ix))); % already unwrapped index
            ix=find(ix);
            already_ix=ix(already_sub_ix);
            std_noise1=std(angle(dph_space(:,already_ix).*exp(-1i*dph_smooth_ifg(:,already_ix))));
            std_noise2=std(angle(dph_space(:,already_ix).*exp(-1i*dph_smooth_sub(:,already_sub_ix))));
            keep_ix=true(n_sub,1);
            keep_ix(already_sub_ix(std_noise1<std_noise2))=false; % keep least noisy
            dph_smooth_ifg(:,ix(keep_ix))=dph_smooth_sub(:,keep_ix);
          end
        end

        dph_noise=angle(dph_space.*exp(-1i*dph_smooth_ifg));
        dph_noise(std(dph_noise,0,2)>1.2,:)=nan;

    else
        x=(day-day(1))*(n-1)/(day(end)-day(1)); % use dates for smoothing
        if predef_flag=='y'
            n_dph=size(dph_space,1);
            dph_space_angle=double(angle(dph_space));
            dph_space_angle(predef_ix)=dph_space_uw;
            dph_space_series=zeros(n,n_dph);
            for i=1:n_dph
                 W=predef_ix(i,:)+0.01; % give more weight to the predfined unwrapped
                 dph_space_series(2:end,i)=lscov(double(G(:,2:end)),dph_space_angle(i,:)',W);
            end
        else
            dph_space_series=[zeros(1,ui.n_edge);double(G(:,2:end))\double(angle(dph_space))'];
        end

        dph_smooth_series=zeros(size(G,2),ui.n_edge,'single');

        for i1=1:n
            time_diff_sq=(day(i1)-day).^2;
            weight_factor=exp(-time_diff_sq/2/time_win^2);
            weight_factor=weight_factor/sum(weight_factor);
            dph_smooth_series(i1,:)=sum(dph_space_series.*repmat(weight_factor,1,ui.n_edge));
        end

        dph_smooth_ifg=(G*dph_smooth_series)';
        dph_noise=angle(dph_space.*exp(-1i*dph_smooth_ifg));

        if strcmpi(unwrap_method,'3D_SMALL_DEF')|...
           strcmpi(unwrap_method,'3D_QUICK')
          not_small_ix=find(std(dph_noise,0,2)>1.3)';
          fprintf('   %d edges with high std dev in time (elapsed time=%ds)\n',length(not_small_ix),round(toc))
          dph_noise(not_small_ix,:)=nan;
        else % 3D
          uw=load('uw_grid');
          ph_noise=angle(uw.ph.*conj(uw.ph_lowpass));
          clear uw
          dph_noise_sf=((ph_noise(ui.edgs(:,3),:)-(ph_noise(ui.edgs(:,2),:))));
          m_minmax=repmat([-pi,pi],5,1).*repmat([0.5;0.25;1;0.25;1],1,2);
          anneal_opts=[1;15;0;0;0;0;0];
          covm=cov((dph_noise_sf)); % estimate of covariance
          [W,P]=chol(inv(covm)); % weighting matrix
          if P~=0
              W=diag(1./sqrt(diag(covm)));
          end
          not_small_ix=find(std(dph_noise,0,2)>1)';
          fprintf('   Performing complex smoothing on %d edges (elapsed time=%ds)\n',length(not_small_ix),round(toc))

          n_proc=0;
          for i=not_small_ix
            dph=angle(dph_space(i,:))';
            dph_smooth_series(:,i)=uw_sb_smooth_unwrap(m_minmax,anneal_opts,G,W,dph,x);

            n_proc=n_proc+1;
            if round(n_proc/1000)==n_proc/1000
                save('uw_unwrap_time','G','dph_space','dph_smooth_series');
                fprintf('%d edges of %d reprocessed (elapsed time=%ds)\n',n_proc,length(not_small_ix),round(toc))
            end
          end
          dph_smooth_ifg=(G*dph_smooth_series)';
          dph_noise=angle(dph_space.*exp(-1i*dph_smooth_ifg));
        end
    end
    clear dph_space
    dph_space_uw=dph_smooth_ifg+dph_noise;
    clear dph_smooth_ifg

    if strcmpi(la_flag,'y')
        dph_space_uw=dph_space_uw+K*bperp';   % equal to dph_space + integer cycles
    end
    if strcmpi(temp_flag,'y')
        dph_space_uw=dph_space_uw+Kt*temp';   % equal to dph_space + integer cycles
    end

    if strcmpi(scf_flag,'y')

        fprintf('   Calculating local phase gradients (elapsed time=%ds)\n',round(toc))
        ifreq_ij=nan(n_ps,n_ifg,'single');
        jfreq_ij=nan(n_ps,n_ifg,'single');
        ifgw=zeros(nrow,ncol);
        uw=load('uw_grid');
        for i=1:n_ifg
            ifgw(nzix)=uw.ph(:,i);
            [ifreq,jfreq,grad_ij,Hmag]=gradient_filt(ifgw,prefilt_win);
            ix=~isnan(ifreq)&Hmag./((abs(ifreq))+1)>3;
            if sum(ix(:))>2
                ifreq_ij(:,i)=griddata(grad_ij(ix,2),grad_ij(ix,1),ifreq(ix),ij(:,2),ij(:,1),'linear');
            end
            ix=~isnan(jfreq)&Hmag./((abs(jfreq))+1)>3;
            if sum(ix(:))>2
                jfreq_ij(:,i)=griddata(grad_ij(ix,2),grad_ij(ix,1),jfreq(ix),ij(:,2),ij(:,1),'linear');
            end
        end
        clear uw

        spread2=zeros(size(spread),'single');
        dph_smooth_uw2=nan(ui.n_edge,n_ifg,'single');

        fprintf('   Smoothing using local phase gradients (elapsed time=%ds)\n',round(toc))
        for i=1:ui.n_edge
            nodes_ix=ui.edgs(i,[2:3]);
            ifreq_edge=mean(ifreq_ij(nodes_ix,:));
            jfreq_edge=mean(jfreq_ij(nodes_ix,:));
            diff_i=diff(ij(nodes_ix,1));
            diff_j=diff(ij(nodes_ix,2));
            dph_smooth_uw2(i,:)=diff_i*ifreq_edge+diff_j*jfreq_edge;
%           spread2(i,:)=diff_i*diff(ifreq_ij(nodes_ix,:))+diff_j*diff(jfreq_ij(nodes_ix,:));
            spread2(i,:)=diff(ifreq_ij(nodes_ix,:))+diff(jfreq_ij(nodes_ix,:));
        end
        fprintf('   Choosing between time and phase gradient smoothing (elapsed time=%ds)\n',round(toc))
        std_noise=std(dph_noise,0,2);
        dph_noise2=angle(exp(j*(dph_space_uw-dph_smooth_uw2)));
        std_noise2=std(dph_noise2,0,2);
        dph_noise2(std_noise2>1.3,:)=nan;
        shaky_ix=isnan(std_noise) | std_noise>std_noise2; % spatial smoothing works better index

        fprintf('   %d arcs smoothed in time, %d in space (elapsed time=%ds)\n',ui.n_edge-sum(shaky_ix),sum(shaky_ix),round(toc))

        dph_noise(shaky_ix,:)=dph_noise2(shaky_ix,:);
        dph_space_uw(shaky_ix,:)=dph_smooth_uw2(shaky_ix,:)+dph_noise2(shaky_ix,:);
        spread(shaky_ix,:)=spread2(shaky_ix,:);
    else
        shaky_ix=[];
    end

    save('uw_space_time','dph_space_uw','dph_noise','G','spread','ifreq_ij','jfreq_ij','shaky_ix','predef_ix');
end


% Codes for CVPR-15 work `Face Alignment by Coarse-to-Fine Shape Searching'
% Any question please contact Shizhan Zhu: zhshzhutah2@gmail.com
% Released on July 25, 2015

function newPose = transPoseFwd(oldPose,T)
% newPose = transPoseFwd(oldPose,T)
% produce new pose according to oldPose and T
% size(oldPose) = [m,2n], each row must in the format of X1...Xn,Y1...Yn.
% T is a array containing n structures. Each one is the trans for one
% sample.
% newPose has the same size as oldPose.

m = size(oldPose,1);
if length(T)~=m,error('length(T) must be the same as size(oldPose,1)!');end;
n = size(oldPose,2);
if mod(n,2),error('size(pose,2) is odd!');end
n = n / 2;
ind_nan = find(isnan(oldPose));
oldPose(ind_nan) = 0;

if (m<100) || (matlabpool('size')==0)
    newPose = arrayfun(@(i)reshape(tformfwd(T(i),reshape(oldPose(i,:),n,2)),1,2*n),1:m,'UniformOutput',false);
    newPose = cell2mat(newPose(:));
    if m>=100
        warning('Please launch matlabpool to speed up your program!');
    end
else
    newPose = zeros(size(oldPose));
    parfor i = 1:m
        newPose(i,:) = reshape(tformfwd(T(i),reshape(oldPose(i,:),n,2)),1,2*n);
    end
end
newPose(ind_nan) = NaN;

end


function [outputs, out_blob_names_total] = caffe_forward_net(net, input, out_blob_names_extra)
%
% This file is part of the code that implements the following ICCV2015 accepted paper:
% title: "Object detection via a multi-region & semantic segmentation-aware CNN model"
% authors: Spyros Gidaris, Nikos Komodakis
% institution: Universite Paris Est, Ecole des Ponts ParisTech
% Technical report: http://arxiv.org/abs/1505.01749
% code: https://github.com/gidariss/mrcnn-object-detection
% c
%
% AUTORIGHTS
% --------------------------------------------------------
% Copyright (c) 2015 Spyros Gidaris
%
% "Object detection via a multi-region & semantic segmentation-aware CNN model"
% Technical report: http://arxiv.org/abs/1505.01749
% Licensed under The MIT License [see LICENSE for details]
% ---------------------------------------------------------

if ~exist('out_blob_names_extra', 'var'), out_blob_names_extra = {}; end
assert(iscell(out_blob_names_extra));

input_size  = caffe_get_blobs_size(net, net.inputs);
assert(numel(input_size) == numel(input));
size_in     = size(input{1});
num_feats   = size_in(end);

out_blob_names       = net.outputs;
num_out_blobs        = length(out_blob_names);
num_out_blobs_extra  = length(out_blob_names_extra);
num_out_blobs_total  = num_out_blobs + num_out_blobs_extra;
out_blob_names_total = [out_blob_names(:); out_blob_names_extra(:)];

output_size  = caffe_get_blobs_size(net, out_blob_names_total);
outputs      = cell(num_out_blobs_total,1);
out_feat_dim = zeros(num_out_blobs_total,1);

batch_size  = input_size{1}(4);
num_batches = ceil(num_feats/batch_size);

for i = 1:num_out_blobs_total
    out_feat_dim(i) = prod(output_size{i}(1:(end-1)));
    if output_size{i}(end) == batch_size
        outputs{i} = zeros(out_feat_dim(i), num_feats, 'single');
    else
        outputs{i} = zeros(out_feat_dim(i), output_size{i}(end) * num_batches, 'single');
    end
end

for i = 1:num_batches
    start_idx = (i-1) * batch_size + 1;
    stop_idx  = min(i * batch_size, num_feats);
    batch_size_this = stop_idx - start_idx + 1;

    % forward propagate batch of region images
    out = net.forward(prepare_batch(input, input_size, start_idx, stop_idx));
    for j = (num_out_blobs+1):num_out_blobs_total
        out{j} = net.blobs(out_blob_names_total{j}).get_data;
    end

    for j = 1:num_out_blobs_total
        out{j} = reshape(out{j}, [out_feat_dim(j), output_size{j}(end)]);
        if output_size{j}(end) == batch_size
            if (i == num_batches)
                out{j} = out{j}(:,1:batch_size_this);
            end
            outputs{j}(:,start_idx:stop_idx) = out{j};
        else
            start_idx0 = (i-1) * output_size{j}(end) + 1;
            stop_idx0  = i * output_size{j}(end);
            outputs{j}(:,start_idx0:stop_idx0) = out{j};
        end
    end
end

end

function batch = prepare_batch(input, input_size, start_idx, stop_idx)
assert(numel(input_size) == numel(input));
batch_size_this   = stop_idx - start_idx + 1;

batch = cell(1,numel(input_size));
for i = 1:numel(input_size)
    batch{i}          = zeros(input_size{i}, 'single');
    reshape_vector    = input_size{i};
    reshape_vector(4) = batch_size_this;
    batch{i}(:,:,:,1:batch_size_this) = reshape(input{i}(:,start_idx:stop_idx), reshape_vector);
end
end


function [DEM] = spm_ADEM(DEM)
% Dynamic expectation maximisation:  Active inversion
% FORMAT DEM   = spm_ADEM(DEM)
%
% DEM.G  - generative process
% DEM.M  - recognition  model
% DEM.C  - causes
% DEM.U  - prior expectation of causes
%__________________________________________________________________________
%
% This implementation of DEM is the same as spm_DEM but integrates both the
% generative process and model inversion in parallel. Its functionality is
% exactly the same apart from the fact that confounds are not accommodated
% explicitly.  The generative model is specified by DEM.G and the veridical
% causes by DEM.C; these may or may not be used as priors on the causes for
% the inversion model DEM.M (i.e., DEM.U = DEM.C).  Clearly, DEM.G does not
% require any priors or precision components; it will use the values of the
% parameters specified in the prior expectation fields.
%
% This routine is not used for model inversion per se but to simulate the
% dynamical inversion of models.  Critically, it includes action
% variables a - that couple the model back to the generative process
% This enables active inference (c.f., action-perception) or embodied
% inference.
%
% hierarchical models M(i)
%--------------------------------------------------------------------------
%   M(i).g  = y(t)  = g(x,v,P)    {inline function, string or m-file}
%   M(i).f  = dx/dt = f(x,v,P)    {inline function, string or m-file}
%
%   M(i).pE = prior expectation of p model-parameters
%   M(i).pC = prior covariances of p model-parameters
%   M(i).hE = prior expectation of h hyper-parameters (cause noise)
%   M(i).hC = prior covariances of h hyper-parameters (cause noise)
%   M(i).gE = prior expectation of g hyper-parameters (state noise)
%   M(i).gC = prior covariances of g hyper-parameters (state noise)
%   M(i).Q  = precision components (input noise)
%   M(i).R  = precision components (state noise)
%   M(i).V  = fixed precision (input noise)
%   M(i).W  = fixed precision (state noise)
%   M(i).xP = precision (states)
%
%   M(i).m  = number of inputs v(i + 1);
%   M(i).n  = number of states x(i)
%   M(i).l  = number of output v(i)
%   M(i).k  = number of action a(i)

% hierarchical process G(i)
%--------------------------------------------------------------------------
%   G(i).g  = y(t)  = g(x,v,a,P)    {inline function, string or m-file}
%   G(i).f  = dx/dt = f(x,v,a,P)    {inline function, string or m-file}
%
%   G(i).pE = model-parameters
%   G(i).U  = precision (action)
%   G(i).V  = precision (input noise)
%   G(i).W  = precision (state noise)
%
%   G(1).R  = restriction or rate matrix for action [default: 1];
%   G(i).aP = precision (action)   [default: exp(-2)]
%
%   G(i).m  = number of inputs v(i + 1);
%   G(i).n  = number of states x(i)
%   G(i).l  = number of output v(i)
%   G(i).k  = number of action a(i)
%
%
% Returns the following fields of DEM
%--------------------------------------------------------------------------
%
% true model-states - u
%--------------------------------------------------------------------------
%   pU.x    = true hidden states
%   pU.v    = true causal states v{1} = response (Y)
%   pU.C    = prior covariance: cov(v)
%   pU.S    = prior covariance: cov(x)
%
% model-parameters - p
%--------------------------------------------------------------------------
%   pP.P    = parameters for each level
%
% hyper-parameters (log-transformed) - h,g
%--------------------------------------------------------------------------
%   pH.h    = cause noise
%   pH.g    = state noise
%
% conditional moments of model-states - q(u)
%--------------------------------------------------------------------------
%   qU.a    = Action
%   qU.x    = Conditional expectation of hidden states
%   qU.v    = Conditional expectation of causal states
%   qU.z    = Conditional prediction errors (v)
%   qU.C    = Conditional covariance: cov(v)
%   qU.S    = Conditional covariance: cov(x)
%
% conditional moments of model-parameters - q(p)
%--------------------------------------------------------------------------
%   qP.P    = Conditional expectation
%   qP.C    = Conditional covariance
%
% conditional moments of hyper-parameters (log-transformed) - q(h)
%--------------------------------------------------------------------------
%   qH.h    = Conditional expectation (cause noise)
%   qH.g    = Conditional expectation (state noise)
%   qH.C    = Conditional covariance
%
% F         = log evidence = log marginal likelihood = negative free energy
%__________________________________________________________________________
%
% spm_ADEM implements a variational Bayes (VB) scheme under the Laplace
% approximation to the conditional densities of states (u), parameters (p)
% and hyperparameters (h) of any analytic nonlinear hierarchical dynamic
% model, with additive Gaussian innovations.  It comprises three
% variational steps (D,E and M) that update the conditional moments of u, p
% and h respectively
%
%                D: qu.u = max <L>q(p,h)
%                E: qp.p = max <L>q(u,h)
%                M: qh.h = max <L>q(u,p)
%
% where qu.u corresponds to the conditional expectation of hidden states x
% and causal states v and so on.  L is the ln p(y,u,p,h|M) under the model
% M. The conditional covariances obtain analytically from the curvature of
% L with respect to u, p and h.
%
% The D-step is embedded in the E-step because q(u) changes with each
% sequential observation.  The dynamical model is transformed into a static
% model using temporal derivatives at each time point.  Continuity of the
% conditional trajectories q(u,t) is assured by a continuous ascent of F(t)
% in generalised co-ordinates.  This means DEM can deconvolve online and
% represents an alternative to Kalman filtering or alternative Bayesian
% update procedures.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_ADEM.m 7679 2019-10-24 15:54:07Z spm $

% check model, data, priors and unpack
%--------------------------------------------------------------------------
DEM   = spm_ADEM_set(DEM);
M     = DEM.M;
G     = DEM.G;
C     = DEM.C;
U     = DEM.U;

% check whether to print
%--------------------------------------------------------------------------
try
    db = DEM.db;
catch
    db = 1;
end

% find or create a DEM figure
%--------------------------------------------------------------------------
if db
    Fdem = spm_figure('GetWin','DEM');
end

% ensure embedding dimensions are compatible
%--------------------------------------------------------------------------
G(1).E.n = M(1).E.n;
G(1).E.d = M(1).E.n;

% order parameters (d = n = 1 for static models) and checks
%==========================================================================
d    = M(1).E.d + 1;                      % embedding order of q(v)
n    = M(1).E.n + 1;                      % embedding order of q(x)
s    = M(1).E.s;                          % smoothness - s.d. (bins)

% number of states and parameters - generative model
%--------------------------------------------------------------------------
nY   = size(C,2);                         % number of samples
nl   = size(M,2);                         % number of levels
nv   = sum(spm_vec(M.m));                 % number of v (causal states)
nx   = sum(spm_vec(M.n));                 % number of x (hidden states)
ny   = M(1).l;                            % number of y (inputs)
nc   = M(end).l;                          % number of c (prior causes)
nu   = nv*d + nx*n;                       % number of generalised states

% number of states and parameters - generative process
%--------------------------------------------------------------------------
gr   = sum(spm_vec(G.l));                 % number of v (outputs)
ga   = sum(spm_vec(G.k));                 % number of a (active states)
gx   = sum(spm_vec(G.n));                 % number of x (hidden states)
gy   = G(1).l;                            % number of y (inputs)
na   = ga;                                % number of a (action)

% number of iterations
%--------------------------------------------------------------------------
try, nE = M(1).E.nE; catch, nE = 16; end
try, nM = M(1).E.nM; catch, nM = 8;  end
try, dt = M(1).E.dt; catch, dt = 1;  end

% initialise regularisation parameters
%--------------------------------------------------------------------------
te = 2;                                   % log integration time for E-Step
global t

% precision (roughness) of generalised fluctuations
%--------------------------------------------------------------------------
iV    = spm_DEM_R(n,s);
iG    = spm_DEM_R(n,s);

% time-delay operators (absorb motor delays into motor gain matrix)
%--------------------------------------------------------------------------
try
    nG = norm(iG);
    iG = iG*spm_DEM_T(n,-M(1).Ta);
    iG = iG*nG/norm(iG);
end
try
    Ty = spm_DEM_T(n,-M(1).Ty);
    Ty = kron(Ty,speye(ny,ny));
end

% precision components Q{} requiring [Re]ML estimators (M-Step)
%==========================================================================
Q     = {};
for i = 1:nl
    q0{i,i} = sparse(M(i).l,M(i).l); %#ok<AGROW>
    r0{i,i} = sparse(M(i).n,M(i).n);
end
Q0    = kron(iV,spm_cat(q0));
R0    = kron(iV,spm_cat(r0));
for i = 1:nl
    for j = 1:length(M(i).Q)
        q          = q0;
        q{i,i}     = M(i).Q{j};
        Q{end + 1} = blkdiag(kron(iV,spm_cat(q)),R0);
    end
    for j = 1:length(M(i).R)
        q          = r0;
        q{i,i}     = M(i).R{j};
        Q{end + 1} = blkdiag(Q0,kron(iV,spm_cat(q)));
    end
end

% and fixed components P
%--------------------------------------------------------------------------
Q0    = kron(iV,spm_cat(spm_diag({M.V})));
R0    = kron(iV,spm_cat(spm_diag({M.W})));
Qp    = blkdiag(Q0,R0);
nh    = length(Q);                           % number of hyperparameters
iR    = [zeros(1,ny),ones(1,nv),ones(1,nx)]; % for empirical priors
iR    = kron(speye(n,n),diag(iR));

% restriction or rate matrices - in terms of precision
%--------------------------------------------------------------------------
q0{1} = G(1).U;
Q0    = kron(iG,spm_cat(q0));
R0    = kron(iG,spm_cat(r0));
iG    = blkdiag(Q0,R0);

% restriction or rate matrices - in terms of dE/da
%--------------------------------------------------------------------------
try
    R         = sparse(sum(spm_vec(G.l)),na);
    R(1:ny,:) = G(1).R;
    R         = kron(spm_speye(n,1,0),R);
catch
    R = 1;
end

% fixed priors on action (a)
%--------------------------------------------------------------------------
try
    aP = G(1).aP;
catch
    aP = exp(-2);
end

% fixed priors on states (u)
%--------------------------------------------------------------------------
xP    = spm_cat(spm_diag({M.xP}));
Px    = kron(iV(1:n,1:n),speye(nx,nx)*exp(-8) + xP);
Pv    = kron(iV(1:d,1:d),speye(nv,nv)*exp(-8));
Pa    = spm_speye(na,na)*aP;
Pu    = spm_cat(spm_diag({Px Pv}));

% hyperpriors
%--------------------------------------------------------------------------
ph.h  = spm_vec({M.hE M.gE});             % prior expectation of h
ph.c  = spm_cat(spm_diag({M.hC M.gC}));   % prior covariances of h
qh.h  = ph.h;                             % conditional expectation
qh.c  = ph.c;                             % conditional covariance
ph.ic = spm_inv(ph.c);                    % prior precision

% priors on parameters (in reduced parameter space)
%==========================================================================
pp.c  = cell(nl,nl);
qp.p  = cell(nl,1);
for i = 1:(nl - 1)

    % eigenvector reduction: p <- pE + qp.u*qp.p
    %----------------------------------------------------------------------
    qp.u{i}   = spm_svd(M(i).pC);                    % basis for parameters
    M(i).p    = size(qp.u{i},2);                     % number of qp.p
    qp.p{i}   = sparse(M(i).p,1);                    % initial qp.p
    pp.c{i,i} = qp.u{i}'*M(i).pC*qp.u{i};            % prior covariance

    try
        qp.e{i} = qp.p{i} + qp.u{i}'*(spm_vec(M(i).P) - spm_vec(M(i).pE));
    catch
        qp.e{i} = qp.p{i};                           % initial qp.e
    end

end
Up    = spm_cat(spm_diag(qp.u));

% initialise and augment with confound parameters B; with flat priors
%--------------------------------------------------------------------------
np    = sum(spm_vec(M.p));                  % number of model parameters
pp.c  = spm_cat(pp.c);
pp.ic = spm_inv(pp.c);

% initialise conditional density q(p) (for D-Step)
%--------------------------------------------------------------------------
qp.e  = spm_vec(qp.e);
qp.c  = sparse(np,np);

% initialise cell arrays for D-Step; e{i + 1} = (d/dt)^i[e] = e[i]
%==========================================================================
qu.x      = cell(n,1);
qu.v      = cell(n,1);
qu.a      = cell(1,1);
qu.y      = cell(n,1);
qu.u      = cell(n,1);
pu.v      = cell(n,1);
pu.x      = cell(n,1);
pu.z      = cell(n,1);
pu.w      = cell(n,1);

[qu.x{:}] = deal(sparse(nx,1));
[qu.v{:}] = deal(sparse(nv,1));
[qu.a{:}] = deal(sparse(na,1));
[qu.y{:}] = deal(sparse(ny,1));
[qu.u{:}] = deal(sparse(nc,1));
[pu.v{:}] = deal(sparse(gr,1));
[pu.x{:}] = deal(sparse(gx,1));
[pu.z{:}] = deal(sparse(gr,1));
[pu.w{:}] = deal(sparse(gx,1));

% initialise cell arrays for hierarchical structure of x[0] and v[0]
%--------------------------------------------------------------------------
qu.x{1}   = spm_vec({M(1:end - 1).x});
qu.v{1}   = spm_vec({M(1 + 1:end).v});
qu.a{1}   = spm_vec({G.a});
pu.x{1}   = spm_vec({G.x});
pu.v{1}   = spm_vec({G.v});

% derivatives for Jacobian of D-step
%--------------------------------------------------------------------------
Dx    = kron(spm_speye(n,n,1),spm_speye(nx,nx,0));
Dv    = kron(spm_speye(d,d,1),spm_speye(nv,nv,0));
Dc    = kron(spm_speye(d,d,1),spm_speye(nc,nc,0));
Da    = kron(spm_speye(1,1,1),sparse(na,na));
Du    = spm_cat(spm_diag({Dx,Dv}));
Dq    = spm_cat(spm_diag({Dx,Dv,Dc,Da}));

Dx    = kron(spm_speye(n,n,1),spm_speye(gx,gx,0));
Dv    = kron(spm_speye(n,n,1),spm_speye(gr,gr,0));
Dp    = spm_cat(spm_diag({Dv,Dx,Dv,Dx}));
dfdw  = kron(speye(n,n),speye(gx,gx));
dydv  = kron(speye(n,n),speye(gy,gr));

% and null blocks
%--------------------------------------------------------------------------
dVdc  = sparse(d*nc,1);

% gradients and curvatures for conditional uncertainty
%--------------------------------------------------------------------------
dWdu  = sparse(nu,1);
dWduu = sparse(nu,nu);

% preclude unnecessary iterations
%--------------------------------------------------------------------------
if ~np && ~nh, nE = 1; end

% create innovations (and add causes)
%--------------------------------------------------------------------------
[z,w]  = spm_DEM_z(G,nY);
z{end} = C + z{end};
a      = {G.a};
Z      = spm_cat(z(:));
W      = spm_cat(w(:));
A      = spm_cat(a(:));

% Iterate DEM
%==========================================================================
F      = -Inf;
for iE = 1:nE

    % get time and clear persistent variables in evaluation routines
    %----------------------------------------------------------------------
    tic; clear spm_DEM_eval

    % E-Step: (with embedded D-Step)
    %======================================================================

    % [re-]set accumulators for E-Step
    %----------------------------------------------------------------------
    dFdp  = zeros(np,1);
    dFdpp = zeros(np,np);
    EE    = sparse(0);
    ECE   = sparse(0);
    EiSE  = sparse(0);
    qp.ic = sparse(0);
    Hqu.c = sparse(0);

    % [re-]set precisions using [hyper]parameter estimates
    %----------------------------------------------------------------------
    iS    = Qp;
    for i = 1:nh
       iS = iS + Q{i}*exp(qh.h(i));
    end

    % precision for empirical priors
    %----------------------------------------------------------------------
    iP    = iR*iS*iR;

    % [re-]set states & their derivatives
    %----------------------------------------------------------------------
    try
        qu = qU(1);
        pu = pU(1);
    end

    % D-Step: (nY samples)
    %======================================================================
    for iY = 1:nY

        % time (GLOBAL variable for non-automomous systems)
        %------------------------------------------------------------------
        t      = iY/nY;

        % pass action to pu.a (external states)
        %==================================================================
        try, A = spm_cat({qU.a qu.a}); end

        % derivatives of responses and random fluctuations
        %------------------------------------------------------------------
        pu.z = spm_DEM_embed(Z,n,iY);
        pu.w = spm_DEM_embed(W,n,iY);
        pu.a = spm_DEM_embed(A,n,iY);
        qu.u = spm_DEM_embed(U,n,iY);

        % evaluate generative process
        %------------------------------------------------------------------
        [pu,dg,df] = spm_ADEM_diff(G,pu);

        % and pass response to qu.y
        %==================================================================
        for i = 1:n
            y       = spm_unvec(pu.v{i},{G.v});
            qu.y{i} = y{1};
        end

        % sensory delays
        %------------------------------------------------------------------
        try, qu.y = spm_unvec(Ty*spm_vec(qu.y),qu.y); end

        % evaluate generative model
        %------------------------------------------------------------------
        [E,dE] = spm_DEM_eval(M,qu,qp);

        % conditional covariance [of states {u}]
        %------------------------------------------------------------------
        qu.c   = spm_inv(dE.du'*iS*dE.du + Pu);
        pu.c   = spm_inv(dE.du'*iP*dE.du + Pu);
        Hqu.c  = Hqu.c + spm_logdet(qu.c);

        % save at qu(t)
        %------------------------------------------------------------------
        qE{iY} = E;
        qC{iY} = qu.c;
        pC{iY} = pu.c;
        qU(iY) = qu;
        pU(iY) = pu;

        % and conditional precision
        %------------------------------------------------------------------
        if nh
            ECEu  = dE.du*qu.c*dE.du';
            ECEp  = dE.dp*qp.c*dE.dp';
        end

        % uncertainty about parameters dWdv, ... ; W = ln(|qp.c|)
        %==================================================================
        if np
            for i = 1:nu
                CJp(:,i)   = spm_vec(qp.c*dE.dpu{i}'*iS);
                dEdpu(:,i) = spm_vec(dE.dpu{i}');
            end
            dWdu  = CJp'*spm_vec(dE.dp');
            dWduu = CJp'*dEdpu;
        end

        % tensor products for Jacobian (generative process)
        %------------------------------------------------------------------
        Dgda  = kron(spm_speye(n,1,1),dg.da);
        Dgdv  = kron(spm_speye(n,n,1),dg.dv);
        Dgdx  = kron(spm_speye(n,n,1),dg.dx);
        dfda  = kron(spm_speye(n,1,0),df.da);
        dfdv  = kron(spm_speye(n,n,0),df.dv);
        dfdx  = kron(spm_speye(n,n,0),df.dx);

        dgda  = kron(spm_speye(n,1,0),dg.da);
        dgdx  = kron(spm_speye(n,n,0),dg.dx);

        % change in error w.r.t. action
        %------------------------------------------------------------------
        Dfdx  = 0;
        for i = 1:n
            Dfdx = Dfdx + kron(spm_speye(n,n,-i),df.dx^(i - 1));
        end

        % dE/da with restriction (R)
        %------------------------------------------------------------------
        dE.dv = dE.dy*dydv;
        dE.da = dE.dv*((dgda + dgdx*Dfdx*dfda).*R);

        % first-order derivatives
        %------------------------------------------------------------------
        dVdu  = -dE.du'*iS*E - Pu*spm_vec({qu.x{1:n} qu.v{1:d}}) - dWdu/2;
        dVda  = -dE.da'*iG*E - Pa*spm_vec( qu.a{1:1});

        % and second-order derivatives
        %------------------------------------------------------------------
        dVduu = -dE.du'*iS*dE.du - Pu - dWduu/2 ;
        dVdaa = -dE.da'*iG*dE.da - Pa;
        dVduv = -dE.du'*iS*dE.dv;
        dVduc = -dE.du'*iS*dE.dc;
        dVdua = -dE.du'*iS*dE.da;
        dVdav = -dE.da'*iG*dE.dv;
        dVdau = -dE.da'*iG*dE.du;
        dVdac = -dE.da'*iG*dE.dc;

        % D-step update: of causes v{i}, and hidden states x(i)
        %==================================================================

        % states and conditional modes
        %------------------------------------------------------------------
        p     = {pu.v{1:n} pu.x{1:n} pu.z{1:n} pu.w{1:n}};
        q     = {qu.x{1:n} qu.v{1:d} qu.u{1:d} qu.a{1:1}};
        u     = [p q];

        % gradient
        %------------------------------------------------------------------
        dFdu  = [                              Dp*spm_vec(p);
                 spm_vec({dVdu; dVdc; dVda}) + Dq*spm_vec(q)];

        % Jacobian (variational flow)
        %------------------------------------------------------------------
        dFduu = spm_cat(...
                {Dgdv  Dgdx Dv   []   []       []    Dgda;
                 dfdv  dfdx []   dfdw []       []    dfda;
                 []    []   Dv   []   []       []    [];
                 []    []   []   Dx   []       []    [];
                 dVduv []   []   []   Du+dVduu dVduc dVdua;
                 []    []   []   []   []       Dc    []
                 dVdav []   []   []   dVdau    dVdac dVdaa});

        % update states q = {x,v,z,w} and conditional modes
        %==================================================================
        du    = spm_dx(dFduu,dFdu,dt);
        u     = spm_unvec(spm_vec(u) + du,u);

        % and save them
        %------------------------------------------------------------------
        pu.v(1:n) = u((1:n));
        pu.x(1:n) = u((1:n) + n);
        qu.x(1:n) = u((1:n) + n + n + n + n);
        qu.v(1:d) = u((1:d) + n + n + n + n + n);
        qu.a(1:1) = u((1:1) + n + n + n + n + n + d + d);

        % Gradients and curvatures for E-Step: W = tr(C*J'*iS*J)
        %==================================================================
        if np
            for i = 1:np
                CJu(:,i)   = spm_vec(qu.c*dE.dup{i}'*iS);
                dEdup(:,i) = spm_vec(dE.dup{i}');
            end
            dWdp  = CJu'*spm_vec(dE.du');
            dWdpp = CJu'*dEdup;

            % Accumulate; dF/dP = <dL/dp>, dF/dpp = ...
            %--------------------------------------------------------------
            dFdp  = dFdp  - dWdp/2  - dE.dp'*iS*E;
            dFdpp = dFdpp - dWdpp/2 - dE.dp'*iS*dE.dp;
            qp.ic = qp.ic           + dE.dp'*iS*dE.dp;

        end

        % accumulate SSE
        %------------------------------------------------------------------
        EiSE = EiSE + E'*iS*E;

        % and quantities for M-Step
        %------------------------------------------------------------------
        if nh
            EE  = E*E'+ EE;
            ECE = ECE + ECEu + ECEp;
        end

        if nE == 1

            % evaluate objective function (F)
            %======================================================================
            J(iY) = - trace(E'*iS*E)/2  ...            % states (u)
                    + spm_logdet(qu.c)  ...            % entropy q(u)
                    + spm_logdet(iS)/2;                % entropy - error
        end

    end % sequence (nY)

    % augment with priors
    %----------------------------------------------------------------------
    dFdp   = dFdp  - pp.ic*qp.e;
    dFdpp  = dFdpp - pp.ic;
    qp.ic  = qp.ic + pp.ic;
    qp.c   = spm_inv(qp.ic);

    % E-step: update expectation (p)
    %======================================================================

    % update conditional expectation
    %----------------------------------------------------------------------
    dp     = spm_dx(dFdpp,dFdp,{te});
    qp.e   = qp.e + dp;
    qp.p   = spm_unvec(qp.e,qp.p);

    % M-step - hyperparameters (h = exp(l))
    %======================================================================
    mh     = zeros(nh,1);
    dFdh   = zeros(nh,1);
    dFdhh  = zeros(nh,nh);
    for iM = 1:nM

        % [re-]set precisions using [hyper]parameter estimates
        %------------------------------------------------------------------
        iS    = Qp;
        for i = 1:nh
            iS = iS + Q{i}*exp(qh.h(i));
        end
        S     = spm_inv(iS);
        dS    = ECE + EE - S*nY;

        % 1st-order derivatives: dFdh = dF/dh
        %------------------------------------------------------------------
        for i = 1:nh
            dPdh{i}        =  Q{i}*exp(qh.h(i));
            dFdh(i,1)      = -trace(dPdh{i}*dS)/2;
        end

        % 2nd-order derivatives: dFdhh
        %------------------------------------------------------------------
        for i = 1:nh
            for j = 1:nh
                dFdhh(i,j) = -trace(dPdh{i}*S*dPdh{j}*S*nY)/2;
            end
        end

        % hyperpriors
        %------------------------------------------------------------------
        qh.e  = qh.h  - ph.h;
        dFdh  = dFdh  - ph.ic*qh.e;
        dFdhh = dFdhh - ph.ic;

        % update ReML estimate of parameters
        %------------------------------------------------------------------
        dh    = spm_dx(dFdhh,dFdh);
        qh.h  = qh.h + dh;
        mh    = mh   + dh;

        % conditional covariance of hyperparameters
        %------------------------------------------------------------------
        qh.c  = -spm_inv(dFdhh);

        % convergence (M-Step)
        %------------------------------------------------------------------
        if (dFdh'*dh < 1e-2) || (norm(dh,1) < exp(-8)), break, end

    end % M-Step

    % evaluate objective function (F)
    %======================================================================
    L   = - trace(EiSE)/2  ...               % states (u)
        - trace(qp.e'*pp.ic*qp.e)/2  ...     % parameters (p)
        - trace(qh.e'*ph.ic*qh.e)/2  ...     % hyperparameters (h)
        + Hqu.c/2             ...            % entropy q(u)
        + spm_logdet(qp.c)/2  ...            % entropy q(p)
        + spm_logdet(qh.c)/2  ...            % entropy q(h)
        - spm_logdet(pp.c)/2  ...            % entropy - prior p
        - spm_logdet(ph.c)/2  ...            % entropy - prior h
        + spm_logdet(iS)*nY/2 ...            % entropy - error
        - n*ny*nY*log(2*pi)/2;

    % if F is increasing, save expansion point and derivatives
    %----------------------------------------------------------------------
    if L > F(end) || iE < 3

        % save model-states (for each time point)
        %==================================================================
        for t = 1:length(qU)

            % states
            %--------------------------------------------------------------
            a     = spm_unvec(qU(t).a{1},{G.a});
            v     = spm_unvec(pU(t).v{1},{G.v});
            x     = spm_unvec(pU(t).x{1},{G.x});
            z     = spm_unvec(pU(t).z{1},{G.v});
            w     = spm_unvec(pU(t).w{1},{G.x});
            for i = 1:nl
                try
                    PU.v{i}(:,t) = spm_vec(v{i});
                    PU.z{i}(:,t) = spm_vec(z{i});
                end
                try
                    PU.x{i}(:,t) = spm_vec(x{i});
                    PU.w{i}(:,t) = spm_vec(w{i});
                end
                try
                    QU.a{i}(:,t) = spm_vec(a{i});
                end
            end

            % conditional modes
            %--------------------------------------------------------------
            v     = spm_unvec(qU(t).v{1},{M(1 + 1:end).v});
            x     = spm_unvec(qU(t).x{1},{M(1:end - 1).x});
            z     = spm_unvec(qE{t}(1:(ny + nv)),{M.v});
            w     = spm_unvec(qE{t}((1:nx) + (ny + nv)*n),{M.x});
            for i = 1:(nl - 1)
                if M(i).m, QU.v{i + 1}(:,t) = spm_vec(v{i}); end
                if M(i).l, QU.z{i}(:,t)     = spm_vec(z{i}); end
                if M(i).n, QU.x{i}(:,t)     = spm_vec(x{i}); end
                if M(i).n, QU.w{i}(:,t)     = spm_vec(w{i}); end
            end
            QU.v{1}(:,t)  = spm_vec(qU(t).y{1}) - spm_vec(z{1});
            QU.z{nl}(:,t) = spm_vec(z{nl});

            % and conditional covariances
            %--------------------------------------------------------------
            i       = (1:nx);
            QU.S{t} = qC{t}(i,i);
            PU.S{t} = pC{t}(i,i);
            i       = (1:nv) + nx*n;
            QU.C{t} = qC{t}(i,i);
            PU.C{t} = pC{t}(i,i);
        end

        % save conditional densities
        %------------------------------------------------------------------
        B.QU  = QU;
        B.PU  = PU;
        B.qp  = qp;
        B.qh  = qh;

        % decrease regularisation
        %------------------------------------------------------------------
        F(iE) = L;
        te    = min(te + 1,8);

    else

        % otherwise, return to previous expansion point and break
        %------------------------------------------------------------------
        QU    = B.QU;
        PU    = B.PU;
        qp    = B.qp;
        qh    = B.qh;

        % increase regularisation
        %------------------------------------------------------------------
        F(iE) = F(end);
        te    = min(te - 1,0);

    end

    % report and break if convergence
    %======================================================================
    if db
        figure(Fdem)
        spm_DEM_qU(QU)
        if np
            subplot(nl,4,4*nl)
            bar(full(Up*qp.e))
            xlabel({'parameters';'{minus prior}'})
            axis square, grid on
        end
        if length(F) > 2
            subplot(nl,4,4*nl - 1)
            plot(F - F(1))
            xlabel('updates')
            title('log-evidence')
            axis square, grid on
        end
        drawnow

        % report (EM-Steps)
        %------------------------------------------------------------------
        str{1} = sprintf('ADEM: %i (%i)',iE,iM);
        str{2} = sprintf('F:%.4e',full(L - F(1)));
        str{3} = sprintf('p:%.2e',full(dp'*dp));
        str{4} = sprintf('h:%.2e',full(mh'*mh));
        str{5} = sprintf('(%.2e sec)',full(toc));

        fprintf('%-16s%-16s%-14s%-14s%-16s\n',str{:})
    end

    if (norm(dp,1) < exp(-8)) && (norm(mh,1) < exp(-8)), break, end

end

% assemble output arguments
%==========================================================================

% conditional moments of model-parameters (rotated into original space)
%--------------------------------------------------------------------------
qP.P   = spm_unvec(Up*qp.e + spm_vec(M.pE),M.pE);
qP.C   = Up*qp.c*Up';
qP.V   = spm_unvec(diag(qP.C),M.pE);

% conditional moments of hyper-parameters (log-transformed)
%--------------------------------------------------------------------------
qH.h   = spm_unvec(qh.h,{{M.hE} {M.gE}});
qH.g   = qH.h{2};
qH.h   = qH.h{1};
qH.C   = qh.c;
qH.V   = spm_unvec(diag(qH.C),{{M.hE} {M.gE}});
qH.W   = qH.V{2};
qH.V   = qH.V{1};

% Fill in DEM with response and its causes
%--------------------------------------------------------------------------
DEM.pP.P = {G.pE};            % parameters encoding process

DEM.M  = M;                   % generative model
DEM.U  = U;                   % causes
DEM.Y  = PU.v{1};             % response

DEM.pU = PU;                  % prior moments of model-states
DEM.qU = QU;                  % conditional moments of model-states
DEM.qP = qP;                  % conditional moments of model-parameters
DEM.qH = qH;                  % conditional moments of hyper-parameters

DEM.F  = F;                   % [-ve] Free energy
try
    DEM.J  = J;               % [-ve] Free energy (over samples)
end


function vectorRgb = dtiTrackAllFibers(samplingRateMm, dataDirectory, faThresh, faTrackThresh, exclusionRoi)
%
% Track all fibers for a particular subject. These are stored in one
% massive data file in the specified path.
%
%   dtiTrackAllFibers([samplingRateMm=2], [dataDirectory],
%   [faMaskThresh=0.25], [faTrackThresh=0.15], [exclusionRoi=[]], [hemisphere='both'])
%
%  samplingRateMm: the grid resolution for seed points, in millimeters.
%  Default is 2 mm.
%
%  dataDirectory: the path to the directory containing the tensors.nii.gz
%  file. If unspecified, a directory selection dialog will pop up.
%
%
% Author: DA

if(~exist('samplingRateMm','var')||isempty(samplingRateMm))
    samplingRateMm = 2;
end;
if(~exist('dataDirectory','var') || isempty(dataDirectory))
    dataDirectory = uigetdir('', 'Set subject bin directory');
    if(isnumeric(dataDirectory)), disp('dtiTrackAllFibers canceled.'); return; end
end;
if(~exist('faThresh','var')||isempty(faThresh))
    faThresh = 0.25;
end
if(~exist('faTrackThresh','var')||isempty(faTrackThresh))
    faTrackThresh = 0.15;
end
if(~exist('exclusionRoi','var'))
    exclusionRoi = [];
end
if(~isempty(exclusionRoi))
  if(~isstruct(exclusionRoi))
	exclusionRoi = dtiReadRoi(exclusionRoi);
  end
end

disp ('Loading tensors...');

opts.stepSizeMm = 1;
opts.faThresh = faTrackThresh;
opts.lengthThreshMm = 30;
opts.angleThresh = 50;
opts.wPuncture = 0.2;
opts.whichAlgorithm = 1;
opts.whichInterp = 1;
opts.seedVoxelOffsets = [0];

dt6 = niftiRead(fullfile(dataDirectory,'tensors.nii.gz'));
% check for the new NIFTI-compliant 5d format
if(dt6.ndim>4)
  % Convert from the 5d, lower-tri row order NIFTI tensor format
  % (Dxx Dxy Dyy Dxz Dyz Dzz) to our 4d tensor format (Dxx Dyy Dzz Dxy Dxz Dyz).
  dt6.data = double(squeeze(dt6.data(:,:,:,1,[1 3 6 2 4 5])));
end
if(exist(fullfile(dataDirectory,'brainMask.nii.gz')))
  mask = niftiRead(fullfile(dataDirectory,'brainMask.nii.gz'));
  for(ii=1:6)
	tmp = dt6.data(:,:,:,ii);
	tmp(~mask.data) = 0;
	dt6.data(:,:,:,ii) = tmp;
  end
  clear tmp mask;
end

% Create WM ROIs
[vec,val] = dtiEig(dt6.data);
fa = dtiComputeFA(val);
fa(fa>1) = 1; fa(fa<0) = 0;
if(nargout>0)
  img = abs(squeeze(vec(:,:,:,[1 2 3],1)));
  img(isnan(img)|img<0) = 0;
  img(img>1) = 1;
  for(ii=1:3) img(:,:,:,ii) = img(:,:,:,ii).*fa; end
  vectorRgb = niftiGetStruct(single(img),dt6.qto_xyz);
  clear img;
end
clear vec val;

samplingRateVox = samplingRateMm./dt6.pixdim;

% first create an enormous array of seeds and figure out which ones fall
% within the masked region:

xSeed = 1:samplingRateVox(1):size(dt6.data,1);
ySeed = 1:samplingRateVox(2):size(dt6.data,2);
zSeed = 1:samplingRateVox(3):size(dt6.data,3);

mask = fa>=faThresh;
midline = mrAnatXformCoords(dt6.qto_ijk,[0 0 0]);

[xIndex, yIndex, zIndex] = ind2sub ([size(xSeed,2), size(ySeed,2), size(zSeed,2)], ...
                                    find (mask (round(xSeed), round(ySeed), round(zSeed))));

pathsFilename = fullfile(dataDirectory, sprintf('all_paths_%dmm.pdb',samplingRateMm));

dtiWritePDBHeader (dt6.qto_xyz, pathsFilename);

fileOffsets = [];

% track in index blocks of width XX:
xStepSize = 10;
nFibers = 0;
for xIter = 1:xStepSize:size(xIndex,1)
    xRange = find (xIndex >= xIter & xIndex < xIter+xStepSize);
    seeds = [xSeed(xIndex(xRange)); ySeed(yIndex(xRange)); zSeed(zIndex(xRange))]';
    acpcSeeds = mrAnatXformCoords(dt6.qto_xyz, seeds);
    seedIndices = [xIndex(xRange), yIndex(xRange), zIndex(xRange)];
    if (isempty(seeds))
        continue;
    end;
    [fg, opts, kept] = dtiFiberTrackWrapper(dt6.data, seeds, dt6.pixdim(1:3), dt6.qto_xyz, ['FG'], opts);
    if (size(fg.fibers,1) == 0)
        continue;
    end;
    if(~isempty(exclusionRoi))
       fg = dtiIntersectFibersWithRoi([], {'not'}, [], exclusionRoi, fg);
    end
    nFibers = nFibers + length(fg.fibers);
    newOffsets = dtiAppendPathwaysToPDB (fg, pathsFilename);
    fileOffsets = [fileOffsets newOffsets];
    clear fg;
end

dtiAppendFileOffsetsToPDB (fileOffsets, pathsFilename);

return;


function select_segments_on_base_and_followup_trees(baseCerrFile,baseTreeFile,...
    followupTreeFile,vfFile,segmentsFile)
% function  select_segments_on_base_and_followup_trees(baseCerrFile,baseTreeFile,...
%     followupTreeFile,vfFile)
%
% Example
%
% APA, 5/4/2021

global airwayStateS stateS
% airwayStateS.baseAddNodes = 0;
% airwayStateS.baseShowInCerrViewer = 1;
% airwayStateS.baseRemoveNodes = 0;
% airwayStateS.followupAddNodes = 0;
% airwayStateS.followupShowInCerrViewer = 1;
% airwayStateS.followupRemoveNodes = 0;
% airwayStateS.baseSegmentStartNodes = [];
% airwayStateS.baseSegmentEndNodes = [];
% airwayStateS.basehStartNodes = [];
% airwayStateS.basehEndNodes = [];
% airwayStateS.followupSegmentStartNodes = [];
% airwayStateS.followupSegmentEndNodes = [];
% airwayStateS.followuphStartNodes = [];
% airwayStateS.followuphEndNodes = [];
% airwayStateS.hBaseSegment = [];
% airwayStateS.hFollowupSegment = [];
airwayStateS.selectBaseStartNode = 1;
airwayStateS.selectFollowupStartNode = 1;
% airwayStateS.hBaseStart
% airwayStateS.hBaseEnd
% airwayStateS.hFollowupStart
% airwayStateS.hFollowupEnd

% Create crosshairs
numAxis = length(stateS.handle.CERRAxis);
for i = 1:numAxis
    hCrosshairV(i) = plot(0,0, ...
        'parent', stateS.handle.CERRAxis(i), 'Marker','+', 'MarkerSize',10,...
        'color', [1 1 0], 'hittest', 'off','linewidth',3,'visible','off');
end

airwayStateS.hCrosshairV = hCrosshairV;

% Load base scan with dose
planC = loadPlanC(baseCerrFile,tempdir);
planC = updatePlanFields(planC);
planC = quality_assure_planC(baseCerrFile,planC);

% Get grid for vf
scanNum = 1;
indexS = planC{end};
[xUnifV,yUnifV,zUnifV] = getUniformScanXYZVals(planC{indexS.scan}(scanNum));
xFieldV = [xUnifV(1),xUnifV(2)-xUnifV(1),xUnifV(end)];
yFieldV = [yUnifV(end),yUnifV(1)-yUnifV(2),yUnifV(1)];

% % Load base tree
% load(baseTreeFile)
% elemDistBaseV = elemDistV;
% elemAreaBaseV = elemAreaV;
% elemBaseM = elemM;
% nodeXyzBaseM = nodeXyzM;

% Load base tree
load(baseTreeFile)
minDistBaseV = minDistV;
elemBaseM = elemM;
nodeXyzBaseM = nodeXyzM;
centerTreeXyzBaseM = centerTreeXyzM;
% nodeDistFromCarinaBaseV = nodeDistFromCarinaV;
treeS.nodeXyzM = nodeXyzM;
treeS.elemM = elemM;
xyzStartM = [nodeXyzM(elemM(:,1),1),nodeXyzM(elemM(:,1),2),nodeXyzM(elemM(:,1),3)];
xyzEndM = [nodeXyzM(elemM(:,2),1),nodeXyzM(elemM(:,2),2),nodeXyzM(elemM(:,2),3)];
elemDistV = sum((xyzStartM - xyzEndM).^2,2).^0.5;
airwayGraph = graph(elemM(:,1),elemM(:,2),elemDistV);
treeS.airwayGraph = airwayGraph;
treeS.minDistV = minDistV;
airwayStateS.baseTreeS = treeS;

% Load followup tree
load(followupTreeFile)
minDistFollowupV = minDistV;
elemFollowupM = elemM;
nodeXyzFollowupM = nodeXyzM;
nodeXyzFollowupSmoothM = centerTreeXyzM;
treeS.nodeXyzM = nodeXyzM;
treeS.elemM = elemM;
xyzStartM = [nodeXyzM(elemM(:,1),1),nodeXyzM(elemM(:,1),2),nodeXyzM(elemM(:,1),3)];
xyzEndM = [nodeXyzM(elemM(:,2),1),nodeXyzM(elemM(:,2),2),nodeXyzM(elemM(:,2),3)];
elemDistV = sum((xyzStartM - xyzEndM).^2,2).^0.5;
airwayGraph = graph(elemM(:,1),elemM(:,2),elemDistV);
treeS.airwayGraph = airwayGraph;
% Smooth radius calculation
minDistBaseV = smoothRadius(nodeXyzBaseM,centerTreeXyzBaseM,minDistBaseV);
minDistFollowupV = smoothRadius(nodeXyzFollowupM,nodeXyzFollowupM,minDistFollowupV);
treeS.minDistV = minDistFollowupV;
airwayStateS.followupTreeS = treeS;

% Get deformation at nodes
load(vfFile)

% Get location of base points on followup scan
xDeformV = finterp3(nodeXyzBaseM(:,1),nodeXyzBaseM(:,2),nodeXyzBaseM(:,3),...
    flip(vf(:,:,:,1),1),xFieldV,yFieldV,zUnifV);
yDeformV = finterp3(nodeXyzBaseM(:,1),nodeXyzBaseM(:,2),nodeXyzBaseM(:,3),...
    flip(vf(:,:,:,2),1),xFieldV,yFieldV,zUnifV);
zDeformV = finterp3(nodeXyzBaseM(:,1),nodeXyzBaseM(:,2),nodeXyzBaseM(:,3),...
    flip(vf(:,:,:,3),1),xFieldV,yFieldV,zUnifV);

nodeXyzInterpM = nodeXyzBaseM + [xDeformV(:), yDeformV(:), zDeformV(:)];

airwayStateS.nodeXyzInterpM = nodeXyzInterpM;

% Calculate radius change
distTolerance = 1; %cm
radiusDiffV = calculateRadiusChange(nodeXyzBaseM,nodeXyzFollowupM,...
    minDistBaseV,minDistFollowupV,distTolerance,...
    vf,xFieldV,yFieldV,zUnifV);

% Calculate dose
doseNum = 1;
% nodeDoseV = getDoseAt(doseNum, centerTreeXyzBaseM(:,1), centerTreeXyzBaseM(:,2),...
%     centerTreeXyzBaseM(:,3), planC);
nodeOrigDoseV = getDoseAt(doseNum, nodeXyzBaseM(:,1), nodeXyzBaseM(:,2),...
    nodeXyzBaseM(:,3), planC);

airwayStateS.baseTreeS.nodeOrigDoseV = nodeOrigDoseV;

% Initialize figure to show airway trees
hFig = figure('CloseRequestFcn','airwaySegmentCallback(''closeRequest'')',...
    'numbertitle','off');
% baseAxis = subplot(1,2,1);
% followupAxis = subplot(1,2,2);
% set(baseAxis,'NextPlot','add')
% set(followupAxis,'NextPlot','add')

baseAxis = axes('parent', hFig, 'units', 'normalized','NextPlot','add', ...
    'position', [0.2 0.1 0.38 0.8], 'color', [1,1,1]);
followupAxis = axes('parent', hFig, 'units', 'normalized','NextPlot','add', ...
    'position', [0.6 0.1 0.38 0.8], 'color', [1,1,1]);
hSegList = uicontrol(hFig,'units','normalized','Position',[0.02 0.55, 0.15, 0.4],...
    'Style','list','callback','airwaySegmentCallback(''SEGMENT_CLICKED'')');

hSegAdd = uicontrol(hFig,'units','normalized','Position',[0.02 0.5, 0.03, 0.05],...
    'Style','push','String','+','callback','airwaySegmentCallback(''ADD_SEGMENT'')');
hSegRemove = uicontrol(hFig,'units','normalized','Position',[0.06 0.5, 0.03, 0.05],...
    'Style','push','String','-','callback','airwaySegmentCallback(''REMOVE_SEGMENT'')');
hSegRename = uicontrol(hFig,'units','normalized','Position',[0.1 0.5, 0.07, 0.05],...
    'Style','push','String','Name','callback','airwaySegmentCallback(''RENAME_SEGMENT'')');

hLoadSegments = uicontrol(hFig,'units','normalized','Position',[0.02 0.4, 0.15, 0.05],...
    'Style','push','String','Load','callback','airwaySegmentCallback(''LOAD_SEGMENTS'')');
hSaveSegments = uicontrol(hFig,'units','normalized','Position',[0.02 0.33, 0.15, 0.05],...
    'Style','push','String','Save','callback','airwaySegmentCallback(''SAVE_SEGMENTS'')');
hSegViewer = uicontrol(hFig,'units','normalized','Position',[0.02 0.26, 0.15, 0.05],...
    'Style','push','String','Viewer','callback','airwaySegmentCallback(''SHOW_IN_CERR_VIEWER'')');
hSelectSegNodes = uicontrol(hFig,'units','normalized','Position',[0.02 0.19, 0.15, 0.05],...
    'Style','push','String','Select seg nodes','callback','airwaySegmentCallback(''SELECT_SEGMENT_NODES'')');
hLockBaseTreeToggle = uicontrol(hFig,'units','normalized','Position',[0.02 0.12, 0.15, 0.05],...
    'Style','toggle','String','Lock base tree','callback','airwaySegmentCallback(''LOCK_BASE_TREE'')');
airwayStateS.hAxisBase = baseAxis;
airwayStateS.hAxisFollowup = followupAxis;
airwayStateS.hSegList = hSegList;
airwayStateS.hSegAdd = hSegAdd;
airwayStateS.hSegRemove = hSegRemove;
airwayStateS.hSegRename = hSegRename;
airwayStateS.hSegViewer = hSegViewer;
airwayStateS.hLoadSegments = hLoadSegments;
airwayStateS.hSaveSegments = hSaveSegments;
airwayStateS.hSelectSegNodes = hSelectSegNodes;
airwayStateS.hLockBaseTreeToggle = hLockBaseTreeToggle;
airwayStateS.viewerMode = 1;
airwayStateS.segSelectMode = 0;
segS = struct('segName','segment','nodeXyzM',[],...
    'startNode',[],'endNode',[],'allNodes',[],...
    'hPlot',[],'assocScanUID','');
airwayStateS.followupSegmentS = segS;
airwayStateS.followupSegmentS(:) = [];
airwayStateS.baseSegmentS = segS;
airwayStateS.baseSegmentS(:) = [];
airwayStateS.totalSegments = 0;
airwayStateS.currentSegment = 0;

basePt = plot3(baseAxis,0,0,0,'marker','o','markerSize',12,...
    'MarkerEdgeColor',[0,0,0],'visible','off','hittest','off');
airwayStateS.hBaseStart = plot3(baseAxis,0,0,0,'marker','o','markerSize',12,...
    'MarkerEdgeColor','g','visible','off','color','g','hittest','off');
airwayStateS.hBaseEnd = plot3(baseAxis,0,0,0,'marker','o','markerSize',12,...
    'MarkerEdgeColor','r','visible','off','color','r','hittest','off');

set(baseAxis,'zDir','reverse')
hPlotBase = scatter3(baseAxis,nodeXyzBaseM(:,1),nodeXyzBaseM(:,2),nodeXyzBaseM(:,3),...
'sizeData',12,'MarkerEdgeColor','None',...
'MarkerFaceColor','flat','MarkerFaceAlpha',0.5,'CData',[0.5,0.5,0.5]);
airwayStateS.hPlotBase = hPlotBase;
airwayStateS.basePt = basePt;
%set(hp,'ButtonDownFcn',{buttonDownFcn,varargin{:},nodeValV});
axis(baseAxis,'equal')
view(baseAxis,3)
xlabel(baseAxis,'R - L','fontsize',10)
ylabel(baseAxis,'A - P','fontsize',10)
zlabel(baseAxis,'I - S','fontsize',10)
grid(baseAxis,'on')
title(baseAxis,'Baseline')

% Load followup tree
% load(followupTreeFile)
% baseFig = figure;
% hold on,
% baseAxis = gca;
followupPt = plot3(followupAxis,0,0,0,'marker','o','markerSize',12,...
    'MarkerEdgeColor',[0,0,0],'visible','off','hittest','off');
airwayStateS.hFollowupStart = plot3(followupAxis,0,0,0,'marker','o','markerSize',12,...
    'MarkerEdgeColor','g','visible','off','color','g','hittest','off');
airwayStateS.hFollowupEnd = plot3(followupAxis,0,0,0,'marker','o','markerSize',12,...
    'MarkerEdgeColor','r','visible','off','color','r','hittest','off');
airwayStateS.followupPt = followupPt;
set(followupAxis,'zDir','reverse')
hPlotFollowup = scatter3(followupAxis,nodeXyzFollowupM(:,1),...
    nodeXyzFollowupM(:,2),nodeXyzFollowupM(:,3),...
'sizeData',12,'MarkerEdgeColor','None',...
'MarkerFaceColor','flat','MarkerFaceAlpha',0.5,'CData',[0.5,0.5,0.5]);
airwayStateS.hPlotFollowup = hPlotFollowup;

axis(followupAxis,'equal')
view(followupAxis,3)
xlabel(followupAxis,'R - L','fontsize',10)
ylabel(followupAxis,'A - P','fontsize',10)
zlabel(followupAxis,'I - S','fontsize',10)
grid(followupAxis,'on')
title(followupAxis,'Followup')

airwaySegmentCallback('SHOW_IN_CERR_VIEWER')

if exist('segmentsFile','var') && ~isempty(segmentsFile)
    airwaySegmentCallback('LOAD_SEGMENTS',segmentsFile);
end

% % Callback to add nodes
% buttonDownFcn = @pickStatrStopNodes;
% set(hPlotFollowup,'ButtonDownFcn',{buttonDownFcn,followupAxis});

% % Callback to show nodes in Viewer
% buttonDownFcn = @showNodeInCerrViewer;
% set(hPlotBase,'ButtonDownFcn',{buttonDownFcn,vf,xFieldV,yFieldV,zUnifV,...
%     basePt,followupPt})

% % Callback to remove nodes
% buttonDownFcn = @removeStatrStopNodes;
% set(hPlotFollowup,'ButtonDownFcn',{buttonDownFcn,followupAxis});

% Set buttondown function

%% Plots

% Create contextmenu
% baseMenu = uicontextmenu('Callback', 'airwayAxisMenu(''init_start_stop_view_nodes'')',...
%     'userdata', {'base',baseAxis,hPlotBase,basePt,vf,xFieldV,yFieldV,zUnifV,followupPt,...
%     minDistBaseV, radiusDiffV, nodeOrigDoseV}, 'parent', hFig);
baseMenu = uicontextmenu('Callback', 'airwayAxisMenu(''init_start_stop_view_nodes'')',...
    'userdata', {'base',baseAxis,hPlotBase,basePt,nodeXyzInterpM,followupPt,...
    minDistBaseV, radiusDiffV, nodeOrigDoseV}, 'parent', hFig);
% followupMenu = uicontextmenu('Callback', 'airwayAxisMenu(''init_start_stop_view_nodes'')',...
%     'userdata', {'followup',followupAxis,hPlotFollowup,followupPt}, 'parent', hFig);
set(baseAxis, 'UIContextMenu', baseMenu);
%set(followupAxis, 'UIContextMenu', followupMenu);

% buttonDownFcn = @showNodeInCerrViewer;
% set(hPlotBase,'ButtonDownFcn',{buttonDownFcn,'base',basePt,nodeXyzInterpM,...
%    followupPt})
% set(hPlotFollowup,'ButtonDownFcn',{buttonDownFcn,'followup',followupPt})

% followupMenu = uicontextmenu('Callback', 'airwayAxisMenu(''choose_start_stop'')',...
%     'userdata', {}, 'parent', followupFig);
% set(baseAxis, 'UIContextMenu', followupMenu);

% m1 = uimenu(baseCtxMenu,'Text','Airway Radius');
% m2 = uimenu(baseCtxMenu,'Text','RT Dose');
% m3 = uimenu(baseCtxMenu,'Text','Change in Airway Radius');

% % Plot elements color-coded by opening radius
% radiusDiffV = (elemDistV(minIndV)-elemDistBaseV)./elemDistBaseV*100;
% radiusDiffV(minDistV > 0.5) = NaN;
% minDist = min(radiusDiffV);
% maxDist = max(radiusDiffV);
% maxMinusMinDist = maxDist - minDist;
% cmapM = CERRColorMap('jetmod');
% cmapSiz = size(cmapM,1)-1;
% cmapIndV = round((radiusDiffV - minDist) / maxMinusMinDist * cmapSiz) + 1;
%
% xyzStartM = [nodeXyzBaseM(elemBaseM(:,1),1),nodeXyzBaseM(elemBaseM(:,1),2),nodeXyzBaseM(elemBaseM(:,1),3)];
% xyzEndM = [nodeXyzBaseM(elemBaseM(:,2),1),nodeXyzBaseM(elemBaseM(:,2),2),nodeXyzBaseM(elemBaseM(:,2),3)];
%
% figure ,hold on,
% set(gca,'zDir','reverse')
% for i = 1:size(xyzStartM,1)
%     if ~isnan(elemDistV(minIndV(i))) && ~isnan(elemDistBaseV(i)) && ~isnan(cmapIndV(i))
%         xV = [xyzStartM(i,1), xyzEndM(i,1)];
%         yV = [xyzStartM(i,2), xyzEndM(i,2)];
%         zV = [xyzStartM(i,3), xyzEndM(i,3)];
%         plot3(xV,yV,zV,'color',[cmapM(cmapIndV(i),:),1],...
%             'linewidth',3,'ButtonDownFcn',@showNodeOnFollowupTree); %@gotoNodeInCerrViewer
%         %plot3(xV,yV,zV,'b.')
%     end
% end
% set(gca,'cLim',[minDist,maxDist])
% colormap(cmapM)
% ticksV = linspace(minDist,maxDist,10);
% tickC = {};
% for i = 1:length(ticksV)
%     tickC{i} = num2str(ticksV(i));
% end
% hCbar = colorbar('Ticks',ticksV,'TickLabels',tickC);
% axis('equal')
% view(3)
% xlabel('x','fontsize',18)
% ylabel('y','fontsize',18)
% zlabel('z','fontsize',18)
% grid('on')
% title(titleStr)


function obj = coef_J_pure_pnp_func_new(in1,in2,in3,scale)
b1 = in1(:,1);
b2 = in1(:,2);
cx = in3(:,3);
cy = in3(:,4);
fx = in3(:,1);
fy = in3(:,2);
r1 = in2(:,1);
r2 = in2(:,2);
r3 = in2(:,3);
t4 = b1.*r3;
t5 = cx.*r3;
t6 = fx.*r1;
t2 = -t4+t5+t6;
t7 = b2.*r3;
t8 = cy.*r3;
t9 = fy.*r2;
t3 = -t7+t8+t9;
t10 = b1.*r2.*2.0;
t16 = cx.*r2.*2.0;
t11 = t10-t16;
t12 = b2.*r2.*2.0;
t13 = fy.*r3.*2.0;
t22 = cy.*r2.*2.0;
t14 = t12+t13-t22;
t15 = t3.^2;
t17 = b1.*r1.*2.0;
t18 = fx.*r3.*2.0;
t26 = cx.*r1.*2.0;
t19 = t17+t18-t26;
t20 = b2.*r1.*2.0;
t24 = cy.*r1.*2.0;
t21 = t20-t24;
t23 = fx.*r2.*t2.*4.0;
t25 = t21.*t3.*2.0;
t27 = t2.^2;
t28 = t11.*t2.*2.0;
t29 = t4-t5+t6;
t30 = t2.*t29.*2.0;
t31 = t7-t8+t9;
t32 = t3.*t31.*2.0;
t33 = t14.*t3.*2.0;
t34 = fy.*r1.*t14.*4.0;
t35 = -t17+t18+t26;
t36 = fy.*r1.*t3.*4.0;
t37 = fx.*r2.*t19.*4.0;
t38 = t11.^2;
t39 = t21.^2;
t40 = -t12+t13+t22;
t41 = fx.^2;
t42 = r2.^2;
t43 = fy.^2;
t44 = r1.^2;
t45 = fy.*r1.*t21.*4.0;
t46 = b1-cx;
t47 = b2-cy;
t48 = t19.*t2.*2.0;
t49 = fx.*r2.*t11.*4.0;
t50 = fx.*r2.*t29.*4.0;
t51 = fy.*r1.*t31.*4.0;
t52 = t30+t32-t41.*t42.*4.0-t43.*t44.*4.0;
t53 = fx.*r2.*t35.*4.0;
t54 = t29.^2;
t55 = fy.*scale.*t3.*2.0;
t56 = t3.*t47.*2.0;
t57 = fy.*r1.*t40.*4.0;
t58 = t11.*t35.*2.0;
t59 = t21.*t40.*2.0;
t60 = r1.*scale.*t43.*4.0;
t61 = fx.*r2.*t46.*4.0;
t62 = t21.*t31.*2.0;
t63 = t29.*t35.*2.0;
t64 = fy.*scale.*t21.*2.0;
t65 = t21.*t47.*2.0;
t66 = t31.^2;
t67 = fx.*scale.*t2.*2.0;
t68 = t2.*t46.*2.0;
t69 = t11.*t29.*2.0;
t70 = t31.*t40.*2.0;
t71 = t11.*t46.*2.0;
t72 = fx.*scale.*t29.*2.0;
t73 = fy.*scale.*t31.*2.0;
obj = [scale.*(t15+t27),-scale.*(t28+t33),scale.*(t25+t48),-scale.*(t23-fy.*r1.*t3.*4.0),scale.*(t15.*-2.0+t30+t38+t14.^2),scale.*(t23+t36-t11.*t19.*2.0-t14.*t21.*2.0),-scale.*(t25+t34-t2.*t35.*2.0-fx.*r2.*t11.*4.0),scale.*(t27.*-2.0+t32+t39+t19.^2),-scale.*(t28+t37-t3.*t40.*2.0-fy.*r1.*t21.*4.0),-scale.*t52,t67,t55,-scale.*(t56+t68),scale.*(t33-t11.*t29.*2.0),-scale.*(t25+t34+t49-t19.*t29.*2.0),-scale.*(t36+t50+t58-t14.*t21.*2.0),scale.*(t28+t37+t45-t14.*t31.*2.0),scale.*(t38.*2.0-t39.*2.0+t19.*t35.*2.0-t14.*t40.*2.0-t41.*t42.*8.0+t43.*t44.*8.0),-scale.*(t45+t53-t11.*t29.*2.0-t14.*t31.*2.0),fx.*scale.*t11.*-2.0,fy.*scale.*t14.*-2.0,scale.*(t71+t14.*t47.*2.0),-scale.*(t48-t21.*t31.*2.0),scale.*(t23+t51+t59-t11.*t19.*2.0),scale.*(t49+t57-t19.*t29.*2.0-t21.*t31.*2.0),fx.*scale.*t19.*2.0,t64,-scale.*(t65+t19.*t46.*2.0),scale.*(t50-t51),r2.*scale.*t41.*-4.0,t60,scale.*(t61-fy.*r1.*t47.*4.0),scale.*(t15+t54),-scale.*(t36-t50),scale.*(t25+t63),-scale.*t52,-scale.*(t45-t53+t69+t3.*t40.*2.0),scale.*(t32+t39-t54.*2.0+t35.^2),t72,-t55,scale.*(t56-t29.*t46.*2.0),-scale.*(t23-t51),-scale.*(t49-t57+t62+t2.*t35.*2.0),-scale.*(t50+t51+t58+t59),r2.*scale.*t41.*4.0,t60,-scale.*(t61+fy.*r1.*t47.*4.0),scale.*(t62-t63),fx.*scale.*t35.*2.0,-t64,scale.*(t65-t35.*t46.*2.0),scale.*(t27+t66),scale.*(t28+t70),scale.*(t30+t38-t66.*2.0+t40.^2),-t67,t73,scale.*(t68-t31.*t47.*2.0),scale.*(t69-t70),fx.*scale.*t11.*-2.0,fy.*scale.*t40.*2.0,scale.*(t71-t40.*t47.*2.0),scale.*(t54+t66),-t72,-t73,scale.*(t29.*t46.*2.0+t31.*t47.*2.0),scale.*t41,fx.*scale.*t46.*-2.0,scale.*t43,fy.*scale.*t47.*-2.0,scale.*(t46.^2+t47.^2)];


% params
totalNum = 7385291;
bValue = 3000;
lamda = 0.5;
qFile = '/home/yinhuan/Data/mapModel/kitti_10/weightVector.txt';
visFilesDir = '/home/yinhuan/Data/mapModel/kitti_10/visMatrix/';
maxQ = 48;
minQ = 1;

splitLength_0 = 50;
saveResultsDir_0 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/0/';
saveReIndexFile_0 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/index/0.txt';
saveNewQFile_0 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/weight/0.txt';

splitLength_1 = 100;
saveResultsDir_1 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/1/';
saveReIndexFile_1 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/index/1.txt';
saveNewQFile_1 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/weight/1.txt';

splitLength_2 = 200;
saveResultsDir_2 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/2/';
saveReIndexFile_2 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/index/2.txt';
saveNewQFile_2 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/weight/2.txt';

splitLength_3 = 400;
saveResultsDir_3 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/3/';
saveReIndexFile_3 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/index/3.txt';
saveNewQFile_3 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/weight/3.txt';

splitLength_4 = 800;
saveResultsDir_4 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/4/';
saveReIndexFile_4 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/index/4.txt';
saveNewQFile_4 = '/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/weight/4.txt';

%% iteration of optimization

visCells_origin = fromVisDirToCells(visFilesDir);

[ epsilon_soft_0, time_sum_0  ] = Uniform_loopCompress_Cells( lamda, qFile, visCells_origin, splitLength_0, totalNum, bValue, saveResultsDir_0 );
min_cost_0 = get_min_cost(minQ, maxQ, saveResultsDir_0, qFile, lamda, epsilon_soft_0);
[totalNum_0, visCells_0] = deleteZeroPoints_Cells( qFile, visCells_origin, saveResultsDir_0, saveReIndexFile_0, saveNewQFile_0 );

[ epsilon_soft_1, time_sum_1  ] = Uniform_loopCompress_Cells( lamda, saveNewQFile_0, visCells_0, splitLength_1, totalNum_0, bValue, saveResultsDir_1 );
min_cost_1 = get_min_cost(minQ, maxQ, saveResultsDir_1, saveNewQFile_0, lamda, epsilon_soft_1);
[totalNum_1, visCells_1] = deleteZeroPoints_Cells( saveNewQFile_0, visCells_0, saveResultsDir_1, saveReIndexFile_1, saveNewQFile_1 );

[ epsilon_soft_2, time_sum_2  ] = Uniform_loopCompress_Cells( lamda, saveNewQFile_1, visCells_1, splitLength_2, totalNum_1, bValue, saveResultsDir_2 );
min_cost_2 = get_min_cost(minQ, maxQ, saveResultsDir_2, saveNewQFile_1, lamda, epsilon_soft_2);
[totalNum_2, visCells_2] = deleteZeroPoints_Cells( saveNewQFile_1, visCells_1, saveResultsDir_2, saveReIndexFile_2, saveNewQFile_2 );

[ epsilon_soft_3, time_sum_3  ] = Uniform_loopCompress_Cells( lamda, saveNewQFile_2, visCells_2, splitLength_3, totalNum_2, bValue, saveResultsDir_3 );
min_cost_3 = get_min_cost(minQ, maxQ, saveResultsDir_3, saveNewQFile_2, lamda, epsilon_soft_3);
[totalNum_3, visCells_3] = deleteZeroPoints_Cells( saveNewQFile_2, visCells_2, saveResultsDir_3, saveReIndexFile_3, saveNewQFile_3 );

[ epsilon_soft_4, time_sum_4  ] = Uniform_loopCompress_Cells( lamda, saveNewQFile_3, visCells_3, splitLength_4, totalNum_3, bValue, saveResultsDir_4 );
min_cost_4 = get_min_cost(minQ, maxQ, saveResultsDir_4, saveNewQFile_3, lamda, epsilon_soft_4);
[totalNum_4, visCells_4] = deleteZeroPoints_Cells( saveNewQFile_3, visCells_3, saveResultsDir_4, saveReIndexFile_4, saveNewQFile_4 );

%% save the results

compressIndex_3 = anal_reindex_last(saveResultsDir_4, saveReIndexFile_3);
compressIndex_2 = anal_reindex_middle(compressIndex_3, saveReIndexFile_2);
compressIndex_1 = anal_reindex_middle(compressIndex_2, saveReIndexFile_1);
compressIndex_0 = anal_reindex_middle(compressIndex_1, saveReIndexFile_0);

dlmwrite('/home/yinhuan/Data/mapModel/kitti_10/iter_b_3000/compressIndex_final.txt', compressIndex_0, 'precision', '%d');