/******************************************************************************************
> [KAZOE]                                                                                 <
> Initial Sofware by [Yannick Verdie, Florent Lafarge], Copyright (C) Inria (GEOMETRICA)  <
> All Rights Reserved, 2012, [Version 1.0].                                               <
******************************************************************************************/
#include "DataTree.h"

int node::ms_minsize;
int node::ms_max_level;
int node::ms_min_ratio;  //if we have more than that number of candidate, we subdivise
int node::ms_threshold;	  //if a pixel has a value higher than this threshold, it is a candidate
int node::m_limit_x,  node::m_limit_y;
std::vector<Point_cell>*	node::ms_list_points;
std::map<Point_cell, int>*     node::ms_map_points_index;
int* node::ms_image; int node::ms_w; int node::ms_h;


Point_cell::Point_cell(int x, int y, int level)
{
	m_x = x;
	m_y = y;
	m_levelP = level;
}

Point_cell::Point_cell(const Point_cell& p)
{
	m_x = p.x();
	m_y = p.y();
	m_levelP = p.getLevelP();
}

Point_cell& Point_cell::operator= (const Point_cell &p)
{
	m_x = p.x();
	m_y = p.y();
	m_levelP = p.getLevelP();

	// return the existing object
	return *this;
}

void Point_cell::set(int x, int y, int level)
{
	m_x = x;
	m_y = y;
	m_levelP = level;
}



//for root
node::node(int x1, int y1, int x2, int y2, int level,int min_size, int max_level,float min_ratio, int threshold, int* image,  vector<Point_cell>	*list_points, map<Point_cell,int> *list_points_index, int limit_x, int limit_y)
{
	m_size = (x2 - x1)*(y2 - y1);

	list_points->push_back(Point_cell(x1,y2-1,0));
	list_points->push_back(Point_cell(x2-1,y2-1,0));
	list_points->push_back(Point_cell(x2-1,y1,0));
	list_points->push_back(Point_cell(x1,y1,0));
	(*list_points_index)[list_points->at(0)] = 0;
	(*list_points_index)[list_points->at(1)] = 1;
	(*list_points_index)[list_points->at(2)] = 2;
	(*list_points_index)[list_points->at(3)] = 3;

	m_levelQ = level;
	ms_minsize = min_size;
	ms_max_level =  max_level;
	ms_list_points = list_points;
	m_limit_x = limit_x;
	m_limit_y = limit_y;
	ms_image = image;
	ms_w = x2;
	ms_h = y2;
	ms_map_points_index = list_points_index;
	m_parent = NULL;
	ms_min_ratio = min_ratio;  //if we have more than that number of candidate, we subdivise
	ms_threshold = threshold;

	int index[] = {0,1,2,3};
	init(m_levelQ, index, true);
}

//for child
node::node(node* parent, int* index, int level, bool not_empty)
{
	m_levelQ = level;
	m_parent = parent;
	int x1 =  ms_list_points->at(index[3]).x();
	int y1 =  ms_list_points->at(index[3]).y();
	int x2 =  ms_list_points->at(index[1]).x();
	int y2 =  ms_list_points->at(index[1]).y();
	m_size = (x2 - x1)*(y2 - y1);

	init(m_levelQ, index, not_empty);
}

//see Spatial Decomposition And Quadtree Mesh
//http://www-tcad.stanford.edu/tcad/pubs/theses/danyang/Chap3.ps
//-----------------------------------------------------------START---------------------------------------------------
void node::balance_tree(node* currentNode)
{
	if (currentNode == 0) return;
	for (int i=0;i<4;i++)
		if (currentNode->subnode(i) != 0)
			node::balance_tree(currentNode->subnode(i));

	// modify the level control function values if necessary *)
	currentNode->ModifyLCF();
	// refine the quadrant *)
	currentNode->SubQuadrantGeneration();
}

void node::recompute_vertices_neighbor(node* currentNode)
{
	for (int i=0;i<4;i++)
		if (currentNode->subnode(i) != 0)
			node::recompute_vertices_neighbor(currentNode->subnode(i));


	currentNode->get_and_update_Neighbor();
}

