//--------------------------------------------------------------------//
// Digital Scenario Framework                                         //
//  by Giovanni Paolo Vigano', 2021                                   //
//--------------------------------------------------------------------//
//
// Distributed under the MIT Software License.
// See http://opensource.org/licenses/MIT
//

#include <SampleAgent/SampleAgent.h>

#include <discenfw/xp/Experience.h>

#include <discenfw/util/MessageLog.h>
#include <discenfw/util/Rand.h>

#include <boost/config.hpp> // for BOOST_SYMBOL_EXPORT

#include <iostream>
#include <math.h>

namespace sample_agent
{

	using namespace discenfw;
	using namespace discenfw::xp;

	// TODO: define a namespace if needed

	SampleAgent::SampleAgent()
	{
		Configuration = std::make_shared<SampleAgentConfig>();
	}

	SampleAgent::~SampleAgent()
	{
	}

	void SampleAgent::Reset()
	{
		// TODO: clear and reset data

		ActionChoiceCount = 0;
		ExplorationActionCount = 0;
		KnownActions.clear();
		SuccessActions.clear();
	}

	int SampleAgent::ChooseAction(
		const std::shared_ptr<Experience> experience,
		const std::vector<ActionRef>& possibleActions,
		StateRef envState,
		bool updatePolicy
		)
	{
		if (possibleActions.empty())
		{
			return -1;
		}

		if (updatePolicy)
		{
			// Update your policy/policies here
		}

		int chosenAction = -1;

		StateActionRef stateAction;
		stateAction.State = envState;

		// choose an action (index) according to a policy

		// TODO: test a rollout policy

		// Use a Boltzmann distribution for policy

		// temperature: high values make the probability
		// distribution virtually uniform, while low values
		// make the policy nearly a greedy selection.
		float temperature = Configuration->NewActionValue;
		if (temperature < 1.0f)
		{
			temperature = 1.0f;
		}

		// Accumulator of probability distribution:
		// each value represent an upper limit for the range related
		// to the probability of choosing an action.
		// A random number (0..1) will select an action.
		std::vector<float> distribution(possibleActions.size(), 0.0f);
		float denominatorBoltzmann = 0.0f;
		for (int i = 0; i < (int)possibleActions.size(); i++)
		{
			stateAction.Action = possibleActions[i];
			denominatorBoltzmann += expf(GetStateActionValue(experience, stateAction) / temperature);
		}

		//float maxStateVal = -1.0f;

		//bool found = false;

		for (int i = 0; i < (int)possibleActions.size(); i++)
		{
			const ActionRef& action = possibleActions[i];
			float stateActionValue = 0;
			stateAction.Action = possibleActions[i];
			// boost exploration
			if (updatePolicy && Configuration->BoostExploration
				&& !IsStateActionKnown(stateAction))
			{
				chosenAction = i;
				//found = true;
				ExplorationActionCount++;
				ActionChoiceCount++;
				return chosenAction;
			}
			stateActionValue = GetStateActionValue(experience, stateAction);

			// Calculate Boltzmann distribution
			float p = expf(stateActionValue / temperature) / denominatorBoltzmann;
			distribution[i] = i > 0 ? distribution[i - 1] + p : p;

			//if (!found || stateActionValue > maxStateVal)
			//{
			//	maxStateVal = stateActionValue;
			//	found = true;
			//}
			//if (found && stateActionValue > maxStateVal)
			//{
			//	chosenAction = i;
			//}
		}

		// Random number (0..1) used to select an action.
		float randomValue = RandFloat(0.0f, 1.0f);
		for (int i = 0; i < (int)possibleActions.size(); i++)
		{
			if (randomValue < distribution[i])
			{
				chosenAction = i;
				stateAction.Action = possibleActions[i];
				if (!IsStateActionKnown(stateAction))
				{
					ExplorationActionCount++;
				}
				ActionChoiceCount++;
				break;
			}
		}

		//if (chosenAction >= 0)
		//{
		//	ActionChoiceCount++;
		//}
		//else
		//{
		//	chosenAction = RandIndex((int)possibleActions.size());
		//	ExplorationActionCount++;
		//}

		return chosenAction;
	}

	int SampleAgent::GetActionChoiceCount() const
	{
		return ActionChoiceCount;
	}

	int SampleAgent::GetExplorationActionCount() const
	{
		// TODO: return -1 if you cannot provide this information
		return ExplorationActionCount;
	}

	void SampleAgent::ResetStats()
	{
		ActionChoiceCount = 0;
		ExplorationActionCount = 0;
	}

	void SampleAgent::Learn(
		const std::shared_ptr<Experience> experience,
		const std::vector<Transition>& transitionSequence,
		ActionResult lastActionResult
		)
	{
		// TODO: implement here your learning algorithm

		// you can access role datato store/retrieve action values
		// e.g.: role->GetTransitionValue(...); role->SetTransitionValue(...);

		const Transition& lastTransition = transitionSequence.back();
		std::shared_ptr<RoleInfo> role = experience->GetRole();
		EnvironmentStateInfo stateInfo = role->GetStateInfo(lastTransition.EndState);
		KnownActions.insert({ lastTransition.StartState, lastTransition.ActionTaken });

		// State reward
		float R = (float)stateInfo.Reward;
		if (!stateInfo.IsInProgress())
		{
			experience->SetStateActionValue({lastTransition.StartState, lastTransition.ActionTaken}, R);
			for (int i = (int)transitionSequence.size() - 2; i >= 0; i--)
			{
				const Transition& transition = transitionSequence[i];
				const Transition& nextTransition = transitionSequence[i + 1];
				StateActionRef stateAction{ transition.StartState, transition.ActionTaken };
				StateActionRef nextStateAction{ nextTransition.StartState, nextTransition.ActionTaken };
				if (stateInfo.Succeded())
				{
					SuccessActions.insert(nextStateAction);
				}
				if (stateInfo.Succeded() || SuccessActions.find(stateAction) == SuccessActions.end())
				{
					float nextValue = experience->GetStateActionValue(nextStateAction);
					float value = experience->GetStateActionValue(stateAction);
					const float alpha = Configuration->LearningRate;
					value = (1.0f - alpha) * value + alpha * nextValue;

					experience->SetStateActionValue(stateAction, value);
					if (!stateInfo.Succeded())
					{
						//stop here
						break;
					}
				}
			}
		}
	}

	void SampleAgent::SetConfiguration(const std::shared_ptr<IAgentConfiguration> config)
	{
		if (!config)
		{
			LogMessage(LOG_ERROR, "Null agent configuration.", "SampleAgent");
			return;
		}

		// TODO: if you are using a different configuration replace RLConfig
		if (!config->IsA("SampleAgentConfig"))
		{
			LogMessage(LOG_ERROR, "Invalid agent configuration.", "SampleAgent");
			return;
		}

		// TODO: if you are using a different configuration replace RLConfig
		Configuration = std::static_pointer_cast<SampleAgentConfig>(config);

		// TODO: if you are using a different configuration replace the following code

		// fix parameters
		//Configuration->CheckParameters();
	}

	bool SampleAgent::IsStateActionKnown(StateActionRef stateAction)
	{
		return KnownActions.find(stateAction) != KnownActions.end();
	}

	float SampleAgent::GetStateActionValue(
		const std::shared_ptr<Experience> experience,
		const discenfw::xp::StateActionRef& stateAction
		)
	{
		if (experience->StateActionValueDefined(stateAction))
		{
			return experience->GetStateActionValue(stateAction);
		}
		else
		{
			return Configuration->NewActionValue;
		}
	}

	extern "C" BOOST_SYMBOL_EXPORT SampleAgent Agent;
	SampleAgent Agent;

} // namespace sample_agent


#include <iostream>
#include <Eigen/Core>
using namespace std;

void output_eigenMatrix(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>& _matrix){
    std::cout << _matrix << std::endl;
}

int main() {
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> temp;
    Eigen::Matrix<float,  Eigen::Dynamic, Eigen::Dynamic> temp2;
    temp.resize(1, 5);

    temp2.resize(5, 1);
    temp << 1,2,3,4,5;
    cout << temp << endl;
    temp2 << 1,2,3,4,5;
    output_eigenMatrix(temp);
    output_eigenMatrix(temp2);
    std::cout << "Hello, World!" << std::endl;
    return 0;
}


#ifndef UTILS_HPP
#define UTILS_HPP

/**
    @file utils.hpp
    Defines useful utilities.

    @brief Defines useful utility functions.
*/
#include <asl_fixedwing/FloatVecStamped.h>

#include <Eigen/Dense>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Vector3.h>

namespace Utils {

void point_to_eigen3d(const geometry_msgs::PointStamped& p, Eigen::Vector3d& x);

void quat_to_eigen4d(const geometry_msgs::Quaternion& q, Eigen::Vector4d& x);

void vec3_to_eigen3d(const geometry_msgs::Vector3& v, Eigen::Vector3d& x);

void eigen3d_to_point(const Eigen::Vector3d x, geometry_msgs::PointStamped& p);

void eigen4d_to_quat(const Eigen::Vector4d& x, geometry_msgs::Quaternion& q);

void eigen3d_to_vec3(const Eigen::Vector3d& x, geometry_msgs::Vector3& v);

void eigenxd_to_floatvec(const Eigen::VectorXd& x, asl_fixedwing::FloatVecStamped& v);

}

#endif // PLANE_HPP


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_ECLIPTIC_COORD_HPP
#define BOOST_ASTRONOMY_ECLIPTIC_COORD_HPP

#include <iostream>
#include <boost/static_assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

/**
 * The plane containing the Earth’s orbit around the Sun is called
 * the ecliptic and the other planets in our Solar System also move
 * in orbits close to this plane.
 *
 * When making calculations on objects in the Solar System it is
 * therefore often convenient to define positions with respect to
 * the ecliptic, that is, to use the ecliptic coordinate system.
 *
 * Ecliptic Latitude
 * The ecliptic plane is extended until it intersects the celestial
 * sphere to create an “ecliptic equator” as the reference point from
 * which Ecliptic Latitudes are measured. The Ecliptic Latitude, denoted
 * by β, is the angular distance that an object P lies above or below the
 * ecliptic plane and falls within the range ±90◦.
 * Latitudes above the ecliptic plane are positive angles while latitudes
 * below the ecliptic plane are negative angles. An object, such as the Sun,
 * whose orbit lies entirely within the ecliptic plane has an ecliptic latitude
 * of 0◦.
 *
 * Ecliptic Longitude
 * The Ecliptic Longitude, designated by λ, measures how far away an object is
 * from the First Point of Aries. The Ecliptic Longitude is in the range [0◦, 360◦]
 * and measured along the ecliptic toward the First Point of Aries.
 */

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

template
<
    typename CoordinateType = double,
    typename LatQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename LonQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>
