/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2015 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library 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 Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                              SOFA :: Framework                              *
*                                                                             *
* Authors: The SOFA Team (see Authors.txt)                                    *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_HELPER_PROJECTTOMESH_H
#define SOFA_HELPER_PROJECTTOMESH_H

#include "initContactMapping.h"

#include <sofa/defaulttype/VecTypes.h>
#include <sofa/helper/vector.h>
#include <sofa/core/topology/BaseMeshTopology.h>
#include <vector>
#include <map>
#include <sofa/helper/logging/Messaging.h>
#include <SofaMeshCollision/RayTriangleIntersection.h>
#include <SofaGeneralMeshCollision/TriangleOctree.h>


namespace sofa
{

namespace helper
{

/**
* This class allows to find the closest point on a mesh from a source point
*
*  @author Benjamin Gilles
**/


template<class Coord>
class ProjectToMesh
{
public:
    typedef typename Coord::value_type Real;
    enum { dim=Coord::total_size };
    typedef helper::vector<Coord> VecCoord;

    typedef typename core::topology::BaseMeshTopology::index_type index_type;
    typedef typename core::topology::BaseMeshTopology::Edge Edge;
    typedef typename core::topology::BaseMeshTopology::SeqEdges SeqEdges;
    typedef typename core::topology::BaseMeshTopology::Triangle Triangle;
    typedef typename core::topology::BaseMeshTopology::SeqTriangles SeqTriangles;
    typedef typename core::topology::BaseMeshTopology::Quad Quad;
    typedef typename core::topology::BaseMeshTopology::SeqQuads SeqQuads;
    typedef typename component::collision::TriangleOctreeRoot TriangleOctreeRoot;
    typedef typename component::collision::TriangleOctree TriangleOctree;
    typedef std::pair<index_type,Real> IndexToWeightPair;

    struct ClosestStruct
    {
        std::set<IndexToWeightPair> indexWeightSet; // barycentric coordinates of the projection (in increasing index order to facilitate insertion in sparse matrix)
        Real dist; // signed distance between point and its projection (positive outside)
        Coord u; // unit vector so that the projected point is c = p - dist.u
        Coord n; // keep track of plane normal or edge unit vector
        bool isPlane; // make the difference between edge and plane projections

        friend std::ostream& operator<<( std::ostream& out, const ClosestStruct& c )
        {
            return out << c.dist << " ";
        }

        friend std::istream& operator>>( std::istream& in, ClosestStruct& /*c*/ )
        {
            msg_warning("ProjectToMesh") << "ClosestStruct is not yet readable";
            std::string bidon;
            in >> bidon;
            return in;
        }

    };


    /// compute closest point on mesh from p (test all primitives)
    void getClosest(ClosestStruct& closest,const Coord& p, const VecCoord& positions, const SeqTriangles& triangles, const SeqQuads& quads, const SeqEdges& edges)
    {
        closest.dist=std::numeric_limits<Real>::max();

        for(size_t i=0;i<triangles.size();++i) Closest_InTriangle(closest,p,positions,triangles[i]);
        for(size_t i=0;i<quads.size();++i) { Closest_InTriangle(closest,p,positions,Triangle(quads[i][0],quads[i][1],quads[i][2])); Closest_InTriangle(closest,p,positions,Triangle(quads[i][0],quads[i][2],quads[i][3])); } //split quad in 2 triangles. Could do better using bilinear interpolation.. but more complicated
        for(size_t i=0;i<edges.size();++i) Closest_InEdge(closest,p,positions,edges[i]);

        bool outside=true;
        if (closest.isPlane) {
            // in that case we need to compute closest.n and outside
            computeNormal(closest, positions, triangles, quads);

            // update outside : use (averaged) normal for in/out test
            Coord pp1=positions[closest.indexWeightSet.begin()->first]-p;
            if(dot(pp1,closest.n)>0) outside=false;
            //  outside = isOutside(closest, p, positions, triangles, quads); // bg: don't know what this function is supposed to fix..
        }

        if(closest.dist<0) closest.dist=0; // fix zero but slightly negative distances
        closest.dist = sqrt(closest.dist); // convert squared dist (used in point-primitive distance computation) to dist
        if(!outside) closest.dist = -closest.dist; // convert to signed distance
        ComputeProjectionVector(closest,p,positions);
    }


