WeekendParty7

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

[thrustRowb, thrustColb] = nickelfel(chart, aIndex, bIndex, maxThrottle);
scoreb = runsolution(thrustRowb, thrustColb, chart, aIndex, bIndex);
if scoreb>40
    [thrustRowa, thrustCola] = nick(chart, aIndex, bIndex, maxThrottle);
else
    thrustCol = thrustColb;
    thrustRow = thrustRowb;
    return;
end
scorea = runsolution(thrustRowa, thrustCola, chart, aIndex, bIndex);
if scorea < scoreb
    thrustCol = thrustCola;
    thrustRow = thrustRowa;
else
    thrustCol = thrustColb;
    thrustRow = thrustRowb;
end

end

function [n,thrustRow,thrustCol,pi,pj,vi,vj,dB,bs,bn] = firstwhile(chart, aIndex, bIndex, maxThrottle)
iW = chart(:,:,1);
jW = chart(:,:,2);
nrow = size(iW,1);

ai = rem(aIndex-1, nrow) + 1;
aj = (aIndex - ai)/nrow + 1;
bi = rem(bIndex-1, nrow) + 1;
bj = (bIndex - bi)/nrow+ 1;
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;
mtt = 401;

mt = ceil(maxThrottle/2);
mt2 = ceil(3*maxThrottle/4);
sqd = inf;
[nrow,ncol] = size(iW);

ai = rem(aIndex-1, nrow) + 1;
aj = (aIndex - ai)/nrow + 1;
while (sqd>100)
    sqd = inf;
    mcum = 0;
    stopn = 0;
    srec = [pi pj vi vj];
    while ((n<1000)&&((pi>=1)&&(pj>=1)&&(pi<=nrow)&&(pj<=ncol))&&((pi~=bi)|(pj~=bj)))
        [gs,ti,tj,ui,uj] = score_glide2(bi,bj,pi,pj,vi,vj,iW,jW,1000-n,mt);
        [mgs,mgsi] = min(gs);
        if (mgs > 1)
            [wk,mi,mj,ws] = lesscycstill(n,sqd,mtt,ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,mt,mt2)            ;
            if (isinf(ws(wk)))
                break;
            end;
        else
            ws = inf;
            wk = 1;
            mi = 0;
            mj = 0;
        end;
        
        if (mgs<100)&&(mgs <= ws(wk)+abs(mi)+abs(mj))
            % end early
            n = n+mgsi-1;
            pi = ti(mgsi);
            pj = tj(mgsi);
            vi = ui(mgsi);
            vj = uj(mgsi);
            vws(n) = mgs;
            break
        end;
        
        % stop if too much motor
        mcum = mcum+abs(mi)+abs(mj);
        if (mcum>sqd)
            % undo moves:
            thrustRow(stopn+1:n) = 0;
            thrustCol(stopn+1:n) = 0;
            n = stopn;
            % return to position:
            pi = srec(1);
            pj = srec(2);
            vi = srec(3);
            vj = srec(4);
            pi = pi+vi;
            pj = pj+vj;
            % add coasting
            [gs,ti,tj,ui,uj] = score_glide2(bi,bj,pi,pj,vi,vj,iW,jW,1000-n,mt);
            [~,mgsi] = min(gs);
            n = n+mgsi-1;
            pi = ti(mgsi);
            pj = tj(mgsi);
            vi = ui(mgsi);
            vj = uj(mgsi);
            break;
        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;
        if (ws(wk)<sqd)
            sqd = ws(wk);
            mcum = 0;
            stopn = n;
            srec = [pi pj vi vj];
        end;
        
        % advance one turn
        pi = pi+vi;
        pj = pj+vj;
    end;
    if (sqd > 100)
        if (mt < mt2)
            % reset and try again with full thrust
            thrustRow = zeros(1,1000);
            thrustCol = zeros(1,1000);
            vws = inf(1,1000);
            pi = ai;
            pj = aj;
            vi = 0;
            vj = 0;
            n = 0;
            mt = mt2;
        elseif (mt < maxThrottle)
            % reset and try again with full thrust
            thrustRow = zeros(1,1000);
            thrustCol = zeros(1,1000);
            vws = inf(1,1000);
            pi = ai;
            pj = aj;
            vi = 0;
            vj = 0;
            n = 0;
            mt = maxThrottle;
        else
            % give up
            sqd = 0;
        end;
    end;
