/*
 * This file is part of the Geneva library collection.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 * The following license applies to the code IN THIS FILE:
 *
 * ***************************************************************************
 *
 * Boost Software License - Version 1.0 - August 17th, 2003
 *
 * Permission is hereby granted, free of charge, to any person or organization
 * obtaining a copy of the software and accompanying documentation covered by
 * this license (the "Software") to use, reproduce, display, distribute,
 * execute, and transmit the Software, and to prepare derivative works of the
 * Software, and to permit third-parties to whom the Software is furnished to
 * do so, all subject to the following:
 *
 * The copyright notices in the Software and this entire statement, including
 * the above license grant, this restriction and the following disclaimer,
 * must be included in all copies of the Software, in whole or in part, and
 * all derivative works of the Software, unless such copies or derivative
 * works are solely in the form of machine-executable object code generated by
 * a source language processor.
 *
 * 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 * ***************************************************************************
 *
 * NOTE THAT THE BOOST-LICENSE DOES NOT APPLY TO ANY OTHER FILES OF THE
 * GENEVA LIBRARY, UNLESS THIS IS EXPLICITLY STATED IN THE CORRESPONDING FILE!
 */

#pragma once

// Global checks, defines and includes needed for all of Geneva
#include "common/GGlobalDefines.hpp"

// Standard headers go here
#include <cmath>
#include <cstdlib>

// Boost headers go here
#include <boost/math/special_functions.hpp>

// Geneva headers go here
#include "common/GExceptions.hpp"
#include "common/GLogger.hpp"
#include "common/GErrorStreamer.hpp"

namespace Gem {
namespace Common {

/******************************************************************************/

/** @brief Calculates the acos value of a float value */
G_API_COMMON float gacos(const float &);
/** @brief Calculates the acos value of a double value */
G_API_COMMON double gacos(const double &);

/** @brief Calculates the asin value of a float value */
G_API_COMMON float gasin(const float &);
/** @brief Calculates the asin value of a double value */
G_API_COMMON double gasin(const double &);

/** @brief Calculates the tan value of a float value */
G_API_COMMON float gtan(const float &);
/** @brief Calculates the tan value of a double value */
G_API_COMMON double gtan(const double &);

/** @brief Calculates the tanh value of a float value */
G_API_COMMON float gtanh(const float &);
/** @brief Calculates the tanh value of a double value */
G_API_COMMON double gtanh(const double &);

/** @brief Calculates the atan value of a float value */
G_API_COMMON float gatan(const float &);
/** @brief Calculates the atan value of a double value */
G_API_COMMON double gatan(const double &);

/** @brief Calculates the sinh value of a float value */
G_API_COMMON float gsinh(const float &);
/** @brief Calculates the sinh value of a double value */
G_API_COMMON double gsinh(const double &);

/** @brief Calculates the cosh value of a float value */
G_API_COMMON float gcosh(const float &);
/** @brief Calculates the cosh value of a double value */
G_API_COMMON double gcosh(const double &);

/** @brief Calculates the ceil value of a float value */
G_API_COMMON float gceil(const float &);
/** @brief Calculates the ceil value of a double value */
G_API_COMMON double gceil(const double &);

/** @brief Calculates the maximum value of two float values */
G_API_COMMON float gmax(const float &, const float &);
/** @brief Calculates the maximum value of two double values */
G_API_COMMON double gmax(const double &, const double &);

/** @brief Calculates the minimum value of two float values */
G_API_COMMON float gmin(const float &, const float &);
/** @brief Calculates the minimum value of two double values */
G_API_COMMON double gmin(const double &, const double &);

/** @brief Calculates the floor value of a float value */
G_API_COMMON float gfloor(const float &);
/** @brief Calculates the floor value of a double value */
G_API_COMMON double gfloor(const double &);
/** @brief Calculates the floor value of a double value */
G_API_COMMON long double gfloor(const long double &);

/** @brief Calculates the fabs value of a float value */
G_API_COMMON float gfabs(const float &);
/** @brief Calculates the fabs value of a double value */
G_API_COMMON double gfabs(const double &);
/** @brief Calculates the fabs value of a long double value */
G_API_COMMON long double gfabs(const long double &);

/** @brief Calculates the abs value of an int value */
G_API_COMMON int giabs(const int &);
/** @brief Calculates the abs value of a long int value */
G_API_COMMON long giabs(const long &);

/** @brief Calculates the sqrt value of a float value */
G_API_COMMON float gsqrt(const float &);
/** @brief Calculates the sqrt value of a double value */
G_API_COMMON double gsqrt(const double &);

/** @brief Calculates the sin value of a float value */
G_API_COMMON float gsin(const float &);
/** @brief Calculates the sin value of a double value */
G_API_COMMON double gsin(const double &);

/** @brief Calculates the cos value of a float value */
G_API_COMMON float gcos(const float &);
/** @brief Calculates the cos value of a double value */
G_API_COMMON double gcos(const double &);

/** @brief Calculates the log value of a float value */
G_API_COMMON float glog(const float &);
/** @brief Calculates the log value of a double value */
G_API_COMMON double glog(const double &);

/** @brief Calculates the log10 value of a float value */
G_API_COMMON float glog10(const float &);
/** @brief Calculates the log10 value of a double value */
G_API_COMMON double glog10(const double &);

/** @brief Calculates the pow value of a float value */
G_API_COMMON float gpow(const float &, const float &);
/** @brief Calculates the pow value of a double value */
G_API_COMMON double gpow(const double &, const double &);

/** @brief Calculates the hypot value of a float value */
G_API_COMMON float ghypot(const float &, const float &);
/** @brief Calculates the hypot value of a double value */
G_API_COMMON double ghypot(const double &, const double &);

/** @brief Performs alpha blending for floats */
G_API_COMMON float gmix(const float &, const float &, const float &);
/** @brief Performs alpha blending for doubles */
G_API_COMMON double gmix(const double &, const double &, const double &);

/** @brief Calculates the exp value of a float value */
G_API_COMMON float gexp(const float &);
/** @brief Calculates the exp value of a double value */
G_API_COMMON double gexp(const double &);

/** @brief Calculates the sign value of a float value */
G_API_COMMON float gsign(const float &);
/** @brief Calculates the sign value of a double value */
G_API_COMMON double gsign(const double &);

/** @brief A sigmoid function with user-defined minimum / maximum values (float version) */
G_API_COMMON float gsigmoid(const float &, const float &, const float &);
/** @brief A sigmoid function with user-defined minimum / maximum values (double version) */
G_API_COMMON double gsigmoid(const double &, const double &, const double &);

} /* namespace Common */
} /* namespace Gem */


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/9/problem9.hpp"

BOOST_AUTO_TEST_SUITE( Problem9 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem9::solve(12);
        BOOST_CHECK_EQUAL(res, 60);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem9::solve();
        BOOST_CHECK_EQUAL(res, 31875000);
    }

BOOST_AUTO_TEST_SUITE_END()


#ifndef MATRIX_DEFINITIONS_H_
#define MATRIX_DEFINITIONS_H_

#include <Eigen/Core>

using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using DiagonalMatrix = Eigen::DiagonalMatrix<double, Eigen::Dynamic>;

#endif


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sinc.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/mindenormal.hpp>

STF_CASE_TPL(" sinc",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sinc;

  STF_EXPR_IS(sinc(T()),T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sinc(bs::Inf<T>()), bs::Zero<T>(), 0.5);
  STF_ULP_EQUAL(sinc(bs::Minf<T>()), bs::Zero<T>(), 0.5);
  STF_ULP_EQUAL(sinc(bs::Nan<T>()), bs::Nan<T>(), 0.5);
#endif
  STF_ULP_EQUAL(sinc(-bs::Pio_2<T>()), T(2)/(bs::Pi<T>()), 0.5);
  STF_ULP_EQUAL(sinc(-bs::Pio_4<T>()), bs::sin(bs::Pio_4<T>())/(bs::Pio_4<T>()), 0.5);
  STF_ULP_EQUAL(sinc(bs::Pio_2<T>()),  T(2)/(bs::Pi<T>()), 0.5);
  STF_ULP_EQUAL(sinc(bs::Pio_4<T>()), bs::sin(bs::Pio_4<T>())/(bs::Pio_4<T>()), 0.5);
  STF_ULP_EQUAL(sinc(bs::Eps<T>()), bs::One<T>(), 0.5);
  STF_ULP_EQUAL(sinc(bs::Mindenormal<T>()), bs::One<T>(), 0.5);
  STF_ULP_EQUAL(sinc(bs::Zero<T>()), bs::One<T>(), 0.5);
}


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

BZ_USING_NAMESPACE(blitz)

int main()
{
    Array<double,1> F(4);

    F = -1.0, -2.0, -3.0, -4.0;

    TinyVector<int,1> i = maxIndex(F);
    double f = max(F);

    BZTEST(i[0] == 0);
    BZTEST(f == -1.0);
    return 0;
}


// File       : gemm.cpp
// Created    : Tue Oct 16 2018 10:45:45 AM (+0200)
// Description: General Matrix-Matrix multiplication with ISPC
// Copyright 2018 ETH Zurich. All Rights Reserved.
#include <cassert>
#include <iostream>
#include <chrono>
#include <string>
#include <random>
#include <cassert>
#include <cstring>
#include <cmath>
using namespace std;

#include "common.h"
#ifdef _USE_ISPC_
#include "gemm_sse2.h" // kernel signature for the ISPC SSE2 code
#include "gemm_avx2.h" // kernel signature for the ISPC AVX2 code
using namespace ispc;
#endif /* _USE_ISPC_ */

#ifdef _WITH_EIGEN_
#include <Eigen/Core>

template <typename T>
using EMat = Eigen::Map< Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> >;

// compiled in separate compilation unit with optimization flags tuned for
// Haswell architecture, see the gemm_eigen.o target in the Makefile
void gemm_eigen(const EMat<Real>&, const EMat<Real>&, EMat<Real>&);
#endif /* _WITH_EIGEN_ */

/**
 * @brief General matrix-matrix multiplication kernel (GEMM). Computes C = AB.
 * Naive implementation.
 *
 * @tparam T Real type parameter
 * @param A Matrix dimension p x r
 * @param B Matrix dimension r x q
 * @param C Matrix dimension p x q
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 */
template <typename T>
static void gemm_serial(const T* const A, const T* const B, T* const C,
        const int p, const int r, const int q)
{
    for (int i = 0; i < p; ++i)
        for (int j = 0; j < q; ++j)
        {
            T sum = 0.0;
            for (int k = 0; k < r; ++k)
                sum += A[i*r + k] * B[k*q + j]; // Note: The access pattern
                                                // into matrix B is bad!
            C[i*q + j] = sum;
        }
}

