try03

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)

thrustRow=[];thrustCol=[];

maxlength=100;
maxcomp=40;
schart=[size(chart,1),size(chart,2)];
[rBeg,cBeg]=ind2sub(schart,aIndex);
[rEnd,cEnd]=ind2sub(schart,bIndex);

maxthrottle=maxThrottle;
[DR,DC]=ndgrid(-maxthrottle:maxthrottle,-maxthrottle:maxthrottle);
idx=find(abs(DR)+abs(DC)<=maxThrottle);
DR=DR(idx);DC=DC(idx);

tr=rBeg+chart(rBeg,cBeg,1)+DR;
tc=cBeg+chart(rBeg,cBeg,2)+DC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=DR(idx);dc=DC(idx);
tBegf=cell(1,numel(dr));
sBegf=zeros(1,numel(dr));
cBegf=zeros(1,numel(dr));
for niter=1:numel(dr),
    rdr=dr(niter);
    cdc=dc(niter);
    tBegf{niter}=trajforw(rBeg,cBeg,0,0,rdr,cdc);
    sBegf(niter)=size(tBegf{niter},1);
    cBegf(niter)=sum(abs(tBegf{niter}(end-1,:)-[rEnd,cEnd]).^2)+sum(abs(tBegf{niter}(end,:)-tBegf{niter}(end-1,:)));
end
[nill,icBegf]=sort(cBegf);

tr=rEnd-DR;
tc=cEnd-DC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=DR(idx);dc=DC(idx);
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);
    tEndb{niter}=trajback(rEnd,cEnd,rdr,cdc,0,0);
    sEndb(niter)=size(tEndb{niter},1);
    cEndb(niter)=sum(abs(tEndb{niter}(2,:)-[rBeg,cBeg]).^2)+sum(abs(tEndb{niter}(end,:)-tEndb{niter}(end-1,:)));
end
[nill,icEndb]=sort(cEndb);

tr=rEnd+chart(rEnd,cEnd,1)+DR;
tc=cEnd+chart(rEnd,cEnd,2)+DC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=DR(idx);dc=DC(idx);
tEndf=cell(1,numel(dr));
sEndf=zeros(1,numel(dr));
cEndf=zeros(1,numel(dr));
for niter=1:numel(dr),
    rdr=dr(niter);
    cdc=dc(niter);
    tEndf{niter}=trajforw(rEnd,cEnd,0,0,rdr,cdc);
    sEndf(niter)=size(tEndf{niter},1);
    cEndf(niter)=sum(abs(tEndf{niter}(end-1,:)-[rBeg,cBeg]).^2)+sum(abs(tEndf{niter}(1,:)-tEndf{niter}(2,:)));
end
[nill,icEndf]=sort(cEndf);

tr=rBeg-DR;
tc=cBeg-DC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=DR(idx);dc=DC(idx);
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);
    tBegb{niter}=trajback(rBeg,cBeg,rdr,cdc,0,0);
    sBegb(niter)=size(tBegb{niter},1);
    cBegb(niter)=sum(abs(tBegb{niter}(2,:)-[rEnd,cEnd]).^2)+sum(abs(tBegb{niter}(2,:)-tBegb{niter}(1,:)));
end
[nill,icBegb]=sort(cBegb);

[dBegfEndb,iBegfiEndb]=trajdistfb(tBegf,tEndb,sBegf,sEndb,icBegf,icEndb);
[dEndbEndf]=trajdistbf(tEndb,tEndf,sEndb,sEndf,icEndb,icEndf);
[dEndfBegb,iEndfiBegb]=trajdistfb(tEndf,tBegb,sEndf,sBegb,icEndf,icBegb);

