//
// This file is part of 3DCellAtlas. 
// Copyright (C) 2015 George W. Bassel and collaborators.
//
// If you use 3DCellAtlas in your work, please cite:
//   http://dx.doi.org/10.1105/tpc.15.00175
//
// 3DCellAtlas is an AddOn for MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// 3DCellAtlas and MorphoGraphX are 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 "RootCellAnalyzing.hpp"

#include <Information.hpp>

#include <Misc.hpp>
#include <Subdivide.hpp>
#include <Attributes.hpp>

#include <CellAtlasUtils.hpp>

namespace mgx
{



// generates a map label -> triangles
bool RootCellAnalyzing::generateMeshTriangleVector(const vvGraph& S, triVector& triVec)
{
  // now run
  forall(const vertex &v, S){
    forall(const vertex &n, S.neighbors(v)){
      if(!progressAdvance()) continue;
      const vertex& m = S.nextTo(v,n);
      if(S.uniqueTri(v, n, m)){
        Triangle currentTriangle(v,n,m);
        //currentTriangle[0] = v;
        //currentTriangle[1] = n;
        //currentTriangle[2] = m;
        triVec.push_back(currentTriangle);
      }
    }
  }
  if(!progressAdvance()) return false;

  return true;
  }


// generates a map label -> triangles
// 
bool RootCellAnalyzing::generateLabelTriangleMap(int numCells, std::vector<int>& uniqueLabels, const vvGraph& segmentedMesh, 
  labelVertexMap& lvMap, std::map<int, triVector>& cellTriangles)
{
 // Progress progress("Analyze Cells 3D - Generate Label/Triangle Map", 0);
  progressStart("Analyze Cells 3D - Generate Label/Triangle Map", 0);

  // init
  for(int i=0; i<numCells; i++){
    triVector t;
    int currentLabel = uniqueLabels[i];
    cellTriangles[currentLabel] = t;
  }

  // now run
  ////#pragma omp parallel for
  for(int i=0; i<numCells; i++){
    int currentLabel = uniqueLabels[i];
    forall(const vertex &v, lvMap[currentLabel]){
      forall(const vertex &n, segmentedMesh.neighbors(v)){
        if(!progressAdvance()) continue;
        //if(v->label == n->label){
          const vertex& m = segmentedMesh.nextTo(v,n);
          ////#pragma omp critical
          ////{
            if(segmentedMesh.uniqueTri(v, n, m)){
            //if(m->label == n->label){
              Triangle currentTriangle(v,n,m);
              //currentTriangle[0] = v;
              //currentTriangle[1] = n;
              //currentTriangle[2] = m;
              
              cellTriangles[v->label].push_back(currentTriangle);
              //triangleCellLabelMap[currentTriangle].insert(v->label);
            //}
            ////}
          }
        //}
      }
    }
  }
  if(!progressAdvance()) return false;

  return true;
  }


 bool RootCellAnalyzing::analyzeCellCentroidsVolumes(std::map<int, triVector>& cellTriangles, RootCellProcessing& rcp, double minVolume)
  {

  int numCells = rcp.rootData.uniqueLabels.size();

  std::map<int, Point3d>& cellCentroids = rcp.rootData.cellCentroids;
  std::map<int, Point3d>& signalCentroids = rcp.rootData.signalCentroids;
  std::map<int,double>& cellVolumes = rcp.rootData.cellVolumes;
  std::map<int, bool>& cellBad = rcp.rootData.cellBad;

  // calculate cell centroids and cell volumes
  Information::out << "*** The following cells will be ignored due to small volumes. label: ";
  progressStart("Analyze Cells 3D - Calculate cell centroids and cell volumes", 0);
  for(int i=0; i<numCells; i++){
    int currentLabel = rcp.rootData.uniqueLabels[i];
    cellVolumes[currentLabel] = 0;
    cellCentroids[currentLabel] = 0;
    signalCentroids[currentLabel] = 0;
    double signalSum = 0;
    forall(const Triangle &t, cellTriangles[currentLabel]){

      // Compute x component of center of mass
      float volume = signedTetraVolume(t.v[0]->pos, t.v[1]->pos, t.v[2]->pos);
      cellCentroids[currentLabel] +=volume * (t.v[0]->pos + t.v[1]->pos + t.v[2]->pos) / 4.0;
      double signalFac = triangleArea(t.v[0]->pos,t.v[1]->pos,t.v[2]->pos) * (t.v[0]->signal + t.v[1]->signal + t.v[2]->signal) / 3.0;
      signalSum+=signalFac;
      signalCentroids[currentLabel] += signalFac * (t.v[0]->pos + t.v[1]->pos + t.v[2]->pos) / 3.0;
      cellVolumes[currentLabel] += volume;
    }
    // Find cell center
    if(cellVolumes[currentLabel] > minVolume) {
      cellCentroids[currentLabel] /= cellVolumes[currentLabel];
      signalCentroids[currentLabel] /= signalSum;
      cellBad[currentLabel] = false;
    } else {
      cellBad[currentLabel] = true;
      Information::out << currentLabel << ", ";
      cellVolumes[currentLabel] = 0;
      cellCentroids[currentLabel] = Point3d(0,0,0);
    }
  }
  Information::out << "." << endl;

  // rcp.rootData.cellCentroids = cellCentroids;
  // rcp.rootData.signalCentroids = signalCentroids;
  // rcp.rootData.cellVolumes = cellVolumes;
  // rcp.rootData.cellBad = cellBad;

  return true;
  }


