import RPi.GPIO as GPIO
import time
import sys
import websockets
import asyncio

pin_right = 13 # blue right
pin_left = 6 # purple left

pin_up = 20 # white up
pin_down = 21 # green down


GPIO.setmode(GPIO.BCM)

GPIO.setup(pin_up, GPIO.OUT)
GPIO.setup(pin_down, GPIO.OUT)
GPIO.setup(pin_left, GPIO.OUT)
GPIO.setup(pin_right, GPIO.OUT)

# PWM(pin, frequency_in_Hz)
pwm_pin_up = GPIO.PWM(pin_up, 150)
pwm_pin_down = GPIO.PWM(pin_down, 150)

pwm_pin_left = GPIO.PWM(pin_left, 150)
pwm_pin_right = GPIO.PWM(pin_right, 150)

socket_connections = {}

async def handle_client(websocket, path):
    global socket_connections

    print("packet connected")
    socket_connections[websocket] = ""

    try:
        while True:
            data = await websocket.recv()
            for command in data.split(":"):
                print(f"data is: {command}")
                await update_from_packet(command)
    except websockets.exceptions.ConnectionClosed:
        print("Connection closed with: ", websocket.remote_address)
    except Exception as e:
        print(f"There was an error: {e}")
    finally:
        print("closing websocket")
        del socket_connections[websocket]

axis_x = 0
axis_y = 0
async def update_from_packet(data):
    global axis_x, axis_y

    if data[0] == 'L':
        axis_x = -int(data[1:]) - 10
    if data[0] == 'R':
        axis_x = int(data[1:]) + 10
    if data[0] == 'U':
        axis_y = int(data[1:]) + 10
    if data[0] == 'D':
        axis_y = -int(data[1:]) - 10

    # if speed_up > 0:
    if axis_y > 0:
        pwm_pin_down.stop()
        pwm_pin_up.start(min(axis_y, 100))
        GPIO.output(pin_down, GPIO.LOW)
    
    # if speed_down > 0:
    if axis_y < 0:
        pwm_pin_up.stop()
        pwm_pin_down.start(min(abs(axis_y), 100))
        GPIO.output(pin_up, GPIO.LOW)

    #if speed_left > 0:
    if axis_x < 0:
        pwm_pin_right.stop()
        pwm_pin_left.start(min(abs(axis_x), 100))
        GPIO.output(pin_right, GPIO.LOW)

    # if speed_right > 0:
    if axis_x > 0:
        pwm_pin_left.stop()
        pwm_pin_right.start(min(axis_x, 100))
        GPIO.output(pin_left, GPIO.LOW)

    if axis_y == 0:
        pwm_pin_up.stop()
        pwm_pin_down.stop()
    if axis_x == 0:
        pwm_pin_left.stop()
        pwm_pin_right.stop()

async def main():
    server = await websockets.serve(handle_client, "0.0.0.0", 8765)

    print("server started")

    await server.wait_closed()

try:
    asyncio.run(main())
finally:
    pwm_pin_up.stop()
    pwm_pin_down.stop()
    pwm_pin_left.stop()
    pwm_pin_right.stop()

    GPIO.cleanup()