Blackbeard

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


thrustRow= [];
thrustCol= [];

rowWind= chart(:,:,1);
colWind= chart(:,:,2);
[rN,cN]= size(rowWind);

Size= [rN,cN];
[rA,cA]= ind2sub([rN,cN],aIndex);
[rB,cB]= ind2sub([rN,cN],bIndex);

PosA= [rA,cA];
PosB= [rB,cB];
Pos= PosA;
Vel= [0,0];
Destination= PosB;
Direction= 0;
Continue= 1;

Step=0;
while (Continue==true)
Step=Step+1;
if (Step==1000) break; end

    rWind= rowWind(Pos(1),Pos(2));
    cWind= colWind(Pos(1),Pos(2));
    Wind= [rWind,cWind];

    Input= [0,0];
    BestInput= Input;
    [BestScore,BestCost]= ScoreInput(Pos,Vel,Wind,Input,rowWind,colWind,Destination,Size,maxThrottle);
    if (isnan(BestScore)) BestScore= -inf; BestCost= nan; end
    
    
    for rInp= -maxThrottle:maxThrottle
    for cInp= -maxThrottle:maxThrottle
    if (abs(rInp)+abs(cInp)<=maxThrottle)
        Input= [rInp,cInp];
        [Score,Cost]= ScoreInput(Pos,Vel,Wind,Input,rowWind,colWind,Destination,Size,maxThrottle);
        if (Score>BestScore) BestInput= Input; BestScore= Score; BestCost= Cost; end
        %
        %if (Step==5) Score, end
        %Score
        %
    end
    end
    end
    
    
    SquDist= SquaredDistance(Pos,Destination);

%[nan nan nan nan nan nan nan Step]
%PosOld= Pos
%Vel
%Wind
%BestInput

    if (BestCost>SquDist)
        if (Direction==0)
            Destination= PosA;
            Direction= 1;
        else
            Continue= 0;
        end
    else
        Pos= Pos+Vel+Wind+BestInput;
        Vel= Vel+Wind+BestInput;
        thrustRow= [thrustRow;BestInput(1)];
        thrustCol= [thrustCol;BestInput(2)];
    end

%PosNew= Pos

if (~ValidPosition(Pos,Size)) return; end
    
end


%[thrustRow,thrustCol]

end




function [Score,Cost]= ScoreInput(Pos,Vel,Wind,Inp,rowWind,colWind,Dest,Size,MaxAcc)

    FirstPos= Pos+Vel+Wind+Inp;
    
    SameSolution= (Pos==FirstPos) & (Inp==-Wind);

    if (ValidPosition(FirstPos,Size)==true & (~SameSolution))
        rWind= rowWind(FirstPos(1),FirstPos(2));
        cWind= colWind(FirstPos(1),FirstPos(2));
        FirstWind= [rWind,cWind];
        
        SecondPosDrift= FirstPos+Vel+Wind+Inp+FirstWind;

        if (DistanceToValidPosition(SecondPosDrift,Size)<round(0.6*MaxAcc))
            CurSquDist= SquaredDistance(Pos,Dest);
            FirstSquDist= SquaredDistance(FirstPos,Dest);
            SecondSquDist= SquaredDistance(SecondPosDrift,Dest);
            NewSquDist= min(FirstSquDist,SecondSquDist);
            Improvement= CurSquDist-NewSquDist;
        
            Cost= sum(abs(Inp));
            %Penalty= (Cost+1)^2;
            Penalty= Cost+1;
            if (Improvement<0) Penalty= 1/Penalty; end
        
            Score= Improvement/Penalty;
        else
            Score= nan;
            Cost= nan;
        end
    else
        Score= nan;
        Cost= nan;
    end
end



function Valid = ValidPosition(Pos,Size)
    if (Pos(1)<1 | Pos(1)>Size(1) | Pos(2)<1 | Pos(2)>Size(2)) Valid= false;
    else Valid= true; end
end

function Distance = DistanceToValidPosition(Pos,Size)
    Distance= [];
    if (Pos(1)<1) Distance= [Distance,1-Pos(1)]; end
    if (Pos(1)>Size(1)) Distance= [Distance,Pos(1)-Size(1)]; end
    if (Pos(2)<1) Distance= [Distance,1-Pos(2)]; end
    if (Pos(2)>Size(2)) Distance= [Distance,Pos(2)-Size(2)]; end
    if (numel(Distance)>0) Distance= max(Distance); else Distance= 0; end
end


function SquDist = SquaredDistance(PosA,PosB)
    SquDist= (PosA(1)-PosB(1))^2+(PosA(2)-PosB(2))^2;
end