function timeString = BF_TheTime(tsec,formatLong)
% BF_TheTime    Converts a duration (seconds) to a human-interpretable string
%
% e.g., converts to minutes or hours or days as appropriate) output is something
% like '25.5 minutes' or '3.2 days' -- always displays to one decimal place.
%
% This code is useful for displaying user feedback on tic/toc statements.
%
%---INPUTS:
% tsec, the duration in seconds
% formatLong, (i) 0: display short units (like 's' instead of 'seconds')
%                    [default]
%             (ii) 1: display long units (like 'seconds' instead of 's')
%
%---OUTPUT:
% timeString, an interpretable text version of the input time.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if nargin < 2 || isempty(formatLong)
    formatLong = false; % Set to 1 to use a longer format for the unit
end

if tsec < 1E-3
    if formatLong
        timeString = '< 1 milliseconds';
    else
        timeString = '< 1ms';
    end
elseif tsec < 1 % less than a second, display in integer number of milliseconds
    if formatLong
        timeString = sprintf('%.0f milliseconds',tsec*1000);
    else
        timeString = sprintf('%.0fms',tsec*1000);
    end
elseif tsec <= 60 % less than a minute, display in seconds
    if formatLong
        timeString = sprintf('%.1f seconds',tsec);
    else
    	timeString = sprintf('%.1fs',tsec);
    end
elseif tsec <= 60*60 % less than an hour, display in minutes
    if formatLong
        timeString = sprintf('%.1f minutes',tsec/60);
    else
        timeString = sprintf('%.1fmin',tsec/60);
    end
elseif tsec <= 60*24*60 % less than a day, display in hours
    if formatLong
        timeString = sprintf('%.1f hours',tsec/60/60);
    else
        timeString = sprintf('%.1fh',tsec/60/60);
    end
% elseif tsec<=60*24*7*60 % less than a week, display in days
else % display in days
	timeString = sprintf('%.1f days',tsec/60/60/24);
end

end


function h = text(m,varargin)
% plot Miller indece
%
% Input
%  m  - Miller
%
% Options
%  symmetrised - plot symmetrically equivalent directions
%  antipodal   - include antipodal symmetry
%  labeled     - plot Miller indice as label
%  label       - plot user label
%
% See also
% vector3d/text

% symmetrise
if check_option(varargin,'symmetrised') && ~check_option(varargin,'skipSymmetrise')

  [m,l] = symmetrise(m,varargin{:},'unique','noAntipodal');

  % symmetrise labels
  if ~check_option(varargin,'labeled') && ~isempty(varargin)
    strings = ensurecell(varargin{1});
    if iscellstr(varargin{1}) && ~isempty(strings)

      if numel(strings)==1
        strings = repcell(strings{1},length(m),1);
      else
        strings = strings(repelem(1:numel(strings),l));
      end
      varargin{1} = strings;
    end
  end

  varargin = [varargin,{'skipSymmetrise','noAntipodal'}];
end

% ensure specific plot options
varargin = [varargin(1),m.CS.plotOptions,varargin(2:end)];

h = text@vector3d(m,varargin{:});

if nargout == 0, clear h; end


function rbfhKernDisplay(kern, spacing)

% RBFHKERNDISPLAY Display parameters of the RBFH kernel.
% FORMAT
% DESC displays the parameters of the radial basis function heat
% kernel and the kernel type to the console.
% ARG kern : the kernel to display.
%
% FORMAT does the same as above, but indents the display according
% to the amount specified.
% ARG kern : the kernel to display.
% ARG spacing : how many spaces to indent the display of the kernel by.
%
% SEEALSO : rbfhKernParamInit, modelDisplay, kernDisplay
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin > 1
  spacing = repmat(32, 1, spacing);
else
  spacing = [];
end
spacing = char(spacing);
fprintf(spacing);
fprintf('RBFH inverse width time: %2.4f (length scale %2.4f)\n', ...
        kern.inverseWidthTime, 1/sqrt(kern.inverseWidthTime));
fprintf(spacing);
fprintf('RBFH inverse width space: %2.4f (length scale %2.4f)\n', ...
        kern.inverseWidthSpace, 1/sqrt(kern.inverseWidthSpace));


function[image] = labelMapToColorImage(labelMap, labelCount, cmap)
% [image] = labelMapToColorImage(labelMap, [labelCount], [cmap])
%
% Convert an mxn labelMap to a color image.
% Each index is replaced by a certain color from a colormap.
% If labelCount is specified, we use absolute colors, otherwise the colors
% are sorted by label indices.
%
% Copyright by Holger Caesar, 2014

% Default arguments
if ~exist('labelCount', 'var'),
    labelCount = [];
end;
if ~exist('cmap', 'var'),
    cmap = @jet;
end;

% Get a unique list of all labels
labelList = unique(labelMap(:));
labelList(labelList == 0) = [];
labelListCount = numel(labelList);

% Initialize result
image = zeros(size(labelMap, 1), size(labelMap, 2), 3);
if isempty(labelCount),
    % Take a variable color scheme
    colormap = cmap(labelListCount);
else
    % Take a fixed color scheme relative to the number of labels in the
    % dataset
    colormap = cmap(labelCount);
    colormap = colormap(labelList, :);
end;

% Go through each label and replace its pixels by a color
for labelListIdx = 1 : labelListCount,
    labelIdx = labelList(labelListIdx);
    indices = labelMap == labelIdx;

    curImage = cat(3, ...
        indices .* colormap(labelListIdx, 1), ...
        indices .* colormap(labelListIdx, 2), ...
        indices .* colormap(labelListIdx, 3));
    image = image + curImage;
end;


% DEMO3C - Chaining files using ncml and finding attributes using 'value4key'

echo('on')
o2_ = 'Oxygen';
time_ = 'esecs';

%% ---- Create the full pathname for an ncml file

scriptname = which('demo3c');  %% find the absolute path of a local file
fName = strrep(scriptname,'.m','.ncml') ; % create a path to the associated ncml file

if ~exist(fName,'file')
  error(strcat(fName,' does not exist.'));
end

%% ---- Access the OpenDAP datasets through an ncml file

nc = ncdataset(fName); % read the local ncml file
t = [nc.time(time_)];
o2 = [nc.data(o2_)];

%% ---- Find the units of Oxygen and label the display
attr = nc.attributes(o2_);
units = value4key(attr, 'units'); % Retrieve the units value
name = value4key(attr, 'long_name'); % Retrieve the long_name value

%% ---- Plot the data
figure;
plot(t, o2);...
datetick('x');...
ylabel([name ' [' units ']']);...
title({'M1 Mooring in Monterey Bay',nc.location},'interpreter','none');...
grid;...
shg
echo('off')


function freqplot(axs_hand,mode)
% FREQPLOT Frequency response plotting.
%          FREQPLOT handles the plotting of all graphs involved with
%          frequency.

% Author: Craig Borghesani
% Date: 8/8/94
% Revised: 10/18/94, 11/16/94, 12/10/94
% Copyright (c) 1999, Prentice-Hall

% mode represents all the options for frequency response.
% mode = 13, switching systems
% mode = 14, switching pages
% mode = 15, changing layouts

% if no frequency plots on current page, dont do anything
if ~length(axs_hand), return; end

% obtain handle information
f = gcf;
ui_data = get(f,'userdata');
ui_han = ui_data{1};

% current plant response
cur_sys   = get(ui_han(30),'userdata');
oth_sys   = [1:(cur_sys-1),(cur_sys+1):3];
plant_mat = get(ui_han(3+cur_sys),'userdata');
for_mat   = get(ui_han(6+cur_sys),'userdata');
bac_mat   = get(ui_han(9+cur_sys),'userdata');
sys_res   = get(ui_han(cur_sys),'userdata');
font_size = get(ui_han(42),'userdata');
stat_bar  = get(ui_han(43),'userdata');
cur_axs   = get(ui_han(32),'userdata');
w         = sys_res(1,:);
sys_cplx  = sys_res(2,:);

sys_axes  = ui_han(12);
show_all  = ui_han(25);
open_loop = ui_han(60);
clos_loop = ui_han(61);
exact     = ui_han(62);
straight  = ui_han(63);
margins   = ui_han(64);
range1    = 60:61;
range2    = 62:64;
fontname = 'times';

% extract data stored in axis handles
axs_data = [];
for k = 1:length(axs_hand),
 axs_data = [axs_data;get(axs_hand(k),'userdata')];
end
nyq = find(axs_data(:,1)==1);
bodm = find(axs_data(:,1)==2);
bodp = find(axs_data(:,1)==3);
nic = find(axs_data(:,1)==4);

if length(mode) & any(mode == [1,2]),

 if mode == 1, % open loop
  if strcmp(get(open_loop,'checked'),'on'),
   set(open_loop,'checked','off');
  else
   set(open_loop,'checked','on');
  end
 end

 if mode == 2, % closed loop
  if strcmp(get(clos_loop,'checked'),'on'),
   set(clos_loop,'checked','off');
  else
   set(clos_loop,'checked','on');
  end
 end

 ct = 2;
 for k = range2,
  if strcmp(get(ui_han(k),'checked'),'on') & ...
     strcmp(get(ui_han(k),'enable'),'on'),
   mode(ct) = k-(range1(1)-1);
  else
   mode(ct) = 0;
  end
  ct = ct + 1;
 end

elseif length(mode) & any(mode == [3,4,5]),

 ct = 2;
 for k = range1,
  if strcmp(get(ui_han(k),'checked'),'on') & ...
     strcmp(get(ui_han(k),'enable'),'on'),
   mode(ct) = k-(range1(1)-1);
  else
   mode(ct) = 0;
  end
  ct = ct + 1;
 end

end

if length(mode) < 2,
 if ~length(mode) | any(mode == [12:15]),

% determine various states of frequency response environments
  if ~length(mode), mode = 0; end
  ct = 2;
  for k = [range1,range2],
   if strcmp(get(ui_han(k),'checked'),'on') & ...
      strcmp(get(ui_han(k),'enable'),'on'),
    mode(ct) = k-(range1(1)-1);
   else
    mode(ct) = 0;
   end
   ct = ct + 1;
  end

 end
end

open_flag = strcmp(get(open_loop,'checked'),'on');
clos_flag = strcmp(get(clos_loop,'checked'),'on');
show_flag = strcmp(get(show_all,'label'),'Show All Systems');
hide_flag = strcmp(get(show_all,'label'),'Hide Other Systems');

if clos_flag,
 bac_cplx = termcplx(bac_mat,w);
 tmp_cplx = sys_cplx./bac_cplx;
 clos_cplx = tmp_cplx./(1+sys_cplx);
end

mode_len = 3;
if length(mode) == 3 & hide_flag,
 mode = [mode,14];
 mode_len = 4;
end

if any(mode==3), % exact plot

 if strcmp(get(exact,'checked'),'off') | length(mode)>mode_len,
  set(exact,'checked','on');

  if length(nyq), % nyquist plot

% update current system frequency response
   if all(mode ~= 12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(nyq,1+oth_sys),'vis','off');
     set(axs_data(nyq,4+oth_sys),'vis','off');
    elseif any(mode == 13),
     if open_flag,
      set(axs_data(nyq,1+oth_sys),'color','r');
      set(axs_data(nyq,4+oth_sys),'color','b');
     end
%     if clos_flag,
%      set(axs_data(nyq,4+oth_sys),'color','b');
%     end
    end

    if open_flag & any(mode == 1),
     set(stat_bar,'string','Computing exact open-loop frequency response');
     rl = real(sys_cplx);  im = imag(sys_cplx);
     set(axs_data(nyq,1+cur_sys),'xdata',rl,'ydata',im,...
           'color','g','vis','on');
     set(axs_data(nyq,4+cur_sys),'xdata',rl,'ydata',-im,...
           'color','m','vis','on');
    elseif any(mode == 1),
     set(stat_bar,'string','Removing exact open-loop frequency response');
     set(axs_data(nyq,1+cur_sys),'vis','off');
     set(axs_data(nyq,4+cur_sys),'vis','off');
    end

