function xs = searchSeries(x,gpstruct,LB,UB,optimState,options)
%SEARCHSERIES Search step by time series prediction (UNSUPPORTED).

if nargin < 1
    xs = 'srs';
    return;
end

MeshSize = optimState.meshsize;
SearchFactor = optimState.searchfactor;

xsuccess = bsxfun(@rdivide,optimState.xsuccess,optimState.scale);
fsuccess = optimState.fsuccess;

D = size(x,2);
t = size(xsuccess,1);

% Random rotation matrix
[R,~] = qr(randn(D));
R = eye(D);

fmu = zeros(1,D);
fs = zeros(1,D);

xsuccess = xsuccess*R';

for i = 1:D
    index = max(1,t-10):t;

    stdy = sqrt(std(xsuccess(index,i)).^2 + optimState.TolMesh.^2);
    gptemp = gpinit(stdy,options,optimState);
    gptemp.x = (index)';
    % gptemp.x = -fsuccess(index);
    gptemp.y = xsuccess(index,i);
    gptemp.hyp(1).lik = log(MeshSize);
    gptemp.prior.lik{1}{2} = gptemp.hyp(1).lik;

    if numel(index) < 3 || 1
        lastindex = 1:numel(index);
    else
        lastindex = ceil(numel(index)/2):numel(index);
    end
    if numel(gptemp.hyp(1).mean) == 1
        gptemp.hyp(1).mean = mean(gptemp.y(lastindex));
        gptemp.prior.mean{1}{2} = gptemp.hyp(1).mean;
    else
        gptemp.hyp(1).mean = polyfit(gptemp.x(lastindex),gptemp.y(lastindex),1)';
        gptemp.prior.mean{1}{2} = gptemp.hyp(1).mean(1);
        gptemp.prior.mean{2}{2} = gptemp.hyp(1).mean(2);
    end

    bounds = unwrap2vec(gptemp.bounds);
    lb = bounds(1:2:end-1);
    ub = bounds(2:2:end);

    prior = @(theta_) independent_prior(gptemp.prior,theta_);
    gptemp.inf = {@inference_with_prior, @exact_inference, prior};

    % hyp0(1) = gptemp.hyp;
    %s = min(max(unwrap2vec(independent_prior(gptemp.prior)),lb),ub);
    %hyp0(2) = rewrap(gptemp.hyp,s);

    % Quick-and-dirty grid optimization
    % ell = 0:0.1:2;
    ell = 1;
    sf = log(stdy) + (-2:0.5:2);
    theta = combvec(ell,sf)';
    nlZ = zeros(1,size(theta,1));
    for j = 1:size(theta,1)
        nlZ(j) = gp_optimizer(theta(j,:), gptemp.hyp, gptemp.inf, gptemp.mean, gptemp.cov, gptemp.lik, gptemp.x, gptemp.y);
    end
    [~,index] = min(nlZ);
    gptemp.hyp.cov = theta(index,:)';

    cov(i,:) = gptemp.hyp.cov(:)';

    % optoptions = optimset('TolFun',0.01,'TolX',1e-4,'MaxFunEval',100);
    % optoptions.Hessian = 'user-supplied';
    % gptemp.hyp = gpHyperOptimize(hyp0,gptemp,optoptions);

%   [~,~,fmu(i),fs2] = gpgrad(0,gptemp);
    dt = exp(SearchFactor-1);
    [~,~,fmu(i),fs2] = gpgrad(t+dt,gptemp);
    fs(i) = sqrt(fs2);
end

% cov

fs = sqrt(fs.^2 + (MeshSize*options.PollMeshMultiplier^(-options.SearchGridNumber)).^2);

xs = bsxfun(@plus,bsxfun(@times,randn(options.Nsearch,D),fs),fmu);

xs = xs*R;

if 1
    figure(5);
    hold off;
    %plot(-fsuccess,xsuccess(:,1:end-1));
    plot(1:t,xsuccess*R);
    hold on;
    scatter((t+dt)*ones(size(fmu)),fmu);
    %scatter(0*ones(size(fmu)),fmu);
    drawnow;
    set(gcf,'Color','w');
    set(gca,'TickDir','out');
end

end

%--------------------------------------------------------------------------
function gptemp = gpinit(stdy,options,optimState)
%BGA_GPINIT

TolFun = options.TolFun;
TolMesh = optimState.TolMesh;

%% gp covariance function

% Base covariance function properties: smoothness and length scales
% gptemp.cov = {@isotropic_matern_covariance, 5};
gptemp.cov = {@isotropic_sqdexp_covariance};
gptemp.covbase = gptemp.cov;

ncov = str2double(feval(gptemp.cov{1}));
gptemp.hyp.cov = zeros(ncov,1);

% Covariance priors
for i = 1:ncov-1
     gptemp.prior.cov{i} = {@gaussian_prior, 1, 2^2};
     gptemp.bounds.cov{i} = [-5;Inf];
end
gptemp.hyp.cov = [log(3);log(stdy)];
gptemp.prior.cov{ncov} = {@gaussian_prior, log(stdy), 2^2};
gptemp.bounds.cov{ncov} = [log(TolFun);Inf];

if 0
    gptemp.cov = {@covSum,{gptemp.covbase,{@covPERiso,{@covMaterniso,Smoothness*2+1}}}};
    gptemp.hyp.cov = [gptemp.hyp.cov;zeros(ncov+1,1)];
    for i = ncov+(1:ncov)
         gptemp.prior.cov{i} = {@gaussian_prior,-1,2^2};
         gptemp.bounds.cov{i} = [-5;Inf];
    end
    gptemp.prior.cov{ncov*2+1} = {@gaussian_prior,0,2^2};
    gptemp.bounds.cov{ncov*2+1} = [log(TolFun);Inf];
elseif 0
    gptemp.cov = {@covariance_product,{gptemp.covbase,@NNone_covariance}};
    gptemp.hyp.cov = [gptemp.hyp.cov;zeros(ncov,1)];
    for i = ncov+(1:ncov-1)
         gptemp.prior.cov{i} = {@gaussian_prior,-1,2^2};
         gptemp.bounds.cov{i} = [-5;Inf];
    end
    gptemp.prior.cov{ncov*2} = {@gaussian_prior,0,2^2};
    gptemp.bounds.cov{ncov*2} = [log(TolFun);Inf];
end

%% gp likelihood function

gptemp.lik = @likGauss;   % Gaussian likelihood
likmu = log(TolMesh); liks2 = 1^2;
gptemp.hyp.lik = likmu;
gptemp.prior.lik = {{@delta_prior, likmu}};
% gptemp.prior.lik = {{@gaussian_prior, likmu, liks2}};

gptemp.hyp.lik = gptemp.hyp.lik(:);
gptemp.bounds.lik = {[log(TolMesh); 10]};

%% gp mean function
if 1
    gptemp.mean = @constant_mean;             % Constant mean function
    gptemp.hyp.mean = NaN;
    gptemp.prior.mean = {{@delta_prior, 0}};      % Fixed mean
else
    gptemp.mean = {@meanSum,{@meanLinear,@meanConst}};
    gptemp.hyp.mean = NaN(2,1);
    gptemp.prior.mean{1} = {@delta_prior, 0};      % Fixed prior
    gptemp.prior.mean{2} = {@delta_prior, 0};
end

%% gp sampling weight
gptemp.hypweight = 1;
gptemp.hypmean = [];

%% gp inference method
gpinf = @inference_with_prior;              % Exact inference
gptemp.infMethod = 'exact';

prior = @(theta_) independent_prior(gptemp.prior,theta_);
gptemp.inf = {@inference_with_prior, @exact_inference, prior};

% gptemp.inf = {@infPrior,gpinf,gptemp.prior};

gptemp = gpstruct_check(gptemp, 1);

end

%--------------------------------------------------------------------------
function nlZ = gp_optimizer(theta, hyp, inf, mean, cov, lik, x, y)
%GP_OPTIMIZER Wrapper function for GP optimization

hyp.cov = theta(:);
f = @() (feval(inf{:}, hyp, mean, cov, lik, x, y));
[~, nlZ] = f();

end


function symb_pvec = sdisplay2(p,names)

% TODO: sdpvar a b
%       x = (a+b)^0.3        -- writes "mpower_internal"
%
% TODO: sdpvar h x k
%       sdisplay2(h*x - k)   -- misses the minus in front of "k"

if nargin < 2
    LinearVariables = 1:yalmip('nvars');
    x = recover(LinearVariables);
    names = cell(length(x),1);

    W = evalin('caller','whos');
    for i = 1:size(W,1)
        if strcmp(W(i).class,'sdpvar') | strcmp(W(i).class,'ncvar')
            % Get the SDPVAR variable
            thevars = evalin('caller',W(i).name);

            % Distinguish 4 cases
            % 1: Sclalar varible x
            % 2: Vector variable x(i)
            % 3: Matrix variable x(i,j)
            % 4: Variable not really defined
            if is(thevars,'scalar') & is(thevars,'linear') & length(getvariables(thevars))==1 & isequal(getbase(thevars),[0 1])
                index_in_p = find(ismember(LinearVariables,getvariables(thevars)));

                if ~isempty(index_in_p)
                    already = ~isempty(names{index_in_p});
                    if already
                        already = ~strfind(names{index_in_p},'internal');
                        if isempty(already)
                            already = 0;
                        end
                    end
                else
                    already = 0;
                end

                if ~isempty(index_in_p) & ~already
                    % Case 1
                    names{index_in_p}=W(i).name;
                end
            elseif is(thevars,'lpcone')

                if size(thevars,1)==size(thevars,2)
                    % Case 2
                    vars = getvariables(thevars);
                    indicies = find(ismember(vars,LinearVariables));
                    for ii = indicies
                        index_in_p = find(ismember(LinearVariables,vars(ii)));

                        if ~isempty(index_in_p)
                            already = ~isempty(names{index_in_p});
                            if already
                                already = ~strfind(names{index_in_p},'internal');
                                if isempty(already)
                                    already = 0;
                                end
                            end
                        else
                            already = 0;
                        end

                        if ~isempty(index_in_p) & ~already
                            B = reshape(getbasematrix(thevars,vars(ii)),size(thevars,1),size(thevars,2));
                            [ix,jx,kx] = find(B);
                            ix=ix(1);
                            jx=jx(1);
                            names{index_in_p}=[W(i).name '(' num2str(ix) ',' num2str(jx) ')'];
                        end
                    end

                else
                    % Case 3
                    vars = getvariables(thevars);
                    indicies = find(ismember(vars,LinearVariables));
                    for ii = indicies
                        index_in_p = find(ismember(LinearVariables,vars(ii)));

                        if ~isempty(index_in_p)
                            already = ~isempty(names{index_in_p});
                            if already
                                already = ~strfind(names{index_in_p},'internal');
                                if isempty(already)
                                    already = 0;
                                end
                            end
                        else
                            already = 0;
                        end

                        if ~isempty(index_in_p) & ~already
                            names{index_in_p}=[W(i).name '(' num2str(ii) ')'];
                        end
                    end
                end

            elseif is(thevars,'sdpcone')
                % Case 3
                vars = getvariables(thevars);
                indicies = find(ismember(vars,LinearVariables));
                for ii = indicies
                    index_in_p = find(ismember(LinearVariables,vars(ii)));
                    if ~isempty(index_in_p)
                        already = ~isempty(names{index_in_p});
                        if already
                            already = ~strfind(names{index_in_p},'internal');
                        end
                    else
                        already = 0;
                    end

                    if ~isempty(index_in_p) & ~already
                        B = reshape(getbasematrix(thevars,vars(ii)),size(thevars,1),size(thevars,2));
                        [ix,jx,kx] = find(B);
                        ix=ix(1);
                        jx=jx(1);
                        names{index_in_p}=[W(i).name '(' num2str(ix) ',' num2str(jx) ')'];
                    end
                end

            else
                % Case 4
                vars = getvariables(thevars);
                indicies = find(ismember(vars,LinearVariables));

                for i = indicies
                    index_in_p = find(ismember(LinearVariables,vars(i)));
                    if ~isempty(index_in_p) & isempty(names{index_in_p})
                        names{index_in_p}=['internal(' num2str(vars(i)) ')'];
                    end
                end

            end
        end
    end
end

[mt,vt] = yalmip('monomtable');
ev = yalmip('extvariables');

for i = 1:size(p, 1)
    for j = 1:size(p, 2)
        symb_pvec{i, j} = symbolicdisplay(p(i, j), names, vt, ev, mt);
    end
end

%------------------------------------------------------------------------
function expression = symbolicdisplay(p,names,vt,ev,mt)

sp = size(p);
if any(sp > 1)
    out = '[';
else
    out = '';
end
p_orig = p;
for i1 = 1:sp(1)
    for i2 = 1:sp(2)
        p = p_orig(i1, i2);
        basis = getbase(p);
        if basis(1)~=0
            expression = [num2str(basis(1)) '+'];
        else
            expression = [''];
        end

        [dummy, variables, coeffs] = find(basis(2:end));
        variables = getvariables(p);
        for i = 1:length(coeffs)
            if coeffs(i)==1
                expression = [expression symbolicmonomial(variables(i), ...
                    names,vt,ev,mt) '+'];
            else
                expression = [expression num2str(coeffs(i)) '*' ...
                    symbolicmonomial(variables(i),names,vt,ev,mt) '+'];
            end
        end
        expression(end) = [];
        out = [out expression ','];
    end
    out(end) = ';';