 bool RootCellAnalyzing::analyzeCellCentroids2D(std::map<int, triVector>& cellTriangles, RootCellProcessing& rcp)
  {

  int numCells = rcp.rootData.uniqueLabels.size();

  std::map<int, Point3d>& cellCentroids = rcp.rootData.cellCentroids;
  std::map<int,double>& cellVolumes = rcp.rootData.cellVolumes;
  std::map<int, bool>& cellBad = rcp.rootData.cellBad;

  // calculate cell centroids and cell volumes
  progressStart("Analyze Cells 3D - Calculate cell centroids (2D)", 0);
  for(int i=0; i<numCells; i++){
    int currentLabel = rcp.rootData.uniqueLabels[i];
    cellVolumes[currentLabel] = 0;
    cellCentroids[currentLabel] = 0;
    forall(const Triangle &t, cellTriangles[currentLabel]){

      // Compute x component of center of mass
      float volume = triangleArea(t.v[0]->pos,t.v[1]->pos,t.v[2]->pos);
      Point3d center = (t.v[0]->pos + t.v[1]->pos + t.v[2]->pos)/3.0;
      cellCentroids[currentLabel] +=volume*center;
      cellVolumes[currentLabel] += volume;
    }
    // Find cell center
    if(cellVolumes[currentLabel] > 0) {
      cellCentroids[currentLabel] /= cellVolumes[currentLabel];
      cellBad[currentLabel] = false;
    } else {
      cellBad[currentLabel] = true;
      cellVolumes[currentLabel] = 0;
      cellCentroids[currentLabel] = Point3d(0,0,0);
    }
  }

  // rcp.rootData.cellCentroids = cellCentroids;
  // rcp.rootData.cellVolumes = cellVolumes;
  // rcp.rootData.cellBad = cellBad;

  return true;
  }

 bool RootCellAnalyzing::analyzeCellCalcDisSurface(RootCellProcessing& rcp, const vvGraph& surfaceMesh, bool checkArc, std::map<int, double>& surfaceArclengths, 
  std::map<int, double>& surfaceRadialDis, std::map<int, Point3d>& bMap, bool considerOrientation)
  {
  int numCells = rcp.rootData.uniqueLabels.size();

  for(int i=0; i<numCells; i++){
    int currentLabel = rcp.rootData.uniqueLabels[i];
    Point3d p (0,0,0);
    rcp.rootData.radialDis[currentLabel] = 0;
    rcp.rootData.scaledRadialDis[currentLabel] = 0;
    rcp.rootData.dirRad[currentLabel] = p;
    rcp.rootData.nearestSurfacePoint[currentLabel] = p;
  }

  progressStart("Analyze Cells 3D - Calculate distance of each cell to the surface mesh", 0);
  ////#pragma omp parallel for
  for(int i=0; i<numCells; i++){
    if(!progressAdvance()) continue;
    int currentLabel = rcp.rootData.uniqueLabels[i];
    Point3d currentCentroid = rcp.rootData.cellCentroids[currentLabel];
    double minDis = 1E20;
    Point3d minPos, minNrml;
    int minId;
    forall(const vertex &v, surfaceMesh){
      //surfDisVec.push_back(norm(currentCentroid-v->pos));
      double epsilon = 0.015*10;
      double arcDif = 0;
      if(checkArc) arcDif = surfaceArclengths[v->saveId] - rcp.rootData.arclengths[currentLabel];
      if(arcDif < 0) arcDif = -arcDif;
      if(arcDif < epsilon) {
        double dis = norm(currentCentroid-v->pos);
        if(dis < minDis){
          minDis = dis;
          minPos = v->pos;
          minId = v->saveId;
          minNrml = v->nrml;
        }
      }
    }
    if(minDis == 1E20){
      cout << "Warning: Radial of cell " << currentLabel << " set to 0 (could not calculate distance to the surface mesh)" << endl; 
      

      int index;
      double weight;
      Point3d closestPoint = calcNearestPointOnBezierLine(currentCentroid, bMap, index, weight);

      Point3d closestVec = currentCentroid - closestPoint;

      rcp.rootData.radialDis[currentLabel] = norm(closestVec);
      rcp.rootData.scaledRadialDis[currentLabel] = 0;

      rcp.rootData.dirRad[currentLabel] = closestVec / rcp.rootData.radialDis[currentLabel];
      rcp.rootData.nearestSurfacePoint[currentLabel] = currentCentroid;
    } else {
      rcp.rootData.nearestSurfacePoint[currentLabel] = minPos;
      if(considerOrientation){
        if((minPos - currentCentroid) * minNrml < 0) rcp.rootData.nearestSurfacePoint[currentLabel] *=-1;
      }
      if(checkArc){
        rcp.rootData.radialDis[currentLabel] = std::max(0.0,surfaceRadialDis[minId] - minDis); // norm(currentCentroid - bezInterp[currentLabel]);// 
        rcp.rootData.scaledRadialDis[currentLabel] = rcp.rootData.radialDis[currentLabel]/surfaceRadialDis[minId];
      } else {
        rcp.rootData.radialDis[currentLabel] = minDis;
        rcp.rootData.scaledRadialDis[currentLabel] = minDis;
      }

      // generate rad coord system
      rcp.rootData.dirRad[currentLabel] = (minPos - currentCentroid)/minDis;
    }
  }
  if(!progressAdvance()) return false;

  return true;
  }

