#!/usr/bin/env python

import rospy, time

from linedetector import LineDetector
from motordriver import MotorDriver

class AutoDrive:

    def __init__(self):
        rospy.init_node('xycar_driver')
        self.line_detector = LineDetector('/usb_cam/image_raw')
        self.driver = MotorDriver('/xycar_motor_msg')

    def trace(self):
        Lx1, Ly1, Lx2, Ly2, Rx1, Ry1, Rx2, Ry2 = self.line_detector.detect_lines()
        self.line_detector.show_images()
        lGradient = (Lx2 - Lx1) / (Ly2 - Ly1)
        rGradient = (Rx2 - Rx1) / (Ry2 - Ry1)
        print(lGradient, rGradient)

        angle = self.steer(lGradient, rGradient)
        speed = self.accelerate(angle)
        self.driver.drive(angle + 90, speed + 90)

    def steer(self, left, right):
        left


    def accelerate(self, angle):
        if angle < -20 or angle > 20:
            speed = 20
        else:
            speed = 30
        return speed

    def exit(self):
        print('finished')

if __name__ == '__main__':
    car = AutoDrive()
    time.sleep(3)
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        car.trace()
        rate.sleep()
    rospy.on_shutdown(car.exit)
