//
//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2016 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
#include <GraphUtils.hpp>
#include <Progress.hpp>
#include <limits>

namespace mgx 
{
  // Get the label for a triangle
  int getLabel(const vertex &v1, const vertex &v2, const vertex &v3)
  {
    if(!v1->label or !v2->label or !v3->label)
      return 0;
    if(v1->label < 0 and v2->label < 0 and v3->label < 0)
      return -1;
  
    int labcnt = 0, label = 0;
    if(v1->label > 0) {
      labcnt++;
      label = v1->label;
    }
    if(v2->label > 0) {
      if(v2->label != label) {
        labcnt++;
        label = v2->label;
      }
    }
    if(v3->label > 0) {
      if(v3->label != label) {
        labcnt++;
        label = v3->label;
      }
    }
    if(labcnt != 1)
      return 0;
  
    return label;
  }

  // Count the labels for each vertex
  bool getLabelCount(const vvGraph &S, VtxIntMap &labCount)
  {
    forall(vertex v, S) {
      if(v->label == -1) {
        std::set<int> L;
        forall(vertex n, S.neighbors(v))
          if(n->label > 0)
            L.insert(n->label);
        labCount[v] = L.size();
        // record margin cells in case we need to erase them later
      } else
        labCount[v] = 0;
    }
    return true;
  }

  // Merge vertices together
  int mergeVertices(vvGraph &S, const VtxVec &vs)
  {
    VtxIntMap weight;
    int count = 0;
    bool vertexDeleted;
    do {
      vertexDeleted = false;
      forall(vertex a, vs) {
        forall(vertex b, S.neighbors(a))
          if(std::find(vs.begin(), vs.end(), b) != vs.end() and spliceNhbs(S, a, b)) {
            if(weight.count(a) == 0)
              weight[a] = 1;
            if(weight.count(b) == 0)
              weight[b] = 1;
            a->pos = (a->pos * weight[a] + b->pos * weight[b]) / float(weight[a] + weight[b]);
            a->signal = (a->signal * weight[a] + b->signal * weight[b]) / float(weight[a] + weight[b]);
            weight[a] += weight[b];
            a->label = max(a->label, b->label);
            S.erase(b);
            count++;
            vertexDeleted = true;
            break;
          }
        if(vertexDeleted)
          break;
      }
      if(!progressAdvance(count))
        break;
    } while(vertexDeleted);

    return count;
  }

  bool splitEdge(vvGraph &S, vertex v1, vertex v2, Subdivide *sDiv)
  {
    // Check the edge is there
    if(!S.edge(v1, v2) or !S.edge(v2, v1))
      return false;

    // Create vertex
    vertex v;
    S.insert(v);
    v->pos = (v1->pos + v2->pos)/2.0;
    v->label = max(v1->label, v2->label);
    if(v1->selected or v2->selected)
      v->selected = true;

    // Split the edge
    S.insertEdge(v, v1);
    S.insertEdge(v, v2);
    S.spliceBefore(v1, v2, v);
    S.spliceAfter(v2, v1, v);

    // If there is a left neighbor
    vertex l = S.nextTo(v1, v2);
    if(l == S.prevTo(v2, v1)) {
      S.spliceAfter(l, v1, v);
      S.spliceBefore(v, v1, l);
    }

    // If there is a right neighbor
    vertex r = S.prevTo(v1, v);
    if(r == S.nextTo(v2, v)) {
      S.spliceBefore(r, v1, v);
      S.spliceAfter(v, v1, r);
    }

    // Propagate data
    if(sDiv)
      sDiv->updateEdgeData(v1, v, v2, .5);

    // Delete old edges
    S.eraseEdge(v1, v2);
    S.eraseEdge(v2, v1);

    return true;
  }

  bool subdivideBisect(vvGraph &S, const vertex &v1, const vertex &v2, 
                             const vertex &v3, std::vector<vertex>* vs, Subdivide *subdiv)
  {
    // v1, v2, v3 must be in order
    vertex a(0), b(0), c(0);
  
    // Find longest edge and divide
    a = v1;
    b = v2;
    c = v3;
    if((v2->pos - v3->pos).norm() > (a->pos - b->pos).norm()) {
      a = v2;
      b = v3;
      c = v1;
    } else if((v3->pos - v1->pos).norm() > (a->pos - b->pos).norm()) {
      a = v3;
      b = v1;
      c = v2;
    }
  
    // Insert vertex in long edge if not already there
    if(S.edge(a, b)) {
      vertex d;
      d->type = 'j';
      if(a->selected or b->selected)
        d->selected = true;
      if(vs)
        vs->push_back(d);
      S.insert(d);
      S.insertEdge(d, a);
      S.insertEdge(d, b);
      d->pos = (a->pos + b->pos) / 2.0;
      d->signal = (a->signal + b->signal) / 2.0;
      // RSS We could make a system Subdivide object to inherit from
      // or maybe it is not so important
      // VtxPoint2fAttr& tex2d = mesh->texCoord2d();
      //if(tex2d.size() > 0)
      //  d->*tex2d = (a->*tex2d + b->*tex2d) / 2.0;
      if(a->label == b->label)
        d->label = a->label;
      S.spliceBefore(a, b, d); 
      S.spliceBefore(b, a, d); 
      if(subdiv)
        subdiv->updateEdgeData(a, d, b, .5);
      S.eraseEdge(a, b);
      S.eraseEdge(b, a);
    } else {
      // Already there, must be the last triangle
      const vertex& d = S.prevTo(a, c);
      S.spliceAfter(c, a, d);
      S.spliceBefore(d, a, c);
      return true;
    }
  
    // Create dividing edge
    vertex d(0);
    if(S.edge(a, c)) {
      d = S.prevTo(a, c);
      S.spliceAfter(c, a, d);
      S.spliceBefore(d, a, c);
      const vertex& v = S.nextTo(c, d);
      // If edge c,b split by v, connect d to v
      if(v != b) {
        S.spliceBefore(d, c, v);
        S.spliceAfter(v, c, d);
      }
    } else if(S.edge(b, c)) {
      d = S.nextTo(b, c);
      S.spliceBefore(c, b, d);
      S.spliceAfter(d, b, c);
      const vertex& v = S.prevTo(c, d);
      // If edge c,a split by v, connect d to v
      if(v != a) {
        S.spliceAfter(d, c, v);
        S.spliceBefore(v, c, d);
      }
    } else {
      Information::out << "subdivideBisect::Error:Both incoming short edges divided" << endl;
      return false;
    }
  
    // Find neighbor
    vertex n(0);
    if(S.prevTo(a, d) == S.nextTo(b, d)) {
      n = S.prevTo(a, d);
      // Neighbor already divided
      if(S.edge(d, n))
        return true;
      return subdivideBisect(S, b, a, n, vs, subdiv);
    }
    return true;
  }

