#pragma once

#include "modprop/ModpropTypes.h"
#include "modprop/utils/MatrixUtils.hpp"
#include <memory>
#include <boost/foreach.hpp>
#include <iostream>

namespace percepto
{

// Class of objects with parameters
// TODO Make this an interface so ParameterWrapper isn't so awkward
class Parameters
{
public:

	typedef std::shared_ptr<Parameters> Ptr;

	Parameters() {}

	void Initialize( unsigned int dim )
	{
		_params = VectorType( dim );
	}

	virtual void AccumulateDerivs( const MatrixType& delDodw )
	{
		if( delDodw.cols() != ParamDim() )
		{
			std::cout << "delDodw size: " << delDodw.size() << std::endl;
			std::cout << "ParamDim: " << ParamDim() << std::endl;
			throw std::runtime_error( "Parameters: Accumulation dim error." );
		}

		if( _dodw.size() == 0 ) { _dodw = delDodw; }
		else { _dodw += delDodw; }
	}

	virtual void ResetAccumulators()
	{
		_dodw = MatrixType();
	}

	virtual const MatrixType& GetDerivs() const
	{
		return _dodw;
	}

	virtual unsigned int ParamDim() const { return _params.size(); }

	virtual const VectorType& GetParamsVec() const { return _params; }

	virtual void SetParamsVec( const VectorType& vec )
	{
		if( vec.size() != _params.size() )
		{
			throw std::runtime_error( "Parameters: Cannot change size of parameters." );
		}
		_params = vec;
	}

protected:

	MatrixType _dodw;
	VectorType _params;
};

// Wraps a group of parameters
// Should not be treated as parameter memory as the local params
// copy is not updated
class ParameterWrapper
: public Parameters
{

public:

	typedef std::vector<Parameters::Ptr> ContainerType;
	typedef std::shared_ptr<ParameterWrapper> Ptr;

	ParameterWrapper() {}

	// TODO Remove this interface
	ParameterWrapper( const ContainerType& p )
	: _items( p )
	{
		Initialize( ParamDim() );
	}

	bool AddParameters( const Parameters::Ptr& p )
	{
		if( !p ) { return false; }
		BOOST_FOREACH( Parameters::Ptr& item, _items )
		{
			if( item == p ) { return false; }
		}
		_items.push_back( p );
		Initialize( ParamDim() );
		return true;
	}

	// This can get called by modules that operate directly on weights,
	// like ParameterL2Cost
	virtual void AccumulateDerivs( const MatrixType& dodw )
	{
		if( dodw.cols() != ParamDim() )
		{
			throw std::runtime_error( "ParametersWrapper: Accumulate dim error!" );
		}

		unsigned int ind = 0;
		BOOST_FOREACH( Parameters::Ptr& item, _items )
		{
			unsigned int n = item->ParamDim();
			item->AccumulateDerivs( dodw.block( 0, ind, dodw.rows(), n ) );
			ind += n;
		}
	}

	virtual void ResetAccumulators()
	{
		_accDodw = MatrixType();
		BOOST_FOREACH( Parameters::Ptr& item, _items )
		{
			item->ResetAccumulators();
		}
	}

	virtual const MatrixType& GetDerivs() const
	{
		_accDodw = MatrixType();
		if( _items.empty() ) { return _accDodw; }

		// First determine the sys output dim by checking for the
		// first item with a valid dodw
		unsigned int sysOutDim = 0;
		BOOST_FOREACH( const Parameters::Ptr& item, _items )
		{
			if( item->GetDerivs().size() == 0 ) { continue; }
			sysOutDim = item->GetDerivs().rows();
			break;
		}
		if( sysOutDim == 0 ) { return _accDodw; }

		_accDodw = MatrixType( sysOutDim, ParamDim() );
		unsigned int ind = 0;
		BOOST_FOREACH( const Parameters::Ptr& item, _items )
		{
			unsigned int n = item->ParamDim();
			MatrixType itemDodw = item->GetDerivs();
			if( itemDodw.size() == 0 )
			{
				itemDodw = MatrixType::Zero( sysOutDim, n );
			}
			_accDodw.block( 0, ind, sysOutDim, n ) = itemDodw;
			ind += n;
		}
		return _accDodw;
	}

	virtual unsigned int ParamDim() const
	{
		unsigned int acc = 0;
		BOOST_FOREACH( const Parameters::Ptr& item, _items )
		{
			acc += item->ParamDim();
		}
		return acc;
	}

	virtual const VectorType& GetParamsVec() const
	{
		_accParams = VectorType( ParamDim() );
		unsigned int ind = 0;
		BOOST_FOREACH( const Parameters::Ptr& item, _items )
		{
			unsigned int n = item->ParamDim();
			_accParams.segment( ind, n ) = item->GetParamsVec();
			ind += n;
		}
		return _accParams;
	}

	virtual void SetParamsVec( const VectorType& vec )
	{
		if( vec.size() != ParamDim() )
		{
			throw std::runtime_error( "ParametersWrapper: Invalid params vec size in SetParamsVec()." );
		}
		unsigned int ind = 0;
		BOOST_FOREACH( Parameters::Ptr& item, _items )
		{
			unsigned int n = item->ParamDim();
			item->SetParamsVec( vec.segment( ind, n ) );
			ind += n;
		}
		_accParams = vec;
	}

private:

	mutable VectorType _accParams;
	mutable MatrixType _accDodw;
	std::vector<Parameters::Ptr> _items;

};

}


/*
   Copyright (c) Marshall Clow 2017.

   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)

    For more information, see http://www.boost.org
*/

#include <vector>
#include <functional>
#include <numeric>

#include <boost/config.hpp>
#include <boost/algorithm/cxx11/iota.hpp>
#include <boost/algorithm/cxx17/transform_inclusive_scan.hpp>

#include "iterator_test.hpp"

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

namespace ba = boost::algorithm;

int triangle(int n) { return n*(n+1)/2; }

template <class _Tp>
struct identity
{
    const _Tp& operator()(const _Tp& __x) const { return __x;}
};

template <class Iter1, class BOp, class UOp, class Iter2>
void
transform_inclusive_scan_test(Iter1 first, Iter1 last, BOp bop, UOp uop, Iter2 rFirst, Iter2 rLast)
{
    std::vector<typename std::iterator_traits<Iter1>::value_type> v;
//  Test not in-place
    ba::transform_inclusive_scan(first, last, std::back_inserter(v), bop, uop);
    BOOST_CHECK(std::distance(first, last) == v.size());
    BOOST_CHECK(std::equal(v.begin(), v.end(), rFirst));

//  Test in-place
    v.clear();
    v.assign(first, last);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), bop, uop);
    BOOST_CHECK(std::distance(first, last) == v.size());
    BOOST_CHECK(std::equal(v.begin(), v.end(), rFirst));
}

template <class Iter>
void
transform_inclusive_scan_test()
{
          int ia[]     = {  1,  3,   5,   7,    9};
    const int pResI0[] = {  1,  4,   9,  16,   25};        // with identity
    const int mResI0[] = {  1,  3,  15, 105,  945};
    const int pResN0[] = { -1, -4,  -9, -16,  -25};        // with negate
    const int mResN0[] = { -1,  3, -15, 105, -945};
    const unsigned sa = sizeof(ia) / sizeof(ia[0]);
    BOOST_CHECK(sa == sizeof(pResI0) / sizeof(pResI0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResI0) / sizeof(mResI0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(pResN0) / sizeof(pResN0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResN0) / sizeof(mResN0[0]));       // just to be sure

    for (unsigned int i = 0; i < sa; ++i ) {
        transform_inclusive_scan_test(Iter(ia), Iter(ia + i), std::plus<int>(),       identity<int>(),    pResI0, pResI0 + i);
        transform_inclusive_scan_test(Iter(ia), Iter(ia + i), std::multiplies<int>(), identity<int>(),    mResI0, mResI0 + i);
        transform_inclusive_scan_test(Iter(ia), Iter(ia + i), std::plus<int>(),       std::negate<int>(), pResN0, pResN0 + i);
        transform_inclusive_scan_test(Iter(ia), Iter(ia + i), std::multiplies<int>(), std::negate<int>(), mResN0, mResN0 + i);
        }
}

//  Basic sanity
void basic_transform_inclusive_scan_tests()
{
    {
    std::vector<int> v(10);
    std::fill(v.begin(), v.end(), 3);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), std::plus<int>(), identity<int>());
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == (int)(i+1) * 3);
    }

    {
    std::vector<int> v(10);
    ba::iota(v.begin(), v.end(), 0);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), std::plus<int>(), identity<int>());
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == triangle(i));
    }

    {
    std::vector<int> v(10);
    ba::iota(v.begin(), v.end(), 1);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), std::plus<int>(), identity<int>());
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == triangle(i + 1));
    }

    {
    std::vector<int> v, res;
    ba::transform_inclusive_scan(v.begin(), v.end(), std::back_inserter(res), std::plus<int>(), identity<int>());
    BOOST_CHECK(res.empty());
    }
}

void test_transform_inclusive_scan()
{
    basic_transform_inclusive_scan_tests();

//  All the iterator categories
    transform_inclusive_scan_test<input_iterator        <const int*> >();
    transform_inclusive_scan_test<forward_iterator      <const int*> >();
    transform_inclusive_scan_test<bidirectional_iterator<const int*> >();
    transform_inclusive_scan_test<random_access_iterator<const int*> >();
    transform_inclusive_scan_test<const int*>();
    transform_inclusive_scan_test<      int*>();
}

template <class Iter1, class BOp, class UOp, class T, class Iter2>
void
transform_inclusive_scan_init_test(Iter1 first, Iter1 last, BOp bop, UOp uop, T init, Iter2 rFirst, Iter2 rLast)
{
    std::vector<typename std::iterator_traits<Iter1>::value_type> v;
//  Test not in-place
    ba::transform_inclusive_scan(first, last, std::back_inserter(v), bop, uop, init);
    BOOST_CHECK(std::distance(rFirst, rLast) == v.size());
    BOOST_CHECK(std::equal(v.begin(), v.end(), rFirst));

//  Test in-place
    v.clear();
    v.assign(first, last);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), bop, uop, init);
    BOOST_CHECK(std::distance(rFirst, rLast) == v.size());
    BOOST_CHECK(std::equal(v.begin(), v.end(), rFirst));
}

template <class Iter>
void
transform_inclusive_scan_init_test()
{
          int ia[]     = {  1,  3,   5,    7,     9};
    const int pResI0[] = {  1,  4,   9,   16,    25};        // with identity
    const int mResI0[] = {  0,  0,   0,    0,     0};
    const int pResN0[] = { -1, -4,  -9,  -16,   -25};        // with negate
    const int mResN0[] = {  0,  0,   0,    0,     0};
    const int pResI2[] = {  3,  6,  11,   18,    27};        // with identity
    const int mResI2[] = {  2,  6,  30,  210,  1890};
    const int pResN2[] = {  1, -2,  -7,  -14,   -23};        // with negate
    const int mResN2[] = { -2,  6, -30,  210, -1890};
    const unsigned sa = sizeof(ia) / sizeof(ia[0]);
    BOOST_CHECK(sa == sizeof(pResI0) / sizeof(pResI0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResI0) / sizeof(mResI0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(pResN0) / sizeof(pResN0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResN0) / sizeof(mResN0[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(pResI2) / sizeof(pResI2[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResI2) / sizeof(mResI2[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(pResN2) / sizeof(pResN2[0]));       // just to be sure
    BOOST_CHECK(sa == sizeof(mResN2) / sizeof(mResN2[0]));       // just to be sure

    for (unsigned int i = 0; i < sa; ++i ) {
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::plus<int>(),       identity<int>(),    0, pResI0, pResI0 + i);
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::multiplies<int>(), identity<int>(),    0, mResI0, mResI0 + i);
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::plus<int>(),       std::negate<int>(), 0, pResN0, pResN0 + i);
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::multiplies<int>(), std::negate<int>(), 0, mResN0, mResN0 + i);
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::plus<int>(),       identity<int>(),    2, pResI2, pResI2 + i);
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::multiplies<int>(), identity<int>(),    2, mResI2, mResI2 + i);
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::plus<int>(),       std::negate<int>(), 2, pResN2, pResN2 + i);
        transform_inclusive_scan_init_test(Iter(ia), Iter(ia + i), std::multiplies<int>(), std::negate<int>(), 2, mResN2, mResN2 + i);
        }
}

