% Start a connection with Webots / ePuck
ePic = ePicKernel();
port = 'COM8';
[ePic result] = connect(ePic, port);
 
epuck_max_speed = 6.28;
epuck_speed_unit = 0.00628;

e_puck_matrix = [150, -35;
                 100, -15;
                  80, -10;
                 -10, -10;
                 -10, -10;
                 -10,  80;
                 -30, 100;
                 -20, 150];
                
%max_speed = epuck_max_speed;
speed_unit = epuck_speed_unit;
matrix = e_puck_matrix;
range = 512;

[ePic] = activate(ePic,'proxi');

MAX_SPEED = 666.28;
DISTANCE = 35;

for k = 1:1e6
  ePic = update(ePic);
  [sensors_value, up] = get(ePic, 'proxi');
    
 right_obstacle =  sensors_value(1) > DISTANCE |  sensors_value(2) > DISTANCE |  sensors_value(3) > DISTANCE;
 left_obstacle =  sensors_value(6) > DISTANCE |  sensors_value(7) > DISTANCE |  sensors_value(8) > DISTANCE;
  
  if (left_obstacle)
      disp("an obstacle to the left");
  end
  
  if (right_obstacle)
      disp("an obstacle to the right");
  end
  
  
  
  % initialize motor speeds at 50% of MAX_SPEED.
  speed(1)  = 0.5 * MAX_SPEED;
  speed(2) = 0.5 * MAX_SPEED;
  % modify speeds according to obstacles
  if left_obstacle
    % turn right
    speed(1) =  MAX_SPEED;
    speed(2) = 0.0;
  elseif right_obstacle
    % turn left
    speed(1)  = 0.0;
    speed(2) = MAX_SPEED;
  end
  
  ePic=set(ePic,'speed', [speed(1)  speed(2)]);
  
  
%   if (up >= 1)
%       speed = [0.0, 0.0];
%       for i = 1:2
%           for j = 1:8
%               speed(i) = speed(i) + speed_unit * matrix(j,i) * (1.0 - (sensors_value(j) / range));
%           end
%       end
%       ePic=set(ePic,'speed', [speed(1)  speed(2)]);
%   end
  
end

[ePic result] = disconnect(ePic);