void node::ModifyLCF()
{
	// modify the level control function for mid-edge vertices if necessary *)
	for (int i=0;i<4;i++)
		//Let pi and pj be two neighbor vertices of p with respect to Q;
	{
		if (getLevelP(i) == getLevelQ())
			if (getLevelPi(i) == getLevelP(i) || getLevelPj(i) == getLevelP(i))
			{
				int levelQ = getLevelQ();
				//(* modified the L values for the mid-edge vertices *)
				if (getLevelPmi(i) == levelQ)
					setLevelPmi(i,levelQ+1);

				if (getLevelPmj(i) == levelQ)
					setLevelPmj(i,levelQ+1);

				//(* modified the L value for the center vertex *)
				if (getLevelPc() == levelQ)
					setLevelPc(levelQ+1);
			}

	}
}

bool node::isIrregular()
{
	int nb_q = 0;
	for (int i=0;i<4;i++)
		if (subnode(i) != 0)
			nb_q++;

	return (nb_q != 0 && nb_q != 4);
}

void node::SubQuadrantGeneration()
{
	for (int i=0;i<4;i++)
	{
		if (getLevelPi(i) > getLevelQ())
		{
			//(* create a sub-quadrant around vertex pi *)
			node* Qi = SubQuadrant(i,false);
			balance_tree(Qi);
		}else if(isIrregular() && getLevelPc() > getLevelQ() + 1)
		{
			//(* create a sub-quadrant around vertex pi *)
			node* Qi = SubQuadrant(i,false);
			balance_tree(Qi);
			//Delete Qi but not its subquadrants;
		}
	}
}

//------------------------------------------------------

void node::init(int level,  int* ind, bool recursive_build)
{
	m_index_points_in_vector[0] = ind[3];	m_index_points_in_vector[1] = -1;	m_index_points_in_vector[2] = ind[2];
	m_index_points_in_vector[3] = -1;		m_index_points_in_vector[4] = -1;	m_index_points_in_vector[5] = -1;
	m_index_points_in_vector[6] = ind[0];	m_index_points_in_vector[7] = -1;	m_index_points_in_vector[8] = ind[1];

	m_subnode[0] = 0;m_subnode[1] = 0;m_subnode[2] = 0;m_subnode[3] = 0;//ll, lr, ur, ul
	//------------------------
	//printf("mlevel, %d , msize %d\n",level, m_size); 
	if (m_size > ms_minsize*ms_minsize && recursive_build && level < ms_max_level)//the stop criteria
		SubQuadrant(0, recursive_build);
}

bool node::isWithDataInside(int* index)
{
	int x1 =  ms_list_points->at(index[3]).x();
	int y1 =  ms_list_points->at(index[3]).y();
	int x2 =  ms_list_points->at(index[1]).x();
	int y2 =  ms_list_points->at(index[1]).y();

	int count = 0;
	for (int j=y1;j<y2 && j < ms_h;j++)
		for (int i=x1;i<x2 && i < ms_w;i++)
			if (ms_image[i+ms_w*j] >= ms_threshold)
				count++;


	return (count/(m_size/4.) >  ms_min_ratio);
}


