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

    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
    Generic distance algorithms for different geometries. 

Author
    Tomislav Maric maric@csi.tu-darmstadt.de, tomislav@sourceflux.de
    Mathematical Modeling and Analysis
    Center of Smart Interfaces, TU Darmstadt

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

#ifndef Distance_H 
#define Distance_H 

#include "tag.hpp"
#include "Vector.hpp"

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

namespace geophase { 

    template<typename Vector, typename Halfspace, typename FT=double> 
    enable_if_types_tagged_t
    <
        FT, 
        Halfspace, halfspace_tag, Vector, vector_tag
    >
    signed_distance(Vector const& vec, Halfspace const& hspace)
    {
        return dot((vec - hspace.p()), hspace.n());
    }

    template<typename Halfspace, typename Vector, typename FT=double> 
    enable_if_types_tagged_t
    <
        FT, 
        Halfspace, halfspace_tag, Vector, vector_tag
    >
    signed_distance(Halfspace const& hspace, Vector const& vec)
    {
        return signed_distance(vec, hspace); 
    }

    struct distanceErrorPair 
    {
        double distance; 
        double abserror; 

        distanceErrorPair()
            :
                distance(std::numeric_limits<double>::max()), 
                abserror(std::numeric_limits<double>::max()) 
        {}

        distanceErrorPair(double distance, double abserror)
            :
                distance(distance), 
                abserror(abserror)
        {}
    };

    template<typename Halfspace, typename Vector> 
    enable_if_types_tagged_t
    <
        distanceErrorPair,
        Halfspace, halfspace_tag, Vector, vector_tag
    >
    signed_distance_error(Halfspace const& hspace, Vector const& r)
    {
        using RT = typename Vector::value_type; 

        const auto& p = hspace.p(); 
        const auto& n = hspace.n(); 

        RT n0p0r0 = n[0] * (r[0] - p[0]);  
        RT n1p1r1 = n[1] * (r[1] - p[1]);  
        RT n2p2r2 = n[2] * (r[2] - p[2]);  

        RT distance = n0p0r0 + n1p1r1 + n2p2r2; 

        RT error = 0.5*std::numeric_limits<RT>::epsilon() * // Linearized error estimate 
            (
                std::fabs(n0p0r0) + std::fabs(n1p1r1) + std::fabs(n2p2r2) + 
                std::fabs(n0p0r0 + n1p1r1) + std::fabs(distance) + 
                std::fabs(n[0]*p[0] + n[0]*r[0] + n1p1r1 + n2p2r2)
            ); 

        return distanceErrorPair(distance, error);
    }

    template<typename Vector, typename Halfspace> 
    enable_if_types_tagged_t
    <
        distanceErrorPair,
        Halfspace, halfspace_tag, Vector, vector_tag
    >
    signed_distance_error(Vector const& r, Halfspace const& hspace)
    {
        return signed_distance_error(hspace, r);
    }
    

} // End namespace geophase 

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

#endif

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