/* Fixed quadtree implementation using array.  Some quadtree implementations
can dynamically expand the tree, in which case you will need to use pointers
to a node's children and (maybe) parent.  But if we pre-construct a full
tree up to a certain depth, it is possible to use an array to represent the
tree, similar to the implementation of a priority heap:

root at index 0
given node at index i, first child at index i*4+1, last child at i*4+4
given node at index i, parent node at (i-1)/4.

quads: 0= NW, 1 = NE, 2 = SW, 3 = SE
*/
import java.util.*;

// object that can be inserted into a quadtree must implement the following:
interface qtobject extends Comparable<qtobject>
{
    int X(); // x and y coordinates
    int Y();
    int R(); // radius
    void react(qtobject m); // react on collision (observer pattern update)
    // included by "extend": int compareTo(qtobject x)
} 

class qnode // a quadtree node
{
    ////////////////// STATIC (GLOBAL) SETUP /////////////////
    protected static qnode[] Tree;  // global, to be initialized externally
    public static void setup(qnode[] t) 	{ Tree = t; }
    public static void setup(int depth, int n, int e, int s, int w)
	{
	    int m = 0; // compute size based on depth of the tree.
	    int power = 1;
	    for(int i=0;i<depth;i++) { m += power; power *=4; }
	    Tree = new qnode[m];
	    // now make the tree based on initial bounds.
	    makenode(0,n,e,s,w);
	}
    protected static void makenode(int i, int n, int e, int s, int w)
    {
	if (i>=Tree.length || i<0) return;  // check index
	Tree[i] = new qnode(i, n, e, s, w);
	makenode(i*4+1,n,(e+w)/2,(n+s)/2,w); // NW subquad
	makenode(i*4+2,n,e,(n+s)/2,(e+w)/2); // NE 
	makenode(i*4+3,(n+s)/2,(e+w)/2,s,w); // SW
	makenode(i*4+4,(n+2)/2,e,s,(e+w)/2); // SE
    }
    ///////////////////////////////////////////////////////////

    int NB, SB, EB, WB; // quadrant boundaries, W,N inclusive
    //ArrayList<qtobject> Elements; 
    HashSet<qtobject> Elements; // qtobjects that intersect subquads.
    int index; // index inside global tree array
    public qnode(int i, int a, int b, int c, int d)
	{
	    NB=a; EB =b; SB=c; WB=d;  
	    index = i;
	    //	    Elements = new ArrayList<qtobject>();
	    Elements = new HashSet<qtobject>();
	}// constructor

    protected boolean intersect(qtobject m) // inquad or straddles border:
    {
	int x = m.X(), y = m.Y(), r = m.R();
	return x>=WB && y<EB && y>=NB && y<SB;
    } // not used for now

    protected boolean inquad(qtobject m) // completely inside this quad
    {
	int x = m.X(), y = m.Y(), r = m.R();
	return x-r>=WB && x+r<EB && y-r>=NB && y+r<SB;
    }

    public static qnode add(qtobject m) { return Tree[0].insert(m); }
    public qnode insert(qtobject m) // insert object into quad, returns where
    {
	if (!inquad(m)) return null;
	qnode ax = null;
	// attempt to insert into one of subquads
	int child = 1;
	while (child<=4 && ax==null)
	    {
		int ci = index*4+child; // child index
		if (ci<Tree.length)
		    { ax = Tree[ci].insert(m);  child++; }
		else { child=5; } // force termination
	    }
	if (ax!=null) return ax; // insert successfull, else insert here:
	Elements.add(m);
	return this;
    }//insert

    // to speedup delete, qtobject should maintain pointer to its qnode
    public boolean delete(qtobject m)
    {
	boolean ax = Elements.remove(m);
	for(int i=1;i<=4 && !ax;i++)
	    ax = ax || Tree[index*4+i].delete(m);
	return ax;
    }
    
    // move - change in coordinates requires moving the object
    public qnode move(qtobject m)
    {
        if (!inquad(m))
	    {	
		boolean ax = delete(m);
		if (ax) return Tree[0].insert(m);
		else return null;
	    }
	// assume still in quad, but does it need to move to subquad?
	boolean bx = false;
	for(int i=1;i<=4 && !bx;i++)
	    bx = index*4+i<Tree.length && Tree[index*4+i].inquad(m);
	if (bx) 
	    {
		boolean cx = delete(m);
		if (cx) return insert(m); // resert from here.
		else return null;
	    }
	return this; // no need to move
    }

    ///////////////// Collision detection - may want to abstract over this

    public static boolean collide(qtobject n, qtobject m)
    {  
	if (n==m) return false;
       int mindist = n.R()+ m.R(); // minimum distance for collision
       int dx = n.X()-m.X(), dy = n.Y()-m.Y();
       if (dx<0)dx *=-1; if (dy<0) dy*=-1; // absolute values
       return dx<mindist && dy<mindist;
      // note that this does not use floating point calculations.
    }

    static double distance(int x0, int y0, int x1, int y1)  // note private 
    { int x = (x0-x1)*(x0-x1);
      int y = (y0-y1)*(y0-y1);
      return Math.sqrt(x+y);
    }
    
    public static boolean collide2(qtobject n, qtobject m)
    {  
       return distance(n.X(),n.Y(),m.X(),m.Y()) <= n.R() + m.R();
    }

    public int collusiondetect(qtobject m)
    {
        int cx = 0; // counts collisions
        // check collision against elements
        for(qtobject n:Elements)
	    if ((n.compareTo(m)<0) && collide2(n,m)) 
		{
		    cx++;
                    n.react(m);
                } 
	for(int ci=index*4+1;ci<=index*4+4;ci++) // for each subquad
	    if (ci<Tree.length) cx += Tree[ci].collusiondetect(m);
	    else ci = index*4+5; // force termination
        return cx;
    }
}//qnode