/*
 * (C) Copyright Ian D. Romanick 2007
 * All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * on the rights to use, copy, modify, merge, publish, distribute, sub
 * license, and/or sell copies of the Software, and to permit persons to whom
 * the Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice (including the next
 * paragraph) shall be included in all copies or substantial portions of the
 * Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.  IN NO EVENT SHALL
 * AUTHORS, COPYRIGHT HOLDERS, AND/OR THEIR SUPPLIERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
 * USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <stdio.h>
#include <float.h>
#include <assert.h>
#include "rtree.h"
#include <string.h>
#include <algorithm>


#define MIN(a,b)  (((a) < (b)) ? (a) : (b))
#define MAX(a,b)  (((a) > (b)) ? (a) : (b))

#define M (int)(.4 * MAX_CHILDREN)

bool compare_node_x(const void *a, const void *b)
{
    const rtree_node *av = (rtree_node *)a;
    const rtree_node *bv = (rtree_node *)b;

    return av->box.compare_x(bv->box);
}


bool compare_node_y(const void *a, const void *b)
{
    const rtree_node *av = (rtree_node *)a;
    const rtree_node *bv = (rtree_node *)b;

    return av->box.compare_y(bv->box);
}


bool compare_node_z(const void *a, const void *b)
{
    const rtree_node *av = (rtree_node *)a;
    const rtree_node *bv = (rtree_node *)b;

    return av->box.compare_z(bv->box);
}


volume_rtree::volume_rtree(void)
{
    root = new volume_node;
}


void volume_rtree::insert(const idr_vec4 &position, const void *data)
{
    aabb add(position);

    volume_node *const split = root->insert(add, data);
    
    if (split) {
        volume_node *old_root = root;
        
        root = new volume_node;
        root->children[0].n = old_root;
        root->children[1].n = split;
        root->leaf = false;
        root->used_children = 2;

        root->box = old_root->box;
        root->box.intersect_with(split->box);
    }
}


float volume_node::surface_increase(const aabb &add_box) const
{
    aabb temp = box;

    temp.intersect_with(add_box);

    return temp.surface_area() - box.surface_area();
}


float volume_node::expansion(const aabb &add_box) const
{
    aabb temp = box;

    temp.intersect_with(add_box);

    return temp.volume() - box.volume();
}


float volume_node::child_cost(const aabb &add_box) const
{
    float cost = FLT_MAX;

    for (unsigned j = 0; j < used_children; j++) {
        const idr_vec4 v(add_box.min_corner() 
                         - children[j].child->box.min_corner());
        const float dist_squared = v.dot4(v);

        cost = MIN(dist_squared, cost);
    }

    return cost;
}


unsigned volume_node::choose_subtree(const aabb &add_box) const
{
    unsigned possible_children[MAX_CHILDREN];
    unsigned num_possible_children = 0;

    /* If the child pointers point to leaves:
     *     Choose the child point-to-point distance increases the least.
     */
    if (children[0].n->is_leaf()) {
        float best = FLT_MAX;

        for (unsigned i = 0; i < used_children; i++) {
            float j_dist = FLT_MAX;
            volume_node *const child = children[i].n;

            j_dist = child->child_cost(add_box);
            if (j_dist < best) {
                possible_children[0] = i;
                num_possible_children = 1;

                best = j_dist;
            }
            else if (j_dist == best) {
                possible_children[num_possible_children] = i;
                num_possible_children++;
            }
        }
    }
    else {
        for (unsigned i = 0; i < used_children; i++) {
            possible_children[i] = i;
        }

        num_possible_children = used_children;
    }

    if (num_possible_children == 1) {
        return possible_children[0];
    }

    /* If the child pointers do not point to leaves (or to resolve ties):
     *     Choose the child whose volume increases the least.
     *
     *     Resolve ties by choosing the child whose surface area increases
     *     the least.
     *
     *     Resolve second ties by choosing the child with the smallest initial
     *     volume.
     */
    float best_expansion = FLT_MAX;
    unsigned best = 0;

    for (unsigned i = 0; i < num_possible_children; i++) {
        const unsigned j = possible_children[i];
        const float expansion = children[j].n->expansion(add_box);

        if (expansion < best_expansion) {
            best_expansion = expansion;
            best = j;
        }
        else if (expansion == best_expansion) {
            const float best_surface = 
                children[best].n->surface_increase(add_box);
            const float surface = 
                children[best].n->surface_increase(add_box);

            if ((surface < best_surface)
                || ((surface == best_surface)
                    && (children[j].n->volume() < children[best].n->volume()))) {
                best_expansion = expansion;
                best = j;
            }
        }
    }

    return best;
}


void volume_node::get_AABB_for_groups(unsigned first_group_size, 
                                      aabb *first_group_AABB,
                                      aabb *second_group_AABB) const
{
    unsigned i;

    first_group_AABB->clear();
    second_group_AABB->clear();

    for (i = 0; i < first_group_size; i++) {
        first_group_AABB->intersect_with(children[i].n->box);
    }

    for (/* empty */; i < used_children; i++) {
        second_group_AABB->intersect_with(children[i].n->box);
    }
}


float volume_node::margin_value(unsigned first_group_size) const
{
    aabb first_group_AABB;
    aabb second_group_AABB;


    get_AABB_for_groups(first_group_size, & first_group_AABB,
                        & second_group_AABB);

    return first_group_AABB.margin() + second_group_AABB.margin();
}


