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

    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
    Algorithms for iterative positioning of a piecewise-linear interface in 
    non-convex polyhedra with non-planar faces. 

    Extension of the predicted Newton method proposed by 

    Chen, X., & Zhang, X. (2019). A predicted-Newton’s method for solving the
    interface positioning equation in the MoF method on general polyhedrons.
    Journal of Computational Physics, 384, 60–76.
    https://doi.org/10.1016/j.jcp.2018.12.038 
    
Author
    Tomislav Maric maric@mma.tu-darmstadt.de

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

#ifndef IterativePositioner_HPP
#define IterativePositioner_HPP

#include "Vector.hpp"
#include "Halfspace.hpp"
#include "AxisAlignedBoundingBox.hpp"
#include "Polyhedron.hpp"
#include "Volume.hpp"
#include "DoIntersect.hpp"
#include "Intersect.hpp"
#include "IsInside.hpp"
#include "polynomial.hpp"
#include <numeric>
#include <algorithm> 
#include <bitset>

#ifdef TIMING 
#include <chrono>
#endif

#include "WriteVtkPolyData.hpp"

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

namespace geophase {

/*---------------------------------------------------------------------------*\
                     Class IterativePositioner Declaration
\*---------------------------------------------------------------------------*/

template
<
    typename Tetrahedra, 
    typename Volumes, 
    typename Halfspace
> 
double intersect_volume(
    Tetrahedra const& tetrahedra, 
    Volumes const& volumes, 
    Halfspace const& hspace
)
{
    double vol = 0; 

    auto tetit = tetrahedra.begin(); 
    auto volit = volumes.begin(); 
    const auto tetend = tetrahedra.cend(); 
    
    for(;tetit != tetend; ++tetit, ++volit) 
    {
        if (do_intersect_error(*tetit, hspace))
        {
            auto tetIntersection = intersect_tolerance(hspace, *tetit); 
            const auto& tetPolyhedron = tetIntersection.polyhedron(); 
            if (tetPolyhedron.size()) 
                vol += volume_by_surf_tri(tetPolyhedron); 
        }
        else if (is_inside_error(*tetit, hspace) == position::INSIDE) 
        {
            vol += *volit;
        }
    }

    return vol; 
}

template
<
    typename Triangles, 
    typename Halfspace
> 
double intersect_volume_area(
    Triangles const& triangles, 
    Halfspace const& hspace
)
{
    double vol = 0; 

    auto triangles_it = triangles.begin(); 
    const auto triangles_end = triangles.cend(); 
    const double LOWEST = std::numeric_limits<double>::lowest(); 

    //vtkPolyDataOStream vtk_result("vtk_result.vtk");

    // Encode triangle distances into a bitset of 3 bits.
    // See the STL documentation for the bitset: 
    //  111: all points are outside of the halfspace. 
    //  000: all points are inside the halfspace. 
    std::bitset<3> triangle_distbits;
    for(;triangles_it != triangles_end; ++triangles_it) 
    {
        // Set triangle point distance bits to 111.
        triangle_distbits = pow(2,3) - 1;
        const auto& triangle = *triangles_it; 

        std::cout << triangle << std::endl; // Debugging.

        std::array<distanceErrorPair, 3> point_dist_errors;
        point_dist_errors[0] = signed_distance_error(triangle[0],hspace); 
        point_dist_errors[1] = signed_distance_error(triangle[1],hspace); 
        point_dist_errors[2] = signed_distance_error(triangle[2],hspace); 

        std::array<position, 3> point_positions; 

        // See if the triangle is intersected by hspace.
        point_positions[0] = is_inside_error(point_dist_errors[0]);
        if (point_positions[0] == position::OUTSIDE) 
            triangle_distbits.flip(0);

        point_positions[1] = is_inside_error(point_dist_errors[1]);
        if (point_positions[1] == position::OUTSIDE) 
            triangle_distbits.flip(1);

        point_positions[2] = is_inside_error(point_dist_errors[2]);
        if (point_positions[2] == geophase::position::OUTSIDE) 
            triangle_distbits.flip(2);

        // Triangle area and unit normal vectors.
        auto tri_area_normal = 0.5 *cross(
            (triangle[1]  - triangle[0]), 
            (triangle[2]  - triangle[0]) 
        );
        auto tri_unit_normal = tri_area_normal / mag(tri_area_normal);

        // If the triangle is intersected by hspace 
        using geom_vector = decltype(triangle[0]); 
        if ((!triangle_distbits.all()) && triangle_distbits.any()) 
        {
            vectorPolygonIntersection triangle_intersection; 
            intersect_tolerance(triangle_intersection, triangle, hspace); 
            const auto& cut_polygon = triangle_intersection.polygon();
            if (cut_polygon.size() > 2)
            {
                vol += dot(
                    area_normal_oriented(cut_polygon), 
                    centroid(cut_polygon)
                );
                //vtk_result << cut_polygon;
            }
        }
        else if (triangle_distbits.all()) // Triangle is inside hspace.
        {
            vol += dot( 
                // Triangle centroid
                1./3. * (triangle[0] + triangle[1] + triangle[2]), 
                tri_area_normal
            );
            //vtk_result << triangle;
        } // Triangle is outside hspace and doesn't contribute to the volume.
    }
    return 1./3. * vol; 
}

template<typename Triangulation>
class BisectionData 
{
    public: 
        using Polyhedron = typename Triangulation::value_type;
        using Polygon = typename Polyhedron::value_type; 
        using Point = typename Polygon::value_type;
        using Doubles = std::vector<double>;

