# 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.objective import *
from ..spaces.statespace import *
from ..spaces.configurationspace import *
from ..spaces.edgechecker import *
from ..spaces.metric import *
from ..planners.problem import PlanningProblem


class FlappyControlSpace(ControlSpace):
    def __init__(self,flappy):
        self.flappy = flappy
    def configurationSpace(self):
        return self.flappy.configurationSpace()
    def controlSet(self,x):
        return MultiSet(TimeBiasSet(self.flappy.time_range,self.flappy.controlSet()),self.flappy.controlSet())
        #return MultiSet(BoxSet([0],[self.flappy.time_range]),self.flappy.controlSet())
    def nextState(self,x,u):
        return self.eval(x,u,1.0)
    def eval(self,x,u,amount):
        x_i,y_i,vy_i = x
        t,thrust = u
        tc = t*amount
        #instantaneous version
        #net_acceler = self.flappy.gravity
        #vy_i += thrust*self.flappy.thrust
        #yilun's version
        net_acceler = self.flappy.gravity + thrust*self.flappy.thrust
        return [x_i+self.flappy.v_x*tc, 
                y_i+vy_i*tc+0.5*net_acceler*(tc**2), 
                vy_i+net_acceler*tc]

    def interpolator(self,x,u):

# 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/flappy.py
from OpenGL.GL import *
from .geometric import *
from ..spaces.objective import *
from ..spaces.statespace import *
from ..spaces.configurationspace import *
from ..spaces.edgechecker import *
from ..spaces.metric import *
from ..planners.problem import PlanningProblem


class FlappyControlSpace(ControlSpace):
    def __init__(self,flappy):
        self.flappy = flappy
    def configurationSpace(self):
        return self.flappy.configurationSpace()
    def controlSet(self,x):
        return MultiSet(TimeBiasSet(self.flappy.time_range,self.flappy.controlSet()),self.flappy.controlSet())
        #return MultiSet(BoxSet([0],[self.flappy.time_range]),self.flappy.controlSet())
    def nextState(self,x,u):
        return self.eval(x,u,1.0)
    def eval(self,x,u,amount):
        x_i,y_i,vy_i = x
        t,thrust = u
        tc = t*amount
        #instantaneous version
        #net_acceler = self.flappy.gravity
        #vy_i += thrust*self.flappy.thrust
        #yilun's version
        net_acceler = self.flappy.gravity + thrust*self.flappy.thrust
        return [x_i+self.flappy.v_x*tc, 
                y_i+vy_i*tc+0.5*net_acceler*(tc**2), 
                vy_i+net_acceler*tc]

    def interpolator(self,x,u):

        return LambdaInterpolator(lambda s:self.eval(x,u,s),self.configurationSpace(),10)