>
struct ecliptic_coord : public coord_sys
    <2, bg::cs::spherical<bg::radian>, CoordinateType>
{
  ///@cond INTERNAL
  BOOST_STATIC_ASSERT_MSG(
      ((std::is_same<typename bu::get_dimension<LatQuantity>::type,
          bu::plane_angle_dimension>::value) &&
       (std::is_same<typename bu::get_dimension<LonQuantity>::type,
           bu::plane_angle_dimension>::value)),
      "Latitude and Longitude must be of plane angle type");
  BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
                          "CoordinateType must be a floating-point type");
  ///@endcond
public:
    typedef LatQuantity quantity1;
    typedef LonQuantity quantity2;

    //Default constructor
    ecliptic_coord() {}

    ecliptic_coord
    (
            LatQuantity const &Lat,
            LonQuantity const &Lon
    )
    {
        this->set_lat_lon(Lat, Lon);
    }

    //Create a tuple of Ecliptic Latitude and Ecliptic Longitude
    std::tuple<LatQuantity, LonQuantity> get_lat_lon() const
    {
        return std::make_tuple(this->get_lat(), this->get_lon());
    }

    //Get Ecliptic Latitude
    LatQuantity get_lat() const
    {
        return static_cast<LatQuantity>
        (
            bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<0>(this->point))
        );
    }

    //Get Ecliptic Longitude
    LonQuantity get_lon() const
    {
        return static_cast<LonQuantity>
        (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                        (bg::get<1>(this->point))
        );
    }

    //Set value of Ecliptic Latitude and Ecliptic Longitude
    void set_lat_lon
    (
            LatQuantity const &Lat,
            LonQuantity const &Lon
    )
    {
        this->set_lat(Lat);
        this->set_lon(Lon);
    }

    //Set Ecliptic Latitude
    void set_lat(LatQuantity const &Lat)
    {
        bg::set<0>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Lat).value()
            );
    }

    //Set Ecliptic Longitude
    void set_lon(LonQuantity const &Lon)
    {
        bg::set<1>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Lon).value()
            );
    }

}; //ecliptic_coord

//Make Ecliptic Coordinate
template
<
    typename CoordinateType,
    template<typename Unit2, typename CoordinateType_> class LatQuantity,
    template<typename Unit1, typename CoordinateType_> class LonQuantity,
    typename Unit1,
    typename Unit2
>
ecliptic_coord
<
    CoordinateType,
    LatQuantity<Unit2, CoordinateType>,
    LonQuantity<Unit1, CoordinateType>
> make_ecliptic_coord
(
    LatQuantity<Unit2, CoordinateType> const &Lat,
    LonQuantity<Unit1, CoordinateType> const &Lon
)
{
    return ecliptic_coord
        <
            CoordinateType,
            LatQuantity<Unit2, CoordinateType>,
            LonQuantity<Unit1, CoordinateType>
        > (Lat, Lon);
}

//Print Ecliptic Coordinates
template
<
    typename CoordinateType,
    class LatQuantity,
    class LonQuantity
>
std::ostream &operator << (std::ostream &out, ecliptic_coord
        <CoordinateType, LatQuantity, LonQuantity> const &point) {
    out << "Ecliptic Coordinate (Ecliptic Latitude: "
        << point.get_lat() << ", Ecliptic Longitude: "
        << point.get_lon() << ")";

    return out;
}

}}}

#endif //BOOST_ASTRONOMY_ECLIPTIC_COORD_HPP


#include "RouteFinder.h"

#include <cassert>
#include <cmath>
#include <algorithm>
#include <queue>
#include <set>
#include <map>

#include <boost/math/constants/constants.hpp>

namespace carto { namespace sgre {
    Result RouteFinder::find(const Query& query) const {
        struct EndPoint {
            Point point = Point();
            Graph::TriangleId triangleId = Graph::TriangleId(-1);
            std::set<Graph::EdgeId> edgeIds;
        };

        auto calculateAvgLngScale = [](const Point& point1, const Point& point2) -> double {
            return std::max(std::cos((point1(1) + point2(1)) * 0.5 * boost::math::constants::pi<double>() / 180.0), 0.01);
        };

        // Find nearest edges to the endpoints. Note that there could be multiple nearest edges for both endpoints (two-way edges)
        std::vector<EndPoint> endPoints[2];
        for (int i = 0; i < 2; i++) {
            StaticGraph::SearchOptions searchOptions;
            searchOptions.zSensitivity = _routeOptions.zSensitivity;
            std::vector<std::pair<Graph::EdgeId, Point>> edgePoints = _graph->findNearestEdgePoint(query.getPos(i), query.getFilter(i), searchOptions);
            if (edgePoints.empty()) {
                return Result();
            }

            for (const std::pair<Graph::EdgeId, Point>& edgePoint : edgePoints) {
                const Graph::Edge& edge = _graph->getEdge(edgePoint.first);

                EndPoint endPoint;
                endPoint.point = edgePoint.second;
                endPoint.triangleId = edge.triangleId;
                endPoint.edgeIds = std::set<Graph::EdgeId> { edgePoint.first };

                bool found = false;
                for (std::size_t j = 0; j < endPoints[i].size(); j++) {
                    double lngScale = calculateAvgLngScale(endPoints[i][j].point, endPoint.point);
                    double dist = calculateDistance(endPoints[i][j].point, endPoint.point, lngScale);
                    if (dist < DIST_EPSILON && endPoints[i][j].triangleId == endPoint.triangleId) {
                        endPoints[i][j].edgeIds.insert(edgePoint.first);
                        found = true;
                        break;
                    }
                }
                if (!found) {
                    endPoints[i].push_back(endPoint);
                }
            }
        }

        // Link all endpoint combinations.
        auto graph = std::make_shared<DynamicGraph>(_graph);
        double lngScale = calculateAvgLngScale(endPoints[0][0].point, endPoints[1][0].point);

        std::vector<Graph::NodeId> initialNodeIds;
        std::vector<Graph::NodeId> finalNodeIds;
        for (std::size_t i0 = 0; i0 < endPoints[0].size(); i0++) {
            Graph::NodeId initialNodeId = createNode(*graph, endPoints[0][i0].point);
            linkNodeToEdges(*graph, endPoints[0][i0].edgeIds, initialNodeId, 0);
            initialNodeIds.push_back(initialNodeId);
        }
        for (std::size_t i1 = 0; i1 < endPoints[1].size(); i1++) {
            Graph::NodeId finalNodeId = createNode(*graph, endPoints[1][i1].point);
            linkNodeToEdges(*graph, endPoints[1][i1].edgeIds, finalNodeId, 1);
            finalNodeIds.push_back(finalNodeId);
        }
        for (std::size_t i0 = 0; i0 < endPoints[0].size(); i0++) {
            for (std::size_t i1 = 0; i1 < endPoints[1].size(); i1++) {
                linkNodesToCommonEdges(*graph, endPoints[0][i0].edgeIds, endPoints[1][i1].edgeIds, initialNodeIds[i0], finalNodeIds[i1]);
            }
        }

        // Create evaluated attributes table
        std::vector<EvaluatedAttributes> attributesTable(graph->getAttributesIdRangeEnd());
        for (Graph::AttributesId attribsId = 0; attribsId < graph->getAttributesIdRangeEnd(); attribsId++) {
            const Graph::Attributes& baseAttribs = graph->getAttributes(attribsId);

            EvaluatedAttributes attribs;
            attribs.speed = std::visit(FloatParameterEvaluator(_paramValues, DEFAULT_SPEED), baseAttribs.speed);
            attribs.zSpeed = std::visit(FloatParameterEvaluator(_paramValues, DEFAULT_ZSPEED), baseAttribs.zSpeed);
            attribs.turnSpeed = std::visit(FloatParameterEvaluator(_paramValues, DEFAULT_TURNSPEED), baseAttribs.turnSpeed);
            attribs.delay = std::visit(FloatParameterEvaluator(_paramValues, DEFAULT_DELAY), baseAttribs.delay);
            attributesTable[attribsId] = attribs;
        }

        // Try to find the fastest route
        std::optional<Route> route = findFastestRoute(*graph, attributesTable, initialNodeIds, finalNodeIds, lngScale, _routeOptions.tesselationDistance);
        if (!route) {
            return Result();
        }

        // Do optional path straightening. This is very important for polygon/hybrid graphs.
        if (_routeOptions.pathStraightening) {
            route = straightenRoute(*graph, *route, lngScale);
        }

        // Build final result (remove duplicate nodes, create instructions)
        return buildResult(*graph, attributesTable, *route, lngScale, _routeOptions.minTurnAngle, _routeOptions.minUpDownAngle);
    }

    std::unique_ptr<RouteFinder> RouteFinder::create(std::shared_ptr<const StaticGraph> graph, const picojson::value& configDef) {
        RouteOptions routeOptions;
        if (configDef.contains("pathstraightening")) {
            routeOptions.pathStraightening = configDef.get("pathstraightening").get<bool>();
        }
        if (configDef.contains("tesselationdistance")) {
            routeOptions.tesselationDistance = configDef.get("tesselationdistance").get<double>();
        }
        if (configDef.contains("zsensitivity")) {
            routeOptions.zSensitivity = configDef.get("zsensitivity").get<double>();
        }
        if (configDef.contains("min_turnangle")) {
            routeOptions.minTurnAngle = configDef.get("min_turnangle").get<double>();
        }
        if (configDef.contains("min_updownangle")) {
            routeOptions.minUpDownAngle = configDef.get("min_updownangle").get<double>();
        }

        auto routeFinder = std::make_unique<RouteFinder>(std::move(graph));
        routeFinder->setRouteOptions(routeOptions);
        return routeFinder;
    }

    Graph::NodeId RouteFinder::createNode(DynamicGraph& graph, const Point& point) {
        Graph::Node node;
        node.nodeFlags = Graph::NodeFlags(0);
        node.points = std::array<Point, 2> {{ point, point }};
        return graph.addNode(node);
    }

    void RouteFinder::linkNodeToEdges(DynamicGraph& graph, const std::set<Graph::EdgeId>& edgeIds, Graph::NodeId nodeId, int nodeIdx) {
        // Find all 'linked edge' ids. For triangles this means finding all edges of the triangle.
        std::set<Graph::EdgeId> linkedEdgeIds;
        for (Graph::EdgeId edgeId : edgeIds) {
            const Graph::Edge& edge = graph.getEdge(edgeId);
            linkedEdgeIds.insert(edgeId);
            if (edge.triangleId != Graph::TriangleId(-1)) {
                const Graph::Node& node = graph.getNode(edge.nodeIds[1 - nodeIdx]);
                for (Graph::EdgeId linkedEdgeId : node.edgeIds) {
                    const Graph::Edge& linkedEdge = graph.getEdge(linkedEdgeId);
                    if (linkedEdge.triangleId == edge.triangleId) {
                        linkedEdgeIds.insert(linkedEdgeId);

                        // For final node, we need to do another hop as we do not store incoming edge ids for nodes
                        if (nodeIdx == 1) {
                            const Graph::Node& nextNode = graph.getNode(linkedEdge.nodeIds[1]);
                            for (Graph::EdgeId nextLinkedEdgeId : nextNode.edgeIds) {
                                const Graph::Edge& nextLinkedEdge = graph.getEdge(nextLinkedEdgeId);
                                if (nextLinkedEdge.triangleId == edge.triangleId) {
                                    linkedEdgeIds.insert(nextLinkedEdgeId);
                                }
                            }
                        }
                    }
                }
            }
        }

        // Add new edges to the graph based on the found linked edges
        for (Graph::EdgeId linkedEdgeId : linkedEdgeIds) {
            Graph::Edge linkedEdge = graph.getEdge(linkedEdgeId);
            linkedEdge.edgeFlags = Graph::EdgeFlags(0);
            linkedEdge.nodeIds[nodeIdx] = nodeId;
            graph.addEdge(linkedEdge);
        }
    }