std::vector<Point_cell> node::get_and_update_Neighbor()
{
	//1 subdivide
	//n0------------n1------------n2
	// |             |            |
	//n3------------n4------------n5
	// |             |            |
	//n6------------n7------------n8
	//with n6 = mPoints_grid_result[mIndex_Face[0]]
	//with n8 = mPoints_grid_result[mIndex_Face[1]]
	//with n2 = mPoints_grid_result[mIndex_Face[2]]
	//with n0 = mPoints_grid_result[mIndex_Face[3]]

	std::vector<Point_cell> n(9);
	n[0] = (*ms_list_points)[m_index_points_in_vector[0]];
	n[8] = (*ms_list_points)[m_index_points_in_vector[8]];
	n[6] = (*ms_list_points)[m_index_points_in_vector[6]];
	n[2] = (*ms_list_points)[m_index_points_in_vector[2]];				 //  MOT05809184

	n[1] = Point_cell((n[0].x()+n[2].x())/2,n[0].y(),getLevelQ());
	n[3] = Point_cell(n[0].x(),(n[0].y()+n[6].y())/2,getLevelQ());
	n[5] = Point_cell(n[2].x(),(n[2].y()+n[8].y())/2,getLevelQ());
	n[7] = Point_cell((n[6].x()+n[8].x())/2,n[6].y(),getLevelQ());
	n[4] = Point_cell((n[3].x()+n[5].x())/2,n[3].y(),getLevelQ());

	std::map<Point_cell,int>::iterator it;

	//found the existing point within the neighborhood
	it = ms_map_points_index->find(n[1]);
	if (it != ms_map_points_index->end())
		m_index_points_in_vector[1] = it->second;

	it = ms_map_points_index->find(n[3]);
	if (it != ms_map_points_index->end())
		m_index_points_in_vector[3] = it->second;

	it = ms_map_points_index->find(n[5]);
	if (it != ms_map_points_index->end())
		m_index_points_in_vector[5] = it->second;

	it = ms_map_points_index->find(n[7]);
	if (it != ms_map_points_index->end())
		m_index_points_in_vector[7] = it->second;

	it = ms_map_points_index->find(n[4]);
	if (it != ms_map_points_index->end())
		m_index_points_in_vector[4] = it->second;

	return n;
}

bool node::isNotOutside(int* index)
{
	int x1 =  ms_list_points->at(index[3]).x();
	int y1 =  ms_list_points->at(index[3]).y();
	int x2 =  ms_list_points->at(index[1]).x();
	int y2 =  ms_list_points->at(index[1]).y();

	//return true;
	return !(x1 >= m_limit_x || y1 >= m_limit_y);
}

node* node::SubQuadrant(int a, bool recursive_build)
{	

	std::vector<Point_cell> n = get_and_update_Neighbor();

	//---------------update the L cost
	setLevelP(0,max(getLevelQ(),getLevelP(0)));
	setLevelP(1,max(getLevelQ(),getLevelP(1)));
	setLevelP(2,max(getLevelQ(),getLevelP(2)));
	setLevelP(3,max(getLevelQ(),getLevelP(3)));
	//-----------------------------------------

	//pointer in correct order
	static const int orderlistPoints[][4] = {{6,7,4,3},{7,8,5,4},{4,5,2,1},{3,4,1,0}};

	for (int i= 0;i<4;i++)
	{
		if (m_subnode[i] == 0)
		{
			int new_index[4];

			for (int j= 0;j<4;j++)
			{
				new_index[j] = m_index_points_in_vector[orderlistPoints[i][j]];
				if (new_index[j] == -1)
				{
					ms_list_points->push_back(n[orderlistPoints[i][j]]);								//add point in the grid
					new_index[j] = ms_list_points->size()-1;										//save index for child node
					m_index_points_in_vector[orderlistPoints[i][j]]		= new_index[j];				//save index for this node
					(*ms_map_points_index)[ n[orderlistPoints[i][j]] ]	= new_index[j];				//update map point index in order to avoid duplicate vertex
				}
			}

			if (isWithDataInside(new_index) && isNotOutside(new_index))
				m_subnode[i] = new node(this,new_index,m_levelQ+1,true);

			//here we create empty cell
			//m_subnode[i] = new node(this,new_index,m_levelQ+1,isWithDataInside(new_index));
		}
	}

	return m_subnode[a];
} 


ImgSaver::ImgSaver(int* vecValues ,int hwidth,int hheight )
{
	ptrCImgInt = new cimg_library::CImg<int> ( hwidth , hheight , 1 , 4 );
	width = hwidth; height = hheight;
	int i = 0;
	for ( int y = 0 ; y < height ; y++ )
		for ( int x = 0 ; x < width  ; x++ )
		{
			(*ptrCImgInt)(x,y,0,0) = vecValues[i];
			(*ptrCImgInt)(x,y,0,1) = vecValues[i];
			(*ptrCImgInt)(x,y,0,2) = vecValues[i];
			(*ptrCImgInt)(x,y,0,3) = 255;
			i++;
		}
}

