///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_FDDP_HPP_
#define CROCODDYL_CORE_SOLVERS_FDDP_HPP_

#include <Eigen/Cholesky>
#include <vector>

#include "crocoddyl/core/solvers/ddp.hpp"

namespace crocoddyl {

/**
 * @brief Feasibility-driven Differential Dynamic Programming (FDDP) solver
 *
 * The FDDP solver computes an optimal trajectory and control commands by iterates running `backwardPass()` and
 * `forwardPass()`. The backward pass accepts infeasible guess as described in the `SolverDDP::backwardPass()`.
 * Additionally, the forward pass handles infeasibility simulations that resembles the numerical behaviour of
 * a multiple-shooting formulation, i.e.:
 * \f{eqnarray}
 *   \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0 - (1 - \alpha)\mathbf{\bar{f}}_0,\\
 *   \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\
 *   \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k) - (1 -
 * \alpha)\mathbf{\bar{f}}_{k+1}.
 * \f}
 * Note that the forward pass keeps the gaps \f$\mathbf{\bar{f}}_s\f$ open according to the step length \f$\alpha\f$
 * that has been accepted. This solver has shown empirically greater globalization strategy. Additionally, the
 * expected improvement computation considers the gaps in the dynamics:
 * \f{equation}
 *   \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2,
 * \f}
 * with
 * \f{eqnarray}
 *   \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k} -
 *   V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1}
 *   \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2 V_{\mathbf{xx}_k}\mathbf{x}_k
 * - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \f}
 *
 * For more details about the feasibility-driven differential dynamic programming algorithm see:
 * \include mastalli-icra20.bib
 *
 * \sa `SolverDDP()`, `backwardPass()`, `forwardPass()`, `expectedImprovement()` and `updateExpectedImprovement()`
 */
class SolverFDDP : public SolverDDP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the FDDP solver
   *
   * @param[in] problem  shooting problem
   */
  explicit SolverFDDP(boost::shared_ptr<ShootingProblem> problem);
  virtual ~SolverFDDP();

  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
                     const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t maxiter = 100,
                     const bool is_feasible = false, const double regInit = 1e-9);

  /**
   * @copybrief SolverAbstract::expectedImprovement
   *
   * This function requires to first run `updateExpectedImprovement()`. The expected improvement computation considers
   * the gaps in the dynamics: \f{equation} \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2, \f} with
   * \f{eqnarray}
   *   \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k}
   * - V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1}
   *   \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2
   * V_{\mathbf{xx}_k}\mathbf{x}_k
   * - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \f}
   */
  virtual const Eigen::Vector2d& expectedImprovement();

  /**
   * @brief Update internal values for computing the expected improvement
   */
  void updateExpectedImprovement();
  virtual void forwardPass(const double stepLength);

  /**
   * @brief Return the threshold used for accepting step along ascent direction
   */
  double get_th_acceptnegstep() const;

  /**
   * @brief Modify the threshold used for accepting step along ascent direction
   */
  void set_th_acceptnegstep(const double th_acceptnegstep);

 protected:
  double dg_;  //!< Internal data for computing the expected improvement
  double dq_;  //!< Internal data for computing the expected improvement
  double dv_;  //!< Internal data for computing the expected improvement

 private:
  double th_acceptnegstep_;  //!< Threshold used for accepting step along ascent direction
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_FDDP_HPP_


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra vector lax
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/feature/core/data_customization_point/masked_raster.h"
#include "fern/algorithm/algebra/vector/lax.h"

namespace fa = fern::algorithm;

BOOST_AUTO_TEST_CASE(algorithm)
{
    // Create input raster:
    // +----+----+----+----+
    // |  0 |  1 |  2 |  3 |
    // +----+----+----+----+
    // |  4 |  5 |  6 |  7 |
    // +----+----+----+----+
    // |  8 |  9 | 10 | 11 |
    // +----+----+----+----+
    // | 12 | 13 | 14 | 15 |
    // +----+----+----+----+
    // | 16 | 17 | 18 | 19 |
    // +----+----+----+----+
    size_t const nr_rows = 5;
    size_t const nr_cols = 4;
    auto extents = fern::extents[nr_rows][nr_cols];

    double const cell_width = 2.0;
    double const cell_height = 3.0;
    double const west = 0.0;
    double const north = 0.0;

    using MaskedRaster = fern::MaskedRaster<double, 2>;

    MaskedRaster::Transformation transformation{{west, cell_width,
        north, cell_height}};
    MaskedRaster raster(extents, transformation);

    std::iota(raster.data(), raster.data() + raster.num_elements(), 0);

    fa::SequentialExecutionPolicy sequential;

    double const fraction = 0.6;

    // Calculate lax.
    MaskedRaster result(extents, transformation);

    // Without masking input and output values.
    {
        fa::algebra::lax(sequential, raster, fraction, result);

        /// // Verify the result.
        BOOST_CHECK_EQUAL(get(result, index(result, 0, 0)),
            ((1.0 - fraction) * 0.0) + (fraction * 25.0 / 8.0));
        BOOST_CHECK_EQUAL(get(result, index(result, 1, 1)),
            ((1.0 - fraction) * 5.0) + (fraction * 100.0 / 20.0));
    }

    using InputNoDataPolicy = fa::InputNoDataPolicies<
        fa::DetectNoDataByValue<fern::Mask<2>>>;
    using OutputNoDataPolicy = fa::MarkNoDataByValue<fern::Mask<2>>;

    // With masking input and output values.
    {
        result.fill(999.0);
        result.mask().fill(false);
        raster.mask()[1][1] = true;

        InputNoDataPolicy input_no_data_policy{{raster.mask(), true}};
        OutputNoDataPolicy output_no_data_policy(result.mask(), true);

        fa::algebra::lax(
            input_no_data_policy,
            output_no_data_policy,
            sequential,
            raster, fraction, result);

        // Verify the result.
        BOOST_CHECK_EQUAL(get(result.mask(), index(result.mask(), 0, 0)),
            false);
        BOOST_CHECK_EQUAL(get(result, index(result, 0, 0)),
            ((1.0 - fraction) * 0.0) + (fraction * 15.0 / 6.0));

        BOOST_CHECK_EQUAL(get(result.mask(), index(result.mask(), 1, 1)), true);
        BOOST_CHECK_EQUAL(get(result, index(result, 1, 1)), 999.0);
    }
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathSpecialFunctions,logical_neq) {
  using stan::math::logical_neq;
  EXPECT_TRUE(logical_neq(0,1));
  EXPECT_TRUE(logical_neq(1.0,0));
  EXPECT_TRUE(logical_neq(1, 2));
  EXPECT_TRUE(logical_neq(2.0, -1.0));

  EXPECT_FALSE(logical_neq(1,1));
  EXPECT_FALSE(logical_neq(5.7,5.7));
  EXPECT_FALSE(logical_neq(0,0.0));
}

TEST(MathFunctions, logical_neq_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_TRUE(stan::math::logical_neq(1.0, nan));
  EXPECT_TRUE(stan::math::logical_neq(nan, 2.0));
  EXPECT_TRUE(stan::math::logical_neq(nan, nan));
}


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
typedef Vector<u_char,Dynamic> ImgLine;
typedef Matrix<ImgLine,Dynamic,Dynamic> FlattenedImages;

unsigned char max(u_char a, u_char b){
    if (a<b) return b;
    else return a;
}

unsigned char min(u_char a, u_char b){
    if (a>b) return b;
    else return a;
}