c=dBegfEndb;
[c,i1]=min(c,[],1);
c=repmat(c',[1,size(dEndbEndf,2)])+dEndbEndf;
[c,i2]=min(c,[],1);
c=repmat(c',[1,size(dEndfBegb,2)])+dEndfBegb;
[c,i3]=min(c,[],1);
[c,i4]=min(c);
i3=i3(i4);
i2=i2(i3);
i1=i1(i2);

if ~isinf(c),
    [iBegf,iEndb]=ind2sub([size(tBegf{i1},1)-2,size(tEndb{i2},1)-2],iBegfiEndb(i1,i2));iBegf=iBegf+1;iEndb=iEndb+2;
    [iEndf,iBegb]=ind2sub([size(tEndf{i3},1)-2,size(tBegb{i4},1)-2],iEndfiBegb(i3,i4));iEndf=iEndf+1;iBegb=iBegb+2;
    Pos=cat(1,tBegf{i1}(2:iBegf,:),tEndb{i2}(iEndb:end-1,:),tEndf{i3}(2+1:iEndf,:),tBegb{i4}(iBegb:end-1,:));
    [thrustRow,thrustCol]=trajinv(Pos);
end

if isempty(thrustRow)
    [dBegfBegb,iBegfiBegb]=trajdistfbm(tBegf,tBegb,sBegf,sBegb,icBegf,icBegb);
    c=dBegfBegb;
    [c,i1]=min(c,[],1);
    [c,i2]=min(c);
    i1=i1(i2);
    
    if ~isinf(c),
        [iBegf,iBegb]=ind2sub([size(tBegf{i1},1)-2,size(tBegb{i2},1)-2],iBegfiBegb(i1,i2));iBegf=iBegf+1;iBegb=iBegb+2;
        Pos=cat(1,tBegf{i1}(2:iBegf,:),tBegb{i2}(iBegb:end-1,:));
        [thrustRow,thrustCol]=trajinv(Pos);
    end
end


    function [md,id]=trajdistfb(x1,x2,s1,s2,ic1,ic2)
        m1=numel(x1);
        m2=numel(x2);
        md=inf(m1,m2);
        id=zeros(m1,m2);
        for nn1=1:min(m1,maxcomp),
            n1=ic1(nn1);
            if s1(n1)>2,
                for nn2=1:min(m2,maxcomp),
                    n2=ic2(nn2);
                    if s2(n2)>2,
                        dt=abs(x1{n1}(:,ones(1,size(x2{n2},1)))-x2{n2}(:,ones(1,size(x1{n1},1)))')+...
                            abs(x1{n1}(:,2+zeros(1,size(x2{n2},1)))-x2{n2}(:,2+zeros(1,size(x1{n1},1)))');
                        dt2=dt(2:end-1,2:end-1)+dt(3:end,3:end);
                        [md(n1,n2),id(n1,n2)]=min(dt2(:));
                    end
                end
            end
        end
    end

    function [md]=trajdistbf(x1,x2,s1,s2,ic1,ic2)
        m1=numel(x1);
        m2=numel(x2);
        md=inf(m1,m2);
        for nn1=1:min(m1,maxcomp),
            n1=ic1(nn1);
            if s1(n1)>2,
                x1t=x1{n1}(s1(n1)-1:end,:);
                for nn2=1:min(m2,maxcomp),
                    n2=ic2(nn2);
                    if s2(n2)>2,
                        dt=abs(x1t(:,1)-x2{n2}(2:3,1))+...
                            abs(x1t(:,2)-x2{n2}(2:3,2));
                        md(n1,n2)=dt(1)+dt(2);
                    end
                end
            end
        end
    end

    function [md,id]=trajdistfbm(x1,x2,s1,s2,ic1,ic2)
        m1=numel(x1);
        m2=numel(x2);
        md=inf(m1,m2);
        id=zeros(m1,m2);
        for nn1=1:min(m1,maxcomp),
            n1=ic1(nn1);
            if s1(n1)>2,
                dt0=abs(x1{n1}(2:end-1,1)-rEnd).^2+abs(x1{n1}(2:end-1,2)-cEnd).^2;
                for nn2=1:min(m2,maxcomp),
                    n2=ic2(nn2);
                    if s2(n2)>2,
                        dt=abs(x1{n1}(:,ones(1,size(x2{n2},1)))-x2{n2}(:,ones(1,size(x1{n1},1)))')+...
                            abs(x1{n1}(:,2+zeros(1,size(x2{n2},1)))-x2{n2}(:,2+zeros(1,size(x1{n1},1)))');
                        dt2=dt(2:end-1,2:end-1)+dt(3:end,3:end)+dt0(:,ones(1,size(x2{n2},1)-2));
                        [md(n1,n2),id(n1,n2)]=min(dt2(:));
                    end
                end
            end
        end
    end

    function y=trajforw(xr,xc,vr,vc,ar,ac)
        x=zeros(maxlength+2,2);
        x(1,:)=[xr-vr,xc-vc];
        for ns=2:maxlength+1,
            x(ns,:)=[xr,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;
                x(ns,:)=[xr,xc];
                break;
            end
        end
        y=x(1:ns,:);
    end
    function y=trajback(xr,xc,vr,vc,ar,ac)
        x=zeros(maxlength+2,2);
        x(1,:)=[xr+vr+chart(xr,xc,1)+ar,xc+vc+chart(xr,xc,2)+ac];
        for ns=2:maxlength+1,
            x(ns,:)=[xr,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;
                x(ns,:)=[xr,xc];
                break;
            end
            ar=0;ac=0;
            vr=vr-chart(xr,xc,1)-ar;
            vc=vc-chart(xr,xc,2)-ac;
        end
        y=x(ns:-1:1,:);
    end
    function [tar,tac]=trajinv(ty)
        ny=size(ty,1);
        tar=zeros(ny-1,1);
        tac=zeros(ny-1,1);
        xr=ty(1,1);xc=ty(1,2);
        vr=0;vc=0;
        nns=1;
        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=[];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=[];break;end
                vr=vr+tar(nns);
                vc=vc+tac(nns);
                xr=xr+vr;
                xc=xc+vc;
                nns=nns+1;
            else
                vr=vr+tar(nns);
                vc=vc+tac(nns);
                xr=xr+vr;
                xc=xc+vc;
                nns=nns+1;
            end
        end
    end

end