function [caNodeIndices, vResolution_] = ex_CreateIndexCatalog3D(mCatalog, mPolygon, bMap, nGriddingMode, fSmpValue, fSmpBnd, fSizeRectHorizontal, fSizeRectDepth)
    % Creates a cell-array with subcatalogs for every grid node defined by mPolygon.
    %
    % [caNodeIndices] = ex_CreateIndexCatalog(mCatalog, mPolygon, bMap, nGriddingMode,
    %                                                  nNumberEvents, fRadius, fSizeRectHorizontal, fSizeRectDepth)
    % -------------------------------------------------------------------------------------------------------------
    % Creates a cell-array with subcatalogs for every grid node defined by mPolygon. These subcatalogs
    %   contain only indices to the earthquake "rows" in mCatalog.
    %
    % Input parameters:
    %   mCatalog              Earthquake catalog
    %   mPolygon              Polygon (defined by ex_selectgrid)
    %   bMap                  Calculate cell-array for a map (true) or a cross-section (false)
    %   nGriddingMode         Mode of creating grid node subcatalogs
    %                         0: Constant number of events
    %                         1: Constant radius
    %                         2: Rectangular grid node samples
    %                         3: Spherical grid node samples with constant no
    %                            of events
    %                         4: Spherical grid node samples with constant radius
    %   nNumberEvents         Number of events per grid node (nGriddingMode == 0)
    %   fRadius               Radius of grid node sample (nGriddingMode == 1)
    %   fSizeRectHorizontal   Latitude/horizontal size of rectangle (nGriddingMode == 2)
    %   fSizeRectDepth        Longitude/depth size of rectangle (nGriddingMode == 2)
    %
    % Output parameters:
    %   caNodeIndices         Cell-array with index-catalogs per grid node of mPolygon
    %
    % Danijel Schorlemmer
    % June 17, 2002

    report_this_filefun();

    % Create the catalogs for each node with pointers to the overall catalog
    nNumberNodes_ = length(mPolygon(:,1));
    caNodeIndices = cell(nNumberNodes_, 1);

    % If cross-section calculate the length along cross-section
    if ~bMap
        nRow_ = mCatalog.Count;
        vXSecX_ = mCatalog(:,nColumn_);  % length along x-section
        vXSecY_ = (-1) * mCatalog.Depth;  % depth of hypocenters
    end

    % vResolution give the radius (for nGriddingMode = 4) and no. of events
    % (for nGriddingMode = 3)
    vResolution_(:,1)=nan(nNumberNodes_,1)
    if ((nGriddingMode == 3) || (nGriddingMode ==4)) clear vResolution_; end

    % Loop over all points of the polygon
    for nNode_ = 1:nNumberNodes_
        % Get the grid node coordinates
        fX_ = mPolygon(nNode_, 1);
        fY_ = mPolygon(nNode_, 2);
        if size(mPolygon,2)==3
            fZ_ = mPolygon(nNode_, 3);
        end

        if (nGriddingMode == 0) | (nGriddingMode == 1)  % Fixed radius or fixed number
            % Calculate distance from center point
            if bMap
                vDistances_ = sqrt(((mCatalog.Longitude-fX_)*cosd(fY_)*111).^2 + ((mCatalog.Latitude-fY_)*111).^2);
            else
                vDistances_ = sqrt(((vXSecX_ - fX_)).^2 + ((vXSecY_ - fY_)).^2);
            end
            if nGriddingMode == 0 % Fixed number
                if mCatalog.Count == 0
                    caNodeIndices{nNode_} = [];
                    % NaN for no events
                    vResolution_(nNode_) = nan;
                elseif nNumberEvents > mCatalog.Count
                    caNodeIndices{nNode_} = vIndices(1:mCatalog.Count);
                    % take the maximal distance for all eq. in the catalog
                    vResolution_(nNode_) = max(vdistances_);
                else
                    % Use first nNumberEvents events
                    [vTmp, vIndices] = sort(vDistances_);
                    caNodeIndices{nNode_} = vIndices(1:nNumberEvents);
                    % radius of the nNumberEvents-th event in the sorted vDistances_
                    vResolution_(nNode_) = vTmp(nNumberEvents);
                end
            else % Fixed radius
                % Use all events within fRadius
                caNodeIndices{nNode_} = find(vDistances_ <= fRadius);
                vResolution_(nNode_) = length(find(vDistances_ <= fRadius));
            end
        elseif nGriddingMode ==2  % Rectangular gridding (nGriddingMode == 2)
            if bMap
                vSel_ = ((mCatalog.Longitude >= (fX_ - fSizeRectHorizontal/2)) & (mCatalog.Longitude < (fX_ + fSizeRectHorizontal/2)) & ...
                    (mCatalog.Latitude >= (fY_ - fSizeRectDepth/2)) & (mCatalog.Latitude < (fY_ + fSizeRectDepth/2)));
                vResolution_(nNode_) = length(find(vSel_ > 0))
            else
                vSel_ = ((vXSecX_ >= (fX_ - fSizeRectHorizontal/2)) & (vXSecX_ < (fX_ + fSizeRectHorizontal/2)) & ...
                    (vXSecY_ >= (fY_ - fSizRectDepth/2)) & (vXSecY_ < (fY_ + fSizeRectDepth/2)));
                vResolution_(nNode_) = length(find(vSel_ > 0))
            end
            caNodeIndices{nNode_} = find(vSel_ == 1);
        elseif ((nGriddingMode == 3) || (nGriddingMode == 4))     % Spherical grid node samples with constant no of events
            vDistances_ = sqrt(((mCatalog.Longitude-fX_)*cosd(fY_)*111).^2 + ((mCatalog.Latitude-fY_)*111).^2 + (mCatalog.Depth-fZ_).^2);
            if nGriddingMode == 3  % Spherical grid node samples with constant no of events
                nNumberEvents=fSmpValue;
                fRadius=fSmpBnd;
                if mCatalog.Count == 0
                    caNodeIndices{nNode_} = [];
                    % NaN for no events
                    vResolution_{nNode_} = nan;
                    %               vResolution_(nNode_) = nan;
                elseif nNumberEvents > mCatalog.Count
                    caNodeIndices{nNode_} = vIndices(1:mCatalog.Count);
                    % take the maximal distance for all eq. in the catalog
                    vResolution_{nNode_} = vdistances_;
                    %               vResolution_(nNode_) = max(vdistances_);
                else
                    % Use first nNumberEvents events
                    [vTmp, vIndices] = sort(vDistances_);
                    caNodeIndices{nNode_} = vIndices(1:nNumberEvents);
                    % radius of the nNumberEvents-th event in the sorted vDistances_
                    vResolution_{nNode_} = vTmp(nNumberEvents);
                    %               vResolution_(nNode_) = vTmp(nNumberEvents);
                end
            elseif nGriddingMode == 4     % Spherical grid node samples with constant radius
                nNumberEvents=fSmpBnd;
                fRadius=fSmpValue;
                %  muss noch gemacht werden....

            end
        end
    end % of for nNode_
end


%% KUKA sunrise toolbox example.
% moving end-effector of the robot on an ellipse by utilizing the point to
% point elliptical motion functions of the KST

% Copy right: Mohammad SAFEEA
% 30-April-2018

global t_Kuka;
ip='172.31.1.147';
t_Kuka=net_establishConnection(ip);

if ~exist('t_Kuka','var') || isempty(t_Kuka) || strcmp(t_Kuka.Status,'closed')
  disp('Connection could not be establised, script aborted');
  return;
end

%% Go to some initial configuration
disp('moving in joint space to initial configuration');
jPos={0., pi / 180 * 30, 0, -pi / 180 * 60, 0,...
                        pi / 180 * 90, 0};
disp(jPos);
relVel=0.15;
movePTPJointSpace( t_Kuka , jPos, relVel); % move to initial configuration

%% move a little bit back on the X direction
disp('moving -60 mm in the X direction')
deltaX=-60;deltaY=0;deltaZ=0.;
Pos{1}=deltaX;
Pos{2}=deltaY;
Pos{3}=deltaZ;
vel=50;
movePTPLineEefRelBase( t_Kuka , Pos, vel);

%% put the pen on the level of the page
disp('moving -85 mm in the Z direction')
deltaX=0;deltaY=0;deltaZ=-85.;
Pos{1}=deltaX;
Pos{2}=deltaY;
Pos{3}=deltaZ;
vel=50;
movePTPLineEefRelBase( t_Kuka , Pos, vel);
pause(1);

%% Define the ellipse,
disp('Drawing an ellipse in a plane parallel to XY axes of the base')
c=[0; 50]; % this is the displacement of the center of the ellipse with respect to the current position of EEF,
% taken in the XY plane of the robot base
ratio=0.5; % the radious ratio (a/b) of the ellipse
velocity=40;
accel=25;
theta=2*pi;
TefTool=eye(4);
movePTPEllipse_XY(t_Kuka,c,ratio,theta,velocity,accel,TefTool);

%% Define the ellipse, dimentsions are in (meter)
disp('Drawing an ellipse in a plane parallel to XY axes of the base')
c=[0; 50]; % this is the displacement of the center of the ellipse with respect to the current position of EEF,
% taken in the XZ plane of the robot base
ratio=0.5; % the radious ratio (a/b) of the ellipse
velocity=40;
accel=25;
theta=2*pi;
TefTool=eye(4);
movePTPEllipse_XZ(t_Kuka,c,ratio,theta,velocity,accel,TefTool);

%% Define the ellipse, dimentsions are in (meter)
disp('Drawing an ellipse in a plane parallel to XY axes of the base')
c=[0; 50]; % this is the displacement of the center of the ellipse with respect to the current position of EEF,
% taken in the YZ plane of the robot base
ratio=0.5; % the radious ratio (a/b) of the ellipse
velocity=40;
accel=25;
theta=2*pi;
TefTool=eye(4);
movePTPEllipse_YZ(t_Kuka,c,ratio,theta,velocity,accel,TefTool);

net_turnOffServer(t_Kuka);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%    gb 05/05/05
%
% This script has to be executed directly in the command line
% It plots the MSE graphs for all the sujects that are in the repository
% folder : /Snarp/u1/data/reading_longitude/fmri
%
% After a graph is displayed, type Enter to plot the next one.
%
% Type Ctrl + c in the command line to terminate the program
%
% Just make sure the VISTASOFT directory is in the path and type :
% scriptReadMSE
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all
clear all

if isunix
    networkPath = '/snarp1/u1';
else
    networkPath = '\\snarp\u1\';
end

rootDir = fullfile(networkPath,'data','reading_longitude','fmri');

cd(rootDir);
[fileNum dirName] = countDirs(pwd);

for count = 3:fileNum
    try
        currentDir = dirName{count};

        cd(fullfile(rootDir,currentDir));
        motionCompPlotMSE(currentDir);
        pause;

    end

    close all;

end


function [cpulse, verbose] = tapas_physio_get_cardiac_pulses_manual_template(...
    c, t, pulse_detect_options, verbose)
% Detects R-peaks via matched-filter smoothing & peak detection using a
% manually defined QRS-wave (or R-peak environment)
%
%   [cpulse, verbose] = tapas_physio_get_cardiac_pulses_manual_template(...
%    c, t, thresh_min, dt120, verbose)
%
% IN
%   c               [nSamples, 1] raw pulse oximeter samples
%   t               [nSamples, 1] time vector corresponding to samples (un seconds)
%   pulse_detect_options
%                   physio.thresh.cardiac.initial_cpulse_select-substructure
%                   with elements
%                   .method 'manual' or 'load'/'load_template'
%                           'manual' - select template manually
%                           'load'/'load_template' - load from template
%                   .min     threshold for correlation with QRS-wave to find cardiac pulses
%                   .file   variable saving an example cardiac QRS-wave to correlate with ECG time series
%   verbose         Substructure of Physio, holding verbose.level and
%                   verbose.fig_handles with plotted figure handles
%                   debugging plots for thresholding are only provided, if verbose.level >=2
%
% OUT
%
% EXAMPLE
%   tapas_physio_get_cardiac_pulses_manual_template
%
%   See also tapa_physio_new

% Author: Lars Kasper
% Created: 2012-02-20
% Copyright (C) 2014 TNU, Institute for Biomedical Engineering, University of Zurich and ETH Zurich.
%
% This file is part of the physIO toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

if nargin < 5
    verbose.level = 0;
    verbose.fig_handles = [];
end

% manual peak selection, if no file selected and loading is
% specified

