//
// This file is part of 3DCellAtlasOvule.
// Copyright (C) 2015-2016 George W. Bassel and collaborators.
//
// 3DCellAtlasOvule is an AddOn for MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2012 Richard S. Smith and collaborators.
//
// MorphoGraphX is free software, and are licensed under under the terms of the
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
//
#include <CellAtlasOvule.hpp>
#include <Progress.hpp>
#include <Geometry.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <iterator>

#include <CGAL/point_generators_3.h>
#include <CGAL/algorithm.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/convex_hull_3.h>
#include <CGAL/Cartesian.h>

#include <Triangulate.hpp>
#include <MeshBuilder.hpp>

#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_2_algorithms.h>
#include <iostream>

#include <gsl/gsl_fit.h>

#include "StackProcess.hpp" // findNeighborsLabel

// CGal Convex Hull
namespace QHull
{
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
//typedef CGAL::Cartesian<double> K;

typedef CGAL::Polyhedron_3<K>                     Polyhedron_3;
typedef K::Point_3                                Point_3;
typedef Polyhedron_3::Facet Facet;
typedef Polyhedron_3::Vertex Vertex;
typedef Polyhedron_3::Vertex_handle Vertex_handle;
typedef Polyhedron_3::Halfedge_handle Halfedge_handle;

}



using namespace std;

namespace mgx
{

   // finds and deletes duplicated points in a vector
   std::vector<Point3d> uniquePointsOfVec(const std::vector<Point3d>& inputVertices, double eps)
    {
    std::vector<Point3d> uniquePoints;
  
    // fill the points with MDX mesh data
    forall(const Point3d &p, inputVertices) {
      bool unique = true;
      forall(const Point3d& u, uniquePoints){
        if(norm(u-p)<eps)
          unique = false;
      }
      if(unique)
        uniquePoints.push_back(p);
    }
  
  
    return uniquePoints;
    }



  // calcConvexHull
  bool calcConvexHull(std::vector<Point3d>& inputVertices, std::vector<Point3d>& outputVertices, std::vector<Point3i>& triList)
  {

    if(isDataPlanar(inputVertices)) // to avoid problems
      return false;

    // // // // convert points vector into qhull data type // // // //
    int counter = 0;
    int numpoints = inputVertices.size();

    // 2 - Compute convex hull
    std::vector<QHull::Point_3> qh_pts(inputVertices.size());

    for(size_t i = 0 ; i < inputVertices.size() ; ++i)
      qh_pts[i] = QHull::Point_3(inputVertices[i].x(), inputVertices[i].y(), inputVertices[i].z());


    //cout << "CGAL qhull " << endl;
    QHull::Polyhedron_3 poly;
    CGAL::convex_hull_3(qh_pts.begin(), qh_pts.end(), poly);

    // 3 - parse the result
    //std::unordered_map<const QHull::Vertex*, vertex> vtx_map;

    std::map<QHull::Point_3, int> pointVertexMap;
    typedef std::pair<Point3f, int> PointVertexId;

    int vertCounter = 0;

    typedef QHull::Polyhedron_3::Vertex_iterator Vertex_iterator;
    //for ( Vertex_iterator v = poly.vertices_begin(); v != poly.vertices_end(); ++v)
      //std::cout << v->point() << std::endl;

    typedef QHull::Polyhedron_3::Facet_iterator Facet_iterator;
    for ( Facet_iterator fa = poly.facets_begin(); fa != poly.facets_end(); ++fa){

      // save points
      QHull::Polyhedron_3::Halfedge_around_facet_circulator h = fa->facet_begin();

      QHull::Halfedge_handle hfh = fa->halfedge();
      QHull::Vertex_handle vh1 = hfh->vertex();
      QHull::Vertex_handle vh2 = hfh->next()->vertex();
      QHull::Vertex_handle vh3 = hfh->opposite()->vertex();

      QHull::Point_3 p1 = vh1->point();
      QHull::Point_3 p2 = vh2->point();
      QHull::Point_3 p3 = vh3->point();

      Point3d p1f ((double)p1.x(), (double)p1.y(), (double)p1.z());
      Point3d p2f ((double)p2.x(), (double)p2.y(), (double)p2.z());
      Point3d p3f ((double)p3.x(), (double)p3.y(), (double)p3.z());

      if(pointVertexMap.find(p1) == pointVertexMap.end()){
        pointVertexMap[p1] = vertCounter;
        //vertex v;
        //v->pos = Point3d((float)p1.x(), (float)p1.y(), (float)p1.z());
        //v->saveId = vertCounter;
        outputVertices.push_back(Point3d((float)p1.x(), (float)p1.y(), (float)p1.z()));
        vertCounter++;
      }
      if(pointVertexMap.find(p2) == pointVertexMap.end()){
        pointVertexMap[p2] = vertCounter;
        //vertex v;
        //v->pos = Point3d((float)p2.x(), (float)p2.y(), (float)p2.z());
        //v->saveId = vertCounter;
        outputVertices.push_back(Point3d((float)p2.x(), (float)p2.y(), (float)p2.z()));
        vertCounter++;
      }
      if(pointVertexMap.find(p3) == pointVertexMap.end()){
        pointVertexMap[p3] = vertCounter;
        //vertex v;
        //v->pos = Point3d((float)p3.x(), (float)p3.y(), (float)p3.z());
        //v->saveId = vertCounter;
        outputVertices.push_back(Point3d((float)p3.x(), (float)p3.y(), (float)p3.z()));
        vertCounter++;
      }

      // save triangle
      Point3i tri (pointVertexMap[p1], pointVertexMap[p2], pointVertexMap[p3]);
      triList.push_back(tri);

    }

  return true;
  }