%    if clos_flag,
%     ers = 'norm';
%     if ~open_flag, ers = 'xor'; end
%     rl = real(clos_cplx);  im = imag(clos_cplx);
%     set(axs_data(nyq,4+cur_sys),'xdata',rl,'ydata',im,...
%           'color','m','vis','on');
%    elseif any(mode == 2),
%     set(axs_data(nyq,4+cur_sys),'vis','off');
%    end

   end

% switching pages or requesting to veiw all systems
   if (any(mode == 12) & show_flag) | (any(mode == 14) & hide_flag) | ...
      (any(mode == 15) & hide_flag),

    for k = 1:2,
     oth_res = get(ui_han(oth_sys(k)),'userdata');
     oth_cplx = oth_res(2,:);

%     if clos_flag,
%      oth_bac = get(ui_han(9+oth_sys(k)),'userdata');
%      bac_cplx = termcplx(oth_bac,w);
%      tmp_cplx = oth_cplx./bac_cplx;
%      oth_clos = tmp_cplx./(1+oth_cplx);
%     end

     if open_flag,
      rl=real(oth_cplx); im=imag(oth_cplx);
      set(axs_data(nyq,1+oth_sys(k)),'xdata',rl,'ydata',im,...
                'color','r','vis','on','erase','norm');
      set(axs_data(nyq,4+oth_sys(k)),'xdata',rl,'ydata',-im,...
            'color','b','vis','on');
     end

%     if clos_flag,
%      rl=real(oth_clos); im=imag(oth_clos);
%      set(axs_data(nyq,4+oth_sys(k)),'xdata',rl,'ydata',im,...
%                'color','b','vis','on','erase','norm');
%     end
    end

   elseif any(mode == 12) | any(mode == 14) | any(mode == 15),

    set(axs_data(nyq,1+oth_sys),'vis','off');
    set(axs_data(nyq,4+oth_sys),'vis','off');

   end

   if any(mode == 15), % setting axis limits to full after page layout
    pageview(1,axs_hand(nyq));
   end

  end

  if length(bodm), % bode (magnitude) plot

% update current system frequency response
   if all(mode~=12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(bodm,1+oth_sys),'vis','off');
     set(axs_data(bodm,4+oth_sys),'vis','off');
    elseif any(mode == 13),
     if open_flag,
      set(axs_data(bodm,1+oth_sys),'color','r');
     end
     if clos_flag,
      set(axs_data(bodm,4+oth_sys),'color','b');
     end
    end

    if open_flag & any(mode == 1),
     set(stat_bar,'string','Computing exact open-loop frequency response');
     wdb = w;
     db = 20*log10(abs(sys_cplx));
     set(axs_data(bodm,1+cur_sys),'xdata',wdb,'ydata',db,...
           'color','g','vis','on');
    elseif any(mode == 1),
     set(stat_bar,'string','Removing exact open-loop frequency response');
     set(axs_data(bodm,1+cur_sys),'vis','off');
    end

    if clos_flag & any(mode == 2),
     set(stat_bar,'string','Computing exact closed-loop frequency response');
     ers = 'norm';
     if ~open_flag, ers = 'norm'; end
     wdb = w;
     db = 20*log10(abs(clos_cplx));
     set(axs_data(bodm,4+cur_sys),'xdata',wdb,'ydata',db,...
           'color','m','vis','on');
    elseif any(mode == 2),
     set(stat_bar,'string','Removing exact closed-loop frequency response');
     set(axs_data(bodm,4+cur_sys),'vis','off');
    end

   end

% switching pages or requesting to veiw all systems
   if (any(mode == 12) & show_flag) | (any(mode == 14) & hide_flag) | ...
      (any(mode == 15) & hide_flag),

    for k = 1:2,
     oth_res = get(ui_han(oth_sys(k)),'userdata');
     oth_w = oth_res(1,:);
     oth_cplx = oth_res(2,:);

     if clos_flag,
      oth_bac = get(ui_han(9+oth_sys(k)),'userdata');
      bac_cplx = termcplx(oth_bac,w);
      tmp_cplx = oth_cplx./bac_cplx;
      oth_clos = tmp_cplx./(1+oth_cplx);
     end

     if open_flag,
      wdb = oth_w;
      db = 20*log10(abs(oth_cplx));
      set(axs_data(bodm,1+oth_sys(k)),'xdata',wdb,'ydata',db,'color','r',...
                                    'vis','on','erase','norm');
     end

     if clos_flag,
      wdb = oth_w;
      db = 20*log10(abs(oth_clos));
      set(axs_data(bodm,1+oth_sys(k)),'xdata',wdb,'ydata',db,'color','b',...
                                    'vis','on','erase','norm');
     end
    end

   elseif any(mode == 12) | any(mode == 14) | any(mode == 15),

    set(axs_data(bodm,1+oth_sys),'vis','off');
    set(axs_data(bodm,4+oth_sys),'vis','off');

   end

   if any(mode == 15), % setting axis limits to full after page change
    pageview(1,axs_hand(bodm));
   end

  end

  if length(bodp), % bode (phase) plot

   axsp=[get(axs_hand(bodp),'xlim'),get(axs_hand(bodp),'ylim')];

% update current system frequency response
   if all(mode~=12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(bodp,1+oth_sys),'vis','off');
     set(axs_data(bodp,4+oth_sys),'vis','off');
    elseif any(mode == 13),
     if open_flag,
      set(axs_data(bodp,1+oth_sys),'color','r');
     end
     if clos_flag,
      set(axs_data(bodp,4+oth_sys),'color','b');
     end
    end

    wph = w;
    if open_flag & any(mode == 1),
     set(stat_bar,'string','Computing exact open-loop frequency response');
     ph = phasecor(sys_cplx,[-360,0]);

     brk=find(abs(diff(ph))>170);
     t=1; pht=[]; wpht=[];
     for k=brk,
      pht=[pht,ph(t:k),NaN];
      wpht=[wpht,wph(t:k),NaN];
      t=k+1;
     end
     pht=[pht,ph(t:length(ph))];
     wpht=[wpht,wph(t:length(wph))];

% update current system frequency response
     set(axs_data(bodp,1+cur_sys),'xdata',wpht,'ydata',pht,...
          'color','g','vis','on');
    elseif any(mode == 1),
     set(stat_bar,'string','Removing exact open-loop frequency response');
     set(axs_data(bodp,1+cur_sys),'vis','off');
    end

    if clos_flag & any(mode == 2),
     set(stat_bar,'string','Computing exact closed-loop frequency response');
     ers = 'norm';
     if ~open_flag, ers = 'norm'; end

     ph = phasecor(clos_cplx,[-360,0]);

     brk=find(abs(diff(ph))>170);
     t=1; pht=[]; wpht=[];
     for k=brk,
      pht=[pht,ph(t:k),NaN];
      wpht=[wpht,wph(t:k),NaN];
      t=k+1;
     end
     pht=[pht,ph(t:length(ph))];
     wpht=[wpht,wph(t:length(wph))];

% update current system frequency response
     set(axs_data(bodp,4+cur_sys),'xdata',wpht,'ydata',pht,...
          'color','m','vis','on');
    elseif any(mode == 2),
     set(stat_bar,'string','Removing exact closed-loop frequency response');
     set(axs_data(bodp,4+cur_sys),'vis','off');
    end

   end

% switching pages or requesting to veiw all systems
   if (any(mode == 12) & show_flag) | (any(mode == 14) & hide_flag) | ...
      (any(mode == 15) & hide_flag),

    for k = 1:2,
     oth_res = get(ui_han(oth_sys(k)),'userdata');
     oth_w = oth_res(1,:);
     oth_cplx = oth_res(2,:);

     if clos_flag,
      oth_bac = get(ui_han(9+oth_sys(k)),'userdata');
      bac_cplx = termcplx(oth_bac,w);
      tmp_cplx = oth_cplx./bac_cplx;
      oth_clos = tmp_cplx./(1+oth_cplx);
     end

     wph = oth_w;
     if open_flag,
      ph = phasecor(oth_cplx,[-360,0]);

      brk=find(abs(diff(ph))>170);
      t=1; pht=[]; wpht=[];
      for k2=brk,
       pht=[pht,ph(t:k2),NaN];
       wpht=[wpht,wph(t:k2),NaN];
       t=k2+1;
      end
      pht=[pht,ph(t:length(ph))];
      wpht=[wpht,wph(t:length(wph))];

      set(axs_data(bodp,1+oth_sys(k)),'xdata',wpht,'ydata',pht,'color','r',...
                                    'vis','on','erase','norm');
     end

     if clos_flag,
      ph = phasecor(oth_clos,[-360,0]);

      brk=find(abs(diff(ph))>170);
      t=1; pht=[]; wpht=[];
      for k2=brk,
       pht=[pht,ph(t:k2),NaN];
       wpht=[wpht,wph(t:k2),NaN];
       t=k2+1;
      end
      pht=[pht,ph(t:length(ph))];
      wpht=[wpht,wph(t:length(wph))];

      set(axs_data(bodp,4+oth_sys(k)),'xdata',wpht,'ydata',pht,'color','b',...
                                    'vis','on','erase','norm');
     end

    end
   elseif any(mode == 12) | any(mode == 14) | any(mode == 15),

    set(axs_data(bodp,1+oth_sys),'vis','off');
    set(axs_data(bodp,4+oth_sys),'vis','off');

   end

   if any(mode == 15), % setting axis limits to full after page change
    pageview(1,axs_hand(bodp));
   end

  end

  if length(nic), % nichols plot

   axs = [get(axs_hand(nic),'xlim'),get(axs_hand(nic),'ylim')];

% update current system frequency response
   if all(mode~=12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(nic,1+oth_sys),'vis','off');
%     set(axs_data(nic,4+oth_sys),'vis','off');
    elseif any(mode == 13),
     if open_flag,
      set(axs_data(nic,1+oth_sys),'color','r');
     end
%     if clos_flag,
%      set(axs_data(nic,4+oth_sys),'color','b');
%     end
    end

    if open_flag & any(mode == 1),
     set(stat_bar,'string','Computing exact open-loop frequency response');
     db = 20*log10(abs(sys_cplx));
     ph = phasecor(sys_cplx,[-360,0]);
     brk=find(abs(diff(ph))>170);
     t=1; pht=[]; dbt=[];
     for k=brk,
      pht=[pht,ph(t:k),NaN];
      dbt=[dbt,db(t:k),NaN];
      t=k+1;
     end
     pht=[pht,ph(t:length(ph))];
     dbt=[dbt,db(t:length(db))];

% update current system frequency response
     set(axs_data(nic,1+cur_sys),'xdata',pht,'ydata',dbt,...
           'color','g','vis','on');

    elseif any(mode == 1),
     set(stat_bar,'string','Removing exact open-loop frequency response');
     set(axs_data(nic,1+cur_sys),'vis','off');
    end

%    if clos_flag,
%     ers = 'norm';
%     if ~open_flag, ers = 'norm'; end
%
%     db = 20*log10(abs(clos_cplx));
%     ph = phasecor(clos_cplx,[-360,0]);
%
%     brk=find(abs(diff(ph))>170);
%     t=1; pht=[]; dbt=[];
%     for k=brk,
%      pht=[pht,ph(t:k),NaN];
%      dbt=[dbt,db(t:k),NaN];
%      t=k+1;
%     end
%     pht=[pht,ph(t:length(ph))];
%     dbt=[dbt,db(t:length(db))];

% update current system frequency response
%     set(axs_data(nic,4+cur_sys),'xdata',pht,'ydata',dbt,...
%           'color','m','vis','on');
%    elseif any(mode == 2),
%     set(axs_data(nic,4+cur_sys),'vis','off');
%    end

   end

