spicy noodles 5

function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Basic solver, does nothing

% Copyright 2010 The MathWorks, Inc.

  sz = [size(chart,1) size(chart,2)];
 
  [ay,ax] = ind2sub(sz,aIndex);
  [by,bx] = ind2sub(sz,bIndex);
  
  [ty1,tx1,vy1,vx1,by,bx] = oneway(chart,ay,ax,by,bx,0,0,maxThrottle);
  [ty2,tx2,vy2,vx2,by,bx] = oneway(chart,by,bx,ay,ax,vy1,vx1,maxThrottle);

  thrustRow = [ty1 ty2];
  thrustCol = [tx1 tx2];
  
function [ty,tx,vy,vx,by,bx] = oneway(chart,ay,ax,by,bx,vy,vx,maxThrottle)
  rows = size(chart,1);
  cols = size(chart,2);

  % --------------------------------------
  % dijkstra

  Dis = inf(rows,cols);
  Vel = zeros(rows,cols,2);
  Vel(ay,ax,:) = [vy vx];
  Dis(ay,ax) = 0;
  
  % create differences to neighbors
  r = 5;
  nb = ones(r*2+1);
  nb(r+1,r+1) = 0;
  r = (size(nb,1)-1)/2;
  [nby,nbx] = find(nb);
  nb = [nby-r-1 nbx-r-1];
  
  % do the dijkstra
  Q = zeros(2,rows*cols);
  Q(:,1) = [ay; ax];
  bottom = 1;
  top = 1;
  inQ = zeros(rows,cols);
  inQ(ay,ax) = 1;
  while top >= bottom
    % pop
    x = Q(2,bottom);
    y = Q(1,bottom);
    bottom = bottom + 1;
    inQ(y,x) = inQ(y,x)-1;
    
    % this pos occurs again later in queue
    if inQ(y,x)
      continue
    end
    
    % neighbors
    for j=1:size(nb,1)
      nx = nb(j,2)+x;
      ny = nb(j,1)+y;
      
      if nx < 1 || ny < 1 || nx > cols || ny > rows
	continue;
      end     
      
      my = ny-y-Vel(y,x,1)-chart(y,x,1);
      mx = nx-x-Vel(y,x,2)-chart(y,x,2);
      
      % check motor crash
      mtot = abs(my)+abs(mx);
      if mtot > maxThrottle
	continue;
      end
      
      % better path?
      dold = Dis(ny,nx);
      dnew = Dis(y,x)+mtot; 
      if dold > dnew
	vnewy = Vel(y,x,1)+chart(y,x,1)+my;
	vnewx = Vel(y,x,2)+chart(y,x,2)+mx;
	
	Vel(ny,nx,1) = vnewy;
	Vel(ny,nx,2) = vnewx;
	Dis(ny,nx) = dnew;
	
	if top == size(Q,2)
	  Q(:,1:top-bottom+1) = Q(:,bottom:top);
	  top = top - bottom + 1;
	  bottom = 1;
	end
	
	Q(:,top+1) = [ny; nx];
	top = top+1;
	inQ(ny,nx) = inQ(ny,nx) + 1;
      end
    end
  end

  % --------------------------------------
  % extract motor power

  ty = [];
  tx = [];

  % find best turning spot
  Dis = Dis ...
	+ repmat(abs((1:cols)  - bx),rows,1) ...
	+ repmat(abs((1:rows)' - by),1,cols);
  [dummy,targetIndex] = min(Dis(:));
  [by,bx] = ind2sub([rows cols],targetIndex);
  vy = Vel(by,bx,1);
  vx = Vel(by,bx,2);
  
  % walk the path backwards
  curPos = [by; bx];
  while curPos(1) ~= ay || curPos(2) ~= ax
    x = curPos(2);
    y = curPos(1);
    prevPos = curPos - permute(Vel(y,x,:),[3 1 2]);
    py = prevPos(1);
    px = prevPos(2);
    m = Vel(y,x,:)-chart(py,px,:)-Vel(py,px,:);
    ty(end+1) = m(1);
    tx(end+1) = m(2);
    curPos = prevPos;
  end  
  
  % reverse motor power b/c of backwards walk
  ty = fliplr(ty);
  tx = fliplr(tx);