 // 3D Lobyness stuff
 ///////////////////

  void generateCellVerticesMap(const vvGraph& S, std::map<int, std::set<vertex> >& cellVertices)
  {
    forall(const vertex &v, S){
      cellVertices[v->label].insert(v);
    }
  }

  // from Cell Atlas, remove duplicated code !
 void generateCellTriangleMap(const vvGraph& S, std::map<int, std::set<vertex> >& cellVertices, std::map<int, std::set<Triangle> >& cellTriangles)
{
  cellVertices.clear();
  cellTriangles.clear();

  if(cellVertices.empty()){
    generateCellVerticesMap(S, cellVertices);
  }

  std::set<int> allLabels;
  forall(const vertex& v, S){
    if(v->label < 1) continue;
    allLabels.insert(v->label);
  }

  forall(int label, allLabels){
    forall(const vertex &v, cellVertices[label]){
      forall(const vertex &n, S.neighbors(v)){
        const vertex& m = S.nextTo(v,n);
        if(S.uniqueTri(v, n, m)){
          Triangle t(v,n,m);
          cellTriangles[v->label].insert(t);
        }
      }
    }
  }

  }


  // MGX3D version
 void generateCellTriangleMap(const CellTissue& T, std::map<int, std::set<vertex> >& cellVertices, std::map<int, std::set<Triangle> >& cellTriangles)
{

  cellVertices.clear();
  cellTriangles.clear();


  forall(const cell& c, T.C){
    forall(const vertex &v, c->S){
      cellVertices[c->label].insert(v);

      forall(const vertex &n, c->S.neighbors(v)){
        const vertex& m = c->S.nextTo(v,n);
        if(c->S.uniqueTri(v, n, m)){
          Triangle t(v,n,m);
          cellTriangles[c->label].insert(t);
        }
      }
    }
  }

}


  double cellVolume(std::set<Triangle>& cellTris)
  {
    double volume = 0;
    forall(const Triangle &t, cellTris){
      volume += signedTetraVolume(t.v[0]->pos, t.v[1]->pos, t.v[2]->pos);
    }
    return volume;
  }

  double cellSurfaceArea(std::set<Triangle>& cellTris)
  {
    double surfArea = 0;
    forall(const Triangle &t, cellTris){
      surfArea += triangleArea(t.v[0]->pos, t.v[1]->pos, t.v[2]->pos);
    }
    return surfArea;
  }

  std::set<Triangle> getConvexHullTris3D(std::vector<Point3d> inputPoints)
  {

    std::vector<Point3d> cHullPoints;
    std::vector<vertex> verticesList;
    std::vector<Point3i> triList;
  
    if(!calcConvexHull(inputPoints, cHullPoints, triList))
      std::cout << "error convex hull" << std::endl;

    forall(Point3d p, cHullPoints){
      vertex v;
      v->pos = p;
      verticesList.push_back(v);
    }

    std::set<Triangle> convHullTris;

    forall(Point3i tri, triList){
      Triangle t(verticesList[tri.x()], verticesList[tri.y()], verticesList[tri.z()]);
      convHullTris.insert(t);
    }

    return convHullTris;
  }

  std::vector<Point3d> getConvexHullPoints3D(std::vector<Point3d> inputPoints)
  {

    std::vector<Point3d> cHullPoints;
    std::vector<Point3i> triList;
  
    if(!calcConvexHull(inputPoints, cHullPoints, triList))
      std::cout << "error convex hull" << std::endl;

    return cHullPoints;
  }

  std::vector<Point3d> vtxsPosFromTris(std::set<Triangle>& cellTris)
  {
    std::vector<Point3d> vtxs;
    forall(Triangle t, cellTris){
      vtxs.push_back(t.v[0]->pos);
      vtxs.push_back(t.v[1]->pos);
      vtxs.push_back(t.v[2]->pos);
    }

    return uniquePointsOfVec(vtxs,0.01);
  }

  double convexitySurfaceArea(std::set<Triangle>& cellTris)
  {
    // surface area cell / surface area convex hull
    std::set<Triangle> chTris = getConvexHullTris3D(vtxsPosFromTris(cellTris));
    return cellSurfaceArea(chTris)/cellSurfaceArea(cellTris);
  }

  double convexityVolume(std::set<Triangle>& cellTris)
  {
    // volume convex hull / volume cell
    std::set<Triangle> chTris = getConvexHullTris3D(vtxsPosFromTris(cellTris));
    return cellVolume(cellTris)/cellVolume(chTris);
  }

  double sphericity(std::set<Triangle>& cellTris)
  {
    // old version: PI^1/3 * (6*Volume)^2/3 / surface area
    double PI = 3.14159;
    return 36 * PI * pow(cellVolume(cellTris),2.0) / pow(cellSurfaceArea(cellTris),3.0);
  }