ImgSaver::ImgSaver(int hwidth,int hheight )
{
	ptrCImgInt = new cimg_library::CImg<int> ( hwidth , hheight , 1 , 4 , 0);  //x,y,z,nbchannel,defVal
	width = hwidth; height = hheight;
}

 void ImgSaver::blank()
 {
	ptrCImgInt->fill(0);
 }

ImgSaver::~ImgSaver()
{
	if (ptrCImgInt) delete ptrCImgInt;
}

void ImgSaver::applyTo ( int x, int y )
{
	if ( x >= 0 && x < width &&
		y >= 0 && y < height )
	{
		if ( ptrCImgInt != NULL )
		{
			(*ptrCImgInt)(x,y,0,0) = 255;
			(*ptrCImgInt)(x,y,0,1) = 0;
			(*ptrCImgInt)(x,y,0,2) = 0;
			(*ptrCImgInt)(x,y,0,3) = 255;
		}
	}
}	

//from http://paulbourke.net/texture_colour/colourramp/
/*
   Return a RGB colour value given a scalar v in the range [vmin,vmax]
   In this case each colour component ranges from 0 (no contribution) to
   1 (fully saturated), modifications for other ranges is trivial.
   The colour is clipped at the end of the scales if v is outside
   the range [vmin,vmax]
*/
   typedef struct {
      float r,g,b;
   } COLOR;

COLOR GetColor(float v,float vmin,float vmax)
{
   COLOR c = {1.0,1.0,1.0}; // white
   float dv;

   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;
   dv = vmax - vmin;

   if (v < (vmin + 0.25 * dv)) {
      c.r = 0;
      c.g = 4 * (v - vmin) / dv;
   } else if (v < (vmin + 0.5 * dv)) {
      c.r = 0;
      c.b = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
   } else if (v < (vmin + 0.75 * dv)) {
      c.r = 4 * (v - vmin - 0.5 * dv) / dv;
      c.b = 0;
   } else {
      c.g = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
      c.b = 0;
   }

   return(c);
}


void ImgSaver::createColorMap ( int vmin , int vmax)
{
	for(int x = 0; x < width;x++)
		for(int y = 0; y < height;y++)
			if ( ptrCImgInt != NULL )
			{
				 COLOR c = GetColor((*ptrCImgInt)(x,y,0,0),vmin,vmax);
				(*ptrCImgInt)(x,y,0,0) = c.r*255;
				(*ptrCImgInt)(x,y,0,1) = c.g*255;;
				(*ptrCImgInt)(x,y,0,2) = c.b*255;;
				(*ptrCImgInt)(x,y,0,3) = 255;
			}
}

void ImgSaver::createColorMap_Florent( int vmin , int vmax)
{
	int val;
	for(int x = 0; x < width;x++)
		for(int y = 0; y < height;y++)
			if ( ptrCImgInt != NULL )
			{
				val = min(max(255*(1-float((*ptrCImgInt)(x,y,0,0)-vmin)/(vmax-vmin)),0),255);   //make sure between 0 and 255
				(*ptrCImgInt)(x,y,0,1) = val;
				(*ptrCImgInt)(x,y,0,2) = val;
				(*ptrCImgInt)(x,y,0,0) = 255;
				(*ptrCImgInt)(x,y,0,3) = 255;
			}
}

void ImgSaver::applyTo ( int x, int y , int val)
{
	if ( x >= 0 && x < width &&
		y >= 0 && y < height )
	{
		if ( ptrCImgInt != NULL )
		{
			(*ptrCImgInt)(x,y,0,0) = val;
			(*ptrCImgInt)(x,y,0,1) = 0;
			(*ptrCImgInt)(x,y,0,2) = 0;
			(*ptrCImgInt)(x,y,0,3) = val;
		}
	}
}


void ImgSaver::save ( const char* const pathPicture )
{
	if ( ptrCImgInt     != NULL ) ptrCImgInt -> save ( pathPicture );
}