    void RouteFinder::linkNodesToCommonEdges(DynamicGraph& graph, const std::set<Graph::EdgeId>& edgeIds0, const std::set<Graph::EdgeId>& edgeIds1, Graph::NodeId nodeId0, Graph::NodeId nodeId1) {
        // Find intersection of edge ids
        std::set<Graph::EdgeId> commonEdgeIds;
        std::set_intersection(edgeIds0.begin(), edgeIds0.end(), edgeIds1.begin(), edgeIds1.end(), std::inserter(commonEdgeIds, commonEdgeIds.begin()));

        // Connect the shared edges
        for (Graph::EdgeId commonEdgeId : commonEdgeIds) {
            const Graph::Edge& commonEdge = graph.getEdge(commonEdgeId);
            if (commonEdge.triangleId == Graph::TriangleId(-1)) {
                // Check that the target point is further along the edge compared to the starting point
                const Point& pos0 = graph.getNode(commonEdge.nodeIds[0]).points[0];
                double relDist0 = cglib::length(graph.getNode(nodeId0).points[0] - pos0);
                double relDist1 = cglib::length(graph.getNode(nodeId1).points[0] - pos0);
                if (relDist0 > relDist1) {
                    continue;
                }
            }

            // Insert the edge linking starting and target nodes
            Graph::Edge linkedEdge = commonEdge;
            linkedEdge.edgeFlags = Graph::EdgeFlags(0);
            linkedEdge.nodeIds[0] = nodeId0;
            linkedEdge.nodeIds[1] = nodeId1;
            graph.addEdge(linkedEdge);
        }
    }

    bool RouteFinder::isNodeVisible(const Graph& graph, Graph::NodeId nodeId0, double t0, Graph::NodeId nodeId1, double t1, double lngScale, std::set<Graph::NodeId> visitedNodeIds) {
        if (nodeId0 == nodeId1) {
            return true;
        }
        if (visitedNodeIds.count(nodeId0) > 0) {
            return false;
        }
        visitedNodeIds.insert(nodeId0);

        const Graph::Node& node0 = graph.getNode(nodeId0);
        const Graph::Node& node1 = graph.getNode(nodeId1);
        Point pos0 = node0.points[0] + (node0.points[1] - node0.points[0]) * t0;
        Point pos1 = node1.points[0] + (node1.points[1] - node1.points[0]) * t1;
        cglib::vec3<double> posDelta = pos1 - pos0;

        for (Graph::EdgeId edgeId : node0.edgeIds) {
            Graph::NodeId targetNodeId = graph.getEdge(edgeId).nodeIds[1];
            const Graph::Node& targetNode = graph.getNode(targetNodeId);
            cglib::vec3<double> pointsDelta = targetNode.points[1] - targetNode.points[0];

            // Calculate distances along 2 axes for intersection point
            double s = 0;
            if (cglib::norm(posDelta) != 0) {
                cglib::vec3<double> normal = cglib::vector_product(cglib::vector_product(pointsDelta, posDelta), pointsDelta);
                double dot = cglib::dot_product(posDelta, normal);
                if (dot != 0) {
                    s = std::min(1.0, std::max(0.0, cglib::dot_product(targetNode.points[0] - pos0, normal) / dot));
                } else {
                    s = std::min(1.0, std::max(0.0, cglib::dot_product(targetNode.points[0] - pos0, posDelta) / cglib::norm(posDelta)));
                }
            }

            double t = 0;
            if (cglib::norm(pointsDelta) != 0) {
                t = std::min(1.0, std::max(0.0, cglib::dot_product(pos0 + posDelta * s - targetNode.points[0], pointsDelta) / cglib::norm(pointsDelta)));
            }

            // If the resulting points are close, try moving along the edge recursively
            double dist = calculateDistance(pos0 + posDelta * s, targetNode.points[0] + pointsDelta * t, lngScale);
            if (dist < DIST_EPSILON) {
                if (isNodeVisible(graph, targetNodeId, t, nodeId1, t1, lngScale, visitedNodeIds)) {
                    return true;
                }
            }
        }
        return false;
    }

    Result RouteFinder::buildResult(const Graph& graph, const EvaluatedAttributesTable& attributesTable, const Route& route, double lngScale, double minTurnAngle, double minUpDownAngle) {
        // Build both route geometry and instructions in one pass
        std::vector<Point> points;
        std::vector<Instruction> instructions;
        Graph::FeatureId lastFeatureId = Graph::FeatureId(-1);
        for (std::size_t i = 0; i < route.size(); i++) {
            const RouteNode& routeNode = route[i];

            // Add route point
            const Graph::Node& targetNode = graph.getNode(routeNode.targetNodeId);
            points.push_back(targetNode.points[0] + (targetNode.points[1] - targetNode.points[0]) * routeNode.targetNodeT);
            if (i == 0) {
                continue;
            }

            // Read node feature and attributes
            const Graph::FeatureProperties& properties = graph.getFeatureProperties(routeNode.featureId);
            const EvaluatedAttributes& attribs = attributesTable[routeNode.attributesId];

            // Add optional waiting instruction.
            if (attribs.delay > 0) {
                Instruction waitInstruction(Instruction::Type::WAIT, properties, 0, attribs.delay, points.size() - 2);
                instructions.push_back(std::move(waitInstruction));
            }

            // Calculate the turning angle/mode starting from the second point
            Instruction::Type type = Instruction::Type::HEAD_ON;
            double turnAngle = 0;
            if (points.size() > 2) {
                if (points.size() > 2) {
                    cglib::vec3<double> v0(0, 0, 0);
                    for (std::size_t j = points.size() - 2; j > 0; j--) {
                        cglib::vec3<double> delta = points[j] - points[j - 1];
                        v0 = cglib::vec3<double>(delta(0) * lngScale, delta(1), 0);
                        if (cglib::norm(v0) > 0) {
                            v0 = cglib::unit(v0);
                            break;
                        }
                    }
                    cglib::vec3<double> v1(0, 0, 0);
                    {
                        cglib::vec3<double> delta = points[points.size() - 1] - points[points.size() - 2];
                        v1 = cglib::vec3<double>(delta(0) * lngScale, delta(1), 0);
                        if (cglib::norm(v1) > 0) {
                            v1 = cglib::unit(v1);
                        }
                    }

                    // Calculate instruction type based on the turning angle
                    cglib::vec3<double> cross = cglib::vector_product(v0, v1);
                    double signedTurnAngle = std::asin(std::max(-1.0, std::min(1.0, cross(2)))) * 180.0 / boost::math::constants::pi<double>();
                    if (signedTurnAngle < -minTurnAngle) {
                        type = Instruction::Type::TURN_RIGHT;
                    } else if (signedTurnAngle > minTurnAngle) {
                        type = Instruction::Type::TURN_LEFT;
                    } else {
                        // Note: in theory we should check the graph node for 'alternatives'.
                        // If there are 2 options: slight turn to left and slight turn to right, we should still use turn actions.
                        type = Instruction::Type::GO_STRAIGHT;
                    }

                    if (cglib::norm(v0) > 0 && cglib::norm(v1) > 0) {
                        double dot = cglib::dot_product(v0, v1);
                        turnAngle = std::acos(std::max(-1.0, std::min(1.0, dot))) * 180.0 / boost::math::constants::pi<double>();
                    }
                }
            }

            // Check if we need to move vertically
            const Point& pos0 = points[points.size() - 2];
            const Point& pos1 = points[points.size() - 1];
            std::pair<double, double> dist2D = calculateDistance2D(pos0, pos1, lngScale);
            double minUpDownAngleTan = std::tan(minUpDownAngle / 180.0 * boost::math::constants::pi<double>());
            if (dist2D.second > 0 && dist2D.second / minUpDownAngleTan > dist2D.first) {
                if (pos1(2) > pos0(2)) {
                    type = Instruction::Type::GO_UP;
                } else {
                    type = Instruction::Type::GO_DOWN;
                }
                turnAngle = 0;
            }

            // Calculate distance and time. We will add turning time here and do not apply delay (as its included in the previous 'wait' instruction)
            double dist = calculateDistance(pos0, pos1, lngScale);
            double time = calculateTime(attribs, false, turnAngle, pos0, pos1, lngScale);
            if (!std::isfinite(time)) {
                return Result();
            }

            // Store the instruction. But first check if we can merge this instruction with the last one based on type
            std::size_t geometryIndex = points.size() - 2;
            if (instructions.size() > 0 && routeNode.featureId == lastFeatureId && type == Instruction::Type::GO_STRAIGHT) {
                type = instructions.back().getType();
                dist += instructions.back().getDistance();
                time += instructions.back().getTime();
                geometryIndex = instructions.back().getGeometryIndex();
                instructions.pop_back();

                // Remove redundant points from straight lines
                if (points.size() > 2) {
                    double dist0 = calculateDistance(points[points.size() - 2], points[points.size() - 3], lngScale);
                    double dist1 = calculateDistance(points[points.size() - 1], points[points.size() - 2], lngScale);
                    double dist2 = calculateDistance(points[points.size() - 1], points[points.size() - 3], lngScale);
                    if (dist0 + dist1 <= dist2 + DIST_EPSILON) {
                        points.erase(points.begin() + points.size() - 2);
                    }
                }
            }
            Instruction instruction(type, properties, dist, time, geometryIndex);
            instructions.push_back(std::move(instruction));

            // Add the final instruction if we are at the end
            if (i + 1 == route.size()) {
                Instruction finalInstruction(Instruction::Type::REACHED_YOUR_DESTINATION, properties, 0, 0, points.size() - 1);
                instructions.push_back(std::move(finalInstruction));
            }

            lastFeatureId = routeNode.featureId;
        }

        return Result(std::move(instructions), std::move(points));
    }