/**
 * @brief General matrix-matrix multiplication kernel (GEMM). Computes C = AB.
 * Implementation with optimized memory access.
 *
 * @tparam T Real type parameter
 * @param A Matrix dimension p x r
 * @param B Matrix dimension r x q
 * @param C Matrix dimension p x q
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 */
template <typename T>
static void gemm_serial_tile(const T* const A, const T* const B, T* const C,
        const int p, const int r, const int q)
{
    assert(q%_HTILE_ == 0);
    assert(p%_VTILE_ == 0);
    T tile[_VTILE_][_HTILE_]; // tmp storage to exploit temporal locality.
                              // This must fit into L1 cache, otherwise
                              // performance will degrade

    // outer loops over dimension of matrix C.  Note the necessary stride on
    // the loop counters because of our cached data structure tile (above).
    for (int i = 0; i < p; i += _VTILE_)
        for (int j = 0; j < q; j += _HTILE_)
        {
            // initialize tile to zero
            for (int tv = 0; tv < _VTILE_; ++tv)
                for (int th = 0; th < _HTILE_; ++th)
                    tile[tv][th] = (T)0.0;

            // start of inner product
            for (int k = 0; k < r; ++k)
            {
                for (int tv = 0; tv < _VTILE_; ++tv)
                {
                    const T Aik = A[(i+tv)*r + k];
                    for (int th = 0; th < _HTILE_; ++th)
                        tile[tv][th] += Aik * B[k*q + j + th]; // Optimized memory access
                                                               // for matrix B!
                }
            }

            for (int tv = 0; tv < _VTILE_; ++tv)
                for (int th = 0; th < _HTILE_; ++th)
                    C[(i+tv)*q + j + th] = tile[tv][th]; // Optimized writes
                                                         // into C also
        }
}

/**
 * @brief Compute the Frobenius norm between the difference of two input
 * matrices
 *
 * @tparam T Real type parameter
 * @param M Test matrix dimension p x q
 * @param truth Reference matrix dimension p x q
 * @param p Dimensional parameter
 * @param q Dimensional parameter
 *
 * @return
 */
template <typename T>
static T validate(const T* const M, const T* const truth,
        const int p, const int q)
{
    T res = 0.0;
    for (int i = 0; i < p; ++i)
        for (int j = 0; j < q; ++j)
        {
            const T diff = M[i*q + j] - truth[i*q + j];
            res += diff * diff;
        }
    return std::sqrt(res/(p*q));
}

/**
 * @brief Initialize matrices A and B to uniform random values
 *
 * @tparam T Real type parameter
 * @param A Matrix dimension p x r
 * @param B Matrix dimension r x q
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 * @param seed Used for random number generator
 */
template <typename T>
static void initialize(T* const A, T* const B,
        const int p, const int r, const int q, const int seed=101)
{
    default_random_engine gen(seed);
    uniform_real_distribution<T> dist(0.0, 1.0);
    for (int i = 0; i < p; ++i)
        for (int k = 0; k < r; ++k)
            A[i*r + k] = dist(gen);

    for (int k = 0; k < r; ++k)
        for (int j = 0; j < q; ++j)
            B[k*q + j] = dist(gen);
}

/**
 * @brief Benchmark a test kernel versus a baseline kernel
 *
 * @tparam T Real type parameter
 * @tparam MEMORY_OPTIMIZED Flag to switch between naive and memory optimized
 * base kernel
 * @tparam WITH_EIGEN Flag to enable Eigen kernel for benchmarking
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 * @param func Function pointer to test kernel
 * @param test_name String to describe additional output
 */
template <typename T, bool MEMORY_OPTIMIZED=false, bool WITH_EIGEN=false>
void benchmark(const int p, const int r, const int q,
        void (*func)(const T* const, const T* const, T* const, const int, const int, const int),
        const string test_name)
{
    T *A, *B, *C, *truth;
    posix_memalign((void**)&A, 32, p*r*sizeof(T));
    posix_memalign((void**)&B, 32, r*q*sizeof(T));
    posix_memalign((void**)&C, 32, p*q*sizeof(T));
    posix_memalign((void**)&truth, 32, p*q*sizeof(T));
    typedef chrono::steady_clock Clock;

    // initialize random matrices
    initialize(A, B, p, r, q);
    memset(C, 0, p*q*sizeof(T));
    memset(truth, 0, p*q*sizeof(T));

    // compute truth
    auto t1 = Clock::now();
    if (WITH_EIGEN)
    {
        (*func)(A, B, truth, p, r, q);
    }
    else
    {
        if (MEMORY_OPTIMIZED)
            gemm_serial_tile(A, B, truth, p, r, q);
        else
            gemm_serial(A, B, truth, p, r, q);
    }
    auto t2 = Clock::now();
    const double t_gold = chrono::duration_cast<chrono::nanoseconds>(t2 - t1).count();
    const T norm_truth = validate(truth, C, p, q);

    // test kernel
#ifdef _WITH_EIGEN_
    // Matrix wrappers
    const EMat<T> Ap(A, p, r);
    const EMat<T> Bp(B, r, q);
    EMat<T> Cp(C, p, q);
#endif /* _WITH_EIGEN_ */

    auto tt1 = Clock::now();
#ifdef _WITH_EIGEN_
    if (WITH_EIGEN)
        gemm_eigen(Ap, Bp, Cp);
    else
#endif /* _WITH_EIGEN_ */
        (*func)(A, B, C, p, r, q);
    auto tt2 = Clock::now();
    const double t = chrono::duration_cast<chrono::nanoseconds>(tt2 - tt1).count();

    // validate
    const T diff = validate(C, truth, p, q);

    cout << test_name << ":" << endl;
    cout << "  Data type size:     " << sizeof(T) << " byte" << endl;
    cout << "  Number of elements: A=" << p*r << "; B=" << r*q << "; C=" << p*q << endl;
    cout << "  Norm of truth:      " << norm_truth << endl;
    cout << "  Error:              " << diff << endl;
    cout << "  Time reference:     " << t_gold*1.0e-6 << " millisec" << endl;
    cout << "  Time test kernel:   " << t*1.0e-6 << " millisec" << endl;
    cout << "  Speedup:            " << t_gold/t << endl;

    // clean up
    free(A);
    free(B);
    free(C);
    free(truth);
}

int main(void)
{
    // problem size:
    // Matrix $A \in\mathbb{R}^{ p \times r }$
    // Matrix $B \in\mathbb{R}^{ r \times q }$
    // Matrix $C \in\mathbb{R}^{ p \times q }$
    //
    // We solve C = A * B
    constexpr int p = 512;
    constexpr int r = 1024;
    constexpr int q = 1024;

    // benchmark serial case naive vs. memory optimized
    benchmark<Real>(p, r, q, gemm_serial_tile, "GEMM serial access optimized");

#ifdef _USE_ISPC_
    // to be fair, we must use our best serial implementation to benchmark
    // against the vectorized implementations --> use memory optimized version
    // of serial GEMM
    constexpr bool MEMORY_OPTIMIZED = true;
    benchmark<Real,MEMORY_OPTIMIZED>(p, r, q, gemm_sse2, "GEMM ISPC SSE2");
    benchmark<Real,MEMORY_OPTIMIZED>(p, r, q, gemm_avx2, "GEMM ISPC AVX2");
#endif /* _USE_ISPC_ */

#ifdef _WITH_EIGEN_
    // benchmark against the Eigen library
    constexpr bool USE_EIGEN = true;
    benchmark<Real,true,USE_EIGEN>(p, r, q, gemm_avx2, "GEMM Eigen vs. ISPC AVX2");
#endif /* _WITH_EIGEN_ */

    return 0;
}


#include <boost/math/complex/asin.hpp>


//
// Copyright (c) 2018-2020 CNRS INRIA
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/algorithm/regressor.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

//  const std::string joint_name = "larm5_joint";
//  const JointIndex joint_id = model.getJointId(joint_name);

  const VectorXd q = randomConfiguration(model);

  forwardKinematics(model,data,q);

  const double eps = 1e-8;
  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
  {
    Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));

    Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));

    computeJointKinematicRegressor(model, data, joint_id, LOCAL, kinematic_regressor_L);
    computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA);
    computeJointKinematicRegressor(model, data, joint_id, WORLD, kinematic_regressor_W);

    Model model_plus = model; Data data_plus(model_plus);
    const SE3 & oMi = data.oMi[joint_id];
    const SE3 Mi_LWA = SE3(oMi.rotation(),SE3::Vector3::Zero());
    const SE3 & oMi_plus = data_plus.oMi[joint_id];
    for(int i = 1; i < model.njoints; ++i)
    {
      Motion::Vector6 v = Motion::Vector6::Zero();
      const SE3 & M_placement = model.jointPlacements[(JointIndex)i];
      SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i];
      for(Eigen::DenseIndex k = 0; k < 6; ++k)
      {
        v[k] = eps;
        M_placement_plus = M_placement * exp6(Motion(v));

        forwardKinematics(model_plus,data_plus,q);

        const Motion diff_L = log6(oMi.actInv(oMi_plus));
        kinematic_regressor_L_fd.middleCols<6>(6*(i-1)).col(k) = diff_L.toVector()/eps;
        const Motion diff_LWA = Mi_LWA.act(diff_L);
        kinematic_regressor_LWA_fd.middleCols<6>(6*(i-1)).col(k) = diff_LWA.toVector()/eps;
        const Motion diff_W = oMi.act(diff_L);
        kinematic_regressor_W_fd.middleCols<6>(6*(i-1)).col(k) = diff_W.toVector()/eps;
        v[k] = 0.;
      }

      M_placement_plus = M_placement;
    }

    BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd,sqrt(eps)));
    BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd,sqrt(eps)));
    BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd,sqrt(eps)));
  }
}

BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint_placement)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  const VectorXd q = randomConfiguration(model);

  forwardKinematics(model,data,q);
  forwardKinematics(model,data_ref,q);

  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
  {
    Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));

    computeJointKinematicRegressor(model, data, joint_id, LOCAL, SE3::Identity(), kinematic_regressor_L);
    computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, SE3::Identity(), kinematic_regressor_LWA);
    computeJointKinematicRegressor(model, data, joint_id, WORLD, SE3::Identity(), kinematic_regressor_W);

    Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));

    computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL, kinematic_regressor_L_ref);
    computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA_ref);
    computeJointKinematicRegressor(model, data_ref, joint_id, WORLD, kinematic_regressor_W_ref);

    BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref));
    BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref));
    BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref));
  }
}

BOOST_AUTO_TEST_CASE(test_kinematic_regressor_frame)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  const std::string joint_name = "larm5_joint";
  const JointIndex joint_id = model.getJointId(joint_name);
  model.addBodyFrame("test_body", joint_id, SE3::Random(), -1);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  const VectorXd q = randomConfiguration(model);

  forwardKinematics(model,data,q);
  updateFramePlacements(model,data);
  forwardKinematics(model,data_ref,q);

  const double eps = 1e-8;
  for(FrameIndex frame_id = 1; frame_id < (FrameIndex)model.nframes; ++frame_id)
  {
    const Frame & frame = model.frames[frame_id];

    Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));

    computeFrameKinematicRegressor(model, data, frame_id, LOCAL, kinematic_regressor_L);
    computeFrameKinematicRegressor(model, data, frame_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA);
    computeFrameKinematicRegressor(model, data, frame_id, WORLD, kinematic_regressor_W);

    Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));

    computeJointKinematicRegressor(model, data_ref, frame.parent, LOCAL, frame.placement, kinematic_regressor_L_ref);
    computeJointKinematicRegressor(model, data_ref, frame.parent, LOCAL_WORLD_ALIGNED, frame.placement, kinematic_regressor_LWA_ref);
    computeJointKinematicRegressor(model, data_ref, frame.parent, WORLD, frame.placement, kinematic_regressor_W_ref);

    BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref));
    BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref));
    BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref));

    Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
    Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));

    Model model_plus = model; Data data_plus(model_plus);
    const SE3 & oMf = data.oMf[frame_id];
    const SE3 Mf_LWA = SE3(oMf.rotation(),SE3::Vector3::Zero());
    const SE3 & oMf_plus = data_plus.oMf[frame_id];
    for(int i = 1; i < model.njoints; ++i)
    {
      Motion::Vector6 v = Motion::Vector6::Zero();
      const SE3 & M_placement = model.jointPlacements[(JointIndex)i];
      SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i];
      for(Eigen::DenseIndex k = 0; k < 6; ++k)
      {
        v[k] = eps;
        M_placement_plus = M_placement * exp6(Motion(v));

        forwardKinematics(model_plus,data_plus,q);
        updateFramePlacements(model_plus,data_plus);

        const Motion diff_L = log6(oMf.actInv(oMf_plus));
        kinematic_regressor_L_fd.middleCols<6>(6*(i-1)).col(k) = diff_L.toVector()/eps;
        const Motion diff_LWA = Mf_LWA.act(diff_L);
        kinematic_regressor_LWA_fd.middleCols<6>(6*(i-1)).col(k) = diff_LWA.toVector()/eps;
        const Motion diff_W = oMf.act(diff_L);
        kinematic_regressor_W_fd.middleCols<6>(6*(i-1)).col(k) = diff_W.toVector()/eps;
        v[k] = 0.;
      }

      M_placement_plus = M_placement;
    }

    BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd,sqrt(eps)));
    BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd,sqrt(eps)));
    BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd,sqrt(eps)));
  }
}