  // Check for border triangles, clear and mark them for selection if required
  bool checkBorderTris(const vvGraph &S, bool select)
  {
    // Clear selection if required
    if(select) {
      forall(const vertex &v, S)
        v->selected = false;
    }

    int found = false;
    forall(const vertex &v, S) {
      if(v->label == 0) {
        // Check unlabeled
        if(select)
          v->selected = true;
        found = true;
      } else if(v->label < 0) {
        // Check for border triangles
        forall(vertex n, S.neighbors(v)) {
          // get unique triangles
          vertex m = S.nextTo(v, n);
          if(!S.uniqueTri(v, n, m))
            continue;
          if(n->label < 0 and m->label < 0) {
            if(select)
              v->selected = n->selected = m->selected = true;
            found = true;
          }
        }

        // Check for extra borders ("islands") within skinny cells
        if(S.valence(v) < 3)
          continue;
        vertex start = S.anyIn(v);
        vertex n = start;
        do {
          if(n->label == -1) {
            vertex nn = S.nextTo(v, n);
            vertex pn = S.prevTo(v, n);
            if(nn->label > 0 and nn->label == pn->label) {
              if(select)
                v->selected = nn->selected = pn->selected = true;
              found = true;
            }
          }
          n = S.nextTo(v, n);
        } while(n != start);
      }
    }
    return found;
  }

  // Delete a group of cells from a cell mesh (MGXC)
  bool deleteCells(vvGraph &S, const VtxVec &V)
  {
    // grab the junctions
    vvGraph J;
    forall(const vertex &v, V)
      if(v->label > 0) {
        forall(const vertex &j, S.neighbors(v))
          J.insert(j);
      }

    // delete the centers
    forall(const vertex &v, V)
      if(v->label > 0)
        S.erase(v);

    // remove junctions that are no longer required
    forall(const vertex &j, J) {
      bool keep = false;
      forall(const vertex &n, S.neighbors(j))
        if(n->label > 0) {
          keep = true;
          break;
        }
      if(!keep)
        S.erase(j);
    }
    return true;
  }

  int fixBorderTris(vvGraph &S)
  {
    // Find quads and triangles with -1 on all labels
    typedef Vector<3, vertex> Tri;
    std::vector<Tri> triList;
    typedef Vector<4, vertex> Quad;
    std::vector<Quad> quadList;

    int count = 0;
    int cnt = 0, pcnt = 0;
    do {
      forall(const vertex &v, S) {
        if(v->label >= 0)
          continue;
        forall(const vertex &n, S.neighbors(v)) {
          if(n->label >= 0)
            continue;
          vertex m = S.nextTo(v, n);
          if(m->label >= 0 or !S.uniqueTri(v, n, m))
            continue;
          vertex q = S.nextTo(v, m);
  
          if(q->label < 0)
            quadList.push_back(Quad(v, n, m, q));
          else
            triList.push_back(Tri(v, n, m));
        }
      }
  
      // Merge quads first
      forall(const Quad &q, quadList) {
        if(!S.contains(q[0]) or !S.contains(q[1]) or !S.contains(q[2]) or !S.contains(q[3]))
          continue;
        VtxVec vs;
        vs.push_back(q[0]);
        vs.push_back(q[2]);
        mergeVertices(S, vs);
        count+=2;
      }
      // Now merge triangles
      forall(const Tri &t, triList) {
        if(!S.contains(t[0]) or !S.contains(t[1]) or !S.contains(t[2]))
          continue;
        VtxVec vs;
        // If we are on the margin, only merge the margin pair
        if(t[0]->margin and t[1]->margin) {
          vs.push_back(t[0]);
          vs.push_back(t[1]);
        } else if(t[1]->margin and t[2]->margin) {
          vs.push_back(t[1]);
          vs.push_back(t[2]);
        } else if(t[2]->margin and t[2]->margin) {
          vs.push_back(t[2]);
          vs.push_back(t[0]);
        } else {
          vs.push_back(t[0]);
          vs.push_back(t[1]);
          vs.push_back(t[2]);
        }
        mergeVertices(S, vs);
        count++;
      }
      pcnt = cnt;
      cnt = quadList.size() + triList.size();

    } while(cnt > 0 and cnt != pcnt);

    return count;
  }

  // correct wrong MGXC normals (occur with non-convex cells)
  void correctNormals(vvGraph &S)
  {
    forall(const vertex& v, S) {
      if(v->label > 0) { // only consider cells
        forall(const vertex& n, S.neighbors(v)) {
          //std::cout << "b " << v->nrml << "/" << n->nrml << "/" << v->nrml * n->nrml << "//" << v->label << "//" << n->label << std::endl;
          if(v->nrml * n->nrml < 0){ // flip boundary normal
            //std::cout << "flipped old: " << n->nrml << std::endl;
            n->nrml *= -1.;
            //std::cout << "flipped new: " << n->nrml << std::endl;
          }

        }
      }
    }
    /*
    // debug
    forall(const vertex& v, S) {
      if(v->nrml.z() < 0)
        std::cout << "c " << v->nrml << "//" << v->label << std::endl;
      else
        std::cout << "d";
    }*/

  }