    RouteFinder::Route RouteFinder::straightenRoute(const Graph& graph, const Route& route, double lngScale) {
        Route optimizedRoute;
        if (!route.empty()) {
            optimizedRoute.push_back(route.front());
        }
        for (std::size_t i = 1; i < route.size(); ) {
            const RouteNode& routeNode0 = optimizedRoute.back();

            // Find first node that is not 'visible' from current node
            std::size_t j = i + 1;
            for (; j < route.size(); j++) {
                // Stop if feature id has changed
                if (route[j - 1].featureId != route[j].featureId) {
                    break;
                }

                const RouteNode& routeNode1 = route[j];

                std::set<Graph::NodeId> visitedNodeIds;
                if (!isNodeVisible(graph, routeNode0.targetNodeId, routeNode0.targetNodeT, routeNode1.targetNodeId, routeNode1.targetNodeT, lngScale, visitedNodeIds)) {
                    break;
                }
            }

            // Store previous node of the found node
            optimizedRoute.push_back(route[j - 1]);

            i = j;
        }
        return optimizedRoute;
    }

    std::optional<RouteFinder::Route> RouteFinder::findFastestRoute(const Graph& graph, const EvaluatedAttributesTable& attributesTable, const std::vector<Graph::NodeId>& initialNodeIds, const std::vector<Graph::NodeId>& finalNodeIds, double lngScale, double tesselationDistance) {
        struct NodeRecord {
            double time; // best estimated time to final some from some initial node. Estimated time must not exceed actual time.
            Graph::NodeId nodeId;
            std::size_t routeEdgeIndex; // outgoing route edge index

            bool operator < (const NodeRecord& rec) const { return rec.time < time; }
        };

        struct RouteEdge {
            double time; // time spent up to this point
            Graph::EdgeId edgeId;
            std::size_t routeEdgeIndex; // incoming route edge index
            double nodeT; // outgoing edge node T value
        };

        // Find fastest attributes for A*
        EvaluatedAttributes fastestAttributes;
        fastestAttributes.speed = 0;
        fastestAttributes.zSpeed = 0;
        fastestAttributes.turnSpeed = 0;
        fastestAttributes.delay = 0;
        for (const EvaluatedAttributes& attribs : attributesTable) {
            fastestAttributes.speed = std::max(fastestAttributes.speed, attribs.speed);
            fastestAttributes.zSpeed = std::max(fastestAttributes.zSpeed, attribs.zSpeed);
            fastestAttributes.turnSpeed = std::max(fastestAttributes.turnSpeed, attribs.turnSpeed);
        }

        // Initialize route map and node queue
        std::unordered_map<Graph::NodeId, std::vector<RouteEdge>> bestRouteMap;
        std::priority_queue<NodeRecord> nodeQueue;
        for (Graph::NodeId initialNodeId : initialNodeIds) {
            const Graph::Node& initialNode = graph.getNode(initialNodeId);

            // Store initial node of the path
            bestRouteMap[initialNodeId] = std::vector<RouteEdge> {{ 0.0, Graph::EdgeId(-1), 0, 0.0 }};

            // Calculate best possible time from the node to the closest final node
            double bestTotalEstTime = std::numeric_limits<double>::infinity();
            for (Graph::NodeId finalNodeId : finalNodeIds) {
                const Graph::Node& finalNode = graph.getNode(finalNodeId);
                double totalEstTime = calculateTime(fastestAttributes, false, 0.0, initialNode.points[0], finalNode.points[0], lngScale);
                bestTotalEstTime = std::min(bestTotalEstTime, totalEstTime);
            }
            nodeQueue.push({ bestTotalEstTime, initialNodeId, 0 });
        }

        // Process the node queue
        Graph::NodeId finalNodeId = Graph::NodeId(-1);
        while (!nodeQueue.empty()) {
            NodeRecord rec = nodeQueue.top();
            nodeQueue.pop();

            // Check if we have reached a final node
            auto nodeIt = std::find(finalNodeIds.begin(), finalNodeIds.end(), rec.nodeId);
            if (nodeIt != finalNodeIds.end()) {
                finalNodeId = *nodeIt;
                break;
            }

            Graph::NodeId nodeId = rec.nodeId;
            const Graph::Node& node = graph.getNode(nodeId);
            const RouteEdge& routeEdge = bestRouteMap[nodeId][rec.routeEdgeIndex];
            Point nodePos = node.points[0] + (node.points[1] - node.points[0]) * routeEdge.nodeT;
            double time = routeEdge.time;

            // Process each edge from the current node
            for (Graph::EdgeId edgeId : node.edgeIds) {
                const Graph::Edge& edge = graph.getEdge(edgeId);
                assert(edge.nodeIds[0] == nodeId);
                Graph::NodeId targetNodeId = edge.nodeIds[1];
                const Graph::Node& targetNode = graph.getNode(targetNodeId);

                // Read/initialize edges to target node
                std::vector<RouteEdge>& targetRouteEdges = bestRouteMap[targetNodeId];
                if (targetRouteEdges.empty()) {
                    std::size_t tesselationLevel = static_cast<std::size_t>(std::ceil(calculateDistance(targetNode.points[0], targetNode.points[1], lngScale) / tesselationDistance));
                    targetRouteEdges.resize(tesselationLevel + 1, { std::numeric_limits<double>::infinity(), Graph::EdgeId(-1), 0, 0.0 });
                }

                // Tesselate all triangle edges based on tesselation distance
                for (std::size_t i = 0; i < targetRouteEdges.size(); i++) {
                    // Fast preliminary check - if we already have result that we can not improve upon, skip further calculations
                    if (targetRouteEdges[i].time <= time) {
                        continue;
                    }

                    // Check if we found a better route to target node compared to existing route
                    double targetNodeT = static_cast<double>(i) / std::max(targetRouteEdges.size() - 1, std::size_t(1));
                    Point targetNodePos = targetNode.points[0] + (targetNode.points[1] - targetNode.points[0]) * targetNodeT;
                    double targetTime = time + calculateTime(attributesTable[edge.attributesId], true, 0.0, nodePos, targetNodePos, lngScale);
                    if (targetRouteEdges[i].time <= targetTime || !std::isfinite(targetTime)) {
                        continue;
                    }

                    // We have a better route edge. Store it.
                    targetRouteEdges[i] = { targetTime, edgeId, rec.routeEdgeIndex, targetNodeT };

                    // Calculate the fastest possible estimation from target node to the closest final node and push it to queue
                    double bestTotalEstTime = std::numeric_limits<double>::infinity();
                    for (Graph::NodeId finalNodeId : finalNodeIds) {
                        const Graph::Node& finalNode = graph.getNode(finalNodeId);
                        double totalEstTime = targetTime + calculateTime(fastestAttributes, false, 0.0, targetNodePos, finalNode.points[0], lngScale);
                        bestTotalEstTime = std::min(bestTotalEstTime, totalEstTime);
                    }
                    nodeQueue.push({ bestTotalEstTime, targetNodeId, i });
                }
            }
        }

        // Success?
        if (finalNodeId == Graph::NodeId(-1)) {
            return std::optional<Route>();
        }

        // Reconstruct the fastest route backwards.
        Route bestRoute;
        Graph::NodeId lastNodeId = finalNodeId;
        std::size_t lastRouteEdgeIndex = 0;
        while (true) {
            const RouteEdge& routeEdge = bestRouteMap[lastNodeId].at(lastRouteEdgeIndex);
            if (routeEdge.edgeId == Graph::EdgeId(-1)) {
                break;
            }

            const Graph::Edge& edge = graph.getEdge(routeEdge.edgeId);
            RouteNode routeNode;
            routeNode.featureId = edge.featureId;
            routeNode.attributesId = edge.attributesId;
            routeNode.targetNodeId = edge.nodeIds[1];
            routeNode.targetNodeT = routeEdge.nodeT;
            bestRoute.emplace_back(routeNode);

            lastNodeId = edge.nodeIds[0];
            lastRouteEdgeIndex = routeEdge.routeEdgeIndex;
        }
        RouteNode initialRouteNode;
        initialRouteNode.targetNodeId = lastNodeId;
        bestRoute.emplace_back(initialRouteNode);

        // Done, reverse the generated route
        std::reverse(bestRoute.begin(), bestRoute.end());
        return bestRoute;
    }

    double RouteFinder::calculateTime(const EvaluatedAttributes& attribs, bool applyDelay, double turnAngle, const Point& pos0, const Point& pos1, double lngScale) {
        std::pair<double, double> dist2D = calculateDistance2D(pos0, pos1, lngScale);

        double time = 0;
        if (applyDelay) {
            time += attribs.delay;
        }
        time += (turnAngle != 0 ? std::abs(turnAngle) / attribs.turnSpeed : 0);
        time += (dist2D.first > 0 ? dist2D.first / attribs.speed : 0);
        time += (dist2D.second > 0 ? dist2D.second / attribs.zSpeed : 0);
        return time;
    }

    double RouteFinder::calculateDistance(const Point& pos0, const Point& pos1, double lngScale) {
        std::pair<double, double> dist2D = calculateDistance2D(pos0, pos1, lngScale);
        return std::sqrt(dist2D.first * dist2D.first + dist2D.second * dist2D.second);
    }

    std::pair<double, double> RouteFinder::calculateDistance2D(const Point& pos0, const Point& pos1, double lngScale) {
        static constexpr double EARTH_RADIUS = 6378137.0;

        cglib::vec3<double> posDelta = pos1 - pos0;
        double distXY = cglib::length(cglib::vec2<double>(posDelta(0) * lngScale, posDelta(1)));
        double distZ = std::abs(posDelta(2));
        return std::make_pair(distXY * EARTH_RADIUS * boost::math::constants::pi<double>() / 180.0, distZ);
    }
} }


#include <Eigen\Dense>

int main()
{
	Eigen::MatrixXd d;
}


#ifndef _planeseg_Types_hpp_
#define _planeseg_Types_hpp_

//#define PCL_NO_PRECOMPILE
#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

namespace planeseg {

  /*
struct Point {
  PCL_ADD_POINT4D;
  float delta;
  int label;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(Point,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, delta, delta)
                                  (int, label, label)
)
  */
typedef pcl::PointXYZL Point;

typedef pcl::PointCloud<pcl::Normal> NormalCloud;
typedef pcl::PointCloud<Point> LabeledCloud;
typedef Eigen::Matrix<float,Eigen::Dynamic,3> MatrixX3f;

}

#endif


// Copyright (C) 2016 Gideon May (gideon@borges.xyz)
//
// Permission to copy, use, sell and distribute this software is granted
// provided this copyright notice appears in all copies.
// Permission to modify the code and to distribute modified code is granted
// provided this copyright notice appears in all copies, and a notice
// that the code was modified is included with the copyright notice.
//
// This software is provided "as is" without express or implied warranty,
// and with no claim as to its suitability for any purpose.

#include <boost/python/def.hpp>
#include <boost/lexical_cast.hpp>

#include <osg/Math>
#include <osg/Array>

#include "Math.hpp"

using namespace boost::python;

