// Copyright (c) 2019, Christoph Peters
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Karlsruhe Institute of Technology nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


#pragma once
#include "nanoflann.hpp"
#include <array>
#include <vector>

/*! A nearest-neighbor lookup structure for points in 3D space that uses the
	Euclidean distance.*/
class NearestNeighborIndex {
public:
	/*! A point in 3D space.*/
	typedef std::array<float, 3> Point;
	/*! A list of points.*/
	typedef std::vector<std::array<float, 3>> PointList;

	/*! Prepares nearest neighbor lookups for points in the given point list. 
		\param rPointToIgnore Points with this exact value are masked out. They 
				will never be returned as nearest neighbor. Indices of other 
				points remain unaffacted.*/
	NearestNeighborIndex(const PointList& rPointList, const Point& rPointToIgnore);

	/*! Given a point, this function returns the index of the point with
		minimal Euclidean distance in the input point set.*/
	std::size_t query(const std::array<float, 3>& rQueryPoint);

private:
	/*! A finite set of 3D points that meets the requirements for a nanoflann
		DatasetAdaptor.*/
	class PointListAdaptor {
	public:
		/*! Stores the reference.*/
		inline PointListAdaptor(const PointList& rPointList) :
			m_rPointList(rPointList)
		{}

		/*! \return The number of held points.*/
		inline std::size_t kdtree_get_point_count() const {
			return m_rPointList.size();
		}

		/*! \return The specified coordinate of the specified point. Indices
			must be in range, otherwise behavior is undefined.*/
		inline float kdtree_get_pt(std::size_t iPoint, std::size_t iDimension) const {
			return m_rPointList[iPoint][iDimension];
		}

		/*! We let nanoflann compute the bounding box.*/
		template <class BBOX>
		bool kdtree_get_bbox(BBOX&) const {
			return false;
		}

		/*! The list of all points in the data set.*/
		const PointList& m_rPointList;
	};
	/*! The nanoflann type providing the nearest neighbor lookup structure.*/
	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, PointListAdaptor>, PointListAdaptor, 3, std::size_t> NanoflannNearestNeighborIndex;

	/*! The object holding all the points.*/
	PointListAdaptor m_pointListAdaptor;
	/*! A copy of the provided point list without ignored points.*/
	PointList m_reducedPointList;
	/*! A vector of same length as m_reducedPointList mapping each point to its 
		index in the original list provided by the user.*/
	std::vector<std::size_t> m_reducedPointListIndex;
	/*! The nearest neighbor lookup structure.*/
	NanoflannNearestNeighborIndex m_nearestNeighborIndex;
};