  // calculate the distance of all the cells to the bezier grid
  void RootCellAnalyzing::analyzeBezierGrid(std::vector<int>& uniqueLabels, std::map<int,Point3d>& cellCentroids,
   std::vector<std::vector<Point3d> >& bezier, std::map<int,Point3d>& nearestPoints, std::map<int,Point2i>& nearestIndex)
  {
  int numCells = uniqueLabels.size();
  //int bezSize = bMap.size();

  // calculate arclengths by finding the shortest distance to the bezier line for each centroid
  progressStart("Analyze Cells 3D - Calculate Longitudinal Coordinates", 0);
  //#pragma omp parallel for
  for(int i=0; i<numCells; i++){
    int currentLabel = uniqueLabels[i];
    Point2i bezCoord;
    Point3d closestPoint = calcNearestPointOnBezierGrid(cellCentroids[currentLabel], bezier, bezCoord);

    arclengths[currentLabel] = bezCoord[0];//(double)(index-1+weight)/(double)bezSize;
    azimCoord[currentLabel] = bezCoord[1];//(double)(index-1+weight)/(double)bezSize;
    //diffBezInterp[currentLabel] = (1-weight) * diffbMap[index-1] + weight * diffbMap[index];
    nearestPoints[currentLabel] = closestPoint;
    nearestIndex[currentLabel] = bezCoord;
    //if(arclengths[currentLabel] < 0.01)
    //cout << "cell " << currentLabel << "/" << arclengths[currentLabel] << "/" << bezInterp[currentLabel] << "/" << index << "/" << weight << endl;
  }

  }

  // calculate the distance of all the cells to the bezier grid
  void RootCellAnalyzing::analyzeSurface(std::vector<int>& uniqueLabels, std::map<int,Point3d>& cellCentroids, const vvGraph& surfaceMesh,
    std::map<int,Point3d>& nearestSurfacePoints)
  {
  int numCells = uniqueLabels.size();
  //int bezSize = bMap.size();

  // calculate arclengths by finding the shortest distance to the bezier line for each centroid
  progressStart("Analyze Cells 3D - Calculate Longitudinal Coordinates", 0);
  //#pragma omp parallel for
  for(int i=0; i<numCells; i++){
    int currentLabel = uniqueLabels[i];
    nearestSurfacePoints[currentLabel] = nearestPointOnMesh(cellCentroids[currentLabel], surfaceMesh);
  }

  }

  // calculate the distance of all the cells to the bezier line
  void RootCellAnalyzing::analyzeBezierLine(std::vector<int>& uniqueLabels, std::map<int,Point3d>& cellCentroids, std::map<int, Point3d>& bMap, std::map<int, Point3d>& diffbMap, bool flipBezier)
  {
  int numCells = uniqueLabels.size();
  int bezSize = bMap.size();

  double totalBezLength = 0.;

  for(int j=1; j<bezSize; j++){
    totalBezLength += norm(bMap[j]-bMap[j-1]);
  }

  // calculate arclengths by finding the shortest distance to the bezier line for each centroid
  progressStart("Analyze Cells 3D - Calculate Longitudinal Coordinates", 0);
  for(int i=0; i<numCells; i++){
    int currentLabel = uniqueLabels[i];
    double weight;
    int index;
    Point3d closestPoint = calcNearestPointOnBezierLine(cellCentroids[currentLabel], bMap, index, weight);

    arclengths[currentLabel] = (double)(index-1+weight)/(double)bezSize;

    arclengthsUM[currentLabel] = arclengths[currentLabel]*totalBezLength;
    diffBezInterp[currentLabel] = (1-weight) * diffbMap[index-1] + weight * diffbMap[index];
    bezInterp[currentLabel] = closestPoint;

    if(flipBezier){
      arclengths[currentLabel] = 1 - arclengths[currentLabel];
      arclengthsUM[currentLabel] = totalBezLength - arclengthsUM[currentLabel];
    }
    //if(arclengths[currentLabel] < 0.01)
    //std::cout << "bez " << currentLabel << "/" << totalBezLength << "/" << arclengths[currentLabel] << "/" << arclengthsUM[currentLabel] << std::endl;
    //cout << "cell " << currentLabel << "/" << arclengths[currentLabel] << "/" << bezInterp[currentLabel] << "/" << index << "/" << weight << endl;
  }

  }