quadtree::quadtree(int* image, int w, int h, int min_size, bool initRand, int border, bool force_squared_and_min_size)
{
	int max_level = 50;
	float min_ratio = 0.9;
	int threshold = 5;
	m_give_index = false;

	if (border <= 0)
	{
		m_image = (int*) malloc(w*h*sizeof(int));
		memcpy(m_image, image, w*h*sizeof(int));
		m_w = w; m_h = h;
	    border = 0;
	}else{
		if (force_squared_and_min_size)
		{
			float R = max(w,h) + 2 * border;
			max_level = ceil(log(R/min_size)/log(2.));	 //log is ln in c/c++
			m_w = m_h = min_size*pow(2.,max_level);	 
			//printf("quadree x%d w%d border%d\n",m_w, m_h, border); 
		}else{
			//create image with black boundary around of size elargeSize
			m_w = w + 2 * border;m_h = h + 2 * border;
		}

		m_image = (int*) malloc(m_w*m_h*sizeof(int));
		memset(m_image, 0, m_w*m_h*sizeof(int));

		for (int i=0;i< h ;i++)
			memcpy(m_image+(border + i)*m_w + border,image+i*w,w*sizeof(int));

	}

	m_force_squared_and_min_size = force_squared_and_min_size;
	m_min_size = min_size;
	m_w_image_orig = w; m_h_image_orig = h;
	m_border = border;
	m_avg_cell = 0;

	m_rootnode = new node(0,0,m_w,m_h,0,min_size,max_level, min_ratio, threshold, m_image, &list_points, &map_point_index, w + 2 * border, h + 2 * border);
}

quadtree::quadtree(int* image, int w, int h, bool initRand)
{
	m_give_index = false;
	m_image = image;m_w = w;m_h = h;
	m_w_image_orig = w; m_h_image_orig = h;
	m_border = 0;
	m_avg_cell = 0;
	int min_size = 24;
	int max_level = 5;
	float min_ratio = 0.9;
	int threshold = 5;

	m_rootnode = new node(0,0,m_w,m_h,0,min_size,max_level, min_ratio, threshold, m_image, &list_points, &map_point_index);
}

quadtree::~quadtree()
{
	kill(m_rootnode);
	free(m_image);
}

void quadtree::kill(node* n)
{
	//if (!n) return;

	for (int i=0;i<4;i++)
		if (n->subnode(i))
			kill(n->subnode(i));

	delete n;
	n = NULL;

}

void quadtree::saveImage(char* Path, char* name)
{
	 printf("%d %d\n"   , m_w , m_h);
	std::ostringstream stm;
	ImgSaver saver(   m_image , m_w, m_h );
	stm << Path<<"/"<<name<<"_Image.bmp";
	saver . save ( stm.str().c_str() );
}


void quadtree::draw(char*  ptrPath)
{	        
	ImgSaver saver(   m_image , m_w, m_h );
	draw_cells(&saver, m_rootnode);
	saver . save ( ptrPath );
}

void quadtree::draw_levels(std::string Path, std::string name, bool blackImage)
{	        
	int level = 0; bool more = false;
	while(true)
	{
		ImgSaver saver(   m_image , m_w, m_h );;
		if (blackImage) 
			saver.blank();

		draw_cells_fromLevel(&saver, m_rootnode,level, &more);//return level + 1 if available, -1 otherwise
		std::ostringstream stm;
		stm << Path<<"/"<<name<<"_L"<<level<<".bmp";
		saver . save ( stm.str().c_str() );
		if (more)level++;else break;

		more = false;
	}
}


void quadtree::balance()
{
	node::balance_tree(m_rootnode);
	node::recompute_vertices_neighbor(m_rootnode);
}

void quadtree::prepare()
{
   	prepare(m_rootnode,0);
	coalescingMem();
}


//this tell us which grid and level to run
unsigned int quadtree::getOffset(int method)
{

   float selected;
   unsigned int offset = 0;
   
	selected = rand()%(1+CDF.back());
    while(selected > CDF[offset]){offset++; if (offset == CDF.size() -1)break; };
    return offset;

			
}

unsigned int quadtree::getStartIndex(int offset)
{
	if (offset == 0) return 0;
    return CDF[offset-1];
}

unsigned int quadtree::getCDF(int offset)
{
   return CDF[offset];
}

unsigned int quadtree::getNbCells(int offset)
{
	return nb_cells[offset];
}