float volume_node::overlap_value(unsigned first_group_size) const
{
    aabb first_group_AABB;
    aabb second_group_AABB;


    get_AABB_for_groups(first_group_size, & first_group_AABB,
                        & second_group_AABB);


    first_group_AABB.union_with(second_group_AABB);
    return first_group_AABB.volume();
}


float volume_node::area_value(unsigned first_group_size) const
{
    aabb first_group_AABB;
    aabb second_group_AABB;


    get_AABB_for_groups(first_group_size, & first_group_AABB,
                        & second_group_AABB);

    return first_group_AABB.surface_area() + second_group_AABB.surface_area();
}


typedef bool (*axis_cmp_func)(const void *, const void *);

void volume_node::choose_split_axis(void)
{
    unsigned i;
    float margin_sum[3] = { 0.0, 0.0, 0.0 };
    static const axis_cmp_func axis_comparitors[3] = {
        compare_node_x,
        compare_node_y,
        compare_node_z,
    };


    for (unsigned axis = 0; axis < 3; axis++) {
        std::sort((void **) & children[0],
                  (void **) & children[used_children],
                  axis_comparitors[axis]);
        for (i = 2; i < M; i++) {
            margin_sum[axis] += margin_value(i);
        }
    }

    unsigned best;
    best = (margin_sum[0] < margin_sum[1]) ? 0 : 1;
    best = (margin_sum[best] < margin_sum[2]) ? best : 2;

    if (best != 2) {
        std::sort((void **) & children[0], 
                  (void **) & children[used_children], 
                  axis_comparitors[best]);
    }
}


unsigned volume_node::choose_split_index(void) const
{
    unsigned best = 2;
    float best_overlap = overlap_value(2);

    for (unsigned i = 3; i < M; i++) {
        const float overlap = overlap_value(i);
        
        if ((overlap < best_overlap) 
            || ((overlap == best_overlap)
                && (area_value(i) < area_value(best)))) {
            best = i;
            best_overlap = overlap;
        }
    }

    return best;
}


volume_node *volume_node::do_split(void)
{
    choose_split_axis();
    const unsigned split_point = choose_split_index();


    /* Create the sibling node.  The sibling has the same type (i.e., either
     * leaf node or inner node) as the node being split.
     */
    volume_node *sibling = new volume_node;
    sibling->leaf = leaf;


    /* Move all children after the split point to the sibling node.
     */
    for (unsigned i = split_point; i < used_children; i++) {
        sibling->box.intersect_with(children[i].n->box);

        sibling->children[i - split_point] = children[i];
        sibling->used_children++;

        children[i].child = NULL;
    }


    /* Update the child count on the node that was split.
     */
    used_children = split_point;


    /* After some of the child nodes have been removed from the current
     * node, resize the bounding box.
     */
    box.clear();
    for (unsigned i = 0; i < used_children; i++) {
        box.intersect_with(children[i].n->box);
    }

    return sibling;
}


volume_node *volume_node::insert(const aabb &add, const void *data)
{
    volume_node *add_to = this;
    volume_node *sibling = NULL;
    rtree_node *added_node = NULL;
    aabb add_box = add;

    if (is_leaf()) {
        added_node = new volume_data(add_box, data);
    }
    else {
        added_node = children[choose_subtree(add)].n->insert(add_box, data);
    }


    if (added_node) {
        if (is_full()) {
            /* The node is full, so split it.
             */
            sibling = do_split();


            /* Determine whether it's better to add the new data to the
             * old node (this) or the new node (sibling).
             */
            float sibling_cost = is_leaf()
                ? sibling->child_cost(added_node->box)
                : sibling->expansion(added_node->box);
            float my_cost = is_leaf()
                ? child_cost(added_node->box)
                : expansion(added_node->box);

            add_to = (sibling_cost < my_cost) ? sibling : this;
            add_box = added_node->box;
        }

        
        /* Add the new node as a child of the correct existing node.  If
         * *this split, this may be its new sibling.
         */
        add_to->children[add_to->used_children].child = added_node;
        add_to->used_children++;
    }

    add_to->box.intersect_with(add_box);
    return sibling;
}


volume_rtree_iterator &volume_rtree::search(const idr_vec4 *corners)
{
    volume_rtree_iterator *it = new volume_rtree_iterator(root, -1, corners);

    ++(*it);
    
    return *it;
}


volume_rtree_iterator &volume_rtree_iterator::operator++(void)
{
    while (atom != NULL) {
        volume_node *const n = get_node();
        unsigned i = get_index() + 1;

        for (/* empty */; i < n->used_children; i++) {
            if (n->children[i].child->overlaps(box)) {
                if (n->is_leaf()) {
                    set_index(i);
                    return *this;
                }
                else {
                    set_index(i);
                    push(n->children[i].n, -1);
                    break;
                }
            }
        }

        /* No more matches.  Pop the current atom and continue.
         */
        if (i == n->used_children) {
            pop();
        }
    }

    return *this;
}

void volume_node::dump(unsigned depth)
{
    printf("[%u] -> %f %s%s\n", depth, volume(), 
           (used_children == 0) ? "empty " : "",
           is_leaf() ? "leaf" : "");

    if (!is_leaf()) {
        for (unsigned i = 0; i < used_children; i++) {
            children[i].n->dump(depth + 1);
        }
    }
    else {
#if 0
        for (unsigned i = 0; i < used_children; i++) {
            printf("    %.2f %.2f %.2f\n",
                   children[i].d->position[0],
                   children[i].d->position[1],
                   children[i].d->position[2]);
        }
#endif
    }
}


void volume_rtree::dump(void)
{
    root->dump(0);
}