  /**
   * Generate a cell mesh (MGXC) type mesh from a normal (MGXM) one
   */
  bool mgxmToMgxc(const vvGraph &srcS, vvGraph &S, double wallMax) 
  {
    // If not the same, copy the graph
    if(&srcS != &S)
      S = srcS;

    // Check wall max parameter
    if(wallMax < 0)
      wallMax = std::numeric_limits<double>::min();
    else if(wallMax == 0)
      wallMax = std::numeric_limits<double>::max();

    // Delete unlabeled vertices
    uint n = S.size();
    for(size_t i = 0; i < n; ++i) {
      size_t j = n - i - 1;
      if(S[j]->label == 0)
        S.erase(S.begin() + j);
    }
		// Delete vertices with valence <=2
    n = S.size();
    for(size_t i = 0; i < n; ++i) {
      size_t j = n - i - 1;
			if(S.valence(S[j]) <= 2)
        S.erase(S.begin() + j);
    }

    // Fix border triangles
    fixBorderTris(S);

    // Mark the (maybe new) margin
    markMargins(S);

    // Check for border triangles and unlabeled vertices, at this point there should not be any
    if(checkBorderTris(S))
      throw(QString("Error, unlabeled vertices, border triangles, or islands of label found"));

    // Create new graph
    vvGraph T;

    // Make centers
    IntVtxMap C;
    VtxFloatMap labArea;
    forall(vertex v, S) {
			// Clear the type, we'll mark only "real" junctions with 'j' 
			// Required by PDG code (this is nasty)
			if(v->label > 0)
				v->type = 'c';
			else
		    v->type = ' ';
      forall(vertex n, S.neighbors(v)) {
        vertex m = S.nextTo(v, n);
        // get unique triangles
        if(!S.uniqueTri(v, n, m))
          continue;
        int label = getLabel(v, n, m);
        if(label <= 0)
          continue;
        // If new label add to map
        IntVtxMap::iterator it = C.find(label);
        if(it == C.end()) {
          vertex c;
          c->label = label;
          c->pos = 0;
          c->type = 'c';
          labArea[c] = 0;
          T.insert(c);
          it = C.insert(IntVtxPair(label, c)).first;
        }
        vertex c = it->second;
        float area = triangleArea(v->pos, n->pos, m->pos);
        c->pos += (v->pos + n->pos + m->pos) / 3.0 * area;
        labArea[c] += area;
      }
    }
  
    // Get the count of the number of unique labels in neighborhood of each border vertex
    VtxIntMap labCount;
    getLabelCount(S, labCount);

    // Find all junctions
    for(IntVtxMap::iterator it = C.begin(); it != C.end(); it++) {
      vertex c = it->second;
  
      // Find a vertex labeled the same as c connected to an edge
      vertex v(0), n(0);
      bool found = false;
      forall(vertex w, S) {
        if(w->label == -1) {
          forall(vertex m, S.neighbors(w))
            if(m->label == c->label) {
              found = true;
              v = w;
              n = m;
              break;
            }
        }
        if(found)
          break;
      }
      if(!found)
        throw(QString("Error, label not found finding junctions, cell label:%1").arg(c->label));
  
      // Walk along edge until junction found (> 2 labels or margin and > 1 label)
      vertex start = v;
      double length = 0;
      do {
        // if at a junction
        if(labCount[v] > 2 or (labCount[v] > 1 and v->margin) or v->type == 'j' or length > wallMax) {
          length = 0;
					// Insert if not there from previous cell
          if(v->type != 'j') {
            T.insert(v);
						v->type = 'j';
				  }
        }
        n = S.prevTo(v, n);
        while(n->label != -1)
          n = S.prevTo(v, n);
        vertex m = n;
        length += (v->pos - n->pos).norm();
        n = v;
        v = m;
      } while(v != start);
    }
  
    // Connect centers to junctions     
		for(IntVtxMap::iterator it = C.begin(); it != C.end(); it++) {
      vertex c = it->second;
      c->pos /= labArea[c];
  
      // Find a vertex labeled the same as c connected to an edge
      vertex v(0), n(0);
      bool found = false;
      forall(const vertex& w, S) {
        if(w->label == -1) {
          forall(const vertex& m, S.neighbors(w))
            if(m->label == c->label) {
              found = true;
              v = w;
              n = m;
              break;
            }
        }
        if(found)
          break;
      }
      if(!found) {
        Information::out << QString("Error, label %1 not found connecting centers")
                                                                   .arg(c->label) << endl;
        continue;
      }
  
      // Walk along edge until junction found
      vertex start = v;
      vertex pv(0);
      do {
        if(v->type == 'j') {       // at a junction
          // Join centers to junctions
          if(pv)
            T.spliceAfter(c, pv, v);
          else
            T.insertEdge(c, v);
          pv = v;
        }
        n = S.prevTo(v, n);
        while(n->label != -1)
          n = S.prevTo(v, n);
        vertex m = n;
        n = v;
        v = m;
      } while(v != start);
    }
  
    // Join junctions to centers
    forall(vertex v, T) {
      if(v->type == 'j') {     // at a junction
        int labcount = labCount[v];
        vertex n = S.anyIn(v);
        vertex pn(0);
        while(labcount--) {
          while(n->label == -1)
            n = S.nextTo(v, n);
          int label = n->label;
          IntVtxMap::iterator it = C.find(label);
          if(it == C.end()) {
            Information::out << 
                QString("mgxmToMgxc: Error, label %1 not found connecting centers")
                                                                   .arg(label) << endl;
            continue;
          }
          if(pn)
            T.spliceAfter(v, pn, it->second);
          else
            T.insertEdge(v, it->second);
          pn = it->second;
          while(n->label == label)
            n = S.nextTo(v, n);
        }
      }
    }
  
    // Connect junctions to each other
    forall(vertex c, T) {
      if(c->type != 'c')
        continue;
      forall(vertex n, T.neighbors(c)) {
        vertex m = T.nextTo(c, n);
        if(!T.edge(n, m))
          T.spliceBefore(n, c, m);
        if(!T.edge(m, n))
          T.spliceAfter(m, c, n);
      }
    }
    S.swap(T);
		markMargins(S);
		setNormals(S);

    //correctNormals(S);

    return true; 
  }

  //  Mark the margin vertices.
  void markMargins(vvGraph &M, vvGraph &S, bool remborders)
  {
    if(M.empty() or S.empty())
      return;

    // Mark margin and borders
    #pragma omp parallel for
    for(uint i = 0; i < M.size(); i++) {
      vertex v = M[i];
      markMargin(S, v, remborders);
    }
  }

  //  Mark the margin vertices.
  void markMargins(vvGraph &S, bool remborders) 
  { 
    markMargins(S, S, remborders); 
  }

  // Mark the margin vertices
  void markMargin(vvGraph &S, vertex v, bool remborders)
  {
    if(S.empty())
      return;

    // Mark margin and borders
    v->margin = false;

    // All vertices with valence <= 2 are border and margin
    if(S.valence(v) <= 2) {
      v->margin = true;
      if(v->type != 'l')
        v->label = -1;
      return;
    }

    int label = 0, labcount = 0, zcount = 0;
    forall(const vertex& n, S.neighbors(v)) {
      // If there is no edge between a pair of vertices next to each other
      // in a neighborhood, they must be on the margin
      vertex m = S.nextTo(v, n);
      if(!S.edge(n, m)) {
        v->margin = true;
        if(v->type != 'l')
          v->label = -1;
        return;
      }
      if(n->label == 0)
        zcount++;
      else if(n->label > 0 and n->label != label) {
        labcount++;
        label = n->label;
      }
    }
    if(v->type == 'l')
      return;

    // Adjacent to more than one label means a cell boundary
    if(labcount > 1  )
      v->label = -1;
    // Clear hanging border lines,
    if(v->label == -1 and remborders) {
      // On zero labeled area, only clear if remborders set (so we can draw)
      if(labcount == 0)
        v->label = 0;
      else if(labcount == 1 and zcount == 0)
        v->label = label;
    }
  }

  // Set the normals in a graph, return count of bad vertices
  int setNormals(const vvGraph &S)
  {
    int bad = 0;
    //#pragma omp parallel for
    for(uint i = 0; i < S.size(); i++) {
      vertex v = S[i];
      if(!setNormal(S, v))
        //#pragma omp atomic
        bad++;
    } 
    return bad;
  }

  // Set the normal of a vertex
  bool setNormal(const vvGraph &S, const vertex &v)
  {
    return calcNormal(S, v, v->nrml);
  }

  // Calculate the normal for a vertex
  bool calcNormal(const vvGraph &S, const vertex &v, Point3d &nrml)
  {
    nrml = Point3d(0,0,0);

    forall(const vertex& n, S.neighbors(v)) {
      vertex m = S.nextTo(v, n);
      // add to normal
      if(n != m and S.edge(n, m)) {
        if(S.nextTo(n, m) != v)
          nrml -= (n->pos - v->pos) ^ (m->pos - v->pos);
        else
          nrml += (n->pos - v->pos) ^ (m->pos - v->pos);
      }
    }
    // Normalize the normal
    double s = norm(nrml);
    if(s > 0)
      nrml /= s;
    else {
      nrml = Point3d(0,0,1);
      return false;
    }

    // Check for nan
    if(fabs(1.0 - norm(nrml)) < 1e-4)
      ;
    else {
      nrml = Point3d(0, 0, 1);
      return false;
    }
    return true;
  }

