function value = p32_r8vec ( action, name, dim_num, value )

%*****************************************************************************80
%
%% P32_R8VEC sets or gets R8VEC parameters for problem 32.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string ACTION, the action.
%    'D' sets the internal value of the object to a default value.
%    If NAME = '*', then all variables are defaulted.
%    'G' means the current value of the object should be returned.
%    'R' means randomize the object and return it.
%    'S' means the input values of the object and its dimension should
%    be stored.
%
%    Input, string NAME, the name of the parameter.
%    'C' is the first vector.
%    'Z' is the base vector.
%
%    Input, integer DIM_NUM, the dimension of the object.
%
%    Input/output, real VALUE(DIM_NUM), the value of the object.
%
  persistent c
  persistent dim_num_save;
  persistent z

  if ( size ( dim_num_save ) == 0 )
    dim_num_save = 0;
  end

  if ( dim_num_save ~= dim_num )
    dim_num_save = 0;
    c = [];
    z = [];
  end

  if ( dim_num_save == 0 )
    dim_num_save = dim_num;
  end

  if ( action == 'D' | action == 'd' )

    if ( name == 'C' | name == 'c' | name == '*' )
      c(1:dim_num) = 0.5^( 1.0 / dim_num );
    end

    if ( name == 'Z' | name == 'z' | name == '*' )
      z(1:dim_num) = 0.5^( 1.0 / dim_num );
    end

  elseif ( action == 'G' | action == 'g' )

    if ( name == 'C' | name == 'c' )
      value(1:dim_num) = c(1:dim_num);
    elseif ( name == 'Z' | name == 'z' )
      value(1:dim_num) = z(1:dim_num);
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P32_R8VEC - Fatal error!\n' );
      fprintf ( 1, '  Unrecognized name = "%s".\n', name );
      error ( 'P32_R8VEC - Fatal error!' );
    end

  elseif ( action == 'R' | action == 'r' )

    if ( name == 'C' | name == 'c' )
      c = rand ( 1, dim_num );
      value(1:dim_num) = c(1:dim_num);
    elseif ( name == 'Z' | name == 'z' )
      z = rand ( 1, dim_num );
      value(1:dim_num) = z(1:dim_num);
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P32_R8VEC - Fatal error!\n' );
      fprintf ( 1, '  Unrecognized name = "%s".\n', name );
      error ( 'P32_R8VEC - Fatal error!' );
    end

  elseif ( action == 'S' | action == 's' )

    if ( name == 'C' | name == 'c' )
      c(1:dim_num) = value(1:dim_num);
    elseif ( name == 'Z' | name == 'z' )
      z(1:dim_num) = value(1:dim_num);
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P32_R8VEC - Fatal error!\n' );
      fprintf ( 1, '  Unrecognized name = "%s".\n', name );
      error ( 'P32_R8VEC - Fatal error!' );
    end

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P32_R8VEC - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized action = "%s".\n', action );
    error ( 'P32_R8VEC - Fatal error!' );

  end

  return
end


function [k, sk] = biasKernCompute(kern, x, x2)

% BIASKERNCOMPUTE Compute the BIAS kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the bias
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the inpute matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the bias
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : biasKernParamInit, kernCompute, kernCreate, biasKernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

if nargin< 3
  dims = [size(x, 1), size(x, 1)];
else
  dims = [size(x, 1), size(x2, 1)];
end
k = repmat(kern.variance, dims);
if nargout > 1
  sk = ones(dims);
end


% The code is to generate the memory base.
% Input: RGB images (img_path)
% Input: Label map (label_path)
% Output: segment, corresponding mask
%% output structure:
% library_img (training data): segmented rgb patches based on connectivity
%
% library_mask: connected segment mask, 1 indicates exist
% library_mask_pole: (optional)Special handling for cityscapes dataset,
% because many objects often separated by thin pole structures (pole,
% light, traffic sign),
% this is not very important just for sightly better results
% for other datasets, can be deleted.

%% Function: Get the corresponding segment based on connectivity analysis.

img_path = '../traindata/RGB256Full/';
label_path = '../traindata/label_refine/';
list = dir([img_path '*.png']);
list = struct2cell(list);
load('cityscapes_colormap.mat');
pixel_matrix = zeros(256,512,3,'uint8');

save_path_library = '../traindata/original_segment/';

mkdir(save_path_library);

img_size_h = 256;
img_size_w = 512;
mapping = load('mapping.mat');
mapping = mapping.mapping;
pole_matrix = zeros(img_size_h,img_size_w,3,'uint8');

%% think structure
% cityscape thin structure effect remove, we find in cityscapes, only
% single buding often split out by the pole, tranffic sign, and traffic
% light, segments with the same class that are separated by these thin
% structures are merged together
pole_matrix(:,:,1) = mapping(6,1);
pole_matrix(:,:,2) = mapping(6,2);
pole_matrix(:,:,3) = mapping(6,3);

sign_matrix = pole_matrix;
sign_matrix(:,:,1) = mapping(7,1);
sign_matrix(:,:,2) = mapping(7,2);
sign_matrix(:,:,3) = mapping(7,3);

light_matrix = pole_matrix;
light_matrix(:,:,1) = mapping(8,1);
light_matrix(:,:,2) = mapping(8,2);
light_matrix(:,:,3) = mapping(8,3);

for c = 1:19

    pixel_matrix = zeros(img_size_h,img_size_w,3,'uint8');
    pixel_matrix(:,:,1) = mapping(c,1);
    pixel_matrix(:,:,2) = mapping(c,2);
    pixel_matrix(:,:,3) = mapping(c,3);
    save_path_library_class = [save_path_library sprintf('%02d',c)];
    mkdir([save_path_library_class '/']);

    for i = size(list,2):size(list,2)
        i
        img = imread([img_path, list{1,i}]);
        img = im2double(img);
        label = imread([label_path list{1,i}]);
        mask = single(label)-single(pixel_matrix);
        mask = sum(mask.^2,3);
        mask(mask~=0) = -1;
        mask = mask + 1;
        mask_original = mask;
        %% sanity check for think structures, comment this if you do not use this
        % comment begin
        if(c~=6 & c~=7 & c~=8)
            mask_pole = sum((single(label)-single(pole_matrix)).^2,3);
            mask_pole(mask_pole~=0) = -1;
            mask_pole = mask_pole + 1;

            mask_light = sum((single(label)-single(light_matrix)).^2,3);
            mask_light(mask_light~=0) = -1;
            mask_light = mask_light + 1;

            mask_sign = sum((single(label)-single(sign_matrix)).^2,3);
            mask_sign(mask_sign~=0) = -1;
            mask_sign = mask_sign + 1;
            small_mask = mask_pole + mask_sign + mask_light;
            mask(mask_pole==1 | mask_light==1 | mask_sign == 1) = 1;
        end
        % comment end

        %% find connected component
        connected_component = bwlabel(mask);
        conn = unique(connected_component);

        library_img = zeros(size(img,1),size(img,2),3,size(conn,1),'uint8');
        library_mask = zeros(size(img,1),size(img,2),size(conn,1),'uint8');
        library_mask_pole = zeros(size(img,1),size(img,2),size(conn,1),'uint8');

        empty_ind = [];
        for j = 1:size(conn,1)
            [r, col] = find(connected_component==conn(j));
          if(c~=6 & c~=7 & c~=8)
            %  t = size(r,1) < 1000
            if((sum(sum(small_mask(sub2ind([img_size_h,img_size_w],r,col))))>=sum(sum(mask_original(sub2ind([img_size_h,img_size_w],r,col))))))
                empty_ind(end+1) = j;
                continue;
            end

          end
            if(mask(r(1),col(1))==1)

                tmp_mask = mask;
                tmp_mask((connected_component~=conn(j))) = 0;

                tmp_img = img.*repmat(tmp_mask,[1,1,3]);
                tmp_img = tmp_img.*repmat(mask_original,[1,1,3]);
                library_img(:,:,:,j) = uint8(tmp_img*255.0);
                library_mask(:,:,j) = uint8(tmp_mask.*mask_original);
                [row_lm, col_lm] = find(library_mask(:,:,j)==1);
                range_row = min(row_lm(:)):max(row_lm(:));
                range_col = min(col_lm(:)):max(col_lm(:));
                library_mask_pole(range_row,range_col,j) = uint8(tmp_mask(range_row,range_col));

            end
        end

        library_img(:,:,:,empty_ind) = [];

        library_mask(:,:,empty_ind) = [];
        library_mask_pole(:,:,empty_ind) = [];
        save([save_path_library_class,'/',list{1,i}(1:end-4),'.mat'],'library_img','library_mask','library_mask_pole');

    end
end


function plot_gc_traj_main(gcTrajFile)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.

    % load gcTrajData
    [id, gc_x, gc_y, gc_z] = load_gc_traj_data(gcTrajFile);

    subplot(1, 1, 1,'replace');

    % draw cube
    edge1_x = [0 0];
    edge1_y = [0 0];
    edge1_z = [0 36];

    edge2_x = [0 0];
    edge2_y = [0 36];
    edge2_z = [0 0];

    edge3_x = [0 36];
    edge3_y = [0 0];
    edge3_z = [0 0];

    edge4_x = [0 36];
    edge4_y = [36 36];
    edge4_z = [0 0];

    edge5_x = [0 0];
    edge5_y = [36 36];
    edge5_z = [0 36];

    edge6_x = [36 36];
    edge6_y = [0 36];
    edge6_z = [0 0];

    edge7_x = [36 36];
    edge7_y = [0 0];
    edge7_z = [0 36];

    edge8_x = [0 0];
    edge8_y = [0 36];
    edge8_z = [36 36];

    edge9_x = [0 36];
    edge9_y = [0 0];
    edge9_z = [36 36];

    edge10_x = [0 36];
    edge10_y = [36 36];
    edge10_z = [36 36];

    edge11_x = [36 36];
    edge11_y = [0 36];
    edge11_z = [36 36];

    edge12_x = [36 36];
    edge12_y = [36 36];
    edge12_z = [0 36];

    plot3(edge1_x,edge1_y,edge1_z, '-k');
    hold on
    plot3(edge2_x,edge2_y,edge2_z, '-k');
    hold on
    plot3(edge3_x,edge3_y,edge3_z, '-k');
    hold on
    plot3(edge4_x,edge4_y,edge4_z, '-k');
    hold on
    plot3(edge5_x,edge5_y,edge5_z, '-k');
    hold on
    plot3(edge6_x,edge6_y,edge6_z, '-k');
    hold on
    plot3(edge7_x,edge7_y,edge7_z, '-k');
    hold on
    plot3(edge8_x,edge8_y,edge8_z, '-k');
    hold on
    plot3(edge9_x,edge9_y,edge9_z, '-k');
    hold on
    plot3(edge10_x,edge10_y,edge10_z, '-k');
    hold on
    plot3(edge11_x,edge11_y,edge11_z, '-k');
    hold on
    plot3(edge12_x,edge12_y,edge12_z, '-k');

    view(20,20)

    % draw gc trajectory

    hold on;
    plot3(gc_y, gc_x, gc_z, '.r', 'MarkerSize',6);

    hold off;
    xl = xlabel('y', 'FontSize',24);
    yl = ylabel('x', 'FontSize',24);
    z1 = zlabel('z', 'FontSize',24);
    view(33,12)

