/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
//
// C++ Interface: SpatialGridContainer
//
// Description: 
//
//
// Author: The SOFA team <http://www.sofa-framework.org>, (C) 2006
//
// Copyright: See COPYING file that comes with this distribution
//
//

#ifndef SOFA_COMPONENT_CONTAINER_SPATIALGRIDCONTAINER_INL
#define SOFA_COMPONENT_CONTAINER_SPATIALGRIDCONTAINER_INL

#include <sofa/component/container/SpatialGridContainer.h>

#include <sofa/component/topology/PointSetTopologyChange.h>
#include <sofa/component/topology/PointSetTopologyModifier.h>

#include <sofa/simulation/common/AnimateBeginEvent.h>
#include <sofa/simulation/common/AnimateEndEvent.h>
#include <sofa/helper/system/gl.h>
#include <sofa/component/container/MechanicalObject.h>


namespace sofa
{

namespace component
{

namespace container
{

      using namespace sofa::helper;

      template<class DataTypes>
      typename SpatialGrid<DataTypes>::Grid SpatialGrid<DataTypes>::emptyGrid;

      template<class DataTypes>
      SpatialGrid<DataTypes>::SpatialGrid(Real cellWidth)
  : cellWidth(cellWidth), invCellWidth(1/cellWidth)
      {
      }

      template<class DataTypes>
      const typename SpatialGrid<DataTypes>::Grid* SpatialGrid<DataTypes>::findGrid(const Key& k) const
      {
	typename Map::const_iterator it = map.find(k);
	if (it == map.end()) return &emptyGrid;
	else return it->second;
      }

      template<class DataTypes>
      typename SpatialGrid<DataTypes>::Grid* SpatialGrid<DataTypes>::getGrid(const Key& k)
      {
	Grid* & g = map[k];
	if (g == NULL)
	  {
	    g = new Grid;
	    Grid* g2;
	    typename Map::const_iterator it;
	    typename Map::const_iterator end = map.end();
	    it = map.find(Key(k[0]-1, k[1]  , k[2]  ));
	    if (it != end)
	      {
		g2 = it->second;
		g->neighbors[0] = g2; g2->neighbors[1] = g;
	      }
	    else
	      g->neighbors[0] = &emptyGrid;
	    it = map.find(Key(k[0]+1, k[1]  , k[2]  ));
	    if (it != end)
	      {
		g2 = it->second;
		g->neighbors[1] = g2; g2->neighbors[0] = g;
	      }
	    else
	      g->neighbors[1] = &emptyGrid;
	    it = map.find(Key(k[0]  , k[1]-1, k[2]  ));
	    if (it != end)
	      {
		g2 = it->second;
		g->neighbors[2] = g2; g2->neighbors[3] = g;
	      }
	    else
	      g->neighbors[2] = &emptyGrid;
	    it = map.find(Key(k[0]  , k[1]+1, k[2]  ));
	    if (it != end)
	      {
		g2 = it->second;
		g->neighbors[3] = g2; g2->neighbors[2] = g;
	      }
	    else
	      g->neighbors[3] = &emptyGrid;
	    it = map.find(Key(k[0]  , k[1]  , k[2]-1));
	    if (it != end)
	      {
		g2 = it->second;
		g->neighbors[4] = g2; g2->neighbors[5] = g;
	      }
	    else
	      g->neighbors[4] = &emptyGrid;
	    it = map.find(Key(k[0]  , k[1]  , k[2]+1));
	    if (it != end)
	      {
		g2 = it->second;
		g->neighbors[5] = g2; g2->neighbors[4] = g;
	      }
	    else
	      g->neighbors[5] = &emptyGrid;
	  }
	return g;
      }

      template<class DataTypes>
      typename SpatialGrid<DataTypes>::Cell* SpatialGrid<DataTypes>::getCell(const Coord& x)
      {
	int ix = rfloor(x[0]*invCellWidth);
	int iy = rfloor(x[1]*invCellWidth);
	int iz = rfloor(x[2]*invCellWidth);
	Key k(ix >> GRIDDIM_LOG2, iy >> GRIDDIM_LOG2, iz >> GRIDDIM_LOG2);
	ix &= GRIDDIM-1;
	iy &= GRIDDIM-1;
	iz &= GRIDDIM-1;
	Grid* g = getGrid(k);
	return g->cell+(ix+GRIDDIM*iy+GRIDDIM*GRIDDIM*iz);
      }

