%% USAR Bug1

start=[6 -3]';
qgoal = [-7 0]';


%% wall-following numbers
follow = 0;     %bug knows when it is/is not following an obstacle
perim = [0 0];  %first is total perimiter; seconcd is perimiter since last closest point to the goal
qhit = [0 0]';  %so the bug knows when it has circled the obstacle
qleave = [0 0]';% closest point to goal around the obstacle
hug = 1;		%stay about 1m away from wall
rightleft = 1;	%1=hang a left at an obstacle; -1=turn right
closest = 0;    %stores closest distance to goal around the perimeter of an obstacle

pts = zeros(2,0);

m = .75;

turnConst = 80;	%movement constants
moveConst = 40;


%% initialize robot
rob = initializeRobot('rob','P2AT',[start' 1.8],[0 0 0]);
pause(2);


%% start the loop
t = -linspace(-pi,pi,180)';

INS = getINSReadings(rob);
pos = INS.Position(1:2);
orient = INS.Orientation(3);

lastpos = pos;

v = [];

while(norm(pos - qgoal) > 1)
	
	lrf = getLaserSensorReadings(rob);
	data = lrf.Scans;
	
	if (~follow)
		%% We are not currently following an obstacle.
		%  Check for the existence of an obstacle.
		
		% assign column unit vector "v" as the desired direction of travel
		%%%%%%%%%%%%%  YOUR CODE    %%%%%%%%%%%%%%%%%%%%%%%
		
	else
		%% manuevering around an obstacle
		
		%% here, assign "v" desired to move around obstacle
		
		%%%%%%%%%%%%%  YOUR CODE    %%%%%%%%%%%%%%%%%%%%%%%
		
		% if follow == 1, we are moving around the obstacle to find the
		% closest point to the goal. If follow == 2, we have found that
		% point and should be moving there.
		if(follow == 1) %finding the closest place to the goal
			
			%%%%%%%%%%%%%  YOUR CODE    %%%%%%%%%%%%%%%%%%%%%%%
			
			
		else %follow == 2, we know the closest place to the goal and we're moving there
			
			%%%%%%%%%%%%%  YOUR CODE    %%%%%%%%%%%%%%%%%%%%%%%
			
		end
		
	end
	
	% "v" should now be assigned. The robot will attempt to move in that
	% direction.a
	vH = [cos(orient);sin(orient)];
	omega = turnConst*v'*[0 -1;1 0]*vH;
	V = moveConst*v'*vH;
	
	sendDriveCommand(rob,V+[omega,-omega]);
	
	pause(.05);
	
	lastpos = pos;
	
	INS = getINSReadings(rob);
	pos = INS.Position(1:2);
	orient = INS.Orientation(3);
	
	
end


shutdownRobot(rob);

