%robots

clear all;
close all;


%this is a simulation of a room where there are randomly palced objects (just like home)
%and robots, whose aim is to organize the objects. The objects are marked as red and the robots are
%marked as black


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

%the states in the room are the following:
%0 means an empty space, we can move a robot here, or put an object here
%0.5 robot -marked as black. Two robots can not occupy the same position
%1 object - an object, marked as red
Room=zeros(RoomSize); %white color codes empty room

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

%place the robots randomly
RobotPositions=zeros(RobotsNum,2);
RobotEmpty=ones(RobotsNum,1);
for r=1:RobotsNum
	NotPlaced=true;
	while NotPlaced %try paces randomly, till we find an empty one
		TryToPlace=[ ceil(rand(1,2).*RoomSize) ];
		if Room(TryToPlace(1),TryToPlace(2))==0%if room is empty we palce the robot
			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 %try paces randomly, till we find an empty one
		TryToPlace=[ ceil(rand(1,2).*RoomSize) ];
		if Room(TryToPlace(1),TryToPlace(2))==0 %if room is empty we place the object
			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
		%we have to wrtie this function, this will determine what a robot does
		%the robot receives the map of the Room
		[Movement, PickUp, PutDown] = MoveRobot(RobotPositions(r,:),RobotEmpty(r),Room);
		%the funciton returns three vectors:
		%Movement- the displacement of the robot
		%pickup - an order to pickup an object from this direction
		%putdown - an order to put down an object here
		%can only move 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). iteárció',t,Time)) 
	pause(deltat)
	set(h,'cdata',Room)
end