from hub import button, port, motion_sensor as ms
import motor
import motor_pair as mp
import color_sensor as cs
import color
import sys
import runloop
import time

# await mp.move_for_degrees(mp.PAIR_1, 360, 45, velocity=280)

# motor.run_for_degrees(portNum, 200, 300)
# await runloop.sleep_ms(2000)

# mp.move_tank(mp.PAIR_1, 280, -280)
# await runloop.sleep_ms(2000)

# WANTED_COLOR = color.BLACK

async def move_on_color():
    start_time = time.time()
    print(ms.tilt_angles())
    while True:
        roll_value = ms.tilt_angles()[2]
        if cs.reflection(port.C) < 60:
            mp.move_tank(mp.PAIR_1, -100, 100)
        elif cs.reflection(port.E) < 60:
            mp.move_tank(mp.PAIR_1, 100, -100)
        elif roll_value < -150: # uphill function
            mp.move(mp.PAIR_1, 0, velocity=450)
        else:
            mp.move(mp.PAIR_1, 0, velocity=200)

    # Geradeaus

    # linkskurve
    # rechtskurve 

    # Kreuzung (grüner Punkt)
    # wenn grün, dann biege um 90 Grad in die richtung ab (wenn vor schwarzer linie)


    # Sackgasse (= zwei grüne Punkte)
    # -> Drehung 180 Grad

    # Bergauf
    # Bergab

    # Speedbumper (Schräge Bumper)


# Wippe Notizen
def flipflop(state, r, s):
    """
    Logik implementiert in Rampen Logik. Bleibe stehen, wenn der Rollwinkel sich um mehr als 10 ändert.
    RS-Flipflop-Logik: 
        Set (= tue nichts), wenn der Winkel sich ändert; 
        Reset (= bleibe stehen), solange sich der Rollwinkel ändert, danach fahre normal weiter
    """
    return False if r else (True if s else state)

# Obstacle Notizen

# Ziellinie Notizen
# Wenn beide Sensoren rot, stoppe die Motoren

async def main():
    mp.pair(mp.PAIR_1, port.A, port.D)
    await move_on_color()

    sys.exit()

runloop.run(main())