    /// compute closest point on mesh from p given its closest vertex c (test only neighboring primitives of c)
    void getClosest(ClosestStruct& closest,const Coord& p, const unsigned int& c, const VecCoord& positions, const SeqTriangles& triangles, const SeqQuads& quads, const SeqEdges& edges)
    {
        if(neighboringTriangles.size()==0 && neighboringQuads.size()==0 && neighboringEdges.size()==0) init(triangles,quads,edges); // TODO: is this init required ?

        closest.dist=std::numeric_limits<Real>::max();

        typename IndexToIndicesMap::iterator it=neighboringTriangles.find(c);
        if(it!=neighboringTriangles.end()) for(index_type iTri: it->second) Closest_InTriangle(closest, p,positions,triangles[iTri]);

        it=neighboringQuads.find(c);
        if(it!=neighboringQuads.end()) for(index_type iQuad: it->second)  {
            Quad quad = quads[iQuad];
            Closest_InTriangle(closest, p,positions,Triangle(quad[0],quad[1],quad[2]));
            Closest_InTriangle(closest, p,positions,Triangle(quad[0],quad[2],quad[3]));
        } //split quad in 2 triangles. Could do better using bilinear interpolation.. but more complicated

        it=neighboringEdges.find(c);
        if(it!=neighboringEdges.end()) for(index_type iEdge: it->second) Closest_InEdge(closest, p,positions,edges[iEdge]);

        bool outside=true;
        if (closest.isPlane) {
            // in that case we need to compute closest.n and outside
            computeNormal(closest, positions, triangles, quads);

            // update outside : use (averaged) normal for in/out test
            Coord pp1=positions[closest.indexWeightSet.begin()->first]-p;
            if(dot(pp1,closest.n)>0) outside=false;
            //  outside = isOutside(closest, p, positions, triangles, quads); // bg: don't know what this function is supposed to fix..
        }
        // else TODO when closest is an edge or vertex, inside can be detected with adjacent faces

        if(closest.dist<0) closest.dist=0; // fix zero but slightly negative distances
        closest.dist = sqrt(closest.dist); // convert squared dist (used in point-primitive distance computation) to dist
        if(!outside) closest.dist = -closest.dist; // convert to signed distance
        ComputeProjectionVector(closest,p,positions);

    }

    /// compute closest point on tangent plane from p given its closest vertex c
    void getClosest(ClosestStruct& closest,const Coord& p, const unsigned int& c, const VecCoord& positions, const VecCoord& normals)
    {
        closest.dist = dot( p - positions[c] , normals[c] );
        closest.u = closest.n = normals[c];
        closest.isPlane = true;
        closest.indexWeightSet.clear();
        closest.indexWeightSet.insert(IndexToWeightPair(c,1));
    }


