%% planRRT
%%  - basic coverage algorithm
%%
%% Last Modified - 11/15/2010 - R. Beard

function path=planCoverRRTDubins(wpp_start, R, map)

    % desired down position is down position of start node
    pd = wpp_start(3);

    % specify start node from wpp_start
    start_node = [wpp_start(1), wpp_start(2), pd, 0, 0, 0];
    % format is [N, E, D, chi, cost, parent]

    % return map
    returnMapSize = 30;  % this is a critical parameter!
    return_map = 50*ones(returnMapSize,returnMapSize)+ rand(returnMapSize,returnMapSize);
    plotReturnMap(return_map), %pause

    % construct search path by doing N search cycles
    SEARCH_CYCLES = 100;  % number of search cycles

    % look ahead tree parameters
    L = 2*R+1;  % segment Length
    vartheta = pi/4; % search angle
    depth = 5; % depth of look ahead tree

    % initialize path and tree
    path = start_node;
    for i=1:SEARCH_CYCLES,
        tree = extendTree(path(end,:),L,vartheta,depth,map,return_map,pd);
        next_path = findMaxReturnPath(tree);
        path = [path; next_path(1,:)];
        % update the return map
        return_map = updateReturnMap(next_path(1,:),return_map,map);
        plotReturnMap(return_map), %pause
        % set the end of the path as the root of the tree
    end

    % remove path segments where there is no turn
    path_=path;
    path = path(1,:);
    for i=2:size(path_,1),
        if path_(i,4)~=path_(i-1,4);
            path = [path; path_(i,:)];
        end
    end

%     % specify that these are straight-line paths.
%     for i=1:size(path,1),
%         path(i,4)=-9999;
%     end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% extendTree
%%   extend tree by randomly selecting point and growing tree toward that
%%   point
function tree = extendTree(start_node,L,vartheta,depth,map,return_map,pd)

  tree_ = [start_node, 0];  % the last variable marks node as expanded

  % extend tree initially
  for d = 1:depth,
      newnodes = [];
      for j=1:size(tree_,1),
          if tree_(j,7)~=1, % process unexpanded nodes
              for i=1:3,
                  if     i==1, theta = tree_(j,4)-vartheta;
                  elseif i==2, theta = tree_(j,4);
                  elseif i==3, theta = tree_(j,4)+vartheta;
                  end
                  newnode_ = [...
                      tree_(j,1)+L*cos(theta),...
                      tree_(j,2)+L*sin(theta),...
                      tree_(j,3),...
                      theta,...
                      0,...
                      j,...
                      0,...
                      ];
                  if collision(tree_(j,:), newnode_, map)==0,
                      newnode_(5) = tree_(j,5)...
                          +findReturn(newnode_(1),newnode_(2),return_map,map);
                      newnodes = [newnodes; newnode_];
                  end
              end
              tree_(j,7)=1; % mark as expanded
          end
      end
      tree_ = [tree_; newnodes];
  end
  tree = tree_(:,1:6);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% collision
%%   check to see if a node is in collsion with obstacles
function collision_flag = collision(start_node, end_node, map)
    collision_flag = 0;
    sigma = 0:.1:1; % define where collisions will be checked
    for i=1:length(sigma),
        X = (1-sigma(i))*start_node(1) + sigma(i)*end_node(1);
        Y = (1-sigma(i))*start_node(2) + sigma(i)*end_node(2);
        Z = (1-sigma(i))*start_node(3) + sigma(i)*end_node(3);
        if Z >= downAtNE(map, X, Y),
            collision_flag = 1;
        end
        % check to see if outside of world
        if (X>map.width) | (X<0) | (Y>map.width) | (Y<0),
            collision_flag = 1;
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% downAtNE
%%   find the map down coordinate at a specified (n,e) location
function down = downAtNE(map, n, e)

      [d_n,idx_n] = min(abs(n - map.buildings_n));
      [d_e,idx_e] = min(abs(e - map.buildings_e));

      if (d_n<=map.BuildingWidth) & (d_e<=map.BuildingWidth),
          down = -map.heights(idx_e,idx_n);
      else
          down = 0;
      end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% findReturn
%%   compute the return value at a particular location
function return_value = findReturn(pn,pe,return_map,map);

  [pn_max,pe_max] = size(return_map);
  fn = pn_max*pn/map.width;
  fn = min(pn_max,round(fn));
  fn = max(1,fn);
  fe = pe_max*pe/map.width;
  fe = min(pe_max,round(fe));
  fe = max(1,fe);
  return_value = return_map(fn,fe);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% findMaxReturnPath
%%   find the maximum return path in the tree
function path = findMaxReturnPath(tree)

     % find node with max return
    [tmp,idx] = max(tree(:,5));

    % construct path with maximum return
    path = tree(idx,:);
    parent_node = tree(idx,6);
    %while parent_node>1,
    while parent_node>1,
        path = [tree(parent_node,:); path];
        parent_node = tree(parent_node,6);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% updateReturnMap
%%   update the return map to indicate where MAV has been
function new_return_map = updateReturnMap(path,return_map,map)

  new_return_map = return_map;
  for i=1:size(path,1),
    pn = path(i,1);
    pe = path(i,2);
    [pn_max,pe_max] = size(return_map);
    fn = pn_max*pn/map.width;
    fn = min(pn_max,round(fn));
    fn = max(1,fn);
    fe = pe_max*pe/map.width;
    fe = min(pe_max,round(fe));
    fe = max(1,fe);

    new_return_map(fn,fe) = return_map(fn,fe) - 50;
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotReturnMap
%%   plot the return map
function plotReturnMap(return_map)

  figure(2), clf
  mesh(return_map)
end


% Saving Movie Files Example
%
% This example demonstrates how to save the simulation animations as a
% movie. It builds on the Heterogeneous Propagation Medium Example.
%
% author: Bradley Treeby
% date: 30th June 2009
% last update: 17th October 2011
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all;

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

% define the properties of the propagation medium
medium.sound_speed = 1500*ones(Nx, Ny); % [m/s]
medium.sound_speed(1:Nx/2, :) = 1800;   % [m/s]
medium.density = 1000*ones(Nx, Ny);     % [kg/m^3]
medium.density(:, Ny/4:end) = 1200;     % [kg/m^3]

% create initial pressure distribution using makeDisc
disc_magnitude = 5; % [Pa]
disc_x_pos = 50;    % [grid points]
disc_y_pos = 50;    % [grid points]
disc_radius = 8;    % [grid points]
disc_1 = disc_magnitude*makeDisc(Nx, Ny, disc_x_pos, disc_y_pos, disc_radius);

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

source.p0 = disc_1 + disc_2;

% define a centered circular sensor
sensor_radius = 4e-3;   % [m]
num_sensor_points = 50;
sensor.mask = makeCartCircle(sensor_radius, num_sensor_points);

% define the input arguments
input_args = {'PlotPML', false, 'RecordMovie', true, 'MovieName', 'example_movie_1', 'MovieType', 'image', 'PlotFreq', 5, 'MovieArgs', {'fps', 30}};

% run the simulation
kspaceFirstOrder2D(kgrid, medium, source, sensor, input_args{:});

% define a second set of input arguments
input_args = {'MeshPlot', true, 'DisplayMask', 'off', 'PlotPML', false, 'RecordMovie', true, 'MovieName', 'example_movie_2', 'PlotFreq', 5, 'MovieArgs', {'fps', 30}};

% run the simulation without a sensor input
kspaceFirstOrder2D(kgrid, medium, source, [], input_args{:});


function g = whiteblockKernDiagGradient(kern, x, covDiag)

% WHITEBLOCKKERNDIAGGRADIENT WHITEBLOCK kernel's diagonal gradient wrt par.
% FORMAT
% DESC computes the gradient of the diagonal of the white noise kernel
% block matrix with respect to the parameters of the kernel. The
% parameters' gradients are returned in the order given by the
% whiteblockKernExtractParam command.
% ARG kern : the kernel structure for which the gradients are
% computed.
% ARG x : the input data for which the gradient is being computed.
% ARG covDiag : partial derivatives of the function of interest with
% respect to the diagonal elements of the kernel.
% RETURN g : gradients of the relevant function with respect to each
% of the parameters. Ordering should match the ordering given in
% whiteblockKernExtractParam.
%
% SEEALSO : whiteblockKernParamInit, kernDiagGradient
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

g = zeros(1, kern.nout);
startOne = 1;
endOne = 0;
for i=1:kern.nout
    endOne = endOne + size(x,1);
    g(1, i) = sum(covDiag(startOne:endOne));
    startOne = endOne + 1;
end


function [dataout] = ft_eventtiminganalysis(cfg, data)

% FT_EVENTTIMINGANALYSIS computes a model of single trial event-
% related activity, by estimating per trial the latency (and
% amplitude) of event-related signal components.
%
% Use as
%   [dataout] = ft_eventtiminganalysis(cfg, data)
% where data is single-channel raw data as obtained by FT_PREPROCESSING
% and cfg is a configuration structure according to
%
%  cfg.method  = method for estimating event-related activity
%                 'aseo', analysis of single-trial ERP and ongoing
%                         activity (according to Xu et al, 2009)
%                 'gbve', graph-based variability estimation
%                         (according to Gramfort et al, IEEE TBME 2009)
%  cfg.channel = Nx1 cell-array with selection of channels (default = 'all'),
%                see FT_CHANNELSELECTION for details
%  cfg.trials  = 'all' or a selection given as a 1xN vector (default = 'all')
%  cfg.output  = 'model', or 'residual', which returns the modelled data,
%                or the residuals.
%
% Method specific options are specified in the appropriate substructure.
%
% For the ASEO method, the following options can be specified:
%   cfg.aseo.noiseEstimate   = 'non-parametric' or 'parametric', estimate noise
%                              using parametric or non-parametric (default) method
%   cfg.aseo.tapsmofrq       = value, smoothing parameter of noise for
%                              nonparametric estimation (default = 5)
%   cfg.aseo.jitter          = value, time jitter in initial timewindow
%                              estimate (in seconds). default 0.050 seconds
%   cfg.aseo.numiteration    = value, number of iteration (default = 1)
%   cfg.aseo.initlatency     = Nx2 matrix, initial set of latencies in seconds of event-
%                              related components, give as [comp1start, comp1end;
%                              comp2start, comp2end] (default not
%                              specified). For multiple channels it should
%                              be a cell-array, one matrix per channel
%  Alternatively, rather than specifying a (set of latencies), one can also
%  specify:
%
%   cfg.aseo.initcomp        = vector, initial estimate of the waveform
%                              components. For multiple channels it should
%                              be a cell-array, one matrix per channel.
%
% For the GBVE method, the following options can be specified:
%   cfg.gbve.sigma             = vector, range of sigma values to explore in
%                                cross-validation loop (default: 0.01:0.01:0.2)
%   cfg.gbve.distance          = scalar, distance metric to use as
%                                evaluation criterion, see plugin code for
%                                more informatoin
%   cfg.gbve.alpha             = vector, range of alpha values to explor in
%                                cross-validation loop (default: [0 0.001 0.01 0.1])
%   cfg.gbve.exponent          = scalar, see plugin code for information
%   cfg.gbve.use_maximum       = boolean, (default: 1) consider the positive going peak
%   cfg.gbve.show_pca          = boolean, see plugin code (default 0)
%   cfg.gbve.show_trial_number = boolean, see plugin code (default 0)
%   cfg.gbve.verbose           = boolean (default: 1)
%   cfg.gbve.disp_log          = boolean, see plugin code (default 0)
%   cfg.gbve.latency           = vector [min max], latency range in s
%                                (default: [-inf inf])
%   cfg.gbve.xwin              = scalar smoothing parameter for moving
%                                average smoothing (default: 1), see
%                                eeglab's movav function for more
%                                information.
%
% To facilitate data-handling and distributed computing you can use
%   cfg.inputfile   =  ...
%   cfg.outputfile  =  ...
% If you specify one of these (or both) the input data will be read from a *.mat
% file on disk and/or the output data will be written to a *.mat file. These mat
% files should contain only a single variable, corresponding with the
% input/output structure.
%
% See also FT_SINGLETRIALANALYSIS_ASEO

