#include <Eigen/Core>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int16.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>

#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <actionlib/server/simple_action_server.h>
#include <collect_calib_data/rotation_broadcasterAction.h>

#include "./../include/rotation_broadcaster.hpp"

namespace CalibrationVelodyne
{

    RotationBroadcaster::RotationBroadcaster(ros::NodeHandle &nh) :
        success (false),
        server (nh, "rotation_broadcaster", false),
        degree_sub (nh.subscribe("degree", 1, &RotationBroadcaster::degree_cb, this))
    {
        ros::param::get("/rotation_broadcaster/x", x);
        ros::param::get("/rotation_broadcaster/y", y);
        ros::param::get("/rotation_broadcaster/z", z);

        server.registerGoalCallback(boost::bind(&RotationBroadcaster::goal_callback, this));
        server.registerPreemptCallback(boost::bind(&RotationBroadcaster::preempt_callback, this));
        server.start();
    }

    RotationBroadcaster::~RotationBroadcaster()
    {
    }

    // 回転行列Rの初期位置を設定
    void RotationBroadcaster::set_R(
                Eigen::Matrix3f &R,
                const float roll,
                const float pitch,
                const float yaw)
    {
        Eigen::Matrix3f R_tmp;
        Eigen::Matrix3f Rpi;
        Eigen::Matrix3f Rya;
        Eigen::Matrix3f Rro;

        Rro << 1, 0, 0,
               0, cos(roll), -sin(roll),
               0, sin(roll), cos(roll);

        Rya << cos(yaw), 0, sin(yaw),
               0, 1, 0,
               -sin(yaw), 0, cos(yaw);

        Rpi << cos(pitch), -sin(pitch), 0,
               sin(pitch), cos(pitch), 0,
               0, 0, 1;

        R = Rro * Rpi * Rya;
        //R = R_tmp.inverse();

        //std::cout << "R : \n" << R << std::endl;
        feedback.work.data = true;
        feedback.work_method.data = "Done set rotation matrix";
        server.publishFeedback(feedback);
    }

    Eigen::Vector3f RotationBroadcaster::set_diff_xyz(
        const float roll,
        const float pitch,
        const float yaw,
        const float x,
        const float y,
        const float z)
    {
        Eigen::Matrix3f R_3x3;
        Eigen::Vector3f t;
        t << x, y, z;

        set_R(R_3x3, roll, pitch, yaw);
        t = R_3x3*t;

        return t;
    }

    void RotationBroadcaster::set_ts(
        const float roll,
        const float pitch,
        const float yaw,
        geometry_msgs::TransformStamped &ts)
    {
        Eigen::Vector3f t;
        t = set_diff_xyz(x, y, z, roll, pitch, yaw);

        ts.header.stamp = ros::Time::now();
        ts.header.frame_id = "mount";
        ts.child_frame_id = "velodyne";

        tf2::Vector3 translation;
	    translation.setValue(t(0), t(1), t(2));
	    ts.transform.translation.x = translation.x();
	    ts.transform.translation.y = translation.y();
	    ts.transform.translation.z = translation.z();

	    tf2::Quaternion rotation;
	    rotation.setRPY(roll, pitch, yaw);
	    ts.transform.rotation.x = rotation.x();
	    ts.transform.rotation.y = rotation.y();
	    ts.transform.rotation.z = rotation.z();
	    ts.transform.rotation.w = rotation.w();

        feedback.work.data = true;
        feedback.work_method.data = "Done set TransformStamp";
        server.publishFeedback(feedback);
    }

    void RotationBroadcaster::degree_cb(
        const std_msgs::Int16 &potentio_cb)
    {
        if(degree != potentio_cb.data)
        {
            degree = potentio_cb.data;
            cb_success = true;
        }
        else
        {
            cb_success = false;
        }
    }

    void RotationBroadcaster::goal_callback()
    {
        if(server.isNewGoalAvailable())
        {
          goal = server.acceptNewGoal();
        }

        ros::Rate rate(1.0);
        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);
        geometry_msgs::TransformStamped ts;

        while(ros::ok())
        {
            try
            {
                ts = tfBuffer.lookupTransform("map", "velodyne", ros::Time());
                break;
            }
            catch(tf2::TransformException &ex)
            {
                ROS_ERROR("%s", ex.what());
                ros::Duration(1.0).sleep();
                ros::spinOnce();
                continue;
            }

            /*
            if(cb_success)
            {
                pitch = degree * (M_PI/180.0);
                set_ts(0.0, pitch, 0.0, ts);
                cb_success = false;
                break;
            }
            else
            {
                ROS_WARN("Do not movement the mount");
                //ROS_WARN("potentio degree : %d", degree);
                rate.sleep();
                ros::spinOnce();
            }
            */
        }

        result.success_work.data = true;
        result.result_deg.data = degree;
        result.result_rotation = ts;
        server.setSucceeded(result, "ts");
    }

    void RotationBroadcaster::preempt_callback()
    {
        result.success_work.data = false;
        server.setPreempted(result, "preempted");
    }

    void RotationBroadcaster::run()
    {
        ros::spin();
    }

}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "rotation_broadcaster");
    ros::NodeHandle nh;

    CalibrationVelodyne::RotationBroadcaster *server;
    server = new CalibrationVelodyne::RotationBroadcaster(nh);
    server->run();
    delete server;

    return 0;
}


// SPDX-License-Identifier: BSD-3-Clause
// Copyright (c) 2022 Scipp contributors (https://github.com/scipp)
/// @file
/// @author Simon Heybrock
#include <Eigen/Geometry>

#include "scipp/variable/misc_operations.h"

#include "docstring.h"
#include "pybind11.h"

using namespace scipp;
using namespace scipp::variable::geometry;

namespace py = pybind11;

void init_geometry(py::module &m) {
  auto geom_m = m.def_submodule("geometry");

  geom_m.def(
      "position",
      [](const Variable &x, const Variable &y, const Variable &z) {
        return position(x, y, z);
      },
      py::arg("x"), py::arg("y"), py::arg("z"),
      py::call_guard<py::gil_scoped_release>());

  geom_m.def(
      "rotation_matrix_from_quaternion_coeffs", [](py::array_t<double> value) {
        if (value.size() != 4)
          throw std::runtime_error("Incompatible list size: expected size 4.");
        return Eigen::Quaterniond(value.cast<std::vector<double>>().data())
            .toRotationMatrix();
      });
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_HYPOT_HPP_INCLUDED

#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( hypot_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<A0> >
                          , bd::generic_<bd::floating_<A0> >
                          , bs::fast_tag
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1
                                    , fast_tag const&) const BOOST_NOEXCEPT
    {
      return boost::simd::sqrt(bs::fma(a0, a0, sqr(a1)));
    }
  };
} } }

#endif


#include <boost/math/special_functions/trigamma.hpp>


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/AutodiffTypes.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <Eigen/Dense>

//local assembler for helmolhz equation, see Laplace
namespace polyfem
{
	class Helmholtz
	{
	public:
		Eigen::Matrix<double, 1, 1> assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;
		Eigen::Matrix<double, 1, 1> compute_rhs(const AutodiffHessianPt &pt) const;

		Eigen::Matrix<AutodiffScalarGrad, Eigen::Dynamic, 1, 0, 3, 1> kernel(const int dim, const AutodiffScalarGrad &r) const;

		inline int size() const { return 1; }

		//sets the k parameter
		void set_parameters(const json &params);

	private:
		double k_ = 1;
	};
} // namespace polyfem


#include <ros/ros.h>
#include <std_msgs/String.h>
#include <model_based_shared_control/State.h>
#include <model_based_shared_control/Control.h>
#include "robotlib/dynamicalSystems/koopman/koopman_operator.hpp"
#include "robotlib/dynamicalSystems/koopman/basis_functions/linear_basis.hpp"
#include "robotlib/dynamicalSystems/koopman/basis_functions/nonlinear_basis.hpp"
#include <armadillo>

class HumanRobot {

public:

  // publishers and subscribers
  ros::Subscriber state_sub;
  ros::Subscriber shutdown_sub;
  bool has_initialized = false;

  // messages
  model_based_shared_control::State state;
  model_based_shared_control::Control control;

  // data vectors
  arma::vec current_state;
  arma::vec dataIn;
  arma::vec dataOut;
  arma::vec cdataIn;
  arma::vec hdataIn;
  arma::vec hdataOut;

  // Koopman models
  KoopmanOperator* linear_koopman_operator;
  KoopmanOperator* nonlinear_koopman_operator;

  HumanRobot(ros::Rate* loop_rate) {

    ros::NodeHandle nh;

    // set up subscribers
    state_sub = nh.subscribe("/state", 1, &HumanRobot::get_state, this);
    shutdown_sub = nh.subscribe("/shutdown", 1, &HumanRobot::get_shutdown, this);

    // set up data vectors
    dataIn = arma::zeros<arma::vec>(6);
    dataOut = arma::zeros<arma::vec>(6);
    cdataIn = arma::zeros<arma::vec>(2);
    hdataIn = arma::zeros<arma::vec>(2);
    hdataOut = arma::zeros<arma::vec>(2);
    current_state = arma::zeros<arma::vec>(6);

    // set up Linear Koopman
    linear_koopman_operator = new KoopmanOperator(new LinearBasisFunction());

    // set up Non Linear Koopman
    nonlinear_koopman_operator = new KoopmanOperator(new NonLinearBasisFunction());

  }

