""""Robot finds the object and drives near it."""
from PiBot import PiBot
import rospy
robot = PiBot()
robot.set_grabber_height(100)
m = robot.get_front_middle_ir()

def find_object(r, l):
    """Arka."""
    m = robot.get_front_middle_ir()
    if r == l:
        if robot.is_simulation():
            # Running in simulationыыыыыы
            ran = test(15, 15)
        else:
            # Running in real life
            ran = test(11, 11)
    else:
        ran = (r, l)
    a = []
    while m > 0.5:
        r = robot.get_front_middle_ir()
        while r > 0.5:
            robot.set_right_wheel_speed(-ran[0])
            robot.set_left_wheel_speed(ran[1])
            r = robot.get_front_middle_ir()
        print(r)
        robot.set_wheels_speed(0)
        for i in range(3):
            rospy.sleep(0.05)
            a.append(robot.get_front_middle_ir())
        print(a)
        b = sum(a) / len(a)
        dif = 10000
        if r < 0.5:
            for i in a:
                dif1 = abs(i - b)
                if dif1 < dif:
                    dif = dif1
                    m = i
        a.clear()
    robot.set_wheels_speed(0)
    return ran


def go_to_object(r, l):
    """Arka."""
    a = []
    ran = find_object(r, l)
    r = ran[0]
    le = ran[1]
    m = robot.get_front_middle_ir()
    robot.set_wheels_speed(0)
    while 0.52 >= m >= 0.17:
        for i in range(3):
            robot.set_left_wheel_speed(le)
            robot.set_right_wheel_speed(r)
            rospy.sleep(0.05)
            a.append(robot.get_front_middle_ir())
        b = sum(a) / len(a)
        dif = 10000
        for i in a:
            dif1 = abs(i - b)
            if dif1 < dif:
                dif = dif1
                m = i
        print(m)
        a.clear()
    if m > 0.5:
        robot.set_wheels_speed(0)
        go_to_object(r, le)
    robot.set_wheels_speed(0)
    print("Konec")


def test(l_speed, r_speed):
    """Arka."""
    print("test")
    robot.set_right_wheel_speed(r_speed)
    robot.set_left_wheel_speed(l_speed)
    r = robot.get_right_wheel_encoder()
    lin = robot.get_left_wheel_encoder()
    rospy.sleep(0.05)
    robot.set_wheels_speed(0)
    rospy.sleep(0.05)
    l1 = robot.get_left_wheel_encoder()
    r1 = robot.get_right_wheel_encoder()
    difr = r1 - r
    difl = l1 - lin
    print(difr, difl)
    if difl > difr and max(difr, difl) - min(difr, difl) > 5:
        r_speed += 1
        return test(l_speed, r_speed)
    elif difr > difl and max(difr, difl) - min(difr, difl) > 5:
        l_speed += 1
        return test(l_speed, r_speed)
    else:
        print("======")
        print(r_speed, l_speed)
        a = [r_speed, l_speed]
        return a


print(go_to_object(12, 12))