//  Basic sanity
void basic_transform_inclusive_scan_init_tests()
{
    {
    std::vector<int> v(10);
    std::fill(v.begin(), v.end(), 3);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), std::plus<int>(), identity<int>(), 50);
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == 50 + (int) (i + 1) * 3);
    }

    {
    std::vector<int> v(10);
    ba::iota(v.begin(), v.end(), 0);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), std::plus<int>(), identity<int>(), 30);
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == 30 + triangle(i));
    }

    {
    std::vector<int> v(10);
    ba::iota(v.begin(), v.end(), 1);
    ba::transform_inclusive_scan(v.begin(), v.end(), v.begin(), std::plus<int>(), identity<int>(), 40);
    for (size_t i = 0; i < v.size(); ++i)
        BOOST_CHECK(v[i] == 40 + triangle(i + 1));
    }

    {
    std::vector<int> v, res;
    ba::transform_inclusive_scan(v.begin(), v.end(), std::back_inserter(res), std::plus<int>(), identity<int>(), 1);
    BOOST_CHECK(res.empty());
    }
}

void test_transform_inclusive_scan_init()
{
	basic_transform_inclusive_scan_init_tests();

//  All the iterator categories
    transform_inclusive_scan_init_test<input_iterator        <const int*> >();
    transform_inclusive_scan_init_test<forward_iterator      <const int*> >();
    transform_inclusive_scan_init_test<bidirectional_iterator<const int*> >();
    transform_inclusive_scan_init_test<random_access_iterator<const int*> >();
    transform_inclusive_scan_init_test<const int*>();
    transform_inclusive_scan_init_test<      int*>();

}

BOOST_AUTO_TEST_CASE( test_main )
{
  test_transform_inclusive_scan();
  test_transform_inclusive_scan_init();
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <string>
#include <iomanip>
#include <map>
#include <set>
#include <bitset>
#include <vector>
#include <boost/algorithm/string/replace.hpp>

using namespace std;

#define FOR(i, n) for (int i = 0; i < n; i++)
#define OUT(s) cout << s << endl;
#define IN(a) cin >> a;
#define PACK(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cin >> a[i];
#define LIST(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cout << "[" << i << "] " << a[i] << endl;

int factorial(int i) {
  if (i > 1) {
    return factorial(i - 1);
  } else {
    return 1;
  }
}

int main() {
  int N;
  IN(N);
  int P[N];
  PACK(P, N);

  vector<int> data(P, P + N);
  int uniqueSize = set<int>(data.begin(), data.end()).size();
  OUT(uniqueSize + 1 + N);
}


/*
 *  asset_swap.cpp
 *  pybg
 *
 *  Created by BART MOSLEY on 9/2/12.
 *  Copyright 2012 BG Research LLC. All rights reserved.
 *
 */

#include <bg/bondgeek.hpp>

#include <iostream>

#include <boost/timer.hpp>

using namespace std;
using namespace QuantLib;
using namespace bondgeek;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {
    Integer sessionId() { return 0; }
}
#endif

void showAssetSwapCashFlows(AssetSwap &aswap)
{
    Leg fixedLeg = aswap.bondLeg();
    Leg floatLeg = aswap.floatingLeg();

    Date cfDate;
    double cfAmt;

    cout << endl << "Fixed Leg: " << endl;
    Leg::iterator fxIt;
    for (fxIt=fixedLeg.begin(); fxIt < fixedLeg.end(); fxIt++)
    {
        cfDate = (*fxIt)->date();
        cfAmt = (*fxIt)->amount();
        cout << cfDate << " | " << cfAmt << endl;
    }
    cout << endl << "Floating Leg: " << endl;
    Leg::iterator flIt;
    for (flIt=floatLeg.begin(); flIt < floatLeg.end(); flIt++)
    {
        cfDate = (*flIt)->date();
        cfAmt = (*flIt)->amount();
        cout << cfDate << " | " << cfAmt << endl;
    }

}

int main (int argc, char * const argv[])
{

    Date today = Date(31, August, 2012);
    Settings::instance().evaluationDate() = today;

    cout << endl << "Today: " << today << endl;
    cout << "Price with a bg curve " << endl;

    string depotenors[] = {"1W", "1M", "3M", "6M", "9M"};
    double depospots[] = {0.0018400, 0.0023050, 0.0041825,
        0.0070765, 0.0086780};
    string swaptenors[] = {"2y", "3y", "5y", "10Y",
        "15Y", "20Y", "30Y"};
    double swapspots[] = {.0040100, 0.0042600, 0.0077300, 0.0106595,
        0.0211555, 0.02310205, 0.0247007};

    cout << "Test with new curve " << endl;
    RateHelperCurve usdLiborCurve = RateHelperCurve(USDLiborCurve("3M"));
    usdLiborCurve.update(depotenors,
                         depospots,
                         5,
                         swaptenors,
                         swapspots,
                         7,
                         today);

    boost::shared_ptr<PricingEngine> discEngine = \
    createPriceEngine<DiscountingBondEngine>(
                                        usdLiborCurve.discountingTermStructure()
                                        );

    // set up the callable bond
    // assuming IL POB 6.63% 2/1/2035, ignoring sinking fund

    Real coupon = .0663;
    Date maturity(1, February, 2035);
    Date dated(1, February, 2010);

    Natural settlementDays = 3;  // Bloomberg OAS1 settle is Oct 19, 2007
    Frequency frequency = Semiannual;

    Calendar bondCalendar = UnitedStates(UnitedStates::Settlement);
    DayCounter bondDayCounter = Thirty360(Thirty360::BondBasis);

    Real faceAmount = 100.0; // Notional amount
    Real redemption = 100.0; // Amount paid on redemption

    BusinessDayConvention accrualConvention = Unadjusted;
    BusinessDayConvention paymentConvention = Unadjusted;

    BulletBond testbond(coupon,
               maturity,
               dated,
               bondCalendar,
               settlementDays,
               bondDayCounter,
               frequency,
               redemption,
               faceAmount,
               accrualConvention,
               paymentConvention
               );

    testbond.setPricingEngine(discEngine);

    cout << "test: " << testbond.cleanPrice() << endl;

    cout << "\n\nNow, Asset Swap" << endl;
    // create index and bond

    boost::shared_ptr< Bond > abnd(new BulletBond(coupon,
                                                  maturity,
                                                  dated,
                                                  bondCalendar,
                                                  settlementDays,
                                                  bondDayCounter,
                                                  frequency,
                                                  redemption,
                                                  faceAmount,
                                                  accrualConvention,
                                                  paymentConvention
                                                  ));

    abnd->setPricingEngine(discEngine);

    cout << "Bond: "
    << "cpn/mty: " << io::rate(coupon) << " due " << maturity << endl
    << "theoretical price: " << abnd->cleanPrice()
    << "ytm: " << abnd->yield(abnd->cleanPrice(),
                              bondDayCounter, Compounded, frequency) << endl
    << "accrued: " << abnd->accruedAmount() << endl
    << endl;

    USDLiborBase libor3m("3M");
    boost::shared_ptr<IborIndex> libor3m_ptr = libor3m( \
     usdLiborCurve.yieldTermStructurePtr()
     );

    bool payBondCoupon = false;
    Real bondCleanPrice = 112.363 ;
    Spread spread=0.00;
    Schedule floatSchedule=Schedule();
    DayCounter floatingDayCount=DayCounter();
    bool parAssetSwap=true;

    AssetSwap aswap(payBondCoupon,
                    abnd,
                    bondCleanPrice,
                    libor3m_ptr,
                    spread,
                    floatSchedule,
                    floatingDayCount,
                    parAssetSwap);

    boost::shared_ptr<PricingEngine> swapEngine = \
    createPriceEngine<DiscountingSwapEngine>(
                                    usdLiborCurve.discountingTermStructure()
                                    );

    aswap.setPricingEngine(swapEngine);

    cout << "swap: "
    << "fair spread: " << io::rate(aswap.fairSpread()) << endl
    << "fair cleanPrice: " <<aswap.fairCleanPrice() << endl
    << endl
    << endl;

    showAssetSwapCashFlows(aswap);

    BulletAssetSwap aswap2(abnd,
                           libor3m_ptr,
                           bondCleanPrice);

    aswap2.setEngine(usdLiborCurve);

    showAssetSwapCashFlows(aswap2);

    cout << "\n\nswap2: "
    << "fair spread: " << io::rate(aswap2.fairSpread()) << endl
    << "fair cleanPrice: " <<aswap2.fairCleanPrice() << endl
    << endl
    << endl;

    return 0;
}


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

#ifndef NQS_EXACT_TIME_PROPAGATION_HPP
#define NQS_EXACT_TIME_PROPAGATION_HPP

#include <unordered_map>

#include <Eigen/Dense>

#include "Dynamics/TimeStepper/abstract_time_stepper.hpp"
#include "Operator/operator.hpp"
#include "Output/json_output_writer.hpp"
#include "Stats/binning.hpp"

namespace nqs {

class ExactTimePropagation {
 public:
  using StateVector = Eigen::VectorXcd;
  using Stepper = ode::AbstractTimeStepper<StateVector>;
  using Matrix = std::function<StateVector(const StateVector&)>;
  using StatsMap = std::unordered_map<std::string, Binning<double>::Stats>;

  static Matrix MakeMatrix(const AbstractOperator& op,
                           const std::string& type) {
    if (type == "dense") {
      struct Function {
        Eigen::MatrixXcd matrix;
        StateVector operator()(const StateVector& x) const {
          return matrix * x;
        }
      };
      return Function{op.ToDense()};
    } else if (type == "direct") {
      struct Function {
        const AbstractOperator& matrix;
        StateVector operator()(const StateVector& x) const {
          return matrix.Apply(x);
        }
      };
      return Function{op};
    } else if (type == "sparse") {
      struct Function {
        Eigen::SparseMatrix<Complex> matrix;
        StateVector operator()(const StateVector& x) const {
          return matrix * x;
        }
      };
      return Function{op.ToSparse()};
    } else {
      std::stringstream str;
      str << "Unknown matrix type: " << type;
      throw InvalidInputError(str.str());
    }
  }

  ExactTimePropagation(const AbstractOperator& hamiltonian, Stepper& stepper,
                       double t0, StateVector initial_state,
                       const std::string& matrix_type = "sparse",
                       const std::string& propagation_type = "exact")
      : matrix_{MakeMatrix(hamiltonian, matrix_type)},
        stepper_(stepper),
        t_(t0),
        state_(std::move(initial_state)) {
    if (propagation_type == "imaginary") {
      ode_system_ = [this](const StateVector& x, StateVector& dxdt,
                           double /*t*/) { dxdt.noalias() = -matrix_(x); };
      normalize_ = true;
    } else if (propagation_type == "real") {
      static constexpr const Complex mi{0, -1};
      ode_system_ = [this](const StateVector& x, StateVector& dxdt,
                           double /*t*/) { dxdt.noalias() = mi * matrix_(x); };
      normalize_ = false;
    } else {
      throw InvalidInputError{
          "ExactTimePropagation: propagation_type must be 'real' or "
          "'imaginary'."};
    }
  }

  void AddObservable(const AbstractOperator& observable,
                     const std::string& name,
                     const std::string& matrix_type = "sparse") {
    observables_.emplace_back(name, MakeMatrix(observable, matrix_type));
  }

  void Advance(double dt) {
    // Propagate the state
    stepper_.Propagate(ode_system_, state_, t_, dt);
    if (normalize_) {
      // Renormalize state to prevent unbounded growth in imaginary time
      // propagation
      state_.normalize();
    }
    ComputeObservables(state_);
    t_ += dt;
  }

  void ComputeObservables(const StateVector& state) {
    const auto mean_variance = MeanVariance(matrix_, state);
    observable_stats_["Energy"] = {mean_variance.first.real(), 0.0, 0.0};
    observable_stats_["EnergyVariance"] = {mean_variance.second, 0.0, 0.0};

    for (const auto& entry : observables_) {
      const auto& name = entry.first;
      const auto& obs = entry.second;
      observable_stats_[name] = {Mean(obs, state).real(), 0.0, 0.0};
    }
  }

  const StatsMap& GetObservableStats() const noexcept { return observable_stats_; }

  double GetTime() const noexcept { return t_; }
  void SetTime(double t) { t_ = t; }

  const StateVector &GetState() const noexcept { return state_; }
  void SetState(const StateVector &state) { state_ = state; }

 private:
  Matrix matrix_;
  Stepper& stepper_;
  ode::OdeSystemFunction<StateVector> ode_system_;
  bool normalize_;

  double t_;
  StateVector state_;

  using ObsEntry = std::pair<std::string, Matrix>;
  std::vector<ObsEntry> observables_;
  StatsMap observable_stats_;
};

}  // namespace nqs

#endif  // NQS_EXACT_TIME_PROPAGATION_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_POLY_FRANKLIN_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_POLY_FRANKLIN_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace within
{

/*!
\brief Within detection using cross counting
\ingroup strategies
\tparam Point \tparam_point
\tparam PointOfSegment \tparam_segment_point
\tparam CalculationType \tparam_calculation
\author adapted from Randolph Franklin algorithm
\author Barend and Maarten, 1995
\author Revised for templatized library, Barend Gehrels, 2007
\return true if point is in ring, works for closed rings in both directions
\note Does NOT work correctly for point ON border

\qbk{
[heading See also]
[link geometry.reference.algorithms.within.within_3_with_strategy within (with strategy)]
}
 */

template
<
    typename Point,
    typename PointOfSegment = Point,
    typename CalculationType = void
>
class franklin
{
    typedef typename select_calculation_type
        <
            Point,
            PointOfSegment,
            CalculationType
        >::type calculation_type;

        /*! subclass to keep state */
        class crossings
        {
            bool crosses;

        public :

            friend class franklin;
            inline crossings()
                : crosses(false)
            {}
        };

public :

    typedef Point point_type;
    typedef PointOfSegment segment_point_type;
    typedef crossings state_type;

    static inline bool apply(Point const& point,
            PointOfSegment const& seg1, PointOfSegment const& seg2,
            crossings& state)
    {
        calculation_type const& px = get<0>(point);
        calculation_type const& py = get<1>(point);
        calculation_type const& x1 = get<0>(seg1);
        calculation_type const& y1 = get<1>(seg1);
        calculation_type const& x2 = get<0>(seg2);
        calculation_type const& y2 = get<1>(seg2);

        if (
            ( (y2 <= py && py < y1) || (y1 <= py && py < y2) )
            && (px < (x1 - x2) * (py - y2) / (y1 - y2) + x2)
            )
        {
            state.crosses = ! state.crosses;
        }
        return true;
    }

    static inline int result(crossings const& state)
    {
        return state.crosses ? 1 : -1;
    }
};

}} // namespace strategy::within

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_POLY_FRANKLIN_HPP


