import socket
import pygame
from argparse import ArgumentParser

parser = ArgumentParser()
parser.add_argument('ip', metavar='ip', type=str, help='ip address of the controlled robot')
args = parser.parse_args()

ip = args.ip

pygame.init()
pygame.display.set_mode((640, 480))

rc_socket = socket.socket()
try:
    rc_socket.connect((ip, 1234)) # connect to robot
except socket.error:
    print("could not connect to socket")

running = True
while running:
    u1 = 0
    u2 = 0
    vmax = 1.0
    events = pygame.event.get()
    for event in events:
        if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_LEFT:
                u1 = -vmax 
                u2 = vmax
                print(f"turn left: ({u1},{u2})")
            elif event.key == pygame.K_RIGHT:
                u1 = vmax
                u2 = -vmax
                print(f"turn right: ({u1},{u2})")
            elif event.key == pygame.K_UP:
                u1 = vmax
                u2 = vmax
                print(f"forward: ({u1},{u2})")
            elif event.key == pygame.K_DOWN:
                u1 = -vmax 
                u2 = -vmax 
                print(f"backward: ({u1},{u2})")
            elif event.key == pygame.K_ESCAPE:
                print("quit")
                running = False
                u1 = 0.0
                u2 = 0.0
            rc_socket.send(f'({u1},{u2})\n'.encode())
        elif event.type == pygame.KEYUP:
            print("key released, resetting: ({},{})".format(u1, u2))
            rc_socket.send(f'({u1},{u2})\n'.encode())