BOOST_AUTO_TEST_CASE(test_static_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  VectorXd q = randomConfiguration(model);
  computeStaticRegressor(model,data,q);

  VectorXd phi(4*(model.njoints-1));
  for(int k = 1; k < model.njoints; ++k)
  {
    const Inertia & Y = model.inertias[(size_t)k];
    phi.segment<4>(4*(k-1)) << Y.mass(), Y.mass() * Y.lever();
  }

  Vector3d com = centerOfMass(model,data_ref,q);
  Vector3d static_com_ref;
  static_com_ref <<  com;

  Vector3d static_com = data.staticRegressor * phi;

  BOOST_CHECK(static_com.isApprox(static_com_ref));
}

BOOST_AUTO_TEST_CASE(test_body_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  Inertia I(Inertia::Random());
  Motion v(Motion::Random());
  Motion a(Motion::Random());

  Force f = I*a + I.vxiv(v);

  Inertia::Vector6 f_regressor = bodyRegressor(v,a) * I.toDynamicParameters();

  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
}

BOOST_AUTO_TEST_CASE(test_joint_body_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::manipulator(model);
  pinocchio::Data data(model);

  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;

  VectorXd q = randomConfiguration(model);
  VectorXd v = Eigen::VectorXd::Random(model.nv);
  VectorXd a = Eigen::VectorXd::Random(model.nv);

  rnea(model,data,q,v,a);

  Force f = data.f[JOINT_ID];

  Inertia::Vector6 f_regressor = jointBodyRegressor(model,data,JOINT_ID) * model.inertias[JOINT_ID].toDynamicParameters();

  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
}

BOOST_AUTO_TEST_CASE(test_frame_body_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::manipulator(model);

  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;

  const SE3 & framePlacement = SE3::Random();
  FrameIndex FRAME_ID = model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1);

  pinocchio::Data data(model);

  VectorXd q = randomConfiguration(model);
  VectorXd v = Eigen::VectorXd::Random(model.nv);
  VectorXd a = Eigen::VectorXd::Random(model.nv);

  rnea(model,data,q,v,a);

  Force f = framePlacement.actInv(data.f[JOINT_ID]);
  Inertia I = framePlacement.actInv(model.inertias[JOINT_ID]);

  Inertia::Vector6 f_regressor = frameBodyRegressor(model,data,FRAME_ID) * I.toDynamicParameters();

  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
}

BOOST_AUTO_TEST_CASE(test_joint_torque_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = randomConfiguration(model);
  VectorXd v = Eigen::VectorXd::Random(model.nv);
  VectorXd a = Eigen::VectorXd::Random(model.nv);

  rnea(model,data_ref,q,v,a);

  Eigen::VectorXd params(10*(model.njoints-1));
  for(JointIndex i=1; i<(Model::JointIndex)model.njoints; ++i)
      params.segment<10>((int)((i-1)*10)) = model.inertias[i].toDynamicParameters();

  computeJointTorqueRegressor(model,data,q,v,a);

  Eigen::VectorXd tau_regressor = data.jointTorqueRegressor * params;

  BOOST_CHECK(tau_regressor.isApprox(data_ref.tau));
}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>

#include "graph.hpp"
#include "range.hpp"

//a class to hold the coordinates of the straight line embedding
struct coord_t {
  std::size_t x;
  std::size_t y;
};

template<class G>
std::vector<coord_t> straight_line_drawing(G const & gIn) {
  using namespace boost;

  typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_index_t, int>,
      property<edge_index_t, int> > Graph;

  Graph g;
  copy_edges(gIn, g);

  //Define the storage type for the planar embedding
  typedef std::vector<std::vector<graph_traits<Graph>::edge_descriptor>> embedding_storage_t;
  typedef iterator_property_map<embedding_storage_t::iterator,
      property_map<Graph, vertex_index_t>::type> embedding_t;

  // Create the planar embedding
  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index, g));

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                               boyer_myrvold_params::embedding = embedding);

  make_connected(g);
  make_biconnected_planar(g, embedding);
  make_maximal_planar(g, embedding);

  // Find a canonical ordering
  std::vector<typename graph_traits<Graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector<coord_t> drawing_storage_t;
  typedef boost::iterator_property_map<drawing_storage_t::iterator,
      property_map<Graph, vertex_index_t>::type> drawing_t;

  drawing_storage_t drawing_storage(num_vertices(g));
  drawing_t drawing(drawing_storage.begin(), get(vertex_index, g));

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g, embedding, ordering.begin(),
                                      ordering.end(), drawing);

  return drawing_storage;
}

int main(int argc, char** argv) {
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;

  // you can use the functions make_connected, make_biconnected_planar,
  // and make_maximal planar in sequence to add a set of edges
  // to any undirected planar graph to make it maximal planar.

  graph g(7);
  add_edge(0, 1, g);
  add_edge(1, 2, g);
  add_edge(2, 3, g);
  add_edge(3, 0, g);
  //add_edge(3, 4, g);
  add_edge(4, 5, g);
  add_edge(5, 6, g);
  //add_edge(6, 3, g);
  add_edge(0, 4, g);
  add_edge(1, 3, g);
  add_edge(3, 5, g);
  add_edge(2, 6, g);
  add_edge(1, 4, g);
  add_edge(1, 5, g);
  add_edge(1, 6, g);

  auto coords = straight_line_drawing(g);

  std::cout << "The straight line drawing is: " << std::endl;
  for (auto v : ::range(vertices(g))) {
    coord_t coord(coords[v]);
    std::cout << v << " -> (" << coord.x << ", " << coord.y << ")" << std::endl;
  }

  return 0;
}


#include <igl/readOBJ.h>
#include <igl/readMESH.h>
#include <igl/viewer/Viewer.h>
#include <igl/edge_flaps.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <igl/tetrahedron_tetrahedron_adjacency.h>
#include "../src/igl_dev/tetrahedron_tetrahedron_adjacency.h"
#include "../src/igl_dev/tetrahedron_tuple.h"
#include "../src/igl_dev/retain_tetrahedral_adjacency.h"
#include "../src/igl_dev/edge_removal.h"
#include "../src/igl_dev/multi_face_removal.h"
#include "../src/util/tetrahedral_improvement.h"

#include <iostream>
#include <list>
using vecV2d = std::vector<Eigen::RowVector2d>;
//                           Eigen::aligned_allocator<Eigen::RowVector2d>>;
using vecV3d = std::vector<Eigen::RowVector3d>;
using vecV3i = std::vector<Eigen::RowVector3i>;
using vecV4i = std::vector<Eigen::RowVector4i>;
using T_t = vecV4i;
using TT_t = T_t;
using TTie_t = std::vector<Eigen::Matrix<int, 4, 3>>;

// https://github.com/janba/DSC/blob/master/is_mesh/util.h#L415
struct test_utils {
  using vec3 = Eigen::RowVector3d;
  inline static double ms_length(const vec3 &a,
                                 const vec3 &b,
                                 const vec3 &c,
                                 const vec3 &d) {
    double result = 0.;
    result += (a - b).squaredNorm();
    result += (a - c).squaredNorm();
    result += (a - d).squaredNorm();
    result += (b - c).squaredNorm();
    result += (b - d).squaredNorm();
    result += (c - d).squaredNorm();
    return result / 6.;
  }

  inline static double rms_length(const vec3 &a,
                                  const vec3 &b,
                                  const vec3 &c,
                                  const vec3 &d) {
    return sqrt(ms_length(a, b, c, d));
  }

  inline static double signed_volume(const vec3 &a,
                                     const vec3 &b,
                                     const vec3 &c,
                                     const vec3 &d) {
    return (a - d).dot((b - d).cross(c - d)) / 6.;
  }

// https://hal.inria.fr/inria-00518327
  inline static double quality(const vec3 &a,
                               const vec3 &b,
                               const vec3 &c,
                               const vec3 &d) {
    double v = signed_volume(a, b, c, d);
    double lrms = rms_length(a, b, c, d);

    double q = 8.48528 * v / (lrms * lrms * lrms);
#ifdef DEBUG
    assert(!isnan(q));
#endif
    return q;
  }
};

template <int enable=1>
int test_tetrahedron_adjacency() {
  using namespace std;
  using namespace Eigen;

  vecV3d V(9);
  vecV4i T(7);
  V[0] << 0, 0, -1;
  V[1] << 0, 0, 1;
  V[2] << 0, 1, 0;
  V[3] << -1, 0, 0;
  V[4] << -0.8, -0.5, 0;
  V[5] << -0.2, -0.8, 0;
  V[6] << 0.2, -0.8, 0;
  V[7] << 0.8, -0.5, 0;
  V[8] << 1, 0, 0;

  T[0] << 0, 2, 3, 1;
  T[1] << 0, 3, 4, 1;
  T[2] << 0, 4, 5, 1;
  T[3] << 0, 5, 6, 1;
  T[4] << 0, 6, 7, 1;
  T[5] << 0, 7, 8, 1;
  T[6] << 0, 8, 2, 1;

  MatrixXi
      mT = Eigen::Map<Matrix<int, -1, 4, RowMajor>>(T[0].data(), T.size(), 4);
  MatrixXi mTT, mTTif, mTTie;
  vecV4i TT, TTif;
  TTie_t TTie;
  igl::tetrahedron_tetrahedron_adjacency(mT, mTT, mTTif, mTTie);
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);

//  std::cout << mT << std::endl << endl
//            << mTT << endl << endl
//            << mTTie << endl;
//  std::cout << "Here goes our new function" << endl;
//  for (auto r:T) cout << r << endl;
//  for (auto r:TT) cout << r << endl;
//  for (auto r:TTif) cout << r << endl;
//  for (auto r:TTie) cout << r << endl;

  return 0;
}

template <int enable=1>
int test_retain_tetrahedron_adjacency() {
  using namespace Eigen;
  using namespace std;

  // test a 2-3 flip
  vecV4i T(7);
  T[0] << 0, 2, 3, 1;
  T[1] << 0, 3, 4, 1;
  T[2] << 0, 4, 5, 1;
  T[3] << 0, 5, 6, 1;
  T[4] << 0, 6, 7, 1;
  T[5] << 0, 7, 8, 1;
  T[6] << 0, 8, 2, 1;

  auto manual_T = T;
  manual_T.resize(8);
  manual_T[0] << 0, 8, 3, 1;
  manual_T[6] << 8, 2, 3, 1;
  manual_T[7] << 0, 2, 3, 8;
  T_t new_T;
  new_T.push_back(manual_T[0]);
  new_T.push_back(manual_T[6]);
  new_T.push_back(manual_T[7]);

  vecV4i TT, TTif;
  TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);