    /// precompute the one ring neighboring primitives
    /// TODO by working directly on a Topology (given by a singlelink) rather than on list of primitives, this (and more) would already be available
    void init(const SeqTriangles& triangles, const SeqQuads& quads, const SeqEdges& edges)
    {
        neighboringTriangles.clear();
        for(size_t i=0;i<triangles.size();++i)
            for(size_t j=0;j<3;++j)
            {
                typename IndexToIndicesMap::iterator it=neighboringTriangles.find(triangles[i][j]);
                if(it==neighboringTriangles.end()) it=neighboringTriangles.insert( std::make_pair(triangles[i][j], IndexSet())).first;
                it->second.insert(i);
            }
        neighboringQuads.clear();
        for(size_t i=0;i<quads.size();++i)
            for(size_t j=0;j<4;++j)
            {
                typename IndexToIndicesMap::iterator it=neighboringQuads.find(quads[i][j]);
                if(it==neighboringQuads.end()) it=neighboringQuads.insert(std::make_pair(quads[i][j], IndexSet())).first;
                it->second.insert(i);
            }
        neighboringEdges.clear();
        for(size_t i=0;i<edges.size();++i)
            for(size_t j=0;j<2;++j)
            {
                typename IndexToIndicesMap::iterator it=neighboringEdges.find(edges[i][j]);
                if(it==neighboringEdges.end()) it=neighboringEdges.insert(std::make_pair(edges[i][j], IndexSet())).first;
                it->second.insert(i);
            }
    }



    /// compute closest ray/mesh intersection from p given a direction (test all primitives)
    void getRayIntersection(ClosestStruct& closest,const Coord& p, const Coord& direction, const VecCoord& positions, const SeqTriangles& triangles, const SeqQuads& quads, const bool maximizeDistances)
    {
        closest.dist=maximizeDistances?std::numeric_limits<Real>::min():std::numeric_limits<Real>::max();
        closest.isPlane=true;
        closest.indexWeightSet.clear();
        closest.u=direction; closest.u.normalize(Coord(1,1,1).normalized());

        static component::collision::RayTriangleIntersection intersectionSolver;
        SReal t, u, v;

        for(size_t i=0;i<triangles.size();++i)
        {
            const Triangle& triangle = triangles[i];
            if (intersectionSolver.NewComputation(positions[triangle[0]], positions[triangle[1]], positions[triangle[2]], p, closest.u, t, u, v))
                Closest_RayTriangleIntersection(closest,positions,triangle,t,u,v,maximizeDistances);
        }

        for(size_t i=0;i<quads.size();++i)  //split quad in 2 triangles. Could do better using bilinear interpolation.. but more complicated
        {
            Triangle triangle(quads[i][0],quads[i][1],quads[i][2]), triangle2(quads[i][0],quads[i][2],quads[i][3]);
            if (intersectionSolver.NewComputation(positions[triangle[0]], positions[triangle[1]], positions[triangle[2]], p, closest.u, t, u, v))
                Closest_RayTriangleIntersection(closest,positions,triangle,t,u,v,maximizeDistances);
            if (intersectionSolver.NewComputation(positions[triangle2[0]], positions[triangle2[1]], positions[triangle2[2]], p, closest.u, t, u, v))
                Closest_RayTriangleIntersection(closest,positions,triangle2,t,u,v,maximizeDistances);
        }
        closest.u = -closest.u; // invert u so that the projected point is c = p - dist.u
        if(!closest.indexWeightSet.size()) closest.dist=0;
    }



    /// compute closest ray/mesh intersection from p given a direction (use Octree acceleration)
    void getRayIntersection(ClosestStruct& closest,const Coord& p, const Coord& direction, const VecCoord& positions, const TriangleOctreeRoot& octree, const bool maximizeDistances)
    {
        closest.dist=maximizeDistances?std::numeric_limits<Real>::min():std::numeric_limits<Real>::max();
        closest.isPlane=true;
        closest.indexWeightSet.clear();
        closest.u=direction; closest.u.normalize(Coord(1,1,1).normalized());

        const SeqTriangles& triangles = *octree.octreeTriangles;

        if(!maximizeDistances)
        {
            TriangleOctree::traceResult result;
            if(octree.octreeRoot->trace(p, closest.u, result)!=-1)
                Closest_RayTriangleIntersection(closest,positions,triangles[result.tid],result.t,result.u,result.v,maximizeDistances);
        }
        else
        {
            helper::vector< TriangleOctree::traceResult > results;
            octree.octreeRoot->traceAll(p, closest.u, results);
            for (unsigned int i=0; i<results.size(); ++i) Closest_RayTriangleIntersection(closest,positions,triangles[results[i].tid],results[i].t,results[i].u,results[i].v,maximizeDistances);
        }
        closest.u = -closest.u; // invert u so that the projected point is c = p - dist.u
        if(!closest.indexWeightSet.size()) closest.dist=0;
    }



protected :

