/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_FUNDAMENTAL_HPP
#define PIC_COMPUTER_VISION_FUNDAMENTAL_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../image.hpp"

#include "../filtering/filter_luminance.hpp"
#include "../filtering/filter_gaussian_2d.hpp"

#include "../util/math.hpp"
#include "../util/eigen_util.hpp"

#include "../features_matching/orb_descriptor.hpp"
#include "../features_matching/feature_matcher.hpp"
#include "../features_matching/binary_feature_lsh_matcher.hpp"
#include "../computer_vision/nelder_mead_opt_fundamental.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief estimateFundamental estimates the foundamental matrix between image 1 to image 2
 * @param points0 is an array of points computed from image 1.
 * @param points1 is an array of points computed from image 2.
 * @return It returns the fundamental matrix, F_{1,2}.
 */
PIC_INLINE Eigen::Matrix3d estimateFundamental(std::vector< Eigen::Vector2f > &points0,
                                    std::vector< Eigen::Vector2f > &points1)
{
    Eigen::Matrix3d F;

    if((points0.size() != points1.size()) || (points0.size() < 8)) {
        F.setZero();
        return F;
    }

    //shift and scale points for numerical stability
    Eigen::Vector3f transform_0 = ComputeNormalizationTransform(points0);
    Eigen::Vector3f transform_1 = ComputeNormalizationTransform(points1);

    Eigen::Matrix3d mat_0 = getShiftScaleMatrix(transform_0);
    Eigen::Matrix3d mat_1 = getShiftScaleMatrix(transform_1);

    Eigen::MatrixXd A(points0.size(), 9);

    //set up the linear system
    for(unsigned int i = 0; i < points0.size(); i++) {

        //transform coordinates for increasing stability of the system
        Eigen::Vector2f p0 = points0[i];
        Eigen::Vector2f p1 = points1[i];

        p0[0] = (p0[0] - transform_0[0]) / transform_0[2];
        p0[1] = (p0[1] - transform_0[1]) / transform_0[2];

        p1[0] = (p1[0] - transform_1[0]) / transform_1[2];
        p1[1] = (p1[1] - transform_1[1]) / transform_1[2];

        A(i, 0) = p0[0] * p1[0];
        A(i, 1) = p0[0] * p1[1];
        A(i, 2) = p0[0];
        A(i, 3) = p0[1] * p1[0];
        A(i, 4) = p0[1] * p1[1];
        A(i, 5) = p0[1];
        A(i, 6) = p1[0];
        A(i, 7) = p1[1];
        A(i, 8) = 1.0;
    }

    //solve the linear system
    Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);
    Eigen::MatrixXd V = svd.matrixV();

    int n = int(V.cols()) - 1;

    F(0, 0) = V(0, n);
    F(1, 0) = V(1, n);
    F(2, 0) = V(2, n);

    F(0, 1) = V(3, n);
    F(1, 1) = V(4, n);
    F(2, 1) = V(5, n);

    F(0, 2) = V(6, n);
    F(1, 2) = V(7, n);
    F(2, 2) = V(8, n);

    //compute the final F matrix
    Eigen::Matrix3d mat_1_t = Eigen::Transpose< Eigen::Matrix3d >(mat_1);
    F = mat_1_t * F * mat_0;

    //enforce singularity
    Eigen::JacobiSVD< Eigen::MatrixXd > svdF(F, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix3d Uf = svdF.matrixU();
    Eigen::Matrix3d Vf = svdF.matrixV();
    Eigen::Vector3d Df = svdF.singularValues();
    Df[2] = 0.0;

    Eigen::Matrix3d F_new = Uf * DiagonalMatrix(Df) * Eigen::Transpose< Eigen::Matrix3d >(Vf);

    double norm = MAX(Df[0], Df[1]);
    return F_new / norm;
}

/**
 * @brief estimateFundamentalRansac
 * @param points0
 * @param points1
 * @param inliers
 * @param maxIterations
 * @return
 */
PIC_INLINE Eigen::Matrix3d estimateFundamentalRansac(std::vector< Eigen::Vector2f > &points0,
                                          std::vector< Eigen::Vector2f > &points1,
                                          std::vector< unsigned int > &inliers,
                                          unsigned int maxIterations = 100,
                                          double threshold = 0.01,
                                          unsigned int seed = 1)
{
    if(points0.size() < 9) {
        return estimateFundamental(points0, points1);
    }

    Eigen::Matrix3d F;
    int nSubSet = 8;

    std::mt19937 m(seed);

    unsigned int n = int(points0.size());

    unsigned int *subSet = new unsigned int [nSubSet];

    inliers.clear();

    for(unsigned int i = 0; i < maxIterations; i++) {
        getRandomPermutation(m, subSet, nSubSet, n);

        std::vector< Eigen::Vector2f > sub_points0;
        std::vector< Eigen::Vector2f > sub_points1;

        for(int j = 0; j < nSubSet; j++) {
            unsigned int k = subSet[j];
            sub_points0.push_back(points0[k]);
            sub_points1.push_back(points1[k]);
        }

        Eigen::Matrix3d tmpF = estimateFundamental(sub_points0, sub_points1);

        //is it a good one?
        std::vector< unsigned int > tmp_inliers;

        for(unsigned int j = 0; j < n; j++) {
            Eigen::Vector3d p0 = Eigen::Vector3d(points0[j][0], points0[j][1], 1.0);
            Eigen::Vector3d p1 = Eigen::Vector3d(points1[j][0], points1[j][1], 1.0);

            Eigen::Vector3d tmpF_p0 = tmpF * p0;
            double n0 = sqrt(tmpF_p0[0] * tmpF_p0[0] + tmpF_p0[1] * tmpF_p0[1]);
            if(n0 >  0.0) {
                tmpF_p0 /= n0;
            }

            double err = fabs(tmpF_p0.dot(p1));

            if(err < threshold){
                tmp_inliers.push_back(j);
            }
        }

        //get the inliers
        if(tmp_inliers.size() > inliers.size()) {
            F = tmpF;
            inliers.clear();
            inliers.assign(tmp_inliers.begin(), tmp_inliers.end());
        }
    }

    //improve estimate with inliers only
    if(inliers.size() > 7) {

        #ifdef PIC_DEBUG
            printf("Better estimate using inliers only.\n");
        #endif

        std::vector< Eigen::Vector2f > sub_points0;
        std::vector< Eigen::Vector2f > sub_points1;

        for(unsigned int i = 0; i < inliers.size(); i++) {
            sub_points0.push_back(points0[inliers[i]]);
            sub_points1.push_back(points1[inliers[i]]);
        }

        F = estimateFundamental(sub_points0, sub_points1);
    }

    return F;
}

/**
 * @brief estimateFundamentalWithNonLinearRefinement
 * @param F
 * @return
 */
PIC_INLINE Eigen::Matrix3d estimateFundamentalWithNonLinearRefinement(std::vector< Eigen::Vector2f > &points0,
                                                           std::vector< Eigen::Vector2f > &points1,
                                                           std::vector< unsigned int >    &inliers,
                                                           unsigned int maxIterationsRansac = 100,
                                                           double thresholdRansac = 0.01,
                                                           unsigned int seed = 1,
                                                           unsigned int maxIterationsNonLinear = 10000,
                                                           float thresholdNonLinear = 1e-4f
                                                           )
{
    Eigen::Matrix3d F = estimateFundamentalRansac(points0, points1, inliers, maxIterationsRansac, thresholdRansac, seed);

    //non-linear refinement using Nelder-Mead
    NelderMeadOptFundamental nmf(points0, points1, inliers);

    float F_data_opt[9];
    nmf.run(getLinearArrayFromMatrix(F), 9, thresholdNonLinear, maxIterationsNonLinear, &F_data_opt[0]);
    F = getMatrixdFromLinearArray(F_data_opt, 3, 3);

    return F;
}

/**
 * @brief noramalizeFundamentalMatrix
 * @param F
 * @return
 */
