from PIL import Image, ImageDraw
import numpy as np
import random

default_color = (255, 0, 0)

default_size = (1000, 1000)
default_boundary_z = 10

default_points_number = 500


class Node:
    def __init__(self, point, value):
        self.x = point[0]
        self.y = point[1]
        self.value = value

    def __str__(self):
        return "<" + str(self.x) + ", " + str(self.y) + ">"

    def __repr__(self):
        return str(self)


class Triangle:
    def __init__(self, nodes):
        self.nodes = nodes

    def __str__(self):
        return str(self.nodes)

    def __repr__(self):
        return str(self)


def draw_line(image, xy1, xy2, fill=default_color):
    draw = ImageDraw.Draw(image)
    draw.line(xy1 + xy2, fill, width=1)
    del draw


def draw_value(image, point, value):
    draw = ImageDraw.Draw(image)
    draw.text(point, str(value), fill=(0, 0, 0, 1))
    del draw


def draw_interpolated(image, point, value):
    draw = ImageDraw.Draw(image)
    draw.ellipse((point[0] - 10, point[1] - 10, point[0] + 10, point[1] + 10), fill=default_color)
    draw.text((point[0], point[1]), str(value), fill=(0, 0, 0, 1))
    del draw

# ---------------------
#  Private methods
# ---------------------

def to_node(points):
    return [Node(point[:-1], point[2]) for point in points]


def pairs(array):
    return zip(array[:-1] + [array[-1]], array[1:] + [array[0]])


def point_in_triangle(point, triangle):
    def sign(p1, p2, p3):
        return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y)
    v1, v2, v3 = triangle.nodes
    return sign(point, v1, v2) > 0.0 and sign(point, v2, v3) > 0.0 and sign(point, v3, v1) > 0.0


# ---------------------
# Triangulation methods
# ---------------------


def bounding_box(grid, image_size):
    min_x, max_x = image_size[0], 0
    min_y, max_y = image_size[1], 0
    for point in grid:
        if point[0] < min_x:
            min_x = point[0]
        if point[0] > max_x:
            max_x = point[0]
        if point[1] < min_y:
            min_y = point[1]
        if point[1] > max_y:
            max_y = point[1]

    return [(min_x, min_y, default_boundary_z),
            (max_x, min_y, default_boundary_z),
            (max_x, max_y, default_boundary_z),
            (min_x, max_y, default_boundary_z)]


def in_circle_robust(node, triangle):
    """Check if point p is inside of circle around
     the triangle tri.
    This is a robust predicate, slower than compare distance to centers
    ref: http://www.cs.cmu.edu/~quake/robust.html
    :param triangle: Triangle
    :param node:  Node
    """

    m1 = np.asarray([[n.x - node.x, n.y - node.y] for n in triangle.nodes])
    m2 = np.sum(np.square(m1), axis=1).reshape((3, 1))
    m = np.hstack((m1, m2))    # The 3x3 matrix to check
    return np.linalg.det(m) > 0


def polygon(connected_triangles):
    if len(connected_triangles) == 1:
        return connected_triangles[0].nodes

    def _polygon(triangles, polygon_nodes):
        if len(triangles) == 0:
            return polygon_nodes
        if len(polygon_nodes) == 0:
            triangle_first = triangles[0]
            return _polygon(triangles[1:], polygon_nodes=triangle_first.nodes)

        def common_nodes(tri, nodes):
            return [node for node in tri.nodes if node in nodes]

        triangle_candidate = next(tri for tri in triangles
                                  if len(common_nodes(tri, polygon_nodes)) == 2)
        triangles_except_candidate = [tri for tri in triangles
                                      if tri != triangle_candidate]
        point_new = [node for node in triangle_candidate.nodes
                     if node not in polygon_nodes]
        point_sep_1, point_sep_2 = [node for node in triangle_candidate.nodes
                                    if node in polygon_nodes]
        index_sep_1, index_sep_2 = polygon_nodes.index(point_sep_1),\
                                   polygon_nodes.index(point_sep_2)
        if abs(index_sep_1 - index_sep_2) == 1:
            separation_index = min(index_sep_1, index_sep_2) + 1
            polygon_nodes_new = polygon_nodes[:separation_index] +\
                                point_new + polygon_nodes[separation_index:]
            return  _polygon(triangles_except_candidate, polygon_nodes=polygon_nodes_new)
        else:
            separation_index = 0
            polygon_nodes_new = polygon_nodes[:separation_index] + point_new + \
                                polygon_nodes[separation_index:]
            return  _polygon(triangles_except_candidate, polygon_nodes=polygon_nodes_new)

    return _polygon(connected_triangles, [])


