VertexMap* MeshBuilder::addVertexMap( int dimensions, const String& name,
	const VertexMapFormat& format )
{
	VertexMap* item = allocateVertexMap();
	item->setDimensions( dimensions );
	item->setName( name );
	item->setFormat( format );
	m_this->vertexMaps.add( item );
	return item;
}

VertexMap* MeshBuilder::getVertexMapByName( const String& name )
{
	int i;
	for ( i = 0 ; i < (int)m_this->vertexMaps.size() ; ++i )
	{
		VertexMap* item = m_this->vertexMaps[i];
		if ( item->name() == name )
			return item;
	}

	return 0;
}

bool testMapToVertexMap()
{
  typedef  Z2i::Point Point;
  typedef  Z2i::Point Vertex;
  typedef int Value;
  typedef map<Vertex, Value> Map;
  typedef STLMapToVertexMapAdapter<Map> VertexMap;
  VertexMap myMap;
  BOOST_CONCEPT_ASSERT((CVertexMap<VertexMap>));
  myMap.setValue(Point(1,1), 10);
  myMap.setValue(Point(2,3), 2);
  
  return (myMap(Point(1,1)) == 10 && myMap(Point(2,3)) == 2);
}

std::vector<Vertex> ConstructPath( VertexMap& aFrom, const Vertex& aCurrentNode)
{
	VertexMap::iterator i = aFrom.find(aCurrentNode);
	if( i != aFrom.end() )
	{
		std::vector<Vertex> path = ConstructPath(aFrom,(*i).second);
		path.push_back(aCurrentNode);
		return path;
	}else
	{
		std::vector<Vertex> path;
		path.push_back(aCurrentNode);
		return path;
	}
}

// input is data from mesh.Append(append, TMap, VMap) - copy/rewrite faces from appendpolys
void MeshPolygons::AppendPolygons( VFTriangleMesh & mesh, 
								   VFTriangleMesh & append, MeshPolygons & appendpolys, 
								   VertexMap & VMap, TriangleMap & TMap, UVMap * pUVMap )
{
	std::vector<IMesh::VertexID> vBoundary;
	std::vector<unsigned int> vBoundaryUV;
	id_iterator curp(appendpolys.begin()), endp(appendpolys.end());
	while ( curp != endp ) {
		PolygonID sID = *curp++;

		const std::vector<IMesh::TriangleID> & vTris = appendpolys.GetTriangles(sID);
		size_t nCount = vTris.size();
		PolygonID sNewID = CreatePolygon((unsigned int)nCount);
		for ( unsigned int k = 0; k < nCount; ++k )
			AppendTriangle( sNewID, TMap.GetNew(vTris[k]) );

		const std::vector<IMesh::VertexID> & vOldBoundary = appendpolys.GetBoundary(sID);
		const std::vector<unsigned int> & vOldBoundaryUV = appendpolys.GetBoundaryUV(sID);
		nCount = vOldBoundary.size();
		vBoundary.resize(nCount);
		for ( unsigned int k = 0; k < nCount; ++k )
			vBoundary[k] = VMap.GetNew( vOldBoundary[k] );
		if ( pUVMap ) {
			nCount = vOldBoundaryUV.size();
			vBoundaryUV.resize(nCount);
			for ( unsigned int k = 0; k < nCount; ++k )
				vBoundaryUV[k] = pUVMap->GetNew( vOldBoundaryUV[k] );
		} else
			vBoundaryUV = vOldBoundaryUV;

		SetBoundary(sNewID, vBoundary, &vBoundaryUV);
	}
}

void IConnectionLoader::addConnection(
        VertexMap& vertices, SystemTypeGetter* info,
        int from, int to, TypeSet tags)
{
	TypeSet toType, fromType;

	toType.insert(info->getType(to));
	toType.insert(tags.begin(), tags.end());
	fromType.insert(info->getType(from));
	fromType.insert(tags.begin(), tags.end());

    //Add vertices if they dont exists already
    //this will fail if it already exists
    vertices.insert(std::make_pair(from, Vertex(from))); 
    vertices.insert(std::make_pair(to, Vertex(to)));

    //Add edges to the vertices
    vertices.find(from)->second.addEdge(to, toType);
	vertices.find(to)->second.addEdge(from, fromType);
}

