Angle debugging

To do: re-insert closeThresh and thetaThresh, change -pi/2 to pi/2 case
to atan(-deltaY,-deltaX), set alpha = 0 for rho<<