function [A, vA, vB, bb_rel] = crop_borders(A, bcol, padding)
%CROP_BORDERS Crop the borders of an image or stack of images
%
%   [B, vA, vB, bb_rel] = crop_borders(A, bcol, [padding])
%
%IN:
%   A - HxWxCxN stack of images.
%   bcol - Cx1 background colour vector.
%   padding - scalar indicating how much padding to have in relation to
%             the cropped-image-size (0<=padding<=1). Default: 0
%
%OUT:
%   B - JxKxCxN cropped stack of images.
%   vA     - coordinates in A that contain the cropped image
%   vB     - coordinates in B where the cropped version of A is placed
%   bb_rel - relative bounding box (used for eps-cropping)

% 06/03/15: Improved image cropping thanks to Oscar Hartogensis
% 08/06/15: Fixed issue #76: case of transparent figure bgcolor

    if nargin < 3
        padding = 0;
    end
    [h, w, c, n] = size(A);
    if isempty(bcol)  % case of transparent bgcolor
        bcol = A(ceil(end/2),1,:,1);
    end
    if isscalar(bcol)
        bcol = bcol(ones(c, 1));
    end

    % Crop margin from left
    bail = false;
    for l = 1:w
        for a = 1:c
            if ~all(col(A(:,l,a,:)) == bcol(a))
                bail = true;
                break;
            end
        end
        if bail
            break;
        end
    end

    % Crop margin from right
    bcol = A(ceil(end/2),w,:,1);
    bail = false;
    for r = w:-1:l
        for a = 1:c
            if ~all(col(A(:,r,a,:)) == bcol(a))
                bail = true;
                break;
            end
        end
        if bail
            break;
        end
    end

    % Crop margin from top
    bcol = A(1,ceil(end/2),:,1);
    bail = false;
    for t = 1:h
        for a = 1:c
            if ~all(col(A(t,:,a,:)) == bcol(a))
                bail = true;
                break;
            end
        end
        if bail
            break;
        end
    end

    % Crop margin from bottom
    bcol = A(h,ceil(end/2),:,1);
    bail = false;
    for b = h:-1:t
        for a = 1:c
            if ~all(col(A(b,:,a,:)) == bcol(a))
                bail = true;
                break;
            end
        end
        if bail
            break;
        end
    end

    % Crop the background, leaving one boundary pixel to avoid bleeding on resize
    %v = [max(t-padding, 1) min(b+padding, h) max(l-padding, 1) min(r+padding, w)];
    %A = A(v(1):v(2),v(3):v(4),:,:);
    if padding == 0  % no padding
        padding = 1;
    elseif abs(padding) < 1  % pad value is a relative fraction of image size
        padding = sign(padding)*round(mean([b-t r-l])*abs(padding)); % ADJUST PADDING
    else  % pad value is in units of 1/72" points
        padding = round(padding);  % fix cases of non-integer pad value
    end

    if padding > 0  % extra padding
        % Create an empty image, containing the background color, that has the
        % cropped image size plus the padded border
        B = repmat(bcol,(b-t)+1+padding*2,(r-l)+1+padding*2);
        % vA - coordinates in A that contain the cropped image
        vA = [t b l r];
        % vB - coordinates in B where the cropped version of A will be placed
        vB = [padding+1, (b-t)+1+padding, padding+1, (r-l)+1+padding];
        % Place the original image in the empty image
        B(vB(1):vB(2), vB(3):vB(4), :) = A(vA(1):vA(2), vA(3):vA(4), :);
        A = B;
    else  % extra cropping
        vA = [t-padding b+padding l-padding r+padding];
        A = A(vA(1):vA(2), vA(3):vA(4), :);
        vB = [NaN NaN NaN NaN];
    end

    % For EPS cropping, determine the relative BoundingBox - bb_rel
    bb_rel = [l-1 h-b-1 r+1 h-t+1]./[w h w h];
end

function A = col(A)
    A = A(:);
end


function r = mldivide(p,q)
%MLDIVIDE     Implements  p \ q  for univariate polynomials (same as q/p)
%

% written  11/02/05     S.M. Rump
%

  r = q / p;


function output = apply_net_filter(input_v, input_h)
    global config mem;
    input_size = size(input_v);
    %output = config.NEW_MEM(zeros(size(input_v, 1), size(input_v, 2), config.chs, config.batch_size));
    output = zeros(size(input_v, 1), size(input_v, 2), config.chs, config.batch_size);
    p_size = config.MEM.p_size;
    size_differ = config.size_differ;
    %count = config.NEW_MEM(zeros(size(input_v, 1), size(input_v, 2)));
    count = zeros(size(input_v, 1), size(input_v, 2));
    for v = 1 : length(config.MEM.start_rows)
        for h = 1 : length(config.MEM.start_cols)
            v_start = config.MEM.start_rows(v);
            v_end = v_start + p_size - 1;
            h_start = config.MEM.start_cols(h);
            h_end = h_start + p_size - 1;
            if(v_end > input_size(1))
                v_end = input_size(1);
                v_start = v_end - p_size + 1;
            end
            if(h_end > input_size(2))
                h_end = input_size(2);
                h_start = h_end - p_size + 1;
            end
            input_piece = cat(4, input_v(v_start:v_end, h_start:h_end,:), permute(input_h(v_start:v_end, h_start:h_end,:), [2 1 3]));

            op_test_pipe(input_piece, mem.fake_output_for_test);
            %output_piece = mem.output;
            output_piece = gather(mem.output);
            if(size_differ(1) ~= size_differ(2))
                output_piece = output_piece(size_differ(2)/2+1:size(output_piece,1)-size_differ(2)/2, size_differ(1)/2+1:size(output_piece,2)-size_differ(1)/2,:,:);
            end
            output_piece(:,:,:,2) = permute(output_piece(:,:,:,2), [2 1 3]);

            output(v_start+(max(size_differ)/2):v_end-(max(size_differ)/2), h_start+(max(size_differ)/2):h_end-(max(size_differ)/2),:,:) = ...
                    output(v_start+(max(size_differ)/2):v_end-(max(size_differ)/2), h_start+(max(size_differ)/2):h_end-(max(size_differ)/2),:,:) + output_piece;
            count(v_start+(max(size_differ)/2):v_end-(max(size_differ)/2), h_start+(max(size_differ)/2):h_end-(max(size_differ)/2)) = ...
                    count(v_start+(max(size_differ)/2):v_end-(max(size_differ)/2), h_start+(max(size_differ)/2):h_end-(max(size_differ)/2)) + 1;
        end
    end
    count = max(count, 1);
    output = bsxfun(@rdivide, output, count);
    %output = gather(output);

    %output = output_piece;
    %output = padarray(output, [1 1]);
end


function varargout = process_cohere2_time_2021( varargin )
% PROCESS_COHERE2_TIME_2021: Compute the time-resolved coherence between all the pairs of signals, in one file.

% @=============================================================================
% 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: Elizabeth Bock, 2015
%          Francois Tadel, 2015-2021
%          Hossein Shahabi, 2019-2020

eval(macro_method);
end

%% ===== GET DESCRIPTION =====
function sProcess = GetDescription() %#ok<DEFNU>
    % Description the process
    sProcess.Comment     = 'Time-resolved coherence AxB [2021]';
    sProcess.Category    = 'Custom';
    sProcess.SubGroup    = 'Connectivity';
    sProcess.Index       = 660;
    sProcess.Description = 'https://neuroimage.usc.edu/brainstorm/Tutorials/Connectivity';
    % Definition of the input accepted by this process
    sProcess.InputTypes  = {'data',     'results',  'matrix'};
    sProcess.OutputTypes = {'timefreq', 'timefreq', 'timefreq'};
    sProcess.nInputs     = 2;
    sProcess.nMinFiles   = 1;
    sProcess.isPaired    = 1;

    % === CONNECT INPUT
    sProcess = process_corr2('DefineConnectOptions', sProcess);
    % === REMOVE EVOKED REPONSE
    sProcess.options.removeevoked.Comment = 'Remove evoked response from each trial';
    sProcess.options.removeevoked.Type    = 'checkbox';
    sProcess.options.removeevoked.Value   = 0;
    sProcess.options.removeevoked.Group   = 'input';
    % === Time window
    sProcess.options.slide_win.Comment = 'Sliding time window duration:';
    sProcess.options.slide_win.Type    = 'value';
    sProcess.options.slide_win.Value   = {.350, 'ms', []};
    % === WinOverlap for Sliding window (Time)
    sProcess.options.slide_overlap.Comment = 'Sliding window overlap:';
    sProcess.options.slide_overlap.Type    = 'value';
    sProcess.options.slide_overlap.Value   = {50, '%', []};
    % === COHERENCE METHOD
    sProcess.options.cohmeasure.Comment = {...
        ['Magnitude-squared Coherence
' ...
        '|C|^2 = |Gxy|^2/(Gxx*Gyy)'], ...
        ['Imaginary Coherence (2019)
' ...
        'IC    = |imag(C)|'], ...
        ['Lagged Coherence (2019)
' ...
        'LC    = |imag(C)|/sqrt(1-real(C)^2)'], ...
        [' Imaginary Coherence (before 2019)
' ...
        ' IC    = imag(C)^2 / (1-real(C)^2) ']; ...
        'mscohere', 'icohere2019','lcohere2019', 'icohere'};
    sProcess.options.cohmeasure.Type    = 'radio_label';
    sProcess.options.cohmeasure.Value   = 'mscohere';
    % === WINDOW LENGTH
    sProcess.options.win_length.Comment = 'Window length for PSD estimation:';
    sProcess.options.win_length.Type    = 'value';
    sProcess.options.win_length.Value   = {1, 's', []};
    % === OVERLAP
    sProcess.options.overlap.Comment = 'WinOverlap for PSD estimation:' ;
    sProcess.options.overlap.Type    = 'value';
    sProcess.options.overlap.Value   = {50, '%', []};
    % === HIGHEST FREQUENCY OF INTEREST
    sProcess.options.maxfreq.Comment = 'Highest frequency of interest:';
    sProcess.options.maxfreq.Type    = 'value';
    sProcess.options.maxfreq.Value   = {60,'Hz',2};
    % === OUTPUT FILE TAG
    sProcess.options.commenttag.Comment = 'File tag: ';
    sProcess.options.commenttag.Type    = 'text';
    sProcess.options.commenttag.Value   = '';
    sProcess.options.commenttag.Group   = 'output';
end

%% ===== FORMAT COMMENT =====
function Comment = FormatComment(sProcess) %#ok<DEFNU>
    Comment = sProcess.Comment;
end

