# Path: pomp/planners/problem.py
# class PlanningProblem:
#     def __init__(self,space,
#                  start=None,
#                  goal=None,
#                  objective=None,
#                  visualizer=None,
#                  heuristic=None,
#                  costLowerBound=None,
#                  goalRadius=None,
#                  euclidean=False):
#         self.space = space
#         if isinstance(space,ControlSpace):
#             self.controlSpace = space
#             self.configurationSpace = space.configurationSpace()
#         else:
#             self.controlSpace = None
#             self.configurationSpace = space
#         if goalRadius != None and isinstance(goal,(list,tuple)):
#             goal = NeighborhoodSubset(self.configurationSpace,goal,goalRadius)
#         self.start = start
#         self.goal = goal
#         self.objective = objective
#         self.visualizer = visualizer
#         self.heuristic = heuristic
#         self.costLowerBound = costLowerBound
#         self.euclidean = euclidean
# 
#     def cartesian(self):
#         return self.euclidean
#     def pointToPoint(self):
#         return isinstance(self.goal,SingletonSubset) or isinstance(goal,(list,tuple))
#     def differentiallyConstrained(self):
#         return self.controlSpace != None
#     def planner(self,type,**params):
#         d = metric.euclideanMetric if self.euclidean else self.configurationSpace.distance
#         return allplanners.makePlanner(type,space=self.space,
#                            start=self.start,goal=self.goal,
#                            objective=self.objective,
#                            heuristic=self.heuristic,
#                            metric=d,
#                            costLowerBound=self.costLowerBound,
#                            **params)
# 
# Path: pomp/klampt/so2.py
# def identity():
# def inv(a):
# def apply(a,pt):
# def normalize(a):
# def diff(a,b):
# def interp(a,b,u):
# def compose(a,b):
# def matrix(a):
# def from_matrix(R):


from OpenGL.GL import *
from .geometric import *
from ..spaces.objectives import *
from ..spaces.statespace import *
from ..spaces.configurationspace import *
from ..spaces.edgechecker import *
from ..spaces.metric import *
from ..planners.problem import PlanningProblem
from ..spaces.so2space import *
from ..klampt import so2

    #controlSpace.setDistanceBounds(-0.25,0.25)
    controlSpace.setDistanceBounds(-0.1,0.1)
    controlSpace.setTurnRateBounds(-3.14,3.14)
    numControlsPerSample = 1
    if numControlsPerSample > 1:
        controlSpace = RepeatedControlSpace(controlSpace,numControlsPerSample)
    objective = DubinsCarDistanceObjectiveFunction(numControlsPerSample)
    goalRadius = 0.1
    return PlanningProblem(controlSpace,start,goal,
                           objective=objective,
                           visualizer=DubinsVisualizer(cspace),
                           goalRadius=goalRadius,
                           costLowerBound=lambda x,y:vectorops.distance(x[:2],y[:2]))

def dubinsTest2():
    cspace = Geometric2DCSpace()
    cspace.addObstacle(Box(0.5,0.4,0.9,0.7))
    cspace.addObstacle(Box(0.0,0.4,0.2,0.7))
    cspace.addObstacle(Box(0.0,0.9,1.0,1.0))
    vspace = BoxConfigurationSpace([-1,-1],[1,1])
    start = [0.8,0.3,0]
    goal = [0.8,0.8,math.pi]
    controlSpace = DubinsCarSpace(MultiConfigurationSpace(cspace,SO2Space()))
    controlSpace.setDistanceBounds(-0.25,0.25)
    controlSpace.setTurnRateBounds(-3.14,3.14)
    numControlsPerSample = 1
    if numControlsPerSample > 1:
        controlSpace = RepeatedControlSpace(controlSpace,numControlsPerSample)
    objective = DubinsCarDistanceObjectiveFunction(numControlsPerSample)
    goalRadius = 0.1