  double compactness(std::set<Triangle>& cellTris)
  {
    // old version: PI^1/3 * (6*Volume)^2/3 / surface area
    double PI = 3.14159;
    std::vector<Point3d> chPos = getConvexHullPoints3D(vtxsPosFromTris(cellTris));

    // brute force!
    double maxDis = -HUGE_VAL;
    forall(Point3d p, chPos){
      forall(Point3d n, chPos){
        double dis = norm(p-n);
        if(dis > maxDis) maxDis = dis;
      }
    }

    double surfEnclosingSphere = PI * maxDis * maxDis;

    return 36 * PI * pow(cellVolume(cellTris),2.0) / pow(surfEnclosingSphere,3.0);
  }


  double sumSquaresPoint(Point3d p)
  {
    return (p.x()*p.x() + p.y()*p.y() + p.z()*p.z());
  }

  double circumscribedCircleTetra(const std::vector<Point3d>& tetra, Point3d& center)
  {
    //double radius;

    Matrix<4,4,double> matA, matX, matY, matZ;

    for(uint i = 0; i<4; i++){
      double sumSq = sumSquaresPoint(tetra[i]);

      matA[0][i] = tetra[i].x();
      matA[1][i] = tetra[i].y();
      matA[2][i] = tetra[i].z();
      matA[3][i] = 1;

      matX[0][i] = sumSq;
      matX[1][i] = tetra[i].y();
      matX[2][i] = tetra[i].z();
      matX[3][i] = 1;

      matY[0][i] = sumSq;
      matY[1][i] = tetra[i].x();
      matY[2][i] = tetra[i].z();
      matY[3][i] = 1;

      matZ[0][i] = sumSq;
      matZ[1][i] = tetra[i].x();
      matZ[2][i] = tetra[i].y();
      matZ[3][i] = 1;

    }

    double detA = det(matA);
    double detX = det(matX);
    double detY = -det(matY);
    double detZ = det(matZ);

    center = Point3d(detX/2/detA, detY/2/detA, detZ/2/detA);

    //std::cout << "ttt " << norm(center - tetra[0]) << "/" << norm(center - tetra[1]) << "/" << norm(center - tetra[2]) << "/" << norm(center - tetra[3]) << std::endl;

    return norm(center - tetra[0]);
  }

  double largestEmptySphere(vvGraph& S, std::set<Triangle>& cellTris, Point3d& bestCenter)
  {
//     // delaunay 3D
//     CGALVoronoi3D cv;
//     std::vector<Point3d> vertices = vtxsPosFromTris(cellTris);
//     cv.setVoronoiCenters(vertices);
//     std::vector<std::vector<Point3d> > tetras;
//     cv.delaunay3D(tetras);

//     std::map<double, Point3d> radCenterMap;

//     for(uint i = 0; i < tetras.size(); i++){
//       std::vector<Point3d> currentTetra = tetras[i];
//       if(heightTetra(currentTetra) < 0.1) continue;
//       Point3d center;
//       double currentRad = circumscribedCircleTetra(currentTetra, center);
//       radCenterMap[currentRad] = center;
//     }

//     for(auto it = radCenterMap.end(); it != radCenterMap.begin(); it--){
//       if(pointUnderMesh(cellTris, it->second)){
//         bestCenter = it->second;
//         return it->first;
//       }
//     }

//     //
    return -1;
  }

  bool Lobyness3D::run(Mesh *m, const QString &measure)
  {           
    const vvGraph& S = m->graph();
    const QString meshType = m->tissue().meshType();

    std::set<int> allLabels;

    std::vector<vertex> vtxVec;
    std::map<int, std::vector<Point3i> > labelTriMap;

    std::map<int, std::set<vertex> > cellVertices;
    std::map<int, std::set<Triangle> > cellTriangles;

    if(meshType == "MGX3D"){
      forall(const cell& c, m->tissue().C){
        allLabels.insert(c->label);
      }
      generateCellTriangleMap(m->tissue(),cellVertices,cellTriangles);
    } else if(meshType == "MGXM"){
      forall(const vertex& v, S){
        if(v->label < 1) continue;
        allLabels.insert(v->label);
      }
      generateCellTriangleMap(S,cellVertices,cellTriangles);
    }

    std::vector<int> allLabelsVec;
    forall(int l, allLabels){
      allLabelsVec.push_back(l);
    }
    
    // go through every cell and calculate measures
    #pragma omp parallel for schedule(guided)
    for(uint i = 0; i < allLabelsVec.size(); i++){
      int l = allLabelsVec[i];
      double value = 0;
      if(measure == "Convexity"){
        value = convexitySurfaceArea(cellTriangles[l]);
      } else if(measure == "Solidity"){
        value = convexityVolume(cellTriangles[l]);
      } else if(measure == "Sphericity"){
        value = sphericity(cellTriangles[l]);
      } else if(measure == "Compactness"){
        value = compactness(cellTriangles[l]);
      } else if(measure == "LargestEmptySphere"){
        Point3d bestCenter;
        value = largestEmptySphere(m->graph(),cellTriangles[l],bestCenter);
      } else {
        std::cout << "not implemented yet" << std::endl;
      }
      #pragma omp critical
        m->labelHeat()[l] = value;
    }

    m->setShowLabel("Label Heat");
    m->heatMapBounds() = m->calcHeatMapBounds();
    m->updateTriangles();

    return true;
  }
  REGISTER_PROCESS(Lobyness3D);

