""""Robot must find object."""

from PiBot import PiBot
import rospy

robot = PiBot()
if robot.is_simulation():
    speed = 15
else:
    speed = 14
front_sensor = robot.get_front_middle_ir()
front_left_sensor = robot.get_front_left_ir()
front_right_sensor = robot.get_front_right_ir()
start_timestamp = rospy.get_time()

def find_object():
    turning = True
    dict_one = {}
    while turning is True:
        robot.get_right_wheel_encoder()
        robot.get_front_middle_ir()
        robot.set_right_wheel_speed(speed)
        robot.set_left_wheel_speed(-speed)
        print(robot.get_front_middle_ir())
        if len(str(robot.get_front_middle_ir())) > 6:
            dict_one[robot.get_right_wheel_encoder()] = robot.get_front_middle_ir()
        if robot.get_right_wheel_encoder() > 2016:
            robot.set_wheels_speed(0)
            turning = False
    rospy.sleep(1)
    a = min(dict_one, key=lambda x: dict_one.get(x))
    turning1 = True
    while turning1 is True:
        robot.get_front_middle_ir()
        robot.get_right_wheel_encoder()
        robot.set_right_wheel_speed(speed)
        robot.set_left_wheel_speed(-speed)
        print(robot.get_front_middle_ir())
        if robot.get_right_wheel_encoder() >= a + 1895:
            turning1 = False
            robot.set_wheels_speed(0)
    rospy.sleep(1)


# def buff():
#     """Buffer."""
#     global buffer
#     if len(buffer) < 15:
#         buffer.append(front_sensor)
#     elif len(buffer) == 15:
#         buffer.pop(0)
#         buffer.append(front_sensor)
#     return sum(buffer) / len(buffer)

    # global front_sensor
    # global finish
    # average = buff()
    # left_encoder_0 = robot.get_left_wheel_encoder()
    # right_encoder_0 = robot.get_right_wheel_encoder()
    # while average > 0.3 and not finish:
    #     if front_sensor > 0.9:
    #         rospy.sleep(0.1)
    #         find_object()
    #         left_encoder_0 = robot.get_left_wheel_encoder()
    #         right_encoder_0 = robot.get_right_wheel_encoder()
    #     print("Front sensor:" + str(round(front_sensor, 3)) + ", " + "Average: " + str(round(average, 3)))
    #     left_encoder = robot.get_left_wheel_encoder()
    #     right_encoder = robot.get_right_wheel_encoder()
    #     left_encoder_change = left_encoder - left_encoder_0
    #     right_encoder_change = right_encoder - right_encoder_0
    #     error = right_encoder_change - left_encoder_change
    #     left_encoder_0 = robot.get_left_wheel_encoder()
    #     right_encoder_0 = robot.get_right_wheel_encoder()
    #     robot.set_left_wheel_speed(int(speed + round((error / 2) / speed)))
    #     robot.set_right_wheel_speed(int(speed - round((error / 2) / speed)))
    #     rospy.sleep(0.05)
    #     front_sensor = robot.get_front_middle_ir()
    #     average = buff()
    # rospy.sleep(0.05)

def drive_to_the_object():
    drive = True
    print("Robot ljaheb objekti juurde.")
    print(robot.get_front_middle_ir())
    rospy.sleep(1)
    while robot.get_front_middle_ir() > 0.17:
        robot.get_front_middle_ir()
        while drive is True:
            print('Driving')
            robot.get_front_middle_ir()
            robot.set_wheels_speed(speed)
            print(robot.get_front_middle_ir())
            if len(str(robot.get_front_middle_ir())) > 6:
                if round(robot.get_front_middle_ir(), 3) < 0.16:
                    drive = False
                    robot.set_wheels_speed(0)
                    print('Stop here')
                    break

def main():
    find_object()
    drive_to_the_object()


if __name__ == '__main__':
    main()