void MeshPolygons::RewriteBoundaries( const VertexMap & VMap )
{
	std::set<Polygon>::iterator curt(m_vPolygons.begin()), endt(m_vPolygons.end());
	while ( curt != endt ) {
		Polygon & t = const_cast<Polygon&>(*curt++);
		size_t nVerts = t.vBoundary.size();
		for ( unsigned int k = 0; k < nVerts; ++k ) {
			IMesh::VertexID vNew = VMap.GetNew(t.vBoundary[k]);
			if ( vNew != IMesh::InvalidID )
				t.vBoundary[k] = vNew;
		}
	}
}

  bool vtkPolyData_to_polygon_mesh(vtkPolyData* poly_data,
                                   TM& tmesh,
                                   VertexMap& vertex_map,
                                   FaceMap& face_map)
  {
    typedef typename boost::property_map<TM, CGAL::vertex_point_t>::type VPMap;
    typedef typename boost::property_map_value<TM, CGAL::vertex_point_t>::type Point_3;
    typedef typename boost::graph_traits<TM>::vertex_descriptor vertex_descriptor;
    typedef typename boost::graph_traits<TM>::face_descriptor   face_descriptor;

    VPMap vpmap = get(CGAL::vertex_point, tmesh);

    // get nb of points and cells
    vtkIdType nb_points = poly_data->GetNumberOfPoints();
    vtkIdType nb_cells = poly_data->GetNumberOfCells();

    //extract points
    for (vtkIdType i = 0; i<nb_points; ++i)
    {
      double coords[3];
      poly_data->GetPoint(i, coords);

      vertex_descriptor v = add_vertex(tmesh);
      put(vpmap, v, Point_3(coords[0], coords[1], coords[2]));
      vertex_map.insert(std::make_pair(i, v));
    }

    //extract cells
    for (vtkIdType i = 0; i<nb_cells; ++i)
    {
      vtkCell* cell_ptr = poly_data->GetCell(i);

      vtkIdType nb_vertices = cell_ptr->GetNumberOfPoints();
      if (nb_vertices != 3){
        std::cerr << "Error a cell with " << nb_vertices << " found\n";
        return false;
      }
      std::vector<vertex_descriptor> vr;
      vr.push_back(vertex_map[cell_ptr->GetPointId(0)]);
      vr.push_back(vertex_map[cell_ptr->GetPointId(1)]);
      vr.push_back(vertex_map[cell_ptr->GetPointId(2)]);

      face_descriptor f = CGAL::Euler::add_face(vr, tmesh);
      face_map.insert(std::make_pair(i, f));
    }
    return true;
  }

void MeshPolygons::Rewrite( const VertexMap & VMap, const TriangleMap & TMap )
{
	m_PolygonMap.clear();

	std::set<Polygon>::iterator curt(m_vPolygons.begin()), endt(m_vPolygons.end());
	while ( curt != endt ) {
		Polygon & t = const_cast<Polygon&>(*curt++);
		size_t nTris = t.vTris.size();
		for ( unsigned int k = 0 ; k < nTris; ++k ) {
			IMesh::TriangleID tNew = TMap.GetNew( t.vTris[k] );
			if ( tNew != IMesh::InvalidID )
				t.vTris[k] = tNew;
			m_PolygonMap.insert( std::pair<IMesh::TriangleID, PolygonID>( t.vTris[k], t.id ) );
		}
		size_t nVerts = t.vBoundary.size();
		for ( unsigned int k = 0; k < nVerts; ++k ) {
			IMesh::VertexID vNew = VMap.GetNew( t.vBoundary[k] );
			if ( vNew != IMesh::InvalidID )
				t.vBoundary[k] = vNew;
		}
	}
}

