#include "dkdata.h"

static octree_t *Octree_NewNode(cube_t *c, int leaf)
{
    octree_t *newNode;

    if ((newNode = malloc(sizeof(octree_t))) == NULL)
    {
	    return(NULL);
    }

    memset(newNode, 0x00, sizeof(octree_t));

    // fill in new node members
    memcpy(&newNode->cube, c, sizeof(cube_t));
    newNode->leaf = leaf;

    return(newNode);
}



static void PrintCube(cube_t *c)
{
    printf("(%d %d %d) (%d %d %d)\n",
        c->x1, c->y1, c->z1,
	c->x2, c->y2, c->z2);
}


// static int Octree_Branch(data_t *data, int level)
static int Octree_Branch(octree_t *p, int x, int y, int z, cube_t *c)
{
    int x1, y1, z1;
    int x2, y2, z2;
    int x3, y3, z3;
    cube_t  cube[8];
    int i;

    x1 = p->cube.x1; y1 = p->cube.y1; z1 = p->cube.z1;
    x2 = p->cube.x2; y2 = p->cube.y2; z2 = p->cube.z2;

    x3 = (x1 + ((x2 - x1) / 2));
    y3 = (y1 + ((y2 - y1) / 2));
    z3 = (z1 + ((z2 - z1) / 2));

    // out of the current level's cube, create 8 sub cubes
    cube[0].x1 = x1; cube[0].y1 = y1; cube[0].z1 = z1;
    cube[0].x2 = x3; cube[0].y2 = y3; cube[0].z2 = z3;

    cube[1].x1 = x3; cube[1].y1 = y1; cube[1].z1 = z1;
    cube[1].x2 = x2; cube[1].y2 = y3; cube[1].z2 = z3;

    cube[2].x1 = x1; cube[2].y1 = y3; cube[2].z1 = z1;
    cube[2].x2 = x3; cube[2].y2 = y2; cube[2].z2 = z3;

    cube[3].x1 = x3; cube[3].y1 = y3; cube[3].z1 = z1;
    cube[3].x2 = x2; cube[3].y2 = y2; cube[3].z2 = z3;

    cube[4].x1 = x1; cube[4].y1 = y1; cube[4].z1 = z3;
    cube[4].x2 = x3; cube[4].y2 = y3; cube[4].z2 = z2;

    cube[5].x1 = x3; cube[5].y1 = y1; cube[5].z1 = z3;
    cube[5].x2 = x2; cube[5].y2 = y3; cube[5].z2 = z2;

    cube[6].x1 = x1; cube[6].y1 = y3; cube[6].z1 = z3;
    cube[6].x2 = x3; cube[6].y2 = y2; cube[6].z2 = z2;

    cube[7].x1 = x3; cube[7].y1 = y3; cube[7].z1 = z3;
    cube[7].x2 = x2; cube[7].y2 = y2; cube[7].z2 = z2;

    // ok now find the space that out x,y,z fit into
    for (i = 0; i < 8; i++)
    {
	    if ( (x >= cube[i].x1) && (x < cube[i].x2) &&
	         (y >= cube[i].y1) && (y < cube[i].y2) &&
	         (z >= cube[i].z1) && (z < cube[i].z2) )
	    {

	        memcpy(c, &cube[i], sizeof(cube_t));
	        return(i);
	    }
    }

    // this is bad
    return(-1);
}





void Octree_AddColor(octree_t *p, float r, float g, float b, float weight)
{
    cube_t    c;
    int i;

    if (p == NULL)
    {
		Error("ERROR -- Octree_ColorNode: NULL octree pointer");
    }

    // if we are at level 8 then we are done
    if (p->leaf >= 8)
    {
		// if count is not zero then rgb better be the same
		if (p->count)
		{
			if ((p->r == r) && (p->g == g) && (p->b == b))
			{
				p->count += weight;
			}
			else
			{
				Error("Octree_AddColor: node in use!!!!");
			}
		}

		else
		{
			p->r = r;
			p->g = g;
			p->b = b;
			p->count = weight;
		}

	    return;
    }

    // otherwise find subspace to put it into
    else
    {
	    // find subspace
        i = Octree_Branch(p, (int) r, (int) g, (int) b, &c);

	    // if i == -1 then we have a serious bug
	    if (i == -1)
	    {
	        Error("ERROR -- Octree_Branch failed. Leaf: %d", p->leaf);
	    }

	    // ok create a new node for this branch
	    // if branch already exists then we don't need to create a new one
	    if (!p->octant[i]) 
		{
			p->octant[i] = Octree_NewNode(&c, p->leaf + 1);
		}

	    // on we go
	    Octree_AddColor(p->octant[i], r, g, b, weight);
    }
}



octree_t *Octree_FindColor(octree_t *p, float r, float g, float b)
{
    int i;
    cube_t *c;
	int x, y, z;

	x = (int) r;
	y = (int) g;
	z = (int) b;

    // see if we are done
	if (p->count)
	{
		if ((p->r == r) && (p->g == g) && (p->b == b))
		{
			return(p);
		}
		else
		{
			Error("Octree_FindColor: bad leaf.");
			// return(NULL);
		}
	}


    // figure out which cube this child belons in and recurse to that
    // child
    // if we find no cube then pick first child to return
    for (i = 0; i < 8; i++)
    {
	    if (p->octant[i])
	    {
	        c = &p->octant[i]->cube;

	        if ( (x >= c->x1) && (x < c->x2) &&
	             (y >= c->y1) && (y < c->y2) &&
	             (z >= c->z1) && (z < c->z2) )
	        {
	            return(Octree_FindColor(p->octant[i], r, g, b));
	        }
	    }
    }

    // ok no subspace was found, rgb not found
	return(NULL);
}



octree_t *Octree_Init(void)
{
    octree_t *node;
    cube_t    cube;

    // set initial cube dimensions
    cube.x1 = cube.y1 = cube.z1 = 0;
    cube.x2 = cube.y2 = cube.z2 = 256;

    // create at least one empty node
    if ((node = Octree_NewNode(&cube, 0)) == NULL)
    {
	    return(NULL);
    }

    return(node);
}


void Octree_Free(octree_t *p)
{
	int i;

	// see if we are at bottom
	if (p->count) return;

	for (i = 0; i < 8; i++)
	{
		if (p->octant[i])
		{
			// free all of this octants memory
			Octree_Free(p->octant[i]);

			// free this octant
			free(p->octant[i]);

			p->octant[i] = NULL;
		}
	}
}






#ifdef NEW

void Octree_Print(octree_t *p)
{
    int i;

    if (p == NULL) return;
    if (p->data) Print_Data(p->data);

    // recurse
    for (i = 0; i < 8; i++)
    {
	    Octree_Print(p->octant[i]);
    }
}

#endif


