function [stc] = mne_read_stc_file(filename)
%
% [stc] = mne_read_stc_file(filename)
%
% Reads an stc file. The returned structure has the following fields
%
%     tmin           The first time point of the data in seconds
%     tstep          Time between frames in seconds
%     vertices       vertex indices (0 based)
%     data           The data matrix (nvert * ntime)
%
%

%
%
%   Author : Matti Hamalainen, MGH Martinos Center
%   License : BSD 3-clause
%
%   Revision 1.7  2006/04/23 15:29:41  msh
%   Added MGH to the copyright
%
%   Revision 1.6  2006/04/10 23:26:54  msh
%   Added fiff reading routines
%
%   Revision 1.5  2005/12/05 20:23:21  msh
%   Added fiff_save_evoked. Improved error handling.
%
%   Revision 1.4  2005/11/21 05:40:24  msh
%   Required number of arguments was wrong.
%
%   Revision 1.3  2005/11/21 03:19:12  msh
%   Improved error handling
%
%   Revision 1.2  2005/11/21 02:15:51  msh
%   Added more routines
%
%   Revision 1.1  2005/11/21 01:41:57  msh
%   Introduced structures and start all function names with mne_
%
%
me='MNE:mne_read_stc_file';
if(nargin ~= 1)
   error(me,'usage: [stc] = mne_read_stc_file(filename)');
end

[fid,message] = fopen(filename,'r','ieee-be');
if fid == -1
    error(me,message)
end

STATUS = fseek(fid, 0, 'eof');
filelen= ftell(fid);
STATUS = fseek(fid, 0, 'bof');

% read tmin in ms
[stc.tmin count]=fread(fid,1,'float32');
stc.tmin=stc.tmin/1000.0;
% read sampling rate in ms
[stc.tstep count]=fread(fid,1,'float32');
stc.tstep=stc.tstep/1000.0;
% read number of vertices/sources
[vertices_n count]=fread(fid,1,'uint32');
% read the source vector
[stc.vertices count]=fread(fid,vertices_n,'uint32');
stc.vertices=uint32(stc.vertices);
% read the number of timepts
[data_n count]=fread(fid,1,'uint32');

if (mod((filelen/4-4-vertices_n),data_n*vertices_n) ~= 0)
   error(me,'incorrect stc file size')
end

% read the data matrix
[stc.data count]=fread(fid,vertices_n*data_n,'float32');
stc.data=squeeze(reshape(stc.data,vertices_n,data_n));
% close the file
fclose(fid);
return;


function L = kernPriorLogProb(kern)

% KERNPRIORLOGPROB Compute penalty terms associated with kernel priors.

% KERN

L = 0;
switch kern.type
 case {'cmpnd', 'multi', 'tensor'}
  for i = 1:length(kern.comp)
    L = L + kernPriorLogProb(kern.comp{i});
  end
 otherwise
  if isfield(kern, 'priors')
    fhandle = str2func([kern.type 'KernExtractParam']);
    params = fhandle(kern);
    if iscell(kern.priors),
      for i = 1:length(kern.priors)
	index = kern.priors{i}.index;
	L = L + priorLogProb(kern.priors{i}, params(index));
      end
    else
      for i = 1:length(kern.priors)
	index = kern.priors(i).index;
	L = L + priorLogProb(kern.priors(i), params(index));
      end
    end
  end
end


function [Eft, Covft, ljpyt, Eyt, Covyt] = gpia_jpred(gp_array, x, y, varargin)
%GPIA_JPRED  Prediction with Gaussian Process GP_IA solution.
%
%  Description
%    [EFT, COVFT] = GPIA_JPRED(GP_ARRAY, X, Y, XT, OPTIONS)
%    takes a cell array of GP structures together with matrix X of
%    training inputs and vector Y of training targets, and
%    evaluates the predictive distribution at test inputs XT with
%    parameters marginalized out with IA. Returns a posterior mean
%    EFT and covariance COVFT of latent variables.
%
%    [EFT, COVFT, JPYT, EYT, COVYT] = GPIA_JPRED(GP, X, Y, XT, 'yt', YT, ...)
%    returns also logarithm of the predictive joint density PYT of
%    the observations YT at test input locations XT with parameters
%    marginalized out with IA. This can be used for example in the
%    cross-validation. Here Y has to be vector. Returns also
%    posterior predictive mean EYT and covariance COVYT.
%
%    [EFT, COVFT, LJPYT, EYT, COVYT, FT, PFT] = ...
%      GPIA_JPRED(GP_ARRAY, X, Y, XT, OPTIONS)
%    returns also the numerical representation of the marginal
%    posterior of latent variables at each XT. FT is a vector of
%    latent values and PFT_i = p(FT_i) is the posterior density for
%    FT_i.
%
%    [EF, COVF, LJPY, EY, COVY, F, PF] = GPIA_JPRED(GP, X, Y, OPTIONS)
%    evaluates the predictive distribution at training inputs X
%    and logarithm of the predictive density PY of the training
%    observations Y.
%
%    OPTIONS is optional parameter-value pair
%      predcf - index vector telling which covariance functions are
%               used for prediction. Default is all (1:gpcfn). See
%               additional information below.
%      tstind - a vector/cell array defining, which rows of X belong
%               to which training block in *IC type sparse models.
%               Deafult is []. In case of PIC, a cell array
%               containing index vectors specifying the blocking
%               structure for test data. IN FIC and CS+FIC a vector
%               of length n that points out the test inputs that
%               are also in the training set (if none, set TSTIND=[])
%      yt     - optional observed yt in test points (see below)
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case
%               of Poisson likelihood we have z_i=E_i, that is,
%               expected value for ith case.
%      zt     - optional observed quantity in triplet (xt_i,yt_i,zt_i)
%               Some likelihoods may use this. For example, in case
%               of Poisson likelihood we have z_i=E_i, that is, the
%               expected value for the ith case.
%
%    NOTE! In case of FIC and PIC sparse approximation the
%    prediction for only some PREDCF covariance functions is just
%    an approximation since the covariance functions are coupled in
%    the approximation and are not strictly speaking additive
%    anymore.
%
%    For example, if you use covariance such as K = K1 + K2 your
%    predictions Eft1 = gpia_pred(gp_array, X, Y, X, 'predcf', 1) and
%    Eft2 = gpia_pred(gp_array, x, y, x, 'predcf', 2) should sum up to
%    Eft = gpia_pred(gp_array, x, y, x). That is Eft = Eft1 + Eft2. With
%    FULL model this is true but with FIC and PIC this is true only
%    approximately. That is Eft \approx Eft1 + Eft2.
%
%    With CS+FIC the predictions are exact if the PREDCF covariance
%    functions are all in the FIC part or if they are CS
%    covariances.
%
%    NOTE! When making predictions with a subset of covariance
%    functions with FIC approximation the predictive variance can
%    in some cases be ill-behaved i.e. negative or unrealistically
%    small. This may happen because of the approximative nature of
%    the prediction.
%
%  See also
%    GP_PRED, GP_SET, GP_IA
%

% Copyright (c) 2009 Ville Pietil�inen
% Copyright (c) 2009-2010 Jarno Vanhatalo
% Copyright (c) 2011-2012 Ville Tolvanen
% Copyright (c) 2012 Aki Vehtari