    typedef std::set<index_type> IndexSet;
    typedef std::map<index_type, IndexSet > IndexToIndicesMap;
    IndexToIndicesMap neighboringTriangles;
    IndexToIndicesMap neighboringQuads;
    IndexToIndicesMap neighboringEdges;



    // compute normal, a mean normal is computed when the closest point is on an edge or is at a vertex
    void computeNormal(ClosestStruct& closest, const VecCoord& positions, const SeqTriangles& triangles, const SeqQuads& quads)
    {
        switch (closest.indexWeightSet.size()) {
        case 1: {
            // closest primitive is a vertex
            IndexToIndicesMap::const_iterator itPrimitive;
            closest.n.clear();
            itPrimitive = neighboringTriangles.find(closest.indexWeightSet.begin()->first);
            if (itPrimitive!=neighboringTriangles.end())
                for (index_type iTriangle : itPrimitive->second ) {
                    Triangle tri = triangles[iTriangle];
                    index_type i1=tri[0],i2=tri[1],i3=tri[2];
                    Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                    closest.n += cross(p1p2,p1p3);
                }
            itPrimitive = neighboringQuads.find(closest.indexWeightSet.begin()->first);
            if (itPrimitive!=neighboringQuads.end())
                for (index_type iQuad : itPrimitive->second ) {
                    Quad quad = quads[iQuad];
                    index_type i1=quad[0],i2=quad[1],i3=quad[2];
                    Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                    closest.n += cross(p1p2,p1p3);
                }
            closest.n.normalize(Coord(1,1,1).normalized());
            break;
        }
        case 2: {
            // closest primitive is an edge
            closest.n.clear();
            Edge edge;
            std::size_t i=0;
            for (IndexToWeightPair p: closest.indexWeightSet) {
                edge[i]=p.first;
                ++i;
            }
            std::size_t count=0; // an edge has 2 faces maximum, we can stop searching when they both have been found.
            IndexToIndicesMap::const_iterator itTri0 = neighboringTriangles.find(edge[0]);
            IndexToIndicesMap::const_iterator itTri1 = neighboringTriangles.find(edge[1]);
            if (itTri0!=neighboringTriangles.end() && itTri1!=neighboringTriangles.end())
                for (index_type iTriangle : itTri0->second) if (itTri1->second.find(iTriangle)!=itTri1->second.end()) {
                    Triangle tri = triangles[iTriangle];
                    index_type i1=tri[0],i2=tri[1],i3=tri[2];
                    Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                    closest.n += cross(p1p2,p1p3);
                    count++;
                    if (count==2) break;
                }
            if (count<2) {
                IndexToIndicesMap::const_iterator itQuad0 = neighboringQuads.find(edge[0]);
                IndexToIndicesMap::const_iterator itQuand1 = neighboringQuads.find(edge[1]);
                if (itQuad0!=neighboringQuads.end() && itQuand1!=neighboringQuads.end())
                    for (index_type iQuad : itQuad0->second) if (itQuand1->second.find(iQuad)!=itQuand1->second.end()) {
                        Quad quad = quads[iQuad];
                        index_type i1=quad[0],i2=quad[1],i3=quad[2];
                        Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                        closest.n += cross(p1p2,p1p3);
                        count++;
                        if (count==2) break;
                    }
            }
            closest.n.normalize(Coord(1,1,1).normalized());
            break;
        }
        case 3:
            // closest primitive is a triangle
            // We have the 3 triangle vertices but do not know their order to compute the correct normal
            // In that case the normal is being computed in Closest_InTriangle
            break;
        default:
            // should never get here !
            break;
        }
    }

