#include <math.h>
#include <stdlib.h>
#include "tree.h"

dot voxel = {.c = {1.0, 1.0, 1.0, 0.0}};

void calc_bounding_box (const dot *set, unsigned int n, dot *min, dot *max)
{
    int i;
    min->vect = set[0].vect;
    max->vect = set[0].vect;

    for (i=1; i<n; i++)
    {
        min->vect = __builtin_ia32_minps (min->vect, set[i].vect);
        max->vect = __builtin_ia32_maxps (max->vect, set[i].vect);
    }
}

dot calc_avg (const dot *set, unsigned int n)
{
    dot res;
    int i;
    __builtin_ia32_xorps (res.vect, res.vect);
    for (i=0; i<n; i++) res.vect += set[i].vect;
    res.vect /= (float)n;
    return res;
}

uint8_t get_subspace_idx (dot center, dot d)
{
    uint8_t res = 0;
    int i;

    for (i=0; i<N; i++) res |= ((center.c[i] > d.c[i]) ? 1 : 0) << i;
    return res;
}

dot align_on_voxel (dot d)
{
    int i;
    dot res;

    for (i=0; i<N; i++)
    {
        float tmp = ceilf (d.c[i] / voxel.c[i]);
        res.c[i] = tmp*voxel.c[i];
    }
    return res;
}

unsigned int filter_set (const dot *in, dot *out, unsigned int n, uint8_t idx, dot center)
{
    unsigned int counter = 0;
    int i,j;

    for (i=0; i<n; i++)
    {
        if (get_subspace_idx (center, in[i]) == idx)
        {
            out[counter].vect =  in[i].vect;
            counter++;
        }
    }

    return counter;
}

struct node* make_tree (const dot *set, unsigned int n)
{
    struct node *res = malloc (sizeof (struct node));
    res->flags = 0;

    if (n > 0)
    {
        res->flags |= 1<<FULL;
        calc_bounding_box (set, n, &(res->bb_min), &(res->bb_max));
    }

    if (n <= MAX_DOTS)
    {
        res->flags |= 1<<LEAF;
        leaf_data *leaf = &(res->data.leaf);
        leaf->dots_num = n;

        // FIXME: Maybe memcpy?
        int i;
        for (i=0; i<n; i++) leaf->dots[i].vect = set[i].vect;
    }

    else
    {
        inner_data *inner = &(res->data.inner);
        dot center = calc_avg (set, n);
        inner->center = align_on_voxel (center);
        int i;
        dot *subset = malloc (sizeof (dot) * n);
        for (i=0; i<NS; i++)
        {
            int sub_n = filter_set (set, subset, n, i, inner->center);
            inner->children[i] = make_tree (subset, sub_n);
        }
        free (subset);
    }

    return res;
}