  // calculate the distance of all the cells to the bezier line
  void RootCellAnalyzing::minMaxToBezierPerCell(vvGraph& S, std::vector<int>& uniqueLabels, std::map<int, Point3d>& bMap, bool flipBezier)
  {
  int numCells = uniqueLabels.size();
  int bezSize = bMap.size();

  double totalBezLength = 0.;

  for(int j=1; j<bezSize; j++){
    totalBezLength += norm(bMap[j]-bMap[j-1]);
  }

  for(int i=0; i<numCells; i++){
    int currentLabel = uniqueLabels[i];
    arclengthsMinUM[currentLabel] = 1E20;
    arclengthsMaxUM[currentLabel] = -1E20;
  }

  // calculate arclengths by finding the shortest distance to the bezier line for each centroid
  //progressStart("Analyze Cells 3D - Calculate Longitudinal Coordinates", 0);

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

    double weight;
    int index;

    Point3d closestPoint = calcNearestPointOnBezierLine(v->pos, bMap, index, weight);

    double a = (double)(index-1+weight)/(double)bezSize * totalBezLength;

    if(flipBezier){
      a = a - totalBezLength;
    }

    forall(const int l, currentLabels){
      if(a < arclengthsMinUM[l]) arclengthsMinUM[l] = a;
      if(a > arclengthsMaxUM[l]) arclengthsMaxUM[l] = a;
    }
    
  }

  }

  // calculates a "simple" radial coordinate by calculating the distance of each cell to the bezier
  // requires the bezInterp map of analyzeBezierLine
  void RootCellAnalyzing::calcSimpleRadial(std::vector<int>& uniqueLabels, RootCellProcessing& rcp, std::map<int, Point3d>& cellCentroids)
  {
  int numCells = uniqueLabels.size();
  for(int i=0; i<numCells; i++){
    int currentLabel = uniqueLabels[i];
    Point3d currentCentroid = cellCentroids[currentLabel];
    rcp.rootData.radialDis[currentLabel] = norm(currentCentroid - rcp.rootData.bezInterp[currentLabel]);
  }

  }


  // creates the surfaceMap which maps a pair of long and circ coords to the distance to the surface
  void RootCellAnalyzing::generateSurfaceMap(primDataStructure& p1)
  {
    std::map<int, Point3d> bMap, diffbMap, bMapRadRef;
    triVector triVec;

    bMap = p1.bez;
    triVec = p1.meshTris;
    diffbMap = p1.diffBez;
    bMapRadRef = p1.bMapRadRef;

    typedef std::pair<int, Point3d> IntP3d;
    std::map<std::pair<int,int>, double> sMap;

    for(uint j=0; j<bMap.size(); j++){
      for(uint i=0; i<radRes; i++){
        std::pair<int,int> mapIdx = std::make_pair(j,i);
        sMap[mapIdx] = HUGE_VAL;
      }
    }

    std::cout << "b " << bMap.size() << std::endl;

    Point3d dirLongRef = diffbMap[0];
    Point3d dirRadRef = orthogonalVector(dirLongRef);//bMapRadRef[j];

    //forall(const IntP3d& p, bMap){
    #pragma omp parallel for
    for(uint j=0; j<bMap.size(); j++){
    //std::cout << "bb " << j << std::endl;
      //IntP3d p;
      //p.first = j;//it->first;
      //p.second = bMap[j];//it->second;

      dirLongRef = diffbMap[j];

      for(int i=0; i<radRes; i++){
        double radiant = 2*M_PI*(double)i/(double)radRes;

        Point3d standardDir = inPrimCoord(radiant, dirRadRef, dirLongRef); // correct rotation
        //std::cout << "surfaceMap " << j << "/" << bMap[j] << "//" << i << "/" << radiant << "/" << standardDir << std::endl;

        int stopCounter = 0;
        Point3d radMeshPoint;
        double dis = HUGE_VAL;
        //forall(tri t, triVec){
        //  Point3d tPos = (t[0]->pos + t[1]->pos + t[2]->pos)/3.;
        //  double d = norm(tPos - )
        //  if(
        //}
        if(findIntersectPoint(bMap[j], standardDir, triVec, stopCounter, radMeshPoint)){
          dis = norm(radMeshPoint-bMap[j]);
        }
        std::pair<int,int> mapIdx = std::make_pair(j,i);
        //std::cout << "surfaceMap2 " << radMeshPoint << "/" << dis << std::endl;
        
        // write the map
        #pragma omp critical
        sMap[mapIdx] = dis;
        }
      }

      surfaceMap = sMap;

  }

  Point3d RootCellAnalyzing::organToCartesianCoords(Point3d organCoords, primDataStructure& p1)
  {

    triVector& meshTris = p1.meshTris;
    std::map<int, Point3d>& bMap = p1.bez;
    std::map<int, Point3d>& diffbMap = p1.diffBez;
    std::map<int, Point3d>& bMapRadRef = p1.bMapRadRef;

    double weight = organCoords.x()*bMap.size() + 1;
    int index = (int)weight;
    weight -= index;

    Point3d dirRadRef = (1-weight) * bMapRadRef[index-1] + weight * bMapRadRef[index];
    Point3d dirLongRef = (1-weight) * diffbMap[index-1] + weight * diffbMap[index];
    Point3d bezPoint = (1-weight) * bMap[index-1] + weight * bMap[index];

    //cout << "bez " << weight << "/" << index << "/" << bMap[index-1] << "/" << bMap[index] << endl;
    
    Point3d standardVecX (1,0,0);
    Point3d standardVecZ (0,0,1);
    Point3d standardDir (cos(organCoords.z()), sin(organCoords.z()),0);

    Matrix3d rotMatrix1 = calcRotMatrix(standardVecZ,dirLongRef);
    Matrix3d rotMatrix2 = calcRotMatrix(standardVecX,rotMatrix1*dirRadRef);

    //cout << "refso " << dirRadRef << "/" << dirLongRef << "/" << rotMatrix1*dirRadRef << endl;

    standardDir = -standardDir * rotMatrix2 * rotMatrix1;
    //cout << "dirsssss " << Point3d(cos(organCoords.z()), sin(organCoords.z()),0) << "/" << standardDir << endl;
    int stopCounter = 0;

    Point3d radMeshPoint;
    double radial = 0;
    if(findIntersectPoint(bezPoint, standardDir, meshTris, stopCounter, radMeshPoint)){
      radial = organCoords.y();//*norm(radMeshPoint - bezPoint);
      //cout << "hit!!! " << bezPoint<< "/" << standardDir << "/" << radMeshPoint << endl;
      return bezPoint + standardDir * radial;
    } else {
      //cout << "no hit " << bezPoint<< "/" << standardDir << "/" << endl;
      return Point3d(0,0,0);
    }

  }

  Point3d RootCellAnalyzing::organToCartesianCoordsWithMap(Point3d organCoords, primDataStructure& p1)//triVector& meshTris, std::map<int, Point3d>& bMap, 
   // std::map<int, Point3d>& diffbMap, std::map<int, Point3d>& bMapRadRef)
  {

    //triVector& meshTris = p1.meshTris;
    std::map<int, Point3d>& bMap = p1.bez;
    std::map<int, Point3d>& diffbMap = p1.diffBez;
    std::map<int, Point3d>& bMapRadRef = p1.bMapRadRef;

    double weight = organCoords.x()*bMap.size() + 1;
    int index = (int)weight;
    weight -= index;
    //weight = 0;

    Point3d dirRadRef = (1-weight) * bMapRadRef[index-1] + weight * bMapRadRef[index];
    Point3d dirLongRef = (1-weight) * diffbMap[index-1] + weight * diffbMap[index];
    Point3d bezPoint = (1-weight) * bMap[index-1] + weight * bMap[index];
/*
    double sN;
    Point3d radOrth;
    planeLineIntersect(bezPoint, dirLongRef, dirRadRef, dirRadRef-dirLongRef, sN, radOrth);

    dirRadRef = radOrth - bezPoint;
    dirRadRef/=norm(dirRadRef);
*/
    double radiant = organCoords.z()*2.*M_PI/(double)radRes;

    //standardDir = -standardDir * rotMatrix2 * rotMatrix1;
    //standardDir = standardDir*rotM;
    Point3d standardDir = inPrimCoord(radiant, dirRadRef, dirLongRef);
    //cout << "refso " << dirRadRef << "/" << dirLongRef << "/" << standardDir << "/" << index << "/" << organCoords.z() << endl;
    //int stopCounter = 0;

    //Point3d radMeshPoint;
    double radial = 0;
    std::pair<int,int> pairInt = std::make_pair(index, organCoords.z());
    if(surfaceMap[pairInt] != HUGE_VAL and organCoords.y()!=-1 and organCoords.y()<1.2){
      radial = organCoords.y() * surfaceMap[pairInt];//*norm(radMeshPoint - bezPoint);
      //cout << "hit!!! " << bezPoint<< "/" << standardDir << "/" << radMeshPoint << endl;
      return bezPoint + standardDir * radial;
    } else {
      //cout << "no hit " << bezPoint<< "/" << standardDir << "/" << endl;
      return Point3d(0,0,0);
    }

  }

  Point3d RootCellAnalyzing::cartesianToOrganCoords(Point3d cartesianCoords, primDataStructure p1)//std::map<int, Point3d>& bMap, std::map<int, Point3d>& diffbMap, std::map<int, Point3d>& bMapRadRef)
  {
    //triVector& meshTris = p1.meshTris;
    std::map<int, Point3d>& bMap = p1.bez;
    std::map<int, Point3d>& diffbMap = p1.diffBez;
    std::map<int, Point3d>& bMapRadRef = p1.bMapRadRef;


    Point3d result (0,0,0);

    int index = 0;
    double weight = 0.;
    Point3d closestPoint;// = calcNearestPointOnBezierLine(cartesianCoords, bMap, index, weight);
    //weight = 0;
    
    // longitudinal
    result.x() = cartesianToOrganCoordsLong(cartesianCoords, bMap, closestPoint); //(double)(index-1+weight)/(double)bMap.size();

    // radial
    result.y() = norm(closestPoint - cartesianCoords);

    //Point3d dirRadRef = (1-weight) * bMapRadRef[index-1] + weight * bMapRadRef[index];
    Point3d dirRadRef = (1-weight) * bMap[index-1] + weight * bMap[index];
    Point3d dirLongRef = (1-weight) * diffbMap[index-1] + weight * diffbMap[index];
    // circ
/*
    double sN;
    Point3d radOrth;
    planeLineIntersect(closestPoint, dirLongRef, dirRadRef, dirRadRef-dirLongRef, sN, radOrth);

    dirRadRef = radOrth - closestPoint;
    dirRadRef/=norm(dirRadRef);
*/
    result.z() = (int)(calcCircumferential(dirRadRef, dirLongRef, cartesianCoords, closestPoint)*radRes/2./M_PI);
    std::pair<int,int> pairInt = std::make_pair(index, result.z());

    //cout << "refsc " << dirRadRef << "/" << dirLongRef << "/" << (cartesianCoords-closestPoint)/result.y() << "/" << index << "/" << result.z() << endl;

// surfacemaps currently doesnt work properly!
    /*if(surfaceMap[pairInt] != HUGE_VAL and surfaceMap[pairInt]> 0){
      result.y() = result.y()/surfaceMap[pairInt];
    } else {
      result.y() = -1;
    }*/

    return result;
  }


  // creates the necessary data structures for a radial symmetric coord system along the bezier
  // for leaf prim 
  // calculates organ coords for all given points
  void RootCellAnalyzing::createCoordSystem(std::vector<Point3d>& points, RootCellProcessing& rcp, std::map<int, Point3d>& bMap,
                                            std::map<int, Point3d>& diffbMap, std::map<int, Point3d>& bMapRadRef)
  {

   // for each vtx:
  for(std::vector<Point3d>::size_type i=0; i<points.size(); i++){

    Point3d p = points[i];
    int index;
    double weight;
    Point3d closestPoint = calcNearestPointOnBezierLine(p, bMap, index, weight);

    // radial
    rcp.rootData.radialDis[i] = norm(closestPoint - p);
    // longitudinal
    rcp.rootData.arclengths[i] = (double)(index-1+weight)/(double)bMap.size();

    rcp.rootData.diffBezInterp[i] = (1-weight) * diffbMap[index-1] + weight * diffbMap[index];

    Point3d dirRadRef = (1-weight) * bMapRadRef[index-1] + weight * bMapRadRef[index];
    Point3d dirLongRef = rcp.rootData.diffBezInterp[i];
    // circ
    rcp.rootData.azimuthal[i] = calcCircumferential(dirRadRef, dirLongRef, p, closestPoint);
  }

  }

