/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <ql/instruments/impliedvolatility.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <qle/pricingengines/baroneadesiwhaleyengine.hpp>
#include <qle/termstructures/blackvariancesurfacesparse.hpp>
#include <qle/termstructures/equityoptionsurfacestripper.hpp>

using std::pair;
using std::vector;
using namespace QuantLib;

namespace {

class PriceError {
public:
    PriceError(const VanillaOption& option, SimpleQuote& vol, Real targetValue)
        : option_(option), vol_(vol), targetValue_(targetValue){};
    Real operator()(Volatility x) const;

private:
    const VanillaOption& option_;
    SimpleQuote& vol_;
    Real targetValue_;
};

Real PriceError::operator()(Volatility x) const {
    vol_.setValue(x);
    Real npv;
    // Barone Adesi Whaley fails for very small variance, so wrap in a try/catch
    try {
        npv = option_.NPV();
    } catch (...) {
        npv = 0.0;
    }
    return npv - targetValue_;
}

} // namespace

namespace QuantExt {

EquityOptionSurfaceStripper::EquityOptionSurfaceStripper(const boost::shared_ptr<OptionInterpolatorBase>& callSurface,
                                                         const boost::shared_ptr<OptionInterpolatorBase>& putSurface,
                                                         const Handle<EquityIndex>& eqIndex, const Calendar& calendar,
                                                         const DayCounter& dayCounter, Exercise::Type type,
                                                         bool lowerStrikeConstExtrap, bool upperStrikeConstExtrap,
                                                         bool timeFlatExtrapolation)
    : callSurface_(callSurface), putSurface_(putSurface), eqIndex_(eqIndex), calendar_(calendar),
      dayCounter_(dayCounter), type_(type), lowerStrikeConstExtrap_(lowerStrikeConstExtrap),
      upperStrikeConstExtrap_(upperStrikeConstExtrap), timeFlatExtrapolation_(timeFlatExtrapolation) {

    // the call and put surfaces should have the same expiries/strikes/reference date/day counters, some checks to
    // ensure this
    QL_REQUIRE(callSurface_->referenceDate() == putSurface_->referenceDate(),
               "Mismatch between Call and Put reference dates in EquityOptionPremiumCurveStripper");

    // register with all market data
    registerWith(eqIndex);
    registerWith(Settings::instance().evaluationDate());
}

void EquityOptionSurfaceStripper::performCalculations() const {
    // create a set of all dates
    std::set<Date> allExpiries;
    std::vector<Date> callExpiries = callSurface_->expiries();
    std::vector<Date> putExpiries = putSurface_->expiries();
    for (auto expiry : callExpiries)
        allExpiries.insert(expiry);
    for (auto expiry : putExpiries)
        allExpiries.insert(expiry);

    boost::shared_ptr<SimpleQuote> volQuote;
    boost::shared_ptr<PricingEngine> engine;

    boost::shared_ptr<BlackVarianceSurfaceSparse> callVolSurface, putVolSurface;

    // if the surface is a price surface then we have premiums
    bool premiumSurfaces = boost::dynamic_pointer_cast<OptionPriceSurface>(callSurface_) != NULL;
    if (premiumSurfaces) {
        // first also check the put surface
        QL_REQUIRE(boost::dynamic_pointer_cast<OptionPriceSurface>(putSurface_) != NULL,
                   "Call price surface provided, but no put price surface");

        // Set up the engine for implying the vols
        // term structures needed to get implied vol
        volQuote = boost::make_shared<SimpleQuote>(0.1);
        Handle<BlackVolTermStructure> volTs(boost::make_shared<BlackConstantVol>(
            callSurface_->referenceDate(), calendar_, Handle<Quote>(volQuote), dayCounter_));

        // a black scholes process
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp = boost::make_shared<BlackScholesMertonProcess>(
            eqIndex_->equitySpot(), eqIndex_->equityDividendCurve(), eqIndex_->equityForecastCurve(), volTs);

        // hard code the engines here
        // BaroneAdesiWhaley for American options - much faster than alternatives
        // Black for European
        if (type_ == Exercise::American) {
            engine = boost::make_shared<QuantExt::BaroneAdesiWhaleyApproximationEngine>(gbsp);
        } else if (type_ == Exercise::European) {
            engine = boost::make_shared<QuantExt::AnalyticEuropeanEngine>(gbsp);
        } else {
            QL_FAIL("Unsupported exercise type for option stripping");
        }
    } else {
        // we have variance surfaces, explicitly cast so we can look up vol later
        callVolSurface = boost::dynamic_pointer_cast<BlackVarianceSurfaceSparse>(callSurface_);
        putVolSurface = boost::dynamic_pointer_cast<BlackVarianceSurfaceSparse>(putSurface_);
    }

    vector<Real> volStrikes;
    vector<Real> volData;
    vector<Date> volExpiries;

    // loop over each expiry
    for (auto exp : allExpiries) {
        // get the forward price at time
        Real forward = eqIndex_->fixing(exp);

        vector<Real> callStrikes, putStrikes;
        auto itc = std::find(callExpiries.begin(), callExpiries.end(), exp);
        if (itc != callExpiries.end()) {
            auto pos = std::distance(callExpiries.begin(), itc);
            callStrikes = callSurface_->strikes().at(pos);
        }
        auto itp = std::find(putExpiries.begin(), putExpiries.end(), exp);
        if (itp != putExpiries.end()) {
            auto pos = std::distance(putExpiries.begin(), itp);
            putStrikes = putSurface_->strikes().at(pos);
        }

        // We want a set of prices both sides of ATM forward
        // We take calls where strike < atm, and puts where strike > atm

        bool haveCalls = false, havePuts = false;
        // check if calls in the correct range
        if (callStrikes.size() > 0 && callStrikes.front() < forward)
            haveCalls = true;

        // check if puts in the correct range
        if (putStrikes.size() > 0 && putStrikes.back() > forward)
            havePuts = true;

        for (auto cs : callStrikes) {
            if (!havePuts || cs < forward) {
                volStrikes.push_back(cs);
                volExpiries.push_back(exp);
                if (premiumSurfaces) {
                    volData.push_back(implyVol(exp, cs, Option::Call, engine, volQuote));
                } else {
                    volData.push_back(callVolSurface->blackVol(exp, cs));
                }
            }
        }
        for (auto ps : putStrikes) {
            if (!haveCalls || ps > forward) {
                volStrikes.push_back(ps);
                volExpiries.push_back(exp);
                if (premiumSurfaces) {
                    volData.push_back(implyVol(exp, ps, Option::Put, engine, volQuote));
                } else {
                    volData.push_back(putVolSurface->blackVol(exp, ps));
                }
            }
        }
    }
    volSurface_ = boost::make_shared<BlackVarianceSurfaceSparse>(
        callSurface_->referenceDate(), calendar_, volExpiries, volStrikes, volData, dayCounter_,
        lowerStrikeConstExtrap_, upperStrikeConstExtrap_, timeFlatExtrapolation_);
}

Real EquityOptionSurfaceStripper::implyVol(Date expiry, Real strike, Option::Type type,
                                           boost::shared_ptr<PricingEngine> engine,
                                           boost::shared_ptr<SimpleQuote> volQuote) const {

    // create an american option for current strike/expiry and type
    boost::shared_ptr<StrikedTypePayoff> payoff(new PlainVanillaPayoff(type, strike));
    boost::shared_ptr<Exercise> exercise;
    if (type_ == Exercise::American) {
        exercise = boost::make_shared<AmericanExercise>(expiry);
    } else if (type_ == Exercise::European) {
        exercise = boost::make_shared<EuropeanExercise>(expiry);
    } else {
        QL_FAIL("Unsupported exercise type for option stripping");
    }
    VanillaOption option(payoff, exercise);
    option.setPricingEngine(engine);

    // option.setPricingEngine(engine);
    Real targetPrice =
        type == Option::Call ? callSurface_->getValue(expiry, strike) : putSurface_->getValue(expiry, strike);

    // calculate the implied volatility using a solver
    Real vol;
    try {
        PriceError f(option, *volQuote, targetPrice);
        Brent solver;
        solver.setMaxEvaluations(100);
        solver.setLowerBound(0.0001);
        vol = solver.solve(f, 0.0001, 0.2, 0.01);
    } catch (...) {
        vol = 0.0;
    }
    return vol;
}

boost::shared_ptr<QuantLib::BlackVolTermStructure> EquityOptionSurfaceStripper::volSurface() {
    calculate();
    return volSurface_;
}

} // namespace QuantExt


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      130124    S. Hirsh          File created.
 *
 *    References
 *    Hackworth, M. Magnetic Fields and Magnetic Forces,
 *          http://www2.cose.isu.edu/~hackmart/Magnetic%20Fields_engphys.pdf, last accessed: 20th
 *          January, 2014.
 *    Khan, S. Magnetism 3 Whats happens when a speeding proton goes through a magnetic field,
 *          https://www.khanacademy.org/science/physics/electricity-and-magnetism/v/magnetism-3,
 *          2013, last accessed: 16th January, 2014.
 *    Learning About Electronics Magnetic Force Calculator,
 *          http://www.learningaboutelectronics.com/Articles/Magnetic-force-calculator.php#answer1,
 *          last accessed: 20th January, 2014.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/ElectroMagnetism/lorentzStaticMagneticForce.h"
#include "Tudat/Astrodynamics/ElectroMagnetism/lorentzStaticMagneticAcceleration.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_lorentz_static_magnetic_acceleration_and_force_models )

//! Test implementation of static magnetic force model.
BOOST_AUTO_TEST_CASE( testStaticMagneticForceKhanAcademy )
{
    // Benchmark data is obtained using online video (Khan, 2013).

    // Tests approximate force on a proton due to a static uniform magnetic field.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticForce
                = Eigen::Vector3d( 0.0, -4.80653199e-12, 0.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 6.0e7, 0.0, 0.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 0.0, 0.0, 0.5 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 1.60217733e-19;

    // Compute static magnetic force [N].
    const Eigen::Vector3d computedStaticMagneticForce
                = electro_magnetism::computeLorentzForceDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic force vectors.
    BOOST_CHECK_SMALL( computedStaticMagneticForce.x( ),
                       std::numeric_limits< double >::min( ) );

    BOOST_CHECK_CLOSE_FRACTION( computedStaticMagneticForce.y( ),
                                expectedStaticMagneticForce.y( ),
                                std::numeric_limits< double >::epsilon( ) );

    BOOST_CHECK_SMALL( computedStaticMagneticForce.z( ),
                       std::numeric_limits< double >::min( ) );
}

//! Test implementation of static magnetic force model.
BOOST_AUTO_TEST_CASE( testStaticMagneticForceArbitraryCharge )
{
    // Benchmark data is obtained using Learning About Electronics.

    // Tests force using arbitrary values for parameters.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticForce = Eigen::Vector3d( 218.0, -172.0, 4.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 4.0, 5.0, -3.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 2.0, 3.0, 20.0 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 2.0;

    // Compute static magnetic force [N].
    const Eigen::Vector3d computedStaticMagneticForce
                = electro_magnetism::computeLorentzForceDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic force vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedStaticMagneticForce,
                                       expectedStaticMagneticForce,
                                       std::numeric_limits< double >::epsilon( ) );
}

