Mighty ship of steel 1

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
%chart, aIndex, bIndex, maxThrottle %absolute indices, convert with




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

initialPosition = [aRow, aCol];
initialVelocity = [0, 0];

searchDepth = 10;
enginePenalty = 2;
speedLimit = max(maxThrottle*0.5, maxThrottle-3);

thrustRow = zeros(0,1);
thrustCol = zeros(0,1);


%mot B
while (true)

 
    %engine1, engine2, steps
    searchMatrix = Inf*ones(2*maxThrottle+1, 2*maxThrottle+1, searchDepth);
    %engine1, engine2, steps, [row, col, velocityrow, velocitycol]
    searchPositions = -ones(2*maxThrottle+1, 2*maxThrottle+1, searchDepth, 4);
    
    for engine1 = -maxThrottle:maxThrottle
        for engine2 = -(maxThrottle - abs(engine1)):(maxThrottle - abs(engine1))
            
            velocity = initialVelocity + [engine1 engine2];
            position = initialPosition;
            initialDistance = abs(bRow - initialPosition(1)) + abs(bCol - initialPosition(2));
            
            for step = 1:searchDepth
                
                velocity = velocity + [chart(position(1),position(2),1) chart(position(1),position(2),2)];
                position = position + velocity;
                if (position(1) <= 0 || position(2) <= 0 || position(1) > chartSize(1) || position(2) > chartSize(2))
                  
                    break
                elseif (abs(bRow - position(1)) + abs(bCol - position(2)) < initialDistance) && (sum(abs(velocity)) < speedLimit)
                    searchMatrix(engine1+maxThrottle+1, engine2+maxThrottle+1, step) = abs(bRow - position(1)) + abs(bCol - position(2)) + enginePenalty*(abs(engine1) + abs(engine2));
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 1) = position(1);
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 2) = position(2);
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 3) = velocity(1);
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 4) = velocity(2);
                end
                
            end
            
        end
    end
    
    [minPos, minIndex] = min(searchMatrix(:));
    [tR, tC, Nsteps] = ind2sub(size(searchMatrix), minIndex);
    
    if minPos == Inf
        break
    end
    
    tRow = zeros(Nsteps,1);
    tCol = zeros(Nsteps,1);
    tRow(1) = tR - maxThrottle - 1;
    tCol(1) = tC - maxThrottle - 1;
    
    thrustRow = [thrustRow; tRow];
    thrustCol = [thrustCol; tCol];
    
    initialPosition(1) = searchPositions(tR, tC, Nsteps, 1);
    initialPosition(2) = searchPositions(tR, tC, Nsteps, 2);
    initialVelocity(1) = searchPositions(tR, tC, Nsteps, 3);
    initialVelocity(2) = searchPositions(tR, tC, Nsteps, 4);
    
    if (initialPosition(1) == bRow && initialPosition(2) == bCol)
        break
    end
    
end







%mot A
while (true)

 
    %engine1, engine2, steps
    searchMatrix = Inf*ones(2*maxThrottle+1, 2*maxThrottle+1, searchDepth);
    %engine1, engine2, steps, [row, col, velocityrow, velocitycol]
    searchPositions = -ones(2*maxThrottle+1, 2*maxThrottle+1, searchDepth, 4);
    
    for engine1 = -maxThrottle:maxThrottle
        for engine2 = -(maxThrottle - abs(engine1)):(maxThrottle - abs(engine1))
            
            velocity = initialVelocity + [engine1 engine2];
            position = initialPosition;
            initialDistance = abs(aRow - initialPosition(1)) + abs(aCol - initialPosition(2));
            
            for step = 1:searchDepth
                
                velocity = velocity + [chart(position(1),position(2),1) chart(position(1),position(2),2)];
                position = position + velocity;
                if (position(1) <= 0 || position(2) <= 0 || position(1) > chartSize(1) || position(2) > chartSize(2))
                    break
                elseif (abs(aRow - position(1)) + abs(aCol - position(2)) < initialDistance) && (sum(abs(velocity)) < speedLimit)
                    searchMatrix(engine1+maxThrottle+1, engine2+maxThrottle+1, step) = abs(aRow - position(1)) + abs(aCol - position(2)) + enginePenalty*(abs(engine1) + abs(engine2));
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 1) = position(1);
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 2) = position(2);
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 3) = velocity(1);
                    searchPositions(engine1+maxThrottle+1, engine2+maxThrottle+1, step, 4) = velocity(2);
                end
                
            end
            
        end
    end
    
    [minPos, minIndex] = min(searchMatrix(:));
    [tR, tC, Nsteps] = ind2sub(size(searchMatrix), minIndex);
    if minPos == Inf
        break
    end
    
    tRow = zeros(Nsteps,1);
    tCol = zeros(Nsteps,1);
    tRow(1) = tR - maxThrottle - 1;
    tCol(1) = tC - maxThrottle - 1;
    
    thrustRow = [thrustRow; tRow];
    thrustCol = [thrustCol; tCol];
    
    initialPosition(1) = searchPositions(tR, tC, Nsteps, 1);
    initialPosition(2) = searchPositions(tR, tC, Nsteps, 2);
    initialVelocity(1) = searchPositions(tR, tC, Nsteps, 3);
    initialVelocity(2) = searchPositions(tR, tC, Nsteps, 4);
    
    if (initialPosition(1) == bRow && initialPosition(2) == bCol)
        break
    end
    
end







return

end