function showPropulsionModel(propulsionModel)
%
% Shows propulsion model details: (See function 'TEST_definePropulsionModel.m')
%   - Location of propellers in aircraft body frame
%   - Plots for:
%       RPM vs throttle
%       Thrust vs RPM
%       Torque vs RPM
%
% Input:
%   propulsionModel (See function 'definePropulsionModel.m')
%
% Written by Conrad McGreal

%% plot propellers
n_motors = numel(propulsionModel) ;
legendcell = cell(n_motors,1) ;

%% Plot propulsion system geometry
figure
for i=1:n_motors
    d_prop = propulsionModel(i).d_prop ;
    loc = propulsionModel(i).thrustLocation ;
    ax = propulsionModel(i).thrustAxis ;
    plotPropLoc(d_prop, loc, ax) ;

    % labeling this motor
    x = loc(1) ;
    y = loc(2) ;
    z = loc(3) ;
    dx = 0.1 ; % label offset (from datapoint)
    dy = 0.1 ;

    this_label = strcat(['Motor ',num2str(i)]);
    legendcell{i} = this_label ;
    if propulsionModel(i).isSpinDirectionCCW
        dir_label = 'Spins CCW' ;
    else
        dir_label = 'Spins CW' ;
    end
    c1 = cellstr(this_label) ; % make it a cell
    c2 = cellstr(dir_label) ;
    text(x+dx, y+dy,z, c1) ; % print label to figure
    text(x+dx, y, z, c2) ; % print label to figure
end
title('Propeller Locations - Top view')
xlabel('x position [m]'); ylabel('y position [m]'); zlabel('z position [m]');
axis equal
drawnow

%% Plot performance; i.e. Thrust, Torque, & RPM vs throttle
figure
rho = 1.225 ; % set default air density

for i = 1:n_motors
motorid = i ; % select motor to plot for; at the moment only plots figure for one motor
RPM = 0:100:propulsionModel(motorid).maxRPM ;
throttle = linspace(0,1,numel(RPM)) ;
d_prop = propulsionModel(motorid).d_prop ;
C_t = propulsionModel(motorid).C_t ;
C_q = propulsionModel(motorid).C_q ;
[thrust, torque] = computePropOpPoint(RPM, rho, d_prop, C_t, C_q) ;

% plot it
ax1 = subplot(3,1,1) ;
plot(throttle,RPM); grid on; hold on;
xlabel('throttle []'); ylabel('RPM');

subplot(3,1,2)
plot(RPM,thrust); grid on; hold on;
xlabel('RPM'); ylabel('thrust [N]');

subplot(3,1,3)
plot(RPM,torque); grid on; hold on;
xlabel('RPM'); ylabel('torque [Nm]');
end
titleCell = 'motor model performance details' ;
title(ax1, titleCell)
legend(ax1, legendcell,'location','southeast')
drawnow


%> @brief struct_permute Generate a set of full factorial permutations
%> based on arguments given as fields in a structure.
%> Meant to be used stand-alone or in conjunction with struct_overlay()
%> @note Derived from optPermute() on Mathworks
%> @author Eric Cousineau <eacousineau@gmail.com>
%> @example ???
%>
%> 	opt_values = struct('mass', {{5, 10, 15}}, 'pos', {{1, 2}}, 'extra',
%> 	struct('x', {{2, 3}}));
%> 	opt_set = struct_permute(opt_values);
function [opt_set] = struct_permute(opt_values)
	opt_set = {};

	%Test overlay?

	% Full factorial experiment
	fields = fieldnames(opt_values);
	nfields = length(fields);

	% Start recursion
	opt_blank = struct();
	do_recursion(opt_blank, 1);

	function do_recursion(opt, index)
		if index <= nfields
			field = fields{index};
			value = opt_values.(field);
			if iscell(value)
				% Loop through, concatenate
				for i = 1:length(value)
					opt.(field) = value{i};
					% Recurse through the rest of the options
					do_recursion(opt, index + 1);
				end
			elseif isstruct(value)
				% Recurse with a new set of options for that given option...
				% Get the set of options for that set
				% Incorporate these into permutations
				opt_subset = struct_permute(value);
				% And like normal
				for i = 1:length(opt_subset)
					opt.(field) = opt_subset{i};
					% And so on
					do_recursion(opt, index + 1);
				end
			end
		else
			% End of permute
			% Add final options to the list of permutations
			opt_set{end + 1} = opt;
		end
	end
end


function c=comp_filterbank(f,g,a);
%COMP_FILTERBANK  Compute filtering
%
%   Function groups filters in g according to a presence of .h and .H
%   fields. If .H is present, it is further decided whether it is a full
%   frequency response or a band-limited freq. resp.
%

[L,W]=size(f);
M=numel(g);
c = cell(M,1);

% Divide filters into time domain and frequency domain groups
mFreq = 1:M;
mTime = mFreq(cellfun(@(gEl) isfield(gEl,'h') ,g)>0);
mFreq(mTime) = [];

if ~isempty(mTime)
   % Pick imp. resp.
   gtime = cellfun(@(gEl) gEl.h, g(mTime),'UniformOutput',0);

   % Call the routine
   gskip = cellfun(@(gEl) gEl.offset ,g(mTime));
   c(mTime) = comp_filterbank_td(f,gtime,a(mTime),gskip,'per');
end

if ~isempty(mFreq)
   % Filtering in the frequency domain
   F=fft(f);
   % Pick frequency domain filters
   gfreq = g(mFreq);
   % Divide filters into the full-length and band-limited groups
   mFreqFullL = 1:numel(gfreq);
   amFreqCell = mat2cell(a(mFreq,:).',size(a,2),ones(1,numel(mFreq)));
   mFreqBL = mFreqFullL(cellfun(@(gEl,aEl) numel(gEl.H)~=L || (numel(aEl)>1 && aEl(2) ~=1), gfreq(:),amFreqCell(:))>0);
   mFreqFullL(mFreqBL) = [];

   mFreqFullL = mFreq(mFreqFullL);
   mFreqBL = mFreq(mFreqBL);

   if ~isempty(mFreqFullL)
      G = cellfun(@(gEl) gEl.H, g(mFreqFullL),'UniformOutput',0);
      c(mFreqFullL) = comp_filterbank_fft(F,G,a(mFreqFullL));
   end

   if ~isempty(mFreqBL)
      G = cellfun(@(gEl) gEl.H, g(mFreqBL),'UniformOutput',0);
      foff = cellfun(@(gEl) gEl.foff, g(mFreqBL));
      % Cast from logical to double.
      realonly = cellfun(@(gEl) cast(isfield(gEl,'realonly') && gEl.realonly,'double'), g(mFreqBL));
      c(mFreqBL) = comp_filterbank_fftbl(F,G,foff,a(mFreqBL,:),realonly);
   end
end


classdef ttopKLogits < matlab.unittest.TestCase
    % ttopKLogits   Unit tests for sampling.topKLogits

    % Copyright 2020 The MathWorks, Inc.

    methods(Test)
        function testForKIsOne(test)
            % When K=1 topKLogits followed by softmax is just the one-hot
            % for the argmax.
            numClasses = 100;
            % Make an arbitrary class the largest logit.
            class = randi([1,numClasses]);
            x = rand([numClasses,1]);
            x(class) = max(x,[],'all')+1;
            % Call topKLogits with K=1
            topK = 1;
            logits = sampling.topKLogits(x,topK);
            logits = dlarray(logits);
            % Apply softmax to get a probability distribution over the
            % classes.
            actProb = softmax(logits,'DataFormat','C');
            % The expected output is a delta distribution on the arbitrary
            % class.
            expProb = zeros([numClasses,1]);
            expProb(class) = 1;
            test.verifyEqual(extractdata(actProb),expProb);
        end

        function testForLargeK(test)
            % When K>1 the topKLogits followed by softmax are only
            % non-zero at the largest K classes. If those classes have
            % equal logits the distribution is uniform.
            numClasses = 100;
            K = 10;
            classes = (1:K)+randi([1,numClasses-K]);
            x = rand([numClasses,1]);
            x(classes) = max(x,[],'all')+1;
            logits = sampling.topKLogits(x,K);
            logits = dlarray(logits);
            actProb = softmax(logits,'DataFormat','C');
            expProb = zeros([numClasses,1]);
            expProb(classes) = 1/K;
            test.verifyEqual(extractdata(actProb),expProb);
        end

        function dlarrayIsSupported(test)
            % Test we can call topKLogits on a dlarray.
            numClasses = 10;
            k = 5;
            x = dlarray(rand([numClasses,1]));
            yAct = sampling.topKLogits(x,k);
            yExp = sampling.topKLogits(extractdata(x),k);
            test.verifyEqual(extractdata(yAct),yExp);
        end
    end

end


function comp_random_grlex_test ( )

%*****************************************************************************80
%
%% COMP_RANDOM_GRLEX_TEST tests COMP_RANDOM_GRLEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'COMP_RANDOM_GRLEX_TEST\n' );
  fprintf ( 1, '  A COMP is a composition of an integer N into K parts.\n' );
  fprintf ( 1, '  Each part is nonnegative.  The order matters.\n' );
  fprintf ( 1, '  COMP_RANDOM_GRLEX selects a random COMP in\n' );
  fprintf ( 1, '  graded lexicographic (grlex) order between indices RANK1 and RANK2.\n' );
  fprintf ( 1, '\n' );

  kc = 3;
  rank1 = 20;
  rank2 = 60;
  seed = 123456789;

  for test = 1 : 5
    [ xc, rank, seed ] = comp_random_grlex ( kc, rank1, rank2, seed );
    nc = sum ( xc(1:kc) );
    fprintf ( 1, '   %3d: ', rank );
    fprintf ( 1, '    %2d = ', nc );
    for j = 1 : kc - 1
      fprintf ( 1, '%2d + ', xc(j) );
    end
    fprintf ( 1, '%2d\n', xc(kc) );
  end

  return
end


function movePTPEllipse_XY_1(iiwa,c,ratio,theta,velocity,accel,TefTool)
%% This funciton is used to
% Move end-effector of the robot on an ellipse the plane of the ellipse is
% parallel to the XY plane of the robot base.

%% Syntax:
% movePTPEllipse_XY(iiwa,c,ratio,theta,velocity,accel,TefTool)

%% Arreguments:
% iiwa: KST obejct.
% c: 2x1 vector,  is a vector defining the displacment of the the center
% point of the ellipse in the XY plane in relation to the start point of the ellipse,
% position units in (mm).
% ratio: the radious ratio (a/b) of the ellipse.
% theta: is the angle of the ellipe part , for drawing a complete
% ellipse this angle is equal to 2*pi.
% TefTool: 4X4 Transform matrix of the end-efector in the reference frame
% of the flange of the KUKA iiwa, position units in (mm).
% accel: is the acceleration (mm/sec2).
% velocity: is the motion velocity (mm/sec).

% Copy right: Mohammad SAFEEA
% 27th-June-2018

%% Convert inputs to a column vectors
c=colVec(c);
ratio=colVec(ratio);
theta=colVec(theta);
velocity=colVec(velocity);
accel=colVec(accel);

%% Check input variables
if(size(c,1)~=2)
fprintf('Error: the vector (c) shall be a 2x1 vector \n');
return;
end

if(size(ratio,1)~=1)
fprintf('Error: the ratio shall be a scalar  \n');
return;
end

if(size(theta,1)~=1)
fprintf('Error: the variable (theta) shall be a scalar  \n');
return;
end

if(size(TefTool,1)~=4)
fprintf('Error: the transform matrix (TefTool) shall be 4x4  \n');
return;
end

if(size(TefTool,2)~=4)
fprintf('Error: the transform matrix (TefTool) shall be 4x4  \n');
return;
end

if(size(velocity,1)~=1)
fprintf('Error: the velocity on the path shall be a scalar  \n');
return;
end

if(size(accel,1)~=1)
fprintf('Error: the acceleration on the path shall be a scalar  \n');
return;
end

dir=[1;0;0];
c=[c;
    0];

% perform the elliptical motion
movePTPEllipse_1(iiwa,c,dir,ratio,theta,velocity,accel,TefTool);
end

function y=colVec(x)
if size(x,2)==1
    y=x;
else
    y=x';
end
end


function [R, nt] = buildr(S, varargin)