% switching pages or requesting to veiw all systems
   if (any(mode == 12) & show_flag) | (any(mode == 14) & hide_flag) | ...
      (any(mode == 15) & hide_flag),

    for k = 1:2,
     oth_res = get(ui_han(oth_sys(k)),'userdata');
     oth_w = oth_res(1,:);
     oth_cplx = oth_res(2,:);

%     if clos_flag,
%      oth_bac = get(ui_han(9+oth_sys(k)),'userdata');
%      bac_cplx = termcplx(oth_bac,w);
%      tmp_cplx = oth_cplx./bac_cplx;
%      oth_clos = tmp_cplx./(1+oth_cplx);
%     end

     if open_flag,
      db = 20*log10(abs(oth_cplx));
      ph = phasecor(oth_cplx,[-360,0]);

      brk=find(abs(diff(ph))>170);
      t=1; pht=[]; dbt=[];
      for k2=brk,
       pht=[pht,ph(t:k2),NaN];
       dbt=[dbt,db(t:k2),NaN];
       t=k2+1;
      end
      pht=[pht,ph(t:length(ph))];
      dbt=[dbt,db(t:length(db))];

      set(axs_data(nic,1+oth_sys(k)),'xdata',pht,'ydata',dbt,'color','r',...
                                    'vis','on','erase','norm');
     end

%     if clos_flag,
%      db = 20*log10(abs(oth_clos));
%      ph = phasecor(oth_clos,[-360,0]);
%
%      brk=find(abs(diff(ph))>170);
%      t=1; pht=[]; dbt=[];
%      for k2=brk,
%       pht=[pht,ph(t:k2),NaN];
%       dbt=[dbt,db(t:k2),NaN];
%       t=k2+1;
%      end
%      pht=[pht,ph(t:length(ph))];
%      dbt=[dbt,db(t:length(db))];
%
%      set(axs_data(nic,4+oth_sys(k)),'xdata',pht,'ydata',dbt,'color','b',...
%                                    'vis','on','erase','norm');
%     end

    end
   elseif any(mode == 12) | any(mode == 14) | any(mode == 15),

    set(axs_data(nic,1+oth_sys),'vis','off');
%    set(axs_data(nic,4+oth_sys),'vis','off');

   end

   if any(mode == 15), % setting axis limits to full after page change
    pageview(1,axs_hand(nic));
   end

  end
 else

  set(stat_bar,'string','Removing exact frequency response');
  set(axs_data(:,2:7),'vis','off');
  set(exact,'checked','off');

 end
end

if any(mode == 4), % straight line approximation of bode

 if strcmp(get(straight,'checked'),'off') | length(mode)>mode_len,

  if length(bodm) | length(bodp),
   set(straight,'checked','on');
   if clos_flag,
    [nump,denp] = termextr(plant_mat);
    [numg,deng] = termextr(for_mat);
    [numh,denh] = termextr(bac_mat);
    numcl = conv(conv(nump,numg),denh);
    num = conv(conv(nump,numg),numh);
    den = conv(conv(denp,deng),denh);
    ln = length(num); ld = length(den);
    num = [zeros(1,ld-ln),num];
    den = [zeros(1,ln-ld),den];
    dencl = den + num;
    clos_mat = termpars(numcl,dencl);
   end
  end
  term_mat = termjoin(plant_mat,for_mat,bac_mat);

  if length(bodm), % bode (magnitude)

   if all(mode~=12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(bodm,7+oth_sys),'vis','off');
     set(axs_data(bodm,10+oth_sys),'vis','off');
    elseif any(mode == 13),
     if open_flag,
      set(axs_data(bodm,7+oth_sys),'color','c');
     end
     if clos_flag,
      set(axs_data(bodm,10+oth_sys),'color','c');
     end
    end

    if open_flag & any(mode == 1),
     set(stat_bar,'string','Computing open-loop straight-line approximation');
     [frq_mag,st_mag]=straitln(term_mat,w,1);
     set(axs_data(bodm,7+cur_sys),'xdata',frq_mag,'ydata',st_mag,...
            'color','r','vis','on');
    elseif any(mode==1),
     set(stat_bar,'string','Removing open-loop straight-line approximation');
     set(axs_data(bodm,7+cur_sys),'vis','off');
    end

    if clos_flag & any(mode == 2),
     set(stat_bar,'string','Computing closed-loop straight-line approximation');
     [frq_mag,st_mag]=straitln(clos_mat,w,1);
     set(axs_data(bodm,10+cur_sys),'xdata',frq_mag,'ydata',st_mag,...
            'color','r','vis','on');
    elseif any(mode==2),
     set(stat_bar,'string','Removing closed-loop straight-line approximation');
     set(axs_data(bodm,10+cur_sys),'vis','off');
    end

   end

% switching pages or requesting to veiw all systems
   if (any(mode == 12) & show_flag) | (any(mode == 14) & hide_flag),

    for k = 1:2,
     oth_res = get(ui_han(oth_sys(k)),'userdata');
     oth_w = oth_res(1,:);

     if open_flag,
      oth_plant = get(ui_han(3+oth_sys(k)),'userdata');
      oth_for = get(ui_han(6+oth_sys(k)),'userdata');
      oth_bac = get(ui_han(9+oth_sys(k)),'userdata');
      oth_open = termjoin(oth_plant,oth_for,oth_bac);

      [frq_mag,st_mag]=straitln(oth_open,oth_w,1);

      set(axs_data(bodm,7+oth_sys(k)),'xdata',frq_mag,'ydata',st_mag,...
              'color','c','vis','on');
     end

     if clos_flag,
      [nump,denp] = termextr(oth_plant);
      [numg,deng] = termextr(oth_for);
      [numh,denh] = termextr(oth_bac);
      numcl = conv(conv(nump,numg),denh);
      num = conv(conv(nump,numg),numh);
      den = conv(conv(denp,deng),denh);
      ln = length(num); ld = length(den);
      num = [zeros(1,ld-ln),num];
      den = [zeros(1,ln-ld),den];
      dencl = den + num;
      oth_clos = termpars(numcl,dencl);

      [frq_mag,st_mag]=straitln(oth_clos,oth_w,1);

      set(axs_data(bodm,7+oth_sys(k)),'xdata',frq_mag,'ydata',st_mag,...
              'color','c','vis','on');
     end
    end

   elseif any(mode == 12) | any(mode == 14),

    set(axs_data(bodm,7+oth_sys),'vis','off');
    set(axs_data(bodm,10+oth_sys),'vis','off');

   end

  end

  if length(bodp), % bode (phase)

   if all(mode~=12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(bodp,7+oth_sys),'vis','off');
     set(axs_data(bodp,10+oth_sys),'vis','off');
    elseif any(mode == 13),
     if open_flag,
      set(axs_data(bodp,7+oth_sys),'color','c');
     end
     if clos_flag,
      set(axs_data(bodp,10+oth_sys),'color','c');
     end
    end

    if open_flag & any(mode == 1),
     set(stat_bar,'string','Computing open-loop straight-line phase approximation');
     [frq_mag,st_mag]=straitln(term_mat,w,2);
     set(axs_data(bodp,7+cur_sys),'xdata',frq_mag,'ydata',st_mag,...
            'color','r','vis','on');
    elseif any(mode==1),
     set(stat_bar,'string','Removing open-loop straight-line phase approximation');
     set(axs_data(bodp,7+cur_sys),'vis','off');
    end

    if clos_flag & any(mode == 2),
     set(stat_bar,'string','Computing closed-loop straight-line phase approximation');
     [frq_mag,st_mag]=straitln(clos_mat,w,2);
     set(axs_data(bodp,10+cur_sys),'xdata',frq_mag,'ydata',st_mag,...
            'color','r','vis','on');
    elseif any(mode==2),
     set(stat_bar,'string','Removing closed-loop straight-line phase approximation');
     set(axs_data(bodp,10+cur_sys),'vis','off');
    end

   end

% switching pages or requesting to veiw all systems
   if (any(mode == 12) & show_flag) | (any(mode == 14) & hide_flag),

    for k = 1:2,
     oth_res = get(ui_han(oth_sys(k)),'userdata');
     oth_w = oth_res(1,:);

     if open_flag,
      oth_plant = get(ui_han(3+oth_sys(k)),'userdata');
      oth_for = get(ui_han(6+oth_sys(k)),'userdata');
      oth_bac = get(ui_han(9+oth_sys(k)),'userdata');
      oth_open = termjoin(oth_plant,oth_for,oth_bac);

      [frq_mag,st_mag]=straitln(oth_open,oth_w,2);

      set(axs_data(bodp,7+oth_sys(k)),'xdata',frq_mag,'ydata',st_mag,...
              'color','c','vis','on');
     end

     if clos_flag,
      [nump,denp] = termextr(oth_plant);
      [numg,deng] = termextr(oth_for);
      [numh,denh] = termextr(oth_bac);
      numcl = conv(conv(nump,numg),denh);
      num = conv(conv(nump,numg),numh);
      den = conv(conv(denp,deng),denh);
      ln = length(num); ld = length(den);
      num = [zeros(1,ld-ln),num];
      den = [zeros(1,ln-ld),den];
      dencl = den + num;
      oth_clos = termpars(numcl,dencl);

      [frq_mag,st_mag]=straitln(oth_clos,oth_w,2);

      set(axs_data(bodp,7+oth_sys(k)),'xdata',frq_mag,'ydata',st_mag,...
              'color','c','vis','on');
     end
    end

   elseif any(mode == 12) | any(mode == 14),

    set(axs_data(bodp,7+oth_sys),'vis','off');
    set(axs_data(bodp,10+oth_sys),'vis','off');

   end

  end

 else

  set(stat_bar,'string','Removing straight-line approximation');
  set(axs_data([bodm,bodp],8:13),'vis','off');
  set(straight,'checked','off');

 end

end

if any(mode == 5), % margins

 if (strcmp(get(margins,'checked'),'off') | length(mode)>mode_len) & open_flag,
  set(margins,'checked','on');

  set(stat_bar,'string','Displaying margins');

  if all(mode ~= 12),
   db = 20*log10(abs(sys_cplx));
   deg = phase4(sys_cplx)*180/pi;
   [Gm,Wcg,Pm,Wcp] = termmarg(db,deg,w);
  end

