#include "GEdge.h"
#include "nodalBasis.h"
#include "SVector3.h"
#include <algorithm>
#include <limits>
#include "OptHomIntegralBoundaryDist.h"
#include "discreteFrechetDistance.h"
#include "MLine.h"

parametricLineNodalBasis::parametricLineNodalBasis(const nodalBasis &basis,
                                                   const std::vector<SPoint3> &xyz):
  _basis(basis), _xyz(xyz) , psi(_xyz.size())
{
}


SPoint3 parametricLineNodalBasis::operator()(double xi) const
{
  SPoint3 p(0, 0, 0);
  _basis.f(-1 + 2 * xi, 0, 0, &psi[0]);
  for (size_t j = 0; j < psi.size(); ++j) {
    p[0] += psi[j] * _xyz[j].x();
    p[1] += psi[j] * _xyz[j].y();
    p[2] += psi[j] * _xyz[j].z();
  }
  return p;
}

SVector3 parametricLineNodalBasis::derivative(double xi) const
{
  double dpsi[1256][3];
  SVector3 p(0, 0, 0);
  _basis.df(-1 + 2 * xi, 0, 0, dpsi);
  for (size_t j = 0; j < _xyz.size(); ++j) {
    p[0] += dpsi[j][0] * _xyz[j].x();
    p[1] += dpsi[j][0] * _xyz[j].y();
    p[2] += dpsi[j][0] * _xyz[j].z();
  }
  return p;
}

SVector3 parametricLineNodalBasis::secondDerivative(double xi) const
{
  double ddpsi[1256][3][3];
  SVector3 p(0, 0, 0);
  _basis.ddf(-1 + 2 * xi, 0, 0, ddpsi);
  for (size_t j = 0; j < _xyz.size(); ++j) {
    p[0] += ddpsi[j][0][0] * _xyz[j].x();
    p[1] += ddpsi[j][0][0] * _xyz[j].y();
    p[2] += ddpsi[j][0][0] * _xyz[j].z();
  }
  return p;
}

parametricLineGEdge::parametricLineGEdge(const GEdge *edge, double t0, double t1):
  _edge(edge), _t0(t0), _t1(t1) {}

SPoint3 parametricLineGEdge::operator()(double xi) const
{
  GPoint gp = _edge->point(_t0 + (_t1 - _t0) * xi);
  return SPoint3 (gp.x(), gp.y(), gp.z());
}

SVector3 parametricLineGEdge::derivative(double xi) const
{
  return _edge->firstDer(_t0 + (_t1 - _t0) * xi);
}

SVector3 parametricLineGEdge::secondDerivative(double xi) const
{
  return _edge->secondDer(_t0 + (_t1 - _t0) * xi);
}

static void oversample (std::vector<SPoint3> &s, double tol)
{
  std::vector<SPoint3> t;
  for (unsigned int i=1;i<s.size();i++){
    SPoint3 p0 = s[i-1];
    SPoint3 p1 = s[i];
    double d = p0.distance(p1);
    int N = (int) (d / tol);
    //    printf("N = %d %g %g\n",N,d,tol);
    t.push_back(p0);
    for (int j=1;j<N;j++){
      const double xi = (double) j/ N;
      t.push_back(p0 + (p1-p0)*xi);
    }
  }
  t.push_back(s[s.size()-1]);
  s = t;
}

// FAST IMPLEMENTATION OF DISCRETE UNIDIRECTIONAL HAUSDORFF DISTANCE
double computeBndDistH(GEdge *edge, std::vector<double> & params, // the model edge
		       const std::vector<MVertex*> &vs,
		       const nodalBasis &basis, const std::vector<SPoint3> &xyz,
		       const double tolerance) // the mesh edge
{
  if (edge->geomType() == GEntity::Line)return 0.0;
  std::vector<SPoint3> dpts;
  std::vector<double> ts;
  std::vector<MVertex*> hov;
  for (unsigned int i=2;i<vs.size();i++)hov.push_back(vs[i]);
  MLineN l (vs[0],vs[1],hov);
  l.discretize (tolerance,dpts,ts);
  oversample(dpts,tolerance);
  double maxDist = 0.0;
  for (unsigned int i = 0; i<dpts.size(); i++){
    maxDist = std::max (maxDist,dpts[i].distance(edge->closestPoint(dpts[i],tolerance)));
  }
  return maxDist;
}