%BUILDR Builds response matrix for input to functions ENTROPY and
% INFORMATION
%
%   ------
%   SYNTAX
%   ------
%
%       [R, nt] = build_R(S, R1, R2, ..., RL)
%
%   -----------
%   DESCRIPTION
%   -----------
%   Let's consider an experiment in which, during each trial, a stimulus is
%   presented out of NS available stimuli and L distinct neural responses
%   are recorded simultaneously. Within this framework the input to BUILDR
%   is as follow:
%   - S is the stimulus-array, i.e., S(i) stores the value of the stimulus
%     presented during the i-th trial.
%   - Rj, j=1,...,L, is the j-th response array, i.e., Rj(i) stores the
%     value of the j-th response recorded during the i-th experiment.
%
%   The function will return the response matrix R which can be input to
%   the routines ENTROPY and INFORMATION. The routine also outputs the
%   trials per stimulus array, NT, which can be used as one of the
%   parameters of the option structure for ENTROPY and INFORMATION.
%
%   -------
%   REMARKS
%   -------
%
%   - Although the one described above is the framework which was kept in
%     mind while creating the toolbox functions, it has to be noted that
%     this function (and also the other in the toolbox) can be easily
%     applied to several other situations.
%
%   - The goal of this function is to help the user get familiar with the
%     structure of the response-matrix R which is input to the ENTROPY and
%     INFORMATION functions and, in particular, with the fact that the
%     stimulus values are not provided to these functions. This is because,
%     for the sake of information computation the only important parameter
%     concerning the stimulus is the number of times each stimulus was
%     presented: the value of each stimulus can thus be mapped to an
%     integer value and made implicit as the index to a page of the
%     response matrix R. It needs to be noted, however, that BUILDR, having
%     to be as generic as possible, will also be relatively slow. Often,
%     the way responses are recorded or computed allows building a response
%     matrix much more quickly than the way done by BUILR. For
%     computationally intensive tasks it is thus suggested that custom
%     routines are used instead of this built-in tool.
%
%   See also ENTROPY, INFORMATION

%   Copyright (C) 2009 Cesare Magri
%   Version: 1.0.4

% -------
% LICENSE
% -------
% This software is distributed free under the condition that:
%
% 1. it shall not be incorporated in software that is subsequently sold;
%
% 2. the authorship of the software shall be acknowledged and the following
%    article shall be properly cited in any publication that uses results
%    generated by the software:
%
%      Magri C, Whittingstall K, Singh V, Logothetis NK, Panzeri S: A
%      toolbox for the fast information analysis of multiple-site LFP, EEG
%      and spike train recordings. BMC Neuroscience 2009 10(1):81;
%
% 3.  this notice shall remain in place in each source file.

if ~isvector(S)
    error('Stimulus array must be a 1-D array');
end

totNt = length(S);

L = length(varargin);

R1toL = zeros(L, totNt);
for k=1:L
    if ~isvector(varargin{k})
        msg = 'Response arrays must be 1-D.';
        error('buildr:respNot1D', msg);
    end

    if length(varargin{k}) ~= totNt
        msg = 'Each response-array must have the same length as the stimulus array';
        error('buildr:differentTotNt', msg);
    end

    R1toL(k,:) = varargin{k};
end

uniqueS = unique(S);
Ns = length(uniqueS);

% Dispalying informations:
disp('Building R and nt:');
disp(['- number of stimuli = ' num2str(Ns)]);
disp(['- number of responses = ' num2str(L)]);

nt = zeros(Ns,1);
tFlag = false(totNt, Ns);
for s=1:Ns
    tFlag(:,s) = S==uniqueS(s);
	nt(s) = sum(tFlag(:,s));
end

maxNt = max(nt);
disp(['- maximum numer of trials = ' num2str(maxNt)]);
disp(['- minimum numer of trials = ' num2str(min(nt))]);
R = zeros(L, maxNt, Ns);
for s=1:Ns
    if nt(s)>0
        R(:,1:nt(s), s) = R1toL(:, tFlag(:,s));
    else
        msg = 'One or more stimuli with no corresponding response.';
        error('buildr:noResponseStimulus', msg);
    end
end


% Copyright (C) 1994-2015 John W. Eaton
%
% This file is part of Octave.
%
% Octave 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.
%
% Octave 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

% -*- texinfo -*-
% @deftypefn  {Function File} {[@var{h}, @var{w}] =} freqz (@var{b}, @var{a}, @var{n}, "whole")
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@var{b})
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@var{b}, @var{a})
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@var{b}, @var{a}, @var{n})
% @deftypefnx {Function File} {@var{h} =} freqz (@var{b}, @var{a}, @var{w})
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@dots{}, @var{Fs})
% @deftypefnx {Function File} {} freqz (@dots{})
%
% Return the complex frequency response @var{h} of the rational IIR filter
% whose numerator and denominator coefficients are @var{b} and @var{a},
% respectively.
%
% The response is evaluated at @var{n} angular frequencies between 0 and
% @ifnottex
% 2*pi.
% @end ifnottex
% @tex
% $2\pi$.
% @end tex
%
% @noindent
% The output value @var{w} is a vector of the frequencies.
%
% If @var{a} is omitted, the denominator is assumed to be 1 (this
% corresponds to a simple FIR filter).
%
% If @var{n} is omitted, a value of 512 is assumed.  For fastest computation,
% @var{n} should factor into a small number of small primes.
%
% If the fourth argument, @qcode{'whole'}, is omitted the response is
% evaluated at frequencies between 0 and
% @ifnottex
% pi.
% @end ifnottex
% @tex
% $\pi$.
% @end tex
%
% @code{freqz (@var{b}, @var{a}, @var{w})}
%
% Evaluate the response at the specific frequencies in the vector @var{w}.
% The values for @var{w} are measured in radians.
%
% @code{[@dots{}] = freqz (@dots{}, @var{Fs})}
%
% Return frequencies in Hz instead of radians assuming a sampling rate
% @var{Fs}.  If you are evaluating the response at specific frequencies
% @var{w}, those frequencies should be requested in Hz rather than radians.
%
% @code{freqz (@dots{})}
%
% Plot the magnitude and phase response of @var{h} rather than returning them.
%
% @seealso{freqz_plot}
% @end deftypefn

% Author: jwe ???

function [h_r, f_r] = freqz (b, a, n, region, Fs)

  if (nargin < 1 || nargin > 5)
    print_usage ();
  elseif (nargin == 1)
    % Response of an FIR filter.
    a = [];
    n = [];
    region = [];
    Fs = [];
  elseif (nargin == 2)
    % Response of an IIR filter
    n = [];
    region = [];
    Fs = [];
  elseif (nargin == 3)
    region = [];
    Fs = [];
  elseif (nargin == 4)
    Fs = [];
    if (~ischar (region) && ~isempty (region))
      Fs = region;
      region = [];
    end
  end

  if (isempty (b))
    b = 1;
  elseif (~isvector (b))
    error ('freqz: B must be a vector');
  end
  if (isempty (a))
    a = 1;
  elseif (~isvector (a))
    error ('freqz: A must be a vector');
  end
  if (isempty (n))
    n = 512;
  elseif (isscalar (n) && n < 1)
    error ('freqz: N must be a positive integer');
  end
  if (isempty (region))
    if (isreal (b) && isreal (a))
      region = 'half';
    else
      region = 'whole';
    end
  end
  if (isempty (Fs))
    freq_norm = true;
    if (nargout == 0)
      Fs = 2;
    else
      Fs = 2*pi;
    end
  else
    freq_norm = false;
  end
  plot_output = (nargout == 0);
  whole_region = strcmp (region, 'whole');

  a = a(:);
  b = b(:);

  if (~isscalar (n))
    % Explicit frequency vector given
    w = n;
    f = n;
    if (nargin == 4)
      % Sampling rate Fs was specified
      w = 2*pi*f/Fs;
    end
    k = max (length (b), length (a));
    hb = polyval (oc_postpad (b, k), exp (j*w));
    ha = polyval (oc_postpad (a, k), exp (j*w));
  else
    % polyval(fliplr(P),exp(jw)) is O(p n) and fft(x) is O(n log(n)),
    % where p is the order of the polynomial P.  For small p it
    % would be faster to use polyval but in practice the overhead for
    % polyval is much higher and the little bit of time saved isn't
    % worth the extra code.
    k = max (length (b), length (a));
    if (k > n/2 && nargout == 0)
      % Ensure a causal phase response.
      n = n * 2 .^ ceil (log2 (2*k/n));
    end

    if (whole_region)
      N = n;
      if (plot_output)
        f = Fs * (0:n).' / N;    % do 1 more for the plot
      else
        f = Fs * (0:n-1).' / N;
      end
    else
      N = 2*n;
      if (plot_output)
        n = n+1;
      end
      f = Fs * (0:n-1).' / N;
    end

    pad_sz = N*ceil (k/N);
    b = oc_postpad (b, pad_sz);
    a = oc_postpad (a, pad_sz);

    hb = zeros (n, 1);
    ha = zeros (n, 1);

    for i = 1:N:pad_sz
      fftb = fft(oc_postpad (b(i:i+N-1), N));
      ffta = fft(oc_postpad (a(i:i+N-1), N));
      hb = hb + fftb(1:n);
      ha = ha + ffta(1:n);
    end

  end

  h = hb ./ ha;

  if (plot_output)
    % Plot and don't return values.
    if (whole_region)
      h(end+1) = h(1); % Solution is periodic.  Copy first value to end.
    end
    freqz_plot (f, h, freq_norm);
  else
    % Return values and don't plot.
    h_r = h;
    f_r = f;
  end

end


function sgmga_importance_to_aniso_test ( dim_num, importance )

%*****************************************************************************80
%
%% SGMGA_IMPORTANCE_TO_ANISO_TEST calls SGMGA_IMPORTANCE_TO_ANISO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 November 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SGMGA_IMPORTANCE_TO_ANISO_TEST\n' );
  fprintf ( 1, '  Importances:\n' );
  for dim = 1 : dim_num
    fprintf ( 1, '  %12f', importance(dim) );
  end
  fprintf ( 1, '\n' );

  level_weight = sgmga_importance_to_aniso ( dim_num, importance );

  fprintf ( 1, '  Anistropic coefficients:\n' );
  for dim = 1 : dim_num
    fprintf ( 1, '  %12f', level_weight(dim) );
  end
  fprintf ( 1, '\n' );

  return
end


function h = or(f, g)
%|   SINGFUN logical OR.
%   F | G performs a logical OR of the SINGFUN objects F and G and returns a
%   SMOOTHFUN containing elements set to either logical 1 (TRUE) or logical 0
%   (FALSE).  The output SMOOTHFUN is set to 1 if either input SINGFUN
%   contains a non-zero element at that point, otherwise it is set to 0.
%   F and G must either be identically zero or have roots in their domains.
%   If this is not the case, garbage is returned with no warning.

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

if ( isa(f, 'singfun') )
    f = f.smoothPart;
end

if ( isa(g, 'singfun') )
    g = g.smoothPart;
end

h = f | g;

end


function calpak_test622 ( )

%*****************************************************************************80
%
%% CALPAK_TEST622 tests YEAR_TO_DOMINICAL_JULIAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST622\n' );
  fprintf ( 1, '  For the Julian calendar,\n' );
  fprintf ( 1, '  YEAR_TO_DOMINICAL_JULIAN determines the dominical number.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Year  Dominical Number\n' );
  fprintf ( 1, '\n' );

  for y = 1577 : 1587

    s = y_to_s ( y );
    [ n1, n2 ] = year_to_dominical_julian ( y );
    d1 = i4_to_a ( n1 );

    if ( n1 == n2 )
      fprintf ( 1, '  %10s  %d  %s\n', s, n1, d1 );
    else
      d2 = i4_to_a ( n2 );
      fprintf ( 1, '  %10s  %d  %s  %d  %s\n', s, n1, d1, n2, d2 );
    end

  end

  return
end


function [nodes2, edges2] = grRemoveNodes(nodes, edges, rmNodes)
%GRREMOVENODES Remove several nodes in a graph.
%
%   usage:
%   [NODES2 EDGES2] = grRemoveNodes(NODES, EDGES, NODES2REMOVE)
%   remove the nodes with indices NODE2REMOVE from array NODES, and also
%   remove edges containing the nodes NODE2REMOVE.
%
%   Example
%     nodes = [...
%         10 10; 20 10; 30 10; ...
%         10 20; 20 20; 30 20];
%     edges = [...
%         1 2; 1 4; 1 5; ...
%         2 3; 2 5; 2 6; ...
%         3 6; 4 5; 5 6];
%     toRemove = [3 4];
%     [nodes2 edges2] = grRemoveNodes(nodes, edges, toRemove);
%     drawGraph(nodes2, edges2);
%     axis equal; axis([0 40 0 30]);
%
%   See also
%     grRemoveEdges
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-08-13
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

%% edges processing

% remove all edges connected to one of the nodes to remove
edges2 = edges(~any(ismember(edges, rmNodes), 2), :);

% change edges information, due to the node index shift
for i = 1:length(rmNodes)
    inds = edges2 > (rmNodes(i) - i + 1);
    edges2(inds) = edges2(inds) - 1;
end

%% nodes processing

% number of nodes
N   = size(nodes, 1);
NR  = length(rmNodes);
N2  = N-NR;

% allocate memory
nodes2 = zeros(N2, 2);

% process the first node
nodes2(1:rmNodes(1)-1,:) = nodes(1:rmNodes(1)-1,:);

