/****************************************************************************
* VCGLib                                                            o o     *
* Visual and Computer Graphics Library                            o     o   *
*                                                                _   O  _   *
* Copyright(C) 2004-2016                                           \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program is free software; you can redistribute it and/or modify      *
* it under the terms of the GNU General Public License as published by      *
* the Free Software Foundation; either version 2 of the License, or         *
* (at your option) any later version.                                       *
*                                                                           *
* This program is distributed in the hope that it will be useful,           *
* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/

#ifndef __VCGLIB_FACE_DISTANCE
#define __VCGLIB_FACE_DISTANCE

#include <vcg/math/base.h>
#include <vcg/space/point3.h>
#include <vcg/space/segment3.h>
#include <vcg/space/distance3.h>

namespace vcg {
    namespace face{
/*
  Basic Wrapper for getting point-triangular face distance
  distance is unsigned;

  return true if the closest point <q> on <f> is nearer than the passed <dist>;
  return false otherwiswe (and q is not valid)

  This wrapper requires that your face has
  - Per Face Flags well initialized
  - Per Face EdgePlane component initialized.
  Initialization must be done with:

      tri::UpdateEdges<MeshType>::Set(yourMesh);
 */
    template <class FaceType>
    bool PointDistanceEP(	const FaceType &f,
                        const vcg::Point3<typename FaceType::ScalarType> & q,
                        typename FaceType::ScalarType & dist,
                        vcg::Point3<typename FaceType::ScalarType> & p )
    {
      typedef typename FaceType::ScalarType ScalarType;
      const ScalarType EPS = ScalarType( 0.000001);

        ScalarType b,b0,b1,b2;

        ScalarType d = SignedDistancePlanePoint( f.cPlane(), q );
        if( d>dist || d<-dist ) return false;

        Point3<ScalarType> t = f.cPlane().Direction();
        p = q - t*d; // p is the projection of q on the face plane
        // Now Choose the best plane and test to see if p is inside the triangle

        switch( f.cFlags() & (FaceType::NORMX|FaceType::NORMY|FaceType::NORMZ) )
        {
        case FaceType::NORMX:
            b0 = f.cEdge(1)[1]*(p[2] - f.cP(1)[2]) - f.cEdge(1)[2]*(p[1] - f.cP(1)[1]);
            if(b0<=0)
            {
                b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                if(dist>b0) { dist = b0; return true; }
                else return false;
            }
            b1 = f.cEdge(2)[1]*(p[2] - f.cP(2)[2]) - f.cEdge(2)[2]*(p[1] - f.cP(2)[1]);
            if(b1<=0)
            {
                b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                if(dist>b1) { dist = b1; return true; }
                else return false;
            }
            b2 = f.cEdge(0)[1]*(p[2] - f.cP(0)[2]) - f.cEdge(0)[2]*(p[1] - f.cP(0)[1]);
            if(b2<=0)
            {
                b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
                if(dist>b2) { dist = b2; return true; }
                else return false;
            }
            // if all these tests failed the projection p should be inside.
            // Some further tests for more robustness...
            if( (b=std::min(b0,std::min(b1,b2)) ) < EPS*DoubleArea(f))
            {
              ScalarType bt;
              if(b==b0) 	    bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
              else if(b==b1) 	bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
              else {          assert(b==b2);
                bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
              }
              if(dist>bt) { dist = bt; return true; }
              else return false;
            }
            break;

          case FaceType::NORMY:
            b0 = f.cEdge(1)[2]*(p[0] - f.cP(1)[0]) - f.cEdge(1)[0]*(p[2] - f.cP(1)[2]);
            if(b0<=0)
            {
                b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                if(dist>b0) { dist = b0; return true; }
                else return false;
            }
            b1 = f.cEdge(2)[2]*(p[0] - f.cP(2)[0]) - f.cEdge(2)[0]*(p[2] - f.cP(2)[2]);
            if(b1<=0)
            {
                b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                if(dist>b1) { dist = b1; return true; }
                else return false;
            }
            b2 = f.cEdge(0)[2]*(p[0] - f.cP(0)[0]) - f.cEdge(0)[0]*(p[2] - f.cP(0)[2]);
            if(b2<=0)
            {
                b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
                if(dist>b2) { dist = b2; return true; }
                else return false;
            }
            if( (b=math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
            {
              ScalarType bt;
              if(b==b0) 	    bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
              else if(b==b1) 	bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
              else { assert(b==b2);
                bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
              }
              if(dist>bt) { dist = bt; return true; }
                else return false;
            }
            break;

          case FaceType::NORMZ:
            b0 = f.cEdge(1)[0]*(p[1] - f.cP(1)[1]) - f.cEdge(1)[1]*(p[0] - f.cP(1)[0]);
            if(b0<=0)
            {
                b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                if(dist>b0) { dist = b0; return true; }
                else return false;
            }
            b1 = f.cEdge(2)[0]*(p[1] - f.cP(2)[1]) - f.cEdge(2)[1]*(p[0] - f.cP(2)[0]);
            if(b1<=0)
            {
                b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                if(dist>b1) { dist = b1; return true; }
                else return false;
            }
            b2 = f.cEdge(0)[0]*(p[1] - f.cP(0)[1]) - f.cEdge(0)[1]*(p[0] - f.cP(0)[0]);
            if(b2<=0)
            {
                b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
                if(dist>b2) { dist = b2; return true; }
                else return false;
            }
            if( (b=math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
            {
              ScalarType bt;
              if(b==b0) 	    bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
              else if(b==b1) 	bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
              else { assert(b==b2);
                bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
              }

              if(dist>bt) { dist = bt; return true; }
                else return false;
            }
            break;

        } // end switch

        dist = ScalarType(fabs(d));
        return true;
    }

    template <class S>
    class PointDistanceEPFunctor {
    public:
        typedef S ScalarType;
        typedef Point3<ScalarType> QueryType;
        static inline const Point3<ScalarType> &  Pos(const QueryType & qt)  {return qt;}

        template <class FACETYPE, class SCALARTYPE>
        inline bool operator () (const FACETYPE & f, const Point3<SCALARTYPE> & p, SCALARTYPE & minDist, Point3<SCALARTYPE> & q) {
            const Point3<typename FACETYPE::ScalarType> fp = Point3<typename FACETYPE::ScalarType>::Construct(p);
            Point3<typename FACETYPE::ScalarType> fq;
            typename FACETYPE::ScalarType md = (typename FACETYPE::ScalarType)(minDist);
            const bool ret = PointDistanceEP(f, fp, md, fq);
            minDist = (SCALARTYPE)(md);
            q = Point3<SCALARTYPE>::Construct(fq);
            return (ret);
        }
    };



    template <class S>
    class PointNormalDistanceFunctor {
    public:
        typedef typename S::ScalarType ScalarType;
        typedef S QueryType;
        static inline const Point3<ScalarType> &  Pos(const QueryType & qt)  {return qt.P();}


        static ScalarType & Alpha(){static ScalarType alpha = 1.0; return alpha;}
        static ScalarType & Beta (){static ScalarType beta  = 1.0; return beta;}
        static ScalarType & Gamma(){static ScalarType gamma = 1.0; return gamma;}
        static ScalarType & InterPoint(){static ScalarType interpoint= 1.0; return interpoint;}


        template <class FACETYPE, class SCALARTYPE>
        inline bool operator () (const FACETYPE &f, const typename FACETYPE::VertexType &p,
            SCALARTYPE & minDist,Point3<SCALARTYPE> & q)
        {
            const Point3<typename FACETYPE::ScalarType> fp = Point3<typename FACETYPE::ScalarType>::Construct(p.cP());
            const Point3<typename FACETYPE::ScalarType> fn = Point3<typename FACETYPE::ScalarType>::Construct(p.cN());
            Point3<typename FACETYPE::ScalarType> fq;
            typename FACETYPE::ScalarType md = (typename FACETYPE::ScalarType)(minDist);
            const bool ret=PointDistance(f,fp,md,fq);

            SCALARTYPE  dev=InterPoint()*(pow((ScalarType)(1-f.cN().dot(fn)),(ScalarType)Beta())/(Gamma()*md+0.1));

            if (md+dev < minDist){
                minDist = (SCALARTYPE)(md+dev);
                q = Point3<SCALARTYPE>::Construct(fq);
                //q.N() = f.N();
                return (ret);
            }
            return false;
        }
    };

        /// BASIC VERSION of the Point-face distance that does not require the EdgePlane Additional data.
        /// Given a face and a point, returns the closest point of the face to p.

        template <class FaceType>
            bool PointDistanceBase(
                                                    const FaceType &f,																		/// the face to be tested
                                                    const vcg::Point3<typename FaceType::ScalarType> & q, /// the point tested
                                                    typename FaceType::ScalarType & dist,                 /// bailout distance. It must be initialized with the max admittable value.
                                                    vcg::Point3<typename FaceType::ScalarType> & p )
        {
                typedef typename FaceType::ScalarType ScalarType;

                if(f.cN()==Point3<ScalarType>(0,0,0)) // to correctly manage the case of degenerate triangles we consider them as segments.
                {
                  Box3<ScalarType> bb;
                  f.GetBBox(bb);
                  Segment3<ScalarType> degenTri(bb.min,bb.max);
                  Point3<ScalarType> closest;
                  ScalarType d;
                  if(bb.Diag()>0)
                    vcg::SegmentPointDistance<ScalarType>(degenTri,q,closest,d);
                  else // very degenerate triangle (just a point)
                  {
                    closest = bb.min;
                    d=Distance(q,closest);
                  }
                  if( d>dist) return false;
                  dist=d;
                  p=closest;
                  assert(!math::IsNAN(dist));
                  return true;
                }

                Plane3<ScalarType,true> fPlane;
                fPlane.Init(f.cP(0),f.cN());
                const ScalarType EPS = ScalarType( 0.000001);
                ScalarType b,b0,b1,b2;
                // Calcolo distanza punto piano
                ScalarType d = SignedDistancePlanePoint( fPlane, q );
                if( d>dist || d<-dist )			// Risultato peggiore: niente di fatto
                    return false;

                // Projection of query point onto the triangle plane
                p = q - fPlane.Direction()*d;

                Point3<ScalarType> fEdge[3];
                fEdge[0] = f.cP(1); fEdge[0] -= f.cP(0);
                fEdge[1] = f.cP(2); fEdge[1] -= f.cP(1);
                fEdge[2] = f.cP(0); fEdge[2] -= f.cP(2);

                /*
                This piece of code is part of the EdgePlane initialization structure: note that the edges are scaled!.

                if(nx>ny && nx>nz) { f.Flags() |= FaceType::NORMX; d = 1/f.Plane().Direction()[0]; }
                else if(ny>nz)     { f.Flags() |= FaceType::NORMY; d = 1/f.Plane().Direction()[1]; }
                else               { f.Flags() |= FaceType::NORMZ; d = 1/f.Plane().Direction()[2]; }
                f.Edge(0)*=d; f.Edge(1)*=d;f.Edge(2)*=d;

                So we must apply the same scaling according to the plane orientation, eg in the case of NORMX
                  scaleFactor= 1/fPlane.Direction()[0];
                  fEdge[0]*=d; fEdge[1]*=d;fEdge[2]*=d;
                */

                int bestAxis;
                if(fabs(f.cN()[0])>fabs(f.cN()[1]))
                {
                  if(fabs(f.cN()[0])>fabs(f.cN()[2])) bestAxis = 0;
                  else bestAxis = 2;
                } else {
                  if(fabs(f.cN()[1])>fabs(f.cN()[2])) bestAxis=1; /* 1 > 0 ? 2 */
                  else bestAxis=2; /* 2 > 1 ? 2 */
                }

                ScalarType scaleFactor;

                switch( bestAxis )
                {
                    case 0:  /************* X AXIS **************/
                        scaleFactor= 1/fPlane.Direction()[0];
                        fEdge[0]*=scaleFactor; fEdge[1]*=scaleFactor; fEdge[2]*=scaleFactor;

                        b0 = fEdge[1][1]*(p[2] - f.cP(1)[2]) - fEdge[1][2]*(p[1] - f.cP(1)[1]);
                        if(b0<=0)
                        {
                            b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                            if(dist>b0) { dist = b0; return true; }
                            else return false;
                        }
                            b1 = fEdge[2][1]*(p[2] - f.cP(2)[2]) - fEdge[2][2]*(p[1] - f.cP(2)[1]);
                        if(b1<=0)
                        {
                            b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                            if(dist>b1) { dist = b1; return true; }
                            else return false;
                        }
                            b2 = fEdge[0][1]*(p[2] - f.cP(0)[2]) - fEdge[0][2]*(p[1] - f.cP(0)[1]);
                        if(b2<=0)
                        {
                            b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
                            if(dist>b2) { dist = b2; return true; }
                            else return false;
                        }
                            // sono tutti e tre > 0 quindi dovrebbe essere dentro;
                            // per sicurezza se il piu' piccolo dei tre e' < epsilon (scalato rispetto all'area della faccia
                            // per renderlo dimension independent.) allora si usa ancora la distanza punto
                            // segmento che e' piu robusta della punto piano, e si fa dalla parte a cui siamo piu'
                            // vicini (come prodotto vettore)
                            // Nota: si potrebbe rendere un pochino piu' veloce sostituendo Area()
                            // con il prodotto vettore dei due edge in 2d lungo il piano migliore.
                        if( (b=vcg::math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
                            {
                                ScalarType bt;
                                if(b==b0) 	    bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                                else if(b==b1) 	bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                                else {assert(b==b2); bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);}
                                //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt);
                                if(dist>bt) { dist = bt; return true; }
                                else return false;
                            }
                            break;

                    case 1:  /************* Y AXIS **************/
                        scaleFactor= 1/fPlane.Direction()[1];
                        fEdge[0]*=scaleFactor; fEdge[1]*=scaleFactor; fEdge[2]*=scaleFactor;

                        b0 = fEdge[1][2]*(p[0] - f.cP(1)[0]) - fEdge[1][0]*(p[2] - f.cP(1)[2]);
                        if(b0<=0)
                        {
                            b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                            if(dist>b0) { dist = b0; return true; }
                            else return false;
                        }
                            b1 = fEdge[2][2]*(p[0] - f.cP(2)[0]) - fEdge[2][0]*(p[2] - f.cP(2)[2]);
                        if(b1<=0)
                        {
                            b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                            if(dist>b1) { dist = b1; return true; }
                            else return false;
                        }
                            b2 = fEdge[0][2]*(p[0] - f.cP(0)[0]) - fEdge[0][0]*(p[2] - f.cP(0)[2]);
                        if(b2<=0)
                        {
                            b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
                            if(dist>b2) { dist = b2; return true; }
                            else return false;
                        }
            if( (b=vcg::math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
                            {
                                ScalarType bt;
                                if(b==b0) 	    bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                                else if(b==b1) 	bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                                else{ assert(b==b2); bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);}
                                //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt);
                                if(dist>bt) { dist = bt; return true; }
                                else return false;
                            }
                            break;

                    case 2:  /************* Z AXIS **************/
                        scaleFactor= 1/fPlane.Direction()[2];
                        fEdge[0]*=scaleFactor; fEdge[1]*=scaleFactor; fEdge[2]*=scaleFactor;

                        b0 = fEdge[1][0]*(p[1] - f.cP(1)[1]) - fEdge[1][1]*(p[0] - f.cP(1)[0]);
                        if(b0<=0)
                        {
                            b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                            if(dist>b0) { dist = b0; return true; }
                            else return false;
                        }
                            b1 = fEdge[2][0]*(p[1] - f.cP(2)[1]) - fEdge[2][1]*(p[0] - f.cP(2)[0]);
                        if(b1<=0)
                        {
                            b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                            if(dist>b1) { dist = b1; return true; }
                            else return false;
                        }
                            b2 = fEdge[0][0]*(p[1] - f.cP(0)[1]) - fEdge[0][1]*(p[0] - f.cP(0)[0]);
                        if(b2<=0)
                        {
                            b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
                            if(dist>b2) { dist = b2; return true; }
                            else return false;
                        }
            if( (b=vcg::math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
                            {
                                ScalarType bt;
                                if(b==b0) 	    bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
                                else if(b==b1) 	bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
                                else { assert(b==b2); bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p); }
                                //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt);

                                if(dist>bt) { dist = bt; return true; }
                                else return false;
                            }
                            break;
                        default: assert(0); // if you get this assert it means that you forgot to set the required UpdateFlags<MeshType>::FaceProjection(m);

                }

                dist = ScalarType(fabs(d));
                //dist = Distance(p,q);
                return true;
        }

    template <class S>
    class PointDistanceBaseFunctor {
public:
            typedef S ScalarType;
            typedef Point3<ScalarType> QueryType;

          static inline const Point3<ScalarType> & Pos(const Point3<ScalarType> & qt)  {return qt;}
            template <class FACETYPE, class SCALARTYPE>
            inline bool operator () (const FACETYPE & f, const Point3<SCALARTYPE> & p, SCALARTYPE & minDist, Point3<SCALARTYPE> & q) {
                const Point3<typename FACETYPE::ScalarType> fp = Point3<typename FACETYPE::ScalarType>::Construct(p);
                Point3<typename FACETYPE::ScalarType> fq;
                typename FACETYPE::ScalarType md = (typename FACETYPE::ScalarType)(minDist);
                const bool ret = PointDistanceBase(f, fp, md, fq);
                minDist = (SCALARTYPE)(md);
                q = Point3<SCALARTYPE>::Construct(fq);
                return (ret);
            }
        };



}	 // end namespace face

}	 // end namespace vcg


#endif