Finish 2010-11-17 12:00:00 UTC

Hi ho Hi ho off to work I go

by Mike Russell

Status: Passed
Results: 3997 (cyc: 20, node: 10868)
CPU Time: 94.126
Score: 872.294
Submitted at: 2010-11-17 12:00:57 UTC
Scored at: 2010-11-17 12:04:37 UTC

Current Rank: 884th (Highest: 707th )
Based on: He's just that good (diff)
Basis for: clean up (diff)
Basis for: Snowwhite (diff)

Comments
Please login or create a profile.
Code
function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)

[thrustRow0, thrustCol0] = nickelfelpeterv2(chart, aIndex, bIndex, maxThrottle);
score0 = runsolution(thrustRow0, thrustCol0, chart(:,:,2), chart(:,:,1), aIndex, bIndex);
[thrustRow1, thrustCol1] = nickelfelpeterv3(chart, aIndex, bIndex, maxThrottle);
score1 = runsolution(thrustRow1, thrustCol1, chart(:,:,2), chart(:,:,1), aIndex, bIndex);
if score0<score1,
    thrustRow=thrustRow0;
    thrustCol=thrustCol0;
else
    thrustRow=thrustRow1;
    thrustCol=thrustCol1;
end

end

function [thrustRow, thrustCol] = nickelfelpeterv1(chart, aIndex, bIndex, maxThrottle)
%Less Significant
%by Amitabh Verma 
%r5240
%t31

