#ifndef _BLASR_PRIORITY_SEARCH_TREE_IMPL_HPP_
#define _BLASR_PRIORITY_SEARCH_TREE_IMPL_HPP_

/*
 * Define a priority search tree on a point that implements
 * the following interface:
 *
 * int T_point::GetIndex()
 *    - Return the index of the point in a list of points.
 * int T_point::GetKey()
 *    - Return the key value that the points are sorted by (x-value in a 2D query)
 * int T_point::GetValue()
 *    - Return the value of a point.
 * int T_point::SetValue(int value)
 *    - sets the value of a point.
 *
 * This class implements a query FindMax(key), which returns
 * the index of the point with greatest value of all points with key [0...key).
 *
 *
 */
template <typename T_Point>
PSTVertex<T_Point>::PSTVertex()
{
    isALeaf = 0;
    leftChildIndex = 0;
    rightChildIndex = 0;
    maxScoreNode = -1;
    maxKey = 0;
    medianKey = 0;
    pointIndex = 0;
}

template <typename T_Point>
PrioritySearchTree<T_Point>::PrioritySearchTree()
{
    treePtr = NULL;
}

template <typename T_Point>
int PrioritySearchTree<T_Point>::GetMedianIndex(int start, int end)
{
    return (end + start) / 2;
}

template <typename T_Point>
inline KeyType PrioritySearchTree<T_Point>::CreateTree(std::vector<T_Point> &points, int start,
                                                       int end, unsigned int &iterativeIndex)
{
    assert(iterativeIndex < (*treePtr).size());

    //
    // Look to see if this vertex is the parent of a leaf
    // -- when there are only two points below.
    //
    int medianIndex = GetMedianIndex(start, end);
    int curVertexIndex = iterativeIndex;
    (*treePtr)[curVertexIndex].medianKey = points[medianIndex].GetKey();

    if (end == start) {
        // No children for this node, done.
        (*treePtr)[curVertexIndex].pointIndex = start;
        return (*treePtr)[curVertexIndex].medianKey;
    }

    //
    // Check to see if the current
    // node is a leaf node.  No recursion on this node.
    //
    if (end - start == 1) {
        (*treePtr)[curVertexIndex].isALeaf = 1;
        (*treePtr)[curVertexIndex].medianKey = points[start].GetKey();
        (*treePtr)[curVertexIndex].pointIndex = start;
        //
        // Return the key of this vertex.  The parent
        // will know what to do with it.  If this is
        // a left child, the parent will use the key to
        // distinguish what is on the left side of the branches.
        // If it is the right side of a (*treePtr), it is ignored.
        //
        return (*treePtr)[curVertexIndex].medianKey;
    } else {
        //
        // This vertex contains at least two children, so it is not
        // a leaf.  Recurse assigning leaves.
        //
        (*treePtr)[curVertexIndex].isALeaf = 0;
        (*treePtr)[curVertexIndex].leftChildIndex = ++iterativeIndex;
        KeyType leftTreeKey, rightTreeKey;
        leftTreeKey = CreateTree(points, start, medianIndex, iterativeIndex);

        //
        // The leftTreeKey separates the branches BELOW this vertex.
        //
        (*treePtr)[curVertexIndex].medianKey = leftTreeKey;

        (*treePtr)[curVertexIndex].rightChildIndex = ++iterativeIndex;
        rightTreeKey = CreateTree(points, medianIndex, end, iterativeIndex);
        //
        // The rightTreeKey will separate the parent's left tree from the right.
        //
        (*treePtr)[curVertexIndex].maxKey = rightTreeKey;
        return rightTreeKey;
    }
}