static int test()
{
	MeshBuilder mesh;
	Vertex* v[4];
	int i;
	for ( i = 0 ; i < 4 ; ++i )		// 0 1
		v[i] = mesh.addVertex();	// 3 2

	Polygon* a = mesh.addPolygon();
	a->addVertex( v[0] );
	a->addVertex( v[1] );
	a->addVertex( v[2] );

	Polygon* b = mesh.addPolygon();
	b->addVertex( v[0] );
	b->addVertex( v[2] );
	b->addVertex( v[3] );

	VertexMap* vmap = mesh.addVertexMap( 2, "TXUV", VertexMapFormat::VERTEXMAP_TEXCOORD );
	float uv[4][2] = { {0, 0}, {1,0}, {1,1}, {0,1} };
	vmap->addValue( v[2]->index(), uv[2], 2 );
	vmap->addValue( v[1]->index(), uv[1], 2 );
	vmap->addValue( v[3]->index(), uv[3], 2 );
	vmap->addValue( v[0]->index(), uv[0], 2 );

	v[1] = a->getVertex(1)->clone();
	a->setVertex( 1, v[1] );
	assert( mesh.vertices() == 5 );

	float uv1[4][2];
	memset( uv1, 0, sizeof(uv1) );
	for ( i = 0 ; i < 4 ; ++i )
	{
		vmap->getValue( v[i]->index(), uv1[i], 2 );
		assert( !memcmp(uv[i],uv1[i],sizeof(uv[i])) );
	}

	assert( vmap->dimensions() == 2 );
	assert( vmap->name() == "TXUV" );
	return 0;
}

/**
 * This is a straight forward C++ implementation of Wikipedia's A* pseudo code.
 * As proof of this the pseudo code is embedded in the source, just above the
 * C++ statements.
 *
 * See http://en.wikipedia.org/wiki/A*#Pseudocode for the complete pseudo code
 *
 * But also see the discussion page: it explains why the algorithm is only
 * correct if the cost function is monotone.This algorithm does not give the
 * correct solution in our case. The routes and hence costs differ whether we
 * go from (0,0) to (10,10) or vice versa:
 *
 * 0-0 -> 3-2 -> 4-5 -> 4-8 -> 6-9 -> 10-10, cost = 15
 *
 * 10-10 -> 10-7 -> 7-5 -> 6-3 -> 3-2 -> 0-0, cost = 14
 *
 * Use this piece of code as inspiration only.
 *
 */
