"""Joonejärgija."""
from PiBot import PiBot
from math import pi
robot = PiBot()


speed = 13
sleep = 0.05
robot.set_grabber_height(100)
wheel_d = robot.WHEEL_DIAMETER
robot_d = robot.AXIS_LENGTH


"""
get_front_left_laser()
get_front_middle_laser()
get_front_right_laser()
get_front_lasers()
"""


def left():
    print("left")
    global speed
    """Robot spins/ turns left."""
    robot.set_left_wheel_speed(0)
    robot.set_right_wheel_speed(speed)
    robot.sleep(sleep)


def right():
    print("right")
    global speed
    """Robot spins/ turns right."""
    robot.set_right_wheel_speed(0)
    robot.set_left_wheel_speed(speed)
    robot.sleep(sleep)


def straight():
    print("straight")
    """Robot moves straight."""
    robot.set_wheels_speed(speed)


def find_encoder(wheel_d, robot_d):
    return 360 * ((pi * wheel_d) / (2 * pi * robot_d))


def find_closest():
    encoder = find_encoder()


def find_object():
    print("finding")
    """Robot spins until finds object."""
    while robot.get_front_middle_laser() > 0.1 and robot.get_front_left_laser() < 2 and robot.get_front_right_laser() < 2:
        print("doing stuff")
        left()


def moves_to_object():
    """Robot goes to object and corrects itself."""
    print("moving")
    while robot.get_front_left_laser() == robot.get_front_right_laser():
        straight()
    while robot.get_front_left_laser() < robot.get_front_right_laser():
        left()
    while robot.get_front_left_laser() > robot.get_front_right_laser():
        right()


while True:
    print("start")
    while robot.get_front_middle_laser() > 0.1 and robot.get_front_left_laser() < 2 and robot.get_front_right_laser() < 2:
        print("läks kuskile")
        moves_to_object()
    robot.set_wheels_speed(0)