/*
 * Copyright 2018, LAAS-CNRS
 * Author: Steve Tonneau
 */

#ifndef BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
#define BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H

#include <hpp/bezier-com-traj/local_config.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>

#include <Eigen/Dense>

namespace bezier_com_traj {

/**
 * @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
 * computes a discretization of the curve
 * @param wps original waypoints
 * @param bernstein berstein polynoms for
 * @param numSteps desired number of wayoints
 * @return a vector of waypoint representing the discretization of the curve
 */
BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
    const std::vector<waypoint6_t>& wps, const std::vector<ndcurves::Bern<double> >& bernstein, int numSteps);

/**
 * @brief compute6dControlPointInequalities Given linear and angular control waypoints,
 * compute the inequality matrices A and b, A x <= b that constrain the desired control point x.
 * @param cData data for the current contact phase
 * @param wps waypoints or the linear part of the trajectory
 * @param wpL waypoints or the angular part of the trajectory
 * @param useAngMomentum whether the angular momentum is consider or equal to 0
 * @param fail set to true if problem is found infeasible
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
    const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
    const bool useAngMomentum, bool& fail);

/**
 * @brief compute6dControlPointEqualities Given linear and angular control waypoints,
 * compute the equality matrices D and d, D [x; Beta]' = d that constrain the desired control point x and contact
 * forces Beta.
 * @param cData data for the current contact phase
 * @param wps waypoints or the linear part of the trajectory
 * @param wpL waypoints or the angular part of the trajectory
 * @param useAngMomentum whether the angular momentum is consider or equal to 0
 * @param fail set to true if problem is found infeasible
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointEqualities(
    const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
    const bool useAngMomentum, bool& fail);

/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog
 * @param A Inequality matrix
 * @param b Inequality vector
 * @param H Cost matrix
 * @param g cost Vector
 * @param x initGuess initial guess
 * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x. Unbounded elements should be lesser or equal to solvers::UNBOUNDED_UP;
 * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x Unbounded elements should be higher or lower than solvers::UNBOUNDED_DOWN;
 * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g,
                                        Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog
 * @param A Inequality matrix
 * @param b Inequality vector
 * @param D Equality matrix
 * @param d Equality vector
 * @param H Cost matrix
 * @param g cost Vector
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX D, Cref_vectorX d,
                                        Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);

/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog, with x of fixed dimension 3
 * @param Ab Inequality matrix and vector
 * @param Hg Cost matrix and vector
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Hg,
                                        const VectorX& init,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);

/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b  and D*x = c using quadprog, with x of fixed dimension 3
 * @param Ab Inequality matrix and vector
 * @param Dd Equality matrix and vector
 * @param Hg Cost matrix and vector
 * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x. Unbounded elements should be equal to -std::numeric_limits<double>::infinity();
 * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x Unbounded elements should be equal to  std::numeric_limits<double>::infinity();
 * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Dd,
                                        const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
                                        Cref_vectorX maxBounds, const VectorX& init,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);

template <typename Point>
BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > computeDiscretizedWaypoints(const ProblemData& pData,
                                                                                double T,
                                                                                const T_time& timeArray);

template <typename Point>
BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > computeDiscretizedAccelerationWaypoints(
    const ProblemData& pData, double T, const T_time& timeArray);

}  // end namespace bezier_com_traj

#include "common_solve_methods.inl"

#endif


#include <iostream>
#include <Eigen/IterativeLinearSolvers>

#include <libv/lma/lm/solver/solver.hpp>
#include <libv/lma/lm/solver/verbose.hpp>

using namespace Eigen;

void eigen_pcg(const Eigen::MatrixXd& m, const Eigen::VectorXd& jte)
{

  std::cout << "\n\n EIGEN PCG " << std::endl;
  size_t n = jte.size();
  VectorXd x(n), b(jte);
//   typedef SparseMatrix<double> Mat;
  typedef Eigen::MatrixXd Mat;
//   Mat A(n,n);
  Mat A = m;
//   for(size_t i = 0 ; i < m.cols() ; ++i)
//     for(size_t j = 0 ; j < m.rows() ; ++j)
//       A.coeff(j,i) = m(j,i);

//   for(size_t i = 0 ; i < n ; ++i)
//     A.coeffRef(i,i) = 1.0;
//   A.setIdentity();
  std::cout << A << std::endl;
  x.setZero();
  std::cout <<"\n X = " <<  x.transpose() << std::endl;
  std::cout <<"\n B = " <<  b.transpose() << std::endl;
  // fill A and b
  ConjugateGradient<Mat> cg;
  cg.compute(A);
  x = cg.solve(b);
  std::cout << "#iterations:     " << cg.iterations() << std::endl;
  std::cout << "estimated error: " << cg.error()      << std::endl;
  // update b, and solve again
//   x = cg.solve(b);
  std::cout << "\n X = " << x.transpose() << std::endl;
}

// typedef Eigen::Matrix<double,2,1> Type0;
// typedef Eigen::Matrix<double,3,1> Type1;
// typedef Eigen::Matrix<double,1,1> Type2;
//
// struct F
// {
//   bool operator()(const Type0& , const Type1& , const Type2&, Eigen::Matrix<double,2,1>&) const
//   {
//     return true;
//   }
// };

int main()
{
  /*
  Type0 t0;
  Type1 t1;
  Type2 t2;
  std::cout << " Test pcg " << std::endl;
  lma::Solver<F> solver(1,1);

//   solver.algo.norm_eq.seuil=0.9999;
//   solver.algo.norm_eq.max_iteration=100000typedef typename SelectAlgo<Container,Container::NbClass,AlgoTag>::type Algorithm;
//       Algorithm algo(config);;

  auto i0 = ttt::Indice<Type0*>(0);
  auto i1 = ttt::Indice<Type1*>(0);
  auto i2 = ttt::Indice<Type2*>(0);
  solver.add(F(),&t0,&t1,&t2);//lma::bf::make_vector(i0,i1,i2),F());
//   solver.solve(lma::enable_verbose_output());
//   solver.algo.compute_b(solver.algo.ba_);
//   solver.algo.compute_delta_a(solver.algo.ba_);
  using namespace lma;
  typedef typename SelectAlgo<lma::Solver<F>::Container,lma::Solver<F>::Container::NbClass,ImplicitSchurTag<1>>::type Algorithm;
  Algorithm algo(ImplicitSchurTag<1>(0.9999,100000));
  algo.init(solver.bundle);

  lma::bf::at_key<lma::bf::pair<Type0*,Type0*>>(algo.ba_.h())(i0,0) <<
    1,0,
    0,2;

  lma::bf::at_key<lma::bf::pair<Type2*,Type2*>>(algo.ba_.h())(i2,0) <<
    1;

  lma::bf::at_key<lma::bf::pair<Type0*,Type1*>>(algo.ba_.h())(i0,0) <<
    3.2,0.5,2,
    0,-2.5,-1.5;

  lma::bf::at_key<lma::bf::pair<Type1*,Type1*>>(algo.ba_.h())(i1,0) <<
    3,0,0,
    0,4,0,
    0,0,5;

  lma::bf::at_key<Type0*>(algo.ba_.jte())(i0) << 1,2;
  lma::bf::at_key<Type1*>(algo.ba_.jte())(i1) << 3,4,5;
  lma::bf::at_key<Type2*>(algo.ba_.jte())(i2) << 1;

  std::cout << std::endl;
  std::cout << " A = \n" << lma::to_mat(algo.ba_.h()) << std::endl;
//   std::cout << " B = " << lma::to_vect(solver.algo.schur_.bs_).transpose() << std::endl;
  std::cout << " B = " << lma::to_vect(algo.ba_.jte()).transpose() << std::endl;

  algo.compute_y(algo.ba_);
  algo.compute_b(algo.ba_);
  algo.compute_delta_a(algo.ba_);

  std::cout << " X = " << lma::to_vect(algo.ba_.delta()).transpose() << std::endl;

  eigen_pcg(lma::to_mat(algo.ba_.h()),lma::to_vect(algo.ba_.jte()));*/
}


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

#define BOOST_TEST_MODULE multiexpr_test

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <cstdio>
#include <vector>
#include <chrono>
#include <ctime>

#include <nil/crypto3/algebra/multiexp/multiexp.hpp>
#include <nil/crypto3/algebra/multiexp/policies.hpp>

#include <nil/crypto3/algebra/curves/alt_bn128.hpp>
#include <nil/crypto3/algebra/curves/bls12.hpp>
//#include <nil/crypto3/algebra/curves/bn128.hpp>
// #include <nil/crypto3/algebra/curves/brainpool_r1.hpp>
#include <nil/crypto3/algebra/curves/edwards.hpp>
// #include <nil/crypto3/algebra/curves/frp_v1.hpp>
// #include <nil/crypto3/algebra/curves/gost_A.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>
// #include <nil/crypto3/algebra/curves/p192.hpp>
// #include <nil/crypto3/algebra/curves/p224.hpp>
// #include <nil/crypto3/algebra/curves/p256.hpp>
// #include <nil/crypto3/algebra/curves/p384.hpp>
// #include <nil/crypto3/algebra/curves/p521.hpp>
// #include <nil/crypto3/algebra/curves/secp.hpp>
// #include <nil/crypto3/algebra/curves/sm2p_v1.hpp>
// #include <nil/crypto3/algebra/curves/x962_p.hpp>

#include <nil/crypto3/algebra/curves/params/multiexp/alt_bn128.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
//#include <nil/crypto3/algebra/curves/params/multiexp/bn128.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/brainpool_r1.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/edwards.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/frp_v1.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/gost_A.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt4.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/mnt6.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/p192.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/p224.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/p256.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/p384.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/p521.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/secp.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/sm2p_v1.hpp>
// #include <nil/crypto3/algebra/curves/params/multiexp/x962_p.hpp>

#include <nil/crypto3/algebra/random_element.hpp>

using namespace nil::crypto3::algebra;

template<typename GroupType>
using run_result_t = std::pair<long long, std::vector<typename GroupType::value_type>>;

template<typename T>
using test_instances_t = std::vector<std::vector<T>>;

template<typename GroupType>
test_instances_t<GroupType> generate_group_elements(std::size_t count, std::size_t size) {
    // generating a random group element is expensive,
    // so for now we only generate a single one and repeat it
    test_instances_t<GroupType> result(count);

    for (size_t i = 0; i < count; i++) {

        typename GroupType::value_type x =
            random_element<GroupType>().to_projective();    // djb requires input to be in special form

        for (size_t j = 0; j < size; j++) {
            result[i].push_back(x);
            // result[i].push_back(curve_random_element<GroupType>());
        }
    }

    return result;
}

template<typename FieldType>
test_instances_t<FieldType> generate_scalars(std::size_t count, std::size_t size) {
    // we use SHA512_rng because it is much faster than
    // FieldType::random_element()
    test_instances_t<FieldType> result(count);

    for (size_t i = 0; i < count; i++) {
        for (size_t j = 0; j < size; j++) {
            result[i].push_back(random_element<FieldType>(i * size + j));
        }
    }

    return result;
}

long long get_nsec_time() {
    auto timepoint = std::chrono::high_resolution_clock::now();
    return std::chrono::duration_cast<std::chrono::nanoseconds>(timepoint.time_since_epoch()).count();
}

template<typename GroupType, typename FieldType, typename MultiexpMethod>
run_result_t<GroupType>
    profile_multiexp(test_instances_t<GroupType> group_elements, test_instances_t<FieldType> scalars) {
    long long start_time = get_nsec_time();

    std::vector<typename GroupType::value_type> answers;
    for (size_t i = 0; i < group_elements.size(); i++) {
        answers.push_back(multiexp<MultiexpMethod>(group_elements[i].cbegin(), group_elements[i].cend(),
                                                   scalars[i].cbegin(), scalars[i].cend(), 1));
    }

    long long time_delta = get_nsec_time() - start_time;

    return run_result_t<GroupType>(time_delta, answers);
}

