% EF 230 OLR-1: P0
% Noah Marks <nmarks1>; Curtis Schunk <cschunk>
% Date Finalized: April 12, 2018 
% Objective: Create a function that would allow a roomba robot to travel
%            across a floor region and perform reconnasaince on the
%            surroundings. Counting objects may be feasible.
% Requirements: All inputs (bumpers, light bumpers, cliff sensors, camera)
%               must be used in some form (reactive to the surroundings).
%               Camera output should not rely on or only on RGB means values.
% Bonus Ideas: Internal clock or timer? Yes
%              Slow movement or fast movement? At Light Bumper values
%              Single video or many videos? No, images pamorama style
%              Map? Formation or random? Return to origin? Yes, Random, No
%              Samples? Counting subfunction, does not discriminate shapes


function [E,T] = reconnaissance(r,t,v)

mustBePositive(t); mustBePositive(v); % must be positive values
r.timeStart;% initiate timer
T=r.timeGet;
E=0; % image counter
D=0; % distance variable
v_b=v; % set base velocity
n=1;x(n)=0;y(n)=0;a(n)=0;d(n)=0;
r.setLEDDigits(num2str(E))

 scatter(x,y,'r') % place origin on map
 title(sprintf('Period: %0.1f seconds  Velocity: %0.1f m/s',t,v))
 xlabel('Longitude, m')
 ylabel('Latitude, m')
 hold on
 
while T<t
 r.setDriveVelocity(v_b) % constant speed
 A=r.getBumpers;
 B=r.getLightBumpers;
 C=r.getCliffSensors;
 D=D+r.getDistance;
 
 if A.right~=0 || A.front~=0 % hits a wall
  r.stop
  [x,y,n,a,d]=plotr(n,x,y,d,a,'k');
  r.moveDistance(-0.1,0.1)
  r.turnAngle(70,0.2) % turn left
  v_b=v;
 elseif A.left~=0 
  r.stop
  [x,y,n,a,d]=plotr(n,x,y,d,a,'k');
  r.moveDistance(-0.1,0.1)
  r.turnAngle(-70,0.2) % turn right
  v_b=v;
 elseif B.left>50 || B.leftFront>50 || B.leftCenter>50 || B.rightCenter>50 || B.rightFront>50 || B.right>50
  v_b=0.1041*log(v)+0.3121; % caution as it approaches wall
 elseif C.left<1000 || C.leftFront<1000
  r.stop
  [x,y,n,a,d]=plotr(n,x,y,d,a,'b');
  r.moveDistance(-0.2,0.25)
  r.turnAngle(-100,0.2)
  v_b=v;
 elseif C.rightFront<1000 || C.right<1000
  r.stop
  [x,y,n,a,d]=plotr(n,x,y,d,a,'b');
  r.moveDistance(-0.2,0.25)
  r.turnAngle(100,0.2)
  v_b=v;  
 elseif D>3 % maximum distance before reconaissance
  r.stop
  [x,y,n,a,d]=plotr(n,x,y,d,a,'g'); 
  for i=1:8
   r.turnAngle(-45,0.1)
   img=r.getImage;
   img_m=img(70:300,:,:); % modify image taken; avoid time stamp
   E_i=CountR(img_m);
   E=E+E_i;
   r.setLEDDigits(num2str(E)) % display counter
  end
   v_b=v;
   D=0;
  else
  v_b=v; % reset when all is normal
 end
 T=r.timeGet; % change time with each movement
end
r.stop;

function [x,y,n,a,d]=plotr(n,x,y,d,a,color)
    n=n+1;
    d(n)=r.getDistance;
    a(n)=r.getAngle+a(n-1);
    x(n)=(d(n).*cosd(a(n)))+x(n-1);
    y(n)=(d(n).*sind(a(n)))+y(n-1);

    scatter(x(n),y(n),color)
    hold on    
end % plotr

function [W] = CountR(img)
% set to grayscale
img1=rgb2gray(img);
% set to only black and white
img2=imbinarize(img1,graythresh(img1));
% invert colors
img2=~img2;
% Identify boundaries
B = bwboundaries(img2);
% Count objects detected
figure;imshow(img2)
W = length(B);
end % CountR

T=r.timeGet;
end % reconnaisance