PIC_INLINE Eigen::Matrix3d noramalizeFundamentalMatrix(Eigen::Matrix3d F)
{
    Eigen::JacobiSVD< Eigen::Matrix3d > svdF(F, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix3d Uf = svdF.matrixU();
    Eigen::Matrix3d Vf = svdF.matrixV();
    Eigen::Vector3d Df = svdF.singularValues();
    Df[2] = 0.0;

    Eigen::Matrix3d F_new = Uf * DiagonalMatrix(Df) * Eigen::Transpose< Eigen::Matrix3d >(Vf);

    double norm = MAX(Df[0], Df[1]);
    return F_new / norm;
}

/**
 * @brief extractFundamentalMatrix
 * @param M0
 * @param M1
 * @param e0
 * @param e1
 * @return
 */
PIC_INLINE Eigen::Matrix3d extractFundamentalMatrix(Eigen::Matrix34d &M0, Eigen::Matrix34d &M1, Eigen::VectorXd &e0, Eigen::VectorXd &e1) {

    Eigen::Matrix3d M0_3 = getSquareMatrix(M0);
    Eigen::Matrix3d M1_3 = getSquareMatrix(M1);

    Eigen::Matrix3d M0_inv = M0_3.inverse();
    Eigen::Vector3d c0 = - M0_inv * getLastColumn(M0);
    e1 = M1 * addOne(c0);

    Eigen::Matrix3d M1_inv = M1_3.inverse();
    Eigen::Vector3d c1 = - M1_inv * getLastColumn(M1);
    e0 = M0 * addOne(c1);

    Eigen::Matrix3d F;

    F(0, 0) =  0.0;
    F(0, 1) = -e1(2);
    F(0, 2) =  e1(1);

    F(1, 0) =  e1(2);
    F(1, 1) =  0.0;
    F(1, 2) = -e1(0);

    F(2, 0) = -e1(1);
    F(2, 1) =  e1(0);
    F(2, 2) =  0.0;

    F = F * M1_3 * M0_inv;

    Eigen::JacobiSVD< Eigen::Matrix3d > svdF(F, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Vector3d Df = svdF.singularValues();

    double norm = MAX(Df[0], MAX(Df[1], Df[2]));
    return F / norm;
}

/**
 * @brief estimateFundamentalFromImages
 * @param img0
 * @param img1
 * @return
 */
PIC_INLINE  Eigen::Matrix3d estimateFundamentalFromImages(Image *img0,
                                                          Image *img1,
                                                          std::vector< Eigen::Vector2f > &m0,
                                                          std::vector< Eigen::Vector2f > &m1,
                                                          std::vector< unsigned int > &inliers)
{
    Eigen::Matrix3d F;
    if(img0 == NULL || img1 == NULL) {
        return F;
    }

    m0.clear();
    m1.clear();
    inliers.clear();

    //corners
    std::vector< Eigen::Vector2f > corners_from_img0;
    std::vector< Eigen::Vector2f > corners_from_img1;

    //compute the luminance images
    Image *L0 = FilterLuminance::execute(img0, NULL, LT_CIE_LUMINANCE);
    Image *L1 = FilterLuminance::execute(img1, NULL, LT_CIE_LUMINANCE);

    //extract corners
    HarrisCornerDetector hcd(2.5f, 5);
    hcd.execute(L0, &corners_from_img0);
    hcd.execute(L1, &corners_from_img1);

    //compute ORB descriptors for each corner and image

    //apply a gaussian filter to luminance images
    Image *L0_flt = FilterGaussian2D::execute(L0, NULL, 2.5f);
    Image *L1_flt = FilterGaussian2D::execute(L1, NULL, 2.5f);

    //compute ORB descriptor
    ORBDescriptor b_desc(31, 512);

    std::vector< unsigned int *> descs0;
    b_desc.getAll(L0_flt, corners_from_img0, descs0);

    std::vector< unsigned int *> descs1;
    b_desc.getAll(L1_flt, corners_from_img1, descs1);

    //match ORB descriptors
    std::vector< Eigen::Vector3i > matches;
    int n = b_desc.getDescriptorSize();

    //BinaryFeatureBruteForceMatcher bffm_bin(&descs1, n);
    BinaryFeatureLSHMatcher bffm_bin(&descs1, n, 64);
    bffm_bin.getAllMatches(descs0, matches);

    //get matches
    FeatureMatcher<unsigned int>::filterMatches(corners_from_img0, corners_from_img1, matches, m0, m1);

    //estimate the fundamental matrix
    F = estimateFundamentalWithNonLinearRefinement(m0, m1, inliers, 1000, 0.5, 1, 1000, 1e-4f);

    delete L0;
    delete L1;
    delete L0_flt;
    delete L1_flt;

    return F;
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_FUNDAMENTAL_HPP


#include "gtest/gtest.h"
#include <Eigen/Core>
#include <vector>
#include "MeshIntersection.h"
#include "IntersectionPaths.h"
#include "MeshFactory.h"

namespace {
  using namespace Geotree;

  class IntersectionPathsTest : public ::testing::Test {
  };

  TEST_F(IntersectionPathsTest, basic)
  {
    MeshFactory mf;

    Mesh cube1 = mf.makeCube(10,10,10);
    Mesh cube2 = mf.makeCube(12,12,10);
    cube2.translate(Vector(-1, -1, 1));
    MeshIntersection mi(cube1,cube2);

    EXPECT_EQ(mi.points.size(), 8);
    EXPECT_EQ(mi.paths.size(), 1);

    FaceSet faces = mi.points.getIntersectedFaces();

    std::vector <FaceSet> subpaths = mi.paths.getSubPaths(2);

    EXPECT_EQ(subpaths.size(), 1);

    for (int i : subpaths[0])
      {
	std::cout << "intersected face:" << i << std::endl;
      }

  }
}


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

//! @author Jeff Ichnowski

#ifndef LINEAR_H
#define LINEAR_H

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace nao_cup {
    template <typename S>
    using Transform = Eigen::Transform<S, 3, Eigen::Isometry>;

    template <typename S>
    using Vec2 = Eigen::Matrix<S, 2, 1>;
    template <typename S>
    using Vec3 = Eigen::Matrix<S, 3, 1>;
    template <typename S>
    using Vec4 = Eigen::Matrix<S, 4, 1>;

    template <typename S>
    S v3_len(const Vec3<S> *v) { return v->norm(); }

    template <typename S>
    S v3_dot(const Vec3<S> *a, const Vec3<S> *b) {
        return a->dot(*b);
    }

    template <typename S>
    S v3_dist(const Vec3<S> *a, const Vec3<S> *b) {
        return (*a - *b).norm();
    }

    template <typename S>
    void v3_sub(Vec3<S>* r, const Vec3<S>* a, const Vec3<S>* b) { *r = *a - *b; }

    template <typename S>
    void v3_add(Vec3<S>* r, const Vec3<S>* a, const Vec3<S>* b) { *r = *a + *b; }

    template <typename S>
    void v3_scale(Vec3<S>* r, const Vec3<S> *v, S s) { *r = *v * s; }

    template <typename S>
    void m4_mul(
        Transform<S>* r,
        const Transform<S> *a,
        const Transform<S> *b)
    {
        *r = *a * *b;
    }

    template <typename S>
    void m4_rotate(
        Transform<S> *m,
        const Transform<S> *t,
        S a, S x, S y, S z)
    {
        *m = *t * Eigen::AngleAxis<S>(a, Vec3<S>(x, y, z));
    }

    template <typename S>
    void m4_translate(Transform<S> *m, const Transform<S> *t, S x, S y, S z) {
        *m = *t * Eigen::Translation<S, 3>(x, y, z);
    }

    template <typename S>
    void m4_transform_i3(Vec3<S> *r, const Transform<S> *m, S x, S y, S z) {
        *r = *m * Vec3<S>(x, y, z);
    }

    template <typename S>
    void m4_extract_translation(
        Vec3<S>* r,
        const Transform<S> *m)
    {
        *r = m->translation().template head<3>();
    }

    template <typename S>
    void m4_transform_i(Vec4<S> *r, const Transform<S> *m, S x, S y, S z, S w) {
        *r = *m * Vec4<S>(x, y, z, w);
    }

    template <typename S>
    void m4_transform_i(Vec3<S>* r, const Transform<S> *m, S x, S y, S z) {
        *r = *m * Vec3<S>(x, y, z);
    }

    template <typename S>
    void v3_norm(Vec3<S> *r, const Vec3<S> *a) {
        *r = a->normalized();
    }

    template <typename S>
    S v3_dist_segment_point(const Vec3<S> *s0, const Vec3<S> *s1, const Vec3<S> *pt) {
        Vec3<S> v, w;
        S c1, c2, b;

        v3_sub(&v, s1, s0);
        v3_sub(&w, pt, s0);

        c1 = v3_dot(&w, &v);
        if (c1 <= 0.0) {
            return v3_len(&w);
        }

        c2 = v3_dot(&v, &v);
        if (c2 <= c1) {
            return v3_dist(pt, s1);
        }

        b = c1 / c2;

        v3_scale(&v, &v, b);
        v3_add(&v, s0, &v);

        return v3_dist(&v, pt);
    }
}

#endif /* LINEAR_H */


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

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

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

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

/*! \file portfolio/builders/creditdefaultswap.hpp
\brief
\ingroup builders
*/

#pragma once

#include <ored/portfolio/builders/cachingenginebuilder.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/utilities/log.hpp>

#include <qle/pricingengines/midpointcdsengine.hpp>

#include <boost/make_shared.hpp>

namespace ore {
namespace data {

//! Engine Builder base class for Credit Default Swaps
/*! Pricing engines are cached by creditCurveId
 \ingroup builders
*/

class CreditDefaultSwapEngineBuilder : public CachingPricingEngineBuilder<string, const Currency&, const string&> {
protected:
    CreditDefaultSwapEngineBuilder(const std::string& model, const std::string& engine)
        : CachingEngineBuilder(model, engine, {"CreditDefaultSwap"}) {}

    virtual string keyImpl(const Currency&, const string& creditCurveId) override { return creditCurveId; }
};

//! Midpoint Engine Builder class for CreditDefaultSwaps
/*! This class creates a MidPointCdsEngine
 \ingroup builders
*/

class MidPointCdsEngineBuilder : public CreditDefaultSwapEngineBuilder {
public:
    MidPointCdsEngineBuilder() : CreditDefaultSwapEngineBuilder("DiscountedCashflows", "MidPointCdsEngine") {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const Currency& ccy, const string& creditCurveId) override {

        Handle<YieldTermStructure> yts = market_->discountCurve(ccy.code(), configuration(MarketContext::pricing));
        Handle<DefaultProbabilityTermStructure> dpts =
            market_->defaultCurve(creditCurveId, configuration(MarketContext::pricing));
        Handle<Quote> recovery = market_->recoveryRate(creditCurveId, configuration(MarketContext::pricing));

        return boost::make_shared<QuantExt::MidPointCdsEngine>(dpts, recovery->value(), yts);
    }
};

} // namespace data
} // namespace ore


// CHECKLIST:
// 3º) Implement your method (replace METHOD CODE with your code).
// 4º) Search every TODO, follow it, and delete it.
// 5º) Make a copy and change the relevant bits on a run_*.cpp and maybe the
//    test_*.cpp (basically execute steps 1 and 2 on them, including the
//    filename).
// 6º) Change the Makefile to compile your run_*.cpp and test_*.cpp too.
// 7º) Delete this comment.
// Notes: if you want to pass commandline flags you can remove the wrapper_t
//  and make your own function that receives argv/argc.
#ifndef HBM_BABAYEV_HPP
#define HBM_BABAYEV_HPP

#include "ukp_common.hpp"
#include "wrapper.hpp"
#include "type_name.hpp"
#include "mtu.hpp" // babayev make use of the u3 upper bound

#include <chrono>
#include <boost/rational.hpp>

#ifndef HBM_PROFILE_PRECISION
  #define HBM_PROFILE_PRECISION 5
#endif

#ifndef HBM_PRINT_VAR
  #define HBM_PRINT_VAR(var) out << #var ": " << var << std::endl
#endif

namespace hbm {
  template <typename W, typename P, typename I>
  struct babayev_extra_info_t : extra_info_t {
    // TODO: check if this struct is adequated or not. If you want to have
    // those profiling times you will need to put HBM_START_TIMER and
    // HBM_STOP_TIMER on your code (at METHOD CODE). Check the other codes
    // to see how it works. Also, the variables n and c need to be set.
    std::string algorithm_name{"babayev"};
    #ifdef HBM_PROFILE
    double sort_time{0};        ///< Time used sorting items.
    double vector_alloc_time{0};///< Time used allocating vectors for DP.
    double linear_comp_time{0}; ///< Time used by linear time preprocessing.
    double dp_time{0};     ///< Time used creating partial solutions.
    double sol_time{0};    ///< Time used to assemble solution.
    double total_time{0};  ///< Time used by all the algorithm.
    #endif //HBM_PROFILE
    I n{0};      ///< Instance number of items.
    W c{0};      ///< Instance capacity.

    virtual std::string gen_info(void) {
      std::stringstream out("");

      HBM_PRINT_VAR(algorithm_name);
      out <<  "git_head_at_compilation: "
          << HBM_GIT_HEAD_AT_COMPILATION << std::endl;
      out << "type_W: " << hbm::type_name<W>::get() << std::endl;
      out << "type_P: " << hbm::type_name<P>::get() << std::endl;
      out << "type_I: " << hbm::type_name<I>::get() << std::endl;
      HBM_PRINT_VAR(c);
      HBM_PRINT_VAR(n);

      #ifdef HBM_PROFILE
      out << "HBM_PROFILE: defined" << std::endl;
      const double sum_time = sort_time + vector_alloc_time +
        linear_comp_time + dp_time + sol_time;

      std::streamsize old_precision = out.precision(HBM_PROFILE_PRECISION);
      const int two_first_digits_and_period = 3;
      const int percent_size = two_first_digits_and_period+HBM_PROFILE_PRECISION;
      std::ios_base::fmtflags old_flags = out.setf(std::ios::fixed, std:: ios::floatfield);
      char old_fill = out.fill(' ');

      #ifndef HBM_PRINT_TIME
        #define HBM_PRINT_TIME(name, var)\
          out << name << " time: " << var << "s (";\
          out << std::setw(percent_size) << (var/total_time)*100.0;\
          out << "%)" << std::endl
      #endif

      HBM_PRINT_TIME("Sort", sort_time);
      HBM_PRINT_TIME("Vect", vector_alloc_time);
      HBM_PRINT_TIME("O(n)", linear_comp_time);
      HBM_PRINT_TIME("DP  ", dp_time);
      HBM_PRINT_TIME("sol ", sol_time);
      HBM_PRINT_TIME("Sum ", sum_time);
      HBM_PRINT_TIME("All ", total_time);

      out.fill(old_fill);
      out.setf(old_flags);
      out.precision(old_precision);
      #else //HBM_PROFILE
      out << "HBM_PROFILE: undefined" << std::endl;
      #endif //HBM_PROFILE

      return out.str();
    }
  };

  namespace hbm_babayev_impl {
    using namespace std;
    using namespace std::chrono;
    using namespace boost;

    // This methods isn't described exactly in the paper. By trial and error,
    // we determined it as a lower bound version of u3. Yet, at cost of an
    // scan over the items (O(n)) we can get a better lower bound in many
    // cases.
    template<typename W, typename P>
    P greedy_lower_bound(instance_t<W, P> &ukpi, bool already_sorted) {
      /*P z = 0;
      W c_ = ukpi.c;
      if (!already_sorted) sort_by_eff(ukpi.items);
      for (auto& i : ukpi.items) {
        auto qt = c_ / i.w;
        z += qt*i.p;
        c_ -= qt*i.w;
      }

      return z;*/
      auto &v = ukpi.items;
      W c = ukpi.c;
      W c1 = c % v[0].w;
      W c2 = c1 % v[1].w;
      P z = (c/v[0].w)*v[0].p + (c1/v[1].w)*v[1].p + (c2/v[2].w)*v[2].p;

      return z;
    }

    // TODO: quick comment on the method
    template<typename W, typename P, typename I>
    void babayev(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted) {
      // Extra Info Pointer == eip
      babayev_extra_info_t<W, P, I>* eip = new babayev_extra_info_t<W, P, I>();
      extra_info_t* upcast_ptr = dynamic_cast<extra_info_t*>(eip);
      sol.extra_info = std::shared_ptr<extra_info_t>(upcast_ptr);

      #ifdef HBM_PROFILE
      // Used to compute the time all the execution algorithm.
      steady_clock::time_point all_babayev_begin = steady_clock::now();

      // Used by HBM_START_TIMER and HBM_STOP_TIMER if HBM_PROFILE is defined.
      steady_clock::time_point begin;
      #endif

      // NOTE: the paper's notation is used here, so 'c' isn't used to denote
      // the knapsack capacity (as in other algorithms of this repository) but
      // to denote the optimal profit value (or its lower/upper bound, or a
      // tentative value for it). The variable 'b' is used to denote the
      // knapsack capacity.
      const W b = ukpi.c;
      const I n = ukpi.items.size();
      // STEP 1: Determine upper_c -- an upper bound for c_star,
      //   upper_c >= c_star;

      // Get the three most efficient items. TODO: do this in a better way?
      P lower_c = 0, c = 0, upper_c = 0;
      if (!already_sorted) sort_by_eff(ukpi.items); // already sort for the greedy algorithm
      sol.y_opt = 0;
      sol.opt = upper_c = u3(ukpi.items[0], ukpi.items[1], ukpi.items[2], ukpi.c);
      lower_c = greedy_lower_bound(ukpi, true);
      // STEP 2: c := upper_c
      c = upper_c;

      // STEP 3: Compute beta and test consistency of Eq. 7;
      // TODO: On page four, it's said that M1 is more efficient on a
      // certain condition than M2, and vice-versa. Check the condition and
      // implement both methods after.
      rational<P> R(ukpi.items[0].p, ukpi.items[0].w); // defined at page 3

      // Computing w and v with M1, see first paragraph of "Procedure M1" (p.3)
      // for reference.

      // Eq. 18
      P k = 0;
      if (R.denominator() == 1) {
        k = R.numerator() + 1;
      } else {
        k = rational_cast<P>(R) + 1;
      }
      // Eq. 16 (multiplied by -1) and adding one for rounding purposes
      P v1 = - (((upper_c - 1) / k) + 1);
      // Eq. 17 (multiplied by -1) and adding one for rounding purposes
      P v2 = - (rational_cast<P>((b*R - lower_c - 1)/(k - R)) + 1);
      // We want the "smallest absolute value" between two negative numbers.
      P v = max(v1, v2);

      // Eq. 15
      W w = -v*k + 1;

      // Important: don't forget we need to check if they are coprime with gcd.
      // END OF PROCEDURE M1 (w and v defined)
      auto &out = cout;
      HBM_PRINT_VAR(v);
      HBM_PRINT_VAR(w);
      HBM_PRINT_VAR(k);
      HBM_PRINT_VAR(R);

      myvector<P> agg_eq;
      agg_eq.resize(n + 1);
      agg_eq[n] = w; // slack variable / dummy item
      P alpha1 = w;
      for (I i = 0; i < n; ++i) {
        auto it = ukpi.items[i];
        agg_eq[i] = it.p*v + it.w*w;
        if (alpha1 > agg_eq[i]) alpha1 = agg_eq[i];
        cout << "agg_eq[" << i << "]: " << agg_eq[i] << endl;
      }
      cout << "agg_eq[n]: " << w << endl;
      cout << "agg_eq[n+1]: " << w*b << v << "*c" << endl;
      HBM_PRINT_VAR(alpha1);

      // IMPORTANT: the lower bound lower_c on their computational results
      // is given by solving the reduced problem (1% most efficient) exactly
      // (page 6).
      // IMPORTANT: two approachs for solving the consistency problem are
      // presented. On page 6, it's said that approach two was used to
      // generate the results presented.

      vector<P> r(alpha1, 0);

      // The algorithm isn't fully specified on the paper "A New Knapsack
      // Approach by Integer Equivalent Aggregation and Consistency
      // Determination", it references the paper "Integer Programming over a
      // Finite Additive Group" for core of the algorithm (the consistency
      // testing). We know the algorithm is right until there because the
      // values match the ones given at the the paper ("A New [...]"). Using
      // GLPK and a simple mathprog model we can verify that a consistency
      // check as described on the paper really would yield the results
      // presented. I abandoned this implementation task because there wasn't
      // enough time before ending master's.
      //
      // param beta, integer, > 0;
      //
      // param n, integer, > 0;
      // /* number of items */
      //
      // set I := 1..n;
      // /* set of items */
      //
      // param a{i in 1..n}, integer, >= 0;
      // /* the aggregate value between weight and profit */
      //
      // var x{i in I}, integer, >= 0;
      // /* x[i] = m, means m copies of item i are in the knapsack */
      //
      // minimize obj: sum{i in I} a[i]*x[i];
      // /* objective is to maximize the profit, but for this we minimize the aggregate function value */
      //
      // s.t. the_only_constraint: sum{i in I} a[i] * x[i] = beta;
      //
      // solve;
      //
      // for {i in I} { printf 'Item nº %d: %d\n', i, x[i]; }
      // printf "Result: %d\n", sum {i in I} a[i]*x[i];
      //
      // data;
      //
      // param beta := 229; /* will fail (unfeasible) */
      // /* param beta := 267;*/ /* will find one solution */
      // param n := 6;
      // param a := 1 48, 2 43, 3 127, 4 41, 5 121, 6 115;
      //
      // end;

      // STEP 4: If Eq. 7 is inconsistent, then set c := c - 1 and return to
      //  step 3.
      // STEP 5: If Eq. 7 is consistent, then c_star = c and the corresponding
      //   equation of Eq. 7 is a solution to the original knapsack problem
      //   (1)--(3). End.

      #ifdef HBM_PROFILE
      eip->total_time = difftime_between_now_and(all_babayev_begin);
      #endif
    }

    // -------------------- WRAPPERS --------------------
    template<typename W, typename P, typename I>
    struct babayev_wrap : wrapper_t<W, P, I> {
      virtual void operator()(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted) const {
        // Calls the overloaded version with the third argument as a bool
        hbm_babayev_impl::babayev(ukpi, sol, already_sorted);

        return;
      }

      virtual const std::string& name(void) const {
        static const std::string name = "babayev";
        return name;
      }
    };

    template<typename W, typename P, typename I = size_t>
    void babayev(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol,
                int argc, argv_t argv) {
      simple_wrapper(babayev_wrap<W, P, I>(), ukpi, sol, argc, argv);
    }
  }
  // -------------------- EXTERNAL FUNCTIONS --------------------

  /// Solves an UKP instance by the unnamed algorithm presented at
  /// "A New Knapsack Approach by Integer Equivalent Aggregation and
  /// Consistency Determination", and stores the results at sol. The
  /// name 'babayev' is the surname of the paper's first author.
  ///
  /// @note Unfinished implementation. Currently waiting a response from
  ///   Babayev and Jennifer Ryan/Sanchez.
  /// @param ukpi The UKP instance to be solved.
  /// @param sol The object where the results will be written.
  /// @param already_sorted If the ukpi.items vector needs to be sorted by
  ///   efficiency.
  template<typename W, typename P, typename I>
  void babayev(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, bool already_sorted) {
    hbm_babayev_impl::babayev(ukpi, sol, already_sorted);
  }

  /// An overloaded function, it's used as argument to test_common functions.
  ///
  /// The only parameter recognized is "--already-sorted". If this parameter is
  /// given the ukpi.items will NOT be sorted by non-increasing efficiency. If
  /// it's ommited the ukpi.items will be sorted by non-increasing efficiency.
  /// If more than one item have the same efficiency, then those items will be
  /// sorted by weight (in relation to each other).
  ///
  /// @see main_take_path
  /// @see babayev(instance_t<W, P> &, solution_t<W, P, I> &, bool)
  template<typename W, typename P, typename I>
  void babayev(instance_t<W, P> &ukpi, solution_t<W, P, I> &sol, int argc, argv_t argv) {
    hbm_babayev_impl::babayev(ukpi, sol, argc, argv);
  }
}

#endif //HBM_BABAYEV_HPP


#include <iostream>
#include <optional>
#include <sstream>
#include <stdexcept>

#include <ceres/ceres.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <Eigen/Dense>
#include <boost/asio.hpp>

#include "farm_ng/calibration/align_sensor_rig.pb.h"
#include "farm_ng/calibration/calibrate_multi_view_apriltag_rig.pb.h"
#include "farm_ng/calibration/calibrate_multi_view_lidar.pb.h"
#include "farm_ng/calibration/local_parameterization.h"
#include "farm_ng/calibration/multi_view_apriltag_rig_calibrator.h"
#include "farm_ng/calibration/multi_view_lidar_model.pb.h"

#include "farm_ng/perception/apriltag.h"
#include "farm_ng/perception/point_cloud.h"

#include "farm_ng/core/blobstore.h"
#include "farm_ng/core/event_log_reader.h"
#include "farm_ng/core/init.h"
#include "farm_ng/core/ipc.h"

#include "farm_ng/perception/pose_utils.h"
#include "farm_ng/perception/tensor.h"

#include "farm_ng/perception/time_series.h"

#include <Eigen/Dense>

DEFINE_string(result, "aligned_sensor_rig.json", "Output path.");
DEFINE_string(output_config, "",
              "output a config json file to the given path.");
DEFINE_string(config, "", "Load config from ajson file.");

DEFINE_string(multi_view_lidar_model, "",
              "Path to the multi_view_lidar_model to align");

DEFINE_int32(floor_tag, 313, "A tag on the floor.");
DEFINE_double(floor_distance_threshold, 0.1,
              "Distance from floor to consider tags belonging to the floor.");

namespace fs = boost::filesystem;

namespace farm_ng::calibration {
Eigen::Matrix3Xd ToMatrix(const std::vector<Eigen::Vector3d>& points) {
  return Eigen::Map<const Eigen::Matrix3Xd>(points[0].data(), 3, points.size());
}
Eigen::Hyperplane<double, 3> FitPlaneToPoints(const Eigen::Matrix3Xd& points) {
  Eigen::Vector3d mean = points.rowwise().mean();
  Eigen::Matrix3Xd points_centered = points.colwise() - mean;
  auto svd =
      points_centered.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeThinV);
  Eigen::Vector3d normal = svd.matrixU().col(2);
  LOG(INFO) << "p=" << mean.transpose() << " n=" << normal.transpose();
  return Eigen::Hyperplane<double, 3>(normal, mean);
}

// based on https://github.com/nghiaho12/rigid_transform_3D/blob/master/rigid_transform_3D.py
// http://nghiaho.com/?page_id=671
Sophus::SE3d FitAPoseB(const Eigen::Matrix3Xd& points_a,
                       const Eigen::Matrix3Xd& points_b) {
  Eigen::Vector3d mean_a = points_a.rowwise().mean();
  Eigen::Vector3d mean_b = points_b.rowwise().mean();
  Eigen::Matrix3Xd points_a_centered = points_a.colwise() - mean_a;
  Eigen::Matrix3Xd points_b_centered = points_b.colwise() - mean_b;
  Eigen::MatrixXd a_H_b = points_a_centered * points_b_centered.transpose();
  auto svd = a_H_b.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::MatrixXd V = svd.matrixV();
  Eigen::MatrixXd U = svd.matrixU();
  Eigen::Matrix3d b_R_a = V * U.transpose();
  if (b_R_a.determinant() < 0) {
    V.col(2) *= -1;
    b_R_a = V * U.transpose();
    CHECK_GE(b_R_a.determinant(), 0);
  }
  Eigen::Matrix4d b_pose_a = Eigen::Matrix4d::Identity();
  b_pose_a.block<3, 3>(0, 0) = b_R_a;
  b_pose_a.block<3, 1>(0, 3) = mean_b - b_R_a * mean_a;
  auto a_pose_b = Sophus::SE3d(b_pose_a).inverse();
  return a_pose_b;
}

int align_sensor_rig(AlignSensorRigConfiguration config) {
  CHECK_GT(config.floor_tag_ids_size(), 0);
  MultiViewLidarModel lidar_model =
      core::ReadProtobufFromResource<MultiViewLidarModel>(config.model());

  perception::PoseGraph pose_graph;
  for (const perception::ApriltagRig::Node& node :
       lidar_model.apriltag_rig().nodes()) {
    pose_graph.AddPose(node.pose());
  }
  std::string floor_tag_name = perception::FrameRigTag(
      lidar_model.apriltag_rig().name(), config.floor_tag_ids(0));
  perception::PoseGraph floor_tag_graph =
      pose_graph.AveragePoseGraph(floor_tag_name);
  double floor_distance_thresh = config.floor_distance_threshold();
  std::vector<Eigen::Vector3d> floor_points_tag_rig;

  for (const perception::ApriltagRig::Node& node :
       lidar_model.apriltag_rig().nodes()) {
    auto floor_tag_pose_tag =
        floor_tag_graph.CheckAverageAPoseB(floor_tag_name, node.frame_name());
    double err = 0;
    auto points_tag = perception::PointsTag(node);
    for (auto point_tag : points_tag) {
      // LOG(INFO) << "point_tag: " << point_tag.transpose();
      err = std::max(err, std::abs((floor_tag_pose_tag * point_tag).z()));
    }
    if (err < floor_distance_thresh) {
      LOG(INFO) << node.frame_name() << " on floor err: " << err;
      Sophus::SE3d tag_rig_pose_tag = ProtoToSophus(
          node.pose(), lidar_model.apriltag_rig().name(), node.frame_name());
      for (auto point_tag : points_tag) {
        floor_points_tag_rig.push_back(tag_rig_pose_tag * point_tag);
      }
    } else {
      // LOG(INFO) << node.frame_name() << " err : " << err;
    }
  }

  std::vector<Eigen::Vector3d> points_camera_rig;

  for (int i = 0; i < lidar_model.measurements_size(); ++i) {
    const auto& m_i0 = lidar_model.measurements(i);
    Sophus::SE3d camera_rig_pose_apriltag_rig = ProtoToSophus(
        m_i0.camera_rig_pose_apriltag_rig(), lidar_model.camera_rig().name(),
        lidar_model.apriltag_rig().name());
    for (auto floor_point_tag_rig : floor_points_tag_rig) {
      points_camera_rig.push_back(camera_rig_pose_apriltag_rig *
                                  floor_point_tag_rig);
    }
  }
  perception::PoseGraph sensor_rig_graph;
  sensor_rig_graph.AddPoses(lidar_model.lidar_poses());
  auto plane_camera_rig = FitPlaneToPoints(ToMatrix(points_camera_rig));
  for (auto pose : lidar_model.lidar_poses()) {
    auto sensor_name = pose.frame_b();
    auto camera_rig_pose_sensor = sensor_rig_graph.CheckAverageAPoseB(
        lidar_model.camera_rig().name(), sensor_name);
    LOG(INFO) << "sensor_name: " << sensor_name << " distance to ground: "
              << plane_camera_rig.signedDistance(
                     camera_rig_pose_sensor.translation());
  }

  std::vector<Eigen::Vector3d> points_base;
  std::vector<Eigen::Vector3d> points_rig;
  Eigen::Hyperplane<double, 3> plane_base(Eigen::Vector3d(0, 0, 1.0),
                                          Eigen::Vector3d(0, 0, 0));
  for (auto pose : config.base_pose_sensor_measured()) {
    Sophus::SE3d base_pose_sensor =
        perception::ProtoToSophus(pose, config.base_frame(), pose.frame_b());
    points_base.push_back(base_pose_sensor.translation());
    points_base.push_back(
        plane_base.projection(base_pose_sensor.translation()));

    Sophus::SE3d rig_pose_sensor = sensor_rig_graph.CheckAverageAPoseB(
        lidar_model.camera_rig().name(), pose.frame_b());
    points_rig.push_back(rig_pose_sensor.translation());
    points_rig.push_back(
        plane_camera_rig.projection(rig_pose_sensor.translation()));
  }
  Sophus::SE3d base_pose_rig =
      FitAPoseB(ToMatrix(points_base), ToMatrix(points_rig));
  AlignSensorRigResult result;
  result.set_base_frame(config.base_frame());
  for (auto pose : lidar_model.lidar_poses()) {
    auto sensor_name = pose.frame_b();
    auto camera_rig_pose_sensor = sensor_rig_graph.CheckAverageAPoseB(
        lidar_model.camera_rig().name(), sensor_name);
    auto base_pose_sensor = base_pose_rig * camera_rig_pose_sensor;
    perception::SophusToProto(base_pose_sensor, config.base_frame(),
                              sensor_name, result.add_base_pose_sensor());
    LOG(INFO) << result.base_pose_sensor(result.base_pose_sensor_size() - 1)
                     .ShortDebugString();
  }

  farm_ng::core::WriteProtobufToJsonFile(FLAGS_result, result);

  return 0;
}

}  // namespace farm_ng::calibration

int main(int argc, char* argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  FLAGS_logtostderr = 1;
  std::string filename = boost::filesystem::path(argv[0]).filename().string();
  google::InitGoogleLogging(filename.c_str());
  farm_ng::calibration::AlignSensorRigConfiguration config;
  if (!FLAGS_config.empty()) {
    config = farm_ng::core::ReadProtobufFromJsonFile<
        farm_ng::calibration::AlignSensorRigConfiguration>(FLAGS_config);
  } else {
    config.set_floor_distance_threshold(FLAGS_floor_distance_threshold);
    config.add_floor_tag_ids(FLAGS_floor_tag);
    config.mutable_model()->set_path(FLAGS_multi_view_lidar_model);
    config.mutable_model()->set_content_type(
        farm_ng::core::ContentTypeProtobufBinary<
            farm_ng::calibration::MultiViewLidarModel>());
  }
  if (!FLAGS_output_config.empty()) {
    farm_ng::core::WriteProtobufToJsonFile(FLAGS_output_config, config);
    return 0;
  }
  return farm_ng::calibration::align_sensor_rig(config);
}


#include <boost/random/gamma_distribution.hpp>

/// non-module alias for amrand (defined in resamplekin.f90)
extern "C" void amrand_c_alias(double *out);

namespace {

/***
 * Wraps default amber random engine
 *
 * Implements interface of boost Uniform Random Number Generator
 * https://www.boost.org/doc/libs/1_66_0/doc/html/boost_random/reference.html#boost_random.reference.concepts.uniform_random_number_generator
 * */
class AmrandGenerator {
public:
    typedef double result_type;

    result_type operator()() {
        double x;
        amrand_c_alias(&x);
        return x;
    }

    result_type min() const { return 0.0; }

    result_type max() const { return 1.0; }
};

}
extern "C" {
/**
 * Returns random value distributed as gamma_distribution of order \a rank
 * Uses default Amber random generator (amrand)
 * */
double gamdev(int rank) {
    typedef boost::random::gamma_distribution<double>::param_type param_type;
    static boost::random::gamma_distribution<double> gamma_distribution;
    static AmrandGenerator urng;
    return gamma_distribution(urng, param_type(rank, 1));
}
};


#include "common/string_util.h"
#include "cryptonote_config.h"
#include "cryptonote_basic/hardfork.h"
#include "common/oxen.h"
#include "epee/int-util.h"
#include <oxenc/endian.h>
#include <limits>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <cfenv>

#include "oxen_economy.h"
#include "service_node_rules.h"

using cryptonote::hf;

namespace service_nodes {

uint64_t get_staking_requirement(cryptonote::network_type nettype, hf hardfork)
{
  assert(hardfork >= hf::hf16_pulse);
  return nettype == cryptonote::network_type::MAINNET
    ? oxen::STAKING_REQUIREMENT
    : oxen::STAKING_REQUIREMENT_TESTNET;
}

// TODO(oxen): Move to oxen_economy, this will also need access to oxen::exp2
uint64_t get_staking_requirement(cryptonote::network_type nettype, uint64_t height)
{
  if (nettype != cryptonote::network_type::MAINNET)
      return oxen::STAKING_REQUIREMENT_TESTNET;

  if (is_hard_fork_at_least(nettype, hf::hf16_pulse, height))
    return oxen::STAKING_REQUIREMENT;

  if (is_hard_fork_at_least(nettype, hf::hf13_enforce_checkpoints, height))
  {
    constexpr int64_t heights[] = {
        385824,
        429024,
        472224,
        515424,
        558624,
        601824,
        645024,
    };

    constexpr int64_t lsr[] = {
        20458'380815527,
        19332'319724305,
        18438'564443912,
        17729'190407764,
        17166'159862153,
        16719'282221956,
        16364'595203882,
    };

    assert(static_cast<int64_t>(height) >= heights[0]);
    constexpr uint64_t LAST_HEIGHT      = heights[oxen::array_count(heights) - 1];
    constexpr uint64_t LAST_REQUIREMENT = lsr    [oxen::array_count(lsr) - 1];
    if (height >= LAST_HEIGHT)
        return LAST_REQUIREMENT;

    size_t i = 0;
    for (size_t index = 1; index < oxen::array_count(heights); index++)
    {
      if (heights[index] > static_cast<int64_t>(height))
      {
        i = (index - 1);
        break;
      }
    }

    int64_t H      = height;
    int64_t result = lsr[i] + (H - heights[i]) * ((lsr[i + 1] - lsr[i]) / (heights[i + 1] - heights[i]));
    return static_cast<uint64_t>(result);
  }

  uint64_t hardfork_height = 101250;
  if (height < hardfork_height) height = hardfork_height;

  uint64_t height_adjusted = height - hardfork_height;
  uint64_t base = 0, variable = 0;
  std::fesetround(FE_TONEAREST);
  if (is_hard_fork_at_least(nettype, hf::hf11_infinite_staking, height))
  {
    base     = 15000 * oxen::COIN;
    variable = (25007.0 * oxen::COIN) / oxen::exp2(height_adjusted/129600.0);
  }
  else
  {
    base      = 10000 * oxen::COIN;
    variable  = (35000.0 * oxen::COIN) / oxen::exp2(height_adjusted/129600.0);
  }

  uint64_t result = base + variable;
  return result;
}

uint64_t portions_to_amount(uint64_t portions, uint64_t staking_requirement)
{
  return mul128_div64(staking_requirement, portions, cryptonote::old::STAKING_PORTIONS);
}

bool check_service_node_portions(hf hf_version, const std::vector<std::pair<cryptonote::account_public_address, uint64_t>>& portions)
{
  // When checking portion we always use HF18 rules, even on HF19, because a registration actually
  // generated under HF19+ won't get here.
  if (hf_version == hf::hf19_reward_batching)
    hf_version = hf::hf18;
  else if (hf_version > hf::hf19_reward_batching)
  {
    LOG_PRINT_L1("Registration tx rejected: portions-based registrations not permitted after HF19");
    return false;
  }
  if (portions.size() > oxen::MAX_CONTRIBUTORS_V1) {
    LOG_PRINT_L1("Registration tx rejected: too many contributors (" << portions.size() << " > " << oxen::MAX_CONTRIBUTORS_V1 << ")");
    return false;
  }

  uint64_t reserved = 0;
  uint64_t remaining = cryptonote::old::STAKING_PORTIONS;
  for (size_t i = 0; i < portions.size(); ++i)
  {

    const uint64_t min_portions = get_min_node_contribution(hf_version, cryptonote::old::STAKING_PORTIONS, reserved, i);
    if (portions[i].second < min_portions) {
      LOG_PRINT_L1("Registration tx rejected: portion " << i << " too small (" << portions[i].second << " < " << min_portions << ")");
      return false;
    }
    if (portions[i].second > remaining) {
      LOG_PRINT_L1("Registration tx rejected: portion " << i << " exceeds available portions");
      return false;
    }

    reserved += portions[i].second;
    remaining -= portions[i].second;
  }

  return true;
}

bool check_service_node_stakes(hf hf_version, cryptonote::network_type nettype, uint64_t staking_requirement, const std::vector<std::pair<cryptonote::account_public_address, uint64_t>>& stakes)
{
  if (hf_version < hf::hf19_reward_batching) {
    LOG_PRINT_L1("Registration tx rejected: amount-based registrations not accepted before HF19");
    return false; // OXEN-based registrations not accepted before HF19
  }
  if (stakes.size() > oxen::MAX_CONTRIBUTORS_HF19) {
    LOG_PRINT_L1("Registration tx rejected: too many contributors (" << stakes.size() << " > " << oxen::MAX_CONTRIBUTORS_HF19 << ")");
    return false;
  }

  const auto operator_requirement = nettype == cryptonote::network_type::MAINNET
    ? oxen::MINIMUM_OPERATOR_CONTRIBUTION
    : oxen::MINIMUM_OPERATOR_CONTRIBUTION_TESTNET;

  uint64_t reserved = 0;
  uint64_t remaining = staking_requirement;
  for (size_t i = 0; i < stakes.size(); i++) {
    const uint64_t min_stake = i == 0 ? operator_requirement : get_min_node_contribution(hf_version, staking_requirement, reserved, i);

    if (stakes[i].second < min_stake) {
      LOG_PRINT_L1("Registration tx rejected: stake " << i << " too small (" << stakes[i].second << " < " << min_stake << ")");
      return false;
    }
    if (stakes[i].second > remaining) {
      LOG_PRINT_L1("Registration tx rejected: stake " << i << " (" << stakes[i].second << ") exceeds available remaining stake (" << remaining << ")");
      return false;
    }

    reserved += stakes[i].second;
    remaining -= stakes[i].second;
  }

  return true;
}

crypto::hash generate_request_stake_unlock_hash(uint32_t nonce)
{
  static_assert(sizeof(crypto::hash) == 8 * sizeof(uint32_t) && alignof(crypto::hash) >= alignof(uint32_t));
  crypto::hash result;
  oxenc::host_to_little_inplace(nonce);
  for (size_t i = 0; i < 8; i++)
    reinterpret_cast<uint32_t*>(result.data)[i] = nonce;
  return result;
}

uint64_t get_locked_key_image_unlock_height(cryptonote::network_type nettype, uint64_t node_register_height, uint64_t curr_height)
{
  uint64_t blocks_to_lock = staking_num_lock_blocks(nettype);
  uint64_t result         = curr_height + (blocks_to_lock / 2);
  return result;
}

static uint64_t get_min_node_contribution_pre_v11(uint64_t staking_requirement, uint64_t total_reserved)
{
  return std::min(staking_requirement - total_reserved, staking_requirement / oxen::MAX_CONTRIBUTORS_V1);
}

uint64_t get_max_node_contribution(hf version, uint64_t staking_requirement, uint64_t total_reserved)
{
  if (version >= hf::hf16_pulse)
    return (staking_requirement - total_reserved) * cryptonote::MAXIMUM_ACCEPTABLE_STAKE::num
      / cryptonote::MAXIMUM_ACCEPTABLE_STAKE::den;
  return std::numeric_limits<uint64_t>::max();
}

uint64_t get_min_node_contribution(hf version, uint64_t staking_requirement, uint64_t total_reserved, size_t num_contributions)
{
  if (version < hf::hf11_infinite_staking)
    return get_min_node_contribution_pre_v11(staking_requirement, total_reserved);

  const uint64_t needed = staking_requirement - total_reserved;

  const size_t max_contributors = version >= hf::hf19_reward_batching ? oxen::MAX_CONTRIBUTORS_HF19 : oxen::MAX_CONTRIBUTORS_V1;
  if (max_contributors <= num_contributions) return UINT64_MAX;

  const size_t num_contributions_remaining_avail = max_contributors - num_contributions;
  return needed / num_contributions_remaining_avail;
}

uint64_t get_min_node_contribution_in_portions(hf version, uint64_t staking_requirement, uint64_t total_reserved, size_t num_contributions)
{
  uint64_t atomic_amount = get_min_node_contribution(version, staking_requirement, total_reserved, num_contributions);
  uint64_t result        = (atomic_amount == UINT64_MAX) ? UINT64_MAX : (get_portions_to_make_amount(staking_requirement, atomic_amount));
  return result;
}

uint64_t get_portions_to_make_amount(uint64_t staking_requirement, uint64_t amount, uint64_t max_portions)
{
  uint64_t lo, hi, resulthi, resultlo;
  lo = mul128(amount, max_portions, &hi);
  if (lo > UINT64_MAX - (staking_requirement - 1))
    hi++;
  lo += staking_requirement-1;
  div128_64(hi, lo, staking_requirement, &resulthi, &resultlo);
  return resultlo;
}

static bool get_portions_from_percent(double cur_percent, uint64_t& portions) {
  if(cur_percent < 0.0 || cur_percent > 100.0) return false;

  // Fix for truncation issue when operator cut = 100 for a pool Service Node.
  if (cur_percent == 100.0)
  {
    portions = cryptonote::old::STAKING_PORTIONS;
  }
  else
  {
    portions = (cur_percent / 100.0) * (double)cryptonote::old::STAKING_PORTIONS;
  }

  return true;
}

std::optional<double> parse_fee_percent(std::string_view fee)
{
  if (tools::ends_with(fee, "%"))
    fee.remove_suffix(1);

  double percent;
  try {
    percent = boost::lexical_cast<double>(fee);
  } catch(...) {
    return std::nullopt;
  }

  if (percent < 0 || percent > 100)
    return std::nullopt;

  return percent;
}

bool get_portions_from_percent_str(std::string cut_str, uint64_t& portions) {

  if (auto pct = parse_fee_percent(cut_str))
    return get_portions_from_percent(*pct, portions);

  return false;
}

} // namespace service_nodes


#include <algorithm>
//#include <boost/iterator/counting_iterator.hpp>
#include <ibs>
#include <iterator>
#include <math.h>
#include <numeric>
#include <vector>
// gets the particle times and uses the phase acceptance and number of bins
// to generate an integer which is than binned/histogram by
// ParticleTimesToHistogram
struct ParticleTimesToInteger {
  /*
  0 -> tauahat = half the phase acceptance
  1 -> nbins = numbers of bins to use in binning
  2 -> t synchronous to shift the center of the distribution to zero for the
  binning
  */
  int operator()(std::vector<double> &data, std::vector<double> &params) const {
    double out;
    double dtsamp2 = 2 * params[0] / params[1];
    // std::printf("%-30s %12.8e\n", "dtsamp2", dtsamp2);
    out = (data[4] - params[2] + params[0]) / dtsamp2;
    // std::printf("%-30s %12.8e\n", "data[4]", data[4]);
    // std::printf("%-30s %12.8e\n", "out doub", out);
    out = (int)(out + 0.5);
    // std::printf("%-30s %12.8e\n", "out int", out);
    return out;
  }
};

void denseHistogram(std::vector<int> data, std::vector<int> &histogram,
                    int nbins, double tauhat) {
  std::vector<int> input = data;
  std::sort(input.begin(), input.end());

  int numBins = nbins + 1;
  histogram.resize(numBins);
  for (int i = 0; i < numBins; i++) {
    auto upper = std::upper_bound(input.begin(), input.end(), i);
    histogram[i] = std::distance(input.begin(), upper);
  }
  /*
    for (std::vector<int>::iterator i = histogram.begin(); i != histogram.end();
         i++) {
      std::printf("%12i\n", *i);
    }
  */
  std::adjacent_difference(histogram.begin(), histogram.end(),
                           histogram.begin());
}

std::vector<int>
ParticlesTimesToHistogram(std::vector<std::vector<double>> &data, int nbins,
                          double tauhat, double ts) {

  int n = data.size();
  std::vector<int> timecomponent(n);
  std::vector<int> histogram;
  std::vector<double> param;

  param.push_back(tauhat);
  param.push_back(nbins);
  param.push_back(ts);
  std::vector<std::vector<double>> p(n);
  std::fill(p.begin(), p.end(), param);
  /*
  std::printf("%-30s %12.8e\n", "tauhat", tauhat);
  std::printf("%-30s %12i\n", "nbins", nbins);
  std::printf("%-30s %12.8e\n", "ts", ts);

  for (std::vector<double>::iterator i = param.begin(); i != param.end(); i++) {
    std::printf("%12.8e\n", *i);
  }

  // print out for debug
  std::for_each(p.begin(), p.end(), [](std::vector<double> &particle) {
    std::printf("%12.8e %12.8e %12.8e \n", particle[0], particle[1],
                particle[2]);
  });

  std::printf("\n");
*/
  std::transform(data.begin(), data.end(), p.begin(), timecomponent.begin(),
                 ParticleTimesToInteger());
  /*
    for (std::vector<int>::iterator i = timecomponent.begin();
         i != timecomponent.end(); i++) {
      std::printf("%12i\n", *i);
    }
    */
  denseHistogram(timecomponent, histogram, nbins, tauhat);
  /*
  for (std::vector<int>::iterator i = histogram.begin(); i != histogram.end();
       i++) {
    std::printf("%3i", *i);
  };
  std::printf("\n");
  */
  return histogram;
}

/*
 ********************************************************************************
 ********************************************************************************
 * REF:
 * https://stackoverflow.com/questions/14924912/computing-column-sums-of-matrix-vectorvectordouble-with-iterators
 ********************************************************************************
 */
std::vector<double> getColumnMeans(std::vector<std::vector<double>> &dist) {
  std::vector<double> colsums(dist[0].size());

  std::for_each(dist.begin(), dist.end(), [&](const std::vector<double> &row) {
    std::transform(
        row.begin(), row.end(), colsums.begin(), colsums.begin(),
        [&](double d1, double d2) { return (d1 + d2) / dist.size(); });
  });

  return colsums;
}

std::vector<double> vectorMultiply(std::vector<double> x,
                                   std::vector<double> y) {
  std::transform(x.begin(), x.end(), y.begin(), x.begin(),
                 std::multiplies<double>());
  return x;
}

std::vector<double> vectorAdd(std::vector<double> x, std::vector<double> y) {
  std::transform(x.begin(), x.end(), y.begin(), x.begin(), std::plus<double>());
  return x;
}

std::vector<double> vectorSub(std::vector<double> x, std::vector<double> y) {
  std::transform(x.begin(), x.end(), y.begin(), x.begin(),
                 std::minus<double>());
  return x;
}
std::vector<double> CalcRMS(std::vector<std::vector<double>> &dist) {
  std::vector<double> avg = getColumnMeans(dist);

  // for (std::vector<double>::const_iterator i = avg.begin(); i != avg.end();
  // ++i)
  //  std::printf("%-30s %12.8e\n", "avg", *i);

  std::vector<std::vector<double>> avgarr(dist.size());
  std::fill(avgarr.begin(), avgarr.end(), avg);

  // std::for_each(avgarr.begin(), avgarr.end(), [](std::vector<double> &a) {
  //  std::printf("%12.8e %12.8e %12.8e %12.8e %12.8e %12.8e\n", a[0], a[1],
  //  a[2],
  //              a[3], a[4], a[5]);
  //});
  std::vector<std::vector<double>> distcopy = dist;
  std::transform(dist.begin(), dist.end(), avgarr.begin(), distcopy.begin(),
                 vectorSub);
  std::transform(distcopy.begin(), distcopy.end(), distcopy.begin(),
                 distcopy.begin(), vectorMultiply);

  std::vector<double> MS = getColumnMeans(distcopy);
  std::transform(MS.begin(), MS.end(), MS.begin(), (double (*)(double))sqrt);

  return MS;
}

std::vector<double> HistogramToSQRTofCumul(std::vector<int> inputHistogram,
                                           double coeff) {

  /* The name is a badly chose here as it was originally used to calculate
   * cumulated distributions which      */
  /* turned out to be not necessary - the function takes a histogram as input,
   * multiplies it with a constant  */
  /* vector before taking the sqrt of each element. This produces a vector
   * that is used in the IBSNew routine */
  /* to multiply with particle momenta representing the IBS contribution */
  int n = inputHistogram.size();

  std::vector<double> vcoeff(n);
  std::fill(vcoeff.begin(), vcoeff.end(), coeff);
  /*
  for (std::vector<double>::const_iterator i = vcoeff.begin();
       i != vcoeff.end(); ++i)
    std::printf("%-30s %12.8e\n", "avg", *i);
    */
  // fill constant vector

  // multiply with constant
  std::transform(inputHistogram.begin(), inputHistogram.end(), vcoeff.begin(),
                 vcoeff.begin(), std::multiplies<double>());

  // take sqrt
  std::transform(vcoeff.begin(), vcoeff.end(), vcoeff.begin(),
                 (double (*)(double))sqrt);

  return vcoeff;
}

std::map<std::string, double> readInput(std::string filename) {
  std::vector<std::string> ALLOWEDKEYS{
      "bucket",      "atomNumber", "charge",    "nMacro",
      "nReal",       "nbins",      "sigs",      "seed",
      "ex",          "ey",         "timeRatio", "fracibstot",
      "ibsCoupling", "model",      "nturns",    "nwrite"};

  std::map<std::string, double> out;
  std::string line;
  std::ifstream file(filename);

  std::getline(file, line);

  // check if file is open
  if (file.is_open()) {
    std::string key;
    double value;
    std::istringstream iss(line);
    iss >> key >> value;

    vector<string>::iterator it =
        find(ALLOWEDKEYS.begin(), ALLOWEDKEYS.end(), key);
    // cout << key << " " << value << " " << endl;
    if (it != ALLOWEDKEYS.end()) {
      out[key] = value;
    }
    while (!file.eof()) {
      std::getline(file, line);
      std::istringstream iss(line);
      iss >> key >> value;

      vector<string>::iterator it =
          find(ALLOWEDKEYS.begin(), ALLOWEDKEYS.end(), key);
      // cout << key << " " << value << " " << endl;
      if (it != ALLOWEDKEYS.end()) {
        out[key] = value;
      }
    }

    file.close();
  }
  return out;
}

bool isInLong(std::vector<double> particle, double tauhat, double synctime) {
  return !(abs(particle[4] - synctime) < tauhat);
}


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

  @copyright 2016 NumScale SAS

  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_SIMD_FUNCTION_ULPDIST_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ULPDIST_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/constant/eps.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD_IF(ulpdist_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using itype = bd::as_integer_t<A0>;
        itype e1, e2;
        A0 m1, m2;
        std::tie(m1, e1) = pedantic_(ifrexp)(a0);
        std::tie(m2, e2) = pedantic_(ifrexp)(a1);
        itype expo = -bs::max(e1, e2);
        A0 e = if_else( bs::is_equal(e1, e2)
                      , bs::abs(m1-m2)
                      , bs::abs( bs::pedantic_(ldexp)(a0, expo)
                                 - bs::pedantic_(ldexp)(a1, expo))
                      );
        return if_zero_else(logical_or( logical_and(bs::is_nan(a0), bs::is_nan(a1))
                                      , bs::is_equal(a0, a1)
                                      )
                           , e/Eps<A0>()
                           );
      }
   };
} } }

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_SIMD_FUNCTION_ULPDIST_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ULPDIST_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/frexp.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/if_zero_else.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#include <boost/simd/function/simd/is_nan.hpp>
#include <boost/simd/function/simd/ldexp.hpp>
#include <boost/simd/function/simd/logical_and.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#include <boost/simd/function/simd/max.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/constant/eps.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD(ulpdist_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using itype = bd::as_integer_t<A0>;
        itype e1, e2;
        A0 m1 = bs::frexp(a0, e1);
        A0 m2 = bs::frexp(a1, e2);
        itype expo = -bs::max(e1, e2);
        A0 e = if_else( bs::is_equal(e1, e2)
                      , bs::abs(m1-m2)
                      , bs::abs( bs::ldexp(a0, expo) - bs::ldexp(a1, expo))
                      );
        return if_zero_else(logical_or( logical_and(bs::is_nan(a0), bs::is_nan(a1))
                                      , bs::is_equal(a0, a1)
                                      )
                           , e/Eps<A0>()
                           );
      }
   };
} } }