  void get_state(const model_based_shared_control::State::ConstPtr& msg) {
    if (has_initialized == false) {
      dataOut[0] = msg->x;
      dataOut[1] = msg->y;
      dataOut[2] = msg->theta;
      dataOut[3] = msg->x_dot;
      dataOut[4] = msg->y_dot;
      dataOut[5] = msg->theta_dot;
      hdataOut[0] = msg->u_1;
      hdataOut[1] = msg->u_2;
      has_initialized = true;
    } else {
       if (std::abs(dataOut[0] - msg->x) + std::abs(dataOut[1] - msg->y) < 0.3){
        dataIn = dataOut;
        hdataIn = hdataOut;
        dataOut[0] = msg->x;
        dataOut[1] = msg->y;
        dataOut[2] = msg->theta;
        dataOut[3] = msg->x_dot;
        dataOut[4] = msg->y_dot;
        dataOut[5] = msg->theta_dot;
        hdataOut[0] = msg->u_1;
        hdataOut[1] = msg->u_2;
        linear_koopman_operator->gradStep(dataIn, hdataIn, dataOut, hdataOut);
        nonlinear_koopman_operator->gradStep(dataIn, hdataIn, dataOut, hdataOut);
        current_state = dataOut;
      } else {
        has_initialized = false;
      }
		}
	}

  void get_shutdown(const std_msgs::String::ConstPtr& msg) {
    std::string filePath = msg->data;
    std::string linearFilePath = filePath + "-koopman-linear";
    std::string nonlinearFilePath = filePath + "-koopman-nonlinear";
    linear_koopman_operator->saveOperator(linearFilePath);
    nonlinear_koopman_operator->saveOperator(nonlinearFilePath);
  }

};

int main(int argc, char** argv) {
  ros::init(argc, argv,"human_robot");
  ros::NodeHandle nh;
  ros::Rate loop_rate(10);
  HumanRobot sys(&loop_rate);

  while (ros::ok()) {
    loop_rate.sleep();
    ros::spinOnce();
  }

  return 0;
}


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <iomanip>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include "SFMText.hpp"
#include "SFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

namespace sfmt {
    using namespace NTL;
    using namespace std;
    static void calc_minimal(GF2X& minimal, SFMText& sfmt, int bitpos)
    {
	uint32_t mask[4];
	for (int i = 0; i < 4; i++) {
	    mask[i] = 0;
	}
	int pos = bitpos / 32;
	uint32_t m = 1 << (bitpos % 32);
	mask[pos] = m;
	int maxdegree = sfmt.get_maxdegree();
	vec_GF2 seq;
	seq.SetLength(2 * maxdegree);
	for (int i = 0; i < 2 * maxdegree; i++) {
	    seq[i] = sfmt.next(mask);
	}
	MinPolySeq(minimal, seq, maxdegree);
#ifdef DEBUG
	if (deg(minimal) == 0) {
	    cout << "deg minimal:" << dec << deg(minimal) << endl;
	    cout << "minimal:" << minimal << endl;
	    cout << "seq:" << seq << endl;
	}
#endif
    }

    static void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
	GF2X gcd;
	mul(lcm, x, y);
	GCD(gcd, x, y);
	lcm /= gcd;
    }

    void get_characteristic(GF2X& lcmpoly, SFMText& sfmt) {
	GF2X minimal;
	GF2X tmp;
	int maxdegree = sfmt.get_maxdegree();
	sfmt.seeding(1234);
	for (int bitpos = 0; bitpos < 128; bitpos++) {
	    calc_minimal(minimal, sfmt, bitpos);
	    LCM(tmp, lcmpoly, minimal);
	    lcmpoly = tmp;
	    if (deg(lcmpoly) == maxdegree) {
		return;
	    }
	}
	for (int i = 0; i < maxdegree; i++) {
	    sfmt.init_basis();
	    for (int bitpos = 0; bitpos < 128; bitpos++) {
		calc_minimal(minimal, sfmt, bitpos);
		LCM(tmp, lcmpoly, minimal);
		lcmpoly = tmp;
		if (deg(lcmpoly) == maxdegree) {
		    return;
		}
	    }
	}
	cerr << "deg:" << deg(lcmpoly) << endl;
	throw new logic_error("can't find lcm");
    }

    static void check(SFMText& sfmt) {
	sfmt.seeding(1234);

	for (int i = 0; i < 10; i++) {
	    w128_t x = sfmt.next();
	    for (int j = 0; j < 4; j++) {
		cout << dec << x.u[j] << endl;
	    }
	}
    }

    static int has_large_irreducible(GF2X& fpoly, int degree) {
	static const GF2X t2(2, 1);
	static const GF2X t1(1, 1);
	GF2X t2m;
	GF2X t;
	GF2X alpha;
	int m;

	t2m = t2;
	if (deg(fpoly) < degree) {
	    return 0;
	}
	t = t1;
	t += t2m;

	for (m = 1; deg(fpoly) > degree; m++) {
	    for(;;) {
		GCD(alpha, fpoly, t);
		if (IsOne(alpha)) {
		    break;
		}
		fpoly /= alpha;
		if (deg(fpoly) < degree) {
		    return 0;
		}
	    }
	    t2m *= t2m;
	    t2m %= fpoly;
	    add(t, t2m, t1);
	}
	if (deg(fpoly) != degree) {
	    return 0;
	}
	return IterIrredTest(fpoly);
    }
}
#if defined(MAIN)
using namespace sfmt;
using namespace NTL;
using namespace std;

