%% pathRRT
%%  - create a path from a start node to an end node
%%    using the RRT algorithm.
%%  - RRT = Rapidly-exploring Random Tree
%%
%% Last Modified - 6/8/2006 - R. Beard
%%               - 4/15/2010 - R. Beard

function path_out=planRRT(wpp_start, wpp_end, map)

    % standard length of path segments
    segmentLength = 100;

    % desired down position is down position of end node
    pd = wpp_end(3);
    chi = -9999;

    % specify start and end nodes from wpp_start and wpp_end
    start_node = [wpp_start(1), wpp_start(2), pd, chi, 0, 0, 0];
    end_node = [wpp_end(1), wpp_end(2), pd, chi, 0, 0, 0];
    % format:  [N, E, D, chi, cost, parent_idx, flag_connect_to_goal]

    % establish tree starting with the start node
    tree = start_node;

    % check to see if start_node connects directly to end_node
    if ( (norm(start_node(1:3)-end_node(1:3))<segmentLength )...
            &(collision(start_node,end_node,map)==0) )
        path = [start_node; end_node];
    else
        numPaths = 0;
        while numPaths<3,
            [tree,flag] = extendTree(tree,end_node,segmentLength,map,pd,chi);
            numPaths = numPaths + flag;
        end
    end

    % find path with minimum cost to end_node
    path = findMinimumPath(tree,end_node);
    path_out = smoothPath(path,map);
    plotmap(map,path,path_out,tree);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generateRandomNode
%%   create a random node (initialize)
function node=generateRandomNode(map,pd,chi)

    % randomly pick configuration
    pn       = map.width*rand;
    pe       = map.width*rand;
    pd       = pd; % constant altitute paths
    cost     = 0;
    node     = [pn, pe, pd, chi, cost, 0, 0];
    % format:  [N, E, D, chi, cost, parent_idx, flag_connect_to_goal]

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% collision
%%   check to see if a node is in collsion with obstacles
function collision_flag = collision(start_node, end_node, map)

    collision_flag = 0;

    [X,Y,Z] = pointsAlongPath(start_node, end_node, 0.1);

    for i = 1:length(X),
        if Z(i) >= downAtNE(map, X(i), Y(i)),
            collision_flag = 1;
        end
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% pointsAlongPath
%%   Find points along straight-line path separted by Del (to be used in
%%   collision detection)
function [X,Y,Z] = pointsAlongPath(start_node, end_node, Del)

    X = [start_node(1)];
    Y = [start_node(2)];
    Z = [start_node(3)];

    q = [end_node(1:3)-start_node(1:3)];
    L = norm(q);
    q = q/L;

    w = start_node(1:3);
    for i=2:floor(L/Del),
        w = w + Del*q;
        X = [X, w(1)];
        Y = [Y, w(2)];
        Z = [Z, w(3)];
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% downAtNE
%%   find the world down coordinate at a specified (n,e) location
function down = downAtNE(map, n, e)

      [d_n,idx_n] = min(abs(n - map.buildings_n));
      [d_e,idx_e] = min(abs(e - map.buildings_e));

      if (d_n<=map.BuildingWidth) && (d_e<=map.BuildingWidth),
          down = -map.heights(idx_e,idx_n);
      else
          down = 0;
      end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% extendTree
%%   extend tree by randomly selecting point and growing tree toward that
%%   point
function [new_tree,flag] = extendTree(tree,end_node,segmentLength,map,pd,chi)

  flag1 = 0;
  loop_count = 0;
  while flag1==0,
    % select a random point
    randomNode=generateRandomNode(map,pd,chi);

    % find leaf on node that is closest to randomPoint
    tmp = tree(:,1:3)-ones(size(tree,1),1)*randomNode(1:3);
    [dist,idx] = min(diag(tmp*tmp'));
    L = min(sqrt(dist), segmentLength);
    cost     = tree(idx,5) + L;
    tmp = randomNode(1:3)-tree(idx,1:3);
    new_point = tree(idx,1:3)+L*(tmp/norm(tmp));
    new_node = [new_point, chi, cost, idx, 0];
    loop_count = loop_count+1
    if collision(tree(idx,:), new_node, map)==0,
      new_tree = [tree; new_node];
      flag1=1;
    end
  end

  % check to see if new node connects directly to end_node
  if ( (norm(new_node(1:3)-end_node(1:3))<segmentLength )...
      &&(collision(new_node,end_node,map)==0) )
    flag = 1;
    new_tree(end,7)=1;  % mark node as connecting to end.
  else
    flag = 0;
  end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% findMinimumPath
%%   find the lowest cost path to the end node
function path = findMinimumPath(tree,end_node)

    % find nodes that connect to end_node
    connectingNodes = [];
    for i=1:size(tree,1),
        if tree(i,7)==1,
            connectingNodes = [connectingNodes; tree(i,:)];
        end
    end

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,5));

    % construct lowest cost path
    path = [connectingNodes(idx,:); end_node];
    parent_node = connectingNodes(idx,6);
    while parent_node>1,
        parent_node = tree(parent_node,6);
        path = [tree(parent_node,:); path];
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% smoothPath
%%   smooth the waypoint path
function newPath = smoothPath(path,map)

    newPath = path(1,:); % add the start node
    ptr =2;  % pointer into the path
    while ptr <= size(path,1)-1,
        if collision(newPath(end,:), path(ptr+1,:), map)~=0, % if there is a collision
            newPath = [newPath; path(ptr,:)];  % add previous node
        end
        ptr=ptr+1;
    end
    newPath = [newPath; path(end,:)];

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotmap
%%   plot obstacles and path
function plotmap(map,path,smoothedPath,tree)

    % setup plot
    figure(3), clf
    axis([0,map.width,0,map.width,0,2*map.MaxHeight]);
    xlabel('E')
    ylabel('N')
    zlabel('h')
    hold on

    % plot buildings
    V = [];
    F = [];
    patchcolors = [];
    count = 0;
    for i=1:map.NumBlocks,
        for j=1:map.NumBlocks,
            [Vtemp,Ftemp,patchcolorstemp] = buildingVertFace(map.buildings_n(i),...
                map.buildings_e(j),map.BuildingWidth,map.heights(j,i));
            V = [V; Vtemp];
            Ftemp = Ftemp + count;
            F = [F; Ftemp];
            count = count + 8;
            patchcolors = [patchcolors;patchcolorstemp];
        end
    end

    patch('Vertices', V, 'Faces', F,...
                     'FaceVertexCData',patchcolors,...
                     'FaceColor','flat');

    % draw tree
    for i=2:size(tree,1),
        X = [tree(i,1), tree(tree(i,6),1)];
        Y = [tree(i,2), tree(tree(i,6),2)];
        Z = [tree(i,3), tree(tree(i,6),3)];
        plot3(Y,X,-Z,'g')
    end

    % draw path
    X = path(:,1);
    Y = path(:,2);
    Z = path(:,3);
    plot3(Y,X,-Z,'r','linewidth',2);

    % draw smooth path
    X = smoothedPath(:,1);
    Y = smoothedPath(:,2);
    Z = smoothedPath(:,3);
    plot3(Y,X,-Z,'k','linewidth',2);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% buildingVertFace(x,y,width,height)
%%   define patches for a building located at (x,y)
function [V,F,patchcolors] = buildingVertFace(n,e,width,height)

  % vertices of the building
  V = [...
        e+width/2, n+width/2, 0;...
        e+width/2, n-width/2, 0;...
        e-width/2, n-width/2, 0;...
        e-width/2, n+width/2, 0;...
        e+width/2, n+width/2, height;...
        e+width/2, n-width/2, height;...
        e-width/2, n-width/2, height;...
        e-width/2, n+width/2, height;...
        ];
  % define faces of fuselage
  F = [...
        1, 4, 8, 5;... % North Side
        1, 2, 6, 5;... % East Side
        2, 3, 7, 6;... % South Side
        3, 4, 8, 7;... % West Side
        5, 6, 7, 8;... % Top
        ];

  myred = [1, 0, 0];
  mygreen = [0, 1, 0];
  myblue = [0, 0, 1];
  myyellow = [1,1,0];
  mymagenta   = [0, 1, 1];

  patchcolors = [...
    mygreen;... % North
    mygreen;... % East
    mygreen;... % South
    mygreen;... % West
    myyellow;...  % Top
    ];

end


function sVF1 = rdivide(sVF1, sVF2)
%
% Syntax
%
%   sVF = sVF ./ a
%   sVF = sVF ./ sF
%
% Input
%  sVF - @S2VectorFieldHarmonic
%  sF  - @S2Fun
%  a   - double
%
% Output
%  sVF - @S2VectorFieldHarmonic
%

if isnumeric(sVF2)
  sVF1.sF = sVF1.sF .* (1./sVF2);
elseif isa(sVF2,'S2Fun')
  sVF1.sF = sVF1.sF ./ sVF2;
end

end


function  [data_hull,sources] = get_reduced_graph_weights_sources(data,hull);

data_hull.hull = hull;
switch data.dag_type
	case 'grid + input_space'

		affinity_hull = cell(1,size(hull,1));
		weights_hull = zeros(1,size(hull,1))';
		for i=1:size(hull,1)
			affinity_hull{i} = find(all(hull - repmat(hull(i,:),size(hull,1),1) >= 0,2));
			temp = 1;
			if all(hull(i,:)==1)

				weights_hull(i) = data.weight0;
			else
				for j=1:data.p, temp = temp * data.weightA^(hull(i,j)-1); end
				weights_hull(i) = temp;
			end
		end

		% prepare data
		X_hull = [];
		groups_hull = cell(1,size(hull,1));
		groupdescs_hull = cell(1,size(hull,1));

		ai1=1;
		for i1=1:size(hull,1)
			Xloc = get_data_reduced(hull(i1,:),data);
			X_hull = [ X_hull, Xloc ];
			groups_hull{i1} = ai1:ai1+size(Xloc,2)-1;   % records the indices of that group
			ai1 = ai1 + size(Xloc,2);
		end

		for i1=1:size(hull,1)
			groupdescs_hull{i1}=[];
			for i2=1:length(affinity_hull{i1})
				groupdescs_hull{i1} = [ groupdescs_hull{i1}, groups_hull{affinity_hull{i1}(i2)} ];
			end
		end

		qq =data.qs;

		% sources = find_sources_complement_grid_fast_c(hull,data.qs);
        % this function finds the sources of the complement of the active
        % set defined by the hull

        if nargout>1

        		sources = compute_sources(hull,data.qs);