#endif


/// ---------------------------------------------------------------------------
/// @section LICENSE
///
/// Copyright (c) 2016 Georgia Tech Research Institute (GTRI)
///               All Rights Reserved
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
/// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
/// DEALINGS IN THE SOFTWARE.
/// ---------------------------------------------------------------------------
/// @file filename.ext
/// @author Kevin DeMarco <kevin.demarco@gtri.gatech.edu>
/// @author Eric Squires <eric.squires@gtri.gatech.edu>
/// @version 1.0
/// ---------------------------------------------------------------------------
/// @brief A brief description.
///
/// @section DESCRIPTION
/// A long description.
/// ---------------------------------------------------------------------------
#include <scrimmage/common/Utilities.h>
#include <scrimmage/parse/ParseUtils.h>
#include <scrimmage/plugin_manager/RegisterPlugin.h>
#include <scrimmage/entity/Entity.h>
#include <scrimmage/math/State.h>
#include "DoubleIntegrator.h"
#include <scrimmage/entity/Entity.h>
#include <boost/algorithm/clamp.hpp>

REGISTER_PLUGIN(scrimmage::MotionModel, DoubleIntegrator, DoubleIntegrator_plugin)

namespace pl = std::placeholders;
using boost::algorithm::clamp;

enum ModelParams {X, Y, Z, VX, VY, VZ, STATE_SIZE};