// calc circumferential coord with the same reference directions for all cells
 void RootCellAnalyzing::calcCircumferentialEqual(std::vector<int>& uniqueLabels, std::map<int, Point3d>& cellCentroids)
  {

  int numCells = uniqueLabels.size();

  progressStart("Analyze Cells 3D - Calculate Circumferential Angles", 0);
  for(int i=0; i<numCells; i++){
    int currentLabel = uniqueLabels[i];
    azimCoord[currentLabel] = calcCircumferential(firstRad, firstLong, cellCentroids[currentLabel], bezInterp[currentLabel]);
  }
  }

  void RootCellAnalyzing::calcCoordSystemCartesian(std::vector<int>& uniqueLabels)
  {

  for(uint i=0; i<uniqueLabels.size(); i++){
    // interpolate diffBez in arclengths points for all 3 dimensions
    int currentLabel = uniqueLabels[i];

    dirLong[currentLabel] = Point3d(1,0,0);
    dirCirc[currentLabel] = Point3d(0,1,0);
    dirRad[currentLabel] = Point3d(0,0,1);
  }


  }

  void RootCellAnalyzing::calcCoordSystem(std::vector<int>& uniqueLabels, std::map<int,Point3d>& diffBezInterp, std::map<int,Point3d>& dirRad, std::map<int,Point3d>& dirLong, std::map<int,Point3d>& dirCirc)
  {

  int numCells = uniqueLabels.size();

  for(int i=0; i<numCells; i++){
    // interpolate diffBez in arclengths points for all 3 dimensions
    int currentLabel = uniqueLabels[i];

    dirLong[currentLabel] = diffBezInterp[currentLabel]/norm(diffBezInterp[currentLabel]);
    dirCirc[currentLabel] = dirLong[currentLabel] ^ dirRad[currentLabel];
    dirLong[currentLabel] = - dirCirc[currentLabel] ^ dirRad[currentLabel];

    dirLong[currentLabel]/=norm(dirLong[currentLabel]);
    dirCirc[currentLabel]/=norm(dirCirc[currentLabel]);
    dirRad[currentLabel]/=norm(dirRad[currentLabel]);
  }


  }