unsigned char LBPvalue(OCTET* image, int x, int y, int nW, int nH){
    int count =0;
    int* binaryNumber = new int[8];

    for(int i =-1;i<2;i++){
        for (int j=-1;j<2;j++){
            if (i!=0 && j!=0){
                if (image[(x+i)*nW+j+y]<image[x*nW+y]){
                    binaryNumber[count]=0;
                }
                if (image[(x+i)*nW+j+y]>=image[x*nW+y]){
                    binaryNumber[count]=1;
                }
                count++;
            }
        }
    }
    unsigned char res = (unsigned char)(1*binaryNumber[7] + 2*binaryNumber[6] + 4*binaryNumber[5] + 8*binaryNumber[4] + 16*binaryNumber[3] + 32*binaryNumber[2] + 64*binaryNumber[1] + 128*binaryNumber[0]);
    return (res);
}

double minutie(int* hist, int size,int nH, int nW, int gridX, int gridY){
    double res =0.0;
    for (int i =0;i<size;i++){
        res+=i*(double)hist[i]/((double)(nW*nH)/(double)(gridY*gridX));//(double)size;
    }
    return res;
}

double divergenceLK(int* hist1, int* hist2, int size){
    double res = 0.0;
    for (int k=0;k<size;k++){
        res+=(hist1[k]+1.0)*log((hist1[k]+1.0)/(hist2[k]+1.0));
    }
    return res;
}

int main(){

}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> test_double_D_D(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & M)
{
	return M;
}
void export_double_D_D()
{
	boost::python::def("test_double_D_D",test_double_D_D);
}


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>

#include <boost/algorithm/string.hpp>
#include <numeric>

typedef unsigned long long nombre;

ENREGISTRER_PROBLEME(59, "XOR decryption") {
    // Each character on a computer is assigned a unique code and the preferred standard is ASCII
    // (American Standard Code for Information Interchange). For example, uppercase A = 65, asterisk (*) = 42,
    // and lowercase k = 107.
    //
    // A modern encryption method is to take a text file, convert the bytes to ASCII, then XOR each byte
    // with a given value, taken from a secret key. The advantage with the XOR function is that using
    // the same encryption key on the cipher text, restores the plain text; for example, 65 XOR 42 = 107,
    // then 107 XOR 42 = 65.
    //
    // For unbreakable encryption, the key is the same length as the plain text message, and the key is made
    // up of random bytes. The user would keep the encrypted message and the encryption key in different
    // locations, and without both "halves", it is impossible to decrypt the message.
    //
    // Unfortunately, this method is impractical for most users, so the modified method is to use a password
    // as a key. If the password is shorter than the message, which is likely, the key is repeated cyclically
    // throughout the message. The balance for this method is using a sufficiently long password key for security,
    // but short enough to be memorable.
    //
    // Your task has been made easy, as the encryption key consists of three lower case characters.
    // Using cipher.txt (right click and 'Save Link/Target As...'), a file containing the encrypted ASCII codes,
    // and the knowledge that the plain text must contain common English words, decrypt the message and find
    // the sum of the ASCII values in the original text.
    std::ifstream ifs("data/p059_cipher.txt");
    std::string entree;
    ifs >> entree;
    std::vector<std::string> names;
    boost::split(names, entree, boost::is_any_of(","));

    std::set<char> lettres = {' ', ',', '(', ')', '[', ']', '.', '!', '\'', ';', '"', '+', '-', '/', '*', ':'};
    for (char c = 'a'; c <= 'z'; ++c) lettres.insert(c);
    for (char c = 'A'; c <= 'Z'; ++c) lettres.insert(c);
    for (char c = '0'; c <= '9'; ++c) lettres.insert(c);

    std::vector<char> data;
    std::transform(names.begin(), names.end(), std::back_inserter(data),
                   [](const std::string &str) { return std::stoi(str); });

    std::string message;
    for (char key1 = 'a'; key1 <= 'z'; ++key1) {
        if (lettres.find(data[0] ^ key1) == lettres.end())
            continue;
        for (char key2 = 'a'; key2 <= 'z'; ++key2) {
            if (lettres.find(data[1] ^ key2) == lettres.end())
                continue;
            for (char key3 = 'a'; key3 <= 'z'; ++key3) {
                if (lettres.find(data[2] ^ key3) == lettres.end())
                    continue;

                std::string key = {key1, key2, key3};
                std::string decode;
                for (size_t n = 0; n < data.size(); ++n) {
                    const char c = data[n] ^key[n % 3];
                    if (lettres.find(c) == lettres.end())
                        break;
                    decode.push_back(c);
                }
                if (decode.size() == data.size()) {
                    message = decode;
                    std::cout << key << ": " << decode << std::endl;
                }
            }
        }
    }

    nombre resultat = std::accumulate(message.begin(), message.end(), 0ULL,
                                      [](const nombre r, const char c) { return r + static_cast<nombre>(c); }
    );
    return std::to_string(resultat);
}


#include "stdafx.h"
#include "KeplerLawSystem.h"
#include "TimeController.h"
#include "Scale.h"
#include <unordered_set>
#include <unordered_map>
#include <boost/algorithm/cxx11/copy_if.hpp>

using glm::vec2;
using glm::vec3;

CKeplerLawSystem::CKeplerLawSystem(ITimeController &controller)
    : m_timeController(controller)
{
}

void CKeplerLawSystem::Update()
{
    const double time = m_timeController.GetSpaceTime();

    // Имена объектов, орбиты которых ещё следует обновить.
    // Используются для разрешения зависимостей при систем
    //  типа Солнце->Земля->Луна.
    std::unordered_set<std::string> remainingNames;
    for (const auto &entity : getEntities())
    {
        const auto &body = entity.getComponent<CSpaceBodyComponent>();
        remainingNames.insert(body.m_name);
    }

    // Словарь с известным позициями объектов,
    //  используется для расчёта позиций спутников.
    std::unordered_map<std::string, vec3> knownPositions;

    // Прекращаем обновлять позиции, если ничего не изменилось
    //  за один обход всех космических тел.
    bool mayContinue = false;
    do
    {
        mayContinue = false;
        for (const auto &entity : getEntities())
        {
            const auto &body = entity.getComponent<CSpaceBodyComponent>();
            auto &orbit = entity.getComponent<CEllipticOrbitComponent>();

            // Пропускаем итерацию, если спутник уже обновлён,
            //  либо не обновлён владелец спутника.
            if (!remainingNames.count(body.m_name) || remainingNames.count(orbit.m_ownerName))
            {
                continue;
            }
            mayContinue = true;

            const vec2 pos2D = orbit.PlanetPosition2D(time);
            const vec3 pos3D = scale::AU_SIZE * vec3(pos2D.x, 0.f, pos2D.y);
            const vec3 ownerPos3D = knownPositions[orbit.m_ownerName];

            auto &transform = entity.getComponent<CTransformComponent>();
            transform.m_position = pos3D + ownerPos3D;

            knownPositions[body.m_name] = transform.m_position;
            remainingNames.erase(body.m_name);
        }
    }
    while (mayContinue);

    // Если ещё есть необновлённые объекты, значит,
    //  есть циклические зависимости.
    if (!remainingNames.empty())
    {
        throw std::runtime_error("Cannot resolve orbit dependency cycle");
    }
}


#pragma once

#include "types.hpp"
#include <Eigen/Dense>

/// Compute the gradients of q_bar.
/** Note: this routine does not care if q are the conserved
 *  or primitive variables.
 *
 * @param [out] dqdx  approximation of dq/dx. Has shape (n_cells, 4).
 * @param [out] dqdy  approximation of dq/dy. Has shape (n_cells, 4).
 * @param       q_bar the cell-averages of `q`. Has shape (n_cells, 4).
 * @param       mesh
 */
void compute_gradients(Eigen::MatrixXd &dqdx,
                       Eigen::MatrixXd &dqdy,
                       const Eigen::MatrixXd &q_bar,
                       const Mesh &mesh) {

    // Compute the gradient of all 4 components of q_bar
}