    bool isOutside(ClosestStruct& closest, const Coord& p, const VecCoord& positions, const SeqTriangles& triangles, const SeqQuads& quads)
    {
        Coord pp1=positions[closest.indexWeightSet.begin()->first]-p;
        switch (closest.indexWeightSet.size()) {
        case 1: {
            // closest primitive is a vertex
            IndexToIndicesMap::const_iterator itPrimitive;
            itPrimitive = neighboringTriangles.find(closest.indexWeightSet.begin()->first);
            if (itPrimitive!=neighboringTriangles.end())
                for (index_type iTriangle : itPrimitive->second ) {
                    Triangle tri = triangles[iTriangle];
                    index_type i1=tri[0],i2=tri[1],i3=tri[2];
                    Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                    if (dot(pp1,cross(p1p2,p1p3))<0) return true;
                }
            itPrimitive = neighboringQuads.find(closest.indexWeightSet.begin()->first);
            if (itPrimitive!=neighboringQuads.end())
                for (index_type iQuad : itPrimitive->second ) {
                    Quad quad = quads[iQuad];
                    index_type i1=quad[0],i2=quad[1],i3=quad[2];
                    Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                    if (dot(pp1,cross(p1p2,p1p3))<0) return true;
                }
            return false;
        }
        case 2: {
            // closest primitive is an edge
            Edge edge;
            std::size_t i=0;
            for (IndexToWeightPair p: closest.indexWeightSet) {
                edge[i]=p.first;
                ++i;
            }
            std::size_t count=0; // an edge has 2 faces maximum, we can stop searching when they both have been found.
            IndexToIndicesMap::const_iterator itTri0 = neighboringTriangles.find(edge[0]);
            IndexToIndicesMap::const_iterator itTri1 = neighboringTriangles.find(edge[1]);
            if (itTri0!=neighboringTriangles.end() && itTri1!=neighboringTriangles.end())
                for (index_type iTriangle : itTri0->second) if (itTri1->second.find(iTriangle)!=itTri1->second.end()) {
                    Triangle tri = triangles[iTriangle];
                    index_type i1=tri[0],i2=tri[1],i3=tri[2];
                    Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                    if (dot(pp1,cross(p1p2,p1p3))<0) return true;
                    count++;
                    if (count==2) return false;
                }
            if (count<2) {
                IndexToIndicesMap::const_iterator itQuad0 = neighboringQuads.find(edge[0]);
                IndexToIndicesMap::const_iterator itQuand1 = neighboringQuads.find(edge[1]);
                if (itQuad0!=neighboringQuads.end() && itQuand1!=neighboringQuads.end())
                    for (index_type iQuad : itQuad0->second) if (itQuand1->second.find(iQuad)!=itQuand1->second.end()) {
                        Quad quad = quads[iQuad];
                        index_type i1=quad[0],i2=quad[1],i3=quad[2];
                        Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1];
                        if (dot(pp1,cross(p1p2,p1p3))<0) return true;
                        count++;
                        if (count==2) return false;
                    }
            }
            break;
        }
        case 3: {
            // closest primitive is a triangle
            // update outside
            return !(dot(pp1,closest.n)>0);
        }
        default:
            // should never get here !
            break;
        }
        return true;
    }

public:


    // compute unit vector so that the projected point is c = p - dist.u
    static void ComputeProjectionVector(ClosestStruct& closest,const Coord& p,const VecCoord& positions)
    {
        static const Real EPSILON = std::numeric_limits<Real>::epsilon();
        if(fabs(closest.dist)<EPSILON)
        {
            closest.u=closest.n; // use normal as a default contact direction when distance is infinitesimal
        }
        else
        {
            closest.u = p;
            for(typename std::set<IndexToWeightPair>::iterator it=closest.indexWeightSet.begin(); it!=closest.indexWeightSet.end(); it++)
                closest.u -=positions[it->first]*it->second;
            closest.u/=closest.dist;
            // closest.u.normalize();
            // if(closest.dist<0) closest.u=-closest.u;
        }
    }