% Copyright (C) 2018-2019, Jan-Mathijs Schoffelen DCCN
%
% 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$

ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble loadvar    data
ft_preamble provenance data
ft_preamble trackconfig

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  % do not continue function execution in case the outputfile is present and the user indicated to keep it
  return
end

% ensure that the input data is valid for this function
data = ft_checkdata(data, 'datatype', {'raw+comp', 'raw'}, 'feedback', 'yes', 'hassampleinfo', 'yes');

% ensure that the required options are present
cfg         = ft_checkconfig(cfg, 'required', {'method'});
cfg.trials  = ft_getopt(cfg, 'trials',  'all', 1); % all trials as default
cfg.channel = ft_getopt(cfg, 'channel', 'all');
cfg.output  = ft_getopt(cfg, 'output',  'model');
% ensure that the options are valid
cfg = ft_checkopt(cfg, 'method', 'char', {'aseo' 'gbve'});

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the actual computation is done in the middle part
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% select trials of interest
tmpcfg = keepfields(cfg, {'trials' 'channel' 'showcallinfo'});
data   = ft_selectdata(tmpcfg, data);
% restore the provenance information
[cfg, data] = rollback_provenance(cfg, data);

% some error checks
if isfield(data, 'trial') && numel(data.trial)==0, ft_error('no trials were selected'); end
if numel(data.label)==0, ft_error('no channels were selected'); end

switch cfg.method
  case 'aseo'
    % define general variables that are used locally
    fsample = data.fsample; % Sampling Frequency in Hz
    nchan   = numel(data.label);
    nsample = numel(data.time{1}); %FIXME ASSUMING FIXED TIME AXIS ACROSS ALL TRIALS

    % setting a bunch of options, to be passed on to the lower level function
    if ~isfield(cfg, 'aseo'), cfg.aseo = []; end
    cfg.aseo.thresholdAmpH = ft_getopt(cfg.aseo, 'thresholdAmpH', 0.5);
    cfg.aseo.thresholdAmpL = ft_getopt(cfg.aseo, 'thresholdAmpL', 0.1);
    cfg.aseo.thresholdCorr = ft_getopt(cfg.aseo, 'thresholdCorr', 0.2);
    cfg.aseo.maxOrderAR    = ft_getopt(cfg.aseo, 'maxOrderAR',    5);
    cfg.aseo.noiseEstimate = ft_getopt(cfg.aseo, 'noiseEstimate', 'nonparametric');
    cfg.aseo.numiteration  = ft_getopt(cfg.aseo, 'numiteration',  1);
    cfg.aseo.tapsmofrq     = ft_getopt(cfg.aseo, 'tapsmofrq',     5);
    cfg.aseo.fsample       = fsample;
    cfg.aseo.nsample       = nsample;
    cfg.aseo.pad           = ft_getopt(cfg.aseo, 'pad', (2.*nsample)/fsample);

    % deal with the different ways with which the initial waveforms can be defined
    initlatency      = ft_getopt(cfg.aseo, 'initlatency', {});
    initcomp         = ft_getopt(cfg.aseo, 'initcomp',    {});
    jitter           = ft_getopt(cfg.aseo, 'jitter',      0.050); % half temporal width of shift in s

    if isempty(initlatency) && isempty(initcomp)
      ft_error('for the ASEO method you should supply either an initial estimate of the waveform component, or a set of latencies');
    elseif ~isempty(initlatency)
      % this takes precedence, and should contain per channel the begin and
      % end points of the subwindows in time, based on which the initial
      % subcomponents are estimated

      % ensure it to be a cell-array if the input is a matrix
      if ~iscell(initlatency)
        initlatency = repmat({initlatency},[1 nchan]);
      end
      make_init = true;
    elseif ~isempty(initcomp)
      % ensure it to be a cell-array if the input is a matrix
      if ~iscell(initcomp)
        initcomp = repmat({initcomp}, [1 nchan]);
      end
      make_init = false;
    end

    if make_init
      assert(numel(initlatency)==nchan);
      for k = 1:nchan
        % preprocessing data
        tmp     = cellrowselect(data.trial,k);
        chandat = cat(1,tmp{:});
        chandat = ft_preproc_baselinecorrect(chandat, nearest(data.time{1}, -inf), nearest(data.time{1}, 0));
        avgdat  = nanmean(chandat, 1);

        % set the initial ERP waveforms according to the preset parameters
        ncomp       = size(initlatency{k},1);
        initcomp{k} = zeros(nsample, ncomp);
        for m = 1:ncomp
          begsmp = nearest(data.time{1},initlatency{k}(m, 1));
          endsmp = nearest(data.time{1},initlatency{k}(m, 2));
          if begsmp<1,       begsmp = 1;       end
          if endsmp>nsample, endsmp = nsample; end

          tmp = avgdat(begsmp:endsmp)';
          initcomp{k}(begsmp:endsmp, m) = tmp;
        end
        initcomp{k} = initcomp{k} - repmat(mean(initcomp{k}),nsample,1);
      end
    else
      assert(numel(initcomp)==nchan);
    end

    if ~iscell(jitter)
      jitter = repmat({jitter}, [1 nchan]);
    end

    for k = 1:numel(jitter)
      if ~isempty(jitter{k})
        if size(jitter{k},1)~=size(initcomp{k},2), jitter{k} = repmat(jitter{k}(1,:),[size(initcomp{k},2) 1]); end
      end
    end

    % initialize the output data
    dataout = removefields(data, 'cfg');
    for k = 1:numel(data.trial)
      dataout.trial{k}(:) = nan;
    end

    % initialize the struct that will contain the output parameters
    params = struct([]);

    % do the actual computations
    for k = 1:nchan
      % preprocessing data
      tmp     = cellrowselect(data.trial,k);
      chandat = cat(1,tmp{:});

      % baseline correction
      chandat = ft_preproc_baselinecorrect(chandat, nearest(data.time{1}, -inf), nearest(data.time{1}, 0));

      % do zero-padding and FFT to the signal and initial waveforms
      npad         = cfg.aseo.pad*fsample;    % length of data + zero-padding number
      nfft         = 2.^(ceil(log2(npad)))*2;
      initcomp_fft = fft(initcomp{k}, nfft);  % Fourier transform of the initial waveform
      chandat_fft  = fft(chandat', nfft);     % Fourier transform of the signal

      cfg.aseo.jitter = jitter{k};
      output       = ft_singletrialanalysis_aseo(cfg, chandat_fft, initcomp_fft);

      params(k).latency    = output(end).lat_est./fsample;
      params(k).amplitude  = output(end).amp_est;
      params(k).components = output(end).erp_est;
      params(k).rejectflag = output(end).rejectflag;
      params(k).noise      = output(end).noise;

      for m = 1:numel(data.trial)
        if output(end).rejectflag(m)==0
          switch cfg.output
            case 'model'
              dataout.trial{m}(k,:) = data.trial{m}(k,:)-output(end).residual(:,m)';
            case 'residual'
              dataout.trial{m}(k,:) = output(end).residual(:,m)';
          end
        end
      end
    end

case 'gbve'
  ft_hastoolbox('lagextraction', 1);
  ft_hastoolbox('eeglab',        1); % because the low-level code might use a specific moving average function from EEGLAB
  ft_hastoolbox('cellfunction',  1);

  if ~isfield(cfg, 'gbve'), cfg.gbve = []; end
  cfg.gbve.NORMALIZE_DATA    = ft_getopt(cfg.gbve, 'NORMALIZE_DATA',     true);
  cfg.gbve.CENTER_DATA       = ft_getopt(cfg.gbve, 'CENTER_DATA',        false);
  cfg.gbve.USE_ADAPTIVE_SIGMA= ft_getopt(cfg.gbve, 'USE_ADAPTIVE_SIGMA', false);
  cfg.gbve.sigma             = ft_getopt(cfg.gbve, 'sigma',    0.01:0.01:0.2);
  cfg.gbve.distance          = ft_getopt(cfg.gbve, 'distance', 'corr2');
  cfg.gbve.alpha             = ft_getopt(cfg.gbve, 'alpha',    [0 0.001 0.01 0.1]);
  cfg.gbve.exponent          = ft_getopt(cfg.gbve, 'exponent', 1);
  cfg.gbve.use_maximum       = ft_getopt(cfg.gbve, 'use_maximum', 1); % consider the positive going peak
  cfg.gbve.show_pca          = ft_getopt(cfg.gbve, 'show_pca',          false);
  cfg.gbve.show_trial_number = ft_getopt(cfg.gbve, 'show_trial_number', false);
  cfg.gbve.verbose           = ft_getopt(cfg.gbve, 'verbose',           true);
  cfg.gbve.disp_log          = ft_getopt(cfg.gbve, 'disp_log',          false);
  cfg.gbve.latency           = ft_getopt(cfg.gbve, 'latency',  [-inf inf]);
  cfg.gbve.xwin              = ft_getopt(cfg.gbve, 'xwin',     1); % default is a bit of smoothing

  nchan = numel(data.label);
  ntrl  = numel(data.trial);

  tmin  = nearest(data.time{1}, cfg.gbve.latency(1));
  tmax  = nearest(data.time{1}, cfg.gbve.latency(2));

  % initialize the struct that will contain the output parameters
  dataout = removefields(data, 'cfg');
  params  = struct([]);
  for k = 1:nchan
    % preprocessing data
    options = cfg.gbve;

    fprintf('--- Processing channel %d\n',k);

    tmp     = cellrowselect(data.trial,k);
    chandat = cat(1,tmp{:});
    points  = chandat(:,tmin:tmax);

    % perform a loop across alpha values, cross validation
    alphas = options.alpha;

    if length(alphas) > 1 % Use Cross validation error if multiple alphas are specified
      best_CVerr = -Inf;

      K = 5;
      disp(['--- Running K Cross Validation (K = ',num2str(K),')']);

      block_idx = fix(linspace(1, ntrl, K+1)); % K cross validation
      for jj=1:length(alphas)
        options.alpha = alphas(jj);

        CVerr = 0;
        for kk = 1:K
          bidx = block_idx(kk):block_idx(kk+1);
          idx = 1:ntrl;
          idx(bidx) = [];

          data_k       = chandat(idx,:);
          points_k     = points(idx,:);
          [order,lags] = extractlag(points_k,options);

          data_reordered = data_k(order,:);
          lags           = lags + tmin;
          [data_aligned, dum] = perform_realign(data_reordered, data.time{1}, lags);
          data_aligned(~isfinite(data_aligned)) = nan;
          ep_evoked = nanmean(data_aligned);
          ep_evoked = ep_evoked ./ norm(ep_evoked);

          data_k = chandat(bidx,:);
          data_norm = sqrt(sum(data_k.^2,2));
          data_k = diag(1./data_norm)*data_k;
          data_k(data_norm==0,:) = 0;

          for pp=1:length(bidx)
            c     = xcorr(ep_evoked,data_k(pp,:));
            CVerr = CVerr + max(c(:));
          end
        end

        CVerr = CVerr/ntrl;

        if CVerr > best_CVerr
          best_CVerr = CVerr;
          best_alpha = alphas(jj);
        end
      end
      options.alpha = best_alpha;
    end

    if options.use_maximum
      [order,lags] = extractlag( points, options );
    else
      [order,lags] = extractlag( -points, options );
    end
    disp(['---------- Using alpha = ',num2str(options.alpha)]);
    data_reordered = chandat(order,:);
    lags = lags + tmin;
    [data_aligned] = perform_realign(data_reordered, data.time{1}, lags );
    data_aligned(~isfinite(data_aligned)) = nan;

    [dum,order_inv] = sort(order);
    lags_no_order = lags(order_inv);
    data_aligned  = data_aligned(order_inv,:);

    params(k).latency = data.time{1}(lags_no_order)';
    switch cfg.output
      case 'model'
        tmp = mat2cell(data_aligned, ones(1,size(data_aligned,1)), size(data_aligned,2))';
        dataout.trial = cellrowassign(dataout.trial, tmp, k);
      case 'residual'
        % to be done
        error('not yet implemented');
    end
  end

end

dataout.params = params;
dataout.cfg    = cfg;

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug
ft_postamble trackconfig
ft_postamble previous   data
ft_postamble provenance dataout
ft_postamble history    dataout
ft_postamble savevar    dataout


function [rec,prec,ap] = VOCevaldet_modified(DATAopts, cls, loadName, draw, flipBoxes)

% load test set
tic;
cp=sprintf(DATAopts.annocachepath,DATAopts.testset);
if exist(cp,'file')
    fprintf('%s: pr: loading ground truth\n',cls);
    load(cp,'gtids','recs');
else
%     [gtids,t]=textread(sprintf(DATAopts.imgsetpath,DATAopts.testset),'%s %d');
    gtids = GetImagesPlusLabels(DATAopts.testset);
    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
        recs(i)=PASreadrecord(sprintf(DATAopts.annopath,gtids{i}));
    end
    save(cp,'gtids','recs');
end

fprintf('%s: pr: evaluating detections\n',cls);

% hash image ids
hash=VOChash_init_modified(gtids);

% extract ground truth objects

npos=0;
gt(length(gtids))=struct('BB',[],'diff',[],'det',[]);
for i=1:length(gtids)
    % extract objects of class
    clsinds=strmatch(cls,{recs(i).objects(:).class},'exact');
    gt(i).BB=cat(1,recs(i).objects(clsinds).bbox)';
    gt(i).diff=[recs(i).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(DATAopts.detrespath,id,cls),'%s %f %f %f %f %f');
[ids,confidence,b1,b2,b3,b4]=textread(loadName,'%s %f %f %f %f %f');

if exist('flipBoxes', 'var') && flipBoxes == true
    BB=[b2 b1 b4 b3]';
else
    BB=[b1 b2 b3 b4]';
end

% 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=VOChash_lookup_modified(hash,ids{d});
    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>=DATAopts.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);