SVector3 parametricLine::curvature(double xi) const
{
  SVector3 xp  = derivative(xi);
  SVector3 xpp = secondDerivative(xi);
  const double nxp = xp.norm();
  const double onxp = 1./nxp;
  SVector3 c = (onxp*onxp*onxp)*(xpp*nxp-xp*dot(xp,xpp)*onxp);
  return c;
}

double parametricLine::frechetDistance(const parametricLine &l,
				       SPoint3 &p1, SPoint3 &p2,
				       double tol) const
{
  std::vector<SPoint3> dpts1,dpts2;
  std::vector<double> ts1,ts2;
  discretize (dpts1,ts1,tol);
  l.discretize (dpts2,ts2,tol);
  //  printf("discretizing gives %d %d points\n",dpts1.size(),dpts2.size());
  oversample(dpts1,tol);
  oversample(dpts2,tol);
  //  printf("after oversaplinf an discretizing gives %d %d points\n",dpts1.size(),dpts2.size());
  return discreteFrechetDistance(dpts1,dpts2);
}
double parametricLine::hausdorffDistance(const parametricLine &l, SPoint3 &p1, SPoint3 &p2,
					 double tolerance) const
{
  std::vector<SPoint3> dpts1,dpts2;
  std::vector<double> ts1,ts2;
  discretize (dpts1,ts1,tolerance);
  l.discretize (dpts2,ts2,tolerance);

  //  oversample(dpts1,tolerance);
  //  oversample(dpts2,tolerance);
  //  printf("coucou4 %d %d points\n",dpts1.size(),dpts2.size());
  double h1 = 0.0;
  int I1=0,J1=0;
  int I2=0,J2=0;
  for (unsigned int i = 0; i<dpts1.size();i++){
    double hl = 1.e22;
    int JLOC = 0;
    for (unsigned int j = 0; j<dpts2.size();j++){
      double H = dpts1[i].distance(dpts2[j]);
      if (hl < H){
	hl = H;
	JLOC = j;
      }
    }
    if (hl > h1){
      h1 = hl;
      J1 = JLOC;
      I1 = i;
    }
  }
  double h2 = 0.0;
  for (unsigned int i = 0; i<dpts2.size();i++){
    double hl = 1.e22;
    int JLOC = 0;
    for (unsigned int j = 0; j<dpts1.size();j++){
      double H = dpts1[j].distance(dpts2[i]);
      if (hl < H){
	hl = H;
	JLOC = j;
      }
    }
    if (hl > h2){
      h2 = hl;
      J2 = JLOC;
      I2 = i;
    }
  }
  if (h1 > h2){
    p1 = dpts1[I1];
    p2 = dpts2[J1];
    return h1;
  }
  else{
    p1 = dpts2[I2];
    p2 = dpts1[J2];
    return h2;
  }
}

// DISCRETE FRECHET DISTANCE
double computeBndDistF(GEdge *edge,
		       std::vector<double> & params, // the model edge
		       const nodalBasis &basis,
		       const std::vector<SPoint3> &xyz,
		       const double tolerance) // the mesh edge
{
  if (edge->geomType() == GEntity::Line)return 0.0;
  parametricLineGEdge l1 = parametricLineGEdge(edge, params[0],params[1]);
  parametricLineNodalBasis l2 = parametricLineNodalBasis(basis, xyz);
  SPoint3 p1,p2;
  return l1.frechetDistance(l2,p1,p2,tolerance);
}

// GMSH's DISTANCE
/*
 */
double computeBndDistGb(GEdge *edge, std::vector<double> & params, // the model edge
		       const nodalBasis &basis, const std::vector<SPoint3> &xyz, double tolerance) // the mesh edge
{
  parametricLineGEdge l1 = parametricLineGEdge(edge, params[0], params[1]);
  parametricLineNodalBasis l2 = parametricLineNodalBasis(basis, xyz);
  const unsigned int N = 20;
  SPoint3 P1[N];
  SPoint3 P2[N];
  double D = 0.0;
  for (unsigned int i=0;i<N;i++){
    const double _x2 = (double)i / (N-1);
    P1[i] = l1(_x2);
    P2[i] = l2(_x2);
  }
  double L = 0.0;
  for (unsigned int i=0;i<N-1;i++){
    SPoint3 p11 = P1[i];
    SPoint3 p12 = P1[i+1];
    SPoint3 p21 = P2[i];
    SPoint3 p22 = P2[i+1];
    SVector3 v1  (p21,p11);
    SVector3 v2  (p12,p11);
    SVector3 v12 (p22,p11);
    SVector3 vl (p22,p21);
    SVector3 x1 = crossprod(v12,v1);
    SVector3 x2 = crossprod(v12,v2);
    D += 0.5*(x1.norm()+x2.norm());
    L += vl.norm();
  }
  return D;
}

