%% Function A4PotField_user
%  Function takes in arguments pos, and goal, being position and goal
%  point, respectively. It returns a column vector denoting desired heading
%  of the robot.
function desvH = Key_PotField_user(pos,goal,bob)

% If variable bob exists, do a 3D plot of the nav field
if (nargin == 3)
	figure(12)
	Xpts = linspace(-10,10,100);
	Ypts = linspace(-10,10,100);
	phiVis = zeros(100,100);
	for iX = 1:length(Xpts)
		for iY = 1:length(Ypts)
			phiVis(iX,iY) = getPotential([Xpts(iX);Ypts(iY)],goal);
		end
	end
	surf(Ypts,Xpts,phiVis');%,'edgealpha',0)
end

% take a discrete gradient using a helper function to give numeric values
% for the potential field.
val = getPotential(pos,goal);
valdx = getPotential(pos + [.01;0],goal);
valdy = getPotential(pos + [0;.01],goal);

desvH = -([valdx;valdy] - val)/.01;



end


%% getPotential
%  Takes in position and goal points, and returns a value denoting the
%  magnitude of the potential field at that point. val is normailized and
%  will always be between 0 and 1.
function val = getPotential(x,goal)

%list of obstacles in the workspace
obst = [ 0 0 8;0 3.4 1;-3.4 0 1;2.4 -2.6 1.5]';

%repulsive term
B = prod(abs(sum((repmat(x,1,size(obst,2)) - obst(1:2,:)).^2,1).^.5 - obst(3,:)))^.125;

%attractive term
G = norm(x - goal);

%normalized value for potential
val = G/(B*3 + G);

end