      template<class DataTypes>
      const typename SpatialGrid<DataTypes>::Cell* SpatialGrid<DataTypes>::getCell(const Grid* g, int x, int y, int z)
      {
	if (x<0)
	  {
	    g = g->neighbors[0];
	    x += GRIDDIM;
	  }
	else if (x >= GRIDDIM)
	  {
	    g = g->neighbors[1];
	    x -= GRIDDIM;
	  }
	if (y<0)
	  {
	    g = g->neighbors[2];
	    y += GRIDDIM;
	  }
	else if (y >= GRIDDIM)
	  {
	    g = g->neighbors[3];
	    y -= GRIDDIM;
	  }
	if (z<0)
	  {
	    g = g->neighbors[4];
	    z += GRIDDIM;
	  }
	else if (z >= GRIDDIM)
	  {
	    g = g->neighbors[5];
	    z -= GRIDDIM;
	  }
	return g->cell + (x*DX + y*DY + z*DZ);
      }

      template<class DataTypes> template<class NeighborListener>
      void SpatialGrid<DataTypes>::findNeighbors(NeighborListener* dest, const Real dist2, const Cell** cellsBegin, const Cell** cellsEnd)
      {
	const Cell* c0 = *cellsBegin;
	const typename std::list<Entry>::const_iterator end = c0->plist.end();
	for (typename std::list<Entry>::const_iterator it = c0->plist.begin(); it != end; it++)
	  {
	    const Entry& p1 = *it;
	    typename std::list<Entry>::const_iterator it2 = it;
	    ++it2;
	    for (; it2 != end; it2++)
	      {
		const Entry& p2 = *it2;
		const Real r2 = (p2.pos - p1.pos).norm2();
		if (r2 < dist2)
		  dest->addNeighbor(p1.index, p2.index, r2, dist2);
	      }
	    for (const Cell** c = cellsBegin+1; c != cellsEnd; ++c)
	      {
		const typename std::list<Entry>::const_iterator end2 = (*c)->plist.end();
		for (typename std::list<Entry>::const_iterator it2 = (*c)->plist.begin(); it2 != end2; it2++)
		  {
		    const Entry& p2 = *it2;
		    const Real r2 = (p2.pos - p1.pos).norm2();
		    if (r2 < dist2)
		      {
			dest->addNeighbor(p1.index, p2.index, r2, dist2);
		      }
		  }
	      }
	  }
      }

template<class DataTypes> template<class NeighborListener>
      void SpatialGrid<DataTypes>::findNeighbors(NeighborListener* dest, Real dist)
      {
	const Real dist2 = dist*dist;
	for (typename Map::iterator itg = map.begin();itg != map.end(); itg++)
	  {
	    Grid* g = itg->second;
	    if (g->empty) continue;
	    const Cell* cell[14];
	    int ic = 0;
	    int x,y,z;
	    cell[0] = g->cell;
	    for (z = 0; z<GRIDDIM; z++)
	      {
		for (y = 0; y<GRIDDIM; y++)
		  {
		    cell[ 0] = g->cell+ic;
		    cell[ 2] = getCell(g,  0, y+1, z  );
		    cell[ 4] = getCell(g,  0, y  , z+1); //(z < GRIDDIM-1) ? g->cell+(ic + DZ) : g->neighbors[5]->cell+(ic - (GRIDDIM-1)*DZ);
		    cell[ 6] = getCell(g,  0, y+1, z+1); //(y < GRIDDIM-1) ? cell[4] + DY : (z < GRIDDIM-1) ? cell[2] + DZ : g->neighbors[3]->neighbors[5]->cell+(ic - (GRIDDIM-1)*(DY+DZ));
		    cell[ 8] = getCell(g, -1, y+1, z  ); //(y < GRIDDIM-1) ? g->neighbors[0]->cell+(ic - (GRIDDIM-1)*DX + DY) : g->neighbors[0]->neighbors[3]->cell+(ic - (GRIDDIM-1)*(DX+DY));
		    cell[ 9] = getCell(g, -1, y  , z+1); //(z < GRIDDIM-1) ? g->neighbors[0]->cell+(ic - (GRIDDIM-1)*DX + DZ) : g->neighbors[0]->neighbors[5]->cell+(ic - (GRIDDIM-1)*(DX+DZ));
		    cell[10] = getCell(g, -1, y+1, z+1); //(y < GRIDDIM-1) ? cell[9] + DY : (z < GRIDDIM-1) ? cell[8] + DZ : g->neighbors[0]->neighbors[3]->neighbors[5]->cell+(ic - (GRIDDIM-1)*(DX+DY+DZ));
		    cell[11] = getCell(g, -1, y-1, z+1); //(y > 0) ? cell[9] - DY : (z < GRIDDIM-1) ? cell[8] + DZ : g->neighbors[0]->neighbors[3]->neighbors[5]->cell+(ic - (GRIDDIM-1)*(DX+DY+DZ));
		    cell[12] = getCell(g,  0, y-1, z+1);
		    for (x = 0; x<GRIDDIM-1; x++)
		      {
			cell[ 1] = cell[ 0] + DX;
			cell[ 3] = cell[ 2] + DX;
			cell[ 5] = cell[ 4] + DX;
			cell[ 7] = cell[ 6] + DX;
			cell[13] = cell[12] + DX;
			if (!cell[0]->plist.empty())
			  this->findNeighbors(dest, dist2, cell, cell+14);
			cell[ 8] = cell[ 2];
			cell[ 9] = cell[ 4];
			cell[10] = cell[ 6];
			cell[11] = cell[12];
			cell[12] = cell[13];
			cell[ 0] = cell[ 1];
			cell[ 2] = cell[ 3];
			cell[ 4] = cell[ 5];
			cell[ 6] = cell[ 7];
			++ic;
		      }
		    if (!cell[0]->plist.empty())
		      {
			cell[ 1] = g->neighbors[1]->cell+(ic - (GRIDDIM-1)*DX);
			cell[ 3] = getCell(g, GRIDDIM, y+1, z  ); //(y < GRIDDIM-1) ? cell[1] + DY : g->neighbors[3]->cell+(ic - (GRIDDIM-1)*(DX+DY));
			cell[ 5] = getCell(g, GRIDDIM, y  , z+1); //(z < GRIDDIM-1) ? cell[1] + DZ : g->neighbors[5]->cell+(ic - (GRIDDIM-1)*(DX+DZ));
			cell[ 7] = getCell(g, GRIDDIM, y+1, z+1); //(y < GRIDDIM-1) ? cell[5] + DY : (z < GRIDDIM-1) ? cell[3] + DZ : g->neighbors[3]->neighbors[5]->cell+(ic - (GRIDDIM-1)*(DX+DY+DZ));
			cell[13] = getCell(g, GRIDDIM, y-1, z+1); //cell[12] + DX;
			this->findNeighbors(dest, dist2, cell, cell+14);
		      }
		    ++ic;
		  }
	      }
	  }
      }

