# 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
        if self.root == None: return []
        closest,dmin = self._nearest(self.root,x,float('inf'),filter)
        if closest is None:
            input()
        return closest

    def _knearest(self,node,x,res,filter=None):
        if node.splitvalue == None:
            #base case, it's a leaf
            for p in node.points:
                if filter != None and filter(*p):
                    continue
                d = self.metric(p[0],x)
                res.tryadd(p,d)
            return res
        #recursive case, it's a parent
        dhi = 0
        dlo = 0
        if self.partitionFn(node.splitdim,node.splitvalue,x) < 0:
            dlo = 0
            dhi = self.minDistanceFn(node.splitdim,node.splitvalue,x)
        else:
            dhi = 0
            dlo = self.minDistanceFn(node.splitdim,node.splitvalue,x)

        if dhi > res.maximum_distance():  #only check left
            res = self._knearest(node.left,x,res,filter)
            return res
        elif dlo > res.maximum_distance(): #only check right
            res = self._knearest(node.right,x,res,filter)

# 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

        if self.root == None: return []
        closest,dmin = self._nearest(self.root,x,float('inf'),filter)
        if closest is None:
            input()
        return closest

    def _knearest(self,node,x,res,filter=None):
        if node.splitvalue == None:
            #base case, it's a leaf
            for p in node.points:
                if filter != None and filter(*p):
                    continue
                d = self.metric(p[0],x)
                res.tryadd(p,d)
            return res
        #recursive case, it's a parent
        dhi = 0
        dlo = 0
        if self.partitionFn(node.splitdim,node.splitvalue,x) < 0:
            dlo = 0
            dhi = self.minDistanceFn(node.splitdim,node.splitvalue,x)
        else:
            dhi = 0
            dlo = self.minDistanceFn(node.splitdim,node.splitvalue,x)

        if dhi > res.maximum_distance():  #only check left
            res = self._knearest(node.left,x,res,filter)
            return res
        elif dlo > res.maximum_distance(): #only check right
            res = self._knearest(node.right,x,res,filter)

            return res