Giving it 35%, coach!

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

% Copyright 2010 the cyclist

maxChartDim = sqrt(sum(size(chart(:,:,1)).^2));

[homeRow, homeCol] = ind2sub(size(chart(:,:,1)),aIndex);
[isleRow, isleCol] = ind2sub(size(chart(:,:,1)),bIndex);

thrustRow = [];
thrustCol = [];

currentRow = homeRow;
currentCol = homeCol;

currentRowVelocity = 0;
currentColVelocity = 0;

isleBound  = true;
% disp('isle-bound')
for nturn = 1:maxChartDim,
    % disp('isle-bound')
    [rowThrottle,colThrottle,currentRow,currentCol,currentRowVelocity,currentColVelocity] = calcThrust(homeRow,homeCol,isleRow,isleCol,chart,maxThrottle,isleBound,currentRow,currentCol,currentRowVelocity,currentColVelocity);
    
    thrustRow = [thrustRow; rowThrottle];
    thrustCol = [thrustCol; colThrottle];

    if (currentRow==isleRow) && (currentCol==isleCol)
        break
    end
    
end

isleBound  = false;
% disp('home-bound')
for nturn = 1:maxChartDim,
    [rowThrottle,colThrottle,currentRow,currentCol,currentRowVelocity,currentColVelocity] = calcThrust(homeRow,homeCol,isleRow,isleCol,chart,maxThrottle,isleBound,currentRow,currentCol,currentRowVelocity,currentColVelocity);

    thrustRow = [thrustRow; rowThrottle];
    thrustCol = [thrustCol; colThrottle];
    
    if (currentRow==homeRow) && (currentCol==homeCol)
        break
    end


end

end


function [rowThrottle,colThrottle,currentRow,currentCol,currentRowVelocity,currentColVelocity] = calcThrust(homeRow,homeCol,isleRow,isleCol,chart,maxThrottle,isleBound,currentRow,currentCol,currentRowVelocity,currentColVelocity)

maxVel = floor(max(1,0.35*maxThrottle));

if isleBound
    rowDist =  (isleRow - currentRow);
    colDist =  (isleCol - currentCol);
else
    rowDist = homeRow - currentRow;
    colDist = homeCol - currentCol;    
end

localRowWind = chart(currentRow,currentCol,1); 
localColWind = chart(currentRow,currentCol,2); 

if sign(localRowWind+currentRowVelocity)==sign(rowDist)
    % desiredRowThrust = rowDist - currentRowVelocity - localRowWind; % Maybe make zero instead
    % desiredRowThrust = 0;
    desiredRowThrust = -sign(localRowWind+currentRowVelocity)*(abs(localRowWind+currentRowVelocity)-maxVel);
else
    desiredRowThrust = sign(rowDist)*min(abs(rowDist),1+round(2*rand)) - currentRowVelocity - localRowWind;
end

if sign(localColWind+currentColVelocity)==sign(colDist)
    % desiredColThrust = colDist - currentColVelocity - localColWind; % Maybe make zero instead
    % desiredColThrust = 0;
    desiredColThrust = -sign(localColWind+currentColVelocity)*(abs(localColWind+currentColVelocity)-maxVel);
else
    desiredColThrust = sign(colDist)*min(abs(colDist),1+round(2*rand)) - currentColVelocity - localColWind;
end

rowThrottle = desiredRowThrust;
colThrottle = desiredColThrust;

if (abs(rowThrottle)+abs(colThrottle)) > maxThrottle

    reduceFactor = (abs(rowThrottle)+abs(colThrottle))/maxThrottle;

    rowThrottle = fix(rowThrottle/reduceFactor);
    colThrottle = fix(colThrottle/reduceFactor);

end

% if ((currentRowVelocity + localRowWind + rowThrottle)==0 && ...
%     (currentColVelocity + localColWind + colThrottle)==0) || rand > 1 % How often the crew gets drunk
if  rand > 1 % How often the crew gets drunk
    rowThrottle = fix(rowThrottle+(2*rand-1));
    colThrottle = fix(colThrottle+(2*rand-1));
end

% if ((currentRowVelocity + localRowWind + rowThrottle)==0 && ...
%     (currentColVelocity + localColWind + colThrottle)==0)
%     rowThrottle = rowThrottle + sign(rowDist);
%     colThrottle = colThrottle + sign(colDist);
% end


currentRowVelocity = currentRowVelocity + localRowWind + rowThrottle;
currentColVelocity = currentColVelocity + localColWind + colThrottle;    

currentRow = currentRow + currentRowVelocity;
currentCol = currentCol + currentColVelocity;

if currentRow > size(chart,1)
    adjust = currentRow - size(chart,1);
    currentRow = size(chart,1);
    currentRowVelocity = currentRowVelocity - adjust;
    rowThrottle = rowThrottle - adjust;
    colThrottle=sign(colThrottle)*min(maxThrottle-abs(rowThrottle),abs(colThrottle));
end

if currentCol > size(chart,2)
    adjust = currentCol - size(chart,2);
    currentCol = size(chart,2);
    currentColVelocity = currentColVelocity - adjust;
    colThrottle = colThrottle - adjust;
    rowThrottle=sign(rowThrottle)*min(maxThrottle-abs(colThrottle),abs(rowThrottle));
end

if currentRow < 1
    adjust = 1 - currentRow;
    currentRow = 1;
    currentRowVelocity = currentRowVelocity + adjust;
    rowThrottle = rowThrottle + adjust;
    colThrottle=sign(colThrottle)*min(maxThrottle-abs(rowThrottle),abs(colThrottle));
end

if currentCol < 1
    adjust = 1 - currentCol;
    currentCol = 1;
    currentColVelocity = currentColVelocity + adjust;
    colThrottle = colThrottle + adjust;
    rowThrottle=sign(rowThrottle)*min(maxThrottle-abs(colThrottle),abs(rowThrottle));
end

end