/* thread_socket_interface.cpp */

#include "haptics/thread_socket_interface.h"
#include "haptics/UDPSocket.h"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;

using Eigen::Vector3d;
using Eigen::Matrix3d;

UDPSocket receiver = UDPSocket();
UDPSocket sender;
string buf;
char outputString[] = "HEAD%d,%.4f,%.4f,%.4f,%d,TAIL\nHEAD%d,%.4f,%.4f,%.4f,%d,TAIL";
vector<string> vectbuf, vect1, vect2;
int i,j;

void parse(string buf, vector<string> &vect) {
  int front, back;
  // get data segment
  front = buf.find("HEAD") + 6;
  back = buf.find("TAIL", front);
  string data = buf.substr(front, back-front-1);

  boost::split(vect, data, boost::is_any_of(","));
}

void connectionInit() {
  // Initialize sockets
  if(! (receiver.create())) {
		cout << "Error creating socket!" << endl;
		exit(-1);
	}
	if(! (receiver.bind(RECEIVE_PORT))) {
		cout << "Client: error connecting to ip: " << IP << "  port: " << RECEIVE_PORT << endl;
		exit(-1);
	}
  //receiver.set_non_blocking(true);
  receiver.set_timeout(0);             // using recv()

  if(! (sender.create())) {
		cout << "Error creating socket!" << endl;
		exit(-1);
	}
	if(! (sender.bind(SEND_PORT))) {
		cout << "Client: error connecting to ip: " << IP << "  port: " << SEND_PORT << endl;
		exit(-1);
	}
	if(! (sender.setDestination(IP, SEND_PORT))) {
		cout << "error setting destination" << endl;
		exit(-1);
	}
	//sender.set_non_blocking(true);
	sender.set_timeout(0);
}

bool getDeviceState (Vector3d& start_proxy_pos, Matrix3d& start_proxy_rot, bool start_proxybutton[], Vector3d& end_proxy_pos, Matrix3d& end_proxy_rot, bool end_proxybutton[]) {
  if (receiver.recv(buf)) {
    while (receiver.recv(buf));

    boost::split(vectbuf, buf, boost::is_any_of("\n"));
    parse(vectbuf[0], vect1);
    parse(vectbuf[1], vect2);

  	for(i=0; i<2; i++) {
      for(j=0; j<3; j++) {
          start_proxy_rot(j,i) = boost::lexical_cast<double>(vect1[(2-i)*4+j].c_str());
          end_proxy_rot(j,i) 	 = boost::lexical_cast<double>(vect2[(2-i)*4+j].c_str());
      }
  	}
  	for(j=0; j<3; j++) {
    	start_proxy_rot(j,i) = -1 * boost::lexical_cast<double>(vect1[(2-i)*4+j].c_str());
    	end_proxy_rot(j,i) 	 = -1 * boost::lexical_cast<double>(vect2[(2-i)*4+j].c_str());
    }

  	// the three for loops above is equivalent to doing the following, but slightly more efficient.
  	/*
  	for(int i=0; i<3; i++) {
      for(int j=0; j<3; j++) {
          start_proxy_rot(j,i) = boost::lexical_cast<double>(vect1[i*4+j].c_str());
          end_proxy_rot(j,i) 	 = boost::lexical_cast<double>(vect2[i*4+j].c_str());
      }
  	}

  	Matrix3d rot_ztonegx;
  	rot_ztonegx(0,2) = -1;
  	rot_ztonegx(1,1) = 1;
  	rot_ztonegx(2,0) = 1;

  	start_proxy_rot = start_proxy_rot * rot_ztonegx;
  	end_proxy_rot = end_proxy_rot * rot_ztonegx;
  	*/

 		start_proxy_pos(0) = 30 * boost::lexical_cast<double>(vect1[12].c_str());
    end_proxy_pos(0)   = 30 * boost::lexical_cast<double>(vect2[12].c_str());
    start_proxy_pos(1) = 30 * boost::lexical_cast<double>(vect1[13].c_str());
    end_proxy_pos(1)   = 30 * boost::lexical_cast<double>(vect2[13].c_str());
    start_proxy_pos(2) = 60 * boost::lexical_cast<double>(vect1[14].c_str()) - 6;
    end_proxy_pos(2)	 = 60 * boost::lexical_cast<double>(vect2[14].c_str()) - 6;

    start_proxybutton[0] = boost::lexical_cast<int>(vect1[16].c_str());
    start_proxybutton[1] = boost::lexical_cast<int>(vect1[17].c_str());
    end_proxybutton[0]  = boost::lexical_cast<int>(vect2[16].c_str());
    end_proxybutton[1]  = boost::lexical_cast<int>(vect2[17].c_str());

    return true;
  }
  return false;
}

void sendDeviceState (const Vector3d& start_feedback_pos, bool start_feedback_enabled, const Vector3d& end_feedback_pos, bool end_feedback_enabled) {
	char buf[1024];
	memset(&buf, 0, sizeof(buf));
	sprintf((char*)&buf, outputString, 0, start_feedback_pos(0)/30, start_feedback_pos(1)/30, (start_feedback_pos(2)+6)/60, start_feedback_enabled ? 1 : 0,
																		 1, end_feedback_pos(0)/30, 	end_feedback_pos(1)/30, 	(end_feedback_pos(2)+6)/60,  	end_feedback_enabled   ? 1 : 0);
	sender.send(string(buf));
}

void getDeviceState (double start_proxyxform[], bool start_proxybutton[], double end_proxyxform[], bool end_proxybutton[]) {
  if (receiver.recv(buf)) {
    while (receiver.recv(buf));

    boost::split(vectbuf, buf, boost::is_any_of("\n"));
    parse(vectbuf[0], vect1);
    parse(vectbuf[1], vect2);

		//sample configurator
		//double start_proxyxform[16] = {-0.1018,-0.9641,0.2454,0.0000,0.0806,0.2379,0.9680,0.0000,-0.9915,0.1183,0.0534,0.0000,-0.5611,-0.1957,-0.8401,1.0000};
		//double end_proxyxform[16] = {0.5982,0.7791,-0.1877,0.0000,0.3575,-0.0498,0.9326,0.0000,0.7172,-0.6250,-0.3083,0.0000,1.1068,0.2221,-0.7281,1.0000};

    // Extract transform matrix
    for (i=0; i<16; i++) {
        start_proxyxform[i] = boost::lexical_cast<double>(vect1[i].c_str());
        end_proxyxform[i]   = boost::lexical_cast<double>(vect2[i].c_str());
    }
    //cout << vect1[16] << "\t" << vect2[16] << endl;
    start_proxybutton[0] = boost::lexical_cast<int>(vect1[16].c_str());
    start_proxybutton[1] = boost::lexical_cast<int>(vect1[17].c_str());
    end_proxybutton[0]  = boost::lexical_cast<int>(vect2[16].c_str());
    end_proxybutton[1]  = boost::lexical_cast<int>(vect2[17].c_str());
  }
}

void sendDeviceState (double start_feedback_pos[], bool start_feedback_enabled, double end_feedback_pos[], bool end_feedback_enabled) {
	char buf[1024];
	memset(&buf, 0, sizeof(buf));
	sprintf((char*)&buf, outputString, 0, start_feedback_pos[0], start_feedback_pos[1], start_feedback_pos[2], start_feedback_enabled ? 1 : 0,
																		 1, end_feedback_pos[0],   end_feedback_pos[1],   end_feedback_pos[2],   end_feedback_enabled   ? 1 : 0);
	sender.send(string(buf));
}


#pragma once

#include <polyvec/api.hpp>

#include <Eigen/Core>

NAMESPACE_BEGIN(polyfit)
NAMESPACE_BEGIN(mc)

// ======== PUBLIC API

// Read the pixel regions along with their color from a bmp or png file
// also works with png, the name is misleading.
void get_pixel_regions_from_bmp(
  const std::string fname,
  std::vector<Eigen::Matrix2Xi>& regions,
  std::vector<Eigen::Vector4d>& colors);

// ======== Private API -- exposed for testing

// processes the regions
// since it discards the CW regions, out2in is the mapping between
// input/output regions.
void post_process_pixel_regions(
  const std::vector<Eigen::Matrix2Xd> &in,
  std::vector<Eigen::Matrix2Xd> &out,
  std::vector<int> & out2in );

NAMESPACE_END(mc)
NAMESPACE_END(polyfit)


#include <Eigen/Core>
USING_PART_OF_NAMESPACE_EIGEN
using namespace std;

template<typename Derived>
const Eigen::Eval<Eigen::Transpose<Derived> >
evaluatedTranspose(const MatrixBase<Derived>& m)
{
  return m.transpose().eval();
}

