class PointCloud
{
  // Description:
  //   Spacial Structure for A Bin of points that we want to add to and query from.
  //   Internally implemented as a very very simplistic octree
  
  OctNode root;
  
  PointCloud( int levels, float girth )
  {
    root = new OctNode(levels, new PVector(-girth, -girth, -girth), new PVector( girth, girth, girth ) );
  }
  
  void Update( int delta )
  {
    root.Update( delta );
  }
  
  void AddPoint( DrawPoint drawpoint )
  {
    root.AddPoint( drawpoint );
  }
  
  void GatherPoints( PVector location, float radius, ArrayList out )
  {
    root.GatherPoints( location, radius, out );
  }
}

class OctNode
{
  OctNode[] children;
  
  PVector   mins;
  PVector   maxs;
  
  LinkedList points;
  
  OctNode( int levels, PVector inner, PVector outer )
  {
    // Store Defining Data
    mins = new PVector( min( inner.x, outer.x), min( inner.y, outer.y), min( inner.z, outer.z) );
    maxs = new PVector( max( inner.x, outer.x), max( inner.y, outer.y), max( inner.z, outer.z) );
    
    // Create Children
    if( levels > 0 )
    { // If Tier node, create more OctNodes as children
      this.children = new OctNode[8];
    
      PVector center = PVector.add( inner, outer );
      center.mult( .5 );
      
      PVector[] bounds = GetBounds();  
      for( int i = 0; i < 8; i++ )
      {
        children[i] = new OctNode( levels - 1, center, bounds[i]);
      }
    }
    else
    { // If Leaf node, create a bin for Points
      points = new LinkedList();
    }
  }
  
  void Update( int delta )
  {
    if( children != null )
    {
      for( int i = 0; i < 8; i++ )
      {
        children[i].Update( delta );
      }
    }
    
    if( points != null )
    {
      ListIterator it = points.listIterator();
      while( it.hasNext() )
      {
        DrawPoint p = (DrawPoint)it.next();
        p.Update( delta );
        if( p.IsDead() )
        {
          it.remove();
        }
      }
    }
  }
  
  PVector[] GetBounds()
  {
    PVector[] bounds = new PVector[8];
    bounds[0] = mins.get();
    bounds[1] = new PVector( mins.x, mins.y, maxs.z );
    bounds[2] = new PVector( mins.x, maxs.y, mins.z );
    bounds[3] = new PVector( mins.x, maxs.y, maxs.z );
    bounds[4] = new PVector( maxs.x, mins.y, mins.z );
    bounds[5] = new PVector( maxs.x, mins.y, maxs.z );
    bounds[6] = new PVector( maxs.x, maxs.y, mins.z );
    bounds[7] = maxs.get(); 
    
    return bounds;
  }
  
  boolean Contained( PVector location )
  {
    return ( mins.x < location.x && location.x < maxs.x )
        && ( mins.y < location.y && location.y < maxs.y )
        && ( mins.z < location.z && location.z < maxs.z );
  }
  
  void AddPoint( DrawPoint drawpoint )
  {
    if( Contained( drawpoint.pos ) )
    {
      if( children != null )
      {
        for( int i = 0; i < 8; i++ )
        {
          children[i].AddPoint( drawpoint );
        }
      }
      else
      {
        points.add( drawpoint );
      }
    }
  }
  
  boolean Intersects( PVector location, float radius )
  {
    float radiusSq = radius * radius;
    float diagonalSq = 0;
    
    float[] center = location.array();
    float[] boxMin = mins.array();
    float[] boxMax = maxs.array();

    for( int i = 0; i < 3; i++ )
    {
      float minDiff = center[i] - boxMin[i];
      float maxDiff = center[i] - boxMax[i];
      
      if( minDiff < 0 )
      {
        diagonalSq += minDiff * minDiff;
      }
      else if ( maxDiff > 0 )
      {
        diagonalSq += maxDiff * maxDiff;
      }
    }
    
    return( diagonalSq <= radiusSq );
  }
  
  void GatherPoints( PVector location, float radius, ArrayList out )
  {
    if( Intersects( location , radius ) )
    {
      if( children != null )
      {
        for( int i = 0; i < 8; i++ )
        {
          children[i].GatherPoints( location, radius, out );
        }
      }
      
      else
      {  // Point is in Sphere
        float radSq = radius * radius;
        ListIterator it = points.listIterator();
        while( it.hasNext() )
        {
          DrawPoint p = (DrawPoint)it.next();
          PVector between = PVector.sub( p.pos, location );
          if( between.dot( between ) < radSq )
          {
            out.add( p );
          }
        }
        //for( int i = points.size() - 1; i >=0; i-- )
        //{
        //  DrawPoint cloudPoint = (DrawPoint)points.get( i );
        //  PVector between = PVector.sub( cloudPoint.pos, location );
        //  if( between.dot( between ) < radSq )
        //  {
        //    out.add( cloudPoint );
        //  }
        //}   
      }
    }
  }
}