j4

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
    Wr = chart(:,:,1); Wc = chart(:,:,2);
    [Ar, Ac] = ind2sub(size(chart(:,:,1)),aIndex);
    [Br, Bc] = ind2sub(size(chart(:,:,1)),bIndex);
    A = [Ar Ac];
    B = [Br Bc];
    
    %Create available throttle matrix
    T=[];
    for i = -maxThrottle:1:maxThrottle
        for j=-maxThrottle:1:maxThrottle
            if abs(i)+abs(j)<= maxThrottle
                T(end+1,:) = [i j];
            end
        end
    end
    
    IX = A;

MaxPool = 50;
Depth = 5;
SpeedFactor = 1;
S = [];

while true

    IXp = IX;
    Tp = zeros(1,0);
    Vp = [0 0];
    minObj = inf;
    OBJp = inf*ones(1,3);
    
    %Way to island B
    for k = 1:Depth
        [IXn, Tn, Vn, OBJn ] = DoSearch (IXp, Tp, Vp, OBJp, A, B, T, Wr, Wc,SpeedFactor);
        %Merge old and new points
        IXp  = [IXp;IXn];
        Vp   = [Vp;Vn];
        OBJp = [OBJp;OBJn];
        Tp   = [[Tp nan*ones(size(Tp,1), size(Tn,2) - size(Tp,2))]; Tn];
        
        %         IXp = IXn;
        %         Vp = Vn;
        %         OBJp = OBJn;
        %         Tp = Tn;
        
        errTargetB = min(OBJp(:,2));
        
        if size(IXp,1) > MaxPool
            %Delete points over the maximum pool size
            [~,ixSort] = sort(sum(OBJp(:,[2 3]),2));
            IXp(ixSort(MaxPool:end),:)  = [];
            Vp(ixSort(MaxPool:end),:)   = [];
            OBJp(ixSort(MaxPool:end),:) = [];
            Tp(ixSort(MaxPool:end),:)   = [];
        end
    end
    
    %Way back to A
    for k = 1:Depth
        [IXn, Tn, Vn, OBJn ] = DoSearch (IXp, Tp, Vp, OBJp, A,B, T, Wr, Wc,SpeedFactor);
        %Merge old and new points
        IXp  = [IXp;IXn];
        Vp   = [Vp;Vn];
        OBJp = [OBJp;OBJn];
        Tp   = [[Tp nan*ones(size(Tp,1), size(Tn,2) - size(Tp,2))]; Tn];
        
        %         IXp = IXn;
        %         Vp = Vn;
        %         OBJp = OBJn;
        %         Tp = Tn;
        
        errTargetA = min(OBJp(:,1));
        
        if size(IXp,1) > MaxPool
            %Delete points over the maximum pool size
            [~,ixSort] = sort(sum(OBJp,2));
            IXp(ixSort(MaxPool:end),:)  = [];
            Vp(ixSort(MaxPool:end),:)   = [];
            OBJp(ixSort(MaxPool:end),:) = [];
            Tp(ixSort(MaxPool:end),:)   = [];
        end
    end
    sOBJ = sum(OBJp,2);
    [~,ixSort] = sort(sOBJ);
    TpBest = Tp(ixSort(1),:);
    notNAN = ~isnan(TpBest);
    TpBest = Tp(ixSort(1),notNAN);
    thrustRow = TpBest(:,1:2:end-1).';
    thrustCol = TpBest(:,2:2:end).';

    S(end+1).score = sOBJ(ixSort(1));
    S(end).thrustRow = thrustRow;
    S(end).thrustCol = thrustCol;

    if sOBJ(ixSort(1)) > 1000 && SpeedFactor > 0.5
        SpeedFactor = 0.8 * SpeedFactor;
    else
        break
    end
end
score = [S.score];
i = find(score==min(score),1);
thrustRow = S(i).thrustRow;
thrustCol = S(i).thrustCol;

end

function [IXn, Tn, Vn, OBJn ] = DoSearch (IXp, Tp, Vp, OBJp , targetA, targetB, T, Wr, Wc,SpeedFactor)
    IXn  = zeros(0,2);
    Vn  = zeros(0,2);
    Tn   = zeros(0,size(Tp,2)+2);
    OBJn = zeros(0,3);
    
    %     s = size(IXp,1);
    %     t = size(Tp,2);
    %     IXn (1:s, :)   = IXp;
    %     Vn  (1:s, :)   = Vp;
    %     Tn  (1:s, 1:t) = Tp;
    %     OBJn(1:s, :)   = OBJp;
    
    bX = size(Wr,1);
    bY = size(Wr,2);
    
    for i = 1:size(IXp,1)
        W = [Wr(IXp(i,1),IXp(i,2)) Wc(IXp(i,1),IXp(i,2))];
        [IXnew,Vnew, Tnew] = FindAvailableTargets(T, IXp(i,:), Vp(i,:), W,  bX, bY,SpeedFactor);
        
        sn = size(IXnew,1);
        if isfinite(OBJp(i,2))
            oB = OBJp(i,2);
        else
            oB = inf;
        end
        
        OBJnew      = zeros(sn,2);
        OBJnew(:,1) = sum((repmat2(targetA,sn,1) - IXnew).^2,2);
        OBJnew(:,2) = min(oB, sum((repmat2(targetB,sn,1) - IXnew).^2,2));
        notNAN = ~isnan(Tp(i,:));
        OBJnew(:,3) = sum(abs([repmat2(Tp(i,notNAN),sn,1) Tnew]),2);
        
        IXn  (end+1:end+sn,:)  = IXnew;
        Vn   (end+1:end+sn,:)  = Vnew;
        Tn   (end+1:end+sn, :) = [repmat2(Tp(i,:),sn,1) Tnew];
        OBJn (end+1:end+sn,:)  = OBJnew;
    end
    
    
end

function Xn = repmat2(X,r,c)
    s = size(X);
    if s(2) == 1
        Xn = X(:, ones(c,r));
    elseif s(1) == 1
        X = X.';
        Xn = X(:, ones(c,r)).';
    else
        Xn = repmat(X,r,c);
    end
end

function [IXnew, Vnew , Tnew] = FindAvailableTargets(T, IX, V, W, bX, bY,SpeedFactor)
    IXnew = zeros(size(T,1),2);
    Vnew  = zeros(size(T,1),2);
    Tnew  = T;
    ts=size(T,1);
%     for i =1:size(T,1)
%         Vnew(i,:)  = T(i,:) + W + V;
%         IXnew(i,:) = IX + Vnew(i,:);
%     end

    Vnew  = T + repmat2(W,ts,1) + repmat2(V,ts,1);
    IXnew = repmat2(IX,ts,1) + Vnew;


    
    MaxT=max(T(:,1))*SpeedFactor;

    %Avoid points where the bounds are exceeded, and also when the velocity is
    %bigger than the maximum available throttle
    Ierr = IXnew(:,1) < 1 | IXnew(:,1)>bX | IXnew(:,2) < 1 | IXnew(:,2)>bY | abs(Vnew(:,1))>MaxT | abs(Vnew(:,2 ))>MaxT;
    if all(Ierr)
        Ierr = IXnew(:,1) < 1 | IXnew(:,1)>bX | IXnew(:,2) < 1 | IXnew(:,2)>bY;
    end
    IXnew(Ierr,:) = [];
    Vnew(Ierr,:)  = [];
    Tnew(Ierr,:)  = [];
end