end;
bs = ws(wk);
bn = n;
[vpi,vpj] = mapMoves(ai,aj,iW,jW,thrustRow(1:n),thrustCol(1:n));
dB = (vpi-bi).^2+(vpj-bj).^2;
end
function [wk,mi,mj,ws] = lesscycstill(n,sqd,mtt,ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,mt,mt2)
if (n>0)&&(sqd>mtt)
    % use maxThrottle to get out of difficult situation
    [wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,1000-n);
else
    [wk,mi,mj,ws] = lowercyc1(ti,tj,ui,uj,bi,bj,iW,jW,mt2,n,maxThrottle,mt);
end;
end
function [thrustRow, thrustCol] = nick(chart, aIndex, bIndex, maxThrottle)

iW = chart(:,:,1);
jW = chart(:,:,2);
mt2 = ceil(3*maxThrottle/4);
[nrow,ncol] = size(iW);

ai = rem(aIndex-1, nrow) + 1;
aj = (aIndex - ai)/nrow + 1;
bi = rem(bIndex-1, nrow) + 1;
bj = (bIndex - bi)/nrow+ 1;
vws = inf(1,1000);

% set up initial conditions
mtt = 400;

% try to approach b
[n,thrustRow,thrustCol,pi,pj,vi,vj,dB,bs,bn] = firstwhile(chart, aIndex, bIndex, maxThrottle);

mt = ceil(2*maxThrottle/3);
% now try to approach a
sqd = inf;
while (bn>1)&&(sqd > dB(bn-1))
    sqd = inf;
    mcum = 0;
    stopn = bn;
    srec = [pi pj vi vj];
    while ((n<1000)&&((pi>=1)&(pj>=1)&(pi<=nrow)&(pj<=ncol))&&((pi~=ai)|(pj~=aj)))
        [gs,ti,tj,ui,uj] = score_glide(ai,aj,pi,pj,vi,vj,iW,jW,1000-n);
        [mgs,mgsi] = min(gs);
        if (mgs > 1)
            if (n>bn)&(sqd>mtt)
                % use maxThrottle to get out of difficult situation
                [wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,maxThrottle,1000-n);
            else
                [wk,mi,mj,ws] = lowercyc2(ti,tj,ui,uj,ai,aj,iW,jW,mt2,n,maxThrottle,mt);
            end;
            if (isinf(ws(wk)))
                break;
            end;
        else
            ws = inf;
            wk = 1;
            mi = 0;
            mj = 0;
        end;
        
        if (mgs<100)&(mgs <= ws(wk)+abs(mi)+abs(mj))
            % end early
            n = n+mgsi-1;
            pi = ti(mgsi);
            pj = tj(mgsi);
            vi = ui(mgsi);
            vj = uj(mgsi);
            vws(n) = mgs;
            break
        end;
        
        % stop if too much motor
        mcum = mcum+abs(mi)+abs(mj);
        if (isinf(ws(wk)))|(mcum>sqd)
            % undo moves:
            thrustRow(stopn+1:n) = 0;
            thrustCol(stopn+1:n) = 0;
            n = stopn;
            % return to position:
            pi = srec(1);
            pj = srec(2);
            vi = srec(3);
            vj = srec(4);
            pi = pi+vi;
            pj = pj+vj;
            % add coasting
            [gs,ti,tj,ui,uj] = score_glide2(bi,bj,pi,pj,vi,vj,iW,jW,1000-n,mt);
            [~,mgsi] = min(gs);
            n = n+mgsi-1;
            pi = ti(mgsi);
            pj = tj(mgsi);
            vi = ui(mgsi);
            vj = uj(mgsi);
            break;
        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,mcum,stopn,srec] = lesscyc(ws,wk,sqd,n,pi,pj,vi,vj)        ;
        % advance one turn
        pi = pi+vi;
        pj = pj+vj;
    end;
    [thrustRow,thrustCol,bn,n,pi,pj,vi,vj,sqd] = lowercyc7(bn,sqd,dB,n,ai,aj,thrustRow,thrustCol,pi,pj,vi,vj);
