import pygame
import serial
pygame.init()
joysticks = []
clock = pygame.time.Clock()
keepPlaying = True

left_right_stick = 0
forward_stick = 0
backward_stick = 0

def sign(num):
  if num == 0:
    return 0
  elif num > 0:
    return 1
  else:
    return -1

serport = serial.Serial("/dev/ttyACM0", 9600, timeout=1)
def send_speed(left, right):
  total = ""
  total += f"{(sign(left) + 1):02x}"
  total += f"{(abs(left)):02x}"
  total += f"{(sign(right) + 1):02x}"
  total += f"{(abs(right)):02x}"

  serport.write(bytes(total, 'utf-8'))
  serport.flush()
  print(total)

for i in range(0, pygame.joystick.get_count()):
  joysticks.append(pygame.joystick.Joystick(i))
  joysticks[-1].init()
  print ("Detected joystick "),joysticks[-1].get_name(),"'"

def make_values():
  left = 255 - max(0, left_right_stick) * 255
  right = 255 + min(0, left_right_stick) * 255
  speed_mod = backward_stick + forward_stick
  return (int(left * speed_mod) , int(right * speed_mod))

while keepPlaying:
  clock.tick(60)
  values = make_values()
  send_speed(values[0], values[1])
  for event in pygame.event.get():
    if event.type != 1536: continue
    if event.axis == 0: left_right_stick = event.value
    if event.axis == 2: backward_stick = event.value * -0.5 - 1
    if event.axis == 5: forward_stick = event.value * 0.5 + 1