DoubleIntegrator::DoubleIntegrator() {
    x_.resize(STATE_SIZE);
}

bool DoubleIntegrator::init(std::map<std::string, std::string> &info,
                            std::map<std::string, std::string> &params) {
    max_vel_ = std::stod(params.at("max_vel"));
    max_acc_ = std::stod(params.at("max_acc"));

    x_[X] = state_->pos()(0);
    x_[Y] = state_->pos()(1);
    x_[Z] = state_->pos()(2);

    x_[VX] = clamp(state_->vel()(0), -max_vel_, max_vel_);
    x_[VY] = clamp(state_->vel()(1), -max_vel_, max_vel_);
    x_[VZ] = clamp(state_->vel()(2), -max_vel_, max_vel_);

    state_->pos() << x_[X], x_[Y], x_[Z];

    return true;
}

bool DoubleIntegrator::step(double t, double dt) {
    u_ = std::static_pointer_cast<Controller>(parent_.lock()->controllers().back())->u();
    u_(0) = clamp(u_(0), -max_acc_, max_acc_);
    u_(1) = clamp(u_(1), -max_acc_, max_acc_);
    u_(2) = clamp(u_(2), -max_acc_, max_acc_);

    ode_step(dt);

    x_[VX] = clamp(x_[VX], -max_vel_, max_vel_);
    x_[VY] = clamp(x_[VY], -max_vel_, max_vel_);
    x_[VZ] = clamp(x_[VZ], -max_vel_, max_vel_);

    state_->pos() << x_[X], x_[Y], x_[Z];
    state_->vel() << x_[VX], x_[VY], x_[VZ];
    state_->quat().set(0, 0, atan2(x_[VY], x_[VX]));
    return true;
}

