function writehtk(file,d,fp,tc)
%WRITEHTK write data in HTK format []=(FILE,D,FP,TC)
%
% Inputs:
%    FILE = name of file to write (no default extension)
%       D = data to write: one row per frame
%      FP = frame period in seconds
%      TC = type code = the sum of a data type and (optionally) one or more of the listed modifiers
%             0  WAVEFORM     Acoustic waveform
%             1  LPC          Linear prediction coefficients
%             2  LPREFC       LPC Reflection coefficients:  -lpcar2rf([1 LPC]);LPREFC(1)=[];
%             3  LPCEPSTRA    LPC Cepstral coefficients
%             4  LPDELCEP     LPC cepstral+delta coefficients (obsolete)
%             5  IREFC        LPC Reflection coefficients (16 bit fixed point)
%             6  MFCC         Mel frequency cepstral coefficients
%             7  FBANK        Log Fliter bank energies
%             8  MELSPEC      linear Mel-scaled spectrum
%             9  USER         User defined features
%            10  DISCRETE     Vector quantised codebook
%            11  PLP          Perceptual Linear prediction
%            12  ANON
%            64  _E  Includes energy terms                  hd(1)
%           128  _N  Suppress absolute energy               hd(2)
%           256  _D  Include delta coefs                    hd(3)
%           512  _A  Include acceleration coefs             hd(4)
%          1024  _C  Compressed                             hd(5)
%          2048  _Z  Zero mean static coefs                 hd(6)
%          4096  _K  CRC checksum (not implemented yet)     hd(7) (ignored)
%          8192  _0  Include 0'th cepstral coef             hd(8)
%         16384  _V  Attach VQ index                        hd(9)
%         32768  _T  Attach delta-delta-delta index         hd(10)

%   Thanks to Scott Otterson for fixing a bug in writing ultra-long frames.

%      Copyright (C) Mike Brookes 2005
%      Version: $Id: writehtk.m,v 1.7 2006/11/16 08:49:29 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   ftp://prep.ai.mit.edu/pub/gnu/COPYING-2.0 or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fid=fopen(file,'w','b');
if fid < 0
    error(sprintf('Cannot write to file %s',file));
end
tc=bitset(tc,13,0);                 % silently ignore a checksum request

[nf,nv]=size(d);
nhb=10;                             % number of suffix codes
ndt=6;                              % number of bits for base type
hb=floor(tc*pow2(-(ndt+nhb):-ndt));
hd=hb(nhb+1:-1:2)-2*hb(nhb:-1:1);   % extract bits from type code
dt=tc-pow2(hb(end),ndt);            % low six bits of tc represent data type

if ~dt & (size(d,1)==1)             % if waveform is a row vector
    d=d(:);                         % ... convert it to a column vector
    [nf,nv]=size(d);
end

if hd(5)                            % if compressed
    dx=max(d,[],1);
    dn=min(d,[],1);
    a=ones(1,nv);                   % default compression factors for cols with max=min
    b=dx;
    mk=dx>dn;
    a(mk)=65534./(dx(mk)-dn(mk));   % calculate compression factors for each column
    b(mk)=0.5*(dx(mk)+dn(mk)).*a(mk);
    d=d.*repmat(a,nf,1)-repmat(b,nf,1); % compress the data
    nf=nf+4;                        % adjust frame count to include compression factors
end
fwrite(fid,nf,'long');              % write frame count
fwrite(fid,round(fp*1.E7),'long');  % write frame period (in 100 ns units)
if any(dt==[0,5,10]) | hd(5)        % write data as shorts
    if dt==5                        % IREFC has fixed scale factor
        d=d*32767;
        if hd(5)
            error('Cannot use compression with IREFC format');
        end
    end
    nby=nv*2;
    if nby<=32767
        fwrite(fid,nby,'short');        % write byte count
        fwrite(fid,tc,'short');         % write type code
        if hd(5)
            fwrite(fid,a,'float');        % write compression factors
            fwrite(fid,b,'float');
        end
        fwrite(fid,d.','short');        % write data array
    end
else
    nby=nv*4;
    if nby<=32767
        fwrite(fid,nby,'short');        % write byte count
        fwrite(fid,tc,'short');         % write type code
        fwrite(fid,d.','float');        % write data array
    end
end
fclose(fid);
if nby>32767
    delete(file);                       % remove file if byte count is rubbish
    error(sprintf('byte count of frame is %d which exceeds 32767 (is data transposed?)',nby));
end


function plot_distribution(QSM,fig,rela,cumu,dis,dis2,dis3,dis4)

% ---------------------------------------------------------------------
% PLOT_DISTRIBUTION     Plots the specified distribution(s) in the
%                           "treedata" field of the QSM structure array.
%
% Version 1.1.0
% Latest update     3 May 2022
%
% Copyright (C) 2020-2022 Pasi Raumonen
% ---------------------------------------------------------------------
%
% Inputs:
% QSM       The output of treeqsm function, may contain multiple models if
%               only one distribution. If multiple distributions are plotted,
%               then only one model.
% fig       Figure number
% rela      If rela = 1, then plots relative values (%), otherwise plots
%               absolute values
% cumu      If cumu = 1, then plot cumulative distribution
% dis       Distribution to be plotted, string name, e.g. 'VolCylDia'.
%               The name string is the one used in the "treedata"
% dis2      Optional, Second distribution to be plotted. Notice with more
%               than one distribution, only one model.
% dis3      Optional, Third distribution to be plotted
% dis4      Optional, Fourth distribution to be plotted
% ---------------------------------------------------------------------

% Changes from version 1.0.0 to 1.1.0, 3 May 2022:
% 1) Added new input "cum" for plottig the distributions as cumulative.
% 2) Added return if distributions are empty or all zero

% Generate strings for title, xlabel and ylabel:
if strcmp(dis(1:3),'Vol')
  str = 'volume';
  ylab = 'Volume (L)';
elseif strcmp(dis(1:3),'Are')
  str = 'area';
  ylab = 'Area (m^2)';
elseif strcmp(dis(1:3),'Len')
  str = 'length';
  ylab = 'Length (m)';
elseif strcmp(dis(1:3),'Num')
  str = 'number';
  ylab = 'Number';
end

if strcmp(dis(end-2:end),'Dia')
  str2 = 'diameter';
  xlab = 'diameter (cm)';
elseif strcmp(dis(end-2:end),'Hei')
  str2 = 'height';
  xlab = 'height (m)';
elseif strcmp(dis(end-2:end),'Ord')
  str2 = 'order';
  xlab = 'order';
elseif strcmp(dis(end-2:end),'Ang')
  str2 = 'angle';
  xlab = 'angle (deg)';
elseif strcmp(dis(end-2:end),'Azi')
  str2 = 'azimuth direction';
  xlab = 'azimuth direction (deg)';
elseif strcmp(dis(end-2:end),'Zen')
  str2 = 'zenith direction';
  xlab = 'zenith direction (deg)';
end

% Collect the distributions
if nargin == 5
  % Multiple QSMs, one and the same distribution
  m = max(size(QSM));
  D = QSM(1).treedata.(dis);
  n = size(D,2);
  for i = 2:m
    d = QSM(i).treedata.(dis);
    k = size(d,2);
    if k > n
      n = k;
      D(m,n) = 0;
      D(i,1:n) = d;
    elseif k < n
      D(i,1:k) = d;
    else
      D(i,:) = d;
    end
  end
  D = D(:,1:n);
else
  % One QSM, multiple distributions of the same type
  % (e.g. diameter distributions: 'NumCylDia', 'VolCylDia' and 'LenCylDia')
  m = nargin-4;
  D = QSM.treedata.(dis);
  n = size(D,2);
  if n == 0 || all(D == 0)
    return
  end
  for i = 2:m
    if i == 2
      D(m,n) = 0;
      D(i,:) = QSM.treedata.(dis2);
    elseif i == 3
      D(i,:) = QSM.treedata.(dis3);
    else
      D(i,:) = QSM.treedata.(dis4);
    end
  end
end

if rela
  % use relative value
  for i = 1:m
    D(i,:) = D(i,:)/sum(D(i,:))*100;
  end
  ylab = 'Relative value (%)';
end

if cumu
  % use cumulative distribution
  D = cumsum(D,2);
end

