/**
   @file SearchTree.cpp
   @author Morgan McGuire, morgan@cs.williams.edu
 */

#include "SearchTree.h"
#include <ANN/ANN.h>

SearchTree::SearchTree() : tree(NULL), numDimensions(-1), balanced(false) {
}

void SearchTree::addPoint(const Vector2int16& v, const Array<ANNcoord>& point) {
    debugAssert(! balanced);
    
    if (pointArray.size() == 0) {
        // First point
        debugAssert(numDimension == -1);
        numDimensions = point.size();
    }

    debugAssertM(point.size() == numDimensions, "All feature vectors must have the same number of dimensions");

    ANNpoint pointPtr = new ANNcoord[point.size()];
    System::memcpy(pointPtr, point.getCArray(), sizeof(ANNcoord) * point.size());
    pointArray.append(pointPtr);
    sourceArray.append(v);
}


void SearchTree::balance() {
    debugAssert(! balanced);
    debugAssert(tree == NULL);

    // ANN manual, page 8.
    tree = new ANNkd_tree(pointArray.getCArray(), pointArray.size(),  numDimensions);

    balanced = true;
}


Vector2int16 SearchTree::findNearestNeighbor(const Array<ANNcoord>& features, float& fdistance) const {
    debugAssert(balanced);

    const int k = 1; // number of near neighbors to find 
    ANNidx index; // nearest neighbor array (modified) 
    ANNdist distance; // dist to near neighbors (modified) 
    double errorBound = 0.00000; // error bound 

    alwaysAssertM(features.size() == numDimensions, 
                  "Feature vector for query must have the same number "
                  "of dimensions as the data used to build the tree.");

    tree->annkSearch(const_cast<ANNcoord*>(features.getCArray()), k, &index, &distance, errorBound);
    fdistance = (float)distance;

    return sourceArray[index];
}


SearchTree::~SearchTree() {
    delete tree;
    tree = NULL;

    // Delete the feature vectors
    pointArray.deleteAll();
}