bool RootCellAnalyzing::analyzeCells(std::vector<int>& uniqueLabels, const vvGraph& segmentedMesh, const vvGraph& surfaceMesh, std::map<int, Point3d>& bMap, 
  std::map<int, Point3d>& diffbMap, labelVertexMap& lvMap, RootCellProcessing& rcp, int firstLabel, double minVolume)
 {
  return true;
 }

bool RootCellAnalyzing::createCartesianCoordSystem(RootCellProcessing& rcp)
{

  uniqueLabels = rcp.rootData.uniqueLabels;

  for(uint i=0; i<uniqueLabels.size(); i++){
    // interpolate diffBez in arclengths points for all 3 dimensions
    int currentLabel = uniqueLabels[i];

    arclengths[currentLabel] = cellCentroids[currentLabel].x();
    azimCoord[currentLabel] = cellCentroids[currentLabel].y();
    radialDis[currentLabel] = cellCentroids[currentLabel].z();
  }
  

  progressStart("Analyze Cells 3D - Generate cell coord system", 0);
  calcCoordSystemCartesian(uniqueLabels);

  rcp.rootData.radialDis = radialDis;
  rcp.rootData.dirLong = dirLong;
  rcp.rootData.dirCirc = dirCirc;
  rcp.rootData.dirRad = dirRad;

  return true;
}


bool RootCellAnalyzing::createLayerCoordSystem(const vvGraph& surfaceMesh, std::vector<std::vector<Point3d> >& bezier, RootCellProcessing& rcp)
{

  std::map<int,Point3d> nearestBezPoints;
  std::map<int,Point2i> nearestIndex;
std::cout << "grid" << std::endl;
  // closest points to bezier grid and surface -> dirRad = surface - bezier
  analyzeBezierGrid(uniqueLabels, cellCentroids, bezier, nearestBezPoints, nearestIndex);
  
  std::map<int,Point3d> nearestSurfacePoints;
std::cout << "surface" << std::endl;
  analyzeSurface(uniqueLabels, cellCentroids, surfaceMesh, nearestSurfacePoints);
std::cout << "done" << std::endl;
  for(uint i=0; i<uniqueLabels.size(); i++){
    int currentLabel = uniqueLabels[i];
    scaledRadialDis[currentLabel] = norm(nearestBezPoints[currentLabel] - cellCentroids[currentLabel])/norm(nearestSurfacePoints[currentLabel] - nearestBezPoints[currentLabel]);
    radialDis[currentLabel] = norm(nearestBezPoints[currentLabel] - cellCentroids[currentLabel]);
    //lengthLong[currentLabel] = norm(nearestSurfacePoints[currentLabel] - cellCentroids[currentLabel]);
    //lengthRad[currentLabel] = norm(nearestBezPoints[currentLabel] - cellCentroids[currentLabel]);
    dirRad[currentLabel] = scaledRadialDis[currentLabel] * (nearestSurfacePoints[currentLabel] - cellCentroids[currentLabel]) +
                           (1-scaledRadialDis[currentLabel]) * (cellCentroids[currentLabel] - nearestBezPoints[currentLabel]);
    dirRad[currentLabel]/=norm(dirRad[currentLabel]);

    Point3d diffU, diffV;
    differentialOfBezierGrid(bezier, nearestIndex[currentLabel], diffU, diffV);

    dirLong[currentLabel] = diffU;
    dirCirc[currentLabel] = dirLong[currentLabel] ^ dirRad[currentLabel];
    dirCirc[currentLabel]/=norm(dirCirc[currentLabel]);
    dirLong[currentLabel] = - dirCirc[currentLabel] ^ dirRad[currentLabel];

    
    dirLong[currentLabel]/=norm(dirLong[currentLabel]);

  }


  rcp.rootData.radialDis = radialDis;
  rcp.rootData.scaledRadialDis = scaledRadialDis;
  
  return true;
}


