from casadi import *
import matplotlib.pyplot as plt
import math
import operator

N = 7 # number of enclosed circles

# this function reads and processes data for optimal circle packaging obtained form packomania.com
def read_circle_data(N):
    coords_raw = open('cci/cci{}.txt'.format(N))
    radii_raw = open('cci/radii.txt'.format(N))

    coords_raw = coords_raw.readlines()
    coords_raw = [c.split() for c in coords_raw if c[0] != '#']
    coords = {}
    for c in coords_raw:
        coords[int(c[0])] = (float(c[1]), float(c[2]))

    coords = sort_ccw(coords, (0,0))

    radii_raw = radii_raw.readlines()
    radii_raw = [r.split() for r in radii_raw if r[0] != '#']
    radii = {}
    for r in radii_raw:
        radii[int(r[0])] = float(r[1])

    return radii[N], coords

# this function sorts enclosed circle coordinates counter-clockwise w.r.t. the center point
# TODO: there is a problem when circles are present that are not touching the boundary of the enclosing circle (e.g. N = 7)
def sort_ccw(coords, center):
    a = {}
    for c in coords:
        a[c] = math.atan2(coords[c][1] - center[1], coords[c][0] - center[0])
    a_sort = sorted(a.items(), key=operator.itemgetter(1))

    coords_sort = []
    for a in a_sort:
        coords_sort.append(coords[a[0]])

    return coords_sort

# read radius and center coordinates for enclosed circles
rtilde, coords = read_circle_data(N)

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

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

plt.ion()
plt.show()

for p in coords:
    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.plot(c[0], c[1], 'o')

for k in range(0, N):
    p1 = coords[k]
    p2 = coords[(k+1) % N]

    # 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.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
    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(R == lamb + r)
    opti.subject_to(r >= 0)
    opti.subject_to(r <= 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)

    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)

pass