//! Test implementation of static magnetic force model.
BOOST_AUTO_TEST_CASE( testStaticMagneticForceHackworth )
{
    // Benchmark data is obtained from pdf (Hackworth, M.).
    // Tests force on charge in uniform magnetic field.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticForce = Eigen::Vector3d( 77.0, 28.0, -10.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( -2.0, 3.0, -7.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 4.0, -11.0, 0.0 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = -1.0;

    // Compute static magnetic force [N].
    const Eigen::Vector3d computedStaticMagneticForce
                = electro_magnetism::computeLorentzForceDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic force vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedStaticMagneticForce,
                                       expectedStaticMagneticForce,
                                       std::numeric_limits< double >::epsilon( ) );
}

//! Test implementation of static magnetic acceleration model.
BOOST_AUTO_TEST_CASE( testStaticMagneticAccelerationKhanAcademy )
{
    // Benchmark data is obtained using online video (Khan, 2013).

    // Tests approximate force on a proton due to a static uniform magnetic field.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticAcceleration =
            Eigen::Vector3d( 0.0, -2.8736514e15, 0.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 6.0e7, 0.0, 0.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 0.0, 0.0, 0.5 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 1.60217733e-19;

    // Set mass of accelerated body [kg].
    const double massOfBodySubjectToAcceleration = 1.67262178e-27;

    // Compute static magnetic acceleration [N].
    const Eigen::Vector3d computedStaticMagneticAcceleration
                = electro_magnetism::computeLorentzAccelerationDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration,
                    massOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic acceleration vectors.
    BOOST_CHECK_SMALL( computedStaticMagneticAcceleration.x( ),
                       std::numeric_limits< double >::min( ) );

    BOOST_CHECK_CLOSE_FRACTION( computedStaticMagneticAcceleration.y( ),
                                expectedStaticMagneticAcceleration.y( ),
                                1.0e-7 );

    BOOST_CHECK_SMALL( computedStaticMagneticAcceleration.z( ),
                       std::numeric_limits< double >::min( ) );
}

//! Test implementation of static magnetic acceleration model.
BOOST_AUTO_TEST_CASE( testStaticMagneticAccelerationArbitraryCharge )
{
    // Benchmark data is obtained using Learning About Electronics.

    // Tests force using arbitrary values for parameters.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticAcceleration
                = Eigen::Vector3d( 43.6, -34.4, 0.8 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 4.0, 5.0, -3.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 2.0, 3.0, 20.0 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 2.0;

    // Set mass of accelerated body [kg].
    const double massOfBodySubjectToAcceleration = 5.0;

    // Compute static magnetic acceleration [N].
    const Eigen::Vector3d computedStaticMagneticAcceleration
                = electro_magnetism::computeLorentzAccelerationDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration,
                    massOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedStaticMagneticAcceleration,
                                       expectedStaticMagneticAcceleration,
                                       std::numeric_limits< double >::epsilon( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/**
 * Copyright (c) 2015 Johannes Reifferscheid
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
 * associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT
 * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#ifndef CTA_EVALUATOR_HPP__
#define CTA_EVALUATOR_HPP__

#include <tuple>
#include <cmath>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/facilities/expand.hpp>
#include "macros.hpp"

namespace cta {
namespace detail {

template <typename A>
struct evaluator {
	template <typename... Args>
	static const A& evaluate(const A& a, const Args&...) {
		return a;
	}
};

template <typename number, int var>
struct evaluator<var_type<number, var>> {
	template <typename... Args>
	static number evaluate(const var_type<number, var>&, const Args&... args) {
		static_assert(var < sizeof...(args), "required variable not passed to eval");
		return std::get<var>(std::make_tuple(args...));
	}
};

template <typename number, int var, int index>
struct evaluator<dvar_type<number, var, index>> {
	template <typename... Args>
	static number evaluate(const dvar_type<number, var, index>&, const Args&... args) {
		static_assert(var < sizeof...(args), "required variable not passed to eval");
		return std::get<index>(std::get<var>(std::make_tuple(args...)));
	}
};

template <typename number, typename T, int p>
struct evaluator<static_power<number, T, p>> {
	template <typename... Args>
	static number evaluate(const static_power<number, T, p>& v, const Args&... args) {
		return pow(evaluator<T>::evaluate(v.a0, args...), p);
	}
};

template <typename... Args>
void silence_unused(const Args&...) {}

// evaluator for `type' with n arguments, operation type (infix, prefix, ...), argument for
// operation
#define CTA_VISIT_TYPE(type, n, op, arg, ...)                                                      \
	template <CTA_TEMPLATE_DECL_ARGS(n)>                                                           \
	struct evaluator<type<CTA_TEMPLATE_USE_ARGS(n)>> {                                             \
		template <typename... Args>                                                                \
		static number evaluate(const type<CTA_TEMPLATE_USE_ARGS(n)>& t, const Args&... args) {     \
			(void) t;                                                                              \
			silence_unused(args...);                                                               \
			return op(arg, n);                                                                     \
		}                                                                                \
	};

// evaluation of argument n
#define CTA_EVAL_ARG(n) evaluator<BOOST_PP_CAT(A, n)>::evaluate(t.BOOST_PP_CAT(a, n), args...)

// infix operator
#define CTA_INFIX(operator, ...) CTA_EVAL_ARG(0) operator CTA_EVAL_ARG(1)
// prefix operator
#define CTA_PREFIX(operator, ...) operator CTA_EVAL_ARG(0)
// function application of `funname' with n evaluated arguments
#define CTA_EVAL_ARG_LIST(z, n, _) BOOST_PP_COMMA_IF(n) CTA_EVAL_ARG(n)
#define CTA_FUN(funname, n) funname(BOOST_PP_REPEAT(n, CTA_EVAL_ARG_LIST, _))
// return the given value
#define CTA_RET(value, ...) value

#include "types.hpp"

#undef CTA_EVAL_ARG
#undef CTA_EVAL_ARG_LIST

} // namespace detail

template <typename A, typename... Args>
auto evaluate(A& a, const Args&... args) -> decltype(detail::evaluator<A>().evaluate(a, args...)) {
	return detail::evaluator<A>::evaluate(a, args...);
}

template <typename number, typename T>
template <typename... Args>
number detail::cta_type<number, T>::operator()(const Args&... args) const {
	return detail::evaluator<T>::evaluate(*static_cast<const T*>(this), args...);
}

} // namespace cta

#endif


#ifndef _MATRIX_SLICE_HPP_
#define _MATRIX_SLICE_HPP_

#include <boost/array.hpp>

namespace matrix {

    template<size_t N = 2>
    struct slice {
	typedef int index_type;
	typedef size_t size_type;
	typedef boost::array<size_type,2> size_array;
	typedef boost::array<index_type,2> index_array;
	template<class M> class expression;
	slice(const index_array &from, const index_array &to) {
	    for (uint i = 0; i < N; ++i) {
		index_[i] = from[i];
		size_[i] = to[i] - from[i] + 1;
	    }
	}
	template<class M>
	expression<M> operator()(M &A) const {
	    return expression<M>(A, index_, size_);
	}
    private:
	size_array size_;
	index_array index_;
    };

    template<size_t N> template<class M>
    struct slice<N>::expression {
	typedef int index_type;
	typedef typename M::size_type size_type;
	typedef typename M::value_type value_type;
	typedef typename M::reference reference;
	typedef typename M::const_reference const_reference;
	typedef boost::array<size_type,2> size_array;
	typedef boost::array<index_type,2> index_array;
 	expression(M &A) : A_(A) {}
	expression(M &A, const index_array &index, const size_array &size)
	    : A_(A), index_(index), size_(size) {}
	size_type size1() const { return size_[0]; }
	size_type size2() const { return size_[1]; }
	reference operator()(index_type i, index_type j) {
	    //printf("%i %i\n", i-index_[0], j-index_[1]);
	    return A_(i+index_[0], j+index_[1]);
	}
	const_reference operator()(index_type i, index_type j) const {
	    //printf("%i %i\n", i-index_[0], j-index_[1]);
	    return A_(i+index_[0], j+index_[1]);
	}
    private:
	M &A_;
	index_array index_;
	size_array size_;
    };

}

#endif /* _MATRIX_SLICE_HPP_ */


#include <qubus/qtl/lower_top_level_sums.hpp>

#include <qubus/IR/qir.hpp>

#include <qubus/pattern/IR.hpp>
#include <qubus/pattern/core.hpp>

#include <qubus/qtl/pattern/all.hpp>

#include <boost/range/adaptor/reversed.hpp>

#include <mutex>
#include <string>
#include <vector>

namespace qubus
{
namespace qtl
{

namespace
{

std::unique_ptr<expression> remove_top_level_sums(const expression& expr,
                                                  std::vector<variable_declaration>& sum_indices)
{
    using qubus::pattern::variable;
    using pattern::sum;

    variable<const expression&> body;
    variable<variable_declaration> index;

    auto m = qubus::pattern::make_matcher<expression, std::unique_ptr<expression>>()
                 .case_(sum(body, index),
                        [&] {
                            sum_indices.push_back(index.get());

                            return remove_top_level_sums(body.get(), sum_indices);
                        })
                 .case_(body, [&] { return clone(body.get()); });

    return qubus::pattern::match(expr, m);
}
}

std::unique_ptr<expression> lower_top_level_sums(const expression& expr)
{
    using qubus::pattern::variable;
    using pattern::sum;

    variable<const expression &> a, b, c, d;
    variable<const sum_expr&> s;
    variable<variable_declaration> idx;
    variable<std::vector<std::reference_wrapper<expression>>> sub_exprs;
    variable<binary_op_tag> tag;

    using qubus::pattern::_;

    auto m = qubus::pattern::make_matcher<expression, std::unique_ptr<expression>>()
                 .case_(pattern::for_all(idx, b),
                        [&] { return for_all(idx.get(), lower_top_level_sums(b.get())); })
                 .case_(for_(idx, b, c, d),
                        [&] {
                            return for_(idx.get(), clone(b.get()), clone(c.get()),
                                        lower_top_level_sums(d.get()));
                        })
                 .case_(compound(sub_exprs),
                        [&] {
                            std::vector<std::unique_ptr<expression>> new_sub_expressions;

                            for (const auto& sub_expr : sub_exprs.get())
                            {
                                new_sub_expressions.push_back(lower_top_level_sums(sub_expr));
                            }

                            return sequenced_tasks(std::move(new_sub_expressions));
                        })
                 .case_(assign(a, bind_to(sum(_, _), s)),
                        [&] {
                            std::vector<variable_declaration> sum_indices;

                            auto new_rhs = remove_top_level_sums(s.get(), sum_indices);

                            auto initializer = assign(clone(a.get()), double_literal(0));

                            std::unique_ptr<expression> new_expr =
                                plus_assign(clone(a.get()), std::move(new_rhs));

                            for (const auto& sum_index : sum_indices | boost::adaptors::reversed)
                            {
                                new_expr = for_all(sum_index, std::move(new_expr));
                            }

                            return sequenced_tasks(std::move(initializer), std::move(new_expr));
                        })
                 .case_(a, [&] { return clone(a.get()); });

    return qubus::pattern::match(expr, m);
}

}
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestEqual
#include <boost/test/unit_test.hpp>

#include <boost/compute/algorithm/equal.hpp>
#include <boost/compute/algorithm/fill.hpp>
#include <boost/compute/container/string.hpp>
#include <boost/compute/container/vector.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(equal_int)
{
    int data1[] = { 1, 2, 3, 4, 5, 6 };
    int data2[] = { 1, 2, 3, 7, 5, 6 };

    boost::compute::vector<int> vector1(data1, data1 + 6);
    boost::compute::vector<int> vector2(data2, data2 + 6);

    BOOST_CHECK(boost::compute::equal(vector1.begin(), vector1.end(), vector2.begin()) == false);
    BOOST_CHECK(boost::compute::equal(vector1.begin(), vector1.begin() + 2, vector2.begin()) == true);
    BOOST_CHECK(boost::compute::equal(vector1.begin() + 4, vector1.end(), vector2.begin() + 4) == true);
}

BOOST_AUTO_TEST_CASE(equal_string)
{
    boost::compute::string a = "abcdefghijk";
    boost::compute::string b = "abcdefghijk";
    boost::compute::string c = "abcdezghijk";

    BOOST_CHECK(boost::compute::equal(a.begin(), a.end(), b.begin()) == true);
    BOOST_CHECK(boost::compute::equal(a.begin(), a.end(), c.begin()) == false);
}

BOOST_AUTO_TEST_CASE(equal_different_range_sizes)
{
    boost::compute::vector<int> a(10, context);
    boost::compute::vector<int> b(20, context);

    boost::compute::fill(a.begin(), a.end(), 3, queue);
    boost::compute::fill(b.begin(), b.end(), 3, queue);

    BOOST_CHECK(boost::compute::equal(a.begin(), a.end(), b.begin(), b.end(), queue) == false);
    BOOST_CHECK(boost::compute::equal(a.begin(), a.end(), a.begin(), a.end(), queue) == true);
    BOOST_CHECK(boost::compute::equal(b.begin(), b.end(), a.begin(), a.end(), queue) == false);
    BOOST_CHECK(boost::compute::equal(b.begin(), b.end(), b.begin(), b.end(), queue) == true);
}

BOOST_AUTO_TEST_SUITE_END()


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

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

 * See LICENSE for the license information

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

/**
 * @file    timeSLAMlike.cpp
 * @brief   Times solving of random SLAM-like graphs
 * @author  Richard Roberts
 * @date    Aug 30, 2010
 */

#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>

#include <boost/random.hpp>
#include <boost/timer.hpp>
#include <boost/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <vector>

using namespace gtsam;
using namespace std;
using namespace boost::lambda;

typedef EliminationTree<JacobianFactor> GaussianEliminationTree;

static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));

bool _pair_compare(const pair<Index, Matrix>& a1, const pair<Index, Matrix>& a2) { return a1.first < a2.first; }

int main(int argc, char *argv[]) {

  size_t vardim = 3;
  size_t blockdim = 3;
  int nVars = 500;
  size_t blocksPerVar = 5;
  size_t varsPerBlock = 2;
  size_t varSpread = 10;

  size_t nTrials = 10;

  double blockbuild, blocksolve;

  cout << "\n" << nVars << " variables of dimension " << vardim << ", " <<
      blocksPerVar << " blocks for each variable, blocks of dimension " << blockdim << " measure " << varsPerBlock << " variables\n";
  cout << nTrials << " trials\n";

  boost::variate_generator<boost::mt19937, boost::uniform_int<> > ri(boost::mt19937(), boost::uniform_int<>(-varSpread, varSpread));

  /////////////////////////////////////////////////////////////////////////////
  // Timing test with blockwise Gaussian factor graphs

  {
    // Build GFG's
    cout << "Building SLAM-like Gaussian factor graphs... ";
    cout.flush();
    boost::timer timer;
    timer.restart();
    vector<GaussianFactorGraph> blockGfgs;
    blockGfgs.reserve(nTrials);
    for(size_t trial=0; trial<nTrials; ++trial) {
      blockGfgs.push_back(GaussianFactorGraph());
      SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
      for(int c=0; c<nVars; ++c) {
        for(size_t d=0; d<blocksPerVar; ++d) {
          vector<pair<Index, Matrix> > terms; terms.reserve(varsPerBlock);
          if(c == 0 && d == 0)
            // If it's the first factor, just make a prior
            terms.push_back(make_pair(0, eye(vardim)));
          else if(c != 0) {
            // Generate a random Gaussian factor
            for(size_t h=0; h<varsPerBlock; ++h) {
              int var;
              // If it's the first factor for this variable, make it "odometry"
              if(d == 0 && h == 0)
                var = c-1;
              else if(d == 0 && h == 1)
                var = c;
              else
                // Choose random variable ids
                do
                  var = c + ri();
                while(var < 0 || var > nVars-1 || find_if(terms.begin(), terms.end(),
                    boost::bind(&pair<Index, Matrix>::first, boost::lambda::_1) == Index(var)) != terms.end());
              Matrix A(blockdim, vardim);
              for(size_t j=0; j<blockdim; ++j)
                for(size_t k=0; k<vardim; ++k)
                  A(j,k) = rg();
              terms.push_back(make_pair(Index(var), A));
            }
          }
          Vector b(blockdim);
          sort(terms.begin(), terms.end(), &_pair_compare);
          for(size_t j=0; j<blockdim; ++j)
            b(j) = rg();
          if(!terms.empty())
            blockGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(terms, b, noise)));
        }
      }

//      if(trial == 0)
//        blockGfgs.front().print("GFG: ");
    }
    blockbuild = timer.elapsed();
    cout << blockbuild << " s" << endl;

    // Solve GFG's
    cout << "Solving SLAM-like Gaussian factor graphs... ";
    cout.flush();
    timer.restart();
    for(size_t trial=0; trial<nTrials; ++trial) {
//      cout << "Trial " << trial << endl;
    	VectorValues soln(*GaussianMultifrontalSolver(blockGfgs[trial]).optimize());
    }
    blocksolve = timer.elapsed();
    cout << blocksolve << " s" << endl;
  }

  /////////////////////////////////////////////////////////////////////////////
  // Print per-graph times
  cout << "\nPer-factor-graph times for building and solving\n";
  cout << "  total " << (1000.0*(blockbuild+blocksolve)/double(nTrials)) <<
      "  build " << (1000.0*blockbuild/double(nTrials)) <<
      "  solve " << (1000.0*blocksolve/double(nTrials)) << " ms/graph\n";
  cout << endl;

  return 0;
}

/**
 * @file    timeSLAMlike.cpp
 * @brief
 * @author  Richard Roberts
 * @date Aug 30, 2010
 */


#include "SPlisHSPlasH/Common.h"
#include "GL/glew.h"
#include "Visualization/MiniGL.h"
#include "GL/glut.h"
#include "SPlisHSPlasH/TimeManager.h"
#include <Eigen/Dense>
#include <iostream>
#include "SPlisHSPlasH/Utilities/Timing.h"
#include "Utilities/PartioReaderWriter.h"
#include "SPlisHSPlasH/StaticRigidBody.h"
#include "Utilities/OBJLoader.h"
#include "SPlisHSPlasH/Utilities/PoissonDiskSampling.h"
#include "Demos/Common/DemoBase.h"
#include "Utilities/FileSystem.h"

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
	#define new DEBUG_NEW
#endif
#endif

using namespace SPH;
using namespace Eigen;
using namespace std;

void timeStep ();
void initBoundaryData();
void render ();
void renderBoundary();
void reset();
void simulationMethodChanged();

DemoBase base;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS;

	base.init(argc, argv, "StaticBoundaryDemo");
	initBoundaryData();
	base.buildModel();
	base.setSimulationMethodChangedFct(simulationMethodChanged);

	MiniGL::setClientIdleFunc(50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setClientSceneFunc(render);

	glutMainLoop ();

	base.cleanup ();

	Timing::printAverageTimes();
	Timing::printTimeSums();

	return 0;
}

void reset()
{
	Timing::printAverageTimes();
	Timing::reset();

	base.getSimulationMethod().simulation->reset();
	TimeManager::getCurrent()->setTime(0.0);
}

void simulationMethodChanged()
{
	reset();
}

void timeStep ()
{
	if ((base.getPauseAt() > 0.0) && (base.getPauseAt() < TimeManager::getCurrent()->getTime()))
		base.setPause(true);

	if (base.getPause())
		return;

	// Simulation code
	for (unsigned int i = 0; i < base.getNumberOfStepsPerRenderUpdate(); i++)
	{
		START_TIMING("SimStep");
		base.getSimulationMethod().simulation->step();
		STOP_TIMING_AVG;
	}
}

void render()
{
	MiniGL::coordinateSystem();

	base.renderFluid();
	renderBoundary();
}

void renderBoundary()
{
	DemoBase::SimulationMethod &simulationMethod = base.getSimulationMethod();
	Shader &shader = base.getShader();
	Shader &meshShader = base.getMeshShader();
	SceneLoader::Scene &scene = base.getScene();
	const int renderWalls = base.getRenderWalls();
	GLint context_major_version = base.getContextMajorVersion();

	float wallColor[4] = { 0.1f, 0.6f, 0.6f, 1.0f };
	if ((renderWalls == 1) || (renderWalls == 2))
	{
		if (context_major_version > 3)
		{
			shader.begin();
			glUniform3fv(shader.getUniform("color"), 1, &wallColor[0]);
			glEnableVertexAttribArray(0);
			for (int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
			{
				if ((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
				{
					FluidModel::RigidBodyParticleObject *rb = simulationMethod.model.getRigidBodyParticleObject(body);
					glVertexAttribPointer(0, 3, GL_DOUBLE, GL_FALSE, 0, &simulationMethod.model.getPosition(body + 1, 0));
					glDrawArrays(GL_POINTS, 0, rb->numberOfParticles());
				}
			}
			glDisableVertexAttribArray(0);
			shader.end();
		}
		else
		{
			glDisable(GL_LIGHTING);
			glPointSize(4.0f);

			glBegin(GL_POINTS);
			for (int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
			{
				if ((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
				{
					FluidModel::RigidBodyParticleObject *rb = simulationMethod.model.getRigidBodyParticleObject(body);
					for (unsigned int i = 0; i < rb->numberOfParticles(); i++)
					{
						glColor3fv(wallColor);
						glVertex3v(&simulationMethod.model.getPosition(body + 1, i)[0]);
					}
				}
			}
			glEnd();
			glEnable(GL_LIGHTING);
		}
	}
	else if ((renderWalls == 3) || (renderWalls == 4))
	{
		for (int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
		{
			if ((renderWalls == 3) || (!scene.boundaryModels[body]->isWall))
			{
				meshShader.begin();
				glUniform1f(meshShader.getUniform("shininess"), 5.0f);
				glUniform1f(meshShader.getUniform("specular_factor"), 0.2f);

				GLfloat matrix[16];
				glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
				glUniformMatrix4fv(meshShader.getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
				GLfloat pmatrix[16];
				glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
				glUniformMatrix4fv(meshShader.getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);

				glUniform3fv(meshShader.getUniform("surface_color"), 1, wallColor);

				FluidModel::RigidBodyParticleObject *rb = simulationMethod.model.getRigidBodyParticleObject(body);
				MiniGL::drawMesh(((StaticRigidBody*)rb->m_rigidBody)->getGeometry(), wallColor);

				meshShader.end();
			}
		}
	}
}

void initBoundaryData()
{
	std::string base_path = FileSystem::getFilePath(base.getSceneFile());
	SceneLoader::Scene &scene = base.getScene();
	const bool useCache = base.getUseParticleCaching();

	for (unsigned int i = 0; i < scene.boundaryModels.size(); i++)
	{
		string meshFileName = FileSystem::normalizePath(base_path + "/" + scene.boundaryModels[i]->meshFile);

		std::vector<Vector3r> boundaryParticles;
		if (scene.boundaryModels[i]->samplesFile != "")
		{
			string particleFileName = base_path + "/" + scene.boundaryModels[i]->samplesFile;
			PartioReaderWriter::readParticles(particleFileName, scene.boundaryModels[i]->translation, scene.boundaryModels[i]->rotation, scene.boundaryModels[i]->scale[0], boundaryParticles);
		}

		// Cache sampling
		std::string mesh_base_path = FileSystem::getFilePath(scene.boundaryModels[i]->meshFile);
		std::string mesh_file_name = FileSystem::getFileName(scene.boundaryModels[i]->meshFile);
		std::string scene_path = FileSystem::getFilePath(base.getSceneFile());
		std::string scene_file_name = FileSystem::getFileName(base.getSceneFile());
		string cachePath = scene_path + "/" + mesh_base_path + "/Cache";
		string particleFileName = FileSystem::normalizePath(cachePath + "/" + scene_file_name + "_" + mesh_file_name + "_" + std::to_string(i) + ".bgeo");

		StaticRigidBody *rb = new StaticRigidBody();
		TriangleMesh &geo = rb->getGeometry();
		OBJLoader::loadObj(meshFileName, geo, scene.boundaryModels[i]->scale);
		for (unsigned int j = 0; j < geo.numVertices(); j++)
			geo.getVertices()[j] = scene.boundaryModels[i]->rotation * geo.getVertices()[j] + scene.boundaryModels[i]->translation;

		geo.updateNormals();
		geo.updateVertexNormals();

		if (scene.boundaryModels[i]->samplesFile == "")
		{
			bool foundCacheFile = false;
			if (useCache)
			{
				foundCacheFile = PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), 1.0, boundaryParticles);
				if (foundCacheFile)
					std::cout << "Loaded cached boundary sampling: " << particleFileName << "\n";

			}

			if (!useCache || !foundCacheFile)
			{
				std::cout << "Surface sampling of " << meshFileName << "\n";
				START_TIMING("Poisson disk sampling");
				PoissonDiskSampling sampling;
				sampling.sampleMesh(geo.numVertices(), geo.getVertices().data(), geo.numFaces(), geo.getFaces().data(), scene.particleRadius, 10, 1, boundaryParticles);
				STOP_TIMING_AVG;

				// Cache sampling
				if (useCache && (FileSystem::makeDir(cachePath) == 0))
				{
					std::cout << "Save particle sampling: " << particleFileName << "\n";
					PartioReaderWriter::writeParticles(particleFileName, (unsigned int)boundaryParticles.size(), boundaryParticles.data(), NULL, scene.particleRadius);
				}
			}
		}
		base.getSimulationMethod().model.addRigidBodyObject(rb, static_cast<unsigned int>(boundaryParticles.size()), &boundaryParticles[0]);
	}
}


#include "DrakeJoint.h"
#include "QuaternionFloatingJoint.h"
#include "RollPitchYawFloatingJoint.h"
#include "HelicalJoint.h"
#include "PrismaticJoint.h"
#include "RevoluteJoint.h"
#include <memory>
#include <vector>
#include <string>
#include <iostream>
#include <Eigen/Core>
#include "mex.h"
#include "matrix.h"
#include "drakeMexUtil.h"

using namespace Eigen;
using namespace std;

mxArray* safelyGetField(const mxArray* array, string field_name, int index = 0)
{
  mxArray* ret = mxGetField(array, index, field_name.c_str());
  if (!ret)
  {
    mexErrMsgTxt(("Field not found: " + field_name).c_str());
  }
  return ret;
}

void safelySetField(mxArray* array, const string& fieldname, mxArray* data, int index = 0)
{
  int fieldnum;
  fieldnum = mxGetFieldNumber(array, fieldname.c_str());
  if (fieldnum < 0) {
    fieldnum = mxAddField(array, fieldname.c_str());
  }
  mxSetFieldByNumber(array, index, fieldnum, data);
}

void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
  if (nrhs != 1 || nlhs != 1) {
    mexErrMsgIdAndTxt("Drake:testDrakeJointsmex:BadInputs","Usage struct_out = testGeometryGradientsmex(struct_in)");
  }
  string prismaticJointName = "prismatic";
  mxArray* mxPrismatic = safelyGetField(prhs[0], prismaticJointName);
  Vector3d prismatic_joint_axis = matlabToEigen<3, 1>(safelyGetField(mxPrismatic, "joint_axis"));

  string revoluteJointName = "revolute";
  mxArray* mxRevolute = safelyGetField(prhs[0], revoluteJointName);
  Vector3d revolute_joint_axis = matlabToEigen<3, 1>(safelyGetField(mxRevolute, "joint_axis"));

  string helicalJointName = "helical";
  mxArray* mxHelical = safelyGetField(prhs[0], helicalJointName);
  Vector3d helical_joint_axis = matlabToEigen<3, 1>(safelyGetField(mxHelical, "joint_axis"));
  double helical_joint_pitch = *mxGetPr(safelyGetField(mxHelical, "pitch"));

  Isometry3d transform_to_parent = Isometry3d::Identity();

  vector<unique_ptr<DrakeJoint>> joints;
  joints.push_back(unique_ptr<DrakeJoint>(new PrismaticJoint(prismaticJointName, transform_to_parent, prismatic_joint_axis)));
  joints.push_back(unique_ptr<DrakeJoint>(new RevoluteJoint(revoluteJointName, transform_to_parent, revolute_joint_axis)));
  joints.push_back(unique_ptr<DrakeJoint>(new HelicalJoint(helicalJointName, transform_to_parent, helical_joint_axis, helical_joint_pitch)));
  joints.push_back(unique_ptr<DrakeJoint>(new QuaternionFloatingJoint("quaternion_floating", transform_to_parent)));
  joints.push_back(unique_ptr<DrakeJoint>(new RollPitchYawFloatingJoint("rpy_floating", transform_to_parent)));

  const mwSize ndim = 1;
  mwSize dims[] = {1};
  plhs[0] = mxCreateStructArray(ndim, dims, 0, nullptr);
  for (vector<unique_ptr<DrakeJoint>>::const_iterator it = joints.begin(); it != joints.end(); it++) {
    const unique_ptr<DrakeJoint>& joint = (*it);
    mxArray* joint_struct_in = safelyGetField(prhs[0], joint->getName());
    VectorXd q = matlabToEigen<Eigen::Dynamic, 1>(safelyGetField(joint_struct_in, "q"));
    VectorXd v = matlabToEigen<Eigen::Dynamic, 1>(safelyGetField(joint_struct_in, "v"));
    string name = joint->getName();

    mxArray* joint_struct_out = mxCreateStructArray(1, dims, 0, nullptr);

    Isometry3d joint_transform = joint->jointTransform(q);
    safelySetField(joint_struct_out, "joint_transform", eigenToMatlab(joint_transform.matrix()));

    Eigen::Matrix<double, TWIST_SIZE, Eigen::Dynamic, 0, TWIST_SIZE, DrakeJoint::MAX_NUM_VELOCITIES> motion_subspace(6, joint->getNumVelocities());
    MatrixXd dmotion_subspace;
    joint->motionSubspace(q, motion_subspace, &dmotion_subspace);
    safelySetField(joint_struct_out, "motion_subspace", eigenToMatlab(motion_subspace));
    safelySetField(joint_struct_out, "dmotion_subspace", eigenToMatlab(dmotion_subspace));

    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    Vector6d motion_subspace_dot_times_v;
    Gradient<Vector6d, Eigen::Dynamic>::type dmotion_subspace_dot_times_vdq;
    Gradient<Vector6d, Eigen::Dynamic>::type dmotion_subspace_dot_times_vdv;

    joint->motionSubspaceDotTimesV(q, v, motion_subspace_dot_times_v,
        &dmotion_subspace_dot_times_vdq,
        &dmotion_subspace_dot_times_vdv);
    safelySetField(joint_struct_out, "motion_subspace_dot_times_v", eigenToMatlab(motion_subspace_dot_times_v));
    safelySetField(joint_struct_out, "dmotion_subspace_dot_times_vdq", eigenToMatlab(dmotion_subspace_dot_times_vdq));
    safelySetField(joint_struct_out, "dmotion_subspace_dot_times_vdv", eigenToMatlab(dmotion_subspace_dot_times_vdv));

    Matrix<double, Dynamic, Dynamic, 0, DrakeJoint::MAX_NUM_VELOCITIES, DrakeJoint::MAX_NUM_POSITIONS> qdot_to_v;
    MatrixXd dqdot_to_v;
    joint->qdot2v(q, qdot_to_v, &dqdot_to_v);
    safelySetField(joint_struct_out, "qdot_to_v", eigenToMatlab(qdot_to_v));
    safelySetField(joint_struct_out, "dqdot_to_v", eigenToMatlab(dqdot_to_v));

    Matrix<double, Dynamic, Dynamic, 0, DrakeJoint::MAX_NUM_POSITIONS, DrakeJoint::MAX_NUM_VELOCITIES> v_to_qdot;
    MatrixXd dv_to_qdot;
    joint->v2qdot(q, v_to_qdot, &dv_to_qdot);
    safelySetField(joint_struct_out, "v_to_qdot", eigenToMatlab(v_to_qdot));
    safelySetField(joint_struct_out, "dv_to_qdot", eigenToMatlab(dv_to_qdot));

    int fieldnum = mxAddField(plhs[0], name.c_str());
    mxSetFieldByNumber(plhs[0], 0, fieldnum, joint_struct_out);
  }
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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

\brief test_dimensionless_quantity.cpp

\details
Test unit class.

Output:
@verbatim
@endverbatim
**/

#include "test_header.hpp"

#include <boost/units/pow.hpp>

namespace bu = boost::units;

BOOST_STATIC_CONSTEXPR double E_ = 2.718281828459045235360287471352662497757;

int main()
{
    // default constructor
    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless>           E1;
    BOOST_TEST(E1.value() == double());

    // value_type constructor
    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless>           E2(E_);
    BOOST_TEST(E2.value() == E_);

    // copy constructor
    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless>           E3(E2);
    BOOST_TEST(E3.value() == E_);

    // operator=
    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless>           E4 = E2;
    BOOST_TEST(E4.value() == E_);

    // implicit copy constructor value_type conversion
    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless,float>     E5(E2);
    BOOST_UNITS_CHECK_CLOSE(E5.value(), float(E_));

    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless,long>      E6(E2);
    BOOST_TEST(E6.value() == long(E_));

    // implicit operator= value_type conversion
    // narrowing conversion disallowed
//    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless,float>     E7 = E2;
//    BOOST_UNITS_CHECK_CLOSE(E7.value(),float(E_));

    // narrowing conversion disallowed
//    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless,long>      E8 = E2;
//    BOOST_TEST(E8.value() == long(E_));

    // const construction
    bu::quantity<bu::dimensionless>                 E9(E2);
    BOOST_TEST(E9.value() == E_);

//    // value assignment
//    E9.value() = 1.5*bu::dimensionless();
//    BOOST_TEST(E9.value() == 1.5);
//
//    // value assignment with implicit conversion
//    E9.value() = 1.5;
//    BOOST_TEST(E9.value() == 1.5);
//
//    // value assignment with implicit value_type conversion
//    E9.value() = 2*bu::dimensionless();
//    BOOST_TEST(E9.value() == double(2));
//
//    // value assignment with implicit value_type conversion
//    E9.value() = 2;
//    BOOST_TEST(E9.value() == double(2));

    // operator+=(this_type)
    E9 = 2.0;
    E9 += E9;
    BOOST_TEST(E9.value() == 4.0);

    // operator-=(this_type)
    E9 = 2.0;
    E9 -= E9;
    BOOST_TEST(E9.value() == 0.0);

    // operator*=(value_type)
    E9 = 2.0;
    E9 *= 2.0;
    BOOST_TEST(E9.value() == 4.0);

    // operator/=(value_type)
    E9 = 2.0;
    E9 /= 2.0;
    BOOST_TEST(E9.value() == 1.0);

    // static construct quantity from value_type
    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless>           E(bu::quantity<bu::dimensionless>::from_value(2.5));
    BOOST_TEST(E.value() == 2.5);

    // implicit conversion to value_type
    const double    V1(E9);
    BOOST_TEST(V1 == E9.value());

    const double    V2 = E9;
    BOOST_TEST(V2 == E9.value());

    // unit * scalar
    BOOST_TEST(bu::dimensionless()*2.0 == bu::quantity<bu::dimensionless>::from_value(2.0));

    // unit / scalar
    BOOST_TEST(bu::dimensionless()/2.0 == bu::quantity<bu::dimensionless>::from_value(0.5));

    // scalar * unit
    BOOST_TEST(2.0*bu::dimensionless() == bu::quantity<bu::dimensionless>::from_value(2.0));

    // scalar / unit
    BOOST_TEST(2.0/bu::dimensionless() == bu::quantity<bu::dimensionless>::from_value(2.0));

    //  quantity * scalar
    BOOST_TEST(E*2.0 == bu::quantity<bu::dimensionless>::from_value(5.0));

    //  quantity / scalar
    BOOST_TEST(E/2.0 == bu::quantity<bu::dimensionless>::from_value(1.25));

    // scalar * quantity
    BOOST_TEST(2.0*E == bu::quantity<bu::dimensionless>::from_value(5.0));

    // scalar / quantity
    BOOST_TEST(2.0/E == bu::quantity<bu::dimensionless>::from_value(0.8));

    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless>    D1(1.0),
                                                                D2(2.0);

    // unit * quantity
    BOOST_TEST(bu::dimensionless()*D1 == D1);

    // unit / quantity
    BOOST_TEST(bu::dimensionless()/D1 == D1);

    // quantity * unit
    BOOST_TEST(D1*bu::dimensionless() == D1);

    // quantity / unit
    BOOST_TEST(D1*bu::dimensionless() == D1);

    // +quantity
    BOOST_TEST(+D1 == 1.0*bu::dimensionless());

    // -quantity
    BOOST_TEST(-D1 == -1.0*bu::dimensionless());

    // quantity + quantity
    BOOST_TEST(D2+D1 == 3.0*bu::dimensionless());

    // quantity - quantity
    BOOST_TEST(D2-D1 == 1.0*bu::dimensionless());

    // quantity * quantity
    BOOST_TEST(D1*D2 == 2.0*bu::dimensionless());

    // quantity / quantity
    BOOST_TEST(D2/D1 == 2.0*bu::dimensionless());

    // integer power of quantity
    BOOST_TEST(2.0*bu::pow<2>(D2) == 2.0*std::pow(2.0,2.0)*bu::dimensionless());

    // rational power of quantity
    BOOST_TEST((2.0*bu::pow< bu::static_rational<2,3> >(D2) == 2.0*std::pow(2.0,2.0/3.0)*bu::dimensionless()));

    // integer root of quantity
    BOOST_TEST(2.0*bu::root<2>(D2) == 2.0*std::pow(2.0,1.0/2.0)*bu::dimensionless());

    // rational root of quantity
    BOOST_TEST((2.0*bu::root< bu::static_rational<3,2> >(D2) == 2.0*std::pow(2.0,2.0/3.0)*bu::dimensionless()));

    BOOST_CONSTEXPR_OR_CONST bu::quantity<bu::dimensionless>    A1(0.0),
                                                                A2(0.0),
                                                                A3(1.0),
                                                                A4(-1.0);

    // operator==
    BOOST_TEST((A1 == A2) == true);
    BOOST_TEST((A1 == A3) == false);

    // operator!=
    BOOST_TEST((A1 != A2) == false);
    BOOST_TEST((A1 != A3) == true);

    // operator<
    BOOST_TEST((A1 < A2) == false);
    BOOST_TEST((A1 < A3) == true);

    // operator<=
    BOOST_TEST((A1 <= A2) == true);
    BOOST_TEST((A1 <= A3) == true);

    // operator>
    BOOST_TEST((A1 > A2) == false);
    BOOST_TEST((A1 > A4) == true);

    // operator>=
    BOOST_TEST((A1 >= A2) == true);
    BOOST_TEST((A1 >= A4) == true);

    return boost::report_errors();
}


#include "space_age.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <cmath>

BOOST_AUTO_TEST_CASE(age_in_seconds)
{
    const space_age::space_age age(1000000);

    BOOST_REQUIRE_EQUAL(1000000, age.seconds());
}

#if defined(EXERCISM_RUN_ALL_TESTS)
namespace
{
const double accuracy = 0.005;
}

BOOST_AUTO_TEST_CASE(age_in_earth_years)
{
    const space_age::space_age age(1000000000);

    BOOST_REQUIRE_LT(std::abs(31.69 - age.on_earth()), accuracy);
}

BOOST_AUTO_TEST_CASE(age_in_mercury_years)
{
    const space_age::space_age age(2134835688);

    BOOST_REQUIRE_LT(std::abs(67.65 - age.on_earth()), accuracy);
    BOOST_REQUIRE_LT(std::abs(280.88 - age.on_mercury()), accuracy);
}

BOOST_AUTO_TEST_CASE(age_in_venus_years)
{
    const space_age::space_age age(189839836);

    BOOST_REQUIRE_LT(std::abs(6.02 - age.on_earth()), accuracy);
    BOOST_REQUIRE_LT(std::abs(9.78 - age.on_venus()), accuracy);
}

BOOST_AUTO_TEST_CASE(age_in_mars_years)
{
    const space_age::space_age age(2329871239);

    BOOST_REQUIRE_LT(std::abs(73.83 - age.on_earth()), accuracy);
    BOOST_REQUIRE_LT(std::abs(39.25 - age.on_mars()), accuracy);
}

BOOST_AUTO_TEST_CASE(age_in_jupiter_years)
{
    const space_age::space_age age(901876382);

    BOOST_REQUIRE_LT(std::abs(28.58 - age.on_earth()), accuracy);
    BOOST_REQUIRE_LT(std::abs(2.41 - age.on_jupiter()), accuracy);
}

BOOST_AUTO_TEST_CASE(age_in_saturn_years)
{
    const space_age::space_age age(3000000000);

    BOOST_REQUIRE_LT(std::abs(95.06 - age.on_earth()), accuracy);
    BOOST_REQUIRE_LT(std::abs(3.23 - age.on_saturn()), accuracy);
}

BOOST_AUTO_TEST_CASE(age_in_uranus_years)
{
    const space_age::space_age age(3210123456);

    BOOST_REQUIRE_LT(std::abs(101.72 - age.on_earth()), accuracy);
    BOOST_REQUIRE_LT(std::abs(1.21 - age.on_uranus()), accuracy);
}

BOOST_AUTO_TEST_CASE(age_in_neptune_year)
{
    const space_age::space_age age(8210123456);

    BOOST_REQUIRE_LT(std::abs(260.16 - age.on_earth()), accuracy);
    BOOST_REQUIRE_LT(std::abs(1.58 - age.on_neptune()), accuracy);
}
#endif


/*
Copyright 2012-2014 Rogier van Dalen.

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
Tests for arithmetic_magma on reals.
*/

#ifndef MATH_TEST_ARITHMETIC_MAGMA_TESTS_REAL_HPP_INCLUDED
#define MATH_TEST_ARITHMETIC_MAGMA_TESTS_REAL_HPP_INCLUDED

#include "math/arithmetic_magma.hpp"

#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/special_functions/next.hpp>

#include <boost/mpl/assert.hpp>

#include "range/std/container.hpp"

#include "math/check/check_magma.hpp"

/* Produce example values. */

template <class Type> std::vector <Type> get_unsigned_real_examples() {
    std::vector <Type> examples;
    examples.push_back (Type (0));
    examples.push_back (Type (0.2));
    examples.push_back (Type (0.5));
    examples.push_back (Type (1.));
    examples.push_back (Type (2.3));
    examples.push_back (Type (5.));
    return std::move (examples);
}

template <class Type> std::vector <Type> get_signed_real_examples() {
    std::vector <Type> examples = get_unsigned_real_examples <Type>();
    // Different from the positive examples so that signed_log_float does not
    // end up in problems.
    examples.push_back (Type (-5.11));
    examples.push_back (Type (-2.27));
    examples.push_back (Type (-1.25));
    examples.push_back (Type (-0.68));
    examples.push_back (Type (-0.23));
    return std::move (examples);
}

template <class Type> std::vector <Type> get_signed_float_examples() {
    std::vector <Type> examples = get_signed_real_examples <Type>();
    // Find a value whose inversion is approximate, i.e. (1/value) * value != 1.
    Type candidate = Type (3);
    Type inverse;
    do {
        candidate = boost::math::float_next (candidate);
        inverse = Type (1) / candidate;
    } while (candidate * inverse == Type (1));

    BOOST_CHECK (!(
        math::invert <math::callable::times> (candidate) * candidate
        == Type (1)));

    examples.push_back (candidate);

    return std::move (examples);
}

/* Actual tests. */

template <class Type, class Examples>
    void test_arithmetic_magma_real (Examples const & examples)
{
    Type a (3);
    Type b (5);
    Type c (2.5);

    BOOST_MPL_ASSERT ((
        math::has <math::callable::non_member <Type const &>()>));
    BOOST_MPL_ASSERT ((math::has <math::callable::is_member (Type const)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::equal (Type &, Type const)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::approximately_equal (Type, Type const &)>));

    // compare.
    BOOST_MPL_ASSERT ((math::has <math::callable::compare (Type, Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::approximate <
        math::callable::compare (Type, Type)>));
    BOOST_CHECK_EQUAL (math::compare (a, a), false);
    BOOST_CHECK_EQUAL (math::compare (a, b), true);
    BOOST_CHECK_EQUAL (math::compare (a, c), false);
    BOOST_CHECK_EQUAL (math::compare (b, a), false);
    BOOST_CHECK_EQUAL (math::compare (b, b), false);
    BOOST_CHECK_EQUAL (math::compare (b, c), false);
    BOOST_CHECK_EQUAL (math::compare (c, a), true);
    BOOST_CHECK_EQUAL (math::compare (c, b), true);
    BOOST_CHECK_EQUAL (math::compare (c, c), false);

    // times.
    BOOST_MPL_ASSERT ((math::has <
        math::callable::times (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::times (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::identity <Type &, math::callable::times>()>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::identity <Type &> (math::callable::times)>));
    BOOST_MPL_ASSERT ((math::has <math::callable::one <Type const &>()>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::annihilator <Type &, math::callable::times> ()>));

    BOOST_CHECK_EQUAL (math::one <Type>(), Type (1));
    BOOST_CHECK_EQUAL ((math::identity <Type, math::callable::times>()),
        Type (1));
    BOOST_CHECK_EQUAL (math::identity <Type> (math::times), Type (1));
    BOOST_CHECK_EQUAL ((math::annihilator <Type, math::callable::times>()),
        Type (0));
    BOOST_CHECK_EQUAL (math::annihilator <Type> (math::times), Type (0));

    BOOST_MPL_ASSERT ((math::is::commutative <math::callable::times, Type &>));
    BOOST_MPL_ASSERT ((
        math::is::associative <math::callable::times, Type const &>));
    BOOST_MPL_ASSERT_NOT ((
        math::is::idempotent <math::callable::times, Type>));
    BOOST_MPL_ASSERT_NOT ((
        math::is::path_operation <math::callable::times, Type>));

    // plus.
    BOOST_MPL_ASSERT ((math::has <
        math::callable::plus (Type const &, Type &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::plus (Type const &, Type &)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::identity <Type &, math::callable::plus>()>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::identity <Type &> (math::callable::plus)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::zero <Type const>()>));
    BOOST_MPL_ASSERT_NOT ((math::has <
        math::callable::annihilator <Type &, math::callable::plus> ()>));

    BOOST_CHECK_EQUAL (math::zero <Type>(), Type (0));
    BOOST_CHECK_EQUAL (
        (math::identity <Type, math::callable::plus>()), Type (0));
    BOOST_CHECK_EQUAL (math::identity <Type> (math::plus), Type (0));

    BOOST_MPL_ASSERT ((math::is::associative <math::callable::plus, Type &>));
    BOOST_MPL_ASSERT ((math::is::commutative <math::callable::plus, Type &>));
    BOOST_MPL_ASSERT_NOT ((
        math::is::path_operation <math::callable::plus, Type &>));
    BOOST_MPL_ASSERT_NOT ((
        math::is::idempotent <math::callable::plus, Type &>));

    // divide.
    BOOST_MPL_ASSERT ((math::has <
        math::callable::divide<> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::divide <math::left> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::divide <math::right> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::divide<> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::divide <math::left> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::divide <math::right> (Type &, Type const &)>));

    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::left, math::callable::times> (Type)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::right, math::callable::times> (Type)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::either, math::callable::times> (Type)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::callable::times> (Type)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert<> (math::callable::times, Type)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::invert <math::callable::times> (Type)>));

    // reverse.
    BOOST_MPL_ASSERT ((
        math::has <math::callable::reverse <math::callable::plus> (Type)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::reverse<> (math::callable::plus, Type)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::reverse <math::callable::times> (Type)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::reverse<> (math::callable::times, Type)>));

    // Check for sensible results.
    {
        Type result = math::times (a, b);
        BOOST_CHECK_EQUAL (result, Type (15));
    }
    {
        Type result = math::plus (a, b);
        BOOST_CHECK_EQUAL (result, Type (8));
    }
    // divide.
    {
        // The result of this is 0.5, but it is not exact for log_float.
        Type result = math::divide (c, b);
        BOOST_CHECK_EQUAL (result, Type (2.5) / Type (5));
        result = math::divide <math::left> (c, b);
        BOOST_CHECK_EQUAL (result, Type (2.5) / Type (5));
        result = math::divide <math::right> (c, b);
        BOOST_CHECK_EQUAL (result, Type (2.5) / Type (5));
    }
    // invert.
    {
        Type result = math::invert <math::left, math::callable::times> (a);
        BOOST_CHECK_EQUAL (result, Type (1)/a);
        result = math::invert <math::right, math::callable::times> (b);
        BOOST_CHECK_EQUAL (result, Type (1)/b);
        result = math::invert <math::either, math::callable::times> (c);
        BOOST_CHECK_EQUAL (result, Type (1)/c);
        result = math::invert <math::callable::times> (c);
        BOOST_CHECK_EQUAL (result, Type (1)/c);

        result = math::invert <math::left> (math::times, a);
        BOOST_CHECK_EQUAL (result, Type (1)/a);
        result = math::invert <math::right> (math::times, b);
        BOOST_CHECK_EQUAL (result, Type (1)/b);
        result = math::invert <math::either> (math::times, c);
        BOOST_CHECK_EQUAL (result, Type (1)/c);
        result = math::invert (math::times, c);
        BOOST_CHECK_EQUAL (result, Type (1)/c);
    }
    // reverse.
    {
        BOOST_CHECK_EQUAL (math::reverse <math::callable::plus> (a), a);
        BOOST_CHECK_EQUAL (math::reverse (math::plus, b), b);
        BOOST_CHECK_EQUAL (math::reverse <math::callable::times> (a), a);
        BOOST_CHECK_EQUAL (math::reverse (math::times, b), b);
    }

    // inverse_operation on times.
    {
        // The result of this is 0.5, but it is not exact for log_float.
        Type result = math::inverse_operation (math::times) (c, b);
        BOOST_CHECK_EQUAL (result, Type (2.5) / Type (5));
        result = math::inverse_operation <math::left, math::callable::times>()
            (c, b);
        BOOST_CHECK_EQUAL (result, Type (2.5) / Type (5));
        result = math::inverse_operation <math::right> (math::times) (c, b);
        BOOST_CHECK_EQUAL (result, Type (2.5) / Type (5));
    }

    // Check for consistency.
    math::check_equal_on (examples);

    math::check_semiring <Type, math::either> (
        math::times, math::plus, examples);
}

template <class Type> void test_arithmetic_magma_real_signed() {
    Type a = 3;
    Type b = 5;

    // minus.
    BOOST_MPL_ASSERT ((math::has <
        math::callable::minus<> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::minus <math::left> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::minus <math::right> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::minus<> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::minus <math::left> (Type &, Type const &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::minus <math::right> (Type &, Type const &)>));

    // invert.
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::left, math::callable::plus> (Type)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::right, math::callable::plus> (Type)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::either, math::callable::plus> (Type)>));
    BOOST_MPL_ASSERT ((math::has <
        math::callable::invert <math::callable::plus> (Type)>));
    BOOST_MPL_ASSERT_NOT ((math::is::approximate <
        math::callable::invert <math::callable::plus> (Type)>));

    // minus.
    {
        Type result = math::minus (a, b);
        BOOST_CHECK_EQUAL (result, - (Type (5) - Type (3)));
        result = math::minus <math::left> (a, b);
        BOOST_CHECK_EQUAL (result, - (Type (5) - Type (3)));
        result = math::minus <math::right> (a, b);
        BOOST_CHECK_EQUAL (result, - (Type (5) - Type (3)));
    }

    // inverse_operation on plus.
    {
        Type result = math::inverse_operation (math::plus) (a, b);
        BOOST_CHECK_EQUAL (result, - (Type (5) - Type (3)));
        result = math::inverse_operation <math::left> (math::plus) (a, b);
        BOOST_CHECK_EQUAL (result, - (Type (5) - Type (3)));
        result = math::inverse_operation <math::right, math::callable::plus>()
            (a, b);
        BOOST_CHECK_EQUAL (result, - (Type (5) - Type (3)));
    }
}

#endif // MATH_TEST_ARITHMETIC_MAGMA_TESTS_REAL_HPP_INCLUDED


//=======================================================================
// Copyright (c)
//
// 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 steiner_tree_greedy_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-20
 */

#include "test_utils/logger.hpp"
#include "test_utils/sample_graph.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/greedy/steiner_tree_greedy.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/property_map/function_property_map.hpp>

#include <vector>
#include <map>

BOOST_AUTO_TEST_SUITE(steiner_tree_greedy)
typedef sample_graphs_metrics SGM;

template <typename Graph, typename Tree> int get_sum(Graph g, Tree &edges) {
    auto weight = get(boost::edge_weight, g);
    auto sum = 0;
    for (auto e : edges) {
        sum += get(weight, e);
    }
    return sum;
}

template <typename Graph> int test(Graph g) {
    typedef typename boost::graph_traits<decltype(g)>::edge_descriptor Edge;
    std::vector<Edge> steiner_edges;
    auto result = paal::steiner_tree_greedy(
        g, std::inserter(steiner_edges, steiner_edges.begin()));
    auto result_check = get_sum(g, steiner_edges);
    BOOST_CHECK_EQUAL(result_check,result.first);
    return result.first;
}
BOOST_AUTO_TEST_CASE(steiner_tree_test) {
    const int OPTIMAL = 4;
    const int APPROXIMATION_RATIO = 2;
    LOGLN("steiner_tree_multi_edges");
    check_result(test(SGM::get_graph_steiner()), OPTIMAL, APPROXIMATION_RATIO);
}

BOOST_AUTO_TEST_CASE(not_conected_terminals) {
    LOGLN("not conected teerminals");
    BOOST_CHECK_EQUAL(test(SGM::two_points_steiner()), 0);
}

BOOST_AUTO_TEST_CASE(steiner_tree_multi_edges_test) {
    LOGLN("steiner_tree_multi_edges");
    BOOST_CHECK_EQUAL(test(SGM::get_graph_steiner_multi_edges()), 23);
}

BOOST_AUTO_TEST_CASE(steiner_tree_test_cycle) {
    LOGLN("steiner_tree_test_cycle");
    BOOST_CHECK_EQUAL(test(SGM::get_graph_stainer_tree_cycle()), 10);
}

BOOST_AUTO_TEST_CASE(steiner_tree_test_no_weight_in_graph) {
    LOGLN("no weights in graph");
    typedef sample_graphs_metrics SGM;
    auto g = SGM::get_graph_steiner_edge();
    typedef boost::graph_traits<decltype(g)>::edge_descriptor Edge;
    std::vector<Edge> steiner_edges;
    auto color = get(boost::vertex_color, g);
    std::map<Edge, int> edge_weight;
    boost::associative_property_map<std::map<Edge, int>>
    edge_weight_property_map(edge_weight);
    for (auto w : boost::make_iterator_range(edges(g))) {
        edge_weight_property_map[w] = 1;
    }
    paal::steiner_tree_greedy(
        g, std::inserter(steiner_edges, steiner_edges.begin()),
        edge_weight_property_map, color);
}

BOOST_AUTO_TEST_CASE(steiner_tree_test_Parameters) {
    const int OPTIMAL = 4;
    const int APPROXIMATION_RATIO = 2;
    LOGLN("steiner_tree_test_Parameters");
    typedef sample_graphs_metrics SGM;
    auto g = SGM::get_graph_steiner();
    typedef boost::graph_traits<decltype(g)>::edge_descriptor Edge;
    std::vector<Edge> steiner_edges;
    std::vector<int> color(num_vertices(g));
    {
        auto c = &color[0];
        put(c, SGM::A, paal::Terminals::TERMINAL);
        put(c, SGM::B, paal::Terminals::TERMINAL);
        put(c, SGM::C, paal::Terminals::TERMINAL);
        put(c, SGM::D, paal::Terminals::TERMINAL);
        put(c, SGM::E, paal::Terminals::NONTERMINAL);
    }
    auto index = get(boost::vertex_index, g);
    auto colorPMap = boost::make_iterator_property_map(color.begin(), index);

    paal::steiner_tree_greedy(
        g, std::inserter(steiner_edges, steiner_edges.begin()),
        boost::vertex_color_map(colorPMap));
    BOOST_CHECK(std::size_t(3) <= steiner_edges.size() &&
                steiner_edges.size() <= std::size_t(4));
    check_result(get_sum(g, steiner_edges), OPTIMAL, APPROXIMATION_RATIO);
}

BOOST_AUTO_TEST_SUITE_END()


#include <thread>
#include <unordered_map>
#include "kvsimple.hpp"
#include <random>
#include <format>
#include <boost/algorithm/string.hpp>
#include <fmt/core.h>

int gen_num(int a, int b)
{
	std::random_device rd;  //Will be used to obtain a seed for the random number engine
	std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
	std::uniform_int_distribution<> distrib(a, b);
	return distrib(gen);
}

class state_manager
{
private:
	std::unordered_map<std::string, kvsimple> mp;
	zmqpp::context_t& ctx_;
	std::unique_ptr<zmqpp::actor> actor_;

public:
	state_manager(zmqpp::context_t& ctx) :
		mp{},
		ctx_{ctx}
	{
		actor_ = std::make_unique<zmqpp::actor>([this](zmqpp::socket_t* pipe) -> bool {
			return run(pipe);
			});
	}

	zmqpp::socket_t* get_pipe() { return actor_->pipe(); }

private:
	bool run(zmqpp::socket_t* pipe)
	{
		using namespace std::string_literals;
		pipe->send(zmqpp::signal::ok);

		zmqpp::socket_t snapshot(ctx_, zmqpp::socket_type::router);
		snapshot.bind("tcp://*:5556");
		fmt::print("Start listening at port 5556\n");

		zmqpp::poller_t poller;
		poller.add(*pipe);
		poller.add(snapshot);

		int state_seq{};
		while (true) {
			poller.poll(1000);

			// apply state updates from main thread
			if (poller.events(*pipe) == zmqpp::poller_t::poll_in) {
				kvsimple kvmsg = kvsimple::recv(*pipe);
				this->mp.insert_or_assign(kvmsg.key_, kvmsg);
				state_seq = kvmsg.sequence_;
			}

			// execute state snapshot request
			if (poller.events(snapshot) == zmqpp::poller_t::poll_in) {
				zmqpp::message_t msg;
				snapshot.receive(msg);

				std::string identity;
				msg >> identity;

				std::string req;
				msg >> req;

				if (!boost::iequals(req, "ICANHAZ?"s)) {
					fmt::print("E: bad request, aborting\n");
					break;
				}

				for (auto&& e : mp) {
					fmt::print("sending message: {} {} {}\n", e.second.key_, e.second.sequence_, e.second.body_);
					e.second.send(snapshot, identity);
				}

				// now send end message with get_sequence number
				fmt::print("Sending state snapshot = {}\n", state_seq);
				kvsimple msgstate("KTHXBAI", state_seq, "");
				msgstate.send(snapshot, identity);
			}
		}
		return true;
	}
};

int main()
{
	zmqpp::context_t ctx;
	zmqpp::socket_t publisher(ctx, zmqpp::socket_type::pub);
	publisher.bind("tcp://*:5557");

	state_manager smgr(ctx);
	int seq{};
	fmt::print("Start listening at port 5557\n");
	while (1) {
		int curseq = ++seq;
		auto key = std::format("{}", gen_num(0, 10000));
		auto body = std::format("{}", gen_num(0, 1000000));

		kvsimple kvmsg(key, curseq, body);
		kvmsg.send(publisher);
		kvmsg.send(*smgr.get_pipe());
		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
	}

	return 0;
}

zeromq/5-Advanced Pub-Sub Patterns/5.4-Reliable Pub-Sub (Clone Pattern)/clonesrv2.cpp


#include <limits>
#include <string>
#include <utility>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/optional.hpp>
#include <geometry_msgs/Point.h>
#include <grid_map_msgs/GridMap.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <nav_msgs/Path.h>
#include <visualization_msgs/MarkerArray.h>

#include "costmap_generator/collision_checker.h"
#include "f1tenth_utils/math.h"
#include "f1tenth_utils/tf2_wrapper.h"
#include "local_planner/lattice.h"

/* -------------------------------------------------------------------------- */
/*                              Lattice Position                              */
/* -------------------------------------------------------------------------- */

Lattice::Position::Position()
{
  layer_ = 0;
  lateral_position_ = 0;
}

Lattice::Position::Position(int layer, int lateral_position)
{
  layer_ = layer;
  lateral_position_ = lateral_position;
}

bool Lattice::Position::operator==(const Position& other) const
{
  return (layer_ == other.layer_ && lateral_position_ == other.lateral_position_);
}

/* -------------------------------------------------------------------------- */
/*                                   Vertex                                   */
/* -------------------------------------------------------------------------- */

Lattice::Vertex::Vertex()
{
}

Lattice::Vertex::Vertex(const double x, const double y, const double yaw, const double lateral_offset)
{
  x_ = x;
  y_ = y;
  yaw_ = yaw;
  lateral_offset_ = lateral_offset;
}

bool Lattice::Vertex::isOnSameSide(const Lattice::Vertex& v) const
{
  return lateral_offset_ * v.lateral_offset_ >= 0;
}

double Lattice::Vertex::distanceTo(const Vertex& v) const
{
  return calculateDistance(x_, y_, v.x_, v.y_);
}

geometry_msgs::Point Lattice::Vertex::getPoint() const
{
  geometry_msgs::Point point;
  point.x = x_;
  point.y = y_;

  return point;
}

geometry_msgs::Pose Lattice::Vertex::getPose() const
{
  geometry_msgs::Pose pose;
  pose.position = getPoint();
  pose.orientation = TF2Wrapper::quatFromYaw(yaw_);

  return pose;
}

/* -------------------------------------------------------------------------- */
/*                                    Edge                                    */
/* -------------------------------------------------------------------------- */

Lattice::Edge::Edge()
{
}

Lattice::Edge::Edge(const double weight)
{
  weight_ = weight;
}

/* -------------------------------------------------------------------------- */
/*                              Lattice Generator                             */
/* -------------------------------------------------------------------------- */

Lattice::Generator::Generator(const int num_layers, const double layer_spacing, const int num_lateral_samples_per_side,
                              const double lateral_spacing, const double max_curvature, const double k_movement,
                              const std::shared_ptr<CollisionChecker>& collision_checker_ptr)
{
  setNumLayers(num_layers);
  setNumLateralSamplesPerSide(num_lateral_samples_per_side);
  setLayerSpacing(layer_spacing);
  setLateralSpacing(lateral_spacing);
  setMaxCurvature(max_curvature);
  setMovementWeight(k_movement);

  if (collision_checker_ptr == nullptr)
  {
    throw std::invalid_argument("Collision checker cannot be nullptr");
  }

  collision_checker_ptr_ = collision_checker_ptr;
}

Lattice Lattice::Generator::generate(const geometry_msgs::Pose& source_pose) const
{
  if (global_path_ == nullptr || global_path_->poses.empty())
  {
    throw std::runtime_error("Global path has not been set");
  }

  std::vector<geometry_msgs::Pose> reference_poses = getReferencePoses(source_pose);
  PositionMap map(reference_poses.size());

  Graph graph;
  double source_offset = TF2Wrapper::doTransform<geometry_msgs::Pose>(source_pose, reference_poses.at(0)).position.y;
  Vertex source_vertex(source_pose.position.x, source_pose.position.y, TF2Wrapper::yawFromQuat(source_pose.orientation),
                       source_offset);
  VertexDescriptor source_id = boost::add_vertex(source_vertex, graph);
  map.at(0).insert({ 0, source_id });

  for (int i = 1; i < reference_poses.size(); ++i)
  {
    geometry_msgs::Pose reference_pose = reference_poses.at(i);

    for (int j = -num_lateral_samples_per_side_; j <= num_lateral_samples_per_side_; ++j)
    {
      Vertex v = generateVertex(reference_pose, i, j);
      VertexDescriptor id = boost::add_vertex(v, graph);
      map.at(i).insert({ j, id });

      for (const auto& pair : map.at(i - 1))
      {
        auto prev_id = pair.second;
        Vertex u = graph[prev_id];

        if (calculateCurvature(u.getPose(), v.getPose().position) < max_curvature_ && !checkCollision(u, v))
        {
          double lateral_movement = v.lateral_offset_ - u.lateral_offset_;

          double cumulative_offset;

          if (u.isOnSameSide(v))
          {
            cumulative_offset = std::abs(0.5 * (u.lateral_offset_ + v.lateral_offset_));
          }
          else
          {
            double intercept = -u.lateral_offset_ / (v.lateral_offset_ - u.lateral_offset_);
            cumulative_offset =
                0.5 * (intercept * std::abs(u.lateral_offset_) + (1 - intercept) * std::abs(v.lateral_offset_));
          }

          double weight = k_movement_ * pow(lateral_movement, 2) + (1 - k_movement_) * cumulative_offset;

          if (!v.isOnSameSide(source_vertex))
          {
            weight *= 1.5;  // temp value
          }

          Edge e(weight);
          boost::add_edge(prev_id, id, e, graph);
        }
      }
    }
  }

  return Lattice(graph, map, source_id, num_layers_, 2 * num_lateral_samples_per_side_ + 1);
}

int Lattice::Generator::getNearestWaypointId(const geometry_msgs::Pose& current_pose) const
{
  int nearest_wp_id = -1;
  double nearest_wp_dist = std::numeric_limits<double>::max();

  for (int i = 0; i < global_path_->poses.size(); ++i)
  {
    double dist =
        calculateDistance(current_pose.position.x, current_pose.position.y, global_path_->poses.at(i).pose.position.x,
                          global_path_->poses.at(i).pose.position.y);

    if (dist < nearest_wp_dist)
    {
      nearest_wp_id = i;
      nearest_wp_dist = dist;
    }
  }

  return nearest_wp_id;
}

std::vector<geometry_msgs::Pose> Lattice::Generator::getReferencePoses(const geometry_msgs::Pose& source_pose) const
{
  bool is_loop = global_path_->poses.at(global_path_->poses.size() - 1) == global_path_->poses.at(0);
  int cur_id = getNearestWaypointId(source_pose);
  geometry_msgs::Pose nearest_pose = global_path_->poses.at(cur_id).pose;
  std::vector<geometry_msgs::Pose> poses = { nearest_pose };

  geometry_msgs::Point prev_pos = nearest_pose.position;

  for (int i = 0; i < num_layers_ - 1; ++i)
  {
    double cur_dist = 0.0;

    while (cur_dist < layer_spacing_)
    {
      if (cur_id == global_path_->poses.size())
      {
        if (is_loop)
        {
          cur_id = 0;
        }
        else
        {
          poses.push_back(global_path_->poses.at(global_path_->poses.size() - 1).pose);
          return poses;
        }
      }

      geometry_msgs::Point cur_pos = global_path_->poses.at(cur_id).pose.position;
      cur_dist += calculateDistance(cur_pos.x, cur_pos.y, prev_pos.x, prev_pos.y);
      prev_pos = cur_pos;
      ++cur_id;
    }

    poses.push_back(global_path_->poses.at(cur_id - 1).pose);
  }

  return poses;
}

Lattice::Vertex Lattice::Generator::generateSourceVertex(const geometry_msgs::Pose source_pose,
                                                         const geometry_msgs::Pose reference_pose) const
{
  geometry_msgs::Pose relative_pose = TF2Wrapper::doTransform<geometry_msgs::Pose>(source_pose, reference_pose);

  return Vertex(source_pose.position.x, source_pose.position.y, TF2Wrapper::yawFromQuat(source_pose.orientation),
                relative_pose.position.y);
}

Lattice::Vertex Lattice::Generator::generateVertex(const geometry_msgs::Pose& reference_pose, const int layer,
                                                   const int lateral_pos) const
{
  double yaw = TF2Wrapper::yawFromQuat(reference_pose.orientation);
  double offset = lateral_pos * lateral_spacing_;
  double x = reference_pose.position.x + offset * cos(yaw + M_PI_2);
  double y = reference_pose.position.y + offset * sin(yaw + M_PI_2);

  return Vertex(x, y, TF2Wrapper::yawFromQuat(reference_pose.orientation), offset);
}

bool Lattice::Generator::checkCollision(const Lattice::Vertex& source, const Lattice::Vertex& target) const
{
  geometry_msgs::PointStamped source_point;
  geometry_msgs::PointStamped target_point;

  source_point.header.frame_id = global_path_->header.frame_id;
  source_point.point.x = source.x_;
  source_point.point.y = source.y_;

  target_point.header.frame_id = global_path_->header.frame_id;
  target_point.point.x = target.x_;
  target_point.point.y = target.y_;

  return collision_checker_ptr_->checkCollision(source_point, target_point);
}

void Lattice::Generator::setGlobalPath(const nav_msgs::PathConstPtr& path)
{
  global_path_ = path;
}

void Lattice::Generator::setNumLayers(const int num_layers)
{
  if (num_layers < 4)
  {
    throw std::invalid_argument("Number of layers must be at least 4");
  }

  layer_spacing_ = num_layers_ = num_layers;
}

void Lattice::Generator::setNumLateralSamplesPerSide(const int num_lateral_samples_per_side)
{
  if (num_lateral_samples_per_side < 0)
  {
    throw std::invalid_argument("Number of lateral samples per side must be non-negative");
  }

  num_lateral_samples_per_side_ = num_lateral_samples_per_side;
}

void Lattice::Generator::setLayerSpacing(const double layer_spacing)
{
  if (layer_spacing <= 0.0)
  {
    throw std::invalid_argument("Layer spacing must be positive");
  }

  layer_spacing_ = layer_spacing;
}

void Lattice::Generator::setLateralSpacing(const double lateral_spacing)
{
  if (lateral_spacing <= 0.0)
  {
    throw std::invalid_argument("Lateral spacing must be positive");
  }

  lateral_spacing_ = lateral_spacing;
}

void Lattice::Generator::setMaxCurvature(const double curvature)
{
  if (curvature <= 0.0)
  {
    throw std::invalid_argument("Maximum curvature must be positive");
  }

  max_curvature_ = curvature;
}

void Lattice::Generator::setMovementWeight(const double weight)
{
  if (weight < 0.0 || weight > 1.0)
  {
    throw std::invalid_argument("Weight must be between 0 and 1");
  }

  k_movement_ = weight;
}

/* -------------------------------------------------------------------------- */
/*                                   Lattice                                  */
/* -------------------------------------------------------------------------- */

Lattice::Lattice(const Graph& graph, const PositionMap& position_map, const VertexDescriptor source_id,
                 const int num_layers, const int num_lateral_samples)
{
  graph_ = graph;
  position_map_ = position_map;
  source_id_ = source_id;
  num_layers_ = num_layers;
  num_lateral_samples_ = num_lateral_samples;
}

void Lattice::computeShortestPaths()
{
  distances_.reserve(boost::num_vertices(graph_));
  predecessors_.reserve(boost::num_vertices(graph_));

  boost::dijkstra_shortest_paths(
      graph_, source_id_,
      boost::weight_map(boost::get(&Edge::weight_, graph_))
          .distance_map(boost::make_iterator_property_map(distances_.begin(), boost::get(boost::vertex_index, graph_)))
          .predecessor_map(
              boost::make_iterator_property_map(predecessors_.begin(), boost::get(boost::vertex_index, graph_))));
}

std::vector<Lattice::Vertex> Lattice::getVertices() const
{
  std::vector<Vertex> vertices;

  auto vs = boost::vertices(graph_);
  for (auto vit = vs.first; vit != vs.second; ++vit)
  {
    Vertex v = graph_[*vit];
    vertices.push_back(v);
  }

  return vertices;
}

boost::optional<std::pair<std::vector<geometry_msgs::Point>, double>> Lattice::getShortestPath(const int layer,
                                                                                const int offset) const
{
  VertexDescriptor goal_id = getVertexIdFromPosition({ layer, offset });

  if (predecessors_[goal_id] == goal_id)
  {
    return boost::none;
  }

  std::vector<geometry_msgs::Point> path;

  for (VertexDescriptor current = goal_id; current != predecessors_[current]; current = predecessors_[current])
  {
    Vertex v = graph_[current];
    geometry_msgs::Point point;
    point.x = v.x_;
    point.y = v.y_;
    path.push_back(point);
  }
  Vertex v = graph_[source_id_];
  geometry_msgs::Point point;
  point.x = v.x_;
  point.y = v.y_;
  path.push_back(point);

  std::reverse(path.begin(), path.end());

  return std::make_pair(path, distances_[goal_id]);
}

Lattice::VertexDescriptor Lattice::getVertexIdFromPosition(const Position& pos) const
{
  if (pos.layer_ >= position_map_.size())
  {
    throw std::invalid_argument("Vertex position does not exist in lattice");
  }

  auto it = position_map_.at(pos.layer_).find(pos.lateral_position_);

  if (it == position_map_.at(pos.layer_).end())
  {
    throw std::invalid_argument("Vertex position does not exist in lattice");
  }

  return it->second;
}

int Lattice::getNumLayers() const
{
  return num_layers_;
}

int Lattice::getNumLateralSamples() const
{
  return num_lateral_samples_;
}

visualization_msgs::Marker Lattice::generateVertexMarker(const int marker_id, const std::string& ns, const double scale,
                                                         const double r, const double g, const double b,
                                                         const double a) const
{
  visualization_msgs::Marker vertex_marker;
  vertex_marker.header.frame_id = "map";
  vertex_marker.action = visualization_msgs::Marker::ADD;
  vertex_marker.type = visualization_msgs::Marker::SPHERE_LIST;
  vertex_marker.ns = ns;
  vertex_marker.id = marker_id;
  vertex_marker.lifetime = ros::Duration(0.1);
  vertex_marker.scale.x = scale;
  vertex_marker.scale.y = scale;
  vertex_marker.scale.z = scale;
  vertex_marker.color.r = r;
  vertex_marker.color.g = g;
  vertex_marker.color.b = b;
  vertex_marker.color.a = a;

  auto vertices = boost::vertices(graph_);

  for (auto it = vertices.first; it != vertices.second; ++it)
  {
    vertex_marker.points.push_back(graph_[*it].getPoint());
  }

  return vertex_marker;
}

visualization_msgs::Marker Lattice::generateEdgeMarker(const int marker_id, const std::string& ns, const double scale,
                                                       const double r, const double g, const double b,
                                                       const double a) const
{
  visualization_msgs::Marker edge_marker;
  edge_marker.header.frame_id = "map";
  edge_marker.action = visualization_msgs::Marker::ADD;
  edge_marker.type = visualization_msgs::Marker::LINE_LIST;
  edge_marker.ns = ns;
  edge_marker.id = marker_id;
  edge_marker.lifetime = ros::Duration(0.1);
  edge_marker.scale.x = scale;
  edge_marker.color.r = r;
  edge_marker.color.g = g;
  edge_marker.color.b = b;
  edge_marker.color.a = a;

  auto edges = boost::edges(graph_);

  for (auto it = edges.first; it != edges.second; ++it)
  {
    edge_marker.points.push_back(graph_[boost::source(*it, graph_)].getPoint());
    edge_marker.points.push_back(graph_[boost::target(*it, graph_)].getPoint());
  }

  return edge_marker;
}

visualization_msgs::MarkerArray Lattice::generateWeightMarkers(int marker_id, const std::string& ns, const double scale,
                                                               const double r, const double g, const double b,
                                                               const double a) const
{
  visualization_msgs::MarkerArray weight_markers;
  auto edges = boost::edges(graph_);

  for (auto it = edges.first; it != edges.second; ++it)
  {
    visualization_msgs::Marker text_marker;

    text_marker.header.frame_id = "map";
    text_marker.action = visualization_msgs::Marker::ADD;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.ns = ns;
    text_marker.id = marker_id++;
    text_marker.lifetime = ros::Duration(0.1);
    text_marker.scale.z = scale;
    text_marker.color.r = r;
    text_marker.color.g = g;
    text_marker.color.b = b;
    text_marker.color.a = a;

    std::ostringstream oss;
    oss << std::fixed << std::setprecision(2) << graph_[*it].weight_;
    text_marker.text = oss.str();

    auto u = graph_[boost::source(*it, graph_)];
    auto v = graph_[boost::target(*it, graph_)];

    geometry_msgs::Point text_point;
    text_point.x = u.x_ + (v.x_ - u.x_) / 8;
    text_point.y = u.y_ + (v.y_ - u.y_) / 8;
    text_point.z = 0.1;

    text_marker.pose.position = text_point;
    weight_markers.markers.push_back(text_marker);
  }

  return weight_markers;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ZK_BLUEPRINT_VERIFY_R1CS_SCHEME_COMPONENT_TEST_HPP
#define CRYPTO3_ZK_BLUEPRINT_VERIFY_R1CS_SCHEME_COMPONENT_TEST_HPP

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>

#include <nil/crypto3/algebra/curves/edwards.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::zk;
using namespace nil::crypto3::algebra;

template<typename CurveType,
         typename SchemeType = snark::r1cs_gg_ppzksnark<CurveType>>
bool verify_component(components::blueprint<typename CurveType::scalar_field_type> bp){

    if (bp.num_variables() == 0x00){
        std::cout << "Empty blueprint!" << std::endl;
        return false;
    }

    using field_type = typename CurveType::scalar_field_type;
    using scheme_type = SchemeType;

    const snark::r1cs_constraint_system<field_type> constraint_system = bp.get_constraint_system();

    auto begin = std::chrono::high_resolution_clock::now();
    const typename scheme_type::keypair_type keypair = snark::generate<scheme_type>(constraint_system);
    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "Key generation finished, time: " << elapsed.count() * 1e-9 << std::endl;

    begin = std::chrono::high_resolution_clock::now();
    const typename scheme_type::proof_type proof = snark::prove<scheme_type>(keypair.first, bp.primary_input(), bp.auxiliary_input());
    end = std::chrono::high_resolution_clock::now();
    elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "Proving finished, time: " << elapsed.count() * 1e-9 << std::endl;

    begin = std::chrono::high_resolution_clock::now();
    bool verified = snark::verify<scheme_type>(keypair.second, bp.primary_input(), proof);
    end = std::chrono::high_resolution_clock::now();
    elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);

    std::cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << std::endl;
    std::cout << "Verification finished, time: " << elapsed.count() * 1e-9 << std::endl;
    std::cout << "Verification status: " << verified << std::endl;

    return verified;
}

template<>
bool verify_component<curves::edwards<183>,
                      snark::r1cs_gg_ppzksnark<curves::edwards<183>>>(components::blueprint<typename curves::edwards<183>::scalar_field_type> bp){
    std::cout << "Warning! r1cs_gg_ppzksnark for Edwards-183 is not implemented yet" << std::endl;

    return false;
}

#endif    // CRYPTO3_ZK_BLUEPRINT_VERIFY_R1CS_SCHEME_COMPONENT_TEST_HPP