      template<class DataTypes>
      void SpatialGrid<DataTypes>::computeField(ParticleField* field, Real dist)
      {
	//dist /= cellWidth;
	const Real dist2 = dist*dist;
	const int r = rceil(dist/cellWidth)+1;
	int x,y,z;
	int x2,y2,z2;
	if (r > GRIDDIM)
	  {
	    std::cerr << "Distance too large in SpatialGrid::computeField ("<<r<<" > "<<GRIDDIM<<")\n";
	    return;
	  }
	//std::cout << "accumulate particles with radius "<<dist<<std::endl;
	for (typename Map::iterator itg = map.begin();itg != map.end(); itg++)
	  {
	    Grid* g = itg->second;
	    Coord pos;
	    pos[0] = (Real)(itg->first[0]*GRIDDIM);
	    pos[1] = (Real)(itg->first[1]*GRIDDIM);
	    pos[2] = (Real)(itg->first[2]*GRIDDIM);
	    if (!g->empty)
	      {
		Cell* c;
		Cell* c2;
		for (z = 0, c = g->cell; z<GRIDDIM; z++)
		  {
		    int z0 = (z<r)?0:z-r+1;
		    int z1 = (z>GRIDDIM-1-r)?GRIDDIM-1:z+r;
		    for (y = 0; y<GRIDDIM; y++)
		      {
			int y0 = (y<r)?0:y-r+1;
			int y1 = (y>GRIDDIM-1-r)?GRIDDIM-1:y+r;
			for (x = 0; x<GRIDDIM; x++, c++)
			  {
			    if (!c->plist.empty())
			      {
				int x0 = (x<r)?0:x-r+1;
				int x1 = (x>GRIDDIM-1-r)?GRIDDIM-1:x+r;
				typename std::list<Entry>::const_iterator begin = c->plist.begin();
				typename std::list<Entry>::const_iterator end = c->plist.end();
				typename std::list<Entry>::const_iterator it;
				//std::cout << "accumulate "<<c->plist.size()<<" particles from <"<<pos<<">+<"<<x<<" "<<y<<" "<<z<<"> to area <"<<x0<<" "<<y0<<" "<<z0<<">-<"<<x1<<" "<<y1<<" "<<z1<<">"<<std::endl;
				c2 = g->cell+(x0*DX+y0*DY+z0*DZ);
				const int dy = DY-(x1-x0+1)*DX;
				const int dz = DZ-(y1-y0+1)*DY;
				Coord p;
				for (z2 = z0; z2<=z1; z2++, c2+=dz)
				  {
				    p[2] = (pos[2]+z2)*cellWidth;
				    for (y2 = y0; y2<=y1; y2++, c2+=dy)
				      {
					p[1] = (pos[1]+y2)*cellWidth;
					for (x2 = x0; x2<=x1; x2++, c2+=DX)
					  {
					    p[0] = (pos[0]+x2)*cellWidth;
					    for (it=begin; it != end; ++it)
					      {
						Real r2 = (it->pos-p).norm2();
						if (r2 < dist2)
						  c2->data.add(field, it->index, r2, dist2);
					      }
					  }
				      }
				  }
			      }
			  }
		      }
		  }
	      }
	    for (int gz=-1; gz<=1; gz++)
	      {
		for (int gy=-1; gy<=1; gy++)
		  {
		    for (int gx=-1; gx<=1; gx++)
		      {
			const Grid* g2 = g;
			if (gx==-1) g2 = g2->neighbors[0]; else if (gx==1) g2 = g2->neighbors[1];
			if (gy==-1) g2 = g2->neighbors[2]; else if (gy==1) g2 = g2->neighbors[3];
			if (gz==-1) g2 = g2->neighbors[4]; else if (gz==1) g2 = g2->neighbors[5];
			if (g2 == g) continue;
			if (!g2->empty)
			  {
			    int g2_x0 = 0, g2_x1 = GRIDDIM-1; //, g_x0 = 0, g_x1 = GRIDDIM-1, g_dx0 = 0, g_dx1 = 0;
			    int g2_y0 = 0, g2_y1 = GRIDDIM-1; //, g_y0 = 0, g_y1 = GRIDDIM-1, g_dy0 = 0, g_dy1 = 0;
			    int g2_z0 = 0, g2_z1 = GRIDDIM-1; //, g_z0 = 0, g_z1 = GRIDDIM-1, g_dz0 = 0, g_dz1 = 0;
						
			    if (gx<0)      { g2_x0 = GRIDDIM-r; } // g_x1 = 0;  g_dx1 = 1; }
			    else if (gx>0) { g2_x1 = r-1; } // g_x0 = GRIDDIM-r;  g_dx0 = 1; }
						
			    if (gy<0)      { g2_y0 = GRIDDIM-r; } // g_y1 = 0;  g_dy1 = 1; }
			    else if (gy>0) { g2_y1 = r-1; } // g_y0 = GRIDDIM-r;  g_dy0 = 1; }
						
			    if (gz<0)      { g2_z0 = GRIDDIM-r; } // g_z1 = 0;  g_dz1 = 1; }
			    else if (gz>0) { g2_z1 = r-1; } // g_z0 = GRIDDIM-r;  g_dz0 = 1; }
						
			    //std::cout << "accumulate neighbors particles from <"<<g2_x0<<" "<<g2_y0<<" "<<g2_z0<<">-<"<<g2_x1<<" "<<g2_y1<<" "<<g2_z1<<">"<<std::endl;
			    //int z0 = g_z0;
			    //int z1 = g_z1;
			    const Cell* cz = g2->cell+(g2_x0*DX+g2_y0*DY+g2_z0*DZ);
			    for (z = g2_z0; z<=g2_z1; z++, cz+=DZ) //, z0+=g_dx0, z1+=g_dx1)
			      {
				//int y0 = g_y0;
				//int y1 = g_y1;
				const Cell* cy = cz;
				for (y = g2_y0; y<=g2_y1; y++, cy+=DY) //, y0+=g_dy0, y1+=g_dy1)
				  {
				    //int x0 = g_x0;
				    //int x1 = g_x1;
				    const Cell* c = cy;
				    for (x = g2_x0; x<=g2_x1; x++, c++) //, x0+=g_dx0, x1+=g_dx1)
				      {
					if (!c->plist.empty())
					  {
					    typename std::list<Entry>::const_iterator begin = c->plist.begin();
					    typename std::list<Entry>::const_iterator end = c->plist.end();
					    typename std::list<Entry>::const_iterator it;
					    int x0 = x + gx*GRIDDIM - r+1; if (x0<0) x0 = 0;
					    int x1 = x + gx*GRIDDIM + r; if (x1>GRIDDIM-1) x1 = GRIDDIM-1;
					    int y0 = y + gy*GRIDDIM - r+1; if (y0<0) y0 = 0;
					    int y1 = y + gy*GRIDDIM + r; if (y1>GRIDDIM-1) y1 = GRIDDIM-1;
					    int z0 = z + gz*GRIDDIM - r+1; if (z0<0) z0 = 0;
					    int z1 = z + gz*GRIDDIM + r; if (z1>GRIDDIM-1) z1 = GRIDDIM-1;
					    //std::cout << "accumulate neighbors "<<c->plist.size()<<" particles from <"<<pos<<">+<"<<x+gx*GRIDDIM<<" "<<y+gy*GRIDDIM<<" "<<z+gz*GRIDDIM<<"> to area <"<<x0<<" "<<y0<<" "<<z0<<">-<"<<x1<<" "<<y1<<" "<<z1<<">"<<std::endl;
					    Cell* c2 = g->cell+(x0*DX+y0*DY+z0*DZ);
					    const int dy = DY-(x1-x0+1)*DX;
					    const int dz = DZ-(y1-y0+1)*DY;
					    Coord p;
					    for (z2 = z0; z2<=z1; z2++, c2+=dz)
					      {
						p[2] = (pos[2]+z2)*cellWidth;
						for (y2 = y0; y2<=y1; y2++, c2+=dy)
						  {
						    p[1] = (pos[1]+y2)*cellWidth;
						    for (x2 = x0; x2<=x1; x2++, c2+=DX)
						      {
							p[0] = (pos[0]+x2)*cellWidth;
							for (it=begin; it != end; ++it)
							  {
							    Real r2 = (it->pos-p).norm2();
							    if (r2 < dist2)
							      c2->data.add(field, it->index, r2, dist2);
							  }
						      }
						  }
					      }
					  }
				      }
				  }
			      }
			  }
		      }
		  }
	      }
	  }
      }

