// Gmsh - Copyright (C) 1997-2015 C. Geuzaine, J.-F. Remacle
//
// See the LICENSE.txt file for license information. Please report all
// bugs and problems to the public mailing list <gmsh@geuz.org>.

/*
  compute the hausdorff distance between two polygonal curves
  in n*m time where n and m are the nb of points of the
  polygonal curves
*/

#include "SVector3.h"
#include "hausdorffDistance.h"

static double intersect (SPoint3 &q, SVector3 &n, // a plane
			 SPoint3 &p1, SPoint3 &p2, // a segment
			 SPoint3 &result)
{
  // n * (x-q) = 0
  // x = p1 + t (p2-p1)
  // n *(p1 + t (p2-p1) - q) = 0
  // t = n*(q-p1) / n*(p2-p1)

  const double t = dot(n,q-p1)/dot(n,p2-p1);
  result = p1 *(1.-t)+ p2*t;
  return t;
}

static double projection (SPoint3 &p1, SPoint3 &p2, SPoint3 &q, SPoint3 &result)
{
  // x = p1 + t (p2 - p1)
  // (x - q) * (p2 - p1) = 0
  // (p1 + t (p2 - p1) - q) (p2 - p1) = 0
  // t = (q-p1) * (p2-p1) / (p2-p1)^2
  SVector3 p21 = p2-p1;
  const double t = dot(q-p1,p21)/dot(p21,p21);
  result = p1 *(1.-t)+ p2*t;
  return t;
}

static SPoint3 closestPoint (SPoint3 &p1, SPoint3 &p2, SPoint3 &q)
{
  double result;
  const double t = projection (p1,p2,q,result);
  if (t >= 0.0 && t <= 1.0)return result;
  if (t < 0)return p1;
  return p2;
}

double closestPoint (const std::vector<SPoint3> &P, const SPoint3 &p, SPoint3 & result)
{
  double closestDistance = 1.e22;
  for (unsigned int i=1;i<P.size();i++){
    SPoint3 q = closestPoint (P[i-1],P[i],p);
    const double pq = p.distance(q);
    if (pq < closestDistance){
      closestDistance = pq;
      result = q;
    }
  }
  return closestDistance;
}

// we test all points of P plus all points that are the intersections
// of angle bissectors of Q with P
double oneSidedHausdorffDistance (const std::vector<SPoint3> &P,
				  const std::vector<SPoint3> &Q,
				  SPoint3 &p1, SPoint3 &p2)
{
  const double hausdorffDistance = 0.0;

  // first test the points
  for (unsigned int i=0;i<P.size();i++){
    SPoint3 result;
    double d = closestPoint (Q, P[i], result);
    if (d > hausdorffDistance){
      hausdorffDistance = d;
      p1 = P[i];
      p2 = result;
    }
  }
  // compute angle bissectors intersections
  std::vector<SPoint3> intersections;
  for (unsigned int i=1;i<Q.size()-1;i++){
    SPoint3 a = Q[i-1];
    SPoint3 b = Q[i];
    SPoint3 c = Q[i+1];
    SVector3 ba = b-a;
    SVector3 ca = c-a;
    SVector3 bissector = (ba+ca);
    SVector3 n; // normal to the bissector plane
    if (bissector.norm == 0){
      ba.normalize();
      n = ba;
    }
    else{
      SVector3 b = crossprod (bissector,ba);
      n = crossprod (b,bissector);
      n.normalize();
    }
    for (unsigned int i=1;i<P.size();i++){
      SPoint3 result;
      const double t = intersect (b, n, P[i-1],P[i],result);
      if (t >=0 && t <=1)intersections.push_back(result);
    }
  }

  for (unsigned int i=0;i<intersections.size();i++){
    SPoint3 result;
    double d = closestPoint (Q, intersections[i], result);
    if (d > hausdorffDistance){
      hausdorffDistance = d;
      p1 = P[i];
      p2 = result;
    }
  }
  return hausdorffDistance;
}

double hausdorffDistance (const std::vector<SPoint3> &P,
			  const std::vector<SPoint3> &Q)
{
  return std::max(oneSidedHausdorffDistance (P,Q),
		  oneSidedHausdorffDistance (Q,P));
}