void quadtree::coalescingMem()
{
	//	level:		| L00 L01 L02 L03 | L10 L11 L12 L13 |	... such as Lij is Level i, grid j (j E [0,3])
	//	nb_cell		|  1   0   0   0  | ...
	//	offset      |  0   1   2   3  | ...
    //	cdf         |  1   1   1   1  | ...


	cell tmp;
	//vector is memory aligned (continuous)
	nb_cells.resize(list_cells.size()*4,0);
	CDF.resize(list_cells.size()*4,0);

	//printf("in coalescing w%d b%d\n",m_w_image_orig,m_border);

	//int x_grid, y_grid;
	unsigned int val;
	for (int i=0;i<	list_cells.size();i++)	 //level
	{
		for (int j=0;j<	list_cells[i].size();j++)  //micset
		{
			  CDF[i*4 + j] = (i==0 && j==0 ? 0 : CDF[i*4 + j - 1] );	
			 nb_cells[i*4 + j] = list_cells[i][j].size();//printf("%d %d %d\n",i,j,nb_cells[i*4 + j]);

			 //Those different proba have been tested in the Journal paper
			 //"Detecting_parametric_objects_in_large_scenes_by_Monte_Carlo_sampling"
			 //--------------------------------------------------------------------------------------
			 //CDF[i*4 + j] += 500*expf( - powf( i - *(list_cells.size()-1)/10 , 2)/5); //prob 5  50*(EXP(-((B7-2*MAX(B7:B38)/3 )^2)/5))      = (-(level+1) + b1) / sum 	
		     //CDF[i*4 + j] += (50.f*powf(nb_cells[i*4 + j]+1,0.1f));                   //prob 4   (nbCell+1)^0.1	/sum
			 //CDF[i*4 + j] += 50*(list_cells.size()/(i+1));                            //prob3   (1/(level+1))/sum	   =   ((maxLevel+1)/(level+1))	/ sum
			 //CDF[i*4 + j] +=  50*(i+1);	                                            //prob2	  (level+1)/sum
			 //CDF[i*4 + j] +=  50;	                                                    //prob 1   1 / sum
			 CDF[i*4 + j] = nb_cells[i*4 + j]+1;							            //prob0	   (nbCell+1)	/sum
			 //--------------------------------------------------------------------------------------

			 int nb_cell_w = ceil( float(m_w)/m_min_size );

			 for (int k=0;k<list_cells[i][j].size();k++)
			 {
				tmp = list_cells[i][j][k];

				if (m_force_squared_and_min_size)
				{

					if (m_give_index)//m_min_size
					{
						val = ((tmp.m_y/m_min_size) * nb_cell_w+ (tmp.m_x/m_min_size));
					}
					else
					{
						val = (tmp.m_y * (m_w_image_orig+2*m_border)+ tmp.m_x);
					}
					arrayx.push_back(val);

				}else{
					arrayx.push_back(tmp.m_x);
					arrayy.push_back(tmp.m_y);
					arrayw.push_back(tmp.m_w);
					arrayh.push_back(tmp.m_h);
				}
			 }
		}
	}

	//system("pause");
	//avg nb cell = sum{ i[0,n] (16^i)/	 sum{ j[0,n] (4^j)}}
	m_avg_cell = 0;
	for (int i=0;i<CDF.size();i++)
	m_avg_cell+=nb_cells[i]*nb_cells[i]/CDF.back();
	//system("pause");
	//printf("nb_cell avg birth %d\n",m_avg_cell);
}

void quadtree::prepare(node* n, int index_subnode)
{
	if (n == 0) return; 
	if (list_cells.size() < n->getLevelQ()+1)	//create the vector
	{
		vector<cell> yop;
		vector<vector<cell> > yop2(4,yop);
		list_cells.push_back(yop2);
	}

	int id_grid = index_subnode;// E [0 , 3]
	int x1 =  list_points.at(n->indexFace(3)).x();
	int y1 =  list_points.at(n->indexFace(3)).y();
	int x2 =  list_points.at(n->indexFace(1)).x();
	int y2 =  list_points.at(n->indexFace(1)).y();

	list_cells[n->getLevelQ()][id_grid].push_back(cell(x1,y1,x2-x1,y2-y1));

	//this is the order I need, so I use subnodeinOrder(int i)
	//important for coalescingMem()
	//-------------
	//|	Li0	| Li1 |
	//|	--- + --- |
	//|	Li2	| Li3 |
	//-------------

   for (int i=0;i<4;i++)
		if (n->subnodeinOrder(i) != 0)
			prepare(n->subnodeinOrder(i),i);
			
}