%     set(xl,'Rotation',15);
    set(yl,'Rotation',30);

    axis([0 36 0 36 0 36]);

    set(gca,'xtick',0:9:36)
    set(gca,'ytick',0:18:36)
    set(gca,'ztick',0:9:36)
    set(gca,'FontSize',24, 'LineWidth',1.5); % axis font

    fig = get(groot,'CurrentFigure');
    set (fig,'Position',[500,300,400,300], 'color','w')
end


function E = material_boundary_facets(F,FI)
  % E = material_boundary_facets(F,FI)
  %
  % Inputs:
  %   F  #F by 3 list of triangle indices into rows of some V
  %   FI #F list of material ids
  % Outputs:
  %   E  #E by 2 list of unique edge boundaries between elements with different
  %     ids
  [uE,~,EMAP] = unique(sort([F(:,[2 3]);F(:,[3 1]);F(:,[1 2])],2),'rows');
  uE2FI = sparse(EMAP,repmat(FI,3,1),1);
  E = uE(sum(uE2FI,2)==1 | sum(uE2FI>0,2)==2,:);
end


function sample_points = tapas_physio_get_sample_points(ons_secs, sqpar, slicenum)
% gets times of slice scan events of a particular slice number in every
% volume acquired
%
% USAGE
%   sample_points = tapas_physio_get_sample_points(ons_secs, sqpar, slicenum)
%
% INPUTS:
%   ons_secs
%   sqpar
%   slicenum    - slice number (1<=slicenum<=Nslices) where signal shall be
%                 sampled; alternative: specify sqpar.onset_slice
%
% OUTPUT:
%   sample_points   - absolute time (in seconds) where the specified slice was
%                     aquired for every volume

% Author: Lars Kasper
%
% 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/>.

% slicenum should be field of onset_slice
if nargin<3
    if isfield(sqpar, 'onset_slice')
        slicenum = sqpar.onset_slice;
    end
end

% default timing: first slice
if isempty(slicenum)
    slicenum = 1;
end

nSampleSlices = length(slicenum);
sample_points = zeros(sqpar.Nscans*nSampleSlices,1);
for n = 1:sqpar.Nscans
    spulse = ons_secs.spulse_per_vol{n + sqpar.Ndummies};
    if length(spulse) < max(slicenum)
        error('scan %d: only %d slice scan events. Cannot resample to slice %d', ...
            n, length(spulse), max(slicenum));
    else
        sample_points((n-1)*nSampleSlices + (1:nSampleSlices)) = spulse(slicenum);
    end
end

end


function obj_io_test04 ( output_file_name )

%*****************************************************************************80
%
%% TEST04 tests OBJ_WRITE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2008
%
%  Author:
%
%    John Burkardt
%
  face_num = 12;
  node_num = 8;
  normal_num = 0;
  order_max = 3;

  face_node = [ ...
    1, 3, 2; ...
    2, 3, 4; ...
    1, 6, 5; ...
    1, 2, 6; ...
    3, 7, 4; ...
    4, 7, 8; ...
    5, 6, 8; ...
    5, 8, 7; ...
    1, 5, 7; ...
    1, 7, 3; ...
    2, 4, 6; ...
    6, 4, 8 ]';
  face_order = [ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 ];
  node_xyz = [ ...
    0.0, 0.0, 0.0; ...
    1.0, 0.0, 0.0; ...
    0.0, 1.0, 0.0; ...
    1.0, 1.0, 0.0; ...
    0.0, 0.0, 1.0; ...
    1.0, 0.0, 1.0; ...
    0.0, 1.0, 1.0; ...
    1.0, 1.0, 1.0 ]';
  normal_vector = [];
  vertex_normal = [];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  OBJ_WRITE writes an ASCII OBJ file.\n' );
  fprintf ( 1, '  Here, we do NOT supply any normal vectors.\n' );

  obj_write ( output_file_name, node_num, face_num, normal_num, ...
    order_max, node_xyz, face_order, face_node, normal_vector, vertex_normal );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Graphics data was written to the OBJ file "%s".\n', ...
    output_file_name );

  return
end


% IN ORDER TO SIMULATE THE PROGRAM:
%   A) FIRST, LOAD A ROBOT
%       robot = load_robot('abb','irb140');
%   B) NEXT, LOAD SOME EQUIPMENT.
%       robot.equipment = load_robot('equipment','tables/table_small');
%       OR
%       robot.equipment = load_robot('equipment','bumper_cutting');
%   C) NOW, LOAD AN END TOOL
%       robot.tool= load_robot('equipment','end_tools/parallel_gripper_0');
%   D) FINALLY, LOAD A PIECE TO GRAB BY THE ROBOT
%       robot.piece=load_robot('equipment','cylinders/cylinder_tiny');
%
%   E) IF NECESSARY, CHANGE THE POSITION AND ORIENTATION OF THE ROBOT'S
%   BASE
%       robot.piece.T0= [1 0 0 -0.35;
%                        0 1 0 -0.55;
%                        0 0 1 0.2;
%                        0 0 0 1];
%
% during the simulation, call simulation_open_tool; to open the tool and
% simulation_close_tool; to close it.
% To grip the piece, call simulation_grip_piece; and
% simulation_release_piece to release it.
% The call to each function must be correct, thus, typically the correct
% sequence is:
% simulation_open_tool;
% approach the piece to grab.
% simulation_close_tool;
% simulation_grip_piece; --> the piece will be drawn with the robot
% move to a different place
% simulation_open_tool;
% simulation_release_piece

function test_1

global RT_tp1 RT_tp2 TD_tool0