% switching pages or requesting to veiw all systems
  if (any(mode == 12) & show_flag) | ...
     (any(mode == 14) & hide_flag),

   for k = 1:2,
    oth_res = get(ui_han(oth_sys(k)),'userdata');
    oth_w = oth_res(1,:);
    oth_cplx = oth_res(2,:);
    db = 20*log10(abs(oth_cplx));
    deg = phase4(oth_cplx)*180/pi;
    [oGm,oWcg,oPm,oWcp] = termmarg(db,deg,oth_w);
    oth_Gm(k) = oGm; oth_Wcg(k) = oWcg;
    oth_Pm(k) = oPm; oth_Wcp(k) = oWcp;
   end
  end

  if length(nyq), % nyquist gain and phase margins

   if all(mode ~= 12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(nyq,7+oth_sys),'vis','off');
     set(axs_data(nyq,10+oth_sys),'vis','off');
    elseif any(mode == 13),
     set(axs_data(nyq,7+oth_sys),'color','c');
     set(axs_data(nyq,10+oth_sys),'color','c');
    end

    if ~isnan(Wcg),
     set(axs_data(nyq,7+cur_sys),'xdata',[-1,-1/Gm],'ydata',[0,0],...
            'color','k','vis','on');
    else
     set(axs_data(nyq,7+cur_sys),'xdata',0,'ydata',0,'vis','off');
    end

    if ~isnan(Wcp),
     set(axs_data(nyq,10+cur_sys),...
               'xdata',[0,real(exp(i*(-180+Pm)*pi/180))],...
               'ydata',[0,imag(exp(i*(-180+Pm)*pi/180))],...
               'color','k','vis','on');
    else
     set(axs_data(nyq,10+cur_sys),'xdata',0,'ydata',0,'vis','off');
    end

   end

   if (any(mode == 12) & show_flag) | ...
      (any(mode == 14) & hide_flag),

    for k = 1:2,
     if ~isnan(oth_Wcg(k)),
      set(axs_data(nyq,7+oth_sys(k)),'xdata',[-1,-1/oth_Gm(k)],'ydata',[0,0],...
            'color','c','vis','on');
     else
      set(axs_data(nyq,7+oth_sys(k)),'xdata',0,'ydata',0,'vis','off');
     end

     if ~isnan(oth_Wcp(k)),
      set(axs_data(nyq,10+oth_sys(k)),...
                'xdata',[0,real(exp(i*(-180+oth_Pm(k))*pi/180))],...
                'ydata',[0,imag(exp(i*(-180+oth_Pm(k))*pi/180))],...
                'color','c','vis','on');
     else
      set(axs_data(nyq,10+oth_sys(k)),'xdata',0,'ydata',0,'vis','off');
     end

    end

   elseif any(mode == 12) | any(mode == 14),

    set(axs_data(nyq,7+oth_sys),'vis','off');
    set(axs_data(nyq,10+oth_sys),'vis','off');

   end
  end

  if length(bodm), % bode (magnitude)

   if all(mode ~= 12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(bodm,13+oth_sys),'vis','off');
    elseif any(mode == 13),
     set(axs_data(bodm,13+oth_sys),'color','c');
    end

    if ~isnan(Wcg),
     set(axs_data(bodm,13+cur_sys),'xdata',[Wcg,Wcg],'ydata',[0,-20*log10(Gm)],...
      'color','k','vis','on');
    else
     set(axs_data(bodm,13+cur_sys),'xdata',0,'ydata',0,'vis','off');
    end

   end

   if (any(mode == 12) & show_flag) | ...
      (any(mode == 14) & hide_flag),

    for k = 1:2,
     if ~isnan(oth_Wcg),
      set(axs_data(bodm,13+oth_sys(k)),'xdata',[oth_Wcg(k),oth_Wcg(k)],...
                  'ydata',[0,-20*log10(oth_Gm(k))],...
                  'color','c','vis','on');
     else
      set(axs_data(bodm,13+oth_sys(k)),'xdata',0,'ydata',0,'vis','off');
     end

    end

   elseif any(mode == 12) | any(mode == 14),

    set(axs_data(bodm,13+oth_sys),'vis','off');

   end

  end

  if length(bodp), % bode (phase)

   if all(mode ~= 12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(bodp,13+oth_sys),'vis','off');
    elseif any(mode == 13),
     set(axs_data(bodp,13+oth_sys),'color','c');
    end

    if ~isnan(Wcp),
     set(axs_data(bodp,13+cur_sys),'xdata',[Wcp,Wcp],'ydata',[-180,-180+Pm],...
            'color','k','vis','on');
    else
     set(axs_data(bodp,13+cur_sys),'xdata',0,'ydata',0,'vis','off');
    end

   end

   if (any(mode == 12) & show_flag) | ...
      (any(mode == 14) & hide_flag),

    for k = 1:2,
     if ~isnan(oth_Wcp(k)),
      set(axs_data(bodp,13+oth_sys(k)),...
          'xdata',[oth_Wcp(k),oth_Wcp(k)],'ydata',[-180,-180+oth_Pm(k)],...
          'color','c','vis','on');
     else
      set(axs_data(bodp,13+oth_sys(k)),'xdata',0,'ydata',0,'vis','off');
     end
    end

   elseif any(mode == 12) | any(mode == 14),

    set(axs_data(bodp,13+oth_sys),'vis','off');

   end

  end

  if length(nic), % nichols gain and phase margins

   if all(mode~=12),

% switching systems
    if any(mode == 13) & show_flag,
     set(axs_data(nic,7+oth_sys),'vis','off');
     set(axs_data(nic,10+oth_sys),'vis','off');
    elseif any(mode == 13),
     set(axs_data(nic,7+oth_sys),'color','c');
     set(axs_data(nic,10+oth_sys),'color','c');
    end

    if ~isnan(Wcg),
     set(axs_data(nic,7+cur_sys),'xdata',[-180,-180],...
            'ydata',[0,-20*log10(Gm)],'color','k','vis','on');
    else
     set(axs_data(nic,7+cur_sys),'xdata',0,'ydata',0,'vis','off');
    end

    if ~isnan(Wcp),
     set(axs_data(nic,10+cur_sys),'xdata',[-180,-180+Pm],...
            'ydata',[0,0],'color','k','vis','on');
    else
     set(axs_data(nic,10+cur_sys),'vis','off');
    end

   end

   if (any(mode == 12) & show_flag) | ...
      (any(mode == 14) & hide_flag),

    for k = 1:2,
     if ~isnan(oth_Wcg(k)),
      set(axs_data(nic,7+oth_sys(k)),'xdata',[-180,-180],...
            'ydata',[0,-20*log10(oth_Gm(k))],'color','c','vis','on');
     else
      set(axs_data(nic,7+oth_sys(k)),'xdata',0,'ydata',0,'vis','off');
     end

     if ~isnan(oth_Wcp(k)),
      set(axs_data(nic,10+oth_sys(k)),'xdata',[-180,-180+oth_Pm(k)],...
             'ydata',[0,0],'color','c','vis','on');
     else
      set(axs_data(nic,10+oth_sys(k)),'xdata',0,'ydata',0','vis','off');
     end
    end

   elseif any(mode == 12) | any(mode == 14),

    set(axs_data(nic,7+oth_sys),'vis','off');
    set(axs_data(nic,10+oth_sys),'vis','off');

   end

  end

 else

  if open_flag,
   set(stat_bar,'string','Removing margins');
   set(margins,'checked','off');
  end
  set(axs_data(nyq,8:13),'vis','off');
  set(axs_data(bodm,14:16),'vis','off');
  set(axs_data(bodp,14:16),'vis','off');
  set(axs_data(nic,8:13),'vis','off');

 end
end

if any(mode == 12) & show_flag,
 set(show_all,'label','Hide Other Systems');
elseif any(mode == 12),
 set(show_all,'label','Show All Systems');
end


function varargout=corr(this,varargin)
% Computes correlation for time series: It is an interface to MATLAB implementation of corr
%
% ::
%
%    varargout = corr(db,varargin);
%
% Args:
%
%    db (ts object): times series object
%
%    varargin: varargin for corr function in MATLAB
%
% Returns:
%    :
%
%    varargout: output from corr function
%

if ~isempty(varargin) && isa(varargin{1},'ts')

    this=this & varargin{1};

    varargin=varargin(2:end);

end

data=this.data;

if size(data,3)>1

    error([mfilename,':: this operation is only defined for databases with one page'])

end

nout=nargout;

bad=any(isnan(data),2);

if any(bad)

    data(bad,:)=[];

    disp([int2str(sum(bad)),' nan rows above discarded'])

end

[varargout{1:nout}]=utils.stat.corr(data,varargin{:});

end


function [params, names] = rbfard2KernExtractParam(kern)

% RBFARD2KERNEXTRACTPARAM Extract parameters from the RBFARD2 kernel structure.
% FORMAT
% DESC Extract parameters from the automatic relevance determination
% radial basis function kernel structure into a vector of parameters for
% optimisation.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
%
% DESC Extract parameters and parameter names from the automatic
% relevance determination radial basis function kernel structure.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
% RETURN names : cell array of strings containg the parameter names.
%
% SEEALSO rbfard2KernParamInit, rbfard2KernExpandParam, kernExtractParam, scg, conjgrad
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006
%
% COPYRIGHT : Michalis K. Titsias, 2009
%
% KERN

params = [kern.variance kern.inputScales];
if nargout > 1
  names = {'variance'};
  for i = 1:length(kern.inputScales)
    names{1+i} = ['input scale ' num2str(i)];
  end
end


function f = plus(f, g)
%+   Addition of two TRIGTECH objects.
%   F + G adds F and G, where F and G may be TRIGTECH objects or scalars.
%
%   If F is an array-valued TRIGTECH, then F + C is supported if C is a row
%   vector of doubles with the same number of columns as F.
%
% See also MINUS, UPLUS.

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

if ( isempty(f) || isempty(g) ) % TRIGTECH + [] = [].

    f = [];

elseif ( isa(g, 'double') ) % TRIGTECH + double.

    % Update values (use bsxfun() to handle the case in which g is a vector
    % and f is an array-valued TRIGTECH):
    f.values = bsxfun(@plus, f.values, g);

    % Update coeffs:
    if ( (size(g, 2) > 1) && (size(f.coeffs, 2) == 1) )
        % Perform singleton expansion of f:
        f.coeffs = repmat(f.coeffs, 1, size(g, 2));
    end
    N = size(f.coeffs,1);

    % Determine the index in the coefficient vector where the constant term
    % is stored. The way we have arranged the coefficients means it should
    % be in the middle of the array, but that depends on whether the number
    % of coefficients is even or odd.
    if mod(N,2) == 1
        const_index = (N+1)/2;
    else
        const_index = N/2+1;
    end
    f.coeffs(const_index,:) = f.coeffs(const_index,:) + g;

    % Update isReal:
    f.isReal = f.isReal & isreal(g);

elseif ( isa(f, 'double') ) % double + TRIGTECH.

    % Switch argument order and call TRIGTECH/PLUS again:
    f = plus(g, f);

elseif ( isa(f, 'trigtech') && isa(g, 'trigtech') )  % TRIGTECH + TRIGTECH.

    % We will simply add the values together then compute the coefficients
    % of the result. This is probably not the most efficient means of
    % determing the sum.
    nf = size(f.values, 1);
    ng = size(g.values, 1);
    if ( nf > ng )
        % Increase the length of g (via PROLONG):
        g = prolong(g, nf);
    elseif ( nf < ng )
        % Increase the length of f (via PROLONG):
        f = prolong(f, ng);
    end

    % Update values and coefficients:
    f.values = f.values + g.values;
    f.coeffs = f.vals2coeffs(f.values);

    % Update isReal:
    f.isReal = f.isReal & g.isReal;

    % Force the values to be real where f is real.
    f.values(:,f.isReal) = real(f.values(:,f.isReal));

    % Look for a zero output:
    if ( ~any(f.values(:)) || ~any(f.coeffs(:)) )
        % Create a zero TRIGTECH:
        ishappy = f.ishappy && g.ishappy;
        z = zeros(1, size(f.values, 2));
        data.vscale = z;
        f = f.make(z, data);
        f.ishappy = ishappy;
    else
        f.ishappy = f.ishappy && g.ishappy;
    end

else    % Don't know how to do the addition of the objects.

    error('CHEBFUN:TRIGTECH:plus:typeMismatch',['Incompatible operation between objects.\n', ...
    'Make sure functions are of the same type.']);

end

end


function kern = rbfwhiteKernExpandParam(kern, params)

% RBFWHITEKERNEXPANDPARAM Create kernel structure from RBF-WHITE kernel's
% parameters.
% FORMAT
% DESC returns a RBF-WHITE kernel structure filled with the parameters in
% the given vector. This is used as a helper function to enable parameters
% to be optimised in, for example, the NETLAB optimisation functions.
% ARG kern : the kernel structure in which the parameters are to be
% placed.
% ARG param : vector of parameters which are to be placed in the
% kernel structure.
% RETURN kern : kernel structure with the given parameters in the
% relevant locations.
%
% SEEALSO : rbfwhiteKernParamInit, rbfwhiteKernExtractParam, kernExpandParam
%
% COPYRIGHT : David Luengo, 2009

% KERN

kern.inverseWidth = params(1);
kern.variance = params(2);


function matlab2hypreParVectors(matlab_mvector,num_procs,vector_filename,varargin)
%MATLAB2HYPREPARVECTORS Converts MATLAB Vectors to HYPRE IJ Format.
%
%   This program was tested on Matlab 7.1.
%
%   Description:
%     matlab2hypreParVectors(matlab_mvector,num_procs) converts MATLAB
%     formatted vectors to HYPRE formatted ones, and writes the files to
%     the disk. This function takes as an input an NxM matlab matrix
%     matlab_mvector, where M represents the number of vectors and N stands
%     for the size of each vector and writes the matrix to the number of
%     files specified by giving num_procs argument
%
%     matlab2hypreParVectors(matlab_mvector,num_procs,vector_filename)
%     writes the HYPRE formatted vectors to the output file named
%     vector_filename e.g. if vector_filename is 'vectors' the program
%     will generate first 4 files and the output files will be:
%       vectors.0.0, vectors.0.1, vectors.0.INFO.0, vectors.0.INFO.1.
%     the Mth output will be, consequently,
%       vectors.m.0
%       vectors.m.1
%       vectors.m.INFO.0
%       vectors.m.INFO.1.
%
%     matlab2hypreParVectors(matlab_mvector,num_procs,vector_filename,
%       precision) converts from MATLAB to HYPRE with specified precision
%       given by floating point precision.
%
%     Examples:
%       matlab2hypreParVectors(A,2,'vectors')
%       1) Create a matrix A in matlab, consisting of M vectors of size
%         N to be converted.
%       2)Set num_procs to 2. (Specifies the number of processors)
%       3)'vectors' specifies the hypre output filenames:
%         vectors.0.0
%         vectors.0.1
%         vectors.0.INFO.0
%         vectors.0.INFO.1
%
%       matlab2hypreParVectors(A,2,'vectors','15.10e')
%       changes format of floating point numbers in file
%
%   See also matlabIJ2hypre, testIJmatlabhypre.m,
%      hypreIJ2matlab.m, hypreParVectors2matlab.m
%
%   Author: Diana Zakaryan, Dept of Mathematics, University of Colorado,
%      Denver, 15-Mar-2005.
%

