function [stat] = ft_spike_xcorr(cfg, spike)

% FT_SPIKE_XCORR computes the cross-correlation histogram and shift predictor.
%
% Use as
%   [stat] = ft_spike_xcorr(cfg, data)
%
% The input SPIKE should be organised as the spike or the raw datatype, obtained from
% FT_SPIKE_MAKETRIALS or FT_PREPROCESSING (in that case, conversion is done
% within the function). A mex file is located in /contrib/spike/private
% which will be automatically mexed.
%
% Configurations options for xcorr general:
%   cfg.maxlag           = number in seconds, indicating the maximum lag for the
%                          cross-correlation function in sec (default = 0.1 sec).
%   cfg.debias           = 'yes' (default) or 'no'. If 'yes', we scale the
%                          cross-correlogram by M/(M-abs(lags)), where M = 2*N -1 with N
%                          the length of the data segment.
%   cfg.method           = 'xcorr' or 'shiftpredictor'. If 'shiftpredictor'
%                           we do not compute the normal cross-correlation
%                           but shuffle the subsequent trials.
%                           If two channels are independent, then the shift
%                           predictor should give the same correlogram as the raw
%                           correlogram calculated from the same trials.
%                           Typically, the shift predictor is subtracted from the
%                           correlogram.
%   cfg.outputunit       = - 'proportion' (value in each bin indicates proportion of occurence)
%                          - 'center' (values are scaled to center value which is set to 1)
%                          - 'raw' (default) unnormalized crosscorrelogram.
%   cfg.binsize          = [binsize] in sec (default = 0.001 sec).
%   cfg.channelcmb       = Mx2 cell-array with selection of channel pairs (default = {'all' 'all'}),
%                          see FT_CHANNELCOMBINATION for details
%   cfg.latency          = [begin end] in seconds, 'max' (default), 'min', 'prestim'(t<=0), or
%                          'poststim' (t>=0).%
%   cfg.vartriallen      = 'yes' (default) or 'no'.
%                          If 'yes' - accept variable trial lengths and use all available trials
%                          and the samples in every trial.
%                          If 'no'  - only select those trials that fully cover the window as
%                          specified by cfg.latency and discard those trials that do not.
%                          if cfg.method = 'yes', then cfg.vartriallen
%                          should be 'no' (otherwise, fewer coincidences
%                          will occur because of non-overlapping windows)
%   cfg.trials           = numeric selection of trials (default = 'all')
%   cfg.keeptrials       = 'yes' or 'no' (default)
%
% A peak at a negative lag for stat.xcorr(chan1,chan2,:) means that chan1 is leading
% chan2. Thus, a negative lag represents a spike in the second dimension of
% stat.xcorr before the channel in the third dimension of stat.stat.
%
% Variable trial length is controlled by the option cfg.vartriallen. If it is
% specified as cfg.vartriallen='yes', all trials are selected that have a minimum
% overlap with the latency window of cfg.maxlag. However, the shift predictor
% calculation demands that following trials have the same amount of data, otherwise,
% it does not control for rate non-stationarities. If cfg.vartriallen = 'yes', all
% trials should fall in the latency window, otherwise we do not compute the shift
% predictor.
%
% Output:
%    stat.xcorr            = nchans-by-nchans-by-(2*nlags+1) cross correlation histogram with dimord 'chan_chan_time'
%   or
%    stat.shiftpredictor   = nchans-by-nchans-by-(2*nlags+1) shift predictor with dimord 'chan_chan_time'
% and
%    stat.lags             = (2*nlags + 1) vector with lags in seconds.
%    stat.trial            = ntrials-by-nchans-by-nchans-by-(2*nlags + 1) with single trials and dimord 'rpt_chan_chan_time'
%    stat.label            = corresponding labels to channels in stat.xcorr
%    stat.cfg              = configurations used in this function

% Copyright (C) 2010-2012, Martin Vinck
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble provenance spike

% check input spike structure
spike = ft_checkdata(spike, 'datatype', 'spike', 'feedback', 'yes');

% get the default options
cfg.trials         = ft_getopt(cfg, 'trials', 'all');
cfg.latency        = ft_getopt(cfg, 'latency','maxperiod');
cfg.keeptrials     = ft_getopt(cfg, 'keeptrials', 'no');
cfg.method         = ft_getopt(cfg, 'method', 'xcorr');
cfg.channelcmb     = ft_getopt(cfg, 'channelcmb', 'all');
cfg.vartriallen    = ft_getopt(cfg, 'vartriallen', 'yes');
cfg.debias         = ft_getopt(cfg, 'debias', 'yes');
cfg.maxlag         = ft_getopt(cfg, 'maxlag', 0.01);
cfg.binsize        = ft_getopt(cfg, 'binsize', 0.001);
cfg.outputunit     = ft_getopt(cfg, 'outputunit', 'proportion');

% ensure that the options are valid
cfg = ft_checkopt(cfg, 'latency', {'char', 'ascendingdoublebivector'});
cfg = ft_checkopt(cfg, 'trials', {'char', 'doublevector', 'logical'});
cfg = ft_checkopt(cfg, 'keeptrials', 'char', {'yes', 'no'});
cfg = ft_checkopt(cfg, 'method', 'char', {'xcorr', 'shiftpredictor'});
cfg = ft_checkopt(cfg, 'channelcmb', {'char', 'cell'});
cfg = ft_checkopt(cfg, 'vartriallen', 'char', {'no', 'yes'});
cfg = ft_checkopt(cfg, 'debias', 'char', {'yes', 'no'});
cfg = ft_checkopt(cfg, 'maxlag', 'doublescalar');
cfg = ft_checkopt(cfg, 'binsize', 'doublescalar');
cfg = ft_checkopt(cfg, 'outputunit', 'char', {'proportion', 'center', 'raw'});

cfg = ft_checkconfig(cfg, 'allowed', {'latency', 'trials', 'keeptrials', 'method', 'channelcmb', 'vartriallen', 'debias', 'maxlag', 'binsize', 'outputunit'});

doShiftPredictor  = strcmp(cfg.method, 'shiftpredictor'); % shift predictor

% determine the corresponding indices of the requested channel combinations
cfg.channelcmb = ft_channelcombination(cfg.channelcmb, spike.label(:), true);
cmbindx        = zeros(size(cfg.channelcmb));
for k=1:size(cfg.channelcmb,1)
  cmbindx(k,1) = strmatch(cfg.channelcmb(k,1), spike.label, 'exact');
  cmbindx(k,2) = strmatch(cfg.channelcmb(k,2), spike.label, 'exact');
end
nCmbs 	   = size(cmbindx,1);
chansel    = unique(cmbindx(:)); % get the unique channels
nChans     = length(chansel);
if isempty(chansel), error('No channel was selected'); end

% get the number of trials or change SPIKE according to cfg.trials
nTrials = size(spike.trialtime,1);
if  strcmp(cfg.trials,'all')
  cfg.trials = 1:nTrials;
elseif islogical(cfg.trials) || all(cfg.trials==0 | cfg.trials==1)
  cfg.trials = find(cfg.trials);
end
cfg.trials = sort(cfg.trials(:));
if max(cfg.trials)>nTrials, error('maximum trial number in cfg.trials should not exceed length of DATA.trial'); end
if isempty(cfg.trials), errors('No trials were selected in cfg.trials'); end
nTrials = length(cfg.trials);

% get the latencies
begTrialLatency = spike.trialtime(cfg.trials,1);
endTrialLatency = spike.trialtime(cfg.trials,2);
trialDur = endTrialLatency - begTrialLatency;

% select the latencies
if strcmp(cfg.latency,'minperiod')
  cfg.latency = [max(begTrialLatency) min(endTrialLatency)];
elseif strcmp(cfg.latency,'maxperiod')
  cfg.latency = [min(begTrialLatency) max(endTrialLatency)];
elseif strcmp(cfg.latency,'prestim')
  cfg.latency = [min(begTrialLatency) 0];
elseif strcmp(cfg.latency,'poststim')
  cfg.latency = [0 max(endTrialLatency)];
end

% check whether the time window fits with the data
if (cfg.latency(1) < min(begTrialLatency)), cfg.latency(1) = min(begTrialLatency);
  warning('Correcting begin latency of averaging window');
end
if (cfg.latency(2) > max(endTrialLatency)), cfg.latency(2) = max(endTrialLatency);
  warning('Correcting begin latency of averaging window');
end

% get the maximum number of lags in samples
fsample = (1/cfg.binsize);
if cfg.maxlag>(cfg.latency(2)-cfg.latency(1))
  warning('Correcting cfg.maxlag since it exceeds latency period');
  cfg.maxlag = (cfg.latency(2) - cfg.latency(1));
end
nLags        = round(cfg.maxlag*fsample);
lags         = (-nLags:nLags)/fsample; % create the lag axis

