
% Matlab Simulator for Meccano Robot
% Programmed by Olac El Terrible

worldsize = 360;
maxtime = 10000;
world = 250*ones(worldsize,worldsize);
world(20:worldsize-20,20:worldsize-20)=world(20:worldsize-20,20:worldsize-20)*0;
obstacle_size = 40;
tot_obstacles = 7;
obstacle = 250*ones(obstacle_size,obstacle_size);
start = [21; 21];
goal = [worldsize;worldsize];
obstacles = [60,60; 140,140; 220,100;140,220;220,180;260,260;60,180];
for i=1:tot_obstacles
	start_row = obstacles(i,1);
	start_col = obstacles(i,2);
	world(start_row:start_row+obstacle_size-1,start_col:start_col+obstacle_size-1) = obstacle;
end

ir_source = goal;
pos = start;
theta = 1;
power = 5;

image(world);
for i=1:maxtime
	row=pos(1);
	col = pos(2);
	[theta power]
	pos
	sx = 5*[cos(theta+1.57);sin(theta+1.57)];
	sy = 20*[cos(theta);sin(theta)];
	vertices = [pos+(sx+sy),pos+sx,pos-sx,pos+(-sx+sy)];
	mpl = round((vertices(:,1)+vertices(:,2))*.5);
	mpr = round((vertices(:,3)+vertices(:,4))*.5);
	rv = round(vertices);
	bump_fl = world(rv(1,1),rv(2,1));
	bump_fr = world(rv(1,4),rv(2,4));
	bump_back = world(rv(1,2),rv(2,2))+world(rv(1,3),rv(2,3));
	bump_l = world(mpl(1),mpl(2));
	bump_r = world(mpr(1),mpr(2));
%	image(world);
	patch(vertices(2,:),vertices(1,:),125);
	drawnow;
	line_to_beacon = ir_source - pos;
	front_beacon_angle = atan2(line_to_beacon(2),line_to_beacon(1))-theta;
	front_beacon = max(255*cos(front_beacon_angle),0);
	left_beacon = max(255*cos(front_beacon_angle-1.5708),0)	;
	right_beacon = max(255*cos(front_beacon_angle+1.5708),0);
	back_beacon = max(255*cos(front_beacon_angle-pi),0);
	beacons = [front_beacon,left_beacon,right_beacon,back_beacon];
	photo_front = world(round(row+cos(theta)*35),round(col+sin(theta)*35));
	photo_left = world(round(row+cos(theta-.2)*35),round(col+sin(theta-.2)*35));
	photo_right = world(round(row+cos(theta+.2)*35),round(col+sin(theta+.2)*35));
	[photo_front photo_left photo_right]
	if bump_fl
		power = -5;
		theta = theta -.7;
	elseif bump_fr
		power = -5;
		theta = theta +.7;
	elseif bump_l	
		theta = theta - .3;
	elseif bump_r
		theta = theta + .3;
	elseif bump_back
		power = 5;
	else
		if photo_left 
			if photo_front 	
				theta = theta + .6;
			else
				theta = theta + .3;
			end
		else 
			if photo_right
				if photo_front 	
					theta = theta - .6;
				else
					theta = theta - .3;
				end
			end
		end	
	end

	pos = pos + power*[cos(theta);sin(theta)];	
end