  bool Lobyness3DConvexSurfArea::run(Mesh *m)
  {
    if(m->graph().empty()) return false;
    Lobyness3D l3d(*this);
    l3d.run(m,"Convexity");

    AttrMap<int, double>& attrData = m->attributes().attrMap<int, double>("Measure Label Double Lobyness3D/Convexity");
    attrData.clear();

    forall(IntFloatPair p, m->labelHeat())
      attrData[p.first] = p.second;

    return true;

  }
  REGISTER_PROCESS(Lobyness3DConvexSurfArea);

  bool Lobyness3DConvexVolume::run(Mesh *m)
  {
    if(m->graph().empty()) return false;
    Lobyness3D l3d(*this);
    l3d.run(m,"Solidity");

    AttrMap<int, double>& attrData = m->attributes().attrMap<int, double>("Measure Label Double Lobyness3D/Solidity");
    attrData.clear();

    forall(IntFloatPair p, m->labelHeat())
      attrData[p.first] = p.second;

    return true;

  }
  REGISTER_PROCESS(Lobyness3DConvexVolume);

  bool Lobyness3DSphericity::run(Mesh *m)
  {
    if(m->graph().empty()) return false;
    Lobyness3D l3d(*this);
    l3d.run(m,"Sphericity");

    AttrMap<int, double>& attrData = m->attributes().attrMap<int, double>("Measure Label Double Lobyness3D/Sphericity");
    attrData.clear();

    forall(IntFloatPair p, m->labelHeat())
      attrData[p.first] = p.second;

    return true;

  }
  REGISTER_PROCESS(Lobyness3DSphericity);

  bool Lobyness3DCompactness::run(Mesh *m)
  {
    if(m->graph().empty()) return false;
    Lobyness3D l3d(*this);
    l3d.run(m,"Compactness");

    AttrMap<int, double>& attrData = m->attributes().attrMap<int, double>("Measure Label Double Lobyness3D/Compactness");
    attrData.clear();

    forall(IntFloatPair p, m->labelHeat())
      attrData[p.first] = p.second;

    return true;

  }
  REGISTER_PROCESS(Lobyness3DCompactness);


//   bool Lobyness3DLargestEmptySphere::run(Mesh *m)
//   {
//     if(m->graph().empty()) return false;
//     Lobyness3D l3d(*this);
//     l3d.run(m,"LargestEmptySphere");

//     AttrMap<int, double>& attrData = m->attributes().attrMap<int, double>("Measure Label Double /Lobyness 3D/LargestEmptySphere");
//     attrData.clear();

//     forall(IntFloatPair p, m->labelHeat())
//       attrData[p.first] = p.second;

//     return true;

//   }
//   REGISTER_PROCESS(Lobyness3DLargestEmptySphere);

