function AAM = build_model_2d_from_files(directory, varargin)
% AAM = build_model_2d_from_files(directory)
% AAM = build_model_2d_from_files(directory, 'triangulation', triangulation)
%
% FIXME: Mostly a cut/paste from build_model_2d, but this
% method was needed to avoid out-of-memory issues when dealing with large training
% sets.
%
% Builds an Active Appearance Model using shape and appearance data retrieved from
% the given directory.
% Based on the 'Active Appearance Models Revisited' paper by Iain Matthews and Simon Baker.
%
%                         PARAMETERS
%
% directory Path to a directory containing shape and appearance data in the appropriate format.
%           See the examples to see what this format is, or use one of the provided datasets.
%
%                     OPTIONAL PROPERTIES
%
% 'triangulation' A matrix property providing a triangular mesh over the
%                 shape (in the same format as the one returned by delaunay).
%                 If not provided, a triangulation will be determined using
%                 delaunay on the mean shape.
%
%
%                        RETURN VALUE
%
% AAM is a structure with the following fields:
%
% 	s0 The mean shape as a column vector [i1 i2 i3 ... j1 j2 j3 ...]'.
%
%   s  The matrix of base shapes with each shape stored on a column vector
%      with the same structure as s0
%
%   s_star The basis for the global normalizing transform. They are four column
%          vectors whose linear combination allow for translation, scale and
%          rotation of the base shape
%
%   shape_eiv The eigenvalues associated to each shape principal
%             component
%
%   warp_map A modelh-by-modelw lookup table for determining to which
%            triangle warp_map(i,j) the pixel (i,j) belongs. If 0, the
%            pixel is outside the mean shape
%
%   shape_mesh The triangles indexed by warp_map. It is a Tx3 matrix
%              containing indices to vertices in the shape. Look at
%              the delaunay function for further information
%
%   alpha_coords An image containing the alpha barycentric coordinate
%                for pixel (i,j) wrt of the triangle given by
%                shape_mesh(warp_map(i,j),:). Value is undefined if
%                warp_map(i,j) is 0
%
%   beta_coords An image containing the beta barycentric coordinate
%               for pixel (i,j) wrt of the triangle given by
%               shape_mesh(warp_map(i,j),:). Value is undefined if
%               warp_map(i,j) is 0
%
%   adj_list Array of N cells containing indices to the triangles sharing
%            a given vertex. Such that for the k-th cell, the list will
%            contain the indices to the triangles in shape_mesh sharing
%            the k-th vertex.
%
%   A0 Mean appearance as a column vector, sorted firstly by color
%      component, then j coordinate and finally i coordinate
%      Example:
%       X1 = [ 1 2; 3 4];
%       X2 = [ 5 6; 7 8];
%       X3 = [ 9 10; 11 12];
%       X(:,:,1)=X1;
%       X(:,:,2)=X2;
%       X(:,:,3)=X3;
%       reshape(X,[],1) =>
%          _________
%        1  j==1
%        3 _____   C1
%        2  j==2
%        4 _________
%        5  j==1
%        7 _____   C2
%        6  j==2
%        8 _________
%        9  j==1
%       11 _____   C3
%       10  j==2
%       12 _________
%
%   A  The matrix of base appearances, with each appearance stored on
%      the colomuns in the same manner as A0
%
%   app_eiv The eigenvalues associated to each appearance principal
%           component
%
%   dA0 The gradient of the mean appearance stored such that for pixel (i,j),
%       dA0(i,j,c,1) gives the i direction of the gradient for color c
%       and dA0(i,j,c,2) gives the j direction of the gradient for color c
%
%   dW_dp Jacobian of the warp with respect to the shape parameters p
%
%   dN_dq Jacobian of the global normalizing warp with respect of the
%         global transform parameters q
%
%   SD Steepest descent images. They are stored as row vectors (of the same length
%      as A0). The first 4 images are associated with the q
%      paramenters, the other are associated with the p parameters
%
%   H The hessian, it's a square NxN matrix where N is the number of steepest
%     descent images (or, equivalently, the sum of the number of p and
%     q parameters)
%
%   invH Inverse hessian, it's a square NxN matrix where N is the number of steepest
%     descent images (or, equivalently, the sum of the number of p and
%     q parameters)
%
%   R Simply the product between the inverse hessian and the steepest descent images,
%     invH * SD. Encodes the linear relationship between the error image and the incremental
%     parameters.
%
% Author: Luca Vezzaro (elvezzaro@gmail.com)

	shape_triangles = zeros(0, 3, 'uint32');

	i = 1;
	while i < numel(varargin)
		if strcmp(varargin{i}, 'triangulation')
			% Be sure the triangle indices are unsigned integers.
			shape_triangles = uint32(varargin{i+1});
		else
			error(sprintf('Unsupported property: %s.', varargin{i}));
		end

		i = i + 2;
	end

	tic;

	landmark_files = dir(sprintf('%s/*.mat', directory));
	disp 'Number of shapes:'
	% size of training set
	ns = numel(landmark_files)

	for j=1:ns
		% Load associated landmarks
		load(sprintf('%s/%s', directory, landmark_files(j).name));
		info = imfinfo(sprintf('%s/%s', directory, landmark_files(j).name(1:end-4)));
		shape_data(:,:,j) = xy2ij(annotations, info.Height);
	end

	% number of points
	np = size(shape_data, 1);

	% use first shape as initial mean
	mean_shape = shape_data(:,:,1);

	reference_shape = mean_shape;

	% Matrix containing the aligned shapes
	aligned_data = shape_data;

	% Less iterations should be enough, but better safe than sorry, efficiency
	% is not an issue here
	for it=1:100
		for i=1:ns
			% Align each shape to the mean
			[ D Y ] = procrustes(mean_shape, aligned_data(:,:,i));
			aligned_data(:,:,i) = Y;
		end

		% New mean shape, store in a different variable so we can compare it to
		% the old one
		new_mean_shape = mean(aligned_data, 3);
		[ D Y ] = procrustes(reference_shape, new_mean_shape);
		new_mean_shape = Y;

		mean_shape = new_mean_shape;
	end

	mean_shape = mean(aligned_data, 3);

	% Determine the region of interest
	mini = min(mean_shape(:,1));
	minj = min(mean_shape(:,2));
	maxi = max(mean_shape(:,1));
	maxj = max(mean_shape(:,2));

	% Place the origin in the upper left corner of the rectangle
	% representing the bounding box of the mean shape. Add a 1
	% pixel offset to avoid artifacts in gradient computations.
	mean_shape = mean_shape - repmat([mini - 2, minj - 2], [np, 1]);

	disp 'Width of model:'
	% Determine width and height of the model, add 1 pixel offset
	% to avoid gradient artifacts
	modelw = ceil(maxj - minj + 3)
	disp 'Height of model:'
	modelh = ceil(maxi - mini + 3)

	disp 'Time required for shape data alignment:'

	toc

	%%
	%% DEBUG STUFF
	%%
%	for i=1:ns
%		plot(aligned_data(:,2,i), aligned_data(:,1,i), 'o');
%		hold on;
%		pause;
%	end
%	plot(mean_shape(:,2), mean_shape(:,1), 'rx');
%	hold off;
%	return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% Definitive mean shape for AAM, stored on a column vector [i1 i2 i3 ... j1 j2 j3 ...]'
	AAM.s0 = reshape(mean_shape, 2*np, 1);

	% Prepare shape data for PCA
	shape_matrix = reshape(aligned_data, [2*np ns]) - repmat(AAM.s0, [1 ns]);
	clear aligned_data;

	% Get modes covering 98% of variation
	[pc eiv] = var_pca(shape_matrix, 0.98);
	clear shape_matrix;

	disp 'Number of Shape Modes:'
	size(pc, 2)

	AAM.shape_eiv = eiv;

	% Build the basis for the global shape transform, we do it here because
	% they are used to orthonormalize the shape principal vectors
	% It is done differently to the paper as we're using a different coordinate
	% frame. Here u -> i, v -> j
	s1_star = AAM.s0;
	s2_star(1:np) = -AAM.s0(np+1:end);
	s2_star(np+1:2*np) = AAM.s0(1:np);
	s3_star(1:np) = ones(np,1);
	s3_star(np+1:2*np) = zeros(np,1);
	s4_star(1:np) = zeros(np,1);
	s4_star(np+1:2*np) = ones(np,1);

	% Stack the basis we found before with the shape basis so
	% we can orthonormalize
	s_star_pc(:,1) = s1_star;
	s_star_pc(:,2) = s2_star;
	s_star_pc(:,3) = s3_star;
	s_star_pc(:,4) = s4_star;

	s_star_pc(:,5:size(pc,2)+4) = pc;

	% Orthogonalize the basis (should already be close to orthogonal)
	s_star_pc = gs_orthonorm(s_star_pc);

	% Basis for the global shape transform
	AAM.s_star = s_star_pc(:,1:4);
	% Basis for the shape model
	AAM.s = s_star_pc(:,5:end);

	disp 'Time required for building shape model:'
	toc

	%%
	%% DEBUG STUFF
	%%

%	disp 'Plotting principal vectors as deformation directions of the mean...'
%
%	plot_s0 = reshape(AAM.s0, [np 2]);
%
%	for i=1:size(AAM.s, 2)
%		hold off;
%		plot(plot_s0(:,2), plot_s0(:,1), 'o');
%		hold on;
%
%		fprintf('Principal vector %d\n.', i);
%
%		delta_s = reshape(AAM.s(:,i), [np 2]);
%		plot_s = plot_s0 + delta_s * 16;
%
%		for j=1:5
%			plot(plot_s(:,2), plot_s(:,1), 'r.');
%			plot_s = plot_s + delta_s * 16;
%		end
%		hold off;
%		disp 'Waiting for keypress...'
%		pause;
%	end
%	return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% Check if a triangulation was already provided
	if size(shape_triangles, 1) == 0
		% Triangle mesh used for warping. We determine it on the base shape.
		% Returned result is double, convert to unsigned integer.
		shape_triangles = delaunay(AAM.s0(np+1:2*np)', AAM.s0(1:np)');
		%triplot(shape_triangles, AAM.s0(1:np)', AAM.s0(np+1:2*np)');
	end

	% Bitmap mapping pixels to the triangle they belong to (the triangle used
	% for warping)
	warp_map = zeros(modelh, modelw, 'uint32');

	% Bitmap containing the first barycentric coordinate of each pixel wrt to the
	% triangle specified by warp_map
	alpha_coords = zeros(modelh, modelw);

	% Bitmap containing the second barycentric coordinate of each pixel wrt to the
	% triangle specified by warp_map
	beta_coords = zeros(modelh, modelw);

	% For each pixel, find the triangle it belongs to
	for j=1:modelw
		for i=1:modelh
			% For each triangle
			for k=1:size(shape_triangles, 1)
				t = shape_triangles(k,:);

				% Vertices of the triangle in the mean shape
				i1 = AAM.s0(t(1));
				j1 = AAM.s0(np + t(1));
				i2 = AAM.s0(t(2));
				j2 = AAM.s0(np + t(2));
				i3 = AAM.s0(t(3));
				j3 = AAM.s0(np + t(3));

				% Compute the two barjcentric coordinates
				den = (i2 - i1) * (j3 - j1) - (j2 - j1) * (i3 - i1);
				alpha = ((i - i1) * (j3 - j1) - (j - j1) * (i3 - i1)) / den;
				beta = ((j - j1) * (i2 - i1) - (i - i1) * (j2 - j1)) / den;

				if alpha >= 0 && beta >= 0 && (alpha + beta) <= 1
					% Found the triangle, save data to the bitmaps and break
					warp_map(i,j) = k;
					alpha_coords(i,j) = alpha;
					beta_coords(i,j) = beta;
					break;
				end
			end
		end
	end

	% Build adjacency list
	AAM.adj_list = cell(np, 1);

	for i=1:size(shape_triangles, 1)
		for j=1:3
			v = shape_triangles(i,j);
			AAM.adj_list{v} = [AAM.adj_list{v} i];
		end
	end

	% IMPORTANT: If the datatype is changed from 'uint32', the mex-file will
	% need to be changed!
	AAM.warp_map = uint32(warp_map);
	AAM.shape_mesh = uint32(shape_triangles);
	AAM.alpha_coords = alpha_coords;
	AAM.beta_coords = beta_coords;

	disp 'Time required for warp pre-computations:'
	toc

	%%
	%% DEBUG STUFF
	%%
