Working quadtree and code cleanup