search_nearest.h

// Description: Nearest neighbors searching in a kd-tree
// Documentation: nearestset.txt

#ifndef PASTELGEOMETRY_SEARCH_NEAREST_H
#define PASTELGEOMETRY_SEARCH_NEAREST_H

// Template concepts

#include "pastel/sys/indicator/indicator_concept.h"
#include "pastel/sys/output/output_concept.h"
#include "pastel/sys/point/point_concept.h"
#include "pastel/math/normbijection/normbijection_concept.h"
#include "pastel/geometry/nearestset/nearestset_concept.h"
#include "pastel/geometry/search_nearby.h"

// Template defaults

#include "pastel/math/normbijection/euclidean_normbijection.h"
#include "pastel/sys/indicator/all_indicator.h"
#include "pastel/sys/output/null_output.h"

// Implementation requirements

#include "pastel/sys/rankedset/rankedset.h"

namespace Pastel
{

    //! Finds the nearest neighbors of a point in a nearest-set.
    /*!
   nearestSet (NearestSet):
   The nearest-set in which to search neighbors in.

   searchPoint (Point):
   The point for which to search a neighbor for.

   Optional arguments
   ------------------

   accept (Indicator(PointId)):
   An indicator which decides whether to accept a point 
   as a neighbor or not.
   Default: allIndicator()

   kNearest (integer >= 0):
   The number of nearest neighbors to search for.
   Default: 1

   maxDistance2 (Real >= 0):
   The distance after which points are not considered neighbors
   anymore. Can be set to (Real)Infinity(). This distance
   is in terms of the used norm bijection.
   Default: (Real)Infinity()

   sortDistances (bool):
   Whether to report the neighbors in increasing
   order of distance, as opposed to arbitrary order.
   Default: true

   report (Output(Real, PointId)):
   An output to which the found neighbors 
   are reported to. The reporting is done in the 
   form report(distance, point). 
   Default: nullOutput()

   reportMissing (bool):
   Whether to always report kNearest points, even
   if there are not so many neighbors. The missing 
   points are reported as (Infinity(), nearestSet.notFound()).
   Default: true

   normBijection:
   The norm used to measure distance.

   returns (std::pair<Real, PointId>)
   ----------------------------------------------
   
   The first element is the distance 
   (in terms of the norm bijection) to the k:th 
   nearest neighbor. If the k:th nearest neighbor 
   does not exist, (Real)Infinity().
   
   The second element is the k:th nearest neighbor. 
   If the k:th nearest neighbor does not exist, nearestSet.notFound().
   */
    template <
        typename NearestSet,
        typename Search_Point,
        typename... ArgumentSet,
        Requires<
            Models<NearestSet, NearestSet_Concept>,
            Models<Search_Point, Point_Concept>
        > = 0
    >
    auto searchNearest(
        const NearestSet& nearestSet,
        const Search_Point& searchPoint,
        ArgumentSet&&... argumentSet)
    {
        using std::begin;

        using PointId = PointSet_PointId<NearestSet>;
        using Real = Point_Real<Search_Point>;

        auto&& report = 
            PASTEL_ARG(
                report, 
                []() {return nullOutput();},
                [](auto input) {return std::true_type();}
            );

        auto&& accept = 
            PASTEL_ARG(
                accept, 
                []() {return allIndicator();},
                [](auto input) 
                {
                    return Models<decltype(input), 
                        Indicator_Concept(PointId)>();
                }
            );

        auto&& normBijection = 
            PASTEL_ARG(
                normBijection, 
                []() {return Euclidean_NormBijection<real>();},
                [](auto input) 
                {
                    return implicitArgument(
                        Models<decltype(input), 
                        NormBijection_Concept>());
                }
            );

        integer kNearest = PASTEL_ARG_S(kNearest, 1);
        Real maxDistance2 = PASTEL_ARG_S(maxDistance2, (Real)Infinity());
        bool reportMissing = PASTEL_ARG_S(reportMissing, false);
        bool sortDistances = PASTEL_ARG_S(sortDistances, true);

        ENSURE_OP(kNearest, >=, 0);
        ENSURE_OP(maxDistance2, >=, 0);

        using Result = std::pair<Real, PointId>;
        Result notFound((Real)Infinity(), *begin(nearestSet.pointSet()));

        if (kNearest == 0 || maxDistance2 == 0)
        {
            // There is nothing to search for.
            // Note that we consider the search-ball open.
            return notFound;
        }

        struct Less
        {
            bool operator()(const Result& left, const Result& right) const
            {
                return left.first < right.first;
            }
        };

        // The number of points in the point-set.
        const integer n = setSize(nearestSet);

        // There cannot be more than n neighbors; 
        // avoid allocating storage in case kNearest
        // is excessive. If we are counting, we 
        // do not track any neighbor.
        const integer resultSetSize = std::min(kNearest, n);

        // This set contains the points currently closest
        // to the search-point. 
        using ResultSet = RankedSet<Result, Less>;
        // There will be at most k elements in this set.
        ResultSet resultSet(resultSetSize);

        auto reportCandidate = [&](
            const Real& distance2,
            const auto& pointId)
        {
            resultSet.push(Result(distance2, pointId));
            if (resultSet.full())
            {
                return resultSet.top().first;
            }

            return (Real)Infinity();
        };

        searchNearby(
            nearestSet,
            searchPoint,
            PASTEL_TAG(accept),
            accept,
            PASTEL_TAG(report),
            reportCandidate,
            PASTEL_TAG(normBijection),
            normBijection,
            PASTEL_TAG(maxDistance2),
            maxDistance2);

        // Sort the neighbors in order of
        // increasing distance.
        auto sortedSet = resultSet.release(sortDistances);

        // Report the nearest neighbors.
        RANGES_FOR(auto&& entry, sortedSet)
        {
            report(entry.first, entry.second);
        }

        // There should be at most k neighbors.
        const integer neighbors = sortedSet.size();
        ASSERT_OP(neighbors, <=, kNearest);

        if (reportMissing)
        {
            // Report "not found" for the missing neighbors.
            for (integer i = neighbors;i < kNearest;++i)
            {
                report(notFound.first, notFound.second);
            }
        }

        if (neighbors < kNearest)
        {
            // Since there are less than k neighbors,
            // the k:th nearest neighbor does not
            // exist.
            return notFound;
        }

        // Return the k:th nearest neighbor,
        // or the farthest neighbor when
        // counting.
        return sortedSet.back();
    }

}

#endif