%% ===== RUN =====
function OutputFiles = Run(sProcess, sInputA, sInputB) %#ok<DEFNU>
    % Initialize returned values
    OutputFiles = {};
    % Output mode 2021: Forcing the average cross-spectra of input files (one output file)
    sProcess.options.outputmode.Value = 'avgcoh';
    % Input options
    OPTIONS = process_corr2('GetConnectOptions', sProcess, sInputA, sInputB);
    if isempty(OPTIONS)
        return
    end
    CommentTag = sProcess.options.commenttag.Value;
    % Metric options
    OPTIONS.Method = 'cohere';
    OPTIONS.RemoveEvoked  = sProcess.options.removeevoked.Value;
    OPTIONS.WinLen        = sProcess.options.win_length.Value{1};
    OPTIONS.MaxFreq       = sProcess.options.maxfreq.Value{1};
    OPTIONS.CohOverlap    = 0.50;
    OPTIONS.pThresh       = 0.05;
    OPTIONS.isSave        = 0;
    OPTIONS.CohMeasure    = sProcess.options.cohmeasure.Value;
    % Sliding time windows options
    WinLength  = sProcess.options.slide_win.Value{1};
    WinOverlap = sProcess.options.slide_overlap.Value{1};

    % Read time information
    TimeVectorA  = in_bst(sInputA(1).FileName, 'Time');
    sfreq        = round(1/(TimeVectorA(2) - TimeVectorA(1)));
    % Get time window of first fileA if none specified in parameters
    if isempty(OPTIONS.TimeWindow)
        OPTIONS.TimeWindow = TimeVectorA([1, end]);
    end
    % Select input time window
    TimeVectorA = TimeVectorA((TimeVectorA >= OPTIONS.TimeWindow(1)) & (TimeVectorA <= OPTIONS.TimeWindow(2)));
    nTime       = length(TimeVectorA);

    % Compute sliding windows length
    Lwin  = round(WinLength * sfreq);
    Loverlap = round(Lwin * WinOverlap / 100);
    Nwin = floor((nTime - Loverlap) ./ (Lwin - Loverlap));
    % If window is bigger than the data
    if (Lwin > nTime)
        bst_report('Error', sProcess, sInputA, 'Sliding window for the coherence estimation is too long compared with the epochs in input.');
        return;
    end

    % Check that time is the same for FilesB
    if ~isempty(sInputB)
        TimeVectorB = in_bst(sInputB(1).FileName, 'Time');
        if (length(TimeVectorA) ~= length(TimeVectorB))
            bst_report('Error', sProcess, sInputA, 'Files A and B must share the same time vector.');
            return;
        end
    end

    % Get progress bar position
    posProgress = bst_progress('get');
    % Loop over all the time windows
    for iWin = 1:Nwin
        % Set the progress bar at the same level at every iteration
        bst_progress('set', posProgress);
        % Select time window
        iTimes = (1:Lwin) + (iWin-1)*(Lwin - Loverlap);
        OPTIONS.TimeWindow = TimeVectorA(iTimes([1,end]));
        % Compute metric
        if ~isempty(sInputB)
            ConnectMat = bst_connectivity({sInputA.FileName}, {sInputB.FileName}, OPTIONS);
        else
            ConnectMat = bst_connectivity({sInputA.FileName}, [], OPTIONS);
        end
        % Processing errors
        if isempty(ConnectMat) || ~iscell(ConnectMat) || ~isstruct(ConnectMat{1}) || isempty(ConnectMat{1}.TF)
            bst_report('Error', sProcess, sInputA, 'Coherence for the selected time segment could not be calculated.');
            return;
        end
        % Start a new brainstorm structure
        if (iWin == 1)
            NewMat = ConnectMat{1};
            NewMat.Time = OPTIONS.TimeWindow(1);
            NewMat.TimeBands = [];
        % Add next time point
        else
            NewMat.TF(:,iWin,:) = ConnectMat{1}.TF;
            NewMat.Time(end+1) = OPTIONS.TimeWindow(1);
        end
    end

    % Fix time vector
    if (length(NewMat.Time) == 1)
        bst_report('Warning', sProcess, sInputA, 'Only one sliding time window could be estimated.');
        NewMat.Time = [TimeVectorA(1), TimeVectorA(end)];
    end
    % Add comment tag
    if ~isempty(CommentTag)
        NewMat.Comment = [NewMat.Comment ' | ' CommentTag];
    end
    % File tag
    if (length(NewMat.RefRowNames) == 1)
        fileTag = 'connect1';
    else
        fileTag = 'connectn';
    end
    % Output filename
    sOutputStudy = bst_get('Study', OPTIONS.iOutputStudy);
    OutputFiles{1} = bst_process('GetNewFilename', bst_fileparts(sOutputStudy.FileName), ['timefreq_' fileTag '_cohere_time']);
    % Save file
    bst_save(OutputFiles{1}, NewMat, 'v6');
    % Add file to database structure
    db_add_data(OPTIONS.iOutputStudy, OutputFiles{1}, NewMat);
end


function rgb = skull

% This returns a predefined color as [red green blue] values
%   red               = [255   0   0]/255;
%   green             = [  0 192   0]/255;
%   blue              = [  0   0 255]/255;
%   magenta           = [255 255   0]/255;
%   cyan              = [  0 255 255]/255;
%   yellow            = [255 255   0]/255;
%   white             = [255 255 255]/255;
%   black             = [  0   0   0]/255;
%
%   skull             = [140  85  85]/255
%   cortex            = [255 213 119]/255;
%   cortex_light      = [199 194 169]/255;
%   cortex_dark       = [100  97  85]/255;
%   skin              = [249 223 192]/255;
%   skin_light        = [249 223 192]/255;
%   skin_medium_light = [225 194 158]/255;
%   skin_medium       = [188 142 106]/255;
%   skin_medium_dark  = [155 102	65]/255;
%   skin_dark         = [ 91  71  61]/255;

rgb = [140 85 85]/255;


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

params.use_gpu = false;
params.gpu_id = 1;

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

% grayscale_params.colorspace='gray';
% grayscale_params.cell_size = 1;

% cn_params.tablename = 'CNnorm';
% cn_params.useForGray = false;
% cn_params.cell_size = 4;
% cn_params.compressed_dim = 3;
%
% ic_params.tablename = 'intensityChannelNorm6';
% ic_params.useForColor = false;
% ic_params.cell_size = 4;
% ic_params.compressed_dim = 3;

% cnn_params.nn_name = 'imagenet-vgg-m-2048_new.mat'; % Name of the network
cnn_params.nn_name = 'UDT_Unsupervised.mat';

cnn_params.output_layer = [4];               % Which layers to use
cnn_params.downsample_factor = [4];           % How much to downsample each output layer
cnn_params.compressed_dim = [32];            % Compressed dimensionality of each output layer
cnn_params.input_size_mode = 'adaptive';        % How to choose the sample size
cnn_params.input_size_scale = 1;

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

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

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

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

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

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

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

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

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

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

% Scale parameters for the translation model
% Only used if: params.use_scale_filter = false

% params.number_of_scales = 3;            % Number of scales to run the detector
% params.scale_step = 1.02;               % The scale factor

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

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

% Initialize
params.seq = seq;

% Run tracker
results = tracker(params);


% greBasedSegment.m
%
% APA, 3/20/2017

% directory containing all files
dirName = 'H:\Public\Aditya\mimExtensions\CERR_files_Sandra_contours_PC';
dirName = 'H:\Public\Aditya\mimExtensions\CERR_files_Sandra_contours_CT';
dirName = 'H:\Public\Aditya\mimExtensions\Atlas_Sanne\PC_cerr';
% dirName = 'H:\Public\Aditya\mimExtensions\Atlas_Sanne\CT_cropped_cerr';

% directory for writing the registered files (must have \ or / as last character)
registeredDir = 'H:\Public\Aditya\mimExtensions\registered_to_ROBINSON^HEATH_35487047\';
registeredDir = 'H:\Public\Aditya\mimExtensions\registered_to_ROBINSON^HEATH_35487047_CT\';
registeredDir = 'H:\Public\Aditya\mimExtensions\registered_to_MT160_PC\';
% registeredDir = 'H:\Public\Aditya\mimExtensions\registered_to_MT160_CT\';

dirS = dir(dirName);
dirS(1:2) = [];

% base scan file name
indBase = 3; %9 for Sandra's atlas, 3 for Sanne's
baseScan = fullfile(dirName,dirS(indBase).name);

% moving scan file names
indV = 1:length(dirS);
indV(indBase) = [];
movScanC = fullfile(dirName,{dirS(indV).name});

% registration callback
strNameToWarp = 'Parotid_L_SvD';
registerToAtlas(baseScan,movScanC,registeredDir,strNameToWarp)

% combine using the STAPLE and the GRE metric
regDirS = dir(registeredDir);
regDirS(1:2) = [];
regFilesC = strcat(registeredDir,{regDirS.name});
indBase = [3];
regFilesC(indBase) = [];
structNum = 2;
doseNum = 1;
scanNum = 1;
doseAllM = [];
strAllM = logical([]);
for i = 1:length(regFilesC)
    planC = loadPlanC(regFilesC{i},tempdir);
    indexS = planC{end};
    % Calculate the GRE metric
    baseScanNum = 1;
    movScanNum = 2;
    planC = calculateGRE(baseScanNum,movScanNum,planC);
    dose3M = getDoseOnCT(doseNum, scanNum, 'uniform', planC);
    str3M = getUniformStr(structNum,planC);
    strAllM(:,i) = str3M(:);
    doseAllM(:,i) = dose3M(:) .* str3M(:);
end

siz = size(str3M);

% STAPLE
numIter = 50;
confidence = 0.8;
numObservers = size(strAllM,2);
p = ones(1,numObservers)*0.999;
q = p;
[W,p,q] = staple(strAllM,confidence,p,q);
stapleStr3M = reshape(W > confidence,siz);
isUniform = 1;
scanNum = 1;
maskToCERRStructure(stapleStr3M,isUniform,scanNum,'STAPLE_80_pct_conf')