//  std::cout << "raw: T,TT,TTi:" << endl;
//  for (auto r:T) cout << r << endl;cout<<endl;
//  for (auto r:TT) cout << r << endl;cout<<endl;
//  for (auto r:TTif) cout << r << endl;cout<<endl;
//  for (auto r:TTie) cout << r << endl;cout<<endl;
  vecV4i mTT, mTTif;
  TTie_t mTTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(manual_T, mTT, mTTif, mTTie);
//  std::cout << "manual: T,TT,TTi:" << endl;
//  for (auto r:manual_T) cout << r << endl;cout<<endl;
//  for (auto r:TT) cout << r << endl;cout<<endl;
//  for (auto r:TTif) cout << r << endl;cout<<endl;
//  for (auto r:TTie) cout << r << endl<<endl;cout<<endl;

  std::set<int> delete_id {0,6};
  std::set<int> surround_id {1,5};

  retain_tetrahedral_adjacency(delete_id, surround_id,new_T, T,TT,TTif,TTie);
  for (int i = 0; i < T.size(); i++) {
    assert(mTT[i] == TT[i]);
    assert(mTTif[i] == TTif[i]);
    assert(mTTie[i] == TTie[i]);
  }
//  std::cout << "modified: T,TT,TTi:" << endl;
//  for (auto r:T) cout << r << endl;cout<<endl;
//  for (auto r:TT) cout << r << endl;cout<<endl;
//  for (auto r:TTif) cout << r << endl;cout<<endl;
//  for (auto r:TTie) cout << r << endl<<endl;cout<<endl;

  return 0;
}

template <int enable=1>
int test_retain_fix_end() {

  using namespace Eigen;
  using namespace std;
  using vecV2d = std::vector<Eigen::RowVector2d>;
  using vecV3d = std::vector<Eigen::RowVector3d>;
  using vecV3i = std::vector<Eigen::RowVector3i>;
  using vecV4i = std::vector<Eigen::RowVector4i>;
  using T_t = vecV4i;
  using TT_t = T_t;
  using TTie_t = std::vector<Eigen::Matrix<int, 4, 3>>;

  vecV3d V(9);
  vecV4i T(7);
  V[0] << 0, -0, 1;
  V[1] << 0, -0, -1;
  V[2] << 0, 1, 0;
  V[3] << -1, 0, 0;
  V[4] << -0.8, -0.5, 0;
  V[5] << -0.2, -0.8, 0;
  V[6] << 0.2, -0.8, 0;
  V[7] << 0.8, -0.5, 0;
  V[8] << 1, 0, 0;

  T[0] << 1, 2, 3, 0;
  T[1] << 1, 3, 4, 0;
  T[2] << 1, 4, 5, 0;
  T[3] << 1, 5, 6, 0;
  T[4] << 1, 6, 7, 0;
  T[5] << 1, 7, 8, 0;
  T[6] << 1, 8, 2, 0;

  std::set<int> delete_id, influence_id;
  delete_id.insert(0);
  delete_id.insert(1);
  delete_id.insert(3);
  delete_id.insert(5);

  vecV4i TT, TTif;
  TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);

  for (auto t:delete_id)
    for (auto f:{0, 1, 2, 3})
      influence_id.insert(TT[t][f]);
  influence_id.erase(-1);

  // retain_connectivity.
  std::set<int> surround_id;
  std::set_difference(influence_id.begin(), influence_id.end(),
                      delete_id.begin(), delete_id.end(),
                      std::inserter(surround_id, surround_id.end()));

  vecV4i new_tets(2);
//  new_tets[2] << 1, 2, 3, 0;
  new_tets[0] << 1, 3, 4, 0;
  new_tets[1] << 1, 7, 8, 0;

  retain_tetrahedral_adjacency(delete_id, surround_id,new_tets, T,TT,TTif,TTie);
  return 0;
}

template <int enable=1>
int test_extraction_recurse()
{
  using namespace Eigen;
  using namespace std;

  static MatrixXi K_table(4,6);
  K_table << 0,0,2,3,4,4,
  0,0,0,3,4,3,
  0,0,0,0,4,4,
  0,0,0,0,0,5;
  K_table -= MatrixXi::Ones(4,6);

  static std::vector<Eigen::RowVector3i> new_tri;
  struct recurse_extract {
    static void op(int i,int j){
      if(j>= i+2) {
        int k=K_table(i,j);
        recurse_extract::op(i,k);
        recurse_extract::op(k,j);
        new_tri.emplace_back(i,j,k);
      }
    };
  };
  recurse_extract::op(0,5);
//  std::cout<<K_table<<std::endl;
//  for(auto r:new_tri) cout<<r<<endl;
  return 0;
}

template <int enable=1>
int test_single_edge_removal() {
  using namespace Eigen;
  using namespace std;

  vecV3d V(9);
  vecV4i T(7);
  V[0] << 0, 0, 1;
  V[1] << 0, 0, -1;
  V[2] << 0, 10, 0;
  V[3] << -1, 0, 0;
  V[4] << -0.8, -0.5, 0;
  V[5] << -0.2, -0.8, 0;
  V[6] << 0.2, -0.8, 0;
  V[7] << 0.8, -0.5, 0;
  V[8] << 1, 0, 0;

  T[0] << 0, 2, 3, 1;
  T[1] << 0, 3, 4, 1;
  T[2] << 0, 4, 5, 1;
  T[3] << 0, 5, 6, 1;
  T[4] << 0, 6, 7, 1;
  T[5] << 0, 7, 8, 1;
  T[6] << 0, 8, 2, 1;

  vecV4i TT, TTif; TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);

  auto tet_quality = [&V](int a, int b, int c, int d) {
    return test_utils::quality(V[a],V[b],V[c],V[d]);
  };

  igl::dev::tet_tuple_edge_removal(0,
                                   1,
                                   1,
                                   true,
                                   tet_quality,
                                   T,
                                   TT,
                                   TTif,
                                   TTie );
//  for (auto r:T) cout << r << endl;cout<<endl;
  decltype(TT) nTT;
  decltype(TTif) nTTif;
  decltype(TTie) nTTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, nTT, nTTif, nTTie);
  for (int i = 0; i < T.size(); i++) {
    assert(nTT[i] == TT[i]);
    assert(nTTif[i] == TTif[i]);
    assert(nTTie[i] == TTie[i]);
  }
  return 0;
}

template <int enable=1>
int test_simple_face_removal() {

  using namespace std;
  using namespace Eigen;

  std::list<std::tuple<int, int>> dt;
  dt.emplace_back(8, 0);
  std::vector<std::tuple<int, int>> vdt(dt.begin(), dt.end());
  using vecV2d = std::vector<Eigen::RowVector2d>;
//                           Eigen::aligned_allocator<Eigen::RowVector2d>>;
  using vecV3d = std::vector<Eigen::RowVector3d>;
  using vecV3i = std::vector<Eigen::RowVector3i>;
  using vecV4i = std::vector<Eigen::RowVector4i>;
  using T_t = vecV4i;
  using TT_t = T_t;
  using TTie_t = std::vector<Eigen::Matrix<int, 4, 3>>;

  vecV3d V(9);
  vecV4i T(10);
  V[0] << 0.7, -0.1, 1;
  V[1] << 0.7, -0.1, -1;
  V[2] << 0, 1, 0;
  V[3] << -1, 0, 0;
  V[4] << -0.8, -0.5, 0;
  V[5] << -0.2, -0.8, 0;
  V[6] << 0.2, -0.8, 0;
  V[7] << 0.8, -0.5, 0;
  V[8] << 1, 0, 0;

  T[0] << 1, 3, 5, 4;
  T[1] << 3, 5, 4, 0;
  T[2] << 1, 3, 6, 5;
  T[3] << 3, 6, 5, 0;
  T[4] << 1, 3, 7, 6;
  T[5] << 3, 7, 6, 0;
  T[6] << 1, 3, 8, 7;
  T[7] << 3, 8, 7, 0;
  T[8] << 1, 3, 2, 8;
  T[9] << 3, 2, 8, 0;

  vecV4i TT, TTif;
  TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);

  auto tet_quality = [&V](int a, int b, int c, int d) -> double {
    return -test_utils::quality(V[a], V[b], V[c], V[d]);
  };
  auto correct_orientation = [&V](int a, int b, int c, int d) -> bool {
    return -test_utils::signed_volume(V[a], V[b], V[c], V[d]) > 0;
  };
  std::vector<int> dummy;
  igl::dev::tet_tuple_multi_face_removal(7,
                                         3,
                                         0,
                                         true,
                                         tet_quality,
                                         correct_orientation,
                                         T,
                                         TT,
                                         TTif,
                                         TTie,
                                         dummy);
  decltype(TT) nTT;
  decltype(TTif) nTTif;
  decltype(TTie) nTTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, nTT, nTTif, nTTie);
  for (int i = 0; i < T.size(); i++) {
    assert(nTT[i] == TT[i]);
    assert(nTTif[i] == TTif[i]);
    assert(nTTie[i] == TTie[i]);
  }
  return 0;
}

// topological improvement
template <int enable=1>
int test_edge_removal_pass() {

  using namespace std;
  using namespace Eigen;

  std::list<std::tuple<int, int>> dt;
  dt.emplace_back(8, 0);
  std::vector<std::tuple<int, int>> vdt(dt.begin(), dt.end());
  using vecV2d = std::vector<Eigen::RowVector2d>;
//                           Eigen::aligned_allocator<Eigen::RowVector2d>>;
  using vecV3d = std::vector<Eigen::RowVector3d>;
  using vecV3i = std::vector<Eigen::RowVector3i>;
  using vecV4i = std::vector<Eigen::RowVector4i>;
  using T_t = vecV4i;
  using TT_t = T_t;
  using TTie_t = std::vector<Eigen::Matrix<int, 4, 3>>;

  vecV3d V(9);
  vecV4i T(7);
  V[0] << 0, -0, 10;
  V[1] << 0, -0, -1;
  V[2] << 0, 1, 0;
  V[3] << -1, 0, 0;
  V[4] << -0.8, -0.5, 0;
  V[5] << -0.2, -0.8, 0;
  V[6] << 0.2, -0.8, 0;
  V[7] << 0.8, -0.5, 0;
  V[8] << 1, 0, 0;

  T[0] << 0, 2, 3, 1;
  T[1] << 0, 3, 4, 1;
  T[2] << 0, 4, 5, 1;
  T[3] << 0, 5, 6, 1;
  T[4] << 0, 6, 7, 1;
  T[5] << 0, 7, 8, 1;
  T[6] << 0, 8, 2, 1;

  vecV4i TT, TTif;
  TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);

  auto tet_quality = [&V](int a, int b, int c, int d) -> double {
    return test_utils::quality(V[a], V[b], V[c], V[d]);
  };
  auto correct_orientation = [&V](int a, int b, int c, int d) -> bool {
    return test_utils::signed_volume(V[a], V[b], V[c], V[d]) > 0;
  };

//  for(auto r:T) cout<<r<<":"<<tet_quality(r(0),r(1),r(2),r(3)) <<endl;
//  cout<<endl;
  edge_removal_pass(tet_quality, correct_orientation, T,TT,TTif,TTie);

//  for(auto r:T) cout<<r<<":"<<tet_quality(r(0),r(1),r(2),r(3)) <<endl;
}

