# Path: pomp/spaces/biassets.py
# class InfiniteBiasSet(Set):
#     """An infinite set of dimension d.
#     
#     The variable is sampled from a multivariate gaussian distribution with
#     the given concentration parameter
#     """
#     def __init__(self,d,concentration=1.0):
#         self.d = d
#         self.concentration = concentration
#         
#     def __str__(self):
#         return self.__class__.__name__+" of dim "+str(self.d)
#         
#     def dimension(self):
#         return self.d
#         
#     def bounds(self):
#         return None
#         
#     def sample(self):
#         return (np.random.randn(self.d)/self.concentration).tolist()
#         
#     def contains(self,x):
#         assert len(x) == self.d
#         return True
#     
#     def project(self,x):
#         return x
# 
#     def signedDistance(self,x):
#         return -float('inf')
#     
#     def signedDistance_gradient(self,x):
#         return np.zeros(len(x))
# 
# 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 InfiniteBiasSet
from ..planners.problem import PlanningProblem
import numpy as np

    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])

    def drawGoalGL(self,goal):
        #self.base.drawGoalGL(goal)
        pass

    def drawInterpolatorGL(self,interpolator):
        self.base.drawInterpolatorGL(interpolator)


def lqrTest():
    #double integrator test
    n = 2
    m = 1
    cspace = Geometric2DCSpace()
    cspace.box.bmin = [-2,-2]
    cspace.box.bmax = [2,2]
    cspace.addObstacle(Circle(0,0,0.5))
    uspace = InfiniteBiasSet(m,1)
    start = [0.9,1.0]

# Path: pomp/spaces/biassets.py
# class InfiniteBiasSet(Set):
#     """An infinite set of dimension d.
#     
#     The variable is sampled from a multivariate gaussian distribution with
#     the given concentration parameter
#     """
#     def __init__(self,d,concentration=1.0):
#         self.d = d
#         self.concentration = concentration
#         
#     def __str__(self):
#         return self.__class__.__name__+" of dim "+str(self.d)
#         
#     def dimension(self):
#         return self.d
#         
#     def bounds(self):
#         return None
#         
#     def sample(self):
#         return (np.random.randn(self.d)/self.concentration).tolist()
#         
#     def contains(self,x):
#         assert len(x) == self.d
#         return True
#     
#     def project(self,x):
#         return x
# 
#     def signedDistance(self,x):
#         return -float('inf')
#     
#     def signedDistance_gradient(self,x):
#         return np.zeros(len(x))
# 
# 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/lqr.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 InfiniteBiasSet
from ..planners.problem import PlanningProblem
import numpy as np


    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])

    def drawGoalGL(self,goal):
        #self.base.drawGoalGL(goal)
        pass

    def drawInterpolatorGL(self,interpolator):
        self.base.drawInterpolatorGL(interpolator)


def lqrTest():
    #double integrator test
    n = 2
    m = 1
    cspace = Geometric2DCSpace()
    cspace.box.bmin = [-2,-2]
    cspace.box.bmax = [2,2]
    cspace.addObstacle(Circle(0,0,0.5))
    uspace = InfiniteBiasSet(m,1)
    start = [0.9,1.0]

    goal = InfiniteBiasSet(n,2)