end
out(end) = [];
if any(sp > 1)
    out = [out ']'];
end
expression = out;

%------------------------------------------------------------------------
function s = symbolicmonomial(variable,names,vt,ev,mt)

terms = find(mt(variable,:));
if ismember(variable,ev)
    q = yalmip('extstruct',variable);
    s = [q.fcn '(' symbolicdisplay(q.arg{1},names,vt,ev,mt)];
    for i = 2:length(q.arg)-1
        s = [s ',' symbolicdisplay(q.arg{i}, names, vt, ev, mt)];
    end
    s = [s ')'];

elseif ~vt(variable)
    % Linear expression
    s = names{variable};
else
    % Fancy display of a monomial
    s = [''];
    for i = 1:length(terms)
        if mt(variable,terms(i)) == 1
            exponent = '';
        else
            exponent = ['^' num2str(mt(variable,terms(i)))];
        end
        s = [s symbolicmonomial(terms(i),names,vt,ev,mt) exponent '*'];
    end
    s(end)=[];
end
% s = strrep(s,'^1+','+');
% s = strrep(s,'^1*','*');


%%  min cost of each iteration

iter = [1;2;3;4;5];
min_cost_2000 = [min_cost_0_2000; min_cost_1_2000; min_cost_2_2000; min_cost_3_2000; min_cost_4_2000];
min_cost_3000 = [min_cost_0_3000; min_cost_1_3000; min_cost_2_3000; min_cost_3_3000; min_cost_4_3000];
min_cost_1000 = [min_cost_0_1000; min_cost_1_1000; min_cost_2_1000; min_cost_3_1000; min_cost_4_1000];
min_cost_4000 = [min_cost_0_4000; min_cost_1_4000; min_cost_2_4000; min_cost_3_4000; min_cost_4_4000];

ff = figure;
plot(iter, min_cost_4000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'm'); hold on;
plot(iter, min_cost_3000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'b'); hold on;
plot(iter, min_cost_2000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'r'); hold on;
plot(iter, min_cost_1000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'k'); hold on;

grid on;
l=legend('b=4000', 'b=3000',  'b=2000', 'b=1000');
title(l, 'Cost after Progm');
xlabel('Iteration', 'FontWeight', 'bold', 'FontSize', 14);
ylabel('Cost', 'FontWeight', 'bold', 'FontSize', 14);
set(ff, 'Renderer', 'painters');
set(gca,'Xtick', 1:1:5);
set(gca,'FontSize',11);
set(gca,'Xticklabel',{'Iter 1','Iter 2', 'Iter 3', 'Iter 4', 'Iter 5'})

%% totalNum and Kept ratio

iter = [1;2;3;4;5];
totalNum_4000 = [totalNum_0_4000; totalNum_1_4000; totalNum_2_4000; totalNum_3_4000; totalNum_4_4000 ];
totalNum_3000 = [totalNum_0_3000; totalNum_1_3000; totalNum_2_3000; totalNum_3_3000; totalNum_4_3000];
totalNum_2000 = [totalNum_0_2000; totalNum_1_2000; totalNum_2_2000; totalNum_3_2000; totalNum_4_2000 ];
totalNum_1000 = [totalNum_0_1000; totalNum_1_1000; totalNum_2_1000; totalNum_3_1000; totalNum_4_1000 ];

% ff = figure;
% plot(iter, totalNum_2000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'r'); hold on;
% plot(iter, totalNum_1000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'm'); hold on;
% plot(iter, totalNum_4000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'k'); hold on;
% plot(iter, totalNum_4000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'm'); hold on;

% grid on;
% l=legend('b=2000', 'b=3000', 'b=1000', 'b=500');
% title(l, 'Number of Points after Progm');
% xlabel('Iteration', 'FontWeight', 'bold', 'FontSize', 14);
% ylabel('Number of Points', 'FontWeight', 'bold', 'FontSize', 14);
% set(ff, 'Renderer', 'painters');
% set(gca,'Xtick', 1:1:8, 'fontsize',14);
% set(gca,'Xticklabel',{'Iter 1','Iter 2', 'Iter 3', 'Iter 4', 'Iter 5', 'Iter 6', 'Iter 7', 'Iter 8'});
% set(gca,'FontSize',12);

iter = [1;2;3;4;5];
ratio_2000 = totalNum_2000 / totalNum;
ratio_3000 = totalNum_3000 / totalNum;
ratio_1000 = totalNum_1000 / totalNum;
ratio_4000 = totalNum_4000 / totalNum;

ff = figure;
plot(iter, ratio_4000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'm'); hold on;
plot(iter, ratio_3000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'b'); hold on;
plot(iter, ratio_2000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'r'); hold on;
plot(iter, ratio_1000, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'k'); hold on;

grid on;
l=legend('b=4000', 'b=3000', 'b=2000', 'b=1000');
title(l, 'Ratio after Progm');
xlabel('Iteration', 'FontWeight', 'bold', 'FontSize', 14);
ylabel('Compression Ratio', 'FontWeight', 'bold', 'FontSize', 14);
set(ff, 'Renderer', 'painters');
set(gca,'Xtick', 1:1:5);
set(gca,'Xticklabel',{'Iter 1','Iter 2', 'Iter 3', 'Iter 4', 'Iter 5'});
set(gca,'FontSize',11);

%%  Total time cost
%
% time_sum_2000 = sum([time_sum_0, time_sum_1, time_sum_2, time_sum_3, time_sum_4, time_sum_5, time_sum_6, time_sum_7]);
% time_sum_3000 = sum([time_sum_0, time_sum_1, time_sum_2, time_sum_3, time_sum_4, time_sum_5, time_sum_6, time_sum_7]);
% time_sum_1200 = sum([time_sum_0, time_sum_1, time_sum_2, time_sum_3, time_sum_4, time_sum_5, time_sum_6, time_sum_7]);
% time_sum_1000 = sum([time_sum_0, time_sum_1, time_sum_2, time_sum_3, time_sum_4, time_sum_5, time_sum_6, time_sum_7]);
% time_sum_800 = sum([time_sum_0, time_sum_1, time_sum_2, time_sum_3, time_sum_4, time_sum_5, time_sum_6, time_sum_7]);
%
%
%
% iter = [1;2;3;4;5];
% times = [time_sum_2000; time_sum_3000; time_sum_1200; time_sum_1000; time_sum_800];
% ff = figure;
% plot(iter, times, '--bs', 'MarkerSize', 10,  'MarkerFaceColor', 'r'); hold on;
% grid on;
% l=legend('time costs');
% title(l, 'Total time under Diff b');
% xlabel('Iteration', 'FontWeight', 'bold', 'FontSize', 12);
% ylabel('Time Costs (second)', 'FontWeight', 'bold', 'FontSize', 12);
% set(ff, 'Renderer', 'painters');
% set(gca,'Xtick', 1:1:8, 'fontsize',10);
% set(gca,'Xticklabel',{'b=2000','b=3000', 'b=1200', 'b=1000', 'b=800'});


%% RECIPROCAL VO/FORMATION CONTROL AGENT (agent_formation_RVO.m) %%%%%%%%%%
% This agent is a hybrid of the formation control element agent class and
% the Reciprocal Velocity Obstacle (VO) class.

% Author: James A. Douthwaite

classdef agent_formation_RVO < agent_formation_VO & agent_RVO
    %% INITIALISE THE AGENT SPECIFIC PARAMETERS
    properties

    end
    %% ///////////////////////// MAIN METHODS /////////////////////////////
    methods
        % Constructor
        function obj = agent_formation_RVO(varargin)
            % CALL THE SUPERCLASS CONSTRUCTOR
            obj = obj@agent_formation_VO(varargin);

            % //////////////// Check for user overrides ///////////////////
            [obj] = obj.ApplyUserOverrides(varargin); % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % NOTES:
        % This class simply overrides the 'process_timeCycle' function of
        % the superclass 'agent_formation_VO' such that it considers the
        % 'avoidanceCorrection' of the second super class 'agent_RVO'.
    end
end
% AGENT STATE VECTOR [x;y;z;phi;theta;psi;xdot;ydot;zdot;phidot;thetadot;psidot]


function grdwrite2(x,y,z,file);
%GRDWRITE2  Write a GMT grid file
%
% Uses built-in NetCDF capability (MATLAB R2008b or later) to
% write a COARDS-compliant netCDF grid file
% Duplicates (some) functionality of the program grdwrite (which requires
% compilation as a mexfile-based function on each architecture) using
% Matlab 2008b (and later) built-in NetCDF functionality
% instead of GMT libraries.
%
% GRDWRITE2(X,Y,Z,'filename') will create a grid file containing the
% data in the matrix Z.  X and Y should be either vectors with
% dimensions that match the size of Z or two-component vectors
% containing the max and min values for each.
%
% See also GRDREAD2, GRDINFO2

% For more information on GMT grid file formats, see:
% http://www.soest.hawaii.edu/gmt/gmt/doc/gmt/html/GMT_Docs/node70.html
% Details on Matlab's native netCDF capabilities are at:
% http://www.mathworks.com/access/helpdesk/help/techdoc/ref/netcdf.html