double DoubleIntegrator::update_dvdt(double vel, double acc) {
    if (vel >= max_vel_ && acc > 0) {
        return 0;
    } else if (vel <= -max_vel_ && acc < 0) {
        return 0;
    } else {
        return acc;
    }
}

void DoubleIntegrator::model(const vector_t &x , vector_t &dxdt , double t) {
    dxdt[X] = clamp(x_[VX], -max_vel_, max_vel_);
    dxdt[Y] = clamp(x_[VY], -max_vel_, max_vel_);
    dxdt[Z] = clamp(x_[VZ], -max_vel_, max_vel_);

    dxdt[VX] = update_dvdt(x[VX], u_(0));
    dxdt[VY] = update_dvdt(x[VY], u_(1));
    dxdt[VZ] = update_dvdt(x[VZ], u_(2));
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

template<typename Scalar, int Mode, int Options> void non_projective_only()
{
    /* this test covers the following files:
     Cross.h Quaternion.h, Transform.cpp
  */
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Quaternion<Scalar> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;
  typedef Transform<Scalar,3,Mode,Options> Transform3;
  typedef DiagonalMatrix<Scalar,3> AlignedScaling3;
  typedef Translation<Scalar,3> Translation3;

  Vector3 v0 = Vector3::Random(),
          v1 = Vector3::Random();

  Transform3 t0, t1, t2;

  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));

  Quaternionx q1, q2;

  q1 = AngleAxisx(a, v0.normalized());

  t0 = Transform3::Identity();
  VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());

  t0.linear() = q1.toRotationMatrix();

  v0 << 50, 2, 1;
  t0.scale(v0);

  VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template head<3>().norm(), v0.x());

  t0.setIdentity();
  t1.setIdentity();
  v1 << 1, 2, 3;
  t0.linear() = q1.toRotationMatrix();
  t0.pretranslate(v0);
  t0.scale(v1);
  t1.linear() = q1.conjugate().toRotationMatrix();
  t1.prescale(v1.cwiseInverse());
  t1.translate(-v0);

  VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>()));

  t1.fromPositionOrientationScale(v0, q1, v1);
  VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
  VERIFY_IS_APPROX(t1*v1, t0*v1);

  // translation * vector
  t0.setIdentity();
  t0.translate(v0);
  VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1);

  // AlignedScaling * vector
  t0.setIdentity();
  t0.scale(v0);
  VERIFY_IS_APPROX((t0 * v1).template head<3>(), AlignedScaling3(v0) * v1);
}

