function test_suite = test_surficial_neighborhood
% tests for cosmo_surficial_neighborhood
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite;

function test_surficial_neighborhood_surface_dijkstra
    if cosmo_skip_test_if_no_external('surfing')
        return;
    end
    warning_state=warning();
    warning_resetter=onCleanup(@()warning(warning_state));
    warning('off');

    opt=struct();
    opt.progress=false;

    ds=cosmo_synthetic_dataset('type','surface');

    [vertices,faces]=get_synthetic_surface();

    % dijkstra neighborhood fixed number of voxels
    args={{vertices,faces},'count',4,'metric','dijkstra',opt};
    nh1=cosmo_surficial_neighborhood(ds,args{:});
    assertFalse(isfield(nh1.a,'vol'));
    assert_equal_cell(nh1.neighbors,{ [ 1 2 4 3 ]
                                        [ 2 1 3 5 ]
                                        [ 3 2 6 5 ]
                                        [ 4 1 5 2 ]
                                        [ 5 2 4 6 ]
                                        [ 6 3 5 2 ] });

    assertEqual(nh1.fa.radius,[2 1 sqrt(2) sqrt(2) 1 2]);
    assertEqual(nh1.fa.node_indices,1:6);
    check_area(vertices,faces,nh1);

    args={{vertices,faces},'radius',2.5,'metric','dijkstra',opt};
    nh2=cosmo_surficial_neighborhood(ds,args{:});

    assert_equal_cell(nh2.neighbors,{[1 2 3 4 5];
                                     [1 2 3 4 5 6];
                                     [1 2 3 4 5 6];
                                     [1 2 3 4 5 6];
                                     [1 2 3 4 5 6];
                                     [2 3 4 5 6]; });
    assertEqual(nh2.fa.radius,[2,2,1+sqrt(2),1+sqrt(2),2,2]);
    assertEqual(nh2.fa.node_indices,1:6);
    check_partial_neighborhood(ds,nh2,args);
    check_area(vertices,faces,nh2);

    args{1}{1}([2,6],:)=NaN;

    nh3=cosmo_surficial_neighborhood(ds,args{:});
    assert_equal_cell(nh3.neighbors,{[1 4 5];
                                     [];
                                     [3 4 5];
                                     [1 3 4 5];
                                     [1 3 4 5];
                                     []; });
    assertEqual(nh3.fa.radius,[2,NaN,1+sqrt(2),1+sqrt(2),2,NaN]);
    check_partial_neighborhood(ds,nh3,args);
    check_area(args{1}{:},nh3);

    args{2}='count';
    args{3}=3;

    nh4=cosmo_surficial_neighborhood(ds,args{:});
    assert_equal_cell(nh4.neighbors,{[1 4 5];
                                     [];
                                     [3 5 4];
                                     [4 1 5];
                                     [5 4 3];
                                     []; });
    assertEqual(nh4.fa.radius,[2,NaN,1+sqrt(2),1,sqrt(2),NaN]);
    check_area(args{1}{:},nh4);

    args{1}{1}=vertices;
    args{1}{1}([2,5],:)=NaN; % split in two surfaces
    args{3}=2;
    nh5=cosmo_surficial_neighborhood(ds,args{:});
    assert_equal_cell(nh5.neighbors,{[1 4];
                                     [];
                                     [3 6];
                                     [4 1];
                                     [];
                                     [6 3]; });
    assertEqual(nh5.fa.radius,[1,NaN,1,1,NaN,1]);
    check_area(args{1}{:},nh5);

    % throw error when too many nodes asked for
    aet=@(varargin)assertExceptionThrown(@()...
                    cosmo_surficial_neighborhood(varargin{:}),'');
    args{2}='count';
    args{3}=3;

    aet(ds,args{:})

function test_surficial_neighborhood_surface_direct
    if cosmo_skip_test_if_no_external('surfing')
        return;
    end
    warning_state=warning();
    warning_resetter=onCleanup(@()warning(warning_state));
    warning('off');

    opt=struct();
    opt.progress=false;

    ds=cosmo_synthetic_dataset('type','surface');

    [vertices,faces]=get_synthetic_surface();

    % direct neighborhood
    args={{vertices,faces},'direct',true,opt};
    nh3=cosmo_surficial_neighborhood(ds,args{:});
    assert_equal_cell(nh3.neighbors,{ [ 1 2 4 ]
                                        [ 2 3 1 4 5 ]
                                        [ 3 2 5 6 ]
                                        [ 4 1 2 5 ]
                                        [ 5 3 6 2 4 ]
                                        [ 6 3 5 ] });
    assertElementsAlmostEqual(nh3.fa.radius,sqrt([1 2 2 2 2 1]));
    check_partial_neighborhood(ds,nh3,args);

    args{1}{1}([2 5],:)=NaN;
    nh4=cosmo_surficial_neighborhood(ds,args{:});
    assert_equal_cell(nh4.neighbors,{ [ 1 4 ]
                                        []
                                        [ 3 6 ]
                                        [ 1 4 ]
                                        []
                                        [6 3] });

    check_partial_neighborhood(ds,nh4,args);

function test_surficial_neighborhood_surface_geodesic
    if cosmo_skip_test_if_no_external('fast_marching') || ...
            cosmo_skip_test_if_no_external('surfing')
        return;
    end

    warning_state=warning();
    warning_resetter=onCleanup(@()warning(warning_state));
    warning('off');

    opt=struct();
    opt.progress=false;

    ds=cosmo_synthetic_dataset('type','surface');%,'size','normal');

    [vertices,faces]=get_synthetic_surface();

    args={{vertices,faces},'count',4,opt};
    nh=cosmo_surficial_neighborhood(ds,args{:});
    assert_equal_cell(nh.neighbors,{[ 1 2 4 5 ]
                                    [ 2 1 3 5 ]
                                    [ 3 2 6 5 ]
                                    [ 4 1 5 2 ]
                                    [ 5 2 4 6 ]
                                    [ 6 3 5 2 ] });
    assertEqual(nh.fa.node_indices,1:6);
    assertEqual(nh.fa.radius,[sqrt(.5)+1 1 sqrt(2) sqrt(2) 1 sqrt(.5)+1]);

    vertices2=[NaN NaN NaN; vertices;NaN NaN NaN];
    faces2=[faces+1; 1 1 8];
    args={{vertices2,faces2},'count',4,opt};
    nh2=cosmo_surficial_neighborhood(ds,args{:});

    assertEqual(nh2.neighbors,{ zeros(1,0)
                                 [ 2 3 5 6 ]
                                 [ 3 2 4 6 ]
                                 [ 4 3 6 2 ]
                                 [ 5 2 6 3 ]
                                 [ 6 3 5 4 ] });

function test_surficial_neighborhood_volume_geodesic
    if cosmo_skip_test_if_no_external('fast_marching') || ...
            cosmo_skip_test_if_no_external('surfing')
        return;
    end
    warning_state=warning();
    warning_resetter=onCleanup(@()warning(warning_state));
    warning('off');

    opt=struct();
    opt.progress=false;

    ds=cosmo_synthetic_dataset();
    vertices=[-2 0 2 -2 0 2;...
                -1 -1 -1 1 1 1;...
                -1 -1 -1 -1 -1 -1]';
    faces= [ 3 2 3 2;...
                2 1 5 4;...
                5 4 6 5 ]';

    pial=vertices;
    pial(:,3)=pial(:,3)+1;
    white=vertices;
    white(:,3)=white(:,3)-1;
    nh1=cosmo_surficial_neighborhood(ds,{vertices,[-1 1],faces},...
                                    'count',4,opt);
    nh2=cosmo_surficial_neighborhood(ds,{pial,white,faces},...
                                    'count',4,opt);
    assert_equal_cell(nh1.neighbors,{[ 1 2 4 5 ]
                                        [ 1 2 3 5 ]
                                        [ 2 3 5 6 ]
                                        [ 4 1 5 2 ]
                                        [ 5 2 4 6 ]
                                        [ 6 3 5 2 ] });
    assertEqual(nh1.fa.node_indices,1:6);
    assertEqual(nh1,nh2);

function test_surficial_neighborhood_volume_dijkstra
    if cosmo_skip_test_if_no_external('surfing')
        return;
    end
    warning_state=warning();
    warning_resetter=onCleanup(@()warning(warning_state));
    warning('off');

    opt=struct();
    opt.progress=false;

    ds=cosmo_synthetic_dataset();
    vertices=[-2 0 2 -2 0 2;
                -1 -1 -1 1 1 1
                -1 -1 -1 -1 -1 -1]';
    faces= [ 3 2 3 2
                2 1 5 4
                5 4 6 5 ]';

    pial=vertices;
    pial(:,3)=pial(:,3)+1;
    white=vertices;
    white(:,3)=white(:,3)-1;

    args3={{vertices,[-1 1],faces},'metric','dijkstra','count',4,opt};
    args4={{pial,white,faces},'metric','dijkstra','count',4,opt};
    nh3=cosmo_surficial_neighborhood(ds,args3{:});
    nh4=cosmo_surficial_neighborhood(ds,args4{:});
    assert_equal_cell(nh3.neighbors,{ [ 1 2 4 3 ]
                                        [ 2 1 3 5 ]
                                        [ 3 2 6 5 ]
                                        [ 4 1 5 2 ]
                                        [ 5 2 4 6 ]
                                        [ 6 3 5 2 ] });
    assertEqual(nh3.fa.node_indices,1:6);
    assert_equal_cell(nh4.neighbors,nh3.neighbors);
    assertFalse(isfield(nh3.a,'vol'));

    % TODO
    % check_partial_neighborhood(ds,nh3,args3);
    % check_partial_neighborhood(ds,nh3,args3);

function test_surficial_neighborhood_exceptions
    if cosmo_skip_test_if_no_external('surfing')
        return
    end
    ds=cosmo_synthetic_dataset('type','surface');%,'size','normal');
    [vertices,faces]=get_synthetic_surface();

    aet=@(varargin)assertExceptionThrown(@()...
                    cosmo_surficial_neighborhood(varargin{:},...
                                        'progress',false),'');
    aet(ds,{vertices,faces});

    % need surfaces
    aet(ds,{},'radius',2);

    % center_ids not supported for surface dataset
    aet(ds,{vertices,faces},'radius',2,'center_ids',1);

    % cannot have duplicate feature ids
    ds_double=cosmo_stack({ds,ds},2);
    ds_double.a.fdim.values{1}=[1:6,1:6];
    aet(ds_double,{vertices,faces},'radius',2);

    % outside range
    ds_double.a.fdim.values{1}=[1:5,1:5];
    aet(ds_double,{vertices,faces},'radius',2);

    % cannot have fmri and surface dataset combined
    ds2=cosmo_synthetic_dataset();
    ds2.a.fdim.values{end+1}=ds.a.fdim.values{1};
    ds2.a.fdim.labels{end+1}=ds.a.fdim.labels{1};
    ds2.fa.node_indices=ds.fa.node_indices;
    aet(ds2,{vertices,faces},'radius',2);

    % cannot have MEEG dataset
    ds_meeg=cosmo_synthetic_dataset('type','meeg');
    aet(ds_meeg,{vertices,faces},'radius',2);

    % need positive scalar radius
    aet(ds,{vertices,faces},'radius',-1);
    aet(ds,{vertices,faces},'radius',eye(2));

