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():
    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'))