for j=1:size(matlab_mvector,2)
    % create filename
    filename = strcat(vector_filename, '.', num2str(j-1));
    B=matlab_mvector(:,j);

    % call single vector convertor
    matlab2hypreParVector(B,num_procs,filename,varargin);
end

function matlab2hypreParVector(matlab_mvector,num_procs,filename,varargin)

% check
if isvector(matlab_mvector)~=1
    error('The argument must be a vector.')
end

% default print format
my_format='20.19e';
if (nargin>4)
    my_format=varargin(1);
end
prt_format=strcat('%',char(my_format));

% intialization
n=size(matlab_mvector,1);

% generate partitioning
part=generate_part(n,num_procs);

% generate Hypre input files
for i=1:num_procs
    s=int2str(i-1);
    filename2=strcat(filename,'.',s);
    fprintf('Generating file: %s\n',filename2);
    X=matlab_mvector(part(i)+1:part(i+1));
    nrows=part(i+1)-part(i);
    dlmwrite(filename2, nrows, 'precision', '%d');
    dlmwrite(filename2, X, '-append','precision',prt_format);

    % writing INFO file
    filename2=strcat(filename,'.','INFO','.',s);
    fprintf('Generating INFO file: %s\n',filename2);
    Y = [n;0;nrows];
    dlmwrite(filename2, Y, 'precision', '%d');
    clear ('X', 'Y');
end
return

function [partitioning]=generate_part(length,num_procs)
% [partitioning]=generate_part(length,num_procs)
% generate partitioning across processes
% See Hypre getpart.c
partitioning=zeros(num_procs+1,1);
size = floor(length/num_procs);
rest= length - size*num_procs;
for i=1:num_procs-1
    partitioning(i+1) = partitioning(i)+size;
    if (i==1)
        partitioning(i+1)=partitioning(i+1)+rest;
    end
end
partitioning(num_procs+1)=length;
return


function table = i4mat_read ( input_filename )

%*****************************************************************************80
%
%% I4MAT_READ reads information from an I4MAT file.
%
%  Discussion:
%
%    An I4MAT is an array of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer TABLE(M,N), the point coordinates.
%
  [ m, n ] = i4mat_header_read ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_READ - Fatal error!\n' );
    fprintf ( 1, '  The spatial dimension is not positive.\n' );
  end

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_READ - Fatal error!\n' );
    fprintf ( 1, '  The number of points is not positive.\n' );
  end

  table(1:m,1:n) = i4mat_data_read ( input_filename, m, n );

  return
end


function Tpix = conv_3d_T_from_phys_to_pix(Tphys, spc)
    Tpix = Tphys;
    for j = 1 : size(Tphys, 5)
    for i = 1 : size(Tphys, 4)
        Tpix(:,:,:, i, j) = Tpix(:,:,:,i, j) / spc(i);
    end
    end
end


function Con = createControls(Robot)

% CREATECONTROLS  Create controls structure Con.
%   Con = CREATECONTROLS(ROBOT) generates a control structure Con. This
%   structure depends on the robot's motion model, which contains the
%   fields:
%       .u      nominal value of control
%       .uStd   Standard deviation of control noise
%       .U      Covariances matrix
%
%   The resulting structure needs to be updated during execution time with
%   data from one of these origins:
%       1. read from odometry sensors
%       2. generated by a control software
%       3. generated by the simulator.
%
%   See also CREATEROBOTS.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

switch Robot.motion

    case {'constVel'}

        Con.u    = [Robot.dv;deg2rad(Robot.dwDegrees)];
        Con.uStd = [Robot.dvStd;deg2rad(Robot.dwStd)];
        Con.U    = diag(Con.uStd.^2);

    case {'odometry'}

        Con.u    = [Robot.dx;deg2rad(Robot.daDegrees)];
        Con.uStd = [Robot.dxStd;deg2rad(Robot.daStd)];
        Con.U    = diag(Con.uStd.^2);

    otherwise
        error('Unknown motion model %s from robot %d.',Robot.motion,Robot.id);
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function params = parseFloatAssertInputs(varargin)
%parseFloatAssertInputs Parse inputs for floating-point assertion functions.
%   params = parseFloatAssertInputs(varargin) parses the input arguments for
%   assertElementsAlmostEqual, assertVectorsAlmostEqual, and compareFcn. It
%   returns a parameter struct containing the fields:
%
%       A    B    ToleranceType    Tolerance    FloorTolerance

%   Steven L. Eddins
%   Copyright 2008-2009 The MathWorks, Inc.

error(nargchk(2, 6, nargin, 'struct'));

params = struct('A', {[]}, 'B', {[]}, 'ToleranceType', {[]}, ...
    'Tolerance', {[]}, 'FloorTolerance', {[]}, 'Message', {''});

% The first two input arguments are always A and B.
params.A = varargin{1};
params.B = varargin{2};
varargin(1:2) = [];

% If the last argument is a message string, process it and remove it from the list.
if (numel(varargin) >= 1) && ischar(varargin{end}) && ...
        ~any(strcmp(varargin{end}, {'relative', 'absolute'}))
    params.Message = varargin{end};
    varargin(end) = [];
end

checkAB(params.A, params.B);

epsilon = max(eps(class(params.A)), eps(class(params.B)));

if numel(varargin) < 3
    % floor_tol not specified; set default.
    params.FloorTolerance = epsilon;
else
    params.FloorTolerance = varargin{3};
end

if numel(varargin) < 2
    % tol not specified; set default.
    params.Tolerance = sqrt(epsilon);
else
    params.Tolerance = varargin{2};
end

if numel(varargin) < 1
    % tol_type not specified; set default.
    params.ToleranceType = 'relative';
else
    params.ToleranceType = varargin{1};
end

%===============================================================================
function checkAB(A, B)
if ~isfloat(A) || ~isfloat(B)
    error('MTEST:parseFloatAssertInputs:inputsNotFloat', ...
        'A and B must be floating-point arrays.');
end

if ~isequal(size(A), size(B))
    error('MTEST:parseFloatAssertInputs:sizeMismatch', ...
        'A and B must have the same size.');
end


function vel = veldim(vel,from,to)
%VELDIM  Convert velocity units
%
%   velOut = VELDIM(velIn, FROM, TO) converts velIn from the units
%   specified by the string FROM to the units specified by the string
%   TO.  FROM and TO are case-insensitive, and may equal any of the
%   following:
%
%        'meters per second', 'mps' or 'm/s'
%        'feet per second', 'ftps' or 'ft/s'   <== U.S. survey feet
%        'kilometers per hour', 'kmph', 'km/hr', or 'km/h'
%        'knots', 'kts', 'nm/hr' or 'nm/h'
%        'miles per hour', 'mph', 'mi/hr', 'mi/h', sm/hr' or 'sm/h' <== statute miles
%
%   Exercise caution with 'feet' and 'miles'
%   ----------------------------------------
%   VELDIM interprets 'feet per second', 'ftps', and 'ft/s' as U.S. survey
%   feet per second, and does not support international feet at all.
%
%   By definition, one international foot is exactly 0.3048 meters and
%   one U.S. survey foot is exactly 1200/3937 meters.  For many
%   applications, the difference is significant.
%
%   Likewise, VELDIM interprets 'miles per hour', 'mph', 'mi/h', and 'sm/h'
%   as statute miles per hour (also known as U.S. survey miles), and does
%   not support international miles per hour at all.  By definition, one
%   international mile is 5280 international feet and one statute mile is
%   5280 survey feet.
%
%   See also  MPS2FTPS, MPS2KMPH,  MPS2KTS,  MPS2MPH,
%             FTPS2MPS, FTPS2KMPH, FTPS2KTS, FTPS2MPH,
%             KMPH2MPS, KMPH2FTPS, KMPH2KTS, KMPH2MPH,
%             KTS2MPS,  KTS2FTPS,  KTS2KMPH, KTS2MPH,
%             MPH2MPS,  MPH2FTPS,  MPH2KMPH, MPH2KTS

error(nargchk(3, 4, nargin, 'struct'))

% Warn and convert to real if VEL is complex.
vel = ignoreComplex(vel, mfilename, 'VEL');

% Convert units only if there's something to change.
if ~strcmp(from, to)
    vel = applyconversion(vel, from, to);
end

function vel = applyconversion(vel, from, to)

from = lower(from);
to = lower(to);

toIsSupported   = true;
fromIsSupported = true;