// topological improvement
template <int enable=1>
int test_tetmesh_topology_improvement() {

// https://github.com/janba/DSC/blob/master/is_mesh/util.h#L415
  struct test_utils {
    using vec3 = Eigen::RowVector3d;
    inline static double ms_length(const vec3 &a,
                                   const vec3 &b,
                                   const vec3 &c,
                                   const vec3 &d) {
      double result = 0.;
      result += (a - b).squaredNorm();
      result += (a - c).squaredNorm();
      result += (a - d).squaredNorm();
      result += (b - c).squaredNorm();
      result += (b - d).squaredNorm();
      result += (c - d).squaredNorm();
      return result / 6.;
    }

    inline static double rms_length(const vec3 &a,
                                    const vec3 &b,
                                    const vec3 &c,
                                    const vec3 &d) {
      return sqrt(ms_length(a, b, c, d));
    }

    inline static double signed_volume(const vec3 &a,
                                       const vec3 &b,
                                       const vec3 &c,
                                       const vec3 &d) {
      return (a - d).dot((b - d).cross(c - d)) / 6.;
    }

// https://hal.inria.fr/inria-00518327
    inline static double quality(const vec3 &a,
                                 const vec3 &b,
                                 const vec3 &c,
                                 const vec3 &d) {
      double v = signed_volume(a, b, c, d);
      double lrms = rms_length(a, b, c, d);

      double q = 8.48528 * v / (lrms * lrms * lrms);
#ifdef DEBUG
      assert(!isnan(q));
#endif
      return q;
    }
  };

  using namespace std;
  using namespace Eigen;

  std::list<std::tuple<int, int>> dt;
  dt.emplace_back(8, 0);
  std::vector<std::tuple<int, int>> vdt(dt.begin(), dt.end());
  using vecV2d = std::vector<Eigen::RowVector2d>;
//                           Eigen::aligned_allocator<Eigen::RowVector2d>>;
  using vecV3d = std::vector<Eigen::RowVector3d>;
  using vecV3i = std::vector<Eigen::RowVector3i>;
  using vecV4i = std::vector<Eigen::RowVector4i>;
  using T_t = vecV4i;
  using TT_t = T_t;
  using TTie_t = std::vector<Eigen::Matrix<int, 4, 3>>;

  MatrixXd TV; MatrixXi TTo,TF;
  igl::readMESH("../models/bumpy.mesh",TV,TTo,TF);

  vecV3d V(TV.rows());
  vecV4i T(TTo.rows());

  for(int i=0; i< V.size(); i++) V[i]<<TV.row(i);
  for(int i=0; i< T.size(); i++) T[i]<<TTo.row(i);

  auto tet_quality = [&V](int a, int b, int c, int d) -> double {
    return -test_utils::quality(V[a], V[b], V[c], V[d]);
  };
  auto correct_orientation = [&V](int a, int b, int c, int d) -> bool {
    return -test_utils::signed_volume(V[a], V[b], V[c], V[d]) > 0;
  };

  vecV4i TT, TTif;
  TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);
//  {double old_q= INFINITY;
//    for(auto r:T) old_q = std::min(old_q, tet_quality(r(0),r(1),r(2),r(3)));
//    cout<<old_q<<endl;}
  // let's try to perturb this.
  int perturb = 0;
  for(auto t=0; t<T.size(); t++)
    for (auto f:{0,1,2,3})
      for(auto e:{0,1,2}) {
        if (igl::dev::tet_tuple_edge_removal_force(t,
                                                   f,
                                                   e,
                                                   true,
                                                   tet_quality,
//                                                         correct_orientation,
                                                   T,
                                                   TT,
                                                   TTif,
                                                   TTie)) {
          perturb++; //t+=10;
          if(perturb == 3)
            goto out;
          break;
        }
      }
out: ;
  /*{
    MatrixXd iV(8,3);
    MatrixXi F(8,3);
    int x=0;
    for (auto t:{0,1398}) {
      for(auto v:{0,1,2,3})
        iV.row(x++) = V[T[t](v)];
    }
    F<< 3, 2, 1,
        2, 3, 0,
        1, 0, 3,
        0, 1, 2,
        7, 6, 5,
        6, 7, 4,
        5, 4, 7,
        4, 5, 6;
    igl::viewer::Viewer vvvv;
    vvvv.data.set_mesh(iV, F);
    igl::writeOBJ("temp.obj",iV,F);
    vvvv.launch();
  }*/

  double old_q = INFINITY, new_q= INFINITY;
  for(auto r:T) old_q = std::min(old_q, tet_quality(r(0),r(1),r(2),r(3)));
//  cout<<old_q<<endl;
  face_removal_pass(tet_quality, correct_orientation, T,TT,TTif,TTie);
  for(auto r:T) new_q = std::min(new_q, tet_quality(r(0),r(1),r(2),r(3)));
//  cout<<new_q<<endl;
}

template <int enable=1>
int test_bumpy_vert_tet_query() {
  using namespace std;
  using namespace Eigen;

  MatrixXd V, Vall; MatrixXi F;
  MatrixXd TV;
  MatrixXi T,TF;

  igl::readMESH("../models/bumpy.mesh", TV,T,TF);

  std::vector<RowVector4i> vT(T.rows()),vTT,vTTif;
  std::vector<Matrix<int,4,3>> vTTie;
  for(int i=0; i<T.rows(); i++) vT[i] = T.row(i);

  igl::dev::tetrahedron_tetrahedron_adjacency(vT,vTT,vTTif,vTTie);

  for(int ti = 0; ti<T.rows(); ti++) {
    for (int fi:{0, 1, 2, 3}) {
      int ei = 0;

      int query_v =
          igl::dev::tet_tuple_get_vert(ti, fi, ei, true, vT, vTT, vTTif, vTTie);

      auto get_neighbor =
          igl::dev::tet_tuple_get_tets_with_vert(ti, fi, ei, true,
                                                 vT, vTT, vTTif, vTTie);

      std::set<int> test_neighbor;
      for (int i = 0; i < T.rows(); i++) {
        for (int j:{0, 1, 2, 3})
          if (T(i, j) == query_v)
            test_neighbor.insert(i);
      }

      assert(get_neighbor == test_neighbor);
    }
  }
  return 0;
}

#include "../src/igl_dev/tet_refine_operations.h"
template <int enable=1>
int test_edge_split()
{
  using namespace Eigen;
  using namespace std;

  using vecV2d = std::vector<Eigen::RowVector2d>;
  using vecV3d = std::vector<Eigen::RowVector3d>;
  using vecV3i = std::vector<Eigen::RowVector3i>;
  using vecV4i = std::vector<Eigen::RowVector4i>;
  using T_t = vecV4i;
  using TT_t = T_t;
  using TTie_t = std::vector<Eigen::Matrix<int, 4, 3>>;

  vecV3d V(9);
  vecV4i T(7);
  V[0] << 0, -0, 1;
  V[1] << 0, -0, -1;
  V[2] << 0, 1, 0;
  V[3] << -1, 0, 0;
  V[4] << -0.8, -0.5, 0;
  V[5] << -0.2, -0.8, 0;
  V[6] << 0.2, -0.8, 0;
  V[7] << 0.8, -0.5, 0;
  V[8] << 1, 0, 0;

  T[0] << 1, 2, 3, 0;
  T[1] << 1, 3, 4, 0;
  T[2] << 1, 4, 5, 0;
  T[3] << 1, 5, 6, 0;
  T[4] << 1, 6, 7, 0;
  T[5] << 1, 7, 8, 0;
  T[6] << 1, 8, 2, 0;

  auto tet_quality = [&V](int a, int b, int c, int d) -> double {
    return -test_utils::quality(V[a], V[b], V[c], V[d]);
  };
//  for(auto r:T) cout<<r<<":"<<tet_quality(r(0),r(1),r(2),r(3)) <<endl;

  vecV4i TT, TTif;
  TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);

//  cout<< igl::dev::tet_tuple_get_vert(0, 1, 1, true,
//                                      T,TT, TTif, TTie)
//      <<"->"
//      << igl::dev::tet_tuple_get_vert(0, 1, 1, false,
//                                      T,TT, TTif, TTie)<<endl;

  igl::dev::tet_tuple_edge_split(0, 1, 1, true, tet_quality,[](int){return
                                     true;}, V,T,TT, TTif,
                                 TTie);
//  for(auto r:T) cout<<r<<":"<<tet_quality(r(0),r(1),r(2),r(3)) <<endl;
  return 0;
}

template <int enable = 1>
int test_edge_contraction() {

  using namespace Eigen;
  using namespace std;

  using vecV2d = std::vector<Eigen::RowVector2d>;
  using vecV3d = std::vector<Eigen::RowVector3d>;
  using vecV3i = std::vector<Eigen::RowVector3i>;
  using vecV4i = std::vector<Eigen::RowVector4i>;
  using T_t = vecV4i;
  using TT_t = T_t;
  using TTie_t = std::vector<Eigen::Matrix<int, 4, 3>>;

  vecV3d V(9);
  vecV4i T(7);
  V[0] << 0, -0, 1;
  V[1] << 0, -0, -1;
  V[2] << 0, 1, 0;
  V[3] << -1, 0, 0;
  V[4] << -0.8, -0.5, 0;
  V[5] << -0.2, -0.8, 0;
  V[6] << 0.2, -0.8, 0;
  V[7] << 0.8, -0.5, 0;
  V[8] << 1, 0, 0;

  T[0] << 1, 2, 3, 0;
  T[1] << 1, 3, 4, 0;
  T[2] << 1, 4, 5, 0;
  T[3] << 1, 5, 6, 0;
  T[4] << 1, 6, 7, 0;
  T[5] << 1, 7, 8, 0;
  T[6] << 1, 8, 2, 0;

  auto tet_quality = [&V](int a, int b, int c, int d) -> double {
    return -test_utils::quality(V[a], V[b], V[c], V[d]);
  };
//   for(auto r:T) cout<<r<<":"<<tet_quality(r(0),r(1),r(2),r(3)) <<endl;

  vecV4i TT, TTif;
  TTie_t TTie;
  igl::dev::tetrahedron_tetrahedron_adjacency(T, TT, TTif, TTie);
//
//   cout<< igl::dev::tet_tuple_get_vert(0, 1, 1, true,
//                                       T,TT, TTif, TTie)
//       <<"->"
//       << igl::dev::tet_tuple_get_vert(0, 1, 1, false,
//                                       T,TT, TTif, TTie)<<endl;

  std::vector<int> dummy;

  igl::dev::tet_tuple_edge_split(0, 1, 1, true, tet_quality,[](int){return
      true;}, V,T,TT, TTif,
                                 TTie, dummy);
//   for(auto r:T) cout<<r<<":"<<tet_quality(r(0),r(1),r(2),r(3)) <<endl;

  V[1](2) -= 9;
  V[9](2) -= 1;
//   cout<< igl::dev::tet_tuple_get_vert(7, 1, 1, true,
//                                       T,TT, TTif, TTie)
//       <<"->"
//       << igl::dev::tet_tuple_get_vert(7, 1, 1, false,
//                                       T,TT, TTif, TTie)<<endl;
  igl::dev::tet_tuple_edge_split(7, 1, 1, true, tet_quality,[](int){return
                                     true;}, V,T,TT, TTif,
                                 TTie,dummy);

  V[9](2) += 0.9;
  V[1](2) += 9;
//  for(auto r:T) cout<<r<<endl;
  igl::dev::tet_tuple_edge_contraction(7,
                                       1,
                                       1,
                                       true,
                                       tet_quality,[](int){return
          true;},
                                       T,
                                       TT,
                                       TTif,
                                       TTie,
                                       dummy);

//  for(auto r:T) cout<<r<<":"<<tet_quality(r(0),r(1),r(2),r(3)) <<endl;
  return 0;
}
/*
int test_all() {
  test_edge_split<1>();
  test_edge_contraction();
  test_bumpy_vert_tet_query();
  test_tetmesh_topology_improvement();
  test_simple_face_removal();
  test_edge_removal_pass();
  test_single_edge_removal();
  test_extraction_recurse();
  test_retain_fix_end();
  test_retain_tetrahedron_adjacency();
  test_tetrahedron_adjacency();
}
*/


