SPLF 5

function [bestRow, bestCol] = solver(chart, aIndex, bIndex, maxThrottle)

[rows, cols, unused] = size(chart);

aRow = mod(aIndex - 1, rows) + 1;
aCol = floor((aIndex - 1) / rows) + 1;
bRow = mod(bIndex - 1, rows) + 1;
bCol = floor((bIndex - 1) / rows) + 1;

[allThrustRow, allThrustCol] = getAllThrusts(maxThrottle);

[initThrustRow,   initThrustCol, history] = thrustToPos(chart,             0,             0, aRow, aCol, bRow, bCol, allThrustRow, allThrustCol, maxThrottle, false);

highScore = inf;
for n=1:size(history, 1)
    [finalThrustRow, finalThrustCol     ] = thrustToPos(chart, history(n, 3), history(n, 4), history(n, 1), history(n, 2), aRow, aCol, allThrustRow, allThrustCol, maxThrottle, true);

    thrustRow = [initThrustRow(1:n) finalThrustRow];
    thrustCol = [initThrustCol(1:n) finalThrustCol];
    [thrustRow thrustCol score] = pruneBasedOnScore(chart, aIndex, bIndex, thrustRow, thrustCol);
    if score < highScore
        highScore = score;
        bestRow = thrustRow;
        bestCol = thrustCol;
    end
end


function [thrustRowVec, thrustColVec, history] = thrustToPos(chart, velRow, velCol, startRow, startCol, endRow, endCol, allThrustRow, allThrustCol, maxThrust, allowDeadNextMove)

history = [];
thrustRowVec = [];
thrustColVec = [];
iterations = 1;
while ~isempty(startRow) && (startRow ~= endRow || startCol ~= endCol) && iterations < 100
    [thrustRow, thrustCol, newVelRow, newVelCol, newStartRow, newStartCol] = aimFor(chart, velRow, velCol, startRow, startCol, endRow, endCol, allThrustRow, allThrustCol, maxThrust, allowDeadNextMove, history);
    if isempty(newStartRow)
        break
    end
    velRow = newVelRow;
    velCol = newVelCol;
    startRow = newStartRow;
    startCol = newStartCol;
    history = [history; startRow startCol velRow velCol];
    thrustRowVec = [thrustRowVec thrustRow];
    thrustColVec = [thrustColVec thrustCol];
    iterations = iterations + 1;
end

function [thrustRow, thrustCol] = getAllThrusts(maxThrottle)

elems = 2 * (maxThrottle ^ 2 + maxThrottle) + 1;
thrustRow = zeros(elems, 1);
thrustCol = zeros(elems, 1);
i = 1;
for r = -maxThrottle:maxThrottle
    maxcThrottle = maxThrottle - abs(r);
    c = -maxcThrottle:maxcThrottle;
    j = i + length(c) - 1;
    thrustCol(i:j) = c;
    thrustRow(i:j) = r;
    i = j + 1;
end

function [thrustRow, thrustCol, velRow, velCol, posRow, posCol] = aimFor(chart, initVelRow, initVelCol, startRow, startCol, endRow, endCol, allThrustRow, allThrustCol, maxThrust, allowDeadNextMove, history)

[velRow, velCol, posRow, posCol] = applyThrust(chart, initVelRow, initVelCol, startRow, startCol, allThrustRow, allThrustCol);
invalid = posRow < 1 | posRow > size(chart, 1) | posCol < 1 | posCol > size(chart, 2);

velRow(invalid) = [];
velCol(invalid) = [];
allThrustRow(invalid) = [];
allThrustCol(invalid) = [];
posRow(invalid) = [];
posCol(invalid) = [];
inertiaRow = [];
inertiaCol = [];

if ~allowDeadNextMove || ~any(posRow == endRow & posCol == endCol)
    windRow = chart(sub2ind(size(chart), posRow, posCol, ones(size(posRow))));
    windCol = chart(sub2ind(size(chart), posRow, posCol, 2 * ones(size(posRow))));
    inertiaRow = posRow + velRow + windRow;
    inertiaCol = posCol + velCol + windCol;
    thrustRequiredToNotDie = max(1 - inertiaRow, 0) + max(inertiaRow - size(chart, 1), 0) + ...
                             max(1 - inertiaCol, 0) + max(inertiaCol - size(chart, 2) , 0);

    invalid = thrustRequiredToNotDie > maxThrust;

    if ~all(invalid)
        velRow(invalid) = [];
        velCol(invalid) = [];
        allThrustRow(invalid) = [];
        allThrustCol(invalid) = [];
        posRow(invalid) = [];
        posCol(invalid) = [];
        windRow(invalid) = [];
        windCol(invalid) = [];
        inertiaRow(invalid) = [];
        inertiaCol(invalid) = [];
    end

    if ~isempty(velRow)
        if all(abs(velRow + windRow) + abs(velCol + windCol) > maxThrust)
            maxThrust = min(abs(velRow + windRow) + abs(velCol + windCol));
        end

        invalid = abs(velRow + windRow) + abs(velCol + windCol) > maxThrust;

        if ~all(invalid)
            velRow(invalid) = [];
            velCol(invalid) = [];
            allThrustRow(invalid) = [];
            allThrustCol(invalid) = [];
            posRow(invalid) = [];
            posCol(invalid) = [];
            inertiaRow(invalid) = [];
            inertiaCol(invalid) = [];
        end
    end
else
    inertiaRow = zeros(size(posRow));
    inertiaCol = zeros(size(posRow));
end

distance = (endRow - posRow) .^ 2 + (endCol - posCol) .^ 2;
distance2 = (endRow - inertiaRow) .^ 2 + (endCol - inertiaCol) .^ 2;
distance = min(distance, distance2);
cost = (abs(allThrustRow) + abs(allThrustCol));
minDistance = min(distance);
minIndex = find(distance == minDistance);
[minCost, minCostIndex] = min(cost(minIndex));
minIndex = minIndex(minCostIndex);
thrustRow = allThrustRow(minIndex);
thrustCol = allThrustCol(minIndex);
velRow = velRow(minIndex);
velCol = velCol(minIndex);
posRow = posRow(minIndex);
posCol = posCol(minIndex);

repeatedPosVel = intersect([posRow posCol velRow velCol], history, 'rows');
if ~isempty(repeatedPosVel)
    thrustRow = [];
    thrustCol = [];
    velRow = [];
    velCol = [];
    posRow = [];
    posCol = [];
end

function [velRow, velCol, posRow, posCol] = applyThrust(chart, initVelRow, initVelCol, startRow, startCol, thrustRow, thrustCol)

velRow = initVelRow + thrustRow + chart(startRow, startCol, 1);
velCol = initVelCol + thrustCol + chart(startRow, startCol, 2);

posRow = startRow + velRow;
posCol = startCol + velCol;

function [thrustRow thrustCol highScore] = pruneBasedOnScore(chart, aIndex, bIndex, thrustRow, thrustCol)

highScore = inf;
index = inf;
for n=0:length(thrustRow)
    [dA, dB] = runsolution(thrustRow(1:n), thrustCol(1:n), chart, aIndex, bIndex);
    score = scoresolution(dA,dB,thrustRow(1:n),thrustCol(1:n));
    if score < highScore
        highScore = score;
        index = n;
    end
end
thrustRow = thrustRow(1:index);
thrustCol = thrustCol(1:index);

function [dA, dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex)

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

function score = scoresolution(dA,dB,thrustRow,thrustCol)

score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));