% Snake AI: Dynamic Hamiltonian Cycle Repair (with some strategic stuff)
% by Brian Haidet for AlphaPhoenix
% published 4/11/2020
% CC non-commercial, attribution
% See readme for details

%there are two methods that this file uses to reconstruct the hamiltonian
%cycle. first, it checks to see if the turn the snake WANTS to take splits
%the hamcycle into two closed loops. if so, it looks for a location where
%both loops have a flat edge to each other and splices the loops together.

%if that fails, there's a special method for a "loop of two" where a single
%line of two nodes was isolated and needs to be re-fused to tha path.

%if that fails, then there's a more expensive algorythm that attempts to
%draw a brand new hamiltionian cycle by expanding the current path into
%unclaimed regions. It's not vry smart, but it does ocasionally succeed
%(but normally leaves little one-off missing nodes everywhere and doesn't
%complete.)

%if none of these methods solve for a new hamiltionian path, the snake
%follows the old hamiltionian path.

%I'm very sure that somewhere in this file there are some missing minus
%signs or some flipped indeces to matrices or vectors because i sometimes
%see the snake fail to repair the path in a way that I believe it should
%know how, but as you can see, there are an awful lot of IFs and (-)s in
%this file, and I lacked the patience to find every glitch when working on
%this initially. Apparently whatever sometimes fails simply causes this
%file to return no hamiltonian path instead of a wrong or illegal
%hamiltonian path, and that was good enough at the time. if you find
%anything, god you must be bored, but if you do, let me know!!  :)

hypsnake=[nextstepinds,snake];
newcycle=[]; % populated if sucessful

% map cycle #1 (the one with the snake)
oldcutoffpointer_pathind=find(hampathinds==nextstepinds);
cycle1=hampathinds(oldcutoffpointer_pathind:end);
%map cycle #2 (without the snake)
cycle2=hampathinds(1:oldcutoffpointer_pathind-1);
if mod(length(cycle2),2)==1
    disp("it's broken")
