import RPi.GPIO as GPIO

LEFT_MOTOR_EN = 0
RIGHT_MOTOR_EN = 1

LEFT_MOTOR_LPWM = 3
LEFT_MOTOR_RPWM = 4

RIGHT_MOTOR_LPWM = 5
RIGHT_MOTOR_RPWM = 6

class driver:
    LM_LPWM = GPIO.PWM(LEFT_MOTOR_LPWM, 8000)
    LM_RPWM = GPIO.PWM(LEFT_MOTOR_RPWM, 8000)
    RM_LPWM = GPIO.PWM(RIGHT_MOTOR_LPWM, 8000)
    RM_RPWM = GPIO.PWM(RIGHT_MOTOR_RPWM, 8000)

    def setupMotors(self):
        GPIO.setup(LEFT_MOTOR_EN, GPIO.OUT)
        GPIO.setup(RIGHT_MOTOR_EN, GPIO.OUT)
        
        GPIO.setup(LEFT_MOTOR_LPWM, GPIO.OUT)
        GPIO.setup(LEFT_MOTOR_RPWM, GPIO.OUT)
        self.LM_LPWM.start(0)
        self.LM_RPWM.start(0)
        
        GPIO.setup(RIGHT_MOTOR_LPWM, GPIO.OUT)
        GPIO.setup(RIGHT_MOTOR_RPWM, GPIO.OUT)
        self.RM_LPWM.start(0)
        self.RM_RPWM.start(0)
        
    def enableMotors() :
        GPIO.output(LEFT_MOTOR_EN, GPIO.HIGH)
        GPIO.output(RIGHT_MOTOR_EN, GPIO.HIGH)
        
    def disableMotors() :
        GPIO.output(LEFT_MOTOR_EN, GPIO.LOW)
        GPIO.output(RIGHT_MOTOR_EN, GPIO.LOW)
        
    #input range 0.0-1.0; 0.5 = no forward no reverse
    def writeLeftPWM(self, dutycycle) :
        if dutycycle > 0.5 :
            self.LM_LPWM.ChangeDutyCycle(0)
            self.LM_RPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
        elif dutycycle < 0.5 :
            self.LM_LPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
            self.LM_RPWM.ChangeDutyCycle(0)
        else :
            self.LM_LPWM.ChangeDutyCycle(0)
            self.LM_RPWM.ChangeDutyCycle(0)
    
    #input range 0.0-1.0; 0.5 = no forward no reverse
    def writeRightPWM(self, dutycycle) :
        if dutycycle > 0.5 :
            self.RM_LPWM.ChangeDutyCycle(0)
            self.RM_RPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
        elif dutycycle < 0.5 :
            self.RM_LPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
            self.RM_RPWM.ChangeDutyCycle(0)
        else :
            self.RM_LPWM.ChangeDutyCycle(0)
            self.RM_RPWM.ChangeDutyCycle(0)