# Path: pomp/spaces/metric.py
# def euclideanMetric(a,b):
# def L2Metric(a,b):
# def L1Metric(a,b):
# def LinfMetric(a,b):
#     def __init__(self,w):
#     def __call__(self,a,b):
#     def __init__(self,p):
#     def __call__(self,a,b):
# class WeightedEuclideanMetric:
# class LpMetric:
# 
# 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 six import iteritems
from .knn import *
from ..spaces import metric
from ..klampt import so2
import math
                    tempp2[d] = math.pi+value
                    return min(self.metric(tempp,x),self.metric(tempp2,x))
                return self.metric(tempp,x)
            def partitionFn(d,value,x):
                #HACK: for pendulum
                if SO2_HACK and d == 0:
                    math.copysign(1,so2.diff(x[d],value))
                #HACK: for SE2
                if SE2_HACK and d == 2:
                    math.copysign(1,so2.diff(x[d],value))
                return math.copysign(1,x[d]-value)
            self.minDistanceFn = minDistFn
            self.partitionFn = partitionFn

        self.maxPointsPerNode = 10
        self.maxDepth = 0
        self.numNodes = 0

    def locate(self,point):
        """Find the Node which contains a point"""
        return self._locate(self.root,point)

    def _locate(self,node,point):
        if node.splitvalue == None: return node
        if self.partitionFn(node.splitdim,node.splitvalue,point) < 0: return self._locate(node.left,point)
        else: return self._locate(node.right,point)

    def set(self,points,data):
        """Set the KD tree to contain a list of points.  O(log(n)*n*d)
        running time."""

# Path: pomp/spaces/metric.py
# def euclideanMetric(a,b):
# def L2Metric(a,b):
# def L1Metric(a,b):
# def LinfMetric(a,b):
#     def __init__(self,w):
#     def __call__(self,a,b):
#     def __init__(self,p):
#     def __call__(self,a,b):
# class WeightedEuclideanMetric:
# class LpMetric:
# 
# 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/structures/kdtree.py
from six import iteritems
from .knn import *
from ..spaces import metric
from ..klampt import so2
import math

                    tempp2[d] = math.pi+value
                    return min(self.metric(tempp,x),self.metric(tempp2,x))
                return self.metric(tempp,x)
            def partitionFn(d,value,x):
                #HACK: for pendulum
                if SO2_HACK and d == 0:
                    math.copysign(1,so2.diff(x[d],value))
                #HACK: for SE2
                if SE2_HACK and d == 2:
                    math.copysign(1,so2.diff(x[d],value))
                return math.copysign(1,x[d]-value)
            self.minDistanceFn = minDistFn
            self.partitionFn = partitionFn

        self.maxPointsPerNode = 10
        self.maxDepth = 0
        self.numNodes = 0

    def locate(self,point):
        """Find the Node which contains a point"""
        return self._locate(self.root,point)

    def _locate(self,node,point):
        if node.splitvalue == None: return node
        if self.partitionFn(node.splitdim,node.splitvalue,point) < 0: return self._locate(node.left,point)
        else: return self._locate(node.right,point)

    def set(self,points,data):
        """Set the KD tree to contain a list of points.  O(log(n)*n*d)
        running time."""

        self.maxDepth = 0