P2S3Magellan

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

% really don't go out of bounds

iW = chart(:,:,1);
jW = chart(:,:,2);
mt = ceil(2*maxThrottle/3);
[nrow,ncol] = size(iW);
[ai,aj] = ind2sub([nrow,ncol],aIndex);
[bi,bj] = ind2sub([nrow,ncol],bIndex);
thrustRow = zeros(1,1000);
thrustCol = zeros(1,1000);
vws = inf(1,1000);

% set up initial conditions
pi = ai;
pj = aj;
vi = 0; 
vj = 0;
n = 0;
seen = false(nrow,ncol);

% try to approach b
while ((n<1000)&&((pi>=1)&(pj>=1)&(pi<=nrow)&(pj<=ncol))&&~seen(pi,pj)&&((pi~=bi)|(pj~=bj)))
    seen(pi,pj) = true;
    [ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,1000-n);
    [wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,mt,1000-n);
    
    if (isinf(ws(wk)))
        [wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,maxThrottle,1000-n);
        if (isinf(ws(wk)))
            break;
        end;
    end;
    
    % apply winning move
    thrustRow(n+wk) = mi;
    thrustCol(n+wk) = mj;
    n = n+wk;
    pi = ti(wk);
    pj = tj(wk);
    vi = ui(wk+1)+mi;
    vj = uj(wk+1)+mj;
    vws(n) = ws(wk);%+(pi-ai).^2+(pj-aj).^2;
    
    % advance one turn
    pi = pi+vi;
    pj = pj+vj;
end;
bs = ws(wk);
bn = n;

% now try to approach a
sqd = inf;
while (sqd > 1000)&(bn>1)
    seen = false(nrow,ncol);
    sqd = inf;
    while ((n<1000)&&((pi>=1)&(pj>=1)&(pi<=nrow)&(pj<=ncol))&&~seen(pi,pj)&&((pi~=ai)|(pj~=aj)))
        seen(pi,pj) = true;
        [ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,1000-n);
        [wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,mt,1000-n);
        
        if (isinf(ws(wk)))
            [wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,maxThrottle,1000-n);
            if (isinf(ws(wk)))
                break;
            end;
        end;
        
        % apply winning move
        thrustRow(n+wk) = mi;
        thrustCol(n+wk) = mj;
        n = n+wk;
        pi = ti(wk);
        pj = tj(wk);
        vi = ui(wk+1)+mi;
        vj = uj(wk+1)+mj;
        vws(n) = bs+ws(wk);
        sqd = min(sqd,ws(wk));
        
        % advance one turn
        pi = pi+vi;
        pj = pj+vj;
    end;
    if (sqd > 1000)&(bn>1)
        % prepare for repeat
        thrustRow(bn:n) = 0;
        thrustCol(bn:n) = 0;
        bn = bn-1;
        n = bn;
        pi = ai;
        pj = aj;
        vi = 0;
        vj = 0;
        for k = 1:n
            vi = vi+iW(pi,pj)+thrustRow(k);
            vj = vj+jW(pi,pj)+thrustCol(k);
            pi = pi+vi;
            pj = pj+vj;
        end;
    end;
end;

% trim excess
thrustRow = thrustRow(1:n);
thrustCol = thrustCol(1:n);
end


function [wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,gi,gj,iW,jW,mt,N)
n = numel(ti);
wvi = zeros(1,n);
wvj = zeros(1,n);
ws = zeros(1,n);
for k = 1:n
    ks = inf(2*mt+1);
    for i = -mt:mt
        for j = -(mt-abs(i)):(mt-abs(i))
            ks(i+mt+1,j+mt+1) = min(score_glide(gi,gj,ti(k),tj(k),ui(k)+i,uj(k)+j,iW,jW,N-k));
        end;
    end;
    ks(mt+1,mt+1) = inf;
    [m1,m1i] = min(ks);
    [ws(k),m2i] = min(m1);
    wvi(k) = m1i(m2i)-mt-1;
    wvj(k) = m2i-mt-1;
end;
[~,wk] = min(ws);
mi = wvi(wk);
mj = wvj(wk);
end