int main(int, char**)
{
  Matrix2f M = Matrix2f::random();
  Matrix2f m;
  m = M;
  cout << "Here is the matrix m:" << endl << m << endl;
  cout << "Now we want to replace m by its own transpose." << endl;
  cout << "If we do m = m.transpose(), then m becomes:" << endl;
  m = m.transpose();
  cout << m << endl << "which is wrong!" << endl;
  cout << "Now let us instead do m = evaluatedTranspose(m). Then m becomes" << endl;
  m = M;
  m = evaluatedTranspose(m);
  cout << m << endl << "which is right." << endl;

  return 0;
}


/** Test cases for Segment class.
 *
 */

#include "tmfsc/simulator.h"

#ifndef LINK_STATIC
#define BOOST_TEST_DYN_LINK
#endif
#define BOOST_TEST_MODULE SimulatorTest
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <memory>

using namespace quest::tmfsc;
using namespace std;

BOOST_AUTO_TEST_CASE(basics)
{

    point A = {0, 0};
    point B = {20, 0};
    point C = {20, 20};
    point D = {0, 20};

    Device::ptr dev = make_shared<Device>();
    dev->addPoint(A);
    dev->addPoint(B);
    dev->addPoint(C);
    dev->addPoint(D);

    dev->addEdge(0, 1);
    dev->addEdge(1, 2);
    dev->addEdge(2, 3);
    dev->addEdge(3, 0);

    dev->edgeType(1, Edge::EDGE_ABSORB);

    Simulator sim(dev);
    sim.setMaxNumStepsPerTraj(100);

    point p = {10, 5};
    TrajectoryVect trajs = sim.calcTraj(p, 0.0, 10, 0, 0.15);
    cout << " *** TRAJECTORY *** " << endl;
    for (auto traj : trajs) {
        for (int ip = 0; ip < traj.path.size(); ip += 1) {
            cout << "[" << ip << "]" << traj.path[ip] << endl;
        }
    }

}


#ifndef LBP_CSLBP_HPP
#define LBP_CSLBP_HPP

#include <lbp/defs.hpp>
#include <lbp/utils.hpp>
#include <lbp/detail/neighborhoods.hpp>
#include <lbp/detail/sampling.hpp>

#include <opencv2/core.hpp>

#include <boost/hana/fold.hpp>
#include <boost/hana/tuple.hpp>

#include <boost/integer.hpp>

//
// @article{Heikkila:2009:DIR:1456722.1456979,
//  author = {Heikkil\"{a}, Marko and Pietik\"{a}inen, Matti and Schmid, Cordelia},
//  title = {Description of Interest Regions with Local Binary Patterns},
//  journal = {Pattern Recogn.},
//  issue_date = {March, 2009},
//  volume = {42},
//  number = {3},
//  month = mar,
//  year = {2009},
//  issn = {0031-3203},
//  pages = {425--436},
//  numpages = {12},
//  url = {http://dx.doi.org/10.1016/j.patcog.2008.08.014},
//  doi = {10.1016/j.patcog.2008.08.014},
//  acmid = {1456979},
//  publisher = {Elsevier Science Inc.},
//  address = {New York, NY, USA},
//  keywords = {Image matching, Local binary patterns, Object recognition, Region description, Region detection, SIFT},
// }
//

namespace lbp {
namespace cslbp_detail {

template< typename T >
auto cslbp = [](auto neighborhood, auto sampler) {
    return [=](const cv::Mat& src, size_t i, size_t j, const T& epsilon) {
        using namespace cv;
        using namespace hana::literals;

        return boost::hana::fold (
            neighborhood, 0, [&, S = 0](auto accum, auto x) mutable {
                const auto a = sampler (src, i + x [0_c], j + x [1_c]);
                const auto b = sampler (src, i - x [0_c], j - x [1_c]);
                return accum + ((a >= saturate_cast< T > (b + epsilon)) << S++);
            });
    };
};

} // namespace cslbp_detail

template< typename T, size_t R, size_t P >
auto cslbp = [](const cv::Mat& src, const T& epsilon = T { }) {
    LBP_STATIC_ASSERT_MSG (0 == (P % 2), "odd-sized neighborhood");

    using value_type = typename boost::uint_t< P/2 >::least;

    cv::Mat dst (src.size (), opencv_type< value_type >, cv::Scalar (0));

    auto op = cslbp_detail::cslbp< T > (
        detail::semicircular_neighborhood< R, P >,
        detail::nearest_sampler< T >);

#pragma omp parallel for
    for (size_t i = R; i < src.rows - R; ++i) {
        for (size_t j = R; j < src.cols - R; ++j) {
            dst.at< value_type > (i, j) = op (src, i, j, epsilon);
        }
    }

    return dst;
};

} // namespace lbp

#endif // LBP_CSLBP_HPP


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// ---------------------------------------------------------------------------

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE symmetric group magic
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <fstream>
#include <algorithm>
#include <set>

#include <permlib/permutation.h>

#include "test-common.h"
#include <permlib/symmetric_group.h>
#include <permlib/change/deterministic_base_transpose.h>
#include <permlib/change/conjugating_base_change.h>

using namespace permlib;

BOOST_AUTO_TEST_CASE( symmetric_group )
{
	typedef Permutation PERM;
	typedef SymmetricGroup<PERM>::TRANS TRANS;
	typedef DeterministicBaseTranspose<PERM,TRANS> BASETRANSPOSE;

	const unsigned long n = 19;

	SymmetricGroup<PERM> s_n(n);

	BOOST_REQUIRE( s_n.B.size() >= n-1 );

	for (unsigned int i=0; i<n; ++i) {
		PERM* p = s_n.U[0].at(i);
		BOOST_REQUIRE( p );
		BOOST_CHECK( *p / s_n.B[0] == i );
		BOOST_CHECK( *p / i == s_n.B[0] );
		delete p;
	}

	ConjugatingBaseChange<PERM, TRANS, BASETRANSPOSE> cbc(s_n);
	const unsigned long newBaseLength = 5;
	unsigned long newBase[newBaseLength] = { 3, 1, 0, 9, 17 };
	cbc.change(s_n, newBase, newBase+newBaseLength);

	BOOST_REQUIRE( s_n.B.size() >= n-1 );

	for (unsigned int i=0; i<newBaseLength; ++i) {
		BOOST_CHECK(s_n.B[i] == newBase[i]);
	}

	std::set<unsigned long> seenBaseElements;
	for (unsigned int i=0; i<n; ++i) {
		seenBaseElements.insert(s_n.B[i]);
	}

	BOOST_CHECK( s_n.B.size() == seenBaseElements.size() );
}


// Copyright (C) 2017-2019 Chris N. Richardson and Garth N. Wells
//
// This file is part of FEniCS-miniapp (https://www.fenicsproject.org)
//
// SPDX-License-Identifier:    MIT

#include "elasticity_problem.h"
#include "mesh.h"
#include "poisson_problem.h"
#include <boost/program_options.hpp>
#include <dolfinx/common/Timer.h>
#include <dolfinx/common/subsystem.h>
#include <dolfinx/common/timing.h>
#include <dolfinx/common/version.h>
#include <dolfinx/fem/Form.h>
#include <dolfinx/fem/Function.h>
#include <dolfinx/fem/FunctionSpace.h>
#include <dolfinx/fem/utils.h>
#include <dolfinx/io/XDMFFile.h>
#include <dolfinx/la/Vector.h>
#include <string>
#include <utility>
#include <petscsys.h>

namespace po = boost::program_options;

void solve(int argc, char* argv[])
{
  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print usage message")(
      "problem_type", po::value<std::string>()->default_value("poisson"),
      "problem (poisson or elasticity)")(
      "mesh_type", po::value<std::string>()->default_value("cube"),
      "mesh (cube or unstructured)")(
      "scaling_type", po::value<std::string>()->default_value("weak"),
      "scaling (weak or strong)")(
      "output", po::value<std::string>()->default_value(""),
      "output directory (no output unless this is set)")(
      "ndofs", po::value<std::size_t>()->default_value(50000),
      "number of degrees of freedom")(
      "order", po::value<std::size_t>()->default_value(1), "polynomial order");

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv)
                .options(desc)
                .allow_unregistered()
                .run(),
            vm);
  po::notify(vm);

  if (vm.count("help"))
  {
    std::cout << desc << "\n";
    return;
  }

  const std::string problem_type = vm["problem_type"].as<std::string>();
  const std::string mesh_type = vm["mesh_type"].as<std::string>();
  const std::string scaling_type = vm["scaling_type"].as<std::string>();
  const std::size_t ndofs = vm["ndofs"].as<std::size_t>();
  const int order = vm["order"].as<std::size_t>();
  const std::string output_dir = vm["output"].as<std::string>();
  const bool output = (output_dir.size() > 0);

  bool strong_scaling;
  if (scaling_type == "strong")
    strong_scaling = true;
  else if (scaling_type == "weak")
    strong_scaling = false;
  else
    throw std::runtime_error("Scaling type '" + scaling_type + "` unknown");

  // Get number of processes
  const std::size_t num_processes = dolfinx::MPI::size(MPI_COMM_WORLD);

  // Assemble problem
  std::shared_ptr<dolfinx::mesh::Mesh> mesh;
  std::shared_ptr<dolfinx::la::Vector<PetscScalar>> b;
  std::shared_ptr<dolfinx::fem::Function<PetscScalar>> u;
  std::function<int(dolfinx::fem::Function<PetscScalar>&,
                    const dolfinx::la::Vector<PetscScalar>&)>
      solver_function;

  const int ndofs_per_node = (problem_type == "elasticity") ? 3 : 1;

  dolfinx::common::Timer t0("ZZZ Create Mesh");
  if (mesh_type == "cube")
  {
    mesh = std::make_shared<dolfinx::mesh::Mesh>(create_cube_mesh(
        MPI_COMM_WORLD, ndofs, strong_scaling, ndofs_per_node, order));
  }
  else
  {
    mesh = create_spoke_mesh(MPI_COMM_WORLD, ndofs, strong_scaling,
                             ndofs_per_node);
  }
  t0.stop();

  dolfinx::common::Timer t_ent("ZZZ Create facets and facet->cell connectivity");
  mesh->topology_mutable().create_entities(2);
  mesh->topology_mutable().create_connectivity(2, 3);
  t_ent.stop();

  if (problem_type == "poisson")
  {
    // Create Poisson problem
    std::tie(b, u, solver_function) = poisson::problem(mesh, order);
  }
  else if (problem_type == "elasticity")
  {
    // Create elasticity problem. Near-nullspace will be attached to the
    // linear operator (matrix).
    std::tie(b, u, solver_function) = elastic::problem(mesh, order);
  }
  else
    throw std::runtime_error("Unknown problem type: " + problem_type);

  // Print simulation summary
  if (dolfinx::MPI::rank(MPI_COMM_WORLD) == 0)
  {
    char petsc_version[256];
    PetscGetVersion(petsc_version, 256);

    const std::int64_t num_dofs
        = u->function_space()->dofmap()->index_map->size_global()
          * u->function_space()->dofmap()->index_map_bs();
    const int tdim = mesh->topology().dim();
    const std::int64_t num_cells
        = mesh->topology().index_map(tdim)->size_global();
    std::cout
        << "----------------------------------------------------------------"
        << std::endl;
    std::cout << "Test problem summary" << std::endl;
    std::cout << "  dolfinx version: " << DOLFINX_VERSION_STRING << std::endl;
    std::cout << "  dolfinx hash:    " << DOLFINX_VERSION_GIT << std::endl;
    std::cout << "  ufl hash:        " << UFC_SIGNATURE << std::endl;
    std::cout << "  petsc version:   " << petsc_version << std::endl;
    std::cout << "  Problem type:    " << problem_type << std::endl;
    std::cout << "  Scaling type:    " << scaling_type << std::endl;
    std::cout << "  Num processes:   " << num_processes << std::endl;
    std::cout << "  Num cells        " << num_cells << std::endl;
    std::cout << "  Total degrees of freedom:               " << num_dofs
              << std::endl;
    std::cout << "  Average degrees of freedom per process: "
              << num_dofs / dolfinx::MPI::size(MPI_COMM_WORLD) << std::endl;
    std::cout
        << "----------------------------------------------------------------"
        << std::endl;
  }

  dolfinx::common::Timer t5("ZZZ Solve");
  int num_iter = solver_function(*u, *b);
  t5.stop();

  if (output)
  {
    dolfinx::common::Timer t6("ZZZ Output");
    std::string filename
        = output_dir + "/solution-" + std::to_string(num_processes) + ".xdmf";
    dolfinx::io::XDMFFile file(MPI_COMM_WORLD, filename, "w");
    file.write_mesh(*mesh);
    file.write_function(*u, 0.0);
    t6.stop();
  }

  // Display timings
  dolfinx::list_timings(MPI_COMM_WORLD, {dolfinx::TimingType::wall});

  // Report number of Krylov iterations
  double norm = u->x()->norm();
  if (dolfinx::MPI::rank(MPI_COMM_WORLD) == 0)
  {
    std::cout << "*** Number of Krylov iterations: " << num_iter << std::endl;
    std::cout << "*** Solution norm:  " << norm << std::endl;
  }
}

