from PiBot import PiBot
import rospy
robot = PiBot()


def follow_line():
    """Line follower."""
    turn_counter = 0
    while True:
        robot.get_third_line_sensor_from_right()
        robot.get_third_line_sensor_from_left()
        robot.get_leftmost_line_sensor()
        robot.get_rightmost_line_sensor()
        if robot.get_rightmost_line_sensor() < 200 and robot.get_leftmost_line_sensor() < 200:
            turn(turn_counter)
            turn_counter += 1
        if robot.get_third_line_sensor_from_left() == 900 and robot.get_third_line_sensor_from_right() == 900:
            robot.set_wheels_speed(17)
            rospy.sleep(0.05)
            robot.get_third_line_sensor_from_right()
            robot.get_third_line_sensor_from_left()
        elif robot.get_third_line_sensor_from_left() > 200 and robot.get_third_line_sensor_from_right() < 200:
            robot.set_left_wheel_speed(-17)
            robot.set_right_wheel_speed(0)
            rospy.sleep(0.05)
            robot.get_third_line_sensor_from_right()
            robot.get_third_line_sensor_from_left()
            robot.get_leftmost_line_sensor()
            robot.get_rightmost_line_sensor()
        elif robot.get_third_line_sensor_from_right() > 200 and robot.get_third_line_sensor_from_left() < 200:
            robot.set_right_wheel_speed(-17)
            robot.set_left_wheel_speed(0)
            rospy.sleep(0.05)
            robot.get_third_line_sensor_from_right()
            robot.get_third_line_sensor_from_left()
            robot.get_leftmost_line_sensor()
            robot.get_rightmost_line_sensor()
        elif robot.get_third_line_sensor_from_right() < 150 and robot.get_third_line_sensor_from_left() < 150:
            robot.set_wheels_speed(-17)
            rospy.sleep(0.05)
            robot.get_third_line_sensor_from_right()
            robot.get_third_line_sensor_from_left()
            robot.get_leftmost_line_sensor()
            robot.get_rightmost_line_sensor()


def turn(value: int):
    """Turn robot."""
    if value % 3 == 2:
        robot.set_right_wheel_speed(-20)
        robot.set_left_wheel_speed(15)
        rospy.sleep(0.7)  # 3.14 * robot.AXIS_LENGTH / 2
    elif value % 3 == 0:
        robot.set_right_wheel_speed(15)
        robot.set_left_wheel_speed(-20)
        rospy.sleep(0.7)  # 3.14 * robot.AXIS_LENGTH / 2
    elif value % 3 == 1:
        robot.set_right_wheel_speed(-15)
        robot.set_left_wheel_speed(-15)
        rospy.sleep(0.4)  # 3.14 * robot.AXIS_LENGTH / 2
    else:
        pass


follow_line()