function check_partial_neighborhood(ds,nh,args)
    % see if when have a partial dataset, the neighborbood reflects
    % that too

    nf=size(ds.samples,2);

    rp=randperm(nf);
    keep_count=round(nf*.7);
    keep_sel=rp(1:keep_count);
    keep_all=[keep_sel keep_sel keep_sel];

    ds_sel=cosmo_slice(ds,keep_all,2);

    fdim=ds_sel.a.fdim.values{1};
    rp_fdim=randperm(numel(fdim));
    ds_sel.a.fdim.values{1}=fdim(rp_fdim);

    nh_sel=cosmo_surficial_neighborhood(ds_sel,args{:});

    assertEqual(numel(nh_sel.neighbors), numel(keep_sel));

    assertEqual(nh_sel.a.fdim.labels,nh.a.fdim.labels);
    assertEqual(nh_sel.a.fdim.values{1},nh.a.fdim.values{1}(rp_fdim));
    assertEqual(numel(nh_sel.a.fdim.values),numel(nh.a.fdim.values));

    assertEqual(ds_sel.a,nh_sel.a);

    opt=cosmo_structjoin(args(2:end));

    if isfield(opt,'radius')
        metric=opt.metric;
        metric_arg=opt.radius;
    elseif isfield(opt,'count')
        metric=opt.metric;
        metric_arg=[10 opt.count];
    elseif isfield(opt,'direct')
        metric='direct';
        if opt.direct
            metric_arg=NaN;
        else
            metric_arg=0;
        end
    else
        assert(false);
    end

    faces=args{1}{2};
    n2f=surfing_invertmapping(faces);

    nodes_ds_sel=ds_sel.a.fdim.values{1}(ds_sel.fa.node_indices);
    nodes_ds=ds.a.fdim.values{1}(ds.fa.node_indices);

    nodes_nh_sel=nh_sel.a.fdim.values{1}(nh_sel.fa.node_indices);

    vertices=args{1}{1};

    nvertices=size(vertices,1);
    nodes_kept=cosmo_match(1:nvertices,nodes_ds_sel);
    vertices(~nodes_kept,:)=NaN;

    node_mask=all(isfinite(vertices),2);

    nodes_removed=setdiff(nodes_ds(:)',nodes_ds_sel(:)');
    assertEqual(setxor(nodes_removed,nodes_ds_sel),1:nf);

    nb_sel=nh_sel.neighbors;
    for k=1:numel(nh_sel.neighbors)
        sel_center_node=nodes_nh_sel(k);
        idx=find(nodes_ds==sel_center_node);
        assert(numel(idx)==1);
        center_node=nodes_ds(idx);

        assertEqual(sel_center_node, center_node);

        switch metric
            case 'direct'
                if node_mask(sel_center_node)
                    direct_neighbors=surfing_surface_nbrs(faces',...
                                                            vertices');
                    around_nodes=direct_neighbors(sel_center_node,:);
                    msk=cosmo_match(around_nodes, ...
                                            find(isfinite(vertices(:,1))));
                    % add node itself
                    around_nodes=[sel_center_node,...
                                    around_nodes(msk & around_nodes>0)];
                else
                    around_nodes=[];
                end
            otherwise
                around_nodes=surfing_circleROI(vertices',faces',...
                            sel_center_node,metric_arg,metric,n2f);
        end

        sel_around_nodes=nodes_ds_sel(nb_sel{k});

        if isempty(sel_around_nodes)
            assertTrue(isempty(around_nodes));
        else
            assertEqual(unique(sel_around_nodes),...
                        setdiff(around_nodes, nodes_removed))
        end
    end

function test_surface_subsampling
    if cosmo_skip_test_if_no_external('surfing')
        return;
    end
    vertices=[0 -1 -2 -1  1  2  1  3  4  3;
          0 -2  0  2  2  0 -2  2  0 -2;
          0  0  0  0  0  0  0  0  0  0]';

    faces=[1 1 1 1 1 1 5 8 6  6;
           2 3 4 5 6 7 8 9 9 10;
           3 4 5 6 7 2 6 6 10 7]';

    % make custom volume
    ds=cosmo_synthetic_dataset('size','small');
    cp=cosmo_cartprod({1:7,1:3})';
    ds.fa.i=cp(1,:);
    ds.fa.j=cp(2,:);
    ds.fa.k=ds.fa.i*0+1;

    ds.a.fdim.values={1:7;1:3;1};
    ds.samples=zeros(numel(ds.sa.targets),numel(ds.fa.i));
    ds.a.vol.dim=cellfun(@numel,ds.a.fdim.values)';
    ds.a.vol.mat(2,4)=-4;
    ds.a.vol.mat(3,4)=-1;
    ds.a.vol.mat(1,1)=1;
    ds.a.vol.mat(2,2)=2;
    ds.a.vol.mat(3,3)=1;

    surfs={vertices,faces,[-4 5]};

    opt=struct();
    opt.progress=false;
    opt.radius=3;
    opt.metric='euclidean';
    nh=cosmo_surficial_neighborhood(ds,surfs,opt);

    assertEqual(nh.neighbors,{ [ 2 4 8 10 12 16 18 ]
                             [ 2 4 8 10 ]
                             [ 2 8 10 16 ]
                             [ 8 10 16 18 ]
                             [ 10 12 16 18 20 ]
                             [ 4 6 10 12 14 18 20 ]
                             [ 2 4 6 10 12 ]
                             [ 12 14 18 20 ]
                             [ 6 12 14 20 ]
                             [ 4 6 12 14 ] });
    assertEqual(nh.origin.fa,ds.fa);
    assertEqual(nh.origin.a,ds.a);

    % test subsampling
    subsample=2;
    surfs={vertices,faces,[-4 5],subsample};
    nh2=cosmo_surficial_neighborhood(ds,surfs,opt);
    assertEqual(nh2.neighbors,{ [ 2 4 8 10 ]
                                 [ 2 8 10 16 ]
                                 [ 8 10 16 18 ]
                                 [ 10 12 16 18 20 ]
                                 [ 2 4 6 10 12 ]
                                 [ 12 14 18 20 ]
                                 [ 6 12 14 20 ]
                                 [ 4 6 12 14 ] });

    assertEqual(nh2.origin.fa,ds.fa);
    assertEqual(nh2.origin.a,ds.a);

    % subsampling with pial surface
    pial=bsxfun(@plus,vertices,[0 0 1]);
    white=bsxfun(@plus,vertices,[0 0 -1]);
    [vo,fo]=surfing_subsample_surface(vertices,faces,2,.2,0);
    surfs={pial,white,faces,vo,fo};
    nh3=cosmo_surficial_neighborhood(ds,surfs,opt);
    assertEqual(nh2,nh3);

    % check center ids options
    slice_ids=[5 3 2];
    nh4=cosmo_surficial_neighborhood(ds,surfs,opt,'center_ids',slice_ids);
    nh4_sl=struct();
    nh4_sl.neighbors=nh3.neighbors(slice_ids);
    nh4_sl.fa=cosmo_slice(nh3.fa,slice_ids,2,'struct');
    nh4_sl.a=nh3.a;

    assertEqual(nh4.a.fdim.values{1}(nh4.fa.node_indices),...
                    nh4_sl.a.fdim.values{1}(nh4_sl.fa.node_indices));
    assertEqual(nh4.neighbors,nh4_sl.neighbors);

    % try with file names
    fn_pial=cosmo_make_temp_filename('pial','.asc');
    fn_white=cosmo_make_temp_filename('white','.asc');
    fn_tiny=cosmo_make_temp_filename('tiny','.asc');

    cleaner1=onCleanup(@()delete(fn_pial));
    cleaner2=onCleanup(@()delete(fn_white));
    cleaner3=onCleanup(@()delete(fn_tiny));

    surfing_write(fn_pial, pial, faces);
    surfing_write(fn_white, white, faces);
    surfing_write(fn_tiny, vo, fo);

    surfs={fn_pial,fn_white,fn_tiny};
    nh5=cosmo_surficial_neighborhood(ds,surfs,opt);
    assertEqual(nh2,nh5);

    % should work with alternative voldef
    ds_bad_vol=ds;
    ds_bad_vol.a.vol.mat(:)=NaN;
    ds_bad_vol.a.vol.dim(:)=NaN;
    nh6=cosmo_surficial_neighborhood(ds_bad_vol,surfs,opt,...
                                            'vol_def',ds.a.vol);
    nh6.origin.a.vol=ds.a.vol;
    assertEqual(nh5,nh6);

    % check exceptions
    aet=@(varargin)assertExceptionThrown(@()...
                    cosmo_surficial_neighborhood(varargin{:}),'');

    surfs={fn_pial,fn_pial,fn_tiny};
    aet(ds,surfs,opt);

    white_bad=white;
    white_bad=white_bad(2:end,:);
    aet(ds,{pial,white_bad,faces},opt);

    % missing faces for output surface
    aet(ds,{fn_pial,fn_white,vo},opt);

    % face mismatch
    faces_bad=faces;
    faces_bad=faces_bad(end:-1:1,:);

    surfing_write(fn_white, white, faces_bad);
    aet(ds,{fn_pial,fn_white},opt);

    % too many surf arguments
    aet(ds,{fn_pial,fn_white,fn_tiny,fn_tiny},opt);
    aet(ds,{pial,white,faces,pial,white,white},opt);
    aet(ds,{pial,white,faces,fn_pial,white},opt);

    % surfs are not a cell
    aet(ds,struct,opt);
    aet(ds,{pial,white,{}});

function check_area(vertices,faces,nh)
    assert(isfield(nh.fa,'area'));
    area=surfing_surfacearea(vertices,faces);

    node_idxs=nh.fa.node_indices;
    n_nodes=numel(node_idxs);
    for k=1:n_nodes
        nbr_idxs=node_idxs(nh.neighbors{k});
        expected_area=sum(area(nbr_idxs));
        if isnan(expected_area)
            assertEqual(expected_area,nh.fa.area(k))
        else
            assertElementsAlmostEqual(expected_area,nh.fa.area(k));
        end
    end

function [vertices,faces]=get_synthetic_surface()
    % return the following surface (face indices in [brackets])
    %
    %  1-----2-----3
    %  |    /|    /|
    %  |[2]/ |[1]/ |
    %  |  /  |  /  |
    %  | /[4]| /[3]|
    %  |/    |/    |
    %  4-----5-----6

    vertices=[0 0 0 1 1 1;
                1 2 3 1 2 3;
                0 0 0 0 0 0]';
    faces= [ 3 2 3 2
                2 1 5 4
                5 4 6 5 ]';

function assert_equal_cell(x,y)
    % small helper
    assertEqual(size(x),size(y))
    for k=1:numel(x)
        xk=x{k};
        yk=y{k};
        if isempty(xk)
            assertTrue(isempty(yk));
        else
            assertEqual(sort(xk),sort(yk));
        end
    end


classdef ZCR < Algorithm

    methods (Access = public)

        function obj = ZCR()
            obj.name = 'ZCR';
            obj.inputPort = DataType.kSignal;
            obj.outputPort = DataType.kFeature;
        end

        function result = compute(~,signal)
            N = length(signal);
            result = sum(abs(diff(signal>0))) / N;
        end

        function metrics = computeMetrics(~,input)
            n = size(input,1);
            flops = 5 * n;
            memory = 1;
            outputSize = Constants.kFeatureBytes;
            metrics = Metric(flops,memory,outputSize);
        end
    end
end


function []=olc1()
n1=128;
K=zeros(n1);
for i=1:n1
    for j=i:n1
        K(i,j)=1;
    end
end
for i=2:n1
    K(i,i-1)=(-n1)+(i-1);
end

hgot=getappdata(0,'H_pass');
k1=char(hgot);
msgbox('Click on the folder saved by the name Test S');
pause(2);
TP = uigetdir('C:', 'Select path where Query image is to be searched');
%TP=char(TP);
l3=strcat(TP,'\',k1,'.pgm');
J=imread(l3);
J=imresize(J,[128 128]);
I=zeros(n1);
for i=1:n1
    L=J(:,i);
    L=double(L);
    M=K*L;
    I(:,i)=M;
end
A=K*I*K';
A=mat2gray(A,[0 63]);
%figure, imshow(A);
Y=reshape(A,n1*n1,1);
M=zeros(n1,1);
I1=zeros(n1);
nr=zeros(60,2);
msgbox('Click on the folder saved by the name S');
pause(2);
l32 = uigetdir('C:', 'Select path where Database image is to be searched');
for w=1:100
    w1=num2str(w);
    w2=strcat(l32,'\',w1,'.pgm');
S=imread(w2);
S=imresize(S,[128 128]);

for j=1:n1
    L1=S(:,j);
    L1=double(L1);
    M=K*L1;
    I1(:,j)=M;
end

B=K*I1*K';
B=mat2gray(B,[0 63]);
Z=reshape(B,n1*n1,1);
T=norm(Y-Z);

nr(w,1)=T;
nr(w,2)=w;
end
 [m q]=size(nr);
pa=m+1;
for i1=2:pa
    for i2=(pa-1):-1:i1
     if (nr(i2-1,1)>nr(i2,1))
         l1=nr(i2-1,1);
         l2=nr(i2-1,2);
         nr(i2-1,1)=nr(i2,1);
         nr(i2-1,2)=nr(i2,2);
         nr(i2,1)=l1;
         nr(i2,2)=l2;
     end
    end
end
 figure;
 subplot(1,2,1); imshow(l3); title('Provided Image');

 for i7=1:m
    n12=i7;
    j=nr(i7,2);
    j=num2str(j);
    str=strcat(l32,'\',j,'.pgm');
    I=imread(str);
    %b1=num2str(i7);
    str1=strcat('Image recognized From Database');
    subplot(1,2,2);imshow(I);title(str1);% Displaays Similar images
    if (n12>1)
        break;
    end
 end
return


%This Matlab script can be used to reproduce Figure 5.13 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Load SE simulation data, generated using the code from Section 4
load section5_Mvarying_K10_20;

%Number of UEs per BS
k_index = 2; %Selecting K = 10 from the loaded SE results
K = Krange(k_index);

%Fractions of data samples used for UL and DL
ULfraction = 1/3;
DLfraction = 2/3;

%Compute joint UL/DL sum SE using the fractions of UL/DL data
sumSE_MMMSE = ULfraction*sumSE_MMMSE_UL(:,k_index) + DLfraction*sumSE_MMMSE_DL(:,k_index);
sumSE_SMMSE = ULfraction*sumSE_SMMSE_UL(:,k_index) + DLfraction*sumSE_SMMSE_DL(:,k_index);
sumSE_RZF = ULfraction*sumSE_RZF_UL(:,k_index) + DLfraction*sumSE_RZF_DL(:,k_index);
sumSE_ZF = ULfraction*sumSE_ZF_UL(:,k_index) + DLfraction*sumSE_ZF_DL(:,k_index);
sumSE_MR = ULfraction*sumSE_MR_UL(:,k_index) + DLfraction*sumSE_MR_DL(:,k_index);

%Number of BSs
L = 16;

%Communication bandwidth
B = 20e6;

%PA efficiency UEs and BSs
mu_UE = 0.4;
mu_BS = 0.5;

%Define the pilot reuse factor
f = 1;

%Select length of coherence block
tau_c = 200;

%Compute length of pilot sequences
tau_p = f*K;

%Transmit power per UE in W
p = 0.1;

%Compute total effective transmit power
ETP_total =  K*p*(tau_p/mu_UE + (tau_c-tau_p)*(ULfraction/mu_UE + DLfraction/mu_BS))/tau_c;

%% Go through the two value sets of the CP model
for valueset = 1:2

    %Compute the total CP with different schemes
    [P_MR,P_RZF,P_MMMSE,P_ZF,P_SMMSE] = functionCPcomputation(Mrange,K,L,B,tau_c,tau_p,valueset,sumSE_MR,sumSE_RZF,sumSE_MMMSE,sumSE_ZF,sumSE_SMMSE);

    %Compute EE with M-MMSE
    EE_MMMSE = (B*sumSE_MMMSE)./(ETP_total + P_MMMSE);

    %Compute EE with S-MMSE
    EE_SMMSE = (B*sumSE_SMMSE)./(ETP_total + P_SMMSE);

    %Compute EE with RZF
    EE_RZF = (B*sumSE_RZF)./(ETP_total + P_RZF);

    %Compute EE with ZF
    EE_ZF = (B*sumSE_ZF)./(ETP_total + P_ZF);

    %Compute EE with MR
    EE_MR = (B*sumSE_MR)./(ETP_total + P_MR);

    %Plot simulation results
    figure;

    plot(B*sumSE_MMMSE/10^6,EE_MMMSE/10^6, 'rd-','LineWidth',1);hold on;
    plot(B*sumSE_SMMSE/10^6,EE_SMMSE/10^6,'b:','LineWidth',1);
    plot(B*sumSE_RZF/10^6,EE_RZF/10^6,'k-.','LineWidth',1);
    plot(B*sumSE_ZF/10^6,EE_ZF/10^6,'r--','LineWidth',1);
    plot(B*sumSE_MR/10^6,EE_MR/10^6,'bs-','LineWidth',1);

    xlabel('Throughput [Mbit/s/cell]');
    ylabel('EE [Mbit/Joule/cell]');
    legend('M-MMSE','S-MMSE','RZF','ZF','MR','Location','NorthWest');

end


% SP_PRECOMPUTE: compute the basis functions in the whole mesh. The output
%  can consume a lot of memory. Use with care.
%
%     space = sp_precompute (space, msh);
%     space = sp_precompute (space, msh, 'option', value);
%
% INPUT:
%
%    space: object representing the discrete function space (see sp_vector)
%    msh: mesh object containing the quadrature information (see msh_cartesian)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            divergence |      false      |  compute shape_function_divs
%            curl       |      false      |  compute shape_function_curls
%
% OUTPUT:
%
%    space: object containing the information of the input object, plus the
%            fields of the old structure, that are listed below. If no option
%            is given all the fields are computed. If an option is given,
%            only the selected fields will be computed.
%
%    FIELD_NAME      (SIZE)                               DESCRIPTION
%    ncomp           (scalar)                              number of components of the functions of the space
%    ndof            (scalar)                              total number of degrees of freedom
%    ndof_dir        (ncomp x ndim matrix)                 for each component, number of degrees of freedom along each direction
%    nsh_max         (scalar)                              maximum number of shape functions per element
%    nsh             (1 x msh.nel vector)                  actual number of shape functions per each element
%    connectivity    (nsh_max x msh.nel vector)            indices of basis functions that do not vanish in each element
%    shape_functions (ncomp x msh.nqn x nsh_max x msh.nel) basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%             (ncomp x rdim x msh.nqn x nsh_max x msh.nel) basis function gradients evaluated at each quadrature node in each element
%    shape_function_divs (msh.nqn x nsh_max x msh.nel)     basis function divergence evaluated at each quadrature node in each element
%    shape_function_curls
%         2D:  (msh.nqn x nsh_max x msh.nel)               basis function curl evaluated at each quadrature node in each element
%         3D:  (3 x msh.nqn x nsh_max x msh.nel)
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2015, 2019 Rafael Vazquez
%
%    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/>.

function sp_out = sp_precompute (sp, msh, varargin)

value = true;
gradient = false;
divergence = false;
curl = false;
hessian = false;

if (~isempty (varargin))
  if (~rem (length (varargin), 2) == 0)
    error ('sp_precompute: options must be passed in the [option, value] format');
  end
  for ii=1:2:length(varargin)-1
    if (strcmpi (varargin {ii}, 'value'))
      value = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'gradient'))
      gradient = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'curl'))
      curl = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'divergence'))
      divergence = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'hessian'))
      hessian = varargin {ii+1};
    else
      warning ('Ignoring unknown option %s', varargin {ii});
    end
  end