int main(int argc, char* argv[])
{
  dolfinx::common::subsystem::init_logging(argc, argv);
  dolfinx::common::subsystem::init_mpi();
  dolfinx::common::subsystem::init_petsc(argc, argv);

  solve(argc, argv);

  dolfinx::common::subsystem::finalize_petsc();
  dolfinx::common::subsystem::finalize_mpi();
  return 0;
}


/**
 * Created by Beck on July 8th 2018
 * Indirect Extended Kalman Filter for the translation of the camera
 * fusing visual information with the accelerometer, given the pose;
 * With imu in 400Hz and camera translation in 30Hz
 * V1: estimation for rotation only
 * V2: give the translation of the shield, added acc bias
 * V3: estimate the translation and rotation
 */
#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <queue>
//#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;

ros::Publisher pose_pub, debug_pub, odom_pub, debug_propagate_pub;
string imu_topic, pose_topic, visual_topic;
string debug_topic, publisher_topic;
double acc_weight, acc_bias_weight;
double gravity_q_weight, visual_t_weight;
int sleep_time;

/**
 * Define states:
 *      x = [quaternion w, x, y, z; translation_x, y, z; velocity_x, y, z; acc bias; gyro bias]
 * Define inputs:
 *      u = [acc; gyro], raw accelerometer, gyroscope
 * Define noises:
 *      n = [n_acc, n_gyro, n_bias_acc, n_bias_gyro]
 */
VectorXd x(16);         // state
MatrixXd P = MatrixXd::Identity(15, 15); // covariance
MatrixXd R = MatrixXd::Identity(12, 12); // prediction noise covariance
MatrixXd Q = MatrixXd::Identity(6, 6); // observation noise covariance

// buffers to save gyro and visual reading
queue<sensor_msgs::Imu::ConstPtr> imu_buf;
queue<geometry_msgs::TwistStamped::ConstPtr> visual_buf;
queue<Matrix<double, 16, 1>> x_history;
queue<Matrix<double, 15, 15>> P_history;
Vector3d G = {0, 0, 9.8}; // Consider to add initialization later

double t_prev;          // previous propagated time

// Initialization
const int IMU_INIT_COUNT = 10;
const int MAX_GYRO_QUEUE_SIZE = 400;
const double IMU_UPDATE_TIME = 0.0025; // 1 / 400Hz
int imu_count  = 0;
bool imu_initialized = false;
bool visual_initialized = false;
bool visual_valid = false;
MatrixXd imu_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
Vector3d imu_T_camera = MatrixXd::Zero(3, 1);

//// DEBUG only
double t_prev_update;   // previous update time
Vector3d world_T_shield_prev = MatrixXd::Zero(3, 1); // previous visual translation
Vector3d a_int = MatrixXd::Zero(3, 1);
Vector3d a_int_int = MatrixXd::Zero(3, 1);

void pub_shield_odom(const std_msgs::Header& header)
{
    nav_msgs::Odometry odom;
    odom.header = header;
    odom.child_frame_id = "world";

    odom.pose.pose.orientation.w = x(0);
    odom.pose.pose.orientation.x = x(1);
    odom.pose.pose.orientation.y = x(2);
    odom.pose.pose.orientation.z = x(3);
    odom.pose.pose.position.x = x(4);
    odom.pose.pose.position.y = x(5);
    odom.pose.pose.position.z = x(6);
    odom.twist.twist.linear.x = x(7);
    odom.twist.twist.linear.y = x(8);
    odom.twist.twist.linear.z = x(9);

    odom.pose.covariance[0]  = P(0, 0);
    odom.pose.covariance[7]  = P(1, 1);
    odom.pose.covariance[14] = P(2, 2);
    odom.pose.covariance[21] = P(3, 3);
    odom.pose.covariance[28] = P(4, 4);
    odom.pose.covariance[35] = P(5, 5);
    odom.pose.covariance[3]  = P(0, 3);
    odom.pose.covariance[10] = P(1, 4);
    odom.pose.covariance[17] = P(2, 5);
    odom.pose.covariance[18] = P(3, 0);
    odom.pose.covariance[25] = P(4, 1);
    odom.pose.covariance[32] = P(5, 2);
    odom_pub.publish(odom);
}

void pub_debug_update(const std_msgs::Header& header,
                      const Ref<const Vector3d> p,
                      const Quaterniond& q,
                      double t_update)
{
    nav_msgs::Odometry odom;
    odom.header = header;
    odom.child_frame_id = "world";
    odom.pose.pose.position.x = p[0];
    odom.pose.pose.position.y = p[1];
    odom.pose.pose.position.z = p[2];
    odom.pose.pose.orientation.w = q.w();
    odom.pose.pose.orientation.x = q.x();
    odom.pose.pose.orientation.y = q.y();
    odom.pose.pose.orientation.z = q.z();
    if (visual_initialized) {
        double dt_update = t_update - t_prev_update;
        odom.twist.twist.linear.x = (p[0] - world_T_shield_prev[0]) / dt_update;
        odom.twist.twist.linear.y = (p[1] - world_T_shield_prev[1]) / dt_update;
        odom.twist.twist.linear.z = (p[2] - world_T_shield_prev[2]) / dt_update;

        world_T_shield_prev = p;
        t_prev_update = t_update;
    }
    else {
        odom.twist.twist.linear.x = 0;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.linear.z = 0;
        world_T_shield_prev = p;
        t_prev_update = t_update;
    }

    debug_pub.publish(odom);
}

void pub_debug_propagate(const std_msgs::Header& header,
                         const Ref<const Vector3d> acc_world,
                         const Ref<const Vector3d> a_int)
{
    geometry_msgs::TwistStamped acc_compare;
    acc_compare.header = header;
    acc_compare.twist.linear.x = acc_world[0];
    acc_compare.twist.linear.y = acc_world[1];
    acc_compare.twist.linear.z = acc_world[2];
    acc_compare.twist.angular.x= a_int[0];
    acc_compare.twist.angular.y= a_int[1];
    acc_compare.twist.angular.z= a_int[2];
    debug_propagate_pub.publish(acc_compare);
}

void propagate(const sensor_msgs::Imu &imu)
{
    double cur_t = imu.header.stamp.toSec();
    Vector3d a, a_x, acc_wo_g, w, w_x;
    a(0) = imu.linear_acceleration.x;
    a(1) = imu.linear_acceleration.y;
    a(2) = imu.linear_acceleration.z;
    w(0) = imu.angular_velocity.x;
    w(1) = imu.angular_velocity.y;
    w(2) = imu.angular_velocity.z;
    a_x = a - x.segment<3>(10);
    w_x = w - x.segment<3>(13);
    Quaterniond world_R_imu(x(0), x(1), x(2), x(3));

    double dt = cur_t - t_prev;
    dt = (dt < IMU_UPDATE_TIME * 5) ? dt : IMU_UPDATE_TIME * 5;
    ROS_INFO("dt in propagate is %f, at the cur_t %f", dt, cur_t);
    // acc_wo_g = world_R_imu.toRotationMatrix() * (a - x.segment<3>(6)) - G;
    acc_wo_g = a_x - world_R_imu.toRotationMatrix().transpose() * G;
    x.segment<3>(4) += x.segment<3>(7) * dt + 0.5 * acc_wo_g * dt * dt;
    x.segment<3>(7) += acc_wo_g * dt;

    a_int_int += a_int * dt + 0.5 * acc_wo_g * dt * dt;
    a_int += acc_wo_g * dt;
    pub_debug_propagate(imu.header, acc_wo_g, a_int);

    Vector3d domg = w_x * dt / 2;
    Quaterniond dR(sqrt(1 - domg.squaredNorm()), domg(0), domg(1), domg(2));
    Quaterniond R_now;
    R_now = (world_R_imu * dR).normalized();
    x.segment<4>(0) << R_now.w(), R_now.x(), R_now.y(), R_now.z();

    Matrix3d R_w_x, R_a_x;
    R_w_x << 0, -w_x(2),  w_x(1),
             w_x(2),  0, -w_x(0),
            -w_x(1), w_x(0), 0;
    R_a_x << 0, -a_x(2),  a_x(1),
             a_x(2),  0, -a_x(0),
            -a_x(1), a_x(0), 0;

    MatrixXd A = MatrixXd::Zero(15, 15);
    A.block<3, 3>(0, 0) = -R_w_x;
    A.block<3, 3>(0, 12)= -MatrixXd::Identity(3, 3);
    A.block<3, 3>(3, 6) =  MatrixXd::Identity(3, 3);
    A.block<3, 3>(6, 0) = -R_a_x;
    A.block<3, 3>(6, 9) = -world_R_imu.toRotationMatrix();

    MatrixXd U = -MatrixXd::Zero(15, 12);
    U.block<3, 3>(0, 0) = -MatrixXd::Identity(3, 3);
    U.block<3, 3>(6, 3) = -world_R_imu.toRotationMatrix();
    U.block<3, 3>(9, 6) =  MatrixXd::Identity(3, 3);
    U.block<3, 3>(12,9) =  MatrixXd::Identity(3, 3);

    MatrixXd F, V;
    F = MatrixXd::Identity(15, 15) + dt * A;
    V = dt * U;
//    cout << "F " << endl << F << endl;
//    cout << "R " << endl << R << endl;
//    cout << "V " << endl << V << endl;
    P = F * P * F.transpose() + V * R * V.transpose();

    t_prev = cur_t;
//    cout << "P " << endl << P << endl;
//    cout << "x " << endl << x.transpose() << endl;
}

static void throwState(double current_time)
{
    while(imu_buf.size() > 1 &&
          imu_buf.front()->header.stamp.toSec() < current_time)
    {
        // trace backward the time to the imu timestamp
        t_prev = imu_buf.front()->header.stamp.toSec();
        ROS_INFO("throw state with time: %f", t_prev);
        imu_buf.pop();
        x_history.pop();
        P_history.pop();
    }
}

static void repropagate()
{
    while (!x_history.empty()) x_history.pop();
    while (!P_history.empty()) P_history.pop();

    queue <sensor_msgs::Imu::ConstPtr> temp_imu_buf;
    while (!imu_buf.empty() && imu_buf.size() > 1)
    {
        propagate(*imu_buf.front());
        temp_imu_buf.push(imu_buf.front());
        x_history.push(x);
        P_history.push(P);
        imu_buf.pop();
    }
    if (!temp_imu_buf.empty()) {
        pub_shield_odom(temp_imu_buf.back()->header);
    }
    swap(imu_buf, temp_imu_buf);
}

