%% Regression test data generator
% This program should be executed at the very beginning of refactoring a
% major revision. This is for making legacy STRAIGHT v40_007d to be
% compatible with MATLAB R2015b and Octave

% Copyright(c) 2016, Hideki Kawahara, (kawahara@sys.wakayama-u.ac.jp)

clear all
close all

original_speech_dir = '~/Music/VCTK_CORPUS/VCTK-Corpus/wav48/';
target_analysis_dir = '~/m-file/STRAIGHTV40_007e/analysisData/';
target_wave_dir = '~/m-file/STRAIGHTV40_007e/waveData/';

mkdir(target_analysis_dir);
mkdir(target_wave_dir);

dir_list = dir([original_speech_dir 'p*']);

%%

n_dirs = length(dir_list);
n_files = 0;
for ii = 1:n_dirs
  tmp_files = dir([original_speech_dir dir_list(ii).name '/*.wav']);
  n_files = n_files + length(tmp_files);
end;
%%
n_test = 2; % number of files tested for each speaker
l_segment = 0.1; % 100 ms segment
for ii = 1:n_dirs
  rng(12345); % initialize frozen random number
  seq_id = 0;
  basic_stat_table = zeros(n_files, 4);
  tmp_files = dir([original_speech_dir dir_list(ii).name '/*.wav']);
  for jj = 1:length(tmp_files)
    [x, fs] = audioread([original_speech_dir dir_list(ii).name '/' ...
      tmp_files(jj).name ]);
    seq_id = seq_id + 1;
    l_in_sample_segment = min(length(x), round(fs * l_segment));
    n_segment = floor(length(x) / l_in_sample_segment);
    rms_level = zeros(n_segment, 1);
    for kk = 1:n_segment
      rms_level(kk) = 20 * ...
        log10(std(x((kk - 1) * l_in_sample_segment + ...
        (1:l_in_sample_segment))));
    end;
    sorted_level = sort(rms_level);
    basic_stat_table(seq_id, 1) = length(x) / fs;
    basic_stat_table(seq_id, 2) = max(rms_level) - min(rms_level);
    basic_stat_table(seq_id, 3) = ...
      sorted_level(round(length(sorted_level) * 0.85));
    basic_stat_table(seq_id, 4) = max(abs(x));
  end;
  basic_stat_table = basic_stat_table(1:seq_id, :);
  % select safe region
  sorted_length = sort(basic_stat_table(:, 1));
  sorted_dynamic_range = sort(basic_stat_table(:, 2));
  sorted_85percent = sort(basic_stat_table(:, 3));
  l_10 = sorted_length(round(seq_id * 0.1));
  l_90 = sorted_length(round(seq_id * 0.9));
  d_10 = sorted_dynamic_range(round(seq_id * 0.1));
  d_90 = sorted_dynamic_range(round(seq_id * 0.9));
  v_10 = sorted_85percent(round(seq_id * 0.1));
  v_90 = sorted_85percent(round(seq_id * 0.9));
  index_list = 1:seq_id;
  safe_index = index_list( ...
    l_10 < basic_stat_table(:, 1) & l_90 > basic_stat_table(:, 1) & ...
    d_10 < basic_stat_table(:, 2) & d_90 > basic_stat_table(:, 2) & ...
    v_10 < basic_stat_table(:, 3) & v_90 > basic_stat_table(:, 3) & ...
    basic_stat_table(:, 4) < 0.95);
  selection_index = 1:length(safe_index);
  [~, tmp_index] = sort(rand(n_test, 1));
  selection_index = selection_index(tmp_index(1:n_test));
  for kk = 1:n_test
    id = safe_index(selection_index(kk));
    [x, fs] = audioread([original_speech_dir dir_list(ii).name '/' ...
      tmp_files(id).name ]);
    rng(12345); % initialize frozen random number
    f0raw = MulticueF0v14(x,fs);
    ap = exstraightAPind(x,fs,f0raw);
    n3sgram=exstraightspec(x,f0raw,fs);
    rng(12345); % initialize frozen random number
    y = exstraightsynth(f0raw,n3sgram,ap,fs);
    disp([num2str(kk) ': ' tmp_files(id).name  ' at:' datestr(now)]);
    audiowrite([target_wave_dir '/' tmp_files(id).name], ...
      y / max(abs(y)) * 0.9, fs);
    path_name_f0 = [target_analysis_dir '/' tmp_files(id).name(1:end-4) 'f0.bin'];
    path_name_ap = [target_analysis_dir '/' tmp_files(id).name(1:end-4) 'ap.bin'];
    path_name_sp = [target_analysis_dir '/' tmp_files(id).name(1:end-4) 'sp.bin'];
    WriteBinaryData(path_name_f0, f0raw)
    WriteBinaryData(path_name_ap, ap)
    WriteBinaryData(path_name_sp, n3sgram)
  end;
end;


load EB.mat
load EBresults.mat
ngenes=length(c1mtEBA);
[void, order] = sort(llr);

nt=length(times2);

printpath='outeb';
printfileall=fullfile(printpath,'all_EB');
genes_vect(1,find(strcmp(genes_vect, 'OCT3/4')))={'OCT3'};
tmu = [0 times2]';

for i = order(end:-1:end-10)'

  model1=models{i,1};
  model2=models{i,2};
  [mu21, varsigma21] = gpPosteriorMeanVar(model2.comp{1}, tmu);
  [mu22, varsigma22] = gpPosteriorMeanVar(model2.comp{2}, tmu);

  rt=mu21-mu22;

  times2 = model2.comp{1}.X;

  h=figure

  %plot(tmu, rt,'b'), hold on
  plot(tmu, mu21, 'g'), hold on
  plot(tmu, mu21+sqrt(varsigma21), 'g:');
  plot(tmu, mu21-sqrt(varsigma21), 'g:');
  plot(tmu, mu22, 'r');
  plot(tmu, mu22+sqrt(varsigma22), 'r:');
  plot(tmu, mu22-sqrt(varsigma22), 'r:');
  plot(model2.comp{1}.X, model2.comp{1}.y, 'k+')
  plot(model2.comp{2}.X, model2.comp{2}.y, 'ko')
  plot(times2(1:7), c1ptEBA(:,i), 'r+')
  plot(times2(8:end), c3ptEBA(:,i), 'r*')
  plot(times2(1:7), c1mtEBA(:,i), 'g+')
  plot(times2(8:end), c3mtEBA(:,i), 'g*')

  dt=sprintf('%s llr=%2.5f',genes_vect{1, i},llr(i));
  title(dt)

  %legend('mt-pt gp','mt gp','pt gp','c1pt','c3pt','c1mt','c3mt')
  %printfile=fullfile(printpath,genes_vect{1, i});
  %print_str=sprintf('print -depsc2 %s',printfile);
  %print_str_all=sprintf('print -dpsc2 -append %s',printfileall);
  %eval(print_str)
  %eval(print_str_all)
  %close(h)
end


%% Script to illustrate the computation of the hydration layer fraction.
%  s_HLF
%
% For reference see:
%

%% Load and align the data
%
dataDir = 'C:\u\brian\Matlab\mrDataExample\hydrationLayer';
refIm = 't1.nii.gz';
rawDir = 'raw';

% 1 for trilinear, 7 for b-spline
interp = 7;
if(interp==1), outDirName = 'trilin';
else           outDirName = 'bsplin'; end

% Fit method:
%   'f' for fminsearch
%   'l' for lsqnolin
%   'pf','pl' for parallel
% versions with disttoolbox
% What is disttoolbox?
fitMethod = 'ls';
outMontage = ['./wst1.png'];

outBaseName = outDirName;
outFile = fullfile(dataDir,[outBaseName '_aligned.mat']);
if(exist(outFile,'file'))
    disp(['Loading aligned data from ' outFile '...']);
    load(outFile);
else
    outDir = dataDir;
    % Load all the series in the struct 's'
    s = dicomLoadAllSeries(rawDir);
    ref = niftiRead(refIm);
    %mmPerVox = ref.pixdim(1:3);
    mmPerVox = [0.9 0.9 0.9];
    % Align all the series to this subject's reference volume
    [s,xform,alignInds] = relaxAlignAll(s,ref,mmPerVox,true,interp);
    s = s(alignInds);
    save(outFile,'s', 'xform', 'dataDir', 'refIm', 'outDir', 'mmPerVox');
end

% Drop the first measure- too much motion
s = s(2:end);

outDir = fullfile(outDir, [outDirName fitMethod(end)]);
if(~exist(outDir,'dir')), mkdir(outDir); end

%
% Sort out image types
%
sequenceNames = {s(:).sequenceName};
spgrInds = false(size(sequenceNames));

% find series that are *either* SPGR or FSPGR
spgrInds(strmatch('3DGRASS',sequenceNames)) = 1;
spgrInds(strmatch('EFGRE3D',sequenceNames)) = 1;

tiInds = spgrInds & [s(:).inversionTime]>0;
t1Inds = spgrInds & ~tiInds;

% Mean image, use only t1Inds to calculate it
mn = mean(cat(4,s([2,4,5]).imData),4);
[brainMask,checkSlices] = mrAnatExtractBrain(mn, mmPerVox, 0.40);
%brainMask = dtiCleanImageMask(brainMask,3,1,.2);
%brainMask = dtiCleanImageMask(mrAnatHistogramClip(mn,0.3,0.9)>0.4,10,1,.3);
brainMask(any(cat(4,s(t1Inds).imData)<=0,4)) = 0;

tr = [s(t1Inds).TR];
if(~all(tr==tr(1))), error('TR''s do not match!'); end
tr = tr(1);

flipAngles = [s(t1Inds).flipAngle];

% compute t1 with no b1 bias correction
[t1,gPD_T2s] = relaxFitT1(cat(4,s(t1Inds).imData),flipAngles,tr);
t1(~brainMask) = 0; gPD_T2s(~brainMask) = 0;
dtiWriteNiftiWrapper(single(t1), xform, fullfile(outDir,'T1_nob1.nii.gz'));
dtiWriteNiftiWrapper(single(pd), xform, fullfile(outDir,'gPD_T2s_nob1.nii.gz'));

%% Compute a b1 map using the DESPOT1-HIFI method
%b1Map = 1.0;
if(~exist(fullfile(outDir,'b1.nii.gz')))
    b1Map = relaxFitDespoT1(s, brainMask);
    dtiWriteNiftiWrapper(single(b1Map), xform, fullfile(outDir,'b1.nii.gz'));
else
    ni = niftiRead(fullfile(outDir,'b1.nii.gz'));
    b1Map = double(ni.data);
end

[t1,gPD_T2s] = relaxFitT1(cat(4,s(t1Inds).imData),flipAngles,tr,b1Map);
t1(~brainMask) = 0; gPD_T2s(~brainMask) = 0;
t1(t1>5) = 5;

dtiWriteNiftiWrapper(single(t1), xform, fullfile(outDir,'T1.nii.gz'));
dtiWriteNiftiWrapper(single(gPD_T2s), xform, fullfile(outDir,'gPD_T2s.nii.gz'));

% Compute R1
t1(t1>3) = 3;
t1(brainMask & t1<0.5) = 0.5;
r1 = zeros(size(t1));
r1(brainMask) = 1./t1(brainMask);
dtiWriteNiftiWrapper(single(r1), xform, fullfile(outDir,'R1.nii.gz'));

% FSL commands
% fast R1.nii.gz
% run_first_all -b -i R1.nii.gz -o first

%% GridFit to the gPDt2* image the T2* in neglected hopefully with good reason
[PDc CoilG]=BaisGridFit(t1,PD,xform,outDir);

%% Final HLF computation
mField = 3;
[HLF,Wf] = hlfCompute(t1,pd,xform,outDir,mField)


function DCM = spm_dcm_diagnose(DCM,varargin)
% Post hoc diagnosis of DCMs (under the Laplace approximation)
% FORMAT spm_dcm_diagnose(DCM,'field','field',...)
%
% DCM        - DCM stricture (inverted)
% field      - field name(s) of parameters to consider
%
%--------------------------------------------------------------------------
% This routine searches over all possible reduced models of a full model
% (DCM) and uses post hoc model selection to select the best. Reduced
% models mean all permutations of free parameter sets (parameters with non-
% zero prior covariance), where models are defined in terms of their prior
% covariance. The full model should be inverted prior to post hoc
% optimization. If there are more than 8 free-parameter sets, this routine
% will implement a greedy search: This entails searching over all
% permutations of the 8 sets whose removal (shrinking the prior
% variance to zero) produces the smallest reduction (greatest increase)
% in model evidence. This procedure is repeated until all sets
% are retained in the best model or there are no more parameters to
% consider.
%
% A parameter set is specified implicitly by the structure (DCM.Ep). Each
% set corresponds to a column of (the cell arrays or matrix) each field of
% DCM.Ep.
%
% if only one field is specified the log-evidence is computed as a function
% of the scaled prior variance. Redundant parameters have a log-evidence
% that keeps increasing as the prior variance shrinks.
%
% The outputs of this routine are graphics reporting the model reduction
% (optimisation). Red means weak evidence; blue strong evidence (> 3) and
% cyan very strong evidence (> 5)
%__________________________________________________________________________
% Copyright (C) 2008-2011 Wellcome Trust Centre for Neuroimaging

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