namespace {
double inRadians(double angle) { return osg::inRadians(angle); }
double clampTo(double v, double min, double max) { return osg::clampTo(v, min, max); }
double clampAbove(double v, double min) { return osg::clampAbove(v, min); }
double clampBelow(double v, double max) { return osg::clampBelow(v, max); }
double sign(double v) { return osg::sign(v); }
double square(double v) { return osg::square(v); }
double signedSquare(double v) { return osg::signedSquare(v); }
}

namespace PyOSG {
void init_math()
{
    def("inDegrees", (double (*)(double)) &osg::inDegrees);
    def("inRadians", (double (*)(double)) &inRadians);
    def("clampTo", &clampTo);
    def("clampAbove", &clampAbove);
    def("clampBelow", &clampBelow);
    def("sign", &sign);
    def("square", &square);
    def("signedSquare", &signedSquare);
    def("DegreesToRadians", (double (*)(double)) &osg::DegreesToRadians);
    def("RadiansToDegrees", (double (*)(double)) &osg::RadiansToDegrees);
    def("isNaN", (bool (*)(float)) &osg::isNaN);

#   define SETATTR_OSG_CONST(CONST) scope().attr(#CONST) = object(osg::CONST);
    SETATTR_OSG_CONST(PI);
    SETATTR_OSG_CONST(PI_2);
    SETATTR_OSG_CONST(PI_4);
}
} // namespace PyOSG


//=======================================================================
// Copyright (c) 2015
//
// 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)
//=======================================================================
/**
 * @file sketch_accuracy_check.hpp
 * @brief
 * @author Tomasz Strozak
 * @version 1.0
 * @date 2015-04-21
 */
#ifndef PAAL_SKETCH_ACCURACY_CHECK_HPP
#define PAAL_SKETCH_ACCURACY_CHECK_HPP

#include <boost/algorithm/cxx11/all_of.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/bindings/lapack/syevd.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/test/unit_test.hpp>

template <typename MatrixOriginal,
          typename MatrixSketch>
void check_frequent_directions(MatrixOriginal &&original_matrix,
        MatrixSketch &&sketch, typename MatrixOriginal::value_type epsilon,
        typename MatrixOriginal::value_type ratio) {
    using value_type = typename MatrixOriginal::value_type;

    MatrixOriginal a = boost::numeric::ublas::prod(boost::numeric::ublas::trans(original_matrix), original_matrix);
    MatrixSketch b = boost::numeric::ublas::prod(boost::numeric::ublas::trans(sketch), sketch);
    MatrixSketch diff_matrix = a - b;

    auto n = diff_matrix.size1();
    boost::numeric::ublas::vector<value_type> eigenvalues(n);
    boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major> m{std::move(diff_matrix)};

    //diff_matrix is a symmetric matrix so we can check positive semi-definite
    //and compute second norm using its eigenvalues
    auto info = boost::numeric::bindings::lapack::syevd('N', 'L', m,
        eigenvalues, boost::numeric::bindings::lapack::optimal_workspace());
    BOOST_CHECK(!info);

    //check if diff matrix is positive semi-definite
    BOOST_CHECK(boost::algorithm::all_of(eigenvalues,
            [&](value_type x){ return x > -epsilon; }));

    auto diff_matrix_norm_2 = *boost::max_element(eigenvalues);
    auto original_matrix_norm_frobenius = boost::numeric::ublas::norm_frobenius(original_matrix);

    //check norm inequality
    BOOST_CHECK_LE(diff_matrix_norm_2, original_matrix_norm_frobenius
        * original_matrix_norm_frobenius / ratio);
}

template <typename MatrixOriginal,
          typename MatrixSketch>
void check_frequent_directions(MatrixOriginal &&original_matrix,
        MatrixSketch &&sketch, typename MatrixOriginal::value_type epsilon) {
    check_frequent_directions(std::forward<MatrixOriginal>(original_matrix),
            std::forward<MatrixSketch>(sketch), epsilon,
            (typename MatrixOriginal::value_type)sketch.size1() / 2);
}

#endif // PAAL_SKETCH_ACCURACY_CHECK_HPP


#include <boost/hana.hpp>
#include <functional>

// #define USE_YCOMBINATOR

int main()
{
#ifndef USE_YCOMBINATOR
    std::function<int(int)> f = [&](int x)
    {
        if(x == 0)
            return 1;
        return x * f(x - 1);
    };
#else
    auto f = boost::hana::fix([](auto self, int x) -> int
        {
            if(x == 0)
                return 1;
            return x * self(x - 1);
        });
#endif

    volatile auto res = f(6);
}


//  (C) Copyright Gennadiy Rozental 2011-2012.
//  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)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : defines range generator
// ***************************************************************************

#ifndef BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_XRANGE_HPP_112011GER
#define BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_XRANGE_HPP_112011GER

// Boost.Test
#include <boost/test/data/config.hpp>

#include <boost/test/data/monomorphic/generators/keywords.hpp>
#include <boost/test/data/monomorphic/generate.hpp>

// Boost
#include <boost/optional.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_unsigned.hpp>

// STL
#include <limits>
#include <cmath>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace unit_test {
namespace data {
namespace monomorphic {

// ************************************************************************** //
// **************             monomorphic::xrange_t            ************** //
// ************************************************************************** //

template<typename SampleType, typename StepType=SampleType>
class xrange_t {
public:
    typedef SampleType data_type;

    xrange_t( SampleType const& begin, StepType const& step, data::size_t size )
    : m_curr( begin )
    , m_step( step )
    , m_size( size )
    {}

    // Generator interface
    data::size_t    capacity() const { return m_size; }
    SampleType      next()
    {
        BOOST_TEST_DS_ASSERT( m_size != 0, "No more elements in range" );

        SampleType res = m_curr;

        m_curr += m_step;
        --m_size;

        return res;
    }
private:
    // Data members
    SampleType      m_curr;
    StepType        m_step;
    data::size_t    m_size;
};

//____________________________________________________________________________//

namespace ds_detail {

template<typename SampleType, typename StepType=SampleType>
struct make_xrange {
    static StepType    abs( StepType s, mpl::true_* )   { return s; }
    static StepType    abs( StepType s, mpl::false_* )  { return std::abs(s); }

    typedef xrange_t<SampleType, StepType> range_gen;

    template<typename Params>
    static generated_by<range_gen>
    _( Params const& params )
    {
        SampleType           begin_val  = params.has( data::begin )  ? params[data::begin] : SampleType();
        optional<SampleType> end_val    = params.has( data::end )    ? params[data::end]   : optional<SampleType>();
        StepType             step_val   = params.has( data::step )   ? params[data::step]  : 1;

        BOOST_TEST_DS_ASSERT( step_val != 0, "Range step can't be zero" );

        data::size_t size;
        if( !end_val.is_initialized() )
            size = BOOST_TEST_DS_INFINITE_SIZE;
        else {
            BOOST_TEST_DS_ASSERT( (step_val < 0) ^ (begin_val < *end_val), "Invalid step direction" );

            SampleType  abs_distance    = step_val < 0 ? begin_val - *end_val : *end_val-begin_val;
            StepType    abs_step        = make_xrange::abs(step_val, (typename boost::is_unsigned<StepType>::type*)0 );
            std::size_t s = static_cast<std::size_t>(abs_distance/abs_step);

            if( static_cast<SampleType>(s*abs_step) < abs_distance )
                s++;

            size = s;
        }

        return generated_by<range_gen>( range_gen( begin_val, step_val, size ) );
    }
};

} // namespace ds_detail
} // namespace monomorphic

//____________________________________________________________________________//

template<typename SampleType, typename Params>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( Params const& params )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::_( params );
}

//____________________________________________________________________________//

template<typename SampleType>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( SampleType const& end_val )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::_( data::end=end_val );
}

//____________________________________________________________________________//

template<typename SampleType, typename Params>
inline typename enable_if_c<nfp::is_named_params<Params>::value,monomorphic::generated_by<monomorphic::xrange_t<SampleType> > >::type
xrange( SampleType const& end_val, Params const& params )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::
            _(( params, data::end=end_val ));
}

//____________________________________________________________________________//

template<typename SampleType>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( SampleType const& begin_val, SampleType const& end_val )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::
            _(( data::begin=begin_val, data::end=end_val ));
}

//____________________________________________________________________________//

template<typename SampleType,typename StepType>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( SampleType const& begin_val, SampleType const& end_val, StepType const& step_val )
{
    return monomorphic::ds_detail::make_xrange<SampleType,StepType>::
            _(( data::begin=begin_val, data::end=end_val, data::step=step_val ));
}

//____________________________________________________________________________//

} // namespace data
} // namespace unit_test
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_XRANGE_HPP_112011GER


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_FDLIBM_FUNCTION_SCALAR_MODF_HPP_INCLUDED
#define NT2_TOOLBOX_FDLIBM_FUNCTION_SCALAR_MODF_HPP_INCLUDED
#include <boost/fusion/include/at.hpp>
#include <boost/fusion/include/vector.hpp>
#include <nt2/sdk/meta/strip.hpp>

  extern "C"{
    extern double fd_modf ( double,double* );
  }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is double_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(fdlibm::tag::modf_, tag::cpu_,
                      (A0),
                      (double_<A0>)
                     )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<fdlibm::tag::modf_(tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::strip<A0>::type            etype;
      typedef boost::fusion::vector<etype, etype>        type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typename NT2_RETURN_TYPE(1)::type res;
      boost::fusion::at_c<0>(res) = fd_modf(a0, &boost::fusion::at_c<1>(res));
      return res;
    }

  };
} }

#endif
// modified by jt the 29/12/2010


/*
 * Copyright (c) 2018, Matheus Izvekov <mizvekov@gmail.com>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
 * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
 * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
 * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE
 * OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
 * PERFORMANCE OF THIS SOFTWARE.
 */

#pragma once

#include <boost/multiprecision/cpp_bin_float.hpp>

namespace boost { namespace multiprecision {
	template<unsigned Digits, backends::digit_base_type base, class Allocator, class Exp, Exp Min, Exp Max, class T, T V>
	static auto operator<<(const number<cpp_bin_float<Digits,base,Allocator,Exp,Min,Max>> &a,
	                       const ::std::integral_constant<T,V> &) {
		return ldexp(a, +V);
	}

	template<unsigned Digits, backends::digit_base_type base, class Allocator, class Exp, Exp Min, Exp Max, class T, T V>
	static auto operator>>(const number<cpp_bin_float<Digits,base,Allocator,Exp,Min,Max>> &a,
	                       const ::std::integral_constant<T,V> &) {
		return ldexp(a, -V);
	}

