% Open the file with the laser scan.
fileid = fopen('/home/g33ky/Documents/Programming/Python/visualise_logs/simulated_data/oneframe');

% Read a line/scan, remove the trailing whitespace and split the
% measurements. Should be 181 laser beams / measurements for the simulated
% data, because the degrees are in the range [-90, 90] moving with 1
% degree.
tline = deblank(fgets(fileid));
cellscan = regexp(tline, ' ', 'split');

% Convert into an array of numbers.
measurements = cellfun(@str2num, cellscan)
maxdist = max(max(measurements)); % max distance.

% Number of scans.
numofscans = length(measurements);

% Image results: original image + 2 grayscales.
original = zeros(200);
graydist = zeros(200);
grayangle = zeros(200);

%imshow(original)

for deg = 1:numofscans

    % Convert to radians.
    anglerad = degtorad(deg-91);

    % Find Euclidean coordinates. Adjust to matlab arrays (+1)
    x = cos(anglerad) * measurements(deg) + 1;
    y = sin(anglerad) * measurements(deg) + 1;

    % Round the coordinates, after some scaling (multiply with 10) to
    % get cm accuracy. Robot's position at [1, 100].
    imgx = round(10 * x);
    imgy = round(10 * y + 100);


    orginal(imgx, imgy) = 255;

end

%imshow(original)
%original

% Close the file.
fclose(fileid);