Add debugging information

[aaron97neu]

Throughout the code, we see occasional debugging statements that have been commented out (and some that have not) like such:

# Debugging
# print "123"
# print nextPoint
# print "456"

These should be converted to real debug statements that allow for global enabling/disabling of debug statements. They should note the function/calltrace and other debug information, like such:

# Debugging
if debug == True
  print "Function: get_current_point()\t| var: nextPoint\t| value: "+nextpoint

or better yet

def debug_print(str)
  if debug == True
    print str

# Later in code 
debug_print("Function: get_current_point()\t| var: nextPoint\t| value: "+nextpoint)

I am not set on any specific debug syntax, but it should be standardized by whoever chooses to work on this issue