%-Get priors and posteriors
%==========================================================================
qE      = DCM.Ep;
qC      = DCM.Cp;
pE      = DCM.M.pE;
pC      = DCM.M.pC;

if isstruct(pC);
    pV  = pC;
    pC  = diag(spm_vec(pC));
end

% find the partition of parameters
%--------------------------------------------------------------------------
str   = {};
ind   = {};
if isstruct(qE)

    % get fields (parameters) to consider
    %----------------------------------------------------------------------
    if ~isempty(varargin)
        field = varargin;
    else
        field = fieldnames(qE);
    end
    for i = 1:length(field)
        q = getfield(qE,field{i});
        k = spm_fieldindices(qE,field{i});
        for j = 1:size(q,2)
            u            = 1:length(spm_vec(q(:,j)));
            str{end + 1} = [field{i} '(' num2str(j) ')'];
            ind{end + 1} = k(u); k(u) = [];
        end
    end

else
    field = {};
    k     = 1:length(spm_vec(qE));
    for j = 1:size(qE,2)
        u            = 1:length(spm_vec(qE(:,j)));
        str{end + 1} = ['qE(' num2str(j) ')'];
        ind{end + 1} = k(u); k(u) = [];
    end
end

% find free parameters
%--------------------------------------------------------------------------
ip    = {};
sp    = {};
for i = 1:length(str)
    j = find(diag(pC(ind{i},ind{i})));
    if ~isempty(j)
        ip{end + 1} = ind{i}(j);
        sp{end + 1} = str{i};
    end
end

% model search over shrinking priors
% -------------------------------------------------------------------------
n     = length(pC);
if length(field) == 1

    r     = (0:64)/32;
    for j = 1:length(r)
        for i = 1:length(ip)
            R      = speye(n,n) - sparse(ip{i},ip{i},1,n,n)*(1 - r(j));
            Z(i,j) = spm_log_evidence(qE,qC,pE,pC,pE,R*pC*R);

        end
    end

    subplot(2,1,1)
    plot(r,Z)
    title(['log-evidence for ' field{1}],'FontSize',16);
    xlabel('proportion of full prior')
    ylabel('negative free-energy')
    return

end

% model search over new prior without the i-th parameter set
% -------------------------------------------------------------------------
for i = 1:length(ip)
    R    = speye(n,n) - sparse(ip{i},ip{i},1,n,n);
    Z(i) = spm_log_evidence(qE,qC,pE,pC,pE,R*pC*R);
end

% find parameters with the least evidence
%--------------------------------------------------------------------------
[Z i] = sort(-Z);
ip    = ip(i);
sp    = sp(i);

% If there are too many subsets search use those with the least evidence
%--------------------------------------------------------------------------
if length(ip) > 8

    % flag a greedy search
    %----------------------------------------------------------------------
    ip     = ip(1:8);
    repeat = 1;

else
    repeat = 0;
end

% Create model space in terms of free parameter indices
%--------------------------------------------------------------------------
K     = spm_perm_mtx(length(ip));

% and get log-evidences
%==========================================================================
for i = 1:size(K,1)
    k    = spm_vec(ip(find(K(i,:))));
    R    = speye(n,n) - sparse(k,k,1,n,n);
    S(i) = spm_log_evidence(qE,qC,pE,pC,pE,R*pC*R);
end

% Find a good model with the most parameters
% =========================================================================
[i k]  = max(S);
k      = find(K(k,:));

nelim  = length(k);
repeat = repeat & nelim;

% Continue greedy search if any parameters have been eliminated
%--------------------------------------------------------------------------
if repeat

    fprintf('removing %i parameter set: ',nelim)
    fprintf('%s, ',sp{k});fprintf('\n')

    % Optimise selected model (prior covariance)
    %----------------------------------------------------------------------
    k         = spm_vec(ip(k));
    R         = speye(n,n) - sparse(k,k,1,n,n);
    rC        = R*pC*R;

    % Get posterior of selected model (rC)
    % ---------------------------------------------------------------------
    [F Ep Cp] = spm_log_evidence(qE,qC,pE,pC,pE,rC);

    DCM.M.pC  = rC;
    DCM.Ep    = Ep;
    DCM.Cp    = Cp;
    DCM.F     = F;
    DCM       = spm_dcm_diagnose(DCM,field{:});
    return

else

    % model posterior
    % ---------------------------------------------------------------------
    S     = S - S(end);
    p     = exp(S - max(S));
    p     = p/sum(p);

    fprintf('\n%i Irrelevant parameter sets: ',length(k))
    fprintf('%s, ',sp{k});fprintf('\n')

    % Organise field names and show results
    % ---------------------------------------------------------------------
    spm_figure('Getwin','Graphics');

    for i = 1:4
        j = (1:8) + (i - 1)*8;
        try
            tsr{i,1} = sprintf('%s,',sp{j});
        catch
            tsr{i,1} = sprintf('%s,',sp{j(1):end});
        end
    end

    subplot(2,1,1)
    bar(Z,'c'),hold on
    bar(Z(Z < 5),'b'),hold on
    bar(Z(Z < 3),'r'),hold off
    title('relative log evidence (with vs. without)','FontSize',16)
    xlabel(tsr,'FontSize',16)
    ylabel('negative free-energy','FontSize',12)

    subplot(2,2,3)
    if size(K,1) > 32, plot(S,'k'), else, bar(S,'c'), end
    title('log-posterior','FontSize',16)
    xlabel('model','FontSize',12)
    ylabel('log-probability','FontSize',12)
    axis square

    subplot(2,2,4)
    if size(K,1) > 32, plot(p,'k'), else, bar(p,'r'), end
    title('model posterior','FontSize',16)
    xlabel('model','FontSize',12)
    ylabel('probability','FontSize',12)
    axis square

end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs, non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

%RL = springs_info(:,4); % resting-length vector

% Contraction Frequency
freq = 1.0;

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
springs_info(19:end,4) = abs( cos(freq*current_time*pi) );

%NOTE: not 2*pi*ft b/c of abs()


classdef TestNet
    %TestNet

    properties (Constant)
        im1 = fullfile(mexopencv.root(), 'test', 'cat.jpg');
        im2 = fullfile(mexopencv.root(), 'test', 'space_shuttle.jpg');
    end

    methods (Static)
        function test_blobs
            imgs = cell(1,4);
            for i=1:numel(imgs)
                imgs{i} = rand(50, 50, 3, 'single');
            end
            blob = cv.Net.blobFromImages(imgs, 'SwapRB',false, 'Crop',false);
            out = cv.Net.imagesFromBlob(blob);
            assert(isequal(out, imgs));
        end

        function test_caffe_googlenet
            % load net and images
            net = load_bvlc_googlenet();
            img1 = cv.imread(TestNet.im1, 'FlipChannels',false);
            img2 = cv.imread(TestNet.im2, 'FlipChannels',false);

            % forward pass (blob from one image)
            blob = cv.Net.blobFromImages(img1, 'Size',[224 224]);
            net.setInput(blob, 'data');
            prob = net.forward('prob');
            validateattributes(prob, {'numeric'}, ...
                {'vector', 'real', 'numel',1000, '>=',0, '<=',1});
            [p,idx] = max(prob,[],2);
            %assert(abs(sum(prob) - 1) < 1e-4);

            % forward pass (blob from multiple images)
            blob = cv.Net.blobFromImages({img1,img2}, 'Size',[224 224]);
            net.setInput(blob);
            prob = net.forward();
            validateattributes(prob, {'numeric'}, ...
                {'size',[2 1000], 'real', '>=',0, '<=',1});
            [p,idx] = max(prob,[],2);
            %assert(all(abs(sum(prob,2) - 1) < 1e-4));
        end

        function test_intermediate_blobs
            net = load_bvlc_googlenet();
            img1 = cv.imread(TestNet.im1, 'FlipChannels',false);
            img2 = cv.imread(TestNet.im2, 'FlipChannels',false);

            blob = cv.Net.blobFromImages(img1, 'Size',[224 224]);
            net.setInput(blob, 'data');
            blobs = net.forward({
                'conv1/7x7_s2'
                'conv1/relu_7x7'
                'inception_4c/1x1'
                'inception_4c/relu_1x1'
            });

            blob = cv.Net.blobFromImages({img1,img2}, 'Size',[224 224]);
            net.setInput(blob, 'data');
            blobs = net.forwardAndRetrieve('conv1/7x7_s2');
        end

        function test_shrink_caffe_fp16
            model = fullfile(mexopencv.root(), 'test', 'dnn', 'GoogLeNet', ...
                'bvlc_googlenet.caffemodel');
            model_fp16 = fullfile(tempdir(), 'bvlc_googlenet_fp16.caffemodel');
            if exist(model, 'file') ~= 2
                error('mexopencv:testskip', 'missing data');
            end

            cObj = onCleanup(@() delete(model_fp16));
            cv.Net.shrinkCaffeModel(model, model_fp16);
            assert(exist(model_fp16, 'file') == 2);
        end

        function test_layers
            % Convolution layer
            % (64 filters each of size 7x7, 3x3 padding, 2x2 stride)
            % output size of convolution is: (H + 2*pad - kernel_size)/stride + 1
            lp = struct();
            lp.name = 'conv/7x7_s2';
            lp.type = 'Convolution';
            lp.dict.num_output = int32(64);
            lp.dict.kernel_size = int32(7);
            lp.dict.stride = int32(2);
            lp.dict.pad = int32(3);
            lp.dict.bias_term = true;
            lp.blobs = {
                rand(64,3,7,7,'single')*2-1,  % weights
                rand(1,1,1,64,'single')*2-1   % biases
            };

            % add layer
            net = cv.Net();
            lp_id = net.addLayer(lp.name, lp.type, lp);
            validateattributes(lp_id, {'int32'}, {'scalar', 'integer'});
            assert(lp_id ~= -1);

            % connect input layer to conv layer
            net.setInputsNames('data');
            if true
                net.connect(0, 0, lp_id, 0);
            elseif true
                %net.connect('_input.0', lp.name);
                net.connect('_input', lp.name);
            else
                net.connect('_input.data', lp.name);
            end

            % feed net batch of two 224x224 BGR images, and run forward pass
            net.setInput(rand(2,3,224,224,'single')*255, 'data');
            out = net.forward(lp.name);
            assert(isequal(size(out), [2 64 112 112]))

            b0 = net.getParam(lp_id, 0);
            validateattributes(b0, {'numeric'}, {'real'});
            %assert(isequal(b0, lp.blobs{1}));

            b1 = net.getParam(lp.name, 1);
            validateattributes(b1, {'numeric'}, {'real'});
            %assert(isequal(b1, lp.blobs{2}));

            net.setParam(lp_id, 0, lp.blobs{1});
            net.setParam(lp.name, 1, lp.blobs{2});

            net.setPreferableBackend('Default') % 'Halide'
            net.setPreferableTarget('CPU')      % 'OpenCL'
            net.enableFusion(true);

            id = net.getUnconnectedOutLayers();
            validateattributes(id, {'numeric'}, {'vector', 'integer'});
            %assert(isequal(id, lp_id))  % output layer

            id = net.getLayerId('_input');
            assert(id == 0);  % special input layer with id=0

            types = net.getLayerTypes();
            assert(iscellstr(types));

            counts = cellfun(@(t) net.getLayersCount(t), types);
            validateattributes(counts, {'numeric'}, {'vector', 'integer'});

            names = net.getLayerNames();
            assert(iscellstr(names));
            %assert(isequal(names{1}, lp.name))

            id = net.getLayerId(names{1});
            validateattributes(id, {'numeric'}, {'scalar', 'integer'});
            assert(lp_id ~= -1);
            %assert(isequal(id, lp_id))

            if true
                layer = net.getLayer(id);
            else
                layer = net.getLayer(names{1});
            end
            assert(isstruct(layer) && isscalar(layer));
            validateattributes(layer.blobs, {'cell'}, {'vector', 'numel',2});
            %assert(isequal(layer.name, lp.name))
            %assert(isequal(layer.type, lp.type))
            %assert(isequal(layer.blobs(:), lp.blobs(:)))

            layers = net.getLayerInputs(id);
            assert(isstruct(layers));

            %if true
            %    net.deleteLayer(lp_id);
            %else
            %    net.deleteLayer(lp.name);
            %end
        end
    end

end

