constructor working

/**
 * @file kdtree.cpp
 * Implementation of KDTree class.
 */

#include <utility>
#include <algorithm>
#include <iostream>

using namespace std;

template <int Dim>
bool KDTree<Dim>::smallerDimVal(const Point<Dim>& first,                    //probably working
                                const Point<Dim>& second, int curDim) const
{
    /**
     * @todo Implement this function!
     */
    if (first[curDim] < second[curDim]) {
      return true;
    }
    else if (first[curDim] > second[curDim]) {
      return false;
    }
    return (first < second);
}

template <int Dim>
bool KDTree<Dim>::shouldReplace(const Point<Dim>& target,               //probably working
                                const Point<Dim>& currentBest,
                                const Point<Dim>& potential) const
{
    /**
     * @todo Implement this function!
     */
    double curr_dist = 0;
    double pot_dist = 0;
    for (int i = 0; i < Dim; i++) {
      curr_dist += pow(target[i] - currentBest[i], 2);
      pot_dist += pow(target[i] - potential[i], 2);
    }
    if (pot_dist < curr_dist) {
      return true;
    }
    else if (curr_dist < pot_dist) {
      return false;
    }
    return (potential < currentBest);
}

template <int Dim>
KDTree<Dim>::KDTree(const vector<Point<Dim>>& newPoints)          //probably working
{
    /**
     * @todo Implement this function!
     */
    vector<Point<Dim>> sorted(newPoints);
    root = constructTree(0, sorted.size()-1, 0, sorted);
}

template <int Dim>
typename KDTree<Dim>::KDTreeNode * KDTree<Dim>::constructTree(int l, int r, int dim, vector<Point<Dim>>& sorted_points) { //probably working
    KDTreeNode * newNode = NULL;

    if (l > r) {
      return newNode;
    }
    int median = (l+r)/2;
    Point<Dim> point_holder = quickSelect(l, r, median, dim%Dim, sorted_points);

    newNode = new KDTreeNode(point_holder);
    newNode->left = constructTree(l, median-1, (dim+1)%Dim, sorted_points);
    newNode->right = constructTree(median+1, r, (dim+1)%Dim, sorted_points);

    //returns head of current subtree.
    return newNode;
}

template <int Dim>
Point<Dim> KDTree<Dim>::quickSelect(int l, int r, int k, int dim, vector<Point<Dim>>& sorted_points) {
    if (l == r) {
      return sorted_points[l];
    }

    int piv_idx = (l+r)/2;
    piv_idx = partition(sorted_points, l, r, piv_idx, dim);

    if (k < piv_idx) {
      return quickSelect(l, piv_idx-1, k, dim, sorted_points);
    }
    else if (k == piv_idx) {
      return sorted_points[k];
    }
    else {
      return quickSelect(piv_idx+1, r, k, dim, sorted_points);
    }
}

template <int Dim>
int KDTree<Dim>::partition(vector<Point<Dim>>& sorted_points, int l, int r, int piv_idx, int dim) {

  Point<Dim> pivValue = sorted_points[piv_idx];
  iter_swap(sorted_points.begin() + piv_idx, sorted_points.begin() + r);

  int str_idx = l;
  for (int i = l; i < r; i++) {
    if (smallerDimVal(sorted_points[i], pivValue, dim)) {
      iter_swap(sorted_points.begin() + str_idx, sorted_points.begin() + i);
      str_idx++;
    }
  }

  iter_swap(sorted_points.begin() + r, sorted_points.begin() + str_idx);
  return str_idx;
}

template <int Dim>
KDTree<Dim>::KDTree(const KDTree<Dim>& other) { //maybe working??
    /**
     * @todo Implement this function!
     */
    root = _copy(other);
}

template <int Dim>
const KDTree<Dim>& KDTree<Dim>::operator=(const KDTree<Dim>& rhs) { //maybe working??
    /**
     * @todo Implement this function!
     */
    if (&rhs != this) {
        _destroy(root);
        root = _copy(rhs);
    }
    return *this;
}

template <int Dim>
KDTree<Dim>::~KDTree() {
    /**
     * @todo Implement this function!
     */

    _destroy(root);
}

template <int Dim>
Point<Dim> KDTree<Dim>::findNearestNeighbor(const Point<Dim>& query) const
{
    /**
     * @todo Implement this function!
     */
    Point<Dim> currentBest;
    double distance = 0;
    //bool firstSide = true; not sure about this one, i'll uncomment if needed.
    //findNearestNeighborHelper(root, query, currentBest, 0, distance);
    return currentBest;
}

template <int Dim>
typename KDTree<Dim>::KDTreeNode * KDTree<Dim>::_copy(const KDTree & other) { //maybe working??
    if (other.root == NULL ) {
      return NULL;
    }

    root = new KDTreeNode(other.root.point);
    root->left = _copy(other.root.left);
    root->right = _copy(other.root.right);
    return root;
}

template <int Dim>
void KDTree<Dim>::_destroy(KDTreeNode * root) { //definitely not working :(
    if (root == NULL)  {
      return;
    }
    _destroy(root->left);
    _destroy(root->right);
    delete root;
    root = NULL;
}

template <int Dim>
double KDTree<Dim>::get_distance(const Point<Dim>& first, const Point<Dim>& second) const {
  double distance = 0;

  for (int i=0; i<Dim; i++) {
    distance += pow((first[i] - second[i]), 2);
  }

  return distance;
}