Still lots of work left (test)

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

rowWind = chart(:,:,1);
colWind = chart(:,:,2);
[nR,nC] = size(rowWind);

[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)),bIndex);

bestScore = inf;

for throttle=1:12
    for diag=[0 1 4]
        for i=1:3
            [tR, tC,vR,vC] = recur(chart, aRow, aCol, bRow, bCol,[],[],maxThrottle,i,0,0,diag,0,throttle);
            [score,iR,iC] = runsolution(tR, tC, chart, aRow, aCol, bRow, bCol,maxThrottle);
            if score < bestScore
                bestScore = score;
                bestRow = tR;
                bestCol = tC;
            end
            if iR>nR || iR<1 || iC>nC || iC<1
                    bestRow =tR(1:end-1);
                    bestCol = tC(1:end-1);
            end
        end
    end
end

% Find a route back home
[score,iR,iC] = runsolution(bestRow,bestCol, chart, aRow, aCol, bRow, bCol,maxThrottle);

for throttle=1:12
    for diag=[0 1 4]
        for i=1:3
            [tR, tC,vR,vC] = recur(chart,iR,iC, aRow, aCol,[],[],maxThrottle,i,0,0,diag,0,throttle);
            [score,iR,iC] = runsolution(tR, tC, chart, aRow, aCol, bRow, bCol,maxThrottle);
            if score < bestScore
                bestScore = score;
                bestRow = tR;
                bestCol = tC;
            end
            if iR>nR || iR<1 || iC>nC || iC<1
                [score,iR,iC] = runsolution(tR(1:end-1), tC(1:end-1), chart, aRow, aCol, bRow, bCol,maxThrottle);
                if score < bestScore
                    bestScore = score;
                    bestRow = tR(1:end-1);
                    bestCol = tC(1:end-1);
                end
                break % out of bounds
            end
            tR = [bestRow -rowWind(iR,iC) tR];
            tC = [bestCol -colWind(iR,iC) tC];
            [score,iR,iC] = runsolution(tR, tC, chart, aRow, aCol, bRow, bCol,maxThrottle);
            if score < bestScore
                bestScore = score;
                bestRow = tR;
                bestCol = tC;
            end
        end
    end
end

return

end

function [tR, tC,vR,vC] = recur(chart, aRow, aCol, bRow, bCol,tR,tC,maxThrottle,dir,vR,vC,diag, counter,throttle)

counter = counter +1;
if(counter >= 200)
    return
end

rowWind = chart(:,:,1);
colWind = chart(:,:,2);
[nR,nC] = size(rowWind);

if aRow>nR || aRow<1 || aCol>nC || aCol<1
    return
end
    
rWind = rowWind(aRow, aCol);
cWind = colWind(aRow, aCol);
rDist = abs(bRow-aRow);
cDist = abs(bCol-aCol);

if dir == 1
    dirR = sign(bRow-aRow)*min(rDist,throttle);
    dirC = sign(bCol-aCol)*min(cDist,throttle);
end
if dir == 2
    dirR = sign(bRow-aRow)*min(rDist,throttle);
    dirC = 0;
end
if dir == 3
    dirR = 0;
    dirC = sign(bCol-aCol)*min(cDist,throttle);
end

if maxThrottle < abs(-rWind+dirR-vR)+abs(-cWind+dirC-vC)
    return
end

tR = [tR, -rWind+dirR-vR];
tC = [tC, -cWind+dirC-vC];

vR = vR + tR(end) + rWind;
vC = vC + tC(end) + cWind;

if(rDist <=0 && cDist <=0)
    return
end

if rDist <=0
    dir = 3;
end
if cDist <=0
    dir = 2;
end
if(rDist == cDist)
    if(diag)
        dir=1;
    end
end
if(diag == 4)
    dir=1;
end

aRow = aRow + dirR;
aCol = aCol + dirC;

[tR, tC,a,s] = recur(chart, aRow, aCol, bRow, bCol,tR,tC,maxThrottle,dir,vR,vC,diag, counter,throttle);

return
end




function [score,iR,iC] = runsolution(thrustRow, thrustCol, chart, AR,AC, BR,BC, maxThrottle)

[thrustRow, thrustCol] = validatesolution(thrustRow, thrustCol, maxThrottle);

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

iR = AR;
iC = AC;
% 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 = scoresolution(dA,dB,thrustRow,thrustCol);

end

function score = scoresolution(dA,dB,thrustRow,thrustCol)
score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end


function [thrustRow, thrustCol] = validatesolution(thrustRow, thrustCol, maxThrottle)

% Check maximum throttle
h = (abs(thrustRow) + abs(thrustCol)) > maxThrottle;
if any(h)
    % Motor is blown for every turn the throttle exceeds the motor capacity!
    thrustRow(h) = 0;
    thrustCol(h) = 0;
end

end