TimS-V1

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Tim's solver

rowWind = 1;
colWind = 2;
[nRow nCol] = size(chart(:,:,1));
[aRow, aCol] = ind2sub([nRow nCol],aIndex);
[bRow, bCol] = ind2sub([nRow nCol],bIndex);

vRow = 0;
vCol = 0;
lRow = aRow;
lCol = aCol;
cRow = lRow;
cCol = lCol;

%Tuning variables
thrustscale = 0.5;
recursedepth = 5;


thrustRow = [];
thrustCol = [];

bestsol = gradesolution(lRow, aRow, lCol, aCol, ...
        lRow, bRow, lCol, bCol, thrustRow, thrustCol);
closestb = (lRow - bRow)^2 + (lCol - bCol)^2;

visitedb = 0;
turn = 0;
while turn <= 1000
    turn = turn+1;
    %Work out if aiming for a or b
    if visitedb
        tRow = aRow;
        tCol = aCol;
    else
        tRow = bRow;
        tCol = bCol;
    end
    
    %Work out which way to go
    goRow = tRow-lRow;
    goCol = tCol-lCol;
    
    %Check if already at target
    if goRow == 0 && goCol == 0
        %If yes, if already visited b, then finish, otherwise, go back to a
        if visitedb
            break
        else
            visitedb = 1;
            cRow = bRow;
            cCol = bCol;
            closesta = (lRow - aRow)^2 + (lCol - aCol)^2;
        end
    end
    
    %Once this far, there is still work to do
    thrustRowtest = round(goRow*thrustscale)-vRow;
    thrustColtest = round(goCol*thrustscale)-vCol;
    
    %Reduce thrust if breaking max throttle rule
    if abs(thrustRowtest) + abs(thrustColtest) > maxThrottle
        totalthrusttest = abs(thrustRowtest) + abs(thrustColtest);
        thrustRowtest = round((goRow*thrustscale-vRow)/totalthrusttest*maxThrottle);
        thrustColtest = round((goCol*thrustscale-vCol)/totalthrusttest*maxThrottle);
        while abs(thrustRowtest) + abs(thrustColtest) > maxThrottle
            %Randomly reduce thrust in one direction
            if round(rand)
                thrustRowtest = thrustRowtest-(1*sign(thrustRowtest));
            else
                thrustColtest = thrustRowtest-(1*sign(thrustRowtest));
            end
        end
    end
    
    
    newvRow = vRow+thrustRowtest+chart(lRow,lCol,rowWind);
    newvCol = vCol+thrustColtest+chart(lRow,lCol,colWind);
    
    newlRow = lRow + newvRow;
    newlCol = lCol + newvCol;
    
    try 
        chart(newlRow, newlCol, rowWind);
    catch
        return
    end
    
    if ~visitedb
        newclosestb = (newlRow - bRow)^2 + (newlCol - bCol)^2;
        if newclosestb < closestb
            newcRow = newlRow;
            newcCol = newlCol;
        else
            newcRow = cRow;
            newcCol = cCol;
        end
    end
%         else
%         newclosesta = (newlRow - aRow)^2 + (newlCol - aCol)^2;
%         if newclosesta < closesta
%             acceptthrust = 1;
%         end
    
    result = gradesolution(newlRow, aRow, newlCol, aCol, ...
        newcRow, bRow, newcCol, bCol, ...
        [thrustRow; thrustRowtest;], [thrustCol; thrustColtest;]);
    
    if result < bestsol
        thrustRow(turn,1) = thrustRowtest;
        thrustCol(turn,1) = thrustColtest;
        vRow = newvRow;
        vCol = newvCol;
        lRow = newlRow;
        lCol = newlCol;
        cRow = newcRow;
        cCol = newcCol;
        bestsol = result;
    else
        return;
    end
    
    
end
    
    


end


function result = gradesolution(finalRow, aRow, finalCol, aCol, ...
        closestRow, bRow, closestCol, bCol, thrustRow, thrustCol)

    result = (finalRow - aRow)^2 + (finalCol - aCol)^2 + ...
        (closestRow - bRow)^2 + (closestCol - bCol)^2 + ...
        sum(abs(thrustRow)) + sum(abs(thrustCol));

end