end;

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

function [sqd,mcum,stopn,srec] = lesscyc(ws,wk,sqd,n,pi,pj,vi,vj)
if (ws(wk)<sqd)
    sqd = ws(wk);
    mcum = 0;
    stopn = n;
    srec = [pi pj vi vj];
end;
end

function [thrustRow,thrustCol,bn,n,pi,pj,vi,vj,sqd] = lowercyc7(bn,sqd,dB,n,ai,aj,thrustRow,thrustCol,pi,pj,vi,vj)
if (bn>1)&&(sqd > dB(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;
    sqd = inf;
end;
end




function [wk,mi,mj,ws] = lowercyc2(ti,tj,ui,uj,ai,aj,iW,jW,mt2,n,maxThrottle,mt)
[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,mt2,1000-n);
    if (isinf(ws(wk)))
        [wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,maxThrottle,1000-n);
    end;
end;
end
function [wk,mi,mj,ws] = lowercyc1(ti,tj,ui,uj,bi,bj,iW,jW,mt2,n,maxThrottle,mt)
[wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,mt,1000-n);
if (isinf(ws(wk))&(mt~=maxThrottle))
    [wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,mt2,1000-n);
    if (isinf(ws(wk))&(mt~=maxThrottle))
        [wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,1000-n);
    end;
end;
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))+abs(i)+abs(j);
        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;
    ws(k) = ws(k)-abs(wvi(k))-abs(wvj(k));
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)

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_glide2(gi,gj,ti(k),tj(k),ui(k)+i,uj(k)+j,iW,jW,N-k,mt))+abs(i)+abs(j);
        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;
    ws(k) = ws(k)-abs(wvi(k))-abs(wvj(k));
end;
[mws,wk] = min(ws);
if isinf(mws)
    mi = 0;
    mj = 0;
else
    mi = wvi(wk);
    mj = wvj(wk);
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


function [s,ti,tj,ui,uj] = score_glide2(gi,gj,pi,pj,vi,vj,iW,jW,n,mt)
[ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,n);
s = (ti-gi).^2+(tj-gj).^2+max(0,abs(ui(2:end))+abs(uj(2:end))-mt/3).^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);
n = min(ceil(sqrt(nrow*ncol)),n);
%seen = cell(nrow,ncol);
seen = false(nrow,ncol);
ti = zeros(1,n);
tj = ti;
ui = ti;
uj = ti;
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) = 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);
vpi = zeros(1,n+1);
vpj = zeros(1,n+1);
vvi = zeros(1,n+1);
vvj = zeros(1,n+1);
vpi(1) = ai;
vpj(1) = aj;
vvi(1) = 0;
vvj(1) = 0;
for k = 1:n
    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

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

rowWind = chart(:,:,1);
colWind = chart(:,:,2);
[nR,nC] = size(rowWind);
[AR,AC] = ind2sub([nR,nC],aIndex);
[BR,BC] = ind2sub([nR,nC],bIndex);

% 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;
    if iR>nR || iR<1 || iC>nC || iC<1
        break % out of bounds
    end
    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

