from sympy import solve_poly_system
from sympy import Symbol
from sympy import Derivative
from sympy import diff

r1__1_1 = Symbol('r1__1_1')
r1__1_2 = Symbol('r1__1_2')
r1__2_1 = Symbol('r1__2_1')
r1__2_2 = Symbol('r1__2_2')
t11 = Symbol('t11')
t12 = Symbol('t12')
b1__1_1 = Symbol('b1__1_1')
b1__1_2 = Symbol('b1__1_2')
b1__2_2 = Symbol('b1__2_2')


g1_1_1=r1__1_1**2+r1__1_2**2-1
g1_1_2=r1__1_1*r1__2_1+r1__1_2*r1__2_2
g1_2_2=r1__2_1**2+r1__2_2**2-1
sum_sqr_distances=(27.0754884973587*r1__1_1+5.43898084995351*r1__1_2+t11-2.44419085235647)**2+(27.0754884973587*r1__2_1+5.43898084995351*r1__2_2+t12-3.99741956299719)**2+(30.9948792168917*r1__1_1+9.17425463683924*r1__1_2+t11-2.40613020603636)**2+(30.9948792168917*r1__2_1+9.17425463683924*r1__2_2+t12+1.41667733884696)**2+(32.9492412637827*r1__1_1+2.43365042139265*r1__1_2+t11+3.79884332684437)**2+(32.9492412637827*r1__2_1+2.43365042139265*r1__2_2+t12-1.86258777027033)**2+(32.4175892802701*r1__1_1+6.90025112689204*r1__1_2+t11+.217706363614488)**2+(32.4175892802701*r1__2_1+6.90025112689204*r1__2_2+t12+.859292782679988)**2+(32.1924450222795*r1__1_1+10.990926652328*r1__1_2+t11-2.88011222250676)**2+(32.1924450222795*r1__2_1+10.990926652328*r1__2_2+t12+3.54030680703508)**2+(34.3505699538533*r1__1_1+7.786759826202*r1__1_2+t11+.923638891795672)**2+(34.3505699538533*r1__2_1+7.786759826202*r1__2_2+t12+2.86527663706515)**2+(33.0354067310853*r1__1_1+2.7454240937639*r1__1_2+t11+3.63454881779986)**2+(33.0354067310853*r1__2_1+2.7454240937639*r1__2_2+t12-1.58395786369682)**2+(30.0192780747077*r1__1_1+9.48570872019747*r1__1_2+t11-3.30809201654554)**2+(30.0192780747077*r1__2_1+9.48570872019747*r1__2_2+t12+.931634941675125)**2
h=sum_sqr_distances+g1_1_1*b1__1_1+g1_1_2*b1__1_2+g1_2_2*b1__2_2
diff_t11 = diff(h, t11)
diff_t12 = diff(h,t12)
diff_r1__1_1=diff(h,r1__1_1)
diff_r1__2_1=diff(h,r1__2_1)
diff_r1__1_2=diff(h,r1__1_2)
diff_r1__2_2=diff(h,r1__2_2)
print('Trying to solve the equations. Please wait...')
sols = solve_poly_system([g1_1_1, g1_1_2, g1_2_2, diff_t11, diff_t12, diff_r1__1_1, diff_r1__2_1, diff_r1__1_2, diff_r1__2_2], r1__1_1, r1__1_2, r1__2_1, r1__2_2, t11, t12, b1__1_1, b1__1_2, b1__2_2)
print('Finished solving the equations!')
print(sols)