function net = load_bvlc_googlenet()
    %TODO: download files (or run the sample "caffe_googlenet_demo.m")
    rootdir = fullfile(mexopencv.root(), 'test', 'dnn', 'GoogLeNet');
    modelTxt = fullfile(rootdir, 'deploy.prototxt');
    modelBin = fullfile(rootdir, 'bvlc_googlenet.caffemodel');
    if exist(modelTxt, 'file') ~= 2 || exist(modelBin, 'file') ~= 2
        error('mexopencv:testskip', 'missing data');
    end

    net = cv.Net('Caffe', modelTxt, modelBin);
    assert(~net.empty());
end


function [t, x, u] = fst_mpc(fst, output_data, varargin)
    warning off all;
%%
    fst.net_load = fst.load *2 - fst.PV/3 - fst.wind; %Power Balance

    %   Solve the optimal control problem
    t_Start = tic;
    [fst.u0, V_current, exitflag, output] = solveOptimalControlProblem( fst );
    t_Elapsed = toc( t_Start );

    if ( fst.iprint >= 1 )
        printSolution(fst.printClosedloopData, fst.mpciter, fst.xmeasure, fst.u0, fst.iprint, exitflag, t_Elapsed,output_data);
    end

    % Step (3):
    %Save current information:
    t = output;
    x = computeOpenloopSolution(fst,fst.u0);
    u = fst.u0;

end