ap=VOCap(rec,prec);

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


function varargout = pow2(varargin)

switch class(varargin{1})

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = CreateBasicOperator('convex','increasing','positive','callback');
        operator.derivative = @(x)(log(2)*pow2(x));

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error(['SDPVAR/' upper(mfilename) ' called with weird argument']);
end


function [sol] = ls_solaat(rhs)

% Yin Zhang, 1997

global probData

% ----- check input rhs -----
if issparse(rhs)
   error('RHS vector must be in full matrix format.');
end;
m = length(probData.PERM);
if (max(size(rhs)) ~= m) || (min(size(rhs)) ~= 1)
   error('No symbolic factor or input sizes mismatch.');
end;

% ------ back solve ------
sol = zeros(m,1);
sol(probData.PERM) = ls_blkslv(probData.XLNZ,probData.XSUPER,probData.XLINDX,probData.LINDX,probData.LNZ0,rhs(probData.PERM));


% fir_filterdcpadded() - Pad data with DC constant and filter
%
% Usage:
%   >> data = fir_filterdcpadded(b, a, data, causal);
%
% Inputs:
%   b             - vector of filter coefficients
%   a             - 1
%   data          - raw data (times x chans)
%   causal        - boolean perform causal filtering {default 0}
%   usefftfilt    - boolean use fftfilt instead of filter
%
% Outputs:
%   data      - smoothed data
%
% Note:
%   firfiltdcpadded always operates (pads, filters) along first dimension.
%   Not memory optimized.
%
% Author: Andreas Widmann, University of Leipzig, 2014

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2013 Andreas Widmann, University of Leipzig, widmann@uni-leipzig.de
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
%
% $Id$

function [ data ] = fir_filterdcpadded(b, a, data, causal, usefftfilt)

% Defaults
if nargin < 4 || isempty(usefftfilt)
    usefftfilt = 0;
end
if nargin < 3 || isempty(causal)
    causal = 0;
end

% Check arguments
if nargin < 2
    ft_error('Not enough input arguments.');
end

% Is FIR?
if ~isscalar(a) || a ~= 1
    ft_error('Not a FIR filter. onepass-zerophase and onepass-minphase filtering is available for FIR filters only.')
end

% Group delay
if mod(length(b), 2) ~= 1
    ft_error('Filter order is not even.');
end
groupDelay = (length(b) - 1) / 2;

% Filter symmetry
isSym = all(b(1:groupDelay) == b(end:-1:groupDelay + 2));
isAntisym = all([b(1:groupDelay) == -b(end:-1:groupDelay + 2) b(groupDelay + 1) == 0]);
if causal == 0 && ~(isSym || isAntisym)
    ft_error('Filter is not anti-/symmetric. For onepass-zerophase filtering the filter must be anti-/symmetric.')
end

% Padding
if causal
    startPad = repmat(data(1, :), [2 * groupDelay 1]);
    endPad = [];
else
    startPad = repmat(data(1, :), [groupDelay 1]);
    endPad = repmat(data(end, :), [groupDelay 1]);
end

% Filter data (with double precision)
isSingle = isa(data, 'single');

if usefftfilt
    data = fftfilt(double(b), double([startPad; data; endPad]));
else
    data = filter(double(b), 1, double([startPad; data; endPad])); % Pad and filter with double precision
end

% Convert to single
if isSingle
    data = single(data);
end

% Remove padded data
data = data(2 * groupDelay + 1:end, :);

end


