#pragma once

#include "splines.h"
#include <algorithm>
#include <iterator>
#include <functional>
#include <cmath>			// for fmin/fmax
#include <cassert>
#ifdef MSVC_LIMITATIONS
#include <boost/math/special_functions/binomial.hpp>
#endif
#include "general math.h"	// for lerp
#include "misc.h"			// for Reserve()

#pragma region CompositePoint
namespace Math::Splines
{
	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::AddAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst += src; }
	};

	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::SubAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst -= src; }
	};

	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::MulAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst *= src; }
	};

	template<class Pos, class ...Attribs>
	struct CompositePoint<Pos, Attribs...>::DivAssign
	{
		template<typename Dst, typename Src>
		constexpr void operator ()(Dst &dst, const Src &src) const { dst /= src; }
	};

	// op point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx>
	inline auto CompositePoint<Pos, Attribs...>::OpPoint(std::index_sequence<idx...>) const
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(pos))>, std::decay_t<decltype(std::declval<Functor>()(std::get<idx>(attribs)))>...>
#endif
	{
		constexpr Functor op;
		return{ op(pos), op(std::get<idx>(attribs))... };
	}

	// point op point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx, class RightPos, class ...RightAttribs>
	inline auto CompositePoint<Pos, Attribs...>::PointOpPoint(std::index_sequence<idx...>, const CompositePoint<Pos, Attribs...> &left, const CompositePoint<RightPos, RightAttribs...> &right)
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(left.pos, right.pos))>, std::decay_t<decltype(std::declval<Functor>()(std::get<idx>(left.attribs), std::get<idx>(right.attribs)))>...>
#endif
	{
		constexpr Functor op;
		return{ op(left.pos, right.pos), op(std::get<idx>(left.attribs), std::get<idx>(right.attribs))... };
	}

	// point op scalar
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::PointOpScalar(std::index_sequence<idx...>, const CompositePoint<Pos, Attribs...> &left, const Scalar &right)
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(left.pos, right))>, std::decay_t<decltype(std::declval<Functor>()(std::get<idx>(left.attribs), right))>...>
#endif
	{
		constexpr Functor op;
		return{ op(left.pos, right), op(std::get<idx>(left.attribs), right)... };
	}

	// scalar op point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t ...idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::ScalarOpPoint(std::index_sequence<idx...>, const Scalar &left, const CompositePoint<Pos, Attribs...> &right)
#ifdef MSVC_LIMITATIONS
		-> CompositePoint<Pos, Attribs...>
#else
		-> CompositePoint<std::decay_t<decltype(std::declval<Functor>()(left, right.pos))>, std::decay_t<decltype(std::declval<Functor>()(left, std::get<idx>(right.attribs)))>...>