      template<class DataTypes>
      void SpatialGrid<DataTypes>::begin()
      {
	for (typename Map::iterator itg = map.begin();itg != map.end(); itg++)
	  {
	    Grid* g = itg->second;
	    //g->clear();
	    delete g;
	  }
	map.clear();
      }

      template<class DataTypes>
      void SpatialGrid<DataTypes>::add(int i, const Coord& pos, bool allNeighbors)
      {
	int ix = rfloor(pos[0]*invCellWidth);
	int iy = rfloor(pos[1]*invCellWidth);
	int iz = rfloor(pos[2]*invCellWidth);
	Key k(ix >> GRIDDIM_LOG2, iy >> GRIDDIM_LOG2, iz >> GRIDDIM_LOG2);
	ix &= GRIDDIM-1;
	iy &= GRIDDIM-1;
	iz &= GRIDDIM-1;
	Grid* g = getGrid(k);
	if (g->empty)
	  {
	    g->empty = false;
	    // create direct neighbor grids
	    if (g->neighbors[0] == &emptyGrid) getGrid(Key(k[0]-1, k[1]  , k[2]  ));
	    if (g->neighbors[1] == &emptyGrid) getGrid(Key(k[0]+1, k[1]  , k[2]  ));
	    if (g->neighbors[2] == &emptyGrid) getGrid(Key(k[0]  , k[1]-1, k[2]  ));
	    if (g->neighbors[3] == &emptyGrid) getGrid(Key(k[0]  , k[1]+1, k[2]  ));
	    if (g->neighbors[4] == &emptyGrid) getGrid(Key(k[0]  , k[1]  , k[2]-1));
	    if (g->neighbors[5] == &emptyGrid) getGrid(Key(k[0]  , k[1]  , k[2]+1));
	    if (allNeighbors)
	      {   // create all 26 neighbors
		if (g->neighbors[0]->neighbors[2] == &emptyGrid) getGrid(Key(k[0]-1, k[1]-1, k[2]  ));
		if (g->neighbors[1]->neighbors[2] == &emptyGrid) getGrid(Key(k[0]+1, k[1]-1, k[2]  ));
		if (g->neighbors[0]->neighbors[3] == &emptyGrid) getGrid(Key(k[0]-1, k[1]+1, k[2]  ));
		if (g->neighbors[1]->neighbors[3] == &emptyGrid) getGrid(Key(k[0]+1, k[1]+1, k[2]  ));
		if (g->neighbors[0]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]-1, k[1]  , k[2]-1));
		if (g->neighbors[1]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]+1, k[1]  , k[2]-1));
		if (g->neighbors[0]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]-1, k[1]  , k[2]+1));
		if (g->neighbors[1]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]+1, k[1]  , k[2]+1));
		if (g->neighbors[2]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]  , k[1]-1, k[2]-1));
		if (g->neighbors[3]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]  , k[1]+1, k[2]-1));
		if (g->neighbors[2]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]  , k[1]-1, k[2]+1));
		if (g->neighbors[3]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]  , k[1]+1, k[2]+1));

		if (g->neighbors[0]->neighbors[2]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]-1, k[1]-1, k[2]-1));
		if (g->neighbors[1]->neighbors[2]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]+1, k[1]-1, k[2]-1));
		if (g->neighbors[0]->neighbors[3]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]-1, k[1]+1, k[2]-1));
		if (g->neighbors[1]->neighbors[3]->neighbors[4] == &emptyGrid) getGrid(Key(k[0]+1, k[1]+1, k[2]-1));
		if (g->neighbors[0]->neighbors[2]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]-1, k[1]-1, k[2]+1));
		if (g->neighbors[1]->neighbors[2]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]+1, k[1]-1, k[2]+1));
		if (g->neighbors[0]->neighbors[3]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]-1, k[1]+1, k[2]+1));
		if (g->neighbors[1]->neighbors[3]->neighbors[5] == &emptyGrid) getGrid(Key(k[0]+1, k[1]+1, k[2]+1));
	      }
	  }
	g->cell[ix+GRIDDIM*iy+GRIDDIM*GRIDDIM*iz].plist.push_back(Entry(i,pos));
      }

      template<class DataTypes>
      void SpatialGrid<DataTypes>::end()
      {
	//for (typename Map::iterator itg = map.begin();itg != map.end(); itg++)
	//{
	//	const Key& k = itg->first;
	//	Grid* g = itg->second;
	//	g->neighbors[0] = g;
	//	for (int i=1;i<8;i++)
	//	{
	//		g->neighbors[i] = findGrid(Key(k[0]+(i&1),k[1]+((i>>1)&1),k[2]+((i>>2)&1)));
	//	}
	//	g->neighbors[ 8] = findGrid(Key(k[0]-1,k[1]+1,k[2]  ));
	//	g->neighbors[ 9] = findGrid(Key(k[0]-1,k[1]  ,k[2]+1));
	//	g->neighbors[10] = findGrid(Key(k[0]-1,k[1]+1,k[2]+1));
	//	g->neighbors[11] = findGrid(Key(k[0]-1,k[1]-1,k[2]+1));
	//	g->neighbors[12] = findGrid(Key(k[0]  ,k[1]-1,k[2]+1));
	//	g->neighbors[13] = findGrid(Key(k[0]+1,k[1]-1,k[2]+1));
	//}
      }

