import RPi.GPIO as GPIO 
import time
import sys
import atexit
import socket

def at_exit():
    off()

atexit.register(at_exit)

def setup():
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(31,GPIO.OUT)
    GPIO.setup(33,GPIO.OUT)
    GPIO.setup(35,GPIO.OUT)
    GPIO.setup(37,GPIO.OUT)

def backward():
    GPIO.output(31,GPIO.HIGH)
    GPIO.output(33,GPIO.LOW)

def forward():
    GPIO.output(31,GPIO.LOW)
    GPIO.output(33,GPIO.HIGH)

def stop():
    GPIO.output(31,GPIO.LOW)
    GPIO.output(33,GPIO.LOW)
    
def left():
    GPIO.output(35,GPIO.HIGH)
    GPIO.output(37,GPIO.LOW)

def right():
    GPIO.output(35,GPIO.LOW)
    GPIO.output(37,GPIO.HIGH)

def middle():
    GPIO.output(35,GPIO.LOW)
    GPIO.output(37,GPIO.LOW)

def off():
    middle()
    stop()
    GPIO.cleanup()

def main():
    
    setup()
    
    UDP_IP = "192.168.1.105"
    UDP_PORT = 2319

    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.bind((UDP_IP, UDP_PORT))

    while True:
        sock.listen(1)
        connection, adresse = sock.accept()
        data = connection.recv(2048)
        print ("Received message: ", data)
    
        if data == b"forward":
            forward()
        elif data == b"backward":
            backward()
        elif data == b"right":
            right()
        elif data == b"left":
            left()
        elif data == b"middle":
            middle()
        elif data == b"stop":
            stop()

if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        off()