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


def svg_circle(id, name, c, r):
    # create circle object in svg notation
    text = ['      <circle\n',
            '       id="circle{}"\n'.format(id),
            '       inkscape:label="{}"\n'.format(name),
            '       style="fill:none;stroke:#000000;stroke-width:1.0mm"\n',
            '       r="{}mm"\n'.format(r),
            '       cy="{}mm"\n'.format(c[1]),
            '       cx="{}mm" />\n'.format(c[0])]

    return text

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


class PlateLayout:
    def __init__(self, N, plate_radius):
        self.N = N # number of enclosed circles
        self.plate_radius = plate_radius

    def compute_layout(self):
        # read radius and center coordinates for enclosed circles
        rtilde, coords = read_circle_data(self.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, self.N):
            p1 = coords[k]
            p2 = coords[(k+1) % self.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)

        tube1 = {}
        tube2 = {}

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

        print("\n------------------")
        print("optimal values")
        print("plate radius  = {:6.3} m = {:6.2f} mm".format(outer_radius / 1000, outer_radius))
        print("big circles:")
        print("  radius = {:6.3} m = {:6.2f} mm".format(tube1_radius / 1000, tube1_radius))
        print("  diameter = {:6.3} m = {:6.2f} mm".format(2 * tube1_radius / 1000, 2 * tube1_radius))
        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))

        # compute coordinates and various measurements for fixed radii of plate and tubes
        self.target_plate_radius = 155.0
        self.target_radius_1 = 50.0
        self.target_radius_2 = 20.0
        teeth = 200

        D = 2 * self.target_plate_radius
        m = D/teeth

        print("plate radius        = {:6.2f} mm".format(self.target_plate_radius))
        print("big circle radius   = {:6.2f} mm".format(self.target_radius_1))
        print("small circle radius = {:6.2f} mm".format(self.target_radius_2))

        print("number of teeth: N = {}".format(teeth))
        print("pitch diameter:  D = {} mm".format(D))
        print("module:          M = {} mm".format(m))


        # parameters for dispenser gears
        self.dispenser_module = 1.5
        dispenser_1_target_pitch_diameter_big = 80.0
        dispenser_1_target_pitch_diameter_small = 25.0
        dispenser_1_teeth_big = int(dispenser_1_target_pitch_diameter_big/self.dispenser_module)
        dispenser_1_teeth_small = int(dispenser_1_target_pitch_diameter_small/ self.dispenser_module)

        dispenser_1_pitch_diameter_big = self.dispenser_module * dispenser_1_teeth_big
        dispenser_1_outer_diameter_big = dispenser_1_pitch_diameter_big + 2 * self.dispenser_module
        dispenser_1_pitch_diameter_small = self.dispenser_module * dispenser_1_teeth_small
        dispenser_1_outer_diameter_small = dispenser_1_pitch_diameter_small + 2 * self.dispenser_module

        dispenser_2_target_pitch_diameter_big = 40.0
        dispenser_2_target_pitch_diameter_small = 25.0
        dispenser_2_teeth_big = int(dispenser_2_target_pitch_diameter_big / self.dispenser_module)
        dispenser_2_teeth_small = int(dispenser_2_target_pitch_diameter_small / self.dispenser_module)

        dispenser_2_pitch_diameter_big = self.dispenser_module * dispenser_2_teeth_big
        dispenser_2_outer_diameter_big = dispenser_2_pitch_diameter_big + 2 * self.dispenser_module
        dispenser_2_pitch_diameter_small = self.dispenser_module * dispenser_2_teeth_small
        dispenser_2_outer_diameter_small = dispenser_2_pitch_diameter_small + 2 * self.dispenser_module

        # plot plate
        plt.figure(2)
        plt.plot(0.0, 0.0, 'o')
        circle = plt.Circle((0.0, 0.0), self.target_plate_radius, fill=False)
        plt.gca().add_artist(circle)
        plt.xlim((-self.target_plate_radius*1.1, self.target_plate_radius*1.1))
        plt.ylim((-self.target_plate_radius*1.1, self.target_plate_radius*1.1))
        plt.gca().set_aspect('equal', 'box')

        # plate coordinates
        print("plate coordinates: (x,y) = ({:8.3f}, {:8.3f})".format(0.0, 0.0))

        self.tube_1_coords = {}
        self.tube_2_coords = {}
        self.tube_1_tangents = {}
        self.tube_1_tangent_angles = {}
        self.tube_2_tangents = {}
        self.tube_2_tangent_angles = {}

        print(" big circle coordinates:")
        for k in range(0,self.N):
            x = coords[k][0] * outer_radius
            y = coords[k][1] * outer_radius

            angle = arctan2(y, x) * 360.0 / (2.0 * math.pi)

            self.tube_1_coords[k] = (x,y)

            print("   k = {},  (x,y) = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg".format(k, x, y, angle))

            circle = plt.Circle((x,y), self.target_radius_1, fill=False)
            plt.gca().add_artist(circle)

        p1 = dispenser_1_pitch_diameter_big
        p2 = dispenser_1_pitch_diameter_small
        a1 = self.dispenser_module
        a2 = self.dispenser_module
        offset_1 = sqrt((p1 / 2 + p2 / 2) ** 2 - (p1 / 2 + a1) ** 2) - p2 / 2 - a2
        print("dispenser 1 offset = {}".format(offset_1))

        print(" big circle tangent points: ")
        for k in range(0, self.N):
            x = coords[k][0] * outer_radius
            y = coords[k][1] * outer_radius

            t1, t2 = compute_tangent_points((0, 0), (x, y), self.target_radius_1)

            angle1 = arctan2(t1[1], t1[0]) * 360.0 / (2.0 * math.pi)
            angle2 = arctan2(t2[1], t2[0]) * 360.0 / (2.0 * math.pi)

            self.tube_1_tangents[k] = (t1, t2)
            self.tube_1_tangent_angles[k] = (angle1, angle2)

            print(
            "   k = {},  t1 = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg\n           t2 = ({:8.3f}, {:8.3f}),  angle "
            "= {:8.3f} deg".format(k, t1[0], t1[1], angle1, t2[0], t2[1], angle2))

            plt.plot(t1[0], t1[1], 'o')
            plt.plot(t2[0], t2[1], 'o')


        print(" small circle coordinates:")
        for k in range(0,self.N):
            x = coords_2[k][0] * outer_radius
            y = coords_2[k][1] * outer_radius

            angle = arctan2(y, x) * 360.0 / (2.0 * math.pi)

            self.tube_2_coords[k] = (x, y)

            print("   k = {},  (x,y) = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg".format(k, x, y, angle))

            circle = plt.Circle((x, y), self.target_radius_2, fill=False)
            plt.gca().add_artist(circle)

        p1 = dispenser_2_pitch_diameter_big
        p2 = dispenser_2_pitch_diameter_small
        a1 = self.dispenser_module
        a2 = self.dispenser_module
        offset_2 = sqrt((p1 / 2 + p2 / 2) ** 2 - (p1 / 2 + a1) ** 2) - p2 / 2 - a2
        print("dispenser 2 offset = {}".format(offset_2))

        print(" small circle tangent points: ")
        for k in range(0, self.N):
            x = coords_2[k][0] * outer_radius
            y = coords_2[k][1] * outer_radius

            t1, t2 = compute_tangent_points((0, 0), (x, y), self.target_radius_2)
            angle1 = arctan2(t1[1], t1[0]) * 360.0 / (2.0 * math.pi)
            angle2 = arctan2(t2[1], t2[0]) * 360.0 / (2.0 * math.pi)
            self.tube_2_tangents[k] = (t1, t2)
            self.tube_2_tangent_angles[k] = (angle1, angle2)

            print("   k = {},  t1 = ({:8.3f}, {:8.3f}),  angle = {:8.3f} deg\n           t2 = ({:8.3f}, {:8.3f}),  angle "
                  "= {:8.3f} deg".format(k, t1[0], t1[1], angle1, t2[0], t2[1], angle2))

            plt.plot(t1[0], t1[1], 'o')
            plt.plot(t2[0], t2[1], 'o')

    pass


    """
    input: file = svg with plate gear centered at (0,0)
    """
    def generate_svg(self, file):

        f = open(file)
        f_lines = f.readlines()
        f.close()

        f_lines.remove(f_lines[-1])

        # output plate as svg
        text = svg_circle(0, 'plate', (0,0), self.target_plate_radius)
        f_lines = f_lines + text

        # output big circles as svg
        for k, c in self.tube_1_coords.items():
            text = svg_circle(k, 'big circle', c, self.target_radius_1)
            f_lines = f_lines + text
            pass

        # output small circles as svg
        for k, c in self.tube_2_coords.items():
            text = svg_circle(k, 'small circle', c, self.target_radius_2)
            f_lines = f_lines + text
            pass

        f_lines.append('</svg>\n')

        # write new svg image
        fw = open('output.svg', 'w')
        fw.writelines(f_lines)
        fw.close()

        pass