end
%does cycle2 close? (AND isn't a 2-node line!)
if (sum(abs(coords(cycle2(1))-coords(cycle2(end))))==1) && length(cycle2)>2 %yes it closes
    cycle1fill=zeros(l);
    cycle1fill(cycle1)=1;
    hypfield=zeros(l);
    hypfield(hypsnake)=1;
    hashperim=10*(cycle1fill-hypfield)+~cycle1fill;
    [hitx,hity]=find(conv2(hashperim,[1,1;1,1])==22);
    for c=1:length(hitx)
        %find out if the adjacent parts of the paths are "flat" ie. connectable
        %for each (manually)
        %find candidate coords
        candidatesite=[hitx(c),hity(c)]+[-1,-1;0,-1;-1,0;0,0];
        candidatesiteinds=inds(candidatesite);
        candidatechains=[0,0,0,0];%candidates belong to cycle1 (0) or cycle2 (1)
        %where does each point?
        candidatepointers=[0,0,0,0];
        cyloc=[0;0;0;0];
        for p=1:4
            if cycle1fill(inds(candidatesite(p,:)))==1
                cyloc(p)=find(cycle1==candidatesiteinds(p,:));
                candidatepointers(p)=cycle1(mod(cyloc(p),length(cycle1))+1);
            else
                candidatechains(p)=1;
                cyloc(p)=find(cycle2==candidatesiteinds(p,:));
                candidatepointers(p)=cycle2(mod(cyloc(p),length(cycle2))+1);
            end
        end
        matchers=intersect(candidatesiteinds,candidatepointers);
        if length(matchers)==2%they match!

            if sum((matchers(1)==candidatesiteinds)&candidatechains') % if matchers(2) belongs to cycle 1
                cy2ind=sum((matchers(1)==candidatesiteinds).*cyloc);
                cy1ind=sum((matchers(2)==candidatesiteinds).*cyloc);
            else
                cy1ind=sum((matchers(1)==candidatesiteinds).*cyloc);
                cy2ind=sum((matchers(2)==candidatesiteinds).*cyloc);
            end
            newcycle=[cycle1(mod((1:cy1ind-1)-1,length(cycle1))+1);cycle2(mod(cy2ind-1:cy2ind+length(cycle2)-2,length(cycle2))+1);cycle1(cy1ind:end)];
            break;
        end
    end
else%no it does not close
    cycle1save=cycle1;
    cycle1=[repmat([-1],l,1);cycle1(1:end-length(snake));repmat([-1],l,1)];
    if length(cycle2) <=900 %try to fix the thing
        makingprogress=false;
        madeprogress=false;
        spliced=false;
        pi=1;%segment being spliced
        while true
            spliced=false;
            if abs(cycle2(pi)-cycle2(pi+1))==1 %splicing vertical segment
                pc1=find(cycle1==cycle2(pi)-l);%look to the left of pi
                if length(pc1)==1% if that spot IS on chain 1
                    if cycle1(pc1+1)==cycle2(pi+1)-l
                        newcycle=[cycle1(1:pc1) ; cycle2(pi) ; cycle2(pi+1) ; cycle1(pc1+1:end)];
                        madeprogress=true;
                        spliced=true;
                    elseif cycle1(pc1-1)==cycle2(pi+1)-l
                        newcycle=[cycle1(1:pc1-1) ; cycle2(pi+1) ; cycle2(pi) ; cycle1(pc1:end)];
                        madeprogress=true;
                        spliced=true;
                    end
                end
                if spliced==false %look to the right if the left isnt on chain 1
                    pc1=find(cycle1==cycle2(pi)+l);%look to the right of pi
                    if length(pc1)==1 % if that spot IS on chain 1
                        if cycle1(pc1+1)==cycle2(pi+1)+l
                            newcycle=[cycle1(1:pc1) ; cycle2(pi) ; cycle2(pi+1) ; cycle1(pc1+1:end)];
                            madeprogress=true;
                            spliced=true;
                        elseif cycle1(pc1-1)==cycle2(pi+1)+l
                            newcycle=[cycle1(1:pc1-1) ; cycle2(pi+1) ; cycle2(pi) ; cycle1(pc1:end)];
                            madeprogress=true;
                            spliced=true;
                        end
                    end
                end
            else %splicing horizontal segment
                if ~(mod(cycle2(pi),l)==1) %if on top row, don't look up
                    pc1=find(cycle1==cycle2(pi)-1);%look to the up of pi
                    if length(pc1)==1 % if that spot IS on chain 1
                        if cycle1(pc1+1)==cycle2(pi+1)-1
                            newcycle=[cycle1(1:pc1) ; cycle2(pi) ; cycle2(pi+1) ; cycle1(pc1+1:end)];
                            madeprogress=true;
                            spliced=true;
                        elseif cycle1(pc1-1)==cycle2(pi+1)-1
                            newcycle=[cycle1(1:pc1-1) ; cycle2(pi+1) ; cycle2(pi) ; cycle1(pc1:end)];
                            madeprogress=true;
                            spliced=true;
                        end
                    end
                end
                if spliced==false%look to the down if the up isnt on chain 1
                    if ~(mod(cycle2(pi),l)==0) %if on bottom row, don't look down
                        pc1=find(cycle1==cycle2(pi)+1);%look to the down of pi
                        if length(pc1)==1 % if that spot IS on chain 1
                            if cycle1(pc1+1)==cycle2(pi+1)+1
                                newcycle=[cycle1(1:pc1) ; cycle2(pi) ; cycle2(pi+1) ; cycle1(pc1+1:end)];
                                madeprogress=true;
                                spliced=true;
                            elseif cycle1(pc1-1)==cycle2(pi+1)+1
                                newcycle=[cycle1(1:pc1-1) ; cycle2(pi+1) ; cycle2(pi) ; cycle1(pc1:end)];
                                madeprogress=true;
                                spliced=true;
                            end
                        end
                    end
                end
            end
            if spliced==true
                cycle2(pi:pi+1)=[];
                cycle1=newcycle;
            else
                pi=pi+2;
            end
            if isempty(cycle2)
%                 disp('stage1')
                break;
            end
            if pi>length(cycle2)
                if madeprogress
                    pi=1;
                    madeprogress=false;
%                     %%plotting for debug
%                     figure(3)
%                     plot(snakecoords(:,2),snakecoords(:,1),'Color',[0 1 0],'LineWidth',15)
%                     xlim([0.5,l+.5]);
%                     ylim([0.5,l+.5]);
%                     axis square
%                     set(gca,'Ydir','reverse')
%                     set(gca,'Color','k')
%                     hold on
%                     plot(optimalPath(:,2),optimalPath(:,1),'Color',[1 0 0],'LineWidth',5)
%                     ncc2=coords(cycle1);
%                     plot(ncc2(:,2),ncc2(:,1),'Color',[0 .3 0],'LineWidth',3)
%                     hold off
                else
%                     disp('stage1 - FAIL')
                    break;
                end
            end

        end
        if ~isempty(cycle2)
            makingprogress=false;
            madeprogress=false;
            spliced=false;
            pi=l+1;%segment being spliced
            while true
                spliced=false;
                if abs(cycle1(pi)-cycle1(pi+1))==1 %splicing vertical segment
                    pc2a=find(cycle2==cycle1(pi)-l);%look to the left of pi
                    pc2b=find(cycle2==cycle1(pi+1)-l);%look to the left of pi+1
                    if (length(pc2a)==1)&&(length(pc2b)==1)% if those spot ARE on chain 2
                        newcycle=[cycle1(1:pi) ; cycle2(pc2a) ; cycle2(pc2b) ; cycle1(pi+1:end)];
                        madeprogress=true;
                        spliced=true;
                    end
                    if spliced==false %look to the right if the left isnt on chain 1
                        pc2a=find(cycle2==cycle1(pi)+l);%look to the right of pi
                        pc2b=find(cycle2==cycle1(pi+1)+l);%look to the right of pi+1
                        if (length(pc2a)==1)&&(length(pc2b)==1)% if those spot ARE on chain 2
                            newcycle=[cycle1(1:pi) ; cycle2(pc2a) ; cycle2(pc2b) ; cycle1(pi+1:end)];
                            madeprogress=true;
                            spliced=true;
                        end
                    end
                else %splicing horizontal segment
                    if ~(mod(cycle1(pi),l)==1) %if on top row, don't look up
                        pc2a=find(cycle2==cycle1(pi)-1);%look to the up of pi
                        pc2b=find(cycle2==cycle1(pi+1)-1);%look to the up of pi+1
                        if (length(pc2a)==1)&&(length(pc2b)==1)% if those spot ARE on chain 2
                            newcycle=[cycle1(1:pi) ; cycle2(pc2a) ; cycle2(pc2b) ; cycle1(pi+1:end)];
                            madeprogress=true;
                            spliced=true;
                        end
                    end
                    if spliced==false%look to the down if the up isnt on chain 1
                        if ~(mod(cycle1(pi),l)==0) %if on bottom row, don't look down
                            pc2a=find(cycle2==cycle1(pi)+1);%look to the down of pi
                            pc2b=find(cycle2==cycle1(pi+1)+1);%look to the down of pi+1
                            if (length(pc2a)==1)&&(length(pc2b)==1)% if those spot ARE on chain 2
                                newcycle=[cycle1(1:pi) ; cycle2(pc2a) ; cycle2(pc2b) ; cycle1(pi+1:end)];
                                madeprogress=true;
                                spliced=true;
                            end
                        end
                    end
                end
                if spliced==true
                    cycle2([pc2a,pc2b])=[];
                    cycle1=newcycle;
                else
                    pi=pi+1;
                end
                if isempty(cycle2)
%                     disp('stage2')
                    break;
                end
                if pi>length(cycle1)-2*l
                    if madeprogress
                        pi=l+1;
                        madeprogress=false;
%                         %%plotting for debug
%                         figure(3)
%                         plot(snakecoords(:,2),snakecoords(:,1),'Color',[0 1 0],'LineWidth',15)
%                         xlim([0.5,l+.5]);
%                         ylim([0.5,l+.5]);
%                         axis square
%                         set(gca,'Ydir','reverse')
%                         set(gca,'Color','k')
%                         hold on
%                         plot(optimalPath(:,2),optimalPath(:,1),'Color',[1 0 0],'LineWidth',5)
%                         ncc2=coords(cycle1);
%                         plot(ncc2(:,2),ncc2(:,1),'Color',[0 .3 .7],'LineWidth',3)
%                         hold off
                    else
%                         disp('stage2 - FAIL')
                        break;
                    end
                end

            end
        end
        if madeprogress
            newcycle=[newcycle(l+1:end-l); cycle1save(end-length(snake)+1:end)];
        else
            newcycle=[];
        end
    end
end


clear;
clc;

ori_root = './train/';
ori_images = dir(fullfile([ori_root 'ShadowImages/'],'*.jpg'));
ori_gts = dir(fullfile([ori_root 'ShadowMasks/'],'*.png'));

save_path = './train_agru/';

for i=1: length(ori_images)
    i
    ro_degree = 0;%rand(1,1)*360-180;

    w_e = rand(1,1)*-0.3;
    h_e = rand(1,1)*-0.3;

    %  b_h = rand(1,1)*90+10;
    % b_w = rand(1,1)*90+10;

    image = imread([ori_root 'ShadowImages/' ori_images(i).name]);
    gt = imread([ori_root 'ShadowMasks/' ori_gts(i).name]);

    [h,w,~]=size(image);

    new_im = imrotate(image,ro_degree);
    new_gt = imrotate(gt,ro_degree);
    [new_h,new_w,~]=size(new_im);

    if (ro_degree<=45 && ro_degree>=-45) || (ro_degree>=135 && ro_degree<=-135)
        f_new_im = new_im(max(new_h/2-h/2,1):min(h/2+new_h/2,size(new_im,1)),max(new_w/2-w/2,1):min(w/2+new_w/2,size(new_im,2)),:);
        f_new_gt = new_gt(max(new_h/2-h/2,1):min(h/2+new_h/2,size(new_im,1)),max(new_w/2-w/2,1):min(w/2+new_w/2,size(new_im,2)),:);
    else
        f_new_im = new_im(max(new_w/2-w/2,1):min(w/2+new_w/2,size(new_im,1)),max(new_h/2-h/2,1):min(h/2+new_h/2,size(new_im,2)),:);
        f_new_gt = new_gt(max(new_w/2-w/2,1):min(w/2+new_w/2,size(new_im,1)),max(new_h/2-h/2,1):min(h/2+new_h/2,size(new_im,2)),:);
    end

%     f_new_im = image;
%     f_new_gt = gt;

    f_new_im = imresize(f_new_im, [h*(1+h_e), w*(1+w_e)]);
    f_new_gt = imresize(f_new_gt, [h*(1+h_e), w*(1+w_e)]);

  %   figure(2),imshow(f_new_im);
% %
  %   figure(1),imshow(f_new_gt);

%%
    imwrite(f_new_im,[save_path 'ShadowImages/a_' ori_images(i).name]);
    imwrite(f_new_gt,[save_path 'ShadowMasks/a_' ori_gts(i).name]);

end


% data,labels are the training data and labels.
% info has the information about these examples (expi,flies,t)
% newdata,newlabels are the data and labels for examples not part of training data.
% newinfo has information about these examples.
% expdirs have the expdirs, trx has the loaded tracks for that experiment, and moviefilename is 'movie.ufmf';

%% Bouts.
bouts = struct('ndx',[],'label',[],'timestamp',[]);
for expNdx = 1:obj.nexps
  for flyNdx = 1:obj.nflies_per_exp(expNdx)
    curLabels = obj.GetLabels(expNdx,flyNdx);
    for boutNum = 1:numel(curLabels.t0s)
      idx =  obj.FlyNdx(expNdx,flyNdx) & ...
        obj.windowdata.t >= curLabels.t0s(boutNum) & ...
        obj.windowdata.t < curLabels.t1s(boutNum);
      bouts.ndx(end+1,:) = obj.FlyNdx(expNdx,flyNdx) & ...
        obj.windowdata.t >= curLabels.t0s(boutNum) & ...
        obj.windowdata.t < curLabels.t1s(boutNum);
      bouts.label(end+1) = find(strcmp(obj.labelnames,curLabels.names{boutNum}));
      bouts.timestamp(end+1) = curLabels.timestamp(boutNum);
    end

  end
end

zz = bouts.timestamp;

moviefilename = 'movie.ufmf';

clparams = struct('iter',100,'iter_updates',10,...
      'numSample',2500,'numBins',30,'CVfolds',7,...
      'baseClassifierTypes',{'Decision Stumps'},'baseClassifierSelected',1);

allbagModels = {}; celldistmat = {};

%% To show the images.. load the trx.
for ndx = 1:obj.nexps
  trxfile = fullfile(obj.expdirs{ndx},obj.GetFileName('trx'));
  trx{ndx} = load_tracks(trxfile);
end
expdirs = obj.expdirs;
%%

for outqndx = 1:numel(Q.traintimes_chaseactive)-1
%%
  qq(1) = Q.traintimes_chaseactive(outqndx);
  if outqndx+1>numel(Q.traintimes_chaseactive)
    qq(2) = Q.traintimes_chaseactive(end);
  else
    qq(2) = Q.traintimes_chaseactive(outqndx+1);
  end

%%

oldbouts = find(zz<qq(1));
oldidx = false(size(bouts.ndx(1,:)));
oldidxall = false(size(bouts.ndx(1,:)));
for ndx = oldbouts(:)'
  nn = find(bouts.ndx(ndx,:),1);
  xx = find(bouts.ndx(ndx,:),1,'last');
  ii = round( (nn+xx)/2);
  oldidx(ii) = true;
  oldidxall = oldidxall | bouts.ndx(ndx,:);
end

oldndxintoall = find(oldidx(oldidxall));
data = obj.windowdata.X(oldidxall,:);
labels = obj.windowdata.labelidx_cur(oldidxall);
info = struct;
info.expi = obj.windowdata.exp(oldidxall);
info.flies = obj.windowdata.flies(oldidxall);
info.t = obj.windowdata.t(oldidxall);

newbouts = find(zz>=qq(1) & zz<qq(2) );
newidx = false(size(bouts.ndx(1,:)));
newidxall = false(size(bouts.ndx(1,:)));
for ndx = newbouts(:)'
  nn = find(bouts.ndx(ndx,:),1);
  xx = find(bouts.ndx(ndx,:),1,'last');
  ii = round( (nn+xx)/2);
  newidx(ii) = true;
  newidxall = newidxall | bouts.ndx(ndx,:);
end

newdata = obj.windowdata.X(newidxall,:);
newlabels = obj.windowdata.labelidx_cur(newidxall);
newinfo = struct;
newinfo.expi = obj.windowdata.exp(newidxall);
newinfo.flies = obj.windowdata.flies(newidxall);
newinfo.t = obj.windowdata.t(newidxall);
fprintf('Training,pos:%d,neg:%d Test:pos%d,neg:%d\n',nnz(labels==1),...
  nnz(labels==2),nnz(newlabels==1),nnz(newlabels==2));

%%
binVals = findThresholds(data,clparams);
bins = findThresholdBins(data,binVals);

%%
%{
[bagModels distmat] = doBagging(data,labels,[],binVals,bins,clparams);
allbagModels{outqndx} = bagModels;
celldistmat{outqndx} = distmat;
%}

%%
bagModels = allbagModels{outqndx};
distmat = celldistmat{outqndx};

%%
newdistmat = findBagCoords(newdata,bagModels);

alldistmat = [distmat; newdistmat];
alllabels = [labels(:)' (newlabels(:)'+2)];

numex = size(alldistmat,1);
xdistmat = zeros(numex);
for ndx = 1:numex
    tt = repmat(alldistmat(ndx,:),numex,1);
    xdistmat(:,ndx) = nanmean(abs(tt-alldistmat),2);
end

wt = getWeights(sign([labels(:)' newlabels(:)']-1.5));
wt = sqrt(wt*wt');
% wt = ones(size(xdistmat));
Z = mdscale(xdistmat,2,'Start','random','Weights',wt);
allnlabels = [labels(:)' newlabels(:)'];
R(1) = sum(Z(1:numel(labels),1).*sign(labels-1.5));
R(2) = sum(Z(1:numel(labels),2).*sign(labels-1.5));
Z = Z(:,1:2);
theta = -atan2(R(2),R(1));
rotmat = [cos(theta) sin(theta); -sin(theta) cos(theta)];
Zr = Z*rotmat;
Zold = Z;
Z = Zr;
Z(:,1) = (Z(:,1)-min(Z(:,1)))/(max(Z(:,1))-min(Z(:,1)));
Z(:,2) = (Z(:,2)-min(Z(:,2)))/(max(Z(:,2))-min(Z(:,2)));

%%
%{
end
save embedBagsChase allbagModels celldistmat;
%}
%% Scatter plot the embedded points.

wt = getWeights(sign(labels-1.5));
[~,bb] = loglossboostLearnRandomFeatures(data,sign(labels-1.5),100,wt,binVals,bins,clparams);
allpred = myBoostClassify([data;newdata],bb);
tr = prctile(abs(allpred),80);
allnlabels = [labels(:)' newlabels(:)'];

%%
C = zeros(numel(alllabels),3);
% S = max(10,1./(1+exp(allpred.*(sign(allnlabels'-1.5))))*100);
S = [];
for ndx = 1:numel(alllabels)
  switch alllabels(ndx)
    case 1
      C(ndx,:) = [1 0 0];
    case 2
      C(ndx,:) = [0 0 1];

    case 3
      C(ndx,:) = [1 0.6 0];
    case 4
      C(ndx,:) = [0 0.6 1];
  end

%  S(ndx) = 1/(min(1,abs(allpred(ndx)/tr)))*10;

%   if sign(allpred(ndx))== sign(allnlabels(ndx)-1.5)
%     S(ndx) = 20;
%   else
%     S(ndx) = 100;
%   end
end
% Scale by mfac so that the image patches are more separate

%%
%{
mfac = 6000;
figure; f = scatter(Z(:,1)*mfac,Z(:,2)*mfac,S,C,'filled');

%% Connect frames from same bouts

hold on;
curZ = Z(1:numel(labels),:);
for ndx = 1:numel(bouts.timestamp)

  if bouts.timestamp(ndx)>qq(1); continue; end;
  curidx = bouts.ndx(ndx,:);
  lndx = find(curidx(oldidxall));
  if bouts.label(ndx)==1
    col = [1 0 0];
  else
    col = [0 0 1];
  end
  plot(curZ(lndx,1)*mfac,curZ(lndx,2)*mfac,'Color',col);
end

curZ = Z(numel(labels)+1:end,:);
for ndx = 1:numel(bouts.timestamp)
  if bouts.timestamp(ndx)>qq(2) || bouts.timestamp(ndx)<qq(1); continue; end;
  curidx = bouts.ndx(ndx,:);
  lndx = find(curidx(newidxall));
  if bouts.label(ndx)==1
    col = [1 0.6 0];
  else
    col = [0 0.6 1];
  end
  plot(curZ(lndx,1)*mfac,curZ(lndx,2)*mfac,'Color',col);
end

%% To show the images.. load the trx.

for ndx = 1:obj.nexps
  trxfile = fullfile(obj.expdirs{ndx},obj.GetFileName('trx'));
  trx{ndx} = load_tracks(trxfile);
end

%% Load images and tracks for training data.
im = {}; traj = {};
for ndx = 1:numel(info.expi)
  expi = info.expi(ndx);
  flies = info.flies(ndx);
  t = info.t(ndx);
  mname = fullfile(expdirs{expi},moviefilename);
  [im{ndx},traj{ndx}] = vid2im(mname,flies,t,trx{expi});
end

%% Load images and tracks for test data.
newim = {}; newtraj = {};
for ndx = 1:numel(newinfo.expi)
  expi = newinfo.expi(ndx);
  flies = newinfo.flies(ndx);
  t = newinfo.t(ndx);
  mname = fullfile(expdirs{expi},moviefilename);
  [newim{ndx},newtraj{ndx}] = vid2im(mname,flies,t,trx{expi});
end

%% Show the most negative images on out of training bouts

hold on;

curZ = Z(numel(labels)+1:end,:);
for ndx = 1:numel(bouts.timestamp)
  if bouts.timestamp(ndx)>qq(2) || bouts.timestamp(ndx)<qq(1); continue; end;
  curidx = bouts.ndx(ndx,:);
  lndx = find(curidx(newidxall));
  curScores = myBoostClassify(newdata(lndx,:),bb);
  [~,minndx] = min(curScores*sign(bouts.label(ndx)-1.5));
  curlndx = lndx(minndx);
  ii = uint8(repmat(newim{curlndx},[1 1 3]));
  newX = curZ(curlndx,1)*mfac; newY = curZ(curlndx,2)*mfac;
  image(newX,newY,ii);
  plot(newtraj{curlndx}.Y+newX,newtraj{curlndx}.X+newY)
end
axis equal;

%% Show the most negative images on  training bouts

hold on;

curZ = Z(1:numel(labels),:);
for ndx = 1:numel(bouts.timestamp)
  if bouts.timestamp(ndx)>qq(1), continue; end;
  curidx = bouts.ndx(ndx,:);
  lndx = find(curidx(oldidxall));
  curScores = myBoostClassify(data(lndx,:),bb);
  [~,minndx] = min(curScores*sign(bouts.label(ndx)-1.5));
  curlndx = lndx(minndx);
  ii = uint8(repmat(im{curlndx},[1 1 3]));
  newX = curZ(curlndx,1)*mfac; newY = curZ(curlndx,2)*mfac;
  image(newX,newY,ii);
  plot(traj{curlndx}.Y+newX,traj{curlndx}.X+newY)
end
axis equal;

%% Show the training images.

hold on;
curZ = Z(1:size(distmat,1),:);
for ndx = 1:numel(im)
  ii = uint8(repmat(im{ndx},[1 1 3]));
  newX = curZ(ndx,1)*mfac; newY = curZ(ndx,2)*mfac;
  image(newX,newY,ii);
  plot(traj{ndx}.Y+newX,traj{ndx}.X+newY)
end
axis equal;

%% Show the "out of training" images

hold on;
curZ = Z( (size(distmat,1)+1):end,:);
for ndx = 1:numel(newim)
  ii = uint8(repmat(newim{ndx},[1 1 3]));
  newX = curZ(ndx,1)*mfac; newY = curZ(ndx,2)*mfac;
  image(newX,newY,ii);
  plot(newtraj{ndx}.Y+newX,newtraj{ndx}.X+newY)
end
axis equal;
%
%%
mfac = 1200; sfac = 30;
clear Zm;
Zm(:,1) = Z(:,1)-min(Z(:,1));
Zm(:,2) = Z(:,2)-min(Z(:,2));

zszx = max(Zm(:,1))*mfac/sfac;
zszy = max(Zm(:,2))*mfac/sfac;
bimg = ones(round(zszy)+1,round(zszx)+1,3);

counts = zeros(round(zszy)+1,round(zszx)+1);
for ndx= 1:numel(labels)
  curx = round( Zm(ndx,1) *mfac/sfac)+1;
  cury = round( Zm(ndx,2) *mfac/sfac)+1;
  bimg(cury,curx,:) = reshape(C(ndx,:),[1 1 3]);
end

bbimg = imfilter(bimg,fspecial('gaussian',2*3*8,8),'symmetric');
hsvbimg = rgb2hsv(bbimg);
satimg = hsvbimg(:,:,2);
satimg = satimg-mean(satimg(:))+0.1;
satimg(satimg>1) = 1;
hsvbimg(:,:,2) = satimg;
bbimg = hsv2rgb(hsvbimg);
fbimg = imresize(bbimg,sfac);

fbimg(fbimg(:)>1) = 1;

figure; image(fbimg);
%}

%%

mfac = 400; sfac = 3;

clear Zm;
Zm(:,1) = Z(:,1)-min(Z(:,1))+0.15;
Zm(:,2) = Z(:,2)-min(Z(:,2))+0.15;

zszx = (max(Zm(:,1))+0.15)*mfac/sfac;
zszy = (max(Zm(:,2))+0.15)*mfac/sfac;

numpos = nnz(labels==1);
numneg = nnz(labels==2);
ratio = numneg/numpos;
rad = sqrt(ratio);
totArea = 4*rad.^2*numpos + numneg;
bfac = sqrt(8000/totArea);

bimgy = round(zszy)+round(bfac+rad)+sfac;
bimgx = round(zszx)+round(bfac+rad)+sfac;
bimg = ones(bimgy,bimgx,3);

for ndx= 1:numel(labels)
  curx = round( Zm(ndx,1) *mfac/sfac)+1;
  cury = round( Zm(ndx,2) *mfac/sfac)+1;

  bimg(cury,curx,:) = reshape(C(ndx,:),[1 1 3]);
  if labels(ndx) ==1
    bboxy = cury+round(-rad*bfac:rad*bfac);
    bboxy(bboxy<1) = [];
    bboxx = curx+round(-rad*bfac:rad*bfac);
    bboxx(bboxx<1) = [];
    bimg(bboxy,bboxx,1) = C(ndx,1);
    bimg(bboxy,bboxx,2) = C(ndx,2);
    bimg(bboxy,bboxx,3) = C(ndx,3);
  else
    bboxy = cury+round(-bfac:bfac);
    bboxy(bboxy<1) = [];
    bboxx = curx+round(-bfac:bfac);
    bboxx(bboxx<1) = [];
    bimg(bboxy,bboxx,1) = C(ndx,1);
    bimg(bboxy,bboxx,2) = C(ndx,2);
    bimg(bboxy,bboxx,3) = C(ndx,3);

  end

end
aoccupied = bimg(:,:,2)~=1;
fprintf('Area occupied for %d:%d\n ',outqndx,nnz(aoccupied));
bbimg = imfilter(bimg,fspecial('gaussian',2*3*32,32),'symmetric');
fbimg = imresize(bbimg,sfac);

fbimg(fbimg(:)>1) = 1;
%%

overall = figure;
image(fbimg); hold on;

overall1 = figure;
image(fbimg); hold on;

bname = sprintf('/groups/branson/home/kabram/paperFigs/classifier_chase_images_%d/',outqndx);
if ~exist(bname,'dir'), mkdir(bname); end
imwrite(fbimg,fullfile(bname,'background.png'));

%
% scatter(Zm(numel(labels)+1:end,1)*mfac,Zm(numel(labels)+1:end,2)*mfac,...
%   S(numel(labels)+1:end),C(numel(labels)+1:end,:),'filled');
% hold on;

occupied = false(size(fbimg));
rem = 55;

curZ = Zm(numel(labels)+1:end,:);
px = []; py = [];
bx_p = []; by_p = [];
bx_n = []; by_n = [];
bx_np = []; by_np = [];
bx_nn = []; by_nn = [];
phandle = [];
count = 0;
for ndx = 1:numel(bouts.timestamp)
  if bouts.timestamp(ndx)>qq(2) || bouts.timestamp(ndx)<qq(1); continue; end;
  curidx = bouts.ndx(ndx,:);
  lndx = find(curidx(newidxall));
  curScores = myBoostClassify(newdata(lndx,:),bb);
  [~,minndx] = min(curScores*sign(bouts.label(ndx)-1.5));
  curlndx = lndx(minndx);
  expi = newinfo.expi(curlndx);
  flies = newinfo.flies(curlndx);
  t = newinfo.t(curlndx);
  mname = fullfile(expdirs{expi},moviefilename);
  [im,traj] = vid2im(mname,flies,t,trx{expi});

  ii = uint8(repmat(flipud(im),[1 1 3]));
  ii(:,[1:rem end-rem-1:end],:)=[];
  halfsz = round(size(ii,1)/2);
  bottomout = size(ii,1)-halfsz-30+1;
  ii(halfsz+30:end,:,:) = [];

  alpha = (ii(:,:,2)<170);
  newX = curZ(curlndx,1)*mfac-size(ii,1)/2;
  newY = curZ(curlndx,2)*mfac-size(ii,2)/2;

  testX = floor(newX:newX+size(ii,1)); testX(testX<1) = []; testX(testX>size(occupied,1))=[];
  testY = floor(newY:newY+size(ii,1)); testY(testY<1) = []; testY(testY>size(occupied,2))=[];
  oo = occupied(testX,testY);
  if(nnz(oo(:))/numel(oo)>0.4), continue; end;
  count = count+1;

  if bouts.label(ndx) == 1
    bx_np = [bx_np nan newX newX newX+size(ii,2) newX+size(ii,2) newX];
    by_np = [by_np nan newY newY+size(ii,1) newY+size(ii,1) newY newY];
  else
    bx_nn = [bx_nn nan newX newX newX+size(ii,2) newX+size(ii,2) newX];
    by_nn = [by_nn nan newY newY+size(ii,1) newY+size(ii,1) newY newY];
  end

  hszx = size(ii,1) + bottomout;

  occupied(testX,testY) = true;

  figure(overall1);
  if bouts.label(ndx) == 1
    plot(newX+size(ii,1)/2,newY+size(ii,2)/2,'Marker','*','MarkerEdgeColor',[1 0.6 0],'MarkerSize',3);
  else
    plot(newX+size(ii,2)/2,newY+size(ii,2)/2,'Marker','*','MarkerEdgeColor',[0 0.6 1],'MarkerSize',3);
  end

  imwrite(ii,sprintf('%s/individual_images_test_%d.png',bname,count));

  imagetrxtemp = figure; imagebothtemp = figure; hold on;
  figure(imagebothtemp);
  image(1,1,ii,'AlphaData',alpha);
  plot(-traj.Y+hszx-rem,-traj.X+hszx,'LineStyle','none','Marker','.','MarkerSize',10,'MarkerEdgeColor','k');
  axis off; axis image;
  fname = sprintf('%s/individual_both_test_%d',bname,count);
  SaveFigLotsOfWays(imagebothtemp,fname);

  figure(imagetrxtemp); axis off; axis image;
  plot(-traj.Y+hszx-rem,-traj.X+hszx,'LineStyle','none','Marker','.','MarkerSize',10,'MarkerEdgeColor','k');
  fname = sprintf('%s/individual_trx_test_%d',bname,count);
  axis off; axis image;
  SaveFigLotsOfWays(imagetrxtemp,fname);

  close(imagebothtemp); close(imagetrxtemp);

  figure(overall);
  image(newX,newY,ii,'AlphaData',alpha);
  hszx = size(ii,1) + bottomout;
  px = [px nan -traj.Y+newX+hszx-rem];
  py = [py nan -traj.X+newY+hszx];

end

pxtest = px;
pytest = py;
px = []; py = [];

curZ = Zm(1:numel(labels),:);
count = 0;
for ndx = 1:numel(bouts.timestamp)
  if bouts.timestamp(ndx)>qq(1), continue; end;
  curidx = bouts.ndx(ndx,:);
  lndx = find(curidx(oldidxall));
  curlndx = round(median(lndx));
  expi = info.expi(curlndx);
  flies = info.flies(curlndx);
  t = info.t(curlndx);
  mname = fullfile(expdirs{expi},moviefilename);
  [im,traj] = vid2im(mname,flies,t,trx{expi});

  ii = uint8(repmat(flipud(im),[1 1 3]));
  ii(:,[1:rem end-rem-1:end],:)=[];
  halfsz = round(size(ii,1)/2);
  bottomout = size(ii,1)-halfsz-30+1;
  ii(halfsz+30:end,:,:) = [];
  alpha = (ii(:,:,2)<170);
  newX = curZ(curlndx,1)*mfac - size(ii,1)/2;
  newY = curZ(curlndx,2)*mfac - size(ii,2)/2;

  testX = floor(newX:newX+size(ii,1)); testX(testX<1) = []; testX(testX>size(occupied,1))=[];
  testY = floor(newY:newY+size(ii,1)); testY(testY<1) = []; testY(testY>size(occupied,2))=[];
  oo = occupied(testX,testY);
  if(nnz(oo(:))/numel(oo)>0.4), continue; end;
  count = count+1;

  if bouts.label(ndx) == 1
    bx_p = [bx_p nan newX newX newX+size(ii,2) newX+size(ii,2) newX];
    by_p = [by_p nan newY newY+size(ii,1) newY+size(ii,1) newY newY];
  else
    bx_n = [bx_n nan newX newX newX+size(ii,2) newX+size(ii,2) newX];
    by_n = [by_n nan newY newY+size(ii,1) newY+size(ii,1) newY newY];
  end

  occupied(testX,testY) = true;

  imwrite(ii,sprintf('%s/individual_images_train_%d.png',bname,count));
  imagetrxtemp = figure; imagebothtemp = figure; hold on;
  figure(imagebothtemp);axis off; axis image;
  image(1,1,ii,'AlphaData',alpha);
  plot(-traj.Y+hszx-rem,-traj.X+hszx,'LineStyle','none','Marker','.','MarkerSize',6,'MarkerEdgeColor','k');
  fname = sprintf('%s/individual_both_train_%d',bname,count);
  axis off; axis image;
  SaveFigLotsOfWays(imagebothtemp,fname);

  figure(imagetrxtemp);axis off; axis image;
  plot(-traj.Y+hszx-rem,-traj.X+hszx,'LineStyle','none','Marker','.','MarkerSize',6,'MarkerEdgeColor','k');
  fname = sprintf('%s/individual_trx_train_%d',bname,count);
  axis off; axis image;
  SaveFigLotsOfWays(imagetrxtemp,fname);

  close(imagebothtemp); close(imagetrxtemp);

  figure(overall);
  image(newX,newY,ii,'AlphaData',alpha);
  figure(overall1);
  image(newX,newY,ii,'AlphaData',alpha);

  hszx = size(ii,1) + bottomout;
  px = [px nan -traj.Y+newX+hszx-rem];
  py = [py nan -traj.X+newY+hszx];
end

figure(overall);
phandle = [phandle plot(bx_np,by_np,'-','Color',[1 0.6 0])];
phandle = [phandle plot(bx_nn,by_nn,'-','Color',[0 0.6 1])];
phandle = [phandle plot(bx_p,by_p,'-','Color',[1 0 0])];
phandle = [phandle plot(bx_n,by_n,'-','Color',[0 0 1])];

figure(overall1);
phandle = [phandle plot(bx_p,by_p,'-','Color',[1 0 0])];
phandle = [phandle plot(bx_n,by_n,'-','Color',[0 0 1])];

for pndx = 1:numel(phandle);
  uistack(phandle(pndx),'bottom');
  uistack(phandle(pndx),'up');
end

figure(overall);
ppx = [pxtest px]; ppy = [pytest py];
plot(ppx,ppy,'LineStyle','none','Marker','.','MarkerSize',6,'MarkerEdgeColor','k');
% plot(ppx+0.5,ppy,'LineStyle','none','Marker','.','MarkerSize',1.5,'MarkerEdgeColor','k');
% plot(px,py+0.5,'LineStyle','none','Marker','.','MarkerSize',1.5,'MarkerEdgeColor','k');
% plot(px+0.5,py+0.5,'LineStyle','none','Marker','.','MarkerSize',1.5,'MarkerEdgeColor','k');
axis equal; axis tight; axis off;
saveas(overall,[bname '/overall.fig']);
SaveFigLotsOfWays(overall,[bname '/overall']);
close(overall);

figure(overall1);
plot(px,py,'LineStyle','none','Marker','.','MarkerSize',6,'MarkerEdgeColor','k');
% plot(px+0.5,py,'LineStyle','none','Marker','.','MarkerSize',1.5,'MarkerEdgeColor','k');
% plot(px,py+0.5,'LineStyle','none','Marker','.','MarkerSize',1.5,'MarkerEdgeColor','k');
% plot(px+0.5,py+0.5,'LineStyle','none','Marker','.','MarkerSize',1.5,'MarkerEdgeColor','k');
axis equal; axis tight; axis off;
saveas(overall1,[bname '/overall1.fig']);
SaveFigLotsOfWays(overall1,[bname '/overall1']);
close(overall1);

% title(sprintf('Train:%d',outqndx));
% fname = sprintf('/groups/branson/home/kabram/paperFigs/classifier_chase_new_plotted_%d',outqndx);
% SaveFigLotsOfWays(hfig,fname);
% close(hfig);
end


classdef stressgrid < ZmapHGridFunction
    % STRESSGRID calculate stress grid for event that have Dip, DipDirection, and Rake
    properties
        calcmethod
    end

    properties(Constant)
        PlotTag         = 'stressgrid';
        ReturnDetails   = cell2table({ ...
            ...
            'S1Trend',  'S1Trend','';...
            'S1Plunge', 'S1Plunge','';...
            'S2Trend',  'S2Trend','';...
            'S2Plunge', 'S2Plunge','';...
            'S3Trend',  'S3Trend','';...
            'S3Plunge', 'S3Plunge','';...
            'Variance', 'Variance',''...
            }, 'VariableNames', {'Names','Descriptions','Units'});

        CalcFields = {...
            'S1Trend',  'S1Plunge',...
            'S2Trend',  'S2Plunge',...
            'S3Trend',  'S3Plunge',...
            'Variance'};

        ExtDir = fullfile(ZmapGlobal.Data.hodi, 'external');
        ParameterableProperties = [];

        %reference may not be the correct one
        References = "Michael AJ (1984) Determination of stress from slip data: faults and folds. J Geophys Res 89:11517–11526. doi:10.1029/JB089iB13p11517";
    end
    methods
        function obj = stressgrid(zap, varargin)
            % STRESSGRID calculate stress tensor using Michaels  code

            % NotImplemented: or Gephards code

            obj@ZmapHGridFunction(zap, 'S1Trend');
            report_this_filefun();

            obj.parseParameters(varargin);
            obj.StartProcess();

        end

        function InteractiveSetup(obj)
            %{
                dx = 0.1;
                dy = 0.1 ;
                ni = 50;
                ra = ZG.ra;
                Nmin = 0;
            %}

            zdlg = ZmapDialog();
            zdlg.AddPopup('calcmethod','Calc Method',{'Michaels Method'},1,...
                'Choose the only method that is available. (sorry, no other options)');

            obj.AddDialogOption(zdlg, 'EventSelector')
            zdlg.Create('Name', 'Stress grid options', 'WriteToObj',obj, 'OkFcn',@obj.doIt);
        end

        function results = Calculate(obj)
            % get the grid-size interactively and
            % calculate the b-value in the grid by sorting
            % the seismicity and selectiong the ni neighbors
            % to each grid point

            % get new grid if needed
            d = pwd;
            slick_cmd = fullfile(obj.ExtDir, append_system_specific_postfix('slick'))
            slfast_cmd = fullfile(obj.ExtDir, append_system_specific_postfix('slfast'))

            try
                cd(obj.ExtDir);
                itter=1;
                % calculate at all points
                obj.gridCalculations(@calculation_function);

                cd(d);

            catch ME

                cd(d);
                rethrow(ME)

            end

            if nargout
                results=obj.Result.values;
            end
            f=gcf;
            try
            obj.view_stressmap()
            catch ME
                warning(ME.message)
            end
            figure(f);

            function bvg = calculation_function(b)
                % returns: S1Trend S1Plunge S2Trend S2Plunge S3Trend S3Plunge Variance Radius b-value

                %estimate the completeness and b-value
                % Take the focal mechanism from actual catalog
                % tmpi-input: [dip direction (East of North), dip , rake (Kanamori)]
                %{
                tic
                itter=itter+1;
                inputFile = "dataz"+itter;
                outputFile = inputFile + ".oput";
                outputFileBoot = inputFile + ".slboot";

                % disp('writing raw inversion data to : ' + inputFile);
                % Create file for inversion
                fid = fopen(inputFile,'w');
                fprintf(fid,'%s \n', 'Inversion data');
                for i=1:b.Count
                    fprintf(fid,'%7.3f  %7.3f  %7.3f\n', b.DipDirection(i), b.Dip(i), b.Rake(i));
                end
                fclose(fid);

                % slick calculates the best solution for the stress tensor according to
                % Michael(1987): creates data2.oput
                % disp('slick')
                [mystatus, myresult] = system(slick_cmd + inputFile);

                % Get data from data2.oput

                [fBeta, fStdBeta, fTauFit, fAvgTau, fStdTau] = import_slickoput(outputFile);
                % Stress tensor inversion
                [mystatus, myresult] = system(slfast_cmd + inputFile); % --> writes to outputFileBoot

                sGetFile = fullfile(obj.ExtDir, outputFileBoot); %'data2.slboot'

                % Description of data2
                % Line 1: Variance S11 S12 S13 S22 S23 S33 => Variance and components of stress tensor (S = sigma)
                % Line 2: Phi S1t S1p S2t S2p S3t S3p   => Phi is relative size S3/S1, t=trend, p=plunge (other description)

                                % Result matrix
                % S1Trend, S1Plunge, S2Trend, S2Plunge, S3Trend, S3Plunge, Variance // ignored: Radius, bvalue

                d0=load(sGetFile, inputFile);
                bvg_old = [d0(2,2:7) d0(1,1)];

                if isfile(inputFile)
                    delete(inputFile);
                end
                if isfile(outputFile)
                    delete(outputFile);
                end
                if isfile(outputFileBoot)
                    delete(outputFileBoot);
                end

                toc
                disp('^-old   new-v');
                %}
                %tic

                %[fBeta2, fStdBeta2, fTauFit2, fAvgTau2, fStdTau2]=slick([b.DipDirection b.Dip b.Rake]);
                b = b.getAddon('MomentTensor');
                [bvg(7), ~, ~, bvg(1), bvg(2), bvg(3), bvg(4), bvg(5), bvg(6)]= slick(...
                    [b.DipDirection b.Dip b.Rake]);
                % toc

            end
        end

        function view_stressmap(obj)
            % view_stressmap
            % pulled into function due to its tight bonding from an m file
            % Author: S. Wiemer
            % updated: 19.05.2005, j.woessner@sed.ethz.ch
            % turned into function by Celso G Reyes 2017
            %FIXME this has never been updated

            values = obj.Result.values;

            ZG=ZmapGlobal.Data;
            SA = 1;
            SA2 = 3;

            % Matrix bvg contains:
            % bvg : [S1Trend S1Plunge S2Trend S2Plunge S3Trend S3Plunge Variance Radius b-value]
            % ste : [S1Plunge S1Trend+180 S2Plunge S2Trend+180 S3Plunge S3Trend+180 Variance];
            %ste = [bvg(:,2) bvg(:,1)+180  bvg(:,4) bvg(:,3)+180 bvg(:,6) bvg(:,5)+180 bvg(:,7) ];
            ste = [values.S1Plunge values.S1Trend+180  values.S2Plunge values.S2Trend+180 values.S3Plunge values.S3Trend+180 values.Variance ];
            sor = ste;
            % sor : [S1Plunge S1Trend+270 S2Plunge S2Trend+180 S3Plunge S3Trend+180 Variance];
            sor(:,SA*2) = sor(:,SA*2)+90;

            % Create matrices
            normlap2=NaN(height(values),1);

            valueMap=normlap2;%reshape(normlap2,length(yvect),length(xvect));
            %s11 = valueMap;

            % Create figure
            % fig=figure('visible','off');
            l_normal =  ste(:,1) > 52 &   ste(:,5) < 35 ;
            l_notnormal = l_normal < 1;
            ax=axes(figure);
            plq = quiver(ax,values.x(l_notnormal),values.y(l_notnormal),-cos(sor(l_notnormal,SA*2)*pi/180),sin(sor(l_notnormal,SA*2)*pi/180),0.6,'.');
            set(plq,'LineWidth',0.5,'Color','k')
            px = get(plq,'Xdata');
            py = get(plq,'Ydata');

            % fig=figure('visible','off')
            ax=axes(figure);
            plq_n = quiver(ax,values.x(l_normal),values.y(l_normal),-cos(sor(l_normal,SA2*2)*pi/180),sin(sor(l_normal,SA2*2)*pi/180),0.6,'.');
            set(plq_n,'LineWidth',0.5,'Color','r')

            drawnow
            px_n = get(plq_n,'Xdata');
            py_n = get(plq_n,'Ydata');
            %close

            fig=figure('Name','Faulting style map','pos',[100 100 860 600]);
            watchon;
            %whitebg(gcf);
            set(fig,'color','w');
            ax=axes('pos',[0.12 0.12 0.8 0.8]);
            set(ax,'NextPlot','add')
            n = 0;
            l0 = []; l1 = []; l2 = []; l3 = []; l4 = []; l5 = [];

            ste = sor(l_notnormal,:);
            for i = 1:3:length(px)-1
                n = n+1;
                j = jet;
                col = floor(ste(n,SA*2-1)/60*62)+1;
                if col > 64
                    col = 64;
                end
                pl = line(ax,px(i:i+1),py(i:i+1),'Linewidth',1,'Markersize',1);
                ax.NextPLot = 'add';

                dx = px(i)-px(i+1);
                dy = py(i) - py(i+1);
                pl2 = line(ax, px(i),py(i),'Marker','o','Markersize',0.1,'Linewidth',0.5);
                l0 = pl2;
                pl3 = line(ax,[px(i) px(i)+dx],[py(i) py(i)+dy],'Linewidth',1);

                % Select faulting style according to Zoback(1992)
                thecolor = [0 0 0];
                if ste(n,1) < 40  && ste(n,3) > 45  && ste(n,5) < 20
                    thecolor = [0.2 0.8 0.2];
                    l3 = pl;
                end
                if ste(n,1) < 20  && ste(n,3) > 45  && ste(n,5) < 40
                    thecolor =[0.2 0.8 0.2];
                    l3 = pl;
                end
                if ste(n,1) > 40  &&                 ste(n,1) < 52  && ste(n,5) < 20
                    thecolor = [1 0 1];
                    l2 = pl;
                end
                if ste(n,1) < 20  &&                 ste(n,5) > 40  &&  ste(n,5) <  52
                    thecolor = [0 1 1];
                    l4 = pl;
                end
                if ste(n,1) < 37  && ste(n,5) > 47
                    thecolor = [0 0 1];
                    l5 = pl;
                end
                set([pl pl2 pl3], 'color', thecolor);

            end
            %drawnow
            ste = sor(l_normal,:);
            n = 0;

            for i = 1:3:length(px_n)-1
                n = n+1;j = jet;
                col = floor(ste(n,SA*2-1)/60*62)+1;
                if col > 64 ; col = 64; end
                dx_n = px_n(i)-px_n(i+1);
                dy_n= py_n(i) - py_n(i+1);
                pl_n = plot(ax, px_n(i:i+1),py_n(i:i+1),'k','Linewidth',1,'Markersize',1,'color',[ 0 0 0  ] );
                set(ax,'NextPlot','add')
                dx = px_n(i)- px_n(i+1);
                dy = py_n(i) - py_n(i+1);
                pl2_n = plot(ax, px_n(i),py_n(i),'ko','Markersize',0.1,'Linewidth',0.5,'color',[0 0 0] );
                l0 = pl2;
                pl3_n = plot(ax, [px_n(i) px_n(i)+dx_n],[py_n(i) py_n(i)+dy_n],'k','Linewidth',1,'color',[0 0 0] );

                if ste(n,1) > 52  &&                 ste(n,5) < 35
                    set([pl_n pl3_n],'color','r');
                    set(pl2_n,'color','r');
                    l1 = pl_n;
                end
            end

            if isempty(l1)
                pl2 = plot(px,py,'kx','Linewidth',1,'color','r'); l1 = pl2; %set(l1,'visible','off');
            end
            if isempty(l2)
                pl2 = plot(px,py,'kx','Linewidth',1,'color','m'); l2 = pl2; %set(l2,'visible','off');
            end
            if isempty(l3)
                pl2 = plot(px,py,'kx','Linewidth',1,'color',[0.2 0.8 0.2] ); l3 = pl2; %set(l3,'visible','off');
            end
            if isempty(l4)
                pl2 = plot(px,py,'kx','Linewidth',1,'color','c' ); l4 = pl2;% set(l4,'visible','off');
            end
            if isempty(l5)
                pl2 = plot(px,py,'kx','Linewidth',1,'color','b' ); l5 = pl2; %set(l5,'visible','off');
            end
            if isempty(l0)
                l0 = plot(px,py,'kx','Linewidth',1,'color',[0 0 0 ] );  % set(l0,'visible','off');
            end

            try
                legend([l1 l2 l3 l4 l5 l0],'NF','NS','SS','TS','TF','U');
            catch
                disp('Legend could not be drawn')
            end

            % Figure settings
            set(ax,'NextPlot','add')
            axis(ax, 'equal')
            % zmap_update_displays();
            set(ax,'PlotBoxAspectRatio',[0.827 1 1])
            %axis(ax,[ s2_west s1_east s4_south s3_north])
            title(ax,sprintf('%s;  %g to %g', name, t0b, teb),'FontSize',ZmapGlobal.Data.fontsz.s,...
                'Color','k','FontWeight','normal');
            xlabel(ax,'Longitude ','FontWeight','normal','FontSize',ZmapGlobal.Data.fontsz.s)
            ylabel(ax,'Latitude ','FontWeight','normal','FontSize',ZmapGlobal.Data.fontsz.s)
            set(ax,'visible','on','FontSize',ZmapGlobal.Data.fontsz.s,'FontWeight','normal',...
                'FontWeight','normal','LineWidth',1,...
                'Box','on','TickDir','out','Ticklength',[0.01 0.01])

            watchoff;

            % View the variance map
            valueMap = r;
            ZG.shading_style = 'interp';
            set(ax,'NextPlot','add')

            obj.add_grid_centers();

            for n=1:numel(obj.features)
                ft=obj.ZG.features(obj.features{n});
                copyobj(ft,ax);
            end

        end
    end
    methods(Static)

        function h=  AddMenuItem(parent, zapFcn, varargin)
            % create a menu item
            label = 'Map Stress Tensor';
            h = uimenu(parent, 'Label', label,...
                'MenuSelectedFcn', @(~,~)XYfun.stressgrid(zapFcn()),...
                varargin{:});
        end
    end % static methods
end
%{
function bvg = stressgrid
    % stressgrid create a grid (interactively), calculate stress tensor using Michaels or Gephards code
    %
    % Incoming data:
    % requires catalog contains
    %   * DipDirection (East of North)
    %   * Dip,
    %   * Rake (Kanamori Convention)
    %
    % original: Stefan Wiemer 1/95
    %
    % updated by: J. Woessner
    % turned into function by Celso G Reyes 2017

    ZG=ZmapGlobal.Data; % used by get_zmap_globals

    report_this_filefun();
    fs=filesep;

    % get the grid parameter
    % initial values
    %

    dx = 0.1;
    dy = 0.1 ;
    ni = 50;
    ra = ZG.ra;
    Nmin = 0;

    zdlg = ZmapDialog();
    zdlg.AddPopup('calcmethod','Calc Method',{'Michaels Method'},1,...
        'Choose the only method that is available. (sorry, no other options)');

    zdlg.AddEventSelector('EventSelector', obj.EventSelector)
    [res, okpressed]=zdlg.Create('Name', 'Stress grid');
    if ~okpressed
        return
    end
    disp(res)
    Grid=ZG.gridopt;
    EventSelector=res.EventSelector;

    origdir=pwd;
    try
    my_calculate();
    catch ME
        cd(origdir)
        rethrow ME
    end

end
%}


function varargout=ceil(varargin)
%CEIL (overloaded)

switch class(varargin{1})

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.

        x = varargin{1};

        dim = size(x);
        x = reshape(x,prod(dim),1);
        y = [];
        for i = 1:prod(dim)
            y = [y;yalmip('addextendedvariable',mfilename,extsubsref(x,i))];
        end
        y = reshape(y,dim);
        varargout{1} = y;

    case 'char' % YALMIP send 'graph' when it wants the epigraph or hypograph
        switch varargin{1}
            case {'milp','graph'}
                % Description using epigraphs
                t = varargin{2};
                X = varargin{3};

                c = intvar(1,1);
                F = (x <= c <= x + 1);

                varargout{1} = F;
                varargout{2} = struct('convexity','milp','monotonicity','milp','definiteness','milp');
                varargout{3} = X;

            otherwise
                error('SDPVAR/SORT called with CHAR argument?');
        end
    otherwise
        error('Strange type on first argument in SDPVAR/SORT');
end


function [locate_voi_min_x,locate_voi_max_x,locate_voi_min_y,locate_voi_max_y,locate_voi_min_z,locate_voi_max_z] = dicomrt_voiboundaries(dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use,PatientPosition)
% dicomrt_voiboundaries(dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use,PatientPosition)
%
% Returns the voxel index of the smallest and largest coordinate of the selected VOI in the specified Z slice.
%
% slice is the slice number where the mins and maxs will be calculated.
%      Slice number is relative to voiref.
% voiref is the voi of reference
% dose_xmesh,dose_ymesh, are coordinates of the center of the dose-pixel
% VOI is a cell array which contain the patients VOIs as read by dicomrt_loadvoi
% voi2use is a vector pointing to the number of VOIs to be used
%
% Example:
%
% [xmin,xmax,ymin,ymax]=dicomrt_voiboundariesZ(12,1,xmesh,xmesh,VOI,5)
%
% returns in xmin,xmax,ymin,ymax the pixel numbers which correspond to the edges of
% structure "5" (e.g. target) in slice number 12 with reference to structure number "1".
%
% See also dicomrt_voiboundaries, dicomrt_surfdose
%
% Copyright (C) 2002 Emiliano Spezi (emiliano.spezi@physics.org)

% Check input
[VOI_temp]=dicomrt_checkinput(VOI);
VOI=dicomrt_varfilter(VOI_temp);

nslices=size(VOI{voi2use,2},1);

voiref=1;

for kk=1:nslices
    [locate_voi_min_1,locate_voi_max_1,locate_voi_min_2,locate_voi_max_2]=dicomrt_voiboundaries_single(kk,dose_xmesh,dose_ymesh,VOI_temp,voi2use,PatientPosition);
    if kk==1
        locate_voi_min_x=locate_voi_min_1;
        locate_voi_max_x=locate_voi_max_1;
        locate_voi_min_y=locate_voi_min_2;
        locate_voi_max_y=locate_voi_max_2;
    else
        if locate_voi_min_1 < locate_voi_min_x
            locate_voi_min_x=locate_voi_min_1;
        end
        if locate_voi_max_1 > locate_voi_max_x
            locate_voi_max_x=locate_voi_max_1;
        end
        if locate_voi_min_2 < locate_voi_min_y
            locate_voi_min_y=locate_voi_min_2;
        end
        if locate_voi_max_2 > locate_voi_max_y
            locate_voi_max_y=locate_voi_max_2;
        end
    end
end

% z mesh is always sorted
min_z_voi=VOI_temp{2,1}{voi2use,2}{1}(1,3);
max_z_voi=VOI_temp{2,1}{voi2use,2}{end}(1,3);

locate_voi_min_z=dicomrt_findpointVECT(dose_zmesh,min_z_voi);
locate_voi_max_z=dicomrt_findpointVECT(dose_zmesh,max_z_voi);


function [distAll, scoresAll, pointsAll] = assignGTinference(keypointsAll,annolist,pidxsAll,parts,thresh_gt,marg_scores,nTrials)

if (nargin < 5)
    thresh_gt = 0.5;
end

scoresAll = cell(length(pidxsAll),1);
pointsAll = cell(length(pidxsAll),1);
distAll = cell(length(pidxsAll),1);

for i = 1:length(pidxsAll)
    scoresAll{i} = cell(length(annolist),1);
    pointsAll{i} = cell(length(annolist),1);
    distAll{i} = cell(length(annolist),1);
end

for imgidx = 1:length(annolist)

%     figure(200); clf; imagesc(imread(annolist(imgidx).image.name));
%     hold on; axis equal;

    dist = cell(2,1);

    det_c = {'g','b'};
    for detidx = 1:nTrials

        dist{detidx} = inf(length(pidxsAll),length(annolist(imgidx).annorect));
        for i = 1:length(pidxsAll)
            pidx = pidxsAll(i);
            % part is a joint
            assert(parts(pidx+1).pos(1) == parts(pidx+1).pos(2));
            jidx = parts(pidx+1).pos(1);

            det = keypointsAll(imgidx).det{jidx+1};
            if (~isempty(det))
                [val,id] = max(det(:,4+detidx));
                pp = det(id,1:2);
            else
                val = -inf;
                pp = [0 0];
            end
            pointsAll{i}{imgidx} = [pointsAll{i}{imgidx}; pp];
            if (marg_scores)
                scoresAll{i}{imgidx} = [scoresAll{i}{imgidx}; val];
            else
                scoresAll{i}{imgidx} = [scoresAll{i}{imgidx}; det(id,3)];
            end

            for ridx = 1:length(annolist(imgidx).annorect)
                rect = annolist(imgidx).annorect(ridx);
                refDist = util_get_head_size(rect);
                if (isfield(rect, 'annopoints') && isfield(rect.annopoints, 'point'))
                    p = util_get_annopoint_by_id(rect.annopoints.point, jidx);
                    if (~isempty(p))
                        dist{detidx}(i,ridx) = norm([p.x p.y] - pp)/refDist;
%                         plot(p.x,p.y,'ro','MarkerFaceColor','y','MarkerEdgeColor','k','MarkerSize',10);
                    else
                        dist{detidx}(i,ridx) = nan;
                    end
                end
            end
%             plot(pp(1),pp(2),'ro','MarkerFaceColor',det_c{detidx},'MarkerEdgeColor','k','MarkerSize',10);
        end
    end

    for detidx = 1:2
        pckSum = zeros(1,size(dist{detidx},2));
        idxsNotNaN = ~isnan(dist{detidx});
        for i=1:size(dist{detidx},2)
            pckSum(i) = sum(dist{detidx}(idxsNotNaN(:,i),i) <= thresh_gt);
        end
        [val,idx] = max(pckSum);
        idx2 = setdiff(1:size(dist{detidx},2),idx);
        dist{detidx}(idxsNotNaN(:,idx2),idx2) = inf;
        for i = 1:length(pidxsAll)
            distAll{i}{imgidx} = [distAll{i}{imgidx}; dist{detidx}(i,:)];
        end
    end

end

end


function test_pull434

% WALLTIME 00:10:00
% MEM 1gb
% DEPENDENCY

% this script tests the appending of sensor structures
% and specifically the handling of overlapping channels and/or electrodes
% see https://github.com/fieldtrip/fieldtrip/pull/434
% and the preceding https://github.com/fieldtrip/fieldtrip/pull/428

%% construct some electrode structures

elec1.label = {'1'; '2'; '3'};
elec1.elecpos = [1 1 1; 2 2 2; 3 3 3];
elec1.chanpos = [1 1 1; 2 2 2; 3 3 3];
elec1.tra = eye(3);

elec2.label = {'3'; '4'};
elec2.elecpos = [3 3 3; 4 4 4];
elec2.chanpos = [3 3 3; 4 4 4];
elec2.tra = eye(2);

elec3.label = {'4'; '5'; '6'};
elec3.elecpos = [4 4 4; 5 5 5; 6 6 6];
elec3.chanpos = [4 4 4; 5 5 5; 6 6 6];
elec3.tra = eye(3);

%% append them in different ways

cfg = [];

append1   = ft_appendsens(cfg, elec1);
append11  = ft_appendsens(cfg, elec1, elec1);
append111 = ft_appendsens(cfg, elec1, elec1, elec1);

assert(isequal(append1.label,   elec1.label));
assert(isequal(append1.chanpos,   elec1.chanpos));
assert(isequal(append1.tra,   elec1.tra));
assert(isequal(append11.label,   elec1.label));
assert(isequal(append11.chanpos,   elec1.chanpos));
assert(isequal(append11.tra,   elec1.tra));
assert(isequal(append111.label,   elec1.label));
assert(isequal(append111.chanpos,   elec1.chanpos));
assert(isequal(append111.tra,   elec1.tra));

%%

append13  = ft_appendsens(cfg, elec1, elec3);
append12  = ft_appendsens(cfg, elec1, elec2);
append23  = ft_appendsens(cfg, elec2, elec3);

assert(numel(append13.label)==6);
assert(numel(append12.label)==4);
assert(numel(append23.label)==4);

assert(size(append13.chanpos,1)==6);
assert(size(append12.chanpos,1)==4);
assert(size(append23.chanpos,1)==4);

assert(size(append13.elecpos,1)==6);
assert(size(append12.elecpos,1)==4);
assert(size(append23.elecpos,1)==4);

assert(isequal(size(append13.tra), [6 6]));
assert(isequal(size(append12.tra), [4 4]));
assert(isequal(size(append23.tra), [4 4]));

%%

append123 = ft_appendsens(cfg, elec1, elec2, elec3);

assert(numel(append123.label)==6);
assert(size(append123.chanpos,1)==6);
assert(size(append123.elecpos,1)==6);
assert(isequal(size(append123.tra), [6 6]));

%%

elec1b.label = {'1b', '2b', '3b'};
elec1b.elecpos = [1 1 1; 2 2 2; 3 3 3];      % same electrodes
elec1b.chanpos = [1 1 1; 2 2 2; 3 3 3] + 10; % different channels
elec1b.tra = eye(3);

elec1c.label = {'1c', '2c', '3c'};
elec1c.elecpos = [1 1 1; 2 2 2; 3 3 3] + 10; % different electrodes
elec1c.chanpos = [1 1 1; 2 2 2; 3 3 3];      % same channels
elec1c.tra = eye(3);

%%

append11b = ft_appendsens(cfg, elec1, elec1b); % same elec, diff chan

assert(numel(append11b.label)==6);
assert(size(append11b.chanpos,1)==6);
assert(size(append11b.elecpos,1)==3);
assert(isequal(size(append11b.tra), [6 3]));

%%

if false
append11c = ft_appendsens(cfg, elec1, elec1c); % same chan, diff elec
% this should crash because there are 6 labels and only 3 chanpos
end


function [ state ] =movePTP_ConditionalTorque_ArcXZ_AC(t_Kuka,theta,c,VEL,joints_indices,max_torque,min_torque)
%% This function is used for moving the endeffector on an arc in the XZ plane, for the KUKA iiwa 7 R 800.
% this motion is interruptible when one or more of the joints-torques exceed the predefined limits

%% Syntax:
% [ state ] =movePTP_ConditionalTorque_ArcXZ_AC(t_Kuka,theta,c,VEL,joints_indices,max_torque,min_torque)

%% About:
% This function is used to move the end-effector on an arc in the XZ plane,

%% Arreguments:
% t_Kuka: is the TCP/IP connection
% theta: is the arc angle, in radians
% c: the XZ coordinates of the center of the circle, it is 1x2 vector.
% VEL : is a double, defines the motion velocity mm/sec.
% joints_indices: is a vector of the indices of the joints where the
% torques limits are to be imposed, the joints are indexed starting from one.
% max_torque: is a vector of the maximum torque limits for the joints
% specified in the (joints_indices) vector.
% min_torque: is a vecotr of the minimum torque limits for the joints
% specified in the (joints_indices) vector.

%% Return value:
% state: a number equals to one if the motion is completed sccessfully or a
% zero if the motion was interrupted due to external contact with the
% robot.
% if there is an error, the return value is minus one

% Copy right, Mohammad SAFEEA, 9th of April 2018

if((size(VEL,1)==1)&&(size(VEL,2)==1))
else
    disp('Error, velocity is a double and shall not be an array');
    state=-1;
    return;
end

k=[0;1;0];
pos=getEEFPos( t_Kuka );
c=colVec(c);
c1=[c(1);pos{2};c(2)];
[ state ] =movePTP_ConditionalTorque_Arc_AC(t_Kuka,theta,c1,k,VEL,joints_indices,max_torque,min_torque);

end

function [ y ] = colVec( v)
% Convert a vector to a column vector:
    if(size(v,2)==1)
        y=v;
    else
        y=v';
    end
end


classdef TestAdaptiveThreshold
    %TestAdaptiveThreshold

    properties (Constant)
        im = fullfile(mexopencv.root(),'test','sudoku.jpg');
    end

    methods (Static)
        function test_1
            img = cv.imread(TestAdaptiveThreshold.im, ...
                'Grayscale',true, 'ReduceScale',2);
            out = cv.adaptiveThreshold(img);
            validateattributes(out, {class(img)}, {'size',size(img)});
        end

        function test_2
            img = cv.imread(TestAdaptiveThreshold.im, ...
                'Grayscale',true, 'ReduceScale',2);
            out = cv.adaptiveThreshold(img, 'Method','Gaussian', ...
                'Type','BinaryInv', 'BlockSize',7, 'C',1, 'MaxValue',255);
            validateattributes(out, {class(img)}, {'size',size(img)});
        end

        function test_error_argnum
            try
                cv.adaptiveThreshold();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


 function y = mtimes(ob, x)
%function y = mtimes(ob, x)
% y = G * x	or x = G' * y
% Copyright 2002-2-20, Jeff Fessler, The University of Michigan

%
% scalar * G
%
if isa(ob, 'double') & length(x) == 1 & isa(x, 'Gtomo2_dsc')
	y = x;
	y.scale = ob;
	return
end

if ob.apower ~= 1, error notdone, end

%
% full forward projection
%
if ~ob.is_transpose

	% if needed, expand concise column(s)
	if ob.is_masked
		idim = size(x);
		np = sum(ob.mask(:));
		nxy = numel(ob.mask);
		if idim(1) ~= np
			error 'size mismatch'
		end
		x = embed(x, ob.mask);
		x = reshape(x, nxy, idim(2));
	end

	y = wtfmex('dsc,proj', ob.arg', single(x), uint8(ob.mask), ...
		int32(0), int32(1), int32(ob.nthread), int32(ob.chat));

%
% full back-projection
%
else
	y = wtfmex('dsc,back', ob.arg', single(x), uint8(ob.mask), ...
		int32(0), int32(1), int32(ob.nthread), int32(ob.chat));

	if ob.is_masked
		if size(y,1) == numel(ob.mask)
			y = y(ob.mask,:);	% [nxy,nz] -> [np,nz]
		else
			y = y(ob.mask);		% [nx,ny] -> [np]
		end
	end
end

y = ob.scale * double(y);	% trick: no '(:)' here; let wtfmex do the job!


function catalogobj = associate(obj, maxTimeDiff, sites, source)
%ASSOCIATE Associate detections into events
% catalogobj = associate(detectionObj, maxTimeDiff) will scan through an
% Detection object and look
% for times where there are at least 2 detections on
% within maxTimeDiff seconds of each other, and declare an event. No
% checking is done to see if arrivals are on different channels
%
% catalogobj = associate(arrivals, maxTimeDiff, sites) will
% reduce the detection times by traveltimes in the sites structure. This
% allows a smaller maxTimeDiff to be used (since all airwaves should now
% line up, and all P waves etc.).
%
% Example: Imagine you have run dbdetect. You also notice that the time to cross
% the network is less than 30s. Then the following code will load the detections, subset
% for "D" states, and associate into events wherever 2 channels have an "D"
% detection within 30s. The final line returns a Catalog object - the GISMO
% container for multiple events.
%
%   dbpath = '/home/t/thompsong/NEWTON/pavlof2007';
%   detobj = Detection.retrieve(dbpath, 'status=~/D/');
%   catalogobj = detobj.associate(30);
%
% A significant problem is that there may be more than 1 event in a
% 30-second window. So a different approach is to apply a differential
% travel time correction to each detection, to reduce it to a known source
% location. Then a wave might propagate across the reduced network in a
% time of 1-second, for example, allowing multiple events to be detected in
% a 30-seconds time window.
%
%   dbpath = '/home/t/thompsong/NEWTON/pavlof2007/db';
%   detobj = Detection.retrieve(dbpath, 'status=~/D/');
%   sites = antelope.dbget_site_locations(dbpath, unique(detobj.channelinfo))
%   source.lat = 55.4203;
%   source.lon = -161.8931;
%   wavespeed = 330; % m/s
%   difftt = calc_traveltime(source, sites, wavespeed);
%   source.lat = 55.4173; source.lon = -161.8937; elev = 2.518;
%   chantags = ChannelTag(unique(detobj.channelinfo))
%   sites = antelope.dbget_site_locations(dbpath, chantags, startTime, endTime);
%   seismicspeed = 330; infrasoundspeed = 330; % m/s
%   sites = compute_travel_times(source, sites, seismicspeed, infrasoundspeed);
%   association_time_window = 2; % max seconds from seismic to infrasound arrival
%   catalogobj = detobj.associate(maxtimediff, sites);

    % break up detection types
    detections_on = obj.subset('state', 'ON');
    detections_d = obj.subset('state', 'D');
    detections_off = obj.subset('state', 'OFF');
    if detections_d.numel > 0
        obj = detections_d;
    elseif detections_on.numel > 0
        obj = detections_on;
    end

    %% REDUCE BY SUBTRACTING TRAVEL TIME
    % If sites exist, let's correct the arrival times first
    if exist('sites', 'var')
        disp('Reducing travel times')
        obj.traveltime = NaN(size(obj.time));
        for c=1:numel(sites)
            thissite = sites(c);
            thischanstr = thissite.channeltag.string();
            i = strcmp(obj.channelinfo, thischanstr)==1;
            obj.traveltime(i) = sites(c).traveltime;
            obj.time(i) = obj.time(i) - sites(c).traveltime/86400;
        end
    end
    % sort obj in ascending time order
    disp('Sorting')
    [detection_time,indices]=sort(obj.time);
    obj = obj.subset(indices);

    % First we begin with each detection and find how many detections occur in the
    % maxTimeDiff seconds that follow. This way each arrival is assigned a
    % weight equal to that number of detections. The point of this is to help us
    % identify the beginning of each event.
    fprintf('\nFinding how many detections within %e sec of each detection\n',maxTimeDiff)
    for c=1:numel(detection_time)
        associated_indices{c} = find(detection_time>=detection_time(c) & detection_time<=detection_time(c)+maxTimeDiff/86400);
        numdetections(c) = numel(associated_indices{c});
    end

    % remove decrementing series, e.g. replace sequence
    % like 6 5 4 3 2 1 with 6 0 0 0 0 0
    disp('Removing decrementing series')
    numdetections = [numdetections(1) diff(numdetections)+1];
    %numdetections(numdetections<=1)=0;

    % Now loop over numdetections and create events
    fprintf('Making events')
    eventnum = 0;
    duplicatecount = 0;
    for c=1:numel(numdetections)
        fprintf('.')
        if numdetections(c)>0
            fprintf('*')
            eventnum = eventnum + 1;
            otime(eventnum) = detection_time(c);
    %         lastDetectionTime(eventnum) = detection_time(c + numdetections(c) - 1);

            try
                eventdetobj = obj.subset([c : c + numdetections(c) - 1]);
            catch
                numdetections(c)
                c+numdetections(c)-1
                numel(numdetections)
                rethrow();
            end

            % remove the reduced time
            if exist('sites','var')
                for cc=1:numel(eventdetobj.time)
                    eventdetobj.time(cc) = eventdetobj.time(cc) + eventdetobj.traveltime(cc)/86400;
                end
            end

            % remove duplicate channels
            [uc,ia]  = unique(eventdetobj.channelinfo, 'stable');
            duplicatecount = duplicatecount + numel(eventdetobj.channelinfo) - numel(uc);
            eventdetobj = eventdetobj.subset(ia);

            % set the arrivalobj for this event
            arrivalobj{eventnum} = detection2arrival(eventdetobj);
            firstDetectionTime(eventnum) = min(eventdetobj.time);
            lastDetectionTime(eventnum) = max(eventdetobj.time);

        end

        if mod(c,30) == 0
            fprintf('\nProcessed %d out of %d\n',c, numel(numdetections));
        end
    end

    %% CREATE CATALOG
    if numel(otime)==0
        % no events
        catalogobj = Catalog();
        return
    end

    fprintf('\nCreating Catalog\n')
    olon=[];
    olat=[];
    if exist('source','var')
        olon = source.lon*ones(size(otime));
        olat = source.lat*ones(size(otime));
    end
    catalogobj = Catalog(otime, olon, olat, [], [], {}, {}, 'ontime', firstDetectionTime, 'offtime', lastDetectionTime);
    catalogobj.arrivals = arrivalobj;
    fprintf('%d detections were determined to be duplicates using a time window of %.1f seconds\n',duplicatecount, maxTimeDiff);

end

function arrivalobj = detection2arrival(detectionobj)
    if ~isa(detectionobj.channelinfo,'ChannelTag')
        ctag = ChannelTag(detectionobj.channelinfo);
    else
        ctag = detectionobj.channelinfo;
    end
    sta = get(ctag, 'station');
    chan = get(ctag, 'channel');
    arrivalobj = Arrival(cellstr(sta), cellstr(chan), detectionobj.time, ...
        cellstr(detectionobj.state), 'signal2noise', detectionobj.signal2noise);

end

% function result = alreadyHaveArrivalFromThisChannel(ctaglist, thisctag)
%   result = sum(cellfun(@(s) ~isempty(strfind(this
% end


function net = resnet52_new_hope_RankLoss()

%----------------------load pretrained model----------------------
netStruct = load('./data/vgg19_coco_batch32_pool_shift_both_drop0.5/net-epoch-70.mat') ;
net = dagnn.DagNN.loadobj(netStruct.net) ;

for i = 1:36 %img cnn
    if(mod(i,2)==0)
        net.params(i).learningRate= 0.02;
    else net.params(i).learningRate= 0.001;
    end
    net.params(i).weightDecay=1;
end
net.params(1).learningRate = 0.0001;

net.addLayer('lrn1',dagnn.LRN('param',[4096,1e-8,1,0.5]),{'fc1_1bnx'},{'fc1bn_n'},{});
net.addLayer('lrn2',dagnn.LRN('param',[4096,1e-8,1,0.5]),{'fc5_2bnx'},{'fc2bn_n'},{});

%--for get harder sample
%net.addLayer('Multiple',dagnn.Multiple(),{'fc1bn_n','fc2bn_n'},{'Score'},{});

lossBlock = dagnn.RankLoss('rate',1);
net.addLayer('RankLoss',lossBlock,{'fc1bn_n','fc2bn_n'},'objective_f');

%net.conserveMemory = false;
%net.eval({'data',single(rand(224,224,3)),'data2',single(rand(1,1,20074))});
end


function M1 = spm_eeg_inv_datareg(S)
% Co-registration of two sets of fiducials according to sets of
% corresponding points and (optionally) headshapes.
% rigid co-registration
%           1: fiducials based (3 landmarks: nasion, left ear, right ear)
%           2: surface matching between sensor mesh and headshape
%           (starts with a type 1 registration)
%
% FORMAT M1 = spm_eeg_inv_datareg(S)
%
% Input:
%
% S  - input struct
% fields of S:
%
% S.sourcefid  - EEG fiducials (struct)
% S.targetfid = MRI fiducials
% S.template  - 1 - input is a template (for EEG)
%               0 - input is an individual head model
%               2 - input is a template (for MEG) - enforce uniform scaling
%
% S.useheadshape - 1 use headshape matching 0 - don't
%
%
% Output:
% M1 = homogenous transformation matrix
%
% If a template is used, the senor locations are transformed using an
% affine (rigid body) mapping.  If headshape locations are supplied
% this is generalized to a full twelve parameter affine mapping (n.b.
% this might not be appropriate for MEG data).
%__________________________________________________________________________
% Copyright (C) 2005-2017 Wellcome Trust Centre for Neuroimaging

% Jeremie Mattout
% $Id: spm_eeg_inv_datareg.m 7544 2019-03-15 16:20:16Z vladimir $

if ~isfield(S, 'targetfid')
    error('Target fiducials are missing');
else
    targetfid = ft_convert_units(S.targetfid, 'mm');
end

if ~isfield(S, 'sourcefid')
    error('Source are missing');
else
    sourcefid = ft_convert_units(S.sourcefid, 'mm');
    [sel1, sel2] = spm_match_str(targetfid.fid.label, sourcefid.fid.label);
    sourcefid.fid.pnt = sourcefid.fid.pnt(sel2, :);
    sourcefid.fid.label = sourcefid.fid.label(sel2);

    targetfid.fid.pnt = targetfid.fid.pnt(sel1, :);
    targetfid.fid.label = targetfid.fid.label(sel1);
end

if ~isfield(S, 'template')
    S.template = 0;
end

% Estimate-apply rigid body transform to sensor space
%--------------------------------------------------------------------------
M1 = spm_eeg_inv_rigidreg(targetfid.fid.pnt', sourcefid.fid.pnt');

sourcefid = ft_transform_geometry(M1, sourcefid);

if S.template

    % constrained affine transform
    %--------------------------------------------------------------------------
    aff   = S.template;
    for i = 1:64

        % scale
        %----------------------------------------------------------------------
        M       = pinv(sourcefid.fid.pnt(:))*targetfid.fid.pnt(:);
        M       = sparse(1:4,1:4,[M M M 1]);

        sourcefid = ft_transform_geometry(M, sourcefid);

        M1      = M*M1;

        % and move
        %----------------------------------------------------------------------
        M       = spm_eeg_inv_rigidreg(targetfid.fid.pnt', sourcefid.fid.pnt');

        sourcefid = ft_transform_geometry(M, sourcefid);

        M1      = M*M1;

        if (norm(M)-1)< eps
            break;
        end
    end
else
    aff = 0;
end

% Surface matching between the scalp vertices in MRI space and the
% headshape positions in data space
%--------------------------------------------------------------------------
if ~isempty(sourcefid.pnt) && S.useheadshape

    headshape = sourcefid.pnt;
    scalpvert = targetfid.pnt;

    % load surface locations from sMRI
    %----------------------------------------------------------------------
    if size(headshape,2) > size(headshape,1)
        headshape = headshape';
    end
    if size(scalpvert,2) > size(scalpvert,1)
        scalpvert = scalpvert';
    end

    % intialise plot
    %----------------------------------------------------------------------
    if ~spm('CmdLine')
        h    = spm_figure('GetWin','Graphics');
        spm_figure('Select',h);
        spm_clf(h);
        Fmri = plot3(scalpvert(:,1),scalpvert(:,2),scalpvert(:,3),'ro','MarkerFaceColor','r');
        hold on;
        Fhsp = plot3(headshape(:,1),headshape(:,2),headshape(:,3),'bs','MarkerFaceColor','b');
        axis off image
        drawnow
    else
        Fmri = [];
        Fhsp = [];
    end

    % nearest point registration
    %----------------------------------------------------------------------
    M = spm_eeg_inv_icp(scalpvert',headshape',targetfid.fid.pnt',sourcefid.fid.pnt',Fmri,Fhsp,aff);

    % transform headshape and eeg fiducials
    %----------------------------------------------------------------------
    sourcefid = ft_transform_geometry(M, sourcefid);
    M1        = M*M1;
end


% ==============================================================================
%
%    Software License Agreement (BSD License)
%    Copyright (c) 2019
%    (www.aimlab.wpi.edu)
%
%    All rights reserved.
%
%    Redistribution and use in source and binary forms, with or without
%    modification, are permitted provided that the following conditions
%    are met:
%
%    * Redistributions of source code must retain the above copyright
%    notice, this list of conditions and the following disclaimer.
%
%    * Redistributions in binary form must reproduce the above
%    copyright notice, this list of conditions and the following
%    disclaimer in the documentation and/or other materials provided
%    with the distribution.
%
%    * Neither the name of authors nor the names of its contributors may
%    be used to endorse or promote products derived from this software
%    without specific prior written permission.
%
%    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
%    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
%    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
%    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
%    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
%    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
%    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
%    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
%    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
%    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
%    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
%    POSSIBILITY OF SUCH DAMAGE.
%
%    \author:    <http://www.aimlab.wpi.edu>
%    \author:    <amunawar@wpi.edu>
%    \author:    Adnan Munawar
%    \version:   0.1$
% ==============================================================================

function [nIterations,sizePath,run_time] =  RRTconnect3D(dim,segmentLength,random_world,show_output)
% dim = 2;
% segmentLength = 5;
% random_world = 0;
% standard length of path segments
if dim ==2
    start_cord = [5,5];
    goal_cord = [95,95];

else

    start_cord = [5,5,5];
    goal_cord = [95,95,95];
end

% create random world
Size = 100;
NumObstacles = 100;

if random_world ==1
    world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
    [world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end
% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];
% establish tree starting with the start node
tree = start_node;

a = clock;

% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...
        &&(collision(start_node,end_node,world,dim)==0) )
    path = [start_node; end_node];
else
    nIterations = 0;
    numPaths = 0;
    flag = 0;
    while numPaths<1,
        [tree,flag] = extendTree(tree,end_node,segmentLength,world,dim);
        numPaths = numPaths + flag;
        nIterations = nIterations+1;
    end
end

path = findMinimumPath(tree,end_node,dim);
sizePath = size(path,1);

b = clock;

run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));

if show_output == 1
    % find path with minimum cost to end_node
    figure;
    plotExpandedTree(world,tree,dim);
    plotWorld(world,path,dim);
    % figure(2);
    % plotWorld(world,path,dim);
    %plotExpandedTree(world,tree,dim);
end
end

function world = createWorld(NumObstacles, endcorner, origincorner,dim)

if dim == 2

    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2));
        maxRadius = 5*maxRadius/NumObstacles/2;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
        end
    end

elseif dim ==3;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];
        maxRadius = min(bounds);
        maxRadius = 5*maxRadius/NumObstacles;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            cz = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
            world.cz(i) = cz;
        end
    end
end
end

function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
if dim == 2
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
        disp('Not valid corner specifications!')
        world=[];
        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;

        world.radius(2) = maxRadius;
        cx = 75;
        cy = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;

        world.radius(3) = maxRadius;
        cx = 25;
        cy = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        world.cx(4) = cx;
        world.cy(4) = cy;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        world.cx(5) = cx;
        world.cy(5) = cy;
    end

elseif dim == 3

    NumObstacles = 9;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        cz = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;
        world.cz(1) = cz;

        world.radius(2) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;
        world.cz(2) = cz;

        world.radius(3) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;
        world.cz(3) = cz;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 75;
        world.cx(4) = cx;
        world.cy(4) = cy;
        world.cz(4) = cz;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 25;
        world.cx(5) = cx;
        world.cy(5) = cy;
        world.cz(5) = cz;

        world.radius(6) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 25;
        world.cx(6) = cx;
        world.cy(6) = cy;
        world.cz(6) = cz;

        world.radius(7) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 25;
        world.cx(7) = cx;
        world.cy(7) = cy;
        world.cz(7) = cz;

        world.radius(8) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 75;
        world.cx(8) = cx;
        world.cy(8) = cy;
        world.cz(8) = cz;

        world.radius(9) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 75;
        world.cx(9) = cx;
        world.cy(9) = cy;
        world.cz(9) = cz;
    end
end
end

function node=generateRandomNode(world,dim)

if dim ==2;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, chi, cost, 0];
    end

elseif dim ==3;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;
    pz       = (world.endcorner(3)-world.origincorner(3))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, pz, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;
        pz       = (world.endcorner(3)-world.origincorner(3))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, pz, chi, cost, 0];
    end

end

end

function collision_flag = collision(node, parent, world,dim)

collision_flag = 0;

for i=1:dim
    if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
        collision_flag = 1;
    end
end

if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end
end
end

function collision_flag = is_point_valid(point, world,dim)

collision_flag = 0;

for i=1:dim
    if (point(i)>world.endcorner(i))||(point(i)<world.origincorner(i))
        collision_flag = 1;
    end
end

if collision_flag == 0 && dim ==2

    p = point(1:dim);
    % check each obstacle
    for i=1:world.NumObstacles,
        if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
            collision_flag = 1;
            break;
        end
    end

elseif collision_flag == 0 && dim ==3
    p = point(1:dim);
    % check each obstacle
    for i=1:world.NumObstacles,
        if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),
            collision_flag = 1;
            break;
        end
    end
end
end

function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
flag = 0;
% check only last node added to tree since others have been checked
if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...
        & (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),
    flag = 1;
end

end

function [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,dim)

flag = 0;
% select a random point
randomPoint = zeros(1,dim);
for i=1:dim
    randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
end

% find leaf on node that is closest to randomPoint
tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;
sqrd_dist = sqr_eucl_dist(tmp,dim);
[min_dist,idx] = min(sqrd_dist);

min_parent_idx = idx;

new_point = tree(idx,1:dim);
new_node = tree(idx,:);

pflag = 0;

while norm(new_point-randomPoint)>0 && pflag==0

    if norm(new_point-randomPoint)<segmentLength

        pflag = collision(randomPoint,tree(min_parent_idx,:),world,dim);
        if pflag == 0

            new_point = randomPoint;
            min_cost = cost_np(tree(min_parent_idx,:),new_point,dim);
            new_node = [new_point,0,min_cost,min_parent_idx];
            tree = [tree;new_node];
            pflag = 1;
            goal_flag = is_goal(new_node,end_node,segmentLength,world,dim);

            if goal_flag == 1;
                tree(end,dim+1)=1;
                flag = 1;
            end
        end

    else

        new_point = (randomPoint-tree(min_parent_idx,1:dim));
        new_point = tree(min_parent_idx,1:dim)+(new_point/norm(new_point))*segmentLength;

        min_cost  = cost_np(tree(min_parent_idx,:),new_point,dim);
        new_node  = [new_point, 0, min_cost, min_parent_idx];

        pflag = collision(new_node,tree(min_parent_idx,:),world,dim);

        if pflag == 0
            tree = [tree ; new_node];
            min_parent_idx = size(tree,1);

            goal_flag = is_goal(new_node,end_node,segmentLength,world,dim);

            if goal_flag == 1;
                tree(end,dim+1)=1;  % mark node as connecting to end.
                pflag = 1;
                flag = 1;
            end
        end
    end
end

new_tree = tree;

end

function goal_flag = is_goal(node,end_node,segmentLength,world,dim)

goal_flag = 0;
if (norm(node(1:dim)-end_node(1:dim))<segmentLength )...
        && (collision(node,end_node,world,dim)==0)
    goal_flag = 1;
end

end

function e_dist = sqr_eucl_dist(array,dim)

sqr_e_dist = zeros(size(array,1),dim);
for i=1:dim

    sqr_e_dist(:,i) = array(:,i).*array(:,i);

end
e_dist = zeros(size(array,1),1);
for i=1:dim

    e_dist = e_dist+sqr_e_dist(:,i);

end

end

%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)

diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)

diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

function [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
end

function path = findMinimumPath(tree,end_node,dim)

% find nodes that connect to end_node
connectingNodes = [];
for i=1:size(tree,1),
    if tree(i,dim+1)==1,
        connectingNodes = [connectingNodes ; tree(i,:)];
    end
end

% find minimum cost last node
[tmp,idx] = min(connectingNodes(:,dim+2));

% construct lowest cost path
path = [connectingNodes(idx,:); end_node];
parent_node = connectingNodes(idx,dim+3);
while parent_node>1,
    parent_node = tree(parent_node,dim+3);
    path = [tree(parent_node,:); path];
end

end

function plotExpandedTree(world,tree,dim)
ind = size(tree,1);
while ind>0
    size(tree);
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
    while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
    end
    ind = ind - 1;

    if dim == 2
        X = branch(:,1);
        Y = branch(:,2);

        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;

    elseif dim == 3
        X = branch(:,1);
        Y = branch(:,2);
        Z = branch(:,3);

        p = plot3(X,Y,Z);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;
    end
end
end

function plotWorld(world,path,dim)
% the first element is the north coordinate
% the second element is the south coordinate
if dim ==2

    N = 10;
    th = 0:2*pi/N:2*pi;
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2)]);
    hold on

    for i=1:world.NumObstacles,
        X = world.radius(i)*sin(th) + world.cx(i);
        Y = world.radius(i)*cos(th) + world.cy(i);
        fill(X,Y,'blue');
    end

    X = path(:,1);
    Y = path(:,2);
    p = plot(X,Y);

elseif dim ==3
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2),...
        world.origincorner(3), world.endcorner(3)]);
    hold on

    for i=1:world.NumObstacles,
        [X Y Z] = sphere(10);
        X = (X*world.radius(i));
        Y = (Y*world.radius(i));
        Z = (Z*world.radius(i));
        surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));
        colormap([0.5 0.2 0.3]);
    end

    X = path(:,1);
    Y = path(:,2);
    Z = path(:,3);
    p = plot3(X,Y,Z);
end
set(p,'Color','black','LineWidth',3)
xlabel('X axis');
ylabel('Y axis');
zlabel('Z axis');
title('RRT Connect Algorithm');
end