from Python.robolink.robolink import *
from Python.robodk import *
import numpy as np
import math
sim = Robolink()
robot = sim.Item('H-Bot')
def jointValue():
    jointMatrix = robot.Joints()
    jointArray = []
    for x in jointMatrix:
        for y in x:
            jointArray.append(y)
    return jointArray
dhtable = [[0, -90, 285, "theta"],
     [700, 0, 0, "theta"],
     [0, -90, 0, "theta"],
     [0, 90, 650, "theta"],
     [0, -90, 0, "theta"],
     [0, 0, 170, "theta"]]
joints = jointValue()
for x in range(0, len(dhtable)):
    dhtable[x][3] = (joints[x]*math.pi)/180
def aMatrices(table):
    A = []
    for y in range (0,len(table)):
        a = table[y][0]
        alpha = math.radians(table[y][1])
        d = table[y][2]
        theta = math.radians(table[y][3])
        test = np.array([[math.cos(theta), -(math.cos(alpha)*math.sin(theta)), math.sin(alpha)*math.sin(theta), a*math.cos(theta)],
                  [math.sin(theta), math.cos(alpha)*math.cos(theta), -(math.sin(alpha)*math.cos(theta)), a*math.sin(theta)],
                  [0, math.sin(alpha), math.cos(alpha), d],
                  [0, 0, 0, 1 ]])
        A.append(test)

    return A
def A03(a):
    af = np.matmul(a[0], a[1])
    A03 = np.matmul(af, a[2])
    return A03
def A36(a):
    A36 = np.matmul(np.matmul(a[3], a[4]), a[5])
    return A36
def hMatrix(a):
    H = np.matmul(a[0], a[1])
    for x in range (2,len(a)):
        H = np.matmul(H, a[x])
    return H

def inversePos():
    links = robot.JointPoses()
    o6 = links[6]
    d6 = 170
    x = o6[0,3]-d6*o6[0,2]
    y = o6[1,3]-d6*o6[1,2]
    z = o6[2,3]-d6*o6[2,2]
    a2 = 650
    a3 = 700
    d = 0
    print("sadsadasda", o6[0,3], o6[1,3], o6[2,3])
    print("daibfackvxonwe0porjpflkn", x, y, z)
    Dd = (math.pow(x,2)+math.pow(y,2)-math.pow(d,2)+math.pow(z-285,2)-math.pow(a2,2)-math.pow(a3,2))/(2*a2*a3)
    theta1 = math.atan2(y, x)
    theta11 = math.pi+theta1
    theta3 = math.atan2((1-(Dd*Dd)), Dd )
    print("Dd: ", Dd)
    theta33 = math.atan2((-1-(Dd*Dd)), Dd)
    theta2 = math.atan2((z-285),math.sqrt(math.pow(x,2)+math.pow(y,2)-math.pow(d,2)))-math.atan2(a3*math.sin(theta3),a2+(a3*math.cos(theta3)))
    theta22 = math.atan2(z-285,math.sqrt(math.pow(x,2)+math.pow(y,2)-math.pow(d,2)))-math.atan2(a3*math.sin(theta33),a2+a3*math.cos(theta33))
    theta1 = np.degrees(theta1)
    theta2 = np.degrees(theta2)
    theta3 = np.degrees(theta3)
    if theta11 > 180:
        theta11 = -180+theta1
    dhtable[0][3] = theta1
    dhtable[1][3] = theta2
    dhtable[2][3] = theta3
    print("Theta 1: ", theta1, " Theta 2: ", theta2, " Theta 3: ", theta3)
    print("Invers theta A03: \n", A03(aMatrices(dhtable)))
print("H matrix:  \n", hMatrix(aMatrices(dhtable)))
inversePos()