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

    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
    Orient algorithms.  

    Orients the geometry based on the provided vector. 

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

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

#ifndef Orient_HPP
#define Orient_HPP

#include "constants.hpp"
#include "Equal.hpp"
#include "Vector.hpp"
#include "tag.hpp"
#include <algorithm>

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

namespace geophase { 

    template<typename Polygon, typename Vector> 
    std::enable_if_t 
    <
        tag_base_of<Polygon, sequence_polygon_tag>() &&
        type_tagged<Vector, vector_tag>(), 
        Polygon
    >
    orient(
        Polygon const & polygon, 
        Vector const & normal,
        double tolerance = std::numeric_limits<double>::epsilon()
    )
    {
        Polygon result;

        using Point = typename Polygon::value_type; 
        
        // Remove duplicate points coming from the intersection. 
        auto uniquePoints (polygon); 
        auto last = std::unique(
            uniquePoints.begin(), 
            uniquePoints.end(), 
            EqualByTolerance<double>(tolerance)
        ); 
        last = uniquePoints.erase(last, uniquePoints.end()); 
        // Remove the possible duplicated first-last pair of points.
        auto back = std::prev(uniquePoints.end()); 
        if(equal_by_tolerance(uniquePoints.front(), *back, tolerance))
            last = back;  
        uniquePoints.erase(last, uniquePoints.end()); 
        // Return empty result if a polygon cannot be constructed
        // from unique points.
        if (uniquePoints.size() < 3)
            return result; 

        // Check if it is a triangle and adjust orientation.
        if (uniquePoints.size() == 3)
        {
            const auto& P0 = uniquePoints.front(); 
            const auto& P1 = *std::next(uniquePoints.begin()); 
            const auto& P2 = uniquePoints.back();  

            if (dot(cross((P1 - P0),(P2 - P0)), normal) > 0)
            {
                result.push_back(P0); 
                result.push_back(P1); 
                result.push_back(P2); 
            }
            else 
            {
                result.push_back(P0); 
                result.push_back(P2); 
                result.push_back(P1); 
            }
            return result; 
        }

        // Compute P0P1 vector used to find the point with the min
        // and max projection to P0P1.  
        auto P0 = uniquePoints.front(); 
        const auto& P1 = uniquePoints.back();  
        const auto P0P1 = P1 - P0;  

        // Find the minimal and maximal projections to P0P1.
        // TODO: Remove, once checked. 
        //Point Pmin (-GREAT, -GREAT, -GREAT); 
        //scalar min = GREAT; 
        //Point  Pmax (GREAT, GREAT, GREAT); 
        //scalar max = -GREAT; 
        Point Pmin {LOWEST, LOWEST, LOWEST}; 
        double min = MAX; 
        Point  Pmax {MAX, MAX, MAX};
        double max = LOWEST; 

        for (const auto& Pc : uniquePoints)
        {
            auto projection = dot((Pc - P0), P0P1); 

            if (projection < min)
            {
                Pmin = Pc; 
                min = projection;
            }

            if (projection >= max)
            {
                Pmax = Pc; 
                max = projection;
            }
        }
        // Vector between the minimal and maximal projection points.
        auto PminPmax = Pmax - Pmin;

        using PointProjection = std::pair<Point, double>; 
        using PointProjectionVector = std::vector<PointProjection>; 

        // Sort the oriented points into two sets: left anr right
        // with respect to PminPmax. 
        PointProjectionVector leftPoints;  
        leftPoints.reserve(decltype(polygon.size())(0.5 * polygon.size() + 1)); 
        PointProjectionVector rightPoints; 
        rightPoints.reserve(decltype(polygon.size())(0.5 * polygon.size() + 1)); 

        for (const auto& Pc : uniquePoints)
        {
            auto orientation = dot(normal, cross(PminPmax,(Pc - Pmin))); 
            auto projection = dot(P0P1, (Pc - Pmin)); 

            // TODO: Check this tolerance. 
            if (orientation > 5*tolerance)
            {
                leftPoints.push_back(std::make_pair(Pc, projection)); 
            } else if (orientation < -5*tolerance)
            {
                rightPoints.push_back(std::make_pair(Pc, projection)); 
            }
        }

        // Left points have ascending projections.  
        std::sort(
            leftPoints.begin(), 
            leftPoints.end(), 
            [](PointProjection const & pp1, PointProjection const &pp2)->bool{ 
                return pp1.second > pp2.second; 
            }
        );

        // Right points have descending projections.
        std::sort(
            rightPoints.begin(), 
            rightPoints.end(), 
            [](PointProjection const & pp1, PointProjection const &pp2)->bool{ 
                return pp1.second < pp2.second; 
            }
        );
       
        // Add the minimal projection point.
        result.push_back(Pmin);

        // Add the right points. 
        for (const auto& pointProjection : rightPoints) 
        {
            result.push_back(pointProjection.first); 
        }
        
        // Add the maximal projection point.
        result.push_back(Pmax);

        // Add the left points.
        for (const auto& pointProjection : leftPoints) 
        {
            result.push_back(pointProjection.first); 
        }

        //Remove duplicates throughout the result sequence.
        last = std::unique(
            result.begin(), 
            result.end(), 
            EqualByTolerance<double>(tolerance)
        ); 
        // Remove the possible duplicate element at the end. 
        back = std::prev(last); 
        if(equal_by_tolerance(result.front(), *back, tolerance))
            last = back;  
        result.erase(last, result.end()); 

        return result; 
    }

} // End namespace geophase 

#endif

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