function montage = spm_eeg_montage_ui(montage)
% GUI for EEG montage (rereference EEG data to new reference channel(s))
% FORMAT montage = spm_eeg_montage_ui(montage)
%
% montage     - structure with fields:
%   tra       - MxN matrix
%   labelnew  - Mx1 cell-array - new labels
%   labelorg  - Nx1 cell-array - original labels
%
% Output is empty if the GUI is closed.
%__________________________________________________________________________
% Copyright (C) 2008-2017 Wellcome Trust Centre for Neuroimaging

% Jean Daunizeau
% $Id: spm_eeg_montage_ui.m 7755 2019-12-16 13:19:28Z spm $

% Create the figure
%--------------------------------------------------------------------------
fig  = figure;
S0   = spm('WinSize','0',1);
pos  = get(fig,'position');
pos2 = [40 70 pos(3)-60 pos(4)-100];
pos  = [S0(1) S0(2) 0 0] + [pos(1) pos(2) 1.8*pos(3) pos(4)];
set(fig,...
    'menubar',     'none',...
    'position',    pos,...
    'numbertitle', 'off',...
    'name',        'Montage edition');

addButtons(fig);

% Display the uitable component
%--------------------------------------------------------------------------
table    = cat(2,montage.labelnew(:),num2cell(montage.tra));
colnames = cat(2,'channel labels',montage.labelorg(:)');

pause(1e-1) % This is weird, but fixes java troubles.
ht       = my_uitable(table,colnames);
set(ht,'position',pos2, 'units','normalized');

% Display the matrix representation of the montage
%--------------------------------------------------------------------------
ax = axes('position',[0.6 0.18 0.4 0.77]);
hi = imagesc(montage.tra,'parent',ax);
axis('image');
colormap('bone');
zoom(fig,'on');

% Store info in figure's userdata and wait for user interaction
%--------------------------------------------------------------------------
ud.hi      = hi;
ud.ht      = ht;
ud.montage = montage;
set(fig,'userdata',ud);
uiwait(fig);

% Get the montage from the GUI
%--------------------------------------------------------------------------
try
    ud = get(fig,'userdata');
    montage = ud.montage;
    close(fig);
catch % GUI was closed without 'OK' button
    montage = [];
end

%==========================================================================
function doAddRow(obj,evd,h)
%==========================================================================
% 'add row' button subfunction
ud = get(h,'userdata');
[M,newLabels] = getM(ud.ht);
M = [M;zeros(1,size(M,2))];
newLabels = cat(1,newLabels(:),num2str(size(M,1)));
set(ud.ht,'units','pixels');
pos = get(ud.ht,'Position');
delete(ud.ht);
table = cat(2,newLabels,num2cell(M));
colnames = cat(2,'channel labels',ud.montage.labelorg(:)');
pause(1) % This is weird, but fixes java troubles.
ht = my_uitable(table,colnames);
set(ht,'position',pos, 'units','normalized');
ud.ht = ht;
set(h,'userdata',ud);
doCheck(obj,evd,h);

%==========================================================================
function doLoad(obj,evd,h)
%==========================================================================
% 'load' button subfunction
[t,sts] = spm_select(1,'mat','Load montage file');
if sts
    montage = load(t);
    if ismember('montage', fieldnames(montage))
        montage    = montage.montage;
        ud         = get(h,'userdata');
        set(ud.ht,'units','pixels');
        pos        = get(ud.ht,'Position');
        delete(ud.ht);
        table      = cat(2,montage.labelnew(:),num2cell(montage.tra));
        colnames   = cat(2,'channel labels',montage.labelorg(:)');
        pause(1) % This is weird, but fixes java troubles.
        ht         = my_uitable(table,colnames);
        set(ht,'position',pos,...
            'units','normalized');
        ud.ht      = ht;
        ud.montage = montage;
        set(h,'userdata',ud);
        pause(1)
        doCheck(obj,evd,h);
    else
        spm('alert!','File did not contain any montage!','Montage edition');
    end
end

%==========================================================================
function doSave(obj,evd,h)
%==========================================================================
% 'save as' button subfunction
doCheck(obj,evd,h);
ud = get(h,'userdata');
[M,newLabels] = getM(ud.ht);
% delete row if empty:
ind              = ~any(M,2);
M(ind,:)         = [];
newLabels(ind)   = [];
montage.tra      = M;
montage.labelorg = ud.montage.labelorg;
montage.labelnew = newLabels;
[filename, pathname] = uiputfile({'*.mat','MAT-files (*.mat)'}, ...
    'Save montage', 'SPMeeg_montage.mat');
if ~isequal(filename, 0)
    save(fullfile(pathname, filename), 'montage', spm_get_defaults('mat.format'));
end

%==========================================================================
function doOK(obj,evd,h)
%==========================================================================
% 'OK' button subfunction
doCheck(obj,evd,h);
ud               = get(h,'userdata');
[M, newLabels]   = getM(ud.ht);
% delete row if empty:
ind              = ~any(M,2);
M(ind,:)         = [];
newLabels(ind)   = [];
montage.tra      = M;
montage.labelorg = ud.montage.labelorg(:);
montage.labelnew = newLabels(:);
ud.montage = montage;
set(h,'userdata',ud);
uiresume(h);

%==========================================================================
function doCheck(obj,evd,h)
%==========================================================================
% Update the montage display
ud = get(h,'userdata');
M  = getM(ud.ht);
set(ud.hi,'cdata',M);
set(gca,'xlim',[0.5 size(M,1)]);
set(gca,'ylim',[0.5 size(M,2)]);
axis('image');
drawnow;

%==========================================================================
function [M,newLabels] = getM(ht)
%==========================================================================
% extracting montage from java object
nnew = get(ht,'NumRows');
nold = get(ht,'NumColumns')-1;
M    = zeros(nnew,nold);
data = get(ht,'data');
for i =1:nnew
    if ~isempty(data(i,1))
        newLabels{i} = data(i,1);
    else
        newLabels{i} = [];
    end
    for j =1:nold
        if ~isempty(data(i,j+1))
            if ~ischar(data(i,j+1))
                M(i,j) = data(i,j+1);
            else
                M0 = str2double(data(i,j+1));
                if ~isempty(M0)
                    M(i,j) = M0;
                else
                    M(i,j) = 0;
                end
            end
        else
            M(i,j) = 0;
        end
    end
end

%==========================================================================
function addButtons(h)
%==========================================================================
% adding buttons to the montage GUI
hAdd = uicontrol('style','pushbutton',...
    'string','Add row','callback',{@doAddRow,h},...
    'position',[60 20 80 20]);
set(hAdd,'units','normalized');
hLoad = uicontrol('style','pushbutton',...
    'string','Load file','callback',{@doLoad,h},...
    'position',[180 20 80 20]);
set(hLoad,'units','normalized');
hSave = uicontrol('style','pushbutton',...
    'string','Save as','callback',{@doSave,h},...
    'position',[280 20 80 20]);
set(hSave,'units','normalized');
hOK = uicontrol('style','pushbutton',...
    'string',' OK ','callback',{@doOK,h},...
    'position',[400 20 80 20]);
set(hOK,'units','normalized');
hCheck = uicontrol('style','pushbutton',...
    'string',' Refresh display ','callback',{@doCheck,h},...
    'position',[760 20 120 20]);
set(hCheck,'units','normalized');

%==========================================================================
function [ht,hc] = my_uitable(varargin)
%==========================================================================
% conversion layer for various MATLAB versions
persistent runOnce
try
    if spm_check_version('matlab','8.4') >= 0
        if isempty(runOnce)
            warning('Consider migrating to the new uitable component.');
            runOnce = true;
        end
        [ht,hc] = uitable('v0',varargin{:});
    else
        [ht,hc] = spm_uitable(varargin{:});
    end
catch
    [ht,hc]     = deal([]);
end


function [model, ll] = gpsimMapUpdateF(model, options)

% GPSIMMAPUPDATEF Update posterior mean of f.
% FORMAT
% DESC updates the stored version of the posterior mean for f in
% the GPSIMMAP model.
% ARG model : the model for which f is to be updated.
% ARG options : options vector.
% RETURN model : the model with f updated.
% RETURN ll : the log likelihood after update.
%
% SEEALSO : gpsimMapFunctionalLogLikeGradient,
% gpsimMapFunctionalLogLikeHessian, gpsimMapCreate
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% MODIFIED : Pei Gao, 2008

% SHEFFIELDML

display = options(1);
tolf = options(2);
tol = options(3);
iters = options(14);
if iters == 0
    iters = 100;
end
f = gpsimMapFunctionalExtractParam(model);

if isfield(model, 'priorProtein') && ~isempty(model.priorProtein)
  model.consLambda = 5;
  maxLambda = 8;
  nCons = length(model.priorProtein);
else
  model.consLambda = 0;
  maxLambda = 1;
end

llold = gpsimMapFunctionalLogLikelihood(model);

for j = 1:maxLambda
  %fprintf('lambda = %d; display = %d\n', lambda, display);
  %if model.isConcave
  flag = 0;
  for i = 1:iters
    ll = llold - 1;
    gf = gpsimMapFunctionalLogLikeGradients(model);
    if model.consLambda
      consMat = zeros(size(model.invCovf));
      for k=1:nCons
        ftimeIndex = find((model.priorProteinTimes(k)-model.mapt)==0);
        consMat(ftimeIndex, ftimeIndex) = model.consLambda;
      end
      hf = inv(model.invCovf+consMat);
    else
      hf = model.covf;
    end

    new_drn = (hf*gf')';
    factor = 1;
    count = 0;
    fold = f;
    while ll < llold
        f = fold + factor*new_drn;
        model = gpsimMapFunctionalExpandParam(model, f);
        ll = gpsimMapFunctionalLogLikelihood(model);
        lldiff = ll - llold;
        count = count + 1;
        if count > 0 & display
          fprintf('gpsimMapUpdateF, lldiff: %2.4f, factor %2.4f\n', lldiff, ...
                  factor);
        end
        factor = factor/2;
        if lldiff < tol & max(abs(fold - f))<tolf
            flag = 1;
            break
        end
    end
    if display
      fprintf('Iteration %d, log likelihood %2.4f\n', i, ll);
    end
    llold = ll;
    if flag == 1
      break
    end
  end
  model.consLambda = model.consLambda*4;
end

if isempty(find(model.f))
  warning('TF is zero!');
end
%else
%  error('Not yet implemented non-concave update')
%end


function h = gp_plot(gp, x, y, varargin)
%GP_PLOT  Make a plot with Gaussian process
%
%  Description
%    GP_PLOT(GP, X, Y, XT, OPTIONS)
%    takes a GP structure together with matrix X of training
%    inputs and vector Y of training targets and plots predictions
%    evaluated at test inputs XT.
%
%    [EF, VARF, LPY, EY, VARY] = GP_PLOT(GP, X, Y, OPTIONS)
%    plots predictions evaluated at training inputs X.
%
%    The form of the plot depends on the dimensionality of X and options.
%      - 1D with Gaussian: mean and 5% and 95% quantiles of f and y
%      - 1D with non-Gaussian: median and 5% and 95% quantiles of mu or f
%      - 2D: Conditional predictions, see GP_CPRED + 2D plot of mu or f
%      - ND with N>2: Conditional predictions, see GP_CPRED
%
%    OPTIONS is optional parameter-value pair
%      target - option for choosing what is computed 'mu' (default)
%               or 'f'
%      normdata - a structure with fields xmean, xstd, ymean, and ystd
%               to allow plotting in the original data scale (see
%               functions normdata and denormdata)
%      xlabels - a cell array of covariate label strings
%      predcf - an 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.
%               Default 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 = []).
%      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.
%      fcorr  - Method used for latent marginal posterior corrections.
%               Default is 'off'. Possible methods are 'fact' for EP
%               and either 'fact' or 'cm2' for Laplace. If method is
%               'on', 'fact' is used for EP and 'cm2' for Laplace.
%
%    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 Ef1 = gp_plot(GP, X, Y, X, 'predcf', 1) and Ef2 =
%    gp_plot(gp, x, y, x, 'predcf', 2) should sum up to Ef =
%    gp_plot(gp, x, y, x). That is Ef = Ef1 + Ef2. With FULL model
%    this is true but with FIC and PIC this is true only
%    approximately. That is Ef \approx Ef1 + Ef2.
%
%    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_SET, GP_OPTIM, DEMO_REGRESSION*
%
% Copyright (c) 2014 Aki Vehtari

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

ip=inputParser;
ip.FunctionName = 'GP_PLOT';
ip.addRequired('gp',@(x) isstruct(x) || iscell(x));
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('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('xlabels', [], @(x) isempty(x) || iscell(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)))
ip.addParamValue('fcorr', 'off', @(x) ismember(x, {'off', 'fact', 'cm2', 'on'}));
ip.addParamValue('tr', 0.25, @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('target', 'mu', @(x) ismember(x,{'f','mu'}))
ip.addParamValue('normdata', struct(), @(x) isempty(x) || isstruct(x))
if numel(varargin)==0 || isnumeric(varargin{1})
  % inputParser should handle this, but it doesn't
  ip.parse(gp, x, y, varargin{:});
else
  ip.parse(gp, x, y, [], varargin{:});
end
xt=ip.Results.xt;
zt=ip.Results.zt;
options=struct();
options.predcf=ip.Results.predcf;
options.tstind=ip.Results.tstind;
z=ip.Results.z;
if ~isempty(z)
  options.z=z;
end
if ~isempty(zt)
  options.zt=zt;
end
if isempty(zt)
  options.zt=z;
end
if isempty(xt)
  if size(x,2)==1
    [xt,xi]=sort(x);
    if ~isempty(z)
      zt=z(xi);
    else
      zt=[];
    end
  else
    xt=x;
    zt=z;
  end
end
target = ip.Results.target;
if iscell(gp)
  liktype=gp{1}.lik.type;
else
  liktype=gp.lik.type;
end
if isequal(liktype, 'Coxph') && isequal(target,'mu')
    target='f';
    warning('GP_CPRED: Target ''mu'' not applicable for a Cox-PH model. Switching to target ''f''')
end
tr = ip.Results.tr;
xlabels=ip.Results.xlabels;
% normdata
nd=ip.Results.normdata;
ipnd=inputParser;
ipnd.FunctionName = 'normdata';
ipnd.addParamValue('xmean',0,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('xstd',1,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('xlog',0,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('ymean',0,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('ystd',1,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('ylog',0,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.parse(nd);
nd=ipnd.Results;

if iscell(gp)
  liktype=gp{1}.lik.type;
else
  liktype=gp.lik.type;
end

[n,m]=size(x);

switch m
  case 1
    if isequal(liktype, 'Gaussian')
      [Ef, Varf,~,Ey,Vary] = gp_pred(gp, x, y, xt, options);
      Stdf=sqrt(Varf);
      Stdy=sqrt(Vary);
      xt=denormdata(xt,nd.xmean,nd.xstd);
      Ef=denormdata(Ef,nd.ymean,nd.ystd);
      Stdf=Stdf*nd.ystd;
      Ey=denormdata(Ey,nd.ymean,nd.ystd);
      Stdy=Stdy*nd.ystd;
      hh=plot(xt, Ef, '-b', xt, Ef-1.64*Stdf, '--b', xt, Ef+1.64*Stdf, '--b',xt, Ey-1.64*Stdy, ':b', xt, Ey+1.64*Stdy, ':b');
    else
      switch target
        case 'f'
          [Ef, Varf] = gp_pred(gp, x, y, xt, options);
          Stdf=sqrt(Varf);
          xt=denormdata(xt,nd.xmean,nd.xstd);
          Ef=denormdata(Ef,nd.ymean,nd.ystd);
          Stdf=Stdf*nd.ystd;
          hh=plot(xt, Ef, '-b', xt, Ef-1.64*Stdf, '--b', xt, Ef+1.64*Stdf, '--b');
        case 'mu'
          prctmu = gp_predprctmu(gp, x, y, xt, options);
          xt=denormdata(xt,nd.xmean,nd.xstd);
          prctmu=denormdata(prctmu,nd.ymean,nd.ystd);
          Ef = prctmu; Varf = [];
          hh=plot(xt, prctmu(:,2), '-b', xt, prctmu(:,1), '--b', xt, prctmu(:,3), '--b');
      end
    end
    if ~isempty(xlabels)
        xlabel(xlabels{1})
    else
        xlabel('x')
    end
  case 2
    subplot(2,2,1);
    gp_cpred(gp,x,y,xt,1,'z',z,'zt',zt,'target',target,'plot','on',varargin{:});
    if ~isempty(xlabels)
        xlabel(xlabels{1})
    else
        xlabel('x1')
    end
    subplot(2,2,2);
    gp_cpred(gp,x,y,xt,2,'z',z,'zt',zt,'target',target,'plot','on',varargin{:});
    if ~isempty(xlabels)
        xlabel(xlabels{2})
    else
        xlabel('x2')
    end
    subplot(2,1,2);
    gp_cpred(gp,x,y,xt,[1 2],'z',z,'zt',zt,'target',target,'plot','on','tr',1e9,varargin{:});
    axis square
    if ~isempty(xlabels)
        xlabel(xlabels{1})
        ylabel(xlabels{2})
    else
        xlabel('x1')
        ylabel('x2')
    end
    view(3)
    shading faceted
    hhh=get(gca,'children');
    hh=hhh(1);
  otherwise
    sn1=ceil(sqrt(m));
    sn2=ceil(m/sn1);
    for xi=1:m
      subplot(sn1,sn2,xi);
      gp_cpred(gp,x,y,xt,xi,'z',z,'zt',zt,'target',target,'plot','on','normdata',nd,options);
      if ~isempty(xlabels)
          xlabel(xlabels{xi})
      else
          xlabel(sprintf('x%d',xi))
      end
      drawnow
      hhh=get(gca,'children');
      hh(xi)=hhh(1);
    end
end
if nargout>0
  h=hh;
end


function [I_hyper,I_rgb,I_gt,g,G,extra,wl,wl_ori,Sel,s,bbl,H1] = ...
    prepare_fusion_dataset(dataset, options)
%PREPARE_FUSION_DATASET Summary of this function goes here
%   Detailed explanation goes here
if nargin < 2
    options = [];
end

I_hyper = [];
I_rgb = [];

switch dataset
    case {'pavia_nonrigid_algo','pavia_nonrigid_gt','pavia_robustness'}
        if strcmp(dataset, 'pavia_nonrigid_gt')
            all_data = FusionDataLoader.PaviaFusion_GT_Reg();
        elseif strcmp(dataset, 'pavia_nonrigid_algo')
            all_data = FusionDataLoader.PaviaFusion_LSQfreeform_Reg();
        elseif strcmp(dataset, 'pavia_robustness')
            all_data = FusionDataLoader.PaviaFusion_robustness_LSQfreeform_Reg();
        end

        data_index = parse_param(options,'data_index',7);
        selected_data = all_data(data_index);

        I_gt = selected_data.I_gt;
        I_hyper = selected_data.I_hyper;
        wl = selected_data.wl;
        I_rgb = selected_data.I_rgb;
        s = selected_data.s; % s = (sx, sy)
        extra = selected_data.extra; % extra = [extra_x, extra_y]
        g = selected_data.g;
        G = selected_data.G;
        Sel = selected_data.S;
        H1 = selected_data.H1;
        bbl = true(1,length(wl));

        wl_ori = wl;
    case 'gulfport'
        load('gulfport_fusion.mat');
        I_rgb = double(I_rgb);
        wl_ori = wl;
    case 'salton_sea'
        load('salton_sea_roi.mat');
        I1 = imread('salton_sea_roi_3.2015.jpg');
        load('reg_result_salton_sea.mat');
        bbl = logical(bbl);

        wl_ori = wl;
        [I_hyper,wl,I_rgb,g,H1,extra,s,G,Sel] = interpret_reg_params(I,wl,bbl,...
            I1,U2,V2,rho,s2,T2,sigma2);

        I_gt = zeros(size(I_rgb,1), size(I_rgb,2), length(bbl));
    otherwise
end

H1 = H1/255;
I_rgb = I_rgb/255;

% the competing methods do not have the extra margin boundary
if extra(1) > 0
    I_rgb = remove_rgb_margin(I_rgb, extra);
    I_gt = remove_rgb_margin(I_gt, extra);
end

end


function F = times( F , G )
%.*   Times of two SPHEREFUNV objects.
%   F.*G if F is a SPHEREFUNV and G is double returns the SPHEREFUNV after
%   componentwise multiplication.
%
%   F.*G if F is a SPHEREFUN and G is a SPHEREFUNV returns the SPHEREFUNV
%   after multiplication of F by each component of G.
%
%   F.*G if F is a double and G is a SPHEREFUNV returns the SPHEREFUNV
%   after multiplication of F by each component of G.

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

% Empty check:
if ( isempty(F) || isempty(G) )
    F = spherefunv;
    return
end

if ( ~isa(F, 'spherefunv') )
    F = times(G, F);
    return
end

nF = 3;

if ( isa(G, 'double') )             % SPHEREFUNV.*double
    if ( numel(G) == 1 )            % SPHEREFUNV.*scalar
        scalar = G;
        for jj = 1 : nF
            F.components{jj} = times(F.components{jj}, scalar);
        end
    elseif ( ( size(G, 1) == nF ) || ( F.isTransposed && (size(G, 2) == nF) ) )
        for jj = 1 : nF
            F.components{jj} = times(F.components{jj}, G(jj));
        end
    else
        error('SPHEREFUN:SPHEREFUNV:times:double', ...
            'SPHEREFUNV and double size mismatch.');
    end

elseif ( isa(G, 'spherefunv') )      % SPHEREFUNV . * SPHEREFUNV
    for jj = 1:3
        F.components{jj} = times(F.components{jj}, G.components{jj});
    end

elseif ( isa(G, 'spherefun') )       % SPHEREFUN * SPHEREFUNV
    for jj = 1 : nF
        F.components{jj} = times(F.components{jj}, G);
    end

else  % error
    error( 'SPHEREFUN:SPHEREFUNV:times:inputs', ...
        'Unrecognized input arguments.');
end

end


function x=cover(A,B)
x.t=min(A.t,B.t);
x.b=max(A.b,B.b);
x.l=min(A.l,B.l);
x.r=max(A.r,B.r);


function A = meanZero(hyp, x, i)

% Zero mean function. The mean function does not have any parameters.
%
% m(x) = 0
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-01-10.
%
% See also MEANFUNCTIONS.M.

if nargin<2, A = '0'; return; end             % report number of hyperparameters
A = zeros(size(x,1),1);                                    % derivative and mean


function CPD = hhmmF_CPD(bnet, self, Qself, Fbelow, varargin)
% HHMMF_CPD Make the CPD for an F node in a hierarchical HMM
% CPD = hhmmF_CPD(bnet, self, Qself,  Fbelow, ...)
%
%        Qps
%          \
%           \
%           Fself
%         /   |
%        /    |
%       Qself Fbelow
%
% We assume nodes are ordered (numbered) as follows: Qps, Q, Fbelow, F
% All nodes numbers should be from slice 1.
%
% If Fbelow if missing, this becomes a regular tabular_CPD.
% Qps may be omitted.
%
% optional args [defaults]
%
% Qps - node numbers.
% termprob - termprob(k,i,2) = prob finishing given Q(d)=i and Q(1:d-1)=k [ finish in last state wp 0.9]
%
% hhmmF_CPD is a subclass of tabular_CPD so we inherit inference methods like CPD_to_pot, etc.
%
% We create an isolated tabular_CPD with no F parent to learn termprob
% so we can avail of e.g., entropic or Dirichlet priors.
%
% For details, see "Linear-time inference in hierarchical HMMs", Murphy and Paskin, NIPS'01.

Qps = [];
% get parents
for i=1:2:length(varargin)
  switch varargin{i},
   case 'Qps', Qps = varargin{i+1};
  end
end

ns = bnet.node_sizes(:);
Qsz = ns(Qself);
Qpsz = prod(ns(Qps));
CPD.Qsz = Qsz;
CPD.Qpsz = Qpsz;

ps = parents(bnet.dag, self);
CPD.Fbelow_ndx = find_equiv_posns(Fbelow, ps);
CPD.Qps_ndx = find_equiv_posns(Qps, ps);
CPD.Qself_ndx = find_equiv_posns(Qself, ps);

% set default arguments
p = 0.9;
%termprob(k,i,t) Might terminate if i=Qsz; will not terminate if i<Qsz
termprob = zeros(Qpsz, Qsz, 2);
termprob(:, Qsz, 2) = p;
termprob(:, Qsz, 1) = 1-p;
termprob(:, 1:(Qsz-1), 1) = 1;

for i=1:2:length(varargin)
  switch varargin{i},
   case 'termprob', termprob = varargin{i+1};
  end
end

CPD.sub_CPD_term = mk_isolated_tabular_CPD([Qpsz Qsz 2], {'CPT', termprob});
S = struct(CPD.sub_CPD_term);
CPD.termprob = S.CPT;

CPD = class(CPD, 'hhmmF_CPD', tabular_CPD(bnet, self));

CPD = update_CPT(CPD);


function randlc_test01 ( )

%*****************************************************************************80
%
%% RANDLC_TEST01 tests RANDLC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2010
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'RANDLC_TEST01\n' );
  fprintf ( 1, '  RANDLC computes pseudorandom values\n' );
  fprintf ( 1, '  in the interval [0,1].\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The initial seed is %d\n', seed );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         I          RANDLC\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 10
    [ x, seed ] = randlc ( seed );
    fprintf ( 1, '  %8d  %14f\n', i, x );
  end

  return
end


function rtk=baserefpos(rtk,opt,obs,nav)

global glc

if opt.basepostype==glc.POSOPT_POS
    rtk.basepos=opt.basepos;
elseif opt.basepostype==glc.POSOPT_SPP
    rtk.basepos=avepos(rtk,opt,obs,nav);
elseif opt.basepostype==glc.POSOPT_RINEX
    if norm(obs.sta.pos)<=0
        error('Have no base position in renix header!!!\n');
    end
    [~,Cne]=xyz2blh(obs.sta.pos);
    dr=Cne'*obs.sta.del;
    rtk.basepos=obs.sta.pos+dr;
else
    error('Unable to obtain base position!!!\n');
end

if norm(rtk.basepos)==0
    error('The coordinates of the base station is zero!!!');
end

return


function [assignments] = FMC_interpret(AllSals, numClusters, AllPs, A_D, beta)
%% Interpretation of FMC data and grain assignment

%% run interpret 3D

A_D = A_D|A_D';

[i,~] = find(A_D);
cs = [0 full(cumsum(sum(A_D)))];

NNlist = cell(size(A_D,1),1);
for k = 1:size(A_D,1)
   NNlist{k} = i(cs(k)+1:cs(k+1))';
end

%% sort by saliency
% sort all course nodes for all s by saliency from lowest to highest
[tmp,elements] = sort(AllSals,'ascend'); %elements is list of indices representing the sorted AllSals list
% all points assigned to own node Nx2 [assignment,confidence]
% pointData is euler1 euler2 euler3 x y z confidence
numPoints = numClusters(1);
assignmentsX=zeros(numPoints,1);  % assignment of each poing
assignmentsY=zeros(numPoints,1);  % probability of that assignment

%% s values of each node
% vector sAssign has s value of all nodes
numS = length(numClusters); %number of iterations of s
sAssign=ones(numClusters(1),1); %initiating sAssign
lengthsub=zeros(1,numS);

%1 spot in sAssign for each node, increasing number for each saliency set
for curS=2:numS
    sAssign=[sAssign; curS*ones(numClusters(curS),1)];
end

% build assignments
for curS = numS-4:numS
    vdisp(['Considering S = ', int2str(curS)]);

    %find nodes for current s
    elemS = elements(sAssign(elements) == curS)';   %nodes of curS
    sLen = numClusters(curS);
    numNodes = length(elemS);

    % for each s from current s to s=1, iteratively find which nodes
    % are inside of the node by multiplying by Ps
    backPs = AllPs{curS};
    for iterS=curS-1:-1:2
        backPs=AllPs{iterS}*backPs;
    end

    % find probability of points assigned to each node
    allUs = [backPs assignmentsY];

    %assign clusters to points based on the strongest probability of any
    %point in the cluster
    [Prob, NodeIndex] = max(allUs,[],2);
    %assign clusters to points based on the strongest probability of any
%     %point in the cluster
    allUs = allUs >= beta;

    for point = find((NodeIndex <= numNodes) & (Prob < 1))'
      nodePoints = allUs(:,NodeIndex(point));
      assignmentsX(nodePoints) = NodeIndex(point);
      assignmentsY(nodePoints) = Prob(point);
    end

end

%% Break up non-continuous (Michigan) clusters
rLimit = 15;   %Recursion limit (MATLAB tends to crash around 840)
%N.B. This runs considerably faster with a low recursion limit due to
%MATLAB's huge overhead. Better to make more calls than fewer, deeper calls
% set(0,'RecursionLimit',rLimit)

vdisp('Breaking non-continuous clusters')

flag = zeros(length(assignmentsX),1);
assignmentsN = zeros(length(assignmentsX),1);
contPoints = [];
for point = 2:length(flag)
    if assignmentsN(point) == 0
        oldClust = assignmentsX(point);
        newClust = point;
        if (flag(point)>1)     %if the point has been reserved, continue the same grouping
            newClust = flag(point);
        end
        assignmentsN(point) = newClust;
        rDepth = 1;

        [assignmentsN, flag, contPoints] = clusterBreaker(assignmentsN, point, assignmentsX, flag, NNlist, oldClust, newClust, contPoints, rDepth, rLimit-5);

        while isempty(contPoints) == 0
            pointC = contPoints(1);
            contPoints = contPoints(2:end);
            assignmentsN(pointC) = newClust;
            rDepth = 1;
            [assignmentsN, flag, contPoints] = clusterBreaker(assignmentsN, pointC, assignmentsX, flag, NNlist, oldClust, newClust, contPoints, rDepth, rLimit-5);
        end
    end
end

singlePoints = find(flag == -1 & sum(A_D,2)>1);
for pt = singlePoints'
assignmentsN(pt) = assignmentsN(NNlist{pt}(2));
end

assignmentsX = assignmentsN;
% set(0,'RecursionLimit',500)

assignments = [assignmentsX assignmentsY];

end


function g = sdrbfKernGradient(sdrbfKern, t1, t2, covGrad)

% SDRBFKERNGRADIENT Gradient of SDRBF kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the switching
% dynamical radial basis function kernel's parameters. As well as the
% kernel structure and the input positions, the user provides a matrix
% PARTIAL which gives the partial derivatives of the function with respect
% to the relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG t : the input locations for which the gradients are being
% computed.
% ARG covGrad : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in t1.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG t1 : the input locations associated with the rows of the
% kernel matrix.
% ARG t2 : the input locations associated with the columns of the
% kernel matrix.
% ARG covGrad : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as t1 and the same number of columns
% as t2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO sdrbfKernParamInit, kernGradient
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 4
    covGrad = t2;
    t2 = t1;
end

if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end

%Form the basic kernels
rbfKern = struct();

% Create structures that will make easy the computation of the kernels
% spVector = [sdrbfKern.switchingTimes t1(end)+0.1]; % For the last interval include the last point

spVector = [cumsum(sdrbfKern.switchingTimes) t1(end)+100];

dim1 = zeros(1, sdrbfKern.nIntervals);
dim2 = zeros(1, sdrbfKern.nIntervals);

for i =1:sdrbfKern.nIntervals
    for j=1:sdrbfKern.nlfPerInt
        rbfKern(i,j).variance = sdrbfKern.variance;
        rbfKern(i,j).inverseWidth = sdrbfKern.inverseWidth(j,i);
        rbfKern(i,j).isNormalised = sdrbfKern.isNormalised;
    end
    newt1 = t1(t1> spVector(i) & t1<spVector(i+1));
    newt2 = t2(t2> spVector(i) & t2<spVector(i+1));
    dim1(i) = length(newt1);
    dim2(i) = length(newt2);
end

gradIW = zeros(sdrbfKern.nlfPerInt, sdrbfKern.nIntervals);
gradSP = zeros(1, sdrbfKern.nIntervals);
%dimParam = sdrbfKern.nIntervals*ones(1, 2);
%gC = cell(1, sdrbfKern.nlfPerInt);
%indxIW = 1:dimParam(1);
%indxSP = dimParam(1)+1:sum(dimParam);

covGrad2 = covGrad;
for q=1:sdrbfKern.nlfPerInt
    startValOne = 1;
    endValOne   = 0;
    startValThree = 1;
    endValThree = 0;
    gradSPTemp = zeros(1, sdrbfKern.nIntervals);
    if iscell(covGrad2)
        covGrad = covGrad2{q};
    end
    for i=1:sdrbfKern.nIntervals
        endValOne = endValOne + dim1(i);
        endValThree = endValThree + dim2(i);
        gP = rbfKernGradient(rbfKern(i,q), t1(startValOne:endValOne) - spVector(i), ...
            t2(startValThree:endValThree) - spVector(i), ...
            covGrad(startValOne:endValOne, startValThree:endValThree));
        gradIW(q,i)= gP(1);
        gX = rbfKernGradX(rbfKern(i,q), t1(startValOne:endValOne) - spVector(i), ...
            t2(startValThree:endValThree) - spVector(i));
        gXR = reshape(gX, dim1(i), dim2(i))';
        gradSPTemp(i) = gradSPTemp(i) - 2*sum(sum(gXR.*covGrad(startValOne:endValOne, startValThree:endValThree)));
        startValThree = endValThree + 1;
        startValOne = endValOne + 1;
    end
    gradSP = gradSP + gradSPTemp;
    %gC{q}(indxIW) = gradIW(q,:);
    %gC{q}(indxSP) = gradSPTemp;
end

%g = [gradIW(:)' gradSP];

%%%%%%
tempGradSP = fliplr(gradSP);
tempGradSP = cumsum(tempGradSP);
g = [gradIW(:)' fliplr(tempGradSP)];
%%%%%%


%% clear the workspace and select data
clear; clc; close all;

%% choose multiple datasets or just one
neuron = Sources2D();
nams = {'./data_1p.tif'};          % you can put all file names into a cell array; when it's empty, manually select files
nams = neuron.select_multiple_files(nams);  %if nam is [], then select data interactively

%% parameters
% -------------------------    COMPUTATION    -------------------------  %
pars_envs = struct('memory_size_to_use', 8, ...   % GB, memory space you allow to use in MATLAB
    'memory_size_per_patch', 0.5, ...   % GB, space for loading data within one patch
    'patch_dims', [64, 64],...  %GB, patch size
    'batch_frames', 1000);           % number of frames per batch
  % -------------------------      SPATIAL      -------------------------  %
gSig = 3;           % pixel, gaussian width of a gaussian kernel for filtering the data. 0 means no filtering
gSiz = 13;          % pixel, neuron diameter
ssub = 1;           % spatial downsampling factor
with_dendrites = true;   % with dendrites or not
if with_dendrites
    % determine the search locations by dilating the current neuron shapes
    updateA_search_method = 'dilate';  %#ok<UNRCH>
    updateA_bSiz = 5;
    updateA_dist = neuron.options.dist;
else
    % determine the search locations by selecting a round area
    updateA_search_method = 'ellipse'; %#ok<UNRCH>
    updateA_dist = 5;
    updateA_bSiz = neuron.options.dist;
end
spatial_constraints = struct('connected', true, 'circular', false);  % you can include following constraints: 'circular'
spatial_algorithm = 'hals';

% -------------------------      TEMPORAL     -------------------------  %
Fs = 10;             % frame rate
tsub = 1;           % temporal downsampling factor
deconv_options = struct('type', 'ar1', ... % model of the calcium traces. {'ar1', 'ar2'}
    'method', 'foopsi', ... % method for running deconvolution {'foopsi', 'constrained', 'thresholded'}
    'smin', -5, ...         % minimum spike size. When the value is negative, the actual threshold is abs(smin)*noise level
    'optimize_pars', true, ...  % optimize AR coefficients
    'optimize_b', true, ...% optimize the baseline);
    'max_tau', 100);    % maximum decay time (unit: frame);

nk = 3;             % detrending the slow fluctuation. usually 1 is fine (no detrending)
% when changed, try some integers smaller than total_frame/(Fs*30)
detrend_method = 'spline';  % compute the local minimum as an estimation of trend.

% -------------------------     BACKGROUND    -------------------------  %
bg_model = 'ring';  % model of the background {'ring', 'svd'(default), 'nmf'}
nb = 1;             % number of background sources for each patch (only be used in SVD and NMF model)
bg_neuron_factor = 1.4;
ring_radius = round(bg_neuron_factor * gSiz);  % when the ring model used, it is the radius of the ring used in the background model.
%otherwise, it's just the width of the overlapping area
num_neighbors = 50; % number of neighbors for each neuron

% -------------------------      MERGING      -------------------------  %
show_merge = false;  % if true, manually verify the merging step
merge_thr = 0.65;     % thresholds for merging neurons; [spatial overlap ratio, temporal correlation of calcium traces, spike correlation]
method_dist = 'max';   % method for computing neuron distances {'mean', 'max'}
dmin = 5;       % minimum distances between two neurons. it is used together with merge_thr
dmin_only = 2;  % merge neurons if their distances are smaller than dmin_only.
merge_thr_spatial = [0.8, 0.4, -inf];  % merge components with highly correlated spatial shapes (corr=0.8) and small temporal correlations (corr=0.1)

% -------------------------  INITIALIZATION   -------------------------  %
K = [];             % maximum number of neurons per patch. when K=[], take as many as possible.
min_corr = 0.8;     % minimum local correlation for a seeding pixel
min_pnr = 8;       % minimum peak-to-noise ratio for a seeding pixel
min_pixel = gSig^2;      % minimum number of nonzero pixels for each neuron
bd = 0;             % number of rows/columns to be ignored in the boundary (mainly for motion corrected data)
frame_range = [];   % when [], uses all frames
save_initialization = false;    % save the initialization procedure as a video.
use_parallel = true;    % use parallel computation for parallel computing
show_init = true;   % show initialization results
choose_params = true; % manually choose parameters
center_psf = true;  % set the value as true when the background fluctuation is large (usually 1p data)
% set the value as false when the background fluctuation is small (2p)

% -------------------------  Residual   -------------------------  %
min_corr_res = 0.7;
min_pnr_res = 6;
seed_method_res = 'auto';  % method for initializing neurons from the residual
update_sn = true;

% ----------------------  WITH MANUAL INTERVENTION  --------------------  %
with_manual_intervention = false;

% -------------------------  FINAL RESULTS   -------------------------  %
save_demixed = true;    % save the demixed file or not
kt = 3;                 % frame intervals

% -------------------------    UPDATE ALL    -------------------------  %
neuron.updateParams('gSig', gSig, ...       % -------- spatial --------
    'gSiz', gSiz, ...
    'ring_radius', ring_radius, ...
    'ssub', ssub, ...
    'search_method', updateA_search_method, ...
    'bSiz', updateA_bSiz, ...
    'dist', updateA_bSiz, ...
    'spatial_constraints', spatial_constraints, ...
    'spatial_algorithm', spatial_algorithm, ...
    'tsub', tsub, ...                       % -------- temporal --------
    'deconv_options', deconv_options, ...
    'nk', nk, ...
    'detrend_method', detrend_method, ...
    'background_model', bg_model, ...       % -------- background --------
    'nb', nb, ...
    'ring_radius', ring_radius, ...
    'num_neighbors', num_neighbors, ...
    'merge_thr', merge_thr, ...             % -------- merging ---------
    'dmin', dmin, ...
    'method_dist', method_dist, ...
    'min_corr', min_corr, ...               % ----- initialization -----
    'min_pnr', min_pnr, ...
    'min_pixel', min_pixel, ...
    'bd', bd, ...
    'center_psf', center_psf);
neuron.Fs = Fs;

%% distribute data and be ready to run source extraction
neuron.getReady_batch(pars_envs);

%% initialize neurons in batch mode
neuron.initComponents_batch(K, save_initialization, use_parallel);

%% udpate spatial components for all batches
neuron.update_spatial_batch(use_parallel);

%% udpate temporal components for all bataches
neuron.update_temporal_batch(use_parallel);

%% update background
neuron.update_background_batch(use_parallel);

%% delete neurons

%% merge neurons

%% get the correlation image and PNR image for all neurons
neuron.correlation_pnr_batch();

%% concatenate temporal components
neuron.concatenate_temporal_batch();
neuron.viewNeurons([],neuron.C_raw);

%% save workspace
neuron.save_workspace_batch();


function op = linop_handles( sz, Af, At, cmode )
%LINOP_HANDLES Linear operator from user-supplied function handles.
%OP = LINOP_HANDLES( SZ, AF, AT, CMODE )
%    Constructs a TFOCS-compatible linear operator from separate function
%    handles that compute the forward and adjoint operations. The first
%    argument, SZ, gives the size of the linear operator; and the forward
%    and adjoint handles are AF and AT, respectively.
%
%    If the inputs and outputs are simple vectors, then SZ can take the
%    standard Matlab form [M,N], where N is the input length and M is the
%    output length. If the input or output is a matrix or array, then SZ
%    should take the form { S_in, S_out }, where S_in is the size of the
%    input and S_out is the size of the output.
%
%    If the input or output space of the operator is complex, then the
%    CMODE string must be supplied, which describes the forward operation:
%       'R2R': real input and output
%       'R2C': real input, complex output
%       'C2R': complex input, real output
%       'C2C': complex input, complex output
%    If CMODE is not supplied, then 'R2R' is assumed. If the operator
%    detects a complex input or output when it is not expected, then an
%    error results. Therefore, you must make sure that your operators
%    return real values when they are expected to do so.

error(nargchk(3,4,nargin));
if numel( sz ) ~= 2,
    error( 'Size must have two elements.' );
elseif isnumeric( sz ),
    sz = { [sz(2),1], [sz(1),1] };
elseif ~isa( sz, 'cell' ),
    error( 'Invalid operator size specification.' );
end
if ~isa( Af, 'function_handle' ) || ~isa( At, 'function_handle' ),
    error( 'Second and third arguments must be function handles.' );
end
if nargin < 4 || isempty( cmode ),
    cmode = 'R2R';
elseif ~ischar( cmode ) || size( cmode, 1 ) ~= 1,
    error( 'Fourth argument must be a string.' );
else
    cmode = upper( cmode );
end

switch cmode,
    case 'R2R', op = @(x,mode)linop_handles_r2r( sz, Af, At, x, mode );
    case {'R2C','R2CC'}, op = @(x,mode)linop_handles_r2c( sz, Af, At, x, mode );
    case {'C2R','CC2R'}, op = @(x,mode)linop_handles_c2r( sz, Af, At, x, mode );
    case 'C2C', op = @(x,mode)linop_handles_c2c( sz, Af, At, x, mode );
    otherwise,
        error( 'Invalid complex mode: %s', cmode );
end

function y = linop_handles_r2r(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = realcheck( Af( realcheck( x ) ) );
    case 2, y = realcheck( At( realcheck( x ) ) );
end

function y = linop_handles_r2c(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = Af( realcheck( x ) );
    case 2, y = realcheck( At( x ) );
end

function y = linop_handles_c2r(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = realcheck( Af( x ) );
    case 2, y = At( realcheck( x ) );
end

function y = linop_handles_c2c(sz, Af, At, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = Af( x );
    case 2, y = At( x );
end

function y = realcheck( y )
if ~isreal( y ),
    error( 'Unexpected complex value in linear operation.' );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function l = IsFirstAfter(timestamps,list,varargin)

%IsFirstAfter - Identify first item after each of a list of timestamps.
%
% For each element i of a list of test timestamps, find in a list of reference timestamps
% the first element j larger than i. Returns a list of logical indices (the elements of the
% test list which are greater than all the elements of the reference timestamps are ignored).
%
%  USAGE
%
%    logical = IsFirstAfter(timestamps,list,<options>)
%
%    timestamps     a list of reference timestamps
%    list           a list of test timestamps
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'strict'      'on' for strict (>) comparisons, 'off' otherwise (>=)
%                   (default = 'off')
%    =========================================================================
%
%  SEE
%
%    See also IsLastBefore.

% Copyright (C) 2004-2011 by Michaël Zugaro
%
% 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.

strict = 'off';

if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help IsFirstAfter'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
  if ~ischar(varargin{i}),
    error(['Parameter ' num2str(i+1) ' is not a property (type ''help IsFirstAfter'' for details).']);
  end
  switch(lower(varargin{i})),
    case 'strict',
		strict = lower(varargin{i+1});
		if ~isstring_FMAT(strict,'on','off'),
			error('Incorrect value for property ''strict'' (type ''help IsFirstAfter'' for details).');
		end
    otherwise,
      error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help IsFirstAfter'' for details).']);
  end
end

% Make sure 'timestamps' looks like [t1;t2;...;tn] and 'list' like [l1 l2 ... lm]
if min(size(timestamps)) ~= 1 | min(size(list)) ~= 1,
	error('Incorrect sizes (must be vectors)');
end
if size(timestamps,1) == 1,
	timestamps = timestamps';
end
if size(list,2) == 1,
	list = list';
end

% Now compare [t1 t1 ... t1;t2 t2 ... t2;...;tn tn ... tn] with [l1 l2 ... lm;l1 l2 ... lm;...;l1 l2 ... lm]
if strcmp(strict,'on'),
	where = repmat(timestamps,1,length(list)) > repmat(list,length(timestamps),1);
else
	where = repmat(timestamps,1,length(list)) >= repmat(list,length(timestamps),1);
end
% This yielded something like [0 0 0;0 0 0;...;1 0 0;...;1 1 0;...;1 1 1;...;1 1 1]
% where the transition from 0 to 1 in each column indicates the index of the first element
% in 'timestamps' superior to the corresponding element in 'list'. Now, find these transitions.
% (we append zeros before and ones after 'where' to handle the special cases where the first
% element in 'timestamps' superior to the element of 'list' is the first element of 'timestamps'
% or does not exist).
where = diff([zeros(1,size(where,2));where;ones(1,size(where,2))]);
% Convert the resulting logical matrix into a list of logical indices
l = sum(where,2);
l = l(1:length(timestamps));
l = logical(l==1);


function hded1(currentfile)

[path,nam,ext] = fileparts(currentfile);
sig = signal(currentfile);
title = ['Axes Editor for ' nam];
lineNo = 1;
prompt = {};
defaults = {};
for i=1:ndim(sig)
	a = getaxis(sig, i);
	prompt{end+1} = ['Name of dimension ' num2str(i)];
	prompt{end+1} = ['Unit of dimension ' num2str(i)];
	prompt{end+1} = ['Start value and sampling rate for dimension ' num2str(i)];
	defaults{end+1} = name(a);
	defaults{end+1} = label(unit(a));
	defaults{end+1} = num2str([first(a) samplerate(a)]);
end

answer  = inputdialog(prompt,title,lineNo, defaults);
if ~isempty(answer)
	for i=1:ndim(sig)
		a = getaxis(sig, i);
		a = setunit(a, unit(answer{3*i-1}));
		if ~isempty(answer(3*i-2))
			a = setname(a, answer{3*i-2});
		end
		rs = str2num(answer{3*i});
		first = rs(1);
		rate =  rs(2);
		if rate > 0
			delta = 1/rate;
		else
			delta = 1;
		end
		a = setfirst(a, first);
		a = setdelta(a, delta);
		sig = setaxis(sig, i, a);
	end
	write(sig, currentfile);
end


% SR_QUANT_EVAL
%
% Script for computing averaged PSNR, SSIM, and IVC.
% The script reproduces the quantitative results on our project page on
% Github: https://github.com/jbhuang0604/SelfExSR
%
% Jia-Bin Huang
% Electrical and Computer Engineering
% University of Illinois, Urbana-Champaign
% www.jiabinhuang.com

clc;
clear;
close all;

% Add pathes for running SSIM and IVC
addpath(genpath('quant_eval'));

% Dataset list
numDataset = 4;
dataset = cell(numDataset,1);
dataset{1}.name = 'Urban100';
dataset{2}.name = 'BSD100';
dataset{3}.name = 'Set5';
dataset{4}.name = 'Set14';

% Super-resolution factor
dataset{1}.SRF  = [2, 4];
dataset{2}.SRF  = [2, 3, 4];
dataset{3}.SRF  = [2, 3, 4];
dataset{4}.SRF  = [2, 3, 4];

% Number of images in the dataset
dataset{1}.numImg = 100;
dataset{2}.numImg = 100;
dataset{3}.numImg = 5;
dataset{4}.numImg = 14;

% Method list
methodList = {'Bicubic', 'ScSR', 'Kim', 'Abhishek', 'Glasner', 'SRCNN', 'A+', 'SelfExSR'};
numMethod  = length(methodList);
indValidMethod = [1, 1, 1, 1, 1, 1, 1, 1]; % Test only

% Initialize result path

resPath = fullfile('quant_eval', 'result');
if(~exist(resPath, 'dir'))
    mkdir(resPath);
end
% =========================================================================
% Start quantitative evaluation
% =========================================================================
for indDataset = 1 : numDataset
    datasetName   = dataset{indDataset}.name;
    numImgDataset = dataset{indDataset}.numImg;
    numSRF        = size(dataset{indDataset}.SRF,2);

    % Run each super-resolution factor
    for indSRF = 1: numSRF
        SRF = dataset{indDataset}.SRF(indSRF);
        imgPath = fullfile('data', datasetName, ['image_SRF_',num2str(SRF)]);

        resName = ['result_', datasetName, '_SRF_', num2str(SRF),'.mat'];
        if(~exist(fullfile(resPath, resName), 'file'))
            % Initialize result table
            SSIM_table = zeros(numImgDataset, numMethod);
            PSNR_table = zeros(numImgDataset, numMethod);
            IFC_table  = zeros(numImgDataset, numMethod);
            reverseStr = '';
            for indImg = 1: numImgDataset
                % Load groundtruth high-resolution image
                imgName = ['img_',num2str(indImg, '%03d'), '_SRF_', num2str(SRF), '_HR.png'];
                imgGT = imread(fullfile(imgPath, imgName));

                % Load super-resolution results
                for indMethod= 1:numMethod
                    if(indValidMethod(indMethod))
                        imgName = ['img_',num2str(indImg, '%03d'), '_SRF_', num2str(SRF), '_', methodList{indMethod}, '.png'];
                        img = imread(fullfile(imgPath, imgName), 'png');

                        % Compute image quality
                        [psnr, ssim, ifc] = compute_difference(img, imgGT, SRF);

                        PSNR_table(indImg,indMethod) = psnr;
                        SSIM_table(indImg,indMethod) = ssim;
                        IFC_table(indImg, indMethod)  = ifc;
                    end
                    % Display progress
                    percentDone = 100*((indImg-1)*numMethod + indMethod)/(numMethod*numImgDataset);
                    msg = sprintf('Evaluating dataset %s on SRF %d, progress: %3.2f', datasetName, SRF, percentDone);
                    fprintf([reverseStr, msg]);
                    reverseStr = repmat(sprintf('\b'), 1, length(msg));
                end
            end

            % Save results
            resName = ['result_', datasetName, '_SRF_', num2str(SRF),'.mat'];
            save(fullfile(resPath, resName), 'PSNR_table', 'SSIM_table', 'IFC_table');
        else
            load(fullfile(resPath, resName));
        end

        % Display results
        avgPSNR = mean(PSNR_table,1);
        avgSSIM = mean(SSIM_table,1);
        avgIFC  = mean(IFC_table, 1);
        fprintf('\n\n=== Quantitative results for dataset %s on SRF %d === \n\n', datasetName, SRF);
        fprintf('Peak signal-to-noise ratio (PSNR) \n')
        fprintf('     %8s\t%8s\t%8s\t%8s\t%8s\t%8s\t%8s\t%8s\t \n', methodList{1}, methodList{2}, ...
            methodList{3}, methodList{4}, methodList{5}, methodList{6}, methodList{7}, methodList{8});
        fprintf('PSNR|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t| \n', ...
            avgPSNR(1), avgPSNR(2), avgPSNR(3), avgPSNR(4), avgPSNR(5), avgPSNR(6), avgPSNR(7), avgPSNR(8));
        fprintf('Structural similarity index (SSIM) \n')
        fprintf('     %8s\t%8s\t%8s\t%8s\t%8s\t%8s\t%8s\t%8s\t \n', methodList{1}, methodList{2}, ...
            methodList{3}, methodList{4}, methodList{5}, methodList{6}, methodList{7}, methodList{8});
        fprintf('SSIM|%8.04f\t|%8.04f\t|%8.04f\t|%8.04f\t|%8.04f\t|%8.04f\t|%8.04f\t|%8.04f\t| \n', ...
            avgSSIM(1), avgSSIM(2), avgSSIM(3), avgSSIM(4), avgSSIM(5), avgSSIM(6), avgSSIM(7), avgSSIM(8));
        fprintf('Information fidelity criterion (IFC) \n')
        fprintf('    %8s\t%8s\t%8s\t%8s\t%8s\t%8s\t%8s\t%8s\t \n', methodList{1}, methodList{2}, ...
            methodList{3}, methodList{4}, methodList{5}, methodList{6}, methodList{7}, methodList{8});
        fprintf('IFC|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t|%8.02f\t| \n\n', ...
            avgIFC(1), avgIFC(2), avgIFC(3), avgIFC(4), avgIFC(5), avgIFC(6), avgIFC(7), avgIFC(8));
        fprintf('\n');
    end
end


% This demo script runs the STRCF tracker with deep features on the
% included "Human3" video.

% Add paths
setup_paths();

%  Load video information
base_path  =  './sequences';
%video  = choose_video(base_path);
video = 'Human3';

video_path = [base_path '/' video];
[seq, gt_boxes] = load_video_info(video_path);

% Run STRCF
results = run_STRCF(seq);
%results = run_DeepSTRCF(seq);

pd_boxes = results.res;
thresholdSetOverlap = 0: 0.05 : 1;
success_num_overlap = zeros(1, numel(thresholdSetOverlap));
res = calcRectInt(gt_boxes, pd_boxes);
for t = 1: length(thresholdSetOverlap)
    success_num_overlap(1, t) = sum(res > thresholdSetOverlap(t));
end
cur_AUC = mean(success_num_overlap) / size(gt_boxes, 1);
FPS_vid = results.fps;
display([video  '---->' '   FPS:   ' num2str(FPS_vid)   '    op:   '   num2str(cur_AUC)]);


function box = mergeBoxes(box1, box2)
%MERGEBOXES Merge two boxes, by computing their greatest extent.
%
%   BOX = mergeBoxes(BOX1, BOX2);
%
%   Example
%   box1 = [5 20 5 30];
%   box2 = [0 15 0 15];
%   mergeBoxes(box1, box2)
%   ans =
%       0 20 0 30
%
%
%   See also
%   boxes2d, drawBox, intersectBoxes
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2010-07-26, using Matlab 7.9.0.529 (R2009b)
% Copyright 2010-2022 INRA - Cepia Software Platform

% unify sizes of data
if size(box1,1) == 1
    box1 = repmat(box1, size(box2,1), 1);
elseif size(box2, 1) == 1
    box2 = repmat(box2, size(box1,1), 1);
elseif size(box1,1) ~= size(box2,1)
    error('Bad size for inputs');
end

% compute extreme coords
mini = min(box1(:,[1 3]), box2(:,[1 3]));
maxi = max(box1(:,[2 4]), box2(:,[2 4]));

% concatenate result into a new box structure
box = [mini(:,1) maxi(:,1) mini(:,2) maxi(:,2)];


caffe.reset_all();
caffe.set_mode_gpu();
gpu_id = 0;  % we will use the first gpu in this demo
caffe.set_device(gpu_id);

addpath('../PrototxtGen');
net_model = 'net_define.prototxt';
inception_file = 'inception.prototxt';
inception_content = fileread(inception_file);
conv1x1_file = '1x1conv.prototxt';
conv1x1_content = fileread(conv1x1_file);
pooling_file = 'pooling.prototxt';
pooling_content = fileread(pooling_file);
output_file = 'output.prototxt';
output_content = fileread(output_file);

activation = 'ReLU';

layers = {
    struct('type', 'convolution', 'outputmaps', 10, 'kernelsize', 3, 'activation', activation) %convolution layer
    struct('type', 'convolution', 'outputmaps', 10, 'kernelsize', 3, 'activation', activation) %convolution layer
%     struct('type', 'convolution', 'outputmaps', 100, 'kernelsize', 3, 'activation', activation) %convolution layer
%     struct('type', 'pooling', 'scale', 2, 'method', 'AVE')
%     struct('type', 'inception', 'node1x1', 100, 'reduce3x3', 50, 'node3x3', 100, 'reduce5x5', 50, 'node5x5', 100, 'poolconv', 100)
%     struct('type', 'inception', 'node1x1', 20, 'reduce3x3', 10, 'node3x3', 20, 'reduce5x5', 10, 'node5x5', 20, 'poolconv', 20)
%     struct('type', 'inception', 'node1x1', 20, 'reduce3x3', 10, 'node3x3', 20, 'reduce5x5', 10, 'node5x5', 20, 'poolconv', 20)
    struct('type', 'convolution', 'outputmaps', 20, 'kernelsize', 1, 'activation', activation) %convolution layer
    struct('type', 'convolution', 'outputmaps', 20, 'kernelsize', 1, 'activation', activation) %convolution layer
%     struct('type', 'pooling', 'scale', 2, 'method', 'AVE')
};
width = 600;
height = 400;
border = 5;

fid = fopen(net_model,'w');
proto_file{1} = 'name: "mnist_siamese_train_test"';
proto_file{2} = 'input: "data"';
proto_file{3} = 'input_dim: 1';
proto_file{4} = 'input_dim: 2';
proto_file{5} = ['input_dim: ' num2str(width)];
proto_file{6} = ['input_dim: ' num2str(height)];
for i=1:6
    fprintf(fid,'%s\r\n',proto_file{i});
end;
top_layer = 'data';
top_layer_exp = 'top: "(.+?)"';
for i=1:length(layers)
    if strcmp(layers{i}.type,'convolution')
        this_layer = strrep(conv1x1_content,'{num}',num2str(i));
        this_layer = strrep(this_layer,'{node_num}',num2str(layers{i}.outputmaps));
        this_layer = strrep(this_layer,'{bottom_name}',top_layer);
        this_layer = strrep(this_layer,'{kernel_size}',num2str(layers{i}.kernelsize));
        this_layer = strrep(this_layer,'{activation}',layers{i}.activation);
    elseif strcmp(layers{i}.type,'pooling')
        this_layer = strrep(pooling_content,'{num}',num2str(i));
        this_layer = strrep(this_layer,'{bottom_name}',top_layer);
        this_layer = strrep(this_layer,'{method}',layers{i}.method);
        this_layer = strrep(this_layer,'{scale}',num2str(layers{i}.scale));
    elseif strcmp(layers{i}.type,'inception')
        this_layer = strrep(inception_content,'{num}',num2str(i));
        this_layer = strrep(this_layer,'{bottom_name}',top_layer);
        this_layer = strrep(this_layer,'{1x1node}',num2str(layers{i}.node1x1));
        this_layer = strrep(this_layer,'{3x3reduce}',num2str(layers{i}.reduce3x3));
        this_layer = strrep(this_layer,'{3x3node}',num2str(layers{i}.node3x3));
        this_layer = strrep(this_layer,'{5x5reduce}',num2str(layers{i}.reduce5x5));
        this_layer = strrep(this_layer,'{5x5node}',num2str(layers{i}.node5x5));
        this_layer = strrep(this_layer,'{poolconv}',num2str(layers{i}.poolconv));
    end;
    top_layer = regexp(this_layer,top_layer_exp,'tokens');
    top_layer = top_layer{end}{1};
    fprintf(fid,'%s\r\n',this_layer);
end;
this_layer = strrep(output_content,'{bottom_name}',top_layer);
fprintf(fid,'%s\r\n',this_layer);
fclose(fid);

train_net = caffe.Net(net_model,'train');

input_data = zeros(height, width, 2, 1, 'single');
input_data(:,:,1,1) = repmat((1:height)',1,width) / height;
input_data(:,:,2,1) = repmat((1:width),height,1) / width;
input_data = input_data - 0.5;

output_data = train_net.forward({input_data});

output = output_data{1};
output = output(border+1:end-border,border+1:end-border,:);
output = bsxfun(@minus,output,min(min(output,[],1),[],2));
output = bsxfun(@rdivide,output,max(max(output,[],1),[],2));
figure(1);
imshow(Lab2RGB(output));
% imshow(output);


function [nb] = find_triangle_neighbours(pnt, tri)

% FIND_TRIANGLE_NEIGHBOURS determines the three neighbours for each triangle
% in a mesh. It returns NaN's if the triangle does not have a neighbour on
% that particular side.
%
% [nb] = find_triangle_neighbours(pnt, tri)

% Copyright (C) 2003, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

npnt = size(pnt,1);
ntri = size(tri,1);

% each triangle has maximally three neighbours, assuming that the
% surface mesh is not degenerate
nb = nan(size(tri));

% for i=1:ntri
%   for j=setdiff(1:ntri, i)
%     if length(intersect(tri(i,[1 2]), tri(j,:)))==2
%       nb(i,1) = j;
%       continue;
%     end
%     if length(intersect(tri(i,[2 3]), tri(j,:)))==2
%       nb(i,2) = j;
%       continue;
%     end
%     if length(intersect(tri(i,[3 1]), tri(j,:)))==2
%       nb(i,3) = j;
%       continue;
%     end
%   end
%   if all(~isnan(nb(i,:)))
%     continue;
%   end
% end

for i=1:ntri
  % find all neighbouring triangles
  tmp1 = (tri==tri(i,1));
  tmp2 = (tri==tri(i,2));
  tmp3 = (tri==tri(i,3));
  tmp  = (tmp1|tmp2|tmp3);
  sel = find(sum(tmp,2)==2);

  % ensure that each neighbour is assigned to the proper edge
  if length(sel)>3
    ft_error('more than three neighbours found for triangle %d', i);
  else
    for j=1:length(sel)
      if isempty(setdiff(intersect(tri(i,:), tri(sel(j),:)), tri(i,[1 2])))
        nb(i,1) = sel(j);
      elseif isempty(setdiff(intersect(tri(i,:), tri(sel(j),:)), tri(i,[2 3])))
        nb(i,2) = sel(j);
      elseif isempty(setdiff(intersect(tri(i,:), tri(sel(j),:)), tri(i,[3 1])))
        nb(i,3) = sel(j);
      end
    end
  end
end


function T = prep_trpca_data( impath, dataname, scale )

% impath = the root dir of the images

if ~exist( 'scale', 'var' ) || isempty( scale )
    scale = 1.0;
end

files = dir( [impath, '\*.bmp'] );
N = length( files );
% N = N - 1;
img = imresize( imread( [impath, '\', files(1).name], 'bmp' ), scale );
img = rgb2gray( img );
sz = size( img );   lsz = length(sz);
sz(lsz+1) = N;
X = zeros( sz );

for i = 1:N
    img = imresize( imread( [impath, '\', files(i).name], 'bmp' ), scale );
    img = rgb2gray( img );
    if lsz == 2
        X( :, :, i ) = double(img) / 255;
    else
        X( :, :, :, i ) = double(img) / 255;
    end
end

% img = imresize( imread( [impath, '\', files(N+1).name], 'bmp' ), scale );
% img = rgb2gray( img );
% data.truth = double(img) / 255;

T = tensor( X );

data.X = T;

save( ['..\..\data\',dataname], 'data' );
end


function a = ctranspose(a)
%CTRANSPOSE Complex conjugate transpose for tenmat.
%
%   C = CTRANSPOSE(A) swaps the row and column indices of A.
%
%   See also TENMAT.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

tmp = a.rindices;
a.rindices = a.cindices;
a.cindices = tmp;
a.data = ctranspose(a.data);


%                            [delta,h,alpha] = iswnbr(vSQR,thetaSQR)
% ISWNBR  Checks feasibility w.r.t. wide region/neighborhood of Sturm-Zhang.
%   vTAR:= (1-alpha)*max(h,v) projection v onto theta-central region
%   delta = (sqrt(n)/theta) * norm(vTAR - v) / norm(v)
%
% **********  INTERNAL FUNCTION OF SEDUMI **********
%
% See also sedumi

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

disp('The SeDuMi binaries are not installed.')
disp('In Matlab, launch "install_sedumi" in the folder you put the SeDuMi files.')
disp('For more information see the file Install.txt.')
error(' ')

% ----------------------------------------
% r = n/thetaSQR
% hSQR = sumwNT/(r-|T|),  hubSQR = sumwNT/(r-|T| - |Q|)
% sumdifv = h*|T| - sumvT   (sumvT = sum(v_T), growing )
% sumdifw = hSQR*|T| - sumwT
% alpha = sumdifv / (r*h)
% deltaSQR = r * ( 2*alpha-alpha^2 - (1-alpha)^2 * sumdifw/gap )
% WE UPDATE sumdifv AND sumdifw IN A STABLE WAY
% ----------------------------------------
n = length(w); gap = sum(w); %#ok
sumwNT = gap;
r = n / thetaSQR;
cardT = 0; wQ = []; sumdifv = 0; sumdifw = 0;
cardQ = n;
hSQR = sumwNT / (r - cardT);  hubSQR = sumwNT / (r-(n-1));
for j = 1:n
    wj = w(j);
    if wj >= hubSQR              % wj >= hubSQR ==> not in T
        cardQ = cardQ - 1;
        hubSQR = sumwNT / (r-cardT-cardQ);
    elseif wj < hSQR             % wj < hSQR ==> in T
        cardT = cardT + 1;
        cardQ = cardQ - 1;
        hubSQR = (1-wj/sumwNT) * hubSQR;
        sumwNT = sumwNT - wj;
        oldhSQR = hSQR;
        hSQR = sumwNT / (r - cardT);
        sumdifw = sumdifw + (oldhSQR-wj) + cardT * (hSQR-oldhSQR);
        sumdifv = sumdifv + (sqrt(oldhSQR)-sqrt(wj)) + ...
            cardT * (sqrt(hSQR)-sqrt(oldhSQR));
    else                    % Inconclusive: j in Q
        wQ = [wQ;wj];
    end % if
end % for
% ----------------------------------------
% The same treatment for the Q set, but we
% sort the (presumably short) wQ first.
% ----------------------------------------
if ~isempty(wQ)
    sort(wQ);
    STOP = 0; j = 1;
    while ~STOP
        wj = wQ(j);
        if wj >= hSQR
            STOP = 1;
        else
            cardT = cardT + 1;
            sumwNT = sumwNT - wj;
            oldhSQR = hSQR;
            hSQR = sumwNT / (r - cardT);
            sumdifw = sumdifw + (oldhSQR-wj) + cardT * (hSQR-oldhSQR);
            sumdifv = sumdifv + (sqrt(oldhSQR)-sqrt(wj)) + ...
                cardT * (sqrt(hSQR)-sqrt(oldhSQR));
            j = j+1;
            if j > length(wQ)
                STOP = 1;
            end
        end
    end
end % treatment Q
% ----------------------------------------
% alpha = sumdifv/(r*h)
% deltaSQR = r * ( 2*alpha-alpha^2 - (1-alpha)^2 * sumdifw/gap )
%  (THE ABOVE DIFFERENCE SHOULD NOT BE NUMERICALLY DANGEROUS,
%    SINCE alpha IS *SIGNIF* BIGGER THAN sumdifw/gap )
% ----------------------------------------
h = sqrt(hSQR);
alpha = sumdifv/ (r*h);
deltaSQR = alpha*(2-alpha) - (1-alpha)^2 * sumdifw/gap;
delta = sqrt(r*deltaSQR);


function [dets, boxes, t] = cascade_detect(pyra, model, thresh)

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2009-2012 Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

th = tic();

% gather PCA root filters for convolution
numrootfilters = length(model.rootfilters);
rootfilters = cell(numrootfilters, 1);
for i = 1:numrootfilters
  rootfilters{i} = model.rootfilters{i}.wpca;
end

% compute PCA projection of the feature pyramid
projpyra = project_pyramid(model, pyra);

% stage 0: convolution with PCA root filters is done densely
% before any pruning can be applied

% Precompute location/scale scores
loc_f      = loc_feat(model, pyra.num_levels);
loc_scores = cell(model.numcomponents, 1);
for c = 1:model.numcomponents
  loc_w         = model.loc{c}.w;
  loc_scores{c} = loc_w * loc_f;
end
pyra.loc_scores = loc_scores;

numrootlocs = 0;
nlevels = size(pyra.feat,1);
rootscores = cell(model.numcomponents, nlevels);
s = 0;  % will hold the amount of temp storage needed by cascade()
for i = 1:pyra.num_levels
  s = s + size(pyra.feat{i},1)*size(pyra.feat{i},2);
  if i > model.interval
    scores = fconv_var_dim(projpyra.feat{i}, rootfilters, 1, numrootfilters);
    for c = 1:model.numcomponents
      u = model.components{c}.rootindex;
      v = model.components{c}.offsetindex;
      rootscores{c,i} = scores{u} + model.offsets{v}.w + loc_scores{c}(i);
      numrootlocs = numrootlocs + numel(scores{u});
    end
  end
end
s = s*length(model.partfilters);
model.thresh = thresh;
% run remaining cascade stages and collect object hypotheses
coords = cascade(model, pyra, projpyra, rootscores, numrootlocs, s);

boxes = coords';
dets = boxes(:,[1:4 end-1 end]);
t = toc(th);


function gray = ip2volTSeries(inplane,gray,selectedScans,method)
%
% function gray = ip2volTSeries(inplane,gray,[selectedScans],[method])
%
% Uses point sampling and nearest neighbor interpolation to map
% tSeries from inplane view to gray view. Inplane and
% gray views must already be open. Loads the inplane tSeries as
% it goes.
%
% Output tSeries matrices are (as usual) nFrames x nVoxels in size
% where nVoxels is the number of gray voxels that correspond to the
% inplanes, i.e., size(gray.grayCoords,2).
%
% selectedScans:
%   0 - do all scans
%   number or list of numbers - do only those scans
%   default - prompt user via selectScans dialog
% %
% method : 'nearest' [default], 'linear' interpolation
%
% If you change this function make parallel changes in:
%    ip2volCorAnal, ip2volParMap, ip2volSpatialGradient,
%    vol2flatCorAnal, vol2flatParMap, vol2flatTSeries
%
% djh, 2/2001
% ras, 10/2005, fixed to agree with a concomittant change in
% upSampleFactor.
% sod, 11/2005 added linear interpolation

% Don't do this unless inplane is really an inplane and gray is really a gray
if ~strcmp(inplane.viewType,'Inplane') || ~(strcmp(gray.viewType,'Gray') || strcmp(gray.viewType,'Volume'))
    myErrorDlg('ip2grayTSeries can only be used to transform from inplane to gray.');
end

% Check that both gray & flat are properly initialized
if isempty(inplane)
    myErrorDlg('Inplane view must be open.  Use "Open Inplane Window" from the Window menu.');
end
if isempty(gray)
    myErrorDlg('Gray view must be open.  Use "Open Gray Window" from the Window menu.');
end

% check for compatible data types
checkTypes(inplane,gray);

nScans = viewGet(gray, 'numScans');

% (Re-)set scanList
if ~exist('selectedScans','var') || isempty(selectedScans),
    selectedScans = er_selectScans(inplane);
elseif selectedScans == 0
    selectedScans = 1:nScans;
end
if isempty(selectedScans)
    disp('Analysis aborted')
    return
end

if nargin < 4,
    method = 'nearest';
end;
fprintf('[%s]: using %s interpolation.\n',mfilename,method);

% Size of the output tSeries matrices is: nFrames x nVoxels
% Also need to know number of inplane slices
nVoxels = size(gray.coords,2);

% open mrvWaitbar
verbose = prefsVerboseCheck;
if verbose,
    waitHandle = mrvWaitbar(0, 'Interpolating tSeries.  Please wait...');
end

% Tranform gray coords to inplane functional coords. Previously, the code
% to do this xform was duplicated in many functions, including this one.
% It is now a separate routine. The third argument when set to true returns
% the precise (non-integer) functional coords, which are interpolated
% below.
ipCoords = ip2volXformCoords(gray, inplane, true);

% Loop through the scans
for scan = selectedScans

    % Scale and round the grayCoords
    fprintf('Xforming scan %i ...\n',scan);

    % only round for nearest neighbor interpolation
    switch method,
        case 'nearest', ipCoords=round(ipCoords);
        case 'linear'
        otherwise,
            fprintf('Unknown interpolation method: %s\n',method);
            return
    end

    nFrames = viewGet(gray,'numFrames',scan);

    % Reset to NaNs
    tSeries = repmat(single(NaN), [nFrames nVoxels]);

    [~,nii] = loadtSeries(inplane,scan);
    funcData = niftiGet(nii, 'data');

    for frame = 1:nFrames
        subData = double(funcData(:,:,:,frame));
        tSeries(frame,:) = interp3(subData, ...
            ipCoords(2,:), ...
            ipCoords(1,:), ...
            ipCoords(3,:), ...
            method);
    end

    % Save tSeries
    % This should not need to be changed to new version because it is only
    % for the gray view
    savetSeries(tSeries, gray, scan, 1);

    % update the mrvWaitbar
    if verbose,
        mrvWaitbar(find(selectedScans==scan)/nScans, waitHandle);
    end
end %for

% close mrvWaitbar
if verbose, close(waitHandle); end

fprintf('Done xforming tSeries.\n');

return


classdef ARdrone < quadcopter
    properties
        config = 'OUTDOOR'; % Indoor/outdoor dynamics
        % Performance Parameters
        battery_capacity  = 1000;	% Maximum battery capacity (mAh)
        battery_voltage   = 11.1;	% Maximum battery voltage (V)
        flight_time       = 720; 	% Rated flight time (s)
    end
    methods
        % Constructor
        function [this] = ARdrone(varargin)
            % Call the super class
            this@quadcopter(varargin);

            % Create the dynamics of a quadcopter
            this.SENSORS  = this.CreateSENSORS();     % Get the sensor parameters
            this.DYNAMICS = this.CreateDYNAMICS();
            this.GEOMETRY = this.CreateGEOMETRY();
            this.radius   = this.GEOMETRY.length/2;   % Use the "length" to define the radius

            % //////////////// Check for user overrides ///////////////////
            this = this.ApplyUserOverrides(varargin); % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % Setup
        function [this] = setup(this,localXYZVelocity,localXYZrotations)
            % This function calculates the intial state for a quadrotor
            % object.

            % For a state vector defined as x_k:
            % [x,y,z,phi,theta,psi,dx,dy,dz,wx,wy,wz]

            % THIS MODEL OPERATES IN THE GLOBAL FRAME
            %             p0 = this.GetGLOBAL('position');   % True global position
            %             v0 = this.GetGLOBAL('velocity');   % True global velocity
            %             % GET THE ROTATION MATRIX fixed local axes -> rotated global pose
            %             eta0 = OMAS_geometry.eulersToRotationMatrix(localXYZrotations);
            %             % Build an initial (global) state vector
            %             vecR0   = reshape(R0',9,1);        % Defines local vector to global vector
            %             omega0  = zeros(3,1);
            %             x0      = [p0;v0;vecR0;omega0];

            p0 = this.GetGLOBAL('position');   % True global position
            v0 = this.GetGLOBAL('velocity');   % True global velocity
            eta0   = localXYZrotations;
            omega0 = zeros(3,1);
            x0 = [p0;eta0;v0;omega0];

            % ASSIGN THE LOCAL FRD STATE
            this.SetGLOBAL('priorState',x0);
            this.localState = x0;
        end
        % Main
        function [this] = main(this,ENV,varargin)

            % //////////// CHECK FOR NEW INFORMATION UPDATE ///////////////
            % Update the agent with the environmental data
            [this,~,~] = this.GetAgentUpdate(ENV,varargin{1});
            % /////////////////////////////////////////////////////////////

            % The state reference
            desiredPosition = [1;1;0];

            % Call the controller loop
            this = this.Controller(ENV,desiredPosition);

            % /////////////// RECORD THE AGENT-SIDE DATA //////////////////
            this.DATA.inputNames = {'$\dot{x}$ (m/s)','$\dot{y}$ (m/s)','$\dot{z}$ (m/s)',...
                '$p$ (rad/s)','$q$ (rad/s)','$r$ (rad/s)'};
            % Record the control inputs
            this.DATA.inputs(1:length(this.DATA.inputNames),ENV.currentStep) = this.localState(7:end);
        end
    end
    %% ////////////////////// AUXILLARY METHODS ///////////////////////////
    methods
        % Get the state update (using ode45)
        function [x_k_plus] = UpdateLocalState(this,ENV,x_k,y_desired)
            % This function computes the state update for the current agent
            % using the ode45 function.

            % Integrate across the time delta "dt"
            opts = odeset('RelTol',1e-2,'AbsTol',ENV.dt*1E-1);

            [~,Xset] = ode45(@(t,X) this.ClosedLoopDynamics_position(X,y_desired),[0 ENV.dt],x_k,opts);
            %            [~,Xset] = ode45(@(t,X) this.ClosedLoopDynamics_velocity(X,y_desired),[0 ENV.dt],x_k,opts);

            % Assign the integral state
            x_k_plus = Xset(end,:)';
        end
        % ARdrone Dynamics (Closed-loop)
        function [dxdt] = ClosedLoopDynamics_position(this,x_k,p_desired)

            % Extract the current state properties
            %             p_k   = x_k(1:3,1);
            %             eta_k = x_k(4:6,1);
            %             v_k   = x_k(7:9,1);
            %             w_k   = x_k(10:12,1);
            %             x_k = [p_desired(1:3);zeros(3,1);[0;0;psi_desired];zeros(3,1)];

            psi_desired = 0;
            % Define the desired state
            x_desired = [...
                p_desired(1:3);...
                [0;0;psi_desired];...
                zeros(3,1);...
                zeros(3,1)];

            % Extract DYNAMIC parameters
            e3 = this.DYNAMICS.e3;
            g  = this.DYNAMICS.g;
            m  = this.DYNAMICS.m;
            I  = this.DYNAMICS.I;

            % X = [dx dy dz wx wy wz ddx ddy ddz dwx dwy dwz]
            % State matrix
            A = this.DYNAMICS.A;
            A(1:6,7:12) = eye(6);
            A(7:9,4:6)  = g*[ sin(psi_desired), cos(psi_desired), 0;
                             -cos(psi_desired), sin(psi_desired), 0;
                                            0,                0, 0];
%             A_prev = zeros(12);
%             A_prev(1:3,4:6) = eye(3);
%             A_prev(4:6,7:9) = g*[ sin(psi_desired), cos(psi_desired), 0;
%                                  -cos(psi_desired), sin(psi_desired), 0;
%                                                  0,                0, 0];
%             A_prev(7:9,10:12)=eye(3);

            % Input matrix
            B = this.DYNAMICS.B;
            B(7:9,1) = e3/m;
            B(10:12,2:4) = inv(I);

            % LQR control gain
            [K,~,~] = lqr(A,B,eye(12),eye(4));
            % Calculate the error
            e_k  = x_k - x_desired;
            du_k = -K*e_k;
            % Calculate the inputs
            u_k = [m*g;0;0;0] + du_k;

            % Provide the inputs to the open-loop dynamics
            [dxdt] = this.OpenLoopDynamics(x_k,u_k);
        end
        % Quadcopter Dynamics (Open-loop)
        function [dxdt] = OpenLoopDynamics(this,x_k,u_k)
            % Designed for a state vector defined as:
            % x_k = [x,y,z,phi,theta,psi,dx,dy,dz,p,q,r]

            % Extract the state parameters
            eta_k = x_k(4:6,1);
            v_k   = x_k(7:9,1);
            w_k   = x_k(10:12,1);

            % Define the rotation matrix from the euler angles
%             R_k = reshape(x_k(7:15),3,3);
            R_k = OMAS_geometry.eulersToRotationMatrix(eta_k);

            % Extract the inputs
            f   = u_k(1);
            tau = u_k(2:4);

            % Get the constants
            m  = this.DYNAMICS.m;
            g  = this.DYNAMICS.g;
            I  = this.DYNAMICS.I;
            e3 = this.DYNAMICS.e3;

            % nonlinear dynamics based on rotation dynamics
            p_dot    = v_k;
            v_dot    = f/m*R_k*e3-g*e3;
            % Euler angles
            %R_dot    = R_k*skew(w_k);
            %eta_dot  = OMAS_geometry.rotationMatrixToEulers(R_dot);
            eta_dot = w_k;
            %vecR_dot = reshape(R_dot,9,1);

            w_dot    = inv(I)*(tau-skew(w_k)*I*w_k);

            % THE STATE DIFFERENCE
            dxdt = [p_dot;eta_dot;v_dot;w_dot];
        end
        % Global update from the new state
        function [this] = GlobalUpdate(this,x_k_plus)
            % This function updates the global structure from the new state
            % definition: [p_k;eta_k;v_k;omega_k]

            % Sanity Check
            assert(IsColumn(x_k_plus,12),"Expecting a 12DOF state vector.");

            % Define the rotation matrix from the euler angles
            R_k_plus = OMAS_geometry.eulersToRotationMatrix(x_k_plus(4:6,1));
            % Define the new quaternion from R
            q_k_plus = OMAS_geometry.rotationMatrixToQuaternion(R_k_plus');

            % //////////////// UPDATE GLOBAL PROPERTIES ///////////////////
            [this] = this.GlobalUpdate_direct(...
                x_k_plus(1:3),...   % The global position is within the state
                x_k_plus(7:9),...   % The global velocity is within the state
                q_k_plus);          % Append the global quaternion

            % Ensure the local state is re-assigned
            this.localState = x_k_plus;
        end
    end
    methods (Static)
        % Define the open-loop dynamics
        function [dxdt] = ARdrone_OpenLoopDynamics(x_k,u_k)
            %#codegen

            %    This function was generated by the Symbolic Math Toolbox version 8.1.
            %    17-Jul-2018 15:13:41

            PHI_t = x_k(4,:);
            PHI_dot = x_k(10,:);
            PSI_dot = x_k(12,:);
            THETA_t = x_k(5,:);
            THETA_dot = x_k(11,:);
            omega_1 = u_k(1,:);
            omega_2 = u_k(2,:);
            omega_3 = u_k(3,:);
            omega_4 = u_k(4,:);
            x_dot = x_k(7,:);
            y_dot = x_k(8,:);
            z_dot = x_k(9,:);
            t2 = cos(PHI_t);
            t3 = cos(THETA_t);
            t4 = sin(PHI_t);
            t5 = sin(THETA_t);
            t6 = omega_1.^2;
            t7 = sqrt(2.0);
            t8 = omega_2.^2;
            t9 = omega_3.^2;
            t10 = omega_4.^2;
            t11 = t7.*t9.*8.516167950913242e-5;
            t12 = t7.*t10.*8.516167950913242e-5;
            t13 = PSI_dot.^2;
            dxdt = [x_dot;y_dot;z_dot;PHI_dot;THETA_dot;PSI_dot;t5.*(-9.81e2./1.0e2)+THETA_dot.*t4.*y_dot+THETA_dot.*t2.*z_dot-PSI_dot.*t2.*t3.*y_dot+PSI_dot.*t3.*t4.*z_dot;-PHI_dot.*z_dot+t3.*t4.*(9.81e2./1.0e2)+PSI_dot.*t5.*z_dot-THETA_dot.*t4.*x_dot+PSI_dot.*t2.*t3.*x_dot;t6.*(-3.73475e-5)-t8.*3.73475e-5-t9.*3.73475e-5-t10.*3.73475e-5+PHI_dot.*y_dot+t2.*t3.*(9.81e2./1.0e2)-PSI_dot.*t5.*y_dot-THETA_dot.*t2.*x_dot-PSI_dot.*t3.*t4.*x_dot;t11+t12+THETA_dot.*omega_1.*3.231842180365297e-3-THETA_dot.*omega_2.*3.231842180365297e-3+THETA_dot.*omega_3.*3.231842180365297e-3-THETA_dot.*omega_4.*3.231842180365297e-3-t6.*t7.*8.516167950913242e-5-t7.*t8.*8.516167950913242e-5-THETA_dot.^2.*t4.*9.996789383561644e-1-PSI_dot.*THETA_dot.*t2-t3.*t4.*t13+PSI_dot.*THETA_dot.*t2.*t3.*9.996789383561644e-1;-t11+t12+PHI_dot.*PSI_dot-PHI_dot.*omega_1.*3.231842180365297e-3+PHI_dot.*omega_2.*3.231842180365297e-3-PHI_dot.*omega_3.*3.231842180365297e-3+PHI_dot.*omega_4.*3.231842180365297e-3+t6.*t7.*8.516167950913242e-5-t7.*t8.*8.516167950913242e-5-t5.*t13+PHI_dot.*THETA_dot.*t4.*9.996789383561644e-1-PHI_dot.*PSI_dot.*t2.*t3.*9.996789383561644e-1;t6.*4.175141847767905e-4-t8.*4.175141847767905e-4+t9.*4.175141847767905e-4-t10.*4.175141847767905e-4-PHI_dot.*THETA_dot.*1.000321164757521+PHI_dot.*THETA_dot.*t2.*1.000321164757521+PSI_dot.*THETA_dot.*t5.*1.000321164757521+PHI_dot.*PSI_dot.*t3.*t4.*1.000321164757521];
        end
    end
    % Properties
    methods
        % Get the (ARdrone) SENSOR structure
        function [SENSORS]  = CreateSENSORS(this)
            % This function gets the ARdrone 2.0's sensor-specific data and
            % imports them to OMAS's obj.SENSOR structure.

            % Get the default SENSOR structure
            SENSORS = this.SENSORS;
            SENSORS.ultrasound_freq = 40E3;     % Ultrasonic range-finder frequency (Hz)
            SENSORS.ultrasound_range = 6;       % Ultrasonic range-fidner range (m)
            SENSORS.camera_freq = 30;           % Camera capture frequency (fps)
        end
        % Get the (generic) quadcopter dynamic properties
        function [DYNAMICS] = CreateDYNAMICS(this)

            % Import the default properties of a simple quadcopter
            DYNAMICS = this.CreateDYNAMICS_default();

            % ALTER THE DYNAMICS BASED ON THE CONFIGURATION
            switch upper(this.config)
                case 'NONE'
                    DYNAMICS.m = 0.366;
                case 'OUTDOOR'
                    DYNAMICS.m = 0.400; % Arm length equiv : 0.3196
                case 'INDOOR'
                    DYNAMICS.m = 0.436;
                otherwise
                    error('Configuration not recognised');
            end
            % Assign the ARdrone properties
%             DYNAMICS.I = 1.0E-06.*[...
%                 2.8032E+04,0.0000E+00,0.0000E+00;
%                 0.0000E+00,2.8032E+04,0.0000E+00;
%                 0.0000E+00,0.0000E+00,2.8023E+04];
%
            % Control properties
            % Plant matrix
            DYNAMICS.A = zeros(12);
%             DYNAMICS.A(4:6,7:9)   = eye(3)*DYNAMICS.g;
%             DYNAMICS.A(1:3,4:6)   = eye(3);
%             DYNAMICS.A(7:9,10:12) = eye(3);
            % Input matrix
            DYNAMICS.B = zeros(12,4);
%             DYNAMICS.B(4:6,1)     = DYNAMICS.e3/DYNAMICS.m;
%             DYNAMICS.B(10:12,2:4) = inv(DYNAMICS.I);
            % Observation matrix
            DYNAMICS.C = eye(12);
            % Feed-forward matrix
            DYNAMICS.D = eye(4);
        end
        % Get the (ARdrone) GEOMETRY structure
        function [GEOMETRY] = CreateGEOMETRY(this)

            % Parse the geometry associated with the object
            [GEOMETRY] = this.GetObjectGeometry(this);

            % ALTER THE DYNAMICS BASED ON THE CONFIGURATION
            switch upper(this.config)
                case 'NONE'
                    GEOMETRY.width  = 0.450;
                    GEOMETRY.length = 0.290;
                case 'OUTDOOR'
                    GEOMETRY.width  = 0.452;
                    GEOMETRY.length = 0.452; % Arm length equiv : 0.3196
                case 'INDOOR'
                    GEOMETRY.width  = 0.515;
                    GEOMETRY.length = 0.515;
                otherwise
                    error('Configuration not recognised');
            end
        end
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Function that outputs a struct "settings" with the settings used in the
% GNSS-aided INS
%
% Edit: Isaac Skog (skog@kth.se), 2016-09-01
% Revised: Bo Bernhardsson 2018-01-01
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function settings = gnss_imu_local_tan_example_settings()

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%              GENERAL PARAMETERS         %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
settings.gnss_outage = 'on';
settings.outagestart = 120;
settings.outagestop = 150;
settings.non_holonomic = 'off';
settings.speed_aiding = 'off';

settings.init_heading = 0*pi/180;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%             FILTER PARAMETERS           %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Process noise covariance (Q)
% Standard deviations, need to be squared
settings.sigma_acc = 0.2;
settings.sigma_gyro = deg2rad(0.5);
settings.sigma_acc_bias = 0.00;
settings.sigma_gyro_bias = deg2rad(0.00);

% GNSS position measurement noise covariance (R)
% Standard deviations, need to be squared
settings.sigma_gps =2/sqrt(3); %[m]
settings.sigma_speed = 1; %[m/s]  Trim here
%settings.sigma_speed = 24; %[m/s]  Trim here
settings.sigma_non_holonomic = 20; %[m/s] Trim here
%settings.sigma_non_holonomic = 3; %[m/s] Trim here

% Initial Kalman filter uncertainties (standard deviations)
settings.factp(1) = 100;                                 % Position [m]
settings.factp(2) = 100;                                  % Velocity [m/s]
settings.factp(3:5) = (pi/180*[50 50 50]');     % Attitude (roll,pitch,yaw) [rad]
settings.factp(6) = 0.02;                               % Accelerometer biases [m/s^2]
settings.factp(7) = deg2rad(2);                     % Gyro biases [rad/s]

settings.gravity  = [0, 0, 9.8184]';        % NED frame 重力为正

end


% demo for creating an SPM M/EEG dataset from arbitrary data using
% conversion of simple Fieldtrip raw data struct.
% SPM8 internal format is quite complex but is transparent to the user
% as meeg object's methods take care of maintaining its consistency.
% The most straightforward way to convert arbitrary data that is available
% as an ASCII file or *.mat file with some variables to SPM8 is to create
% a quite simple Fieldtrip raw data struct and then use SPM's
% spm_eeg_ft2spm to convert this struct to SPM8 file. Missing information
% can then be supplemented using meeg methods and SPM functions.
% Fieldtrip raw struct must contain the following fields:
% .fsample - sampling rate (Hz)
% .trial - cell array of matrices with identical dimensions channels x time
% .time - cell array of time vectors (in sec), the same length as the
%         second dimension of the data. For SPM8 they must be identical.
% .label - cell array of strings, list of channel labels. Same length as
%         the first dimension of the data.
%__________________________________________________________________________
% Copyright (C) 2010 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: spm_eeg_convert_arbitrary_data.m 5404 2013-04-12 15:08:57Z vladimir $

% Initialize SPM
%--------------------------------------------------------------------------
spm('defaults','EEG');

% Some details about the data
%--------------------------------------------------------------------------
Nchannels = 5;
Nsamples  = 500;
Ntrials   = 50;
TimeOnset = -0.1; % in sec
Fsample = 1000;

chlabels = {
            'LFP1'
            'LFP2'
            'LFP3'
            'LFP4'
            'LFP5'
            };

% define the output file name
%--------------------------------------------------------------------------
fname = 'arbitrary_data_example';

% create data array
%--------------------------------------------------------------------------
data = randn([Nchannels, Nsamples, Ntrials]);

% create the time axis (should be the same for all trials)
%--------------------------------------------------------------------------
timeaxis = [0:(Nsamples-1)]./Fsample + TimeOnset;

% Create the Fieldtrip raw struct
%--------------------------------------------------------------------------

ftdata = [];

for i = 1:Ntrials
   ftdata.trial{i} = squeeze(data(:, :, i));
   ftdata.time{i} = timeaxis;
end

ftdata.fsample = Fsample;
ftdata.label = chlabels;
ftdata.label = ftdata.label(:);

% Convert the ftdata struct to SPM M\EEG dataset
%--------------------------------------------------------------------------
D = spm_eeg_ft2spm(ftdata, fname);

% Examples of providing additional information in a script
% [] comes instead of an index vector and means that the command
% applies to all channels/all trials.
%--------------------------------------------------------------------------
D = type(D, 'single');                        % Sets the dataset type
D = chantype(D, ':', 'LFP');                   % Sets the channel type
D = conditions(D, 1:Ntrials, 'Condition 1');  % Sets the condition label

% save
%--------------------------------------------------------------------------
save(D);


function L = listdijkstra(L,W,s,d)

index=size(W,1);
while index>0
    if W(2,d)==W(size(W,1),d)
        L=[L s];
        index=0;
    else
        index2=size(W,1);
        while index2>0
            if W(index2,d)<W(index2-1,d)
                L=[L W(index2,1)];
                L=listdijkstra(L,W,s,W(index2,1));
                index2=0;
            else
                index2=index2-1;
            end
            index=0;
        end
    end
end


function a = gt( x, y )

%Disciplined convex programming information for GT (>):
%   The right-hand side of a less-than constraint must be convex. The
%   left-hand side must be concave. Of course, real constant and affine
%   expressions are both convex and concave and can be used on either
%   side as well.
%
%Disciplined geometric programming information for GT (>):
%   The right-hand side of a less-than constraint must be log-convex---
%   including positive constants, monomials, posynomials, generalized
%   posynomials, and products thereof. The left-hand side must be log-
%   concave---including positive constants, monomials, reciprocals of
%   log-convex expressions, and products thereof.
%
%Note that CVX does not distinguish between strict greater-than (>) and
%greater-than-or-equal (<=) constraints; they are treated identically.
%Feasible interior-point solvers tend to return points which satisfy
%strict inequality, but not all solvers do.

b = newcnstr( evalin( 'caller', 'cvx_problem', '[]' ), x, y, '>' );
if nargout, a = b; end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.