clear                   % clear all variables
more off                % prevent pagination of output
format short            % display floats in short form


% This function implements the ray-casting operation
% A "ray" is cast on the grid from point (x0, y0) to point (x1, y1)
% Matrix H (Hits) marks the cell where the rays end
% Matrix M (Misses) marks the cells traversed by the ray

function [H, M] = draw_line( x0, y0, x1, y1, H, M)
   dy = y1 - y0;
   dx = x1 - x0;

   stepy = 0;
   stepx = 0;

   if ( dy < 0 )
     dy = -dy;
     stepy = -1;
   else
     stepy = 1;
   endif

   if ( dx < 0 )
     dx = -dx;
     stepx = -1;
   else
     stepx = 1;
   endif

   dy = dy * 2;
   dx = dx * 2;

   if (x0 == x1 && y0 == y1)
      H( x0 , y0 ) = H( x0 , y0 ) + 1;
   else
      M( x0 , y0 ) = M( x0 , y0 ) + 1;
   endif

   if ( dx > dy )
     fraction = 2*dy - dx;

     while ( x0 != x1 )
       if ( fraction >= 0 )
         y0 = y0 + stepy;
         fraction = fraction - dx;
       endif

       x0 = x0 + stepx;
       fraction = fraction + dy;

       if (x0 == x1 && y0 == y1)
          H( x0 , y0 ) = H( x0 , y0 ) + 1;
       else
          M( x0 , y0 ) = M( x0 , y0 ) + 1;
       endif

      endwhile
   else
     fraction = 2*dx - dy;

     while ( y0 != y1 )
       if ( fraction >= 0 )
         x0 = x0 + stepx;
         fraction = fraction - dy;
       endif

       y0 = y0 + stepy;
       fraction = fraction + dx;

       if (x0 == x1 && y0 == y1)
          H( x0 , y0 ) = H( x0 , y0 ) + 1;
       else
          M( x0 , y0 ) = M( x0 , y0 ) + 1;
       endif

     endwhile
   endif

endfunction

% Build the empty grid
data = load("-ascii", "log4.log"); % load log file

% column 9: number of readings
% column 371: laser pose x
% column 372: laser pose y
% column 373: laser pose theta

max_range = 10.0; % add a margin to the map

min_x = min ( data(:,371) ) - max_range;
min_y = min ( data(:,372) ) - max_range;
max_x = max ( data(:,371) ) + max_range;
max_y = max ( data(:,372) ) + max_range;


% if you run out of memory, increase this number
resolution = 0.1; % size of each cell

size_x = ceil ( (max_x - min_x) / resolution ) + 1;
size_y = ceil ( (max_y - min_y) / resolution ) + 1;

M = zeros ( size_x, size_y );
H = zeros ( size_x, size_y );

counter = 0; % for numerating resulting images
filename = "gridmap";

for i = 1:length (data(:,1)) % iterating over all entries
   num_readings = data(i,9);
   start_angle = data(i,3);
   angular_resolution = data(i,5);
   
   laser_pose_x = data(i,371);
   laser_pose_y = data(i,372);
   laser_pose_theta = data(i,373);

   angle = laser_pose_theta;

   step = 1; % if your computer is too slow, you can increase the size of the step
   for j = 1:step:num_readings; % go over each reading
      angle = laser_pose_theta + start_angle + angular_resolution*(j-1);
      range_reading = data(i, 9 +j);

      % ignore readings larger than 15 meters
      if ( range_reading < 15 )
         % compute the end point of the reading
	 reading_end_point_x = laser_pose_x + range_reading * cos(angle);
         reading_end_point_y = laser_pose_y + range_reading * sin(angle);

         % compute the position of the robot and the end point within the grid
         laser_pose_i = floor( (laser_pose_x - min_x) / resolution ) + 1;
         laser_pose_j = floor( (laser_pose_y - min_y) / resolution ) + 1;

         reading_end_point_i = floor( (reading_end_point_x - min_x) / resolution ) + 1;
         reading_end_point_j = floor( (reading_end_point_y - min_y) / resolution ) + 1;

         % TODO: upodate the hit and misses matrix according to the reading
      endif

   endfor

   % TODO: compute the cell probabilities
   G = zeros ( size_x, size_y );


   % display grid map
   imshow ( G )

   % save every 50th map update
   if ( mod( i, 50 ) == 1 )
      counter = counter + 1;
      filename = sprintf("gridmap-%.3d.png", counter); 
      print(filename)
   endif

   sleep(0.1)

   printf("processing scan %d/%d\n", i, length(data(:,1)));

endfor

disp("done!")