% check which trials will be used based on the latency
fullDur       = trialDur>=(cfg.maxlag); % only trials which are larger than maximum lag
overlaps      = endTrialLatency>(cfg.latency(1)+cfg.maxlag) & begTrialLatency<(cfg.latency(2)-cfg.maxlag);
hasWindow     = ones(nTrials,1);
if strcmp(cfg.vartriallen,'no') % only select trials that fully cover our latency window
  startsLater    = single(begTrialLatency) > (single(cfg.latency(1)) + 0.5*cfg.binsize);
  endsEarlier    = single(endTrialLatency) < (single(cfg.latency(2)) - 0.5*cfg.binsize);
  hasWindow      = ~(startsLater | endsEarlier); % check this in all other funcs
end
trialSel           = fullDur(:) & overlaps(:) & hasWindow(:);
cfg.trials         = cfg.trials(trialSel);
begTrialLatency    = begTrialLatency(trialSel);
endTrialLatency    = endTrialLatency(trialSel);

if isempty(cfg.trials), warning('No trials were selected'); end
if length(cfg.trials)<2&&doShiftPredictor
  error('Shift predictor can only be calculated with more than 1 selected trial.');
end

% if we allow variable trial lengths
if strcmp(cfg.vartriallen,'yes') && doShiftPredictor && ~strcmp(cfg.outputunit,'proportion')
  warning('using cfg.vartriallen = "yes" and shift predictor method: please use cfg.outputunit = "proportion"');
end
nTrials   = length(cfg.trials); % only now reset nTrials

% preallocate the sum and the single trials and the shift predictor
keepTrials   = strcmp(cfg.keeptrials,'yes');
s     = zeros(nChans,nChans,2*nLags);
if keepTrials
  warning('storing single trials for cross correlation is memory expensive, please check');
  singleTrials = zeros(nTrials,nChans,nChans,2*nLags);
end

if strcmp(cfg.method,'shiftpredictor'), singleTrials(1,:,:,:) = NaN; end

if ((cfg.latency(2)-cfg.latency(1))/cfg.maxlag)<2.5
  warning('selected latency will cause highly variable xcorr at borders');
end

ft_progress('init', 'text',     'Please wait...');
for iTrial = 1:nTrials
  origTrial = cfg.trials(iTrial);
  ft_progress(iTrial/nTrials, 'Processing trial %d from %d', iTrial, nTrials);

  for iCmb = 1:nCmbs
    % take only the times that are in this trial and latency window
    indx  = cmbindx(iCmb,:);
    inTrial1 = spike.trial{indx(1)}==origTrial;
    inTrial2 = spike.trial{indx(2)}==origTrial;
    inWindow1 = spike.time{indx(1)}>=cfg.latency(1) &  spike.time{indx(1)}<=cfg.latency(2);
    inWindow2 = spike.time{indx(2)}>=cfg.latency(1) &  spike.time{indx(2)}<=cfg.latency(2);
    ts1 = sort(spike.time{indx(1)}(inTrial1(:) & inWindow1(:)));
    ts2 = sort(spike.time{indx(2)}(inTrial2(:) & inWindow2(:)));

    switch cfg.method
      case 'xcorr'
        % compute the xcorr if both are non-empty
        if ~isempty(ts1) && ~isempty(ts2)
          if indx(1)<=indx(2)
            [x]   = spike_crossx_matlab(ts1(:),ts2(:),cfg.binsize,nLags*2+1);  % removed the mex file for now, until issue on windows is resolved
          else
            [x]   = spike_crossx_matlab(ts2(:),ts1(:),cfg.binsize,nLags*2+1); % removed the mex file for now, until issue on windows is resolved
          end

          % remove the center peaks from the auto-correlogram
          if indx(1)==indx(2), x(nLags:nLags+1) = 0; end

          if strcmp(cfg.debias,'yes')
            lags = (-nLags:nLags)*cfg.binsize;
            lags = (lags(2:end)+lags(1:end-1))/2;
            T    = nanmin([endTrialLatency(iTrial);cfg.latency(2)])-nanmax([begTrialLatency(iTrial);cfg.latency(1)]);
            sc = (T./(T-abs(lags(:))));
            sc = length(sc)*sc./sum(sc);
            x  = x(:).*sc(:);
          end
          % sum the xcorr
          s(indx(1),indx(2),:) = s(indx(1),indx(2),:) + shiftdim(x(:),-2);
          s(indx(2),indx(1),:) = s(indx(2),indx(1),:) + shiftdim(flipud(x(:)),-2);

          % store individual trials if requested
          if keepTrials
            singleTrials(iTrial,indx(1),indx(2),:) = shiftdim(x(:),-3);
            singleTrials(iTrial,indx(2),indx(1),:) = shiftdim(flipud(x(:)),-3);
          end
        end
      case 'shiftpredictor'
        if iTrial>1
          % symmetric, get x21 to x12 and x22 to x11
          inTrial1_old = spike.trial{indx(1)}==cfg.trials(iTrial-1);
          ts1_old      = sort(spike.time{indx(1)}(inTrial1_old(:) & inWindow1(:)));
          inTrial2_old = spike.trial{indx(2)}==cfg.trials(iTrial-1);
          ts2_old      = sort(spike.time{indx(2)}(inTrial2_old(:) & inWindow2(:)));

          % compute both combinations
          for k = 1:2

            % take chan from this and previous channel
            if k==1
              A = ts1;
              B = ts2_old;
            else
              A = ts1_old;
              B = ts2;
            end
            if ~isempty(A) && ~isempty(B)
              if indx(1)<=indx(2)
                [x]   = spike_crossx_matlab(A(:),B(:),cfg.binsize,nLags*2+1);
              else
                [x]   = spike_crossx_matlab(B(:),A(:),cfg.binsize,nLags*2+1);
              end
              % remove the center peaks from the auto-correlogram
              if indx(1)==indx(2), x(nLags:nLags+1) = 0; end

              if strcmp(cfg.debias,'yes')
                lags = (-nLags:nLags)*cfg.binsize;
                lags = (lags(2:end)+lags(1:end-1))/2;
                T    = nanmin([endTrialLatency(iTrial);cfg.latency(2)])-nanmax([begTrialLatency(iTrial);cfg.latency(1)]);
                sc = (T./(T-abs(lags(:))));
                sc = length(sc)*sc./sum(sc);
                x  = x(:).*sc(:);
              end
              % compute the sum
              s(indx(1),indx(2),:) =  s(indx(1),indx(2),:) + shiftdim(x(:),-2);
              s(indx(2),indx(1),:) =  s(indx(2),indx(1),:) + shiftdim(flipud(x(:)),-2);
              if keepTrials
                singleTrials(iTrial,indx(1),indx(2),:) = shiftdim(x(:)/2,-3);
                singleTrials(iTrial,indx(2),indx(1),:) = shiftdim(flipud(x(:))/2,-3);
              end
            end
          end
        end
    end % symmetric shift predictor loop
  end % combinations
end % trial loop
ft_progress('close')
% multiply the shift sum by a factor so it has the same scale as raw
dofShiftPred = 2*(nTrials-1);
if doShiftPredictor, s = s*nTrials/dofShiftPred; end
switch cfg.outputunit
  case 'proportion'
    sm = repmat(nansum(s,3),[1 1 nLags*2]);
    s = s./sm;
    if keepTrials
      singleTrials       = singleTrials./repmat(shiftdim(sm,-1),[nTrials 1 1 1]);
    end
  case 'center'
    center   = repmat(s(:,:,nLags+1),[1 1 nLags*2]);
    s        = s./center;
    if keepTrials
      singleTrials       = singleTrials./repmat(shiftdim(center,-1),[nTrials 1 1 1]);
    end
end

% return the results
stat.(cfg.method)      = s;
lags = (-nLags:nLags)*cfg.binsize;
lags = (lags(2:end)+lags(1:end-1))/2;
stat.time              = lags;
stat.dimord            = 'chan_chan_time';
if keepTrials
  stat.trial   = singleTrials;
  stat.dimord  = 'trial_chan_chan_time';
end
stat.label       = spike.label(chansel);

% do the general cleanup and bookkeeping at the end of the function

ft_postamble previous   spike
ft_postamble provenance stat
ft_postamble history    stat

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [C] = spike_crossx_matlab(tX,tY,binsize,nbins)
tX = sort(tX(:));
tY = sort(tY(:));

minLag = - binsize * (nbins-1) / 2;
j = 0:nbins-1;
B = minLag + j * binsize;
tX(tX<(tY(1)+minLag) | tX>(tY(end)-minLag))   = [];
if isempty(tX)
  C = zeros(1,length(B)-1);
  return;
end
tY(tY>(tX(end)-minLag) | tY<(tX(1)+minLag)) = [];
if isempty(tY)
  C = zeros(1,length(B)-1);
  return;
