#include "inmost.h"
#if defined(USE_MESH)


namespace INMOST
{
	
	__INLINE static void vec_diff(const Storage::real * vecin1, const Storage::real * vecin2, Storage::real * vecout, unsigned int size)
	{
		for(unsigned int i = 0; i < size; i++)
			vecout[i] = vecin1[i] - vecin2[i];
	}
	
	__INLINE static void vec_cross_product(const Storage::real * vecin1, const Storage::real * vecin2, Storage::real * vecout)
	{
		Storage::real temp[3];
		temp[0] = vecin1[1]*vecin2[2] - vecin1[2]*vecin2[1];
		temp[1] = vecin1[2]*vecin2[0] - vecin1[0]*vecin2[2];
		temp[2] = vecin1[0]*vecin2[1] - vecin1[1]*vecin2[0];
		vecout[0] = temp[0];
		vecout[1] = temp[1];
		vecout[2] = temp[2];
	}
	
	__INLINE static Storage::real vec_dot_product(const Storage::real * vecin1,const Storage::real * vecin2, unsigned int size)
	{
		Storage::real ret = 0;
		for(unsigned int i = 0; i < size; i++)
			ret += vecin1[i]*vecin2[i];
		return ret;
	}
	
	__INLINE static Storage::real vec_len2(const Storage::real * vecin, unsigned int size)
	{
		return vec_dot_product(vecin,vecin,size);
	}
	
	__INLINE static Storage::real vec_len(const Storage::real * vecin, unsigned int size)
	{
		return sqrt(vec_len2(vecin,size));
	}
	
	__INLINE static Storage::real det3d(Storage::real a, Storage::real b, Storage::real c,
	                         Storage::real d, Storage::real e, Storage::real f,
	                         Storage::real g, Storage::real h, Storage::real i ) 
	{
		return a*e*i - c*e*g + b*f*g - a*f*h + c*d*h - b*d*i;
	}
	
	__INLINE static Storage::real det3v(const Storage::real * x,const Storage::real * y,const Storage::real * z) 
	{
		return det3d(x[0], x[1], x[2],  y[0], y[1], y[2],  z[0], z[1], z[2]);
	}
	
	__INLINE static Storage::real det4v(const Storage::real * w, const Storage::real * x, const Storage::real * y, const Storage::real * z) 
	{
		return det3d(x[0]-w[0], x[1]-w[1], x[2]-w[2],  y[0]-w[0], y[1]-w[1], y[2]-w[2],  z[0]-w[0], z[1]-w[1], z[2]-w[2]);
	}	
	
	
	__INLINE static Storage::real vec_normalize(Storage::real * vecin, unsigned int size)
	{
		Storage::real len = 0;
		for(unsigned int i = 0; i < size; i++)
			len += vecin[i]*vecin[i];
		len = sqrt(len);
		for(unsigned int i = 0; i < size; i++)
			vecin[i] /= len;
		return len;
	}
	
	
	adjacent<Cell> Cell::NeighbouringCells()
	{
		adjacent<Cell> ret;
		adjacent<Face> faces = getFaces();
		for(adjacent<Face>::iterator f = faces.begin(); f != faces.end(); f++)
		{
			Cell * c = Neighbour(&*f);
			if( c != NULL ) ret.push_back(c);
		}
		return ret;
	}
	
	
	Cell * Cell::Neighbour(Face * f)
	{
		Cell * b = f->BackCell();
		if( b == this )
			return f->FrontCell();
		return b;
	}
	
	bool Cell::Inside(Storage::real * point) //check for 2d case
	{
		unsigned int dim = GetMeshLink()->GetDimensions();
		unsigned int vp = 0, vm = 0, vz = 0;
		Storage::real eps = GetMeshLink()->GetEpsilon();
		real c,d;
		if( dim != 3 ) throw UndefinedBehaviorInGeometry; 
		adjacent<Element> elements = getAdjElements(GetElementType()>>1);
		for(adjacent<Element>::iterator f = elements.begin(); f != elements.end(); f++)
		{
			d = 0.0;
			adjacent<Node> nodes = f->getNodes();
			for (unsigned int i=1; i<nodes.size()-1; i++) 
				d += c = det4v(point, nodes[0].Coords().data(), nodes[i].Coords().data(), nodes[i+1].Coords().data());
			if (f->getAsFace()->FaceOrientedOutside(this) == 0)  c = -1.0;  else c = 1.0;
			if (c*d > eps)  vp++;  else if (c*d < eps)  vm++;  else  vz++;
		}
		if (vp*vm > 0)  return false;
		return true;
	}
	
	
	void Face::UnitNormal(Storage::real * nrm) 
	{
		m_link->GetGeometricData(this,NORMAL,nrm); 
		unsigned dim = GetMeshLink()->GetDimensions(); 
		Storage::real l = sqrt(vec_dot_product(nrm,nrm,dim)); 
		if( fabs(l) > GetMeshLink()->GetEpsilon()) 
			for(unsigned i = 0; i < dim; i++) 
				nrm[i] /= l; 
	}
	
	void Face::OrientedNormal(Cell * c, Storage::real * nrm) 
	{
		Normal(nrm); 
		if( !FaceOrientedOutside(c) ) 
			for(unsigned i = 0; i < GetMeshLink()->GetDimensions(); i++) 
				nrm[i] = -nrm[i];
	}
	
	
	void Face::OrientedUnitNormal(Cell * c, Storage::real * nrm) 
	{
		UnitNormal(nrm); 
		if( !FaceOrientedOutside(c) ) 
			for(unsigned i = 0; i < GetMeshLink()->GetDimensions(); i++) 
				nrm[i] = -nrm[i];
	}
	
	typedef dynarray< std::pair<Element *,int> ,64> find_and_add_type;
	//~ typedef std::vector< std::pair<Element *,int> > find_and_add_type;
	
	
	__INLINE void find_and_add(find_and_add_type & array, Element * e)
	{
		bool flag = false;
		for(find_and_add_type::iterator it = array.begin(); it != array.end(); it++)
			if( it->first == e ) 
			{
				flag = true;
				it->second++;
				break;
			}
		if( !flag ) array.push_back( std::pair<Element *,int>(e,1) );
	}
	
	
	
	bool Mesh::TestClosure(Element ** elements, unsigned num)
	{
		unsigned int i;
		find_and_add_type e_visit;
		find_and_add_type::iterator it;
		if( !HideMarker() )
		{
			for(i = 0; i < num; i++)
			{
				for(Element::adj_iterator jt = elements[i]->LowConn().begin(); jt!= elements[i]->LowConn().end()	; jt++)
					find_and_add(e_visit,*jt);
			}
		}
		else
		{
			for(i = 0; i < num; i++) if( !elements[i]->GetMarker(HideMarker()) )
			{
				for(Element::adj_iterator jt = elements[i]->LowConn().begin(); jt!= elements[i]->LowConn().end()	; jt++) if( !(*jt)->GetMarker(HideMarker()) )
					find_and_add(e_visit,*jt);
			}
		}
		for(it = e_visit.begin(); it != e_visit.end(); it++)
			if( it->second != 2 ) return false;
		return true;
	}
	