static void update(const geometry_msgs::TwistStamped &pnp)
{
    double cur_t = pnp.header.stamp.toSec();
    Vector3d camera_T_shield;

    camera_T_shield[0] = pnp.twist.linear.x;
    camera_T_shield[1] = pnp.twist.linear.y;
    camera_T_shield[2] = pnp.twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    imu_T_shield *= 0.001; // Convert millimeter to meter

    throwState(cur_t);

    ROS_INFO("Update, at time %f", cur_t);
    Quaterniond world_R_imu;

    if (imu_buf.empty()) {
        world_R_imu.setIdentity();
    }
    else {
        world_R_imu = Quaterniond(imu_buf.front()->orientation.w,
                                  imu_buf.front()->orientation.x,
                                  imu_buf.front()->orientation.y,
                                  imu_buf.front()->orientation.z);
    }
    Vector3d world_T_shield = world_R_imu.toRotationMatrix() * imu_T_shield;
    // pub_debug_update(pnp.header, world_T_shield, world_R_imu, cur_t);
    pub_debug_update(pnp.header, imu_T_shield, world_R_imu, cur_t);

    MatrixXd C = MatrixXd::Zero(6, 15);
    C.block<3, 3>(0, 0) = MatrixXd::Identity(3, 3);
    C.block<3, 3>(3, 3) = MatrixXd::Identity(3, 3);

    // Matrix3d W = MatrixXd::Identity(6, 6);

    MatrixXd K(15, 6);
    K = P * C.transpose() * (C * P * C.transpose() + Q).inverse();
//    cout << "C " << endl << C << endl;
//    cout << "K " << endl << K << endl;
//    cout << "Q " << endl << Q << endl;
    // x = x + K * (world_T_shield - C * x);
    // x = x + K * (imu_T_shield - C * x);

    VectorXd r(6);
    // Quaterniond qm(world_R_imu);
    Quaterniond q  = Quaterniond(x(0), x(1), x(2), x(3));
    Quaterniond dq = q.conjugate() * world_R_imu;
    r.head<3>() = 2 * dq.vec();
    r.tail<3>() = imu_T_shield - x.segment<3>(4);
    VectorXd _r = K * r;
    Vector3d dw(0.5 * _r(0), 0.5 * _r(1), 0.5 * _r(2));
    dq = Quaterniond(1, dw(0), dw(1), dw(2)).normalized();
    q = q * dq;

    x(0) = q.w();
    x(1) = q.x();
    x(2) = q.y();
    x(3) = q.z();

    x.segment<12>(4) += _r.tail(12);

    P = P - K * C * P;
//    cout << "P " << endl << P << endl;
//    cout << "x " << endl << x.transpose() << endl;

    if (imu_buf.size() > 1) {
        // repropagate();
    }
}

/**
 * initialize the imu messages
 * @param imu
 */
static void initialize_imu(const sensor_msgs::Imu::ConstPtr &imu)
{
    t_prev = imu->header.stamp.toSec();
    x_history.push(x);
    P_history.push(P);
    imu_buf.push(imu);
    imu_count++;
    if (imu_count == IMU_INIT_COUNT) {
        imu_initialized = true;
    }
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    double cur_t = pnp->header.stamp.toSec();
    Vector3d camera_T_shield;
    ROS_INFO("visual init at %f", cur_t);

    camera_T_shield[0] = pnp->twist.linear.x;
    camera_T_shield[1] = pnp->twist.linear.y;
    camera_T_shield[2] = pnp->twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    imu_T_shield *= 0.001; // Convert millimeter to meter

    throwState(cur_t);

    Quaterniond world_R_imu;

    if (imu_buf.empty()) {
        world_R_imu.setIdentity();
    }
    else {
        world_R_imu = Quaterniond(imu_buf.front()->orientation.w,
                                  imu_buf.front()->orientation.x,
                                  imu_buf.front()->orientation.y,
                                  imu_buf.front()->orientation.z);
    }
    // Vector3d world_T_shield = world_R_imu.toRotationMatrix() * imu_T_shield;
    pub_debug_update(pnp->header, imu_T_shield, world_R_imu, cur_t);

    x.setZero();
    x.segment<4>(0) << world_R_imu.w(), world_R_imu.x(), world_R_imu.y(), world_R_imu.z();
    x.segment<3>(4) << imu_T_shield(0), imu_T_shield(1), imu_T_shield(2);

    cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;

    visual_initialized = true;

    // repropagate();
}

/**
 * handle, save, and process visual messages
 * @param pnp
 */
void visual_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    visual_valid = !(pnp->twist.linear.x == 0 &&
        pnp->twist.linear.y == 0 &&
        pnp->twist.linear.z == 0 );

    if (visual_valid) {
        if (visual_initialized) {
            update(*pnp);
        }
        else {
            initialize_visual(pnp);
        }
    }
}

/**
 * handle 400Hz raw imu data
 * @param imu
 */
void imu_callback(const sensor_msgs::Imu::ConstPtr &imu)
{
    if (!imu_initialized) {
        initialize_imu(imu);
    }
    else {
        propagate(*imu);
        x_history.push(x);
        P_history.push(P);
        imu_buf.push(imu);
		if (imu_buf.size() > MAX_GYRO_QUEUE_SIZE) {
			x_history.pop();
			P_history.pop();
			imu_buf.pop();
		}
        pub_shield_odom(imu->header);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "visual_gyro_fused");
    ros::NodeHandle n("~");

//    n.param("imu_raw", imu_topic, string("/dji_sdk/imu")); // 400Hz
    n.param("imu_pose", pose_topic, string("/attitude_estimator/imu")); // 400Hz
    n.param("visual_topic", visual_topic, string("/pnp_twist"));
    n.param("publisher_topic", publisher_topic, string("/visual_ekf/shield_T_world"));
    n.param("debug_topic", debug_topic, string("/visual_ekf/debug_odom"));
    n.param("accelerometer_noise_weight", acc_weight, 1000.0);
    n.param("accelerometer_bias_noise_weight", acc_bias_weight, 100.0);
    n.param("gravity_pose_weight", gravity_q_weight, 10.0);
    n.param("visual_translation_weight", visual_t_weight, 10.0);
    n.param("node_sleep_time", sleep_time, 0);

    ros::Duration(sleep_time).sleep();

    // TODO: initalize the R and Q matrix
    R.topLeftCorner(6, 6)     = acc_weight * MatrixXd::Identity(6, 6); // gyro, accelerometer noise
    R.bottomRightCorner(6, 6) = acc_bias_weight * MatrixXd::Identity(6, 6); // accelerometer bias, gyro bias noise
    Q.topLeftCorner(3, 3)     = gravity_q_weight * MatrixXd::Identity(3, 3); // orientation noise
    Q.bottomRightCorner(3, 3) = visual_t_weight * MatrixXd::Identity(3, 3); // position noise

    x.setZero();

    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;

    imu_T_camera <<  200, 50, 0; // in millimeter

    ros::Subscriber s2 = n.subscribe(visual_topic, 10, visual_callback);
    ros::Subscriber s3 = n.subscribe(pose_topic, 100, imu_callback);
//    pose_pub = n.advertise<geometry_msgs::PoseStamped>(publisher_topic, 100);
    odom_pub = n.advertise<nav_msgs::Odometry>(publisher_topic, 100);
    debug_pub= n.advertise<nav_msgs::Odometry>(debug_topic, 100);
    debug_propagate_pub = n.advertise<geometry_msgs::TwistStamped>(string("/visual_ekf/propagate"), 100);

    ros::Rate r(100);
    ros::spin();
}

/**
 *  0   q_w     rotation from imu to world
 *  1   q_x
 *  2   q_y
 *  3   q_z
 *  4   T_x     translation of the shield in imu frame
 *  5   T_y
 *  6   T_z
 *  7   v_x     velocity of the shield in imu frame
 *  8   v_y
 *  9   v_z
 *  10  ba_x    bias in accelerometer
 *  11  ba_y
 *  12  ba_z
 *  13  bw_x    bias in gyroscope
 *  14  bw_y
 *  15  bw_z
 */


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace std;
    using mtl::dense2D; using mtl::dense_vector;

    dense2D<double> *A;
    A = new dense2D<double>[2];
    A[0] = dense2D<double>(3,3);

    A[1] = dense2D<double>(4,4);
    A[0]= 0;
    A[1]= 0;
    cout << A[0] << endl;
    cout << A[1] << endl;
    delete[] A;

    dense_vector<double> *x;
    x = new dense_vector<double>[2];
    x[0] = dense_vector<double>(3);
    x[1] = dense_vector<double>(4);
    x[0] = 0;
    x[1] = 0;
    // x[1] = dense_vector<double>(5); // Must throw an exception because
    cout << x[0] << endl;
    cout << x[1] << endl;
    delete[] x;

    return 0;
}


#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#define USE_PERIODIC_FFT
#include <imgproc/derivative_gradient.hpp>
#include <imgproc/susan.hpp>
#include <imgproc/rcmg.hpp>
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/quadratureSF.hpp>
#include <imgproc/quadratureLGF.hpp>
#include <imgproc/pc_sqf.hpp>
#include <imgproc/pc_lgf.hpp>
#include <imgproc/pc_matlab.hpp>
#include <imgproc/laplace.hpp>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

using namespace lsfm;
using namespace std;
namespace fs = boost::filesystem;

constexpr int runs = 1;

constexpr int ENTRY_SQR = 1;
constexpr int ENTRY_RGB = 2;
constexpr int ENTRY_NO_3 = 4;
constexpr int ENTRY_NO_5 = 8;

typedef double FT;

constexpr FT mag_th = static_cast<FT>(0.05);

struct Entry {
    Entry() {}

    Entry(const cv::Ptr<FilterI<uchar>>& a, const std::string& b, int f = 0)
        : filter(a), name(b), flags(f) {}

    cv::Ptr<FilterI<uchar>> filter;
    std::string name;
    int flags;

    Value value(const std::string &name) {
        return dynamic_cast<ValueManager*>(&(*filter))->value(name);
    }

    void value(const std::string &name, const Value &v) {
        dynamic_cast<ValueManager*>(&(*filter))->value(name, v);
    }

    inline bool rgb() const {
        return flags & ENTRY_RGB;
    }

    inline bool sqr() const {
        return flags & ENTRY_SQR;
    }

    inline int border() const {
        if (flags & ENTRY_NO_3)
            return 2;
        if (flags & ENTRY_NO_5)
            return 3;
        return 0;
    }

    inline lsfm::FilterResults process(const cv::Mat src) {
        filter->process(src);
        return filter->results();
    }
};

inline cv::Vec3b dirColor(FT val) {
    static FT r = static_cast<FT>(CV_PI / 4);
    static cv::Vec3b colors[] = { cv::Vec3b(255,0  ,150),
                                  cv::Vec3b(255,0  ,255),
                                  cv::Vec3b(0  ,0  ,255),
                                  cv::Vec3b(0  ,150,255),
                                  cv::Vec3b(0  ,255,255),
                                  cv::Vec3b(0  ,255,0),
                                  cv::Vec3b(255,255,0),
                                  cv::Vec3b(255,0  ,0),
                                  cv::Vec3b(255,0  ,150) };
    FT div = val / r;
    int divi = static_cast<int>(div);
    cv::Vec<FT, 3> a = colors[divi + 4], b = colors[divi + 4 + (val < 0 ? -1 : 1)];
    /*cv::Vec<FT, 3> c = b - a;
    FT mul = abs(div) - abs(divi);
    cv::Vec<FT, 3> d = c * mul;
    cv::Vec3b res = a + d;
    return res;*/
    return  a + (b - a) * (abs(div) - abs(divi));
}

void saveDir(cv::Mat &data, const std::string &name, const cv::Mat &mask = cv::Mat()) {
    cv::Mat tmp(data.size(), CV_8UC3);
    cv::Vec3b *pdst = tmp.ptr<cv::Vec3b>();
    if (data.isContinuous() && data.isContinuous()) {
        const FT *pd = data.ptr<FT>();
        int size = data.rows * data.cols;
        for (int j = 0; j != size; ++j) {
            pdst[j] = dirColor(pd[j]);
        }
    }
    else {
        for (int i = 0; i != data.rows; ++i, pdst += data.cols) {
            const FT *pd = data.ptr<FT>(i);
            for (int j = 0; j != data.cols; ++j) {
                pdst[j] = dirColor(pd[j]);
            }
        }
    }
    if (!mask.empty())
        tmp.setTo(cv::Scalar(0, 0, 0), mask < mag_th);
    cv::imwrite("./grad/" + name + ".png", tmp);
}