        BisectionData() = default;

        BisectionData(
            Triangulation&& tetrahedra, 
            double totalVolume,
            double tolerance = 1e-12,
            std::size_t maxIterations = 100 
        )
            :
                tetrahedra_(std::move(tetrahedra)), 
                volumes_(volumes_by_surf_tri(tetrahedra_)),
                totalVolume_(totalVolume),
                iteration_(0),
                s0_(0), 
                alpha0_(0), 
                s1_(1), 
                alpha1_(0), 
                s2_(0), 
                alpha2_(0), 
                tolerance_(tolerance),
                maxIterations_(maxIterations)
#ifdef TIMING
                ,
                rootTime_(0), 
                polyTime_(0), 
                geomTime_(0)
#endif

        {}

        double tolerance() const 
        {
            return tolerance_; 
        }

        int maxIterations() const 
        {
            return maxIterations_;
        }

        double rootValue() const
        {
            return std::abs(alpha2_); 
        }

        Point const& position() const
        {
            return position_; 
        }

        int iterations() const
        {
            return iteration_;
        }

        double totalVolume() const
        {
            return totalVolume_; 
        }

#ifdef TIMING
        void timingReset()
        {
            rootTime_ = std::chrono::microseconds{0};
            polyTime_ = std::chrono::microseconds{0}; 
            geomTime_ = std::chrono::microseconds{0}; 
        }
        decltype(auto) rootTime() const
        {
            return rootTime_.count();
        }
        decltype(auto) polyTime() const
        {
            return polyTime_.count();
        }
        decltype(auto) geomTime() const
        {
            return geomTime_.count();
        }
#endif

    protected: 
        Triangulation tetrahedra_; 
        Doubles volumes_;
        double totalVolume_;
        std::size_t iteration_; 
        double s0_,alpha0_,
               s1_,alpha1_,
               s2_,alpha2_, 
               tolerance_;
        std::size_t maxIterations_;
        Point position_; 

#ifdef TIMING // Timing data
    std::chrono::microseconds rootTime_; 
    std::chrono::microseconds polyTime_; 
    std::chrono::microseconds geomTime_; 
#endif 

};

template<typename Triangulation> 
class BisectionPositioner
:
    public BisectionData<Triangulation>
{

public:

    using Base = BisectionData<Triangulation>; 

    using Base::Base;

    std::string name() const  { return "BISECTION_POSITIONER"; }; 

    template<typename Vector>
    inline void positionInterface(
        const double alpha, 
        Vector const& normalVector,
        std::pair<Vector,Vector>&& extremePoints 
    )
    {
        this->iteration_ = 0;

        // Initialize the search:
        this->s0_ = 0;

        this->alpha0_ = 1 - alpha; 
        this->s1_ = dot(extremePoints.second - extremePoints.first, normalVector); 
        this->alpha1_ = -alpha; 

        // Secant-method initial guess. 
        this->s2_ = this->s1_ - alpha; 

        for (this->iteration_ = 1; this->iteration_ < this->maxIterations_; this->iteration_++) 
        {
            this->position_ = extremePoints.first + this->s2_*normalVector;
            halfspace hspace(this->position_, normalVector); 

            double intersectedVolume = intersect_volume(
                this->tetrahedra_, 
                this->volumes_,
                hspace
            );

            this->alpha2_ = (intersectedVolume / this->totalVolume_) - alpha;  

            if (std::abs(this->alpha2_) <= this->tolerance_) break;

            if ((this->alpha0_ * this->alpha2_) < 0) // Root between 0 and 2 
            {
                this->s1_ = this->s2_; 
                this->alpha1_ = this->alpha2_; 
                this->s2_ = 0.5 * (this->s0_ + this->s2_); 
            }
            else if ((this->alpha2_* this->alpha1_) < 0) // Root between 2 and 1
            {
                this->s0_ = this->s2_; 
                this->alpha0_ = this->alpha2_; 
                this->s2_ = 0.5 * (this->s2_ + this->s1_); 
            }
        } 
    }
};
using bisectionPositioner = BisectionPositioner<std::vector<vectorPolyhedron>>;

struct volAreaPair
{
    double volume_; 
    double area_; 

