/* ImLib3D
 * Copyright (c) 2001, ULP-IPB Strasbourg.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 * 
 * This program 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
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
/*! \file   Neighbors3D.hpp */

#ifndef _Neighbors3D_hpp
#define _Neighbors3D_hpp

#include<ImLib3D/Image3D.hpp>
#include<ImLib3D/CppTools.hpp>
#include<ImLib3D/Vect3D.hpp>
#include<vector>
#include<map>
#include<algorithm>



template<class Typ>
class Neighbors3D : public Container3D< vector< pair< Vect3Df , Typ > > >
{
	RectZone3Df *support;// cache ptr to property, for faster acces
public:
	typedef Container3D< vector< pair< Vect3Df , Typ > > > _Base;
	typedef pair<Vect3Df,Typ> VT;
	typedef vector<VT > Im3DValue;
	Vect3Df RPosToIPos(const Vect3Df &p)
	{
		return Support().SelfCoords(p)%(Vect3Df)SizeV();
	}
	RectZone3Df NeighborZoneF(const Vect3Df center,double radius)
	{
		Vect3Df r(radius,radius,radius);
		return RectZone3Df(RPosToIPos(center-r),
						   RPosToIPos(center+r));
	}
	RectZone3Di NeighborZone(const Vect3Df center,double radius)
	{
		RectZone3Df fzone=NeighborZoneF(center,radius);
		RectZone3Df imzone(Vect3Df(0,0,0),(Vect3Df)SizeV()-Vect3Df(1,1,1));
		RectZone3Df t=fzone.Intersection(imzone);
		return RectZone3Di((int)t.x0,(int)t.y0,(int)t.z0,
						   (int)ceil(t.x1),(int)ceil(t.y1),(int)ceil(t.z1 ));
	}
	Im3DValue NearestNeighbors(const Vect3Df centerR,uint n=1)
	{
		// center point in image coords
		Vect3Df centerI(RPosToIPos(centerR));
		typedef pair<float,VT> ppair;
		Vect3Di cp((int)centerI.x,(int)centerI.y,(int)centerI.z);
		typedef multimap<float,VT > Maptyp;
		// list of candidate neighbors
		multimap<float,VT > neighbors;
		int mx0=-1;
		float dmax02=-1;
		// walk the voxels concentrically, starting from center
		typename _Base::iteratorBoxConcentric p(cp);
		for(p=begin();p!=end();++p)
		{
//  			p.Pos().Show("p:","\n");
			// check if we've gone far enough
			Vect3Di dp=p.Pos()-cp;
			int mx=max(abs(dp.x),max(abs(dp.y),abs(dp.z)));
			if(mx0>=0 && mx>mx0){break;}
			// incorporate this voxel's particles to the sorted candidate nn list
			for(uint i=0;i<(*p).size();i++)
			{
				VT neighbor=(*p)[i];
				float dist2=neighbor.first.Distance2(centerR);
				neighbors.insert(ppair(dist2,neighbor));
			}
			// if we have enough neighbors, see how far away we have to go
			// to be sure we've got them all
			if(neighbors.size()>=n)
			{
				// find distance of current n'th nearest neighbor
				typename Maptyp::iterator it;uint i;
				for(it=neighbors.begin(),i=1;i<n;++i,it++){;}
				float dmax2=(*it).first;
				// trash all other neigbors >n 
				if(neighbors.size()>n){it++;neighbors.erase(it,neighbors.end());}
				// see how far we must go
				if(dmax02<0 || dmax2<dmax02)
				{
					dmax02=dmax2;
					float dmax=sqrt(dmax2);
					Vect3Df vmax(dmax,dmax,dmax);
					Vect3Df pminI=RPosToIPos(centerR-vmax);
					Vect3Df pmaxI=RPosToIPos(centerR+vmax);
					Vect3Di pmin((int)pminI.x,(int)pminI.y,(int)pminI.z);
					Vect3Di pmax((int)pmaxI.x,(int)pmaxI.y,(int)pmaxI.z);
//  					pminI.Show("pminI:","\n");
//  					pmaxI.Show("pmaxI:","\n");
//  					pmin.Show("pmin :","\n");
//  					pmax.Show("pmax :","\n");
					Vect3Di dp=pmin-cp;
					mx0=max(abs(dp.x),max(abs(dp.y),abs(dp.z)));
					dp=pmax-cp;
					mx0=max(mx0,max(abs(dp.x),max(abs(dp.y),abs(dp.z))));
//    					mprintf("dmax:%f mx0:%d cp:%d %d %d\n",dmax,mx0,cp.x,cp.y,cp.z);
				}
			}
		}
		if(neighbors.size()<n){ThrowError("NearestNeighbors found only:%d requested:%d",neighbors.size(),n);}
//    		mprintf("found %d neighbors. requested %d\n",neighbors.size(),n);
		vector<VT> res;
		res.reserve(n);
		typename Maptyp::iterator it;
		uint i;
		for(it=neighbors.begin(),i=0;i<n;++i,it++){res.push_back((*it).second);}
		return res;
	}

//FIXME, needs testing
#ifdef NOTDEF 
	VT NearestNeighbor(const Vect3Df centerR)
	{
		// center point in image coords
		Vect3Df centerI(RPosToIPos(centerR));
		typedef pair<float,VT> ppair;
		Vect3Di cp((int)centerI.x,(int)centerI.y,(int)centerI.z);
		Vect3Df nearest;
		float distMin2=-1;
		VT res;

		int mx0=-1;
		float dmax02=-1;
		bool talk=false;
		if(!(rand()%10000)){talk=true;}
		
		if(talk){centerI.Show("nearest neighbor at:","\n");}
		// walk the voxels concentrically, starting from center
		typename ImageType::iteratorBoxConcentric p(cp);
		for(p=begin();p!=end();++p)
		{
  			if(talk)p.Pos().Show("starting box p:","\n");
  			if(talk)printf("number of points here:%d min:%f\n",(*p).size(),distMin2);
			// check if we've gone far enough
			Vect3Di dp=p.Pos()-cp;
			int mx=max(abs(dp.x),max(abs(dp.y),abs(dp.z)));
			if(mx0>=0 && mx>mx0){break;}
			// incorporate this voxel's particles to the sorted candidate nn list
			for(uint i=0;i<(*p).size();i++)
			{
				const VT &neighbor=(*p)[i];
				float dist2=neighbor.first.Distance2(centerR);
				if(talk){neighbor.first.Show(SPrintf("%f:",dist2),"\n");}
				if(dist2<distMin2 || distMin2<0){nearest=neighbor.first;distMin2=dist2;res=neighbor;}
			}
			// if we have enough neighbors, see how far away we have to go
			// to be sure we've got them all
			if(distMin2>=0)
			{
				float dmax2=distMin2;
				// see how far we must go
				if(dmax02<0 || dmax2<dmax02)
				{
					dmax02=dmax2;
					float dmax=sqrt(dmax2);
					Vect3Df vmax(dmax,dmax,dmax);
					Vect3Df pminI=RPosToIPos(centerR-vmax);
					Vect3Df pmaxI=RPosToIPos(centerR+vmax);
					Vect3Di pmin((int)pminI.x,(int)pminI.y,(int)pminI.z);
					Vect3Di pmax((int)pmaxI.x,(int)pmaxI.y,(int)pmaxI.z);
//  					pminI.Show("pminI:","\n");
//  					pmaxI.Show("pmaxI:","\n");
//  					pmin.Show("pmin :","\n");
//  					pmax.Show("pmax :","\n");
					Vect3Di dp=pmin-cp;
					mx0=max(abs(dp.x),max(abs(dp.y),abs(dp.z)));
					dp=pmax-cp;
					mx0=max(mx0,max(abs(dp.x),max(abs(dp.y),abs(dp.z))));
//    					mprintf("dmax:%f mx0:%d cp:%d %d %d\n",dmax,mx0,cp.x,cp.y,cp.z);
				}
			}
		}
		if(distMin2<0){ThrowError("NearestNeighbors found no neighbors");}
		return res;
	}
#endif// NOTDEF 
	Im3DValue Neighbors(const Vect3Df center,double radius)
	{
		typename _Base::iteratorZone pi(NeighborZone(center,radius));
		Im3DValue res;
		double radius2=radius*radius;
		for(pi=begin();pi!=end();++pi)
		{
			Im3DValue &candidates=*pi;
			for(uint i=0;i<candidates.size();i++)
			{
				if(candidates[i]->position.Distance2(center)<=radius2)
				{
					res.push_back(candidates[i]);
				}
			}
		}
		return res;
	}
	void Remove(const Vect3Df &pos,Typ v){Remove(VT(pos,v));}
	void Remove(const VT &p)
	{
		Im3DValue &voxel=Get(p.first);
		typename Im3DValue::iterator f=find(voxel.begin(),voxel.end(),p);
		if(f==voxel.end()){ThrowError("Neighbors3D::Remove not found");}
		voxel.erase(f);
	}
	// FIXME FIXME, rarelly fails Get? (on optimize compile)
	void Insert(const VT &p){Get(p.first).push_back(p);}
	void Insert(const Vect3Df &pos,Typ v){Get(pos).push_back(VT(pos,v));}
	Im3DValue &Get(Vect3Di P){Vect3Df p1=RPosToIPos(P);return (*this)((int)p1.x,(int)p1.y,(int)p1.z);}
	Im3DValue &Get(Vect3Df P){Vect3Df p1=RPosToIPos(P);return (*this)((int)p1.x,(int)p1.y,(int)p1.z);}
	Neighbors3D(RectZone3Df _support,int _width, int _height, int _depth):
		Container3D<Im3DValue>(_width,_height,_depth)
	{
		AddSupport(_support.GetP0(),_support.GetP1());
//  		properties.support=new Support3D<Container3D<Im3DValue> >(this,support.GetP0(),support.GetP1());
	}
};
#endif// _Neighbors3D_hpp
