"""Go to object O1."""


from PiBot import PiBot
import rospy


def find_objects():
    """Find objects."""
    furthest_object = 0
    object_count = 0

    while object_count < 4:
        if object_count < 2:
            robot.set_wheels_speed(0)
            object_count += 1
            rospy.sleep(0.05)
        if 0.7 > robot.get_front_middle_ir() > 0.1:
            object_count += 1
            if furthest_object < robot.get_front_middle_ir():
                furthest_object = robot.get_front_middle_ir()
                rospy.sleep(0.05)
            robot.set_right_wheel_speed(-16)
            robot.set_left_wheel_speed(16)
            rospy.sleep(0.2)

        robot.get_front_middle_ir()
        rospy.sleep(0.05)
        find_object()

    robot.get_front_middle_ir()
    while True:
        if robot.get_front_middle_ir() > (furthest_object - 0.03) and robot.get_front_middle_ir() <\
                (furthest_object + 0.03):
            break

        robot.get_front_middle_ir()
        robot.set_right_wheel_speed(-16)
        robot.set_left_wheel_speed(16)
        rospy.sleep(0.05)

    robot.set_wheels_speed(0)
    rospy.sleep(0.05)


def go_to_furthest(sleep=0.8):
    """Go to the furthest object."""
    if sleep <= 0:
        return

    find_objects()
    robot.set_wheels_speed(16)
    rospy.sleep(sleep)
    go_to_furthest(sleep - 0.1)


def find_object():
    """Find object."""
    while robot.get_front_middle_ir() > 0.7 or robot.get_front_middle_ir() < 0.1:
        robot.get_front_middle_ir()
        robot.set_right_wheel_speed(-16)
        robot.set_left_wheel_speed(16)
        rospy.sleep(0.05)


if __name__ == "__main__":
    robot = PiBot()
    go_to_furthest()