% Smooth contour
structNum = 2;
for slc = 1:length(planC{indexS.structures}(structNum).contour)
    for seg = 1:length(planC{indexS.structures}(structNum).contour(slc).segments)
        ptsM = planC{indexS.structures}(structNum).contour(slc).segments(seg).points;
        if isempty(ptsM)
            continue;
        end
        numPts = size(ptsM,1);
        intrvl = ceil(numPts*0.2/10);
        pts1M = spcrv(ptsM(1:intrvl:end,1:2)',3,100)';
        pts1M(:,3) = ptsM(1,3)*pts1M(:,1).^0;
        pts1M(end+1,:) = pts1M(1,:);
        planC{indexS.structures}(structNum).contour(slc).segments(seg).points = pts1M;
    end
end
reRasterAndUniformize

% GRE map
atlasGreV = sum(doseAllM,1) ./ sum(strAllM,1);
indToUse = atlasGreV < prctile(atlasGreV,30);
indToUse = 1:12;
weightedSegM = bsxfun(@times, strAllM(:,indToUse), (1./atlasGreV(indToUse)).^5);
weightedSegM(weightedSegM == 0) = NaN;
Wv = nansum(weightedSegM,2); % voxels weighted by GRE per registration

% Wv = nanmean(weightedSegM,2) < prctile(atlasGreV,50);
% % Wv = strAllM(:,2);
% segM = reshape(Wv,siz);
% maskToCERRStructure(segM,1,1,'GRE Weighted Majority')

% Combine GRE for each voxel
% gama = 1;
% invDoseAllM = 1./(doseAllM(:,indToUse)+eps);
% indZeroV = doseAllM(:,indToUse) > 0;
% invThrV = nanmean(doseAllM(:,indToUse),2);
% thrM = 1 ./ invThrV;
% thrM(invThrV < eps) = 0;
% thrV = mean(thrM,2)+1e10;
% % thr = mean(invDoseAllM(indZeroV)); % global thr
% % indZeroV = indZeroV & invDoseAllM > thrV;
% indZeroV = indZeroV & bsxfun(@le,invDoseAllM', thrV')';
% invDoseAllM(~indZeroV) = NaN;
% Wv = nansum(invDoseAllM.^gama , 2);
% % numMembersV = sum(strAllM,2);
% % Wv = Wv ./ numMembersV;

weightM = reshape(Wv,siz);
weightM(isnan(weightM)) = 0;
showIMDose(weightM,'ConsensusGRE',1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MRiLab auto generated file: DO NOT EDIT!     %
% Generated by MRiLab "DoWriteXML2m" Generator %
% MRiLab Version 1.3                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [rfAmp,rfPhase,rfFreq,rfCoil,GzAmp,GyAmp,GxAmp,ADC,Ext,uts,ts,flags]=PSD_SPGR3DME
global VCtl
global VVar
CV1=2e-3;
CV10=0;
CV11=0;
CV12=0;
CV13=0;
CV14=0;
CV2=3e-3;
CV3=abs(rem(VVar.TRCount,2)*2-1);
CV4=0e-3;
CV5=0;
CV6=0;
CV7=0;
CV8=0;
CV9=0;
rfAmpAll=[];
rfPhaseAll=[];
rfFreqAll=[];
rfCoilAll=[];
GzAmpAll=[];
GyAmpAll=[];
GxAmpAll=[];
ADCAll=[];
ExtAll=[];
rfTimeAll=[];
GzTimeAll=[];
GyTimeAll=[];
GxTimeAll=[];
ADCTimeAll=[];
ExtTimeAll=[];
SEtAll=[];
uts=[];
ts=[];
flags=[];
if VCtl.PlotSeq == 1
rfAmp=[];
rfPhase=[];
rfFreq=[];
rfCoil=[];
GzAmp=[];
GyAmp=[];
GxAmp=[];
ADC=[];
Ext=[];
Freq=1;
Notes='regular TR section';
AttributeOpt={'on','off'};
Switch=AttributeOpt{1};
TREnd=Inf;
TRStart=1;
tE=VCtl.TR;
tS=0;
if VVar.TRCount<TRStart | VVar.TRCount>TREnd | mod(VVar.TRCount-TRStart,Freq)~=0 | strcmp(Switch,'off')
% do nothing
else
ts = [ts tS tE];
end
ts = [0 max(ts)-min(ts)];
return;
end
%==============Pulses 1==============
rfAmp=[];
rfPhase=[];
rfFreq=[];
rfCoil=[];
GzAmp=[];
GyAmp=[];
GxAmp=[];
ADC=[];
Ext=[];
rfTime=[];
GzTime=[];
GyTime=[];
GxTime=[];
ADCTime=[];
ExtTime=[];
Freq=1;
Notes='regular TR section';
AttributeOpt={'on','off'};
Switch=AttributeOpt{1};
TREnd=Inf;
TRStart=1;
tE=VCtl.TR;
tS=0;
if isempty(tS) | isempty(tE) | (tS>=tE)
error('SE setting is incorrect for Pulses 1!');
end
if VVar.TRCount<TRStart | VVar.TRCount>TREnd | mod(VVar.TRCount-TRStart,Freq)~=0 | strcmp(Switch,'off')
% do nothing
else
%--------------------
AttributeOpt={'on','off'};
p.AnchorTE=AttributeOpt{1};
AttributeOpt={'Non','Hamming','Hanning'};
p.Apod=AttributeOpt{2};
p.CoilID=1;
p.DupSpacing=0;
p.Duplicates=1;
p.FA=VCtl.FlipAng;
p.Notes='sinc rf pulse';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.TBP=4;
p.dt=20e-6;
p.rfFreq=0;
p.rfPhase=0;
p.tEnd=CV4+0.5e-3;
p.tStart=CV4;
if strcmp(p.Switch,'on')
if strcmp(p.AnchorTE,'on')
switch VCtl.TEAnchor
case 'Start'
VCtl.TEAnchorTime=p.tStart;
case 'Middle'
VCtl.TEAnchorTime=(p.tStart+p.tEnd)/2;
case 'End'
VCtl.TEAnchorTime=p.tEnd;
end
end
[rfAmp1,rfPhase1,rfFreq1,rfCoil1,rfTime1]=rfSinc(p);
if strcmp(VCtl.MultiTransmit,'off')
if VCtl.MasterTxCoil==rfCoil1(1)
rfAmp=[rfAmp rfAmp1];
rfPhase=[rfPhase rfPhase1];
rfFreq=[rfFreq rfFreq1];
rfCoil=[rfCoil rfCoil1];
rfTime=[rfTime rfTime1];
end
else
rfAmp=[rfAmp rfAmp1];
rfPhase=[rfPhase rfPhase1];
rfFreq=[rfFreq rfFreq1];
rfCoil=[rfCoil rfCoil1];
rfTime=[rfTime rfTime1];
end
end
p=[];
%--------------------
AttributeOpt={'on','off'};
p.AnchorTE=AttributeOpt{2};
AttributeOpt={'Non','Hamming','Hanning'};
p.Apod=AttributeOpt{2};
p.CoilID=1;
p.DupSpacing=0;
p.Duplicates=1;
p.FA=0;
p.Notes='sinc rf pulse';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.TBP=4;
p.dt=0.05e-3;
p.rfFreq=0;
p.rfPhase=0;
p.tEnd=VCtl.TR;
p.tStart=CV4+0.6e-3;
if strcmp(p.Switch,'on')
if strcmp(p.AnchorTE,'on')
switch VCtl.TEAnchor
case 'Start'
VCtl.TEAnchorTime=p.tStart;
case 'Middle'
VCtl.TEAnchorTime=(p.tStart+p.tEnd)/2;
case 'End'
VCtl.TEAnchorTime=p.tEnd;
end
end
[rfAmp2,rfPhase2,rfFreq2,rfCoil2,rfTime2]=rfSinc(p);
if strcmp(VCtl.MultiTransmit,'off')
if VCtl.MasterTxCoil==rfCoil2(1)
rfAmp=[rfAmp rfAmp2];
rfPhase=[rfPhase rfPhase2];
rfFreq=[rfFreq rfFreq2];
rfCoil=[rfCoil rfCoil2];
rfTime=[rfTime rfTime2];
end
else
rfAmp=[rfAmp rfAmp2];
rfPhase=[rfPhase rfPhase2];
rfFreq=[rfFreq rfFreq2];
rfCoil=[rfCoil rfCoil2];
rfTime=[rfTime rfTime2];
end
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Gz1Sign=1;
p.Gz2Sign=0;
p.Notes='cartesian phase';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.t1End=CV4+CV2;
p.t1Start=CV4+CV1;
p.t2End=VCtl.TE+CV2;
p.t2Start=VCtl.TE+CV1;
p.tRamp=100e-6;
if strcmp(p.Switch,'on')
[GzAmp1,GzTime1]=GzCartesian(p);
GzAmp=[GzAmp GzAmp1];
GzTime=[GzTime GzTime1];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Gy1Sign=1;
p.Gy2Sign=0;
p.Notes='cartesian phase';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.t1End=CV4+CV2;
p.t1Start=CV4+CV1;
p.t2End=VCtl.TE+CV2;
p.t2Start=VCtl.TE+CV1;
p.tRamp=100e-6;
if strcmp(p.Switch,'on')
[GyAmp1,GyTime1]=GyCartesian(p);
GyAmp=[GyAmp GyAmp1];
GyTime=[GyTime GyTime1];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Gx1Sign=-1;
p.Gx2Sign=1;
p.Gx3Sign=0;
p.Notes='cartesian frequency';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.t1Start=CV4+CV1;
p.t2Middle=VCtl.TE;
p.t3Start=VCtl.TE+CV1;
p.tRamp=100e-6;
if strcmp(p.Switch,'on')
[GxAmp1,GxTime1]=GxCartesian(p);
GxAmp=[GxAmp GxAmp1];
GxTime=[GxTime GxTime1];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Notes='cartesian readout';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tMiddle=VCtl.TE;
if strcmp(p.Switch,'on')
[ADC1,ADCTime1]=ADCCartesian(p);
ADC=[ADC ADC1];
ADCTime=[ADCTime ADCTime1];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Ext=5;
p.Notes='calculate remaining scan time';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tStart=0;
if strcmp(p.Switch,'on')
[Ext1,ExtTime1]=ExtBit(p);
Ext=[Ext Ext1];
ExtTime=[ExtTime ExtTime1];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Ext=1;
p.Notes='reset K space location';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tStart=CV4+0.5e-3;
if strcmp(p.Switch,'on')
[Ext2,ExtTime2]=ExtBit(p);
Ext=[Ext Ext2];
ExtTime=[ExtTime ExtTime2];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Ext=6;
p.Notes='dephase Mxy';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tStart=VCtl.TR*(99/100);
if strcmp(p.Switch,'on')
[Ext3,ExtTime3]=ExtBit(p);
Ext=[Ext Ext3];
ExtTime=[ExtTime ExtTime3];
end
p=[];
%--------------------
SEt=[tS tE];
rfAmp(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
rfPhase(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
rfFreq(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
rfCoil(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
GzAmp(GzTime<0 | GzTime>SEt(2)-SEt(1)) = [];
GyAmp(GyTime<0 | GyTime>SEt(2)-SEt(1)) = [];
GxAmp(GxTime<0 | GxTime>SEt(2)-SEt(1)) = [];
ADC(ADCTime<0 | ADCTime>SEt(2)-SEt(1)) = [];
Ext(ExtTime<0 | ExtTime>SEt(2)-SEt(1)) = [];
rfTime(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
GzTime(GzTime<0 | GzTime>SEt(2)-SEt(1)) = [];
GyTime(GyTime<0 | GyTime>SEt(2)-SEt(1)) = [];
GxTime(GxTime<0 | GxTime>SEt(2)-SEt(1)) = [];
ADCTime(ADCTime<0 | ADCTime>SEt(2)-SEt(1)) = [];
ExtTime(ExtTime<0 | ExtTime>SEt(2)-SEt(1)) = [];
rfAmp(abs(rfAmp)<eps) = 0;
rfTime = rfTime + SEt(1);
GzTime = GzTime + SEt(1);
GyTime = GyTime + SEt(1);
GxTime = GxTime + SEt(1);
ADCTime = ADCTime + SEt(1);
ExtTime = ExtTime + SEt(1);
rfAmpAll=[rfAmpAll rfAmp];
rfPhaseAll=[rfPhaseAll rfPhase];
rfFreqAll=[rfFreqAll rfFreq];
rfCoilAll=[rfCoilAll rfCoil];
GzAmpAll=[GzAmpAll GzAmp];
GyAmpAll=[GyAmpAll GyAmp];
GxAmpAll=[GxAmpAll GxAmp];
ADCAll=[ADCAll ADC];
ExtAll=[ExtAll Ext];
rfTimeAll=[rfTimeAll rfTime];
GzTimeAll=[GzTimeAll GzTime];
GyTimeAll=[GyTimeAll GyTime];
GxTimeAll=[GxTimeAll GxTime];
ADCTimeAll=[ADCTimeAll ADCTime];
ExtTimeAll=[ExtTimeAll ExtTime];
SEtAll=[SEtAll SEt];
end
%====================================
if isempty(rfTimeAll)
error('rf sequence line can not be empty! Master Tx coil element must be used.');
end
if isempty(GzTimeAll)
error('GzSS sequence line can not be empty!');
end
if isempty(GyTimeAll)
error('GyPE sequence line can not be empty!');
end
if isempty(GxTimeAll)
error('GxR sequence line can not be empty!');
end
if isempty(ADCTimeAll)
error('ADC sequence line can not be empty!');
end
if isempty(ExtTimeAll)
error('Ext sequence line can not be empty!');
end
SEflag=repmat([0 0 0 0 0 0]',[1 2]);
rfflag=repmat([1 0 0 0 0 0]',[1 max(size(rfTimeAll))]);
Gzflag=repmat([0 1 0 0 0 0]',[1 max(size(GzTimeAll))]);
Gyflag=repmat([0 0 1 0 0 0]',[1 max(size(GyTimeAll))]);
Gxflag=repmat([0 0 0 1 0 0]',[1 max(size(GxTimeAll))]);
ADCflag=repmat([0 0 0 0 1 0]',[1 max(size(ADCTimeAll))]);
Extflag=repmat([0 0 0 0 0 1]',[1 max(size(ExtTimeAll))]);
ts=[[min(SEtAll) max(SEtAll)] rfTimeAll GzTimeAll GyTimeAll GxTimeAll ADCTimeAll ExtTimeAll]-min(SEtAll);
flags=[SEflag rfflag Gzflag Gyflag Gxflag ADCflag Extflag];
[ts,ind]=sort(ts);
uts=unique(ts);
flags=flags(:,ind);
[rfTime,ind]=sort(rfTimeAll-min(SEtAll));
rfAmp=rfAmpAll(:,ind);
rfPhase=rfPhaseAll(:,ind);
rfFreq=rfFreqAll(:,ind);
rfCoil=rfCoilAll(:,ind);
[GzTime,ind]=sort(GzTimeAll-min(SEtAll));
GzAmp=GzAmpAll(:,ind);
[GyTime,ind]=sort(GyTimeAll-min(SEtAll));
GyAmp=GyAmpAll(:,ind);
[GxTime,ind]=sort(GxTimeAll-min(SEtAll));
GxAmp=GxAmpAll(:,ind);
[ADCTime,ind]=sort(ADCTimeAll-min(SEtAll));
ADC=ADCAll(:,ind);
[ExtTime,ind]=sort(ExtTimeAll-min(SEtAll));
Ext=ExtAll(:,ind);
rfAmp(1) = 0;
rfPhase(1) = 0;
rfFreq(1) = 0;
GzAmp(1) = 0;
GyAmp(1) = 0;
GxAmp(1) = 0;
ADC(1) = 0;
Ext(1) = 0;
rfAmp(end) = 0;
rfPhase(end) = 0;
rfFreq(end) = 0;
GzAmp(end) = 0;
GyAmp(end) = 0;
GxAmp(end) = 0;
ADC(end) = 0;
Ext(end) = 0;
end


%% Reads ECG recording in HES format
% Reads ECG recordings in HES (Biosigna) format. Implements the documentation
% available in the help of the application provided with the database (not
% available with the ECHkit).
%
% Arguments:
%   + filename: recording to be read.
%   + start_sample: (opt) start sample to read. Default 1.
%   + end_sample: (opt) end sample to read. Default min(All recording, ECG block of 200 Mbytes)
%
% Output:
%   + ECG: the ECG block
%   + heasig: header with the ECG properties.
%   + ann: annotations for the ECG recordings.
%
% Limits:
% This routine is limited to read blocks smaller than 200 Mbytes for
% performance reasons. You can disable this limit by doing:
% MaxIOread = Inf; %megabytes
%
% See also read_HES_ann, read_HES_header, read_ECG, ECGwrapper
%
% Author: Mariano Llamedo Soria
% <matlab:web('mailto:llamedom@electron.frba.utn.edu.ar','-browser') (email)>
% Version: 0.1 beta
% Birthdate: 17/12/2010
% Last update: 19/11/2014
% Copyright 2008-2015
%
function [ECG heasig ann last_sample] = read_HES_format( filename, start_sample, end_sample )

ann = [];
heasig = [];
ECG = [];
last_sample = [];

tablas_y_constantes;

%No leer bloques mas grandes de 200 megabytes
MaxIOread = 200; %megabytes

if( nargin < 2 || isempty( start_sample ) )
    start_sample = 1;
else
    start_sample = max(1,start_sample);
end

fidECG = fopen( filename, 'r');

if( fidECG > 0 )

    fseek(fidECG, 900, 'bof');

    num_of_leads = fread(fidECG, 1, 'uint8');
    num_of_leads_simult = fread(fidECG, 1, 'uint8');

    if( num_of_leads ~= num_of_leads_simult)
        error('Sampleo no simult�neo, revisar.')
    end

    heasig.nsig = num_of_leads_simult;

    lead_description_idx = fread(fidECG, heasig.nsig, 'uint8');
    [dummy lead_description_table_idx] = intersect(Lead_description_idx, lead_description_idx);
    heasig.desc = char(cLead_description_table(lead_description_table_idx,1));

    fseek(fidECG, 992, 'bof');

    aux = fread(fidECG, 5, 'uint16');

    sample_interval_usec = aux(1);
    heasig.freq = 1/sample_interval_usec*1e6;
    heasig.gain = repmat(1/aux(3),1,heasig.nsig);
    heasig.units = repmat('nV',heasig.nsig,1);
    heasig.adcres = repmat(aux(4),1,heasig.nsig);
    heasig.adczero = repmat(2^(aux(4)-1),1,heasig.nsig);
    heasig.nsamp = fread(fidECG, 1, 'uint32');

    if( nargin < 3 || isempty( end_sample ) )
        %Intento la lectura total por defecto
        samples2read = heasig.nsamp - (start_sample-1);
    else
        samples2read = min(heasig.nsamp, end_sample) - (start_sample-1);
    end

    if( (samples2read*heasig.nsig*2) > (MaxIOread * 1024^2) )
        samples2read = (MaxIOread * 1024^2) / heasig.nsig / 2;
        warning(['No es recomendable leer mas de ' num2str(MaxIOread) ' Mb. Realice varias lecturas.'])
    end

    ECG = nan(samples2read,heasig.nsig);

    try

        fseek(fidECG, 1024+((start_sample-1)*heasig.nsig)*2, 'bof');

        ECG = fread(fidECG, [heasig.nsig samples2read], '*int16')';

        fclose(fidECG);

    catch ME
        fclose(fidECG);
        rethrow(ME)
    end

    last_sample = size(ECG,1) + start_sample - 1;

    [~, heasig.recname] = fileparts(filename);
    heasig.btime = '00:00:00';
    heasig.bdate = '01/01/2000';

    if( nargout > 2 )
        ann = read_HES_ann([ filename(1:end-4) '.lst' ]);
        ann.time = round(ann.time * heasig.freq);
    end

end


function Rob = motion(Rob, Tim)
%MOTION Robot motion.
%   ROB = MOTION(ROB, TIM) performs one EKF-prediction motion step to robot
%   Rob in the global map Map, following the motion model in Rob.motion.
%   Both Rob and Map are updated. The time information Tim is used only if
%   the motion model requires it, but it has to be provided because MOTION
%   is a generic method.
%
%   The following motion models are supported:
%       'odometry'   uses function odo3()
%       'constVel'   uses function constVel()
%   Edit this file to add new motion models.
%
%   See also SIMMOTION, CONSTVEL, ODO3, UPDATEFRAME.

%   Copyright 2009 David Marquez @ LAAS-CNRS.

global Map

% Update rob and sen info from map
Rob = map2rob(Rob);

% robot state range
r = Rob.state.r;

switch Rob.motion

    case  {'constVel'} % constant velocity

        % motion model of the robot: mean and Jacobians
        [Map.x(r), F_x, F_u] = constVel(Map.x(r),Rob.con.u,Tim.dt);

        % update Rob and Map structures - mean only
        Rob = map2rob(Rob);

        % Covariances matrix update
        predictBlockEkf(r, F_x, Rob.con.U, F_u);

    case  {'odometry'}  % 3D odometry

        % motion model of the robot: mean and Jacobians
        [Rob.frame, F_x, F_u]   = odo3(Rob.frame,Rob.con.u);

        % update Rob and Map structures - mean only
        Map.x(Rob.frame.r) = Rob.frame.x;

        % Covariances matrix update
        predictBlockEkf(r, F_x, Rob.con.U, F_u);

    otherwise

        error('??? Unknown motion model ''%s''.',Rob.motion);
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [BSL,BKG] = BSLcalcShedsVis(Wsheds,nSheds,IMAGE)
%"BSLcalcShedsVis"
%   Strips away low values watersheds and calculates BSL
%   -- Returns BSL and Background estimates after each water shed is
%   removed and returns vectors of these values
%
% CRS 07/17/13
%
%Usage:
%   [BSL,BKG] = BSLcalcShedsVis(Wsheds,nSheds,IMAGE)
%       Wsheds  = Struture that holds:
%          Wsheds.PET    = PET VOI
%          Wsheds.Shed   = Watersheds ordered from low mean uptake to high
%          Wsheds.voxVol = voxel volume
%       nSheds = total number of sheds
%       Image  = Flag for display of smoothed BSL vector
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.%
%
%
%
%% BSL calc for all sheds
if (IMAGE == 1)
    figure,
end
bsl = zeros([nSheds 1]);
bkg = zeros([nSheds 1]);

PT = Wsheds.PET;
Shed = Wsheds.Shed;
voxVol = Wsheds.voxVol;
[pX pY pZ] = size(PT);

% build shed masks
ShedTmp = zeros(size(PT));
indNZ = find(Shed);
Shed(isnan(Shed)) = 0;
Shed(isinf(Shed)) = 0;
ShedTmp(indNZ) = Shed(indNZ);

SUVcut = 0.2;
ShedTmp(ShedTmp < SUVcut) = 0;
PT(ShedTmp <= 0) = 0;

% Shed Trimming
k = 0;
while ( sum(ShedTmp(:)) > 0 )
    voxVec = [];

    minShed = min(nonzeros(ShedTmp(:)));
    indS = find(ShedTmp == minShed);
    ShedTmp(indS) = 0;
    PT(indS) = 0;
    if (isempty(indS) == 1)
        continue
    end
    k = k + 1;
    voxVec = nonzeros(PT);

    if (sum(voxVec) <=0)
        bsl(k) = -1;
        bkg(k) = -1;
        BKG = bkg(1:k);
        BSL = bsl(1:k);
        if (IMAGE ~= 0)
            hold off
        end
        return
    end

    nVox = numel(voxVec);
    SUViqr = iqr(voxVec);
    IVHBinWidth = 2 * SUViqr * nVox^(-1/3);
    %%%
    if (numel(voxVec) < 5)
        BKG = bkg(1:k);
        BSL = bsl(1:k);
        if (IMAGE ~= 0)
            hold off
        end
        return
    end
    %%%
    [binHistV, volHistV]  = doseHist(voxVec, voxVol*ones(size(voxVec)), IVHBinWidth);
    nBins = numel(binHistV);

    [Hmax iBkg] = max(volHistV);
    Hbkg = binHistV(iBkg);
    if (iBkg > 1 && iBkg < numel(binHistV))
        Hbkg = ( volHistV(iBkg-1)*binHistV(iBkg-1) ...
            + volHistV(iBkg)*binHistV(iBkg) ...
            + volHistV(iBkg+1)*binHistV(iBkg+1) ) ...
            / ( volHistV(iBkg-1) + volHistV(iBkg) + volHistV(iBkg+1) );
    else
        if (iBkg < numel(binHistV))
            Hbkg = ( volHistV(iBkg)*binHistV(iBkg) ...
                + volHistV(iBkg+1)*binHistV(iBkg+1) ) ...
                / ( volHistV(iBkg) + volHistV(iBkg+1) );
        else
            Hbkg = ( volHistV(iBkg-1)*binHistV(iBkg-1) ...
                + volHistV(iBkg)*binHistV(iBkg) ) ...
                / ( volHistV(iBkg-1) + volHistV(iBkg) );
        end
    end
    SUVcut = Hbkg/3;

    voxVec(voxVec < SUVcut) = 0;
    voxVec = nonzeros(voxVec);
    ShedTmp(ShedTmp < SUVcut) = 0;
    PT(ShedTmp < SUVcut) = 0;

    nVox = numel(voxVec);
    SUViqr = iqr(voxVec);
    IVHBinWidth = 2 * SUViqr * nVox^(-1/3);

    [binHistV, volHistV]  = doseHist(voxVec, voxVol*ones(size(voxVec)), IVHBinWidth);
    nBins = numel(binHistV);
    if (IMAGE == 1)
        colorWidth = 2;
        colorPlot = mod(k,7);
        switch colorPlot
            case (1)
                plot(binHistV,volHistV,'-c','LineWidth',colorWidth)
            case (2)
                plot(binHistV,volHistV,'-g','LineWidth',colorWidth)
            case (3)
                plot(binHistV,volHistV,'-y','LineWidth',colorWidth)
            case (4)
                plot(binHistV,volHistV,'-r','LineWidth',colorWidth)
            case (5)
                plot(binHistV,volHistV,'-k','LineWidth',colorWidth)
            case (6)
                plot(binHistV,volHistV,'-m','LineWidth',colorWidth)
            otherwise
                plot(binHistV,volHistV,'-b','LineWidth',colorWidth)
        end
        drawnow, hold on
    end
    if (IMAGE == 2)
        plot3(binHistV,-k*ones(numel(binHistV)),volHistV), drawnow
        hold on
    end
    if (IMAGE == 3)
        colorWidth = 4;
        colorPlot = mod(k,7);
        switch colorPlot
            case (1)
                plot(binHistV,volHistV,'-c','LineWidth',colorWidth)
            case (2)
                plot(binHistV,volHistV,'-g','LineWidth',colorWidth)
            case (3)
                plot(binHistV,volHistV,'-y','LineWidth',colorWidth)
            case (4)
                plot(binHistV,volHistV,'-r','LineWidth',colorWidth)
            case (5)
                plot(binHistV,volHistV,'-k','LineWidth',colorWidth)
            case (6)
                plot(binHistV,volHistV,'-m','LineWidth',colorWidth)
            otherwise
                plot(binHistV,volHistV,'-b','LineWidth',colorWidth)
        end
        drawnow, hold on
    end
    %% Gaussian FITS
    fitOptions = optimset('Display','off');
    lBins = ceil(SUViqr/IVHBinWidth);

    plusBins  = iBkg + lBins;
    if (plusBins > numel(binHistV))
        plusBins = numel(binHistV);
    end
    minusBins = iBkg  - lBins;
    if (minusBins < 1)
        minusBins = 1;
    end
    if (numel(minusBins:plusBins) < 3)
        if (minusBins+2 <= numel(binHistV))
            plusBins = minusBins + 2;
        else
            minusBins = plusBins - 2;
        end
    end
    if (minusBins < 1)
        BKG = bkg(1:k);
        BSL = bsl(1:k);
        if (IMAGE ~= 0)
            hold off
        end
        return
    end

    sL2 = [];
    initVar = [ Hmax*sqrt(2*pi())*Hbkg Hbkg SUViqr ];
    lBound =  [ 0.01  0.01  0.01  ];
    uBound =  [ inf   inf   inf  ];

    sL2  = lsqnonlin(@(X) (...
        ( volHistV(minusBins:plusBins) - X(1)*normpdf(binHistV(minusBins:plusBins),X(2),X(3)) ) ...
        / sum(volHistV(minusBins:plusBins)) ).*sqrt(volHistV(minusBins:plusBins)), ...
        initVar,lBound,uBound,fitOptions);

    estGauss2 = sL2(1)*normpdf(binHistV(:),sL2(2),sL2(3));

    bkg(k) = sL2(2);

    %% Volume and TLG Estimation
    iEst2 = round( 2*sL2(3)/IVHBinWidth + sL2(2)/IVHBinWidth );
    if  (sL2(2) <= 0)
        iEst2 = round( 2*sL2(3)/IVHBinWidth + iBkg );
    end
    volGauss2 = estGauss2(iEst2:end);

    % Gauss volumes
    vecG2 = zeros(size(volGauss2));
    vecG2 = volHistV(iEst2:end)' - volGauss2;
    vecG2(vecG2 < 0) = 0;
    volG2 = sum(vecG2);

    bsl(k) = vecG2'*binHistV(iEst2:end)';
    if (bsl(k) <= 0 && k > nSheds/10)
        BKG = bkg(1:k);
        BSL = bsl(1:k);
        if (IMAGE ~= 0)
            hold off
        end
        return;
    end

end
BKG = bkg(1:k);
BSL = bsl(1:k);
if (IMAGE ~= 0)
    hold off
end


function spm_dcm_search(P)
% Post hoc optimisation of DCMs (under Laplace approximation)
% FORMAT spm_dcm_search(P)
%
% P         -  character/cell array of DCM filenames
%
%--------------------------------------------------------------------------
% spm_dcm_search operates on different DCMs of the same data to identify
% the best model. It will invert the full model whose free-parameters are
% the union (superset) of all free parameters in each model specified. The
% routine then uses a post hoc selection procedure to evaluate the log-
% evidence and conditional density over free-parameters of each model
% specified.
%
% The DCM specified does not need to be estimated. spm_dcm_search will
% invert the requisite (full DCM) automatically.
%
% The outputs of this routine are graphics reporting the model space search
% (optimisation) and a DCM_optimum (in the first DCMs directory) for the
% best DCM. The structural and function (spectral embedding) graphs are
% based on this DCM.
%
% DCM_optimum  contains the fields:
%        DCM.P   - character/cell array of DCM filenames
%        DCM.PF  - their associated free energies
%        DCM.PP  - and posterior (model) probabilities
%
% In addition, the free energies and posterior estimates of each DCM in P
% are saved for subsequent searches over different partitions of model
% space.
%
% See alos: spm_dcm_post_hoc.m
%
%__________________________________________________________________________
% Copyright (C) 2008-2011 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_search.m 6615 2015-11-30 12:56:02Z peter $

% get filenames
%--------------------------------------------------------------------------
try
    P;
catch
    [P, sts] = spm_select([2 Inf],'^DCM.*\.mat$','Select DCM*.mat files');
    if ~sts, return; end
end

if ischar(P), P = cellstr(P); end
N = numel(P);

%-Check models are compatible in terms of their data
%==========================================================================
for j = 1:N

    % get prior covariances
    %----------------------------------------------------------------------
    load(P{j});

    % and compare it with the first model
    %----------------------------------------------------------------------
    if j == 1
        Y = DCM.Y.y;
    else
        try
            if any(any(Y - DCM.Y.y))
                fprintf('Please check model %i for compatibility',j)
                return
            end
        catch
            fprintf('Please check model %i for compatibility',j)
            return
        end
    end

    % accumate model in terms of which parameters are free
    %----------------------------------------------------------------------
    A = DCM.a;
    B = DCM.b;
    C = DCM.c;
    D = DCM.d;

    % Get full models free parameters
    %----------------------------------------------------------------------
    if j == 1
        a    = A;
        b    = B;
        c    = C;
        d    = D;
    else
        a    = a | A;
        b    = b | B;
        c    = c | C;
        d    = d | D;
    end
end

%-Estimate full model
%==========================================================================
DCM.a = a;
DCM.b = b;
DCM.c = c;
DCM.d = d;

DCM.name = 'optimum';

% Get full priors and posteriors
% -------------------------------------------------------------------------
FUL   = spm_dcm_estimate(DCM);

qE    = FUL.Ep;
qC    = FUL.Cp;
pE    = FUL.M.pE;
pC    = FUL.M.pC;

%-Loop through models and get log-evidences
%==========================================================================
for j = 1:N

    load(P{j});

    % Fix for endogenous DCM (to match spm_dcm_estimate)
    % ---------------------------------------------------------------------
    if isempty(DCM.c) || isempty(U.u)
        DCM.c  = zeros(DCM.n,1);
        DCM.b  = zeros(DCM.n,DCM.n,1);
    end

    % Get model (priors) and evaluate (reduced) free-energy and posteriors
    % ---------------------------------------------------------------------
    [rE,rC]   = spm_dcm_fmri_priors(DCM.a,DCM.b,DCM.c,DCM.d,DCM.options);
    [F,Ep,Cp] = spm_log_evidence(qE,qC,pE,pC,rE,rC);

    % Put reduced conditional estimates in DCM
    % =====================================================================

    % Bayesian inference and variance
    %----------------------------------------------------------------------
    sw       = warning('off','SPM:negativeVariance');
    Pp       = spm_unvec(1 - spm_Ncdf(0,abs(spm_vec(Ep)),diag(Cp)),Ep);
    Vp       = spm_unvec(diag(Cp),Ep);
    warning(sw);

    % Store parameter estimates
    %----------------------------------------------------------------------
    DCM.M.pC = rC;
    DCM.Ep   = Ep;
    DCM.Cp   = Cp;
    DCM.Pp   = Pp;
    DCM.Vp   = Vp;
    DCM.T    = 0;

    % Store predictions of states from full model for simplicity
    %----------------------------------------------------------------------
    DCM.Ce   = FUL.Ce;
    DCM.H1   = FUL.H1;
    DCM.K1   = FUL.K1;
    DCM.R    = FUL.R;
    DCM.y    = FUL.y;

    % Save approximations to model evidence: negative free energy, AIC, BIC
    %----------------------------------------------------------------------
    evidence = spm_dcm_evidence(DCM);
    DCM.F    = F;
    DCM.AIC  = evidence.aic_overall;
    DCM.BIC  = evidence.bic_overall;

    % Save DCM
    %======================================================================
    save(P{j},'DCM','F','Ep','Cp', spm_get_defaults('mat.format'));

    % Record free-energy
    %----------------------------------------------------------------------
    G(j) = F;

end

% Model evidences and best model
% =========================================================================
G     = G - min(G);
p     = exp(G - max(G));
p     = p/sum(p);

% Get selected model
%--------------------------------------------------------------------------
[q,j] = max(p);
load(P{j});

i   = spm_fieldindices(DCM.Ep,'A','B','C','D');
qE  = spm_vec(FUL.Ep);
Ep  = spm_vec(DCM.Ep);
qC  = DCM.Cp;
Cp  = DCM.Cp;
F   = DCM.F;

% Show results
% -------------------------------------------------------------------------
spm_figure('Getwin','Graphics'); clf

subplot(2,2,1)
if length(P) > 32, plot(G,'k'), else bar(G,'c'), end
title('log-posterior','FontSize',16)
xlabel('model','FontSize',12)
ylabel('log-probability','FontSize',12)
axis square

subplot(2,2,2)
if length(P) > 32, plot(p,'k'), else, bar(p,'r'), end
title('model posterior','FontSize',16)
xlabel('model','FontSize',12)
ylabel('probability','FontSize',12)
axis square

% Show full and reduced conditional estimates (for optimum DCM)
%--------------------------------------------------------------------------
subplot(2,2,3)
spm_plot_ci(qE(i),qC(i,i))
title('MAP connections (full)','FontSize',16)
axis square
a   = axis;

subplot(2,2,4)
spm_plot_ci(Ep(i),Cp(i,i))
title('MAP connections (optimum)','FontSize',16)
axis square
axis(a)

% Show structural and functional graphs
%--------------------------------------------------------------------------
spm_figure('Getwin','Graph'); clf

spm_dcm_graph(DCM.xY,DCM.Ep.A)

%-Save optimum and full DCM
%==========================================================================
DCM.P  = P;
DCM.PF = G;
DCM.PP = p;

% Reduced model (optimum)
%--------------------------------------------------------------------------
pth      = fileparts(P{1});
filename = fullfile(pth,'DCM_optimum.mat');
save(filename,'DCM','F','Ep','Cp', spm_get_defaults('mat.format'));

% Full model
%--------------------------------------------------------------------------
DCM      = FUL;
Ep       = FUL.Ep;
Cp       = FUL.Cp;
F        = FUL.F;
filename = fullfile(pth,'DCM_full');
save(filename,'DCM','F','Ep','Cp', spm_get_defaults('mat.format'));


function computeFluxConsistentReactionPresence(modelFolder,propertiesFolder,reconVersion)
% This function extracts the presence of flux consistent reactions for a
% resource of reconstructions that were refined through the semi-automatic
% refinement pipeline (1 = present in the flux consistent submodel, 0 = not
% present in the flux consistent submodel).
%
% USAGE
%   computeFluxConsistentReactionPresence(modelFolder,propertiesFolder,reconVersion)
%
% INPUTS
% modelFolder                                                                                Folder with COBRA models to be analyzed
% propertiesFolder      Folder where the retrieved reaction presences will
%                       be stored (default: current folder)
% reconVersion          Name assigned to the reconstruction resource
%
%   - AUTHOR
%   Almut Heinken, 12/2020

mkdir([propertiesFolder filesep 'ReactionMetabolitePresence'])
currentDir=pwd;
cd([propertiesFolder filesep 'ReactionMetabolitePresence'])

dInfo = dir(modelFolder);
modelList={dInfo.name};
modelList=modelList';
modelList(~(contains(modelList(:,1),{'.mat','.sbml','.xml'})),:)=[];

% check if output file already exists
if isfile(['ReactionPresence_' reconVersion '.txt'])
    reactionPresence = readInputTableForPipeline(['ReactionPresence_' reconVersion '.txt']);
    allRxns=ReactionPresence(1,2:end)';
else
    % restart from existing data if possible
    if isfile([propertiesFolder filesep 'Reactions_' reconVersion '.txt'])
        reactions = readInputTableForPipeline([propertiesFolder filesep 'Reactions_' reconVersion '.txt']);
        allRxns=reactions(:,1);
    else
        allRxns={};
        for i=1:length(modelList)
            i
            model=readCbModel([modelFolder filesep modelList{i} '.mat']);
            allRxns=unique(vertcat(allRxns,model.rxns));
        end
    end
end

% remove models that were already retrieved
modelsRenamed=strrep(modelList(:,1),'.mat','');
modelsRenamed=strrep(modelsRenamed,'.sbml','');
modelsRenamed=strrep(modelsRenamed,'.xml','');
[C,IA]=intersect(modelsRenamed,ReactionPresence(2:end,1));
modelList(IA,:)=[];

% define the intervals in which the computations will be performed
if length(modelList)>5000
    steps=2000;
elseif length(modelList)>200
    steps=200;
else
    steps=25;
end

% in case of reruns, skip if all models are already analyzed
if ~isempty(modelList)
    for i=1:steps:length(modelList)
        if length(modelList)-i>=steps-1
            endPnt=steps-1;
        else
            endPnt=length(modelList)-i;
        end

        modelsToLoad={};
        for j=i:i+endPnt
            if j <= length(modelList)
                modelsToLoad{j}=[modelFolder filesep modelList{j} '.mat'];
            end
        end

        rxnsTmp={};
        parfor j=i:i+endPnt
            model=readCbModel(modelsToLoad{j});
            [~, ~, ~, ~, model] = findFluxConsistentSubset(model);
            rxnsTmp{j}=model.rxns;
        end

        for j=i:i+endPnt
            plusonerow=size(ReactionPresence,1)+1;

            ReactionPresence{plusonerow,1}=strrep(modelList{j},'.mat','');
            for k=1:length(allRxns)
                if ~isempty(find(ismember(rxnsTmp{j},allRxns{k})))
                    ReactionPresence{plusonerow,k+1}=1;
                else
                    ReactionPresence{plusonerow,k+1}=0;
                end
            end
        end
        % export the results as a table
        writetable(cell2table(ReactionPresence),['FluxConsistent_ReactionPresence_' reconVersion],'FileType','text','WriteVariableNames',false,'Delimiter','tab');
    end
end

cd(currentDir)

end


n = 500;

% You need to return the following variables correctly.
x = zeros(n, 1);

word_indices=[100,212,312,423,345,46,37];

for i=1:length(word_indices)
	if (word_indices(i))
		x(i)=1
	else
		x(i)=0;
	end
end


plot(p,h,'LineWidth',2,'Color','b'),grid,ylabel('Altitude: h [m]'),xlabel('Pressure: p [Pa]')


function [model] = tapas_sem_multiv_model(data, ptheta, pars)
%% Set up the model.
%
% Input
%       hgf         -- Hgf model.
%       pars        -- pars structure.
% Output
%       model       -- Model structre.
%

% aponteeduardo@gmail.com
% copyright (C) 2016
%
%% Define the model.
%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

model = struct('graph', []);
model.graph = cell(4, 1);

for i = 1:4
    model.graph{i} = struct('llh', [], 'htheta', []);
end

model.graph{1}.llh = @tapas_sem_multiv_llh;
model.graph{2}.llh = @tapas_mdlinear_hier_llh;
model.graph{3}.llh = @tapas_mdlinear_llh;
model.graph{4}.llh = [];

% Computes the likelihood for a single node.
model.graph{2}.llh_sn = @tapas_mdlinear_hier_llh_sn;

model.graph{1}.htheta = struct('pe', 0.5, 'T', pars.T, 'model', ptheta);

model.graph{2}.htheta = struct('T', ones(size(pars.T, 2)));
model.graph{3}.htheta = struct('T', ones(size(pars.T, 2)));

% The last level is a dummy used to store the hyperpriors.

model.graph{4}.htheta = struct('y', [], 'u', []);

end


%% This function is used for moving the endeffector on a circle, for the KUKA iiwa 7 R 800.
function [ state ] = movePTPCirc1OrintationInterCheck( t , f1,f2, relVel)
% This function is used to move the end-effector on a circle,

%% Arreguments
% t: is the TCP/IP connection
% f1: intermediate frame, to specify a point from the circle, it is 1x6 cell array.
% f2: final frame, to specify the end point of the circle, it is 1x6 cell array.
% the first three elements of cell array represent the X,Y and Z position of
% the frame
% the second three elements of cell array represent the alpha,beta
% and gamma rotaion angles (rads) that represent the frame orientation
% relVel: is a double, the over-ride relative velocity.

%% Return value:
% state: returns (true) if the circular motion is valid, returns false
% otherwise

% Copyright, Mohammad SAFEEA, 9th of May 2017

    theCommand=['jRelVel_',num2str(relVel),'_']; % set over ride.
    fprintf(t, theCommand);
    message=fgets(t);
    % The new position of end-effector, described in robot
    newPos={0,0,0,0,0,0};

    newPos{1}=f1{1};
    newPos{2}=f1{2};
    newPos{3}=f1{3};
    newPos{4}=f1{4};
    newPos{5}=f1{5};
    newPos{6}=f1{6};

    sendCirc1FramePos( t ,newPos); % send first frame of circle to server on controller.

    newPos{1}=f2{1};
    newPos{2}=f2{2};
    newPos{3}=f2{3};
    newPos{4}=f2{4};
    newPos{5}=f2{5};
    newPos{6}=f2{6};

    sendCirc2FramePos( t ,newPos); % send second frame of circle to server on controller.

    theCommand='doPTPinCSCircle1_';
    fprintf(t, theCommand); % start the point to point motion.
    message=fgets(t);

    readingFlag=false;

    message='';

    while readingFlag==false
        message=fgets(t);

        if checkAcknowledgment(message)
            state=true;
            break;
        end

        if checkErrorMessage(message)
            state=false;
            break;
         end
        pause(0.1);  %% This is to enable breaking the program from the outside, using Ctrl+C for example
    end

end

function [ output_args ] = sendCirc1FramePos( t ,jPos)
%% sendCircFramePos
% This function is used to send the first frame of the circle
% to the robot

% Pos: is 6 cells array of doubles
% t: is the TCP/IP connection object
% Copy right, Mohammad SAFEEA, 3rd of May 2017

theCommand='cArtixanPositionCirc1_';

for i=1:6
    x=sprintf('%0.2f',jPos{i});
    theCommand=[theCommand,x,'_'];
end

fprintf(t, theCommand);
message=fgets(t);
end

function [ output_args ] = sendCirc2FramePos( t ,jPos)
%% sendCircFramePos
% This function is used to send the first frame of the circle
% to the robot

% Pos: is 6 cells array of doubles
% t: is the TCP/IP connection object
% Copy right, Mohammad SAFEEA, 3rd of May 2017

theCommand='cArtixanPositionCirc2_';

for i=1:6
    x=sprintf('%0.2f',jPos{i});
    theCommand=[theCommand,x,'_'];
end

fprintf(t, theCommand);
message=fgets(t);
end


function [baseMVA, bus, gen, branch, areas, gencost] = case57
%CASE57    Power flow data for IEEE 57 bus test case.
%   Please see 'help caseformat' for details on the case file format.
%   This data was converted from IEEE Common Data Format
%   (ieee57cdf.txt) on 20-Sep-2004 by cdf2matp, rev. 1.11
%   See end of file for warnings generated during conversion.
%
%   Converted from IEEE CDF file from:
%       http://www.ee.washington.edu/research/pstca/
%
%   Manually modified Qmax, Qmin on generator 1 to 200, -140, respectively.
%
%  08/25/93 UW ARCHIVE           100.0  1961 W IEEE 57 Bus Test Case

%   MATPOWER
%   $Id: case57.m,v 1.5 2004/09/21 01:47:48 ray Exp $

%%-----  Power Flow Data  -----%%
%% system MVA base
baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
bus = [
	1	3	55	17	0	0	1	1.04	0	0	1	1.06	0.94;
	2	2	3	88	0	0	1	1.01	-1.18	0	1	1.06	0.94;
	3	2	41	21	0	0	1	0.985	-5.97	0	1	1.06	0.94;
	4	1	0	0	0	0	1	0.981	-7.32	0	1	1.06	0.94;
	5	1	13	4	0	0	1	0.976	-8.52	0	1	1.06	0.94;
	6	2	75	2	0	0	1	0.98	-8.65	0	1	1.06	0.94;
	7	1	0	0	0	0	1	0.984	-7.58	0	1	1.06	0.94;
	8	2	150	22	0	0	1	1.005	-4.45	0	1	1.06	0.94;
	9	2	121	26	0	0	1	0.98	-9.56	0	1	1.06	0.94;
	10	1	5	2	0	0	1	0.986	-11.43	0	1	1.06	0.94;
	11	1	0	0	0	0	1	0.974	-10.17	0	1	1.06	0.94;
	12	2	377	24	0	0	1	1.015	-10.46	0	1	1.06	0.94;
	13	1	18	2.3	0	0	1	0.979	-9.79	0	1	1.06	0.94;
	14	1	10.5	5.3	0	0	1	0.97	-9.33	0	1	1.06	0.94;
	15	1	22	5	0	0	1	0.988	-7.18	0	1	1.06	0.94;
	16	1	43	3	0	0	1	1.013	-8.85	0	1	1.06	0.94;
	17	1	42	8	0	0	1	1.017	-5.39	0	1	1.06	0.94;
	18	1	27.2	9.8	0	10	1	1.001	-11.71	0	1	1.06	0.94;
	19	1	3.3	0.6	0	0	1	0.97	-13.2	0	1	1.06	0.94;
	20	1	2.3	1	0	0	1	0.964	-13.41	0	1	1.06	0.94;
	21	1	0	0	0	0	1	1.008	-12.89	0	1	1.06	0.94;
	22	1	0	0	0	0	1	1.01	-12.84	0	1	1.06	0.94;
	23	1	6.3	2.1	0	0	1	1.008	-12.91	0	1	1.06	0.94;
	24	1	0	0	0	0	1	0.999	-13.25	0	1	1.06	0.94;
	25	1	6.3	3.2	0	5.9	1	0.982	-18.13	0	1	1.06	0.94;
	26	1	0	0	0	0	1	0.959	-12.95	0	1	1.06	0.94;
	27	1	9.3	0.5	0	0	1	0.982	-11.48	0	1	1.06	0.94;
	28	1	4.6	2.3	0	0	1	0.997	-10.45	0	1	1.06	0.94;
	29	1	17	2.6	0	0	1	1.01	-9.75	0	1	1.06	0.94;
	30	1	3.6	1.8	0	0	1	0.962	-18.68	0	1	1.06	0.94;
	31	1	5.8	2.9	0	0	1	0.936	-19.34	0	1	1.06	0.94;
	32	1	1.6	0.8	0	0	1	0.949	-18.46	0	1	1.06	0.94;
	33	1	3.8	1.9	0	0	1	0.947	-18.5	0	1	1.06	0.94;
	34	1	0	0	0	0	1	0.959	-14.1	0	1	1.06	0.94;
	35	1	6	3	0	0	1	0.966	-13.86	0	1	1.06	0.94;
	36	1	0	0	0	0	1	0.976	-13.59	0	1	1.06	0.94;
	37	1	0	0	0	0	1	0.985	-13.41	0	1	1.06	0.94;
	38	1	14	7	0	0	1	1.013	-12.71	0	1	1.06	0.94;
	39	1	0	0	0	0	1	0.983	-13.46	0	1	1.06	0.94;
	40	1	0	0	0	0	1	0.973	-13.62	0	1	1.06	0.94;
	41	1	6.3	3	0	0	1	0.996	-14.05	0	1	1.06	0.94;
	42	1	7.1	4.4	0	0	1	0.966	-15.5	0	1	1.06	0.94;
	43	1	2	1	0	0	1	1.01	-11.33	0	1	1.06	0.94;
	44	1	12	1.8	0	0	1	1.017	-11.86	0	1	1.06	0.94;
	45	1	0	0	0	0	1	1.036	-9.25	0	1	1.06	0.94;
	46	1	0	0	0	0	1	1.05	-11.89	0	1	1.06	0.94;
	47	1	29.7	11.6	0	0	1	1.033	-12.49	0	1	1.06	0.94;
	48	1	0	0	0	0	1	1.027	-12.59	0	1	1.06	0.94;
	49	1	18	8.5	0	0	1	1.036	-12.92	0	1	1.06	0.94;
	50	1	21	10.5	0	0	1	1.023	-13.39	0	1	1.06	0.94;
	51	1	18	5.3	0	0	1	1.052	-12.52	0	1	1.06	0.94;
	52	1	4.9	2.2	0	0	1	0.98	-11.47	0	1	1.06	0.94;
	53	1	20	10	0	6.3	1	0.971	-12.23	0	1	1.06	0.94;
	54	1	4.1	1.4	0	0	1	0.996	-11.69	0	1	1.06	0.94;
	55	1	6.8	3.4	0	0	1	1.031	-10.78	0	1	1.06	0.94;
	56	1	7.6	2.2	0	0	1	0.968	-16.04	0	1	1.06	0.94;
	57	1	6.7	2	0	0	1	0.965	-16.56	0	1	1.06	0.94;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin
gen = [
	1	128.9	-16.1	200	-140	1.04	100	1	575.88	0;
	2	0	-0.8	50	-17	1.01	100	1	100	0;
	3	40	-1	60	-10	0.985	100	1	140	0;
	6	0	0.8	25	-8	0.98	100	1	100	0;
	8	450	62.1	200	-140	1.005	100	1	550	0;
	9	0	2.2	9	-3	0.98	100	1	100	0;
	12	310	128.5	155	-150	1.015	100	1	410	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status
branch = [
	1	2	0.0083	0.028	0.129	9900	0	0	0	0	1;
	2	3	0.0298	0.085	0.0818	9900	0	0	0	0	1;
	3	4	0.0112	0.0366	0.038	9900	0	0	0	0	1;
	4	5	0.0625	0.132	0.0258	9900	0	0	0	0	1;
	4	6	0.043	0.148	0.0348	9900	0	0	0	0	1;
	6	7	0.02	0.102	0.0276	9900	0	0	0	0	1;
	6	8	0.0339	0.173	0.047	9900	0	0	0	0	1;
	8	9	0.0099	0.0505	0.0548	9900	0	0	0	0	1;
	9	10	0.0369	0.1679	0.044	9900	0	0	0	0	1;
	9	11	0.0258	0.0848	0.0218	9900	0	0	0	0	1;
	9	12	0.0648	0.295	0.0772	9900	0	0	0	0	1;
	9	13	0.0481	0.158	0.0406	9900	0	0	0	0	1;
	13	14	0.0132	0.0434	0.011	9900	0	0	0	0	1;
	13	15	0.0269	0.0869	0.023	9900	0	0	0	0	1;
	1	15	0.0178	0.091	0.0988	9900	0	0	0	0	1;
	1	16	0.0454	0.206	0.0546	9900	0	0	0	0	1;
	1	17	0.0238	0.108	0.0286	9900	0	0	0	0	1;
	3	15	0.0162	0.053	0.0544	9900	0	0	0	0	1;
	4	18	0	0.555	0	9900	0	0	0.97	0	1;
	4	18	0	0.43	0	9900	0	0	0.978	0	1;
	5	6	0.0302	0.0641	0.0124	9900	0	0	0	0	1;
	7	8	0.0139	0.0712	0.0194	9900	0	0	0	0	1;
	10	12	0.0277	0.1262	0.0328	9900	0	0	0	0	1;
	11	13	0.0223	0.0732	0.0188	9900	0	0	0	0	1;
	12	13	0.0178	0.058	0.0604	9900	0	0	0	0	1;
	12	16	0.018	0.0813	0.0216	9900	0	0	0	0	1;
	12	17	0.0397	0.179	0.0476	9900	0	0	0	0	1;
	14	15	0.0171	0.0547	0.0148	9900	0	0	0	0	1;
	18	19	0.461	0.685	0	9900	0	0	0	0	1;
	19	20	0.283	0.434	0	9900	0	0	0	0	1;
	21	20	0	0.7767	0	9900	0	0	1.043	0	1;
	21	22	0.0736	0.117	0	9900	0	0	0	0	1;
	22	23	0.0099	0.0152	0	9900	0	0	0	0	1;
	23	24	0.166	0.256	0.0084	9900	0	0	0	0	1;
	24	25	0	1.182	0	9900	0	0	1	0	1;
	24	25	0	1.23	0	9900	0	0	1	0	1;
	24	26	0	0.0473	0	9900	0	0	1.043	0	1;
	26	27	0.165	0.254	0	9900	0	0	0	0	1;
	27	28	0.0618	0.0954	0	9900	0	0	0	0	1;
	28	29	0.0418	0.0587	0	9900	0	0	0	0	1;
	7	29	0	0.0648	0	9900	0	0	0.967	0	1;
	25	30	0.135	0.202	0	9900	0	0	0	0	1;
	30	31	0.326	0.497	0	9900	0	0	0	0	1;
	31	32	0.507	0.755	0	9900	0	0	0	0	1;
	32	33	0.0392	0.036	0	9900	0	0	0	0	1;
	34	32	0	0.953	0	9900	0	0	0.975	0	1;
	34	35	0.052	0.078	0.0032	9900	0	0	0	0	1;
	35	36	0.043	0.0537	0.0016	9900	0	0	0	0	1;
	36	37	0.029	0.0366	0	9900	0	0	0	0	1;
	37	38	0.0651	0.1009	0.002	9900	0	0	0	0	1;
	37	39	0.0239	0.0379	0	9900	0	0	0	0	1;
	36	40	0.03	0.0466	0	9900	0	0	0	0	1;
	22	38	0.0192	0.0295	0	9900	0	0	0	0	1;
	11	41	0	0.749	0	9900	0	0	0.955	0	1;
	41	42	0.207	0.352	0	9900	0	0	0	0	1;
	41	43	0	0.412	0	9900	0	0	0	0	1;
	38	44	0.0289	0.0585	0.002	9900	0	0	0	0	1;
	15	45	0	0.1042	0	9900	0	0	0.955	0	1;
	14	46	0	0.0735	0	9900	0	0	0.9	0	1;
	46	47	0.023	0.068	0.0032	9900	0	0	0	0	1;
	47	48	0.0182	0.0233	0	9900	0	0	0	0	1;
	48	49	0.0834	0.129	0.0048	9900	0	0	0	0	1;
	49	50	0.0801	0.128	0	9900	0	0	0	0	1;
	50	51	0.1386	0.22	0	9900	0	0	0	0	1;
	10	51	0	0.0712	0	9900	0	0	0.93	0	1;
	13	49	0	0.191	0	9900	0	0	0.895	0	1;
	29	52	0.1442	0.187	0	9900	0	0	0	0	1;
	52	53	0.0762	0.0984	0	9900	0	0	0	0	1;
	53	54	0.1878	0.232	0	9900	0	0	0	0	1;
	54	55	0.1732	0.2265	0	9900	0	0	0	0	1;
	11	43	0	0.153	0	9900	0	0	0.958	0	1;
	44	45	0.0624	0.1242	0.004	9900	0	0	0	0	1;
	40	56	0	1.195	0	9900	0	0	0.958	0	1;
	56	41	0.553	0.549	0	9900	0	0	0	0	1;
	56	42	0.2125	0.354	0	9900	0	0	0	0	1;
	39	57	0	1.355	0	9900	0	0	0.98	0	1;
	57	56	0.174	0.26	0	9900	0	0	0	0	1;
	38	49	0.115	0.177	0.003	9900	0	0	0	0	1;
	38	48	0.0312	0.0482	0	9900	0	0	0	0	1;
	9	55	0	0.1205	0	9900	0	0	0.94	0	1;
];

%%-----  OPF Data  -----%%
%% area data
areas = [
	1	1;
];

%% generator cost data
%	1	startup	shutdown	n	x0	y0	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
gencost = [
	2	0	0	3	0.0775795	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.25	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.0222222	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.0322581	20	0;
];

return;

% Warnings from cdf2matp conversion:
%
% ***** Qmax = Qmin at generator at bus    1 (Qmax set to Qmin + 10)
% ***** area data conversion not yet implemented (creating dummy area data)
% ***** Insufficient generation, setting Pmax at slack bus (bus 1) to 575.88
% ***** MVA limit of branch 1 - 2 not given, set to 9900
% ***** MVA limit of branch 2 - 3 not given, set to 9900
% ***** MVA limit of branch 3 - 4 not given, set to 9900
% ***** MVA limit of branch 4 - 5 not given, set to 9900
% ***** MVA limit of branch 4 - 6 not given, set to 9900
% ***** MVA limit of branch 6 - 7 not given, set to 9900
% ***** MVA limit of branch 6 - 8 not given, set to 9900
% ***** MVA limit of branch 8 - 9 not given, set to 9900
% ***** MVA limit of branch 9 - 10 not given, set to 9900
% ***** MVA limit of branch 9 - 11 not given, set to 9900
% ***** MVA limit of branch 9 - 12 not given, set to 9900
% ***** MVA limit of branch 9 - 13 not given, set to 9900
% ***** MVA limit of branch 13 - 14 not given, set to 9900
% ***** MVA limit of branch 13 - 15 not given, set to 9900
% ***** MVA limit of branch 1 - 15 not given, set to 9900
% ***** MVA limit of branch 1 - 16 not given, set to 9900
% ***** MVA limit of branch 1 - 17 not given, set to 9900
% ***** MVA limit of branch 3 - 15 not given, set to 9900
% ***** MVA limit of branch 4 - 18 not given, set to 9900
% ***** MVA limit of branch 4 - 18 not given, set to 9900
% ***** MVA limit of branch 5 - 6 not given, set to 9900
% ***** MVA limit of branch 7 - 8 not given, set to 9900
% ***** MVA limit of branch 10 - 12 not given, set to 9900
% ***** MVA limit of branch 11 - 13 not given, set to 9900
% ***** MVA limit of branch 12 - 13 not given, set to 9900
% ***** MVA limit of branch 12 - 16 not given, set to 9900
% ***** MVA limit of branch 12 - 17 not given, set to 9900
% ***** MVA limit of branch 14 - 15 not given, set to 9900
% ***** MVA limit of branch 18 - 19 not given, set to 9900
% ***** MVA limit of branch 19 - 20 not given, set to 9900
% ***** MVA limit of branch 21 - 20 not given, set to 9900
% ***** MVA limit of branch 21 - 22 not given, set to 9900
% ***** MVA limit of branch 22 - 23 not given, set to 9900
% ***** MVA limit of branch 23 - 24 not given, set to 9900
% ***** MVA limit of branch 24 - 25 not given, set to 9900
% ***** MVA limit of branch 24 - 25 not given, set to 9900
% ***** MVA limit of branch 24 - 26 not given, set to 9900
% ***** MVA limit of branch 26 - 27 not given, set to 9900
% ***** MVA limit of branch 27 - 28 not given, set to 9900
% ***** MVA limit of branch 28 - 29 not given, set to 9900
% ***** MVA limit of branch 7 - 29 not given, set to 9900
% ***** MVA limit of branch 25 - 30 not given, set to 9900
% ***** MVA limit of branch 30 - 31 not given, set to 9900
% ***** MVA limit of branch 31 - 32 not given, set to 9900
% ***** MVA limit of branch 32 - 33 not given, set to 9900
% ***** MVA limit of branch 34 - 32 not given, set to 9900
% ***** MVA limit of branch 34 - 35 not given, set to 9900
% ***** MVA limit of branch 35 - 36 not given, set to 9900
% ***** MVA limit of branch 36 - 37 not given, set to 9900
% ***** MVA limit of branch 37 - 38 not given, set to 9900
% ***** MVA limit of branch 37 - 39 not given, set to 9900
% ***** MVA limit of branch 36 - 40 not given, set to 9900
% ***** MVA limit of branch 22 - 38 not given, set to 9900
% ***** MVA limit of branch 11 - 41 not given, set to 9900
% ***** MVA limit of branch 41 - 42 not given, set to 9900
% ***** MVA limit of branch 41 - 43 not given, set to 9900
% ***** MVA limit of branch 38 - 44 not given, set to 9900
% ***** MVA limit of branch 15 - 45 not given, set to 9900
% ***** MVA limit of branch 14 - 46 not given, set to 9900
% ***** MVA limit of branch 46 - 47 not given, set to 9900
% ***** MVA limit of branch 47 - 48 not given, set to 9900
% ***** MVA limit of branch 48 - 49 not given, set to 9900
% ***** MVA limit of branch 49 - 50 not given, set to 9900
% ***** MVA limit of branch 50 - 51 not given, set to 9900
% ***** MVA limit of branch 10 - 51 not given, set to 9900
% ***** MVA limit of branch 13 - 49 not given, set to 9900
% ***** MVA limit of branch 29 - 52 not given, set to 9900
% ***** MVA limit of branch 52 - 53 not given, set to 9900
% ***** MVA limit of branch 53 - 54 not given, set to 9900
% ***** MVA limit of branch 54 - 55 not given, set to 9900
% ***** MVA limit of branch 11 - 43 not given, set to 9900
% ***** MVA limit of branch 44 - 45 not given, set to 9900
% ***** MVA limit of branch 40 - 56 not given, set to 9900
% ***** MVA limit of branch 56 - 41 not given, set to 9900
% ***** MVA limit of branch 56 - 42 not given, set to 9900
% ***** MVA limit of branch 39 - 57 not given, set to 9900
% ***** MVA limit of branch 57 - 56 not given, set to 9900
% ***** MVA limit of branch 38 - 49 not given, set to 9900
% ***** MVA limit of branch 38 - 48 not given, set to 9900
% ***** MVA limit of branch 9 - 55 not given, set to 9900


function this = alt_ba_optical_flow(varargin)
%
%ALT_BA_OPTICAL_FLOW
%
%   ALT_BA_OPTICAL_FLOW([IMGS]) constructs a ALT_BA optical flow object
%   with the optional image sequence IMGS ([n x m x 2] array).
%   ALT_BA_OPTICAL_FLOW(O) constructs BA optical flow object by copying O.
%
%   This is a member function of the class 'alt_ba_optical_flow'.
%
% Authors: Deqing Sun, Department of Computer Science, Brown University
% Contact: dqsun@cs.brown.edu
% $Date: 2009 $
% $Revision: $
%
% Copyright 2009-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

error(nargchk(0, 1, length(varargin)));

  switch (length(varargin))
    case 0

      this.images          = [];
      this.lambda          = 5;
      this.lambda_q        = 5;    % Quadratic formulation of the objective function

      this.sor_max_iters   = 1e4;       % 100 seems sufficient

      this.limit_update    = true;      % limit the flow incrment to be less than 1 per linearization step
      this.display         = false;

      this.solver          = 'backslash';   % 'sor' 'pcg' for machines with limited moemory
      this.warping_mode    = 'backward';

      this.texture              = false;     % use texture component as input
      this.deriv_filter         = [1 -8 0 8 -1]/12; % 5-point 7 point [-1 9 -45 0 45 -9 1]/60;
      this.median_filter_size   = []; %[5 5];
      this.interpolation_method = 'cubic';  % 'bi-cubic', 'cubic', 'bi-linear'

      % For Graduated Non-Convexity (GNC) optimization
      this.gnc_iters       = 3;
      this.alpha           = 1;             % change linearly from 1 to 0 through the GNC stages

      this.max_iters       = 10;            % number of warping per pyramid level
      this.max_linear      = 1;             % maximum number of linearization performed per warping, 1 OK for HS

      % For GNC stage 1
      this.pyramid_levels  = 4;
      this.pyramid_spacing = 2;

      % For GNC stage 2 to end
      this.gnc_pyramid_levels     = 2;
      this.gnc_pyramid_spacing    = 1.25;

      method = 'lorentzian'; %'geman_mcclure'; %
      this.spatial_filters = {[1 -1], [1; -1]};
      for i = 1:length(this.spatial_filters);
          this.rho_spatial_u{i}   = robust_function(method, 0.03); % 0.1
          this.rho_spatial_v{i}   = robust_function(method, 0.03);
      end;
      this.rho_data        = robust_function(method, 1.5); % 6.3

      this.seg             = [];    % sore segementatio result
      this.mfT             = 15;    % threshold for intensity-median-filter
      this.imfsz           = [7 7]; % for intensity-median-filter
      this.qterm            = true;   % true: use the qterm
      this.lambda2         = 1e-1;      % weight for coupling term
      this.lambda3         = 1;         % weight for non local term term
      this.weightRatio     = 1;     % lambdaA/weight on the forth term
      this.itersLO         = 1;     % # Li & Osher iterations
      this.color_images     = [];
      this.replacment       = true;

%       this.rho_couple       = robust_function('quadratic', 1);      % penalty function for coupling term
      this.rho_couple       = robust_function('charbonnier', 1e-3);      % penalty function for coupling term

      this = class(this, 'alt_ba_optical_flow');

    case 1
      if isa(varargin{1}, 'alt_ba_optical_flow')
        this = varargin{1};
      else
          this = alt_ba_optical_flow;
          this.images = varargin{1};
      end

    otherwise
      error('Incompatible arguments!');

  end


function [reac, exMets, micRea, binOrg, patOrg, reacPat, reacNumb, reacSet, reacTab, reacAbun, reacNumber] = getMappingInfo(modPath, organisms, abunFilePath)
% This function automatically extracts information from strain abundances in
% different individuals and combines this information into different tables.
%
% USAGE:
%
%    [reac, exMets, micRea, binOrg, patOrg, reacPat, reacNumb, reacSet, reacTab, reacAbun, reacNumber] = getMappingInfo(modPath, organisms, abunFilePath, patNumb)
%
% INPUTS:
%   organisms:         nx1 cell array cell array with names of organisms in the study
%   modPath:           char with path of directory where models are stored
%   abunFilePath:      char with path and name of file from which to retrieve abundance information
%   patNumb:           number of individuals in the study
%
% OUTPUTS:
%   reac:              cell array with all the unique set of reactions
%                      contained in the models
%   exMets:            cell array with all unique extracellular metabolites
%                      contained in the models
%   micRea:            binary matrix assessing presence of set of unique
%                      reactions for each of the microbes
%   binOrg:            binary matrix assessing presence of specific strains in
%                      different individuals
%   reacPat:           matrix with number of reactions per individual
%                      (organism resolved)
%   reacSet:           matrix with names of reactions of each individual
%   reacTab:           char with names of individuals in the study
%   reacAbun:          binary matrix with presence/absence of reaction per
%                      individual: to compare different individuals
%   reacNumber:        number of unique reactions of each individual
%
% .. Author: Federico Baldini 2017-2018

reac = {}; % array with unique set of all the reactions present in the models
exMets = {}; % array with unique set of all the extracellular metabolites present in the models

models = {};
parfor i = 1:length(organisms) % find the unique set of all the reactions contained in the models
    model =readCbModel([modPath filesep organisms{i,1} '.mat']);
    models{i, 1} = model;
end

for i = 1:length(organisms) % find the unique set of all the reactions contained in the models
    smd = models{i, 1};
    reac = union(reac,smd.rxns);
    findmets = smd.mets(find(contains(smd.mets,'[e]')));
    exMets = union(exMets,findmets);
end

% Code to detect reaction presence in each model and create inary matrix
% assessing presence of set of unique reactions for each of the microbes

micRea = zeros(length(models), length(reac));

mdlt = length(models);
parfor i = 1:mdlt
    model = models{i, 1};
    micRea(i,:) = ismember(reac,model.rxns)
end

% creating binary table for abundances
[abundance] = readtable(abunFilePath);

[binary] = abundance;
s = size(binary);
s = s(1, 2);
binary = binary(:, 2:s);  % removing model info and others
binary{:,:} = double(binary{:,:}~=0);
binOrg = binary;

% Compute number of reactions per individual (species resolved)

reacPat = zeros(length(table2cell(binOrg(:, 1))), length(table2cell(binOrg(1, :))));
cleantabc = table2cell(binOrg);
for j = 1:length(table2cell(binOrg(1, :)))
    for i = 1:length(table2cell(binOrg(:, 1)))
        temp = cell2mat(cleantabc(i, j));
        if temp == 1
            reacPat(i, j) = sum(micRea(i, :));
        end
    end
end

% Computing overall (non unique) number of reactions per individual

totReac = [];
for i = 1:length(reacPat(1, :))
    totReac(i, 1) = sum(reacPat(:, i));
end

% Computing number of reactions per organism

reacNumb = [];
for i = 1:length(micRea(:, 1))
    reacNumb(i, 1) = sum(micRea(i, :));
end

% Computing number of organism per individual

patOrg = [];
for i = 1:length(cleantabc(1, :))
    patOrg(i, 1) = sum(table2array(binOrg(:, i)));
end
patOrg = patOrg';

% number and names of UNIQUE reactions per patient
% Briefly, the nonunique reaction content of each individual (reacvec) is
% retrieved from the binary matrix of microbial presence (binOrg) and each of
% the related models. The same is also done using the abundance table for
% establishing reactions coefficients (abunvec) on the base of microbial presence.
% We end up with two nonunique matrices: (completeset) containing reaction content
% for each individual and (completeabunnorm).  Finally, for each individual using
% a list of unique reactions in all the study (reac) all the matches are found and
% the correspondent abundances summed up (numbtab).

reacSet = {};
reacNumber = [];

for j = 1: length(table2cell(binOrg(1, :)))
    abunvec = [];
    reacvec = [];
    for i = 1: length(table2cell(binOrg(:, 1)))
        if (cell2mat(table2cell(binOrg(i, j)))) == 1
            model = models{i, 1};
            reacvec = vertcat(reacvec, model.rxns);
            abunvec((length(abunvec) + 1): ((length(abunvec)) + length(model.rxns))) = table2array(abundance(i, j + 1));
        end
    end

    completeset(1:length(reacvec), j) = reacvec;  % to get lists of reactions per each individual
    completeabunorm(1:length(reacvec), j) = abunvec';  % matrix with abundance coefficients for normalization
    reacSet(1:length(unique(reacvec)), j) = unique(reacvec);  % to get lists of reactions per each individual
    reacNumber(j) = length(unique(reacvec));
end

reacLng = length(reac);

parfor j = 2:size(abundance,2)
    for i = 1:reacLng
        indrxn = find(strcmp(reac(i, 1), completeset(:, j-1)));
        numbtab(i, j-1) = sum(completeabunorm(indrxn,j-1));
    end
end

reacAbun = [reac, num2cell(numbtab)];

% presence/absence of reaction per patient: to compare different patients
% with pCoA
reacTab = zeros(length(reac), length(reacPat(1, :)));

parfor k = 1: length(reacPat(1, :))
    match = zeros(1,length(reac));
        for i = 1: length(reac)
            for j = 1: length(reacSet(:, 1))
                if strcmp(reac(i), reacSet(j, k)) == 1  % the 2 reactions are equal
                    match(i) = 1;
                end
            end
        end
    reacTab(:, k) = match
end
end


clear;close all;clc;j=1i;
Global_Parameters;
%% Hardware Parameters
Mode='transmitRepeat'; % Select Mode
tx_object = sdrtx('ZedBoard and FMCOMMS2/3/4', ...
           'IPAddress',            '192.168.3.2', ...
           'CenterFrequency',      Parameters_struct.CenterFrequency, ...
           'BasebandSampleRate',   Parameters_struct.Bandwidth, ...  % Bandwidth
           'Gain',                 -10, ...
           'ChannelMapping',       1);
%          'EnableBurstMode',1,...

%% Button Setting
figure('Name','TX','NumberTitle','off');
TransmittingDisplay = uicontrol('Style', 'text', 'Position',[55,150,155,35],'String', 'Transmitting','FontSize',20,'HorizontalAlignment','left','BackgroundColor',[0.937 0.867 0.867]);
button = uicontrol; % Generate GUI button
set(button,'String','Stop !','Position',[80 50 100 60]); % Add "Stop !" text
set(gcf,'Units','centimeters','position',[3 3 7 6]); % Set the postion of GUI
%% TX Load
load('TX_signal'); % [1x972]
% transmitRepeat Mode
TX_Hardware = repmat(TX_signal.',5,1); % Transmit Data must be >= 4096 % [4860x1]
state = 1;
%% Main
switch Mode
    case 'step'
        while(state == 1)
           step(tx_object,TX_Hardware);
           % ----- Button Behavior -----%
           set(button,'Callback','setstate0_TX'); % Set the reaction of pushing button
           drawnow;
        end
        release(tx_object);

    case 'transmitRepeat'
        transmitRepeat(tx_object,TX_Hardware);
        % ----- Button Behavior -----%
        set(button,'Callback','setstate0_TX'); % Set the reaction of pushing button
end


function test_bug2770

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

% the *.set file is actually a MATLAB file with the EEG structure in it
% but it require EEGLAB to read it together with the ftd (which has the binary data)

% I imported the data in EEGLAB with the GUI, and then saved it to a *.mat file
load(dccnpath('/home/common/matlab/fieldtrip/data/test/bug2770/164_MIST_prac.mat'));

% the eeglab2fieldtrip function is maintained
ft_hastoolbox('eeglab', 1);

data = eeglab2fieldtrip(EEG, 'preprocessing', 'none');

cfg = [];
cfg.method = 'mtmfft';
cfg.taper = 'hanning';
cfg.foilim = [4 7];
cfg.output = 'pow';
freq = ft_freqanalysis(cfg,data); % this failed

cfg = [];
cfg.trials = 'all';
cfg.channel = 'all';
data1 = ft_selectdata(cfg, data);
assert(isfield(data1, 'trial')); % this failed

% this is actually at the core of the problem
assert(numel(data.label)==size(data.trial{1},1));