	Element::GeometricType Mesh::ComputeGeometricType(ElementType etype, Element ** lc, INMOST_DATA_ENUM_TYPE s)
	{
		Element::GeometricType ret = Element::Unset;
		if( s == 0 && etype != NODE) return ret;
		switch(etype)
		{
			case NODE: ret = Element::Vertex; break;
			case EDGE:
				if( s == 1 )
					ret = Element::Vertex;
				else if( s == 2 )
					ret = Element::Line;
				break;
			case FACE:
				if( lc[0]->GetElementDimension() == 0 )
				{ 
					ret = Element::Line;
				}
				else
				{
					if( !GetTopologyCheck(NEED_TEST_CLOSURE) || TestClosure(lc,s) )
					{
						if( s == 3 )
							ret = Element::Tri;
						else if( s == 4 )
							ret = Element::Quad;
						else
							ret = Element::Polygon;
					}
					else ret = Element::MultiLine;
				}
				break;
			case CELL:
				if( lc[0]->GetElementDimension() == 1 )
				{
					if( !GetTopologyCheck(NEED_TEST_CLOSURE) || TestClosure(lc,s) )
					{
						if( s == 3 )
							ret = Element::Tri;
						else if( s == 4 )
							ret = Element::Quad;
						else
							ret = Element::Polygon;
					}
					else ret = Element::MultiLine;
				}
				else 
				{
					if( !GetTopologyCheck(NEED_TEST_CLOSURE) ||  TestClosure(lc,s) )
					{
						//test c_faces closure, if no closure, set as MultiPolygon
						INMOST_DATA_ENUM_TYPE quads = 0,tris = 0,i;
						for(i = 0; i < s; i++)
						{
							if( lc[i]->GetGeometricType() == Element::Tri )
								tris++;
							else if( lc[i]->GetGeometricType() == Element::Quad )
								quads++;
						}
						if( tris == 4 && s == 4 )
							ret = Element::Tet;
						else if( quads == 6 && s == 6 )
							ret = Element::Hex;
						else if( tris == 4 && quads == 1 && s == tris+quads)
							ret = Element::Pyramid;
						else if( quads == 3 && tris == 2 && s == tris+quads)
							ret = Element::Prism;
						else
							ret = Element::Polyhedron;
					}
					else ret = Element::MultiPolygon;
				}
				break;
			case ESET: ret = Element::Set; break;
		}
		return ret;
	}

	void Element::ComputeGeometricType()
	{
		SetGeometricType(Unset);
		adjacent<Element> lc = getAdjElements(GetElementType() >> 1);
		if( !lc.empty() )
			SetGeometricType(GetMeshLink()->ComputeGeometricType(GetElementType(),lc.data(),lc.size()));
		/*
		if( lc.size() == 0 && etypenum != 0) return;
		switch(etypenum)
		{
			case 0: m_type = Vertex; break;
			case 1:
				if( lc.size() == 1 )
					m_type = Vertex;
				else if( lc.size() == 2 )
					m_type = Line;
				break;
			case 2:
				if( lc[0].GetElementDimension() == 0 )
				{ 
					m_type = Line;
				}
				else
				{
					if( GetMeshLink()->TestClosure(lc.data(),lc.size()) )
					{
						if( lc.size() == 3 )
							m_type = Tri;
						else if( lc.size() == 4 )
							m_type = Quad;
						else
							m_type = Polygon;
					}
					else m_type = MultiLine;
				}
				break;
			case 3:
				if( lc[0].GetElementDimension() == 1 )
				{
					if( GetMeshLink()->TestClosure(lc.data(),lc.size()) )
					{
						if( lc.size() == 3 )
							m_type = Tri;
						else if( lc.size() == 4 )
							m_type = Quad;
						else
							m_type = Polygon;
					}
					else m_type = MultiLine;
				}
				else 
				{
					if( GetMeshLink()->TestClosure(lc.data(),lc.size()) )
					{
						//test c_faces closure, if no closure, set as MultiPolygon
						unsigned int quads = 0,tris = 0;
						for(unsigned i = 0; i < lc.size(); i++)
						{
							if( lc[i].GetGeometricType() == Tri )
								tris++;
							else if( lc[i].GetGeometricType() == Quad )
								quads++;
						}
						if( tris == 4 && lc.size() == 4 )
							m_type = Tet;
						else if( quads == 6 && lc.size() == 6 )
							m_type = Hex;
						else if( tris == 4 && quads == 1 && lc.size() == tris+quads)
							m_type = Pyramid;
						else if( quads == 3 && tris == 2 && lc.size() == tris+quads)
							m_type = Prism;
						else
							m_type = Polyhedron;
					}
					else m_type = MultiPolygon;
				}
				break;
			case 4: m_type = Set; break;
		}
		*/
	}
	
	
	void Mesh::RecomputeGeometricData(Element * e)
	{
		//static std::map<Element *, int> numfixes;
		GeometricData d ;
		for(d = CENTROID; d <= NORMAL; d++) // first compute centroids and normals 
		{
			if( HaveGeometricData(d,e->GetElementType()) ) //compute centroid first
			{
				Tag t = GetGeometricTag(d);
				Storage::real * a = &e->RealDF(t);
				HideGeometricData(d,e->GetElementType());
				GetGeometricData(e,d,a);
				ShowGeometricData(d,e->GetElementType());
			}
		}


		if( e->GetElementType() == CELL && HaveGeometricData(ORIENTATION,FACE)) //then correct the normal
		{
			for(Element::adj_iterator it = e->LowConn().begin(); it != e->LowConn().end(); ++it)
				if( !(*it)->GetMarker(HideMarker()) && (*it)->HighConn().size() == 1 )
				{
					(*it)->getAsFace()->FixNormalOrientation();
				}
		}
		for(d = MEASURE; d <= BARYCENTER; d++) // compute the rest
		{
			if( HaveGeometricData(d,e->GetElementType()) )
			{
				Tag t = GetGeometricTag(d);
				Storage::real * a = &e->RealDF(t);
				HideGeometricData(d,e->GetElementType());
				GetGeometricData(e,d,a);
				ShowGeometricData(d,e->GetElementType());
			}
		}
	}
	
	
	void Mesh::RemoveGeometricData(GeomParam table)
	{
		for(GeomParam::iterator it = table.begin(); it != table.end(); ++it)
		{
			if( it->first == MEASURE    ) 
			{
				if(measure_tag.isValid())    
					measure_tag    = DeleteTag(measure_tag   ,it->second);
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(MEASURE,etype);
			}
			if( it->first == CENTROID   ) 
			{
				if(centroid_tag.isValid())   
					centroid_tag   = DeleteTag(centroid_tag  ,it->second);
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(CENTROID,etype);
			}
			if( it->first == BARYCENTER ) 
			{
				if(barycenter_tag.isValid()) 
					barycenter_tag = DeleteTag(barycenter_tag,it->second);
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(BARYCENTER,etype);
			}
			if( it->first == NORMAL     ) 
			{
				if(normal_tag.isValid())
					normal_tag     = DeleteTag(normal_tag    ,it->second);
				for(ElementType etype = FACE; etype <= CELL; etype = etype << 1) if( etype & it->second) HideGeometricData(NORMAL,etype);
			}
			if( it->first == ORIENTATION) 
				if( FACE & it->second) HideGeometricData(ORIENTATION,FACE);
		}
	}
	
