Spaceship 7

function [rT, cT] = spaceship(chart, a, b, mT)
%

% Spaceship 2
% results: 67580
% time: 0.16
% complexity: 14
% node count: 639?

% Spaceship 3
% results: 46675
% time: 0.23
% complexity: 15
% node count: 735?

% Spaceship 4
% results: 29664
% time: 0.25
% complexity: 15
% node count: 755?

% Spaceship 5
% results: 25612
% time: 0.29
% complexity: 15
% node count: 784?

% Spaceship 7
% results: 25532
% time: 0.78
% complexity: 15
% node count: 786?

% Out of turns
% 148, (124), 119, 77, 39, (32), 15

global verbose
verbose = false;

% winds
rW = chart(:,:,1);
cW = chart(:,:,2);

% dimensions
[m, n] = size(rW);

% start
[ai, aj] = ind2sub([m n], a);
% island
[bi, bj] = ind2sub([m n], b);

% current pos
pi0 = ai;
pj0 = aj;

% current vel
vi0 = 0;
vj0 = 0;

N = 20;
dMax = 2*mT;
B = 4^2;

% 1st target: try to reach b
dM = dMax + 1;
k = -1;
while k == -1 && dM > 1
    dM = dM - 1;
    if verbose,
        fprintf('trying dM = %d\n', dM)
    end
    [k, pi, pj, vi, vj, rT1, cT1] = reachTarget(0, pi0, pj0, vi0, vj0, ...
        bi, bj, rW, cW, mT, dM, B, N);

    if k > 0 && k < 1000
        % 2st target: try to return to a
        [k, pi, pj, vi, vj, rT2, cT2] = reachTarget(k, pi, pj, vi, vj, ...
            ai, aj, rW, cW, mT, dM, B, N);
    else
        rT2 = [];
        cT2 = [];
    end
end

if k == -1
    fprintf('Failed\n')
end

% thrust
rT = [rT1; rT2];
cT = [cT1; cT2];

if numel(rT1) == N || numel(rT2) == N
    fprintf('Out of turns: %d, %d\n', numel(rT1) == N, numel(rT2) == N)
end

end

function [k, pi, pj, vi, vj, rT, cT] = reachTarget(k0, pi, pj, vi, vj, ...
    ti, tj, rW, cW, mT, dMax, B, N)

global verbose

rT = zeros(N,1);
cT = zeros(N,1);

dM = dMax;

% dimensions
[m, n] = size(rW);

for k = 1:N % N turns one away, N the other
    
    if verbose
        fprintf('turn %d: pos = (%d,%d), vel = (%d,%d)\n', ...
            k0+k, pi, pj, vi, vj)
    end
    
    % wind
    wi = rW(pi, pj);
    wj = cW(pi, pj);

    qi = 1; qj = 1;
    
    while qi ~= 0 || qj ~= 0
        % desired direction
        di = sat(ti - pi, dM);
        dj = sat(tj - pj, dM);

        % thrust
        r = di - (vi + wi);
        c = dj - (vj + wj);
        [r, c] = sat2(r, c, mT);
        
        % update position
        [npi, nvi, r, qi] = keepInMap(pi, vi, r, wi, m, mT-abs(c));
        [npj, nvj, c, qj] = keepInMap(pj, vj, c, wj, n, mT-abs(r));
        
        if abs(r) + abs(c) > mT
            fprintf('Motor blown.\n')
        end
        
        if qi ~= 0 || qj ~= 0
            dM = dM - 1; % move slower
        else
            dM = min(dM + 1, dMax); % move faster
        end
        if dM == 0
            break;
        end
    end
    
    pi = npi;
    pj = npj;
    vi = nvi;
    vj = nvj;
    
    % check bounds
    if qi ~= 0 || qj ~= 0
        if verbose
            fprintf(['next move would place ship out of bounds: ' ...
                'turn %d, stopped at (%d,%d), wind (%d,%d)\n'], ...
                k0+k, pi, pj, wi, wj)
        end
        rT = rT(1:k-1);
        cT = cT(1:k-1);
        k = -1;
        return;
    end

    % save thrust
    rT(k) = r;
    cT(k) = c;
    
    % reached target?
    %if pi == ti && pj == tj
    % close enough
    %dT = abs(pi - ti) + abs(pj - tj);
    dT = (pi - ti)^2 + (pj - tj)^2;
    if dT <= B
        rT = rT(1:k);
        cT = cT(1:k);
        if verbose
            %fprintf('reached target (%d,%d)\n', ti, tj)
            fprintf('changed target at (%d,%d), distance %d\n', ...
                ti, tj, dT)
        end
        return;
    end
    
end

if verbose
    fprintf('out of turns (%d): stopped at (%d,%d)\n', ...
        k, pi, pj)
end

end

function u = sat(u, mT)

u = sign(u)*min(mT, abs(u));

end

function [u, v] = sat2(u0, v0, mT)

u = u0;
v = v0;
n1 = abs(u) + abs(v);
while n1 > mT
    u = u - sign(u);
    v = v - sign(v);
    n1 = abs(u) + abs(v);
end

end

function [p, v, t, q] = keepInMap(p0, v0, t, w, d, m)

t = sat(t, m);
v = v0 + t + w;
p = p0 + v;
q = 1*(p < 1) - 1*(p > d); % = 1 if p < 1, -1 if p > n
while (t < m && q == 1) || (t > 0 && q == -1)
    t = t + q;
    v = v0 + t + w;
    p = p0 + v;
    q = 1*(p < 1) - 1*(p > d);
end

end