import sys

import matplotlib.pyplot as plt
from matplotlib.path import Path
import matplotlib.patches as patches
from matplotlib import collections as mc
import numpy as np
import math
import queue

'''
Set up matplotlib to create a plot with an empty square
'''
def setupPlot():
    fig = plt.figure(num=None, figsize=(5, 5), dpi=120, facecolor='w', edgecolor='k')
    ax = fig.subplots()
    ax.set_axisbelow(True)
    ax.set_ylim(-1, 11)
    ax.set_xlim(-1, 11)
    ax.grid(which='minor', linestyle=':', alpha=0.2)
    ax.grid(which='major', linestyle=':', alpha=0.5)
    return fig, ax

'''
Make a patch for a single pology
'''
def createPolygonPatch(polygon, color):
    verts = []
    codes= []
    for v in range(0, len(polygon)):
        xy = polygon[v]
        verts.append((xy[0], xy[1]))
        if v == 0:
            codes.append(Path.MOVETO)
        else:
            codes.append(Path.LINETO)
    verts.append(verts[0])
    codes.append(Path.CLOSEPOLY)
    path = Path(verts, codes)
    patch = patches.PathPatch(path, facecolor=color, lw=1)

    return patch


'''
Render the problem
'''
def drawProblem(robotStart, robotGoal, polygons):
    _, ax = setupPlot()
    patch = createPolygonPatch(robotStart, 'green')
    ax.add_patch(patch)
    patch = createPolygonPatch(robotGoal, 'red')
    ax.add_patch(patch)
    for p in range(0, len(polygons)):
        patch = createPolygonPatch(polygons[p], 'gray')
        ax.add_patch(patch)
    plt.show()

'''
Grow a simple RRT
'''
def growSimpleRRT(points):
    newPoints = {}
    adjListMap = {}

    bool = True
    count = 0
    # Your code goes here
    for i in points:
        if len(newPoints) == 0:
            newPoints[i] = points[i]
            newPoints[i+1] = points[i+1]
            adjListMap[i] = [i+1]
            adjListMap[i+1] = [i]
        elif len(newPoints) == 2 and bool:
            bool = False
            continue
        else:
            num = 0
            index = 0
            point1 = 0
            point2 = 0
            d = sys.maxsize                             #distance from random point to nearest point on line
            x = 0
            y = 0
            for j in list(adjListMap):                  # j is the key in adjListMap. j just goes through each point
                for k in adjListMap[j]:                # k goes through every point that j is connected to
                    m1 = (newPoints[k][1]-newPoints[j][1])/(newPoints[k][0]-newPoints[j][0])
                    b1 = newPoints[j][1] - m1*newPoints[j][0]
                    m2 = -1/m1
                    b2 = points[i][1] - m2*points[i][0]
                    newPointX = (b2-b1)/(m1-m2)
                    newPointY = m1*newPointX + b1
                    if not collisionDetection(adjListMap,newPoints,[points[i],(newPointX,newPointY)],j,k):
                        if (newPoints[k][0] <= newPointX <= newPoints[j][0] or newPoints[k][0] >= newPointX >= newPoints[j][0]) and (newPoints[k][1] <= newPointY <= newPoints[j][1] or newPoints[k][1] >= newPointY >= newPoints[j][1]):
                            if d > math.sqrt((newPointX-points[i][0])**2+(newPointY-points[i][1])**2):
                                d = math.sqrt((newPointX-points[i][0])**2+(newPointY-points[i][1])**2)
                                x = newPointX
                                y = newPointY
                                point1 = j
                                point2 = k
                                num = 0
                        else:
                            if newPoints[k][0] > newPoints[j][0]:
                                if newPointX > newPoints[k][0]:
                                    if d > math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2):
                                        d = math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2)
                                        index = k
                                        num = 1
                                else:
                                    if d > math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2):
                                        d = math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2)
                                        index = j
                                        num = 1
                            else:
                                if newPointX > newPoints[j][0]:
                                    if d > math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2):
                                        d = math.sqrt((newPoints[j][0]-points[i][0])**2+(newPoints[j][1]-points[i][1])**2)
                                        index = j
                                        num = 1
                                else:
                                    if d > math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2):
                                        d = math.sqrt((newPoints[k][0]-points[i][0])**2+(newPoints[k][1]-points[i][1])**2)
                                        index = k
                                        num = 1

            if newPointX != points[i][0] and newPointY != points[i][1]:
                temp = i+count
                newPoints[temp] = points[i]
                if num == 0:
                    count+=1
                    newPoints[i+count] = (x,y)
                    if adjListMap.get(temp) == None:
                        adjListMap[temp] = [i+count]
                    else:
                        adjListMap[temp].append(i+count)
                    adjListMap[i+count] = [temp]
                    adjListMap[i+count].append(point1)
                    adjListMap[i+count].append(point2)
                    newList = adjListMap[point1]
                    newList.remove(point2)
                    newList.append(i+count)
                    adjListMap[point1] = newList
                    newList = adjListMap[point2]
                    newList.remove(point1)
                    newList.append(i+count)
                    adjListMap[point2] = newList
                else:
                    adjListMap[temp] = [index]
                    adjListMap[index].append(temp)


    return newPoints, adjListMap