int main(int argc, char *argv[]) {
    if (argc < 11) {
	cout << argv[0]
	     << " mexp sl1 sl2 sr1 sr2 pos1 mask1 mask2 mask3 mask4"
	     << " parity1 parity2 parity3 parity4"
	     << endl;
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    int sl2 = strtol(argv[4], NULL, 10);
    int sr1 = strtol(argv[5], NULL, 10);
    int sr2 = strtol(argv[6], NULL, 10);
    uint32_t mask[4];
    mask[0] = strtoull(argv[7], NULL, 16);
    mask[1] = strtoull(argv[8], NULL, 16);
    mask[2] = strtoull(argv[9], NULL, 16);
    mask[3] = strtoull(argv[10], NULL, 16);
    uint32_t parity[4];
    parity[0] = strtoull(argv[11], NULL, 16);
    parity[1] = strtoull(argv[12], NULL, 16);
    parity[2] = strtoull(argv[13], NULL, 16);
    parity[3] = strtoull(argv[14], NULL, 16);
#if defined(DEBUG)
    cout << "mexp:" << dec << mexp << endl;
    cout << "pos1:" << dec << pos1 << endl;
    cout << "sl1:" << dec << sl1 << endl;
    cout << "sl2:" << dec << sl2 << endl;
    cout << "sr1:" << dec << sr1 << endl;
    cout << "sr2:" << dec << sr2 << endl;
    cout << "mask1:" << hex<< mask[0] << endl;
    cout << "mask2:" << hex << mask[1] << endl;
    cout << "mask3:" << hex << mask[2] << endl;
    cout << "mask4:" << hex << mask[3] << endl;
    cout << "parity1:" << hex<< parity[0] << endl;
    cout << "parity2:" << hex << parity[1] << endl;
    cout << "parity3:" << hex << parity[2] << endl;
    cout << "parity4:" << hex << parity[3] << endl;
#endif
    SFMText sfmt(mexp, sl1, sl2, sr1, sr2, pos1,
		 mask, parity);
    GF2X characteristic(0, 1);
#if defined(DEBUG)
    check(sfmt);
#endif
    get_characteristic(characteristic, sfmt);
    GF2X work;
    work = characteristic;
#if defined(DEBUG)
    cout << "degree:" << deg(characteristic) << endl;
    cout << characteristic << endl;
#endif
    if (!has_large_irreducible(characteristic, mexp)) {
	cout << "error?" << endl;
	return -1;
    }
    string x;
    polytostring(x, work);
#if defined(DEBUG)
    cout << "characteristic:" << x << endl;
#endif
    cout << "#" << dec << mexp;
    cout << "," << dec << pos1;
    cout << "," << dec << sl1;
    cout << "," << dec << sl2;
    cout << "," << dec << sr1;
    cout << "," << dec << sr2;
    cout << "," << hex << mask[0];
    cout << "," << hex << mask[1];
    cout << "," << hex << mask[2];
    cout << "," << hex << mask[3];
    cout << "," << hex << parity[0];
    cout << "," << hex << parity[1];
    cout << "," << hex << parity[2];
    cout << "," << hex << parity[3];
    cout << endl;
    cout << x << endl;
    cout << dec << flush;
    return 0;
}
#endif


//[ LazyVector
///////////////////////////////////////////////////////////////////////////////
//  Copyright 2008 Eric Niebler. 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)
//
// This example constructs a mini-library for linear algebra, using
// expression templates to eliminate the need for temporaries when
// adding vectors of numbers.
//
// This example uses a domain with a grammar to prune the set
// of overloaded operators. Only those operators that produce
// valid lazy vector expressions are allowed.

#include <vector>
#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
namespace mpl = boost::mpl;
namespace proto = boost::proto;
using proto::_;

// This grammar describes which lazy vector expressions
// are allowed; namely, vector terminals and addition
// and subtraction of lazy vector expressions.
struct LazyVectorGrammar
  : proto::or_<
        proto::terminal< std::vector<_> >
      , proto::plus< LazyVectorGrammar, LazyVectorGrammar >
      , proto::minus< LazyVectorGrammar, LazyVectorGrammar >
    >
{};

// Expressions in the lazy vector domain must conform
// to the lazy vector grammar
struct lazy_vector_domain;

// Here is an evaluation context that indexes into a lazy vector
// expression, and combines the result.
template<typename Size = std::size_t>
struct lazy_subscript_context
{
    lazy_subscript_context(Size subscript)
      : subscript_(subscript)
    {}

    // Use default_eval for all the operations ...
    template<typename Expr, typename Tag = typename Expr::proto_tag>
    struct eval
      : proto::default_eval<Expr, lazy_subscript_context>
    {};

    // ... except for terminals, which we index with our subscript
    template<typename Expr>
    struct eval<Expr, proto::tag::terminal>
    {
        typedef typename proto::result_of::value<Expr>::type::value_type result_type;

        result_type operator ()( Expr const & expr, lazy_subscript_context & ctx ) const
        {
            return proto::value( expr )[ ctx.subscript_ ];
        }
    };

    Size subscript_;
};

// Here is the domain-specific expression wrapper, which overrides
// operator [] to evaluate the expression using the lazy_subscript_context.
template<typename Expr>
struct lazy_vector_expr
  : proto::extends<Expr, lazy_vector_expr<Expr>, lazy_vector_domain>
{
    typedef proto::extends<Expr, lazy_vector_expr<Expr>, lazy_vector_domain> base_type;

    lazy_vector_expr( Expr const & expr = Expr() )
      : base_type( expr )
    {}

    // Use the lazy_subscript_context<> to implement subscripting
    // of a lazy vector expression tree.
    template< typename Size >
    typename proto::result_of::eval< Expr, lazy_subscript_context<Size> >::type
    operator []( Size subscript ) const
    {
        lazy_subscript_context<Size> ctx(subscript);
        return proto::eval(*this, ctx);
    }
};

// Here is our lazy_vector terminal, implemented in terms of lazy_vector_expr
template< typename T >
struct lazy_vector
  : lazy_vector_expr< typename proto::terminal< std::vector<T> >::type >
{
    typedef typename proto::terminal< std::vector<T> >::type expr_type;

    lazy_vector( std::size_t size = 0, T const & value = T() )
      : lazy_vector_expr<expr_type>( expr_type::make( std::vector<T>( size, value ) ) )
    {}

    // Here we define a += operator for lazy vector terminals that
    // takes a lazy vector expression and indexes it. expr[i] here
    // uses lazy_subscript_context<> under the covers.
    template< typename Expr >
    lazy_vector &operator += (Expr const & expr)
    {
        std::size_t size = proto::value(*this).size();
        for(std::size_t i = 0; i < size; ++i)
        {
            proto::value(*this)[i] += expr[i];
        }
        return *this;
    }
};

// Tell proto that in the lazy_vector_domain, all
// expressions should be wrapped in laxy_vector_expr<>
struct lazy_vector_domain
  : proto::domain<proto::generator<lazy_vector_expr>, LazyVectorGrammar>
{};

int main()
{
    // lazy_vectors with 4 elements each.
    lazy_vector< double > v1( 4, 1.0 ), v2( 4, 2.0 ), v3( 4, 3.0 );

    // Add two vectors lazily and get the 2nd element.
    double d1 = ( v2 + v3 )[ 2 ];   // Look ma, no temporaries!
    std::cout << d1 << std::endl;

    // Subtract two vectors and add the result to a third vector.
    v1 += v2 - v3;                  // Still no temporaries!
    std::cout << '{' << v1[0] << ',' << v1[1]
              << ',' << v1[2] << ',' << v1[3] << '}' << std::endl;

    // This expression is disallowed because it does not conform
    // to the LazyVectorGrammar
    //(v2 + v3) += v1;

    return 0;
}
//]


#include "crypto_square.h"
#include <boost/algorithm/string/case_conv.hpp>
#include <algorithm>
#include <cctype>
#include <iterator>

using namespace std;

namespace
{

string normalize(string const &text)
{
    string result;
    copy_if(text.begin(), text.end(), back_inserter(result),
        [](const char c) { return !isspace(c) && !ispunct(c); });
    return boost::to_lower_copy(result);
}

}

namespace crypto_square
{

cipher::cipher(string const& text)
    : text_(normalize(text))
{
}

string cipher::normalize_plain_text() const
{
    return text_;
}

size_t cipher::size() const
{
    size_t length = 1;
    while (length*length < text_.size()) {
        ++length;
    }
    return length;
}

vector<string> cipher::plain_text_segments() const
{
    vector<string> segments;
    const size_t segment_size{size()};
    for (size_t i = 0; i < text_.length(); i += segment_size) {
        segments.push_back(text_.substr(i, segment_size));
    }
    return segments;
}

string cipher::cipher_text() const
{
    string result;
    const vector<string> segments{plain_text_segments()};
    const size_t rows{size()};
    const size_t num_segments{segments.size()};
    for (size_t i = 0; i < rows; ++i) {
        for (size_t j = 0; j < num_segments; ++j) {
            if (i < segments[j].length()) {
                result += segments[j][i];
            }
        }
    }
    return result;
}

string cipher::normalized_cipher_text() const
{
    const string encoded{cipher_text()};
    const auto num_rows = size();
    string result{encoded.substr(0, num_rows)};
    for (auto i = num_rows; i < encoded.length(); i += num_rows) {
        result += ' ' + encoded.substr(i, num_rows);
    }
    return result;
}

}


//  (C) Copyright Gaetano Mendola 2010, Simon West 2011.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum_kahan.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_sum_kahan
//
void test_sum_kahan()
{
    accumulator_set<float, stats<tag::sum_kahan> > acc;

    BOOST_CHECK_EQUAL(0.0f, sum_kahan(acc));

    for (size_t i = 0; i < 1e6; ++i) {
        acc(1e-6f);
    }

    BOOST_CHECK_EQUAL(1.0f, sum_kahan(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_sum_of_weights_kahan
//
void test_sum_of_weights_kahan()
{
    accumulator_set<float, stats<tag::sum_of_weights_kahan>, float > acc;

    BOOST_CHECK_EQUAL(0.0f, sum_of_weights_kahan(acc));

    for (size_t i = 0; i < 1e6; ++i) {
        acc(0, weight = 1e-6f);
    }

    BOOST_CHECK_EQUAL(1.0f, sum_of_weights_kahan(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_sum_of_variates_kahan
//
void test_sum_of_variates_kahan()
{
    accumulator_set<
        float,
        stats<tag::sum_of_variates_kahan<float, tag::covariate1> >,
        float
    >
    acc;

    BOOST_CHECK_EQUAL(0.0f, sum_of_variates_kahan(acc));

    for (size_t i = 0; i < 1e6; ++i) {
        acc(0, covariate1 = 1e-6f);
    }

    BOOST_CHECK_EQUAL(1.0f, sum_of_variates_kahan(acc));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("sum kahan tests");

    test->add(BOOST_TEST_CASE(&test_sum_kahan));
    test->add(BOOST_TEST_CASE(&test_sum_of_weights_kahan));
    test->add(BOOST_TEST_CASE(&test_sum_of_variates_kahan));

    return test;
}


#include <memory>

#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "imu-integrator/imu-integrator-eigen.h"

using namespace imu_integrator;  // NOLINT

Eigen::Vector4d getOrientationFromState(
    const Eigen::Matrix<double, kStateSize, 1>& state) {
  return state.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0);
}
Eigen::Vector3d getGyroBiasFromState(
    const Eigen::Matrix<double, kStateSize, 1>& state) {
  return state.block<kGyroBiasBlockSize, 1>(
      kStateGyroBiasOffset, 0);
}
Eigen::Vector3d getVelocityFromState(
    const Eigen::Matrix<double, kStateSize, 1>& state) {
  return state.block<kVelocityBlockSize, 1>(
      kStateVelocityOffset, 0);
}
Eigen::Vector3d getAccelBiasFromState(
    const Eigen::Matrix<double, kStateSize, 1>& state) {
  return state.block<kAccelBiasBlockSize, 1>(
      kStateAccelBiasOffset, 0);
}
Eigen::Vector3d getPositionFromState(
    const Eigen::Matrix<double, kStateSize, 1>& state) {
  return state.block<kPositionBlockSize, 1>(
      kStatePositionOffset, 0);
}

class ImuIntegratorEigenFixture : public ::testing::Test {
 protected:
  virtual void SetUp() {
    gravity_magnitude_ = 9.81;
    gyro_noise_sigma_ = 0.0;
    gyro_bias_sigma_ = 0.0;
    acc_noise_sigma_ = 0.0;
    acc_bias_sigma_ = 0.0;
    delta_time_seconds_ = 0.0;
  }

  void constructIntegrator() {
    integrator_ = std::shared_ptr<ImuIntegratorEigen>(
        new ImuIntegratorEigen(
            gyro_noise_sigma_, gyro_bias_sigma_, acc_noise_sigma_,
            acc_bias_sigma_, gravity_magnitude_));
  }

  void integrate() {
    integrator_->integrate(
        current_state_, debiased_imu_readings_, delta_time_seconds_,
        &next_state_, &next_phi_, &next_cov_);
  }

  std::shared_ptr<ImuIntegratorEigen> integrator_;

  // imu integrator constructor arguments
  double gyro_noise_sigma_;
  double gyro_bias_sigma_;
  double acc_noise_sigma_;
  double acc_bias_sigma_;
  double gravity_magnitude_;

  // imu integrator integrate() arguments
  Eigen::Matrix<double, kStateSize, 1> current_state_;
  Eigen::Matrix<double, 2 * kImuReadingSize, 1>
      debiased_imu_readings_;
  Eigen::Matrix<double, kStateSize, 1> next_state_;
  Eigen::Matrix<double, kErrorStateSize, kErrorStateSize>
      next_phi_;
  Eigen::Matrix<double, kErrorStateSize, kErrorStateSize>
      next_cov_;
  double delta_time_seconds_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};


/*=============================================================================
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2014      John Fletcher

    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)
==============================================================================*/

#include <cmath>
#include <boost/function.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>
#include <boost/phoenix/stl/cmath.hpp>
#include <boost/detail/lightweight_test.hpp>

int main()
{
    double eps = 0.000001;
    using namespace boost::phoenix::arg_names;
    boost::function<bool(double, double)> f = boost::phoenix::fabs(_1 - _2) < eps;

    double x = boost::phoenix::pow(_1,_2)(2.,2.);
    double y = boost::phoenix::atan2(_1,_2)(1.,1.);
    double z = boost::phoenix::tan(_1)(y);

    BOOST_TEST(f(0.0, 0 * eps));
    BOOST_TEST(!f(0.0, eps));
    BOOST_TEST(std::fabs(x-4.) < eps );
    BOOST_TEST(std::fabs(z-1.) < eps );
}


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

const int N=1000;

// To return an expression that references local arrays, the
// expression has to be wrapped in safeToReturn(), which turns
// FastArrayIterators into FastArrayCopyIterators which keep a copy of
// the arrays instead of referencing them.

// this is safe
BzBinaryExprResult<Multiply, Array<double,1>, Array<double,1> >::T_result::T_range_result
good()
{
  Array<double,1> a(N);
  a=tensor::i;
  Array<double,1> b(N);
  b=sin(a);
  return safeToReturn(a*b);
}

// this is unsafe
BzBinaryExprResult<Multiply, Array<double,1>, Array<double,1> >::T_result
bad()
{
  Array<double,1> a(N);
  a=tensor::i;
  Array<double,1> b(N);
  b=sin(a);
  return a*b;
}

int main()
{
  // switch to expr=bad() and it will crash.
  BzBinaryExprResult<Multiply, Array<double,1>, Array<double,1> >::T_result::T_range_result expr=good();

  //BzBinaryExprResult<Multiply, Array<double,1>, Array<double,1> >::T_result expr=bad();
  Array<double,1> result(N);
  result=expr;
  BZTEST(all(abs(result-tensor::i*sin(1.0*tensor::i))<1e-5));

    return 0;
}


#include "rect.h"

#include <algorithm>

#include <boost/algorithm/clamp.hpp>

csc::FloatRect::FloatRect(const Float2 p1, const Float2 p2)
{
	const float min_x{ std::min(p1.x, p2.x) };
	const float max_x{ std::max(p1.x, p2.x) };

	const float min_y{ std::min(p1.y, p2.y) };
	const float max_y{ std::max(p1.y, p2.y) };

	const float size_x{ max_x - min_x };
	const float size_y{ max_y - min_y };

	_begin = Float2{ min_x, min_y };
	_size = Float2{ size_x, size_y };
}

csc::Float2 csc::FloatRect::clamp(const csc::Float2 point) const
{
	const float result_x{ boost::algorithm::clamp(point.x, begin().x, end().x) };
	const float result_y{ boost::algorithm::clamp(point.y, begin().y, end().y) };
	return csc::Float2{ result_x, result_y };
}

csc::Float2 csc::FloatRect::normalized_pos(const csc::Float2 point) const
{
	const csc::Float2 relative_pos{ point - begin() };
	const csc::Float2 result{ relative_pos.x / size().x, relative_pos.y / size().y };
	return result;
}

csc::Float2 csc::FloatRect::scaled_pos(const csc::Float2 point) const
{
	const float x{ point.x * (end().x - begin().x) + begin().x };
	const float y{ point.y * (end().y - begin().y) + begin().y };
	return Float2{ x, y };
}

csc::FloatRect csc::FloatRect::grow(const float amount) const
{
	const Float2 delta{ amount, amount };
	const Float2 new_begin{ begin() - delta };
	const Float2 new_end{ end() + delta };
	return FloatRect{ new_begin, new_end };
}

csc::FloatRect csc::FloatRect::with_point(const Float2 point) const
{
	if (contains(point)) {
		// This rectangle already contains the point
		return FloatRect{ *this };
	}

	// Return the bounding box of p0, p1, and p2
	const Float2 p0{ begin() };
	const Float2 p1{ end() };
	const Float2 p2{ point };

	const float min_x{ std::min(std::min(p0.x, p1.x), p2.x) };
	const float min_y{ std::min(std::min(p0.y, p1.y), p2.y) };

	const float max_x{ std::max(std::max(p0.x, p1.x), p2.x) };
	const float max_y{ std::max(std::max(p0.y, p1.y), p2.y) };

	const Float2 begin{ min_x, min_y };
	const Float2 end{ max_x, max_y };

	return FloatRect{ begin, end };
}

bool csc::FloatRect::contains(const Float2 point) const
{
	const bool x{ point.x >= _begin.x && point.x <= end().x };
	const bool y{ point.y >= _begin.y && point.y <= end().y };
	return x && y;
}

bool csc::FloatRect::overlaps(const FloatRect other) const
{
	const auto this_end{ end() };
	const auto other_end{ other.end() };

	// Check horizontal overlap
	if (_begin.x > other_end.x || other._begin.x > this_end.x) {
		return false;
	}

	// Check vertical overlap
	if (_begin.y > other_end.y || other._begin.y > this_end.y) {
		return false;
	}

	return true;
}


//  Boost GCD & LCM common_factor.hpp test program  --------------------------//

//  (C) Copyright Daryle Walker 2001, 2006.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org for most recent version including documentation.

//  Revision History
//  01 Dec 2006  Various fixes for old compilers (Joaquin M Lopez Munoz)
//  10 Nov 2006  Make long long and __int64 mutually exclusive (Daryle Walker)
//  04 Nov 2006  Use more built-in numeric types, binary-GCD (Daryle Walker)
//  03 Nov 2006  Use custom numeric types (Daryle Walker)
//  02 Nov 2006  Change to Boost.Test's unit test system (Daryle Walker)
//  07 Nov 2001  Initial version (Daryle Walker)

#define BOOST_TEST_MAIN  "Boost.Math GCD & LCM unit tests"

#include <boost/config.hpp>              // for BOOST_MSVC, etc.
#include <boost/detail/workaround.hpp>
#include <boost/math/common_factor.hpp>  // for boost::math::gcd, etc.
#include <boost/mpl/list.hpp>            // for boost::mpl::list
#include <boost/operators.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <istream>  // for std::basic_istream
#include <limits>   // for std::numeric_limits
#include <ostream>  // for std::basic_ostream

namespace {

// TODO: add polynominal/non-real type; especially after any switch to the
// binary-GCD algorithm for built-in types

// Custom integer class (template)
template < typename IntType, int ID = 0 >
class my_wrapped_integer
    : private ::boost::shiftable1<my_wrapped_integer<IntType, ID>,
        ::boost::operators<my_wrapped_integer<IntType, ID> > >
{
    // Helper type-aliases
    typedef my_wrapped_integer    self_type;
    typedef IntType self_type::*  bool_type;

    // Member data
    IntType  v_;

public:
    // Template parameters
    typedef IntType  int_type;

    BOOST_STATIC_CONSTANT(int,id = ID);

    // Lifetime management (use automatic destructor and copy constructor)
    my_wrapped_integer( int_type const &v = int_type() )  : v_( v )  {}

    // Accessors
    int_type  value() const  { return this->v_; }

    // Operators (use automatic copy assignment)
    operator bool_type() const  { return this->v_ ? &self_type::v_ : 0; }

    self_type &  operator ++()  { ++this->v_; return *this; }
    self_type &  operator --()  { --this->v_; return *this; }

    self_type  operator ~() const  { return self_type( ~this->v_ ); }
    self_type  operator !() const  { return self_type( !this->v_ ); }
    self_type  operator +() const  { return self_type( +this->v_ ); }
    self_type  operator -() const  { return self_type( -this->v_ ); }

    bool  operator  <( self_type const &r ) const  { return this->v_ < r.v_; }
    bool  operator ==( self_type const &r ) const  { return this->v_ == r.v_; }

    self_type &operator *=(self_type const &r) {this->v_ *= r.v_; return *this;}
    self_type &operator /=(self_type const &r) {this->v_ /= r.v_; return *this;}
    self_type &operator %=(self_type const &r) {this->v_ %= r.v_; return *this;}
    self_type &operator +=(self_type const &r) {this->v_ += r.v_; return *this;}
    self_type &operator -=(self_type const &r) {this->v_ -= r.v_; return *this;}
    self_type &operator<<=(self_type const &r){this->v_ <<= r.v_; return *this;}
    self_type &operator>>=(self_type const &r){this->v_ >>= r.v_; return *this;}
    self_type &operator &=(self_type const &r) {this->v_ &= r.v_; return *this;}
    self_type &operator |=(self_type const &r) {this->v_ |= r.v_; return *this;}
    self_type &operator ^=(self_type const &r) {this->v_ ^= r.v_; return *this;}

    // Input & output
    friend std::istream & operator >>( std::istream &i, self_type &x )
    { return i >> x.v_; }

    friend std::ostream & operator <<( std::ostream &o, self_type const &x )
    { return o << x.v_; }

};  // my_wrapped_integer

template < typename IntType, int ID >
my_wrapped_integer<IntType, ID>  abs( my_wrapped_integer<IntType, ID> const &x )
{ return ( x < my_wrapped_integer<IntType, ID>(0) ) ? -x : +x; }

typedef my_wrapped_integer<int>          MyInt1;
typedef my_wrapped_integer<unsigned>     MyUnsigned1;
typedef my_wrapped_integer<int, 1>       MyInt2;
typedef my_wrapped_integer<unsigned, 1>  MyUnsigned2;

// Without these explicit instantiations, MSVC++ 6.5/7.0 does not find
// some friend operators in certain contexts.
MyInt1       dummy1;
MyUnsigned1  dummy2;
MyInt2       dummy3;
MyUnsigned2  dummy4;

// Various types to test with each GCD/LCM
typedef ::boost::mpl::list<signed char, short, int, long,
#ifdef BOOST_HAS_LONG_LONG
 boost::long_long_type,
#elif defined(BOOST_HAS_MS_INT64)
 __int64,
#endif
 MyInt1>  signed_test_types;
typedef ::boost::mpl::list<unsigned char, unsigned short, unsigned,
 unsigned long,
#ifdef BOOST_HAS_LONG_LONG
 boost::ulong_long_type,
#elif defined(BOOST_HAS_MS_INT64)
 unsigned __int64,
#endif
 MyUnsigned1, MyUnsigned2>  unsigned_test_types;

}  // namespace

#define BOOST_NO_MACRO_EXPAND /**/

// Specialize numeric_limits for _some_ of our types
namespace std
{

template < >
class numeric_limits< MyInt1 >
{
    typedef MyInt1::int_type             int_type;
    typedef numeric_limits<int_type>  limits_type;

public:
    BOOST_STATIC_CONSTANT(bool, is_specialized = limits_type::is_specialized);

    static MyInt1 min BOOST_NO_MACRO_EXPAND() throw()  { return (limits_type::min)(); }
    static MyInt1 max BOOST_NO_MACRO_EXPAND() throw()  { return (limits_type::max)(); }

    BOOST_STATIC_CONSTANT(int, digits      = limits_type::digits);
    BOOST_STATIC_CONSTANT(int, digits10    = limits_type::digits10);
    BOOST_STATIC_CONSTANT(bool, is_signed  = limits_type::is_signed);
    BOOST_STATIC_CONSTANT(bool, is_integer = limits_type::is_integer);
    BOOST_STATIC_CONSTANT(bool, is_exact   = limits_type::is_exact);
    BOOST_STATIC_CONSTANT(int, radix       = limits_type::radix);
    static MyInt1 epsilon() throw()      { return limits_type::epsilon(); }
    static MyInt1 round_error() throw()  { return limits_type::round_error(); }

    BOOST_STATIC_CONSTANT(int, min_exponent   = limits_type::min_exponent);
    BOOST_STATIC_CONSTANT(int, min_exponent10 = limits_type::min_exponent10);
    BOOST_STATIC_CONSTANT(int, max_exponent   = limits_type::max_exponent);
    BOOST_STATIC_CONSTANT(int, max_exponent10 = limits_type::max_exponent10);

    BOOST_STATIC_CONSTANT(bool, has_infinity             = limits_type::has_infinity);
    BOOST_STATIC_CONSTANT(bool, has_quiet_NaN            = limits_type::has_quiet_NaN);
    BOOST_STATIC_CONSTANT(bool, has_signaling_NaN        = limits_type::has_signaling_NaN);
    BOOST_STATIC_CONSTANT(float_denorm_style, has_denorm = limits_type::has_denorm);
    BOOST_STATIC_CONSTANT(bool, has_denorm_loss          = limits_type::has_denorm_loss);

    static MyInt1 infinity() throw()      { return limits_type::infinity(); }
    static MyInt1 quiet_NaN() throw()     { return limits_type::quiet_NaN(); }
    static MyInt1 signaling_NaN() throw() {return limits_type::signaling_NaN();}
    static MyInt1 denorm_min() throw()    { return limits_type::denorm_min(); }

    BOOST_STATIC_CONSTANT(bool, is_iec559  = limits_type::is_iec559);
    BOOST_STATIC_CONSTANT(bool, is_bounded = limits_type::is_bounded);
    BOOST_STATIC_CONSTANT(bool, is_modulo  = limits_type::is_modulo);

    BOOST_STATIC_CONSTANT(bool, traps                    = limits_type::traps);
    BOOST_STATIC_CONSTANT(bool, tinyness_before          = limits_type::tinyness_before);
    BOOST_STATIC_CONSTANT(float_round_style, round_style = limits_type::round_style);

};  // std::numeric_limits<MyInt1>

template < >
class numeric_limits< MyUnsigned1 >
{
    typedef MyUnsigned1::int_type        int_type;
    typedef numeric_limits<int_type>  limits_type;

public:
    BOOST_STATIC_CONSTANT(bool, is_specialized = limits_type::is_specialized);

    static MyUnsigned1 min BOOST_NO_MACRO_EXPAND() throw()  { return (limits_type::min)(); }
    static MyUnsigned1 max BOOST_NO_MACRO_EXPAND() throw()  { return (limits_type::max)(); }

    BOOST_STATIC_CONSTANT(int, digits      = limits_type::digits);
    BOOST_STATIC_CONSTANT(int, digits10    = limits_type::digits10);
    BOOST_STATIC_CONSTANT(bool, is_signed  = limits_type::is_signed);
    BOOST_STATIC_CONSTANT(bool, is_integer = limits_type::is_integer);
    BOOST_STATIC_CONSTANT(bool, is_exact   = limits_type::is_exact);
    BOOST_STATIC_CONSTANT(int, radix       = limits_type::radix);
    static MyUnsigned1 epsilon() throw()      { return limits_type::epsilon(); }
    static MyUnsigned1 round_error() throw(){return limits_type::round_error();}

    BOOST_STATIC_CONSTANT(int, min_exponent   = limits_type::min_exponent);
    BOOST_STATIC_CONSTANT(int, min_exponent10 = limits_type::min_exponent10);
    BOOST_STATIC_CONSTANT(int, max_exponent   = limits_type::max_exponent);
    BOOST_STATIC_CONSTANT(int, max_exponent10 = limits_type::max_exponent10);

    BOOST_STATIC_CONSTANT(bool, has_infinity             = limits_type::has_infinity);
    BOOST_STATIC_CONSTANT(bool, has_quiet_NaN            = limits_type::has_quiet_NaN);
    BOOST_STATIC_CONSTANT(bool, has_signaling_NaN        = limits_type::has_signaling_NaN);
    BOOST_STATIC_CONSTANT(float_denorm_style, has_denorm = limits_type::has_denorm);
    BOOST_STATIC_CONSTANT(bool, has_denorm_loss          = limits_type::has_denorm_loss);

    static MyUnsigned1 infinity() throw()    { return limits_type::infinity(); }
    static MyUnsigned1 quiet_NaN() throw()  { return limits_type::quiet_NaN(); }
    static MyUnsigned1 signaling_NaN() throw()
        { return limits_type::signaling_NaN(); }
    static MyUnsigned1 denorm_min() throw(){ return limits_type::denorm_min(); }

    BOOST_STATIC_CONSTANT(bool, is_iec559  = limits_type::is_iec559);
    BOOST_STATIC_CONSTANT(bool, is_bounded = limits_type::is_bounded);
    BOOST_STATIC_CONSTANT(bool, is_modulo  = limits_type::is_modulo);

    BOOST_STATIC_CONSTANT(bool, traps                    = limits_type::traps);
    BOOST_STATIC_CONSTANT(bool, tinyness_before          = limits_type::tinyness_before);
    BOOST_STATIC_CONSTANT(float_round_style, round_style = limits_type::round_style);

};  // std::numeric_limits<MyUnsigned1>

#if BOOST_WORKAROUND(BOOST_MSVC,<1300)
// MSVC 6.0 lacks operator<< for __int64, see
// http://support.microsoft.com/default.aspx?scid=kb;en-us;168440

inline ostream& operator<<(ostream& os, __int64 i)
{
    char buf[20];
    sprintf(buf,"%I64d", i);
    os << buf;
    return os;
}

inline ostream& operator<<(ostream& os, unsigned __int64 i)
{
    char buf[20];
    sprintf(buf,"%I64u", i);
    os << buf;
    return os;
}
#endif

}  // namespace std

// GCD tests
BOOST_AUTO_TEST_SUITE( gcd_test_suite )

// GCD on signed integer types
BOOST_AUTO_TEST_CASE_TEMPLATE( gcd_int_test, T, signed_test_types )
{
#ifndef BOOST_MSVC
    using boost::math::gcd;
#else
    using namespace boost::math;
#endif

    // Originally from Boost.Rational tests
    BOOST_CHECK_EQUAL( gcd<T>(  1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( gcd<T>( -1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( gcd<T>(  1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( gcd<T>( -1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( gcd<T>(  0,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( gcd<T>(  7,   0), static_cast<T>( 7) );
    BOOST_CHECK_EQUAL( gcd<T>(  0,   9), static_cast<T>( 9) );
    BOOST_CHECK_EQUAL( gcd<T>( -7,   0), static_cast<T>( 7) );
    BOOST_CHECK_EQUAL( gcd<T>(  0,  -9), static_cast<T>( 9) );
    BOOST_CHECK_EQUAL( gcd<T>( 42,  30), static_cast<T>( 6) );
    BOOST_CHECK_EQUAL( gcd<T>(  6,  -9), static_cast<T>( 3) );
    BOOST_CHECK_EQUAL( gcd<T>(-10, -10), static_cast<T>(10) );
    BOOST_CHECK_EQUAL( gcd<T>(-25, -10), static_cast<T>( 5) );
    BOOST_CHECK_EQUAL( gcd<T>(  3,   7), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( gcd<T>(  8,   9), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( gcd<T>(  7,  49), static_cast<T>( 7) );
}

// GCD on unmarked signed integer type
BOOST_AUTO_TEST_CASE( gcd_unmarked_int_test )
{
#ifndef BOOST_MSVC
    using boost::math::gcd;
#else
    using namespace boost::math;
#endif

    // The regular signed-integer GCD function performs the unsigned version,
    // then does an absolute-value on the result.  Signed types that are not
    // marked as such (due to no std::numeric_limits specialization) may be off
    // by a sign.
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   1,  -1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(  -1,   1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   1,   1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(  -1,  -1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   0,   0 )), MyInt2( 0) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   7,   0 )), MyInt2( 7) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   0,   9 )), MyInt2( 9) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(  -7,   0 )), MyInt2( 7) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   0,  -9 )), MyInt2( 9) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(  42,  30 )), MyInt2( 6) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   6,  -9 )), MyInt2( 3) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>( -10, -10 )), MyInt2(10) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>( -25, -10 )), MyInt2( 5) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   3,   7 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   8,   9 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(gcd<MyInt2>(   7,  49 )), MyInt2( 7) );
}

// GCD on unsigned integer types
BOOST_AUTO_TEST_CASE_TEMPLATE( gcd_unsigned_test, T, unsigned_test_types )
{
#ifndef BOOST_MSVC
    using boost::math::gcd;
#else
    using namespace boost::math;
#endif

    // Note that unmarked types (i.e. have no std::numeric_limits
    // specialization) are treated like non/unsigned types
    BOOST_CHECK_EQUAL( gcd<T>( 1u,   1u), static_cast<T>( 1u) );
    BOOST_CHECK_EQUAL( gcd<T>( 0u,   0u), static_cast<T>( 0u) );
    BOOST_CHECK_EQUAL( gcd<T>( 7u,   0u), static_cast<T>( 7u) );
    BOOST_CHECK_EQUAL( gcd<T>( 0u,   9u), static_cast<T>( 9u) );
    BOOST_CHECK_EQUAL( gcd<T>(42u,  30u), static_cast<T>( 6u) );
    BOOST_CHECK_EQUAL( gcd<T>( 3u,   7u), static_cast<T>( 1u) );
    BOOST_CHECK_EQUAL( gcd<T>( 8u,   9u), static_cast<T>( 1u) );
    BOOST_CHECK_EQUAL( gcd<T>( 7u,  49u), static_cast<T>( 7u) );
}

// GCD at compile-time
BOOST_AUTO_TEST_CASE( gcd_static_test )
{
#ifndef BOOST_MSVC
    using boost::math::static_gcd;
#else
    using namespace boost::math;
#endif

    // Can't use "BOOST_CHECK_EQUAL", otherwise the "value" member will be
    // disqualified as compile-time-only constant, needing explicit definition
    BOOST_CHECK( (static_gcd< 1,  1>::value) == 1 );
    BOOST_CHECK( (static_gcd< 0,  0>::value) == 0 );
    BOOST_CHECK( (static_gcd< 7,  0>::value) == 7 );
    BOOST_CHECK( (static_gcd< 0,  9>::value) == 9 );
    BOOST_CHECK( (static_gcd<42, 30>::value) == 6 );
    BOOST_CHECK( (static_gcd< 3,  7>::value) == 1 );
    BOOST_CHECK( (static_gcd< 8,  9>::value) == 1 );
    BOOST_CHECK( (static_gcd< 7, 49>::value) == 7 );
}

// TODO: non-built-in signed and unsigned integer tests, with and without
// numeric_limits specialization; polynominal tests; note any changes if
// built-ins switch to binary-GCD algorithm

BOOST_AUTO_TEST_SUITE_END()

// LCM tests
BOOST_AUTO_TEST_SUITE( lcm_test_suite )

// LCM on signed integer types
BOOST_AUTO_TEST_CASE_TEMPLATE( lcm_int_test, T, signed_test_types )
{
#ifndef BOOST_MSVC
    using boost::math::lcm;
#else
    using namespace boost::math;
#endif

    // Originally from Boost.Rational tests
    BOOST_CHECK_EQUAL( lcm<T>(  1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( lcm<T>( -1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( lcm<T>(  1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( lcm<T>( -1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( lcm<T>(  0,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( lcm<T>(  6,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( lcm<T>(  0,   7), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( lcm<T>( -5,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( lcm<T>(  0,  -4), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( lcm<T>( 18,  30), static_cast<T>(90) );
    BOOST_CHECK_EQUAL( lcm<T>( -6,   9), static_cast<T>(18) );
    BOOST_CHECK_EQUAL( lcm<T>(-10, -10), static_cast<T>(10) );
    BOOST_CHECK_EQUAL( lcm<T>( 25, -10), static_cast<T>(50) );
    BOOST_CHECK_EQUAL( lcm<T>(  3,   7), static_cast<T>(21) );
    BOOST_CHECK_EQUAL( lcm<T>(  8,   9), static_cast<T>(72) );
    BOOST_CHECK_EQUAL( lcm<T>(  7,  49), static_cast<T>(49) );
}

// LCM on unmarked signed integer type
BOOST_AUTO_TEST_CASE( lcm_unmarked_int_test )
{
#ifndef BOOST_MSVC
    using boost::math::lcm;
#else
    using namespace boost::math;
#endif

    // The regular signed-integer LCM function performs the unsigned version,
    // then does an absolute-value on the result.  Signed types that are not
    // marked as such (due to no std::numeric_limits specialization) may be off
    // by a sign.
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   1,  -1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(  -1,   1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   1,   1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(  -1,  -1 )), MyInt2( 1) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   0,   0 )), MyInt2( 0) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   6,   0 )), MyInt2( 0) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   0,   7 )), MyInt2( 0) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(  -5,   0 )), MyInt2( 0) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   0,  -4 )), MyInt2( 0) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(  18,  30 )), MyInt2(90) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(  -6,   9 )), MyInt2(18) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>( -10, -10 )), MyInt2(10) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(  25, -10 )), MyInt2(50) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   3,   7 )), MyInt2(21) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   8,   9 )), MyInt2(72) );
    BOOST_CHECK_EQUAL( abs(lcm<MyInt2>(   7,  49 )), MyInt2(49) );
}

// LCM on unsigned integer types
BOOST_AUTO_TEST_CASE_TEMPLATE( lcm_unsigned_test, T, unsigned_test_types )
{
#ifndef BOOST_MSVC
    using boost::math::lcm;
#else
    using namespace boost::math;
#endif

    // Note that unmarked types (i.e. have no std::numeric_limits
    // specialization) are treated like non/unsigned types
    BOOST_CHECK_EQUAL( lcm<T>( 1u,   1u), static_cast<T>( 1u) );
    BOOST_CHECK_EQUAL( lcm<T>( 0u,   0u), static_cast<T>( 0u) );
    BOOST_CHECK_EQUAL( lcm<T>( 6u,   0u), static_cast<T>( 0u) );
    BOOST_CHECK_EQUAL( lcm<T>( 0u,   7u), static_cast<T>( 0u) );
    BOOST_CHECK_EQUAL( lcm<T>(18u,  30u), static_cast<T>(90u) );
    BOOST_CHECK_EQUAL( lcm<T>( 3u,   7u), static_cast<T>(21u) );
    BOOST_CHECK_EQUAL( lcm<T>( 8u,   9u), static_cast<T>(72u) );
    BOOST_CHECK_EQUAL( lcm<T>( 7u,  49u), static_cast<T>(49u) );
}

// LCM at compile-time
BOOST_AUTO_TEST_CASE( lcm_static_test )
{
#ifndef BOOST_MSVC
    using boost::math::static_lcm;
#else
    using namespace boost::math;
#endif

    // Can't use "BOOST_CHECK_EQUAL", otherwise the "value" member will be
    // disqualified as compile-time-only constant, needing explicit definition
    BOOST_CHECK( (static_lcm< 1,  1>::value) ==  1 );
    BOOST_CHECK( (static_lcm< 0,  0>::value) ==  0 );
    BOOST_CHECK( (static_lcm< 6,  0>::value) ==  0 );
    BOOST_CHECK( (static_lcm< 0,  7>::value) ==  0 );
    BOOST_CHECK( (static_lcm<18, 30>::value) == 90 );
    BOOST_CHECK( (static_lcm< 3,  7>::value) == 21 );
    BOOST_CHECK( (static_lcm< 8,  9>::value) == 72 );
    BOOST_CHECK( (static_lcm< 7, 49>::value) == 49 );
}

// TODO: see GCD to-do

BOOST_AUTO_TEST_SUITE_END()


#include "libsnark/gadgetlib1/gadgets/basic_gadgets.hpp"
#include "libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp"
#include "libsnark/common/default_types/r1cs_ppzksnark_pp.hpp"
#include "libff/common/utils.hpp"
#include <boost/optional.hpp>

using namespace libsnark;
using namespace std;

#include "payment_in_out_gadget.hpp"
#include "payment_multi_gadget.hpp"

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair()
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_in_out_gadget<FieldT> g(pb);
    g.generate_payment_in_out_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair_multi()
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_multi_gadget<FieldT> g(pb);
    g.generate_payment_multi_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_payment_in_out_proof(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const bit_vector &h_startbalance,
                                                                   const bit_vector &h_endbalance,
                                                                   const bit_vector &h_incoming,
                                                                   const bit_vector &h_outgoing,
                                                                   const bit_vector &r_startbalance,
                                                                   const bit_vector &r_endbalance,
                                                                   const bit_vector &r_incoming,
                                                                   const bit_vector &r_outgoing
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_in_out_gadget<FieldT> g(pb);
    g.generate_payment_in_out_constraints();
    g.generate_payment_in_out_witness(h_startbalance, h_endbalance, h_incoming, h_outgoing, r_startbalance, r_endbalance, r_incoming, r_outgoing);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_payment_multi_proof(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                            const bit_vector &h_startbalance,
                                                            const bit_vector &h_endbalance,
                                                            const bit_vector *h_incoming,
                                                            const bit_vector *h_outgoing,
                                                            const bit_vector &r_startbalance,
                                                            const bit_vector &r_endbalance,
                                                            const bit_vector *r_incoming,
                                                            const bit_vector *r_outgoing
                                                            )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_multi_gadget<FieldT> g(pb);
    g.generate_payment_multi_constraints();
    g.generate_payment_multi_witness(h_startbalance, h_endbalance, h_incoming, h_outgoing, r_startbalance, r_endbalance, r_incoming, r_outgoing);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
bool verify_payment_in_out_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const bit_vector &h_startbalance,
                  const bit_vector &h_endbalance,
                  const bit_vector &h_incoming,
                  const bit_vector &h_outgoing
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    const r1cs_primary_input<FieldT> input = l_input_map<FieldT>(h_startbalance, h_endbalance, h_incoming, h_outgoing);

    std::cout << "**** After l_input_map *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}

template<typename ppzksnark_ppT>
bool verify_payment_multi_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const bit_vector &h_startbalance,
                  const bit_vector &h_endbalance,
                  const bit_vector *h_incoming,
                  const bit_vector *h_outgoing
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;
    const r1cs_primary_input<FieldT> input = l_input_map_multi<FieldT>(h_startbalance, h_endbalance, h_incoming, h_outgoing);

    std::cout << "**** After l_input_map_multi *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_SINHCOSH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_SINHCOSH_HPP_INCLUDED

#include <boost/simd/arch/common/detail/generic/sinh_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/average.hpp>
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <boost/simd/function/scalar/bitwise_xor.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/if_else.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sinhcosh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    //////////////////////////////////////////////////////////////////////////////
    // if x = abs(a0) is less than 1 sinh is computed using a polynomial(float)
    // respectively rational(double) approx from cephes.
    // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
    // computed
    // * in the first case sinh is (e-rec(e))/2 and cosh (e+rec(e))/2
    // * in the second     sinh and cosh are (e/2)*e (avoiding undue overflow)
    // Threshold is Maxlog - Log_2
    //////////////////////////////////////////////////////////////////////////////
    BOOST_FORCEINLINE void operator() ( A0 a0,A0 & a1,A0 & a2) const BOOST_NOEXCEPT
    {
      A0 x = bs::abs(a0);
      auto test1 = (x >  Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = Half<A0>()*tmp;
      A0 rtmp = rec(tmp);
      A0 r =  test1 ? tmp1*tmp : tmp1-Half<A0>()*rtmp;
      a1 = bitwise_xor(((x < One<A0>())
                  ? detail::sinh_kernel<A0>::compute(x, sqr(x))
                  : r),
                 bitofsign(a0));
      a2 = test1 ? r : bs::average(tmp, rtmp);
    }
  };
} } }

#endif


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

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

#define BOOST_TEST_MODULE conversion_graph

#include <iostream>

#include <boost/units/io.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si/plane_angle.hpp>

#include <boost/astronomy/coordinate/coord_sys/horizon_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/ecliptic_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/galactic_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/equatorial_ra_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/equatorial_ha_coord.hpp>

#include <boost/astronomy/coordinate/conversion/conversion_graph.hpp>

#include <boost/test/unit_test.hpp>

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::astronomy::coordinate;

namespace bud = boost::units::degree;
namespace bac = boost::astronomy::coordinate;

BOOST_AUTO_TEST_SUITE(conversion_graph)

BOOST_AUTO_TEST_CASE(ecliptic_to_horizon) {

  ecliptic_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
    ec(97.638119 * bud::degrees, -17.857969 * bud::degrees);

  quantity<bud::plane_angle, double> phi = 52.175 * bud::degree;
  quantity<bud::plane_angle, double> st = 77.337 * bud::degree;
  quantity<bud::plane_angle, double> obliquity = 23.446 * bud::degree;

  matrix<double> ans = convert(COORDINATE_SYSTEM::ECLIPTIC,COORDINATE_SYSTEM::HORIZON,phi,st,obliquity,ec);

  auto theta = bac::extract_coordinates(ans).get_coordinates().first;
  auto gama = bac::extract_coordinates(ans).get_coordinates().second;

  BOOST_CHECK_CLOSE(theta.value() * 180.0 / PI, 153.491944, 0.001);
  BOOST_CHECK_CLOSE(gama.value() * 180.0 / PI, 40.399444, 0.001);
}

BOOST_AUTO_TEST_CASE(horizon_to_ecliptic) {

  horizon_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      hc(153.491944 * bud::degrees, 40.399444  * bud::degrees);

  quantity<bud::plane_angle, double> phi = 52.175 * bud::degree;
  quantity<bud::plane_angle, double> st = 77.337 * bud::degree;
  quantity<bud::plane_angle, double> obliquity = 23.446 * bud::degree;

  matrix<double> ans = convert(COORDINATE_SYSTEM::HORIZON,COORDINATE_SYSTEM::ECLIPTIC,phi,st,obliquity,hc);

  auto theta = bac::extract_coordinates(ans).get_coordinates().first;
  auto gama = bac::extract_coordinates(ans).get_coordinates().second;

  BOOST_CHECK_CLOSE(theta.value() * 180.0 / PI, 97.638119 , 0.001);
  BOOST_CHECK_CLOSE(gama.value() * 180.0 / PI, -17.857969, 0.001);
}

BOOST_AUTO_TEST_SUITE_END()


#include <algorithm>
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include "bicycle/bicycle.h"
#include "control_design_functions.h"
#include "firmware_generator.h"
#include "robot_bicycle_parameters.h"

int main(int argc, char ** argv)
{
  bicycle::Bicycle rb = bicycle::robot_bicycle();

  design_parameters params;
  params.N = 101;
  params.Ts = 0.005;
  params.lowest_speed = 0.5;
  params.highest_speed = 10.0;
  // LQR design parameters
  constexpr double pi = M_PI;
  constexpr double max_lean = 2.0*pi/180.0;    // rad
  constexpr double max_steer = 5.0*pi/180.0;   // rad
  constexpr double max_lean_frequency = 1*pi;  // rad / s
  constexpr double max_steer_frequency = 20*pi;// rad / s
  constexpr double max_steer_torque = 0.5;     // N * m
  params.Q = Eigen::MatrixXd::Zero(4, 4);
  params.Q(0, 0) = std::pow(max_lean, -2.0);
  params.Q(1, 1) = std::pow(max_steer, -2.0);
  params.Q(2, 2) = std::pow(max_lean_frequency * max_lean, -2.0);
  params.Q(3, 3) = std::pow(max_steer_frequency * max_steer, -2.0);
  params.R.resize(1, 1);
  params.R << std::pow(max_steer_torque, -2.0);
  // Observer pole placement factor
  params.pole_placement_factor = 3.0;
  // Kalman design
  constexpr double lean_acc_std = 1e-3;          // rad / s / s
  constexpr double steer_acc_std = 1e-2;         // rad / s / s
  constexpr double lean_vel_std = .6827 * lean_acc_std;      // rad / s
  constexpr double steer_vel_std = .6827 * steer_acc_std;     // rad / s
  params.W.resize(4, 4);          // Process noise covariance
  params.W << std::pow(lean_vel_std, 2.0), 0.0, 0.0, 0.0,
              0.0, std::pow(steer_vel_std, 2.0), 0.0, 0.0,
              0.0, 0.0, std::pow(lean_acc_std, 2.0), 0.0,
              0.0, 0.0, 0.0, std::pow(steer_acc_std, 2.0);

  params.V.resize(2, 2);          // Measurement noise covariance
  params.V << std::pow(2*pi/20000, 2.0), 0,
              0, std::pow(0.00227631723111, 2.0);

  std::vector<model_data> md = design_controller(params, rb);
  std::sort(md.begin(), md.end());
  firmware_generator(md);
}


#include <gtest/gtest.h>
#include <canopen_motor_node/unit_converter.h>
#include <canopen_motor_node/handle_layer.h>
#include <boost/bind.hpp>

using namespace canopen;

double * mapVariable(const std::string &, double *p) {
    return p;
}

TEST(TestMuparser, CheckNorm){
    double input = 0;
    UnitConverter uc("norm(in,-1000,1000)", boost::bind(HandleLayer::assignVariable, _1, &input, "in"));
    input = 0; EXPECT_EQ(0, uc.evaluate());
    input = 10; EXPECT_EQ(10, uc.evaluate());
    input = -10; EXPECT_EQ(-10, uc.evaluate());
    input = 1000; EXPECT_EQ(-1000, uc.evaluate());
    input = 1001; EXPECT_EQ(-999, uc.evaluate());
    input = 2000; EXPECT_EQ(0, uc.evaluate());
    input = 2001; EXPECT_EQ(1, uc.evaluate());
    input = -1000; EXPECT_EQ(-1000, uc.evaluate());
    input = 999; EXPECT_EQ(999, uc.evaluate());
}

int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/* boost random/normal_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: normal_distribution.hpp,v 1.16 2002/12/22 22:03:10 jmaurer Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

// deterministic polar method, uses trigonometric functions
template<class UniformRandomNumberGenerator, class RealType = double,
        class Adaptor = uniform_01<UniformRandomNumberGenerator, RealType> >
class normal_distribution
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

  explicit normal_distribution(base_type & rng,
                               const result_type& mean = result_type(0),
                               const result_type& sigma = result_type(1))
    : _rng(rng), _mean(mean), _sigma(sigma), _valid(false)
  {
    assert(sigma >= result_type(0));
  }

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  // compiler-generated copy constructor is NOT fine, need to purge cache
  normal_distribution(const normal_distribution& other)
    : _rng(other._rng), _mean(other._mean), _sigma(other._sigma), _valid(false)
  {
  }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng; }
  base_type& base() const { return _rng.base(); }
  RealType mean() const { return _mean; }
  RealType sigma() const { return _sigma; }

  void reset() { _valid = false; }

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::sqrt; using std::log; using std::sin; using std::cos;
#endif
    if(!_valid) {
      _r1 = _rng();
      _r2 = _rng();
      _cached_rho = sqrt(-result_type(2) * log(result_type(1)-_r2));
      _valid = true;
    } else {
      _valid = false;
    }
    // Can we have a boost::mathconst please?
    const result_type pi = result_type(3.14159265358979323846);

    return _cached_rho * (_valid ?
                          cos(result_type(2)*pi*_r1) :
                          sin(result_type(2)*pi*_r1))
      * _sigma + _mean;
  }
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const normal_distribution& x,
                         const normal_distribution& y)
  {
    return x._mean == y._mean && x._sigma == y._sigma &&
      x._valid == y._valid && x._rng == y._rng;
  }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const normal_distribution& nd)
  {
    os << nd._mean << " " << nd._sigma << " "
       << nd._valid << " " << nd._cached_rho << " " << nd._r1;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, normal_distribution& nd)
  {
    is >> std::ws >> nd._mean >> std::ws >> nd._sigma
       >> std::ws >> nd._valid >> std::ws >> nd._cached_rho
       >> std::ws >> nd._r1;
    return is;
  }
#endif

#else
  // Use a member function
  bool operator==(const normal_distribution& rhs) const
  {
    return _mean == rhs._mean && _sigma == rhs._sigma &&
      _valid == rhs._valid && _rng == rhs._rng;
  }
#endif
private:
  adaptor_type _rng;
  result_type _mean, _sigma;
  result_type _r1, _r2, _cached_rho;
  bool _valid;
};

} // namespace boost

#endif // BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP


///////////////////////////////////////////////////////////////////////////////
// vector_space::example::equal.cpp                                          //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <vector>
#include <iterator>
#include <boost/assert.hpp>
#include <boost/vector_space/functional/equal.hpp>
#include <libs/vector_space/example/equal.h>

void example_equal(std::ostream& out) {
    out << "-> example_equal : ";
    using namespace boost;
    namespace vs = boost::statistics::detail::vector_space;

    typedef unsigned val_;
    typedef std::vector<val_> row_;
    typedef std::vector<row_> mat_;
    typedef range_iterator<mat_>::type iterator_row_;
    typedef range_iterator<row_>::type iterator_elem_;
    typedef range_reference<mat_>::type ref_row_;
    typedef range_reference<row_>::type ref_elem_;
    typedef vs::equal<row_&> equal_;

    const unsigned n = 3;
    const val_ eps = 1; // defaul eps = 0, so |i-j| < 1 need, i,j integers
    row_ row(n);
    equal_ equal(row,eps);
/*

    std::cout << equal << std::endl;
    mat_ mat(n,row);
    val_ val = 0;
    for(unsigned i = 0; i<n; i++){
        val = i * n;
        ref_row_ ref_row = mat[i];
        for(unsigned j = 0; j<n; j++){
            val += j;
            row[j] = val;
            ref_row[j] = val;
        }
        std::copy(
            begin( row ),
            end( row ),
            std::ostream_iterator<int>(out," ")
        );
        out << std::endl;
        BOOST_ASSERT(
            equal(ref_row)
        );
    }
*/
    out << "<-" << std::endl;
}


