import sys, rone

########  part 0: helper functions ########
# global PWM setting for all motion
# oyu might need to increase this a bit if your robot doesn't move
MOTOR_PWM = 65

# stops the robot for the argument time
# arguments: time
# return: nothing
def move_stop(time):
    print 'move_stop'
    # your code goes here
    

# moves forward for the argument time
# arguments: time
# return: nothing
def move_forward(time):
    print 'move_forward'
    # your code goes here

    
# rotate right for the argument time
# arguments: time
# return: nothing
def move_rotate_right(time):
    print 'move_rotate_right'
    # your code goes here
    

# rotate left for the argument time
# arguments: time
# return: nothing
def move_rotate_left(time):
    print 'move_rotate_left'
    # your code goes here
    
    # your code stops here
    move_stop(0)
    

########  part 1: square motion ########

# tests forward motion
# arguments: nothing
# return: nothing
def move_test():
    # wait for 2 seconds to let you unplug the robot...
    print 'move_test'
    sys.sleep(2000)
    # your code goes here

    
# drives the robot in a square
# arguments: nothing
# return: nothing
def square_motion():
    # wait for 2 seconds to let you unplug the robot...
    print 'square dance!'
    sys.sleep(2000)
    # your code goes here




########  part 2: move towards light ########
# compute and return the difference between the left and the right light sensor
# arguments: nothing
# return: difference between left sensor and right sensor
def light_diff():
    # your code goes here


# Tests your light diff function.  Should print the difference between left and right sensors
# arguments: nothing
# return: nothing
def light_diff_test():
    diff_start = light_diff()
    print "diff_start", diff_start
    sys.sleep(1000)
    while True:
        diff = light_diff() - diff_start
        print "diff", diff

        # wait a bit before we print again
        sys.sleep(50)



# Move towards Light! Use the structure below, and the movement helper functions from above
# arguments: nothing
# return: nothing
def light_follow():
    diff_start = light_diff()
    print "diff_start", diff_start
    sys.sleep(1000)
    while True:
        diff = light_diff() - diff_start
        print "diff", diff
        
        # your code goes here




########  part 3: avoid obstacles with bump sensors ########
# Checks the bump sensor for impacts from the left
# arguments: nothing
# return: True if the bump sensor is pressed from the left, False otherwise
def bump_left_get_value():
    bump_bits = rone.bump_sensors_get_value()
    return ((bump_bits & 7) > 0)

# Checks the bump sensor for impacts from the frontt
# arguments: nothing
# return: True if the bump sensor is pressed from the front, False otherwise
def bump_front_get_value():
    bump_bits = rone.bump_sensors_get_value()
    return (bump_bits == 129)

# Checks the bump sensor for impacts from the right
# arguments: nothing
# return: True if the bump sensor is pressed from the right, False otherwise
def bump_right_get_value():
    bump_bits = rone.bump_sensors_get_value()
    return ((bump_bits & 224) > 0)


# Prints the status of the bump sensor functions
# arguments: nothing
# return: nothing
def bump_test():
    print "bump_test()"
    sys.sleep(1000)
    while True:
        # your code goes here
        

# Move the robot away rom obstacles using the bump sensors
# arguments: nothing
# return: nothing
def bump_avoid():
    print "bump_avoid()"
    sys.sleep(1000)
    while True:
        # your code goes here
            


########  part 4: avoid obstacles with IR sensors ########

# Use the IR sensors to detect obstacles
# arguments: nothing
# return: tuple of booleans (obs_front, obs_left, obs_right)
def obstacle_detect():
    obs_front = False
    obs_left = False
    obs_right = False
    rone.ir_comms_send_message('E128');
    sys.sleep(20)
    msg = rone.ir_comms_get_message()
    if msg != None:
        (msg, recv_list, xmit_list, range_bits) = msg
        if (0 in recv_list) and (7 in recv_list):
            obs_front = True
        if (0 in recv_list) or (1 in recv_list):
            obs_left = True
        if (6 in recv_list) or (7 in recv_list):
            obs_right = True
    return (obs_front, obs_left, obs_right)


# Tests our obstacle_detect function.  Should print true or false depending of the nearby obstacles
# arguments: nothing
# return: nothing
def obstacle_detect_test():
    while True:
        # your code goes here
        

# Move away from obstacles! Use the structure below, and the movement helper functiosn from above
# arguments: nothing
# return: nothing
def obstacle_avoid():
    sys.sleep(1000)
    while True:
        (obs_front, obs_left, obs_right) = obstacle_detect()
        # your code goes here




########  Initial function call to test all parts of the code ########
#move_test()
#square_motion()

#light_diff_test()
#light_follow()

#bump_test()        
#bump_avoid()

#obstacle_detect_test()
#obstacle_avoid()