function [wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,gi,gj,iW,jW,mt,N)
[nrow,ncol] = size(iW);
n = numel(ti);
wvi = zeros(1,n);
wvj = zeros(1,n);
ws = zeros(1,n);
for k = 1:n
    ks = inf(2*mt+1);
    for i = -mt:mt
        for j = -(mt-abs(i)):(mt-abs(i))
            ks(i+mt+1,j+mt+1) = min(score_glide(gi,gj,ti(k),tj(k),ui(k)+i,uj(k)+j,iW,jW,N-k))+controlMargin(ui(k)+i,uj(k)+j,ti(k),tj(k),nrow,ncol,mt);
        end;
    end;
    ks(mt+1,mt+1) = inf;
    [m1,m1i] = min(ks);
    [ws(k),m2i] = min(m1);
    wvi(k) = m1i(m2i)-mt-1;
    wvj(k) = m2i-mt-1;
end;
[mws,wk] = min(ws);
if isinf(mws)
    mi = 0;
    mj = 0;
else
    mi = wvi(wk);
    mj = wvj(wk);
end;
end


function add = controlMargin(vi,vj,pi,pj,nrow,ncol,mt)
if (vi < 0)
    ni = pi-1;
else
    ni = nrow-pi;
end;
if (vj < 0)
    nj = pj-1;
else
    nj = ncol-pj;
end;
si = ceil(abs(vi)/mt);
sj = ceil(abs(vj)/mt);
sij = ceil((abs(vi)+abs(vj))/mt);
margin = -min([0.5*mt*si.^2+abs(vi)*si-ni,0.5*mt*sj.^2+abs(vj)*sj-nj,0.5*mt*sij.^2+(abs(vi)+abs(vj))*sij-(ni+nj)]);
if (margin<0)
    add = inf;
elseif (margin < 10)
    add = 2*nrow*ncol;
elseif (margin < 20)
    add = nrow*ncol;
else
    add = 0;
end;
end


function [s,ti,tj,ui,uj] = score_glide(gi,gj,pi,pj,vi,vj,iW,jW,n)
[ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,n);
s = (ti-gi).^2+(tj-gj).^2;
s(1) = inf;
end


% returns the future trajectory given initial point and velocity
% computes up to n steps ahead
function [ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,n)
[nrow,ncol] = size(iW);
%seen = cell(nrow,ncol);
seen = false(nrow,ncol);
ti = zeros(1,n);
tj = zeros(1,n);
ui = zeros(1,n);
uj = zeros(1,n);
ti(1) = pi;
tj(1) = pj;
ui(1) = vi;
uj(1) = vj;
ui(2) = vi+iW(pi,pj);
uj(2) = vj+jW(pi,pj);
for k = 2:n
    %seen{pi,pj} = [seen{pi,pj}; vi,vj];
    seen(pi,pj) = true;
    vi = ui(k);
    vj = uj(k);
    pi = pi+vi;
    pj = pj+vj;
    if (((pi<1)|(pj<1)|(pi>nrow)|(pj>ncol))||(seen(pi,pj)))
        ti = ti(1:k-1);
        tj = tj(1:k-1);
        ui = ui(1:k);
        uj = uj(1:k);
        break;
    end;
    ti(k) = pi;
    tj(k) = pj;
    ui(k+1) = vi+iW(pi,pj);
    uj(k+1) = vj+jW(pi,pj);
end;
end


function [vpi,vpj,vvi,vvj] = mapMoves(ai,aj,iW,jW,thrustRow,thrustCol)
n = numel(thrustRow)+1;
vpi = zeros(1,n);
vpj = zeros(1,n);
vvi = zeros(1,n);
vvj = zeros(1,n);
vpi(1) = ai;
vpj(1) = aj;
vvi(1) = 0;
vvj(1) = 0;
for k = 1:n-1
    vvi(k+1) = vvi(k)+iW(vpi(k),vpj(k))+thrustRow(k);
    vvj(k+1) = vvj(k)+jW(vpi(k),vpj(k))+thrustCol(k);
    vpi(k+1) = vpi(k)+vvi(k+1);
    vpj(k+1) = vpj(k)+vvj(k+1);
end;
end