switch from
    case {'meters per second','mps','m/s'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = mps2ftps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = mps2kmph(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = mps2kts(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = mps2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'feet per second','ftps','ft/s'}
        switch to
            case {'meters per second','mps','m/s'}
                vel = ftps2mps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = ftps2kmph(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = ftps2kts(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = ftps2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'kilometers per hour','kmph','km/h','km/hr'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = kmph2ftps(vel);
            case {'meters per second','mps','m/s'}
                vel = kmph2mps(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = kmph2kts(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = kmph2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'knots','kts','nm/h','nm/hr'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = kts2ftps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = kts2kmph(vel);
            case {'meters per second','mps','m/s'}
                vel = kts2mps(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = kts2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = mph2ftps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = mph2kmph(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = mph2kts(vel);
            case {'meters per second','mps','m/s'}
                vel = mph2mps(vel);
            otherwise
                toIsSupported = false;
        end

    otherwise
        fromIsSupported = false;
end

assert(toIsSupported, 'map:distdim:UnsupportedToUnits', ...
    'Unsupported ''TO'' units: %s.', to)

assert(fromIsSupported, 'map:distdim:UnsupportedFromUnits', ...
    'Unsupported ''FROM'' units: %s.', from)


function edgeFaces = meshEdgeFaces(vertices, edges, faces) %#ok<INUSL>
%MESHEDGEFACES Compute index of faces adjacent to each edge of a mesh.
%
%   EF = meshEdgeFaces(V, E, F)
%   Compute index array of faces adjacent to each edge of a mesh.
%   V, E and F define the mesh: V is vertex array, E contains vertex
%   indices of edge extremities, and F contains vertex indices of each
%   face, either as a numerical array or as a cell array.
%   The result EF has as many rows as the number of edges, and two column.
%   The first column contains index of faces located on the left of the
%   corresponding edge, whereas the second column contains index of the
%   face located on the right. Some indices may be 0 if the mesh is not
%   'closed'.
%
%   Note: a faster version is available for triangular meshes.
%
%   Example
%   meshEdgeFaces
%
%   See also
%   meshes3d, trimeshEdgeFaces, meshDihedralAngles, polyhedronMeanBreadth

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

Ne = size(edges, 1);

% indices of faces adjacent to each edge
edgeFaces = zeros(Ne, 2);

% different method for extracting current face depending if faces are
% stored as index 2D array or as cell array of 1D arrays.
if isnumeric(faces)
    Nf = size(faces, 1);
    for i = 1:Nf
        face = faces(i, :);
        processFace(face, i)
    end
elseif iscell(faces)
    Nf = length(faces);
    for i = 1:Nf
        face = faces{i};
        processFace(face, i)
    end
end

    function processFace(face, indFace)
        % iterate on face edges
        for j = 1:length(face)
            % build edge: array of vertices
            j2 = mod(j, length(face)) + 1;

            % do not process edges with same vertices
            if face(j) == face(j2)
                continue;
            end

            % vertex indices of current edge
            currentEdge = [face(j) face(j2)];

            % find index of current edge, assuming face is left-located
            b1 = ismember(edges, currentEdge, 'rows');
            indEdge = find(b1);
            if ~isempty(indEdge)
                if edgeFaces(indEdge, 1) ~= 0
                    error('meshes3d:IllegalTopology', ...
                        'Two faces were found on left side of edge %d ', indEdge);
                end

                edgeFaces(indEdge, 1) = indFace;
                continue;
            end

            % otherwise, assume the face is right-located
            b2 = ismember(edges, currentEdge([2 1]), 'rows');
            indEdge = find(b2);
            if ~isempty(indEdge)
                if edgeFaces(indEdge, 2) ~= 0
                    error('meshes3d:IllegalTopology', ...
                        'Two faces were found on left side of edge %d ', indEdge);
                end

                edgeFaces(indEdge, 2) = indFace;
                continue;
            end

            % If face was neither left nor right, error
            warning('meshes3d:IllegalTopology', ...
                'Edge %d of face %d was not found in edge array', ...
                j, indFace);
            continue;
        end
    end

end


classdef ImgHash < handle
    %IMGHASH  Base class for Image Hashing algorithms
    %
    % This module brings implementations of different image hashing algorithms.
    % It provides algorithms to extract the hash of images and fast way to
    % figure out most similar images in huge data set.
    %
    % ### Supported Algorithms
    %
    % - Average hash (also called Different hash)
    % - PHash (also called Perceptual hash)
    % - Marr Hildreth Hash
    % - Radial Variance Hash
    % - Block Mean Hash (modes 0 and 1)
    % - Color Moment Hash (this is the one and only hash algorithm resist to
    %   rotation attack (-90~90 degree)
    %
    % You can study more about image hashing from the paper and websites
    % listed in the references section.
    %
    % ### Performance under different attacks
    %
    % ![Performance chart](https://docs.opencv.org/3.3.0/attack_performance.JPG)
    %
    % ### Speed comparison with PHash library (100 images from ukbench)
    %
    % ![Hash Computation chart](https://docs.opencv.org/3.3.1/hash_computation_chart.JPG)
    % ![Hash comparison chart](https://docs.opencv.org/3.3.1/hash_comparison_chart.JPG)
    %
    % As you can see, hash computation speed of img_hash module outperform
    % [PHash library](http://www.phash.org/) a lot.
    %
    % PS: I do not list out the comparison of Average hash, PHash and Color
    % Moment hash, because I cannot find them in PHash.
    %
    % ### Motivation
    %
    % Collects useful image hash algorithms into OpenCV, so we do not need to
    % rewrite them by ourselves again and again or rely on another 3rd party
    % library (ex: PHash library). BOVW or correlation matching are good and
    % robust, but they are very slow compare with image hash, if you need to
    % deal with large scale CBIR (content based image retrieval) problem,
    % image hash is a more reasonable solution.
    %
    % ### More info
    %
    % You can learn more about img_hash modules from following links, these
    % links show you how to find similar image from ukbench dataset, provide
    % thorough benchmark of different attacks (contrast, blur, noise
    % (gaussion, pepper and salt), jpeg compression, watermark, resize).
    %
    % * [Introduction to image hash module of OpenCV](http://qtandopencv.blogspot.my/2016/06/introduction-to-image-hash-module-of.html)
    % * [Speed up image hashing of OpenCV (img_hash) and introduce color moment hash](http://qtandopencv.blogspot.my/2016/06/speed-up-image-hashing-of-opencvimghash.html)
    %
    % ### Contributors
    %
    % [Tham Ngap Wei](mailto:thamngapwei@gmail.com)
    %
    % ## References
    % [lookslikeit]:
    % > Neal Krawetz.
    % > [Looks Like It](http://www.hackerfactor.com/blog/?/archives/432-Looks-Like-It.html).
    %
    % [tang2012perceptual]:
    % > Zhenjun Tang, Yumin Dai, Xianquan Zhang. "Perceptual Hashing for Color
    % > Images Using Invariant Moments. Appl. Math, 6(2S):643S-650S, 2012.
    %
    % [zauner2010implementation]:
    % > Christoph Zauner. "Implementation and benchmarking of perceptual image
    % > hash functions". 2010.
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    %% Constructor/destructor
    methods
        function this = ImgHash(alg, varargin)
            %IMGHASH  Constructor
            %
            %     obj = cv.ImgHash(alg)
            %     obj = cv.ImgHash(alg, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __alg__ image hash algorithm, one of:
            %   * __AverageHash__ Computes average hash value of the input
            %     image. This is a fast image hashing algorithm, but only work
            %     on simple case. For more details, please refer to
            %     [lookslikeit].
            %   * __BlockMeanHash__ Image hash based on block mean. See
            %     [zauner2010implementation] for details.
            %   * __ColorMomentHash__ Image hash based on color moments. See
            %     [tang2012perceptual] for details.
            %   * __MarrHildrethHash__ Marr-Hildreth Operator Based Hash,
            %     slowest but more discriminative. See
            %     [zauner2010implementation] for details.
            %   * __PHash__ Slower than average_hash, but tolerant of minor
            %     modifications. This algorithm can combat more variation than
            %     than AverageHash, for more details please refer to
            %     [lookslikeit].
            %   * __RadialVarianceHash__ Image hash based on Radon transform.
            %     See [tang2012perceptual] for details.
            %
            % ## Options
            % The following are options for the various algorithms:
            %
            % ### `BlockMeanHash`
            % * __Mode__ block mean hash mode. default 'Mode0'
            %
            % ### `MarrHildrethHash`
            % * __Alpha__ scale factor for marr wavelet. default 2
            % * __Scale__ level of scale factor. default 1
            %
            % ### `RadialVarianceHash`
            % * __Sigma__ Gaussian kernel standard deviation. default 1
            % * __NumOfAngleLine__ Number of angles to consider. default 180
            %
            % See also: cv.ImgHash.compute, cv.ImgHash.compare
            %
            this.id = ImgHash_(0, 'new', alg, varargin{:});
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     obj.delete()
            %
            % See also: cv.ImgHash
            %
            if isempty(this.id), return; end
            ImgHash_(this.id, 'delete');
        end

        function typename = typeid(this)
            %TYPEID  Name of the C++ type (RTTI)
            %
            %     typename = obj.typeid()
            %
            % ## Output
            % * __typename__ Name of C++ type
            %
            typename = ImgHash_(this.id, 'typeid');
        end
    end

    %% ImgHashBase
    methods
        function hash = compute(this, img)
            %COMPUTE  Computes hash of the input image
            %
            %     hash = obj.compute(img)
            %
            % ## Input
            % * __img__ input image wanting to compute its hash value.
            %
            % ## Output
            % * __hash__ hash of the image.
            %
            % See also: cv.ImgHash.compare
            %
            hash = ImgHash_(this.id, 'compute', img);
        end

        function val = compare(this, hashOne, hashTwo)
            %COMPARE  Compare two hash values
            %
            %     val = obj.compare(hashOne, hashTwo)
            %
            % ## Input
            % * __hashOne__ Hash value one.
            % * __hashTwo__ Hash value two.
            %
            % ## Output
            % * __val__ indicate similarity between the two hashes, the
            %   meaning of the value vary from algorithm to algorithm.
            %
            % See also: cv.ImgHash.compute
            %
            val = ImgHash_(this.id, 'compare', hashOne, hashTwo);
        end
    end

    %% Util functions
    methods (Static)
        function hash = averageHash(img)
            %AVERAGEHASH  Calculates the average hash in one call
            %
            %     hash = cv.ImgHash.averageHash(img)
            %
            % ## Input
            % * __img__ input image want to compute hash value, type should be
            %   `uint8` with 1/3/4 channels.
            %
            % ## Output
            % * __hash__ Hash value of input, it will contain 16 hex decimal
            %   number, return type is `uint8`
            %
            % See also: cv.ImgHash.ImgHash, cv.ImgHash.compute
            %
            hash = ImgHash_(0, 'averageHash', img);
        end

        function hash = blockMeanHash(img, varargin)
            %BLOCKMEANHASH  Computes block mean hash of the input image
            %
            %     hash = cv.ImgHash.blockMeanHash(img)
            %     hash = cv.ImgHash.blockMeanHash(img, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __img__ input image want to compute hash value, type should be
            %   `uint8` with 1/3/4 channels.
            %
            % ## Output
            % * __hash__ Hash value of input, it will contain 16 hex decimal
            %   number, return type is `uint8`.
            %
            % ## Options
            % * __Mode__ block mean hash mode, one of:
            %   * __Mode0__ (default) use fewer blocks and generates 16*16/8
            %     `uint8` hash values.
            %   * __Mode1__ use block blocks (step_sizes/2) and generates
            %     `fix(31*31/8)+1` `uint8` hash values.
            %
            % See also: cv.ImgHash.ImgHash, cv.ImgHash.compute
            %
            hash = ImgHash_(0, 'blockMeanHash', img, varargin{:});
        end

        function hash = colorMomentHash(img)
            %COLORMOMENTHASH   Computes color moment hash of the input
            %
            %     hash = cv.ImgHash.colorMomentHash(img)
            %
            % ## Input
            % * __img__ input image want to compute hash value, type should be
            %   `uint8` with 1/3/4 channels.
            %
            % ## Output
            % * __hash__ 42 hash values with type `double`.
            %
            % The algorithm comes from the paper [tang2012perceptual].
            %
            % See also: cv.ImgHash.ImgHash, cv.ImgHash.compute
            %
            hash = ImgHash_(0, 'colorMomentHash', img);
        end

        function hash = marrHildrethHash(img, varargin)
            %MARRHILDRETHHASH  Computes average hash value of the input image
            %
            %     hash = cv.ImgHash.marrHildrethHash(img)
            %     hash = cv.ImgHash.marrHildrethHash(img, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __img__ input image want to compute hash value, type should be
            %   `uint8` with 1/3/4 channels.
            %
            % ## Output
            % * __hash__ Hash value of input, it will contain 16 hex decimal
            %   number, return type is `uint8`.
            %
            % ## Options
            % * __Alpha__ scale factor for marr wavelet. default 2
            % * __Scale__ level of scale factor. default 1
            %
            % See also: cv.ImgHash.ImgHash, cv.ImgHash.compute
            %
            hash = ImgHash_(0, 'marrHildrethHash', img, varargin{:});
        end

        function hash = pHash(img)
            %PHASH  Computes pHash value of the input image
            %
            %     hash = cv.ImgHash.pHash(img)
            %
            % ## Input
            % * __img__ input image want to compute hash value, type should be
            %   `uint8` with 1/3/4 channels.
            %
            % ## Output
            % * __hash__ Hash value of input, it will contain 8 `uint8` values.
            %
            % See also: cv.ImgHash.ImgHash, cv.ImgHash.compute
            %
            hash = ImgHash_(0, 'pHash', img);
        end

        function hash = radialVarianceHash(img, varargin)
            %RADIALVARIANCEHASH  Computes radial variance hash of the input image
            %
            %     hash = cv.ImgHash.radialVarianceHash(img)
            %     hash = cv.ImgHash.radialVarianceHash(img, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __img__ input image want to compute hash value, type should be
            %   `uint8`, with 1/3/4 channels.
            %
            % ## Output
            % * __hash__ Hash value of input, contains 40 `uint8` values.
            %
            % ## Options
            % * __Sigma__ Gaussian kernel standard deviation. default 1
            % * __NumOfAngleLine__ The number of angles to consider.
            %   default 180
            %
            % See also: cv.ImgHash.ImgHash, cv.ImgHash.compute
            %
            hash = ImgHash_(0, 'radialVarianceHash', img, varargin{:});
        end
    end

end


function [vnz,rnz,parent,c,leftmost,p,q] = cs_sqr (A)                       %#ok
%CS_SQR symbolic sparse QR factorization.
%   [vnz,rnz,parent,c,leftmost,p] = cs_sqr(A): symbolic QR of A(p,:).
%   [vnz,rnz,parent,c,leftmost,p,q] = cs_sqr(A) computes the symbolic QR
%   factorization of A(p,q).  The fill-reducing ordering q is found via
%   q = cs_amd(A,3).
%
%   vnz is the number of entries in the matrix of Householder vectors, V.
%   rnz is the number of entries in R.  parent is elimination tree.
%   c(i) is the number of entries in R(i,:).  leftmost(i) = min(find(A(i,q))).
%   p is the row permutation used to ensure R has a symbolically zero-free
%   diagonal (it can be larger than m if A is structurally rank deficient).
%   q is the fill-reducing ordering, if requested.
%
%   Example:
%       Prob = UFget ('HB/ibm32') ; A = Prob.A ;
%       [vnz, rnz, parent, c, leftmost, p, q] = cs_sqr (A) ;
%       cspy (A (p,q)) ;
%
%   See also CS_AMD, CS_QR.
%
%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

error ('cs_sqr mexFunction not found') ;


%This Matlab script can be used to reproduce Figure 6.4 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Range of number of UEs per BS
Krange = [10 20];

%Compute maximum number of UEs
Kmax = max(Krange);

%Number of BS antennas
M = 100;

%Define the range of pilot reuse factors
fRange = [1 2 4];

%Select the number of setups with random UE locations
nbrOfSetups = 1;

%Select the number of channel realizations per setup
nbrOfRealizations = 200;

%Select the range of hardware qualities. The values at the same position in
%the different vectors are considered simultaneously.
kappatUE = [0.9 0.92 0.94 0.96 0.97 0.98 0.99 0.995 1];
kapparBS = kappatUE;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Define total uplink transmit power per UE (mW)
p = 100;

%Define noise figure at BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Prepare to save simulation results
sumSE_MR = zeros(length(kappatUE),length(fRange),nbrOfSetups,length(Krange));
sumSE_RZF = zeros(length(kappatUE),length(fRange),nbrOfSetups,length(Krange));
sumSE_MMMSE = zeros(length(kappatUE),length(fRange),nbrOfSetups,length(Krange));

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,Kmax,M,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoise = channelGaindB - noiseVariancedBm;

    %Go through all pilot reuse factors
    for s = 1:length(fRange)

        %Output simulation progress
        disp([num2str(s) ' reuse factors out of ' num2str(length(fRange))]);

        %Extract pilot reuse factor
        f = fRange(s);

        %Go through all hardware qualities
        for r = 1:length(kappatUE)

            for kind = 1:length(Krange)

                %Extract number of UEs
                K = Krange(kind);

                %Generate channel realizations with estimates and estimation
                %error correlation matrices, using LMMSE estimation
                [Hhat,C,tau_p,~,H] = functionChannelEstimates_impairments(R(:,:,1:K,:,:),channelGainOverNoise(1:K,:,:),nbrOfRealizations,M,K,L,p,f,kappatUE(r),kapparBS(r));

                %Compute SEs using Monte Carlo realizations
                [SE_MR,SE_RZF,SE_MMMSE] = functionComputeSE_UL_impairments(H,Hhat,C,tau_c,tau_p,nbrOfRealizations,M,K,L,p,kappatUE(r),kapparBS(r));

                %Save results
                sumSE_MR(r,s,n,kind) = mean(sum(SE_MR,1));
                sumSE_RZF(r,s,n,kind) = mean(sum(SE_RZF,1));
                sumSE_MMMSE(r,s,n,kind) = mean(sum(SE_MMMSE,1));

                %Delete large matrices
                clear Hhat C H;

            end

        end

    end

    %Delete large matrices
    clear R;

end

%% Plot the simulation results
for kind = 1:length(Krange)

    figure;
    hold on; box on;

    plot(kappatUE,max(mean(sumSE_MMMSE(:,:,:,kind),3),[],2),'rd-','LineWidth',1);
    plot(kappatUE,max(mean(sumSE_RZF(:,:,:,kind),3),[],2),'k-.','LineWidth',1);
    plot(kappatUE,max(mean(sumSE_MR(:,:,:,kind),3),[],2),'bs-','LineWidth',1);

    xlabel('Hardware quality');
    ylabel('Average sum SE [bit/s/Hz/cell]');
    legend('M-MMSE','RZF','MR','Location','NorthWest');
    ylim([0 ceil(max(max(mean(sumSE_MMMSE(:,:,:,kind),3),[],2))/10)*10]);

end


function [MVMResult,TWAResult] = eval_mvm_twa(ecg,ann,fs)
%OVERVIEW This function evaluates MVM and TWA for ecg data sampled at 1000
% Hz and the following list of annotations, qrs onset, q, r, s, qrs offsett,
% t offset.
%
%   INPUTS          MANDATORY       DESCRIPTION
%                   ecg             N by M array of ecg data. Contains M
%                                   channels of ecg with N data points.
%
%                   fs              Sampling frequency for ecg, needs to 1000 Hz.
%
%                   OPTIONAL
%                   ann             Structure containing the following
%                                   annotations, qrs onset, q, r, s, qrs
%                                   offset, t offset. If annotations are
%                                   not available pass an empty variable
%                                   ([]) as input for ann. Annotations will
%                                   be generated.
%
%   OUTPUTS
%                   MVMResult                       Structure containing the results of MVM
%                                                   analysis. The fields in the structure
%                                                   are listed below.
%
%                   Structure.Field
%                   MVMResult.energyinband_array    Array containing the energy in the every 2-7
%                                                   beat interval computed in the beatquency domain, for each analysis window.
%
%                   MVMResult.sqi_array             signal quality index
%                                                   for each analysis window.
%
%                   MVMResult.heart_rate_est_arr    average heart rate
%                                                   estimate for each analysis window.
%
%                   TWAResult               Structure containing the
%                                           results of TWA analysis. The fields in the structure
%                                           are listed below.
%
%                   TWAResult.HR            Estimate of average heart rate for each analysis window.
%                   TWAResult.VAlt          Estimate of TWA amplitude for
%                                           each analysis window.
%                   TWAResult.VAlt_Sig      Estimate of TWA amplitude for
%                                           each analysis window which are statistically
%                                           significant compared to the noise threshold.
%                   TWAResult.Noise_Median  The median of the gamma
%                                           distribution used to model the noise.
%                   TWAResult.Noise_95      The 95th percentile of the
%                                           gamma distribution used to model the noise.
%                   TWAResult.VAltPt        The location on the S-T offset
%                                           segment corresponding to the maximum difference between
%                                           the average even and average odd beat.
%                   TWAResult.successful    Flag if set to 1 means TWA
%                                           analysis is successful.
%
%   REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Ismail Sadiq
%	COPYRIGHT (C) 2019
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information. The license may be found in
%       the Documents folder of the Physionet-Cardiovascular-Signal-Toolbox.
%

% compute minimum amplitude resolution
amplitude_res = diff(ecg);
amplitude_res = min(amplitude_res(amplitude_res > eps));
if (amplitude_res > 0.006) % if amplitude resolution > 6 uV give warning on accuracy of twa results.
    disp(['Warning: Amplitude resolution for input ecg is ' num2str(amplitude_res) '. This greater than the suggested minimum resolution of 6 uV. Results for MV analysis may not be reliable.']);
end

if (fs == 1000)

    % Add dependencies
    addpath(genpath('../../../../PhysioNet-Cardiovascular-Signal-Toolbox-master')); % Cardio vascular toolbox
    addpath(genpath('./Tools/')); % Helper functions for mvtoolbox

    % check if ann provided as input
    annfields = {'QRSon', 'Q', 'R', 'S', 'QRSoff', 'Toff'};
    if (~isempty(ann))
        ann_check = zeros(1,length(annfields));
        for ii = 1:length(ann_check)
            if (isfield(ann,annfields{1,ii}) && ~isempty(getfield(ann,annfields{1,ii})))
                ann_check(ii) = 1;
            end
        end

        % check if any field missing
        missingi = find(ann_check == 0);
        if (~isempty(missingi))
            anntxt = [];
            for ii = 1:length(missingi)
                anntxt = [anntxt annfields{1,missingi(ii)} ', '];
            end
            disp(['Warning: Missing annotations, ' anntxt(1:end-2)])
            % generate annotations if missing
            disp('Incomplete annotations provided as input. Generating missing annotations.')
            HRVparams = InitializeHRVparams('MVanalysis'); %ecg = double(ECG_signal(:,7))/max(double(ECG_signal(:,7)));%std(double(ECG_signal(:,7)));
            [qrs_pos,sign,en_thres] = jqrs(ecg,HRVparams);
            ECG_header.nsig = 1; ECG_header.freq = fs; ECG_header.nsamp = length(ecg);
            wavedet_config.setup.wavedet.QRS_detection_only = 0;
            % plot(ecg); hold on; scatter(qrs_pos, ecg(qrs_pos)); hold off;
            [ann,~,~] = wavedet_3D_detector(ecg, qrs_pos', ECG_header, wavedet_config );

            [detection_flg,ann] = improvfiducials(ann, fs, ecg);

            if(~detection_flg)
                disp('Warning: Unable to improve fiducial point detection.')
            end

        end

    else
        disp('Annotations not provided as input. Generating missing annotations.')
        HRVparams = InitializeHRVparams('MVanalysis'); %ecg = double(ECG_signal(:,7))/max(double(ECG_signal(:,7)));%std(double(ECG_signal(:,7)));
        [qrs_pos,sign,en_thres] = jqrs(ecg,HRVparams);
        ECG_header.nsig = 1; ECG_header.freq = fs; ECG_header.nsamp = length(ecg);
        wavedet_config.setup.wavedet.QRS_detection_only = 0;
        % plot(ecg); hold on; scatter(qrs_pos, ecg(qrs_pos)); hold off;
        [ann,~,~] = wavedet_3D_detector(ecg', qrs_pos', ECG_header, wavedet_config );

        [detection_flg,ann] = improvfiducials(ann, fs, ecg);

        if(~detection_flg)
            disp('Warning: Unable to improve fiducial point detection.')
        end

    end

    % figure(1); plot(ecg); hold on; % Check annotation if needed
    % stem(ann.QRSon, ecg(ann.QRSon)); stem(ann.Q, ecg(ann.Q));
    % stem(ann.R, ecg(ann.R)); stem(ann.S, ecg(ann.S));
    % stem(ann.QRSoff, ecg(ann.QRSoff)); stem(ann.Toff, ecg(ann.Toff)); hold off
    % legend('ECG','QRSon','Q','R','S','QRSoff','Toff')

    % Variable for storing mvm for qrs
    numofleads = size(ecg,2);
    energyinband_array = cell(1,numofleads);
    sqi_array = cell(1,numofleads);
    heart_rate_est_arr = cell(1,numofleads);
    % Compute Morph Var QRS
    disp(['Evaluating QRS MVM ...']);
    normalize = 1;
    segment_size = 5;
    for lead = 1:size(ecg,2)
        [energyinband,sqi,heart_rate_est] = ComputeMVM(ecg(:,lead),ann,fs,segment_size,normalize);
        energyinband_array(1,lead) = mat2cell(energyinband, size(energyinband,1), size(energyinband,2));
        sqi_array(1,lead) = mat2cell(sqi, size(sqi,1), size(sqi,2));
        heart_rate_est_arr(1,lead) = mat2cell(heart_rate_est, size(heart_rate_est,1), size(heart_rate_est,2));
    end
    MVMResult.energyinband_array = energyinband_array;
    MVMResult.sqi_array = sqi_array;
    MVMResult.heart_rate_est_arr = heart_rate_est_arr;

    % Twa analysis
    pause(2)
    disp('Evaluating TWA ...');
    TWAResult = ComputeTWA(ecg,ann,fs);

else
    disp(['Input ecg is sampled at ' num2str(fs) ' Hz, needs to be sampled at 1000 Hz. Unable to perform MV analysis.'])
    MVMResult = NaN; TWAResult = NaN;
end

end


dataset_path = '/home/zhixuan/data1/HUMBI/HUMBI_uploaded/Gaze_81_140';
subject = 82;
frame = 1;
showAxis = false; % whether show 3D axis when plotting
useFitParams = false; % if true, read fit params and reconstruct face vertices/kps
                     % if false, read reconstructed face vertices/kps directly
vis3D = true; % if true, visualize 3D mesh and kps
               % if false, visualize their reprojections on image
%%%%%%%%%%%% modify above lines accordingly %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
config;
addpath('../face');
load('../face/model/model.mat', 'tri_list', 'shape_basis_3DMM', 'expression_basis_3DMM');
left_eye_tri_list = load_tri_list('./model/mesh_tri_left_eye.txt');
right_eye_tri_list = load_tri_list('./model/mesh_tri_right_eye.txt');
left_eye_vertex_indices = unique(left_eye_tri_list(:));
right_eye_vertex_indices = unique(right_eye_tri_list(:));

% extract camera params
intrinsics_path = sprintf(intrinsics_path_format, subject);
extrinsics_path = sprintf(extrinsics_path_format, subject);
[M, C, ~, ~] = extractCameraParameters(intrinsics_path, extrinsics_path, num_cam);
[ground_plane_normal, ground_plane_center] = getGroundPlaneFromCameras(C(:, 2:2:66), cameras_to_plane_offset);

% read data
reconstruction_dir = sprintf(recon_dir_format, subject, frame);
if useFitParams
    [~, ~, fitted_params] = read_face_recon(reconstruction_dir);
    [scale, R, t, shape_coefficients, expression_coefficients] = parse_fitted_params(fitted_params);
    vertices = reconstruct_face_fitted(meanface_3DMM, shape_basis_3DMM(:, 1:10), expression_basis_3DMM, ...
        shape_coefficients, expression_coefficients, scale, R, t);
else
    [vertices, ~, ~] = read_face_recon(reconstruction_dir);
end

% visualization
set(gcf, 'OuterPosition', get(0, 'Screensize')); % maximize the figure
if vis3D  % visualize 3D mesh and kps
    setup_vis; campos(C(:, 33)); camva(7); camlight('left');
    h = trimesh(tri_list+1, vertices(:,1), vertices(:,2), vertices(:,3), ...
        'FaceColor', [0 150 230]/255, 'EdgeColor', 'none', 'LineWidth', 0.1);
    h.FaceLighting = 'flat'; % or 'gouraud'
    hl = trimesh(left_eye_tri_list, vertices(:,1), vertices(:,2), vertices(:,3), ...
        'FaceColor', 'r', 'EdgeColor', 'none', 'LineWidth', 0.1);
    hl.FaceLighting = 'flat'; % or 'gouraud'
    hr = trimesh(right_eye_tri_list, vertices(:,1), vertices(:,2), vertices(:,3), ...
        'FaceColor', 'r', 'EdgeColor', 'none', 'LineWidth', 0.1);
    hr.FaceLighting = 'flat'; % or 'gouraud'
    title(sprintf('subject %d, frame %d', subject, frame));
else
    bboxes = readmatrix([sprintf(img_dir_format, subject, frame), '/list.txt']); % n x 5, bboxes on original image
    for i = 1:size(bboxes, 1)
        % compute reprojections
        cam = bboxes(i, 1);
        bbox = bboxes(i, 2:end); % 1 x 4, [xmin, xmax, ymin, ymax]
        scale_x = (bbox(2) - bbox(1) + 1) / img_size(1);
        scale_y = (bbox(4) - bbox(3) + 1) / img_size(2);
        % Notice: scale_x and scale_y may not be exactly the same but should be very close
        rvertices = reproject(vertices, M(:, :, cam+1)); % 3448 x 2
        rvertices = (rvertices - [bbox(1), bbox(3)]) ./ [scale_x, scale_y];
        rvertices = rvertices([left_eye_vertex_indices, right_eye_vertex_indices], :);

        % draw
        img_path = sprintf([img_dir_format, '/image%07d.png'], subject, frame, cam);
        img = imread(img_path);
        imshow(img); hold on
        scatter(rvertices(:,1), rvertices(:,2), 100, '.', 'MarkerEdgeColor', 'r', 'MarkerEdgeAlpha', 0.5);
        title(sprintf('subject %d, frame %d, cam %d (press "Enter" to go to next view)', subject, frame, cam));
        pause;
    end
end

function tri_list = load_tri_list(filename, startRow, endRow)
if nargin<=2
    startRow = 1;
    endRow = inf;
end
formatSpec = '%16f%16f%f%[^\n\r]';
fileID = fopen(filename,'r');
dataArray = textscan(fileID, formatSpec, endRow(1)-startRow(1)+1, 'Delimiter', '', 'WhiteSpace', '', 'TextType', 'string', 'EmptyValue', NaN, 'HeaderLines', startRow(1)-1, 'ReturnOnError', false, 'EndOfLine', '\r\n');
for block=2:length(startRow)
    frewind(fileID);
    dataArrayBlock = textscan(fileID, formatSpec, endRow(block)-startRow(block)+1, 'Delimiter', '', 'WhiteSpace', '', 'TextType', 'string', 'EmptyValue', NaN, 'HeaderLines', startRow(block)-1, 'ReturnOnError', false, 'EndOfLine', '\r\n');
    for col=1:length(dataArray)
        dataArray{col} = [dataArray{col};dataArrayBlock{col}];
    end
end
fclose(fileID);
tri_list = [dataArray{1:end-1}];
end


function sys=relaxdouble(X)
%RELAXDOUBLE Return numerial value treating nonlinear variables as independent

solution = yalmip('getsolution');
lmi_variables = X.lmi_variables;
opt_variables = solution.variables;

values = zeros(1+length(lmi_variables),1);
values(1)=1;
for i=1:length(lmi_variables)
    opt_index = find(lmi_variables(i)==opt_variables);
    if isempty(opt_index)
        values(i+1,1)=NaN;
    else
        values(i+1,1)=solution.optvar(opt_index);
    end
end

sys = X.basis*values;
sys = full(reshape(sys,X.dim(1),X.dim(2)));


%%*******************************************************************
%% HSDNTrhsfun: compute the right-hand side vector of the
%%              Schur complement equation for the NT direction.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

function [rhs,EinvRc,hRd] = HSDNTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu,hRd,dX,dZ)
global spdensity

m = par.m;
if (nargin > 8)
    corrector = 1;
else
    corrector = 0;
    hRd = zeros(m+2,1);
end
hEinvRc = zeros(m+2,1);
EinvRc  = cell(size(blk,1),1);
if length(sigmu)==1; sigmu = sigmu*ones(1,size(blk,1)); end
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    n = sum(pblk{2});  numblk = length(pblk{2});
    if strcmp(pblk{1},'l')
        if (corrector)
            Rq = dX{p}.*dZ{p};
        else
            Rq = sparse(n,1);
            tmp  = par.dd{p}.*Rd{p};
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = sigmu(p)./Z{p}-X{p} -Rq./Z{p};
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'q')
        w = sqrt(par.gamz{p}./par.gamx{p});
        if (corrector)
            hdx = qops(pblk,w,par.ff{p},5,dX{p});
            hdz = qops(pblk,w,par.ff{p},6,dZ{p});
            hdxdz = Arrow(pblk,hdx,hdz);
            vv = qops(pblk,w,par.ff{p},5,X{p});
            Vihdxdz = Arrow(pblk,vv,hdxdz,1);
            Rq = qops(pblk,w,par.ff{p},6,Vihdxdz);
        else
            Rq = sparse(n,1);
            tmp  = par.dd{p}.*Rd{p} + qops(pblk,qops(pblk,Rd{p},par.ee{p},1),par.ee{p},3);
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = qops(pblk,-sigmu(p)./(par.gamz{p}.*par.gamz{p}),Z{p},4)-X{p}-Rq;
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'s')
        n2 = pblk{2}.*(pblk{2}+1)/2;
        if (corrector)
            hdZ = Prod3(pblk,par.G{p},dZ{p},par.G{p}',1);
            hdX = spdiags(-par.sv{p},0,n,n)-hdZ;
            tmp = Prod2(pblk,hdX,hdZ,0);
            tmp = 0.5*(tmp+tmp');
            if (numblk == 1)
                d = par.sv{p};
                e = ones(pblk{2},1);
                Rq = 2*tmp./(d*e'+e*d');
                if (nnz(Rq) <= spdensity*n2); Rq = sparse(Rq); end
            else
                Rq = sparse(n,n);
                s = [0, cumsum(pblk{2})];
                for i = 1:numblk
                    pos = s(i)+1 : s(i+1);
                    d = par.sv{p}(pos); e = ones(length(pos),1);
                    Rq(pos,pos) = 2*tmp(pos,pos)./(d*e' + e*d'); %#ok
                end
            end
        else
            Rq = sparse(n,n);
            EinvRc{p} = Prod3(pblk,par.W{p},Rd{p},par.W{p},1,par.nzlistAy{p});
            tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
            hRd = hRd + tmp2;
        end
        tmp = spdiags(sigmu(p)./par.sv{p} -par.sv{p},0,n,n);
        EinvRc{p} = Prod3(pblk,par.G{p}',tmp-Rq,par.G{p},1);
        tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
        hEinvRc = hEinvRc + tmp2;
    end
end
%%
rhs = rp + hRd - hEinvRc;
rhs(m+1) = rhs(m+1) + (par.mu/par.tau - par.kap);
if (corrector)
    rhs(m+1) = rhs(m+1) - par.dtau*par.dkap/par.tau;
end
%%*******************************************************************


function pyra = projectpyramid(model, pyra)

% pyra = projectpyramid(model, pyra)
%
% Project feature pyramid pyra onto PCA eigenvectors stored
% in model.coeff.

for i = 1:length(pyra.feat)
  pyra.feat{i} = project(pyra.feat{i}, model.coeff);
end