/// Change particles ordering inside a given cell have contiguous indices
///
/// Fill the old2new and new2old arrays giving the permutation to apply
template<class DataTypes>
void SpatialGrid<DataTypes>::reorderIndices(helper::vector<unsigned int>* old2new, helper::vector<unsigned int>* new2old)
{
    unsigned int next = 0;
    for (typename Map::iterator itg = map.begin();itg != map.end(); itg++)
    {
	Key k = itg->first;
	Grid* g = itg->second;
	if (g->empty) continue;
	for (int i=0; i<NCELL; ++i)
	{
	    int j=0;
	    for (int s=0;s<GRIDDIM_LOG2;++s)
		for(int c=0;c<3;++c)
		    j += ((i>>(3*s+c))&1)<<(GRIDDIM_LOG2*c+s);
	    Cell* c = g->cell+j;
	    for (typename std::list<Entry>::iterator it = c->plist.begin(), itend = c->plist.end(); it != itend; ++it)
	    {
		unsigned int old = it->index;
		if (old2new != NULL)
		{
		    if (old >= old2new->size()) old2new->resize(old+1);
		    (*old2new)[old] = next;
		}
		if (new2old != NULL)
		{
		    if (next >= new2old->size()) new2old->resize(next+1);
		    (*new2old)[next] = old;
		}
		it->index = next;
		++next;
	    }
	}
    }
}

      template<class DataTypes>
      void SpatialGrid<DataTypes>::draw()
      {
	const float cscale = (float)(cellWidth);
	const float gscale = (float)(cellWidth*GRIDDIM);
	glBegin(GL_LINES);
	for (typename Map::iterator itg = map.begin();itg != map.end(); itg++)
	  {
	    Key k = itg->first;
	    Grid* g = itg->second;
	    glColor3f(1,1,1);
	    glVertex3f((k[0]  )*gscale, (k[1]  )*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]  )*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]+1)*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]+1)*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]  )*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]  )*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]+1)*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]+1)*gscale, (k[2]+1)*gscale);

	    glVertex3f((k[0]  )*gscale, (k[1]  )*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]+1)*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]  )*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]+1)*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]  )*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]+1)*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]  )*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]+1)*gscale, (k[2]+1)*gscale);

	    glVertex3f((k[0]  )*gscale, (k[1]  )*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]  )*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]+1)*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]  )*gscale, (k[1]+1)*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]  )*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]  )*gscale, (k[2]+1)*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]+1)*gscale, (k[2]  )*gscale);
	    glVertex3f((k[0]+1)*gscale, (k[1]+1)*gscale, (k[2]+1)*gscale);

	    if (g->neighbors[0] == &emptyGrid)
	      {
		glColor3f(1.0f,0.0f,0.0f);
		glVertex3f((k[0]     )*gscale, (k[1]+0.3f)*gscale, (k[2]+0.3f)*gscale);
		glVertex3f((k[0]     )*gscale, (k[1]+0.7f)*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]     )*gscale, (k[1]+0.3f)*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]     )*gscale, (k[1]+0.7f)*gscale, (k[2]+0.3f)*gscale);
	      }
	    if (g->neighbors[1] == &emptyGrid)
	      {
		glColor3f(1.0f,0.5f,0.5f);
		glVertex3f((k[0]+1   )*gscale, (k[1]+0.3f)*gscale, (k[2]+0.3f)*gscale);
		glVertex3f((k[0]+1   )*gscale, (k[1]+0.7f)*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]+1   )*gscale, (k[1]+0.3f)*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]+1   )*gscale, (k[1]+0.7f)*gscale, (k[2]+0.3f)*gscale);
	      }
	    if (g->neighbors[2] == &emptyGrid)
	      {
		glColor3f(0.0f,1.0f,0.0f);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]     )*gscale, (k[2]+0.3f)*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]     )*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]     )*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]     )*gscale, (k[2]+0.3f)*gscale);
	      }
	    if (g->neighbors[3] == &emptyGrid)
	      {
		glColor3f(0.5f,1.0f,0.5f);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]+1   )*gscale, (k[2]+0.3f)*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]+1   )*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]+1   )*gscale, (k[2]+0.7f)*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]+1   )*gscale, (k[2]+0.3f)*gscale);
	      }
	    if (g->neighbors[4] == &emptyGrid)
	      {
		glColor3f(0.0f,0.0f,1.0f);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]+0.3f)*gscale, (k[2]     )*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]+0.7f)*gscale, (k[2]     )*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]+0.3f)*gscale, (k[2]     )*gscale);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]+0.7f)*gscale, (k[2]     )*gscale);
	      }
	    if (g->neighbors[5] == &emptyGrid)
	      {
		glColor3f(0.5f,0.5f,1.0f);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]+0.3f)*gscale, (k[2]+1   )*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]+0.7f)*gscale, (k[2]+1   )*gscale);
		glVertex3f((k[0]+0.7f)*gscale, (k[1]+0.3f)*gscale, (k[2]+1   )*gscale);
		glVertex3f((k[0]+0.3f)*gscale, (k[1]+0.7f)*gscale, (k[2]+1   )*gscale);
	      }
	    if (!g->empty)
	      {
		glColor3f(0.5f,0.5f,0.5f);
		int x,y,z;
		for (z = 0; z<=GRIDDIM; z++)
		  for (y = 0; y<=GRIDDIM; y++)
		    {
		      if ((y==0 || y==GRIDDIM) && (z==0 || z==GRIDDIM)) continue;
		      glVertex3f((k[0]*GRIDDIM          )*cscale, (k[1]*GRIDDIM + y    )*cscale, (k[2]*GRIDDIM + z    )*cscale);
		      glVertex3f((k[0]*GRIDDIM + GRIDDIM)*cscale, (k[1]*GRIDDIM + y    )*cscale, (k[2]*GRIDDIM + z    )*cscale);
		    }
		for (z = 0; z<=GRIDDIM; z++)
		  for (x = 0; x<=GRIDDIM; x++)
		    {
		      if ((x==0 || x==GRIDDIM) && (z==0 || z==GRIDDIM)) continue;
		      glVertex3f((k[0]*GRIDDIM + x    )*cscale, (k[1]*GRIDDIM          )*cscale, (k[2]*GRIDDIM + z    )*cscale);
		      glVertex3f((k[0]*GRIDDIM + x    )*cscale, (k[1]*GRIDDIM + GRIDDIM)*cscale, (k[2]*GRIDDIM + z    )*cscale);
		    }
		for (y = 0; y<=GRIDDIM; y++)
		  for (x = 0; x<=GRIDDIM; x++)
		    {
		      if ((x==0 || x==GRIDDIM) && (y==0 || y==GRIDDIM)) continue;
		      glVertex3f((k[0]*GRIDDIM + x    )*cscale, (k[1]*GRIDDIM + y    )*cscale, (k[2]*GRIDDIM          )*cscale);
		      glVertex3f((k[0]*GRIDDIM + x    )*cscale, (k[1]*GRIDDIM + y    )*cscale, (k[2]*GRIDDIM + GRIDDIM)*cscale);
		    }
	      }
	  }
	glEnd();
      }

