import serial
import time
import numpy as np

def connect():
    ser = serial.Serial('/dev/ttyUSB0',38400,timeout=None)
    time.sleep(10)      # Nach dem Öffnen 10 Sekunden warten
    ser.write('connect\n'.encode())
    ser.readline().decode()
    return ser

def move_pose(ser,w,gripServo,duration):
    BASE_MIN = -2.6112
    BASE_MAX = 2.6061
    SHOULDER_MIN = -1.5708
    SHOULDER_MAX = 1.5708
    ELBOW_MIN = -2.0043
    ELBOW_MAX = 1.5555
    WRIST_MIN = -1.6218
    WRIST_MAX = 1.6779
    WROT_MIN = -2.6112
    WROT_MAX = 2.6061
    GRIP_MIN = 0
    GRIP_MAX = 511
    DURATION_MIN = 250
    DURATION_MAX = 65535
    
    ###################### Code-Teil von Shefqet Grdlay
    v = np.zeros(5)
    d = [111.6, 0, 0, 0, 155]
    a = [0, 148.22, 147, 0, 0]
    v[0] = np.arctan2(w[1], w[0])
    v234 = np.arctan2(-(np.cos(v[0]) * w[3] + np.sin(v[0]) * w[4]), -w[5])
    b1 = np.cos(v[0]) * w[0] + np.sin(v[0]) * w[1] + d[4] * np.sin(v234)
    b2 = d[0] - d[4] * np.cos(v234) - w[2]

    v[2] = np.arccos((np.square(np.linalg.norm([[b1], [b2]])) - np.square(a[1]) - np.square(a[2])) / (2 * a[1] * a[2]))
    v[1] = np.arctan2((a[1] + a[2] * np.cos(v[2])) * b2 - a[2] * np.sin(v[2]) * b1,
                      (a[1] + a[2] * np.cos(v[2])) * b1 + a[2] * np.sin(v[2]) * b2)

    v[3] = v234 - v[1] - v[2]
    v[4] = np.pi * np.log(np.sqrt(np.square(w[3]) + np.square(w[4]) + np.square(w[5])))
    v[1] = v[1] + 1.315014068517125
    v[2] = v[2] - 1.314998850007427
    v[3] = v[3] + 1.570781108285199

    ######################
    
    # Überprüfung der zulässigen Werte
    if v[0] < BASE_MIN:
        return "False"
    if v[0] > BASE_MAX:
        return "False"
    if v[1] < SHOULDER_MIN:
        return "False"
    if v[1] > SHOULDER_MAX:
        return "False"
    if v[2] < ELBOW_MIN:
        return "False"
    if v[2] > ELBOW_MAX:
        return "False"
    if v[3] < WRIST_MIN:
        return "False"
    if v[3] > WRIST_MAX:
        return "False"
    if v[4] < WROT_MIN:
        return "False"
    if v[4] > WROT_MAX:
        return "False"
    if gripServo < GRIP_MIN:
        return "False"
    if gripServo > GRIP_MAX:
        return "False"
    if duration < DURATION_MIN:
        return "False"
    if duration > DURATION_MAX:
        return "False"
    
    if np.any(np.isinf(v)) or np.any(np.isnan(v)):        # Probe auf Inf und NaN
        return "False"
    else:
        resp = set_joint_angles(ser,v[0],v[1],v[2],v[3],v[4],gripServo,duration)
        return resp

def set_joint_angles(ser,baseServo,shoulderServo,elbowServo,wristServo,wristRotServo,gripServo,duration):
    msg = (f"move {baseServo:.4f} {shoulderServo:.4f} {elbowServo:.4f} {wristServo:.4f} {wristRotServo:.4f} {int(gripServo)} {int(duration)}\n")
    ser.write(msg.encode())
    resp = ser.readline().decode()
    return resp

def get_joint_angles(ser):
    ser.write('getJointAngles\n'.encode())
    resp = list()
    for i in range(0,6):
        msg = ser.readline().decode()
        resp.append(msg)
    return resp

def sleep(ser):
    ser.write('sleep\n'.encode())

def disconnect(ser):
    ser.write('disconnect\n'.encode())
    ser.readline().decode()
    ser.close()