template<typename GroupType, typename FieldType>
void print_performance_csv(size_t expn_start, std::size_t expn_end_fast, std::size_t expn_end_naive,
                           bool compare_answers) {
    for (size_t expn = expn_start; expn <= expn_end_fast; expn++) {
        printf("%ld", expn);
        fflush(stdout);

        test_instances_t<GroupType> group_elements = generate_group_elements<GroupType>(10, 1 << expn);
        test_instances_t<FieldType> scalars = generate_scalars<FieldType>(10, 1 << expn);

        run_result_t<GroupType> result_bos_coster =
            profile_multiexp<GroupType, FieldType, policies::multiexp_method_bos_coster>(group_elements, scalars);
        printf("\t%lld", result_bos_coster.first);
        fflush(stdout);

        run_result_t<GroupType> result_djb =
            profile_multiexp<GroupType, FieldType, policies::multiexp_method_BDLO12>(group_elements, scalars);
        printf("\t%lld", result_djb.first);
        fflush(stdout);

        if (compare_answers && (result_bos_coster.second != result_djb.second)) {
            fprintf(stderr, "Answers NOT MATCHING (bos coster != djb)\n");
        }

        if (expn <= expn_end_naive) {
            run_result_t<GroupType> result_naive =
                profile_multiexp<GroupType, FieldType, policies::multiexp_method_naive_plain>(group_elements, scalars);
            printf("\t%lld", result_naive.first);
            fflush(stdout);

            if (compare_answers && (result_bos_coster.second != result_naive.second)) {
                fprintf(stderr, "Answers NOT MATCHING (bos coster != naive)\n");
            }
        }

        printf("\n");
    }
}

BOOST_AUTO_TEST_SUITE(multiexp_test_suite)