template<class DataTypes>
SpatialGridContainer<DataTypes>::SpatialGridContainer()
: grid(NULL)
, d_cellWidth(initData(&d_cellWidth, (Real)1.0, "cellWidth", "Width each cell in the grid. If it is used to compute neighboors, it should be greater that the max radius considered."))
, d_showGrid(initData(&d_showGrid, false, "showGrid", "activate rendering of the grid"))
, d_autoUpdate(initData(&d_autoUpdate, false, "autoUpdate", "Automatically update the grid at each iteration."))
, d_sortPoints(initData(&d_sortPoints, false, "sortPoints", "Sort points depending on which cell they are in the grid. This is required for efficient collision detection."))
, mstate(NULL)
{
    this->f_listening.setValue(true);
}

template<class DataTypes>
SpatialGridContainer<DataTypes>::~SpatialGridContainer()
{
    if (grid != NULL)
	delete grid;
}

template<class DataTypes>
void SpatialGridContainer<DataTypes>::init()
{
    mstate = dynamic_cast<core::componentmodel::behavior::MechanicalState<DataTypes>*>(this->getContext()->getMechanicalState());
    grid = new Grid(d_cellWidth.getValue());
}

template<class DataTypes>
void SpatialGridContainer<DataTypes>::reinit()
{
    if (grid == NULL || grid->getCellWidth() != d_cellWidth.getValue())
    {
	if (grid != NULL)
	    delete grid;
	grid = new Grid(d_cellWidth.getValue());
    }
}

