BeginningTweaks24

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

ywinds = chart(:,:,1);
xwinds = chart(:,:,2);
[nr,nc] = size(xwinds);
ne = nr*nc;

ay = rem(aIndex-1, nr) + 1;
ax = (aIndex - ay)/nr+ 1;
by = rem(bIndex-1, nr) + 1;
bx = (bIndex - by)/nr+ 1;

x = (1:nc);
y = (1:nr)';
xval = x(ones(nr,1),:);
yval = y(:,ones(nc,1));
position = zeros(nr,nc);
position(:) = 1:ne;

targetScore = 0.024;
maxIter = 2000;

ydirection = 2*(by > ay) - 1;
xdirection = 2*(bx > ax) - 1;
%%
fuel = Inf(nr,nc);
fuel(aIndex) = 0;
fueltoreverse = fuel;
xvel = zeros(nr,nc);

yvel = zeros(nr,nc);
checked = false(nr,nc);
previousstop = zeros(nr,nc);

iteration=0;
minval=0;
costtoB = Inf;
while(minval < costtoB && ~all(checked(:)) && costtoB > targetScore*iteration && iteration<maxIter)
    iteration=iteration+1;
    [minval,minidx]=min(fuel(~checked));
    reference = position(~checked);
    minidx = reference(minidx);
    i_y = rem(minidx-1, size(fuel,1)) + 1;
    i_x = (minidx - i_y)/size(fuel,1) + 1;
    i_vy = yvel(minidx) + ywinds(minidx); %velocity to point minidx plus localwind
    i_vx = xvel(minidx) + xwinds(minidx);
    
    i_thrust = abs(xval-i_x-i_vx)+abs(yval-i_y-i_vy); %xpos=i_x+i_vx
    i_newvy = yval-i_y;
    i_newvx = xval-i_x;
    
    i_fuel = i_thrust + minval; %fuel used to get to position.
    i_improvement = i_thrust < maxThrottle & i_fuel < fuel;
    fuel(i_improvement) = i_fuel(i_improvement);    
    xvel(i_improvement) = i_newvx(i_improvement);
    yvel(i_improvement) = i_newvy(i_improvement);
    
    fueltoreverse(i_improvement) = fuel(i_improvement) + ...
        abs(yvel(i_improvement)).*(yvel(i_improvement)*ydirection>0) + ...
        abs(xvel(i_improvement)).*(xvel(i_improvement)*xdirection>0) ;
    
    previousstop(i_improvement) = minidx;
    
    costtoB = min(min(fueltoreverse + (xval-bx).^2+(yval-by).^2));
    
    checked(i_improvement) = false;
    checked(minidx) = true;
    
end


%%
fuel = fuel + (xval-bx).^2 + (yval-by).^2;


costtoA = min(min(fuel + (xval-ax).^2 + (yval-ay).^2));


checked = false(nr,nc);
returnpreviousstop = zeros(nr,nc);

iteration=0;
minval=0;

while(minval < costtoA && ~all(checked(:)) && (costtoA-costtoB) > targetScore*iteration && iteration<maxIter)
    iteration=iteration+1;
    [minval,minidx]=min(fuel(~checked));
    reference = position(~checked);
    minidx = reference(minidx);
    i_y = rem(minidx-1, size(fuel,1)) + 1;
    i_x = (minidx - i_y)/size(fuel,1) + 1;
    i_vy = yvel(minidx) + ywinds(minidx); %velocity to point minidx plus localwind
    i_vx = xvel(minidx) + xwinds(minidx);
    
    i_thrust = abs(xval-i_x-i_vx)+abs(yval-i_y-i_vy); %xpos=i_x+i_vx
    i_newvy = yval-i_y;
    i_newvx = xval-i_x;
    
    i_fuel = i_thrust + minval; %fuel used to get to position.
    i_improvement = i_thrust < maxThrottle & i_fuel < fuel;
    fuel(i_improvement) = i_fuel(i_improvement);    
    xvel(i_improvement) = i_newvx(i_improvement);
    yvel(i_improvement) = i_newvy(i_improvement);
    
    
    returnpreviousstop(i_improvement) = minidx;
    
    costtoA = min(min(fuel + (xval-ax).^2 + (yval-ay).^2));
    
    checked(i_improvement) = false;
    checked(minidx) = true;
    
end

%%
costtoA = fuel + (xval-ax).^2 + (yval-ay).^2;
[minval,minidx] = min(costtoA(:));
indices = minidx(1);
nextindex = returnpreviousstop(indices(end));
[indices] = lowercyc2(nextindex,indices,returnpreviousstop,previousstop);
indices = rot90(indices,2);

ypos = rem(indices-1, nr) + 1;
xpos = (indices - ypos)/nr + 1;
xw = xwinds(indices); %n+1
yw = ywinds(indices); %n+1

xspeed = diff(xpos); %n
yspeed = diff(ypos); %n

xdrive = diff([0 xspeed]); %n
ydrive = diff([0 yspeed]); %n

[xmotor,ymotor] = lowercyc(xdrive,ydrive,xw,yw);

thrustRow = ymotor;
thrustCol = xmotor;
end
function [indices] = lowercyc2(nextindex,indices,returnpreviousstop,previousstop)
while(nextindex)
    indices(end+1) = nextindex;
    nextindex = returnpreviousstop(indices(end));
end
nextindex = previousstop(indices(end));
while(nextindex)
    indices(end+1) = nextindex;
    nextindex = previousstop(indices(end));
end
end

function [xmotor,ymotor] = lowercyc(xdrive,ydrive,xw,yw)
if(~isempty(xdrive))
    xmotor = xdrive - xw(1:end-1);
    ymotor = ydrive - yw(1:end-1);
    
else
    xmotor = [];
    ymotor = [];
end
end