rand(1,14);
y_winds         = chart(:,:,1);
x_winds         = chart(:,:,2);
[ny,nx]         = size(y_winds);
ay              = rem(aIndex-1, ny) + 1;
ax              = (aIndex - ay)/ny  + 1;
by              = rem(bIndex-1, ny) + 1;
bx              = (bIndex - by)/ny  + 1;
X = repmat(1:nx, ny,1);
Y = repmat((1:ny)', 1,nx);

dist_to_B       = ((X - bx).^2 + (Y - by).^2).^1.15;
dist_to_A       = ((X - ax).^2 + (Y - ay).^2).^1.155;

dist_to_B_w     = dist_to_B*1e-3;
dist_to_A_w     = dist_to_A*1e-3;

xvel            = 0*x_winds;
fuel            = xvel+100;
fuel(aIndex)    = 0;
yvel            = xvel;
checked         = xvel;
previous_stop   = xvel;
min_fuel        = 0;
cost_to_B = dist_to_B(aIndex);
zoon = nx*ny*[10 0.37];
ziter = [500 700] .* (1-0.05*rand(1,2));
sf = [0.31 0.19];

for i = 1:2
    
    SDB_tf = dist_to_B(:)<=zoon(i);
    SDB_idx = find(SDB_tf);
    
    SDBchecked = checked(SDB_tf);
    SDBfuel = fuel(SDB_tf);
    SDBdist_to_B_w = dist_to_B_w(SDB_tf);
    SDBdist_to_B = dist_to_B(SDB_tf);
    SDBxvel = xvel(SDB_tf);
    SDByvel = yvel(SDB_tf);
    SDBX = X(SDB_tf);
    SDBY = Y(SDB_tf);
    SDBprevious_stop = previous_stop(SDB_tf);
    
    slowdown = maxThrottle+SDBdist_to_B*sf(i);
    
    it = 0;
    metrix                  = SDBfuel + SDBchecked + SDBdist_to_B_w;
    while (  min_fuel <= cost_to_B  && ~all(SDBchecked) && it < ziter(i))
        it                      = it + 1;
        
        [dummy,i_p]             = min(metrix);
        i_p_full                = SDB_idx(i_p);
        min_fuel                = SDBfuel(i_p);
        i_y                     = rem(i_p_full - 1,ny) + 1;
        i_x                     = (i_p_full - i_y)/ny  + 1;
        i_y_winds               = y_winds(i_p_full);
        i_x_winds               = x_winds(i_p_full);
        i_vy                    = SDByvel(i_p) + i_y_winds;
        i_vx                    = SDBxvel(i_p) + i_x_winds;
        i_new_vy                = SDBY - i_y;
        i_new_vx                = SDBX - i_x;
        i_thrust                = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
        i_fuel                  = i_thrust + min_fuel;
        i_impr_tf               = (i_thrust <= maxThrottle) & (abs(i_new_vy+i_y_winds)+abs(i_new_vx+i_x_winds)<slowdown) & (i_fuel < SDBfuel);
        i_impr_idx              = find(i_impr_tf);
        SDBchecked(i_p)            = 100; %true;
        metrix(i_p)             = SDBfuel(i_p) + 100 + SDBdist_to_B_w(i_p);
        if isempty(i_impr_idx)
            continue;
        end
        i_fuel_impr             = i_fuel  (i_impr_idx) ;
        SDBxvel           (i_impr_idx) = i_new_vx(i_impr_idx);
        SDByvel           (i_impr_idx) = i_new_vy(i_impr_idx);
        SDBfuel           (i_impr_idx) = i_fuel_impr;
        SDBprevious_stop  (i_impr_idx) = i_p_full;
        cost_to_B = min(cost_to_B,min(SDBfuel(i_impr_idx) + SDBdist_to_B(i_impr_idx)));
        SDBchecked(i_impr_idx)     = 0; %false;
        metrix(i_impr_idx) = SDBfuel(i_impr_idx) + SDBdist_to_B_w(i_impr_idx);
    end
    
    fuel(SDB_tf) = SDBfuel;
    previous_stop(SDB_tf) = SDBprevious_stop;
    xvel(SDB_tf) = SDBxvel;
    yvel(SDB_tf) = SDByvel;
    
end

fuel                    = fuel + dist_to_B;
cost_to_A               = min(fuel(:) + dist_to_A(:));

checked(:)                 = 0;
return_previous_stop    = checked;

min_fuel                = 0;
metrix              = fuel + checked + dist_to_A_w; %****
while ( min_fuel < cost_to_A &&  ~all(checked(:)))
    
    [dummy,i_p]             = min(metrix(:)); %****
    min_fuel            = fuel(i_p); %****
    
    i_y                 = rem(i_p - 1,ny) + 1;
    i_x                 = (i_p - i_y)/ny  + 1;
    i_vy                = yvel(i_p) + y_winds(i_p);
    i_vx                = xvel(i_p) + x_winds(i_p);
    i_new_vy            = Y-i_y;
    i_new_vx            = X-i_x;
    i_thrust            = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
    i_fuel              = i_thrust + min_fuel;
    i_impr_tf           = i_thrust <= maxThrottle & i_fuel < fuel;
    checked(i_p)            = 100; %true;
    metrix(i_p) = fuel(i_p) + 100 + dist_to_A_w(i_p);
        i_impr_idx          = find(i_impr_tf);
    if isempty(i_impr_idx)
        continue;
    end
    fuel                (i_impr_idx) = i_fuel(i_impr_idx);
    xvel                (i_impr_idx) = i_new_vx(i_impr_idx);
    yvel                (i_impr_idx) = i_new_vy(i_impr_idx);
    return_previous_stop(i_impr_idx) = i_p;
    cost_to_A = min(cost_to_A,min(fuel(i_impr_idx) + dist_to_A(i_impr_idx)));
    checked(i_impr_idx) = 0;
    metrix(i_impr_idx) = fuel(i_impr_idx) + dist_to_A_w(i_impr_idx);
end

cost_to_A           = fuel + dist_to_A;
[dummy,min_fuel]        = min(cost_to_A(:));
indices             = zeros(nx*ny,1);
indices(1)          = min_fuel(1);
next_move           = return_previous_stop(indices(1));
k                   = 1;
[indices, k] = while_next_move(indices, k, next_move, return_previous_stop);
next_move           = previous_stop(indices(k));
[indices, k] = while_next_move(indices, k, next_move, previous_stop);
indices             = indices(k:-1:1);

ypos                = rem(indices - 1, ny) + 1;
xpos                = (indices - ypos)/ny + 1;
xw                  = x_winds(indices);
yw                  = y_winds(indices);

thrustCol           = diff([0; diff(xpos)]) - xw(1:end-1);
thrustRow           = diff([0; diff(ypos)]) - yw(1:end-1);

end

function [thrustRow, thrustCol] = nickelfelpeterv0(chart, aIndex, bIndex, maxThrottle)
%dodisp=0;
%if dodisp,disp([size(chart,1),size(chart,2),max(abs(chart(:))),maxThrottle]);end

thrustRow=[];thrustCol=[];

maxlength=100;
maxcost=10;
maxtraj=4*[128,128,128,128];
maxcomp=4*[32,32,32,32];
maxdepth=4;
maxwidth=3;
INF=1e6;
INFl=1e4;
schart=[size(chart,1),size(chart,2)];
[rBeg,cBeg]=ind2sub(schart,aIndex);
[rEnd,cEnd]=ind2sub(schart,bIndex);
maxthrottle=2*maxThrottle;
[BR,BC]=ndgrid(-maxthrottle:maxthrottle,-maxthrottle:maxthrottle);
idx=find(abs(BR)+abs(BC)<=maxthrottle);
BR=BR(idx);BC=BC(idx);
[DR,DC]=ndgrid(-maxThrottle:maxThrottle,-maxThrottle:maxThrottle);
idx=find(abs(DR)+abs(DC)<=maxThrottle);
DR=DR(idx);DC=DC(idx);
[ER,EC]=ndgrid(1:schart(1),1:schart(2));
idx=find(abs(2*rEnd-ER+chart(rEnd,cEnd,1)-max(1,min(schart(1),2*rEnd-ER+chart(rEnd,cEnd,1))))+abs(2*cEnd-EC+chart(rEnd,cEnd,2)-max(1,min(schart(2),2*cEnd-EC+chart(rEnd,cEnd,2))))<=maxThrottle);
ERboard=sparse(ER(idx),EC(idx),1,schart(1),schart(2));
baseboard=max(schart)+maxThrottle+max(abs(chart(:)));
maxboard=2*baseboard+max(schart);
DIST=1:maxboard;
DIST=DIST(ones(maxboard,1),:);
DIST=abs(DIST-DIST');
DR0=[-1,0,1,-1,0,1,-1,0,1];
DC0=[-1,-1,-1,0,0,0,1,1,1];
test_trajectory=zeros(maxlength+2,2);


% first try
tBegf={};tEndb={};tEndf={};tBegb={};
sBegf=[];sEndb=[];sEndf=[];sBegb=[];
cBegf=[];cEndb=[];cEndf=[];cBegb=[];
icBegf=[];icEndb=[];icEndf=[];icBegb=[];
aBegf=[];aBegb=[];
rBaseDif=0;cBaseDif=0;
create_trajectories(rBeg,cBeg,rBeg,cBeg,rBeg,cBeg,[],[],[1,1,1,1]);
DIST12=[];DIST23=[];DIST34=[];DIST2T=[];DISTT3=[];COST12=[];COST23=[];COST34=[];COST12a=[];COST23a=[];COST34a=[];COST12b=[];COST23b=[];COST34b=[];
[Pos,c,cc,ccc,mPos]=link_trajectories;

% modify number of segments
for nmodtry=1:3,
    modtry=0;
    idxout=find(ccc>maxThrottle);
    if any(idxout==2|idxout==3),%12
        maxcomp(1)=2*maxcomp(1);maxcomp(2)=2*maxcomp(2);
        maxtraj(1)=2*maxtraj(1);maxtraj(2)=2*maxtraj(2);
        modtry=1;
    end
    if any(idxout==4|idxout==6),%23
        maxcomp(2)=2*maxcomp(2);maxcomp(3)=2*maxcomp(3);
        maxtraj(2)=2*maxtraj(2);maxtraj(3)=2*maxtraj(3);
        modtry=1;
    end
    if any(idxout==7|idxout==8),%34
        maxcomp(3)=2*maxcomp(3);maxcomp(4)=2*maxcomp(4);
        maxtraj(3)=2*maxtraj(3);maxtraj(4)=2*maxtraj(4);
        modtry=1;
    end
    if modtry,
        tBegbbak=tBegb;tBegfbak=tBegf;tEndbbak=tEndb;tEndfbak=tEndf;sBegbbak=sBegb;sBegfbak=sBegf;sEndbbak=sEndb;sEndfbak=sEndf;cBegbbak=cBegb;cBegfbak=cBegf;cEndbbak=cEndb;cEndfbak=cEndf;icBegbbak=icBegb;icBegfbak=icBegf;icEndbbak=icEndb;icEndfbak=icEndf;aBegfbak=aBegf;aBegbbak=aBegb;
        create_trajectories(rBeg,cBeg,rBeg,cBeg,rBeg,cBeg,[],[],[1,1,1,1]);
        DIST12bak=DIST12;DIST23bak=DIST23;DIST34bak=DIST34;DIST2Tbak=DIST2T;DISTT3bak=DISTT3;COST12bak=COST12;COST23bak=COST23;COST34bak=COST34;COST12abak=COST12a;COST23abak=COST23a;COST34abak=COST34a;COST12bbak=COST12b;COST23bbak=COST23b;COST34bbak=COST34b;
        DIST12=[];DIST23=[];DIST34=[];DIST2T=[];DISTT3=[];COST12=[];COST23=[];COST34=[];COST12a=[];COST23a=[];COST34a=[];COST12b=[];COST23b=[];COST34b=[];
        [Pos2,c2,cc2,ccc2,mPos2]=link_trajectories;
        if c2<c,
            Pos=Pos2; c=c2; cc=cc2; ccc=ccc2; mPos=mPos2;
        else
            tBegb=tBegbbak;tBegf=tBegfbak;tEndb=tEndbbak;tEndf=tEndfbak;sBegb=sBegbbak;sBegf=sBegfbak;sEndb=sEndbbak;sEndf=sEndfbak;cBegb=cBegbbak;cBegf=cBegfbak;cEndb=cEndbbak;cEndf=cEndfbak;icBegb=icBegbbak;icBegf=icBegfbak;icEndb=icEndbbak;icEndf=icEndfbak;aBegf=aBegfbak;aBegb=aBegbbak;
            DIST12=DIST12bak;DIST23=DIST23bak;DIST34=DIST34bak;DIST2T=DIST2Tbak;DISTT3=DISTT3bak;COST12=COST12bak;COST23=COST23bak;COST34=COST34bak;COST12a=COST12abak;COST23a=COST23abak;COST34a=COST34abak;COST12b=COST12bbak;COST23b=COST23bbak;COST34b=COST34bbak;
        end
    else
        break
    end
end

% modify endpoint
wdc=1;
rEnd0=rEnd;cEnd0=cEnd;
while wdc>0&&ccc(4)+ccc(6)>ccc(5)&&(wdc>.9||ccc(4)+ccc(6)>INFl),
    dc=wdc*DIST(ER+baseboard,rEnd0+baseboard).^2+wdc*DIST(EC+baseboard,cEnd0+baseboard).^2+DIST(ER+chart(:,:,1)+baseboard,rBeg+baseboard)+DIST(EC+chart(:,:,2)+baseboard,cBeg+baseboard);
    [nill,idx]=sort(dc);
    rEndnew=1+rem(idx(1)-1,schart(1));
    cEndnew=1+(idx(1)-rEndnew)/schart(1);
    if (rEndnew~=rEnd||cEndnew~=cEnd)&&(rBaseDif+rEnd-rEndnew)^2+(cBaseDif+cEnd-cEndnew)^2<c,
        rEndbak=rEnd;cEndbak=cEnd;
        tEndbbak=tEndb;tEndfbak=tEndf;sEndbbak=sEndb;sEndfbak=sEndf;cEndbbak=cEndb;cEndfbak=cEndf;icEndbbak=icEndb;icEndfbak=icEndf;aBegfbak=aBegf;aBegbbak=aBegb;
        ERboardbak=ERboard;
        
        DIST12bak=DIST12;DIST23bak=DIST23;DIST34bak=DIST34;DIST2Tbak=DIST2T;DISTT3bak=DISTT3;COST12bak=COST12;COST23bak=COST23;COST34bak=COST34;COST12abak=COST12a;COST23abak=COST23a;COST34abak=COST34a;COST12bbak=COST12b;COST23bbak=COST23b;COST34bbak=COST34b;
        rBaseDifbak=rBaseDif;cBaseDifbak=cBaseDif;
        rEnd=rEndnew;cEnd=cEndnew;
        idx=find(abs(2*rEnd-ER+chart(rEnd,cEnd,1)-max(1,min(schart(1),2*rEnd-ER+chart(rEnd,cEnd,1))))+abs(2*cEnd-EC+chart(rEnd,cEnd,2)-max(1,min(schart(2),2*cEnd-EC+chart(rEnd,cEnd,2))))<=maxThrottle);
        ERboard=sparse(ER(idx),EC(idx),1,schart(1),schart(2));
        rBaseDif=(rEnd0-rEnd);cBaseDif=(cEnd0-cEnd);
        create_trajectories(rBeg,cBeg,rBeg,cBeg,rBeg,cBeg,[],[],[0,1,1,0]);
        
        DIST12=[];DIST23=[];DIST34=[];DIST2T=[];DISTT3=[];COST12=[];COST23=[];COST34=[];COST12a=[];COST23a=[];COST34a=[];COST12b=[];COST23b=[];COST34b=[];
        [Pos2,c2,cc2,ccc2,mPos2]=link_trajectories;
        %if dodisp, disp(['step1 ', num2str(c),' ',num2str(c2)]); end
        if c2<c,
            Pos=Pos2; c=c2; cc=cc2; ccc=ccc2; mPos=mPos2;
        else
            rEnd=rEndbak; cEnd=cEndbak;
            tEndb=tEndbbak;tEndf=tEndfbak;sEndb=sEndbbak;sEndf=sEndfbak;cEndb=cEndbbak;cEndf=cEndfbak;icEndb=icEndbbak;icEndf=icEndfbak;aBegf=aBegfbak;aBegb=aBegbbak;
            ERboard=ERboardbak;
            DIST12=DIST12bak;DIST23=DIST23bak;DIST34=DIST34bak;DIST2T=DIST2Tbak;DISTT3=DISTT3bak;COST12=COST12bak;COST23=COST23bak;COST34=COST34bak;COST12a=COST12abak;COST23a=COST23abak;COST34a=COST34abak;COST12b=COST12bbak;COST23b=COST23bbak;COST34b=COST34bbak;
            rBaseDif=rBaseDifbak;cBaseDif=cBaseDifbak;
        end
    end
    wdc=wdc-.1;
end

% modify trajectories
Pos0=Pos;
for nsteps=1:0,
    if nsteps+2>min([mPos,size(Pos,1)-mPos,size(Pos,1)]),break; end
    create_trajectories(Pos(nsteps+1,1),Pos(nsteps+1,2),Pos(nsteps,1),Pos(nsteps,2),Pos(end-nsteps,1),Pos(end-nsteps,2),Pos(end-nsteps+1,1),Pos(end-nsteps+1,2),[1,0,0,1]);
    DIST12=[];DIST34=[];COST12=[];COST34=[];
    [Pos2,c2,cc2,ccc2,mPos2]=link_trajectories;
    Posnew=[Pos(1:nsteps,:);Pos2;Pos(end-nsteps+1:end,:)];
    ccnew=[cc(1:nsteps,:);cc2;cc(end-nsteps+1:end,:)];
    %if dodisp,disp(['step2 ', num2str(sum(cc)),' ',num2str(sum(ccnew))]);end
    if sum(ccnew)<sum(cc), Pos=Posnew; cc=ccnew; mPos=nsteps+mPos2; end
end

[thrustRow,thrustCol,cost]=convert_trajectory_to_acceleration(Pos);
% [thrustRow0,thrustCol0,cost0]=convert_trajectory_to_acceleration(Pos0);
% disp([cost0,cost]);

%if dodisp,disp(['Cost ',num2str([c,cost])]);end
%if dodisp,fh=gcf;figure(3);disptraj([]);disptraj(Pos);set(gcf,'name',num2str([c,cost]));figure(fh);end

    function create_trajectories(trBegf,tcBegf,prevtrBegf,prevtcBegf,trBegb,tcBegb,nexttrBegb,nexttcBegb,computesegment)
        %segment 1, BEGF, beginning forward (from origin to somewhere)
        if computesegment(1)
            tr=2*trBegf-prevtrBegf+chart(trBegf,tcBegf,1)+DR;
            tc=2*tcBegf-prevtcBegf+chart(trBegf,tcBegf,2)+DC;
            idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
            dr=DR(idx);dc=DC(idx);tr=tr(idx);tc=tc(idx);
            tr2=tr+dr+chart(tr);
            tc2=tc+dc+chart(schart(1)*schart(2)+tr);
            [nill,idx]=sort(abs(dr)+abs(dc)+.1*abs(tr2-rEnd)+.1*abs(tc2-cEnd));dr=dr(idx(1:min(numel(dr),maxtraj(1))));dc=dc(idx(1:min(numel(dc),maxtraj(1))));
            tBegf=cell(1,numel(dr));
            sBegf=zeros(1,numel(dr));
            cBegf=zeros(1,numel(dr));
            aBegf=zeros(1,numel(dr));
            for niter=1:numel(dr),
                rdr=dr(niter);
                cdc=dc(niter);
                tt=single_trajectory_forward(trBegf,tcBegf,trBegf-prevtrBegf,tcBegf-prevtcBegf,rdr,cdc);
                tBegf{niter}=tt;
                sBegf(niter)=size(tt,1);
                aBegf(niter)=abs(rdr)+abs(cdc);
                [tcost,itcost]=min( DIST(tt(2:end,1)+baseboard,rEnd+baseboard)+DIST(tt(2:end,2)+baseboard,cEnd+baseboard), [],1);
                cBegf(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
            end
            [nill,icBegf]=sort(cBegf(2:end));
            icBegf=[1,1+icBegf];
        end
        
        %segment 2 ENDB, end backward (from somewhere to target)
        if computesegment(2),
            otherpos=ERboard;%sparse(ER,EC,1,schart(1),schart(2));
            tr=rEnd-DR;
            tc=cEnd-DC;
            idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
            otherpos=otherpos+sparse(tr(idx),tc(idx),1,schart(1),schart(2));
            [dr,dc]=find(otherpos);
            dr=rEnd-dr;
            dc=cEnd-dc;
            [nill,idx]=sort(abs(dr)+abs(dc));dr=dr(idx(1:min(numel(dr),maxtraj(2))));dc=dc(idx(1:min(numel(dc),maxtraj(2))));
            
            tEndb=cell(1,numel(dr));
            sEndb=zeros(1,numel(dr));
            cEndb=zeros(1,numel(dr));
            for niter=1:numel(dr),
                rdr=dr(niter);
                cdc=dc(niter);
                tt=single_trajectory_backward(rEnd,cEnd,rdr,cdc,0,0);
                tEndb{niter}=tt;
                sEndb(niter)=size(tt,1);
                [tcost,itcost]=min( DIST(tt(1:end-1,1)+baseboard,trBegf+baseboard)+DIST(tt(1:end-1,2)+baseboard,tcBegf+baseboard), [],1);
                cEndb(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
            end
            [nill,icEndb]=sort(cEndb);
        end
        
        %segment 3 ENDF, end forward (from target to somewhere)
        if computesegment(3),
            otherpos=ERboard;%sparse(ER,EC,1,schart(1),schart(2));
            tr=rEnd+chart(rEnd,cEnd,1)+BR;
            tc=cEnd+chart(rEnd,cEnd,2)+BC;
            idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
            otherpos=otherpos+sparse(tr(idx),tc(idx),1,schart(1),schart(2));
            [dr,dc]=find(otherpos);
            dr=dr-rEnd-chart(rEnd,cEnd,1);
            dc=dc-cEnd-chart(rEnd,cEnd,2);
            [nill,idx]=sort(abs(dr)+abs(dc));dr=dr(idx(1:min(numel(dr),maxtraj(3))));dc=dc(idx(1:min(numel(dc),maxtraj(3))));
            
            tEndf=cell(1,numel(dr));
            sEndf=zeros(1,numel(dr));
            cEndf=zeros(1,numel(dr));
            aEndf=zeros(1,numel(dr));
            for niter=1:numel(dr),
                rdr=dr(niter);
                cdc=dc(niter);
                tt=single_trajectory_forward(rEnd,cEnd,0,0,rdr,cdc);
                tEndf{niter}=tt;
                sEndf(niter)=size(tt,1);
                aEndf(niter)=abs(rdr)+abs(cdc);
                [tcost,itcost]=min( DIST(tt(2:end,1)+baseboard,trBegb+baseboard)+DIST(tt(2:end,2)+baseboard,tcBegb+baseboard), [],1);
                cEndf(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
            end
            [nill,icEndf]=sort(cEndf);
        end
        
        %segment 4 BEGB, beginning backward (from somewhere to origin)
        if computesegment(4),
            if isempty(nexttrBegb),
                BR2=repmat(BR,[1,9]);
                BC2=repmat(BC,[1,9]);
                rdelta=repmat(DR0,[numel(BR),1]);
                cdelta=repmat(DC0,[numel(BR),1]);
                tr=trBegb+rdelta-BR2;
                tc=tcBegb+cdelta-BC2;
                tr2=trBegb+rdelta+BR2+chart(trBegb,tcBegb,1);
                tc2=tcBegb+cdelta+BC2+chart(trBegb,tcBegb,2);
                idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2)&trBegb+rdelta>=1&trBegb+rdelta<=schart(1)&tcBegb+cdelta>=1&tcBegb+cdelta<=schart(2)&((~rdelta&~cdelta)|tr2<1|tr2>schart(1)|tc2<1&tc2>schart(2)));
                dr=BR2(idx);dc=BC2(idx);rdelta=rdelta(idx);cdelta=cdelta(idx);tr=tr(idx);tc=tc(idx);
                [nill,idx]=sort(abs(dr)+abs(dc)+.1*abs(tr-rEnd)+.1*abs(tc-cEnd));dr=dr(idx(1:min(numel(dr),maxtraj(4))));dc=dc(idx(1:min(numel(dc),maxtraj(4))));rdelta=rdelta(idx(1:min(numel(rdelta),maxtraj(4))));cdelta=cdelta(idx(1:min(numel(cdelta),maxtraj(4))));
                aBegb=abs(rdelta)'.^2+abs(cdelta)'.^2;
            else
                dr=nexttrBegb-trBegb-chart(trBegb,tcBegb,1)-DR;
                dc=nexttcBegb-tcBegb-chart(trBegb,tcBegb,2)-DC;
                tr=trBegb-dr;
                tc=tcBegb-dc;
                idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
                dr=dr(idx);dc=dc(idx);
                aBegb=abs(dr-(nexttrBegb-trBegb-chart(trBegb,tcBegb,1)))+abs(dc-(nexttcBegb-tcBegb-chart(trBegb,tcBegb,2)));
                [nill,idx]=sort(aBegb);dr=dr(idx(1:min(numel(dr),maxtraj(4))));dc=dc(idx(1:min(numel(dc),maxtraj(4))));aBegb=aBegb(idx(1:min(numel(dc),maxtraj(4))))';
                rdelta=zeros(1,numel(dr));
                cdelta=zeros(1,numel(dr));
            end
            tBegb=cell(1,numel(dr));
            sBegb=zeros(1,numel(dr));
            cBegb=zeros(1,numel(dr));
            for niter=1:numel(dr),
                rdr=dr(niter);
                cdc=dc(niter);
                tt=single_trajectory_backward(trBegb+rdelta(niter),tcBegb+cdelta(niter),rdr,cdc,0,0);
                tBegb{niter}=tt;
                sBegb(niter)=size(tt,1);
                [tcost,itcost]=min( DIST(tt(1:end-1,1)+baseboard,rEnd+baseboard)+DIST(tt(1:end-1,2)+baseboard,cEnd+baseboard), [],1);
                cBegb(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
            end
            [nill,icBegb]=sort(cBegb(2:end));
            icBegb=[1,1+icBegb];
        end
        
        
    end

    function [pos,costmin,costPos,coststeps,mpos]=link_trajectories
        
        x1=tBegf;s1=sBegf;ic1=icBegf;
        x2=tEndb;s2=sEndb;ic2=icEndb;
        x3=tEndf;s3=sEndf;ic3=icEndf;
        x4=tBegb;s4=sBegb;ic4=icBegb;
        
        l1=numel(ic1);
        l2=numel(ic2);
        l3=numel(ic3);
        l4=numel(ic4);
        if ~l1,pos=[];costmin=inf;costPos=[];coststeps=[inf,inf,inf,0,0,0,0,0,0];mpos=0;return;
        elseif ~l2,pos=[];costmin=inf;costPos=[];coststeps=[0,inf,inf,inf,inf,inf,0,0,0];mpos=0;return;
        elseif ~l3,pos=[];costmin=inf;costPos=[];coststeps=[0,0,0,inf,inf,inf,inf,inf,0];mpos=0;return;
        elseif ~l4,pos=[];costmin=inf;costPos=[];coststeps=[0,0,0,0,0,0,inf,inf,inf];mpos=0;return;
        end
        is1=s1(ic1(1:min(l1,maxcomp(1))));
        is2=s2(ic2(1:min(l2,maxcomp(2))));
        is3=s3(ic3(1:min(l3,maxcomp(3))));
        is4=s4(ic4(1:min(l4,maxcomp(4))));
        bix1=[1;1+cumsum(is1(1:end-1))'];                       % index to beginning of each segment in x1 (set 1)
        bix2=[1;1+cumsum(is2(1:end-1))'];                       % index to beginning of each segment in x2 (set 2)
        bix3=[1;1+cumsum(is3(1:end-1))'];                       % index to beginning of each segment in x3 (set 3)
        bix4=[1;1+cumsum(is4(1:end-1))'];                       % index to beginning of each segment in x4 (set 4)
        eix1=cumsum(is1);                                       % index to end of each segment (set 1)
        eix2=cumsum(is2);                                       % index to end of each segment (set 2)
        eix3=cumsum(is3);                                       % index to end of each segment (set 3)
        eix4=cumsum(is4);                                       % index to end of each segment (set 4)
        ix1=zeros(eix1(end),1);ix1(bix1)=1;ix1=cumsum(ix1);     % segment # (for each node) (set 1, index to ic1)
        ix2=zeros(eix2(end),1);ix2(bix2)=1;ix2=cumsum(ix2);     % segment # (for each node) (set 2, index to ic2)
        ix3=zeros(eix3(end),1);ix3(bix3)=1;ix3=cumsum(ix3);     % segment # (for each node) (set 3, index to ic3)
        ix4=zeros(eix4(end),1);ix4(bix4)=1;ix4=cumsum(ix4);     % segment # (for each node) (set 4, index to ic4)
        
        % compute distances
        xx1=cat(1,x1{ic1(1:min(l1,maxcomp(1)))})+baseboard;
        xx2=cat(1,x2{ic2(1:min(l2,maxcomp(2)))})+baseboard;
        xx3=cat(1,x3{ic3(1:min(l3,maxcomp(3)))})+baseboard;
        xx4=cat(1,x4{ic4(1:min(l4,maxcomp(4)))})+baseboard;
        if isempty(DIST12), DIST12=DIST(xx1(:,1),xx2(:,1))+DIST(xx1(:,2),xx2(:,2)); end        % cityblock distance between nodes (set 1 vs. set 2)
        if isempty(DIST23), DIST23=DIST(xx2(:,1),xx3(:,1))+DIST(xx2(:,2),xx3(:,2)); end        % cityblock distance between nodes (set 2 vs. set 3)
        if isempty(DIST34), DIST34=DIST(xx3(:,1),xx4(:,1))+DIST(xx3(:,2),xx4(:,2)); end        % cityblock distance between nodes (set 3 vs. set 4)
        if isempty(DIST2T), DIST2T=DIST(xx2(:,1),rBaseDif+rEnd+baseboard).^2+DIST(xx2(:,2),cBaseDif+cEnd+baseboard).^2; end
        if isempty(DISTT3), DISTT3=DIST(rBaseDif+rEnd+baseboard,xx3(:,1)).^2+DIST(cBaseDif+cEnd+baseboard,xx3(:,2)).^2; end
        
        % compute transition costs (forward viterbi step)
        % segment 1 - segment 2a
        c1=aBegf(ic1(1:min(l1,maxcomp(1))))';
        c1=c1(ix1);
        c1(bix1)=INF;c1(eix1)=INF;          c1(bix1+1)=c1(bix1+1)+INFl;
        if isempty(COST12),
            COST12a=DIST12(:,[1,1:end-1]);
            COST12b=DIST12([2:end,end],:);
            COST12=COST12a+COST12b;
        end
        c12=c1(:,ones(1,eix2(end)))+COST12;    % cost of jumping from node i (set 1) to node j (set 2) and maintaining the trajectory of node j
        %         c12(:,bix2)=INF;c12(:,eix2)=INF;
        %       idx=COST12a>maxThrottle|COST12b>maxThrottle;c12(idx)=c12(idx)+INFl;
        [cost2a,idxset1]=min(c12,[],1);                             % best set 1 node for each set 2 node
        idx=find(COST12a(idxset1+size(c12,1)*(0:size(c12,2)-1))>maxThrottle|COST12b(idxset1+size(c12,1)*(0:size(c12,2)-1))>maxThrottle);cost2a(idx)=cost2a(idx)+INFl;
        
        % segment 2a - segment 2b
        c2=cost2a';                                                  % within-segment transition matrix
        c2(bix2)=INF;c2(eix2)=INF;          %c2(bix2+1)=c2(bix2+1)+INFl;
        cost2b=zeros(1,numel(c2));idxset2a=cost2b;
        c2_nset=1; c2_min=INF; c2_minidx=1;
        for n1=1:numel(c2),
            cost2b(n1)=c2_min; idxset2a(n1)=c2_minidx;
            if c2(n1)<c2_min,c2_min=c2(n1);c2_minidx=n1;end;
            if n1==eix2(c2_nset), c2_min=inf; c2_nset=c2_nset+1; end
        end
        
        
        % segment 2b - segment 3a
        c2=cost2b';
        c2(bix2)=INF;c2(eix2)=INF;          %c2(bix2+1)=c2(bix2+1)+INFl;
        if isempty(COST23),
            COST23a=DIST23(:,[1,1:end-1]);
            COST23b=DIST23([2:end,end],:);
            COST23=COST23a+COST23b;
        end
        c23=c2(:,ones(1,eix3(end)))+COST23;    % cost of jumping from node i (set 2) to node j (set 3) and maintaining the trajectory of node j
        c23=c23+min(DISTT3(ones(eix2(end),1),:),DIST2T(:,ones(1,eix3(end))));
        %         c23(:,bix3)=INF;c23(:,eix3)=INF;
        %       idx=COST23a>maxThrottle|COST23b>maxThrottle;c23(idx)=c23(idx)+INFl;
        [cost3a,idxset2b]=min(c23,[],1);                             % best set 2 node for each set 3 node
        idx=find(COST23a(idxset2b+size(c23,1)*(0:size(c23,2)-1))>maxThrottle|COST23b(idxset2b+size(c23,1)*(0:size(c23,2)-1))>maxThrottle);cost3a(idx)=cost3a(idx)+INFl;
        
        
        % segment 3a - segment 3b
        c2=cost3a';                                                 % within-segment transition matrix
        c2(bix3)=INF;c2(eix3)=INF;          %c2(bix3+1)=c2(bix3+1)+INFl;
        cost3b=zeros(1,numel(c2));idxset3a=cost3b;
        c2_nset=1; c2_min=INF; c2_minidx=1;
        for n1=1:numel(c2),
            cost3b(n1)=c2_min; idxset3a(n1)=c2_minidx;
            if c2(n1)<c2_min,c2_min=c2(n1);c2_minidx=n1;end;
            if n1==eix3(c2_nset), c2_min=inf; c2_nset=c2_nset+1; end
        end
        
        % segment 3b - segment 4
        c3=cost3b';
        c3(bix3)=INF;c3(eix3)=INF;          %c3(bix3+1)=c3(bix3+1)+INFl;
        if isempty(COST34),
            COST34a=DIST34(:,[1,1:end-1]);
            COST34b=DIST34([2:end,end],:);
            COST34=COST34a+COST34b;
        end
        c34=c3(:,ones(1,eix4(end)))+COST34;    % cost of jumping from node i (set 3) to node j (set 4) and maintaining the trajectory of node j
        %         c34(:,eix4)=INF;c34(:,eix4-1)=INF;
        %       idx=COST34a>maxThrottle|COST34b>maxThrottle;c34(idx)=c34(idx)+INFl;
        [cost4,idxset3b]=min(c34,[],1);                             % best set 3 node for each set 4 node
        idx=find(COST34a(idxset3b+size(c34,1)*(0:size(c34,2)-1))>maxThrottle|COST34b(idxset3b+size(c34,1)*(0:size(c34,2)-1))>maxThrottle);cost4(idx)=cost4(idx)+INFl;
        
        c4=aBegb(ic4(1:min(l4,maxcomp(4))));
        c4=c4(ix4);
        cost4=cost4+c4;
        
        % compute transition path (inverse viterbi step)
        cost4(bix4)=INF;cost4(eix4)=INF;
        [costmin,idxset4]=min(cost4);                               % set 4 node in optimal trajectory
        idxset3b=idxset3b(idxset4);                                   % set 3 node in optimal trajectory
        idxset3a=idxset3a(idxset3b);                                 % set 3 node in optimal trajectory
        idxset2b=idxset2b(idxset3a);                                % set 2 node in optimal trajectory
        idxset2a=idxset2a(idxset2b);                                  % set 2 node in optimal trajectory
        idxset1=idxset1(idxset2a);                                   % set 1 node in optimal trajectory
        
        %         Pos=[];
        %         for n1=1:numel(idxset1),Pos=cat(1,Pos,xx1(bix1(ix1(idxset1{n1}))+1:idxset1{n1},:)); end
        %         Pos=cat(1,Pos,xx2(idxset2a:idxset2b,:),xx3(idxset3a:idxset3b,:));
        %         for n1=1:numel(idxset4),Pos=cat(1,Pos,xx4(idxset4{n1}:eix4(ix4(idxset4{n1}))-1,:)); end
        %         Pos=Pos-baseboard;
        pos=cat(1,...
            xx1(bix1(ix1(idxset1))+1:idxset1,:),...
            xx3(idxset3a:idxset3b,:),...
            xx2(idxset2a:idxset2b,:),...
            xx4(idxset4:eix4(ix4(idxset4))-1,:))  -baseboard;
        
        coststeps=[aBegf(ic1(ix1(idxset1))),...
            COST12b(idxset1,idxset2a),...
            COST12a(idxset1,idxset2a),...
            COST23b(idxset2b,idxset3a),...
            COST23a(idxset2b,idxset3a),...
            COST34b(idxset3b,idxset4),...
            COST34a(idxset3b,idxset4),...
            min(DIST2T(idxset2b),DISTT3(idxset3a)),...
            aBegb(ic4(ix4(idxset4)))];
        costPos=zeros(size(pos,1),1);
        ncostPos=0;
        ncostpos=idxset1-(bix1(ix1(idxset1))+1)+1;  costPos(ncostPos+1)=costPos(ncostPos+1)+coststeps(1); costPos(ncostPos+ncostpos)=costPos(ncostPos+ncostpos)+coststeps(2); costPos(ncostPos+ncostpos+1)=costPos(ncostPos+ncostpos+1)+coststeps(3); ncostPos=ncostPos+ncostpos;
        ncostpos=(idxset2b-idxset2a)+1;             costPos(ncostPos+ncostpos)=costPos(ncostPos+ncostpos)+coststeps(4)+coststeps(5)/2; costPos(ncostPos+ncostpos+1)=costPos(ncostPos+ncostpos+1)+coststeps(6)+coststeps(5)/2;mpos=ncostPos+ncostpos+.5;ncostPos=ncostPos+ncostpos;
        ncostpos=(idxset3b-idxset3a)+1;             costPos(ncostPos+ncostpos)=costPos(ncostPos+ncostpos)+coststeps(7); costPos(ncostPos+ncostpos+1)=costPos(ncostPos+ncostpos+1)+coststeps(8); ncostPos=ncostPos+ncostpos;
        ncostpos=eix4(ix4(idxset4))-1-idxset4+1;    costPos(ncostPos+ncostpos)=costPos(ncostPos+ncostpos)+coststeps(9); ncostPos=ncostPos+ncostpos;
        
        
        %ix#(idxset#)                                            %     segment number (index to ic#)
        %
    end

    function y=single_trajectory_forward(xr,xc,vr,vc,ar,ac)
        test_trajectory(1,1)=xr-vr-ar;
        test_trajectory(1,2)=xc-vc-ac;
        for ns=2:maxlength+1,
            test_trajectory(ns,1)=xr;
            test_trajectory(ns,2)=xc;
            vr=vr+chart(xr,xc,1)+ar;
            vc=vc+chart(xr,xc,2)+ac;
            xr=xr+vr;
            xc=xc+vc;
            ar=0;ac=0;
            if xr<1||xr>schart(1)||xc<1||xc>schart(2)||(vr==0&&vc==0),
                ns=ns+1;
                test_trajectory(ns,1)=xr;
                test_trajectory(ns,2)=xc;
                break;
            end
        end
        y=test_trajectory(1:ns,:);
    end

    function y=single_trajectory_backward(xr,xc,vr,vc,ar,ac)
        test_trajectory(1,1)=xr+vr+chart(xr,xc,1)+ar;
        test_trajectory(1,2)=xc+vc+chart(xr,xc,2)+ac;
        for ns=2:maxlength+1,
            test_trajectory(ns,1)=xr;
            test_trajectory(ns,2)=xc;
            xr=xr-vr;
            xc=xc-vc;
            if xr<1||xr>schart(1)||xc<1||xc>schart(2)||(vr==0&&vc==0),
                ns=ns+1;
                test_trajectory(ns,1)=xr;
                test_trajectory(ns,2)=xc;
                break;
            end
            ar=0;ac=0;
            vr=vr-chart(xr,xc,1)-ar;
            vc=vc-chart(xr,xc,2)-ac;
        end
        y=test_trajectory(ns:-1:1,:);
    end

    function [tar,tac,cost]=convert_trajectory_to_acceleration(ty)
        ny=size(ty,1);
        if ny<2, tar=[]; tac=[]; cost=inf; return; end
        tar=zeros(ny-1,1);
        tac=zeros(ny-1,1);
        xr=ty(1,1);xc=ty(1,2);
        vr=0;vc=0;
        nns=1;
        cost=min(abs(ty(:,1)-rEnd).^2+abs(ty(:,2)-cEnd).^2);
        for ns=1:ny-1,
            vr=vr+chart(xr,xc,1);
            vc=vc+chart(xr,xc,2);
            tar(nns)=ty(ns+1,1)-ty(ns,1)-vr;
            tac(nns)=ty(ns+1,2)-ty(ns,2)-vc;
            if abs(tar(nns))+abs(tac(nns))>maxThrottle
                tar(nns)=-vr;
                tac(nns)=-vc;
                if abs(tar(nns))+abs(tac(nns))>maxThrottle,
                    tar=[];tac=[];
                    %if dodisp,disp(['not possible to fix ',num2str(ty(ns,:)),' to ',num2str(ty(ns+1,:))]);end
                    break;end
                vr=vr+tar(nns);
                vc=vc+tac(nns);
                xr=xr+vr;
                xc=xc+vc;
                nns=nns+1;
                vr=vr+chart(xr,xc,1);
                vc=vc+chart(xr,xc,2);
                tar(nns)=ty(ns+1,1)-ty(ns,1)-vr;
                tac(nns)=ty(ns+1,2)-ty(ns,2)-vc;
                if abs(tar(nns))+abs(tac(nns))>maxThrottle,
                    tar=[];tac=[];
                    %if dodisp,disp(['not possible to fix ',num2str(ty(ns,:)),' to ',num2str(ty(ns+1,:))]);end
                    break;end
                vr=vr+tar(nns);
                vc=vc+tac(nns);
                xr=xr+vr;
                xc=xc+vc;
                nns=nns+1;
                %if dodisp,disp('fixed');end
            else
                vr=vr+tar(nns);
                vc=vc+tac(nns);
                xr=xr+vr;
                xc=xc+vc;
                nns=nns+1;
            end
        end
        cost=cost+sum(abs(tar))+sum(abs(tac));
        if isempty(tar)
            cost=inf;
            if isempty(tar),
                %if dodisp,disp('fix1');end
                if ns==1,vr1=0;vc1=0; else vr1=ty(ns,1)-ty(ns-1,1); vc1=ty(ns,2)-ty(ns-1,2); end
                if ns==ny-1,vr2=0;vc2=0; else vr2=ty(ns+2,1)-ty(ns+1,1); vc2=ty(ns+2,2)-ty(ns+1,2); end
                [tylink,tylinkok]=find_link(ty(ns,1),ty(ns,2),ty(ns+1,1),ty(ns+1,2),vr1,vc1,vr2,vc2,0);
                if tylinkok,
                    tty=[ty(1:ns-1,:);tylink;ty(ns+2:end,:)];
                    [tar,tac,cost]=convert_trajectory_to_acceleration(tty);
                end
            end
            if isempty(tar)||cost>maxcost,
                idxns=find(ty([ns,ns+1],1)==rEnd&ty([ns,ns+1],2)==cEnd,1);
                if ~isempty(idxns)
                    %if dodisp,disp('fix2');end
                    ns=ns+idxns-1;
                    ty1=ty(1:ns-1,:);
                    ty2=ty(ns+1:end,:);
                    d=inf(ns-1,ny-ns-1);
                    for n=2:ns-1,
                        for m=1:ny-ns-1
                            a1 = (ty2(m,:)-ty1(n,:))-( (ty1(n,:)-ty1(n-1,:))+shiftdim(chart(ty1(n,1),ty1(n,2),:),1) );
                            a2 = (ty2(m+1,:)-ty2(m,:))-( (ty2(m,:)-ty1(n,:))+shiftdim(chart(ty2(m,1),ty2(m,2),:),1) );
                            c1 = sum(abs(a1));
                            c2 = sum(abs(a2));
                            d(n,m)=c1+c2+min(abs(ty1(n,1)-rEnd).^2+abs(ty1(n,2)-cEnd).^2,abs(ty2(m,1)-rEnd).^2+abs(ty2(m,2)-cEnd).^2);
                        end
                    end
                    [mind,imind]=sort(d(:));
                    imind=imind(~isinf(mind));
                    for nmind=1:numel(imind),
                        [n,m]=ind2sub(size(d),imind(nmind));
                        [tylink,tylinkok]=find_link(ty1(n,1),ty1(n,2),ty2(m,1),ty2(m,2),ty1(n,1)-ty1(n-1,1),ty1(n,2)-ty1(n-1,2),ty2(m+1,1)-ty2(m,1),ty2(m+1,2)-ty2(m,2),0);
                        if tylinkok,
                            tty=[ty1(1:n-1,:);tylink;ty2(m+1:end,:)];
                            [tar2,tac2,cost2]=convert_trajectory_to_acceleration(tty);
                            if cost2<=cost, tar=tar2;tac=tac2;cost=cost2; break; end
                        end
                    end
                end
            end
        end
        if isempty(tar), cost=inf; end
    end

    function [tylink,ok] = find_link(xr,xc,yr,yc,vxr,vxc,vyr,vyc,level)
        %if dodisp, disp(['iter', num2str([level,xr,xc,yr,yc])]); end
        if level<maxdepth,
            tylink=[];ok=0;
            tr=xr+chart(xr,xc,1)+vxr+DR;
            tc=xc+chart(xr,xc,2)+vxc+DC;
            if ~vxr&&~vxc, idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2)&(tr~=xr|tc~=xc));
            else idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2)); end
            tr=tr(idx);tc=tc(idx);
            
            a1r = (yr-tr)-( (tr-xr)+chart(tr+schart(1)*(tc-1)) );
            a1c = (yc-tc)-( (tc-xc)+chart(tr+schart(1)*(tc-1)+schart(1)*schart(2)) );
            a2r = vyr-( (yr-tr)+chart(yr,yc,1) );
            a2c = vyc-( (yc-tc)+chart(yr,yc,2) );
            cdr=(abs(a1r)+abs(a1c)+abs(a2r)+abs(a2c))+(abs(tr-xr)+abs(tc-xc));
            [nill,idr]=sort(cdr);
            for ndr=1:min(maxwidth,numel(idr)),
                if abs(a1r(idr(ndr)))+abs(a1c(idr(ndr)))<=maxThrottle && abs(a2r(idr(ndr)))+abs(a2c(idr(ndr)))<=maxThrottle,
                    tylink=[xr,xc;tr(idr(ndr)),tc(idr(ndr));yr,yc];
                    ok=1;
                    break;
                elseif tr(idr(ndr))~=xr||tc(idr(ndr))~=xc||vxr||vxc
                    [tylink,ok] = find_link(tr(idr(ndr)),tc(idr(ndr)),yr,yc,tr(idr(ndr))-xr,tc(idr(ndr))-xc,vyr,vyc,level+1);
                    if ok,
                        tylink=cat(1,[xr,xc],tylink);
                        break;
                    end
                end
            end
        else
            tylink=[];ok=0;
        end
    end

%     function disptraj(t)
%         if isempty(t)
%             clf;
%             [ty,tx]=ndgrid(1:schart(1), 1:schart(2));
%             quiver(tx,ty,chart(:,:,2),chart(:,:,1));
%             hold all;
%             plot(cBeg,rBeg,'ks',cEnd,rEnd,'kv','markerfacecolor','k');
%             axis tight;
%         else
%             plot(t(:,2),t(:,1),'.-',t(1:2,2),t(1:2,1),'o');
%         end
%     end
end


function [thrustRow, thrustCol] = nickelfelpeterv2(chart, aIndex, bIndex, maxThrottle)
%REP
%by Gwendolyn Fischer
%r4537 t44

y_winds         = chart(:,:,1);
x_winds         = chart(:,:,2);
[ny,nx]         = size(x_winds);
ay              = rem(aIndex-1, ny) + 1;
ax              = (aIndex - ay)/ny  + 1;
by              = rem(bIndex-1, ny) + 1;
bx              = (bIndex - by)/ny  + 1;
x               = (1:nx);
y               = (1:ny)';
X               = x(ones(ny,1),:);
Y               = y(:,ones(nx,1));
dist_to_B       = (X - bx).^2 + (Y - by).^2;
dist_to_A       = (X - ax).^2 + (Y - ay).^2;

maxIter         = 1034;
SDBiter         = [200 200];
SDBzoom         = [inf 1/2];
INF             = 100000;

dist_to_B_w     = dist_to_B*1e-4;
dist_to_A_w     = dist_to_A*1e-4;

y_dir           = 2*(by > ay) - 1;
x_dir           = 2*(bx > ax) - 1;
fuel            = Inf(ny,nx);
fuel(aIndex)    = 0;
fuel_to_reverse = fuel;
xvel            = zeros(ny,nx);
yvel            = zeros(ny,nx);
checked         = zeros(ny,nx);
previous_stop   = zeros(ny,nx);
min_fuel        = 0;
[cost_to_B cost_to_B_idx] = min(fuel_to_reverse(:) + dist_to_B(:));


for i = 1:length(SDBzoom)
    
    SDB_tf = dist_to_B(:)<=nx*ny*SDBzoom(i)*SDBzoom(i);
    SDB_idx = find(SDB_tf);
    
    SDBchecked = checked(SDB_tf);
    SDBfuel = fuel(SDB_tf);
    SDBfuel_to_reverse = fuel_to_reverse(SDB_tf);
    SDBdist_to_B_w = dist_to_B_w(SDB_tf);
    SDBdist_to_B = dist_to_B(SDB_tf);
    SDBxvel = xvel(SDB_tf);
    SDByvel = yvel(SDB_tf);
    SDBX = X(SDB_tf);
    SDBY = Y(SDB_tf);
    SDBprevious_stop = previous_stop(SDB_tf);
    SDBcost_to_B_idx = find(cost_to_B_idx==SDB_idx);
    
    it = 0;
    
    while (  min_fuel <= cost_to_B  && ~all(SDBchecked) && it < SDBiter(i))
        it                      = it + 1;
        metrix                  = SDBfuel + SDBchecked + SDBdist_to_B_w;
        [dummy,i_p]              = min(metrix(:));
        i_p_full                = SDB_idx(i_p);
        min_fuel                = SDBfuel(i_p);
        i_y                     = rem(i_p_full - 1,ny) + 1;
        i_x                     = (i_p_full - i_y)/ny  + 1;
        i_vy                    = SDByvel(i_p) + y_winds(i_p_full);
        i_vx                    = SDBxvel(i_p) + x_winds(i_p_full);
        i_new_vy                = SDBY - i_y;
        i_new_vx                = SDBX - i_x;
        i_thrust                = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
        i_fuel                  = i_thrust + min_fuel;
        i_reacheable            = (i_thrust <= maxThrottle).*(abs(i_new_vy+y_winds(i_p_full))+abs(i_new_vx+x_winds(i_p_full))<maxThrottle+SDBdist_to_B/4-2);
        i_optimum               = i_fuel < SDBfuel;
        i_impr_tf               = i_reacheable & i_optimum;
        i_impr_idx              = find(i_impr_tf);
        i_new_vx_impr           = i_new_vx(i_impr_idx);
        i_new_vy_impr           = i_new_vy(i_impr_idx);
        i_fuel_impr             = i_fuel  (i_impr_idx) ;
        i_fuel_to_reverse_impr  = i_fuel_impr + ...
            0.2*abs(i_new_vy_impr).*(i_new_vy_impr*y_dir>0) + ...
            0.2*abs(i_new_vx_impr).*(i_new_vx_impr*x_dir>0) ;
        SDBxvel           (i_impr_idx) = i_new_vx_impr;
        SDByvel           (i_impr_idx) = i_new_vy_impr;
        SDBfuel           (i_impr_idx) = i_fuel_impr;
        SDBfuel_to_reverse(i_impr_idx) = i_fuel_to_reverse_impr;
        SDBprevious_stop  (i_impr_idx) = i_p_full;
        if i_impr_tf(SDBcost_to_B_idx)
            [cost_to_B SDBcost_to_B_idx] = min(SDBfuel_to_reverse(:) + SDBdist_to_B(:));
        else
            [cost_to_B_new SDBcost_to_B_idx_new] = min(SDBfuel_to_reverse(i_impr_idx) ...
                + SDBdist_to_B(i_impr_idx));
            
            if cost_to_B_new<cost_to_B
                cost_to_B = cost_to_B_new;
                SDBcost_to_B_idx = i_impr_idx(SDBcost_to_B_idx_new);
            end
        end
        SDBchecked(i_impr_idx)     = 0; %false;
        SDBchecked(i_p)            = INF; %true;
    end
    
    fuel(SDB_tf) = SDBfuel;
    previous_stop(SDB_tf) = SDBprevious_stop;
    xvel(SDB_tf) = SDBxvel;
    yvel(SDB_tf) = SDByvel;
    
end

fuel                    = fuel + dist_to_B;
[cost_to_A cost_to_A_idx] = min(fuel(:) + dist_to_A(:));

checked                 = checked*0;
return_previous_stop    = zeros(ny,nx);

it                      = 0;
min_fuel                = 0;

while ( min_fuel < cost_to_A &&  ~all(checked(:)) && it < maxIter )
    
    it                  = it + 1;
    metrix              = fuel + checked + dist_to_A_w; %****
    [dummy,i_p]             = min(metrix(:)); %****
    min_fuel            = fuel(i_p); %****
    
    i_y                 = rem(i_p - 1,ny) + 1;
    i_x                 = (i_p - i_y)/ny  + 1;
    i_vy                = yvel(i_p) + y_winds(i_p);
    i_vx                = xvel(i_p) + x_winds(i_p);
    i_new_vy            = Y-i_y;
    i_new_vx            = X-i_x;
    i_thrust            = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
    i_fuel              = i_thrust + min_fuel;
    i_reacheable        = i_thrust <= maxThrottle;
    i_optimum           = i_fuel < fuel;
    i_impr_tf           = i_reacheable & i_optimum;
    i_impr_idx          = find(i_impr_tf);
    fuel                (i_impr_idx) = i_fuel(i_impr_idx);
    xvel                (i_impr_idx) = i_new_vx(i_impr_idx);
    yvel                (i_impr_idx) = i_new_vy(i_impr_idx);
    return_previous_stop(i_impr_idx) = i_p;
    if i_impr_tf(cost_to_A_idx)
        [cost_to_A cost_to_A_idx] = min(fuel(:) + dist_to_A(:));
    else
        [cost_to_A_new cost_to_A_idx_new] = min(fuel(i_impr_idx) ...
            + dist_to_A(i_impr_idx));
        if cost_to_A_new<cost_to_A
            cost_to_A = cost_to_A_new;
            cost_to_A_idx = i_impr_idx(cost_to_A_idx_new);
        end
    end
    checked(i_impr_idx) = 0;
    checked(i_p)        = INF;
    
end

cost_to_A           = fuel + dist_to_A;
[dummy,min_fuel]        = min(cost_to_A(:));
indices             = zeros(nx*ny,1);
indices(1)          = min_fuel(1);
next_move           = return_previous_stop(indices(1));
k                   = 1;
[indices, k] = while_next_move(indices, k, next_move, return_previous_stop);
next_move           = previous_stop(indices(k));
[indices, k] = while_next_move(indices, k, next_move, previous_stop);
indices             = indices(k:-1:1);

ypos                = rem(indices - 1, ny) + 1;
xpos                = (indices - ypos)/ny + 1;
xw                  = x_winds(indices);
yw                  = y_winds(indices);

xspeed              = diff(xpos);
yspeed              = diff(ypos);

xdrive              = diff([0; xspeed]);
ydrive              = diff([0; yspeed]);

thrustCol           = xdrive - xw(1:end-1);
thrustRow           = ydrive - yw(1:end-1);

end

function [thrustRow, thrustCol] = nickelfelpeterv3(chart, aIndex, bIndex, maxThrottle)
%continue
%by Sebastian Ullmann
%r4107 t50

dx = rand(1,14);
y_winds         = chart(:,:,1);
x_winds         = chart(:,:,2);
[ny,nx]         = size(x_winds);
ay              = rem(aIndex-1, ny) + 1;
ax              = (aIndex - ay)/ny  + 1;
by              = rem(bIndex-1, ny) + 1;
bx              = (bIndex - by)/ny  + 1;
x               = (1:nx);
y               = (1:ny)';
X               = x(ones(ny,1),:);
Y               = y(:,ones(nx,1));

dist_to_B       = ((X - bx).^2 + (Y - by).^2).^1.15;
dist_to_A       = ((X - ax).^2 + (Y - ay).^2).^1.155;

dist_to_B_w     = dist_to_B*1e-4;
dist_to_A_w     = dist_to_A*1e-4;

fuel            = 100*ones(ny,nx);
fuel(aIndex)    = 0;
xvel            = zeros(ny,nx);
yvel            = xvel;
checked         = xvel;
previous_stop   = xvel;
min_fuel        = 0;
cost_to_B = dist_to_B(aIndex);
zoon = nx*ny*[10 0.37];
ziter = [500 700] .* mxrand(-0.05);
sf = [0.31 0.19];

for i = 1:2
    
    SDB_tf = dist_to_B(:)<=zoon(i);
    SDB_idx = find(SDB_tf);
    
    SDBchecked = checked(SDB_tf);
    SDBfuel = fuel(SDB_tf);
    SDBdist_to_B_w = dist_to_B_w(SDB_tf);
    SDBdist_to_B = dist_to_B(SDB_tf);
    SDBxvel = xvel(SDB_tf);
    SDByvel = yvel(SDB_tf);
    SDBX = X(SDB_tf);
    SDBY = Y(SDB_tf);
    SDBprevious_stop = previous_stop(SDB_tf);
    
    slowdown = maxThrottle+SDBdist_to_B*sf(i);
    
    it = 0;
    
    while (  min_fuel < cost_to_B  && ~all(SDBchecked) && it < ziter(i))
        it                      = it + 1;
        metrix                  = SDBfuel + SDBchecked + SDBdist_to_B_w;
        [dummy,i_p]             = min(metrix);
        i_p_full                = SDB_idx(i_p);
        min_fuel                = SDBfuel(i_p);
        i_y                     = rem(i_p_full - 1,ny) + 1;
        i_x                     = (i_p_full - i_y)/ny  + 1;
        i_y_winds               = y_winds(i_p_full);
        i_x_winds               = x_winds(i_p_full);
        i_vy                    = SDByvel(i_p) + i_y_winds;
        i_vx                    = SDBxvel(i_p) + i_x_winds;
        i_new_vy                = SDBY - i_y;
        i_new_vx                = SDBX - i_x;
        i_thrust                = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
        i_fuel                  = i_thrust + min_fuel;
        i_impr_tf               = (i_thrust <= maxThrottle) & (abs(i_new_vy+i_y_winds)+abs(i_new_vx+i_x_winds)<slowdown) & (i_fuel < SDBfuel);
        i_impr_idx              = find(i_impr_tf);
        if isempty(i_impr_idx)
            SDBchecked(i_p)            = 100; %true;
            continue;
        end
        i_fuel_impr             = i_fuel  (i_impr_idx) ;
        SDBxvel           (i_impr_idx) = i_new_vx(i_impr_idx);
        SDByvel           (i_impr_idx) = i_new_vy(i_impr_idx);
        SDBfuel           (i_impr_idx) = i_fuel_impr;
        SDBprevious_stop  (i_impr_idx) = i_p_full;
        cost_to_B = min(cost_to_B,min(SDBfuel(i_impr_idx) + SDBdist_to_B(i_impr_idx)));
        SDBchecked(i_impr_idx)     = 0; %false;
        SDBchecked(i_p)            = 100; %true;
    end
    
    fuel(SDB_tf) = SDBfuel;
    previous_stop(SDB_tf) = SDBprevious_stop;
    xvel(SDB_tf) = SDBxvel;
    yvel(SDB_tf) = SDByvel;
    
end

fuel                    = fuel + dist_to_B;
cost_to_A               = min(fuel(:) + dist_to_A(:));

checked(:)                 = 0;
return_previous_stop    = checked;

it                      = 0;
min_fuel                = 0;

while ( min_fuel < cost_to_A &&  ~all(checked(:)) && it < min(nx*ny,2000))
    
    it                  = it + 1;
    metrix              = fuel + checked + dist_to_A_w; %****
    [dummy,i_p]             = min(metrix(:)); %****
    min_fuel            = fuel(i_p); %****
    
    i_y                 = rem(i_p - 1,ny) + 1;
    i_x                 = (i_p - i_y)/ny  + 1;
    i_vy                = yvel(i_p) + y_winds(i_p);
    i_vx                = xvel(i_p) + x_winds(i_p);
    i_new_vy            = Y-i_y;
    i_new_vx            = X-i_x;
    i_thrust            = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
    i_fuel              = i_thrust + min_fuel;
    i_impr_tf           = i_thrust <= maxThrottle & i_fuel < fuel;
    i_impr_idx          = find(i_impr_tf);
    if isempty(i_impr_idx)
        checked(i_p)            = 100; %true;
        continue;
    end
    fuel                (i_impr_idx) = i_fuel(i_impr_idx);
    xvel                (i_impr_idx) = i_new_vx(i_impr_idx);
    yvel                (i_impr_idx) = i_new_vy(i_impr_idx);
    return_previous_stop(i_impr_idx) = i_p;
    cost_to_A = min(cost_to_A,min(fuel(i_impr_idx) + dist_to_A(i_impr_idx)));
    checked(i_impr_idx) = 0;
    checked(i_p)        = 100;
    
end

cost_to_A           = fuel + dist_to_A;
[dummy,min_fuel]        = min(cost_to_A(:));
indices             = zeros(nx*ny,1);
indices(1)          = min_fuel(1);
next_move           = return_previous_stop(indices(1));
k                   = 1;
[indices, k] = while_next_move(indices, k, next_move, return_previous_stop);
next_move           = previous_stop(indices(k));
[indices, k] = while_next_move(indices, k, next_move, previous_stop);
indices             = indices(k:-1:1);

ypos                = rem(indices - 1, ny) + 1;
xpos                = (indices - ypos)/ny + 1;
xw                  = x_winds(indices);
yw                  = y_winds(indices);

thrustCol           = diff([0; diff(xpos)]) - xw(1:end-1);
thrustRow           = diff([0; diff(ypos)]) - yw(1:end-1);

end

function [indices, k] = while_next_move(indices, k, next_move, previous_stop)

while(next_move)
    k               = k + 1;
    indices(k)      = next_move;
    next_move       = previous_stop(next_move);
end

end

function r = myrand(x)
r = 1 + x * (rand(1,2) - 0.5) * 2;
end
function r = mxrand(x)
r = 1 + x * rand(1,2);
end

function score = runsolution(thrustRow, thrustCol, colWind, rowWind, aIndex, bIndex)
% RUNSOLUTION Simulates the navigation trajectory given the winds and the
% motor thrust.

[ny,nx]         = size(colWind);

AR              = rem(aIndex-1, ny) + 1;
AC              = (aIndex - AR)/ny  + 1;
BR              = rem(bIndex-1, ny) + 1;
BC              = (bIndex - BR)/ny  + 1;


% Initialize variables at start point (A)
fR = AR; fC =AC;
fvR = 0; fvC = 0;
dB = (fR-BR)^2 + (fC-BC)^2;

for i = 1:numel(thrustRow)
    ivR = fvR + thrustRow(i) + rowWind(fR,fC);
    ivC = fvC + thrustCol(i) + colWind(fR,fC);
    iR = fR + ivR;
    iC = fC + ivC;
    fR = iR;
    fC = iC;
    fvR = ivR;
    fvC = ivC;
    % Verify if this is the closest point to B
    if ((fR-BR)^2 + (fC-BC)^2) < dB
        dB = (fR-BR)^2 + (fC-BC)^2;
    end
end
dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end