%robots

clear all;
close all;


RoomSize=[50,60]; %size of the room
Time=5000; %running time of the simulation
deltat=0.01; %show an image for this long
RobotMaxStep=10; %10; %maximum amount a robot can step

%the states in the room are the following:
%0 empty
%0.5 robot
%1 object
Room=zeros(RoomSize); %white color codes empty room

RobotsNum=50; %50; %number of robots
ObjectsNum=200; %200; %200; %number of objects

%place the robots randomly
RobotPositions=zeros(RobotsNum,2);
RobotEmpty=ones(RobotsNum,1);
for r=1:RobotsNum
	NotPlaced=true;
	while NotPlaced
		TryToPlace=[ ceil(rand(1,2).*RoomSize) ];
		if Room(TryToPlace(1),TryToPlace(2))==0%if room is empty
			Room(TryToPlace(1),TryToPlace(2))=0.5;
			RobotPositions(r,:)=TryToPlace;
			NotPlaced=false;
		end
	end
end

%place the objects
ObjectPositions=zeros(ObjectsNum,2);
for o=1:ObjectsNum
	NotPlaced=true;
	while NotPlaced
		TryToPlace=[ ceil(rand(1,2).*RoomSize) ];
		if Room(TryToPlace(1),TryToPlace(2))==0 %if room is empty
			Room(TryToPlace(1),TryToPlace(2))=1;
			ObjectPositions(r,:)=TryToPlace;
			NotPlaced=false;
		end
	end
end



%show the map
h = imshow(Room); set(h,'erasemode','xor'); 
truesize(gcf(),[400 600]) %magnify the room to be 400x600
C = [1 1 1; 0 0 0; 1 0 0];
colormap(C);

%run the simulation
for t=1:Time
	for  r=1:RobotsNum
		[Movement, PickUp, PutDown] = MoveRobot(RobotPositions(r,:),RobotEmpty(r),Room);
        
        % check the return values:
		%can only handle objects close to the robot
		Movement=min([RobotMaxStep RobotMaxStep],max(Movement,[-RobotMaxStep,-RobotMaxStep]));
		PickUp=min([1 1],max(PickUp,[-1,-1]));
		PutDown=min([1 1],max(PutDown,[-1,-1]));
		RobotActed=false;
		%first priortiy pickup object
		if any(PickUp~=[0,0]) & RobotEmpty(r)==true
			RobotActed=true;
			Desiredposition=RobotPositions(r,:)+PickUp;
			if (all(Desiredposition>0)) & (all(Desiredposition<(RoomSize+1)))
				if Room(Desiredposition(1),Desiredposition(2))==1
					RobotEmpty(r)=false;
					Room(Desiredposition(1),Desiredposition(2))=0;
				end

			end
		end
		%second priority putting down an object
		if RobotActed==false & any(PutDown~=[0,0]) &  RobotEmpty(r)==false
			RobotActed=true;
			Desiredposition=RobotPositions(r,:)+PutDown;
			if (all(Desiredposition>0)) & (all(Desiredposition<(RoomSize+1)))
				if Room(Desiredposition(1),Desiredposition(2))==0
					RobotEmpty(r)=true;
					Room(Desiredposition(1),Desiredposition(2))=1;
				end

			end
		end
		Desiredposition=RobotPositions(r,:)+Movement;
		%check if possible
		if (all(Desiredposition>0)) & (all(Desiredposition<(RoomSize+1)))
			if Room(Desiredposition(1),Desiredposition(2))==0
				Room(RobotPositions(r,1),RobotPositions(r,2))=0;
				RobotPositions(r,:)=[Desiredposition(1),Desiredposition(2)];
				Room(Desiredposition(1),Desiredposition(2))=0.5;
			end
		end

	end
	title(sprintf('%d(/%d). iteracio',t,Time)) 
	pause(deltat)
	set(h,'cdata',Room)
end

    
    