void quadtree::draw_grid(int offset, std::string Path, std::string name)
{	        
	int level = offset/4;
	int grid = offset%4;

	if (list_cells.size() < level) return;

		ImgSaver saver (   m_image , m_w, m_h );
		
		 for (int i=0;i<list_cells[level][grid].size();i++)
		 {
			 list_cells[level][grid][i].draw(&saver);
		 }
		std::ostringstream stm;
		stm << Path<<"/"<<name<<"_L"<<level<<"_G"<<grid<<".bmp";
		saver . save ( stm.str().c_str() );

}

void quadtree::draw_pixelMap(std::string Path, std::string name)
{	        
	if (list_cells.size() > 255) return;

		ImgSaver saver ( m_w, m_h );
	
	for (int i=0;i<list_cells.size();i++)
		 for (int j=0;j<list_cells[i].size();j++)
			 for (int k=0;k<list_cells[i][j].size();k++)
				 list_cells[i][j][k].increase(&saver);

		saver.createColorMap ( 1 , list_cells.size());
		std::ostringstream stm;
		stm << Path<<"/"<<name<<"_pixelMap_level.bmp";
		saver . save ( stm.str().c_str() );

}



//done for Florent
void quadtree::draw_pixelMap_pretty(std::string Path, std::string name)
{	 

	if (list_cells.size() > 255) return;

		ImgSaver saver ( m_w, m_h );
	
		//create the white-red image
	
	unsigned max = 1;
	unsigned int tmp;	
	for (int i=0;i<list_cells.size();i++)//level i
	{
		 for (int j=0;j<list_cells[i].size();j++)//subset j  
		 {	 
			 for (int k=0;k<list_cells[i][j].size();k++)   //cell k
			 {
				 //tmp = list_cells[i][j][k].increase(&saver,list_cells[i][j].size());     //prob0
				 //tmp = list_cells[i][j][k].increase(&saver,1);	                     //prob1
				 //tmp = list_cells[i][j][k].increase(&saver,i);                         //prob2
				 tmp = list_cells[i][j][k].increase(&saver,(list_cells.size()+1)/(i+1));     //prob3

				 if (tmp > max) max  = tmp;
			 }
		 }
	}
	  

		saver.createColorMap_Florent( 1 , max);//		 count =  list_cells[i].size();
		 
		 //add the grid on top
		 draw_cells(&saver, m_rootnode, 0);


		std::ostringstream stm;
		stm << Path<<"/"<<name<<"_pixelMap_level_Florent.bmp";
		saver . save ( stm.str().c_str() );

}

void quadtree::draw_pixelMap_grid(std::string Path, std::string name)
{	        
	if (list_cells.size() > 255) return;

	int max = 0, tmp;

	ImgSaver saver ( m_w, m_h );
	
	for (int i=0;i<list_cells.size();i++)   //level	i
		 for (int j=0;j<list_cells[i].size();j++)   //grid j
			 for (int k=0;k<list_cells[i][j].size();k++) //cell k
			 {
				 tmp = list_cells[i][j][k].increase(&saver, list_cells[i][j].size());
				 if (tmp > max) max = tmp;
			 }
		printf("max %d\n",max);
		saver.createColorMap ( 1 , max);
		std::ostringstream stm;
		stm << Path<<"/"<<name<<"_pixelMap_grid.bmp";
		saver . save ( stm.str().c_str() );

}

