hqmove

function inst = onemove(map, p)

%   Here we define the constants in this function
%   and, since we're filling inst using a FOR loop
%   we preallocate it with zeros for performance.
numberofrovers = 5;
numberoftimesteps = 500;
r=p(:,1); c=p(:,2); d=p(:,3);
inst=zeros(numberoftimesteps,numberofrovers);

%   Mark the initial locations as visited.
for moo=1:numberofrovers
   map(r(moo),c(moo)) = 0;
end
map(map==-1)=-99999;

%   Loop over time, then over each rover
%   generating instructions for each rover

for t = 1:numberoftimesteps
   for rover = 1:numberofrovers
      row=r(rover);
      col=c(rover);

      action = nextmove(map,row,col,d(rover));
      inst(t,rover) = action;

     [map row col r c d]=update_state(action,map,row,col,r,c,d,rover);
   end
end

%__________________________________________________________________________


function [map,row,col,r,c,d]=update_state(action,map,row,col,r,c,d,rover)

% update state
switch action
case 1 % move forward
   switch d(rover)
   case 1
      row = row - (row ~= 1);
   case 2
      col = col + (col ~= 50);
   case 3
      row = row + (row ~= 50);
   case 4
      col = col - (col ~= 1);
   end
   % If it is a valid region, update the state, otherwise, it
   % doesn't move.
   if map(row,col) ~= -99999,
      map(row,col)=map(row,col)-1;
   end
case 2 % turn left
   d(rover) = mod(d(rover)+2,4)+1;
case 3 % turn right
   d(rover) = d(rover)+1;
   if d(rover) == 5,  d(rover)=1; end;
end

r(rover)=row; c(rover)=col;


%__________________________________________________________________________

function next=nextmove(map,r,c,d)

%   nesw = north,east,south,west
%   look at the squares around the current position
nesw=zeros(4,1)-99999;
if r>1,  nesw(1)=map(r-1,c); end
if c<50, nesw(2)=map(r,c+1); end
if r<50, nesw(3)=map(r+1,c); end
if c>1,  nesw(4)=map(r,c-1); end

%   frbl = front,right,back,left
%   reorient according to the direction I'm facing
frbl = nesw([d:end 1:d-1]);

pd=max(frbl);

if (frbl(2)<pd) & (frbl(4)==pd),
   next=2;
elseif (frbl(1)==pd) & (rand(1)>.01*abs(pd-2)),
   next=1;
elseif (frbl(2)==pd) & (frbl(4)<pd),
   next=3;
elseif (frbl(2)==pd) & (frbl(4)==pd),
   next=2+(rand(1)>.5);
elseif frbl(3)==pd,  
   next=2+(rand(1)>.5);
else
   next=1;

end;