  bool ConvexHullCGAL::run(Mesh* mesh, Mesh* mesh2, bool workOnSel, Matrix4d mGLTot)
  { 
    std::vector<Point3d> allPoints, uniquePoints;
    vvGraph &S = mesh->graph();
    vvGraph &S2 = mesh2->graph();
    forall(const vertex &v, S){
      if(v->selected or !workOnSel){
        Point3d currentPos = v->pos;
        currentPos = multMatrix4Point3(mGLTot, currentPos);
        allPoints.push_back(currentPos);
      }
    }

    // make points unique to avoid trouble
    uniquePoints = uniquePointsOfVec(allPoints,0.0001);

    std::vector<Point3d> verticesList;
    std::vector<Point3i> triList;
 
    if(!isDataPlanar(uniquePoints)) {
      if(!calcConvexHull(uniquePoints, verticesList, triList))  // this does the main job and returns a list of vertices and triangles 
        throw QString("ConvexHullCGAL::run Please select vertices in a mesh or set Work On Selection to false to use all");
    } else
      throw QString("ConvexHullCGAL::run Data planar. It is not possible to run this process on 2D data");

    MeshBuilder mb;

    forall(Point3i t, triList)
      mb.addTri(verticesList[t.x()],verticesList[t.y()],verticesList[t.z()]);

    mb.writeVertexVec(false);
  
    vvGraph Temp = S2;
    mesh2->reset();
    
    if(!Mesh::meshFromTriangles(S2, mb.vtxVec, mb.triVec)) {
      S2 = Temp;
      throw(QString("Error, cannot add all the triangles")); 
    }

    mesh2->updateAll();
    return true;
  }
  REGISTER_PROCESS(ConvexHullCGAL);


// detect cell layers
  bool LabelOvules::run(Mesh *m1, int maxSize, QString ovuleAttr)
  {

    vvGraph& S = m1->graph();

    AttrMap<int, Point3d>& centroids = m1->attributes().attrMap<int, Point3d>("Measure Label Vector CellCentroids");

    // create cell graph
    NhbdGraphInfo info;
    neighborhoodGraph(S, 0.0001, info); // now in graphutils

    std::map<IntInt, double> neighMap = info.sharedWallArea;
    std::map<IntInt, double> neighMapEucl;

    forall(auto p, neighMap){
      neighMapEucl[p.first] = 1./norm(centroids[p.first.first] - centroids[p.first.second]);
    }



    // find selected cells
    std::set<int> selectedLabels = findAllSelectedLabels(S);
    std::set<int> allCells = findAllLabelsSet(S);

    std::vector<int> selectedLabelsVec;
    forall(int l, selectedLabels){
      selectedLabelsVec.push_back(l);
    }

    std::map<int, std::map<int, double> > mapOfDistances;
    std::map<int, std::map<int, double> > mapOfDistancesEucl;

    //#pragma omp parallel for schedule(guided)
    for(uint i = 0; i < selectedLabelsVec.size(); i++){
      //run cell distance for single label
      std::set<int> selectedLabel;
      selectedLabel.insert(selectedLabelsVec[i]);
      std::map<int, double> heatDijk = dijkstra(allCells, neighMap, selectedLabel, true, 1000, 0.0001);

      std::map<int, double> heatDijkEucl = dijkstra(allCells, neighMapEucl, selectedLabel, true, 1000, 0.0001);

      // write to attr map
      mapOfDistances[i] = heatDijk;
      mapOfDistancesEucl[i] = heatDijkEucl;
    }

    std::map<int, int > shortestMap;

    forall(int l, allCells){
      int lowest = maxSize;
      double dis = HUGE_VAL;
      int map = 0;
      forall(auto p, mapOfDistances){
        if(p.second[l] < lowest){
          lowest = p.second[l];
          dis = mapOfDistancesEucl[p.first][l];
          map = p.first+1;
        } else if(p.second[l] == lowest and mapOfDistancesEucl[p.first][l] < dis){
          map = p.first+1;
          dis = mapOfDistancesEucl[p.first][l];
        }
      }
      shortestMap[l] = map;
      m1->parents()[l] = map;
    }
    
    m1->updateTriangles();
    m1->setUseParents(true);


    if(ovuleAttr != ""){
      AttrMap<int, int>& ovules = m1->attributes().attrMap<int, int>("Measure Label Int " + ovuleAttr);
      ovules.clear();
      forall(auto p, m1->parents()){
        ovules[p.first] = p.second;
      }
    }

    return true;
  }
  REGISTER_PROCESS(LabelOvules);


// detect cell layers
  bool LabelCellsFromHeat::run(Mesh *m1, int h1, int h2, int h3, int h4, int h5, int h6, bool overwrite)
  {

    int h1V = 1;
    int h2V = 2;
    int h3V = 3;
    int h4V = 4;
    int h5V = 5;
    int h6V = 6;


    forall(auto& p, m1->labelHeat()){
      if(m1->parents()[p.first] > 0 and !overwrite) continue;
      if(abs(p.second - h1V) < 0.001 and h1 >= 0){
        m1->parents()[p.first] = h1;
      } else if(abs(p.second - h2V) < 0.001 and h2 >= 0){
        m1->parents()[p.first] = h2;
      } else if(abs(p.second - h3V) < 0.001 and h3 >= 0){
        m1->parents()[p.first] = h3;
      } else if(abs(p.second - h4V) < 0.001 and h4 >= 0){
        m1->parents()[p.first] = h4;
      } else if(abs(p.second - h5V) < 0.001 and h5 >= 0){
        m1->parents()[p.first] = h5;
      } else if(p.second >= h6V and h6 >= 0){
        m1->parents()[p.first] = h6;
      }
    }

    m1->updateTriangles();
    m1->setUseParents(true);
    return true;
  }
  REGISTER_PROCESS(LabelCellsFromHeat);


// detect cell layers
  bool DetectCellLayersStack::run(Mesh *m1, Stack* s1, Store* store1, int layers, int minShared, QString exportAttr)
  {

    if(layers < 1) return setErrorMessage("Layers must be >=1");

    progressStart("Detecting Layers", 0);
    m1->parents().clear();

    const HVecUS& dataStore = store1->data();
    Point3i imgSize(s1->size());

    std::map<int, std::set<int> > neighMap;
    std::map<IntInt, int> wallMap;

    Point3i startCoord(0,0,0);
    Point3i endCoord = imgSize;

    #pragma omp parallel for schedule(guided)
    for(int x=startCoord.x(); x<endCoord.x();x++){
     for(int y=startCoord.y(); y<endCoord.y();y++){
       for(int z=startCoord.z(); z<endCoord.z();z++){
          Point3i p(x,y,z);
          int label = dataStore[s1->offset(p)];
          if(label == 0) continue;
          std::set<int> n = findNeighborsLabel(store1, imgSize, p);
          if(n.size() < 2) continue;

          forall(int l, n){
            if(l == label) continue;
            IntInt wall = std::make_pair(label, l);
            #pragma omp critical
            {
              neighMap[label].insert(l);
              wallMap[wall]++;
            }
          }
        }
      }
    }

    // detect L1
    forall(auto p, neighMap){
      int currentLabel = p.first;

      IntInt wall = std::make_pair(currentLabel, 0);
      if(wallMap.find(wall) == wallMap.end()) continue;
      if(wallMap[wall] >= minShared) m1->parents()[currentLabel] = 1;
    }

    int parentLabel = 1;

    while(layers > 1){
      layers--;
      parentLabel++;

      std::set<int> neighbors;

      // detect the next layer
      forall(auto p, neighMap){
        int currentLabel = p.first;
        if(m1->parents()[currentLabel] != parentLabel-1) continue;
        forall(int l, p.second){
          if(m1->parents()[l] > 0) continue;
          IntInt wall = std::make_pair(currentLabel, l);
          if(wallMap[wall] > minShared) m1->parents()[l] = parentLabel;
        }
      }

    }

    m1->updateTriangles();
    m1->setUseParents(true);

    if(exportAttr != ""){
      AttrMap<int, int>& layers = m1->attributes().attrMap<int, int>("Measure Label Int " + exportAttr);
      layers.clear();
      forall(auto p, m1->parents()){
        layers[p.first] = p.second;
      }
    }


    return true;
  }
  REGISTER_PROCESS(DetectCellLayersStack);



// detect cell layers
  bool NeighborVoxelsStack::run(Mesh *m1, Stack* s1, Store* store1, QString heatmap)
  {

    const HVecUS& dataStore = store1->data();
    Point3i imgSize(s1->size());

    std::map<int, std::set<int> > neighMap;
    std::map<IntInt, int> wallMap;
    std::map<int, int> outsideVoxels;

    Point3i startCoord(0,0,0);
    Point3i endCoord = imgSize;

    #pragma omp parallel for schedule(guided)
    for(int x=startCoord.x(); x<endCoord.x();x++){
     for(int y=startCoord.y(); y<endCoord.y();y++){
       for(int z=startCoord.z(); z<endCoord.z();z++){
          Point3i p(x,y,z);
          int label = dataStore[s1->offset(p)];
          if(label == 0) continue;
          std::set<int> n = findNeighborsLabel(store1, imgSize, p);

          forall(int l, n){
            if(l > 0) continue;
            #pragma omp critical
            {
              outsideVoxels[label]++;
            }
          }
        }
      }
    }


    m1->labelHeat().clear();
    forall(auto p, outsideVoxels){
      int currentLabel = p.first;
      if(currentLabel < 1) continue;
      m1->labelHeat()[currentLabel] = p.second;
    }



    m1->setShowLabel("Label Heat");
    m1->heatMapUnit() = "Voxels";
    m1->heatMapBounds() = m1->calcHeatMapBounds();
    m1->updateTriangles();

    return true;
  }
  REGISTER_PROCESS(NeighborVoxelsStack);