BOOST_AUTO_TEST_CASE(multiexp_test_case) {

    std::cout << "Testing BLS12-381 G1" << std::endl;
    print_performance_csv<curves::bls12<381>::g1_type<>, curves::bls12<381>::scalar_field_type>(2, 20, 14, true);

    std::cout << "Testing BLS12-381 G2" << std::endl;
    print_performance_csv<curves::bls12<381>::g2_type<>, curves::bls12<381>::scalar_field_type>(2, 20, 14, true);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2010-2021 Internet Systems Consortium, Inc. ("ISC")
//
// 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 <config.h>

#include <perfdhcp/random_number_generator.h>

#include <gtest/gtest.h>
#include <boost/shared_ptr.hpp>

#include <iostream>

using namespace isc;
using namespace isc::perfdhcp;
using namespace std;

/// \brief Test Fixture Class for uniform random number generator
///
/// The hard part for this test is how to test that the number is random?
/// and how to test that the number is uniformly distributed?
/// Or maybe we can trust the boost implementation
class UniformRandomIntegerGeneratorTest : public ::testing::Test {
public:
    UniformRandomIntegerGeneratorTest():
        gen_(min_, max_)
    {
    }
    virtual ~UniformRandomIntegerGeneratorTest(){}

    int gen() { return (gen_()); }
    int max() const { return (max_); }
    int min() const { return (min_); }

private:
    UniformRandomIntegerGenerator gen_;

    const static int min_ = 1;
    const static int max_ = 10;
};

// Some validation tests will incur performance penalty, so the tests are
// made only in "debug" version with assert(). But if NDEBUG is defined
// the tests will be failed since assert() is non-op in non-debug version.
// The "#ifndef NDEBUG" is added to make the tests be performed only in
// non-debug environment.
// Note: the death test is not supported by all platforms.  We need to
// compile tests using it selectively.
#if !defined(NDEBUG)
// Test of the constructor
TEST_F(UniformRandomIntegerGeneratorTest, Constructor) {
    // The range must be min<=max
    ASSERT_THROW(UniformRandomIntegerGenerator(3, 2), InvalidLimits);
}
#endif

// Test of the generated integers are in the range [min, max]
TEST_F(UniformRandomIntegerGeneratorTest, IntegerRange) {
    vector<int> numbers;

    // Generate a lot of random integers
    for (int i = 0; i < max()*10; ++i) {
        numbers.push_back(gen());
    }

    // Remove the duplicated values
    sort(numbers.begin(), numbers.end());
    vector<int>::iterator it = unique(numbers.begin(), numbers.end());

    // make sure the numbers are in range [min, max]
    ASSERT_EQ(it - numbers.begin(), max() - min() + 1);
}

/// \brief Test Fixture Class for weighted random number generator
class WeightedRandomIntegerGeneratorTest : public ::testing::Test {
public:
    WeightedRandomIntegerGeneratorTest()
    { }

    virtual ~WeightedRandomIntegerGeneratorTest()
    { }
};

// Test of the weighted random number generator constructor
TEST_F(WeightedRandomIntegerGeneratorTest, Constructor) {
    vector<double> probabilities;

    // If no probabilities is provided, the smallest integer will always
    // be generated
    WeightedRandomIntegerGenerator gen(probabilities, 123);
    for (int i = 0; i < 100; ++i) {
        ASSERT_EQ(gen(), 123);
    }

/// Some validation tests will incur performance penalty, so the tests are
/// made only in "debug" version with assert(). But if NDEBUG is defined
/// the tests will be failed since assert() is non-op in non-debug version.
/// The "#ifndef NDEBUG" is added to make the tests be performed only in
/// non-debug environment.
#if !defined(NDEBUG)
    //The probability must be >= 0
    probabilities.push_back(-0.1);
    probabilities.push_back(1.1);
    ASSERT_THROW(WeightedRandomIntegerGenerator gen2(probabilities),
                 InvalidProbValue);

    //The probability must be <= 1.0
    probabilities.clear();
    probabilities.push_back(0.1);
    probabilities.push_back(1.1);
    ASSERT_THROW(WeightedRandomIntegerGenerator gen3(probabilities),
                 InvalidProbValue);

    //The sum must be equal to 1.0
    probabilities.clear();
    probabilities.push_back(0.2);
    probabilities.push_back(0.9);
    ASSERT_THROW(WeightedRandomIntegerGenerator gen4(probabilities), SumNotOne);

    //The sum must be equal to 1.0
    probabilities.clear();
    probabilities.push_back(0.3);
    probabilities.push_back(0.2);
    probabilities.push_back(0.1);
    ASSERT_THROW(WeightedRandomIntegerGenerator gen5(probabilities), SumNotOne);
#endif
}

// Test the randomization of the generator
TEST_F(WeightedRandomIntegerGeneratorTest, WeightedRandomization) {
    const int repeats = 100000;
    // We repeat the simulation for N=repeats times
    // for each probability p, its average is mu = N*p
    // variance sigma^2 = N * p * (1-p)
    // sigma = sqrt(N*2/9)
    // we should make sure that mu - 4sigma < count < mu + 4sigma
    // which means for 99.99366% of the time this should be true
    {
        double p = 0.5;
        vector<double> probabilities;
        probabilities.push_back(p);
        probabilities.push_back(p);

        // Uniformly generated integers
        WeightedRandomIntegerGenerator gen(probabilities);
        int c1 = 0;
        int c2 = 0;
        for (int i = 0; i < repeats; ++i){
            int n = gen();
            if (n == 0) {
                ++c1;
            } else if (n == 1) {
                ++c2;
            }
        }
        double mu = repeats * p;
        double sigma = sqrt(repeats * p * (1 - p));
        ASSERT_TRUE(fabs(c1 - mu) < 4*sigma);
        ASSERT_TRUE(fabs(c2 - mu) < 4*sigma);
    }

    {
        vector<double> probabilities;
        int c1 = 0;
        int c2 = 0;
        double p1 = 0.2;
        double p2 = 0.8;
        probabilities.push_back(p1);
        probabilities.push_back(p2);
        WeightedRandomIntegerGenerator gen(probabilities);
        for (int i = 0; i < repeats; ++i) {
            int n = gen();
            if (n == 0) {
                ++c1;
            } else if (n == 1) {
                ++c2;
            }
        }
        double mu1 = repeats * p1;
        double mu2 = repeats * p2;
        double sigma1 = sqrt(repeats * p1 * (1 - p1));
        double sigma2 = sqrt(repeats * p2 * (1 - p2));
        ASSERT_TRUE(fabs(c1 - mu1) < 4*sigma1);
        ASSERT_TRUE(fabs(c2 - mu2) < 4*sigma2);
    }

    {
        vector<double> probabilities;
        int c1 = 0;
        int c2 = 0;
        double p1 = 0.8;
        double p2 = 0.2;
        probabilities.push_back(p1);
        probabilities.push_back(p2);
        WeightedRandomIntegerGenerator gen(probabilities);
        for (int i = 0; i < repeats; ++i) {
            int n = gen();
            if (n == 0) {
                ++c1;
            } else if (n == 1) {
                ++c2;
            }
        }
        double mu1 = repeats * p1;
        double mu2 = repeats * p2;
        double sigma1 = sqrt(repeats * p1 * (1 - p1));
        double sigma2 = sqrt(repeats * p2 * (1 - p2));
        ASSERT_TRUE(fabs(c1 - mu1) < 4*sigma1);
        ASSERT_TRUE(fabs(c2 - mu2) < 4*sigma2);
    }

    {
        vector<double> probabilities;
        int c1 = 0;
        int c2 = 0;
        int c3 = 0;
        double p1 = 0.5;
        double p2 = 0.25;
        double p3 = 0.25;
        probabilities.push_back(p1);
        probabilities.push_back(p2);
        probabilities.push_back(p3);
        WeightedRandomIntegerGenerator gen(probabilities);
        for (int i = 0; i < repeats; ++i){
            int n = gen();
            if (n == 0) {
                ++c1;
            } else if (n == 1) {
                ++c2;
            } else if (n == 2) {
                ++c3;
            }
        }
        double mu1 = repeats * p1;
        double mu2 = repeats * p2;
        double mu3 = repeats * p3;
        double sigma1 = sqrt(repeats * p1 * (1 - p1));
        double sigma2 = sqrt(repeats * p2 * (1 - p2));
        double sigma3 = sqrt(repeats * p3 * (1 - p3));
        ASSERT_TRUE(fabs(c1 - mu1) < 4*sigma1);
        ASSERT_TRUE(fabs(c2 - mu2) < 4*sigma2);
        ASSERT_TRUE(fabs(c3 - mu3) < 4*sigma3);
    }
}

// Test the reset function of generator
TEST_F(WeightedRandomIntegerGeneratorTest, ResetProbabilities) {
    const int repeats = 100000;
    vector<double> probabilities;
    int c1 = 0;
    int c2 = 0;
    double p1 = 0.8;
    double p2 = 0.2;
    probabilities.push_back(p1);
    probabilities.push_back(p2);
    WeightedRandomIntegerGenerator gen(probabilities);
    for (int i = 0; i < repeats; ++i) {
        int n = gen();
        if (n == 0) {
            ++c1;
        } else if (n == 1) {
            ++c2;
        }
    }
    double mu1 = repeats * p1;
    double mu2 = repeats * p2;
    double sigma1 = sqrt(repeats * p1 * (1 - p1));
    double sigma2 = sqrt(repeats * p2 * (1 - p2));
    ASSERT_TRUE(fabs(c1 - mu1) < 4*sigma1);
    ASSERT_TRUE(fabs(c2 - mu2) < 4*sigma2);

    // Reset the probabilities
    probabilities.clear();
    c1 = c2 = 0;
    p1 = 0.2;
    p2 = 0.8;
    probabilities.push_back(p1);
    probabilities.push_back(p2);
    gen.reset(probabilities);
    for (int i = 0; i < repeats; ++i) {
        int n = gen();
        if (n == 0) {
            ++c1;
        } else if (n == 1) {
            ++c2;
        }
    }
    mu1 = repeats * p1;
    mu2 = repeats * p2;
    sigma1 = sqrt(repeats * p1 * (1 - p1));
    sigma2 = sqrt(repeats * p2 * (1 - p2));
    ASSERT_TRUE(fabs(c1 - mu1) < 4*sigma1);
    ASSERT_TRUE(fabs(c2 - mu2) < 4*sigma2);
}


#include "potts.h"
#include "ising.h"
#include "util/observable.hpp"
#include "util/squarelattice.hpp"
#include <iostream>
#include <boost/random.hpp>
#include <ctime>
#include <mpi.h>
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/timer/timer.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

int main(int argc, char **argv)
{
  typedef util::SquareLattice Lattice;
  typedef boost::variate_generator<boost::mt19937, boost::uniform_real<> > RNG01;

  RNG01 rnd(boost::mt19937(static_cast<uint32_t>(std::time(0))), boost::uniform_real<>(0.0, 1.0));

  using namespace boost;
  using namespace boost::program_options;
  options_description opt("options");
  opt.add_options()
    ("help", "show this message")
    ("q,q", value<int>()->default_value(2), "number of state of a spin")
    ("L,L", value<int>()->default_value(10), "length of lattice")
    ("beta-min", value<double>()->default_value(0.1), "minimum beta")
    ("beta-max", value<double>()->default_value(1.0), "maximum beta")
    ("beta-num", value<int>()->default_value(10), "number of beta")
    ("thermalization", value<int>()->default_value(8), "number of exchange to thermalize")
    ("mcs", value<int>()->default_value(64), "number of exchange to measure")
    ;

  variables_map vm;
  store(parse_command_line(argc, argv, opt), vm);
  notify(vm);

  if( vm.count("help") ){
    std::cout << opt << std::endl;
    return 0;
  }

  const int q = vm["q"].as<int>();
  const int L = vm["L"].as<int>();
  const double bmin = vm["beta-min"].as<double>();
  const double bmax = vm["beta-max"].as<double>();
  const int nbeta = vm["beta-num"].as<int>();
  const int therm = vm["thermalization"].as<int>();
  const int MCS = vm["mcs"].as<int>();

  const double dbeta = (bmax-bmin)/(nbeta-1);

  potts::Potts<Lattice, RNG01> model(q, L);

  for(int ibeta=0; ibeta<nbeta; ++ibeta){
    const double beta = bmin + dbeta*ibeta;
    util::Observable obs_time;
    util::Observable obs_speed;

    for(int mcs=0; mcs < therm+MCS; ++mcs){
      boost::timer::cpu_timer tm;
      model.SW_update(beta, rnd);
      const double sec = tm.elapsed().wall * 1.0e-9;
      const double speed = 1.0/sec;
      obs_time << sec;
      obs_speed << speed;
    }
    std::cout << beta
       << " " << obs_time.mean() << " " << obs_time.error()
       << " " << obs_speed.mean() << " " << obs_speed.error()
       << std::endl;
  }

  return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DInterpolator.h>
#include <mrpt/system/datetime.h>

#include <Eigen/Dense>

template class mrpt::CTraitsTest<mrpt::poses::CPose3DInterpolator>;

TEST(CPose3DInterpolator, interp)
{
	using namespace mrpt::poses;
	using namespace mrpt;  // for 0.0_deg
	using mrpt::DEG2RAD;
	using mrpt::math::CMatrixDouble44;
	using mrpt::math::TPose3D;

	auto t0 = mrpt::Clock::now();
	mrpt::Clock::duration dt(std::chrono::milliseconds(100));

	CPose3DInterpolator pose_path;

	pose_path.insert(t0, TPose3D(1., 2., 3., 30.0_deg, .0_deg, .0_deg));
	pose_path.insert(
		t0 + 2 * dt,
		TPose3D(
			1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0), .0_deg, .0_deg));

	TPose3D interp;
	bool valid;
	pose_path.interpolate(t0 + dt, interp, valid);

	EXPECT_TRUE(valid);
	const TPose3D interp_good(
		1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), .0_deg, .0_deg);
	EXPECT_NEAR(
		.0,
		(CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
		 CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
			.array()
			.abs()
			.sum(),
		2e-4);
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  int array[12];
for(int i = 0; i < 12; ++i) array[i] = i;
cout << Map<MatrixXi, 0, OuterStride<> >(array, 3, 3, OuterStride<>(4)) << endl;

  return 0;
}


#include <iostream>
#include <cassert>
#include <time.h>
#include "timer.h"
#include "Encoding.h"
#include "PolyTree.h"
#include "EncodingEnigmaBN.h"

#include <boost/program_options.hpp>
using namespace std;
using namespace boost::program_options;

void microbenchmarks(variables_map& config) {

#ifdef _DEBUG
	printf("\nRunning in DEBUG mode -- performance measurements should not be trusted!\n\n");
#endif

	string option = config["micro"].as<string>();
	int num_trials = config["trials"].as<int>();
	printf("About to run %d tests...\n", num_trials);

	Encoding* encoding = NewEncoding(config["encoding"].as<string>());
	if (encoding == NULL) { return; }
	Field* field = encoding->getSrcField();
  if (option.compare("mont-test") == 0) {
    assert(config["encoding"].as<string>().compare("enigma-bn") == 0);
    field = ((EncodingEnigmaBN*)encoding)->getMontField();

    ((EncodingEnigmaBN*)encoding)->testToFromMont(field, num_trials);
  }

	FieldEltArray* field1 = field->newEltArray(num_trials, true);
	FieldEltArray* field2 = field->newEltArray(num_trials, true);
	FieldEltArray* field32 = field->newEltArray(num_trials, true);

	EncodedEltArray* lencodings = encoding->new_elt_array(L, num_trials, true);
	EncodedEltArray* rencodings = encoding->new_elt_array(R, num_trials, true);
	EncodedEltArray* lencodings2 = encoding->new_elt_array(L, num_trials, true);
	EncodedEltArray* rencodings2 = encoding->new_elt_array(R, num_trials, true);
	EncodedProduct** prods = new EncodedProduct*[num_trials];

	FieldElt *fieldg1 = field->newElt(), *fieldg0 = field->newElt();
	// I'll assume 5^i are unique values, for i in [0,num_trials)
	field->set(fieldg1, 5);
	field->set(fieldg0, 1);
	for (int i = 0; i < num_trials; i++) {
		field->assignRandomElt(field1->elt(i));
		field->assignRandomElt(field2->elt(i));
		field->assignRandomElt(field32->elt(i));

    field->truncate(field1->elt(i), 254);
    field->truncate(field2->elt(i), 254);
		field->truncate(field32->elt(i), 32);

		prods[i] = encoding->new_prod();
	}

	Timer* total = timers.newTimer("Total", NULL);
	total->start();

	/////////////// Field ops ///////////////////////////
  if (option.compare("all") == 0 || option.compare("field") == 0 || option.compare("mont-test") == 0) {
		TIMEREP(field->add(field1->elt(i), field2->elt(i), field1->elt(i)), num_trials, "FieldAdd", "Total");
		TIMEREP(field->sub(field1->elt(i), field2->elt(i), field1->elt(i)), num_trials, "FieldSub", "Total");
		TIMEREP(field->mul(field1->elt(i), field2->elt(i), field1->elt(i)), num_trials, "FieldMul", "Total");
		TIMEREP(field->mul(field1->elt(i), field2->elt(i), field1->elt(i)), num_trials, "FieldDiv", "Total");

		printf("Finished field ops\n");  fflush(stdout);
	}

	/////////////// Encoding ops ///////////////////////////
	if (option.compare("all") == 0 || option.compare("encoding") == 0) {
		TIMEREP(encoding->encode(L, field1->elt(i), lencodings->elt(i)), num_trials, "EncodeSlowL", "Total");
		TIMEREP(encoding->encode(R, field1->elt(i), rencodings->elt(i)), num_trials, "EncodeSlowR", "Total");

		TIME(encoding->prepareForManyEncodings(num_trials, config["mem"].as<int>(), false), "PrepForEnc", "Total");
		TIMEREP(encoding->encode(L, field1->elt(i), lencodings->elt(i)), num_trials, "EncodeFastL", "Total");
		TIMEREP(encoding->encode(R, field1->elt(i), rencodings->elt(i)), num_trials, "EncodeFastR", "Total");
		TIMEREP(encoding->encode(L, field2->elt(i), lencodings2->elt(i)), num_trials, "ignoreL", "Total");
		TIMEREP(encoding->encode(R, field2->elt(i), rencodings2->elt(i)), num_trials, "ignoreR", "Total");
		encoding->doneWithManyEncodings();

		printf("Finished encoding test values\n");  fflush(stdout);

		TIMEREP(encoding->add(L, lencodings->elt(i), lencodings2->elt(i), lencodings2->elt(i)), num_trials, "EncAddL", "Total");
		TIMEREP(encoding->add(R, rencodings->elt(i), rencodings2->elt(i), rencodings2->elt(i)), num_trials, "EncAddR", "Total");

		ip_handle_t hL, hR;
    int field_elt_max_bitsize = field->eltSize()*sizeof(digit_t) * 8;
    TIME(hL = encoding->prepareForInnerProduct(lencodings, num_trials, 1, field_elt_max_bitsize, config["mem"].as<int>(), false), "PrepInnerL", "Total");
		TIME(encoding->innerProduct(hL, field1, num_trials, lencodings2->elt(0)), "InnerProdL", "Total");
		encoding->doneWithInnerProduct(hL);

    TIME(hR = encoding->prepareForInnerProduct(rencodings, num_trials, 1, field_elt_max_bitsize, config["mem"].as<int>(), false), "PrepInnerR", "Total");
		TIME(encoding->innerProduct(hR, field1, num_trials, rencodings2->elt(0)), "InnerProdR", "Total");
		encoding->doneWithInnerProduct(hR);

		TIMEREP(encoding->mul(lencodings->elt(i), rencodings->elt(i), prods[i]), num_trials, "Pairing", "Total");

		printf("Finished large encoding tests\n");  fflush(stdout);

		// Measure the time for smaller field elements
		TIME(encoding->prepareForManyEncodings(num_trials, config["mem"].as<int>(), false), "PrepForEnc", "Total");
		TIMEREP(encoding->encode(L, field32->elt(i), lencodings2->elt(i)), num_trials, "EncodeFast32L", "Total");
		TIMEREP(encoding->encode(R, field32->elt(i), rencodings2->elt(i)), num_trials, "EncodeFast32R", "Total");
		encoding->doneWithManyEncodings();

		TIME(hL = encoding->prepareForInnerProduct(lencodings, num_trials, 1, 32, config["mem"].as<int>(), false), "PrepInnerL", "Total");
		TIME(encoding->innerProduct(hL, field32, num_trials, lencodings2->elt(0)), "InnerProd32L", "Total");
		encoding->doneWithInnerProduct(hL);

		TIME(hR = encoding->prepareForInnerProduct(rencodings, num_trials, 1, 32, config["mem"].as<int>(), false), "PrepInnerR", "Total");
		TIME(encoding->innerProduct(hR, field32, num_trials, rencodings2->elt(0)), "InnerProd32R", "Total");
		encoding->doneWithInnerProduct(hR);

		printf("Finished 32-bit encoding tests\n");  fflush(stdout);
	}

	/////////////// Poly Ops ///////////////////////////
  if (option.compare("all") == 0 || option.compare("poly-all") == 0 || option.compare("poly-fast") == 0 || option.compare("poly-slow") == 0 || option.compare("mont-test") == 0) {
		Timer* polyPreComp = timers.newTimer("PolyPreComp", "Total");
		polyPreComp->start();
		PolyTree* tree = new PolyTree(field, field1, num_trials);
		FieldEltArray* denominators = Poly::genLagrangeDenominators(field, *tree->polys[tree->height-1][0], tree, field1, num_trials);
		polyPreComp->stop();

		TIME(Poly::interpolate(field, field2, num_trials, tree, denominators), "PolyInterp", "Total");
		TIME(Poly::interpolateGeometric(field, fieldg0, fieldg1, field2, num_trials), "PolyInterpGeom", "Total");

		// Compare slow vs. optimized polynomial multiplication
		Poly p1(field), p2(field), r(field);
		Poly::polyRand(field, p1, num_trials);
		Poly::polyRand(field, p2, num_trials);
		if (option.compare("poly-all") == 0 || option.compare("poly-fast") == 0) {
			TIME(Poly::mul(p1, p2, p1), "PolyMul", "Total");
		}
		if (option.compare("poly-all") == 0 || option.compare("poly-slow") == 0) {
			TIME(Poly::mulSlow(p1, p2, p1), "PolyMulSlow", "Total");
		}
	}

	total->stop();

	if (config.count("raw")) {
		timers.printRaw();
	} else {
		timers.printStats();
	}
	printf("\n");

	delete field1, field2, field32;
	delete lencodings,  rencodings;
	delete lencodings2, rencodings2;
	delete [] prods;

	delete encoding;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <gtest/gtest.h>
#include <mrpt/math/CMatrixFixed.h>

#include <Eigen/Dense>

TEST(CMatrixFixed, CtorUninit)
{
	mrpt::math::CMatrixFixed<double, 2, 2> M(mrpt::math::UNINITIALIZED_MATRIX);
	// do nothing, just test that the ctor above compiles
	(void)M(0, 0);
}

TEST(CMatrixFixed, CtorAllZeros)
{
	mrpt::math::CMatrixFixed<double, 2, 2> M;
	for (int i = 0; i < 2; i++)
		for (int j = 0; j < 2; j++)
			EXPECT_EQ(M(i, j), .0);
}

TEST(CMatrixFixed, Identity)
{
	mrpt::math::CMatrixFixed<double, 3, 3> M;
	M.setIdentity();
	for (int i = 0; i < 3; i++)
	{
		EXPECT_EQ(M(i, i), 1.0);
		// Also test access via data():
		EXPECT_EQ(M(i, i), M.data()[i + i * 3]);
	}

	// Check that access via data() is what we expect:
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++)
			EXPECT_EQ(&M(r, c), &M.data()[c + r * 3]);
}

TEST(CMatrixFixed, asString)
{
	mrpt::math::CMatrixFixed<double, 2, 2> M;
	M.setIdentity();
	EXPECT_EQ(std::string("1 0\n0 1"), M.asString());
}

TEST(CMatrixFixed, GetSetEigen)
{
	{
		mrpt::math::CMatrixFixed<double, 3, 3> M;
		auto em = M.asEigen();
		em.setIdentity();
		for (int i = 0; i < 3; i++)
			EXPECT_EQ(M(i, i), 1.0);
	}
	{
		mrpt::math::CMatrixFixed<double, 3, 3> M;
		auto em = M.asEigen();
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
			{
				const auto n = ((i + 1) * 3) + (j * 1001);
				em(i, j) = n;
				EXPECT_NEAR(M(i, j), em(i, j), 1e-9)
					<< "(i,j)=(" << i << "," << j << ")\n";
			}
	}
}


/*
 * This file is part of the Interpolated Polyline (https://github.com/fzi-forschungszentrum-informatik/P3IV),
 * copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
 */

#pragma once
#include <iostream>
#include <Eigen/Core>
#include <glog/logging.h>

namespace util_probability {

template <typename T, int Dim>
struct NormalDistribution {

    using Mean = Eigen::Matrix<T, Dim, 1>;
    using Covariance = Eigen::Matrix<T, Dim, Dim>;

    NormalDistribution() : _mean{Mean::Zero()}, _covariance{Covariance::Zero()} {
    }

    NormalDistribution(const Eigen::Ref<const Mean>& mean_) : _mean{mean_}, _covariance{Covariance::Zero()} {
    }

    NormalDistribution(const Eigen::Ref<const Mean>& mean_, const Eigen::Ref<const Covariance>& covariance_)
            : _mean{mean_}, _covariance{covariance_} {
        LOG_ASSERT(_covariance.rows() == _covariance.cols());
        LOG_ASSERT(_mean.size() == _covariance.rows());
    }

    NormalDistribution(const Eigen::Ref<const Mean>& mean_, Eigen::Matrix<T, Dim, 1>& variance_)
            : _mean{mean_}, _covariance{Covariance::Zero()} {

        for (size_t i = 0; i < variance_.size(); i++) {
            _covariance(i, i) = variance_(i);
        }
    }

    size_t dimension() const {
        return static_cast<size_t>(_mean.size());
    }

    Mean mean() const {
        return _mean;
    }

    T mean(size_t r, size_t c) const {
        return _mean(r, c);
    }

    Covariance covariance() const {
        return _covariance;
    }

    T covariance(size_t r, size_t c) const {
        return _covariance(r, c);
    }

    virtual Eigen::Matrix<T, Dim, 1> variance() const {
        Eigen::Matrix<T, Dim, 1> variance;
        for (size_t i = 0; i < _covariance.rows(); i++) {
            variance(i) = _covariance(i, i);
        }
        return variance;
    }

protected:
    Mean _mean;
    Covariance _covariance;
};

} // namespace util_probability


// Servo command generator
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <ros/node_handle.h>

#include <servomodel/servocommandgenerator.h>

#include <math.h>
#include <Eigen/Core>

using namespace Eigen;

const int ServoCommandGenerator::DefaultTicksPerRev = 4096;

ServoCommandGenerator::ServoCommandGenerator()
 : m_coeff(4)
 , m_ticksPerRev(DefaultTicksPerRev)
 , m_pValue(2)
 , m_latency(0)
 , m_voltage(15.0)
{
	m_coeff << 0.17, 0.553952330165401, -0.0023, 0.0250731405677264;
}

inline double sgn(double x)
{
	if(x > 0.0) return 1.0;
	else if(x < 0.0) return -1.0;
	else return 0.0;
}

VectorXd ServoCommandGenerator::commandPartsFor(double pos, double vel, double acc, double outsideTorque) const
{
	VectorXd ret(4);
	// Motor model
	double stribeckFactor = exp(-fabs(vel / 0.1));
	ret <<
		outsideTorque,
		vel,                           // viscous friction / back-EMF
		sgn(vel) * (1-stribeckFactor), // stribeck I
		sgn(vel) * stribeckFactor      // stribeck II
	;

	return ret;
}

double ServoCommandGenerator::currentFactor() const
{
	return (15.0 / m_voltage) / m_pValue * 2.0;
}

double ServoCommandGenerator::servoCommandFor(double pos, double vel, double acc, double outside) const
{
	return pos + currentFactor() * (
		m_coeff.dot(commandPartsFor(pos, vel, acc, outside))
	);
}

double ServoCommandGenerator::servoTorqueFromCommand(double pos_cmd, double pos_cur, double vel) const
{
	Eigen::VectorXd frictionParts = commandPartsFor(0.0, vel, 0.0, 0.0);

	double d = (pos_cmd - pos_cur) / currentFactor();

	double torquePart = d - m_coeff.dot(frictionParts);

	return torquePart / m_coeff(0);
}

void ServoCommandGenerator::setCoefficients(const VectorXd& coeff)
{
	m_coeff = coeff;
	update();
}

void ServoCommandGenerator::setKM(double val)
{
	m_coeff(0) = val;
	update();
}

void ServoCommandGenerator::setViscousFriction(double val)
{
	m_coeff(1) = val;
	update();
}

void ServoCommandGenerator::setStribeckOne(double val)
{
	m_coeff(2) = val;
	update();
}

void ServoCommandGenerator::setStribeckTwo(double val)
{
	m_coeff(3) = val;
	update();
}

void ServoCommandGenerator::setTicksPerRev(double val)
{
	m_ticksPerRev = val;
	update();
}

void ServoCommandGenerator::setMinTickValue(int val)
{
	m_minTickValue = val;
	update();
}

void ServoCommandGenerator::setMaxTickValue(int val)
{
	m_maxTickValue = val;
	update();
}

void ServoCommandGenerator::setPValue(int p)
{
	m_pValue = p;
	update();
}

void ServoCommandGenerator::setLatency(double val)
{
	m_latency = val;
	update();
}

void ServoCommandGenerator::setVoltage(double volt)
{
	m_voltage = volt;
	update();
}

void ServoCommandGenerator::update()
{
}

std::string ServoCommandGenerator::serializeCoefficients()
{
	std::stringstream ss;

	for(int i = 0; i < m_coeff.rows(); ++i)
		ss << m_coeff(i) << " ";

	return ss.str();
}

bool ServoCommandGenerator::deserializeCoefficients(const std::string& coeff)
{
	std::stringstream ss;
	ss.str(coeff);

	Eigen::VectorXd newCoeff(m_coeff.rows());

	for(int i = 0; i < m_coeff.rows(); ++i)
	{
		ss >> newCoeff(i);

		if(ss.fail())
			return false;
	}

	setCoefficients(newCoeff);
	return true;
}


/*

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_ALGORITHMS_POISSON_SOLVER_HPP
#define PIC_ALGORITHMS_POISSON_SOLVER_HPP

#include "../base.hpp"

#include "../image.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Sparse"
    #include "../externals/Eigen/src/SparseCore/SparseMatrix.h"
#else
    #include <Eigen/Sparse>
    #include <Eigen/src/SparseCore/SparseMatrix.h>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief computePoissonSolver
 * @param f
 * @param ret
 * @return
 */
PIC_INLINE Image *computePoissonSolver(Image *f, Image *ret = NULL)
{
    if(f == NULL) {
        return NULL;
    }

    //allocate the output
    if(ret == NULL) {
        ret = f->allocateSimilarOne();
    }

    int width = f->width;
    int height = f->height;
    int tot = height * width;

    #ifdef PIC_DEBUG
        printf("Init matrix...");
    #endif

    std::vector< Eigen::Triplet< double > > tL;

    for(int i = 0; i < height; i++) {
        int tmpI = i * width;

        for(int j = 0; j < width; j++) {
            int indI = tmpI + j;

            tL.push_back(Eigen::Triplet< double > (indI, indI, 4.0f));

            if(((indI + 1) < tot) &&
               ((indI % width) != (width - 1))) {

                tL.push_back(Eigen::Triplet< double > (indI, indI + 1, -1.0f));
                tL.push_back(Eigen::Triplet< double > (indI + 1, indI, -1.0f));
            }
        }
    }

    for(int i = 0; i < (tot - width); i++) {
        tL.push_back(Eigen::Triplet< double > (i + width, i         , -1.0f));
        tL.push_back(Eigen::Triplet< double > (i        , i +  width, -1.0f));
    }

    #ifdef PIC_DEBUG
        printf("Ok\n");
    #endif

    //solve the linear system for each color channel
    Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);
    A.setFromTriplets(tL.begin(), tL.end());
    Eigen::SimplicialCholesky<Eigen::SparseMatrix<double> > solver(A);

    for(int k = 0; k < f->channels; k++) {

        Eigen::VectorXd b, x;
        b = Eigen::VectorXd::Zero(tot);

        //copy values from f to b
        for(int i = 0; i < height; i++) {
            int tmpI = i * width;
            for(int j = 0; j < width; j++) {
                int indI = (tmpI + j);
                b[indI] = - f->data[indI * f->channels + k];
            }
        }

        x = solver.solve(b);

        if(solver.info() != Eigen::Success) {
            #ifdef PIC_DEBUG
                printf("SOLVER FAILED!\n");
            #endif

            return ret;
        }

        #ifdef PIC_DEBUG
            printf("SOLVER SUCCESS!\n");
        #endif

        for(int i = 0; i < height; i++) {
            int tmpI = i * width;

            for(int j = 0; j < width; j++) {
                (*ret)(j, i)[k] = float(x(tmpI + j));
            }
        }
    }

    return ret;
}