 // from here the code of the old cellgraph3d plugin


// Put triangle in standard form
// (from the compute 3d graph plugin)
Point3i triIndex(Point3i t)
{
  if(t.x() > t.y())
    std::swap(t.x(), t.y());
  if(t.y() > t.z())
    std::swap(t.y(), t.z());
  if(t.x() > t.y())
    std::swap(t.x(), t.y());
  return t;
}

// Get vertex index, combine positions closer that the tolerance
// (from the compute 3d graph plugin)
int vIndex(const Point3d &pos, Point3dIntMap &vMap, float tolerance)
{
  if(vMap.count(pos) > 0)
    return vMap[pos];
  else if(vMap.empty() or tolerance <= 0)
    return (vMap[pos] = vMap.size());
  else {
    Point3d p = pos;
    p.x() -= tolerance;
    Point3dIntMap::iterator it = vMap.lower_bound(p);
    if(it == vMap.end())
      --it;
    while(it != vMap.end() and it->first.x() - pos.x() <= tolerance) {
      if(norm(pos - it->first) <= tolerance)
        return it->second;
      ++it;
    }
    return (vMap[pos] = vMap.size());
  }
}

void addWallToList(std::map<vertex, IntInt>& vtxWallMap, const vertex& v, IntInt wall)
 {
    // if there is entry and 
    if(vtxWallMap.find(v) != vtxWallMap.end()){
      if(vtxWallMap[v] == wall){
        // do nothing
      } else {
        vtxWallMap[v] = std::make_pair(-1,-1);
      }
    } else { // not yet there
      vtxWallMap[v] = wall;
    }

   //v->selected = true;

 }

// create the neighborhood graph (copied from the compute 3d graph plugin)
bool neighborhoodGraph(const vvGraph& S, double tolerance, NhbdGraphInfo& info, bool throwError)
{
  progressStart("Create neighborhood graph", 0);
  std::map<int,double> cellWallArea;
  std::map<int,double> outsideWallArea;

  std::map<Point3i, std::set<Triangle> > triVtxSetMap; // map from tri idx to vtxs of tri

  std::map<int, std::set<vertex> > posVtxMap;
  std::map<vertex, IntInt> vtxWallMap;

  // output maps
  std::map<vertex, int> vtxNewLabelMap;
  std::map<vertex, int> vtxCellCounterMap;
  std::map<IntInt, double> sharedWallArea;

  Point3dIntMap vMap;
  Point3iIntSetMap triCell; // This is set of cells for each triangle
  //#pragma omp parallel for
  for(size_t i=0; i<S.size(); i++){
  //forall(const vertex& v, S) {
    const vertex& v = S[i];
    forall(const vertex& n, S.neighbors(v)) {
      //if(!progressAdvance(i)) continue;
      const vertex& m = S.nextTo(v,n);
      if(!S.uniqueTri(v,n,m)) // Only once per triangle in the mesh
        continue;
      // If the labels are not the same, this means we have more than one label on the same cell
      if((v->label != n->label or v->label != m->label) and throwError)
        throw QString("Error, the mesh has volumetric cells with more than one label.");
      int cell = v->label;
      cellWallArea[cell]+=triangleArea(v->pos,n->pos, m->pos);
      // Add the cell to the triangle map, points with similar positions will get the same index
      triCell[triIndex(Point3i(vIndex(v->pos, vMap, tolerance), vIndex(n->pos, vMap, tolerance), 
                             vIndex(m->pos, vMap, tolerance)))].insert(cell);

      Triangle tr1(v,n,m);
      tr1.v[0] = v;
      tr1.v[1] = n;
      tr1.v[2] = m;

      triVtxSetMap[triIndex(Point3i(vIndex(v->pos, vMap, tolerance), vIndex(n->pos, vMap, tolerance), 
                             vIndex(m->pos, vMap, tolerance)))].insert(tr1);

      posVtxMap[vIndex(v->pos, vMap, tolerance)].insert(v);
      posVtxMap[vIndex(n->pos, vMap, tolerance)].insert(n);
      posVtxMap[vIndex(m->pos, vMap, tolerance)].insert(m);
    }
  }

  // Generate opposite map, vertex indices to positions
  IntPoint3dMap pMap;
  forall(const Point3dIntPair &pr, vMap)
    pMap[pr.second] = pr.first;

  // Now calculate wall areas from triangle list
  std::map<IntInt, int> allWalls;
  int wallCounter = 1; // label for walls
  allWalls[std::make_pair(-1,-1)] = -1;

  forall(const Point3iIntSetPair &pr, triCell) {
    if(pr.second.size() > 2) {
      Information::out << "Error, triangle belongs to more than 2 cells:";
      forall(int cell, pr.second)
        Information::out << " " << cell;
      Information::out << " Area:" << triangleArea(pMap[pr.first.x()],pMap[pr.first.y()], pMap[pr.first.z()]) << endl;
      continue;
        
    } else if(pr.second.size() == 0) {
      Information::out << "Error, triangle belongs to no cell:";
      forall(int cell, pr.second)
        Information::out << " " << cell;
      Information::out << " Area:" << triangleArea(pMap[pr.first.x()],pMap[pr.first.y()], pMap[pr.first.z()]) << endl;
      continue;
    } else if(pr.second.size() == 1) { // tri belongs to only one cell -> outside wall
      IntSet::iterator cell1 = pr.second.begin();

      outsideWallArea[*cell1]+=triangleArea(pMap[pr.first.x()],pMap[pr.first.y()], pMap[pr.first.z()]);

      IntInt wall = std::make_pair(*cell1, 0);
      int wallLabel;
      if(allWalls.find(wall) != allWalls.end())
        wallLabel = allWalls[wall];
      else {
        wallLabel = wallCounter++;
        allWalls[wall] = wallLabel;
      }

      std::set<Triangle>& vv = triVtxSetMap[pr.first];
      Triangle p = *vv.begin();

      for(int i = 0; i<3; i++){
        addWallToList(vtxWallMap, p.v[i], wall);
      }
      // forall(const vertex& v, p){
      //   addWallToList(vtxWallMap, v, wall);
      // }

      continue;
    }
    // now pr.second.size() == 2 - the normal case, wall shared between two cells

    IntSet::iterator cell1 = pr.second.begin();
    IntSet::iterator cell2 = cell1;
    cell2++;
    float area = triangleArea(pMap[pr.first.x()],pMap[pr.first.y()], pMap[pr.first.z()]);
    sharedWallArea[std::make_pair(*cell1, *cell2)] += area;
    sharedWallArea[std::make_pair(*cell2, *cell1)] += area;

    IntInt wall1 = std::make_pair(*cell1, *cell2);
    IntInt wall2 = std::make_pair(*cell2, *cell1);
    // create unique labels for the wall (if not created already)
    int wallLabel1;
    if(allWalls.find(wall1) != allWalls.end())
      wallLabel1 = allWalls[wall1];
    else {
      wallLabel1 = wallCounter++;
      allWalls[wall1] = wallLabel1;
    }
    int wallLabel2;
    if(allWalls.find(wall2) != allWalls.end())
      wallLabel2 = allWalls[wall2];
    else {
      wallLabel2 = wallCounter++;
      allWalls[wall2] = wallLabel2;
    }

    std::set<Triangle>& vv = triVtxSetMap[pr.first];
    std::pair<Triangle,Triangle> cellPair;

    for(std::set<Triangle>::iterator it = vv.begin(); it!= vv.end(); it++){
      if(it == vv.begin()) cellPair.first = *it;
      else cellPair.second = *it;

      Triangle p = *it;
      //forall(const vertex& v, p){
      for(int i = 0; i<3; i++){
        if(p.v[i]->label == *cell1)
          addWallToList(vtxWallMap, p.v[i], wall1);
        if(p.v[i]->label == *cell2)
          addWallToList(vtxWallMap, p.v[i], wall2);
      }

    }
    info.sharedTris[std::make_pair(*cell1, *cell2)].insert(cellPair);

  }

    // extract information
    typedef std::pair<vertex, IntInt> viiP;
    forall(const viiP& p, vtxWallMap){
      vtxNewLabelMap[p.first] = allWalls[p.second];
    }

    typedef std::pair<int, std::set<vertex> > IntVtxSetP;
    forall(const IntVtxSetP& p, posVtxMap){
      forall(const vertex& v, p.second){
        vtxCellCounterMap[v] = p.second.size();
      }
    }

   // for cell atlas
   info.sharedWallArea = sharedWallArea;
   info.cellWallArea = cellWallArea;
   info.outsideWallArea = outsideWallArea;

   // for wall labeling plugin
   info.vtxCellCounterMap = vtxCellCounterMap;
   info.vtxNewLabelMap = vtxNewLabelMap;

   info.allWalls = allWalls;

  return true;
  }