bool RootCellAnalyzing::createRootCoordSystem(const vvGraph& surfaceMesh, std::map<int, Point3d>& bMap, 
  std::map<int, Point3d>& diffbMap, RootCellProcessing& rcp)
{

  // calculate arclengths by finding the shortest distance to the bezier line for each centroid
  analyzeBezierLine(uniqueLabels, cellCentroids, bMap, diffbMap);

  Point3d firstCentroid = cellCentroids[firstLabel];
  Point3d firstBez = bezInterp[firstLabel];
  firstRad = (firstCentroid - firstBez)/norm(firstCentroid - firstBez);
  firstLong = diffBezInterp[firstLabel]/norm(diffBezInterp[firstLabel]);

  Point3d planeNormal = firstLong ^ firstRad;

  calcCircumferentialEqual(uniqueLabels, cellCentroids);

  // calculate cell sizes
  // create cell local coordinate system

  // distance of each vertex to the bezier
  std::map<int, double> surfaceRadialDis;
  std::map<int, double> surfaceArclengths;

  std::map<int, Point3d> surfDiffBezInterp;
  std::map<int, Point3d> surfBezInterp;

  std::map<int, double> surfaceAzim;

  // init for openmp
  forall(const vertex &v, surfaceMesh){
    Point3d t (0,0,0);
    surfaceRadialDis[v->saveId] = 0;
    surfDiffBezInterp[v->saveId] = t;
    surfBezInterp[v->saveId] = t;
    surfaceArclengths[v->saveId] = 0;
    surfaceAzim[v->saveId] = 0;
  }

  progressStart("Analyze Cells 3D - Finding radial of surface mesh nodes", 0);
  // Finding radial of surface mesh nodes
  ////#pragma omp parallel for
  for(size_t i = 0; i < surfaceMesh.size(); i++){
    if(!progressAdvance()) continue;
    const vertex v = surfaceMesh[i];
    //double minDis = 1E20, arclength = 0;
    int index;
    double weight;
    Point3d closestPoint = calcNearestPointOnBezierLine(v->pos, bMap, index, weight);

    surfaceRadialDis[v->saveId] = norm(closestPoint - v->pos);
    surfDiffBezInterp[v->saveId] = (1-weight) * diffbMap[index-1] + weight * diffbMap[index];
    surfBezInterp[v->saveId] = closestPoint;
    surfaceArclengths[v->saveId] = (double)(index-1+weight)/(double)bMap.size();
  }
  progressStart("Analyze Cells 3D - Calculate Circumferential Angles of the surface mesh", 0);
  ////#pragma omp parallel for
  for(size_t i = 0; i < surfaceMesh.size(); i++) {
    if(!progressAdvance()) continue;
    const vertex v = surfaceMesh[i];
    Point3d currentPos(v->pos);
    surfaceAzim[v->saveId] = calcCircumferential(firstRad, firstLong, currentPos, bezInterp[v->saveId]);
  }

  // Finding local cell axes
  
  //std::map<int, Point3d> nearestSurfacePoint;

  // distance of each cell to trimming (surface) mesh
  //int sizeSurfaceMesh = surfaceMesh.size();
  //std::vector<double> surfDisVec;

  rcp.rootData.arclengths = arclengths;
  rcp.rootData.arclengthsUM = arclengthsUM;
  rcp.rootData.bezLength = bezLength;
  rcp.rootData.arclengthsMinUM = arclengthsMinUM;
  rcp.rootData.arclengthsMaxUM = arclengthsMaxUM;

  if(!analyzeCellCalcDisSurface(rcp, surfaceMesh, true, surfaceArclengths, surfaceRadialDis, bMap)) return false;

  
  //nearestSurfacePoint = rcp.rootData.nearestSurfacePoint;
  //radialDis = rcp.rootData.radialDis;
  //scaledRadialDis = rcp.rootData.scaledRadialDis;

  //generate tan & binorm coord system

  progressStart("Analyze Cells 3D - Generate cell coord system", 0);
  dirRad = rcp.rootData.dirRad;
  calcCoordSystem(uniqueLabels, diffBezInterp, dirRad, dirLong, dirCirc);

  return true;
}

  void RootCellAnalyzing::estimateAllCellLengths(RootCellProcessing& rcp, std::map<int, triVector>& cellTriangles)
  {

  rcp.rootData.lengthRad.clear();
  rcp.rootData.lengthLong.clear();
  rcp.rootData.lengthCirc.clear();


  for(uint i=0; i<rcp.rootData.uniqueLabels.size(); i++){
    int currentLabel = rcp.rootData.uniqueLabels[i];


    rcp.rootData.lengthLong[currentLabel] = estimateCellLength(currentLabel, rcp.rootData.cellCentroids[currentLabel], rcp.rootData.dirLong[currentLabel], cellTriangles[currentLabel]);

    rcp.rootData.lengthRad[currentLabel] = estimateCellLength(currentLabel, rcp.rootData.cellCentroids[currentLabel], rcp.rootData.dirRad[currentLabel], cellTriangles[currentLabel]);

    rcp.rootData.lengthCirc[currentLabel] = estimateCellLength(currentLabel, rcp.rootData.cellCentroids[currentLabel], rcp.rootData.dirCirc[currentLabel], cellTriangles[currentLabel]);

    //std::cout << "l " << currentLabel << "/" << rcp.rootData.cellCentroids[currentLabel] << "/" << rcp.rootData.dirLong[currentLabel] << "/" << rcp.rootData.lengthLong[currentLabel] << std::endl;

  }

  }


