# Path: pomp/spaces/biassets.py
# class BoxBiasSet(BoxSet):
#     """A set that samples a box near its extrema, helpful for bang-bang control.
#     
#     Assume the box is [-1,1]^d.  A dimension k is picked, and a variable s is 
#     sampled by s = rand()^(1/c) where rand() samples uniformly from [0,1].  
#     Then its sign is randomly flipped with probability 0.5.  u[k] is then
#     set to s. The remaining dimensions are sampled as usual.
#     
#     To get back to an arbitrary box the range [-1,1]^d is simply scaled.
#     """
#     def __init__(self,bmin,bmax,concentration=3):
#         BoxSet.__init__(self,bmin,bmax)
#         if math.isinf(concentration):
#             self.power = 0
#         else:
#             self.power = 1.0/concentration
#             
#     def sample(self):
#         res = BoxSet.sample(self)
#         d = random.randint(0,len(res)-1)
#         (a,b) = (self.bmin[d],self.bmax[d])
#         sign = random.choice([-1,1])
#         s = math.pow(random.random(),self.power)
#         res[d] = (a+b)*0.5 + sign*(b-a)*0.5*s
#         return res
# 
# 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)


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 ..spaces.biassets import BoxBiasSet
from ..planners.problem import PlanningProblem


class DoubleIntegratorVisualizer:
    def __init__(self,workspace):
        self.base = workspace

    def toScreen(self,q):
        return q[0],q[1]

    def toState(self,x,y):
        return (x,y,0,0)

    def drawObstaclesGL(self):
        self.base.drawObstaclesGL()

    def drawVerticesGL(self,qs):
        self.base.drawVerticesGL(qs)

    def drawRobotGL(self,q):
        glColor3f(0,0,1)
        glPointSize(7.0)
        self.drawVerticesGL([q])
        l = 0.05
        glBegin(GL_LINES)
        glVertex2f(q[0],q[1])
        glVertex2f(q[0]+l*q[2],q[1]+l*q[3])

# Path: pomp/spaces/biassets.py
# class BoxBiasSet(BoxSet):
#     """A set that samples a box near its extrema, helpful for bang-bang control.
#     
#     Assume the box is [-1,1]^d.  A dimension k is picked, and a variable s is 
#     sampled by s = rand()^(1/c) where rand() samples uniformly from [0,1].  
#     Then its sign is randomly flipped with probability 0.5.  u[k] is then
#     set to s. The remaining dimensions are sampled as usual.
#     
#     To get back to an arbitrary box the range [-1,1]^d is simply scaled.
#     """
#     def __init__(self,bmin,bmax,concentration=3):
#         BoxSet.__init__(self,bmin,bmax)
#         if math.isinf(concentration):
#             self.power = 0
#         else:
#             self.power = 1.0/concentration
#             
#     def sample(self):
#         res = BoxSet.sample(self)
#         d = random.randint(0,len(res)-1)
#         (a,b) = (self.bmin[d],self.bmax[d])
#         sign = random.choice([-1,1])
#         s = math.pow(random.random(),self.power)
#         res[d] = (a+b)*0.5 + sign*(b-a)*0.5*s
#         return res
# 
# 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/example_problems/doubleintegrator.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 ..spaces.biassets import BoxBiasSet
from ..planners.problem import PlanningProblem


class DoubleIntegratorVisualizer:
    def __init__(self,workspace):
        self.base = workspace

    def toScreen(self,q):
        return q[0],q[1]

    def toState(self,x,y):
        return (x,y,0,0)

    def drawObstaclesGL(self):
        self.base.drawObstaclesGL()

    def drawVerticesGL(self,qs):
        self.base.drawVerticesGL(qs)

    def drawRobotGL(self,q):
        glColor3f(0,0,1)
        glPointSize(7.0)
        self.drawVerticesGL([q])
        l = 0.05
        glBegin(GL_LINES)
        glVertex2f(q[0],q[1])
        glVertex2f(q[0]+l*q[2],q[1]+l*q[3])

        glEnd()