% 		sources_old =find_sources_complement_grid_fast_int_c( int32(hull'),int32(data.qs));

        end
		data_hull.X = X_hull;
		data_hull.weights = weights_hull;
		data_hull.affinity = affinity_hull;
		data_hull.groups = groups_hull;
		data_hull.groupdescs = groupdescs_hull;

	case 'mkl + input_space'

		affinity_hull = cell(1,size(hull,1));
		weights_hull = zeros(1,size(hull,1))';
		for i=1:size(hull,1)
			affinity_hull{i} = find(all(hull - repmat(hull(i,:),size(hull,1),1) >= 0,2));
			temp = 1;
			if all(hull(i,:)==1)

				weights_hull(i) = data.weight0;
			else

				weights_hull(i) = data.weightA;
			end
		end

		% prepare data
		X_hull = [];
		groups_hull = cell(1,size(hull,1));
		groupdescs_hull = cell(1,size(hull,1));

		included_kernels = [];
		ai1=1;
		for i1=1:size(hull,1)
			Xloc = get_data_reduced(hull(i1,:),data);
			X_hull = [ X_hull, Xloc ];
			groups_hull{i1} = ai1:ai1+size(Xloc,2)-1;   % records the indices of that group
			ai1 = ai1 + size(Xloc,2);
		end

		for i1=1:size(hull,1)
			groupdescs_hull{i1}=[];
			for i2=1:length(affinity_hull{i1})
				groupdescs_hull{i1} = [ groupdescs_hull{i1}, groups_hull{affinity_hull{i1}(i2)} ];
			end
		end

		%sources = find_sources_complement_mkl(hull,data.p,data.qs);
		included_kernels = find(max(hull,[],1)==2);
		temp = 1:data.p;
		temp(included_kernels) = [];
		sources = [];
		for i=1:length(temp)
			hloc = ones(1,data.p);

			hloc(temp(i))=2;
			sources =  [ sources; hloc];
		end

		data_hull.X = X_hull;
		data_hull.weights = weights_hull;
		data_hull.affinity = affinity_hull;
		data_hull.groups = groups_hull;
		data_hull.groupdescs = groupdescs_hull;

	case 'bimkl + input_space'

		affinity_hull = cell(1,size(hull,1));
		weights_hull = zeros(1,size(hull,1))';
		for i=1:size(hull,1)
			temp = 1;
			if all(hull(i,:)==1)
				affinity_hull{i} =  1:size(hull,1);
				weights_hull(i) = data.weight0;
			else
				affinity_hull{i} =i;
				for j=1:data.p, temp = temp * data.weightA^(hull(i,j)-1); end
				weights_hull(i) = temp;
			end
		end

		% prepare data
		X_hull = [];
		groups_hull = cell(1,size(hull,1));
		groupdescs_hull = cell(1,size(hull,1));

		ai1=1;
		for i1=1:size(hull,1)
			Xloc = get_data_reduced(hull(i1,:),data);
			X_hull = [ X_hull, Xloc ];
			groups_hull{i1} = ai1:ai1+size(Xloc,2)-1;   % records the indices of that group
			ai1 = ai1 + size(Xloc,2);
		end

		for i1=1:size(hull,1)
			groupdescs_hull{i1}=[];
			for i2=1:length(affinity_hull{i1})
				groupdescs_hull{i1} = [ groupdescs_hull{i1}, groups_hull{affinity_hull{i1}(i2)} ];
			end
		end

		qq =data.qs;
		save temp hull qq

		p = data.p;

		included_kernels_one = zeros(p,1);
		included_kernels_two = zeros(p,p);
		for i=1:size(hull,1);
			tt = find(hull(i,:)==2);
			if length(tt)==1
				included_kernels_one(tt)=1;
			elseif length(tt)==2
				included_kernels_one(tt(1),tt(2)) = 1;
				included_kernels_one(tt(2),tt(1)) = 1;
			end
		end

		sources = [];
		for i=1:p
			if (included_kernels_one(i)==0),
				hloc = ones(1,p);
				hloc(i)=2;
				sources =  [ sources; hloc];
			end
		end
		for i=2:p
			for j=1:i-1
				if (included_kernels_two(i,j)==0),
					hloc = ones(1,p);
					hloc(i)=2; hloc(j)=2;
					sources =  [ sources; hloc];
				end
			end
		end

		data_hull.X = X_hull;
		data_hull.weights = weights_hull;
		data_hull.affinity = affinity_hull;
		data_hull.groups = groups_hull;
		data_hull.groupdescs = groupdescs_hull;

end
data_hull.dag_type = data.dag_type;

% check that the total dimension of the input space is smaller than the
% number of observations. If not -> store kernel matrices
switch data.dag_type
	case {'grid + input_space', 'mkl + input_space', 'bimkl + input_space'}

		if size(data_hull.X,2) > size(data_hull.X,1)
			n = size(data_hull.X,1);
			data_hull.kernels = zeros(n*(n+1)/2,size(hull,1),'single');
			for i=1:size(hull,1)
				data_hull.kernels(:,i) = symmetric_vectorize_single( single(data_hull.X(:,data_hull.groups{i})*data_hull.X(:,data_hull.groups{i})' ) );
			end
			switch data.dag_type
				case 'grid + input_space'
					data_hull.dag_type = 'grid + kernels';
				case 'mkl + input_space'
					data_hull.dag_type = 'mkl + kernels';
				case 'bimkl + input_space'
					data_hull.dag_type = 'bimkl + kernels';
			end

		end

end


function [rot, trans, Mf] = regEstRot4(rot, trans, scaleFac, volume, numSlices, sagSize, NCoarseIter, coarseFlag, fineFlag);
%
% regEstRotFunction -   Function to compute automatically the rotation and translation,
%                       to be used as a callback.
%                       It returns the rotation and translation in workspace variables
%                       rot and trans.
%
%  Oscar Nestares - 5/99
%  ON - Added option to use the rotation and translation in bestrotvol
%       as initial alignment (useful to recompute automatically previous
%       manual alignments).
%  AB 4/21    - added NCoarseIter = [];
%  SL 7/15/02 - Created regEstRot4 for use in mrAlign4 (based largely on regEstRot).
%               Changed it from a script into a function and eliminated the need for
%               'Usebestrotvol' parameter and the function regParamInit.
%  SL 7/29/02 - removed NCoarseIter = [];
%  SL 8/02/02 - added coarseFlag and fineFlag parameters

global INPLANE

% registering
[rot, trans, Mf]=regVolInp4(...
                 reshape(volume,[sagSize, numSlices]),...   % volume
                 INPLANE.anat,...                           % inplanes
                 scaleFac,...                               % inverse voxel size for inplanes and volume
                 rot,...                                    % initial rotation
                 trans,...                                  % initial translation
                 NCoarseIter,...                            % number of coarse iterations
                 coarseFlag,...                             % coarse iterations flag
                 fineFlag,...                               % fine iterations flag
                 'regEstFilIntGrad',...                     % function to estimate the intensity grad.
                 0);                                        % Plane by Plane flag = 0 (=>works globaly)

% done

msgbox('Automatic computation done');


filename='Gripping_tetrahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'PROJECTED GRADIENT'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.2;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


%% Demo to show the results of MCG
clear all;close all;home;

% Read an input image
I = imread(fullfile(root_dir, 'demos','101087.jpg'));

tic;
% Test the 'fast' version, which takes around 5 seconds in mean
[candidates_scg, ucm2_scg] = im2mcg(I,'fast');
toc;

tic;
% Test the 'accurate' version, which tackes around 30 seconds in mean
[candidates_mcg, ucm2_mcg] = im2mcg(I,'accurate');
toc;

%% Show UCM results (dilated for visualization)
figure;
subplot(1,3,1)
imshow(I), title('Image')

subplot(1,3,2)
imshow(imdilate(ucm2_scg,strel(ones(3))),[]), title('Fast UCM (SCG)')

subplot(1,3,3)
imshow(imdilate(ucm2_mcg,strel(ones(3))),[]), title('Accurate UCM (MCG)')

%% Show Object Candidates results and bounding boxes
% Candidates in rank position 11 and 12
id1 = 11; id2 = 12;

% Get the masks from superpixels and labels
mask1 = ismember(candidates_mcg.superpixels, candidates_mcg.labels{id1});
mask2 = ismember(candidates_mcg.superpixels, candidates_mcg.labels{id2});

% Bboxes is a matrix that contains the four coordinates of the bounding box
% of each candidate in the form [up,left,down,right]. See folder bboxes for
% more function to work with them

% Show results
figure;
subplot(1,3,1)
imshow(I), title('Image')
subplot(1,3,2)
imshow(mask1), title('Candidate + Box')
hold on
plot([candidates_mcg.bboxes(id1,4) candidates_mcg.bboxes(id1,4) candidates_mcg.bboxes(id1,2) candidates_mcg.bboxes(id1,2) candidates_mcg.bboxes(id1,4)],...
     [candidates_mcg.bboxes(id1,3) candidates_mcg.bboxes(id1,1) candidates_mcg.bboxes(id1,1) candidates_mcg.bboxes(id1,3) candidates_mcg.bboxes(id1,3)],'r-')
subplot(1,3,3)
imshow(mask2), title('Candidate + Box')
hold on
plot([candidates_mcg.bboxes(id2,4) candidates_mcg.bboxes(id2,4) candidates_mcg.bboxes(id2,2) candidates_mcg.bboxes(id2,2) candidates_mcg.bboxes(id2,4)],...
     [candidates_mcg.bboxes(id2,3) candidates_mcg.bboxes(id2,1) candidates_mcg.bboxes(id2,1) candidates_mcg.bboxes(id2,3) candidates_mcg.bboxes(id2,3)],'r-')


function calpak_test75 ( )

%*****************************************************************************80
%
%% CALPAK_TEST75 tests YMD_INC_YMD_COMMON and YMDF_DIF_YMDF_COMMON.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST75\n' );
  fprintf ( 1, '  For the Common calendar,\n' );
  fprintf ( 1, '  YMD_INC_YMD_COMMON increments a YMDF date by YMDF;\n' );
  fprintf ( 1, '  YMDF_DIF_YMDF_COMMON finds the YMDF difference.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Date1        increment    Date2          difference\n' );
  fprintf ( 1, '\n' );

  y1 = 1900;
  m1 = 5;
  d1 = 27;
  f1 = 0.0;

  yn1 = 50;
  mn1 = 9;
  dn1 = 10;
  fn2 = 0.0;

  [ y2, m2, d2 ] = ymd_inc_ymd_common ( y1, m1, d1, yn1, mn1, dn1 );
  f2 = 0.0;

  [ yn2, mn2, dn2, fn2, ierror ] = ymdf_dif_ymdf_common ( y1, m1, d1, f1, ...
  y2, m2, d2, f2 );

  s1 = ymdf_to_s_common ( y1, m1, d1, f1 );

  s2 = ymdf_to_s_common ( y2, m2, d2, f2 );

  fprintf ( 1, '  %15s  %3d  %3d  %3d  %15s  %3d  %3d  %3d\n', ...
    s1, yn1, mn1, dn1, s2, yn2, mn2, dn2 );

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment1B_K10_N30
close all;
global parameters

%STOMP PARAMETERS
%number of particles
K = 10;

%repeat experiment number of times
parameters.n_repeat = 3;
parameters.experiment_name = 'experiment1B_K10_N30_time.mat';
parameters.experiment_number = 1;
parameters.animate = 0;

%repeat the experiment E times
random_manips=[];
Gout = [];
tic;
for i=1:parameters.n_repeat
    close all
    [pk, final_manip] = path_planning_SCO_4DOF(K);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
end
elapsed = toc;
fprintf('TIC TOC: %g\n', elapsed/parameters.n_repeat);

'ended'
parameters.experiment_name


% This plots the convergence of the human observer study in terms of the
% Kendall coefficients of agreement versus different numbers of workers and
% error thresholds for the sanity checks.
function plotObserverStudyConvergence(resultDir, outputDir)

    % Get precomputed coefficients.
    load([resultDir, '/observerStudy/observerStudyConvergence.mat']);

    % Mean and standard deviation of the Kendall coefficient of agreement
    % over the samples of the Monte Carlo simulation.
    kendallMean = mean(kendall, 3);
    kendallStd = std(kendall, [], 3);

    % Plot Kendall coefficient of agreement vs. number of workers.
    lineStyle = {'-', '--', '-.', ':'};
    lineColor = {[0 0 1], [1 0 0], [1 0.65 0], [0 1 0]};
    figure;
    for maxSanityCheckFailsIdx = 1:length(maxSanityCheckFails)
        errorbar(numWorkers, kendallMean(maxSanityCheckFailsIdx,:), kendallStd(maxSanityCheckFailsIdx,:), ...
            lineStyle{maxSanityCheckFailsIdx}, 'Color', lineColor{maxSanityCheckFailsIdx}, 'LineWidth', 1.5);
        legendStr{maxSanityCheckFailsIdx} = sprintf('$n_f$ = %s', num2str(maxSanityCheckFails(maxSanityCheckFailsIdx)));
        hold on;
    end
    hold off;
    grid on;
    xlabel('Number of user sessions');
    ylabel('Coefficient of agreement');
    xlim([numWorkers(1) numWorkers(end)]);

    % Add legend
    legend(legendStr, 'Location', 'NorthEast', 'Orientation', 'horizontal', 'Interpreter', 'latex');

    % Save to TikZ.
    matlab2tikz([outputDir, '/', 'userStudyConvergence.tikz'], ...
        'height', '\figureheight', 'width', '\figurewidth', 'showInfo', false, ...
        'extraaxisoptions', ['xlabel near ticks,', ...
        'ylabel near ticks,', ...
        'scaled y ticks=false,', ...
        'yticklabel style={/pgf/number format/fixed, /pgf/number format/precision=2},']);


function varargout = removeMeshFaces(v, f, fI)
%REMOVEMESHFACES Remove faces from a mesh by face indices.
%   [V2, F2] = removeMeshFaces(V, F, FI) removes faces from the mesh by
%   the face indices FI into faces F of the mesh. The mesh is represented
%   by the vertex array V and the face array F. The result is the new set
%   of vertices V2 and faces F2 without the faces indexed by FI. FI can be
%   either a linear or a logical index.
%
%   [V2, F2] = removeMeshFaces(MESH, FI) with the struct MESH containing
%   the fields "vertices" (V) and "faces" (F)
%
%   MESH2 = removeMeshFaces(V, F, FI) with the struct MESH2 containing the
%   fields "vertices" (V2) and "faces" (F2)
%
%   MESH2 = removeMeshFaces(MESH, FI) with the structs MESH and MESH2
%   containing the fields "vertices" (V, V2) and "faces" (F, F2)
%
%   Example
%     [v, f] = createSoccerBall;
%     f = triangulateFaces(f);
%     fI = true(length(f),1);
%     fI(1:length(f)/2) = false;
%     [v2, f2] = removeMeshFaces(v, f, fI);
%     drawMesh(v, f, 'faceColor', 'none', 'faceAlpha', .2);
%     drawMesh(v2, f2, 'faceAlpha', .7);
%     view(3); axis equal
%
%   See also
%   meshes3d, drawMesh

% ------
% Authors: oqilipo, David Legland
% E-mail: david.legland@inrae.fr
% Created: 2017-07-04
% Copyright 2017-2022

% parse inputs
narginchk(2,3)
nargoutchk(1,2)

if nargin == 2
    fI = f;
    [v, f] = parseMeshData(v);
end

p = inputParser;
isIndexToFaces = @(x) ...
    (islogical(x) && isequal(length(x), size(f,1))) || ...
    (all(floor(x)==x) && min(x)>=1 && max(x)<=size(f,1));
addRequired(p,'fI',isIndexToFaces)
parse(p, fI);
if ~islogical(p.Results.fI)
    fI=false(size(f,1),1);
    fI(p.Results.fI)=true;
else
    fI=p.Results.fI;
end

% algorithm
f2 = f(~fI,:);
[unqVertIds, ~, newVertIndices] = unique(f2);
v2 = v(unqVertIds,:);
f2 = reshape(newVertIndices,size(f2));

% parse outputs
if nargout == 1
    mesh2.vertices=v2;
    mesh2.faces=f2;
    varargout{1}=mesh2;
else
    varargout{1}=v2;
    varargout{2}=f2;
end

end


%  Make NIfTI structure specified by an N-D matrix. Usually, N is 3 for
%  3D matrix [x y z], or 4 for 4D matrix with time series [x y z t].
%  Optional parameters can also be included, such as: voxel_size,
%  origin, datatype, and description.
%
%  Once the NIfTI structure is made, it can be saved into NIfTI file
%  using "save_nii" command (for more detail, type: help save_nii).
%
%  Usage: nii = make_nii(img, [voxel_size], [origin], [datatype], [description])
%
%  Where:
%
%	img:		Usually, img is a 3D matrix [x y z], or a 4D
%			matrix with time series [x y z t]. However,
%			NIfTI allows a maximum of 7D matrix. When the
%			image is in RGB format, make sure that the size
%			of 4th dimension is always 3 (i.e. [R G B]). In
%			that case, make sure that you must specify RGB
%			datatype, which is either 128 or 511.
%
%	voxel_size (optional):	Voxel size in millimeter for each
%				dimension. Default is [1 1 1].
%
%	origin (optional):	The AC origin. Default is [0 0 0].
%
%	datatype (optional):	Storage data type:
%		2 - uint8,  4 - int16,  8 - int32,  16 - float32,
%		32 - complex64,  64 - float64,  128 - RGB24,
%		256 - int8,  511 - RGB96,  512 - uint16,
%		768 - uint32,  1792 - complex128
%			Default will use the data type of 'img' matrix
%			For RGB image, you must specify it to either 128
%			or 511.
%
%	description (optional):	Description of data. Default is ''.
%
%  e.g.:
%     origin = [33 44 13]; datatype = 64;
%     nii = make_nii(img, [], origin, datatype);    % default voxel_size
%
%  NIFTI data format can be found on: http://nifti.nimh.nih.gov
%
%  - Jimmy Shen (jimmy@rotman-baycrest.on.ca)
%
function nii = make_nii(varargin)

   nii.img = varargin{1};
   dims = size(nii.img);
   dims = [length(dims) dims ones(1,8)];
   dims = dims(1:8);

   voxel_size = [0 ones(1,7)];
   origin = zeros(1,5);
   descrip = '';

   switch class(nii.img)
      case 'uint8'
         datatype = 2;
      case 'int16'
         datatype = 4;
      case 'int32'
         datatype = 8;
      case 'single'
         if isreal(nii.img)
            datatype = 16;
         else
            datatype = 32;
         end
      case 'double'
         if isreal(nii.img)
            datatype = 64;
         else
            datatype = 1792;
         end
      case 'int8'
         datatype = 256;
      case 'uint16'
         datatype = 512;
      case 'uint32'
         datatype = 768;
      otherwise
         error('Datatype is not supported by make_nii.');
   end

   if nargin > 1 & ~isempty(varargin{2})
      voxel_size(2:4) = double(varargin{2});
   end

   if nargin > 2 & ~isempty(varargin{3})
      origin(1:3) = double(varargin{3});
   end

   if nargin > 3 & ~isempty(varargin{4})
      datatype = double(varargin{4});

      if datatype == 128 | datatype == 511
         dims(5) = [];
         dims(1) = dims(1) - 1;
         dims = [dims 1];
      end
   end

   if nargin > 4 & ~isempty(varargin{5})
      descrip = varargin{5};
   end

   if ndims(nii.img) > 7
      error('NIfTI only allows a maximum of 7 Dimension matrix.');
   end

   maxval = round(double(max(nii.img(:))));
   minval = round(double(min(nii.img(:))));

   nii.hdr = make_header(dims, voxel_size, origin, datatype, ...
	descrip, maxval, minval);

   switch nii.hdr.dime.datatype
   case 2
      nii.img = uint8(nii.img);
   case 4
      nii.img = int16(nii.img);
   case 8
      nii.img = int32(nii.img);
   case 16
      nii.img = single(nii.img);
   case 32
      nii.img = single(nii.img);
   case 64
      nii.img = double(nii.img);
   case 128
      nii.img = uint8(nii.img);
   case 256
      nii.img = int8(nii.img);
   case 511
      img = double(nii.img(:));
      img = single((img - min(img))/(max(img) - min(img)));
      nii.img = reshape(img, size(nii.img));
      nii.hdr.dime.glmax = double(max(img));
      nii.hdr.dime.glmin = double(min(img));
   case 512
      nii.img = uint16(nii.img);
   case 768
      nii.img = uint32(nii.img);
   case 1792
      nii.img = double(nii.img);
   otherwise
      error('Datatype is not supported by make_nii.');
   end

   return;					% make_nii

%---------------------------------------------------------------------
function hdr = make_header(dims, voxel_size, origin, datatype, ...
	descrip, maxval, minval)

   hdr.hk   = header_key;
   hdr.dime = image_dimension(dims, voxel_size, datatype, maxval, minval);
   hdr.hist = data_history(origin, descrip);

   return;					% make_header

%---------------------------------------------------------------------
function hk = header_key

    hk.sizeof_hdr       = 348;			% must be 348!
    hk.data_type        = '';
    hk.db_name          = '';
    hk.extents          = 0;
    hk.session_error    = 0;
    hk.regular          = 'r';
    hk.dim_info         = 0;

    return;					% header_key

%---------------------------------------------------------------------
function dime = image_dimension(dims, voxel_size, datatype, maxval, minval)

   dime.dim = dims;
   dime.intent_p1 = 0;
   dime.intent_p2 = 0;
   dime.intent_p3 = 0;
   dime.intent_code = 0;
   dime.datatype = datatype;

   switch dime.datatype
   case 2,
      dime.bitpix = 8;  precision = 'uint8';
   case 4,
      dime.bitpix = 16; precision = 'int16';
   case 8,
      dime.bitpix = 32; precision = 'int32';
   case 16,
      dime.bitpix = 32; precision = 'float32';
   case 32,
      dime.bitpix = 64; precision = 'float32';
   case 64,
      dime.bitpix = 64; precision = 'float64';
   case 128
      dime.bitpix = 24;  precision = 'uint8';
   case 256
      dime.bitpix = 8;  precision = 'int8';
   case 511
      dime.bitpix = 96;  precision = 'float32';
   case 512
      dime.bitpix = 16; precision = 'uint16';
   case 768
      dime.bitpix = 32; precision = 'uint32';
   case 1792,
      dime.bitpix = 128; precision = 'float64';
   otherwise
      error('Datatype is not supported by make_nii.');
   end

   dime.slice_start = 0;
   dime.pixdim = voxel_size;
   dime.vox_offset = 0;
   dime.scl_slope = 0;
   dime.scl_inter = 0;
   dime.slice_end = 0;
   dime.slice_code = 0;
   dime.xyzt_units = 0;
   dime.cal_max = 0;
   dime.cal_min = 0;
   dime.slice_duration = 0;
   dime.toffset = 0;
   dime.glmax = maxval;
   dime.glmin = minval;

   return;					% image_dimension

%---------------------------------------------------------------------
function hist = data_history(origin, descrip)

   hist.descrip = descrip;
   hist.aux_file = 'none';
   hist.qform_code = 0;
   hist.sform_code = 0;
   hist.quatern_b = 0;
   hist.quatern_c = 0;
   hist.quatern_d = 0;
   hist.qoffset_x = 0;
   hist.qoffset_y = 0;
   hist.qoffset_z = 0;
   hist.srow_x = zeros(1,4);
   hist.srow_y = zeros(1,4);
   hist.srow_z = zeros(1,4);
   hist.intent_name = '';
   hist.magic = '';
   hist.originator = origin;

   return;					% data_history


new_folder = [];
max_memo_GB = [];
% This code requires the file read_Intan_RHD2000_file.m which is provided
% on the Intan Technologies web site.
% new_folder: output folder with the parsed channels and metadata.
% If it doen't exist or is [], it will be 'wc_data'.
% max_memo_GB is an idea of the number of GB allocated for the data to be
% stored in RAM, so it is used to compute the number of segments in which
% the data should be split for processing
% Based in read_intan_data.m version 1.1, June 26, 2010
% (c) 2010, Intan Technologies, LLC
% For more information, see http://www.intantech.com
% For updates and latest version, see http://www.intantech.com/software.html

with_memory=true;
try
	memory;
catch
	with_memory=false;
end
if with_memory
	[userview,systemview] = memory;
	memo_avaible = floor(systemview.PhysicalMemory.Available*0.80);
	if exist('max_memo_GB','var') && ~isempty(max_memo_GB)
        max_memo = max_memo_GB*(1024)^3;
		if max_memo > memo_avaible
			error('max_memo_GB > 80% of Physical Memory Available')
		end
	else
		max_memo = memo_avaible;
	end
else
	max_memo = max_memo_GB*(1024)^3;
end

read_Intan_RHD2000_file;

sr = frequency_parameters.amplifier_sample_rate;
num_channels = length(amplifier_channels); % info sale de la header file

if ~exist('new_folder','var') || isempty(new_folder)
    new_folder = [pwd filesep 'wc_data'];
end

mkdir(new_folder)

% Read 'info.rhd'

% Read 'amplifier.dat'
fileinfo = dir([path filesep 'amplifier.dat']);
lts = fileinfo.bytes/(num_channels * 2); % int16 = 2 bytes
samples_per_channel = ceil(max_memo/(num_channels * 4)); %single (4 bytes)
num_segments = ceil(lts/samples_per_channel);
channels = 1:num_channels;
save([new_folder filesep 'intan_meta_data'],'sr','lts','channels')

fid = fopen([path filesep 'amplifier.dat'], 'r');

outfile_handles = cell(1,num_channels);

[fname] = 'ch';
for i = 1:num_channels
    outfile_handles{i} = fopen([new_folder filesep  fname '_' num2str(channels(i)) '.intch'],'w','l');
end

for j=1:num_segments
    ini = (j-1)*samples_per_channel;
    fin = min(j*samples_per_channel,lts);
    data2 = single(fread(fid,(fin-ini)*(num_channels),'int16')) * 0.195;
    for ind = 1:num_channels
        fwrite(outfile_handles{ind},data2(ind:num_channels:end),'single');
    end
    fprintf('Segment %d out of %d processed.\n',j,num_segments);
end
fclose('all');
clearvars
%Auxiliar para gui_plot
%time = num_samples / sr; %total time amplifier
%j=-1;
%save('trial','j')


function [c, ceq] = pathConstraint(z)
% [c, ceq] = pathConstraint(z)
%
% Computes the path constraint so that the chain integrator dynamics match
% the pendulum dynamics.
%
% INPUTS:
%   z = [x;v1;v2];
%
% OUTPUTS:
%   dz = dz/dt
%
% NOTES:
%   ddx = dv1   % definition
%   v1 = v2     % path constraint
%   dv2 = u2    % dynamics
%

% x = z(1,:); %Unused
v1 = z(2,:);
v2 = z(3,:);

c = [];
ceq = v1-v2;

end


function CPD = update_ess(CPD, fmarginal, evidence, ns, cnodes, hidden_bitv)
% UPDATE_ESS Update the Expected Sufficient Statistics of a hhmmF node.
% function CPD = update_ess(CPD, fmarginal, evidence, ns, cnodes, hidden_bitv)

% Figure out the node numbers associated with each parent
% so we extract evidence from the right place
dom = fmarginal.domain; % Q(1) .. Q(d) F(d+1) F(d)
Qps = fmarginal.domain(1:end-2);
Q = Qps(end);
Qps = Qps(1:end-1);

Qsz = CPD.Qsizes(CPD.Q);
Qpsz = prod(CPD.Qsizes(CPD.Qps)); % may be 1

% We assume the F node are always hidden, but allow some of the Q nodes
% to be observed. We do case analysis for speed.
%We only extract prob from fmarginal.T when F(d+1)=2 i.e., model below has finished.
% wrong -> % We sum over the possibilities that F(d+1) = 1 or 2

obs_self = ~hidden_bitv(Q);
if obs_self
  self_val = evidence{Q};
end

if isempty(Qps) % independent of parent context
  counts = zeros(Qsz, 2);
  %fmarginal.T(Q(d), F(d+1), F(d))
  if obs_self
    marg = myreshape(fmarginal.T, [1 2 2]);
    counts(self_val,:) = marg(1,2,:);
    %counts(self_val,:) = marg(1,1,:) + marg(1,2,:);
  else
    marg = myreshape(fmarginal.T, [Qsz 2 2]);
    counts = squeeze(marg(:,2,:));
    %counts = squeeze(marg(:,2,:)) + squeeze(marg(:,1,:));
  end
else
  counts = zeros(Qpsz, Qsz, 2);
  %fmarginal.T(Q(1:d-1), Q(d), F(d+1), F(d))
  obs_Qps = ~any(hidden_bitv(Qps));  % we assume that all or none of the Q  parents are observed
  if obs_Qps
    Qps_val = subv2ind(Qpsz, cat(1, evidence{Qps}));
  end
  if obs_self & obs_Qps
    marg = myreshape(fmarginal.T, [1 1 2 2]);
    counts(Qps_val, self_val, :) = squeeze(marg(1,1,2,:));
    %counts(Qps_val, self_val, :) = squeeze(marg(1,1,2,:)) + squeeze(marg(1,1,1,:));
  elseif ~obs_self & obs_Qps
    marg = myreshape(fmarginal.T, [1 Qsz 2 2]);
    counts(Qps_val, :, :) = squeeze(marg(1,:,2,:));
    %counts(Qps_val, :, :) = squeeze(marg(1,:,2,:)) + squeeze(marg(1,:,1,:));
  elseif obs_self & ~obs_Qps
    error('not yet implemented')
  else
    marg = myreshape(fmarginal.T, [Qpsz Qsz 2 2]);
    counts(:, :, :) = squeeze(marg(:,:,2,:));
    %counts(:, :, :) = squeeze(marg(:,:,2,:)) + squeeze(marg(:,:,1,:));
  end
end

CPD.sub_CPD_term = update_ess_simple(CPD.sub_CPD_term, counts);


function w = dbnc(dsTrain, varargin)

	prtrace(mfilename);

	% Settings for the different training algorithms.
    if(~exist('nnet','dir'))
		error('Neural network toolbox not found')
    end

    cInput_types = {'gaussian', 'logsigm'};

    %argument definition
    p = inputParser;   % Create instance of inputParser class.
    p.addRequired('dsTrain', @(x)( isdataset(x)));

    p.addParamValue('input_type', 'logsigm', @(x)( any(strcmpi(x,cInput_types))) );
    p.addParamValue('num_hidden', 3, @(x)( isnumeric(x) && x > 0 ));
    p.addParamValue('size_hidden', [], @(x)( (all(isnumeric(x)) && all(x > 0)) || isempty(x) ) );
    p.addParamValue('Initialization_dataset', [], @(x)( isdataset(x) )  );
    p.addParamValue('Validation_dataset', [], @(x)( isdataset(x) )  );

	% Check arguments
    try
        p.parse( dsTrain, varargin{:} );
    catch MyError
        rethrow(MyError);
    end

    dsTrain = p.Results.dsTrain;
    dsInitialization = p.Results.Initialization_dataset;
    dsValidation = p.Results.Validation_dataset;
    num_hidden = p.Results.num_hidden;
    size_hidden = p.Results.size_hidden;
    input_type = p.Results.input_type;

    % Dont know why this variable uses a lot of bytes to store at disk.
    clear p

    %Check the datasets
    islabtype(dsTrain,'crisp');
    isvaldfile(dsTrain,1,2); 							% At least 1 object per class, 2 classes
    dsTrain = testdatasize(dsTrain);
    %force domain to be in [0-1]
    w_scale_domain = scalem(dsTrain, 'domain');
    dsTrain = dsTrain * w_scale_domain;

    if( isempty(dsValidation) )
        dsValidation = dsTrain;
    else
        dsValidation = testdatasize(dsValidation);
        iscomdset(dsTrain, dsValidation);   							% Check whether training and tuning set match
        dsValidation = dsValidation * w_scale_domain;
    end

    if( isempty(dsInitialization) )
        dsInitialization = dsTrain;
    else
        dsInitialization = dsInitialization * w_scale_domain;
    end

    [train_m, train_k, train_c] = getsize(dsTrain);
    [ train_labels, train_lablist] = getnlab(dsTrain);

    if( isempty(size_hidden) )
        % num_hidden layers each of train_k size by default
        size_hidden = repmat(train_k, num_hidden, 1);
    end

    %train the first RBM on data
    top = +dsInitialization;
    %train all other RBM's on top of each other
    for ii=1:num_hidden
        [model{ii} , top ] = rbmBB(top, size_hidden(ii));
    end
    clear top

    model{num_hidden}.Wc = 0.1*randn(train_c, size(model{num_hidden}.W,2) );
    model{num_hidden}.cc = 0.1*randn(1, train_c);
    model{num_hidden}.labels = 1:train_c;

    %fine-tuning of the DBN
    targets = zeros(train_m, train_c);
    for ii = 1:train_c
        targets(train_labels == ii, ii) = 1;
    end

    for epoch = 1:20

        %%%%%%%%%%%%%%% PERFORM CONJUGATE GRADIENT WITH 3 LINESEARCHES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        max_iter=3;

        if epoch<6  % First update top-level weights holding other weights fixed.

            top = +dsTrain;
            for ii=1:num_hidden
                top = rbmVtoH(model{ii}, top);
            end

            VV = colvec([ model{num_hidden}.Wc'; model{num_hidden}.cc ]);
            Dim = [ size_hidden(end); train_c];
            XX = minimize(VV,'CG_CLASSIFY_INIT',max_iter,Dim,top,targets);
            aux = reshape(XX, size_hidden(end)+1, train_c);
            model{num_hidden}.Wc = aux(1:end-1,:)';
            model{num_hidden}.cc = aux(end,:);

        else

            VV = [];
            for ii=1:num_hidden
                VV = [VV; colvec([model{ii}.W; model{ii}.b ]) ];
            end
            VV = [VV; colvec([model{ii}.Wc'; model{ii}.cc ])];

            Dim = [ train_k; colvec(size_hidden); train_c];

            XX = minimize(VV,'CG_CLASSIFY',max_iter,Dim, +dsTrain,targets);

            xxx = 0;
            for ii=1:num_hidden
                aux = reshape(XX(xxx+1:xxx+(Dim(ii)+1)*Dim(ii+1)),Dim(ii)+1,Dim(ii+1));
                model{ii}.W = aux(1:end-1,:);
                model{ii}.b = aux(end,:);
                xxx = xxx+(Dim(ii)+1)*Dim(ii+1);
            end
            aux = reshape(XX(xxx+1:xxx+(Dim(ii+1)+1)*Dim(ii+2)),Dim(ii+1)+1,Dim(ii+2));
            model{ii}.Wc = aux(1:end-1,:)';
            model{ii}.cc = aux(end,:);

        end

    end

    % Create mapping.

    w = w_scale_domain*mapping('dbnPredict', 'trained', model, train_lablist, train_k, train_c);
    w = setname(w, 'DBN classifier');
%     w = setcost(w,a);

return


function TCP = jeongLungTCPmodel(paramS,doseBinsV,volHistV)
% Usage: TCP = jeongLungTCPmodel(paramS);
% Lung TCP model
% Based on code by Jeho Jeong jeongj@mskcc.org
%------------------------------------------------------------------------------------
% INPUTS:
% paramS : Parameter dictionary with fields
%          -frxSize (Fraction size (Gy)),
%          -treatmentSchedule(Vector indicating treatment days)
% Example:
% paramS.frxSize.val = 2;
% paramS.treatmentSchedule.val = [ 1  2  3  4  5 ...
%                           8  9 10 11 12 ...
%                           15 16 17 18 19 ...
%                           22 23 24 25 26];
%
%----------------------------------------------------------------------------
% AI 8/30/18

%% Get fractionation
fx_in = paramS.frxSize.val;
nFrx = paramS.numFractions.val;

%% Get treatment days
schedule_in = paramS.treatmentSchedule.val;
if  isfield(paramS,'scheduleType')
    scheduleType = paramS.scheduleType;
else
    scheduleType = 'weekday';
end
%Check for numeric input
if ~isnumeric(schedule_in)
    scheduleV = str2num(schedule_in);
else
    scheduleV = schedule_in;
end
%Otherwise, assume function specified
if isempty(scheduleV)
    scheduleV = eval([schedule_in,'(',num2str(nFrx),',scheduleType)']);
    %scheduleV = getTreatmentSchedule(nFrx); %every weekday with weekend
                                             %breaks
end

%% Input variables for the analysis
alpha_p_ori=0.305;
a_over_b=2.8;
oer_i=1.7;
rho_t=10^6;
v_t_ref=3e4;
f_s=0.01;
t_c=2;
f_p_pro_in=0.5;
ht_loss=2;
k_m=0.3;
ht_lys=3;
oer_h=1.37;
F_p_cyc=[0.56;0.24;0.2];
Alpha_ratio_p_cyc=[2;3];

d_t=15;
clf_in=0.92;
gf_in=0.25;

beta_p_ori=alpha_p_ori/a_over_b;

%% EQD2 estimation for each cohort

EQD2=[];
n_pt=[];
v_t=3e4;
alpha_p=alpha_p_ori;
beta_p=beta_p_ori;
n_t=rho_t*v_t;                %No. cells
n_t_ref=rho_t*v_t_ref;
total_clono_cell=n_t*f_s;
delta_t=d_t/(60*24);          %dt in days
t_start=0;

IC=[];
GF=[];
TCP=[];
TD50=[];
BED=[];
Reox_time=[];
Reox_time2=[];
Treat_duration=[];
vec_leng=[];
comp_size(1)=0;       %Compartment size (P)
comp_size(2)=0;       %Compartment size (H)
comp_size(3)=0;       %Compartment size (I)
comp_size_ref(1)=0;
comp_size_ref(2)=0;
comp_size_ref(3)=0;
p_pre=[];
i_pre=[];
h_pre=[];
T_end=[];

clf=clf_in;
gf=gf_in;

%% Run sub-routine for specific CLF and GF
%---Variables for the initial st-st distribution--%
f_p_pro=f_p_pro_in;

comp_size(1)=gf/f_p_pro*n_t;
comp_size(2)=(1-gf*(1/f_p_pro_in+clf*ht_loss/t_c))*n_t;
comp_size(3)=clf*gf*ht_loss/t_c*n_t;

comp_size_ref(1)=gf/f_p_pro*n_t_ref;
comp_size_ref(2)=(1-gf*(1/f_p_pro_in+clf*ht_loss/t_c))*n_t_ref;
comp_size_ref(3)=clf*gf*ht_loss/t_c*n_t_ref;
%--- end ----%

%Record  number of cells
f_p=comp_size(1)/sum(comp_size);
f_i=comp_size(2)/sum(comp_size);
f_h=comp_size(3)/sum(comp_size);

for d=fx_in
    Treat_day=scheduleV;

    %Cell cycle and dose-dependent radiosensitivity
    f = @(alpha_s)F_p_cyc(1)*exp(-Alpha_ratio_p_cyc(1)*alpha_s*2-...
        Alpha_ratio_p_cyc(1)*(alpha_s/a_over_b)*4)+F_p_cyc(2)*...
        exp(-alpha_s*2-(alpha_s/a_over_b)*4)+F_p_cyc(3)*...
        exp(-Alpha_ratio_p_cyc(2)*alpha_s*2-...
        Alpha_ratio_p_cyc(2)*(alpha_s/a_over_b)*4)-exp(-alpha_p*2-...
        (alpha_p/a_over_b)*4);

    alpha_s=0.3; grid=0.1; pre_f=f(alpha_s);
    while abs(f(alpha_s)) >= eps
        if pre_f*f(alpha_s)<0
            grid=grid*0.1;
        end
        pre_f = f(alpha_s);
        if f(alpha_s)>0
            alpha_s=alpha_s+grid;
        else
            alpha_s=alpha_s-grid;
        end
    end
    Alpha_p_cyc(2)= alpha_s;

    Alpha_p_cyc(1)=Alpha_p_cyc(2)*Alpha_ratio_p_cyc(1);
    Alpha_p_cyc(3)=Alpha_p_cyc(2)*Alpha_ratio_p_cyc(2);

    %Effective alpha, beta from survival fractions
    Su_p=F_p_cyc(1)*exp(-Alpha_p_cyc(1)*d-(Alpha_p_cyc(1)/a_over_b)*d^2)...
        +F_p_cyc(2)*exp(-Alpha_p_cyc(2)*d-(Alpha_p_cyc(2)/a_over_b)*d^2)...
        +F_p_cyc(3)*exp(-Alpha_p_cyc(3)*d-(Alpha_p_cyc(3)/a_over_b)*d^2);
    alpha_p_eff=-log(Su_p)/(d*(1+(d/a_over_b)));
    beta_p_eff=(alpha_p_eff/a_over_b);

    Su_i_2gy=exp(-alpha_p/oer_i*2-(alpha_p/a_over_b)/(oer_i^2)*2^2);
    oer_i_g1=(-(Alpha_p_cyc(1)*2)-sqrt((Alpha_p_cyc(1)*2)^2-...
        4*log(Su_i_2gy)*(Alpha_p_cyc(1)/a_over_b)*2^2))/(2*log(Su_i_2gy));
    Su_h_2gy=exp(-alpha_p/oer_h*2-(alpha_p/a_over_b)/(oer_h^2)*2^2);
    oer_h_g1=(-(Alpha_p_cyc(1)*2)-sqrt((Alpha_p_cyc(1)*2)^2-...
        4*log(Su_h_2gy)*(Alpha_p_cyc(1)/a_over_b)*2^2))/(2*log(Su_h_2gy));
    alpha_i=Alpha_p_cyc(1)/oer_i_g1;
    beta_i=(Alpha_p_cyc(1)/a_over_b)/(oer_i_g1^2);
    alpha_h=Alpha_p_cyc(1)/oer_h_g1;
    beta_h=(Alpha_p_cyc(1)/a_over_b)/(oer_h_g1^2);

    alpha_p=alpha_p_eff;
    beta_p=beta_p_eff;

    % Run sub-routine for a specific CLF and GF
    %--- RT fractional dose for SBRT schedule ---%

    % Assign proliferating fraction to the initial value
    f_p_pro=f_p_pro_in;

    % Cell distribution in each compartment
    % (1:Pv, 2:Pd, 3:Iv, 4:Id, 5:Hv, 6:Hd, 7:lysis)
    % Initially all compartments are fully filled with viable cells
    % "comp_size" is the size of each compartment (1:P, 2:I, 3:H)

    cell_dist=[];
    cell_dist(1)=comp_size(1);
    cell_dist(2)=0;
    cell_dist(3)=comp_size(2);
    cell_dist(4)=0;
    cell_dist(5)=comp_size(3);
    cell_dist(6)=0;
    cell_dist(7)=0;

    % variables (t:time(day), j:# of fraction, add_time:additional time for
    %           weekend break, cum_cell_dist: cumulative cell distribution for
    %           each time increment)
    t=0;
    j=0;
    cum_cell_dist_sbrt=[];

    % Treat for specific SBRT schedule
    while t<t_start+(max(Treat_day)-1)+delta_t/2

        % Change in f_p_pro (k_p) as blood supply improves
        f_p_pro=1-0.5*(cell_dist(1)+cell_dist(2))/comp_size(1);

        % RT fraction
        if  t>(t_start+(Treat_day(j+1)-1)-delta_t/2) &&...
                t<(t_start+(Treat_day(j+1)-1)+delta_t/2)

            cell_dist(2)=cell_dist(2)+cell_dist(1)*(1-exp(-alpha_p*d-beta_p*d^2));
            cell_dist(1)=cell_dist(1)*exp(-alpha_p*d-beta_p*d^2);
            cell_dist(4)=cell_dist(4)+cell_dist(3)*(1-exp(-alpha_i*d-beta_i*d^2));
            cell_dist(3)=cell_dist(3)*exp(-alpha_i*d-beta_i*d^2);
            cell_dist(6)=cell_dist(6)+cell_dist(5)*(1-exp(-alpha_h*d-beta_h*d^2));
            cell_dist(5)=cell_dist(5)*exp(-alpha_h*d-beta_h*d^2);

            j=j+1;
        end

        % Cell Proliferation & Death
        cell_dist(1)=cell_dist(1)*(2)^(f_p_pro*delta_t/t_c);
        h_pre=cell_dist(5)+cell_dist(6);
        cell_dist(5)=cell_dist(5)*(0.5)^(delta_t/ht_loss);
        cell_dist(6)=cell_dist(6)*(0.5)^(delta_t/ht_loss);
        p_d_pre=cell_dist(2);
        cell_dist(2)=cell_dist(2)*(2)^(f_p_pro*(2*k_m-1)*delta_t/t_c);

        % Mitotically dead cell in 1 time step
        md=p_d_pre-cell_dist(2)+(h_pre-cell_dist(5)-cell_dist(6));
        cell_dist(7)=cell_dist(7)+md;
        cell_dist(7)=cell_dist(7)*(0.5)^(delta_t/ht_lys);

        % Recompartmentalization of the cell
        if cell_dist(1)+cell_dist(2)>=comp_size(1)
            p_ex=(cell_dist(1)+cell_dist(2))-comp_size(1);
            p_ratio=cell_dist(1)/(cell_dist(1)+cell_dist(2));
            cell_dist(1)=comp_size(1)*p_ratio;
            cell_dist(2)=comp_size(1)*(1-p_ratio);
            cell_dist(3)=cell_dist(3)+p_ex*p_ratio;
            cell_dist(4)=cell_dist(4)+p_ex*(1-p_ratio);
        else
            if cell_dist(3)+cell_dist(4)>0
                if cell_dist(3)+cell_dist(4)>comp_size(1)-...
                        (cell_dist(1)+cell_dist(2))
                    p_def=comp_size(1)-(cell_dist(1)+cell_dist(2));
                    i_ratio=cell_dist(3)/(cell_dist(3)+cell_dist(4));
                    cell_dist(1)=cell_dist(1)+p_def*i_ratio;
                    cell_dist(2)=cell_dist(2)+p_def*(1-i_ratio);
                    cell_dist(3)=cell_dist(3)-p_def*i_ratio;
                    cell_dist(4)=cell_dist(4)-p_def*(1-i_ratio);
                else
                    cell_dist(1)=cell_dist(1)+cell_dist(3);
                    cell_dist(2)=cell_dist(2)+cell_dist(4);
                    cell_dist(3)=0; cell_dist(4)=0;
                    if cell_dist(5)+cell_dist(6)>0
                        if cell_dist(5)+cell_dist(6)>comp_size(1)-...
                                (cell_dist(1)+cell_dist(2))
                            p_def=comp_size(1)-(cell_dist(1)+cell_dist(2));
                            h_ratio=cell_dist(5)/(cell_dist(5)+cell_dist(6));
                            cell_dist(1)=cell_dist(1)+p_def*h_ratio;
                            cell_dist(2)=cell_dist(2)+p_def*(1-h_ratio);
                            cell_dist(5)=cell_dist(5)-p_def*h_ratio;
                            cell_dist(6)=cell_dist(6)-p_def*(1-h_ratio);
                        else
                            cell_dist(1)=cell_dist(1)+cell_dist(5);
                            cell_dist(2)=cell_dist(2)+cell_dist(6);
                            cell_dist(5)=0; cell_dist(6)=0;
                        end
                    end
                end
            end
        end
        if cell_dist(3)+cell_dist(4)>=comp_size(2)
            i_ex=(cell_dist(3)+cell_dist(4))-comp_size(2);
            i_ratio=cell_dist(3)/(cell_dist(3)+cell_dist(4));
            cell_dist(3)=comp_size(2)*i_ratio;
            cell_dist(4)=comp_size(2)*(1-i_ratio);
            cell_dist(5)=cell_dist(5)+i_ex*i_ratio;
            cell_dist(6)=cell_dist(6)+i_ex*(1-i_ratio);
        else
            if cell_dist(5)+cell_dist(6)>0
                if cell_dist(5)+cell_dist(6)>comp_size(2)-...
                        (cell_dist(3)+cell_dist(4))
                    i_def=comp_size(2)-(cell_dist(3)+cell_dist(4));
                    h_ratio=cell_dist(5)/(cell_dist(5)+cell_dist(6));
                    cell_dist(3)=cell_dist(3)+i_def*h_ratio;
                    cell_dist(4)=cell_dist(4)+i_def*(1-h_ratio);
                    cell_dist(5)=cell_dist(5)-i_def*h_ratio;
                    cell_dist(6)=cell_dist(6)-i_def*(1-h_ratio);
                else
                    cell_dist(3)=cell_dist(3)+cell_dist(5);
                    cell_dist(4)=cell_dist(4)+cell_dist(6);
                    cell_dist(5)=0; cell_dist(6)=0;
                end
            end
        end

        % time step increase and store the number of cells in each compartment
        t=t+delta_t;
        cum_cell_dist_sbrt=[cum_cell_dist_sbrt cell_dist'];

    end

    s_sbrt=cell_dist(1)+cell_dist(3)+cell_dist(5);
    sf_sbrt=s_sbrt/sum(comp_size);
    ntd2=length(Treat_day)*d*(1+(d/a_over_b))/(1+(2/a_over_b));
    d_sbrt=d;
    n_frac_sbrt=length(Treat_day);
    duration_sbrt=max(Treat_day);
    t_sbrt=t;
    %-------------------------------------------------------------------------------%

    %% EQD2 calculation
    d=2;
    alpha_p=alpha_p_ori;            beta_p=beta_p_ori;
    alpha_i=alpha_p_ori/oer_i;      beta_i=beta_p_ori/(oer_i^2);
    alpha_h=alpha_p_ori/oer_h;      beta_h=beta_p_ori/(oer_h^2);
    s_eqd2=0;       sf_eqd2=0;      eqd2=0;

    %----- RT fractional dose for EQD2 estimation ----%

    % Assign proliferating fraction to the initial value

    f_p_pro=f_p_pro_in;

    % Cell distribution in each compartment
    % (1:Pv, 2:Pd, 3:Iv, 4:Id, 5:Hv, 6:Hd, 7:lysis)
    % Initially all compartments are fully filled with viable cells
    % "comp_size" is the size of each compartment (1:P, 2:I, 3:H)

    cell_dist=[];
    cell_dist(1)=comp_size_ref(1);
    cell_dist(2)=0;
    cell_dist(3)=comp_size_ref(2);
    cell_dist(4)=0;
    cell_dist(5)=comp_size_ref(3);
    cell_dist(6)=0;
    cell_dist(7)=0;

    % variables (t:time(day), j:# of fraction, add_time:additional time for
    %           weekend break, cum_cell_dist: cumulative cell distribution for
    %           each time increment)
    t=0;
    j=0;
    add_time=0;
    cum_cell_dist=[];

    % Treat until the SF becomes equivalent to SBRT regime
    while (cell_dist(1)+cell_dist(3)+cell_dist(5))>s_sbrt

        % Change in f_p_pro (k_p) as blood supply improves
        f_p_pro=1-0.5*(cell_dist(1)+cell_dist(2))/comp_size(1);

        % RT fraction
        if  t>(t_start+j+add_time-delta_t/2) && t<(t_start+j+add_time+delta_t/2)

            cell_dist(2)=cell_dist(2)+cell_dist(1)*(1-exp(-alpha_p*d-beta_p*d^2));
            cell_dist(1)=cell_dist(1)*exp(-alpha_p*d-beta_p*d^2);
            cell_dist(4)=cell_dist(4)+cell_dist(3)*(1-exp(-alpha_i*d-beta_i*d^2));
            cell_dist(3)=cell_dist(3)*exp(-alpha_i*d-beta_i*d^2);
            cell_dist(6)=cell_dist(6)+cell_dist(5)*(1-exp(-alpha_h*d-beta_h*d^2));
            cell_dist(5)=cell_dist(5)*exp(-alpha_h*d-beta_h*d^2);

            j=j+1;

            % Week-end break
            if rem(j,5)==0
                add_time=add_time+2;
            end

        end

        % Cell Proliferation & Death
        cell_dist(1)=cell_dist(1)*(2)^(f_p_pro*delta_t/t_c);
        h_pre=cell_dist(5)+cell_dist(6);
        cell_dist(5)=cell_dist(5)*(0.5)^(delta_t/ht_loss);
        cell_dist(6)=cell_dist(6)*(0.5)^(delta_t/ht_loss);
        p_d_pre=cell_dist(2);
        cell_dist(2)=cell_dist(2)*(2)^(f_p_pro*(2*k_m-1)*delta_t/t_c);

        % Mitotically dead cell in 1 time step
        md=p_d_pre-cell_dist(2)+(h_pre-cell_dist(5)-cell_dist(6));
        cell_dist(7)=cell_dist(7)+md;
        cell_dist(7)=cell_dist(7)*(0.5)^(delta_t/ht_lys);

        % Recompartmentalization of the cell
        if cell_dist(1)+cell_dist(2)>=comp_size(1)
            p_ex=(cell_dist(1)+cell_dist(2))-comp_size(1);
            p_ratio=cell_dist(1)/(cell_dist(1)+cell_dist(2));
            cell_dist(1)=comp_size(1)*p_ratio;
            cell_dist(2)=comp_size(1)*(1-p_ratio);
            cell_dist(3)=cell_dist(3)+p_ex*p_ratio;
            cell_dist(4)=cell_dist(4)+p_ex*(1-p_ratio);
        else
            if cell_dist(3)+cell_dist(4)>0
                if cell_dist(3)+cell_dist(4)>comp_size(1)-...
                        (cell_dist(1)+cell_dist(2))
                    p_def=comp_size(1)-(cell_dist(1)+cell_dist(2));
                    i_ratio=cell_dist(3)/(cell_dist(3)+cell_dist(4));
                    cell_dist(1)=cell_dist(1)+p_def*i_ratio;
                    cell_dist(2)=cell_dist(2)+p_def*(1-i_ratio);
                    cell_dist(3)=cell_dist(3)-p_def*i_ratio;
                    cell_dist(4)=cell_dist(4)-p_def*(1-i_ratio);
                else
                    cell_dist(1)=cell_dist(1)+cell_dist(3);
                    cell_dist(2)=cell_dist(2)+cell_dist(4);
                    cell_dist(3)=0; cell_dist(4)=0;
                    if cell_dist(5)+cell_dist(6)>0
                        if cell_dist(5)+cell_dist(6)>comp_size(1)-...
                                (cell_dist(1)+cell_dist(2))
                            p_def=comp_size(1)-(cell_dist(1)+cell_dist(2));
                            h_ratio=cell_dist(5)/(cell_dist(5)+cell_dist(6));
                            cell_dist(1)=cell_dist(1)+p_def*h_ratio;
                            cell_dist(2)=cell_dist(2)+p_def*(1-h_ratio);
                            cell_dist(5)=cell_dist(5)-p_def*h_ratio;
                            cell_dist(6)=cell_dist(6)-p_def*(1-h_ratio);
                        else
                            cell_dist(1)=cell_dist(1)+cell_dist(5);
                            cell_dist(2)=cell_dist(2)+cell_dist(6);
                            cell_dist(5)=0; cell_dist(6)=0;
                        end
                    end
                end
            end
        end
        if cell_dist(3)+cell_dist(4)>=comp_size(2)
            i_ex=(cell_dist(3)+cell_dist(4))-comp_size(2);
            i_ratio=cell_dist(3)/(cell_dist(3)+cell_dist(4));
            cell_dist(3)=comp_size(2)*i_ratio;
            cell_dist(4)=comp_size(2)*(1-i_ratio);
            cell_dist(5)=cell_dist(5)+i_ex*i_ratio;
            cell_dist(6)=cell_dist(6)+i_ex*(1-i_ratio);
        else
            if cell_dist(5)+cell_dist(6)>0
                if cell_dist(5)+cell_dist(6)>comp_size(2)-...
                        (cell_dist(3)+cell_dist(4))
                    i_def=comp_size(2)-(cell_dist(3)+cell_dist(4));
                    h_ratio=cell_dist(5)/(cell_dist(5)+cell_dist(6));
                    cell_dist(3)=cell_dist(3)+i_def*h_ratio;
                    cell_dist(4)=cell_dist(4)+i_def*(1-h_ratio);
                    cell_dist(5)=cell_dist(5)-i_def*h_ratio;
                    cell_dist(6)=cell_dist(6)-i_def*(1-h_ratio);
                else
                    cell_dist(3)=cell_dist(3)+cell_dist(5);
                    cell_dist(4)=cell_dist(4)+cell_dist(6);
                    cell_dist(5)=0; cell_dist(6)=0;
                end
            end
        end

        % time step increase and store the number of cells in each compartment
        t=t+delta_t;
        cum_cell_dist=[cum_cell_dist cell_dist'];

        s_eqd2_pre=s_eqd2;
        sf_eqd2_pre=sf_eqd2;
        eqd2_pre=eqd2;

        s_eqd2=cell_dist(1)+cell_dist(3)+cell_dist(5);
        sf_eqd2=s_eqd2/sum(comp_size);
        tcp=exp(-s_eqd2*f_s);
        eqd2=j*d;

    end

    %----------------------------------------------------------------------%

    eqd2=eqd2_pre+((eqd2-eqd2_pre)/(s_eqd2_pre-s_eqd2))*(s_eqd2_pre-s_sbrt);

end

%% Compute TCP
TD_50 = 62.1;
gamma_50 = 1.5;
TCP_upper_bound = 0.95;

TCP=TCP_upper_bound/(1+(TD_50/eqd2)^(4*gamma_50));


function DoToHDF5(Simuh)

% convert MRiLab output to HDF5 file which works with Gadgetron
global VCtl
global VSig
global VCoi
global VObj

vctl.protocolName               = VCtl.SeriesName;
vctl.systemVendor               = 'MRiLab';
vctl.systemModel                = '2013a';
vctl.systemFieldStrength_T      = single(VCtl.B0);
vctl.receiverChannels           = uint16(VCoi.RxCoilNum);
vctl.institutionName            = 'WIMR';
vctl.stationName                = 'L1122-E';
vctl.H1resonanceFrequency_Hz    = uint32(VCtl.B0 * VObj.Gyro/(2*pi));
vctl.ESMatrixSizeX              = uint16(VCtl.ResFreq);
vctl.ESMatrixSizeY              = uint16(VCtl.ResPhase);
vctl.ESMatrixSizeZ              = uint16(VCtl.SliceNum);
vctl.ESFOVX                     = single(VCtl.FOVFreq  * 1000);
vctl.ESFOVY                     = single(VCtl.FOVPhase * 1000);
vctl.ESFOVZ                     = single(VCtl.FOVSlice * 1000);
vctl.RSMatrixSizeX              = uint16(VCtl.ResFreq);
vctl.RSMatrixSizeY              = uint16(VCtl.ResPhase);
vctl.RSMatrixSizeZ              = uint16(VCtl.SliceNum);
vctl.RSFOVX                     = single(VCtl.FOVFreq  * 1000);
vctl.RSFOVY                     = single(VCtl.FOVPhase * 1000);
vctl.RSFOVZ                     = single(VCtl.FOVSlice * 1000);
vctl.trajectory                 = VCtl.TrajType;
vctl.TR                         = single(VCtl.TR);
vctl.TE                         = single(VCtl.TE);
vctl.BW                         = single(VCtl.BandWidth);
vctl.outputFile                 = [Simuh.OutputDir filesep 'Series' num2str(Simuh.ScanSeriesInd) '.h5'];

Sx=single(sum(reshape(VSig.Sx, length(VSig.Sx)/VObj.TypeNum, VObj.TypeNum),2));
Sy=single(sum(reshape(VSig.Sy, length(VSig.Sy)/VObj.TypeNum, VObj.TypeNum),2));
Kx=single(VSig.Kx);
Ky=single(VSig.Ky);
Kz=single(VSig.Kz);
vsig.S                          = [Sx'; Sy'];
vsig.K                          = [Kx; Ky; Kz];
vsig.echoNumber                 = uint32(VCtl.TEPerTR);
vsig.firstPhaseNumber           = uint32(VCtl.FirstPhNum);
vsig.secondPhaseNumber          = uint32(VCtl.SecondPhNum);
vsig.readoutNumber              = uint32(length(VSig.Sx)/(VCtl.TEPerTR*VCtl.FirstPhNum*VCtl.SecondPhNum*VCoi.RxCoilNum*VObj.TypeNum));

% convert MRiLab output to HDF5 file
DoMatToHDF5(vsig,vctl);

end


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% dslam_open is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with dslam_open. If not, see <http://www.gnu.org/licenses/>.

function writeG2oPose(file_id, robot_idx, frame_idx, T_W_C)

frame_id = gtsamFrameId(robot_idx, frame_idx);

x = T_W_C(1, 4); y = T_W_C(2, 4); z = T_W_C(3, 4);
R = T_W_C(1:3, 1:3);
assert(sum(sum(imag(R))) == 0);
% Seems to be necessary to avoid complex quaternions
% (to avoid trace(R) < -1)
R = R / (det(R) + 1e-5);
q = rot2quat(R, 1e-4);
%quat2rot(q) - R
%assert(all(all(quat2rot(q) - R < 1e-5)));
assert(norm(q)>1e-3);
q = q/norm(q);
assert(norm(q)>1e-3);
qw = q(1); qx = q(2); qy = q(3); qz = q(4);
if (sum(imag(q)) ~= 0)
    q
    assert(false)
end

fprintf(file_id, ...
    'VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n', ...
    frame_id, x, y, z, qx, qy, qz, qw);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Objectness measure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Top-level routine implementation
% of the system described in the paper:
%
% B. Alexe, T. Deselaers, V. Ferrari
% What is an object?
% CVPR 2010
%
% The author and copyright holder is Bogdan Alexe.
% You might redistribute, reuse, and mofidy this code for
% research purposes. Any commercial application
% is forbidden without prior agreement in written
% by the author.

function boxes = runObjectness(img,numberProposals,params)
%This function computes the objectness measure and samples boxes from it.

%INPUT
%img - input image
%numberProposals - number of windows sampled from the objectness measure
%params - struct containing parameters of the function (loaded in startup.m)

%OUTPUT
%boxes - samples windows from the objectness measure
%      - each row contains a window using the format [xmin ymin xmax ymax score]

%dir_root = rootpath;%change this to an absolute path

img = gray2rgb(img);
%{
if nargin < 3
    try
        struct = load([dir_root '/Data/params.mat']);
        params = struct.params;
        clear struct;
    catch
        params = defaultParams(dir_root);
        save([dir_root '/Data/params.mat'],'params');
    end
end
%params = updatePath(dir_root,params);
%}
if length(params.cues)==1
    %single cues

    distributionBoxes = computeScores(img,params.cues{1},params);

    switch lower(params.sampling)
        case 'nms'
            %nms sampling

            %consider only params.distribution_windows (= 100k windows)
            if size(distributionBoxes,1) > params.distribution_windows
                indexSamples = scoreSampling(distributionBoxes(:,5),params.distribution_windows,1);
                distributionBoxes = distributionBoxes(indexSamples,:);
            end

            %sampling
            boxes = nms_pascal(distributionBoxes, 0.5,numberProposals);

        case 'multinomial'
            %multinomial sampling

            %sample from the distribution of the scores
            indexSamples = scoreSampling(distributionBoxes(:,end),numberProposals,1);
            boxes = distributionBoxes(indexSamples,:);

        otherwise
            display('sampling procedure unknown')
    end

else
    %combination of cues

    if not(ismember('MS',params.cues))
        display('ERROR: combinations have to include MS');
        boxes = [];
        return
    end

    if length(unique(params.cues)) ~= length(params.cues)
        display('ERROR: repetead cues in the combination');
        boxes = [];
        return
    end

    distributionBoxes = computeScores(img,'MS',params);
    %rearrange the cues such that 'MS' is the first cue
    if ~strcmp(params.cues{1},'MS')
        params.cues{strcmp(params.cues,'MS')} = params.cues{1};
        params.cues{1} ='MS';
    end

    score = zeros(size(distributionBoxes,1),length(params.cues));
    score(:,1) = distributionBoxes(:,end);
    windows = distributionBoxes(:,1:4);
    for idx = 2:length(params.cues)
        temp = computeScores(img,params.cues{idx},params,windows);
        score(:,idx) = temp(:,end);
    end
    scoreBayes = integrateBayes(params.cues,score,params);

    switch lower(params.sampling)
        case 'nms'
            %nms sampling

            distributionBoxes(:,5) = scoreBayes;
            boxes = nms_pascal(distributionBoxes, 0.5, numberProposals);

        case 'multinomial'
            %multinomial sampling

            %sample from the distribution of the scores
            indexSamples = scoreSampling(scoreBayes,numberProposals,1);
        boxes = [windows(indexSamples,:) scoreBayes(indexSamples,:)];

        otherwise
            display('sampling procedure unknown')
    end

end


%==================================================================
%                        General Data File
% Title: Default_title
% Units: SI
% Dimensions: 2D
% Type of problem: Plane_Stress
% Type of Phisics: Stokes
% Micro/Macro: MACRO
%
%==================================================================

%% Data
order={'QUADRATIC','LINEAR'};
Data_prb = {
'TRIANGLE';
'SI';
'2D';
'Plane_Stress';
'Stokes';
'MACRO'
};
state = 'Transient';
dtime = 0.01;
finalTime = 1;

%% Coordinates
% Node                X                Y                Z

coord = [
1            0            0            0
2            0         0.25            0
3         0.25            0            0
4         0.25         0.25            0
5            0          0.5            0
6          0.5            0            0
7         0.25          0.5            0
8          0.5         0.25            0
9          0.5          0.5            0
10         0.75            0            0
11            0         0.75            0
12         0.75         0.25            0
13         0.25         0.75            0
14         0.75          0.5            0
15          0.5         0.75            0
16            1            0            0
17            0            1            0
18            1         0.25            0
19         0.25            1            0
20         0.75         0.75            0
21            1          0.5            0
22          0.5            1            0
23         0.75            1            0
24            1         0.75            0
25            1            1            0
];

%% Conectivities
% Element        Node(1)                Node(2)                Node(3)                Material

connec = [
1 6 8 3 0
2 8 9 4 0
3 3 4 1 0
4 8 4 3 0
5 9 7 4 0
6 7 5 2 0
7 4 2 1 0
8 7 2 4 0
9 16 18 10 0
10 18 21 12 0
11 10 12 6 0
12 18 12 10 0
13 21 14 12 0
14 14 9 8 0
15 12 8 6 0
16 14 8 12 0
17 9 15 7 0
18 15 22 13 0
19 7 13 5 0
20 15 13 7 0
21 22 19 13 0
22 19 17 11 0
23 13 11 5 0
24 19 11 13 0
25 21 24 14 0
26 24 25 20 0
27 14 20 9 0
28 24 20 14 0
29 25 23 20 0
30 23 22 15 0
31 20 15 9 0
32 23 15 20 0
];

%% Variable Prescribed
% Node            Dimension                Value

velocityBC.domain = 'Border';
velocityBC.value  = 0;

velocity = [
1 1 0
1 2 0
5 1 0
5 2 0
6 1 0
6 2 0
16 1 0
16 2 0
17 1 0
17 2 0
21 1 0
21 2 0
22 1 0
22 2 0
25 1 0
25 2 0
];

pressure = [
25 1 0
];

nu=1;
Vol_force = @(x,y){nu*4*(x^3*((6 - 12*y)) + x^4*((-3 + 6*y)) + y*(1 - 3*y + 2*y^2)-6*x*y*(1 - 3*y + 2*y^2)...
+ 3*x^2*((-1 + 4*y - 6*y^2 + 4*y^3)))+(2*x-1)*(y-1);
-4*nu*(-3*((-1 + y)^2)*y^2 - 3*x^2*((1 - 6*y + 6*y^2))+2*x^3*((1 - 6*y + 6*y^2)) +...
x*(1 - 6*y + 12*y^2 - 12*y^3 + 6*y^4))+x*(x-1)};

% nu=1;
% Vol_force = @(x,y){nu*4*(x^3*((6 - 12*y)) + x^4*((-3 + 6*y)) + y*(1 - 3*y + 2*y^2)-6*x*y*(1 - 3*y + 2*y^2)...
% + 3*x^2*((-1 + 4*y - 6*y^2 + 4*y^3)))+nu*y*(1 - y)*(1 - 2*x);
% -4*nu*(-3*((-1 + y)^2)*y^2 - 3*x^2*((1 - 6*y + 6*y^2))+2*x^3*((1 - 6*y + 6*y^2)) +...
% x*(1 - 6*y + 12*y^2 - 12*y^3 + 6*y^4)+x*(1 - x)*(1 - 2*y))};

%% Group Elements
% Element        Group_num

Group = [
];

%% Initial Holes
% Elements that are considered holes initially
% Element

Initial_holes = [
];

%% Boundary Elements
% Elements that can not be removed
% Element

Boundary_elements = [
];

%% Micro gauss post
%
% Element

Micro_gauss_post = [
];

%% Micro Slave-Master
% Nodes that are Slaves
% Nodes             Value (1-Slave,0-Master)

Micro_slave = [
];

%% Nodes solid
% Nodes that must remain
% Nodes

% nodesolid = unique(pointload_complete(:,1));

%% External border Elements
% Detect the elements that define the edge of the domain
% Element               Node(1)           Node(2)

External_border_elements = [
1 3 6
3 1 3
6 5 2
7 2 1
9 16 18
9 10 16
10 18 21
11 6 10
21 22 19
22 17 11
22 19 17
23 11 5
25 21 24
26 24 25
29 25 23
30 23 22
];

%% External border Nodes
% Detect the nodes that define the edge of the domain
% Node

External_border_nodes = [
];

%% Materials
% Materials that have been used
% Material_Num              Mat_density        Young_Modulus        Poisson

Materials = [
];


function element_node = gmsh_mesh2d_element_data_example ( element_num, ...
  element_order )

%*****************************************************************************80
%
%% GMSH_MESH2D_ELEMENT_DATA_EXAMPLE returns element information for the example.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Output, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM),
%    the indices of the nodes that make up each element.
%
  element_node = [ ...
    1,  2,  6; ...
    7,  6,  2; ...
    2,  3,  7; ...
    8,  7,  3; ...
    3,  4,  8; ...
    9,  8,  4; ...
    4,  5,  9; ...
   10,  9,  5; ...
    6,  7, 11; ...
   12, 11,  7; ...
    7,  8, 12; ...
   13, 12,  8; ...
    8,  9, 13; ...
   14, 13,  9; ...
    9, 10, 14; ...
   15, 14, 10; ...
   11, 12, 16; ...
   17, 16, 12; ...
   12, 13, 17; ...
   18, 17, 13; ...
   16, 17, 19; ...
   20, 19, 17; ...
   17, 18, 20; ...
   21, 20, 18 ]';

  return
end


function yp = p00_fun ( test, neqn, t, y )

%*****************************************************************************80
%
%% P00_FUN evaluates the right hand side of any test problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TEST, the test problem index.
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, the value of the independent variable.
%
%    Input, real Y(NEQN,1), the value of the dependent variables.
%
%    Output, real YP(NEQN,1), the value of the derivative of the
%    dependent variables, as specified by the ODE.
%
  yp = zeros ( neqn, 1 );

  if ( test == 1 )
    yp = p01_fun ( neqn, t, y );
  elseif ( test == 2 )
    yp = p02_fun ( neqn, t, y );
  elseif ( test == 3 )
    yp = p03_fun ( neqn, t, y );
  elseif ( test == 4 )
    yp = p04_fun ( neqn, t, y );
  elseif ( test == 5 )
    yp = p05_fun ( neqn, t, y );
  elseif ( test == 6 )
    yp = p06_fun ( neqn, t, y );
  elseif ( test == 7 )
    yp = p07_fun ( neqn, t, y );
  elseif ( test == 8 )
    yp = p08_fun ( neqn, t, y );
  elseif ( test == 9 )
    yp = p09_fun ( neqn, t, y );
  elseif ( test == 10 )
    yp = p10_fun ( neqn, t, y );
  elseif ( test == 11 )
    yp = p11_fun ( neqn, t, y );
  elseif ( test == 12 )
    yp = p12_fun ( neqn, t, y );
  elseif ( test == 13 )
    yp = p13_fun ( neqn, t, y );
  elseif ( test == 14 )
    yp = p14_fun ( neqn, t, y );
  elseif ( test == 15 )
    yp = p15_fun ( neqn, t, y );
  elseif ( test == 16 )
    yp = p16_fun ( neqn, t, y );
  elseif ( test == 17 )
    yp = p17_fun ( neqn, t, y );
  elseif ( test == 18 )
    yp = p18_fun ( neqn, t, y );
  elseif ( test == 19 )
    yp = p19_fun ( neqn, t, y );
  elseif ( test == 20 )
    yp = p20_fun ( neqn, t, y );
  elseif ( test == 21 )
    yp = p21_fun ( neqn, t, y );
  elseif ( test == 22 )
    yp = p22_fun ( neqn, t, y );
  elseif ( test == 23 )
    yp = p23_fun ( neqn, t, y );
  elseif ( test == 24 )
    yp = p24_fun ( neqn, t, y );
  elseif ( test == 25 )
    yp = p25_fun ( neqn, t, y );
  elseif ( test == 26 )
    yp = p26_fun ( neqn, t, y );
  elseif ( test == 27 )
    yp = p27_fun ( neqn, t, y );
  elseif ( test == 28 )
    yp = p28_fun ( neqn, t, y );
  elseif ( test == 29 )
    yp = p29_fun ( neqn, t, y );
  elseif ( test == 30 )
    yp = p30_fun ( neqn, t, y );
  elseif ( test == 31 )
    yp = p31_fun ( neqn, t, y );
  elseif ( test == 32 )
    yp = p32_fun ( neqn, t, y );
  elseif ( test == 33 )
    yp = p33_fun ( neqn, t, y );
  elseif ( test == 34 )
    yp = p34_fun ( neqn, t, y );
  elseif ( test == 35 )
    yp = p35_fun ( neqn, t, y );
  elseif ( test == 36 )
    yp = p36_fun ( neqn, t, y );
  elseif ( test == 37 )
    yp = p37_fun ( neqn, t, y );
  elseif ( test == 38 )
    yp = p38_fun ( neqn, t, y );
  elseif ( test == 39 )
    yp = p39_fun ( neqn, t, y );
  elseif ( test == 40 )
    yp = p40_fun ( neqn, t, y );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_FUN - Fatal error!\n' );
    fprintf ( 1, '  Illegal problem index TEST = %d\n', test );
    error ( 'P00_FUN - Fatal error!' );
  end

  return
end


% DEMHUMANPOSESVARGPLVMMISSING Run the shared variational GPLVM on various kinds of data
% allowing missing inputs.
% DESC Run the shared variational GPLVM on various kinds of data
% allowing missing inputs.
%
% COPYRIGHT: Andreas C. Damianou, Carl Henrik Ek,  2011
% SEEALSO : demHumanPoseSvargplvm1
%
% VARGPLVM

% Human pose data with the whole silhouette
%clear ;close all; experimentNo=404; imageSilhouette=1;initLatent='ppca';
%latentDimPerModel=7;dataSetNames =
%{};toyDataCreate='humanPose';initVardistIters = 380;
%itNo = [500 200 200 200 200 200 200 200 200 200];  indPoints=100;TEMPdemSharedVargplvm

%___

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

% Define constants (in a manner that allows other scripts to parametrize
% this one).
if ~exist('experimentNo') ,  experimentNo = 404;      end
if ~exist('itNo')         ,  itNo = [500 500];              end     % Default: 2000
if ~exist('indPoints')    ,  indPoints = 100;          end     % Default: 49
if ~exist('latentDimPerModel')    ,  latentDimPerModel = 6;          end
% Set to 1 to use dynamics or to 0 to use the standard var-GPLVM
if ~exist('dynUsed')      ,  dynUsed = 0;             end
if ~exist('initVardistIters'), initVardistIters = 180;      end
if ~exist('dynamicKern')   ,     dynamicKern = {'rbf', 'white', 'bias'}; end
if ~exist('mappingKern')   ,  mappingKern = {'rbfard2', 'white', 'bias'}; end

% 0.1 gives around 0.5 init.covars. 1.3 biases towards 0.
if ~exist('vardistCovarsMult'),  vardistCovarsMult=1.3;                  end
% Set to empty value {} to work with toy data

if ~exist('invWidthMultDyn'),    invWidthMultDyn = 100;                     end
if ~exist('invWidthMult'),       invWidthMult = 5;                     end
if ~exist('initX'),     initX ='ppca';   end % That's for the dynamics initialisation
if ~exist('dataType'), dataType = 'humanPose'; end
if ~exist('enableParallelism'), enableParallelism = 1; end
if ~exist('DgtN'), DgtN = false; end
% Create initial X by doing e.g. ppca in the concatenated model.m's or by
% doing ppca in the model.m's separately and concatenate afterwards?
if ~exist('initial_X'), initial_X = 'separately'; end % Other options: 'together'
% Which indices to use for training, rest for test
if ~exist('indTr'), indTr = -1; end

demHumanPosePrepareData

dataSetNames={'silhouette', 'pose'};
% X_init = Xp;
%mappingKern = {'linard2', 'white'};
%mappingKern = {'rbfard2', 'white'};
latentDim = 5; % Anything > 2 and < 10
%xyzankurAnim(Z_test, 3);
numberOfDatasets = length(Yall);

%-- Load datasets
for i=1:numberOfDatasets
    Y = Yall{i};

    dims{i} = size(Y,2);
    N{i} = size(Y,1);
    if indTr == -1
        indTr = 1:N{i};
    end
    %t{i} = linspace(0, 2*pi, size(Yall{i}, 1)+1)'; t{i} = t{i}(1:end-1, 1);

    indTs = setdiff(1:size(Y,1), indTr);
    Ytr{i} = Y(indTr,:);
    Yts{i} = Y(indTs,:);

    d{i} = size(Ytr{i}, 2);
end
% timeStampsTraining = t{1}(indTr,1); %timeStampsTest = t(indTs,1);

t = linspace(0, 2*pi, length(indTr)+1)'; t = t(1:end-1, 1);

% Fix times:
prevSeq = 1;
timeStampsTraining = [];
dt=0.05;
for i=1:length(seq)
    t = ([0:(seq(i)-prevSeq)].*dt)';
    prevSeq = seq(i)+1;
    timeStampsTraining = [timeStampsTraining ;t];
end;

dt=t(2)-t(1);
timeStampsTest = ([0:size(Y_test,1)-1].*dt)';

for i=2:numberOfDatasets
    if N{i} ~= N{i-1}
        error('The number of observations in each dataset must be the same!');
    end
end

model = svargplvmRestorePrunedModel(prunedModel, Ytr);
if isfield(model, 'dynamics') & ~isempty(model.dynamics)
    dynUsed=1;
else
    dynUsed = 0;
end
makePlots=0;

%%
%---------------------------- PREDICTIONS ---------------

% Set to 1 to test on the training data itself, set to 0 to use the test
% dataset.
if ~exist('testOnTraining')
    testOnTraining=0;
end

% 1 is for the HoG image features. 2 is for the pose features.
obsMod = 1; % one of the involved sub-models (possible values: 1 or 2).
infMod = setdiff(1:2, obsMod);

% Find the dimensions that are shared for obsMod and infMod
if ~exist('sharedDims')
    s1 = model.comp{obsMod}.kern.comp{1}.inputScales;
    s2 = model.comp{infMod}.kern.comp{1}.inputScales;
    % Normalise values between 0 and 1
    s1 = s1 / max(s1);
    s2 = s2 / max(s2);

    %  thresh = max(model.comp{obsMod}.kern.comp{1}.inputScales) * 0.001;
    thresh = 0.005;

    retainedScales{obsMod} = find(s1 > thresh);
    %thresh = max(model.comp{infMod}.kern.comp{1}.inputScales) * 0.001;
    retainedScales{infMod} = find(s2  > thresh);
    sharedDims = intersect(retainedScales{obsMod}, retainedScales{infMod});
end

% Find X_* only for the shared dimensions (Xs*):
if ~exist('privateDims')
    privateDims = setdiff(1:model.comp{obsMod}.q, sharedDims);
end

% Number of test points to use
numberTestPoints = 10;
if testOnTraining
    perm = randperm(model.N);
    testInd = perm(1:numberTestPoints);
else
    startInd = 60;
    missingInd = 1:25;
    Y_testOrig = Y_test;
    Y_test(startInd:end, missingInd) = NaN;

    %model.comp{2}.y(startInd:end, missingInd)=NaN; %%%???????????
    disp('# Initializing latent points...');
    indexPresent = setdiff(1:model.comp{1}.d, missingInd);

    Yts{obsMod} = Y_test;
    Yts{infMod} = Z_test;
    testInd = 1:size(Yts{1},1);
    ZpredAll = zeros(size(Z_test));
    indsAll = zeros(size(Z_test,1),1);
end

scrsz = get(0,'ScreenSize');

x_star = zeros(length(testInd), size(model.X,2));
if ~(~testOnTraining & dynUsed) % If we have a test set and dynamics, then we need a different inference procedure
    for i=1:length(testInd)
        curInd = testInd(i);
        fprintf('# Testing indice number %d ', curInd);
        if testOnTraining
            fprintf('taken from the training set\n');
            y_star = model.comp{obsMod}.y(curInd,:);
            x_star(i,:) = model.comp{obsMod}.vardist.means(curInd,:);
            varx_star = model.comp{obsMod}.vardist.covars(curInd,:);
        else
            fprintf('taken from the test set\n');
            y_star = Yts{obsMod}(curInd,:);
            z_star = Yts{infMod}(curInd,:);
            dst = dist2(y_star(indexPresent), Y_test(:,indexPresent));

            [mind, mini(i)] = min(dst);
            %miniAll(i) = mini;
            Init(i,:) = model.vardist.means(mini(i),:);
            vardistx = vardistCreate(model.comp{obsMod}.vardist.means(mini(i),:), model.q, 'gaussian');
            vardistx.covars = model.comp{obsMod}.vardist.covars(mini(i),:);
            model.comp{obsMod}.vardistx = vardistx;
            display=1;
            iters = 250;
            % Find p(X_* | Y_*) which is approximated by q(X_*)
            [x_star(i,:), varx_star, modelUpdated] = vargplvmOptimisePoint(model.comp{obsMod}, vardistx, y_star, display, iters);%%%
        end
    end
    fprintf('# Predicting images from the NN of X_* ');

    for i=1:length(testInd)
        curInd = testInd(i);
        y_star = Yts{obsMod}(curInd,:);
        z_star = Yts{infMod}(curInd,:);

        numberOfNN = 1;
        % Now we selected a datapoint X_* by taking into account only the
        % private dimensions for Y. Now, based on the shared dimensions of
        % that, we select the closest (in a NN manner) X from the training
        % data.
      %   w = s1(sharedDims)+s2(sharedDims);
      %   [ind, distInd] = nn_class2(model.X(:,sharedDims), x_star(i,sharedDims), numberOfNN, 'weighted', w);
          [ind, distInd] = nn_class(model.X(:,sharedDims), x_star(i,sharedDims), numberOfNN, 'euclidean');
        indsAll(i) = ind(1);

        ZpredMu = zeros(length(ind), size(model.comp{infMod}.y,2));
        ZpredSigma = zeros(length(ind), size(model.comp{infMod}.y,2));

        % Find p(y_*|x_*) for every x_* found from the NN
        for k=1:numberOfNN
            %fprintf('.');
            x_cur = model.X(ind(k),:);
            % Make the shared dimensions of the current NN the same as the
            % ones of the latent point for y_star
           %  x_cur(sharedDims) = x_star(i,sharedDims); %%% OPTIONAL!!!

            % Make the shared dimensions for the current NN the same as the
            % ones of the closest NN to y_star but from the Z dataset
            % x_cur(sharedDims) = model.X(ind(1),sharedDims); %%%%% OPTIONAL #2 !!!!

            %[ZpredMu(k,:), ZpredSigma(k,:)] = vargplvmPosteriorMeanVar(model.comp{infMod}, model.X(ind(k),:));
            %if ~testOnTraining
            ZpredMu(k,:) = vargplvmPosteriorMeanVar(model.comp{infMod}, x_cur);
            %else
            %    ZpredMu(k,:) = model.comp{infMod}(ind(k),:);
            %end
        end
        ZpredAll(i,:) = ZpredMu(1,:);

        %-- Plots
        if exist('makePlots') & ~makePlots
            continue
        end
        % Open a big figure (first 2 args control the position, last 2 control
        % the size)
        figure('Position',[scrsz(3)/100.86 scrsz(4)/6.666 scrsz(3)/1.0457 scrsz(4)/1.0682],...
            'Name',['Fig: ' num2str(i) ' (Exp: ' num2str(experimentNo) ')'],'NumberTitle','off')
        numRows = 3;

        if testOnTraining
            numCols = ceil((numberOfNN+1)/numRows)*2;
            plotCounter = 2;
        else
            % For the real test image!
            numCols = ceil((numberOfNN+2)/numRows);
            plotCounter = 2;
        end

        if ~testOnTraining
            sil_star = Yim_test(curInd,:);
            pose_star = Z_test(curInd,:);
            subplot(numRows, numCols, 1)
            imagesc(reshape(sil_star,height,width))
            if obsMod == 1
                title(['Given (image #' num2str(curInd) ')']), colormap('gray')
            else
                title('Corresponding');
            end
            subplot(numRows, numCols, 2)
            handle = xyzankurVisualise2(pose_star);
            if obsMod == 1
                title(['Given (image #' num2str(curInd) ')']), colormap('gray')
            else
                title('Corresponding');
            end

            for k=1:numberOfNN
                subplot(numRows, numCols, k+plotCounter)
                if infMod == 1 &&  exist('imageSilhouette') && imageSilhouette
                    imagesc(reshape( ZpredMu(k,:),height,width)), title(['NN #' num2str(k)]), colormap('gray')
                else
                    handle = xyzankurVisualise2(ZpredMu(k,:)); title(['NN #' num2str(k)])
                end
            end
            %mserror(i) = mean(abs(ZpredMu(1,:) - Z_test(i,:)));
        else
            subplot(numRows, numCols, 1)
            imagesc(reshape(Yim(curInd,:),height,width)), title(['Original y (image #' num2str(curInd) ')']), colormap('gray')
            subplot(numRows, numCols, 2)
            handle = xyzankurVisualise2(model.comp{2}.y(curInd,:));
            % Start plotting from 2, the first is always the same as
            for k=2:numberOfNN
                % Start from k=2, the first NN we know it's the same as the
                % given point.
                subplot(numRows, numCols, k+plotCounter-1)
                imagesc(reshape(Yim(ind(k),:),height,width)), title(['NN #' num2str(k)]), colormap('gray')
                subplot(numRows, numCols, k+plotCounter)
                handle = xyzankurVisualise2(model.comp{2}.y(ind(k),:)); title(['NN #' num2str(k)])
                plotCounter = plotCounter+1;
            end
        end
        %     pause
        %     close
    end
    if ~testOnTraining
        meanPose = repmat(mean(Ytr{2}),size(Y_test,1),1);
        errors.meanPose = xyzankurError(meanPose, Z_test);
        errors.NNYspace = xyzankurError(Ytr{2}(mini,:), Z_test);
        errors.NNXspace = xyzankurError(Ytr{2}(indsAll,:),Z_test);
        errors.svargplvm = xyzankurError(ZpredAll, Z_test);
        fprintf('# Mean Pose Error: %d\n', errors.meanPose)
        fprintf('# NN in the Y space Error: %d\n',errors.NNYspace)
        fprintf('# NN in the X space Error: %d\n',errors.NNXspace)
        fprintf('# Svargplvm Error: %d\n', errors.svargplvm)
        xyzankurAnimCompareMultipleTEMP(Z_test, {ZpredAll, Ytr{2}(mini,:), Ytr{2}(indsAll,:)},-1,{'Gr. Truth', 'Svargplvm', 'NN_Y','NN_X'});
    end
else
    %%% Dynamics

    model.dynamics.t_star = timeStampsTest;
    model.comp{1}.dynamics.t_star = timeStampsTest;
    model.comp{2}.dynamics.t_star = timeStampsTest;

    for i=1:size(Y_test,1)
        % initialize the latent points using the nearest neighbour from the training data
        dst = dist2(Y_test(i,indexPresent), Ytr{1}(:, indexPresent));
        [mind, mini(i)] = min(dst);
    end

    indexMissingData = startInd:size(Y_test,1);

    vardistx = vardistCreate(model.dynamics.vardist.means(mini,:), model.q, 'gaussian');
    vardistx.covars = 0.2*ones(size(vardistx.covars));
    model.vardistx = vardistx;
    model.comp{1}.vardistx = vardistx;
    model.comp{2}.vardistx = vardistx;

    iters = 4000;

    [x, varx] = vargplvmOptimiseSeqDyn(model.comp{obsMod}, vardistx, Y_test, 1, iters);
    % keep the optimized variational parameters
    barmu = x;
    lambda = varx;

    % Get the variational means and variacnes for the new test sequcen and
    % update the model to be prepared for prediction
    [x_star, varx_star, modelUpdated] = vargplvmDynamicsUpdateModelTestVar(model.comp{obsMod}, barmu, lambda, Y_test);

    % Latent variables corresponding to the data  with missing dimensions
    Testmeans = x(indexMissingData, :);
    Testcovars = varx(indexMissingData, :);

    modelOrig = model;
    model.comp{obsMod} = modelUpdated;
    model.vardist = modelUpdated.vardist;
    model.comp{1}.vardist = modelUpdated.vardist;
    model.comp{2}.vardist = modelUpdated.vardist;
    model.dynamics = modelUpdated.dynamics;
    model.comp{1}.dynamics = modelUpdated.dynamics;
    model.comp{2}.dynamics = modelUpdated.dynamics;
    model.X = model.vardist.means;
    model.comp{1}.X = model.X;
    model.comp{2}.X = model.X;

    numberOfNN = 2;

    fprintf('# Finding the %d NN of X_* with the training X based only on the shared dims.\n', numberOfNN);
    ZpredAll = zeros(size(Z_test));
    indsAll = zeros(size(Z_test,1),1);
    indsAllOrig = zeros(size(Z_test,1),1);
    fprintf('# Predicting images from the NN of X_* ');
    for i=1:size(Z_test,1)
        [ind2,distInd] = nn_class(modelOrig.X(:,sharedDims), x_star(i,sharedDims), numberOfNN, 'euclidean');
        indsAllOrig(i) = ind2(1);

      % Actually this is a weighted NN.
      %  w = s1(sharedDims)+s2(sharedDims);
      %  [ind, distInd] = nn_class2(model.X(:,sharedDims), x_star(i,sharedDims), numberOfNN, 'weighted', w);
        [ind, distInd] = nn_class(model.X(:,sharedDims), x_star(i,sharedDims), numberOfNN, 'euclidean');
        indsAll(i) = ind(1);
        ZpredMu = zeros(length(ind), size(model.comp{infMod}.y,2));
        ZpredSigma = zeros(length(ind), size(model.comp{infMod}.y,2));
        % Find p(y_*|x_*) for every x_* found from the NN
        for k=1:numberOfNN
            x_cur = model.X(ind(k),:);%%%%  % modelOrig.X(ind2(k));
            x_cur(sharedDims) = x_star(i,sharedDims); %%% OPTIONAL!!!

                        %---- OPTIONAL 3
          %  xcurOrig  = x_cur(sharedDims);
          %  s1new = s1/sum(s1);
          %  x_cur(sharedDims) = s1new(sharedDims).*x_star(i,sharedDims) + (1-s1new(sharedDims)).*xcurOrig;
            %----

            ZpredMu(k,:) = vargplvmPosteriorMeanVar(model.comp{infMod}, x_cur, varx_star(i,:)); % varx_star needed?
        end
        ZpredAll(i,:) = ZpredMu(1,:);
    end
    fprintf('\n');
    meanPose = repmat(mean(Ytr{2}),size(Z_test,1),1);
    errors.meanPose = xyzankurError(meanPose, Z_test);
    errors.NNYspace = xyzankurError(Ytr{2}(mini,:), Z_test);
    errors.NNXspace = xyzankurError(Ytr{2}(indsAllOrig,:),Z_test);
    errors.svargplvm = xyzankurError(ZpredAll, Z_test);
    fprintf('# Mean Pose Error: %d\n', errors.meanPose)
    fprintf('# NN in the Y space Error: %d\n',errors.NNYspace)
    fprintf('# NN in the X space Error: %d\n',errors.NNXspace)
    fprintf('# Svargplvm Error: %d\n', errors.svargplvm)
    prunedModelUpdated = vargplvmPruneModel(modelUpdated);
   % save(['demHumanPoseSvargplvm' num2str(experimentNo) '.mat'],'barmu','lambda','prunedModelUpdated','errors');

   % xyzankurAnimCompareMultipleTEMP(Z_test, {ZpredAll, Ytr{2}(mini,:), Ytr{2}(indsAllOrig,:)},-1,{'Gr. Truth', 'Svargplvm', 'NN_Y','NN_X'});
end


function [varargout]=grid2patch(varargin)

% function [F,V,C,CV]=grid2patch(X,Y,Z,C,perdiocOpt);
% ------------------------------------------------------------------------
%
%
% 2019/12/16: Created as alternative to surf2patch (missing in Octave)
% ------------------------------------------------------------------------

%% parse input

switch nargin
    case 2
        X=varargin{1};
        Y=varargin{2};
        Z=zeros(size(X));
        C=[];
        perdiocOpt=false(1,2);
    case 3
        X=varargin{1};
        Y=varargin{2};
        Z=varargin{3};
        C=[];
        perdiocOpt=false(1,2);
    case 4
        X=varargin{1};
        Y=varargin{2};
        Z=varargin{3};
        C=varargin{4};
        perdiocOpt=false(1,2);
    case 5
        X=varargin{1};
        Y=varargin{2};
        Z=varargin{3};
        C=varargin{4};
        perdiocOpt=varargin{5};
end

if isempty(C)
    C=Z(:);
end
siz=size(X);

%%

V=[X(:) Y(:) Z(:)]; %Vertex set

%Create row of faces
f=[1 1+siz(1) 2+siz(1) 2]; %First element
q=(0:1:siz(1)-2)';
qq=q(:,ones(4,1));
ff=f(ones(siz(1)-1,1),:)+qq; %Row of faces by copying first

if perdiocOpt(1)==1
    ff(end+1,:)=[siz(1) siz(1)+siz(1) 1+siz(1) 1];
end

%Create grid of faces by copying column
q=0:siz(1):(siz(1)*(siz(2)-2));
if perdiocOpt(1)==1
    Q=q(ones(siz(1),1),:);
else
    Q=q(ones(siz(1)-1,1),:);
end
Q=Q(:);
Q=Q(:,ones(1,4));
F=repmat(ff,[siz(2)-1 1])+Q; %Grid of faces

if perdiocOpt(1)==1 && perdiocOpt(2)==1
    t=siz(1)*(siz(2)-1);
    f=[2 2+t 1+t 1]; %First element
    q=(0:1:siz(1)-2)';
    qq=q(:,ones(4,1));
    ff=f(ones(siz(1)-1,1),:)+qq; %Row of faces by copying first
    ff(end+1,:)=[1 1+t siz(1)+t siz(1)];
    F=[F;ff];
elseif perdiocOpt(2)==1
    t=(siz(1)*(siz(2)-1));
    f=[2 2+t 1+t 1]; %First element
    q=(0:1:siz(1)-2)';
    qq=q(:,ones(4,1));
    ff=f(ones(siz(1)-1,1),:)+qq; %Row of faces by copying first
    F=[F;ff];
end

%% Collect output

varargout{1}=F;
varargout{2}=V;
if nargout==3
    varargout{3}=vertexToFaceMeasure(F,C(:));
end
if nargout==4
    A=C;
    varargout{4}=A(:);
end

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


function res = imLUT(img, lut)
%IMLUT apply a lut to a gray-scale image.
%
%   IM2 = imLUT(IMG, LUT).
%   IMG is a gray-scale image, 1, 8 or 16 bits image, LUT is a double array
%   with 2**Nbits elements.
%   Each element x in IMG will by replaced by the value of the (x+1)-th
%   element in the LUT.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 25/10/2004.
%

%   HISTORY

res = zeros(size(img));
for i=0:length(lut)-1
    res(img==i) = lut(i+1);
end


function [poly,poly_idx,max_index,max_size] = extdom_polygon(bnde,pts,order,line,min_size)
% DESCRIPTION: Given a set of boundary edges of a singly- or multi-
%              polygonal region, organize them in a winding order.
%
% INPUTS:
%         bnde: the indices of each boundary edge as a nbnde x 2 matrix
%         pts:  the x,y locations of all the points in the region
%               stored as an np x 2 matrix.
%         order:the order in which the traversal takes place
%               counter-clockwise (0) or clockwise (1) or add a negative
%               sign to append NaNs in each cell.
%          line:if desired output will be polylines
%       min_size: if the lenght of the polygon is less than min_size
%                  points, throw it out. zero by default.
% OUTPUTS:
%          poly: the boundary of each enclosing polygon sorted in winding-order
%                poly is returned as a cell-array of length number of polys.
%      poly_idx: indices of the polygon coordinates in the same format as
%                poly
%     max_index: is the index into poly that is the largest
%      max_size: is the size of the largest poly
%
% Last Edited:
% kjr,UND,CHL,2017
%   kjr,UND,CHL -->revised for massive speed improvements March 2018.
%
%                                           TRAVERSAL METHOD
% Pick any unvisited edge segment [v_start,v_next] and add these vertices to the polygon loop.
% Find the unvisited edge segment [v_i,v_j] that has either v_i = v_next or v_j = v_next and add the other vertex (the one not equal to v_next) to the polygon loop.
% Reset v_next as this newly added vertex, mark the edge as visited and continue from 2.
% Traversal is done when we get back to v_start.
% NOTE: that the signed area will be positive if the vertices are
% oriented counterclockwise, and will be negative if it is oriented clockwise

% NOTE: By flipping the edges left-to-right, we can easily see the nodal
% connectivity of the triangulation. However, since we are effectively
% adding new "edges", we must also quickly locate and flag the "flipped" edge
% in addition to the current edge under consideration. This is accomplished with the invmap
% array which allows one, given a vertex gid, to quickly find it's linear
% index inside the array bnde((boundary edges). This localizes the search to find
% the connectivity to continue the "walk" on the boundary making the
% calculation massively more efficient than compared to searching the entire bnde
% array with edge under consideration.

% if storing lines
if(nargin < 4); line = 0; end
if(nargin < 5); min_size = 1; end

bnde = [bnde; fliplr(bnde)];
bnde = sortrows(bnde,1);
ned = length(bnde);
an  = sign(order);
active = true(ned,1);

p = 0 ;
% given a vertex with num gid, return where it exists last lid.
for lid = 1 : ned
    gid = bnde(lid,1);
    invmap(gid) = lid ;
end

while any(active)
    p = p + 1;

    rn = find(active,1);

    temp  = pts(bnde(rn,:)',:);
    temp2 = bnde(rn,:)';

    v_start= bnde(rn,1);
    v_next = bnde(rn,2);

    active(rn) = false;

    % flag flipped edge too
    flipped = fliplr(bnde(rn,:));
    idx = invmap(flipped(1));
    st   = max((idx - 1),1);
    ed   = min(idx,ned);
    rn = find(flipped(2)==bnde(st:ed,2),1);

    active(rn+st-1) = false;

    k = 2 ;
    while v_next~=v_start

        % form local set to search for continuation of boundary walk.
        idx  = invmap(v_next);
        st   = max((idx - 1),1);
        ed   = min(idx,ned);

        r = find(v_next==bnde(st:ed,1) & active(st:ed),1);
        tsel = bnde(st+r-1,:); tsel_inv = fliplr(tsel) ;
        sel=tsel(tsel~=v_next);

        if(line)
            if(isempty(sel))
                break
            end
        end
        % store points.
        k = k + 1;
        temp(k,:) = pts(sel,:);
        temp2(k,:)= sel;

        active(r+st-1) = false;

        % flag flipped edge too
        idx = invmap(tsel_inv(1));
        st   = max((idx - 1),1);
        ed   = min(idx,ned);
        r = find(tsel_inv(2)==bnde(st:ed,2),1);
        active(r+st-1) = false;

        v_next = sel;

    end

    if length(temp > min_size)
        poly{p}     = temp;
        poly_idx{p} = temp2;
    else
        continue
    end
    if an < 0
        poly{p}(end+1,:) = [NaN NaN];
        poly_idx{p}(end+1,:) = NaN;
    end
    [area]=parea(poly{p}(:,1),poly{p}(:,2));
    if order==0 % ccw
        if sign(area)<0
            poly{p} = flipud(poly{p});
            poly_idx{p} = flipud(poly_idx{p});
        end
    else % cw
        if sign(area)>0
            poly{p} = flipud(poly{p});
            poly_idx{p} = flipud(poly_idx{p});
        end
    end
end
[max_size, max_index] = max(cellfun('size', poly, 1));
end
% helper function, computes area of polygon
function [area]=parea(x,y)
n    = length(x);
xp   = [x; x(1)];
yp   = [y; y(1)];
area = 0;
for i = 1:n
    area = area + det([xp(i), xp(i+1); yp(i), yp(i+1)]);
end
area = 1/2*area;
end