void savePhase(cv::Mat &data, const std::string &name, const cv::Mat &energy) {
    double vmin, vmax;
    cv::minMaxIdx(energy, &vmin, &vmax);
    cv::Mat en = energy / vmax;

    cv::Mat tmp(data.size(), CV_8UC3);
    cv::Vec3b *pdst = tmp.ptr<cv::Vec3b>();
    if (data.isContinuous() && en.isContinuous()) {
        const FT *pd = data.ptr<FT>();
        const FT *pe = en.ptr<FT>();
        int size = data.rows * data.cols;
        for (int j = 0; j != size; ++j) {
            pdst[j] = dirColor(pd[j]) * pe[j];
        }
    }
    else {
        for (int i = 0; i != data.rows; ++i, pdst += data.cols) {
            const FT *pd = data.ptr<FT>(i);
            const FT *pe = en.ptr<FT>(i);
            for (int j = 0; j != data.cols; ++j) {
                pdst[j] = dirColor(pd[j]) * pe[j];
            }
        }
    }
    cv::imwrite("./grad/" + name + ".png", tmp);
}

void saveNormalized(cv::Mat &data, const std::string &name) {
    double vmin, vmax;
    cv::minMaxIdx(data, &vmin, &vmax);
    if (vmin < 0) {
        vmax = std::max(vmax, -vmin);
        cv::Mat tmp = cv::abs(data);
        tmp *= 255.0 / vmax;
        tmp.convertTo(tmp, CV_8U);
        cv::imwrite("./grad/" + name + "_abs.png", tmp);
        data += vmax;
        data *= 127.5 / vmax;
    } else
        data *= 255.0 / vmax;
    data.convertTo(data, CV_8U);
    //std::cout << "write: " << "./grad/" + name + ".png" << std::endl;
    cv::imwrite("./grad/" + name + ".png", data);
}

void saveResults(const lsfm::FilterResults &results, const std::string &name, bool sqr = false, int border = 0) {
    for_each(results.begin(), results.end(), [&](const FilterResult &r) {
        cv::Mat tmp;
        if (r.second.data.type() != cv::DataType<FT>::type)
            r.second.data.convertTo(tmp, cv::DataType<FT>::type);
        else
            r.second.data.copyTo(tmp);

        if (r.first == "phase") {
            lsfm::FilterResults::const_iterator f = results.find("energy");
            if (f != results.end())
                savePhase(tmp, name + "_" + r.first, f->second.data);
            else
                saveDir(tmp, name + "_" + r.first);
            return;
        }
        if (r.first == "dir") {
            lsfm::FilterResults::const_iterator f = results.find("odd");
            if (f == results.end())
                f = results.find("mag");
            if (f != results.end())
                saveDir(tmp, name + "_" + r.first, f->second.data);
            else
                saveDir(tmp, name + "_" + r.first);
            return;
        }
        if (r.first == "mag" && sqr)
            cv::sqrt(tmp, tmp);

        if (border > 0) {
            tmp.colRange(0, border).setTo(0);
            tmp.colRange(tmp.cols - border, tmp.cols).setTo(0);
            tmp.rowRange(0, border).setTo(0);
            tmp.rowRange(tmp.rows - border, tmp.rows).setTo(0);
        }

        saveNormalized(tmp, name + "_" + r.first);
    });
}

void parseFolder(const fs::path &folder, std::vector<fs::path> &files) {
    fs::directory_iterator end_iter;
    for_each(fs::directory_iterator(folder), fs::directory_iterator(), [&files](const fs::path& file) {
        if (fs::is_regular_file(file))
        {
            std::string ext = file.extension().generic_string();
            boost::algorithm::to_lower(ext);
            if (ext == ".jpg" || ext == ".png") {
                files.push_back(file);
            }
        }
        if (fs::is_directory(file))
            parseFolder(file, files);
    });
}

void processPath(std::vector<Entry> &entries, const std::pair<fs::path, std::string> &path) {
    std::cout << "processing " << path.first << std::endl;
    std::vector<fs::path> files;
    parseFolder(path.first, files);

    std::for_each(files.begin(), files.end(), [&](const fs::path& file) {
        cv::Mat rgb = cv::imread(file.generic_string());
        if (rgb.empty())
        {
            cout << "Can not open " << file.generic_string() << endl;
            return;
        }
        std::cout << file << std::endl;
        cv::Mat src;
        cv::cvtColor(rgb, src, CV_BGR2GRAY);

        for_each(entries.begin(), entries.end(), [&](Entry &e) {
            saveResults(e.process(e.rgb() ? rgb : src), path.second + "/" + file.stem().generic_string() + "_" + e.name,e.sqr(),e.border());
        });
    });
}

int main(int argc, char** argv)
{
    std::vector<std::pair<fs::path, std::string>> sets;
    //sets.push_back(std::pair<fs::path, std::string>("../../images/noise", "noise"));
    sets.push_back(std::pair<fs::path, std::string>("../../images/Selection", "Selection"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/BSDS500", "BSDS500"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-Q", "MDB-Q"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-H", "MDB-H"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-F", "MDB-F"));

    std::vector<Entry> filter;
    /*filter.push_back(Entry(new DerivativeGradient<uchar,short, FT, FT, RobertsDerivative>,"Roberts_(2x2)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, PrewittDerivative>, "Prewitt_(3x3)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short, FT, FT, ScharrDerivative>, "Scharr_(3x3)"));

    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative>, "Sobel_(3x3)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative>({ NV("grad_kernel_size",5) }), "Sobel_(5x5)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative>({ NV("grad_kernel_size",7) }), "Sobel_(7x7)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative>({ NV("grad_kernel_size",9) }), "Sobel_(9x9)"));

    filter.push_back(Entry(new DerivativeGradient<uchar, FT, FT, FT, GaussianDerivative, Magnitude>({ NV("grad_kernel_size",3), NV("grad_range",1.5) }), "Gauss_(3x3)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, FT, FT, FT, GaussianDerivative, Magnitude>({ NV("grad_kernel_size",5), NV("grad_range",2.3) }), "Gauss_(5x5)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, FT, FT, FT, GaussianDerivative, Magnitude>({ NV("grad_kernel_size",7), NV("grad_range",3.0) }), "Gauss_(7x7)"));
    filter.push_back(Entry(new DerivativeGradient<uchar, FT, FT, FT, GaussianDerivative, Magnitude>({ NV("grad_kernel_size",9), NV("grad_range",3.5) }), "Gauss_(9x9)"));

    filter.push_back(Entry(new SusanGradient<short, int>(20), "Susan_(37)"));
    filter.push_back(Entry(new SusanGradient<short, int>(20,true), "Susan_(3x3)"));
    filter.push_back(Entry(new RCMGradient<uchar, 1, short, int>(3,1), "RMG_(3x3)", ENTRY_NO_3));
    filter.push_back(Entry(new RCMGradient<uchar, 1, short, int>(5, 3), "RMG_(5x5)", ENTRY_NO_5));
    filter.push_back(Entry(new RCMGradient<uchar, 3, short, int>(3, 1), "RCMG_(3x3)", ENTRY_NO_3 | ENTRY_SQR | ENTRY_RGB));

    filter.push_back(Entry(new LaplaceSimple<uchar, short>, "Laplace_(3x3)"));
    filter.push_back(Entry(new LaplaceCV<uchar, short>(5), "Laplace_Sobel_(5x5)"));
    filter.push_back(Entry(new LaplaceCV<uchar, short>(7), "Laplace_Sobel_(7x7)"));
    filter.push_back(Entry(new LaplaceCV<uchar, short>(9), "Laplace_Sobel_(9x9)"));*/

    /*filter.push_back(Entry(new LoG<uchar, FT>(3, 1.240080), "LoG_(3x3)"));
    filter.push_back(Entry(new LoG<uchar, FT>(5, 1.008030), "LoG_(5x5)"));
    filter.push_back(Entry(new LoG<uchar, FT>(7, 0.873228), "LoG_(7x7)"));
    filter.push_back(Entry(new LoG<uchar, FT>(9, 0.781859), "LoG_(9x9)"));*/

    /*filter.push_back(Entry(new QuadratureG2<uchar, FT>(3, 1.24008), "QF_StG_(3x3)"));
    filter.push_back(Entry(new QuadratureG2<uchar, FT>(5, 1.008),   "QF_StG_(5x5)"));
    filter.push_back(Entry(new QuadratureG2<uchar, FT>(7, 0.873226), "QF_StG_(7x7)"));
    filter.push_back(Entry(new QuadratureG2<uchar, FT>(9, 0.781854), "QF_StG_(9x9)"));*/

    /*filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 3, 1.2), "SQF_Po_(3x3)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 5, 1.2), "SQF_Po_(5x5)"));
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 7, 1.2), "SQF_Po_(7x7)"));*/
    filter.push_back(Entry(new QuadratureS<uchar, FT, FT>(1, 2, 9, 1.2), "SQF_Po_(9x9)"));

    /*filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 3, 1.2)), "SQF_Po_(3x3)"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 5, 1.2)), "SQF_Po_(5x5)"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 7, 1.2)), "SQF_Po_(7x7)"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 9, 1.2)), "SQF_Po_(9x9)"));*/

    //filter.push_back(Entry(new QuadratureSF<uchar, FT>(1, 2, 1.2), "SQFF_Po_12"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar,FT,FT,FT, FT>*>(new PCLSqf<uchar, FT>(1, 2, 1.2)), "SQFF_Po_12"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSqf<uchar, FT>(1, 3, 2.5)), "SQFF_Po"));
    //filter.push_back(Entry(new QuadratureLGF<uchar, FT>(5, 0.55), "SQFF_Lg"));

    //filter.push_back(Entry(new PCLgf<uchar, FT>(4, 3,2.1,0.55), "PCF_Lg"));
    //filter.push_back(Entry(new PCMatlab<uchar>(4, 3, 2.1, 0.55), "PCF_Ml"));
    //filter.push_back(Entry(new PCSqf<uchar, FT>(1,3,2.5), "PCF_Po"));

    fs::create_directory("./grad");
    for_each(sets.begin(), sets.end(), [&](const std::pair<fs::path, std::string> &data) {
        fs::create_directory("./grad/" + data.second);
        processPath(filter, data);
    });

    return 0;
}