# Path: pomp/planners/problem.py
# class PlanningProblem:
#     def __init__(self,space,
#                  start=None,
#                  goal=None,
#                  objective=None,
#                  visualizer=None,
#                  heuristic=None,
#                  costLowerBound=None,
#                  goalRadius=None,
#                  euclidean=False):
#         self.space = space
#         if isinstance(space,ControlSpace):
#             self.controlSpace = space
#             self.configurationSpace = space.configurationSpace()
#         else:
#             self.controlSpace = None
#             self.configurationSpace = space
#         if goalRadius != None and isinstance(goal,(list,tuple)):
#             goal = NeighborhoodSubset(self.configurationSpace,goal,goalRadius)
#         self.start = start
#         self.goal = goal
#         self.objective = objective
#         self.visualizer = visualizer
#         self.heuristic = heuristic
#         self.costLowerBound = costLowerBound
#         self.euclidean = euclidean
# 
#     def cartesian(self):
#         return self.euclidean
#     def pointToPoint(self):
#         return isinstance(self.goal,SingletonSubset) or isinstance(goal,(list,tuple))
#     def differentiallyConstrained(self):
#         return self.controlSpace != None
#     def planner(self,type,**params):
#         d = metric.euclideanMetric if self.euclidean else self.configurationSpace.distance
#         return allplanners.makePlanner(type,space=self.space,
#                            start=self.start,goal=self.goal,
#                            objective=self.objective,
#                            heuristic=self.heuristic,
#                            metric=d,
#                            costLowerBound=self.costLowerBound,
#                            **params)
# 
# Path: pomp/klampt/so2.py
# def identity():
# def inv(a):
# def apply(a,pt):
# def normalize(a):
# def diff(a,b):
# def interp(a,b,u):
# def compose(a,b):
# def matrix(a):
# def from_matrix(R):


# Path: pomp/example_problems/dubins.py
from OpenGL.GL import *
from .geometric import *
from ..spaces.objectives import *
from ..spaces.statespace import *
from ..spaces.configurationspace import *
from ..spaces.edgechecker import *
from ..spaces.metric import *
from ..planners.problem import PlanningProblem
from ..spaces.so2space import *
from ..klampt import so2

    #controlSpace.setDistanceBounds(-0.25,0.25)
    controlSpace.setDistanceBounds(-0.1,0.1)
    controlSpace.setTurnRateBounds(-3.14,3.14)
    numControlsPerSample = 1
    if numControlsPerSample > 1:
        controlSpace = RepeatedControlSpace(controlSpace,numControlsPerSample)
    objective = DubinsCarDistanceObjectiveFunction(numControlsPerSample)
    goalRadius = 0.1
    return PlanningProblem(controlSpace,start,goal,
                           objective=objective,
                           visualizer=DubinsVisualizer(cspace),
                           goalRadius=goalRadius,
                           costLowerBound=lambda x,y:vectorops.distance(x[:2],y[:2]))

def dubinsTest2():
    cspace = Geometric2DCSpace()
    cspace.addObstacle(Box(0.5,0.4,0.9,0.7))
    cspace.addObstacle(Box(0.0,0.4,0.2,0.7))
    cspace.addObstacle(Box(0.0,0.9,1.0,1.0))
    vspace = BoxConfigurationSpace([-1,-1],[1,1])
    start = [0.8,0.3,0]
    goal = [0.8,0.8,math.pi]
    controlSpace = DubinsCarSpace(MultiConfigurationSpace(cspace,SO2Space()))
    controlSpace.setDistanceBounds(-0.25,0.25)
    controlSpace.setTurnRateBounds(-3.14,3.14)
    numControlsPerSample = 1
    if numControlsPerSample > 1:
        controlSpace = RepeatedControlSpace(controlSpace,numControlsPerSample)
    objective = DubinsCarDistanceObjectiveFunction(numControlsPerSample)
    goalRadius = 0.1

    return PlanningProblem(controlSpace,start,goal,
