# Fernsteurung RoboCar 
from machine import Pin
import time
links1 = Pin(33, Pin.IN, Pin.PULL_UP)  #  erhöht 'gang' um 1
links2 = Pin(25, Pin.IN, Pin.PULL_UP)  #  setzt 'gang' auf 0
links3 = Pin(26, Pin.IN, Pin.PULL_UP)  #  vermindert 'gang' um 1

rechts1 = Pin(27, Pin.IN, Pin.PULL_UP)  #  kurve links
rechts2 = Pin(14, Pin.IN, Pin.PULL_UP)  #  gerade
rechts3 = Pin(12, Pin.IN, Pin.PULL_UP)  #  kruve rechts

#          -3    -2    -1   0   1    2    3    4    5
gaenge = [-900, -800, -666, 0, 666, 750, 830, 900, 1023]

pressed = False  # L-aktive Eingänge
gang = gang_links = gang_rechts = 0
gang_max = 5; gang_min = -3

richtung = 0;   # -1 - links, 0 - gerade, 1 - rechte
cnt = 0
print(cnt," Start ",  gang, gaenge[-gang_min + gang])
while True:
    cnt += 1
    l1 = links1.value()==pressed
    l2 = links2.value()==pressed
    l3 = links3.value()==pressed
    r1 = rechts1.value()==pressed
    r2 = rechts2.value()==pressed
    r3 = rechts3.value()==pressed
    
    time.sleep(0.5)
    if l1 or l2 or l3:
        if l1 and l2 and l3:
            pass
        elif l1 and l2:
            pass
        elif l1 and l3:
            pass
        elif l2 and l3:
            pass
        elif l1:
            gang -= 1
            if gang < gang_min:
                gang = gang_min
        elif l2:
            gang = 0
        elif l3:
            gang += 1
            if gang > gang_max:
                gang = gang_max
        print(cnt," links ", l1, l2, l3, gang, gaenge[-gang_min + gang])
    else:
        if r1 or r2 or r3:    # Steuerung der Richtung durch unterschiedliche Gänge der Motoren 
            if richtung == 0:
                gang_links = gang_rechts = gang
            if r1 and r2 and r3:
                pass
            elif r1 and r2:
                pass
            elif r1 and r3:
                pass
            elif r2 and r3:
                pass
            elif r1:    # nach links
                if richtung == 0:
                    richtung = -1
                if richtung == -1:
                    gang_links -= 1
                    if gang_links<gang_min:
                        gang_links = gang_min
                if richtung == 1:
                    gang_rechts += -1
                    if gang_rechts == 0:
                        richtung = 0
            elif r2:
                gang_links = gang_rechts = gang
            elif r3:  # nach rechts
                if richtung == 0:
                    richtung = 1
                if richtung == 1:
                    gang_rechts += 1
                    if gang_rechts>gang_max:
                        gang_rechts = gang_max
                elif richtung == -1:
                    gang_links += 1
                    if gang_links == 0:
                        richtung = 0
                        gang_links = gang_rechts = gang

                if richtung == -1:
                    gang_links -= 1
                    if gang_links<gang_min:
                        gang_links = gang_min

                gang = 0
            print(cnt, " ", r1, r2, r3, gang, gang_links, gang_rechts, gaenge[-gang_min+gang_links], gaenge[-gang_min+gang_rechts])