	template<unsigned ADigits, backends::digit_base_type Abase, class AAllocator, class AExp, AExp AMin, AExp AMax,
	         unsigned BDigits, backends::digit_base_type Bbase, class BAllocator, class BExp, BExp BMin, BExp BMax>
	static auto operator%(const number<cpp_bin_float<ADigits,Abase,AAllocator,AExp,AMin,AMax>> &a,
	                      const number<cpp_bin_float<BDigits,Bbase,BAllocator,BExp,BMin,BMax>> &b) {
		return fmod(a, b);
	}
}; };


#ifndef TOOL_BOX_EIGEN_INTEROP_HPP
#define TOOL_BOX_EIGEN_INTEROP_HPP

#include <toolbox_maths/matrix3x3.hpp>
#include <toolbox_maths/quat.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

/**
    Convertions between the Eigen lib and our types.
*/

// =============================================================================
namespace tbx {
// =============================================================================

template<class Real> static inline
Eigen::Vector3d to_eigen_Vector3d(const tbx::Vector3<Real>& v){
    return Eigen::Vector3d(double(v.x), double(v.y), double(v.z));
}

// -----------------------------------------------------------------------------

template<class Real>
static inline
Eigen::Matrix3d to_eigen_Matrix3d(const tbx::Matrix3x3<Real>& m){
    Eigen::Matrix3d mat;
    mat << double(m.a), double(m.b), double(m.c),
           double(m.d), double(m.e), double(m.f),
           double(m.g), double(m.h), double(m.i);
    return mat;
}

// -----------------------------------------------------------------------------

static inline
tbx::Vec3 to_vec3(Eigen::Vector3f v){ return tbx::Vec3( v(0), v(1), v(2)); }

// -----------------------------------------------------------------------------

static inline
tbx::Vec3 to_vec3(const Eigen::Vector3d& v){
    return tbx::Vec3(float(v(0)), float(v(1)), float(v(2)));
}

// -----------------------------------------------------------------------------

static inline
tbx::Mat3 to_mat3(const Eigen::Matrix3d& m){
    tbx::Mat3 mat(float(m(0,0)), float(m(0,1)), float(m(0,2)),
                  float(m(1,0)), float(m(1,1)), float(m(1,2)),
                  float(m(2,0)), float(m(2,1)), float(m(2,2)));
    return mat;
}

// -----------------------------------------------------------------------------

static inline
tbx::Mat3 to_mat3(const Eigen::Matrix3f& m){
    tbx::Mat3 mat((m(0,0)), (m(0,1)), (m(0,2)),
                  (m(1,0)), (m(1,1)), (m(1,2)),
                  (m(2,0)), (m(2,1)), (m(2,2)));
    return mat;
}

// -----------------------------------------------------------------------------

static inline
std::string to_string(Eigen::ComputationInfo info){
    switch(info){
    case Eigen::Success: return ("Success, Computation was successful.");
    case Eigen::NumericalIssue: return ("NumericalIssue, The provided data did not satisfy the prerequisites.");
    case Eigen::NoConvergence: return ("InvalidInput, Iterative procedure did not converge.");
    case Eigen::InvalidInput: return ("InvalidInput, The inputs are invalid, or the algorithm has been improperly called. When assertions are enabled, such errors trigger an assert.");
    }
    return ("UnknownError(")+std::to_string(int(info))+("), maybe this routine is not up to date check Eigen documentation");
}

}// END tbx ====================================================================

#include "toolbox_maths/settings_end.hpp"

