template <int Dim>
Point<Dim> KDTree<Dim>::findNearestNeighbor(const Point<Dim>& query) const
{
    /**
     * @todo Implement this function!
     */
    KDTreeNode* oof = findNearestNeighbor(this->root, this->root, query, 0);
    std::cout<<oof->point<<std::endl;
    return oof->point;
}
template <int Dim>
typename KDTree<Dim>::KDTreeNode* KDTree<Dim>::findNearestNeighbor(KDTreeNode* root, KDTreeNode* currBest, const Point<Dim>& query, int dim) const
{
    /**
     * @todo Implement this function!
     */
    Point<Dim> best = root->point;
    if((root->left == NULL && root->right == NULL)){
      return root;
    }
    if(root->point == query)
      return root;

    else if(smallerDimVal(query, root->point, dim)){
      if(root->left != NULL)
        currBest = findNearestNeighbor(root->left, currBest, query, (dim+1)%Dim);
      else
        currBest = root;
      //second step of the john
      if(shouldReplace(query, currBest->point, best))
      {
        currBest = root;
      }
      if((root->point[dim] - query[dim])*(root->point[dim] - query[dim]) <= dist(query, currBest->point) && root->right != NULL){
        KDTreeNode* newNode = findNearestNeighbor(root->right, currBest, query, (dim+1) % Dim);
        if(shouldReplace(query, currBest->point, newNode->point))
          currBest = newNode;
      }
    }
    else{
      if(root->right != NULL)
        currBest = findNearestNeighbor(root->right, currBest, query, (dim+1)%Dim);
      else
        currBest = root;
      //second step of the john
      if(shouldReplace(query, currBest->point, best))
      {
        currBest = root;
      }
      if((root->point[dim] - query[dim])*(root->point[dim] - query[dim]) <= dist(query, currBest->point) && root->left != NULL){
        KDTreeNode* newNode = findNearestNeighbor(root->left, currBest, query, (dim+1) % Dim);
        if(shouldReplace(query, currBest->point, newNode->point))
          currBest = newNode;
      }
    }
    return currBest;
}