template <typename T_Point>
int PrioritySearchTree<T_Point>::FindIndexOfMaxPoint(int curVertexIndex,
                                                     std::vector<T_Point> &points, KeyType maxKey,
                                                     int &maxPointValue, int &maxPointIndex)
{
    //
    // Attempt to find the leaf vertex beneath this vertex that has
    // the largest score, with a key less than max key.
    //
    // On return:
    //   Return 1 if a value is assigned to maxPointValue, 0 otherwise.
    //   If a value is assigned to maxPointValue, this sets:
    //      maxPointValue is the score of the maximum point.
    //      maxPointIndex the index of the point in 'points' that has
    //      the maximum score.
    //

    //
    // The vertex at curVertexIndex has a max score node beneath it,
    // if it has been initialized.  If the maxScoreNode has a key less
    // than the current maxKey, then we know the maximum value is
    // contained beneath this vertex, AND that its key is within the
    // range in the rage maximum query.
    // That means that there is no need to continue the search below here.
    //
    if ((*treePtr)[curVertexIndex].maxScoreNode == -1) {
        return 0;
    }
    T_Point thisPoint = points[(*treePtr)[curVertexIndex].maxScoreNode];
    if (thisPoint.GetKey() < maxKey) {
        if (thisPoint.GetScore() >= maxPointValue) {
            maxPointValue = thisPoint.GetScore();
            maxPointIndex = (*treePtr)[curVertexIndex].maxScoreNode;
            return 1;
        } else {
            return 0;
        }
    }
    //
    // Otherwise, the maximum scoring node beneath this node has a
    // key greater than the max key. That means that the search must
    // continue for the maximum value node with a key less than 'maxKey'.
    //
    // The search has two cases:
    // First, if the median key of this node is greater than the maxKey,
    // all keys on the right side of the tree are greater than maxKey,
    // so do not search there.
    //
    // If the median key of this node si less than maxKey, there may
    // be a node on the left or right child of the current node with
    // a maximum key.  Search both to the left and right.
    //
    else {
        if (!(*treePtr)[curVertexIndex].isALeaf) {
            if (maxKey <= (*treePtr)[curVertexIndex].medianKey) {
                return FindIndexOfMaxPoint((*treePtr)[curVertexIndex].leftChildIndex, points,
                                           maxKey, maxPointValue, maxPointIndex);
            } else {
                int foundValueLeft, foundValueRight;
                foundValueLeft = FindIndexOfMaxPoint((*treePtr)[curVertexIndex].leftChildIndex,
                                                     points, maxKey, maxPointValue, maxPointIndex);

                foundValueRight = FindIndexOfMaxPoint((*treePtr)[curVertexIndex].rightChildIndex,
                                                      points, maxKey, maxPointValue, maxPointIndex);
                return (foundValueLeft or foundValueRight);
            }
        } else {
            //
            // The current node is a leaf node, but due to the condition
            // from before, its key is greater than or equal to the max key,
            // therefore its score cannot be used for the maximum score.
            // Returning 0 here signifies that this search-branch did not
            // turn up any candidates for
            // the maximum scoring node.
            return 0;
        }
    }
}

template <typename T_Point>
void PrioritySearchTree<T_Point>::CreateTree(std::vector<T_Point> &points,
                                             std::vector<PSTVertex<T_Point> > *bufTreePtr)
{
    //
    // Precondition: points is sorted according to key.
    //
    //
    // The tree is a binary tree containing all the points.  The
    // perfectly balanced tree is of maximum size points.size()-1,
    // so go ahead and preallocate that now.
    //
    if (bufTreePtr != NULL) {
        treePtr = bufTreePtr;
    } else {
        treePtr = &tree;
    }
    treePtr->resize((points.size() * 2) - 1);
    unsigned int curVertexIndex = 0;
    CreateTree(points, 0, points.size(), curVertexIndex);
}

//
// Implement the tree as an array of interior nodes.
// Since there is already space allocated for the
//
template <typename T_Point>
int PrioritySearchTree<T_Point>::FindPoint(KeyType pointKey, int curVertexIndex,
                                           int &pointVertexIndex)
{

    if ((*treePtr)[curVertexIndex].isALeaf) {
        pointVertexIndex = curVertexIndex;
        return (*treePtr)[curVertexIndex].medianKey == pointKey;
    } else {
        if (pointKey <= (*treePtr)[curVertexIndex].medianKey) {
            return FindPoint(pointKey, (*treePtr)[curVertexIndex].leftChildIndex, pointVertexIndex);
        } else {
            return FindPoint(pointKey, (*treePtr)[curVertexIndex].rightChildIndex,
                             pointVertexIndex);
        }
    }
}

template <typename T_Point>
void PrioritySearchTree<T_Point>::Activate(std::vector<T_Point> &points, int pointIndex)
{

    int pointScore = points[pointIndex].GetScore();
    // Now, update the pMax scores in the (*treePtr).

    int curVertexIndex = 0;
    KeyType pointKey = points[pointIndex].GetKey();
    unsigned int itIndex = 0;
    while (pointIndex != -1 and (*treePtr)[curVertexIndex].isALeaf == 0) {
        assert(itIndex < (*treePtr).size());
        int nodeIndex = (*treePtr)[curVertexIndex].maxScoreNode;
        if (nodeIndex == -1 or points[nodeIndex].GetScore() < pointScore) {
            (*treePtr)[curVertexIndex].maxScoreNode = pointIndex;
            pointIndex = nodeIndex;
        }

        if (pointKey <= (*treePtr)[curVertexIndex].medianKey) {
            curVertexIndex = (*treePtr)[curVertexIndex].leftChildIndex;
        } else {
            curVertexIndex = (*treePtr)[curVertexIndex].rightChildIndex;
        }

        // Keep track of the number of times this loop is executed... an
        // infinite loop will bomb.
        ++itIndex;
    }
}

template <typename T_Point>
int PrioritySearchTree<T_Point>::FindIndexOfMaxPoint(std::vector<T_Point> &points,
                                                     KeyType maxPointKey, int &maxPointIndex)
{

    // start at the root
    int curVertexIndex = 0;
    if ((*treePtr)[curVertexIndex].maxScoreNode == -1) {
        //
        // This case can only be hit if none of the points have been
        // activated.
        //
        return 0;
    }
    int maxPointValue = 0;
    return FindIndexOfMaxPoint(0, points, maxPointKey, maxPointValue, maxPointIndex);
}

#endif