    // compute projection on an edge, and update 'closest' if necessary
    static void Closest_InEdge(ClosestStruct& closest, const Coord& p, const VecCoord& positions,const Edge& edge)
    {
        index_type i1=edge[0],i2=edge[1];
        Coord p1p2=positions[i2]-positions[i1], p1p=p-positions[i1];
        Real fa=dot(p1p2,p1p2),fd=dot(p1p2,p1p);
        Real fs,dist2;
        if(fd<=0) // region 1
        {
            dist2 = dot(p1p,p1p);
            fs = 0;
        }
        else if(fd>=fa) // region 2
        {
            Coord p2p = p-positions[i2];
            dist2 = dot(p2p,p2p);
            fs = 1;
        }
        else // region 0 (interior)
        {
            fs = fd/fa;
            dist2 = dot(p1p,p1p) - fs*fd;
        }

        if(dist2<closest.dist )
        {
            closest.dist = dist2;
            closest.isPlane=false;
            closest.n=p1p2;
            if(fa>0) closest.n/=sqrt(fa);
            else closest.n=Coord(1,0,0); // if edge has a null length, use arbitrary direction
            closest.indexWeightSet.clear();
            if(fs!=1) closest.indexWeightSet.insert(IndexToWeightPair(i1,1-fs));
            if(fs!=0) closest.indexWeightSet.insert(IndexToWeightPair(i2,fs));
        }

    }