hasKrpeakLogfile = exist(pulse_detect_options.file,'file') || ...
    exist([pulse_detect_options.file '.mat'],'file');

% if no file exists, also do manual peak-find
doSelectTemplateManually = any(strcmpi(...
    pulse_detect_options.method, ...
    {'manual', 'manual_template'})) || ~hasKrpeakLogfile;

if doSelectTemplateManually
    pulse_detect_options.kRpeak = [];
    hasECGMin = isfield(pulse_detect_options, 'min') && ~isempty(pulse_detect_options.min);
    if ~hasECGMin
        pulse_detect_options.min = 0.5;
    end
else
    fprintf('Loading %s\n', pulse_detect_options.file);
    ECGfile = load(pulse_detect_options.file);
    pulse_detect_options.min = ECGfile.ECG_min;
    pulse_detect_options.kRpeak = ECGfile.kRpeak;
end

inp_events = [];
ECG_min = pulse_detect_options.min;
kRpeak = pulse_detect_options.kRpeak;
if doSelectTemplateManually
    while ECG_min
        [cpulse, ECG_min_new, kRpeak] = tapas_physio_find_ecg_r_peaks(t,c, ECG_min, [], inp_events);
        fprintf('Press 0, then return, if right ECG peaks were found\n');
        ECG_min = input('otherwise type next numerical choice for ECG_min and continue the selection: ');
    end
else
    [cpulse, ECG_min_new, kRpeak] = tapas_physio_find_ecg_r_peaks(t,c, ECG_min, kRpeak, inp_events);
end
ECG_min = ECG_min_new;
cpulse = t(cpulse);

% save manually found peak parameters to file
if doSelectTemplateManually
    save(pulse_detect_options.file, 'ECG_min', 'kRpeak');
end


function [VOLLOCS, LOCS] = tapas_physio_create_scan_timing_nominal(t, ...
    sqpar, align_scan, durationPhyslogAfterEndOfLastScan)
% Creates locations of scan volume and slice events in time vector of SCANPHYSLOG-files
%
%   [VOLLOCS, LOCS] = tapas_physio_create_scan_timing_nominal(t, sqpar);
%
% In cases where the SCANPHYSLOG-file has no gradient entries (column
% 7-9), the actual time-course of the sequence has to be inferred from the
% nominal sequence parameters in sqpar. Here, the corresponding slice scan
% events are generated for the resampling of regressors in the GLM under
% the assumption that the SCANPHYSLOG-file ended exactly when the scan ended
% ...which is usually the case if a scan is not stopped manually
%
% Additionally, one can set a buffer end time, i.e., the duration of the
% phys logging lasting longer after the end of the last scan
%
% IN
%   t       - timing vector of SCANPHYSLOG-file, usually sampled with 500
%             Hz (Philips)
%   sqpar                   - sequence timing parameters
%           .Nslices        - number of slices per volume in fMRI scan
%           .NslicesPerBeat - usually equals Nslices, unless you trigger with the heart beat
%           .TR             - repetition time in seconds
%           .Ndummies       - number of dummy volumes
%           .Nscans         - number of full volumes saved (volumes in nifti file,
%                             usually rows in your design matrix)
%           .Nprep          - number of non-dummy, volume like preparation pulses
%                             before 1st dummy scan. If set, logfile is read from beginning,
%                             otherwise volumes are counted from last detected volume in the logfile
%           .time_slice_to_slice - time between the acquisition of 2 subsequent
%                             slices; typically TR/Nslices or
%                             minTR/Nslices, if minimal temporal slice
%                             spacing was chosen
%           .onset_slice    - slice whose scan onset determines the adjustment of the
%                             regressor timing to a particular slice for the whole volume
%   align_scan              'first' or 'last' (default)
%                           'first' t == 0 will be aligned to first scan
%                                   volume, first slice
%                           'last'  t(end) will be aligned to last scan
%                                   volume, last slice
%   durationPhyslogAfterEndOfLastScan
%                            duration (in seconds) of physiological logfile
%                            after end of last scan volume in the run
%                           default: 0
% OUT
%           VOLLOCS         - locations in time vector, when volume scan
%                             events started
%           LOCS            - locations in time vector, when slice or volume scan
%                             events started
% EXAMPLE
%   [VOLLOCS, LOCS] = tapas_physio_create_scan_timing_nominal(t, sqpar);
%
%   See also

% Author: Lars Kasper
% Created: 2013-02-07
% Copyright (C) 2013 Institute for Biomedical Engineering, ETH/Uni Zurich.
%
% This file is part of the PhysIO toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

if nargin < 3
    align_scan = 'last';
end

if nargin < 4
    durationPhyslogAfterEndOfLastScan = 0;
end

Nscans          = sqpar.Nscans;
Ndummies        = sqpar.Ndummies;

NallVols = (Ndummies+Nscans);
VOLLOCS = NaN(NallVols,1);
TR = sqpar.TR;

%% First, find volume starts either forward or backward through time series
do_count_from_start = strcmpi(align_scan, 'first');
if do_count_from_start % t = 0 is assumed to be the start of the scan
    for n = 1:NallVols
        [tmp, VOLLOCS(n)] = min(abs(t - TR*(n-1)));
    end
else
    tEndLastScan = t(end)-durationPhyslogAfterEndOfLastScan;

    tStartPhys = t(1);
    for n = 1:NallVols

        tStartVol = (tEndLastScan-TR*(NallVols-n+1));

        if tStartPhys > tStartVol
            VOLLOCS(n) = NaN;
        else
            [tmp, VOLLOCS(n)] = min(abs(t - tStartVol));
        end
    end
end

%% Then, find slice starts between determined volume starts

LOCS = tapas_physio_create_LOCS_from_VOLLOCS(VOLLOCS, t, sqpar);

end


function build_mmx(verbose)
% BUILD_MMX - compiles mmx() for different platforms and provides help
%            regarding compilation.
%
%  BUILD_MMX will try to compile, in this order, 3 different builds of mmx:
%  mmx_mkl_single    - linked to Intel's single-threaded MKL library (usually fastest)
%  mmx_mkl_multi     - linked to the multithreaded BLAS/LAPACK libraries that come
%                      with Matlab.
%  mmx_naive         - does not link to anything, uses simple C-loops.
%
%  The first time BUILD_MMX succeeds, it will compile again to 'mmx', so
%  that the mex-file mmx should be the fastest possible build on your
%  system.
%
%  BUILD_MMX has been tested on Win32, Win64, OSX, Linux 64
%

% %% FOR LINUX OR MAC SYSTEMS:
%
% To properly link to Intel's MKL, user needs to repackage their libraries
% into one single statically linked library. The instructions are as
% follows:
%
%
% Download Intel MKL for Linux here:
% http://software.intel.com/en-us/articles/non-commercial-software-download/
%
% Donwload Intel MKL for Mac here:
% https://registrationcenter.intel.com/RegCenter/AutoGen.aspx?ProductID=1518&AccountID=&EmailID=&ProgramID=&RequestDt=&rm=EVAL&lang=
%
% The Default installation directory for both Linux and Mac will be
% /opt/intel/
% with the MKL libraries in /opt/intel/mkl
%
% %% To build needed static Library
%    assuming default installation directory
%
% Run the following commands in Linux/Mac terminal:
%
% sudo -s
% cd /opt/intel/mkl/tools/builder
% cat blas_example_list > blas_lapack_list
% cat lapack_example_list >> blas_lapack_list
%
% For Linux 64 bit:
% make libintel64 interface=ilp64 export=blas_lapack_list name=libsingle_mkl_ilp64 threading=sequential
% For Linux 32 bit:
% make libia32 interface=lp64 export=blas_lapack_list name=libsingle_mkl_32 threading=sequential
%
% For Mac:
% make libuni interface=ilp64 export=blas_lapack_list name=libsingle_mkl_ilp64 threading=sequential
%
% A new libsingle_mkl_ilp64.so, libsingle_mkl_32.so, or
% libsingle_mkl_ilp64.dylib will appear.
% This needs to be copied to Matlab's external libraries directory.
%
% For Mac:
% cp libsingle_mkl_ilp64* MATLAB_ROOT/extern/lib/maci64
%
% For Linux 64 bit:
% cp libsingle_mkl_ilp64* MATLAB_ROOT/extern/lib/glnxa64
% For Linux 32 bit:
% cp libsingle_mkl_32* MATLAB_ROOT/extern/lib/glnx86
%
% Where MATLAB_ROOT is the installation directory of your Matlab.

if nargin == 0
   verbose = false;
end

clc

build_names  = {'mmx_mkl_single', 'mmx_mkl_multi','mmx_naive'};

built_mmx   = false;

arch        = computer('arch');

for b = 1:3
   name = build_names{b};

   [link, define]  = deal({});
   [inc_dir, link_dir, Cflags, Lflags]  = deal('');

   switch arch
      case {'win64','win32'}
         switch name
            case 'mmx_naive'
               define   = {'WIN_SYSTEM'};

            case 'mmx_mkl_multi'
               root     = matlabroot;
               if strcmp(arch,'win32')
                  inc_dir  = [root '\extern\lib\win32\microsoft'];
               else
                  inc_dir  = [root '\extern\lib\win64\microsoft'];
               end
               link     = {'libmwblas','libmwlapack'};
               define   = {'WIN_SYSTEM','USE_BLAS'};

            case 'mmx_mkl_single'
               root     = 'C:\Program Files (x86)\Intel\Composer XE 2011 SP1\mkl';
               inc_dir  = [root '\include'];
               if strcmp(arch,'win32')
                  link_dir  = [root '\lib\ia32'];
                  link     = {'mkl_intel_c','mkl_sequential','mkl_core'};
                  define   = {'WIN_SYSTEM','USE_BLAS','MKL_32'};
               else
                  link_dir  = [root '\lib\intel64'];
                  link     = {'mkl_intel_ilp64','mkl_sequential','mkl_core'};
                  define   = {'WIN_SYSTEM','USE_BLAS','MKL_ILP64'};
               end
         end
      case {'glnxa64','glnx86'}
         switch name
            case 'mmx_naive'
               link     = {'pthread'};
               define   = {'UNIX_SYSTEM'};
            case 'mmx_mkl_multi'
               if strcmp(arch,'glnx86')
               inc_dir  = [matlabroot '/extern/lib/glnx86'];
               else
               inc_dir  = [matlabroot '/extern/lib/glnxa64'];
               end
               link     = {'mwblas','mwlapack','pthread'};
               define   = {'UNIX_SYSTEM','USE_BLAS'};
            case 'mmx_mkl_single'
               root = '/opt/intel/mkl';
               inc_dir  = [ root '/include'];
               if strcmp(arch,'glnx86')
                link_dir  = [matlabroot '/extern/lib/glnx86'];
                link     = {'single_mkl_32','pthread'};
                define   = {'UNIX_SYSTEM', 'USE_BLAS', 'MKL_32'};
               else
                link_dir  = [matlabroot '/extern/lib/glnxa64'];
                link     = {'small_mkl_ilp64','pthread'};
                define   = {'UNIX_SYSTEM', 'USE_BLAS', 'MKL_ILP64'};
               end
         end
      case {'maci64'}
         switch name
            case 'mmx_naive'
               link     = {'pthread'};
               define   = {'UNIX_SYSTEM'};

            case 'mmx_mkl_multi'
               root     = matlabroot;
               inc_dir  = [root '/extern/lib/maci64'];
               link     = {'mwblas','mwlapack','pthread'};
               define   = {'UNIX_SYSTEM','USE_BLAS'};

            case 'mmx_mkl_single'
               root     = '/opt/intel/mkl';
               inc_dir  = [ root '/include'];
               link_dir = [matlabroot '/extern/lib/maci64'];
               link     = {'single_mkl_ilp64','pthread'};
               %link     = {'small_mkl_ilp64','pthread'};
               define   = {'UNIX_SYSTEM', 'USE_BLAS', 'MKL_ILP64'};
         end

      otherwise
         error unsupported_architecture
   end

   if ~isempty(link_dir)
      if strcmp(arch,'glnxa64') || strcmp(arch,'maci64')
         L_dir  = {['LDFLAGS="\$LDFLAGS -L' link_dir  ' ' Lflags '"']};
      else
         L_dir  = {['-L' link_dir]};
      end
   else
      L_dir  = {};
   end

   if ~isempty(inc_dir)
      if strcmp(arch,'glnxa64') || strcmp(arch,'maci64')
         I_dir  = {['CXXFLAGS="\$CXXFLAGS -I' inc_dir ' ' Cflags '"']};
      else
         I_dir  = {['-I' inc_dir]};
      end
   else
      I_dir  = {};
   end

   prefix   = @(pref,str_array) cellfun(@(x)[pref x],str_array,'UniformOutput',0);
   l_link   = prefix('-l',link);
   D_define = prefix('-D',define);

   if verbose
      verb  = {'-v'};
   else
      verb  = {};
   end

   try
      check_dir(link_dir, link)
      check_dir(inc_dir)
      clear(name)
      command = {verb{:}, I_dir{:}, L_dir{:}, l_link{:}, D_define{:}}; %#ok<*CCAT>
      fprintf('==========\nTrying to compile ''%s'', using \n',name);
      fprintf('%s, ',command{:})
      fprintf('\n')
      mex(command{:}, '-output', name, 'mmx.cpp');
      fprintf('Compilation of ''%s'' succeeded.\n',name);
      if ~built_mmx
         fprintf('Compiling again to ''mmx'' target using ''%s'' build.\n',name);
         mex(command{:}, '-output','mmx','mmx.cpp');
         built_mmx = true;
      end
   catch err
      fprintf('Compilation of ''%s'' failed with error:\n%s\n',name,err.message);
   end
