'''
Mission 7, Objective 9
'''
from botcore import *
from time import sleep

power = 1
thresh = 75
go_motors = False

def cal_thresh():
    global thresh
    det = 101
    sensed = prox.range(10, power)
    if sensed[LEFT] > 0:
        det = sensed[LEFT]
    if sensed[RIGHT] > 0:
        det = min(det, sensed[RIGHT])
    if det > 100:
        thresh = 100
    else:
        thresh = det - 5
    print("Sensed=", sensed, ", set thresh=", thresh)

def cal_power():
    global power
    power = 0
    while power < 8:
        power = power + 1
        cal_thresh()
        if thresh < 100:
            break

# -- Main Program --
motors.enable(go_motors)
while True:
    if buttons.was_pressed(1):
        cal_power()
        print("CAL: Power=", power, ", thresh=", thresh)

    if buttons.was_pressed(0):
        go_motors = not go_motors
        motors.enable(go_motors)
        leds.user_num(0, go_motors)
    
    p = prox.detect(power, thresh)
    leds.prox(p)

    if p[LEFT] and p[RIGHT]:
        motors.run(LEFT, 20)
        motors.run(RIGHT, 20)
    elif p[LEFT]:
        motors.run(LEFT, -20)
        motors.run(RIGHT, 20)
    elif p[RIGHT]:
        motors.run(LEFT, 20)
        motors.run(RIGHT, -20)
    else:
        motors.run(LEFT, 0)
        motors.run(RIGHT, 0)