# Path: pomp/klampt/cspace.py
# class CSpace:
#     """Used alongside MotionPlan to define a configuration space for
#     motion planning.
# 
#     To define a custom CSpace, subclasses will need to override:
#         
#     - feasible(x): returns true if the vector x is in the feasible space
#     - *sample(): returns a new vector x from a superset of the feasible space
#     - *sampleneighborhood(c,r): returns a new vector x from a neighborhood of c
#       with radius r
#     - *visible(a,b): returns true if the path between a and b is feasible
#     - *distance(a,b): return a distance between a and b
#     - *interpolate(a,b,u): interpolate between a, b with parameter u
#     (* indicates an optional override.)
# 
#     To avoid memory leaks, CSpace.close() or motionplanning.destroy() must
#     be called when you are done.  (The latter deallocates all previously
#     created cspaces and planners)
# 
#     If sample() is not defined, then subclasses should set self.bound to be a
#     list of pairs defining an axis-aligned bounding box.  The setBounds method
#     is a convenient way of defining this.
# 
#     If visible() is not defined, then paths are checked by subdivision, with
#     the collision tolerance self.eps.
# 
#     To help planners know a bit more about the CSpace, you can set the
#     self.properties member to a map from strings to values.  Useful values
#     are
#     
#     - euclidean (0 or 1): indicates a euclidean space
#     - geodesic (0 or 1): indicates whether the interpolation is along
#       geodesics.
#     - volume (real): a size of the space
#     - minimum, maximum (real array): bounds on the space
#     - metric (string): the metric used by distance, can be "euclidean",
#       "weighted euclidean", "manhattan", "weighted manhattan", "Linf", etc.
#     - metricWeights (real array): weights used by weighted metrics
#     
#     volume is necessary for Lazy PRM* and Lazy RRG* to work.
#     
#     metricWeights is necessary for KD-tree point location structures to work,
#     for FMM methods to work, etc.
# 
#     minimum/maximum can be used by grid-based methods (optional for FMM, FMM*).
#     """
#     def __init__(self):
#         self.cspace = None
#         self.eps = 1e-3
#         self.bound = [(0,1)]
#         self.properties = {}
# 
#     def setBounds(self,bounds):
#         """Convenience function: sets the sampling bound and the
#         space properties in one line."""
#         self.bounds = bounds
#         self.properties["minimum"] = [b[0] for b in bounds]
#         self.properties["maximum"] = [b[1] for b in bounds]
# 
#     def close(self):
#         """This method must be called to free the memory associated with the
#         planner.  Alternatively, motionplanning.destroy() can be called to
#         free all previously constructed CSpace and MotionPlan objects."""
#         if self.cspace != None:
#             self.cspace.destroy()
#             self.cspace = None
#     
#     def setup(self):
#         """Called internally by the MotionPlan class to set up planning
#         hooks."""
#         assert self.cspace == None
#         self.cspace = motionplanning.CSpaceInterface()
#         if hasattr(self,'feasible'):
#             self.cspace.setFeasibility(getattr(self,'feasible'))
#         else:
#             raise 'Need feasible method'
#         if hasattr(self,'visible'):
#             self.cspace.setVisibility(getattr(self,'visible'))
#         else:
#             self.cspace.setVisibilityEpsilon(self.eps)
#         if hasattr(self,'sample'):
#             self.cspace.setSampler(getattr(self,'sample'))
#         else:
#             raise 'Need sample method'
#         if hasattr(self,'sampleneighborhood'):
#             self.cspace.setNeighborhoodSampler(getattr(self,'sampleneighborhood'))
#         if hasattr(self,'distance'):
#             self.cspace.setDistance(getattr(self,'distance'))
#         if hasattr(self,'interpolate'):
#             self.cspace.setInterpolate(getattr(self,'interpolate'))
#         for (k,v) in self.properties:
#             if isinstance(v,(list,tuple)):
#                 self.cspace.setPropety(k," ".join([str(item) for item in v]))
#             else:
#                 self.cspace.setProperty(k,str(v))
# 
#     def sample(self):
#         """Overload this to define a nonuniform sampler.
#         By default, it will sample from the axis-aligned bounding box
#         defined by self.bound. To define a different domain, set self.bound
#         to the desired bound.
#         """
#         return [random.uniform(*b) for b in self.bound]
# 
#     def feasible(self,x):
#         """Overload this to define your new feasibility test"""
#         return all(a<=xi<=b for (xi,(a,b)) in zip(x,self.bound))


