from time import sleep
import turtle
import serial
import random
import math

ArduinoData = serial.Serial('/dev/cu.usbserial-1420', 9600)

sleep(5)

RANGE_OF_MOTION = 2
DISTANCE_PER_SECOND = 35

BICEP = 192
FOREARM = 146

WRIST_90 = 80

hypotenuse = 0
servo_angle = 0


def find_height(hypotenuse, angle):
    height = math.sin(math.radians(angle))*hypotenuse
    return height


def distance():
    global hypotenuse, servo_angle
    ArduinoData.write(b"distang")

    while hypotenuse == 0:
        try:
            hypotenuse = int(ArduinoData.readline())
        except ValueError:
            hypotenuse = 0

    servo_angle = int(ArduinoData.readline())-WRIST_90
    hypotenuse = hypotenuse

    print("Servo angled at:", servo_angle, "Distance to Object:", hypotenuse)

    distance = round(math.cos(math.radians(servo_angle))*hypotenuse)
    print("Lateral Distance:", distance)
    return distance


def grab_components():
    ArduinoData.write(b"grabComponents")
    elbow_angle = 0
    hypotenuse = 0
    servo_angle = 0

    while hypotenuse == 0:
        try:
            hypotenuse = int(ArduinoData.readline())*10
        except ValueError:
            hypotenuse = 0

    while servo_angle == 0:
        try:
            servo_angle = int(ArduinoData.readline())
        except ValueError:
            servo_angle = 0

    while elbow_angle == 0:
        try:
            elbow_angle = int(ArduinoData.readline())
        except ValueError:
            elbow_angle = 0

    original_shoulder = int(ArduinoData.readline())

    return hypotenuse+80, servo_angle, elbow_angle, original_shoulder


def wrist_angle(distance, height):
    return math.degrees(math.atan(height/distance))


def absolute(num):
    return ((num)**2)**0.5


def convert_to_3_digits(value):
    value = str(round(value))
    if len(value) == 1:
        value = '00' + value
    elif len(value) == 2:
        value = '0' + value

    return value


def target(coordinates):
    pass


def find(object):
    pass


def draw_quadrilateral(AB, BC, CD, AD, angle_A, angle_B, angle_C, angle_D):
    t = turtle.Turtle()
    turtle.Screen().bgcolor("black")
    t.hideturtle()
    turtle.colormode(255)
    t.pencolor((32, 161, 236))
    t.clear()
    t.pensize(2)
    shoulder_displacement = 90-(180-angle_D-(90-(angle_C-WRIST_90)))
    t.left(shoulder_displacement+angle_A)

    t.penup()
    t.goto(-100, -100)
    t.pendown()

    t.forward(AB)
    t.left(180-angle_B)
    t.forward(BC)
    t.left(180-angle_C)
    t.forward(CD)
    t.left(180-angle_D)
    t.forward(AD)

    t.penup()
    t.goto(-100, -100)
    t.pendown()


