<reponame>SuhrudhSarathy/indbot
'''
    Basic Implementaion of RRT without any Path Optimiser
'''
import matplotlib.pyplot as plt 
import numpy as np 
from shapely.geometry import Point, LineString, Polygon
import random
import time


def plot_obstacle(obstacle):
    point_list = obstacle.exterior.coords
    x_list = [p[0] for p in point_list]
    y_list = [p[1] for p in point_list]
    plt.fill(x_list, y_list, color='black')

class Rectangle():
    def __init__(self, center):
        self.x, self.y = center[0], center[1]
        self.polygon = Polygon([(self.x + 0.25, self.y + 0.25), (self.x - 0.25, self.y + 0.25), (self.x - 0.25, self.y - 0.25), (self.x + 0.25, self.y - 0.25)])

def generate_random_map(number):
    obstacle_list = []
    for i in range(number):
        rectangle = Rectangle((random.random()*10, random.random()*10))
        obstacle_list.append(rectangle.polygon)
    for obstacle in obstacle_list:
        plot_obstacle(obstacle)
    return obstacle_list

def obstacles():
    poly1 = Polygon([(2, 10), (7, 10), (7, 1), (6, 1), (6, 6), (4, 6), (4, 9), (2, 9)])
    poly2 = Polygon([(4, 0), (4, 5), (5, 5), (5, 0)])
    poly3 = Polygon([(8, 2), (8, 7), (10, 7), (10, 2)])
    '''cirlce1 = Point(8, 3).buffer(1)
    circle2 = Point(2, 7).buffer(1.5)
    poly4 = Polygon([(11, 10), (11, 13), (12, 13.75), (13, 12)])
    circle3 = Point(10, 1).buffer(0.75)
    circle4 = Point(11, 1.2).buffer(0.86)
    circle5 = Point(5, 15).buffer(1)
    circle6 = Point(4, 10).buffer(1)
    circle7 = Point(5, 10.7).buffer(1)'''
    obstacle_list = [poly1, poly2, poly3]
    return obstacle_list



def collisionCheck(point1, point2, obstacle_list):
    try:
        line = LineString([(point1.x, point1.y), (point2.x, point2.y)])
    except:
        line = LineString([(point1[0], point1[1]), (point2[0], point2[1])])
    intersection = 0
    for obstacle in obstacle_list:
        if line.intersects(obstacle):
            intersection += 1
        else:
            pass
    if intersection == 0:
        collision = False
    else:
        collision = True
    return collision

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

def new_vector(point1, point2, threshold):
    vector = Node((point2.x - point1.x), (point2.y - point1.x))
    vector.x, vector.y = vector.x/np.sqrt((vector.x**2 + vector.y**2)), vector.y/np.sqrt((vector.x**2 + vector.y**2))
    vector.x, vector.y = point1.x + vector.x * threshold, point1.y + vector.y * threshold
    return vector
class Node():
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.distance = 0
class RRT():

    '''
        Main RRT Class used for path planning
        Args:
            threshold: A threshold for deciding the delta parameter
            max_iter : Maximum number for iterations
    '''
    def __init__(self, threshold, max_iter):

        self.threshold = threshold
        self.nodes = []
        self.is_reached = False
        self.max_iter = max_iter
        self.goal_sample_rate = 0.1

    def reset(self, max_iter):
        self.nodes = []
        self.is_reached = False
        self.max_iter = max_iter
        self.goal_sample_rate = 0.1

    def _start_tree(self, start, goal, obstacle_list, max_iter):
        self.reset(max_iter)
        self.start = start
        self.goal = goal
        self.nodes.append(self.start)
        self.obstacle_list = obstacle_list
        #time1 = time.time()
        while self.max_iter > 0:
            print('planning', self.max_iter)
            new_node = self.generate_random_node()
            
            for node in self.nodes:
                node.distance = distance(node, new_node)
            self.nodes = sorted(self.nodes, key=lambda node: node.distance)
            nearby_node = self.nodes[0]
            for node in self.nodes:
                node.distance = 0 
            if distance(nearby_node, new_node) <= self.threshold:
                if collisionCheck(nearby_node, new_node, self.obstacle_list) == False:
                    new_node.parent = nearby_node
                    self.nodes.append(new_node)
                else : 
                    pass
            else:
                new_node = new_vector(nearby_node, new_node, self.threshold)
                if collisionCheck(new_node, nearby_node, self.obstacle_list) == False:
                    new_node.parent = nearby_node
                    self.nodes.append(new_node)
                else :
                    pass
            if new_node.x == self.goal.x and new_node.y == self.goal.y:
                self.is_reached == True
                print('-'*30 + 'Reached' + '-'*30)
                break
            self.max_iter -= 1
        

    def plot_points(self):
        plt.scatter([p.x for p in self.nodes], [p.y for p in self.nodes], color='red')
        for node in self.nodes :
            plt.plot([node.x, node.parent.x], [node.y, node.parent.y], color = 'green')
        plt.scatter([self.start.x, self.goal.x], [self.start.y, self.goal.y], color='yellow')

    def generate_random_node(self):
        if np.random.random_sample() > self.goal_sample_rate:
            x = np.random.uniform(-5, 10)
            y = np.random.uniform(-5, 10)
            new_node = Node(x, y)
        else:
            new_node = self.goal
        return new_node
    
    def get_path(self, start, goal, obstacle_list, max_iter):
        #print(obstacle_list)
        self._start_tree(start, goal, obstacle_list, max_iter)
        path = [self.nodes[-1]]
        current = self.nodes[-1]
        print(len(self.nodes))
        while current.parent != None:
            if current.x == start.x and current.y == start.y:
                path.append(current)
                break
            else:
                path.append(current.parent)
                current = current.parent
        path_planned = [(p.x, p.y, 0) for p in path]
        return path_planned[::-1]
    
    def path_optimiser(self, path, obstacles):
        pass

    
    

