"""My fifth robott."""
import time
from PiBot import PiBot
robot = PiBot()

def go_straight():
    """Go straight."""
    counter = 0
    distance = robot.get_front_middle_laser()
    distance2 = robot.get_front_left_laser()
    distance3 = robot.get_front_right_laser()
    while True:
        if distance > 0.1 and counter < 40 and distance2 > 0.1 and distance3 > 0.1:
            robot.set_wheels_speed(12)
            robot.sleep(0.05)
            counter += 1
            distance = robot.get_front_middle_laser()
            distance2 = robot.get_front_left_laser()
            distance3 = robot.get_front_right_laser()
        else:
            robot.set_wheels_speed(0)
            break
    robot.set_wheels_speed(0)


def find_object():
    """Turn 360 degrees and get difference in sensors."""
    last_distance = robot.get_front_middle_laser()
    while True:
        distance = robot.get_front_middle_laser()
        if abs(last_distance - distance) > 0.7:
            robot.set_wheels_speed(0)
            print("heading to first object")
            go_straight()
            robot.sleep(0.05)
            break
        else:
            print("turning to find first object")
            robot.set_left_wheel_speed(-10)
            robot.set_right_wheel_speed(10)
        last_distance = distance
        robot.sleep(0.05)
    robot.set_wheels_speed(0)


def turn_for_a_few_seconds():
    """Turn."""
    print("start")
    start_time = time.time()
    while True:
        print("turn for 0.1 seconds")
        if abs(start_time - time.time()) < 0.5:
            robot.set_left_wheel_speed(-11)
            robot.set_right_wheel_speed(11)
        else:
            robot.set_wheels_speed(0)
            break
        robot.sleep(0.05)
    robot.set_wheels_speed(0)


def find_second_object():
    """Turn 360 degrees and get difference in sensors."""
    last_distance = robot.get_front_middle_laser()
    while True:
        distance = robot.get_front_middle_laser()
        if last_distance - distance > 0.4:  # 0.3
            robot.set_wheels_speed(0)
            print(f"i found the distance: {distance}")
            return round(distance)
            break
        else:
            print("turning to find second object")
            robot.set_left_wheel_speed(-11)
            robot.set_right_wheel_speed(11)
        last_distance = distance
        robot.sleep(0.05)
    robot.set_wheels_speed(0)


def go_in_between_objects():
    """Drive between objects."""
    desired_distance = (find_second_object() / 2) - 0.1
    counter = 0
    distance = robot.get_front_middle_laser()
    distance2 = robot.get_front_left_laser()
    distance3 = robot.get_front_right_laser()
    while True:
        if distance > desired_distance and counter < 40 and distance2 > desired_distance and distance3 > desired_distance:
            robot.set_wheels_speed(11)
            robot.sleep(0.05)
            counter += 1
            distance = robot.get_front_middle_laser()
            distance2 = robot.get_front_left_laser()
            distance3 = robot.get_front_right_laser()
        else:
            robot.set_wheels_speed(0)
            break
    robot.set_wheels_speed(0)
    start_rotation = robot.get_rotation()
    while True:
        print("turning 90 degrees")
        current_rotation = robot.get_rotation()
        if abs(start_rotation - current_rotation) < 90:
            robot.set_right_wheel_speed(11)
            robot.set_left_wheel_speed(-11)
        else:
            robot.set_wheels_speed(0)
            break
        robot.sleep(0.05)
    robot.set_wheels_speed(0)
    print("calculating triangle height")
    triangle_height = 0.5 * (((desired_distance * 2 + 0.35) ** 2) - ((desired_distance + 0.35) ** 2))
    distance = robot.get_front_middle_laser()
    distance2 = robot.get_front_left_laser()
    distance3 = robot.get_front_right_laser()
    counter = 0
    while True:
        print("driving until triangle height is reached")
        if distance > triangle_height and counter < 40 and distance2 > triangle_height and distance3 > triangle_height:
            robot.set_wheels_speed(11)
            robot.sleep(0.05)
            counter += 1
            distance = robot.get_front_middle_laser()
            distance2 = robot.get_front_left_laser()
            distance3 = robot.get_front_right_laser()
        else:
            robot.set_wheels_speed(0)
            break
    robot.sleep(0.05)
    robot.set_wheels_speed(0)


print("find_object")
find_object()
print("turn for a few seconds")
turn_for_a_few_seconds()
print("find second object and drive between")
go_in_between_objects()