#!/usr/bin/env python3

import sys, argparse
import time
import threading

import serial
import pygame


def serialreceive(ser):
  print("Serial running")
  while ser:
    try:
      if ser.in_waiting > 0:
        line = ser.read(ser.in_waiting)
        print(line)
    except Exception:
      pass


def main(argv):
  parser = argparse.ArgumentParser()
  parser.add_argument('-P', '--serialport', help="serial port to open", default="/dev/ttyUSB0")
  parser.add_argument('--delay', help="add delay before startup", type=int, default=1)
  args = parser.parse_args()

  pygame.init()
  pygame.joystick.init()
  joysticks = [pygame.joystick.Joystick(x) for x in range(pygame.joystick.get_count())]
  for joystick in joysticks:
    joystick.init()
    name = joystick.get_name()
    print("Found joystick: ")
    print(name)

  with serial.Serial(args.serialport, 115200, timeout=12) as ser:
    receivethread = threading.Thread(target=serialreceive, args=(ser,))
    receivethread.daemon = True
    receivethread.start()
    time.sleep(args.delay)

    axis_data = {0:0,1:0,2:0,3:0,4:0}
    try:
      while True:
        for event in pygame.event.get():
          if event.type == pygame.JOYAXISMOTION:
            axis_data[event.axis] = round(event.value,2)
            if abs(axis_data[event.axis]) <= 0.15:
              axis_data[event.axis] = 0;
          print(axis_data)
          
          x = axis_data[0]
          y = axis_data[1]
          r = axis_data[3]
          m0 = +y - x - r
          m1 = +y + x - r
          m2 = -y + x - r
          m3 = -y - x - r
          m0 *= 4000
          m1 *= 4000
          m2 *= 4000
          m3 *= 4000
          command = 'S '+str(int(m0))+','+str(int(m1))+','+str(int(m2))+','+str(int(m3))+'\n'
          ser.write(bytes(command, "utf8"))
          #ser.flush()
          #time.sleep(0.05)
          print(command)
    except KeyboardInterrupt:
      print("KeyboardInterrupt. Exiting")

    #ser.write(b'S 800,800,-800,-800\n')
    #time.sleep(2)
    ser.write(b'DISABLE')

  print("serial closed")

if __name__ == "__main__":
  main(sys.argv[1:])