function [ai,aj] = ind2sub(sz,aIndex)
ai = rem(aIndex-1, sz(1)) + 1;
aj = (aIndex - ai)/sz(1)+ 1;
end

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

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));
targetScore     = 0.013;
maxIter         = 1950;
dist_to_B       = (X - bx).^2 + (Y - by).^2;
dist_to_A       = (X - ax).^2 + (Y - ay).^2;
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         = false(ny,nx);
previous_stop   = zeros(ny,nx);
INF             = 100000;
it              = 0;
min_fuel        = 0;
cost_to_B       = Inf;

while (  min_fuel < cost_to_B  && ~all(checked(:)) && cost_to_B > targetScore*it && it < maxIter)
    it                      = it + 1;
    fuel_not_checked        = fuel + INF*checked;
    [min_fuel,i_p]          = min(fuel_not_checked(:));
    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                  = i_reacheable & i_optimum;
    i_new_vx_impr           = i_new_vx(i_impr);
    i_new_vy_impr           = i_new_vy(i_impr);
    i_fuel_impr             = i_fuel  (i_impr);
    i_fuel_to_reverse_impr  = i_fuel_impr + ...
                                abs(i_new_vy_impr).*(i_new_vy_impr*y_dir>0) + ...
                                abs(i_new_vx_impr).*(i_new_vx_impr*x_dir>0) ;
    xvel           (i_impr) = i_new_vx_impr;
    yvel           (i_impr) = i_new_vy_impr;
    fuel           (i_impr) = i_fuel_impr;
    fuel_to_reverse(i_impr) = i_fuel_to_reverse_impr;
    previous_stop  (i_impr) = i_p;
    cost_to_B               = min(min(fuel_to_reverse + dist_to_B));
    checked(i_impr)         = false;
    checked(i_p)            = true;
end

fuel                    = fuel + dist_to_B;
cost_to_A               = min(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(:)) && (cost_to_A - cost_to_B) > targetScore*it  && it < maxIter )
    
    it                  = it + 1;
    fuel_not_checked    = fuel + INF*checked;
    [min_fuel,i_p]      = min(fuel_not_checked(:));
    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              = i_reacheable & i_optimum;
    fuel                (i_impr) = i_fuel(i_impr);
    xvel                (i_impr) = i_new_vx(i_impr);
    yvel                (i_impr) = i_new_vy(i_impr);
    return_previous_stop(i_impr) = i_p;
    cost_to_A           = min(min(fuel + dist_to_A));
    checked(i_impr)     = false;
    checked(i_p)        = true;
    
end

cost_to_A           = fuel + dist_to_A;
[~,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;
while(next_move)
    k               = k + 1;
    indices(k)      = next_move;
    next_move       = return_previous_stop(next_move);
end
next_move           = previous_stop(indices(k));
while(next_move)
    k               = k + 1;
    indices(k)      = next_move;
    next_move       = previous_stop(next_move);
end
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]);

[xmotor,ymotor]     = lowercyc(xdrive,ydrive,xw,yw,xpos,ypos,xspeed,yspeed,ax,ay,maxThrottle);

thrustRow           = ymotor;
thrustCol           = xmotor;

end

function [xmotor,ymotor] = lowercyc(xdrive,ydrive,xw,yw,xpos,ypos,xspeed,yspeed,ax,ay,maxThrottle)

if (~isempty(xdrive))
    
    xmotor      = xdrive - xw(1:end-1);
    ymotor      = ydrive - yw(1:end-1);
    
    x_tomove    = ax - xpos(end);
    x_todrive   = x_tomove - xspeed(end) - xw(end);
    y_tomove    = ay - ypos(end);
    y_todrive   = y_tomove - yspeed(end) - yw(end);
    
    if abs(x_todrive) + abs(y_todrive) < maxThrottle
        
        if abs(x_todrive) + abs(y_todrive) <  abs(ax-xpos(end)) + abs(ay-ypos(end))
            
            xmotor = [xmotor; x_todrive];
            ymotor = [ymotor; y_todrive];
            
        end
    end
    
else
    xmotor = [];
    ymotor = [];
end

end