function value = sum(SO3F, varargin)
% Calculates the integral of a SO3FunHarmonic based on
%
% $$ v = \int f(R) dR $$
%
% with $vol(SO(3)) = \int_{SO(3)} 1 dR = 8\pi^2$.
%
% If there is a second argument it sums up along a specified dimension of a
% vector-valued SO3FunHarmonic.
%
% Syntax
%   value = sum(SO3F)
%   SO3F = sum(SO3F, d)
%
% Input
%  SO3F - @SO3FunHarmonic
%  d    - dimension to take the sum value over
%
% Output
%  SO3F  - @SO3FunHarmonic
%  value - double
%
% Description
%
% SO3F is a 3x3 SO3Fun
% sum(SO3F) returns a 3x3 matrix with the integrals of each function
% sum(SO3F, 1) returns a 1x3 SO3Fun which contains the pointwise sums along the first dimension
%

if nargin == 1
  value = 8*pi^2*mean(SO3F,varargin{:});
else
  SO3F.fhat = sum(SO3F.fhat, varargin{1}+1);
  value = SO3F;
end

end


function [ imgOut ] = hyperOrthorectify( imgIn, altitude, hpbw )
%HYPERORTHORECTIFY Orthorectifies areal observed data.
%   Orthorectifies areal observed data using nearest neighbor interpolation.
%
% Inputs
%   imgIn       Input image (m x n) or (m x n x p)
%   altitude    Sensor altitude (meters)
%   hpbw        Half power beam width (radians).
% Outputs
%   imgOut      Orthorectified image.

% Input parameters
if (ndims(imgIn) == 2)
    [h, w] = size(imgIn);
    p = 1;
elseif (ndims(imgIn) == 3)
    [h, w, p] = size(imgIn);
end

radPerPix = hpbw/w;
x = tan(hpbw/2)*altitude;  % m
gsd = altitude*radPerPix;  % m
n = x/gsd;

outImg = zeros(h, floor(n)*2, p);
for k=1:p
    for j=1:h
        for i=-floor(n):1:floor(n)-1
            boresiteDistance = gsd*i;
            theta = atan(boresiteDistance/altitude);
            imagePix = round(theta / radPerPix);
            imgOut(j, floor(n)+i+1, k) = imgIn(j, (w/2)+imagePix+1, k);
        end
    end
end


