"""POLARIS."""
from PiBot import PiBot
import rospy
robot = PiBot()


print("It's gonna be good...")
rospy.sleep(1)
middle_sensor = robot.get_front_middle_ir()
problem_solved = False
list_of_buffer = []
if robot.is_simulation():
    speed = 16
else:
    speed = 10


def to_filter_sensors():
    """Buffer."""
    global list_of_buffer
    if len(list_of_buffer) < 15:
        list_of_buffer.append(middle_sensor)
    elif len(list_of_buffer) == 15:
        list_of_buffer.pop(0)
        list_of_buffer.append(middle_sensor)
    return sum(list_of_buffer) / len(list_of_buffer)


def looking_for_some_juice_object():
    """Find object."""
    global middle_sensor
    global problem_solved
    previous_distance = middle_sensor
    diff = previous_distance - middle_sensor
    while diff < 0.5 and not problem_solved:
        print(round(to_filter_sensors(), 3))
        previous_distance = middle_sensor
        robot.set_right_wheel_speed(-speed)
        robot.set_left_wheel_speed(speed)
        rospy.sleep(0.05)
        middle_sensor = robot.get_front_middle_ir()
        diff = previous_distance - middle_sensor
        buffer = to_filter_sensors()
        dif_from_buff = middle_sensor - buffer
        if dif_from_buff < 0.02 and middle_sensor < 0.6:
            break
        elif middle_sensor < 0.3:
            break


def running_in_the_90s():
    """Go to this object."""
    print("Speed to max, cause I see object.")
    global middle_sensor
    global problem_solved
    buffer_average = to_filter_sensors()
    left_encoder_start = robot.get_left_wheel_encoder()
    right_encoder_start = robot.get_right_wheel_encoder()
    while buffer_average > 0.3 and not problem_solved:
        if middle_sensor > 0.9:
            rospy.sleep(0.1)
            looking_for_some_juice_object()
            left_encoder_start = robot.get_left_wheel_encoder()
            right_encoder_start = robot.get_right_wheel_encoder()
        else:
            print("Middle sensor: " + str(round(middle_sensor, 3)) + ", " + "Some filtered value: " + str(round(buffer_average, 3)))
            left_encoder = robot.get_left_wheel_encoder()
            right_encoder = robot.get_right_wheel_encoder()
            left_encoder_change = left_encoder - left_encoder_start
            right_encoder_change = right_encoder - right_encoder_start
            problem = right_encoder_change - left_encoder_change
            left_encoder_start = robot.get_left_wheel_encoder()
            right_encoder_start = robot.get_right_wheel_encoder()
            robot.set_left_wheel_speed(int(speed + round((problem / 2) / speed)))
            robot.set_right_wheel_speed(int(speed - round((problem / 2) / speed)))
            rospy.sleep(0.05)
            middle_sensor = robot.get_front_middle_ir()
            buffer_average = to_filter_sensors()
    rospy.sleep(0.05)


while True:
    while not problem_solved:
        if middle_sensor > 0.3 and not problem_solved:
            looking_for_some_juice_object()
            running_in_the_90s()
            average = to_filter_sensors()
        rospy.sleep(0.5)
        problem_solved = True
        print("Nothing personal kid, but I need to stop here...")
        robot.set_wheels_speed(0)