for i = 2:NR
    inds = rmNodes(i-1)+1:rmNodes(i)-1;
    if isempty(inds)
        continue;
    end
    nodes2(inds - i + 1, :) = nodes(inds, :);
end

% process the last node
nodes2(rmNodes(NR)-NR+1:N2, :) = nodes(rmNodes(NR)+1:N, :);


function [tm,im] = mapvert(tr,pi)
%MAPVERT find the tree-to-vertex mappings.
%   [TM,IM] = MAPVERT(TR,PI) returns the tree-to-vertex and
%   vertex-to-tree mappings for a given aabb-tree TR and a
%   collection of query vertices PI.
%
%   The tree-to-item mapping TM is a structure representing
%   the intersection of the items PI with the tree TR. TM.II
%   is an M-by-1 array of tree indices and TM.LL is an
%   M-by-1 cell array of item lists. Specifically, items in
%   the list TM.LL{JJ} intersect with the node TM.II(JJ).
%
%   The item-to-tree mapping IM is a structure representing
%   the inverse mapping. IM.II is an N-by-1 array of item
%   indices and IM.LL is an N-by-1 cell array of node lists.
%   Specifically, nodes in the list IM.LL{JJ} intersect with
%   the item IM.II(JJ).
%
%   See also QUERYSET, MAPRECT, MAKETREE

%   Darren Engwirda : 2014 --
%   Email           : de2363@columbia.edu
%   Last updated    : 06/04/2017

%----------------------- call SCANTREE to do the actual work
    if (nargout == +1)
       [tm   ] = scantree(tr,pi,@partvert);
    else
       [tm,im] = scantree(tr,pi,@partvert);
    end

end

function [j1,j2] = partvert(pi,b1,b2)
%PARTVERT partition points between boxes B1,B2 for SCANTREE.

    j1 = true(size(pi,1),1);
    j2 = true(size(pi,1),1);

    nd = size(b1,2) / +2;

    for ax = +1 : nd
%--------------- remains TRUE if inside bounds along axis AX
    j1 = j1 & pi(:,ax)>=b1(ax+nd*0) ...
            & pi(:,ax)<=b1(ax+nd*1) ;

%--------------- remains TRUE if inside bounds along axis AX
    j2 = j2 & pi(:,ax)>=b2(ax+nd*0) ...
            & pi(:,ax)<=b2(ax+nd*1) ;
    end

end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcLoadSeg(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

[filename,pathname]=uigetfile({'*.mat','MAT-files (*.mat)'},'MultiSelect','off');
if filename==0
    return;
end
load([pathname filename]);
try
    if ~isequal(size(handles.Mask),size(Mask))
        error('Segmentation mask has different matrix size.');
    end
    handles.V.Segs=Segs;
    handles.Mask=Mask;
    handles=MU_update_image(handles.Matrix_display_axes,{handles.TMatrix,handles.Mask},handles,0);
catch me
    error_msg{1,1}='ERROR!!! Loading segmentation fails.';
    error_msg{2,1}=me.message;
    errordlg(error_msg);
    return;
end
guidata(handles.MU_matrix_display, handles);

end


%% Stereo Image Matching
%
% Example of stereo image matching to produce a disparity map and point cloud
% generation.
%
% Resulting .ply file can also be viewed using
% <http://meshlab.sourceforge.net/ MeshLab>.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/stereo_match.py>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/stereo_match.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/gpu/stereo_match.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/calib3d/stereoBM/SBM_Sample.cpp>
% * <https://docs.opencv.org/3.2.0/dd/d53/tutorial_py_depthmap.html>
%

%% Theory
%
% Previously, we saw basic concepts like epipolar constraints and other
% related terms. We also saw that if we have two images of same scene, we can
% get depth information from that in an intuitive way. Below is an image and
% some simple mathematical formulas which prove that intuition:
%
% <<https://docs.opencv.org/3.2.0/stereo_depth.jpg>>
%
% The above diagram contains equivalent triangles. Writing their equivalent
% equations will yield us following result:
%
% $$disparity = x - x' = \frac{Bf}{Z}$$
%
% $x$ and $x'$ are the distance between points in image plane corresponding to
% the scene point 3D and their camera center. $B$ is the distance between two
% cameras (which we know) and $f$ is the focal length of camera (already
% known). So in short, the above equation says that the depth of a point in a
% scene is inversely proportional to the difference in distance of
% corresponding image points and their camera centers. So with this
% information, we can derive the depth of all pixels in an image.
%
% So it finds corresponding matches between two images. We have already seen
% how epiline constraint make this operation faster and accurate. Once it
% finds matches, it finds the disparity.
%

%% Code

function stereo_match_demo()
    %% Images
    % load pair of images
    % (SGBM works with either grayscale or color images, BM only grayscale)
    imgL = cv.imread(fullfile(mexopencv.root(),'test','aloeL.jpg'), 'Color',true);
    imgR = cv.imread(fullfile(mexopencv.root(),'test','aloeR.jpg'), 'Color',true);
    subplot(121), imshow(imgL), title('Left')
    subplot(122), imshow(imgR), title('Right')

    %%
    % downscale for faster processing
    scale = 0.5;
    [imgL, imgR] = scale_images(imgL, imgR, scale);
    whos imgL imgR
    [h,w,cn] = size(imgL);

    %%
    % disparity-to-depth mapping 4x4 matrix, used to compute point cloud
    if true
        % manually enter Q matrix
        % (turns points 180 deg around x-axis, so that y-axis looks up)
        f = 0.8*w;  % guess for local length
        Q = [1 0 0 -0.5*w; 0 -1 0 0.5*h; 0 0 0 -f; 0 0 1 0];
    elseif false
        % images must be rectified, if not we rectify and get Q matrix
        % (requires calibrated stereo camera, see stereo_calibration_demo.m)
        intrinsicFile = fullfile(tempdir(), 'stereo_intrinsic.yml');
        extrinsicFile = fullfile(tempdir(), 'stereo_extrinsic.yml');
        [imgL, imgR, Q] = rectify_images(imgL, imgR, scale, ...
            intrinsicFile, extrinsicFile);
    else
        % when empty, point cloud generation is skipped
        Q = [];
    end
    display(Q)

    %% Stereo Matching
    % create stereo matcher, with params tuned for 'aloe' image pair
    win_size = 3;
    min_disp = 16;             % 0
    num_disp = 112 - min_disp; % fix(w/8) + 15
    num_disp = double(bitand(int32(num_disp), int32(-16))); % divisible by 16
    stereo = cv.StereoSGBM('MinDisparity',min_disp, 'NumDisparities',num_disp, ...
        'BlockSize',16, 'P1',8*cn*win_size^2, 'P2',32*cn*win_size^2, ...
        'Disp12MaxDiff',1, 'UniquenessRatio',10, ...
        'SpeckleWindowSize',100, 'SpeckleRange',32, 'Mode','SGBM');
    display(stereo)

    %%
    % compute disparity map (from a pair of rectified stereo images)
    tic, D = stereo.compute(imgL, imgR); toc
    fprintf('16-bit disparity map: min=%d, max=%d\n', min(D(:)), max(D(:)));
    D = single(D) / 16; % fixed-point numbers with 4 fractional bits -> float
    DD = (D - stereo.MinDisparity) / stereo.NumDisparities; % normalized [0,1]
    figure, imshow(DD), colorbar, title('Disparity')

    %% Point Cloud
    if ~isempty(Q)
        % generate 3d point cloud
        ptc = create_point_cloud(D, Q, imgL, false);
        fprintf('%d point cloud\n', ptc.Count);

        % write point cloud to PLY file
        plyfile = fullfile(tempdir(), 'aloe.ply');
        write_point_cloud(ptc, plyfile);
        fprintf('Point cloud saved to: %s\n', plyfile);

        % visualize point cloud
        if ~mexopencv.isOctave()
            %HACK: Octave hangs if we plot too many scatter points
            figure, vis_point_cloud(ptc);
            title('Point Cloud'); xlabel('X'); ylabel('Y'); zlabel('Z');
            %axis([-10 10 -10 10 -20 0])
        end
    end
end

%% Helper functions

function [imgL, imgR] = scale_images(imgL, imgR, scale)
    if scale ~= 1
        if scale < 1
            interpo = 'Area';
        else
            interpo = 'Cubic';
        end
        imgL = cv.resize(imgL, scale, scale, 'Interpolation',interpo);
        imgR = cv.resize(imgR, scale, scale, 'Interpolation',interpo);
    end
end

function [imgL, imgR, Q] = rectify_images(imgL, imgR, scale, intrinsicFile, extrinsicFile)
    % load params from previously calibrated stereo camera
    assert(exist(intrinsicFile, 'file') == 2, 'missing intrinsic params');
    assert(exist(extrinsicFile, 'file') == 2, 'missing extrinsic params');
    I = cv.FileStorage(intrinsicFile);  % intrinsic: M1, D1, M2, D2
    E = cv.FileStorage(extrinsicFile);  % extrinsic: R, T

    % account for new image size by scaling camera matrix: fx, fy, cx, cy
    I.M1 = I.M1 * scale;
    I.M2 = I.M2 * scale;

    % Note: we assume that cameras were calibrated using images of same size
    % as the original image size here, i.e: [I.width, I.height] == sz/scale)
    sz = [size(imgL,2) size(imgL,1)];

    % re-rectify using scaled image size
    RCT = cv.stereoRectify(I.M1, I.D1, I.M2, I.D2, sz, E.R, E.T);
    Q = RCT.Q;

    % apply rectification
    [map11, map12] = cv.initUndistortRectifyMap(I.M1, I.D1, sz, ...
        'P',RCT.P1, 'R',RCT.R1);
    [map21, map22] = cv.initUndistortRectifyMap(I.M2, I.D2, sz, ...
        'P',RCT.P2, 'R',RCT.R2);
    imgL = cv.remap(imgL, map11, map12, 'Interpolation','Linear');
    imgR = cv.remap(imgR, map21, map22, 'Interpolation','Linear');
end

function ptc = create_point_cloud(D, Q, imgL, cvst)
    xyz = cv.reprojectImageTo3D(D, Q);
    mask = repmat(D > min(D(:)), [1 1 3]);  % where disparity was not computed
    xyz = reshape(xyz(mask), [], 3);
    if size(imgL,3) == 3
        clr = reshape(imgL(mask), [], 3);
    else
        clr = repmat(imgL(mask(:,:,1)), 1, 3);
    end

    if nargin < 4, cvst = true; end
    if cvst && ~mexopencv.isOctave() && mexopencv.require('vision')
        ptc = pointCloud(xyz, 'Color',clr);
    else
        %HACK: pointCloud and related functions are not implemented in Octave
        ptc = struct('Location',xyz, 'Color',clr, 'Count',size(xyz,1));
    end
end

function vis_point_cloud(ptc)
    if isobject(ptc)
        pcshow(ptc);
    else
        scatter3(ptc.Location(:,1), ptc.Location(:,2), ptc.Location(:,3), ...
            6, single(ptc.Color)/255, '.')
        axis tight vis3d
        rotate3d on
    end
end

