"""Objektid."""
from PiBot import PiBot
import math

robot = PiBot()

speed = 10
sleep = 0.05
robot.set_grabber_height(100)
buffer = 0.1

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


def anti_left():
    """Robot spins/ turns backwards left."""
    global speed
    robot.set_left_wheel_speed(speed)
    robot.set_right_wheel_speed(-speed)
    robot.sleep(sleep)


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


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


def solve_triangle(a, b, angle):
    """Find side and angle of current triangle."""
    radian_angle = math.radians(angle)
    side = math.sqrt(a ** 2 + b ** 2 + 2 * a * b * math.cos(radian_angle))
    return side


def position_finder(master_list):
    """Scan the list and find the objects."""
    print("alustan position finderit")
    list_of_objects = []
    biggest_change = master_list[0][0]
    last_biggest = []
    current_biggest = []
    for i in master_list:
        if i[0] > biggest_change:
            last_biggest = current_biggest
            biggest_change = i[0]
            current_biggest = i
    list_of_objects.append(current_biggest)
    list_of_objects.append(last_biggest)
    return list_of_objects


def make_master_list():
    """Spin around and make a list of, what u see."""
    global buffer
    b = robot.get_front_middle_laser()
    a = robot.get_front_middle_laser()
    list_of_everything = []
    while abs(robot.get_rotation()) < 360:
        if (a - b) < 0.3 and a < 0.7 and b < 0.7:
            xx = 360 - robot.get_rotation()
            print(f"Delta middle laser = {a - b}")
            print(f"Delta encoder = {xx}")
            print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
            list_of_everything.append([a - b, xx, b])
            a = b
            b = robot.get_front_middle_laser()
        left()
    print("ring sai täis")
    print(list_of_everything)
    robot.set_wheels_speed(0)
    return list_of_everything


def turn_back(two_object_list):
    """Turn back to the rotation, where the object is."""
    print("alustasin turn backi")
    place = two_object_list[0][1]
    while (360 - (robot.get_rotation())) < place:
        anti_left()
        print(f" rotation = {360 - robot.get_rotation()}")
    robot.set_wheels_speed(0)
    print("Asukoht lukus")


def encoder_finder(distance):
    """How many degrees needed to spin."""
    robot.close_grabber(0)
    a = robot.get_left_wheel_encoder()
    b = robot.WHEEL_DIAMETER
    c = 3.14 * b
    d = distance / c
    e = d * 360
    while robot.get_left_wheel_encoder() < a + e:
        straight()
    robot.set_wheels_speed(0)


def move_to_object(two_object_list):
    """Robot goes to object and corrects itself."""
    print("alustan move to object")
    distance = two_object_list[0][2]
    robot.close_grabber(0)
    encoder_finder(distance)
    # while robot.get_front_middle_laser() > 0.1:
    #     straight()
    #     print(robot.get_front_middle_laser())
    robot.set_wheels_speed(0)
    print("kohal!!!")


def turn_to_correct_angle(two_object_list):
    """When next to the object turn to the correct angle."""
    angle = two_object_list[0][1] - two_object_list[1][1]
    needed_angle = 60 - angle
    start_rotation = robot.get_rotation()
    while start_rotation + needed_angle < robot.get_rotation():
        anti_left()


def back_to_correct_place(two_object_list):
    """Back out to the correct location until laser sees same distance as length between two objects."""
    first_length = two_object_list[0][2]
    second_length = two_object_list[1][2]
    angle = two_object_list[0][1] - two_object_list[1][1]
    correct_length = solve_triangle(first_length, second_length, angle)
    encoder_finder(correct_length)


list_of_everything = make_master_list()
list_of_objects = position_finder(list_of_everything)

print(f"first object positions = {list_of_objects[0]}")
print(f"first object positions = {list_of_objects[1]}")
turn_back(list_of_objects)
move_to_object(list_of_objects)
turn_to_correct_angle(list_of_objects)
back_to_correct_place(list_of_objects)