NonmanifoldLevel::NonmanifoldLevel(subdivision::Mesh& subdivision)
{
  VertexPositionMapType averagedCenters;
  VertexPositionMapType averagedMidpoints;

  VertexEdgeCountMapType valencies;

  VertexAdjacencyMapType cornerAdjacentMaps;
  VertexAdjacencyMapType midpointAdjacentMaps;

  VertexVectorType corners;
  VertexVertexMapType vertexMap;

  for (int i = 0; i < subdivision.getNumberVertices(); i++) {
    subdivision::Vertex& vertex = subdivision.getVertex(i);
    subdivision::Vertex& created = createVertex(vertex);
    corners.push_back(&created);
    vertexMap[&vertex] = &created;
    valencies[&created] = 0;

    float* averagedCenter = new float[3];
    averagedCenters[&created] = averagedCenter;
    averagedCenter[0] = 0;
    averagedCenter[1] = 0;
    averagedCenter[2] = 0;

    float* averagedEnd = new float[3];
    averagedMidpoints[&created] = averagedEnd;
    averagedEnd[0] = 0;
    averagedEnd[1] = 0;
    averagedEnd[2] = 0;
  }

  VertexVectorType midpoints;
  for (int i = 0; i < subdivision.getNumberEdges(); i++) {
    subdivision::Edge& edge = subdivision.getEdge(i);

    subdivision::HalfEdge& firstHalfEdge = edge.getFirstHalf();
    subdivision::HalfEdge& secondHalfEdge = edge.getSecondHalf();
    
    subdivision::Vertex& fromVertex = firstHalfEdge.getFromVertex();
    subdivision::Vertex& toVertex = firstHalfEdge.getToVertex();

    subdivision::Vertex* midpoint = new subdivision::Vertex();
    midpoints.push_back(midpoint);
    _subdivision.add(*midpoint);

    float* averagedCenter = new float[3];
    averagedCenters[midpoint] = averagedCenter;
    averagedCenter[0] = 0;
    averagedCenter[1] = 0;
    averagedCenter[2] = 0;

    _midpointMappings[&firstHalfEdge] = midpoint;
    _midpointMappings[&secondHalfEdge] = midpoint;

    subdivision::Vertex* mappedFromVertex = vertexMap[&fromVertex];
    subdivision::Vertex* mappedToVertex = vertexMap[&toVertex];

    valencies[mappedFromVertex]++;
    valencies[mappedToVertex]++;

    _subdivision.createEdge(*mappedFromVertex, *midpoint);
    _subdivision.createEdge(*midpoint, *mappedToVertex);

    midpoint->_coordinates[0] = fromVertex._coordinates[0] + toVertex._coordinates[0];
    midpoint->_coordinates[1] = fromVertex._coordinates[1] + toVertex._coordinates[1];
    midpoint->_coordinates[2] = fromVertex._coordinates[2] + toVertex._coordinates[2];
    midpoint->_coordinates[3] = 1;

    averagedMidpoints[mappedFromVertex][0] += midpoint->_coordinates[0];
    averagedMidpoints[mappedFromVertex][1] += midpoint->_coordinates[1];
    averagedMidpoints[mappedFromVertex][2] += midpoint->_coordinates[2];
    
    averagedMidpoints[mappedToVertex][0] += midpoint->_coordinates[0];
    averagedMidpoints[mappedToVertex][1] += midpoint->_coordinates[1];
    averagedMidpoints[mappedToVertex][2] += midpoint->_coordinates[2];
  }

  VertexVectorType centers;
  for (int i = 0; i < subdivision.getNumberFaces(); i++) {
    subdivision::Face& face = subdivision.getFace(i);

    subdivision::Vertex* center = new subdivision::Vertex();
    centers.push_back(center);

    _centerMappings[&face] = center;
    _subdivision.add(*center);

    for (int j = 0; j < face.getNumberHalfEdges(); j++) {
      subdivision::HalfEdge& halfEdge = face.getHalfEdge(j);
      subdivision::Vertex* midpoint = _midpointMappings[&halfEdge];
      subdivision::Edge* edge = _subdivision.createEdge(*midpoint, *center);
    }

    center->_coordinates[0] = 0;
    center->_coordinates[1] = 0;
    center->_coordinates[2] = 0;
    center->_coordinates[3] = 1;

    for (int j = 0; j < face.getNumberVertices(); j++) {
      subdivision::Vertex& vertex = face.getVertex(j);
      center->_coordinates[0] += vertex._coordinates[0];
      center->_coordinates[1] += vertex._coordinates[1];
      center->_coordinates[2] += vertex._coordinates[2];      
    }

    center->_coordinates[0] /= face.getNumberVertices();
    center->_coordinates[1] /= face.getNumberVertices();
    center->_coordinates[2] /= face.getNumberVertices();
  }

  for (int i = 0; i < subdivision.getNumberFaces(); i++) {
    subdivision::Face& face = subdivision.getFace(i);
    subdivision::Vertex* center = _centerMappings[&face];

    int numberEdges = face.getNumberHalfEdges();
    subdivision::HalfEdge& lastHalfEdge = face.getHalfEdge(numberEdges - 1);

    subdivision::Vertex* last = _midpointMappings[&lastHalfEdge];
    for (int j = 0; j < face.getNumberHalfEdges(); j++) {
      subdivision::HalfEdge& halfEdge = face.getHalfEdge(j);
      subdivision::Vertex& oldCorner = halfEdge.getFromVertex();
      subdivision::Vertex* corner = vertexMap[&oldCorner];
      subdivision::Vertex* next = _midpointMappings[&halfEdge];

      _subdivision.createFace(*corner, *next, *center, *last);

      averagedCenters[next][0] += center->_coordinates[0];
      averagedCenters[next][1] += center->_coordinates[1];
      averagedCenters[next][2] += center->_coordinates[2];

      averagedCenters[corner][0] += center->_coordinates[0];
      averagedCenters[corner][1] += center->_coordinates[1];
      averagedCenters[corner][2] += center->_coordinates[2];

      last = next;
    }
  }

  for (int i = 0; i < corners.size(); i++) {
    subdivision::Vertex* corner = corners[i];

    int valence = valencies[corner];

    corner->_coordinates[0] *= (valence - 3) / 3;
    corner->_coordinates[1] *= (valence - 3) / 3;
    corner->_coordinates[2] *= (valence - 3) / 3;

    corner->_coordinates[0] += averagedMidpoints[corner][0] / (valence * 3);
    corner->_coordinates[1] += averagedMidpoints[corner][1] / (valence * 3);
    corner->_coordinates[2] += averagedMidpoints[corner][2] / (valence * 3);

    corner->_coordinates[0] += averagedCenters[corner][0] / (valence * 3);
    corner->_coordinates[1] += averagedCenters[corner][1] / (valence * 3);
    corner->_coordinates[2] += averagedCenters[corner][2] / (valence * 3);
  }

  for (int i = 0; i < midpoints.size(); i++) {
    subdivision::Vertex* midpoint = midpoints[i];

    midpoint->_coordinates[0] += averagedCenters[midpoint][0];
    midpoint->_coordinates[1] += averagedCenters[midpoint][1];
    midpoint->_coordinates[2] += averagedCenters[midpoint][2];

    midpoint->_coordinates[0] /= 4;
    midpoint->_coordinates[1] /= 4;
    midpoint->_coordinates[2] /= 4;
  }
}
void mitk::ConnectomicsNetworkWriter::Write()
{
    MITK_INFO << "Writing connectomics network";
    InputType::ConstPointer input = dynamic_cast<const InputType*>(this->GetInput());
    if (input.IsNull() )
    {
        MITK_ERROR <<"Sorry, input to ConnectomicsNetworkWriter is NULL!";
        return;
    }
    if ( this->GetOutputLocation().empty() )
    {
        MITK_ERROR << "Sorry, filename has not been set!" ;
        return ;
    }

    std::string ext = itksys::SystemTools::GetFilenameLastExtension(this->GetOutputLocation());
    ext = itksys::SystemTools::LowerCase(ext);

    // default extension is .cnf
    if(ext == "")
    {
        ext = ".cnf";
        this->SetOutputLocation(this->GetOutputLocation() + ext);
    }

    if (ext == ".cnf")
    {
        // Get geometry of the network
        mitk::BaseGeometry* geometry = input->GetGeometry();

        // Create XML document
        TiXmlDocument documentXML;
        {   // begin document
            TiXmlDeclaration* declXML = new TiXmlDeclaration( "1.0", "", "" ); // TODO what to write here? encoding? etc....
            documentXML.LinkEndChild( declXML );

            TiXmlElement* mainXML = new TiXmlElement(mitk::ConnectomicsNetworkDefinitions::XML_CONNECTOMICS_FILE);
            mainXML->SetAttribute(mitk::ConnectomicsNetworkDefinitions::XML_FILE_VERSION, mitk::ConnectomicsNetworkDefinitions::VERSION_STRING);
            documentXML.LinkEndChild(mainXML);

            TiXmlElement* geometryXML = new TiXmlElement(mitk::ConnectomicsNetworkDefinitions::XML_GEOMETRY);
            {   // begin geometry
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_XX, geometry->GetMatrixColumn(0)[0]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_XY, geometry->GetMatrixColumn(0)[1]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_XZ, geometry->GetMatrixColumn(0)[2]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_YX, geometry->GetMatrixColumn(1)[0]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_YY, geometry->GetMatrixColumn(1)[1]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_YZ, geometry->GetMatrixColumn(1)[2]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_ZX, geometry->GetMatrixColumn(2)[0]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_ZY, geometry->GetMatrixColumn(2)[1]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_MATRIX_ZZ, geometry->GetMatrixColumn(2)[2]);

                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_ORIGIN_X, geometry->GetOrigin()[0]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_ORIGIN_Y, geometry->GetOrigin()[1]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_ORIGIN_Z, geometry->GetOrigin()[2]);

                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_SPACING_X, geometry->GetSpacing()[0]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_SPACING_Y, geometry->GetSpacing()[1]);
                geometryXML->SetDoubleAttribute(mitk::ConnectomicsNetworkDefinitions::XML_SPACING_Z, geometry->GetSpacing()[2]);

            } // end geometry
            mainXML->LinkEndChild(geometryXML);

            TiXmlElement* verticesXML = new TiXmlElement(mitk::ConnectomicsNetworkDefinitions::XML_VERTICES);
            {   // begin vertices section
                VertexVectorType vertexVector = dynamic_cast<const InputType*>(this->GetInput())->GetVectorOfAllNodes();
                for( unsigned int index = 0; index < vertexVector.size(); index++ )
                {
                    // not localized as of yet TODO
                    TiXmlElement* vertexXML = new TiXmlElement(mitk::ConnectomicsNetworkDefinitions::XML_VERTEX );
                    vertexXML->SetAttribute( mitk::ConnectomicsNetworkDefinitions::XML_VERTEX_ID , vertexVector[ index ].id );
                    vertexXML->SetAttribute( mitk::ConnectomicsNetworkDefinitions::XML_VERTEX_LABEL , vertexVector[ index ].label );
                    vertexXML->SetDoubleAttribute( mitk::ConnectomicsNetworkDefinitions::XML_VERTEX_X , vertexVector[ index ].coordinates[0] );
                    vertexXML->SetDoubleAttribute( mitk::ConnectomicsNetworkDefinitions::XML_VERTEX_Y , vertexVector[ index ].coordinates[1] );
                    vertexXML->SetDoubleAttribute( mitk::ConnectomicsNetworkDefinitions::XML_VERTEX_Z , vertexVector[ index ].coordinates[2] );
                    verticesXML->LinkEndChild(vertexXML);
                }
            } // end vertices section
            mainXML->LinkEndChild(verticesXML);

            TiXmlElement* edgesXML = new TiXmlElement(mitk::ConnectomicsNetworkDefinitions::XML_EDGES);
            {   // begin edges section
                EdgeVectorType edgeVector = dynamic_cast<const InputType*>(this->GetInput())->GetVectorOfAllEdges();
                for(unsigned  int index = 0; index < edgeVector.size(); index++ )
                {
                    TiXmlElement* edgeXML = new TiXmlElement(mitk::ConnectomicsNetworkDefinitions::XML_EDGE );
                    edgeXML->SetAttribute( mitk::ConnectomicsNetworkDefinitions::XML_EDGE_ID , index );
                    edgeXML->SetAttribute( mitk::ConnectomicsNetworkDefinitions::XML_EDGE_SOURCE_ID , edgeVector[ index ].second.sourceId );
                    edgeXML->SetAttribute( mitk::ConnectomicsNetworkDefinitions::XML_EDGE_TARGET_ID , edgeVector[ index ].second.targetId );
                    edgeXML->SetAttribute( mitk::ConnectomicsNetworkDefinitions::XML_EDGE_WEIGHT_ID , edgeVector[ index ].second.weight );
                    edgeXML->SetDoubleAttribute( mitk::ConnectomicsNetworkDefinitions::XML_EDGE_DOUBLE_WEIGHT_ID , edgeVector[ index ].second.edge_weight );
                    edgesXML->LinkEndChild(edgeXML);
                }
            } // end edges section
            mainXML->LinkEndChild(edgesXML);

        } // end document
        documentXML.SaveFile( this->GetOutputLocation().c_str() );
        MITK_INFO << "Connectomics network written";

    }
}