%Function which creates a map of points seen by a robot

%Input comes from file 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 centimeters

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

%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('input.txt', ' ');

points = [];
%loop through each line of file
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
            objX = (objR * cos(objTheta)) + x;
            objY = (objR * sin(objTheta)) + y;
            ObjectVector = [objX, objY];
            
            points = [points; ObjectVector];
        end
    end
end

%graph all the points
figure;
toGraph = points';
plot(toGraph(1,:), toGraph(2,:), '*');