// analyzes the root based on: Bezier, segmeneted mesh and surface mesh
// creates multiple outputs:
// cellTriangles: map of label -> unique triangles
// cellCentroids: center of all cells
bool RootCellAnalyzing::analyzeEmbryo(std::vector<int>& uniqueLabels, const vvGraph& segmentedMesh, const vvGraph& surfaceMesh,
  std::vector<std::vector<Point3d> >& bezGrid, std::map<int, Point3d>& bMap, 
  std::map<int, Point3d>& diffbMap, labelVertexMap& lvMap, RootCellProcessing& rcp, int firstLabel, double minVolume, QString mode)
{


  this->firstLabel = firstLabel;
  this->uniqueLabels = uniqueLabels;

  std::cout << "no OpenMP" << std::endl;
  progressStart("Analyze Cells 3D - Analyze Cells", 0);
  // data structure with all cells (unique cell labels)
  int numCells = uniqueLabels.size();

  // generate labelTriMap
  std::map<int, triVector> cellTriangles;

  if(!generateLabelTriangleMap(numCells, uniqueLabels, segmentedMesh, lvMap, cellTriangles)) return false;

  if(!analyzeCellCentroidsVolumes(cellTriangles, rcp, minVolume)) return false;

  cellCentroids = rcp.rootData.cellCentroids;
 
  // check for mode
  if(mode == "Layer")
    createLayerCoordSystem(surfaceMesh, bezGrid, rcp);
  else if(mode == "Cartesian")
    createCartesianCoordSystem(rcp);
  else { // Root
    createRootCoordSystem(surfaceMesh, bMap, diffbMap, rcp);

    for(int i=0; i<numCells; i++){
      int currentLabel = uniqueLabels[i];
      lengthLong[currentLabel] = 0;
      lengthRad[currentLabel] = 0;
      lengthCirc[currentLabel] = 0;
    }

  }


  // extract cell morphology
  progressStart("Analyze Cells 3D - Calculate cell lengths", 0);
  ////#pragma omp parallel for
  for(int i=0; i<numCells; i++){
    if(!progressAdvance()) continue;
    int currentLabel = uniqueLabels[i];

    lengthLong[currentLabel] = estimateCellLength(currentLabel, cellCentroids[currentLabel], dirLong[currentLabel], cellTriangles[currentLabel]);

    lengthRad[currentLabel] = estimateCellLength(currentLabel, cellCentroids[currentLabel], dirRad[currentLabel], cellTriangles[currentLabel]);

    lengthCirc[currentLabel] = estimateCellLength(currentLabel, cellCentroids[currentLabel], dirCirc[currentLabel], cellTriangles[currentLabel]);

  }
  
  progressStart("Analyze Cells 3D - Fill data structures", 0);
  // write all data to root data structure
  //rcp.rootData.cellVolumes = cellVolumes;

  //rcp.rootData.cellBad = cellBad;

  //rcp.rootData.nearestSurfacePoint = nearestSurfacePoint;

  //rcp.rootData.radialDis = radialDis;
  //rcp.rootData.scaledRadialDis = scaledRadialDis;
  //rcp.rootData.arclengths = arclengths;

  rcp.rootData.dirRad = dirRad;
  rcp.rootData.dirLong = dirLong;
  rcp.rootData.dirCirc = dirCirc;
  
  rcp.rootData.lengthRad = lengthRad;
  rcp.rootData.lengthLong = lengthLong;
  rcp.rootData.lengthCirc = lengthCirc;
  rcp.rootData.cellBad = badCells;
  //rcp.rootData.azimuthal = azimCoord;

  rcp.rootData.bez = bMap;
  rcp.rootData.diffBez = diffbMap;
  rcp.rootData.cellCentroids = cellCentroids;
  rcp.rootData.bezInterp = bezInterp;
  rcp.rootData.diffBezInterp = diffBezInterp;

  rcp.rootData.labelFirstCell = firstLabel;
  writeDataFields(rcp);
  progressStart("Analyze Cells 3D - Correct Dirs", 0);

  //rcp.correctDirections();

  // curently not needed
  //rootDataBox.rootData.lvMap = lvMap;
  //rootDataBox.rootData.cellTriangles = cellTriangles;
  //rootDataBox.rootData.cellCentroids = cellCentroids;

  return true;

}

  void RootCellAnalyzing::writeDataFields(RootCellProcessing& rcp)
  {

  rcp.rootData.arclengths = arclengths;
  rcp.rootData.arclengthsUM = arclengthsUM;
  rcp.rootData.arclengthsMinUM = arclengthsMinUM;
  rcp.rootData.arclengthsMaxUM = arclengthsMaxUM;
  rcp.rootData.bezInterp = bezInterp;
  rcp.rootData.diffBezInterp = diffBezInterp;
  rcp.rootData.azimuthal = azimCoord;

  }

}