#endif

/**
 * @brief computePoissonSolverIterative
 * @param img
 * @param laplacian
 * @param coords
 * @param maxSteps
 * @return
 */
PIC_INLINE Image *computePoissonSolverIterative(Image *img, Image *laplacian,
                              std::vector<int> coords,
                              int maxSteps = 100)
{
    #ifdef PIC_DEBUG
        printf("Iterative Poisson solver... ");
    #endif

    if(maxSteps < 1) {
        maxSteps = 100;
    }

    Image *tmpImg = img->clone();
    Image *tmpSwap = NULL;

    int c, x, y;

    for(int i = 0; i < maxSteps; i++) {
        for(unsigned int j = 0; j < coords.size(); j++) {
            int coord = coords[j];
            img->reverseAddress(coord, x, y);

            float workValue = -laplacian->data[coord];

            c = img->getAddress(x + 1, y);
            workValue += img->data[c];

            c = img->getAddress(x - 1, y);
            workValue += img->data[c];

            c = img->getAddress(x, y + 1);
            workValue += img->data[c];

            c = img->getAddress(x, y - 1);
            workValue += img->data[c];

            tmpImg->data[coord] = workValue / 4.0f;
        }

        tmpSwap = img;
        img     = tmpImg;
        tmpImg  = tmpSwap;
    }

    #ifdef PIC_DEBUG
        printf("done.\n");
    #endif

    return img;
}

} // end namespace pic

#endif /* PIC_ALGORITHMS_POISSON_SOLVER_HPP */


//
// Created by tim on 22.02.21.
//
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <iostream>
#include "graphSlamSaveStructure.h"

