#!/usr/bin/env python
"""
===================================================================
RRTController.py - Rapidly-Exploring Random Trees Motion Controller
===================================================================

Uses Rapidly-exploring Random Tree Algorithm to generate paths given the starting position and the goal point.
"""

from numpy import *
from __is_inside import *
import math
import sys,os
from scipy.linalg import norm
from numpy.matlib import zeros
import __is_inside
import time, sys,os
import scipy as Sci
import scipy.linalg
import Polygon, Polygon.IO
import Polygon.Utils as PolyUtils
import Polygon.Shapes as PolyShapes
from math import sqrt, fabs , pi
import random
import thread
import threading

# importing matplotlib to show the path if possible
try:
    import matplotlib.pyplot as plt
    import matplotlib.animation as animation
    import_matplotlib = True
except:
    print "matplotlib is not imported. Plotting is disabled"
    import_matplotlib = False

class motionControlHandler:
    def __init__(self, proj, shared_data,robot_type,max_angle_goal,max_angle_overlap,plotting):
        """
        Rapidly-Exploring Random Trees alogorithm motion planning controller

        robot_type (int): Which robot is used for execution. BasicSim is 1, ODE is 2, ROS is 3, Nao is 4, Pioneer is 5(default=1)
        max_angle_goal (float): The biggest difference in angle between the new node and the goal point that is acceptable. If it is bigger than the max_angle, the new node will not be connected to the goal point. The value should be within 0 to 6.28 = 2*pi. Default set to 6.28 = 2*pi (default=6.28)
        max_angle_overlap (float): difference in angle allowed for two nodes overlapping each other. If you don't want any node overlapping with each other, put in 2*pi = 6.28. Default set to 1.57 = pi/2 (default=1.57)
        plotting (bool): Check the box to enable plotting. Uncheck to disable plotting (default=True)
        """

        self.system_print       = False       # for debugging. print on GUI ( a bunch of stuffs)
        self.finish_print       = False       # set to 1 to print the original finished E and V before trimming the tree
        self.orientation_print  = False        # show the orientation information of the robot

        # Get references to handlers we'll need to communicate with
        self.drive_handler = proj.h_instance['drive']
        self.pose_handler = proj.h_instance['pose']

        # Get information about regions
        self.proj              = proj
        self.coordmap_map2lab  = proj.coordmap_map2lab
        self.coordmap_lab2map  = proj.coordmap_lab2map
        self.last_warning      = 0
        self.previous_next_reg = None

        # Store the Rapidly-Exploring Random Tress Built
        self.RRT_V              = None               # array containing all the points on the RRT Tree
        self.RRT_E              = None               # array specifying the connection of points on the Tree
        self.E_current_column   = None               # the current column on the tree (to find the current heading point)
        self.Velocity           = None
        self.currentRegionPoly  = None
        self.nextRegionPoly     = None
        self.map                = {}
        self.all                = Polygon.Polygon()
        self.trans_matrix       = mat([[0,1],[-1,0]])   # transformation matrix for find the normal to the vector
        self.stuck_thres        = 20          # threshold for changing the range of sampling omega

        # Information about the robot (default set to ODE)
        if robot_type not in [1,2,3,4,5]:
            robot_type = 1
        self.system = robot_type

        # Information about maximum turning angle allowed from the latest node to the goal point
        if max_angle_goal > 2*pi:
            max_angle_goal = 2*pi
        if max_angle_goal < 0:
            max_angle_goal = 0
        self.max_angle_allowed = max_angle_goal

        # Information about maximum difference in angle allowed between two overlapping nodes
        if max_angle_overlap > 2*pi:
            max_angle_overlap = 2*pi
        if max_angle_overlap < 0:
            max_angle_overlap = 0
        self.max_angle_overlap = max_angle_overlap


        # Information about whether plotting is enabled.
        if plotting is True and import_matplotlib == True:
            self.plotting          = True
        else:
            self.plotting          = False

        # Specify the size of the robot
        # 1: basicSim; 2: ODE; 3: ROS  4: Nao; 5: Pioneer
        #  self.radius: radius of the robot
        #  self.timestep  : number of linear segments to break the curve into for calculation of x, y position
        #  self.step_size  : the length of each step for connection to goal point
        #  self.velocity   : Velocity of the robot in m/s in control space (m/s)
        if self.system  == 1:
            self.radius = 5
            self.step_size  = 25
            self.timeStep = 10
            self.velocity = 2    # 1.5
        if  self.system == 2:
            self.radius = 5
            self.step_size  = 15
            self.timeStep = 10
            self.velocity = 2    # 1.5
        elif self.system == 3:
            self.ROSInitHandler = shared_data['ROS_INIT_HANDLER']
            self.radius = self.ROSInitHandler.robotPhysicalWidth/2
            self.step_size  = self.radius*3 #0.2
            self.timeStep = 8
            self.velocity  = self.radius/2  #0.08
        elif self.system == 4:
            self.radius = 0.15*1.2
            self.step_size  = 0.2      #set the step_size for points be 1/5 of the norm  ORIGINAL = 0.4
            self.timeStep = 5
            self.velocity  = 0.05
        elif self.system == 5:
            self.radius = 0.15
            self.step_size  = 0.2      #set the step_size for points be 1/5 of the norm  ORIGINAL = 0.4
            self.timeStep = 5
            self.velocity  = 0.05


        # Operate_system (int): Which operating system is used for execution.
        # Ubuntu and Mac is 1, Windows is 2
        if sys.platform in ['win32', 'cygwin']:
            self.operate_system = 2
        else:
            self.operate_system = 1

        if self.system_print == True:
            print "The operate_system is "+ str(self.operate_system)

        # Generate polygon for regions in the map
        for region in self.proj.rfi.regions:
            self.map[region.name] = self.createRegionPolygon(region)
            for n in range(len(region.holeList)): # no of holes
                self.map[region.name] -= self.createRegionPolygon(region,n)

        # Generate the boundary polygon
        for regionName,regionPoly in self.map.iteritems():
            self.all += regionPoly

        # Start plotting if operating in Windows
        if self.operate_system == 2 and self.plotting ==True:
            # start using anmination to plot the robot
            self.fig = plt.figure()
            self.ax = self.fig.add_subplot(111)
            self.scope = _Scope(self.ax,self)
            thread.start_new_thread(self.jplot,())

    def gotoRegion(self, current_reg, next_reg, last=False):
        """
        If ``last`` is True, we will move to the center of the destination region.
        Returns ``True`` if we've reached the destination region.
        """

        if current_reg == next_reg and not last:
            # No need to move!
            self.drive_handler.setVelocity(0, 0)  # So let's stop
            return True

        # Find our current configuration
        pose = self.pose_handler.getPose()

        # Check if Vicon has cut out
        # TODO: this should probably go in posehandler?
        if math.isnan(pose[2]):
            print "WARNING: No Vicon data! Pausing."
            self.drive_handler.setVelocity(0, 0)  # So let's stop
            time.sleep(1)
            return False

        ###This part will be run when the robot goes to a new region, otherwise, the original tree will be used.
        if not self.previous_next_reg == next_reg:
            # Entered a new region. New tree should be formed.
            self.nextRegionPoly    = self.map[self.proj.rfi.regions[next_reg].name]
            self.currentRegionPoly = self.map[self.proj.rfi.regions[current_reg].name]

            if self.system_print == True:
                print "next Region is " + str(self.proj.rfi.regions[next_reg].name)
                print "Current Region is " + str(self.proj.rfi.regions[current_reg].name)

            #set to zero velocity before tree is generated
            self.drive_handler.setVelocity(0, 0)
            if last:
                transFace = None
            else:
                # Determine the mid points on the faces connecting to the next region (one goal point will be picked among all the mid points later in buildTree)
                transFace   = None
                q_gBundle   = [[],[]] # list of goal points (midpoints of transition faces)
                face_normal = [[],[]] # normal of the trnasition faces
                for i in range(len(self.proj.rfi.transitions[current_reg][next_reg])):
                    pointArray_transface = [x for x in self.proj.rfi.transitions[current_reg][next_reg][i]]
                    transFace = asarray(map(self.coordmap_map2lab,pointArray_transface))
                    bundle_x = (transFace[0,0] +transFace[1,0])/2    #mid-point coordinate x
                    bundle_y = (transFace[0,1] +transFace[1,1])/2    #mid-point coordinate y
                    q_gBundle     = hstack((q_gBundle,vstack((bundle_x,bundle_y))))

                    #find the normal vector to the face
                    face          = transFace[0,:] - transFace[1,:]
                    distance_face = norm(face)
                    normal        = face/distance_face * self.trans_matrix
                    face_normal   = hstack((face_normal,vstack((normal[0,0],normal[0,1]))))


                if transFace is None:
                    print "ERROR: Unable to find transition face between regions %s and %s.  Please check the decomposition (try viewing projectname_decomposed.regions in RegionEditor or a text editor)." % (self.proj.rfi.regions[current_reg].name, self.proj.rfi.regions[next_reg].name)

            # Run algorithm to build the Rapid-Exploring Random Trees
            self.RRT_V = None
            self.RRT_E = None

            # For plotting
            if self.operate_system == 2:
                if self.plotting == True:
                    self.ax.cla()
                else:
                    self.ax = None
            else:
                self.ax = None

            if self.operate_system == 1 and self.plotting == True:
                plt.cla()
                self.plotMap(self.map)
                plt.plot(pose[0],pose[1],'ko')

            self.RRT_V,self.RRT_E,self.E_current_column = self.buildTree(\
            [pose[0], pose[1]],pose[2],self.currentRegionPoly, self.nextRegionPoly,q_gBundle,face_normal)

            """
            # map the lab coordinates back to pixels
            V_tosend = array(mat(self.RRT_V[1:,:])).T
            V_tosend = map(self.coordmap_lab2map, V_tosend)
            V_tosend = mat(V_tosend).T
            s = 'RRT:E'+"["+str(list(self.RRT_E[0]))+","+str(list(self.RRT_E[1]))+"]"+':V'+"["+str(list(self.RRT_V[0]))+","+str(list(V_tosend[0]))+","+str(list(V_tosend[1]))+"]"+':T'+"["+str(list(q_gBundle[0]))+","+str(list(q_gBundle[1]))+"]"
            #print s
            """

        # Run algorithm to find a velocity vector (global frame) to take the robot to the next region
        self.Velocity = self.getVelocity([pose[0], pose[1]], self.RRT_V,self.RRT_E)
        #self.Node = self.getNode([pose[0], pose[1]], self.RRT_V,self.RRT_E)
        self.previous_next_reg = next_reg

        # Pass this desired velocity on to the drive handler
        self.drive_handler.setVelocity(self.Velocity[0,0], self.Velocity[1,0], pose[2])
        #self.drive_handler.setVelocity(self.Node[0,0], self.Node[1,0], pose[2])
        RobotPoly = Polygon.Shapes.Circle(self.radius,(pose[0],pose[1]))

        # check if robot is inside the current region
        departed = not self.currentRegionPoly.overlaps(RobotPoly)
        arrived  = self.nextRegionPoly.covers(RobotPoly)

        if departed and (not arrived) and (time.time()-self.last_warning) > 0.5:
            # Figure out what region we think we stumbled into
            for r in self.proj.rfi.regions:
                pointArray = [self.coordmap_map2lab(x) for x in r.getPoints()]
                vertices = mat(pointArray).T

                if is_inside([pose[0], pose[1]], vertices):
                    print "I think I'm in " + r.name
                    print pose
                    break
            self.last_warning = time.time()

        #print "arrived:"+str(arrived)
        return arrived

    def createRegionPolygon(self,region,hole = None):
        """
        This function takes in the region points and make it a Polygon.
        """
        if hole == None:
            pointArray = [x for x in region.getPoints()]
        else:
            pointArray = [x for x in region.getPoints(hole_id = hole)]
        pointArray = map(self.coordmap_map2lab, pointArray)
        regionPoints = [(pt[0],pt[1]) for pt in pointArray]
        formedPolygon= Polygon.Polygon(regionPoints)
        return formedPolygon

    def getVelocity(self,p, V, E, last=False):
        """
        This function calculates the velocity for the robot with RRT.
        The inputs are (given in order):
            p        = the current x-y position of the robot
            E        = edges of the tree  (2 x No. of nodes on the tree)
            V        = points of the tree (2 x No. of vertices)
            last = True, if the current region is the last region
                 = False, if the current region is NOT the last region
        """

        pose     = mat(p).T

        #dis_cur = distance between current position and the next point
        dis_cur  = vstack((V[1,E[1,self.E_current_column]],V[2,E[1,self.E_current_column]]))- pose

        heading = E[1,self.E_current_column]        # index of the current heading point on the tree
        if norm(dis_cur) < 1.5*self.radius:         # go to next point
            if not heading == shape(V)[1]-1:
                self.E_current_column = self.E_current_column + 1
                dis_cur  = vstack((V[1,E[1,self.E_current_column]],V[2,E[1,self.E_current_column]]))- pose
            #else:
            #    dis_cur  = vstack((V[1,E[1,self.E_current_column]],V[2,E[1,self.E_current_column]]))- vstack((V[1,E[0,self.E_current_column]],V[2,E[0,self.E_current_column]]))

        Vel = zeros([2,1])
        Vel[0:2,0] = dis_cur/norm(dis_cur)*0.5                    #TUNE THE SPEED LATER
        return Vel

    def getNode(self,p, V, E, last=False):
        """

        This function calculates the velocity for the robot with RRT.
        The inputs are (given in order):
            p        = the current x-y position of the robot

            E        = edges of the tree  (2 x No. of nodes on the tree)
            V        = points of the tree (2 x No. of vertices)
            last = True, if the current region is the last region
                 = False, if the current region is NOT the last region

        """

        pose     = mat(p).T

        #dis_cur = distance between current position and the next point
        dis_cur  = vstack((V[1,E[1,self.E_current_column]],V[2,E[1,self.E_current_column]]))- pose

        heading = E[1,self.E_current_column]        # index of the current heading point on the tree
        if norm(dis_cur) < 1.5*self.radius:         # go to next point
            if not heading == shape(V)[1]-1:
                self.E_current_column = self.E_current_column + 1
                dis_cur  = vstack((V[1,E[1,self.E_current_column]],V[2,E[1,self.E_current_column]]))- pose


        Node = zeros([2,1])
        Node[0,0] = V[1,E[1,self.E_current_column]]
        Node[1,0] = V[2,E[1,self.E_current_column]]
        #Vel[0:2,0] = dis_cur/norm(dis_cur)*0.5                    #TUNE THE SPEED LATER
        return Node


    def buildTree(self,p,theta,regionPoly,nextRegionPoly,q_gBundle,face_normal, last=False):
        """
        This function builds the RRT tree.
        p                : x,y position of the robot
        theta            : current orientation of the robot
        regionPoly       : current region polygon
        nextRegionPoly   : next region polygon
        q_gBundle        : coordinates of q_goals that the robot can reach
        face_normal      : the normal vector of each face corresponding to each goal point in q_gBundle
        """

        q_init          = mat(p).T
        V               = vstack((0,q_init))
        theta           = self.orientation_bound(theta)
        V_theta         = array([theta])

        #!!! CONTROL SPACE: generate a list of omega for random sampling
        omegaLowerBound = -math.pi/20      # upper bound for the value of omega
        omegaUpperBound = math.pi/20       # lower bound for the value of omega
        omegaNoOfSteps  = 20
        self.omega_range = linspace(omegaLowerBound,omegaUpperBound,omegaNoOfSteps)
        self.omega_range_escape = linspace(omegaLowerBound*4,omegaUpperBound*4,omegaNoOfSteps*4)    # range used when stuck > stuck_thres

        regionPolyOld = Polygon.Polygon(regionPoly)
        regionPoly += PolyShapes.Circle(self.radius*2.5,(q_init[0,0],q_init[1,0]))

        # check faces of the current region for goal points
        E     = [[],[]]       # the tree matrix
        Other = [[],[]]
        path                        = False          # if path formed then = 1
        stuck                       = 0              # count for changing the range of sampling omega
        append_after_latest_node    = False       # append new nodes to the latest node

        if self.system_print == True:
            print "plotting in buildTree is " + str(self.plotting)
        if self.plotting == True:
            if not plt.isinteractive():
                plt.ion()
            plt.hold(True)


        while not path:
            #step -1: try connection to q_goal (generate path to goal)
            i = 0

            if self.system_print == True:
                print "Try Connection to the goal points"

            # pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
            q_pass = [[],[],[]]
            q_pass_dist = []
            q_gBundle = mat(q_gBundle)
            face_normal = mat(face_normal)

            while i < q_gBundle.shape[1]:
                q_g_original = q_gBundle[:,i]
                q_g = q_gBundle[:,i]+face_normal[:,i]*1.5*self.radius    ##original 2*self.radius
                #q_g = q_gBundle[:,i]+(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius    ##original 2*self.radius
                if not regionPolyOld.isInside(q_g[0],q_g[1]):
                    #q_g = q_gBundle[:,i]-(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius    ##original 2*self.radius
                    q_g = q_gBundle[:,i]-face_normal[:,i]*1.5*self.radius    ##original 2*self.radius

                #forming polygon for path checking
                EdgePolyGoal = PolyShapes.Circle(self.radius,(q_g[0,0],q_g[1,0])) + PolyShapes.Circle(self.radius,(V[1,shape(V)[1]-1],V[2:,shape(V)[1]-1]))
                EdgePolyGoal = PolyUtils.convexHull(EdgePolyGoal)
                dist = norm(q_g - V[1:,shape(V)[1]-1])

                #check connection to goal
                connect_goal = regionPoly.covers(EdgePolyGoal)   #check coverage of path from new point to goal

                # compare orientation difference
                thetaPrev = V_theta[shape(V)[1]-1]

                theta_orientation = abs(arctan((q_g[1,0]- V[2,shape(V)[1]-1])/(q_g[0,0]- V[1,shape(V)[1]-1])))
                if q_g[1,0] > V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
                        theta_orientation = pi - theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
                        theta_orientation = theta_orientation
                elif q_g[1,0] < V[2,shape(V)[1]-1]:
                    if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
                        theta_orientation = pi + theta_orientation
                    elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
                        theta_orientation =  2*pi - theta_orientation

                # check the angle between vector(new goal to goal_original ) and vector( latest node to new goal)
                Goal_to_GoalOriginal = q_g_original - q_g
                LatestNode_to_Goal   = q_g - V[1:,shape(V)[1]-1]
                Angle_Goal_LatestNode= arccos(vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))/norm(Goal_to_GoalOriginal)/norm(LatestNode_to_Goal))

                # if connection to goal can be established and the max change in orientation of the robot is smaller than max_angle, tree is said to be completed.
                if self.orientation_print == True:
                    print "theta_orientation is " + str(theta_orientation)
                    print "thetaPrev is " + str(thetaPrev)
                    print "(theta_orientation - thetaPrev) is " + str(abs(theta_orientation - thetaPrev))
                    print "self.max_angle_allowed is " + str(self.max_angle_allowed)
                    print "abs(theta_orientation - thetaPrev) < self.max_angle_allowed" + str(abs(theta_orientation - thetaPrev) < self.max_angle_allowed)
                    print"Goal_to_GoalOriginal: " + str( array(Goal_to_GoalOriginal)) + "; LatestNode_to_Goal: " + str( array(LatestNode_to_Goal))
                    print vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))
                    print "Angle_Goal_LatestNode" + str(Angle_Goal_LatestNode)

                if connect_goal and (abs(theta_orientation - thetaPrev) < self.max_angle_allowed) and (Angle_Goal_LatestNode < self.max_angle_allowed):

                    path = True
                    q_pass = hstack((q_pass,vstack((i,q_g))))
                    q_pass_dist = hstack((q_pass_dist,dist))

                i = i + 1

            if self.system_print == True:
                print "checked goal points"

            self.E = E
            self.V = V
            # connection to goal has established
            # Obtain the closest goal point that path can be formed.
            if path:
                if shape(q_pass_dist)[0] == 1:
                    cols = 0
                else:
                    (cols,) = nonzero(q_pass_dist == min(q_pass_dist))
                    cols = asarray(cols)[0]
                q_g = q_pass[1:,cols]
                """
                q_g = q_g-(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*3*self.radius   #org 3
                if not nextRegionPoly.isInside(q_g[0],q_g[1]):
                    q_g = q_g+(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*6*self.radius   #org 3
                """
                if self.plotting == True :
                    if self.operate_system == 1:
                        plt.suptitle('Rapidly-exploring Random Tree', fontsize=12)
                        plt.xlabel('x')
                        plt.ylabel('y')
                        if shape(V)[1] <= 2:
                            plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        plt.plot(q_g[0,0],q_g[1,0],'ko')
                        plt.figure(1).canvas.draw()
                    else:
                        BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
                        self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                        if shape(V)[1] <= 2:
                            self.ax.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        else:
                            self.ax.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
                        self.ax.plot(q_g[0,0],q_g[1,0],'ko')

                # trim the path connecting current node to goal point into pieces if the path is too long now
                numOfPoint = floor(norm(V[1:,shape(V)[1]-1]- q_g)/self.step_size)
                if numOfPoint < 3:
                    numOfPoint = 3
                x = linspace( V[1,shape(V)[1]-1], q_g[0,0], numOfPoint )
                y = linspace( V[2,shape(V)[1]-1], q_g[1,0], numOfPoint )
                for i in range(x.shape[0]):
                    if i != 0:
                        V = hstack((V,vstack((shape(V)[1],x[i],y[i]))))
                        E = hstack((E,vstack((shape(V)[1]-2,shape(V)[1]-1))))

                #push the goal point to the next region
                q_g = q_g+face_normal[:,q_pass[0,cols]]*3*self.radius    ##original 2*self.radius
                if not nextRegionPoly.isInside(q_g[0],q_g[1]):
                    q_g = q_g-face_normal[:,q_pass[0,cols]]*6*self.radius    ##original 2*self.radius
                V = hstack((V,vstack((shape(V)[1],q_g[0,0],q_g[1,0]))))
                E = hstack((E,vstack((shape(V)[1]-2 ,shape(V)[1]-1))))

                if self.plotting == True :
                    if self.operate_system == 1:
                        plt.plot(q_g[0,0],q_g[1,0],'ko')
                        plt.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')
                        plt.figure(1).canvas.draw()
                    else:
                        self.ax.plot(q_g[0,0],q_g[1,0],'ko')
                        self.ax.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')

            # path is not formed, try to append points onto the tree
            if not path:

                # connection_to_tree : connection to the tree is successful
                if append_after_latest_node:
                    V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode(V,V_theta,E,Other,regionPoly,stuck, append_after_latest_node)
                else:
                    connection_to_tree = False
                    while not connection_to_tree:
                        V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode  (V,V_theta,E,Other,regionPoly,stuck)


        if self.finish_print:
            print 'Here is the V matrix:', V, 'Here is the E matrix:',E
            print >>sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n',E

        #B: trim to a single path
        single = 0
        while single == 0:
            trim  = 0
            for j in range(shape(V)[1]-3):
                (row,col) = nonzero(E == j+1)
                if len(col) == 1:
                    E = delete(E, col[0], 1)
                    trim = 1

            if trim == 0:
                single = 1;

        ####print with matlib
        if self.plotting ==True :
            if self.operate_system == 1:
                plt.plot(V[1,:],V[2,:],'b')
                for i in range(shape(E)[1]):
                    plt.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                    plt.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
                plt.figure(1).canvas.draw()
            else:
                BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
                self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
                self.ax.plot(V[1,:],V[2,:],'b')
                for i in range(shape(E)[1]):
                    self.ax.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
                    self.ax.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)

        #return V, E, and the current node number on the tree
        V = array(V)
        return V, E, 0


    def generateNewNode(self,V,V_theta,E,Other,regionPoly,stuck,append_after_latest_node =False):
        """
        Generate a new node on the current tree matrix
        V         : the node matrix
        V_theta   : the orientation matrix
        E         : the tree matrix (or edge matrix)
        Other     : the matrix containing the velocity and angular velocity(omega) information
        regionPoly: the polygon of current region
        stuck     : count on the number of times failed to generate new node
        append_after_latest_node : append new nodes to the latest node (True only if the previous node addition is successful)
        """


        if self.system_print == True:
            print "In control space generating path,stuck = " + str(stuck)

        connection_to_tree = False   # True when connection to the tree is successful

        if stuck > self.stuck_thres:
            # increase the range of omega since path cannot ge generated
            omega = random.choice(self.omega_range_escape)
        else:
            #!!!! CONTROL SPACE STEP 1 - generate random omega
            omega = random.choice(self.omega_range)


        #!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
        if append_after_latest_node:
            tree_index = shape(V)[1]-1
        else:
            if random.choice([1,2]) == 1:
                tree_index = random.choice(array(V[0])[0])
            else:
                tree_index = shape(V)[1]-1


        xPrev     = V[1,tree_index]
        yPrev     = V[2,tree_index]
        thetaPrev = V_theta[tree_index]

        j = 1
        #!!!! CONTROL SPACE STEP 3 - Check path of the robot
        path_robot = PolyShapes.Circle(self.radius,(xPrev,yPrev))
        while j <= self.timeStep:
            xOrg      = xPrev
            yOrg      = yPrev
            xPrev     = xPrev + self.velocity/omega*(sin(omega* 1 + thetaPrev)-sin(thetaPrev))
            yPrev     = yPrev - self.velocity/omega*(cos(omega* 1 + thetaPrev)-cos(thetaPrev))
            thetaPrev = omega* 1 + thetaPrev
            path_robot = path_robot + PolyShapes.Circle(self.radius,(xPrev,yPrev))

            j = j + 1

        thetaPrev = self.orientation_bound(thetaPrev)
        path_all = PolyUtils.convexHull(path_robot)
        in_bound = regionPoly.covers(path_all)
        """
        # plotting
        if plotting == True:
            self.plotPoly(path_all,'r',1)
        """

        stuck = stuck + 1

        if in_bound:
            robot_new_node = PolyShapes.Circle(self.radius,(xPrev,yPrev))
            # check how many nodes on the tree does the new node overlaps with
            nodes_overlap_count = 0
            for k in range(shape(V)[1]-1):
                robot_old_node = PolyShapes.Circle(self.radius,(V[1,k],V[2,k]))
                if robot_new_node.overlaps(robot_old_node):
                    if  abs(thetaPrev - V_theta[k]) <   self.max_angle_overlap:
                        nodes_overlap_count += 1


            if nodes_overlap_count == 0 or (stuck > self.stuck_thres+1 and nodes_overlap_count < 2) or (stuck > self.stuck_thres+500):
                if  stuck > self.stuck_thres+1:
                    append_after_latest_node  = False

                if (stuck > self.stuck_thres+500):
                    stuck = 0
                stuck = stuck - 20
                # plotting
                if self.plotting == True:
                    self.plotPoly(path_all,'b',1)

                if self.system_print == True:
                    print "node connected"

                V = hstack((V,vstack((shape(V)[1],xPrev,yPrev))))
                V_theta = hstack((V_theta,thetaPrev))
                E = hstack((E,vstack((tree_index ,shape(V)[1]-1))))
                Other = hstack((Other,vstack((self.velocity,omega))))
                ##################### E should add omega and velocity
                connection_to_tree = True
                append_after_latest_node  = True
            else:
                append_after_latest_node = False

                if self.system_print == True:
                    print "node not connected. check goal point"

        else:
            append_after_latest_node = False

        return  V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree


    def orientation_bound(self,theta):
        """
        make sure the returned angle is between 0 to 2*pi
        """
        while theta > 2*pi or theta < 0:
            if theta > 2*pi:
                theta = theta - 2*pi
            else:
                theta = theta + 2*pi

        return theta

    def plotMap(self,mappedRegions):
        """
        Plotting regions and obstacles with matplotlib.pyplot

        number: figure number (see on top)
        """

        #if not plt.isinteractive():
        #    plt.ion()
        #plt.hold(True)

        if self.operate_system == 1:
            for regionName,regionPoly in mappedRegions.iteritems():
                self.plotPoly(regionPoly,'k')
            plt.figure(1).canvas.draw()

    def plotPoly(self,c,string,w = 1):
        """
        Plot polygons inside the boundary
        c = polygon to be plotted with matlabplot
        string = string that specify color
        w      = width of the line plotting
        """
        if bool(c):
            for i in range(len(c)):
                #toPlot = Polygon.Polygon(c.contour(i))
                toPlot = Polygon.Polygon(c.contour(i)) & self.all
                if bool(toPlot):
                    for j in range(len(toPlot)):
                        #BoundPolyPoints = asarray(PolyUtils.pointList(toPlot.contour(j)))
                        BoundPolyPoints = asarray(PolyUtils.pointList(Polygon.Polygon(toPlot.contour(j))))
                        if self.operate_system == 2:
                            self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],string,linewidth=w)
                            self.ax.plot([BoundPolyPoints[-1,0],BoundPolyPoints[0,0]],[BoundPolyPoints[-1,1],BoundPolyPoints[0,1]],string,linewidth=w)
                        else:
                            plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],string,linewidth=w)
                            plt.plot([BoundPolyPoints[-1,0],BoundPolyPoints[0,0]],[BoundPolyPoints[-1,1],BoundPolyPoints[0,1]],string,linewidth=w)
                            plt.figure(1).canvas.draw()

    def data_gen(self):
        #self.ax.cla()
        for regionName,regionPoly in self.map.iteritems():
            self.plotPoly(regionPoly,'k')
        """
        #for i in range(len(self.V)):
        if shape(V)[1] <= 2:
            plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
        else:
            plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')

        self.plotPoly(self.realRobot, 'r')
        self.plotPoly(self.robot, 'b')
        """
        pose = self.pose_handler.getPose()
        self.ax.plot(pose[0],pose[1],'bo')
        """
        self.ax.plot(self.q_g[0],self.q_g[1],'ro')
        self.plotPoly(self.overlap,'g')
        self.plotPoly(self.m_line,'b')
        """
        yield(pose[0],pose[1])
        """
        self.ax.plot(self.prev_follow[0],self.prev_follow[1],'ko')
        """

    def jplot(self):
        ani = animation.FuncAnimation(self.fig, self.scope.update, self.data_gen)
        plt.show()

class _Scope:
    def __init__(self, ax, motion, maxt=2, dt=0.02):
        self.i = 0
        self.ax = ax
        self.line, = self.ax.plot(1)
        self.ax.set_ylim(0, 1)
        self.motion = motion

    def update(self,data):
        (data1) = self.motion.data_gen()
        a = data1.next()
        self.line.set_data(a)
        self.ax.relim()
        self.ax.autoscale()
        return self.line,
