from ev3robot import *
 
# Roboter initialisieren
robot = LegoRobot()
 
# Linker und rechter Motor hinzufuegen
right = Motor(MotorPort.A)
robot.addPart(right)
 
left = Motor(MotorPort.B)
robot.addPart(left)

# Hebearm hinzufuegen
heber = Motor(MotorPort.C)
robot.addPart(heber)
heber.setSpeed(10)
 
# Ultraschall Distanzsensor hinzufuegen
us = UltrasonicSensor(SensorPort.S4)
robot.addPart(us)

if us.getDistance()==-2:
    print("FEHLER: Distanzsensor nicht korrekt angeschlossen. Bitte Port und stecker kontrollieren!")
    robot.exit()
    exit()  # Programm ganz beenden
  
# Lichtsensor hinzufuegen und starten
ls = LightSensor(SensorPort.S3)      # Ueberpruefen, ob der Sensor auch wirklich an Port 3 angeschlossen ist!
robot.addPart(ls)
ls.activate(True)

#
##################################
## Platz fuer eigene Funktionen ##
##################################
# Beispiele: 
#    steuern(0,0)    # stoppt die Motoren
#    steuern(20,-20) # rechts im Kreis drehen
#    steuern(40,40)  # gerade aus
#    steuern(10,30)  # Links-Kurve
def steuern(links, rechts):
    left.setSpeed(int(abs(links)))
    if (links>=0):
        left.forward()
    elif (links<0):
        left.backward()
    right.setSpeed(int(abs(rechts)))
    if (rechts>=0):
        right.forward()
    elif (rechts<0):
        right.backward() 
        

# heberAction(-140) z.B. faehrt den Heber runter zum Aufladen 
def heberAction(grad):
    heber.continueTo(grad)
    Tools.delay(500)
    
# Diese Funktion soll einer Kante folgen, links schwarz, rechts weiss
# bis der Distanzsensor eine gegebene Distanz unterschreitet.
# Z.B. followToDist(20) bewirkt ein Folgen bis zur Distanz 20.
# Bevor die Funktion beendet wird, sollen die Motoren gestoppt werden.

def followToDist(limit):
    while True:
        dist = us.getDistance() # Aktuelle Distanz
        hell = ls.getValue()    # Aktuelle Helligkeit
        # Daten auswerten, Motoren steuern, mit break Schleife und damit Funktion verlassen.

        # TODO: Hier fehlt Ihr Code



# Platz fuer weitere praktische Funktionen        
    
        
###########################################
## B E G I N   D E S   P R O G R A M M S ##
###########################################

# Startposition vom Hebel: Ganz oben (liegt am Gehaeuse an)
 


# Challenge 1: Folgen, aufladen, rueckwaerts, drehen, abladen.
def challenge1():
    # Messen und ausgeben (damit man eine Ahnung der Werte hat)
    hell = ls.getValue()
    dist = us.getDistance()
    print("Hell = %d,  Dist = %d" % (hell,dist))   
    heberAction(-140)  # Auf Winkel -140
    heberAction(0)     # Wieder in Ausgangsposition
    # TODO: Hier fehlt Ihr code   
       

# Funktion aufrufen
challenge1()


robot.exit()         # Programm ordnungsgemäss beenden.