def RRT(robot, obstacles, startPoint, goalPoint):

    max = 50
    points = dict()
    tree = dict()
    path = []
    while len(points) == 0:
        x = random.uniform(0.1,9.9)
        y = random.uniform(0.1,9.9)
        if not (withinPoly(obstacles,[x,y]) and not isCollisionFree(robot,(x,y),obstacles)):
            points[1] = (x,y)
    # Your code goes here.
    for i in range(2,max+1):
        x = random.uniform(0.1,9.9)
        y = random.uniform(0.1,9.9)
        if (x,y) in points.values():
            i-=1
            continue
        if withinPoly(obstacles,[x,y]) and not isCollisionFree(robot,(x,y),obstacles):
            i-=1
            continue
        if i == 2:
            # Make sure points 1 and 2 can connect before adding x,y to points
            if not lineWithinPoly(obstacles,[(x,y),points[1]]):
                tree[1] = [2]
                tree[2] = [1]
                points[i] = (x,y)
                continue
            else:
                i-=1
                continue
        d = sys.maxsize
        index = 0
        for j in list(tree):
            for k in tree[j]:
                if i == 3:
                    if not lineWithinPoly(obstacles,[(x,y),points[k]]):
                        if d > math.sqrt((x-points[k][0])**2+(y-points[k][1])**2):
                            d = math.sqrt((x-points[k][0])**2+(y-points[k][1])**2)
                            index = k
                elif not collisionDetection(tree,points,[points[k],(x,y)],j,k,1) and not lineWithinPoly(obstacles,[(x,y),points[k]]):
                    if d > math.sqrt((x-points[k][0])**2+(y-points[k][1])**2):
                        d = math.sqrt((x-points[k][0])**2+(y-points[k][1])**2)
                        index = k
        if index != 0:
            points[i] = (x,y)
            if tree.get(index) == None:
                tree[index] = [i]
            else:
                tree[index].append(i)
            tree[i] = [index]
        else:
            i-=1
            continue

    dStart = sys.maxsize
    dGoal = sys.maxsize
    indexS = 0
    indexG = 0
    for i in points:
        if not collisionDetection(tree,points,[startPoint,points.get(i)],0,i,1) and not lineWithinPoly(obstacles,[startPoint,points.get(i)]):
            if d > math.sqrt((startPoint[0]-points.get(i)[0])**2+(startPoint[1]-points.get(i)[1])**2):
                d = math.sqrt((startPoint[0]-points.get(i)[0])**2+(startPoint[1]-points.get(i)[1])**2)
                indexS = i
        if not collisionDetection(tree,points,[goalPoint,points.get(i)],0,i,1) and not lineWithinPoly(obstacles,[goalPoint,points.get(i)]):
            if d > math.sqrt((goalPoint[0]-points.get(i)[0])**2+(goalPoint[1]-points.get(i)[1])**2):
                d = math.sqrt((goalPoint[0]-points.get(i)[0])**2+(goalPoint[1]-points.get(i)[1])**2)
                indexG = i
    points[max+1] = startPoint
    points[max+2] = goalPoint

    if indexS != 0:
        tree[max+1] = [indexS]
        print("start index = ",indexS)
        tree[indexS].append(max+1)
    else:
        print("failed connecting startPoint")

    if indexG != 0:
        tree[max+2] = [indexG]
        tree[indexG].append(max+2)
    else:
        print("failed connecting endPoint")

    print("points made:",str(points))
    print("")
    print("tree:",str(tree))
    return points, tree, path