# ESP32-espnow-receiver-ADXL345-robocar4w-mecanum.py
import network, espnow, time, machine
from robocar4w import *

wlan_sta = network.WLAN(network.STA_IF)
wlan_sta.active(True)

e = espnow.ESPNow()
e.active(True)

peer = b'\xff'*6     # alle Sender
e.add_peer(peer)

led = machine.Pin(2,machine.Pin.OUT)
#      'f -1023 -1023 -1023 -1023', 'f -1023 -1023 -1023 -1023', 'f -1023 -1023 -1023 -1023', 'f -1023 -1023 -1023 -1023', 'f -1023 -1023 -1023 -1023', 'f -1023 -1023 -1023 -1023', 'f -1023 -1023 -1023 -1023',
#                   -3                           -2                           -1                            0                            1                            2                            3
fw =[ ['f 700 1023',                'f 850 1023',                'f 950 1023',                'f 1000 1000 1000 1000',     'f 1023 950',                'f 1023 850',                'f 1023 700'],
      ['f 650 900',                 'f 750  900',                'f 850 900',                 'f 850 850 850 850',         'f 900 950',                 'f 900 850',                 'f 900 700'],
      ['f 650 800',                 'f 700  800',                'f 750 800',                 'f 700 700 700 700',         'f 800 750',                 'f 800 700',                 'f 800 650'],
      ['f 900 -900 -900 900',       'f 800 -800 -800 800',       'f 700 -700 -700 700',       'f 0',                       'f -700 700 700 -700',       'f -700 700 700 -700',       'f -700 700 700 -700'],
      ['f -650 -800',               'f -700  -800',              'f -750 -800',               'f -700 -700 -700 -700',     'f -800 -750',               'f -800 -700',               'f -800 -650'],
      ['f -700 -900',               'f -750  -900',              'f -850 -900',               'f -850 -850 -850 -850',     'f -900 -850',               'f -900 -800',               'f -900 -700'],
      ['f -700 -1000',              'f -750  -1000',             'f -850 -1000',              'f -1000 -1000 -1000 -1000', 'f -1000 -850',              'f -1000 -800',              'f -1000 -700']
    ]

ende = False
while not ende:
    host, msg = e.irecv(500)   # ms bis Timeout
    if msg:
        tmp = msg.decode().replace("\x00","").split(",")
        if len(tmp)!=5:
            print(".", end="")
            continue
        #ADXL345-a,68,15,-20,281
        kennung = tmp[0]
        if kennung != "ADXL345-a":
            print("Falsche Kennung: " + kennung)
            continue 
        cnt     = int(tmp[1])
        ac_x    = int(tmp[2])
        ac_y    = int(tmp[3])
        ac_z    = int(tmp[4])

        dw = 100
        spa = (ac_x+dw//2)//dw; zei = (ac_y+dw//2)//dw
        # formatierte Ausgabe mit der f'-Strings
        eingabe = fw[3-zei][3+spa]
        print(f'{ac_x:6}, {ac_y:6}, {ac_z:6}, {zei:2}, {spa:2}, {eingabe}')
        led.value(not led.value())
        if ac_z<-100:
            print("Ende")
            eingabe="e"
        if eingabe=="":
            print("Nix passiert!")
            continue
        elif eingabe == "e":
            print("'Ende' wird eingeleitet!")
            ende = True
            continue
        elif eingabe == "h":
            halt()
        else:
            werte = eingabe.split(" ")
            l = len(werte)
            print( "werte: ", werte )
            if werte[0]=="f":
                if l==2:
                    fahre( int(werte[1]) )
                elif l==3:
                    fahre( int(werte[1]), int(werte[2]))
                elif l==5:
                    fahre( int(werte[1]), int(werte[2]),  int(werte[3]), int(werte[4]) )
                else:
                    pass
    else:
        print(".", end="")