template<typename Scalar, int Mode, int Options> void transformations()
{
  /* this test covers the following files:
     Cross.h Quaternion.h, Transform.cpp
  */
  using std::cos;
  using std::abs;
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,4,4> Matrix4;
  typedef Matrix<Scalar,2,1> Vector2;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Matrix<Scalar,4,1> Vector4;
  typedef Quaternion<Scalar> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;
  typedef Transform<Scalar,2,Mode,Options> Transform2;
  typedef Transform<Scalar,3,Mode,Options> Transform3;
  typedef typename Transform3::MatrixType MatrixType;
  typedef DiagonalMatrix<Scalar,3> AlignedScaling3;
  typedef Translation<Scalar,2> Translation2;
  typedef Translation<Scalar,3> Translation3;

  Vector3 v0 = Vector3::Random(),
          v1 = Vector3::Random();
  Matrix3 matrot1, m;

  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
  Scalar s0 = internal::random<Scalar>(), s1 = internal::random<Scalar>();

  while(v0.norm() < test_precision<Scalar>()) v0 = Vector3::Random();
  while(v1.norm() < test_precision<Scalar>()) v1 = Vector3::Random();

  VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0);
  VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(EIGEN_PI), v0.unitOrthogonal()) * v0);
  if(abs(cos(a)) > test_precision<Scalar>())
  {
    VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0));
  }
  m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint();
  VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized()));
  VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m);

  Quaternionx q1, q2;
  q1 = AngleAxisx(a, v0.normalized());
  q2 = AngleAxisx(a, v1.normalized());

  // rotation matrix conversion
  matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX())
          * AngleAxisx(Scalar(0.2), Vector3::UnitY())
          * AngleAxisx(Scalar(0.3), Vector3::UnitZ());
  VERIFY_IS_APPROX(matrot1 * v1,
       AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix()
    * (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix()
    * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1)));

  // angle-axis conversion
  AngleAxisx aa = AngleAxisx(q1);
  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);

  if(abs(aa.angle()) > NumTraits<Scalar>::dummy_precision())
  {
    VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
  }

  aa.fromRotationMatrix(aa.toRotationMatrix());
  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
  if(abs(aa.angle()) > NumTraits<Scalar>::dummy_precision())
  {
    VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
  }

  // AngleAxis
  VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(),
    Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix());

  AngleAxisx aa1;
  m = q1.toRotationMatrix();
  aa1 = m;
  VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(),
    Quaternionx(m).toRotationMatrix());

  // Transform
  // TODO complete the tests !
  a = 0;
  while (abs(a)<Scalar(0.1))
    a = internal::random<Scalar>(-Scalar(0.4)*Scalar(EIGEN_PI), Scalar(0.4)*Scalar(EIGEN_PI));
  q1 = AngleAxisx(a, v0.normalized());
  Transform3 t0, t1, t2;

  // first test setIdentity() and Identity()
  t0.setIdentity();
  VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
  t0.matrix().setZero();
  t0 = Transform3::Identity();
  VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());

  t0.setIdentity();
  t1.setIdentity();
  v1 << 1, 2, 3;
  t0.linear() = q1.toRotationMatrix();
  t0.pretranslate(v0);
  t0.scale(v1);
  t1.linear() = q1.conjugate().toRotationMatrix();
  t1.prescale(v1.cwiseInverse());
  t1.translate(-v0);

  VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>()));

  t1.fromPositionOrientationScale(v0, q1, v1);
  VERIFY_IS_APPROX(t1.matrix(), t0.matrix());

  t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix());
  t1.setIdentity(); t1.scale(v0).rotate(q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix());
  VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix());

  // More transform constructors, operator=, operator*=

  Matrix3 mat3 = Matrix3::Random();
  Matrix4 mat4;
  mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose();
  Transform3 tmat3(mat3), tmat4(mat4);
  if(Mode!=int(AffineCompact))
    tmat4.matrix()(3,3) = Scalar(1);
  VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix());

  Scalar a3 = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
  Vector3 v3 = Vector3::Random().normalized();
  AngleAxisx aa3(a3, v3);
  Transform3 t3(aa3);
  Transform3 t4;
  t4 = aa3;
  VERIFY_IS_APPROX(t3.matrix(), t4.matrix());
  t4.rotate(AngleAxisx(-a3,v3));
  VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity());
  t4 *= aa3;
  VERIFY_IS_APPROX(t3.matrix(), t4.matrix());

  v3 = Vector3::Random();
  Translation3 tv3(v3);
  Transform3 t5(tv3);
  t4 = tv3;
  VERIFY_IS_APPROX(t5.matrix(), t4.matrix());
  t4.translate(-v3);
  VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity());
  t4 *= tv3;
  VERIFY_IS_APPROX(t5.matrix(), t4.matrix());

  AlignedScaling3 sv3(v3);
  Transform3 t6(sv3);
  t4 = sv3;
  VERIFY_IS_APPROX(t6.matrix(), t4.matrix());
  t4.scale(v3.cwiseInverse());
  VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity());
  t4 *= sv3;
  VERIFY_IS_APPROX(t6.matrix(), t4.matrix());

  // matrix * transform
  VERIFY_IS_APPROX((t3.matrix()*t4).matrix(), (t3*t4).matrix());

  // chained Transform product
  VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix());

  // check that Transform product doesn't have aliasing problems
  t5 = t4;
  t5 = t5*t5;
  VERIFY_IS_APPROX(t5, t4*t4);

  // 2D transformation
  Transform2 t20, t21;
  Vector2 v20 = Vector2::Random();
  Vector2 v21 = Vector2::Random();
  for (int k=0; k<2; ++k)
    if (abs(v21[k])<Scalar(1e-3)) v21[k] = Scalar(1e-3);
  t21.setIdentity();
  t21.linear() = Rotation2D<Scalar>(a).toRotationMatrix();
  VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(),
    t21.pretranslate(v20).scale(v21).matrix());

  t21.setIdentity();
  t21.linear() = Rotation2D<Scalar>(-a).toRotationMatrix();
  VERIFY( (t20.fromPositionOrientationScale(v20,a,v21)
        * (t21.prescale(v21.cwiseInverse()).translate(-v20))).matrix().isIdentity(test_precision<Scalar>()) );

  // Transform - new API
  // 3D
  t0.setIdentity();
  t0.rotate(q1).scale(v0).translate(v0);
  // mat * aligned scaling and mat * translation
  t1 = (Matrix3(q1) * AlignedScaling3(v0)) * Translation3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  t1 = (Matrix3(q1) * Eigen::Scaling(v0)) * Translation3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  t1 = (q1 * Eigen::Scaling(v0)) * Translation3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // mat * transformation and aligned scaling * translation
  t1 = Matrix3(q1) * (AlignedScaling3(v0) * Translation3(v0));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0.setIdentity();
  t0.scale(s0).translate(v0);
  t1 = Eigen::Scaling(s0) * Translation3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  t0.prescale(s0);
  t1 = Eigen::Scaling(s0) * t1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0 = t3;
  t0.scale(s0);
  t1 = t3 * Eigen::Scaling(s0,s0,s0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  t0.prescale(s0);
  t1 = Eigen::Scaling(s0,s0,s0) * t1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0 = t3;
  t0.scale(s0);
  t1 = t3 * Eigen::Scaling(s0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  t0.prescale(s0);
  t1 = Eigen::Scaling(s0) * t1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0.setIdentity();
  t0.prerotate(q1).prescale(v0).pretranslate(v0);
  // translation * aligned scaling and transformation * mat
  t1 = (Translation3(v0) * AlignedScaling3(v0)) * Transform3(q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // scaling * mat and translation * mat
  t1 = Translation3(v0) * (AlignedScaling3(v0) * Transform3(q1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  t0.setIdentity();
  t0.scale(v0).translate(v0).rotate(q1);
  // translation * mat and aligned scaling * transformation
  t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // transformation * aligned scaling
  t0.scale(v0);
  t1 *= AlignedScaling3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // transformation * translation
  t0.translate(v0);
  t1 = t1 * Translation3(v0);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
  // translation * transformation
  t0.pretranslate(v0);
  t1 = Translation3(v0) * t1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // transform * quaternion
  t0.rotate(q1);
  t1 = t1 * q1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // translation * quaternion
  t0.translate(v1).rotate(q1);
  t1 = t1 * (Translation3(v1) * q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // aligned scaling * quaternion
  t0.scale(v1).rotate(q1);
  t1 = t1 * (AlignedScaling3(v1) * q1);
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // quaternion * transform
  t0.prerotate(q1);
  t1 = q1 * t1;
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // quaternion * translation
  t0.rotate(q1).translate(v1);
  t1 = t1 * (q1 * Translation3(v1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // quaternion * aligned scaling
  t0.rotate(q1).scale(v1);
  t1 = t1 * (q1 * AlignedScaling3(v1));
  VERIFY_IS_APPROX(t0.matrix(), t1.matrix());

  // test transform inversion
  t0.setIdentity();
  t0.translate(v0);
  do {
    t0.linear().setRandom();
  } while(t0.linear().jacobiSvd().singularValues()(2)<test_precision<Scalar>());
  Matrix4 t044 = Matrix4::Zero();
  t044(3,3) = 1;
  t044.block(0,0,t0.matrix().rows(),4) = t0.matrix();
  VERIFY_IS_APPROX(t0.inverse(Affine).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4));
  t0.setIdentity();
  t0.translate(v0).rotate(q1);
  t044 = Matrix4::Zero();
  t044(3,3) = 1;
  t044.block(0,0,t0.matrix().rows(),4) = t0.matrix();
  VERIFY_IS_APPROX(t0.inverse(Isometry).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4));

  Matrix3 mat_rotation, mat_scaling;
  t0.setIdentity();
  t0.translate(v0).rotate(q1).scale(v1);
  t0.computeRotationScaling(&mat_rotation, &mat_scaling);
  VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling);
  VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
  VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));
  t0.computeScalingRotation(&mat_scaling, &mat_rotation);
  VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation);
  VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
  VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));

  // test casting
  Transform<float,3,Mode> t1f = t1.template cast<float>();
  VERIFY_IS_APPROX(t1f.template cast<Scalar>(),t1);
  Transform<double,3,Mode> t1d = t1.template cast<double>();
  VERIFY_IS_APPROX(t1d.template cast<Scalar>(),t1);

  Translation3 tr1(v0);
  Translation<float,3> tr1f = tr1.template cast<float>();
  VERIFY_IS_APPROX(tr1f.template cast<Scalar>(),tr1);
  Translation<double,3> tr1d = tr1.template cast<double>();
  VERIFY_IS_APPROX(tr1d.template cast<Scalar>(),tr1);

  AngleAxis<float> aa1f = aa1.template cast<float>();
  VERIFY_IS_APPROX(aa1f.template cast<Scalar>(),aa1);
  AngleAxis<double> aa1d = aa1.template cast<double>();
  VERIFY_IS_APPROX(aa1d.template cast<Scalar>(),aa1);

  Rotation2D<Scalar> r2d1(internal::random<Scalar>());
  Rotation2D<float> r2d1f = r2d1.template cast<float>();
  VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1);
  Rotation2D<double> r2d1d = r2d1.template cast<double>();
  VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);

  for(int k=0; k<100; ++k)
  {
    Scalar angle = internal::random<Scalar>(-100,100);
    Rotation2D<Scalar> rot2(angle);
    VERIFY( rot2.smallestPositiveAngle() >= 0 );
    VERIFY( rot2.smallestPositiveAngle() < Scalar(2)*Scalar(EIGEN_PI) );
    VERIFY_IS_APPROX( std::cos(rot2.smallestPositiveAngle()), std::cos(rot2.angle()) );
    VERIFY_IS_APPROX( std::sin(rot2.smallestPositiveAngle()), std::sin(rot2.angle()) );

    VERIFY( rot2.smallestAngle() >= -Scalar(EIGEN_PI) );
    VERIFY( rot2.smallestAngle() <=  Scalar(EIGEN_PI) );
    VERIFY_IS_APPROX( std::cos(rot2.smallestAngle()), std::cos(rot2.angle()) );
    VERIFY_IS_APPROX( std::sin(rot2.smallestAngle()), std::sin(rot2.angle()) );
  }

  s0 = internal::random<Scalar>(-100,100);
  s1 = internal::random<Scalar>(-100,100);
  Rotation2D<Scalar> R0(s0), R1(s1);

  t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0));
  t21 = Translation2(v20) * R0 * Eigen::Scaling(s0);
  VERIFY_IS_APPROX(t20,t21);

  t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0));
  t21 = Translation2(v20) * Eigen::Scaling(s0);
  VERIFY_IS_APPROX(t20,t21);

  VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle());
  VERIFY_IS_APPROX(R1.smallestPositiveAngle(), (R0.slerp(1, R1)).smallestPositiveAngle());
  VERIFY_IS_APPROX(R0.smallestPositiveAngle(), (R0.slerp(0.5, R0)).smallestPositiveAngle());

  if(std::cos(s0)>0)
    VERIFY_IS_MUCH_SMALLER_THAN((R0.slerp(0.5, R0.inverse())).smallestAngle(), Scalar(1));
  else
    VERIFY_IS_APPROX(Scalar(EIGEN_PI), (R0.slerp(0.5, R0.inverse())).smallestPositiveAngle());

  // Check path length
  Scalar l = 0;
  for(int k=0; k<100; ++k)
  {
    Scalar a1 = R0.slerp(Scalar(k)/Scalar(100), R1).angle();
    Scalar a2 = R0.slerp(Scalar(k+1)/Scalar(100), R1).angle();
    l += std::abs(a2-a1);
  }
  VERIFY(l<=EIGEN_PI);

  // check basic features
  {
    Rotation2D<Scalar> r1;           // default ctor
    r1 = Rotation2D<Scalar>(s0);     // copy assignment
    VERIFY_IS_APPROX(r1.angle(),s0);
    Rotation2D<Scalar> r2(r1);       // copy ctor
    VERIFY_IS_APPROX(r2.angle(),s0);
  }
}

