/*---------------------------------------------------------------------------*\

    geophase: A C++ template library for geometrical operations on non-convex
    polyhedra with non-planar faces
     
    Copyright (C) 2019 Tomislav Maric, TU Darmstadt 

    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 3 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 for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

Description
    Intersection calculation algorithms. 

Author
    Tomislav Maric maric@mma.tu-darmstadt.de 

\*---------------------------------------------------------------------------*/

#ifndef Intersect_HPP
#define Intersect_HPP

#include "Distance.hpp"
#include "DoIntersect.hpp"
#include "tolerance.hpp"
#include "Orient.hpp"
#include "PolygonIntersection.hpp"
#include "PolyhedronIntersection.hpp"

// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //

namespace geophase { 

    template <typename Vector, typename Halfspace>
    enable_if_types_tagged_t<Vector, Vector, vector_tag, Halfspace, halfspace_tag>
    intersect_float(Vector const& x1, Vector const& x2, Halfspace const& hspace)
    {
        return x1 + (dot((hspace.p()-x1),hspace.n()) / dot((x2-x1),hspace.n())) * (x2 - x1);
    }

    template <typename Halfspace, typename Vector>
    enable_if_types_tagged_t<Vector, Vector, vector_tag, Halfspace, halfspace_tag>
    intersect_float(Halfspace const& hspace, Vector const& x1, Vector const& x2)
    {
        return intersect(x1,x2,hspace);  
    }

    template
    <
        typename Polygon,  
        typename Halfspace, 
        typename IntersectionTest = no_test, 
        typename IntersectionResult = PolygonIntersection<Polygon> 
    >
    std::enable_if_t
    < 
        tag_base_of<Polygon, sequence_polygon_tag>() && 
        type_tagged<Halfspace, halfspace_tag>(), 
        void
    >
    intersect_tolerance(
        IntersectionResult& result,
        Polygon const& polygon, 
        Halfspace const& halfspace,
        double tolerance = std::numeric_limits<double>::epsilon()
    )
    {
        typedef typename Polygon::value_type Point;

        if ((polygon.size() > 2) && IntersectionTest::apply(polygon, halfspace))
        {
            auto curPointIt = polygon.begin();
            auto nextPointIt = std::next(curPointIt); 
            auto end = polygon.end(); 

            auto curDist = signed_distance(halfspace, *curPointIt); 
            auto nextDist = signed_distance(halfspace, *nextPointIt); 

            // Compute the intersection for the first edge. 
            // Tolerance based INSIDE and COINCIDENT points.
            if (curDist >= (-2 * tolerance))
            {
                result.addPoint(*curPointIt); 
                if (curDist <=  (2 * tolerance))
                    result.addIntersectionPoint(*curPointIt); 
            }

            // Tolerance based INTERSECTION points.
            if (
                (curDist * nextDist < 0) &&
                (std::fabs(curDist) > (2 * tolerance)) && 
                (std::fabs(nextDist) > (2 * tolerance)) 
            )
            {
                Point intersectionPoint = intersect_float(
                    *nextPointIt, 
                    *curPointIt, 
                    halfspace
                ); 

                result.addPoint(intersectionPoint);
                result.addIntersectionPoint(intersectionPoint); 
            }

            ++curPointIt;
            ++nextPointIt;

            //while (nextPointIt != end) 
            for(; nextPointIt != end; ++curPointIt, ++nextPointIt)
            {
                curDist = nextDist;  
                nextDist = signed_distance(halfspace, *nextPointIt); 
                // Tolerance based INSIDE and COINCIDENT points.
                if (curDist >= (-2 * tolerance))
                {
                    result.addPoint(*curPointIt); 
                    if (curDist <=  (2 * tolerance))
                        result.addIntersectionPoint(*curPointIt); 
                }

                // Tolerance based INTERSECTION points.
                if (
                    (curDist * nextDist < 0) &&
                    (std::fabs(curDist) > (2 * tolerance)) && 
                    (std::fabs(nextDist) > (2 * tolerance)) 
                )
                {
                    Point intersectionPoint = intersect_float(
                        *nextPointIt, 
                        *curPointIt, 
                        halfspace
                    ); 

                    result.addPoint(intersectionPoint);
                    result.addIntersectionPoint(intersectionPoint); 
                }

            } 

            curPointIt = std::prev(polygon.end()); 
            nextPointIt = polygon.begin(); 

            curDist = nextDist; 
            nextDist = signed_distance(halfspace, *nextPointIt); 
                   
            // Tolerance based INSIDE and COINCIDENT points.
            if (curDist >= (-2 * tolerance))
            {
                result.addPoint(*curPointIt); 
                if (curDist <=  (2 * tolerance))
                    result.addIntersectionPoint(*curPointIt); 
            }

            // Tolerance based INTERSECTION points.
            if (
                (curDist * nextDist < 0) &&
                (std::fabs(curDist) > (2 * tolerance)) && 
                (std::fabs(nextDist) > (2 * tolerance)) 
            )
            {
                Point intersectionPoint = intersect_float(
                    *nextPointIt, 
                    *curPointIt, 
                    halfspace
                ); 

                result.addPoint(intersectionPoint);
                result.addIntersectionPoint(intersectionPoint); 
            }
        }
    }