double computeBndDistG_(GEdge *edge, std::vector<double> & p, // the model edge
		       const nodalBasis &basis, const std::vector<SPoint3> &xyz,
		       const unsigned int NN) // the mesh edge
{
  const unsigned int N = 200;
  std::vector<int> o;
  o.push_back(0);
  for (unsigned int i=2; i < p.size();i++)o.push_back(i);
  o.push_back(1);

  //  printf("computing diustance with tolerance %g\n",tolerac);

  double D = 0.0;
  const double U0 = basis.points(0,0);
  const double U1 = basis.points(1,0);
  for (int i = 0 ; i < basis.order ; i++) {
    const double u0 = basis.points(o[i],0);
    const double u1 = basis.points(o[i+1],0);

    // U0 ----u0-----u1-----U1

    const double t0 = p[o[i]];
    const double t1 = p[o[i+1]];
    parametricLineGEdge l1 = parametricLineGEdge(edge, t0, t1);
    parametricLineNodalBasis l2 = parametricLineNodalBasis(basis, xyz);
    std::vector<SPoint3> P1(N), P2(N);
    for (unsigned int i=0;i<N;i++){
      const double _x2 = (double)i / (N-1);
      const double u = u0 + _x2 * (u1-u0);
      // U0 + uu * (U1 - U0) = u
      const double uu = (u - U0) / (U1-U0);
      P1[i] = l1(_x2);
      P2[i] = l2(uu);
    }
    //    double L = 0.0;
    for (unsigned int i=0;i<N-1;i++){
      SPoint3 p11 = P1[i];
      SPoint3 p12 = P1[i+1];
      SPoint3 p21 = P2[i];
      SPoint3 p22 = P2[i+1];
      SVector3 v1  (p21,p11);
      SVector3 v2  (p12,p11);
      SVector3 v12 (p22,p11);
      //      SVector3 vl (p22,p21);
      SVector3 x1 = crossprod(v12,v1);
      SVector3 x2 = crossprod(v12,v2);
      D += 0.5*(x1.norm()+x2.norm());
      //      L += vl.norm();
    }
  }
  return D;
}

double computeBndDistG(GEdge *edge, std::vector<double> & p, // the model edge
		       const nodalBasis &basis, const std::vector<SPoint3> &xyz,
		       double tolerance) // the mesh edge
{
  int N = 5;
  double d = computeBndDistG_(edge, p, basis, xyz, N);

  //  printf("GO !!\n");
  while (1){
    N *= 2;
    double dp = computeBndDistG_(edge, p, basis, xyz, N);
    //    printf("N %d %12.5E %12.5E %12.5E %12.5E\n",N,d,dp,fabs(d - dp),tolerance);
    if (fabs(d - dp) < tolerance*(d+dp)) // Richardson with assumed linear convergence ...
      return dp;
    d = dp;
  }
}

void parametricLine::recur_discretize(const double &t1, const double &t2,
				      const SPoint3 &p1, const SPoint3 &p2,
				      std::vector<SPoint3> &dpts,
				      std::vector<double> &ts,
				      double Prec, int depth) const
{
  double t = 0.5 * (t2 + t1);
  SPoint3 p = (*this)(t);
  SVector3 dx (p,(p1+p2)*0.5);
  //  printf("%g %g -- %g %g dist %12.5E %12.5E\n",p1.x(),p1.y(),p2.x(),p2.y(),dx.norm(),Prec);
  if ((depth > 20 && dx.norm() < Prec) || depth > 45){
    dpts.push_back(p);
    ts.push_back(t);
    dpts.push_back(p2);
    ts.push_back(t2);
  }
  else {
    recur_discretize (t1,t,p1,p,dpts,ts,Prec,depth+1);
    recur_discretize (t,t2,p,p2,dpts,ts,Prec,depth+1);
  }
}

void parametricLine::discretize(std::vector<SPoint3> &dpts,
				std::vector<double> &ts,
				double Prec, double t0, double t1) const
{
  dpts.push_back((*this)(t0));
  ts.push_back(t0);
  recur_discretize (t0,t1,dpts[0],(*this)(t1),dpts,ts,Prec,0);

  //  printf("discretizing from %g to %g\n",t0,t1);
  //  for (unsigned int i=0;i<ts.size();i++){
  //    printf("%g ",ts[i]);
  //  }
  //  printf("\n");

}