#endif // TOOL_BOX_EIGEN_INTEROP_HPP


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_j0.hpp>
#include <eve/function/diff/cyl_bessel_j0.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_j0"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_j0(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_j0(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_j0 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 4.0),
                              eve::test::randoms(4.0, 6.0),
                              eve::test::randoms(5.0, 9.0),
                              eve::test::randoms(8.0, 2000.0)
                             )
         )
   <typename T>(T const& a0, T const& a1, T const& a2, T const& a3)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_j0 =  [](auto x) { return eve::cyl_bessel_j0(x); };
  auto std__cyl_bessel_j0 =  [](auto x)->v_t { return boost::math::cyl_bessel_j(v_t(0), x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::minf(eve::as< T>())), eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::inf(eve::as< T>())),  T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(1500)), std__cyl_bessel_j0(v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(500)), std__cyl_bessel_j0(v_t(500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(10)), std__cyl_bessel_j0(v_t(10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(5)),  std__cyl_bessel_j0(v_t(5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(2)),  std__cyl_bessel_j0(v_t(2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(1.5)),std__cyl_bessel_j0(v_t(1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(0.5)),std__cyl_bessel_j0(v_t(0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(1)),  std__cyl_bessel_j0(v_t(1))   , 2.5);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(0)),  eve::one(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(1500)), T(std__cyl_bessel_j0(v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(500)),  T(std__cyl_bessel_j0(v_t(500)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(10)) ,  T(std__cyl_bessel_j0( v_t(10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(5))  ,  T(std__cyl_bessel_j0( v_t(5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(2))  ,  T(std__cyl_bessel_j0( v_t(2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(1.5)),  T(std__cyl_bessel_j0( v_t(1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(0.5)),  T(std__cyl_bessel_j0( v_t(0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(1))  ,  T(std__cyl_bessel_j0( v_t(1))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(0))  , eve::one(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j0(a0), map(std__cyl_bessel_j0, a0), 500.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(a1), map(std__cyl_bessel_j0, a1), 500.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(a2), map(std__cyl_bessel_j0, a2), 500.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(a3), map(std__cyl_bessel_j0, a3), 500.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-1500)), std__cyl_bessel_j0(v_t(-1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-500)), std__cyl_bessel_j0(v_t(-500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-10)), std__cyl_bessel_j0(v_t(-10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-5)),  std__cyl_bessel_j0(v_t(-5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-2)),  std__cyl_bessel_j0(v_t(-2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-1.5)),std__cyl_bessel_j0(v_t(-1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-0.5)),std__cyl_bessel_j0(v_t(-0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(-1)),  std__cyl_bessel_j0(v_t(-1))   , 2.5);

  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-1500)), T(std__cyl_bessel_j0(v_t(-1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-500)),  T(std__cyl_bessel_j0(v_t(-500)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-10)) ,  T(std__cyl_bessel_j0( v_t(-10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-5))  ,  T(std__cyl_bessel_j0( v_t(-5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-2))  ,  T(std__cyl_bessel_j0( v_t(-2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-1.5)),  T(std__cyl_bessel_j0( v_t(-1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-0.5)),  T(std__cyl_bessel_j0( v_t(-0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(-1))  ,  T(std__cyl_bessel_j0( v_t(-1))  ), 2.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j0(-a0), map(std__cyl_bessel_j0, -a0), 500.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(-a1), map(std__cyl_bessel_j0, -a1), 500.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(-a2), map(std__cyl_bessel_j0, -a2), 500.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(-a3), map(std__cyl_bessel_j0, -a3), 500.0);
};

EVE_TEST( "Check behavior of diff(cyl_bessel_j0) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-60.0, 60.0))
        )
  <typename T>(T a0 )
{
  using v_t =  eve::element_type_t<T>;
  auto eve__diff_bessel_j0 =  [](auto x) { return eve::diff(eve::cyl_bessel_j0)(x); };
  auto std__diff_bessel_j0 =  [](auto x)->v_t { return boost::math::cyl_bessel_j_prime(v_t(0), x); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_j0(a0),   map(std__diff_bessel_j0, a0), 1.0e-3);
};


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with DSGD. We first creates factors and then a data matrix
 * from these factors. This process ensures that we know the best factorization of the input.
 * These matrices are distributed across a cluster. We then try to reconstruct the factors
 * using DSGD.
 *
 * Run with: mpirun --hosts localhost,localhost dsgd
 * (make sure to use a production build, otherwise it will be slow)
 */
#include <iostream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>

#include <mpi2/mpi2.h>
#include <mf/mf.h>

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace rg;
using namespace boost::numeric::ublas;

// type of SGD
typedef UpdateTruncate<UpdateSl> Update;
typedef RegularizeTruncate<RegularizeSl> Regularize;
typedef SlLoss Loss;

int main(int argc, char* argv[]) {
	// initialize mf library and mpi2
	boost::mpi::communicator& world = mfInit(argc, argv);

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 5000000;
	mf_size_type r = 10;

	// parameters for distribution
	int tasksPerRank = 2;
	mf_size_type blocks1 = world.size() * tasksPerRank;
	mf_size_type blocks2 = world.size() * tasksPerRank;

	// parameters for SGD
	double epsMax = 0.001;
	mf_size_type epochs = 20;
	SgdOrder order = SGD_ORDER_WOR;
	StratumOrder stratumOrder = STRATUM_ORDER_WOR;
	Update update = Update((UpdateSl()), 0, 100);
	Regularize regularize((RegularizeSl()), 0, 100);
	Loss loss;

	// start mf library
	// TODO: need automatic registration
	mfStart();

	if (world.rank() == 0)
	{
#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

		// TODO: distribute matrix generation
		// generate original factors by sampling from a uniform(0,1) distribution
		Random32 random; // note: this takes a default seed (not randomized!)
		DenseMatrix wIn(size1, r);
		DenseMatrixCM hIn(r, size2);
		generateRandom(wIn, random, boost::uniform_real<>(0,1));
		generateRandom(hIn, random, boost::uniform_real<>(0,1));

		// generate a sparse matrix by selecting random entries from the generated factors
		// and sample from a Poisson with mean equal to the entry
		// TODO: this generation process does not match the factorization model since we sample
		//       from the Poisson only at some entries of wh
		SparseMatrix v;
		generateRandom(v, nnz, wIn, hIn, random);
		LOG4CXX_INFO(logger, "Data matrix: "
			<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
		LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));

		// take a small sample and remove empty rows/columns
		ProjectedSparseMatrix Vsample;
		projectRandomSubmatrix(random, v, Vsample, v.size1()/5, v.size2()/5);
		projectFrequent(Vsample, 0);
		LOG4CXX_INFO(logger, "Sample matrix: "
			<< Vsample.data.size1() << " x " << Vsample.data.size2()
			<< ", " << Vsample.data.nnz() << " nonzeros");

		// generate initial factors by sampling from a uniform[0,1] distribution
		DenseMatrix w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(w, random, boost::uniform_real<>(0, 1));
		generateRandom(h, random, boost::uniform_real<>(0, 1));

		// distribute the input matrices
		DistributedSparseMatrix dv = distributeMatrix("V", blocks1, blocks2, true, v);
		LOG4CXX_INFO(logger, "Distributed data matrix: "
					<< dv.blocks1() << " x " << dv.blocks2() << " blocks");
		DistributedDenseMatrix dw = distributeMatrix("W", blocks1, 1, true, w);
		DistributedDenseMatrixCM dh = distributeMatrix("H", 1, blocks2, false, h);
		LOG4CXX_INFO(logger, "Distributed factor matrices");

		// initialize the DSGD
		Timer t;
		DsgdRunner dsgdRunner(random);
		DsgdJob<Update,Regularize> dsgdJob(dv, dw, dh, update, regularize, order,
				stratumOrder, false, tasksPerRank);
		DistributedDecayAuto<Update,Regularize,Loss> decay(dsgdJob, loss, Vsample, "decay", epsMax,
				world.size()*tasksPerRank);
		Trace trace;

		// run DSGD to try to reconstruct the original factors
		t.start();
		dsgdRunner.run(dsgdJob, loss, epochs, decay, trace);
		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// write trace to an R file
		LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/dsgd-gkl-trace.R");
		trace.toRfile("/tmp/dsgd-gkl-trace.R", "dsgd.gkl");
	}

	mfStop();
	mfFinalize();

	return 0;
}


/*
 Copyright (c) 2010, The Barbarian Group
 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.

 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 "cinder/PolyLine.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/multi.hpp>

namespace cinder {

template<typename T>
bool PolyLineT<T>::isClockwise( bool *isColinear ) const
{
	if( mPoints.size() < 3 ) {
		if( isColinear != nullptr ) *isColinear = true;
		return false;
	}

	size_t last = mPoints.size() - 1;
	// If the first and last point are the same (on a closed polygon), ignore one.
	if( mPoints.front() == mPoints.back() ) --last;

	// Find an extreme point since we know it will be on the hull...
	size_t smallest = 0;
	for( size_t i = 1; i <= last; ++i ) {
		if( mPoints[i].x < mPoints[smallest].x ) {
			smallest = i;
		}
		else if( mPoints[i].x == mPoints[smallest].x && mPoints[i].y < mPoints[smallest].y ) {
			smallest = i;
		}
	};
	// ...then get the next and previous point
	size_t prev = ( smallest == 0 )    ? last : ( smallest - 1 );
	size_t next = ( smallest == last ) ? 0    : ( smallest + 1 );
	T a = mPoints[next], b = mPoints[smallest], c = mPoints[prev];

	// The sign of the determinate indicates the orientation:
	//   positive is clockwise
	//   zero is colinear
	//   negative is counterclockwise
	double determinate = ( b.x - a.x ) * ( c.y - a.y ) - ( c.x - a.x ) * ( b.y - a.y );
	if( isColinear != nullptr ) *isColinear = determinate == 0.0;
	return determinate > 0.0;
}

template<typename T>
bool PolyLineT<T>::isCounterclockwise( bool *isColinear ) const
{
	bool colinear;
	bool clockwise = this->isClockwise( &colinear );
	if( isColinear != nullptr ) *isColinear = colinear;
	return colinear ? false : ! clockwise;
}

template<typename T>
T PolyLineT<T>::getPosition( float t ) const
{
	typedef typename T::value_type R;
	if( mPoints.size() <= 1 ) return T();
	if( t >= 1 ) return mPoints.back();
	if( t <= 0 ) return mPoints[0];

	size_t numSpans = mPoints.size() - 1;
	size_t span = (size_t)math<R>::floor( t * numSpans );
	R lerpT = ( t - span / (R)numSpans ) * numSpans;
	return mPoints[span] * ( 1 - lerpT ) + mPoints[span+1] * lerpT;
}

template<typename T>
T PolyLineT<T>::getDerivative( float t ) const
{
	typedef typename T::value_type R;
	if( mPoints.size() <= 1 ) return T();
	if( t >= 1 ) return mPoints.back() - mPoints[mPoints.size()-2];
	if( t <= 0 ) return mPoints[1] - mPoints[0];

	size_t numSpans = mPoints.size() - 1;
	size_t span = (size_t)math<R>::floor( t * numSpans );
	return mPoints[span+1] - mPoints[span];
}

template<typename T>
void PolyLineT<T>::scale( const T &scaleFactor, T scaleCenter )
{
	for( typename std::vector<T>::iterator ptIt = mPoints.begin(); ptIt != mPoints.end(); ++ptIt )
		*ptIt = scaleCenter + ( *ptIt - scaleCenter ) * scaleFactor;
}

template<typename T>
PolyLineT<T> PolyLineT<T>::scaled( const T &scaleFactor, T scaleCenter ) const
{
	PolyLineT<T> result( *this );
	result.scale( scaleFactor, scaleCenter );
	return result;
}

template<typename T>
void PolyLineT<T>::offset( const T &offsetBy )
{
	for( typename std::vector<T>::iterator ptIt = mPoints.begin(); ptIt != mPoints.end(); ++ptIt )
		*ptIt += offsetBy;
}

template<typename T>
PolyLineT<T> PolyLineT<T>::getOffset( const T &offsetBy ) const
{
	PolyLineT<T> result( *this );
	result.offset( offsetBy );
	return result;
}

template<typename T>
void PolyLineT<T>::reverse()
{
	std::reverse( mPoints.begin(), mPoints.end() );
}

template<typename T>
PolyLineT<T> PolyLineT<T>::reversed() const
{
	PolyLineT result( *this );
	std::reverse( result.mPoints.begin(), result.mPoints.end() );
	return result;
}

template<typename T>
T linearYatX( const glm::tvec2<T, glm::defaultp> p[2], T x )
{
	if( p[0].x == p[1].x ) 	return p[0].y;
	return p[0].y + (p[1].y - p[0].y) * (x - p[0].x) / (p[1].x - p[0].x);
}

template<typename T>
size_t linearCrossings( const glm::tvec2<T, glm::defaultp> p[2], const vec2 &pt )
{
	if( (p[0].x < pt.x && pt.x <= p[1].x ) ||
		(p[1].x < pt.x && pt.x <= p[0].x )) {
		if( pt.y > linearYatX<T>( p, pt.x ) )
			return 1;
	}
	return 0;
}

template<typename T>
bool PolyLineT<T>::contains( const vec2 &pt ) const
{
	if( mPoints.size() <= 2 )
		return false;

	size_t crossings = 0;
	for( size_t s = 0; s < mPoints.size() - 1; ++s ) {
		crossings += linearCrossings( &(mPoints[s]), pt );
	}

	T temp[2];
	temp[0] = mPoints[mPoints.size()-1];
	temp[1] = mPoints[0];
	crossings += linearCrossings( &(temp[0]), pt );

	return (crossings & 1) == 1;
}

template<typename T>
double PolyLineT<T>::calcArea() const
{
	double sum = 0;
	const size_t numPoints = mPoints.size();
	if( numPoints > 2 ) {
		for( size_t i = 0; i < numPoints - 1; ++i )
			sum += mPoints[i].x * mPoints[i+1].y - mPoints[i+1].x * mPoints[i].y;
		sum += mPoints[numPoints-1].x * mPoints[0].y - mPoints[0].x * mPoints[numPoints-1].y;
	}

	return glm::abs(sum * 0.5);
}

template<typename T>
T PolyLineT<T>::calcCentroid() const
{
	T result( 0 );

	const size_t numPoints = mPoints.size();
	double area = 0;
	if( numPoints > 2 ) {
		for( size_t i = 0; i < numPoints - 1; ++i ) {
			double subExpr = mPoints[i].x * mPoints[i+1].y - mPoints[i+1].x * mPoints[i].y;
			result.x += ( mPoints[i].x + mPoints[i+1].x ) * subExpr;
			result.y += ( mPoints[i].y + mPoints[i+1].y ) * subExpr;
			area += subExpr;
		}
		double subExpr = mPoints[numPoints-1].x * mPoints[0].y - mPoints[0].x * mPoints[numPoints-1].y;
		result.x += ( mPoints[numPoints-1].x + mPoints[0].x ) * subExpr;
		result.y += ( mPoints[numPoints-1].y + mPoints[0].y ) * subExpr;
		area += subExpr;

		result *= 1 / ( area * 3 );
	}

	return result;
}

namespace {
typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

template<typename T>
std::vector<PolyLineT<T> > convertBoostGeometryPolygons( std::vector<polygon> &polygons )
{
	std::vector<PolyLineT<T> > result;
	for( std::vector<polygon>::const_iterator outIt = polygons.begin(); outIt != polygons.end(); ++outIt ) {
		typedef polygon::inner_container_type::const_iterator RingIterator;
		typedef polygon::ring_type::const_iterator PointIterator;

		result.push_back( PolyLineT<T>() );
		for( PointIterator pt = outIt->outer().begin(); pt != outIt->outer().end(); ++pt )
			result.back().push_back( T( boost::geometry::get<0>(*pt), boost::geometry::get<1>(*pt) ) );

		for( RingIterator crunk = outIt->inners().begin(); crunk != outIt->inners().end(); ++crunk ) {
			PolyLineT<T> contour;
			for( PointIterator pt = crunk->begin(); pt != crunk->end(); ++pt )
				contour.push_back( T( boost::geometry::get<0>(*pt), boost::geometry::get<1>(*pt) ) );
			result.push_back( contour );
		}
	}

	return result;
}

template<typename T>
polygon convertPolyLinesToBoostGeometry( const std::vector<PolyLineT<T> > &a )
{
	polygon result;

	for( typename std::vector<T>::const_iterator ptIt = a[0].getPoints().begin(); ptIt != a[0].getPoints().end(); ++ptIt )
		result.outer().push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( ptIt->x, ptIt->y ) );
	for( typename std::vector<PolyLineT<T> >::const_iterator plIt = a.begin() + 1; plIt != a.end(); ++plIt ) {
		polygon::ring_type ring;
		for( typename std::vector<T>::const_iterator ptIt = plIt->getPoints().begin(); ptIt != plIt->getPoints().end(); ++ptIt )
			ring.push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( ptIt->x, ptIt->y ) );
		result.inners().push_back( ring );
	}

	boost::geometry::correct( result );

	return result;
}
} // anonymous namespace

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcUnion( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::union_( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcIntersection( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::intersection( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcXor( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::sym_difference( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcDifference( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::difference( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template class PolyLineT<vec2>;
template class PolyLineT<dvec2>;

} // namespace cinder


#define BOOST_TEST_MODULE pcraster fieldapi interface
#include <boost/test/unit_test.hpp>
#include "pcrtypes.h"
#include "geo_celllocvisitor.h"
#include "fieldapi_interface.h"
#include "fieldapi_testfield.h"

BOOST_AUTO_TEST_CASE(non_mv)
{
  using namespace fieldapi;

  // 1st row all nonMV
  // 2nd row allways some MV
     UINT1  l1[ ] = { 0, 1, 2 , 3, MV_UINT1, 5 };
     UINT1  l2[ ] = { 0, 1, 2 , 3, MV_UINT1, MV_UINT1 };
     REAL4  l3[ ] = { 0, 1, 2 , 3, 70,       80 };
     pcr::setMV(l3[3]);

     TestField<UINT1,2,3> d1(l1);
     TestField<UINT1,2,3> d2(l2);
     TestField<REAL4,2,3> d3(l3);

     ReadWriteData<UINT1,UINT1> t1(TEST_FIELD_INIT(d1));
     ReadWriteData<UINT1,UINT1> t2(TEST_FIELD_INIT(d2));
     ReadWriteData<REAL8,REAL4> t3(TEST_FIELD_INIT(d3));
     ReadOnlyNonSpatial<INT4>   t4(123,t1.nrRows(),t1.nrCols());

     std::vector<const Common*> all;
     all.push_back(&t1);
     all.push_back(&t2);
     all.push_back(&t3);
     all.push_back(&t4);
     for(geo::CellLocVisitor v(t1.nrRows(),t1.nrCols());
          v.valid(); ++v) {
        if ((*v).row() == 0)
          BOOST_CHECK(nonMV(all,*v));
        else
          BOOST_CHECK(!nonMV(all,*v));
     }
}


//  (C) Copyright John Maddock 2007.
//  Use, modification and distribution are subject to 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_MATH_OVERFLOW_ERROR_POLICY
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#endif
#include <boost/math/concepts/real_concept.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/distributions/non_central_beta.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#define BOOST_CHECK_CLOSE_EX(a, b, prec, i) \
      {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK_CLOSE(a, b, prec); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
            {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " } " << std::endl;\
            }\
      }

#define BOOST_CHECK_EX(a, i) \
      {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK(a); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
            {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " } " << std::endl;\
            }\
      }

template <class T>
T nc_beta_cdf(T a, T b, T nc, T x)
{
#ifdef NC_BETA_CDF_FUNCTION_TO_TEST
   return NC_BETA_CDF_FUNCTION_TO_TEST(a, b, nc, x);
#else
   return cdf(boost::math::non_central_beta_distribution<T>(a, b, nc), x);
#endif
}

template <class T>
T nc_beta_ccdf(T a, T b, T nc, T x)
{
#ifdef NC_BETA_CCDF_FUNCTION_TO_TEST
   return NC_BETA_CCDF_FUNCTION_TO_TEST(a, b, nc, x);
#else
   return cdf(complement(boost::math::non_central_beta_distribution<T>(a, b, nc), x));
#endif
}

template <typename Real, typename T>
void do_test_nc_chi_squared(T& data, const char* type_name, const char* test)
{
   typedef Real                   value_type;

   std::cout << "Testing: " << test << std::endl;

   value_type(*fp1)(value_type, value_type, value_type, value_type) = nc_beta_cdf;
   boost::math::tools::test_result<value_type> result;

#if !(defined(ERROR_REPORTING_MODE) && !defined(NC_BETA_CDF_FUNCTION_TO_TEST))
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(fp1, 0, 1, 2, 3),
      extract_result<Real>(4));
   handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "non central beta CDF", test);
#endif
#if !(defined(ERROR_REPORTING_MODE) && !defined(NC_BETA_CCDF_FUNCTION_TO_TEST))
   fp1 = nc_beta_ccdf;
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(fp1, 0, 1, 2, 3),
      extract_result<Real>(5));
   handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "non central beta CDF complement", test);
#endif
   std::cout << std::endl;

}

template <typename Real, typename T>
void quantile_sanity_check(T& data, const char* type_name, const char* test)
{
#ifndef ERROR_REPORTING_MODE
   typedef Real                   value_type;

   //
   // Tests with type real_concept take rather too long to run, so
   // for now we'll disable them:
   //
   if(!boost::is_floating_point<value_type>::value)
      return;

   std::cout << "Testing: " << type_name << " quantile sanity check, with tests " << test << std::endl;

   //
   // These sanity checks test for a round trip accuracy of one half
   // of the bits in T, unless T is type float, in which case we check
   // for just one decimal digit.  The problem here is the sensitivity
   // of the functions, not their accuracy.  This test data was generated
   // for the forward functions, which means that when it is used as
   // the input to the inverses then it is necessarily inexact.  This rounding
   // of the input is what makes the data unsuitable for use as an accuracy check,
   // and also demonstrates that you can't in general round-trip these functions.
   // It is however a useful sanity check.
   //
   value_type precision = static_cast<value_type>(ldexp(1.0, 1 - boost::math::policies::digits<value_type, boost::math::policies::policy<> >() / 2)) * 100;
   if(boost::math::policies::digits<value_type, boost::math::policies::policy<> >() < 50)
      precision = 1;   // 1% or two decimal digits, all we can hope for when the input is truncated to float

   for(unsigned i = 0; i < data.size(); ++i)
   {
      //
      // Test case 493 fails at float precision: not enough bits to get
      // us back where we started:
      //
      if((i == 493) && boost::is_same<float, value_type>::value)
         continue;

      if(data[i][4] == 0)
      {
         BOOST_CHECK(0 == quantile(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]), data[i][4]));
      }
      else if(data[i][4] < 0.9999f)
      {
         value_type p = quantile(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]), data[i][4]);
         value_type pt = data[i][3];
         BOOST_CHECK_CLOSE_EX(pt, p, precision, i);
      }
      if(data[i][5] == 0)
      {
         BOOST_CHECK(1 == quantile(complement(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]), data[i][5])));
      }
      else if(data[i][5] < 0.9999f)
      {
         value_type p = quantile(complement(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]), data[i][5]));
         value_type pt = data[i][3];
         BOOST_CHECK_CLOSE_EX(pt, p, precision, i);
      }
      if(boost::math::tools::digits<value_type>() > 50)
      {
         //
         // Sanity check mode, accuracy of
         // the mode is at *best* the square root of the accuracy of the PDF:
         //
         value_type m = mode(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]));
         if((m == 1) || (m == 0))
            break;
         value_type p = pdf(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]), m);
         if(m * (1 + sqrt(precision) * 10) < 1)
         {
            BOOST_CHECK_EX(pdf(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]), m * (1 + sqrt(precision) * 10)) <= p, i);
         }
         if(m * (1 - sqrt(precision)) * 10 > boost::math::tools::min_value<value_type>())
         {
            BOOST_CHECK_EX(pdf(boost::math::non_central_beta_distribution<value_type>(data[i][0], data[i][1], data[i][2]), m * (1 - sqrt(precision)) * 10) <= p, i);
         }
      }
   }
#endif
}

template <typename T>
void test_accuracy(T, const char* type_name)
{
#if !defined(TEST_DATA) || (TEST_DATA == 1)
#include "ncbeta.ipp"
   do_test_nc_chi_squared<T>(ncbeta, type_name, "Non Central Beta, medium parameters");
   quantile_sanity_check<T>(ncbeta, type_name, "Non Central Beta, medium parameters");
#endif
#if !defined(TEST_DATA) || (TEST_DATA == 2)
#include "ncbeta_big.ipp"
   do_test_nc_chi_squared<T>(ncbeta_big, type_name, "Non Central Beta, large parameters");
   // Takes too long to run:
   // quantile_sanity_check(ncbeta_big, type_name, "Non Central Beta, large parameters");
#endif
}


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/protect.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/list.hpp>

#include <cmath>
#include <limits>
#include <sstream>
#include <boost/safe_float.hpp>
#include <boost/safe_float/convenience.hpp>
#include <boost/safe_float/policy/check_addition_overflow.hpp>
#include <boost/safe_float/policy/check_addition_underflow.hpp>
#include <boost/safe_float/policy/check_addition_inexact.hpp>
#include <boost/safe_float/policy/check_addition_invalid_result.hpp>

//types to be tested
using test_types=boost::mpl::list<
    float, double, long double
>;

using namespace boost::safe_float;

/**
  This test suite checks different policies on addition operations using default parameters for the other policies.
  */
BOOST_AUTO_TEST_SUITE( safe_float_misc_functions_test_suite )

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_istream, FPT, test_types){
    FPT f = std::numeric_limits<FPT>::max();
    safe_float<FPT> sf;

    std::stringstream ssfp;
    ssfp.precision(std::numeric_limits<FPT>::max_digits10);
    ssfp << f;
    ssfp >> sf;

    BOOST_CHECK_EQUAL(sf.get_stored_value(), f);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_ostream, FPT, test_types){
    FPT f = std::numeric_limits<FPT>::max();
    safe_float<FPT> sf = std::numeric_limits<FPT>::max();
    std::stringstream ssfp, sfp;
    ssfp << sf;
    sfp << f;
    BOOST_CHECK_EQUAL(ssfp.str(), sfp.str());
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_comparison_operations, FPT, test_types){
    //comparison operations are not expected to require any policy check
    //comparison is expected to behave as the internal type behaves
    safe_float<FPT> zero = FPT(0.0);
    safe_float<FPT> max = std::numeric_limits<FPT>::max();
    safe_float<FPT> min = std::numeric_limits<FPT>::min();

    BOOST_CHECK( min <  max);
    BOOST_CHECK( zero <= min);
    BOOST_CHECK( min > zero);
    BOOST_CHECK( max >=  zero);
    BOOST_CHECK( zero ==  zero);
    BOOST_CHECK( min !=  max);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef TELEMETRY_TIME_HPP__
#define TELEMETRY_TIME_HPP__

#include <ros/ros.h>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>

class delta_loop_time
{
    private:

        bool metricsEnabled;

        //boost::mutex metricMuxtex;
        //ros::Time does not need a thread lock because it is actually locked internally
        boost::accumulators::accumulator_set<double, boost::accumulators::stats<boost::accumulators::tag::rolling_mean> > acc;
        ros::Time lastTimeMetric;
        double deltaTime;

    public:
        delta_loop_time():
        lastTimeMetric(ros::Time::now().toNSec()),
        acc(boost::accumulators::tag::rolling_window::window_size = 150)
        {
            metricsEnabled = true;
        }
        bool enableMetrics()
        {
            metricsEnabled = true;
            this->lastTimeMetric = ros::Time::now();
            return metricsEnabled;
        };

        bool disableMetrics()
        {
            metricsEnabled = false;
            return metricsEnabled;
        };

        float updateMetrics()
        {
            if(metricsEnabled)
            {
                boost::scoped_ptr<char> output(new char[255]);
                deltaTime = ros::Time::now().toSec()-this->lastTimeMetric.toSec();
                acc(deltaTime);
                this->lastTimeMetric = ros::Time::now();
                return 1/(boost::accumulators::rolling_mean(acc));
            }
            return 0;
        };

        std::string printMetrics(bool printSideEffect)
        {
            if(metricsEnabled)
            {
                boost::scoped_ptr<char> output(new char[255]);
                deltaTime = ros::Time::now().toSec()-this->lastTimeMetric.toSec();
                acc(deltaTime);
                sprintf(output.get(), "Delta Time %2.3f:: Hz %2.2f:: Avg Hz %2.2f",
                                                deltaTime, 1/deltaTime, 1/(boost::accumulators::rolling_mean(acc)));
                this->lastTimeMetric = ros::Time::now();
                if(printSideEffect)
                {
                    ROS_DEBUG("%s", output.get());
                }

                std::string outputString(1, *output);
                return outputString;
            }
            else
            {
                return "";
            }
        };
};

#endif //TELEMETRY_TIME_HPP__