std::vector<Vertex> WikiAStar( Vertex start, Vertex goal)
{
	ClosedSet closedSet;		// The set of nodes already evaluated.
	OpenSet openSet;			// The set of tentative nodes to be evaluated, initially containing the start node
	VertexMap predecessorMap;	// The map of navigated nodes.

	start.actualCost = 0.0; 												// Cost from start along best known path.
	start.heuristicCost = start.actualCost + HeuristicCost(start, goal);	// Estimated total cost from start to goal through y.
	openSet.insert(start);

	//while openset is not empty
	while(!openSet.empty())
	{
		// current := the node in openset having the lowest f_score[] value
		Vertex current = *openSet.begin();
		// if current = goal
		if(current == goal)
		{
			// return reconstruct_path(came_from, goal)
			return ConstructPath(predecessorMap,current);
		}
		// remove current from openset
		openSet.erase(current);
		// add current to closedset
		closedSet.insert(current);

		//for each neighbour in neighbour_nodes(current)
		std::vector< Vertex > neighbours = GetNeighbours(current);
		for(Vertex neighbour : neighbours)
		{
			// if neighbor in closedset continue (with next neighbour)
			if(closedSet.find(neighbour) != closedSet.end())
			{
				continue;
			}
			// tentative_g_score := g_score[current] + dist_between(current,neighbour)
			double calculatedActualNeighbourCost = current.actualCost + ActualCost(current,neighbour);

			// if neighbour not in openset or tentative_g_score < g_score[neighbour]
			if(openSet.find(neighbour) == openSet.end() || calculatedActualNeighbourCost < neighbour.actualCost)
			{
				// Here we deviate from the Wikipedia article, because of the map semantics:
				// we cannot change the object's key values once it in the map so we first
				// set the vales and then put it into the map.

				// g_score[neighbor] := tentative_g_score
				neighbour.actualCost = calculatedActualNeighbourCost;
				// f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
				neighbour.heuristicCost = neighbour.actualCost + HeuristicCost(neighbour, goal);

				// came_from[neighbor] := current
				std::pair<VertexMap::iterator,bool> result = predecessorMap.insert(std::make_pair(neighbour,current));

				// The following if-statement is not part of the pseudo code but a partial fix for a semantic difference
				// in the map of the pseudo code and the c++ std::map
				if(result.second==false)
					(*result.first).second = current;

				// if neighbor not in openset
				if(openSet.find(neighbour) == openSet.end())
				{
					// add neighbor to openset
					openSet.insert(neighbour);
				}
			}
		}
	}
	//return failure
	return std::vector<Vertex>();
}

	void RigidBody::DrawMesh()
	{
		/*
		//Cube!
		glBegin(GL_QUADS);
			glColor3f(0.0f,1.0f,0.0f);
			glVertex3f( 1.0f, 1.0f,-1.0f);
			glVertex3f(-1.0f, 1.0f,-1.0f);
			glVertex3f(-1.0f, 1.0f, 1.0f);
			glVertex3f( 1.0f, 1.0f, 1.0f);
			glColor3f(1.0f,0.5f,0.0f);	
			glVertex3f( 1.0f,-1.0f, 1.0f);
			glVertex3f(-1.0f,-1.0f, 1.0f);
			glVertex3f(-1.0f,-1.0f,-1.0f);
			glVertex3f( 1.0f,-1.0f,-1.0f);
			glColor3f(1.0f,0.0f,0.0f);
			glVertex3f( 1.0f, 1.0f, 1.0f);
			glVertex3f(-1.0f, 1.0f, 1.0f);
			glVertex3f(-1.0f,-1.0f, 1.0f);
			glVertex3f( 1.0f,-1.0f, 1.0f);
			glColor3f(1.0f,1.0f,0.0f);
			glVertex3f( 1.0f,-1.0f,-1.0f);
			glVertex3f(-1.0f,-1.0f,-1.0f);
			glVertex3f(-1.0f, 1.0f,-1.0f);
			glVertex3f( 1.0f, 1.0f,-1.0f);
			glColor3f(0.0f,0.0f,1.0f);
			glVertex3f(-1.0f, 1.0f, 1.0f);
			glVertex3f(-1.0f, 1.0f,-1.0f);
			glVertex3f(-1.0f,-1.0f,-1.0f);
			glVertex3f(-1.0f,-1.0f, 1.0f);
			glColor3f(1.0f,0.0f,1.0f);
			glVertex3f( 1.0f, 1.0f,-1.0f);
			glVertex3f( 1.0f, 1.0f, 1.0f);
			glVertex3f( 1.0f,-1.0f, 1.0f);
			glVertex3f( 1.0f,-1.0f,-1.0f);
		glEnd();
		*/

		typedef map<VertexId, Vertex> VertexMap;
		VertexMap vertices = m_Geom.GetVertices();
		VertexMap::const_iterator end = vertices.end(); 

		/*
		glColor3f(1.0,0.0,0.0);
		glBegin(GL_POINTS);
		for (VertexMap::const_iterator it = vertices.begin(); it != end; ++it)
		{
			glPushMatrix();
			glVertex3f((*it).second.GetPos().GetX(), 
					(*it).second.GetPos().GetY(),
					(*it).second.GetPos().GetZ());
			glPopMatrix();
		}
		glEnd();
		*/

		glCallList(m_RigidBodyList);


		/*
		glColor3f(1.0,1.0,0.0);

		typedef map<FaceId, Face> FaceMap;
		FaceMap faces = m_Geom.GetFaces();
		FaceMap::const_iterator itFaceEnd = faces.end(); 

		for (FaceMap::const_iterator itFace = faces.begin(); itFace != itFaceEnd; ++itFace)
		{
			glBegin(GL_POLYGON);
			vector<VertexId> vertexIds = (*itFace).second.GetVertexIds();
			vector<VertexId>::iterator vItEnd = vertexIds.end();
			for(vector<VertexId>::iterator vIt = vertexIds.begin() ; 
					vIt != vItEnd ;
					vIt++)
			{
				Vertex vertex = m_Geom.GetVertex((*vIt));
				glPushMatrix();
				glVertex3f(vertex.GetPos().GetX(), 
						vertex.GetPos().GetY(),
						vertex.GetPos().GetZ());
				glPopMatrix();
			}
			glEnd();
		}
		*/

	}