%	color_map = rand(max(max(warp_map)), 3);
%	figure(1)
%	imshow(warp_map, color_map);
%
%	figure(2)
%	imshow(alpha_coords);
%
%	figure(3)
%	imshow(beta_coords);
%
%	return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% FIXME: This is somewhat of an hack, but the code is so slow when matrices grow inside loops...
	app = double(imread(sprintf('%s/%s', directory, landmark_files(1).name(1:end-4)))) / 255;
	app_matrix = zeros(modelw*modelh*size(app,3), ns);

	disp 'Color components:'
	nc = size(app, 3)

	for i=1:ns
		% Prepare appearance data
		app = double(imread(sprintf('%s/%s', directory, landmark_files(i).name(1:end-4)))) / 255;
		%app = convn(convn(app, exp(-(-5:5).^2)./sum(exp(-(-5:5).^2)), 'same'), exp(-(-5:5).^2)./sum(exp(-(-5:5).^2))', 'same');

		app_matrix(:,i) = reshape(pa_warp(AAM, shape_data(:,:,i), app), [(modelw*modelh*nc) 1]);
	end

	disp 'Time required for warping all input appearances:'
	toc

	clear app;
	clear landmark_files;

	%%
	%% DEBUG STUFF
	%%

%disp 'Displaying warped images...'
%
%for i=1:ns
%	fprintf('Image %d\n.', i);
%
%	imshow(reshape(app_matrix(:,i), [modelh modelw nc]));
%
%	hold on;
%
%	triplot(shape_triangles, AAM.s0(np+1:2*np)', AAM.s0(1:np)');
%	hold off;
%
%	disp 'Waiting for keypress...'
%	pause;
%end
%hold off;
%return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% FIXME: Should we normalize pixel values like done by Stegmann to
	% improve the generalization of the model?

	% Mean appearance as a column vector, sorted firstly by color
	% component, then j coordinate and finally i coordinate
	%Example:
	%       X1 = [ 1 2; 3 4];
	%       X2 = [ 5 6; 7 8];
	%       X3 = [ 9 10; 11 12];
	%       X(:,:,1)=X1;
	%       X(:,:,2)=X2;
	%       X(:,:,3)=X3;
	%       reshape(X,[],1) =>
	%          _________
	%        1  j==1
	%        3 _____   C1
	%        2  j==2
	%        4 _________
	%        5  j==1
	%        7 _____   C2
	%        6  j==2
	%        8 _________
	%        9  j==1
	%       11 _____   C3
	%       10  j==2
	%       12 _________

	AAM.A0 = mean(app_matrix, 2);
	mean_app = reshape(AAM.A0, [modelh modelw nc]);

	%app_size = modelw * modelh * nc;

	% Prepare apperances for PCA
	app_matrix = app_matrix - repmat(AAM.A0, [1 ns]);

	% FIXME: Goes out of memory without 'econ' flag, look at "An Introduction to Active
	% Shape Models" by Cootes for an optimized PCA
	%[pc eiv] = var_pca(app_matrix, 0.95);
  [pc eiv] = app_pca(app_matrix, 0.95);

	clear app_matrix;

	disp 'Number of Appearance Modes:'
	size(pc, 2)

	AAM.A = pc;
	AAM.app_eiv = eiv;

	disp 'Time required for building apperance model:'
	toc

	%%
	%% DEBUG STUFF
	%%

%  disp 'Displaying results of PCA on appearances...'
%
%	for i=1:size(AAM.A,2)
%   cur_app = reshape(AAM.A(:,i), [modelh modelw nc]);
%
%		fprintf('Showing mean appearance and sums of mean and principal component %d with increasing weight\n.', i);
%
%		subplot(1,3,1), imshow(mean_app)
%		subplot(1,3,2), imshow(mean_app + cur_app*sqrt(eiv(i))*3)
%		subplot(1,3,3), imshow(mean_app - cur_app*sqrt(eiv(i))*3)
%		%subplot(1,4,4), imshow(mean_app + cur_app*sqrt(eiv(i))*3)
%
%		disp 'Waiting for keypress...'
%		pause;
%	end
%	hold off;
%	return;
	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% Now we get into the interesting (and tricky) stuff

	for i=1:nc
		% For each color, estimate the gradient of the mean appearance.
		% dcol is the estimated derivative wrt to columns (which for us is the
		% j coordinate), drow is wrt to rows (which for us is the i coordinate)
		[di dj] = gradient_2d(mean_app(:,:,i), AAM.warp_map);
		mean_app_gradient(:,:,i,1) = di;
		mean_app_gradient(:,:,i,2) = dj;
	end

	AAM.dA0 = mean_app_gradient;

	%%
	%% DEBUG STUFF
	%%
%	figure(1)
%imshow(mean_app);
%	mn = min(min(min(min(AAM.dA0))));
%	mx = max(max(max(max(AAM.dA0))));
%
%	plot_dA0 = (AAM.dA0 - repmat(mn, [modelh modelw nc 2])) / (mx - mn);
%	figure(2)
%	imshow(plot_dA0(:,:,:,1));
%	figure(3);
%	imshow(plot_dA0(:,:,:,2));
%	return;
	%%
	%% END OF DEBUG STUFF
	%%

	disp 'Time required for calculating image gradient:'
	toc

	tic

	dW_dp = zeros(modelh, modelw, 2, size(AAM.s,2));
	dN_dq = zeros(modelh, modelw, 2, 4);

	for j=1:modelw
		for i=1:modelh
			if warp_map(i,j) ~= 0
				% Only the vertices of the triangle containing the pixel are of relevance
				% in determining the Jacobian.
				t = shape_triangles(warp_map(i,j),:);

				% FIXME: this is not how it should be done, there is a way to juggle
				% with the barycentric coordinates we already computed to do the same
				for k=1:3
					% Derivative of vertex 'k' wrt to the shape parameters
					dik_dp = AAM.s(t(k),:);
					djk_dp = AAM.s(t(k)+np,:);

					% Derivative of vertex 'k' wrt to the global transformation parameters
					dik_dq = AAM.s_star(t(k),:);
					djk_dq = AAM.s_star(t(k)+np,:);

					% Now we need the barycentric coordinates of (i,j) computed using
					% point 'k' as the origin. So we rearrange the order of the vertices
					% (it doesn't matter if the result has clockwise or counterclockwise
					% winding)
					t2 = t;
					t2(1) = t(k);
					t2(k) = t(1);

						% Vertices of the triangle in the mean shape
					i1 = AAM.s0(t2(1));
					j1 = AAM.s0(np + t2(1));
					i2 = AAM.s0(t2(2));
					j2 = AAM.s0(np + t2(2));
					i3 = AAM.s0(t2(3));
					j3 = AAM.s0(np + t2(3));

					% Compute the two barycentric coordinates
					den = (i2 - i1) * (j3 - j1) - (j2 - j1) * (i3 - i1);
					alpha = ((i - i1) * (j3 - j1) - (j - j1) * (i3 - i1)) / den;
					beta = ((j - j1) * (i2 - i1) - (i - i1) * (j2 - j1)) / den;

					% Nonzero portion of the Jacobian of the warp wrt to the vertex 'k'.
					% TODO: Isn't this the third barycentric coordinate? It should be
					% easier and faster to compute it directly.
					dW_dij = 1 - alpha - beta;

					% This is how it's formulated on the paper (dW_dq is similar):
					% dW_dp = dW_dp + [dW_dij; 0] * dik_dp + [0; dW_dij] * djk_dp;
					% MATLAB does weird things with dimensions here, so we need squeeze()
					% to make sure the submatrix is actually a 2xn matrix and not a
					% 1x1x2xn
					dW_dp(i,j,:,:) = squeeze(dW_dp(i,j,:,:)) + dW_dij * [dik_dp; djk_dp];
					dN_dq(i,j,:,:) = squeeze(dN_dq(i,j,:,:)) + dW_dij * [dik_dq; djk_dq];
				end
			end
		end
	end

	disp 'Time required for calculating warp Jacobians:'
	toc

	%%
	%% DEBUG STUFF
	%%
%  disp 'Displaying the warp Jacobian...'
%
%	for i=1:size(dW_dp,4)
%	  jacobian_i = dW_dp(:,:,1,i);
%	  jacobian_j = dW_dp(:,:,2,i);
%
%
%		fprintf('Showing warp jacobian i and j components for parameter %d\n.', i);
%
%		subplot(1,2,1), imshow(jacobian_i)
%		subplot(1,2,2), imshow(jacobian_j)
%
%		disp 'Waiting for keypress...'
%		pause;
%	end
%	return;
	%%
	%% END OF DEBUG STUFF
	%%

	tic

	app_modes = reshape(AAM.A, [modelh modelw nc size(AAM.A, 2)]);

	SD = zeros(modelh, modelw, nc, 4 + size(dW_dp, 4));

	% Compute steepest descent images for the 4 global transformation parameters
	% TODO: the loops over c are probably optimizable, but the number of dimensions involved
	% are starting to hurt my head...
	% TODO: test me
	for i=1:4
		prj_diff = zeros(nc, size(AAM.A, 2));
		for j=1:size(AAM.A, 2)
			for c=1:nc
				prj_diff(c,j) = sum(sum(app_modes(:,:,c,j) .* (AAM.dA0(:,:,c,1) .* dN_dq(:,:,1,i) + AAM.dA0(:,:,c,2) .* dN_dq(:,:,2,i))));
			end
		end

		for c=1:nc
			SD(:,:,c,i) = AAM.dA0(:,:,c,1) .* dN_dq(:,:,1,i) + AAM.dA0(:,:,c,2) .* dN_dq(:,:,2,i);
		end

		for j=1:size(AAM.A, 2)
			for c=1:nc
				SD(:,:,c,i) = SD(:,:,c,i) - prj_diff(c,j) * app_modes(:,:,c,j);
			end
		end
	end

	% Compute steepest descent images for the shape parameters
	% TODO: the loops over c are probably optimizable, but the number of dimensions involved
	% are starting to hurt my head...
	% TODO: test me
	for i=1:size(dW_dp, 4)
		prj_diff = zeros(nc, size(AAM.A, 2));
		for j=1:size(AAM.A, 2)
			for c=1:nc
				prj_diff(c,j) = sum(sum(app_modes(:,:,c,j) .* (AAM.dA0(:,:,c,1) .* dW_dp(:,:,1,i) + AAM.dA0(:,:,c,2) .* dW_dp(:,:,2,i))));
			end
		end

		for c=1:nc
			SD(:,:,c,i+4) = AAM.dA0(:,:,c,1) .* dW_dp(:,:,1,i) + AAM.dA0(:,:,c,2) .* dW_dp(:,:,2,i);
		end

		for j=1:size(AAM.A, 2)
			for c=1:nc
				SD(:,:,c,i+4) = SD(:,:,c,i+4) - prj_diff(c,j) * app_modes(:,:,c,j);
			end
		end
	end

	AAM.SD = zeros(size(SD, 4), size(AAM.A,1));

	% FIXME: A posteriori optimization fix, integrate it more nicely with the code.
	for i=1:size(SD, 4)
		AAM.SD(i,:) = reshape(SD(:,:,:,i), 1, []);
	end

	%%
	%% DEBUG STUFF
	%%
%	mn = min(min(min(min(SD))));
%	mx = max(max(max(max(SD))));
%	SD = (SD - repmat(mn, [modelh modelw nc size(SD, 4)])) / (mx - mn);
%	for i=1:size(SD,4)
%		h=figure(i+1);
%		imshow(SD(:,:,:,i));
%	end
	%%
	%% END OF DEBUG STUFF
	%%

	clear SD;

	disp 'Time required for calculating steepest descent images:'
	toc

	tic

	% Compute the Hessian, still not sure if this is the right
	% way to treat the color information (using greyscale images
	% give pretty much the same matching results so it should be ok)
	AAM.H = AAM.SD * AAM.SD';

	AAM.invH = inv(AAM.H);

	AAM.R = AAM.invH * AAM.SD;

	disp 'Time required for determining and inverting Jacobian and calculating invH*SD:'
	toc


function ehmm = obsupdate_ehmm(Gamma,ehmm,residuals,XX,Tfactor)
%
% Update observation model
%
% INPUT
% X             observations
% T             length of series
% Gamma         p(state given X)
% ehmm           ehmm data structure
% residuals     in case we train on residuals, the value of those.
%
% OUTPUT
% ehmm           estimated ehmm model
%
% Author: Diego Vidaurre, OHBA, University of Oxford / Aarhus Univ (2022)

% Some stuff that will be later used
if nargin<6, Tfactor = 1; end

K = ehmm.K;

Gamma = [Gamma prod(1-Gamma,2)];
Gamma = rdiv(Gamma,sum(Gamma,2));

XXGXX = cell(K,1);
for k = 1:K+1
    XXGXX{k} = bsxfun(@times, XX, Gamma(:,k))' * XX;
end

% autoregression coefficients
[ehmm,XW] = updateW_ehmm(ehmm,Gamma,residuals,XX,XXGXX,Tfactor);

% Omega
ehmm = updateOmega_ehmm(ehmm,Gamma,residuals,XX,XXGXX,XW,Tfactor);
%
% % autoregression coefficient priors
ehmm = updateSigma_ehmm(ehmm); % sigma - channel x channel coefficients
ehmm = updateAlpha_ehmm(ehmm); % alpha - one per order

end

% function ehmm = updateW_here(ehmm,Gamma,residuals,XX)
%
% K = size(Gamma,2); np = size(XX,2);
% ndim = size(ehmm.state(end).W.Mu_W,2);
% noGamma = prod(1-Gamma,2);
% residuals0 = residuals;
% m0 = zeros(size(residuals0));
%
% for n = 1:ndim
%     m0(:,n) = bsxfun(@times,XX * ehmm.state(end).W.Mu_W(:,n),noGamma);
%     residuals(:,n) = residuals(:,n) - m0(:,n);
% end
%
% %Gamma = [Gamma noGamma];
% X = zeros(size(XX,1),np * K);
% for k = 1:K
%    X(:,(1:np) + (k-1)*np) = bsxfun(@times, XX, Gamma(:,k));
% end
% % estimation
% for n = 1:ndim
%     ehmm.state_shared(n).Mu_W = ...
%         pinv(X) * residuals(:,n);
% end
%
% % meand = zeros(size(XX,1),ndim);
% % for n = 1:ndim
% %      W = ehmm.state_shared(n).Mu_W;
% %     meand(:,n) = meand(:,n) + X * W;
% % end
% % d = residuals - meand;
% % mean(d.^2)
%
%
% end

% function [e,meand] = getError_here(Gamma,hmm,residuals,XX)
%
% C = hmm.Omega.Gam_shape ./ hmm.Omega.Gam_rate;
% meand = computeStateResponses_here(XX,hmm,Gamma);
% d = residuals - meand;
%
% [ mean(d.^2) ]
%
% Cd = bsxfun(@times, C, d)';
% dist = zeros(size(residuals,1),1);
% for n = 1:size(residuals,2)
%     dist = dist + 0.5 * (d(:,n).*Cd(n,:)');
% end
% e = sum(dist);
%
% end
%
%
% function meand = computeStateResponses_here(XX,ehmm,Gamma)
%
% K = size(Gamma,2); np = size(XX,2);
% ndim = size(ehmm.state(end).W.Mu_W,2);
% noGamma = prod(1-Gamma,2);
% meand = zeros(size(XX,1),ndim);
% for n = 1:ndim
%     meand(:,n) = meand(:,n) + ...
%         bsxfun(@times,XX * ehmm.state(end).W.Mu_W(:,n),noGamma);
%
% end
% % meand = zeros(size(XX,1),ndim);
% %Gamma = [Gamma noGamma];
% X = zeros(size(XX,1),np * K);
% for k = 1:K
%    X(:,(1:np) + (k-1)*np) = bsxfun(@times, XX, Gamma(:,k));
% end
% for n = 1:ndim
%     %W = [ehmm.state_shared(n).Mu_W; ehmm.state(end).W.Mu_W(:,n)];
%     W = ehmm.state_shared(n).Mu_W;
%     %meand(:,n) = meand(:,n) + X * ehmm.state_shared(n).Mu_W;
%     meand(:,n) = meand(:,n) + X * W;
% end
%
% end


%                                     [L.L, L.d, L.skip, L.add] = blkchol(L,X,pars,absd)
% BLKCHOL Fast block sparse Cholesky factorization.
%    The sparse Cholesky factor will be placed in the fields L.L, L.d;
%    the symbolic factorization fields remain unchanged.
%    On input, L should be the symbolic factorization structure of X,
%    as created by SYMBCHOL.
%    Performs LDL' factorization of X(L.perm,L.perm).
%
%   There are important differences with CHOL(X(L.perm,L.perm))':
%
%   -  BLKCHOL uses the supernodal partition L.XSUPER,
%    to use dense linear algebra on dense subblocks.
%    This explains the performance benefit of BLKCHOL over CHOL.
%
%   -  BLKCHOL never fails.
%
%   - To solve "X*y = b", use
%    >> [L.L,L.d,L.skip,L.add] = blkchol(symbchol(X),X);
%    >> L.d(find(L.skip)) = inf;
%    >> y = sparbwslv(L, sparfwslv(L,b) ./ L.d);
%
% For numerical reasons, "blkchol" may skip unstable pivots,
% or add on the diagonal. Such pivots are then listed in L.{skip,add}.
%
% See also symbchol, sparfwslv, sparbwslv, [symbfact, symmmd, chol].

function [LL, Ld, Lskip, Ladd] = blkchol(L,X,pars,absd) %#ok

 %
% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA
 %

error('Build the SeDuMi binaries by typing make at the command prompt.');


function F = in_fread_msr(sFile, sfid, SamplesBounds)
% IN_FREAD_MSR:  Read a block of recordings from a ANT ASA .msm file
%
% USAGE:  F = in_fread_brainamp(sFile, sfid, SamplesBounds=[])

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2017
% Parse inputs
if (nargin < 3) || isempty(SamplesBounds)
    SamplesBounds = round(sFile.prop.times .* sFile.prop.sfreq);
end

% FORMAT: linear matrix [nChannels x nTime]
nChannels = sFile.header.numchannels;
nTime     = round((sFile.prop.times(2) - sFile.prop.times(1)) .* sFile.prop.sfreq) + 1;
bytesize  = 4;
% Get start position
offsetTime  = SamplesBounds(1) * nChannels * bytesize;
% Number of time values to read for each channel
nTimeToRead = SamplesBounds(2) - SamplesBounds(1) + 1;
% Number of values to skip after each channel
nSkipTimeEnd = (nTime - SamplesBounds(2) - 1) * nChannels * bytesize;
nSkip        = nSkipTimeEnd + offsetTime;
% Position file at the beginning of the data block
fseek(sfid, double(offsetTime), 'bof');
% Read everything at once
% => WARNING: CALL TO FREAD WITH SKIP=0 DOES NOT WORK PROPERLY
if (nSkip == 0)
    F = fread(sfid, [nChannels, nTime], '*float32');
else
    precision = sprintf('%d*float32=>float32', nTimeToRead * nChannels);
    F = fread(sfid, [nChannels, nTimeToRead], precision, nSkip);
end

% Convert from microVolts to Volts
F = 1e-6 * F;


% Q = vbfa(D, Y, W_module, X_module, noise_module, ...)
%
% Variational Bayesian (VB) factor analysis (FA) learning algorithm with
% changeable modules for the latent variables.
%
% Optional parameters:
% 'maxiter'             100
% 'update_x'            1
% 'update_w'            1
% 'update_noise'        1
% 'rotate'              1
% 'rotation_checkgrad'  false
% 'rotation_show'       false
% 'rotation_maxiter'    10
% 'debug'               false
% 'autosave'            false
% 'autosavefile'        'vbfa_autosave'

function Q = vbfa(D, Y, W_module, X_module, noise_module, varargin)

[M,N] = size(Y);

options = struct('maxiter', 100, ...
                 'update_x', 1, ...
                 'update_w', 1, ...
                 'update_noise', 1, ...
                 'initialize_x', true, ...
                 'initialize_w', true, ...
                 'initialize_noise', true, ...
                 'rotate', 1, ...
                 'rotation_checkgrad', false, ...
                 'rotation_show', false, ...
                 'rotation_maxiter', 10, ...
                 'loglikelihood', true, ...
                 'debug', false, ...
                 'autosave', false,...
                 'autosavefile', 'vbfa_autosave');

[options, errmsg] = argparse(options, varargin{:});
error(errmsg);

% Initialize
disp('Initializing variables..')
if options.initialize_w
  [Q.W,Q.CovW,Q.rhow] = W_module.initialize(D,M);
else
  QW = W_module.get_struct();
  Q.W = QW.X;
  Q.CovW = QW.CovX;
  Q.rhow = QW.rho;
end
if options.initialize_x
  [Q.X,Q.CovX,Q.rhox] = X_module.initialize(D,N);
else
  QX = W_module.get_struct();
  Q.X = QX.X;
  Q.CovX = QX.CovX;
  Q.rhox = QX.rho;
end
if options.initialize_noise
  [Q.Tau] = noise_module.initialize(M,N);
else
  QTau = noise_module.get_struct();
  Q.Tau = QTau.Tau;
end
%Q.W_module = W_module;
%Q.X_module = X_module;
%Q.noise_module = noise_module;

% Observed values
Obs = ~isnan(Y);
MN_obs = sum(Obs(:));

%%
%% VB learning

loglikelihood_old = -inf;
Q.loglikelihood = nan(options.maxiter,1);

Q.Y = Y;

% Replace missing values with zeros for computational reasons
Y(~Obs) = 0;

KL_W = -inf;
KL_X = -inf;
KL_Tau = -inf;

% Debugging stuff:
klw = nan(options.maxiter,1);
klx = nan(options.maxiter,1);
kltau = nan(options.maxiter,1);
loglike = nan(options.maxiter,1);

%f = zeros(4,options.maxiter);

disp('Starting the VB iteration..')
for ind=1:options.maxiter
  Q.ind = ind;

  startitercpu = cputime;

  %
  % Update variables
  %

  Tau = Q.Tau;
  Tau(~Obs) = 0;

  if index_selected(Q.ind, options.update_x)
    if index_selected(Q.ind, options.debug)
      disp('Update X')
    end
    %[U,Z] = projection(Y,Obs,W,CovW,Tau);
    [Q.X,Q.CovX,Q.rhox,logrhox,KL_X] = X_module.update(Q.ind, Y, Obs, Q.W, ...
                                                      Q.CovW, Q.rhow, Tau);
  end

  if index_selected(Q.ind, options.update_w)
    if index_selected(Q.ind, options.debug)
      disp('Update W')
    end
    %[U,Z] = projection(Y',Obs',X,CovX,Tau');
    [Q.W,Q.CovW,Q.rhow,logrhow,KL_W] = W_module.update(Q.ind, Y', Obs', Q.X, ...
                                                      Q.CovX, Q.rhox, Tau');
  end

  % Compute squared errors <(y_mn-w_m*x_n)^2>
  % (used by noise update and loglikelihood)
  E2 = zeros(size(Y));
  Yh = Q.W'*Q.X;
  E2 = Y.*Y;
  E2 = E2 - 2*Y.*Yh;
  E2 = spdiag(Q.rhow)*E2*spdiag(Q.rhox);
  if ndims(Q.CovW) == 2 && ndims(Q.CovX) == 2
    E2 = E2 ...
         + spdiag(Q.rhow)*(Yh.^2)*spdiag(Q.rhox) ...
         + spdiag(Q.rhow)*(Q.W'.^2)*Q.CovX ...
         + Q.CovW'*(Q.X.^2)*spdiag(Q.rhox) ...
         + Q.CovW'*Q.CovX;
  elseif ndims(Q.CovW) == 3 && ndims(Q.CovX) == 3
    xx = bsxfun(@times, reshape(Q.X*spdiag(Q.rhox),[D,1,N]), reshape(Q.X,[1,D,N])) ...
         + Q.CovX;
    xx = reshape(xx, [D*D,N]);
    ww = bsxfun(@times, reshape(Q.W*spdiag(Q.rhow),[D,1,M]), reshape(Q.W,[1,D,M])) ...
         + Q.CovW;
    ww = reshape(ww, [D*D,M]);
    E2 = E2 + ww'*xx;
  else
    % TODO: Optimize this..
    warning('Optimize this.. and is rho properly here??');
    for m=1:M
      for n=1:N
        if Obs(m,n)
          if ndims(Q.CovW) == 2
            ww = Q.W(:,m)*Q.W(:,m)' + diag(Q.CovW(:,m));
          else
            ww = Q.W(:,m)*Q.W(:,m)' + Q.CovW(:,:,m);
          end
          if ndims(Q.CovX) == 2
            xx = Q.X(:,n)*Q.X(:,n)' + diag(Q.CovX(:,n));
          else
            xx = Q.X(:,n)*Q.X(:,n)' + Q.CovX(:,:,n);
          end
          %WX_WX = WX_WX + traceprod(ww, xx);
          E2(m,n) = E2(m,n) + traceprod(ww, xx);
        end
      end
    end
  end
  E2(~Obs) = 0;

  if index_selected(Q.ind, options.update_noise)
    if index_selected(Q.ind, options.debug)
      disp('Update Tau')
    end

    [Q.Tau,LogTau,KL_Tau] = noise_module.update(Q.ind, E2, Obs);
%    [Tau,lowerbound] = noise_module.update(Y, Obs, v_W, W, CovW, v_X, X, CovX);
  end

  %
  % Rotate
  %

  if index_selected(Q.ind, options.rotate)

    % TODO: You could optimize the hyperparameters at the same time?
    disp('Rotating..')
    A = eye(D);

    if index_selected(Q.ind, options.rotation_checkgrad)
      mycheckgrad(@rotation_cost, A(:) + 0.5*randn(D^2,1), 1e-3, W_module, ...
                  X_module);
    end
    A = minimize(A(:), @rotation_cost, options.rotation_maxiter, W_module, ...
                 X_module);
    A = reshape(A, [D D]);
    if index_selected(Q.ind, options.rotation_show)
      A
    end
    [Q.W, Q.CovW] = W_module.rotate(A);
    [Q.X, Q.CovX] = X_module.rotate(inv(A)');

  end

  %
  % Evaluate VB lower bound
  %

  if index_selected(Q.ind, options.loglikelihood)
  % Likelihood part: <log p(Y|...)>
  logpdf_y = gaussian_logpdf(Q.Tau(Obs)'*E2(Obs), ...
                             0, ...
                             0, ...
                             -sum(LogTau(Obs))-logrhow'*sum(Obs,2)-sum(Obs,1)*logrhox, ...
                             MN_obs);

  % Debugging stuff
  klw(Q.ind) = -KL_W;
  klx(Q.ind) = -KL_X;
  kltau(Q.ind) = -KL_Tau;
  loglike(Q.ind) = logpdf_y;

  % Lower bound
  Q.loglikelihood(Q.ind) = logpdf_y - KL_W - KL_X - KL_Tau;

  if Q.loglikelihood(Q.ind) < loglikelihood_old
    warning(sprintf('Loglikelihood lower bound decreased relatively %e!', ...
                    (loglikelihood_old - Q.loglikelihood(Q.ind)) / ...
                    loglikelihood_old));
    % plot([Q.loglikelihood, klw, klx, kltau, loglike]);
  end

  fprintf('Iteration step %d: loglikelihood=%e (%.2f seconds)\n', Q.ind, ...
          Q.loglikelihood(Q.ind), cputime()-startitercpu);

  loglikelihood_old = Q.loglikelihood(Q.ind);
  end

  if index_selected(Q.ind, options.autosave)
    Q.W_struct = W_module.get_struct();
    Q.X_struct = X_module.get_struct();
    Q.Tau_struct = noise_module.get_struct();
    fprintf('Saving results to %s...', options.autosavefile);
    save(options.autosavefile, '-struct', 'Q');
    fprintf(' done.\n');
  end

end

function [c, dc] = rotation_cost(a, W_module, X_module)
N = sqrt(length(a));
A = reshape(a, N, N);
[U,S,V] = svd(A);
invS = diag(1./diag(S));
invAt = U*invS*V';
[c_w, dc_w] = W_module.rotation_cost(A, U, S, V);
[c_x, dc_x] = X_module.rotation_cost(invAt, U, invS, V);
dc_x = -invAt*dc_x'*invAt;
c = c_w + c_x;
dc = dc_w(:) + dc_x(:);


function scales = hsvargplvmShowScales(model, displ, varargin)
if nargin < 2 || isempty(displ)
    displ = true;
end

% Layers to visualise
layers = model.H:-1:1;

% Do not show individual scales in mult-output
skipComp = false;

displayAsMatrix = false;

if nargin > 2
    if ~isempty(varargin{1}), layers = varargin{1}; end
    if length(varargin) > 1 && ~isempty(varargin{2})
        skipComp = varargin{2};
    end
    if length(varargin) > 2 && ~isempty(varargin{3})
        displayAsMatrix = varargin{3};
    end
end

%{
if ~model.multOutput
    for h=1:model.H
        if displ
            subplot(model.H,1,h)
        end
        vargplvmShowScales(model.layer{h}.comp{1},displ); title(num2str(h));
    end
    return
end
%}

for hCount=1:length(layers)
    h = layers(hCount);
    scalesAll{h} = zeros(1, model.layer{h}.q);
    if model.layer{h}.M > 10 && displ && ~displayAsMatrix
        for i=1:model.layer{h}.M
            sc = vargplvmShowScales(model.layer{h}.comp{i}, ~skipComp);
            scalesAll{h} = scalesAll{h} + sc;
            if ~skipComp
                title(['Scales for layer ' num2str(h) ', model ' num2str(i)])
                pause
            end
        end
        scalesAll{h} = scalesAll{h} ./ max(scalesAll{h});
    else
        if displ && ~displayAsMatrix
            if ~model.multOutput
                subplot(model.H,1,hCount)
            else
                figure
            end
        end
        scales{h} = svargplvmShowScales(model.layer{h}, (displ && ~displayAsMatrix));
        if displ && ~displayAsMatrix
            title(['Layer ' num2str(h)]);
        end
        if model.layer{h}.M < 2
            scalesAll{h} = scales{h}{1};
            scalesAll{h} = scalesAll{h} ./ max(scalesAll{h});
        end
    end

    if model.layer{h}.M > 10 && displ && ~displayAsMatrix
        bar(scalesAll{h}); title(['Normalised sum of scales for layer ' num2str(h)])
    end
end

if displayAsMatrix
    maxQ = length(scalesAll{1});
    for h = 2:model.H
        if length(scalesAll{h}) > maxQ
            maxQ = length(scalesAll{h});
        end
    end

    scalesAllMat = zeros(model.H, maxQ);
    for hh = 1:model.H
        %h = hh;             % This will put layer 1 on top
        h = model.H - hh +1; % This will put layer H on top
        for q = 1:maxQ
            if q <= length(scalesAll{hh})
                scalesAllMat(h,q) = scalesAll{hh}(q);
            else
                scalesAllMat(h,q) = NaN;
            end
        end
    end
    h=imagesc(scalesAllMat);
    set(h,'alphadata',~isnan(scalesAllMat))
    colorbar
end


function pts = getPoints(dens,ind)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% getPoints(P,ind)
%  returns the [Nd x Np] points of the nonparametric density estimate P
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

  if (nargin < 2) ind=1:dens.N; end;
  pts = zeros(dens.D,dens.N);
  pts(:,double(dens.perm(dens.N + (1:dens.N)))+1) = dens.centers(:,dens.N + (1:dens.N));
  pts = pts(:,ind);
%  pts = dens.centers(:,dens.N + ind);


function XYdata = add2XYdata(XYdata,newx,newy)
%This function takes points that have been simulated and adds them to the
%existing data structure
%XYdata is a structure with n elements of .x and .y. n is the number of
% design points tested. .x is a 1xd where d is the number of dimensions. .y
% is 1xr where r is the number of replicates for the design point.

n= size(newx,1);

%If the structure does not exist, create it
if (isempty(XYdata))
    XYdata.x = [];
    XYdata.y = [];
end

%Convert structure to a matrix of points
x = reshape([XYdata.x],length(XYdata(1).x),[])';

%Loop through all of the points to be added
for i=1:n
    %Does the points exist already in the array?
    %TODO this can be searched all at once.
    I = find(ismember(x,newx(i,:),'rows')==1);
    %If the point exists, add the response
    if (I>0)
        XYdata(I).y=[XYdata(I).y,newy(i)];
    %else if the point does not exist, add it
    else
        %Add the points to a temp structure
        temp.x = newx(i,:);
        temp.y = newy(i);
        %Add the point in case there is a future point with the same
        % location.
        x=[x;newx(i,:)];

        %If the first array is empty, add point to the first location
        if (isempty(XYdata(1).x))
            XYdata(1) = temp;
        else
            %else, build the array
            XYdata = [XYdata temp];
        end
    end
end


function gX = biasKernDiagGradX(kern, X)

% BIASKERNDIAGGRADX Gradient of BIAS kernel's diagonal with respect to X.
% FORMAT
% DESC computes the gradient of the diagonal of the bias kernel matrix with
% respect to the elements of the design matrix given in X.
% ARG kern : the kernel structure for which gradients are being computed.
% ARG X : the input data in the form of a design matrix.
% RETURN gX : the gradients of the diagonal with respect to each element
% of X. The returned matrix has the same dimensions as X.
%
% SEEALSO : biasKernParamInit, kernDiagGradX, biaskernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

gX = zeros(size(X));


function cinds = APPgetSpIndsOld(map, imsegs);
% Get pixel indices for each superpixel

nc = max(map(:));
cinds(nc) = struct('inds', [], 'count', 0);

t1 = cputime;
for c = 1:nc
    spind = find(map==c);
    nind = sum(imsegs.npixels(spind));
    cinds(c).inds = zeros(nind, 1);
    cinds(c).count = 0;
end

t2 = cputime;
nseg = imsegs.nseg;
for k = 1:nseg
    ind = find(imsegs.segimage==k);
    c = map(k);
    cinds(c).inds(cinds(c).count+1:cinds(c).count+length(ind)) = ind;
    cinds(c).count = cinds(c).count + length(ind);
end

for c = 1:length(cinds)
    cinds(c).inds = cinds(c).inds(1:cinds(c).count);
end
%segimage = imsegs.segimage;
%for k = 1:numel(segimage)
%    c = map(segimage(k));
%    cinds(c).count = cinds(c).count + 1;
%    cinds(c).inds(cinds(c).count) = k;
%end


function [K, outKern, sumKern, Kgvar] = rbfardjitVardistPsi2Compute(rbfardKern, vardist, Z)

% RBFARDJITVARDISTPSI2COMPUTE one line description
% FORMAT
% DESC description
% RETURN K : description
% RETURN outKern : description
% RETURN sumKern : description
% RETURN Kgvar : description
% ARG rbfardKern : the kernel structure associated with the rbfard2 kernel.
% ARG vardist : description
% ARG Z : description
%
%
% SEEALSO : others
%
%
% COPYRIGHT : Michalis K. Titsias, 2009
%
% COPYRIGHT : Neil D. Lawrence, 2009
%

% VARGPLVM

[K, outKern, sumKern, Kgvar] = rbfard2VardistPsi2Compute(rbfardKern, vardist, Z);


function status = test_binaural_synthesis(modus)
%TEST_BINAURAL_SYNTHESIS tests the binaural synthesis functions
%
%   Usage: status = test_binaural_synthesis(modus)
%
%   Input parameters:
%       modus   - 0: numerical
%                 1: visual (not available)
%
%   Output parameters:
%       status  - true or false

%   TEST_BINAURAL_SYNTHESIS(modus) tests the ir_wfs function for different
%   loudspeaker arrays and source models.

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

status = false;

%% ===== Checking of input parameters ====================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);
if modus
    warning('%s: visual modus not available.',upper(mfilename));
end

%% ===== Settings ========================================================
conf = SFS_config;
conf.c = 343;
conf.fs = 44100;
conf.secondary_sources.x0 = [];
conf.secondary_sources.center = [0 0 0];
conf.secondary_sources.size = 3;
conf.ir.usehcomp = false;
conf.ir.useinterpolation = true;
conf.N = 4096;
conf.dimension = '2.5D';
conf.driving_functions = 'default';
conf.usetapwin = true;
conf.tapwinlen = 0.3;
conf.wfs.usehpre = true;
conf.wfs.hpretype = 'FIR';
conf.wfs.hpreflow = 50;
conf.wfs.hprefhigh = 1200;
conf.delayline.resampling = 'none';
conf.delayline.filter = 'integer';
conf.debug = 0;
conf.showprogress = false;

%% ===== Main ============================================================
% Check if HRTF data set is available, download otherwise
basepath = get_sfs_path();
hrtf_file = [basepath '/data/HRTFs/QU_KEMAR_anechoic_3m.sofa'];
if ~exist(hrtf_file,'file')
    disp('Download');
    url = ['https://github.com/sfstoolbox/data/blob/master/', ...
           'HRTFs/QU_KEMAR_anechoic_3m.sofa?raw=true'];
    download_file(url,hrtf_file);
end
% Load HRTF data set
hrtf = SOFAload(hrtf_file);

%% ===== WFS 2.5D ========================================================
% === Linear array ===
conf.secondary_sources.geometry = 'linear';
conf.secondary_sources.number = 20;
X = [0 -2 0];
phi = pi/2;
conf.xref = X;
% Plane wave
src = 'pw';
xs = [0.5 -1 0];
ir_wfs(X,phi,xs,src,hrtf,conf);
% Point source
src = 'ps';
xs = [0 1 0];
ir_wfs(X,phi,xs,src,hrtf,conf);
% Focused source
src = 'fs';
xs = [0 -1 0 0 -1 0];
ir_wfs(X,phi,xs,src,hrtf,conf);

% === Circular array ===
conf.secondary_sources.geometry = 'circle';
conf.secondary_sources.number = 64;
X = [0 0 0];
phi = pi/2;
conf.xref = X;
% Plane wave
src = 'pw';
xs = [0.5 -1 0];
ir_wfs(X,phi,xs,src,hrtf,conf);
% Point source
src = 'ps';
xs = [0.5 2 0];
ir_wfs(X,phi,xs,src,hrtf,conf);
% Focused source
src = 'fs';
xs = [0.5 0.5 0 -1 -1 0];
ir_wfs(X,phi,xs,src,hrtf,conf);

% === Box shaped array ===
conf.secondary_sources.geometry = 'box';
conf.secondary_sources.number = 80;
X = [0 0 0];
phi = pi/2;
conf.xref = X;
% Plane wave
src = 'pw';
xs = [0.5 -1 0];
ir_wfs(X,phi,xs,src,hrtf,conf);
% Point source
src = 'ps';
xs = [0.5 2 0];
ir_wfs(X,phi,xs,src,hrtf,conf);
% Focused source
src = 'fs';
xs = [0.5 0.5 0 -1 -1 0];
ir_wfs(X,phi,xs,src,hrtf,conf);

status = true;


function p = problemclass(F,h)

p = problemclass(lmi(F),h);


function [h] = dtiComputeFiberDensity(h, fiberGroupNum, endptFlag, fgCountFlag)
%
%   [handles] = dtiComputeFiberDensity(handles, [fiberGroupNum=selectFGs], [endPointFlag=false], [fgCountFlag=false])
%
% Calculate how many fibers pass through each of the voxels.  Normalize
% this number to 1.  Return the values as a 3D image attached to the
% handles structure (image is called 'fiber density').
%
%
% RETURNS:
%  handles, with a new background image representing the fiber density.
%
% HISTORY:
% 2003.12.18 RFD (bob@white.stanford.edu) wrote it.
% 2006.06.09 RFD: removed fiber density normalization in here. This allows
% dtiAddBackground to do the normalization and save the original fiber
% counts so that we don't lose these values.

normalise = false; %TODO: expose normalization as an option

if(isempty(h.fiberGroups))
    error('No fiber groups.');
end
if(~exist('fiberGroupNum','var') ||isempty(fiberGroupNum))
    fiberGroupNum = dtiSelectFGs(h, 'Select fiber group(s) for density calculation');
end
if(any(fiberGroupNum>length(h.fiberGroups)))
    error('Invalid fiber group.');
end
if(~exist('endptFlag','var') || isempty(endptFlag))
    endptFlag = false;
end
if(~exist('fgCountFlag','var') || isempty(fgCountFlag))
    fgCountFlag = false;
end

bb = dtiGet(h,'boundingBox');
imSize = diff(bb)+1;
%imSize = size(h.dt6);imSize = imSize(1:3);

%gd = dtiGet(h, 'acpcGrid', h.vec.mat, h.vec.mmPerVoxel, size(h.vec.img(:,:,:,1)));
% We need to get the T1-space image coords from the current anatomy image.
%imgCoords = mrAnatXformCoords(inv(dtiGet(h,'acpcXform')), [gd.X(:) gd.Y(:) gd.Z(:)]);
%xform = inv(h.xformToAcpc);
% TO DO: this should be based on the actual fiber step size, rather than
% assuming that it's 1mm.
mmPerVoxel = [1 1 1];
ac = mrAnatXformCoords(h.xformToAcpc,[0 0 0]);
%xformImgToAcpc = h.xformToAcpc/diag([h.mmPerVoxel 1]);
xformImgToAcpc = diag([mmPerVoxel 1]);
xformImgToAcpc(1:3,4) = ac';

fdImg = dtiComputeFiberDensityNoGUI(h.fiberGroups, xformImgToAcpc, imSize, normalise, fiberGroupNum, endptFlag, fgCountFlag);

%uniqueFiberInds = unique(fiberInds);
% Calculate how many fibers are associated with each unique fiber index.
% HOW TO DO THIS WITHOUT A LOOP?
%for(ii=uniqueFiberInds')
%    fdImg(ii) = sum(fiberInds==ii);
%end
% USE  [a, b]=hist(uniqueFiberInds)

imName = 'fiber density';
[h,bgNum] = dtiAddBackgroundImage(h, fdImg, imName, mmPerVoxel, xformImgToAcpc);
h = dtiSet(h,'curOverlayNum',bgNum);
return;


function [indClinic,bestCost] = estimateVolClinicalRF(pathRF,nameOutcome,testComb,testCost,nSplit,testSplit,nBoot,seed,output)

% output: optional.

if nargin < 9
    output = 1; % By default
end

startpath = pwd;
tStart = tic;

% INITIALIZATIONS
cd(pathRF)
rng(seed), seeds = ceil(1000000000*rand(2,1));
training = load('training'); training = struct2cell(training); training = training{1};
load('volumeTrain'), Volume = volumeTrain; tableVol = table(Volume);
nComb = numel(testComb);
nCost = numel(testCost);
nTest = nComb *nCost;

% COMPUTATIONS
if strcmp(nameOutcome,'DeathSign')
    outcome = training.outcomes.Death;
else
    outcome = training.outcomes.(nameOutcome);
end
rng(seeds(1)), [trainSets,testSets] = getSplits(outcome,nSplit,testSplit);
maxMetric = 0; count = 0;
for c = 1:nCost
    cost = testCost(c);
    for t = 1:nComb
        tableComb = [tableVol,training.clinical.table(:,testComb{t})];
        cat = [false,logical(training.clinical.categories(testComb{t}))];
        meanAUC = 0; meanSens = 0; meanSpec = 0; count = count + 1;
        if output
            tic, fprintf('\n--> ESTIMATING RF PERFORMANCE USING %u SPLITS FOR TEST %u OF %u: %s ... ',nSplit,count,nTest,nameOutcome)
        end
        for s = 1:nSplit
            Ttrain = tableComb(trainSets(:,s),:); Ttest = tableComb(testSets(:,s),:);
            Ytrain = outcome(trainSets(:,s)); Ytest = outcome(testSets(:,s));
            rng(seeds(2)), [RF] = trainRF_table(Ttrain,Ytrain,cat,nBoot,cost);
            [prob] = predictRF(Ttest,RF);
            [aucSplit,sensSplit,specSplit,~] = calcPerformMetrics(prob,Ytest,0.5);
            meanAUC = meanAUC + aucSplit;
            meanSens = meanSens + sensSplit;
            meanSpec = meanSpec + specSplit;
        end
        meanAUC = meanAUC/nSplit; meanSens = meanSens/nSplit; meanSpec = meanSpec/nSplit;
        metric = 0.5*meanAUC + 0.5*(1-abs(meanSens - meanSpec));
        %metric = meanAUC;
        if metric >= maxMetric
            bestCost = cost;
            indClinic = testComb{t};
            maxMetric = metric;
        end
        if output
            fprintf('DONE!\n'), toc
        end
    end
end
if output
    time = toc(tStart);
    fprintf('\n\n\n-------------------------------------\n')
    fprintf('TOTAL TIME: %f seconds',time)
    fprintf('\n-------------------------------------\n')
end

cd(startpath)
end


function p = eliminateBinary(p,binaries)

vars = p.lmi_variables;
[mt,vt] = yalmip('monomtable');

mt = mt(vars,:);
mt(:,binaries) = min(mt(:,binaries),1);

used_variables = find(any(mt,1));
x = recover(used_variables)';
new_monoms = [];
mt = mt(:,used_variables);

y = recovermonoms(mt,x);
y = p.basis*[1;y];
if isa(y,'sdpvar')
    % copy data to p
    p.basis = y.basis;
    p.lmi_variables = y.lmi_variables;
else
    p = y;
end


function handle = xyzhumanevaVisualise3d(pos,fid)

% XYZHUMANEVAVISUALISE3D
%
% COPYRIGHT : Carl Henrik Ek and Neil Lawrence, 2008

% MOCAP

if(nargin<2)
  fid = 2;
end

figure(fid);

% determine parametrization
if(max(max(abs(pos)))<100)
  type = 'jon';
else
  type = 'raquel';
end

joint = xyzhumaneva2joint(pos);
figure(fid);
handle = xyzhumanevaDraw(joint);axis equal;view([1 1 1]);
switch type
 case 'raquel'
  set(gca,'XLim',[-300 300],'YLim',[-300 300],'ZLim',[-800 800]);
 case 'jon'
  set(gca,'XLim',[-0.35 0.35],'YLim',[-0.35 0.35],'ZLim',[-1 0.3]);
end

return


% moviethresh() - threshold cell arrays that will be sent to brainmovie
%
% Usage:
%   >> data = moviethresh( data, threshold, continous, dimcont);
%
% Inputs:
%   data       - input cell array containing data arrays
%   threshold  - absolute threshold (0=none)
%   continuous - impair number of data points that must be sequentially
%                organized (3,5,7,9 ...)
%   dimcont    - dimention (1=rows, 2=columns) of the data arrays to
%                consider for sequential continuity (default: 2)
%
% Outputs:
%   data       - modified cell array
%
% Example:
%   d = moviethresh( data, 0.1, 3, 2);
%   % remove (i.e) any (absolute) value in the array contained in data
%   % that are below 0.1 and remove any value that is flanked by
%
% See also: BRAINMOVIE, TIMECROSSF

% arno@salk.edu, Arnaud Delorme, CNL / Salk Institute, 2001

% This program is free software; you can redistribute it and/or
% modify it.
% 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.

function data = moviethresh( data, threshold, cnt, dimcontinous);

if nargin < 4
	help moviethresh;
end;

% scan datas
% ----------
nbremoved = 0;
total     = 0;
for index = 1:length(data(:))
	tmp = data{ index };

	% remove values below threshold
	% -----------------------------
	tmp( find(abs(tmp) < threshold) ) = 0;

	% %%%%%%%%%%%%%%%%% TRANSPOSE if dimention 2
	if dimcontinous == 2, tmp = tmp'; end;

	% remove values that are sparse along dimention 1
	% -----------------------------------------------
	s = size(tmp, 1);
	for dimcont = 1:s
		nbcontrigth = zeros(1, size(tmp,2));
		nbcontleft  = zeros(1, size(tmp,2));
		for dim2 = 1:size(tmp,2)
			for ind=1:cnt 									% cont elements on the left
				if dimcont-ind > 0
					if tmp(dimcont-ind , dim2) ~= 0, nbcontleft(dim2)  = nbcontleft(dim2)  + 1;
					else, break; end;
				else, break; end;
			end;
			for ind=1:cnt  									% count elements on the right
				if dimcont+ind <= s
					if tmp(dimcont+ind , dim2) ~= 0, nbcontrigth(dim2)  = nbcontrigth(dim2)  + 1;
					else, break; end;
				else, break; end;
			end;
		end;

		total     = total + sum(tmp(dimcont,:) ~= 0); % non nul values

		nbremoved = nbremoved + sum( (tmp(dimcont,:) .*  ((nbcontleft+nbcontrigth) < cnt)) > 0);
		tmp(dimcont,:) = tmp(dimcont,:) .* ( (nbcontleft+nbcontrigth) >= cnt);
		%fprintf( 'Pos %d: %s ** %s\n', dimcont, int2str( nbcontleft), int2str( nbcontrigth));
	end;

	% %%%%%%%%%%%%%%%%% TRANSPOSE if dimention 2
	if dimcontinous == 2, tmp = tmp'; end;

	data{ index } = tmp;

end;
fprintf('%d removed values out of %d\n', nbremoved, total)

%debug fprintf('\tval:%f\tdestval:%f\tdimcont:%d dim2:%d ind:%d left:%d\n', tmp(dimcont, dim2), tmp(dimcont-ind , dim2), dimcont, dim2, ind, nbcontleft(dim2));


function filtS = processImageUsingPyradiomics(planC,strName,filtType,paramS)
%processImageUsingPyradiomics
%filtType : 'LoG', 'wavlelet'
% AI 06/12/2020

%% Get scan & mask
indexS = planC{end};

if ~isempty(strName)
    strC = {planC{indexS.structures}.structureName};
    strNum = getMatchingIndex(strName,strC,'exact');
    mask3M = getStrMask(strNum,planC);

    scanNum = getStructureAssociatedScan(strNum,planC);
else
    %Use entire scan
    scanNum = 1;
    mask3M = false(size(getScanArray(scanNum,planC)));
end
scan3M = double(getScanArray(scanNum,planC));
CToffset = planC{indexS.scan}(scanNum).scanInfo(1).CTOffset;
scan3M = scan3M - CToffset;

%% Get voxel size
scanS = planC{indexS.scan}(scanNum);
[xV,yV,zV] = getScanXYZVals(scanS);
dx = median(abs(diff(xV)));
dy = median(abs(diff(yV)));
dz = median(diff(zV));
voxelSizeV = [dx,dy,dz]*10; %convert to mm

%% Apply filters

%Add python module to system path & iImport
pyModule = 'pyProcessImage';

try
    py.importlib.import_module(pyModule);
catch
    disp('Python module could not be imported, check the pyradiomics path');
end

%Write scan & mask to NRRD format
fprintf('\nWriting scan and mask to NRRD format...\n');

originV = [0,0,0];
encoding = 'raw';

mask3M = uint16(mask3M);
mask3M = permute(mask3M, [2 1 3]);
mask3M = flip(mask3M,3);

scan3M = permute(scan3M, [2 1 3]);
scan3M = flip(scan3M,3);

scanFilename = strcat(tempdir,'scan.nrrd');
scanRes = nrrdWriter(scanFilename, scan3M, voxelSizeV, originV, encoding);

maskFilename = strcat(tempdir, 'mask.nrrd');
maskRes = nrrdWriter(maskFilename, mask3M, voxelSizeV, originV, encoding);

%Call image processing fn
try

    outPyList = py.pyProcessImage.filtImg(scanFilename, maskFilename,...
        filtType, paramS);
    filtImgC = outPyList{1};
    filtTypeC = outPyList{2};

    filtS = struct();
    paramC = fieldnames(paramS);

    for n = 1:length(filtImgC)
        %Convert python dictionary to matlab struct
        for m = 1:length(paramC)
            filtType = char(filtTypeC{n});
            filtType = strrep(filtType,'-','_');
            outFieldname = filtType;
        end
        pyFiltScan3M = double(filtImgC{n});
        pyFiltScan3M = permute(pyFiltScan3M,[2,3,1]);
        pyFiltScan3M = flip(pyFiltScan3M,3);
        filtS.(outFieldname) = pyFiltScan3M;
    end

catch e
    error('Feature extraction failed with message %s',e.message)
end

end


function eyeInDeg = eyetrack2deg_roughBars(eyeTrackMatFile)
% eyetrack2deg_roughBars - convert fMRI pixel based eyemovement data
% with the roughBars stimuli to degrees of visual angle
%
% eyeInDeg = eyetrack2deg_roughBars(eyeTrackMatFile);
%
% 2007/08 SOD: wrote it.

if ~exist('eyeTrackMatFile','var') || isempty(eyeTrackMatFile)
    error('need eyeTrackMatFile');
end

% frames per second
framerate = 30;

% calculate number of time points
stimrep = 5;
tr      = 1.5;
%prescan = 1
%ntime = (14.*stimrep.*tr).*framerate;

% copied from makeRetinotopyStimulus_roughBars, but we removed the
% last mean luminance presentation because we clipped the eye movie
% from the first stimulus presentation to the last.
sequence = [5:8 9:12 13:16 21,21 17:20 9:12 3,4,1,2 21,21 15,16 13,14 11,12 9,10 7,8 5,6 21,21 19,20 17,18 11,12 9,10 1:4];
fixseq   = ceil(sequence./4);
fixseq = reshape(fixseq,2,numel(fixseq)./2)';% pair up
fixseq = fixseq(:,1);
fixseq = repmat(fixseq,1,tr.*framerate.*stimrep);
fixseq = fixseq';
fixseq = fixseq(:);

% fill in fixseq with nearest values
while sum(fixseq==6),
    a = diff(fixseq==6);
    ii=find(a==1);
    fixseq(ii+1)= fixseq(ii);
    ii=find(a==-1);
    fixseq(ii) = fixseq(ii+1);
end

% load eyetrackdata
if ischar(eyeTrackMatFile)
    load(eyeTrackMatFile);
else
    et = eyeTrackMatFile;
end

% compute error:
slip = abs(numel(fixseq)-size(et,1));
fprintf(1,'[%s]:Timing error (difference) is about: %.2f seconds.\n',mfilename,slip/framerate);

% simply cut the difference
if numel(fixseq)<size(et,1)
    et = et(1:numel(fixseq),:);
else
    et = [et; zeros(slip,size(et,2))];
end

% interpolate missing data
t  = (0:numel(fixseq)-1)'./framerate;
eyeClosed = et(:,1)==0 | et(:,2)==0 | ~isfinite(sum(et(:,1:2),2));

% interpolate single points (probably due to fit failure)
singlePoints = eyeClosed + ...
               [eyeClosed(2:end); 0]+...
               [0; eyeClosed(1:end-1)];
singlePoints = singlePoints==1;
et(:,1) = interp1(t(~singlePoints),et(~singlePoints,1),t,'linear',mean(et(~singlePoints,1)));
et(:,2) = interp1(t(~singlePoints),et(~singlePoints,2),t,'linear',mean(et(~singlePoints,2)));
eyeClosed = eyeClosed & ~singlePoints;

% ignore 10 seconds around eyeblick
goodData = 1-min(eyeClosed+...
                [eyeClosed(2:end); 0]+...
                [eyeClosed(3:end); 0; 0]+...
                [eyeClosed(4:end); 0; 0; 0]+...
                [eyeClosed(5:end); 0; 0; 0; 0]+...
                [eyeClosed(6:end); 0; 0; 0; 0; 0]+...
                [0; eyeClosed(1:end-1)]+...
                [0; 0; eyeClosed(1:end-2)]+...
                [0; 0; 0; eyeClosed(1:end-3)]+...
                [0; 0; 0; 0; eyeClosed(1:end-4)]+...
                [0; 0; 0; 0; 0; eyeClosed(1:end-5)],1);%sigh...
goodData = logical(goodData);
%et_old = et;
et(:,1) = interp1(t(goodData),et(goodData,1),t,'linear',mean(et(goodData,1)));
et(:,2) = interp1(t(goodData),et(goodData,2),t,'linear',mean(et(goodData,2)));

% smooth estimates
% 1: median filter (jumps of 1/30th of seconds are unlikely)
% 2: gaussian filter (smoothness constraint fwhm=100ms)
met = ones(size(et,1),1)*mean(et(:,1:2));
et(:,1:2) = blurTC(et(:,1:2)-met,3,3,10)+met;

% remove data around fixation changes = 0; remove [-1 2] seconds
% find fixation changes
lim = [-1 2];
fc = find(diff(fixseq)~=0)-framerate*abs(min(lim));
fcii = fc(:)*ones(1,framerate*diff(lim))+...
    ones(size(fc(:)))*(0:framerate*diff(lim)-1);
fcii = fcii';
fcii = fcii(:);
badCalData = false(size(fixseq));
badCalData(fcii) = true;

% find good calibration data
goodCalData = goodData & ~badCalData;

%--- limit to good calibration data
f =fixseq(goodCalData);
% measured fixation in pixels
X = et(goodCalData,1:2);
% time
t = t(goodCalData);

Y = [-1 -1;...
     +1 -1;...
      0  0;...
     -1 +1;...
     +1 +1].*5;
Xsummary = zeros(max(fixseq),2);
fixseq_deg = zeros(size(fixseq,1),2);
for n=1:5,
    ii = fixseq==n;
    fixseq_deg(ii,:) = ones(sum(ii),1)*Y(n,:);
    ii = f==n;
    Xsummary(n,:) = mean(X(ii,:));
end

Xt = et(:,1:2);
Xt2 = Xt(goodCalData,:);
for n=1:5,
    ii = f==n;
    Xsummary(n,:) = mean(Xt2(ii,:));
end

% linear xfm
[Xi xfm] = linear_xfm(Xsummary,Y);
xfm(abs(xfm)<eps) = 0;
X2  = [Xt(:,1:2) ones(size(et,1),1)];
Xt = X2*xfm;
Xt = Xt(:,1:2);
xfm1 = [xfm';0 0 1];
xfm2 = xfm1;

c = logical([ones(2,3); zeros(1,3); 1 1 0]);

id = [eye(3); 0 0 1];

a=zeros(1001,9);
a(1,:)=[xfm1(c)' sqrt(sum(skewness(Xt(goodCalData,1:2)).^2))];

% now carefully add perspective scaling with boosting and monitoring the
% skewness we like it if the skewness goes down but not up.
iii = 1;
while(1),
    Xt2 = Xt(goodCalData,:);
    for n=1:5,
        ii = f==n;
        Xsummary(n,:) = mean(Xt2(ii,:));
    end

    % perspective xfm
    [Xi xfm] = perspective_xfm(Xsummary,Y);

    tmp = xfm(c)-id(c);
    tmp(abs(tmp)<10*eps)=0;
    if ~any(tmp), break; end
    [vM iM]  = max(abs(tmp));
    iM  = iM(1);
    vM  = tmp(iM);
    tmp = zeros(size(tmp));
    tmp(iM) = min(abs(vM),1e-3).*sign(vM);
    tmp = tmp + id(c);

    xfm = id;
    xfm(c) = tmp;

    X2  = [Xt(:,1:2) ones(size(et,1),1)]';
    Xtt = ((xfm(1:3,1:3)*X2) ./ (ones(3,1)*(xfm(4,:)*X2)))';
    Xtt = Xtt(:,1:2);

    a(iii+1,:) = [xfm2(c)' sqrt(sum(skewness(Xtt(goodCalData,1:2)).^2))];

    % monitor the skewness
    if round(a(iii+1,9)*1e4)>round(max(1.05*a(iii,9)*1e4,0.2*1e4))
        %[xfm1 xfm2],
        break
    else
        Xt = Xtt;
        xfm2 = xfm2 + xfm - id;
    end
    iii = iii + 1;
end
% % linear xfm
% [Xi xfm] = linear_xfm(Xsummary,Y);
% X2  = [et(:,1:2) ones(size(et,1),1)];
% Xt = (X2*xfm);
% Xt = Xt(:,1:2);
%
% Xt2 = Xt(goodCalData,:);
% for n=1:5,
%     ii = f==n;
%     Xsummary(n,:) = mean(Xt2(ii,:));
% end
% % perspective xfm
% [Xi xfm] = perspective_xfm(Xsummary,Y);
% xfm{1} =
% xfm{2}
% X2  = [Xt(:,1:2) ones(size(et,1),1)]';
% %Xt = ((xfm{1}*X2) ./ (ones(3,1)*(xfm{2}*X2)))';
% Xt = Xt(:,1:2);
%
%

Xi = Xt(goodCalData,:);

% output
eyeInDeg.blinks    = eyeClosed;
eyeInDeg.eye       = Xt;
eyeInDeg.framerate = framerate;
eyeInDeg.fixation  = fixseq_deg;
eyeInDeg.goodData  = goodCalData;

% compute derivative
der = diff(eyeInDeg.eye)*framerate;
der = ([ones(1,size(der,2));der] + [der; ones(1,size(der,2))])./2;
vel = hypot(der(:,1),der(:,2));

eyeInDeg.velocity = vel;

% plot
if ~nargout,
    colors = {'m.','y.','b.','g.','r.'};

    figure(1);clf;hold on;
    for n=1:5,
        plot(X(f==n,1),X(f==n,2),colors{n},'markersize',5);
    end
    ylabel('y (pixels)');
    xlabel('x (pixels)');

    figure(2);clf;hold on;
    for n=1:5,
        plot(Xi(f==n,1),Xi(f==n,2),colors{n},'markersize',5);
    end
    for n=1:5,
        plot(Y(n,1),Y(n,2),'kx','markersize',15,'linewidth',5);
        plot(mean(Xi(f==n,1)),mean(Xi(f==n,2)),'ko','markersize',15,'linewidth',3);
    end
    ylabel('y (deg)');
    xlabel('x (deg)');
    axis([-20 20 -20 20])

    figure(3);clf;
    subplot(2,1,1);hold on;
    for n=1:5,
        plot(t(f==n),Xi(f==n,1),colors{n},'markersize',5);
    end
    title('x');
    ylabel('position (deg)');
    axis([1 max(t) -20 20]);

    subplot(2,1,2);hold on;
    for n=1:5,
        plot(t(f==n),Xi(f==n,2),colors{n},'markersize',5);
    end
    title('y')
    xlabel('time (sec)');
    ylabel('position (deg)');
    axis([1 max(t) -20 20]);

    figure(4);clf;
    subplot(2,1,1);plot(a(1:iii,1:8)-ones(iii,1)*a(1,1:8));
    subplot(2,1,2);plot(a(1:iii,9),'k');
    h=axis;
    axis([h(1) h(2) 0-.01 max(h(4),0.2)+.01]);
end


%Starts from the current decoded state, takes input as minimum distance to
%reach that state and previous state And returns previous state and decoded
%information bit corresponding to that state.

function [prev_state,decoded_bit]=prev_stage(curr_state,distance_prev,metric)
    if(curr_state==1)
        if(distance_prev(1)+metric(1) <= distance_prev(3)+metric(5))
            prev_state=1;decoded_bit=0;
        else
            prev_state=3;decoded_bit=0;
        end
    end

    if(curr_state==2)
        if(distance_prev(1)+metric(2) <= distance_prev(3)+metric(6))
            prev_state=1;decoded_bit=1;
        else
            prev_state=3;decoded_bit=1;
        end
    end

    if(curr_state==3)
        if(distance_prev(2)+metric(3) <= distance_prev(4)+metric(7))
            prev_state=2;decoded_bit=0;
        else
            prev_state=4;decoded_bit=0;
        end
    end

    if(curr_state==4)
        if(distance_prev(2)+metric(4) <= distance_prev(4)+metric(8))
            prev_state=2;decoded_bit=1;
        else
            prev_state=4;decoded_bit=1;
        end
    end

end


function writeParaviewWave(waves, t, numPointsX, numPointsY, domainSize, model, simdate, mooring, paraviewPath,TimeBodyParav,g)
% Method to write ``vtp`` Paraview visualization files for the waveClass.
% Executed by paraviewVisualization.m when simu.paraview.option=1 in the
% wecSimInputFile.m
%
% Parameters
% ------------
%   waves : waveClass
%       Instance of the waveClass that is being written to Paraview files.
%   t : float vector
%       Wave time vector
%   numPointsX : int
%       Number of points for visualization in the x direction
%   numPointsY : int
%       Number of points for visualization in the y direction
%   domainSize : int
%       Length of the wave field in meters
%   model : string
%       The simMechanics ``.slx`` filename
%   simdate : string
%       Date and time of the simulation
%   mooring : int
%       MoorDyn flag
%   paraviewPath : directory
%       Directory the Paraview files were saved
%   TimeBodyParav : float vector
%       Paraview time vector
%   g : float
%       Gravitational acceleration constant
%

% ground plane
filename = [paraviewPath, filesep 'ground.txt'];
fid = fopen(filename, 'w');
fprintf(fid,[num2str(domainSize) '\n']);
fprintf(fid,[num2str(waves.waterDepth) '\n']);
fprintf(fid,[num2str(mooring) '\n']);
fclose(fid);
% wave
x = linspace(-domainSize, domainSize, numPointsX);
y = linspace(-domainSize, domainSize, numPointsY);
[X,Y] = meshgrid(x,y);
lx = length(x);
ly = length(y);
numVertex = lx * ly;
numFace = (lx-1) * (ly-1);
for it = 1:length(t)
    % open file
    filename = [paraviewPath, filesep 'waves' filesep 'waves_' num2str(it) '.vtp'];
    fid = fopen(filename, 'w');
    % calculate wave elevation
    Z = waveElevationGrid (waves, t(it), X, Y, it, g);                % write header
    fprintf(fid, '<?xml version="1.0"?>\n');
    fprintf(fid, ['<!-- WEC-Sim Visualization using ParaView -->\n']);
    fprintf(fid, ['<!--   model: ' model ' - ran on ' simdate ' -->\n']);
    fprintf(fid, ['<!--   wave:  ' waves.type ' -->\n']);
    fprintf(fid, ['<!--   time:  ' num2str(TimeBodyParav(it)) ' -->\n']);
    fprintf(fid, '<VTKFile type="PolyData" version="0.1">\n');
    fprintf(fid, '  <PolyData>\n');
    % write wave info
    fprintf(fid,['    <Piece NumberOfPoints="' num2str(numVertex) '" NumberOfPolys="' num2str(numFace) '">\n']);
    % write points
    fprintf(fid,'      <Points>\n');
    fprintf(fid,'        <DataArray type="Float32" NumberOfComponents="3" format="ascii">\n');
    for jj = 1:length(y)
        for ii = 1:length(x)
            pt = [X(jj,ii), Y(jj,ii), Z(jj,ii)];
            fprintf(fid, '          %5.5f %5.5f %5.5f\n', pt);
        end; clear ii
        clear pt
    end; clear jj
    fprintf(fid,'        </DataArray>\n');
    fprintf(fid,'      </Points>\n');
    % write squares connectivity
    fprintf(fid,'      <Polys>\n');
    fprintf(fid,'        <DataArray type="Int32" Name="connectivity" format="ascii">\n');
    for jj = 1:ly-1
        for ii = 1:lx-1
            p1 = (jj-1)*lx + (ii-1);
            p2 = p1+1;
            p3 = p2 + lx;
            p4 = p1 + lx;
            fprintf(fid, '          %i %i %i %i\n', [p1,p2,p3,p4]);
        end; clear ii
    end; clear jj
    fprintf(fid,'        </DataArray>\n');
    fprintf(fid,'        <DataArray type="Int32" Name="offsets" format="ascii">\n');
    fprintf(fid, '         ');
    for ii = 1:numFace
        n = ii * 4;
        fprintf(fid, ' %i', n);
    end; clear ii n
    fprintf(fid, '\n');
    fprintf(fid,'        </DataArray>\n');
    fprintf(fid, '      </Polys>\n');
    % end file
    fprintf(fid, '    </Piece>\n');
    fprintf(fid, '  </PolyData>\n');
    fprintf(fid, '</VTKFile>');
    % close file
    fclose(fid);
end; clear it
clear  numPoints numVertex numFace x y lx ly X Y Z fid filename p1 p2 p3 p4
end


function [] = aps_weather_model_SAR(model_type)
% [] = aps_weather_model_SAR()
% Script to load weather model data and compute SAR delays
% The DEM file can be .grd file or other. If the latter, the DEM should have an asociated
% ".rsc" file, with the same filename as the DEM. The ".rsc" files should
% contain a WIDTH, LENGTH, X_FIRST, Y_FIRST, X_STEP, Y_STEP and optional a
% FORMAT string. The weather model data is assumed to be structured in date_before(d,:) folders.
%
%
% INPUTS:
% demfile               Full path to the DEM file. The DEM needs to me in meters.
% xlims                 Limits in the x-direction, either in degrees
% ylims                 Limits in the y-direction, either in degrees
% demnull               The value for no DEM data, default is -32768.
% smpres                The output resolution, either in degrees
%                       Units needs to be consistend with xlims and ylims.
%
% OUTPUTS:
% It will give the computed ZENITH hydrostatic and wet delay map in cm for the selected region.
%
%     Copyright (C) 2015  Bekaert David - University of Leeds
%     Email: eedpsb@leeds.ac.uk or davidbekaert.com
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
%
%
% Modified from Richard Walters - Oxford / Leeds University 2012-2013
% Modifcations:
% DB 	10/2013		Convert script to a function, integrate in aps_toolbox,
%                   add syntax. Retrieve parameters automatic.
% DB    10/2013     Output the combined delay map.
% DB    10/2013     Increased processing efficiency
% DB    10/2013     add compatibility with Doris construct_dem.sh script
% DB    10/2013     Bug fix incase the the netcdf is split over two days
% DB    11/2013     Change the computation f saturated pressure to be
%                   identical witt how it was computed in ERA-I model.
% DB    11/2013     Update filenaming to Hydrostatic as its not dry delay.
%                   Compute the hydrostatic delay directly from int(P/T)
% DB    01/2014     Fix bug for the ERA-I longitude boundary, being set to small.
% DB    01/2014     Allow for tiled ERA-I data
% HB-DB 02/2014     Fix to include ECMWF with scaling and offset
% DB    02/2014     Include a parameter for ECMWF data website
% DB    03/2014     Include an extra check for the DEM grd file and the
%                   selected crop
% DB 	03/2014     Save individual time stamp SAR date_before(d,:) processing
% DB    07/2014     Redefine meris_lat(lon)_range to region_lat(lon)_range
% DB    08/2014     Check if netcdf exist (ERA-I is not an operation service)
% DB    11/2014     Include option to output the 3D delays for each date_before(d,:)
% HW    02/2015     check dem file format automatically
% DB 	06/2015     Bug fix for plotting the support information
% DB    06/2015     Fix typo in error message
% DB    11/2015     Branch of the DEM in to seperate function same for all codes
% DB    11/2015     Add multicore option from matlab
% DB 	02/2016     Close netcdf files
% DB    04/2016     Branch into weather model script and include merra too
% SSS   04/2016     Clear variables such memory need is reduced
% DB    07/2016     redefine hydrostatic delay to be based on surface pressure.
% DB    08/2016     Uncomment a keyboard
% DB    07/2016     expand to include ERA5 test model data
% KM    02/2018     expand to include NARR model data

fig_test = 0;           % when 1 show the dem as debug figure
save_3D_delays = 0;     % When 1 saves the tropopsheric delays for each x,y and with height

if nargin<1
    error('Give at least the model_type: era, era5, narr, merra, or merra2')
end
% change to lower caps for saving and filename generation consistency
model_type = lower(model_type);

%% Constants
% Parameters from Jolivet et al 2011, GRL
Rd = 287.05;            % [j/kg/K] specific gas constants dry air
Rv = 461.495;           % [j/kg/K] water vapour
k1 = 0.776;             % [K/Pa]
k2 = 0.716;             % [K/Pa]
k3 = 3.75e3;            % [K^2/Pa]
coeff.Rd = Rd;
coeff.Rv = Rv;

%% Defaults
zref = 15000;       % zref for integration- tropopause
zincr = 15;         % z increment spacing for vertical interpolation before integration
vertres = 5;        % vertical resolution for comparing dem to vert profiles of delay

%% output file names
% output file for the DEM and look angles
smpdem = 'dem_smp.xyz';

% getting the variables from the parms_aps file
stamps_processed = getparm_aps('stamps_processed',1);

%%% defaults for the weather models. If not applicable it will be changed below for the specific model.
if strcmp(model_type,'narr')
    timelist_model = ['0000' ;'0300'; '0600' ; '0900'; '1200' ;'1500'; '1800' ;'2100'; '0000'];
    model_lag = 8*7;    % days
else
    timelist_model= ['0000' ; '0600' ; '1200' ; '1800' ; '0000'];       % the time interval the model is outputed
    model_lag = 0; % ? check lags
end
era_data_type = [];                                                 % the weather model data type for ERA only.

%%% Updating specific weather model information
if strcmpi(model_type,'era')
    weather_model_datapath = getparm_aps('era_datapath',1);
    era_data_type = getparm_aps('era_data_type');         % the datatype of the model either BADC or ERA
elseif strcmpi(model_type,'merra') || strcmpi(model_type,'merra2')
    weather_model_datapath = getparm_aps('merra_datapath',1);
elseif strcmpi(model_type,'narr')
    weather_model_datapath = getparm_aps('narr_datapath',1);

else
    error(['weather model type not supported, either: wrf, era, narr, merra for now'])
end

lambda = getparm_aps('lambda',1)*100;                       % radar wavelength in cm
demfile = getparm_aps('demfile',1);
demnull = getparm_aps('dem_null',1);
UTC_sat =  getparm_aps('UTC_sat',1);
ifgday_matfile = getparm_aps('ifgday_matfile',1);
ifgs_dates = load(ifgday_matfile);

% loading the data
if strcmp(stamps_processed,'y')
   dates = ifgs_dates.day;
   load psver
   fprintf('Stamps processed structure \n')
else
    psver = 2;
    ifgs_dates = ifgs_dates.ifgday;
    dates = reshape(ifgs_dates,[],1);
    dates = unique(dates);
    dates = datenum(num2str(dates),'yyyymmdd');
end

% getting the dates
n_dates = length(dates);

%% Compute and resample DEM
[dem,xmin,xmax,ymin,ymax,smpres,nncols,nnrows] = get_DEM;

% the region which is cropped from the ERA data and used to make the interpolation.
% Should be  larger than the region to which the delay is computed
lonmin = floor(xmin)-1;
lonmax= ceil(xmax)+1;
latmin = floor(ymin)-1;
latmax = ceil(ymax)+1;

% setting the maximum height of the DEM to limit the range at which ERA-I
% needs to be interpolated to
maxdem = ceil(max(max(dem))/100)*100+50; % max height of dem in metres

fprintf(['Interpolate to a maximum dem height of ', num2str(maxdem) ,' m\n'])

%% Compute based on Satellite pass which weather model outputs that will be used
[time_before,time_after, date_before, date_after,f_before,f_after] = aps_weather_model_times(timelist_model,dates,UTC_sat,model_lag);

%% generating a file
[modelfile_before,modelfile_after] = aps_weather_model_filenames(model_type,time_before,time_after,date_before, date_after,weather_model_datapath);

%% performing the calucluation for each date
for d = 1:n_dates

    % the save filenames
    outfile = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZWD.xyz'];
    hydroutfile = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZHD.xyz'];

    no_data = 0;
    for kk = 1:2
        if kk == 1
            file = modelfile_before(d,:);
        end
        if kk == 2
            file = modelfile_after(d,:);
        end

        % check
        if exist(file,'file')~=2
            no_data = no_data+1;
        else

            % loading the weather model data
            if  strcmpi(model_type,'era')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_era(file,era_data_type) ;
            elseif  strcmpi(model_type,'era5')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_era(file,era_data_type) ;
            elseif strcmpi(model_type,'merra') || strcmpi(model_type,'merra2')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_merra(file,model_type,coeff) ;
            elseif strcmpi(model_type,'narr')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_narr(file,model_type,coeff) ;
            end
%             longrid=longrid';latgrid=latgrid';
            % verify and cope with NAN's
            [Temp,e,Geopot,P,longrid,latgrid] =  aps_weather_model_nan_check(Temp,e,Geopot,P,longrid,latgrid) ;

            % define weather model grid nodes
            latlist = reshape(latgrid(:,:,1),[],1);
            lonlist = reshape(longrid(:,:,1),[],1);
            xlist = reshape(xx,[],1);
            ylist = reshape(yy,[],1);

            % Limit weather model to those grid points around the user-defined InSAR box
            % Getting the weather model resolution
            lat_res = abs(diff(unique(latgrid)))*1.5;
            lat_res = lat_res(1);
            lon_res = abs(diff(unique(longrid)))*1.5;
            lon_res = lon_res(1);

            % make sure the weather model and the lonlat specified by user
            % is defined in the same way. Weatehr models can be [0 360]
            % degrees. User can be [-180 180] unlikely [0 360]
            if strcmpi(lon0360_flag,'y')
                % fprintf('This needs to be defined better and some checks are needed \n')
                % not all pixels should be shifted.

                if xmin<0
                   xmin= xmin+360;
                end
                if xmax<0
                   xmax = xmax +360;
                end
            end

            % generation a plot
            if fig_test ==1 & d==1 & kk==1
                fontsize = 15;
                hfig = figure('name',[model_type ' weather model nodes and InSAR region'],'position', [ 200   243   610   603]);
                % for the legend generation
                plot(mean([xmin xmax]),mean([ymax ymin]),'wo','markeredgecolor','k','markerfacecolor','w','markersize',15)
                hold on
                plot(mean([xmin xmax]),mean([ymax ymin]),'r.')
                hold on
                plot(mean([xmin xmax]),mean([ymax ymin]),'r-','linewidth',2)

                imagesc([xmin xmax],[ymax ymin],dem)
                cc=colorbar;
                view(0,90)

                axis xy
                ylabel(cc,'Topography [m]','fontsize',fontsize)
                hold on
                plot(lonlist,latlist,'wo','markeredgecolor','k','markerfacecolor','w','markersize',15)
                hold on
                plot([xmin xmin xmax xmax xmin],[ymin ymax ymax ymin ymin],'r-','linewidth',2)

                title([model_type ' weather model nodes'],'fontsize',fontsize)
                set(gca,'fontsize',fontsize)
                axis equal
                axis tight

                legend('location','northoutside',[model_type ' weather model nodes'],['Used ' model_type ' weather model nodes'],'InSAR box')
            end

            % making the weather model grid slightly larger than the InSAR
            % bounding box. Will use the weather model resolution for this
            % to make sure an extra grid node is included.
            ix = find(ymin-lat_res<= latlist & latlist <= ymax+lat_res  & xmin-lon_res<= lonlist & lonlist<= xmax+lon_res) ;
            xlist = xlist(ix);
            ylist = ylist(ix);
            latlist = latlist(ix);
            lonlist = lonlist(ix);
            numy = length(unique(latlist));
            numx = length(unique(lonlist));
            ulatlist = unique(latlist);
            ulonlist = unique(lonlist);
            uxlist = unique(xlist);
            uylist = unique(ylist);

            if fig_test ==1 & d==1 & kk==1
                figure(hfig)
                hold on
                plot(lonlist,latlist,'r.')
                xlim([xmin-4*lon_res  xmax+4*lon_res])
                ylim([ymin-4*lat_res  ymax+4*lat_res])
                str='';
                while ~strcmpi(str,'y') && ~strcmpi(str,'n')
                    fprintf(['Do the red nodes extend (just) outside the red InSAR rectangle? \n'])
                    str = input('Continue? [y: for yes, n: no] \n','s');
                end
                if strcmpi(str,'n')
                    error('Check your lon lat crop, otherwize extend area of downlaoded weather model data.')
                end
            end

            clear lon_res lat_res ylist xlist %%%SSS 4/16

            % saving the information for support plotting
            eval([model_type '.' model_type '_lonlat =[lonlist latlist];']);                            % era.era_lonlat = [lonlist latlist];
            eval([model_type '.region =[[xmin xmin xmax xmax xmin]'' [ymin ymax ymax ymin ymin]''];']); % era.region = [[xmin xmin xmax xmax xmin]' [ymin ymax ymax ymin ymin]'];
            deminfo.xmin = xmin;
            deminfo.xmax = xmax;
            deminfo.ymax = ymax;
            deminfo.ymin = ymin;
            deminfo.dem = dem;
            eval([model_type '.deminfo =deminfo;']);                                                    % era.deminfo = deminfo;
            clear deminfo
            % checking if the file already exist. Yes append otherwiuze create it
            if exist('tca_support.mat','file')==2
                eval(['save(''tca_support.mat'',''-append'',''' model_type ''');']);                    % save('tca_support.mat','-append','era')
            else
                eval(['save(''tca_support.mat'',''' model_type ''');']);                                % save('tca_support.mat','era')
            end
%             if fig_test ==1 & d==1 & kk==1
%                     if exist(['aps_' model_type],'dir')~=7
%                         mkdir(['aps_' model_type]);
%                     end
%                     print(hfig,'-dpng',['aps_' model_type  filesep model_type '_datapoints.png'])
%                     print(hfig,'-depsc',['aps_' model_type filesep model_type '_datapoints.eps'])
%             end
            eval(['clear ' model_type]);

            % Convert Geopotential to Geopotential Height and then to Geometric Height
            g0 = 9.80665;
            % Calculate Geopotential Height, H
            H = Geopot./g0;

            % map of g with latitude
            g = 9.80616.*(1 - 0.002637.*cosd(2.*latgrid) + 0.0000059.*(cosd(2.*latgrid)).^2);
            % map of Re with latitude
            Rmax = 6378137;
            Rmin = 6356752;
            Re = sqrt(1./(((cosd(latgrid).^2)./Rmax^2) + ((sind(latgrid).^2)./Rmin^2)));

            % Calculate Geometric Height, Z
            Z = (H.*Re)./(g/g0.*Re - H);

            % Find middle of scene to work out glocal and Rlocal for use later
            midx = round(mean(uxlist));
            midy = round(mean(uylist));
            glocal = g(midy,midx,1);
            Rlocal = Re(midy,midx,1);

            cdslices = maxdem/vertres +1;
            cdstack = zeros(numy,numx,cdslices);
            cdstack_dry = zeros(numy,numx,cdslices);
            cdstack_wet = zeros(numy,numx,cdslices);

            XI=(0:zincr:zref)';
            gh = glocal.*(Rlocal./(Rlocal+XI)).^2; %gravity with height for XI height range

            % Interpolate Temp P and e from 0:20:15000 m
            % then integrate using trapz to estimate delay as function of height
            for i = 1:numx;
                for j = 1:numy;
                    xn = uxlist(i);
                    yn = uylist(j);

                    %interpolate at res zincr before doing integration
                    X=double(squeeze(Z(yn,xn,:)));
                    Ye=double(squeeze(e(yn,xn,:)));
                    YeI = interp1(X,Ye,XI,'spline')*100; %change from hPa to Pa

                    YP=double(squeeze(P(yn,xn,:)));
                    YPI = interp1(X,YP,XI,'spline')*100; %change from hPa to Pa

                    YT=double(squeeze(Temp(yn,xn,:)));
                    YTI = interp1(X,YT,XI,'spline');

                    tmp1 = ((k2-(Rd*k1/Rv)).*YeI./YTI + k3.*YeI./(YTI.^2));
                    Lw = (10^-6).*-1*flipud(cumtrapz(flipud(XI),flipud(tmp1)));
                    % L is zenith delay one way in metres
                    % tmp2 = k1.*YPI./YTI;
                    %Ld = (10^-6).*-1*flipud(cumtrapz(flipud(XI),flipud(tmp2)));             % This is using P/T expression (Hanssen, 2001)
                    gm = glocal;
                    Ld = (10^-6).*((k1*Rd/gm).*(YPI - YPI(zref/zincr +1)));                 % This is P0 expression (Hanssen, 2001)

                    % Interpolate important part (i.e. total delay at elevations
                    % less than maxdem) at high res i.e. vertres, and put in cdstack.
                    cdI=(0:vertres:maxdem)';
                    LdI=interp1(XI,Ld,cdI,'spline');
                    LwI=interp1(XI,Lw,cdI,'spline');

                    %cdstack(j,i,:) = LsI;
                    cdstack_dry(j,i,:) = LdI;
                    cdstack_wet(j,i,:) = LwI;

                    if save_3D_delays==1
                        cdI3D=(0:100:max(XI))';
                        LdI3D=interp1(XI,Ld,cdI3D,'spline');
                        LwI3D=interp1(XI,Lw,cdI3D,'spline');

                        %cdstack(j,i,:) = LsI;
                        cdstack_dry3D(j,i,:) = LdI3D;
                        cdstack_wet3D(j,i,:) = LwI3D;
                        clear LdI3D LwI3D
                    end
                end
            end
            clear uxlist uylist ulonlist ulatlist %%%SSS 4/16

            % Interpolate each cdstack layer onto a grid given by the DEM extents
            % in UTM m.
            xsmpres = (xmax-xmin)/nncols;
            ysmpres = (ymax-ymin)/nnrows;
            [xi,yi] = meshgrid(xmin+0.5*xsmpres:xsmpres:xmax-0.5*xsmpres,ymin+0.5*ysmpres:ysmpres:ymax-0.5*ysmpres);

            ix_temp = diff(lonlist);
            ix_temp = find(ix_temp~=0);
            ix_temp = ix_temp(1);
            lonlist_matrix = reshape(lonlist,ix_temp,[]) ;
            latlist_matrix = reshape(latlist,ix_temp,[]) ;
            clear lonlist latlist %%%SSS 4/16

            % saving the outputs
            if save_3D_delays==1
                if kk==1
                    clear hgt lon lat dry1 dry2 wet1 dem_temp hgt_topo %%%SSS 4/16
                    hgt = cdI3D;
                    lon = [lonlist_matrix];
                    lat = latlist_matrix;
                    dry1 = cdstack_dry3D;
                    wet1 = cdstack_wet3D;

                    % also give the station topography
                    dem_temp = dem;
                    dem_temp(isnan(dem_temp))=0;
                    hgt_topo = griddata(xi,yi,dem_temp,lon,lat,'linear');
                    clear dem_temp

                elseif kk==2
                    clear dry2 wet2
                    dry2 = cdstack_dry3D;
                    wet2 = cdstack_wet3D;
                end
            end

            clear cdstack_interp_dry %%%SSS 4/16
            cdstack_interp_dry = zeros(nnrows,nncols,cdslices);
            parfor n = 1:cdslices
                newslice = interp2(lonlist_matrix,latlist_matrix,cdstack_dry(:,:,n),xi,yi,'linear');
                cdstack_interp_dry(:,:,n)= flipud(newslice); % flipud due to ypositive downpage for matlab figs, and ypositive uppage for utmy
            end

            clear cdstack_interp_wet %%%SSS 4/16
            cdstack_interp_wet = zeros(nnrows,nncols,cdslices);
            parfor n = 1:cdslices
               newslice = interp2(lonlist_matrix,latlist_matrix,cdstack_wet(:,:,n),xi,yi,'linear');
               cdstack_interp_wet(:,:,n)= flipud(newslice); % flipud due to ypositive downpage for matlab figs, and ypositive uppage for utmy
            end
            clear lonlist_matrix latlist_matrix %%%SSS 4/16
            % keeping the coordinates in the same grid as the data
            xi = flipud(xi);
            yi = flipud(yi);

            % Pull out delays from cdstack layers that match dem heights
            clear wetcorrection hydrcorrection rounddem %%%SSS 4/16
            wetcorrection = ones(nnrows,nncols);
            hydrcorrection = ones(nnrows,nncols);
            rounddem = round(dem/vertres);
            rounddem(dem < 0)=0;

            % cope with the case that NaN are present. This can be sea-level
            rounddem(isnan(dem))=0;

            for i=1:nnrows
                for j=1:nncols
                    wetcorrection(i,j) = cdstack_interp_wet(i,j,rounddem(i,j)+1);
                end
            end

            for i=1:nnrows
                for j=1:nncols
                    hydrcorrection(i,j) = cdstack_interp_dry(i,j,rounddem(i,j)+1);
                end
            end

            if kk==1
                wetcorrection1 = wetcorrection;
                drycorrection1 = hydrcorrection;
            end
            if kk==2
                wetcorrection2 = wetcorrection;
                drycorrection2 = hydrcorrection;
            end
            clear wetcorrection hydrcorrection

        end
    end

    if sum(no_data)==0
        % note that this is a one way Zenith delay and not a slant delay.
        % Units are in cm

        % saving individual estimates based on the time-stamp
        outfile_wet_before = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZWD_before.xyz'];
        outfile_wet_after = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZWD_after.xyz'];
        outfile_dry_before = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZHD_before.xyz'];
        outfile_dry_after = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZHD_after.xyz'];

        fidwet_before = fopen(outfile_wet_before,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(wetcorrection1,[],1)]';
        tally = fwrite(fidwet_before,data_write,'double');
        fclose(fidwet_before);
        clear data_write tally %%%SSS 4/16
        fidwet_after = fopen(outfile_wet_after,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(wetcorrection2,[],1)]';
        tally = fwrite(fidwet_after,data_write,'double');
        fclose(fidwet_after);
        clear data_write tally %%%SSS 4/16
        fiddry_before = fopen(outfile_dry_before,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(drycorrection1,[],1)]';
        tally = fwrite(fiddry_before,data_write,'double');
        fclose(fiddry_before);
        clear data_write tally %%%SSS 4/16
        fiddry_after = fopen(outfile_dry_after,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(drycorrection2,[],1)]';
        tally = fwrite(fiddry_after,data_write,'double');
        fclose(fiddry_after);
        clear data_write tally %%%SSS 4/16

        % saving the outputs
        if save_3D_delays==1
            wet = wet1*f_before(d) +  wet2*f_after;
            dry = dry1*f_before(d) +  dry2*f_after;
            save([weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_3D.mat'],'dry','wet','hgt','lon','lat','hgt_topo')
            clear wet dry hgt dry1 dry2 wet1 wet2 %%%SSS 4/16
        end

        % Output wet correction
        wetcorrection = wetcorrection1*f_before(d) +  wetcorrection2*f_after(d);
        clear wetcorrection1 wetcorrection2
        wetcorrection = wetcorrection*100;                 % delay in [cm]
        fid = fopen(outfile,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(wetcorrection,[],1)]';
        tally = fwrite(fid,data_write,'double');
        fclose(fid);
        clear data_write tally wetcorrection %%%SSS 4/16

        % Output hydrostatic correction
        hydrcorrection = drycorrection1*f_before(d) +  drycorrection2*f_after(d);
        clear drycorrection1 drycorrection2
        hydrcorrection = hydrcorrection*100;                 % delay in [cm]
        fid = fopen(hydroutfile,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(hydrcorrection,[],1)]';
        tally = fwrite(fid,data_write,'double');
        fclose(fid);
        clear data_write tally hydrcorrection %%%SSS 4/16

        fprintf([num2str(d) ' completed out of ' num2str(n_dates) '\n' ])

    else
        fprintf([num2str(d) ' completed out of ' num2str(n_dates) ' (NO DATA)\n' ])
    end
end


function varargout = waterfall(varargin)
%WATERFALL   Waterfall plot of a SPHEREFUN.
%   WATERFALL(F) displays the waterfall plot of F.
%
%   WATERFALL(F, S) displays the column and row chebfuns of F that are used for
%   its approximation.  This is a 3D version of plot(f,S), where S is a string
%   (see PLOT).
%
%   WATERFALL(F, S, 'nslices', N) displays the first min(N,length(f)) column
%   and rows.
%
%   WATERFALL supports passing options to the plot, similar to standard Matlab
%   plot commands. The options supported are:
%       'color':      Color of lines and markers plotted.
%       'marker':     Marker for pivot points.
%       'markersize': Size of markers plotted at pivot points.
%
%   H = WATERFALL(...) returns a handle to a waterfall plot object.
%
% See also SPHEREFUN/PLOT.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

[varargout{1:nargout}] = waterfall@separableApprox(varargin{:});

end


function [dat, volInfo, masked_vol] = iimg_mask(maskindx,dat,varargin)
% Masks index image vector or filenames in dat
% With img vector or filenames in maskindx
% * Checks dimensions to make sure origins and vox. sizes are same
%
% :Usage:
% ::
%
%     [masked_indx, volInfo, masked_vol] = iimg_mask(maskindx,dat,[volInfo], [outname])
%
% Fastest way to mask an image:
% ::
%
%    [masked_indx_data, volInfo] = iimg_mask(maskindx,dat)
%
%    % or
%
%    [masked_indx, xyz, masked_vol] = iimg_mask(mask filename,image filenames)
%
% even slower: reconstruct a 3-D volume and return in masked_vol

[volInfo, maskindx] = iimg_read_img(maskindx,1);
[volInfod, dat] = iimg_read_img(dat);

% input volInfo overrides; in case maskindx is data vector instead of
% filename
if length(varargin) > 0 && ~isempty(varargin{1}), volInfo = varargin{1}; end

% deal with multiple images
% --------------------------------------------
[nvox,nimgs] = size(dat);

% reduce dat, if maskindx is reduced to in-mask only
% if both are image files, this accomplishes the masking
% --------------------------------------------
if nvox == volInfo.nvox && size(maskindx,1) ~= volInfo.nvox
    dat = dat(volInfo.wh_inmask,:);
end

% check sizes
% --------------------------------------------
if size(maskindx,1) - size(dat,1)
    error('Sizes of mask and image data do not match.');
end

if isstruct(volInfo) && isstruct(volInfod)
    % voxel sizes and origins
    m = [diag(volInfo.mat(1:3,1:3))' volInfo.mat(1:3,4)'];
    m2 = [diag(volInfod.mat(1:3,1:3))' volInfod.mat(1:3,4)'];

    if any(m-m2)
        error('Voxel sizes or origins for mask and image data do not match.');
    end
end

% masking
% --------------------------------------------
dat(~maskindx) = 0;

if nargout > 2
    % slower, reconstruct mask
    %if nargin < 3, error('You must enter volume structure V to reconstruct 3-D vol');, end

    for i = 1:nimgs
        if length(varargin) > 1
            outname = varargin{2};
            masked_vol(:,:,:,i) = iimg_reconstruct_3dvol(dat(:,i),volInfo, 'outname', outname);

        else
            masked_vol(:,:,:,i) = iimg_reconstruct_3dvol(dat(:,i),volInfo);
        end

    end
end

return


function pHW=x2p_(xHW,layer,convnet)
Stride=convnet.targetStride(layer);
centerStart=convnet.targetCenter(layer);
pHW=centerStart+(xHW-1).*Stride;
end


function classplot2random(X,y,flag,colorstr,MarkerSize)
%+++ flag: 0 No marker;
%          1 using clorstr
%          2 using class number;

if nargin<5;MarkerSize=5;end
if nargin<4;colorstr={'g.';'b.';'k*';'gp';'cs';'g>';'c.';};end
if nargin<3;flag=0;end

nclass=unique(y);
n=length(y);
nperm=randperm(n);
hold on;
for i=1:n
  permi=nperm(i);
  if y(permi)==nclass(1);colortemp=colorstr{1};else; colortemp=colorstr{2};end
  plot(X(permi,1),X(permi,2),colortemp,'MarkerSize',MarkerSize);
end