/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   errorTerms.cpp
 * @brief
 * @author Francisco José Mañas Álvarez, Jose Luis Blanco Claraco
 * @date   Aug 4, 2020
 */

#include <mp2p_icp/errorTerms.h>
#include <mp2p_icp/optimal_tf_gauss_newton.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>  // dotProduct()
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>

#include <Eigen/Dense>
#include <iostream>

using namespace mp2p_icp;
using namespace mrpt::math;

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2point(
    const mrpt::tfest::TMatchingPair&                           pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;
    const mrpt::math::TPoint3D&       l = pairing.local;

    const mrpt::math::TPoint3D g = relativePose.composePoint(l);

    error[0] = g.x - pairing.global.x;
    error[1] = g.y - pairing.global.y;
    error[2] = g.z - pairing.global.z;

    // It's possible change the error to scalar with the function
    // g.DistanceTo(l) Eval Jacobian:
    if (jacobian)
    {
        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
        // clang-format off
        J_aux = (Eigen::Matrix<double, 3, 12>() <<
                 l.x,   0,   0, l.y,   0,    0, l.z,   0,   0,  1,  0,  0,
                   0, l.x,   0,   0, l.y,    0,   0, l.z,   0,  0,  1,  0,
                   0,   0, l.x,   0,   0,  l.y,   0,   0, l.z,  0,  0,  1
                 ).finished();
        // clang-format on
    }

    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2line(
    const mp2p_icp::point_line_pair_t&                          pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;
    const auto&                       lnGlob = pairing.ln_global;

    // local and global point:
    const mrpt::math::TPoint3D l = pairing.pt_local;
    const mrpt::math::TPoint3D g = relativePose.composePoint(l);

    // Module of vector director of line
    const auto& u = lnGlob.director;
    const auto  q = (g - lnGlob.pBase);

    const auto uq = mrpt::math::dotProduct<3, double>(u, q);

    error[0] = q[0] - u.x * uq;
    error[1] = q[1] - u.y * uq;
    error[2] = q[2] - u.z * uq;

    if (jacobian)
    {
        // J1
        const Eigen::Matrix<double, 3, 3> J1 =
            (Eigen::Matrix<double, 3, 3>() << 1 - mrpt::square(u.x), -u.x * u.y,
             -u.x * u.z, -u.y * u.x, 1 - mrpt::square(u.y), -u.y * u.z,
             -u.z * u.x, -u.z * u.y, 1 - mrpt::square(u.z))
                .finished();
        // J2
        // clang-format off
        Eigen::Matrix<double, 3, 12> J2 =
            (Eigen::Matrix<double, 3, 12>() <<
             l.x,   0,   0, l.y,   0,    0, l.z,   0,   0,  1,  0,  0,
               0, l.x,   0,   0, l.y,    0,   0, l.z,   0,  0,  1,  0,
               0,   0, l.x,   0,   0,  l.y,   0,   0, l.z,  0,  0,  1
             ).finished();
        // clang-format on
        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();

        J_aux = J1 * J2;
    }
    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2plane(
    const mp2p_icp::point_plane_pair_t&                         pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;
    const auto&                       p      = pairing.pt_local;
    const auto&                       pl_aux = pairing.pl_global.plane;
    const mrpt::math::TPoint3D        l      = TPoint3D(p.x, p.y, p.z);
    mrpt::math::TPoint3D              g;
    relativePose.composePoint(l, g);
    double mod_n = mrpt::square(pl_aux.coefs[0]) +
                   mrpt::square(pl_aux.coefs[1]) +
                   mrpt::square(pl_aux.coefs[2]);

    error[0] = -(pl_aux.coefs[0] / mod_n) *
               (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
                pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
    error[1] = -(pl_aux.coefs[1] / mod_n) *
               (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
                pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
    error[2] = -(pl_aux.coefs[2] / mod_n) *
               (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
                pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
    if (jacobian)
    {
        // Eval Jacobian:
        // J1
        const Eigen::Matrix<double, 3, 3> J1 =
            (Eigen::Matrix<double, 3, 3>()
                 << -mrpt::square(pl_aux.coefs[0]) / mod_n,
             -pl_aux.coefs[0] * pl_aux.coefs[1] / mod_n,
             -pl_aux.coefs[0] * pl_aux.coefs[2] / mod_n,
             -pl_aux.coefs[1] * pl_aux.coefs[0] / mod_n,
             -mrpt::square(pl_aux.coefs[1]) / mod_n,
             -pl_aux.coefs[1] * pl_aux.coefs[2] / mod_n,
             -pl_aux.coefs[2] * pl_aux.coefs[0] / mod_n,
             -pl_aux.coefs[2] * pl_aux.coefs[1] / mod_n,
             -mrpt::square(pl_aux.coefs[2]) / mod_n)
                .finished();
        // J2
        // clang-format off
        const Eigen::Matrix<double, 3, 12> J2 =
            (Eigen::Matrix<double, 3, 12>() <<
             l.x,   0,   0, l.y,   0,    0, l.z,   0,   0,  1,  0,  0,
               0, l.x,   0,   0, l.y,    0,   0, l.z,   0,  0,  1,  0,
               0,   0, l.x,   0,   0,  l.y,   0,   0, l.z,  0,  0,  1
             ).finished();
        // clang-format on

        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
        J_aux                                          = J1 * J2;
    }
    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<4> mp2p_icp::error_line2line(
    const mp2p_icp::matched_line_t&                             pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<4> error;
    mrpt::math::TLine3D               ln_aux;
    mrpt::math::TPoint3D              g;

    const auto& p0 = pairing.ln_local.pBase;
    const auto& u0 = pairing.ln_local.director;
    const auto& p1 = pairing.ln_global.pBase;
    const auto& u1 = pairing.ln_global.director;

    relativePose.composePoint(p0, g);
    ln_aux.pBase = mrpt::math::TPoint3D(g);

    // Homogeneous matrix calculation
    mrpt::math::CMatrixDouble44 aux;
    relativePose.getHomogeneousMatrix(aux);
    const Eigen::Matrix<double, 4, 4> T = aux.asEigen();

    // Projection of the director vector for the new pose
    const Eigen::Matrix<double, 1, 4> U =
        (Eigen::Matrix<double, 1, 4>() << u0[0], u0[1], u0[2], 1).finished();
    const Eigen::Matrix<double, 1, 4> U_T = U * T;
    ln_aux.director                       = {U_T[0], U_T[1], U_T[2]};

    // Angle formed between the lines
    double alfa = getAngle(pairing.ln_global, ln_aux) * 180 / (2 * 3.14159265);
    /*
        std::cout << "\nLine 1:\n"
                  <<  pairing.ln_this << "\nLine 2:\n"
                   << pairing.ln_other << "\nLine 2':\n"
                   << ln_aux << "\nAngle:\n"
                   << alfa << "\nT:\n"
                   <<  T << "\n";
    */

    // p_r0 = (p-r_{0,r}). Ec.20
    const Eigen::Matrix<double, 1, 3> p_r2 =
        (Eigen::Matrix<double, 1, 3>() << ln_aux.pBase.x - p1.x,
         ln_aux.pBase.y - p1.y, ln_aux.pBase.z - p1.z)
            .finished();

    const Eigen::Matrix<double, 1, 3> rv =
        (Eigen::Matrix<double, 1, 3>() << u1[0], u1[1], u1[2]).finished();

    // Relationship between lines
    const double tolerance = 0.01;
    if (abs(alfa) < tolerance)
    {  // Parallel
        // Error: Ec.20
        error[0] = mrpt::square(pairing.ln_global.distance(ln_aux.pBase));
        if (jacobian)
        {
            // Module of vector director of line
            double mod_rv = rv * rv.transpose();

            // J1: Ec.22
            Eigen::Matrix<double, 1, 3> J1 =
                2 * p_r2 - (2 / mod_rv) * (p_r2 * rv.transpose()) * rv;
            // J2: Ec.23
            // clang-format off
            const Eigen::Matrix<double, 3, 12> J2 =
                (Eigen::Matrix<double, 3, 12>() <<
                 p0.x,    0,    0, p0.y,    0,    0, p0.z,    0,    0, 1, 0, 0,
                    0, p0.x,    0,    0, p0.y,    0,    0, p0.z,    0, 0, 1, 0,
                    0,    0, p0.x,    0,    0, p0.y,    0,    0, p0.z, 0, 0, 1
                 ).finished();
            // clang-format on
            // Build Jacobian
            mrpt::math::CMatrixFixed<double, 4, 12>& J_auxp =
                jacobian.value().get();
            J_auxp.block<1, 12>(0, 0) = J1 * J2;
        }
    }
    else
    {  // Rest
        // Error:
        // Cross product (r_u x r_2,v)
        const double rw_x = U_T[1] * u1[2] - U_T[2] * u1[1];
        const double rw_y = -(U_T[0] * u1[2] - U_T[2] * u1[0]);
        const double rw_z = U_T[0] * u1[1] - U_T[1] * u1[0];

        const Eigen::Matrix<double, 1, 3> r_w =
            (Eigen::Matrix<double, 1, 3>() << rw_x, rw_y, rw_z).finished();
        double aux_rw = r_w * r_w.transpose();
        // Error 1. Ec.26
        error[0] = p_r2.dot(r_w) / sqrt(aux_rw);
        // Error 2. Ec.27
        error[1] = U_T[0] - u1[0];
        error[2] = U_T[1] - u1[1];
        error[3] = U_T[2] - u1[2];
        if (jacobian)
        {
            // J1.1: Ec.32
            Eigen::Matrix<double, 1, 3> J1_1 = r_w / sqrt(aux_rw);

            // J1.2:
            // A
            const double A =
                p_r2[0] * r_w[0] + p_r2[1] * r_w[1] + p_r2[2] * r_w[2];
            const double Ax = -u1[2] * p_r2[1] + u1[1] * p_r2[2];
            const double Ay = u1[2] * p_r2[0] - u1[0] * p_r2[2];
            const double Az = -u1[1] * p_r2[0] + u1[0] * p_r2[1];
            // B
            const double B  = sqrt(aux_rw);
            const double Bx = (-u1[2] * r_w[1] + u1[1] * r_w[2]) / B;
            const double By = (u1[2] * r_w[0] + u1[0] * r_w[2]) / B;
            const double Bz = (-u1[1] * r_w[0] + u1[0] * r_w[1]) / B;

#if 0
            std::cout << "\nA: " << A << "\nAx: " << Ax << "\nAy: " << Ay
                      << "\nAz: " << Az << "\nB: " << B << "\nBx: " << Bx
                      << "\nBy: " << By << "\nBz: " << Bz << "\n";
#endif

            // Ec.36
            // clang-format off
            Eigen::Matrix<double, 1, 3> J1_2 =
                (Eigen::Matrix<double, 1, 3>() <<
                 (Ax * B - A * Bx) / mrpt::square(B),
                 (Ay * B - A * By) / mrpt::square(B),
                 (Az * B - A * Bz) / mrpt::square(B)
                 ).finished();
            // clang-format on

            // J1.3: Ec.37-38
            // clang-format off
            Eigen::Matrix<double, 3, 6> J1_3 =
                (Eigen::Matrix<double, 3, 6>() <<
                 0, 0, 0, 1, 0, 0,
                 0, 0, 0, 0, 1, 0,
                 0, 0, 0, 0, 0, 1
                 ).finished();
            // clang-format on

            // J1: Ec.29
            Eigen::Matrix<double, 4, 6> J1;
            J1.block<1, 3>(0, 0) = J1_1;
            J1.block<1, 3>(0, 3) = J1_2;
            J1.block<3, 6>(1, 0) = J1_3;

            // J2: Ec.39-41
            // clang-format off
            const Eigen::Matrix<double, 6, 12> J2 =
                (Eigen::Matrix<double, 6, 12>() <<
                 p0.x,     0,     0,  p0.y,     0,     0,  p0.z,     0,     0, 1, 0, 0,
                    0,  p0.x,     0,     0,  p0.y,     0,     0,  p0.z,     0, 0, 1, 0,
                    0,     0,  p0.x,     0,     0,  p0.y,     0,     0,  p0.z, 0, 0, 1,
                -u0[0],     0,     0, -u0[1],     0,     0, -u0[2],     0,     0, 0, 0, 0,
                    0, -u0[0],     0,     0, -u0[1],     0,     0, -u0[2],     0, 0, 0, 0,
                    0,     0, -u0[0],     0,     0, -u0[1],     0,     0, -u0[2], 0, 0, 0
                ).finished();
            // clang-format on
            // Build Jacobian
            mrpt::math::CMatrixFixed<double, 4, 12>& J_aux =
                jacobian.value().get();
            J_aux.block<4, 12>(0, 0) = J1 * J2;

            std::cout << "\nJ1:\n" << J1 << "\nJ2:\n" << J2 << "\n";
        }
    }
    //    std::cout<<"\nError:\n"<<error;
    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_plane2plane(
    const mp2p_icp::matched_plane_t&                            pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;

    const auto nl = pairing.p_local.plane.getNormalVector();
    const auto ng = pairing.p_global.plane.getNormalVector();

    const auto p_oplus_nl = relativePose.rotateVector(nl);

    for (int i = 0; i < 3; i++) error[i] = p_oplus_nl[i] - ng[i];

    if (jacobian)
    {
        // Eval Jacobian:

        // df_oplus(A,p)/d_A. Section 7.3.2 tech. report:
        // "A tutorial on SE(3) transformation parameterizations and
        // on-manifold optimization"
        // Modified, to discard the last I_3 block, since this particular
        // cost function is insensible to translations.

        // clang-format off
        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
        J_aux = (Eigen::Matrix<double, 3, 12>() <<
                 nl.x,    0,    0, nl.y,    0,    0, nl.z,    0,    0,  0,  0,  0,
                    0, nl.x,    0,    0, nl.y,    0,    0, nl.z,    0,  0,  0,  0,
                    0,    0, nl.x,    0,    0, nl.y,    0,    0, nl.z,  0,  0,  0
                ).finished();
        // clang-format on
    }
    return error;
    MRPT_END
}


// Copyright (c) 2018 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE

#include <gtest/gtest.h>
//------------g alexander mac osx ---------------------//
#if defined (APPLE)
#include </opt/local/include/eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif

#include <franka/rate_limiting.h>

using namespace franka;

const double kNoLimit{std::numeric_limits<double>::max()};

std::array<double, 7> kJointsNoLimit{
    {kNoLimit, kNoLimit, kNoLimit, kNoLimit, kNoLimit, kNoLimit, kNoLimit}};

template <int size>
std::array<double, size> integrateOneSample(std::array<double, size> last_value,
                                            std::array<double, size> derivative,
                                            double delta_t) {
  std::array<double, size> result{};
  for (size_t i = 0; i < size; i++) {
    result[i] = last_value[i] + derivative[i] * delta_t;
  }
  return result;
}

template <int size>
std::array<double, size> differentiateOneSample(std::array<double, size> value,
                                                std::array<double, size> last_value,
                                                double delta_t) {
  std::array<double, size> result{};
  for (size_t i = 0; i < size; i++) {
    result[i] = (value[i] - last_value[i]) / delta_t;
  }
  return result;
}

std::array<double, 16> integrateOneSample(std::array<double, 16> last_pose,
                                          std::array<double, 6> twist,
                                          double delta_t) {
  Eigen::Affine3d pose(Eigen::Matrix4d::Map(last_pose.data()));
  Eigen::Map<Eigen::Matrix<double, 6, 1>> dx(twist.data());
  Eigen::Matrix3d omega_skew;
  omega_skew << 0, -dx[5], dx[4], dx[5], 0, -dx[3], -dx[4], dx[3], 0;
  pose.linear() << pose.linear() + omega_skew * pose.linear() * delta_t;
  pose.translation() << pose.translation() + dx.head(3) * delta_t;

  std::array<double, 16> pose_after_integration{};
  Eigen::Map<Eigen::Matrix4d>(&pose_after_integration[0], 4, 4) = pose.matrix();
  return pose_after_integration;
}

std::array<double, 6> differentiateOneSample(std::array<double, 16> value,
                                             std::array<double, 16> last_value,
                                             double delta_t) {
  Eigen::Affine3d pose(Eigen::Matrix4d::Map(value.data()));
  Eigen::Affine3d last_pose(Eigen::Matrix4d::Map(last_value.data()));
  Eigen::Matrix<double, 6, 1> dx;

  dx.head(3) << (pose.translation() - last_pose.translation()) / delta_t;
  auto delta_rotation = (pose.linear() - pose.linear()) / delta_t;
  Eigen::Matrix3d rotational_twist = delta_rotation * last_pose.linear();
  dx.tail(3) << rotational_twist(2, 1), rotational_twist(0, 2), rotational_twist(1, 0);

  std::array<double, 6> twist{};
  Eigen::Map<Eigen::Matrix<double, 6, 1>>(&twist[0], 6, 1) = dx;
  return twist;
}

bool violatesLimits(double desired_value, double max_value) {
  return std::abs(desired_value) > max_value;
}

bool violatesLimits(std::array<double, 7> values, std::array<double, 7> max_values) {
  bool violates_limits = false;
  for (size_t i = 0; i < 7 && !violates_limits; i++) {
    violates_limits = violates_limits || violatesLimits(values[i], max_values[i]);
  }
  return violates_limits;
}

bool violatesRateLimits(std::array<double, 7> max_derivatives,
                        std::array<double, 7> values,
                        std::array<double, 7> last_desired_values,
                        double delta_t) {
  return violatesLimits(differentiateOneSample<7>(values, last_desired_values, delta_t),
                        max_derivatives);
}

bool violatesRateLimits(std::array<double, 7> max_values,
                        std::array<double, 7> max_derivatives,
                        std::array<double, 7> max_dderivatives,
                        std::array<double, 7> values,
                        std::array<double, 7> last_values,
                        std::array<double, 7> last_dvalues,
                        double delta_t) {
  std::array<double, 7> desired_derivatives =
      differentiateOneSample<7>(values, last_values, delta_t);

  return violatesLimits(values, max_values) ||
         violatesRateLimits(max_derivatives, values, last_values, delta_t) ||
         violatesRateLimits(max_dderivatives, desired_derivatives, last_dvalues, delta_t);
}

bool violatesRateLimits(double max_translational_dx,
                        double max_translational_ddx,
                        double max_translational_dddx,
                        double max_rotational_dx,
                        double max_rotational_ddx,
                        double max_rotational_dddx,
                        std::array<double, 6> cmd_dx,
                        std::array<double, 6> O_dP_EE_c,
                        std::array<double, 6> O_ddP_EE_c,
                        double delta_t) {
  Eigen::Map<Eigen::Matrix<double, 6, 1>> dx(cmd_dx.data());
  Eigen::Map<Eigen::Matrix<double, 6, 1>> last_dx(O_dP_EE_c.data());
  Eigen::Map<Eigen::Matrix<double, 6, 1>> last_ddx(O_ddP_EE_c.data());
  Eigen::Matrix<double, 6, 1> ddx = (dx - last_dx) / delta_t;
  Eigen::Matrix<double, 6, 1> dddx = (ddx - last_ddx) / delta_t;
  return violatesLimits(dx.head(3).norm(), max_translational_dx) ||
         violatesLimits(ddx.head(3).norm(), max_translational_ddx) ||
         violatesLimits(dddx.head(3).norm(), max_translational_dddx) ||
         violatesLimits(dx.tail(3).norm(), max_rotational_dx) ||
         violatesLimits(ddx.tail(3).norm(), max_rotational_ddx) ||
         violatesLimits(dddx.tail(3).norm(), max_rotational_dddx);
}

std::array<double, 7> generateValuesIntoLimits(std::array<double, 7> last_cmd_values,
                                               std::array<double, 7> max_derivatives,
                                               double eps,
                                               double delta_t) {
  std::array<double, 7> cmd_value{};
  for (size_t i = 0; i < 7; i++) {
    // Make sure that the integration yields a value into limits
    cmd_value[i] = last_cmd_values[i] + (max_derivatives[i] - std::min(std::max(std::abs(eps), 0.0),
                                                                       2.0 * max_derivatives[i])) *
                                            delta_t;
  }
  return cmd_value;
}

std::array<double, 7> generateValuesOutsideLimits(std::array<double, 7> last_cmd_values,
                                                  std::array<double, 7> max_derivatives,
                                                  double eps,
                                                  double delta_t) {
  std::array<double, 7> cmd_value{};
  for (size_t i = 0; i < 7; i++) {
    // Make sure that diff yields a value outside limits
    cmd_value[i] =
        last_cmd_values[i] + (max_derivatives[i] + std::max(std::abs(eps), kLimitEps)) * delta_t;
  }
  return cmd_value;
}

std::array<double, 6> generateValuesIntoLimits(std::array<double, 6> last_cmd_values,
                                               double max_translational_derivative,
                                               double max_rotational_derivative,
                                               double eps,
                                               double delta_t) {
  Eigen::Map<Eigen::Matrix<double, 6, 1>> last_values(last_cmd_values.data());
  Eigen::Matrix<double, 6, 1> values;
  Eigen::Vector3d unit_vector(1.0, 0.0, 0.0);
  std::array<double, 6> result;
  values.head(3) << last_values.head(3) + unit_vector *
                                              (max_translational_derivative -
                                               std::min(std::max(std::abs(eps), 0.0),
                                                        2.0 * max_translational_derivative)) *
                                              delta_t;
  values.tail(3) << last_values.tail(3) + unit_vector *
                                              (max_rotational_derivative -
                                               std::min(std::max(std::abs(eps), 0.0),
                                                        2.0 * max_rotational_derivative)) *
                                              delta_t;

  Eigen::Matrix<double, 6, 1>::Map(&result[0], 6) = values;
  return result;
}

std::array<double, 6> generateValuesOutsideLimits(std::array<double, 6> last_cmd_values,
                                                  double max_translational_derivative,
                                                  double max_rotational_derivative,
                                                  double eps,
                                                  double delta_t) {
  Eigen::Map<Eigen::Matrix<double, 6, 1>> last_values(last_cmd_values.data());
  Eigen::Matrix<double, 6, 1> values;
  Eigen::Vector3d unit_vector(1.0, 0.0, 0.0);
  std::array<double, 6> result;
  values.head(3) << last_values.head(3) +
                        unit_vector *
                            (max_translational_derivative + std::max(std::abs(eps), kLimitEps)) *
                            delta_t;
  values.tail(3) << last_values.tail(3) +
                        unit_vector *
                            (max_rotational_derivative + std::max(std::abs(eps), kLimitEps)) *
                            delta_t;

  Eigen::Matrix<double, 6, 1>::Map(&result[0], 6) = values;
  return result;
}

TEST(RateLimiting, MaxDerivative) {
  std::array<double, 7> max_derivatives{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}};
  std::array<double, 7> last_cmd_values{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  double eps{1e-2};

  // Desired values are into limits and unchanged after limitRate
  std::array<double, 7> values_into_limits =
      generateValuesIntoLimits(last_cmd_values, max_derivatives, eps, kDeltaT);
  ASSERT_FALSE(violatesRateLimits(max_derivatives, values_into_limits, last_cmd_values, kDeltaT));
  EXPECT_EQ(values_into_limits, limitRate(max_derivatives, values_into_limits, last_cmd_values));

  // Desired values are outside limits and limited after limitRate
  std::array<double, 7> values_outside_limits =
      generateValuesOutsideLimits(last_cmd_values, max_derivatives, eps, kDeltaT);
  std::array<double, 7> limited_values =
      limitRate(max_derivatives, values_outside_limits, last_cmd_values);
  ASSERT_TRUE(violatesRateLimits(max_derivatives, values_outside_limits, last_cmd_values, kDeltaT));
  EXPECT_NE(values_outside_limits, limited_values);
  EXPECT_FALSE(violatesRateLimits(max_derivatives, limited_values, last_cmd_values, kDeltaT));
}

TEST(RateLimiting, JointVelocity) {
  std::array<double, 7> last_cmd_velocity{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 7> last_cmd_acceleration{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 7> max_acceleration{{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}};
  std::array<double, 7> max_jerk{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}};
  double eps{1e-2};

  // Desired values are into limits and unchanged after limitRate (jerk)
  std::array<double, 7> joint_velocity_into_limits = integrateOneSample<7>(
      last_cmd_velocity, generateValuesIntoLimits(last_cmd_acceleration, max_jerk, eps, kDeltaT),
      kDeltaT);
  ASSERT_FALSE(violatesRateLimits(kJointsNoLimit, kJointsNoLimit, max_jerk,
                                  joint_velocity_into_limits, last_cmd_velocity,
                                  last_cmd_acceleration, kDeltaT));
  EXPECT_EQ(joint_velocity_into_limits,
            limitRate(kJointsNoLimit, kJointsNoLimit, max_jerk, joint_velocity_into_limits,
                      last_cmd_velocity, last_cmd_acceleration));

  // Desired values are into limits and unchanged after limitRate (acceleration)
  joint_velocity_into_limits =
      generateValuesIntoLimits(last_cmd_velocity, max_acceleration, eps, kDeltaT);
  ASSERT_FALSE(violatesRateLimits(kJointsNoLimit, max_acceleration, kJointsNoLimit,
                                  joint_velocity_into_limits, last_cmd_velocity,
                                  last_cmd_acceleration, kDeltaT));
  EXPECT_EQ(joint_velocity_into_limits,
            limitRate(kJointsNoLimit, max_acceleration, kJointsNoLimit, joint_velocity_into_limits,
                      last_cmd_velocity, last_cmd_acceleration));

  // Desired values are outside limits (jerk violation) and limited after limitRate
  std::array<double, 7> joint_velocity_outside_limits = integrateOneSample<7>(
      last_cmd_velocity, generateValuesOutsideLimits(last_cmd_acceleration, max_jerk, eps, kDeltaT),
      kDeltaT);
  std::array<double, 7> limited_joint_velocity =
      limitRate(kJointsNoLimit, kJointsNoLimit, max_jerk, joint_velocity_outside_limits,
                last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(kJointsNoLimit, kJointsNoLimit, max_jerk,
                                 joint_velocity_outside_limits, last_cmd_velocity,
                                 last_cmd_acceleration, kDeltaT));
  EXPECT_NE(joint_velocity_outside_limits, limited_joint_velocity);
  EXPECT_FALSE(violatesRateLimits(kJointsNoLimit, kJointsNoLimit, max_jerk, limited_joint_velocity,
                                  last_cmd_velocity, last_cmd_acceleration, kDeltaT));

  // Desired values are outside limits (acceleration violation) and limited after limitRate
  joint_velocity_outside_limits =
      generateValuesOutsideLimits(last_cmd_velocity, max_acceleration, eps, kDeltaT);
  limited_joint_velocity =
      limitRate(kJointsNoLimit, max_acceleration, kJointsNoLimit, joint_velocity_outside_limits,
                last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(kJointsNoLimit, max_acceleration, kJointsNoLimit,
                                 joint_velocity_outside_limits, last_cmd_velocity,
                                 last_cmd_acceleration, kDeltaT));
  EXPECT_NE(joint_velocity_outside_limits, limited_joint_velocity);
  EXPECT_FALSE(violatesRateLimits(kJointsNoLimit, max_acceleration, kJointsNoLimit,
                                  limited_joint_velocity, last_cmd_velocity, last_cmd_acceleration,
                                  kDeltaT));
}

TEST(RateLimiting, JointPosition) {
  std::array<double, 7> last_cmd_position{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 7> last_cmd_velocity{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 7> last_cmd_acceleration{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 7> max_acceleration{{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}};
  std::array<double, 7> max_jerk{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}};
  double eps{1e-2};

  // Desired values are into limits and unchanged after limitRate (jerk)
  std::array<double, 7> joint_position_into_limits = integrateOneSample<7>(
      last_cmd_position,
      integrateOneSample<7>(last_cmd_velocity,
                            generateValuesIntoLimits(last_cmd_acceleration, max_jerk, eps, kDeltaT),
                            kDeltaT),
      kDeltaT);
  ASSERT_FALSE(violatesRateLimits(
      kJointsNoLimit, kJointsNoLimit, max_jerk,
      differentiateOneSample<7>(joint_position_into_limits, last_cmd_position, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_EQ(joint_position_into_limits,
            limitRate(kJointsNoLimit, kJointsNoLimit, max_jerk, joint_position_into_limits,
                      last_cmd_position, last_cmd_velocity, last_cmd_acceleration));

  // Desired values are into limits and unchanged after limitRate (acceleration)
  joint_position_into_limits = integrateOneSample<7>(
      last_cmd_position,
      generateValuesIntoLimits(last_cmd_velocity, max_acceleration, eps, kDeltaT), kDeltaT);
  ASSERT_FALSE(violatesRateLimits(
      kJointsNoLimit, max_acceleration, kJointsNoLimit,
      differentiateOneSample<7>(joint_position_into_limits, last_cmd_position, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_EQ(joint_position_into_limits,
            limitRate(kJointsNoLimit, max_acceleration, kJointsNoLimit, joint_position_into_limits,
                      last_cmd_position, last_cmd_velocity, last_cmd_acceleration));

  // Desired values are outside limits (jerk violation) and limited after limitRate
  std::array<double, 7> joint_position_outside_limits = integrateOneSample<7>(
      last_cmd_position,
      integrateOneSample<7>(
          last_cmd_velocity,
          generateValuesOutsideLimits(last_cmd_acceleration, max_jerk, eps, kDeltaT), kDeltaT),
      kDeltaT);
  std::array<double, 7> limited_joint_position =
      limitRate(kJointsNoLimit, kJointsNoLimit, max_jerk, joint_position_outside_limits,
                last_cmd_position, last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(
      kJointsNoLimit, kJointsNoLimit, max_jerk,
      differentiateOneSample<7>(joint_position_outside_limits, last_cmd_position, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_NE(joint_position_outside_limits, limited_joint_position);
  EXPECT_FALSE(violatesRateLimits(
      kJointsNoLimit, kJointsNoLimit, max_jerk,
      differentiateOneSample<7>(limited_joint_position, last_cmd_position, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));

  // Desired values outside limits (acceleration violation) and limited after limitRate
  joint_position_outside_limits = integrateOneSample<7>(
      last_cmd_position,
      generateValuesOutsideLimits(last_cmd_velocity, max_acceleration, eps, kDeltaT), kDeltaT);
  limited_joint_position =
      limitRate(kJointsNoLimit, max_acceleration, kJointsNoLimit, joint_position_outside_limits,
                last_cmd_position, last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(
      kJointsNoLimit, max_acceleration, kJointsNoLimit,
      differentiateOneSample<7>(joint_position_outside_limits, last_cmd_position, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_NE(joint_position_outside_limits, limited_joint_position);
  EXPECT_FALSE(violatesRateLimits(
      kJointsNoLimit, max_acceleration, kJointsNoLimit,
      differentiateOneSample<7>(limited_joint_position, last_cmd_position, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
}

TEST(RateLimiting, CartesianVelocity) {
  std::array<double, 6> last_cmd_velocity{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 6> last_cmd_acceleration{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  double max_translational_acceleration{10.0};
  double max_translational_jerk{100.0};
  double max_rotational_acceleration{5.0};
  double max_rotational_jerk{50.0};
  double eps{1e-2};

  // Desired values are into limits and unchanged after limitRate (rotational and translational
  // jerk)
  std::array<double, 6> cartesian_velocity_into_limits =
      integrateOneSample<6>(last_cmd_velocity,
                            generateValuesIntoLimits(last_cmd_acceleration, max_translational_jerk,
                                                     max_rotational_jerk, eps, kDeltaT),
                            kDeltaT);
  ASSERT_FALSE(violatesRateLimits(kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit,
                                  max_rotational_jerk, cartesian_velocity_into_limits,
                                  last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_EQ(
      cartesian_velocity_into_limits,
      limitRate(kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit, max_rotational_jerk,
                cartesian_velocity_into_limits, last_cmd_velocity, last_cmd_acceleration));

  // Desired values are into limits and unchanged after limitRate (rotational and translational
  // acceleration)
  cartesian_velocity_into_limits = generateValuesIntoLimits(
      last_cmd_velocity, max_translational_acceleration, max_rotational_acceleration, eps, kDeltaT);
  ASSERT_FALSE(violatesRateLimits(
      kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit, max_rotational_acceleration,
      kNoLimit, cartesian_velocity_into_limits, last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_EQ(cartesian_velocity_into_limits,
            limitRate(kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit,
                      max_rotational_acceleration, kNoLimit, cartesian_velocity_into_limits,
                      last_cmd_velocity, last_cmd_acceleration));

  // Desired values are outside limits (rotational and translational jerk violation) and limited
  // after limitRate
  std::array<double, 6> cartesian_velocity_outside_limits = integrateOneSample<6>(
      last_cmd_velocity,
      generateValuesOutsideLimits(last_cmd_acceleration, max_translational_jerk,
                                  max_rotational_jerk, eps, kDeltaT),
      kDeltaT);
  std::array<double, 6> limited_cartesian_velocity =
      limitRate(kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit, max_rotational_jerk,
                cartesian_velocity_outside_limits, last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit,
                                 max_rotational_jerk, cartesian_velocity_outside_limits,
                                 last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_NE(cartesian_velocity_outside_limits, limited_cartesian_velocity);
  EXPECT_FALSE(violatesRateLimits(kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit,
                                  max_rotational_jerk, limited_cartesian_velocity,
                                  last_cmd_velocity, last_cmd_acceleration, kDeltaT));

  // Desired values are outside limits (rotational and translational acceleration violation) and
  // limited after limitRate
  cartesian_velocity_outside_limits = generateValuesOutsideLimits(
      last_cmd_velocity, max_translational_acceleration, max_rotational_acceleration, eps, kDeltaT);
  limited_cartesian_velocity = limitRate(
      kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit, max_rotational_acceleration,
      kNoLimit, cartesian_velocity_outside_limits, last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit,
                                 max_rotational_acceleration, kNoLimit,
                                 cartesian_velocity_outside_limits, last_cmd_velocity,
                                 last_cmd_acceleration, kDeltaT));
  EXPECT_NE(cartesian_velocity_outside_limits, limited_cartesian_velocity);
  EXPECT_FALSE(violatesRateLimits(kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit,
                                  max_rotational_acceleration, kNoLimit, limited_cartesian_velocity,
                                  last_cmd_velocity, last_cmd_acceleration, kDeltaT));
}

TEST(RateLimiting, CartesianPose) {
  std::array<double, 16> last_cmd_pose{
      {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}};
  std::array<double, 6> last_cmd_velocity{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 6> last_cmd_acceleration{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
  double max_translational_acceleration{10.0};
  double max_translational_jerk{100.0};
  double max_rotational_acceleration{5.0};
  double max_rotational_jerk{50.0};
  double eps{1e-2};

  // Desired values are into limits and unchanged after limitRate (rotational and translational
  // jerk)
  std::array<double, 16> cartesian_pose_into_limits = integrateOneSample(
      last_cmd_pose,
      integrateOneSample<6>(last_cmd_velocity,
                            generateValuesIntoLimits(last_cmd_acceleration, max_translational_jerk,
                                                     max_rotational_jerk, eps, kDeltaT),
                            kDeltaT),
      kDeltaT);
  ASSERT_FALSE(violatesRateLimits(
      kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit, max_rotational_jerk,
      differentiateOneSample(cartesian_pose_into_limits, last_cmd_pose, kDeltaT), last_cmd_velocity,
      last_cmd_acceleration, kDeltaT));

  std::array<double, 16> cartesian_pose_limited = limitRate(
      kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit, max_rotational_jerk,
      cartesian_pose_into_limits, last_cmd_pose, last_cmd_velocity, last_cmd_acceleration);

  for (size_t i = 0; i < cartesian_pose_into_limits.size(); i++) {
    EXPECT_NEAR(cartesian_pose_into_limits[i], cartesian_pose_limited[i], 1e-6);
  }

  // Desired values are into limits and unchanged after limitRate (rotational and translational
  // acceleration)
  cartesian_pose_into_limits =
      integrateOneSample(last_cmd_pose,
                         generateValuesIntoLimits(last_cmd_velocity, max_translational_acceleration,
                                                  max_rotational_acceleration, eps, kDeltaT),
                         kDeltaT);
  ASSERT_FALSE(violatesRateLimits(
      kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit, max_rotational_acceleration,
      kNoLimit, differentiateOneSample(cartesian_pose_into_limits, last_cmd_pose, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));

  cartesian_pose_limited =
      limitRate(kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit,
                max_rotational_acceleration, kNoLimit, cartesian_pose_into_limits, last_cmd_pose,
                last_cmd_velocity, last_cmd_acceleration);

  for (size_t i = 0; i < cartesian_pose_into_limits.size(); i++) {
    EXPECT_NEAR(cartesian_pose_into_limits[i], cartesian_pose_limited[i], 1e-6);
  }

  // Desired values are outside limits (rotational and translational jerk violation) and limited
  // after limitRate
  std::array<double, 16> cartesian_pose_outside_limits = integrateOneSample(
      last_cmd_pose,
      integrateOneSample<6>(
          last_cmd_velocity,
          generateValuesOutsideLimits(last_cmd_acceleration, max_translational_jerk,
                                      max_rotational_jerk, eps, kDeltaT),
          kDeltaT),
      kDeltaT);
  std::array<double, 16> limited_cartesian_pose = limitRate(
      kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit, max_rotational_jerk,
      cartesian_pose_outside_limits, last_cmd_pose, last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(
      kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit, max_rotational_jerk,
      differentiateOneSample(cartesian_pose_outside_limits, last_cmd_pose, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_NE(cartesian_pose_outside_limits, limited_cartesian_pose);
  EXPECT_FALSE(violatesRateLimits(
      kNoLimit, kNoLimit, max_translational_jerk, kNoLimit, kNoLimit, max_rotational_jerk,
      differentiateOneSample(limited_cartesian_pose, last_cmd_pose, kDeltaT), last_cmd_velocity,
      last_cmd_acceleration, kDeltaT));

  // Desired values are outside limits (rotational and translational acceleration violation) and
  // limited after limitRate
  cartesian_pose_outside_limits = integrateOneSample(
      last_cmd_pose,
      generateValuesOutsideLimits(last_cmd_velocity, max_translational_acceleration,
                                  max_rotational_acceleration, eps, kDeltaT),
      kDeltaT);
  limited_cartesian_pose =
      limitRate(kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit,
                max_rotational_acceleration, kNoLimit, cartesian_pose_outside_limits, last_cmd_pose,
                last_cmd_velocity, last_cmd_acceleration);
  ASSERT_TRUE(violatesRateLimits(
      kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit, max_rotational_acceleration,
      kNoLimit, differentiateOneSample(cartesian_pose_outside_limits, last_cmd_pose, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
  EXPECT_NE(cartesian_pose_outside_limits, limited_cartesian_pose);
  EXPECT_FALSE(violatesRateLimits(
      kNoLimit, max_translational_acceleration, kNoLimit, kNoLimit, max_rotational_acceleration,
      kNoLimit, differentiateOneSample(limited_cartesian_pose, last_cmd_pose, kDeltaT),
      last_cmd_velocity, last_cmd_acceleration, kDeltaT));
}


/*
 * fusion_algebra.hpp
 *
 * Copyright 2009-2012 Karsten Ahnert
 * Copyright 2009-2012 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef FUSION_ALGEBRA_HPP_
#define FUSION_ALGEBRA_HPP_

#include <boost/array.hpp>

#include <iostream>

template< size_t n >
struct fusion_algebra
{
    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , n > &a ,
            const boost::array< T , dim > k_vector[n] , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i];// + a[0]*dt*k_vector[0][i];
            for( size_t j = 0 ; j<n ; ++j )
                x_tmp[i] += a[j]*dt*k_vector[j][i];
        }
    }

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp ,
                const boost::array< double , n > &a ,
                const boost::array< T , dim > k_vector[n] , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = a[0]*dt*k_vector[0][i];
            for( size_t j = 1 ; j<n ; ++j )
                x_tmp[i] += a[j]*dt*k_vector[j][i];
         }
    }

};

/** hand-wise implementation for performance improvement for n = 1..4 **/

/* !!!!!!!   Actually, this is factor 3 slower with intel compiler, so we don'y use it !!!!!
 * Update: It increases performance on msvc 9.0 by about 30%, so it is activated for MSVC
 */

//#ifdef BOOST_MSVC

template<>
struct fusion_algebra< 1 >
{
    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 1 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
                + a[0]*dt*k_vector[0][i];
        }
    }

};

template<>
struct fusion_algebra< 2 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 2 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i];
        }
    }

};

template<>
struct fusion_algebra< 3 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 3 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i];
        }
    }

};

template<>
struct fusion_algebra< 4 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 4 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i]
             + a[3]*dt*k_vector[3][i];
        }
    }

};

template<>
struct fusion_algebra< 5 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 5 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i]
             + a[3]*dt*k_vector[3][i]
             + a[4]*dt*k_vector[4][i];
        }
    }

};

template<>
struct fusion_algebra< 6 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 6 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i]
             + a[3]*dt*k_vector[3][i]
             + a[4]*dt*k_vector[4][i]
             + a[5]*dt*k_vector[5][i];
        }
    }

    template< typename T , size_t dim >
    inline static void foreach(boost::array<T , dim> &x_tmp ,
            const boost::array<double , 6> &a ,
            const boost::array<T , dim> *k_vector , const double dt)
    {
        for (size_t i = 0 ; i < dim ; ++i)
        {
            x_tmp[i] = a[0] * dt * k_vector[0][i] + a[1] * dt * k_vector[1][i]
                    + a[2] * dt * k_vector[2][i] + a[3] * dt * k_vector[3][i]
                    + a[4] * dt * k_vector[4][i] + a[5] * dt * k_vector[5][i];
        }
    }

};

//#endif /* BOOST_MSVC */

#endif /* FUSION_ALGEBRA_HPP_ */


// test file for quaternion.hpp

//  (C) Copyright Hubert Holin 2001.
//  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 <boost/math/quaternion.hpp>

typedef boost::math::quaternion<double> qt;
typedef std::complex<double> ct;

#ifndef BOOST_NO_CXX14_CONSTEXPR

constexpr qt full_constexpr_test(qt a, qt b, double d, ct c)
{
   a.swap(b);
   qt result(a), t;
   result += d;
   result += c;
   result += b;
   t = result;
   t = d;
   t = c;
   result -= d;
   result -= c;
   result -= a;
   result *= d;
   result *= c;
   result *= a;
   result /= d;
   result /= c;
   result /= b;

   result += a + d;
   result += d + a;
   result += a + c;
   result += c + a;
   result += a + b;

   result += a - d;
   result += d - a;
   result += a - c;
   result += c - a;
   result += a - b;

   result += a * d;
   result += d * a;
   result += a * c;
   result += c * a;
   result += a * b;

   result += a / d;
   result += d / a;
   result += a / c;
   result += c / a;
   result += a / b;

   result += norm(a);
   result += conj(a);

   return result;
}

#endif

int main()
{
#ifndef BOOST_NO_CXX11_CONSTEXPR

   constexpr qt q1;
   constexpr qt q2(2.0);
   constexpr qt q3(2.0, 3.0);
   constexpr qt q4(2.0, 3.9, 3.0);
   constexpr qt q5(2.0, 3.9, 3.0, 5.);

   constexpr ct c1(2., 3.);
   constexpr qt q6(c1);
   constexpr qt q7(c1, c1);
   constexpr qt q8(q1);

   constexpr double d1 = q5.real();
   constexpr qt q9 = q1.unreal();
   constexpr double d2 = q1.R_component_1();
   constexpr double d3 = q1.R_component_2();
   constexpr double d4 = q1.R_component_3();
   constexpr double d5 = q1.R_component_4();
   constexpr ct c2 = q1.C_component_1();
   constexpr ct c3 = q1.C_component_1();

   constexpr qt q10 = q1 + d1;
   constexpr qt q11 = d1 + q1;
   constexpr qt q12 = c2 + q1;
   constexpr qt q13 = q1 + c2;
   constexpr qt q14 = q1 + q2;

   constexpr qt q15 = q1 - d1;
   constexpr qt q16 = d1 - q1;
   constexpr qt q17 = c2 - q1;
   constexpr qt q18 = q1 - c2;
   constexpr qt q19 = q1 - q2;

   constexpr qt q20 = q1 * d1;
   constexpr qt q21 = d1 * q1;
   constexpr qt q22 = q5 / d1;

   constexpr double d6 = real(q5);
   constexpr qt q23 = unreal(q1);

   constexpr bool b1 = q1 == d1;
   constexpr bool b2 = d1 == q1;
   constexpr bool b3 = q1 != d1;
   constexpr bool b4 = d1 != q1;

   constexpr bool b5 = q1 == c2;
   constexpr bool b6 = c2 == q1;
   constexpr bool b7 = q1 != c2;
   constexpr bool b8 = c2 != q1;
   constexpr bool b9 = q2 == q1;
   constexpr bool b10 = q1 != q2;

#endif

#ifndef BOOST_NO_CXX14_CONSTEXPR

   constexpr qt q24 = full_constexpr_test(q5, q5 + 1, 3.2, q5.C_component_1());

#endif

   return 0;
}