double trapeze (SPoint3 &p1, SPoint3 &p2){
  return (p2.x() - p1.x())*(p1.y()+p2.y())*0.5;
}

double computeDeviationOfTangents(GEdge *edge,
				  std::vector<double> &p, // parameters of mesh vertices on the model edge
				  const nodalBasis &basis,
				  const std::vector<SPoint3> &xyz) // the mesh edge
{
  //  parametricLineGEdge l1 = parametricLineGEdge(edge,p[0],p[p.size()-1]);
  parametricLineNodalBasis l2 = parametricLineNodalBasis(basis, xyz);
  double  deviation = 0;
  //  double ddeviation = 0;
  std::vector<int> o;
  o.push_back(0);
  for (unsigned int i=2; i < p.size();i++)o.push_back(i);
  o.push_back(1);

  SVector3 dx (xyz[xyz.size() - 1], xyz[0]);

  for (unsigned int i=0; i<p.size();i++){
    const double u = basis.points(o[i],0);
    SVector3 xp = edge->firstDer (p[o[i]]);
    //    SVector3 xpp = edge->secondDer (p[o[i]]);
    //    const double nxp = xp.norm();
    //    const double onxp = 1./nxp;
    //    SVector3 c = (onxp*onxp*onxp)*(xpp*nxp-xp*dot(xp,xpp)*onxp);

    SVector3 t_mesh_edge  = l2.derivative(0.5*(1+u));
    //    SVector3 c2  = l2.curvature(0.5*(1+u));
    //    GPoint p0 = edge->point(p[o[i]]);
    //    SPoint3 p1 = l2 (0.5*(1+u));
    //    printf("%g = %g %g vs %g %g\n",u,p0.x(),p0.y(),p1.x(),p1.y());
    xp.normalize();
    t_mesh_edge.normalize();
    SVector3 diff1 = (dot(xp, t_mesh_edge) > 0) ? xp -  t_mesh_edge : xp +  t_mesh_edge;
    //    SVector3 diff2 = (dot(c, c2) > 0) ? c -  c2 : c +  c2;
    //printf("%g %g %g vs %g %g %g diff %g %g %g\n",c.x(),c.y(),c.z(),c2.x(),c2.y(),c2.z(),diff2.x(),diff2.y(),diff2.z());
    //    printf("%g %g %g vs %g %g %g val %g\n",t_model_edge.x(),t_model_edge.y(),t_model_edge.z(),
    //	   t_mesh_edge.x(),t_mesh_edge.y(),t_mesh_edge.z(),c.norm());
    //     deviation = std::max(diff1.norm(),deviation);
    //    ddeviation = std::max(diff2.norm(),ddeviation);
     deviation += diff1.norm();
     //    ddeviation += diff2.norm();
  }
  const double h =  dx.norm();
  //  printf ("%g %g\n",deviation * h,ddeviation * h * h * 0.5);
  return deviation * h;// + ddeviation * h * h * 0.5;
}

double computeBndDistAccurateArea(GEdge *edge,
				  std::vector<double> &p, // parameters of mesh vertices on the model edge
				  const nodalBasis &basis,
				  const std::vector<SPoint3> &xyz,
				  double tolerance) // the mesh edge
{
  // assume mesh and CAD non intersecting except at mesh vertices
  // compute distance for "order" sub-polygonal curves
  double area = 0.0;
  double length = 0.0;
  std::vector<int> o;
  o.push_back(0);
  for (unsigned int i=2; i < p.size();i++)o.push_back(i);
  o.push_back(1);

  //  printf("computing diustance with tolerance %g\n",tolerac);

  for (int i = 0 ; i < basis.order ; i++) {
    const double u0 = basis.points(o[i],0);
    const double u1 = basis.points(o[i+1],0);
    const double t0 = p[o[i]];
    const double t1 = p[o[i+1]];
    parametricLineGEdge l1 = parametricLineGEdge(edge, t0, t1);
    parametricLineNodalBasis l2 = parametricLineNodalBasis(basis, xyz);
    std::vector<SPoint3> dpts1,dpts2;
    std::vector<double> ts1,ts2;
    l1.discretize (dpts1,ts1,tolerance);
    l2.discretize (dpts2,ts2,tolerance,0.5*(1+u0),0.5*(1+u1));
    //    printf("discretizing : %g %g and %g %g\n",u0,u1,t0,t1);
    // simple 2D version
    double arealocal = 0.0;
    for (unsigned int j=1;j<dpts1.size(); j++) {
      length += dpts1[j-1].distance (dpts1[j]);
      arealocal += trapeze (dpts1[j-1],dpts1[j]);
    }
    for (unsigned int j=1;j<dpts2.size(); j++) {
      arealocal -= trapeze (dpts2[j-1],dpts2[j]);
    }
    area += fabs(arealocal);
  }
  return area;
}