    // compute projection on a triangle, and update 'closest' if necessary
    // returns true iff the current triangle was closer than the given ClosestStruct
    static bool Closest_InTriangle(ClosestStruct& closest, const Coord& p, const VecCoord& positions,const Triangle& triangle)
    {
        index_type i1=triangle[0],i2=triangle[1],i3=triangle[2];
        Coord p1p2=positions[i2]-positions[i1], p1p3=positions[i3]-positions[i1], pp1=positions[i1]-p;

        Real fa=dot(p1p2,p1p2),fb=dot(p1p2,p1p3),fc=dot(p1p3,p1p3),fd=dot(p1p2,pp1),fe=dot(p1p3,pp1),ff=dot(pp1,pp1);
        Real fdet=fabs(fa*fc-fb*fb), fs=fb*fe-fc*fd, ft=fb*fd-fa*fe;
        Real dist2;

        if ( fs + ft <= fdet )
        {
            if ( fs < 0 )
            {
                if ( ft < 0 )  // region 4
                {
                    if ( fd < 0 )
                    {
                        ft = 0;
                        if ( -fd >= fa ) { fs = 1; dist2 = fa+2*fd+ff; }
                        else { fs = -fd/fa; dist2 = fd*fs+ff; }
                    }
                    else
                    {
                        fs = 0;
                        if ( fe >= 0 ) { ft = 0; dist2 = ff; }
                        else if ( -fe >= fc ) { ft = 1; dist2 = fc+2*fe+ff; }
                        else { ft = -fe/fc; dist2 = fe*ft+ff; }
                    }
                }
                else  // region 3
                {
                    fs = 0;
                    if ( fe >= 0 ) { ft = 0; dist2 = ff; }
                    else if ( -fe >= fc ) { ft = 1; dist2 = fc+2*fe+ff; }
                    else { ft = -fe/fc; dist2 = fe*ft+ff; }
                }
            }
            else if ( ft < 0 )  // region 5
            {
                ft = 0;
                if ( fd >= 0 ) { fs = 0; dist2 = ff; }
                else if ( -fd >= fa ) { fs = 1; dist2 = fa+2*fd+ff; }
                else { fs = -fd/fa; dist2 = fd*fs+ff; }
            }
            else  // region 0
            {
                // minimum at interior point
                Real fInvDet = 1./fdet;
                fs *= fInvDet;
                ft *= fInvDet;
                dist2 = fs*(fa*fs+fb*ft+2*fd) +  ft*(fb*fs+fc*ft+2*fe)+ff;
            }
        }
        else
        {
            Real ftmp0, ftmp1, fNumer, fDenom;

            if ( fs < 0 )  // region 2
            {
                ftmp0 = fb + fd;
                ftmp1 = fc + fe;
                if ( ftmp1 > ftmp0 )
                {
                    fNumer = ftmp1 - ftmp0;
                    fDenom = fa-2*fb+fc;
                    if ( fNumer >= fDenom ) { fs = 1; ft = 0; dist2 = fa+2*fd+ff; }
                    else { fs = fNumer/fDenom; ft = 1 - fs; dist2 = fs*(fa*fs+fb*ft+2*fd) +   ft*(fb*fs+fc*ft+2*fe)+ff; }
                }
                else
                {
                    fs = 0;
                    if ( ftmp1 <= 0 ) { ft = 1; dist2 = fc+2*fe+ff; }
                    else if ( fe >= 0 ) { ft = 0; dist2 = ff; }
                    else { ft = -fe/fc; dist2 = fe*ft+ff; }
                }
            }
            else if ( ft < 0 )  // region 6
            {
                ftmp0 = fb + fe;
                ftmp1 = fa + fd;
                if ( ftmp1 > ftmp0 )
                {
                    fNumer = ftmp1 - ftmp0;
                    fDenom = fa-2*fb+fc;
                    if ( fNumer >= fDenom ) { ft = 1; fs = 0; dist2 = fc+2*fe+ff; }
                    else { ft = fNumer/fDenom; fs = 1 - ft; dist2 = fs*(fa*fs+fb*ft+2*fd) +   ft*(fb*fs+fc*ft+2*fe)+ff; }
                }
                else
                {
                    ft = 0;
                    if ( ftmp1 <= 0 ) { fs = 1; dist2 = fa+2*fd+ff; }
                    else if ( fd >= 0 ) { fs = 0; dist2 = ff; }
                    else { fs = -fd/fa; dist2 = fd*fs+ff; }
                }
            }
            else  // region 1
            {
                fNumer = fc + fe - fb - fd;
                if ( fNumer <= 0 ) { fs = 0; ft = 1; dist2 = fc+2*fe+ff; }
                else
                {
                    fDenom = fa-2*fb+fc;
                    if ( fNumer >= fDenom ) { fs = 1; ft = 0; dist2 = fa+2*fd+ff; }
                    else { fs = fNumer/fDenom; ft = 1 - fs; dist2 = fs*(fa*fs+fb*ft+2*fd) +         ft*(fb*fs+fc*ft+2*fe)+ff; }
                }
            }
        }

        if(dist2<closest.dist)
        {
            closest.dist = dist2;
            closest.isPlane=true;
            closest.indexWeightSet.clear();
            if(fs!=0) closest.indexWeightSet.insert(IndexToWeightPair(i2,fs));
            if(ft!=0) closest.indexWeightSet.insert(IndexToWeightPair(i3,ft));
            if(fs+ft!=1) closest.indexWeightSet.insert(IndexToWeightPair(i1,1-fs-ft));
            if (closest.indexWeightSet.size()==3)
            {
                // minimum at interior point, in that case the normal can be computed straight away
                closest.n=cross(p1p2,p1p3);
                closest.n.normalize(Coord(1,1,1).normalized());
            }
            return true;
        }
        return false;
    }


