import Myro
Myro.init("sim")
robot = Myro.getRobot()
Myro.setOption("show-sensors", True)

Myro.getSimulation()

robot.getObstacle(1)

robot.forward(1, 1)

Myro.getSimulation()

robot.getObstacle(1)

robot.forward(1,1)
print(robot.getObstacle(1))

Myro.getSimulation()

from ai.governor import GovernorNetwork
net = GovernorNetwork(2, 0.1, 0.1) # size of inputs + outputs
net.addLayers(1, 3, 1)
trial_count = 0
count = 0

def teacher(distance):
    if distance < .5:
        target = 0.5
    elif distance < .8:
        target = .66
    elif distance < .99:
        target = .83
    else:
        target = 1.0
    return target

robot.setPose(100, 250, 0)
net.setLearning(1)
t = 0

while True:
    count += 1
    obstacle = robot.getObstacle(1)
    distance = float(1 - obstacle/6400) 
    target = teacher(distance)
    speed = target * 2 - 1.0
    if distance < .5:
        if t == 0:
            robot.stop()
            t = Myro.currentTime()
        elif Myro.currentTime() - t >= 1.5:
            trial_count += 1
            robot.setPose(100, 250, 0)
            t = 0
        # else keep waiting
    else:
        robot.forward(speed)
    results = net.step(input=[distance], output=[target])
    calico.animate("Trails: %s, Updates: %s" % (trial_count, count), str(net))


net.setLearning(0)
robot.setPose(100, 250, 0)
data = []
steps = []
step = 0

while True:
    obstacle = robot.getObstacle(1)
    distance = float(1 - obstacle/6400) 
    results = net.propagate(input=[distance])
    data.append([distance, teacher(distance), results[0]])
    steps.append([step, teacher(distance), results[0]])
    speed = (results[0] * 2) - 1.0
    robot.forward(speed)
    calico.animate(speed)
    step += 1


calico.ScatterChart(['step', 'Teacher', 'Network'], steps, 
                    {"hAxis": {"title": "step"}, "vAxis": {"title": "output"}, "height": 500})

calico.ScatterChart(['distance', 'Teacher', 'Network'], data, 
                    {"hAxis": {"title": "distance"}, "vAxis": {"title": "output"}, "height": 500})

robot.setPose(600, 250, 0)

Myro.getSimulation()

data_gen = []

while True:
    obstacle = robot.getObstacle(1)
    distance = float(1 - obstacle/6400) 
    results = net.propagate(input=[distance])
    data_gen.append([distance, None, results[0]])
    speed = (results[0] * 2) - 1.0
    robot.forward(speed)
    calico.animate(speed)

calico.ScatterChart(['distance', 'Teacher', 'Network'], data + data_gen, 
                    {"hAxis": {"title": "distance"}, "vAxis": {"title": "output"}, "height": 500})