def collisionDetection(adjListMap, newPoints, points,a,b):
    bool = 0
    for i in adjListMap:
        for j in adjListMap[i]:
            if i == a and j == b or i == b and j == a:
                continue
            m1 = (newPoints[j][1]-newPoints[i][1])/(newPoints[j][0]-newPoints[i][0])
            b1 = newPoints[i][1] - m1*newPoints[i][0]
            m2 = (points[1][1]-points[0][1])/(points[1][0]-points[0][0])
            b2 = points[1][1]-m2*points[1][0]
            if (m1 == m2):
                continue
            intersectX = (b2-b1)/(m1-m2)
            intersectY = m1*intersectX + b1
            for k in adjListMap:
                for l in adjListMap[k]:
                    if ((newPoints[l][0] > intersectX > newPoints[k][0] or newPoints[l][0] < intersectX < newPoints[k][0]) and (newPoints[l][1] > intersectY > newPoints[k][1] or newPoints[l][1] < intersectY < newPoints[k][1])) and ((points[0][0] < intersectX < points[1][0] or points[0][0] > intersectX > points[1][0]) and (points[1][0] < intersectY < points[1][1] or points[1][0] > intersectY > points[1][1])) and not intersectX == points[1][0] and not intersectY == points[1][1]:
                        bool = 1
                        break
                if bool==1:
                    break
            if bool==1:
                break
        if bool==1:
            break
    if bool==1:
        return True #the lines collide. This is not a valid point
    return False    #the lines do not collide.

'''
Perform basic search
'''
def backtrace(parent, start, end):
    path = [end]
    while path[-1] != start:
        path.append(parent[path[-1]])
    path.reverse()
    return path

def basicSearch(tree, start, goal):
    parent = {}
    path = []
    visited = [False] * (len(tree))
    squeue = queue.Queue()

    squeue.put(start)
    visited[start] = True
    found=False
    while squeue:
        s = squeue.get(0)
        path.append(s)
        if(s == goal):
            found=True
            return backtrace(parent, start, goal)

            # Get all adjacent vertices of the
            # dequeued vertex s. If a adjacent
            # has not been visited, then mark it
            # visited and enqueue it

        for l in tree[s]:
            if visited[l] == False:
                parent[l] = s
                squeue.put(l)
                visited[l] = True


    return path

'''
Display the RRT and Path
'''
def displayRRTandPath(points, adjListMap, path, robotStart=None, robotGoal=None, polygons=None):
    # Your code goes here
    # You could start by copying code from the function
    # drawProblem and modify it to do what you need.
    # You should draw the problem when applicable.
    _, ax = setupPlot()
    patch = createPolygonPatch(robotStart, 'green')
    ax.add_patch(patch)
    patch = createPolygonPatch(robotGoal, 'red')
    ax.add_patch(patch)

    for p in range(0, len(polygons)):
        patch = createPolygonPatch(polygons[p], 'gray')
        ax.add_patch(patch)


    line = []
    for j in adjListMap:
        for k in adjListMap[j]:
            line.append([points[j],points[k]])

    lc = mc.LineCollection(line, linewidths=2, color='black')
    ax.add_collection(lc)


    plt.show()

    #lines = [[(0, 1), (1, 1)], [(2, 3), (3, 3)], [(1, 2), (1, 3)]]
    #lc = mc.LineCollection(lines, linewidths=2)
    #ax.add_collection(lc)
    #plt.scatter(x,y, color='black', marker='o')
    return

