import network
import espnow
import time
from machine import Pin

fwd = Pin(21, Pin.IN)
rev = Pin(20, Pin.IN)
left = Pin(5, Pin.IN)
right = Pin(4, Pin.IN)


# Initialize Wi-Fi in station mode
sta = network.WLAN(network.STA_IF)
sta.active(True)
#sta.config(channel=1)  # Set channel explicitly if packets are not delivered
sta.disconnect()

# Initialize ESP-NOW
e = espnow.ESPNow()
try:
    e.active(True)
except OSError as err:
    print("Failed to initialize ESP-NOW:", err)
    raise

# Receiver's MAC address , replace xx with your receivers MAC Adress ! Important !!!
receiver_mac = b'\xxx\xxx\xxx\xxx\xxx\xxx'


# Add peer
try:
    e.add_peer(receiver_mac)
except OSError as err:
    print("Failed to add peer:", err)
    raise

while True:
    
    try:
        if not fwd.value() :
            message = "forward"
        if not rev.value():
            message = "reverse"
        if not rev.value() and not left.value():
            message = "reverse_left"
        if not rev.value() and not right.value():
            message = "reverse_right"
        if not fwd.value() and not left.value():
            message = "forward_left"
        if not fwd.value() and not right.value():
            message = "forward_right"
        if fwd.value() and rev.value():
            message = "stop"
        
        print(f"message to send : {message}")
        if e.send(receiver_mac, message, True):
            print(f"Sent message: {message}")
        else:
            print("Failed to send message (send returned False)")
            
        time.sleep(0.2)  
        
    except OSError as err:
        print("Error:", err)
        time.sleep(5)
        
    except KeyboardInterrupt:
        print("Stopping sender...")
        e.active(False)
        sta.active(False)
        break