function [u, uu, erriter, num, timet] = test_CMF3D_ML
%%
%   Function test_CMF3D_ML
%
%   The matlab function to show how to use the functions:
%               CMF3D_ML_mex and CMF3D_ML_GPU
%
%   Before using the function CMF3D_ML_mex, you should compile its source file as follows:
%
%       >> mex CMF3D_ML_mex.c
%
%   Before using the function CMF3D_ML_GPU, you should compile its
%   GPU source file (for Windows):
%
%       >> nvmex -f nvmexopts.bat CMF3D_ML_GPU.cu -IC:\cuda\v4.0\include ...
%       >> -LC:\cuda\v4.0\lib\x64 -lcufft -lcudart
%
%   GPU compilation depends on your system and its configurations!
%
%   After compilation, you can define all the parameters (penalty, C_t, para) as follows:
%
%        - penalty: point to the edge-weight penalty parameters to
%                   total-variation function.
%
%          For the case without incorporating image-edge weights,
%          penalty is given by the constant everywhere. For the case
%          with image-edge weights, penalty is given by the pixelwise
%          weight function:
%
%          for example, penalty(x) = b/(1 + a*| grad f(x)|) where b,a > 0 .
%
%        - C_t(x,i=1...nlab): point to the capacities of
%                 sink flow fields pt(x,i=1...nlab);
%
%        - para: a sequence of parameters for the algorithm
%             para[0,1,2]: rows, cols, heights of the given 3D volume
%             para[3]: the number of labels or regions
%             para[4]: the maximum iteration number
%             para[5]: the error bound for convergence
%             para[6]: cc for the step-size of augmented Lagrangian method
%             para[7]: the step-size for the graident-projection step to the
%                    total-variation function. Its optimal range is [0.06, 0.12].
%
%   Outputs:
%
%       - u: the computed continuous labeling result u(x,i = 1...nlab) in [0,1],
%           where nlab is the number of labels/volumes.
%
%       - uu: the final labeled result uu(x), computed by the u(x,i).
%
%       - erriter: it returns the error evaluation of each iteration,
%           i.e. it shows the convergence rate. One can check the algorithm
%           performance.
%
%       - i: gives the total number of iterations, when the algorithm converges.
%
%       - timet: gives the total computation time.
%
%       Example:
%
%             >> [u, erriter, i, timet] = CMF3D_ML_GPU(single(penalty), single(Cs), single(Ct), single(para));
%
%             >> You can choose a 2D slice to demonstrate the final result.
%
%             >> figure, loglog(erriter,'DisplayName','erriterN');figure(gcf)
%
%
%   The original algorithm was proposed in the following papers:
%
%   [1] Yuan, J.; Bae, E.;  Tai, X.-C.
%       A Study on Continuous Max-Flow and Min-Cut Approaches
%       CVPR, 2010
%
%   [2] Yuan, J.; Bae, E.; Tai, X.-C.; Boycov, Y.
%       A Continuous Max-Flow Approach to Potts Model
%       ECCV, 2010
%
%   The mimetic finite-difference discretization method was proposed for
%   the total-variation function in the paper:
%
%   [1] Yuan, J.; Schn{\"o}rr, C.; Steidl, G.
%       Simultaneous Optical Flow Estimation and Decomposition
%       SIAM J.~Scientific Computing, 2007, vol. 29, page 2283-2304, number 6
%
%   This software can be used only for research purposes, you should cite ALL of
%   the aforementioned papers in any resulting publication.
%
%
%   Please email Jing Yuan (cn.yuanjing@gmail.com) for any questions,
%   suggestions and bug reports
%
%   The Software is provided "as is", without warranty of any kind.
%
%               Version 1.0
%   https://sites.google.com/site/wwwjingyuan/
%
%   Copyright 2011 Jing Yuan (cn.yuanjing@gmail.com)
%

nii = load_nii('T1w_fa18_restore_brain.nii');
ur = double(nii.img);
umax = max(max(max(ur)));
umin = min(min(min(ur)));
ur = (ur - umin)/(umax-umin);
ur = ur(21:120, 70:170, 69:170);

% define the label information

nlab = 4;
ulab(1) = 0;
ulab(2) = 0.16;
ulab(3) = 0.4;
ulab(4) = 0.53;

[rows, cols, heights] = size(ur);

% build up the data terms

Ct = zeros(rows,cols,heights,nlab);

for i=1:nlab
    Ct(:,:,:,i) = abs(ur - ulab(i))*10;
end

% set up the input parameter vector
cc = 0.25;
varParas = [rows; cols; heights; nlab; 200; 1e-4; cc; 0.11];
%                para 0,1,2 - rows, cols, heights
%                para 3 - the total number of labels
%                para 4 - the maximum number of iterations
%                para 5 - the error bound for convergence
%                para 6 - cc for the step-size of augmented Lagrangian method
%                para 7 - the step-size for the graident-projection of the spatial flow fields p(x,i)

penalty = ones(rows,cols,heights);

% ----------------------------------------------------------------------
%  Use the function CMF3D_ML_mex to run the algorithm on CPU
% ----------------------------------------------------------------------

% [u, erriter,num,timet] = CMF3D_ML_mex(single(penalty), single(Ct), single(varParas));

% ----------------------------------------------------------------------
%  Use the function CMF3D_ML_GPU to run the algorithm on GPU
% ----------------------------------------------------------------------

[u, erriter,num,timet] = CMF3D_ML_GPU(single(penalty), single(Ct), single(varParas));

% The output u must be multiplied by cc to scale it in [0,1],
% due to the implementation
u = cc*u;

[um,I] = max(u, [], 4);

uu = zeros(rows,cols,heights);

for k=1:rows
    for j=1:cols
        for i=1:heights
            uu(k,j,i) = ulab(I(k,j,i));
        end
    end
end


function DrawClusters(h1,M,gIX,dataFR,numK,stim,fictive,clrmap,rankscore,iswrite)
pos = get(gca,'Position');
barratio = 0.03;

%% Prepare cluster data
% down-sample
displaymax = 1000;
numcell = size(M,1);
if numcell > displaymax,
    skip = round(numcell/displaymax);
    M = M(1:skip:end,:);
    gIX = gIX(1:skip:end,:);
end

numK = double(max(double(numK),double(max(gIX))));

% sort traces by index
im = M;
[nLines,nFrames] = size(im);
[~,I] = sort(gIX);
im = im(I,:);

%% convert imagesc effect into rgb matrix 'RGB'
if dataFR, % is drawing cell-response fluorescence
    cmap = gray(64);
    im = AutoScaleImage0to1(im); % scaling to min/max 0/1
    minlim = 0;
    maxlim = 1;
else % create blue-white-red map, for regression results
    cmap = zeros(64,3);
    cmap(:,1) = [linspace(0,1,32), linspace(1,1,32)];
    cmap(:,2) = [linspace(0,1,32), linspace(1,0,32)];
    cmap(:,3) = [linspace(1,1,32), linspace(1,0,32)];
    minlim = -1; %min(min(im));
    maxlim = 1; %max(max(im));
end
RGB = ImageToRGB(im,cmap,minlim,maxlim); % map image matrix to range of colormap

%% add vertical color code
if exist('clrmap','var'),
    if strcmp(clrmap,'jet'),
        temp = flipud(jet(numK));
    else % 'hsv'
        temp = hsv(round(numK*1.1));
    end
else % 'hsv'
    temp = hsv(round(numK*1.1));
end
cmap2 = vertcat(temp(1:numK,:),[0,0,0]); % extend colormap to include black
bwidth = max(round(nFrames/30),1);

idx = gIX(I);
ix_div = [find(diff(idx));length(idx)];

bars = ones(nLines,bwidth);
if numK>1,
    bars(1:ix_div(1),:) = idx(ix_div(1));
    for i = 2:length(ix_div),
        % paint color
        bars(ix_div(i-1)+1:ix_div(i),:) = idx(ix_div(i));
    end
end
im_bars = reshape(cmap2(bars,:),[size(bars),3]);
% add a white division margin
div = ones(nLines,round(bwidth/2),3);
% put 3 parts together
im = horzcat(RGB,div,im_bars);

%% plot figure

[s1,s2,s3] = size(im);

hold off;
set(h1,'Position',[pos(1),pos(2)+pos(4)*barratio*3,pos(3),pos(4)*(1-4*barratio)]);
if s1<30,
    temp = im;
    im = ones(30,s2,s3);
    im(1:s1,:,:) = temp;
end
image(im);
set(gca, 'box', 'off')

hold on;

% plot cluster division lines
plot([0.5,s2+0.5],[0.5,0.5],'k','Linewidth',0.5);
if numK>1,
    for i = 1:length(ix_div),% = numK-1,
        y = ix_div(i)+0.5;
        plot([0.5,s2+0.5],[y,y],'k','Linewidth',0.5);
    end
end

ylabel(['Number of cells: ' num2str(numcell)]);
set(gca,'YTick',[],'XTick',[]);
set(gcf,'color',[1 1 1]);

colormap gray;

% write in number label
x = s2*1.003;
y_last = 0;
for i = 1:length(ix_div),
    % avoid label crowding
    margin = 0.015*s1;
    y0 = ix_div(i)+0.5;
    y = max(y_last+margin,y0);
    if i<length(ix_div),
        ynext = ix_div(i+1);
    else
        ynext = y0+margin*2;
    end
    % draw if not squeezed too much away
    if y < y0+margin*2 && y < 0.5*(y0+ynext), %nLines+stimheight-margin,
        if exist('iswrite','var') && iswrite,
            try
                text(x,y,[num2str(idx(ix_div(i))) ': ' num2str(rankscore(i))],'HorizontalAlignment','Left','VerticalAlignment','Bottom',...
                    'FontUnits','normalized','FontSize',0.015,'Tag','label');
            catch
                disp('rankscore invalid');
                text(x,y,num2str(idx(ix_div(i))),'HorizontalAlignment','Left','VerticalAlignment','Bottom',...
                    'FontUnits','normalized','FontSize',0.015,'Tag','label');
            end

        else
            text(x,y,num2str(idx(ix_div(i))),'HorizontalAlignment','Left','VerticalAlignment','Bottom',...
                'FontUnits','normalized','FontSize',0.015,'Tag','label');
        end
        y_last = y;
    end
end

%% Draw stim bars
halfbarheight = 100;
stimbar_ = GetStimBar(halfbarheight,stim);

% pad with margin on right to fit combo-image size
barlength = size(im,2);
stimbar = ones(halfbarheight*2,barlength,3);
stimbar(:,1:nFrames,:) = stimbar_(:,1:nFrames,:); % crop to size

% top axis
axes('Position',[pos(1),pos(2)+pos(4)*(1-barratio),pos(3),pos(4)*barratio]);
image(stimbar);axis off;hold on;
plot([1,length(stim)],[1,1],'k','Linewidth',0.5); % plot top border
plot([0.5,0.5],[0,size(stimbar,1)],'k','Linewidth',0.5); % plot left/right border
plot([length(stim),length(stim)],[0,size(stimbar,1)],'k','Linewidth',0.5);

% bottom axis
axes('Position',[pos(1),pos(2)+pos(4)*barratio*2,pos(3),pos(4)*barratio]);
image(stimbar);axis off;hold on;
plot([1,length(stim)],[size(stimbar,1),size(stimbar,1)],'k','Linewidth',0.5); % plot bottom border
plot([0.5,0.5],[0,size(stimbar,1)],'k','Linewidth',0.5); % plot left/right border
plot([length(stim),length(stim)],[0,size(stimbar,1)],'k','Linewidth',0.5);

%% Draw fictive
axes('Position',[pos(1),pos(2),pos(3),pos(4)*barratio*2]);

temp = ones(size(fictive,1),barlength);
temp(:,1:length(fictive)) = fictive;
imagesc(temp);colormap hot
set(gca,'YTick',[],'XTick',[]);
set(gcf,'color',[1 1 1]);
set(gca, 'box', 'off')
hold on;axis ij;

% plot division lines
for i = 0:3,
    y = i+0.5;
    plot([0.5,length(fictive)+0.5],[y,y],'w','Linewidth',0.5);
end
% labels
names = {'Left','Right','Forward','Raw L','Raw R'};
x = -s2*0.05;
for i = 1:5,
    y = i;
    text(x,y,names{i},'Fontsize',7);
end

%% horizontal axis: approximate time length, assuming 2 fps (real is 1.97 for now)
set(gca,'XColor',[1 1 1]);
if s2/2/60<5, % <5 min
    l = xlabel(['Length of display: ~ ' num2str(s2/2) 'sec'],'Color',[0 0 0]);
else
    l = xlabel(['Length of display: ~ ' num2str(s2/2/60) 'min'],'Color',[0 0 0]);
end

%% Draw fish icon:
if exist('fishpic.jpg','file')==2,
    fishpic = imread('fishpic.jpg');
    axes('Position',[0.015, 0.095, 0.03, 0.02]);
    image(fishpic); axis off; axis equal
end
end

%% Internal Functions

function im = AutoScaleImage0to1(im)
temp = mat2gray(im);
low_high = stretchlim(temp,[0.005,0.995]);
temp(temp<low_high(1)) = low_high(1);
temp(temp>low_high(2)) = low_high(2);
im = mat2gray(temp);
end

function RGB = ImageToRGB(im,cmap,minlim,maxlim)
L = size(cmap,1);
ix = round(interp1(linspace(minlim,maxlim,L),1:L,im,'linear','extrap'));
RGB = reshape(cmap(ix,:),[size(ix) 3]); % Make RGB image from scaled.
end


function base = stat_getBaselineDistrib(PConn,baseline,winCenterTimes,noavg)
% Return the distribution of an estimator averaged over a baseline and
% expanded out to the same dimensions as matrices in PConn connectivity
% object
%
% Inputs:
%
%       PConn:     connectivity distribution returned from stat_bootstrap()
%       baseline:  [min max] baseline interval relative to event (t=0 sec)
%
% Optional
%
%       winCenterTimes: vector of times corresponding to window centers
%                      (same units as baseline)
%       noavg:     if true, the baseline distribution is returned
% Outputs:
%
%       base:      the baseline mean distribution (expanded out to same dims as
%                  PConn)
%
% See Also: stat_bootstrap()
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual. Chapter 6.8
%   Available at: http://www.sccn.ucsd.edu/wiki/SIFT
%
% Author: Tim Mullen, 2011, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

if nargin<4
    noavg = false;
end

if isstruct(PConn)
    % connectivity object
    connmethods = hlp_getConnMethodNames(PConn);

    for cnd=1:length(PConn)

        baseidx = getindex(PConn(cnd).erWinCenterTimes,baseline);

        for m=1:length(connmethods)
            sz = size(PConn(cnd).(connmethods{m}));
            if noavg
                base.(connmethods{m}) = PConn(cnd).(connmethods{m})(:,:,:,baseidx(1):baseidx(2),:);
            else
                basetmp = (mean(PConn(cnd).(connmethods{m})(:,:,:,baseidx(1):baseidx(2),:),4));  % collapse baseline over timepoints
                base.(connmethods{m}) = repmat(basetmp,[1 1 1 sz(4) 1]);  % expands out to appropriate size
            end
        end

    end

else
    if nargin<3
        error('SIFT:stat_getBaselineDistrib','You must provide winCenterTimes');
    end

    % single matrix
    baseidx = getindex(winCenterTimes,baseline);
    sz = size(PConn);
    if noavg
        base = PConn(:,:,:,baseidx(1):baseidx(2),:);
    else
        basetmp = (mean(PConn(:,:,:,baseidx(1):baseidx(2),:),4));   % collapse baseline over timepoints
        base = repmat(basetmp,[1 1 1 sz(4) 1]);  % expands out to appropriate size
    end
end


function [a]=times(b,c,varargin)
%A=B.*C
%   [A]=TIMES(B,C) Computes Hadamard (elementwise) product of two
%   TT-matrices
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
n = b.n;
m = b.m;
b = b.tt;
c = c.tt;
a = b.*c;
a = tt_matrix(a, n, m);
end


function [ml] = cm32ml(cm3)
% Convert volume from cubic centimeters to milliliters.
% Chad Greene 2012
ml = cm3;


function varargout = domainCheck(varargin)
%DOMAINCHECK   True if the domains of two CHEBFUN2 objects are the same.
%   DOMAINCHECK(F, G) returns TRUE if the domains of the two
%   CHEBFUN2 objects F and G coincide up to a tolerance depending on their
%   horizontal scales or if both F and G are empty CHEBFUN objects.
%
% See also CHEBFUN/DOMAINCHECK.

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

[varargout{1:nargout}] = domainCheck@separableApprox(varargin{:});

end


function pass = test_plotcoeffs(pref)
% This test just ensures that CHEBMATRIX/PLOTCOEFFS() does not crash.

% Create an invisible figure to plot on:
hfig = figure('Visible', 'off');

%% Setup

d = [-2 2];                   % function domain
x = chebfun(@(x) x, d);

%% Create a couple of CHEBMATRICES and try to call PLOTCOEFFS() on them

% Only CHEBFUNS
cm1 = [sin(x-.32); cos(100*(x+.3)); 3*tanh(50*sin(x-.2))];
cm2 = [sin(x-.32), cos(100*(x+.3)); ...
    3*tanh(50*sin(x-.2)) 4*sin(40*(x+.25))];

% CHEBFUNS and doubles
cm3 = [sin(x-.32); cos(100*(x+.3)); 3];
cm4 = [sin(x-.32), 100; ...
    3 4*sin((x+.25))];

pass(1) = doesNotCrash(@() plotcoeffs(cm1));
pass(2) = doesNotCrash(@() plotcoeffs(cm2));
pass(3) = doesNotCrash(@() plotcoeffs(cm3));
pass(4) = doesNotCrash(@() plotcoeffs(cm4));

%%

close(hfig);

end

function pass = doesNotCrash(fn)

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

end


function draw_full_net(net,name)
%input: net (DAGNN format)  output_filename output: jpg
%This code is modified from http://stackoverflow.com/questions/5518200/automatically-generating-a-diagram-of-function-calls-in-matlab
OutputName = 'net';
if ~isempty(name)
    OutputName = name;
end
dotFile = [OutputName '.dot'];
fid = fopen(dotFile, 'w');
fprintf(fid, 'digraph G {\n');
fprintf(fid,'rankdir=LR;\n');
for i = 1:numel(net.layers)
    for j = 1:numel(net.layers(i).inputs)  % multi-input to one output
        fprintf( fid,'%s -> %s', char(net.layers(i).inputs(j)), char(net.layers(i).outputs));
        block = net.layers(i).block;
        block_class = class(block);
        if (isequal(block_class ,'dagnn.Conv'))
            fh = block.size(1);
            fw = block.size(2);
            fcount=block.size(4);
            step=block.stride(1);
            fprintf (fid,'[label = %s%dx%dx%dStride%d];\n',block_class(7:end),fh,fw,fcount,step);
        elseif isequal(block_class ,'dagnn.Pooling')
            fh = block.poolSize(1);
            fw = block.poolSize(2);
            step=block.stride(1);
            fprintf (fid,'[label = %s%dx%dStride%d];\n',block_class(7:end),fh,fw,step);
       % elseif isequal(block_class ,'dagnn.DropOut')
           % rate = block.rate;
            %fprintf (fid,'[label = %s%.1f];\n',block_class(7:end),rate);
        else
            fprintf (fid,'[label = %s];\n',block_class(7:end));
        end
    end
end
fprintf(fid, '}\n');
fclose(fid);

% Render to image
imageFile = [OutputName '.png'];
% Assumes the GraphViz bin dir is on the path; if not, use full path to dot.exe
cmd = sprintf('dot -Tpng -Gsize="2048,2048" "%s" -o"%s"', dotFile, imageFile);  % for better view, you can use number bigger than 32
system(cmd);
fprintf('Wrote to %s\n', imageFile);
%im = imread(imageFile);
%imshow(im);


%% Lane Marking Identification
% One component of future luxury automobile safety
% systems is warning drivers that they are drifting
% between lanes.  To do this, it is first necessary
% to identify where the lanes are.  In this example,
% we will look at identifying the lanes from a image
% of a road.
%
% Copyright 2007-2013 MathWorks, Inc.
%

%% Setup
clear

%% Initialize Objects
% Video Reader

hvfr = vision.VideoFileReader('viplanedeparture.avi', ...
    'VideoOutputDataType', 'uint8');

%% Configure Video Players
hVidSource = vision.VideoPlayer('Name', 'Source'      );
hVidLanes  = vision.VideoPlayer('Name', 'Verification');

%% Replay Loop
NumLoops = 1;
for loopIdx = 1:NumLoops

    % Loop through video frames
    while ~isDone(hvfr)
        % Read Frame and display
        frameRGB = step(hvfr);
        step(hVidSource, frameRGB);

        % Call algorithm
        laneVertices = lanemarking_Algorithm(frameRGB);

        % Display lanes
        displayLanes(frameRGB, laneVertices);

    % End While
    end

    reset(hvfr);
%% end loop
end

%% Release resources
release(hvfr   );


function  y = At_xy_nonorm(z, Phi)
%z = z./Phi_sum;
y = bsxfun(@times, z, Phi);

end


function sdpvarExpr = parseLMI(X)
%PARSELMI Internal function for dirty parsing of lmi string

% Assume Error

% TypeofConstraint = 1;

% Check for obsolete notation .<, .>, =
single_equality = strfind(X,'=');
if isempty(strfind(X,'>=')) & isempty(strfind(X,'<=')) & (rem(length(single_equality),2) == 1) % There is a single =
    error('Obsolete constraint =, use == in equalities.');
    %X = strrep(X,'=','==');
    %X = strrep(X,'====','=='); % Whoops, == replaced with =====!
end

if ~isempty(strfind(X,'.<'))
    disp(' ');
    disp('Warning: Obsolete constraint .<, use < in equalities.');
    disp('If you have a Hermitian matrix which you want to')
    disp('constrain to have negative values, use ,e.g.,  P(:)<0')
    disp('The constraint has been changed to <')
    disp(' ');
    X = strrep(X,'.<','<');
end

if ~isempty(strfind(X,'.>'))
    disp(' ');
    disp('Warning: Obsolete constraint .>, use > in equalities.');
    disp('If you have a Hermitian matrix which you want to')
    disp('constrain to have negative values, use e.g.  P(:)>0')
    disp('The constraint has been changed to >')
    disp(' ');
    X = strrep(X,'.>','>');
end

% Any norm? If not, we're done!
if isempty(strfind(X,'||'))
    sdpvarExpr = X;
    return
end

% The only actual parsing needed is for the ||Axplusb||<cx+d notation
delimiters = {'.<','.>','<.','>.','<','>','==',''};
delindex = 0;indtodel = [];
while isempty(indtodel) & (delindex<=7)
    delindex = delindex+1;
    indtodel = strfind(X,delimiters{delindex});
end

switch delindex
    case 5 %<
        TypeofConstraint = 1;
        ind_start =  indtodel-1;
        ind_end   =  indtodel+1;
        REVERSE   = 1;
    case 6 %>
        TypeofConstraint = 1;
        ind_start =  indtodel-1;
        ind_end   =  indtodel+1;
        REVERSE   = 0;
    case 7%=
        TypeofConstraint = 3;
        ind_start = indtodel-1;
        ind_end   = indtodel+2;
        REVERSE   = 0;
    case {3,4}
        error('Incorrect argument. Perhaps you mean .> or .<');
    otherwise
        error('Incorrect argument. Could not find <=>.>.<')
end

LeftHand   = X(1:ind_start);
RightHand  = X(ind_end:end);

if REVERSE
    temp = LeftHand;
    LeftHand = RightHand;
    RightHand = temp;
end

% Search for a norm expression
ind_norm_Right = strfind(RightHand,'||');
ind_norm_Left  = strfind(LeftHand,'||');

% Any norm at all, if not, we're done!
if isempty(ind_norm_Right) & isempty(ind_norm_Left)
    sdpvarExpr = [LeftHand '-(' RightHand ')'];
    return
end

% Equality constrained norm?
if (TypeofConstraint == 3) & (ind_norm_Right | ind_norm_Left)
    error('Equality constraints cannot be used with ||..|| operator');
end

% Convex normconstraint?
if ~isempty(ind_norm_Left)
    error('Norm ||..|| must look like || column vector ||< scalar, or scalar>|| vector ||')
end

% Everthing seem ok in norm
TypeofConstraint = 4;
Axplusb = RightHand(2+ind_norm_Right(1):ind_norm_Right(2)-1);
WithoutNorm = strrep(RightHand,RightHand(ind_norm_Right(1):ind_norm_Right(2)+1),'0');
if length(WithoutNorm)==1
    cxplusd = LeftHand;
else
    cxplusd = [LeftHand '-(' WithoutNorm ')'];
end

sdpvarExpr = ['cone( ' Axplusb ',' cxplusd ')'];


function [L,D] = spm_eeg_lgainmat(D,Is,channels)
% Load or compute if necessary a gain matrix
% FORMAT [L,D] = spm_eeg_lgainmat(D,Is,channels)
% D    - Data structure
% Is   - indices of vertices
%
% L    - Lead-field or gain matrix L(:,Is)
%__________________________________________________________________________
% Copyright (C) 2008-2017 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_eeg_lgainmat.m 7757 2019-12-16 15:36:06Z spm $

SVNrev = '$Rev: 7757 $';

%-Get gain or lead-field matrix
%--------------------------------------------------------------------------
val = D.val;

forward = D.inv{val}.forward;

for ind = 1:numel(forward)
    modality = forward(ind).modality;

    %-Channels
    %----------------------------------------------------------------------
    if isequal(modality, 'MEG')
        chanind = D.indchantype({'MEG', 'MEGPLANAR'}, 'GOOD');
    else
        chanind = D.indchantype(modality, 'GOOD');
    end

    if ~isempty(chanind)
        forward(ind).channels = D.chanlabels(chanind);
    else
        error(['No good ' modality ' channels were found.']);
    end
end

if nargin < 3
    channels = [forward(:).channels];
end

try
    fname = D.inv{val}.gainmat;
    G = load(fullfile(D.path, fname)); % Relative path

    label = G.label;
    G     = G.G;
    if numel(label) ~= size(G, 1) || ~all(ismember(channels, label))
        error('Gain matrix has an incorrect number of channels.');
    end
catch
    spm('sFnBanner', mfilename, SVNrev);
    spm('Pointer', 'Watch');

    G     = {};
    label = {};

    for ind = 1:numel(forward)
        %-Create a new lead-field matrix
        %==================================================================

        %-Head Geometry (create tesselation file)
        %------------------------------------------------------------------
        vert = forward(ind).mesh.vert;
        face = forward(ind).mesh.face;

        %-Normals
        %------------------------------------------------------------------
        norm = spm_mesh_normals(struct('faces',face,'vertices',vert),true);

        vol  = forward(ind).vol;

        if ischar(vol)
            vol = ft_read_headmodel(vol);
        end

        modality = forward(ind).modality;

        if isfield(forward, 'siunits') && forward(ind).siunits
            units = D.units(D.indchannel(forward(ind).channels));
            sens  = forward(ind).sensors;
            siunits = isempty(strmatch('unknown', units));
        else
            siunits = false;
            sens = D.inv{val}.datareg(ind).sensors;
        end

        %-Forward computation
        %------------------------------------------------------------------
        [vol, sens] = ft_prepare_vol_sens(vol, sens, 'channel', forward(ind).channels);
        nvert = size(vert, 1);

        spm_progress_bar('Init', nvert, ['Computing ' modality ' leadfields']);
        if nvert > 100, Ibar = floor(linspace(1, nvert,100));
        else Ibar = [1:nvert]; end

        if ~isequal(ft_headmodeltype(vol), 'interpolate')
            Gxyz = zeros(length(forward(ind).channels), 3*nvert);
            for i = 1:nvert
                if siunits
                    Gxyz(:, (3*i - 2):(3*i))  = ft_compute_leadfield(vert(i, :), sens, vol,...
                        'dipoleunit', 'nA*m', 'chanunit', units);
                else
                    Gxyz(:, (3*i - 2):(3*i))  = ft_compute_leadfield(vert(i, :), sens, vol);
                end

                if any(Ibar == i)
                    spm_progress_bar('Set', i);
                end
            end
        else
            if siunits
                Gxyz = ft_compute_leadfield(vert, sens, vol, 'dipoleunit', 'nA*m', 'chanunit', units);
            else
                Gxyz = ft_compute_leadfield(vert, sens, vol);
            end
        end

        spm_progress_bar('Clear');

        spm_progress_bar('Init', nvert, ['Orienting ' modality ' leadfields']);

        G{ind} = zeros(size(Gxyz, 1), size(Gxyz, 2)/3);
        for i = 1:nvert
            G{ind}(:, i) = Gxyz(:, (3*i- 2):(3*i))*norm(i, :)';
            if ismember(i,Ibar)
                spm_progress_bar('Set', i);
            end

        end

        spm_progress_bar('Clear');

        %-Condition the scaling of the lead-field
        %------------------------------------------------------------------
        [Gs, scale] = spm_cond_units(G{ind});

        if siunits && abs(log10(scale))>2
            warning(['Scaling expected to be 1 for SI units, actual scaling ' num2str(scale)]);
            G{ind} = Gs;
        else
            scale = 1;
        end

        label = [label; forward(ind).channels(:)];

        forward(ind).scale = scale;
    end

    if numel(G) > 1
        G = cat(1, G{:});
    else
        G = G{1};
    end

    %-Save
    %----------------------------------------------------------------------
    D.inv{val}.gainmat = ['SPMgainmatrix_' spm_file(D.fname, 'basename') '_' num2str(val) '.mat'];
    save(fullfile(D.path, D.inv{val}.gainmat), 'G', 'label', spm_get_defaults('mat.format'));
    D.inv{val}.forward = forward;
    save(D);

    spm('Pointer', 'Arrow');
end

[sel1, sel2] = spm_match_str(channels, label);

if length(sel2) ~= numel(channels)
    error('Did not find a match for all the requested channels.');
end

L = sparse(G(sel2, :));

%-Retain selected sources if necessary
%--------------------------------------------------------------------------
if nargin > 1 && ~isempty(Is)
    L = L(:,Is);
end

D.inv{val}.forward = forward;


function [noisy, enhanced, clean, enhanced_wav, noisySTFT, mask, variance] = RunEnhanceNN(Data, layer, para)

output = FeatureTree2(Data, para, layer);

noisySTFT = gather(output{1}{1});
noisy = gather(output{1}{2});
enhanced = gather(output{1}{3});

enhanced_wav = abs2wav(exp(enhanced(1:257,:)/2)', angle(noisySTFT)', 400, 240);

if isempty(para.test.clean_idx)
    clean  = [];
else
    clean = gather(output{1}{para.test.clean_idx});
end

if isempty(para.test.mask_idx)
    mask = [];
else
    mask = output{1}{para.test.mask_idx};
end

if isempty(para.test.var_idx)
    variance = [];
else
    variance = output{1}{para.test.var_idx};
    if 0
        enhanced = enhanced - variance.^0.5;
        enhanced_wav = abs2wav(exp(enhanced(1:257,:)/2)', angle(noisySTFT)', 400, 240);
    end
    % [enhancedML,LL] = delta2static_ML(enhanced', variance', 2, 257);
end

end


%%%  HeightControl_Sim
clear
path('./icon/',path);
Init;

% constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
% throttle when UAV is hovering
THR_HOVER = 0.609;
%% initial condition
ModelInit_PosE = [0, 0, -100];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 0;
%% control parameter
% attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;

Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
% position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;
% integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

% max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
% max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
% maximum control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
% throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
HeightControl_Sim


function y =Fault_decision_b2m2_t2(u)
global st17

% Using model defined by the structure st17 to make decision about type2 fault in the pitch beta2m2

res=u(1);u=u(2:end);
if res>0.5
sker=st17.x2sup+(abs(u))'*abs(u)*ones(st17.Nlsup,1)-2*st17.xsup*abs(u);
y=(st17.w)'*exp(-sker./(2*(st17.sigma).^2))+st17.b;
else
    y=0;
end
if y>=0.5
    y=1;
else
    y=0;
end

end


function [traj, infStates] = tapas_hhmm(r, p, varargin)
% Estimates a hierarchical hidden Markov model (HHMM)
%
% This function can be called in two ways:
%
% (1) hhmm(r, p)
%
%     where r is the structure generated by fitModel and p is the parameter vector in native space;
%
% (2) hhmm(r, ptrans, 'trans')
%
%     where r is the structure generated by fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    [p pstruct] = hhmm_transp(r, p);
end

% Fixed configuration elements
%
% Number of possible outcomes
m = r.c_prc.n_outcomes;

% Get model tree (N is for node)
N = pstruct.N;

% Check constraints
if ~isempty(N{1}.V)
    error('tapas:hgf:hhmm:IllegEntryProbRoot', 'Illegal entry probability for root node.');
end

for id = 1:length(N)
    if isempty(N{id}.A) == isempty(N{id}.B)
        error('tapas:hgf:hhmm:IllegCombOfAB', 'Illegal combination of A and B for node no. %d.', id);
    end

    if length(N{id}.children(:)) ~= size(N{id}.A,2)
        error('tapas:hgf:hhmm:NumOfChildIncons', 'Number of children inconsistent with A for node no. %d.', id);
    end

    if ~isempty(N{id}.A) && any(sum(N{id}.A,2)>1)
        error('tapas:hgf:hhmm:IllegA', 'Illegal transition matrix A for node no. %d: row sums have to be less than or equal to 1.', id);
    end

    if ~isempty(N{id}.A)
        for cid = N{id}.children
            cidx = find(N{id}.children==cid);
            if ~isempty(N{cid}.children) && N{id}.A(cidx,cidx) ~= 0
                error('tapas:hgf:hhmm:IllegASelf', 'Illegal transition matrix A for node no. %d: only production nodes may have self-transitions.', id);
            end
        end
    end

    if ~isempty(N{id}.B) && sum(N{id}.B(:))~=1
        error('tapas:hgf:hhmm:IllegB', 'Illegal outcome contingency vector B for node no. %d.', id);
    end

    if ~isempty(N{id}.children)
        Vsum = 0;
        for cid = N{id}.children
            Vsum = Vsum + N{cid}.V;
        end

        if Vsum ~= 1
            error('tapas:hgf:hhmm:IllegV', 'Illegal vertical transition probabilities V from node no. %d.', id);
        end
    end
end

% Flatten the tree into one large transition matrix
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

% Find production nodes
pn = [];
for id = 1:length(N)
    if isempty(N{id}.children)
        pn = [pn, id];
    end
end
flatDim = length(pn);

% Initialize outcome contingency matrix
Bflat = NaN(m,flatDim);

% Fill Bflat
for i = 1:flatDim
    Bflat(:,i) = N{pn(i)}.B';
end

% Check Bflat
if any(~isfinite(Bflat(:)))
    error('tapas:hgf:hhmm:NoOutConMat', 'Could not construct outcome contingency matrix.');
end

% Initialize flattened transitioned matrix
Aflat = NaN(flatDim);

% Fill Aflat
for i = 1:flatDim
    for j = 1:flatDim
        if N{pn(i)}.parent == N{pn(j)}.parent
            % If the production nodes are siblings, read out
            % their parent's A matrix
            pid = N{pn(i)}.parent;
            idx = find(N{pid}.children==pn(i));
            jdx = find(N{pid}.children==pn(j));
            Aflat(i,j) = N{pid}.A(idx,jdx);
        else
            % Otherwise, determine their lowest common ancestor
            % and use that to calculate the transition probability
            caid = ca(N,pn(i),pn(j));
            tp = 1;

            nid  = pn(i);
            pid  = N{nid}.parent;
            nidx = find(N{pid}.children==nid);

            % Move up to one node below lowest common ancestor from start node pn(i)
            while pid ~= caid
                aend = 1-sum(N{pid}.A(nidx,:));
                tp = tp*aend;

                nid = pid;
                pid = N{nid}.parent;
                nidx = find(N{pid}.children==nid);
            end

            % Do the horizontal transition to the ancestral line of target node pn(j)
            ancj = anc(N,pn(j));
            while ancj(1) ~= caid
                ancj(1) = [];
            end
            ancj(1) = [];

            caidxi = nidx;
            caidxj = find(N{caid}.children==ancj(1));
            tp = tp*N{caid}.A(caidxi,caidxj);

            % Go down to the target node pn(j)
            ancj(1) = [];
            while ~isempty(ancj)
                tp = tp*N{ancj(1)}.V;
                ancj(1) = [];
            end

            Aflat(i,j) = tp;
        end
    end
end

% Check Aflat
if any(~isfinite(Aflat(:)))
    error('tapas:hgf:hhmm:NoFlatTransMat', 'Could not flatten transition matrix.');
end

% Calculate prior probabilities of production nodes
pnp = NaN(1,flatDim);
for i = 1:flatDim
    anci = anc(N,pn(i));
    anci(1) = [];
    p = 1;
    while ~isempty(anci)
        p = p*N{anci(1)}.V;
        anci(1) = [];
    end
    pnp(i) = p;
end

% Check pnp
if sum(pnp) ~= 1
    error('tapas:hgf:hhmm:NoPriorProdNodes', 'Cannot calculate prior probabilities of production nodes.');
end

% Input and number of trials
u = r.u(:,1);
n = length(u);

% Initialize alpha-prime
alpr = NaN(n,flatDim);

% alpr(1,:)
altmp = pnp.*Bflat(u(1),:);
llh = sum(altmp);
alpr(1,:) = altmp./llh;

% Pass through alpha-prime update loop
for k = 2:1:n
    if not(ismember(k, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        altmp = Bflat(u(k),:).*(alpr(k-1,:)*Aflat);
        llh = sum(altmp);
        alpr(k,:) = altmp./llh;
    else
        alpr(k,:) = alpr(k-1,:);
    end
end

% Predicted states
alprhat = [pnp; alpr];
alprhat(end,:) = [];

% Create result data structure
traj = struct;

traj.alpr    = alpr;
traj.alprhat = alprhat;

% Create matrix needed by observation model
infStates = traj.alpr;

end % function hhmm

% ----------------------------------------------------------------------------------------
% Find lowest common ancestor of nodes
function ca = ca(N,ida,idb)
    % Find ancestors of ida and idb
    anca = anc(N,ida);
    ancb = anc(N,idb);

    % Determine lowest common ancestor
    ca = NaN;
    aa = anca(1);
    ab = ancb(1);
    while aa == ab && ~isempty(anca) && ~isempty(ancb)
        ca = aa;
        anca(1) = [];
        ancb(1) = [];
        aa = anca(1);
        ab = ancb(1);
    end

    if aa == ab
        ca = aa;
    end
end

% ----------------------------------------------------------------------------------------
% Find ancestors of a node
function anc = anc(N,id)
    anc = id;
    idt = N{id}.parent;
    while ~isempty(idt)
        anc = [idt, anc];
        idt = N{idt}.parent;
    end
end


function [p] = mesh_fit_elec(p)

% mesh_fit_elec - find mesh vertices nearest to electrodes
%
% [p] = mesh_fit_elec(p)
%
% This function is in development.  A transformation matrix,
% with rotations and translations, is in development to
% facilitate coregistration of elec vertices to the scalp
% vertices.  This needs to be refined by a minimisation
% function for the difference between the electrode
% locations and the nearest scalp vertices. The scalp
% mesh should be sufficiently refined to provide unique
% vertices for each electrode vertex.
%
% At present, it works with the BrainStorm meshes and the
% elec_124_cart.txt file in the eeg_example_data folder.
% The BrainStorm meshes contain a 'scalp' mesh with vertices
% in meters and the electrode file contains position
% vectors in centimeters.  The electrodes are converted
% to meters by ELEC_LOAD.  In the example data, they were
% previously coregistered and fitted to the scalp mesh.
%
% The return vertices of the scalp that are nearest to
% the electrodes are returned in the following:
%
% p.mesh.data.meshtype{p.mesh.current}
% p.mesh.data.vertices{p.mesh.current}
% p.mesh.data.faces{p.mesh.current}
%
% where p.mesh.current is also returned.
%
% See also: fiducial_coregister
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:57 $

% Licence:  GNU GPL, no express or implied warranties
% History:  04/2002, Darren.Weber_at_radiology.ucsf.edu
%           09/2002, Darren.Weber_at_radiology.ucsf.edu
%                    - bug fixing location of scalp mesh vertices
%                    that correspond to electrode vertices.
%                    - Still working with example dataset, not
%                    a genuine coregistration algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% debugging on/off
global DB;
DB = 0;
test = 0;  % coregistration testing

fprintf('MESH_FIT_ELEC: In development, not yet a true coregistration\n');

p.mesh.current = mesh_check(p,'scalp');

if isempty(p.mesh.current),
    error('MESH_FIT_ELEC: No ''scalp'' mesh in p struct.\n');
end

% Extract scalp triangulation
FV.vertices = p.mesh.data.vertices{p.mesh.current};
FV.faces    = p.mesh.data.faces{p.mesh.current};

if isempty(p.elec.data),
    error('MESH_FIT_ELEC: ''p.elec.data'' is empty.\n');
else
    x = p.elec.data.x;
    y = p.elec.data.y;
    z = p.elec.data.z;
end

fprintf('MESH_FIT_ELEC: ''Coregistering'' Electrodes...');
tic

if test,
	% Electrode fiducial points
	En = p.elec.data.nasion;
	Er = p.elec.data.rpa;
	El = p.elec.data.lpa;
	reg.fiducials.head = [ En; Er; El ];

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% NEED AN INTERACTIVE PROCESS TO SELECT THESE
	% Mesh fiducial points
	if isempty(p.mri.fiducials),
        if ~isempty(p.mri.data),
            fprintf('\n\nMESH_FIT_ELEC: Select Fiducial Points in MRI...\n\n');
            avw_view(p.mri.data);
            waitfor(gcf);
            p.mri.fiducials = evalin('base',p.mri.fiducials);
		else
            fprintf('\n\nMESH_FIT_ELEC: Select Fiducial Points in MRI...\n\n');
           [p] = mri_open(p);
            avw_view(p.mri.data);
            waitfor(gcf);
            p.mri.fiducials = evalin('base',p.mri.fiducials);
		end
	end
	reg.fiducials.mri = p.mri.fiducials;

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	T = fiducial_coregister(reg.fiducials.head,reg.fiducials.mri);

    E = [p.elec.data.x p.elec.data.y p.elec.data.z];
    E2MRI = [E,ones(size(E,1),1)] * T;
end

% perform least squares fit of elec to scalp???
%options = optimset('fminsearch');
%[sumd,fval,exitflag,output] = fminsearch('mesh_fit_elec_optim',...
%                              0, options, vertices, [x,y,z]);
%fprintf('\n%s%f\n', 'Iterations = ', output.iterations);
%fprintf('\n%s%d\n', 'Exit = ', exitflag);

% Try to adjust the height of the electrodes to
% those of the scalp mesh - really need a full
% translation/rotation/scaling least squares solution
elecmax = max(z);
vertmax = max(FV.vertices(:,3));
z = z + (vertmax - elecmax);

if DB,
    figure; plot3(x,y,z,'ro'); hold on;
    patch('vertices',FV.vertices,'faces',FV.faces,...
          'FaceColor',[0.0 0.0 0.6],'Edgecolor',[.6 .6 .6]);
    set(gca,'Projection','perspective');
    set(gca,'DataAspectRatio',[1 1 1]);
    axis off tight vis3d
end

% Find nearest scalp vertices to electrode positions.
% In the process, reorder the scalp mesh vertices
% and correct the scalp mesh faces so that all scalp
% vertices that lie near the electrodes
% are numbered 1-N, in the same order as the
% electrodes.  This is done here so that the
% mesh_laplacian_interp function will work
% correctly.

% This mess might be better handled by adding
% electrode vertices into the scalp mesh, rather
% than finding the scalp mesh vertices nearest
% to those of the electrodes.  However, this would
% require retesselation of the scalp mesh, which
% is a tricky process

[FV,k] = mesh_nearest_reorder(FV,[x,y,z]);

% Assign the adjusted triangulation back
% into the original structure
p.mesh.data.vertices{p.mesh.current} = FV.vertices;
p.mesh.data.faces{p.mesh.current}    = FV.faces;

p.mesh.data.elecindex{p.mesh.current} = k;

x = FV.vertices(k,1);
y = FV.vertices(k,2);
z = FV.vertices(k,3);

% A quick triangulation of the scalp vertices
% that lie nearest to the electrode vertices
Efaces = convhulln(FV.vertices(k,:));

% Now store the vertices
[p.mesh.current,meshExists] = mesh_check(p,'elec');

p.mesh.data.meshtype{p.mesh.current} = 'elec';
p.mesh.data.vertices{p.mesh.current} = [x,y,z];
p.mesh.data.faces{p.mesh.current} = Efaces;

t = toc; fprintf('done (%6.2f sec).\n',t);

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [FV,k] = mesh_nearest_reorder(FV,elecverts)

    % Find the indices of the nearest vertices in
    % FV to those in vertices
    k = dsearchn(FV.vertices,elecverts);

    if size(k,1) ~= size(elecverts,1),
        msg = sprintf('Sorry, duplicate vertices found in electrode/mesh coregistration');
        error(msg);
    end

    % Debug visualization
    global DB;
    if DB,
        figure; hold on;
        patch('vertices',FV.vertices,'faces',FV.faces,...
              'FaceColor',[0.8 0.8 0.8],'Edgecolor',[.6 .6 .6]);
        set(gca,'Projection','perspective');
        set(gca,'DataAspectRatio',[1 1 1]);
        axis off tight vis3d
    end

    % OK, now reorder the triangulation in FV
    % so that all the mesh vertices are ordered
    % 1:N

    Nelec = size(elecverts,1);

    for Eindex = 1:Nelec,

        % It's important in this process that the
        % mesh indices are found over again, in case
        % they are modified for electrode Eindex-X
        elecpos = elecverts(Eindex,:);
        Kindex = dsearchn(FV.vertices,elecpos);

        if Eindex == Kindex,
            continue;  % great, do nothing
        end

        % Get vertex coordinates at Eindex
        Evertex = FV.vertices(Eindex,:);
        % Get vertex coordinates at Kindex
        Kvertex = FV.vertices(Kindex,:);

        if DB,
            fprintf('Fitting Electrode: %3d\n',Eindex);
            H1 = plot3(Evertex(1),Evertex(2),Evertex(3),'ro');
            H2 = plot3(elecpos(1),elecpos(2),elecpos(3),'rd');
            H3 = plot3(Kvertex(1),Kvertex(2),Kvertex(3),'bo');
            pause
            delete(H1)
            delete(H2)
            delete(H3)
        end

        % Now swap them
        FV.vertices(Kindex,:) = Evertex;
        FV.vertices(Eindex,:) = Kvertex;

        % Swapping the vertices requires
        % rearranging the faces

        % find all faces that contain Vindex
        EfaceIndices = find(FV.faces == Eindex);
        % find all faces that contain Kindex
        KfaceIndices = find(FV.faces == Kindex);

        % Where faces refer to Kvertex, make them
        % refer to Vvertex and vice versa
        FV.faces(KfaceIndices) = Eindex;
        FV.faces(EfaceIndices) = Kindex;

        % Debug testing....
        if DB,
            % Get vertex coordinates at Vindex
            Evertex = FV.vertices(Eindex,:);
            % Get vertex coordinates at Kindex
            Kvertex = FV.vertices(Kindex,:);
            H1 = plot3(Evertex(1),Evertex(2),Evertex(3),'ro');
            H2 = plot3(elecpos(1),elecpos(2),elecpos(3),'rd');
            H3 = plot3(Kvertex(1),Kvertex(2),Kvertex(3),'bo');
            pause

            delete(H1)
            delete(H2)
            delete(H3)
        end

    end

    % Now the nearest vertices in FV to vertices
    % should be numbered 1:Nelec
    k = dsearchn(FV.vertices,elecverts);

    if ~isequal(k,[1:Nelec]'),
        msg = sprintf('MESH_FIT_ELEC: Mesh vertices not in order of electrodes!');
        warning(msg);
    end

return


function score = Task1_Min_value(Population,~)
% <min> <single> <real/integer/label/binary/permutation> <large/none> <constrained/none> <expensive/none> <sparse/none> <multitask>
% The minimum objective value of the first task (for multitask optimization)

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    PopDec = Population.decs;
    score  = Population(PopDec(:,end)==1).best.objs;
    if isempty(score)
        score = nan;
    end
end


function result = nii_read_volume(info)
% function for reading volume of NifTi ( .nii ) volume file
% nii_read_header(file-info)
%
% volume = nii_read_volume(file-header)
%
% examples:
% 1: info = nii_read_header()
%    V = nii_read_volume(info);
%    imshow(squeeze(V(:,:,round(end/2))),[]);
%
% 2: V = nii_read_volume('test.nii');

if(~isstruct(info)) info=nii_read_header(info); end

% Open v3d file
fid=fopen(info.Filename,'rb');

  % Seek volume data start
  datasize=prod(info.Dimensions)*(info.BitVoxel/8);
  fsize=info.Filesize;
  fseek(fid,fsize-datasize,'bof');

  % Read Volume data
  switch(info.DataTypeStr)
      case 'INT8'
        V = int8(fread(fid,datasize,'int8'));
      case 'UINT8'
        V = uint8(fread(fid,datasize,'uint8'));
      case 'INT16'
        V = int16(fread(fid,datasize,'int16'));
      case 'UINT16'
        V = uint16(fread(fid,datasize,'uint16'));
      case 'INT32'
        V = int32(fread(fid,datasize,'int32'));
      case 'UINT32'
        V = uint32(fread(fid,datasize,'uint32'));
      case 'INT64'
        V = int64(fread(fid,datasize,'int64'));
      case 'UINT64'
        V = uint64(fread(fid,datasize,'uint64'));
      case 'FLOAT'
        V = single(fread(fid,datasize,'single'));
      otherwise
        V = uint8(fread(fid,datasize,'uint8'));
  end
fclose(fid);

% Reshape the volume data to the right dimensions
result = reshape(V,info.Dimensions(1:3));


% This file is part of TREEQSM.
%
% TREEQSM 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.
%
% TREEQSM 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 TREEQSM.  If not, see <http://www.gnu.org/licenses/>.

function [Curve,Ind] = boundary_curve(P,Curve0,rball,dmax)

% ---------------------------------------------------------------------
% BOUNDARY_CURVE.M      Determines the boundary curve based on the
%                           previously defined boundary curve.
%
% Version 1.1.0
% Latest update     3 May 2022
%
% Copyright (C) 2015-2022 Pasi Raumonen
% ---------------------------------------------------------------------
%
% Inputs:
% P         Point cloud of the cross section
% Curve0     Seed points from previous cross section curve
% rball     Radius of the balls centered at seed points
% dmax      Maximum distance between concecutive curve points, if larger,
%               then create a new one between the points
% ---------------------------------------------------------------------

% Changes from version 1.0.0 to 1.1.0, 3 May 2022:
% 1) Increased the cubical neighborhood in the generation of the segments

%% Partition the point cloud into cubes
Min = double(min([P(:,1:2); Curve0(:,1:2)]));
Max = double(max([P(:,1:2); Curve0(:,1:2)]));
N = double(ceil((Max-Min)/rball)+5);
% cube coordinates of the section points
CC = floor([P(:,1)-Min(1) P(:,2)-Min(2)]/rball)+3;
% Sorts the points according a lexicographical order
S = [CC(:,1) CC(:,2)-1]*[1 N(1)]';
[S,I] = sort(S);
% Define "partition"
np = size(P,1);
partition = cell(N(1),N(2));
p = 1;              % The index of the point under comparison
while p <= np
  t = 1;
  while (p+t <= np) && (S(p) == S(p+t))
    t = t+1;
  end
  q = I(p);
  partition{CC(q,1),CC(q,2)} = I(p:p+t-1);
  p = p+t;
end

%% Define segments using the previous points
% cube coordinates of the seed points:
CC = floor([Curve0(:,1)-Min(1) Curve0(:,2)-Min(2)]/rball)+3;
I = CC < 3;
CC(I) = 3;
nc = size(Curve0,1);  % number of sets
Dist = 1e8*ones(np,1);  % distance of point to the closest center
SoP = zeros(np,1);  % the segment the points belong to
Radius = rball^2;
for i = 1:nc
  points = partition(CC(i,1)-2:CC(i,1)+2,CC(i,2)-2:CC(i,2)+2);
  points = vertcat(points{:});
  V = [P(points,1)-Curve0(i,1) P(points,2)-Curve0(i,2)];
  dist = sum(V.*V,2);
  PointsInBall = dist < Radius;
  points = points(PointsInBall);
  dist = dist(PointsInBall);
  D = Dist(points);
  L = dist < D;
  I = points(L);
  Dist(I) = dist(L);
  SoP(I) = i;
end

%% Finalise the segments
% Number of points in each segment and index of each point in its segment
Num = zeros(nc,1);
IndPoints = zeros(np,1);
for i = 1:np
  if SoP(i) > 0
    Num(SoP(i)) = Num(SoP(i))+1;
    IndPoints(i) = Num(SoP(i));
  end
end
% Continue if enough non-emtpy segments
if nnz(Num) > 0.05*nc
  % Initialization of the "Seg"
  Seg = cell(nc,1);
  for i = 1:nc
    Seg{i} = zeros(Num(i),1);
  end
  % Define the "Seg"
  for i = 1:np
    if SoP(i) > 0
      Seg{SoP(i),1}(IndPoints(i),1) = i;
    end
  end

  %% Define the new curve points as the average of the segments
  Curve = zeros(nc,3);  % the new boundary curve
  Empty = false(nc,1);
  for i = 1:nc
    S = Seg{i};
    if ~isempty(S)
      Curve(i,:) = mean(P(S,:),1);
      if norm(Curve(i,:)-Curve0(i,:)) > 1.25*dmax
        Curve(i,:) = Curve0(i,:);
      end
    else
      Empty(i) = true;
    end
  end

  %% Interpolate for empty segments
  % For empty segments create points by interpolation from neighboring
  % non-empty segments
  if any(Empty)
    for i = 1:nc
      if Empty(i)
        if i > 1 && i < nc
          k = 0;
          while i+k <= nc && Empty(i+k)
            k = k+1;
          end
          if i+k <= nc
            LineEle = Curve(i+k,:)-Curve(i-1,:);
          else
            LineEle = Curve(1,:)-Curve(i-1,:);
          end
          if k < 5
            for j = 1:k
              Curve(i+j-1,:) = Curve(i-1,:)+j/(k+1)*LineEle;
            end
          else
            Curve(i:i+k-1,:) = Curve0(i:i+k-1,:);
          end
        elseif i == 1
          a = 0;
          while Empty(end-a)
            a = a+1;
          end
          b = 1;
          while Empty(b)
            b = b+1;
          end
          LineEle = Curve(b,:)-Curve(nc-a,:);
          n = a+b-1;
          if n < 5
            for j = 1:a-1
              Curve(nc-a+1+j,:) = Curve(nc-a,:)+j/n*LineEle;
            end
            for j = 1:b-1
              Curve(j,:) = Curve(nc-a,:)+(j+a-1)/n*LineEle;
            end
          else
            Curve(nc-a+2:nc,1:2) = Curve0(nc-a+2:nc,1:2);
            Curve(nc-a+2:nc,3) = Curve0(nc-a+2:nc,3);
            Curve(1:b-1,1:2) = Curve0(1:b-1,1:2);
            Curve(1:b-1,3) = Curve0(1:b-1,3);
          end
        elseif i == nc
          LineEle = Curve(1,:)-Curve(nc-1,:);
          Curve(i,:) = Curve(nc-1,:)+0.5*LineEle;
        end
      end
    end
  end

  % Correct the height
  Curve(:,3) = min(Curve(:,3));

  % Check self-intersection
  [Intersect,IntersectLines] = check_self_intersection(Curve(:,1:2));

  % If self-intersection, try to modify the curve
  j = 1;
  while Intersect && j <= 5
    n = size(Curve,1);
    InterLines = (1:1:n)';
    NumberOfIntersections = cellfun('length',IntersectLines(:,1));
    I = NumberOfIntersections > 0;
    InterLines = InterLines(I);
    CrossLen = vertcat(IntersectLines{I,2});
    if length(CrossLen) == length(InterLines)
      LineEle = Curve([2:end 1],:)-Curve(1:end,:);
      d = sqrt(sum(LineEle.*LineEle,2));
      m = length(InterLines);
      for i = 1:2:m
        if InterLines(i) ~= n
          Curve(InterLines(i)+1,:) = Curve(InterLines(i),:)+...
            0.9*CrossLen(i)/d(InterLines(i))*LineEle(InterLines(i),:);
        else
          Curve(1,:) = Curve(InterLines(i),:)+...
            0.9*CrossLen(i)/d(InterLines(i))*LineEle(InterLines(i),:);
        end
      end
      [Intersect,IntersectLines] = check_self_intersection(Curve(:,1:2));
      j = j+1;
    else
      j = 6;
    end
  end

  %% Add new points if too large distances
  LineEle = Curve([2:end 1],:)-Curve(1:end,:);
  d = sum(LineEle.*LineEle,2);
  Large = d > dmax^2;
  m = nnz(Large);
  if m > 0
    Curve0 = zeros(nc+m,3);
    Ind = zeros(nc+m,2);
    t = 0;
    for i = 1:nc
      if Large(i)
        t = t+1;
        Curve0(t,:) = Curve(i,:);
        if i < nc
          Ind(t,:) = [i i+1];
        else
          Ind(t,:) = [i 1];
        end
        t = t+1;
        Curve0(t,:) = Curve(i,:)+0.5*LineEle(i,:);
        if i < nc
          Ind(t,:) = [i+1 0];
        else
          Ind(t,:) = [1 0];
        end
      else
        t = t+1;
        Curve0(t,:) = Curve(i,:);
        if i < nc
          Ind(t,:) = [i i+1];
        else
          Ind(t,:) = [i 1];
        end
      end
    end
    Curve = Curve0;

  else
    Ind = [(1:1:nc)' [(2:1:nc)'; 1]];
  end

  %% Remove new points if too small distances
  nc = size(Curve,1);
  LineEle = Curve([2:end 1],:)-Curve(1:end,:);
  d = sum(LineEle.*LineEle,2);
  Small = d < (0.333*dmax)^2;
  m = nnz(Small);
  if m > 0
    for i = 1:nc-1
      if ~Small(i) && Small(i+1)
        Ind(i,2) = -1;
      elseif Small(i) && Small(i+1)
        Small(i+1) = false;
      end
    end
    if ~Small(nc) && Small(1)
      Ind(nc,2) = -1;
      Ind(1,2) = -1;
      Small(1) = false;
      Small(nc) = true;
      I = Ind(:,2) > 0;
      Ind(2:end,1) = Ind(2:end,1)+1;
      Ind(I,2) = Ind(I,2)+1;

    end
    Ind = Ind(~Small,:);
    Curve = Curve(~Small,:);
  end

else
  % If not enough new points, return the old curve
  Ind = [(1:1:nc)' [(2:1:nc)'; 1]];
  Curve = Curve0;
end


% [MIN, MAX] = range2(MTX)
%
% Compute minimum and maximum values of MTX, returning them as a 2-vector.

% Eero Simoncelli, 3/97.

function [mn, mx] = range2(mtx)

%% NOTE: THIS CODE IS NOT ACTUALLY USED! (MEX FILE IS CALLED INSTEAD)

% fprintf(1,'WARNING: You should compile the MEX code for "range2", found in the MEX subdirectory.  It is MUCH faster.\n');

if (~isreal(mtx))
  error('MTX must be real-valued');
end

% mn = min(min(mtx));
% mx = max(max(mtx));
mn = min(mtx(:));
mx = max(mtx(:));


function q = conjexp(p)

% conjexp
%
%   1/p+1/q=1

q = p/(p-1);
if p==1
    q = Inf;
end
if p==Inf
    q = p;
end

end


function [f, mergedPts] = merge(f, index, pref)
%MERGE   Remove unnecessary breakpoints in from a CHEBFUN.
%   F = MERGE(F, PREF) removes unnecessary breakpoints from a CHEBFUN F. In
%   particular the kth breakpoint is removed if the resulting FUN on the
%   interval [x_{k-1}, x_{k+1}] can be represented to the same accuracy as F
%   with a fewer than PREF.MAXLENGTH points when PREF.SPLITTING = 0 and
%   PREF.SPLITPREFS.SPLITLENGTH points when PREF.SPLITTING = 1. If a PREF is
%   not passed, then the default CHEBFUN.PREF() is used.
%
%   [F, MERGEDPTS] = MERGE(F) returns the index of the merged endpoints in the
%   vector MERGEDPTS.
%
%   MERGE(F, INDEX) or MERGE(F, INDEX, PREF) attempts to eliminate the endpoints
%   specified in INDEX. MERGE(F, 'all') is equivalent to MERGE(F,
%   [2:length(F.domain)-1]). (Note that it doesn't make sense to consider
%   merging the first and final breakpoints.)
%
%   In all cases, elimination is attempted from left to right, and non-trivial
%   pointValues will prevent merging at the corresponding breakpoints.
%
%   Example:
%       f = chebfun(@(x) abs(x), 'splitting','on');
%       [g, mergedPts] = merge(f.^2);
%
% See also CHEBFUNPREF.

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

if ( numel(f) > 1 )
    % TODO:  Implement this.
    error('CHEBFUN:CHEBFUN:merge:quasi', ...
        'MERGE does not support quasimatrices.');
end

% Convert to a column CHEBFUN so that feval(f, x) returns a column instead of a
% row.  (Needed to construct a CHEBFUN out of @(x) feval(f, x) later on.)
isTrans = f.isTransposed;
if ( isTrans )
    f = f.';
end

% Parse the inputs:
if ( nargin == 1 )
    % Choose all indices by default:
    index = 2:numel(f.funs);
    % Obtain preferences:
    pref = chebfunpref();
elseif ( nargin == 2 )
    if ( isstruct(index) || isa(index, 'chebfunpref') ) % MERGE(F, PREF)
        % index actually is a struct of preferences or a CHEBFUNPREF
        pref = chebfunpref(index);
        % Choose all indices by default:
        index = 2:numel(f.funs);
    else                   % MERGE(F, INDEX)
        % indices passed, obtain preferences:
        pref = chebfunpref();
    end
end

[f, mergedPts] = mergeColumn(f, index, pref);

% Convert back to a row CHEBFUN if we started with one.
if ( isTrans )
    f.isTransposed = true;
end

end

function [f, mergedPts] = mergeColumn(f, index, pref)

% Deal with input arguments:
if ( isempty(index) )
    % No indices requested.
    mergedPts = [];
    return

elseif ( ischar(index) )
    % 'all' indices requested:
    index = 2:numel(f.funs);

else
    % Index of endpoints was provided:
    index = unique(index);
    % Break points must be in range 2:numel(f.funs)
    index((index <= 1) | (index > numel(f.funs))) = [];

    if ( isempty(index) )
        % All the requested indices were trivial.
        mergedPts = [];
        return
    end

end

% Determine the maximum length of the merged pieces:
if ( ~pref.splitting )
    maxn = pref.techPrefs.maxLength;
else
    maxn = pref.splitPrefs.splitLength;
end
pref.techPrefs.maxLength = maxn;

% Splitting forces extrapolate:
if ( pref.splitting )
    pref.techPrefs.extrapolate = true;
end

% Obtain scales of the CHEBFUN:
vs = vscale(f);
hs = hscale(f);
pref.chebfuneps = max(eps, pref.chebfuneps);
mergedPts = [];

% Store data from input CHEBFUN:
oldPointVals = f.pointValues;
oldDom = f.domain;
oldFuns = f.funs;
newPointVals = oldPointVals;
newDom = oldDom;
newFuns = oldFuns;

% Loop through the index:
for k = index
    % Find corresponding break:
    j = find(oldDom(k) == newDom, 1, 'first');
    % And lengths of funs on either side:
    lengthPrevFun = length(newFuns{j-1});
    lengthThisFun = length(newFuns{j});

    % Prevent merge if existing FUN lengths add to more than 1.2*maxn:
    if ( lengthPrevFun + lengthThisFun >= 1.2*maxn )
        % Skip to next breakpoint:
        continue
    end

    % Prevent merging if there are jumps:
    v = [oldPointVals(k,:); get(oldFuns{k-1}, 'rval'); get(oldFuns{k}, 'lval')];
    jumps = norm(v([1, 1],:) - v(2:3,:), inf)./vs;
    if ( all( jumps >= 1e3*pref.chebfuneps ) || any(isinf(v(:))) )
        % Skip to next breakpoint:
        continue
    end

    % Call merge at the FUN level:
    [mergedFun, ishappy] = merge(newFuns{j-1}, newFuns{j}, vs, hs, pref);

    % Prevent merge if the result is not happy:
    if ( ~ishappy )
        % Skip to next breakpoint:
        continue
    end

    % Merging successful!
    mergedPts = [mergedPts, k]; %#ok<AGROW> % Store index of the removed point.
    newFuns{j-1} = mergedFun;   % Insert new fun.
    newFuns(j) = [];            % Remove unneeded fun.
    newDom = [newDom(1:j-1), newDom(j+1:end)]; % Update domain.
    % Update pointValues.
    newPointVals = [newPointVals(1:j-1,:) ; newPointVals(j+1:end,:)];

end

% Assign data to CHEBFUN:
f.domain = newDom;
f.funs = newFuns;
f.pointValues = newPointVals;

end