from six import iteritems
from builtins import range
from .cspace import CSpace
import math
import time

    def stats(self,f):
        """Returns a dictionary describing the statistics of f"""
        res = dict()
        res['average cost']=f._sum_cost/(f._num_pass+f._num_fail)
        res['pass rate']=float(f._num_pass)/(f._num_pass+f._num_fail)
        res['evaluations']=f._num_pass+f._num_fail
        res['max distance']=f._max_dist
        res['average distance']=float(f._sum_dist)/(f._num_pass+f._num_fail)
        return res

    def init_stats(self,f,d):
        """Given a dictionary returned by a stats() call, fills in the
        appropriate statistics of f"""
        f._sum_cost = d['average cost']*d['evaluations']
        f._num_pass = d['evaluations']*d['pass rate']
        f._num_fail = d['evaluations']*(1.0-d['pass rate'])
        f._max_dist = d['max distance']
        f._sum_dist = d['average distance']*d['evaluations']




class AdaptiveCSpace(CSpace,AdaptiveZeroTester):
    """A cspace with an adaptive feasibility checker.  Subclasses
    fill out feasibility tests using addFeasibleTest (binary conditions)
    or addFeasibleComp (inequalities).  The cspace will then learn the
    characteristics of each test and find the (near) optimal testing
    order."""
    def __init__(self):