#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <math.h>

using namespace std;
using namespace Eigen;
ros::Publisher odom_pub;
MatrixXd Q = MatrixXd::Identity(12, 12);
MatrixXd Rt = MatrixXd::Identity(6,6);
//Define some para
int img_checktemp=0;
int temp_ff=0;
Eigen::Vector3d p,q,p_dot,b_g,b_a;
Eigen::Vector3d u_a,u_g;
Eigen::VectorXd ut(6);
Eigen::VectorXd x_dot_prev(15),x_prev(15);
Eigen::VectorXd x_dot(15)     ,x(15);
Eigen::VectorXd Mu(15) ,Mu_bar(15), Mu_prev(15);
Eigen::MatrixXd Sigma(15,15) ,Sigma_bar(15,15), Sigma_prev(15,15);
Eigen::VectorXd process_model(15);
Eigen::Matrix3d R_WorldinCAM;
Eigen::Vector3d T_WorldinCAM;
Eigen::VectorXd Q_WorldinCAM(4);
Eigen::Matrix3d R_IMUinWorld;
Eigen::Vector3d T_IMUinWorld;
Eigen::VectorXd Q_IMUinWorld(4);
double dt, time_old,grav;
//Define some Matrix
Eigen::MatrixXd At(15,15);
Eigen::MatrixXd Ut(15,12);
Eigen::MatrixXd Ft(15,15);
Eigen::MatrixXd Vt(15,12);
Eigen::MatrixXd Ct(6,15);
Eigen::MatrixXd Kt(15,6);
Eigen::MatrixXd H_CAMinIMU(4,4);
Eigen::MatrixXd H_WorldinCAM(4,4);
Eigen::MatrixXd H_IMUinWorld(4,4);
void imu_callback(const sensor_msgs::Imu::ConstPtr &msg)
{
    //your code for propagation
	//Get process model
/*Get Row Data as u*/
	u_a(0) = msg->linear_acceleration.x;
	u_a(1) = msg->linear_acceleration.y;
	u_a(2) = msg->linear_acceleration.z;
	//cout << "u_a(0):" << u_a(0) << "u_a(1):" << u_a(1) << "u_a(2):" << u_a(2) << endl;
	u_g(0) = msg->angular_velocity.x;//as roll
	u_g(1) = msg->angular_velocity.y;//as pitch
	u_g(2) = msg->angular_velocity.z;//as yaw
/*Get Row Data as u*/
}

//Rotation from the camera frame to the IMU frame
Eigen::Matrix3d Rcam;
void odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    //your code for update
    //camera position in the IMU frame = (0, -0.04, -0.02)
    //camera orientaion in the IMU frame = Quaternion(0, 0, 1, 0); w x y z, respectively
    //					   RotationMatrix << -1, 0, 0,
    //							      0, 1, 0,
    //                                                        0, 0, -1;
/*Get the dt from stamp*/
{
	if (!img_checktemp)
    {
	time_old = msg->header.stamp.toSec()-0.03;
	Mu.setZero();
	Sigma.setIdentity();
	//img_checktemp = 1;
	//cout << "Mu_prev" << endl << Mu_prev << endl;
    }
    dt=msg->header.stamp.toSec() - time_old;
    time_old = msg->header.stamp.toSec();
    //cout << "dt" << endl << sin(1.5) << endl;
}
/*Get the dt from stamp*/
/*Prediction Step*/
{
	//move forward
	x_prev     = x;
	x_dot_prev = x_dot;
	Mu_prev    = Mu;
	Sigma_prev = Sigma;
	//get ut from IMU
	ut        << u_a , u_g;
	//cout << "u_a(0):" << u_a(0) << "u_a(1):" << u_a(1) << "u_a(2):" << u_a(2) << endl;
	//cout << "Rcam:"   << Rcam   << endl;
	{//three matrix
	//get matrix At
	At            <<0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
					0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,
  0, 0, 0,0,Mu_prev(9)*sin(Mu_prev(4)) - Mu_prev(11)*cos(Mu_prev(4)) + ut(5)*cos(Mu_prev(4)) - ut(3)*sin(Mu_prev(4)),                                                                                0, 0, 0, 0,                                               -cos(Mu_prev(4)),  0,                                              -sin(Mu_prev(4)),                                                                                0,                                            0,                                                                                0,
  0, 0, 0,(Mu_prev(11)*cos(Mu_prev(4)) - Mu_prev(9)*sin(Mu_prev(4)) - ut(5)*cos(Mu_prev(4)) + ut(3)*sin(Mu_prev(4)))/pow(cos(Mu_prev(3)),2),                                                                                -(sin(Mu_prev(3))*(Mu_prev(9)*cos(Mu_prev(4)) + Mu_prev(11)*sin(Mu_prev(4)) - ut(3)*cos(Mu_prev(4)) - ut(5)*sin(Mu_prev(4))))/cos(Mu_prev(3)),                                                                                0, 0, 0, 0, -(sin(Mu_prev(3))*sin(Mu_prev(4)))/cos(Mu_prev(3)), -1, (cos(Mu_prev(4))*sin(Mu_prev(3)))/cos(Mu_prev(3)),                                                                                0,                                            0,                                                                                0,
  0, 0, 0,-(sin(Mu_prev(3))*(Mu_prev(11)*cos(Mu_prev(4)) - Mu_prev(9)*sin(Mu_prev(4)) - ut(5)*cos(Mu_prev(4)) + ut(3)*sin(Mu_prev(4))))/pow(cos(Mu_prev(3)),2),                                                                                (Mu_prev(9)*cos(Mu_prev(4)) + Mu_prev(11)*sin(Mu_prev(4)) - ut(3)*cos(Mu_prev(4)) - ut(5)*sin(Mu_prev(4)))/cos(Mu_prev(3)),                                                                                0, 0, 0, 0,                          sin(Mu_prev(4))/cos(Mu_prev(3)),  0,                        -cos(Mu_prev(4))/cos(Mu_prev(3)),                                                                                0,                                            0,                                                                                0,
  0, 0, 0, cos(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))*(Mu_prev(12) - ut(0)) - cos(Mu_prev(3))*cos(Mu_prev(4))*sin(Mu_prev(5))*(Mu_prev(14) - ut(2)) - sin(Mu_prev(3))*sin(Mu_prev(5))*(Mu_prev(13) - ut(1)), (Mu_prev(12) - ut(0))*(cos(Mu_prev(5))*sin(Mu_prev(4)) + cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5))) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(4))*cos(Mu_prev(5)) - sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))), (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*sin(Mu_prev(5)) + cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4))) + (Mu_prev(14) - ut(2))*(sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3))) + cos(Mu_prev(3))*cos(Mu_prev(5))*(Mu_prev(13) - ut(1)), 0, 0, 0,                                                                    0,  0,                                                                   0,   sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5)),  cos(Mu_prev(3))*sin(Mu_prev(5)), - cos(Mu_prev(5))*sin(Mu_prev(4)) - cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5)),
  0, 0, 0, cos(Mu_prev(5))*sin(Mu_prev(3))*(Mu_prev(13) - ut(1)) - cos(Mu_prev(3))*cos(Mu_prev(5))*sin(Mu_prev(4))*(Mu_prev(12) - ut(0)) + cos(Mu_prev(3))*cos(Mu_prev(4))*cos(Mu_prev(5))*(Mu_prev(14) - ut(2)), (Mu_prev(12) - ut(0))*(sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3))) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(4))*sin(Mu_prev(5)) + cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4))), cos(Mu_prev(3))*sin(Mu_prev(5))*(Mu_prev(13) - ut(1)) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(5))*sin(Mu_prev(4)) + cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5))) - (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*cos(Mu_prev(5)) - sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))), 0, 0, 0,                                                                    0,  0,                                                                   0, - cos(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4)), -cos(Mu_prev(3))*cos(Mu_prev(5)),   cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3)) - sin(Mu_prev(4))*sin(Mu_prev(5)),
  0, 0, 0,cos(Mu_prev(4))*sin(Mu_prev(3))*(Mu_prev(14) - ut(2)) - cos(Mu_prev(3))*(Mu_prev(13) - ut(1)) - sin(Mu_prev(3))*sin(Mu_prev(4))*(Mu_prev(12) - ut(0)),                                                                                cos(Mu_prev(3))*cos(Mu_prev(4))*(Mu_prev(12) - ut(0)) + cos(Mu_prev(3))*sin(Mu_prev(4))*(Mu_prev(14) - ut(2)),                                                                                0, 0, 0, 0,                                                                    0,  0,                                                                   0,                                                                       cos(Mu_prev(3))*sin(Mu_prev(4)),                       -sin(Mu_prev(3)),                                                                      -cos(Mu_prev(3))*cos(Mu_prev(4)),
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
					0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
					0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
	//get matrix Ut
	Ut            <<0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                               -cos(Mu_prev(4)),  0,                                              -sin(Mu_prev(4)), 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0, -(sin(Mu_prev(3))*sin(Mu_prev(4)))/cos(Mu_prev(3)), -1, (cos(Mu_prev(4))*sin(Mu_prev(3)))/cos(Mu_prev(3)), 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                          sin(Mu_prev(4))/cos(Mu_prev(3)),  0,                        -cos(Mu_prev(4))/cos(Mu_prev(3)), 0, 0, 0, 0, 0, 0,
    sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5)),  cos(Mu_prev(3))*sin(Mu_prev(5)), - cos(Mu_prev(5))*sin(Mu_prev(4)) - cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5)),                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
  - cos(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4)), -cos(Mu_prev(3))*cos(Mu_prev(5)),   cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3)) - sin(Mu_prev(4))*sin(Mu_prev(5)),                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                        cos(Mu_prev(3))*sin(Mu_prev(4)),                       -sin(Mu_prev(3)),                                                                      -cos(Mu_prev(3))*cos(Mu_prev(4)),                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 1, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 1, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 1,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 1, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 1, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 1, 0, 0, 0;

	//get Mu_bar
	grav = 9.8;
	process_model << Mu_prev(6),
                     Mu_prev(7),
                     Mu_prev(8),
  ut(3)*cos(Mu_prev(4)) - Mu_prev(11)*sin(Mu_prev(4)) - Mu_prev(9)*cos(Mu_prev(4)) + ut(5)*sin(Mu_prev(4)),
-(Mu_prev(10)*cos(Mu_prev(3)) - ut(4)*cos(Mu_prev(3)) - Mu_prev(11)*cos(Mu_prev(4))*sin(Mu_prev(3)) + Mu_prev(9)*sin(Mu_prev(3))*sin(Mu_prev(4)) + ut(5)*cos(Mu_prev(4))*sin(Mu_prev(3)) - ut(3)*sin(Mu_prev(3))*sin(Mu_prev(4)))/cos(Mu_prev(3)),
-(Mu_prev(11)*cos(Mu_prev(4)) - Mu_prev(9)*sin(Mu_prev(4)) - ut(5)*cos(Mu_prev(4)) + ut(3)*sin(Mu_prev(4)))/cos(Mu_prev(3)),
  - (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*cos(Mu_prev(5)) - sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(5))*sin(Mu_prev(4)) + cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5))) + cos(Mu_prev(3))*sin(Mu_prev(5))*(Mu_prev(13) - ut(1)),
  - (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*sin(Mu_prev(5)) + cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4))) - (Mu_prev(14) - ut(2))*(sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3))) - cos(Mu_prev(3))*cos(Mu_prev(5))*(Mu_prev(13) - ut(1)),
grav - sin(Mu_prev(3))*(Mu_prev(13) - ut(1)) - cos(Mu_prev(3))*cos(Mu_prev(4))*(Mu_prev(14) - ut(2)) + cos(Mu_prev(3))*sin(Mu_prev(4))*(Mu_prev(12) - ut(0)),
                     0,
                     0,
                     0,
					 0,
					 0,
					 0;
	}
	Mu_bar     = Mu_prev + dt * process_model;
	//get Sigma_bar
	Ft         = MatrixXd::Identity(15, 15) + dt * At;
	Vt         = dt * Ut;
	Sigma_bar  = Ft * Sigma_prev * Ft.transpose() + Vt * Q * Vt.transpose();

	//cout << "process_model" << endl << process_model << endl;
}
/*Prediction Step*/
/*Updata Step*/
{
	//transform imu to world using camera
	/*H_CAMinIMU	<<
	-1,0,0 ,0    ,
	0 ,1,0 ,-0.04,
	0 ,0,-1,-0.02,
	0 ,0,0 ,1    ;*/
	H_CAMinIMU.col(0) << Rcam.col(0)  , 0;
	H_CAMinIMU.col(1) << Rcam.col(1)  , 0;
	H_CAMinIMU.col(2) << Rcam.col(2)  , 0;
	H_CAMinIMU.col(3) << 0,-0.04,-0.02, 1;
	T_WorldinCAM(0) = msg->pose.pose.position.x;
	T_WorldinCAM(1) = msg->pose.pose.position.y;
	T_WorldinCAM(2) = msg->pose.pose.position.z;
	Q_WorldinCAM(0) = msg->pose.pose.orientation.w;
	Q_WorldinCAM(1) = msg->pose.pose.orientation.x;
	Q_WorldinCAM(2) = msg->pose.pose.orientation.y;
	Q_WorldinCAM(3) = msg->pose.pose.orientation.z;
    R_WorldinCAM = Quaterniond(Q_WorldinCAM(0),Q_WorldinCAM(1),Q_WorldinCAM(2),Q_WorldinCAM(3)).toRotationMatrix();

	H_WorldinCAM.col(0) << R_WorldinCAM.col(0), 0;
	H_WorldinCAM.col(1) << R_WorldinCAM.col(1), 0;
	H_WorldinCAM.col(2) << R_WorldinCAM.col(2), 0;
	H_WorldinCAM.col(3) << T_WorldinCAM       , 1;

	H_IMUinWorld = H_WorldinCAM.inverse() * H_CAMinIMU.inverse()  ;//????????????????
	R_IMUinWorld = H_IMUinWorld.topLeftCorner(3, 3);
	T_IMUinWorld = H_IMUinWorld.topRightCorner(3, 1);
	//cout << "H_IMUinWorld" << endl << H_IMUinWorld << endl;
	//get matrix Ct
	Ct   << MatrixXd::Identity(6, 15);
	//get matrix Kt
	Kt   =  Sigma_bar * Ct.transpose() * ((Ct * Sigma_bar * Ct.transpose() + MatrixXd::Identity(6, 6) * Rt * MatrixXd::Identity(6, 6)).inverse());
	//get Mu & Sigma
	x.head(3) = T_IMUinWorld;
	x(3) = asin(R_IMUinWorld(2,1));
	//x(4) = atan(-R_IMUinWorld(2,0)/R_IMUinWorld(2,2));
	//x(5) = atan(-R_IMUinWorld(0,1)/R_IMUinWorld(1,1));
	//x(4) = acos(R_IMUinWorld(2,2)/cos(x(3)));
	x(4) = asin(-R_IMUinWorld(0,2)/cos(x(3)));
	if ((R_IMUinWorld(0,2)/cos(x(3)))>1)
	{
		x(4) = asin(1);
	}
	if ((R_IMUinWorld(0,2)/cos(x(3)))<-1)
	{
		x(4) = asin(-1);
	}
	x(5) = acos(R_IMUinWorld(1,1)/cos(x(3)));
	//x(5) = asin(-R_IMUinWorld(0,1)/cos(x(3)));
	//x.segment(3,3) = R_IMUinWorld.eulerAngles(1, 2, 0);

	Mu   =  Mu_bar + Kt * (Ct*x - Ct*Mu_bar);
	Sigma=  Sigma_bar - Kt*Ct*Sigma_bar;

	if(std::isnan(Mu(0))&&temp_ff==0)
	{
		cout << "asin(-R_IMUinWorld(0,2)/cos(x(3)))" << endl << asin(-R_IMUinWorld(0,2)/cos(x(3))) << endl;
		cout << "R_IMUinWorld(0,2)" << endl << R_IMUinWorld(0,2) << endl;
		cout << "cos(x(3))" << endl << cos(x(3)) << endl;
		cout << "x" << endl << x << endl;
		cout << "Kt" << endl << Kt << endl;
		cout << "Mu_bar" << endl << Mu_prev << endl;
		/*cout << "Mu_prev" << endl << Mu_prev << endl;
		cout << "Ft" << endl << Ft << endl;
		cout << "Sigma_prev" << endl << Sigma_prev << endl;
		cout << "Sigma_bar" << endl << Sigma_bar << endl;*/
		cout << "dt" << endl << dt << endl;
		cout << "img_checktemp" << endl << img_checktemp << endl;
		temp_ff = 1;
	}
	//cout << "Mu" << endl << Mu << endl;
	if (img_checktemp>=0&&img_checktemp<=1)
    {
	//cout << "Sigma_bar" << endl << Sigma_bar << endl;
	//cout << "Mu_bar" << endl << Mu_bar << endl;
	//cout << "Ut" << endl << Mu_prev << endl;
   // cout << "At" << endl << At << endl;
    //cout << "H_WorldinCAM" << endl << H_WorldinCAM << endl;
	cout << "H_CAMinIMU * H_WorldinCAM" << endl << H_CAMinIMU * H_WorldinCAM << endl;
	cout << "H_WorldinCAM * H_CAMinIMU" << endl << H_WorldinCAM * H_CAMinIMU << endl;
    cout << "H_IMUinWorld" << endl << H_IMUinWorld << endl;
	//cout << "H_IMUinWorld.topRightCorner(3, 1)" << endl << H_IMUinWorld.topRightCorner(3, 1) << endl;
	//cout << "H_IMUinWorld.topLeftCorner(3, 3)"  << endl << H_IMUinWorld.topLeftCorner(3, 3)  << endl;
	//cout << "(Ct * Sigma_bar * Ct.transpose())" << endl << (Ct * Sigma_bar * Ct.transpose()) << endl;
	//cout << "Kt" << endl << Kt << endl;
	cout <<endl<< "***********************************"<<endl;
    }
	img_checktemp ++;
	//cout << "Kt" << endl << Kt << endl;
	Matrix3d m_temp;//??????????????
	m_temp <<
	cos(Mu(4))*cos(Mu(5))-sin(Mu(3))*sin(Mu(4))*sin(Mu(5)), -cos(Mu(3))*sin(Mu(5)), sin(Mu(4))*cos(Mu(5))+sin(Mu(3))*cos(Mu(4))*sin(Mu(5)),
    cos(Mu(4))*sin(Mu(5))+sin(Mu(3))*sin(Mu(4))*cos(Mu(5)), cos(Mu(3))*cos(Mu(5)) , sin(Mu(4))*sin(Mu(5))-sin(Mu(3))*cos(Mu(4))*cos(Mu(5)),
    -cos(Mu(3))*sin(Mu(4))                                , sin(Mu(3))            , cos(Mu(3))*cos(Mu(4))                                 ;
	Quaterniond Q_ekf;
    Q_ekf = m_temp;
    nav_msgs::Odometry odom_ekf;
    odom_ekf.header.stamp = msg->header.stamp;
    odom_ekf.header.frame_id = "world";
    odom_ekf.pose.pose.position.x = Mu(0);
    odom_ekf.pose.pose.position.y = Mu(1);
    odom_ekf.pose.pose.position.z = Mu(2);
    odom_ekf.pose.pose.orientation.w = Q_ekf.w();
    odom_ekf.pose.pose.orientation.x = Q_ekf.x();
    odom_ekf.pose.pose.orientation.y = Q_ekf.y();
    odom_ekf.pose.pose.orientation.z = Q_ekf.z();
	odom_ekf.twist.twist.linear.x    = Mu(6);
	odom_ekf.twist.twist.linear.y    = Mu(7);
	odom_ekf.twist.twist.linear.z    = Mu(8);
    odom_pub.publish(odom_ekf);
}
/*Updata Step*/
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ekf");
    ros::NodeHandle n("~");
    ros::Subscriber s1 = n.subscribe("imu", 1000, imu_callback);
    ros::Subscriber s2 = n.subscribe("tag_odom", 1000, odom_callback);
    odom_pub = n.advertise<nav_msgs::Odometry>("ekf_odom", 100);
    Rcam = Quaterniond(0, 0, -1, 0).toRotationMatrix();
    cout << "R_cam" << endl << Rcam << endl;
    // Q imu covariance matrix; Rt visual odomtry covariance matrix
    Q.topLeftCorner(6, 6) = 0.01 * Q.topLeftCorner(6, 6);
    Q.bottomRightCorner(6, 6) = 0.01 * Q.bottomRightCorner(6, 6);
    Rt.topLeftCorner(3, 3) = 0.5 * Rt.topLeftCorner(3, 3);
    Rt.bottomRightCorner(3, 3) = 0.5 * Rt.bottomRightCorner(3, 3);
    Rt.bottomRightCorner(1, 1) = 0.1 * Rt.bottomRightCorner(1, 1);

    ros::spin();
}


// this example shows how to compute elastic model energy, energy gradient and energy hessian

#include <iostream>
#include <math.h>
#include <time.h>
#include <fstream>

#include "MCSFD/MCSFDCore.h"
#include "LoboVolumtricMesh/LoboTetMesh.h"
#include "ElasticModel/HyperelasticModel.h"

#include "Utils/pugixml/pugixml.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "Utils/SparseMatrix/SparseMatrixTopology.h"
#include <omp.h>

LOBO_MAKE_TYPEDEFS(double, t);

using namespace Lobo;

void loadmaterial(Lobo::HyperelasticModel *elasticmodel, const char *xmlfile)
{
    pugi::xml_document xml_doc;
    pugi::xml_parse_result result = xml_doc.load_file(xmlfile);
    pugi::xml_node model_node = xml_doc.child("Scene").child("HyperelasticModel");
    elasticmodel->runXMLscript(model_node);
    elasticmodel->precompute();
    elasticmodel->useMCSFD = false;
    elasticmodel->isinvertible = false;
}