end

function check_dir(dir,files)
if ~isempty(dir)
   here = cd(dir);
   if nargin == 2
      for i = 1:size(files)
         if isempty(ls(['*' files{i} '.*']))
            cd(here);
            error('could not find file %s', files{i});
         end
      end
   end
   cd(here);
end


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

% modified by Yu Xiang

function tracker = LK_tracking(frame_id, dres_image, dres_det, tracker)

% current frame + motion
J = dres_image.Igray{frame_id};
ctrack = apply_motion_prediction(frame_id, tracker);
w = tracker.dres.w(end);
h = tracker.dres.h(end);
BB3 = [ctrack(1)-w/2; ctrack(2)-h/2; ctrack(1)+w/2; ctrack(2)+h/2];
[J_crop, BB3_crop, bb_crop, s] = LK_crop_image_box(J, BB3, tracker);

num_det = numel(dres_det.x);
for i = 1:tracker.num
    BB1 = [tracker.x1(i); tracker.y1(i); tracker.x2(i); tracker.y2(i)];
    I_crop = tracker.Is{i};
    BB1_crop = tracker.BBs{i};

    % LK tracking
    [BB2, xFJ, flag, medFB, medNCC, medFB_left, medFB_right, medFB_up, medFB_down] = LK(I_crop, J_crop, ...
        BB1_crop, BB3_crop, tracker.margin_box, tracker.level_track);

    BB2 = bb_shift_absolute(BB2, [bb_crop(1) bb_crop(2)]);
    BB2 = [BB2(1)/s(1); BB2(2)/s(2); BB2(3)/s(1); BB2(4)/s(2)];

    ratio = (BB2(4)-BB2(2)) / (BB1(4)-BB1(2));
    ratio = min(ratio, 1/ratio);

    if isnan(medFB) || isnan(medFB_left) || isnan(medFB_right) || isnan(medFB_up) || isnan(medFB_down) ...
            || isnan(medNCC) || ~bb_isdef(BB2) || ratio < tracker.max_ratio
        medFB = inf;
        medFB_left = inf;
        medFB_right = inf;
        medFB_up = inf;
        medFB_down = inf;
        medNCC = 0;
        o = 0;
        score = 0;
        ind = 1;
        angle = -1;
        flag = 2;
        BB2 = [NaN; NaN; NaN; NaN];
    else
        % compute overlap
        dres.x = BB2(1);
        dres.y = BB2(2);
        dres.w = BB2(3) - BB2(1);
        dres.h = BB2(4) - BB2(2);
        if isempty(dres_det.fr) == 0
            overlap = calc_overlap(dres, 1, dres_det, 1:num_det);
            [o, ind] = max(overlap);
            score = dres_det.r(ind);
        else
            o = 0;
            score = -1;
            ind = 0;
        end

        % compute angle
        centerI = [(BB1(1)+BB1(3))/2 (BB1(2)+BB1(4))/2];
        centerJ = [(BB2(1)+BB2(3))/2 (BB2(2)+BB2(4))/2];
        v = compute_velocity(tracker);
        v_new = [centerJ(1)-centerI(1), centerJ(2)-centerI(2)] / double(frame_id - tracker.frame_ids(i));
        if norm(v) > tracker.min_vnorm && norm(v_new) > tracker.min_vnorm
            angle = dot(v, v_new) / (norm(v) * norm(v_new));
        else
            angle = 1;
        end
    end

    tracker.bbs{i} = BB2;
    tracker.points{i} = xFJ;
    tracker.flags(i) = flag;
    tracker.medFBs(i) = medFB;
    tracker.medFBs_left(i) = medFB_left;
    tracker.medFBs_right(i) = medFB_right;
    tracker.medFBs_up(i) = medFB_up;
    tracker.medFBs_down(i) = medFB_down;
    tracker.medNCCs(i) = medNCC;
    tracker.overlaps(i) = o;
    tracker.scores(i) = score;
    tracker.indexes(i) = ind;
    tracker.angles(i) = angle;
    tracker.ratios(i) = ratio;
end

% combine tracking and detection results
% [~, ind] = min(tracker.medFBs);
ind = tracker.anchor;
if tracker.overlaps(ind) > tracker.overlap_box
    index = tracker.indexes(ind);
    bb_det = [dres_det.x(index); dres_det.y(index); ...
        dres_det.x(index)+dres_det.w(index); dres_det.y(index)+dres_det.h(index)];
    tracker.bb = mean([repmat(tracker.bbs{ind}, 1, tracker.weight_tracking) bb_det], 2);
else
    tracker.bb = tracker.bbs{ind};
end

% compute pattern similarity
if bb_isdef(tracker.bb)
    pattern = generate_pattern(dres_image.Igray{frame_id}, tracker.bb, tracker.patchsize);
    nccs = distance(pattern, tracker.patterns, 1); % measure NCC to positive examples
    tracker.nccs = nccs';
else
    tracker.nccs = zeros(tracker.num, 1);
end

