%% USAR Bug1


start=[0 0]';
qgoal = [-8 -8]';


%% 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 = .5;		%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

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)    
		%% ano obstacle
        v = qgoal - pos;
        v = v ./ norm(v);
        
		tG = atan2(v(2),v(1));
		X = data.*cos(-t+tG-orient);Y = data.*sin(-t+tG-orient);
        if(any(X > 0 & X < .5 & abs(Y) < .5))    %hit an obstacle
            qhit = pos;   %make sure the bug knows where it hit
            follow = 1;     %flag to wall follow
            closest = norm(qgoal - pos);  %to find the closest point to the end goal
			fprintf('qhit!\n');
        end
        
    else
        %% manuevering around an obstacle
		dataI = 90:rightleft:(90.5+rightleft*89.5);
        [tangent,I] = min(data(dataI));%closest wall and index of closest wall to find angle
        wallangle = t(dataI(I));   %angle to the nearest wall
		
        if (m  <= tangent - hug)    %needs to move closer to wall
            angle = wallangle;
%             disp('too far away');
        elseif(m <= hug - tangent)  %bug needs to move away from wall
            angle = wallangle + pi;
%             disp('too close');
        else              %bug is about where it needs to be; correct a little bit
            angle = wallangle + rightleft * (pi/2 - asin((tangent - hug) / m));
% 			fprintf('correcting\n');
		end
        
		angle = angle + orient;
        v = [cos(angle);sin(angle)];
        
        if(follow == 1) %finding the closest place to the goal
            %find the point on the object that's closest to the goal and
            %remember the perimeter since then
			dif = norm(pos - lastpos);
            perim = perim + [dif dif];  
            if(closest > norm(qgoal - pos))
                closest = norm(qgoal - pos);
				qleave = pos;
                perim(2) = 0;
            end

            %have we circled the perimeter
            if(norm(pos - qhit) < 1*m && perim(1) > 4)
				%circle complete
                follow = 2;
				fprintf('qhit; returning to closest point\n');
                if(perim(1)/2 > perim(2)) 
                    %go backwards to qleave; it's closer that way
                    rightleft = -1;
                end     %otherwise keep going forwards
                perim(1) = 0;
            end
        else %we know the closest place to the goal; we're going there
            perim(1) = perim(1) + norm(pos - lastpos);
            if(norm(pos - qleave) < m) %we've arrived at qleave
               rightleft = 1;   %return to the standard left turn
               follow = 0;
               perim = [0 0];
			   qleave = [0 0];
			   
			   pts = zeros(2,0);
			   fprintf('qleave\n')
            end
        end
        
	end
	
	% v is the direction we want to be going
	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);