% GMT (Generic Mapping Tools, <http://gmt.soest.hawaii.edu>)
% was developed by Paul Wessel and Walter H. F. Smith

% Kelsey Jordahl
% Marymount Manhattan College
% http://marymount.mmm.edu/faculty/kjordahl/software.html

% Time-stamp: <Tue Jul 19 16:28:24 EDT 2011>

% Version 1.1.2, 19-Jul-2011
% Available at MATLAB Central
% <http://www.mathworks.com/matlabcentral/fileexchange/26290-grdwrite2>

if nargin < 4,
  help(mfilename);
  return,
end

% check for appropriate Matlab version (>=7.7)
V=regexp(version,'[ \.]','split');
if (str2num(V{1})<7) | (str2num(V{1})==7 & str2num(V{2})<7),
  ver
  error('grdread2: Requires Matlab R2008b or later!');
end

ncid = netcdf.create(file, 'NC_SHARE');
if isempty(ncid),
  return,
end

% set descriptive variables
conv='COARDS/CF-1.0';
title=file;
history='File written by MATLAB function grdwrite2.m';
desc=['Created ' datestr(now)];
vers='4.x';                             % is "x" OK?

% check X and Y
if (~isvector(x) | ~isvector(y)),
  error('X and Y must be vectors!');
end
if (length(x) ~= size(z,2)),    % number of columns don't match size of x
  minx=min(x); maxx=max(x);
  dx=(maxx-minx)/(size(z,2)-1);
  x=minx:dx:maxx;                       % write as a vector
end
if (length(y) ~= size(z,1)),    % number of rows don't match size of y
  miny=min(y); maxy=max(y);
  dy=(maxy-miny)/(size(z,1)-1);
  y=miny:dy:maxy;                       % write as a vector
end

% match Matlab class to NetCDF data type
switch class(z)
 case 'single'
  nctype='NC_FLOAT';
  nanfill=single(NaN);
 case 'double'
  nctype='NC_DOUBLE';
  nanfill=double(NaN);
 case 'int8'
  nctype='NC_BYTE';
  nanfill=intmin(class(z));
  disp(['Warning: ''No data'' fill value set to ' num2str(nanfill)])
 case 'int16'
  nctype='NC_SHORT';
  nanfill=intmin(class(z));
  disp(['Warning: ''No data'' fill value set to ' num2str(nanfill)])
 case 'int32'
  nctype='NC_INT';
  nanfill=intmin(class(z));
  disp(['Warning: ''No data'' fill value set to ' num2str(nanfill)])
 otherwise
  error(['Don''t know how to handle data of class ''' class(z) '''.  Try converting to a supported data type (int8, int16, int32, single or double).'])
end

% global
netcdf.putAtt(ncid,netcdf.getConstant('NC_GLOBAL'),'Conventions',conv);
netcdf.putAtt(ncid,netcdf.getConstant('NC_GLOBAL'),'title',title);
netcdf.putAtt(ncid,netcdf.getConstant('NC_GLOBAL'),'history',history);
netcdf.putAtt(ncid,netcdf.getConstant('NC_GLOBAL'),'description',desc);
netcdf.putAtt(ncid,netcdf.getConstant('NC_GLOBAL'),'GMT_version',vers);
% X
dimid = netcdf.defDim(ncid,'x',length(x));
varid = netcdf.defVar(ncid,'x','double',dimid);
netcdf.putAtt(ncid,varid,'long_name','x');
netcdf.putAtt(ncid,varid,'actual_range',[min(x) max(x)]);
netcdf.endDef(ncid);
netcdf.putVar(ncid,varid,x);
% Y
netcdf.reDef(ncid);
dimid = netcdf.defDim(ncid,'y',length(y));
varid = netcdf.defVar(ncid,'y','double',dimid);
netcdf.putAtt(ncid,varid,'long_name','y');
netcdf.putAtt(ncid,varid,'actual_range',[min(y) max(y)]);
netcdf.endDef(ncid);
netcdf.putVar(ncid,varid,y);
% Z
netcdf.reDef(ncid);
varid = netcdf.defVar(ncid,'z',nctype,[0 1]);
netcdf.putAtt(ncid,varid,'long_name','z');
netcdf.putAtt(ncid,varid,'_FillValue',nanfill);
netcdf.putAtt(ncid,varid,'actual_range',[min(z(:)) max(z(:))]);
netcdf.endDef(ncid);
netcdf.putVar(ncid,varid,z');
% close file
netcdf.close(ncid);


function [t0, x0, u0] = shift(T, t0, x0, u,f)
% add noise to the control actions before applying it
con_cov = diag([0.005 deg2rad(2)]).^2;
con = u(1,:)' + sqrt(con_cov)*randn(2,1);
st = x0;

f_value = f(st,con);
st = st+ (T*f_value);

x0 = full(st);
t0 = t0 + T;

u0 = [u(2:size(u,1),:);u(size(u,1),:)]; % shift the control action
end


function pdvfile( compleximagefile, outfile, varargin )
%PDVFILE Phase Derivative Value
%    pdvfile(compleximagefile, outfile, 'PropertyName',PropertyValue,...)
%    calculates the phase derivative value of compleximagefile using the
%    properties specified. This version of PDV does NOT require that the
%    complete data fit into memory.  It processes from any format handled
%    by OPEN_READER and outputs to files in SIO format.
%
%       Property name     Description
%       deltax            pixel shift (default = 0.25)
%       filtersize        size of smoothing filter (default = [5 5])
%       filtertype        type of filter, 'mean' (default) or 'median'
%       dim               dimension over which to calculate phase
%                            derivative (default = 1)
%       azlimits          min and max samples in azimuth over which to
%                            compute (default = query user with GUI).  Use
%                            'full' to specify entire range.  If nothing is
%                            selected, a MATLAB GUI will be used to select
%                            an area.
%       rnglimits         min and max lines in range over which to compute
%                            (default = query user with GUI).  Use 'full'
%                            to specify entire range.  If nothing is
%                            selected, a MATLAB GUI will be used to select
%                            an area.
%       framenumber      frame to process if a multi-image file
%
% Written by: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Get image metadata
func_args=parsepdvinputs(varargin{:});
input_params=varargin;

% Read image metadata
fr=open_reader(compleximagefile);
if iscell(fr)
    metadata=fr{func_args.framenumber}.get_meta();
    for i=1:length(fr)
        fr{i}.close();
    end
else
    metadata=fr.get_meta();
    fr.close();
end

try
    % Normalize data if necessary.
    apply_normalization = ~is_normalized_sicd(metadata,func_args.dim);
    if apply_normalization
        tmp=tempname(); % Stored deskewed data in temporary file to process
        normalize_complex_file(compleximagefile,'output_filename',tmp,varargin{:});
        compleximagefile=tmp;
        input_params=[input_params {'azlimits','full','rnglimits','full'}];
    end
catch
    apply_normalization = 0;
end

% Actual PDV
func_hand=@(x) pdvmem(x, func_args);
process_by_lines( compleximagefile, outfile, func_hand,...
    'block_overlap', floor(func_args.filtersize.*[1 1]/2),...
    'function_name', 'PDV', input_params{:} );
if apply_normalization, delete(compleximagefile); end; % Remove temporary file

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function z = searchMaxAcq(data,xi)
%SEARCHMAXACQ Wrapper of acquisition function for SEARCHMAX

if isstruct(xi) && isnumeric(data) % Arguments in wrong order
    temp = data;
    data = xi;
    xi = temp;
end

z = feval(data.acq{:},xi(:)',data.target,data.gpstruct,data.optimState,0);

end


% bsb_test.m
x=[-0.5;-0.4];
beta=0.5;
c=100;bsb(x,beta,c)
text(-0.5,-0.4,'(-0.5,-0.4)')


function output = structureStats(structNum, planC)
%"structureStats"
%   Returns output, a struct containing the xyz coordinates for the
%   Center of mass(COM) of the mask of the structure, and the min
%   and max extent of the structure.
%
%   If a transformation matrix exists for a structure's associated scan,
%   the coordinates are transformed using the matrix.
%
%JRA 3/12/04
%
%Usage:
%   function output = structureStats(structNum, planC)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('planC')
    global planC;
end

indexS = planC{end};

%Get scan associated with structure.
scanSet = getStructureAssociatedScan(structNum, planC);

%Get mask of structure.
uniformStr = getUniformStr(structNum, planC);

%Get r,c,s of voxels inside uniformStr.
[r,c,s] = find3d(uniformStr);
nVoxInStruct = length(r);
clear uniformStr;

%Get scan's original x,y,z coordinates, and it's transformation matrix.
if isempty(planC{indexS.scan}(scanSet).uniformScanInfo)
    planC = setUniformizedData(planC);
end
[xV, yV, zV] = getUniformScanXYZVals(planC{indexS.scan}(scanSet));
transM = getTransM(planC{indexS.scan}(scanSet), planC);
voxVol = abs(xV(2)-xV(1)) * abs(yV(2)-yV(1)) * abs(zV(2)-zV(1));

%Get the x,y,z coords of points in the structure, and transform.
structXV = xV(c); clear c xV;
structYV = yV(r); clear r yV;
structZV = zV(s); clear s zV;
[xT, yT, zT] = applyTransM(transM, structXV, structYV, structZV);
clear structXV structYV structZV

%Find the bounding box around the structure.
minX = min(xT);
minY = min(yT);
minZ = min(zT);
maxX = max(xT);
maxY = max(yT);
maxZ = max(zT);

yCOM = mean(yT);
xCOM = mean(xT);
zCOM = mean(zT);

output.min = min([[minX, minY, minZ];[maxX, maxY, maxZ]]);
output.max = max([[minX, minY, minZ];[maxX, maxY, maxZ]]);
output.COM = [xCOM, yCOM, zCOM];
output.vol = voxVol * nVoxInStruct;
output.name = planC{indexS.structures}(structNum).structureName;


function [d,a] = training(a,d)

  a.dat=d;
  d=set_x(d,feval('get_distance',a,d));


function [m,v]=v_psycdigit(proc,r,mode,p,q,xp,noise,fn,dfile,ofile)
%V_PSYCDIGIT measures psychometric function using TIDIGITS stimuli
%
% Usage:
%         (1)[m,v]=v_psycdigit([],[],'GMPWrn10',[],[],[],[],[],dfile);
%                       % measure SRT using addditive white noise, repetitions allowed, data in .WAV format
%         (2)[m,v]=v_psycdigit(@v_specsub,[],'GMPn10',[],[],[],[],[],dfile);
%                       % compare spectral subtraction with unprocessed noisy speech
%
% Inputs:
%         proc    processing function handle (e.g. @v_specsub) called as follows:
%                    y=proc(x,fs,i,r)  % process noisy waveform x through model i with parameters r
%                    y=proc(x,fs,snr,i,r) % process clean speech degraded to snr through model i with parameters r
%                    y=proc()   % return comment text string describing algorithm (if 'c' option specified)
%         r       parameters for proc (omitted from call if r=[])
%         mode    string containing options (see below for list)
%         p,q,xp  parameters passed on to v_psycest or v_psycestu
%         noise   noise waveform or wav file containing noise
%         fn      noise waveform sample frequency [16000]
%         dfile   path to TIdigits folder
%         ofile   output text file (see below for output file format)
%
% Outputs:
%          m(2,n,3) estimated srt and slope of all models
%                   m(i,n,k): i={1=srt, 2=slope}, n=model number, k={1=mean, 2=mode (MAP), 3=marginal mode}
%          v(3,n)   estimated covariance matrix entries:
%                   [var(srt) cov(srt,slope) var(slope)]' of n'th model
%
% List of options for "mode" input:
%         a       proc adds its own noise
%         b*      base figure number for plotting results [100]
%         c       y=proc() returns comment string
%         e/E*    plot evolving psychometric functions *=1,2,3 for mean, mode, marginal mode (F=after each trial)
%         f/F*    plot psychometric functions *=1,2,3 for mean, mode, marginal mode (F=after each trial)
%         g       prompt with number of digits
%         G       prompt with SNR
%         l*      min token length (in digits)
%         L*      max token length (if different from min)
%         m*      use * external models [default 1]
%         M       include an extra model with no processing
%         n*      *=average number of trials per model
%         p/P     plot pdf (P=after each trial)
%         r       allow repetitions
%         s       respond s to save the noisy stimulus as a .wav file
%         t/T*    taper noise */10 seconds at start and end
%         v/V*    plot srt/slope convergence (V= after each trial)
%                 *=1,2,3 for mean, mode, marginal mode
%         x*      add */10 seconds of noise to the front of the speech before processing
%         X*      truncate */10 seconds from the start of the processed speech
%         W       data is in microsoft WAV format
%         z       omit tokens containing "oh"
%         Z       omit tokens containing "zero"
%
% Output file format
%   Each line starts 'x ' where x is one of the following
%       %  Comment
%       V  File version type
%       O  mode options
%       P  details about proc
%       C  Comment returned by proc
%       M  measurement

% Future mode options:
%        [ d     score as single digits ]
%        [ i/I   plot SRT improvement (I=after each trial) ]
%        [ j*    scaling: 0=autoscale each token, 1=constant speech,2=const noise, 3=const noisy ]
%        [N*     ignore the first * trials ]
%        [o/O    write to output file, O write result of each probe]
%        [ S     save all stimuli as wav files ]
%        [ u     do not normalize the noise level ]
%        [ y*    type of noise ]
%
% Bugs/Suggestions
% (1) Add sounds to indicate error and end of test
% (2) Routine could return a label to replace "SNR" in necessary
% (3) Add an input option to "discard" this sample
% (4) output file should include mode argument + date + IP address + computer name + r arguments + p/q values
% (5) Quote filenames in output file or else never have anything else on the line
% (6) silence detection doesn't work

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

%     25+4     m*      use * external models [default 1]
%     26    M       include an extra model with no processing
%      1   a       proc adds its own noise
%      27+5   n*      *=average number of trials per model
%     23+2    l*      min token length (in digits)
%      24+3   L*      max token length (if different from min)
%      7   d       score as single digits
%      37   s       speech-shaped noise instead of white
%       19+1  j*      scaling: 0=autoscale each token, 1=constant speech, 2=const noise, 3=const noisy
%      35   r       allow repetitions
%     41    u       unimodal psychometric function
%     32    P       plot pdf
%     31   p       plot srt convergence
%     13    g       prompt with number of digits
%      14   G       prompt with SNR
%      47+6   x*      add */10 seconds of noise to the front of the speech
%      48+7   X*      truncate */10 from the start of the processed speech
% input parameters
persistent tok mtok tigflgp digitsp
if nargin<10
    ofile='';
    if nargin<9
        dfile='F:\home\dmb\data\old_speech\tidigits';
        %         dfile='Y:\Speech\TIDigits\Disc 1\TIDIGITS';
        if nargin<8
            fn=16000;
            if nargin<7
                noise=[];
                if nargin<6
                    xp=[];
                    if nargin<5
                        q=[];
                        if nargin<4
                            p=[];
                            if nargin<3
                                mode='';
                                if nargin<2
                                    r=[];
                                end
                            end
                        end
                    end
                end
            end
        end
    end
end

% parse the mode string

i=1;
pv=[0 3 3 1 25  0  0 1 1 1 1 100]; % pv default values if option is unspecified
px=[0 3 3 1 25 20 18 1 1 1 1 100]; % pv values if option is given without a value
pvs='jlLmnxXvVfFb';
pf=zeros(1,52);
mval=0;
lmode=length(mode);
while i<=lmode
    if i<lmode  % read a following integer if it exists
        [v,nv,e,ni]=sscanf(mode(i+1:end),'%d',1);
    else
        nv=0;
        ni=1;
    end
    j=find(mode(i)==pvs,1);
    k=1+2*(double(lower(mode(i)))-'a')+(mode(i)<'a');
    if k>=1 && k<=52
        pf(k)=1-pf(k);
        if ~isempty(j)
            if nv==0
                pv(j)=px(j);
            else
                pv(j)=v;
                mval=mval || j==4;  % m has a value specified
            end
        end
    end
    i=i+ni;
end
if isempty(proc)
    pv(4)=0; % not allowed any processed models if not process is specified
end
% derived input parameters

varthr=20;   % variance threshold for "silence"
nxevo=30; % size of evloving pdf

if pf(23)>pf(24) || pv(2)>pv(3)
    pv(3)=pv(2);                % Make max token length reasonable
end
if pf(24)>pf(23)
    pv(2)=1;                    % min =1 if only max is specified
end
zmodel=pv(4)+(pf(26)>0); % total number of models (including null model)
ntrial=zmodel*pv(5);  % total number of trials
if pf(12)>pf(11)    % 'F' specified but not 'f'
    pv(10)=pv(11);  % copy across the average type selector
end
if pf(11)+pf(12)>0 && (pv(10)<1 || pv(10)>3)
    error('Invalid average type for option f/F');
end

% sort out p argument to v_psycest or v_psycestu
unimode=pf(41);
if isempty(p)
    p=0.75;     % default recognition rate at threshold
end
if size(p,1)<3-unimode
    p(2-unimode,1)=0.04;     % miss probability
    p(3-unimode,1)=0;     % dummy entry for guess probability
end
if size(p,2)<(zmodel)
    p=p(:,1+mod(0:zmodel-1,size(p,2)));
end

% first sort out the digit samples

tigflg=[pv(2:3) pf(51:52)];
if isempty(tok) || any(tigflg~=tigflgp) || ~strcmp(dfile,digitsp)
    disp('Scanning TIDIGITS files ...');
    digitsp=dfile;
    tigflgp=tigflg;
    if any(dfile(end)=='/\')
        dfile(end)=[];  % remove a trailing separator
    end
    dirlist{1}='';
    ntok=0;
    mtok=zeros(1,pv(3));
    tok=cell(1,2);
    while ~isempty(dirlist)
        dd=dir([dfile dirlist{1}]);
        for i=1:length(dd)
            name=dd(i).name;
            if name(1)~='.'   % ignore directories starting with '.'
                if dd(i).isdir
                    dirlist{end+1}=[dirlist{1} '\' name];
                elseif length(name)>4 && strcmpi(name(end-3:end),'.wav')
                    digs=name(1:end-4);
                    digz=upper(digs)=='Z';
                    digo=upper(digs)=='O';
                    digs(digo | digz)='0';
                    digs=digs(digs>='0' & digs<='9');
                    ndigs=length(digs);
                    if ndigs>=tigflg(1) && ndigs<=tigflg(2) && ~(tigflg(3) && any(digo)) && ~(tigflg(4) && any(digz))
                        ntok=ntok+1;
                        mtok(ndigs)=mtok(ndigs)+1;
                        tok{ntok,1}=[dirlist{1} '\' name];
                        tok{ntok,2}=digs;
                    end
                end
            end
        end
        dirlist(1)=[];  % remove this directory from the list
    end
end
ntok=size(tok,1);
if pf(46)
    [s,fs]=v_readwav([dfile tok{1,1}]); % get the first speech token to set fs
else
    [s,fs]=v_readsph([dfile tok{1,1}]); % get the first speech token to set fs
end

% calculate guess probability assuming you get the correct number of digits

if any(mode=='d')
    p(3-unimode,:)=0.1;
else
    p(3-unimode,:)=0.1.^(1:pv(3))*mtok'/ntok;
end

% now initialize the models

if unimode
    [xx,ii,m,v]=v_psycestu(-zmodel,p,q,xp); % initialize all models
    [pact,qact]=v_psycestu(0);  % save the actual parameters
else
    [xx,ii,m,v]=v_psycest(-zmodel,p,q,xp); % initialize all models
    [pact,qact]=v_psycest(0);  % save the actual parameters
end
if pv(4) && pf(5)
    x=[];  % set x=[] to force a description output
    ii=1;   % for now, only do model 1
    if pf(1)   % if process adds its own noise
        if isempty(r)  % proc does not require any auxilliary parameters
            if mval
                procdesc=proc(x,fs,xx,ii);  % process the noisy speech
            else
                procdesc=proc(x,fs,xx);  % process the noisy speech
            end
        else
            if mval
                procdesc=proc(x,fs,xx,ii,r);  % process the noisy speech
            else
                procdesc=proc(x,fs,xx,r);  % process the noisy speech
            end
        end
    else   % if process does not add its own noise
        if isempty(r)  % proc does not require any auxilliary parameters
            if mval
                procdesc=proc(x,fs,ii);  % process the noisy speech
            else
                procdesc=proc(x,fs);  % process the noisy speech
            end
        else
            if mval
                procdesc=proc(x,fs,ii,r);  % process the noisy speech
            else
                procdesc=proc(x,fs,r);  % process the noisy speech
            end
        end
    end
else
    procdesc='';
end

% now initialize the output file

if pf(29) || pf(30)   % o/O output info
    nw=fix(datevec(now));
    of=sprintf('psy%4d%02d%02d%02d%02d.txt',nw(1:5)); % filename includes date and time to nearest minute
    ofid=fopen(of,'wt');
    if ~ofid
        error('Cannot write to %s',of);
    end
    fprintf(ofid,'%% %s evaluation on %s\n',mfilename,datestr(now));
    fmfnm=[mfilename('fullpath') '.m'];
    dd=dir(fmfnm);
    fprintf(ofid,'%% %s = %d bytes = %s\n',fmfnm,dd.bytes,dd.date);
    fprintf(ofid,'V %d\n',2); % print file format version number
    fmfnm=which(func2str(proc));
    dd=dir(fmfnm);
    fprintf(ofid,'P %s = %d bytes = %s\n',fmfnm,dd.bytes,dd.date);
    if numel(procdesc)
        fprintf(ofid,'C %s\n',procdesc);
    end
    fprintf(ofid,'O %s\n',mode);
end

% now start testing

disp(['Testing ' procdesc]);
% now do the main loop
mnt=zeros(2+2*unimode,zmodel,3,ntrial+1);
vnt=zeros(3+unimode,zmodel,ntrial+1);
mnt(:,:,:,1)=m;
vnt(:,:,1)=v;
i=0;
imax=0;
quitit=0;
while ~quitit
    i=i+1;
    isp=min(1+floor(rand(1)*ntok),ntok); % select a token
    if pf(46)
        [s,fs]=v_readwav([dfile tok{isp,1}]); % get the speech token
    else
        [s,fs]=v_readsph([dfile tok{isp,1}]); % get the speech token
    end
    s=[zeros(pv(6)*round(fs/10),1); v_activlev(s(:),fs,'n')]; % preappend zeros and normalize speech level
    if pf(1) && ii<=pv(4)  % if process adds its own noise
        if isempty(r)
            if mval
                y=proc(s,fs,xx,ii);  % process the noisy speech
            else
                y=proc(s,fs,xx);  % process the noisy speech
            end
        else
            if mval
                y=proc(s,fs,xx,ii,r);  % process the noisy speech
            else
                y=proc(s,fs,xx,r);  % process the noisy speech
            end
        end
    else
        nn=randn(length(s),1);
        x=nn+s*10^(xx/20);   % create the data
        if ii<=pv(4)
            if isempty(r)
                if mval
                    y=proc(x,fs,ii);  % process the noisy speech
                else
                    y=proc(x,fs);  % process the noisy speech
                end
            else
                if mval
                    y=proc(x,fs,ii,r);  % process the noisy speech
                else
                    y=proc(x,fs,r);  % process the noisy speech
                end
            end
        else
            y=x;            % unprocessed for last model
        end
    end
    y=y(1+pv(7)*round(fs/10):end);   % remove junk from the start
    if pf(13)
        prg=sprintf(' %d',length(tok{isp,2}));
    else
        prg='';
    end
    if pf(14)
        prG=sprintf('SNR=%d dB, ',round(xx));
    else
        prG='';
    end
    if pf(35)
        prr=sprintf(', ''r'' to repeat');
    else
        prr='';
    end
    prompt=[prG 'enter' prg ' digits (''q'' to quit' prr '): '];
    %     ansr=-(var(y)>varthr);  % meant to detect silences but doesn't work
    ansr=-1;
    say=1;
    while ansr<0
        if say
            tdel=0;
            tic;
            soundsc(y,fs);      % *** probably shouldn't be ...sc
            say=0;
        end
        rv=input(prompt,'s');
        tdel=toc;
        if ~isempty(rv)
            if lower(rv(1))=='q'
                quitit=1;
                ansr=2;
            elseif lower(rv(1))=='r' && pf(35)
                say=1;
            elseif lower(rv(1))=='s' && pf(37)   % save the token
                ofn=input('File name: ','s');
                if numel(ofn)
                    v_writewav(y,fs,ofn);
                end
            elseif all(rv>='0') && all(rv<='9') && ( ~pf(13) || length(rv)==length(tok{isp,2}))
                ansr=strcmp(rv,tok{isp,2});
            end
        end
    end
    quitit=quitit || i==ntrial;   % mark as quiting if we have done all the trials
    jj=ii;  % remember which model has just been updated
    xxold=xx; % and what the SNR was
    if ansr<2  % valid answer: update the pdfs
        if unimode
            [xx,ii,m,v]=v_psycestu(ii,xx,ansr);
        else
            [xx,ii,m,v]=v_psycest(ii,xx,ansr);
        end
        mnt(:,:,:,i+1)=m;
        vnt(:,:,i+1)=v;
        imax=i;
    end
    if pf(30) || quitit && pf(29)       % 'O/o': output
        if ansr>1
            rv=num2str(double(rv(1)));
        end
        % could add in token name and length
        fprintf(ofid,'M %d %d %.3g %s %s %d %.1f',i,ii,xxold,tok{isp,2},rv,ansr,tdel);
        fprintf(ofid,' %.3g',m(:,jj,:),v(:,jj));
        fprintf(ofid,'\n');
    end
    if pf(32)                           % 'P': plot PDF: figures 1:m
        figure(jj);
        if unimode
            v_psycestu(jj);
        else
            v_psycest(jj);
        end
    elseif quitit && pf(31)             % 'p': plot PDF: figures 1:m
        for jj=1:zmodel
            figure(jj);
            if unimode
                v_psycestu(jj);
            else
                v_psycest(jj);
            end
        end
    end
    if pf(12) || quitit && pf(11)       % 'F/f': plot Psychometric function on figure 101
        figure(pv(12)+1);
        if unimode
            qqu.pk=m(1,jj,pv(10));      % peak position
            qqu.w=m(2,1,pv(10));        % peak width
            qqu.ll=m(3,1,pv(10));       % peak slope on low side
            qqu.lh=m(4,1,pv(10));       % peak slope on high side
            qqu.gu=pact(2);             % guess rate
            qqu.la=pact(1);             % lapse rate
            sw=qqu.w*qqu.ll*qqu.lh/(qqu.ll+qqu.lh);   % normalized distance of inflections from peak
            xax=linspace(qqu.pk-(4+sw)/qqu.ll,qqu.pk+(4+sw)/qqu.lh,200);
            bs=(qqu.lh-qqu.ll)/2;
            bu=(qqu.lh+qqu.ll)/2;
            plot(xax,qqu.gu+(1-qqu.gu-qqu.la)*(1+2*exp(-sw)*cosh(bs*(xax-qqu.pk)+bu*abs(xax-qqu.pk))+exp(-2*sw)).^(-1));
        else
            sd=(pact(1,:)-pact(3,:)).*(1-pact(2,:)-pact(1,:))./(m(2,:,pv(10)).*(1-pact(3,:)-pact(2,:)));
            md=m(1,:,pv(10))-sd.*log((pact(1,:)-pact(3,:))./(1-pact(2,:)-pact(1,:)));
            xax=linspace(min(md-3*sd),max(md+3*sd),100);
            for jj=1:zmodel
                plot(xax,v_psychofunc('',[pact(1,jj); m(:,jj,pv(10)); pact(2:3,jj); qact(10)],xax));
                hold on
            end
            hold off
        end
        axis([xax(1) xax(end) 0 1]);
        xlabel('SNR (dB)');
        ylabel('Recognition Probability');
        title(sprintf('Intelligibility: %s',procdesc));
    end
    if pf(10) || quitit && pf(9)        % 'E/e': plot evolving Psychometric function on figure 103
        figure(pv(12)+3);
        psyevo=zeros(ntrial+1,nxevo);   % space for evolving pdf
        if unimode                      % unimodal psychometric function
            qqu.pk=m(1,jj,pv(10));      % peak position
            qqu.w=m(2,1,pv(10));        % peak width
            qqu.ll=m(3,1,pv(10));       % peak slope on low side
            qqu.lh=m(4,1,pv(10));       % peak slope on high side
            qqu.gu=pact(2);             % guess rate
            qqu.la=pact(1);             % lapse rate
            sw=qqu.w*qqu.ll*qqu.lh/(qqu.ll+qqu.lh);   % normalized distance of inflections from peak
            xax=linspace(qqu.pk-(4+sw)/qqu.ll,qqu.pk+(4+sw)/qqu.lh,nxevo);
            for iet=1:imax+1
                qqu.pk=mnt(1,1,pv(10),iet);     % peak position
                qqu.w=mnt(2,1,pv(10),iet);    	% peak width
                qqu.ll=mnt(3,1,pv(10),iet);     % peak slope on low side
                qqu.lh=mnt(4,1,pv(10),iet);     % peak slope on high side
                sw=qqu.w*qqu.ll*qqu.lh/(qqu.ll+qqu.lh);   % normalized distance of inflections from peak
                bs=(qqu.lh-qqu.ll)/2;
                bu=(qqu.lh+qqu.ll)/2;
                psyevo(iet,:)=qqu.gu+(1-qqu.gu-qqu.la)*(1+2*exp(-sw)*cosh(bs*(xax-qqu.pk)+bu*abs(xax-qqu.pk))+exp(-2*sw)).^(-1);
            end
            imagesc(xax,0:ntrial,psyevo);
            axis 'ij'  % put trial 0 at the top
        else                            % monotonic psychometric function (not implemented)
        end
        xlabel('SNR (dB)');
        ylabel('After trial');
        title(sprintf('Intelligibility: %s',procdesc));
    end
    if pf(44) || quitit && pf(43)   % 'V/v': plot convergence on figure 102
        figure(pv(12)+2);
        if unimode                      % unimodal psychometric function
            sw=mnt(2,1,pv(10),1:imax+1).*mnt(3,1,pv(10),1:imax+1).*mnt(4,1,pv(10),1:imax+1)./(mnt(3,1,pv(10),1:imax+1).*mnt(4,1,pv(10),1:imax+1));
            subplot(221)
            plot(0:imax,squeeze(mnt(1,1,pv(10),1:imax+1)));
            set(gca,'xlim',[0 ntrial]);
            xlabel('After trial');
            ylabel('Peak position (dB SNR)');
            subplot(222)
            plot(0:imax,squeeze(mnt(1,1,pv(10),1:imax+1)-sw.*mnt(3,1,pv(10),1:imax+1)),'-b',0:imax,squeeze(mnt(1,1,pv(10),1:imax+1)+sw.*mnt(4,1,pv(10),1:imax+1)),'-b');
            set(gca,'xlim',[0 ntrial]);
            xlabel('After trial');
            ylabel('Inflections (dB)');
            subplot(223)
            plot(0:imax,squeeze(mnt(3,1,pv(10),1:imax+1)));
            set(gca,'xlim',[0 ntrial]);
            xlabel('After trial');
            ylabel('Upwards slope (prob/dB)');
            subplot(224)
            plot(0:imax,squeeze(mnt(4,1,pv(10),1:imax+1)));
            set(gca,'xlim',[0 ntrial]);
            xlabel('After trial');
            ylabel('Downwards slope (prob/dB)');
        else                            % monotonic psychometric function
            subplot(211);
            for jj=1:zmodel
                plot(0:imax,squeeze(mnt(1,jj,pv(10),1:imax+1)));
                hold on
            end
            hold off
            set(gca,'xlim',[0 ntrial]);
            xlabel('After trial');
            ylabel('SRT (dB SNR)');
            subplot(212);
            for jj=1:zmodel
                plot(0:i,squeeze(mnt(2,jj,pv(10),1:imax+1)));
                hold on
            end
            hold off
            set(gca,'xlim',[0 ntrial]);
            xlabel('After trial');
            ylabel('Slope (Prob/dB)');
        end
    end
end   % main loop for each probe value
if pf(29) || pf(30)
    fclose(ofid);
end


% rf_addGrad.m
% Jamie Near, McGill University 2020.
%
% USAGE:
% RF_out=rf_addGrad(RF_in,grad);
%
% DESCRIPTION:
% Add a gradient waveform to the input RF pulse.
%
% INPUTS:
% RF_in     = Input RF pulse definition structure
% grad      = If grad is a scalar, then rf_addGrad will add a constant
%             gradient with amplitude equal to the value of grad.  If grad
%             is a vector, then rf_addGrad will add the specified gradient
%             vector to the RF waveform.  In either case, the gradient
%             should be specified in units of [G/cm].
%
% OUTPUTS:
% RF_out    = Output rf waveform following the addition of gradient
%             waveform.

function RF_out=rf_addGrad(RF_in,grad);

if ~isstruct(RF_in)
    error('ERROR:  the input RF pulse must be in structure format.  Try using rf_readwaveform to convert it!  Aborting.  ');
end

newWaveform=RF_in.waveform;

%Check if the input RF pulse already has a gradient.
if size(newWaveform,2)>3
    keepGoing='n';
    disp('WARNING:  Input waveform already has a gradient waveform!');
    keepGoing=input('Do you wish to overwrite the existing gradient waveform (y or [n])','s');

    if strcmp(keepGoing,'y') || strcmp(keepGoing,'Y');
        disp('OK.  Overwriting existing gradient waveform.');
    elseif strcmp(keepGoing,'n') || strcmp(keepGoing,'N');
        error('ABORTING!!!');
    else
        error('Response not recognized.  ABORTING!!');
    end
end

%Now add the gradient.
if isscalar(grad)
    %Adding a scalar gradient
    newWaveform(:,4)=grad*ones(size(newWaveform,1),1);
else
    %Adding a gradient waveform
    if size(grad) ~= size(newWaveform,1)
        if size(grad') ~= size(newWaveform,1);
            error('ERROR:  Gradient waveform does not match the length of the input RF pulse waveform!!  ABORTING!!');
        end
        newWaveform(:,4)=grad';
    end
    newWaveform(:,4)=grad';
end

%now it's time to find out the time-bandwidth product:
%First make a high resolution plot the pulse profile over a wide bandwidth:
[mv,sc]=bes(rf,Tp*1000,'f',w1max/1000,-5+f0/1000,5+f0/1000,100000);
if isstr(RF_in.type)
    if RF_in.type=='exc'
        index=find(mv(3,:)<0.5);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif RF_in.type=='ref'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif RF_in.type=='inv'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    end
elseif isnumeric(RF_in.type)
    mz=cos(type); %Find out the Mz value immediately following the pulse.
    thr=(1+mz)/2;  %Find out the Mz value mid-way between 1 and the mz (half-max):
    index=find(mv(3,:)<thr);  %Find the indices of the corresponding "full width"
    bw=sc(index(end))-sc(index(1));  %Now find the bandwidth at that point ("Full width at half max").
end

%Now make a very high resolution plot the pulse profile over a narrower bandwidth:
[mv,sc]=bes(rf,Tp*1000,'f',w1max/1000,-bw+f0/1000,bw+f0/1000,100000);
if isstr(RF_in.type)
    if RF_in.type=='exc'
        index=find(mv(3,:)<0.5);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif RF_in.type=='ref'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif RF_in.type=='inv'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    end
elseif isnumeric(RF_in.type)
    mz=cos(RF_in.type); %Find out the Mz value immediately following the pulse.
    thr=(1+mz)/2;  %Find out the Mz value mid-way between 1 and the mz (half-max):
    index=find(mv(3,:)<thr);  %Find the indices of the corresponding "full width"
    bw=sc(index(end))-sc(index(1));  %Now find the bandwidth at that point ("Full width at half max").
end

RF_out=RF_in;
RF_out.waveform=newWaveform;
RF_out.isGM=true;
RF_out.tbw='N/A - gradient modulated pulse';
RF_out.isGM=true;
RF_out.tthk=bw*Tp; %This is the time x sliceThickness product for
                   %gradient modulated pulses.  It is in units [cm.s]


%% kabschRotationMatrix
% Below is a demonstration of the features of the |kabschRotationMatrix| function

%%
clear; close all; clc;

%% Syntax
% |[Q]=kabschRotationMatrix(V1,V2);|

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


function [data] = readmarkerfile(folder);
% function reads a CTF data file (.ds), Read 'markerfile.mrk'
% marker is the read-only 'markerfile.mrk'
% creates structure 'data' which contains number_markers, number_samples,
%       marker_names, trial_times, marker.

%
%
%
%      <>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> %
%      <                                                      > %
%      <                    DISCLAIMER:                       > %
%      <                                                      > %
%      <  THIS PROGRAM IS INTENDED FOR RESEARCH PURPOSES ONLY. > %
%      <  THIS PROGRAM IS IN NO WAY INTENDED FOR CLINICAL OR   > %
%      <                    OFFICIAL USE.                     > %
%      <                                                      > %
%      <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<> %
%
%
%

a = exist(fullfile(folder,'MarkerFile.mrk'));
if a == 0
  disp(' ');
  disp('Error: Your data could not be found. MarkerFile.mrk does not exist in this directory.');
  disp(' ');
  data = [];
  return
end   % error message if incorrect directory is displayed

markr =  textread(fullfile(folder,'MarkerFile.mrk'),'%s','delimiter','\n');
% creates markerfile.mrk in FOLDER directory. Type 'marker' to view.

% define markers
number_markers_id = strmatch('NUMBER OF MARKERS:',markr,'exact');
markers = str2num(markr{number_markers_id+1});

% define samples
number_samples_id = strmatch('NUMBER OF SAMPLES:',markr,'exact');
samples = str2num(char(markr(number_samples_id+1)));

for i = 1:length(samples)
  if samples(i) == 0
    disp('Error: one of the markers is bad.');
    data = [];
    return
  end
end

name_id = strmatch('NAME:',markr,'exact');
names = markr(name_id+1);
% defines names

trial = strmatch('LIST OF SAMPLES',markr)+2;

for i = 1:markers
  trials{i} = str2num(char(markr(trial(i):trial(i)+samples(i))));
  trials{i}(:,1) = trials{i}(:,1) + 1;
end

data = struct('number_markers',{markers},'number_samples',{samples},'marker_names',{names},'trial_times',{trials});
% defines data

return


function [ys, scores] = predict_matconvnet(ims, net, batchsize)

ys = zeros(size(ims, 4), 1);
scores = zeros(1000, size(ims, 4)); % Number of classes
for b = 1:batchsize:size(ims, 4)
    res = vl_simplenn(net, gpuArray(ims(:, :, :, b:(b+batchsize-1))), [], [], 'Mode', 'test');

    cc = squeeze(gather(res(end).x));
    scores(:, b:(b+batchsize-1)) = cc;
    [~, ys(b:(b+batchsize-1))] = max(squeeze(gather(res(end).x)));
end

end


function rx = rxLoadMrVistaAlignment(rx,mrSessPath)
%
% rx = rxLoadMrVistaAlignment([rx],[mrSessPath])
%
% Load a mrVista alignment from a mrSESSION.mat file
% into mrRx.
%
% ras 03/05.
if notDefined('rx')
    cfig = findobj('Tag','rxControlFig');
    rx = get(cfig,'UserData');
end

if notDefined('mrSessPath')
    mrSessPath = fullfile(pwd,'mrSESSION.mat');
end

if ~exist(mrSessPath,'file')
    msg = sprintf('%s not found.',mrSessPath);
    myErrorDlg(msg);
end

h = msgbox('Loading mrVista Alignment...');

load(mrSessPath);

newXform = mrSESSION.alignment;

% flip to (x,y,z) instead of (y,x,z):
newXform(:,[1 2]) = newXform(:,[2 1]);
newXform([1 2],:) = newXform([2 1],:);

rx = rxSetXform(rx,newXform,0);

rxStore(rx,'mrVista Alignment');

close(h);

return


function sqrtPd = getSqrtPrecon(problem, x, d, storedb, key)
% Applies the square root of the Hessian preconditioner at x along d.
%
% function sqrtPd = getSqrtPrecon(problem, x, d)
% function sqrtPd = getSqrtPrecon(problem, x, d, storedb)
% function sqrtPd = getSqrtPrecon(problem, x, d, storedb, key)
%
% Returns as sqrtPd the result of applying the square root of the Hessian
% preconditioner to the tangent vector d at point x. The preconditioner is
% supposed to be a symmetric, positive definite approximation of the
% inverse of the Hessian. Its square root must thus be symmetric and
% positive definite itself.
%
% If no square root of preconditioner is available, sqrtPd = d (identity).
% Note that this may be incompatible with the preconditioner, if that one
% is supplied in the problem description. Always check with canGetPrecon
% and canGetSqrtPrecon.
%
% storedb is a StoreDB object, key is the StoreDB key to point x.
%
% See also: getPrecon canGetPrecon canGetSqrtPrecon getHessian

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 3, 2015.
% Contributors:
% Change log:

    % Allow omission of the key, and even of storedb.
    if ~exist('key', 'var')
        if ~exist('storedb', 'var')
            storedb = StoreDB();
        end
        key = storedb.getNewKey();
    end

    if isfield(problem, 'sqrtprecon')
    %% Apply sqrtprecon for the square root of the preconditioner

        % Check whether this function wants to deal with storedb or not.
        switch nargin(problem.sqrtprecon)
            case 2
                sqrtPd = problem.sqrtprecon(x, d);
            case 3
                % Obtain, pass along, and save the store for x.
                store = storedb.getWithShared(key);
                [sqrtPd, store] = problem.sqrtprecon(x, d, store);
                storedb.setWithShared(store, key);
            case 4
                % Pass along the whole storedb (by reference), with key.
                sqrtPd = problem.sqrtprecon(x, d, storedb, key);
            otherwise
                up = MException('manopt:getSqrtPrecon:badsqrtprecon', ...
                    'sqrtprecon should accept 2, 3 or 4 inputs.');
                throw(up);
        end

    else
    %% No preconditioner square root provided, so just use the identity.

        sqrtPd = d;

    end

end


function [post nlZ dnlZ] = infFITC_EP(hyp, mean, cov, lik, x, y)

% FITC-EP approximation to the posterior Gaussian process. The function is
% equivalent to infEP with the covariance function:
%   Kt = Q + G; G = diag(g); g = diag(K-Q);  Q = Ku'*inv(Kuu + snu2*eye(nu))*Ku;
% where Ku and Kuu are covariances w.r.t. to inducing inputs xu and
% snu2 = sn2/1e6 is the noise of the inducing inputs. We fixed the standard
% deviation of the inducing inputs snu to be a one per mil of the measurement
% noise's standard deviation sn. In case of a likelihood without noise
% parameter sn2, we simply use snu2 = 1e-6.
% For details, see The Generalized FITC Approximation, Andrew Naish-Guzman and
%                  Sean Holden, NIPS, 2007.
%
% The implementation exploits the Woodbury matrix identity
%   inv(Kt) = inv(G) - inv(G)*Ku'*inv(Kuu+Ku*inv(G)*Ku')*Ku*inv(G)
% in order to be applicable to large datasets. The computational complexity
% is O(n nu^2) where n is the number of data points x and nu the number of
% inducing inputs in xu.
% The posterior N(f|h,Sigma) is given by h = m+mu with mu = nn + P'*gg and
% Sigma = inv(inv(K)+diag(W)) = diag(d) + P'*R0'*R'*R*R0*P. Here, we use the
% site parameters: b,w=$b,\pi$=tnu,ttau, P=$P'$, nn=$\nu$, gg=$\gamma$
%
% The function takes a specified covariance function (see covFunctions.m) and
% likelihood function (see likFunctions.m), and is designed to be used with
% gp.m and in conjunction with covFITC.
%
% The inducing points can be specified through 1) the 2nd covFITC parameter or
% by 2) providing a hyp.xu hyperparameters. Note that 2) has priority over 1).
% In case 2) is provided and derivatives dnlZ are requested, there will also be
% a dnlZ.xu field allowing to optimise w.r.t. to the inducing points xu. However
% the derivatives dnlZ.xu can only be computed for one of the following eight
% covariance functions: cov{Matern|PP|RQ|SE}{iso|ard}.
%
% Copyright (c) by Hannes Nickisch, 2013-10-29.
%
% See also INFMETHODS.M, COVFITC.M.

persistent last_ttau last_tnu              % keep tilde parameters between calls
tol = 1e-4; max_sweep = 20; min_sweep = 2;     % tolerance to stop EP iterations
inf = 'infEP';
cov1 = cov{1}; if isa(cov1, 'function_handle'), cov1 = func2str(cov1); end
if ~strcmp(cov1,'covFITC'); error('Only covFITC supported.'), end    % check cov
if isfield(hyp,'xu'), cov{3} = hyp.xu; end  % hyp.xu is provided, replace cov{3}

[diagK,Kuu,Ku] = feval(cov{:}, hyp.cov, x);         % evaluate covariance matrix
if ~isempty(hyp.lik)                          % hard coded inducing inputs noise
  sn2 = exp(2*hyp.lik(end)); snu2 = 1e-6*sn2;               % similar to infFITC
else
  snu2 = 1e-6;
end
[n, D] = size(x); nu = size(Kuu,1);
m = feval(mean{:}, hyp.mean, x);                      % evaluate the mean vector

rot180   = @(A)   rot90(rot90(A));                     % little helper functions
chol_inv = @(A) rot180(chol(rot180(A))')\eye(nu);                 % chol(inv(A))
R0 = chol_inv(Kuu+snu2*eye(nu));           % initial R, used for refresh O(nu^3)
V = R0*Ku; d0 = diagK-sum(V.*V,1)';    % initial d, needed for refresh O(n*nu^2)

% A note on naming: variables are given short but descriptive names in
% accordance with Rasmussen & Williams "GPs for Machine Learning" (2006): mu
% and s2 are mean and variance, nu and tau are natural parameters. A leading t
% means tilde, a subscript _ni means "not i" (for cavity parameters), or _n
% for a vector of cavity parameters.

% marginal likelihood for ttau = tnu = zeros(n,1); equals n*log(2) for likCum*
nlZ0 = -sum(feval(lik{:}, hyp.lik, y, m, diagK, inf));
if any(size(last_ttau) ~= [n 1])      % find starting point for tilde parameters
  ttau = zeros(n,1);             % initialize to zero if we have no better guess
  tnu  = zeros(n,1);
  [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu); % compute initial repres.
  nlZ = nlZ0;
else
  ttau = last_ttau;                    % try the tilde values from previous call
  tnu  = last_tnu;
  [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu); % compute initial repres.
  nlZ = epfitcZ(d,P,R,nn,gg,ttau,tnu,d0,R0,Ku,y,lik,hyp,m,inf);
  if nlZ > nlZ0                                           % if zero is better ..
    ttau = zeros(n,1);                    % .. then initialize with zero instead
    tnu  = zeros(n,1);
    [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu);       % initial repres.
    nlZ = nlZ0;
  end
end

nlZ_old = Inf; sweep = 0;               % converged, max. sweeps or min. sweeps?
while (abs(nlZ-nlZ_old) > tol && sweep < max_sweep) || sweep<min_sweep
  nlZ_old = nlZ; sweep = sweep+1;
  for i = randperm(n)       % iterate EP updates (in random order) over examples
    pi = P(:,i); t = R*(R0*pi);                            % temporary variables
    sigmai = d(i) + t'*t; mui = nn(i) + pi'*gg;           % post moments O(nu^2)

    tau_ni = 1/sigmai-ttau(i);          %  first find the cavity distribution ..
    nu_ni = mui/sigmai+m(i)*tau_ni-tnu(i);          % .. params tau_ni and nu_ni

    % compute the desired derivatives of the indivdual log partition function
    [lZ, dlZ, d2lZ] = feval(lik{:}, hyp.lik, y(i), nu_ni/tau_ni, 1/tau_ni, inf);
    ttaui =                            -d2lZ  /(1+d2lZ/tau_ni);
    ttaui = max(ttaui,0);     % enforce positivity i.e. lower bound ttau by zero
    tnui  = ( dlZ + (m(i)-nu_ni/tau_ni)*d2lZ )/(1+d2lZ/tau_ni);
    [d,P(:,i),R,nn,gg,ttau,tnu] = ...                    % update representation
              epfitcUpdate(d,P(:,i),R,nn,gg, ttau,tnu,i,ttaui,tnui, m,d0,Ku,R0);
  end
  % recompute since repeated rank-one updates can destroy numerical precision
  [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu);
  [nlZ,nu_n,tau_n] = epfitcZ(d,P,R,nn,gg,ttau,tnu,d0,R0,Ku,y,lik,hyp,m,inf);
end

if sweep == max_sweep
  warning('maximum number of sweeps reached in function infEP')
end
last_ttau = ttau; last_tnu = tnu;                       % remember for next call

post.sW = sqrt(ttau);                  % unused for FITC_EP prediction with gp.m
dd = 1./(d0+1./ttau);
alpha = tnu./(1+d0.*ttau);
RV = R*V; R0tV = R0'*V;
alpha = alpha - (RV'*(RV*alpha)).*dd;     % long alpha vector for ordinary infEP
post.alpha = R0tV*alpha;       % alpha = R0'*V*inv(Kt+diag(1./ttau))*(tnu./ttau)
B = R0tV.*repmat(dd',nu,1); L = B*R0tV'; B = B*RV';
post.L = B*B' - L;                      % L = -R0'*V*inv(Kt+diag(1./ttau))*V'*R0

if nargout>2                                           % do we want derivatives?
  dnlZ = hyp;                                   % allocate space for derivatives
  RVdd = RV.*repmat(dd',nu,1);
  for i=1:length(hyp.cov)
    [ddiagK,dKuu,dKu] = feval(cov{:}, hyp.cov, x, [], i); % eval cov derivatives
    dA = 2*dKu'-R0tV'*dKuu;                                       % dQ = dA*R0tV
    w = sum(dA.*R0tV',2); v = ddiagK-w;   % w = diag(dQ); v = diag(dK)-diag(dQ);
    z = dd'*(v+w) - sum(RVdd.*RVdd,1)*v - sum(sum( (RVdd*dA)'.*(R0tV*RVdd') ));
    dnlZ.cov(i) = (z - alpha'*(alpha.*v) - (alpha'*dA)*(R0tV*alpha))/2;
  end
  for i = 1:numel(hyp.lik)                                   % likelihood hypers
    dlik = feval(lik{:}, hyp.lik, y, nu_n./tau_n, 1./tau_n, inf, i);
    dnlZ.lik(i) = -sum(dlik);
    if i==numel(hyp.lik)
      % since snu2 is a fixed fraction of sn2, there is a covariance-like term
      % in the derivative as well
      v = sum(R0tV.*R0tV,1)';
      z = sum(sum( (RVdd*R0tV').^2 )) - sum(RVdd.*RVdd,1)*v;
      z = z + post.alpha'*post.alpha - alpha'*(v.*alpha);
      dnlZ.lik(i) = dnlZ.lik(i) + snu2*z;
    end
  end
  [junk,dlZ] = feval(lik{:}, hyp.lik, y, nu_n./tau_n, 1./tau_n, inf);% mean hyps
  for i = 1:numel(hyp.mean)
    dm = feval(mean{:}, hyp.mean, x, i);
    dnlZ.mean(i) = -dlZ'*dm;
  end
  if isfield(hyp,'xu')                   % derivatives w.r.t. inducing points xu
    xu = cov{3};
    cov = cov{2};             % get the non FITC part of the covariance function
    Kpu  = cov_deriv_sq_dist(cov,hyp.cov,xu,x);             % d K(xu,x ) / d D^2
    Kpuu = cov_deriv_sq_dist(cov,hyp.cov,xu);               % d K(xu,xu) / d D^2
    if iscell(cov), covstr = cov{1}; else covstr = cov; end
    if ~ischar(covstr), covstr = func2str(covstr); end
    if numel(strfind(covstr,'iso'))>0              % characteristic length scale
      e = 2*exp(-2*hyp.cov(1));
    else
      e = 2*exp(-2*hyp.cov(1:D));
    end
    B = (R0'*R0)*Ku;

    W = ttau;
    t = W./(1+W.*d0);
    diag_dK = alpha.*alpha + sum(RVdd.*RVdd,1)' - t;
    v = diag_dK+t;                 % BdK = B * ( dnlZ/dK - diag(diag(dnlZ/dK)) )
    BdK = (B*alpha)*alpha' - B.*repmat(v',nu,1);
    BdK = BdK + (B*RVdd')*RVdd;
    A = Kpu.*BdK; C = Kpuu.*(BdK*B'); C = diag(sum(C,2)-sum(A,2)) - C;
    dnlZ.xu = A*x*diag(e) + C*xu*diag(e);    % bring in data and inducing points
  end
end

% refresh the representation of the posterior from initial and site parameters
% to prevent possible loss of numerical precision after many epfitcUpdates
% effort is O(n*nu^2) provided that nu<n
function [d,P,R,nn,gg] = epfitcRefresh(d0,P0,R0,R0P0, w,b)
  nu = size(R0,1);                                   % number of inducing points
  rot180   = @(A)   rot90(rot90(A));                   % little helper functions
  chol_inv = @(A) rot180(chol(rot180(A))')\eye(nu);               % chol(inv(A))
  t  = 1./(1+d0.*w);                                   % temporary variable O(n)
  d  = d0.*t;                                                             % O(n)
  P  = repmat(t',nu,1).*P0;                                            % O(n*nu)
  T  = repmat((w.*t)',nu,1).*R0P0;                % temporary variable O(n*nu^2)
  R  = chol_inv(eye(nu)+R0P0*T');                                    % O(n*nu^3)
  nn = d.*b;                                                              % O(n)
  gg = R0'*(R'*(R*(R0P0*(t.*b))));                                     % O(n*nu)

% compute the marginal likelihood approximation
% effort is O(n*nu^2) provided that nu<n
function [nlZ,nu_n,tau_n] = ...
                        epfitcZ(d,P,R,nn,gg,ttau,tnu, d0,R0,P0, y,lik,hyp,m,inf)
  T = (R*R0)*P;                                             % temporary variable
  diag_sigma = d + sum(T.*T,1)'; mu = nn + P'*gg;       % post moments O(n*nu^2)
  tau_n = 1./diag_sigma-ttau;              % compute the log marginal likelihood
  nu_n  = mu./diag_sigma-tnu+m.*tau_n;            % vectors of cavity parameters
  lZ = feval(lik{:}, hyp.lik, y, nu_n./tau_n, 1./tau_n, inf);
  nu = size(gg,1);
  U = (R0*P0)'.*repmat(1./sqrt(d0+1./ttau),1,nu);
  L = chol(eye(nu)+U'*U);
  ld = 2*sum(log(diag(L))) + sum(log(1+d0.*ttau));
  t = T*tnu; tnu_Sigma_tnu = tnu'*(d.*tnu) + t'*t;
  nlZ = ld/2 -sum(lZ) -tnu_Sigma_tnu/2  ...
    -(nu_n-m.*tau_n)'*((ttau./tau_n.*(nu_n-m.*tau_n)-2*tnu)./(ttau+tau_n))/2 ...
      +sum(tnu.^2./(tau_n+ttau))/2-sum(log(1+ttau./tau_n))/2;

% update the representation of the posterior to reflect modification of the site
% parameters by w(i) <- wi and b(i) <- bi
% effort is O(nu^2)
% Pi = P(:,i) is passed instead of P to prevent allocation of a new array
function [d,Pi,R,nn,gg,w,b] = ...
                           epfitcUpdate(d,Pi,R,nn,gg, w,b, i,wi,bi, m,d0,P0,R0)
  dwi = wi-w(i); dbi = bi-b(i);
  hi = nn(i) + m(i) + Pi'*gg;                   % posterior mean of site i O(nu)
  t = 1+dwi*d(i);
  d(i) = d(i)/t;                                                          % O(1)
  nn(i) = d(i)*bi;                                                        % O(1)
  r = 1+d0(i)*w(i);
  r = r*r/dwi + r*d0(i);
  v = R*(R0*P0(:,i));
  r = 1/(r+v'*v);
  if r>0
    R = cholupdate(R,sqrt( r)*R'*v,'-');
  else
    R = cholupdate(R,sqrt(-r)*R'*v,'+');
  end
  gg = gg + ((dbi-dwi*(hi-m(i)))/t)*(R0'*(R'*(R*(R0*Pi))));            % O(nu^2)
  w(i) = wi; b(i) = bi;                            % update site parameters O(1)
  Pi = Pi/t;                                                             % O(nu)


function net = resnet52_new_hope()

%----------------------img cnn----------------------
netStruct = load('./data/imagenet-vgg-verydeep-19.mat') ;
net = dagnn.DagNN.loadobj(netStruct) ;
net.removeLayer('fc8');
net.removeLayer('relu7');
net.removeLayer('prob');

for i = 1:numel(net.params)
    if(mod(i,2)==0)
        net.params(i).learningRate= 0; %0.02;
    else net.params(i).learningRate= 0; %0.001;
    end
    net.params(i).weightDecay=0; %1;
end

net.params(1).learningRate = 0; %1e-5;

fc1Block = dagnn.Conv('size',[1 1 4096 4096],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc1',fc1Block,{'x40'},{'fc1'},{'fc1f'});
net.addLayer('fc1bn',dagnn.BatchNorm(),{'fc1'},{'fc1bn'},...
    {'fc1bn_w','fc1bn_b','fc1bn_m'});
net.addLayer('fc1x',dagnn.ReLU(),{'fc1bn'},{'fc1bnx'});
fc1Block = dagnn.Conv('size',[1 1 4096 2048],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc1_1',fc1Block,{'fc1bnx'},{'fc1_1'},{'fc1_1f'});
net.addLayer('fc1_1bn',dagnn.BatchNorm(),{'fc1_1'},{'fc1_1bn'},...
    {'fc1_1bn_w','fc1_1bn_b','fc1_1bn_m'});
net.addLayer('fc1_1x',dagnn.ReLU(),{'fc1_1bn'},{'fc1_1bnx'});
net.addLayer('dropout',dagnn.DropOut('rate',0.5),{'fc1_1bnx'},{'fc1_1bnxd'});

%----------------------char cnn----------------------
% input is 1*32*20074*16
fc2Block = dagnn.Conv('size',[1 1 29972 300],'hasBias',true,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc2',fc2Block,{'data2'},{'fc2'},{'fc2f','fc2b'});
net.addLayer('fc2bn',dagnn.BatchNorm(),{'fc2'},{'fc2bn'},...
    {'fc2bn_w','fc2bn_b','fc2bn_m'});
%net.addLayer('fc2x',dagnn.ReLU(),{'fc2bn'},{'fc2bnx'});
%net.addLayer('dropout_diction',dagnn.DropOut('rate',0.5),{'fc2bn'},{'fc2bnd'});
% 32*256
convBlock = dagnn.Conv('size',[1 1 300 128],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc2_1_1',convBlock,{'fc2bn'},{'fc2_1_1'},{'fc2_1_1f'});
net.addLayer('fc2_1_1bn',dagnn.BatchNorm(),{'fc2_1_1'},{'fc2_1_1bn'},...
    {'fc2_1_1bn_w','fc2_1_1bn_b','fc2_1_1bn_m'});
net.addLayer('fc2_1_1x',dagnn.ReLU(),{'fc2_1_1bn'},{'fc2_1_1bnx'});

convBlock = dagnn.Conv('size',[1 2 128 128],'hasBias',false,'stride',[1,1],'pad',[0,0,1,0]);
net.addLayer('fc2_1_2',convBlock,{'fc2_1_1bnx'},{'fc2_1_2'},{'fc2_1_2f'});
net.addLayer('fc2_1_2bn',dagnn.BatchNorm(),{'fc2_1_2'},{'fc2_1_2bn'},...
    {'fc2_1_2bn_w','fc2_1_2bn_b','fc2_1_2bn_m'});
net.addLayer('fc2_1_2x',dagnn.ReLU(),{'fc2_1_2bn'},{'fc2_1_2bnx'});

convBlock = dagnn.Conv('size',[1 1 128 256],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc2_1_3',convBlock,{'fc2_1_2bnx'},{'fc2_1_3'},{'fc2_1_3f'});
net.addLayer('fc2_1_3bn',dagnn.BatchNorm(),{'fc2_1_3'},{'fc2_1_3bn'},...
    {'fc2_1_3bn_w','fc2_1_3bn_b','fc2_1_3bn_m'});

convBlock = dagnn.Conv('size',[1 1 300 256],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc2_1b',convBlock,{'fc2bn'},{'fc2_1b'},{'fc2_1bf'});
net.addLayer('fc2_1bbn',dagnn.BatchNorm(),{'fc2_1b'},{'fc2_1bbn'},...
    {'fc2_1bbn_w','fc2_1bbn_b','fc2_1bbn_m'});

net.addLayer('fc2_1sum',dagnn.Sum(),{'fc2_1_3bn','fc2_1bbn'},...
    {'fc2_1sum'});
net.addLayer('fc2_1x',dagnn.ReLU(),{'fc2_1sum'},{'fc2_1sumx'});

% 32*256
for i  = 2:3
    convBlock = dagnn.Conv('size',[1 1 256 64],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
    net.addLayer( sprintf('fc2_%d_1',i),convBlock,{sprintf('fc2_%dsumx',i-1)},{sprintf('fc2_%d_1',i)}, ...
        {sprintf('fc2_%d_1f',i)});
    net.addLayer(sprintf('fc2_%d_1bn_1',i),dagnn.BatchNorm(),{sprintf('fc2_%d_1',i)},{sprintf('fc2_%d_1bn',i)},...
        {sprintf('fc2_%d_1bn_w',i),sprintf('fc2_%d_1bn_b',i),sprintf('fc2_%d_1bn_m',i)});
    net.addLayer(sprintf('fc2_%d_1x',i),dagnn.ReLU(),{sprintf('fc2_%d_1bn',i)},{sprintf('fc2_%d_1bnx',i)});

    convBlock = dagnn.Conv('size',[1 2 64 64],'hasBias',false,'stride',[1,1],'pad',[0,0,1,0]);
    net.addLayer( sprintf('fc2_%d_2',i),convBlock,{sprintf('fc2_%d_1bnx',i)},{sprintf('fc2_%d_2',i)}, ...
        {sprintf('fc2_%d_2f',i)});
    net.addLayer(sprintf('fc2_%d_2bn',i),dagnn.BatchNorm(),{sprintf('fc2_%d_2',i)},{sprintf('fc2_%d_2bn',i)},...
        {sprintf('fc2_%d_2bn_w',i),sprintf('fc2_%d_2bn_b',i),sprintf('fc2_%d_2bn_m',i)});
    net.addLayer(sprintf('fc2_%d_2x',i),dagnn.ReLU(),{sprintf('fc2_%d_2bn',i)},{sprintf('fc2_%d_2bnx',i)});

    convBlock = dagnn.Conv('size',[1 1 64 256],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
    net.addLayer( sprintf('fc2_%d_3',i),convBlock,{sprintf('fc2_%d_2bnx',i)},{sprintf('fc2_%d_3',i)}, ...
        {sprintf('fc2_%d_3f',i)});
    net.addLayer(sprintf('fc2_%d_3bn',i),dagnn.BatchNorm(),{sprintf('fc2_%d_3',i)},{sprintf('fc2_%d_3bn',i)},...
        {sprintf('fc2_%d_3bn_w',i),sprintf('fc2_%d_3bn_b',i),sprintf('fc2_%d_3bn_m',i)});

    net.addLayer(sprintf('fc2_%dsum',i),dagnn.Sum(),{sprintf('fc2_%dsumx',i-1),sprintf('fc2_%d_3bn',i)},...
        {sprintf('fc2_%dsum',i)});
    net.addLayer(sprintf('fc2_%dx',i),dagnn.ReLU(),{sprintf('fc2_%dsum',i)},{sprintf('fc2_%dsumx',i)});
end

%32*256
convBlock = dagnn.Conv('size',[1 1 256 512],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc2_4a_1',convBlock,{'fc2_3sumx'},{'fc2_4a_1'},{'fc2_4a_1f'});
net.addLayer('fc2_4a_1bn',dagnn.BatchNorm(),{'fc2_4a_1'},{'fc2_4a_1bn'},...
    {'fc2_4a_1bn_w','fc2_4a_1bn_b','fc2_4a_1bn_m'});
net.addLayer('fc2_4a_1x',dagnn.ReLU(),{'fc2_4a_1bn'},{'fc2_4a_1bnx'});

convBlock = dagnn.Conv('size',[1 2 512 512],'hasBias',false,'stride',[2,2],'pad',[0,0,1,0]);
net.addLayer('fc2_4a_2',convBlock,{'fc2_4a_1bnx'},{'fc2_4a_2'},{'fc2_4a_2f'});
net.addLayer('fc2_4a_2bn',dagnn.BatchNorm(),{'fc2_4a_2'},{'fc2_4a_2bn'},...
    {'fc2_4a_2bn_w','fc2_4a_2bn_b','fc2_4a_2bn_m'});
net.addLayer('fc2_4a_2x',dagnn.ReLU(),{'fc2_4a_2bn'},{'fc2_4a_2bnx'});

convBlock = dagnn.Conv('size',[1 1 512 512],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc2_4a_3',convBlock,{'fc2_4a_2bnx'},{'fc2_4a_3'},{'fc2_4a_3f'});
net.addLayer('fc2_4a_3bn',dagnn.BatchNorm(),{'fc2_4a_3'},{'fc2_4a_3bn'},...
    {'fc2_4a_3bn_w','fc2_4a_3bn_b','fc2_4a_3bn_m'});

convBlock = dagnn.Conv('size',[1 1 256 512],'hasBias',false,'stride',[2,2],'pad',[0,0,0,0]);
net.addLayer('fc2_4b',convBlock,{'fc2_3sumx'},{'fc2_4b'},{'fc2_4bf'});
net.addLayer('fc2_4bbn',dagnn.BatchNorm(),{'fc2_4b'},{'fc2_4bbn'},...
    {'fc2_4bbn_w','fc2_4bbn_b','fc2_4bbn_m'});

%16*512
net.addLayer('fc2_4sum',dagnn.Sum(),{'fc2_4a_3bn','fc2_4bbn'},...
    {'fc2_4sum'});
net.addLayer('fc2_4x',dagnn.ReLU(),{'fc2_4sum'},{'fc3_1sumx'});

for i  = 2:4
    convBlock = dagnn.Conv('size',[1 1 512 128],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
    net.addLayer( sprintf('fc3_%d_1',i),convBlock,{sprintf('fc3_%dsumx',i-1)},{sprintf('fc3_%d_1',i)}, ...
        {sprintf('fc3_%d_1f',i)});
    net.addLayer(sprintf('fc3_%d_1bn_1',i),dagnn.BatchNorm(),{sprintf('fc3_%d_1',i)},{sprintf('fc3_%d_1bn',i)},...
        {sprintf('fc3_%d_1bn_w',i),sprintf('fc3_%d_1bn_b',i),sprintf('fc3_%d_1bn_m',i)});
    net.addLayer(sprintf('fc3_%d_1x',i),dagnn.ReLU(),{sprintf('fc3_%d_1bn',i)},{sprintf('fc3_%d_1bnx',i)});

    convBlock = dagnn.Conv('size',[1 2 128 128],'hasBias',false,'stride',[1,1],'pad',[0,0,1,0]);
    net.addLayer( sprintf('fc3_%d_2',i),convBlock,{sprintf('fc3_%d_1bnx',i)},{sprintf('fc3_%d_2',i)}, ...
        {sprintf('fc3_%d_2f',i)});
    net.addLayer(sprintf('fc3_%d_2bn',i),dagnn.BatchNorm(),{sprintf('fc3_%d_2',i)},{sprintf('fc3_%d_2bn',i)},...
        {sprintf('fc3_%d_2bn_w',i),sprintf('fc3_%d_2bn_b',i),sprintf('fc3_%d_2bn_m',i)});
    net.addLayer(sprintf('fc3_%d_2x',i),dagnn.ReLU(),{sprintf('fc3_%d_2bn',i)},{sprintf('fc3_%d_2bnx',i)});

    convBlock = dagnn.Conv('size',[1 1 128 512],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
    net.addLayer( sprintf('fc3_%d_3',i),convBlock,{sprintf('fc3_%d_2bnx',i)},{sprintf('fc3_%d_3',i)}, ...
        {sprintf('fc3_%d_3f',i)});
    net.addLayer(sprintf('fc3_%d_3bn',i),dagnn.BatchNorm(),{sprintf('fc3_%d_3',i)},{sprintf('fc3_%d_3bn',i)},...
        {sprintf('fc3_%d_3bn_w',i),sprintf('fc3_%d_3bn_b',i),sprintf('fc3_%d_3bn_m',i)});

    net.addLayer(sprintf('fc3_%dsum',i),dagnn.Sum(),{sprintf('fc3_%dsumx',i-1),sprintf('fc3_%d_3bn',i)},...
        {sprintf('fc3_%dsum',i)});
    net.addLayer(sprintf('fc3_%dx',i),dagnn.ReLU(),{sprintf('fc3_%dsum',i)},{sprintf('fc3_%dsumx',i)});
end

convBlock = dagnn.Conv('size',[1 1 512 1024],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc3_5a_1',convBlock,{'fc3_4sumx'},{'fc3_5a_1'},{'fc3_5a_1f'});
net.addLayer('fc3_5a_1bn',dagnn.BatchNorm(),{'fc3_5a_1'},{'fc3_5a_1bn'},...
    {'fc3_5a_1bn_w','fc3_5a_1bn_b','fc3_5a_1bn_m'});
net.addLayer('fc3_5a_1x',dagnn.ReLU(),{'fc3_5a_1bn'},{'fc3_5a_1bnx'});

convBlock = dagnn.Conv('size',[1 2 1024 1024],'hasBias',false,'stride',[2,2],'pad',[0,0,1,0]);
net.addLayer('fc3_5a_2',convBlock,{'fc3_5a_1bnx'},{'fc3_5a_2'},{'fc3_5a_2f'});
net.addLayer('fc3_5a_2bn',dagnn.BatchNorm(),{'fc3_5a_2'},{'fc3_5a_2bn'},...
    {'fc3_5a_2bn_w','fc3_5a_2bn_b','fc3_5a_2bn_m'});
net.addLayer('fc3_5a_2x',dagnn.ReLU(),{'fc3_5a_2bn'},{'fc3_5a_2bnx'});

convBlock = dagnn.Conv('size',[1 1 1024 1024],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc3_5a_3',convBlock,{'fc3_5a_2bnx'},{'fc3_5a_3'},{'fc3_5a_3f'});
net.addLayer('fc3_5a_3bn',dagnn.BatchNorm(),{'fc3_5a_3'},{'fc3_5a_3bn'},...
    {'fc3_5a_3bn_w','fc3_5a_3bn_b','fc3_5a_3bn_m'});

convBlock = dagnn.Conv('size',[1 1 512 1024],'hasBias',false,'stride',[2,2],'pad',[0,0,0,0]);
net.addLayer('fc3_5b',convBlock,{'fc3_4sumx'},{'fc3_5b'},{'fc3_5bf'});
net.addLayer('fc3_5bbn',dagnn.BatchNorm(),{'fc3_5b'},{'fc3_5bbn'},...
    {'fc3_5bbn_w','fc3_5bbn_b','fc3_5bbn_m'});

%8*1024
net.addLayer('fc3_5sum',dagnn.Sum(),{'fc3_5a_3bn','fc3_5bbn'},...
    {'fc3_5sum'});
net.addLayer('fc3_5x',dagnn.ReLU(),{'fc3_5sum'},{'fc4_1sumx'});

for i  = 2:6
    convBlock = dagnn.Conv('size',[1 1 1024 256],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
    net.addLayer( sprintf('fc4_%d_1',i),convBlock,{sprintf('fc4_%dsumx',i-1)},{sprintf('fc4_%d_1',i)}, ...
        {sprintf('fc4_%d_1f',i)});
    net.addLayer(sprintf('fc4_%d_1bn_1',i),dagnn.BatchNorm(),{sprintf('fc4_%d_1',i)},{sprintf('fc4_%d_1bn',i)},...
        {sprintf('fc4_%d_1bn_w',i),sprintf('fc4_%d_1bn_b',i),sprintf('fc4_%d_1bn_m',i)});
    net.addLayer(sprintf('fc4_%d_1x',i),dagnn.ReLU(),{sprintf('fc4_%d_1bn',i)},{sprintf('fc4_%d_1bnx',i)});

    convBlock = dagnn.Conv('size',[1 2 256 256],'hasBias',false,'stride',[1,1],'pad',[0,0,1,0]);
    net.addLayer( sprintf('fc4_%d_2',i),convBlock,{sprintf('fc4_%d_1bnx',i)},{sprintf('fc4_%d_2',i)}, ...
        {sprintf('fc4_%d_2f',i)});
    net.addLayer(sprintf('fc4_%d_2bn',i),dagnn.BatchNorm(),{sprintf('fc4_%d_2',i)},{sprintf('fc4_%d_2bn',i)},...
        {sprintf('fc4_%d_2bn_w',i),sprintf('fc4_%d_2bn_b',i),sprintf('fc4_%d_2bn_m',i)});
    net.addLayer(sprintf('fc4_%d_2x',i),dagnn.ReLU(),{sprintf('fc4_%d_2bn',i)},{sprintf('fc4_%d_2bnx',i)});

    convBlock = dagnn.Conv('size',[1 1 256 1024],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
    net.addLayer( sprintf('fc4_%d_3',i),convBlock,{sprintf('fc4_%d_2bnx',i)},{sprintf('fc4_%d_3',i)}, ...
        {sprintf('fc4_%d_3f',i)});
    net.addLayer(sprintf('fc4_%d_3bn',i),dagnn.BatchNorm(),{sprintf('fc4_%d_3',i)},{sprintf('fc4_%d_3bn',i)},...
        {sprintf('fc4_%d_3bn_w',i),sprintf('fc4_%d_3bn_b',i),sprintf('fc4_%d_3bn_m',i)});

    net.addLayer(sprintf('fc4_%dsum',i),dagnn.Sum(),{sprintf('fc4_%dsumx',i-1),sprintf('fc4_%d_3bn',i)},...
        {sprintf('fc4_%dsum',i)});
    net.addLayer(sprintf('fc4_%dx',i),dagnn.ReLU(),{sprintf('fc4_%dsum',i)},{sprintf('fc4_%dsumx',i)});
end

convBlock = dagnn.Conv('size',[1 1 1024 2048],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc4_7a_1',convBlock,{'fc4_6sumx'},{'fc4_7a_1'},{'fc4_7a_1f'});
net.addLayer('fc4_7a_1bn',dagnn.BatchNorm(),{'fc4_7a_1'},{'fc4_7a_1bn'},...
    {'fc4_7a_1bn_w','fc4_7a_1bn_b','fc4_7a_1bn_m'});
net.addLayer('fc4_7a_1x',dagnn.ReLU(),{'fc4_7a_1bn'},{'fc4_7a_1bnx'});

convBlock = dagnn.Conv('size',[1 2 2048 2048],'hasBias',false,'stride',[1,1],'pad',[0,0,1,0]);
net.addLayer('fc4_7a_2',convBlock,{'fc4_7a_1bnx'},{'fc4_7a_2'},{'fc4_7a_2f'});
net.addLayer('fc4_7a_2bn',dagnn.BatchNorm(),{'fc4_7a_2'},{'fc4_7a_2bn'},...
    {'fc4_7a_2bn_w','fc4_7a_2bn_b','fc4_7a_2bn_m'});
net.addLayer('fc4_7a_2x',dagnn.ReLU(),{'fc4_7a_2bn'},{'fc4_7a_2bnx'});

convBlock = dagnn.Conv('size',[1 1 2048 2048],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc4_7a_3',convBlock,{'fc4_7a_2bnx'},{'fc4_7a_3'},{'fc4_7a_3f'});
net.addLayer('fc4_7a_3bn',dagnn.BatchNorm(),{'fc4_7a_3'},{'fc4_7a_3bn'},...
    {'fc4_7a_3bn_w','fc4_7a_3bn_b','fc4_7a_3bn_m'});

convBlock = dagnn.Conv('size',[1 1 1024 2048],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc4_7b',convBlock,{'fc4_6sumx'},{'fc4_7b'},{'fc4_7bf'});
net.addLayer('fc4_7bbn',dagnn.BatchNorm(),{'fc4_7b'},{'fc4_7bbn'},...
    {'fc4_7bbn_w','fc4_7bbn_b','fc4_7bbn_m'});

%8*2048
net.addLayer('fc4_7sum',dagnn.Sum(),{'fc4_7a_3bn','fc4_7bbn'},...
    {'fc4_7sum'});
net.addLayer('fc4_7x',dagnn.ReLU(),{'fc4_7sum'},{'fc5_1sumx'});

poolBlock = dagnn.Pooling('poolSize',[1 8]);
net.addLayer('fc5_1',poolBlock,{'fc5_1sumx'},{'fc5_1bnx'});
%net.addLayer('fc5_1bn',dagnn.BatchNorm(),{'fc5_1'},{'fc5_1bn'},...
 %   {'fc5_1bn_w','fc5_1bn_b','fc5_1bn_m'});
%net.addLayer('fc5_1x',dagnn.ReLU(),{'fc5_1bn'},{'fc5_1bnx'});

fc5_2Block = dagnn.Conv('size',[1 1 2048 2048],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc5_2',fc5_2Block,{'fc5_1bnx'},{'fc5_2'},{'fc5_2f'});
net.addLayer('fc5_2bn',dagnn.BatchNorm(),{'fc5_2'},{'fc5_2bn'},...
    {'fc5_2bn_w','fc5_2bn_b','fc5_2bn_m'});
net.addLayer('fc5_2x',dagnn.ReLU(),{'fc5_2bn'},{'fc5_2bnx'});
net.addLayer('dropout2',dagnn.DropOut('rate',0.5),{'fc5_2bnx'},{'fc5_2bnxd'});

%----------------------add share layer----------------------
%1

fc_imgBlock = dagnn.Conv('size',[1 1 2048 113287],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc_img',fc_imgBlock,{'fc1_1bnxd'},{'prediction_img'},{'fcsharef'});
net.addLayer('softmaxloss_img',dagnn.Loss('loss','softmaxlog'),{'prediction_img','label_img'},'objective_img');
net.addLayer('top1err_img', dagnn.Loss('loss', 'classerror'), ...
    {'prediction_img','label_img'}, 'top1err_img') ;
net.addLayer('top5err_img', dagnn.Loss('loss', 'topkerror', ...
    'opts', {'topK',5}), ...
    {'prediction_img','label_img'}, 'top5err_img') ;
%2
fc_txtBlock = dagnn.Conv('size',[1 1 2048 113287],'hasBias',false,'stride',[1,1],'pad',[0,0,0,0]);
net.addLayer('fc_txt',fc_txtBlock,{'fc5_2bnxd'},{'prediction_txt'},{'fcsharef'});
net.addLayer('softmaxloss_txt',dagnn.Loss('loss','softmaxlog'),{'prediction_txt','label_txt'},'objective_txt');
net.addLayer('top1err_txt', dagnn.Loss('loss', 'classerror'), ...
    {'prediction_txt','label_txt'}, 'top1err_txt') ;
net.addLayer('top5err_txt', dagnn.Loss('loss', 'topkerror', ...
    'opts', {'topK',5}), ...
    {'prediction_txt','label_txt'}, 'top5err_txt') ;
%}

%}
net.initParams();
%----------NOTICE--------------
first = net.getParamIndex('fc2f');

net.params(first).learningRate = 1e-3;  %w
net.params(first+1).learningRate = 1e-3;  %b

load('./dataset/MSCOCO-prepare/COCO_dictionary.mat');
net.params(first).value = reshape(single(subset.features'),1,1,29972,300);

end


function sd=sdobs(obsr,obsb,f)

global glc

if f<=glc.NFREQ
    pi=obsr.L(f);
else
    pi=obsr.P(f-glc.NFREQ);
end

if f<=glc.NFREQ
    pj=obsb.L(f);
else
    pj=obsb.P(f-glc.NFREQ);
end

if pi==0||pj==0
    sd=0;
else
    sd=pi-pj;
end

return


% The COBRAToolbox: Lp9.m
%
% Purpose:
%     - test if bug fix in LP9 affects the results for test model (without
%     coupling constraints)
%
% Authors:
%     Agnieszka Wegrzyn 2019/04/09, fix compatibility for models with coupling constraints
%

global CBTDIR

% define the features required to run the test
requireOneSolverOf = {'gurobi','ibm_cplex'};

% require the specified toolboxes and solvers, along with a UNIX OS
solversPkgs = prepareTest('requireOneSolverOf', requireOneSolverOf, 'excludeSolvers', {'matlab', 'lp_solve','pdco'});

% save the current path and initialize the test
currentDir = cd(fileparts(which(mfilename)));

% determine the test path for references
testPath = pwd;

% load the model
model = getDistributedModel('ecoli_core_model.mat'); %For all models in the test/models folder and subfolders

% load reference data and input variables
load('testData_LP9.mat');

% load
for k = 1:length(solversPkgs.LP)
    fprintf(' -- Running testLP9.m using the solver interface: %s ... ', solversPkgs.LP{k});

    solverLPOK = changeCobraSolver(solversPkgs.LP{k}, 'LP', 0);

    if solverLPOK
        % created test data for new version
        V = LP9(options.K, options.P, model, options.LPproblem, options.epsilon);
        solTest = model.c'*V;
    end
    assert(isequal(solOri,solTest), '\nResults are not consistent between old and new version of LP9\n')
    % output a success message
    fprintf('\nDone.\n');
end

% change the directory
cd(currentDir)


function impred =  runPatchVDSR(net, imlow, gpu, rf)
v = ceil(size(imlow, 1)/2);
h = ceil(size(imlow, 2)/2);

imTL = imlow(1:v+rf,   1:h+rf);
imBL = imlow(v-rf+1:end, 1:h+rf);
imTR = imlow(1:v+rf,   h-rf+1:end);
imBR = imlow(v-rf+1:end, h-rf+1:end);

if gpu, imTL = gpuArray(imTL); end;
impredTL = runVDSR(net, imTL, gpu);
impredTL = impredTL(1:v, 1:h);

if gpu, imBL = gpuArray(imBL); end;
impredBL = runVDSR(net, imBL, gpu);
impredBL = impredBL(rf+1:end, 1:h);

if gpu, imTR = gpuArray(imTR); end;
impredTR = runVDSR(net, imTR, gpu);
impredTR = impredTR(1:v, rf+1:end);

if gpu, imBR = gpuArray(imBR); end;
impredBR = runVDSR(net, imBR, gpu);
impredBR = impredBR(rf+1:end, rf+1:end);

impredL = cat(1, impredTL, impredBL);
impredR = cat(1, impredTR, impredBR);
impred = cat(2, impredL, impredR);


function L = log_prior(CPD)
% LOG_PRIOR Return log P(theta) for a hhmm CPD
% L = log_prior(CPD)

L = log_prior(CPD.sub_CPD_trans);
if ~isempty(CPD.sub_CPD_start)
  L = L + log_prior(CPD.sub_CPD_start);
end