'''
Collision checking
'''
def isCollisionFree(robot, point, obstacles):

    # Your code goes here.
    for i in range(0,len(robot)):
        robot[i] = (robot[i][0]+point[0],robot[i][1]+point[1])
        if robot[i][0] < 0 or robot[i][1] < 0 or robot[i][0] > 10 or robot[i][1] > 10:
            return False
    bool = False
    for i in robot:
        if i != len(robot)-1:
            m1 = (robot[i+1][1]-robot[i][1])/(robot[i+1][0]-robot[i][0])
            b1 = robot[i][1]-m1*robot[i][0]
            for j in obstacles:
                for k in obstacles[j]:
                    if k != len(obstacles[j])-1:
                        m2 = (obstacles[k][1]-obstacles[j][1])/(obstacles[k][0]-obstacles[j][0])
                        if m1 == m2:
                            continue
                        b2 = obstacles[j][1]-m2*obstacles[j][0]
                        intersectX = (b2-b1)/(m1-m2)
                        intersectY = m1*intersectX + b1
                        if (obstacles[j][0] <= intersectX <= obstacles[k][0] or obstacles[j][0] >= intersectX >= obstacles[k][0]) and (robot[i][0] <= intersectX <= robot[i+1][0] or robot[i][0] >= intersectX >= robot[i+1][0]) and (obstacles[j][1] <= intersectY <= obstacles[k][1] or obstacles[j][1] >= intersectY >= obstacles[k][1]) and (robot[i][1] <= intersectY <= robot[i+1][1] or robot[i][1] >= intersectY >= robot[i+1][1]):
                            bool = True
                            break
                if bool:
                    break
        if bool:
            break
    if bool:
        return False
    return True

'''
The full RRT algorithm
'''
def RRT(robot, obstacles, startPoint, goalPoint):

    points = dict()
    tree = dict()
    path = []
    # Your code goes here.

    return points, tree, path

def main(filename, x1, y1, x2, y2, display=''):
    # Read data and parse polygons
    lines = [line.rstrip('\n') for line in open(filename)]
    robot = []
    obstacles = []
    for line in range(0, len(lines)):
        xys = lines[line].split(';')
        polygon = []
        for p in range(0, len(xys)):
            xy = xys[p].split(',')
            polygon.append((float(xy[0]), float(xy[1])))
        if line == 0 :
            robot = polygon
        else:
            obstacles.append(polygon)

    # Print out the data
    print("Robot:")
    print(str(robot))
    print("Pologonal obstacles:")
    for p in range(0, len(obstacles)):
        print(str(obstacles[p]))
    print("")

    # Visualize
    if display == 'display':
        robotStart = [(x + x1, y + y1) for x, y in robot]
        robotGoal = [(x + x2, y + y2) for x, y in robot]
        #drawProblem(robotStart, robotGoal, obstacles)

    # Example points for calling growSimpleRRT
    # You should expect many mroe points, e.g., 200-500
    points = dict()
    points[1] = (5, 5)
    points[2] = (7, 8.2)
    points[3] = (6.5, 5.2)
    points[4] = (0.3, 4)
    points[5] = (6, 3.7)
    points[6] = (9.7, 6.4)
    points[7] = (4.4, 2.8)
    points[8] = (9.1, 3.1)
    points[9] = (8.1, 6.5)
    points[10] = (0.7, 5.4)
    points[11] = (5.1, 3.9)
    points[12] = (2, 6)
    points[13] = (0.5, 6.7)
    points[14] = (8.3, 2.1)
    points[15] = (7.7, 6.3)
    points[16] = (7.9, 5)
    points[17] = (4.8, 6.1)
    points[18] = (3.2, 9.3)
    points[19] = (7.3, 5.8)
    points[20] = (9, 0.6)

    # Printing the points
    print("")
    print("The input points are:")
    print(str(points))
    print("")

    points, adjListMap = growSimpleRRT(points)
    print("")
    print("The new points are:")
    print(str(points))
    print("")
    print("")
    print("The tree is:")
    print(str(adjListMap))
    print("")

    # Search for a solution
    # change 1 and 20 as you want
    path = basicSearch(adjListMap, 1, 2)
    print("")
    print("The path is:")
    print(str(path))
    print("")

    # Your visualization code
    #if display == 'display':
        #displayRRTandPath(points, adjListMap, path)

    # Solve a real RRT problem
    points, adjListMap, path = RRT(robot, obstacles, (x1, y1), (x2, y2))

    # Your visualization code
    if display == 'display':
        displayRRTandPath(points, adjListMap, path, robotStart, robotGoal, obstacles)


if __name__ == "__main__":
    # Retrive file name for input data
    if(len(sys.argv) < 6):
        print("Five arguments required: python spr.py [env-file] [x1] [y1] [x2] [y2]")
        exit()

    filename = sys.argv[1]
    x1 = float(sys.argv[2])
    y1 = float(sys.argv[3])
    x2 = float(sys.argv[4])
    y2 = float(sys.argv[5])
    display = ''
    if(len(sys.argv) == 7):
        display = sys.argv[6]

    main(filename, x1, y1, x2, y2, display)