end

if (~isstruct (msh))
  msh = msh_precompute (msh);
end

grad_param = gradient || divergence || curl || hessian;
value_param = value || grad_param;
div_param = false; curl_param = false;
switch (lower (sp.transform))
  case {'curl-preserving'}
    curl_param = curl;
  case {'div-preserving'}
    div_param = divergence;
end

sp_out = sp_precompute_param (sp, msh, 'value', value_param, 'gradient', grad_param, 'divergence', div_param, 'curl', curl_param, 'hessian', hessian);

switch (lower (sp.transform))
  case {'grad-preserving'}
    sp_out = sp_vector_grad_preserving_transform (sp_out, msh, value, gradient, curl, divergence, hessian);
  case {'curl-preserving'}
    sp_out = sp_vector_curl_preserving_transform (sp_out, msh, value, curl);
    if (gradient || divergence || hessian)
      warning ('Gradient, divergence and hessian not implemented for curl-preserving transformation')
    end
  case {'div-preserving'}
    sp_out = sp_vector_div_preserving_transform (sp_out, msh, value, gradient, curl, divergence);
    if (hessian)
      warning ('Hessian not implemented for div-preserving transformation')
    end
end

end


function bnet = bayes_update_params(bnet, cases, clamped)
% BAYES_UPDATE_PARAMS Bayesian parameter updating given completely observed data
% bnet = bayes_update_params(bnet, cases, clamped)
%
% If there is a missing data, you must use EM.
% cases(i,m) is the value assigned to node i in case m (this can also be a cell array).
% clamped(i,m) = 1 if node i was set by intervention in case m (default: clamped = zeros).
% Clamped nodes are not updated.
% If there is a single case, clamped is a list of the clamped nodes, not a bit vector.

%if iscell(cases), usecell = 1; else usecell = 0; end

n = length(bnet.dag);
ncases = size(cases, 2);
if n ~= size(cases, 1)
  error('data must be of size nnodes * ncases');
end

if ncases == 1 % clamped is a list of nodes
  if nargin < 3, clamped = []; end
  clamp_set = clamped;
  clamped = zeros(n,1);
  clamped(clamp_set) = 1;
else % each row of clamped is a bit vector
  if nargin < 3, clamped = zeros(n,ncases); end
end

for i=1:n
  e = bnet.equiv_class(i);
  if adjustable_CPD(bnet.CPD{e})
    u = find(clamped(i,:)==0);
    ps = parents(bnet.dag, i);
    bnet.CPD{e} = bayes_update_params(bnet.CPD{e}, cases(i,u), cases(ps,u));
  end
end


function [vclassifier, hclassifier] = mcmcTrainSegmentClassifier2(features, labels, weights, maxdata, classparams)

if exist('classparams') && ~isempty(classparams)
    nnodes = classparams(1);
    ntrees = classparams(2);
    stopval = classparams(3);
else
    nnodes = 8;
    ntrees = 20;
    stopval = 0;
end

if ~exist('maxdata') || isempty(maxdata)
    maxdata = 25000;
end

if ~exist('weights') || isempty(weights)
    for f = 1:numel(labels)
        weights{f} = ones(size(labels{f})) / numel(labels{f});
    end
end

vlabels = labels;
hlabels = labels;
for f = 1:numel(labels)
    vlabels{f} = 1*(labels{f}==1) + 2*((labels{f}>1) & (labels{f}<7)) + ...
        3*(labels{f}==7);
    hlabels{f} = (labels{f}-1).*((labels{f}>1) & (labels{f}<7));
end

[vdata, vlab, vw] = formatData(features, vlabels, weights, maxdata);
[hdata, hlab, hw] = formatData(features, hlabels, weights, maxdata);

vnames = {'000', '090', 'sky'};
hnames = {'045', '090', '135', 'por', 'sol'};

%if size(vdata, 2) > 75
catids = []; % 75
%end
%num2str(find(std(vdata, 1)==0))
vclassifier =  train_boosted_dt_mc(vdata, catids, vnames(vlab)', ntrees, nnodes, stopval, vw, vnames);

hclassifier =  train_boosted_dt_mc(hdata, catids, hnames(hlab)', ntrees, nnodes, stopval, hw, hnames);

%% Reformat the input data to be used by classifier
function [data, lab, w] = formatData(features, labels, weights, maxdata)
% concatenate data

nimages = numel(features);

[tmp, nvars] = size(features{1});

% count segments
nseg = 0;
for f = 1:nimages
    nseg = nseg + sum(labels{f}>0);
end
%disp(num2str(nseg))

data = zeros(nseg, nvars);
lab = zeros(nseg, 1);
w = zeros(nseg, 1);

% concatenate data
vc = 0;
for f = 1:nimages
    ind = find(labels{f}>0);
    %disp(num2str([f size(labels{f}) size(features{f})]))
    data(vc+1:vc+numel(ind), :) = features{f}(ind, :);
    lab(vc+1:vc+numel(ind)) = labels{f}(ind);
    w(vc+1:vc+numel(ind)) = weights{f}(ind); % weight according to area in image
    vc = vc + numel(ind);
end
%disp([vc nseg])

if nseg > maxdata
    rind = randperm(nseg);
    rind = rind(1:maxdata);
    data = data(rind, :);
    lab = lab(rind);
    w = w(rind);
    nseg = maxdata;
end

w = w / sum(w);


function test08 ( dim_num, n, z, ns, sample_routine, seed_init )

%*****************************************************************************80
%
%% TEST08 tests NU_MEASURE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST08\n' );
  fprintf ( 1, '  NU_MEASURE computes the NU measure of quality.\n' );
  fprintf ( 1, '  The cell volume deviation       Nu = %14f\n', ...
    nu_measure ( dim_num, n, z, ns, sample_routine, seed_init ) );

  return
end


% PTrc2sp()

% from emuflight

function [RCcommand_dps] = PTrc2sp(RCcommand, rateSystem)
%function [RCcommand_dps] = PTrc2sp(RCcommand)
%   converts rc command to deg/s

BetaflightRates = 1;
BFActualRates = 2;

EmuflightRates = 2;

    rcCommandf    = RCcommand / 500.0;
    rcCommandfAbs = abs(rcCommandf);

          %  if rc_expo
                expof = rc_expo / 100;
                rcCommandf = rcCommandf * Math.pow(rcCommandfAbs, RC_EXPO_POWER) * expof + rcCommandf * (1-expof);
          %  end

            rcRate = rc_rates / 100.0;
          %  if (rcRate > 2.0)
                rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0);
          %  end

            angleRate = 200.0 * rcRate * rcCommandf;
         %   if rates
                rcSuperfactor = 1.0 / ((1.0 - (rcCommandfAbs * rates / 100.0)));
                angleRate = angleRate * rcSuperfactor;
         %   end

 return calculateSetpointRate(axis, value);

%%%%% From betaflight configurator RC3

 const minRc = 1000;
const midRc = 1500;
const maxRc = 2000;

const RateCurve = function (useLegacyCurve) {
    this.useLegacyCurve = useLegacyCurve;
    this.maxAngularVel = null;

    this.constrain = function (value, min, max) {
        return Math.max(min, Math.min(value, max));
    };

    this.rcCommand = function (rcData, rcRate, deadband) {
        const tmp = Math.min(Math.max(Math.abs(rcData - midRc) - deadband, 0), 500);

        let result = tmp * rcRate;

        if (rcData < midRc) {
            result = -result;
        }

        return result;
    };

    this.getBetaflightRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo, superExpoActive, limit) {
        let angularVel;

        if (rcRate > 2) {
            rcRate = rcRate + (rcRate - 2) * 14.54;
        }

        let expoPower;
        let rcRateConstant;

        if (semver.gte(FC.CONFIG.apiVersion, "1.20.0")) {
            expoPower = 3;
            rcRateConstant = 200;
        } else {
            expoPower = 2;
            rcRateConstant = 205.85;
        }

        if (rcExpo > 0) {
            rcCommandf =  rcCommandf * Math.pow(rcCommandfAbs, expoPower) * rcExpo + rcCommandf * (1-rcExpo);
        }

        if (superExpoActive) {
            const rcFactor = 1 / this.constrain(1 - rcCommandfAbs * rate, 0.01, 1);
            angularVel = rcRateConstant * rcRate * rcCommandf; // 200 should be variable checked on version (older versions it's 205,9)
            angularVel = angularVel * rcFactor;
        } else {
            angularVel = (((rate * 100) + 27) * rcCommandf / 16) / 4.1; // Only applies to old versions ?
        }

        angularVel = this.constrain(angularVel, -1 * limit, limit); // Rate limit from profile

        return angularVel;
    };

    this.getRaceflightRates = function (rcCommandf, rate, rcRate, rcExpo) {
        let angularVel = ((1 + 0.01 * rcExpo * (rcCommandf * rcCommandf - 1.0)) * rcCommandf);
        angularVel = (angularVel * (rcRate + (Math.abs(angularVel) * rcRate * rate * 0.01)));
        return angularVel;
    };

    this.getKISSRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo) {
        const kissRpy = 1 - rcCommandfAbs * rate;
        const kissTempCurve = rcCommandf * rcCommandf;
        rcCommandf = ((rcCommandf * kissTempCurve) * rcExpo + rcCommandf * (1 - rcExpo)) * (rcRate / 10);
        return ((2000.0 * (1.0 / kissRpy)) * rcCommandf);
    };

    this.getActualRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo) {
        let angularVel;
        const expof = rcCommandfAbs * ((Math.pow(rcCommandf, 5) * rcExpo) + (rcCommandf * (1 - rcExpo)));

        angularVel = Math.max(0, rate-rcRate);
        angularVel = (rcCommandf * rcRate) + (angularVel * expof);

        return angularVel;
    };

    this.getQuickRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo) {
        rcRate = rcRate * 200;
        rate = Math.max(rate, rcRate);

        let angularVel;
        const superExpoConfig = (((rate / rcRate) - 1) / (rate / rcRate));
        const curve = Math.pow(rcCommandfAbs, 3) * rcExpo + rcCommandfAbs * (1 - rcExpo);

        angularVel = 1.0 / (1.0 - (curve * superExpoConfig));
        angularVel = rcCommandf * rcRate * angularVel;

        return angularVel;
    };

};