    // Reverse polygon / halfspace intersection. 
    template
    <
        typename Halfspace, 
        typename Polygon,  
        typename IntersectionTest = no_test, 
        typename IntersectionResult = PolygonIntersection<Polygon> 
    >
    std::enable_if_t
    < 
        tag_base_of<Polygon, sequence_polygon_tag>() && 
        type_tagged<Halfspace, halfspace_tag>(), 
        //IntersectionResult
        void
    >
    intersect_tolerance(
        IntersectionResult& result,
        Halfspace const& halfspace, 
        Polygon const& polygon,
        double tolerance = std::numeric_limits<double>::epsilon()
    )
    {
        intersect_tolerance
        <
            Polygon,  
            Halfspace, 
            IntersectionTest, 
            IntersectionResult
        >(result, polygon, halfspace, tolerance);
    }

    // Intersection of a polygon sequence and a halfspace.
    template
    <
        typename IntersectionResult = vectorPolyhedronIntersection, 
        typename IntersectionTest = no_test, 
        typename PolygonSequence,  
        typename Halfspace 
    >
    std::enable_if_t
    <
        tag_base_of<PolygonSequence, sequence_polyhedron_tag>() &&
        type_tagged<Halfspace, halfspace_tag>(), 
        IntersectionResult
    >
    intersect_tolerance(
        PolygonSequence const& polyh, 
        Halfspace const& hspace,
        double tolerance = std::numeric_limits<double>::epsilon() 
    )
    {
        IntersectionResult result;

        if (IntersectionTest::apply(polyh, hspace))
        {
            using Polygon = typename PolygonSequence::value_type; 
            
            PolygonIntersection<Polygon> polygonIntersection;

            for (const auto& polyg : polyh)
            {
                polygonIntersection.reset();
                intersect_tolerance(
                    polygonIntersection, 
                    polyg, 
                    hspace, 
                    tolerance
                ); 

                const auto& clippedPolygon = polygonIntersection.polygon(); 
                const auto& intersectionPoints = polygonIntersection.intersectionPoints();

                bool clippedPolygonIsCoplanar = true; 

                for (const auto& polyPoint : clippedPolygon) 
                {
                    if (signed_distance(polyPoint, hspace) > tolerance) 
                    {
                        clippedPolygonIsCoplanar = false;
                    }
                }
                if (! clippedPolygonIsCoplanar)
                {
                    result.addPolygon(clippedPolygon);
                    result.addIntersectionPoints(intersectionPoints); 
                }
            }

            const auto& intersectionPoints = result.intersectionPoints(); 

            if (intersectionPoints.size() > 2) 
            {
                auto orientedPoints = orient<Polygon>(
                    intersectionPoints, 
                    -1.*hspace.direction()
                );

                if (orientedPoints.size() > 2)
                {
                    result.addPolygon(
                        Polygon(
                            orientedPoints.begin(), 
                            orientedPoints.end()
                        )
                    ); 
                    result.setIntersectionPoints(orientedPoints); 
                }
            }

        }
        return result; 
    }

    // Reversed polygon sequence / halfspace intersection. 
    template
    <
        typename IntersectionResult = vectorPolyhedronIntersection, 
        typename IntersectionTest = no_test, 
        typename Halfspace,
        typename PolygonSequence
    >
    std::enable_if_t
    <
        tag_base_of<PolygonSequence, sequence_polyhedron_tag>() &&
        type_tagged<Halfspace, halfspace_tag>(), 
        IntersectionResult
    >
    intersect_tolerance(
        Halfspace const& halfspace, 
        PolygonSequence const& polyhedron,
        double tolerance = std::numeric_limits<double>::epsilon() 
    )
    {
        return intersect_tolerance
        <
            IntersectionResult, 
            IntersectionTest, 
            PolygonSequence, 
            Halfspace
        >(polyhedron, halfspace, tolerance);
    }

} // End namespace geophase 

// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //

#endif

// ************************************************************************* //