%
% Smooth point-set registration method using neighboring constraints
% -------------------------------------------------------------------
%
% Authors: Gerard Sanromà, René Alquézar and Francesc Serratosa
%
% Contact: gsanorma@gmail.com
% Date: 15/02/2012
%
% Demo of the non-rigid method
%
% For the execution of the non-rigid version you will need the 'ctps_gen'
% and 'ctps_warp_pts' functions from the TPS-RPM implementation by
% Haili Chui and Anand Rangarajan ("A new algorithm for non-rigid point
% matching",IEEE CVPR 2000) available from:
% http://www.cise.ufl.edu/~anand/students/chui/rpm/TPS-RPM.zip
%
% For the correct execution of this demo you will need the synthetic
% datasets by Dr. Haili Chui and Prof. Anand Rangarajan, available at:
% http://www.umiacs.umd.edu/~zhengyf/PointMatchDemo/DataChui.zip
%

clear variables;

if exist('ctps_gen') ~= 2 || exist('ctps_warp_pts') ~= 2
    disp('The MATLAB m-files (i.e., functions) "ctps_gen" and "ctps_warp_pts" are not available');
    disp('You can get them from the TPS-RPM implementation by Haili Chui and Anand Rangarajan');
    disp('"A new algorithm for non-rigid point matching",IEEE CVPR 2000');
    disp('http://www.cise.ufl.edu/~anand/students/chui/rpm/TPS-RPM.zip');
    return;
end

if exist('DataChui','dir') == 0
    disp('Fish and Chinese character datasets are not available');
    disp('You can download them from:')
    disp('http://www.umiacs.umd.edu/~zhengyf/PointMatchDemo/DataChui.zip');
    return;
end

% Type of experiments (uncomment the appropriate one)
exp_type = 'fish_def';
% exp_type = 'fish_noise';
% exp_type = 'fish_outlier';
% exp_type = 'chinese_def';
% exp_type = 'chinese_noise';
% exp_type = 'chinese_outlier';

% Path to the Chui's dataset
path = 'DataChui';
file_pattern = strcat('save_',exp_type,'_');

sample_range = 1:100;
if ~isempty(strfind(exp_type,'noise'))
    perturb_range = 1:6;
else
    perturb_range = 1:5;
end

E = inf(length(perturb_range),length(sample_range));

for perturb = perturb_range
    for sample = sample_range

        disp(['perturb = ',num2str(perturb),' sample = ',num2str(sample)]);

        % Read sample
        load(fullfile(path,strcat(file_pattern,num2str(perturb),'_',num2str(sample))),...
            'x1','y2a');
        gM = [-x1(:,2) x1(:,1)];
        gD = [-y2a(:,2) y2a(:,1)];
        lD = length(gD);
        lM = length(gM);

        if ~isempty(strfind(exp_type,'outlier'))
            vgM = match_chui_outl(gM,gD);
        else
            vgM = match_chui(gM,gD);
        end

        % gm
        r = gD(1:lM,:) - vgM;
        E(perturb,sample) = mean(sqrt(diag(r*r')));
        disp(['ERROR=',num2str(E(perturb,sample))]);

    end
end

save(strcat('nonrigid_',exp_type),'E');


%SF_SIMP_P1BUB Linear Lagrange shape function for simplices with bubble (P1+).
%
%   [ VBASE, NLDOF, XLDOF, SFUN ] = SF_SIMP_P1BUB( I_EVAL, N_SDIM, N_VERT, I_DOF, XI, AINVJAC, VBASE )
%   Evaluates conforming linear P1 Lagrange shape functions on simplices
%   an additional with bubble function. XI Barycentric coordinates.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       i_eval      scalar:  1             Evaluate function values
%                           >1             Evaluate values of derivatives
%       n_sdim      scalar: 1-3            Number of space dimensions
%       n_vert      scalar: 2-4            Number of vertices per cell
%       i_dof       scalar: 1-n_ldof       Local basis function to evaluate
%       xi          [n_sdim+1]             Local coordinates of evaluation point
%       aInvJac     [n,n_sdim+1*n_sdim]    Inverse of transformation Jacobian
%       vBase       [n]                    Preallocated output vector
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       vBase       [n]                    Evaluated function values
%       nLDof       [4]                    Number of local degrees of freedom on
%                                          vertices, edges, faces, and cell interiors
%       xLDof       [n_sdim,n_ldof]        Local coordinates of local dofs
%       sfun        string                 Function name of called shape function
%
%   See also SF_SIMP_P1

% Copyright 2013-2022 Precise Simulation, Ltd.

% Evaluation type flag.


function varargout = hikmeans(varargin)
% VL_HIKMEANS  Hierachical integer K-means
%   [TREE,ASGN] = VL_HIKMEANS(DATA,K,NLEAVES) applies integer K-menas
%   recursively to cluster the data DATA, returing a structure TREE
%   representing the clusters and a vector ASGN with the data to
%   cluster assignments. The depth of the recursive partition is
%   computed so that at least NLEAVES are generated.
%
%   VL_HIKMEANS() is built on top of VL_IKMEANS() and requires the
%   data to be of class UINT8.
%
%   TREE is a structure representing the hierarchical clusters.  Each
%   node of the tree is also a structure with fields:
%
%   DEPTH::
%     Depth of the tree (only at the root node)
%
%   CENTERS::
%     K cluster centers
%
%   SUB::
%     Array of K node structures representing subtrees
%     (this field is missing at leaves).
%
%   ASGN is a matrix with one column per datum and height equal to the
%   depth of the tree. Each column encodes the branch of the tree that
%   correspond to each datum.
%
%   Example::
%     ASGN(:,7) = [1 5 3] means that the tree as depth equal to 3 and
%     that the datum X(:,7) corresponds to the branch
%     ROOT->SUB(1)->SUB(5)->SUB(3).
%
%   See also: VL_HIKMEANSPUSH(), VL_HIKMEANSHIST(), VL_IKMEANS(), VL_HELP().
[varargout{1:nargout}] = vl_hikmeans(varargin{:});


% --------------------------------------------------------------------------- %
% MarrRevisited - Surface Normal Estimation
% Copyright (c) 2016 Adobe Systems Incorporated and Carnegie Mellon University.
% All rights reserved.[see LICENSE for details]
% -------------------------------------------------------------------------- %

% Written by Aayush Bansal. Please contact ab.nsit@gmail.com
function[nums_e] = eval_pred_sn(cache_dir, cache_list)

	num_images = length(cache_list);
	for i = 1:num_images

		% load the file from cache
		display(['Loading image: ', num2str(i, '%06d'),'/',...
					    num2str(num_images, '%06d')]);

		% CHANGE THE NAME OF THE DATA FILE -- if
		pred = load([cache_dir, '/', num2str(cache_list(i), '%06d'),...
							 '.mat'],  'predns');
	        gtd = load(['./dataset/NYU/GT_Normals/test/nm_',...
					 num2str(cache_list(i),'%06d') '.mat']);

		%
		NG = cat(3,gtd.nx,gtd.ny,gtd.nz);
		NV = gtd.depthValid;
		%
		NP = pred.predns;
		%normalize both to be sure
	        NG = bsxfun(@rdivide,NG,sum(NG.^2,3).^0.5);
                NP = bsxfun(@rdivide,NP,sum(NP.^2,3).^0.5);
		%compute the dot product, and keep on the valid
		DP = sum(NG.*NP,3);
		T = min(1,max(-1,DP));
		pixels{i} = T(find(NV));
	end

	E = acosd(cat(1,pixels{:}));
	nums_e = [mean(E(:)),median(E(:)),mean(E.^2).^0.5,mean(E < 11.25)*100,mean(E < 22.5)*100,mean(E < 30)*100]
	display('---------------------------------------');
	display(['Mean: ', num2str(mean(E(:)))]);
	display(['Median: ', num2str(median(E(:)))]);
	display(['RMSE: ', num2str(mean(E.^2).^0.5)]);
	display(['11.25: ', num2str(mean(E < 11.25)*100)]);
	display(['22.5: ', num2str(mean(E < 22.5)*100)]);
	display(['30: ', num2str(mean(E < 30)*100)]);
	display(['45: ', num2str(mean(E < 45)*100)]);
	display('---------------------------------------');
end


seed = 1235;
rng(seed);

[Batch,~,~] = generate_data(5000,[10,10]',[10,10]',0.0,[0,0]',[1,1]',0,1,0,0,0);
% generate_data(T,Nneurons,Dt,NeuronNoise,SeqNoiseTime,SeqNoiseNeuron,shared,diff,stretch,bin,seed)

L = 250; K = 4;
figure(1)
[W,H,temp,Loadings,Power] = seqNMF(Batch, ...
    'K', K, 'L', L, 'lambda',.0005, ...
    'showPlot', 1, 'maxiter', 100, 'shift', 1,...
    'lambdaOrthoW', 0,'lambdaOrthoH',0);
SimpleWHPlot(W,H,[],0)
savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'SharedNeurons';
papersize = [8 6];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_noshared.pdf']));

[W,H,temp,Loadings,Power] = seqNMF(Batch, ...
    'K', K, 'L', L, 'lambda',.00000, ...
    'showPlot', 1, 'maxiter', 100, 'shift', 1,...
    'lambdaOrthoW', 0,'lambdaOrthoH',0);
SimpleWHPlot(W,H,[],0)
savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'SharedNeurons';
papersize = [8 6];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_cnmf.pdf']));

[Batch,~,~] = generate_data(5000,[10,10]',[10,10]',0.0,[0,0]',[1,1]',1,1,0,0,0);
[W,H,temp,Loadings,Power] = seqNMF(Batch, ...
    'K', K, 'L', L, 'lambda',.0005, ...
    'showPlot', 1, 'maxiter', 100, 'shift', 1,...
    'lambdaOrthoW', 0,'lambdaOrthoH',0);
SimpleWHPlot(W,H,[],0)
savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'SharedNeurons';
papersize = [8 6];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_sharedDiff.pdf']));

[Batch,~,~] = generate_data(5000,[10,10]',[10,10]',0.0,[0,0]',[1,1]',1,0,0,0,0);
[W,H,temp,Loadings,Power] = seqNMF(Batch, ...
    'K', K, 'L', L, 'lambda',.0005, ...
    'showPlot', 1, 'maxiter', 100, 'shift', 1,...
    'lambdaOrthoW', 0,'lambdaOrthoH',0);
SimpleWHPlot(W,H,[],0)
savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
saveTitle = 'SharedNeurons';
papersize = [8 6];
set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
    saveas(gcf, fullfile(savedir, [saveTitle '_sharedSame.pdf']));
%% Example parts-based vs events-based factorizations (W or H soft orthogonality)

% change these parameters to switch between parts-based and events-based
lambdaOrthoH = 0; % favor events-based (these can take any value, don't need to be zero and one)
lambdaOrthoW = 1; % favor parts-based

% rng(236); % fixed rng seed for reproduceability
X = trainNEURAL;
K = 3;
L = 2/3; % units of seconds
Lneural = ceil(L*VIDEOfs);
Lsong = ceil(L*SONGfs);
shg
display('For parts-based, set lambdaOrthoW=1 and lambdaOrthoH=0; for events-based, vice versa')
[W, H, ~,loadings,power]= seqNMF(X,'K',K,'L',Lneural,...
            'lambda', .00, 'maxiter', 100, 'showPlot', 1,...
            'lambdaOrthoH', lambdaOrthoH, 'lambdaOrthoW', lambdaOrthoW);

p = .05; % desired p value for factors

display('Testing significance of factors on held-out data')
[pvals,is_significant] = test_significance(testNEURAL,W,p);

W = W(:,is_significant,:);
H = H(is_significant,:);

% plot, sorting neurons by latency within each factor
[max_factor, L_sort, max_sort, hybrid] = helper.ClusterByFactor(W(:,:,:),1);
indSort = hybrid(:,3);
tstart = 180; % plot data starting at this timebin
figure; WHPlot(W(indSort,:,:),H(:,tstart:end), X(indSort,tstart:end), 0,trainSONG(:,floor(tstart*SONGfs/VIDEOfs):end),0)
title(['lambdaOrthoW=' num2str(lambdaOrthoW) ', lambdaOrthoH=' num2str(lambdaOrthoH)])

% savedir = 'C:\Users\emackev\Dropbox (MIT)\SeqNMF\Figures';
% saveTitle = 'PartsBasedOrthW1';
% papersize = [8 6];
% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_raw.pdf']));

% figure; WHPlot(W(indSort,:,:),H(:,tstart:end), ...
%     helper.reconstruct(W(indSort,:,:),H(:,tstart:end)),...
%     0,trainSONG(:,floor(tstart*SONGfs/VIDEOfs):end),0)
% title('SeqNMF reconstruction')

% set(gcf, 'papersize', papersize, 'paperposition', [0 0 papersize]);%, 'color', 'none')
%     saveas(gcf, fullfile(savedir, [saveTitle '_reconstruction.pdf']));


function test_ft_plot_mesh

% MEM 2gb
% WALLTIME 00:15:00
% DEPENDENCY ft_plot_mesh

%% the first is a simple triangluar mesh

bnd.pos = randn(20,3);
bnd.tri = delaunay(bnd.pos(:,1:2));

figure
ft_plot_mesh(bnd);

%% the following was made by Lilla Magyari in 2013 and pertains to FEM meshes

% create an example segmentation
example.dim = [91 104 111]; % slightly different numbers
example.transform = eye(4);
example.coordsys = 'ctf';
example.unit = 'mm';
example.seg = zeros(example.dim);

% adjusting transformation matrix: center of the head-coordinates [0 0 0] should
% be the center of volume

% center of volume in voxel coordinates
x = round(example.dim(1)/2);
y = round(example.dim(2)/2);
z = round(example.dim(3)/2);

x = round(x);
y = round(y);
z = round(z);

origin = [x y z];

example.transform(1:4,4) = [-origin(:); 1]; % head-coordinate [0 0 0] is in the center of
% the volume (x y z in voxel-coordinates)

% compute position for each voxel in voxelspace and in headspace

[X, Y, Z] = ndgrid(1:example.dim(1), 1:example.dim(2), 1:example.dim(3));
voxelpos = [X(:) Y(:) Z(:)];
headpos = ft_warp_apply(example.transform, voxelpos);

% create 3 spheres

radius1 = 40;
radius2 = 30;
radius3 = 20;

for i = 1:size(headpos,1)
  % from small to large
  if norm(headpos(i,:))<radius3
    example.seg(i) = 3;
  elseif norm(headpos(i,:))<radius2
    example.seg(i) = 2;
  elseif norm(headpos(i,:))<radius1
    example.seg(i) = 1;
  end
end

% convert it to probabilistic
seg = ft_datatype_segmentation(example,'segmentationstyle','probabilistic');

% create smaller segmentation

example.dim = [10 11 9]; % slightly different numbers
example.transform = eye(4);
example.coordsys = 'ctf';
example.unit = 'mm';
example.seg = zeros(example.dim);

% adjusting transformation matrix: center of the head-coordinates [0 0 0] should
% be the center of volume

% center of volume in voxel coordinates
x = round(example.dim(1)/2);
y = round(example.dim(2)/2);
z = round(example.dim(3)/2);

x = round(x);
y = round(y);
z = round(z);

origin = [x y z];

example.transform(1:4,4) = [-origin(:); 1]; % head-coordinate [0 0 0] is in the center of
% the volume (x y z in voxel-coordinates)

% compute position for each voxel in voxelspace and in headspace

[X, Y, Z] = ndgrid(1:example.dim(1), 1:example.dim(2), 1:example.dim(3));
voxelpos = [X(:) Y(:) Z(:)];
headpos = ft_warp_apply(example.transform, voxelpos);

% create 3 spheres

radius1 = 4;
radius2 = 3;
radius3 = 2;

for i = 1:size(headpos,1)
  % from small to large
  if norm(headpos(i,:))<radius3
    example.seg(i) = 3;
  elseif norm(headpos(i,:))<radius2
    example.seg(i) = 2;
  elseif norm(headpos(i,:))<radius1
    example.seg(i) = 1;
  end
end

% convert it to probabilistic
seg_small = ft_datatype_segmentation(example,'segmentationstyle','probabilistic');

%% create mesh
cfg = [];
cfg.method = 'hexahedral';
%cfg.tissue =
mesh = ft_prepare_mesh(cfg,seg);

% 1 cube mesh
mesh0.pos = mesh.pos;
mesh0.hex = mesh.hex(1,:);
mesh0.tissue = mesh.tissue(1,:);
mesh0.tissuelabel = mesh.tissuelabel;
mesh0.unit = mesh.unit;

% few hundreds cube mesh

cfg = [];
cfg.method = 'hexahedral';
%cfg.tissue =
mesh2 = ft_prepare_mesh(cfg,seg_small);

%% plot mesh

% larger mesh
figure; ft_plot_mesh(mesh,'surfaceonly','yes');
% smaller mesh
figure; ft_plot_mesh(mesh2,'surfaceonly','yes');
% smallest mesh
figure; ft_plot_mesh(mesh0,'surfaceonly','yes');


function resized_patch = sample_patch(im, pos, sample_sz, output_sz, gparams)

if nargin < 4
    output_sz = [];
end
if nargin < 5 || ~isfield(gparams, 'use_mexResize')
    gparams.use_mexResize = false;
end

% Pos should be integer when input, but floor in just in case.
pos = floor(pos);

% Downsample factor
resize_factor = min(sample_sz ./ output_sz);
df = max(floor(resize_factor - 0.1), 1);
if df > 1
    % pos = 1 + of + df * (npos - 1)

    % compute offset and new center position
    os = mod(pos - 1, df);
    pos = (pos - 1 - os) / df + 1;

    % new sample size
    sample_sz = sample_sz / df;

    % donwsample image
    im = im(1+os(1):df:end, 1+os(2):df:end, :);
end

% make sure the size is not too small and round it
sample_sz = max(round(sample_sz), 2);

xs = pos(2) + (1:sample_sz(2)) - floor((sample_sz(2)+1)/2);
ys = pos(1) + (1:sample_sz(1)) - floor((sample_sz(1)+1)/2);

%check for out-of-bounds coordinates, and set them to the values at
%the borders
xs(xs < 1) = 1;
ys(ys < 1) = 1;
xs(xs > size(im,2)) = size(im,2);
ys(ys > size(im,1)) = size(im,1);

%extract image
im_patch = im(ys, xs, :);

if isempty(output_sz) || isequal(sample_sz(:), output_sz(:))
    resized_patch = im_patch;
else
    if gparams.use_mexResize
        resized_patch = mexResize(im_patch, output_sz, 'linear');
    else
        resized_patch = imresize(im_patch, output_sz, 'bilinear', 'Antialiasing',false);
    end
end

end


function [X,y,isContinuous] = designMatrix(tbl,allTerms,responseVar,varargin)
% Extract a design matix from a linear model.
%  Dummy coded designs for categorical variables and zero-sum covariates for
%  contiunuous variables are returned as X
% INPUT
% tbl =  A table
% allTerms = A cell array of variable names to include in the
% design matrix ( e.g. {'ori','freq','ori:freq'} for two mains and an interaction)
% Parm/Value
% ZeroSumConstraint -  Toggle to apply the zero sum constraint to
%                   each categorical factor (This equates the marginal prior across terms in the
%                           factor; see Rouder et al) [true]
% treatAsRandom  - A char or a cell array of chars with factors that should be
%               treated as random (i.e. not fixed) effects.
%               [{}].
% forceCategorical - Logical to force each term to be treated as a
% categorical variable (useful for grouping variables like subject ID which
% may look continuous...)
% OUTPUT
% X = The complete design matrix. Cell array with one element per term.
% y = The response data
% isContinuous = Logical indicating which columns are continuous co-variates.
%
% BK - 2019.

p = inputParser;
p.addParameter('zeroSumConstraint',true,@islogical);
p.addParameter('treatAsRandom',{},@(x) ischar(x) || iscell(x));
p.addParameter('forceCategorical',false,@islogical);
p.parse(varargin{:});
treatAsRandom =p.Results.treatAsRandom;
if ischar(treatAsRandom);treatAsRandom = {treatAsRandom};end

nrAllTerms =numel(allTerms);
X = cell(1,nrAllTerms);
N = height(tbl);

%Options for modelutils.designmatrix . Its internal alg determines which
%vars are categorical quite well. But use categorical() in the data table
%to be sure (or chars/strings).
isCategorical = bf.internal.isCategorical(tbl);
if p.Results.forceCategorical
    isCategorical = true(size(isCategorical));
end
cateoricalOpts = {'model','linear','intercept',false,'DummyVarCoding','full','responseVar',responseVar,'CategoricalVars',isCategorical};
isContinuous = false(1,nrAllTerms);
for i=1:nrAllTerms
    if any(allTerms{i}==':')
        % An interaction term.
       % names = strsplit(allTerms{i},':')
        aName =extractBefore(allTerms{i},':');
        bName = extractAfter(allTerms{i},':');
        aCategorical  = bf.internal.isCategorical(tbl,aName);
        bCategorical = bf.internal.isCategorical(tbl,bName);
        bothCategorical = aCategorical &&  bCategorical;
        bothContinuous = ~aCategorical && ~bCategorical;
        if bothCategorical || p.Results.forceCategorical
            thisA = classreg.regr.modelutils.designmatrix(tbl,'PredictorVars',aName,cateoricalOpts{:});
            if ~ismember(aName,treatAsRandom) && p.Results.zeroSumConstraint
                thisA = bf.internal.zeroSumConstraint(thisA);
            end
            thisB = classreg.regr.modelutils.designmatrix(tbl,'PredictorVars',bName,cateoricalOpts{:});
            if ~ismember(bName,treatAsRandom) && p.Results.zeroSumConstraint
                thisB = bf.internal.zeroSumConstraint(thisB);
            end
            thisX = bf.internal.interaction(thisA,thisB);
        elseif bothContinuous
            thisX =  tbl.(aName).*tbl.(bName);
            thisX = thisX - mean(thisX);
            isContinuous(i) = true;
        else
            % Interaction between a categorical and a continuous covariate
            if aCategorical
                thisA = classreg.regr.modelutils.designmatrix(tbl,'PredictorVars',aName,'intercept',false,'model','linear','responseVar',tbl.(responseVar),'DummyVarCoding','full');
                if ismember(bName,allTerms)
                    % B is already included as a main effect, have to
                    % remove one level from A to avoid colinearity,
                     thisA = bf.internal.zeroSumConstraint(thisA);
%                    thisA = thisA(:,2:end); % Remove first category
                end
            else
                thisA = tbl.(aName);
            end
            if bCategorical
                thisB = classreg.regr.modelutils.designmatrix(tbl,'PredictorVars',bName,'intercept',false,'model','linear','responseVar',tbl.(responseVar),'DummyVarCoding','full');
                if ismember(aName,allTerms)
                    % A is already included as a main effect, have to
                    % remove one level from B to avoid colinearity,
                    thisB = bf.internal.zeroSumConstraint(thisB);
                    %thisB = thisB(:,2:end); % Remove first category
                end
            else
                thisB = tbl.(bName);
            end
            thisX  =  thisA.*thisB;
            thisX  = thisX-mean(thisX);
            isContinuous(i) = true;
        end
    else
        % A main term
        if  bf.internal.isCategorical(tbl,allTerms{i}) || p.Results.forceCategorical
            thisX = classreg.regr.modelutils.designmatrix(tbl,'PredictorVars',allTerms{i},cateoricalOpts{:});
            % Sum-to-zero contrasts that equates marginal priors across levels.
            if ~ismember(allTerms{i},treatAsRandom) && p.Results.zeroSumConstraint
                thisX = bf.internal.zeroSumConstraint(thisX);
            end
        else %Continuous
             thisX =  tbl.(allTerms{i});
             thisX  = thisX-mean(thisX);
            isContinuous(i)= true;
        end

    end
    X{i} =thisX; % Store for later use
end

if nargout>1
    y= tbl.(responseVar);
end
end


function [ feature_image ] = get_fhog(im, fparam, gparam )
%extract fhog features using piotrs toolbox. Currently takes no parameters
%except hog-cell-size
if ~isfield(fparam, 'nOrients')
    fparam.nOrients = 9;
end
[im_height, im_width,~, num_images] = size(im);
feature_image = zeros(floor(im_height/gparam.cell_size), floor(im_width/gparam.cell_size), fparam.nDim, num_images, 'single');
for k = 1:num_images
    hog_image = fhog(single(im(:,:,:,k)), gparam.cell_size, fparam.nOrients);
    %the last dimension is all 0 so we can discard it
    feature_image(:,:,:,k) = hog_image(:,:,1:end-1);
end
end


function [beta_gibbs,sigma_gibbs,beta_mean,sigma_mean,lambda_posterior]=panel4gibbs(N,n,h,T,k,q,Yi,Xi,s0,omegab,v0,It,Bu,pick,pickf)

% compute first  preliminary elements
% compute sbar
sbar=h+s0;
% compute the inverse of omegab
invomegab=diag(1./diag(omegab));
% initiate the Gibbs sampler
% initiate the counting of iterations
count=1;
pickcount=1;
% initiate the record matrices
beta_gibbs=zeros(q,It-Bu,N);
sigma_gibbs=zeros(n^2,It-Bu,N);
beta_mean=zeros(q,It-Bu);
sigma_mean=zeros(n^2,It-Bu);
lambda_posterior=zeros((It-Bu),1);

% step 1: compute initial values
% initial value for beta (use OLS values)
for ii=1:N
beta(:,ii)=bear.vec((Xi(:,:,ii)'*Xi(:,:,ii))\(Xi(:,:,ii)'*Yi(:,:,ii)));
end
% initial value for b
b=(1/N)*sum(beta,2);
% initial value for lambda1
lambda1=0.01;
sigmab=lambda1*omegab;
% initial value for sigma (use OLS values)
for ii=1:N
eps=Yi(:,:,ii)-Xi(:,:,ii)*reshape(beta(:,ii),k,n);
sigma(:,:,ii)=(1/(T-k-1))*eps'*eps;
end

hbar = bear.parfor_progressbar(It-Bu,'Progress of Panel BVAR Gibbs Sampler');  %create the progress bar

% run the Gibbs sampler
while count<=It

% step 2: obtain b
% first compute betam, the mean value of the betas over all units
betam=(1/N)*sum(beta,2);
% draw b from a multivariate normal N(betam,(1/N)*sigmab))
b=betam+chol(bear.nspd((1/N)*sigmab),'lower')*mvnrnd(zeros(q,1),eye(q))';

% step 3: obtain sigmab
% compute first vbar
for ii=1:N
temp(1,ii)=(beta(:,ii)-b)'*invomegab*(beta(:,ii)-b);
end
vbar=v0+sum(temp,2);
% compute lambda1
lambda1=bear.igrandn(sbar/2,vbar/2);
% recover sigmab
sigmab=lambda1*omegab;

% step 4: draw the series of betas
% first obtain the inverse of sigmab
invsigmab=diag(1./diag(sigmab));
% then loop over units
for ii=1:N
% take the choleski factor of sigma of unit ii, inverse it, and obtain from it the inverse of the original sigma
C=bear.trns(chol(bear.nspd(sigma(:,:,ii)),'Lower'));
invC=C\speye(n);
invsigma=invC*invC';
% obtain omegabar
invomegabar=kron(invsigma,Xi(:,:,ii)'*Xi(:,:,ii))+invsigmab;
% invert
C=bear.trns(chol(bear.nspd(invomegabar),'Lower'));
invC=C\speye(q);
omegabar=invC*invC';
% obtain betabar
betabar=omegabar*(kron(invsigma,Xi(:,:,ii)')*bear.vec(Yi(:,:,ii))+invsigmab*b);
% draw beta
beta(:,ii)=betabar+chol(bear.nspd(omegabar),'lower')*mvnrnd(zeros(q,1),eye(q))';
end

% step 5: draw the series of sigmas
% loop over units
for ii=1:N
% compute Stilde
Stilde=(Yi(:,:,ii)-Xi(:,:,ii)*reshape(beta(:,ii),k,n))'*(Yi(:,:,ii)-Xi(:,:,ii)*reshape(beta(:,ii),k,n));
% draw sigma
sigma(:,:,ii)=bear.iwdraw(Stilde,T);
end

   % record phase
   % if the burn-in sample phase is not yet over
   if count<=Bu
   % simply add 1 to the iteration count
   count=count+1;
   % on the other hand, if the burn-in sample phase is over
   elseif count>Bu
   % adding one iteration to the count will depend on wether post-burn selection applies
      % if there is no post burn selection
      if pick==0
      % record the draw
         % loop over units
         for ii=1:N
         beta_gibbs(:,count-Bu,ii)=beta(:,ii);
         sigma_gibbs(:,count-Bu,ii)=bear.vec(sigma(:,:,ii));
         end
      % and add one to the count
      count=count+1;
      % if there is post burn selection, only one draw over 'fpick' draws will be retained
      elseif pick==1
         % if the iteration does not correspond to fpick, don't record the results, don't increase the regular count, but do increase pickcount by 1
         if pickcount~=pickf
         pickcount=pickcount+1;
         % on the other hand, if the iteration does correspond to fpick
         elseif pickcount==pickf
         % do record the results
            % loop over units
            beta_mean(:,count-Bu)=b;
            sigma_mean(:,count-Bu)=bear.vec(mean(sigma,3));
            lambda_posterior(count-Bu)=lambda1;
            for ii=1:N
            beta_gibbs(:,count-Bu,ii)=beta(:,ii);
            sigma_gibbs(:,count-Bu,ii)=bear.vec(sigma(:,:,ii));
            end
         % then increase the regular count by 1 and re-initialise pickcount
         count=count+1;
         pickcount=1;
         end
      end
   end

   hbar.iterate(1);   % update progress by one iteration

end

close(hbar);   %close progress bar


function sensor = sensorfield(x, y)
    xc = 60; yc = 90;
    sensor = 200./((x-xc).^2 + (y-yc).^2 + 200);


function h=m_streamline(long,lat,u,v,varargin)
% M_STREAMLINE Makes a quiverplot on a map (QUIVER-style)
%    M_STREAMLINE(LONG,LAT,U,V) draws well-spaced streamlines (with direction
%    arrows) with components (U,V) at the points (LONG,LAT) on the currently
%    defined map.The arrays LONG and LAT, which define the coordinates for U and
%    V, must be monotonic, but do not need to be uniformly spaced. The
%    matrices LONG,LAT,U,V must all be the same size. U and V contain the
%    eastward and northward components of velocity (in m/s or equivalent, NOT
%    degrees lat/long  per sec or equivalent). Arrow scaling is automatic.
%
%   Note - this is basically a call to STREAMSLICE, which has some limitations and
%   cannot be used for ALL projections. In particular, STREAMSLICE requires that
%
%            [X,Y]=m_ll2xy(LONG,LAT,'clip','point')
%
%   returns X/Y matrices that themselves must be monotonic and plaid (as if produced
%   by MESHGRID). This means that a) none of the points are from outside the map
%   boundaries (since the M_LL2XY call will turn them into NaN), and b) you are
%   probably limited to cylindrical projections (miller, mercator, equidistant
%   cylindrical).
%
%   M_STREAMLINE(...,density) modifies the automatic spacing of the streamlines.
%   Density must be greater than 0. The default value is 1; higher values
%   produce  more streamlines on each plane. For example, 2 produces
%   approximately twice as many streamlines, while 0.5 produces approximately
%   half as many.
%
%   M_STREAMLINE(...,'arrowsmode') determines if direction arrows are present
%   or not. arrowmode can be:
%
%   arrows   - Draw direction arrows on the streamlines (default).
%   noarrows - Do not draw direction arrows.
%
%   M_STREAMLINE(...,'method') specifies the interpolation method to use.
%   method can be
%
%   linear   - Linear interpolation (default)
%   cubic    - Cubic interpolation
%   nearest  - Nearest-neighbor interpolation
%   See interp3 for more information on interpolation methods.
%
%   M_STREAMLINE(axes_handle,...) plots into the axes object with the handle
%   axes_handle instead of into the current axes object (gca).
%
%   h = M_STREAMLINE(...) returns a vector of handles to the line objects
%   created.
%
%   [vertices arrowvertices] = M_STREAMLINE(...) returns two cell arrays of
%   vertices  for drawing the streamlines and the arrows. You can pass these
%   values to any of the streamline drawing functions (streamline, streamribbon,
%   streamtube).
%

% Shi Weiheng (tfoterye@gmail.com) 29/Jun/17
%
% Based on m_quiver written by Prof Rich Pawlowicz.
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
%  Nov/2017 - cleaned up comments.

global MAP_PROJECTION MAP_VAR_LIST

% Have to have initialized a map first

if isempty(MAP_PROJECTION)
  disp('No Map Projection initialized - call M_PROJ first!');
  return;
end

[X,Y]=m_ll2xy(long,lat,'clip','point');

if any(isnan(X(:)))
  error(['M_Map : ' mfilename ' : InvalidInputs - input data includes points outside the map area']);
end

[XN ,YN ]=m_ll2xy([long(:) long(:)]',[lat(:) lat(:)+.001]','clip','off');
[XE ,YE ]=m_ll2xy([long(:) long(:)+(.001)./cos(lat(:)*pi/180)]',[lat(:) lat(:)]','clip','off');
mU=u.*reshape(diff(XE),size(lat))*1000 + v.*reshape(diff(XN),size(lat))*1000;
mV=u.*reshape(diff(YE),size(lat))*1000 + v.*reshape(diff(YN),size(lat))*1000;

h=streamslice(X,Y,mU,mV,varargin{:});
set(h,'tag','m_streamline');

if nargout==0
 clear h
end


function K = covADD(cov, hyp, x, z, i)

% Additive covariance function using a 1d base covariance function
% cov(x^p,x^q;hyp) with individual hyperparameters hyp.
%
% k(x^p,x^q) = \sum_{r \in R} sf_r \sum_{|I|=r}
%                 \prod_{i \in I} cov(x^p_i,x^q_i;hyp_i)
%
% hyp = [ hyp_1
%         hyp_2
%          ...
%         hyp_D
%         log(sf_R(1))
%          ...
%         log(sf_R(end)) ]
%
% where hyp_d are the parameters of the 1d covariance function which are shared
% over the different values of R(1) to R(end).
%
% Please see the paper Additive Gaussian Processes by Duvenaud, Nickisch and
% Rasmussen, NIPS, 2011 for details.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

R = cov{1};
nh = eval(feval(cov{2}));           % number of hypers per individual covariance
nr = numel(R);                      % number of different degrees of interaction
if nargin<3                                  % report number of hyper parameters
  K = ['D*', int2str(nh), '+', int2str(nr)];
  return
end
if nargin<4, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);                                                % dimensionality
sf2 = exp( 2*hyp(D*nh+(1:nr)) );        % signal variances of individual degrees

Kd = Kdim(cov{2},hyp,x,z);                % evaluate dimensionwise covariances K
if nargin<5                                                        % covariances
  EE = elsympol(Kd,max(R));               % Rth elementary symmetric polynomials
  K = 0; for ii=1:nr, K = K + sf2(ii)*EE(:,:,R(ii)+1); end    % sf2 weighted sum
else                                                               % derivatives
  if i <= D*nh                       % individual covariance function parameters
    j = fix(1+(i-1)/nh);              % j is the dimension of the hyperparameter
    if dg, zj='diag'; else if xeqz, zj=[]; else zj=z(:,j); end, end
    dKj = feval(cov{2},hyp(nh*(j-1)+(1:nh)),x(:,j),zj,i-(j-1)*nh);  % other dK=0
    % the final derivative is a sum of multilinear terms, so if only one term
    % depends on the hyperparameter under consideration, we can factorise it
    % out and compute the sum with one degree less
    E = elsympol(Kd(:,:,[1:j-1,j+1:D]),max(R)-1);  %  R-1th elementary sym polyn
    K = 0; for ii=1:nr, K = K + sf2(ii)*E(:,:,R(ii)); end     % sf2 weighted sum
    K = dKj.*K;
  elseif i <= D*nh+nr
    EE = elsympol(Kd,max(R));             % Rth elementary symmetric polynomials
    j = i-D*nh;
    K = 2*sf2(j)*EE(:,:,R(j)+1);                  % rest of the sf2 weighted sum
  else
    error('Unknown hyperparameter')
  end
end

% evaluate dimensionwise covariances K
function K = Kdim(cov,hyp,x,z)
  [n,D] = size(x);                                              % dimensionality
  nh = eval(feval(cov));            % number of hypers per individual covariance
  if nargin<4, z = []; end                                 % make sure, z exists
  xeqz = numel(z)==0; dg = strcmp(z,'diag') && numel(z)>0;      % determine mode

  if dg                                                        % allocate memory
    K = zeros(n,1,D);
  else
    if xeqz, K = zeros(n,n,D); else K = zeros(n,size(z,1),D); end
  end

  for d=1:D
    hyp_d = hyp(nh*(d-1)+(1:nh));                 % hyperparamter of dimension d
    if dg
      K(:,:,d) = feval(cov,hyp_d,x(:,d),'diag');
    else
      if xeqz
        K(:,:,d) = feval(cov,hyp_d,x(:,d));
      else
        K(:,:,d) = feval(cov,hyp_d,x(:,d),z(:,d));
      end
    end
  end


function [ errors ] = test_sparsify(  )
    gsp_reset_seed(0);

errors = 0;
errors = errors + test1();

errors = errors + test2();

end

function [ errors ] = test1( )

errors = 0;
try
    epsilon = 0.4;
    param.distribute = 1;
    param.Nc = 20;
    G = gsp_sensor(256,param);
    G2 = gsp_graph_sparsify(G,epsilon);
    figure(100);
    gsp_plot_graph(G);
    title('Original graph')
    figure(101);
    gsp_plot_graph(G2);
    title('Sparsified graph')
    close(100);
    close(101);
   fprintf('SPARSIFY: test 1 ok\n');
catch
    errors = errors +1;
    warning('SPARSIFY: test 1 error')
end

end

function [ errors ] = test2( )

errors = 0;
    N = 100;
    epsilon = 0.6;
    G = gsp_sensor(N);
    gsp_reset_seed(0);
    G2 = gsp_graph_sparsify(G,epsilon);
    gsp_reset_seed(0);
    L = gsp_graph_sparsify_old(G.L,epsilon);

if sum(sum(abs(G2.L-L)))<1e-10

   fprintf('SPARSIFY: test  ok\n');
else
    errors = errors+1;
    badness = sum(sum(abs(G2.L-L)))
    warning('SPARSIFY: test 2 error')
end

end


function plotQuadCon(Q,l,rl,ru,data)
%PLOTQUADCON Plot Quadratic Constraints on the current figure
%   plotQuadCon(Q,l,r)

%   Copyright (C) 2013 Jonathan Currie (IPL)

xl = data.xl; yl = data.yl;

hold on;

%Colour
dkg = [0.2 0.2 0.2];

%Determine number of quad constraints
if(iscell(Q))
    no = length(Q);
else
    no = 1;
end
%>= 2014b requires more points...
if(~verLessThan('matlab','8.4'))
    nmul = 2;
else
    nmul = 1;
end

%Generate Constraint Surface Points
[x1,x2] = meshgrid(linspace(xl(1),xl(2),data.npts*nmul),linspace(yl(1),yl(2),data.npts*nmul));

%For each quadratic constraint, plot
for i = 1:no
    %Get Constraint Variables
    if(iscell(Q))
        nQ = Q{i}; nl = l(:,i); nrl = rl(i); nru = ru(i);
    else
        nQ = Q; nl = l; nrl = rl; nru = ru;
    end
    %Form Constraint Function
    if(data.ndec==1)
        con = @(x) x(1)*nQ*x(1) + nl*x(1);
    else
        con = @(x) x.'*nQ*x + nl.'*x;
    end
    %Plot Each Quad Con as General Nonlinear Constraint
    plotNLCon(con,[],nrl,nru,x1,x2,dkg,data);
end
hold off;

%OLD CODE
% %Plot Quadratic Inequality Constraints (Inefficient.. need to solve the quadratic)
% [x1,x2] = meshgrid(linspace(xl(1),xl(2),npts),linspace(yl(1),yl(2),npts));
% nox = size(x1);
% noy = size(x2);
% obj = zeros(nox(1),noy(2));
% if(iscell(Q))
%     no = length(Q);
% else
%     no = 1;
% end
% for i = 1:no
%     %get vars
%     if(iscell(Q))
%         nQ = Q{i}; nl = l(:,i); nrl = rl(i); nru = ru(i);
%     else
%         nQ = Q; nl = l; nrl = rl; nru = ru;
%     end
%     eq = false; sense = 'L';
%     % check for double constraint or equality
%     if(~isinf(nrl) && ~isinf(nru))
%         if(nrl == nru)
%             nr = nru;
%             eq = true;
%         else
%             nr = [nrl nru];
%             sense = 'GL';
%         end
%     elseif(~isinf(nrl))
%         nr = nrl;
%     else
%         nr = nru;
%     end
%     % create surface
%     for n = 1:nox(1)
%         for m = 1:noy(2)
%             x = [x1(n,m) x2(n,m)]';
%             obj(n,m) = x.'*nQ*x + nl.'*x;
%         end
%     end
%     if(eq)
%         contour(x1,x2,obj,'color',[0 0 1],'levellist',nr);
%     else
%         for j = 1:length(nr) %each row constraint (max 2)
%             c = contour(x1,x2,obj,'color',dkg,'levellist',nr(j));
%             %Plot Hatch
%             if(~isempty(c))
%                 %See if we have multiple contours (non-convex or sd)
%                 len = size(c,2)-1;
%                 if(c(2,1) ~= len)
%                     %Build contour array
%                     cstrt = 2; cend = []; n = 2; ind = 1;
%                     while(ind <= len)
%                         ind = ind + c(2,ind) + 1;
%                         cend(n-1) = ind-1; %#ok<AGROW>
%                         cstrt(n) = ind+1;  %#ok<AGROW>
%                         n = n + 1;
%                     end
%                 else
%                     cstrt = 2;
%                     cend = len;
%                 end
%                 %Plot each contour hatch
%                 for n = 1:length(cend)
%                     %Get contour vectors
%                     vecx = diff(c(1,cstrt(n):cend(n)));
%                     vecy = diff(c(2,cstrt(n):cend(n)));
%                     if(isempty(vecx) || isempty(vecy))
%                         continue;
%                     end
%                     %Rotate hatch lines based on infeasible region
%                     xt = [c(1,cstrt(n))+vecy(1) c(2,cstrt(n))-vecx(1)]'; %check rotated -90
%                     fval = xt.'*nQ*xt + nl.'*xt ;
%                     if(fval <= nr(j) || sense(j) == 'G') %rotate 90
%                         hvecx = -vecy;
%                         hvecy = vecx;
%                     else %rotate -90
%                         hvecx = vecy;
%                         hvecy = -vecx;
%                     end
%                     %Normalize
%                     av = mean(sqrt(hvecx.^2 + hvecy.^2));
%                     dirs = atan2(hvecy,hvecx);
%                     hvecx = av*cos(dirs);
%                     hvecy = av*sin(dirs);
%                     %Shift origin
%                     hvecx = c(1,cstrt(n):cend(n)-1) + hvecx;
%                     hvecy = c(2,cstrt(n):cend(n)-1) + hvecy;
%                     %Plot
%                     line([c(1,cstrt(n):cend(n)-1)' hvecx']',[c(2,cstrt(n):cend(n)-1)' hvecy']','Color','k')
%                 end
%             else
%                 optiwarn('opti:plot','Cannot plot inequality constraint as contour data is empty!');
%             end
%         end
%     end
% end
%
% hold off;

% CONVEX QC CODE
% %Get contour vectors
% vecx = diff(c(1,2:end));
% vecy = diff(c(2,2:end));
% %Rotate hatch lines based on infeasible region
% xt = [c(1,2)+vecy(1) c(2,2)-vecx(1)]'; %check rotated -90
% fval = xt.'*nQ*xt + nl.'*xt ;
% if(fval <= nr) %rotate 90
%     hvecx = -vecy;
%     hvecy = vecx;
% else %rotate -90
%     hvecx = vecy;
%     hvecy = -vecx;
% end
% %Normalize
% av = mean(sqrt(hvecx.^2 + hvecy.^2));
% dirs = atan2(hvecy,hvecx);
% hvecx = av*cos(dirs);
% hvecy = av*sin(dirs);
% %Shift origin
% hvecx = c(1,2:end-1) + hvecx;
% hvecy = c(2,2:end-1) + hvecy;
% %Plot
% line([c(1,2:end-1)' hvecx']',[c(2,2:end-1)' hvecy']','Color','k')


function [min_val, max_val] = arange(X)
%ARANGE Returns the range (min and max) of an entire array.
% Usage:
%   R = arange(X)
%   [min_val, max_val] = arange(X)
%
% See also: alims, amin, amax

min_val = min(X(:));
max_val = max(X(:));

if nargout < 2
    min_val = [min_val, max_val];
end
end


  % 1 x 1  % 1 x 1% plot_info
% plot Free energy and other infomation
a_min = 1e-5;
NX = 2;
NY = 2;
alpha_plot = 0;

FE  = Info.FE;

tend = length(FE);

% Start and End of iteration number for plot
tmode = 2;
t0 = 10;
t3 = 600;
switch	tmode
case	1
	t1 = t0;
	t2 = tend;
case	2
	t1 = t3;
	t2 = tend;
case	3
	t1 = t0;
	t2 = t3;
end

Hmode = 1;
Fmode = 2;
% Fmode = 1 :Remove sudden drop of free energy due to weight pruning

if Fmode == 1
	% Remove sudden drop of free energy due to weight pruning
	Fdif = diff(FE);

	ixz  = find( Fdif < 0);
	ixz  = [ixz, ixz + 1, ixz + 2];
	ixz  = ixz(:);

	ix = setdiff2(1:tend,ixz);
else
	ix = 1:tend;
end

% Free energy
nfig = 1;
subplot(NX,NY,nfig)
%hold on
plot(ix,FE(ix))

title('Free energy')
xlim([t1 t2])

%return

% Model entropy

if	Hmode < 2 &isfield(Info,'H')
	nfig = nfig + 1;
	subplot(NX,NY,nfig)

	if isfield(parm,'T')
		switch	Hmode
		case 0
			H   = -Info.H * parm.T/log(parm.T);
		case 1
			H   = -Info.H /log(parm.T);
		end
	else
		H   = -Info.H ;
	end

	plot(ix,H(ix))
	hold on
	title('Effective parameter number')
	xlim([t1 t2])
end

if	Hmode == 2 & isfield(Info,'M')
	nfig = nfig + 1;
	subplot(NX,NY,nfig)

	plot(ix,Info.M(ix), '-b')
	title('Number of input dimension')
	xlim([t1 t2])
end

if	isfield(Info,'LP')
	nfig = nfig + 1;
	subplot(NX,NY,nfig)
	LP  = Info.LP;
	plot(ix,LP(ix))
	title('Expected log-likelihood')
	xlim([t1 t2])
end

if	isfield(Info,'Err')
	nfig = nfig + 1;
	subplot(NX,NY,nfig)
	Err  = Info.Err;
	plot(ix,Err(ix))
	title('Error')
	xlim([t1 t2])
end

if alpha_plot == 0, return; end;

% Hyper variance parameter
if isfield(Model,'A')
	if isfield(Model,'ix_act')
		N = size(Model.A ,1);
		A = zeros(N,Model.M_all);
		A(:,Model.ix_act) = Model.A;
	else
		A =  Model.A;
	end

    nfig = nfig + 1;
	subplot(NX,NY,nfig)
	plot(A')
	title('Hyper variance parameter')

    nfig = nfig + 1;
	subplot(NX,NY,nfig)
	hist(Model.A, 100)
	title('Hyper variance parameter')

	M=length(Model.A);
	K=sum(Model.A < a_min);
	fprintf('# of active dim = %d, # of near zero = %d\n', M, K)
end

if alpha_plot == 1, return; end;

if isfield(Info, 'A')
    AA  = Info.A ;
    nfig = nfig + 1;
	subplot(NX,NY,nfig)
	plot(AA')
	title('Hyper variance histry')
end


function [Hist] = makeHistogram(Bin,XSize,YSize,NrX,NrY,NrBins)
%  This function classifies the greylevels present in the array image into
%  a greylevel histogram. The pLookupTable specifies the relationship
%  between the greyvalue of the pixel (typically between 0 and 4095) and
%  the corresponding bin in the histogram (usually containing only 128 bins).

Hist=zeros(NrX,NrY,NrBins);

for i=1:NrX
    for j=1:NrY
        bin=Bin(1+(i-1)*XSize:i*XSize,1+(j-1)*YSize:j*YSize);
        for i1=1:XSize
            for j1=1:YSize
                Hist(i,j,bin(i1,j1)) = Hist(i,j,bin(i1,j1)) + 1;
            end
        end
    end
end


function pp=lpcra2pp(ra)
%LPCAR2PP LPC: Convert ar filter autocorrelation to power spectrum polynomial in cos(w) PP=(RA)
% pp is a polynomial such that |polyval(ra,e^jw)| = polyval(pp,cos(w))

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lpcra2pp.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent tp;
[nf,p1]=size(ra);
% we check here if p is the same as before and reuse the tp matrix
if size(tp,1)~=p1
   p=p1-1;
   % chebyshev polynomials up to order p
   tp=zeros(p1,p1);
   tp(1,p1)=2;
   tp(2,p)=2;
   for i=3:p1
      tp(i,p+2-i:p)=2*tp(i-1,p+3-i:p1)-tp(i-2,p+2-i:p);
      tp(i,p1)=-tp(i-2,p1);
   end
   tp(1,p1)=1;
end
pp=ra*tp;


function J = calcJ(camera, imuState_k, camStates_k)
% Jacobian of feature observations w.r.t. feature locations

    C_CI = quatToRotMat(camera.q_CI);
    C_IG = quatToRotMat(imuState_k.q_IG);

    J = zeros(6, 12 + 6*size(camStates_k,2));
    J(1:3,1:3) = C_CI;
    J(4:6,1:3) = crossMat(C_IG' * camera.p_C_I);
    J(4:6,10:12) = eye(3);

end


function dimacs = computedimacs(b,c,A,xin,y,s,K);
% COMPUTEDIMACS
%
% min <C,X> s.t     AX = b, X > 0
% max b'y   s.t S-C+A'y =0, S > 0

% If no primal exist, fake till later
if isempty(xin)
    x = c*0;
else
    x = xin;
end

if isempty(s)
    s = c-A'*y;
end

xres = inf;
sres = inf;

% Not officially defined in DIMACS
if K.f>0
    sres = -min(norm(s(1:K.f),inf));
end

% Errors in linear cone
if K.l>0
    xres = min(x(1+K.f:K.f+K.l));
    sres = min(s(1+K.f:K.f+K.l));
end

% Errors in quadratic cone
if K.q(1)>0
    top = K.f+K.l;
    for i = 1:length(K.q)
        X = x(1+top:top+K.q(i));
        S = s(1+top:top+K.q(i));
        xres = min(xres,X(1)-norm(X(2:end)));
        sres = min(sres,S(1)-norm(S(2:end)));
        top = top + K.q(i);
    end
end

% Errors in semidefinite cone
if K.s(1)>0
    top = K.f+K.l+K.q+K.r;
    for i = 1:length(K.s)
        X = reshape(x(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        S = reshape(s(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        xres = min(xres,min(eig(full(X))));
        sres = min(sres,min(eig(full(S))));
        top = top + K.s(i)^2;
    end
end

err1 = norm(b-A*x)/(1 + norm(b,inf));
err2 = max(0,-xres)/(1 + norm(b,inf));
err3 = conenorm(s-(c-A'*y),K)/(1+norm(c,inf));
%err3 = norm(s-(c-A'*y))/(1+norm(c,inf)); % Used by some solvers
err4 = max(0,-sres)/(1+max(abs(c)));
err5 = (c'*x-b'*y)/(1+abs(c'*x)+abs(b'*y));
err6 = x'*(c-A'*y)/(1+abs(c'*x)+abs(b'*y));

% No primal was computed
if isempty(xin)
    err1 = nan;
    err2 = nan;
    err5 = nan;
    err6 = nan;
end
dimacs = [err1 err2 err3 err4 err5 err6];

function t = conenorm(s,K)

% Implementation of the norm described on
% http://plato.asu.edu/dimacs/node3.html

t = 0;

if K.f + K.l>0
    t = t + norm(s(1:K.f+K.l));
end

top = 1+K.f+K.l;
if K.q(1)>0
    for i = 1:length(K.q)
        t = t + norm(s(top:top+K.q(i)-1));
        top  = top + K.q(i);
    end
end

if K.s(1)>0
    for i = 1:length(K.s)
        S = reshape(s(top:top+K.s(i)^2-1),K.s(i),K.s(i));
        t = t + norm(S,'fro');
        top  = top + K.s(i)^2;
    end
end


function bc = specific_bc(xbd,ybd)
%solution3_bc   Reference problem 1.3  boundary condition
%   bc = specific_bc(xbd,ybd);
%   input
%          xbd          x boundary coordinate vector
%          ybd          y boundary coordinate vector
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
bc=2*(1+ybd)./((ybd+1).*(ybd+1) + (xbd+3).*(xbd+3));
return


function [nmps2] = mGal2nmps2(mGal)
% Convert acceleration from milligalileos to nanometers per second squared
% Chad A. Greene 2012
nmps2 = mGal*1e+4;
end


function [axons,packing] = func_axonpack_main(numelobj, d_mean, d_var, gap, iter_max)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       AxonPackin : Simulate arrangement of white matter axons
%                     author : Tom Mingasson
%             https://github.com/neuropoly/axonpacking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       CHANGE INPUTS BELOW
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MAIN INPUTS
N = numelobj;            % number of axons i.e disks to pack
%d_mean = 3;         % theoretical mean of axon diameters in um
%d_var  = 1;         % theoretical variance of axon diameters in um
Delta  = gap;         % gap between the edge of axons in um
%iter_max = 30000;    % number of iteration i.e migrations to perform. Example: iter_max = 30000 ok if N = 1000

% SECONDARY INPUTS
threshold_high = 10;     % no diameter above 'threshold_high'
threshold_low = 0.2;     % no diameter under 'threshold_low'
iter_fvf = iter_max/10;  % to study the packing convergence the disk density i.e Fiber Volume Fraction (FVF) can be computed and displayed every 'iter_fvf' iterations

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                     AxonPacking Process
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=1:length(d_mean)

    % axons features
    axons.N{k}      = N;
    axons.d_mean{k} = d_mean(k);
    axons.d_var{k}  = d_var(k);
    axons.Delta{k}           = Delta(k);
    axons.threshold_high{k} = threshold_high;
    axons.threshold_low{k}  = threshold_low;

    % axon diameters sampling (under a gamma law or lognormal and initialization of positions 'x0' in a square area of length 'side')
    [d, x0, side] = axons_setup(axons,'gamma', k);
    axons.d{k} = d;
    axons.g_ratio{k} = compute_gratio(d);

    % packing process of the axons
    [final_positions, final_overlap, fvf_historic] = process_packing(x0, d, Delta(k), side, iter_max, iter_fvf);

    % store packing results
    % main results
    packing.initial_positions{k}    = reshape(x0,2,length(x0)/2);
    packing.final_positions{k}      = final_positions;
    % secondary results
    packing.final_overlap{k}        = final_overlap;
    packing.FVF_historic{k}         = fvf_historic;
    packing.iter_max{k}             = iter_max;

    % Statistics from the packing
    [FVF, FR, MVF, AVF] = compute_statistics(axons.d{k}, axons.Delta{k}, packing.final_positions{k}, side, axons.g_ratio{k});

    % store stats results
    stats.FVF{k}        = FVF;
    stats.FR{k}         = FR;
    stats.MVF{k}        = MVF;
    stats.AVF{k}        = AVF;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                 Save results in a folder named 'results'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% save_var = num2str(d_var);  save_var(save_var == ' ') = '';
% save_mean = num2str(d_mean); save_mean(save_mean == ' ') = '';
% save_Delta  = num2str(Delta);  save_Delta(save_Delta == ' ') = '';
% save_iter  = num2str(iter_max);
% saveName  = ['Axons', num2str(N), '_Mean', save_mean, '_Var', save_var, '_Delta', save_Delta, '_Iter',save_iter];
%
% mkdir('results')
% cd([pwd,filesep, 'results'])
%
% % save outputs
% save('axons.mat', '-struct', 'axons');
% save('packing.mat', '-struct', 'packing');
% save('stats.mat', '-struct', 'stats');
%
% % save final substrate
% saveas(figure(1000),[saveName,'.png']);
%


function [J,alpha,beta,T] = variationalDynLoreta(Ut,Y,s2,iLV,L,alpha,beta,options)

%[J,alpha,beta,T] = variationalDynLoreta(Ut,Y,s2,iLV,L,alpha,beta,options)
%
% Computes the posterior distribution of the parameters J given some data V.
% The program solves levels of inference: 1) optimization of parameters J, and
% 2) optimization of hyperparameters alpha and beta. See Trujillo-Barreto
% et. al. (2004) for details.
%
% Ut,s2, and iLV are defined as follows:
%     Y: Nsensors x time points data matrix
%     K: N x P predictor matrix
%     L: sparse P x P square root of the precision matrix
%     [U,s,V] = svd( K*inv(L) )
%     iLV = inv(L)*V
%     s2  = s.^2
%
% alpha, beta: hyperparameters
% J: estimated parapeters
%
%                     P(V|J,alpha)*P(J|beta)
% P(J|V,alpha,beta) = ----------------------
%                        P(V|alpha,beta)
%
% Author: Alejandro Ojeda, SCCN/INC/UCSD, Jan-2013
%
% References:
%   Trujillo-Barreto, N., Aubert-Vazquez, E., Valdes-Sosa, P.A., 2004.
%     Bayesian model averaging in EEG/MEG imaging. NeuroImage 21, 1300???1319

if nargin < 5, error('Not enough input arguments.');end
if nargin < 8
    options.maxTol = 1e-3;
    options.maxIter = 100;
    options.verbose = true;
    options.gridSize = 100;
end

s = s2.^(0.5);
n = length(s);
p = size(L,1);
ntp = size(Y,2);

% Initialize hyperparameters
if nargin < 8
    UtY = Ut*Y(:,1);
    tol = max([n p])*eps(max(s));
    lambda2 = logspace(log10(tol),log10(max(s)),options.gridSize);
    gcv = zeros(options.gridSize,1);
    parfor k=1:options.gridSize
        d = lambda2(k)./(s2+lambda2(k));
        f = diag(d)*UtY;
        gcv(k) = dot(f,f,1)/sum(d)^2;
    end
    loc = getMinima(gcv);
    if isempty(loc), loc = 1;end
    loc = loc(end);
    lambda2 = lambda2(loc);

    alpha = 0.001*(Y(:)'*Y(:))/n;
    beta = alpha*lambda2;
end
err = inf;

for it=1:options.maxIter
    if err < options.maxTol, break;end

    % computing statistics
    H = Ut'*diag(alpha.*s2./(alpha.*s2+beta))*Ut;
    SSE = mean( (Y - H*Y).^2 ,2);
    sigma2 = mean(SSE);
    kSjk = diag(H);

    q = diag(alpha.*s./(alpha.*s2+beta))*Ut;%*Y;
    Sj = mean((iLV*q).^2,2);
    aic = -2*log(sigma2) + 2*p;
    alpha_old = alpha;
    beta_old = beta;

    % updating hyperparameters
    alpha = updateAlpha(SSE,kSjk,ntp);
    beta  = updateBeta(Sj,p);

    err = 0.5*abs(sum(alpha_old-alpha)) + 0.5*abs(beta_old-beta);
    if options.verbose
        disp([num2str(it) ' => alpha: ' num2str(alpha_old) '  beta: ' num2str(beta_old) ' sse: ' num2str(mean(SSE)) ' hyrp. error: ' num2str(err) ' aic: ' num2str(aic)]);
    end
end
if it == options.maxIter, warning('Maximum iteration reached. Failed to converge.');end

% parameters's estimation
T = iLV*diag(alpha.*s./(alpha.*s2+beta))*Ut;
J = T*Y;

% standardized Loreta
E = sum(Y-H*Y,2);
sigma = E'*E/(n-trace(H));
dT = 1./sqrt(dot(T,T,2));
S = 1./sigma*dT;
S = S./std(eps+S);
T = bsxfun(@times,T,S);%sqrt(p)*
J = bsxfun(@times,J,S);%sqrt(p)*
end

%---
function indmin = getMinima(x)
fminor = diff(x)>=0;
fminor = ~fminor(1:end-1, :) & fminor(2:end, :);
fminor = [0; fminor; 0];
indmin = find(fminor);
end

function alpha = updateAlpha(SSE,kSjk,ntp,bp,cp)
if nargin < 4, bp = 3;end
if nargin < 5, cp = 3;end
b = 1./( (1/bp) + 0.5*SSE + 0.5*kSjk );
c = cp + ntp/2;
alpha = median(1./(b*c));
end

function beta = updateBeta(r,p,bp,cp)
if nargin < 3, bp = 3;end
if nargin < 4, cp = 3;end
b = 1./( 1/bp + sum(r) );
c = cp+p/2;
beta = 1/(b*c);
end


function Offspring = Operator(Problem,Population)
% Differential evolution in FROFI

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Parameter setting
    PopDec = Population.decs;
    [N,D]  = size(PopDec);
    CR = [0.1 0.2 1]';
    CR = repmat(CR(randi(end,N,1)),1,D);
    F  = [0.6 0.8 1]';
    F  = repmat(F(randi(end,N,1)),1,D);

    %% Parents
    [~,P] = sort(rand(N),2);
    P1    = PopDec(P(:,1),:);
    P2    = PopDec(P(:,2),:);
    P3    = PopDec(P(:,3),:);
    [~,B] = min(Population.objs);
    PB    = repmat(PopDec(B,:),N,1);

    %% Offspring generation
    Rand   = rand(N,D);
    k1     = repmat(rand(N,1)<0.5,1,D);
    k2     = ~k1 & rand(N,D)<CR;
    OffDec = PopDec;
    OffDec(k1) = PopDec(k1) + Rand(k1).*(P1(k1)-PopDec(k1)) + F(k1).*(P2(k1)-P3(k1));
    OffDec(k2) = P1(k2) + Rand(k2).*(PB(k2)-P1(k2)) + F(k2).*(P2(k2)-P3(k2));
    Offspring  = Problem.Evaluation(OffDec);
end


% DEMSPGP1D1 Do a simple 1-D regression after Snelson & Ghahramani's example.

% GP

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

dataSetName = 'spgp1d';
experimentNo = 1;

% load data
[X, y] = mapLoadData(dataSetName);

% Set up model
options = gpOptions('dtc');
options.numActive = 9;

% use the deterministic training conditional.
q = size(X, 2);
d = size(y, 2);

model = gpCreate(q, d, X, y, options);
model.X_u = randn(9, 1)*0.25 - 0.75;
params = gpExtractParam(model);
model = gpExpandParam(model, params);

% Optimise the model.
iters = 1000;
display = 1;

model = gpOptimise(model, display, iters);

% Save the results.
capName = dataSetName;
capName(1) = upper(capName(1));
save(['dem' capName num2str(experimentNo) '.mat'], 'model');

demSpgp1dPlot


classdef TestGeneralTwoRankSequentialLaminate < handle

    properties (Access = private)
      fractionVolume
      directions
      lamParams
      stiffTensor
      weakTensor
      Rank2Ch
      SeqLamCh
    end

    methods (Access = public)

        function obj = TestGeneralTwoRankSequentialLaminate()
            obj.init()
            obj.computeTwoRankSequentialLaminate()
            obj.computeGeneralTwoRankSequentialLaminate()
        end

        function hasPassed = hasPassed(obj)
            RankTwoCh  = obj.Rank2Ch;
            SqCh = obj.SeqLamCh.getValue();
            hasPassed = norm(RankTwoCh - SqCh)/norm(SqCh) > 1e-6;
        end

    end

    methods (Access = private)

        function init(obj)
            obj.fractionVolume = 0.8000;
            obj.lamParams = [1 0];
            obj.loadLaminateDirections()
            obj.createStiffAndWeakTensors()
        end

        function loadLaminateDirections(obj)
            d1 = [rand(1) rand(1)  0];
            d2 = [1     3     0];
            obj.directions{1} = obj.createDirection(d1);
            obj.directions{2} = obj.createDirection(d2);
        end

        function createStiffAndWeakTensors(obj)
            epsilon = 1.0000e-03;
            E1  = 1;
            nu1 = 1/3;
            E0  = epsilon*E1;
            nu0 = 1/3;
            obj.stiffTensor = IsotropicConstitutiveTensor(E1,nu1);
            obj.weakTensor  = IsotropicConstitutiveTensor(E0,nu0);
        end

        function computeTwoRankSequentialLaminate(obj)
            mi    = obj.lamParams;
            dir   = obj.directions;
            theta = obj.fractionVolume;
            C1    = obj.stiffTensor.getValue();
            C0    = obj.weakTensor.getValue();
            mu    = obj.stiffTensor.getMu();
            lambda2D = obj.stiffTensor.getLambda2D();
            homogenizer = RankTwoLaminateHomogenizer(C1,C0,dir,mi,theta,lambda2D,mu);
            obj.Rank2Ch = homogenizer.getTensor;
        end

        function computeGeneralTwoRankSequentialLaminate(obj)
            C0       = obj.weakTensor;
            C1       = obj.stiffTensor;
            mi       = obj.lamParams;
            dir      = obj.directions;
            Theta    = obj.fractionVolume;
            SeqHomog      = VoigtPlaneStressHomogHomogenizer(C0,C1,dir,mi,Theta);
            obj.SeqLamCh  = SeqHomog.getPlaneStressHomogenizedTensor();
        end

    end

    methods (Access = private, Static)

        function dir = createDirection(d)
            dir = Vector3D;
            dir.setValue(d);
            dir.normalize();
        end

    end

end


function ll = fgplvmSequenceLogLikelihood(model, X, Y, varargin)

% FGPLVMSEQUENCELOGLIKELIHOOD Log-likelihood of a sequence for the GP-LVM.
% FORMAT
% DESC returns the log probability of a given latent sequence and an
% associated observed data sequence under the posterior prediction
% induced by the training data for a given GP-LVM model.
% ARG model : the model for which the sequence prediction will be
% made.
% ARG X : the latent sequence for which the posterior distribution
% will be evaluated.
% ARG Y : the observed data sequence for which the posterior
% distribution will be evaluated.
% ARG P1, P2, P3 ... : optional additional arguments to be passed
% to the dynamics' model sequence log likelihood.
% RETURN ll : the sequence log likelihood.
%
% SEEALSO : fgplvmCreate, fgplvmOptimiseSequence, fgplvmSequenceObjective
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006
%
% MODIFICATIONS : Carl Henrik Ek, 2007

% FGPLVM
if(nargin<3)
  error('This function requires at least two arguments.');
end

logTwoPi = log(2*pi);
if model.isMissingData
  [mu, covarSigma]  = gpPosteriorMeanCovar(model, X);
  for i = 1:model.d
    missing = true;
    if ~any(isnan(Y(:, i)))
      U = jitChol(covarSigma{i});
      missing = false;
    end
    Ydiff = (Y(:, i)-mu(:, i));
    ll = 0;
    if missing
      ind = find(~isnan(Ydiff));
      if length(ind) ~= 0
        U = jitChol(covarSigma{i}(ind, ind));
      end
    else
      ind = [1:size(Ydiff, 1)]';
    end
    if length(ind) ~= 0
      UinvYdiff = U'\Ydiff(ind);
      logDet = logdet([], U);
      ll = ll + logDet + (UinvYdiff'*UinvYdiff);
      ll = ll + logTwoPi*length(ind);
    end
  end
else
  [mu, covarSigma, factors] = gpPosteriorMeanCovar(model, X);
  missing = true;
  if ~any(isnan(Y))
    U = jitChol(covarSigma);
    missing = false;
  end
  Ydiff = (Y-mu);
  ll =0;
  for i = 1:model.d
    if missing
      ind = find(~isnan(Ydiff(:, i)));
      if length(ind) ~= 0
        U = jitChol(covarSigma(ind, ind));
      end
    else
      ind = [1:size(Ydiff, 1)]';
    end
    if length(ind) ~= 0
      UinvYdiff = U'\Ydiff(ind, i);
      logDet = logdet([], U);
      ll = ll + logDet + log(factors(i)) + (UinvYdiff'*UinvYdiff)/factors(i);
      ll = ll + logTwoPi*length(ind);
    end
  end
end
ll = -0.5*ll;

if isfield(model, 'dynamics') & ~isempty(model.dynamics)
  % A dynamics model is being used.
  feval = str2func([model.dynamics.type 'SequenceLogLikelihood']);
  if isfield(model, 'dynamicsBalancing') & ~isempty(model.dynamicsBalancing)
    ll = ll + model.dynamicsBalancing*feval(model.dynamics, X, varargin{:});
  else
    ll = ll + feval(model.dynamics, X, varargin{:});
  end
elseif isfield(model, 'prior') &  ~isempty(model.prior)
  for i = 1:size(X, 1)
    ll = ll + priorLogProb(model.prior, X(i, :));
  end
end


% absolute difference in orientation between a fly and the closest fly
% according to type
function [data,units] = compute_absthetadiff(trx,n,type)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);

for i1 = 1:nflies,
  fly1 = flies(i1);

  % fly closest to fly1 according to type
  closestfly = trx(fly1).(['closestfly_',type]);

  % orientation of fly1
  theta_mm1 = trx(fly1).theta_mm;

  % loop over all flies
  for i2 = 1:nflies,

    fly2 = flies(i2);
    if i1 == i2, continue; end

    % frames where this fly is closest
    idx = find(closestfly == fly2);
    if isempty(idx), continue; end

    % orientation of fly2
    off = trx(fly1).firstframe - trx(fly2).firstframe;
    theta_mm2 = trx(fly2).theta_mm(off+idx);

    % absolute difference in orientation
    data{i1}(idx) = abs(modrange(theta_mm2 - theta_mm1(idx),-pi,pi));

  end
end

units = parseunits('rad');


function f = sparsnessCol2(Hkt_T, varargin)
k=varargin{1};
t=varargin{2};
Hkt=exp(reshape(Hkt_T, k, t));
sumH_t = sum(Hkt,2);
% figure
% bar(sumH_t)
sumH_t_sq = sum(sumH_t.^2);
sumH = sum(sumH_t);
nh = length(sumH_t);
f = (sqrt(nh)-sumH/(sqrt(sumH_t_sq)))/(sqrt(nh)-1);


function net = cnn_cifar_init(varargin)
opts.networkType = 'simplenn' ;
opts = vl_argparse(opts, varargin) ;

lr = [.1 2] ;

% Define network CIFAR10-quick
net.layers = {} ;

% Block 1
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{0.01*randn(5,5,3,32, 'single'), zeros(1, 32, 'single')}}, ...
                           'learningRate', lr, ...
                           'stride', 1, ...
                           'pad', 2) ;
net.layers{end+1} = struct('type', 'pool', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', [0 1 0 1]) ;
net.layers{end+1} = struct('type', 'relu') ;

% Block 2
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{0.05*randn(5,5,32,32, 'single'), zeros(1,32,'single')}}, ...
                           'learningRate', lr, ...
                           'stride', 1, ...
                           'pad', 2) ;
net.layers{end+1} = struct('type', 'relu') ;
net.layers{end+1} = struct('type', 'pool', ...
                           'method', 'avg', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', [0 1 0 1]) ; % Emulate caffe

% Block 3
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{0.05*randn(5,5,32,64, 'single'), zeros(1,64,'single')}}, ...
                           'learningRate', lr, ...
                           'stride', 1, ...
                           'pad', 2) ;
net.layers{end+1} = struct('type', 'relu') ;
net.layers{end+1} = struct('type', 'pool', ...
                           'method', 'avg', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', [0 1 0 1]) ; % Emulate caffe

% Block 4
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{0.05*randn(4,4,64,64, 'single'), zeros(1,64,'single')}}, ...
                           'learningRate', lr, ...
                           'stride', 1, ...
                           'pad', 0) ;
net.layers{end+1} = struct('type', 'relu') ;

% Block 5
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{0.05*randn(1,1,64,10, 'single'), zeros(1,10,'single')}}, ...
                           'learningRate', .1*lr, ...
                           'stride', 1, ...
                           'pad', 0) ;

% Loss layer
net.layers{end+1} = struct('type', 'softmaxloss') ;

% Meta parameters
net.meta.inputSize = [32 32 3] ;
net.meta.trainOpts.learningRate = [0.05*ones(1,30) 0.005*ones(1,10) 0.0005*ones(1,5)] ;
net.meta.trainOpts.weightDecay = 0.0001 ;
net.meta.trainOpts.batchSize = 100 ;
net.meta.trainOpts.numEpochs = numel(net.meta.trainOpts.learningRate) ;

% Fill in default values
net = vl_simplenn_tidy(net) ;

% Switch to DagNN if requested
switch lower(opts.networkType)
  case 'simplenn'
    % done
  case 'dagnn'
    net = dagnn.DagNN.fromSimpleNN(net, 'canonicalNames', true) ;
    net.addLayer('error', dagnn.Loss('loss', 'classerror'), ...
             {'prediction','label'}, 'error') ;
  otherwise
    assert(false) ;
end


function [gew,pve,H] = spm_csd2gew(csd,Hz,u)
% Convert cross sspectral density to Geweke Granger causality
% FORMAT [gew,pve,H] = spm_csd2gew(csd,Hz)
%
% ccf  (N,m,m)   - cross covariance functions
% Hz   (n x 1)   - vector of frequencies (Hz)
% u    (1)       - regularizer (default: 1);
%
% gwe  (N,m,m)   - Geweke's frequency domain Granger causality
% pve  (N,m,m)   - proportion of variance explained
% H    (N,m,m)   - transfer function matrix
%
% This routine uses the Wilson-Burg algorithm to perform spectral matrix
% factorisation. The minimum phase factor is then used to form the noise
% covariance (covariance of the innovations) and implicitly derive the
% transfer functions (and spectral Granger causality).
%
% See also:
%  spm_ccf2csd.m, spm_ccf2mar, spm_csd2ccf.m, spm_csd2mar.m, spm_mar2csd.m,
%  spm_csd2coh.m, spm_dcm_mtf.m, spm_Q.m, spm_mar.m and spm_mar_spectral.m
%
%__________________________________________________________________________
% Copyright (C) 2014 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_csd2gew.m 5908 2014-03-05 20:31:57Z karl $

% preliminaries
%--------------------------------------------------------------------------
if nargin < 3
    try
        [gew,pve,H] = spm_csd2gew(csd,Hz,2);
        return
    catch
        [gew,pve,H] = spm_csd2gew(csd,Hz,16);
        return
    end
end

% pad spectrum if necessary
%--------------------------------------------------------------------------
iw    = 1 + round(Hz/(Hz(2) - Hz(1)));
n     = size(csd,2);
nw    = 257;
is    = ceil(nw/2):nw;

% Wilson-Burg algorithm
%==========================================================================

% initialise transfer function
%--------------------------------------------------------------------------
H     = zeros(nw,n,n);
P     = zeros(nw,n,n);
e     = norm(squeeze(max(csd,[],1)))/128;
E     = eye(n,n)*e;

P(iw,:,:) = csd;
for i = 1:n
    P(:,i,i) = P(:,i,i) + e;
    H(:,i,i) = sqrt(P(:,i,i));
end

% iterate until convergence: solve for H*H' = P
%--------------------------------------------------------------------------
for t = 1:128

    % compute left-hand side (deconvolution)
    %----------------------------------------------------------------------
    for w = 1:nw
        S        = squeeze(H(w,:,:)) + E;
        A(w,:,:) = (eye(n,n) + S*S'\squeeze(P(w,:,:)));
    end

    % retain causal signal and half zero lag
    %----------------------------------------------------------------------
    S           = ifft(A);
    S(is,:,:)   = 0;
    S(1,:,:)    = S(1,:,:)/2;
    A           = fft(S);

    % recover next update (convolution)
    %----------------------------------------------------------------------
    nrm   = zeros(nw,1);
    for w = 1:nw
        U        = squeeze(A(w,:,:));
        H(w,:,:) = squeeze(H(w,:,:))*U^(1/u);
        nrm(w)   = nrm(w) + norm(eye(n,n) - U,'inf');
    end

    % break if convergence
    %----------------------------------------------------------------------
    nrm = mean(nrm);
    if nrm < 1e-6, break, end
    if nrm > 8,   return, end

end

% transfer function and noise covariance
%==========================================================================

% get noise covariance
%--------------------------------------------------------------------------
S     = ifft(H);
R     = squeeze(S(1,:,:));
C     = real(R*R');
c     = sqrtm(C);

% recover transfer function
%--------------------------------------------------------------------------
for w = 1:nw
    H(w,:,:) = squeeze(H(w,:,:))/c;
    P(w,:,:) = squeeze(H(w,:,:))*C*squeeze(H(w,:,:))';
end

% Geweke Granger Causality in the Frequency domain
%--------------------------------------------------------------------------
pve   = zeros(nw,n,n);
gew   = zeros(nw,n,n);
for j = 1:n
    for k = 1:n
        rkj        = C(j,j) - (C(j,k)^2)/C(k,k);
        sk         = abs(P(:,k,k));
        hkj        = abs(H(:,k,j)).^2;
        pve(:,k,j) = rkj*hkj./sk;
        gew(:,k,j) = -log(1 - pve(:,k,j));
    end
end

% return  specified frequencies
%--------------------------------------------------------------------------
gew = gew(iw,:,:);
pve = pve(iw,:,:);
H   = H(iw,:,:);


function testGaborFilter
%% by dzhg: zhgdai@126.com
%% This code is to search the direction of the vessel for each point of
%% the Image. I apply Gabor Filter to this work and the corresponding
%% output Gabor filter will be choosed to be the direction of the vessel.
%% if you have any question about this code, please contact with me by
%% zhgdai@126.com without hesitate.
%% This edition is to apply adaptative size of gabor filter to detect the
%% direction of the vessel.

I = imread('00005.bmp');

if isgray(I) ==0
    I = rgb2gray(I);
end

[ImageWidth,ImageHeight] = size(I);
sizeinterval = 10;
Begin_s = 10;
End_s = 40;
angleinterval = 18;

%% papers parameters
s = Begin_s:sizeinterval:End_s;
theta = 0:pi/angleinterval:(angleinterval-1)/angleinterval*pi;

f = 1./s;
Sx = s/pi;
Sy = s/pi;
LengthTheta = length(theta);
LengthS = length(s);

GaborFilter = zeros(ImageWidth,ImageHeight,(LengthTheta-1)*LengthS);
Index = 1;
for j = 1:LengthS
    for i = 1:LengthTheta-1
    [G,gabout] = gabordzhg(I,Sx(j),Sy(j),f(j),theta(i));
    GaborModel{Index} = G;
    GaborFilter(:,:,Index) = gabout;
    Index = Index + 1;
    end
end

imshow(I);
hold on;
[a,b] = ginput;
% a = 275;
% b = 245;

GaborFilterValue = GaborFilter(floor(b),floor(a),1:end);
savevar = [];
for i = 1:LengthS
    tempvar = var(GaborFilterValue((i-1)*(angleinterval-1)+1:i*(angleinterval-1)));
    savevar = [savevar;tempvar];
end
[maxvar,maxindex] = max(savevar);
[minvalue,minindex] = min(GaborFilterValue((maxindex-1)*(angleinterval-1)+1:maxindex*(angleinterval-1)));
imshow(GaborModel{(maxindex-1)*(angleinterval-1)+minindex},[]);


function [ ersp ] = visual_ERSP( data, varargin )
% Description:
%   ERSP (Event-related spectral pergurbation) measures the average dynamic changes
%   in amplitude of the broad band EEG frequency spectrum as a function of time
%   relative to an experimental event
%
% Example code:
%  [dat] = proc_ERSP(data , <OPT>)
%
% Input:
%   data: Data structrue (ex) Epoched data structure
%
% Options:
%   <OPT> :
%      .Channel - Selecting the interested channel in Time-Frequency domain
%                 (e.g. {'Channel', {'C4'}})
%      .Interval - Selecting the interested time intervals
%                 (e.g. {'Interval' , '[-2000 3000]'})
%
% Return:
%    data:  Epoched data structure
%
% See also:
%    opt_cellToStruct , visual_timef
%
% Reference:
%         1. C. Brunner, A. Delorme, and S. Makeig, "EEGLAB?An Open Source
%          Matlab Toolbox for Electrophysiological Research," Biomedical Engineering/Biomedizinische Technik, 2013, pp.1-2.
%         2. EEGLAB tutorial (http://sccn.ucsd.edu/eeglab/)
%          Author: Sigurd Enghoff, Arnaud Delorme & Scott Makeig
%          CNL / Salk Institute 1998- | SCCN/INC, UCSD 2002-
%
%         We used EEGLAB open source toolbox code related in ERSP (timef.m)
%
%
% Ji Hoon, Jeong
% jh_jeong@korea.ac.kr
%
%%
dat = data;
opt = opt_cellToStruct(varargin{:});
chinx = find(strcmp(dat.chan,opt.Channel)==1);
eeg = [];

if ndims(dat.x) == 2
    if ~isfield(opt,'Interval')
        warning('OpenBMI: please input the data intervals');return;
    end
    dat = prep_segmentation(dat, {'interval',opt.Interval});
    dat.x = dat.x(:,:,chinx);
    eeg = reshape(dat.x , [size(dat.x,1)*size(dat.x,2) 1])';
elseif ndims(dat.x) == 3
    dat.x = dat.x(:,:,chinx);
    eeg = reshape(dat.x , [size(dat.x,1)*size(dat.x,2) 1])';
end

figure;
ersp = visual_timef(eeg, size(dat.x,1), [dat.ival(1) dat.ival(end)], dat.fs, [3 0.5], 'plotersp','on','plotitc','off');


function aIdx = getPaddingIndices22(aSize,padSize,method,direction)
%getPaddingIndices is used by padarray and blockproc.
%   Computes padding indices of input image.  This is function is used to
%   handle padding of in-memory images (via padarray) as well as
%   arbitrarily large images (via blockproc).
%
%   aSize : result of size(I) where I is the image to be padded
%   padSize : padding amount in each dimension.
%             numel(padSize) can be greater than numel(aSize)
%   method : X or a 'string' padding method
%   direction : pre, post, or both.
%
%   See the help for padarray for additional information.

% Copyright 2010 The MathWorks, Inc.

% make sure we have enough image dims for the requested padding
if numel(padSize) > numel(aSize)
    singleton_dims = numel(padSize) - numel(aSize);
    aSize = [aSize ones(1,singleton_dims)];
end

switch method
    case 'circular'
        aIdx = CircularPad(aSize, padSize, direction);
    case 'symmetric'
        aIdx = SymmetricPad(aSize, padSize, direction);
    case 'replicate'
        aIdx = ReplicatePad(aSize, padSize, direction);
end

%%%
%%% CircularPad
%%%
function idx = CircularPad(aSize, padSize, direction)

numDims = numel(padSize);

% Form index vectors to subsasgn input array into output array.
% Also compute the size of the output array.
idx   = cell(1,numDims);
for k = 1:numDims
    M = aSize(k);
    dimNums = uint32(1:M);
    p = padSize(k);

    switch direction
        case 'pre'
            idx{k}   = dimNums(mod(-p:M-1, M) + 1);

        case 'post'
            idx{k}   = dimNums(mod(0:M+p-1, M) + 1);

        case 'both'
            idx{k}   = dimNums(mod(-p:M+p-1, M) + 1);

    end
end

%%%
%%% SymmetricPad
%%%
function idx = SymmetricPad(aSize, padSize, direction)

numDims = numel(padSize);

% Form index vectors to subsasgn input array into output array.
% Also compute the size of the output array.
idx   = cell(1,numDims);
for k = 1:numDims
    M = aSize(k);
    dimNums = uint32([1:M M:-1:1]);
    p = padSize(k);

    switch direction
        case 'pre'
            idx{k}   = dimNums(mod(-p:M-1, 2*M) + 1);

        case 'post'
            idx{k}   = dimNums(mod(0:M+p-1, 2*M) + 1);

        case 'both'
            idx{k}   = dimNums(mod(-p:M+p-1, 2*M) + 1);
    end
end

%%%
%%% ReplicatePad
%%%
function idx = ReplicatePad(aSize, padSize, direction)

numDims = numel(padSize);

% Form index vectors to subsasgn input array into output array.
% Also compute the size of the output array.
idx   = cell(1,numDims);
for k = 1:numDims
    M = aSize(k);
    p = padSize(k);
    onesVector = uint32(ones(1,p));

    switch direction
        case 'pre'
            idx{k}   = [onesVector 1:M];

        case 'post'
            idx{k}   = [1:M M*onesVector];

        case 'both'
            idx{k}   = [onesVector 1:M M*onesVector];
    end
end


function TR=IcosahedronMesh
% Name speaks for itself.

% Get the vertex coordinates
t=(1+sqrt(5))/2; % golden ratio
x=[0 1 t];
s=[1 1 1; 1 1 -1; 1 -1 -1; 1 -1 1];
x=repmat(x,[4 1]).*s;
x=[x;circshift(x,[0 -1]);circshift(x,[0 -2])];
x_L2=sqrt(sum(x.^2,2));
x=bsxfun(@rdivide,x,x_L2);

% Triangulate the points
Tri = fliplr(convhulln(x));
TR=TriRep(Tri,x);


function dat= proc_movingAverage(dat, ms, varargin)
%PROC_MOVINGAVERAGE - Moving average (low-pass) filter
%
%Usage:
% DAT= proc_movingAverage(DAT, MSEC, <METHOD='causal'>)
%
%Input:
% DAT    - data structure of continuous or epoched data
% MSEC   - length of interval in which the moving average is
%          to be calculated, unit [msec].
% METHOD - 'centered' or 'causal' (default).
%
%Output:
% DAT    - updated data structure

% Author(s): Benjamin Blankertz

misc_checkType(dat, 'STRUCT(x fs)');
misc_checkType(ms, '!DOUBLE[1]');

nSamples = round(ms*dat.fs/1000);
dat.x(:,:)= procutil_movingAverage(dat.x(:,:), nSamples, varargin{:});


function [U] = dualquatlbs(V,DQ,W)
  % DUALQUATLBS Compute dual quaternions linear blend skinning deformation of
  % vertices V, using rigid transformations stored as dual quaternions, DQ, at
  % some control points, propogated to the mesh using weights W.
  %
  % [U] = dualquatlbs(V,DQ,W)
  %
  % Inputs:
  %  V  list of vertex positions
  %  DQ  list of rigid transformations for each control point stored as dual
  %    quaternions
  %    2 by 4 by #controls
  %  W  weights, # vertices by # handles matrix of weights
  % Output:
  %  U  list of new vertex positions
  %
  % Copyright 2011, Alec Jacobson (jacobson@inf.ethz.ch)
  %
  % See also: lbs
  %

  % pad V with zeros if not 3D
  if(size(V,2) ~= 3)
    was_2d = true;
    V = [V zeros(size(V,1),1)];
  else
    was_2d = false;
  end

  % number of control points
  m = size(DQ,3);
  % should be same in W
  assert(m == size(W,2));

  % number of domain vertices
  n = size(V,1);

  % See algorithm 1 in "Geometric skinning with approximate dual quaternion
  % blending" by Kavan et al

  % compute weighted combination of DQs for each domain vertex
  % DQs seen by every vertex in domain, 2 by 4 by m by n
  WDQ = permute(repmat(W',[1,1,2,4]),[3 4 1 2]) .* repmat(DQ,[1,1,1,n]);
  % sum of weighted dual quaternions, 2 by 4 by n
  VDQ = permute(sum(WDQ,3),[1 2 4 3]);
  % regular part, n by 4
  VDQ1 = permute(VDQ(1,:,:),[3 2 1]);
  % dual part, n by 4
  VDQ2 = permute(VDQ(2,:,:),[3 2 1]);
  %VDQ2 = permute(sum(WDQ(2,:,:,:),3),[4 2 1 3]);
  len = repmat(sqrt(sum(VDQ1.^2,2)),1,4);
  VDQ1 = VDQ1./len;
  VDQ2 = VDQ2./len;

  U = ...
    V + ...
    2*cross( ...
      VDQ1(:,2:4), ...
      cross(VDQ1(:,2:4),V,2)+repmat(VDQ1(:,1),1,3).*V,2) + ...
    2*( ...
      repmat(VDQ1(:,1),1,3).*VDQ2(:,2:4) - ...
      repmat(VDQ2(:,1),1,3).*VDQ1(:,2:4) + ...
      cross(VDQ1(:,2:4),VDQ2(:,2:4),2));

  %VDQ = zeros([2 size(VDQ1)]);
  %VDQ(1,:,:) = VDQ1;
  %VDQ(2,:,:) = VDQ2;
  %% convert DQs into transformations, multiply against vertices
  %% convert rotation to quaternion and translation
  %[Q,T] = udq2quattrans(VDQ);
  %% convert quaterion
  %R = quat2mat(Q);

  % unpad U if V was 2D
  if(was_2d)
    U = U(:,1:2);
  end
end


function [Timg, tThreshFDR, n_signif,index_signif, pvals] = dtiTTestImage(Tvalues, DISTR, df, mask)
%
% Basic interpretations of the dtiTTest results to produce images and other
% values related to false discovery rate
%
%  [Timg, tThreshFDR, n_signif,index_signif, pvals] = ...
%         dtiTTestImage(Tvalues, DISTR, df, mask)
%

% Create an image of the results
Timg = dtiIndToImg(Tvalues, mask, NaN);

% When you view the montage, this sets how many slices we use.
% showSlices = [20:60];
% showSlices = [10:50];
% All the brain containing slices
% showSlices = [25:50];  % Small number for debugging
% showSlices = [15:62];
% figure; imagesc(makeMontage(Timg,showSlices)); axis image; colormap cool; colorbar;
% set(gcf,'Name','FA test'); title(sprintf('tthresh, no FDR (p<10^-^4) = %0.1f',tThresh));

% Figure out the statistical significance now.
tThresh = tinv(1-10^-4, df(1));
tMax = tinv(1-10^-12, df(1));
Timg(abs(Timg)>tMax) = tMax;
tMax = max(Timg(:));

% Perform an FDR analysis for the FA test
%
fdrVal = 0.05;   % This is the p-value we are using.
fdrType = 'general';
Tvalues(isnan(Tvalues)) = 0;
pvals = 1-tcdf(Tvalues, df(1));
[n_signif,index_signif] = fdr(pvals,fdrVal,fdrType,'mean');

% Convert back to an fThreshold.  Needs more comments.  We think that this
% function returns the t-value needed to achieve a significance fdrVal, say
% 0.05 or 0.01.  It is possible that tThreshFDR/tMax is the fThreshold,
% though the comment in the printf doesn't say that.  It just puts the
% value in a parenthesis.
if n_signif > 0
    tThreshFDR = tinv(1-max(pvals(index_signif)), df(1));
    fprintf('t-threshold for FDR (%s) when p < %0.3f: %0.2f (%0.3f).\n',...
        fdrType,fdrVal,tThreshFDR,tThreshFDR/tMax);
else
    tThreshFDR = [];
    fprintf('Nothing returned as significant');
end

return;


%% =======================================================================
%  ARDrone Simulation Example: Hovering and Position Control
%  =======================================================================
%
%  The simulation is used to validate the controller and guidance logic of
%  the ARDRone before flight testing. Control blocks are
%  exactly the same for both simulation and real-time Wi-Fi control.
%
%  Authors:
%       David Escobar Sanabria -> descobar@aem.umn.edu
%       Pieter J. Mosterman -> pieter.mosterman@mathworks.com
%  =======================================================================

%%
%  Cleaning workspace
bdclose all;
clear all;
clc

%%
% Adding ARDrone library path
addpath ../lib;
%% Simulation parameters

% Flight management system sample time. This is the sample time at which
% the control law is executed.
FMS.Ts = 0.065;

% Time delay due to communication between drone and host computer
timeDelay = FMS.Ts*4;

%% Vehicle model based on linear dynamics

% Loading state space representation of vehicle dynamics
setupARModel;

%%
% Loading list of waypoints
waypoints = getWaypoints() ;

%%
% Simulation time
simDT = 0.005 ;

%%
% Loading Simulink model of ARDrone
ARDroneHoverSim ;


function varargout = ChristoffelTensor(S,varargin)
% Christoffel tensor of an elasticity tensor for a given direction
%
% Formula: E_jk = C_ijkl n_j n_l
%
% Input
%  S - elatic compliance @tensor
%  x - list of @vector3d
%
% Output
%  E - Christoffel @tensor
%
% See also
% tensor/directionalMagnitude tensor/rotate

% take formula using stiffness
[varargout{1:nargout}] = ChristoffelTensor(inv(S),varargin{:});