if tracker.is_show
    fprintf('\ntarget %d: frame ids ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%d ', tracker.frame_ids(i))
    end
    fprintf('\n');
    fprintf('target %d: medFB ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.medFBs(i))
    end
    fprintf('\n');

    fprintf('target %d: medFB left ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.medFBs_left(i))
    end
    fprintf('\n');

    fprintf('target %d: medFB right ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.medFBs_right(i))
    end
    fprintf('\n');

    fprintf('target %d: medFB up ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.medFBs_up(i))
    end
    fprintf('\n');

    fprintf('target %d: medFB down ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.medFBs_down(i))
    end
    fprintf('\n');

    fprintf('target %d: medNCC ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.medNCCs(i))
    end
    fprintf('\n');

    fprintf('target %d: overlap ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.overlaps(i))
    end
    fprintf('\n');
    fprintf('target %d: detection score ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.scores(i))
    end
    fprintf('\n');
    fprintf('target %d: flag ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%d ', tracker.flags(i))
    end
    fprintf('\n');
    fprintf('target %d: angle ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.angles(i))
    end
    fprintf('\n');
    fprintf('target %d: ncc ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.nccs(i))
    end
    fprintf('\n\n');
    fprintf('target %d: bb overlaps ', tracker.target_id);
    for i = 1:tracker.num
        fprintf('%.2f ', tracker.bb_overlaps(i))
    end
    fprintf('\n\n');

    if tracker.flags(ind) == 2
        fprintf('target %d: bounding box out of image\n', tracker.target_id);
    elseif tracker.flags(ind) == 3
        fprintf('target %d: too unstable predictions\n', tracker.target_id);
    end
end


classdef ConvergencePlotComputerMaterialDesignExperimentPoisson < handle

    properties (Access = public)

    end

    properties (Access = private)
        filesPath
        testNames
        linesData
        barData
        fieldsData
        outPutPlotPath
        alphas
    end

    properties (Access = private)

    end

    methods (Access = public)

        function obj = ConvergencePlotComputerMaterialDesignExperimentPoisson()
            obj.init();
            obj.loadFieldsData();
            obj.plotData();
        end

    end

    methods (Access = private)

        function init(obj)
            obj.filesPath = '/media/alex/MyPassport/MaterialDesign/CStar/';
            obj.outPutPlotPath = '/home/alex/Dropbox/MaterialDesign/CC/Poisson/';
            %obj.linesData = [1:2,5:10];
            %obj.barData = [3:4];
            obj.linesData = [1:6,9:15];
            obj.barData = [7:8];
            obj.testNames = {'NegPoissonNoPerimeter5x6/'};
        end

        function loadFieldsData(obj)
            for iCase = 1:numel(obj.testNames)
                testPath = fullfile(obj.filesPath,obj.testNames{iCase},'/');
                s.testPath  = testPath;
                s.linesData = obj.linesData;
                s.barData   = obj.barData;
                m = MonitoringDataLoader(s);
                obj.fieldsData{iCase} = m.obtainData();
            end
        end

        function plotData(obj)
            p{1} = obj.plotCost();
            p{2} = obj.plotVolum();
            f = figure(1);
            legend({'$||\bf{C}(\rho) - {C}^*||_2$','$\textrm{Vol}(\rho)$'},'Interpreter','latex','Location','Best');
            p = plotPrinter(f,p);
            p.print(fullfile(obj.outPutPlotPath,'CostAndVolume'))
        end

        function p = plotCost(obj)
            for iCase = 1:numel(obj.testNames)
                fieldData = obj.fieldsData{iCase};
                fieldToPlot = 'C - C not scaled';
                [x,y] = obj.obtainField(fieldToPlot,fieldData);
                p = semilogy(x,y);
                hold on
            end
        end

        function p = plotVolum(obj)
            figure(1)
            hold on
            yyaxis right
            for iCase = 1:numel(obj.testNames)
                fieldData = obj.fieldsData{iCase};
                fieldToPlot = 'Volum';
                [x,y] = obj.obtainField(fieldToPlot,fieldData);
                p = plot(x,y);
                ylim([0,1])
                hold on
            end
        end

    end

    methods (Access = private, Static)

        function [xV,yV] = obtainField(fieldName,fieldData)
            for iField = 1:numel(fieldData)
                title = fieldData{iField}.title;
                if strcmp(fieldName,title)
                    xV = fieldData{iField}.xValue;
                    yV = fieldData{iField}.yValue;
                end
            end
        end

    end

end


%% Example
% This script is used to initialize the hand guiding functionality on KUKA
% robot remotely

% Mohammad SAFEEA, 9th of June 2017

% TO use this example:
% 1- Start the server on KUKA
% 2- Start the client on Matlab
% 3- Press the white button and hand guide the robot
% 4- To save the coordinates of the robot, long click on the green button
% 5- Repeat three times
% 6- The robot gives you an interval of five seconds, clear the area around
% the robot directly, the robot will reproduce the motion tought.

close all;clear;clc;
warning('off')
%% Create the robot object
ip='172.31.1.147'; % The IP of the controller
arg1=KST.LBR7R800; % choose the robot iiwa7R800 or iiwa14R820
arg2=KST.Medien_Flansch_Touch_pneumatisch; % choose the type of flange
Tef_flange=eye(4); % transofrm matrix of EEF with respect to flange
iiwa=KST(ip,arg1,arg2,Tef_flange); % create the object

%% Start a connection with the server
flag=iiwa.net_establishConnection();
if flag==0
  return;
end
pause(1);

iiwa.startHandGuiding()

p1 = iiwa.getJointsPos();

iiwa.startHandGuiding()

p2 = iiwa.getJointsPos();

iiwa.startHandGuiding()

p3 = iiwa.getJointsPos();

iiwa.startHandGuiding()

fprintf('CLear the area round the robot, the robot is going to move');

pause(5)

relVel=0.1;
iiwa.movePTPJointSpace(p1, relVel)
iiwa.movePTPJointSpace(p2, relVel)
iiwa.movePTPJointSpace(p3, relVel)

%% turn off the server
iiwa.net_turnOffServer();


function varargout = aibcutpush(varargin)
% VL_AIBCUTPUSH  Quantize based on VL_AIB cut
%  Y = VL_AIBCUTPUSH(MAP, X) maps the data X to elements of the AIB
%  cut specified by MAP.
%
%  The function is equivalent to Y = MAP(X).
%
%  See also: VL_HELP(), VL_AIB().
[varargout{1:nargout}] = vl_aibcutpush(varargin{:});


function net = pairwise(arg)

% PAIRWISE
%
% Construct a pairwise multi-class support vector classification network.
%
% Examples:
%
%    % default constructor (a 0-class pairwise network!)
%
%    net1 = pairwise;
%
%    % copy constructor
%
%    net2 = pairwise(net1);
%
%    % construct pairwise multi-class svc from a vector of two-class networks
%
%    net3 = pairwise(net)

%
% File        : @pairwise/pairwise.m
%
% Date        : Wednesday 13th September 2000
%
% Author      : Dr Gavin C. Cawley
%
% Description : Constructor for a class providing a framework for constructing
%               multi-class support vector classification networks from a set
%               of two-class networks using the pairwise rule.  Part of an
%               object-oriented implementation of Vapnik's Support Vector
%               Machine, as described in [1].
%
% References  : [1] V.N. Vapnik,
%                   "The Nature of Statistical Learning Theory",
%                   Springer-Verlag, New York, ISBN 0-387-94559-8,
%                   1995.
%
% History     : 13/09/2000 - v1.00
%
% Copyright   : (c) Dr Gavin C. Cawley, September 2000
%
%    This program is free software; you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation; either version 2 of the License, or
%    (at your option) any later version.
%
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program; if not, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
%

if nargin == 0

   % this is the default constructor

   net.net = svc;
   net     = class(net, 'pairwise');

elseif nargin == 1

   if isa(arg, 'pairwise');

      % this is the copy constructor

      net = arg;

   end

elseif nargin > 1

   % there are no other constructors

   help pairwise

else

   net.net = arg;
   net     = class(net, 'pairwise');

end

% bye bye...


%kEllipse 'Ellipse Object Menuform'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros Ellipse.pane file
%
% Parameters:
%
% Example: kEllipse( {})
%
% Khoros helpfile follows below:
% .begin tagged
%
% .item "The Ellipse Visual Object"
% An ellipse visual object supports the display of an ellipse annotation.
% The (x, y) location of the ellipse, its width and height may be specified in
% device coordinates or world coordinates by the application.  The menuform,
% however, displays the world coordinates of the ellipse, regardless of whether
% the application used world or device coordinates to specify that location.
% Attributes of the circle object that can be interactively specified in addition
% to its world coordinates include its border width, border type, border color,
% whether or not it is filled, and (if it is filled), its fill color.
%
% .item "Filled"
% When set to true, this attribute indicates that the ellipse should be filled
% with the fill color (background color).
%
% .item "Border Width"
% This list selection allows you to set the line width used by
% the ellipse object. Line widths range from "Extra Fine" to "Extra Wide".
%
% .item "Border Type"
% This list selection lets you set the line type used on
% the ellipse object. There are seven line types, including "Solid",
% "Dotted", "Dot Dash", "Short Dash", "Long Dash", "Odd Dash", and
% "Grid Dotted".
%
% .item "Border Color"
% This stringlist selection lets you set the border color (foreground color)
% of the ellipse object, using the color name.
%
% .item "Fill Color"
% This stringlist selection lets you set the fill color (background color)
% of the ellipse object, using the color name.
%
% .item "X Circle, Y Circle"
% This pair of double values specifies the (x, y) location of the ellipse object
% on its parent in world coordinates.  The world coordinate range of the ellipse
% is specified by the application, and thus varies depending on context.
%
% .item "Width"
% This double value specifies the width of the ellipse in world coordinates.
%
% .item "Height"
% This double value specifies the height of the ellipse in world coordinates.
%
% .end tagged

function varargout = kEllipse(varargin)
Inputs={};
if nargin ==0
  arglist={'',''};
elseif nargin ==1
  arglist=varargin{1};
else error('Usage: [out1,..] = kEllipse(arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={};
maxval={};
minval={};
istoggle=[];
was_set=istoggle * 0;
paramtype={};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=0; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(0);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
callKhoros([w 'Ellipse" '],Inputs,narglist);


% IMAGESCTC - DEPRECATED. never completed or documented.
% IMAGESCTC - imagesc in true color. Can help plot different
%               colormap on the same window.
%
% Usage: same as imagesctc
%
% Example:
%         figure;
%         colormap(jet);
%         subplot(1,2,1); imagesctc(rand(10,10));
%         colormap(gray);
%         subplot(1,2,2); imagesctc(rand(10,10));
%
% Author: Arnaud Delorme, CNL / Salk Institute, 31 July 2002
%
% See also: IMAGE, IMAGESC

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

if ~isempty(varargin)
	imagesc(a, varargin{:});
else
	imagesc(a);
end
c = caxis;

cm = colormap;
intervals = linspace(c(1), c(2), size(cm,1));
[tmp dest] = histc(a(:), intervals);

for wi = 1:size(a,2)
	for hi = 1:size(a,1)
		aa(hi,wi,1:3) = cm(dest(hi+(wi-1)*size(a,1)),1:3);
	end
end
if ~isempty(varargin)
	imagesc(aa, varargin{:});
else
	imagesc(aa);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cb = compactbit(b)
%
% b = bits array
% cb = compacted string of bits (using words of 'word' bits)

[nSamples nbits] = size(b);
nwords = ceil(nbits/8);
cb = zeros([nSamples nwords], 'uint8');

for j = 1:nbits
    w = ceil(j/8);
    cb(:,w) = bitset(cb(:,w), mod(j-1,8)+1, b(:,j));
end


%testGrowthExpMatch tests the functionality of all the components of growthExpMatch
%
%Procedure to run growthExpMatch:
%(1) obtain all input files (ie. model, CompAbr, and KEGGID are from BiGG, KEGGList is from KEGG website)
%(2) remove desired reaction from model with removeRxns, or set the model
%       on a particular Carbon or Nitrogen source
%(3) create an SUX Matrix by using the function MatricesSUX =
%       generateSUXMatrix(model,Dictionary, KEGGFilename,Compartment)
%(4) run it through growthExpMatch using [solution,b,solInt]=Smiley(MatricesSUX)
%(5) solution.importedRxns contains the solutions to all iterations (in
%       this particular test, we removed reaction ENO from model and
%       obtained 'R00658_f' which is the KEGGID for ENO
%
%   Joseph Kang 11/16/09
%   Adaptions to CI - Thomas Pfau Okt 2017

global CBTDIR

oriFolder = pwd;

%moves to testing folder that contains testGrowthExpMatch
test_folder = fileparts(which('testGrowthExpMatch.m'));
cd(test_folder);

%load Model
model = getDistributedModel('ecoli_core_model.mat');

%removes reaction ENO
disp('------------------------------------')
disp('Removing reaction, ENO, from test model:')
model = removeRxns(model, 'ENO');

%moves to folder w/ input files
% w = what('testing');
% p = w.path;
% cd(p);
d = load([test_folder filesep 'Dictionary.mat']);
KEGGFilename = 'testTest_KEGG_Reaction_List.lst';

%Test for the default solvers
solverPkgs = {'gurobi', 'glpk'};

for k = 1:length(solverPkgs)
    fprintf('   Running solveCobraLPCPLEX using %s ... ', solverPkgs{k});

    % change the COBRA solver
    solverOKLP = changeCobraSolver(solverPkgs{k}, 'LP', 0);
    solverOKMILP = changeCobraSolver(solverPkgs{k}, 'MILP', 0);

    if solverOKLP && solverOKMILP
        %runs growthExpMatch and obtains solution
        [solution]=growthExpMatch(model, KEGGFilename,'[c]', 1, d.dictionary);

        %if R00658_f is solution result, returns a positive answer, else negative
        assert(isequal(solution.importedRxns, {{'R00658_f'}}))
    end
end
%perform cleanup
if exist([test_folder filesep 'GEMLog_solution_1.mat'],'file')
    delete('GEMLog_solution_1.mat')
end
if exist([test_folder filesep 'CobraMILPSolver.log'],'file')
    delete('CobraMILPSolver.log')
end
if exist([test_folder filesep 'GEMLog.txt'],'file')
    delete('GEMLog.txt')
end
close all

cd(oriFolder);


function makespectrogramthumbnails(spectrogramFilename, spectrogramFraction)
debug.printfunctionstack('>');

% figure 1 should be a large spectrogram with traces, cropped nicely. Now remove labels and maximise panels.

ax=get(gcf, 'Children');

% Work out number of channels
pos1 = get(ax(1),'position'); % top trace
pos2 = get(ax(2),'position'); % top sgram
height_per_channel = pos1(4) + pos2(4);
numchannels = 0.95 / height_per_channel;
numpanels = numchannels * 2;

new_height_per_channel = 1 / numchannels;

% Remove all axes, tickmarks, labels, and axis boxes and title from view
for c=1:numpanels
    set(ax(c), 'Visible', 'off')
end

% Move panels
for channelNum = 1:numchannels
    [spectrogramPosition, tracePosition] = iceweb.calculatePanelPositions(numchannels, numchannels - channelNum + 1, spectrogramFraction, 0.0, 0.0, 1, 1);
    set(ax(channelNum*2 - 1), 'position', tracePosition);
    set(ax(channelNum*2), 'position', spectrogramPosition);
end

% we need a name for the labelless large spectrogram
[tmppath, tmpbase, tmpext] = fileparts(spectrogramFilename);
tmpfile = sprintf('%s/%s_labelless%s',tmppath,tmpbase,tmpext);

% print large labelless PNG
iceweb.saveImageFile(tmpfile, 72);

% load then delete temporary file
I = imread(tmpfile);
delete(tmpfile)

% Resize the image (aspect ratio 16:21 same as 576:756) and convert it to an indexed image with 256 colors
% (Note: we were originally creating 150x96, which is far off the aspect ratio of large spectrograms)
%[X,map] = rgb2ind(imresize(I, [126 96]), 256);
%thumbnailfile = sprintf('%s/smallest_%s%s',tmppath, tmpbase, tmpext);
%imwrite(X,map,thumbnailfile,'PNG');
%[X,map] = rgb2ind(imresize(I, [147 112]), 256);
%thumbnailfile = sprintf('%s/smaller_%s%s',tmppath, tmpbase, tmpext);
%imwrite(X,map,thumbnailfile,'PNG');
[X,map] = rgb2ind(imresize(I, [198 151]), 256);
thumbnailfile = sprintf('%s/%s_thumb%s',tmppath, tmpbase, tmpext);
imwrite(X,map,thumbnailfile,'PNG');
close;

debug.printfunctionstack('<');


% Description : This is a matlab file for video enhancement.
% The following file works with color, b&w images.
% User defined functions : 1) image_enhancement_sw - modified retinex algorithm
%                          2) gray_level_images    - contrast enhancement
% Author : Vijay Sridharan.
% Date   : 20th July, 2012.
%% Video Enhancement for color and black and white images
video = VideoReader('air.avi');
    for i = 1:video.NumberOfFrames
        img = read(video,i);
        imwrite(img,sprintf('img%d.jpg',i));
    end
    fprintf('Please wait....');
    filebase = dir('*.jpg');                      % If you are using a continous frame of images, start from here.
    num_files = numel(filebase);
    images = cell(1, num_files);
     MS=cell(1,num_files);
     for k = 1:num_files
         images{k} = imread(filebase(k).name);
         [rows columns color]=size(images{1});
         if (color==3)
             MS{k}=image_enhancement_sw(images{k});
             M(k)=im2frame(MS{k});
         else
             MS{k}=gray_level_images(images{k}); % For gray level images, movie/implay doesn't work. So, I would suggest you
                                                 % to download FIJI or ImageJ to view the sequence of images as Videos. The
         end                                     % default frame rate is 8fps in FIJI
     end
movie(M)


function Script_CECLM_cross_data()

addpath(genpath('../'));

[images, detections, labels] = Collect_menpo_imgs('G:\Datasets\menpo/');

%% loading the CE-CLM model and parameters
[patches, pdm, clmParams, early_term_params] = Load_CECLM_general();
views = [0,0,0; 0,-30,0; 0,30,0; 0,-55,0; 0,55,0; 0,0,30; 0,0,-30; 0,-90,0; 0,90,0; 0,-70,40; 0,70,-40];
views = views * pi/180;

% As early termination weights were trained on part of menpo turn them off,
% to perform a clean cross-data experiment
early_term_params.weights_scale(:) = 1;
early_term_params.weights_add(:) = 0;
early_term_params.cutoffs(:) = -0.2;

% Use the multi-hypothesis model, as bounding box tells nothing about
% orientation
multi_view = true;

%% Setup recording
experiment.params = clmParams;

num_points = numel(pdm.M)/3;

shapes_all = cell(numel(images), 1);
labels_all = cell(numel(images), 1);
lhoods = zeros(numel(images),1);
all_lmark_lhoods = zeros(num_points, numel(images));
all_views_used = zeros(numel(images),1);

% Change if you want to visualize the outputs
verbose = false;
output_img = false;

if(output_img)
    output_root = './ceclm_gen_out/';
    if(~exist(output_root, 'dir'))
        mkdir(output_root);
    end
end
if(verbose)
    f = figure;
end
%% Fitting the model to the provided images
tic
for i=1:numel(images)
    image = imread(images(i).img);
    image_orig = image;

    if(size(image,3) == 3)
        image = rgb2gray(image);
    end

    bbox = squeeze(detections(i,:));

    % The actual work get's done here
    [shape,~,~,lhood,lmark_lhood,view_used] =...
        Fitting_from_bb_multi_hyp(image, [], bbox, pdm, patches, clmParams, views, early_term_params);

    all_lmark_lhoods(:,i) = lmark_lhood;
    all_views_used(i) = view_used;

    shapes_all{i} = shape;
    labels_all{i} = labels{i};

    if(mod(i, 200)==0)
        fprintf('%d done\n', i );
    end

    lhoods(i) = lhood;

    if(output_img)
        v_points = logical(patches(1).visibilities(view_used,:))';
        DrawFaceOnImg(image_orig, shape, sprintf('%s/%s%d.jpg', output_root, 'fit', i), bbox, v_points);
    end

    if(verbose)
        v_points = logical(patches(1).visibilities(view_used,:))';
        DrawFaceOnFig(image_orig, shape, bbox, v_points);
    end

end
toc

experiment.lhoods = lhoods;
experiment.shapes = shapes_all;
experiment.labels = labels_all;
experiment.all_lmark_lhoods = all_lmark_lhoods;
experiment.all_views_used = all_views_used;

%%
output_results = 'results/results_ceclm_cross-data.mat';
save(output_results, 'experiment');

end


function [ctHandle,cMap,window] = matRad_plotCtSlice(axesHandle,ctCube,cubeIdx,plane,slice,cMap,window)
% matRad function that generates the plot for the CT in the GUI
% The function can also be used in personal matlab figures by passing the
% corresponding axes handle
%
% call
%   [ctHandle,cMap,window] = matRad_plotCtSlice(axesHandle,ctCube,cubeIdx,plane,slice)
%   [ctHandle,cMap,window] = matRad_plotCtSlice(axesHandle,ctCube,cubeIdx,plane,slice,cMap)
%   [ctHandle,cMap,window] = matRad_plotCtSlice(axesHandle,ctCube,cubeIdx,plane,slice,window)
%   [ctHandle,cMap,window] = matRad_plotCtSlice(axesHandle,ctCube,cubeIdx,plane,slice,cMap,window)
%
% input
%   axesHandle  handle to axes the slice should be displayed in
%   ctCube      the cell of ct cubes
%   cubeIdx     Index of the desired cube in the ct struct
%   plane       plane view (coronal=1,sagittal=2,axial=3)
%   slice       slice in the selected plane of the 3D cube
%   cMap        optional argument defining the colormap, default is bone
%               if you want to use the default map with the window argument
%               you can use an empty array []
%   window      optional argument defining the displayed range. default is
%               [min(ctCube(:)) max(ctCube(:))]
%
% output
%   ctHandle    handle of the plotted CT axes
%   cMap        used colormap (same as input if set)
%   window      used window (same as input if set)
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

matRad_cfg = MatRad_Config.instance();

%Use default colormap?
if nargin < 6 || isempty(cMap)
    cMap = bone(64);
end

if nargin < 7 || isempty(window)
    window = [min(ctCube{cubeIdx}(:)) max(ctCube{cubeIdx}(:))];
end

cMapScale = size(cMap,1) - 1;

%Prepare the slice and convert it to uint8
if plane == 1 % Coronal plane
	ctIndexed = uint8(cMapScale*(squeeze((ctCube{cubeIdx}(slice,:,:)-window(1))/(window(2) - window(1)))));
elseif plane == 2 % sagittal plane
    ctIndexed = uint8(cMapScale*(squeeze((ctCube{cubeIdx}(:,slice,:)-window(1))/(window(2) - window(1)))));
elseif plane == 3 % Axial plane
    ctIndexed = uint8(cMapScale*(squeeze((ctCube{cubeIdx}(:,:,slice)-window(1))/(window(2) - window(1)))));
else
	matRad_cfg.dispError('Invalid plane ''%d'' selected for visualization!',plane);
end

%This circumenvents a bug in Octave when the index in the image hase the maximum value of uint8
if matRad_cfg.isOctave
	ctIndexed(ctIndexed == 255) = 254;
end

ct_rgb = ind2rgb(ctIndexed,cMap);

ctHandle = image('CData',ct_rgb,'Parent',axesHandle);

end


function [f, lb, ub, A_x, be_u_abs, be_l_abs, s_current_m, error_state] = ...
    prepareOptimizationProblem(VehicleDynamicState, dot_d_numerical, PathPos, v_traj, ...
    ax_diff_traj, ax_traj, ay_traj, ax_lim_mps2, ay_lim_mps2, ...
    d_lim_ub_m, d_lim_lb_m, d_Target_m, dot_d_Target_mps, ...
    vx_lin, kappa_lin, UncertaintyTube, v_terminal_mps, ax_traj_old, ay_traj_old, solution_old, ...
    sys, P_VDC_VirtualController, drag_coefficient, roh_air, vehiclemass_kg, ...
    P_VDC_PositiveAxLimScale, P_VDC_EnableNumLatErrorDer_b, P_VDC_MaxTightening, ...
    P_VDC_MinVelSlipCalc_mps)

% function prepareOptimizationProblem
% Authors:       Martin Euler
%                Salih Guemues
%                Alexander Wischnewski
%
% Description:
%   function used to generate the constraint matrices for the TMPC-Controller
% Inputs/parameters:
%   VehicleDynamicState:    Dynamic state of the racecar
%   PathPos:                Vehicle position on the target path
%   v_traj:                 Target trajectory for the PH (prediction horizon)
%   ax_diff_traj:           Derivative of the target trajectory velocity in the PH
%   ax_traj:                Longitudinal acceleration for the target trajectory in the PH
%   ay_traj:                Lateral acceleration for the target trajectory in the PH
%   ax_lim_mps2             Longitudinal acceleration limits of the target trajectory in the PH
%   ay_lim_mps2             Lateral acceleration limits of the target trajectory in the PH
%   d_lim_ub_m              Size of the driving tube to the right side
%   d_lim_lb_m              Size of the driving tube to the left side
%   d_Target_m              Target trajectory projected into the curvilinear coordinate frame
%   dot_d_Target_mps        Derivative of the target trajectory projected into the c.l. coordinates
%   vx_lin                  Linearization velocity profile
%   kappa_lin               Linearization curvature profile
%   UncertaintyTube         Uncertainy ellipsoids for predicted uncertainty tube
%   v_terminal_set          Terminal set speed
%   ax_traj_old             Previous iteration target trajectory long. acceleration
%   ay_traj_old             Previous iteration target trajectory lat. acceleration
%   solution_old            Previous iteration solution
%   P_VDC_VirtualController Virtual feedback controller used for uncertainty tube calculation
%   drag_coefficient        Vehicle drag coefficient
%   roh_air                 Air density
%   vehiclemass_kg          Vehicle mass
%   P_VDC_PositiveAxLimScale Scaling factor for positive accelerations to consider RWD
%
% Outputs:
%
%   f:                      linear weight matrix for QP
%   lb:                     lower bound vector for inequality constraints
%   ub:                     upper bound vector for inequality constraints
%   A_x:                    nonzero-value-vector of inequality constraint coefficient matrix A_ineq
%   be_u_abs:               upper terminal set bounds in absolute coordinates
%   be_l_abs:               lower terminal set bounds in absolute coordinates
%   s_current:              Vehicle position in raceline coordinates
%   errorState:             current errorState of the system

%% ------- define optimization problem variables ----------------------- %%
% get UNSCALED (in terms of preconditioning) constraint matrices/vectors
A_ineq = zeros(sys.osqp_m, sys.osqp_n);
A_ineq(sys.A_i_lin) = sys.A_x_par;
lb = sys.l_par;
ub = sys.u_par;

%% ------------------------- calculate current error state ------------------------------- %%
% set s_0 to current distance on racetrack
s_current_m = PathPos.s_m;
% calculate assumed velocity. limit to lower value to prevent linearization issues.
v_mps = max(VehicleDynamicState.v_mps, 0.5*P_VDC_MinVelSlipCalc_mps);
% velocity difference: delta_vx = v_path - v_car;
delta_vx = v_traj(1) - v_mps;
% lateral error delta_y as output from local_path_matching with added initial offset
d = PathPos.d_m;
% Delta between vehicle and path heading by interpolation
d_psi = PathPos.psi_rad + VehicleDynamicState.beta_rad;
% lateral error dynamics (either numerical or analytical)
if(P_VDC_EnableNumLatErrorDer_b)
    d_dot = dot_d_numerical;
else
    d_dot = v_mps * sin(d_psi);
end
% current state vector for controller (state vector of error diff. equat.)
error_state = [delta_vx; d; d_dot];
% calculate system response to initial state
x0_resp = sys.Ax0_MPC*error_state;

%% ----------- calc linear weight matrix for QP-Solver ------------%
f = sys.osqp_qpar + 2*(x0_resp'*sys.f_x0)' - 2*(d_Target_m'*sys.f_d_m)' ...
    - 2*(dot_d_Target_mps'*sys.f_dot_d_mps)' ...
    - 2*([ax_traj_old(1); ax_traj]'*sys.f_Dax)' ...
    + 2*([ay_traj_old(1); ay_traj]'*sys.f_Day)' ...
    + 2*(solution_old(1)*sys.f_D_deltaax)' + 2*(solution_old(2)*sys.f_D_deltaay)';
% its minus the ax term since the definitions of delta ax and delta ay are changed

%% - calculation of input and state constraints within prediction horizon - %%
for j = 1:1:sys.N_hor
    % get uncertainty matrix for the inputs
    M_states_current = UncertaintyTube(:, (j-1)*3+1:(j-1)*3+3);
    M_inputs_current = P_VDC_VirtualController*M_states_current*P_VDC_VirtualController';
    % calculate tightenings for all state variables and inputs
    tight_delta_v_mps = sqrt(M_states_current(1, 1));
    tight_d_m = sqrt(M_states_current(2, 2));
    tight_dot_d_mps = sqrt(M_states_current(3, 3));
    % the factors here are swapped with respect to the inputs following this scheme
    % such that only upper bounds are given as the ax_lim has to be modified for the upper bounds
    % ay_lim*ax_util + P_VDC_PositiveAxLimScale*ax_lim*ay_util <= P_VDC_PositiveAxLimScale*ax_lim*ay_lim
    tight_tire1 = sqrt([ay_lim_mps2(j), P_VDC_PositiveAxLimScale*ax_lim_mps2(j)]*M_inputs_current*[ay_lim_mps2(j); P_VDC_PositiveAxLimScale*ax_lim_mps2(j)]);
    % -ay_lim*ax_util + -ax_lim*ay_util <= ax_lim*ay_lim
    tight_tire2 = sqrt([-ay_lim_mps2(j), -ax_lim_mps2(j)]*M_inputs_current*[-ay_lim_mps2(j); -ax_lim_mps2(j)]);
    % -ay_lim*ax_util + ax_lim*ay_util <= ax_lim*ay_lim
    tight_tire3 = sqrt([-ay_lim_mps2(j), ax_lim_mps2(j)]*M_inputs_current*[-ay_lim_mps2(j); ax_lim_mps2(j)]);
    % ay_lim*ax_util + -P_VDC_PositiveAxLimScale*ax_lim*ay_util <= P_VDC_PositiveAxLimScale*ax_lim*ay_lim
    tight_tire4 = sqrt([ay_lim_mps2(j), -P_VDC_PositiveAxLimScale*ax_lim_mps2(j)]*M_inputs_current*[ay_lim_mps2(j); -P_VDC_PositiveAxLimScale*ax_lim_mps2(j)]);
    % calculate normalization factor for acceleration constraints
    norm_acc_con_upper = 1/(P_VDC_PositiveAxLimScale*ax_lim_mps2(j)*ay_lim_mps2(j));
    norm_acc_con_lower = 1/(ax_lim_mps2(j)*ay_lim_mps2(j));

    % limit tigthening to given factor
    tight_d_m = min(d_lim_ub_m(j)*P_VDC_MaxTightening, tight_d_m);
    % get tightened admissible lateral error for current step
    ub(sys.n_constr*j) = d_lim_ub_m(j) - tight_d_m - x0_resp(2+(j-1)*sys.n_sys);
    lb(sys.n_constr*j) = d_lim_lb_m(j) + tight_d_m - x0_resp(2+(j-1)*sys.n_sys);

    % prepare linearization around d = 0, dot_d = 0 and vx = vx_pred for longitudinal acceleration
    ax_grad_d = ax_diff_traj(j)*kappa_lin(j)*vx_lin(j);
    ax_grad_v = ax_diff_traj(j) + roh_air*drag_coefficient*vx_lin(j)/vehiclemass_kg;
    % prepare full gradient for easier specification of constraints
    ax_grad_states = ay_lim_mps2(j)*[ax_grad_v, ax_grad_d, 0];
    ax_grad_inputs = ay_lim_mps2(j)*[-1, 0]; % this is minus one since delta_ax is different sign
    ax_op = ax_diff_traj(j)*(2*vx_lin(j) - v_traj(j)) + 0.5*roh_air*drag_coefficient*vx_lin(j)^2/vehiclemass_kg;

    % prepare linearization around d = 0, dot_d = 0 and vx = vx_pred for lateral acceleration
    ay_grad_d = kappa_lin(j)^2*vx_lin(j)^2;
    ay_grad_dot_d = -ax_diff_traj(j);
    ay_grad_v = -2*kappa_lin(j)*vx_lin(j);
    % prepare full gradient for easier specification of constraints
    ay_grad_states_upper = P_VDC_PositiveAxLimScale*ax_lim_mps2(j)*[ay_grad_v, ay_grad_d, ay_grad_dot_d];
    ay_grad_inputs_upper = P_VDC_PositiveAxLimScale*ax_lim_mps2(j)*[0, 1];
    ay_grad_states_lower = ax_lim_mps2(j)*[ay_grad_v, ay_grad_d, ay_grad_dot_d];
    ay_grad_inputs_lower = ax_lim_mps2(j)*[0, 1];
    ay_op = kappa_lin(j)*vx_lin(j)^2 + 2*kappa_lin(j)*vx_lin(j)*(v_traj(j) - vx_lin(j));

    % update gradients for states and inputs in constraint matrix
    % ay_lim*ax_util + P_VDC_PositiveAxLimScale*ax_lim*ay_util <= P_VDC_PositiveAxLimScale*ax_lim*ay_lim
    A_ineq(sys.n_constr*(j-1)+1, 1:sys.N_hor*sys.m_sys) = ...
        (ax_grad_states+ay_grad_states_upper)*sys.ABK_MPC(1+(j-1)*sys.n_sys:j*sys.n_sys, 1:sys.N_hor*sys.m_sys);
    A_ineq(sys.n_constr*(j-1)+1, sys.m_sys*(j-1)+1:sys.m_sys*j) = ...
        (A_ineq(sys.n_constr*(j-1)+1, sys.m_sys*(j-1)+1:sys.m_sys*j) + ax_grad_inputs + ay_grad_inputs_upper);
    % -ay_lim*ax_util + -ax_lim*ay_util <= ax_lim*ay_lim
    A_ineq(sys.n_constr*(j-1)+2, 1:sys.N_hor*sys.m_sys) = ...
        -(ax_grad_states+ay_grad_states_lower)*sys.ABK_MPC(1+(j-1)*sys.n_sys:j*sys.n_sys, 1:sys.N_hor*sys.m_sys);
    A_ineq(sys.n_constr*(j-1)+2, sys.m_sys*(j-1)+1:sys.m_sys*j) = ...
        (A_ineq(sys.n_constr*(j-1)+2, sys.m_sys*(j-1)+1:sys.m_sys*j) - ax_grad_inputs - ay_grad_inputs_lower);
    % -ay_lim*ax_util + ax_lim*ay_util <= ax_lim*ay_lim
    A_ineq(sys.n_constr*(j-1)+3, 1:sys.N_hor*sys.m_sys) = ...
        (-ax_grad_states+ay_grad_states_lower)*sys.ABK_MPC(1+(j-1)*sys.n_sys:j*sys.n_sys, 1:sys.N_hor*sys.m_sys);
    A_ineq(sys.n_constr*(j-1)+3, sys.m_sys*(j-1)+1:sys.m_sys*j) = ...
        (A_ineq(sys.n_constr*(j-1)+3, sys.m_sys*(j-1)+1:sys.m_sys*j) - ax_grad_inputs + ay_grad_inputs_lower);
    % ay_lim*ax_util + -P_VDC_PositiveAxLimScale*ax_lim*ay_util <= P_VDC_PositiveAxLimScale*ax_lim*ay_lim
    A_ineq(sys.n_constr*(j-1)+4, 1:sys.N_hor*sys.m_sys) = ...
        -(-ax_grad_states+ay_grad_states_upper)*sys.ABK_MPC(1+(j-1)*sys.n_sys:j*sys.n_sys, 1:sys.N_hor*sys.m_sys);
    A_ineq(sys.n_constr*(j-1)+4, sys.m_sys*(j-1)+1:sys.m_sys*j) = ...
        (A_ineq(sys.n_constr*(j-1)+4, sys.m_sys*(j-1)+1:sys.m_sys*j) + ax_grad_inputs - ay_grad_inputs_upper);

    % scale matrices appropriately
    A_ineq(sys.n_constr*(j-1)+1, 1:sys.N_hor*sys.m_sys) = ...
        norm_acc_con_upper*A_ineq(sys.n_constr*(j-1)+1, 1:sys.N_hor*sys.m_sys);
    A_ineq(sys.n_constr*(j-1)+2, 1:sys.N_hor*sys.m_sys) = ...
        norm_acc_con_lower*A_ineq(sys.n_constr*(j-1)+2, 1:sys.N_hor*sys.m_sys);
    A_ineq(sys.n_constr*(j-1)+3, 1:sys.N_hor*sys.m_sys) = ...
        norm_acc_con_lower*A_ineq(sys.n_constr*(j-1)+3, 1:sys.N_hor*sys.m_sys);
    A_ineq(sys.n_constr*(j-1)+4, 1:sys.N_hor*sys.m_sys) = ...
        norm_acc_con_upper*A_ineq(sys.n_constr*(j-1)+4, 1:sys.N_hor*sys.m_sys);

    % limit tire tightening to resonable values
    tight_tire1 = min(P_VDC_PositiveAxLimScale*ax_lim_mps2(j)*ay_lim_mps2(j)*P_VDC_MaxTightening, ...
                        tight_tire1);
    tight_tire2 = min(ax_lim_mps2(j)*ay_lim_mps2(j)*P_VDC_MaxTightening, ...
                        tight_tire2);
    tight_tire3 = min(ax_lim_mps2(j)*ay_lim_mps2(j)*P_VDC_MaxTightening, ...
                        tight_tire3);
    tight_tire4 = min(P_VDC_PositiveAxLimScale*ax_lim_mps2(j)*ay_lim_mps2(j)*P_VDC_MaxTightening, ...
                        tight_tire4);

    % ay_lim*ax_util + P_VDC_PositiveAxLimScale*ax_lim*ay_util <= P_VDC_PositiveAxLimScale*ax_lim*ay_lim
    ub(sys.n_constr*(j-1)+1) = norm_acc_con_upper*(-ax_op*ay_lim_mps2(j) ...
        - ay_op*P_VDC_PositiveAxLimScale*ax_lim_mps2(j) ...
        - ax_grad_states*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        - ay_grad_states_upper*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        - tight_tire1 + P_VDC_PositiveAxLimScale*ax_lim_mps2(j) * ay_lim_mps2(j));
    % -ay_lim*ax_util + -ax_lim*ay_util <= ax_lim*ay_lim
    ub(sys.n_constr*(j-1)+2) = -norm_acc_con_lower*(-ax_op*ay_lim_mps2(j) ...
        - ay_op*ax_lim_mps2(j) ...
        - ax_grad_states*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        - ay_grad_states_lower*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        + tight_tire2 - ax_lim_mps2(j) * ay_lim_mps2(j));
    % -ay_lim*ax_util + ax_lim*ay_util <= ax_lim*ay_lim
    ub(sys.n_constr*(j-1)+3) = norm_acc_con_lower*(ax_op*ay_lim_mps2(j) ...
        - ay_op*ax_lim_mps2(j) ...
        + ax_grad_states*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        - ay_grad_states_lower*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        - tight_tire3 + ax_lim_mps2(j) * ay_lim_mps2(j));
    % ay_lim*ax_util + -P_VDC_PositiveAxLimScale*ax_lim*ay_util <= P_VDC_PositiveAxLimScale*ax_lim*ay_lim
    ub(sys.n_constr*(j-1)+4) = -norm_acc_con_upper*(ax_op*ay_lim_mps2(j) ...
        - ay_op*P_VDC_PositiveAxLimScale*ax_lim_mps2(j) ...
        + ax_grad_states*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        - ay_grad_states_upper*x0_resp(1+(j-1)*sys.n_sys:j*sys.n_sys) ...
        + tight_tire4 - P_VDC_PositiveAxLimScale*ax_lim_mps2(j) * ay_lim_mps2(j));
end

%% -------------------- terminal state constraints ----------------------- %%
be_u = zeros(3, 1);
be_l = zeros(3, 1);
be_u(1) = inf;
be_l(1) = v_traj(end) - v_terminal_mps;

% lateral error is constrained to be
tight_terminal_d_m = sqrt(UncertaintyTube(2, end-1));
tight_terminal_d_m = min(d_lim_ub_m(end)*P_VDC_MaxTightening, tight_terminal_d_m);
be_u(2) = d_lim_ub_m(end) - tight_terminal_d_m;
be_l(2) = d_lim_lb_m(end) + tight_terminal_d_m;
% calculate lateral erorr speed as average of the last five values as they might be noisy
be_u(3) = mean(dot_d_Target_mps(end-5:end));
be_l(3) = mean(dot_d_Target_mps(end-5:end));
% write terminal constraints to logs
be_u_abs = be_u;
be_l_abs = be_l;
% add offset velocity to make logging meaningful
be_u_abs(1) = v_traj(end) - be_u_abs(1);
be_l_abs(1) = v_traj(end) - be_l_abs(1);
% calculate ub for terminal constraint on states
ub(sys.n_constr*sys.N_hor+1:sys.n_constr*sys.N_hor+sys.n_sys) = be_u - ...
    x0_resp(sys.N_hor*sys.n_sys+1:(sys.N_hor+1)*sys.n_sys);
% calculate lb for terminal constraint on states
lb(sys.n_constr*sys.N_hor+1:sys.n_constr*sys.N_hor+sys.n_sys) = be_l - ...
    x0_resp(sys.N_hor*sys.n_sys+1:sys.n_sys*(sys.N_hor+1));
% no terminal constraints on longitudinal acceleration
ub(sys.n_constr*sys.N_hor+sys.n_sys+1) = inf;
lb(sys.n_constr*sys.N_hor+sys.n_sys+1) = -inf;
% lateral acceleration is constrained to be close to target
ub(sys.n_constr*sys.N_hor+sys.n_sys+2) = 0;
lb(sys.n_constr*sys.N_hor+sys.n_sys+2) = 0;

%% --------------------- setup slack constraints ------------------------------------- %%
% limit slacks to be strictly positive
lb(sys.N_hor*sys.n_constr+sys.n_sys+sys.m_sys+1:end) = 0;
% upper limit of tire slacks set to certain percentage of maximum available accelerations
ub(sys.N_hor*sys.n_constr+sys.n_sys+sys.m_sys+1:sys.n_slacks:end) = sys.slack_lim_rel;
ub(sys.N_hor*sys.n_constr+sys.n_sys+sys.m_sys+2:sys.n_slacks:end) = sys.slack_lim_rel;
% upper limit of lateral error slacks set to equal amount of allowed deviation
% which allows to go for the double amount if required
ub(sys.N_hor*sys.n_constr+sys.n_sys+sys.m_sys+3:sys.n_slacks:end) = 0.5*(d_lim_ub_m(1)-d_lim_lb_m(1));
ub(sys.N_hor*sys.n_constr+sys.n_sys+sys.m_sys+4:sys.n_slacks:end) = 0.5*(d_lim_ub_m(1)-d_lim_lb_m(1));

%% ----------- transform A_ineq and P to CSC compatible format using sparsity pattern ----------- %%
A_x = A_ineq(sys.A_i_lin);

end


function [fg, fgRemains, fgLeft, fgRight]=dtiSplitInterhemisphericFibers(fg, dt, maxZ)
%Split fibers that cross between hemispheres below a certain z-coordinate.
%
%   fg=dtiSplitInterhemisphericFibers(fg, dt, [Z=-10])
%
% Due to tractography artifacts, whole brain tractography often produces
% fibers which connect homologous motor cortices, crossing between
% hemispheres at the pons level. This is anatomically implausible: these
% fibers mostl likely cross at the pons level, connecting spinal tract and
% contralateral moter cortex. This function splits interhemisperic fibers,
% sparing (dep. on option chosen) either CC, or ACPC & above. Note:
% cerebellar fibers WILL be split as well. Too bad.
%
% Input parameters:
% fg      - fiber group structure
% dt      - dt6 (tensor) data structure
% maxZ    - (a) maxZ=-10 (default) will split fibers, cutting them by a
%           saggital plane 10 mm below acpc line.  We assume that no
%           interhemispheric fiber should connect below ACPC line. Fornix &
%           CC are comissures located above. Provide your own maxZ (the
%           most superior z coordinate of the cutting midsaggital plane)
%           otherwise. (b) maxZ='AllButCC' allows splitting every
%           interhemispheric fiber except callosal.
%
% Example:
% Split intehemispheric fibers below ACPC
%   dt=dtiLoadDt6(dtFileName);
%   fg = dtiLoadFibers(fgFileName);
%   fg=dtiSplitInterhemisphericFibers(fg, dt, 0]);
%
% (c) Vistalab

%HISTORY:
% 08/2009: ER & LMP   wrote it
% 08/24/2009 ER: drop fibers of 1 node long.

if notDefined('maxZ')
    maxZ=-10;
end

if isnumeric(maxZ) && (maxZ<dt.bb(1, 3) ||maxZ>dt.bb(2, 3))|| (~isnumeric(maxZ) && ~strcmp(maxZ, 'AllButCC'))
    error(['The most superior point Z for cutting should be between ' num2str(dt.bb(1, 3)) ' and ' num2str(dt.bb(2, 3)) ' or "AllButCC"']);
end

if notDefined('dt')
    [f,p] = uigetfile({'*.nii.gz';'*.*'},'Select dt6 file...');
    if(isnumeric(f)), disp('User canceled.'); return; end
    dt = fullfile(p,f);
end

midSagThresh = 5;
fg=dtiCleanFibers(fg);
fgname=fg.name;

if isnumeric(maxZ)

   fprintf(1, 'dtiSplitInterhemisphericFibers: Splitting every fiber below Z=%s \n', num2str(maxZ));
   % Bounding Box = dt.bb  x = [-80 0]; y = [-120 90]; z = [-60 90];
    x = [0];
    y = [dt.bb(1, 2):dt.bb(2, 2)];
    z = [dt.bb(1, 3): maxZ];

    [X,Y,Z] = meshgrid(x, y, z);
    roiCoords = [X(:), Y(:), Z(:)];
    roi = dtiNewRoi('MidSaggitalBelowACPC', 'b', roiCoords);

    [fgToChop,contentiousFibers, keep] = dtiIntersectFibersWithRoi([], {'and'}, [], roi, fg);
    fgRemains = dtiNewFiberGroup([fg.name 'Remains']); fgRemains.fibers=fg.fibers(~keep);
    if isfield(fg, 'subgroup') && ~isempty(fg.subgroup)
        fgRemains.subgroup=fg.subgroup(~keep);
    end

    fgRight = dtiNewFiberGroup([fg.name '_ChoppedR']);
    fgLeft = dtiNewFiberGroup([fg.name '_ChoppedL']);
        clear fg;

%emptyleft=0;
%emptyright=0;

    keepRightID=[];keepLeftID=[];
    for i=1:size(fgToChop.fibers)
        pointsRight=(fgToChop.fibers{i}(1, :)>midSagThresh);
        pointsLeft=(fgToChop.fibers{i}(1, :)<-midSagThresh);
        RightChunk{1}=fgToChop.fibers{i}(:, pointsRight);
        if ~isempty(RightChunk{1})
            fgRight.fibers= [fgRight.fibers(:);RightChunk] ; %Right chunk
            keepRightID=[keepRightID; i];
 %       else emptyright=emptyright+1;
        end
        LeftChunk{1}=fgToChop.fibers{i}(:,  pointsLeft);

        if ~isempty(LeftChunk{1})
            fgLeft.fibers = [fgLeft.fibers(:); LeftChunk]; %Left chunk
            keepLeftID=[keepLeftID; i];
 %       else emptyleft=emptyleft+1;
        end
    end
      if isfield(fgToChop, 'subgroupNames') && ~isempty(fgToChop.subgroupNames)
    fgLeft.subgroupNames=fgToChop.subgroupNames;
    fgRight.subgroupNames=fgToChop.subgroupNames;
      end
    if isfield(fgToChop, 'subgroup') && ~isempty(fgToChop.subgroup)
    fgLeft.subgroup=fgToChop.subgroup(keepLeftID);
    fgRight.subgroup=fgToChop.subgroup(keepRightID);
    end

%    emptyleft
%    emptyright
else

    % Code that LMP wrote to take a fiber group, remove callosal fibers, split
    % the remaining groups, then merge the two groups to create one set that
    % is cut down the mid-line but retains the callosal fibers.
    %Limitation: assumes CC is the only interhemispheric connection
    fprintf(1, 'Splitting every fiber except in the CC \n');

    ccCoords = dtiFindCallosum(dt.dt6,dt.b0,dt.xformToAcpc);
    ccRoi = dtiNewRoi('CC','c',ccCoords);
    ccRoi = dtiRoiClean(ccRoi, 3, {'dilate'});

    fgRemains = dtiIntersectFibersWithRoi([], {'and'}, [], ccRoi, fg);
    fgToChop= dtiIntersectFibersWithRoi([], {'not'}, [], ccRoi, fg);
    clear fg;
    fgRight = dtiClipFiberGroup(fgToChop, [-80 midSagThresh],[],[]);
    fgLeft = dtiClipFiberGroup(fgToChop, [-midSagThresh 80],[],[]);

end

fg = dtiMergeFiberGroups(fgRight,fgLeft,fgname);
fg = dtiMergeFiberGroups(fg,fgRemains,fgname);
fgRemains.name  = [fgname '_NoChop'];
fgLeft.name  = [fgname '_L'];
fgRight.name  = [fgname '_R'];
fg.name=[fgname '_BilaterallySplit'];

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%dtiWriteFibersPdb(newFg,[],newFg.name);

%fgToChop.name=['allConnectingGM_withCST_Mori_ToChop'];
%dtiWriteFiberGroup(fgRemains,fgRemains.name);
%dtiWriteFiberGroup(newFgLeft,newFgLeft.name);
%dtiWriteFiberGroup(newFgRight,newFgRight.name);
%dtiWriteFiberGroup(fgToChop,fgToChop.name);


%LEARNALGOITML Wrapper class to the actual ITML code
classdef LearnAlgoITML < LearnAlgo

    properties
       p %parameters
       s %struct
       available
    end

    properties (Constant)
        type = 'itml'
    end

    methods
        function obj = LearnAlgoITML(p)
           if nargin < 1
              p = struct();
           end

           if ~isfield(p,'roccolor')
                p.roccolor = 'b';
           end
           obj.p  = p;
           check(obj);
        end

        function bool = check(obj)
           bool = exist('ItmlAlg') ~= 0;
           if ~bool
               fprintf('Sorry %s not available\n',obj.type);
           end
           obj.available = bool;
        end

        function s = learnPairwise(obj,X,idxa,idxb,matches)
            if ~obj.available
                s = struct();
                return;
            end

            tic;
            s.M = PairMetricLearning(@ItmlAlg, idxa', idxb', matches, X');
            s.t = toc;
            s.learnAlgo = obj;
            s.roccolor = obj.p.roccolor;
        end

        function s = learn(obj,X,y)
            if ~obj.available
                s = struct();
                return;
            end

            tic;
            s.M = MetricLearning(@ItmlAlg, y', X');
            s.t = toc;
            s.learnAlgo = obj;
            s.roccolor = obj.p.roccolor;
        end

        function d = dist(obj, s, X, idxa,idxb)
            d = cdistM(s.M,X,idxa,idxb);
        end
    end
end


function F = in_fread_compumedics_pfs(sFile, sfid, SamplesBounds, ChannelsRange)
% IN_FREAD_COMPUMEDICS_PFS:  Read a block of recordings from a Compumedics ProFusion Sleep 4 exported binary file (.sdy/.rda).
%
% USAGE:  F = in_fread_compumedics_pfs(sFile, sfid, SamplesBounds=[], ChannelsRange=[])

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

% Parse inputs
if (nargin < 4) || isempty(ChannelsRange)
    ChannelsRange = [1, sFile.header.nchannels];
end
if (nargin < 3) || isempty(SamplesBounds)
    SamplesBounds = round(sFile.prop.times .* sFile.prop.sfreq);
end

% ===== COMPUTE OFFSETS =====
nChannels     = double(sFile.header.nchannels);
nReadTimes    = SamplesBounds(2) - SamplesBounds(1) + 1;
nReadChannels = double(ChannelsRange(2) - ChannelsRange(1) + 1);
% Everything is stored on 32 bit floats
bytesPerVal = 4;
dataClass = 'single';
% Header offset
offsetHeader = sFile.header.rda.segment(1).pos;
% Time offset
offsetTime = round(SamplesBounds(1) * nChannels * bytesPerVal);
% Channel offset at the beginning and end of each channel block
offsetChannelStart = round((ChannelsRange(1)-1) * bytesPerVal);
offsetChannelEnd   = (nChannels - ChannelsRange(2)) * bytesPerVal;
% Start reading at this point
offsetStart = offsetHeader + offsetTime + offsetChannelStart;
% Number of time samples to skip after each channel
offsetSkip = offsetChannelStart + offsetChannelEnd;

% ===== READ DATA BLOCK =====
% Position file at the beginning of the trial
fseek(sfid, offsetStart, 'bof');
% Read trial data
% => WARNING: CALL TO FREAD WITH SKIP=0 DOES NOT WORK PROPERLY
if (offsetSkip == 0)
    F = fread(sfid, [nReadChannels, nReadTimes], dataClass);
else
    precision = sprintf('%d*%s', nReadChannels, dataClass);
    F = fread(sfid, [nReadChannels, nReadTimes], precision, offsetSkip);
end
% Check that data block was fully read
if (numel(F) < nReadTimes * nReadChannels)
    % Error message
    disp(sprintf('BST> ERROR: File is truncated (%d values were read instead of %d)...', numel(F), nReadTimes * nReadChannels));
    % Pad with zeros
    Ftmp = zeros(nReadChannels, nReadTimes);
    Ftmp(1:numel(F)) = F(:);
    F = Ftmp;
end


function [varargout] = size2(x, dim, cellflag)

% SIZE2 Size of cells in cell-array.
%
% Use as:
%   [varargout] = size2(x, dim, cellflag)
%
% Input arguments:
%   x   = cell-array
%   dim = dimension argument, default []
%   cellflag = true (default) or false. If false, size2 behaves as size,
%     otherwise the size of each of the cells is returned.

if nargin<3, cellflag = false; end
if nargin<2, dim      = [];    end

if nargout>1 && ~isempty(dim)
  error('Too many output arguments.');
end

if ischar(cellflag) && strcmp(cellflag, 'cell')
  cellflag = true;
end

if ~cellflag
  % traditional functionality
  numdim = ndims(x);

  if isempty(dim)
    siz    = zeros(1,numdim);
    for k = 1:numdim
      switch k
        case 1
          siz(k) = numel(x(:,1,1,1,1,1,1,1,1,1));
        case 2
          siz(k) = numel(x(1,:,1,1,1,1,1,1,1,1));
        case 3
          siz(k) = numel(x(1,1,:,1,1,1,1,1,1,1));
        case 4
          siz(k) = numel(x(1,1,1,:,1,1,1,1,1,1));
        case 5
          siz(k) = numel(x(1,1,1,1,:,1,1,1,1,1));
        case 6
          siz(k) = numel(x(1,1,1,1,1,:,1,1,1,1));
        case 7
          siz(k) = numel(x(1,1,1,1,1,1,:,1,1,1));
        case 8
          siz(k) = numel(x(1,1,1,1,1,1,1,:,1,1));
        case 9
          siz(k) = numel(x(1,1,1,1,1,1,1,1,:,1));
        case 10
          siz(k) = numel(x(1,1,1,1,1,1,1,1,1,:));
        otherwise
      end
    end
  else
    switch dim
      case 1
        siz = numel(x(:,1,1,1,1,1,1,1,1,1));
      case 2
        siz = numel(x(1,:,1,1,1,1,1,1,1,1));
      case 3
        siz = numel(x(1,1,:,1,1,1,1,1,1,1));
      case 4
        siz = numel(x(1,1,1,:,1,1,1,1,1,1));
      case 5
        siz = numel(x(1,1,1,1,:,1,1,1,1,1));
      case 6
        siz = numel(x(1,1,1,1,1,:,1,1,1,1));
      case 7
        siz = numel(x(1,1,1,1,1,1,:,1,1,1));
      case 8
        siz = numel(x(1,1,1,1,1,1,1,:,1,1));
      case 9
        siz = numel(x(1,1,1,1,1,1,1,1,:,1));
      case 10
        siz = numel(x(1,1,1,1,1,1,1,1,1,:));
      otherwise
    end
  end

  if nargout<=1
    varargout{1} = siz;
  elseif nargout>=numdim
    for k = 1:numdim,           varargout{k} = siz(k); end
    for k = (numdim+1):nargout, varargout{k} = 1;      end
  elseif nargout<numdim
    for k = 1:(nargout-1),      varargout{k} = siz(k); end
    varargout{nargout} = prod(siz((k+1):end));
  end
  return;
else
  % operate on the individual cells
%   siz    = ones(numel(x), 10);
%   numdim = zeros(numel(x), 1);
%   for k = 1:numel(x)
%     numdim(k) = ndims(x{k});
%     siz(k,1:numdim(k)) = size(x{k});
%   end
  numdim = cellfun(@ndims, x);
  siz    = cellfun(@size,  x, 'uniformoutput', false);
  if all(numdim==numdim(1))
    siz = reshape(cat(2,siz{:}),numdim(1),[])';
  end

  if isempty(dim)
    if nargout<=1
      varargout{1} = siz(:,1:max(numdim));
    elseif nargout>=max(numdim)
      for k = 1:numdim,           varargout{k} = reshape(siz(:,k), size(x)); end
      for k = (numdim+1):nargout, varargout{k} = ones(size(x));              end
    end
  else
    varargout{1} = reshape(siz(:,dim), size(x));
  end
  return;
end


function Y=sum(Y)
%SUM (overloaded)

Y.basis = sum(Y.basis,1);
Y.dim(1) = 1;
Y.dim(2) = 1;
% Reset info about conic terms
Y.conicinfo = [0 0];
Y = clean(Y);


close all;
clear all;
clc;

data_file_path = 'bin/ar_omp_phase_transition_gaussian_dict_gaussian_data.mat';
options.export = false;
options.export_dir = 'bin';
options.export_name = 'gaussian_dict_gaussian_data';
%options.chosen_ks = [2, 4, 8, 16, 32, 64];
spx.pursuit.PhaseTransitionAnalysis.print_results(data_file_path, ...
    'OMP-AR', options);

data_file_path = 'bin/omp_phase_transition_gaussian_dict_gaussian_data.mat';
options.export = false;
options.export_dir = 'bin';
options.export_name = 'gaussian_dict_gaussian_data';
%options.chosen_ks = [2, 4, 8, 16, 32, 64];
spx.pursuit.PhaseTransitionAnalysis.print_results(data_file_path, ...
    'OMP', options);


        function polyobj = coarsen_polygon(polyobj,iboubox)
            % Coarsen a polygon using a brute-force vertex reordering algorithm.
            idx = find(isnan(polyobj(:,1)));
            C = mat2cell(polyobj,diff([0;idx]),2);
            new = cell(length(C),1);
            % for each segment
            for iii = 1 : length(C)
                j = 0 ; k = 0;
                [in] = inpoly(C{iii},iboubox(1:end-1,:));
                % Initialise for speed
                new{iii} = zeros(length(C{iii}),2);
                while j < length(C{iii})-1
                    j = j + 1 ;
                    if in(j) % point is in domain keep it
                        k = k + 1;
                        new{iii}(k,:) = C{iii}(j,:);
                    else % pt is out of domain
                        bd = min([j+200,length(in)-1]) ;
                        exte = min(200,bd - j);
                        if sum(in(j:bd))==0 % if next hundred points are out, then we can decimate
                            k = k + 1 ;
                            new{iii}(k,:) = C{iii}(j,:);
                            k = k + 1 ;
                            new{iii}(k,:) = C{iii}(j+exte,:);
                            j = j + exte ;
                        else % otherwise keep
                            k = k + 1 ;
                            new{iii}(k,:) = C{iii}(j,:);
                        end

                    end
                end
                new{iii}(k+1,:) = [NaN NaN] ;
                new{iii}(k+2:end,:) = [];
            end
            polyobj = cell2mat(new);
        end


classdef Bicubic < dagnn.ElementWise
%%% Bicubic resizing layer %%%
%
% Performs bicubic resizing on inputs{1} and stores it in outputs{1}.
% outputs{1} =imresize(input, obj.scale,'bicubic');
% *Back-propagation (backward function) not implemented*

    properties
        scale = 2;
    end

    methods
        function outputs = forward(obj, inputs, params)
            outputs{1} = imresize(inputs{1}, obj.scale, 'bicubic');
        end

        function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
            derInputs{1} = 0*inputs{1} ;
            derParams = {} ;
        end

        function obj = Bicubic(varargin)
            obj.load(varargin);
            obj.scale = obj.scale;
        end
    end
end


function mrAnatComputeVanatSpatialNorm(vAnatFileName, outFileName)
% Computes an SPM2 spatial normalization and its inverse for a vAnatomy.
%
% mrAnatComputeVanatSpatialNorm(vAnatFileName, [outFileName])
%
%
% HISTORY:
% 2005.08.05 RFD: wrote it.
%

if(~exist('vAnatFileName','var') | isempty(vAnatFileName))
    vAnatFileName = mrvSelectFile('r','dat',[],'Select a vAnatomy file...');
    if isempty(vAnatFileName), return;
    elseif ~exist(vAnatFileName,'file'), error(sprintf('%s not found.\n',vAnatFileName));
    end
end
if(~exist('outFileName','var'))
    [p,f,e] = fileparts(vAnatFileName);
    outFileName = fullfile(p,[f '_sn.mat']);
end

[img, mm] = readVolAnat(vAnatFileName);
img = uint8(img);
xform = mrAnatXform(mm,size(img),'vanatomy2acpc');

%
% Compute the normalization params.
%
disp('Computing spatial norm params...');
[sn, Vtemplate] = mrAnatComputeSpmSpatialNorm(img, xform);

%
% Convert the SN params to a deformation field.
%
disp('Computing inverse spatial norm...');
% Build the bounding box needed to sample the deformation field. We want it
% to capture the entire template, so we use the template vox-to-physical xform
% (sn.VG) to generate it.
%
% We first extract scales from the template xform. We want the
% defomation field to be sampled at this same scale. That makes pulling
% values from the inverse deformation easier and saves space, since it
% isn't necessary to sample at a higher resolution than the template.
mmTemplate = sqrt(sum(sn.VG.mat(1:3,1:3).^2));
origin  = sn.VG.mat\[0 0 0 1]';
origin  = origin(1:3)';
bb = [-mmTemplate.*(origin-1) ; mmTemplate.*(sn.VG.dim(1:3)-origin)];
% Now compute the deformation:
d = single(mrAnatSnToDeformation(sn, mmTemplate, bb));

% Invert the deformation. The inverse deformation is essentially a look-up
% table that maps voxel coords to the physical space of the template. This
% is exactly what we want.
%
% 4th arg is 4x4 xform from mm to voxels in the coordinate frame of the inverse deformation field
% 5th arg is xform from voxels to mm in the coordinate frame of the forward deformation field
[defX,defY,defZ] = spm_invdef(d(:,:,:,1), d(:,:,:,2), d(:,:,:,3), sn.VF.dim(1:3), ...
                             inv(sn.VF.mat), sn.VG.mat);

% We can save it as int8, since real brain templates never extend beyond
% 128mm from the origin. But, we'll check, just to be sure.
if(all(abs(defX(:))<128) & all(abs(defY(:))<128) & all(abs(defZ(:))<128))
    voxToTemplateLUT = zeros([3 size(defX)], 'int8');
    voxToTemplateLUT(1,:,:,:) = int8(round(defX));
    voxToTemplateLUT(2,:,:,:) = int8(round(defY));
    voxToTemplateLUT(3,:,:,:) = int8(round(defZ));
else
    voxToTemplateLUT = zeros([3 size(defX)], 'int16');
    voxToTemplateLUT(1,:,:,:) = int16(round(defX));
    voxToTemplateLUT(2,:,:,:) = int16(round(defY));
    voxToTemplateLUT(3,:,:,:) = int16(round(defZ));
end

fprintf('Saving to %s...\n', outFileName);
save(outFileName, 'sn', 'voxToTemplateLUT');
disp('done.');
return;

% DEBUGGING CODE:

mmPerVox = [1 1 1];

% Get a bounding box in ac-pc space that captures the whole template image.
templateMmPerVox = sqrt(sum(sn.VG(1).mat(1:3,1:3).^2));
if det(sn.VG.mat(1:3,1:3))<0, mmPerVox(1) = -mmPerVox(1); end;
templateOrigin  = sn.VG.mat\[0 0 0 1]';
templateOrigin  = templateOrigin(1:3)';
bb = [-templateMmPerVox .* (templateOrigin-1) ; templateMmPerVox.*(sn.VG(1).dim(1:3)-templateOrigin)];
% The code in mrAnatResliceSpm needs to know how to transform the image
% coords from the template's ac-pc space to actual deformation-image space.
% The following was gleaned from spm_reslice_sn. I only vaguely understand
% it, but it seems to work.
og  = -templateMmPerVox .* templateOrigin;
M1  = [templateMmPerVox(1) 0 0 og(1) ; 0 templateMmPerVox(2) 0 og(2) ; 0 0 templateMmPerVox(3) og(3) ; 0 0 0 1];
outMmPerVox = [1 1 1];
of  = -outMmPerVox.*(round(-bb(1,:)./outMmPerVox)+1);
M2  = [outMmPerVox(1) 0 0 of(1) ; 0 outMmPerVox(2) 0 of(2) ; 0 0 outMmPerVox(3) of(3) ; 0 0 0 1];
d.inMat = inv(sn.VG(1).mat*inv(M1)*M2);

dField = mrAnatSnToDeformation(sn, [1 1 1], bb);
d.deformX = dField(:,:,:,1);
d.deformY = dField(:,:,:,2);
d.deformZ = dField(:,:,:,3);
d.outMat = inv(xform);
[imgSn, newXform] = mrAnatResliceSpm(img, d, bb, mmPerVox, [7 7 7 0 0 0]);
imgSn(isnan(imgSn)) = 0;

figure; imagesc(makeMontage(imgSn)); axis image; colormap(gray)


function [bodyInfo] = getBodyInfoStructFromOrbit(inputOrbit)
%getBodyInfoStructFromOrbit Summary of this function goes here
%   Detailed explanation goes here

%     bodyInfo = struct();
    bodyInfo = KSPTOT_BodyInfo();
    bodyInfo.epoch = inputOrbit(7);
    bodyInfo.sma = inputOrbit(1);
    bodyInfo.ecc = inputOrbit(2);
    bodyInfo.inc = rad2deg(inputOrbit(3));
    bodyInfo.raan = rad2deg(inputOrbit(4));
    bodyInfo.arg = rad2deg(inputOrbit(5));
    bodyInfo.mean = rad2deg(inputOrbit(6));
    bodyInfo.id = rand();
    bodyInfo.propTypeEnum = BodyPropagationTypeEnum.TwoBody;
end