template<class DataTypes>
bool SpatialGridContainer<DataTypes>::sortPoints()
{
    if (mstate)
	updateGrid(*mstate->getX());
    if (this->f_printLog.getValue())
	std::cout << "SpatialGridContainer::sortPoints(): sorting..."<<std::endl;
    helper::vector<unsigned int> old2new, new2old;
    grid->reorderIndices(&old2new, &new2old);
    // check if the mapping actually changed something
    bool identity = true;
    for (unsigned int i=0;i<old2new.size();++i)
	if (old2new[i] != i)
	{
	    identity = false;
	    break;
	}
    if (identity)
    {
	if(this->f_printLog.getValue())
	    std::cout << "SpatialGridContainer::sortPoints(): no changes."<<std::endl;
	return false;
    }
    if(this->f_printLog.getValue())
    {
        std::cout << "map:";
        for (unsigned int i=0;i<new2old.size();++i)
            std::cout << " "<<new2old[i]<<"->"<<i;
        std::cout << std::endl;
        std::cout << "invmap:";
        for (unsigned int i=0;i<old2new.size();++i)
            std::cout << " "<<i<<"->"<<old2new[i];
        std::cout << std::endl;
    }

	sofa::component::topology::PointSetTopologyModifier* pointMod; 
	this->getContext()->get(pointMod);

    if (pointMod)
    {
	if(this->f_printLog.getValue())
	    std::cout << "SpatialGridContainer::sortPoints(): renumber using PointSetTopologyModifier."<<std::endl;

		pointMod->renumberPoints(new2old,old2new);
    }
    else
    {
	MechanicalObject<DataTypes>* object = dynamic_cast<MechanicalObject<DataTypes>*>(this->mstate);
	if (object != NULL)
	{
	    if(this->f_printLog.getValue())
		std::cout << "SpatialGridContainer::sortPoints(): renumber using MechanicalObject."<<std::endl;
	    object->renumberValues(new2old);
	}
	else
	{
	    std::cout << "SpatialGridContainer::sortPoints(): no external object supporting renumbering!"<<std::endl;
	}
    }
    return true;
}
template<class DataTypes>
void SpatialGridContainer<DataTypes>::handleEvent(sofa::core::objectmodel::Event* event)
{
    if (/* simulation::AnimateBeginEvent* ev = */ dynamic_cast<simulation::AnimateBeginEvent*>(event))
    //if (simulation::AnimateEndEvent* ev = dynamic_cast<simulation::AnimateEndEvent*>(event))
    {
	if (d_sortPoints.getValue())
	{
	    sortPoints();
	}
	else if (d_autoUpdate.getValue())
	{
	    if (mstate)
		updateGrid(*mstate->getX());
	}
    }
}

template<class DataTypes>
void SpatialGridContainer<DataTypes>::draw()
{
    if (!d_showGrid.getValue())
	return;
    if (grid != NULL)
	grid->draw();
}

} // namespace container

} // namespace component

} // namespace sofa

#endif