// INPUT FCT FOR OPTIMIZATION
double computeBndDistAndGradient(GEdge *edge,
				 std::vector<double> &param, // parameters of mesh vertices on the model edge
				 const std::vector<MVertex*> &vs, // vertices
				 const nodalBasis &basis,  // what is the FE basis of the edge
				 std::vector<SPoint3> &xyz,  // real coordinates of mesh vertices on the model edge
				 std::vector<bool> &onEdge, // tell if a given vertex sits on the model edge and therefore can be movd
				 std::vector<double> &grad,
				 double tolerance)// model tolerance
{
  grad.resize(xyz.size());
  double ref;
  if (tolerance  < 0 ) ref = computeDeviationOfTangents(edge, param,basis, xyz);
  else ref = computeBndDistG(edge, param,basis, xyz, tolerance);
    //double ref = computeBndDistAccurateArea(edge, param,basis, xyz, tolerance);
  double delta = (edge->getUpperBound() - edge->getLowerBound()) * 1e-8;
  for (size_t i = 0; i < xyz.size(); ++i) {
    if (!onEdge[i]) {
      grad[i] = 0;
      continue;
    }
    double p = param[i];
    double delta = 1e-6;
    param[i] += delta;
    xyz[i] = SPoint3(edge->position(param[i]));
    if (tolerance > 0) grad[i] = (computeBndDistG(edge, param,basis, xyz,tolerance) - ref) / delta;
    else grad[i] = (computeDeviationOfTangents(edge, param,basis, xyz) - ref) / delta;
    param[i] = p;
    xyz[i] = SPoint3(edge->position(param[i]));
  }
  return ref;
}

#include "MElement.h"
#include "MVertex.h"
#include "BasisFactory.h"
double computeBndDist(MElement *element, int distanceDefinition, double tolerance)
{
  double dist = 0;
  const nodalBasis &elbasis = *element->getFunctionSpace();
  for (int iEdge = 0; iEdge < element->getNumEdges(); ++iEdge) {
    int clId = elbasis.getClosureId(iEdge, 1);
    const std::vector<int> &closure = elbasis.closures[clId];
    std::vector<SPoint3> xyz;
    GEdge *edge = NULL;
    std::vector<MVertex *> vertices (closure.size());
    for (size_t i = 0; i < closure.size(); ++i) {
      MVertex *v = element->getVertex(closure[i]);
      vertices[i] = v;
      xyz.push_back(v->point());
      if ((int)i >= 2  && v->onWhat() && v->onWhat()->dim() == 1) {
        edge = v->onWhat()->cast2Edge();
      }
    }
    if (edge){
      std::vector<double> params(closure.size());
      for (size_t i = 0; i < closure.size(); ++i) {
	reparamMeshVertexOnEdge(element->getVertex(closure[i]), edge, params[i]);
      }
      if (distanceDefinition == 1)
	dist = std::max(computeBndDistH(edge, params, vertices, *BasisFactory::getNodalBasis(elbasis.getClosureType(clId)), xyz,tolerance),dist);
      else if (distanceDefinition == 2)
	dist = std::max(computeBndDistG(edge, params, *BasisFactory::getNodalBasis(elbasis.getClosureType(clId)), xyz, tolerance),dist);
      else if (distanceDefinition == 4)
	dist = std::max(computeBndDistF(edge, params, *BasisFactory::getNodalBasis(elbasis.getClosureType(clId)), xyz, tolerance),dist);
      else if (distanceDefinition == 5)
	dist = std::max(computeBndDistAccurateArea(edge, params,*BasisFactory::getNodalBasis(elbasis.getClosureType(clId)), xyz, tolerance),dist);
      else if (distanceDefinition == 6)
	dist = std::max(computeDeviationOfTangents(edge, params,*BasisFactory::getNodalBasis(elbasis.getClosureType(clId)), xyz),dist);
      else
	Msg::Fatal("unknown distance definition %d. Choose 1 for Hausdorff and 2 for Area/Length 4 for Discrete Frechet",distanceDefinition);
    }
  }
  return dist;
}