% This software is distributed under the GNU General Public
% Licence (version 3 or later); please refer to the file
% Licence.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'GPIA_JPRED';
  ip.addRequired('gp_array', @iscell);
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addOptional('xt', [], @(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))))
  ip.addParamValue('yt', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('predcf', [], @(x) isempty(x) || ...
                   isvector(x) && isreal(x) && all(isfinite(x)&x>0))
  ip.addParamValue('tstind', [], @(x) isempty(x) || iscell(x) ||...
                   (isvector(x) && isreal(x) && all(isfinite(x)&x>0)))
  if numel(varargin)==0 || isnumeric(varargin{1})
    % inputParser should handle this, but it doesn't
    ip.parse(gp_array, x, y, varargin{:});
  else
    ip.parse(gp_array, x, y, [], varargin{:});
  end
  xt=ip.Results.xt;
  yt=ip.Results.yt;
  z=ip.Results.z;
  zt=ip.Results.zt;
  predcf=ip.Results.predcf;
  tstind=ip.Results.tstind;
  if isempty(xt)
    xt=x;
    if isempty(tstind)
      if iscell(gp)
        gptype=gp{1}.type;
      else
        gptype=gp.type;
      end
      switch gptype
        case {'FULL' 'VAR' 'DTC' 'SOR'}
          tstind = [];
        case {'FIC' 'CS+FIC'}
          tstind = 1:size(x,1);
        case 'PIC'
          if iscell(gp)
            tstind = gp{1}.tr_index;
          else
            tstind = gp.tr_index;
          end
      end
    end
    if isempty(yt)
      yt=y;
    end
    if isempty(zt)
      zt=z;
    end
  end

  % pass these forward
  options=struct();
  if ~isempty(ip.Results.yt);options.yt=ip.Results.yt;end
  if ~isempty(ip.Results.z);options.z=ip.Results.z;end
  if ~isempty(ip.Results.zt);options.zt=ip.Results.zt;end
  if ~isempty(ip.Results.predcf);options.predcf=ip.Results.predcf;end
  if ~isempty(ip.Results.tstind);options.tstind=ip.Results.tstind;end

  if nargout > 2 && isempty(yt)
    pyt = NaN;
  end

  nGP = numel(gp_array);

  for i=1:nGP
    P_TH(i,:) = gp_array{i}.ia_weight;
  end

  % Make predictions with different models in gp_array
  for j = 1:nGP
    if isempty(yt)
      [Eft_grid(j,:), Covft_grid(:,:,j)]=gp_jpred(gp_array{j},x,y,xt,options);
    else
      [Eft_grid(j,:), Covft_grid(:,:,j), ljpyt_grid(j), Eyt_grid(j,:), Covyt_grid(:,:,j)]=gp_jpred(gp_array{j},x,y,xt, options);
    end
  end

  % Sample from the combined predictive distribution
  N = 100; % Sample size per gp in gp_array
  pft = zeros(N*size(Eft_grid,1), 1);
  ft = [];
  for j = 1 : size(Eft_grid,1)
    ftt = repmat(Eft_grid(j,:),N,1)+(chol(Covft_grid(:,:,j))'*randn(size(Covft_grid(:,:,j),1),N))';
    ft = [ft; ftt];
  end

  % Calculate mean and variance of the distributions
  Eft = mean(ft)';
  Covft = cov(ft);

  % Calculate jpyt with weight given in P_TH.
  if nargout > 2
    if ~isempty(yt)
      ljpyt = log(sum(exp(ljpyt_grid)'.*P_TH));
    else
      error('yt must be provided to get ljpyt');
    end
  end

  if nargout > 3
    Eyt = sum(Eyt_grid.*repmat(P_TH,1,size(Eyt_grid,2)),1);
    Covyt = zeros(size(Covyt_grid));
    for i = 1:size(Covyt_grid,3)
      Covyt(:,:,i) = Covyt_grid(:,:,i).*P_TH(i) + diag((Eyt_grid(i,:) - Eyt)).^2;
    end
    Covyt = sum(Covyt,3);
  end

  if nargout > 6
    for j=1:size(Eft_grid,1)
      for i=1:N
        index = N*(j-1)+i;
        pft(index,:) = mnorm_pdf(ft(index,:), Eft_grid(j,:), Covft_grid(:,:,j));
      end
    end
    % Normalize distributions
    pft = bsxfun(@rdivide,pft,sum(pft,1));
  end


function F = ctranspose( F )
%CTRANSPOSE   Conjugate transpose of a DISKFUNV.
%    F = CTRANSPOSE(F) is the same as F'. The orientation of the DISKFUNV
%    is transposed with complex conjugation.
%
%    Since all DISKFUNV objects are real-valued, this command is the same
%    as TRANSPOSE(F)
%
% See also DISKFUNV/CTRANSPOSE

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

F = transpose( F );
F = conj( F );

end


function h = cutfunc(x,a,b)
c1 = (x <= a);
c2 = (x>a & x<b);
c3 = (x >= b);
h = 0*c1+ (x-a)/(b-a).*c2 + 1*c3;

% for i=2:nx
%
%     h(1,i)= (x(1,i)-1)/(-1);
%
%     h(1,1)=1;
% end


function cuboidhyp_omap = generate_cuboid_from_omap(omapmore, vp, OMAP_FACTOR)

[depthorder reglabel regori rpo] = omap_depthorder(omapmore, vp, OMAP_FACTOR);
cuboidhyp_omap = omapobj2cuboidhyp(rpo, reglabel, regori, vp, OMAP_FACTOR);


%  meandpth  finds the average depth for a predefined running
%  window in terms of number of events, and a selected step
%  and plots the results.
%                                                     R.Z. 6/94
%                          Operates on newcat
%
report_this_filefun(mfilename('fullpath'));

% Find out of figure already exists
%
[existFlag,figNumber]=figure_exists('Mean Depth2',1);
newDep2WindowFlag=~existFlag;

% Set up the Seismicity Map window Enviroment
%
if newDep2WindowFlag

    figure_w_normalized_uicontrolunits(...
        'Name','Mean Depth2',...
        'visible','off',...
        'NumberTitle','off', ...
        'MenuBar','none', ...
        'Color',[ 1 1 1], ...
        'NextPlot','new', ...
        'Units','Pixel',  'Position',[wex wey 550 400'])
    depfg2 = gcf;
    hold on
    axis off
    matdraw

    uicontrol('Style','Pushbutton',...
        'Position',[.9 .80 .10 .05],...
        'Units','normalized',...
        'Callback','sta = ''ast'';medispas1','String','AS');
    uicontrol('Style','Pushbutton',...
        'Position',[.9 .70 .10 .05],...
        'Units','normalized',...
        'Callback','sta = ''lta'';medispas1','String','LTA');
    uicontrol('Style','Pushbutton',...
        'Position',[.9 .90 .10 .05],...
        'Units','normalized',...
        'Callback','dispma4','String','Com');

    new = uicontrol('style','edit','value',iwl,...
        'string',num2str(iwl), 'background','y',...
        'Callback','iwl=str2num(get(new,''String''));''String'',num2str(iwl);medispas1',...
        'units','norm','pos',[.90 .30 .10 .06],'min',0.1,'max',100);

    newlabel = uicontrol('style','text','units','norm','pos',[.85 .30 .05 .06]);
    set(newlabel,'string','iwl:','background',color_fbg);

    uicontrol('Units','normal',...
        'Position',[.90 .25 .08 .06],'String','Go',...
         'Callback','medispas1')

end  % if figure exist

figure_w_normalized_uicontrolunits(depfg2)
delete(gca);delete(gca);delete(gca);
set(gca,'visible','off');

%orient tall
set(gcf,'Units','centimeter','PaperPosition',[1 1 8 7])
rect = [0.15, 0.15, 0.65, 0.30];
axes('position',rect)
p5 = gca;

% plot errbar
%errorbar(xt2,meand,er)
%plot(xt2,meand,'co')
%for i = 1:length(xt2)
%boxutil(-me(:,i),1,xt2(i),0.5, 'r.',1,1.5);
%hold on
%end

pl = plot(xt2,meand,'-k')
set(pl,'LineWidth',1.0)
hold on
pl = plot(xt2,meand,'ok')
set(pl,'LineWidth',1.0,'MarkerSize',6)
if isempty(maepi) == 0
    pl =   plot(maepi(:,3),-maepi(:,7),'xk');
    set(pl,'LineWidth',2.0)
end

axis([min(xt2) max(xt2+0.5) min([meand*1.1 ])  max([meand*0.9 ])])
v = axis;
grid
xlabel('Time (years)','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m,'Color','k')
ylabel('Mean Depth (km)','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m,'Color','k')
stro = [' ' file1 '; ' num2str(iwln) ' / ' num2str(step)];
title2(stro,'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m,'Color','k')

set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)

hold off

set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)

set(gca,'visible','on');
set(gcf,'visible','on');

ic = 1;


% compute recall and viewpoint accuracy
% modified from PASCAL3D+ VDPM/compute_recall_precision_accuracy.m
% added an input param: prediction filename
% changed a few relative path names
function [recall, precision, accuracy, ap, aa] = compute_recall_precision_accuracy_3dview(cls, prediction_filename, show_curve, ARP)

BASE_DIR = fullfile(mfilename('fullpath'),'../');
addpath(fullfile(BASE_DIR, '../'));
global_variables;

% backward compatibility
if nargin < 3
    show_curve = 1;
end
if nargin < 4
    ARP = 0;
end

%azimuth_interval = [0 (360/(vnum_test*2)):(360/vnum_test):360-(360/(vnum_test*2))];

% viewpoint annotation path
%path_ann_view = '../Annotations';
path_ann_view = fullfile(g_pascal3d_root_folder, 'Annotations');

% read ids of validation images
addpath(fullfile(g_pascal3d_root_folder, 'VDPM'));
addpath(fullfile(g_pascal3d_root_folder, 'PASCAL/VOCdevkit/VOCcode'));
VOCinit;
%pascal_init;
ids = textread(sprintf(VOCopts.imgsetpath, 'val'), '%s');
M = numel(ids);

% open prediction file
%filename = sprintf('data/%s_%d_test_flip.mat', cls, vnum_train);
filename = prediction_filename;
object = load(filename);
dets_all = object.dets;

% load pre-packaged records (from extract_records.m)
% this step greatly accelerates testing.
try
    object = load(fullfile(BASE_DIR, 'voc12val_records.mat'));
catch
    run(fullfile(BASE_DIR, 'extract_records.m'));
    object = load(fullfile(BASE_DIR, 'voc12val_records.mat'));
end
voc12val_records = object.voc12val_records;

energy = [];
correct = [];
correct_view = [];
overlap = [];
count = zeros(M,1);
num = zeros(M,1);
num_pr = 0;
for i = 1:M
    fprintf('%s %d/%d\n', cls, i, M);
    % read ground truth bounding box
    rec = voc12val_records{i}; %PASreadrecord(sprintf(VOCopts.annopath, ids{i}));
    clsinds = strmatch(cls, {rec.objects(:).class}, 'exact');
    diff = [rec.objects(clsinds).difficult];
    clsinds(diff == 1) = [];
    n = numel(clsinds);
    bbox = zeros(n, 4);
    for j = 1:n
        bbox(j,:) = rec.objects(clsinds(j)).bbox;
    end
    count(i) = size(bbox, 1);
    det = zeros(count(i), 1);

    % read ground truth viewpoint
    if isempty(clsinds) == 0
        filename = fullfile(path_ann_view, sprintf('%s_pascal/%s.mat', cls, ids{i}));
        object = load(filename);
        record = object.record;
        view_gt = zeros(n, 3);
        for j = 1:n
            viewpoint = record.objects(clsinds(j)).viewpoint;
            if record.objects(clsinds(j)).viewpoint.distance == 0
                azimuth = viewpoint.azimuth_coarse;
                elevation = viewpoint.elevation_coarse;
                tilt = viewpoint.theta;
            else
                azimuth = viewpoint.azimuth;
                elevation = viewpoint.elevation;
                tilt = viewpoint.theta;
            end
            %view_gt(j) = find_interval(azimuth, azimuth_interval);
            view_gt(j,:) = [azimuth, elevation, tilt];
        end
    else
        view_gt = [];
    end

    % get predicted bounding box
    dets = dets_all{i};
    num(i) = size(dets, 1);
    % for each predicted bounding box
    for j = 1:num(i)
        num_pr = num_pr + 1;
        energy(num_pr) = dets(j, 8); %dets(j, 6);
        bbox_pr = dets(j, 1:4);
        view_pr = dets(j, 5:7); %find_interval((dets(j, 5) - 1) * (360 / vnum_train), azimuth_interval);

        % compute box overlap
        if isempty(bbox) == 0
            o = box_overlap(bbox, bbox_pr);
            [maxo, index] = max(o);
            if maxo >= 0.5 && det(index) == 0
                overlap{num_pr} = index;
                correct(num_pr) = 1;
                det(index) = 1;
                % check viewpoint
                R_pr = angle2dcm(view_pr(1)/180*pi, view_pr(2)/180*pi, view_pr(3)/180*pi);
                R_gt = angle2dcm(view_gt(index,1)/180*pi, view_gt(index,2)/180*pi, view_gt(index,3)/180*pi);
                R_angle = norm(logm(R_pr' * R_gt)) / sqrt(2); % in [0,2pi]
                azimuth_angle = min(abs(view_pr(1) - view_gt(index,1)), 360-abs(view_pr(1) - view_gt(index,1))); % in degree
                if ARP && R_angle < pi/6  % ARP_pi/6
                    correct_view(num_pr) = 1;
                elseif ARP == 0 && azimuth_angle < 30 % AVP_pi/6
                    correct_view(num_pr) = 1;
                else
                    correct_view(num_pr) = 0;
                end
            else
                overlap{num_pr} = [];
                correct(num_pr) = 0;
                correct_view(num_pr) = 0;
            end
        else
            overlap{num_pr} = [];
            correct(num_pr) = 0;
            correct_view(num_pr) = 0;
        end
    end
end
overlap = overlap';

[threshold, index] = sort(energy, 'descend');
correct = correct(index);
correct_view = correct_view(index);
n = numel(threshold);
recall = zeros(n,1);
precision = zeros(n,1);
accuracy = zeros(n,1);
num_correct = 0;
num_correct_view = 0;
for i = 1:n
    % compute precision
    num_positive = i;
    num_correct = num_correct + correct(i);
    if num_positive ~= 0
        precision(i) = num_correct / num_positive;
    else
        precision(i) = 0;
    end

    % compute accuracy
    num_correct_view = num_correct_view + correct_view(i);
    if num_correct ~= 0
        accuracy(i) = num_correct_view / num_positive;
    else
        accuracy(i) = 0;
    end

    % compute recall
    recall(i) = num_correct / sum(count);
end

ap = VOCap(recall, precision);
fprintf('AP = %.4f\n', ap);

aa = VOCap(recall, accuracy);
fprintf('AA = %.4f\n', aa);

% draw recall-precision and accuracy curve
if show_curve
figure;
hold on;
plot(recall, precision, 'r', 'LineWidth',3);
plot(recall, accuracy, 'g', 'LineWidth',3);
xlabel('Recall');
ylabel('Precision/Accuracy');
tit = sprintf('Average Precision = %.1f / Average Accuracy = %.1f', 100*ap, 100*aa);
title(tit);
hold off;
end

function ind = find_interval(azimuth, a)

for i = 1:numel(a)
    if azimuth < a(i)
        break;
    end
end
ind = i - 1;
if azimuth > a(end)
    ind = 1;
end


function [ obj ] = vb_init( obj )
% Initialize posterior and auxiliary variabels for VB inversion of HUGE.
% Requires obj.dcm and obj.prior to be intialized.
%
% This is a protected method of the tapas_Huge class. It cannot be called
% from outside the class.
%

% Author: Yu Yao (yao@biomed.ee.ethz.ch)
% Copyright (C) 2019 Translational Neuromodeling Unit
%                    Institute for Biomedical Engineering,
%                    University of Zurich and ETH Zurich.
%
% This file is part of TAPAS, which is released under the terms of the GNU
% General Public Licence (GPL), version 3. For further details, see
% <https://www.gnu.org/licenses/>.
%
% This software is provided "as is", without warranty of any kind, express
% or implied, including, but not limited to the warranties of
% merchantability, fitness for a particular purpose and non-infringement.
%
% This software is intended for research only. Do not use for clinical
% purpose. Please note that this toolbox is under active development.
% Considerable changes may occur in future releases. For support please
% refer to:
% https://github.com/translationalneuromodeling/tapas/issues
%

% method for gradient calculation
obj.options.fncBold = @bold_grad_cd;

%% prior
prior = struct();
prior.alpha_0 = obj.prior.alpha_0;
prior.m_0 = obj.prior.m_0; % prior mean
prior.tau_0 = obj.prior.tau_0;
prior.nu_0 = max(obj.prior.nu_0, 1.5^obj.idx.P_c);
prior.nu_0 = min(prior.nu_0, double(realmax('single')));
% scaling matrix of inverse-Wishart
prior.S_0 = obj.prior.S_0*(prior.nu_0 - obj.idx.P_c - 1);
% prior mean of hemodynamic parameters
prior.mu_h = obj.prior.mu_h;
% prior Covariance of hemodynamic parameters
prior.Sigma_h = obj.prior.Sigma_h;
% prior inverse scale of observation noise (b_0 in Figure 1 of REF [1])
prior.b_0 = obj.prior.mu_lambda./obj.prior.s2_lambda;
% prior shape parameter of observation noise (a_0 in Figure 1 of REF [1])
prior.a_0 = obj.prior.mu_lambda.^2./obj.prior.s2_lambda;
% prior mean and covariance over confound coefficients
prior.m_beta_0 = obj.prior.m_beta_0;
prior.S_beta_0 = obj.prior.S_beta_0;

obj.prior = prior;

%% create posterior struct
obj.posterior = struct();
% cluster weights
obj.posterior.alpha = obj.prior.alpha_0;
obj.posterior.alpha(1) = obj.posterior.alpha(1) + obj.N;
% cluster parameters
obj.posterior.m = obj.options.start.clusters;
obj.posterior.tau = repmat(obj.prior.tau_0, obj.K, 1);
obj.posterior.tau(1) = obj.prior.tau_0 + obj.N;
obj.posterior.nu = repmat(obj.prior.nu_0, obj.K, 1);
obj.posterior.nu(1) = obj.prior.nu_0 + obj.N;
obj.posterior.S = repmat(obj.prior.S_0, 1, 1, obj.K);

% assigments
obj.posterior.q_nk = [ones(obj.N, 1),zeros(obj.N, obj.K - 1)];
% DCM parameters
obj.posterior.mu_n = obj.options.start.subjects;
Sigma = zeros(obj.idx.P_c + obj.idx.P_h);
Sigma(1:obj.idx.P_c, 1:obj.idx.P_c) = obj.prior.S_0/(obj.prior.nu_0 - obj.idx.P_c - 1);
Sigma(1:obj.idx.P_c, 1:obj.idx.P_c) = obj.prior.S_0;
Sigma(obj.idx.P_c+1:end, obj.idx.P_c+1:end) = obj.prior.Sigma_h;
obj.posterior.Sigma_n = repmat(Sigma, 1, 1, obj.N);
% noise
obj.posterior.b = repmat(obj.prior.b_0, obj.N, obj.R);
obj.posterior.a = repmat(obj.prior.a_0, obj.N, obj.R);
% confounds
if obj.options.confVar
    if obj.options.confVar > 1
        obj.options.confVar = obj.K;
    end
    obj.posterior.m_beta = repmat(obj.prior.m_beta_0, 1, obj.idx.P_c, ...
        obj.options.confVar);
    obj.posterior.S_beta = repmat(obj.prior.S_beta_0, 1, 1, obj.idx.P_c, ...
        obj.options.confVar);
else
    obj.posterior.m_beta = [];
    obj.posterior.S_beta = [];
end

%% auxiliary variables
obj.aux.epsilon = cell(obj.N, 1);   % residual
obj.aux.G       = cell(obj.N, 1);   % jacobian
if obj.options.confVar    % confounds
    % center and normalize confound
    obj.aux.x_n = [obj.data(:).confounds]';
    obj.aux.x_n = bsxfun(@minus, obj.aux.x_n, mean(obj.aux.x_n));
    tmp = std(obj.aux.x_n);
    tmp(tmp < eps) = 1;
    obj.aux.x_n = bsxfun(@rdivide, obj.aux.x_n, tmp);
    % confound-related auxiliary variabels
    obj.aux.x_n_2 = zeros(obj.M, obj.M, obj.N);
    tmp = zeros(obj.idx.P_c, obj.idx.P_c, obj.N);
    for n = 1:obj.N
        obj.aux.x_n_2(:,:,n) = obj.aux.x_n(n,:)'*obj.aux.x_n(n,:);
        tmp(:,:,n) = obj.aux.x_n(n,:)*obj.prior.S_beta_0*obj.aux.x_n(n,:)'...
            *eye(obj.idx.P_c);
    end
    obj.aux.Sigma_beta = repmat(tmp, 1, 1, 1, obj.options.confVar);
    obj.aux.Pi_beta_0 = inv(obj.prior.S_beta_0);
    obj.aux.Pi_m_beta_0 = obj.aux.Pi_beta_0*obj.prior.m_beta_0;
    obj.aux.ldS_beta = repmat(tapas_huge_logdet(obj.prior.S_beta_0), ...
        obj.idx.P_c, obj.options.confVar);
    obj.aux.mu_beta = zeros(obj.N, obj.idx.P_c, obj.options.confVar);
    for k = 1:obj.options.confVar
        obj.aux.mu_beta(:,:,k) = obj.aux.x_n*obj.posterior.m_beta(:,:,k);
    end
end

obj.aux.q_k = sum(obj.posterior.q_nk, 1)' + realmin;
obj.aux.ldS = repmat(tapas_huge_logdet(obj.prior.S_0), obj.K, 1);
obj.aux.ldSigma = repmat(tapas_huge_logdet(Sigma), obj.N ,1);
obj.aux.mu_prime_h = obj.prior.mu_h/obj.prior.Sigma_h;
obj.aux.Pi_h = inv(obj.prior.Sigma_h);
% nu_k*inv(S_k)
obj.aux.nu_inv_S = zeros(size(obj.posterior.S));
for k = 1:obj.K
    obj.aux.nu_inv_S(:,:,k) = obj.posterior.nu(k).*...
        inv(obj.posterior.S(:,:,k));
end
obj.aux.b_prime = zeros(obj.N, obj.R);

%% check stability of starting values
obj.aux.q_r = zeros(obj.N, 1);
for n = 1:obj.N
    obj.aux.q_r(n) = size(obj.data(n).bold, 1);
    obj = obj.options.fncBold(obj, n);
    assert(~(any(isnan(obj.aux.G{n}(:))) || any(isinf(obj.aux.G{n}(:)))), ...
        'TAPAS:HUGE:Init:Stability',...
        'Starting values for subject %u lead to instable DCM.', n);

    tmp = sum((obj.aux.G{n}*obj.posterior.Sigma_n(:,:,n)).*obj.aux.G{n}, 2);
    obj.aux.b_prime(n,:) = sum(obj.aux.epsilon{n}.^2 + ...
        reshape(tmp, obj.aux.q_r(n), obj.R), 1);
    obj.posterior.b(n,:) = obj.prior.b_0 + obj.aux.b_prime(n,:)/2;
    obj.posterior.a(n,:) = obj.prior.a_0 + obj.aux.q_r(n)/2;
end

%% initialize auxiliary variables
obj.aux.lambda_bar = obj.posterior.a./obj.posterior.b;

obj.aux.mu_k_c(1,:) = mean(obj.posterior.mu_n(:, 1:obj.idx.P_c), 1);
obj.aux.Sigma_k_c(:,:,1) = ...
    sum(obj.posterior.Sigma_n(1:obj.idx.P_c, 1:obj.idx.P_c, :), 3);

% offline part of negative free energy
F_off = -obj.K*(obj.idx.P_c*(obj.idx.P_c - 1)/4*log(pi) + ...
    sum(gammaln((obj.prior.nu_0-obj.idx.P_c+1:obj.prior.nu_0)/2)));
F_off = F_off +.5*obj.K*obj.prior.nu_0*tapas_huge_logdet(obj.prior.S_0);
F_off = F_off +.5*obj.K*obj.idx.P_c*log(obj.prior.tau_0);
F_off = F_off + gammaln(sum(obj.prior.alpha_0));
F_off = F_off - sum(gammaln(obj.prior.alpha_0));
F_off = F_off - gammaln(obj.N + sum(obj.prior.alpha_0));
F_off = F_off - obj.N*obj.R*gammaln(obj.prior.a_0);
F_off = F_off + sum(gammaln(obj.posterior.a(:)));
F_off = F_off + obj.N*obj.R*obj.prior.a_0*log(obj.prior.b_0);
F_off = F_off -.5*obj.N*tapas_huge_logdet(obj.prior.Sigma_h);
F_off = F_off -.5*obj.idx.P_c*tapas_huge_logdet(obj.prior.S_beta_0);
F_off = F_off +.5*obj.N*obj.idx.P_c*log(2);
F_off = F_off -.5*sum(obj.aux.q_r)*obj.R*log(2*pi);
F_off = F_off +.5*obj.N*(obj.idx.P_c + obj.idx.P_h);
F_off = F_off +.5*obj.idx.P_c*(obj.K*(1 + obj.prior.nu_0) + obj.N);
if obj.options.confVar
    F_off = F_off +.5*obj.M*obj.idx.P_c*obj.options.confVar;
end
obj.aux.F_off = F_off;

% evaluate negative free energy (VB)
obj.posterior.nfe = obj.vb_nfe( );

%% initialize trace
obj.trace = struct();
obj.trace.nfe = NaN;
obj.trace.nDcmUpdate = zeros(obj.N, 1);
obj.trace.nRetract   = zeros(obj.N,1);

end


function [report] = TrackDetect(Track, UAV)
%TRACKDETECT 判断航迹是否符合条件(一个agent的)

%% 无人机航迹检测

% 威胁检测
dim = UAV.PointDim;                                         % 仿真环境维度
M = [UAV.Menace.radar; UAV.Menace.other]; % 威胁区

Threat = cell(UAV.num, 1);                                 % 威胁结果树
Angle = cell(UAV.num, 1);                                  % 转角检测结果树
MiniTraj = cell(UAV.num, 1);                              % 最小航迹片段检测结果树
ProbPoint = cell(UAV.num, 1);                           % 问题点

L = cell(UAV.num, 1);                                          % 航迹片段树（累加结构）
Time = cell(UAV.num, 1);                                    % 到达各个点时间树

L_mt = 0;                                                               % 所有无人机航迹之和
totalTime = zeros(UAV.num, 1);                         % 所用时间
totalL = zeros(UAV.num, 1);                                % 每个无人机飞行距离

for i = 1 : UAV.num
      PointNum = UAV.PointNum(i);
      Judge = zeros(1, PointNum+1);
      L_i = zeros(1, PointNum+1);
      Time_i = zeros(1, PointNum+1);
      Angle_i = zeros(1, PointNum+1);
      Traj_i = zeros(1, PointNum+1);
      ProbPoint_i = zeros(1, PointNum+1);
      % 进行检测
      l = 0;
      V = Track.V(i);
      for k = 1 : PointNum
            % 前向检测
            P2 = Track.P{i}(:, k)' ;   % 转置成 1*dim
            if k == 1
                P1 = UAV.S(i, :);
                ZZ = P2 - P1;
                phi0 = atan2(ZZ(2), ZZ(1));   % 偏角检测
                phi1 = phi0;
                d_phi = phi1 - phi0;
                if dim > 2                                % 倾角检测
                    theta0 = atan(ZZ(3) / sqrt(ZZ(1)^2 + ZZ(2)^2));
                    theta1 = theta0;
                    d_theta = theta1 - theta0;
                else
                    d_theta = 0;
                end
            else
                P1 = Track.P{i}(:, k-1)' ;  % 转置成 1*dim
                ZZ = P2 - P1;
                phi1 = atan2(ZZ(2), ZZ(1));
                d_phi = phi1 - phi0;
                phi0 = phi1;
                if dim > 2
                    theta1 = atan(ZZ(3) / sqrt(ZZ(1)^2 + ZZ(2)^2));
                    d_theta = theta1 - theta0;
                    theta0 = theta1;
                else
                    d_theta = 0;
                end
            end

            [across, ~] = CheckThreat(P1, P2, M); % 威胁检测
            Judge(k) = across;

            dl = norm(P1 - P2);
            l = l + dl; % 累加距离
            t = l / V;  % 时间点
            L_i(k) = l;
            Time_i(k) = t;

            if abs(d_phi) > UAV.limt.phi(i, 2)  ||  abs(d_theta) > UAV.limt.theta(i, 2)
                Angle_i(k) = true;
            else
                Angle_i(k) = false;
            end

            if dl < UAV.limt.L(i, 1)
                Traj_i(k) = true;
            else
                Traj_i(k) = false;
            end

            ProbPoint_i(k) = Angle_i(k) | Traj_i(k) | Judge(k); % 问题点

            % 最后一段检测
            if k == PointNum
                P1 = UAV.G(i, :);
                [across, ~] = CheckThreat(P2, P1, M);
                Judge(k+1) = across;

                dl = norm(P1 - P2);
                l = l + dl;
                t = l / V;
                L_i(k+1) = l;
                Time_i(k+1) = t;

                ZZ = P1-P2;
                phi1 = atan2(ZZ(2), ZZ(1));
                d_phi = phi1 - phi0;
                if dim>2
                    theta1 = atan(ZZ(3) / sqrt(ZZ(1)^2 + ZZ(2)^2));
                    d_theta = theta1 - theta0;
                else
                    d_theta = 0;
                end

                if abs(d_phi) > UAV.limt.phi(i, 2)  ||  abs(d_theta) > UAV.limt.theta(i, 2)
                    Angle_i(k+1) = true;
                else
                    Angle_i(k+1) = false;
                end

                if dl < UAV.limt.L(i, 1)
                    Traj_i(k+1) = true;
                else
                    Traj_i(k+1) = false;
                end

                ProbPoint_i(k+1) = Angle_i(k+1) | Traj_i(k+1) | Judge(k+1);

            end
      end
      Threat(i) = {Judge};                % 检测结果（长度比点的数目多一）
      Angle(i) = {Angle_i};               % 转角
      MiniTraj(i) = {Traj_i};              % 航迹片段
      ProbPoint(i) = {ProbPoint_i}; % 问题点

      L(i) = {L_i};                 % 路径长度（累加）
      Time(i) = {Time_i};    % 时间（累加）
      L_mt = L_mt + l;           % 总长度
      totalTime(i) = t;            % 时间
      totalL(i) = l;                   % 长度
end

% 多无人机碰撞检测
d_safe = UAV.ds; % 安全距离
CollideTimes = 0; % 碰撞次数
for i = 2 : UAV.num
    PointNum_i = UAV.PointNum(i);
    for k = 1 : PointNum_i
        P1 = Track.P{i}(:, k)';  % K时刻 i 无人机位置
        t_i = Time{i, 1}(k);                  % K时刻 i 无人机时间
        for j = 1 : i-1
            PointNum_j = UAV.PointNum(j);
            flag = false; %
            % 搜索同一时刻的点
            for kj = 1 : PointNum_j
                if kj ==1
                    t_j_l = 0;
                    P_l =  UAV.S(j, :);
                else
                    t_j_l = Time{j}(kj-1);
                    P_l =  Track.P{j}(:, kj-1)';
                end
                t_j_r = Time{j}(kj);
                P_r =  Track.P{j}(:, kj)';

                if t_i <= t_j_r  &&  t_i >= t_j_l
                    flag = true;
                    P2 =  P_l + (t_i - t_j_l) / (t_j_r - t_j_l) * (P_r - P_l);% K时刻 J 无人机位置
                    % K时刻 j 无人机位置
                end
            end
            % ———————

            if flag  % 查找到P2位置，进行碰撞检测
                collide = CheckCollide(P1, P2, d_safe);
            else     % 没有P2位置，不会碰
                collide = false;
            end
            if collide
                CollideTimes = CollideTimes + 1;
            end
        end
    end
end

% 生成检测报告
report.L_mt = L_mt;                       %总行程之和
report.Threat = Threat;                 %受威胁的航迹点位置
report.AngleProb = Angle;            % 转角不满足的点
report.TrajProb = MiniTraj;           % 不满足最小航迹间隔的点
report.ProbPoint = ProbPoint;     % 有问题的点
report.L = totalL;                            %飞行距离
report.time = totalTime;                %飞行时间
report.col_times = CollideTimes;  %碰撞次数
end

%% 穿越威胁区检测
function [across, across_num] = CheckThreat(P1, P2, M)
    % 威胁区（球或圆区域，不适合圆柱区域）
    O = M(:,1:end-1);  % 圆心
    R = M(:, end);        % 半径

    % 检测线段是否穿过某个障碍区
    total = 0;
    for i = 1 : size(O, 1)
         a = norm(P1 - P2);
         b = norm(P2 - O(i, :));
         c = norm(P1 - O(i, :));
         % P1点是否在圆内
         if c < R(i)
             isHit = true;
         % P2点是否在圆内
         elseif b < R(i)
             isHit = true;
         % P1 P2都不在圆内
         else
             dim = size(O, 2);  % P1： 1*dim 维
             if dim < 3
                 % 平面情况
                 A = P1(2) - P2(2);
                 B = P2(1) - P1(1);
                 C = P1(1)*P2(2) - P2(1)*P1(2);
                 x = O(i, 1);
                 y = O(i, 2);
                 d = abs(A*x + B*y + C) / sqrt(A^2 + B^2);
             else
                 % 空间情况
                 PP = P2 - P1;
                 PO = O(i, :) - P1;
                 d = norm(cross(PP, PO)) / a;
             end
             % 距离判据
             if d >= R(i)
                 isHit = false;  % 不相交
             % 角度判据(距离满足条件时两个角都为锐角时相交)
             elseif d > 0
                 cosP1 = a^2 + c^2 - b^2 / (2*c*a);
                 cosP2 = a^2 + b^2 - c^2 / (2*b*a);
                 if cosP1 > 0  &&  cosP2 > 0
                     isHit = true;
                 else
                     isHit = false;
                 end
             % 两点在外，距离为0情况（共线情况）
             else
                 if a > b  &&  a > c
                    isHit = true;
                 else
                    isHit = false;
                 end
             end
         end

         if isHit
            total = total + 1; %总共碰撞几次
         end
    end

    if total > 0
        across = true;
    else
        across = false;          % 是否穿越禁区
    end
    across_num = total;     % 穿过禁区的个数
end

%% 碰撞检测
function [collide] = CheckCollide(P1, P2, d_safe)
    if norm(P1 - P2) >= d_safe
        collide = false;
    else
        collide = true;
    end
end


function [ output_data ] = normalize_complex_mem( input_data, ...
    DeltaKCOAPoly, dim1_coords_m, dim2_coords_m, ...
    weight_fun, oversample_rate, ...
    fftsign, dim )
%NORMALIZE_COMPLEX_MEM Normalize complex SAR data to bring to a consistent
%point in the processing chain
%
%    output_data = normalize_complex_mem(input_data, ...
%       DeltaKCOAPoly, dim1_coords_m, dim2_coords_m, ... % Deskew parameters
%       weight_fun, oversample_rate, ... % Deweighting paramters
%       fftsign, dim)
%
%       Parameter name    Description
%
%       input_data        Array of complex values to deskew
%       DeltaKCOAPoly     Polynomial that describes center of frequency
%                            support of data in the processing dimension.
%                            Described in the SICD design and exploitation
%                            document.  DeltaKCOAPoly of 0 means no deskew.
%       dim1_coords_m     Coordinate of each "row" in dimension 1.  Used
%                            only for deskew.
%       dim2_coords_m     Coordinate of each "column" in dimension 2.  Used
%                            only for deskew.
%       weight_fun        Description of weighting applied.  Either a
%                            function handle to a function that takes a
%                            single argument (number of elements) and
%                            produces the weighting to apply, or a vector
%                            that is the weighting function sampled.  An
%                            empty weight_fun means to apply no
%                            deweighting.
%       oversample_rate   Amount of sampling beyond the ImpRespBW in the
%                            processing dimension.  (Nyquist sampling = 1).
%                            Used only for deweighting.
%       fftsign           FFT sign in the processing dimension (-1 or +1).
%       dim               Dimension over which to perform normalization
%       output_data       INPUT_DATA with normalization applied
%
% Author: Wade Schwartzkopf, NGA/R
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

output_data = input_data;
if any(DeltaKCOAPoly(:)~=0) % Do we need to center frequency support?
    output_data = deskewmem(input_data, DeltaKCOAPoly, dim1_coords_m, dim2_coords_m, dim, fftsign);
end
if fftsign==1 % -1 is the norm
    output_data = conj(output_data);
    % This transform will maintain the magnitude of the data and make the
    % IFFT behave like an FFT (and vice versa) in magnitude only:
    % abs(x)==abs(conj(x)) and
    % abs(fft(x))==abs(ifft(conj(x))), since fft(x)==conj(ifft(conj(x)))
end
if ~isempty(weight_fun) % Empty array means uniform weighting
    output_data = deweightmem(output_data, weight_fun, oversample_rate, dim);
end

end

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


function [inference] = tapas_linear_prepare_inference(data, model, inference)
%%
%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

nc = numel(model.graph{2}.htheta.T);
np = size(data.u, 1);
nb = size(data.u{1}, 2);

inferece.kernel = cell(4, 1);

inference.kernel{2} = struct('k', [eye(nb)], 's', [0.5]);
inference.kernel{3} = struct('k', [eye(nb)], 's', [0.5]);

end


% Setting An Initial Pressure Gradient Example
%
% This example demonstrates how to set an initial pressure gradient using
% kspaceSecondOrder. It builds on the Comparison Of Modelling Functions
% Example.
%
% author: Bradley Treeby
% date: 28th October 2010
% last update: 17th October 2011
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave 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 Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

clear all;

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 64;            % number of grid points in the x (row) direction
Ny = 64;            % number of grid points in the y (column) direction
dx = 0.1e-3;        % grid point spacing in the x direction [m]
dy = 0.1e-3;        % grid point spacing in the y direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy);
k = kgrid.k;

% define the properties of the propagation medium
medium.sound_speed = 1500;  % [m/s]
medium.alpha_power = 1.5;   % [dB/(MHz^y cm)]
medium.alpha_coeff = 0.75;  % [dB/(MHz^y cm)]

% set plot_frames to true to produce the plots given in the documentation
% set plot_frames to false to visualise the propagation of the pressure
% field
plot_frames = false;
if plot_frames
    % calculate and plot the pressure at a particular value of t
    kgrid.t_array = [0 1000]*1e-9;
else
    % calculate the plot the progression of the pressure field with t
    dt = 5e-9;
    t_end = 5e-6;
    kgrid.t_array = 0:dt:t_end;
end

% create source distribution using makeDisc
disc_magnitude = 4; % [Pa]
disc_x_pos = 25;    % [grid points]
disc_y_pos = 40;    % [grid points]
disc_radius = 4;    % [grid points]
disc_1 = disc_magnitude*makeDisc(Nx, Ny, disc_x_pos, disc_y_pos, disc_radius);

disc_magnitude = 3; % [Pa]
disc_x_pos = 40;    % [grid points]
disc_y_pos = 25;    % [grid points]
disc_radius = 3;    % [grid points]
disc_2 = disc_magnitude*makeDisc(Nx, Ny, disc_x_pos, disc_y_pos, disc_radius);

source_distribution = disc_1 + disc_2;

% define a single sensor point
sensor.mask = zeros(Nx, Ny);
sensor.mask(Nx/2, Ny/2) = 1;

% define the input arguments
input_args = {'PlotFrames', plot_frames, 'MeshPlot', true, 'PlotScale', [0 3], 'ExpandGrid', true};

% assign the source distribution to the initial pressure
source.p0 = source_distribution;
kspaceSecondOrder(kgrid, medium, source, sensor, input_args{:});

% assign the source distribution to the initial pressure gradient
source = rmfield(source, 'p0');
source.dp0dt = 5e6*source_distribution;
kspaceSecondOrder(kgrid, medium, source, sensor, input_args{:});

% assign the source distribution to both the initial pressure and the
% initial pressure gradient
source.p0 = source_distribution;
kspaceSecondOrder(kgrid, medium, source, sensor, input_args{:});


function [ result ] = export_df3( filename, chg )
%EXPORT_DF3 Export volumetric data as a DF3 file.
%   status = export_df3(filename,chg) exports the volumetric data contained
%   the 3 dimensional array chg as a DF3 file. This format is compatible
%   with the POV-Ray ray-tracing package. The data is scaled up by a factor
%   of 1,000,000.

    fid = fopen(filename, 'w');
    if fid==-1
        error(['Error opening ' filename]);
    end
    fwrite(fid, size(chg), 'ushort',0,'ieee-be');
    fwrite(fid, 1e6*chg, 'ulong',0,'ieee-be');
    result = fclose(fid);

end


function X = power(X,d)
% POWER (overloaded)

% Vectorize x if d is vector
if numel(X)==1 & (numel(d)>1)
    X = X.*ones(size(d));
end
% Vectorize if x is a vector
if numel(d)==1 & (numel(X)>1)
    d = d.*ones(size(X));
end
s = size(X);
if isa(X,'sdpvar')
    X = sdpvar(X);
else
    X = X(:);
end
d = reshape(d,[],1);
X = power(X,d);
X = reshape(X,s);


function f = uminus(f)
%UMINUS   Unary minus of a SINGFUN.
%   UMINUS(F) is the negative of F.
%
% See also UPLUS, MINUS.

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

% Negate the smooth part:
f.smoothPart = -f.smoothPart;

end


%%  SVM FOR FAULT DIAGNOSIS
% Script containing the SVM models of the various faults, and the measures data you wish to test,
% and get results of the fault diagnosis method.By Nassim laouti, Date 13.11.2010.
% Latest modification 16.02.2012.
%%

clear all
close all
clc

global st1 st2 st3 st5 st6 st7 st8 st9 st10 st11 st12 st13 st14 st15 st16 st17 st18 theta

%% Parameters
% Constants using in the preprocessing block
c3=10;c4=1000;wr0=1;wg0=100;omega_n=11.11;xi=0.6;P_r=4.8e6;
T1=0.6;T2=0.02;T3=0.06;T4=0.08;T5=0.04;

% Simple time and time
Ts=1/100
Time=Ts:Ts:4400;

%% Load the data_test that contains all measures and the controller output
% data_test=[v_hub_m;omega_r_m1;omega_r_m2;omega_g_m1;omega_g_m2;Tau_g_m;P_g_m;Beta_1_m1;Beta_1_m2;Beta_2_m1;Beta_2_m2;Beta_3_m1;Beta_3_m2;Tau_g_r;Beta_r]

load data_test_3;
measures=measures';
wr0=measures(1,3);wg0=measures(1,5);

%% Load SVM models of the various faults
% Load the svm model of the sensor fault beta1m1 type fixed value
load model_cap1_beta1m1

% Load the svm model of the sensor fault beta1m2 type fixed value
load model_cap1_beta1m2

% Load the svm model of the sensor fault beta2 type gain factor
load model_cap_beta2_t2

% Load the svm model of the sensor fault omega_r_m1 type fixed value
load model_cap1_wr1

% Load the svm model of the sensor fault omega_r_m2 type fixed value
load model_cap1_wr2

% Load the svm model of the sensor fault omega_r_m2 type gain factor
load model_cap_wr2_t2

% Load the svm model of the sensor fault omega_r_m1 type gain factor
load model_cap_wr1_t2

% Load the svm model of the sensor fault omega_g_m1 type fixed value
load model_cap1_wg1

% Load the svm model of the sensor fault omega_g_m2 type fixed value
load model_cap1_wg2

% Load the svm model of the sensor fault omega_g_m2 type gain factor
load model_cap_wg2_t2

% Load the svm model of the sensor fault omega_g_m1 type gain factor
load model_cap_wg1_t2

% Load the svm model of the actuator fault Tau_g type offset
load model_act_couple

% Load the svm model of the system fault
load model_system

% Load the svm model of the sensor fault omega_g type gain factor
load model_cap_wg_t2

% Load the svm model of the sensor fault omega_r type gain factor
load model_cap_wr_t2

% Load the svm model of the sensor fault beta2m2 type gain factor
load model_cap_beta2m2_t2

% Load the svm model of the sensor fault beta2m2 type gain factor
load model_cap_beta2m1_t2

%% Simulation

 open_system('FDI_SVM_WT.mdl');

 tic
 sim('FDI_SVM_WT.mdl',Time(end));
 toc


function display_result_progress(cv, results_to_display, display_progress)

 % Displays the current mean and stdev when a single resample result is left out for different result types
 %  to give an idea whether the algorithm has converged to a stable decoding accuracy
 %  (these values could be used for stopping criteria, rather than running a fixed number of resampling runs)

%==========================================================================

%     This code is part of the Neural Decoding Toolbox.
%     Copyright (C) 2011 by Ethan Meyers (emeyers@mit.edu)
%
%     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/>.

%==========================================================================

i = 1;
num_training_times = 0;
while num_training_times(1) == 0
    if i ~= 4
        num_training_times = size(results_to_display{i}, 3);
        num_test_times = size(results_to_display{i},4);
    else
        num_training_times = size(results_to_display{i}, 4);
        num_test_times = size(results_to_display{i}, 5);
    end
    i = i + 1;
end

 % default behavior if there is only 1 training point and display_progress.training_time_to_display_results == -1, is to just use this one training time for the results
 if (display_progress.training_time_to_display_results == -1)  && (num_training_times == 1)
     display_progress.training_time_to_display_results = 1;
 end

 % display_progress.training_time_to_display_results should be set if the num_training_times ~= num_test_times
 if (display_progress.training_time_to_display_results == -1) && (num_training_times~= num_test_times) && (num_test_times ~= 1)

     warning(['If the number of training times does not equal the number of test times, and one wants to display the resampled CV progress, ' ...
                'then display_progress.training_time_to_display_results should be set.  Using the default value of display_progress.training_time_to_display_results = 1, ' ...
                'to display the results here.'])

     display_progress.training_time_to_display_results = 1;

 end

current_results = [];

 for iResult = 1:length(results_to_display)

     iResample = size(results_to_display{1}, 1);

     if iResample == 0
         continue;
     end

    % if a particular training time that has been specified to display the results
    % note: if only saving results when training and testing at the same time period, then the displayed results will only be for one time point
    if display_progress.training_time_to_display_results > 0

        if iResult ~= 4
           curr_bootstrap_progress_results = squeeze(mean(results_to_display{iResult}(:, :, display_progress.training_time_to_display_results, :), 2));
        else
           curr_bootstrap_progress_results = squeeze(mean(mean(results_to_display{iResult}(:, :, :, display_progress.training_time_to_display_results, :), 3), 2));
        end

        if iResample == 1
           curr_bootstrap_progress_results = curr_bootstrap_progress_results';
        end

    else

        % if only saving the results for training and testing at the same time
        if ((length(size(results_to_display{iResult})) == 3) && (iResult ~=4)) || ((length(size(results_to_display{iResult})) == 4)  && (iResult == 4))

            if iResult ~=4
                 curr_bootstrap_progress_results = squeeze(mean(results_to_display{iResult}, 2));
            else
                 curr_bootstrap_progress_results = squeeze(mean(mean(results_to_display{iResult}, 3), 2));
            end

            if iResample == 1
               curr_bootstrap_progress_results = curr_bootstrap_progress_results';
            end

        else

             for iDisplayResultTrainingTime = 1:num_training_times

                 if iResult ~= 4
                    curr_bootstrap_progress_results(:, iDisplayResultTrainingTime) = squeeze(mean(results_to_display{iResult}(:, :, iDisplayResultTrainingTime, iDisplayResultTrainingTime), 2));
                 else
                    curr_bootstrap_progress_results(:, iDisplayResultTrainingTime)  = squeeze(mean(mean(results_to_display{iResult}(:, :, :, iDisplayResultTrainingTime, iDisplayResultTrainingTime), 3), 2));
                 end
             end

        end

    end

   if iResample == 1
       current_results = [current_results; curr_bootstrap_progress_results; NaN .* ones(size(curr_bootstrap_progress_results))];   % can't show cofficients of variation if only 1 bootstrap has been run (so skip the following part)
   else

       curr_total_mean = mean(curr_bootstrap_progress_results, 1);
       curr_one_bootstrap_left_out_mean = ((repmat(curr_total_mean, [iResample 1]) - (curr_bootstrap_progress_results./iResample)) .* (iResample./(iResample -1)))';   % means over bootstraps with the ith bootstrap result left out
       curr_stdev_one_bootstrap_left_out = std(curr_one_bootstrap_left_out_mean');  % stdev over results when one bootstrap sample is left out

       current_results = [current_results; curr_total_mean; curr_stdev_one_bootstrap_left_out; NaN .* ones(size(curr_total_mean))];

   end

 end

fprintf('\n')
disp(current_results)
fprintf('\n')


function ui = mrViewOverlay(ui);
%
%  ui = mrViewOverlay(ui);
%
% Compute overlays for each image,  combine with
% colormap and threshold settings to produce a
% set of True-Color images to display in
% ui.display.images.
%
% ras 07/05.
if ~exist('ui', 'var') | isempty(ui),  ui = mrViewGet; end
if ishandle(ui),     ui = get(ui, 'UserData');         end

%%%% get needed params
s = ui.settings.space;
b = ui.settings.bounds;
slices = ui.display.slices;
oris = ui.display.oris;
method = ui.settings.mapInterp;

% preference for how to combine multiple overlays:
% 1) average two colors: [1 0 0] + [.2 0 1] => [.6 0 .5];
% 2) add and saturate: [1 0 0] + [.2 0 1] => [1 0 1];
% 3) opaque: [1 0 0] + [.2 0 1] => [.2 0 1]; % last one on top
if ispref('VISTA', 'alphaMethod')
    alphaMethod = getpref('VISTA', 'alphaMethod');
else
    alphaMethod = 1; % average
end

%%%%% get a list of which overlays to show (ignoring ones that are
%%%%% hidden
isHidden = [ui.overlays.hide];
overlayList = find( ~isHidden );
if isempty(overlayList),  return; end

%%%%% loop across overlays,  computing / displaying each in turn
for i = 1:length(ui.display.images)

    imgSz = size(ui.display.images{i});

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % compute each overlay image,  mask of where to show it %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    overlay = []; mask = logical([]);
    for o = overlayList
        ii = find(overlayList==o);
        m = ui.overlays(o).mapNum;

        %%%%% get map volume for this timepoint
        vol = ui.maps(m).data;
        if ndims(vol)>3
            t = ui.overlays(o).subVol;
            vol = vol(:,:,:,t);
        end

        % get the coords for this slice in the base data space
        imgCoords = mrViewGet(ui, 'DataCoords', i);

        % restrict coords to those which pass the threshold
        % of this overlay
        [coords, imageMask] = mrViewRestrict(ui, imgCoords, o, method);

        % keep track of the pixels which pass threshold
        mask(:,:,ii) = logical(reshape(imageMask, imgSz));

        % xform coords into map data coordinates
        C = inv(ui.maps(m).baseXform) * [coords; ones(1, size(coords, 2))];

        % get the values for the overlay map
        sz = size(vol);
        if(method(1)=='n')
            vals = myCinterp3(vol,[sz(1) sz(2)], sz(3), round(C([2,1,3],:)'), 0.0);
        elseif(method(1)=='l')
            vals = myCinterp3(vol,[sz(1) sz(2)], sz(3), C([2,1,3],:)', 0.0);
        else
            vals = interp3(vol, C(2,:), C(1,:), C(3,:), method);
        end

        % store vals in overlay matrix
        overlaySlice = zeros(imgSz);
        overlaySlice(imageMask) = vals;
        overlay(:,:,ii) = overlaySlice;

        % compute clip vals for overlay
        if isequal(ui.overlays(o).clim, 'auto'),  clim = [min(vals) max(vals)];
        else,                                     clim = ui.overlays(o).clim;
        end

        % update colorbar's color limits, label, units
        ui.overlays(o).cbar.clim = clim;
		ui.overlays(o).cbar.label = ui.maps(m).name;
		ui.overlays(o).cbar.units = ui.maps(m).dataUnits;
    end

    % set NaN values to 0
    mask(isnan(overlay)) = false;
    overlay(isnan(overlay)) = 0;

    % compute # of overlays present @ each pixel (for transparency)
    overlaysPerPixel = sum(double(mask), 3);

    % find indices of pixels w/ any overlays
    ok = find(overlaysPerPixel>0);

    % initialize those parts of image to be black:

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % plug in overlays,  taking into account overlap %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % initialize underlay image: black out parts where
    % overlays will be,  make truecolor
    img = rescale(ui.display.images{i}, ui.settings.clim, [1 256]);
    img(ok) = 0; img = ind2rgb(img, ui.settings.cmap);

    for o = overlayList
        ii = find(overlayList==o);
        nColors = size(ui.overlays(o).cbar.cmap, 1);

        % get pixel mask for this overlay
        M = mask(:,:,ii);

        if any(M(:)>0)
            % use overlay data to form an index into the color map:
            vals = overlay(:,:,ii);
            vals = vals(M);
            I = rescale(vals, ui.overlays(o).cbar.clim, [1 nColors]);

            % manually plug values in to each color channel of image
            % (use the alphaMethod pref to determine how to combine
            % multiple overlays)
            for ch = 1:3
                tmp = img(:,:,ch);
                switch alphaMethod
                    case 1,   % standard alpha -- average colors
                        W = 1 ./ overlaysPerPixel(M>0); % weights
                        tmp(M) = tmp(M) + ui.overlays(o).cbar.cmap(I,ch) .* W;
                    case 2,   % add channels and saturate
                        tmp(M) = tmp(M) + ui.overlays(o).cbar.cmap(I,ch);
                        tmp(M) = min(tmp(M), 1);
                    case 3,   % opaque overlays
                        tmp(M) = ui.overlays(o).cbar.cmap(I,ch);
                end
                img(:,:,ch) = tmp;
            end
        end
    end

    ui.display.images{i} = img;
    clear overlay mask
end

return

% % attempt to use alpha map for display
%         % set so that other overlays can peek through
%         mask = uint8(255 .* (1/length(ui.overlays)) .* mask);
%         % combine color map and overlay values to make
%         % truecolor image
%         img = ind2rgb(img, ui.overlays(o).cmap);
%        % display image
%         otherDims = setdiff(1:3, ui.display.oris(i));
%         xx = b(otherDims(2), 1):b(otherDims(2), 2);
%         yy = b(otherDims(1), 1):b(otherDims(1), 2);
%         h = image(xx, yy, img, 'AlphaData', mask, ...
%             'AlphaDataMapping', 'none', ...
%             'Parent', ui.display.axes(i));
%         ui.overlays(o).imgHandle(i) = h;

% old comments for above:
% Compute and display overlay images in a mrViewer UI.
%
% NOTE: The method used to display overlay images involves
% using matlab alpha channels,  which makes it more difficult
% to export these images outside matlab. Moreover,  to overlay
% multiple maps which each have different color maps,  ok've found
% it necessary to convert each image to a true color image.
% Because of the increased memory requirements -- 3 color planes
% plus an alpha mask per overlay image -- ok've decided to
% combine the computation and display steps into one function.
%
% If greater use is made of 'hidden' views,  which don't use
% the mrViewer UI but use the structs,  then this should probably
% be rewritten to keep them separate. As well,  it would be probably
% be better to do away with the use of alpha channels,  and just
% produce a single truecolor image for underlay + all overlays.
% Failing that,  it's probably simpler and more elegant to use
% alpha channels.
%


function [Forecasteval]=bvarfeval(data_endo_c,data_endo_c_lags,data_exo_c,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,const,n,p,k,It,Bu,beta_gibbs,sigma_gibbs,forecast_record,forecast_estimates,names,endo,pref)

% function []=bvarfeval(data_endo_c,data_endo_c_lags,data_exo_c,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,const,n,p,k,It,Bu,beta_gibbs,sigma_gibbs,forecast_estimates,names,endo,datapath)
% calculates, display and saves forecast evaluation results for a BVAR model
% inputs:  - matrix 'data_endo_c': matrix of endogenous data for the forecast evaluation period (i.e. period for which forecast is estimated and actual data exists)
%          - matrix 'data_endo_c_lags': lagged endogenous values to serve as initial conditions to the forecast evaluation period
%          - matrix 'data_exo_c': matrix of predicted data for the forecast period
%          - cell 'stringdates3': date strings for the forecast evaluation period (i.e. period for which forecast is estimated and actual data exists)
%          - string 'Fstartdate': start date of the forecasts
%          - string 'Fcenddate': end date of the forecat evaluation (i.e. period for which forecast is estimated and actual data exists)
%          - integer 'Fcperiods': number of periods for which forecast evaluation can be conducted (i.e.for which forecast is estimated and actual data exists)
%          - integer 'Fcomp': 0-1 value indicating if forecast evaluation is possible
%          - integer 'const': 0-1 value to determine if a constant is included in the model
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'It': total number of iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'Bu': number of burn-in iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - matrix 'beta_gibbs': record of the gibbs sampler draws for the beta vector
%          - matrix'sigma_gibbs': record of the gibbs sampler draws for the sigma matrix (vectorised)
%          - cell 'forecast_estimates': lower bound, point estimates, and upper bound for the unconditional forecasts
%          - cell 'names': cell containing the excel spreadsheet labels (names and dates)
%          - cell 'endo': list of endogenous variables of the model
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: none

% changed ad 2017-01-06 to store results
Forecasteval.RMSE=[];
Forecasteval.MAE=[];
Forecasteval.MAPE=[];
Forecasteval.Ustat=[];
Forecasteval.CRPS_estimates=[];
Forecasteval.S1_estimates=[];
Forecasteval.S2_estimates=[];

% first, note that forecast evaluation can only be conducted if there is some observable data after the beginning of the forecast
if Fcomp==1

   % preliminary task: obtain a matrix of forecasts over the common periods
   for ii=1:n
   forecast_c(:,ii)=forecast_estimates{ii,1}(2,1:Fcperiods)';
   end
   % then compute the matrix of forecast errors
   ferrors=data_endo_c-forecast_c;

   % compute first the sequential RMSE, from (1.8.11)

   % square the forecast error matrix entrywise
   sferrors=ferrors.^2;
   % sum entries sequentially
   sumsferrors=sferrors(1,:);
   for ii=2:Fcperiods
   sumsferrors(ii,:)=sumsferrors(ii-1,:)+sferrors(ii,:);
   end
   % divide by the number of forecast periods and take square roots to obtain RMSE
   for ii=1:Fcperiods
   Forecasteval.RMSE(ii,:)=((1/ii)*sumsferrors(ii,:)).^0.5;
   end

   % compute then the sequential MAE, from (1.8.12)

   % take the absolute value of the forecast error matrix
   absferrors=abs(ferrors);
   % sum entries sequentially
   sumabsferrors=absferrors(1,:);
   for ii=2:Fcperiods
   sumabsferrors(ii,:)=sumabsferrors(ii-1,:)+absferrors(ii,:);
   end
   % divide by the number of forecast periods to obtain MAE
   for ii=1:Fcperiods
   Forecasteval.MAE(ii,:)=(1/ii)*sumabsferrors(ii,:);
   end

   % compute the sequential MAPE, from (1.8.13)

   % divide entrywise by actual values and take absolute values
   absratioferrors=abs(ferrors./data_endo_c);
   % sum entries sequentially
   sumabsratioferrors=absratioferrors(1,:);
   for ii=2:Fcperiods
   sumabsratioferrors(ii,:)=sumabsratioferrors(ii-1,:)+absratioferrors(ii,:);
   end
   % divide by 100*(number of forecast periods) to obtain MAPE
   for ii=1:Fcperiods
   Forecasteval.MAPE(ii,:)=(100/ii)*sumabsratioferrors(ii,:);
   end

   % compute the Theil's inequality coefficient, from (1.8.14)

   % first compute the left term of the denominator
   % square entrywise the matrix of actual data
   sendo=data_endo_c.^2;
   % sum entries sequentially
   sumsendo=sendo(1,:);
   for ii=2:Fcperiods
   sumsendo(ii,:)=sumsendo(ii-1,:)+sendo(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   leftterm(ii,:)=((1/ii)*sumsendo(ii,:)).^0.5;
   end
   % then compute the right term of the denominator
   % square entrywise the matrix of forecast values
   sforecasts=forecast_c.^2;
   % sum entries sequentially
   sumsforecasts=sforecasts(1,:);
   for ii=2:Fcperiods
   sumsforecasts(ii,:)=sumsforecasts(ii-1,:)+sforecasts(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   rightterm(ii,:)=((1/ii)*sumsforecasts(ii,:)).^0.5;
   end
   % finally, compute the U stats
   Forecasteval.Ustat=Forecasteval.RMSE./(leftterm+rightterm);

   % now compute the continuous ranked probability score

   % create the cell storing the results
   Forecasteval.CRPS=cell(n,1);
   % loop over endogenous variables
   for ii=1:n
      % loop over forecast periods on which actual data is known
      for jj=1:Fcperiods
      % compute the continuous ranked probability score
      score=bear.crps(forecast_record{ii,1}(:,jj),forecast_estimates{ii,1}(2,jj));
      Forecasteval.CRPS{ii,1}(1,jj)=score;
      end
   end
   Forecasteval.CRPS_estimates=(cell2mat(Forecasteval.CRPS))';

   % finally, consider the big piece: the computation of the log predictive score
   % this part implements algorithm a.8.1, described p 115 of technical guide
   % details of the procedure can be found p 111-115 of the technical guide

   % preliminary task: if there is a constant in the model, concatenate a column of ones to data_exo_c
   if const==1
   data_exo_c=[ones(Fcperiods,1) data_exo_c];
   end

   % create the cells storing the results
   S1=cell(n,1);
   S2=cell(n,1);

   for ll=1:It-Bu
      % step 2: draw beta and sigma
      beta=beta_gibbs(:,ll);
      sigma=reshape(sigma_gibbs(:,ll),n,n);

      % step 3: obtain mu, the mean vector, from (a.8.19)
      temp=bear.forecastsim(data_endo_c_lags,data_exo_c,beta,n,p,k,Fcperiods);
      mu=reshape(temp',n*Fcperiods,1);

      % step 4: obtain upsilon, the covariance matrix, from (a.8.22)
      % recover the A1,A2,... matrices by reshaping beta
      temp=reshape(beta,k,n);
      temp=(temp(1:p*n,:))';
      Amatrices=cell(1,p);
      for ii=1:p
      Amatrices{1,ii}=temp(:,n*(ii-1)+1:n*ii);
      end
      % initiate upsilon with upsilon_1,1
      upsilon=cell(Fcperiods,Fcperiods);
      upsilon{1,1}=sigma;
      % complete the first column using (a.8.23)
      for ii=2:Fcperiods
      upsilon_ij=Amatrices{1,1}*upsilon{ii-1,1};
      summ=min(ii-1,p);
         for jj=2:summ
         upsilon_ij=upsilon_ij+Amatrices{1,jj}*upsilon{ii-jj,1};
         end
      upsilon{ii,1}=upsilon_ij;
      end
      % complete the first row, using symmetry
      for ii=2:Fcperiods
      upsilon{1,ii}=upsilon{ii,1}';
      end
      % now take care of the other columns
      for jj=2:Fcperiods
      % first complete the variance matrix (the diagonal one), using (a.8.24)
      upsilon_jj=sigma;
      summ=min(jj-1,p);
         for kk=1:summ
         upsilon_jj=upsilon_jj+Amatrices{1,kk}*upsilon{jj-kk,jj};
         end
      upsilon{jj,jj}=upsilon_jj;
      % then complete the rest of the column, using (a.8.23)
         for ii=jj+1:Fcperiods
         % initiate upsilon_ij
         upsilon_ij=Amatrices{1,1}*upsilon{ii-1,jj};
         % continue the summation
         summ=min(ii-1,p);
            for kk=2:summ
            upsilon_ij=upsilon_ij+Amatrices{1,kk}*upsilon{ii-kk,jj};
            end
         upsilon{ii,jj}=upsilon_ij;
         end
         % complete the corresponding row, using symmetry
         for kk=jj+1:Fcperiods
         upsilon{jj,kk}=upsilon{kk,jj}';
         end
      end
      upsilon=cell2mat(upsilon);

      % step 5: obtain the predictive density
      % first consider scenario 1 (forecast at period T+i)
      % loop over variables
      for ii=1:n
         % loop over periods
         for jj=1:Fcperiods
         % define R
         R=zeros(1,n*Fcperiods);
         R(1,n*(jj-1)+ii)=1;
         % define the mean
         mean=R*mu;
         % define the variance
         covar=R*upsilon*(R');
         % define the actual value
         actual=data_endo_c(jj,ii);
         % determine the density
         [~,density]=bear.mndensity(actual,mean,covar,1);
         % record the result
         S1{ii,1}(ll,jj)=density;
         end
      end
      % then consider scenario 2 (forecast from beginning until period T+i)
      % loop over variables
      for ii=1:n
      R=[];
         % loop over periods
         for jj=1:Fcperiods
         % define R
         R=[R;zeros(1,n*Fcperiods)];
         R(jj,n*(jj-1)+ii)=1;
         % define the mean vector
         mean=R*mu;
         % define the covariance matrix
         covar=R*upsilon*(R');
         % define the vector of actual values
         actual=data_endo_c(1:jj,ii);
         % determine the density
         [~,density]=bear.mndensity(actual,mean,covar,jj);
         % record the result
         S2{ii,1}(ll,jj)=density;
         end
      end
   end

   % step 6: compute the log predictive score from (a.8.26)
   Forecasteval.S1_estimates=cell(n,1);
   Forecasteval.S2_estimates=cell(n,1);
   for ii=1:n
      for jj=1:Fcperiods
      Forecasteval.S1_estimates{ii,1}(1,jj)=log((1/(It-Bu))*sum(S1{ii,1}(:,jj)));
      Forecasteval.S2_estimates{ii,1}(1,jj)=log((1/(It-Bu))*sum(S2{ii,1}(:,jj)));
      end
   end
   Forecasteval.S1_estimates=(cell2mat(Forecasteval.S1_estimates))';
   Forecasteval.S2_estimates=(cell2mat(Forecasteval.S2_estimates))';

% if forecast evaluation is not possible, do not do anything
elseif Fcomp==0
end

% now, print the results and display them

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'at');

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

Fevalinfo='Forecast evaluation:';
fprintf('%s\n',Fevalinfo);
fprintf(fid,'%s\n',Fevalinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% if forecast evaluation is not possible, return a message to signal it

if Fcomp==0

finfo1=['Forecast evaluation cannot be conducted.'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);
finfo2=['Forecasts start in ' Fstartdate ', while observable data is available only until ' names{end,1} '.'];
fprintf('%s\n',finfo2);
fprintf(fid,'%s\n',finfo2);
finfo3=['To obtain forecast evaluation, the forecast start date must be anterior to the end of the data set.'];
fprintf('%s\n',finfo3);
fprintf(fid,'%s\n',finfo3);

% if forecast evaluation is possible, display the results
elseif Fcomp==1

finfo1=['Evaluation conducted over ' num2str(Fcperiods) ' periods (from ' Fstartdate ' to ' Fcenddate ').'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);

   % loop over endogenous variables
   for ii=1:n

   fprintf('%s\n','');
   fprintf(fid,'%s\n','');

   endoinfo=['Endogenous: ' endo{ii,1}];
   fprintf('%s\n',endoinfo);
   fprintf(fid,'%s\n',endoinfo);

   temp='fprintf(''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);
   temp='fprintf(fid,''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);

   label='RMSE:       ';
   values=Forecasteval.RMSE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAE:        ';
   values=Forecasteval.MAE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAPE:       ';
   values=Forecasteval.MAPE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Theil''s U:  ';
   values=Forecasteval.Ustat(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='CRPS:       ';
   values=Forecasteval.CRPS_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Log score 1:';
   values=Forecasteval.S1_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Log score 2:';
   values=Forecasteval.S2_estimates(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

    end

end

fclose(fid);


%% FUNCTION Least_Dirty
%   Dirty Multi-Task Learning with Least Squares Loss.
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   rho1: group sparsity regularization parameter
%   rho2: elementwise sparsity regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   C: constant parameters
%   P: group sparsity structure (joint feature selection)
%   Q: elementwise sparsity component
%   funcVal: function (objective) value vector.
%   lossVal: loss value for every iteration.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Pinghua Gong and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Jalali, A. and Ravikumar, P. and Sanghavi, S. and Ruan, C. A dirty
%       model for multi-task learning, NIPS 2010.
%

function [W, C, P, Q, funcVal, lossVal] = Logistic_Dirty(X, Y, rho1, rho2, opts)

if nargin <4
    error('\n Inputs: X, Y, rho1, should be specified!\n');
end
X = multi_transpose(X);

if nargin <5
    opts = [];
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];
lossVal = [];

%initialize a starting point
C0_prep = zeros(1, task_num);
for t_idx = 1: task_num
    m1 = nnz(Y{t_idx} == 1);
    m2 = nnz(Y{t_idx} == -1);
    if ( m1==0 || m2==0 )
        C0_prep(t_idx) = 0;
    else
        C0_prep(t_idx) = log(m1/m2);
    end
end

if opts.init==2
    P0 = zeros(dimension, task_num);
    Q0 = zeros(dimension, task_num);
    C0 = zeros(1, task_num);
elseif opts.init== 0
    P0 = randn(dimension, task_num);
    Q0 = randn(dimension, task_num);
    C0 = C0_prep;
elseif opts.init== 1
    if isfield(opts,'P0')
        P0=opts.P0;
        if (nnz(size(P0)-[dimension, task_num]))
            error('\n Check the input .P0');
        end
    else
        error('\n check opt.init');
    end

     if isfield(opts,'Q0')
        Q0=opts.Q0;
        if (nnz(size(Q0)-[dimension, task_num]))
            error('\n Check the input .Q0');
        end
    else
        error('\n check opt.init');
    end

    if isfield(opts,'C0')
        C0=opts.C0;
    else
        error('\n check opt.init');
    end
end

Pz= P0;
Qz= Q0;
Cz= C0;
Pz_old = P0;
Qz_old = Q0;
Cz_old = C0;

t = 1;
t_old = 0;
iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ps = (1 + alpha) * Pz - alpha * Pz_old;
    Qs = (1 + alpha) * Qz - alpha * Qz_old;
    Cs = (1 + alpha) * Cz - alpha * Cz_old;

    % compute function value and gradients of the search point
    [gWs, gCs, Fs ]  = gradVal_eval(Ps+Qs, Cs);

    % the Armijo Goldstein line search scheme
    while true
        Pzp = proximalL1infnorm(Ps - gWs/gamma, rho1 / gamma);
        Qzp = proximalL11norm(Qs - gWs/gamma, rho2 / gamma);

        Czp = Cs - gCs/gamma;
        Fzp = funVal_eval(Pzp+Qzp, Czp);

        delta_Pzp = Pzp - Ps;
        delta_Qzp = Qzp - Qs;
        delta_Czp = Czp - Cs;
        nm_delta_Pzp=norm(delta_Pzp, 'fro')^2;
        nm_delta_Qzp=norm(delta_Qzp, 'fro')^2;
        nm_delta_Czp=norm(delta_Czp, 'fro')^2;

        r_sum = (nm_delta_Pzp+nm_delta_Qzp+nm_delta_Czp)/2;

%         Fzp_gamma = Fs + sum(sum((delta_Pzp+delta_Qzp).* gWs))...
%             + sum(sum(delta_Czp .* gCs))...
%             + gamma/2 * norm(delta_Pzp + delta_Qzp, 'fro')^2 ...
%             + gamma/2 * nm_delta_Czp;

        Fzp_gamma = Fs + sum(sum((delta_Pzp+delta_Qzp).* gWs))...
            + sum(sum(delta_Czp .* gCs))...
            + gamma/2 * (nm_delta_Pzp +nm_delta_Qzp) ...
            + gamma/2 * nm_delta_Czp;

        if (r_sum <=1e-20)
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Pz_old = Pz;
    Qz_old = Qz;
    Cz_old = Cz;
    Pz = Pzp;
    Qz = Qzp;
    Cz = Czp;

    funcVal = cat(1, funcVal, Fzp + rho1*L1infnorm(Pzp) + rho2*L11norm(Qzp));
    lossVal = cat(1, lossVal, Fzp);

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

P=Pzp;
Q=Qzp;
W = Pzp+Qzp;
C = Czp;

% private functions

    function [grad_W, grad_C, funcVal] = gradVal_eval(W, C)
        grad_W = zeros(dimension, task_num);
        grad_C = zeros(1, task_num);
        lossValVect = zeros (1 , task_num);
        for i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
        end
        % here when computing function value we do not include
        % l1 norm.
        funcVal = sum(lossValVect);
    end

    function [funcVal] = funVal_eval (W, C)
        funcVal = 0;
        for i = 1: task_num
            funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
        end
        % here when computing function value we do not include
        % l1 norm.
    end

end

function [ grad_w, grad_c, funcVal ] = unit_grad_eval( w, c, x, y)
    %gradient and logistic evaluation for each task
    m = length(y);
    weight = ones(m, 1)/m;
    weighty = weight.* y;
    aa = -y.*(x'*w + c);
    bb = max( aa, 0);
    funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
    pp = 1./ (1+exp(aa));
    b = -weighty.*(1-pp);
    grad_c = sum(b);
    grad_w = x * b;
end

function [ funcVal ] = unit_funcVal_eval( w, c, x, y)
    %function value evaluation for each task
    m = length(y);
    weight = ones(m, 1)/m;
    aa = -y.*(x'*w + c);
    bb = max( aa, 0);
    funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
end


function sonorobot_keyboard(r,ns,ds,mapoff)
if nargin<4
    mapoff = false;
    if nargin<3
        ds = .1;
        if nargin<2
            ns = 24;
            if nargin<1;
                r = .37;
            end
        end
    end
end
f_h = figure('name','Sono Robot','numbertitle','off','menubar','none');
set(f_h,'keypressfcn',@robot_move_fcn)
uicontrol(f_h,'style','toggle','position',[505,5,50,25],...
    'string','Quit','callback','close');
msg1 = uicontrol(f_h,'style','text','position',[15,5,480,55]);
msg2 = uicontrol(f_h,'style','text','position',[15,65,480,25],...
    'string','Measurement results...','fontsize',11.5,...
    'horizontal','left');
hold on
axisx = 12.5;
axisy = 10;
axis([0 axisx 0 axisy]);
axis off
%Input walls & robot position
%Environment
nmax = 50;
stpmax = 100;
segset = zeros(nmax,4);%line segment set:[Ax,Ay,Bx,By]
areax = 1;
areay = .5;
dftarea = [axisx-areax,axisy-areay;axisx-areax,axisy;axisx,axisy;axisx,axisy-areay];
okarea = [axisx-areax,axisy-2*areay;axisx-areax,axisy-areay;axisx,axisy-areay;axisx,axisy-2*areay];
fill(okarea(:,1),okarea(:,2),'k')
fill(dftarea(:,1),dftarea(:,2),'w')
stt = true;%true: on, ready to insert; false: off, waiting for the 2nd point
polygon = false;%%
n = 0;%number of segments
%debug%
mdldisp = ['A_1';'A_2';'A_3';'B_1';'B_2';'B_3';'B_4';'B_5';'B_6';'C  ';...
    'D_1';'D_2';'D_3';'D_4';'E  ';'nil';'DK '];
%--%
while n <= nmax
    %display current operation
    if n == 0
        set(msg1,'string',['Insert the first edge. Or click the white '...
            'area and load default data.'],'fontsize',11.5,'horizontal','left');
    else
        if stt
            set(msg1,'string',['Insert a new line: The ',orderdisp(n+1),...
                ' edge. First end point. Or click the black area to finish.'],...
                'fontsize',11.5,'horizontal','left');
        else
            set(msg1,'string',['Insert a new line: The ',orderdisp(n),...
                ' edge. Second end point.'],'fontsize',11.5,...
                'horizontal','left');
        end
    end
    %ginput
    [x,y] = ginput(1);
    if (x-(axisx-areax))*(x-axisx)<0&&(y-(axisy-2*areay))*(y-(axisy-areay))<0 ||...
           ((x-(axisx-areax))*(x-axisx) < 0&&(y-(axisy-areay))*(y-axisy)<0&&n == 0)
        break
    end
    plot(x,y,'ro')
    if stt
        n = n+1;
        segset(n,1:2) = [x,y];
    else
        segset(n,3:4) = [x,y];
        plot(segset(n,[1,3]),segset(n,[2,4]),'b');
    end
    stt = ~stt;
end
if n ~= nmax
    segset = segset(1:n,:);
end

%default map
dftmap = [2,2,2,7;2,7,3,8;3,8,8,8;8,8,8,2;8,2,4,2;4,2,4,6;4,6,6,6;6,6,6,4];
if n == 0
    segset = dftmap;
    n = 8;
end

%Robot
% display current operation
set(msg1,'string','Set original position of the robot. Or click the white area and load default data.',...
    'fontsize',11.5,'horizontal','left');
[rx,ry] = ginput(1);
if (rx-(axisx-areax))*(rx-axisx)<0&&(ry-(axisy-areay))*(ry-axisy)<0
    rx = 5;
    ry = 5;
end
plot(rx,ry,'ro')
a = 2*pi/ns;

%Probing & Moving
set(msg1,'string','Looking for exit...','fontsize',11.5,'horizontal','left');
rtrack = zeros(stpmax,2);rtrack(1,:) = [rx,ry];
stp = 0;
    function robot_move_fcn( ~,evt)
        switch evt.Key
            case {'w','uparrow'}
                ry = ry+ds;
            case {'s','downarrow'}
                ry = ry-ds;
            case {'a','leftarrow'}
                rx = rx-ds;
            case {'d','rightarrow'}
                rx = rx+ds;
        end
        rxo = rx;
        ryo = ry;
        if approachwall(segset,[rx,ry],2*r)
            set(msg1,'string','Approaching wall...',...
                'fontsize',11.5,'horizontal','left');
        else
            set(msg1,'string','Searching exit...',...
                'fontsize',11.5,'horizontal','left');
        end
        [istouch,~] = approachwall(segset,[rx,ry],r);
        if istouch
            set(msg1,'string','Robot has touched wall... Move back.',...
                'fontsize',11.5,'horizontal','left');
            rx = rxo;
            ry = ryo;
        end
        if ~istouch
            stp = stp+1;
            if stp>stpmax
                rtrack = [rtrack;zeros(stpmax,2)];
                stpmax = 2*stpmax;
            end
            rtrack(stp,:) = [rx,ry];
            %reprobe
            rs = sonomeasure(segset,[rx,ry],polygon,ns);
            [modelpt,mptadd,finished,mdl] = sonomodel(rs,r);%!!
            %redraw
            cla
            % environment
            if ~mapoff
                for k = 1:n
                    plot(segset(k,[1,3]),segset(k,[2,4]),'--r','LineWidth',2)
                end
            end
            % probing data
            for k = 1:ns
                plot(rx+rs(k)*cos(((k-1):.1:k)*a),ry+rs(k)*sin(((k-1):.1:k)*a),'b:')
            end
            plot(rx,ry,'ro');%text(rx-.15,ry-.15,'Robot');
            plot(rx+r*cos((0:.1:2)*pi),ry+r*sin((0:.1:2)*pi),'b','LineWidth',1.45);
            % model report
            mdlrpt = [0 0];
            for k = 1:ns
                if finished(k)
                    plot(rx+modelpt(k,[1,3]),ry+modelpt(k,[2,4]),'k','LineWidth',1.5)
                    switch mdl(k)
                        case {1,2,3}
                            plot(rx+(rs(k)-.2)*cos((k-1:.1:k)*a),ry+(rs(k)-.2)*sin((k-1:.1:k)*a),'c','LineWidth',2.5);
                            mdlrpt(1) = 1;
                        case {4,5,6,7,8,9}
                            plot(rx+(rs(k)-.2)*cos((k-1:.1:k)*a),ry+(rs(k)-.2)*sin((k-1:.1:k)*a),'g','LineWidth',2.5);
                            mdlrpt(2) = 1;
                    end
                end
            end
            for k = 1:size(mptadd,1)
                plot(rx+mptadd(k,[1,3]),ry+mptadd(k,[2,4]),'k','LineWidth',1.5)
            end
            if mdlrpt(1)&&mdlrpt(1)
                set(msg2,'string','Faultages found...',...
                    'fontsize',11.5,'horizontal','left');
            elseif mdlrpt(1)
                set(msg2,'string','Faultages on two sides...',...
                    'fontsize',11.5,'horizontal','left');
            elseif mdlrpt(2)
                set(msg2,'string','Faultages on one side...',...
                    'fontsize',11.5,'horizontal','left');
            else
                set(msg2,'string','Measurement results...',...
                    'fontsize',11.5,'horizontal','left');
            end
            % robot track
            plot(rtrack(1:stp,1),rtrack(1:stp,2),'b:')
            %debug%
            % report model
            for k = 1:ns
                text(rx+rs(k)*cos((k-.5)*a),ry+rs(k)*sin((k-.5)*a),mdldisp(mdl(k),:))
            end
        end
    end

end


function [unfoldMesh, nFaces] = mfmBuildSubMesh2(mesh, perimeterEdges, insideNodes, orderedUniquePerimeterPoints)
% Create a portion of the mesh from a larger mesh
%
%  [unfoldMesh, nFaces] = ...
%      mfmBuildSubMesh2(mesh, perimeterEdges, insideNodes, ...
%                orderedUniquePerimeterPoints);
%
% This routine begins with the original large mesh and extracts a
% topologically correct sub-mesh based on the perimeter edges and inside
% nodes.
%
% See also:  unfoldMeshFromGUI
%

% Connection matrix of internal points
unfoldMesh.connectionMatrix = mesh.connectionMatrix(insideNodes,insideNodes);

% Copy mesh dta to unfoldMesh.  Note the name change.
unfoldMesh.normal        = mesh.normals(:,insideNodes)';
unfoldMesh.uniqueVertices= mesh.vertices(:,insideNodes)';
unfoldMesh.uniqueCols    = mesh.colors(:,insideNodes)';
unfoldMesh.dist          = mesh.dist(insideNodes);
unfoldMesh.triangles     = mesh.triangles(:,insideNodes)';

% We need to get uniqueFaceIndexList for the unfoldMesh
indicesOfFacesInSubGroup = findFacesInGroup2(mesh,insideNodes);
% subGroupFaces    = mesh.uniqueFaceIndexList(indicesOfFacesInSubGroup,:);
subGroupFaces    = mesh.triangles(:,indicesOfFacesInSubGroup)';

nFaces = size(subGroupFaces,1);

% Get a lookup table for converting indices into the full node array into
% indices to the unfold mesh nodes.
lookupTable = zeros(meshGet(mesh,'nVertices'),1);
lookupTable(insideNodes) = 1:length(insideNodes);

% Use the lookup table to convert our list of face indices so that they index into unfoldMesh.uniqueVertices.
sgf = lookupTable(subGroupFaces(:)+1);
unfoldMesh.uniqueFaceIndexList = reshape(sgf,nFaces,3);

% Convert the edges to feed into orderMeshPerimeterPoints
fullEdgePointList=perimeterEdges(:);

% How many edges do we have?
[numEdges,x]=size(perimeterEdges); %#ok<NASGU>

newEdges=zeros((numEdges*2),1);

for t=1:(numEdges*2)
    newEdges(t) = find(insideNodes==fullEdgePointList(t));
end

% newEdges = reshape(newEdges,numEdges,2);

% Find the perimeter points in the sub mesh.
unfoldMesh.orderedUniquePerimeterPoints = ...
    zeros(length(orderedUniquePerimeterPoints),1);

for t=1:length(orderedUniquePerimeterPoints)
    unfoldMesh.orderedUniquePerimeterPoints(t) = ...
        find(insideNodes==orderedUniquePerimeterPoints(t));
end

return;


% Demo for Edge Boxes (please see readme.txt first).

%% load pre-trained edge detection model and set opts (see edgesDemo.m)
model=load('models/forest/modelBsds'); model=model.model;
model.opts.multiscale=0; model.opts.sharpen=2; model.opts.nThreads=4;

%% set up opts for edgeBoxes (see edgeBoxes.m)
opts = edgeBoxes;
opts.alpha = .65;     % step size of sliding window search
opts.beta  = .75;     % nms threshold for object proposals
opts.minScore = .01;  % min score of boxes to detect
opts.maxBoxes = 1e4;  % max number of boxes to detect

%% detect Edge Box bounding box proposals (see edgeBoxes.m)
I = imread('peppers.png');
tic, bbs=edgeBoxes(I,model,opts); toc

%% show evaluation results (using pre-defined or interactive boxes)
gt=[122 248 92 65; 193 82 71 53; 410 237 101 81; 204 160 114 95; ...
  9 185 86 90; 389 93 120 117; 253 103 107 57; 81 140 91 63];
if(0), gt='Please select an object box.'; disp(gt); figure(1); imshow(I);
  title(gt); [~,gt]=imRectRot('rotate',0); gt=gt.getPos(); end
gt(:,5)=0; [gtRes,dtRes]=bbGt('evalRes',gt,double(bbs),.7);
figure(1); bbGt('showRes',I,gtRes,dtRes(dtRes(:,6)==1,:));
title('green=matched gt  red=missed gt  dashed-green=matched detect');

%% run and evaluate on entire dataset (see boxesData.m and boxesEval.m)
if(~exist('boxes/VOCdevkit/','dir')), return; end
split='val'; data=boxesData('split',split);
nm='EdgeBoxes70'; opts.name=['boxes/' nm '-' split '.mat'];
edgeBoxes(data.imgs,model,opts); opts.name=[];
boxesEval('data',data,'names',nm,'thrs',.7,'show',2);
boxesEval('data',data,'names',nm,'thrs',.5:.05:1,'cnts',1000,'show',3);


function [res]=tt_mm(mat1,mat2)
%Matrix-by-matrix product in TT1.0 format
%   [RES]=TT_MM(MAT1,MAT2) Matrix MAT1 by matrix MAT2 product in the
%   TT-format. Please avoid its usage: it will be removed in
%   future releases. Use * operator from the object-oriented version.
%
%
% TT-Toolbox 2.2, 2009-2011
%
%This is TT Toolbox, written by Ivan Oseledets, Olga Lebedeva
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
d=size(mat1,1);
res=cell(d,1);
k=1;
for s=1:2
n1=size(mat1{k},1);
m1=size(mat1{k},2);
rm=size(mat1{k},3);
n2=size(mat2{k},1);
m2=size(mat2{k},2);
rv=size(mat2{k},3);
%mat1{1} is (n1 x m1 x rm , mat2{1} is n2xm2xrm, result is n1xm2xrv1*rv)
%res{1}=reshape(permute(mat1{1},[1,3,2],[n1*rm1]),m1)*reshape(mat2{1},[n2,m2*rv2]);
res{k}=reshape(permute(mat1{k},[1,3,2]),[n1*rm,m1])*reshape(mat2{k},[n2,m2*rv]);
%res{1} is n1 x rm x m2 x rv
res{k}=reshape(res{k},[n1,rm,m2,rv]);
res{k}=permute(res{k},[1,3,2,4]);
res{k}=reshape(res{k},[n1,m2,rm*rv]);
if ( k == 1 )
 k=d;
end
end
for i=2:d-1
  n1=size(mat1{i},1);
  m1=size(mat1{i},2);
  rm1=size(mat1{i},3);
  rm2=size(mat1{i},4);
  n2=size(mat2{i},1);
  m2=size(mat2{i},2);
  rv1=size(mat2{i},3);
  rv2=size(mat2{i},4);
  %mat1{i} is n1 x m1 x rm1 x rm2
  %mat2{i} is n2 x m2 x rv1 x rv2
  %n1 x rm1 x rm2 x m1 * n2 x m2 x rv1 x rv2
  %n1 x rm1 x rm2 x m2 x rv1 x rv2
  %Need: n1 x m2 x rm1 x rv1 x rm2 x rv2
  %Permut: 1 4 2 5 3 6
  res{i}=reshape(permute(mat1{i},[1,3,4,2]),[n1*rm1*rm2,m1])*reshape(mat2{i},[n2,m2*rv1*rv2]);
  res{i}=reshape(res{i},[n1,rm1,rm2,m2,rv1,rv2]);
  res{i}=permute(res{i},[1,4,2,5,3,6]);
  res{i}=reshape(res{i},[n1,m2,rm1*rv1,rm2*rv2]);
end
return
end


%script cleanf1
if (~I(i,j))&(~I(i,j-1))...
    &I(i,j+1)&I(i-1,j)...
    &I(i-1,j+1)&(~I(i+1,j))...
    &I(i+1,j-1)&I(i+1,j+1)
  I(i,j)=1;
end