  // finds all (cell) labels
  std::vector<int> findAllLabels(const vvGraph& S)
  {
    std::set<int> allLabels;
    std::vector<int> result;

    forall(const vertex &v, S)
      allLabels.insert(v->label);
    
    for(std::set<int>::iterator it=allLabels.begin(); it!=allLabels.end(); it++)
      if(*it>0)
        result.push_back(*it);

    return result;
  }

  // finds all (cell) labels
  std::set<int> findAllLabelsSet(const vvGraph& S)
  {
    std::set<int> allLabels;

    forall(const vertex &v, S)
      allLabels.insert(v->label);

    return allLabels;
  }

  // find all selected (cell) labels (return a set)
  std::set<int> findAllSelectedLabels(const vvGraph& S)
  {
    std::set<int> allLabels;

    forall(const vertex &v, S)
      if(v->selected)
        allLabels.insert(v->label);

    return allLabels;
  }


std::map<int, double> dijkstra(vvGraph& S, std::map<IntInt, double>& wallAreas, std::set<int>& selectedCells, bool equalWeights, double cutOffDistance, double wallThreshold)
{

    // the map (label->distance) for final results
    std::map<int, double> bestPaths;


    // the map (label->distance) for intermediate results
    std::map<int,double> bestSoFar;

    // multimap (distance->label) that acts as ordered map of current distances
    std::multimap<double, int> distanceLabelMap;

    // for initialization of above maps
    std::map<int, bool> labelInit;

    // init
    forall(const vertex &v, S){
      if(!labelInit[v->label]){
        if(selectedCells.find(v->label) != selectedCells.end()){
          bestSoFar[v->label] = 0;
          bestPaths[v->label] = 0;
          distanceLabelMap.insert(std::make_pair(0,v->label));
        } else {
          bestSoFar[v->label] = cutOffDistance;
          distanceLabelMap.insert(std::make_pair(cutOffDistance,v->label));
        }
      } 
      labelInit[v->label] = true;
    }

    // map of label->vector of all neighbor labels
    std::map<int, std::vector<int> > labelNeighborMap;

    forall(const IntIntDouPair& p, wallAreas){
      if(p.second > wallThreshold){
        labelNeighborMap[p.first.first].push_back(p.first.second);
      }
    }

    // now dijkstra
    while(!distanceLabelMap.empty()){

      std::multimap<double, int>::iterator it = distanceLabelMap.begin();
      int currentLabel = it->second;
      double curentDis = it->first;
      distanceLabelMap.erase(it);

      if(bestPaths.find(currentLabel) != bestPaths.end()) continue; // already searched
      bestPaths[currentLabel] = curentDis; // this is the shortest way to currentlabel

      // go through all (unvisited) neighbors
      for(uint i=0; i<labelNeighborMap[currentLabel].size(); i++){
        int neighborLabel = labelNeighborMap[currentLabel][i];
        if(bestPaths.find(neighborLabel) == bestPaths.end()){ // not already searched neighbors
          // calc current distance
          double dis = 1;
          if(!equalWeights){
            IntInt p = std::make_pair(currentLabel,neighborLabel);
            dis = max(0.,1./wallAreas[p]);
          }
          double newDis = curentDis+dis;
          if(newDis<bestSoFar[neighborLabel]){ // if nearer than previously, update distance
            distanceLabelMap.insert(std::make_pair(newDis,neighborLabel)); // create a new entry
            bestSoFar[neighborLabel] = newDis;
          }
        }
      }

    }

  std::map<int, double> bestPathsNew;

  forall(auto p, bestPaths){
    if(p.second == cutOffDistance) continue;
    bestPathsNew[p.first] = p.second;
  }

  return bestPathsNew;
}



std::map<int, double> dijkstra(std::set<int>& allCellLabels, std::map<IntInt, double>& wallAreas, std::set<int>& selectedCells, bool equalWeights, double cutOffDistance, double wallThreshold)
{

    // the map (label->distance) for final results
    std::map<int, double> bestPaths;


    // the map (label->distance) for intermediate results
    std::map<int,double> bestSoFar;

    // multimap (distance->label) that acts as ordered map of current distances
    std::multimap<double, int> distanceLabelMap;

    // for initialization of above maps
    std::map<int, bool> labelInit;

    // init
    forall(int l, allCellLabels){
      if(!labelInit[l]){
        if(selectedCells.find(l) != selectedCells.end()){
          bestSoFar[l] = 0;
          distanceLabelMap.insert(std::make_pair(0,l));
        } else {
          bestSoFar[l] = cutOffDistance;
          distanceLabelMap.insert(std::make_pair(cutOffDistance,l));
        }
      } 
      labelInit[l] = true;
    }

    // map of label->vector of all neighbor labels
    std::map<int, std::vector<int> > labelNeighborMap;

    forall(const IntIntDouPair& p, wallAreas){
      if(p.second > wallThreshold){
        labelNeighborMap[p.first.first].push_back(p.first.second);
      }
    }

    // now dijkstra
    while(!distanceLabelMap.empty()){

      std::multimap<double, int>::iterator it = distanceLabelMap.begin();
      int currentLabel = it->second;
      double curentDis = it->first;
      distanceLabelMap.erase(it);

      if(bestPaths.find(currentLabel) != bestPaths.end()) continue; // already searched
      bestPaths[currentLabel] = curentDis; // this is the shortest way to currentlabel

      // go through all (unvisited) neighbors
      for(uint i=0; i<labelNeighborMap[currentLabel].size(); i++){
        int neighborLabel = labelNeighborMap[currentLabel][i];
        if(neighborLabel < 1) continue;
        if(bestPaths.find(neighborLabel) == bestPaths.end()){ // not already searched neighbors
          // calc current distance
          double dis = 1;
          if(!equalWeights){
            IntInt p = std::make_pair(currentLabel,neighborLabel);
            dis = max(0.,1./wallAreas[p]);
          }
          double newDis = curentDis+dis;
          if(newDis<bestSoFar[neighborLabel]){ // if nearer than previously, update distance
            distanceLabelMap.insert(std::make_pair(newDis,neighborLabel)); // create a new entry
            bestSoFar[neighborLabel] = newDis;
          }
        }
      }

    }

  std::map<int, double> bestPathsNew;

  forall(auto p, bestPaths){
    if(p.second == cutOffDistance) continue;
    bestPathsNew[p.first] = p.second;
  }

  return bestPathsNew;
}


std::map<int, double> dijkstra(std::set<int>& allCellLabels, std::map<IntInt, double>& wallAreas, std::map<int, double>& initialValues, bool equalWeights, double cutOffDistance, double wallThreshold)
{

    // the map (label->distance) for final results
    std::map<int, double> bestPaths;


    // the map (label->distance) for intermediate results
    std::map<int,double> bestSoFar;

    // multimap (distance->label) that acts as ordered map of current distances
    std::multimap<double, int> distanceLabelMap;

    // for initialization of above maps
    std::map<int, bool> labelInit;

    // init
    forall(int l, allCellLabels){
      if(!labelInit[l]){
        if(initialValues.find(l) != initialValues.end()){
          bestSoFar[l] = initialValues[l];
          distanceLabelMap.insert(std::make_pair(initialValues[l],l));
        } else {
          bestSoFar[l] = cutOffDistance;
          distanceLabelMap.insert(std::make_pair(cutOffDistance,l));
        }
      } 
      labelInit[l] = true;
    }

    // map of label->vector of all neighbor labels
    std::map<int, std::vector<int> > labelNeighborMap;

    forall(const IntIntDouPair& p, wallAreas){
      if(p.second > wallThreshold){
        labelNeighborMap[p.first.first].push_back(p.first.second);
      }
    }

    // now dijkstra
    while(!distanceLabelMap.empty()){

      std::multimap<double, int>::iterator it = distanceLabelMap.begin();
      int currentLabel = it->second;
      double curentDis = it->first;
      distanceLabelMap.erase(it);

      if(bestPaths.find(currentLabel) != bestPaths.end()) continue; // already searched
      bestPaths[currentLabel] = curentDis; // this is the shortest way to currentlabel

      // go through all (unvisited) neighbors
      for(uint i=0; i<labelNeighborMap[currentLabel].size(); i++){
        int neighborLabel = labelNeighborMap[currentLabel][i];
        if(neighborLabel < 1) continue;
        if(bestPaths.find(neighborLabel) == bestPaths.end()){ // not already searched neighbors
          // calc current distance
          double dis = 1;
          if(!equalWeights){
            IntInt p = std::make_pair(currentLabel,neighborLabel);
            dis = max(0.,1./wallAreas[p]);
          }
          double newDis = curentDis+dis;
          if(newDis<bestSoFar[neighborLabel]){ // if nearer than previously, update distance
            distanceLabelMap.insert(std::make_pair(newDis,neighborLabel)); // create a new entry
            bestSoFar[neighborLabel] = newDis;
          }
        }
      }

    }

  std::map<int, double> bestPathsNew;

  forall(auto p, bestPaths){
    if(p.second == cutOffDistance) continue;
    bestPathsNew[p.first] = p.second;
  }

  return bestPathsNew;
}


// this will move to GraphAlgorithms
std::set<int> aStar(std::vector<int>& uniqueLabels, std::map<int, Point3d>& cellCentroids, std::map<IntInt, double>& wallAreas, int label1, int label2,
  double wallThreshold, bool equalWeights)
  {
    // the result will give the labels of the cells with the shortest path
    std::set<int> travelledLabels;


    std::map<int, std::vector<int> > labelNeighborMap;
    forall(const IntIntDouPair& p, wallAreas){
      if(p.second > wallThreshold){
        labelNeighborMap[p.first.first].push_back(p.first.second);
      }
    }


    std::map<int,double> travelledDis;
    std::map<int,int> cameFrom;
    std::map<int,double> estimatedDis;

    for(uint j = 0; j<uniqueLabels.size(); j++){
      int currentLabel = uniqueLabels[j];
      travelledDis[currentLabel] = HUGE_VAL;
      estimatedDis[currentLabel] = HUGE_VAL;
    }

    std::map<int,bool> checkedVtxs;

    std::map<int,double> pQueue;
    std::map<int,double>::iterator it, itMin;
    //typedef std::pair<int,int> IntInt;

    estimatedDis[label1] = norm(cellCentroids[label1] - cellCentroids[label2]);
    travelledDis[label1] = 0;
    pQueue[label1] = estimatedDis[label1];//norm(cellCentroids[label1] - cellCentroids[label2]);
    checkedVtxs[label1] = true;

    while(pQueue.size()!=0){

      itMin = pQueue.begin();
      for(it= pQueue.begin();it!=pQueue.end();it++){
        if(it->second < itMin->second) itMin = it;
      }

      int currentLabel = itMin->first;
      checkedVtxs[currentLabel] = true;

      //cout << "inloop " << itMin->first << "/" << itMin->second << endl;

      for(uint i=0; i<labelNeighborMap[currentLabel].size(); i++){
        int neighborLabel = labelNeighborMap[currentLabel][i];
        if(!checkedVtxs[neighborLabel]){
          double disToTarget = norm(cellCentroids[neighborLabel] - cellCentroids[label2]);
          double dis = norm(cellCentroids[neighborLabel] - cellCentroids[currentLabel]);
          if(equalWeights) dis = 1;
          if(pQueue.find(neighborLabel) == pQueue.end()){ // not yet visited
            pQueue[neighborLabel] = travelledDis[currentLabel] + dis + disToTarget;
            travelledDis[neighborLabel] = travelledDis[currentLabel] + dis;
            cameFrom[neighborLabel] = currentLabel;
          } else if(pQueue[currentLabel] + dis + disToTarget < pQueue[neighborLabel]){ // check if better
            pQueue[neighborLabel] = travelledDis[currentLabel] + dis + disToTarget;
            travelledDis[neighborLabel] = travelledDis[currentLabel] + dis;
            cameFrom[neighborLabel] = currentLabel;
          }
        }
      }
      pQueue.erase(currentLabel);

    }

    if(cameFrom.find(label2) == cameFrom.end()){
      //cout << "nothing found" << endl;
      //return false;
    } 

    int travel = label2;

    while(travel != label1){
      travelledLabels.insert(travel);
      travel = cameFrom[travel];
    }
    travelledLabels.insert(label1);

    return travelledLabels;
  } 

