\

'''
File: motor.py
Author: Mahmoud
description: Motor control logic and responding to 
    the line_detection outputs. communicate with the API for 
    finding the shortest path to the junction requested by the 
    mobile app end.
'''

import RPi.GPIO as gpio
import time

gpio.setmode(gpio.BOARD)
gpio.setwarnings(False)
gpio.setup(16, gpio.OUT)
gpio.setup(18, gpio.OUT)
gpio.setup(32, gpio.OUT)
gpio.setup(33, gpio.OUT)
gpio.setup(11, gpio.OUT)
gpio.setup(13, gpio.OUT)
rm = gpio.PWM(32, 1000)
lm = gpio.PWM(33, 1000)
rm.start(0)
lm.start(0)


def setup():
    pass


def rm_forward(speed):
    gpio.output(18, True)
    gpio.output(16, False)
    rm.ChangeDutyCycle(speed)

def lm_forward(speed):
    gpio.output(11, False)
    gpio.output(13, True)
    lm.ChangeDutyCycle(speed)

def rm_backward(speed):
    gpio.output(18, False)
    gpio.output(16, True)
    rm.ChangeDutyCycle(speed);

def lm_backward(speed):
    gpio.output(11, True)
    gpio.output(13, False)
    lm.ChangeDutyCycle(speed)

def stop():
    gpio.output(18, False)
    gpio.output(16, False)
    gpio.output(11, False)
    gpio.output(13, False)

def straight(speed):
    rm_forward(speed)
    lm_forward(speed)

def start():
    straight(35)

def reverse(speed):
    rm_backward(speed)
    lm_backward(speed + 20)

def steer(curve, lr, jn):
    print(f'recerived: {curve} {lr} jn{jn}')
    # lr gain
    speed = 35
    offset_lr = lr * 0.15
    r_tune = 5 # help make the base speed of each motor same

    #direction control using lr only
    rm_forward(speed - offset_lr)
    lm_forward(speed + offset_lr)


if __name__ == '__main__':
    try:
        straight(40)
        time.sleep(3)
        stop()
        time.sleep(1)
        reverse(40)
        time.sleep(3)
        stop()

        while True:
                pass

    except Exception as e:
        print(e)