  bool RemoveRegressionHeatmap::run(Mesh *m, QString heatX, QString heatY)
  {

    AttrMap<int, double>& xData = m->attributes().attrMap<int, double>("Measure Label Double " + heatX);

    AttrMap<int, double>& yData = m->attributes().attrMap<int, double>("Measure Label Double " + heatY);

    if(xData.empty() or yData.empty()) return setErrorMessage("Attrmaps don't exist!");

    // assemble x and y vectors

    int n = 0;
    std::vector<double> labels;

    forall(auto p, xData){
      int label = p.first;
      if(yData.find(label) == yData.end()) continue;

      labels.push_back(label);
    }

    n = labels.size();
    if(n < 2) return setErrorMessage("Not enough data points!");

    std::cout << "linear regression with data points: " << n << std::endl;

    double x[n];
    double y[n];

    for(int i = 0; i<n; i++){
      int l = labels[i];

      x[i] = xData[l];
      y[i] = yData[l];
    }

    double c0, c1, cov00, cov01, cov11, chisq;
   
    gsl_fit_linear(x, 1, y, 1, n,
                     &c0, &c1, &cov00, &cov01, &cov11,
                     &chisq);

    std::cout << "regr: " << c0 << "/" << c1 << std::endl;

    // remove the regression line from the data
    m->labelHeat().clear();
    forall(auto p, xData){
      int label = p.first;
      if(yData.find(label) == yData.end()){
        //m->labelHeat()
      } else {
        m->labelHeat()[label] = float(yData[label] - c0 - c1 * p.second);
      }

    }

    m->setShowLabel("Label Heat");
    m->heatMapUnit() = "";
    m->heatMapBounds() = m->calcHeatMapBounds();
    m->updateTriangles();

    return true;
  }
  REGISTER_PROCESS(RemoveRegressionHeatmap);


  bool RemoveRegressionStack::run(Mesh *m, Stack* s1, Store* store1, Store* store2, QString dim, double lowT, double highT)
  {

    if(dim != "Z") return setErrorMessage("Only works with Z currently!");

    if(lowT < 0) lowT = 0;
    if(lowT > 100) return setErrorMessage("Set the Low Threshold between 0 and 100");
    if(highT > 100) highT = 100;
    if(highT < lowT) return setErrorMessage("Set the High Threshold to a value higher than the Low Threshold");

    HVecUS& data = store1->data();
    HVecUS& data2 = store2->data();
    Point3i imgSize(s1->size());

    int xDim = imgSize.x();
    int yDim = imgSize.y();
    int zDim = imgSize.z();

    int n = 0;

    int bit16_max = 65535;
    int lowT_value = lowT * bit16_max / 100;
    int highT_value = highT * bit16_max / 100;

    std::cout << "size: " << xDim * yDim * zDim << " / " << lowT_value << "/" << highT_value << std::endl;

    std::vector<double> xV, yV;




    std::cout << "2nd created! " << std::endl;

    //#pragma omp parallel for schedule(guided)
    int k = 0;
    for(int z=0; z<zDim; z++){
      for(int y=0; y<yDim; y++){
        for(int x=0; x<xDim; x++, ++k){
          if(data[k] < lowT_value or data[k] > highT_value) continue;
          xV.push_back(z);
          yV.push_back(data[k]);
          //xVec[k] = z;
          //yVec[k] = data[k];
        }
      }
    }


    double *xD = new double[xV.size()];
    double *yD = new double[xV.size()];

    for(int i=0; i<xV.size(); i++){
      xD[i] = xV[i];
      yD[i] = yV[i];
    }


    std::cout << "done " << xV.size() << std::endl;
    n = xV.size();

    if(n<2) return setErrorMessage("not enough data points, adjust thresholds");

    double c0, c1, cov00, cov01, cov11, chisq;
   
    gsl_fit_linear(xD, 1, yD, 1, n,
                     &c0, &c1, &cov00, &cov01, &cov11,
                     &chisq);

    std::cout << "regr: " << c0 << "/" << c1 << std::endl;

    k = 0;
    for(int z=0; z<zDim; z++){
      for(int y=0; y<yDim; y++){
        for(int x=0; x<xDim; x++, ++k){
          if(data[k] < lowT_value or data[k] > highT_value){
            data2[k] = data[k];
          } else {
            double newValue = data[k] - c1 * z;
            if(newValue < lowT_value) data2[k] = lowT_value;
            else if(newValue > highT_value) data2[k] = highT_value;
            else data2[k] = newValue;
          }

        }
      }
    }

    store2->changed();
    store2->copyMetaData(store1);

    return true;
  }
  REGISTER_PROCESS(RemoveRegressionStack);