template<typename Scalar> void transform_alignment()
{
  typedef Transform<Scalar,3,Projective,AutoAlign> Projective3a;
  typedef Transform<Scalar,3,Projective,DontAlign> Projective3u;

  EIGEN_ALIGN_MAX Scalar array1[16];
  EIGEN_ALIGN_MAX Scalar array2[16];
  EIGEN_ALIGN_MAX Scalar array3[16+1];
  Scalar* array3u = array3+1;

  Projective3a *p1 = ::new(reinterpret_cast<void*>(array1)) Projective3a;
  Projective3u *p2 = ::new(reinterpret_cast<void*>(array2)) Projective3u;
  Projective3u *p3 = ::new(reinterpret_cast<void*>(array3u)) Projective3u;

  p1->matrix().setRandom();
  *p2 = *p1;
  *p3 = *p1;

  VERIFY_IS_APPROX(p1->matrix(), p2->matrix());
  VERIFY_IS_APPROX(p1->matrix(), p3->matrix());

  VERIFY_IS_APPROX( (*p1) * (*p1), (*p2)*(*p3));

  #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
  if(internal::packet_traits<Scalar>::Vectorizable)
    VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Projective3a));
  #endif
}

template<typename Scalar, int Dim, int Options> void transform_products()
{
  typedef Matrix<Scalar,Dim+1,Dim+1> Mat;
  typedef Transform<Scalar,Dim,Projective,Options> Proj;
  typedef Transform<Scalar,Dim,Affine,Options> Aff;
  typedef Transform<Scalar,Dim,AffineCompact,Options> AffC;

  Proj p; p.matrix().setRandom();
  Aff a; a.linear().setRandom(); a.translation().setRandom();
  AffC ac = a;

  Mat p_m(p.matrix()), a_m(a.matrix());

  VERIFY_IS_APPROX((p*p).matrix(), p_m*p_m);
  VERIFY_IS_APPROX((a*a).matrix(), a_m*a_m);
  VERIFY_IS_APPROX((p*a).matrix(), p_m*a_m);
  VERIFY_IS_APPROX((a*p).matrix(), a_m*p_m);
  VERIFY_IS_APPROX((ac*a).matrix(), a_m*a_m);
  VERIFY_IS_APPROX((a*ac).matrix(), a_m*a_m);
  VERIFY_IS_APPROX((p*ac).matrix(), p_m*a_m);
  VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m);
}

void test_geo_transformations()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(( transformations<double,Affine,AutoAlign>() ));
    CALL_SUBTEST_1(( non_projective_only<double,Affine,AutoAlign>() ));

    CALL_SUBTEST_2(( transformations<float,AffineCompact,AutoAlign>() ));
    CALL_SUBTEST_2(( non_projective_only<float,AffineCompact,AutoAlign>() ));
    CALL_SUBTEST_2(( transform_alignment<float>() ));

    CALL_SUBTEST_3(( transformations<double,Projective,AutoAlign>() ));
    CALL_SUBTEST_3(( transformations<double,Projective,DontAlign>() ));
    CALL_SUBTEST_3(( transform_alignment<double>() ));

    CALL_SUBTEST_4(( transformations<float,Affine,RowMajor|AutoAlign>() ));
    CALL_SUBTEST_4(( non_projective_only<float,Affine,RowMajor>() ));

    CALL_SUBTEST_5(( transformations<double,AffineCompact,RowMajor|AutoAlign>() ));
    CALL_SUBTEST_5(( non_projective_only<double,AffineCompact,RowMajor>() ));

    CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|AutoAlign>() ));
    CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|DontAlign>() ));

    CALL_SUBTEST_7(( transform_products<double,3,RowMajor|AutoAlign>() ));
    CALL_SUBTEST_7(( transform_products<float,2,AutoAlign>() ));
  }
}


#include "drake/solvers/fbstab/components/riccati_linear_solver.h"

#include <cmath>
#include <iostream>
#include <stdexcept>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/mpc_data.h"
#include "drake/solvers/fbstab/components/mpc_residual.h"
#include "drake/solvers/fbstab/components/mpc_variable.h"

