learning5

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

y_winds         = chart(:,:,1);
x_winds         = chart(:,:,2);
[ny,nx]         = size(x_winds);
ay              = rem(aIndex-1, ny) + 1;
ax              = (aIndex - ay)/ny  + 1;
by              = rem(bIndex-1, ny) + 1;
bx              = (bIndex - by)/ny  + 1;
x               = (1:nx);
y               = (1:ny)';
X               = x(ones(ny,1),:);
Y               = y(:,ones(nx,1));
targetScore     = 0.013;
maxIter         = 1950;
dist_to_B       = (X - bx).^2 + (Y - by).^2;
dist_to_A       = (X - ax).^2 + (Y - ay).^2;
y_dir           = 2*(by > ay) - 1;
x_dir           = 2*(bx > ax) - 1;
fuel            = Inf(ny,nx);
fuel(aIndex)    = 0;
fuel_to_reverse = fuel;
xvel            = zeros(ny,nx);
yvel            = zeros(ny,nx);
checked         = false(ny,nx);
previous_stop   = zeros(ny,nx);
INF             = 100000;
it              = 0;
min_fuel        = 0;
cost_to_B       = Inf;

while (  min_fuel < cost_to_B  && ~all(checked(:)) && cost_to_B > targetScore*it && it < maxIter)
    it                      = it + 1;
    fuel_not_checked        = fuel + INF*checked;
    [min_fuel,i_p]          = min(fuel_not_checked(:));
    i_y                     = rem(i_p - 1,ny) + 1;
    i_x                     = (i_p - i_y)/ny  + 1;
    i_vy                    = yvel(i_p) + y_winds(i_p);
    i_vx                    = xvel(i_p) + x_winds(i_p);
    i_new_vy                = Y - i_y;
    i_new_vx                = X - i_x;
    i_thrust                = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
    i_fuel                  = i_thrust + min_fuel;
    i_reacheable            = i_thrust <= maxThrottle;
    i_optimum               = i_fuel < fuel;
    i_impr                  = i_reacheable & i_optimum;
    i_new_vx_impr           = i_new_vx(i_impr);
    i_new_vy_impr           = i_new_vy(i_impr);
    i_fuel_impr             = i_fuel  (i_impr);
    i_fuel_to_reverse_impr  = 0.5*i_fuel_impr + ...
                                abs(i_new_vy_impr).*(i_new_vy_impr*y_dir>0) + ...
                                abs(i_new_vx_impr).*(i_new_vx_impr*x_dir>0) ;
    xvel           (i_impr) = i_new_vx_impr;
    yvel           (i_impr) = i_new_vy_impr;
    fuel           (i_impr) = i_fuel_impr;
    fuel_to_reverse(i_impr) = i_fuel_to_reverse_impr;
    previous_stop  (i_impr) = i_p;
    cost_to_B               = min(min(fuel_to_reverse + dist_to_B));
    checked(i_impr)         = false;
    checked(i_p)            = true;
end

fuel                    = fuel + dist_to_B;
cost_to_A               = min(min(fuel + dist_to_A));

checked                 = checked*0;
return_previous_stop    = zeros(ny,nx);

it                      = 0;
min_fuel                = 0;

while ( min_fuel < cost_to_A &&  ~all(checked(:)) && (cost_to_A - cost_to_B) > targetScore*it  && it < maxIter )
    
    it                  = it + 1;
    fuel_not_checked    = fuel + INF*checked;
    [min_fuel,i_p]      = min(fuel_not_checked(:));
    i_y                 = rem(i_p - 1,ny) + 1;
    i_x                 = (i_p - i_y)/ny  + 1;
    i_vy                = yvel(i_p) + y_winds(i_p);
    i_vx                = xvel(i_p) + x_winds(i_p);
    i_new_vy            = Y-i_y;
    i_new_vx            = X-i_x;
    i_thrust            = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
    i_fuel              = i_thrust + min_fuel;
    i_reacheable        = i_thrust <= maxThrottle;
    i_optimum           = i_fuel < fuel;
    i_impr              = i_reacheable & i_optimum;
    fuel                (i_impr) = i_fuel(i_impr);
    xvel                (i_impr) = i_new_vx(i_impr);
    yvel                (i_impr) = i_new_vy(i_impr);
    return_previous_stop(i_impr) = i_p;
    cost_to_A           = min(min(fuel + dist_to_A));
    checked(i_impr)     = false;
    checked(i_p)        = true;
    
end

cost_to_A           = fuel + dist_to_A;
[~,min_fuel]        = min(cost_to_A(:));
indices             = zeros(nx*ny,1);
indices(1)          = min_fuel(1);
next_move           = return_previous_stop(indices(1));
k                   = 1;
while(next_move)
    k               = k + 1;
    indices(k)      = next_move;
    next_move       = return_previous_stop(next_move);
end
next_move           = previous_stop(indices(k));
while(next_move)
    k               = k + 1;
    indices(k)      = next_move;
    next_move       = previous_stop(next_move);
end
indices             = indices(k:-1:1);

ypos                = rem(indices - 1, ny) + 1;
xpos                = (indices - ypos)/ny + 1;
xw                  = x_winds(indices);
yw                  = y_winds(indices);

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

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

[xmotor,ymotor]     = lowercyc(xdrive,ydrive,xw,yw,xpos,ypos,xspeed,yspeed,ax,ay,maxThrottle);

thrustRow           = ymotor;
thrustCol           = xmotor;

end

function [xmotor,ymotor] = lowercyc(xdrive,ydrive,xw,yw,xpos,ypos,xspeed,yspeed,ax,ay,maxThrottle)

if (~isempty(xdrive))
    
    xmotor      = xdrive - xw(1:end-1);
    ymotor      = ydrive - yw(1:end-1);
    
    x_tomove    = ax - xpos(end);
    x_todrive   = x_tomove - xspeed(end) - xw(end);
    y_tomove    = ay - ypos(end);
    y_todrive   = y_tomove - yspeed(end) - yw(end);
    
    if abs(x_todrive) + abs(y_todrive) < maxThrottle && abs(x_todrive) + abs(y_todrive) <  abs(ax-xpos(end)) + abs(ay-ypos(end))
          
            xmotor = [xmotor; x_todrive];
            ymotor = [ymotor; y_todrive];
    end
    
else
    xmotor = [];
    ymotor = [];
end

end