  bool DeleteStackLabelsByParent::run(Mesh *m, Stack* s1, Store* store1, int parentLabel)
  {

    HVecUS& data = store1->data();
    Point3i imgSize(s1->size());

    int xDim = imgSize.x();
    int yDim = imgSize.y();
    int zDim = imgSize.z();

    int k = 0;
    for(int z=0; z<zDim; z++){
      for(int y=0; y<yDim; y++){
        for(int x=0; x<xDim; x++, ++k){
          int label = data[k];
          if(m->parents()[label] == parentLabel) data[k] = 0;
        }
      }
    }

    store1->changed();
    store1->copyMetaData(store1);

    return true;
  }
  REGISTER_PROCESS(DeleteStackLabelsByParent);

  bool DefineML::run(Mesh *m1, Mesh *m2, QString customDir, bool useSurf, double weightSurf)
  {

    if(weightSurf < 0 or weightSurf > 100) return setErrorMessage("weightSurf needs to be a number between 0.0 and 100.0");

    vvGraph& S = m1->graph();

    // find selected cells
    std::set<int> selectedCells;// = findAllSelectedLabels(S);
    std::set<int> allCells;

    forall(const vertex& v, S){
      if(v->selected) selectedCells.insert(v->label);
      allCells.insert(v->label);
    }


    // find all parent labels
    std::set<int> allParents;
    forall(auto p, m1->parents()){
      allParents.insert(p.second);
    }

    std::set<int> selectedParents;
    forall(int l, selectedCells){
      selectedParents.insert(m1->parents()[l]);
    }


    AttrMap<int, Point3d>& centroids = m1->attributes().attrMap<int, Point3d>("Measure Label Vector CellCentroids");
    AttrMap<int, SymmetricTensor>& attrData = m1->attributes().attrMap<int, SymmetricTensor>("Measure Label Tensor CustomDirections");

    if(useSurf and attrData.empty()) return setErrorMessage("No Custom Directions Found!");

    std::cout << "cusdir size " << attrData.size() << std::endl;
 
    std::set<int> newlySelected;

    // foreach: extend orthogonal to the surface from the centroids, select closest cells of other parent labels
    forall(int l, selectedCells){

      Point3d centroid = centroids[l];
      SymmetricTensor cusDir = attrData[l];
      Point3d dir(0,0,0);

      if(useSurf){
        if(customDir == "X"){
          dir = Point3d(cusDir.ev1());
        } else if(customDir == "Y"){
          dir = Point3d(cusDir.ev2());
        } else {
          dir = Point3d(cusDir.evals());
        }
      }

      std::cout << "selected " << l << "/" << centroid << "/" << dir << std::endl;


      std::map<int, double> minDisMap;
      std::map<int, int> minCellMap;
      forall(int l2, allCells){
        if(selectedParents.find(m1->parents()[l2]) != selectedParents.end()) continue; // only consider non-selected parents
        double dis = norm(centroid-centroids[l2]);
        if(useSurf){
          dis = dis*(100.0-weightSurf)/100.0*dis + weightSurf*distLinePoint(centroid, centroid-dir, centroids[l2], false);
        }
        if(minDisMap.find(m1->parents()[l2]) == minDisMap.end()){
          minDisMap[m1->parents()[l2]] = dis;
          minCellMap[m1->parents()[l2]] = l2;
        } else {
          if(dis < minDisMap[m1->parents()[l2]]){
            minDisMap[m1->parents()[l2]] = dis;
            minCellMap[m1->parents()[l2]] = l2;
          }
        }
      }



      forall(auto p, minCellMap){
        std::cout << "min " << p.first << "/" << p.second << "/" << minDisMap[p.first] << std::endl;
        newlySelected.insert(p.second);
      }
    }


    forall(const vertex& v, S){
      if(newlySelected.find(v->label) == newlySelected.end()) continue;
      v->selected = true;
    }


    m1->updateAll();

    return true;
  }
  REGISTER_PROCESS(DefineML);