/*
 * Copyright (c) 2021 The Foundation for Research on Information Technologies in Society (IT'IS).
 *
 * This file is part of iSEG
 * (see https://github.com/ITISFoundation/osparc-iseg).
 *
 * This software is released under the MIT License.
 *  https://opensource.org/licenses/MIT
 */
#include <boost/test/unit_test.hpp>

#include "../itkWeightedDijkstraImageFilter.h"

namespace iseg {

BOOST_AUTO_TEST_SUITE(iSeg_suite);
BOOST_AUTO_TEST_SUITE(TraceTubesWidget_suite);

// TestRunner.exe --run_test=iSeg_suite/TraceTubesWidget_suite/ROI_test --log_level=message
BOOST_AUTO_TEST_CASE(ROI_test)
{
	using image_type = itk::Image<float, 3>;
	auto img = image_type::New();
	{
		itk::Index<3> idx = {0, 0, 0};
		itk::Size<3> size = {10, 20, 30};
		itk::ImageRegion<3> region(idx, size);
		img->SetRegions(region);
	}
	img->Allocate();
	img->FillBuffer(0.f);

	{
		itk::Index<3> idx = {0, 5, 7};
		itk::Size<3> size = {3, 10, 10};
		itk::ImageRegion<3> region(idx, size);

		auto output = image_type::New();
		output->Graft(img);
		output->SetLargestPossibleRegion(img->GetLargestPossibleRegion());
		output->SetBufferedRegion(region);

		BOOST_CHECK_EQUAL(output->GetBufferedRegion().GetIndex()[0], 0);
		BOOST_CHECK_EQUAL(output->GetBufferedRegion().GetIndex()[1], 5);
		BOOST_CHECK_EQUAL(output->GetBufferedRegion().GetIndex()[2], 7);

		BOOST_CHECK_EQUAL(output->GetBufferedRegion().GetNumberOfPixels(), size[0] * size[1] * size[2]);
	}
}

// TestRunner.exe --run_test=iSeg_suite/TraceTubesWidget_suite --log_level=message
BOOST_AUTO_TEST_CASE(DijkstraMetric_test)
{
	using image_type = itk::Image<float, 3>;
	using metric_type = itk::MyMetric<image_type>;

	{
		metric_type m;

		BOOST_CHECK_CLOSE(0.0, m.ComputeAngle(1.0, 0.0, 0.0, 1.0, 0.0, 0.0), 1e-3);
		BOOST_CHECK_CLOSE(1.0, m.ComputeAngle(1.0, 0.0, 0.0, 0.0, 1.0, -1.0), 1e-3);
		BOOST_CHECK_CLOSE(2.0, m.ComputeAngle(1.0, 0.0, 0.0, -1.0, 0.0, 0.0), 1e-3);
	}

	{
		image_type::IndexType iprev = {0, 0, 0};
		image_type::IndexType i = {1, 0, 0};
		image_type::IndexType j = {2, 0, 0};

		image_type::RegionType region;
		region.SetIndex(iprev);
		region.SetUpperIndex(j);

		auto img = image_type::New();
		img->SetRegions(region);
		img->Allocate();
		img->FillBuffer(0.f);

		metric_type m;
		m.Initialize(img, iprev, j);

		BOOST_CHECK_CLOSE(3 * 0 + 1.0 + 0.0, m.GetEdgeWeight(i, j, iprev), 1e-3);
	}
}

BOOST_AUTO_TEST_SUITE_END();
BOOST_AUTO_TEST_SUITE_END();

} // namespace iseg