# Path: pomp/klampt/cspace.py
# class CSpace:
#     """Used alongside MotionPlan to define a configuration space for
#     motion planning.
# 
#     To define a custom CSpace, subclasses will need to override:
#         
#     - feasible(x): returns true if the vector x is in the feasible space
#     - *sample(): returns a new vector x from a superset of the feasible space
#     - *sampleneighborhood(c,r): returns a new vector x from a neighborhood of c
#       with radius r
#     - *visible(a,b): returns true if the path between a and b is feasible
#     - *distance(a,b): return a distance between a and b
#     - *interpolate(a,b,u): interpolate between a, b with parameter u
#     (* indicates an optional override.)
# 
#     To avoid memory leaks, CSpace.close() or motionplanning.destroy() must
#     be called when you are done.  (The latter deallocates all previously
#     created cspaces and planners)
# 
#     If sample() is not defined, then subclasses should set self.bound to be a
#     list of pairs defining an axis-aligned bounding box.  The setBounds method
#     is a convenient way of defining this.
# 
#     If visible() is not defined, then paths are checked by subdivision, with
#     the collision tolerance self.eps.
# 
#     To help planners know a bit more about the CSpace, you can set the
#     self.properties member to a map from strings to values.  Useful values
#     are
#     
#     - euclidean (0 or 1): indicates a euclidean space
#     - geodesic (0 or 1): indicates whether the interpolation is along
#       geodesics.
#     - volume (real): a size of the space
#     - minimum, maximum (real array): bounds on the space
#     - metric (string): the metric used by distance, can be "euclidean",
#       "weighted euclidean", "manhattan", "weighted manhattan", "Linf", etc.
#     - metricWeights (real array): weights used by weighted metrics
#     
#     volume is necessary for Lazy PRM* and Lazy RRG* to work.
#     
#     metricWeights is necessary for KD-tree point location structures to work,
#     for FMM methods to work, etc.
# 
#     minimum/maximum can be used by grid-based methods (optional for FMM, FMM*).
#     """
#     def __init__(self):
#         self.cspace = None
#         self.eps = 1e-3
#         self.bound = [(0,1)]
#         self.properties = {}
# 
#     def setBounds(self,bounds):
#         """Convenience function: sets the sampling bound and the
#         space properties in one line."""
#         self.bounds = bounds
#         self.properties["minimum"] = [b[0] for b in bounds]
#         self.properties["maximum"] = [b[1] for b in bounds]
# 
#     def close(self):
#         """This method must be called to free the memory associated with the
#         planner.  Alternatively, motionplanning.destroy() can be called to
#         free all previously constructed CSpace and MotionPlan objects."""
#         if self.cspace != None:
#             self.cspace.destroy()
#             self.cspace = None
#     
#     def setup(self):
#         """Called internally by the MotionPlan class to set up planning
#         hooks."""
#         assert self.cspace == None
#         self.cspace = motionplanning.CSpaceInterface()
#         if hasattr(self,'feasible'):
#             self.cspace.setFeasibility(getattr(self,'feasible'))
#         else:
#             raise 'Need feasible method'
#         if hasattr(self,'visible'):
#             self.cspace.setVisibility(getattr(self,'visible'))
#         else:
#             self.cspace.setVisibilityEpsilon(self.eps)
#         if hasattr(self,'sample'):
#             self.cspace.setSampler(getattr(self,'sample'))
#         else:
#             raise 'Need sample method'
#         if hasattr(self,'sampleneighborhood'):
#             self.cspace.setNeighborhoodSampler(getattr(self,'sampleneighborhood'))
#         if hasattr(self,'distance'):
#             self.cspace.setDistance(getattr(self,'distance'))
#         if hasattr(self,'interpolate'):
#             self.cspace.setInterpolate(getattr(self,'interpolate'))
#         for (k,v) in self.properties:
#             if isinstance(v,(list,tuple)):
#                 self.cspace.setPropety(k," ".join([str(item) for item in v]))
#             else:
#                 self.cspace.setProperty(k,str(v))
# 
#     def sample(self):
#         """Overload this to define a nonuniform sampler.
#         By default, it will sample from the axis-aligned bounding box
#         defined by self.bound. To define a different domain, set self.bound
#         to the desired bound.
#         """
#         return [random.uniform(*b) for b in self.bound]
# 
#     def feasible(self,x):
#         """Overload this to define your new feasibility test"""
#         return all(a<=xi<=b for (xi,(a,b)) in zip(x,self.bound))


# Path: pomp/klampt/cspaceutils.py
from six import iteritems
from builtins import range
from .cspace import CSpace
import math
import time


    def stats(self,f):
        """Returns a dictionary describing the statistics of f"""
        res = dict()
        res['average cost']=f._sum_cost/(f._num_pass+f._num_fail)
        res['pass rate']=float(f._num_pass)/(f._num_pass+f._num_fail)
        res['evaluations']=f._num_pass+f._num_fail
        res['max distance']=f._max_dist
        res['average distance']=float(f._sum_dist)/(f._num_pass+f._num_fail)
        return res

    def init_stats(self,f,d):
        """Given a dictionary returned by a stats() call, fills in the
        appropriate statistics of f"""
        f._sum_cost = d['average cost']*d['evaluations']
        f._num_pass = d['evaluations']*d['pass rate']
        f._num_fail = d['evaluations']*(1.0-d['pass rate'])
        f._max_dist = d['max distance']
        f._sum_dist = d['average distance']*d['evaluations']




class AdaptiveCSpace(CSpace,AdaptiveZeroTester):
    """A cspace with an adaptive feasibility checker.  Subclasses
    fill out feasibility tests using addFeasibleTest (binary conditions)
    or addFeasibleComp (inequalities).  The cspace will then learn the
    characteristics of each test and find the (near) optimal testing
    order."""
    def __init__(self):

        CSpace.__init__(self)