from shapely.geometry import Polygon, Point
import matplotlib.pyplot as plt 
import random
import numpy as np
import sys

def shapes_collide(shape_1, shape_2):
    """
    Determine if two circles, two polygons, or a circle and a polygon intersect.
    True if they do intersect, else false
    inputs:
        - shape_1 (Shapely obj): the first shape
        - shape_2 (shapely obj): the second shape
    output:
        - bool
    """
    return shape_1.intersects(shape_2)

def break_edge_into_segments(node1, node2,edge_dir, delta):
    edge_segments = []
    p = node1
    while np.linalg.norm(p - node1) < np.linalg.norm(node2 - node1):
        p = p + delta*edge_dir
        edge_segments.append(p)

    return edge_segments

def paths_collide(path_1, path_2, delta=.3):
    """
    Determine if two paths collide with each other. 
    Return true if a collision is detected.
    inputs:
        - path_1 (list): the first path
        - path_2 (list): the second path
        - delta (float): the amount of an edge that is checked at a time
    """
    min_len = min(len(path_1), len(path_2))
    history = []

    for i in range(min_len-1):
        path1_node1 = np.array([path_1[i][0], path_1[i][1]])
        path1_node2 = np.array([path_1[i+1][0], path_1[i+1][1]])
        path2_node1 = np.array([path_2[i][0], path_2[i][1]])
        path2_node2 = np.array([path_2[i+1][0], path_2[i+1][1]])

        edge1 = path1_node2 - path1_node1
        edge2 = path2_node2 - path2_node1
        edge1_dir = edge1 / np.linalg.norm(edge1)
        edge2_dir = edge2 / np.linalg.norm(edge2)

        edge1_segments = break_edge_into_segments(path1_node1,path1_node2, edge1_dir, delta)
        edge2_segments = break_edge_into_segments(path2_node1,path2_node2, edge2_dir, delta)

        num_segs = min(len(edge1_segments), len(edge2_segments))
        edge1_segments = edge1_segments[0:num_segs]
        edge2_segments = edge2_segments[0:num_segs]

        for p1,p2 in zip(edge1_segments, edge2_segments):
            circ1 = Point(p1[0],p1[1])
            circ1 = circ1.buffer(.1)
            circ2 = Point(p2[0],p2[1])
            circ2 = circ2.buffer(.1)

            history.append([circ1,circ2])

            if circ1.intersects(circ2): 
                return True, history

    return False, history
    
def draw_path(ax, path, color):
    for i in range(len(path)-1):
        this_node = path[i]
        next_node = path[i+1]

        ax.plot([this_node[0], next_node[0]], [this_node[1], next_node[1]], '-o', color=color)

def draw_edge_collision_check(ax, hist):
    for circs in hist:
        circ1 = circs[0]
        circ2 = circs[1]
        print(circ1)
        ax.plot(*circ1.exterior.xy, color="red")
        ax.plot(*circ2.exterior.xy, color="red")


if __name__ == "__main__":

    path1 = [[1,1], [2,2], [3,3], [4,4]]
    path2 = [[.7,1.5], [2,1.5], [3,2.5]]

    collide, hist = paths_collide(path1, path2)
    print(collide)

    fig, ax = plt.subplots()

    draw_path(ax,path1,"tab:blue")
    draw_path(ax,path2,"tab:green")

    draw_edge_collision_check(ax, hist)

    plt.show()


    # coords = ((0., 0.), (0., 1.), (1., 1.), (1., 0.), (0., 0.))
    # polygon = Polygon(coords)
    # print(polygon.area)
    # plt.plot(*polygon.exterior.xy)
    # plt.show()

    # p1 = Polygon([(0,0), (1,1), (1,0), (0,0)])
    # p2 = Polygon([(0,1), (1,0), (1,1), (0,1)])
    # plt.plot(*p1.exterior.xy)
    # plt.plot(*p2.exterior.xy)
    # print(p1.intersects(p2))
    # plt.show()

    # circles = []
    # center2 = Point(2, 2)
    # circles.append(center2.buffer(5))
    # center2 = Point(1, 1)
    # circles.append(center2.buffer(5))
    # for circ in circles:
    #     plt.plot(*circ.exterior.xy)

    # plt.show()

    # print(circles[0].intersects(p1))