import serial
import math
from time import sleep

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

sleep(5)

RANGE_OF_MOTION = 10
DISTANCE_PER_SECOND = 35

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())-50
    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 wrist_angle(distance, height):
    return math.degrees(math.atan(height/distance))


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


def target(coordinates):
    pass

def find(object):
    pass

def grab():
    """To find all components of quadrilateral ABCD, given sides AB, BC, CD, and angles A, B, and C, we can use the following steps:

Find the length of side AD.
We can use the law of cosines to find the length of AD:

AD^2 = AB^2 + BC^2 - 2 * AB * BC * cos(C)
AD = sqrt(AB^2 + BC^2 - 2 * AB * BC * cos(C))
AD = sqrt(192^2 + 116^2 - 2 * 192 * 116 * cos(118))
AD = 154.7
Find the angles of triangle ABC.
We can use the law of sines to find the angles of triangle ABC:

sin(A) / BC = sin(B) / AC
sin(C) / AB = sin(A) / AC
AC = sin(A) * BC / sin(B)
AC = sin(100) * 116 / sin(95)
AC = 120.7
sin(C) / AC = sin(B) / AB
BC = sin(C) * AC / sin(B)
BC = sin(118) * 120.7 / sin(95)
BC = 126.8
Now that we know the lengths of all sides of triangle ABC, we can use the law of cosines to find the angles B and C:

cos(B) = (AC^2 + AB^2 - BC^2) / (2 * AC * AB)
B = acos((AC^2 + AB^2 - BC^2) / (2 * AC * AB))
B = 92.9°
cos(C) = (BC^2 + AB^2 - AC^2) / (2 * BC * AB)
C = acos((BC^2 + AB^2 - AC^2) / (2 * BC * AB))
C = 119°
Find the angle of triangle ADC.
The angle of triangle ADC is the sum of angles A and B, minus 180 degrees:

ADC = A + B - 180°
ADC = 100 + 92.9 - 180°
ADC = -11.1°
Now that we know all of the components of quadrilateral ABCD, we can keep decreasing side CD by 1 unit until it is left with 70, and find the angles A, B, and C every time we decrease 1 unit.

To do this, we can use the following steps:

Update the length of side CD.
Find the new length of side AD using the law of cosines.
Find the new angles of triangle ABC using the law of sines and law of cosines.
Find the new angle of triangle ADC by subtracting 180 degrees from the sum of angles A and B.
We can repeat these steps until CD is equal to 70."""
    pass


def go(direction, distance):
    global servo_angle, hypotenuse
    if direction == "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'))