RateCurve.prototype.rcCommandRawToDegreesPerSecond = function (rcData, rate, rcRate, rcExpo, superExpoActive, deadband, limit) {
    let angleRate;

    if (rate !== undefined && rcRate !== undefined && rcExpo !== undefined) {
        let rcCommandf = this.rcCommand(rcData, 1, deadband);
        if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_43)) {
            rcCommandf = rcCommandf / (500 - deadband);
        } else {
            rcCommandf = rcCommandf / 500;
        }

        const rcCommandfAbs = Math.abs(rcCommandf);

        switch(TABS.pid_tuning.currentRatesType) {
            case TABS.pid_tuning.RATES_TYPE.RACEFLIGHT:
                angleRate=this.getRaceflightRates(rcCommandf, rate, rcRate, rcExpo);

                break;

            case TABS.pid_tuning.RATES_TYPE.KISS:
                angleRate=this.getKISSRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo);

                break;

            case TABS.pid_tuning.RATES_TYPE.ACTUAL:
                angleRate=this.getActualRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo);

                break;

            case TABS.pid_tuning.RATES_TYPE.QUICKRATES:
                angleRate=this.getQuickRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo);

                break;

            // add future rates types here
            default: // BetaFlight
                angleRate=this.getBetaflightRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo, superExpoActive, limit);

                break;
        }
    }

    return angleRate;
};

RateCurve.prototype.getMaxAngularVel = function (rate, rcRate, rcExpo, superExpoActive, deadband, limit) {
    let maxAngularVel;
    if (!this.useLegacyCurve) {
        maxAngularVel = this.rcCommandRawToDegreesPerSecond(maxRc, rate, rcRate, rcExpo, superExpoActive, deadband, limit);
    }

    return maxAngularVel;
};

RateCurve.prototype.setMaxAngularVel = function (value) {
    this.maxAngularVel = Math.ceil(value/200) * 200;
    return this.maxAngularVel;

};

RateCurve.prototype.draw = function (rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context) {
    if (rate !== undefined && rcRate !== undefined && rcExpo !== undefined) {
        const height = context.canvas.height;
        const width = context.canvas.width;

        if (this.useLegacyCurve) {
            this.drawLegacyRateCurve(rate, rcRate, rcExpo, context, width, height);
        } else {
            this.drawRateCurve(rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context, width, height);
        }
    }
};


function offobj = offLoader(filename)

% function offobj = offLoader(filename)

offobj = struct();
fid = fopen(filename, 'r');
words = fscanf(fid, 'OFF %d %d %d');
nV = words(1); nF = words(2); nE = words(3);
offobj.vmat = reshape(fscanf(fid, '%f', nV*3), 3, nV);
fstr = textscan(fid, '%s', nF, 'delimiter', '\n', 'MultipleDelimsAsOne', 1);
tfmat = zeros(nF*2,4);
nf3 = 0;
for i=1:nF
    words = sscanf(fstr{1}{i}, '%d');
    if words(1) == 3
        nf3 = nf3 + 1;
        tfmat(nf3, :) = words([4,2,3,4]);
    elseif words(1) == 4
        nf3 = nf3 + 1;
        tfmat(nf3, :) = words([4,2,3,4]);
        nf3 = nf3 + 1;
        tfmat(nf3, :) = words([5,2,4,5]);
    else
        error('size of face is not 3 or 4');
    end
end
offobj.fmat = tfmat(1:nf3, :)';
fclose(fid);

end


function [ Y ] = AfuncAcc( U1, V1, U0, V0, spa, R)