%SDRSAC
% M and B are input point cloud (can be dense)
function [out] = SDRReg(M, B, config)
    add_dependencies;
    %rng(2912670);
    iter=0;
    stop = false;
    out.time = 0;  out.iter = 0;  out.maxInls = 0;

    M_subsampled = subSamplePointCloud(M, config.NSubsample, 'random');
    B_subsampled = subSamplePointCloud(B, config.NSubsample, 'random');
    B_subsampled_Tree=KDTreeSearcher(B_subsampled');
    %Now, compute the surface normals
    normalM = computeNormal(M);    normalB = computeNormal(B);
    MTree = KDTreeSearcher(M'); BTree = KDTreeSearcher(B');
    cMIdx = knnsearch(MTree, M_subsampled');
    cBIdx = knnsearch(BTree, B_subsampled');
    normalM_subsampled = normalM(:, cMIdx);
    normalB_subsampled = normalB(:, cBIdx);

    n = config.pointPerSample;
    maxInls = 0;

    % For large-span sampling
    bb = boundingBox(M_subsampled);
    maxDist = abs(bb(:,2) - bb(:,1));
    config.pointDistance = config.pointDistance*min(maxDist);

    % For parallel
    PTM = cell( config.innerIter,1); PRT = cell( config.innerIter, 1);
    PInls = zeros( config.innerIter,1);   PTime = zeros(config.innerIter, 1);

    % Start sampling
    while (iter < config.maxIter&& ~stop)
        tic;
        idxBPool = []; idxM = [];
        for sp = 1:config.kSample
            CS = [];
            while (isempty(CS))
                disp('---------Subsample for congruent----------');
                [idxMC, r1, r2, d1, d2, angle] = sampleCoplanarPoints(M_subsampled, idxM, config);
                B_subsampled_for_congruent = B_subsampled(:, randsample(size(B_subsampled,2), min(config.SamplesForCongruent, size(B_subsampled,2))));
                CS =  findCongruentSet(B_subsampled_for_congruent, r1, r2, d1, d2, angle, config);

%                 close all; plotPointClouds(M_subsampled, M_subsampled(:, idxMC), 'b.', 'ro');
%                 hold on;
%                 plot_plane( M_subsampled(:, idxMC(1:3)));
%                 if (length(CS)>0)
%                     figure;
%                     plotPointClouds(B_subsampled, B_subsampled(:, CS(1,:)), 'b.', 'ro');
%                     hold on;
%                     plot_plane( B_subsampled(:, CS(1,1:3)));
%                 end
%
            end
            idxM = [idxM; idxMC];
            idxBPool = [idxBPool; CS(:)];
        end
        % Now, a subset of M has been sampled and its congruent sets
        idxBPool = unique(idxBPool);
        M_congruent = M_subsampled(:, idxM);
        normalM_congruent = normalM_subsampled(:, idxM);
        B_congruent = B_subsampled_for_congruent(:, idxBPool);
        normalB_congruent = normalB_subsampled(:, idxBPool);
        m = M_congruent;
        normalm = normalM_congruent;
        nIter = config.innerIter;

        % Generate samples for parfor
        gIdx =zeros(nIter, n);
        for it=1:nIter
            gIdx(it,:) = randsample(size(B_congruent,2), n)';
            %gIdx(it,:) = samplePoints(B_congruent, n, [], config);
        end
        if (size(idxBPool)<=n+1)
            nIter = 1;
            disp('idxBPool'); disp(size(idxBPool));
        end
        out.time = out.time+toc;
        iter = iter + nIter;

        parfor it = 1:nIter
            tic;
            idxB = gIdx(it,:);
            b = B_congruent(:, idxB);
            normalb = normalB_congruent(:, idxB);
            [Rs,ts, ~, corrB] = sdpReg(m, b, normalm, normalb, config);
            if (~isempty(corrB))
                TM = Rs * M_subsampled + repmat(ts, 1, size(M_subsampled, 2));
                %[Ricp, Ticp] = icp(B_subsampled, TM, 'Matching', 'kDtree', 'iter', 20);
                %Ricp = eye(3); Ticp = zeros(3,1);
                [Ricp, Ticp] = icp(B_subsampled, TM, 'Matching', 'kDtree', 'WorstRejection', 0.3, 'iter', 20);
                %[Ricp, Ticp] = huber_icp(B, TM, 20, 5, 1);
                %[Ricp, Ticp] = huber_icp(B_subsampled, TM);
                %Ricp = Rs; Ticp = ts;
                TMICP = Ricp*Rs*M_subsampled + repmat(Ricp*ts + Ticp, 1, size(M_subsampled, 2));
                inls_icp = countCorrespondences(TMICP, B_subsampled_Tree, config.epsilon_sampled);
                TMTree = KDTreeSearcher(TMICP');
                inls_icp2 = countCorrespondences(B_subsampled, TMTree,config.epsilon_sampled);
                inls_icp = min(inls_icp, inls_icp2);

                PRT{it} = [Ricp*Rs Ricp*ts + Ticp];
                %PRT{it} = [Rs ts];
                PTM{it} =  Ricp*Rs*M_subsampled + repmat(Ricp*ts + Ticp, 1, size(M_subsampled, 2));
                PInls(it) = inls_icp;
            end
            PTime(it) = toc;
            disp(PTime(it));
        end
        out.time = out.time + sum(PTime);
        disp(PInls);
        disp(['======CURRENT MAX  = ' num2str(maxInls)]);
        [nInls,idx] = max(PInls);
        if (nInls > maxInls)
            maxInls = nInls;
            bestRT = PRT{idx};
            out.bestR = bestRT(:,1:3); out.bestT = bestRT(:, 4);
            TM = out.bestR*M + repmat(out.bestT, 1, size(M, 2));
            %[Ricp, Ticp] = icp(B, TM, 'Matching', 'kDtree', 'WorstRejection', 0.2, 'iter', 50);
            %[Ricp, Ticp] = huber_icp(B, TM);
%             [Ricp, Ticp] = icp(B, TM);
%             TMICP = Ricp * TM + Ticp;
%
            TMTree = KDTreeSearcher(TM');
            totalInls = countCorrespondences(TM, BTree, config.epsilon);
            totalInls2 = countCorrespondences(B, TMTree, config.epsilon);
            out.inls = maxInls;
            out.totalIns = totalInls;
            close all; plotPointClouds(B_subsampled, PTM{idx}, 'b.','r.');
            figure;
            plotPointClouds(B, TM, 'b.','r.');
            disp('---MaxInls----'); disp(maxInls);
            disp('---Total----'); disp(totalInls);
            disp('---Total2----'); disp(totalInls2);
            disp(out.bestR); disp(out.bestT);
            out.cloudA =  TM; out.cloudB = B;
        end
        T = computeStopping(0.99, maxInls./size(M,2));
        if (iter>=T && iter >= config.minIterRequired)
            stop = true;
        end
    end

end


function varargout = nii_stc(nii, out, timing)
% Perform slice timing correction to the input NIfTI data.
%
% Syntax:
%  nii_stc(filename_in, filename_out);
%  nii_out = NII_STC(filename_in);
%  nii_out = NII_STC(nii_in);
%  nii_out = NII_STC(nii_in, [], FSL_slice_timing);
%
% The mandatory first input is a NIfTI file name or a struct returned by
% nii_tool('load').
%
% If the second optional input, a NIfTI file name, is provided, the corrected
% data will be saved into the file (overwrite if exists).
%
% The third optional input, slice timing, is needed only if there is no timing
% information in nii data. It should be in FSL custom timing style, one number
% per slice in unit of TR, with a positive number meaning to shift forwards in
% time. If this input is provided, it will override the parameters in nii
% header.
%
% The optional output, if requested, returns the nii struct after corrected.
%
% The dicm2nii converter stores slice timing information into nii hdr and ext.
% The numbers range from -0.5 to 0.5 in unit of TR. This means the reference
% slice is the slice acquired at the middle of TR. If nifti from dicm2nii is
% used, there is no need to provide slice order/timing information for NII_STC.
%
% See also NII_VIEWER, NII_TOOL, DICM2NII

% Xiangrui Li (xiangrui.li@gmail.com)
% 160517 Wrote it

toSave = nargin>1 && ~isempty(out);
if toSave && ~ischar(out)
    error('Second input must be nii file name to save data.');
end
if ~toSave && nargout<1
    [out, pth] = uiputfile('*.nii;*.nii.gz', 'Input file name to save result');
    out = fullfile(pth, out);
    toSave = true;
end

if ischar(nii), nii = nii_tool('load', nii); end % file name
if ~isstruct(nii) || ~all(isfield(nii, {'hdr' 'img'}))
    error('Input must be nii struct or existing nii file name.');
end

nSL = nii.hdr.dim(4);
if nargin>2 % error check for timing
    if ~isnumeric(timing) || numel(timing)~=nSL || ~isvector(timing)
        help(mfilename);
        error('timing should have one number per slice.');
    elseif min(timing)<-1 || max(timing)>1 || max(timing)-min(timing)>1
        error('timing out of range: must be in unit of TR.');
    end
end

try
    t = nii.ext.edata_decoded.SliceTiming; % FSL style timing by dicm2nii.m
catch % get it fro hdr, but no valid code for multiband sequence
    TR = nii.hdr.pixdim(5);
    dur = nii.hdr.slice_duration;
    if dur<=0 || dur>TR/nSL, dur = TR / nSL; end % best guess
    t = 0.5 - (0:nSL-1)' * dur / TR;

    switch nii.hdr.slice_code
        case 1                                % ascending
        case 2,	t(nSL:-1:1) = t;              % descending
        case 3,	t([1:2:nSL 2:2:nSL]) = t;     % interleaved ascending, start 1
        case 4, t([nSL:-2:1 nSL-1:-2:1]) = t; % interleaved descending, start n
        case 5,	t([2:2:nSL 1:2:nSL]) = t;     % interleaved ascending, start 2
        case 6,	t([nSL-1:-2:1 nSL:-2:1]) = t; % interleaved descending, start n-1
        otherwise, t = [];
    end
end

if isempty(t)
    if nargin<3
        error('There is no slice order/timing info in nii header.');
    else
        t = timing;
    end
elseif nargin>2 && any(abs(diff(t(:)-timing(:)))>0.01) % timing provided
    warning(['Provided timing is inconsistent with header timing.' char(10 )...
        'The provided timing will be used for ' nii.hdr.file_name]);
    t = timing; % warn but use the provided timing
end

nVol = single(nii.hdr.dim(5));
nFFT = 2^ceil(log2(nVol+40)); % arbituary extra padding to reduce edge problem
F = (0:nFFT/2) / nFFT; % freq in unit of points: last is Nyquist
F = [F -F(end-1:-1:2)] * 2*pi; % symmetric around Nyquist, except 1st 0
F = permute(exp(F * 1i), [1 3 4 2]); % [1 1 1 nFFT]
ramp = single(permute(linspace(1, 0, nFFT-nVol), [1 3 4 2])); % [1 1 1 nRamp]
t = single(t);

nii.img = single(nii.img); % avoid integer type
for i = 1:nSL % slice by slice fft/ifft seems faster than all together
    y = nii.img(:,:,i,:); % [r c 1 nVol]
    pad = bsxfun(@times, y(:,:,1,nVol)-y(:,:,1,1), ramp); % [r c 1 nRamp]
    pad = bsxfun(@plus, pad, y(:,:,1,1)); % linear ramp from last to 1st point
    y = cat(4, y, pad); %  [r c 1 nFFT]
    y = fft(y, [], 4);
    y = bsxfun(@times, y, F.^t(i)); % shift phase
    y = real(ifft(y, [], 4));
    nii.img(:,:,i,:) = y(:,:,1,1:nVol); % update img while drop padding
end

if toSave, nii_tool('save', nii, out); end
if nargout, varargout{1} = nii_tool('update', nii); end


function Mh = hmxPlusMtimes(Mh,alpha,Ml,Mr)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxPlusMtimes.m                               |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Sum and product of H-Matrix with the rule     |
%|  `---'  |                                                              |
%+========================================================================+

% Check dimensions
if (size(Mh,1) ~= size(Ml,1))
    error('hmxPlusMtimes.m : matrix dimensions must agree.')
end
if (size(Mh,2) ~= size(Mr,2))
    error('hmxPlusMtimes.m : matrix dimensions must agree.')
end
if (size(Ml,2) ~= size(Mr,1))
    error('hmxPlusMtimes.m : matrix dimensions must agree.')
end

%%% H-Matrix + alpha * H-Matrix * H-Matrix --> H-Matrix
if isa(Mh,'hmx') && isa(Ml,'hmx') && isa(Mr,'hmx')

    % H-Matrix + H-Matrix * H-Matrix --> H-Matrix   (recursion)
    if (Mh.typ==0) && (Ml.typ==0) && (Mr.typ==0)
        % First bloc : M1 -> M1 + alpha * (Ml1 * Mr1 + Ml2 * Mr3)
        Mh.chd{1} = hmxPlusMtimes(Mh.chd{1},alpha,Ml.chd{1},Mr.chd{1});
        Mh.chd{1} = hmxPlusMtimes(Mh.chd{1},alpha,Ml.chd{2},Mr.chd{3});

        % Second bloc : M2 -> M2 + alpha * (Ml1 * Mr2 + Ml2 * Mr4)
        Mh.chd{2} = hmxPlusMtimes(Mh.chd{2},alpha,Ml.chd{1},Mr.chd{2});
        Mh.chd{2} = hmxPlusMtimes(Mh.chd{2},alpha,Ml.chd{2},Mr.chd{4});

        % Third bloc : M3 -> M3 + alpha * (Ml3 * Mr1 + Ml4 * Mr3)
        Mh.chd{3} = hmxPlusMtimes(Mh.chd{3},alpha,Ml.chd{3},Mr.chd{1});
        Mh.chd{3} = hmxPlusMtimes(Mh.chd{3},alpha,Ml.chd{4},Mr.chd{3});

        % Fourth bloc : M4 -> M4 + alpha * (Ml3 * Mr2  + Ml4 * Mr4)
        Mh.chd{4} = hmxPlusMtimes(Mh.chd{4},alpha,Ml.chd{3},Mr.chd{2});
        Mh.chd{4} = hmxPlusMtimes(Mh.chd{4},alpha,Ml.chd{4},Mr.chd{4});

        % Fusion
        Mh = hmxFusion(Mh);

    % H-Matrix + H-Matrix * Compr --> H-Matrix
    elseif (Mh.typ==0) && (Ml.typ==0) && (Mr.typ==1)
        A  = alpha * ( Ml * Mr.dat{1} );
        B  = Mr.dat{2};
        Mh = hmxPlusAB(Mh,A,B);

    % H-Matrix + H-Matrix * Full --> unavailable
    elseif (Mh.typ==0) && (Ml.typ==0) && (Mr.typ==2)
        error('hmxPlusMtimes : unvailable case')

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

    % H-Matrix + Compr * H-Matrix --> H-Matrix
    elseif (Mh.typ==0) && (Ml.typ==1) && (Mr.typ==0)
        A  = alpha * Ml.dat{1};
        B  = Ml.dat{2} * Mr;
        Mh = hmxPlusAB(Mh,A,B);

    % H-Matrix + Compr * Compr --> H-Matrix
    elseif (Mh.typ==0) && (Ml.typ==1) && (Mr.typ==1)
        A  = alpha * Ml.dat{1};
        B  = (Ml.dat{2} * Mr.dat{1}) * Mr.dat{2};
        Mh = hmxPlusAB(Mh,A,B);

    % H-Matrix + Compr * Full --> H-Matrix
    elseif (Mh.typ==0) && (Ml.typ==1) && (Mr.typ==2)
        error('hmxPlusMtimes : unvailable case')

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

    % H-Matrix + Full * H-Matrix --> Full
    elseif (Mh.typ==0) && (Ml.typ==2)
        error('hmxPlusMtimes : unvailable case')

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

    % Compr + H-Matrix * H-Matrix --> H-Matrix
    elseif (Mh.typ==1) && (Ml.typ==0) && (Mr.typ==0)
        Mh.typ = 0;
        Mh.row = Ml.row;
        Mh.col = Mr.col;
        for i = 1:4
            Mh.chd{i} = hmx(Ml.chd{i}.pos{1},Mr.chd{i}.pos{2},...
                Mh.dat{1}(Ml.row{i},:) , Mh.dat{2}(:,Mr.col{i}) , ...
                Ml.tol ) ;
        end
        Mh.dat = [];
        Mh     = hmxPlusMtimes(Mh,alpha,Ml,Mr);

    % Compr + H-Matrix * Compr --> Compr
    elseif (Mh.typ==1) && (Ml.typ==0) && (Mr.typ==1)
        A  = alpha * (Ml * Mr.dat{1});
        B  = Mr.dat{2};
        Mh = hmxPlusAB(Mh,A,B);

    % Compr + H-Matrix * Full --> Full
    elseif (Mh.typ==1) && (Ml.typ==0) && (Mr.typ==2)
        error('hmxPlusMtimes : unvailable case')

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

    % Compr + Compr * H-Matrix --> Compr
    elseif (Mh.typ==1) && (Ml.typ==1) && (Mr.typ==0)
        A  = alpha * Ml.dat{1};
        B  = Ml.dat{2} * Mr;
        Mh = hmxPlusAB(Mh,A,B);

    % Compr + Compr * Compr --> Compr
    elseif (Mh.typ==1) && (Ml.typ==1) && (Mr.typ==1)
        A  = alpha * Ml.dat{1};
        B  = (Ml.dat{2} * Mr.dat{1}) * Mr.dat{2};
        Mh = hmxPlusAB(Mh,A,B);

    % Compr + Compr * Full --> Compr
    elseif (Mh.typ==1) && (Ml.typ==1) && (Mr.typ==2)
        A  = alpha * Ml.dat{1};
        B  = Ml.dat{2} * Mr.dat;
        Mh = hmxPlusAB(Mh,A,B);

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

    % Compr + Full * H-Matrix --> Full
    elseif (Mh.typ==1) && (Ml.typ==2) && (Mr.typ==0)
        error('hmxPlusMtimes : unvailable case')

    % Compr + Full * Compr --> Compr
    elseif (Mh.typ==1) && (Ml.typ==2) && (Mr.typ==1)
        A  = alpha * (Ml.dat * Mr.dat{1});
        B  = Mr.dat{2};
        Mh = hmxPlusAB(Mh,A,B);

    % Compr + Full * Full --> Full
    elseif (Mh.typ==1) && (Ml.typ==2) && (Mr.typ==2)
        Mh.dat = full(Mh) + alpha * (Ml.dat * Mr.dat);
        Mh.typ = 2;

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

    % Full + --- --> Full
    elseif (Mh.typ==2)
        Mh.dat = full(Mh) + alpha * (full(Ml) * full(Mr));
        Mh.typ = 2;

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

    % Unknown type
    else
        error('hmxPlusMtimes : unvailable case')
    end

%%% Unavailable
else
    error('hmxPlusMtimes : unvailable case')
end
end


function p = addMultipliedInequalityCuts(p)

newRows = [];
for i =  p.linears
    if p.lb(i) >= -1000 | p.ub(i) <= 1000 % Avoid numerically tricky stuff
        for j = 1+p.K.f:p.K.f+p.K.l
            row = p.F_struc(j,:);
            used = find(row(2:end));
            if ~any(p.variabletype(used))
                % Linear constraint
                ok = 1;
                monom = zeros(1,length(used));
                for k = 1:length(used)
                    find1 = findrows(p.bilinears(:,2:end),[i used(k)]);
                    find2 = findrows(p.bilinears(:,2:end),[used(k) i]);
                    if (isempty(find1) & isempty(find2))
                        ok = 0;
                        break
                    else
                        monom(k) = unique([find1 find2]);
                    end
                end
                if ok
                    monom = p.bilinears(monom,1);
                    row0 = row;
                    if p.lb(i) >= -1000
                        row(1+monom) = row(1+used);
                        row(1+used) = 0;
                        row(1+i) = row(1);
                        row(1)=0;
                        newRows = [newRows;row-p.lb(i)*row0];
                    end
                    if p.ub(i) <= 1000
                        row = row0;
                        row(1+monom) = row(1+used);
                        row(1+used) = 0;
                        row(1+i) = row(1);
                        row(1)=0;
                        newRows = [newRows;-row+p.ub(i)*row0];
                    end
                end
            end
        end
    end
end
if ~isempty(newRows)
    p.F_struc = [p.F_struc;newRows];
    p.K.l = p.K.l + size(newRows,1);
end


function p03_title ( )

%*****************************************************************************80
%
%% P03_TITLE prints a title for problem 03.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Problem 03\n' );
  fprintf ( 1, '  Name:       QuintSum\n' );
  fprintf ( 1, '              Davis, Rabinowitz, page 370, #3.\n' );
  fprintf ( 1, '  Region:     0 <= X(i) <= 1\n' );
  fprintf ( 1, '  Integrand:  F(X) = ( sum ( X(i) ) )^5\n' );

  return
end


 function xpad = zpad0(x, nxp, nyp, nzp)
%function xpad = zpad0(x, nxp, nyp, nzp)
% zero pad an input signal x symmetrically around "0" (image center)
% by A. Yendiki

if nargin < 1, ir_usage, end
if nargin == 1 && streq(x, 'test'), zpad0_test, return, end

if nargin == 2
	xpad = ir_pad_into_center(x, nxp);
elseif nargin == 3
	xpad = ir_pad_into_center(x, [nxp nyp]);
elseif nargin == 4
	xpad = ir_pad_into_center(x, [nxp nyp nzp]);
else
	error 'Too many arguments'
end

function zpad0_test
zpad0([1 2 1], 7)
zpad0([1 2 1], 7, 3)


function M = rmPlotGUI_update(M)
% Refresh the Retinotopy Model Plotting GUI.
%
% M = rmPlotGUI_update(M);
%
% Update function for rmPlotGUI. Visualizes the receptive field and time
% series (data, prediction, and residual) for a given voxel for a
% retinotopy model fit.
%
% All the information about the model should be in the struct M, created
% with rmPlotGUI_getModel.
%
% ras, 09/06.
% ras, 11/08: broekn off into separate subfunction.
if ~exist('M','var') || isempty(M), M = get(gcf, 'UserData'); end

% get current voxel from the GUI
voxel = get(M.ui.voxel.sliderHandle, 'Value');

% get needed params
coords = M.coords(:,voxel);
if isequal(M.roi.viewType, 'Gray')  % convert coords into an index
    coords = M.coords(voxel);
end

M.modelNum = get(M.ui.model, 'Value');

%% compute RF, get tSeries for this voxel
[pred, RF, rfParams, variance_explained, blanks] = rmPlotGUI_makePrediction(M, coords);

% check if the prediction for this voxel is empty. If so, don't plot it:
if all( isnan(pred) | isinf(pred) )
	a = axes(M.ui.tsAxes);  cla(a);
	AX = axis;
	text(a, AX(2) + .5*(AX(2)-AX(1)), AX(3) + .5*(AX(4)-AX(3)), ...
		 '(No data available for this voxel)', ...
		 'FontSize', 14, ...
		 'HorizontalAlignment', 'center');
	return
end

% store the current prediction
M.prediction = pred;
M.RF = RF;

%%%%% VISUALIZE
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% (1) plot time series
%%%%%%%%%%%%%%%%%%%%%%%%%%%
axes(M.ui.tsAxes); cla; hold on;

hTSeries = plot(M.x, M.tSeries(:,voxel), 'k--', 'LineWidth', 1.5);
hFit = plot(M.x, pred(:, 1), 'b', 'LineWidth', 1);
hResidual = plot(M.x, M.tSeries(:,voxel)-pred(:,1), 'r:', 'LineWidth', 1);

xlim([min(M.x) max(M.x)]);

% Determine appropriate limits for y-axis. To do so, we concatenate the
% visible series. These include some or all of raw, predicted, and residual
% tseries. The user selects which are visible by checkbox in the GUI.
allPlotted = [];
if get(M.ui.tsCheck,   'Value'), allPlotted = cat(1, allPlotted, M.tSeries(:,voxel)); end
if get(M.ui.predCheck, 'Value'), allPlotted = cat(1, allPlotted, pred(:,1)); end
if get(M.ui.resCheck,  'Value'), allPlotted = cat(1, allPlotted, M.tSeries(:,voxel)-pred(:,1)); end
if max(allPlotted) > min(allPlotted), ylim([min(allPlotted) max(allPlotted)]); end

h = axis;
for n=1:numel(M.sepx),
    plot([1 1].*M.sepx(n), [h(3) h(4)], 'k:', 'LineWidth', 2);
end;
xlabel('Time (sec)');
ylabel('BOLD signal change (%)');

% set a button-down function which will allow the user to double-click on
% any time point and view the stimuls aperture at that time point.
bdf = ['M = get(gcf, ''UserData''); ' ...
	   'pt = get(gca, ''CurrentPoint''); ' ...
	   'pt = round(pt(1) / M.params.stim(1).framePeriod); ' ...
	   'st = get(gcf, ''SelectionType''); ' ...
	   'if isequal(st, ''open''), ' ...
	   '  set(M.ui.overlayStimCheck, ''Value'', 1); ' ...
	   '  mrvSliderSet(M.ui.time, ''Value'', pt); ' ...
	   '  rmPlotGUI(''update''); ' ...
	   'end; ' ...
	   'clear M st pt'];
set(M.ui.tsAxes, 'ButtonDownFcn', bdf);

% Indicate the time points with no stimulus
plot(M.x(blanks), zeros(sum(blanks),1), 'rx')

% set the user data of each checkbox to point to the PLOT curves
% we just created. The callbacks for these checkboxes will then toggle
% the visibility of each curve.
set(M.ui.tsCheck,   'UserData', hTSeries);
set(M.ui.predCheck, 'UserData', hFit);
set(M.ui.resCheck,  'UserData', hResidual);
if get(M.ui.tsCheck,   'Value')==0, set(hTSeries,  'Visible', 'off'); end
if get(M.ui.predCheck, 'Value')==0, set(hFit,      'Visible', 'off'); end
if get(M.ui.resCheck,  'Value')==0, set(hResidual, 'Visible', 'off'); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% (2) set text fields
%%%%%%%%%%%%%%%%%%%%%%%%%%%
% on Windows, we can save space expressing the "degrees" units string
if ispc, degStr = char(176);
else     degStr = 'deg'; end

% print out percent variance (R^2) explained: This calculation is incorrect
% because the predictions contain the trends.
% (ras) so how do we make it correct?
txt = sprintf('Variance explained: %.2f%%', variance_explained*100);
set(M.ui.r2Text, 'String', txt);

% print out coords of voxel
txt = ['Coords: [' num2str(M.params.roi.coords(:,voxel)') ']'];
set(M.ui.coordsText, 'String', txt);

txt = sprintf('x=%.1f, y=%.1f (%s)',rfParams(1,1), rfParams(1,2), degStr);
set(M.ui.xyText, 'String', txt);

[th r] = cart2pol(rfParams(1,1), rfParams(1,2));
th = round( (pi/2 - th) * (180/pi) ); % rad -> deg CW from up
txt = sprintf('r=%.1f %s, theta=%i %s', r, degStr, th, degStr);
set(M.ui.rthText, 'String', txt);

if size(rfParams,1)<5 && rfParams(3)==rfParams(5)
	% sigma major and minor are the same: circular pRF
	% (on the rare instance where an oval Gaussian was fit with exactly the
	% same major and minor axes, I think this should still be sufficiently
	% clear)
	txt = sprintf('sigma=%.1f%s', rfParams(3), degStr);
elseif size(rfParams, 1)==2
    txt = sprintf('sigma_1=%.2f, sigma_2=%.2f %s', rfParams(1,3), rfParams(2,3), degStr);
else
	% oval Gaussian: report both major and minor sigma
	txt = sprintf('sigma_1=%.1f, sigma_2=%.1f %s', rfParams(3), rfParams(5), degStr);
end
set(M.ui.sigmaText, 'String', txt);

txt = sprintf('beta=%.1f %% %s/sec', rfParams(4), degStr);
set(M.ui.betaText, 'String', txt);

% from rmPlot: cache the data for the current time series plot as well
M.currTsData.x    = M.x;
M.currTsData.pred = pred;
M.currTsData.raw  = M.tSeries(:,voxel);
M.currTsData.sepx = M.sepx;
M.currTsData.pRF  = RF;

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% (3) show receptive field
%%%%%%%%%%%%%%%%%%%%%%%%%%%
set(M.ui.peakCheck, 'UserData', hResidual);
rescaleToPeak = isequal( get(M.ui.peakCheck, 'Checked'), 'on' );
if get(M.ui.overlayStimCheck, 'Value')==0
    if min(M.params.analysis.Y) == max(M.params.analysis.Y)
        subplot(M.ui.rfAxes);
        plot(M.params.analysis.X,RF,'bo-','MarkerFaceColor','b');
        axis square

    else % default

        % show the RF only, don't overlay the stimulus image
        if rescaleToPeak==0,
            peak = [];
        else
            TR = M.params.stim(1).framePeriod;  % TODO: deal w/ diff't TRs in diff't scans
            peak = rfParams(4) .* M.params.analysis.sampleRate^2 .*  TR;
        end;

        rfPlot(M.params, RF, M.ui.rfAxes,peak);

        % jw add for exponentiated sigma
        modelName = rmGet(M.model{1}, 'desc');
        if strcmpi(modelName, 'fitprf')
            rfParamsExp    = rfParams;
            rfParamsExp(3) = rfParams(5); % put the exponentiated sigma into the sigma slot
            RFsEXP = rmPlotGUI_makeRFs(modelName, rfParamsExp, M.params.analysis.X, M.params.analysis.Y);
            rfPlot(M.params, RFsEXP, M.ui.rfAxes,peak);
            M.currTsData.pRF = RFsEXP; %store the exponentiated pRF in the User Data
        end
    end

    % make sure the time slider is not visible
    mrvSliderSet(M.ui.time, 'Visible', 'off');

else
	%% overlay the stimulus image on the RF
	TR = M.params.stim(1).framePeriod;  % TODO: deal w/ diff't TRs in diff't scans

    % Our time series starts at t = 0, not t = TR, hence we need to
    % subtract 1 from the x-axis index before multiplying by the TR to get
    % the correct time
    %t = get(M.ui.time.sliderHandle, 'Value') * TR;
    t = (get(M.ui.time.sliderHandle, 'Value') -1 )* TR;

	% show the time point on the time series axes
	axes(M.ui.tsAxes);
	delete( findobj('Parent', gca, 'Tag', 'CurStimPoint') );
	AX = axis;
	hLine = line([t t], AX(3:4), 'LineWidth', 2, 'LineStyle', '--', 'Color', 'm');
	set(hLine, 'Tag', 'CurStimPoint');

	% get the stimulus image for this frame
    %   becausae the t-series starts at t = 0, and the frame number starts at
    %   f = 1, the forumala to derive the frame from the time is
    %       f = (t+tr)/tr;
    %   and not
    %       f = t/tr;
    tr = M.params.stim(1).framePeriod; % MR frame
	f = round((t+tr)/ tr);
	if f < 1 || f > size(M.params.analysis.allstimimages, 1)
		fprintf('Can''t visualize stimulus: selected time point outside scan range.\n');
		return
	end
	[stimImage RF] = getCurStimImage(M, f, RF);
    if max(stimImage(:)) > 1 && max(stimImage(:)) < 256
        stimImage = stimImage/255;
    end
    if min(stimImage(:))<0
        stimImage = max(stimImage,0); % clip
    end

	% overlay and display
	axes(M.ui.rfAxes); cla
	RF_img(:,:,1) = stimImage;
	RF_img(:,:,2) = RF;
	RF_img(:,:,3) = RF;

    [x,y] = prfSamplingGrid(M.params);
    x = x(:); y = y(:);
    imagesc(x, -y, RF_img); hold on;
    plot([min(x) max(x)], [0 0], 'k-');
    plot( [0 0], [min(y) max(y)], 'k-');

    axis image  xy;
    ylabel('y (deg)');
    xlabel('x (deg)');

    % make sure the time slider is visible
	mrvSliderSet(M.ui.time, 'Visible', 'on');
end

% turn on grid if the menu option is selected
if isequal( get(M.ui.grid, 'Checked'), 'on' ),	grid on
else                                            grid off; end

%% store the updated M struct in the figure's user data
M.prevVoxel = voxel;  % also update the most recent voxel
set(M.fig, 'UserData', M);

return;
%--------------------------------------

%--------------------------------------
function [stimImage RF] = getCurStimImage(M, f, RFvals)
% Get a stimulus image matching the sampling positions as the RF.
% Also returns the RF resampled into a square grid.
x = prfSamplingGrid(M.params);

% account for the different stimuli that are shown next to each other
% f originally refers to the frame in the combined time series across scans:
% we want to break this down into scan n, frame f within that scan.
n = 1;
nStimScans = numel(M.params.stim);
while n <= nStimScans,
    tmp = f + M.params.stim(n).prescanDuration;
    if tmp > size(M.params.stim(n).images_org,2),
        f = tmp - size(M.params.stim(n).images_org,2);
        n = n + 1;
    else
        f = tmp;
        break;
    end
end

% stim image
stimImage     = NaN(size(x));
stimImage(M.params.stim(1).instimwindow) = M.params.stim(n).images_org(:,f);
stimImage     = reshape(stimImage, size(x));

% RF
RF     = NaN(size(x));
RF(M.params.stim(1).instimwindow) = normalize(RFvals, 0, 1);
RF     = reshape(RF, size(x));

return
% -------------------------------------


function test_suite = test_createRotationVector3d
%TEST_CREATEROTATIONVECTOR3D  Test case for the file createRotationVector3d
%
%   Test case for the file createRotationVector3d
%
%   Example
%     runtests('test_createRotationVector3d.m')
%
%   See also
%
% ------
% Author: oqilipo
% Created: 2017-08-08, using Matlab R2016b
% Copyright 2017

test_suite = functiontests(localfunctions);

function test_random(testCase)
A=-1+2.*rand(1,3);
B=-2+4.*rand(1,3);
ROT = createRotationVector3d(A,B);
C = transformVector3d(A,ROT);
testCase.assertEqual(normalizeVector3d(B),normalizeVector3d(C), 'AbsTol', 1e-14);
testCase.assertEqual(vectorNorm3d(A),vectorNorm3d(C), 'AbsTol', 1e-14)

function test_parallel(testCase)
A=-1+2.*rand(1,3);
B=A*2;
ROT = createRotationVector3d(A,B);
C = transformVector3d(A,ROT);
testCase.assertEqual(normalizeVector3d(B),normalizeVector3d(C), 'AbsTol', 1e-14);
testCase.assertEqual(vectorNorm3d(A),vectorNorm3d(C), 'AbsTol', 1e-14)

function test_antiparallel(testCase)
A=-1+2.*rand(1,3);
B=A*-3;
ROT = createRotationVector3d(A,B);
C = transformVector3d(A,ROT);
testCase.assertEqual(normalizeVector3d(B),normalizeVector3d(C), 'AbsTol', 1e-14);
testCase.assertEqual(vectorNorm3d(A),vectorNorm3d(C), 'AbsTol', 1e-14)


function idx = argmin(x)

    [~,idx] = min(x);


function newxy=screenxyabs(monitor,xy)
% newxy = screenxyabs(monitor,xy)  SCREEN X-Y ABSolute coordinates.
%   Calculates absolute screen coordinates from the relative monitor screen
%   coordinates. This function simplifies the use of multiple monitors
%   in Matlab through the use of relative coordinates for each monitor.
%   See also SCREENXYMON.M for calculation of relative coordinates.
%   "newxy" is a vector of xy pairs (pixels) calculated for each input xy pair.
%   "monitor" is a number associated with each number (Default=1).
%       Monitor numbering is defined as the row number of the corresponding
%       monitor in the root property 'MonitorPositions'.
%   "xy" is a vector of xy coordinate pairs (pixels) relative to the monitor's
%       origin (1,1) which is the coordinate of the monitor's lower left corner.
%   Examples:
%       y=screenxyabs(2,[1,1]);  y=screenxyabs(2,[1,1,1024,768]);

%   Copyright 2006 Mirtech, Inc.
%   created 09/25/2006  by Mirko Hrovat on Matlab Ver. 7.2
%   Mirtech, Inc.       email: mhrovat@email.com
%   Uses getmondim.m

switch nargin
    case 2
    case 1
        xy=[];
    case 0
        monitor=[];
        xy=[];
    otherwise
        error ('  Too many arguments!')
end  % switch
if isempty(monitor),    monitor=1;  end
if isempty(xy),         xy=[1,1];   end
xypairs=fix(numel(xy)/2);
xyformat=size(xy);
xy=reshape(xy,[2,xypairs]);     % make each row correspond to a different xy pair
mpos=getmondim(monitor);        % get monitor's origin
if isempty(mpos),
    error ('  Monitor %g does not exist',monitor)
else
    newxy=reshape(xy+repmat([mpos(1)-1;mpos(2)-1],[1,xypairs]),xyformat);
end


function varargout = plot(varargin)
%PLOT  Plots the feasible region of a set of constraints
%
% p = plot(C,x,c,n,options)
%
% Note that only convex sets are allowed, or union of convex sets
% represented using binary variables (either defined explictly or
% introduced by YALMIP when modelling, e.g., mixed integer linear
% programming representable operators)
%
% C:  Constraint object
% x:  Plot variables [At most three variables]
% c:  color [double] ([r g b] format) or char from 'rymcgbk'
% n:  #vertices [double ] (default 100 in 2D and 300 otherwise)
% options: options structure from sdpsettings

% Get the onstraints
if nargin<1
    return
end

F = varargin{1};

if length(F)==0
    return;
end

if nargin < 5
    opts = sdpsettings('verbose',0);
else
    opts = varargin{5};
    if isempty(opts)
        opts = sdpsettings('verbose',0);
    end
end
opts.verbose = max(opts.verbose-1,0);

if any(is(F,'uncertain'))
    F = robustify(F,[],opts);
end

if nargin < 3
    color=['rymcgbk']';
else
    color = varargin{3};
    if isa(color,'sdpvar')
        error('The variables should be specified in the second argument.');
    end
    color = color(:)';
    if isempty(color)
        color=['rymcgbk']';
    end
end

% Plot onto this projection (at most in 3D)
if nargin < 2
    x = [];
else
    x = varargin{2};
    if ~isempty(x)
        if ~isa(x,'sdpvar')
            error('Second argument should be empty or an SDPVAR');
        end
        x = x(:);
        x = x(1:min(3,length(x)));
    end
end

if isempty(F)
    return
end

if any(is(F,'sos'))
    % Use image representation, feels safer (I'm not sure about the logic
    % in the code at the moment. Can the dualization mess up something
    % otherwise...)
    if ~(opts.sos.model == 1)
        opts.sos.model = 2;
    end
    % Assume the variables we are plotting are the parametric
    F = compilesos(F,[],opts,x);
end

% Create a model in YALMIPs low level format
% All we change later is the cost vector
%sol = solvesdp(F,sum(x),opts);
[model,recoverdata,diagnostic,internalmodel] = export(F,[],opts,[],[],0);
if isempty(internalmodel) | (~isempty(diagnostic) && diagnostic.problem)
    error('Could not create model. Can you actually solve problems with this model?')
end
internalmodel.options.saveduals = 0;
internalmodel.getsolvertime = 0;
internalmodel.options.dimacs = 0;

% Try to find a suitable set to plot
if isempty(x)
    if isempty(internalmodel.extended_variables) & isempty(internalmodel.aux_variables)
        x = depends(F);
        x = x(1:min(3,length(x)));
        localindex = 1;
        localindex = find(ismember(recoverdata.used_variables,x));
    else
        % not extended variables
        candidates = setdiff(1:length(internalmodel.c),[ internalmodel.aux_variables(:)' internalmodel.extended_variables(:)']);
        % Not nonlinear variables
        candidates = candidates(find(internalmodel.variabletype(candidates)==0));
        % Settle with this guess
        localindex = candidates(1:min(3,length(candidates)));
        x = localindex;
    end
else
    localindex = [];
    for i = 1:length(x)
        localindex = [localindex find(ismember(recoverdata.used_variables,getvariables(x(i))))];
    end
end

if nargin < 4
    if length(x)==3
        n = 300;
    else
        n = 100;
    end
else
    n = varargin{4};
    if isempty(n)
        if length(x)==3
            n = 300;
        else
            n = 100;
        end
    end
    if ~isa(n,'double')
        error('4th argument should be an integer>0');
    end
end

if ~isempty(internalmodel.integer_variables)
    error('PLOT can currently not display sets involving integer variables');
end

% Clean up in case we are plotting a relaxed set and solver has support for
% evaluation, but we don't intend to use that
if internalmodel.solver.evaluation==0
    internalmodel.evalMap = [];
end

% Use common function for lmi/plot and optimizer/plot
if nargout == 0
    plotInternalModel(internalmodel,x,n,localindex,color,opts);
else
    varargout{1} = plotInternalModel(internalmodel,x,n,localindex,color,opts);
end


function [bnet, order, BIC_score, LOGLIKE] = learn_struct_EM(bnet, samplesM, max_loop)
% LEARN_STRUCT_EM(), structural EM algorithm , learn structure and parameters
% from missing data.
% [bnet, order, BIC_score] = learn_struct_EM(bnet, samplesM, max_loop)

tiny = exp(-700);
improve_factor = 0.001;     %when current BIC score is less than old_score+old_score*improve_factor, stop search
N = length(bnet.dag);
ncases = size(samplesM, 2);
log_value = log(ncases);
ns = bnet.node_sizes;
dag = zeros(N,N);
order = zeros(1,N);    %save the label of each node in current dag correspond to the original dag
order = 1:N;           %original dag has nodes label 1:N
CPT = cell(2);         %save the modified node's CPT of the bnet that has the highest score in an iteration
                       %in cases "del" and "add", there is only one CPT will be save, in "rev" need to save two CPTs.
update_samples = cell(N, ncases);   %in this algorithm, because the label of the next dag will be different with the
                                    %last dag, so the label of the trainning data will be modified, too

loop = 0;
evidence = cell(1,N);
while loop<max_loop    % generally set the max_loop to 30
   loop = loop + 1
   engine = jtree_inf_engine(bnet);
   [bnet, LOGLIKE] = learn_params_em(engine, samplesM, 10);     % default set the parameter EM runs 10 iterations
   for i=1:N
      s = struct(bnet.CPD{i});
      counts = s.counts(:);
      ll(i) = sum(log(s.CPT(:) + tiny) .* counts);
   end
   [D,d] = compute_bnet_nparams(bnet);

   [nbrs, ops, nodes, orders] = mk_nbrs_of_dag_topo(bnet.dag);
   nGs = length(nbrs);

   [ec, ec1, LL] = compute_approx_ess(bnet, samplesM, ops, nodes);
   bic_score0 = sum(LL);
   bic_score0 = bic_score0 - 0.5 * D * log_value;  % bic score of current bnet

   bic_score = zeros(1,nGs);   % save each neighbour dag(bnet)'s bic score
   for i=1:nGs
      bic_score(i) = -inf;
   end
   for i=1:nGs
      edge = nodes(i,:);
      switch ops{i}
      case 'del'
         head = edge(1);
         tail = edge(2);
         approx_ess = ec{i}.counts;
         CPT1 = mk_stochastic(approx_ess);

         LL1 = LL;
         LL1(tail) = sum(log(CPT1(:) + tiny) .* approx_ess(:));
         d1 = d;
         d1(tail) = d(tail) / ns(head);
         D1 = sum(d1);
         bic_score(i) = sum(LL1) - 0.5 * D1 * log_value;
         [a, j] = max(bic_score);
         if j==i                                     % if the current dag has the highest bic score, save it's CPT(s)
            CPT{1} = CPT1;
         end

      case 'add'
         head = edge(1);
         tail = edge(2);
         approx_ess = ec{i}.counts;
         if head>tail                     % now, the "ess" is in ascent manner, accord with the labels in the "domain" field.
            n = length(ec{i}.domain);     % need permute , so that "ess" contain the last dimension is about the "tail" node.
            approx_ess = permute(approx_ess, [1:n-2, n, n-1]);       % because there is only one "edge" modified, only need
         end                                                         % to exchange the last two dimension if needed.
         CPT1 = mk_stochastic(approx_ess);

         LL1 = LL;
         d1 = d;
         LL1(tail) = sum(log(CPT1(:) + tiny) .* approx_ess(:));
         d1(tail) = d(tail) * ns(head);
         D1 = sum(d1);
         bic_score(i) = sum(LL1) - 0.5 * D1 * log_value;
         [a, j] = max(bic_score);
         if j==i
            CPT{1} = CPT1;
         end

      case 'rev'          % ops "rev" influent two family, equals the combination of a "del" and an "add"
         % "del" an edge
         head = edge(1);
         tail = edge(2);
         approx_ess = ec1{i}.counts;
         CPT1 = mk_stochastic(approx_ess);
         LL1 = LL;
         LL1(tail) = sum(log(CPT1(:) + tiny) .* approx_ess(:));
         d1 = d;
         d1(tail) = d(tail) / ns(head);

         % "add" an edge
         head = edge(2);
         tail = edge(1);
         approx_ess = ec{i}.counts;
         if head>tail                     % now, the "ess" is in ascent manner, accord with the labels in the "domain" field.
            n = length(ec{i}.domain);     % need permute , so that "ess" contain the last dimension is about the "tail" node.
            approx_ess = permute(approx_ess, [1:n-2, n, n-1]);       % because there is only one "edge" modified, only need
         end                                                         % to exchange the last two dimension if needed.
         CPT2 = mk_stochastic(approx_ess);
         LL1(tail) = sum(log(CPT2(:) + tiny) .* approx_ess(:));
         d1(tail) = d(tail) * ns(head);

         D1 = sum(d1);
         bic_score(i) = sum(LL1) - 0.5 * D1 * log_value;
         [a, j] = max(bic_score);
         if j==i
            CPT{1} = CPT1;
            CPT{2} = CPT2;
         end
      end
   end

   [BIC_score, i] = max(bic_score);
   temp = abs(bic_score0) * improve_factor;      % search will be finish when the improvment of bic score
                                        % less than 0.1% compare with the previous best result
   if BIC_score > (bic_score0 + temp)
      dag1 = nbrs{i};                   % new best dag
      order1 = orders{i};               % labels of each nodes altered from last iteration

      % the labels of each nodes are altered, so the "data" will need to "re-arrange" according to the new order
      for j = 1:N
         row = order1(j);
         for k = 1:ncases
            update_samples{j,k} = samplesM{row,k};
         end
      end
      samplesM = update_samples;

      dag = dag1(order1, order1);       % "reshape" the best dag, make it as an "upper trianglar"
      ns = ns(order1);                  % also must modify the order of "ns"
      CPDs = bnet.CPD;
      bnet = mk_bnet(dag, ns);          % use the best dag now to produce a new bnet, with altered nodes labels
      for j=1:N                         % randomly set the CPTs values of each CPDs
         bnet.CPD{j} = tabular_CPD(bnet, j, 'prior_type', 'dirichlet', 'dirichlet_weight', 0);
      end
      edge = nodes(i,:);

      % update the CPDs of new best bnet(dag) using corresponding CPDs of last iteration.
      % copy the old CPTs that not altered.
      % set the altered CPTs from the saved "CPT" variables
      switch ops{i}
      case 'del'
         tail = edge(2);
         tail = find(order1==tail);
         bnet.CPD{tail} = set_fields(bnet.CPD{tail}, 'CPT', CPT{1});
         forbidden = [tail];
         bnet.CPD = copy_CPD(bnet.CPD, CPDs, order1, forbidden);
      case 'add'
         tail = edge(2);
         tail = find(order1==tail);
         bnet.CPD{tail} = set_fields(bnet.CPD{tail}, 'CPT', CPT{1});
         forbidden = [tail];
         bnet.CPD = copy_CPD(bnet.CPD, CPDs, order1, forbidden);
      case 'rev'
         head = edge(2);
         head = find(order1==head);
         bnet.CPD{head} = set_fields(bnet.CPD{head}, 'CPT', CPT{1});
         tail = edge(1);
         tail = find(order1==tail);
         bnet.CPD{tail} = set_fields(bnet.CPD{tail}, 'CPT', CPT{2});
         forbidden = [head, tail];
         bnet.CPD = copy_CPD(bnet.CPD, CPDs, order1, forbidden);
      end

      % draw a graph for the new best dag with nodes labels are the same as the original
      order = order(order1);
%      labels = cellstr(int2str(order'));
%      figure(loop+1);
%      draw_graph(dag,labels);

      clear bic_score D d;           % for each iteration, re-compute all the expected counts and bic score
      clear ec ec1 LL;
      clear nbrs ops nodes orders;
   else
      BIC_score = bic_score0;    % if there is no improvement in bic score, stop the search, and return
      break;
   end
end
BIC_score

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [D,d]= compute_bnet_nparams(bnet)
%
%
N = length(bnet.dag);
d = zeros(1,N);
for i=1:N
   a = struct(bnet.CPD{i});
   d(i) = a.nparams;
end
D = sum(d);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function  newCPD = copy_CPD(newCPD, CPDs, order, forbidden)
%copy CPDs from old bnet to new bnet, except those nodes has been modified
%
N = length(order);
for i=1:N
   if ~mysubset(i, forbidden)
      a = order(i);
      s = struct(CPDs{a});
      CPT = s.CPT;
      newCPD{i} = set_fields(newCPD{i}, 'CPT', CPT);
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ec, ec1, LL] = compute_approx_ess(bnet, samplesM, ops, nodes)
%compute all neighbours' needed approximate ess based on current bnet.
%
tiny = exp(-700);
N = length(bnet.dag);
ns = bnet.node_sizes;
ncases = size(samplesM, 2);
nGs = length(ops);
ec0 = cell(1,N);
ec = cell(1, nGs);         %store each neighbours' altered family's approximate ess.
ec1 = cell(1, nGs);        %since operator 'rev' need to alter two families, ec1 store the ess of family deleted an edge
copy = zeros(1, nGs);
copy1 = zeros(1, nGs);
LL = zeros(1, N);          %For current bnet, LL store each nodes's LogLike based on approximate ess.
for i =1:nGs
   ec{i}.domain = [];
   ec{i}.counts = [];
   ec1{i}.domain = [];
   ec1{i}.counts = [];
end
for i =1:N
   parents = bnet.parents{i};
   family = [parents, i];
   ec0{i} = 0 * myones(ns(family));
end
for i =1:nGs
   edge = nodes(i, :);
   switch ops{i}
   case 'del'
      head = edge(1);
      tail = edge(2);
      parents = bnet.parents{tail};
      parents = mysetdiff(parents, head);
      domain = [parents, tail];
      copy(i) = find_same_domain(ec, domain, i);
      ec{i}.domain = domain;
      ec{i}.counts = 0 * myones(ns(domain));
   case 'add'
      head = edge(1);
      tail = edge(2);
      parents = bnet.parents{tail};
      parents = [parents, head, tail];
      domain = sort(parents);
      copy(i) = find_same_domain(ec, domain, i);
      ec{i}.domain = domain;
      ec{i}.counts = 0 * myones(ns(domain));
   case 'rev'
      head = edge(1);
      tail = edge(2);
      parents = bnet.parents{tail};
      parents = mysetdiff(parents, head);
      domain = [parents, tail];
      copy1(i) = find_same_domain(ec, domain, i);
      ec1{i}.domain = domain;
      ec1{i}.counts = 0 * myones(ns(domain));

      head = edge(2);
      tail = edge(1);
      parents = bnet.parents{tail};
      parents = [parents, head, tail];
      domain = sort(parents);
      copy(i) = find_same_domain(ec, domain, i);
      ec{i}.domain = domain;
      ec{i}.counts = 0 * myones(ns(domain));
   end
end

engine = jtree_inf_engine(bnet);

for l =1:ncases
   evidence = samplesM(:, l);
   [engine, ll] = enter_evidence(engine, evidence);
   ns_eff = ns;
   ns_eff(~isemptycell(evidence)) = 1;
   Vmarg = cell(1,N);
   for i =1:N
      Vmarg{i} = marginal_nodes(engine, i);
   end
   for i = 1:N
      parents = bnet.parents{i};
      family = [parents, i];
      nfamily = length(family);
      Fmarg = [];
      for j = 1:nfamily
         Fmarg = multiply_one_marginal(Fmarg, Vmarg{family(j)}, ns_eff);
      end
      fullm = add_ev_to_dmarginal(Fmarg, evidence, ns);
      ec0{i} = ec0{i} + fullm.T;
   end

   for i = 1:nGs
      switch ops{i}
      case 'del'
         if ~copy(i)
            domain = ec{i}.domain;
            Fmarg = [];
            for j=1:length(domain)
               Fmarg = multiply_one_marginal(Fmarg, Vmarg{domain(j)}, ns_eff);
            end
            fullm = add_ev_to_dmarginal(Fmarg, evidence, ns);
            ec{i}.counts = ec{i}.counts + fullm.T;
         end
      case 'add'
         if ~copy(i)
            domain = ec{i}.domain;
            Fmarg = [];
            for j=1:length(domain)
               Fmarg = multiply_one_marginal(Fmarg, Vmarg{domain(j)}, ns_eff);
            end
            fullm = add_ev_to_dmarginal(Fmarg, evidence, ns);
            ec{i}.counts = ec{i}.counts + fullm.T;
         end
      case 'rev'
         if ~copy1(i)
            domain = ec1{i}.domain;
            Fmarg = [];
            for j=1:length(domain)
               Fmarg = multiply_one_marginal(Fmarg, Vmarg{domain(j)}, ns_eff);
            end
            fullm = add_ev_to_dmarginal(Fmarg, evidence, ns);
            ec1{i}.counts = ec1{i}.counts + fullm.T;
         end

         if ~copy(i)
            domain = ec{i}.domain;
            Fmarg = [];
            for j=1:length(domain)
               Fmarg = multiply_one_marginal(Fmarg, Vmarg{domain(j)}, ns_eff);
            end
            fullm = add_ev_to_dmarginal(Fmarg, evidence, ns);
            ec{i}.counts = ec{i}.counts + fullm.T;
         end
      end
   end
   clear Vmarg;
end

for i =1:nGs
   if copy(i)
      ec{i}.counts = ec{copy(i)}.counts;
   end
   if copy1(i)
      ec1{i}.counts = ec{copy1(i)}.counts;
   end
end

for i=1:N
   s = struct(bnet.CPD{i});
   counts = ec0{i};
   LL(i) = sum(log(s.CPT(:) + tiny) .* counts(:));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function index = find_same_domain(ec, domain, length)
%
%
index = 0;
for i = 1:length
   if isequal(domain, ec{i}.domain)
      index = i;
      break;
   end
end

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


function dist = sll_distanceeuclcv(data,condid,varargin)

%preliminary code, undocumented; please do not distribute
%data is 3D (variables, timepoints,observations)
%condid is a cell of strings denoting the condition id for each observation
%
%temporalgen output: dsvm.d: (test time x train time x conditions)
%multiclass output: dsvm.d: (actual condition x predicted condition x conditions)
%
% example:
%  d = sll_distanceeuclcv(data,condid,'numpermutation',10,'verbose',2);
%
% Author: Dimitrios Pantazis

%% parse inputs

numpermutation  = sll_inputparser(varargin,'numpermutation',100, @(x) isscalar(x) && x>0 && x == round(x) );
kfold           = sll_inputparser(varargin,'kfold',5, @(x) isscalar(x) && x>=2 && x == round(x));
method          = sll_inputparser(varargin,'method','pairwise',{'pairwise','temporalgen'});
whiten          = sll_inputparser(varargin,'whiten',true);
verbose         = sll_inputparser(varargin,'verbose',false);

%% check inputs and convert string labels to numbers

[condlabel,condidval,conditiongen,data2,condlabel2,condidval2] = sll_checkdatacondid(data,condid,varargin{:});

%% initialize variables

numcond = length(condlabel); %number of conditions
rng('shuffle'); %seed the random number generator based on the current time
d = 0; %store decoding results
d2 = 0; %store decoding results (if cross-condition generalized)

%% compute cross-validated Euclidean distance
for p = 1:numpermutation

    %% verbose permutation

    if verbose & ~rem(p,verbose)
        disp(['Permutation: ' num2str(p) ' out of ' num2str(numpermutation)]);
    end

    %% assign data to k folds

    foldid = sll_createfolds(condidval,kfold);
    for c = 1:numcond
        traindata{c} = mean(data(:,:,condidval==c & foldid<kfold),3);
        testdata{c} = mean(data(:,:,condidval==c & foldid==kfold),3);
        if conditiongen
            n = nnz(condidval==c & foldid==kfold); %we need that many elements from data2
            cndx = find(condidval2==c); %available elements in condition c
            testdata2{c} = mean(data2(:,:,cndx(randperm(length(cndx),n))),3);
        end
    end

    %% multivariate noise normalization (whiten data)

    if whiten
        %compute whitening matrix using only training data
        down = max(round(size(data,2)/100),1); %downsample the data to make computation fast
        W = sll_whitenmatrix(data(:,1:down:end,foldid<kfold),condidval(foldid<kfold));
        %apply whitening matrix
        for c = 1:numcond
            traindata{c} = W*traindata{c}; %multiply data with whitener
            testdata{c} = W*testdata{c}; %multiply data with whitener
            if conditiongen
                testdata2{c} = W*testdata2{c};
            end
        end
    end

    %% Euclidean cross-validated distance

    d = d + distanceeuclcv(traindata,testdata,method);
    if conditiongen
        d2 = d2 + distanceeuclcv(traindata,testdata2,method);
    end

end

%% normalize output

d = d/numpermutation; %divide by number of permutations
d2 = d2/numpermutation; %divide by number of permutations

%% parse output

dist.d                 = d;
dist.condlabel         = condlabel;
dist.numpermutation    = numpermutation;
dist.kfold             = kfold;
dist.whiten            = whiten;
dist.method            = method;
if conditiongen
    dist.d2            = d2;
    dist.condlabel2    = condlabel2;
end

%% local function; computes Euclidean cross-validated distance
function d = distanceeuclcv(traindata,testdata,method)

%initial variables
ncond = length(traindata);
ntimes = size(traindata{1},2);
[x,y] = find(tril(ones(ncond,ncond),-1)); %indices of lower triangle

switch method

    case 'pairwise'

        %pairwise Euclidean cross-validated distance
        d = zeros(ntimes,ncond*(ncond-1)/2,'single');
        for i = 1:length(x)
            d(:,i) = sum( (traindata{x(i)}-traindata{y(i)}).*(testdata{x(i)}-testdata{y(i)}))'; %cross-validated Euclidean distance
        end

    case 'temporalgen'

        %pairwise Euclidean cross-valideated with temporal generalization
        d = zeros(ntimes,ntimes,ncond*(ncond-1)/2,'single');
        for i = 1:length(x)
            d(:,:,i) = (traindata{x(i)}-traindata{y(i)})' * (testdata{x(i)}-testdata{y(i)}); %cross-validated Euclidean distance
        end

end


function wedge_grid_plot ( n, ng, g, header )

%*****************************************************************************80
%
%% WEDGE_GRID_PLOT sets up a GNUPLOT plot of a unit wedge grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 August 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of subintervals.
%
%    Input, integer NG, the number of nodes.
%
%    Input, real G(3,NG), the grid point coordinates.
%
%    Input, string HEADER, the header for the files.
%

%
%  Create the vertex file.
%
  [ v1, v2, v3, v4, v5, v6 ] = wedge_vertices ( );

  vertex_filename = strcat ( header, '_vertices.txt' );
  vertex_unit = fopen ( vertex_filename, 'wt' );

  fprintf ( vertex_unit, '%g  %g  %g\n', v1(1), v1(2), v1(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v2(1), v2(2), v2(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v3(1), v3(2), v3(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v1(1), v1(2), v1(3) );
  fprintf ( vertex_unit, '\n' );

  fprintf ( vertex_unit, '%g  %g  %g\n', v4(1), v4(2), v4(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v5(1), v5(2), v5(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v6(1), v6(2), v6(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v4(1), v4(2), v4(3) );
  fprintf ( vertex_unit, '\n' );

  fprintf ( vertex_unit, '%g  %g  %g\n', v1(1), v1(2), v1(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v4(1), v4(2), v4(3) );
  fprintf ( vertex_unit, '\n' );

  fprintf ( vertex_unit, '%g  %g  %g\n', v2(1), v2(2), v2(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v5(1), v5(2), v5(3) );
  fprintf ( vertex_unit, '\n' );

  fprintf ( vertex_unit, '%g  %g  %g\n', v3(1), v3(2), v3(3) );
  fprintf ( vertex_unit, '%g  %g  %g\n', v6(1), v6(2), v6(3) );
  fprintf ( vertex_unit, '\n' );

  fclose ( vertex_unit );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Created vertex file "%s"\n', vertex_filename );
%
%  Create the node file.
%
  node_filename = strcat ( header, '_nodes.txt' );
  node_unit = fopen ( node_filename, 'wt' );
  for j = 1 : ng
    fprintf ( node_unit, '%g  %g  %g\n', g(1:3,j) );
  end
  fclose ( node_unit );
  fprintf ( 1, '  Created node file "%s"\n', node_filename );
%
%  Create the command file.
%
  command_filename = strcat ( header, '_commands.txt' );
  command_unit = fopen ( command_filename, 'wt' );
  fprintf ( command_unit, '# %s\n', command_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '# Usage:\n' );
  fprintf ( command_unit, '#  gnuplot < %s\n', command_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set term png\n' );
  plot_filename = strcat ( header, '.png' );
  fprintf ( command_unit, 'set output "%s"\n', plot_filename );
  fprintf ( command_unit, 'set xlabel "<--- X --->"\n' );
  fprintf ( command_unit, 'set ylabel "<--- Y --->"\n' );
  fprintf ( command_unit, 'set zlabel "<--- Z --->"\n' );
  fprintf ( command_unit, 'set title "%s"\n', header );
  fprintf ( command_unit, 'set grid\n' );
  fprintf ( command_unit, 'set key off\n' );
  fprintf ( command_unit, 'set view 80, 85\n' );
  fprintf ( command_unit, 'set style data lines\n' );
  fprintf ( command_unit, 'set timestamp\n' );
  fprintf ( command_unit, 'splot "%s" with lines lw 3, \\\n', vertex_filename );
  fprintf ( command_unit, '      "%s" with points pt 7 lt 0\n', node_filename );
  fclose ( command_unit );

  fprintf ( 1, '  Created command file "%s"\n', command_filename );

  return
end


% Test file for chebtech/mrdivide.m

function pass = test_mrdivide(pref)

% Get preferences.
if (nargin < 1)
    pref = chebtech.techPref();
end

% Generate a few random points to use as test values.
seedRNG(6178);
x = 2 * rand(100, 1) - 1;

% Random number to use as a scalar constant.
alpha = -0.194758928283640 + 0.075474485412665i;

for n = 1:2
    if ( n == 1 )
        testclass = chebtech1();
    else
        testclass = chebtech2();
    end

    %%
    % Check division of a chebtech object by a numeric array.

    f_op = @(x) [sin(x) cos(x)];
    f = testclass.make(f_op, [], pref);
    pass(n, 1) = isnan(f / 0);

    g = f / alpha;
    g_exact = @(x) f_op(x) ./ alpha;
    pass(n, 2) = norm(feval(g, x) - g_exact(x), inf) < ...
        10*max(vscale(g)*eps);

    % A "least-squares" case where the solution is obvious.
    I = eye(2);
    g = f / I;
    err = g*I - f;
    pass(n, 3) = max(max(abs(feval(err, x)))) < 10*max(vscale(g)*eps);

    % A less trivial least-squares case for which we still know the answer.
    A = [1 1];
    g = f / A;
    g_exact = @(x) (sin(x) + cos(x))/2;
    pass(n, 4) = norm(feval(g, x) - g_exact(x), inf) < ...
        10*max(vscale(g)*eps);

    %%
    % Check division of a numeric array by a chebtech object.

    f = testclass.make(@(x) sin(x));
    g = alpha / f;
    pass(n, 5) = abs(innerProduct(f, g) - alpha) < 10*max(vscale(g)*eps);

    f = testclass.make(@(x) [sin(2*pi*x) cos(2*pi*x)]);
    g = [1 1]/f;
    g_exact = @(x) (sin(2*pi*x) + cos(2*pi*x));
    pass(n, 6) = norm(feval(g, x) - g_exact(x), inf) < ...
        1e2*max(vscale(g)*eps);

    %%
    % Check error conditions.

    % Catch dimension mismatch errors.
    try
        g = f / [1 2 3];
        pass(n, 7) = false;
    catch ME
        pass(n, 7) = strcmp(ME.identifier, 'CHEBFUN:CHEBTECH:mrdivide:size');
    end

    % Can't do f/g if both f and g are chebtech objects.
    try
        f = testclass.make(@(x) sin(x));
        g = testclass.make(@(x) cos(x));
        h = f / g;
        pass(n, 8) = false;
    catch ME
        pass(n, 8) = strcmp(ME.identifier, ...
            'CHEBFUN:CHEBTECH:mrdivide:chebtechDivChebtech');
    end

    % Can't call mrdivide on a chebtech and a non-chebtech or non-double
    % object.
    try
        f = testclass.make(@(x) sin(x));
        g = f / true;
        pass(n, 9) = false;
    catch ME
        pass(n, 9) = strcmp(ME.identifier, 'CHEBFUN:CHEBTECH:mrdivide:badArg');
    end
end

end


function out = feval(f, x, y)
%FEVAL   Evaluate a SEPARABLEAPPROX at one or more points.
%   FEVAL(F, X, Y) evaluates the SEPARABLEAPPROX F and the point(s) in (X, Y),
%   where X and Y are doubles.
%
%   FEVAL(F, X) evaluates the SEPARABLEAPPROX F at the point(s) in X, where X
%   is a double, interpreted as F(real(X), imag(X)).
%
% See also SUBSREF.

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

% Empty check:
if ( isempty(f) )
    out = [];
    return
end

% Get the low rank representation for f.
[cols, D, rows] = cdr(f);

if ( strcmpi(x, ':') && strcmpi(y, ':') )    % f(:, :)
    % Simply return the SEPARABLEAPPROX:
    out = f;

elseif ( strcmpi(x, ':') && isnumeric( y ) ) % f(:, y)
    % Make evaluation points a vector.
    y = y(:);
    % Evaluate (returns a column chebfun):
    out = feval( cols, y ) * D * rows.';
    % Simplify:
    out = simplify( out, [], 'globaltol' );

elseif ( isnumeric( x ) && strcmpi(y, ':') ) % f(x, :)
    % Make evaluation points a vector.
    x = x( : );
    % Evaluate (returns a row chebfun):
    out = cols * D * feval( rows, x ).';
    % Simplify:
    out = simplify( out, [], 'globaltol' );

elseif ( isnumeric(x) && isnumeric(y) )  % f(x, y)
    if ( ndims(x) >= 3 && isequal(size(x), size(y)) )
        % x and y are tensors. Call from CHEBFUN3.
        sizeX = size(x);
        x = x(:);
        y = y(:);
        out = feval(f, x, y);
        %% RESHAPE FOR OUTPUT:
        out = reshape(out, sizeX);
        return
    end

    takeTranspose = 0;

    % If the evaluation points are derived from meshgrid, then there is a
    % fast way to evaluate a separableApprox. Check for this property.
    if ( min(size(x)) > 1 && all(size(x) == size(y)) && numel(size(x)) == 2 )
        % Check to see if the input is a meshgrid:
        if ( max(max(abs(bsxfun(@minus, x, x(1,:))))) <= 10*eps  && ...
                max(max(abs(bsxfun(@minus, y, y(:,1))))) <= 10*eps )
            % This allows someone to do:
            % [xx,yy] = meshgrid(linspace(-1,1));
            % f(xx,yy)

            x = x(1,:);
            y = y(:,1);

        elseif ( max(max(abs(bsxfun(@minus, y, y(1,:))))) <= 10*eps && ...
                max(max(abs(bsxfun(@minus, x, x(:,1))))) <= 10*eps )
            % This allows someone to do:
            % [yy,xx] = meshgrid(linspace(-1,1));
            % f(xx,yy)

            x = x(:,1);
            y = y(1,:);
            takeTranspose = 1;
        else
            % Evaluate at matrices, but they're not from meshgrid:
            [m,n] = size( x );
            out = zeros( m, n );
            % Unroll the loop that is the longest
            if m > n
                for ii = 1:n
                    out(:,ii) = dot(feval(cols, ...
                        y(:,ii))*D,feval(rows, x(:,ii)),2);
                end
            else
                for jj = 1:m
                    out(jj,:) = dot(feval(cols, ...
                        y(jj,:).')*D,feval(rows, x(jj,:).'),2);
                end
            end
            return
        end
    else
    end

    % Evaluate:
    if ( isvector(x) && ~isscalar(x) && all(size(x) == size(y)) )
        % Determine whether inputs are pure vectors.
        out = feval(cols, y(:)) .* feval(rows, x(:)) * diag(D);
        if ( size(x,1) == 1 )
            takeTranspose = 1;
        end
    else
        out = feval(cols, y(:)) * D * feval(rows, x(:)).';
    end

    % Take transpose:
    if ( takeTranspose )
        out = transpose( out );
    end

else
    error('CHEBFUN:SEPARABLEAPPROX:feval:inputs', ...
        'Unrecognized arguments for evaluation.');

end

end


Network ResNeXt50 {
	Layer CONV1 {
		Type: CONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 64, C: 3, R: 1, S: 1, Y:224, X:224 }
	}

	Layer CONV2_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 128, C: 64, R: 1, S: 1, Y: 56, X: 56 }
	}

	Layer CONV2_1_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 4, C: 128, R: 3, S: 3, Y: 56, X: 56 }
	}

	Layer CONV2_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 256, C: 128, R: 1, S: 1, Y: 56, X: 56 }
	}

	Layer CONV2_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 128, C: 256, R: 1, S: 1, Y: 56, X: 56 }
	}

	Layer CONV2_2_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 4, C: 128, R: 3, S: 3, Y: 56, X: 56 }
	}

	Layer CONV2_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 256, C: 128, R: 1, S: 1, Y: 56, X: 56 }
	}

	Layer CONV2_3_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 128, C: 256, R: 1, S: 1, Y: 56, X: 56 }
	}

	Layer CONV2_3_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 4, C: 128, R: 3, S: 3, Y: 56, X: 56 }
	}

	Layer CONV2_3_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 256, C: 128, R: 1, S: 1, Y: 56, X: 56 }
	}

	Layer CONV3_1_1 {
		Type: CONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 256, C: 256, R: 1, S: 1, Y: 56, X: 56 }
	}

	Layer CONV3_1_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 8, C: 256, R: 3, S: 3, Y: 28, X: 28 }
	}

	Layer CONV3_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 256, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV3_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 256, C: 256, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV3_2_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 8, C: 256, R: 3, S: 3, Y: 28, X: 28 }
	}

	Layer CONV3_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 256, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV3_3_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 256, C: 256, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV3_3_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 8, C: 256, R: 3, S: 3, Y: 28, X: 28 }
	}

	Layer CONV3_3_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 256, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV3_4_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 256, C: 256, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV3_4_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 8, C: 256, R: 3, S: 3, Y: 28, X: 28 }
	}

	Layer CONV3_4_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 256, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV4_1_1 {
		Type: CONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 512, C: 512, R: 1, S: 1, Y: 28, X: 28 }
	}

	Layer CONV4_1_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 16, C: 512, R: 3, S: 3, Y: 14, X: 14 }
	}

	Layer CONV4_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 512, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 1024, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_2_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 16, C: 512, R: 3, S: 3, Y: 14, X: 14 }
	}

	Layer CONV4_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 512, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_3_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 1024, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_3_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 16, C: 512, R: 3, S: 3, Y: 14, X: 14 }
	}

	Layer CONV4_3_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 512, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_4_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 1024, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_4_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 16, C: 512, R: 3, S: 3, Y: 14, X: 14 }
	}

	Layer CONV4_4_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 512, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_5_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 1024, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_5_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 16, C: 512, R: 3, S: 3, Y: 14, X: 14 }
	}

	Layer CONV4_5_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 512, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_6_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 512, C: 1024, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV4_6_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 16, C: 512, R: 3, S: 3, Y: 14, X: 14 }
	}

	Layer CONV4_6_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 512, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV5_1_1 {
		Type: CONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 1024, C: 1024, R: 1, S: 1, Y: 14, X: 14 }
	}

	Layer CONV5_1_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 32, C: 1024, R: 3, S: 3, Y: 7, X: 7 }
	}

	Layer CONV5_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 2048, C: 1024, R: 1, S: 1, Y: 7, X: 7 }
	}

	Layer CONV5_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 2048, R: 1, S: 1, Y: 7, X: 7 }
	}

	Layer CONV5_2_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 32, C: 1024, R: 3, S: 3, Y: 7, X: 7 }
	}

	Layer CONV5_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 2048, C: 1024, R: 1, S: 1, Y: 7, X: 7 }
	}

	Layer CONV5_3_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1024, C: 2048, R: 1, S: 1, Y: 7, X: 7 }
	}

	Layer CONV5_3_2 {
		Type: NGCONV
		Stride { X: 1, Y: 1 }
		Dimensions { G: 32, K: 32, C: 1024, R: 3, S: 3, Y: 7, X: 7 }
	}

	Layer CONV5_3_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 2048, C: 1024, R: 1, S: 1, Y: 7, X: 7 }
	}

	Layer FC1000 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1000, C: 2048, R: 7, S: 7, Y: 7, X: 7 }
	}

}


function [resid, resid_norm, Wxk,Hkt,centers,Vxtpix, Vxtpixbg] = computeresid(res,dpixc,peval)
% [resid, resid_norm, Wxk,Hkt,centers,Vxtpix, Vxtpixbg] =
% computeresid(res,dpixc,peval)
[Wxk,Hkt,centers,Vxtpix]=reshapeGaP(res.hvec,res.cxcy,peval);
Vxtpixbg=reshape(Wxk*Hkt,peval.nx,peval.ny,peval.nt)+peval.bg;
resid=(Vxtpixbg-dpixc);
resid_norm=resid./sqrt(Vxtpixbg);


function [  ] = test2( vis, compressIndex, totalNum )
%TEST2 Summary of this function goes here
%   Detailed explanation goes here

    row = zeros(1, totalNum);
    for i=1:length(vis)
        row(1, vis(i)+1) = 1;
    end

    column = zeros(totalNum, 1);
    for i=1:length(compressIndex)
        column(compressIndex(i),1) = 1;
    end

    b = row*column
end


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

%STOMP PARAMETERS
%number of particles
K = 50;

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

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

fprintf('TIC TOC mean: %g\n', mean(ela));
fprintf('TIC TOC cov: %g\n', cov(ela));
fprintf('TIC TOC 2sigma: %g\n', 2*sqrt(cov(ela)));

'ended'
parameters.experiment_name


function clf = defineClf(obj, params, symbolic_state)
    v = symbolic_state(2);
    vd = params.vd; % desired velocity.

    clf = (v - vd)^2;
end