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

N = 5 # 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


# compute the two tangential points at the circle with center c and radius r intersecting the point p
def compute_tangent_points(p, c, r):
    b = sqrt((p[0] - c[0]) ** 2 + (p[1] - c[1]) ** 2)
    th = acos(r / b)  # angle theta
    d = atan2(p[1] - c[1], p[0] - c[0])  # direction angle of point p from c
    d1 = d + th  # direction angle of point T1 from c
    d2 = d - th  # direction angle of point T2 from c

    T1x = c[0] + r * cos(d1)
    T1y = c[1] + r * sin(d1)
    T2x = c[0] + r * cos(d2)
    T2y = c[1] + r * sin(d2)

    return (T1x, T1y), (T2x, T2y)

# 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')

coords_2 = []
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')


    # optimization problem for computing position and radius for a maximal circle fitting in space between two big circles
    # and being fully contained in enclosing circle
    opti = casadi.Opti()

    r = opti.variable(1)    # radius of new circle
    p = opti.variable(2)    # center of new circle
    lamb = opti.variable(1) # distance of center of new circle to center of enclosing circle

    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))

    coords_2.append(p)

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


# postprocessing solution:
# - output radii for circles
# - output center coordinates, angle w.r.t. origin and distance from origin
outer_radius = 0.15 # desired plate radius in meters
tube1_radius = outer_radius * rtilde
tube2_radius = outer_radius * r

print("\n------------------")
print("plate radius  = {:6.3} m = {:6.2f} mm".format(outer_radius, outer_radius * 1000))
print("big circles:")
print("  radius = {:6.3} m = {:6.2f} mm".format(tube1_radius, tube1_radius * 1000))
print("  diameter = {:6.3} m = {:6.2f} mm".format(2*tube1_radius, 2*tube1_radius * 1000))
print("  coordinates:")
for k in range(0,N):
    x = coords[k][0] * 1000
    y = coords[k][1] * 1000

    t1, t2 = compute_tangent_points((0,0),(x,y), rtilde * 1000)
    plt.plot(t1[0] / 1000, t1[1] / 1000, 'o')
    plt.plot(t2[0] / 1000, t2[1] / 1000, 'o')

    angle = arctan2(y,x) * 360.0 / (2.0 * math.pi)
    dist = (x**2 + y**2)**0.5

    angle_t1 = arctan2(t1[1], t1[0]) * 360.0 / (2.0 * math.pi)
    dist_t1 = (t1[0] ** 2 + t1[1] ** 2) ** 0.5

    angle_t2 = arctan2(t2[1], t2[0]) * 360.0 / (2.0 * math.pi)
    dist_t2 = (t2[0] ** 2 + t2[1] ** 2) ** 0.5
    print("   k = {},  (x,y) = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg,  dist = {:8.3f} mm".format(k, x, y, angle, dist))
    print("              t1 = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg,  dist = {:8.3f} mm".format(t1[0], t1[1], angle_t1,
                                                                                                 dist_t1))
    print("              t2 = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg,  dist = {:8.3f} mm".format(t2[0], t2[1], angle_t2,
                                                                                                 dist_t2))
print("\n")

print("small circles:")
print("  radius = {:6.3} m = {:6.2f} mm".format(tube2_radius, tube2_radius * 1000))
print("  diameter = {:6.3} m = {:6.2f} mm".format(2*tube2_radius, 2*tube2_radius * 1000))
print("  coordinates:")
for k in range(0,N):
    x = coords_2[k][0] * 1000
    y = coords_2[k][1] * 1000

    t1, t2 = compute_tangent_points((0, 0), (x, y), r * 1000)
    plt.plot(t1[0] / 1000, t1[1] / 1000, 'o')
    plt.plot(t2[0] / 1000, t2[1] / 1000, 'o')

    angle = arctan2(y, x) * 360.0 / (2.0 * math.pi)
    dist = (x ** 2 + y ** 2) ** 0.5

    angle_t1 = arctan2(t1[1], t1[0]) * 360.0 / (2.0 * math.pi)
    dist_t1 = (t1[0] ** 2 + t1[1] ** 2) ** 0.5

    angle_t2 = arctan2(t2[1], t2[0]) * 360.0 / (2.0 * math.pi)
    dist_t2 = (t2[0] ** 2 + t2[1] ** 2) ** 0.5
    print("   k = {},  (x,y) = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg,  dist = {:8.3f} mm".format(k, x, y, angle, dist))
    print("              t1 = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg,  dist = {:8.3f} mm".format(t1[0], t1[1], angle_t1,
                                                                                                 dist_t1))
    print("              t2 = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg,  dist = {:8.3f} mm".format(t2[0], t2[1], angle_t2,
                                                                                                 dist_t2))


pass