#include "CellAtlasUtils.hpp"

#define PI 3.14159265

using namespace std;

namespace mgx 
{


 // std::map<IntInt, double> wallArea;
 // labelDataMap cellWallArea;





bool neighborhoodGraphLocal(const vvGraph& S, double tolerance, std::map<int, double>& cellWallArea, std::map<IntInt, double>& wallArea, std::map<int, double>& outsideWallArea)
  {
    NhbdGraphInfo info;
    neighborhoodGraph(S, tolerance, info); // now in graphutils

    wallArea = info.sharedWallArea;
    cellWallArea = info.cellWallArea;
    outsideWallArea = info.outsideWallArea;

    return true;
  }

  // finds one label of selected vertices
  // return 0 if nothing selected
  int findSelectedLabel(const vvGraph& S)
  {
    forall(const vertex &v, S) {
      if(v->selected and v->label!=-1) return v->label;
    }
    return 0;
  }

  // finds two (cell) labels of selected vertices
  // return fals if less than two labels are selected
  bool findTwoSelectedLabels(const vvGraph& S, int& label1, int& label2)
  {
    label1 = 0;
    label2 = 0;

    forall(const vertex &v, S) {
      if(v->selected and v->label!=-1){
        if(label1 == 0) label1 = v->label;
        else if(v->label != label1){
          label2 = v->label;
          return true;
        }
      } 
    }
    return false;
  }
/*
  // 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;
  }


*/
 // return true if pointToTest is inside a defined cone
 bool pointInCone(Point3d axisVec, Point3d& coneTop, Point3d& coneBase, double coneAngle, Point3d& pointToTest)
 {
    Point3d apexToX = coneTop - pointToTest;

    double dp = apexToX * axisVec;
    double denom = norm(axisVec) * norm(apexToX);

    double dir = (pointToTest - coneBase) * axisVec;

    if(dp/denom > cos(coneAngle) and dir > 0) return true;
    else return false;
 }

 // calculates the value of a two dimensional gaussian function at location (x,y)
double gauss1D(double x, double muX, double sigmaX)
{
  return (exp(-0.5*(x-muX)*(x-muX)/sigmaX/sigmaX));
}




  // calculates the circumferential (in rad) of a cell(center)
  // Point3d dirRadRef - reference direction for the circumferential (in radial direction)
  // Point3d dirLongRef - reference direction along the longitudinal (along the bezier)
  // Point3d cellCenter - the cell center
  // Point3d cellOnLong - the nearest point (of the cell center) on the bezier
  double calcCircumferential(Point3d dirRadRef, Point3d dirLongRef, Point3d cellCenter, Point3d cellOnLong)
  {
    double result;

    Point3d planeNormal = dirLongRef ^ dirRadRef;
    planeNormal/=norm(planeNormal);

    //double d = planeNormal * cellOnLong;
    Point3d planePoint;// = closestpoint(planeNormal,d,cellCenter);
    double sN;
    planeLineIntersect(cellOnLong, planeNormal, cellCenter, cellCenter-planeNormal, sN, planePoint);
    Point3d xP = planePoint - cellOnLong;
    Point3d xC = cellCenter - cellOnLong;
    double sP = xP * dirRadRef;
    double sC = xC * planeNormal;
    result = acos(norm(xP) / norm(xC));

    if(sP < 0){
      if(sC < 0){
        result=PI-result;
      } else {
        result+=PI;
      }
    } else {
      if(sC < 0){
        // do nothing
      } else {
        result=2*PI-result;
      }

    }

    return result;
  }

    Point3d inPrimCoord(double radiant, Point3d dirRadRef, Point3d dirLongRef)
  {
    Matrix3d rotMatrix = Matrix3d::rotation(dirLongRef, radiant);
    Point3d standardDir = dirRadRef * rotMatrix;
    return standardDir/norm(standardDir);
  }

 

// finds min and max of a map
void findMinMax(std::map<int,double>& map, double& min, double& max)
{
  min = 1E20;
  max = -1E20;

  for(uint i = 0; i<map.size(); i++){
    if(map[i] > max)
      max = map[i];
    if(map[i] < min)
      min = map[i];
  }
}




  void differentialOfBezierGrid(std::vector<std::vector<Point3d> >& bezierGrid, Point2i& p, Point3d& diffU, Point3d& diffV)
  {

  int discPoints = bezierGrid.size();

  if(p.x() > 0 and p.x() < discPoints-1){
    diffU = bezierGrid[p.x()+1][p.y()] - bezierGrid[p.x()-1][p.y()];
  } else if(p.x() == 0){
    diffU = bezierGrid[p.x()+1][p.y()] - bezierGrid[p.x()][p.y()];
  } else if(p.x() == discPoints-1){
    diffU = bezierGrid[p.x()][p.y()] - bezierGrid[p.x()-1][p.y()];
  }
  diffU /= norm(diffU);

  discPoints = bezierGrid[p.x()].size();

  if(p.y() > 0 and p.y() < discPoints-1){
    diffV = bezierGrid[p.x()][p.y()+1] - bezierGrid[p.x()][p.y()-1];
  } else if(p.y() == 0){
    diffV = bezierGrid[p.x()][p.y()+1] - bezierGrid[p.x()][p.y()];
  } else if(p.y() == discPoints-1){
    diffV = bezierGrid[p.x()][p.y()] - bezierGrid[p.x()][p.y()-1];
  }
  diffV /= norm(diffV);


  }

  double cartesianToOrganCoordsLong(Point3d cartesianCoords, std::map<int, Point3d>& bMap, Point3d& closestPoint)
  {
    int index = 0;
    double weight = 0.;
    closestPoint = calcNearestPointOnBezierLine(cartesianCoords, bMap, index, weight);

    return (double)(index-1+weight)/(double)bMap.size();
  }



}