/*
@copyright Louis Dionne 2014
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/hana/monoid/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/monoid.hpp>
using namespace boost::hana;

template <typename mcd>
void test() {
    constexpr auto mono = detail::minimal::monoid<mcd>;
    BOOST_HANA_CONSTEXPR_ASSERT(plus(mono(2), mono(3)) == mono(5));
    BOOST_HANA_CONSTEXPR_ASSERT(mono(2) + mono(3) == mono(5));
}

int main() {
    test<Monoid::mcd>();
}


#ifndef SAFETYMODULE_H_
#define SAFETYMODULE_H_

#include <Eigen/Geometry>

#include <vector>

inline int sign(const double x) { return x >= 0 ? 1 : -1; }

class SafetyModule
{

public:
  typedef Eigen::Matrix<double, 2, Eigen::Dynamic> LaserMeasurementMatrix;

  SafetyModule() {}
  SafetyModule(double robotRadius, double minimumDistanceToRobot, double timeOffset,
               double maximumDeceleration);
  ~SafetyModule() {}

  bool motionIsSafe(const Eigen::Vector2d& currentPosition,
                    const Eigen::Quaterniond& currentOrientation, const double transVel,
                    const double rotVel, const std::vector<Eigen::Vector2d>& pointsLaser);

  void reset()
  {
    _motionIsSafe = true;
    _statusMsg.clear();
  }

  double computeStoppingDistance(const double transVel);

  Eigen::Vector2d transformPointPolar2KarthesianCoordinates(const double range, const double alpha);

  int getSignOfDistance(const Eigen::Hyperplane<double, 2>& hyperplane,
                        const Eigen::Vector2d& point);

  Eigen::Vector2d computeStoppingPoint(const double transVel, const double rotVel,
                                       const Eigen::Vector2d& positionStart,
                                       const Eigen::Vector2d& positionCenter);

  Eigen::Vector2d computeRotationCenterOfTrajectory(const double radius, const double transVel,
                                                    const double rotVel,
                                                    const Eigen::Vector2d& positionStart,
                                                    const Eigen::Vector2d& currentHeading);

  double computeTrajectoryRadius(const double transVel, const double rotVel);

  bool objectTooClose(const Eigen::Vector2d& laserMeas, const Eigen::Vector2d& positionRobot,
                      double& laserPointDistToCenterSquared);

  double getRotVelThreshold() const { return _rotVelThreshold; }
  void setRotVelThreshold(double rotVelThreshold = 1e-3) { _rotVelThreshold = rotVelThreshold; }

  double getMaxDeceleration() const { return _maxDeceleration; }
  void setMaxDeceleration(double maxDeceleration = 1) { _maxDeceleration = maxDeceleration; }

  double getRobotRadius() const { return _robotRadius; }
  void setRobotRadius(double robotRadius = 0.4) { _robotRadius = robotRadius; }

  double getTimeOffset() const { return _timeOffset; }
  void setTimeOffset(double timeOffset = 0.3) { _timeOffset = timeOffset; }

  double getMinDistToRobot() const { return _minimumDistanceToRobot; }
  void setMinDistToRobot(double minDistToRobot) { _minimumDistanceToRobot = minDistToRobot; }

  double getMinDistToSensor() const { return _minimumDistanceToSensor; }
  void setMinDistToSensor(double minDistToSensor) { _minimumDistanceToSensor = minDistToSensor; }

  double getMinLateralDist() const { return _minimumLateralDistance; }
  void setMinLateralDist(double minLateralDist) { _minimumLateralDistance = minLateralDist; }

  double getMinLongitudinalDist() const { return _minimumLongitudinalDistance; }
  void setMinLongitudinalDist(double minLongitudinalDist)
  {
    _minimumLongitudinalDistance = minLongitudinalDist;
  }

  const std::string& getStatusMessage() const { return _statusMsg; }

private:
  // parameters
  double _robotRadius            = 0.18;
  double _minimumDistanceToRobot = 0.01;  // m
  double _timeOffset      = 0.0;  // s (time until sent velocity command gets executed by the robot)
  double _maxDeceleration = 20.0;  // m/s^2
  double _rotVelThreshold = 1e-3;             // rad/s
  double _distSensorToEdge            = 0.02;  // m
  double _minimumDistanceToSensor     = 0.0;
  double _minimumLateralDistance      = 0.01;
  double _minimumLongitudinalDistance = 0.01;

  // safety state
  bool _motionIsSafe = true;

  std::string _statusMsg;
};

#endif /* SAFETYMODULE_H_ */


/*
 * Model_example.cpp
 *
 *  Created on: Mar 21, 2020
 *     Authors: Oliva Alexander
 *
 *  C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano, A. De Luca, 'Dynamic
 *  Identification of the Franka Emika Panda Robot With Retrieval of Feasible
 *  Parameters Using Penalty-Based Optimization'. IEEE RA-L, 2019.
 *
 *  This example shows how to use the functions of the estimated dynamic model
 *  of the Franka Emika Panda robot.
 *  The code, loads the trajectory parameter of one of the exciting trajectories
 *  used in the paper and available in the repository. Once the one trajectory
 *  step is computed, the joint torque is computed from the model and stored
 *  in some files. The output data of the example can be used e.g. to evaluate the
 *  estimated model accuracy with respect to the real joint torques. See the
 *  accompanying image: Robot_vs_Model_torques.jpg
 *
 */

#include <iostream>
#include <chrono>
#include <fstream>
#include <cmath>
#include <string>
#include <unistd.h>
#include <sstream>

#include<franka_model.h>

#include <Eigen/Core>

using namespace std;

int main(int argc, char **argv)
{

	string  ParamsFile = "../../matlab/data/Exciting_Traj/Trajectory_1/gen_params.txt";
	string  InitPosFile = "../../matlab/data/Exciting_Traj/Trajectory_1/gen_q0.txt";
	bool give_file = false;

	for (int i = 1; i < argc; i++) {
		if (string(argv[i]) == "--help" || string(argv[i]) == "-h") {
			cout << argv[0] << " [--help] [-h] [--params myParams.txt] [--initPos myInitPos.txt]"
					<< "\n";
			return EXIT_SUCCESS;
		}
		else if (string(argv[i]) == "--params" && i + 1 < argc){
			ParamsFile = argv[i + 1];
		}
		else if (string(argv[i]) == "--initPos" && i + 1 < argc){
			InitPosFile = argv[i + 1];
		}
	}
	if (ParamsFile == "" || InitPosFile == "") {
		cout << argv[0] << "\n Specify the input file name with the [--params] and/or [--initPos] option" << endl;
		return EXIT_SUCCESS;
	}else{
		cout << "Trajectory Parameters file: " <<  ParamsFile << endl;
		cout << "Trajectory Initial pos file: " <<  InitPosFile << endl;
	}

	//First load the initial joint configuration
	Eigen::Vector7d q0;
	ifstream ifsParams, ifsInitPos;
	string str;
	unsigned int i = 0, j=0;
	cout << "Initial joint configuration: " << endl;
	ifsInitPos.open(InitPosFile,ios::in);
	if (ifsInitPos.is_open()) {
		while(getline(ifsInitPos, str)) {
			istringstream ss(str);
			ss >>  q0[i];
			cout << q0[i] << " , ";
			i++;
		}
		cout << endl;
	}
	ifsInitPos.close();

	unsigned int nJoints = 7;
	double T = 1, Ttrj= 20;
	Eigen::Vector7d A,final_speed;
	A.Zero();
	Eigen::Matrix< double , 7 , 5 > a_k_l;
	Eigen::Matrix< double , 7 , 5 > b_k_l;

	// compute parameters needed to generate the exciting trajectory
	cout << "Loaded trajectory parameters: " << endl;
	str = "";
	i = 0, j=0;
	ifsParams.open(ParamsFile,ios::in);
	if (ifsParams.is_open()) {
		while(getline(ifsParams, str)) {
			istringstream ss(str);
			j=0;
			if(i < 7){
				while(ss >> a_k_l(i,j)){
					cout << a_k_l(i,j) << " , ";
					j++;
				}
			}else
			{
				while(ss >> b_k_l(i-7,j)){
					cout << b_k_l(i-7,j) << " , ";
					j++;
				}
			}
			i++;
			cout << endl;
		}
	}
	ifsParams.close();

	for(int i=0; i<7; i++){
		for(int j=0; j<5; j++){
			A[i] = A[i] + a_k_l(i,j);
		}
	}

	ofstream torqueFile, positionsFile, velocityFile, accelerationFile, timeFile;
	torqueFile.open("exciting_traj_torques.txt",ios::out);
	positionsFile.open("exciting_traj_positions.txt",ios::out);
	velocityFile.open("exciting_traj_velocity.txt",ios::out);
	accelerationFile.open("exciting_traj_acceleration.txt",ios::out);
	timeFile.open("exciting_traj_time.txt",ios::out);

	// uncomment this section if you want to collect also this data into files
	/*
	ofstream gravityFile, massFile, coriolisFile, frictionFile;
	gravityFile.open("exciting_traj_gravity.txt",ios::out);
	massFile.open("exciting_traj_mass.txt",ios::out);
	coriolisFile.open("exciting_traj_coriolis.txt",ios::out);
	frictionFile.open("exciting_traj_friction.txt",ios::out);
	*/

	Eigen::Vector7d  q,dq,dq_old,ddq, tau,gravity, friction;
	Eigen::Matrix7d mass, coriolis;

	chrono::high_resolution_clock::time_point start, current, tic, toc, old;
	chrono::duration<double> duration = chrono::steady_clock::duration::zero();
	chrono::duration<double> delta_t = chrono::steady_clock::duration::zero();
	start = chrono::high_resolution_clock::now();
	current = start;

	q = q0;
	dq_old.Zero();

	double min = 10, max = 0, mean = 0;
	unsigned int counter = 0;
	while(duration.count() <= Ttrj + T){
		old = current;
		current = chrono::high_resolution_clock::now();
		duration = chrono::duration_cast<chrono::duration<double>>(current-start);
		delta_t = chrono::duration_cast<chrono::duration<double>>(current - old);

		// compute the exciting velocity trajectory and compute the joint acceleration and position from it
		for(int i=0;i<nJoints;i++){
			if (duration.count() < T/2) {
				dq(i) = (A[i]/2)*(1-cos((2*M_PI)*duration.count()));
			}
			else if (duration.count() >= T/2 && duration.count() < (Ttrj+T/2)) {
				dq(i) = 0.0;
				for (int j=0; j<5; j++){
					dq(i) =  dq(i)+ a_k_l(i,j)*cos(0.15*M_PI*(j+1)*(duration.count() - T/2)) + b_k_l(i,j)*sin(0.15*M_PI*(j+1)*(duration.count() - T/2));
				}
				final_speed(i) = dq(i);
			}
			else if (duration.count() >= (Ttrj+T/2)) {
				dq(i) = (final_speed(i)/2)*(1-cos((2*M_PI)*(duration.count() - Ttrj)));
			}
			ddq(i) = (dq(i) - dq_old(i))/delta_t.count();
			q(i) = q(i) + dq(i)*delta_t.count();
			dq_old(i) = dq(i);
		}

		//get the model
		tic = chrono::high_resolution_clock::now();

		mass = MassMatrix(q);
		coriolis = CoriolisMatrix(q, dq);
		gravity = GravityVector(q);
		friction = Friction(dq);
		// compute the joint torques from the Lagrangian equation of motion
		tau = mass*ddq + coriolis*dq + gravity + friction;

		toc = chrono::high_resolution_clock::now();

		timeFile << duration.count() << endl;
		for(int k = 0; k < nJoints; k++){
			torqueFile << tau(k) << endl;
			positionsFile << q(k) << endl;
			velocityFile << dq(k) << endl;
			accelerationFile << ddq(k) << endl;

			// uncomment for store this values into files
			/*
			gravityFile << gravity(k) << endl;
			frictionFile << friction(k) << endl;
			for(int l=0;l < nJoints; l++){
				massFile << mass(k,l) << "\t";
				coriolisFile << coriolis(k,l) << "\t";
			}
			massFile << endl;
			coriolisFile << endl;
             */

		}

		// make some performance analysis on the execution time
		delta_t = chrono::duration_cast<chrono::duration<double>>(toc-tic);
//		cout << "delta time: " << delta_t.count() << endl;
		if(delta_t.count() < min){
			min = delta_t.count();
		}
		if(delta_t.count() > max){
			max = delta_t.count();
		}
		mean += delta_t.count();
		counter++;
	}
	cout << "_____ Model execution time [sec] ___" << endl;
	cout << "  worst    /    best    /    mean  \n" << max << " / " << min << " / " << mean/counter << "\n\n";

	torqueFile.close();
	positionsFile.close();
	velocityFile.close();
	accelerationFile.close();
	timeFile.close();

	cout << "Some files were generated. Use them to compare the results of the estimated model with the real robot. \n" << endl;
	cout << "The collected data from the real robot, for this example trajectory, is located in: " << endl;
	cout << "/FrankaEmikaPandaDynModel/matlab/data/Exciting_Traj/Trajectory_1/rbt_log \n" << endl;

	//uncomment if you stored this values in files
	/*
	coriolisFile.close();
	gravityFile.close();
	massFile.close();
	frictionFile.close();
    */

	cout << "The end" << endl;
	return EXIT_SUCCESS;

}