import pygame
import serial
import time 

ser = serial.Serial('COM5',115200, timeout=0.01)
print(ser.name)

pygame.init()
pygame.joystick.init()
pad = pygame.joystick.Joystick(0)
pad.init()
print(pad.get_name())

def remap(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


while(True):
    pygame.event.pump()

    X_axis = round(pad.get_axis(2),4)
    Y_axis = round(pad.get_axis(3),4)
    Z_axis = round(pad.get_axis(0),4)

    # print(f'Axis 2: {X_axis}\tAxis 3: {Y_axis} \tAxis 0: {Z_axis}')    

    X_axis = round(remap(X_axis,-1,1,0,1000))
    Y_axis = round(remap(Y_axis,-1,1,0,1000))
    Z_axis = round(remap(Z_axis,-1,1,0,1000))

    # print(f'Axis X: {X_axis}\tAxis Y: {Y_axis} \tAxis Z: {Z_axis}')    



    ser.write(('X'+str(X_axis)+'\n').encode())
    ser.write(('Y'+str(Y_axis)+'\n').encode())
    ser.write(('Z'+str(Z_axis)+'\n').encode())

    income = ser.readline()
    if (len(income)>0):
        print(income)

    # time.sleep(0.1)