from casadi import *
import matplotlib.pyplot as plt

file = open('tmp2019_cci_08_26_11_36_00.txt')

c = (0.0, 0.0)  # center of big circle
R = 15.0        # radius of big circle

# center for smaller circles
p1 = (-5.55287862,  -7.64288174)
p2 = (5.55287862,  -7.64288174)
p3 = (-8.98474635,   2.91932105)
p4 = (8.98474635,   2.91932105)
p5 = (0.00000000,   9.44712138)
rtilde = 5.5 # radius of smaller circles

points = [p1, p2, p3, p4, p5]

# midpoint between center of two circles
m = np.mean([p1, p2], axis=0)

# vector in direction of midpoint
v = m - np.array(c)
v = v/np.linalg.norm(v)

plt.xlim((-16, 16))
plt.ylim((-16, 16))
plt.gca().set_aspect('equal', 'box')


for p in points:
    plt.plot(p[0], p[1], 'o')
    circle = plt.Circle(p, rtilde, fill=False)
    plt.gca().add_artist(circle)
circle = plt.Circle(c, R, fill=False)
plt.gca().add_artist(circle)
#plt.show()

plt.plot(c[0], c[1], 'o')
plt.plot(m[0], m[1], 'o')

opti = casadi.Opti()

r = opti.variable(1)    # radius of new circle
p = opti.variable(2)    # center of new circle
#v1 = opti.variable(2)   # direction vector from
#v2 = opti.variable(2)
lamb = opti.variable(1)

opti.minimize(-r)
opti.subject_to(p == c + v * lamb)
opti.subject_to((p[0] - p1[0])**2 + (p[1] - p1[1])**2 >= (rtilde + r)**2)
#opti.subject_to(p == (rtilde + r) * v1 + p1)
#opti.subject_to(p == (rtilde + r) * v2 + p2)
#opti.subject_to((v1[0]**2+v1[1]**2)**0.5 == 1)
#opti.subject_to((v2[0]**2+v2[1]**2)**0.5 == 1)
opti.subject_to(R == lamb + r)
opti.subject_to(r >= 0)
opti.subject_to(r <= R)
#opti.subject_to(lamb >= 0.6 * R)

opti.solver('ipopt')

init_r = 0.1
init_lamb = R - init_r
init_p = c + v * init_lamb


opti.set_initial(r, init_r)
opti.set_initial(p, init_p)
opti.set_initial(lamb, init_lamb)
#opti.set_initial(v1, )

sol = opti.solve()

p = sol.value(p)
r = sol.value(r)
lamb = sol.value(lamb)

print("p = {}".format(p))
print("r = {}".format(r))
print("lambda = {}".format(lamb))
print("v = {}".format(v))

plt.plot(p[0], p[1], 'o')
circle = plt.Circle(p, r, fill=False)
plt.gca().add_artist(circle)
plt.show()

pass