% Generate the bar plot
figure(fig)
if strcmp(dis(end-3:end),'hAzi') || strcmp(dis(end-3:end),'1Azi') || strcmp(dis(end-2:end),'Azi')
  bar(-170:10:180,D')
elseif strcmp(dis(end-2:end),'Zen') || strcmp(dis(end-2:end),'Ang')
  bar(10:10:10*n,D')
else
  bar(1:1:n,D')
end

% Generate the title of the plot
if strcmp(dis(end-2:end),'Ord') && ~strcmp(dis(1:3),'Num')
  tit = ['Branch ',str,' per branching order'];
elseif strcmp(dis(end-2:end),'Ord')
  tit = 'Number of branches per branching order';
elseif strcmp(dis(1:3),'Num')
  tit = ['Number of branches per ',str2,' class'];
elseif strcmp(dis(end-3),'h') || strcmp(dis(end-3),'1')
  tit = ['Branch ',str,' per ',str2,' class'];
else
  tit = ['Tree segment ',str,' per ',str2,' class'];
end
n = nargin;
if n > 5
  if ~strcmp(dis(1:3),dis2(1:3))
    if strcmp(dis(4),'C')
      tit = 'Tree segment distribution';
    else
      tit = 'Branch distribution';
    end
  elseif n > 6
    if ~strcmp(dis(1:3),dis3(1:3))
      if strcmp(dis(4),'C')
        tit = 'Tree segment distribution';
      else
        tit = 'Branch distribution';
      end
    elseif n > 7
      if ~strcmp(dis(1:3),dis4(1:3))
        if strcmp(dis(4),'C')
          tit = 'Tree segment distribution';
        else
          tit = 'Branch distribution';
        end
      end
    end
  end
end
title(tit)

% Generate the x-axis label
if strcmp(dis(end-5:end-3),'Cyl')
  xlab = ['Cylinder ',xlab];
else
  xlab = ['Branch ',xlab];
end
xlabel(xlab)

% Generate the y-axis label
ylabel(ylab);

% Tight axes and grid lines
axis tight
grid on

m = max(size(QSM));
% Add legends, if needed
if m > 1
  L = cell(m,1);
  for i = 1:m
    L{i} = ['model',num2str(i)];
  end
  legend(L,'location','best')
elseif nargin > 5
  m = nargin-4;
  L = cell(m,1);
  for i = 1:m
    if i == 1
      L{i} = dis(1:end-3);
    elseif i == 2
      L{i} = dis2(1:end-3);
    elseif i == 3
      L{i} = dis3(1:end-3);
    else
      L{i} = dis4(1:end-3);
    end
  end
  legend(L,'location','best')
end


%SEQUENTIAL Sequential mapping
%
%   V = SEQUENTIAL(W1,W2)
%   B = SEQUENTIAL(A,W)
%
% INPUT
%   W,W1,W2  Mappings
%   A        Dataset
%
% OUTPUT
%   V        Sequentially combined mapping
%   B        Dataset
%
% DESCRIPTION
% The two mappings W1 and W2 are combined into a single mapping V. Note
% that SEQUENTIAL(W1,W2) is equivalent to W1*W2. If W2 is a mapping of
% a type 'combiner', it is called to make a combination attempt.
%	SEQUENTIAL(A,W) maps the dataset A by the sequential mapping W.
%
% This routine is automatically called to execute W = W1*W2 or B = A*W2 in
% case W2 is a sequential mapping. It should not be directly called by users.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com

function [w,varargout] = sequential(w1,w2,w3)

	% Just necessary to inform PRMAP.
	if (nargin == 0)
		w = prmapping(mfilename,'combiner');
		return;
	end
	[m1,k1] = size(w1);
	[m2,k2] = size(w2);
	if (~isa(w2,'prmapping'))
		error('Second argument should be a mapping.')
	end

	if isa(w1,'prmapping')
		% Definition
    if isempty(w1) % treat empty mapping as unity mapping
      w = w2;
    elseif (iscombiner(w2))
			% Execute the mapping W2.
      varargout = repmat({[]},[1, max((nargout-1),0)]);
			map = getmapping_file(w2);
			pars = getdata(w2);
			[w,varargout{:}] = feval(map,w1,pars{:});
		else
			% W2 is just a general mapping.
			if (k1 > 0) && (m2 > 0) && (k1 ~= m2)
				error('Inner mapping/data sizes do not agree.')
			end

			% Define the mapping type after combining W1 and W2.
			if (isuntrained(w1)) || (isuntrained(w2))
				mappingtype = 'untrained';
      elseif (isgenerator(w2))
        mappingtype = 'generator';
      %elseif (isfixed(w2)) || (isfixed_cell(w2))
      %  mappingtype = 'fixed';
			elseif (istrained(w1)) || (istrained(w2))
				mappingtype = 'trained';
			else
				mappingtype = 'fixed';
			end

			if strcmp(mappingtype,'untrained')
				labels = [];
				size_in = 0;
				size_out = 0;
			elseif (m2 == 0 || k2 == 0) && (m1 ~= 0) && (k1 ~= 0)
				% E.G. TRAINED * FIXED
				labels   = getlabels(w1);
				size_in  = getsize_in(w1);
				size_out = getsize_out(w1);

			elseif (m2 ~= 0) && (k2 ~= 0) && (m1 == 0 || k1 == 0)
				% FIXED * TRAINED
				labels   = getlabels(w2);
				size_in  = getsize_in(w2);
				size_out = getsize_out(w2);

			elseif ~istrained(w2)
				% TRAINED * FIXED
				labels   = getlabels(w1);
				size_in  = getsize_in(w1);
				size_out = getsize_out(w2);

			else
				% TRAINED * TRAINED
				labels = getlabels(w2);
				size_in = getsize_in(w1);
				size_out = getsize_out(w2);
			end
			w = prmapping(mfilename,mappingtype,{w1,w2},labels,size_in,size_out);
    end
    if ismapping(w) && (getbatch(w1) || getbatch(w2))
      [n1,b1,o1] = getbatch(w1);
      [n2,b2,o2] = getbatch(w2);
      if ~n1
        b12 = b2; o12 = o2;
      elseif ~n2
        b12 = b1; o12 = o1;
      else
        b12 = min(b1,b2); o12 = min(o1,o2);
      end
      w = setbatch(w,true,b12,o12);
    end

  elseif isempty(w2) % treat empty mapping as unity mapping
    w = w1;

  else
		% Execution. We are here, when SEQUENTIAL(A,V) is called.
		if nargin == 3 % needed as MAP breaks down sequential mappings
			w2 = w2*w3;  % restore them!
		end
		a = w1;
		if (~isa(a,'double')) && (~isa(a,'prdataset'))
			error('Just datasets or doubles can be mapped.')
		end
		% V can be a more complex mapping.
		% v = +w2; v1 = v{1}; v2 = v{2};
    [v1,v2] = getdata(w2);
		if (isuntrained(v1))
			if (isuntrained(v2))
				u = a*v1;
				w = u*(a*u*v2);
			else
				w = a*v1*v2;
			end
		else
			if (isuntrained(v2)) && (~isgenerator(v1))
				w = v1*(a*v1*v2);
				% may be v1 changed the dimensionality, reset it:
				w = setsize_in(w,size(a,2));
      else
        w1 = a*v1;
        w = w1*v2;
				%w = a*v1*v2;
				featlabels = getlabels(w2);
				if (isdataset(w)) && ~isempty(featlabels) && size(w,2) == size(featlabels,1)
					w = setfeatlab(w,featlabels);
				end
			end
		end
		if ismapping(w)
			w = setbatch(w,getbatch(w2));
		end
	end
return;


% timecrossf() - compute the time frequency content of an array rows and
%                cross-coherence between the rows for. If the rows depict
%                different components activation, these results can then
%                be used to analyse time relation between these
%                components  (using the brainmovie function). You may
%                also input electrode activities into this function to
%                analyse the synchronisation and phase relashionship
%                between the electrodes.
%
% Usage:
%    >> [ allersps, allitcs, allcrossfs, allcrossfphis, times, freqs] ...
%        = timecrossf(data, frames, tlimits, srate, cycles, ...);
%
% Inputs:
%   data        - multi-row data (nb_input,frames*nepochs) for the first
%                 condition. Each row is a different input. The function
%                 will conpute the time frequency content of each row and
%                 the cross-coherence between the rows. If data is a cell
%                 array { data1 data2 }, the function will compute the
%                 time-frequency decomposition for these 2 arrays and the
%                 difference between them.
%   frames      - frames per epoch                        {768}
%   tlimits     - epoch time limits (ms) [mintime maxtime]{[-1000 2000]}
%   srate       - data sampling rate (Hz)                 {256}
%   ...         - see timef or crossf command for details about other
%                 parameters
%
% Outputs:
%   allersps      - cell array (size nb_row,1) of ERSP (event-related
%                   power spectrum content of each row of the data array)
%   allitcs       - cell array (size nb_row,1) of ITC (event-related
%                   inter-trial coherence content of each row of the data
%                   array)
%   allcrossfs    - cell array (size nb_row, nb_row) of cross-coherence
%                   between the row of the data array (only the upper
%                   diagonal part of the cell array is used)
%   allcrossfphis - cell array (size nb_row, nb_row) of cross-coherence
%                   phase between the row of the data array (only the
%                   upper diagonal part of the cell array is used)
%   times         - time array returned by the crossf or timef functions
%   freqs         - frequency array returned by the crossf or timef
%                   functions
%
% Example1: generate a movie, 1 condition, below 10Hz dynamics
%
%   % ICA decomposition, activity in array actICA (31 x (176 x n)),
%   % graph for 6 components, 176 points from -100 ms to 600 ms after
%   % stimulus onset, 250 Hz sampling rate
%   [ allersps, allitcs, allcrossfs, allcrossfphis, times, freqs] ...
%        = timecrossf( actICA(1:6,:), 176, [-100 600], 250, 1, 32, 100);
%   brainmovie( ersps, itcs, crossfs_amp, crossfs_phase, times, [1:2] )
%   %[1:2] indicates the frequency rows to take into account (freqs(1:2))
%
% Example2: generate a movie, 2 condition, below 10Hz dynamics
%
%   % Same as above with actICA2 (31 x (176 x n2)) the activity of the
%   % components for a different condition
%   [ allersps, allitcs, allcrossfs, allcrossfphis, times, freqs] ...
%        = timecrossf( actICA(1:6,:), 176, [-100 600], 250, 1, 32, 100);
%   [ allersps(:,2), allitcs(:,2), allcrossfs(:,:,2), ...
%	                          allcrossfphis(:,:,2)] ...
%        = timecrossf( actICA2(1:6,:), 176, [-100 600], 250, 1, 32, 100);
%   brainmovie( allersps, allitcs, allcrossfs, allcrossfphis, times, ...
%                [1:2] )
%
% See also timef(), crossf(), brainmovie()

% arno@salk.edu, Arnaud Delorme, CNL / Salk Institute, 2001

% This program is free software; you can redistribute it and/or
% modify it.
% 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.

function [ ALLERSP, ALLITC, ALLCROSSF, ALLCROSSFANGLE, times, freqs ] = timecrossf(data, frames, tlimits, srate, cycle, varargin);

if nargin < 3
	help timecrossf;
	return;
end;

SAVE    = 0;	% save graphs in jpeg format
%XDISP   = 'cole:0.0';
nbcompo = size( data, 1);
if iscell(data)
    data1 = data{1};
    data2 = data{2};
    nbcompo = size( data1, 1);
end;

% compute all time frequency maps
% -------------------------------
compter = 1;
for numcompo = 1:nbcompo
	if iscell(data)
        [ersp,itc,powbase,times,freqs,erspboot,itcboot] = newtimef({ data1(numcompo,:) data2(numcompo,:) }, ...
                                                          frames, tlimits, srate, cycle, varargin{:});
        size(ersp{1})
        ALLERSP{numcompo,1}     = applyboot( ersp{1}, erspboot{1});
        ALLERSP{numcompo,2}     = applyboot( ersp{2}, erspboot{2});
        ALLERSP{numcompo,3}     = applyboot( ersp{3}, erspboot{3});
        ALLITC {numcompo,1}     = applyboot( abs(itc{1}) , itcboot{1});
        ALLITC {numcompo,2}     = applyboot( abs(itc{2}) , itcboot{2});
        if ~isreal(itc{3}), itc{3} = abs(itc{3}); end;
        ALLITC {numcompo,3}     = applyboot( itc{3}, itcboot{3});
    else
        [ersp,itc,powbase,times,freqs,erspboot,itcboot] = newtimef( data(numcompo,:), ...
                                                          frames, tlimits, srate, cycle, varargin{:});
        ALLERSP{numcompo,1}    = applyboot( ersp, erspboot);
        ALLITC{numcompo,1}     = applyboot( itc,  itcboot);
    end;
	if SAVE
		%command = sprintf('print -djpeg timef%2.2d', numcompo); eval(command); close;
		command = sprintf('hgsave(''timef%2.2d'');', numcompo); eval(command); close;
	end;
	compter = compter + 1 ;
end;

% compute all cross coherence maps
% --------------------------------
ALLCROSSF = cell(nbcompo, nbcompo);
ALLCROSSFANGLE = cell(nbcompo, nbcompo);
for index1 = 1:nbcompo
	for index2 = 1:nbcompo
		if index2 > index1
            if iscell(data)
                [coh,mcoh,timesout,freqsout,cohboot,cohangles] = newcrossf({ data1(index1,:) data2(index1,:)}, ...
                                                                  { data1(index2,:) data2(index2,:)}, frames,  ...
                                                                  tlimits, srate, cycle, varargin{:});
                size(coh{1})
                ALLCROSSF      { index1, index2, 1 } = applyboot(coh{1}, cohboot{1});
                ALLCROSSF      { index1, index2, 2 } = applyboot(coh{2}, cohboot{2});
                ALLCROSSF      { index1, index2, 3 } = applyboot(coh{3}, cohboot{3});
                ALLCROSSFANGLE { index1, index2, 1 } = cohangles{1};
                ALLCROSSFANGLE { index1, index2, 2 } = cohangles{2};
                ALLCROSSFANGLE { index1, index2, 3 } = cohangles{3};
            else
                [coh,mcoh,timesout,freqsout,cohboot,cohangles] = newcrossf(data(index1,:), data(index2,:), frames,  ...
                                                                  tlimits, srate, cycle, varargin{:});
                ALLCROSSF      { index1, index2 } = applyboot(coh, cohboot);
                ALLCROSSFANGLE { index1, index2 } = cohangles;
                if SAVE
                    %command = sprintf('print -djpeg crossf%2.2d-%2.2d%', index1, index2); eval(command); close;
                    command = sprintf('hgsave(''crossf%2.2d-%2.2d%'');', index1, index2); eval(command); close;
                end;
            end;
		end;
	end;
end;

return;

function array = applyboot(array, arrayboot)
    if isempty(arrayboot), return; end;
    if size(arrayboot,3) == 2
        array(find((array > arrayboot(:,:,1)) & (array < arrayboot(:,:,2)))) = 0;
    elseif size(arrayboot,2) > 2
        array(find(array < arrayboot(:,:))) = 0;
    elseif size(arrayboot,2) == 2
        array(find((array > repmat(arrayboot(:,1),[1 size(array,2)])) & ...
                   (array < repmat(arrayboot(:,2),[1 size(array,2)])))) = 0;
    else
        array(find(array < repmat(arrayboot(:,1),[1 size(array,2)]))) = 0;
    end;


function [V,F] = bwstencil(im,varargin)
  % BWSTENCIL Create a 3D-printable stencil from an input binary image.
  %
  % [V,F] = bwstencil(im)
  % [V,F] = bwstencil(im,'ParameterName',ParameterValue, ...)
  %
  % Inputs:
  %   im  h by w by 1 binary image
  %   Optional inputs (see bwmesh.m)
  % Outputs:
  %   V  #V by 3 list of vertex positions with V(:,3) = 0 or 1 for top or
  %   bottom layer
  %
  % Example:
  %   % Read in image, convert to binary, shrink, invert
  %   im = ~imresize(im2bw(imread('hans-hass.jpg')),0.25);
  %   % Pad image to create boundary frame
  %   im = padarray(im,ceil(max(size(im))*0.125*[1;1]));
  %   % Generate stencil of thickness = 1px
  %   [V,F] = bwstencil(im,'Tol',1.5,'SmoothingIters',50);
  %   % Change thickness to 2.5% size of image
  %   V(:,3) = V(:,3) * 0.025*max(size(im));
  %   % render result:
  %   subplot(2,1,1);
  %   imshow(im);
  %   subplot(2,1,2);
  %   tsurf(F,V,'EdgeColor','none','FaceColor',[0.3 0.4 1.0],'FaceAlpha',0.9);
  %   l = light('Style','infinite','Position',[0.3 0.2 1]);
  %   axis equal;
  %
  % See also: bwmesh
  %
  if ~islogical(im)
    warning('Converting non-binary input to binary');
    im = im2bw(im);
  end
  imh = imfill(im,'holes');
  if any(im(:) ~= imh(:))
    warning('input contained islands, removing...');
    im = imh;
  end
  im = imresize(im,2,'nearest');
  [V,F] = bwmesh(~im,varargin{:});
  [V,F] = extrude(V,F);
  [V,F] = remesh_planar_patches(V,F);
end


function [rec,prec,ap] = VOCevaldet(VOCopts,id,cls,draw)
%% VOCevaldet
% inputs: VOCopts = imdb.details.VOCopts

% load test set
[gtids,t]=textread(sprintf(VOCopts.imgsetpath,VOCopts.testset),'%s %d');

% load ground truth objects
tic;
npos=0;
gt(length(gtids))=struct('BB',[],'diff',[],'det',[]);
for i=1:length(gtids)
    % display progress
    if toc>1
        fprintf('%s: pr: load: %d/%d\n',cls,i,length(gtids));
        drawnow;
        tic;
    end

    % read annotation
    rec=PASreadrecord(sprintf(VOCopts.annopath,gtids{i}));

    % extract objects of class
    clsinds=strmatch(cls,lower( {rec.objects(:).class} ),'exact');
    gt(i).BB=cat(1,rec.objects(clsinds).bbox)';
    gt(i).diff=[rec.objects(clsinds).difficult];
    gt(i).det=false(length(clsinds),1);
    npos=npos+sum(~gt(i).diff);
end

% load results
[ids,confidence,b1,b2,b3,b4]=textread(sprintf(VOCopts.detrespath,id,cls),'%s %f %f %f %f %f');
BB=[b1 b2 b3 b4]';

% sort detections by decreasing confidence
[sc,si]=sort(-confidence);
ids=ids(si);
BB=BB(:,si);

% assign detections to ground truth objects
nd=length(confidence);
tp=zeros(nd,1);
fp=zeros(nd,1);
tic;
for d=1:nd
    % display progress
    if toc>1
        fprintf('%s: pr: compute: %d/%d\n',cls,d,nd);
        drawnow;
        tic;
    end

    % find ground truth image
    i=strmatch(ids{d},gtids,'exact');
    if isempty(i)
        error('unrecognized image "%s"',ids{d});
    elseif length(i)>1
        error('multiple image "%s"',ids{d});
    end

    % assign detection to ground truth object if any
    bb=BB(:,d);
    ovmax=-inf;
    for j=1:size(gt(i).BB,2)
        bbgt=gt(i).BB(:,j);
        bi=[max(bb(1),bbgt(1)) ; max(bb(2),bbgt(2)) ; min(bb(3),bbgt(3)) ; min(bb(4),bbgt(4))];
        iw=bi(3)-bi(1)+1;
        ih=bi(4)-bi(2)+1;
        if iw>0 & ih>0
            % compute overlap as area of intersection / area of union
            ua=(bb(3)-bb(1)+1)*(bb(4)-bb(2)+1)+...
               (bbgt(3)-bbgt(1)+1)*(bbgt(4)-bbgt(2)+1)-...
               iw*ih;
            ov=iw*ih/ua;
            if ov>ovmax
                ovmax=ov;
                jmax=j;
            end
        end
    end
    % assign detection as true positive/don't care/false positive
    if ovmax>=VOCopts.minoverlap
        if ~gt(i).diff(jmax)
            if ~gt(i).det(jmax)
                tp(d)=1;            % true positive
		gt(i).det(jmax)=true;
            else
                fp(d)=1;            % false positive (multiple detection)
            end
        end
    else
        fp(d)=1;                    % false positive
    end
end

% compute precision/recall
fp=cumsum(fp);
tp=cumsum(tp);
rec=tp/npos;
prec=tp./(fp+tp);

% compute average precision

ap=0;
for t=0:0.1:1
    p=max(prec(rec>=t));
    if isempty(p)
        p=0;
    end
    ap=ap+p/11;
end

if draw
    % plot precision/recall
    plot(rec,prec,'-');
    grid;
    xlabel 'recall'
    ylabel 'precision'
    title(sprintf('class: %s, subset: %s, AP = %.3f',cls,VOCopts.testset,ap));
end


function [transV, yawRotV, heightV] = visual_odometry_down(rawImg)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.

    % The simple visual odometry with scanline intensity profile algorithm.
    % the input is a raw image
    % the output including horizontal translational velocity, rotational velocity,
    % vertical translational velocity (vertical)

    %% start to set up the visual odometry

    % define the veriable for drawing sub images used by estimating translational,
    % rotational, and pitch velocity
    global SUB_TRANS_IMG;
    global SUB_YAW_ROT_IMG;
    global SUB_HEIGHT_V_IMG;

    % definition the Y (vertical) range of images for odomentry, including image for
    % translational velocity, image for rotational velocity, and image for
    % height change velocity
    global ODO_IMG_HEIGHT_V_Y_RANGE;
    global ODO_IMG_YAW_ROT_Y_RANGE;

    % definition of X (horizontal) range of the image for odometry
    global ODO_IMG_YAW_ROT_X_RANGE;
    global ODO_IMG_HEIGHT_V_X_RANGE;

    global ODO_IMG_TRANS_Y_RANGE;
    global ODO_IMG_TRANS_X_RANGE;

    % define the size of resized images for odo
    global ODO_IMG_YAW_ROT_RESIZE_RANGE;
    global ODO_IMG_HEIGHT_V_RESIZE_RANGE;
    global ODO_IMG_TRANS_RESIZE_RANGE;

    % define the scale of translational velocity, rotational velocity, and pitch velocity
    global ODO_TRANS_V_SCALE;

    global ODO_YAW_ROT_V_SCALE;
    global ODO_HEIGHT_V_SCALE;

    % difine the maximum threshold of translational velocity, rotational velocity and pitch velocity
    global MAX_TRANS_V_THRESHOLD;
    global MAX_YAW_ROT_V_THRESHOLD;
    global MAX_HEIGHT_V_THRESHOLD;

    % define the veriable for visual odometry shift match in vertical and
    % horizontal
    global ODO_SHIFT_MATCH_VERT;
    global ODO_SHIFT_MATCH_HORI;

    % define the degree of the field of view in horizontal and vertical.
    % Field of View (FOV), Degree (DEG) the horizontal, vertical, and diagonal
    % degrees for all FOVs
    global FOV_HORI_DEGREE;
    global FOV_VERT_DEGREE;

    % x_sums, sum each column of intensity in the current image and previous
    % image perspectively.
    % form one dimensional vector, 1*N array
    global PREV_TRANS_V_IMG_X_SUMS;
    global PREV_YAW_ROT_V_IMG_X_SUMS;
    global PREV_HEIGHT_V_IMG_Y_SUMS;

    % define the previous velocity for keeping stable speed
    global PREV_TRANS_V;
    global PREV_YAW_ROT_V;
    global PREV_HEIGHT_V;

    global DEGREE_TO_RADIAN;

    %%% End up for setting up the visual odometry

    global OFFSET_YAW_ROT;
    global OFFSET_HEIGHT_V;

    %% start to compute the horizontal rotational velocity (yaw)

    % get the sub_image for rotational velocity from raw image with range constrait
    subRawImg = rawImg(ODO_IMG_YAW_ROT_Y_RANGE, ODO_IMG_YAW_ROT_X_RANGE);
    subRawImg = imresize(subRawImg, ODO_IMG_YAW_ROT_RESIZE_RANGE);
    horiDegPerPixel = FOV_HORI_DEGREE / size(subRawImg, 2);

    SUB_YAW_ROT_IMG = subRawImg;
    SUB_TRANS_IMG = subRawImg;

%     % get the size of template image after resized
%     ySizeODOImg = ODO_IMG_YAW_ROT_RESIZE_RANGE(1);
%     xSizeODOImg = ODO_IMG_YAW_ROT_RESIZE_RANGE(2);
%     ySizeNormImg = ySizeODOImg;
%
%     PATCH_SIZE_Y_K = 5;
%     PATCH_SIZE_X_K = 5;
%
%     % define a temp variable for patch normalization
%     % extent the dimension of raw image for patch normalization (extODOImg, extension sub image of vtResizedImg)
%     extODOImg = zeros(ySizeODOImg + PATCH_SIZE_Y_K - 1, xSizeODOImg + PATCH_SIZE_X_K - 1);
%     extODOImg(fix((PATCH_SIZE_Y_K + 1 )/2) : fix((PATCH_SIZE_Y_K + 1 )/2) ...
%         + ySizeNormImg - 1 ,  fix((PATCH_SIZE_X_K + 1 )/2) : fix((PATCH_SIZE_X_K + 1 )/2) ...
%         + xSizeODOImg - 1 ) = subRawImg;
%
%     %% patch normalisation is applied to compensate for changes in lighting condition
%     for v = 1: ySizeNormImg
%         for u = 1 : xSizeODOImg
%             % get patch image
%             patchImg = extODOImg(v : v + PATCH_SIZE_Y_K - 1, u : u + PATCH_SIZE_X_K -1);
%
%             % Find the average of the matrix patch image
%             meanPatchImg = mean2(patchImg);
%
%             % Find the standard deviation of the matrix patch image
%             stdPatchIMG = std2(patchImg);
%
%             % Normalize the sub raw image
%             % normODOImg(v,u) = 11 * 11 * (vtResizedImg(v,u) - meanPatchImg ) / stdPatchIMG ;
%             % normODOImg(v,u) =  PATCH_SIZE_Y_K * PATCH_SIZE_X_K * (vtResizedImg(v,u) - meanPatchImg ) / stdPatchIMG ;
%             normODOImg(v,u) =  (subRawImg(v,u) - meanPatchImg ) / stdPatchIMG/ 255;
%
%         end
%     end
%
%     SUB_YAW_ROT_IMG = normODOImg;
%     SUB_TRANS_IMG = normODOImg;

    % get the x_sum of average sum intensity values in every column of image
    imgXSums = sum(subRawImg);
    avgIntensity = sum(imgXSums) / size(imgXSums, 2);
    imgXSums = imgXSums / avgIntensity;

    % compare the current image with the previous image
    % get the minimum offset and minimum difference of intensity between two images
    [minOffsetYawRot, minDiffIntensityRot] = compare_segments(imgXSums, PREV_YAW_ROT_V_IMG_X_SUMS, ODO_SHIFT_MATCH_HORI, size(imgXSums, 2));

    OFFSET_YAW_ROT = minOffsetYawRot;
    yawRotV = ODO_YAW_ROT_V_SCALE * minOffsetYawRot * horiDegPerPixel;  % in deg

    if abs(yawRotV) > MAX_YAW_ROT_V_THRESHOLD
        yawRotV = PREV_YAW_ROT_V;
    else
        PREV_YAW_ROT_V = yawRotV;
    end

    PREV_YAW_ROT_V_IMG_X_SUMS = imgXSums;
    PREV_TRANS_V_IMG_X_SUMS = imgXSums;
    %%% end up to compute the translational velocity

    %% start to compute total translational velocity

    % principle
    % speeds are estimates based on the rate of image change.
    % the speed measure v is obtained from the filtered average absolute
    % intensity difference between consecutive scanline intensity profiles at
    % the best match for rotation with best offest in yaw and pitch shift

    transV = minDiffIntensityRot * ODO_TRANS_V_SCALE;

    if transV > MAX_TRANS_V_THRESHOLD
       transV = PREV_TRANS_V;
    else
       PREV_TRANS_V = transV;
    end

    %% start to compute the height change velocity

    % get the sub_image for pitch velocity from raw image with range constrait
    subRawImg = rawImg(ODO_IMG_HEIGHT_V_Y_RANGE, ODO_IMG_HEIGHT_V_X_RANGE);
    subRawImg = imresize(subRawImg, ODO_IMG_HEIGHT_V_RESIZE_RANGE);
    vertDegPerPixel = FOV_VERT_DEGREE / size(subRawImg, 1);

    if minOffsetYawRot > 0
        subRawImg = subRawImg(:, minOffsetYawRot + 1 : end);
    else
        subRawImg = subRawImg(:, 1 : end -(-minOffsetYawRot));
    end

    SUB_HEIGHT_V_IMG = subRawImg;

    imageYSums = sum(subRawImg,2);
    avgIntensity = sum(imageYSums) / size(imageYSums, 1);
    imageYSums = imageYSums / avgIntensity;

    [minOffsetHeightV, minDiffIntensityHeight] = compare_segments(imageYSums, PREV_HEIGHT_V_IMG_Y_SUMS, ODO_SHIFT_MATCH_VERT, size(imageYSums, 1));

    if minOffsetHeightV < 0
        minDiffIntensityHeight = - minDiffIntensityHeight;
    end

    OFFSET_HEIGHT_V = minOffsetHeightV;

    % covert the perceptual speed into a physical speed with an empirically
    % determined constant TRANSLATIONAL_VELOCITY_SCALE

%     if abs(minOffsetYawRot) < 2
%         heightV = ODO_HEIGHT_V_SCALE * minDiffIntensityHeight;
%     else
%         heightV = 0;
%     end

%     if abs(minOffsetYawRot) < 2
%         heightV = minOffsetHeightV * 0.1;
%     else
%         heightV = 0;
%     end
%
%     if abs(minOffsetHeightV) < 2
%         heightV = 0;
%     end
    if minOffsetHeightV < 0
        heightV = ODO_HEIGHT_V_SCALE * minDiffIntensityHeight;
    elseif PREV_HEIGHT_V < 0
        heightV = PREV_HEIGHT_V;
    else
        heightV = 0;
    end

    if abs(heightV) > MAX_HEIGHT_V_THRESHOLD
        heightV = PREV_HEIGHT_V;
    else
        PREV_HEIGHT_V = heightV;
    end

%   if abs(minOffsetHeightV) > 1
%         heightV = ODO_HEIGHT_V_SCALE * minDiffIntensityHeight;
%
%     else
%         heightV = 0;
%   end
%
%     if abs(minOffsetHeightV) > 1 && abs(minOffsetYawRot) < 2
%         heightV = ODO_HEIGHT_V_SCALE * minDiffIntensityHeight;
%
%     else
%         heightV = 0;
%     end

    % to detect excessively large translational velocity
    % the threshold Vmax ensured that spuriously high image differences were
    % not used. Large image differences could be caused by sudden illumination
    % changes such as when travelling uphill facing directly into the sun.

    % define a maximum velocity threshold according to the average motion speed

%     if abs(minOffsetHeightV) >3
%         transV = 0;
%     end

    PREV_HEIGHT_V_IMG_Y_SUMS = imageYSums;
    %%% end up to compute the pitch velocity

end


function results = OTB_HC_settings(seq, res_path, bSaveImage, parameters)

close all

s_frames = seq.s_frames;

% Feature specific parameters
hog_params.cell_size = 6;
hog_params.compressed_dim = 10;

cn_params.tablename = 'CNnorm';
cn_params.useForGray = false;
cn_params.cell_size = 4;
cn_params.compressed_dim = 3;

ic_params.tablename = 'intensityChannelNorm6';
ic_params.useForColor = false;
ic_params.cell_size = 4;
ic_params.compressed_dim = 3;

% Which features to include
params.t_features = {
    struct('getFeature',@get_fhog,'fparams',hog_params),...
    struct('getFeature',@get_table_feature, 'fparams',cn_params),...
    struct('getFeature',@get_table_feature, 'fparams',ic_params),...
};

% Global feature parameters1s
params.t_global.normalize_power = 2;    % Lp normalization with this p
params.t_global.normalize_size = true;  % Also normalize with respect to the spatial size of the feature
params.t_global.normalize_dim = true;   % Also normalize with respect to the dimensionality of the feature

% Image sample parameters
params.search_area_shape = 'square';    % The shape of the samples
params.search_area_scale = 4.0;         % The scaling of the target size to get the search area
params.min_image_sample_size = 150^2;   % Minimum area of image samples
params.max_image_sample_size = 200^2;   % Maximum area of image samples

% Detection parameters
params.refinement_iterations = 1;       % Number of iterations used to refine the resulting position in a frame
params.newton_iterations = 5;           % The number of Newton iterations used for optimizing the detection score
params.clamp_position = false;          % Clamp the target position to be inside the image

% Learning parameters
params.output_sigma_factor = 1/16;		% Label function sigma
params.learning_rate = 0.007;	 	 	% Learning rate
params.nSamples = 30;                   % Maximum number of stored training samples
params.sample_replace_strategy = 'lowest_prior';    % Which sample to replace when the memory is full
params.lt_size = 0;                     % The size of the long-term memory (where all samples have equal weight)
params.train_gap = 5;                   % The number of intermediate frames with no training (0 corresponds to training every frame)
params.skip_after_frame = 10;           % After which frame number the sparse update scheme should start (1 is directly)
params.use_detection_sample = true;     % Use the sample that was extracted at the detection stage also for learning

% Factorized convolution parameters
params.use_projection_matrix = true;    % Use projection matrix, i.e. use the factorized convolution formulation
params.update_projection_matrix = true; % Whether the projection matrix should be optimized or not
params.proj_init_method = 'pca';        % Method for initializing the projection matrix
params.projection_reg = 1e-6;	 	 	% Regularization paremeter of the projection matrix

% Generative sample space model parameters
params.use_sample_merge = true;                 % Use the generative sample space model to merge samples
params.sample_update_criteria = 'Merge';        % Strategy for updating the samples
params.weight_update_criteria = 'WeightedAdd';  % Strategy for updating the distance matrix
params.neglect_higher_frequency = false;        % Neglect hiigher frequency components in the distance comparison for speed

% Conjugate Gradient parameters
params.CG_iter = 5;                     % The number of Conjugate Gradient iterations in each update after the first frame
params.init_CG_iter = 10*20;            % The total number of Conjugate Gradient iterations used in the first frame
params.init_GN_iter = 10;               % The number of Gauss-Newton iterations used in the first frame (only if the projection matrix is updated)
params.CG_use_FR = false;               % Use the Fletcher-Reeves (true) or Polak-Ribiere (false) formula in the Conjugate Gradient
params.CG_standard_alpha = true;        % Use the standard formula for computing the step length in Conjugate Gradient
params.CG_forgetting_rate = 50;	 	 	% Forgetting rate of the last conjugate direction
params.precond_data_param = 0.75;	 	% Weight of the data term in the preconditioner
params.precond_reg_param = 0.25;	 	% Weight of the regularization term in the preconditioner
params.precond_proj_param = 60;	 	 	% Weight of the projection matrix part in the preconditioner

% Regularization window parameters
params.use_reg_window = true;           % Use spatial regularization or not
params.reg_window_min = 1e-4;			% The minimum value of the regularization window
params.reg_window_edge = 4e-3;          % The impact of the spatial regularization
params.reg_window_power = 2;            % The degree of the polynomial to use (e.g. 2 is a quadratic window)
params.reg_sparsity_threshold = 0.05;   % A relative threshold of which DFT coefficients that should be set to zero

% Interpolation parameters
params.interpolation_method = 'bicubic';    % The kind of interpolation kernel
params.interpolation_bicubic_a = -0.75;     % The parameter for the bicubic interpolation kernel
params.interpolation_centering = true;      % Center the kernel at the feature sample
params.interpolation_windowing = false;     % Do additional windowing on the Fourier coefficients of the kernel

% Scale parameters for the translation model
% Only used if: params.use_scale_filter = false
% params.number_of_scales = 7;            % Number of scales to run the detector
% params.scale_step = 1.01;               % The scale factor

% Scale filter parameters
% Only used if: params.use_scale_filter = true
params.use_scale_filter = true;        % Use the fDSST scale filter or not (for speed)
params.scale_sigma_factor = 1/16;       % Scale label function sigma
params.scale_learning_rate = 0.025;		% Scale filter learning rate
params.number_of_scales_filter = 17;    % Number of scales
params.number_of_interp_scales = 33;    % Number of interpolated scales
params.scale_model_factor = 1.0;        % Scaling of the scale model
params.scale_step_filter = 1.02;        % The scale factor for the scale filter
params.scale_model_max_area = 32*16;    % Maximume area for the scale sample patch
params.scale_feature = 'HOG4';          % Features for the scale filter (only HOG4 supported)
params.s_num_compressed_dim = 'MAX';    % Number of compressed feature dimensions in the scale filter
params.lambda = 1e-2;					% Scale filter regularization
params.do_poly_interp = true;           % Do 2nd order polynomial interpolation to obtain more accurate scale

% Other parameters
params.visualization = 0;               % Visualiza tracking and detection scores
params.debug = 0;                       % Do full debug visualization

% Initialize
params.init_sz = [seq.init_rect(1,4), seq.init_rect(1,3)];
params.init_pos = [seq.init_rect(1,2), seq.init_rect(1,1)] + (params.init_sz - 1)/2;
params.s_frames = s_frames;

% Run tracker
results = tracker(params);


function F = in_fread_manscan(sFile, sfid, iEpoch, SamplesBounds)
% IN_FREAD_MANSCAN:  Read a block of recordings from a MANSCAN file
%
% USAGE:  F = in_fread_manscan(sFile, sfid, iEpoch, SamplesBounds)  : Read all channels
%         F = in_fread_manscan(sFile, sfid)                         : Read all channels, all the times

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2012-2014

if (nargin < 3) || isempty(iEpoch)
    iEpoch = 1;
end
if (nargin < 4) || isempty(SamplesBounds)
    if ~isempty(sFile.epochs)
        SamplesBounds = round(sFile.epochs(iEpoch).times .* sFile.prop.sfreq);
    else
        SamplesBounds = round(sFile.prop.times .* sFile.prop.sfreq);
    end
end

% Read data block
nChannels  = length(sFile.header.epoch(1).ChannelOrder);
nTimes = SamplesBounds(2) - SamplesBounds(1) + 1;
% Epoch offset
epochOffset = sFile.header.epoch(iEpoch).StartData1;
% Time offset
timeOffset = 2 * SamplesBounds(1) * nChannels;
% Total offset
totalOffset = epochOffset + timeOffset;

% Set position in file
fseek(sfid, totalOffset, 'bof');
% Read value
F = fread(sfid, [nChannels,nTimes], 'int16');

% Apply gains
F = bst_bsxfun(@rdivide, double(F), double(sFile.header.Gains));


function grains = subSet(grains,ind)
%
%
% Input
%  grains - @grain2d
%  ind    - indices
%
% Output
%  grains - @grain2d
%

% restrict boundary
if islogical(ind)
  % the problem is grainId is with respect to grain.id
  % but ind is with respect to the order of the grains
  % therefore we have to enlarge ind
  indLarge = false(max(grains.boundary.grainId(:)),1);
  indLarge(grains.id) = ind;

  grId = grains.boundary.grainId;
  grId(grId>0) = indLarge(grId(grId>0));
  indBd = any(grId,2);
  %repeat for inner Boundary
  indinLarge = false(max(grains.innerBoundary.grainId(:)),1);
  indinLarge(grains.id) = ind;

  grinId = grains.innerBoundary.grainId;
  grinId(grinId>0) = indinLarge(grinId(grinId>0));
  indinnerBd = any(grinId,2);

else
  indBd = any(ismember(grains.boundary.grainId,grains.id(ind)),2);
  indinnerBd = any(ismember(grains.innerBoundary.grainId,grains.id(ind)),2);
end

grains = subSet@dynProp(grains,ind);

grains.poly = grains.poly(ind);
grains.inclusionId = grains.inclusionId(ind);
grains.id = grains.id(ind);
grains.phaseId = reshape(grains.phaseId(ind),[],1);
grains.grainSize = grains.grainSize(ind);

if ~islogical(ind)
  grains.prop.meanRotation = reshape(grains.prop.meanRotation, size(ind));
end

grains.boundary = subSet(grains.boundary,indBd);
grains.innerBoundary = subSet(grains.innerBoundary,indinnerBd);

% if we have only one grain - sort boundary segments
if length(grains) == 1
  FNew = [grains.poly{1}(1:end-1).',grains.poly{1}(2:end).'];

  % remove inclusion embeddings
  if grains.inclusionId > 0
    ie = sum(FNew == FNew(1),2)==1;
    ie([1,end-grains.inclusionId]) = false;
    FNew(ie,:) = [];
  end

  % sort minimum entry first
  FNew = sort(FNew,2);

  % sort such the order of F follows the boundary
  [~,ind1] = sortrows(grains.boundary.F);
  [~,ind2] = sortrows(FNew);
  inverseorder(ind2) = ind1;

  grains.boundary = grains.boundary(inverseorder);

end


% Read single/multi channel waveforms
% Inputs:
%   files: a cell array of files to be read. Each cell represents one
%     sentence could be a cell and could be array of files itself if it is
%     multi-channel recording. A filename may also contains the starting
%     and ending time to be read, e.g. "abc.wav 0.2 0.5" will read the
%     segment from 0.2s to 0.5s of abc.wav.
%   reader: a structure that may contains following fields
%       fs: sampling frequency, if known in advance.
%       array: set to 1 if the sentence has multiple channels
%       multiArrayFiles: set to 1 if each channel is stored in one file.
%       useChannel: an array of integers specifying which channel(s) to be
%           returned.
%
%   Author: Xiong Xiao, Nanyang Technological University, Singapore
%   Last modified: 27 Jul 2016
%
function [all_wav, fs] = Reader_waveform(files, reader)

for i=1:length(files)
    if isfield(reader, 'array') && reader.array
        % read in multichannel waveforms
        if isfield(reader, 'multiArrayFiles') && reader.multiArrayFiles     % when the different channels are stored in different files
            for j=1:length(files{i})
                [tmp_wav, fs] = Reader_waveform_core(files{i}{j}, reader.fs);
                if j==1
                    wav = zeros(length(files{i}), length(tmp_wav));
                end
                wav(j,:) = tmp_wav;
            end
        else                                                                % when the different channels are stored in the same file
            [wav, fs] = Reader_waveform_core(files{i}, reader.fs);
        end
        % optional selection of channels
        if isfield(reader, 'useChannel')
            wav = wav(reader.useChannel, :);
        end
    else
        % read in single channel waveform
        [wav, fs] = Reader_waveform_core(files{i}, reader);
    end
    if isfield(reader, 'precision')
        switch lower(reader.precision)
            case 'int16'
                wav = StoreWavInt16(wav);
            case 'single'
                wav = single(wav);
            case 'double'
                wav = double(wav);
        end
    end
    all_wav{i} = wav;
end
end

%%
function [wav, fs] = Reader_waveform_core(file, reader)

words = ExtractWordsFromString_v2(file);

selectChannel = 0;
selectTime = 0;

filename = words{1};
if length(words)==4 || length(words)==2
    channelID = str2num(words{end});
    selectChannel = 1;
end
if length(words)>=3
    time1 = str2num(words{2});
    time2 = str2num(words{3});
    selectTime = 1;
end

fs = ReturnFieldWithDefaultValue(reader, 'fs', -1);
big_endian = ReturnFieldWithDefaultValue(reader, 'big_endian', 0);

switch lower(reader.name)
    case 'raw'
        [wav] = read08(filename, big_endian);   % you have to set fs correctly in the reader.
    otherwise
        if selectTime
            [wav, fs] = ReadAudioSeg(filename, time1, time2, fs);
        else
            [wav, fs] = audioread(filename);
        end
end

if selectChannel
    wav = wav(:,channelID);
end

wav = wav';

end


%%************************************************************
%% Prod3: compute the entries of Q = A*B*C specified in
%%        nzlistQ.
%%
%% Q = Prod3(blk,A,B,C,sym,nzlistQ)
%% Important: (a) A is assumed to be symmetric if nzlistQ
%%                has 2 columns (since mexProd2nz computes A'*B).
%%            (b) The 2nd column of nzlistQ must be sorted in
%%                ascending order.
%%
%% (optional) sym = 1, if Q is symmetric.
%%                = 0, otherwise.
%% (optional) nzlistQ = list of non-zero elements of Q to be
%%                      computed.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function Q = Prod3(blk,A,B,C,sym,nzlistQ)

  if (nargin<5); sym = 0; end;
  checkcell = [iscell(A) iscell(B) iscell(C)];
  if (nargin==6)
     checkcell(1,4) = iscell(nzlistQ);
  else
     nzlistQ = inf;
  end
%%
  if any(checkcell-1)
     if (size(blk,1) > 1)
        error('Prod3: blk and A,B,C are not compatible');
     end
     if strcmp(blk{1},'s')
        [len,len2] = size(nzlistQ);
        if (len == 0); nzlistQ = inf; len2 = 1; end;
        if (len2 == 1) & (nzlistQ == inf)
           tmp = Prod2(blk,A,B,0);
           Q = Prod2(blk,tmp,C,sym);
        else
           tmp = Prod2(blk,B,C,0);
           Q = mexProd2nz(blk,A,tmp,nzlistQ);
           if sym; Q = 0.5*(Q+Q'); end;
        end
     elseif strcmp(blk{1},'q') | strcmp(blk{1},'l') | strcmp(blk{1},'u')
        Q = A.*B.*C;
     end
  else
     error('Prod3: A,B,C,nzlistQ must all be matrices');
  end
%%************************************************************


%% OPTI Toolbox AMPL Examples
%
% This file loads a number of supplied examples and shows how to solve
% them using the OPTI Toolbox. You should read and complete BasicUsage.m
% BEFORE running the below examples.
%
% The underlying parser uses Netlib's AMPL Solver Library (ASL) Interface.
%
%   Copyright (C) 2014 Jonathan Currie (IPL)

% There is also a page on the Wiki which supplements this example:
web('https://www.inverseproblem.co.nz/OPTI/index.php/File/AMPL');

%% Loading an AMPL Problem
% OPTI Toolbox is supplied with a number of example AMPL problems ranging
% from LP to NLP including continuous and discrete problems. To load an
% AMPL model it must be in .NL format, see the supplied user's guide page
% on AMPL Interfacing in order to generate the .NL file from your model. To
% load an AMPL problem, simply use the command below. Returned will be a
% optiprob structure containing the data in the file.
clc
prob = amplRead('diet.nl')

%% Example 1 - Solving a Loaded AMPL Problem
% Solving a loaded AMPL problem is simple, just pass it to the opti
% constructor and call solve. You will note that the command amplRead will
% automatically determine the problem type (LP, QP, NLP, etc) based on
% properties supplied by the ASL.
clc
Opt = opti(prob)

x = solve(Opt)

%% Loading an AMPL Nonlinear Program
% The AMPL format can easily specify nonlinear problems unlike the LP or
% MPS formats.
clc
prob = amplRead('ch3.nl')

%% Example 2 - Solving a Nonlinear Problem from AMPL
% In order to solve a nonlinear problem the model .nl file
% remains open after the amplRead function is called to allow callbacks for
% the objective, gradient, etc. Calling solve() will automatically close
% the interface for linear and quadratic problems, however you must manually
% call close for nonlinear problems. Alternatively it will be automatically
% called once Matlab is closed to clean up ASL memory.
clc
Opt = opti(prob)

x = solve(Opt)

asl('close')

%% Loading an Integer Problem
% AMPL will reorder the variables from the original model based on internal
% rules (see the supplied pdfs). Based on this the ASL indicates which
% variables have binary or integer constraints, which are passed through
% the amplRead interface:
clc
prob = amplRead('multmip1.nl')

%% Example 3 - Solving an AMPL Integer Problem
% As the optiprob structure can fully define an integer problem, no change
% is required to build and solve the MIP
clc
Opt = opti(prob)

[x,fval] = solve(Opt);
fval

%% A Note On Hessians
% As of OPTI v1.58 the Hessian of an NLP will automatically be added to the
% problem structure. However note it is the Hessian of the Lagrangian, with
% the calling form:
%
%   H = hessian(x,sigma,lambda)
%
% This means the Hessian includes second derivatives of both the objective
% and the constraints. Sigma is a scalar scaling factor on the objective
% derivatives only, and lambda are the lagrange multipliers at the current
% point x (scaling the constraint derivatives).


function determ = rutis5_determinant ( )

%*****************************************************************************80
%
%% RUTIS5_DETERMINANT returns the determinant of the RUTIS5 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real DETERM, the determinant.
%
  determ = 1.0;

  return
end


classdef prtClassKnn < prtClass
    % prtClassKnn  K-nearest neighbors classifier
    %
    %    CLASSIFIER = prtClassKnn returns a K-nearest neighbors classifier
    %
    %    CLASSIFIER = prtClassKnn(PROPERTY1, VALUE1, ...) constructs a
    %    prtClassKnn object CLASSIFIER with properties as specified by
    %    PROPERTY/VALUE pairs.
    %
    %    A prtClassKnn object inherits all properties from the abstract class
    %    prtClass. In addition is has the following properties:
    %
    %    k                  - The number of neigbors to be considered
    %    distanceFunction   - The function to be used to compute the
    %                         distance from samples to cluster centers.
    %                         It must be a function handle of the form:
    %                         @(x1,x2)distFun(x1,x2). Most prtDistance*
    %                         functions will work.
    %
    %    For information on the  K-nearest neighbors classifier algorithm, please
    %    refer to the following URL:
    %
    %    http://en.wikipedia.org/wiki/K-nearest_neighbor_algorithm
    %
    %    A prtClassKnn object inherits the TRAIN, RUN, CROSSVALIDATE and
    %    KFOLDS methods from prtAction. It also inherits the PLOT method
    %    from prtClass.
    %
    %    Example:
    %
    %     TestDataSet = prtDataGenUnimodal;      % Create some test and
    %     TrainingDataSet = prtDataGenUnimodal;  % training data
    %     classifier = prtClassKnn;           % Create a classifier
    %     classifier = classifier.train(TrainingDataSet);    % Train
    %     classified = run(classifier, TestDataSet);         % Test
    %     classifier.plot;
    %
    %    See also prtClass, prtClassLogisticDiscriminant, prtClassBagging,
    %    prtClassMap, prtClassCap, prtClassBinaryToMaryOneVsAll, prtClassDlrt,
    %    prtClassPlsda, prtClassFld, prtClassRvm, prtClassGlrt,  prtClass

    properties (SetAccess=private)

        name = 'K-Nearest Neighbor'   % K-Nearest Neighbor
        nameAbbreviation = 'KNN'      % KNN
        isNativeMary = true;          % true

    end

    properties

        k = 3;   % The number of neighbors to consider in the voting

        distanceFunction = @(x1,x2)prtDistanceEuclidean(x1,x2);   % Function handle to compute distance
    end

    methods
        function Obj = prtClassKnn(varargin)
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
            Obj.verboseStorage = true;
        end
        function Obj = set.k(Obj,val)
            if ~prtUtilIsPositiveScalarInteger(val)
                error('prt:prtClassKnn:k','k must be a positive scalar integer');
            end
            Obj.k = val;
        end

        function Obj = set.distanceFunction(Obj,val)
            if ~isa(val,'function_handle')
                error('prt:prtClassKnn:distanceFunction','distanceFunction must be a function handle');
            end
            Obj.distanceFunction = val;
        end
    end

    methods (Access=protected, Hidden = true)
        function Obj = preTrainProcessing(Obj,DataSet)
            if ~Obj.verboseStorage
                warning('prtClassKnn:verboseStorage:false','prtClassKnn requires verboseStorage to be true; overriding manual settings');
            end
            Obj.verboseStorage = true;
            Obj = preTrainProcessing@prtClass(Obj,DataSet);
        end
        function Obj = trainAction(Obj,twiddle)
            %Do nothing; we've already specified "verboseStorage = true",
            %so the ".dataSet" field will be set when it comes time to test
        end

        function ClassifierResults = runAction(Obj,PrtDataSet)

            x = getObservations(PrtDataSet);
            n = PrtDataSet.nObservations;

            nClasses = Obj.dataSet.nClasses;
            uClasses = Obj.dataSet.uniqueClasses;
            labels = getTargets(Obj.dataSet);
            y = zeros(n,nClasses);

            xTrain = getObservations(Obj.dataSet);

            largestMatrixSize = prtOptionsGet('prtOptionsComputation','largestMatrixSize');
            memBlock = max(floor(largestMatrixSize/size(xTrain,1)),1);

            if n > memBlock
                for start = 1:memBlock:n
                    indices = start:min(start+memBlock-1,n);

                    distanceMat = feval(Obj.distanceFunction,xTrain,x(indices,:));

                    [twiddle,I] = sort(distanceMat,1,'ascend');
                    I = I(1:Obj.k,:);
                    L = labels(I);

                    % MATLAB indexing is inexplicably different if I is
                    % 1xN or (k>1)xN
                    if Obj.k ~= 1
                        L = L';
                    end

                    for class = 1:nClasses
                        y(indices,class) = sum(L == uClasses(class),2);
                    end
                end
            else
                distanceMat = feval(Obj.distanceFunction,xTrain,x);

                [twiddle,I] = sort(distanceMat,1,'ascend');
                I = I(1:Obj.k,:);
                L = labels(I);

                % MATLAB indexing is inexplicably different if I is
                % 1xN or (k>1)xN
                if Obj.k ~= 1
                    L = L';
                end

                for class = 1:nClasses
                    y(:,class) = sum(L == uClasses(class),2);
                end
            end

            [Etc.nVotes,Etc.MapGuessInd] = max(y,[],2);
            Etc.MapGuess = uClasses(Etc.MapGuessInd);
            ClassifierResults = PrtDataSet;
            ClassifierResults.X = y;

        end

    end
end


 function [ hHead,hTail,hPlate ] = arrow3d( orgin,r,h1,h2,dir)
%ARROW3D Summary of this function goes here
%  Detailed explanation goes here

%
% orgin=[0 0 0.2]';
% r=1;
% h=90;
% dir='y';

n=25;
% p=0.15;

hTail=Cylinder(orgin,r/2,h1,dir,n,'open');
hold on

if dir=='x'
    org=orgin+[h1; 0; 0];
elseif dir=='y'
    org=orgin+[0; h1; 0];
elseif dir=='z'
    org=orgin+[0; 0; h1];
end

xlabel('x')
ylabel('y')
zlabel('z')

[hHead hPlate]=Cone( org,r,h2,dir,n,'closed' );
set(hTail,'EdgeAlpha',0)


close all
clear

%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=8;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6gamma2_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories

save('timecom8','nmpcplot1','impcplot1');
save('feasibility8','nmpcinf','impcinf');

distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF

%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end

function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;

%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];

%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
    x0(4,1)*sin(x0(3,1))*dt;
    0;
    0];
system.C =[0 0 0 0;
    0 0 0 0;
    1*dt 0 0 0;
    0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;

%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;

%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
    fprintf('Run NMPC-DCBF-8\n');
    controller_nmpc_dcbf_multiple8 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple8.obs = obs;
    controller_nmpc_dcbf_multiple8.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple8.tt;
end

function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;

tic;

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
%    error('OSQP did not solve the problem!')
        t2=-1;
        return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)%Iterations for the first time-step
    storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return

x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)%Iterations for the left time steps

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    x0 = ctrlx(1:nx);
    testx0 = [testx0 x0];
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end
end

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end


% Test file for @chebfun/waterfall.m.

function pass = test_waterfall(pref)

if ( nargin < 1 )
    pref = chebfunpref();
end

hfig = figure('Visible', 'off');

Q = chebfun(@(x) [sin(pi*x) sin(pi*(x - 0.3)) sin(pi*(x - 0.6))], pref);
pass(1) = doesNotCrash(@() waterfall(Q));
pass(2) = doesNotCrash(@() waterfall(chebfun(@(x) 0*x)));
pass(3) = doesNotCrash(@() waterfall(Q.'));
pass(4) = doesNotCrash(@() waterfall(Q, [0 0.3 0.6]));
pass(5) = doesNotCrash(@() waterfall(cheb2quasi(Q)));
pass(6) = doesNotCrash(@() waterfall(Q, 'LineWidth', 2, 'FaceAlpha', 0.5, ...
    'FaceColor', 'r'));

close(hfig);

end

function pass = doesNotCrash(fn)

try
    fn();
    pass = true;
catch ME;
    pass = false;
end

end


function [V,E,F,H] = readPOLY_pyramid(filename)
  % READPOLY_PYRAMID Read .poly file of pyramid/svr output
  %
  % Inputs:
  %   filename  path to .poly file
  % Outputs:
  %   V  #V list of vertices
  %   E  #E by 2+boundary_markers list of segment indices, indexing V, and
  %     optional boundary markers
  %   F  #F struct containing polygon information arrays
  %     .facets  a #facets list of facets,  each facet is a polygon
  %       **NOTE: facets index E *not* V, contrary to typical (V,F) meshes and
  %       contrary to writePOLY_tetgen prototype
  %     .boundary_markers a #facets list of boundary_markers
  %     OR
  %   F  #F by constant-degree+boundary_markers  list of facets
  %   H  #H by dim list of holes (not supported)
  %

  fp = fopen(filename,'r');
  line = eat_comments(fp,'#');
  [Vhead,count] = sscanf(line,'%d %d %d %d');
  assert(count==4);
  [V,count] = fscanf(fp,'%g',[(1+sum(Vhead(2:end))) Vhead(1)]);
  V = V(2:Vhead(2)+1,:)';
  assert(count == (Vhead(1)*(1+sum(Vhead(2:end)))));
  line = eat_comments(fp,'#');
  [Ehead,count] = sscanf(line,'%d %d');
  assert(count == 2);
  [E,count] = fscanf(fp,'%d',[3+Ehead(2) Ehead(1)]);
  % only segments are supported
  BME = E(4:3+Ehead(2),:)';
  E = E(2:3,:)';
  offset = 1-min(min(E(:,1)),1);
  if offset
    warning('Offseting indices by %d',offset);
    E = E+offset;
  end
  assert(count == prod([3+Ehead(2) Ehead(1)]));
  line = eat_comments(fp,'#');
  [Fhead,count] = sscanf(line,'%d %d');
  % Not sure if boundary markers are allowed here
  if count == 1
    Fhead(2) = 0;
  end
  % preallocate
  %F = repmat(struct('facets',[],'boundary_markers',[]),Fhead(1),1);
  F.facets = cell(Fhead(1),1);
  F.boundary_markers = cell(Fhead(1),1);
  min_F = inf;
  for f = 1:Fhead(1)
    line = eat_comments(fp,'#');
    [EF,count] = sscanf(line,'%d');
    min_F = min(min_F,EF(1));
    assert(count >= 2);
    assert(count == 2+EF(2));
    F.facets{f} = EF(3:end);
    % not supported
    F.boundary_markers{f} = [];
  end

  fs = cell2mat(cellfun(@size,F.facets,'UniformOutput',false));
  % try to collapse F
  if all(fs(:,1) == 1) && all(fs(:,2) == fs(1,2))
      F = cell2mat(F.facets);
  end

  offset = 1-min(min_F,1);
  if offset
    warning('Offseting indices by %d',offset);
    F.facets = cellfun(@plus,F.facets, ...
      mat2cell(offset*ones(numel(F.facets),1),ones(numel(F.facets),1),1), ...
      'UniformOutput',false);
  end

  line = eat_comments(fp,'#');
  [Hhead,count] = sscanf(line,'%d %d');
  assert(Hhead(1) == 0);
  line = eat_comments(fp,'#');
  if ~isempty(line)
    [Rhead,count] = sscanf(line,'%d %d');
    assert(Rhead(1) == 0);
  end

  fclose(fp);

end


function suppData_ClinicalRF(pathSUPP,nBoot,nSplit,seeds,matlabPATH)

startpath = pwd;

cd(pathSUPP), load('training'), load('testing')
nameOutcomes = {'Locoregional','Distant','Death'}; nOutcomes = numel(nameOutcomes);
testSplit = 1/3; % Proportion of test cases in the splits
comb =  {{[1,2,3],...      % Loco        : Age, Subtype, T_Stage
        [1,2,5],...        % Loco        : Age, Subtype, TNM_Stage
        [1,2,3,4]},...     % Loco        : Age, Subtype, T_Stage, N_Stage
        {[1,2,4],...       % Distant     : Age, Subtype, N_Stage
        [1,2,5],...        % Distant     : Age, Subtype, TNM_Stage
        [1,2,3,4]},...     % Distant     : Age, Subtype, T_Stage, N_Stage
        {[1,2,3],...       % Death       : Age, Subtype, T_Stage
        [1,2,4],...        % Death       : Age, Subtype, N_Stage
        [1,2,5],...        % Death       : Age, Subtype, TNM_Stage
        [1,2,3,4]}};       % Death       : Age, Subtype, T_Stage, N_Stage
testCost = 0.4:0.1:2.5; % Emphasis factor on positive instances during random forest training
time = 60;
mkdir('batchLog_RFclinical'), cd('batchLog_RFclinical'), pathBatch = pwd;

% PRODUCE BATCH COMPUTATIONS
seed = seeds(1); batch = 0;
save('workspace','pathSUPP','nameOutcomes','comb','testCost','nSplit','testSplit','nBoot','seed'), pause(2);
for o = 1:nOutcomes
    batch = batch + 1;
    nameScript = ['batch',num2str(batch),'_script.m'];
    fid = fopen(nameScript,'w');
    fprintf(fid,'load(''workspace'')\n');
    fprintf(fid,['[indClinic,bestCost] = estimateClinicalRF(pathSUPP,nameOutcomes{',num2str(o),'},comb{',num2str(o),'},testCost,nSplit,testSplit,nBoot,seed);\n']);
    fprintf(fid,['save(''result_batch',num2str(batch),''',''indClinic'',''bestCost'')\n']);
    fprintf(fid,['system(''touch batch',num2str(batch),'_end'');\n']);
    fprintf(fid,'clear all');
    fclose(fid);
    system([matlabPATH,' -nojvm -nodisplay -nodesktop -nosplash < ',nameScript,' >& ',nameScript(1:end-1),'log &']);
end

% WAITING LOOP
waitBatch(pathBatch,time,nOutcomes)
delete('workspace.mat')

% GROUP RESULTS
cd(pathBatch)
batch = 0; parameters = struct;
for o = 1:nOutcomes
    batch = batch + 1;
    load(['result_batch',num2str(batch)]) % Variables 'indClinic' and 'bestCost' gets out of there
    parameters.clinical.(nameOutcomes{o}) = indClinic;
    parameters.cost.(nameOutcomes{o}) = bestCost;
    delete(['result_batch',num2str(batch),'.mat'])
    clear indClinic bestCost
end
cd(pathSUPP), save('clinicalPARAM','parameters')

% COMPUTE THE RANDOM FORESTS
for o = 1:nOutcomes
    if strcmp(nameOutcomes{o},'DeathSign')
        outcome = training.outcomes.Death;
    else
        outcome = training.outcomes.(nameOutcomes{o});
    end
    indClinic = parameters.clinical.(nameOutcomes{o});
    cost = parameters.cost.(nameOutcomes{o});
    tableTrain = training.clinical.table(:,indClinic);
    cat = logical(training.clinical.categories(indClinic));
    rng(seeds(2)), [RF] = trainRF_table(tableTrain,outcome,cat,nBoot,cost);
    RF = compact(RF); % Compact version
    save(['RFclinical_',nameOutcomes{o}],'RF')
end

% TEST THE RANDOM FORESTS
cd(pathSUPP), load('clinicalPARAM')
for o = 1:nOutcomes
    if strcmp(nameOutcomes{o},'DeathSign')
        outcome = testing.outcomes.Death;
        time = testing.timeToEvents.Death;
    else
        outcome = testing.outcomes.(nameOutcomes{o});
        time = testing.timeToEvents.(nameOutcomes{o});
    end
    censoring = 1 - outcome;
    results = struct;
    RF = load(['RFclinical_',nameOutcomes{o}]); RF = struct2cell(RF); RF = RF{1};
    indClinic = parameters.clinical.(nameOutcomes{o});
    tableTest = testing.clinical.table(:,indClinic);
    [prob] = predictRF(tableTest,RF);
    results.probResponse = prob;
    [AUC,sensitivity,specificity,accuracy] = calcPerformMetrics(prob,outcome,0.5);
    CI = calcCI(prob,time,censoring);
    results.AUC = AUC; results.Sensitivity = sensitivity; results.Specificity = specificity; results.Accuracy = accuracy;
    results.CI = CI;
    save(['testResultsRFclinical_',nameOutcomes{o}],'results'), clear results
end

cd(startpath)
end


function output = callsdpa(interfacedata)

% Retrieve needed data
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
K       = interfacedata.K;
x0      = interfacedata.x0;
ub      = interfacedata.ub;
lb      = interfacedata.lb;

% Bounded variables converted to constraints
if ~isempty(ub)
    [F_struc,K] = addStructureBounds(F_struc,K,ub,lb);
end

% Convert from internal (sedumi) format
[mDIM,nBLOCK,bLOCKsTRUCT,c,F] = sedumi2sdpa(F_struc,c,K);

if options.verbose==0
    options.sdpa.print = 'no';
else
    options.sdpa.print = 'display';
end

if options.savedebug
    ops = options.sdpa;
    save sdpadebug mDIM nBLOCK bLOCKsTRUCT c F ops
end

if options.showprogress;showprogress(['Calling ' interfacedata.solver.tag],options.showprogress);end

solvertime = tic;
[objVal,x,X,Y,INFO]=sdpam(mDIM,nBLOCK,bLOCKsTRUCT,c,F,[],[],[],options.sdpa);
solvertime = toc(solvertime);

% Create variables in YALMIP internal format
Primal = x;

Dual = [];
for i = 1:length(Y)
    Dual = [Dual;Y{i}(:)];
end

Slack = [];
if options.saveduals
    for i = 1:length(X)
        Slack = [Slack;X{i}(:)];
    end
end

switch (INFO.phasevalue)
    case 'pdOPT'
        problem = 0;
    case {'noINFO','pFEAS','dFEAS'}
        problem = 3;
    case {'pdFEAS'}
        problem = 4;
    case 'pFEAS_dINF'
        problem = 2;
    case 'pINF_dFEAS'
        problem = 1;
    case 'pUNBD'
        problem = 2;
    case 'dUNBD'
        problem = 1;
    case 'pdINF'
        problem = 12;
    otherwise
        problem = -1;
end
infostr = yalmiperror(problem,interfacedata.solver.tag);

if options.savesolveroutput
    solveroutput.objVal = objVal;
    solveroutput.x = x;
    solveroutput.X = X;
    solveroutput.Y = Y;
    solveroutput.INFO = INFO;
else
    solveroutput = [];
end

if options.savesolverinput
    solverinput.mDIM = mDIM;
    solverinput.nBLOCK=nBLOCK;
    solverinput.bLOCKsTRUCT=bLOCKsTRUCT;
    solverinput.c=c;
    solverinput.F=F;
else
    solverinput = [];
end

% Standard interface
output = createOutputStructure(Primal,Dual,[],problem,infostr,solverinput,solveroutput,solvertime);


function y = RASTA(x,parameter)

y = rastafilt(x',parameter)';


function [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencycpb(data1,data2,params,fscorr)
% Multi-taper coherency,cross-spectrum and individual spectra - continuous and binned point process data
%
% Usage:
%
% [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencycpb(data1,data2,params,fscorr)
% Inputs:
%       data1 (continuous data in form samples x trials) -- required
%       data2 (binned spike data in form samples x trials) -- required
%       params: structure with fields tapers, pad, Fs, fpass, err, trialave
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%           trialave (average over trials when 1, don't average when 0) - optional. Default 0
%       fscorr   (finite size corrections, 0 (don't use finite size corrections) or 1
%              (use finite size corrections) - optional (available only for spikes). Defaults 0.
% Outputs:
%       C (magnitude of coherency - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       phi (phase of coherency - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       S12 (cross spectrum -  frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       S1 (spectrum 1 - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       S2 (spectrum 2 - frequencies x trials if trialave=0; dimension frequencies if trialave=1)
%       f (frequencies)
%       zerosp (1 for trials where no spikes were found, 0 otherwise)
%       confC (confidence level for C at 1-p %) - only for err(1)>=1
%       phistd - theoretical/jackknife (depending on err(1)=1/err(1)=2) standard deviation for phi
%                Note that phi + 2 phistd and phi - 2 phistd will give 95% confidence
%                bands for phi - only for err(1)>=1
%       Cerr  (Jackknife error bars for C - use only for Jackknife -  err(1)=2)

if nargin < 2; error('Need data1 and data2'); end
if nargin < 3; params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear params
if nargin < 4 || isempty(fscorr); fscorr=0; end;

if nargout > 7 && err(1)==0;
%Cannot compute error bars if err(1)=0. Need to change params and run again.
    error('When errors are desired, err(1) has to be non-zero.');
end;
if nargout > 9 && err(1)~=2;
    error('Cerr computed only for Jackknife. Correct inputs and run again');
end;
[N,Ch]=check_consistency(data1,data2);
zerosp=zeros(1,Ch); % intialize the zerosp variable

nfft=max(2^(nextpow2(N)+pad),N);
[f,findx]=getfgrid(Fs,nfft,fpass);
tapers=dpsschk(tapers,N,Fs); % check tapers
J1=mtfftc(data1,tapers,nfft,Fs); % fourier transform of continuous data
[J2,Msp2,Nsp2]=mtfftpb(data2,tapers,nfft); % fourier transform of point process data
zerosp(Nsp2==0)=1; % set the trials where no spikes were found to have zerosp=1
J1=J1(findx,:,:);
J2=J2(findx,:,:);
S12=squeeze(mean(conj(J1).*J2,2));
S1=squeeze(mean(conj(J1).*J1,2));
S2=squeeze(mean(conj(J2).*J2,2));
if trialave; S12=squeeze(mean(S12,2)); S1=squeeze(mean(S1,2)); S2=squeeze(mean(S2,2)); end;
C12=S12./sqrt(S1.*S2);
C=abs(C12);
phi=angle(C12);
if nargout==10;
  if fscorr==1;
     [confC,phistd,Cerr]=coherr(C,J1,J2,err,trialave,[],Nsp2);
  else
     [confC,phistd,Cerr]=coherr(C,J1,J2,err,trialave);
  end;
elseif nargout==9;
  if fscorr==1;
     [confC,phistd]=coherr(C,J1,J2,err,trialave,[],Nsp2);
  else
     [confC,phistd]=coherr(C,J1,J2,err,trialave);
  end;
end;
clear Msp2


function [Au, idx ,idx2] = uniquecell(A)
    %function [Au, idx, idx2] = uniquecell(A)
    %For A a cell array of matrices (or vectors), returns
    %Au, which contains the unique matrices in A, idx, which contains
    %the indices of the last appearance of each such unique matrix, and
    %idx2, which contains th indices such that Au(idx2) == A
    %
    %Example usage:
    %
    %A = {[1,2,3],[0],[2,3,4],[2,3,1],[1,2,3],[0]};
    %[Au,idx,idx2] = uniquecell(A);
    %
    %Results in:
    %idx = [6,5,4,3]
    %Au  = {[0],[1,2,3],[2,3,1],[2,3,4]}
    %idx2 = [2,1,4,3,2,1]
    %
    %Algorithm: uses cellfun to translate numeric matrices into strings
    %           then calls unique on cell array of strings and reconstructs
    %           the initial matrices
    %
    %See also: unique
    B = cellfun(@(x) num2str(x(:)'),A,'UniformOutput',false);
    if nargout > 2
        [~,idx,idx2] = unique(B);
        Au = A(idx);
    else
        [~,idx] = unique(B);
        Au = A(idx);
    end
end


%DRAWAXIS  Draw coordinate system axis from pose estimation
%
%     img = cv.drawAxis(img, cameraMatrix, distCoeffs, rvec, tvec, length)
%
% ## Input
% * __img__ input image. It must have 1 or 3 channels. In the output, the
%   number of channels is not altered.
% * __cameraMatrix__ input 3x3 floating-point camera matrix
%   `A = [fx 0 cx; 0 fy cy; 0 0 1]`.
% * __distCoeffs__ vector of distortion coefficients
%   `[k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4]` of 4, 5, 8 or 12 elements.
% * __rvec__ rotation vector of the coordinate system that will be drawn.
% * __tvec__ translation vector of the coordinate system that will be drawn.
% * __length__ length of the painted axis in the same unit as `tvec`
%   (usually in meters).
%
% ## Output
% * __img__ output image.
%
% Given the pose estimation of a marker or board, this function draws the
% axis of the world coordinate system, i.e. the system centered on the
% marker/board. Useful for debugging purposes.
%
% See also: cv.estimatePoseBoard, cv.estimatePoseSingleMarkers,
%  cv.estimatePoseCharucoBoard, cv.Rodrigues, cv.line, cv.projectPoints
%


% Copyright Mohammad SAFEEA, 17th-Aug-2017
function factor=jointLimitAvoidance(jointIndex,jPos,dq)
% Return the displacemnt according to joint limits
clearance=pi/24;
maxLimit=[170,120,170,120,170,120,175]-clearance*ones(1,7);
minLimit=-maxLimit;
i=jointIndex;
    if(abs(jPos{i}+dq)>maxLimit(i))
        factor=0;
    else
        factor=1;
    end
end


function [fname,qname] = usgsdems(latlim,lonlim)

    %USGSDEMS  USGS 1-Degree (3-arc-sec resolution) DEM file names
    %
    % [fname,qname] = USGSDEMS(latlim,lonlim) returns cellarrays of the file
    % names and quadrangle names covering the geographic region for 1-degree
    % USGS digital elevation maps (also referred to as "3-arc second" or
    % "1:250,000 scale" DEMs).  The region is specified by scalar latitude and
    % longitude points, or two element vectors of latitude and longitude
    % limits in units of degrees.
    %
    %
    % The digital elevation map data files are available from the U.S.
    % Geological Survey over the internet from
    % <ftp://edcftp.cr.usgs.gov/pub/data/DEM/250/>.
    %
    % See also: USGSDEM

    %  Copyright 1996-2000 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
    % $Revision: 1399 $
    %  Written by:  A. Kim, W. Stumpf

    if nargin~=2
        error('Incorrect number of arguments')
    end

    if  isequal(size(latlim),[1 1])
        latlim = latlim*[1 1];
    elseif ~isequal(size(latlim),[1 2])
        error('Latitude limit input must be a scalar or 2 element vector')
    end

    if isequal(sort(size(lonlim)),[1 1])
        lonlim = lonlim*[1 1];
    elseif ~isequal(sort(size(lonlim)),[1 2])
        error('Longitude limit input must be a scalar or 2 element vector')
    end

    fid = fopen('usgsdems.dat','r');
    if fid==-1
        error('Couldn''t open usgsdems.dat')
    end

    % preallocate bounding rectangle data for speed

    YMIN = zeros(1,924); YMAX = YMIN;
    XMIN = YMIN; XMAX = YMIN;

    % read names and bounding rectangle limits

    for n=1:924
        fnames{n,1} = fscanf(fid,'%s',1);
        YMIN(n) = fscanf(fid,'%d',1);
        YMAX(n) = fscanf(fid,'%d',1);
        XMIN(n) = fscanf(fid,'%d',1);
        XMAX(n) = fscanf(fid,'%d',1);
        qnames{n,1} = fscanf(fid,'%s',1);
    end
    fclose(fid);

    do = ...
        find( ...
        (...
        (latlim(1) <= YMIN & latlim(2) >= YMAX) | ... % tile is completely within region
        (latlim(1) >= YMIN & latlim(2) <= YMAX) | ... % region is completely within tile
        (latlim(1) >  YMIN & latlim(1) <  YMAX) | ... % min of region is on tile
        (latlim(2) >  YMIN & latlim(2) <  YMAX)   ... % max of region is on tile
        ) ...
        &...
        (...
        (lonlim(1) <= XMIN & lonlim(2) >= XMAX) | ... % tile is completely within region
        (lonlim(1) >= XMIN & lonlim(2) <= XMAX) | ... % region is completely within tile
        (lonlim(1) >  XMIN & lonlim(1) <  XMAX) | ... % min of region is on tile
        (lonlim(2) >  XMIN & lonlim(2) <  XMAX)   ... % max of region is on tile
        )...
        );

    if ~isempty(do)
        fname = fnames(do);
        qname = qnames(do);
    else
        fname = [];
        qname = [];
    end