#!/usr/bin/env python

import rospy, time

from linedetector import LineDetector
from motordriver import MotorDriver
from imuread import ImuRead


class AutoDrive:

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

    def trace(self):
        line_l, line_r, red, yellow, green = self.line_detector.detect_lines()
        self.line_detector.show_images(line_l, line_r)
        r, p, y = self.imu.get_data()
        angle = self.steer(line_l, line_r)
        speed = self.accelerate(angle, red, yellow, green)
        self.driver.drive(angle + 90, speed + 90)

        print("Speed : " + speed + 90)
        print(r, p, y)

    def steer(self, left, right):
        if left == -1:
            if 320 < right < 390:
                angle = -50
            else:
                angle = (550 - right) / (-3)
        elif right == -1:
            if 250 < left < 320:
                angle = 50
            else:
                angle = (left - 90) / 3
        else:
            angle = 0
        return angle

    def accelerate(self, angle, red, yellow, green):

        # if red is detected, stop
        if red and (not yellow) and (not green):
            speed = 0
        # if yellow is detected, go low speed
        elif (not red) and yellow and (not green):
            speed = 20
        # if green is detected, go origin speed
        elif (not red) and (not yellow) and green:
            speed = 50

        # This is origin code
        else: #(not red) and (not yellow) and (not green):
            if angle <= -40 or angle >= 40:
                speed = 30
            elif angle <= -25 or angle >= 25:
                speed = 40
            else:
                speed = 50

        print("Red : " + red + "\nYellow : " + yellow + "\nGreen : " + green)

        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)