def grab():
    elbow_origin = 110
    shoulder_origin = 10

    AB = BICEP
    BC = FOREARM
    CD, angle_C, angle_B, original_shoulder = grab_components()
    original_elbow = angle_B
    print(CD, angle_C, angle_B, original_shoulder)

    angle_B_full = (0.511*angle_B)+75
    print(angle_B_full)
    angle_C_full = 80+angle_C

    angle_C = math.radians(angle_C_full)

    BD = math.sqrt(BC**2 + CD**2 - 2 * BC * CD * math.cos(angle_C))

    cos_C = (BC**2 + CD**2 - BD**2) / (2 * BC * CD)
    angle_C = math.degrees(math.acos(cos_C))

    sin_B = (BC * math.sin(math.radians(angle_C))) / BD
    angle_BDC = math.degrees(math.asin(sin_B))

    sin_D = (CD * math.sin(math.radians(angle_C))) / BD
    angle_CBD = math.degrees(math.asin(sin_D))

    angle_B = angle_B_full-angle_CBD

    AD = math.sqrt(AB**2 + BD**2 - 2 * AB * BD *
                   math.cos(math.radians(angle_B)))

    cos_D = (AD ** 2 + BD ** 2 - AB ** 2) / (2 * AD * BD)
    angle_D = math.degrees(math.acos(cos_D))

    angle_A = 180 - angle_B - angle_D

    angle_D = angle_D+angle_BDC

    AD = AD

    original_A = angle_A

    print("-"*100)
    print(f"Angle A = {angle_A:.2f} degrees")
    print(f"Angle B = {angle_B_full:.2f} degrees")
    print(f"Angle C = {angle_C_full:.2f} degrees")
    print(f"Angle D = {angle_D:.2f} degrees")

    print(f"The length of side AB is {AB:.2f}")
    print(f"The length of side BC is {BC:.2f}")
    print(f"The length of side CD is {CD:.2f}")
    print(f"The length of side AD is {AD:.2f}")
    print("-"*100)

    draw_quadrilateral(AB, BC, CD, AD, angle_A, angle_B, angle_C, angle_D)

    CD = 90
    # angle_D = 30

    AC = math.sqrt(CD**2 + AD**2 - 2 * CD * AD *
                   math.cos(math.radians(angle_D)))
    cos_ACD = (AD**2 + AC**2 - CD**2) / (2 * AD * AC)
    angle_CAD = math.degrees(math.acos(cos_ACD))
    angle_ACD = 180 - angle_CAD - angle_D

    cos_B = (AB ** 2 + BC ** 2 - AC ** 2) / (2 * AB * BC)
    angle_B = math.degrees(math.acos(cos_B))

    cos_C = (BC ** 2 + AC ** 2 - AB ** 2) / (2 * BC * AC)
    angle_C = math.degrees(math.acos(cos_C)) + angle_ACD
    angle_A = (180 - angle_B - angle_C)
    cos_A = (AB ** 2 + AC ** 2 - BC ** 2) / (2 * AB * AC)
    angle_A = math.degrees(math.acos(cos_A)) + angle_CAD

    shoulder_displacement = -1*(90-(180-angle_D-(90-(angle_C-WRIST_90))))

    print("-"*100)
    print(f"Angle A = {angle_A:.2f} degrees")
    print(f"Angle B = {angle_B:.2f} degrees")
    print(f"Angle C = {angle_C:.2f} degrees")
    print(f"Angle D = {angle_D:.2f} degrees")
    print(f"Shoulder Displacement = {shoulder_displacement:.2f} degrees")
    print(f"Shoulder from 90 = {shoulder_displacement+angle_A:.2f} degrees")

    print(f"The length of side AB is {AB:.2f}")
    print(f"The length of side BC is {BC:.2f}")
    print(f"The length of side CD is {CD:.2f}")
    print(f"The length of side AD is {AD:.2f}")
    print("-"*100)
    draw_quadrilateral(AB, BC, CD, AD, (180-angle_A),
                       angle_B, angle_C, angle_D)

    # elbow = absolute((angle_B-elbow_origin)*2.1)
    # elbow = (angle_B_full/angle_B)*original_elbow
    # Achieved through manual data regression
    elbow = 1.9*angle_B-140
    wrist = angle_C - 80
    # shoulder = absolute(180 - ((angle_A + shoulder_displacement)-shoulder_origin)*2.1)
    # shoulder = (original_A/angle_A)*original_shoulder
    shoulder = -0.4484848484848485 * (angle_A+shoulder_displacement) + 97.0

    ArduinoData.write(bytes(
        f"grab {convert_to_3_digits(shoulder)} {convert_to_3_digits(elbow)} {convert_to_3_digits(wrist)}", "utf-8"))


def go(direction, distance):
    global servo_angle, hypotenuse
    if direction == "forward":
        print("Going forward")
        height = find_height(hypotenuse, servo_angle)

        if distance > RANGE_OF_MOTION:
            send_distance = distance - RANGE_OF_MOTION
            send_time = send_distance/DISTANCE_PER_SECOND
            remaining_distance = distance-send_distance
            send_angle = wrist_angle(remaining_distance, height)+50
            print("Moving:", send_distance,
                  "\t\tDistance to Object:", remaining_distance)

            ArduinoData.write(bytes(f"forward {int(send_time*1000)}", 'utf-8'))
            sleep(1.5)
            ArduinoData.write(bytes(f"wrist_track {int(send_angle)}", "utf-8"))

    if direction == "backward":
        send_distance = distance - RANGE_OF_MOTION
        send_time = send_distance/DISTANCE_PER_SECOND
        ArduinoData.write(bytes(f"backward {int(send_time*1000)}", 'utf-8'))


# ArduinoData.write(bytes(f"wrist {int(random.randint(0, 180))}", 'utf-8'))
# sleep(5)
grab()
turtle.done()