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

robot = PiBot()

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


def smooth_straight(angular_speed):
    """Robot moves straight and tries to maintain given angular speed"""
    start_time = time.time()
    start_e_right = robot.get_right_wheel_encoder()
    start_e_left = robot.get_left_wheel_encoder()

    time_from_start = 0

    power = 13
    increment = 1
    robot.set_wheels_speed(power)

    while time_from_start < 10:
        delta_time = time.time() - start_time - time_from_start
        time_from_start = time.time() - start_time

        degrees_from_start_r = robot.get_right_wheel_encoder() - start_e_right
        degrees_from_start_l = robot.get_left_wheel_encoder() - start_e_left

        speed_right = degrees_from_start_r / time_from_start
        speed_left = degrees_from_start_l / time_from_start

        if speed_right > angular_speed:
            robot.set_right_wheel_speed(power - increment)
        if speed_right < angular_speed:
            robot.set_right_wheel_speed(power + increment)

        if speed_left > angular_speed:
            robot.set_left_wheel_speed(power - increment)
        if speed_left < angular_speed:
            robot.set_left_wheel_speed(power + increment)
        # robot.sleep(sleep)
        print(f"time_from_start = {time_from_start}")
        print(f"delta_time = {delta_time}")
        print(f"speed_right = {speed_right}")
        print(f"speed_left = {speed_left}")
        print(f"degrees_from_start_r = {degrees_from_start_r}")
        print(f"degrees_from_start_l = {degrees_from_start_l}")
        print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")


def smooth_left(angular_speed):
    """Robot turns left and tries to maintain given angular speed"""
    time_from_start = 0
    power = 13
    increment = 5
    start_time = time.time()
    start_rotation = robot.get_rotation()
    robot.set_right_wheel_speed(power)
    robot.set_left_wheel_speed(-power)

    while time_from_start < 10:
        delta_time = time.time() - start_time - time_from_start
        time_from_start = time.time() - start_time
        rotation_from_start = robot.get_rotation() - start_rotation

        rotation_speed = rotation_from_start / time_from_start

        if rotation_speed > angular_speed:
            robot.set_right_wheel_speed(power - increment)
            robot.set_left_wheel_speed(-power + increment)
        if rotation_speed < angular_speed:
            robot.set_right_wheel_speed(power + increment)
            robot.set_left_wheel_speed(-power - increment)

        # robot.sleep(sleep)
        print(f"time_from_start = {time_from_start}")
        print(f"delta_time = {delta_time}")
        print(f"rotation_speed = {rotation_speed}")
        print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")


def smooth_left2(angular_speed):
    """Robot moves straight and tries to maintain given angular speed"""
    start_time = time.time()
    start_e_right = robot.get_right_wheel_encoder()
    start_e_left = robot.get_left_wheel_encoder()

    time_from_start = 0
    delta_time = 0

    power = 13
    robot.set_right_wheel_speed(power)
    robot.set_left_wheel_speed(-power)

    while time_from_start < 10:
        time_from_start = time.time() - start_time

        degrees_from_start_r = robot.get_right_wheel_encoder() - start_e_right
        degrees_from_start_l = robot.get_left_wheel_encoder() - start_e_left

        speed_right = degrees_from_start_r / time_from_start
        speed_left = degrees_from_start_l / time_from_start

        if speed_right > angular_speed:
            robot.set_right_wheel_speed(power - 2)
        if speed_right < angular_speed:
            robot.set_right_wheel_speed(power + 2)

        if speed_left > angular_speed:
            robot.set_left_wheel_speed(-power + 2)
        if speed_left < angular_speed:
            robot.set_left_wheel_speed(-power - 2)
        robot.sleep(sleep)
        print(f"time_from_start = {time_from_start}")
        print(f"delta_time = {delta_time}")
        print(f"speed_right = {speed_right}")
        print(f"speed_left = {speed_left}")
        print(f"degrees_from_start_r = {degrees_from_start_r}")
        print(f"degrees_from_start_l = {degrees_from_start_l}")
        print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")


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."""
    side = math.sqrt(a ** 2 + b ** 2 + 2 * a * b * math.cos(angle))
    return side


def position_finder(master_list):
    """Scan the list and find the objects."""
    print("alustan position finderit")
    global buffer
    two_objects_list = []
    for i in master_list:
        if len(two_objects_list) == 2:
            two_objects_list.sort(key=lambda x: x[1])
            return two_objects_list
        elif i[0] > buffer:
            two_objects_list.append(i)


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:
        xx = 360 - robot.get_rotation()
        print(f"Delta middle laser = {abs(a - b)}")
        print(f"Delta encoder = {xx}")
        print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
        left()
        list_of_everything.append([abs(a - b), xx, b])
        a = b
        b = robot.get_front_middle_laser()
    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)

# smooth_straight(180)
# smooth_left(130)