bool Graph::visit(int v, std::list<int> & sortedList)
{
    VertexIterator iter = vertices.find(v);
    if (iter != vertices.end())
    {
        if (iter->second.visited == false)
        {
            iter->second.visited = true;

            std::list<Edge>::const_iterator liter = iter->second.adjList.begin();
            for (; liter != iter->second.adjList.end(); ++liter)
            {
                if (!visit(liter->dest, sortedList))
                {
                    return false;
                }
            }
            sortedList.push_front(iter->first);
        }
        else
        {
            return false;
        }
    }
    return true;
}

void Graph::TopologicalSort()
{
    VertexConstIterator iter = vertices.begin();
    std::list<int> sortedList;
    bool cycle = false;
    for (; iter != vertices.end(); ++iter)
    {
        if (iter->second.indegree == 0) 
        {
            if (!visit(iter->first, sortedList))
            {
                cycle = true;
                break;
            }
        }
    }

    if (cycle || sortedList.size() != vertices.size())
    {
        cout << "CYCLE";
        return;
    }
    
    std::list<int>::const_iterator liter = sortedList.begin();
    for (; liter != sortedList.end(); ++liter)
    {
        cout << *liter << " ";
    }
}

void Graph::resetStates()
{
    VertexIterator iter = vertices.begin();
    for (; iter != vertices.end(); ++iter)
    {
        iter->second.visited = false;
    }
}

void Graph::print()
{
    VertexConstIterator iter = vertices.begin();
    for (; iter != vertices.end(); ++iter)
    {
        cout << iter->second << "\n";
    }
}

void Graph::addEdge(int v1, int v2)
{
    VertexIterator iter = vertices.find(v1);

    if (iter != vertices.end())
    {
        Edge edge(v2);
        iter->second.adjList.push_back(edge);
    }

    iter = vertices.find(v2);
    iter->second.indegree++;
}

void Graph::BreadthFirstTour(int v)
{
  cout << "BFS BEGIN\n";

  std::queue<int> q;
  VertexConstIterator iter = vertices.find(v);
  if (iter != vertices.end())
  {
      q.push(iter->first);
  }
  else
  {
      cout << "BFS END\n";
      return;
  }

  VertexIterator iter1;
  while (!q.empty())
  {
      int vtx = q.front();
      q.pop();

      Vertex & v = vertices[vtx];
      if (v.visited == false )
      {
          v.visited = true;
          cout << v.orig << " ";

          std::list<Edge>::const_iterator iter = v.adjList.begin();
          while (iter != v.adjList.end())
            {
                //cout << "dest " << iter->dest << endl;
                VertexConstIterator citer = vertices.find(iter->dest);
                if (citer != vertices.end())
                {
                    q.push(citer->first);
                    //cout << "pushing " << citer->orig << endl;
                }
                ++iter;
            }
      }
  }
  
  cout << "\nBFS END" << endl;
  
  resetStates();
}

void Graph::DepthFirstTour(int v)
{
    VertexIterator iter = vertices.find(v);
    if (iter != vertices.end())
    {
        if (iter->second.visited == false)
        {
            iter->second.visited = true;
            
            std::list<Edge>::iterator liter = iter->second.adjList.begin();
            for (; liter != iter->second.adjList.end(); ++liter)
            {
                DepthFirstTour(liter->dest);
            }
            
            cout << iter->second.orig << " ";
        }
    }
}

void Graph::addVertex(int v) 
{
  vertices.insert( std::make_pair(v, Vertex(v)) );
}