def build_destroy(triangles, nodes_to_insert):
    """

    :param triangles: [Triangle]
    :param nodes_to_insert: node to insert from grid
    :return: triangulation result - [Triangle]
    """
    if len(nodes_to_insert) == 0:
        return triangles

    node = nodes_to_insert[0]
    triangles_to_destroy = [candidate for candidate in triangles
                            if in_circle_robust(node, candidate)]
    triangles_rest = [candidate for candidate in triangles
                      if not in_circle_robust(node, candidate)]

    nodes_polygon = polygon(triangles_to_destroy)

    def _build_triangles_from_polygon(polygon_nodes, node_new):
        assert len(polygon_nodes) >= 3
        return [Triangle([node1, node2, node_new])
                for node1, node2 in pairs(polygon_nodes)]

    triangles_built = _build_triangles_from_polygon(polygon_nodes=nodes_polygon,
                                                    node_new=node)

    return build_destroy(triangles_rest + triangles_built, nodes_to_insert[1:])


def build_triangulation(grid, size):
    box = bounding_box(grid=grid, image_size=size)
    box_nodes = to_node(points=box)
    grid_except_box = [point for point in grid if point not in box]

    triangle1 = Triangle(nodes=box_nodes[:-1])
    triangle2 = Triangle(nodes=box_nodes[2:] + [box_nodes[0]])
    triangles = build_destroy(triangles=[triangle1, triangle2],
                              nodes_to_insert=to_node(grid_except_box))
    return triangles


# ---------------------
# Interpolation
# ---------------------


def interpolate_in_triangle(point, triangle):
    if not point_in_triangle(point, triangle):
        assert False
    v1, v2, v3 = triangle.nodes

    def distance(point1, point2):
        return np.sqrt((point1.x - point2.x)**2 + (point2.y - point1.y)**2)

    d1, d2, d3 = [distance(v, point) for v in [v1, v2, v3]]
    w1, w2, w3 = 1.0 / d1, 1.0 / d2, 1.0 / d3

    interpolated = (w1 * v1.value + w2*v2.value + w3 * v3.value) / (w1 + w2 + w3)
    return interpolated


def interpolate(point, triangulation):
    containing_triangle = [tri for tri in triangulation if point_in_triangle(point, tri)][0]
    return interpolate_in_triangle(point, containing_triangle)


# ---------------------
# Main
# ---------------------


def create_grid():
    return [(
                random.randint(0, default_size[0]),
                random.randint(0, default_size[1]),
                random.randint(0, default_boundary_z)
            ) for _ in range(default_points_number)]


def main():
    array = create_grid()
    interpolating_point = array[-1]
    interpolating_node = to_node([interpolating_point])[0]
    grid = array[:-1]
    triangles = build_triangulation(grid=grid, size=default_size)
    result = interpolate(interpolating_node, triangulation=triangles)
    print("Interpolated: " + str(result))

    image = Image.new("RGB", default_size, color='white')
    for triangle in triangles:
        for node1, node2 in pairs(triangle.nodes):
            draw_line(image, (node1.x, node1.y), (node2.x, node2.y))

    for point in grid:
        draw_value(image, point=(point[0], point[1]), value=point[2])

    draw_interpolated(image, interpolating_point, result)

    image.save("triangulation_result.png", "PNG")
    return


if __name__ == '__main__':
    main()