void quadtree::draw_grid_coal(int offset, std::string Path, std::string name)
{	
	const unsigned int nb_thread_total = getNbCells(offset);
	const unsigned int positionStart = getStartIndex(offset);
	const unsigned int size = getSizeCell(offset);
	const unsigned int level = offset/4;
	const unsigned int grid = offset - level*4;


	printf("level %d size%d (%d %d)\n",level,size, nb_thread_total, list_cells[level][grid].size());

	if (list_cells.size() < level) return;

	ImgSaver saver (   m_image , m_w, m_h );
	
	int x, y;
	for (int k=positionStart;k<positionStart+nb_thread_total;k++)
	{
		if (m_force_squared_and_min_size)
		{
			y = arrayx[k] / (m_w_image_orig+2*m_border);
			x = arrayx[k] - y * (m_w_image_orig+2*m_border);

			for (int i=x;i<x + size && i < saver.getWidth();i++)
			{
				saver.applyTo ( i,y, 255 );
				saver.applyTo ( i,y + size, 255 );
			}

			for (int j=y ;j< y + size && j < saver.getHeight();j++)
			{
				saver.applyTo ( x,j, 255 );
				saver.applyTo ( x + size,j, 255 );
			}
		}else{

			for (int i=arrayx[k];i<arrayx[k]+arrayw[k] && i < saver.getWidth();i++)
			{
				saver.applyTo ( i,arrayy[k], 255 );
				saver.applyTo ( i,arrayy[k] + arrayh[k], 255 );
			}

			for (int j=arrayy[k];j<arrayy[k]+arrayh[k] && j < saver.getHeight();j++)
			{
				saver.applyTo ( arrayx[k],j, 255 );
				saver.applyTo ( arrayx[k] + arrayw[k],j, 255 );
			}


		}
	}

	std::ostringstream stm;
	stm << Path<<"/"<<name<<"_L"<<level<<"_G"<<grid<<"_coal.bmp";
	saver . save ( stm.str().c_str() );

}


void quadtree::draw_cells_fromLevel(ImgSaver *saver, node* n, int level,bool *more)//return level + 1 if available, -1 otherwise
{

	for (int i=0;i<4;i++)
		if (n->getLevelQ() < level)
			if (n->subnode(i) != 0)
				draw_cells_fromLevel(saver, n->subnode(i), level, more);


	if (n->getLevelQ() == level)
	{
		int x1 =  list_points.at(n->indexFace(3)).x();
		int y1 =  list_points.at(n->indexFace(3)).y();
		int x2 =  list_points.at(n->indexFace(1)).x();
		int y2 =  list_points.at(n->indexFace(1)).y();

		for (int i=x1;i<x2 && i < m_w;i++)
		{
			saver->applyTo ( i,y1, 255 );
			saver->applyTo (i ,y2, 255 );
		}

		for (int j=y1;j<y2 && j < m_h;j++)
		{
			saver->applyTo ( x1,j, 255 );
			saver->applyTo ( x2,j, 255 );
		} 

		
		
		if (n->isEnd())
		{
			//this draw a dot on terminal node
			saver->applyTo ( (x2+x1)/2.,(y2+y1)/2., 255 );
		}
		else
		{
			*more = true;
		}
			
	}
}

void quadtree::draw_cell(ImgSaver *saver, int x1, int y1, int x2, int y2, bool drawCenter, int color)
{

	for (int i=x1;i<x2 && i < m_w;i++)
	{
		saver->applyTo ( i,y1, color );
		saver->applyTo ( i,y2, color );
	}

	for (int j=y1;j<y2 && j < m_h;j++)
	{
		saver->applyTo ( x1,j, color );
		saver->applyTo ( x2,j, color );
	} 

	if (drawCenter)
		saver->applyTo ( (x2+x1)/2.,(y2+y1)/2., 255 );
}

void quadtree::draw_cells(ImgSaver *saver, node* n, int color)
{

	for (int i=0;i<4;i++)
		if (n->subnode(i) != 0)
			draw_cells(saver,n->subnode(i), color);


	int x1 =  list_points.at(n->indexFace(3)).x();
	int y1 =  list_points.at(n->indexFace(3)).y();
	int x2 =  list_points.at(n->indexFace(1)).x();
	int y2 =  list_points.at(n->indexFace(1)).y();

	draw_cell(saver, x1, y1, x2, y2, false/*n->isEnd()*/, color);
}