#endif
	{
		constexpr Functor op;
		return { op(left, right.pos), op(left, std::get<idx>(right.attribs))... };
	}

	// point op= point
	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::PointOpPoint(const CompositePoint<SrcPos, SrcAttribs...> &src) -> std::enable_if_t<idx < sizeof...(Attribs), CompositePoint &>
	{
		Functor()(std::get<idx>(attribs), std::get<idx>(src.attribs));
		return PointOpPoint<Functor, idx + 1>(src);
	}

	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::PointOpPoint(const CompositePoint<SrcPos, SrcAttribs...> &src) -> std::enable_if_t<idx == sizeof...(Attribs), CompositePoint &>
	{
		Functor()(pos, src.pos);
		return *this;
	}

	// point op= scalar
	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::PointOpScalar(const Scalar &src) -> std::enable_if_t<idx < sizeof...(Attribs), CompositePoint &>
	{
		Functor()(std::get<idx>(attribs), src);
		return PointOpScalar<Functor, idx + 1>(src);
	}

	template<class Pos, class ...Attribs>
	template<class Functor, size_t idx, typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::PointOpScalar(const Scalar &src) -> std::enable_if_t<idx == sizeof...(Attribs), CompositePoint &>
	{
		Functor()(pos, src);
		return *this;
	}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline CompositePoint<Pos, Attribs...>::CompositePoint(SrcPos &&pos, SrcAttribs &&...attribs) :
	pos(std::forward<SrcPos>(pos)), attribs(std::forward<SrcAttribs>(attribs)...) {}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline CompositePoint<Pos, Attribs...>::CompositePoint(const CompositePoint<SrcPos, SrcAttribs...> &src) :
	pos(src.pos), attribs(src.attribs) {}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline CompositePoint<Pos, Attribs...>::CompositePoint(CompositePoint<SrcPos, SrcAttribs...> &&src) :
	pos(std::move(src.pos)), attribs(std::move(src.attribs)) {}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator =(const CompositePoint<SrcPos, SrcAttribs...> &src) -> CompositePoint &
	{
		pos = src.pos;
		attribs = src.attribs;
		return *this;
	}

	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator =(CompositePoint<SrcPos, SrcAttribs...> &&src) -> CompositePoint &
	{
		pos = std::move(src.pos);
		attribs = std::move(src.attribs);
		return *this;
	}

	template<class Pos, class ...Attribs>
	inline auto CompositePoint<Pos, Attribs...>::operator -() const
	{
		return OpPoint<std::negate<>>(std::index_sequence_for<Attribs...>());
	}

	// point += point
	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator +=(const CompositePoint<SrcPos, SrcAttribs...> &src) -> CompositePoint &
	{
		return PointOpPoint<AddAssign>(src);
	}

	// point -= point
	template<class Pos, class ...Attribs>
	template<class SrcPos, class ...SrcAttribs>
	inline auto CompositePoint<Pos, Attribs...>::operator -=(const CompositePoint<SrcPos, SrcAttribs...> &src) -> CompositePoint &
	{
		return PointOpPoint<SubAssign>(src);
	}

	// point *= scalar
	template<class Pos, class ...Attribs>
	template<typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::operator *=(const Scalar &src) -> CompositePoint &
	{
		return PointOpScalar<MulAssign>(src);
	}

	// point /= scalar
	template<class Pos, class ...Attribs>
	template<typename Scalar>
	inline auto CompositePoint<Pos, Attribs...>::operator /=(const Scalar &src) -> CompositePoint &
	{
		return PointOpScalar<DivAssign>(src);
	}

	// point + point
	template<class LeftPos, class ...LeftAttribs, class RightPos, class ...RightAttribs>
	inline auto operator +(const CompositePoint<LeftPos, LeftAttribs...> &left, const CompositePoint<RightPos, RightAttribs...> &right)
	{
		return CompositePoint<LeftPos, LeftAttribs...>::PointOpPoint<std::plus<>>(std::index_sequence_for<LeftAttribs...>(), left, right);
	}

	// point - point
	template<class LeftPos, class ...LeftAttribs, class RightPos, class ...RightAttribs>
	inline auto operator -(const CompositePoint<LeftPos, LeftAttribs...> &left, const CompositePoint<RightPos, RightAttribs...> &right)
	{
		return CompositePoint<LeftPos, LeftAttribs...>::PointOpPoint<std::minus<>>(std::index_sequence_for<LeftAttribs...>(), left, right);
	}

	// point * scalar
	template<class Pos, class ...Attribs, typename Scalar>
	inline auto operator *(const CompositePoint<Pos, Attribs...> &left, const Scalar &right)
	{
		return CompositePoint<Pos, Attribs...>::PointOpScalar<std::multiplies<>>(std::index_sequence_for<Attribs...>(), left, right);
	}

	// scalar * point
	template<class Pos, class ...Attribs, typename Scalar>
	inline auto operator *(const Scalar &left, const CompositePoint<Pos, Attribs...> &right)
	{
		return CompositePoint<Pos, Attribs...>::ScalarOpPoint<std::multiplies<>>(std::index_sequence_for<Attribs...>(), left, right);
	}

	// point / scalar
	template<class Pos, class ...Attribs, typename Scalar>
	inline auto operator /(const CompositePoint<Pos, Attribs...> &left, const Scalar &right)
	{
		return CompositePoint<Pos, Attribs...>::PointOpScalar<std::divides<>>(std::index_sequence_for<Attribs...>(), left, right);
	}
}
#pragma endregion

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<size_t ...idx>
inline Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(const typename ControlPoints::value_type (&src)[degree + 1], std::index_sequence<idx...>) :
	controlPoints{ typename ControlPoints::value_type(src[idx])... } {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(const typename ControlPoints::value_type (&src)[degree + 1]) :
	CBezier(src, std::make_index_sequence<degree + 1>()) {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(const ControlPoints &controlPoints) :
	controlPoints(controlPoints) {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(ControlPoints &&controlPoints) :
	controlPoints(std::move(controlPoints)) {}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<class ...Points>
Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::CBezier(Points &&...controlPoints) :
controlPoints{ std::forward<Points>(controlPoints)... }
{
	constexpr auto count = degree + 1;
	static_assert(sizeof...(Points) >= count, "too few control points");
	static_assert(sizeof...(Points) <= count, "too many control points");
}

#ifndef MSVC_LIMITATIONS
template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<size_t ...idx>
inline auto Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::operator ()(ScalarType u, std::index_sequence<idx...>) const -> typename ControlPoints::value_type
{
	static_assert(degree >= 2);
	ScalarType factor1 = u;
	const ScalarType factors2[degree] = { 1 - u, factors2[idx] * factors2[0] ... };
	using Combinatorics::C;
	return C<degree, 0> * factors2[degree - 1] * controlPoints[0] +
		C<degree, 1> * factor1 * factors2[degree - 2] * controlPoints[1] +
		(C<degree, idx + 2> * (factor1 *= u) * factors2[degree - 3 - idx] * controlPoints[2 + idx] + ... + C<degree, degree> * factor1 * u * controlPoints[degree]);
}
#endif

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
auto Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::operator ()(ScalarType u) const -> typename ControlPoints::value_type
{
#ifdef MSVC_LIMITATIONS
	ScalarType factor1 = 1, factors2[degree + 1];
	factors2[degree] = 1;
	for (signed i = degree - 1; i >= 0; i--)
		factors2[i] = factors2[i + 1] * (1 - u);
	auto result = typename ControlPoints::value_type();	// value init
	for (unsigned i = 0; i <= degree; i++, factor1 *= u)
		result += boost::math::binomial_coefficient<ScalarType>(degree, i) * factor1 * factors2[i] * controlPoints[i];
	return result;
#else
	if constexpr (degree == 0)
		return controlPoints[0];
	else if constexpr (degree == 1)
		return lerp(controlPoints[0], controlPoints[1], u);
	else
		return operator ()(u, std::make_index_sequence<degree - 2>());
#endif
}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<typename Iterator>
void Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::Tessellate(Iterator output, ScalarType delta, bool emitFirstPoint) const
{
	if (emitFirstPoint) *output++ = controlPoints[0];
	Subdiv(output, delta, controlPoints);
}

template<typename ScalarType, unsigned int dimension, unsigned int degree, class ...Attribs>
template<typename Iterator, size_t ...idx>
void Math::Splines::CBezier<ScalarType, dimension, degree, Attribs...>::Subdiv(Iterator output, ScalarType delta, const ControlPoints &controlPoints, std::index_sequence<idx...>)
{
	// TODO: move to Math
	const auto point_line_dist = [](typename ControlPoints::const_reference point, typename ControlPoints::const_reference lineBegin, typename ControlPoints::const_reference lineEnd) -> ScalarType
	{
		const auto point_dir = GetPos(point) - GetPos(lineBegin), line_dir = GetPos(lineEnd) - GetPos(lineBegin);
		// project point_dir on line_dir
		const auto proj = dot(point_dir, line_dir) / dot(line_dir, line_dir) * line_dir;
		return length(point_dir - proj);
	};

	const auto stop_test = [&point_line_dist, &controlPoints, delta]
	{
#ifdef MSVC_LIMITATIONS
		for (unsigned i = 1; i < degree; i++)
		{
			if (point_line_dist(controlPoints[i], controlPoints[0], controlPoints[degree]) > delta)
				return false;
		}
		return true;
#else
		return (point_line_dist(controlPoints[idx + 1], controlPoints[0], controlPoints[degree]) > delta || ...);
#endif
	};

	if (stop_test())
		*output++ = (controlPoints[degree]);
	else
	{
		constexpr auto intermediate_points_count = (degree + 1) * 2 - 1;
		typename ControlPoints::value_type intermediate_points[intermediate_points_count];	// will hold control points for 2 subdivided curves (with 1 common point)
		for (unsigned i = 0; i <= degree; i++)
			intermediate_points[i * 2] = controlPoints[i];
		for (unsigned insert_idx_begin = 1, insert_idx_end = intermediate_points_count; insert_idx_begin <= degree; insert_idx_begin++, insert_idx_end--)
		{
			for (unsigned insert_idx = insert_idx_begin; insert_idx < insert_idx_end; insert_idx += 2)
				intermediate_points[insert_idx] = lerp(intermediate_points[insert_idx - 1], intermediate_points[insert_idx + 1], ScalarType(.5));
		}
		Subdiv(output, delta, intermediate_points + 0);
		Subdiv(output, delta, intermediate_points + degree);
	}
}

template<template<typename ScalarType, unsigned int dimension, class ...Attribs> class CBezierInterpolationImpl, typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
void Math::Splines::Impl::CBezierInterpolationCommon<CBezierInterpolationImpl, ScalarType, dimension, Attribs...>::Tessellate(Iterator output, ScalarType delta) const
{
	for (Points::size_type i = 1; i < points.size() - 2; i++)
		Segment(i).Tessellate(output, delta, i == 1);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::CCatmullRom(Iterator begin, Iterator end) :
points(begin, end)
{
	assert(points.size() >= 4);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::CCatmullRom(std::initializer_list<Point> points) :
points(points)
{
	assert(points.size() >= 4);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::operator ()(ScalarType u) const -> Point
{
	//assert(u >= 0 && u <= 1);
	// [0..1]->[0..m-2]->[1..m-1]
	u *= points.size() - 3, u++;
	// ensure 1 <= i < m
	//const Points::size_type i = std::min<Points::size_type>(floor(u), points.size() - 3);
	const ScalarType i = fmin(fmax(floor(u), 1), points.size() - 3);
	return Segment(i)(u - i);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CCatmullRom<ScalarType, dimension, Attribs...>::Segment(typename Points::size_type i) const -> Bezier
{
	assert(i >= 1 && i < points.size() - 2);
	return { points[i], points[i] + (points[i + 1] - points[i - 1]) / 6, points[i + 1] - (points[i + 2] - points[i]) / 6, points[i + 1] };
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::CBesselOverhauser(Iterator begin, Iterator end)
{
	Reserve(points, begin, end);
	Init(begin, end);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::CBesselOverhauser(std::initializer_list<Point> points)
{
	this->points.reserve(points.size());
	Init(points.begin(), points.end());
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
template<typename Iterator>
void Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::Init(Iterator begin, Iterator end)
{
	assert(begin != end);
	points.emplace_back(typename Points::value_type(0, *begin));
	transform(std::next(begin), end, std::back_inserter(points), [this](const Point &curPoint)
	{
		/*
			it seems that 'std' namespace used here ('VectorMath::' before 'distance' required)
			TODO: try with other compilers
		*/
		return typename Points::value_type(points.back().first + VectorMath::distance(GetPos(points.back().second), GetPos(curPoint)), curPoint);
	});
	assert(points.size() >= 4);
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::operator ()(ScalarType u) const -> Point
{
	//assert(u >= 0 && u <= 1);
	// [0..1]->[u_begin..u_end]
	const ScalarType u_begin = std::next(points.begin())->first, u_end = std::prev(points.end(), 2)->first;
	u *= u_end - u_begin, u += u_begin;
	// ensure u in [u_begin..u_end]
	//u = fmin(fmax(u, u_begin), u_end);
	struct
	{
		bool operator ()(ScalarType left, typename Points::const_reference right) const
		{
			return left < right.first;
		}
		bool operator ()(typename Points::const_reference left, ScalarType right) const
		{
			return left.first < right;
		}
	} point_order;
	auto p1 = std::lower_bound(points.begin(), points.end(), u, point_order);
	if (std::distance(points.begin(), p1) < 2)
		p1 = std::next(points.begin(), 2);
	else if (std::distance(p1, points.end()) < 2)
		p1 = std::prev(points.end(), 2);
	const auto p0 = std::prev(p1);
	return Segment(std::distance(points.begin(), p0))((u - p0->first) / (p1->first - p0->first));
}

template<typename ScalarType, unsigned int dimension, class ...Attribs>
auto Math::Splines::Impl::CBesselOverhauser<ScalarType, dimension, Attribs...>::Segment(typename Points::size_type i) const -> Bezier
{
	assert(i >= 1 && i < points.size() - 2);
	const auto v_segment = [this](typename Points::size_type j)
	{
		return (points[j + 1].second - points[j].second) / (points[j + 1].first - points[j].first);
	};
	const Point segment_vels[3] = {v_segment(i - 1), v_segment(i), v_segment(i + 1)};
	const auto offset = [this, &segment_vels, i](typename Points::size_type shift)
	{
		return
			(
				(points[i + 1 + shift].first - points[i + shift].first) * segment_vels[0 + shift] +
				(points[i + shift].first - points[i - 1 + shift].first) * segment_vels[1 + shift]
			) /
			((points[i + 1 + shift].first - points[i - 1 + shift].first) * 3) *
			(points[i + 1].first - points[i].first);
	};
	return { points[i].second, points[i].second + offset(0), points[i + 1].second - offset(1), points[i + 1].second };
}


/*
 * Copyright (c) 2015 University of Basel
 * 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 project's author 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.
 *
 */

#include <iostream>

#include <boost/program_options.hpp>

#include <itkDataManager.h>
#include <itkDirectory.h>
#include <itkImageFileReader.h>
#include <itkLowRankGPModelBuilder.h>
#include <itkMeshFileReader.h>
#include <itkStandardImageRepresenter.h>
#include <itkStandardMeshRepresenter.h>
#include <itkStatismoIO.h>
#include <itkStatisticalModel.h>

//Add new kernels in this file (and document their usage in the statismo-build-gp-model.md file)
#include "utils/statismo-build-gp-model-kernels.h"

namespace po = boost::program_options;
using namespace std;

struct programOptions {
    bool bDisplayHelp;
    string strOptionalModelPath;
    string strReferenceFile;
    string strKernel;
    string strType;
    vector<string> vKernelParameters;
    float fKernelScale;
    int iNrOfBasisFunctions;
    unsigned uNumberOfDimensions;
    string strOutputFileName;
};

po::options_description initializeProgramOptions(programOptions& poParameters);
bool isOptionsConflictPresent(programOptions& opt);
template <class DataType, class RepresenterType, class DataReaderType, bool isShapeModel, unsigned Dimenstionality>
void buildAndSaveModel(programOptions opt);
void createKernelMap();
string getAvailableKernelsStr();

int main(int argc, char** argv) {
    createKernelMap();

    programOptions poParameters;

    po::positional_options_description optPositional;
    optPositional.add("output-file", 1);
    po::options_description optAllOptions = initializeProgramOptions(poParameters);

    po::variables_map vm;
    try {
        po::parsed_options parsedOptions = po::command_line_parser(argc, argv).options(optAllOptions).positional(optPositional).run();
        po::store(parsedOptions, vm);
        po::notify(vm);
    } catch (po::error& e) {
        cerr << "An exception occurred while parsing the Command line:"<<endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    }

    if (poParameters.bDisplayHelp == true) {
        cout << optAllOptions << endl;
        return EXIT_SUCCESS;
    }
    if (isOptionsConflictPresent(poParameters) == true)	{
        cerr << "A conflict in the options exists or insufficient options were set." << endl;
        cout << optAllOptions << endl;
        return EXIT_FAILURE;
    }

    try {
        if (poParameters.strType == "shape") {
            typedef itk::StandardMeshRepresenter<float, Dimensionality3D> RepresenterType;
            typedef itk::MeshFileReader<DataTypeShape> DataReaderType;
            buildAndSaveModel<DataTypeShape, RepresenterType, DataReaderType, true, Dimensionality3D>(poParameters);
        } else {
            if (poParameters.uNumberOfDimensions == 2) {
                typedef itk::StandardImageRepresenter<VectorPixel2DType, Dimensionality2D> RepresenterType;
                typedef itk::ImageFileReader<DataType2DDeformation> DataReaderType;
                buildAndSaveModel<DataType2DDeformation, RepresenterType, DataReaderType, false, Dimensionality2D>(poParameters);
            } else {
                typedef itk::StandardImageRepresenter<VectorPixel3DType, Dimensionality3D> RepresenterType;
                typedef itk::ImageFileReader<DataType3DDeformation> DataReaderType;
                buildAndSaveModel<DataType3DDeformation, RepresenterType, DataReaderType, false, Dimensionality3D>(poParameters);
            }
        }

    } catch (itk::ExceptionObject & e) {
        cerr << "Could not build the model:" << endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}

bool isOptionsConflictPresent(programOptions& opt) {
    boost::algorithm::to_lower(opt.strKernel);
    boost::algorithm::to_lower(opt.strType);

    if (opt.strType != "shape" && opt.strType != "deformation") {
        return true;
    }

    if ((opt.strOptionalModelPath == "" && opt.strReferenceFile == "")
            || (opt.strOptionalModelPath != "" & opt.strReferenceFile != "")) {
        return true;
    }

    if (opt.strOutputFileName == "") {
        return true;
    }

    if (opt.strOutputFileName == opt.strReferenceFile) {
        return true;
    }

    if (opt.iNrOfBasisFunctions < 1) {
        return true;
    }

    if (opt.strType == "deformation" && opt.uNumberOfDimensions != 2 && opt.uNumberOfDimensions != 3) {
        return true;
    }

    if (opt.strType == "shape" && opt.uNumberOfDimensions != 3) {
        return true;
    }

    return false;
}

template <class DataType, class RepresenterType, class DataReaderType, bool isShapeModel, unsigned Dimenstionality>
void buildAndSaveModel(programOptions opt) {
    KernelMapType::const_iterator it = kernelMap.find(opt.strKernel);
    if (it == kernelMap.end()) {
        itkGenericExceptionMacro( << "The kernel '" << opt.strKernel << "' isn't available. Available kernels: " << getAvailableKernelsStr());
    }

    typedef typename DataType::PointType PointType;
    typedef boost::scoped_ptr<const statismo::ScalarValuedKernel<PointType> > MatrixPointerType;
    MatrixPointerType pKernel;
    if (isShapeModel == true) {
        pKernel.reset((statismo::ScalarValuedKernel<PointType>*) it->second.createKernelShape(opt.vKernelParameters));
    } else {
        if (Dimenstionality == Dimensionality2D) {
            pKernel.reset((statismo::ScalarValuedKernel<PointType>*) it->second.createKernel2DDeformation(opt.vKernelParameters));
        } else {
            pKernel.reset((statismo::ScalarValuedKernel<PointType>*) it->second.createKernel3DDeformation(opt.vKernelParameters));
        }
    }

    typedef boost::shared_ptr<statismo::MatrixValuedKernel<PointType> > KernelPointerType;
    KernelPointerType pUnscaledKernel(new statismo::UncorrelatedMatrixValuedKernel<PointType>(pKernel.get(), Dimenstionality));
    KernelPointerType pScaledKernel(new statismo::ScaledKernel<PointType>(pUnscaledKernel.get(), opt.fKernelScale));
    KernelPointerType pStatModelKernel;
    KernelPointerType pModelBuildingKernel;

    typedef statismo::StatisticalModel<DataType> RawModelType;
    typedef boost::shared_ptr<RawModelType> RawModelPointerType;
    typedef typename RepresenterType::DatasetPointerType DatasetPointerType;

    RawModelPointerType pRawStatisticalModel;
    typename RepresenterType::Pointer pRepresenter = RepresenterType::New();
    DatasetPointerType pMean;

    if(opt.strOptionalModelPath != "") {
        try {
            pRawStatisticalModel.reset(statismo::IO<DataType>::LoadStatisticalModel(pRepresenter.GetPointer(),
                                                                                opt.strOptionalModelPath.c_str()));
            pStatModelKernel.reset(new statismo::StatisticalModelKernel<DataType>(pRawStatisticalModel.get()));
            pModelBuildingKernel.reset(new statismo::SumKernel<PointType>(pStatModelKernel.get(), pScaledKernel.get()));
        } catch (statismo::StatisticalModelException& s) {
            itkGenericExceptionMacro(<< "Failed to read the optional model: "<< s.what());
        }
        pMean = pRawStatisticalModel->DrawMean();
    } else {
        pModelBuildingKernel = pScaledKernel;
        typename DataReaderType::Pointer pReferenceReader = DataReaderType::New();
        pReferenceReader->SetFileName(opt.strReferenceFile);
        pReferenceReader->Update();
        pRepresenter->SetReference(pReferenceReader->GetOutput());
        pMean = pRepresenter->IdentitySample();
    }

    typedef itk::LowRankGPModelBuilder<DataType> ModelBuilderType;
    typename ModelBuilderType::Pointer gpModelBuilder = ModelBuilderType::New();
    gpModelBuilder->SetRepresenter(pRepresenter);

    typedef itk::StatisticalModel<DataType> StatisticalModelType;
    typename StatisticalModelType::Pointer pModel;
    pModel = gpModelBuilder->BuildNewModel(pMean, *pModelBuildingKernel.get(), opt.iNrOfBasisFunctions);

    itk::StatismoIO<DataType>::SaveStatisticalModel(pModel, opt.strOutputFileName.c_str());
}

string getAvailableKernelsStr() {
    string ret;
    KernelMapType::size_type mapEnd = kernelMap.size();
    KernelMapType::size_type i = 0;
    for (KernelMapType::const_iterator it = kernelMap.begin(); it != kernelMap.end(); ++it, ++i) {
        if (i + 1 == mapEnd && mapEnd > 1) {
            ret += " and ";
        } else if (i > 0) {
            ret += ", ";
        }
        ret += it->first;
    }
    return ret;
}

po::options_description initializeProgramOptions(programOptions& poParameters) {
    string kernelHelp = "Specifies the kernel (covariance function). The following kernels are available: "+getAvailableKernelsStr();

    po::options_description optMandatory("Mandatory options");
    optMandatory.add_options()
    ("type,t", po::value<string>(&poParameters.strType)->default_value("shape"), "Specifies the type of the model: shape and deformation are the two available types")
    ("dimensionality,d", po::value<unsigned>(&poParameters.uNumberOfDimensions)->default_value(3), "Dimensionality of the input image (only available if you're building a deformation model)")
    ("kernel,k", po::value<string>(&poParameters.strKernel), kernelHelp.c_str())
    ("parameters,p", po::value<vector<string> >(&poParameters.vKernelParameters)->multitoken(), "Specifies the kernel parameters. The Parameters depend on the kernel")
    ("scale,s", po::value<float>(&poParameters.fKernelScale)->default_value(1), "A Scaling factor with which the Kernel will be scaled")
    ("numberofbasisfunctions,n", po::value<int>(&poParameters.iNrOfBasisFunctions), "Number of basis functions/parameters the model will have")
    ("output-file,o", po::value<string>(&poParameters.strOutputFileName), "Name of the output file where the model will be saved")
    ;
    po::options_description optAdditional("Optional options");
    optAdditional.add_options()
    ("reference,r", po::value<string>(&poParameters.strReferenceFile), "The reference that will be used to build the model")
    ("input-model,m", po::value<string>(&poParameters.strOptionalModelPath), "Extends an existing model with data from the specified kernel. This is useful to extend existing models in case of insufficient data.")
    ("help,h", po::bool_switch(&poParameters.bDisplayHelp), "Display this help message")

    ;

    po::options_description optAllOptions;
    optAllOptions.add(optMandatory).add(optAdditional);
    return optAllOptions;
}


//
// Copyright (c) 2011-2012 Krzysztof Jusiak (krzysztof at jusiak dot net)
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef QFSM_AUX_FLATTEN_HPP
#define QFSM_AUX_FLATTEN_HPP

#include <boost/mpl/vector.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/back_inserter.hpp>

namespace QFsm
{
namespace Aux
{

template<typename TSequence> struct Flatten : boost::mpl::fold
    <
        TSequence,
        boost::mpl::vector0<>,
        boost::mpl::copy
        <
            boost::mpl::if_
            <
                boost::mpl::is_sequence<boost::mpl::_2>,
                boost::mpl::_2,
                typename boost::mpl::vector<boost::mpl::_2>::type
            >,
            boost::mpl::back_inserter<boost::mpl::_1>
        >
    >
{ };

} // namespace Aux
} // namespace QFsm

#endif


#include <fast_tf/fast_tf.hpp>

#include <Eigen/Core>
// IWYU pragma: no_include "Eigen/src/Core/GeneralProduct.h"
// IWYU pragma: no_include "Eigen/src/Geometry/Quaternion.h"
// IWYU pragma: no_include "Eigen/src/Geometry/Translation.h"
// IWYU pragma: no_include "src/Core/DenseBase.h"
// IWYU pragma: no_include "src/Core/MatrixBase.h"

#include <algorithm>
#include <cassert>
#include <iterator>
#include <stdexcept>
#include <type_traits>
#include <utility>

// the __has_builtin is defined under gcc and under clang
// see https://gcc.gnu.org/onlinedocs/cpp/_005f_005fhas_005fbuiltin.html
// see https://clang.llvm.org/docs/LanguageExtensions.html
// in c++20 we can use this without intrinsics:
// see https://en.cppreference.com/w/cpp/language/attributes/likely
#ifndef __has_builtin
#define __has_builtin(x) 0
#endif

#if __has_builtin(__builtin_expect)
#define likely(x) __builtin_expect((x), 1)
#define unlikely(x) __builtin_expect((x), 0)
#else
// if we don't have the feature then there is no better prediction
#define likely(x) x
#define unlikely(x) x
#endif

namespace fast_tf {
namespace detail {

/// @brief Creates a joined transform from the splitted data.
static Eigen::Isometry3d
join(const transform& _split) noexcept {
  return Eigen::Isometry3d{_split.translation * _split.rotation};
}

/// @brief Creates a split transform from the joined data.
static transform
split(const Eigen::Isometry3d& _join) noexcept {
  return {Eigen::Translation3d{_join.translation()},
          Eigen::Quaterniond{_join.rotation()}};
}

static_transform::static_transform(const Eigen::Isometry3d& _data) noexcept :
    data_(_data) {}

static_transform::static_transform(const transform& _data) noexcept :
    data_(join(_data)) {}

void
static_transform::set(const Eigen::Isometry3d& _data) noexcept {
  data_ = _data;
}

void
static_transform::set(const transform& _data) noexcept {
  data_ = join(_data);
}

counter::scoped_signal::scoped_signal(const counter& _c) :
    counter_(++_c.counter_) {}

counter::scoped_signal::~scoped_signal() { --counter_; }

dynamic_transform::dynamic_transform() noexcept :
    dur_(std::chrono::duration_cast<duration_t>(
        std::chrono::milliseconds(500))) {}

dynamic_transform::dynamic_transform(const duration_t& _dur) noexcept :
    dur_(_dur) {
  assert(dur_ >= duration_t(0) && "duration must be positive");
}

void
dynamic_transform::set(const time_t& _time, const transform& _data) noexcept {
  // mostlikely the data will come in in order and emplace_hint will not change
  // the entry if its already present.
  map_.emplace_hint(map_.end(), _time, _data);

  // check that the map is not empty - we need to acces the last valid element
  assert(!map_.empty() && "map cannot be empty");
  // rebalance
  auto lb = map_.lower_bound(std::prev(map_.end())->first - dur_);
  map_.erase(map_.begin(), lb);
}

void
dynamic_transform::set(const time_t& _time,
                       const Eigen::Isometry3d& _data) noexcept {
  set(_time, split(_data));
}

/// @brief returns the vector interpolation between _l and _r.
/// @param _l left value.
/// @param _r right value.
/// @param _t the scale, must be in the interval [0, 1].
static Eigen::Vector3d
lerp(const Eigen::Vector3d& _l, const Eigen::Vector3d& _r, double _t) noexcept {
  return _l * _t + (1 - _t) * _r;
}

/// @brief returns the transform interpolation between _l and _r.
/// @param _l left value.
/// @param _r right value.
/// @param _t the scale, must be in the interval [0, 1].
static transform
lerp(const transform& _l, const transform& _r, double _t) noexcept {
  assert(_t >= 0 && "ratio must be bigger than 0");
  assert(_t <= 1 && "ratio must be smaller than 1");

  return {Eigen::Translation3d{
              lerp(_l.translation.vector(), _r.translation.vector(), _t)},
          _l.rotation.slerp(_t, _r.rotation)};
}

Eigen::Isometry3d
dynamic_transform::get(const time_t& _time) const {
  if (_time.time_since_epoch().count() == 0) {
    if (map_.empty())
      throw std::runtime_error("no data");
    return join(std::prev(map_.end())->second);
  }

  // get the first element not smaller then _time
  // todo check if assume last helps us..
  const auto lb = map_.lower_bound(_time);

  // we will end up in this case if the map_ is either empty or if the stored
  // data is too old
  if (lb == map_.end())
    throw std::runtime_error("no matching data");

  // if the time stamps match exactly, we don't need to interpolate
  if (lb->first == _time)
    return join(lb->second);
  else {
    // check if we have two points to interpolate
    if (lb == map_.begin())
      throw std::runtime_error("no matching data: extrapolation in the past");
    // get the prev iterator
    const auto lower = std::prev(lb);
    // our count() returns a signed integer. the default duration is
    // nanoseconds. the nanoseconds must cover +/- 292 years. see
    // https://en.cppreference.com/w/cpp/chrono/duration
    const auto ratio = static_cast<double>((_time - lower->first).count()) /
                       (lb->first - lower->first).count();

    return join(lerp(lower->second, lb->second, ratio));
  }
}

transform_tree::node::node(variable_transform&& _data) noexcept :
    data(std::move(_data)) {}

transform_tree::node::node(variable_transform&& _data,
                           const node& _parent) noexcept :
    depth(_parent.depth + 1), parent(&_parent), data(std::move(_data)) {}

/// @brief factory returning either base_sequence or dynamic_transform
/// instances.
static variable_transform
sequence_factory(bool _static) noexcept {
  if (_static)
    return static_transform{};
  else
    return dynamic_transform{};
}

transform_tree::pair_type
transform_tree::emplace_parent(const std::string& _parent_frame,
                               const std::string& _child_frame,
                               bool _is_static) {
  // check if the parent exists - this is the basic default case and we might
  // mark it as likely.
  if (auto parent = data_.find(_parent_frame); parent != data_.end())
    return std::make_pair(parent, false);

  // if the parent does not exist, we may try to create a new parent node.
  // this only works if the tree is either empty or if the child link is the
  // current root. in all other cases we will throw.
  if (_parent_frame.empty())
    throw std::runtime_error("a parent-frame cannot be empty");

  if (!empty()) {
    // check if the child-frame-id is the current root. if not - we must reject
    // the init-call since we already have a valid root-node.
    assert(root_ != data_.end() && "root cannot be data_.end()");
    if (root_->first != _child_frame)
      throw std::runtime_error("cannot reinit the map");

    // increment the map's depth counters
    for (auto& n : data_)
      ++n.second.depth;
  }
  // create the root node and wire up the connections. note: the root node is
  // always static.
  const auto res = data_.emplace(_parent_frame, node{static_transform{}});

  assert(res.second && "failed to create the root node");

  // in the case the child-frame-id is the current root
  if (root_ != data_.end()) {
    root_->second.parent = &res.first->second;
    // the root is always static - but if the root changes, the link might
    // become dynamic.
    if (!_is_static)
      root_->second.data = dynamic_transform{};
  }
  root_ = res.first;
  return res;
}

transform_tree::pair_type
transform_tree::emplace(const std::string& _parent_frame,
                        const std::string& _child_frame, bool _is_static) {
  // check if the input is just self-transform
  if (_parent_frame == _child_frame)
    throw std::runtime_error("self-transform detected");

  // get or create a new parent node. this call will throw if the tree would be
  // ill-formed.
  const auto parent = emplace_parent(_parent_frame, _child_frame, _is_static);

  // check if the child is known
  auto child = data_.find(_child_frame);
  if (unlikely(child == data_.end())) {
    // create a new node
    if (_child_frame.empty())
      throw std::runtime_error("a child-frame cannot be empty");

    auto res = data_.emplace(
        _child_frame, node{sequence_factory(_is_static), parent.first->second});
    assert(res.second && "failed to insert new child");
    return res;
  }
  else {
    // make sure that the newly added link is consistent. this will also
    // prevent us from creating cyclic graphs.
    if (child->second.parent != &parent.first->second)
      throw std::runtime_error("child cannot have multiple parents");

    return std::make_pair(child, parent.second);
  }
}

void
transform_tree::merge(transform_tree& _other) {
  // calling this on an empty tree is a noop.
  if (_other.empty() || empty())
    return;

  // cn denotes the connecting link between the trees
  impl_t::iterator cn;
  if (cn = data_.find(_other.root_->first); cn != data_.end()) {
    // merge other into this
    for (auto& p : _other.data_)
      p.second.depth += cn->second.depth;

    // rewire the pointers to the root
    for (auto& p : _other.data_)
      if (p.second.parent == &_other.root_->second)
        p.second.parent = &cn->second;
  }
  else if (cn = _other.data_.find(root_->first); cn != _other.data_.end()) {
    // merge this into other
    for (auto& p : data_)
      p.second.depth += cn->second.depth;

    for (auto& p : _other.data_)
      if (p.second.parent == &cn->second)
        p.second.parent = &root_->second;
    root_->second = cn->second;
    root_ = _other.root_;
  }
  else
    throw std::runtime_error("cannot merge unconnected trees");

  // pass over the members from _other to this.
  data_.merge(_other.data_);

  // only root shall remain in the _other since this is the connecting link.
  assert(_other.data_.size() == 1 && "duplicated links detected");
  _other.data_.clear();
  _other.root_ = _other.data_.end();
}

transform_buffer::transform_buffer() : trees_(1) {}

transform_tree::pair_type
transform_buffer::emplace(const std::string& _parent_frame,
                          const std::string& _child_frame, bool _is_static) {
  // try to add the new transform to one of the existing trees. mostlikely we
  // will have only one tree.
  for (auto& tree : trees_) {
    try {
      return tree.emplace(_parent_frame, _child_frame, _is_static);
    }
    catch (std::runtime_error& _ex) {
      // completely normal phenomenon
      continue;
    }
  }

  // if we couldn't add the passed _tf, we will try to create a new tree
  transform_tree new_tree;
  // this might still throw - but this would be a logic error!
  const auto node = new_tree.emplace(_parent_frame, _child_frame, _is_static);
  trees_.emplace_back(std::move(new_tree));

  return node;
}

std::pair<transform_buffer::trees_t::const_iterator,
          transform_tree::const_iterator>
transform_buffer::find(const std::string& _frame) const noexcept {
  for (auto tree = trees_.begin(); tree != trees_.end(); ++tree) {
    if (auto res = tree->data_.find(_frame); res != tree->data_.end())
      return std::make_pair(tree, res);
  }
  return std::make_pair(trees_.end(), transform_tree::const_iterator{});
}

bool
transform_buffer::merge(const std::string& _common_frame) {
  auto has_frame = [&_common_frame](const transform_tree& _tree) -> bool {
    return _tree.data_.count(_common_frame);
  };
  auto t1 = std::find_if(trees_.begin(), trees_.end(), has_frame);
  assert(t1 != trees_.end() && "the frame must known to at least one tree");
  auto t2 = std::find_if(std::next(t1), trees_.end(), has_frame);
  if (t2 == trees_.end())
    return false;

  // we may throw if the tree will become ill-formed
  t1->merge(*t2);
  trees_.erase(t2);
  return true;
}

void
transform_buffer::merge(const std::string& _parent_frame,
                        const std::string& _child_frame) {
  // try to merge over the parent frame
  merge(_parent_frame) || merge(_child_frame);
}

void
transform_buffer::set(const std::string& _parent_frame,
                      const std::string& _child_frame,
                      const time_t& _stamp_time, const transform& _tf,
                      bool _is_static) {
  std::unique_lock<std::mutex> l(m_);
  auto node = emplace(_parent_frame, _child_frame, _is_static);

  if (_is_static)
    std::get<0>(node.first->second.data).set(_tf);
  else {
    // dt is the dynamic transform
    auto& dt = std::get<1>(node.first->second.data);
    dt.set(_stamp_time, _tf);
    // check if there is someone waiting for this transform
    if (dt)
      cv_.notify_all();
  }

  // we didn't create a new node - so its fine!
  if (!node.second)
    return;

  // will throw if the tree will become ill-formed
  merge(_parent_frame, _child_frame);

  // notify if someone is waiting
  if (c_)
    cv_.notify_all();
}

void
transform_buffer::set(const std::string& _parent_frame,
                      const std::string& _child_frame,
                      const time_t& _stamp_time, const Eigen::Isometry3d& _tf,
                      bool _is_static) {
  set(_parent_frame, _child_frame, _stamp_time, split(_tf), _is_static);
}

Eigen::Isometry3d
transform_buffer::get_dynamic_transform(
    const time_t& _query_time, const time_t& _end_time,
    const dynamic_transform& _dyn_tr,
    std::unique_lock<std::mutex>& _lock) const {
  counter::scoped_signal signal{_dyn_tr};
  do {
    try {
      return _dyn_tr.get(_query_time);
    }
    catch (const std::runtime_error& _ex) {
      continue;
    }
  } while (cv_.wait_until(_lock, _end_time) != std::cv_status::timeout);
  throw std::runtime_error("no transform available");
}

static void
advance(const transform_tree::node*& _node) {
  _node = _node->parent;
  assert(_node && "tree holds a nullptr");
}

Eigen::Isometry3d
transform_buffer::get(const std::string& _target_frame,
                      const std::string& _source_frame,
                      const time_t& _query_time,
                      const duration_t& _tolerance) const {
  if (_tolerance < duration_t{})
    throw std::runtime_error("tolerance cannot be negative");
  const auto end_time = clock_t::now() + _tolerance;

  std::unique_lock<std::mutex> l{m_};
  // find the correct iterators
  auto target = find(_target_frame);
  auto source = find(_source_frame);

  while (!same_tree(target.first, source.first)) {
    detail::counter::scoped_signal ss{c_};
    if (cv_.wait_until(l, end_time) == std::cv_status::timeout)
      throw std::runtime_error("timeout: unconnected trees");

    // try again to connect the query
    target = find(_target_frame);
    source = find(_source_frame);
  }

  assert(same_tree(target.first, source.first) && "unconnected trees");

  const transform_tree::node* s = &source.second->second;
  const transform_tree::node* t = &target.second->second;

  // walk up the tree until we have reached the same depth for both
  Eigen::Isometry3d source_root(Eigen::Isometry3d::Identity());
  Eigen::Isometry3d target_root(Eigen::Isometry3d::Identity());

  // we need a lot of local variables for this call - so we make it a lambda.
  // note: the static_stansform returns by reference; the dynamic_transform
  // returns by value.
  auto get_transform = [&](const variable_transform& _var_tr,
                           const Eigen::Isometry3d& _right) {
    if (_var_tr.index() == 0)
      return std::get<0>(_var_tr).get() * _right;
    else
      return get_dynamic_transform(_query_time, end_time, std::get<1>(_var_tr),
                                   l) *
             _right;
  };

  // walk up with both until both branches meet
  while (t != s) {
    if (t->depth < s->depth) {
      source_root = get_transform(s->data, source_root);
      advance(s);
    }
    else if (t->depth > s->depth) {
      target_root = get_transform(t->data, target_root);
      advance(t);
    }
    else {
      source_root = get_transform(s->data, source_root);
      target_root = get_transform(t->data, target_root);

      advance(s);
      advance(t);
    }
  }

  return {target_root.inverse() * source_root};
}

}  // namespace detail
}  // namespace fast_tf


#include <iostream>
#include "definicoes.hpp"
#include "lqrdados.hpp"
#include "funcoes_auxiliares.hpp"
#include <Eigen/Dense>

int
main(int argc, char **argv)
{
// Construct LQR
	LQRDados LQR;

	std::cout << ("Classe LQR contruida!") << std::endl;

	LQR.imprime_LQR();

	return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file Potentials.cpp
 * @date March 24, 2011
 * @author Frank Dellaert
 */

#include <gtsam/discrete/Potentials.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <boost/format.hpp>

using namespace std;

namespace gtsam {

  // explicit instantiation
  template class DecisionTree<Index, double> ;
  template class AlgebraicDecisionTree<Index> ;

  /* ************************************************************************* */
  double Potentials::safe_div(const double& a, const double& b) {
    // cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b));
    // The use for safe_div is when we divide the product factor by the sum factor.
    // If the product or sum is zero, we accord zero probability to the event.
    return (a == 0 || b == 0) ? 0 : (a / b);
  }

  /* ******************************************************************************** */
  Potentials::Potentials() :
      ADT(1.0) {
  }

  /* ******************************************************************************** */
  Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) :
      ADT(decisionTree), cardinalities_(keys.cardinalities()) {
  }

  /* ************************************************************************* */
  bool Potentials::equals(const Potentials& other, double tol) const {
    return ADT::equals(other, tol);
  }

  /* ************************************************************************* */
  void Potentials::print(const string& s,
      const IndexFormatter& formatter) const {
    cout << s << "\n  Cardinalities: ";
    BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
      cout << formatter(key.first) << "=" << formatter(key.second) << " ";
    cout << endl;
    ADT::print(" ");
  }

  /* ************************************************************************* */
  template<class P>
  void Potentials::remapIndices(const P& remapping) {
    // Permute the _cardinalities (TODO: Inefficient Consider Improving)
    DiscreteKeys keys;
    map<Index, Index> ordering;

    // Get the original keys from cardinalities_
    BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
      keys & key;

    // Perform Permutation
    BOOST_FOREACH(DiscreteKey& key, keys) {
      ordering[key.first] = remapping[key.first];
      key.first = ordering[key.first];
    }

    // Change *this
    AlgebraicDecisionTree<Index> permuted((*this), ordering);
    *this = permuted;
    cardinalities_ = keys.cardinalities();
  }

  /* ************************************************************************* */
  void Potentials::permuteWithInverse(const Permutation& inversePermutation) {
    remapIndices(inversePermutation);
  }

  /* ************************************************************************* */
  void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) {
    remapIndices(inverseReduction);
  }

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

} // namespace gtsam


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

#ifdef MTL_WITH_INITLIST
template <typename Vector>
void test(const char* name)
{
    const Vector v= {3, 4, 5};

    mtl::io::tout << name << ", v: " << v << "\n";
    MTL_THROW_IF(v[0] != 3.0, mtl::runtime_error("wrong"));

    Vector w;
    w= {2, 4, 7};
    mtl::io::tout << "w: " << w << "\n";
    MTL_THROW_IF(w[0] != 2.0, mtl::runtime_error("wrong"));
}
#else
template <typename Vector>
void test(const char* ) {}
#endif

int main(int , char**)
{
    using mtl::vec::parameters;
    using namespace mtl;

    test<dense_vector<float> >("test float");
    test<dense_vector<double> >("test double");
    test<dense_vector<float, parameters<row_major> > >("test float in row vector");

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// density.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_DENSITY_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_DENSITY_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <functional>
#include <boost/range.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/accumulators_fwd.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>

namespace boost { namespace accumulators
{

///////////////////////////////////////////////////////////////////////////////
// cache_size and num_bins named parameters
//
BOOST_PARAMETER_NESTED_KEYWORD(tag, density_cache_size, cache_size)
BOOST_PARAMETER_NESTED_KEYWORD(tag, density_num_bins, num_bins)

BOOST_ACCUMULATORS_IGNORE_GLOBAL(density_cache_size)
BOOST_ACCUMULATORS_IGNORE_GLOBAL(density_num_bins)

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // density_impl
    //  density histogram
    /**
        @brief Histogram density estimator

        The histogram density estimator returns a histogram of the sample distribution. The positions and sizes of the bins
        are determined using a specifiable number of cached samples (cache_size). The range between the minimum and the
        maximum of the cached samples is subdivided into a specifiable number of bins (num_bins) of same size. Additionally,
        an under- and an overflow bin is added to capture future under- and overflow samples. Once the bins are determined,
        the cached samples and all subsequent samples are added to the correct bins. At the end, a range of std::pair is
        return, where each pair contains the position of the bin (lower bound) and the samples count (normalized with the
        total number of samples).

        @param  density_cache_size Number of first samples used to determine min and max.
        @param  density_num_bins Number of bins (two additional bins collect under- and overflow samples).
    */
    template<typename Sample>
    struct density_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef std::vector<float_type> array_type;
        // for boost::result_of
        typedef iterator_range<typename histogram_type::iterator> result_type;

        template<typename Args>
        density_impl(Args const &args)
            : cache_size(args[density_cache_size])
            , cache(cache_size)
            , num_bins(args[density_num_bins])
            , samples_in_bin(num_bins + 2, 0.)
            , bin_positions(num_bins + 2)
            , histogram(
                num_bins + 2
              , std::make_pair(
                    numeric::average(args[sample | Sample()],(std::size_t)1)
                  , numeric::average(args[sample | Sample()],(std::size_t)1)
                )
              )
            , is_dirty(true)
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            this->is_dirty = true;

            std::size_t cnt = count(args);

            // Fill up cache with cache_size first samples
            if (cnt <= this->cache_size)
            {
                this->cache[cnt - 1] = args[sample];
            }

            // Once cache_size samples have been accumulated, create num_bins bins of same size between
            // the minimum and maximum of the cached samples as well as under and overflow bins.
            // Store their lower bounds (bin_positions) and fill the bins with the cached samples (samples_in_bin).
            if (cnt == this->cache_size)
            {
                float_type minimum = numeric::average((min)(args), (std::size_t)1);
                float_type maximum = numeric::average((max)(args), (std::size_t)1);
                float_type bin_size = numeric::average(maximum - minimum, this->num_bins );

                // determine bin positions (their lower bounds)
                for (std::size_t i = 0; i < this->num_bins + 2; ++i)
                {
                    this->bin_positions[i] = minimum + (i - 1.) * bin_size;
                }

                for (typename array_type::const_iterator iter = this->cache.begin(); iter != this->cache.end(); ++iter)
                {
                    if (*iter < this->bin_positions[1])
                    {
                        ++(this->samples_in_bin[0]);
                    }
                    else if (*iter >= this->bin_positions[this->num_bins + 1])
                    {
                        ++(this->samples_in_bin[this->num_bins + 1]);
                    }
                    else
                    {
                        typename array_type::iterator it = std::upper_bound(
                            this->bin_positions.begin()
                          , this->bin_positions.end()
                          , *iter
                        );

                        std::size_t d = std::distance(this->bin_positions.begin(), it);
                        ++(this->samples_in_bin[d - 1]);
                    }
                }
            }
            // Add each subsequent sample to the correct bin
            else if (cnt > this->cache_size)
            {
                if (args[sample] < this->bin_positions[1])
                {
                    ++(this->samples_in_bin[0]);
                }
                else if (args[sample] >= this->bin_positions[this->num_bins + 1])
                {
                    ++(this->samples_in_bin[this->num_bins + 1]);
                }
                else
                {
                    typename array_type::iterator it = std::upper_bound(
                        this->bin_positions.begin()
                      , this->bin_positions.end()
                      , args[sample]
                    );

                    std::size_t d = std::distance(this->bin_positions.begin(), it);
                    ++(this->samples_in_bin[d - 1]);
                }
            }
        }

        /**
            @pre The number of samples must meet or exceed the cache size
        */
        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                // creates a vector of std::pair where each pair i holds
                // the values bin_positions[i] (x-axis of histogram) and
                // samples_in_bin[i] / cnt (y-axis of histogram).

                for (std::size_t i = 0; i < this->num_bins + 2; ++i)
                {
                    this->histogram[i] = std::make_pair(this->bin_positions[i], numeric::average(this->samples_in_bin[i], count(args)));
                }
            }
            // returns a range of pairs
            return make_iterator_range(this->histogram);
        }

    private:
        std::size_t            cache_size;      // number of cached samples
        array_type             cache;           // cache to store the first cache_size samples
        std::size_t            num_bins;        // number of bins
        array_type             samples_in_bin;  // number of samples in each bin
        array_type             bin_positions;   // lower bounds of bins
        mutable histogram_type histogram;       // histogram
        mutable bool is_dirty;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::density
//
namespace tag
{
    struct density
      : depends_on<count, min, max>
      , density_cache_size
      , density_num_bins
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::density_impl<mpl::_1> impl;

        #ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
        /// tag::density::cache_size named parameter
        /// tag::density::num_bins named parameter
        static boost::parameter::keyword<density_cache_size> const cache_size;
        static boost::parameter::keyword<density_num_bins> const num_bins;
        #endif
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::density
//
namespace extract
{
    extractor<tag::density> const density = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(density)
}

using extract::density;

// So that density can be automatically substituted
// with weighted_density when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::density>
{
    typedef tag::weighted_density type;
};

template<>
struct feature_of<tag::weighted_density>
  : feature_of<tag::density>
{
};

}} // namespace boost::accumulators

#endif


//
//  Copyright (c) 2013, Novartis Institutes for BioMedical Research Inc.
//  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 Novartis Institutes for BioMedical Research Inc.
//       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
// OWNER 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.
//

#include <Geometry/point.h>
#include <Numerics/Vector.h>
#include <GraphMol/RDKitBase.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/Substruct/SubstructMatch.h>
#include "USRDescriptor.h"

#include <boost/flyweight.hpp>
#include <boost/flyweight/key_value.hpp>
#include <boost/flyweight/no_tracking.hpp>

namespace RDKit {

namespace {
void calcDistances(const RDGeom::Point3DConstPtrVect &coords,
                   const RDGeom::Point3D &point,
                   std::vector<double> &distances) {
  distances.resize(coords.size());
  unsigned int i = 0;
  // loop over coordinates
  for (const auto *tpp : coords) {
    distances[i++] = (*tpp - point).length();
  }
}

void calcCentroid(const RDGeom::Point3DConstPtrVect &coords,
                  RDGeom::Point3D &pt) {
  PRECONDITION(!coords.empty(), "no coordinates");
  // set pt to zero
  pt *= 0.0;
  // loop over coordinates
  for (const auto *opt : coords) {
    pt += *opt;
  }
  pt /= coords.size();
}

unsigned int largestValId(const std::vector<double> &v) {
  PRECONDITION(!v.empty(), "no values");
  double res = v[0];
  unsigned int id = 0;
  for (unsigned int i = 1; i < v.size(); ++i) {
    if (v[i] > res) {
      res = v[i];
      id = i;
    }
  }
  return id;
}

unsigned int smallestValId(const std::vector<double> &v) {
  PRECONDITION(!v.empty(), "no values");
  double res = v[0];
  unsigned int id = 0;
  for (unsigned int i = 1; i < v.size(); ++i) {
    if (v[i] < res) {
      res = v[i];
      id = i;
    }
  }
  return id;
}

void calcMoments(const std::vector<double> &dist,
                 std::vector<double> &descriptor, int idx) {
  std::vector<double> moments(3, 0.0);
  unsigned int numPts = dist.size();
  if (numPts > 0) {
    // 1. moment: mean
    for (unsigned int i = 0; i < numPts; ++i) {
      moments[0] += dist[i];
    }
    moments[0] /= numPts;
    // 2. moment: standard deviation
    // 3. moment: cubic root of skewness
    for (unsigned int i = 0; i < numPts; ++i) {
      double diff = dist[i] - moments[0];
      moments[1] += diff * diff;
      moments[2] += diff * diff * diff;
    }
    moments[1] = sqrt(moments[1] / numPts);
    moments[2] /= numPts;
    if (moments[1] == 0) {
      moments[2] = 0.0;
    } else {
#ifdef WIN32
      moments[2] = moments[2] / (moments[1] * moments[1] * moments[1]);
      if (moments[2] >= 0)
        moments[2] = pow(moments[2], 1. / 3.);
      else
        moments[2] = -1. * pow(-1. * moments[2], 1. / 3.);
#else
      moments[2] = cbrt(moments[2] / (moments[1] * moments[1] * moments[1]));
#endif
    }
  }
  // add moments to descriptor
  std::copy(moments.begin(), moments.end(), descriptor.begin() + idx);
}

class ss_matcher {
 public:
  ss_matcher(){};
  ss_matcher(const std::string &pattern) {
    RDKit::RWMol *p = RDKit::SmartsToMol(pattern);
    TEST_ASSERT(p);
    m_matcher.reset(p);
  };
  const RDKit::ROMol *getMatcher() const { return m_matcher.get(); };

 private:
  RDKit::ROMOL_SPTR m_matcher;
};

// Definitions for feature points from
// https://bitbucket.org/aschreyer/usrcat/src/2aa77f970c2c/usrcat/__init__.py
const char *smartsPatterns[4] = {
    "[#6+0!$(*~[#7,#8,F]),SH0+0v2,s+0,S^3,Cl+0,Br+0,I+0]",  // hydrophobic
    "[a]",                                                  // aromatic
    "[$([O,S;H1;v2]-[!$(*=[O,N,P,S])]),$([O,S;H0;v2]),$([O,S;-]),\
$([N&v3;H1,H2]-[!$(*=[O,N,P,S])]),$([N;v3;H0]),$([n,o,s;+0]),F]",  // acceptor
    "[N!H0v3,N!H0+v4,OH+0,SH+0,nH+0]"                              // donor
};
std::vector<std::string> featureSmarts(smartsPatterns, smartsPatterns + 4);
typedef boost::flyweight<boost::flyweights::key_value<std::string, ss_matcher>,
                         boost::flyweights::no_tracking>
    pattern_flyweight;

void getAtomIdsForFeatures(const ROMol &mol,
                           std::vector<std::vector<unsigned int>> &atomIds) {
  unsigned int numFeatures = featureSmarts.size();
  PRECONDITION(atomIds.size() == numFeatures,
               "atomIds must have be the same size as featureSmarts");
  std::vector<const ROMol *> featureMatchers;
  featureMatchers.reserve(numFeatures);
  for (const auto &feature : featureSmarts) {
    const ROMol *matcher = pattern_flyweight(feature).get().getMatcher();
    featureMatchers.push_back(matcher);
  }
  for (unsigned int i = 0; i < numFeatures; ++i) {
    std::vector<MatchVectType> matchVect;
    // to maintain thread safety, we have to copy the pattern molecules:
    SubstructMatch(mol, ROMol(*featureMatchers[i], true), matchVect);
    for (const auto &mv : matchVect) {
      for (auto mi : mv) {
        atomIds[i].push_back(mi.second);
      }
    }
  }  // end loop over features
}

}  // end namespace

namespace Descriptors {

void USR(const ROMol &mol, std::vector<double> &descriptor, int confId) {
  PRECONDITION(descriptor.size() == 12, "descriptor must have 12 elements");
  unsigned int na = mol.getNumAtoms();
  // check that number of atoms > 3
  if (na < 3) {
    throw ValueErrorException("Number of atoms must be greater than 3");
  }
  // check that minimum a conformer exists
  if (mol.getNumConformers() == 0) {
    throw ConformerException("No conformations available on this molecule");
  }

  const Conformer &conf = mol.getConformer(confId);
  RDGeom::Point3DConstPtrVect coords(na);
  // loop over atoms
  for (unsigned int ai = 0; ai < na; ++ai) {
    coords[ai] = &conf.getAtomPos(ai);
  }
  // the four distances
  std::vector<std::vector<double>> dist(4);
  std::vector<RDGeom::Point3D> points(4);
  calcUSRDistributions(coords, dist, points);

  calcUSRFromDistributions(dist, descriptor);
}

void USRCAT(const ROMol &mol, std::vector<double> &descriptor,
            std::vector<std::vector<unsigned int>> &atomIds, int confId) {
  unsigned int na = mol.getNumAtoms();
  // check that number of atoms > 3
  if (na < 3) {
    throw ValueErrorException("Number of atoms must be greater than 3");
  }
  // check that minimum a conformer exists
  if (mol.getNumConformers() == 0) {
    throw ConformerException("No conformations available on this molecule");
  }

  // get atom selections
  unsigned int numClasses = atomIds.size();
  if (numClasses == 0) {  // no user input, use default values
    numClasses = featureSmarts.size();
    atomIds.resize(numClasses);
    getAtomIdsForFeatures(mol, atomIds);
  }
  PRECONDITION(descriptor.size() == 12 * (numClasses + 1),
               "descriptor wrong size");

  const Conformer &conf = mol.getConformer(confId);
  RDGeom::Point3DConstPtrVect coords(na);
  // loop over atoms
  for (unsigned int ai = 0; ai < na; ++ai) {
    coords[ai] = &conf.getAtomPos(ai);
  }
  // the original USR
  std::vector<std::vector<double>> distribs(4);
  std::vector<RDGeom::Point3D> points(4);
  calcUSRDistributions(coords, distribs, points);
  std::vector<double> tmpDescriptor(12);
  calcUSRFromDistributions(distribs, tmpDescriptor);
  std::copy(tmpDescriptor.begin(), tmpDescriptor.end(), descriptor.begin());

  // loop over the atom selections
  unsigned int featIdx = 12;
  for (const auto &atomsInClass : atomIds) {
    // reduce the coordinates to the atoms of interest
    RDGeom::Point3DConstPtrVect reducedCoords;
    reducedCoords.reserve(atomsInClass.size());
    for (const auto idx : atomsInClass) {
      reducedCoords.push_back(coords[idx]);
    }
    calcUSRDistributionsFromPoints(reducedCoords, points, distribs);
    calcUSRFromDistributions(distribs, tmpDescriptor);
    std::copy(tmpDescriptor.begin(), tmpDescriptor.end(),
              descriptor.begin() + featIdx);
    featIdx += 12;
  }
}

void calcUSRDistributions(const RDGeom::Point3DConstPtrVect &coords,
                          std::vector<std::vector<double>> &dist,
                          std::vector<RDGeom::Point3D> &points) {
  PRECONDITION(dist.size() == 4, "dist must have 4 elements");
  PRECONDITION(points.size() == 4, "points must have 4 elements");
  // ctd = centroid
  calcCentroid(coords, points[0]);
  calcDistances(coords, points[0], dist[0]);
  // catc = closest atom to centroid
  points[1] = (*coords[smallestValId(dist[0])]);
  calcDistances(coords, points[1], dist[1]);
  // fatc = farthest atom to centroid
  points[2] = (*coords[largestValId(dist[0])]);
  calcDistances(coords, points[2], dist[2]);
  // fatf = farthest atom to fatc
  points[3] = (*coords[largestValId(dist[2])]);
  calcDistances(coords, points[3], dist[3]);
}

void calcUSRDistributionsFromPoints(const RDGeom::Point3DConstPtrVect &coords,
                                    const std::vector<RDGeom::Point3D> &points,
                                    std::vector<std::vector<double>> &dist) {
  PRECONDITION(points.size() == dist.size(),
               "points and dist must have the same size");
  for (unsigned int i = 0; i < points.size(); ++i) {
    calcDistances(coords, points[i], dist[i]);
  }
}

void calcUSRFromDistributions(const std::vector<std::vector<double>> &dist,
                              std::vector<double> &descriptor) {
  PRECONDITION(descriptor.size() == 3 * dist.size(),
               "descriptor must have 3 times more elements than dist");
  for (unsigned int i = 0; i < dist.size(); ++i) {
    calcMoments(dist[i], descriptor, 3 * i);
  }
}

double calcUSRScore(const std::vector<double> &d1,
                    const std::vector<double> &d2,
                    const std::vector<double> &weights) {
  unsigned int num = 12;  // length of each subset
  PRECONDITION(d1.size() == d2.size(), "descriptors must have the same size");
  PRECONDITION(weights.size() == (d1.size() / num),
               "size of weights not correct");
  double score = 1.0;
  for (unsigned int w = 0; w < (d1.size() / num); ++w) {
    double tmpScore = 0.0;
    unsigned int offset = num * w;
    for (unsigned int i = 0; i < num; ++i) {
      tmpScore += fabs(d1[i + offset] - d2[i + offset]);
    }
    tmpScore /= num;
    score += weights[w] * tmpScore;
  }
  return 1.0 / score;
}

}  // end of namespace Descriptors
}  // end of namespace RDKit


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana.hpp>

#include <boost/hana/concept/comparable.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/comparable.hpp>

#include <type_traits>
using namespace boost::hana;

// Minimal EqualityComparable types
struct eq1 { int value; };
struct eq2 {
    int value;
    constexpr operator eq1() const { return {value}; }
};

template <typename T, typename U, typename = std::enable_if_t<
    (std::is_same<T, eq1>{} || std::is_same<T, eq2>{}) &&
    (std::is_same<U, eq1>{} || std::is_same<U, eq2>{})
>>
constexpr bool operator==(T a, U b)
{ return a.value == b.value; }

template <typename T, typename U, typename = std::enable_if_t<
    (std::is_same<T, eq1>{} || std::is_same<T, eq2>{}) &&
    (std::is_same<U, eq1>{} || std::is_same<U, eq2>{})
>>
constexpr bool operator!=(T a, U b)
{ return !(a == b); }

int main() {
    // equal
    {
        // Two objects of different data types are unequal by default,
        // and no model is provided for two objects of the same data type.
        struct Random1 { }; struct Random2 { };
        BOOST_HANA_CONSTANT_CHECK(not_(equal(Random1{}, Random2{})));
        static_assert(!Comparable<Random1>::value, "");
        static_assert(!Comparable<Random2>::value, "");

        // Provided model for EqualityComparable types
        BOOST_HANA_CONSTEXPR_CHECK(equal(eq1{0}, eq1{0}));
        BOOST_HANA_CONSTEXPR_CHECK(not_(equal(eq1{0}, eq1{1})));
        BOOST_HANA_CONSTEXPR_CHECK(not_(equal(eq1{1}, eq1{0})));

        BOOST_HANA_CONSTEXPR_CHECK(equal(eq1{0}, eq2{0}));
        BOOST_HANA_CONSTEXPR_CHECK(not_(equal(eq1{0}, eq2{1})));
        BOOST_HANA_CONSTEXPR_CHECK(not_(equal(eq1{1}, eq2{0})));

        BOOST_HANA_CONSTEXPR_CHECK(equal(eq2{0}, eq1{0}));
        BOOST_HANA_CONSTEXPR_CHECK(not_(equal(eq2{0}, eq1{1})));
        BOOST_HANA_CONSTEXPR_CHECK(not_(equal(eq2{1}, eq1{0})));
    }

    // laws
    test::TestComparable<int>{make<tuple_tag>(0,1,2,3,4,5)};
    test::TestComparable<unsigned int>{make<tuple_tag>(0u,1u,2u,3u,4u,5u)};
    test::TestComparable<long>{make<tuple_tag>(0l,1l,2l,3l,4l,5l)};
    test::TestComparable<unsigned long>{make<tuple_tag>(0ul,1ul,2ul,3ul,4ul,5ul)};
    test::TestComparable<eq1>{make<tuple_tag>(eq1{0}, eq1{1}, eq1{2}, eq1{3}, eq1{4})};
}