# 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

        #self.controls = BoxSet([-1,-1],[1,1])
        self.controls = DubinsControlSet(-1,1,-1,1)
    def setDistanceBounds(self,minimum,maximum):
        self.controls.bmin[0] = minimum
        self.controls.bmax[0] = maximum
    def setTurnRateBounds(self,minimum,maximum):
        self.controls.bmin[1] = minimum
        self.controls.bmax[1] = maximum
    def configurationSpace(self):
        return self.space
    def controlSet(self,x):
        return self.controls
    def nextState(self,x,u):
        pos = [x[0],x[1]]
        fwd = [math.cos(x[2]),math.sin(x[2])]
        right = [-fwd[1],fwd[0]]
        phi = u[1]
        d = u[0]
        if abs(phi)<1e-8:
            newpos = vectorops.madd(pos,fwd,d)
            return newpos + [x[2]]
        else:
            #rotate about a center of rotation, with radius 1/phi
            cor = vectorops.madd(pos,right,1.0/phi)
            sign = cmp(d*phi,0)
            d = abs(d)
            phi = abs(phi)
            theta=0
            thetaMax=d*phi
            newpos = vectorops.add(so2.apply(sign*thetaMax,vectorops.sub(pos,cor)),cor)

# 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

        #self.controls = BoxSet([-1,-1],[1,1])
        self.controls = DubinsControlSet(-1,1,-1,1)
    def setDistanceBounds(self,minimum,maximum):
        self.controls.bmin[0] = minimum
        self.controls.bmax[0] = maximum
    def setTurnRateBounds(self,minimum,maximum):
        self.controls.bmin[1] = minimum
        self.controls.bmax[1] = maximum
    def configurationSpace(self):
        return self.space
    def controlSet(self,x):
        return self.controls
    def nextState(self,x,u):
        pos = [x[0],x[1]]
        fwd = [math.cos(x[2]),math.sin(x[2])]
        right = [-fwd[1],fwd[0]]
        phi = u[1]
        d = u[0]
        if abs(phi)<1e-8:
            newpos = vectorops.madd(pos,fwd,d)
            return newpos + [x[2]]
        else:
            #rotate about a center of rotation, with radius 1/phi
            cor = vectorops.madd(pos,right,1.0/phi)
            sign = cmp(d*phi,0)
            d = abs(d)
            phi = abs(phi)
            theta=0
            thetaMax=d*phi
            newpos = vectorops.add(so2.apply(sign*thetaMax,vectorops.sub(pos,cor)),cor)

            return newpos + [so2.normalize(x[2]+sign*thetaMax)]
