import math
import cmath
import numpy as np
from math import cos as cos
from math import sin as sin
from math import atan2 as atan2
from math import acos as acos
from math import asin as asin
from math import sqrt as sqrt
from math import pi as pi
from numpy import linalg

#Globale Variablen
global mat
mat = np.matrix
global d1, a2, a3, a7, d4, d5, d6
d = mat([180.7, 0, 0, 174.15,119.85, 165.5]) #Ur10-Werte laut Hersteller
a = mat([0 ,-612.7 ,-571.55 ,0 ,0 ,0]) #Ur10-Werte laut Hersteller
alph = mat([math.pi/2, 0, 0, math.pi/2, -math.pi/2, 0 ])  #Ur10-Werte laut Hersteller in Radiant
#alph = mat([90, 0, 0, 90, -90, 0 ])  #Ur10-Werte laut Hersteller umgeschrieben in Degree
d1 = 180.7
a2 = -612.7
a3 = -571.55
d4 = 174.15
d5 = 119.85
d6 = 165.5
#a7 = 0.075 #brauch ich das? Anlehnung an UR5

# ************************************************** FORWARD KINEMATICS

def AH( n,th,c  ):
  #Translation I:  
  T_a = mat(np.identity(4), copy=False) #numpy.identity(n, dtype = None) : Gibt eine Identitätsmatrix zurück, d. h. eine quadratische Matrix mit Einsen auf der Hauptdiagonale.
  T_a[0,3] = a[0,n-1]
  #Translation II:
  T_d = mat(np.identity(4), copy=False) #numpy.identity(n, dtype = None) : Gibt eine Identitätsmatrix zurück, d. h. eine quadratische Matrix mit Einsen auf der Hauptdiagonale.
  T_d[2,3] = d[0,n-1]
  #homogene 4x4 Rz-Transformationsmatrix (Rotations I)
  Rzt = mat([[cos(th[n-1,c]), -sin(th[n-1,c]), 0 ,0],
	         [sin(th[n-1,c]),  cos(th[n-1,c]), 0, 0],
	         [0,               0,              1, 0],
	         [0,               0,              0, 1]],copy=False)
      
  #homogene 4x4 Rx-Transformationsmatrix (Rotations II)
  Rxa = mat([[1, 0,                 0,                  0],
			 [0, cos(alph[0,n-1]), -sin(alph[0,n-1]),   0],
			 [0, sin(alph[0,n-1]),  cos(alph[0,n-1]),   0],
			 [0, 0,                 0,                  1]],copy=False)

  A_i = T_d * Rzt * T_a * Rxa # allg. Form direkte DH-Transformation
    

  return A_i #A_i ist Platzhalter für A_1, A_2 usw.

def HTrans(th,c ):  #Multiplizierte DH-Transformation Matrix 5T6
  A_1=AH( 1,th,c  ) #A01-Matrix mit Thetawinkel 1 (Gelenk 0 zu Gelenk 1)
  A_2=AH( 2,th,c  ) #A12-Matrix mit Thetawinkel 2
  A_3=AH( 3,th,c  ) #A23-Matrix mit Thetawinkel 3
  A_4=AH( 4,th,c  ) #A34-Matrix mit Thetawinkel 4
  A_5=AH( 5,th,c  ) #A45-Matrix mit Thetawinkel 5
  A_6=AH( 6,th,c  ) #A56-Matrix mit Thetawinkel 6
      
  T_06=A_1*A_2*A_3*A_4*A_5*A_6 #Transformation 0 bis Gelenk 6

  return T_06

# ************************************************** INVERSE KINEMATICS 

def invKine(desired_pos):# T60 (Rückwärtstransformation Endeffektor zu Gelenk 0)
  th = mat(np.zeros((6, 8))) #erstelle 6x8 große Matrix mit Nullen
  P_05 = (desired_pos * mat([0,0, -d6, 1]).T-mat([0,0,0,1 ]).T) #.T (Transpornierung Punkt_05 Gelenk 5 zu Gelenk 0) ist ein Array der letzte Spalte der 4x4 matrix
  
  # **** theta1 ****
  
  psi = atan2(P_05[2-1,0], P_05[1-1,0]) #atan2 Funktion
  phi = acos(d4 /sqrt(P_05[2-1,0]*P_05[2-1,0] + P_05[1-1,0]*P_05[1-1,0])) #acos Funktion
  #The two solutions for theta1 correspond to the shoulder
  #being either left or right
  th[0, 0:4] = pi/2 + psi + phi #in Radiant!
  th[0, 4:8] = pi/2 + psi - phi
  th = th.real
  
  # **** theta5 ****
  
  cl = [0, 4]# wrist up or down
  for i in range(0,len(cl)):
	      c = cl[i]
	      T_10 = linalg.inv(AH(1,th,c)) #Transformation Gelenk 1 zu Gelenk 0
	      T_16 = T_10 * desired_pos #Transformation Endeffektor zu Gelenk 1
	      th[4, c:c+2] = + acos((T_16[2,3]-d4)/d6);
	      th[4, c+2:c+4] = - acos((T_16[2,3]-d4)/d6);

  th = th.real
  
  # **** theta6 ****
  # theta6 is not well-defined when sin(theta5) = 0 or when T16(1,3), T16(2,3) = 0.

  cl = [0, 2, 4, 6] #Liste
  for i in range(0,len(cl)):
	      c = cl[i]
	      T_10 = linalg.inv(AH(1,th,c)) #Transformation Gelenk 1 zu Gelenk 0
	      T_16 = linalg.inv( T_10 * desired_pos ) #Transformation Endeffektor zu Gelenk 1
	      th[5, c:c+2] = atan2((-T_16[1,2]/sin(th[4, c])),(T_16[0,2]/sin(th[4, c])))
		  
  th = th.real

  # **** theta3 ****
  cl = [0, 2, 4, 6]
  for i in range(0,len(cl)):
	      c = cl[i]
	      T_10 = linalg.inv(AH(1,th,c))
	      T_65 = AH( 6,th,c)
	      T_54 = AH( 5,th,c)
	      T_14 = ( T_10 * desired_pos) * linalg.inv(T_54 * T_65)
	      P_13 = T_14 * mat([0, -d4, 0, 1]).T - mat([0,0,0,1]).T
	      t3 = cmath.acos((linalg.norm(P_13)**2 - a2**2 - a3**2 )/(2 * a2 * a3)) # norm ?
	      th[2, c] = t3.real
	      th[2, c+1] = -t3.real

  # **** theta2 and theta 4 ****

  cl = [0, 1, 2, 3, 4, 5, 6, 7]
  for i in range(0,len(cl)):
	      c = cl[i]
	      T_10 = linalg.inv(AH( 1,th,c ))
	      T_65 = linalg.inv(AH( 6,th,c))
	      T_54 = linalg.inv(AH( 5,th,c))
	      T_14 = (T_10 * desired_pos) * T_65 * T_54
	      P_13 = T_14 * mat([0, -d4, 0, 1]).T - mat([0,0,0,1]).T
	      
	      # theta 2
	      th[1, c] = -atan2(P_13[1], -P_13[0]) + asin(a3* sin(th[2,c])/linalg.norm(P_13))
	      # theta 4
	      T_32 = linalg.inv(AH( 3,th,c))
	      T_21 = linalg.inv(AH( 2,th,c))
	      T_34 = T_32 * T_21 * T_14
	      th[3, c] = atan2(T_34[1,0], T_34[0,0])
  th = th.real

  return th