%Function which creates a heat map of wall location probabilities.

%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: probability density map.
%All output measurements are in centimeters

%TODO
%find conversion factor between IR sensor values and centimeters (currently 1 is being used)
function map(filename)

%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];

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

points = [];

%Map area, defined in centimeters:
[X,Y] = meshgrid(-100:2:100,-100:2:100);
%V = [X1(:),X2(:)];
prob_map = zeros(size(X,1),size(Y,2));


%Set position and range error constant for testing FIXME!
range_error = 250;
pos_error = 500;

%loop through each line of file
i = 1; ir = 1;
%for i=1:size(Data, 1)
    row = Data(i, :);
    x = row(1);
    y = row(2);
    theta = row(3);
	%look at each IR sensor reading
    %for ir=1:5
        %if row(ir+3) ~= -1
            %Find vector of object relative to center of bot
            distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm
            ObjectVector = distance * Angle_IR(ir, :);
            ObjectVector = ObjectVector + Bot_IR(ir, :);
            
            %rotate by theta
            objR = sqrt(ObjectVector(1)^2 + ObjectVector(2)^2);
            objTheta = atan2(ObjectVector(2), ObjectVector(1));
            objTheta = objTheta + theta;
            
            %add absolute position and shift into image
            objX = (objR * cos(objTheta)) + x + 1500;
            objY = (objR * sin(objTheta)) + y + 1500;
            ObjectVector = [objX, objY];
        	
			%Calculate distribution for this point:
			
			%Range variance in y direction, rotated by the object angle.
			rotation = [cos(objTheta),sin(objTheta); -sin(objTheta),cos(objTheta)];
			range_error_cov = inv(rotation)*[0,0;0,range_error]*rotation; 

			%Position variance in the robot direction 
			%(TODO introduce error in position estimation perpendicular to the robot)
			rotation = [cos(theta),sin(theta); -sin(theta),cos(theta)];
			position_error_cov = inv(rotation)*[0,0;0,pos_error]*rotation;

			covar = position_error_cov + range_error_cov;
			
			dist = reshape(mvnpdf(X,[x,y],covar), size(X1,1),size(X2,2))
			surf(X1,X2,dist);
			colormap default;
			return;

			%Using the values of the pdf as an approximation for the prob. density map
			%(needs to change) update as:
			prob_map = prob_map.*(1-dist) + dist;	
        %end
    %end
%end

%graph the probability map.
surf(X1,X2,prob_map);


