
%Function which creates an occupancy grid map giving the probability of of there being an object
%given a series of measurements over the probability of there not being an object
%given the measurements.

%Input comes from 'filename' where each line contains 8 space-separated values
%Each line contains: x, y, theta, IR1, IR2, IR3, IR4, IR5 in that order
%Example line of input file:
%17 23 .45 -1 42 -1 -1 -1
%x and y are centimeters, theta is in radians
%IR values are the values seen by the robot, where -1 means that nothing is seen

%Output: graph of points seen by robot
%All output measurements are in millimeters

function arena = sensor_map(filename)

%aviobj = avifile('test.avi');
%aviobj.Quality = 80;
%aviobj.Compression = 'None';

%Vectors from center of bot to IR sensors
Bot_IR1 = [4.5,3.5];
Bot_IR2 = [7.5,0];
Bot_IR3 = [4.5,-3.5];
Bot_IR4 = [-4,-3.5];
Bot_IR5 = [-4,3.5];
Bot_IR = [Bot_IR1; Bot_IR2; Bot_IR3; Bot_IR4; Bot_IR5];

%Unit vectors with the same angle that the IR sensors are tilted at
Angle_IR1 = [.78,.63];
Angle_IR2 = [1,0];
Angle_IR3 = [.78,-.63];
Angle_IR4 = [0,-1];
Angle_IR5 = [0,1];
Angle_IR = [Angle_IR1; Angle_IR2; Angle_IR3; Angle_IR4; Angle_IR5];

%Define sensor model.

[sensor_model, udata, vdata] = model_sensor;

%Define map area
map_width = 1000;
map_height = 1000;
%xdata = [-1, 1] * map_width/2;
%ydata = [-1, 1] * map_height/2;
xdata = [-200, 800];
ydata = [-400, 300];
arena = ones(map_width,map_height);

%For now, read input in from a file
Data = dlmread(filename, ' ');

points = [];
%loop through each line of file
for i=25:size(Data, 1)
	display(i);
    row = Data(i, :);
    x = row(1);
    y = row(2);
    theta = row(3);
	
	row(4:8)
    for ir=1:5
		if (row(4:8) == [-1 -1 -1 -1 -1])
			break;
		end
		reading = row(ir+3);
        distance = reading * 0.1; %Scale by 0.1 since range is in mm.
        if reading ~= -1
			k_val = floor(distance);
			sensor_k = sensor_model(:,:,k_val);
        	
			%Find vector of object relative to center of bot
        	ObjectVector = distance * Angle_IR(ir, :);
        	ObjectVector = ObjectVector + Bot_IR(ir, :);
        	
			%rotate by theta
        	objTheta = atan2(ObjectVector(2), ObjectVector(1));
        	objTheta = objTheta + theta;
		else
			sensor_k = sensor_model(:,:,71);
			
			%Use the maximum length to clear an empty area.
        	ObjectVector = 80 * Angle_IR(ir, :);
        	ObjectVector = ObjectVector + Bot_IR(ir, :);
        	
			%rotate by theta
        	objTheta = atan2(ObjectVector(2), ObjectVector(1));
        	objTheta = objTheta + theta;
		end

		dx = x + Bot_IR(ir,1);
		dy = y + Bot_IR(ir,2);
			
		translation_mat = [1, 0, 0;
						   0, 1, 0;
						  dx,dy, 1];

		rotation_mat = [cos(objTheta), sin(objTheta), 0;
					   -sin(objTheta), cos(objTheta), 0
						   		    0,			   0, 1];

		transformation_mat = rotation_mat * translation_mat;
		transformation = maketform('affine', transformation_mat);
		
		%Big time waster 1: TODO Speedups.
		sensor_k = imtransform(sensor_k, transformation,... 
			'UData', udata, 'Vdata', vdata,...
			'XData', xdata, 'YData', ydata,...
			'Size', [map_width, map_height], 'FillValues', 1);
		
		%Big time waster 2: TODO Speedups.
		arena = arena.*sensor_k;
		
		%For when you want to show the image every round:
		image(arena.*20);
		drawnow;
		
		%frame = getframe;
		%aviobj = addframe(aviobj,frame.cdata);
    end
end

%aviobj = close(aviobj);
	

