import RPi.GPIO as GPIO
import time

GPIO.setwarnings(False)			# enable warning from GPIO
GPIO.setmode(GPIO.BCM)			# GPIO numbering

class RasPiRCCar():
    def __init__(self, motor_dir_pin, motor_pwm_pin, steering_pwm_pin, debug=False):
        self.debug = debug
        if self.debug:
            print('Initialising RC Car')
        
        self.motor_dir_pin = motor_dir_pin
        
        # Init Throttle
        GPIO.setup(motor_pwm_pin, GPIO.OUT)
        GPIO.setup(motor_dir_pin, GPIO.OUT)
        GPIO.setup(steering_pwm_pin, GPIO.OUT)
        time.sleep(0.01)
        self.throttle_pwm = GPIO.PWM(motor_pwm_pin, 100)
        self.steering_pwm = GPIO.PWM(steering_pwm_pin, 100)
        self.steering_pwm.start(5)

    def _update_steering(self, angle):
        # Move the steering servo to angle (in degrees)
        assert(0.0<=angle<=90.0)
        offset = 0.05
        duty = float(angle+90.0) / 10.0 + offset
        self.steering_pwm.ChangeDutyCycle(duty)

    def _update_throttle(self, speed):
        # Set the pwm duty of the motor Between -100% and 100%
        is_forward = (speed >= 0)
        GPIO.output(self.motor_dir_pin, (not is_forward))
        self.throttle_pwm.start(int(abs(speed)))

    def drive(self, speed, angle):
        if self.debug:
            print('Speed: {}%, Steering Angle: {}deg'.format(speed,angle))
        self._update_throttle(speed)
        self._update_steering(angle)

if __name__ == "__main__":
    # Pinout
    # https://halckemy.s3.amazonaws.com/uploads/attachments/246779/pi-pinout-diagram-01_zvOXVJqUBp.png
    
    # Servo Example    
    # http://razzpisampler.oreilly.com/ch05.html
    
    # Cytron MDD10 Example
    # https://github.com/CytronTechnologies/Cytron_MDD10_Hat
    
    settings = {'motor_dir_pin' : 26,
                'motor_pwm_pin' : 12,
                'steering_pwm_pin' : 18,
                'debug' : True}
    
    car = RasPiRCCar(**settings)
    print('Testing Motors!')

    while True:
        for motor_speed in range(0, 100):
            car.drive(motor_speed, 45.0)
            time.sleep(0.1)

        time.sleep(1)

        for motor_speed in range(0, 100):
            car.drive(-motor_speed, 45.0)
            time.sleep(0.1)

        time.sleep(1)

        for steering_angle in range(0, 90):
            car.drive(0.0, steering_angle)
            time.sleep(0.01)

        for steering_angle in range(0, 90):
            car.drive(0.0, 90-steering_angle)
            time.sleep(0.01)
        
        car.drive(0, steering_angle)
        time.sleep(1)