from machine import Pin, PWM, ADC
from time import sleep

RATIO = 0.5 #-horizontal ratio
DEADZONE = 20 #Deadzone for the joystick, adjust to be larger/smaller as needed

p0 = PWM(Pin(0), freq = 1000) #Forward Left
p1 = PWM(Pin(1), freq = 1000) #Backward Left
p2 = PWM(Pin(19), freq=1000)  #Forward Right
p3 = PWM(Pin(20), freq=1000)  #Backward Right
pi0 = ADC(Pin(26)) #vertical input
pi1 = ADC(Pin(27)) #horizontal input
pi2 = ADC(Pin(28))



def readingToDuty(vread: uint16, hread: uint16):
    vread -= 32768  #Subtracting 2^16 / 2 since 2^16/2 is the default reading
    hread -= 32768  #Feel free to change these numbers if the "home" position is different than 2^16 / 2

    #Sets the readings to 0 if they are within the deadzone
    if(abs(vread) <= DEADZONE): 
        vread = 0
    if(abs(hread) <= DEADZONE):
        hread = 0

    #If there is a vertical component, we set it to be the vertical reading subtracting/adding the hreading times the ratio
    #The *2s are to account for subtracting earlier
    if(vread != 0):
        lduty = (vread * 2) - (hread * 2 * RATIO)
        rduty = (vread * 2) + (hread * 2 * RATIO)
    else:
    #Otherwise, it's just positive and negative horizontal reading
        lduty = -1 * hread * 2
        rduty = hread * 2
    
    #Convert to int and clamp to the uint16 values
    lduty = int(lduty)
    rduty = int(rduty)
    lduty = max(min(lduty, 65535), -65535)
    rduty = max(min(rduty, 65535), -65535)

    #Return the correct set of outputs
    if(lduty >= 0):
        return (lduty, 0, rduty, 0)
    else:
        return (0, abs(lduty), 0, abs(rduty))


while 1:
    #read pins
    vert = pi0.read_u16();
    horiz = pi1.read_u16();
    #Send to function and take reading (f/b = forward/backward l/r = left/right)
    fl, bl, fr, br = readingToDuty(vert, horiz)
    #Print to console for Debug Purposes
    print(fl,bl,fr,br)
    #Apply figures to the motor controller
    p0.duty_u16(fl);
    p1.duty_u16(bl);
    p2.duty_u16(fr);
    p3.duty_u16(br);