void createExampleVertex(graphSlamSaveStructure &graphSaved) {
    Eigen::Vector3f positionVertex0(0, 0, 0);
    Eigen::AngleAxisf rotation_vector0(0.0f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex0(rotation_vector0);
    float covariance0 = 0.0;
    Eigen::Vector3f covarianceVector(1, 1, 0);
    graphSaved.addVertex(0, positionVertex0, rotationVertex0, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex1(1, 1, 0);
    Eigen::AngleAxisf rotation_vector1(-0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex1(rotation_vector1.toRotationMatrix());
    graphSaved.addVertex(1, positionVertex1, rotationVertex1, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex2(1, 1, 0);
    Eigen::AngleAxisf rotation_vector2(-0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex2(rotation_vector2.toRotationMatrix());
    graphSaved.addVertex(2, positionVertex2, rotationVertex2, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex3(1, 1, 0);
    Eigen::AngleAxisf rotation_vector3(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex3(rotation_vector3.toRotationMatrix());
    graphSaved.addVertex(3, positionVertex3, rotationVertex3, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex4(2, 2, 0);
    graphSaved.addVertex(4, positionVertex4, rotationVertex0, covarianceVector * covariance0, covariance0);

}

void createExampleEdge(graphSlamSaveStructure &graphSaved) {

    Eigen::Vector3f eyeVector(1, 1, 0);
    Eigen::Vector3f positionDifference0(8, 10, 0);
    Eigen::AngleAxisf rotation_vector0(-0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference0(rotation_vector0.toRotationMatrix());
    float covariancePosition0 = 0.1;
    float covarianceQuaternion0 = 0.1;
    graphSaved.addEdge(0, 1, positionDifference0, rotationDifference0, eyeVector * covariancePosition0,
                       covarianceQuaternion0);

    Eigen::Vector3f positionDifference1(4, 9, 0);
    Eigen::AngleAxisf rotation_vector1(30.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference1(rotation_vector1.toRotationMatrix());
    float covariancePosition1 = 3;
    float covarianceQuaternion1 = 3;
    graphSaved.addEdge(1, 2, positionDifference1, rotationDifference1, eyeVector * covariancePosition1,
                       covarianceQuaternion1);

    Eigen::Vector3f positionDifference2(0, 12.4, 0);
    Eigen::AngleAxisf rotation_vector2(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference2(rotation_vector2.toRotationMatrix());
    float covariancePosition2 = 0.1;
    float covarianceQuaternion2 = 0.1;
    graphSaved.addEdge(2, 3, positionDifference2, rotationDifference2, eyeVector * covariancePosition2,
                       covarianceQuaternion2);

    Eigen::Vector3f positionDifference3(2, -9, 0);
    Eigen::AngleAxisf rotation_vector3(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference3(rotation_vector3.toRotationMatrix());
    float covariancePosition3 = 0.1;
    float covarianceQuaternion3 = 0.1;
    graphSaved.addEdge(3, 4, positionDifference3, rotationDifference3, eyeVector * covariancePosition3,
                       covarianceQuaternion3);

    Eigen::Vector3f positionDifference4(2, 12.4 - 9, 0);
    Eigen::AngleAxisf rotation_vector4(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference4(rotation_vector4.toRotationMatrix());
    float covariancePosition4 = 0.1;
    float covarianceQuaternion4 = 0.1;
    graphSaved.addEdge(2, 4, positionDifference4, rotationDifference4, eyeVector * covariancePosition4,
                       covarianceQuaternion4);

}

int
main(int argc, char **argv) {
    const int dimension = 3;
    graphSlamSaveStructure graphSaved(dimension);

    createExampleVertex(graphSaved);

    createExampleEdge(graphSaved);

    //graphSaved.optimizeGraphWithSlam();
    graphSaved.printCurrentStateGeneralInformation();
    //graphSaved.printCurrentState();
    //graphSaved.getEdgeBetweenNodes(2,4);
    //std::cout << "now hierachical graph design" << std::endl;
    graphSaved.createHierachicalGraph(1);//this is 1 m
    graphSaved.optimizeGraphWithSlamTopDown(false);
    graphSlamSaveStructure differentGraph = graphSaved;

    differentGraph.optimizeGraphWithSlam(false);

    graphSaved.printCurrentStateGeneralInformation();
    //hierachicalGraph.printCurrentStateGeneralInformation();
    graphSaved.printCurrentStateGeneralInformation();

    return (0);
}


#include <boost/graph/dijkstra_shortest_paths.hpp>


#include "kiteNMPF.h"
#include "integrator.h"

#define  BOOST_TEST_TOOLS_UNDER_DEBUGGER
#define BOOST_TEST_MODULE kite_identification_test
#include <boost/test/included/unit_test.hpp>
#include <fstream>
#include "pseudospectral/chebyshev.hpp"

using namespace casadi;

BOOST_AUTO_TEST_SUITE( kite_identification_test )

BOOST_AUTO_TEST_CASE( first_id_test )
{
    /** Load identification data */
    std::ifstream id_data_file("id_data_state.txt", std::ios::in);
    std::ifstream id_control_file("id_data_control.txt", std::ios::in);
    const int DATA_POINTS = 201;
    const int state_size   = 13;
    const int control_size = 3;

    DM id_data    = DM::zeros(state_size, DATA_POINTS);
    DM id_control = DM::zeros(control_size, DATA_POINTS);

    /** load state trajectory */
    if(!id_data_file.fail())
    {
    for(uint i = 0; i < DATA_POINTS; ++i) {
        for(uint j = 0; j < state_size; ++j){
            double entry;
            id_data_file >> entry;
            id_data(j,i) = entry;
        }
    }
    }
    else
    {
        std::cout << "Could not open : id state data file \n";
        id_data_file.clear();
    }

    /** load control data */
    if(!id_control_file.fail())
    {
    for(uint i = 0; i < DATA_POINTS; ++i){
        for(uint j = 0; j < control_size; ++j){
            double entry;
            id_control_file >> entry;
            /** put in reverse order to comply with Chebyshev method */
            id_control(j,DATA_POINTS - 1 - i) = entry;
        }
    }
    }
    else
    {
        std::cout << "Could not open : id control data file \n";
        id_control_file.clear();
    }

    /** define kite dynamics */
    std::string kite_params_file = "umx_radian.yaml";
    KiteProperties kite_props = kite_utils::LoadProperties(kite_params_file);

    AlgorithmProperties algo_props;
    algo_props.Integrator = CVODES;
    algo_props.sampling_time = 0.02;
    KiteDynamics kite(kite_props, algo_props, true);
    KiteDynamics kite_int(kite_props, algo_props); //integration model
    Function ode = kite_int.getNumericDynamics();

    /** get dynamics function and state Jacobian */
    Function DynamicsFunc = kite.getNumericDynamics();
    SX X = kite.getSymbolicState();
    SX U = kite.getSymbolicControl();
    SX P = kite.getSymbolicParameters();

    /** state bounds */
    DM LBX = DM::vertcat({2.0, -DM::inf(1), -DM::inf(1), -4 * M_PI, -4 * M_PI, -4 * M_PI, -DM::inf(1), -DM::inf(1), -DM::inf(1),
                          -1.05, -1.05, -1.05, -1.05});
    DM UBX = DM::vertcat({DM::inf(1), DM::inf(1), DM::inf(1), 4 * M_PI, 4 * M_PI, 4 * M_PI, DM::inf(1), DM::inf(1), DM::inf(1),
                          1.05, 1.05, 1.05, 1.05});
    /** control bounds */
    DM LBU = DM::vec(id_control);
    DM UBU = DM::vec(id_control);

    /** parameter bounds */
    YAML::Node config = YAML::LoadFile("umx_radian.yaml");
    double CL0 = config["aerodynamic"]["CL0"].as<double>();
    double CLa_tot = config["aerodynamic"]["CLa_total"].as<double>();

    double CD0_tot = config["aerodynamic"]["CD0_total"].as<double>();
    double CYb = config["aerodynamic"]["CYb"].as<double>();
    double Cm0 = config["aerodynamic"]["Cm0"].as<double>();
    double Cma = config["aerodynamic"]["Cma"].as<double>();
    double Cnb = config["aerodynamic"]["Cnb"].as<double>();
    double Clb = config["aerodynamic"]["Clb"].as<double>();

    double CLq = config["aerodynamic"]["CLq"].as<double>();
    double Cmq = config["aerodynamic"]["Cmq"].as<double>();
    double CYr = config["aerodynamic"]["CYr"].as<double>();
    double Cnr = config["aerodynamic"]["Cnr"].as<double>();
    double Clr = config["aerodynamic"]["Clr"].as<double>();
    double CYp = config["aerodynamic"]["CYp"].as<double>();
    double Clp = config["aerodynamic"]["Clp"].as<double>();
    double Cnp = config["aerodynamic"]["Cnp"].as<double>();

    double CLde = config["aerodynamic"]["CLde"].as<double>();
    double CYdr = config["aerodynamic"]["CYdr"].as<double>();
    double Cmde = config["aerodynamic"]["Cmde"].as<double>();
    double Cndr = config["aerodynamic"]["Cndr"].as<double>();
    double Cldr = config["aerodynamic"]["Cldr"].as<double>();

    double Lt = config["tether"]["length"].as<double>();
    double Ks = config["tether"]["Ks"].as<double>();
    double Kd = config["tether"]["Kd"].as<double>();
    double rx = config["tether"]["rx"].as<double>();
    double rz = config["tether"]["rz"].as<double>();

    DM REF_P = DM::vertcat({CL0, CLa_tot, CD0_tot, CYb, Cm0, Cma, Cnb, Clb, CLq, Cmq,
                            CYr, Cnr, Clr, CYp, Clp, Cnp, CLde, CYdr, Cmde, Cndr, Cldr});
    DM LBP = REF_P; DM UBP = REF_P;
    LBP = -DM::inf(21);
    UBP = DM::inf(21);

    LBP[0] = REF_P[0] -  0.1 * fabs(REF_P[0]); UBP[0] = REF_P[0] +  0.1 * fabs(REF_P[0]); // CL0
    LBP[1] = REF_P[1] - 0.05 * fabs(REF_P[1]); UBP[1] = REF_P[1] +  0.1 * fabs(REF_P[1]); // CLa
    LBP[2] = REF_P[2] -  0.1 * fabs(REF_P[2]); UBP[2] = REF_P[2] + 0.25 * fabs(REF_P[2]); // CD0
    LBP[3] = REF_P[3] -  0.5 * fabs(REF_P[3]); UBP[3] = REF_P[3] +  0.5 * fabs(REF_P[3]); // CYb
    LBP[4] = REF_P[4] -  0.5 * fabs(REF_P[4]); UBP[4] = REF_P[4] +  0.5 * fabs(REF_P[4]); // Cm0
    LBP[5] = REF_P[5] -  0.1 * fabs(REF_P[5]); UBP[5] = REF_P[5] + 0.30 * fabs(REF_P[5]); // Cma
    LBP[6] = REF_P[6] -  0.5 * fabs(REF_P[6]); UBP[6] = REF_P[6] +  0.5 * fabs(REF_P[6]); // Cnb
    LBP[7] = REF_P[7] -  0.5 * fabs(REF_P[7]); UBP[7] = REF_P[7] +  0.5 * fabs(REF_P[7]); // Clb
    LBP[8] = REF_P[8] -  0.2 * fabs(REF_P[8]); UBP[8] = REF_P[8] +  0.2 * fabs(REF_P[8]); // CLq
    LBP[9] = REF_P[9] -  0.3 * fabs(REF_P[9]); UBP[9] = REF_P[9] +  0.3 * fabs(REF_P[9]); // Cmq

    LBP[10] = REF_P[10] -  0.3 * fabs(REF_P[10]); UBP[10] = REF_P[10] +  0.3 * fabs(REF_P[10]); // CYr
    LBP[11] = REF_P[11] -  0.5 * fabs(REF_P[11]); UBP[11] = REF_P[11] +  0.5 * fabs(REF_P[11]); // Cnr
    LBP[12] = REF_P[12] -  0.5 * fabs(REF_P[12]); UBP[12] = REF_P[12] +  0.5 * fabs(REF_P[12]); // Clr
    LBP[13] = REF_P[13] -  0.5 * fabs(REF_P[13]); UBP[13] = REF_P[13] +  0.5 * fabs(REF_P[13]); // CYp
    LBP[14] = REF_P[14] -  0.5 * fabs(REF_P[14]); UBP[14] = REF_P[14] +  0.5 * fabs(REF_P[14]); // Clp
    LBP[15] = REF_P[15] -  0.3 * fabs(REF_P[15]); UBP[15] = REF_P[15] +  1.0 * fabs(REF_P[15]); // Cnp
    LBP[16] = REF_P[16] -  0.5 * fabs(REF_P[16]); UBP[16] = REF_P[16] +  0.5 * fabs(REF_P[16]); // CLde
    LBP[17] = REF_P[17] -  0.5 * fabs(REF_P[17]); UBP[17] = REF_P[17] +  0.5 * fabs(REF_P[17]); // CYdr
    LBP[18] = REF_P[18] -  0.5 * fabs(REF_P[18]); UBP[18] = REF_P[18] +  0.5 * fabs(REF_P[18]); // Cmde
    LBP[19] = REF_P[19] -  0.5 * fabs(REF_P[19]); UBP[19] = REF_P[19] +  0.5 * fabs(REF_P[19]); // Cndr
    LBP[20] = REF_P[20] -  0.5 * fabs(REF_P[20]); UBP[20] = REF_P[20] +  0.5 * fabs(REF_P[20]); // Cldr

    // LBP[21] = 2.65;    UBP[21] = 2.75;   // tether length
    // LBP[22] = 150.0;  UBP[22] = 150.0;  // Ks
    // LBP[23] = 0.0;    UBP[23] = 10;   // Kd
    // LBP[24] = 0.0;    UBP[24] = 0.0;   // rx
    // LBP[25] = 0.0;    UBP[25] = 0.0;  // rz

    std::cout << "OK so far \n";

    /** ----------------------------------------------------------------------------------*/
    const int num_segments = 10;
    const int poly_order   = 20;
    const int dimx         = 13;
    const int dimu         = 3;
    const int dimp         = 21;
    const double tf        = 5.0;

    Chebyshev<SX, poly_order, num_segments, dimx, dimu, dimp> spectral;
    SX diff_constr = spectral.CollocateDynamics(DynamicsFunc, 0, tf);
    diff_constr = diff_constr(casadi::Slice(0, num_segments * poly_order * dimx));

    SX varx = spectral.VarX();
    SX varu = spectral.VarU();
    SX varp = spectral.VarP();

    SX opt_var = SX::vertcat(SXVector{varx, varu, varp});

    SX lbg = SX::zeros(diff_constr.size());
    SX ubg = SX::zeros(diff_constr.size());

    /** set inequality (box) constraints */
    /** state */
    SX lbx = SX::repmat(LBX, num_segments * poly_order + 1, 1);
    SX ubx = SX::repmat(UBX, num_segments * poly_order + 1, 1);

    /** control */
    lbx = SX::vertcat({lbx, LBU});
    ubx = SX::vertcat({ubx, UBU});

    /** parameters */
    lbx = SX::vertcat({lbx, LBP});
    ubx = SX::vertcat({ubx, UBP});

    DM Q  = SX::diag(SX({1e3, 1e2, 1e2,  1e2, 1e2, 1e2,  1e1, 1e1, 1e2,  1e2, 1e2, 1e2, 1e2})); //good one as well
    //DM Q = 1e1 * DM::eye(13);
    double alpha = 100.0;

    SX fitting_error = 0;
    SX varx_ = SX::reshape(varx, state_size, DATA_POINTS);
    for (uint j = 0; j < DATA_POINTS; ++j)
    {
        SX measurement = id_data(Slice(0, id_data.size1()), j);
        SX error = measurement - varx_(Slice(0, varx_.size1()), varx_.size2() - j - 1);
        fitting_error += static_cast<double>(1.0 / DATA_POINTS) * SX::sumRows( SX::mtimes(Q, pow(error, 2)) );
    }

    /** add regularisation */
    // fitting_error = fitting_error + alpha * SX::dot(varp - SX({REF_P}), varp - SX({REF_P}));

    /** alternative approximation */
    SX x = SX::sym("x", state_size);
    SX y = SX::sym("y", state_size);
    SX cost_function = SX::sumRows( SX::mtimes(Q, pow(x - y, 2)) );
    Function IdCost = Function("IdCost",{x,y}, {cost_function});
    SX fitting_error2 = spectral.CollocateIdCost(IdCost, id_data, 0, tf);
    fitting_error2 = fitting_error2 + alpha * SX::dot(varp - SX({REF_P}), varp - SX({REF_P}));

    /** formulate NLP */
    SXDict NLP;
    Dict OPTS;
    DMDict ARG;
    NLP["x"] = opt_var;
    NLP["f"] = fitting_error;
    NLP["g"] = diff_constr;

    OPTS["ipopt.linear_solver"]  = "ma97";
    OPTS["ipopt.print_level"]    = 5;
    OPTS["ipopt.tol"]            = 1e-4;
    OPTS["ipopt.acceptable_tol"] = 1e-4;
    OPTS["ipopt.warm_start_init_point"] = "yes";
    //OPTS["ipopt.max_iter"]       = 20;

    Function NLP_Solver = nlpsol("solver", "ipopt", NLP, OPTS);

    std::cout << "Ok here as well \n";

    /** set default args */
    ARG["lbx"] = lbx;
    ARG["ubx"] = ubx;
    ARG["lbg"] = lbg;
    ARG["ubg"] = ubg;

    /** provide initial guess from integrator */
    casadi::DMDict props;
    props["scale"] = 0;
    props["P"] = casadi::DM::diag(casadi::DM({0.1, 1/3.0, 1/3.0, 1/2.0, 1/5.0, 1/2.0, 1/3.0, 1/3.0, 1/3.0, 1.0, 1.0, 1.0, 1.0}));
    props["R"] = casadi::DM::diag(casadi::DM({1/0.15, 1/0.2618, 1/0.2618}));
    PSODESolver<poly_order,num_segments,dimx,dimu>ps_solver(ode, tf, props);

    //here put actual numbers every time?
    //DM x0 = id_data(Slice(0, id_data.size1()), 0);
    DM init_state = id_data(Slice(0, id_data.size1()), 0);
    DM init_control = DM({0.1, 0.0, 0.0});
    init_control = casadi::DM::repmat(init_control, (num_segments * poly_order + 1), 1);

    DMDict solution = ps_solver.solve_trajectory(init_state, init_control, true);
    DM feasible_state = solution.at("x");
    //DM feasible_state = DM::reshape(id_data, 13 * (num_segments * poly_order + 1), 1);
    //DM feasible_state = DM::repmat(id_data(Slice(0, id_data.size1()), 0), (num_segments * poly_order + 1), 1);

    std::ofstream trajectory_file("integrated_trajectory.txt", std::ios::out);

    if(!trajectory_file.fail())
    {
        for (int i = 0; i < varx.size1(); i = i + 13)
        {
            std::vector<double> tmp = feasible_state(Slice(i, i + 13),0).nonzeros();
            for (uint j = 0; j < tmp.size(); j++)
            {
                trajectory_file << tmp[j] << " ";
            }
            trajectory_file << "\n";
        }
    }
    trajectory_file.close();

    std::cout << "Initial guess computed. \n";

    DM feasible_control = (UBU + LBU) / 2;

    //ARG["x0"] = DM::vertcat(DMVector{feasible_state, feasible_control, REF_P});
    ARG["x0"] = DM::vertcat(DMVector{feasible_state, REF_P});
    ARG["lam_g0"] = solution.at("lam_g");
    ARG["lam_x0"] = DM::vertcat({solution.at("lam_x"), DM::zeros(REF_P.size1())});

    int idx_in = num_segments * poly_order * dimx;
    int idx_out = idx_in + dimx;
    ARG["lbx"](Slice(idx_in, idx_out), 0) = init_state;
    ARG["ubx"](Slice(idx_in, idx_out), 0) = init_state;

    DMDict res = NLP_Solver(ARG);
    DM result = res.at("x");

    DM new_params = result(Slice(result.size1() - varp.size1(), result.size1()));
    std::vector<double> new_params_vec = new_params.nonzeros();

    DM trajectory = result(Slice(0, varx.size1()));
    //DM trajectory = DM::reshape(traj, DATA_POINTS, dimx );
    std::ofstream est_trajectory_file("estimated_trajectory.txt", std::ios::out);

    if(!est_trajectory_file.fail())
    {
        for (int i = 0; i < trajectory.size1(); i = i + dimx)
        {
            std::vector<double> tmp = trajectory(Slice(i, i + dimx),0).nonzeros();
            for (uint j = 0; j < tmp.size(); j++)
            {
                est_trajectory_file << tmp[j] << " ";
            }
            est_trajectory_file << "\n";
        }
    }
    est_trajectory_file.close();

    /** update parameter file */
    config["aerodynamic"]["CL0"] = new_params_vec[0];
    config["aerodynamic"]["CLa_total"] = new_params_vec[1];
    config["aerodynamic"]["CD0_total"] = new_params_vec[2];
    config["aerodynamic"]["CYb"] = new_params_vec[3];
    config["aerodynamic"]["Cm0"] = new_params_vec[4];
    config["aerodynamic"]["Cma"] = new_params_vec[5];
    config["aerodynamic"]["Cnb"] = new_params_vec[6];
    config["aerodynamic"]["Clb"] = new_params_vec[7];

    config["aerodynamic"]["CLq"] = new_params_vec[8];
    config["aerodynamic"]["Cmq"] = new_params_vec[9];
    config["aerodynamic"]["CYr"] = new_params_vec[10];
    config["aerodynamic"]["Cnr"] = new_params_vec[11];
    config["aerodynamic"]["Clr"] = new_params_vec[12];
    config["aerodynamic"]["CYp"] = new_params_vec[13];
    config["aerodynamic"]["Clp"] = new_params_vec[14];
    config["aerodynamic"]["Cnp"] = new_params_vec[15];

    config["aerodynamic"]["CLde"] = new_params_vec[16];
    config["aerodynamic"]["CYdr"] = new_params_vec[17];
    config["aerodynamic"]["Cmde"] = new_params_vec[18];
    config["aerodynamic"]["Cndr"] = new_params_vec[19];
    config["aerodynamic"]["Cldr"] = new_params_vec[20];

    // config["tether"]["length"] = new_params_vec[21];
    // config["tether"]["Ks"] = new_params_vec[22];
    // config["tether"]["Kd"] = new_params_vec[23];
    // config["tether"]["rx"] = new_params_vec[24];
    // config["tether"]["rz"] = new_params_vec[25];

    std::ofstream fout("umx_radian_id.yaml");
    fout << config;

    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_SUITE_END()


#include "control/system/ss.h"
#include "gtest/gtest.h"
#include "gmock/gmock.h"
#include <Eigen/Dense>
#include <iostream>

/**
 * Discrete time state-space tests
 * @todo write tests for MIMO
 */
namespace {

class SSTest : public ::testing::Test {
public:
    using ss = control::system::ss<float,2>;
protected:
  ss *P;
  SSTest() {
    ss::TA A;
    ss::TB B;
    ss::TC C;
    ss::TD D;
    A << 1, 1, 0, 1;
    B << 0.5, 1;
    C << 1, 0;
    D << 0;
    P = new ss(A,B,C,D);
  }
};

TEST_F(SSTest, SimpleSSTest) {
  std::vector<float> r(10);
  std::vector<float> v = {0.5, 2, 4.5, 8, 12.5, 18, 24.5, 32, 40.5};
  std::generate(r.begin(), r.end(), [this]{
                ss::Tu u;
                u << 1;
                auto y = P->step(u);
                return y(0);
                });

	for( size_t i = 0; i < v.size(); i++ )
		ASSERT_FLOAT_EQ(r[i], v[i]);

}

}  // namespace


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

class ContactInfo
{
public:

  V m_point;
  V m_normal;
  T m_distance;

};

class MyCallback
  : public geometry::ContactsCallback<V>
{
public:

  std::vector<ContactInfo> m_contacts;

public:

  void operator()(
                  V const & point
                  , V const & normal
                  , typename V::real_type const & distance
                  )
  {
    ContactInfo info;

    info.m_point = point;
    info.m_normal = normal;
    info.m_distance = distance;

    m_contacts.push_back(info);
  }

};

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(contacts_sphere_sphere_test)
{

  // Penetration
  {
    V const centerA = V::make(0.0, 0.0, 0.0);
    T const radiusA = 2.0;
    V const centerB = V::make(5.0, 0.0, 0.0);
    T const radiusB = 4.0;

    geometry::Sphere<V> const A = geometry::make_sphere(centerA, radiusA);
    geometry::Sphere<V> const B = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_sphere_sphere(A, B, 0.0, callback);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0], 1.6666, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -1.0, 0.01);

  }

  // Touching
  {
    V const centerA = V::make(0.0, 0.0, 0.0);
    T const radiusA = 2.0;
    V const centerB = V::make(6.0, 0.0, 0.0);
    T const radiusB = 4.0;

    geometry::Sphere<V> const A = geometry::make_sphere(centerA, radiusA);
    geometry::Sphere<V> const B = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_sphere_sphere(A, B, 0.0, callback);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0], 2.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance,  0.0, 0.01);

  }

  // Separation
  {
    V const centerA = V::make(0.0, 0.0, 0.0);
    T const radiusA = 2.0;
    V const centerB = V::make(7.0, 0.0, 0.0);
    T const radiusB = 4.0;

    geometry::Sphere<V> const A = geometry::make_sphere(centerA, radiusA);
    geometry::Sphere<V> const B = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_sphere_sphere(A, B, 0.0, callback);

    BOOST_CHECK(callback.m_contacts.size() == 0u);
  }

}

BOOST_AUTO_TEST_SUITE_END();


/*
A "C" interface to libigl's bbw.
*/

#include <cassert>

#define IGL_HEADER_ONLY
#define IGL_NO_MOSEK
#include <igl/boundary_conditions.h>
#include <igl/normalize_row_sums.h>
#include <igl/bbw/bbw.h>

#include <Eigen/Dense>

#include <iostream>

#define kVertexDimension 3

extern "C"
{

typedef double real_t;
typedef int index_t;

// Returns 0 for success, anything else is an error.
int bbw(
    /// Input Parameters
    // 'vertices' is a pointer to num_vertices*kVertexDimension floating point values,
    // packed: x0, y0, z0, x1, y1, z1, ...
    // In other words, a num_vertices-by-kVertexDimension matrix packed row-major.
    int num_vertices, real_t* vertices,
    // 'faces' is a pointer to num_faces*3 integers,
    // where each face is three vertex indices: f0.v0, f0.v1, f0.v2, f1.v0, f1.v1, f1.v2, ...
    // Face i's vertices are: vertices[ faces[3*i]*2 ], vertices[ faces[3*i+1]*2 ], vertices[ faces[3*i+2]*2 ]
    // In other words, a num_faces-by-3 matrix packed row-major.
    int num_faces, index_t* faces,
    // 'skeleton_vertices' is a pointer to num_skeleton_vertices*kVertexDimension floating point values,
    // packed the same way as 'vertices' (NOTE: And whose positions must also exist inside 'vertices'.)
    int num_skeleton_vertices, real_t* skeleton_vertices,
    // 'skeleton_point_handles' is a pointer to num_skeleton_point_handles integers,
    // where each element "i" in skeleton_point_handles references the vertex whose data
    // is located at skeleton_vertices[ skeleton_point_handles[i]*kVertexDimension ].
    int num_skeleton_point_handles, index_t* skeleton_point_handles,
    // TODO: Take skeleton bone edges and cage edges

    /// Output Parameters
    // 'Wout' is a pointer to num_vertices*num_skeleton_vertices values.
    // Upon return, W will be filled with each vertex in 'num_vertices' weight for
    // each skeleton vertex in 'num_skeleton_vertices'.
    // The data layout is that all 'num_skeleton_vertices' weights for vertex 0
    // appear before all 'num_skeleton_vertices' weights for vertex 1, and so on.
    // In other words, a num_vertices-by-num_skeleton_vertices matrix packed row-major.
    real_t* Wout
    )
{
    using namespace std;
    using namespace igl;
    using namespace Eigen;

    assert( num_vertices > 0 );
    assert( vertices );
    assert( num_faces > 0 );
    assert( faces );
    assert( num_skeleton_vertices > 0 );
    assert( skeleton_vertices );

    // TODO: These next two asserts need not be true once we add support for the other
    //       kinds of handles.
    assert( num_skeleton_point_handles > 0 );
    assert( skeleton_point_handles );

    assert( Wout );

    // #V by 2 list of mesh vertex positions
    // Make this an Eigen::map from 'vertices'
    MatrixXd V = Eigen::Map< Eigen::Matrix< real_t, Eigen::Dynamic, kVertexDimension, Eigen::RowMajor > >( vertices, num_vertices, kVertexDimension );
    // #F by 3 list of triangle indices
    // Make this an Eigen::map from 'faces'
    MatrixXi F = Eigen::Map< Eigen::Matrix< index_t, Eigen::Dynamic, 3, Eigen::RowMajor > >( faces, num_faces, 3 );

    // "Skeleton" (handles) descriptors:
    // List of control and joint (bone endpoint) positions
    // Make this an Eigen::map from skeleton_vertices.
    MatrixXd C = Eigen::Map< Eigen::Matrix< real_t, Eigen::Dynamic, kVertexDimension, Eigen::RowMajor > >( skeleton_vertices, num_skeleton_vertices, kVertexDimension );
    // List of point handles indexing C
    // Make this an Eigen::map from skeleton_point_handles.
    VectorXi P = Eigen::Map< Eigen::Matrix< index_t, Eigen::Dynamic, 1 > >( skeleton_point_handles, num_skeleton_point_handles, 1 );
    // List of bone edges indexing C
    // TODO: ...
    MatrixXi BE;
    // List of cage edges indexing *P*
    // TODO: ...
    MatrixXi CE;

    // Compute boundary conditions (aka fixed value constraints)
    // List of boundary indices (aka fixed value indices into VV)
    VectorXi b;
    // List of boundary conditions of each weight function
    MatrixXd bc;
    if(!boundary_conditions(V,F,C,P,BE,CE,b,bc))
    {
        return 1;
    }

    // compute BBW
    // Default bbw data and flags
    BBWData bbw_data;
    // Weights matrix
    MatrixXd W;
    if(!bbw(V,F,b,bc,bbw_data,W))
    {
        return 2;
    }

    // Normalize weights.
    normalize_row_sums(W,W);

    // Save output
    // Copy W to Wout.
    Eigen::Map< Eigen::Matrix< real_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > >( Wout, num_vertices, num_skeleton_vertices ) = W;

    return 0;
}

}


#include "lwtnn/LightweightNeuralNetwork.hh"
#include "lwtnn/Stack.hh"
#include "lwtnn/parse_json.hh"

#include <Eigen/Dense>

#include <iostream>
#include <sstream>
#include <string>
#include <fstream>

void usage(const std::string& name) {
  std::cout << "usage: " << name << " <nn config> [<disable>]\n"
            << "\n"
            << "Read in an RNN, feed it data.\n"
            << "With anything for <disable>, just run the random number gen";
}

std::vector<lwt::ValueMap> get_values(
  const std::vector<lwt::Input>& inputs) {
  Eigen::MatrixXd test_pattern = Eigen::MatrixXd::Random(inputs.size(), 40);
  std::vector<lwt::ValueMap> out;
  const auto n_cols = static_cast<std::size_t>(test_pattern.cols());
  for (std::size_t iii = 0; iii < n_cols; iii++) {
    lwt::ValueMap vals;
    for (std::size_t jjj = 0; jjj < inputs.size(); jjj++) {
      vals[inputs.at(jjj).name] = test_pattern(jjj, iii);
    }
    out.push_back(vals);
  }
  return out;
}

lwt::VectorMap get_values_vec(const std::vector<lwt::Input>& inputs) {
  Eigen::MatrixXd test_pattern = Eigen::MatrixXd::Random(inputs.size(), 40);
  lwt::VectorMap out;
  for (std::size_t in_num = 0; in_num < inputs.size(); in_num++) {
    std::vector<double> ins;
    const auto n_cols = static_cast<std::size_t>(test_pattern.cols());
    for (std::size_t iii = 0; iii < n_cols; iii++) {
      ins.push_back(test_pattern(in_num, iii));
    }
    out[inputs.at(in_num).name] = std::move(ins);
  }
  return out;
}

int main(int argc, char* argv[]) {
  if (argc < 2 || argc > 3) {
    usage(argv[0]);
    exit(1);
  }
  bool run_stack = true;
  if (argc == 3) {
    run_stack = false;
  }
  // Read in the configuration.
  std::string in_file_name(argv[1]);
  std::ifstream in_file(in_file_name);
  auto config = lwt::parse_json(in_file);

  if (!config.miscellaneous.count("sort_order")) {
    std::cout << "no sort order given!" << std::endl;
  } else {
    std::cout << "sort order: " << config.miscellaneous.at("sort_order")
              << std::endl;
  }

  std::size_t n_inputs = config.inputs.size();
  lwt::ReductionStack stack(n_inputs, config.layers);
  lwt::LightweightRNN rnn(config.inputs, config.layers, config.outputs);
  Eigen::VectorXd sum_outputs = Eigen::VectorXd::Zero(stack.n_outputs());
  std::size_t n_loops = 1;
  std::cout << "running over " << n_loops << " loops" << std::endl;
  std::cout << "running " << (run_stack ? "fast": "slow") << std::endl;
  for (std::size_t nnn = 0; nnn < n_loops; nnn++) {
    if (run_stack) {
      Eigen::MatrixXd test_pattern = Eigen::MatrixXd::Random(n_inputs, 2);
      std::cout << test_pattern << std::endl;
      sum_outputs += stack.reduce(test_pattern);
    } else {
      const auto inputs = get_values_vec(config.inputs);
      auto out = rnn.reduce(inputs);
      for (std::size_t iii = 0; iii < config.outputs.size(); iii++) {
        sum_outputs(iii) += out.at(config.outputs.at(iii));
      }
    }
  }
  std::cout << "output sum:\n" << sum_outputs << std::endl;
  return 0;
}


/*
 * get_dimension_count.hpp
 *
 *  Created on: Apr 11, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

#pragma once

// libraries
#include <Eigen/Eigen>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "typedefs.hpp"

namespace math {

template<typename Container>
class ContainerWrapper {};

template<typename Scalar>
class ContainerWrapper<Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> >{
public:
	static const int DimensionCount = 2;
	typedef math::Vector2i Coordinates;
	typedef Eigen::Matrix<math::Vector2<Scalar>,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> VectorContainer;
	typedef Eigen::Matrix<math::Matrix2<Scalar>,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixContainer;
};

template<typename Scalar, int DimensionCountIn>
class ContainerWrapper<Eigen::Tensor<Scalar, DimensionCountIn, Eigen::ColMajor>>{
public:
	static const int DimensionCount = DimensionCountIn;
	typedef math::Vector3i Coordinates;
};

template<typename Scalar>
class ContainerWrapper<Eigen::Tensor<Scalar, 3, Eigen::ColMajor>>{
public:
	static const int DimensionCount = 3;
	typedef math::Vector3i Coordinates;
	typedef Eigen::Tensor<math::Vector3<Scalar>,3,Eigen::ColMajor> VectorContainer;
	typedef Eigen::Tensor<math::Matrix3<Scalar>,3,Eigen::ColMajor> MatrixContainer;
};

} // namespace math


#include <Eigen/Dense>
#include <mutex>
#include <chrono>
#include <atomic>
#include <iostream>

#ifndef UTILS_KALMAN_H
#define UTILS_KALMAN_H

namespace utils {

    //https://stackoverflow.com/a/28876046
    template <typename T>
    struct is_chrono_duration
    {
        static constexpr bool value = false;
    };

    template <typename Rep, typename Period>
    struct is_chrono_duration<std::chrono::duration<Rep, Period>>
    {
        static constexpr bool value = true;
    };

    static void PRINT_MATRIX_SIZES(const Eigen::MatrixXd& P_0, const Eigen::VectorXd& z, const Eigen::VectorXd& u) {
        int m = z.size(), l = u.size(), n = P_0.rows();
        std::cout << "\n\n\n--------------------------" << std::endl;
        std::cout << "m = z.size() = " << m << std::endl;
        std::cout << "l = u.size() = " << l << std::endl;
        std::cout << "n = P.rows() = " << n << std::endl;
        std::cout << "A(nxn) = " << n << "x" << n << std::endl;
        std::cout << "B(nxl) = " << n << "x" << l << std::endl;
        std::cout << "C(mxn) = " << m << "x" << n << std::endl;
        std::cout << "Q(nxn) = " << n << "x" << n << std::endl;
        std::cout << "R(mxm) = " << m << "x" << m << std::endl;
        std::cout << "K(nxm) = " << n << "x" << m << std::endl;
        std::cout << "x(n) = " << n << std::endl;
        std::cout << "--------------------------\n\n\n" << std::endl;
    }

    static void PRINT_MATRIX_SIZES(const Eigen::MatrixXd& P_0, const Eigen::VectorXd& z) {
        int m = z.size(), n = P_0.rows();
        std::cout << "\n\n\n--------------------------" << std::endl;
        std::cout << "m = z.size() = " << m << std::endl;
        std::cout << "n = P.rows() = " << n << std::endl;
        std::cout << "A(nxn) = " << n << "x" << n << std::endl;
        std::cout << "C(mxn) = " << m << "x" << n << std::endl;
        std::cout << "Q(nxn) = " << n << "x" << n << std::endl;
        std::cout << "R(mxm) = " << m << "x" << m << std::endl;
        std::cout << "K(nxm) = " << n << "x" << m << std::endl;
        std::cout << "x(n) = " << n << std::endl;
        std::cout << "--------------------------\n\n\n" << std::endl;
    }
    template <typename T = std::chrono::milliseconds>
    class Kalman {
        static_assert(is_chrono_duration<T>::value, "T not derived from std::chrono::duration");
    private:
        Eigen::VectorXd x_;
        std::mutex mtx_;
        std::chrono::steady_clock::time_point last_call_;
        Eigen::MatrixXd P_, K_, I_;
        std::atomic<long> dt_{ 0 };
        std::atomic_bool first_call_ {true};

        /*
        Method to set dt_ for the integral / derivate term
        */
        void setDt();

        /**
         * Initialize the component
         */
        void init();

        /**
         * Update all matrices and dt
         */
        void preupdate();

        /**
         * Update the matrixes and calculate the values based on new x
         * @param z The measured / calculated system state
         */
        void updateStep(const Eigen::VectorXd& z);

    protected:
        Eigen::MatrixXd A_, Q_, B_, R_, C_;

        /**
         * Method to update the A matrix (state transition model) of the Kalman Filter
         */
        virtual void updateA();

        /**
         * Method to update the B matrix (control-input) of the Kalman Filter
         */
        virtual void updateB();

        /**
         * Method to update the C matrix (uutput matrix) of the Kalman Filter
         */
        virtual void updateC();

        /**
         * Method to update the Q matrix (process noise covariance) of the Kalman Filter
         */
        virtual void updateQ();

        /**
         * Method to update the R matrix (measurement noise covariance) of the Kalman Filter
         */
        virtual void updateR();

    public:
        /**
         * Constructor
         * @param A State transition model
         * @param B Control input matrix
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param Q Process noise covariance
         * @param R Measurement noise covariance
         * @param x0 Inital state guess
         */
        Kalman(
            const Eigen::MatrixXd& A,
            const Eigen::MatrixXd& B,
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& Q,
            const Eigen::MatrixXd& R,
            const Eigen::VectorXd& x0
        );

        /**
         * Constructor
         * @param A State transition model
         * @param B Control input matrix
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param Q Process noise covariance
         * @param R Measurement noise covariance
         */
        Kalman(
            const Eigen::MatrixXd& A,
            const Eigen::MatrixXd& B,
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& Q,
            const Eigen::MatrixXd& R
        );

        /**
         * Constructor
         *
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param R Measurement noise covariance
         * @param x0 Inital state guess
         */
        Kalman(
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& R,
            const Eigen::VectorXd& x0
        );

        /**
         * Constructor
         *
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param R Measurement noise covariance
         */
        Kalman(
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& R
        );

        /**
         * Predict the state of the system based on observed state
         * @param out Vector to store prediction
         * @param z The observed state
         *
         */
        void predict(Eigen::VectorXd& out, const Eigen::VectorXd& z);

        /**
         * Predict the state of the system based on control input
         * @param out Vector to store prediction
         * @param z The observed state
         * @param u control input
         */
        void predict(Eigen::VectorXd& out, const Eigen::VectorXd& z, const Eigen::VectorXd& u);

        /**
         * Method to get dt value
         * @returns delta t between last steps
         */
        double getDt();
    };
}

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// 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 "point_solid_signed_squared_distance.h"
#include "points_inside_component.h"
#include "point_mesh_squared_distance.h"
#include "../../list_to_matrix.h"
#include "../../slice_mask.h"
#include <vector>
#include <Eigen/Core>

template <
  typename DerivedQ,
  typename DerivedVB,
  typename DerivedFB,
  typename DerivedD>
IGL_INLINE void igl::copyleft::cgal::point_solid_signed_squared_distance(
  const Eigen::PlainObjectBase<DerivedQ> & Q,
  const Eigen::PlainObjectBase<DerivedVB> & VB,
  const Eigen::PlainObjectBase<DerivedFB> & FB,
  Eigen::PlainObjectBase<DerivedD> & D)
{
  // compute unsigned distances
  Eigen::VectorXi I;
  DerivedVB C;
  point_mesh_squared_distance<CGAL::Epeck>(Q,VB,FB,D,I,C);
  // Collect queries that have non-zero distance
  Eigen::Array<bool,Eigen::Dynamic,1> NZ = D.array()!=0;
  // Compute sign for non-zero distance queries
  DerivedQ QNZ;
  slice_mask(Q,NZ,1,QNZ);
  Eigen::Array<bool,Eigen::Dynamic,1> DNZ;
  igl::copyleft::cgal::points_inside_component(VB,FB,QNZ,DNZ);
  // Apply sign to distances
  DerivedD S = DerivedD::Zero(Q.rows(),1);
  {
    int k = 0;
    for(int q = 0;q<Q.rows();q++)
    {
      if(NZ(q))
      {
        D(q) *= DNZ(k++) ? -1. : 1.;
      }
    }
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::copyleft::cgal::point_solid_signed_squared_distance<Eigen::Matrix<CGAL::Epeck::FT, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<CGAL::Epeck::FT, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Epeck::FT, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Epeck::FT, -1, 1, 0, -1, 1> >&);
#endif


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file multiway_cut_test_long.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-01-17
 */

#include <boost/test/unit_test.hpp>

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

#include "paal/multiway_cut/multiway_cut.hpp"
#include "paal/utils/irange.hpp"

#include <boost/graph/adjacency_list.hpp>

#include <vector>

namespace{
template <typename Graph>
double test(Graph graph){
    auto weight= boost::get(boost::edge_weight, graph);
    std::vector<std::pair<int,int> > vertices_parts;
    LOGLN("multiway_cut: ");
    auto cost_cut=paal::multiway_cut(graph,back_inserter(vertices_parts));
    std::vector<int> vertices_to_parts;
    vertices_to_parts.resize(vertices_parts.size());
    for(auto i:vertices_parts){
        LOG(i.first<<"("<<i.second<<"), ");
        vertices_to_parts[i.first]=i.second;
    }
    LOGLN("");
    int cost_cut_verification=0;
    auto all_edges=edges(graph);
    for(auto i=all_edges.first;i!=all_edges.second;i++){
        if(vertices_to_parts[source(*i,graph)]!=vertices_to_parts[target(*i,graph)])
            cost_cut_verification+=weight(*i);
    }

    LOGLN("Cost Cut:              "<<cost_cut);
    LOGLN("Cost Cut Verification: "<<cost_cut_verification );
    LOGLN("");
    BOOST_CHECK_EQUAL(cost_cut,cost_cut_verification);
    return cost_cut;
}
auto env_to_add_edge =[](std::vector<std::pair<int,int> >& edges,std::vector<long long>& cost_edges){
    return [&](int source,int target,int edge_cost){
        edges.push_back(std::make_pair(source,target));
        cost_edges.push_back(edge_cost);
        LOGLN(source<<" "<<target<<" "<<edge_cost);
    };
};

}
BOOST_AUTO_TEST_SUITE(multiway_cut)

BOOST_AUTO_TEST_CASE(multiway_cut_random) {
    static const int NU_VERTICES=1000;
    static const int NU_RANDOM_EDGES=1000;
    static const int NU_TERMINAL_EDGES=1000;
    static const int NU_COMPONENTS_EDGES=1000;
    static const int SEED=211;
    static const int CONECT_TO_TERMINAL_COST=1000*1000;
    static const int CONECT_IN_COMPONENT_COST=1000;
    static const int CONECT_BETWEEN_COMPONENTS_COST=1;
    static const int NU_COMPONENTS=3;
    std::vector<std::pair<int,int> > edges_p;
    std::vector<long long> cost_edges;
    std::srand(SEED);
    auto add_edge_to_graph=env_to_add_edge(edges_p,cost_edges);
    auto rand_vertex_id=[&](){
        return rand()%NU_VERTICES;
    };
    auto rand_vertex_id_in_component=[&](int component){
        return (((rand_vertex_id())/NU_COMPONENTS)*NU_COMPONENTS+component)%NU_VERTICES;
    };

    {
        //add edges to terminals
        int source,target,nu_edges_copy=NU_TERMINAL_EDGES;
        while(--nu_edges_copy){
            do{
                source=rand_vertex_id();
                target=source%NU_COMPONENTS;
            }while(source==target);
            add_edge_to_graph(source,target,CONECT_TO_TERMINAL_COST);
        }

        //add edges in components
        nu_edges_copy=NU_COMPONENTS_EDGES;
        while(--nu_edges_copy){
            do{
                source=rand_vertex_id();
                target=rand_vertex_id_in_component(source);
            }while(source==target);
            add_edge_to_graph(source,target,CONECT_IN_COMPONENT_COST);
        }

        //add random edges
        nu_edges_copy=NU_RANDOM_EDGES;
        while(--nu_edges_copy){
            do{
                source=rand_vertex_id();
                target=rand_vertex_id();
            }while(source==target);
            add_edge_to_graph(source,target,CONECT_BETWEEN_COMPONENTS_COST);
        }
    }
    std::vector<int> terminals={0,1,2};
    boost::adjacency_list<boost::vecS,boost::vecS,boost::undirectedS,
                    boost::property < boost::vertex_color_t, int>,
                    boost::property < boost::edge_weight_t, int>
                    > graph(edges_p.begin(),edges_p.end(),cost_edges.begin(),NU_VERTICES);

    for(std::size_t i=1;i<=terminals.size();i++)
        put (boost::vertex_color,graph,terminals[i-1],i);
    ::test(graph);
}

BOOST_AUTO_TEST_CASE(multiway_cut_triangle) {
    //We place 33*33 points on the grid
    //each point above diagonal have edge to right, down and left-down neighbor
    //each point on diagonal have edge to left-down neighbor
    //
    //  *-*-*-*-*
    //  |/|/|/|/
    //  *-*-*-* *
    //  |/|/|/
    //  *-*-* * *
    //  |/|/
    //  *-* * * *
    //  |/
    //  * * * * *
    //
    // in the oder words we have triangle
    // and we several times get all triangle add point on each edge and connect
    // them dividing triangle on 4 triangle
    // edges added in each step have ten times smaller cost

    static const int SIZ=33;
    static const int OPTIMAL=324992;
    static const int NU_VERTICES=SIZ*SIZ;
    static const int SEED=211;
    std::vector<std::pair<int,int> > edges_p;
    std::vector<long long> cost_edges;
    std::srand(SEED);
    auto coordinates= [&](int x,int y){return x*SIZ+y;};
    auto size_range = paal::irange(SIZ);
    auto add_edge_to_graph=env_to_add_edge(edges_p,cost_edges);
    auto gen_cost=[](int position){
        long long cost=1;
        while(position%2==0){
            position/=2;
            cost*=10;
        }
        return cost;
    };

    {
        for(auto i:size_range)
        for(auto j:size_range){
            if(i+j<SIZ-1){
                //add horizontal edges
                add_edge_to_graph(coordinates(i,j),coordinates(i,j+1),gen_cost(i+SIZ-1));
                //add vertical edges
                add_edge_to_graph(coordinates(i,j),coordinates(i+1,j),gen_cost(j+SIZ-1));
            }
            if(i+j<SIZ&&i>0){
                //add diagonal edges
                add_edge_to_graph(coordinates(i,j),coordinates(i-1,j+1),gen_cost(i+j));
            }
        }
    }
    std::vector<int> terminals={0,SIZ-1,(SIZ-1)*SIZ};
    boost::adjacency_list<boost::vecS,boost::vecS,boost::undirectedS,
                    boost::property < boost::vertex_color_t, int>,
                    boost::property < boost::edge_weight_t, int>
                    > graph(edges_p.begin(),edges_p.end(),cost_edges.begin(),NU_VERTICES);

    for (std::size_t i = 1; i <= terminals.size(); i++)
        put(boost::vertex_color, graph, terminals[i - 1], i);

    int cost_cut=::test(graph);
    check_result(cost_cut,OPTIMAL,2);
}
BOOST_AUTO_TEST_SUITE_END();