    // update 'closest' given ray/triangle intersection
    static void Closest_RayTriangleIntersection(ClosestStruct& closest, const VecCoord& positions,const Triangle& triangle, const SReal& t, const SReal& u, const SReal& v, const bool maximizeDistances)
    {
        if( (t<closest.dist && !maximizeDistances) || (t>closest.dist && maximizeDistances))
        {
            closest.dist = t;
            closest.indexWeightSet.clear();
            if(u!=0) closest.indexWeightSet.insert(IndexToWeightPair(triangle[1],u));
            if(v!=0) closest.indexWeightSet.insert(IndexToWeightPair(triangle[2],v));
            if(u+v!=1) closest.indexWeightSet.insert(IndexToWeightPair(triangle[0],1-u-v));
            closest.n=cross(positions[triangle[1]]-positions[triangle[0]],positions[triangle[2]]-positions[triangle[0]]);
            closest.n.normalize(Coord(1,1,1).normalized());
        }
    }


    // modified version of component::collision::RayTriangleIntersection (different thresholds)
    /*bool RayTriangleIntersection(const Coord &p1, const Coord &p2, const Coord &p3, const Coord &origin, const Coord &direction,   Real &t,  Real &u, Real &v)
{
    static const Real TOL_W = 1E-10;
    static const Real MIN_DET = 1.0e-100;

    t = 0; u = 0; v = 0;

    Coord edge1 = p2 - p1;
    Coord edge2 = p3 - p1;

    Coord tvec, pvec, qvec;
    Real det, inv_det;

    pvec = direction.cross(edge2);

    det = dot(edge1, pvec);
    if(det<=MIN_DET && det >=-MIN_DET)
    {
        return false;
    }

    inv_det = 1.0 / det;

    tvec = origin - p1;

    u = dot(tvec, pvec) * inv_det;
    if (u < -TOL_W || u > 1+TOL_W)
        return false;

    qvec = tvec.cross(edge1);

    v = dot(direction, qvec) * inv_det;
    if (v < -TOL_W || (u + v) > 1+TOL_W)
        return false;

    t = dot(edge2, qvec) * inv_det;

    //        if (t < 0.0000001 || t!=t || v!=v || u!=u)
    //            return false;

    return true;
}*/


};



/// to iterate only on valid index pairs
///
/// indexPairsSize is the size of the complete vector of index pairs
/// validPairs is an optional list of valid indices. It is considered only when validPairUsed is true (otherwise every pairs are considered as valid).
class IndexPairIterator
{
    bool m_validPairUsed; ///< is validPairs used?
    std::list<size_t>::const_iterator m_valid_it; ///< m_counter_th validPairs index (only valid when m_validPairUsed is true)

    const size_t m_outputSize; ///< output size
    size_t m_counter; ///< output index

public:

    /// indexPairsSize: total size of index pairs
    /// validPairs: list of indices of valid pairs
    /// validPairUsed: should validPairs be used?
    IndexPairIterator( size_t indexPairsSize, const std::list<size_t>& validPairs, bool validPairUsed )
        : m_validPairUsed( validPairUsed )
        , m_outputSize( validPairUsed ? validPairs.size() : indexPairsSize )
        , m_counter(0)
    {
        //        std::cerr<<"IndexPairIterator "<<validPairs<<std::endl;

        if( m_validPairUsed )
            m_valid_it = validPairs.begin();
    }

    /// get the index of the current valid pair
    size_t get() const
    {
        assert( m_counter<m_outputSize );

        //        std::cerr<<"IndexPairIterator "<<m_counter<<" "<<m_validPairUsed<<" "<<(m_validPairUsed?*m_valid_it:-1)<<std::endl;

        if( m_validPairUsed )
            return *m_valid_it;
        else
            return m_counter;
    }

    /// get the counter of the current valid pair
    size_t count() const
    {
        return m_counter;
    }

    /// get the total amount of valid pairs
    size_t size() const
    {
        return m_outputSize;
    }

    /// go to the next valid pair
    void operator++()
    {
        if( m_validPairUsed )
            m_valid_it++;

        m_counter++;
    }

};



} // helper
} // sofa

#endif