namespace drake {
namespace solvers {
namespace fbstab {

using VectorXd = Eigen::VectorXd;
using MatrixXd = Eigen::MatrixXd;

RiccatiLinearSolver::RiccatiLinearSolver(int N, int nx, int nu, int nc) {
  if (N <= 0 || nx <= 0 || nu <= 0 || nc <= 0) {
    throw std::runtime_error(
        "In RiccatiLinearSolver::RiccatiLinearSolver: all inputs must be "
        "positive.");
  }
  N_ = N;
  nx_ = nx;
  nu_ = nu;
  nc_ = nc;
  nz_ = (N_ + 1) * (nx_ + nu_);
  nl_ = (N_ + 1) * (nx_);
  nv_ = (N_ + 1) * (nc_);

  Q_.resize(N + 1);
  S_.resize(N + 1);
  R_.resize(N + 1);

  P_.resize(N + 1);
  SG_.resize(N + 1);
  M_.resize(N + 1);
  L_.resize(N + 1);
  SM_.resize(N + 1);
  AM_.resize(N + 1);

  h_.resize(N + 1);
  th_.resize(N + 1);

  for (int i = 0; i < N + 1; i++) {
    Q_[i].resize(nx, nx);
    S_[i].resize(nu, nx);
    R_[i].resize(nu, nu);

    P_[i].resize(nx, nu);
    SG_[i].resize(nu, nu);
    M_[i].resize(nx, nx);
    L_[i].resize(nx, nx);
    SM_[i].resize(nu, nx);
    AM_[i].resize(nx, nx);

    h_[i].resize(nx);
    th_[i].resize(nx);
  }

  gamma_.resize(nv_);
  mus_.resize(nv_);
  Gamma_.resize(nc, N + 1);

  Etemp_.resize(nc, nx);
  Ltemp_.resize(nc, nu);
  Linv_.resize(nx, nx);

  tx_.resize(nx);
  tu_.resize(nu);
  tl_.resize(nx);
  r1_.resize(nz_);
  r2_.resize(nl_);
  r3_.resize(nv_);
}

bool RiccatiLinearSolver::Initialize(const MpcVariable& x,
                                     const MpcVariable& xbar, double sigma) {
  const MpcData* const data = x.data();
  if (xbar.data_ != data) {
    throw std::runtime_error(
        "In RiccatiLinearSolver::Initialize: x and xbar have mismatched "
        "problem data.");
  }
  if (!MpcVariable::SameSize(x, xbar)) {
    throw std::runtime_error(
        "In RiccatiLinearSolver::Initialize: x and xbar are not the same "
        "size.");
  }
  if (sigma <= 0) {
    throw std::runtime_error(
        "In RiccatiLinearSolver::Initialize: sigma must be positive.");
  }

  Eigen::Vector2d temp;
  for (int i = 0; i < nv_; i++) {
    const double ys = x.y()(i) + sigma * (x.v()(i) - xbar.v()(i));
    temp = PFBGradient(ys, x.v()(i));

    gamma_(i) = temp(0);
    mus_(i) = temp(1) + sigma * temp(0);
    Gamma_(i) = gamma_(i) / mus_(i);
  }
  // Compute the barrier augmented Hessian.
  for (int i = 0; i < N_ + 1; i++) {
    const MatrixXd& Ei = (*data->E_)[i];
    const MatrixXd& Li = (*data->L_)[i];

    Q_[i].triangularView<Eigen::Lower>() =
        (*data->Q_)[i] + sigma * MatrixXd::Identity(nx_, nx_);
    R_[i].triangularView<Eigen::Lower>() =
        (*data->R_)[i] + sigma * MatrixXd::Identity(nu_, nu_);
    S_[i] = (*data->S_)[i];

    // Add barriers associated with E(i)x(i) + L(i)u(i) + d(i) <=0
    // Q(i) += E(i)'*diag(Gamma(i))*E(i)
    Etemp_.noalias() = Gamma_.col(i).asDiagonal() * Ei;
    Q_[i].triangularView<Eigen::Lower>() += Ei.transpose() * Etemp_;

    // R(i) += L(i)'*diag(Gamma(i))*L(i)
    Ltemp_.noalias() = Gamma_.col(i).asDiagonal() * Li;
    R_[i].triangularView<Eigen::Lower>() += Li.transpose() * Ltemp_;

    // S(i) += L(i) ' * diag(Gamma(i)) * E(i)
    S_[i].noalias() += Li.transpose() * Etemp_;
  }

  // Begin the matrix potion of the Riccati recursion.
  // Base case: L(0) = chol(sigma*I).
  L_[0] = sqrt(sigma) * MatrixXd::Identity(nx_, nx_);

#define FBSTAB_LLT_CHECK(llt)           \
  {                                     \
    if (llt.info() != Eigen::Success) { \
      return false;                     \
    }                                   \
  }

  for (int i = 0; i < N_; i++) {
    // Compute inv(L(i)) then
    // compute QQ = Q+inv(L*L') = Q + inv(L)'*inv(L)
    // and factor M = chol(QQ) in place.
    Linv_ = MatrixXd::Identity(nx_, nx_);
    L_[i].triangularView<Eigen::Lower>().solveInPlace(Linv_);
    L_[i].triangularView<Eigen::Lower>().transpose().solveInPlace(Linv_);
    M_[i].triangularView<Eigen::Lower>() = Q_[i] + Linv_;
    Eigen::LLT<Eigen::Ref<MatrixXd>> llt1(M_[i]);
    FBSTAB_LLT_CHECK(llt1);

    // Compute AM = A*inv(M)'.
    AM_[i] = (*data->A_)[i];
    M_[i]
        .triangularView<Eigen::Lower>()
        .transpose()
        .solveInPlace<Eigen::OnTheRight>(AM_[i]);

    // Compute SM = S*inv(M)'.
    SM_[i] = S_[i];
    M_[i]
        .triangularView<Eigen::Lower>()
        .transpose()
        .solveInPlace<Eigen::OnTheRight>(SM_[i]);

    // Factor SG = chol(R - SM*SM') in place.
    SG_[i].noalias() = R_[i] - SM_[i] * SM_[i].transpose();
    Eigen::LLT<Eigen::Ref<MatrixXd>> llt2(SG_[i]);
    FBSTAB_LLT_CHECK(llt2);

    // Compute P = (A*inv(QQ)S' - B)*inv(SG)',
    //           = (AM*SM' - B)*inv(SG)'.
    P_[i].noalias() = AM_[i] * SM_[i].transpose();
    P_[i] -= (*data->B_)[i];
    SG_[i]
        .triangularView<Eigen::Lower>()
        .transpose()
        .solveInPlace<Eigen::OnTheRight>(P_[i]);

    // Compute L(i+1) = chol(Pi)
    // where Pi = P*P' + AM*AM' + sigma I.
    L_[i + 1] = sigma * MatrixXd::Identity(nx_, nx_);
    L_[i + 1].noalias() += P_[i] * P_[i].transpose();
    L_[i + 1].noalias() += AM_[i] * AM_[i].transpose();
    Eigen::LLT<Eigen::Ref<MatrixXd>> llt3(L_[i + 1]);
    FBSTAB_LLT_CHECK(llt3);
  }

  // Finish the recursion, i.e., perform the i = N step.
  Linv_ = MatrixXd::Identity(nx_, nx_);
  L_[N_].triangularView<Eigen::Lower>().solveInPlace(Linv_);
  L_[N_].triangularView<Eigen::Lower>().transpose().solveInPlace(Linv_);

  // Compute M = chol(Q + inv(L*L')).
  M_[N_].triangularView<Eigen::Lower>() = Q_[N_] + Linv_;
  Eigen::LLT<Eigen::Ref<MatrixXd>> llt4(M_[N_]);
  FBSTAB_LLT_CHECK(llt4);

  // Compute SM = S*inv(M)'.
  SM_[N_] = S_[N_];
  M_[N_]
      .triangularView<Eigen::Lower>()
      .transpose()
      .solveInPlace<Eigen::OnTheRight>(SM_[N_]);

  // Compute SG = chol(R - SM*SM').
  SG_[N_].noalias() = R_[N_] - SM_[N_] * SM_[N_].transpose();
  Eigen::LLT<Eigen::Ref<MatrixXd>> llt5(SG_[N_]);
  FBSTAB_LLT_CHECK(llt5);

#undef FBSTAB_LLT_CHECK
  return true;
}

bool RiccatiLinearSolver::Solve(const MpcResidual& r, MpcVariable* dx) const {
  const MpcData* const data = dx->data();
  if (r.nz_ != dx->nz_ || r.nl_ != dx->nl_ || r.nv_ != dx->nv_) {
    throw std::runtime_error(
        "In RiccatiLinearSolver::Solve: r and dx size mismatch.");
  }
  // Compute the post-elimination residual,
  // r1 = rz - A'*(rv./mus) and r2 = -rl.
  r1_ = r.z_;
  r3_ = r.v_.cwiseQuotient(mus_);  // r3_ is used as a temp here
  data->gemvAT(r3_, -1.0, 1.0, &r1_);
  r2_ = -r.l_;
  // Get reshaped aliases for r1 and r2.
  Eigen::Map<MatrixXd> r1(r1_.data(), nx_ + nu_, N_ + 1);
  Eigen::Map<MatrixXd> r2(r2_.data(), nx_, N_ + 1);

  // Begin the vector portion of the Riccati recursion.
  // Base case: theta(0) = -rl(0), h(0) = inv(L*L')*theta(0) - rx(0).
  th_[0] = r2.col(0);
  h_[0] = th_[0];
  L_[0].triangularView<Eigen::Lower>().solveInPlace(h_[0]);
  L_[0].triangularView<Eigen::Lower>().transpose().solveInPlace(h_[0]);
  h_[0].noalias() -= r1.block(0, 0, nx_, 1);  // r1(0) = [rx(0);ru(0)]

  // Main loop:
  for (int i = 0; i < N_; i++) {
    // Compute theta(i+1).
    // tx = inv(M)*h
    tx_ = h_[i];
    M_[i].triangularView<Eigen::Lower>().solveInPlace(tx_);

    // tu = inv(SG)*(SM*tx + ru)
    // r1(i) = [rx(i);ru(i)], block extracts ru(i)
    tu_.noalias() = SM_[i] * tx_;
    tu_.noalias() += r1.block(nx_, i, nu_, 1);
    SG_[i].triangularView<Eigen::Lower>().solveInPlace(tu_);

    const auto rlp = r2.col(i + 1);
    th_[i + 1].noalias() = P_[i] * tu_ + AM_[i] * tx_;
    th_[i + 1].noalias() += rlp;

    // Compute h(i+1).
    const auto rxp = r1.block(0, i + 1, nx_, 1);
    h_[i + 1] = th_[i + 1];
    L_[i + 1].triangularView<Eigen::Lower>().solveInPlace(h_[i + 1]);
    L_[i + 1].triangularView<Eigen::Lower>().transpose().solveInPlace(
        h_[i + 1]);
    h_[i + 1].noalias() -= rxp;
  }

  // Begin the backwards recursion for the solution
  // by computing xN,uN, and lN.
  // u(N) = inv(SG*SG')*(SM*inv(M)*h + ru)
  tx_ = h_[N_];
  M_[N_].triangularView<Eigen::Lower>().solveInPlace(tx_);
  tu_.noalias() = SM_[N_] * tx_;
  tu_ += r1.block(nx_, N_, nu_, 1);
  SG_[N_].triangularView<Eigen::Lower>().solveInPlace(tu_);
  SG_[N_].triangularView<Eigen::Lower>().transpose().solveInPlace(tu_);

  // x(N) = -inv(M')*(inv(M)*h + SM'*u(N))
  tx_ = h_[N_];
  M_[N_].triangularView<Eigen::Lower>().solveInPlace(tx_);
  tx_.noalias() += SM_[N_].transpose() * tu_;
  M_[N_].triangularView<Eigen::Lower>().transpose().solveInPlace(tx_);
  tx_ *= -1.0;

  // l(N) = -inv(L*L')* (xN + theta)
  tl_ = tx_ + th_[N_];
  L_[N_].triangularView<Eigen::Lower>().solveInPlace(tl_);
  L_[N_].triangularView<Eigen::Lower>().transpose().solveInPlace(tl_);
  tl_ *= -1.0;

  // Copy these into the solution vector.
  // Using reshaped aliases to make indexing easier.
  Eigen::Map<MatrixXd> dz(dx->z_->data(), nx_ + nu_, N_ + 1);
  Eigen::Map<MatrixXd> dl(dx->l_->data(), nx_, N_ + 1);

  dz.block(0, N_, nx_, 1) = tx_;
  dz.block(nx_, N_, nu_, 1) = tu_;
  dl.col(N_) = tl_;

  // The main backwards recursion loop.
  for (int i = N_ - 1; i >= 0; i--) {
    // Solve SG'*u(i) = inv(SG)*(SM*inv(M)*h + ru) + P'*l(i+1)
    tx_ = h_[i];
    M_[i].triangularView<Eigen::Lower>().solveInPlace(tx_);

    // This is an alias.
    auto ui = dz.block(nx_, i, nu_, 1);  // dz(i) = [xi;ui], extract ui
    ui.noalias() = SM_[i] * tx_;
    ui += r1.block(nx_, i, nu_, 1);  // SM*tx + ru
    SG_[i].triangularView<Eigen::Lower>().solveInPlace(ui);
    ui.noalias() += P_[i].transpose() * dl.col(i + 1);
    SG_[i].triangularView<Eigen::Lower>().transpose().solveInPlace(ui);

    // Solve -M'*x(i) = inv(M)*h + SM'*u(i) + AM'*l(i+1)
    // This is an alias.
    auto xi = dz.block(0, i, nx_, 1);

    xi = h_[i];
    M_[i].triangularView<Eigen::Lower>().solveInPlace(xi);
    xi.noalias() += SM_[i].transpose() * ui;
    xi.noalias() += AM_[i].transpose() * dl.col(i + 1);
    M_[i].triangularView<Eigen::Lower>().transpose().solveInPlace(xi);
    xi *= -1.0;

    // Solve -L*L' * l(i) = theta(i) + x(i).
    auto li = dl.col(i);
    li = th_[i] + xi;
    L_[i].triangularView<Eigen::Lower>().solveInPlace(li);
    L_[i].triangularView<Eigen::Lower>().transpose().solveInPlace(li);
    li *= -1.0;
  }

  // Recover the inequality duals by solving
  // diag(mus)* dv = (rv + diag(gamma)*A*dz).
  VectorXd& dv = dx->v();
  dv = r.v_;
  // r3_ = A*dz, r3_ is being used as a temp.
  data->gemvA(dx->z(), 1.0, 0.0, &r3_);
  dv += gamma_.asDiagonal() * r3_;
  dv = dv.cwiseQuotient(mus_);

  // Compute dy = b - A*dz.
  VectorXd& dy = dx->y();
  data->gemvA(dx->z(), -1.0, 0.0, &dy);
  data->axpyb(1.0, &dy);

  return true;
}

Eigen::Vector2d RiccatiLinearSolver::PFBGradient(double a, double b) const {
  const double r = sqrt(a * a + b * b);
  const double d = 1.0 / sqrt(2.0);

  Eigen::Vector2d v;
  if (r < zero_tolerance_) {
    v(0) = alpha_ * (1.0 - d);
    v(1) = alpha_ * (1.0 - d);

  } else if ((a > 0) && (b > 0)) {
    v(0) = alpha_ * (1.0 - a / r) + (1.0 - alpha_) * b;
    v(1) = alpha_ * (1.0 - b / r) + (1.0 - alpha_) * a;

  } else {
    v(0) = alpha_ * (1.0 - a / r);
    v(1) = alpha_ * (1.0 - b / r);
  }

  return v;
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake