import math

d2 = 12.9
l1 = 85.0
l2 = 110.0

t1_deg = 474 / 2048.0 * 360.0
t2_deg = 530 / 2048.0 * 360.0

th1 = math.radians(t1_deg)
th2 = math.radians(t2_deg)

e1x = -d2 + l1 * math.cos(th1)
e1y = l1 * math.sin(th1)

e2x = d2 + l1 * math.cos(th2)
e2y = l1 * math.sin(th2)

dx = e2x - e1x
dy = e2y - e1y
dist = math.sqrt(dx*dx + dy*dy)

a = dist / 2
h = math.sqrt(l2*l2 - a*a)

mx = (e1x + e2x) / 2
my = (e1y + e2y) / 2

px1 = mx + h * (dy / dist)
py1 = my + h * (-dx / dist)

px2 = mx - h * (dy / dist)
py2 = my - h * (-dx / dist)

ex = px1 if py1 >= py2 else px2
ey = py1 if py1 >= py2 else py2

with open("output.txt", "w") as f:
    f.write(f"End Effector: {ex:.1f}, {ey:.1f}\n")
    f.write(f"Elbow 1: {e1x:.1f}, {e1y:.1f}\n")
    f.write(f"Elbow 2: {e2x:.1f}, {e2y:.1f}\n")