Y = U1*(V1'*R);
Y = Y + U0*(V0'*R);
Y = Y + spa*R;

end


%CONE Collision checking class for cone-type primitive shapes
%
% Subclass of the Curvilinear class. The base of the cone intersects
% the transformation frame's origin. May be circular or elliptical.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% Syntax:
%  (1) cone = Cylinder(T, s)
%  (2) cone = Cone(T, s, ...)
%  (3) cone = Cone(P, r, ...)
%  (3) cone = Cone()
%
%  (2) is as per (1) but with other properties set in name-value pairs
%  (3) is as per (1) or (2) but is defined by two points and a radius -
%       the cross-section thus must be circular and not elliptical
%  (4) returns a cone with default values
%
% Outputs:
%  cone : Cone object
%
% Inputs:
%  T      : Transformation matrix. If empty, defaults to eye(4).
%  s      : Scale vector. If empty, defaults to [1 1 1].
%  P      : Cell, whose first element is the origin, and the second
%            element is the other endpoint. Cell and points may be
%            either column or row vectors.
%  r      : Scalar radius value, for syntax mode (3)
%  ...    : Options - other properties in name-value pairs
%
% See documentation for information on properties and methods
% (type doc Shape into the command window)
%
% See also Box.Box Curvilinear Cylinder.Cylinder Ellipsoid.Ellipsoid
% Shape Sphere.Sphere

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.

classdef Cone < Curvilinear

    methods
        function s = Cone(T, S, varargin)
            if ~nargin
               T = [];
               S = [];
            end

            p = @(t)(1-t);

            coneDefaults = [{'n',2,'faces',[1 0]}, varargin];
            s = s@Curvilinear(T,S,p,coneDefaults{:});
        end
    end
end


function [AngleR] = AngleSensorRoomba(serPort);
%[AngleR] = AngleSensorRoomba(serPort)
% Displays the angle in radians and degrees that Create has turned since the angle was last requested.
% Counter-clockwise angles are positive and Clockwise angles are negative.

% By; Joel Esposito, US Naval Academy, 2011

%Initialize preliminary return values
AngleR = nan;

try

%Flush Buffer
N = serPort.BytesAvailable();
while(N~=0)
fread(serPort,N);
N = serPort.BytesAvailable();
end

warning off
global td

fwrite(serPort, [142]);  fwrite(serPort,20);

AngleR = fread(serPort, 1, 'int16')*pi/180;
pause(td)
catch
    disp('WARNING:  function did not terminate correctly.  Output may be unreliable.')
end


function [rc,var,ASAcontrol] = burg_s(f,b,max_order,last)
%BURG_S Burg type AR estimator for multiple segments
%   [RC,VAR] = BURG_S(SIG_MTX,MAX_ORDER) estimates a single vector of AR-
%   reflectioncoefficients RC up to order MAX_ORDER, using data from
%   multiple segments simultaneously. The data segments must be of equal
%   length and arranged columnwise in SIG_MTX. VAR is the estimated
%   variance based on all elements of SIG_MTX.
%
%   RC = BURG_S(F,B,ADD_ORDER) estimates ADD_ORDER additional
%   reflectioncoefficients from matrices of forward and backward
%   residuals F and B, that have been internally used in a previous Burg
%   estimation procedure.
%
%   Since forward and backward residual vectors are internally assigned
%   to the ASAglob variables 'ASAglob_final_f' and 'ASAglob_final_b', an
%   appropriate F and B can be retrieved after the program has finished
%   execution. This is most easily done by typing:
%     F = ASAglobretr('ASAglob_final_f');
%     B = ASAglobretr('ASAglob_final_b');
%
%   BURG_S is an ARMASA main function.
%
%   See also: BURG, ASAGLOB, ASAGLOBRETR.

%   References: S. de Waele and P. M. T. Broersen, The Burg Algorithm for
%               Segments, IEEE Transactions on Signal Processing,
%               vol. 48, no. 10, pp. 2876-2880, October 2000.
%               W. Wunderink, The Introduction of a Computationally
%               Efficient Burg Estimator Algorithm,
%               Tech. Report SSC-March00-1, Delft University of
%               Technology, Department of Applied Physics, Systems
%               Signals and Control Group, the Netherlands, March 2000.

%Header
%==============================================================================

%Declaration of variables
%------------------------

%Declare and assign values to local variables
%according to the input argument pattern
switch nargin
case 1
   if isa(f,'struct'), ASAcontrol=f;
   else, error(ASAerr(39))
   end
   f=[]; b=[]; max_order=[];
case 2
   if isa(b,'struct'), error(ASAerr(2,mfilename))
   end
   max_order=b; b=[]; ASAcontrol=[];
case 3
   if isa(max_order,'struct'), ASAcontrol=max_order; max_order=b; b=[];
   else, ASAcontrol=[];
   end
case 4
   if isa(last,'struct'), ASAcontrol=last;
   else, error(ASAerr(39))
   end
otherwise
   error(ASAerr(1,mfilename))
end

state = 0;
if isequal(nargin,1) & ~isempty(ASAcontrol)
      %ASAcontrol is the only input argument
   ASAcontrol.error_chk = 0;
   ASAcontrol.run = 0;
elseif (isequal(nargin,3) & isempty(ASAcontrol)) | ...
      isequal(nargin,4) %f and b have been provided
      %as input arguments (empty b not excluded)
   state = 1;
end

%Declare ASAglob variables
ASAglob = {'ASAglob_final_f';'ASAglob_final_b'};

%Assign values to ASAglob variables by screening the
%caller workspace
for ASAcounter = 1:length(ASAglob)
   ASAvar = ASAglob{ASAcounter};
   eval(['global ' ASAvar]);
   if evalin('caller',['exist(''' ASAvar ''',''var'')'])
      eval([ASAvar '=evalin(''caller'',ASAvar);']);
   else
      eval([ASAvar '=[];']);
   end
end

%ARMASA-function version information
%-----------------------------------

%This ARMASA-function is characterized by
%its current version,
ASAcontrol.is_version = [2000 12 30 20 0 0];
%and its compatability with versions down to,
ASAcontrol.comp_version = [2000 12 30 20 0 0];

%This function calls other functions of the ARMASA
%toolbox. The versions of these other functions must
%be greater than or equal to:
ASAcontrol.req_version.convolrev = [2000 12 6 12 17 20];

%Checks
%------

if ~any(strcmp(fieldnames(ASAcontrol),'error_chk')) | ASAcontrol.error_chk
      %Perform standard error checks
   %Input argument format checks
   ASAcontrol.error_chk = 1;
   if ~isnum(f)
      error(ASAerr(11,'f'))
   elseif isavector(f) & size(f,2)>1
      f = f(:);
      warning(ASAwarn(25,{'row';'f';'column'},ASAcontrol))
   elseif ~(length(size(f))==2)
      error(ASAerr(35,'f'))
   end
   if ~isavector(f)
      warning(ASAwarn(36,ASAcontrol))
   end
   if ~isempty(b)
      if ~isnum(b)
         error(ASAerr(11,'b'))
      elseif isavector(b) & size(b,2)>1
         b = b(:);
         warning(ASAwarn(25,{'row';'b';'column'},ASAcontrol))
      elseif ~(length(size(b))==2)
         error(ASAerr(35,'b'))
      end
   end
   if ~isnum(max_order) | ~isintscalar(max_order) | ...
         max_order<0
      error(ASAerr(17,'max_order'))
   end

   %Input argument value checks
   if ~isreal(f) | (~isempty(b) & ~isreal(b))
      error(ASAerr(13))
   end
   if isempty(b)
      if max_order>size(f,1)-1
         error(ASAerr(21))
      end
   elseif ~isequal(size(f),size(b))
      error(ASAerr(28,{'f','b'}))
   elseif max_order>size(f,1)-2
      error(ASAerr(29,'max_order'))
   end
end

if ~any(strcmp(fieldnames(ASAcontrol),'version_chk')) | ASAcontrol.version_chk
      %Perform version check
   ASAcontrol.version_chk = 1;

   %Make sure the requested version of this function
   %complies with its actual version
   ASAversionchk(ASAcontrol);

   %Make sure the requested versions of the called
   %functions comply with their actual versions
   convolrev(ASAcontrol);
end

if ~any(strcmp(fieldnames(ASAcontrol),'run')) | ASAcontrol.run
      %Run the computational kernel
   ASAcontrol.run = 1;
   ASAcontrol.version_chk = 0;
   ASAcontrol.error_chk = 0;

%Main
%==========================================================================

[N,n] = size(f);
P = max_order;

if P>0
   %The protocol is tuned using Matlab R12,
   %but also works fine on R11.
   if ~isempty(b)
      P = P+1;
      protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
   else
      if N<128
         protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
      elseif N<12000 %C_7 (& C_13)
         if P<(0.1+6.7e-006*N)*N %C_14 & S
            protocol = [0 1 0;0 0 0;0 0 0;0 P 0];
         else
            protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
         end
      else
         if P<0.18*N %S
            protocol = [0 1 0;0 0 0;0 0 0;0 P 0];
         else
            protocol = [0 1 3 0;0 0 0 0;0 1 0 0;0 round(0.03*N) P 0]; %C_5
         end
      end
   end
else
   protocol = zeros(4,2);
end

p = 0;
p_max = 0;
i2 = 2;
while protocol(4,i2)~=0;
   p = protocol(4,i2)-protocol(4,i2-1);
   if p>p_max
      p_max = p;
   end
   i2 = i2+1;
end
i9 = 1;
i10 = 2;
if n>N
   i9 = 2;
   i10 = 1;
end

v = zeros(N+p_max,n,2);
a = zeros(N+p_max,n,2);
if isempty(b)
   v(1:N,:,2) = f;
   a(1+p_max:N+p_max,:,2) = f;
   if protocol(1,2)==3 | protocol(1,2)==0
      var = sum(sum(f(1:N,:).^2,i9),i10)/(n*N);
      den = 2*var*n*N;
   end
else
   v(1:N,:,2) = f;
   a(1+p_max:N+p_max,:,2) = b;
   den = sum(sum(f(1:N,:).^2+b(1:N,:).^2,i9),i10);
   var = [];
end

r1 = p_max+1;
v1 = [1 1];
v2 = [N N];
v3 = [N N];
a1 = [r1 r1];
a2 = [r1 r1];
a3 = [r1+N-1 r1+N-1];
rc = zeros(1,P+1);
i1 = 0;
i2 = 2;
i4 = 0;
c = 0;

while protocol(4,i2)~=0
   i1 = i1+1;
   p = protocol(4,i2);

   if protocol(1,i2)==3 %SP
      r2 = a2(1)+1;
      r3 = v2(1)-1;
      r4 = a2(2)+1;
      r5 = v2(2)-1;
      i5 = i1;
      for i1 = i1:p
         i5 = i5+1;
         if c, c0 = 2; c1 = 1;
         else c0 = 1; c1 = 2;
         end
         v1(c0) = v1(c1)+1;
         a3(c0) = a3(c1)-1;
         v(v1(c0):v2(c0),:,c0) = v(v1(c0):v2(c1),:,c1)+...
            rc(i1)*a(r4:a3(c1),:,c1);
         a(a2(c0):a3(c0),:,c0) = a(a2(c1):a3(c0),:,c1)+...
            rc(i1)*v(v1(c1):r5,:,c1);
         den = (1-rc(i1)^2)*den-...
            sum((v(v1(c1),:,c1)+rc(i1)*a(a2(c1),:,c1)).^2)-...
            sum((a(a3(c1),:,c1)+rc(i1)*v(v2(c1),:,c1)).^2);
         rc(i5) = -2*sum(sum(v(v1(c0):v2(c0),:,c0).*...
            a(a2(c0):a3(c0),:,c0),i9),i10)/den;
         c = ~c;
      end
      v(v1(c1),:,c0) = v(v1(c1),:,c1)+rc(i1)*a(a2(c1),:,c1);
      a(a3(c1),:,c0) = a(a3(c1),:,c1)+rc(i1)*v(v2(c1),:,c1);

   elseif protocol(1,i2)==1 %ACC
      i13 = 2*p;
      i14 = p+1;
      i15 = 0;
      i16 = 0;
      i17 = 0;
      jj = zeros(2,i13);
      jjm_h = zeros(2,i14);
      jj_h = zeros(2,i14);
      jj(2,1) = 1;
      jjm_h(2,1) = 1;
      jj_h(2,2) = 1;
      for i4 = 1:n
         acov_x(:,i4) = convolrev(f(:,i4),p,ASAcontrol);
      end
      i1 = i1+1;
      var = sum(acov_x(1,:));
      den = (2*var-sum(v(v1(2),:,2).^2)-sum(a(a3(2),:,2).^2));
      rc(i1) = -2*sum(acov_x(2,:))/den;
      var = var/(n*N);
      v1(2) = v1(2)+1;
      a3(2) = a3(2)-1;
      t = 0;
      r5 = v1(2)+p-2;
      r6 = a3(2)-p+2;
      if protocol(3,i2)==1
         r11 = r5+1;
         r12 = r6-1;
         i33 = 0;
      else
         r11 = v2(2)-p+2;
         r12 = a2(2)+p-2;
         i33 = 1;
      end
      r7 = v2(2)+1;
      r8 = a2(2)-1;
      r9 = v1(2);
      r10 = a3(2);
      r13 = r5-r9;
      r14 = r10-r6;
      r15 = r11-r9;
      r16 = r10-r12;

      for i1 = i1:p
         i13 = i1+1;
         i14 = i1-1;
         i15 = 2*i1-1;
         i16 = 2*i1-2;
         i17 = 2*i1-3;
         rc_h1 = rc(i1)^2;
         rc_h2 = 2*rc(i1);
         if c, c0 = 2; c1 = 1;
         else c0 = 1; c1 = 2;
         end
         jj(c0,1:2) = jj(c1,1:2);
         jj(c0,3:i15) = jj(c1,3:i15)+rc_h1*jj(c1,i17:-1:1);
         jj(c0,i1:i16) = jj(c0,i1:i16)+rc_h2*jjm_h(c1,1:i14);
         jj(c0,2:i14) = jj(c0,2:i14)+rc_h2*jjm_h(c1,i14:-1:2);
         jjm_h(c0,1:i1) = rc(i1)*jj_h(c1,1:i1);
         jjm_h(c0,1:i14) = jjm_h(c0,1:i14)+(1+rc(i1)^2)*jjm_h(c1,1:i14);
         jj_h(c0,1:i13) = jj(c0,i13:-1:1);
         jj_h(c0,1:i14) = jj_h(c0,1:i14)+jj(c0,i13:i15);
         va_t(1,:) = jj(c0,1:i1)*acov_x(i13:-1:2,:);
         va_t(2,:) = jj(c0,i13:i15)*acov_x(1:i14,:);
         v1(c0) = v1(c1)+1; v3(c0) = v3(c1)+1;
         a1(c0) = a1(c1)-1; a3(c0) = a3(c1)-1;
         t = t+i33;
         v(r9:r5,:,c0) = v(r9:r5,:,c1)+...
            rc(i1)*a(a1(c1):a1(c1)+r13,:,c1);
         v(r11+t:v3(c1),:,c0) = v(r11+t:v3(c1),:,c1)+...
            rc(i1)*a(a1(c1)+r15+t:r10,:,c1);
         v(v3(c0),:,c0) = rc(i1)*f(N,:);
         a(r6:r10,:,c0) = a(r6:r10,:,c1)+...
            rc(i1)*v(v3(c1)-r14:v3(c1),:,c1);
         a(a1(c1):r12-t,:,c0) = a(a1(c1):r12-t,:,c1)+...
            rc(i1)*v(r9:v3(c1)-r16-t,:,c1);
         a(a1(c0),:,c0) = rc(i1)*f(1,:);
         omega_va(c0) = sum(sum(v(r9:v1(c1),:,c0).*a(a1(c0):r8,:,c0)+...
            v(r7:v3(c0),:,c0).*a(a3(c1):r10,:,c0),1));
         den = den*(1-rc_h1)-sum(v(v1(c1),:,c0).^2)-sum(a(a3(c1),:,c0).^2);
         rc(i13) = -2*(sum(sum(va_t,2),1)-omega_va(c0))/den;
         c=~c;
      end
      i1 = p;
   end
   i2 = i2+1;
end

if state %f and b have been provided as input
      %arguments (empty b not excluded)
      if isempty(b)
         rc = rc(2:end);
      else
         rc = rc(3:end);
      end
else
   rc(1) = 1;
end

i2 = i2-1;
ASAglob_final_f = [];
ASAglob_final_b = [];
if protocol(3,i2)==1 | protocol(1,i2)==3
   ASAglob_final_f = v(v1(c1):v2(c0),:,c0);
   ASAglob_final_b = a(a2(c0):a3(c1),:,c0);
elseif isequal(P,0)
   ASAglob_final_f = f;
end

%Footer
%=====================================================

else %Skip the computational kernel
   %Return ASAcontrol as the first output argument
   if nargout>1
      warning(ASAwarn(9,mfilename,ASAcontrol))
   end
   rc = ASAcontrol;
   ASAcontrol = [];
end

%Program history
%======================================================================
%
% Version                Programmer(s)          E-mail address
% -------                -------------          --------------
% former version         S. de Waele            waele@tn.tudelft.nl
% [2000 12 30 20 0 0]    W. Wunderink           wwunderink01@freeler.nl


function [x,fval,exitflag,info] = multiSolveOpti(optObj,user_x0,ndivs,penalty,solveAll)
%MULTISOLVESOPTI an OPTI object
%
%   Called By opti multisolve

%   Very basic and naive implementation of a multi-start solver, using
%   pretty much an exhaustive search. However it has been very useful on
%   problems where an initial guess is hard to find!

%   Copyright (C) 2013 Jonathan Currie (I2C2)

%Number of 'best points' to keep during search
nbestpts = 10;
%Number of points to use for phase 2
nphase2pts = ceil(nbestpts/2);
%Number of divisions for phase 2 grid
ndivs2 = 2; %over written below if user specifies a vector
%Number of points to use for 'around x0' checking
nx0pts = 5;
%'around x0' radius factor
nx0rad = 0.05;

%If penalty is empty, set default
if(isempty(penalty)), penalty = 1e4; end

%Get display level
verb = dispLevel(optObj.opts.display);
%Build a new OPTI object for use here, disabling output as required (note sense = 1 to prevent double inversion)
Opt = opti(optObj,'sense',1,'options',optiset(optObj.opts,'display','off','warnings','none','derivCheck','off'));

%Allocate input args
prob = Opt.prob;
opts = Opt.opts;
%Initialization
where = '';
x_best = []; fval_best = inf; ef_best = []; info_best = []; i_best = 1;
titer = 0; tfeval = 0;

%If divs is empty, attempt to solve for a version that results in approx 100 sols, min 2 divisions
ndec = length(prob.lb);
if(isempty(ndivs))
    if(ndec==1)
        ndivs = 25;
    else
        ndivs = max(ceil(10^(2/ndec)),2);
    end
elseif(length(ndivs) == 2)
    ndivs2 = ndivs(2);
    if(ndivs2 < 2), nphase2pts = 0; end
    ndivs = ndivs(1);
end
%Determine number of points we are going to check
total_pts = ndivs^ndec;
%For search problems, limit nop based on total_pts
nbestpts = min(nbestpts,total_pts);

%Check division input
if(ndivs(1) == 1), error('You must have more than one division to use this function'); end
if(ndivs(1) > 1e4), error('The maximum number of divisions is 1e4'); end

%Check we have finite bounds
if(isempty(prob.lb) || any(isinf(prob.lb))), error('Multisolve currently requires finite lower bounds on all variables'); end
if(isempty(prob.ub) || any(isinf(prob.ub))), error('Multisolve currently requires finite upper bounds on all variables'); end

%Start Timer
t = tic;
%Build bound checking vectors
cvec = cell(ndec,1);
for i = 1:ndec
    cvec{i} = linspace(prob.lb(i),prob.ub(i),ndivs+1);
end

%Display header
if(verb)
    fprintf('------------------------------------------------------\n');
    fprintf('OPTI Multi-Start Solver using %s (beta)\n',upper(opts.solver));
end

%Construct bound selection matrix for phase 1
lbnd = zeros(total_pts,ndec); lbnd2 = [];
ubnd = zeros(total_pts,ndec); ubnd2 = [];
col = ones(1,ndec+1);
for i = 1:total_pts
    for j = 1:ndec
        lbnd(i,j) = cvec{j}(col(j));
        ubnd(i,j) = cvec{j}(col(j)+1);
    end
    col(1) = col(1) + 1;
    ind = col > ndivs;
    while(any(ind))
        idx = find(ind);
        col(idx) = 1;
        col(idx+1) = col(idx+1) + 1;
        ind = col > ndivs;
    end
end

if(verb)
    if(~solveAll)
        fprintf('\n Phase 1: Exhaustively Searching %d bounded regions (%d divisions per variable):\n',total_pts,ndivs);
    else
        fprintf('\n Exhaustively Solving %d bounded regions (%d divisions per variable):\n',total_pts,ndivs);
    end
end

%Begin searching bounded regions
x_search = cell(nbestpts,1); x_search2 = [];
lb_search = cell(nbestpts,1); ub_search = cell(nbestpts,1);
f_search = Inf(nbestpts,1); f_search2 = [];
for i = 1:total_pts
    %Assign Bounds for this iter
    lb = lbnd(i,:)'; ub = ubnd(i,:)';
    %Make x0
    x0 = (ub-lb)./2 + lb;

    %Search Every Point
    if(~solveAll)
        [x_search,f_search,lb_search,ub_search] = searchPoint(prob,x0,lb,ub,penalty,verb,i,total_pts,x_search,f_search,lb_search,ub_search);
    %Else Solve Every Point
    else
        [x_best,fval_best,ef_best,info_best,i_best,where,titer,tfeval] = solvePoint(Opt,lb,ub,x0,verb,i,total_pts,titer,tfeval,x_best,fval_best,ef_best,info_best,i_best,where,'Exhaustive Solve');
    end
end

%If we are not solving every problem, now solve all best points within their bounds, save best solution then enter phase 2
if(~solveAll)
    if(verb), fprintf('\n Solving Problem in the Best %d Phase 1 Regions:\n',nbestpts); end
    for i = 1:length(x_search)
        %Solve the problem within the search region
        [x_best,fval_best,ef_best,info_best,i_best,where,titer,tfeval] = solvePoint(Opt,lb_search{i},ub_search{i},x_search{i},verb,i,nbestpts,titer,tfeval,x_best,fval_best,ef_best,info_best,i_best,where,'Phase 1');
    end

    %Enter Phase 2, take noPhase2 best boxes and divide into divs2 divisions
    noPhase2 = min(nphase2pts,length(lb_search));
    if(noPhase2)
        total_pts2 = ndivs2^ndec;
        if(verb)
            fprintf('\n Phase 2: Exhaustively Searching %d Sub-Regions Within the %d Best Phase 1 Regions using %d Divisions...\n',total_pts2*noPhase2,noPhase2,ndivs2);
        end
        cvec2 = cell(ndec,noPhase2);
        for i = 1:noPhase2
            for j = 1:ndec
                cvec2{j,i} = linspace(lb_search{i}(j),ub_search{i}(j),ndivs2+1);
            end
        end
        lbnd2 = zeros(total_pts2*noPhase2,ndec);
        ubnd2 = zeros(total_pts2*noPhase2,ndec);
        for p = 1:noPhase2
            col = ones(1,ndec+1);
            for i = 1:total_pts2
                for j = 1:ndec
                    lbnd2(i + total_pts2*(p-1),j) = cvec2{j,p}(col(j));
                    ubnd2(i + total_pts2*(p-1),j) = cvec2{j,p}(col(j)+1);
                end
                col(1) = col(1) + 1;
                ind = col > ndivs2;
                while(any(ind))
                    idx = find(ind);
                    col(idx) = 1;
                    col(idx+1) = col(idx+1) + 1;
                    ind = col > ndivs2;
                end
            end
        end
        %Begin searching bounded regions
        x_search2 = cell(nbestpts,1); lb_search2 = cell(nbestpts,1); ub_search2 = cell(nbestpts,1); f_search2 = Inf(nbestpts,1);
        titer = 0; tfeval = 0; t = tic;
        for i = 1:total_pts2*noPhase2
            %Assign Bounds for this iter
            lb = lbnd2(i,:)'; ub = ubnd2(i,:)';
            %Make x0
            x0 = (ub-lb)./2 + lb;
            %Search Point
            [x_search2,f_search2,lb_search2,ub_search2] = searchPoint(prob,x0,lb,ub,penalty,verb,i,total_pts2*noPhase2,x_search2,f_search2,lb_search2,ub_search2);
        end

        if(verb), fprintf('\n Solving Problem in the Best %d Phase 2 Regions:\n',nbestpts); end
        for i = 1:length(x_search2)
            %Solve the problem within the search region
            [x_best,fval_best,ef_best,info_best,i_best,where,titer,tfeval] = solvePoint(Opt,lb_search2{i},ub_search2{i},x_search2{i},verb,i,length(x_search2),titer,tfeval,x_best,fval_best,ef_best,info_best,i_best,where,'Phase 2');
        end

        %Solve Problem within min and max bounds of best phase 2 divisions
        llb = Inf(ndec,1); gub = -Inf(ndec,1);
        for i = 1:length(lb_search2)
            for j = 1:ndec
                llb(j) = min([llb(j) lb_search2{i}(j) ub_search2{i}(j)]);
                gub(j) = max([gub(j) lb_search2{i}(j) ub_search2{i}(j)]);
            end
        end
        if(verb), fprintf('\n Solving Problem within Bounds of the Best Phase 2 Regions\n'); end
        [x_best,fval_best,ef_best,info_best,i_best,where,titer,tfeval] = solvePoint(Opt,llb,gub,(gub-llb)./2 + llb,verb,1,1,titer,tfeval,x_best,fval_best,ef_best,info_best,i_best,where,'Phase 2 Bounds');
    end
end

%Solve from user supplied start point
if(~isempty(user_x0))
    if(verb), fprintf('\n Phase 3: Searching Problem around User Supplied Start Point:\n'); end
    %Create search cells
    x_searchx0 = cell(nx0pts+1,1);
    f_searchx0 = Inf(nx0pts+1,1);
    %Variance of search points about user x0
    var = (prob.ub-prob.lb).*nx0rad;
    %Search user point
    [x_searchx0,f_searchx0] = searchPoint(prob,user_x0,[],[],penalty,verb,1,nx0pts+1,x_searchx0,f_searchx0);
    %Search random points
    for i = 2:nx0pts+1
        %Create random search points within bounds
        x0 = randnbnd(user_x0,var,prob.lb,prob.ub);
        %Search Point
        [x_searchx0,f_searchx0] = searchPoint(prob,x0,[],[],penalty,verb,i,nx0pts+1,x_searchx0,f_searchx0);
    end
    if(verb), fprintf('\n Solving Problem from the Best 3 Phase 3 Points:\n'); end
    %Solve Problem
    [x_best,fval_best,ef_best,info_best,i_best,where,titer,tfeval] = solvePoint(Opt,prob.lb,prob.ub,user_x0,verb,1,3,titer,tfeval,x_best,fval_best,ef_best,info_best,i_best,where,'User x0');
    for i = 1:2
        [x_best,fval_best,ef_best,info_best,i_best,where,titer,tfeval] = solvePoint(Opt,prob.lb,prob.ub,user_x0,verb,i+1,3,titer,tfeval,x_best,fval_best,ef_best,info_best,i_best,where,'Point Around User x0');
    end
else
    x_searchx0 = [];
    f_searchx0 = [];
end

%Check we have a solution so far
if(isempty(x_best))
    error('OPTI Multi-Solve did not find a feasible/successful solution.\n\nPlease increase the number of search points via the third argument to multisolve()%s','.');
end

%Check best solution in original problem
if(verb)
    if(~solveAll)
        fprintf('\nSolving Original Problem from the Best Point found...\n');
    else
        fprintf('\nRe-solving Original Problem from the Best Point found...\n');
    end
end
%Now Resolve from the best point, given original problem
[x,fval,exitflag,info] = solveOpti(optObj,x_best);
%Sum Iterations + FuncEvals
titer = incField(info,'Iterations',titer);
tfeval = incField(info,'FuncEvals',tfeval);
%Print Results
if(verb)
    status = getStatusMsg(exitflag);
    if(~isfield(info,'Iterations'))
        fprintf('Final Solution: Fval %12.5g, Status %s\n',fval,status);
    else
        fprintf('Final Solution: Fval %12.5g, Iter %4d, Status %s\n',fval,info.Iterations,status);
    end
end
%Check if final solution is actually better than best solution
if(fval_best < fval)
    x = x_best;
    fval = fval_best;
    exitflag = ef_best;
    info = info_best;
    if(verb)
        fprintf('\nFinal Solution is worse than best found, returning best found (%s, Run %d)\n',where,i_best);
    end
end

%Modify Info Status
info.Time = toc(t);
if(titer), info.Iterations = titer; end
if(tfeval), info.FuncEvals = tfeval; end

%Save Search Area
optObj.prob.multi.lbnd = lbnd;
optObj.prob.multi.ubnd = ubnd;
optObj.prob.multi.lbnd2 = lbnd2;
optObj.prob.multi.ubnd2 = ubnd2;
optObj.prob.multi.x_search = x_search;
optObj.prob.multi.f_search = f_search;
optObj.prob.multi.x_search2 = x_search2;
optObj.prob.multi.f_search2 = f_search2;
optObj.prob.multi.x_searchx0 = x_searchx0;
optObj.prob.multi.f_searchx0 = f_searchx0;

if(verb)
    fprintf('------------------------------------------------------\n');
end

%Search a Point, print as required
function [x_search,f_search,lb_search,ub_search] = searchPoint(prob,x0,lb,ub,penalty,verb,i,total_pts,x_search,f_search,lb_search,ub_search)
if(nargin < 12), ub_search = []; end
if(nargin < 11), lb_search = []; end
%Evaluate point
[fpen,f,c] = evalPoint(prob,x0,penalty);
%Decide whether we save this point
idx = fpen < f_search;
bstr = 1;
if(any(idx))
    %Find first index we are better than, insert, and shift remainder
    fidx = find(idx); fidx = fidx(1);
    %Insert
    f_search = [f_search(1:fidx-1);fpen;f_search(fidx:end-1)];
    x_search = [x_search(1:fidx-1);x0;x_search(fidx:end-1)];
    if(~isempty(lb_search))
        lb_search = [lb_search(1:fidx-1);lb;lb_search(fidx:end-1)];
        ub_search = [ub_search(1:fidx-1);ub;ub_search(fidx:end-1)];
    end
    %Indicate new solution found
    bstr = 2;
end
if(verb)
    if(~isempty(c))
        fprintf(bstr,'Point %3d of %3d: FPenalty %12.5g, Fval %12.5g, ConViol %12.5g\n',i,total_pts,fpen,f,c);
    else
        fprintf(bstr,'Point %3d of %3d: Fval %12.5g\n',i,total_pts,f);
    end
end

%Solve a Point, print as required
function [x_best,fval_best,ef_best,info_best,i_best,where,titer,tfeval] = solvePoint(Opt,lb,ub,x0,verb,i,total_pts,titer,tfeval,x_best,fval_best,ef_best,info_best,i_best,where,phase)
%Save new bounds into OPTI object
Opt.prob.lb = lb; Opt.prob.ub = ub;
Opt.nlprob.lb = lb; Opt.nlprob.ub = ub;
Opt.nlprob.options.lb = lb; Opt.nlprob.options.ub = ub;
bstr = 1;
%Solve Problem
try
    %Solve Problem
    [xi,fi,ei,ii] = solveOpti(Opt,x0);
    %Save results into object
    Opt.sol = xi; Opt.obj = fi; Opt.ef = ei; Opt.info = ii;
    %If best, save it
    if(fi < fval_best)
        %Check point is actually feasible
        if(checkOptiSol(Opt,1e-4)) %slightly relaxed tol
            x_best = xi; fval_best = fi; ef_best = ei; info_best = ii; i_best = i; bstr = 2; where = phase;
        end
    end
    %Sum Iterations + FuncEvals
    titer = incField(ii,'Iterations',titer);
    tfeval = incField(ii,'FuncEvals',tfeval);
catch ME
    fi = NaN; ei = NaN; ii = NaN;
    optiwarn('OPTI:SolverError',ME.message);
end
%Display Iteration if requested
if(verb)
    status = getStatusMsg(ei);
    if(~isfield(ii,'Iterations'))
        fprintf(bstr,'Run %3d of %3d: Fval %12.5g, Status %s\n',i,total_pts,fi,status);
    else
        fprintf(bstr,'Run %3d of %3d: Fval %12.5g, Iter %4d, Status %s\n',i,total_pts,fi,ii.Iterations,status);
    end
end

function [fpen,f,c] = evalPoint(prob,x0,penalty)
%Evaluate point
f = prob.objective(x0);
if(~isempty(prob.constraints))
    c = sum(prob.constraints(x0));
    fpen = f + c*penalty;
else
    c = [];
    fpen = f;
end

function status = getStatusMsg(ef)
switch(ef)
    case 1
        status = 'OK';
    case 0
        status = 'Exceeded It/Time';
    case -1
        status = 'Infeasible';
    otherwise
        status = 'Error';
end

function val = incField(strct,field,val)
if(isfield(strct,field) && isnumeric(strct.(field)))
    val = val + strct.(field);
end

function x0 = randnbnd(mean,var,lb,ub)
ndec = length(lb);
%Clamp x0 outside bounds
il = mean < lb; mean(il) = lb(il);
iu = mean > ub; mean(iu) = ub(iu);
%Check if avg sitting on a bound, if so try move by var in
for i = 1:ndec
    if(mean(i) == lb(i))
        mean(i) = min(mean(i) + var(i),ub(i));
    elseif(mean(i) == ub(i))
        mean(i) = max(mean(i) - var(i),lb(i));
    end
end
%Now generate random numbers until all fall within bounds, or 100 iter
idx = true(ndec,1); iter = 1; x0 = zeros(ndec,1);
while(any(idx) && iter < 100)
    xnew = mean(idx) + var(idx).*randn(sum(idx),1);
    x0(idx) = xnew;
    idx = x0 < lb | x0 > ub;
    iter = iter + 1;
end
if(iter==100)
    %re clamp
    il = x0 < lb; x0(il) = lb(il);
    iu = x0 > ub; x0(iu) = ub(iu);
end


function path = grFindGeodesicPath(nodes, edges, ind0, ind1, edgeWeights)
%GRFINDGEODESICPATH Find a geodesic path between two nodes in the graph.
%
%   PATH = grFindGeodesicPath(NODES, EDGES, NODE1, NODE2, WEIGHTS)
%   NODES and EDGES defines the graph, NODE1 and NODE2 are indices of the
%   node extremities, and WEIGHTS is the set of weights associated to each
%   edge.
%   The function returns a set of edge indices.
%
%
%   See also
%   grFindMaximalLengthPath

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2011-05-22, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

% ensure weights are defined
if ~exist('edgeWeights', 'var')
    edgeWeights = ones(size(edges, 1), 1);
end

% check indices limits
nNodes = size(nodes, 1);
if max(ind0) > nNodes
    error('Start index exceed number of nodes in the graph');
end
if max(ind1) > nNodes
    error('End index exceed number of nodes in the graph');
end

% find a vertex opposite to the first extremity
dists = grPropagateDistance(nodes, edges, ind0, edgeWeights);

% iterate on neighbors of current node: choose next neighbor with smallest
% cumulated weight, until we are back on source node
path = [];
while true
    % find neighbor with lowest cumulated distance
    neighs = grAdjacentNodes(edges, ind1);
    neighDists = dists(neighs);
    indN = find(neighDists == min(neighDists), 1);
    ind2 = neighs(indN);

    if isempty(ind2)
        warning('graphs:grFindGeodesicPath', ...
            'No neighbor node found for node %d, graph may be not connected', ind1);
        break;
    end

    % add edge index to the path
    indE = find(sum(ismember(edges, [ind1 ind2]), 2) == 2, 1);
    path = [path indE]; %#ok<AGROW>

    % test if path is finished or not
    if ind2 == ind0
        break;
    end
    ind1 = ind2;
end

% reverse path direction
path = path(end:-1:1);


function [Phip,Sigmap] = reorderVAR(Phi,Sigma,reordering)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filippo Ferroni, 6/1/2015
% Revised, 2/15/2017
% Revised, 3/21/2018

% Permute the autoregressive matrix and the variance coveriance of the
% shocks with the new ordering of the variables.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

N           = size(Sigma,1);

if length(reordering) ~= N,
    error('Not enough permutations: the vector with new ordering of variables must have size N.')
end

Phip    = Phi(:,reordering);
Sigmap  = nan(N);

for jj  = 1 : length(reordering)
    for hh = 1 : length(reordering)

        tmp = Sigma(reordering(jj),reordering(hh));
        Sigmap(jj,hh) = tmp;
    end

end

end


function spqr_demo
%SPQR_DEMO short demo of SuiteSparseQR
%
% Example:
%   spqr_demo
%
% See also SPQR, SPQR_SOLVE, SPQR_QMULT, SPQR_MAKE, SPQR_INSTALL.

%   Copyright 2008, Timothy A. Davis
%   http://www.cise.ufl.edu/research/sparse

more on
help spqr
help spqr_solve
help spqr_qmult
help spqr_demo
more off

input ('Hit enter to start the SuiteSparseQR demo: ', 's') ;
fprintf ('\nTesting SuiteSparseQR functions ... please wait ...\n') ;

load west0479 ;
A = west0479 ;
rand ('state', 0) ;     %#ok
m = size (A,1) ;

figure (1)
clf

maxerr = 0 ;

for acomplex = 0:1

    if (acomplex)
        A = A + 1i * sprand (A) ;
    end
    anorm = norm (A,1) ;

    R1 = spqr (A) ;
    err = norm (R1'*R1 - A'*A, 1) / anorm^2 ;
    maxerr = max (maxerr, err) ;

    [Q,R,E] = spqr (A) ;
    err = norm (Q*R-A*E, 1) / anorm ;
    maxerr = max (maxerr, err) ;

    [H,R,E] = spqr (A, struct ('Q', 'Householder')) ;
    err = norm (spqr_qmult (H,R,1) - A*E, 1) / anorm ;
    maxerr = max (maxerr, err) ;

    if (acomplex)
        subplot (2,5,7)  ; spy (R1)  ; title ('R, no permutation (complex)') ;
        subplot (2,5,8)  ; spy (R)   ; title ('R with colamd (complex)') ;
        subplot (2,5,9)  ; spy (Q)   ; title ('Q with colamd (complex)') ;
        subplot (2,5,10) ; spy (H.H) ; title ('H with colamd (complex)') ;
    else
        subplot (2,5,1)  ; spy (A)   ; title ('A') ;
        subplot (2,5,2)  ; spy (R1)  ; title ('R, no permutation') ;
        subplot (2,5,3)  ; spy (R)   ; title ('R with colamd') ;
        subplot (2,5,4)  ; spy (Q)   ; title ('Q with colamd') ;
        subplot (2,5,5)  ; spy (H.H) ; title ('H with colamd') ;
    end
    drawnow

    % test spqr_solve: real/complex, sparse/full,
    % single/multiple right-hand-sides
    for bcomplex = 0:1
        for bsparse = 0:1
            for nrhs = 0:10
                if (bsparse)
                    b = sprand (m, nrhs, 0.1) ;
                else
                    b = rand (m, nrhs) ;
                end
                if (bcomplex)
                    b = b + 1i * sprand (b) ;
                end
                x = spqr_solve (A,b) ;
                err = norm (A*x-b,1) / max (anorm * norm(x,1) + norm (b,1), 1) ;
                maxerr = max (maxerr, err) ;
            end
        end
    end
end

fprintf ('\nQR maximum error: %g\n\n', maxerr) ;
if (maxerr > 1e-12)
    error ('One or more tests failed; error is high!') ;
end

% ------------------------------------------------------------------------------
% compare spqr_solve
% ------------------------------------------------------------------------------

fprintf ('Compare performance with MATLAB on a dense least-squares problem:\n');
A = rand (2000,1000) ;
b = rand (2000,1) ;
S = sparse (A) ;
fprintf ('\nA = rand (2000,1000) ;\nb = rand (2000,1) ;\nS = sparse (A) ;\n') ;

fprintf ('tic, x = spqr_solve(S,b) ; toc  ') ;
tic
x = spqr_solve (S, b) ;
t1 = toc ;
r1 = norm (A*x-b,1) ;
fprintf ('%% time %8.3f residual %8.3e\n', t1,r1) ;

fprintf ('tic, x = A\\b ; toc              ') ;
tic
x = A\b ;
t2 = toc ;
r2 = norm (A*x-b,1) ;
fprintf ('%% time %8.3f residual %8.3e\n', t2,r2) ;

fprintf ('tic, x = S\\b ; toc              ') ;
tic
x = S\b ;
t3 = toc ;
r3 = norm (A*x-b,1) ;
fprintf ('%% time %8.3f residual %8.3e\n', t3,r3) ;

% ------------------------------------------------------------------------------
% compare spqr with 1000-by-2000 system
% ------------------------------------------------------------------------------

fprintf ('\nA = rand (1000,2000) ;\nS = sparse (A) ;\n') ;
A = rand (1000,2000) ;
S = sparse (A) ;

fprintf ('tic, R = spqr(S) ; toc          ') ;
tic
R = spqr (S) ;
t1 = toc ;
r1 = norm (R'*R-A'*A,1) / norm(A,1)^2 ;
fprintf ('%% time %8.3f error %8.3e\n', t1,r1) ;
clear R

fprintf ('tic, R = qr(A)   ; toc          ') ;
tic
R = qr (A) ;
t2 = toc ;
R = triu (R) ;
r2 = norm (R'*R-A'*A,1) / norm(A,1)^2 ;
fprintf ('%% time %8.3f error %8.3e\n', t2,r2) ;
clear R

fprintf ('tic, R = qr(S)   ; toc          ') ;
tic
R = qr (S) ;
t3 = toc ;
r3 = norm (R'*R-A'*A,1) / norm(A,1)^2 ;
fprintf ('%% time %8.3f error %8.3e\n', t3,r3) ;
clear R

% ------------------------------------------------------------------------------
% compare spqr with 100-by-20000 system
% ------------------------------------------------------------------------------

A = rand (100,20000) ;
S = sparse (A) ;
fprintf ('\nA = rand (100,20000) ;\nS = sparse (A) ;\n') ;

fprintf ('tic, R = spqr(S) ; toc          ') ;
tic
R = spqr (S) ;                                                              %#ok
t1 = toc ;
fprintf ('%% time %8.3f\n', t1) ;
clear R

fprintf ('tic, R = qr(A)   ; toc          ') ;
tic
R = qr (A) ;                                                                %#ok
t2 = toc ;
fprintf ('%% time %8.3f\n', t2) ;
clear R

% skip the old MATLAB QR ... it's way to slow ...
%   fprintf ('tic, R = qr(S)   ; toc          ') ;
%   try
%       tic
%       R = qr (S) ;                                                        %#ok
%       t3 = toc ;
%       fprintf ('%% time %8.3f\n', t3) ;
%   catch                                                                   %#ok
%       fprintf ('%% MATLAB sparse qr failed ...\n') ;
%   end
%   clear R

fprintf ('All spqr tests passed\n') ;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This script is used for processing the raw data collectd by TI awr1843
% radar
% Author : Xiangyu Gao (xygao@uw.edu), University of Washingyton
% Input: raw I-Q radar data
% Output: range-angle (RA) image, 3D point clouds
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clearvars
close all

% parameter setting
params = get_params_value();
% constant parameters
c = params.c; % Speed of light in air (m/s)
fc = params.fc; % Center frequency (Hz)
lambda = params.lambda;
Rx = params.Rx;
Tx = params.Tx;

% configuration parameters
Fs = params.Fs;
sweepSlope = params.sweepSlope;
samples = params.samples;
loop = params.loop;

Tc = params.Tc; % us
fft_Rang = params.fft_Rang;
fft_Vel = params.fft_Vel;
fft_Ang = params.fft_Ang;
num_crop = params.num_crop;
max_value = params.max_value; % normalization the maximum of data WITH 1843

% Creat grid table
rng_grid = params.rng_grid;
agl_grid = params.agl_grid;
vel_grid = params.vel_grid;

% Algorithm parameters
data_each_frame = samples*loop*Tx;
set_frame_number = 30;
frame_start = 1;
frame_end = set_frame_number;
Is_Windowed = 1;% 1==> Windowing before doing range and angle fft
Is_plot_rangeDop = 1;

% specify data name and load data as variable data_frames
seq_name = 'pms1000_30fs.mat';
seq_dir = strcat('.\template data\', seq_name);
load(seq_dir); % load data as variable data_frames

for i = frame_start:frame_end
    % read the data of each frame, and then arrange for each chirps
    data_frame = data_frames(:, (i-1)*data_each_frame+1:i*data_each_frame);
    data_chirp = [];
    for cj = 1:Tx*loop
        temp_data = data_frame(:, (cj-1)*samples+1:cj*samples);
        data_chirp(:,:,cj) = temp_data;
    end

    % separate the odd-index chirps and even-index chirps for TDM-MIMO with 2 TXs
    chirp_odd = data_chirp(:,:,1:2:end);
    chirp_even = data_chirp(:,:,2:2:end);

    % permutation with the format [samples, Rx, chirp]
    chirp_odd = permute(chirp_odd, [2,1,3]);
    chirp_even = permute(chirp_even, [2,1,3]);

    % Range FFT for odd chirps
    [Rangedata_odd] = fft_range(chirp_odd,fft_Rang,Is_Windowed);

    % Range FFT for even chirps
    [Rangedata_even] = fft_range(chirp_even,fft_Rang,Is_Windowed);

    % Doppler FFT
    Dopplerdata_odd = fft_doppler(Rangedata_odd, fft_Vel, 0);
    Dopplerdata_even = fft_doppler(Rangedata_even, fft_Vel, 0);
    Dopdata_sum = squeeze(mean(abs(Dopplerdata_odd), 2));

    % Plot range-Doppler image
    if Is_plot_rangeDop
        plot_rangeDop(Dopdata_sum,rng_grid,vel_grid);
    end

    % CFAR detector on Range-Velocity to detect targets
    % Output format: [doppler index, range index(start from index 1), ...
    % cell power]
    Pfa = 1e-4; % probability of false alarm
    [Resl_indx] = cfar_RV(Dopdata_sum, fft_Rang, num_crop, Pfa);
    detout = peakGrouping(Resl_indx);

    % doppler compensation on Rangedata_even using the max-intensity peak
    % on each range bin
    for ri = num_crop+1:fft_Rang-num_crop
        find_idx = find(detout(2, :) == ri);
        if isempty(find_idx)
            continue
        else
            % pick the first larger velocity
            pick_idx = find_idx(1);
            % phase compensation for virtual elements
            pha_comp_term = exp(-1i * pi * (detout(1,pick_idx)-fft_Vel/2-1) / fft_Vel);
            Rangedata_even(ri, :, :) = Rangedata_even(ri, :, :) * pha_comp_term;
        end
    end

    Rangedata_merge = [Rangedata_odd, Rangedata_even];

    % Angle FFT
    Angdata = fft_angle(Rangedata_merge,fft_Ang,Is_Windowed);
    Angdata_crop = Angdata(num_crop + 1:fft_Rang - num_crop, :, :);
    [Angdata_crop] = Normalize(Angdata_crop, max_value);

    % Angle estimation for detected point clouds
    Dopplerdata_merge = permute([Dopplerdata_odd, Dopplerdata_even], [2, 1, 3]);
    [Resel_agl, ~, rng_excd_list] = angle_estim_dets(detout, Dopplerdata_merge, fft_Vel, ...
        fft_Ang, Rx, Tx, num_crop);

    % Transform bin index to range/velocity/angle
    Resel_agl_deg = agl_grid(1, Resel_agl)';
    Resel_vel = vel_grid(detout(1,:), 1);
    Resel_rng = rng_grid(detout(2,:), 1);

    % save_det data format below
    % [range bin, velocity bin, angle bin, power, range(m), velocity (m/s), angle(degree)]
    save_det_data = [detout(2,:)', detout(1,:)', Resel_agl', detout(3,:)', ...
        Resel_rng, Resel_vel, Resel_agl_deg];

    % filter out the points with range_bin within the crop region
    if ~isempty(rng_excd_list)
        save_det_data(rng_excd_list, :) = [];
    end

    % Plot range-angle (RA) image
    plot_rangeAng(Angdata_crop,rng_grid(num_crop+1:fft_Rang-num_crop),agl_grid);

    % Point obtained clouds
    plot_pointclouds(save_det_data);

    break

end


%% overfitting #1
data = importdata('BCPF_fiber_rmselb_m30_r5.csv');
rmse = data(1:10,:);
lb = data(11:20,:);

color = [55,126,184]/255;
gray = [211,211,211]/255;
orange = [ 0.91,0.41,0.17];
blue = [0,0.4470,0.7410];
light_blue = [0.5843,0.8157,0.9882];
light_red =  [255,204,204]/255;

figure('DefaultAxesFontSize',16);
ax = gca;

yyaxis left;
xbound = [1,1,1:200,200,200,fliplr(1:200)];
ybound = [max(rmse(:,1)),min(rmse(:,1)),min(rmse),min(rmse(:,end)),max(rmse(:,end)),fliplr(max(rmse))];
hold on;
s = fill(xbound,ybound,color,'EdgeColor',gray,'linewidth',0.5);
alpha(s,0.10);
hold on;
plot(rmse','-','color',light_blue,'linewidth',0.5);
hold on;
h1 = plot(mean(rmse),'-','color',blue,'linewidth',2,'DisplayName','rmse');

box on;
grid on;
ax.XTick = [0,25,50,100,200];
ax.XTickLabel = [0,25,50,100,200];
ylim([4.60,5.60]);
ax.YTick = [4.60,4.70,4.80,4.90,5.10,5.60];
ax.YTickLabel = ['4.60';'4.70';'4.80';'4.90';'5.10';'5.60'];
ax.GridLineStyle = '-.';
xlabel('Epoch');
ylabel('RMSE (km/h)');

yyaxis right;
xbound = [1,1,1:200,200,200,fliplr(1:200)];
ybound = [max(lb(:,1)),min(lb(:,1)),min(lb),min(lb(:,end)),max(lb(:,end)),fliplr(max(lb))];
hold on;
s = fill(xbound,ybound,gray,'EdgeColor',gray,'linewidth',0.5);
alpha(s,0.10);
hold on;
plot(lb','-','color',light_red,'linewidth',0.5);
hold on;
h2 = plot(mean(lb),'-','color',orange,'linewidth',2,'DisplayName','lb');
ylim([-1.24e7,-1.19e7]);
ylabel('Lower bound');

legend([h1,h2],{'Average of RMSE';'Average of lower bound'},'location','north');

set(gcf, 'PaperSize', [6 4.5]);
set(gcf, 'PaperPositionMode', 'manual');
set(gcf, 'PaperPosition', [0 0 6 4.5]);
saveas(gcf,'overfitting_ms30_r5','pdf');

%% overfitting #2
data = importdata('BCPF_fiber_rmselb_m30_r10.csv');
rmse = data(1:10,:);
lb = data(11:20,:);

figure('DefaultAxesFontSize',16);
ax = gca;

yyaxis left;
xbound = [1,1,1:200,200,200,fliplr(1:200)];
ybound = [max(rmse(:,1)),min(rmse(:,1)),min(rmse),min(rmse(:,end)),max(rmse(:,end)),fliplr(max(rmse))];
hold on;
s = fill(xbound,ybound,color,'EdgeColor',gray,'linewidth',0.5);
alpha(s,0.10);
hold on;
plot(rmse','-','color',light_blue,'linewidth',0.5);
hold on;
h1 = plot(mean(rmse),'-','color',blue,'linewidth',2,'DisplayName','rmse');

box on;
grid on;
ax.XTick = [0,25,50,100,200];
ax.XTickLabel = [0,25,50,100,200];
ylim([4.35,8.60]);
ax.YTick = [4.35,4.60,4.90,5.20,6.00,8.60];
ax.YTickLabel = ['4.35';'4.60';'4.90';'5.20';'6.00';'8.60'];
ax.GridLineStyle = '-.';
xlabel('Epoch');
ylabel('RMSE (km/h)');

yyaxis right;
xbound = [1,1,1:200,200,200,fliplr(1:200)];
ybound = [max(lb(:,1)),min(lb(:,1)),min(lb),min(lb(:,end)),max(lb(:,end)),fliplr(max(lb))];
hold on;
s = fill(xbound,ybound,gray,'EdgeColor',gray,'linewidth',0.5);
alpha(s,0.10);
hold on;
plot(lb','-','color',light_red,'linewidth',0.5);
hold on;
h2 = plot(mean(lb),'-','color',orange,'linewidth',2,'DisplayName','lb');
ylim([-1.205e7,-1.19e7]);
ylabel('Lower bound');

legend([h1,h2],{'Average of RMSE';'Average of lower bound'},'location','north');

set(gcf, 'PaperSize', [6 4.5]);
set(gcf, 'PaperPositionMode', 'manual');
set(gcf, 'PaperPosition', [0 0 6 4.5]);
saveas(gcf,'overfitting_ms30_r10','pdf');


function visualizeData( X, k, IDX, types, C )
% Project high dim. data unto principal components (PCA) for visualization.
%
% Optionally IDX can be specified to indicate different classes for the
% points; in this case points in different classes are displayed using
% different colors. Up to 12 types are handled (for technical reasons
% involving plot), any cluster with a label>12 is assigned the label 12.
%
% USAGE
%  visualizeData( X, k, [IDX], [types], [C] )
%
% INPUTS
%  X       - column vector of data - N vectors of dimension p (X is Nxp)
%  k       - dimension to which to reduce data (2 or 3)
%  IDX     - [] cluster membership [see kmeans2.m]
%  types   - [] cell array of length ntypes of text labels for each type
%  C       - [] cluster centers (Kxp)
%
% OUTPUTS
%
% EXAMPLE
%  X = [randn(100,5); randn(100,5)+4];
%  C = [mean(X(1:100,:)); mean(X(101:200,:))];
%  IDX = [ones(100,1); 2*ones(100,1)];
%  visualizeData( X, 2, IDX, {'type1','type2' }, C);
%
% See also KMEANS2, DEMOCLUSTER
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<3 || isempty(IDX) ); IDX=[]; end
if( nargin<4 || isempty(types) ); types=[]; end
if( nargin<5 || isempty(C) ); C=[]; end

% apply PCA if necessary
if( size(X,2)~= k )
  [ U, mu ] = pca( X' );
  X = pcaApply( X', U, mu, k )';
  if(~isempty(C)); C = pcaApply( C', U, mu, k )'; end
end

%%% get k
k = size(X,2);
if( k==1 ); X = [X zeros(size(X))]; k = 2; end
if( k>3 ); error( 'k must be <= 3'); end

%%% show points
if( isempty(IDX) )
  if( k==2 )
    plot( X(:,1), X(:,2), '.' );
  elseif( k==3 )
    plot3( X(:,1), X(:,2), X(:,3), '.' );
  end;

else
  IDX(IDX>12)=12;  m=max(IDX);

  if( k==2)
    % plot points
    R = cell(1,3*m+3);
    for i=1:m;
      R((3*i-2):(3*i)) = {X(IDX==i,1), X(IDX==i,2), '.'};
    end;
    R((3*m+1):(3*m+3)) = {X(IDX==-1,1), X(IDX==-1,2), 'k.'};
    plot( R{:} );

    % plot centers
    if( ~isempty(C) )
      R=cell(1,3*m);
      for i=1:m;  R((3*i-2):(3*i)) = {C(i,1), C(i,2), 'x'}; end
      hold('on');  plot( R{:}, 'MarkerSize', 30 );  hold('off');
    end;

  elseif( k==3 )
    % plot points
    R = cell(1,4*m+4);
    for i=1:m;
      R((4*i-3):(4*i)) = {X(IDX==i,1), X(IDX==i,2), X(IDX==i,3), '.'};
    end;
    R((4*m+1):(4*m+4)) = {X(IDX==-1,1), X(IDX==-1,2), X(IDX==-1,3), 'k.'};
    plot3( R{:} );

    % plot centers
    if( ~isempty(C) )
      R=cell(1,4*m);
      for i=1:m;  R((4*i-3):(4*i)) = {C(i,1), C(i,2), C(i,3), 'x'}; end
      hold('on'); plot3( R{:}, 'MarkerSize', 30 );  hold('off');
    end
  end
end
axis('equal');

%%% show legend if types is provided
if(~isempty(types));  legend(types); end


% Author: Paul Kienzle <pkienzle@users.sf.net> (2007)
% This program is granted to the public domain.

% -*- texinfo -*-
% @deftypefn  {Function File} {@var{y} =} downsample (@var{x}, @var{n})
% @deftypefnx {Function File} {@var{y} =} downsample (@var{x}, @var{n}, @var{offset})
% Downsample the signal, selecting every @var{n}th element.  If @var{x}
% is a matrix, downsample every column.
%
% For most signals you will want to use @code{decimate} instead since
% it prefilters the high frequency components of the signal and
% avoids aliasing effects.
%
% If @var{offset} is defined, select every @var{n}th element starting at
% sample @var{offset}.
% @seealso{decimate, interp, resample, upfirdn, upsample}
% @end deftypefn

function y = downsample(x, n, phase)

if nargin<2 || nargin>3, print_usage; end

if nargin<3
  phase = 0;
end

if phase > n - 1
  warning('This is incompatible with Matlab (phase = 0:n-1). See octave-forge signal package release notes for details.')
end

if isvector(x)
  y = x(phase + 1:n:end);
else
  y = x(phase + 1:n:end,:);
end

%!assert(downsample([1,2,3,4,5],2),[1,3,5]);
%!assert(downsample([1;2;3;4;5],2),[1;3;5]);
%!assert(downsample([1,2;3,4;5,6;7,8;9,10],2),[1,2;5,6;9,10]);
%!assert(downsample([1,2,3,4,5],2,1),[2,4]);
%!assert(downsample([1,2;3,4;5,6;7,8;9,10],2,1),[3,4;7,8]);


function [ raw_label ] = frame2rawlabel(label, win_len, win_step)

num_frame = length(label);

total_len = (num_frame-1)*win_step + win_len;

raw_label = zeros(1, total_len);
% temp_label = zeros(1,win_len);
start_indx = 0;
i = 1;
while(1)
    if(start_indx+win_len>total_len)
        break;
    end
    if i==1
        raw_label(start_indx+1 : start_indx + win_len) = label(i);
    else
        temp_label(:) = label(i);
        raw_label(start_indx+1 : start_indx + win_len) = raw_label(start_indx+1 : start_indx + win_len) + temp_label;
%         temp_label = zeros(1,total_len);
    end
    i = i + 1;
    start_indx = start_indx + win_step;
end

raw_label = raw_label>= 1;

end


function SR = superresolve_dbrsr(slidingWindows, magFactor)

numberOfFrames = size(slidingWindows.frames,3);

% Set PSF kernels for the different binning factors.
psf{1} = fspecial('gaussian', 5, 0.8); % PSF for binning factor 2
psf{2} = fspecial('gaussian', 7, 1.2); % PSF for binning factor 3
psf{3} = fspecial('gaussian', 9, 1.6); % PSF for binning factor 4

% Specific preprocessing steps for the non-uniform interpolation based multi-frame methods.
weighting_struct = struct('mec_weight_flag',4,'dis_weight_flag',1,'qps_weight_flag',0,'mec_confMapN',[],...
    'mec_thr_scaler',2,'mec_weight_exp',2,'mec_weighting',[],'dis_rho',0.7,'dis_scaler',magFactor*10,'dis_weighting',[],...
    'qps_map',[],'qps_rho',0.7,'qps_weighting',[],'alpha',1,'beta',1,'gamma',1,'whi_mode',2);
warped_meshXN = zeros(size(slidingWindows.referenceFrame,1),size(slidingWindows.referenceFrame,2),numberOfFrames-1);
warped_meshYN = zeros(size(slidingWindows.referenceFrame,1),size(slidingWindows.referenceFrame,2),numberOfFrames-1);

lr_seq_sorted = slidingWindows.frames;
lr_seq_tmp = lr_seq_sorted(:,:,(numberOfFrames+1)/2);
lr_seq_sorted(:,:,(numberOfFrames+1)/2) = lr_seq_sorted(:,:,1);
lr_seq_sorted(:,:,1) = lr_seq_tmp;

lr_seq_sorted = permute(lr_seq_sorted,[1 2 4 3]);

permutation_vec = 1:numberOfFrames-1;
permutation_vec((numberOfFrames+1)/2:end) = permutation_vec((numberOfFrames+1)/2:end) + 1;
permutation_vec(1:(numberOfFrames-1)/2)   = circshift(permutation_vec(1:(numberOfFrames-1)/2),[0 -1]);

fw_optFlowN_xc = zeros(size(lr_seq_sorted,1),size(lr_seq_sorted,2),2,numberOfFrames-1);

for itera = 1:numberOfFrames-1
    [warped_meshXN(:,:,itera),warped_meshYN(:,:,itera)] = lmsSR_warpImgGridLocalTrans(size(slidingWindows.referenceFrame,1),size(slidingWindows.referenceFrame,2),slidingWindows.flowToReference{permutation_vec(itera)}.mvs_xc);

    % Storing the MEC-SSD-Confidence into the Weighting Struct
    weighting_struct.mec_confMapN(:,:,itera) = slidingWindows.mec_confMap{permutation_vec(itera)};

    % Forward MVF for WDBR
    fw_optFlowN_xc(:,:,:,itera) = slidingWindows.flowToReference{permutation_vec(itera)}.mvs_xc;
end

% Actual SR call
[~,wnuisr_nodeconv,~,~] = lmsSR_generateSRusingNUIv4weighted(lr_seq_sorted,warped_meshXN,warped_meshYN,[magFactor, magFactor],psf{magFactor-1},2,weighting_struct);
[SR,~] = lmsSR_generateSRusingWDBR(wnuisr_nodeconv,fw_optFlowN_xc,magFactor,numberOfFrames,psf{magFactor-1},2,weighting_struct);