  vertex nearestVertexOnMesh(const Point3d& targetPoint, const vvGraph& S)
  {

  double minDis = HUGE_VAL;
  vertex minV;

  forall(const vertex& v, S){
    double currentDistance = norm(v->pos-targetPoint);
    if(currentDistance < minDis){
      minDis = currentDistance;
      minV = v;
    }
  }

  return minV;
  }

  Point3d nearestPointOnMesh(const Point3d& targetPoint, const vvGraph& S)
  {

  vertex minV = nearestVertexOnMesh(targetPoint, S);
  return minV->pos;
  }


  // create a 2D neighborhoodmap
  void neighborhood2D(vvGraph& S, std::map<IntInt, double>& neighMap, QString weightType, bool ignoreOutside)
  {

    std::map<IntInt, double> neighMapNew;

    typedef std::pair<vertex, vertex> VtxSeg;

    std::map<int, std::set<VtxSeg> > cellBorderMap;

    std::map<VtxSeg, std::set<int> > segCellMap;
    typedef std::pair<VtxSeg, std::set<int> > VtxSegIntSetP;

    std::map<int, Point3d> labelPosMap;

    /// find 4- or more way junctions
    //////
    std::map<vertex, std::set<int> > vtxNeighborMap;

    forall(const vertex& v, S){
      if(v->label > -1) continue;
      std::set<int> neighbs;
      forall(const vertex& n, S.neighbors(v)){
        if(n->label < 1) continue;
        neighbs.insert(n->label);
      }
      if(neighbs.size() > 3) vtxNeighborMap[v] = neighbs;
    }

    forall(auto p, vtxNeighborMap){
      forall(int l1, p.second){
        forall(int l2, p.second){
          if(l1 == l2) continue;
          neighMapNew[std::make_pair(l1,l2)] = 0;
        }
      }
    }
    //////
    IntFloatMap areas;
    std::map<int, std::set<vertex> > vertices;
    std::map<int, Point3f> centers;
    // Find cell centers, areas and normals.
    forall(const vertex& v, S) {
      forall(const vertex& n, S.neighbors(v)) {
        const vertex& m = S.nextTo(v, n);
        if(!S.uniqueTri(v, n, m))
          continue;
        int label = getLabel(v, n, m);
        if(label == 0)
          continue;
        vertices[label].insert(v);
        vertices[label].insert(n);
        vertices[label].insert(m);
        float area = triangleArea(v->pos, n->pos, m->pos);
        areas[label] += area;
        centers[label] += Point3f((v->pos + n->pos + m->pos) / 3.0 * area);
      }
    }

    forall(const auto& p, areas) {
      int label = p.first;
      float area = p.second;
      if(area > 0) {
        centers[label] /= area;
        labelPosMap[label] = Point3d(centers[label]);
      }
    }



    // create a map of cell border segments and their belonging cell labels
    forall(const vertex& v, S){
      if(v->label < 1) continue;
      //labelPosMap[v->label] = v->pos;
      forall(const vertex& n, S.neighbors(v)){
        forall(const vertex& m, S.neighbors(n)){
          if(m->label > -1) continue;
          bool stillSameCell = false;
          forall(const vertex& k, S.neighbors(m)){
            if(k->label == v->label) stillSameCell = true;
          }
          if(!stillSameCell) continue;

          VtxSeg vs = std::make_pair(n,m);
          if(m < n) vs = std::make_pair(m,n);
          segCellMap[vs].insert(v->label);
        }
      }
    }

    // go through all cell border segments and fill the neighborhood map
    forall(VtxSegIntSetP p, segCellMap){
      if(p.second.size() > 2) std::cout << "Warning: GraphUtils neighborhood2D: a border segments has more than 2 neighboring cells" << std::endl;
      else if(p.second.size() == 2){
        int c1 = *(p.second.begin());
        int c2 = *(++p.second.begin());
        double length = norm(p.first.first->pos - p.first.second->pos);
        neighMapNew[std::make_pair(c1,c2)] += length;
        neighMapNew[std::make_pair(c2,c1)] += length;
      } else if(p.second.size() == 1 and !ignoreOutside){ // outside border
        int c1 = *(p.second.begin());
        double length = norm(p.first.first->pos - p.first.second->pos);
        neighMapNew[std::make_pair(c1,0)] += length;
      }
      
    }

    // euclidean weight
    if(weightType == "Euclidean"){
      for(std::map<IntInt, double >::iterator it = neighMapNew.begin(); it!=neighMapNew.end(); it++){
        it->second = 1./norm(labelPosMap[it->first.first] - labelPosMap[it->first.second]);
      }
    }
    neighMap = neighMapNew;

  }



// finds an intersection point of a given cellmidpoint and a direction vector with the cell wall
// recursive to function with stopcounter to handle rare cases of no solutions, in case of multiple solutions, just take the last one found
bool findIntersectPoint(Point3d coordCellCentroid, Point3d& dirVec, std::vector<Triangle>& cellTriangles, int& stopCounter, Point3d& intersectP)
{
  int counter=0;
  Point3d coord (0,0,0);

  // check which triangles are hit by the vector
  forall(const Triangle &t, cellTriangles){
    Point3d t0 = t.v[0]->pos;
    Point3d t1 = t.v[1]->pos;
    Point3d t2 = t.v[2]->pos;
    Point3d intersectp;
    // filter out degenerated triangles
    double fac = 100;
    double maxTriLength = std::max(norm(t1-t0), std::max(norm(t2-t0), norm(t2-t1)));
    double minTriLength = std::min(norm(t1-t0), std::min(norm(t2-t0), norm(t2-t1)));

    if(maxTriLength < fac*minTriLength and 
      rayTriangleIntersect(coordCellCentroid, coordCellCentroid+dirVec, t0, t1, t2, intersectp)==1){
      double weight = (double)(1)/(double)(counter+1);
      coord = (1-weight) * coord + weight * intersectp;
      counter++;
    }
  }

  if(counter==0) // no point found
  {
    Point3d add (std::rand()/1000.0,std::rand()/1000.0,std::rand()/1000.0); // wiggle point
    if(stopCounter>10) // abort and give back zeroCoord (0,0,0)
    {
      intersectP = coord;
      stopCounter = 0;
      return false;
    } else if(stopCounter>3) { // move the cell center a bit
      Point3d newCoordCellCentroid(coordCellCentroid - add);
      stopCounter++;
      return findIntersectPoint(newCoordCellCentroid, dirVec, cellTriangles, stopCounter, intersectP);
    } else {
      Point3d newCoordCellCentroid(coordCellCentroid + add);
      stopCounter++;
      return findIntersectPoint(newCoordCellCentroid, dirVec, cellTriangles, stopCounter, intersectP);
    }  
  } else { // point found
    stopCounter = counter;
    intersectP = coord;
    return true;
  }


}

// estimates the size of the cell in a given direction
double estimateCellLength(int currentLabel, Point3d coordCellCentroid, Point3d& dirVec, triVector& cellTriangles)
{
  Point3d coordPos, coordNeg;
  Point3d negDirVec = -1*dirVec;
  Point3d zeroCoord (0,0,0);
  int stopCountPos = 0;
  int stopCountNeg = 0;

  if(coordCellCentroid == zeroCoord) // cell too small
    return 0;

  if(!findIntersectPoint(coordCellCentroid, dirVec, cellTriangles, stopCountPos, coordPos) or 
     !findIntersectPoint(coordCellCentroid, negDirVec, cellTriangles, stopCountNeg, coordNeg)){
    Information::out << "*** Could not find intersect & determine cell size at label: " << currentLabel << endl;
    //badCells[currentLabel] = true;
    return 0;
  }

    return norm(coordPos - coordNeg);
}