    volAreaPair()
        :
            volume_(0), 
            area_(0)
    {}
};

template<typename Triangulation, typename Volumes, typename Halfspace> 
volAreaPair intersect_volume_area(
    Triangulation const& triangulation, 
    Volumes const& volumes, 
    Halfspace const& hspace
)
{
    volAreaPair result;  

    auto tetit = triangulation.begin();  
    const auto tetend = triangulation.cend();  
    auto volit = volumes.begin(); 

    for (; tetit != tetend; ++tetit, ++volit)
    {
        if (do_intersect_error(*tetit, hspace))
        {
            // Intersect the cell at the cubic equation root.  
            auto tetIntersection = intersect_tolerance(hspace, *tetit); 
            const auto& tetPolyhedron = tetIntersection.polyhedron(); 

            if (tetPolyhedron.size())
            {
                result.volume_ += volume_by_surf_tri(tetPolyhedron); 

                const auto& capPolygon = tetIntersection.capPolygon(); 
                if (capPolygon.size() > 2) 
                    result.area_ += area_oriented(capPolygon); 
            }
        }
        else if (is_inside_error(*tetit, hspace) == position::INSIDE) 
        {
            result.volume_ += *volit;
        }
    }
    return result;
}

template<typename Triangulation>
class NewtonData
:
    public BisectionData<Triangulation>
{
    public:

        using Base = BisectionData<Triangulation>;
        using Base::Base;

        NewtonData<Triangulation>(
            Triangulation&& tetrahedra, 
            double totalVolume,
            double tolerance = 1e-12,
            std::size_t maxIterations = 100 
        )
            :
                Base(
                    std::move(tetrahedra), 
                    totalVolume, 
                    tolerance, 
                    maxIterations
                ),
                dalpha0_(0), 
                dalpha1_(0), 
                dalpha2_(0)
        {}

    protected:
        double dalpha0_, dalpha1_, dalpha2_;
};

template<typename Triangulation> 
class NetwonCubicSplinePositioner
:
    public NewtonData<Triangulation>
{

public:

    using Base = NewtonData<Triangulation>;

    using Base::Base;

    std::string name() const  { return "NEWTON_HERMITTE_POSITIONER"; }; 

    template<typename Vector>
    inline void positionInterface(
        const double alpha, 
        Vector const& normalVector,
        std::pair<Vector,Vector>&& extremePoints 
    )
    {
        // Search initialization. 
        this->iteration_ = 0;
        this->s0_ = 0;
        this->alpha0_ = 1 - alpha; 
        double s10 = dot(extremePoints.second - extremePoints.first, normalVector); 
        this->s1_ = s10; 
        this->alpha1_ = -alpha; 
        volAreaPair volArea; 

#ifdef TIMING
        this->timingReset(); 
        auto poly0 = std::chrono::steady_clock::now();
#endif 
        cubicHermitePolynomial P3H( // Hermite cubic spline initial guess.
            this->s0_, this->s1_,
            this->s0_, this->alpha0_,this->dalpha0_, 
            this->s1_, this->alpha1_,this->dalpha1_, 
            this->s1_ * (1 - alpha)
        ); 
#ifdef TIMING
        auto poly1 = std::chrono::steady_clock::now();
        this->polyTime_ +=  
            std::chrono::duration_cast<std::chrono::microseconds>(poly1-poly0);
        auto root0 = std::chrono::steady_clock::now();
#endif
        P3H.findRoot(); 
#ifdef TIMING
        auto root1 = std::chrono::steady_clock::now();
        this->rootTime_ +=  
            std::chrono::duration_cast<std::chrono::microseconds>(root1-root0);
#endif
        this->s2_ = P3H.root(); 

        for (this->iteration_ = 0; this->iteration_ < this->maxIterations_; this->iteration_++) 
        {
            this->position_ = extremePoints.first + this->s2_*normalVector;
            halfspace hspace(this->position_, normalVector); 

#ifdef TIMING
            auto geo0 = std::chrono::steady_clock::now();
#endif 
            volArea = intersect_volume_area(this->tetrahedra_, this->volumes_, hspace); 
#ifdef TIMING
            auto geo1 = std::chrono::steady_clock::now();
            this->geomTime_ +=  
                std::chrono::duration_cast<std::chrono::microseconds>(geo1-geo0);
#endif 
            this->alpha2_ = (volArea.volume_ / this->totalVolume_) - alpha;  
            this->dalpha2_ = -volArea.area_  / this->totalVolume_;   

            if (std::abs(this->alpha2_) <= this->tolerance_) break;

            // Update bisection interval. 
            if ((this->alpha0_ * this->alpha2_) < 0)
            {
                this->s1_ = this->s2_; 
                this->alpha1_ = this->alpha2_; 
                this->dalpha1_ = this->dalpha2_; 
            }
            else if ((this->alpha2_ * this->alpha1_) < 0)
            {
                this->s0_ = this->s2_; 
                this->alpha0_ = this->alpha2_; 
                this->dalpha0_ = this->dalpha2_;
            }

            // Newton iteration intermediate value.
            double s2 = this->s2_ - (this->alpha2_ / this->dalpha2_); 

            if ((s2 < 0) || (s2 > s10)) // Intermediate Newton value outside of search interval.
            {
#ifdef TIMING
                auto poly0 = std::chrono::steady_clock::now();
#endif
                // Use Hermite interpolation to find the root.
                cubicHermitePolynomial P3H( 
                    this->s0_, this->s1_,
                    this->s0_, this->alpha0_,this->dalpha0_, 
                    this->s1_, this->alpha1_,this->dalpha1_, 
                    this->s2_ // Previous iteration for the initial guess.
                ); 
#ifdef TIMING
                auto poly1 = std::chrono::steady_clock::now();
                this->polyTime_ +=  
                    std::chrono::duration_cast<std::chrono::microseconds>(poly1-poly0);
                auto root0 = std::chrono::steady_clock::now();
#endif
                //P3H.findRoot(); 
                // Newton-Rhapson, Schroeder, Halley and Quadratic diverge.
                P3H.findRoot(); 
#ifdef TIMING
                auto root1 = std::chrono::steady_clock::now();
                this->polyTime_ +=  
                    std::chrono::duration_cast<std::chrono::microseconds>(root1-root0);
#endif
                this->s2_ = P3H.root(); 
            }
            else
                this->s2_ = s2; // Use the Newton iteration.
        } 
    }

    template<typename Vector>
    Vector positionInterface(
        Vector const& normalVector, 
        const double volumeFraction
    )
    {
        return positionInterface(volumeFraction, normalVector); 
    }

};
using newtonCubicSplinePositioner = NetwonCubicSplinePositioner<std::vector<vectorPolyhedron>>;

template<typename Triangulation> 
class CubicSplinePositioner 
:
    public NewtonData<Triangulation>
{

public:

    using Base = NewtonData<Triangulation>;

    using Base::Base;

    std::string name() const  { return "CUBIC_SPLINE_POSITIONER"; }; 

    template<typename Vector>
    inline void positionInterface(
        const double alpha, 
        Vector const& normalVector,
        std::pair<Vector,Vector>&& extremePoints 
    )
    {


        // Search initialization. 
        this->iteration_ = 0;

        this->s0_ = 0;
        this->alpha0_ = 1 - alpha; 
        this->dalpha0_ = 0;

        double s10 = dot(extremePoints.second - extremePoints.first, normalVector); 

        this->s1_ = s10; 
        this->alpha1_ = -alpha; 
        this->dalpha1_ = 0;
        
#ifdef TIMING
        this->timingReset(); 
        auto poly0 = std::chrono::steady_clock::now();
#endif 
        cubicHermitePolynomial P3H( // Hermite cubic spline initial guess.
            this->s0_, this->s1_,
            this->s0_, this->alpha0_,this->dalpha0_, 
            this->s1_, this->alpha1_,this->dalpha1_, 
            this->s1_*(1 - alpha)
        ); 
#ifdef TIMING
        auto poly1 = std::chrono::steady_clock::now();
        this->polyTime_ +=  
            std::chrono::duration_cast<std::chrono::microseconds>(poly1-poly0);
        auto root0 = std::chrono::steady_clock::now();
#endif
        P3H.findRoot(); 
#ifdef TIMING
        auto root1 = std::chrono::steady_clock::now();
        this->rootTime_ +=  
            std::chrono::duration_cast<std::chrono::microseconds>(root1-root0);
#endif
        this->s2_ = P3H.root(); 

        double s2o = 0, alpha2o = 0, dalpha2o = 0, 
               sa=this->s0_, alphaa = this->alpha0_, dalphaa = this->dalpha0_, 
               sb=this->s1_, alphab = this->alpha1_, dalphab = this->dalpha1_;

        unsigned int nP3H = 0; 
#ifdef OUTPUT
        unsigned int nP3Hglob = 0; 
        unsigned int nNewton = 0; 
#endif 

        volAreaPair volArea; 
        for (this->iteration_ = 0; this->iteration_ < this->maxIterations_; this->iteration_++) 
        {
            this->position_ = extremePoints.first + this->s2_*normalVector;
            halfspace hspace(this->position_, normalVector); 

#ifdef TIMING
            auto geom0 = std::chrono::steady_clock::now();
#endif
            volArea = intersect_volume_area(this->tetrahedra_, this->volumes_, hspace); 
#ifdef TIMING
            auto geom1 = std::chrono::steady_clock::now();
            this->geomTime_ +=  
                std::chrono::duration_cast<std::chrono::microseconds>(geom1-geom0);
#endif
            alpha2o = this->alpha2_;
            this->alpha2_ = (volArea.volume_ / this->totalVolume_) - alpha;  

            if (std::abs(this->alpha2_) <= this->tolerance_) break;

            dalpha2o = this->dalpha2_; 
            this->dalpha2_ = -volArea.area_ / this->totalVolume_;   

            if ((alphaa * this->alpha2_) < 0) 
            {
                sb = this->s2_; 
                alphab = this->alpha2_; 
                dalphab = this->dalpha2_; 
            }
            else if ((this->alpha2_ * alphab) < 0) 
            {
                sa = this->s2_; 
                alphaa = this->alpha2_; 
                dalphaa = this->dalpha2_; 
            }

            if (this->iteration_ == 0)
            {

#ifdef TIMING
                auto poly0 = std::chrono::steady_clock::now();
#endif 
                cubicFillLevelPolynomial P3( 
                    this->s0_, this->s1_,
                    this->s0_, this->alpha0_, 
                    this->s1_, this->alpha1_, 
                    this->s2_,this->alpha2_,this->dalpha2_, 
                    this->s2_ 
                ); 
#ifdef TIMING
                auto poly1 = std::chrono::steady_clock::now();
                this->polyTime_ +=  
                    std::chrono::duration_cast<std::chrono::microseconds>(poly1-poly0);
#endif 
#ifdef TIMING
                auto root0 = std::chrono::steady_clock::now();
#endif 
                P3.findRoot(); 
#ifdef TIMING
                auto root1 = std::chrono::steady_clock::now();
                this->rootTime_ +=  
                    std::chrono::duration_cast<std::chrono::microseconds>(root1-root0);
#endif 
                s2o = this->s2_; 
                this->s2_ = P3.root();
            }
            else
            {
#ifdef TIMING
                auto poly0 = std::chrono::steady_clock::now();
#endif 
                cubicHermitePolynomial P3H( 
                    sa, sb,
                    sa, alphaa, dalphaa, 
                    sb, alphab, dalphab,
                    sa
                ); 
#ifdef TIMING
                auto poly1 = std::chrono::steady_clock::now();
                this->polyTime_ +=  
                    std::chrono::duration_cast<std::chrono::microseconds>(poly1-poly0);
#endif 
                if ((P3H.value(sa) * P3H.value(sb)) < 0)
                {

#ifdef TIMING
                    auto root0 = std::chrono::steady_clock::now();
#endif 
                    P3H.findRoot(); 
#ifdef TIMING
                    auto root1 = std::chrono::steady_clock::now();
                    this->rootTime_ +=  
                        std::chrono::duration_cast<std::chrono::microseconds>(root1-root0);
#endif 
                    s2o = this->s2_; 
                    this->s2_ = P3H.root(); 
                }
                else
                {
                    this->s2_ = this->s2_ - (this->alpha2_ / this->dalpha2_); 
                }

            }
        }
    }

    template<typename Vector>
    Vector positionInterface(
        Vector const& normalVector, 
        const double volumeFraction
    )
    {
        return positionInterface(volumeFraction, normalVector); 
    }
};
using cubicSplinePositioner = CubicSplinePositioner<std::vector<vectorPolyhedron>>;

} // End namespace geophase 

#endif

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