end
nX = length(tX); nY = length(tY);

% compute all distances at once using a multiplication trick
if (nX*nY)<2*10^7  % allow matrix to grow to about 150 MB, should always work
  D = log(exp(-tX(:))*exp(tY(:)'));
  D = D(:);
  D(abs(D)>abs(minLag)) = [];
  [C] = histc(D,B);
  C(end) = [];
else
  % break it down in pieces such that nX*nY<2*10*7
  k   = 2;
  nXs = round(nX/k);
  while (nXs*nY)>=(2*10^7)
    k = k+1;
    nXs = round(nX/k);
  end

  % get the indices
  steps = round(nX/k);
  begs = 1:steps:steps*k;
  ends = begs+(steps-1);
  rm   = begs>nX;
  begs(rm) = [];
  ends(rm) = [];
  ends(end) = nX;
  nSteps    = length(begs);

  C = zeros(1,length(B));
  for iStep = 1:nSteps
    d = log(exp(-tX(begs(iStep):ends(iStep)))*exp(tY(:)'));
    d = d(:);
    d(abs(d)>abs(minLag)) = [];
    C = C + histc(d,B)';
  end
  C(end) = [];
end

if isempty(C)
  C = zeros(1,length(B)-1);
end


function h = plus(f, g)
%+   Plus for BALLFUN.
% F + G adds F and G. F and G can be scalars or BALLFUNs.

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

fIsBallfun = isa(f, 'ballfun');
gIsBallfun = isa(g, 'ballfun');

% Empty check
if isempty( f )
    h = g;
    if ~gIsBallfun
        h = ballfun(h);
    end
    return
end

% Empty check
if isempty( g )
    h = f;
    if ~fIsBallfun
        h = ballfun(h);
    end
    return
end

if (fIsBallfun && gIsBallfun)
    [mf,nf,pf] = size(f);
    [mg,ng,pg] = size(g);
    m = max(mf,mg);
    n = max(nf,ng);
    p = max(pf,pg);
    X = zeros(m,n,p);
    X(1:mf,floor(n/2)+1-floor(nf/2):floor(n/2)+nf-floor(nf/2),floor(p/2)+1-floor(pf/2):floor(p/2)+pf-floor(pf/2)) = f.coeffs;
    X(1:mg,floor(n/2)+1-floor(ng/2):floor(n/2)+ng-floor(ng/2),floor(p/2)+1-floor(pg/2):floor(p/2)+pg-floor(pg/2)) = ...
    X(1:mg,floor(n/2)+1-floor(ng/2):floor(n/2)+ng-floor(ng/2),floor(p/2)+1-floor(pg/2):floor(p/2)+pg-floor(pg/2)) + g.coeffs;
    h = ballfun(X,'coeffs');
elseif (fIsBallfun && isnumeric(g))
    S = size(f);
    X = f.coeffs;
    % Add the constant g
    X(1,floor(S(2)/2)+1,floor(S(3)/2)+1) = X(1,floor(S(2)/2)+1,floor(S(3)/2)+1) + g;
    h = ballfun(X,'coeffs');
elseif (isnumeric(f) && gIsBallfun)
    S = size(g);
    X = g.coeffs;
    % Add the constant f
    X(1,floor(S(2)/2)+1,floor(S(3)/2)+1) = X(1,floor(S(2)/2)+1,floor(S(3)/2)+1) + f;
    h = ballfun(X,'coeffs');
else
    error('BALLFUN:plus:unknown', ...
          ['Undefined function ''plus'' for input arguments of type ' ...
           '%s and %s.'], class(f), class(g));
end
end


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=24;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_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('timecom24','nmpcplot1','impcplot1');
save('feasibility24','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-24\n');
    controller_nmpc_dcbf_multiple24 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple24.obs = obs;
    controller_nmpc_dcbf_multiple24.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple24.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


function [granger, v, n] = ft_connectivity_granger(H, Z, S, varargin)

% FT_CONNECTIVITY_GRANGER computes spectrally resolved granger causality. This
% implementation is loosely based on the code used in Brovelli, et. al., PNAS 101,
% 9849-9854 (2004).
%
% Use as
%   [GRANGER, V, N] = FT_CONNECTIVITY_GRANGER(H, Z, S, ...)
%
% The input data should be
%   H = spectral transfer matrix, Nrpt x Nchan x Nchan x Nfreq (x Ntime),
%       or Nrpt x Nchancmb x Nfreq (x Ntime). Nrpt can be 1.
%   Z = the covariance matrix of the noise, Nrpt x Nchan x Nchan (x Ntime),
%       or Nrpt x Nchancmb (x Ntime).
%   S = the cross-spectral density matrix with the same dimensionality as H.
%
% Additional optional input arguments come as key-value pairs:
%   'dimord'  = required string specifying how to interpret the input data
%               supported values are 'rpt_chan_chan_freq(_time) and
%               'rpt_chan_freq(_time), 'rpt_pos_pos_XXX' and 'rpt_pos_XXX'
%   'method'  = 'granger' (default), or 'instantaneous', or 'total'.
%   'hasjack' = 0 (default) is a boolean specifying whether the input
%               contains leave-one-outs, required for correct variance
%               estimate
%   'powindx' = is a variable determining the exact computation, see below
%
% If the inputdata is such that the channel-pairs are linearly indexed, granger
% causality is computed per quadruplet of consecutive entries, where the convention
% is as follows:
%
%  H(:, (k-1)*4 + 1, :, :, :) -> 'chan1-chan1'
%  H(:, (k-1)*4 + 2, :, :, :) -> 'chan1->chan2'
%  H(:, (k-1)*4 + 3, :, :, :) -> 'chan2->chan1'
%  H(:, (k-1)*4 + 4, :, :, :) -> 'chan2->chan2'
%
% The same holds for the Z and S matrices.
%
% Pairwise block-granger causality can be computed when the inputdata has
% dimensionality Nchan x Nchan. In that case powindx should be specified, as a 1x2
% cell-array indexing the individual channels that go into each 'block'.
%
% See also FT_CONNECTIVITYANALYSIS

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Undocumented option: powindx can be a struct. In that case, blockwise
% conditional granger can be computed.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2009-2017, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

method  = ft_getopt(varargin, 'method',  'granger');
hasjack = ft_getopt(varargin, 'hasjack', 0);
powindx = ft_getopt(varargin, 'powindx', []);
dimord  = ft_getopt(varargin, 'dimord',  []);

%FIXME speed up code and check
siz = size(H);
if numel(siz)==4
  siz(5) = 1;
end
n   = siz(1);
Nc  = siz(2);

outsum = zeros(siz(2:end));
outssq = zeros(siz(2:end));

% crossterms are described by chan_chan_therest
issquare = length(strfind(dimord, 'chan'))==2 || length(strfind(dimord, 'pos'))==2;

switch method
  case 'granger'

    if issquare && isempty(powindx)
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%
      % data are chan_chan_therest
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%

      for kk = 1:n
        for ii = 1:Nc
          for jj = 1:Nc
            if ii ~=jj
              zc     = reshape(Z(kk,jj,jj,:) - Z(kk,ii,jj,:).^2./Z(kk,ii,ii,:),[1 1 1 1 siz(5)]);
              zc     = repmat(zc,[1 1 1 siz(4) 1]);
              numer  = reshape(abs(S(kk,ii,ii,:,:)),[1 1 siz(4:end)]);
              denom  = reshape(abs(S(kk,ii,ii,:,:)-zc.*abs(H(kk,ii,jj,:,:)).^2),[1 1 siz(4:end)]);
              outsum(jj,ii,:,:) = outsum(jj,ii,:,:) + log(numer./denom);
              outssq(jj,ii,:,:) = outssq(jj,ii,:,:) + (log(numer./denom)).^2;
            end
          end
          outsum(ii,ii,:,:) = 0; %self-granger set to zero
        end
      end

    elseif ~issquare && isempty(powindx)
      %%%%%%%%%%%%%%%%%%%%%%%%%%
      %data are linearly indexed
      %%%%%%%%%%%%%%%%%%%%%%%%%%

      for j = 1:n
        for k = 1:Nc
          %FIXME powindx is not used here anymore
          %iauto1  = sum(powindx==powindx(k,1),2)==2;
          %iauto2  = sum(powindx==powindx(k,2),2)==2;
          %icross1 = k;
          %icross2 = sum(powindx==powindx(ones(Nc,1)*k,[2 1]),2)==2;

          % The following is based on hard-coded assumptions: which is fair
          % to do if the order of the labelcmb is according to the output of
          % ft_connectivity_csd2transfer
          if mod(k-1, 4)==0
            continue; % auto granger set to 0
            %iauto1=k;iauto2=k;icross1=k;icross2=k;
          elseif mod(k-1, 4)==1
            iauto1=k+2;iauto2=k-1;icross1=k;icross2=k+1;
          elseif mod(k-1, 4)==2
            iauto1=k-2;iauto2=k+1;icross1=k;icross2=k-1;
          elseif mod(k-1, 4)==3
            continue; % auto granger set to 0
            %iauto1=k;iauto2=k;icross1=k;icross2=k;
          end

          zc      = Z(j,iauto2,:,:) - Z(j,icross1,:,:).^2./Z(j,iauto1,:,:);
          numer   = abs(S(j,iauto1,:,:));
          denom   = abs(S(j,iauto1,:,:)-zc(:,:,ones(1,size(H,3)),:).*abs(H(j,icross1,:,:)).^2);
          outsum(icross2,:,:) = outsum(icross2,:,:) + reshape(log(numer./denom), [1 siz(3:end)]);
          outssq(icross2,:,:) = outssq(icross2,:,:) + reshape((log(numer./denom)).^2, [1 siz(3:end)]);
        end
      end

    elseif issquare && iscell(powindx)
      %%%%%%%%%%%%%%%%%%%
      % blockwise granger
      %%%%%%%%%%%%%%%%%%%

      % H = transfer function nchan x nchan x nfreq
      % Z = noise covariance  nchan x nchan
      % S = crosspectrum      nchan x nchan x nfreq
      % powindx{k} is a list of indices for block k

      nblock = numel(powindx);
      n      = size(H,1);
      nfreq  = size(H,4);

      outsum = zeros(nblock,nblock,nfreq);
      outssq = zeros(nblock,nblock,nfreq);

      for k = 1:nblock
        for m = (k+1):nblock
          indx  = [powindx{k}(:);powindx{m}(:)];
          indx  = cat(1,indx,setdiff((1:size(Z,2))',indx));
          n1    = numel(powindx{k});
          n2    = numel(powindx{m});
          ntot  = numel(indx);
          indx1 = 1:n1;
          indx1_  = (1:ntot)'; indx1_(indx1) = [];
          indx2   = n1+(1:n2);
          indx12_ = (1:ntot)'; indx12_([indx1(:);indx2(:)]) = [];

          for kk = 1:n
            tmpZ = reshape(Z(kk,indx,indx), [ntot ntot]);

            % projection matrix for therest+block2 -> block1
            P1 = [eye(n1)                                zeros(n1,ntot-n1);
              -tmpZ(indx1_,indx1)/tmpZ(indx1,indx1)      eye(ntot-n1)];

            % projection matrix for therest+block1 -> block2
            P2 = [  eye(n1)    -tmpZ(indx1,indx2)/tmpZ(indx2,indx2) zeros(n1,ntot-n1-n2);
                  zeros(n2,n1)   eye(n2) zeros(n2, ntot-n1-n2);
                  zeros(ntot-n1-n2,n1) -tmpZ(indx12_,indx2)/tmpZ(indx2,indx2) eye(ntot-n1-n2)];

            % invert only once
            for jj = 1:nfreq
              % post multiply transfer matrix with the inverse of the projection matrix
              % this is equivalent to time domain pre multiplication with P
              Sj = reshape(S(kk,indx,indx,jj), [ntot ntot]);
              Zj = tmpZ; %(:,:);
              H1 = reshape(H(kk,indx,indx,jj), [ntot ntot])/P1;
              H2 = reshape(H(kk,indx,indx,jj), [ntot ntot])/P2;
              num1 = abs(det(Sj(indx1,indx1))); % numerical round off leads to tiny imaginary components
              num2 = abs(det(Sj(indx2,indx2))); % numerical round off leads to tiny imaginary components
              denom1 = abs(det(H1(indx1,indx1)*Zj(indx1,indx1)*H1(indx1,indx1)'));
              denom2 = abs(det(H2(indx2,indx2)*Zj(indx2,indx2)*H2(indx2,indx2)'));
              %rH1 = real(H1(indx1,indx1));
              %rH2 = real(H2(indx2,indx2));
              %iH1 = imag(H1(indx1,indx1));
              %iH2 = imag(H2(indx2,indx2));
              %h1 = rH1*Zj(indx1,indx1)*rH1' + iH1*Zj(indx1,indx1)*iH1';
              %h2 = rH2*Zj(indx2,indx2)*rH2' + iH2*Zj(indx2,indx2)*iH2';
              %denom1 = abs(det(h1));
              %denom2 = abs(det(h2));

              outsum(m,k,jj) = log( num1./denom1 )    + outsum(m,k,jj);
              outsum(k,m,jj) = log( num2./denom2 )    + outsum(k,m,jj);
              outssq(m,k,jj) = log( num1./denom1 ).^2 + outssq(m,k,jj);
              outssq(k,m,jj) = log( num2./denom2 ).^2 + outssq(k,m,jj);
            end
          end

        end
      end

    elseif ~issquare && isstruct(powindx) && isfield(powindx, 'n')
      %%%%%%%%%%%%%%%%%%%%%%
      %blockwise conditional
      %%%%%%%%%%%%%%%%%%%%%%

      n     = size(H,1);
      ncmb  = size(H,2);
      nfreq = size(H,3);
      ncnd  = size(powindx.cmbindx,1);

      outsum = zeros(ncnd, nfreq);
      outssq = zeros(ncnd, nfreq);
      for k = 1:n
        tmpS = reshape(S, [ncmb nfreq]);
        tmpH = reshape(H, [ncmb nfreq]);
        tmpZ = reshape(Z, [ncmb 1]);
        tmp  = blockwise_conditionalgranger(tmpS,tmpH,tmpZ,powindx.cmbindx,powindx.n);

        outsum = outsum + tmp;
        outssq = outssq + tmp.^2;
      end

    elseif ~issquare && isstruct(powindx)
      %%%%%%%%%%%%%%%%%%%%%%
      %triplet conditional
      %%%%%%%%%%%%%%%%%%%%%%

      % decode from the powindx struct which rows in the data correspond with
      % the triplets, and which correspond with the duplets
      ublockindx = unique(powindx.blockindx);
      nperblock  = zeros(size(ublockindx));
      for k = 1:numel(ublockindx)
        nperblock(k,1) = sum(powindx.blockindx==ublockindx(k));
      end
      if ~all(ismember(nperblock,[4 9]))
        error('the data should be a mixture of trivariate and bivariate decompositions');
      end
      indx_triplets = ismember(powindx.blockindx, ublockindx(nperblock==9)); ntriplets = sum(indx_triplets)./9;
      indx_duplets  = ismember(powindx.blockindx, ublockindx(nperblock==4)); nduplets  = sum(indx_duplets)./4;

      % this assumes well-behaved powindx.cmbindx
      cmbindx2 = reshape(powindx.cmbindx(indx_duplets, 1), 2, [])';
      cmbindx3 = reshape(powindx.cmbindx(indx_triplets,1), 3, [])';

      cmbindx2 = cmbindx2(1:2:end,:);
      cmbindx3 = cmbindx3(1:3:end,:);

      cmbindx  = powindx.outindx;

      n   = size(H,1);
      siz = size(H);

      outsum = zeros(size(cmbindx,1), size(H,3));
      outssq = outsum;

      % call the low-level function
      for k = 1:n
        H3 = reshape(H(k,indx_triplets,:,:), [3 3 ntriplets siz(3:end)]);
        Z3 = reshape(Z(k,indx_triplets),     [3 3 ntriplets]);

        H2 = reshape(H(k,indx_duplets,:,:), [2 2 nduplets siz(3:end)]);
        Z2 = reshape(Z(k,indx_duplets),     [2 2 nduplets]);

        tmp  = triplet_conditionalgranger(H3,Z3,cmbindx3,H2,Z2,cmbindx2,cmbindx);

        outsum = outsum + tmp;
        outssq = outssq + tmp.^2;
      end

    end

  case 'instantaneous'

    if issquare && isempty(powindx)
      % data are chan_chan_therest
      for kk = 1:n
        for ii = 1:Nc
          for jj = 1:Nc
            if ii ~=jj
              zc1    = reshape(Z(kk,jj,jj,:) - Z(kk,ii,jj,:).^2./Z(kk,ii,ii,:),[1 1 1 1 siz(5)]);
              zc2    = reshape(Z(kk,ii,ii,:) - Z(kk,jj,ii,:).^2./Z(kk,jj,jj,:),[1 1 1 1 siz(5)]);
              zc1    = repmat(zc1,[1 1 1 siz(4) 1]);
              zc2    = repmat(zc2,[1 1 1 siz(4) 1]);
              term1  = abs(S(kk,ii,ii,:,:)) - zc1.*abs(H(kk,ii,jj,:,:)).^2;
              term2  = abs(S(kk,jj,jj,:,:)) - zc2.*abs(H(kk,jj,ii,:,:)).^2;
              numer  = term1.*term2;
              denom  = abs(S(kk,ii,ii,:,:).*S(kk,jj,jj,:,:) - S(kk,ii,jj,:,:).*S(kk,jj,ii,:,:));
              outsum(jj,ii,:,:) = outsum(jj,ii,:,:) + reshape(log(numer./denom),  [1 1 siz(4:end)]);
              outssq(jj,ii,:,:) = outssq(jj,ii,:,:) + reshape((log(numer./denom)).^2, [1 1 siz(4:end)]);
            end
          end
          outsum(ii,ii,:,:) = 0; %self-granger set to zero
        end
      end
    elseif ~issquare && isempty(powindx)
      % data are linearly indexed
      for j = 1:n
        for k = 1:Nc
          %iauto1  = sum(powindx==powindx(k,1),2)==2;
          %iauto2  = sum(powindx==powindx(k,2),2)==2;
          %icross1 = k;
          %icross2 = sum(powindx==powindx(ones(Nc,1)*k,[2 1]),2)==2;
          if mod(k-1, 4)==0
            continue; % auto granger set to 0
            %iauto1=k;iauto2=k;icross1=k;icross2=k;
          elseif mod(k-1, 4)==1
            iauto1=k+2;iauto2=k-1;icross1=k;icross2=k+1;
          elseif mod(k-1, 4)==2
            iauto1=k-2;iauto2=k+1;icross1=k;icross2=k-1;
          elseif mod(k-1, 4)==3
            continue; % auto granger set to 0
            %iauto1=k;iauto2=k;icross1=k;icross2=k;
          end

          zc1     = Z(j,iauto1,:, :) - Z(j,icross2,:, :).^2./Z(j,iauto2,:, :);
          zc2     = Z(j,iauto2,:, :) - Z(j,icross1,:, :).^2./Z(j,iauto1,:, :);
          term1   = abs(S(j,iauto2,:,:)) - zc1(:,:,ones(1,size(H,3)),:).*abs(H(j,icross2,:,:)).^2;
          term2   = abs(S(j,iauto1,:,:)) - zc2(:,:,ones(1,size(H,3)),:).*abs(H(j,icross1,:,:)).^2;
          numer   = term1.*term2;
          denom   = abs(S(j,iauto1,:,:).*S(j,iauto2,:,:) - S(j,icross1,:,:).*S(j,icross2,:,:));

          outsum(icross2,:,:) = outsum(icross2,:,:) + reshape(log(numer./denom), [1 siz(3:end)]);
          outssq(icross2,:,:) = outssq(icross2,:,:) + reshape((log(numer./denom)).^2, [1 siz(3:end)]);
        end
      end
    elseif issquare && iscell(powindx)
      % blockwise granger
      % H = transfer function nchan x nchan x nfreq
      % Z = noise covariance  nchan x nchan
      % S = crosspectrum      nchan x nchan x nfreq
      % powindx{1} is a list of indices for block1
      % powindx{2} is a list of indices for block2
      ft_error('instantaneous causality is not implemented for blockwise factorizations');
    elseif isstruct(powindx)
      %blockwise conditional
      ft_error('blockwise conditional instantaneous causality is not implemented');
    else
      ft_error('not implemented');
    end

  case 'total'

    if issquare && isempty(powindx)
      % data are chan_chan_therest
      for kk = 1:n
        for ii = 1:Nc
          for jj = 1:Nc
            if ii ~=jj
              numer  = abs(S(kk,ii,ii,:,:).*S(kk,jj,jj,:,:));
              denom  = abs(S(kk,ii,ii,:,:).*S(kk,jj,jj,:,:) - S(kk,ii,jj,:,:).*S(kk,jj,ii,:,:));
              outsum(jj,ii,:,:) = outsum(jj,ii,:,:) + reshape(log(numer./denom), [1 1 siz(4:end)]);
              outssq(jj,ii,:,:) = outssq(jj,ii,:,:) + reshape((log(numer./denom)).^2, [1 1 siz(4:end)]);
            end
          end
          outsum(ii,ii,:,:) = 0; %self-granger set to zero
        end
      end
    elseif ~issquare && isempty(powindx)
      % data are linearly indexed
      for j = 1:n
        for k = 1:Nc
          %iauto1  = sum(powindx==powindx(k,1),2)==2;
          %iauto2  = sum(powindx==powindx(k,2),2)==2;
          %icross1 = k;
          %icross2 = sum(powindx==powindx(ones(Nc,1)*k,[2 1]),2)==2;
          if mod(k-1, 4)==0
            continue; % auto granger set to 0
            %iauto1=k;iauto2=k;icross1=k;icross2=k;
          elseif mod(k-1, 4)==1
            iauto1=k+2;iauto2=k-1;icross1=k;icross2=k+1;
          elseif mod(k-1, 4)==2
            iauto1=k-2;iauto2=k+1;icross1=k;icross2=k-1;
          elseif mod(k-1, 4)==3
            continue; % auto granger set to 0
            %iauto1=k;iauto2=k;icross1=k;icross2=k;
          end

          numer   = abs(S(j,iauto1,:,:).*S(j,iauto2,:,:));
          denom   = abs(S(j,iauto1,:,:).*S(j,iauto2,:,:) - S(j,icross1,:,:).*S(j,icross2,:,:));
          outsum(icross2,:,:) = outsum(icross2,:,:) + reshape(log(numer./denom), [1 siz(3:end)]);
          outssq(icross2,:,:) = outssq(icross2,:,:) + reshape((log(numer./denom)).^2, [1 siz(3:end)]);
        end
      end
    elseif issquare && iscell(powindx)
      % blockwise total interdependence

      % S = crosspectrum      nchan x nchan x nfreq
      % powindx{k} is a list of indices for block k

      nblock = numel(powindx);
      n      = size(S,1);
      nfreq  = size(S,4);

      outsum = zeros(nblock,nblock,nfreq);
      outssq = zeros(nblock,nblock,nfreq);

      for k = 1:nblock
        for m = (k+1):nblock
          indx  = [powindx{k}(:);powindx{m}(:)];
          n1    = numel(powindx{k});
          n2    = numel(powindx{m});
          ntot  = n1+n2;
          indx1 = 1:n1;
          indx2 = (n1+1):ntot;

          for kk = 1:n
            for jj = 1:nfreq
              Sj = reshape(S(kk,indx,indx,jj), [ntot ntot]);
              num1 = abs(det(Sj(indx1,indx1))); % numerical round off leads to tiny imaginary components
              num2 = abs(det(Sj(indx2,indx2))); % numerical round off leads to tiny imaginary components
              num  = num1.*num2;
              denom = abs(det(Sj));

              outsum(m,k,jj) = log( num./denom )    + outsum(m,k,jj);
              outsum(k,m,jj) = log( num./denom )    + outsum(k,m,jj);
              outssq(m,k,jj) = log( num./denom ).^2 + outssq(m,k,jj);
              outssq(k,m,jj) = log( num./denom ).^2 + outssq(k,m,jj);
            end
          end

        end
      end

    elseif issquare && isstruct(powindx)
      %blockwise conditional
      ft_error('blockwise conditional total interdependence is not implemented');
    end

  case 'iis'
    ft_warning('THIS IS EXPERIMENTAL CODE, USE AT YOUR OWN RISK!');
    % this is experimental
    if ~issquare && isempty(powindx)
      A = transfer2coeffs(shiftdim(H),(0:size(H,3)-1));
      ncmb = size(A,1)./4;
      iis = coeffs2iis(reshape(A,[2 2 ncmb size(A,2)]),reshape(Z,[2 2 ncmb]));
      iis = repmat(iis(:),[1 4])';
      outsum = iis(:);
      outssq = nan(size(outsum));
    else
      ft_error('iis can only be computed when the input contains sets of bivariate factorizations');
    end

  otherwise
    ft_error('unsupported output requested');
end

granger = outsum./n;
if n>1
  if hasjack
    bias = (n-1).^2;
  else
    bias = 1;
  end
  v = bias*(outssq - (outsum.^2)./n)./(n - 1);
else
  v = [];
end


function [g1, g2, g3] = sdlfmaXsdlfmaKernGradientBlockIGJ(lfmKern1, lfmKern2, ...
    t1, t2,  i, j, generalConst, generalConstGrad, covGrad, g1Mean, ...
    g2Mean, gsp1Mean, gsp2Mean, typeMeanParam, typeMeanSwitching, ...
    typeKernParam1, typeKernParam2, typeKernSwitching1, typeKernSwitching2)

% SDLFMAXSDLFMAKERNGRADIENTBLOCKIGJ
% FORMAT
% DESC computes the gradients of the parameters for system 1 and system 2
% when i is greater than j. It assummes both systems represent
% accelerations.
% ARG lfmKern1 : structure containing parameters for the system 1
% ARG lfmKern2 : structure containing parameters for the system 2
% ARG t1 : times at which the system 1 is evaluated
% ARG t2 : times at which the system 2 is evaluated
% ARG covGrad : partial derivatives of the objective function wrt portion
% of the corresponding kernel matrix
% ARG g1Mean : gradients of the parameter of the system 1 obtained from the
% part of the function that uses the funcitons accommpanying the initial
% conditions.
% ARG g2Mean : gradients of the parameter of the system 2 obtained from the
% part of the function that uses the funcitons accommpanying the initial
% conditions.
% ARG gsp1Mean : gradient of the switching point of the system 1 obtained
% from the part of the function that uses the funcitons accommpanying the
% initial conditions.
% ARG gsp2Mean : gradient of the switching point of the system 2 obtained
% from the part of the function that uses the funcitons accommpanying the
% initial conditions.
% ARG typeMeanParam : specify the mean functions used to compute this part
% of the kernel
% ARG typeMeanSwitching : specify the functions used to compute the
% gradients of the swicthing points in part thst uses mean functions
% ARG typeKernParam1 : specify the first kernel function used to compute
% this part of the kernel
% ARG typeKernParam2 : specify the second kernel function used to compute
% this part of the kernel
% ARG typeKernSwitching1 : specify the functions used to compute the
% gradients of the swicthing points in both sides of the kernel function 1
% ARG typeKernSwitching2 : specify the functions used to compute the
% gradients of the swicthing points in both sides of the kernel function 2
% RETURN g1 : gradients of parameters for the system 1
% RETURN g2 : gradients of parameters for the system 2
% RETURN g3 : gradients of switching points
%
% COPYRIGHT : Mauricio A. Alvarez, 2010.

% KERN

if nargin < 14
    typeMeanParam = 'sdlfm';
    typeMeanSwitching = 'sdlfmv';
    typeKernParam1 = 'lfmXlfm';
    typeKernParam2 = 'lfmvXlfm';
    typeKernSwitching1= {'lfmvXlfm', 'lfmvXlfm'};
    typeKernSwitching2 ={'lfmaXlfm', 'lfmvXlfmv'};
end

g3 = [];

fhandleMeanParam = str2func([typeMeanParam 'MeanCompute']);
fhandleMeanGradParam = str2func([typeMeanParam 'MeanGradient']);
fhandleMeanGradSwitching = str2func([typeMeanSwitching 'MeanCompute']);
fhandleKernPosPos = str2func([typeKernParam1 'KernCompute']);
fhandleKernGradPosPos = str2func([typeKernParam1 'KernGradient']);
fhandleKernGradSwitchingPosPos1 = str2func([typeKernSwitching1{1} 'KernCompute']);
fhandleKernGradSwitchingPosPos2 = str2func([typeKernSwitching1{2} 'KernCompute']);
fhandleKernVelPos = str2func([typeKernParam2 'KernCompute']);
fhandleKernGradVelPos = str2func([typeKernParam2 'KernGradient']);
fhandleKernGradSwitchingVelPos1 = str2func([typeKernSwitching2{1} 'KernCompute']);
fhandleKernGradSwitchingVelPos2 = str2func([typeKernSwitching2{2} 'KernCompute']);

c1 = fhandleMeanParam(lfmKern1(1), t1, 'Pos');
e1 = fhandleMeanParam(lfmKern1(1), t1, 'Vel');

if isempty(generalConst{i,j})
    coeffPosPos = c1;
    coeffVelPos = e1;
else
    coeffPosPos = generalConst{i,j}(1,1)*c1 + generalConst{i,j}(2,1)*e1;
    coeffVelPos = generalConst{i,j}(1,2)*c1 + generalConst{i,j}(2,2)*e1;
end
g1Kern = zeros(length(lfmKern1), 5);
g2Kern = zeros(length(lfmKern1), 5);
PosPos = zeros(1,length(t2));
VelPos = zeros(1,length(t2));

for k=1:length(lfmKern1)
    PosPos = PosPos + fhandleKernPosPos(lfmKern2(k), lfmKern1(k), t2, ...
        lfmKern2(k).limit).';
    [g2KLocal, g1KLocal] = fhandleKernGradPosPos(lfmKern2(k), lfmKern1(k), ...
        t2, lfmKern2(k).limit, covGrad.', coeffPosPos.');
    g1Kern(k,:) = g1Kern(k, :) + g1KLocal;
    g2Kern(k,:) = g2Kern(k, :) + g2KLocal;
    VelPos = VelPos + fhandleKernVelPos(lfmKern2(k), lfmKern1(k), ...
        t2, lfmKern2(k).limit).';
    [g2KLocal, g1KLocal] = fhandleKernGradVelPos(lfmKern2(k), lfmKern1(k), ...
        t2, lfmKern2(k).limit, covGrad.', coeffVelPos.');
    g1Kern(k,:) = g1Kern(k, :) + g1KLocal;
    g2Kern(k,:) = g2Kern(k, :) + g2KLocal;
end
[gcAlpha, gcOmega] = fhandleMeanGradParam(lfmKern1(1), t1, 'Pos');
[geAlpha, geOmega] = fhandleMeanGradParam(lfmKern1(1), t1, 'Vel');
[gradAlpha, gradOmega] = getLocalGradAlphaOmega(lfmKern1);
g1Pos = fhandleMeanGradSwitching(lfmKern1(1), t1, 'Pos');
h1Vel = fhandleMeanGradSwitching(lfmKern1(1), t1, 'Vel');
gsp1Kern = zeros(1, length(lfmKern1));
gsp2Kern = zeros(1, length(lfmKern1));
% This means that are only two switching points involved, t1
% and t0 appear in k, like k_{ff}(t1-t0,t'-t0) and
% k_{mf}(t1-t0,t'-t0). The other points appear in c1(t - t1)
% and e1(t - t1)
for k=1:length(lfmKern1)
    temp = fhandleKernGradSwitchingPosPos1(lfmKern2(k), lfmKern1(k), t2, lfmKern2(k).limit).';
    gsp2Kern(k) = gsp2Kern(k) - sum(sum((coeffPosPos*temp).*covGrad));
    gsp1Kern(k) = gsp1Kern(k) + sum(sum((coeffPosPos*temp).*covGrad));
    temp = fhandleKernGradSwitchingPosPos2(lfmKern2(k), lfmKern1(k), t2, lfmKern2(k).limit).';
    gsp2Kern(k) = gsp2Kern(k) - sum(sum((coeffPosPos*temp).*covGrad));
    temp = fhandleKernGradSwitchingVelPos1(lfmKern1(k), lfmKern2(k), lfmKern2(k).limit, t2);
    gsp2Kern(k) = gsp2Kern(k) - sum(sum((coeffVelPos*temp).*covGrad));
    gsp1Kern(k) = gsp1Kern(k) + sum(sum((coeffVelPos*temp).*covGrad));
    temp = fhandleKernGradSwitchingVelPos2(lfmKern2(k), lfmKern1(k), t2, lfmKern2(k).limit).';
    gsp2Kern(k) = gsp2Kern(k) - sum(sum((coeffVelPos*temp).*covGrad));
end
if isempty(generalConst{i,j})
    matGradAlpha = gcAlpha*PosPos + geAlpha*VelPos;
    matGradOmega = gcOmega*PosPos + geOmega*VelPos;
    gCoeff = gradAlpha*sum(sum(matGradAlpha.*covGrad)) + ...
        gradOmega*(sum(sum(matGradOmega.*covGrad)));
    matGradSp1 = g1Pos*PosPos + h1Vel*VelPos;
    gsp1 = - sum(sum(matGradSp1.*covGrad));
    g3(i) = gsp1Mean + sum(gsp1Kern) + gsp1; % switching point 1
    g3(j) = gsp2Mean + sum(gsp2Kern);        % switching point 2
else
    constGradAlpha = generalConstGrad{1}{i,j};
    constGradOmega = generalConstGrad{2}{i,j};
    constVal = generalConst{i,j};
    % Derivative wrt parameters
    matGradAlpha = (constVal(1,1)*gcAlpha + constGradAlpha(1,1)*c1 ...
        + constVal(2,1)*geAlpha + constGradAlpha(2,1)*e1)*PosPos ...
        + (constVal(1,2)*gcAlpha + constGradAlpha(1,2)*c1 ...
        + constVal(2,2)*geAlpha + constGradAlpha(2,2)*e1)*VelPos;
    matGradOmega = (constVal(1,1)*gcOmega + constGradOmega(1,1)*c1 ...
        + constVal(2,1)*geOmega + constGradOmega(2,1)*e1)*PosPos ...
        + (constVal(1,2)*gcOmega + constGradOmega(1,2)*c1 ...
        + constVal(2,2)*geOmega + constGradOmega(2,2)*e1)*VelPos;
    gCoeff = gradAlpha*sum(sum(matGradAlpha.*covGrad)) + ...
        gradOmega*(sum(sum(matGradOmega.*covGrad)));
    % Derivative wrt switching points
    % Firts, notice that gsp1Kern doesn't correspond to the switching point
    % of the interval i (or say 1). It's just the derivative of the inner
    % switching point that lead to the actual switching point for the
    % interval 1. So we rename it first.
    gspInt = sum(gsp1Kern);
    % Compute the derivative of the swicthing point i, not appearing in the
    % constant
    matGradSp1 = (constVal(1,1)*g1Pos + constVal(2,1)*h1Vel)*PosPos + ...
        (constVal(1,2)*g1Pos + constVal(2,2)*h1Vel)*VelPos;
    gsp1 = - sum(sum(matGradSp1.*covGrad));
    % Compute the derivatives for all the switching points apearing in the
    % constant
    constGradSPoint = generalConstGrad{3}{i,j};
    numberSP = size(constGradSPoint,2);
    gspInBetween = zeros(1, numberSP);
    for k=1:numberSP
        temp = (constGradSPoint(1,k)*c1 + constGradSPoint(3,k)*e1)*PosPos + ...
            (constGradSPoint(2,k)*c1 + constGradSPoint(4,k)*e1)*VelPos;
        gspInBetween(k) = sum(sum(temp.*covGrad));
    end
    % Assign derivatives wrt all other switching points, with a correction
    % for the derivative of the innermost switching point in the constant
    gspInBetween(end) =  gspInBetween(end) + gspInt;
    g3(i) = gsp1Mean + gsp1 + gspInBetween(1);
    g3(j) = gsp2Mean + sum(gsp2Kern);
    g3(j+1:i-1) = fliplr(gspInBetween(2:end));
end
% Assign derivatives wrt first system
g1{1} = g1Mean + sum(g1Kern(:,1:3), 1) + gCoeff; % mass 1, spring 1, damper 1
g1{2} = g1Kern(:,4).';                            % inverse widths
g1{3} = g1Kern(:,5).';                            % seinsitivities 1
% Assign derivatives wrt second system
g2{1} = g2Mean + sum(g2Kern(:,1:3), 1); % mass 2, spring 2, damper 2
g2{2} = g2Kern(:,4).';                   % inverse widths
g2{3} = g2Kern(:,5).';


function [cnt1, cnt2] = elec1020_follow(pnt, dhk, v1, v2, v3, feedback)

% ELEC1020_FOLLOW

% Copyright (C) 2003, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.

if nargin<6
  feedback = false;
end

% compute the distribution of edge lengths
edge = [
  pnt(dhk(:,1),:) - pnt(dhk(:,2),:)
  pnt(dhk(:,2),:) - pnt(dhk(:,3),:)
  pnt(dhk(:,3),:) - pnt(dhk(:,1),:)
  ];
edgelength = sqrt(sum(edge.^2,2));

% the tolerance depends on the edge length
tolerance       = min(edgelength)*1e-6;
tolerance_limit = min(edgelength)*1e-3;

npnt = size(pnt,1);
ndhk = size(dhk,1);

pside = nan(1,npnt);
for i=1:npnt
  % determine on which side of the plane each vertex lies
  pside(i) = ptriside(v1, v2, v3, pnt(i,:), tolerance);
end

% dcut = zeros(ndhk,1);
% for i=1:ndhk
%   % find the triangles that are intersected by the plane
%   if sum(pside(dhk(i,:))==0)==2
%     dcut(i) = 1;
%   elseif sum(pside(dhk(i,:))==0)==1 & sum(pside(dhk(i,:))==1)==1 & sum(pside(dhk(i,:))==-1)==1
%     dcut(i) = 1;
%   elseif sum(pside(dhk(i,:))==1)==2 & sum(pside(dhk(i,:))==-1)==1
%     dcut(i) = 1;
%   elseif sum(pside(dhk(i,:))==1)==1 & sum(pside(dhk(i,:))==-1)==2
%     dcut(i) = 1;
%   end
% end
tmp  = pside(dhk);
dcut = true(ndhk,1);
dcut(all(tmp== 1,2)) = false;
dcut(all(tmp==-1,2)) = false;

% continue working with only the intersecting triangles
dhk = dhk(dcut,:);
ndhk = size(dhk,1);

% for each triangle determine the neighbouring triangles
neighb = zeros(ndhk,ndhk);
for i=1:ndhk
  for j=(i+1):ndhk
    if dhk(i,1)==dhk(j,1)
      neighb(i,j) = 1;
    elseif dhk(i,1)==dhk(j,2)
      neighb(i,j) = 1;
    elseif dhk(i,1)==dhk(j,3)
      neighb(i,j) = 1;
    elseif dhk(i,2)==dhk(j,1)
      neighb(i,j) = 1;
    elseif dhk(i,2)==dhk(j,2)
      neighb(i,j) = 1;
    elseif dhk(i,2)==dhk(j,3)
      neighb(i,j) = 1;
    elseif dhk(i,3)==dhk(j,1)
      neighb(i,j) = 1;
    elseif dhk(i,3)==dhk(j,2)
      neighb(i,j) = 1;
    elseif dhk(i,3)==dhk(j,3)
      neighb(i,j) = 1;
    else
      neighb(i,j) = 0;
    end
    neighb(j,i) = neighb(i,j);
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% make a contour from v1 via v2 to v3

% find the nearest triangle on which point v1 projects
v1_dist = inf;
for i=1:ndhk
  % shift a fraction towards v2 to avoid starting in the vertex of a wrong triangle
  [proj, dist] = ptriproj(pnt(dhk(i,1),:), pnt(dhk(i,2),:), pnt(dhk(i,3),:), v1+tolerance*(v2-v1)/norm(v2-v1), 1);
  if dist<v1_dist
    v1_dist = dist;
    v1_indx = i;
    v1_proj = ptriproj(pnt(dhk(i,1),:), pnt(dhk(i,2),:), pnt(dhk(i,3),:), v1, 1);
  end
end

% find the nearest triangle on which point v3 projects
v3_dist = inf;
for i=1:ndhk
  [proj, dist] = ptriproj(pnt(dhk(i,1),:), pnt(dhk(i,2),:), pnt(dhk(i,3),:), v3, 1);
  if dist<v3_dist
    v3_dist = dist;
    v3_indx = i;
    v3_proj = proj;
  end
end

% intersect the triangle containing the projection of v1 with the plane
[l1, l2] = tritrisect(v1, v2, v3, pnt(dhk(v1_indx,1),:), pnt(dhk(v1_indx,2),:), pnt(dhk(v1_indx,3),:));
if pntdist(l1,v2) < pntdist(l2,v2)
  % remember the projection point and l1 as the first line segment
  cnt1 = [v1_proj];
  cnt2 = [l1];
  prev_proj = l1;
  prev_indx = v1_indx;
else
  % remember the projection point and l2 as the first line segment
  cnt1 = [v1_proj];
  cnt2 = [l2];
  prev_proj = l2;
  prev_indx = v1_indx;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

while(1)

  for i=find(neighb(prev_indx,:))

    ncnt = size(cnt1,1);
    c1 = pnt(dhk(i,1),:);
    c2 = pnt(dhk(i,2),:);
    c3 = pnt(dhk(i,3),:);
%
%     if feedback
%       ft_plot_mesh(struct('pos', pnt, 'tri', dhk(i,:)), 'edgecolor', 'b', 'facecolor', 'none')
%       ft_plot_mesh(c1, 'vertexsize', 30, 'vertexcolor', 'r')
%       ft_plot_mesh(c2, 'vertexsize', 30, 'vertexcolor', 'r')
%       ft_plot_mesh(c3, 'vertexsize', 30, 'vertexcolor', 'r')
%       ft_plot_mesh(prev_proj, 'vertexsize', 30, 'vertexcolor', 'g')
%       drawnow
%     end

    [proj, dist] = ptriproj(c1, c2, c3, prev_proj, 1);

    if dist<tolerance
      [l1, l2] = tritrisect(v1, v2, v3, c1, c2, c3);

      if pntdist(l1, cnt1(ncnt,:))<tolerance && pntdist(l2, cnt2(ncnt,:))<tolerance
        continue
      elseif pntdist(l1, cnt2(ncnt,:))<tolerance && pntdist(l2, cnt1(ncnt,:))<tolerance
        continue
      end

      if pntdist(l1, prev_proj) < pntdist(l2, prev_proj)
        cnt1 = [cnt1; l1];
        cnt2 = [cnt2; l2];
        prev_proj = l2;
        prev_indx = i;
        break
      else
        cnt1 = [cnt1; l2];
        cnt2 = [cnt2; l1];
        prev_proj = l1;
        prev_indx = i;
        break
      end
    end

  end

  % stop if no new segment was added
  if ~(size(cnt1,1)>ncnt)
    tolerance = 2*tolerance;
    if tolerance>=tolerance_limit
      ft_warning('premature end of contour')
      break
    else
      ft_warning('increasing tolerance');
    end
  end

  % stop if we arrive on the triangle with the endpoint
  if pntdist(prev_proj, v3_proj)<tolerance
    % replace the current endpoint with the projection of v3
    cnt2(size(cnt2,1),:) = v3_proj;
    break
  end

  % stop if we arrive on the triangle with the endpoint
  if prev_indx==v3_indx
    % replace the current endpoint with the projection of v3
    cnt2(size(cnt2,1),:) = v3_proj;
    break
  end

end % while

if feedback
  X = [cnt1(:,1) cnt2(:,1)];
  Y = [cnt1(:,2) cnt2(:,2)];
  Z = [cnt1(:,3) cnt2(:,3)];
  line(X, Y, Z, 'color', 'k')
  drawnow
end

end % function


function [flow_out, time_shifted_points_out] = em1_flow(...
    events, ...
    feature_pos, ...
    flow_init, ...
    params, ...
    fig)
%EM1_FLOW Estimates optical flow for an event feature.
%
% EM1_FLOW estimates the optical flow of a set of events % combined to form
% a 'feature'. This EM step runs from the events alone, as in:
% Alex Zihao Zhu, Nikolay Atanasov and Kostas Daniilidis.
% "Event-based Feature Tracking with Probabilistic Data Associations", ICRA 2017.
%
% Syntax:  [flow_out, time_shifted_points_out] = EM1_FLOW(...
%            events, ...
%            feature_pos, ...
%            flow_init, ...
%            params, ...
%            fig)
%
% Inputs:
%    events               - 4xN, each column is (x,y,t,p).
%    feature_pos          - 2x1, pixel position of the feature.
%    flow_init            - 2x1, initialization for the flow.
%    params               - parameters, defined in get_params().
%    fig                  - figure handle for plotting.
%
% Outputs:
%    flow_out                - 2x1, estimated flow.
%    time_shifted_points_out - 2xN, input events in the feature window
%                              shifted by the flow [x;y] + dt * flow.
%
% See also GET_PARAMS
%
% Author: Alex Zihao Zhu, University of Pennsylvania
% Email: alexzhu(at)seas.upenn.edu
% Copyright 2018 University of Pennsylvania
% Alex Zihao Zhu, Nikolay Atanasov, Kostas Daniilidis
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS, CONTRIBUTORS, AND THE
% TRUSTEES OF THE UNIVERSITY OF PENNSYLVANIA "AS IS" AND ANY EXPRESS OR
% IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
% OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT OWNER, CONTRIBUTORS OR THE TRUSTEES OF
% THE UNIVERSITY OF PENNSYLVANIA BE LIABLE FOR ANY DIRECT, INDIRECT,
% INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
% NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
% THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
% (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
% THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

%% Initialization
flow = flow_init;
% Estimated flow
flow_out = [nan; nan];
% Final time shifted events.
time_shifted_points_out = [];
% Store change in flow norm at each iteration for plotting.
delta_flows = zeros(params.em1_params.max_iters, 1);
num_iter = 0;
% Plot handle.
scatter_plot_handle = [];
% Boolean array of size equal to the number of events. Is true if an event
% is in the spatial window of the feature, and thus needs to be processed.
event_window = [];
n_in_window = 0;

target_time = events(3, 1);

% Translate the events so that they are centered at the last position of
% the feature.
centered_events = events;
centered_events(1:2, :) = bsxfun(@minus, events(1:2, :), feature_pos);
prev_flow = flow;

%% Main EM Loop
while true
    if num_iter > params.em1_params.max_iters
        return
    end
    % Shift the centered events in time by the current estimate of the
    % flow.
    time_shifted_points = centered_events(1:2,:) + ...
        bsxfun(@times, flow,(target_time - centered_events(3,:)));

    % Only compute the event window once.
    if isempty(event_window)
        event_window = time_shifted_points(1, :) >= -params.window_size/2 & ...
            time_shifted_points(2, :) >= -params.window_size/2 & ...
            time_shifted_points(1, :) <= params.window_size/2 & ...
            time_shifted_points(2, :) <= params.window_size/2;
        n_in_window = sum(event_window);

        % Don't bother optimizing if there aren't enough events.
        if n_in_window < params.min_events_for_em
            return
        end

        centered_events = centered_events(:, event_window);
        time_shifted_points = time_shifted_points(:, event_window);
        %         params.max_distance = n_in_window / 400;
    end

    % Scale the events so that the computed distances are equal to
    % (x1-x2)/(2*sigma^2). Saves us having to divide all the
    % correspondences later.
    normalized_events = time_shifted_points' / (sqrt(2) * params.em1_params.sigma);

    % The KD tree allows us to compute distances between neighboring
    % events, while also performing outlier rejection. Unfortunately, as
    % the time shifted events change every iteration, it must also be
    % reconstructed at every iteration.
    kdtree = KDTreeSearcher(...
        normalized_events, ...
        'Distance', ...
        'euclidean');

    % Threshold distances by the Malhalanobis distance.
    [neighbors_cell, distances_cell] = rangesearch(...
        kdtree, ...
        normalized_events, ...
        params.em1_params.max_distance);

    distancesstacked = cell2mat(cellfun(...
        @transpose, distances_cell, 'UniformOutput', false))';

    % Can't solve for flow without any correspondences :(
    if isempty(distancesstacked)
        return
    end

    % Number of neighbors for each event.
    % NOTE: 'length' is not the same as @length
    num_neighbors_per_event = cellfun('length', neighbors_cell);

    neighbor_inds = cell2mat(cellfun(...
        @transpose, neighbors_cell, 'UniformOutput', false))';

    event_inds = repelem(1:n_in_window, num_neighbors_per_event);

    % The event to neighbor graph is undirected, so no need to double count
    % the event-neighbor correspondences.
    valid_correspondences = neighbor_inds > event_inds;

    neighbor_inds = neighbor_inds(valid_correspondences);
    event_inds = event_inds(valid_correspondences);
    distancesstacked = distancesstacked(valid_correspondences);

    % Distances are already scaled by the variance. Note that the original
    % equation in the paper is the sum of the product of two weights. Here
    % we simplify it with a single weight for speed.
    weights = exp(-distancesstacked);

    %% Simultaneously minimize over flow and translation.
    neighbor_events = centered_events(:, neighbor_inds);
    original_events = centered_events(:, event_inds);

    % These are the X and D matrices specified in the original paper,
    % except the weighting is handled by multiplying the D with the full
    % weight.
    X = original_events(1:2, :) - neighbor_events(1:2, :);
    D = original_events(3, :)-neighbor_events(3, :);
    weighted_D = bsxfun(@times, D, weights);
    DDT = weighted_D*D';
    XDT = X*weighted_D';

    flow = XDT/DDT;

    %% Calculate change in flow, plot debug information.
    if (norm(flow - prev_flow) < params.em1_params.min_err)
        break;
    end

    delta_flows(num_iter+1) = norm(flow - prev_flow);
    prev_flow = flow;

    if params.debug
        set(0, 'CurrentFigure', fig)
        subplot(2,1,1)
        plot(delta_flows(delta_flows > 0),'b')
        title('EM1 change in flow (convergence criterion)')
        xlim([0 params.em1_params.max_iters])

        subplot(2,1,2)
        if (~isempty(scatter_plot_handle))
            delete(scatter_plot_handle)
        end
        scatter_plot_handle = scatter(...
            time_shifted_points(1, :), ...
            time_shifted_points(2, :), 'r.');

        axis equal
        axis([-params.window_size/2-5 params.window_size/2+5 -params.window_size/2-5 params.window_size/2+5])
        axis ij
        title('EM1 Time shifted events')
        pause(0.01)
    end

    num_iter = num_iter + 1;
end

if params.debug
    pause(0.5)
end

flow_out = flow;

% Calculate the final shifted events to be used in EM2 later.
dt = events(3, end) - events(3, 1);
centered_events = events;
centered_events(1:2, :) = bsxfun(@minus, events(1:2, :), feature_pos + flow * dt);
time_shifted_points = centered_events(1:2, :) + ...
    bsxfun(@times, flow,(events(3, end)-centered_events(3, :)));

% Make the window a little bigger.
window_size = round(params.window_size * 1.5);

event_window = time_shifted_points(1, :) >= -window_size/2 & ...
    time_shifted_points(2, :) >= -window_size/2 & ...
    time_shifted_points(1, :) <= window_size/2 & ...
    time_shifted_points(2, :) <= window_size/2;

time_shifted_points_out = time_shifted_points(:, event_window);
end


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=24;%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('timecom24','nmpcplot1','impcplot1');
save('feasibility24','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-24\n');
    controller_nmpc_dcbf_multiple24 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple24.obs = obs;
    controller_nmpc_dcbf_multiple24.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple24.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