int main()
{
    omp_set_dynamic(0); // Explicitly disable dynamic teams
    omp_set_num_threads(
        1); // Use 4 threads for all consecutive parallel regions

    Lobo::LoboTetMesh *tetmesh = new Lobo::LoboTetMesh();
    tetmesh->loadTetMeshAscii("2tet");
    std::cout << tetmesh->tet_indices.size() << std::endl;
    Lobo::HyperelasticModel *stvkmodel = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(stvkmodel, "FEM_stvk.xml");
    Lobo::HyperelasticModel *stvkmodel_csfd = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(stvkmodel_csfd, "FEM_stvkCSFD.xml");

    Lobo::HyperelasticModel *neohookeanmodel = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(neohookeanmodel, "FEM_neohookean.xml");
    Lobo::HyperelasticModel *neohookeanmodel_csfd = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(neohookeanmodel_csfd, "FEM_neohookeanCSFD.xml");

    int num_dofs = tetmesh->getNumVertex() * 3;
    Eigen::VectorXd u(num_dofs);
    Eigen::VectorXd internalforce(num_dofs);

    int flags_all = 0;
    flags_all |= Computeflags_energy | Computeflags_fisrt | Computeflags_second;

    srand(time(NULL));
    u.setRandom();

    stvkmodel->currentdisplacement = u;
    stvkmodel_csfd->currentdisplacement = u;
    neohookeanmodel->currentdisplacement = u;
    neohookeanmodel_csfd->currentdisplacement = u;

    int numtest = 10000;
    clock_t t1 = clock();

    //compute energy, energy gradient, energy hessian with analytical function
    for (int i = 0; i < numtest; i++)
    {
        stvkmodel->getTetForceMatrix(0, stvkmodel->internalforce_list[0], stvkmodel->stiffness_list[0], flags_all);
    }

    clock_t t2 = clock();
    //std::cout << *stvkmodel->stiffness_list[0] << std::endl;
    std::cout << "no MSCFD stvk " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        stvkmodel_csfd->getTetForceMatrix(0, stvkmodel_csfd->internalforce_list[0], stvkmodel_csfd->stiffness_list[0], flags_all);
    }

    t2 = clock();

    //std::cout << *stvkmodel_csfd->stiffness_list[0] << std::endl;
    std::cout << "MSCFD stvk " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        stvkmodel_csfd->getTetForceMatrixCSFD(0, stvkmodel_csfd->internalforce_list[0], stvkmodel_csfd->stiffness_list[0]);
    }

    t2 = clock();

    //std::cout << *stvkmodel_csfd->stiffness_list[0] << std::endl;
    std::cout << "no image MSCFD stvk " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        neohookeanmodel->getTetForceMatrix(0, neohookeanmodel->internalforce_list[0], neohookeanmodel->stiffness_list[0], flags_all);
    }

    t2 = clock();

    //std::cout << *neohookeanmodel->stiffness_list[0] << std::endl;
    std::cout << "no MSCFD neohookean " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        neohookeanmodel_csfd->getTetForceMatrix(0, neohookeanmodel_csfd->internalforce_list[0], neohookeanmodel_csfd->stiffness_list[0], flags_all);
    }

    t2 = clock();

    //std::cout << *neohookeanmodel_csfd->stiffness_list[0] << std::endl;
    std::cout << "MSCFD neohookean " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;
    //delete tetmesh;
    //delete hyperelasticmodel_csfd;
    //delete hyperelasticmodel;

    return 0;
}


// Copyright (c) 2019 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include "pyinterp/detail/gsl/interpolate1d.hpp"
#include "pyinterp/detail/math.hpp"
#include <Eigen/Core>

namespace pyinterp {
namespace detail {
namespace math {

/// Set of coordinates/values used for interpolation
///  * q11 = (x1, y1)
///  * q12 = (x1, y2)
///  * .../...
///  * q1n = (x1, yn)
///  * q21 = (x2, y1)
///  * q22 = (x2, y2).
///  * .../...
///  * q2n = (x2, yn)
///  * .../...
///  * qnn = (xn, yn)
///
/// @code
/// Array({{x1, x2, ..., xn}, {y1, y2, ..., yn}},
///        {q11, q12, ..., q21, q22, ...., qnn})
/// @endcode
class XArray {
 public:
  /// Default constructor
  XArray() = delete;

  /// Creates a new Array
  XArray(const size_t x_size, const size_t y_size) {
    auto nx = x_size << 1U;
    auto ny = y_size << 1U;
    x_.resize(nx);
    y_.resize(ny);
    q_.resize(nx, ny);
  }

  /// Default destructor
  virtual ~XArray() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  XArray(const XArray &rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  XArray(XArray &&rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  XArray &operator=(const XArray &rhs) = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  XArray &operator=(XArray &&rhs) noexcept = default;

  /// Get the half size of the window in abscissa.
  inline size_t nx() const noexcept {
    return static_cast<size_t>(x_.size()) >> 1U;
  }

  /// Get the half size of the window in ordinate.
  inline size_t ny() const noexcept {
    return static_cast<size_t>(y_.size()) >> 1U;
  }

  /// Get x-coordinates
  inline Eigen::VectorXd &x() noexcept { return x_; }

  /// Get x-coordinates
  inline const Eigen::VectorXd &x() const noexcept { return x_; }

  /// Get y-coordinates
  inline Eigen::VectorXd &y() noexcept { return y_; }

  /// Get y-coordinates
  inline const Eigen::VectorXd &y() const noexcept { return y_; }

  /// Get the values from the array for all x and y coordinates.
  inline Eigen::MatrixXd &q() noexcept { return q_; }

  /// Get the values from the array for all x and y coordinates.
  inline const Eigen::MatrixXd &q() const noexcept { return q_; }

  /// Get the ith x-axis.
  inline double x(const size_t ix) const { return x_(ix); }

  /// Get the ith y-axis.
  inline double y(const size_t jx) const { return y_(jx); }

  /// Get the value at coordinate (ix, jx).
  inline double z(const size_t ix, const size_t jx) const { return q_(ix, jx); }

  /// Set the ith x-axis.
  inline double &x(const size_t ix) { return x_(ix); }

  /// Get the ith y-axis.
  inline double &y(const size_t jx) { return y_(jx); }

  /// Get the value at coordinate (ix, jx).
  inline double &z(const size_t ix, const size_t jx) { return q_(ix, jx); }

  /// Normalizes the angle with respect to the first value of the X axis of this
  /// array.
  inline double normalize_angle(const double xi) const {
    return math::normalize_angle(xi, x(0));
  }

  /// Returns true if this instance does not contains at least one Not A Number
  /// (NaN).
  inline bool is_valid() const { return !q_.hasNaN(); }

 private:
  Eigen::VectorXd x_{};
  Eigen::VectorXd y_{};
  Eigen::MatrixXd q_{};
};

/// Extension of cubic interpolation for interpolating data points on a
/// two-dimensional regular grid. The interpolated surface is smoother than
/// corresponding surfaces obtained by bilinear interpolation or
/// nearest-neighbor interpolation.
class Bicubic {
 public:
  /// Default constructor
  ///
  /// @param type method of calculation
  explicit Bicubic(const gsl_interp_type *type = gsl_interp_cspline)
      : type_(type) {}

  /// Return the interpolated value of y for a given point x
  double interpolate(const double x, const double y, const XArray &xr,
                     gsl::Accelerator acc = gsl::Accelerator()) const {
    return evaluate(&gsl::Interpolate1D::interpolate, x, y, xr, std::move(acc));
  }

  /// Return the derivative for a given point x
  double derivative(const double x, const double y, const XArray &xr,
                    gsl::Accelerator acc = gsl::Accelerator()) const {
    return evaluate(&gsl::Interpolate1D::derivative, x, y, xr, std::move(acc));
  }

  /// Return the second derivative for a given point x
  double second_derivative(const double x, const double y, const XArray &xr,
                           gsl::Accelerator acc = gsl::Accelerator()) const {
    return evaluate(&gsl::Interpolate1D::second_derivative, x, y, xr,
                    std::move(acc));
  }

 private:
  using InterpolateFunction =
      double (gsl::Interpolate1D::*)(const double) const;
  const gsl_interp_type *type_;

  /// Evaluation of the GSL function performing the calculation.
  double evaluate(
      const std::function<double(const gsl::Interpolate1D &, double)> &function,
      const double x, const double y, const XArray &xr,
      gsl::Accelerator &&acc) const {
    Eigen::VectorXd fy(xr.x().size());

    // Spline interpolation as function of Y-coordinate
    for (auto ix = 0; ix < xr.x().size(); ++ix) {
      // The block containing the processed row must be copied into a new
      // memory block.
      Eigen::VectorXd row = xr.q().row(ix);
      auto interpolator = gsl::Interpolate1D(type_, xr.y(), row, acc);
      fy(ix) = function(interpolator, y);
    }
    auto interpolator = gsl::Interpolate1D(type_, xr.x(), fy, acc);
    return function(interpolator, x);
  }
};

}  // namespace math
}  // namespace detail
}  // namespace pyinterp


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <math.h>
#include <limits>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include "modules/world/opendrive/plan_view.hpp"
#include "modules/world/opendrive/lane.hpp"
#include "modules/world/opendrive/odrSpiral.hpp"

namespace modules {
namespace world {
namespace opendrive {

namespace bg = boost::geometry;

bool PlanView::add_line(geometry::Point2d start_point,
                        float heading,
                        float length) {
  //! straight line
  reference_line_.add_point(start_point);
  geometry::Point2d end_point(
    bg::get<0>(start_point) + length * cos(heading),
    bg::get<1>(start_point) + length * sin(heading));
  reference_line_.add_point(end_point);
  //! calculate overall length
  length_ = bg::length(reference_line_.obj_);
  return true;
}

bool PlanView::add_spiral(
  geometry::Point2d start_point,
  float heading,
  float length,
  float curvature_start,
  float curvature_end,
  float s_inc) {
  double x = bg::get<0>(start_point);
  double y = bg::get<1>(start_point);
  double t = heading, cDot = (curvature_end - curvature_start) / length;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);

  double s = 0.0;
  for (; s <= length;) {
    odrSpiral(s, x_old, y_old, cDot, curvature_start, heading, &x, &y, &t);
    reference_line_.add_point(geometry::Point2d(x, y));
    if ((length - s < s_inc) && (length - s > 0.))
      s_inc = length - s;
    s += s_inc;
  }
  length_ = bg::length(reference_line_.obj_);
  return true;
}

void PlanView::calc_arc_position(
  const float s,
  float initial_heading,
  float curvature,
  float &dx,
  float &dy) {
  initial_heading = fmod(initial_heading, 2 * M_PI);
  float hdg = initial_heading - M_PI / 2;
  float a = 2 / curvature * sin(s * curvature / 2);
  float alpha = (M_PI - s * curvature) / 2 - hdg;
  dx = -1 * a * cos(alpha);
  dy = a * sin(alpha);
  // tangent = initial_heading + s * initial_curvature;
}

bool PlanView::add_arc(
  geometry::Point2d start_point,
  float heading,
  float length,
  float curvature,
  float s_inc) {
  // add_spiral(start_point, heading, length, curvature, curvature, s_inc);

  float dx, dy;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);
  double s = 0.0;
  for (; s <= length;) {
    calc_arc_position(s, heading, curvature, dx, dy);
    reference_line_.add_point(geometry::Point2d(x_old + dx, y_old + dy));
    if (length - s < s_inc && length - s > 0.)
      s_inc = length - s;
    s += s_inc;
  }
  return true;
}

bool PlanView::apply_offset_transform(float x, float y, float hdg) {
  geometry::Line rotated_line = rotate(reference_line_, hdg);
  geometry::Line transformed_line = translate(rotated_line, x, y);
  reference_line_ = transformed_line;

  return true;
}

}  // namespace opendrive
}  // namespace world
}  // namespace modules