  std::map<int, std::vector<vertex> > generateLabelVertexMap(const vvGraph& S)
  {
    std::map<int, std::vector<vertex> > result;

    forall(const vertex &v, S) {
      result[v->label].push_back(v);
    }

    return result;
  }


void generateLabelTriangleMap(const vvGraph& S, std::map<int, triVector>& cellTriangles)
{
 // Progress progress("Analyze Cells 3D - Generate Label/Triangle Map", 0);
  progressStart("Analyze Cells 3D - Generate Label/Triangle Map", 0);

  std::map<int, triVector> cellTrianglesNew;

  // init
  std::map<int, std::vector<vertex> > lvMap = generateLabelVertexMap(S);

  typedef std::pair<int, std::vector<vertex> > labelVertexVecPair;

  forall(labelVertexVecPair p, lvMap){
    //int currentLabel = p.first;
    forall(const vertex &v, p.second){
      forall(const vertex &n, S.neighbors(v)){
        if(!progressAdvance()) continue;
        //if(v->label == n->label){
          const vertex& m = S.nextTo(v,n);
          ////#pragma omp critical
          ////{
            if(S.uniqueTri(v, n, m)){
            //if(m->label == n->label){
              Triangle currentTriangle(v,n,m);
              //currentTriangle[0] = v;
              //currentTriangle[1] = n;
              //currentTriangle[2] = m;
              
              cellTrianglesNew[v->label].push_back(currentTriangle);
              //triangleCellLabelMap[currentTriangle].insert(v->label);
            //}
            ////}
          }
        //}
      }
    }
  }
  cellTriangles = cellTrianglesNew;

  }