  bool DefineAntPost::run(Mesh *m1, QString sep)
  {

    // sort cells by parent label
    std::set<int> parentLabels;

    forall(auto p, m1->parents()){
      parentLabels.insert(p.second);
    }


  std::map<int, double> parentSeparationMap;
  std::map<int, int> parentCounterMap;

  std::map<int, std::vector<double> > parentHeatValues;

  forall(auto p, m1->labelHeat()){
    int pLabel = m1->parents()[p.first];
    if(pLabel < 1) continue;
    parentCounterMap[pLabel]++;
    parentSeparationMap[pLabel]+=p.second;
    parentHeatValues[pLabel].push_back(p.second);
  }

  if(sep == "Mean"){
    forall(auto p, parentCounterMap){
      parentSeparationMap[p.first]/=p.second;
    }
  } else {
    forall(auto p, parentCounterMap){
      int pLabel = p.first;
      std::sort(parentHeatValues[pLabel].begin(),parentHeatValues[pLabel].end());
      double median = parentHeatValues[pLabel][parentHeatValues[pLabel].size()/2];
      if(parentHeatValues[pLabel].size() % 2 == 0){
        double median = (parentHeatValues[pLabel][parentHeatValues[pLabel].size()/2] + parentHeatValues[pLabel][parentHeatValues[pLabel].size()/2+1]) / 2;
      }

      parentSeparationMap[p.first] = median;
    }
  }



  forall(auto p, m1->labelHeat()){
    // make 2 bins for anterior / posterior based on heat
    int pLabel = m1->parents()[p.first];
    if(pLabel < 1) continue;
    // overwrite parent labels
    if(p.second > parentSeparationMap[pLabel]){
      m1->parents()[p.first] = pLabel*10+1;
    } else {
      m1->parents()[p.first] = pLabel*10+2;
    }
    
  }

  m1->updateTriangles();

  return true;
  }
  REGISTER_PROCESS(DefineAntPost);


  std::vector<std::vector<Point3d> > calcBezDifGrid(std::vector<std::vector<Point3d> >& bezGrid, bool calcU)
  {
    int sizeGrid = bezGrid.size();
    std::vector<std::vector<Point3d> > bezDifGrid(sizeGrid);

    for(int i = 0; i < sizeGrid; i++){
      bezDifGrid[i].resize(sizeGrid);
      for(int j = 0; j < sizeGrid; j++){
        Point3d difBez(0,0,0);
        if(calcU){
          if(i == 0){
            difBez = bezGrid[i+1][j] - bezGrid[i][j];
          } else if(i == sizeGrid-1){
            difBez = bezGrid[i][j] - bezGrid[i-1][j];
          } else {
            difBez = bezGrid[i+1][j] - bezGrid[i-1][j];
          }
        } else {
          if(j == 0){
            difBez = bezGrid[i][j+1] - bezGrid[i][j];
          } else if(j == sizeGrid-1){
            difBez = bezGrid[i][j] - bezGrid[i][j-1];
          } else {
            difBez = bezGrid[i][j+1] - bezGrid[i][j-1];
          }
        }
        difBez /= norm(difBez);
        bezDifGrid[i][j] = difBez;
        
      }
    }
    
    


    return bezDifGrid;
  }


  bool SelectCellCutByBezier::run(const Stack *s1, Mesh *m1)
  {

    // make label Tri map
    vvGraph& S = m1->graph();
    std::map<int, triVector> cellTriangles;
    generateLabelTriangleMap(S, cellTriangles);

    std::map<int, std::set<Point3d> > cellPts;

    CuttingSurface* cutSurf = cuttingSurface();
    Matrix4d rotMatrixS1, rotMatrixCS;

    s1->getFrame().getMatrix(rotMatrixS1.data());

    cutSurf->frame().getMatrix(rotMatrixCS.data());

    Matrix4d mGLTot = transpose(inverse(rotMatrixS1)) * transpose(rotMatrixCS);

    std::vector<std::vector<Point3d> > bezGrid;
    Bezier b = cutSurf->bezier();
    int dataPointsBezier = 100;

    b.discretizeGrid(dataPointsBezier, mGLTot, bezGrid);

    std::vector<std::vector<Point3d> > bezDifGridU = calcBezDifGrid(bezGrid, true);
    std::vector<std::vector<Point3d> > bezDifGridV = calcBezDifGrid(bezGrid, false);

    // check all vtx, find nearest Bez Point, check on which side of Bezier they are on
    forall(auto p, cellTriangles){
      forall(Triangle t, p.second){
        Point3d tAvg = (t.v[0]->pos + t.v[1]->pos + t.v[2]->pos)/3.;
        cellPts[p.first].insert(tAvg);
      }
    }

    std::set<int> cutCells;

    std::vector<int> labelVec;

    forall(auto p, cellPts){
      labelVec.push_back(p.first);
    }

    #pragma omp parallel for
    for(int i = 0; i < labelVec.size(); i++){
      bool above = false;
      bool below = false;
      forall(Point3d tAvg, cellPts[labelVec[i]]){
        Point2i bezIdx;
        Point3d bezP = calcNearestPointOnBezierGrid(tAvg, bezGrid, bezIdx);

        Point3d bezNrml = bezDifGridU[bezIdx.x()][bezIdx.y()] % bezDifGridV[bezIdx.x()][bezIdx.y()];
        double scalarP = ((tAvg - bezP)/norm(tAvg - bezP)) * bezNrml;

        if(scalarP > 0) above = true;
        else below = true;
      }
      if(above and below){
        #pragma omp critical
        {
          cutCells.insert(labelVec[i]);
        }
      }
      
    }

    // if a cell has above and below: select it
    forall(vertex v, S){
      if(cutCells.find(v->label) != cutCells.end()){
        v->selected = true;
      } else {
        v->selected = false;
      }
    }

    m1->updateAll();


    return true;
  }
  REGISTER_PROCESS(SelectCellCutByBezier);





}