	void Mesh::RestoreGeometricTags()
	{
		for(GeometricData gtype = MEASURE; gtype <= NORMAL; gtype++)
		{
			bool restore = false;
			for(ElementType etype = EDGE; etype <= CELL && !restore; etype = etype << 1)
				if( HaveGeometricData(gtype,etype) )
					restore = true;
			if( restore )
			{
				switch(gtype)
				{
				case MEASURE:       measure_tag = GetTag("GEOM_UTIL_MEASURE");    break;
				case CENTROID:     centroid_tag = GetTag("GEOM_UTIL_CENTROID");   break;
				case BARYCENTER: barycenter_tag = GetTag("GEOM_UTIL_BARYCENTER"); break;
				case NORMAL:         normal_tag = GetTag("GEOM_UTIL_NORMAL");     break;
				}
			}
		}
	}
	
	void Mesh::PrepareGeometricData(GeomParam table)
	{
		for(GeomParam::iterator it = table.begin(); it != table.end(); ++it)
		{
			GeometricData types = it->first;
			ElementType mask = it->second;
			if( types == ORIENTATION )
			{
				if( mask & FACE )
					for(Mesh::iteratorFace e = BeginFace(); e != EndFace(); ++e) 
						e->FixNormalOrientation();
				ShowGeometricData(ORIENTATION,FACE);
			}
			if( types == MEASURE )
			{
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1)
				{
					if( (mask & etype) && !HaveGeometricData(MEASURE,etype))
					{
						measure_tag = CreateTag("GEOM_UTIL_MEASURE",DATA_REAL,etype,NONE,1);
						for(Mesh::iteratorElement e = BeginElement(etype); e != EndElement(); ++e)
							GetGeometricData(&*e,MEASURE,&e->RealDF(measure_tag));
						ShowGeometricData(MEASURE,etype);
					}
				}
			}
			if( types == CENTROID )
			{
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1)
				{
					if( (mask & etype) && !HaveGeometricData(CENTROID,etype))
					{
						centroid_tag = CreateTag("GEOM_UTIL_CENTROID",DATA_REAL,etype,NONE,GetDimensions());
						for(INMOST_DATA_INTEGER_TYPE k = 0; k < MaxLocalID(etype); ++k)
						{
							Element * e = ElementByLocalID(etype,k);
							if( e != NULL ) GetGeometricData(e,CENTROID,&e->RealDF(centroid_tag));
						}
						ShowGeometricData(CENTROID,etype);
					}
				}
			}
			if( types == BARYCENTER )
			{
				for(ElementType etype = EDGE; etype <= CELL; etype = etype << 1)
				{
					if( (mask & etype) && !HaveGeometricData(BARYCENTER,etype))
					{
						barycenter_tag = CreateTag("GEOM_UTIL_BARYCENTER",DATA_REAL,etype,NONE,GetDimensions());
						for(Mesh::iteratorElement e = BeginElement(etype); e != EndElement(); ++e)
							GetGeometricData(&*e,BARYCENTER,&e->RealDF(barycenter_tag));
						ShowGeometricData(BARYCENTER,etype);
					}
				}	
			}
			if( types == NORMAL )
			{
				for(ElementType etype = FACE; etype <= CELL; etype = etype << 1)
					if( (mask & etype) && !HaveGeometricData(NORMAL,etype))
					{
						normal_tag = CreateTag("GEOM_UTIL_NORMAL",DATA_REAL,etype,NONE,GetDimensions());
						for(Mesh::iteratorElement e = BeginElement(etype); e != EndElement(); ++e)
							GetGeometricData(&*e,NORMAL,&e->RealDF(normal_tag));
						ShowGeometricData(NORMAL,etype);
					}
			}
		}
	}
	
	void Mesh::GetGeometricData(Element * e, GeometricData type, Storage::real * ret)
	{
		ElementType etype = e->GetElementType();
		INMOST_DATA_ENUM_TYPE edim = e->GetElementDimension();
		INMOST_DATA_ENUM_TYPE mdim = GetDimensions();
		switch(type)
		{
			case MEASURE:
			if( HaveGeometricData(MEASURE,etype) )
			{
				*ret = e->RealDF(measure_tag);
				//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
			}
			else
			{
				switch(edim)
				{
					case 0: *ret = 0; break;
					case 1: //length of edge
					{
						adjacent<Node> nodes = e->getNodes();
						Storage::real c[3];
						vec_diff(nodes[0].Coords().data(),nodes[1].Coords().data(),c,mdim);
						*ret = vec_len(c,mdim);
						//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
						break;
					}
					case 2: //area of face
					{
						adjacent<Node> nodes = e->getNodes();
						real x[3] = {0,0,0};
						Storage::real_array x0 = nodes[0].Coords();
						for(unsigned i = 1; i < nodes.size()-1; i++)
						{
							Storage::real_array v1 = nodes[i].Coords();
							Storage::real_array v2 = nodes[i+1].Coords();
							if( mdim == 3 )
							{
								x[0] += (v1[1]-x0[1])*(v2[2]-x0[2]) - (v1[2]-x0[2])*(v2[1]-x0[1]);
								x[1] += (v1[2]-x0[2])*(v2[0]-x0[0]) - (v1[0]-x0[0])*(v2[2]-x0[2]);
							}
							x[2] += (v1[0]-x0[0])*(v2[1]-x0[1]) - (v1[1]-x0[1])*(v2[0]-x0[0]);
						}
						*ret = sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2])*0.5;
						//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
						break;
					}
					case 3: //volume of cell
					{
						adjacent<Face> faces = e->getFaces();
						*ret = 0;
						/*
						Storage::real d;
						for(unsigned i = 0; i < faces.size(); i++)
						{
							d = 0;
							adjacent<Node> nodes = faces[i].getNodes();
							Storage::real_array a = nodes[0].Coords();
							for(unsigned j = 1; j < nodes.size()-1; j++)
							{
								Storage::real_array b = nodes[j].Coords();
								Storage::real_array c = nodes[j+1].Coords();
								d += det3v(&a[0],&b[0],&c[0]);
							}
							*ret += (2*faces[i].FaceOrientedOutside(e->getAsCell())-1)*d;
						}
						*ret /= 6.0;
						*/
						Storage::real fcnt[3], fnrm[3];// , area;
						for(unsigned i = 0; i < faces.size(); i++)
						{
							faces[i].Centroid(fcnt);
							faces[i].OrientedNormal(e->getAsCell(),fnrm);
							/*
							area = sqrt(vec_dot_product(fnrm,fnrm,mdim));
							if( area > 0 )
							{
								fnrm[0] /= area;
								fnrm[1] /= area;
								fnrm[2] /= area;
								*ret += vec_dot_product(fcnt,fnrm,mdim) * area;
							}
							*/
							*ret += vec_dot_product(fcnt,fnrm,mdim);
						}
						*ret /= 3.0;
						/*
						if( *ret < 0  || (*ret) != (*ret) )
						{
							Storage::real test = 0;
							
							
							//~ Storage::real fcnt[3], fnrm[3], area;
							for(unsigned i = 0; i < faces.size(); i++)
							{
								if( faces[i].FixNormalOrientation() ) std::cout << faces[i].LocalID() << " normal refixed " << faces[i].nbAdjElements(CELL) << std::endl;
								//~ faces[i].Centroid(fcnt);
								//~ faces[i].OrientedNormal(e->getAsCell(),fnrm);
								//~ area = sqrt(vec_dot_product(fnrm,fnrm,mdim));
								//~ fnrm[0] /= area;
								//~ fnrm[1] /= area;
								//~ fnrm[2] /= area;
								//~ test += vec_dot_product(fcnt,fnrm,mdim) * area / 3.0;
							}
							
							//~ e->Centroid(fcnt);
							
							Storage::real d;
							for(unsigned i = 0; i < faces.size(); i++)
							{
								d = 0;
								adjacent<Node> nodes = faces[i].getNodes();
								Storage::real_array a = nodes[0].Coords();
								for(unsigned j = 1; j < nodes.size()-1; j++)
								{
									Storage::real_array b = nodes[j].Coords();
									Storage::real_array c = nodes[j+1].Coords();
									d += det3v(&a[0],&b[0],&c[0]);
								}
								test += (2*faces[i].FaceOrientedOutside(e->getAsCell())-1)*d;
							}
							test /= 6.0;
						
						
							std::cout << "alg1 " << *ret << " alg2 " << test << " on " << Element::GeometricTypeName(e->GetGeometricType()) << " " << e->GlobalID() << " " << e->LocalID() << std::endl;
							//e->Integer(tag_topologyerror) = 1;
						}
						*/
						//~ if( *ret < 0 )
						//~ {
							//~ std::cout << "negative volume! " << *ret << std::endl;
							//~ std::cout << "element " << Element::GeometricTypeName(e->GetGeometricType()) << " faces " << faces.size() << std::endl;
							//~ *ret = 0;
							//~ for(unsigned i = 0; i < faces.size(); i++)
							//~ {
								//~ std::cout << "face " << i << "/" << faces.size() << std::endl;
								//~ d = 0;
								//~ adjacent<Node> nodes = faces[i].getNodes();
								//~ Storage::real_array a = nodes[0].Coords();
								//~ std::cout << "node 0 " << a[0] << " " << a[1] << " " << a[2] << " id " << nodes[0].LocalID() << std::endl; 
								//~ for(unsigned j = 1; j < nodes.size()-1; j++)
								//~ {
									//~ Storage::real_array b = nodes[j].Coords();
									//~ std::cout << "node " << j << " " << b[0] << " " << b[1] << " " << b[2] << " id " << nodes[j].LocalID() << std::endl;
									//~ Storage::real_array c = nodes[j+1].Coords();
									//~ d += det3v(&a[0],&b[0],&c[0]);
								//~ }
								//~ a = nodes[nodes.size()-1].Coords();
								//~ std::cout << "node " << nodes.size()-1 << " " << a[0] << " " << a[1] << " " << a[2] << " id " << nodes[nodes.size()-1].LocalID() << std::endl; 
								//~ std::cout << "d = " << d << std::endl;
								//~ std::cout << "orientation = " << (2*faces[i].FaceOrientedOutside(e->getAsCell())-1) << std::endl;
								//~ Storage::real old = *ret, add = (2*faces[i].FaceOrientedOutside(e->getAsCell())-1)*d;
								//~ *ret = old + add;
								//~ std::cout << old << " + " << add << " = " << *ret << std::endl;
							//~ }
							//~ std::cout << "result " << *ret/6.0 << std::endl;
							//~ std::cout << "trying with fix " << std::endl;
							//~ *ret = 0;
							//~ for(unsigned i = 0; i < faces.size(); i++)
							//~ {
								//~ std::cout << "face " << i << "/" << faces.size() << " fixed " << faces[i].FixNormalOrientation() << " cells " << faces[i].nbAdjElements(CELL) << std::endl;
								//~ d = 0;
								//~ adjacent<Node> nodes = faces[i].getNodes();
								//~ Storage::real_array a = nodes[0].Coords();
								//~ std::cout << "node 0 " << a[0] << " " << a[1] << " " << a[2] << " id " << nodes[0].LocalID() << std::endl; 
								//~ for(unsigned j = 1; j < nodes.size()-1; j++)
								//~ {
									//~ Storage::real_array b = nodes[j].Coords();
									//~ std::cout << "node " << j << " " << b[0] << " " << b[1] << " " << b[2] << " id " << nodes[j].LocalID() << std::endl;
									//~ Storage::real_array c = nodes[j+1].Coords();
									//~ d += det3v(&a[0],&b[0],&c[0]);
								//~ }
								//~ a = nodes[nodes.size()-1].Coords();
								//~ std::cout << "node " << nodes.size()-1 << " " << a[0] << " " << a[1] << " " << a[2] << " id " << nodes[nodes.size()-1].LocalID() << std::endl; 
								//~ std::cout << "d = " << d << std::endl;
								//~ std::cout << "orientation = " << (2*faces[i].FaceOrientedOutside(e->getAsCell())-1) << std::endl;
								//~ Storage::real old = *ret, add = (2*faces[i].FaceOrientedOutside(e->getAsCell())-1)*d;
								//~ *ret = old + add;
								//~ std::cout << old << " + " << add << " = " << *ret << std::endl;
							//~ }
							//~ std::cout << "result " << *ret/6.0 << std::endl;
						//~ }
						
						//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
						break;
					}
				}
				//~ throw -1;
				//~ if( (*ret) != (*ret) || *ret < 0  ) 
				//~ {
					//~ std::cout << "bad measure: " << *ret << " for " << ElementTypeName(e->GetElementType()) << " " << Element::GeometricTypeName(e->GetGeometricType()) << " edim " << edim << std::endl;
					//~ 
				//~ }
				
			}
			//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
			break;
			case CENTROID:
			if(etype == NODE )
				memcpy(ret,e->getAsNode()->Coords().data(),sizeof(real)*mdim);
			else if(HaveGeometricData(CENTROID,etype))
			{
				memcpy(ret,&e->RealDF(centroid_tag),sizeof(real)*mdim);
			}
			else
			{
				adjacent<Node> nodes = e->getNodes();
				Storage::real div = 1.0/nodes.size();
				memset(ret,0,sizeof(real)*mdim);
				assert(nodes.size() != 0);
				for(unsigned i = 0; i < nodes.size(); i++)
				{
					Storage::real_array c =nodes[i].Coords();
					for(unsigned j = 0; j < mdim; j++) ret[j] += c[j];
				}
				for(unsigned j = 0; j < mdim; j++) ret[j] *= div;
			}
			break;
			case BARYCENTER:
			if( etype == NODE )
				memcpy(ret,e->getAsNode()->Coords().data(),sizeof(real)*mdim);
			else if(HaveGeometricData(BARYCENTER,etype))
				memcpy(ret,&e->RealDF(barycenter_tag),sizeof(real)*mdim);
			else
			{
				memset(ret,0,sizeof(real)*mdim);
				if( edim == 1 )
				{
					adjacent<Node> n = e->getNodes();
					Storage::real_array a = n[0].Coords();
					Storage::real_array b = n[1].Coords();
					for(unsigned j = 0; j < dim; j++) ret[j] = (a[j] + b[j])*0.5;
				}
				else if( edim == 2 )
				{
					adjacent<Node> nodes = e->getNodes();
					real s,d, x1[3] = {0,0,0},x2[3] = {0,0,0},x[3] = {0,0,0};
					//here we compute area of polygon
					//~ if( HaveGeometricData(MEASURE,etype) && HaveGeometricData(NORMAL,etype) )
					//~ {
						//~ s = e->RealDF(measure_tag);
						//~ memcpy(x,&e->RealDF(normal_tag),sizeof(Storage::real)*mdim);
					//~ }
					//~ else
					//~ {
						Storage::real_array x0 = nodes[0].Coords();
						for(unsigned i = 1; i < nodes.size()-1; i++)
						{
							Storage::real_array v1 = nodes[i].Coords();
							Storage::real_array v2 = nodes[i+1].Coords();
							if( mdim == 3 )
							{
								x[0] += (v1[1]-x0[1])*(v2[2]-x0[2]) - (v1[2]-x0[2])*(v2[1]-x0[1]);
								x[1] += (v1[2]-x0[2])*(v2[0]-x0[0]) - (v1[0]-x0[0])*(v2[2]-x0[2]);
							}
							x[2] += (v1[0]-x0[0])*(v2[1]-x0[1]) - (v1[1]-x0[1])*(v2[0]-x0[0]);
						}
						s = sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
						x[0] /= s; x[1] /= s; x[2] /= s; //here we obtain the unit normal
					//~ }
					//here we compute the center
					Storage::real_array v0 = nodes[0].Coords();
					for(unsigned j = 1; j < nodes.size()-1; j++)
					{
						Storage::real_array v1 = nodes[j].Coords();
						Storage::real_array v2 = nodes[j+1].Coords();
						for(unsigned k = 0; k < mdim; k++)
						{
							x1[k] = v0[k] - v1[k];
							x2[k] = v0[k] - v2[k];
						}
						d = det3v(x1,x2,x); //here we use unit normal
						for(unsigned k = 0; k < mdim; k++) ret[k] += d*(v0[k]+v1[k]+v2[k]);
					}
					for(unsigned k = 0; k < mdim; k++) ret[k] /= 3.0 * s;
				}
				else if( edim == 3 )
				{
					adjacent<Face> faces = e->getFaces();
					real d,c,vol = 0, y[3];
					for(unsigned i = 0; i < faces.size(); i++)
					{
						d = y[0] = y[1] = y[2] = 0;
						adjacent<Node> nodes = faces[i].getNodes();
						Storage::real_array v0 = nodes[0].Coords();
						for(unsigned j = 1; j < nodes.size()-1; j++)
						{
							Storage::real_array v1 = nodes[j].Coords();
							Storage::real_array v2 = nodes[j+1].Coords();
							c = det3v(v0.data(),v1.data(),v2.data());
							d += c;
							y[0] += c * (v0[0] + v1[0] + v2[0]);
							y[1] += c * (v0[1] + v1[1] + v2[1]);
							y[2] += c * (v0[2] + v1[2] + v2[2]);
						}
						c = faces[i].FaceOrientedOutside(e->getAsCell()) ? 1 : -1;
						ret[0] += c * y[0];
						ret[1] += c * y[1];
						ret[2] += c * y[2];
						vol += c*d;
					}
					ret[0] /= vol*4;
					ret[1] /= vol*4;
					ret[2] /= vol*4;
				}
			}
			break;
			case NORMAL:
			if( HaveGeometricData(NORMAL,etype) )
				memcpy(ret,&e->RealDF(normal_tag),sizeof(real)*GetDimensions());
			else
			{
				memset(ret,0,sizeof(real)*mdim);
				if( edim == 2 )//&& mdim == 3)
				{
					adjacent<Node> nodes = e->getNodes();
					
					Storage::real_array x0 = nodes[0].Coords();
					for(unsigned i = 1; i < nodes.size()-1; i++)
					{
						Storage::real_array a = nodes[i].Coords();
						Storage::real_array b = nodes[i+1].Coords();
						ret[0] += (a[1]-x0[1])*(b[2]-x0[2]) - (a[2]-x0[2])*(b[1]-x0[1]);
						ret[1] += (a[2]-x0[2])*(b[0]-x0[0]) - (a[0]-x0[0])*(b[2]-x0[2]);
						ret[2] += (a[0]-x0[0])*(b[1]-x0[1]) - (a[1]-x0[1])*(b[0]-x0[0]);
					}
					/*
					for(unsigned i = 0; i < nodes.size(); i++)
					{
						Storage::real_array a = nodes[i].Coords();
						Storage::real_array b = nodes[(i+1)%nodes.size()].Coords();
						ret[0] += a[1]*b[2] - a[2]*b[1];
						ret[1] += a[2]*b[0] - a[0]*b[2];
						ret[2] += a[0]*b[1] - a[1]*b[0];
					}
					*/
					ret[0] *= 0.5;
					ret[1] *= 0.5;
					ret[2] *= 0.5;
				}
				else if( edim == 1 )//&& mdim == 2 )
				{
					adjacent<Node> nodes = e->getNodes();
					Storage::real_array a = nodes[0].Coords();
					Storage::real_array b = nodes[1].Coords();
					ret[0] = b[1] - a[1];
					ret[1] = a[0] - b[0];
					Storage::real l = sqrt(ret[0]*ret[0]+ret[1]*ret[1]);
					if( l )
					{
						ret[0] /= l;
						ret[1] /= l;
					}
					l = sqrt((a[0]-b[0])*(a[0]-b[0])+(a[1]-b[1])*(a[1]-b[1]));
					ret[0] *= l;
					ret[1] *= l;
				}
			}
			break;
		}
		//~ if( type == MEASURE )
		//~ {
			//~ if( isnan(*ret) || fabs(*ret) < 1e-15  ) throw -1;
		//~ }
	}
	


	bool Element::Planarity()
	{
		Mesh * m = GetMeshLink();
		unsigned int dim = m->GetDimensions();
		if( dim < 3 ) return true;
		adjacent<Node> p = getNodes();
		if( p.size() <= 3 ) return true;
		unsigned int i, s = p.size();
		Storage::real v[2][3] = {{0,0,0},{0,0,0}};
		vec_diff(p[1].Coords().data(),p[0].Coords().data(),v[0],3);
		vec_diff(p[2].Coords().data(),p[0].Coords().data(),v[1],3);
		vec_cross_product(v[0],v[1],v[1]);
		for(i = 3; i < s; i++)
		{
			vec_diff(p[i].Coords().data(),p[0].Coords().data(),v[0],3);
			if( fabs(vec_dot_product(v[0],v[1],3)) > m->GetEpsilon() ) return false;
		}
		return true;
	}


	bool Cell::Closure()
	{
		return LowConn().size() > 0 ? GetMeshLink()->TestClosure(LowConn().data(),LowConn().size()) : false;
	}

	bool Face::Closure()
	{
		return LowConn().size() > 0 ? GetMeshLink()->TestClosure(LowConn().data(),LowConn().size()) : false;
	}



	

	

	bool Element::Boundary()
	{
		switch(GetElementType())
		{
			case CELL: return false;
			case FACE:
				if( nbAdjElements(CELL) == 1 )
				{
					if( getAsFace()->BackCell()->GetStatus() != Element::Ghost )
						return true;
				}
				return false;
			case EDGE:
			case NODE:
			{
				adjacent<Element> faces = getAdjElements(FACE);
				for(adjacent<Element>::iterator it = faces.begin(); it != faces.end(); it++)
					if( it->Boundary() ) return true;
				return false;
			}
			default: return false;
		}
		return false;
	}
	
	
	bool Face::CheckNormalOrientation()
	{
		Mesh * m = GetMeshLink();
		unsigned dim = m->GetDimensions();
		Cell * c1 = BackCell();
		Cell * c2 = FrontCell();
		if( c1 != NULL )
		{
			Storage::real nf[3], cf[3],fc[3], cc[3], dot, dot2;
			UnitNormal(nf);
			Centroid(fc);
			c1->Centroid(cc);
			//~ if( !c->Inside(cc) ) std::cout << "centroid is outside of " << Element::GeometricTypeName(c->GetGeometricType()) << std::endl;
			vec_diff(cc,fc,cf,dim);
			vec_normalize(cf,dim);
			dot = vec_dot_product(nf,cf,dim);
			//~ c = FrontCell();
			if( fabs(dot) < 0.25 && c2 != NULL )
			{
				c2->Centroid(cc);
				vec_diff(cc,fc,cf,dim);
				vec_normalize(cf,dim);
				dot2 = vec_dot_product(nf,cf,dim);
				if( fabs(dot2) > fabs(dot) )
					dot = -dot2;
			}
			if( dot > 0 )
				return false;
		}
		return true;
	}
	
	bool Face::FixNormalOrientation()
	{
		if( !CheckNormalOrientation() )
		{	
			ReorderEdges();
			if( GetMeshLink()->HaveGeometricData(NORMAL,FACE) )
			{
				Storage::real_array nrm = RealArray(GetMeshLink()->GetGeometricTag(NORMAL));
				for(Storage::real_array::iterator it = nrm.begin(); it != nrm.end(); ++it) *it = -(*it);
			}
			return true;
		}
		return false;
	}

	Storage::real meantri(Storage::real * v0, Storage::real * v1, Storage::real * v2, unsigned dim, Storage::real (*func)(Storage::real* x,Storage::real), Storage::real time)
	{
		Storage::real value = 0;
		static const Storage::real w[4] =   { -0.149570044467670, 0.175615257433204, 0.053347235608839 , 0.077113760890257};
		static const Storage::real a[4][3] =
		{
			{0.333333333333333,0.333333333333333,0.333333333333333},
			{0.479308067841923,0.260345966079038,0.260345966079038},
			{0.869739794195568,0.065130102902216,0.065130102902216},
			{0.638444188569809,0.312865496004875,0.048690315425316}
		};
		Storage::real XYG[13][3];
		for (unsigned i = 0 ; i < dim; i++)
			XYG[0][i] = 0.33333333333333333333*(v0[i]+v1[i]+v2[i]);
		 value += w[0] * func(XYG[0],time);
		for (int i = 0 ; i < 3 ; i++ )
		{
			for (unsigned j = 0 ; j < dim; j++)
				XYG[1+i][j] = v0[j] + (v1[j] - v0[j]) * a[1][i] + (v2[j] - v0[j])*a[1][(i+1)%3];
			value += w[1] * func(XYG[1+i],time);
		}
		for (int i = 0 ; i < 3 ; i++ )
		{
			for (unsigned j = 0 ; j < dim; j++)
				XYG[4+i][j] = v0[j] + (v1[j] - v0[j]) * a[2][i] + (v2[j] - v0[j])*a[2][(i+1)%3];
			value += w[2] * func(XYG[4+i],time);
		}
		for (int i = 0 ; i < 3 ; i++ )
		{
			for (unsigned j = 0 ; j < dim; j++)
			{
				XYG[7+2*i][j] = v0[j] + (v1[j] - v0[j]) * a[3][i] + (v2[j] - v0[j])*a[3][(i+1)%3];
				XYG[8+2*i][j] = v0[j] + (v1[j] - v0[j]) * a[3][(i+1)%3] + (v2[j] - v0[j])*a[3][i];
			}
			value += w[3] * func(XYG[7+2*i],time);
			value += w[3] * func(XYG[8+2*i],time);
		}
		return value;
	}

	Storage::real meantet(Storage::real * v0, Storage::real * v1, Storage::real * v2, Storage::real * v3, Storage::real (*func)(Storage::real* x,Storage::real),Storage::real time)
	{
		Storage::real value;
		static const Storage::real T5A = 0.25, W5A = 0.11851851851852;
		static const Storage::real T5B = 0.09197107805272, T5C = 0.72408676584183, W5B = 0.07193708377902;
		static const Storage::real T5D = 0.31979362782963, T5E = 0.04061911651111;
		static const Storage::real W5C = 0.06906820722627;
		static const Storage::real T5F = 0.05635083268963, T5G = 0.44364916731037;
		static const Storage::real W5D = 0.05291005291005;
		static const Storage::real w[15] = {W5A,W5B,W5B,W5B,W5B,W5C,W5C,W5C,W5C,W5D,W5D,W5D,W5D,W5D,W5D};
		Storage::real XYG[15][3];
		for (int i = 0; i < 3 ;i++)
		{
			XYG[0][i] = T5A * (v0[i] + v1[i] + v2[i] + v3[i]);
			XYG[1][i] = T5C * v0[i] + T5B * (v1[i]+v2[i]+v3[i]);
			XYG[2][i] = T5C * v1[i] + T5B * (v0[i]+v2[i]+v3[i]);
			XYG[3][i] = T5C * v2[i] + T5B * (v0[i]+v1[i]+v3[i]);
			XYG[4][i] = T5C * v3[i] + T5B * (v0[i]+v1[i]+v2[i]);

			XYG[5][i] = T5E * v0[i] + T5D * (v1[i]+v2[i]+v3[i]);
			XYG[6][i] = T5E * v1[i] + T5D * (v0[i]+v2[i]+v3[i]);
			XYG[7][i] = T5E * v2[i] + T5D * (v0[i]+v1[i]+v3[i]);
			XYG[8][i] = T5E * v3[i] + T5D * (v0[i]+v1[i]+v2[i]);

			XYG[9][i] = T5F * (v0[i] + v1[i]) + T5G * (v2[i] + v3[i]);
			XYG[10][i] = T5G * (v0[i] + v1[i]) + T5F * (v2[i] + v3[i]);
			XYG[11][i] = T5F * (v0[i] + v3[i]) + T5G * (v1[i] + v2[i]);
			XYG[12][i] = T5G * (v0[i] + v3[i]) + T5F * (v1[i] + v2[i]);
			XYG[13][i] = T5F * (v0[i] + v2[i]) + T5G * (v1[i] + v3[i]);
			XYG[14][i] = T5G * (v0[i] + v2[i]) + T5F * (v1[i] + v3[i]);
		}
		value = 0;
		for (int i = 0 ; i < 15 ; i++)
			value += w[i] * func(XYG[i],time);
		return value;
	}

	Storage::real Element::Mean(Storage::real (*func)(Storage::real* x,Storage::real),Storage::real time)
	{
		Mesh * m = GetMeshLink();
		if( GetElementDimension() == 2 )
		{
			unsigned int dim = m->GetDimensions();
			Storage::real val = 0, vol = 0, tvol,tval;
			Storage::real normal[3];
			Storage::real v1[3],v2[3],product[3];
			m->GetGeometricData(this,NORMAL,normal);
			adjacent<Node> nodes = getNodes();
			Storage::real_array av0 = nodes.front().RealArray(m->CoordsTag());
			for(adjacent<Node>::iterator it = ++nodes.begin(); it != nodes.end(); it++)
			{
				adjacent<Node>::iterator jt = it++;
				if( it == nodes.end() ) break;
				Storage::real_array av1 = jt->getAsNode()->Coords();
				Storage::real_array av2 = it->getAsNode()->Coords();
				tval = meantri(av0.data(),av1.data(),av2.data(),m->GetDimensions(),func,time);
				vec_diff(av1.data(),av0.data(),v1,dim);
				vec_diff(av2.data(),av0.data(),v2,dim);
				if( dim == 2 ) v1[2] = v2[2] = 0;
				vec_cross_product(v1,v2,product);
				tvol = vec_dot_product(product,normal,dim)*0.5;
				val += tval*tvol;
				vol += tvol;
				it = jt;
			}
			return val / vol;
		}
		else if( GetElementDimension() == 3 )
		{
			
			adjacent<Element> rfaces = getAdjElements(FACE);
			std::vector<int> n(rfaces.size());
			std::vector<Storage::real> v;
			int k = 0;
			for(adjacent<Element>::iterator f = rfaces.begin(); f != rfaces.end(); f++)
			{
				adjacent<Node> nodes = f->getNodes();
				int nn = n[k] = nodes.size();
				for(int i = 0; i < nn; i++)
				{
					Storage::real_array a = nodes[i].Coords();
					v.insert(v.end(),a.begin(),a.end());
				}
				k++;
			}
			int j = 0;
			Storage::real x[3], y[3], d, vol = 0, c;
			x[0] = x[1] = x[2] = 0;
			for(unsigned i = 0; i < n.size(); i++)
			{
				y[0] = y[1] = y[2] = d = 0;
				for(int k = 1; k < n[i] - 1; k++)
				{
					d += c = det3v(&v[j*3],&v[(j+k)*3],&v[(j+k+1)*3]);
					y[0] += c * (v[j*3+0] + v[(j+k)*3+0] + v[(j+k+1)*3+0]);
					y[1] += c * (v[j*3+1] + v[(j+k)*3+1] + v[(j+k+1)*3+1]);
					y[2] += c * (v[j*3+2] + v[(j+k)*3+2] + v[(j+k+1)*3+2]);
				}
				int orient = rfaces[i].getAsFace()->FaceOrientedOutside(getAsCell())?1:-1;
				x[0] += orient*y[0];
				x[1] += orient*y[1];
				x[2] += orient*y[2];
				vol += orient*d;
				j += n[i];
			}
			x[0] /= 4.0 * vol;
			x[1] /= 4.0 * vol;
			x[2] /= 4.0 * vol;
			vol /= 6;
			j = 0;
			vol = 0;
			Storage::real tvol, tval, val = 0;
			Storage::real vv0[3], vv1[3], vv2[3], prod[3];
			for(unsigned i = 0; i < n.size(); i++)
			{
				for(int k = 1; k < n[i] - 1; k++)
				{
					vec_diff(&v[j*3],x,vv0,3);
					vec_diff(&v[(j+k)*3],x,vv1,3);
					vec_diff(&v[(j+k+1)*3],x,vv2,3);
					vec_cross_product(vv1,vv2,prod);
					tvol = vec_dot_product(vv0,prod,3)/6.0 * (rfaces[i].getAsFace()->FaceOrientedOutside(getAsCell())?1:-1);
					tval = meantet(x,&v[j*3],&v[(j+k)*3],&v[(j+k+1)*3],func,time);
					val += tval * tvol;
					vol += tvol;
				}
				j+=n[i];
			}
			return val / vol;
		}
		else if ( GetElementDimension() == 1 ) //Mean value over line.
		{
			adjacent<Node> nodes = getNodes();
			unsigned int dim = m->GetDimensions();
			Storage::real_array x1 = nodes[0].Coords();
			Storage::real_array x2 = nodes[1].Coords();
			Storage::real middle[3];
			for (unsigned int i = 0 ; i < dim ; i++) middle[i] = (x1[i]+x2[i])*0.5;
			//Simpson formula
			return (func(x1.data(),time) + 4*func(middle,time) + func(x2.data(),time))/6e0;
		}
		return 0;
	}
	
	
	
	std::vector<Face *> Mesh::GatherBoundaryFaces()
	{
		std::vector<Face *> ret;
		for(Mesh::iteratorFace f = BeginFace(); f != EndFace(); f++)
			if( f->Boundary() ) ret.push_back(&*f);
		return ret;
	}

	std::vector<Face *> Mesh::GatherInteriorFaces()
	{
		std::vector<Face *> ret;
		if( GetMeshState() == Mesh::Serial )
		{
			for(Mesh::iteratorFace f = BeginFace(); f != EndFace(); f++)
				if( f->nbAdjElements(CELL) == 2 ) ret.push_back(&*f);
		}
		else
		{
			for(Mesh::iteratorFace f = BeginFace(); f != EndFace(); f++)
			{
				adjacent<Cell> cells = f->getCells();
				if( cells.size() == 2 && cells[0].GetStatus() != Element::Ghost && cells[1].GetStatus() != Element::Ghost)
					ret.push_back(&*f);
			}
		}
		return ret;
	}

	Storage::integer Mesh::CountBoundaryFaces()
	{
		Storage::integer ret = 0;
		for(Mesh::iteratorFace f = BeginFace(); f != EndFace(); f++)
			if( f->Boundary() ) ret++;
		return ret;
	}

	Storage::integer Mesh::CountInteriorFaces()
	{
		Storage::integer ret = 0;
		if( GetMeshState() == Mesh::Serial )
		{
			for(Mesh::iteratorFace f = BeginFace(); f != EndFace(); f++)
				if( f->nbAdjElements(CELL) == 2 ) ret++;
		}
		else
		{
			for(Mesh::iteratorFace f = BeginFace(); f != EndFace(); f++)
			{
				adjacent<Cell> cells = f->getCells();
				if( cells.size() == 2 && cells[0].GetStatus() != Element::Ghost && cells[1].GetStatus() != Element::Ghost)
					ret++;
			}
		}

		return ret;
	}
	
	

	
	void Element::CastRay(Storage::real * pos, Storage::real * dir, dynarray< std::pair<Element *, Storage::real> , 16 > & hits)
	{
		Mesh * mm = GetMeshLink();
		unsigned dim = mm->GetDimensions();
		Storage::real eps = mm->GetEpsilon();
		switch(GetElementType())
		{
			case NODE:
			{
				Storage::real_array coords = getAsNode()->Coords();
				Storage::real vec[3],ndir[3], lvec, ldir;
				for(unsigned k = 0; k < dim; k++)
				{
					vec[k] = (coords[k]-pos[k]);
					ndir[k] = dir[k];
				}
				lvec = vec_normalize(vec,dim);
				ldir = vec_normalize(ndir,dim);
				if( vec_dot_product(vec,ndir,dim) >= 1.0 - eps )
					hits.push_back(std::pair<Element *,Storage::real>(this,lvec/ldir));
			}	 
			break;
			case EDGE:
			{
				throw NotImplemented;
			}
			break;
			case FACE:
			{
				Element * shr_nodes[2];
				Storage::real tri[3][3];
				Centroid(tri[2]);
				adjacent<Node> nodes = getNodes();
				memcpy(tri[0],nodes[nodes.size()-1].Coords().data(),sizeof(Storage::real)*dim);
				for(unsigned q = 0; q < nodes.size(); q++)
				{
					memcpy(tri[1],nodes[q].Coords().data(),sizeof(Storage::real)*dim);
					Storage::real a[3],b[3],c[3],n[3], d, k, m;
					Storage::real dot00,dot01,dot02,dot11,dot12,invdenom, uq,vq;
					a[0] = tri[0][0] - tri[2][0];
					a[1] = tri[0][1] - tri[2][1];
					b[0] = tri[1][0] - tri[2][0];
					b[1] = tri[1][1] - tri[2][1];
					if( dim > 2 )
					{
						a[2] = tri[0][2] - tri[2][2];
						b[2] = tri[1][2] - tri[2][2];
					}
					else a[2] = b[2] = 0;
					vec_cross_product(a,b,n);
					d = -vec_dot_product(n,tri[0],dim);
					m =  vec_dot_product(n,dir,dim);
					if( !(fabs(m) < 1.0e-25) )
					{
						k = -(d + vec_dot_product(n,pos,dim))/m;
						if( k >= 0 )
						{
							c[0] = pos[0] + k*dir[0] - tri[2][0];
							c[1] = pos[1] + k*dir[1] - tri[2][1];
							if( dim > 2 ) c[2] = pos[2] + k*dir[2] - tri[2][2]; else c[2] = 0;
							dot00 = vec_dot_product(a,a,dim);
							dot01 = vec_dot_product(a,b,dim);
							dot02 = vec_dot_product(a,c,dim);
							dot11 = vec_dot_product(b,b,dim);
							dot12 = vec_dot_product(b,c,dim);
							invdenom = (dot00*dot11 - dot01*dot01);
							uq = (dot11*dot02-dot01*dot12);
							vq = (dot00*dot12-dot01*dot02);
							if( !(fabs(invdenom) < 1.0e-25  && fabs(uq) >= 0.0 && fabs(vq) >= 0.0) )
							{
								uq = uq/invdenom;
								vq = vq/invdenom;
								if( uq >= -eps && vq >= -eps )
								{
									if( 1.0-(uq+vq) >= -eps ) 
									{
										if( 1.0-(uq+vq) <= eps )
										{
											shr_nodes[0] = &nodes[(q-1+nodes.size())%nodes.size()];
											shr_nodes[1] = &nodes[q];
											if( uq <= eps && vq >= -eps )
												hits.push_back(std::pair<Element *,Storage::real>(shr_nodes[0],k));
											else if ( vq >= -eps && vq <= eps )
												hits.push_back(std::pair<Element *,Storage::real>(shr_nodes[1],k));
											else
												hits.push_back(std::pair<Element *,Storage::real>(mm->FindSharedAdjacency(shr_nodes,2),k));
										} 
										else
											hits.push_back(std::pair<Element *,Storage::real>(this,k));
										break; //we shouldn't have more then one intersection
									}
								}
							}
						}
					}
					memcpy(tri[0],tri[1],sizeof(Storage::real)*dim);
				}
			}
			break;
			case CELL:
			{
				adjacent<Face> faces = getFaces();
				for(adjacent<Face>::iterator it = faces.begin(); it != faces.end(); ++it)
					it->CastRay(pos,dir,hits);
			}
			break;
			/*
			case ESET: //cast ray through set of elements, probably accelerate by tree
			throw NotImplemented;
			break;
			case MESH: //cast ray through all elements, probably accelerate by tree
			throw NotImplemented;
			break;
			*/
		}
	}
}
#endif