/******************************************************************************
*                 SOFA, Simulation Open-Framework Architecture                *
*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
*                                                                             *
* This program 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 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 Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include <CollisionOBBCapsule/detection/intersection/IntrMeshUtility.h>
#include <CollisionOBBCapsule/detection/intersection/IntrUtility3.inl>


namespace collisionobbcapsule::detection::intersection
{
template <class DataTypes>
void IntrUtil<TTriangle<DataTypes> >::triFaceNearestPoints(const IntrTri & tri,const type::Vec<3,Real> *second_face,int second_size, type::Vec<3,Real> &pt_on_first, type::Vec<3,Real> &pt_on_second){
    Real min1 = std::numeric_limits<Real>::max();
    Real min2 = std::numeric_limits<Real>::max();
    Real new_min;
    int first_index1 = -1;
    int first_index2 = -1;
    int second_index1 = -1;
    int second_index2 = -1;

    for(int i = 0 ; i < 3 ; ++i){
        for(int j = 0 ; j < second_size ; ++j){
            new_min = (tri[i] - second_face[j]).norm2();
            if(min1 > new_min){
                min2 = min1;
                min1 = new_min;
                first_index2 = first_index1;
                second_index2 = second_index1;

                first_index1 = i;
                second_index1 = j;
            }
            else if(min2 > new_min){
                first_index2 = i;
                second_index2 = j;
                min2 = new_min;
            }
        }
    }

    if(min2 > min1 - IntrUtil<Real>::ZERO_TOLERANCE() && min2 < min1 + IntrUtil<Real>::ZERO_TOLERANCE()){
        pt_on_first = (tri[first_index1] + tri[first_index2])/2.0;
        pt_on_second = (second_face[second_index1] + second_face[second_index2])/2.0;
    }
    else{
        pt_on_first = tri[first_index1];
        pt_on_second = second_face[second_index1];
    }
}

template <typename DataType>
SReal IntrUtil<TTriangle<DataType> >::triSegNearestPoints(const IntrTri & tri,const type::Vec<3,Real> seg[2], type::Vec<3,Real> & pt_on_face,type::Vec<3,Real> & pt_on_seg){
    Real min = std::numeric_limits<Real>::max();
    type::Vec<3,Real> cur_pt_on_face,cur_pt_on_seg;
    type::Vec<3,Real> face_seg[2];
    Real new_min;

    for(int j = 0 ; j < 3 ; ++j){
        face_seg[0] = tri.p(j);
        if(j < 2){
            face_seg[1] = tri.p(j + 1);
        }
        else{
            face_seg[1] = tri.p(0);
        }

        IntrUtil<Real>::segNearestPoints(face_seg,seg,cur_pt_on_face,cur_pt_on_seg);

        if((new_min = (cur_pt_on_face - cur_pt_on_seg).norm2()) < min){
            min = new_min;
            pt_on_face = cur_pt_on_face;
            pt_on_seg = cur_pt_on_seg;
        }
    }

    return min;
}


template <class DataType>
typename IntrUtil<TTriangle<DataType> >::Real IntrUtil<TTriangle<DataType> >::project(type::Vec<3,Real> & pt,const TTriangle<DataType> & tri){
    Real s,t;
    return IntrUtil<Real>::projectOnTriangle(pt,tri.p1(),tri.p2(),tri.p3(),s,t);
}


template <class TDataTypes1,class TDataTypes2>
bool IntrAxis<TTriangle<TDataTypes1>,TOBB<TDataTypes2> >::Find (const Coord& axis,
    const IntrTri & triangle, const Box& box,
    Real dmax, Real& tfirst,
    int& side, IntrConfiguration<Real>& triCfgFinal,
    IntrConfiguration<Real>& boxCfgFinal,bool & config_modified)
{
    IntrConfiguration<Real> triCfgStart;
    IntrConfigManager<IntrTri>::init(axis, triangle, triCfgStart);

    IntrConfiguration<Real> boxCfgStart;
    IntrConfigManager<Box>::init(axis, box, boxCfgStart);

    return IntrConfigManager<Real>::Find(triCfgStart, boxCfgStart, side,
        triCfgFinal, boxCfgFinal, dmax,tfirst, config_modified);
}

template <class DataTypes>
void IntrConfigManager<TTriangle<DataTypes> >::init (const Coord& axis,
    const IntrTri & triangle, IntrConfiguration<Real>& cfg)
{
    // Find projections of vertices onto potential separating axis.
    Real d0 = axis * triangle.p1();
    Real d1 = axis * triangle.p2();
    Real d2 = axis * triangle.p3();

    // Explicit sort of vertices to construct a IntrConfiguration.
    if (IntrUtil<Real>::inf(d0,d1))
    {
        if (IntrUtil<Real>::inf(d1,d2)) // D0 <= D1 <= D2
        {
            if (IntrUtil<Real>::nequal(d0,d1))
            {
                if (IntrUtil<Real>::nequal(d1,d2))
                {
                    cfg.mMap = IntrConfiguration<Real>::m111;
                }
                else
                {
                    cfg.mMap = IntrConfiguration<Real>::m12;
                }
            }
            else // ( D0 == D1 )
            {
                if (IntrUtil<Real>::nequal(d1,d2))
                {
                    cfg.mMap = IntrConfiguration<Real>::m21;
                }
                else
                {
                    cfg.mMap = IntrConfiguration<Real>::m3;
                }
            }
            cfg.mIndex[0] = 0;
            cfg.mIndex[1] = 1;
            cfg.mIndex[2] = 2;
            cfg.mMin = d0;
            cfg.mMax = d2;
        }
        else if (IntrUtil<Real>::inf(d0,d2)) // D0 <= D2 < D1
        {
            if (IntrUtil<Real>::nequal(d0,d2))
            {
                cfg.mMap = IntrConfiguration<Real>::m111;
                cfg.mIndex[0] = 0;
                cfg.mIndex[1] = 2;
                cfg.mIndex[2] = 1;
            }
            else
            {
                cfg.mMap = IntrConfiguration<Real>::m21;
                cfg.mIndex[0] = 2;
                cfg.mIndex[1] = 0;
                cfg.mIndex[2] = 1;
            }
            cfg.mMin = d0;
            cfg.mMax = d1;
        }
        else // D2 < D0 <= D1
        {
            if (IntrUtil<Real>::nequal(d0,d1))
            {
                cfg.mMap = IntrConfiguration<Real>::m111;
            }
            else
            {
                cfg.mMap = IntrConfiguration<Real>::m12;
            }

            cfg.mIndex[0] = 2;
            cfg.mIndex[1] = 0;
            cfg.mIndex[2] = 1;
            cfg.mMin = d2;
            cfg.mMax = d1;
        }
    }
    else if (IntrUtil<Real>::inf(d2,d1)) // D2 <= D1 < D0
    {
        if (IntrUtil<Real>::nequal(d2,d1))
        {
            cfg.mMap = IntrConfiguration<Real>::m111;
            cfg.mIndex[0] = 2;
            cfg.mIndex[1] = 1;
            cfg.mIndex[2] = 0;
        }
        else
        {
            cfg.mMap = IntrConfiguration<Real>::m21;
            cfg.mIndex[0] = 1;
            cfg.mIndex[1] = 2;
            cfg.mIndex[2] = 0;

        }
        cfg.mMin = d2;
        cfg.mMax = d0;
    }
    else if (IntrUtil<Real>::inf(d2,d0)) // D1 < D2 <= D0
    {
        if (IntrUtil<Real>::nequal(d2,d0))
        {
            cfg.mMap = IntrConfiguration<Real>::m111;
        }
        else
        {
            cfg.mMap = IntrConfiguration<Real>::m12;
        }

        cfg.mIndex[0] = 1;
        cfg.mIndex[1] = 2;
        cfg.mIndex[2] = 0;
        cfg.mMin = d1;
        cfg.mMax = d0;
    }
    else // D1 < D0 < D2
    {
        cfg.mMap = IntrConfiguration<Real>::m111;
        cfg.mIndex[0] = 1;
        cfg.mIndex[1] = 0;
        cfg.mIndex[2] = 2;
        cfg.mMin = d1;
        cfg.mMax = d2;
    }
}

template <class TDataTypes1,class TDataTypes2>
FindContactSet<TTriangle<TDataTypes1>,TOBB<TDataTypes2> >::FindContactSet (const IntrTri& tri,
    const Box& box, const type::Vec<3,Real> & axis,int side, const IntrConfiguration<Real>& triCfg,
    const IntrConfiguration<Real>& boxCfg,Real tfirst,
    type::Vec<3,Real> & pt_on_tri,type::Vec<3,Real> & pt_on_box)
{
    const int* tIndex = triCfg.mIndex;
    const int* bIndex = boxCfg.mIndex;

    if (side == IntrConfiguration<Real>::LEFT)
    {
        // box on left of tri
        if (triCfg.mMap == IntrConfiguration<Real>::m111
        ||  triCfg.mMap == IntrConfiguration<Real>::m12)//triangle's vertex
        {
            //P[0] = triFinal[tIndex[0]];
            pt_on_tri = tri.p(tIndex[0]);
            pt_on_box = pt_on_tri;
            IntrUtil<Box>::project(pt_on_box,box);
            //assert(box.onSurface(pt_on_box));
        }
        else if (boxCfg.mMap == IntrConfiguration<Real>::m1_1)//box's vertex
        {
            //P[0] = GetPointFromIndex(bIndex[7], boxFinal);
            pt_on_box = getPointFromIndex(bIndex[7], box);
            pt_on_tri = pt_on_box;
            IntrUtil<IntrTri>::project(pt_on_tri,tri);
            //assert(box.onSurface(pt_on_box));
        }
        else if (triCfg.mMap == IntrConfiguration<Real>::m21)
        {
            if (boxCfg.mMap == IntrConfiguration<Real>::m2_2)
            {
                // triseg-boxseg intersection
                type::Vec<3,Real> triSeg[2], boxSeg[2];
                triSeg[0] = tri.p(tIndex[0]);
                triSeg[1] = tri.p(tIndex[1]);
                boxSeg[0] = getPointFromIndex(bIndex[6], box);
                boxSeg[1] = getPointFromIndex(bIndex[7], box);
                IntrUtil<Real>::segNearestPoints(triSeg,boxSeg,pt_on_tri,pt_on_box);                
                //assert(box.onSurface(pt_on_box));
            }
            else // boxCfg.mMap == IntrConfiguration<Real>::m44, triangles'edge box's face
            {
                int quantity;
                type::Vec<3,Real> P[2];

                type::Vec<3,Real> triFinal[3] =
                {
                    tri.p(0) - tfirst*axis,
                    tri.p(1) - tfirst*axis,
                    tri.p(2) - tfirst*axis,
                };

                // triseg-boxface intersection
                type::Vec<3,Real> triSeg[2];
                type::Vec<3,Real> boxFace[4];
                triSeg[0] = triFinal[tIndex[0]];
                triSeg[1] = triFinal[tIndex[1]];
                boxFace[0] = getPointFromIndex(bIndex[4], box);
                boxFace[1] = getPointFromIndex(bIndex[5], box);
                boxFace[2] = getPointFromIndex(bIndex[6], box);
                boxFace[3] = getPointFromIndex(bIndex[7], box);
                IntrUtil<Real>::CoplanarSegmentRectangle(triSeg, boxFace, quantity, P);

                if(quantity != 0){
                    IntrUtil<Real>::projectIntPoints(axis,tfirst,P,quantity,pt_on_tri);
                    pt_on_box = pt_on_tri - axis * tfirst;
                }
                else{
                    IntrUtil<Real>::faceSegNearestPoints(boxFace,triSeg,pt_on_box,pt_on_tri);
                    pt_on_tri += tfirst * axis;
                }
            }
        }
        else // triCfg.mMap == IntrConfiguration<Real>::m3, triangle's face
        {
            int quantity;
            type::Vec<3,Real> P[6];

            type::Vec<3,Real> triFinal[3] =
            {
                tri.p(0) - tfirst*axis,
                tri.p(1) - tfirst*axis,
                tri.p(2) - tfirst*axis,
            };

            if (boxCfg.mMap == IntrConfiguration<Real>::m2_2)//box's edge
            {
                // boxseg-triface intersection
                type::Vec<3,Real> boxSeg[2];
                boxSeg[0] = getPointFromIndex(bIndex[6], box);
                boxSeg[1] = getPointFromIndex(bIndex[7], box);
                IntrUtil<Real>::ColinearSegmentTriangle(boxSeg, triFinal, quantity, P);

                if(quantity != 0){
                    IntrUtil<Real>::projectIntPoints(axis,tfirst,P,quantity,pt_on_tri);
                    pt_on_box = pt_on_tri - tfirst * axis;
                    //assert(box.onSurface(pt_on_box));
                }
                else{
                    IntrUtil<IntrTri>::triSegNearestPoints(tri,boxSeg,pt_on_tri,pt_on_box);
                    //assert(box.onSurface(pt_on_box));
                }
            }
            else
            {
                // triface-boxface intersection
                type::Vec<3,Real> boxFace[4];
                boxFace[0] = getPointFromIndex(bIndex[4], box);
                boxFace[1] = getPointFromIndex(bIndex[5], box);
                boxFace[2] = getPointFromIndex(bIndex[6], box);
                boxFace[3] = getPointFromIndex(bIndex[7], box);

                IntrUtil<Real>::CoplanarTriangleRectangle(triFinal, boxFace, quantity, P);

                if(quantity != 0){
                    IntrUtil<Real>::projectIntPoints(axis,tfirst,P,quantity,pt_on_tri);
                    pt_on_box = pt_on_tri - tfirst * axis;
                    //assert(box.onSurface(pt_on_box));
                }
                else{
                    IntrUtil<IntrTri>::triFaceNearestPoints(tri,boxFace,4,pt_on_tri,pt_on_box);
                    //assert(box.onSurface(pt_on_box));
                }
            }
        }
    }
    else // side == RIGHT
    {
        // box on right of tri
        if (triCfg.mMap == IntrConfiguration<Real>::m111
        ||  triCfg.mMap == IntrConfiguration<Real>::m21)//triangle's vertex
        {
            pt_on_tri = tri.p(tIndex[2]);
            pt_on_box = pt_on_tri;
            IntrUtil<Box>::project(pt_on_box,box);
        }
        else if (boxCfg.mMap == IntrConfiguration<Real>::m1_1)//box's vertex
        {
            pt_on_box = getPointFromIndex(bIndex[0], box);
            pt_on_tri = pt_on_box;
            IntrUtil<IntrTri>::project(pt_on_tri,tri);
        }
        else if (triCfg.mMap == IntrConfiguration<Real>::m12)//triangle's edge
        {
            if (boxCfg.mMap == IntrConfiguration<Real>::m2_2)//box's edge
            {
                // segment-segment intersection
                type::Vec<3,Real> triSeg[2], boxSeg[2];
                triSeg[0] = tri[tIndex[1]];
                triSeg[1] = tri[tIndex[2]];
                boxSeg[0] = getPointFromIndex(bIndex[0], box);
                boxSeg[1] = getPointFromIndex(bIndex[1], box);

                IntrUtil<Real>::segNearestPoints(triSeg,boxSeg,pt_on_tri,pt_on_box);
            }
            else // boxCfg.mMap == IntrConfiguration<Real>::m44, box's face
            {
                type::Vec<3,Real> P[2];
                int quantity;
                type::Vec<3,Real> triFinal[3] =
                {
                    tri.p(0) + tfirst*axis,
                    tri.p(1) + tfirst*axis,
                    tri.p(2) + tfirst*axis,
                };

                // triseg-boxface intersection
                type::Vec<3,Real> triSeg[2], boxFace[4];
                triSeg[0] = triFinal[tIndex[1]];
                triSeg[1] = triFinal[tIndex[2]];

                boxFace[0] = getPointFromIndex(bIndex[0], box);
                boxFace[1] = getPointFromIndex(bIndex[1], box);
                boxFace[2] = getPointFromIndex(bIndex[2], box);
                boxFace[3] = getPointFromIndex(bIndex[3], box);


                IntrUtil<Real>::CoplanarSegmentRectangle(triSeg, boxFace, quantity, P);

                if(quantity != 0){
                    IntrUtil<Real>::projectIntPoints(-axis,tfirst,P,quantity,pt_on_tri);
                    pt_on_box = pt_on_tri + tfirst * axis;
                }
                else{
                    IntrUtil<Real>::faceSegNearestPoints(boxFace,4,triSeg,pt_on_box,pt_on_tri);
                    pt_on_tri -= tfirst * axis;
                }
            }
        }
        else // triCfg.mMap == IntrConfiguration<Real>::m3
        {
            type::Vec<3,Real> P[6];
            int quantity;
            type::Vec<3,Real> triFinal[3] =
            {
                tri.p(0) + tfirst*axis,
                tri.p(1) + tfirst*axis,
                tri.p(2) + tfirst*axis,
            };

            if (boxCfg.mMap == IntrConfiguration<Real>::m2_2)
            {
                // boxseg-triface intersection
                type::Vec<3,Real> boxSeg[2];
                boxSeg[0] = getPointFromIndex(bIndex[0], box);
                boxSeg[1] = getPointFromIndex(bIndex[1], box);

                IntrUtil<Real>::ColinearSegmentTriangle(boxSeg, triFinal, quantity, P);

                if(quantity != 0){
                    IntrUtil<Real>::projectIntPoints(-axis,tfirst,P,quantity,pt_on_tri);
                    pt_on_box = pt_on_tri + tfirst * axis;
                }
                else{
                    IntrUtil<IntrTri>::triSegNearestPoints(tri,boxSeg,pt_on_tri,pt_on_box);
                }
            }
            else
            {
                // triface-boxface intersection
                type::Vec<3,Real> boxFace[4];
                boxFace[0] = getPointFromIndex(bIndex[0], box);
                boxFace[1] = getPointFromIndex(bIndex[1], box);
                boxFace[2] = getPointFromIndex(bIndex[2], box);
                boxFace[3] = getPointFromIndex(bIndex[3], box);

                IntrUtil<Real>::CoplanarTriangleRectangle(triFinal, boxFace, quantity, P);

                if(quantity != 0){
                    IntrUtil<Real>::projectIntPoints(-axis,tfirst,P,quantity,pt_on_tri);
                    pt_on_box = pt_on_tri + tfirst * axis;
                }
                else{
                    IntrUtil<IntrTri>::triFaceNearestPoints(tri,boxFace,4,pt_on_tri,pt_on_box);
                }
            }
        }
    }
}


} // namespace collisionobbcapsule::detection::intersection