  // gives an ordered vertex vector of the border vertices of a cell.
  // input: the cell vertices (inclusive the border)
  void findBorderVerticesOfCell(vvGraph& S, int cellLabel, std::set<vertex>& verticesCell, std::vector<vertex>& borderVerticesOrdered)
  {

  // find one border vertex and walk around the border
  vertex lastVertex, firstVertex;

  forall(const vertex v, verticesCell){
    forall(const vertex &w, S.neighbors(v)){
      if((v->label == -1 and w->label == cellLabel) or (cellLabel == 0 and v->margin)){
       firstVertex = v;
       break;
      }
    }
  }

  borderVerticesOrdered.push_back(firstVertex);
  lastVertex = firstVertex;
  //lastVertex->selected = true;
  do{
    bool notFound = true;
    forall(const vertex &v, S.neighbors(lastVertex)){ // look for next border vertex
      if((v->label == -1 and notFound) or (cellLabel == 0 and v->margin and notFound)){
        // check if this is still a border vertex of the cell & its going into the right direction
        bool belongsToCell = false;
        bool notTheSame = false;
        forall(const vertex &w, S.neighbors(v)){
          if(w->label == cellLabel or cellLabel == 0) belongsToCell = true;
        }
        if(borderVerticesOrdered.size()<2) notTheSame = true;
        else if(v != borderVerticesOrdered[borderVerticesOrdered.size()-2]){
          notTheSame = true;
        }

        if(belongsToCell and notTheSame){
          borderVerticesOrdered.push_back(v);
          lastVertex = v;
          notFound = false;
        }

      }
    }
  } while (firstVertex != lastVertex);


  }


  // gives an ordered vertex vector of the border vertices of a cell.
  // input: the cell vertices (inclusive the border)
  void findBorderVerticesOfCellsCombined(vvGraph& S, std::set<int> cellLabels, std::set<vertex>& verticesCells, std::vector<vertex>& borderVerticesOrdered)
  {

  // find one border vertex and walk around the border
  vertex lastVertex, firstVertex;

  forall(const vertex& v, verticesCells){
    if(v->label > -1) continue;
    if(v->margin){
      firstVertex = v;
      break;
    }
    std::cout << "v " << v->label << std::endl;
    // go through all neighbors and look for a label that is not -1 or in cellLabels
    bool belongsToCells = false;
    bool isOnOutside = false;
    int testL = -2;
    forall(const vertex &w, S.neighbors(v)){
      //std::cout << "graph " << (cellLabels.find(w->label) != cellLabels.end()) << "/" << w->label << std::endl;
      if(cellLabels.find(w->label) != cellLabels.end()) belongsToCells = true;
      if(cellLabels.find(w->label) == cellLabels.end() and w->label > -1){
        testL = w->label;
        isOnOutside = true;
      } 

    }
    if(belongsToCells and isOnOutside){
      std::cout << "w " << v->pos  << "/" << testL << std::endl;
      firstVertex = v;
    } break;
  }

  borderVerticesOrdered.push_back(firstVertex);
  lastVertex = firstVertex;
  std::cout << "f " << lastVertex->pos << std::endl;
  lastVertex->selected = true;
  do{
    //std::cout << "f " << lastVertex->pos << "/" << (*cellLabels.begin()) << std::endl;
    //bool notFound = true;
    forall(const vertex &v, S.neighbors(lastVertex)){ // look for next border vertex
      if((v->label == -1) or (v->margin)){
        // check if this is still a border vertex of the cell & its going into the right direction
        bool belongsToCells = false; // min 1 vertex must belong to the cells
        bool notTheSame = false;
        bool commonBorder = true; // min 1 vertex must belong to other cells (unless its margin)
        int testL = -2;
        //bool marginOnlyOne = true;
        forall(const vertex &w, S.neighbors(v)){
          if(cellLabels.find(w->label) != cellLabels.end()) belongsToCells = true;
          if(cellLabels.find(w->label) == cellLabels.end() and w->label > -1){
            testL = w->label;
            commonBorder = false;
          } 
        }
        if(borderVerticesOrdered.size()<2) notTheSame = true;
        else if(v != borderVerticesOrdered[borderVerticesOrdered.size()-2]){
          notTheSame = true;
        }

        if((belongsToCells and notTheSame and !commonBorder) or (v->margin and belongsToCells and notTheSame)){
          borderVerticesOrdered.push_back(v);
          lastVertex = v;

          std::cout << "f_l " << lastVertex->pos << "/" << testL << "/" << belongsToCells << "/" << notTheSame << "/" << commonBorder << "/" << v->margin << std::endl;
          break;
          //notFound = false;
        }

      }
    }

  } while (firstVertex != lastVertex);


  }


  int getKeyOfMaxValue(const std::map<int, double>& mapToTest)
  {
    double maxV = -HUGE_VAL;
    int maxKey = -1;
    forall(auto p, mapToTest){
      if(p.second > maxV){
        maxV = p.second;
        maxKey = p.first;
      }
    }
    return maxKey;
  }

  std::map<int,int> shiftMap(std::map<int, double>& mapToShift, int shift)
  {
    std::map<int,int> newKeyOldKeyMap;
    std::map<int, double> newMap;
    int newStart = mapToShift.size() - shift;
    forall(auto p, mapToShift){
      int newKey;
      if(p.first < shift) newKey = newStart + p.first;
      else newKey = p.first - shift;

      newKeyOldKeyMap[newKey] = p.first;
      newMap[newKey] = p.second;
    }
    mapToShift = newMap;

    return newKeyOldKeyMap;
  }

}