function write_point_cloud(ptc, fname)
    if isobject(ptc)
        pcwrite(ptc, fname, 'Encoding','ascii');
    else
        fid = fopen(fname, 'wt');
        fprintf(fid, 'ply\n');
        fprintf(fid, 'format ascii 1.0\n');
        fprintf(fid, 'element vertex %d\n', ptc.Count);
        fprintf(fid, 'property float x\n');
        fprintf(fid, 'property float y\n');
        fprintf(fid, 'property float z\n');
        fprintf(fid, 'property uchar red\n');
        fprintf(fid, 'property uchar green\n');
        fprintf(fid, 'property uchar blue\n');
        fprintf(fid, 'end_header\n');
        fprintf(fid, '%f %f %f %d %d %d\n', [ptc.Location single(ptc.Color)].');
        fclose(fid);
    end
end


%
% Calculates the time necessary for the calculation of the correlation dimension.
%
%
%
%
org = [2];
startfd;
radm=[];
rasm=[];
tim=[];
nvt=[];

for m= 250:250:3750

    E=ran(1:m,:);
    tic;
    pdc3nofig;
    tim=toc;
    nvt =[nvt; tim];
    E = ran;

end

figure;
ax = gca;
plot(m,nvt,'ko');
xlabel('Number of Points', 'fontsize',12);
ylabel('Seconds', 'fontsize',12);
Title('D-value Computation Time', 'fontsize',14);
set(ax,'fontsize',11);


%%
%
function [LT] = lraNTFbgmodel(T,lraNTF_opts)
  %% 4D TENSOR DEMO
  % inputPath = 'dataset/demo.avi';
  % [video nFrame vidWidth vidHeight] = load_input(inputPath);
  % T = im2double(video); size(T)

  %% 3D TENSOR DEMO
  % load('C:/ABSLibrary/dataset/trafficdb/trafficdb/traffic_patches.mat');
  % V = imgdb{100};
  % V = im2double(V);
  %T = tensor(V); size(T)

  %% Factorize subtensors with PARAFAC/CP algorithm with R = 1
  sT = size(T);
  blksize = 8;
  tdim = numel(size(T));

  if(tdim == 4)
    d = zeros(sT(4),prod(sT(1:2))/blksize^2);
    Tlen = size(T,4);
  end
  if(tdim == 3)
    d = zeros(sT(3),prod(sT(1:2))/blksize^2);
    Tlen = size(T,3);
  end

  kblk = 0;
  xy = zeros(1,2);
  %tic;
  displog('Factorizing subtensors with Parafac/CP algorithm');
  for xu = 1:8:sT(1)-7
    for yu = 1:8:sT(2)-7
      kblk = kblk+1;

      if(tdim == 4)
        Tblk = T(xu:xu+blksize-1, yu:yu+blksize-1,:,:);
        % show_4dvideo(Tblk);
        Yblk = permute(tensor(Tblk),[4 1 2 3]);
      end

      if(tdim == 3)
        Tblk = T(xu:xu+blksize-1, yu:yu+blksize-1,:);
        Yblk = permute(tensor(Tblk),[3 1 2]);
        % show_3dvideo(Tblk);
      end

      % lraNTF_opts = struct('NumOfComp',1,'CPalg','CP_HALS_opts.mat',...
      %  'lra_rank',2,'maxiter',500,'nlssolver','hals','initmode','cp','lra',true);
      [Ycap] = lraNTF(Yblk,lraNTF_opts);
      d(:,kblk) = double(Ycap.U{1});
      xy(kblk,:) = [xu yu];

      %[X_als,Uinit,out] = cp_als(Yblk,1);
      % X_als2 = double(permute(tensor(X_als),[2 3 4 1])); size(X_als2)
      % show_4dvideo(X_als2);
    end
  end
  %toc

  %% Find stationary blocks and build background model
  %tic;
  displog('Finding stationary blocks and build background model');
  maxd = max(d); mind = min(d);
  thresh = 0.005;

  if(tdim == 4) Imbgr = zeros(sT(1:3)); end
  if(tdim == 3) Imbgr = zeros(sT(1:2)); end

  for k = 1:size(d,2)
    edges = [mind(k):thresh:maxd(k) maxd(k)+eps];
    [n,bin] = histc(d(:,k),edges);
    m = mode(bin);
    indbgr = find((d(:,k) >= edges(m)) & (d(:,k) <= edges(m+1)));

    if(tdim == 4)
      bgrblk = median(T(xy(k,1):xy(k,1)+blksize-1, xy(k,2):xy(k,2)+blksize-1,:,indbgr),4);
      Imbgr(xy(k,1):xy(k,1)+blksize-1, xy(k,2):xy(k,2)+blksize-1,:) = bgrblk;
    end

    if(tdim == 3)
      bgrblk = median(double(T(xy(k,1):xy(k,1)+blksize-1, xy(k,2):xy(k,2)+blksize-1,indbgr)),3);
      Imbgr(xy(k,1):xy(k,1)+blksize-1, xy(k,2):xy(k,2)+blksize-1) = bgrblk;
    end
  end
  %toc
  displog('Finished');
  %%
  LT = repmat(Imbgr,[1 1 Tlen]);
end


% Author: James A. Douthwaite

classdef agent_2D_formation_VO < agent_2D_VO & agent_formation
%% INITIALISE THE AGENT SPECIFIC PARAMETERS
    properties
        % PROPERTITIES UNIQUE TO AGENT CLASS
    end
    %% ///////////////////////// MAIN METHODS /////////////////////////////
    methods
        % Constructor
        function [this] = agent_2D_formation_VO(varargin)
            % CALL THE SUPERCLASS CONSTRUCTOR
            this = this@agent_2D_VO(varargin);

            this.maxSamples = 1;

            % //////////////////// SENSOR PARAMETERS //////////////////////
%             [obj.SENSORS] = obj.GetDefaultSensorParameters();       % Default sensing
            [this.SENSORS] = this.GetCustomSensorParameters();       % Experimental sensing
            % /////////////////////////////////////////////////////////////

            % //////////////// Check for user overrides ///////////////////
            [this] = this.ApplyUserOverrides(varargin); % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % Main
        function [this] = main(this,ENV,varargin)
            % INPUTS:
            % obj      - The agent object
            % TIME     - The current time structure
            % varargin - Cell array of inputs
            % OUTPUTS:
            % obj      - The updated object

            % PLOT AGENT FIGURE
            visualiseProblem = 0;
            visualiseAgent = 1;
            if this.objectID == visualiseAgent && visualiseProblem == 1
                overHandle = figure(100 + this.objectID);
                hold on; grid on;
                xlabel('x_{m}'); ylabel('y_{m}'); zlabel('z_{m}');
            end

            % //////////// CHECK FOR NEW INFORMATION UPDATE ///////////////
            % UPDATE THE AGENT WITH THE NEW ENVIRONMENTAL INFORMATION
            [this,obstacleSet,agentSet,~] = this.GetAgentUpdate(ENV,varargin{1});

            % ////////////////// FORMATION CONTROLLER /////////////////////
            % We wish to conduct formation control with the other agents
            % in the simulation only.
            L = 0;
            if ~isempty(agentSet)
                % PASS AGENT SET TO FORMATION CONTROLLER
                [heading,speed,L] = this.formationControl_distance(agentSet);          % Get the force vector

                desiredVelocity = heading*speed;
            end

            % ////////////////// OBSTACLE AVOIDANCE ///////////////////////
            % Modify the desired velocity with the augmented avoidance velocity.
%             avoidanceSet = [obstacleSet;agentSet];
            algorithm_start = tic; algorithm_indicator = 0;
%             if ~isempty(avoidanceSet) && avoidanceEnabled
%                 algorithm_indicator = 1;
%                 % GET THE UPDATED DESIRED VELOCITY
%                 [desiredHeadingVector,desiredSpeed] = this.GetAvoidanceCorrection(...
%                     dt,...
%                     desiredVelocity,...
%                     visualiseProblem);
%                 desiredVelocity = desiredHeadingVector*desiredSpeed;
%             end
            algorithm_dt = toc(algorithm_start);

            % //////////////////// AGENT CONTROL //////////////////////////
            [this] = this.Controller(ENV.dt,desiredVelocity);

            % ////////////// RECORD THE AGENT-SIDE DATA ///////////////////
            this = this.writeAgentData(ENV,algorithm_indicator,algorithm_dt);
            this.DATA.inputNames = {'$v_x$ (m/s)','$v_y$ (m/s)','$\dot{\psi}$ (rad/s)'};
            this.DATA.inputs(1:length(this.DATA.inputNames),ENV.currentStep) = this.localState(4:6);         % Record the control inputs
            this.DATA.lypanov(ENV.currentStep) = L;                        % Record the lypanov value
        end
    end
end


function title = p18_title ( title )

%*****************************************************************************80
%
%% P18_TITLE returns the title of problem 18.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, string TITLE, the title of the problem.
%
  title = '10^14 * (x-1)^7, written term by term.';

  return
end


%% Mean-Shift Video Tracking
% by Sylvain Bernhardt
% July 2008
%% Description
% This is a simple example of how to use
% the Mean-Shift video tracking algorithm
% implemented in 'MeanShift_Algorithm.m'.
% It imports the video 'Ball.avi' from
% the 'Videos' folder and tracks a selected
% feature in it.
% The resulting video sequence is played after
% tracking, but is also exported as a AVI file
% 'Movie_out.avi' in the 'Videos' folder.

clear all
close all

%% Import movie
% and time it with tic/toc
tic
[Length,height,width,Movie]=...
    Import_mov('Videos/Ball.avi');
toc

%% Play the movie
% % Put the figure in the center of the screen,
% % without axes and menu bar.
% scrsz = get(0,'ScreenSize');
% figure('Position',[scrsz(3)/2-width/2 scrsz(4)/2-height/2 width height],...
%     'MenuBar','none');
% axis off
% % Image position inside the figure
% set(gca,'Units','pixels','Position',[1 1 width height])
% % Play the movie
% movie(Movie);

%% Variables
index_start = 1;
% Similarity Threshold
f_thresh = 0.16;
% Number max of iterations to converge
max_it = 5;
% Parzen window parameters
kernel_type = 'Gaussian';
radius = 1;

%% Target Selection in Reference Frame
[T,x0,y0,H,W] = Select_patch(Movie(index_start).cdata,0);
pause(0.2);

%% Run the Mean-Shift algorithm
% Calculation of the Parzen Kernel window
[k,gx,gy] = Parzen_window(H,W,radius,kernel_type,0);
% Conversion from RGB to Indexed colours
% to compute the colour probability functions (PDFs)
[I,map] = rgb2ind(Movie(index_start).cdata,65536);
Lmap = length(map)+1;
T = rgb2ind(T,map);
% Estimation of the target PDF
q = Density_estim(T,Lmap,k,H,W,0);
% Flag for target loss
loss = 0;
% Similarity evolution along tracking
f = zeros(1,(Length-1)*max_it);
% Sum of iterations along tracking and index of f
f_indx = 1;
% Draw the selected target in the first frame
Movie(index_start).cdata = Draw_target(x0,y0,W,H,...
    Movie(index_start).cdata,2);
%%%% TRACKING
WaitBar = waitbar(0,'Tracking in progress, be patient...');
% From 1st frame to last one
for t=1:Length-1
    % Next frame
    I2 = rgb2ind(Movie(t+1).cdata,map);
    % Apply the Mean-Shift algorithm to move (x,y)
    % to the target location in the next frame.
    [x,y,loss,f,f_indx] = MeanShift_Tracking(q,I2,Lmap,...
        height,width,f_thresh,max_it,x0,y0,H,W,k,gx,...
        gy,f,f_indx,loss);
    % Check for target loss. If true, end the tracking
    if loss == 1
        break;
    else
        % Drawing the target location in the next frame
        Movie(t+1).cdata = Draw_target(x,y,W,H,Movie(t+1).cdata,2);
        % Next frame becomes current frame
        y0 = y;
        x0 = x;
        % Updating the waitbar
        waitbar(t/(Length-1));
    end
end
close(WaitBar);
%%%% End of TRACKING

%% Export/Show the processed movie
% Export the video sequence as an AVI file in the Videos folder
% WaitBar = waitbar(0,'Exporting the output AVI file, be patient...');
% movie2avi(Movie,'Videos\Movie_out','Quality',50);
% waitbar(1);
% close(WaitBar);

% Put a figure in the center of the screen,
% without menu bar and axes.
scrsz = get(0,'ScreenSize');
figure(1)
set(1,'Name','Movie Player','Position',...
    [scrsz(3)/2-width/2 scrsz(4)/2-height/2 width height],...
    'MenuBar','none');
axis off
% Image position inside the figure
set(gca,'Units','pixels','Position',[1 1 width height])
% Play the movie
movie(Movie);

%% End of File
%=============%


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure eraction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Pos
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring ants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the beam attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function beams_info = update_nonInv_Beams(dt,current_time,beams_info)

% beams_info:   col 1: 1ST PT.
%               col 2: MIDDLE PT. (where force is exerted)
%               col 3: 3RD PT.
%               col 4: beam stiffness
%               col 5: x-curvature
%               col 6: y-curvature

% GEOMETRIC PARAMETERS
pi = 4*atan(1);
L1 = 8;                              % length of computational domain (m)
N1 = 256;                            % number of Cartesian grid meshwidths at the finest level of the AMR grid
bell_length = 2;                     % bell length (m)
npts_bell = ceil(2.0*(bell_length/L1)*N1);  % number of pos along the length of the bell
npts_circ = 1;                       %number of pos along the circumference (if in 3D)
npts = npts_bell*npts_circ;	         % total number pos
ds1 = bell_length/(npts_bell-1);     % mesh spacing(m) along length of bell

% Values from Alben, Peng, and Miller
betao = 0.5;
betam = 0.3;
to = 0.4;
Zs = L1/8;
xRef = L1/4;

 %These are used to keep track of cycle number and time into the cycle
pulse_time = current_time-floor(current_time); % determine time since beginning of first pulse

    % GIVE BELL STATES
    if (pulse_time<to)  %contract bell
        beta = betao+(betam-betao)*(pulse_time/to);
    else                % expand bell
        beta = betam+(betao-betam)*((pulse_time-to)/(1-to));
    end

    %---------------------
    %Xb and Yb are calculated here and will be used to determine new curvatures
    s1=0;
    zl = Zs;
    ro = xRef;

    % Pre-allocate memory for speed
    Xb_lam = zeros(npts,1);
    Yb_lam = zeros(npts,1);

    %top of bell
    Xb_lam(1)=ro;
    Yb_lam(1)=zl;

    %right side of bell
    for s1 = 2:ceil(npts_bell/2)
        thetaj = -1.55*(1-exp(-(s1)*ds1/beta));
        zl = zl + ds1*sin(thetaj);
        ro = ro + ds1*cos(thetaj);
        Xb_lam(s1)=ro;
        Yb_lam(s1)=zl;
    end

    zl = Zs;
    ro = xRef;

    %left side of bell
    for s1 = (ceil(npts_bell/2))+1:npts_bell
        s2=s1-(ceil(npts_bell/2));
        thetaj = -1.55*(1-exp(-(s2)*ds1/beta));
        zl = zl + ds1*sin(thetaj);
        ro = ro - ds1*cos(thetaj);
        Xb_lam(s1)=ro;
        Yb_lam(s1)=zl;
    end

% CHANGE CURVATURE BTWN SIDES OF JELLYFISH BELL
for s = 1:length( beams_info(:,1) )

    s1 = beams_info(s,2); % gives lag_idx of the middle node

    %top of bell
    if (s1==1)
         beams_info(s,5) = Xb_lam(ceil(npts/2)+1)+Xb_lam(2)-2*Xb_lam(s1);
         beams_info(s,6) = Yb_lam(ceil(npts/2)+1)+Yb_lam(2)-2*Yb_lam(s1);
    elseif ( s1<ceil(npts/2) )
    % right side of bell
        beams_info(s,5) = Xb_lam(s1-1)+Xb_lam(s1+1)-2*Xb_lam(s1);
        beams_info(s,6) = Yb_lam(s1-1)+Yb_lam(s1+1)-2*Yb_lam(s1);
    elseif ( s1==ceil(npts/2)+1 )
    % top-left side of bell
         beams_info(s,5) = Xb_lam(s1+1)+Xb_lam(1)-2*Xb_lam(s1);
         beams_info(s,6) = Yb_lam(s1+1)+Yb_lam(1)-2*Yb_lam(s1);
    elseif (s1 > ceil(npts/2)+1)
    % left side of bell
         beams_info(s,5) = Xb_lam(s1+1)+Xb_lam(s1-1)-2*Xb_lam(s1);
         beams_info(s,6) = Yb_lam(s1+1)+Yb_lam(s1-1)-2*Yb_lam(s1);
    end

end


function [DCM] = VBA_spm_dcm_fmri_check(P)
% post-hoc diagnostics for DCM (bilinear or nonlinear) of fMRI data
% FORMAT [DCM] = spm_dcm_fmri_check(DM)
% DCM - DCM structure or its filename
%
% This routine provides some diagnostics to ensure model inversion has
% converged. It plots the predicted and observed responses over all regions
% and provides the coefficient of determination ? or percent variance
% explained. This should normally be above 10%. An abnormally low
% coefficient of determination is highlighted in red. Quantitatively, one
% would normally expect to see one or more extrinsic (between source)
% connections with the strength of 1/8 Hz or greater. If all the extrinsic
% posterior expectations are below this value, then this suggests a failure
% of convergence or that the data are very noisy (possibly due to using
% very small regions of interest to summarise regional responses). Finally,
% the posterior correlations among all parameters are shown in terms of a
% correlation matrix. The number of effective parameters estimated is
% reported in terms of the (KL) divergence between the posterior and
% prior densities over parameters. This is divided by the log of the
% number of observations, by appealing to the Bayesian information
% criterion. The divergence corresponds to complexity or Bayesian
% surprise. Normally, one would expect the posterior and prior to diverge
% in a non-trivial fashion.
%
% Posterior densities are shown as bars with 90% confidence intervals in
% pink. An informed model inversion would normally provide posterior
% densities with confidence intervals that are, for some connections,
% displaced from prior expectations (at or around zero).
%
% This routine is compatible with DCM8, DCM10 and DCM12 files.
%__________________________________________________________________________
% Copyright (C) 20012 Wellcome Trust Centre for Neuroimaging
% Karl Friston

% $Id: spm_dcm_fmri_check.m karl $

%-Load DCM structure
%--------------------------------------------------------------------------
if ~nargin
  uiopen('load');
elseif isstruct(P)
  DCM = P;
else
  load(P)
end

% Assemble diagnostics
%==========================================================================
% coefficient of determination (percent variance explained)
%--------------------------------------------------------------------------
PSS  = sum(sum(DCM.y.^2));
RSS  = sum(sum(DCM.R.^2));
D(1) = 100*PSS/(PSS + RSS);

% largest absolute posterior expectation (extrinsic connections)
%--------------------------------------------------------------------------
try
   A = DCM.Ep.A;
catch
   A = DCM.A;
end
D(2) = max(max(abs(A - diag(diag(A)))));

% complexity and effective number of parameters estimated
%--------------------------------------------------------------------------
qE = VBA_spm_vec(DCM.Ep);
pE = VBA_spm_vec(DCM.M.pE);
qC = DCM.Cp;
pC = DCM.M.pC;
k  = rank(full(pC));
pC = VBA_spm_inv(pC);

D(3) = (trace(pC*qC) + (pE - qE)'*pC*(pE - qE) - VBA_spm_logdet(qC*pC) - k)/2;
D(3) = D(3)/log(DCM.v);

% Plot summary of inversion
%==========================================================================
VBA_spm_figure('GetWin','DCM diagnostics'); clf

% plot predicted and observed regional responses
%--------------------------------------------------------------------------
subplot(2,1,1);
t = (1:DCM.v)*DCM.Y.dt;

plot(t,DCM.y,t,DCM.y + DCM.R,':');
str = sprintf('variance explained %0.0f%%', D(1));
str = {'responses and predictions',str};
if D(1) > 10
  title(str,'FontSize',16);
else
  title(str,'FontSize',16,'Color','r');
end
xlabel('time {seconds}');

% posterior densities over A parameters
%--------------------------------------------------------------------------
try
  i = VBA_spm_fieldindices(DCM.Ep,'A');
catch
  i = 1 + (1:DCM.n^2);
end
qE = VBA_spm_vec(DCM.Ep);
qC = DCM.Cp;
subplot(2,2,3)
VBA_spm_plot_ci(qE(i),qC(i,i)), hold on
str = sprintf('largest connection strength %0.2f', D(2));
str = {'intrinsic and extrinsic connections',str};
if D(2) > 1/8
  title(str,'FontSize',16);
else
  title(str,'FontSize',16,'Color','r');
end
xlabel('parameter}');
axis square

% posterior correlations among all parameters
%--------------------------------------------------------------------------
subplot(2,2,4)
imagesc(VBA_cov2corr(DCM.Cp))
title('posterior correlations','FontSize',16)
str = sprintf('estimable parameters %0.0f', D(3));
str = {'posterior correlations',str};
if D(3) > 1
  title(str,'FontSize',16);
else
  title(str,'FontSize',16,'Color','r');
end
axis square


function esi = esiFeature(config, x)
%ESIFEATURE     ESI feature extraction.
%   Format: esi = esiFeature(config, x)
%   Inputs:
%       config: Configuration for ESI features.
%       x:      Input waveform or salience map.
%   Output:
%       esi:    ESI features, one column per frame.

    if (any(size(x) == 1)) x = salience(config, x); end
    esi = config.G * x;
    for f = 1:size(esi,2)
        esi(:,f) = esi(:,f) / sum(esi(:,f));
    end
end


% GP_COV - This is a general documentation file for using covariance
%          functions.
%
% The covariance functions are used similarly. The covariance function is
% initialized by giving some function specific inputs, for instance,
%
%   COVFUNC = GP_COV_EXAMPLE(P1, P2, ...)
%
% where the number of inputs and their meaning depends on the covariance
% function. The input arguments can be, for instance, a (squared) distance
% matrix between the inputs, the dimensionality of the input space or
% another covariance function. Thus, be careful especially when changing
% covariance functions that you also change the inputs as needed! For
% instance, some covariance functions may use a distance matrix while
% some others a squared distance matrix.
%
% The returned covariance function COVFUNC can be used to obtain the
% covariance matrix by giving a parameter vector THETA:
%
%   K = COVFUNC(THETA)
%   [K, DK] = COVFUNC(THETA)
%
% where K is the covariance matrix and DK is a cell array of derivatives of
% K with respect to the parameters THETA. The length of THETA and the
% meaning of its elements depends on the covariance function. The parameters
% can be, for instance, characteristic length scale, magnitude or
% period. Again, be sure that you know which element of THETA corresponds to
% which parameter of the covariance function. Especially, if you modify your
% covariance function construction, also modify THETA appropriately!
%
% You can form complex covariance functions by using some functions to chain
% simple covariance functions. For instance, GP_COV_SCALE can be used to add
% a scaling parameter, GP_COV_SUM to sum two covariance functions and
% GP_COV_PRODUCT to multiply two covariance functions. Make sure how the
% additional parameters are added to the vector THETA and how the parameter
% vectors of multiple covariance functions are combined.
%
% For most covariance functions, you can fix some parameters:
%
%   COVFUNC = GP_COV_EXAMPLE(..., 'PARAMETER', VALUE)
%
% Then the parameter is not read from the vector THETA anymore. For
% instance, if you construct a covariance function as
%
%   COVFUNC = GP_COV_PERIODIC(SQRT(SQ_DIST(X1,X2)),'WAVELENGTH',10)
%
% then THETA should contain only one element, the smoothness parameter.
%
% You can obtain some dimensionality information by calling COVFUNC
% without any inputs:
%
%   [N_THETA, N1, N2] = COVFUNC()
%
%   N_THETA : Number of non-fixed parameters
%   N1      : Number of rows in the covariance matrix
%   N2      : Number of columns in the covariance matrix
%
% In order to obtain the covariance matrix of a covariance function with no
% parameters, you should call COVFUNC([]), not COVFUNC(). Note the
% difference!
%
% At least the following covariance functions are implemented:
%
%   GP_COV_SE       : Isotropic squared exponential
%   GP_COV_RQ       : Isotropic rational quadratic
%   GP_COV_PP       : Piecewise polynomial
%   GP_COV_DELTA    : Delta function, isotropic noise
%   GP_COV_SCALE    : Scales a covariance function
%   GP_COV_SUM      : Sums two covariance functions
%   GP_COV_PRODUCT  : Multiplies two covariance functions
%   GP_COV_JITTER   : Adds a small constant to the diagonal for numerical
%                     stability
%   GP_COV_TOEPLITZ : Creates a covariance matrix with Toeplitz structure
%   GP_COV_WRAP     : Makes a covariance function from a covariance matrix
%
% You can write your own covariance functions as long as they fulfill the
% specification described here. In some cases, it might be a good idea to
% write some wrapper functions for the covariance functions to make the
% usage more straightforward. For instance,
%
%   COVFUNC_SE = @(THETA,X1,X2) FEVAL(GP_COV_SE(SQ_DIST(X1,X2)),THETA)
%
% would be used as
%
%   [K, DK] = COVFUNC_SE(THETA, X1, X2)
%
% which some users may prefer. However, this kind of interface may reduce
% efficiency, as one may need to compute same things several times (squared
% distance matrix in the above example).
%
% Note: Despite the similar naming convention, GP_COV_PSEUDO is
% fundamentally different kind of covariance function and can not be used
% similarly to other covariance functions. See GP_COV_PSEUDO for more
% information.
%
% See also GP_LEARN, GP_LEARN_PSEUDO, GP_PREDICT, GP_PREDICT_PSEUDO.

% Last modified 2010-01-27
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function gp_cov()

help gp_cov


function [fh, figurename] = dtiCheckMotion(ecXformFile,visibility)
% Plots rotations and translations from an eddy-current correction
% transform file.
%
%   [fh, figurename]  = dtiCheckMotion([ecXformFile=uigetfile],visibility)
%
% INPUTS:
%   ecXformFile - Eddy Current correction trasformation infromation. This
%                 file is generally generated and saved by dtiInit.m. See
%                 dtiInit.m and also dtiRawRohdeEstimateEddyMotion.m
%
%   visibility  - A figure with the estimates of rotation and translation
%                 will be either displayed (visibility='on') or not
%                 (visibility='off'). The figure will be always saved in
%                 the same directory of the ecXformFile.mat file.
%
% OUTPUTS:
%   fh - Handle to the figure of the motion estimae. Note, this figure
%        might be set to invisible using 'visibility'. To display the
%        figure invoke the following command: set(fh,'visibility','on').
%
%   figurename - Full path to the figure saved out to disk showing the
%                motion estimates.
%
% Franco Pestilli & Bob Dougherty Stanford University
% 03/01/2019 Hiromasa Takemura (CiNet) modified a figure handle

if notDefined('visibility'), visibility = 'on'; end
if(~exist('ecXformFile','var') || isempty(ecXformFile))
   [f,p] = uigetfile('*.mat','Select the ecXform file');
   if(isequal(f,0)), disp('Canceled.'); retun; end
   ecXformFile = fullfile(p,f);
end

% Load the stored trasformation file.
ec = load(ecXformFile);
t = vertcat(ec.xform(:).ecParams);

% We make a plot of the motion correction during eddy current correction
% but we do not show the figure. We only save it to disk.
fh = mrvNewGraphWin([],[],visibility);
if ishandle(fh)
    fh = fh.Number;
end

subplot(2,1,1);
plot(t(:,1:3));
title('Translation');
xlabel('Diffusion image number (diffusion direction)');
ylabel('translation (voxels)');
legend({'x','y','z'});

subplot(2,1,2);
plot(t(:,4:6)/(2*pi)*360);
title('Rotation');
xlabel('Diffusion image number (diffusion direction)');
ylabel('rotation (degrees)');
legend({'pitch','roll','yaw'});

% Save out a PNG figure with the same filename as the Eddy Currents correction xform.
[p,f,~] = fileparts(ecXformFile);
figurename = fullfile(p,[f,'.png']);

if isnumeric(fh)==1
printCommand = ...
    sprintf('print(%s, ''-painters'',''-dpng'', ''-noui'', ''%s'')', ...
    num2str(fh),figurename);
else
printCommand = ...
    sprintf('print(%s, ''-painters'',''-dpng'', ''-noui'', ''%s'')', ...
    num2str(fh.Number),figurename);
end
fprintf('[%s] Saving Eddy Current correction figure: \n %s \n', ...
         mfilename,figurename);
eval(printCommand);

% Write out RMS displacement as a .txt. This is overall
% displacement between each scan
d = vertcat(0, diff(sqrt(t(:,1).^2+t(:,2).^2+t(:,3).^2)));
dlmwrite(fullfile(p,'Voxel_motion.txt'), d);

% Delete output if it was nto requested
if (nargout < 1), close(fh); clear fh figurename; end

return;


  function fs = de_ftab_xform(ftab, fm)
%|function fs = de_ftab_xform(ftab, fm)
%|
%| convert nonlinear BH function "f" to "f*" using jacobian
%| in
%|	ftab	struct	from de_ftab_build()
%|	fm	[n? M]	f_m samples
%| out
%|	fs	[n? L]	f^*_l samples
%|
%| Copyright 2002-2-15, Jeff Fessler, University of Michigan

if nargin < 2, ir_usage, end

[LL MM] = size(ftab.T);
dim = size(fm);
if MM == 1
	dim = [dim 1];
end
if dim(end) ~= MM, keyboard, error 'bad input dim', end
fm = reshape(fm, prod(dim(1:end-1)), MM); % [*n M]
fs = fm * ftab.T'; % [*n L]
fs = reshape(fs, [dim(1:end-1) LL]); % [n? L]


function [pcacoef] =computePca(namefile,ncoef)
% [pcacoef] =computePca(namefile,ncoef)

r=load(namefile);
if isfield(r,'peval')
    if isfield(r.peval, 'data_path')
        if isfield(r.peval,'data_file')
            d=load([r.peval.data_path '/' r.peval.data_file]);
        else
            d=load(r.peval.data_path); % there was some confusion what is path and waht is file
        end
    else
        error('Can not find the location of the data.')
    end
else
    d=load('dpixc'); % Data stored directlu in the directory
end

pcacoef = pca(reshape(d.dpixc,r.peval.nx*r.peval.ny,r.peval.nt),ncoef);


%
% DESC
%
% COPYRIGHT: Andreas C. Damianou, 2015
%
% SEE ALSO:
%
% DEEPGP

%% ------ CONFIGURING THE DEEP GP
%--- Mandatory configurations
if ~exist('Ytr', 'var'), error('You need to specify your outputs in Ytr{1}=...'); end

%--- Optional configurations: Whatever configuration variable is not already set (ie does not exist
% as a variable in the workspace) is set to a default value.
if ~exist('experimentNo','var'), experimentNo = 404; end
if ~exist('K','var'), K = 30; end
if ~exist('Q','var'), Q = 6; end
if ~exist('baseKern','var'), baseKern = 'rbfardjit'; end % {'rbfard2','white','bias'}; end

hsvargplvm_init;

%%
options = hsvargplvmOptions(globalOpt);
options.optimiser = 'scg2';
initXOptions = hsvargplvmInitXOptions(Ytr, options, globalOpt);

% Create a deepGP model, parametrized by its local options, global options
% and options that say how to initialise the latent spaces X
model = hsvargplvmModelCreate(Ytr, options, globalOpt, initXOptions);
%!!!!!!!!!!!!!!!!!!!!!!!!-----------------------
if exist('DEBUG_entropy','var') && DEBUG_entropy
    model.DEBUG_entropy = true;for itmp=1:model.H, model.layer{itmp}.DEBUG_entropy = true; end
end
params = hsvargplvmExtractParam(model); model = hsvargplvmExpandParam(model, params);

%% Optimise deep GP model
model.globalOpt = globalOpt;
[model,modelPruned, modelInitVardist] = hsvargplvmOptimiseModel(model, true, true);

% Uncomment if you decide to train for more iterations later...
%modelOld = model;
%model = hsvargplvmOptimiseModel(model, true, true, [], {0, [1000 1000 1000]});


classdef KTA2 < ALGORITHM
% <multi/many> <real/integer> <expensive>
% Kriging-assisted Two_Arch2
% tau  --- 0.75 --- Proportion of one type noninfluential points in training data
% phi  ---  0.1 --- Number of randomly selected individuals
% wmax ---   10 --- Number of generations before updating CA and DA
% mu   ---    5 --- Number of re-evaluated solutions at each generation

%------------------------------- Reference --------------------------------
% Z. Song, H. Wang, C. He and Y. Jin, A Kriging-assisted two-archive
% evolutionary algorithm for expensive many-objective optimization, IEEE
% Transactions on Evolutionary Computation, 2021, 25(6): 1013-1027.
%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Zhenshou Song
% Email:zssong@stu.xidian.edu.cn

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [tau,phi,wmax,mu] = Algorithm.ParameterSet(0.75,0.1,10,5);

            %% Initialization
            p = 1/Problem.M;
            CAsize = Problem.N;
            N = Problem.N;
            P = UniformPoint(N, Problem.D, 'Latin');
            Population = Problem.Evaluation(repmat(Problem.upper-Problem.lower,N,1).*P+repmat(Problem.lower,N,1));
            All_Population = Population;
            Ho_Population = All_Population;
            CA = UpdateCA([],Population,CAsize);
            DA = Population;
            THETA_S = 5.*ones(Problem.M,Problem.D);
            THETA_IS =  5.*ones(2,Problem.M,Problem.D);
            Model_sensitive = cell(1,Problem.M);
            Model_insensitive = cell(2,Problem.M);
            %% Optimization
            while Algorithm.NotTerminated(All_Population)
                %***** Building influential point-insensitive model********
                % build sensitive model
                Dec = All_Population.decs;
                Obj = All_Population.objs;
                for i = 1:Problem.M
                    dmodel     = dacefit(Dec,Obj(:,i),'regpoly0','corrgauss',THETA_S(i,:),1e-5.*ones(1,Problem.D),100.*ones(1,Problem.D));
                    Model_sensitive{i}   = dmodel;
                    THETA_S(i,:) = dmodel.theta;
                end
                % build insensitive models
                Centers = zeros(Problem.M,2);
                for i = 1 : Problem.M
                    [~,N1] = sort(Obj(:,i));
                    num = ceil(length(All_Population).*tau);
                    mean_index{1} = N1(1:num);
                    mean_index{2} = N1(end-num:end);
                    for j = 1:2
                        Centers(i,j) = mean(Obj(mean_index{j},i));  % lambda and miu
                    end
                    for j = 1 : 2
                        train_X = Dec(mean_index{j},:);
                        train_Y = Obj(mean_index{j},i);
                        dmodel  = dacefit(train_X,train_Y,'regpoly0','corrgauss',THETA_IS(j,i,:),1e-5.*ones(1,Problem.D),100.*ones(1,Problem.D));
                        Model_insensitive{j,i} = dmodel;
                        THETA_IS(j,i,:)        = dmodel.theta;
                    end
                end
                % Set the CCA and CDA as the current CA and DA
                CAobj = CA.objs; CAdec = CA.decs;
                DAobj = DA.objs; DAdec = DA.decs;
                w = 1;
                while w <= wmax   % this part is same as Two_Arch2
                    [~,ParentCdec,~,ParentMdec] = MatingSelection_KTA2(CAobj,CAdec,DAobj,DAdec,Problem.N);
                    OffspringDec = [OperatorGA(Problem,ParentCdec,{1,20,0,0});OperatorGA(Problem,ParentMdec,{0,0,1,20})];
                    PopDec = [DAdec;CAdec;OffspringDec];
                    N      = size(PopDec,1);
                    PopObj = zeros(N,Problem.M);
                    MSE    = zeros(N,Problem.M);
                    %****** Using influential point-insensitive model *****
                    for i = 1:N
                        for j = 1:Problem.M
                            [PopObj(i,j),~,~] = predictor(PopDec(i,:),Model_sensitive{j});
                            if abs(PopObj(i,j)- Centers(j,1)) <= abs(PopObj(i,j)- Centers(j,2))
                                model = Model_insensitive{1,j};
                            else
                                model = Model_insensitive{2,j};
                            end
                            [PopObj(i,j),~,MSE(i,j)] = predictor(PopDec(i,:),model);
                        end
                    end
                    [CAobj,CAdec,~] = K_UpdateCA(PopObj,PopDec,MSE,CAsize);
                    [DAobj,DAdec,DAvar] = K_UpdateDA(PopObj,PopDec,MSE,Problem.N,p);
                    w = w + 1;
                end

                % Adaptive sampling
                Offspring01 = Adaptive_sampling(CAobj,DAobj,CAdec,DAdec,DAvar,DA,mu,p,phi);

                [~,index] = unique(Offspring01 ,'rows');
                PopNew = Offspring01(index,:);
                Offspring02 = [];
                for i = 1:size(PopNew,1)
                    dist2 = pdist2(real( PopNew(i,:)),real(All_Population.decs));
                    if min(dist2) > 1e-5
                        Offspring02 = [Offspring02;PopNew(i,:)];
                    end
                end
                if ~isempty(Offspring02)
                    Offspring = Problem.Evaluation(Offspring02);

                    temp =  All_Population.decs;
                    for i = 1:size(Offspring,2)
                        dist2 = pdist2(real(Offspring(i).decs),real(temp));
                        if min(dist2) > 1e-5
                            All_Population = [All_Population,Offspring(i)];
                        end
                        temp = All_Population.decs;
                    end
                    CA = UpdateCA(CA,Offspring,CAsize);
                    DA = UpdateDA(DA,Offspring,Problem.N,p);
                    Ho_Population = [Ho_Population,Offspring];
                end
            end
        end
    end
end


function  [cent, varargout]=FastPeakFind(d, threshold, filt ,edg, fid)
% Analyze noisy 2D images and find peaks to 1 pixel accuracy.
% The code is desinged to be as fast as possible, so I kept it pretty basic.
% The code assumes that the peaks are relatively sparse, test whether there
% is too much pile up and set threshold or user defined filter accordingly.
%
% Inputs:
%   d           The 2D data raw image - assumes a Double\Single-precision
%               floating-point, uint8 or unit16 array. Please note that the code
%               casts the raw image to uint16 if needed.  If the image dynamic range is
%               between 0 and 1, I multiplied to fit uint16. This might not be optimal for
%               generic use, so modify according to your needs.
%   threshold   A number between 0 and max(raw_image(:)) to remove  background
%   filt        A filter matrix used to smooth the image. The filter size
%               should correspond the characteristic size of the peaks
%   edg         A number>1 for skipping the first few and the last few 'edge' pixels
%   fid         In case the user would like to save the peak positions to
%               a file, the code assumes a "fid = fopen([filename], 'w+');" line
%               in the script that uses this function.
%
% Optional Outputs:
%   cent        a 1xN vector of coordinates of peaks (x1,y1,x2,y2,...
%   [cent cm]   in addition to cent, cm is a binary matrix  of size(d)
%               with 1's for peak positions.
%
%   Example:
%
%   p=FastPeakFind(image);
%   imagesc(image); hold on
%   plot(p(2:2:end),p(1:2:end),'r+')
%
%
%   Natan (nate2718281828@gmail.com)
%   Ver 1.61 , Date: June 5th 2013
%
%
%% defaults
if (nargin < 1)
    d=uint16(conv2(reshape(single( 2^14*(rand(1,1024*1024)>0.99995) ),[1024 1024]) ,fspecial('gaussian', 15,3),'same')+2^8*rand(1024));
    imagesc(d);
end

if ndims(d)>2 %I added this in case one uses imread (JPG\PNG\...).
    d=uint16(rgb2gray(d));
end

if isfloat(d) %For the case the input image is double, casting to uint16 keeps enough dynamic range while speeds up the code.
    if max(d(:))<=1
        d =  uint16( d.*2^16./(max(d(:))));
    else
        d = uint16(d);
    end
end

if (nargin < 2)
    threshold = (max([min(max(d,[],1))  min(max(d,[],2))])) ;
end

if (nargin < 3)
    filt = (fspecial('gaussian', 7,1)); %if needed modify the filter according to the expected peaks sizes
end

if (nargin < 4)
    edg =3;
end

if (nargin < 5)
    savefileflag = false;
else
    savefileflag = true;
end

%% Analyze image
if any(d(:))  ; %for the case of non zero raw image

    d = medfilt2(d,[3,3]);

    % apply threshold
    if isa(d,'uint8')
    d=d.*uint8(d>threshold);
    else
    d=d.*uint16(d>threshold);
    end

    if any(d(:))   ; %for the case of the image is still non zero

        % smooth image
        d=conv2(single(d),filt,'same') ;

        % Apply again threshold (and change if needed according to SNR)
        d=d.*(d>0.9*threshold);

        % peak find - using the local maxima approach - 1 pixle resolution
        % d will be noisy on the edges, since no hits are expected there anyway we'll skip 'edge' pixels.
        sd=size(d);
        [x y]=find(d(edg:sd(1)-edg,edg:sd(2)-edg));

        % initialize outputs
          cent=[];%
          cent_map=zeros(sd);

        x=x+edg-1;
        y=y+edg-1;
        for j=1:length(y)
            if (d(x(j),y(j))>=d(x(j)-1,y(j)-1 )) &&...
                    (d(x(j),y(j))>d(x(j)-1,y(j))) &&...
                    (d(x(j),y(j))>=d(x(j)-1,y(j)+1)) &&...
                    (d(x(j),y(j))>d(x(j),y(j)-1)) && ...
                    (d(x(j),y(j))>d(x(j),y(j)+1)) && ...
                    (d(x(j),y(j))>=d(x(j)+1,y(j)-1)) && ...
                    (d(x(j),y(j))>d(x(j)+1,y(j))) && ...
                    (d(x(j),y(j))>=d(x(j)+1,y(j)+1));

                  %All these alternatives were slower...
                %if all(reshape( d(x(j),y(j))>=d(x(j)-1:x(j)+1,y(j)-1:y(j)+1),9,1))
                %if  d(x(j),y(j)) == max(max(d((x(j)-1):(x(j)+1),(y(j)-1):(y(j)+1))))
                %if  d(x(j),y(j))  == max(reshape(d(x(j),y(j))  >=  d(x(j)-1:x(j)+1,y(j)-1:y(j)+1),9,1))

                        cent = [cent ; x(j) ; y(j)];
                        cent_map(x(j),y(j))=cent_map(x(j),y(j))+1; % if a binary matrix output is desired

            end
        end

        if savefileflag
            % previous version used dlmwrite, which can be slower than  fprinf
            %             dlmwrite([filename '.txt'],[cent],   '-append', ...
            %                 'roffset', 0,   'delimiter', '\t', 'newline', 'unix');+

            fprintf(fid, '%f ', cent(:));
            fprintf(fid, '\n');

        end

    else % in case image after threshold is all zeros
        cent=[];
        cent_map=zeros(size(d));
        if nargout>1 ;  varargout{1}=cent_map; end
        return
    end

else % in case raw image is all zeros (dead event)
    cent=[];
    cent_map=zeros(size(d));
    if nargout>1 ;  varargout{1}=cent_map; end
    return
end

%demo mode - no input to the function
if (nargin < 1); colormap(bone);hold on; plot(cent(2:2:end),cent(1:2:end),'rs');hold off; end

% return binary mask of centroid positions if asked for
if nargout>1 ;  varargout{1}=cent_map; end


function Acc = bssfo_off2on(CNT,general_initparam,bssfo_param)
opt = opt_cellToStruct(general_initparam);
bssfo_opt = opt_cellToStruct(bssfo_param);

% Pre-processing
%Train
CNT_off = prep_selectChannels(CNT{1}, {'Index', opt.channel_index});
CNT_off =prep_filter(CNT_off , {'frequency', bssfo_opt.init_band});
SMT_off = prep_segmentation(CNT_off, {'interval', opt.time_interval});

%Test
CNT_on= prep_selectChannels(CNT{2}, {'Index', opt.channel_index});
CNT_on =prep_filter(CNT_on , {'frequency', bssfo_opt.init_band});
SMT_on = prep_segmentation(CNT_on, {'interval', opt.time_interval});
%% BSSFO - optimal band and weight
[FilterBand]=func_bssfo(SMT_off, {'classes', {'right', 'left'}; ...
    'frequency', {bssfo_opt.mu_band,bssfo_opt.beta_band}; 'std', {5, 25}; 'numBands', bssfo_opt.numBands; ...
    'numCSPPatterns', opt.CSPFilter; 'numIteration', bssfo_opt.numIteration});

%% BSSFO - CSP filter from optimal band
for iii=1:bssfo_opt.numIteration
    CNTtr=prep_filter(CNT{1}, {'frequency', FilterBand.sample(:,iii)'});
    CNTtr= prep_selectChannels(CNTtr, {'Index', opt.channel_index});
    SMT=prep_segmentation(CNTtr, {'interval', opt.time_interval});
    [SMT, CSP_W, CSP_D]=func_csp(SMT,{'nPatterns', opt.CSPFilter});
    FT=func_featureExtraction(SMT, {'feature','logvar'});
    [CF_PARAM]=func_train(FT,{'classifier','LDA'});
    clear CNTclass CNTtr SMT FT CSP_D

    CNTte=prep_filter(CNT{2}, {'frequency', FilterBand.sample(:,iii)'});
    CNTte= prep_selectChannels(CNTte, {'Index', opt.channel_index});
    SMTte=prep_segmentation(CNTte, {'interval', opt.time_interval});
    SMTfb=func_projection(SMTte, CSP_W);
    FTfb=func_featureExtraction(SMTfb, {'feature','logvar'});
    [cf_out]=func_predict(FTfb, CF_PARAM);
    ensemble(:,iii)=cf_out.*FilterBand.weight(iii); %
    clear CNTte SMTte FTfb CSP_W
end
sum_ensemble = sum(ensemble,2); %
%%
[loss out]=eval_calLoss(CNT{2}.y_dec, sum_ensemble); %
Acc=1-loss;
end


function [ds, g, mystd, d, d2, c, c2, mi, b, eigv, eigval] = compare_subjects(varargin)
% This function compares a set of images to one another and does some diagnostics on the similarity among images.
% - It returns multivariate distances and dissimilarities among images
% - It works on the GLOBAL signal after standardizing each image (case 1) or the REGIONAL values in each cluster (case 2)
% - You can also enter a reference image, in which case each image will be correlated with the ref.
%
%
% :Usage:
% ::
%
%     function [ds, g, mystd, d, d2, c, c2, mi, b, eigv, eigval] = compare_subjects([img files or clusters], [mask], ...
%                                        [plot flag], [title on figure], [standardize flag], [text labels], [ref image])
%
% :Inputs:
%
%     a list of image names to compare
%
%     OR
%
%     a clusters structure, with data to compare
%     in timeseries field
%
% If a mask is entered, only voxels in the mask (e.g., with value of 1) will be used.
% You can use this option to specify brain-only or gray-matter only voxels
%
% textlab: optional text labels for each image, can be empty []
%
% If a ref image is entered, each image will be correlated with the ref,
% and values will be saved for the correlation (plot 2 will show these values)
% Useful for comparing anatomical imgs with template, etc.
%
% :Outputs:  from correls with ref image are in variable "c"
%
%   **ds:**
%        multivariate distance (sim. to Mahalanobis) for each image
%        ds is a matrix of squared distances, case numbers, and
%        expected chi2 values (in columns in this order) rows are cases
%
%   **g:**
%        global value for each image
%
%   **d:**
%        global distance from mean image
%        distance, or dissimilarity, is the average absolute deviation between images
%
%   **d2:**
%        matrix of distances among all images
%
%   **c:**
%        correlation between real valued voxels and mean image
%
%   **c2:**
%        correlations among all images (treating voxels as cases)
%
%   **mi:**
%        mutual information between images, with hist2.m
%
%   **b:**
%        principal component scores on correlation matrix for eigenvalues > 1
%
%   **eigv:**
%        eigenvectors
%
%   **eigval:**
%        eigenvalues
%
% :Examples:
% ::
%
%    % Compare normalized anatomcals with standard brain
%    P = get_filename2(['sub*\Anatomy\nscalped_ft1.img']);
%    [ds, g, mystd, d, d2, c, c2, mi] = compare_subjects(P, which('brain_avg152T1.img'), 1, 'intext_countloc', 1, [], which('avg152T1.img'));
%
% ..
%    by Tor Wager
% ..

    doplot = 1;
    dostd = 0;
    do_mi = 1;
    do_corr = 1;
    do_chi2 = 1;
    textlab = [];
    if isempty(varargin)
        error('no inputs.');
    end

    if length(varargin) > 2, doplot = varargin{3}; end
    if length(varargin) > 3, mytitle = varargin{4}; end
    if length(varargin) > 4, dostd = varargin{5}; end
    if length(varargin) > 5, textlab = varargin{6}; end
    if length(varargin) > 6, refimg = varargin{7}; end

    for i=8:length(varargin)
        if(ischar(varargin{i}))
            switch(varargin{i})
                case 'no_mi'
                    do_mi = 0;
                case 'no_corr'
                    do_corr = 0;
                case 'no_chi2'
                    do_chi2 = 0;
            end
        end
    end

    if isstruct(varargin{1})
        clusters = varargin{1};
        error('No method implemented yet.');
    else
        hP = varargin{1};

        disp(' compare subjects.m Running ');

        % Check for images whose dims do not match
        % -----------------------------------------------------------------
        V = spm_vol(hP);
        [bad, whbad] = iimg_check_volinfo(V(1), V);

        hP(whbad, :) = [];
        V(whbad) = [];

        if any(bad)
            disp('Warning!!! Some image dims do not match.  Removing subjects:');
            fprintf('%3.0f ', whbad);
            fprintf('\n');

            if ~isempty(textlab),
                fprintf('%s ', textlab{whbad});
                fprintf('\n');
                textlab(whbad) = [];
            end

        end

        % -----------------------------------------------------------------

        fprintf(1, '\nLoading volumes.\t');

        v = spm_read_vols(V);
        num_images = size(v, 4);

        if length(varargin) > 1
            mP = varargin{2};
            fprintf(1, 'masking volumes.\t')
            % ----------------------------------------------------------------------------------
            % * load mask, and mask all subjects' contrast values
            % so that we show only those voxels existing for all subjects.
            % ----------------------------------------------------------------------------------

            vm = spm_read_vols(spm_vol(mP));
            tmp = size(v);
            if any(size(vm) - tmp(1:3)),
                fprintf(1, '(reslicing mask).\t')
                [tmp, mP] = reslice_imgs(deblank(hP(1,:)), mP);
                vm = spm_read_vols(spm_vol(mP));
            end

            vm = (vm ~= 0);
            vm = +vm;
            vm(vm == 0) = NaN;

            %attempting something less memory-intensive - commented out line
            %required 3 times the size of v in memory... approx 1.2 gigs
            %v = v .* repmat(vm, [1 1 1 size(v, 4)]);
            for i=1:num_images
                v(:,:,:,i) = v(:,:,:,i) .* vm;
            end

            if length(varargin) > 6
                rimg = spm_read_vols(spm_vol(refimg));
                tmp = size(v);
                if any(size(rimg) - tmp(1:3)),
                    fprintf(1, '(reslicing ref image).\t')
                    [tmp, refimg] = reslice_imgs(deblank(hP(1,:)), refimg);
                    rimg = spm_read_vols(spm_vol(refimg));
                end
                rimg(rimg == 0) = NaN;
            end
        end

        % ----------------------------------------------------------------------------------
        % * Standardize, if asked for
        % ----------------------------------------------------------------------------------
        if dostd
            fprintf('\nScaling images to mean 0 and var 1.\t')
        end

        for i = 1:num_images
            tmp = v(:,:,:,i);
            tmp = tmp(:);
            mystd(i) = std(tmp(~isnan(tmp)));
            g(i) = mean(tmp(~isnan(tmp) & tmp ~= 0));

            if dostd
                v(:,:,:,i) = (v(:,:,:,i) - g(i)) ./ mystd(i);
            end
        end
        clear tmp

        % Metrics
        [d, d2, g, mystd] = get_dist(v);

        c = [];
        c2 = [];
        mi = [];
        if exist('rimg', 'var')
            [c, c2, mi] = get_correl(v, do_corr, do_mi, rimg);
        else
            [c, c2, mi] = get_correl(v, do_corr, do_mi);
        end

        if(do_chi2)
            [b, eigv, eigval] = pc(c2, doplot);
            [ds, S, p] = multivar_dist(b);
        else
            ds = [];
            S = [];
            p = [];
            b = [];
            eigv = [];
            eigval = [];
        end

        % plotting stuff

        if doplot
            plot_results();
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Inline Functions
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    function plot_results()
        figure('Color', 'w'); subplot(2, 2, 1);
        bar(g);
        xlabel('Image');
        ylabel('Global for each image');
        hold on;
        plot([0 num_images], [mean(g) mean(g)], 'r-');
        if length(varargin) > 3
            title(mytitle);
        end

        subplot(2, 2, 2);
        bar(mi);
        xlabel('Image');
        if length(varargin) < 7
            varargin{7} = 'refimg';
        end

        if exist('rimg', 'var')
            ylabel('Mutual info with reference');
            title(varargin{7});
        else
            ylabel('Mutual info with mean');
        end

        subplot(2, 2, 3);
        imagesc(d2);
        colormap hot;
        xlabel('Image');
        ylabel('Image');
        title('Avg abs dist');

        if isempty(textlab)
            for j = 1:size(b, 1)
                textlab{j} = num2str(j);
            end
        end

        % mds-like (pca version) on similarities (correlations)
        subplot(2, 2, 4); hold on
        if size(b, 2) > 1
            plot(b(:,1), b(:,2), 'Color', 'w');
            xlabel('Component 1');
            ylabel('Component 2');
            for j = 1:size(b, 1)
                text(b(j, 1), b(j, 2), textlab{j}, 'Color', 'b');
            end
        else
            plot(ones(1, length(b)), b, 'Color', 'w');
            xlabel('Component 1');
            for j = 1:size(b, 1)
                text(1, b(j), textlab{j}, 'Color', 'b');
            end
            set(gca, 'XTick', [-1 1]);
        end

        title('MDS of global image values');
    end
end

function [d, d2, g, mystd] = get_dist(v)
    % get the distances from the average
    fprintf('getting distances from mean.\t')
    gmn = mean(v, 4);
    d2 = zeros(size(v, 4), size(v, 4));

    for i = 1:size(v, 4)
        dd = abs((v(:,:,:,i) - gmn));
        d(i) = nanmean(dd(:));
        gg = v(:,:,:,i);
        gg = gg(~isnan(gg));
        g(i) = mean(gg(:));
        mystd(i) = std(gg(:));

        % get the distances from all other images
        d2(i, i) = 0;
        for j = (i+1):size(v, 4)
            dd = abs((v(:,:,:,i) - v(:,:,:,j)));
            d2(i, j) = mean(dd(~isnan(dd)));
        end
    end

    %fill in lower triangle
    d2 = d2 + d2';
    if any(isnan(g)), disp('Warning!  Some images have no valid values.'); end
end

function [c, c2, mi] = get_correl(v, do_corr, do_mi, varargin)
    % get correlations with the average and with all
    fprintf(1, 'getting correlations.\t')

    if length(varargin) > 0  %#ok  % we have a ref image instead of the grand mean
        rimg = varargin{1};
        gv = rimg(:);
    end

    if ~exist('gv', 'var') || isempty(gv)
        gmn = mean(v, 4);
        gv = gmn(:);
    end

    szv = size(v);
    v = reshape(v, prod(szv(1:3)), szv(4));

    % eliminate NaNs
    wh = union(find(isnan(gv)), find(any(isnan(v), 2)));
    if ~isempty(wh)
        gv(wh,:) = [];
        v(wh,:) = [];
    end

    c = [];
    mi = [];
    for i = 1:size(v, 2),
        if(do_corr)
            cc = corrcoef(gv, v(:,i));
            c(i) = cc(1, 2);
        end

        % mutual information
        if(do_mi)
            fprintf(1, 'MI.')
            [H, mi(i)] = hist2(gv, v(:,i), 64);
        end
    end

    c2 = corrcoef(v);
end

function [b, v, d] = pc(a, doplot)
    % a is original matrix, b is principal components, v is eigenvectors
    % (weights on columns, which = weights on voxels)

    [v, d] = eig(a);
    b = (pinv(v) * a')' ./ repmat((diag(d)').^.5, size(a, 1), 1);
    b = fliplr(b);
    v = fliplr(v);

    num = min(10, sum(diag(d) >= 1));
    b = b(:,1:num);
    v = v(:,1:num);
    origd = diag(d);
    d = diag(d)';
    d = fliplr(d);

    if doplot
        figure('Color', 'w');
        bar(real(d.^.5));
        cumprct = cumsum(d ./ sum(d));
        for i = 1:length(d)
            str = sprintf('%3.2f', cumprct(i));
            text(i-.2, d(i)^.5+.2, str);
        end
        title('Eigenvalues');
    end

    d = d(1:num);

    if num == 0
        warning('No eigenvalues above 1!');
        disp(origd);
    end
end


function [f_output f_votes]= eval_Stochastic_Bosque(Data,Random_Forest,varargin)

%Returns the output of the ensemble (f_output) as well
%as a [num_treesXnum_samples] matrix (f_votes) containing
%the outputs of the individual trees.
%
%The 'oobe' flag allows the out-of-bag error to be used to
%weight the final response (only for classification).

okargs =   {'oobe'};
defaults = {0};
[eid,emsg,oobe_flag] = internal.stats.getargs(okargs,defaults,varargin{:});

f_votes = zeros(numel(Random_Forest),size(Data,1));
oobe = zeros(numel(Random_Forest),1);

for i = 1 : numel(Random_Forest)
    f_votes(i,:) = eval_cartree(Data,Random_Forest(i));
    oobe(i) = Random_Forest(i).oobe;
end

switch lower(Random_Forest(1).method)
    case {'c','g'}
        [unique_labels,~,f_votes]= unique(f_votes);
        f_votes = reshape(f_votes,numel(Random_Forest),size(Data,1));
        [~, f_output] = max(weighted_hist(f_votes,~oobe_flag+oobe_flag*oobe,numel(unique_labels)),[],1); %#ok<ASGLU>
        f_output = unique_labels(f_output);
    case 'r'
        f_output = mean(f_votes,1);
    otherwise
        error('No idea how to evaluate this method');
end


% GoDec (Zhou and Tao 2011)
% process_video('RPCA', 'GoDec', 'dataset/cctv.avi', 'output/demo_GoDec.avi');
rank = 1;
card = numel(M); %card = 3.1e+5;
power = 0;
[L,S] = GoDec(M,rank,card,power);


function value = year_length_months_julian ( y )

%*****************************************************************************80
%
%% YEAR_LENGTH_MONTHS_JULIAN returns the number of months in a Julian year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, integer VALUE, the number of months
%    in the year.
%
  value = 12;

  return
end


function test_ft_spike_isi()

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_spike_isi

clear
nTrials = 100;
data = [];
time       = linspace(0,1,1000);
data.trial(1:nTrials) = {zeros(1,length(time))};
data.time(1:nTrials) = {time};
for iTrial = 1:nTrials
  for iUnit = 1:3
      data.trial{iTrial}(iUnit,:) = double(rand(1,1000)<0.05);
  end
end
data.fsample = 1000;
data.hdr = [];
data.cfg.trl = [];
data.label{1} = 'chan1';
data.label{end+1} = 'chan2';
data.label{end+1} = 'chan3';

% show that the psth works also with the poisson format
spike = ft_checkdata(data,'datatype', 'spike');
for iUnit  = 1:3
  spike.time{iUnit} =   spike.time{iUnit} + 0.001*rand(1,length(spike.time{iUnit}));
end
%%
% now test the isi
cfgIsi = [];
cfgIsi.keeptrials = 'yes';
isih = ft_spike_isi(cfgIsi,spike);
%%
figure
cfgIsi = [];
cfgIsi.spikechannel = 1;
cfgIsi.plotfit = 'no'
cfgIsi.latency = [0 0.2];
h = ft_spike_plot_isi(cfgIsi,isih)
%%
cfgIsi = [];
cfgIsi.keeptrials = 'yes';
isih = ft_spike_isi(cfgIsi,spike);
%%
cfgIsi = [];
cfgIsi.spikechannel = 1;
isihS = ft_spike_isi(cfgIsi,spike);
%%
cfgIsi = [];
cfgIsi.spikechannel = 'all';
isihS = ft_spike_isi(cfgIsi,spike);
%%
cfgRet = [];
cfgRet.window = 'gausswin'
cfgRet.interpolate = 5
cfgRet.scattersize = 1;
cfgRet.density = 'yes'
cfgRet.spikechannel = 1;
figure
H = ft_spike_plot_isireturn(cfgRet,isihS);
%%
figure
H = ft_spike_plot_isireturn([],isihS);
%%


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function varargout = overlayImage2D(T,R,omega,m,varargin)
%
% A 2D image viewer for overlaying images R and T
%
% Input:
%   T           discretized image
%   R           discretized image
%   omega       describing the domain
%    m          number of discretization points
%   varargin    optional parameters like {'colorT','r'}
%
% Output:
%  i1,i2        image handles for T and R
%==============================================================================

function varargout = overlayImage2D(T,R,omega,m,varargin)

if nargin==0
    runMinimalExample;
	return;
end

colorT = [1,0,0];
colorR = [0,1,1];
scale  = 1;
alphadata = 0.5;

for k=1:2:length(varargin), % overwrites defaults
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

h  = (omega(2:2:end)-omega(1:2:end))./m;
xi = @(i) (omega(2*i-1)+h(i)/2:h(i):omega(2*i)-h(i)/2)';

if scale == 1,
  T = 255/max(T(:))*T;
  R = 255/max(R(:))*R;
end;

if length(size(T)) == 2,
  T = permute(reshape(uint8(T*colorT),[m,3]),[2,1,3]);
  R = permute(reshape(uint8(R*colorR),[m,3]),[2,1,3]);
else
  11
end;

cla;
i1  = image(xi(1),xi(2),R); axis xy image; hold on;
i2  = image(xi(1),xi(2),T); axis xy image; hold off;
set(i2,'alphaData',alphadata);

if nargout == 1, varargout = {[i1,i2]};  end;

%------------------------------------------------------------------------------
function runMinimalExample

help(mfilename)
setup2DhandData;
overlayImage2D(dataT(:),dataR(:),omega,m);
%==============================================================================


function connect = mocapConnections(fileName, pointNames);

% MOCAPCONNECTIONS Give a connection matrix for the motion capture data.
% FORMAT
% DESC load a connection matrix for txt file based mocap data.
% ARG fileName : the file from which to load the connectivity data.
% ARG pointNames : the names of the points in the file, with the
% ordering matching orderings in any data files to be loaded in (as
% returned by MOCAPPARSETEXT).
%
% SEEALSO : mocapLoadTextData, mocapParseText
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% DATASETS

fid = fopen(fileName);
i = 1;
rem = fgets(fid);
while(rem ~= -1)
  [token, rem] = strtok(rem, ',');
  connections{i, 1} = fliplr(deblank(fliplr(deblank(token))));
  [token, rem] = strtok(rem, ',');
  connections{i, 2} = fliplr(deblank(fliplr(deblank(token))));
  i = i + 1;
  rem = fgets(fid);
end

connect = zeros(length(pointNames));
fclose(fid);
for i = 1:size(connections, 1);
  for j = 1:length(pointNames)
    if strcmp(pointNames{j}, connections{i, 1}) | ...
          strcmp(pointNames{j}, connections{i, 2})
      for k = 1:length(pointNames)
        if k == j
          break
        end
        if strcmp(pointNames{k}, connections{i, 1}) | ...
              strcmp(pointNames{k}, connections{i, 2})
          connect(j, k) = 1;
        end
      end
    end
  end
end
connect = sparse(connect);


function p = prior_sqinvunif(varargin)
%PRIOR_SQINVUNIF  Uniform prior structure for the square inverse of the parameter
%
%  Description
%    P = PRIOR_SQINVUNIF creates uniform prior structure for the
%    square inverse of the parameter.
%
%  See also
%    PRIOR_*

% Copyright (c) 2009 Jarno Vanhatalo
% Copyright (c) 2010,2012 Aki Vehtari

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

  ip=inputParser;
  ip.FunctionName = 'PRIOR_SQINVUNIFORM';
  ip.addOptional('p', [], @isstruct);
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 'SqInv-Uniform';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'SqInv-Uniform')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  if init
    % set functions
    p.fh.pak = @prior_sqinvunif_pak;
    p.fh.unpak = @prior_sqinvunif_unpak;
    p.fh.lp = @prior_sqinvunif_lp;
    p.fh.lpg = @prior_sqinvunif_lpg;
    p.fh.recappend = @prior_sqinvunif_recappend;
  end

end

function [w, s] = prior_sqinvunif_pak(p, w)
  w=[];
  s={};
end

function [p, w] = prior_sqinvunif_unpak(p, w)
  w = w;
  p = p;
end

function lp = prior_sqinvunif_lp(x, p)
  lJ = -log(x)*3 + log(2);  % log(-2/x^3) log(|J|) of transformation
  lp = sum(lJ);
end

function lpg = prior_sqinvunif_lpg(x, p)
  lJg = -3./x;              % gradient of log(|J|) of transformation
  lpg = lJg;
end

function rec = prior_sqinvunif_recappend(rec, ri, p)
% The parameters are not sampled in any case.
  rec = rec;
end