RT_tp1=[[0.4000, 0.3000, 0.7000],[1.0000, -0.0000, -0.0000, -0.0000], [0, -1, -1, 0], [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
RT_tp2=[[0.1000, -0.4000, 0.7000],[0.0000, -0.0000, 0.0000, 1.0000], [-1, -1, -2, 0], [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

%definition of the end effector.
TD_tool0=[1,[[0,0,0],[1,0,0,0]],[0,[0,0,0],[1,0,0,0],0,0,0]];

%finally, call the main function
main
end

function main
global RT_tp1 RT_tp2 TD_tool0

MoveJ(RT_tp1, 'vmax' , 'fine' , TD_tool0, 'wobj0');
MoveJ(RT_tp2, 'vmax' , 'fine' , TD_tool0, 'wobj0');
MoveL(RT_tp1, 'vmax' , 'fine' , TD_tool0, 'wobj0');
MoveL(RT_tp2, 'vmax' , 'fine' , TD_tool0, 'wobj0');

end


function deri = deri_inherit(deri_cost)
    global config mem;
    deri = deri_cost .* config.DERI_NONLINEARITY(mem.output) / config.batch_size;
end


%GROUPRECTANGLES_MEANSHIFT  Groups the object candidate rectangles using meanshift
%
%     [rects, weights] = cv.groupRectangles_meanshift(rects, weights, scales)
%     [...] = cv.groupRectangles_meanshift(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __rects__ Input cell array of rectangles, where each rectangle is
%   represented as a 4-element vector `{[x,y,w,h], ...}`, or a numeric
%   Nx4/Nx1x4/1xNx4 array.
% * __weights__ Input vector of associated weights.
% * __scales__ Input vector of corresponding rectangles scales.
%
% ## Output
% * __rects__ Output array of retained and grouped rectangles. Same format as
%   input `rects` (either Nx4 numeric array or cell array).
% * __weights__ Output updated weights.
%
% ## Options
% * __DetectThreshold__ detection threshold (weight) above which resulting
%   modes are kept. default 0.0
% * __WinDetSize__ window size `[w,h]`. default [64,128]
%
% See also: cv.groupRectangles, cv.SimilarRects
%


function [expected_overlaps, evaluated_lengths, practical_difference] = estimate_expected_overlap(tracker, experiment, sequences, varargin)
% estimate_expected_overlap Estimates expected average overlap for
% different sequence lengths
%
% This function estimates the expected average overlap for sequence of a
% given lengths based on the data
%
% Input:
% - tracker (struct): A valid tracker descriptor.
% - experiment (struct): A valid experiment descriptor.
% - sequences (cell): An array of valid sequence descriptors.
% - varargin[Lengths] (vector): A vector of sequence lengths for which the
% overlap should be evaluated.
% - varargin[Weights] (vector): A vector of per-sequence weigths that indicate
% how much does each sequence contributes to the estimate.
% - varargin[Tags] (cell): A set of tags for which to perform
% calculation. If not set then only 'all' is used.
%
% Output:
% - expected_overlaps (vector): Expected overlaps for corresponding
% lengths.
% - evaluated_lengths (vector): A filtered array of lengths (removed duplicates).
% - practical_difference (vector): An estimate of the practical difference for
% corresponding expected overlap.

lengths = [];
weights = ones(numel(sequences), 1);
tags = {'all'};

for j=1:2:length(varargin)
    switch lower(varargin{j})
        case 'lengths', lengths = varargin{j+1};
        case 'weights', weights = varargin{j+1};
        case 'tags', tags = varargin{j+1};
        otherwise, error(['unrecognized argument ' varargin{j}]);
    end
end

context.failures = {};
context.overlaps = {};
context.practical = {};
context.sources = [];
context = iterate(experiment, tracker, sequences, 'iterator', @collect_segments, 'context', context);
failures = context.failures;
segments = context.overlaps;
practical = context.practical;
occurences = hist(context.sources, max(context.sources));
% if isempty(context.practical)
%   occurences = 0;
% else
%   occurences = hist(context.sources, max(context.sources));
% end

if isempty(lengths)
    maxlen = max(cellfun(@(x) numel(x), segments, 'UniformOutput', true));
    lengths = 1:maxlen;
end

% sort and remove duplicates
lengths = unique(lengths);

skipping = experiment.parameters.skip_initialize;

fragments_count = sum(cellfun(@(x) numel(x) + 1, failures, 'UniformOutput', true));
fragments_length = max(lengths);

tag_count = numel(tags);

if isempty(segments)
    expected_overlaps = zeros(0, tag_count);
    practical_difference = zeros(0, tag_count);
    evaluated_lengths = [];
    return;
end

expected_overlaps = zeros(numel(lengths), tag_count);
practical_difference = zeros(numel(lengths), tag_count);

for l = 1:tag_count

    sequence_weights = weights(context.sources(:));
    frequency = occurences(context.sources(:));
    sequence_weights = sequence_weights(:) ./ frequency(:);

    tag = tags{l};

    fragments = nan(fragments_count, fragments_length);
    fpractical = nan(fragments_count, fragments_length);
    fweights = nan(fragments_count, 1);
    f = 1;
    for i = 1:numel(segments)
        % calculate number of failures and their positions in the trajectory
        F = numel(failures{i});
        if F > 0
            % add first part of the trajectory to the fragment list
            points = failures{i}' + skipping;
            points = [1, points(points <= numel(segments{i}))];

            for j = 1:numel(points)-1;
                o = segments{i}(points(j):points(j+1)); o(isnan(o)) = 0;
                fragments(f, :) = 0;
                fragments(f, 1:min(numel(o), fragments_length)) = o;

                o = practical{i}(points(j):points(j+1)); o(isnan(o)) = 0;
                fpractical(f, :) = 0;
                fpractical(f, 1:min(numel(o), fragments_length)) = o;

                w = numel(sequence_query_tag(sequences{context.sources(i)}, tag, points(j):(points(j+1)))) ...
                    / (points(j+1) - points(j) + 1);

                fweights(f) = sequence_weights(i) * w;

                f = f + 1;
            end;

            o = segments{i}(points(end):end); o(isnan(o)) = 0;
            fragments(f, 1:min(numel(o), fragments_length)) = o;
            o = practical{i}(points(end):end); o(isnan(o)) = 0;
            fpractical(f, 1:min(numel(o), fragments_length)) = o;

            w = numel(sequence_query_tag(sequences{context.sources(i)}, tag, points(end):length(segments{i}))) ...
                / (sequences{context.sources(i)}.length - points(end) + 1);

            fweights(f) = sequence_weights(i) * w;

            f = f + 1;
        else
        % process also last part of the trajectory - segment without failure
            if numel(segments{i}) >= fragments_length
                % tracker did not fail on this sequence and it is longer than
                % observed interval
                fragments(f, :) = segments{i}(1:fragments_length);
                fpractical(f, :) = practical{i}(1:fragments_length);

                w = numel(sequence_query_tag(sequences{context.sources(i)}, tag, 1:fragments_length)) ...
                    / fragments_length;
            else
                fragments(f, 1:numel(segments{i})) = segments{i};
                fpractical(f, 1:numel(practical{i})) = practical{i};

                w = numel(sequence_query_tag(sequences{context.sources(i)}, tag)) ...
                    / sequences{context.sources(i)}.length;
            end

            fweights(f) = sequence_weights(i) * w;
            f = f + 1;
        end
    end

    for e = 1:size(expected_overlaps, 1)
        len = lengths(e);
        % do not calculate for Ns == 1: overlap on first frame is always NaN
        if len == 1
            expected_overlaps(e, l) = 1;
            continue;
        end

        usable = ~isnan(fragments(:, len));

        if ~any(usable)
            continue;
        end;

        % for each len get a single number - average overlap
        expected_overlaps(e, l) = sum(mean(fragments(usable, 2:len), 2) .* fweights(usable)) ./ sum(fweights(usable));
        practical_difference(e, l) = sum(mean(fpractical(usable, 2:len), 2) .* fweights(usable)) ./ sum(fweights(usable));

    end

end

evaluated_lengths = lengths;

end

function context = collect_segments(event, context)

switch (event.type)

    case 'sequence_enter'

        sequence = event.sequence;

        sequence_directory = fullfile(event.tracker.directory, event.experiment.name, ...
            sequence.name);

        switch event.experiment.type
            case {'supervised', 'realtime'}

                for i = 1:event.experiment.parameters.repetitions

                    result_file = fullfile(sequence_directory, sprintf('%s_%03d.txt', event.sequence.name, i));

                    if i == 4 && is_deterministic(sequence, 3, sequence_directory)
                        break;
                    end;

                    if ~exist(result_file, 'file')
                        continue;
                    end;

                    trajectory = read_trajectory(result_file);

                    [~, frames] = estimate_accuracy(trajectory, sequence);

                    [~, failures] = estimate_failures(trajectory, sequence);

                    practical = sequence_get_frame_value(sequence, 'practical');

                    context.failures{end+1} = failures(failures <= sequence.length);
                    context.overlaps{end+1} = frames;
                    context.sources(end+1) = event.sequence_index;

                    if isempty(practical)
                        context.practical{end+1} = zeros(sequence.length, 1);
                    else
                        context.practical{end+1} = practical;
                    end

                end;

            otherwise, error(['unsupported type ', event.experiment.type]);
        end

end;

end


function pick = nms_IoMin(boxes, overlap)
% top = nms(boxes, overlap)
% Non-maximum suppression. (FAST VERSION)
% Greedily select high-scoring detections and skip detections
% that are significantly covered by a previously selected
% detection.
%
% NOTE: This is adapted from Pedro Felzenszwalb's version (nms.m),
% but an inner loop has been eliminated to significantly speed it
% up in the case of a large number of boxes

% Copyright (C) 2011-12 by Tomasz Malisiewicz
% All rights reserved.
%
% This file is part of the Exemplar-SVM library and is made
% available under the terms of the MIT license (see COPYING file).
% Project homepage: https://github.com/quantombone/exemplarsvm

if isempty(boxes)
  pick = [];
  return;
end

x1 = boxes(:,1);
y1 = boxes(:,2);
x2 = boxes(:,3);
y2 = boxes(:,4);
s = boxes(:,end);

area = (x2-x1+1) .* (y2-y1+1);
[vals, I] = sort(s);

pick = s*0;
counter = 1;
while ~isempty(I)
  last = length(I);
  i = I(last);
  pick(counter) = i;
  counter = counter + 1;

  xx1 = max(x1(i), x1(I(1:last-1)));
  yy1 = max(y1(i), y1(I(1:last-1)));
  xx2 = min(x2(i), x2(I(1:last-1)));
  yy2 = min(y2(i), y2(I(1:last-1)));

  w = max(0.0, xx2-xx1+1);
  h = max(0.0, yy2-yy1+1);

  inter = w.*h;
%   o = inter ./ (area(i) + area(I(1:last-1)) - inter);
  o = inter ./ min(area(i),area(I(1:last-1)));

  I = I(find(o<=overlap));
end

pick = pick(1:(counter-1));


function  gB = reorder(gB,varargin)
% nicely reorder grain boundaries

% compute tours through the adjecency matrix
[idV,idE] = EulerTours(gB.A_V);

% remove breaks
id = isnan(idE);
idE(id) = [];
idV(id) = [];

% reorder edges
F = gB.F(idE,:);

% and also all other properties
gB.ebsdId = gB.ebsdId(idE,:);
gB.grainId = gB.grainId(idE,:);
gB.phaseId = gB.phaseId(idE,:);
gB.misrotation = gB.misrotation(idE);

% which needs to be flipped?
flip = F(:,1) ~= idV(:);

% flip vertices
F(flip,:) = fliplr(F(flip,:));
gB.F = F;


close all
clear

%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=24;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6gamma2_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories

save('timecom24','nmpcplot1','impcplot1');
save('feasibility24','nmpcinf','impcinf');

distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF

%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end

function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;

%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];

%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
    x0(4,1)*sin(x0(3,1))*dt;
    0;
    0];
system.C =[0 0 0 0;
    0 0 0 0;
    1*dt 0 0 0;
    0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;

%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;

%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;

%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
    fprintf('Run NMPC-DCBF-24\n');
    controller_nmpc_dcbf_multiple24 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
    controller_nmpc_dcbf_multiple24.obs = obs;
    controller_nmpc_dcbf_multiple24.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple24.tt;
end

function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;

tic;

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
%    error('OSQP did not solve the problem!')
        t2=-1;
        return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)%Iterations for the first time-step
    storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return

x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)%Iterations for the left time steps

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
%        error('OSQP did not solve the problem!')
        t2=-1;
        return
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    x0 = ctrlx(1:nx);
    testx0 = [testx0 x0];
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end
end

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end


function subpak_test06 ( )

%*****************************************************************************80
%
%% TEST06 tests BAR_CHECK, BAR_CODE, BAR_DIGIT_CODE_LEFT and BAR_DIGIT_CODE_RIGHT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST06\n' );
  fprintf ( 1, '  BAR_CHECK checks digits for a barcode;\n' );
  fprintf ( 1, '  BAR_CODE computes the barcode for a string of\n' );
  fprintf ( 1, '  11 digits;\n' );
  fprintf ( 1, '  BAR_DIGIT_CODE_LEFT returns the left digit code.\n' );
  fprintf ( 1, '  BAR_DIGIT_CODE_RIGHT returns the right digit code.\n' );

  for i = 1 : 11
    digit(i) = mod ( i-1, 10 );
  end

  check = bar_check ( digit );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The check digit is %d\n', check );

  digit(12) = check;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The left and right digit codes:\n' );
  fprintf ( 1, '\n' );
  for i = 0 : 9
    codel = bar_digit_code_left ( i );
    coder = bar_digit_code_right ( i );
    fprintf ( 1, '  %2d  %s  %s\n', i, codel, coder );
  end

  bar = bar_code ( digit );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Bar code:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    %s\n', bar(1:9) );
  fprintf ( 1, '    %s\n', bar(10:12) );
  fprintf ( 1, '    %s\n', bar(13:19) );
  fprintf ( 1, '    %s\n', bar(20:26) );
  fprintf ( 1, '    %s\n', bar(27:33) );
  fprintf ( 1, '    %s\n', bar(34:40) );
  fprintf ( 1, '    %s\n', bar(41:47) );
  fprintf ( 1, '    %s\n', bar(48:54) );
  fprintf ( 1, '    %s\n', bar(55:59) );
  fprintf ( 1, '    %s\n', bar(60:66) );
  fprintf ( 1, '    %s\n', bar(67:73) );
  fprintf ( 1, '    %s\n', bar(74:80) );
  fprintf ( 1, '    %s\n', bar(81:87) );
  fprintf ( 1, '    %s\n', bar(88:94) );
  fprintf ( 1, '    %s\n', bar(95:101) );
  fprintf ( 1, '    %s\n', bar(102:104) );
  fprintf ( 1, '    %s\n', bar(105:113) );

  return
end


function inspect_bug3141

% WALLTIME 00:10:00
% MEM 2gb
% DEPENDENCY ft_defacemesh ft_defacevolume

%% anatomical mri

mri = ft_read_mri(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.mri'));

cfg = [];
defaced = ft_defacevolume(cfg, mri);

cfg = [];
ft_sourceplot(cfg, defaced);

%% head shape

headshape = ft_read_headshape(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.shape'));

cfg = [];
defaced = ft_defacemesh(cfg, headshape);

figure
ft_plot_mesh(defaced);

%% 3D grid source model

% this MATLAB file contains the variable sourcemodel
load(dccnpath('/home/common/matlab/fieldtrip/template/sourcemodel/standard_sourcemodel3d4mm.mat'));

cfg = [];
defaced = ft_defacemesh(cfg, sourcemodel);

figure
ft_plot_mesh(defaced.pos(defaced.inside,:));

%% cortical sheet source model

sourcemodel = ft_read_headshape(dccnpath('/home/common/matlab/fieldtrip/template/sourcemodel/cortex_8196.surf.gii'));

cfg = [];
defaced = ft_defacemesh(cfg, sourcemodel);

figure
ft_plot_mesh(defaced);
camlight
lighting phong


function plotLowLevelPerformanceMrf

load('./results/classifierComparisonResult_stage3.mat');
load('./results/classifierComparisonResult_stage3mrf.mat');
load('./results/classifierComparisonResult_stage3mrfg.mat');

figure(1), hold off
plot(pr_pb.r, pr_pb.p, 'b:', 'LineWidth', 4);

hold on
plot(pr_im.r, pr_im.p, 'g--', 'LineWidth', 3);
plot(pr_all.r, pr_all.p, 'r', 'LineWidth', 3);
plot(pr_mrf.r, pr_mrf.p, '--c', 'LineWidth', 4);
plot(pr_mrfg.r, pr_mrfg.p, '--m', 'LineWidth', 4);

legend({'Pb Only', 'Pb+Edge/Region Cues', 'Pb+Edge/Region+3D Cues', 'All Cues + MRF'}, ...
    'Location', 'SouthWest', 'FontSize', 12);
title('Boundary Classification', 'FontSize', 20);
xlabel('Recall', 'FontSize', 18);
ylabel('Precision', 'FontSize', 18);
axis([0 1 0 1])
set(gca, 'FontSize', 14);


function plot_3d_odomap_without_groundtruth(odoMapFile, ...
    xOdoMapScaling, yOdoMapScaling, zOdoMapScaling, ...
    xOdoMapTrans, yOdoMapTrans, zOdoMapTrans, ...
    xGtScaling, yGtScaling, zGtScaling)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.

    % load ground truth data
%     [frameId, gt_x, gt_y, gt_z, gt_rx, gt_ry, gt_rz] = load_ground_truth_data(groundTruthFile);

    % load experience map data
    [id, odomap_x, odomap_y, odomap_z, odomap_yaw, odomap_pitch] = load_exp_map_data(odoMapFile);

    figure('color',[1 1 1]);
    hold on

    plot3(odomap_y * yOdoMapScaling + yOdoMapTrans, ...
        odomap_x * xOdoMapScaling + xOdoMapTrans, ...
        odomap_z * zOdoMapScaling + zOdoMapTrans, '.b');
%     plot3((gt_x - gt_x(1)) * xGtScaling,(gt_y - gt_y(1)) * yGtScaling, (gt_z - gt_z(1)) * zGtScaling, '.r');
    hold off;

%     view(3)
view(0, 90);
    grid on
%     xlabel('x');
%     ylabel('y');
    xl = xlabel('x');
    yl = ylabel('y');
    zlabel('z');
    set(xl,'Rotation',15);
    set(yl,'Rotation',-30);
%     title('3D Experience Map');
    %                     legend('Result','Truth' ,'1');
    % axis([-10 20 -10 20 -10 20]);
    %                     axis equal
%     legend('Experience map','Ground truth', 'location', '0');
    hh = legend('Odomap','Ground truth');

    set(hh,'position',[.76 .82 .05 .05]);
    axis on
    rotate3d on
end


classdef Settings %< handle%& matlab.mixin.Copyable

    properties %optmizer access
        optimizerSettings
        plotting = true
        showBC = true
        BCscale_factor = 0.10
        rotation_per_it = 0
        printing = true
        printing_physics = false
        monitoring = true
        monitoring_interval = 10
        maxiter = 2000
        constraint_case = {'EQUALITY'}
        HJiter0
        e2
        ub = 1;
        lb = 0;
        volumeMicro
        superEllipseRatio
    end

    properties %target parameters
        Vfrac_initial
        optimality_initial
        constr_initial
        Vfrac_final
        optimality_final
        constr_final
        epsilon_initial
        epsilon_final
        Perimeter_target
        epsilon_isotropy_initial
        epsilon_isotropy_final
        stressNormExponent_initial
        stressNormExponent_final
    end

    properties
        perimeter = struct;
    end

    properties %LevelSetCreator
        levelSetDataBase
    end

    properties    %topopt access
        ptype
        pdim
        case_file
        filename
        material
        initial_case
        cost
        weights
        constraint
        optimizer
        optimizerUnconstrained
        line_search_initiator
        incrementFactor
        rate
        filter
        unfitted_mesh_algorithm='DELAUNAY'
        TOL = struct;
        target_parameters = struct;
        nsteps
        micro = struct;
        selectiveC_Cstar
        nconstr
        warningHoleBC
        printIncrementalIter
        printChangingFilter
        printMode = 'DesignAndShapes';
        homegenizedVariablesComputer
        materialInterpolation
        designVariable
        vademecumFileName
        nelem
        m1
        m2
        alpha0
        rho0
        isDesignVariableFixed
        costDomainNotOptimizable
        constraintDomainNotOptimizable
    end

    properties %exploring tests
        shFuncParamsName
    end

    methods
        function obj = Settings(case_file)
            run(case_file)
            obj.case_file=case_file;
            obj.filename = filename;
            obj.ptype = ptype;

            if exist('method','var')
                obj.materialInterpolation = method;
            end

            if exist('materialType','var')
                obj.material = materialType;
            end

            if exist('initial_case','var')
                 obj.initial_case = initial_case;
                 if isequal(initial_case,'full')
                    obj.levelSetDataBase.type = 'initial_case';
                 end
            end

            if exist('isDesignVariableFixed','var')
                femReader = FemInputReader_GiD();
                s = femReader.read(obj.filename);
                coord = s.mesh.coord;
                obj.isDesignVariableFixed.nodes  = isDesignVariableFixed.nodes(coord);
                obj.isDesignVariableFixed.values = isDesignVariableFixed.values(coord);
            end

            if exist('costDomainNotOptimizable','var')
                femReader = FemInputReader_GiD();
                s = femReader.read(obj.filename);
                coord = transpose(s.mesh.computeBaricenter);
                for i = 1:numel(costDomainNotOptimizable)
                    cD = costDomainNotOptimizable{i};
                    if ~isempty(cD)
                        obj.costDomainNotOptimizable{i} = cD(coord);
                    else
                        obj.costDomainNotOptimizable{i} = false(size(coord,1));
                    end
                end
            end
            if exist('constraintDomainNotOptimizable','var')
                femReader = FemInputReader_GiD();
                s = femReader.read(obj.filename);
                coord = transpose(s.mesh.computeBaricenter);
                for i = 1:numel(constraintDomainNotOptimizable)
                    cD = constraintDomainNotOptimizable{i};
                    if ~isempty(cD)
                        obj.constraintDomainNotOptimizable{i} = cD(coord);
                    else
                        obj.constraintDomainNotOptimizable{i} = false(size(coord,1),1);
                    end
                end
            end

            obj.cost = cost;
            obj.weights = weights;
            obj.constraint = constraint;
            obj.nconstr = length(constraint);
            obj.optimizer = optimizer;
            if exist('incrementFactor','var')
               obj.incrementFactor = incrementFactor;
            end
            obj.filter = filterType;
            obj.nsteps = nsteps;
            if exist('TOL','var')
                obj.TOL.rho_plus = TOL.rho_plus;
                obj.TOL.rho_minus = TOL.rho_minus;
                obj.TOL.E_plus = TOL.E_plus;
                obj.TOL.E_minus = TOL.E_minus;
                obj.TOL.nu_plus = TOL.nu_plus;
                obj.TOL.nu_minus = TOL.nu_minus;
            end

            obj.Vfrac_initial = Vfrac_initial;
            obj.optimality_initial  = optimality_initial;
            obj.constr_initial = constr_initial;
            obj.optimality_final = optimality_final;
            obj.constr_final = constr_final;

            if exist('line_search_initiator','var')
                obj.line_search_initiator = line_search_initiator;
            else
                if strcmp(obj.optimizer,'PROJECTED GRADIENT')
                    obj.line_search_initiator = 'INCREASING LAST STEP';
                else
                    obj.line_search_initiator = 'STANDARD';
                end
            end

            if exist('constraint_case','var')
                obj.constraint_case = constraint_case;
            end

            if exist('plotting','var')
                obj.plotting = plotting;
            end
            if exist('showBC','var')
                obj.showBC = showBC;
            end
            if exist('BCscale_factor','var')
                obj.BCscale_factor = BCscale_factor;
            end
            if exist('rotation_per_it','var')
                obj.rotation_per_it = rotation_per_it;
            end
            if exist('printing','var')
                obj.printing = printing;
            end
            if exist('printing_physics','var')
                obj.printing_physics = printing_physics;
            end
            if ~obj.printing && obj.printing_physics
                warning('Physical variables will not be printed.')
            end
            if exist('monitoring','var')
                obj.monitoring = monitoring;
            end
            if exist('monitoring_interval','var')
                obj.monitoring_interval = monitoring_interval;
            end

%             if ~contains(case_file,'test','IgnoreCase',true)
%                 fprintf('Loaded %s: \n -Optimizer: %s \n -Cost: ',case_file,obj.optimizer)
%                 fprintf('%s, ',obj.cost{:})
%                 fprintf('\n -Constraint: ')
%                 fprintf('%s, ', obj.constraint{:})
%                 fprintf('\n -Incremental Steps: %f \n ',obj.nsteps)
%             end

            if exist('maxiter','var')
                obj.maxiter = maxiter;
                if ~contains(case_file,'test','IgnoreCase',true)
%                     fprintf('-Max iters: %f \n ',obj.maxiter)
                end
            end

            if exist('Vfrac_final','var')
                obj.Vfrac_final = Vfrac_final;
                if ~contains(case_file,'test','IgnoreCase',true)
%                     fprintf('-Volume target: %f \n ',obj.Vfrac_final)
                end
            end
            if exist('Perimeter_target','var')
                obj.Perimeter_target = Perimeter_target;
                if ~contains(case_file,'test','IgnoreCase',true)
%                     fprintf('-Perimeter target: %f \n',obj.Perimeter_target)
                end
            end
            if exist('epsilon_initial','var')
                obj.epsilon_initial = epsilon_initial;
                obj.epsilon_final = epsilon_final;
            end
            if exist('HJiter0','var')
                obj.HJiter0 = HJiter0;
            end
            if exist('e2','var')
                obj.e2 = e2;
            else
                obj.e2 = 1;
            end

            if exist('micro','var')
                obj.micro.alpha = micro.alpha;
                obj.micro.beta = micro.beta;
            end
            if exist('epsilon_isotropy_initial','var')
                obj.epsilon_isotropy_initial = epsilon_isotropy_initial;
                obj.epsilon_isotropy_final = epsilon_isotropy_final;
            end
            if exist('selectiveC_Cstar','var')
                obj.selectiveC_Cstar = selectiveC_Cstar;
            end

            if exist('widthH','var')
                obj.levelSetDataBase.widthH = widthH;
            end

            if exist('widthV','var')
                obj.levelSetDataBase.widthV = widthV;
            end

            if exist('widthSquare','var')
                obj.levelSetDataBase.widthSquare = widthSquare;
            end

            if exist('N_holes','var')
                obj.levelSetDataBase.nHoles = N_holes;
            end
            if exist('R_holes','var')
                obj.levelSetDataBase.rHoles = R_holes;
            end
            if exist('phase_holes','var')
                obj.levelSetDataBase.phaseHoles = phase_holes;
            end

            if exist('warningHoleBC','var')
                obj.levelSetDataBase.warningHoleBC = warningHoleBC;
            end

            if exist('fracRadius','var')
                obj.levelSetDataBase.fracRadius = fracRadius;
            end

            if exist('levFib','var')
                obj.levelSetDataBase.levFib = levFib;
            end

            if exist('yn','var')
                obj.levelSetDataBase.yn = yn;
            end

            if exist('levelFibers','var')
               obj.levelSetDataBase.levelFibers = levelFibers;
            end

            if exist('volumeFibers','var')
               obj.levelSetDataBase.volumeFibers = volumeFibers;
            end

            if exist('shFuncParamsName','var')
               obj.shFuncParamsName = shFuncParamsName;
            end

            if exist('designVariable','var')
               obj.designVariable = designVariable;
            end

            if exist('homegenizedVariablesComputer','var')
               obj.homegenizedVariablesComputer = homegenizedVariablesComputer;
            else
               obj.homegenizedVariablesComputer = 'ByInterpolation';

            end

            if exist('vademecumFileName','var')
                obj.vademecumFileName = vademecumFileName;
            end

            if  ~(contains(filename,'test','IgnoreCase',true) || contains(filename,'RVE') || obj.hasToAddSpaceBecauseOfIncremental())
                fprintf('\n')
            end

            if ~exist('optimizerUnconstrained','var')
                switch obj.optimizer
                    case {'SLERP','HAMILTON-JACOBI','PROJECTED GRADIENT'}
                        obj.optimizerUnconstrained = obj.optimizer;
                        obj.optimizer = 'AlternatingPrimalDual';
                end
            else
                obj.optimizerUnconstrained = optimizerUnconstrained;
            end

            if exist('rate','var')
                obj.rate = rate;
            else
                obj.rate = 0.5;
            end

            if exist('ub','var')
                obj.ub = ub;
            else
                obj.ub = 1;
            end

            if exist('lb','var')
                obj.lb = lb;
            else
                obj.lb = 0;
            end

            if exist('superEllipseRatio','var')
               obj.levelSetDataBase.vigdergauzDataBase.superEllipseRatio = superEllipseRatio;
            end

            if exist('volumeMicro','var')
               obj.levelSetDataBase.vigdergauzDataBase.volumeMicro = volumeMicro;
            end

            if exist('vigdergauzType','var')
               obj.levelSetDataBase.vigdergauzDataBase.type =  vigdergauzType;
            end

            if exist('vigdergauzStrainMacro','var')
               obj.levelSetDataBase.vigdergauzDataBase.strain =  vigdergauzStrainMacro;
               if exist('TOL','var')
                   obj.levelSetDataBase.vigdergauzDataBase.E1 = TOL.E_plus;
                   obj.levelSetDataBase.vigdergauzDataBase.E0 = TOL.E_minus;
                   obj.levelSetDataBase.vigdergauzDataBase.nu1 = TOL.nu_plus;
                   obj.levelSetDataBase.vigdergauzDataBase.nu0 = TOL.nu_minus;
               end
            end

            if exist('rho0','var')
                obj.rho0 = rho0;
            end

            if exist('m1','var')
               obj.m1 = m1;
            end

            if exist('m2','var')
               obj.m2 = m2;
            end

            if exist('alpha0','var')
               obj.alpha0 = alpha0;
            end

            if exist('stressNormExponent_initial','var')
                obj.stressNormExponent_initial = stressNormExponent_initial;
            end

            if exist('stressNormExponent_final','var')
                obj.stressNormExponent_final = stressNormExponent_final;
            end

        end

    end

    methods (Access = private)

        function itHas = hasToAddSpaceBecauseOfIncremental(obj)
            itHas = true;
            if ~isempty(obj.printIncrementalIter)
                itHas = obj.printIncrementalIter;
            end
        end

    end
end


function [rec,prec,ap] = VOCevaldet(VOCopts,id,cls,draw,display_progress)

if nargin < 5
    display_progress = true;
end

% load test set
[gtids,t]=textread(sprintf(VOCopts.imgsetpath,VOCopts.testset),'%s %d');

% load ground truth objects
tic;
npos=0;
gt(length(gtids))=struct('BB',[],'diff',[],'det',[]);
for i=1:length(gtids)
    % display progress
    if display_progress && toc>1
        fprintf('%s: pr: load: %d/%d\n',cls,i,length(gtids));
        drawnow;
        tic;
    end

    % read annotation
    rec=PASreadrecord(sprintf(VOCopts.annopath,gtids{i}));

    % extract objects of class
    clsinds=strmatch(cls,{rec.objects(:).class},'exact');
    gt(i).BB=cat(1,rec.objects(clsinds).bbox)';
    gt(i).diff=[rec.objects(clsinds).difficult];
    gt(i).det=false(length(clsinds),1);
    npos=npos+sum(~gt(i).diff);
end

% load results
[ids,confidence,b1,b2,b3,b4]=textread(sprintf(VOCopts.detrespath,id,cls),'%s %f %f %f %f %f');
BB=[b1 b2 b3 b4]';

% sort detections by decreasing confidence
[sc,si]=sort(-confidence);
ids=ids(si);
BB=BB(:,si);

% assign detections to ground truth objects
nd=length(confidence);
tp=zeros(nd,1);
fp=zeros(nd,1);
tic;
for d=1:nd
    % display progress
    if display_progress && toc>1
        fprintf('%s: pr: compute: %d/%d\n',cls,d,nd);
        drawnow;
        tic;
    end

    % find ground truth image
    i=strmatch(ids{d},gtids,'exact');
    if isempty(i)
        error('unrecognized image "%s"',ids{d});
    elseif length(i)>1
        error('multiple image "%s"',ids{d});
    end

    % assign detection to ground truth object if any
    bb=BB(:,d);
    ovmax=-inf;
    for j=1:size(gt(i).BB,2)
        bbgt=gt(i).BB(:,j);
        bi=[max(bb(1),bbgt(1)) ; max(bb(2),bbgt(2)) ; min(bb(3),bbgt(3)) ; min(bb(4),bbgt(4))];
        iw=bi(3)-bi(1)+1;
        ih=bi(4)-bi(2)+1;
        if iw>0 & ih>0
            % compute overlap as area of intersection / area of union
            ua=(bb(3)-bb(1)+1)*(bb(4)-bb(2)+1)+...
               (bbgt(3)-bbgt(1)+1)*(bbgt(4)-bbgt(2)+1)-...
               iw*ih;
            ov=iw*ih/ua;
            if ov>ovmax
                ovmax=ov;
                jmax=j;
            end
        end
    end
    % assign detection as true positive/don't care/false positive
    if ovmax>=VOCopts.minoverlap
        if ~gt(i).diff(jmax)
            if ~gt(i).det(jmax)
                tp(d)=1;            % true positive
		gt(i).det(jmax)=true;
            else
                fp(d)=1;            % false positive (multiple detection)
            end
        end
    else
        fp(d)=1;                    % false positive
    end
end

% compute precision/recall
fp=cumsum(fp);
tp=cumsum(tp);
rec=tp/npos;
prec=tp./(fp+tp);

% compute average precision

ap=0;
for t=0:0.1:1
    p=max(prec(rec>=t));
    if isempty(p)
        p=0;
    end
    ap=ap+p/11;
end

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


% gsn - training an GSN (stochastic backprop)
% Copyright (C) 2013 KyungHyun Cho
%
% 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
%
function [G] = gsn(G, patches, n_walkback, valid_patches, valid_portion);

if nargin < 3
    n_walkback = G.structure.n_layers * 2;
end

valid_err = 0;

if nargin < 4
    early_stop = 0;
    valid_patches = [];
    valid_portion = 0;
else
    early_stop = 1;
    valid_err = -Inf;
    valid_best_err = -Inf;
    valid_violate_cnt = 0;
end

actual_lrate = G.learning.lrate;

n_samples = size(patches, 1);

layers = G.structure.layers;
n_layers = length(layers);

if layers(1) ~= size(patches, 2)
    error('Data is not properly aligned');
end

minibatch_sz = G.learning.minibatch_sz;
n_minibatches = ceil(n_samples / minibatch_sz);

n_epochs = G.iteration.n_epochs;

momentum = G.learning.momentum;
weight_decay = G.learning.weight_decay;

biases_grad = cell(n_layers, 1);
W_grad = cell(n_layers, 1);
biases_grad_old = cell(n_layers, 1);
W_grad_old = cell(n_layers, 1);
for l = 1:n_layers
    biases_grad{l} = zeros(size(G.biases{l}))';
    if l < n_layers
        W_grad{l} = zeros(size(G.W{l}));
    end
    biases_grad_old{l} = zeros(size(G.biases{l}))';
    if l < n_layers
        W_grad_old{l} = zeros(size(G.W{l}));
    end
end

min_recon_error = Inf;
min_recon_error_update_idx = 0;
stopping = 0;

do_normalize = G.do_normalize;
do_normalize_std = G.do_normalize_std;

if G.data.binary == 0
    if do_normalize == 1
        % make it zero-mean
        patches_mean = mean(patches, 1);
        patches = bsxfun(@minus, patches, patches_mean);
    end

    if do_normalize_std ==1
        % make it unit-variance
        patches_std = std(patches, [], 1);
        patches = bsxfun(@rdivide, patches, patches_std);
    end
end

anneal_counter = 0;
actual_lrate0 = actual_lrate;

rerr_ma = 0;

% try
%     use_gpu = gpuDeviceCount;
% catch errgpu
%     use_gpu = false;
%     disp(['Could not use CUDA. Error: ' errgpu.identifier])
% end
use_gpu = 0;

% figure;

for step=1:n_epochs
    if G.verbose
        fprintf(2, 'Epoch %d/%d: ', step, n_epochs)
    end
    if use_gpu
        % push
        G = push_to_gpu (G);
    end

    for mb=1:n_minibatches
        G.iteration.n_updates = G.iteration.n_updates + 1;

        mb_start = (mb - 1) * minibatch_sz + 1;
        mb_end = min(mb * minibatch_sz, n_samples);

        % p_0
        v0 = patches(mb_start:mb_end, :);
        mb_sz = size(v0,1);

        if use_gpu > 0
            v0 = gpuArray(single(v0));
        end

        % add error
        v0_clean = v0;

        % forward pass
        h0 = cell(n_layers, n_walkback * 2 + 1);
        v1clean = cell(1, n_walkback * 2 + 1);
        occupied = zeros(n_layers, n_walkback * 2 + 1);

        h0{1,1} = v0;
        v1clean{1,1} = v0;
        occupied(1,1) = 1;

        for wi = 1:(n_walkback * 2 + 1)
            for l = 1:min(wi,n_layers)
                if sum(occupied(1, 2:end)) == n_walkback
                    break;
                end

                if l == 2
                    if G.data.binary == 0 && G.noise.level > 0
                        if use_gpu
                            h0{l-1,wi-1} = h0{l-1,wi-1} + G.noise.level * parallel.gpu.GPUArray.randn(size(h0{l-1,wi-1}));
                        else
                            h0{l-1,wi-1} = h0{l-1,wi-1} + G.noise.level * randn(size(h0{l-1,wi-1}));
                        end
                    end

                    if G.noise.drop > 0
                        mask = binornd(1, 1-G.noise.drop, size(h0{l-1,wi-1}));
                        if G.data.binary
                            sandp = binornd(1, 0.5, size(h0{l-1,wi-1}));
                        else
                            sandp = zeros(size(h0{l-1,wi-1}));
                        end
                        h0{l-1,wi-1} = h0{l-1,wi-1} .* mask + (1 - mask) .* sandp;
                        clear mask;
                    end
                end

                if l == 1 && wi == 1
                    continue;
                end

                prev_exist = 0;

                if l > 1 && occupied(l-1, wi-1) == 1
                    prev_exist = prev_exist + 1;
                end
                if l < n_layers && occupied(l+1, wi-1) == 1
                    prev_exist = prev_exist + 1;
                end

                if prev_exist == 0
                    continue;
                end

                occupied(l, wi) = 1;
                h0{l, wi} = zeros(mb_sz, G.structure.layers(l));
                if l > 1 && occupied(l-1, wi-1) == 1
                    h0{l, wi} = h0{l, wi} + h0{l-1, wi-1} * G.W{l-1};
                end
                if l < n_layers && occupied(l+1, wi-1) == 1
                    h0{l, wi} = h0{l, wi} + h0{l+1, wi-1} * G.W{l}';
                end
                h0{l, wi} = bsxfun(@plus, h0{l, wi}, G.biases{l}');

                % pre-sigmoid noise
                if l > 1
                    if G.hidden.add_noise(l)
                        if use_gpu
                            h0{l, wi} = h0{l, wi} + G.hidden.noise_level * parallel.gpu.GPUArray.randn(mb_sz, G.structure.layers(l));
                        else
                            h0{l, wi} = h0{l, wi} + G.hidden.noise_level * randn(mb_sz, G.structure.layers(l));
                        end
                    end
                end

                if l == 1
                    if G.data.binary
                        h0{l, wi} = sigmoid(h0{l, wi});
                    end
                    v1clean{l, wi} = h0{l, wi};
                else
                    h0{l, wi} = sigmoid(h0{l, wi}, G.hidden.use_tanh);
                end

                % FIXME: unable to train a GSN with post-sigmoid noise.
%                % post-sigmoid noise
%                if l > 1
%                    if G.hidden.add_noise(l)
%                        if use_gpu
%                            h0{l, wi} = h0{l, wi} + G.hidden.noise_level * parallel.gpu.GPUArray.randn(mb_sz, G.structure.layers(l));
%                        else
%                            h0{l, wi} = h0{l, wi} + G.hidden.noise_level * randn(mb_sz, G.structure.layers(l));
%                        end
%                    end
%                end
            end

            if sum(occupied(1, 2:end)) == n_walkback
                break;
            end
        end

        % reset gradients
        for l = 1:n_layers
            biases_grad{l} = 0 * biases_grad{l};
            if l < n_layers
                W_grad{l} = 0 * W_grad{l};
            end
        end

        % error backprop
        delta = cell(n_layers, n_walkback * 2 + 1);

        rerr = 0;
        for wi = 2:(n_walkback * 2 + 1)
            if occupied(1, wi)
                rerr = rerr + mean(sum((v1clean{1, wi} - v0_clean).^2,2));
            end
        end

        if use_gpu > 0
            rerr = gather(rerr);
        end

        rerr_ma = rerr * 0.1 + rerr_ma * 0.9;

        G.signals.recon_errors = [G.signals.recon_errors rerr];

        for wi = (n_walkback * 2 + 1):-1:1
            for l = 1:min(wi,n_layers)
                if occupied(l, wi) == 0
                    continue;
                end

                delta{l, wi} = zeros(mb_sz, G.structure.layers(l));

                if wi < (n_walkback * 2 + 1)
                    if l > 1 && occupied(l-1,wi+1) == 1
                        delta{l, wi} = delta{l, wi} + delta{l-1,wi+1} * G.W{l-1};
                    end
                    if l < n_layers && occupied(l+1,wi+1) == 1
                        delta{l, wi} = delta{l, wi} + delta{l+1,wi+1} * G.W{l}';
                    end
                end

                if l == 1
                    if G.data.binary && wi < (n_walkback * 2 + 1)
                        delta{l, wi} = delta{l, wi} .* dsigmoid(h0{l, wi});
                    end

                    delta{l, wi} = delta{l, wi} + (v1clean{l, wi} - v0_clean);
                    %delta{l, wi} = delta{l, wi} + (h0{l, wi} - v0_clean);
                else
                    delta{l, wi} = delta{l, wi} .* dsigmoid(h0{l, wi}, G.hidden.use_tanh);
                end

                if wi > 1
                    biases_grad{l} = biases_grad{l} + mean(delta{l, wi}, 1);
                    if l > 1 && occupied(l-1,wi-1) == 1
                        W_grad{l-1} = W_grad{l-1} + (h0{l-1, wi-1}' * delta{l, wi})/size(v0,1);
                    end
                    if l < n_layers && occupied(l+1,wi-1) == 1
                        W_grad{l} = W_grad{l} + (delta{l, wi}' * h0{l+1, wi-1})/size(v0,1);
                    end
                end
            end
        end

        % learning rate
        if G.adadelta.use
            % update
            for l = 1:n_layers
                biases_grad_old{l} = (1 - momentum) * biases_grad{l} + momentum * biases_grad_old{l};
                if l < n_layers
                    W_grad_old{l} = (1 - momentum) * W_grad{l} + momentum * W_grad_old{l};
                end
            end

            if G.iteration.n_updates == 1
                adamom = 0;
            else
                adamom = G.adadelta.momentum;
            end

            for l = 1:n_layers
                if l < n_layers
                    G.adadelta.gW{l} = adamom * G.adadelta.gW{l} + (1 - adamom) * W_grad_old{l}.^2;
                end

                G.adadelta.gbiases{l} = adamom * G.adadelta.gbiases{l} + (1 - adamom) * biases_grad_old{l}.^2';
            end

            for l = 1:n_layers
                dbias = -(biases_grad_old{l}' + ...
                    weight_decay * G.biases{l}) .* (sqrt(G.adadelta.biases{l} + G.adadelta.epsilon) ./ ...
                    sqrt(G.adadelta.gbiases{l} + G.adadelta.epsilon));
                G.biases{l} = G.biases{l} + dbias;

                G.adadelta.biases{l} = adamom * G.adadelta.biases{l} + (1 - adamom) * dbias.^2;
                clear dbias;

                if l < n_layers
                    dW = -(W_grad_old{l} + ...
                        weight_decay * G.W{l}) .* (sqrt(G.adadelta.W{l} + G.adadelta.epsilon) ./ ...
                        sqrt(G.adadelta.gW{l} + G.adadelta.epsilon));
                    G.W{l} = G.W{l} + dW;

                    G.adadelta.W{l} = adamom * G.adadelta.W{l} + (1 - adamom) * dW.^2;

                    clear dW;
                end

            end
        else
            if G.learning.lrate_anneal > 0 && (step >= G.learning.lrate_anneal * n_epochs)
                anneal_counter = anneal_counter + 1;
                actual_lrate = actual_lrate0 / anneal_counter;
            else
                if G.learning.lrate0 > 0
                    actual_lrate = G.learning.lrate / (1 + G.iteration.n_updates / G.learning.lrate0);
                else
                    actual_lrate = G.learning.lrate;
                end
                actual_lrate0 = actual_lrate;
            end

            G.signals.lrates = [G.signals.lrates actual_lrate];

            % update
            for l = 1:n_layers
                biases_grad_old{l} = (1 - momentum) * biases_grad{l} + momentum * biases_grad_old{l};
                if l < n_layers
                    W_grad_old{l} = (1 - momentum) * W_grad{l} + momentum * W_grad_old{l};
                end
            end

            for l = 1:n_layers
                G.biases{l} = G.biases{l} - actual_lrate * (biases_grad_old{l}' + weight_decay * G.biases{l});
                if l < n_layers
                    G.W{l} = G.W{l} - actual_lrate * (W_grad_old{l} + weight_decay * G.W{l});
                end
            end
        end

        if G.verbose == 1
            fprintf(2, '.%f.', rerr);
        end

        if use_gpu > 0
            clear v0 h0 h0d h0e v0_clean vr hr deltae deltad v1clean
        end

        if early_stop
            n_valid = size(valid_patches, 1);
            rndidx = randperm(n_valid);
            v0valid = gpuArray(single(valid_patches(rndidx(1:round(n_valid * valid_portion)),:)));
            valid_sz = size(v0valid, 1);

            valid0 = cell(n_layers, 1);
            valid0{1} = v0valid;
            for l = 2:n_layers
                valid0{l} = zeros(valid_sz, G.structure.layers(l));
            end

            valid0 = gsn_sample(G, valid0, 1);

            rerr = mean(sum((v0valid - valid0{1}).^2, 1));

            if use_gpu > 0
                rerr = gather(rerr);
            end

            G.signals.valid_errors = [G.signals.valid_errors rerr];

            if valid_err == -Inf
                valid_err = rerr;
                valid_best_err = rerr;
                valid_violate_cnt = 0;

                M_best = G;
                M_best = pull_from_gpu (M_best);
            else
                prev_err = valid_err;
                valid_err = 0.99 * valid_err + 0.01 * rerr;

                if step > G.valid_min_epochs
                    if (1.1 * valid_best_err) < valid_err
                        fprintf(2, 'Early-stop! %f, %f\n', valid_err, valid_best_err);
                        stopping = 1;
                        break;
                    end

                    if valid_best_err <= valid_err
                        valid_violate_cnt = valid_violate_cnt + 1;
                        if valid_violate_cnt > (n_minibatches * G.valid_min_epochs)
                            fprintf(2, 'Unable to improve! %f, %f\n', valid_err, valid_best_err);
                            stopping = 1;
                            break;
                        end
                    else
                        valid_violate_cnt = 0;
                    end

                end

                if valid_err < valid_best_err
                    valid_best_err = valid_err;

                    G_best = G;
                    G_best = pull_from_gpu (G_best);
                end
            end
        else
            if G.stop.criterion > 0
                if G.stop.criterion == 1
                    if min_recon_error > G.signals.recon_errors(end)
                        min_recon_error = G.signals.recon_errors(end);
                        min_recon_error_update_idx = G.iteration.n_updates;
                    else
                        if G.iteration.n_updates > min_recon_error_update_idx + G.stop.recon_error.tolerate_count
                            fprintf(2, '\nStopping criterion reached (recon error) %f > %f\n', ...
                                G.signals.recon_errors(end), min_recon_error);
                            stopping = 1;
                            break;
                        end
                    end
                else
                    error ('Unknown stopping criterion %d', G.stop.criterion);
                end
            end
        end

        if length(G.hook.per_update) > 1
            err = G.hook.per_update{1}(G, G.hook.per_update{2});

            if err == -1
                stopping = 1;
                break;
            end
        end

    end

    if use_gpu > 0
        % pull
        G = pull_from_gpu (G);
    end

    if length(G.hook.per_epoch) > 1
        err = G.hook.per_epoch{1}(G, G.hook.per_epoch{2});

        if err == -1
            stopping = 1;
        end
    end

    if stopping == 1
        break;
    end

    if G.verbose == 1
        fprintf(2, '\n');
    end

    fprintf(2, 'Epoch %d/%d - recon_error: %f(%f) valid_error: %f\n', step, n_epochs, rerr_ma, rerr, valid_err);
end

if use_gpu > 0
    % pull
    G = pull_from_gpu (G);
end

if early_stop
    G = G_best;
end

end

function [G] = push_to_gpu (G)
    n_layers = length(G.structure.layers);

    % push
    for l = 1:n_layers
        if l < n_layers
            G.W{l} = gpuArray(single(G.W{l}));
        end
        G.biases{l} = gpuArray(single(G.biases{l}));
    end

    if G.adadelta.use
        for l = 1:n_layers
            if l < n_layers
                G.adadelta.gW{l} = gpuArray(single(G.adadelta.gW{l}));
                G.adadelta.W{l} = gpuArray(single(G.adadelta.W{l}));
            end
            G.adadelta.gbiases{l} = gpuArray(single(G.adadelta.gbiases{l}));
            G.adadelta.biases{l} = gpuArray(single(G.adadelta.biases{l}));
        end
    end
end

function [G] = pull_from_gpu (G)
    n_layers = length(G.structure.layers);

    for l = 1:n_layers
        if l < n_layers
            G.W{l} = gather(G.W{l});
        end
        G.biases{l} = gather(G.biases{l});
    end

    if G.adadelta.use
        for l = 1:n_layers
            if l < n_layers
                G.adadelta.W{l} = gather(G.adadelta.W{l});
                G.adadelta.gW{l} = gather(G.adadelta.gW{l});
            end
            G.adadelta.biases{l} = gather(G.adadelta.biases{l});
            G.adadelta.gbiases{l} = gather(G.adadelta.gbiases{l});
        end
    end

end


function [c] = mrdivide(a,b)
%C=A./B
%   [C]=MRDIVIDE(A,B) Division of a TT-matrix A by number B
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
if isa(a,'tt_matrix') && numel(b) == 1
c=a*(1.0./b);
else
    error('Use mrdivide(full(A),full(B)).');
end


% *************************************************************************
% Video Super-Resolution with Convolutional Neural Networks
%
% If you use this code, please cite the following publication:
%     A.Kappeler, S.Yoo, Q.Dai, A.K.Katsaggelos, "Video Super-Resolution
%     with Convolutional Neural Networks", to appear in IEEE Transactions
%     on Computational Imaging
%
% Installation Instructions:
% 1) install Caffe from "http://caffe.berkeleyvision.org/"
% 2) run the following command in Matlab:
%       cd external_functions/CLG-TV-matlab
%       mex applyBilateralFilterToDataTerms.cpp
%       cd ../..
% 3) specify the CAFFEPATH on line 53 in VSRnet_demo.m
% 4) set the experiment you want to execute to "true" (only one at the time)
%
%
% Version 1.0
%
% Created by:   Armin Kappeler
% Date:         02/19/2016
%
% http://ivpl.eecs.northwestern.edu/software
%
% *************************************************************************
%
% Because of the data size, we only provide a subset of our videos on the
% website. For additional testvideos and our training database, please
% contact us directly.
%
% *************************************************************************
% Copyright (C) 2016 Armin Kappeler
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% For a copy of the GNU General Public License,
% please see <http://www.gnu.org/licenses/>.
%
% *************************************************************************

clearvars
%% GENERAL PARAMETERS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

CAFFEPATH = '/HOMES/baetz/develop/CAFFE/caffe/';   % path to caffe installation

LOW_MEMORY_MODE = 1;    % if Matlab crashes, try a higher number:
                        % 0 = high GPU memory usage -> fastest
                        % 1 = medium GPU memory usage -> fast
                        % 2 = low GPU memory usage -> slow

USE_GPU = false;         % set to false, if no GPU available -> slowest
GPU_ID = 0;             % GPU ID -> should normally be 0

%% RUN VSRnet on Video %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% specify TESTVIDEO_PATCH and UPSCALE_FACTOR and run
if true
    TESTVIDEO_PATH = 'data/lr_video/u2/foreman_LR.mat'; % path to input video frames
                                        % a variable "frames" with dimensions (width x height x nrFrames) is expected
    UPSCALE_FACTOR = 2;                 % upscale factor: 2,3,4 are available

    MOTIONCOMPENSATION = true;          % use Motion Compensation (much faster)
                                        % Note: MOTIONCOMPENSATION=false is only available for upscale factor 3
    ADAPTIVEMOTIONCOMPENSATION = false; % use Adaptive Motion Compensation
    TESTONLY1FRAME = 0;                 % 0: all frames will be tested, otherwise only one frame will be tested
    RESULTFILE_PATH = ['results/magi_quick_test'];% filename for results
    PREPROCESSED_INPUT = false;         % do not change
end

%% REPRODUCE RESULTS FROM PAPER %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% these experiments reproduce the results from the paper
% the testvideos here are already preprocessed
% do not change the parameters below this point

% Myanmar upscale factor 2
if false
    TESTVIDEO_PATH = 'data/preprocessed/u2/matTestimages_Myanmar.mat';
    UPSCALE_FACTOR = 2;

    MOTIONCOMPENSATION = true;
    ADAPTIVEMOTIONCOMPENSATION = false;
    TESTONLY1FRAME = 0;
    RESULTFILE_PATH = ['results/dummy'];
    PREPROCESSED_INPUT = true;
end

% Myanmar upscale factor 3
if false
    TESTVIDEO_PATH = 'data/preprocessed/u3/matTestimages_Myanmar.mat';
    UPSCALE_FACTOR = 3;

    MOTIONCOMPENSATION = true;
    ADAPTIVEMOTIONCOMPENSATION = false;
    TESTONLY1FRAME = 0;
    RESULTFILE_PATH = ['results/dummy'];
    PREPROCESSED_INPUT = true;
end

% Myanmar upscale factor 4
if false
    TESTVIDEO_PATH = 'data/preprocessed/u4/matTestimages_Myanmar.mat';
    UPSCALE_FACTOR = 4;

    MOTIONCOMPENSATION = true;
    ADAPTIVEMOTIONCOMPENSATION = false;
    TESTONLY1FRAME = 0;
    RESULTFILE_PATH = ['results/dummy'];
    PREPROCESSED_INPUT = true;
end

% Myanmar upscale factor 3, no motion compensation
if false
    TESTVIDEO_PATH = 'data/preprocessed/u3/matTestimages_Myanmar_noMotionCompensation.mat';
    UPSCALE_FACTOR = 3;

    MOTIONCOMPENSATION = false;
    ADAPTIVEMOTIONCOMPENSATION = false;
    TESTONLY1FRAME = 0;
    RESULTFILE_PATH = ['results/dummy'];
    PREPROCESSED_INPUT = true;
end

% Foreman, upscale factor 3, normal motion compensation (MC)
if false
    TESTVIDEO_PATH = 'data/preprocessed/u3/matTestimages_foreman.mat';
    UPSCALE_FACTOR = 3;

    MOTIONCOMPENSATION = true;
    ADAPTIVEMOTIONCOMPENSATION = false;
    TESTONLY1FRAME = 7;
    RESULTFILE_PATH = ['results/dummy'];
    PREPROCESSED_INPUT = true;
end

% Foreman, upscale factor 3, adaptive motion compensation (AMC)
if false
    TESTVIDEO_PATH = 'data/preprocessed/u3/matTestimages_foreman_adaptiveMotionCompensation.mat';
    UPSCALE_FACTOR = 3;

    MOTIONCOMPENSATION = true;
    ADAPTIVEMOTIONCOMPENSATION = true;
    TESTONLY1FRAME = 7;
    RESULTFILE_PATH = ['results/dummy'];
    PREPROCESSED_INPUT = true;
end

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

%% start processing
addpath('functions','external_functions/CLG-TV-matlab',[CAFFEPATH '/matlab'])
run('main_VSRnet_test.m')
run('evaluate_result.m')


function [bitstream,char_table]=make_bits(msg)
% make a test transmission from msg
% Copyright 2002-2010 The MathWorks, Inc.
[vctable char_table]=load_alpha;   % varicode table index to 1+abs('c')
bitstream = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];   % idle bits
for k=1:length(msg)
    index=1+abs(msg(k));        % get ascii index (interesting use of the abs() function)
    vc=char(vctable(index));    % look up varicode bits
    kof=length(bitstream);
    bitstream([kof+(1:2)])=[0,0]; % 2 consecutive zeros is a character break
    for i=1:length(vc)
        bitstream(kof+2+i)=str2num(vc(i));
    end;
end;


function demoImMaxGeodesicPath(varargin)
%DEMOIMGEODESICPATH  Demo for function imGeodesicPath
%
%   output = demoImGeodesicPath(input)
%
%   Example
%   demoImGeodesicPath
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-02-22,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% Initialisation de l'image a traiter

% lecture image
img = imread('rice.png');

% segmentation basique
img2 = img - imopen(img, ones(30, 30));
bin =img2 > 50;

% etiquetage
lbl = bwlabel(bin, 4);

figure;
imshow(label2rgb(lbl, 'jet', 'w'));

%% Calcule des chemins geodesiques maximaux

% affiche image
imshow(img);
hold on;

% pour chaque particule, calcule et affiche le chemin
tic;
for i=1:max(lbl(:))
    path = imMaxGeodesicPath(lbl == i);
    plot(path(:,1), path(:,2), 'color', 'r', 'linewidth', 2);
end

% affiche le temps ecoule
t = toc;
disp(sprintf('Temps ecoule : %f', t)); %#ok<DSPS>


%% (Internal) Reshape a matrix into a row vector
% Return x as a row vector. This function is useful when a function returns a
% column vector or matrix and you want to immediately reshape it in a functional
% way. Suppose f(a,b,c) returns a column vector, matlab will not let you write
% f(a,b,c)(:)' - you would have to first store the result. With this function you
% can write rowvec(f(a,b,c)) and be assured that the result is a row vector.
%
%    x = colvec(x)
%
% Arguments:
%
%      + x: data
%
% Output:
%
%      + x: columnized data
%
% Example:
%
% See also colvec
%
% This file is from pmtk3.googlecode.com
%
%
% 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.,
% 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
%
% Copyright 2008-2015
%
function x = rowvec(x)
    x = x(:)';
end


% ASL  Read an AMPL .NL file and convert to Matlab Values
%
%   NOTE - This API has substantially changed from OPTI v1.79
%
% THIS IS A LOW LEVEL FUNCTION - USE amplRead() INSTEAD!
%
% asl uses the Netlib AMPL Solver Library to open and parse the file. The
% MEX file is a little different in that it stays open after a call to
% asl(), in order to evaluate nonlinear functions later. You must use the
% close command in order to close the mex file.
%
%   [varargout] = asl(command,varargin)
%
%   Input arguments:
%       command - a string containing the command to execute (see below)
%       varargin - command specific arguments
%
%   Return arguments:
%       varagout - command specific return variables
%
%   Command: 'open' [Open an AMPL file and read basic information + check if LP/QP/QCQP]
%       [aslprob,sizes] = asl('open',path,isNLP)
%
%       path - full path to the file on your PC
%       isNLP - 1 to skip identification of LP/QP/QCQP (assume (MI)NLP)
%
%       aslprob structure fields:
%           H  - Quadratic Objetive H matrix (sparse, if QP)
%           f  - Linear Objective f vector (if LP/QP)
%           lb - Decision variable lower bounds
%           ub - Decision variable upper bounds
%           A  - Linear Constraints A matrix (sparse, if LP/QP)
%           cl - Constraints lower bounds
%           cu - Constraints upper bounds
%           Q  - Cell array of quadratic constraint Q matrices (sparse, if QCQP)
%           l  - Matrix of quadratic constraint linear vectors (if QCQP)
%           qcind - Vector of indices of Quadratic Constraints (to remove from A and cl,cu, if QCQP)
%           x0 - initial decision variable guess
%           v0 - initial dual variable guess
%           sense - 1: minimization, -1: maximization
%           conlin - vector of constraint linearity (0 constant, 1 linear, 2 quadratic, 3 general nonlinear)
%
%       sizes - vector of problem sizes (see amplRead.m)
%
%   Command: 'isopen' [Check if AMPL ASL interface has been opened]
%       asl('isopen')
%
%   Command: 'close' [Close an opened AMPL file and destory MEX file memory]
%       asl('close')
%
%   Command: 'fun' [Evaluate objective function at x]
%       f = asl('fun',x)
%
%   Command: 'grad' [Evaluate objective gradient at x]
%       g = asl('grad',x)
%
%   Command: 'con' [Evaluate constraint function at x]
%       c = asl('con',x)
%
%   Command: 'jac' [Evaluate constraint Jacobian at x]
%       j = asl('jac',x)
%       js = asl('jac',x,1) %return sparse Jacobian
%
%   Command: 'jacstr' [Evaluate constraint Jacobian Sparsity Structure]
%       s = asl('jacstr')
%
%   Command: 'hess' [Evaluate Hessian of the Lagrangian (L = sigma*Hobj + sum(v*Hcon))]
%       h = asl('hess',x,sigma,lambda)
%       hs = asl('hess',x,sigma,lambda,1) %return sparse Hessian
%
%   Command: 'hessstr' [Evaluate Hessian Sparsity Structure]
%       s = asl('hessstr')
%
%   Copyright (C) 2011-2013 Jonathan Currie (IPL)


function [Pro,FreeNodec] = transferP1red(elemc,elemf,FreeNode)
%% TRANSFERP2RED
%
%

if ~exist('FreeNode','var'), FreeNode = []; end

%% Data structure
N = max(elemc(:));
if exist('elemf','var')
    [elem,HB] = uniformcoarsenred(elemf);
else
    [elem2dof,edge] = dofP2(elemc);
    NE = size(edge,1);
    HB(:,[1 2 3]) = [(N+1:N+NE)', edge(:,1:2)];
end

%% Prolongation matrix
coarseNode = (1:N)';


function [pg, data, imsegs] = msTestImage(im, imsegs, classifiers, nsegments, normalize, ...
    smaps, spdata, adjlist, edata)
%  [pg, data, imsegs] = msTestImage(im, imsegs, classifiers, nsegments, smaps,
%  spdata, adjlist, edata)
%
% Computes the marginals of the labels for the given input image
% spdata, adjlist, edata are optional inputs
% Note: only first three arguments are required

% if imsegs is a cell, it contains the system command to make the
% segmentation image and the filename of the segmentation image

if iscell(imsegs)
    syscall = imsegs{1};
    outfn = imsegs{2};
    system(syscall);
    imsegs = processSuperpixelImage(outfn);
end

if ~exist('normalize', 'var') || isempty(normalize)
    normalize = 1;
end

labelclassifier = classifiers.labelclassifier;
segclassifier = classifiers.segclassifier;

imdata = mcmcComputeImageData(im, imsegs);

if ~exist('spdata', 'var') || isempty(spdata)
    spdata = mcmcGetSuperpixelData(im, imsegs);
end

if ~exist('smaps', 'var') || isempty(smaps)

    if ~exist('adjlist', 'var') || ~exist('edata', 'var') || isempty(adjlist) || isempty(edata)
        [edata, adjlist] = mcmcGetEdgeData(imsegs, spdata);
    end

    eclassifier = classifiers.eclassifier;
    ecal = classifiers.ecal;
    if isfield(classifiers, 'vclassifierSP')
        vclassifierSP = classifiers.vclassifierSP;
        hclassifierSP = classifiers.hclassifierSP;
        [pvSP, phSP, pE] = mcmcInitialize(spdata, edata, ...
            adjlist, imsegs, vclassifierSP, hclassifierSP, eclassifier, ecal, 'none');
    else
        pE = test_boosted_dt_mc(eclassifier, edata);
        pE = 1 ./ (1+exp(ecal(1)*pE+ecal(2)));
    end
    smaps = generateMultipleSegmentations2(pE, adjlist, imsegs.nseg, nsegments);
end

labdata = cell(1, size(smaps, 2));
for k = 1:size(smaps, 2)
    labdata{1, k} = mcmcGetSegmentFeatures(imsegs, spdata, imdata, smaps(:, k), (1:max(smaps(:, k))));
end

pg = msTest(imsegs, labdata, {smaps}, labelclassifier, segclassifier, normalize);

% nsp = imsegs.nseg;
%
% nclasses = size(labelclassifier.wcs, 2);
% pg = zeros(nsp, nclasses);
%
% segs = cell(nsp, 1);
%
% for k = 1:size(smaps, 2)
%
%     for s = 1:max(smaps(:, k))
%
%         [segs, ind] = checksegs(segs, smaps(:, k), s);
%
%         if ~isempty(ind)
%
%             labdata = mcmcGetSegmentFeatures(imsegs, spdata, imdata, smaps(:, k), s);
%
%             vconf = test_boosted_dt_mc(vclassifier, labdata);
%             vconf = 1 ./ (1+exp(-vconf));
%             vconf = vconf / sum(vconf);
%
%             hconf = test_boosted_dt_mc(hclassifier, labdata);
%             hconf = 1 ./ (1+exp(-hconf));
%             hconf = hconf / sum(hconf);
%
%             sconf = test_boosted_dt_mc(sclassifier, labdata);
%             sconf = 1 ./ (1+exp(-sconf));
%
%             pgs = [vconf(1) vconf(2)*hconf vconf(3)]*sconf;
%
%             pg(ind, :) = pg(ind, :) + repmat(pgs, numel(ind), 1);
%         end
%
%     end
%
% end
%
% pg = pg ./ max(repmat(sum(pg, 2), 1, size(pg, 2)), 0.00001);
pg = pg{1};

data.smaps = smaps;
data.edata = edata;
data.adjlist = adjlist;
data.spdata = spdata;
data.imdata = imdata;

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

function [segs, ind] = checksegs(segs, map, s)
% Checks whether this segment has been seen before

ind = find(map==s);

if isempty(ind)
    return;
end

% if numel(ind)==1 % already accounted for by superpixels
%     ind = [];
%     return;
% end

oldsegs = segs{ind(1)};

for k = 1:numel(oldsegs)
    if (numel(oldsegs{k})==numel(ind)) && all(oldsegs{k}==ind)
        ind = [];
        return;
    end
end

segs{ind(1)}{end+1} = ind;