/*
 * kd implementation
 *
 * Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
 *
 * Released under the GPL version 3.
 *
 */

/** @file 
 *  @brief An optimized k-d tree implementation
 *  @author Remus Dumitru. Jacobs University Bremen, Germany
 *  @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany
 *  @author Andreas Nuechter. Jacobs University Bremen, Germany.
 *  @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany.
 *  @author Thomas Escher Inst. of CS, University of Osnabrueck, Germany.
 */

#ifdef _MSC_VER
#define  _USE_MATH_DEFINES
#endif

#include "slam6d/kd.h"
#include "slam6d/globals.icc"

#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <algorithm>
using std::swap;
#include <cmath>
#include <cstring>
#include <limits>
#include <vector>

// KDtree class static variables
template<class PointData, class AccessorData, class AccessorFunc>
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];

/**
 * Constructor
 *
 * Create a KD tree from the points pointed to by the array pts
 *
 * @param pts 3D array of points
 * @param n number of points
 */
KDtree::KDtree(double **pts, int n)
{
    create(Void(), pts, n);
}

KDtree::~KDtree()
{
}

/**
 * Finds the closest point within the tree,
 * wrt. the point given as first parameter.
 * @param _p point
 * @param maxdist2 maximal search distance.
 * @param threadNum Thread number, for parallelization
 * @return Pointer to the closest point
 */
double *KDtree::FindClosest(double *_p,
					   double maxdist2,
					   int threadNum) const
{
  params[threadNum].closest = 0;
  params[threadNum].closest_d2 = maxdist2;
  params[threadNum].p = _p;
  _FindClosest(Void(), threadNum);
  return params[threadNum].closest;
}

double *KDtree::FindClosestAlongDir(double *_p,
							 double *_dir,
							 double maxdist2,
							 int threadNum) const
{
  params[threadNum].closest = NULL;
  params[threadNum].closest_d2 = maxdist2;
  params[threadNum].p = _p;
  params[threadNum].dir = _dir;
  _FindClosestAlongDir(Void(), threadNum);
  return params[threadNum].closest;
}

vector<Point> KDtree::kNearestNeighbors(double *_p,
								int k,
								double sqRad2,
								int threadNum) const
{
    vector<Point> result;
    params[threadNum].closest = 0;
    params[threadNum].closest_d2 = sqRad2;
    params[threadNum].p = _p;
    params[threadNum].heap.clear();
    _KNNSearch(Void(), threadNum);

    while (k > 0 && params[threadNum].heap.empty() == false) {
        Point pt = params[threadNum].heap.front().first;
        result.push_back(pt);
        std::pop_heap(params[threadNum].heap.begin(), params[threadNum].heap.end(), PointCompare());
        params[threadNum].heap.pop_back();
        k--;
    }

    return result;
}

vector<Point> KDtree::fixedRangeSearch(double *_p,
							    double sqRad2,
							    int threadNum) const
{
    vector<Point> result;
    params[threadNum].closest = 0;
    params[threadNum].closest_d2 = sqRad2;
    params[threadNum].p = _p;
    params[threadNum].heap.clear();
    _FixedRangeSearch(Void(), threadNum);

    for (vector<std::pair<Point, double> >::iterator it = params[threadNum].heap.begin(); it != params[threadNum].heap.end(); ++it) {
        result.push_back(it->first);
    }

    return result;
}