// Copyright 2006. Peter Gottschling, Matthias Troyer, Rolf Bonderer
// 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.

#ifndef LA_VECTOR_CONCEPTS_INCLUDE
#define LA_VECTOR_CONCEPTS_INCLUDE

#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/linear_algebra/ets_concepts.hpp>

#ifdef __GXX_CONCEPTS__
#  include <concepts>
#else
#  include <boost/numeric/linear_algebra/pseudo_concept.hpp>
#endif

namespace math {

/** @addtogroup Concepts
 *  @{
 */

#ifdef __GXX_CONCEPTS__
concept VectorSpace<typename Vector, typename Scalar = typename Vector::value_type>
: AdditiveAbelianGroup<Vector>
{
    requires Field<Scalar>;
    requires Multiplicable<Scalar, Vector>;
    requires MultiplicableWithAssign<Vector, Scalar>;
    requires DivisibleWithAssign<Vector, Scalar>;

    requires std::Assignable<Vector, Multiplicable<Scalar, Vector>::result_type>;
    requires std::Assignable<Vector, Multiplicable<Vector, Scalar>::result_type>;
    requires std::Assignable<Vector, Divisible<Vector, Scalar>::result_type>;

    // Associated types of Field<Scalar> and AdditiveAbelianGroup<Vector> collide
    // typename result_type = AdditiveAbelianGroup<Vector>::result_type;
    // typename assign_result_type = AdditiveAbelianGroup<Vector>::assign_result_type;

    axiom Distributivity(Vector v, Vector w, Scalar a, Scalar b)
    {
	a * (v + w) == a * v + a * w;
	(a + b) * v == a * v + b * v;
	// The following properties are implied by the above, Field and Abelian group
	// Can we be sure that compilers can deduce/interfere it?
	(v + w) * a == v * a + w * a;
	v * (a + b) == v * a + v * b;
    }
}
#else
    //! Concept VectorSpace
    /*!
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Requires:
	- Field < Scalar >;
	- Multiplicable <Scalar, Vector>;
	- MultiplicableWithAssign <Vector, Scalar>;
	- DivisibleWithAssign <Vector, Scalar>;
	- std::Assignable <Vector, Multiplicable<Scalar, Vector>::result_type>;
	- std::Assignable <Vector, Multiplicable<Vector, Scalar>::result_type>;
	- std::Assignable <Vector, Divisible<Vector, Scalar>::result_type>;

    */
    template <typename Vector, typename Scalar = typename Vector::value_type>
    struct VectorSpace
      : AdditiveAbelianGroup<Vector>
    {
	/// Invariant: Distributivity of scalars and vectors from left and from right
	axiom Distributivity(Vector v, Vector w, Scalar a, Scalar b)
	{
	    /// a * (v + w) == a * v + a * w;       // Scalar from left

	    /// Vector from right: (a + b) * v == a * v + b * v;

	    /// Scalar from right: (v + w) * a == v * a + w * a;

	    /// Vector from left:  v * (a + b) == v * a + v * b;
	}
    };
#endif

#ifdef __GXX_CONCEPTS__
concept Norm<typename N, typename Vector,
	     typename Scalar = typename Vector::value_type>
  : std::Callable1<N, Vector>
{
    requires VectorSpace<Vector, Scalar>;
    requires RealMagnitude<Scalar>;
    typename magnitude_type = MagnitudeType<Scalar>::type;
    requires std::Convertible<magnitude_type, Scalar>;

    typename result_type_norm = std::Callable1<N, Vector>::result_type;
    requires std::Convertible<result_type_norm, RealMagnitude<Scalar>::magnitude_type>;
    requires std::Convertible<result_type_norm, Scalar>;

    // Version with function instead functor, as used by Rolf and Matthias
    // Axioms there defined without norm functor and concept has only 2 types
#if 0
    typename result_type_norm;
    result_type_norm norm(const Vector&);
    requires std::Convertible<result_type_norm, magnitude_type>;
    requires std::Convertible<result_type_norm, Scalar>;
#endif

    axiom Positivity(N norm, Vector v, magnitude_type ref)
    {
	norm(v) >= zero(ref);
    }

    // The following is covered by RealMagnitude
    // requires AbsApplicable<Scalar>;
    // requires std::Convertible<AbsApplicable<Scalar>::result_type, magnitude_type>;
    // requires Multiplicable<magnitude_type>;

    axiom PositiveHomogeneity(N norm, Vector v, Scalar a)
    {
	norm(a * v) == abs(a) * norm(v);
    }

    axiom TriangleInequality(N norm, Vector u, Vector v)
    {
	norm(u + v) <= norm(u) + norm(v);
    }
}
#else
    //! Concept Norm
    /*!
        Semantic requirements of a norm

	\param N        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- std::Callable1 <N, Vector>

	\par Associated types:
	- magnitude_type
	- result_type_norm

	\par Requires:
	- VectorSpace <Vector, Scalar>;
	- RealMagnitude < Scalar >;
	- std::Convertible <magnitude_type, Scalar>;
	- std::Convertible <result_type_norm, RealMagnitude<Scalar>::magnitude_type>;
	- std::Convertible <result_type_norm, Scalar>;

    */
template <typename N, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct Norm
  : std::Callable1<N, Vector>
{
    /// Associated type to represent real values in teh Field of scalar (with default)
    /** By default MagnitudeType<Scalar>::type */
    typedef associated_type magnitude_type;

    /// Associated type for result of norm functor
    /** Automatically detected */
    typedef associated_type result_type_norm;

    /// Invariant: norm of vector is larger than zero
    axiom Positivity(N norm, Vector v, magnitude_type ref)
    {
	/// norm(v) >= zero(ref);
    }

    /// Invariant: positive homogeneity with scalar
    axiom PositiveHomogeneity(N norm, Vector v, Scalar a)
    {
	/// norm(a * v) == abs(a) * norm(v);
    }

    /// Invariant: triangle inequality
    axiom TriangleInequality(N norm, Vector u, Vector v)
    {
	/// norm(u + v) <= norm(u) + norm(v);
    }
};
#endif

#ifdef __GXX_CONCEPTS__
concept SemiNorm<typename N, typename Vector,
		 typename Scalar = typename Vector::value_type>
  : Norm<N, Vector, Scalar>
{
    axiom PositiveDefiniteness(N norm, Vector v, magnitude_type ref)
    {
	if (norm(v) == zero(ref))
	    v == zero(v);
	if (v == zero(v))
	    norm(v) == zero(ref);
    }
}
#else
    //! Concept SemiNorm
    /*!
        Semantic requirements of a semi-norm

	\param N        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- Norm <N, Vector, Scalar>
    */
template <typename N, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct SemiNorm
  : Norm<N, Vector, Scalar>
{
    /// The norm of a vector is zero if and only if the vector is the zero vector
    axiom PositiveDefiniteness(N norm, Vector v, magnitude_type ref)
    {
	/// if (norm(v) == zero(ref)) v == zero(v);

	/// if (v == zero(v)) norm(v) == zero(ref);
    }
};
#endif

#ifdef __GXX_CONCEPTS__
concept BanachSpace<typename N, typename Vector,
		    typename Scalar = typename Vector::value_type>
  : Norm<N, Vector, Scalar>,
    VectorSpace<Vector, Scalar>
{};
#else
    //! Concept BanachSpace
    /*!
        A Banach space is a vector space with a norm

	\param N        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- Norm <N, Vector, Scalar>
	- VectorSpace <Vector, Scalar>

	\note
	- The (expressible) requirements of Banach Space are already given in Norm.
	- The difference between the requirements is the completeness of the
	  Banach space, i.e. that every Cauchy sequence w.r.t. norm(v-w) has a limit
	  in the space. Unfortunately, completeness is never satisfied for
	  finite precision arithmetic types.
	- Another subtle difference is that Norm is not a refinement of Vectorspace
    */
template <typename N, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct BanachSpace
  : Norm<N, Vector, Scalar>,
    VectorSpace<Vector, Scalar>
{};
#endif

#ifdef __GXX_CONCEPTS__
concept InnerProduct<typename I, typename Vector,
		     typename Scalar = typename Vector::value_type>
  : std::Callable2<I, Vector, Vector>
{
    // Result of the inner product must be convertible to Scalar
    requires std::Convertible<std::Callable2<I, Vector, Vector>::result_type, Scalar>;

    // Let's try without this
    // requires ets::InnerProduct<I, Vector, Scalar>;

    requires HasConjugate<Scalar>;

    axiom ConjugateSymmetry(I inner, Vector v, Vector w)
    {
	inner(v, w) == conj(inner(w, v));
    }

    axiom SequiLinearity(I inner, Scalar a, Scalar b, Vector u, Vector v, Vector w)
    {
	inner(v, b * w) == b * inner(v, w);
	inner(u, v + w) == inner(u, v) + inner(u, w);
	// This implies the following (will compilers infere/deduce?)
	inner(a * v, w) == conj(a) * inner(v, w);
	inner(u + v, w) == inner(u, w) + inner(v, w);
    }

    requires RealMagnitude<Scalar>;
    typename magnitude_type = RealMagnitude<Scalar>::type;
    // requires FullLessThanComparable<magnitude_type>;

    axiom NonNegativity(I inner, Vector v, MagnitudeType<Scalar>::type magnitude)
    {
	// inner(v, v) == conj(inner(v, v)) implies inner(v, v) is real
	// ergo representable as magnitude type
	magnitude_type(inner(v, v)) >= zero(magnitude)
    }

    axiom NonDegeneracy(I inner, Vector v, Vector w, Scalar s)
    {
	if (v == zero(v))
	    inner(v, w) == zero(s);
	if (inner(v, w) == zero(s))
	    v == zero(v);
    }
};
#else
    //! Concept InnerProduct
    /*!
        Semantic requirements of a inner product

	\param I        The inner product functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- std::Callable2 <I, Vector, Vector>

	\par Associated types:
	- magnitude_type

	\par Requires:
	- std::Convertible<std::Callable2 <I, Vector, Vector>::result_type, Scalar> ;
	  result of inner product convertible to scalar to be used in expressions
	- HasConjugate < Scalar >
	- RealMagnitude < Scalar > ; the scalar value needs a real magnitude type
    */
template <typename I, typename Vector,
          typename Scalar = typename Vector::value_type>
struct InnerProduct
  : std::Callable2<I, Vector, Vector>
{
    /// Associated type: the  real magnitude type of the scalar
    /** By default RealMagnitude<Scalar>::type */
    typename associated_type magnitude_type;
    // requires FullLessThanComparable<magnitude_type>;

    /// The arguments can be changed and the result is then the complex conjugate
    axiom ConjugateSymmetry(I inner, Vector v, Vector w)
    {
	/// inner(v, w) == conj(inner(w, v));
    }

    /// The inner product is linear in the second argument and conjugate linear in the first one
    /** The equalities are partly redundant with ConjugateSymmetry */
    axiom SequiLinearity(I inner, Scalar a, Scalar b, Vector u, Vector v, Vector w)
    {
	/// inner(v, b * w) == b * inner(v, w);

	/// inner(u, v + w) == inner(u, v) + inner(u, w);

	/// inner(a * v, w) == conj(a) * inner(v, w);

	/// inner(u + v, w) == inner(u, w) + inner(v, w);
    }

    /// The inner product of a vector with itself is not negative
    /** inner(v, v) == conj(inner(v, v)) implies inner(v, v) is representable as real */
    axiom NonNegativity(I inner, Vector v, MagnitudeType<Scalar>::type magnitude)
    {
	/// magnitude_type(inner(v, v)) >= zero(magnitude);
    }

    /// Non-degeneracy not representable with axiom
    axiom NonDegeneracy(I inner, Vector v, Vector w, Scalar s)
    {
	/// \f$\langle v, w\rangle = 0 \forall w \Leftrightarrow v = \vec{0}\f$
    }
};
#endif

#ifdef __GXX_CONCEPTS_
// A dot product is only a semantically special case of an inner product
// Questionable if we want such a concept
concept DotProduct<typename I, typename Vector,
		   typename Scalar = typename Vector::value_type>
  : InnerProduct<I, Vector, Scalar>
{};
#else
    //! Concept DotProduct
    /*!
        Semantic requirements of dot product. The dot product is a specific inner product.

	\param I        Norm functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined

	\par Refinement of:
	- InnerProduct <I, Vector, Scalar>
    */
template <typename I, typename Vector,
	  typename Scalar = typename Vector::value_type>
struct DotProduct
  : InnerProduct<I, Vector, Scalar>
{};
#endif

// Norm induced by inner product
// Might be moved to another place later
// Definition as class and function
// Conversion from scalar to magnitude_type is covered by norm concept
template <typename I, typename Vector,
	  typename Scalar = typename Vector::value_type>
  _GLIBCXX_WHERE(InnerProduct<I, Vector, Scalar>
		 && RealMagnitude<Scalar>)
struct induced_norm_t
{
    // Return type evtl. with macro to use concept definition
    typename magnitude_type_trait<Scalar>::type
    operator() (const I& inner, const Vector& v)
    {
	// Check whether inner product is positive real
	// assert(Scalar(abs(inner(v, v))) == inner(v, v));

	// Similar check while accepting small imaginary values
	// assert( (abs(inner(v, v)) - inner(v, v)) / abs(inner(v, v)) < 1e-6; )

	// Could also be defined with abs but that might introduce extra ops
	// typedef RealMagnitude<Scalar>::type magnitude_type;

	typedef typename magnitude_type_trait<Scalar>::type magnitude_type;
	return sqrt(static_cast<magnitude_type> (inner(v, v)));
    }
};

#if 0
template <typename I, typename Vector,
	  typename Scalar = typename Vector::value_type>
  LA_WHERE( InnerProduct<I, Vector, Scalar>
	    && RealMagnitude<Scalar> )
magnitude_type_trait<Scalar>::type
induced_norm(const I& inner, const Vector& v)
{
    return induced_norm_t<I, Vector, Scalar>() (inner, v);
}
#endif

#ifdef __GXX_CONCEPTS__

concept HilbertSpace<typename I, typename Vector,
		     typename Scalar = typename Vector::value_type,
		     typename N = induced_norm_t<I, Vector, Scalar> >
  : InnerProduct<I, Vector, Scalar>,
    BanachSpace<N, Vector, Scalar>
{
    axiom Consistency(Vector v)
    {
	math::induced_norm_t<I, Vector, Scalar>()(v) == N()(v);
    }
};
#else
    //! Concept HilbertSpace
    /*!
        A Hilbert space is a vector space with an inner product that induces a norm

	\param I        Inner product functor
	\param Vector   The the type of a vector or a collection
        \param Scalar   The scalar over which the vector field is defined
	\param N        Norm functor

	\par Refinement of:
	- InnerProduct <I, Vector, Scalar>
	- BanachSpace <N, Vector, Scalar>

	\note
	- The (expressible) requirements of Banach Space are already given in InnerProduct
	  (besides consistency of the functors).
	- A difference is that InnerProduct is not a refinement of Vectorspace
    */
template <typename I, typename Vector,
          typename Scalar = typename Vector::value_type,
	  typename N = induced_norm_t<I, Vector, Scalar> >
struct HilbertSpace
  : InnerProduct<I, Vector, Scalar>,
    BanachSpace<N, Vector, Scalar>
{
    /// Consistency between norm and induced norm
    axiom Consistency(Vector v)
    {
	/// math::induced_norm_t<I, Vector, Scalar>()(v) == N()(v);
    }
};
#endif // __GXX_CONCEPTS__

/*@}*/ // end of group Concepts

} // namespace math

#endif // LA_VECTOR_CONCEPTS_INCLUDE


#pragma once
#include <Eigen/Core>
#include <type_traits>
#include "ear/helpers/assert.hpp"
#include "ear/helpers/output_gains.hpp"

namespace ear {

  /** @brief Check if two containers have intersecting elements
   */
  template <class InputIterator1, class InputIterator2>
  bool doIntersect(InputIterator1 first1, InputIterator1 last1,
                   InputIterator2 first2, InputIterator2 last2) {
    while (first1 != last1 && first2 != last2) {
      if (*first1 < *first2)
        ++first1;
      else if (*first2 < *first1)
        ++first2;
      else {
        return true;
      }
    }
    return false;
  }

  template <int N, int M>
  double interp(double x, const Eigen::Matrix<double, N, 1> &xp,
                const Eigen::Matrix<double, M, 1> &yp) {
    ear_assert(std::is_sorted(xp.begin(), xp.end()),
               "in interp: unsorted x values");
    ear_assert(xp.size() == yp.size(),
               "in interp: must have same number of x and y points");
    ear_assert(xp.size() >= 2, "in interp: must have at least 2 points");

    if (x <= xp(0)) {
      return yp(0);
    }
    for (int i = 0; i < xp.size() - 1; ++i) {
      if (xp(i + 1) > x) {
        double x0 = xp(i);
        double x1 = xp(i + 1);
        double y0 = yp(i);
        double y1 = yp(i + 1);
        return y0 + (y1 - y0) / (x1 - x0) * (x - x0);
      }
    }
    return yp(yp.size() - 1);
  }

  // write to a vector in a way which is compatible with OutputGains
  template <typename VecT, typename ValueT>
  void vec_write(VecT &vec, size_t i, ValueT value) {
    vec[i] = value;
  }
  template <typename ValueT>
  void vec_write(OutputGains &vec, size_t i, ValueT value) {
    vec.write(i, value);
  }

  /// `mask_write(out, mask, values)` is equivalent to numpy
  /// `out[mask] = values` for boolean masks
  template <typename OutT, typename MaskT, typename ValuesT>
  void mask_write(OutT &&out, const MaskT &mask, const ValuesT &values) {
    Eigen::Index out_size = out.size();
    Eigen::Index mask_size = mask.size();
    Eigen::Index values_size = values.size();

    ear_assert(
        out_size == mask_size,
        "in mask_write: out_size and mask_write must be the same length");

    Eigen::Index j = 0;
    for (Eigen::Index i = 0; i < mask_size; i++)
      if (mask[i]) vec_write(out, i, values[j++]);

    ear_assert(j == values_size,
               "in mask_size: length of values must equal the number of "
               "entries in mask");
  }

  /// make an eigen copy of a std::vector (or other type with size() and
  /// operator[]), for use when Eigen::Map can't be used, e.g. with
  /// std::vector<bool>
  template <typename EigenT, typename ParamT,
            typename std::enable_if<EigenT::RowsAtCompileTime == Eigen::Dynamic,
                                    int>::type = 0>
  inline EigenT copy_vector(const ParamT &x) {
    Eigen::Index size = x.size();
    EigenT rv(size);

    for (Eigen::Index i = 0; i < size; i++) rv[i] = x[i];

    return rv;
  }

  template <typename EigenT, typename ParamT,
            typename std::enable_if<EigenT::RowsAtCompileTime != Eigen::Dynamic,
                                    int>::type = 0>
  inline EigenT copy_vector(const ParamT &x) {
    Eigen::Index size = x.size();
    EigenT rv;
    ear_assert(rv.rows() == size,
               "in copy_vector: incorrect size vector for Eigen type used");

    for (Eigen::Index i = 0; i < size; i++) rv[i] = x[i];

    return rv;
  }
}  // namespace ear


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_CBRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_CBRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/real_splat.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/bitofsign.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/fast_frexp.hpp>
#include <boost/simd/function/simd/fast_ldexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/simd/is_gez.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/negate.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotomnmbo_3.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/simd/is_less.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/simd/is_inf.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(cbrt_
                          , (typename A0,typename A1,typename X)
                          , bd::cpu_
                          , bs::pack_<bd::single_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( ) const BOOST_NOEXCEPT
      {
        A0 z =  nt2::abs(a0);
        using int_type =  bs::as_integer_t<A0, signed>;
        using stype =  bd::scalar_of_t<A0>;
  #ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = is_less(z, Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
  #endif
        const A0 CBRT2  = Constant< A0, 0x3fa14518> ();
        const A0 CBRT4  = Constant< A0, 0x3fcb2ff5> ();
        const A0 CBRT2I = Constant< A0, 0x3f4b2ff5> ();
        const A0 CBRT4I = Constant< A0, 0x3f214518> ();
        int_type e;
        A0 x = fast_frexp(z, e);
        x = horn <stype, 0xbe09e49a,
                         0x3f0bf0fe,
                         0xbf745265,
                         0x3f91eb77,
                         0x3ece0609)
                 > (x);
        auto flag = is_gez(e);
        int_type e1 =  nt2::abs(e);
        int_type rem = e1;
        e1 /= Three<int_type>();
        rem -= e1*Three<int_type>();
        e = negate(e1, e);
        const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
        const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
        A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
        fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
        x = fast_ldexp(x*fact, e);
        x -= (x-z/sqr(x))*Third<A0>();
  #ifndef BOOST_SIMD_NO_DENORMALS
        x = bitwise_or(x, bitofsign(a0))*f;
  #else
        x = bitwise_or(x, bitofsign(a0));
  #endif
  #ifndef BOOST_SIMD_NO_INFINITIES
        return if_else(logical_or(is_eqz(a0),is_inf(a0)), a0, x);
  #else
        return if_else(is_eqz(a0), a0, x);
  #endif
      }
   };

} }

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2018 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
 */

#include <ql/math/ode/adaptiverungekutta.hpp>
#include <ql/methods/finitedifferences/schemes/methodoflinesscheme.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

namespace QuantLib {

    MethodOfLinesScheme::MethodOfLinesScheme(
        const Real eps,
        const Real relInitStepSize,
        const ext::shared_ptr<FdmLinearOpComposite> & map,
        const bc_set& bcSet)
    : dt_(Null<Real>()),
      eps_(eps),
      relInitStepSize_(relInitStepSize),
      map_(map),
      bcSet_(bcSet) {
    }

    Disposable<std::vector<Real> >
    MethodOfLinesScheme::apply(Time t, const std::vector<Real>& u) const {
        map_->setTime(t, t + 0.0001);
        bcSet_.applyBeforeApplying(*map_);

        const Array dxdt = -map_->apply(Array(u.begin(), u.end()));

        std::vector<Real> retVal(dxdt.begin(), dxdt.end());
        return retVal;
    }

    void MethodOfLinesScheme::step(array_type& a, Time t) {
        QL_REQUIRE(t-dt_ > -1e-8, "a step towards negative time given");

        const std::vector<Real> v =
           AdaptiveRungeKutta<Real>(eps_, relInitStepSize_*dt_)(
               boost::bind(&MethodOfLinesScheme::apply, this, _1, _2),
               std::vector<Real>(a.begin(), a.end()),
               t, std::max(0.0, t-dt_));

        Array y(v.begin(), v.end());

        bcSet_.applyAfterSolving(y);

        a = y;
    }

    void MethodOfLinesScheme::setStep(Time dt) {
        dt_ = dt;
    }
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <complex>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <cmath>

#include <boost/timer.hpp>

#include "dune/common/stdstreams.hh"
#include "dune/grid/sgrid.hh"
#include "dune/grid/uggrid.hh"
#include "dune/grid/geometrygrid.hh"
#include "dune/grid/common/gridinfo.hh"
#include "dune/istl/solvers.hh"

#include "fem/assemble.hh"
#include "fem/embedded_errorest.hh"
#include "fem/istlinterface.hh"
#include "fem/functional_aux.hh"
#include "fem/hierarchicspace.hh"
#include "fem/lagrangespace.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/triplet.hh"
#include "linalg/iluprecond.hh"
#include "linalg/additiveschwarz.hh"
#include "linalg/hyprecond.hh"
#include "io/vtk.hh"
#include "io/amira.hh"

#include "utilities/kaskopt.hh"

#include "ht.hh"

 namespace Dune
  {

    namespace Capabilities
    {

      template< class Grid >
      struct hasHierarchicIndexSet
      {
        static const bool v = false;
      };

      template< class Grid >
      struct hasHierarchicIndexSet< const Grid >
      {
        static const bool v = hasHierarchicIndexSet< Grid >::v;
      };

    }

  }

class SquareToCircle
  : public Dune::AnalyticalCoordFunction< double, 2, 2, SquareToCircle >
  {
    typedef SquareToCircle This;
    typedef Dune::AnalyticalCoordFunction< double, 2, 2, This > Base;

  public:
    typedef Base::DomainVector DomainVector;
    typedef Base::RangeVector RangeVector;

    void evaluate ( const DomainVector &x, RangeVector &y ) const
    {
      double enorm = sqrt(x[0]*x[0]+x[1]*x[1]);

      if(enorm > 0.00001)
      {

        double radius = sqrt(2.0)*std::max(std::fabs(x[0]),std::fabs(x[1]));

      double scaling = radius/enorm;

      y[ 0 ] = x[ 0 ]*scaling;
      y[ 1 ] = x[ 1 ]*scaling;
      } else
      {
      y[ 0 ] = x[ 0 ];
      y[ 1 ] = x[ 1 ];
      }

    }
  };

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

	//   two-dimensional space: dim=2
	int const dim=2;
	typedef Dune::UGGrid<dim> Grid;

        typedef Dune::GeometryGrid<Grid,SquareToCircle> GGrid;

	Dune::GridFactory<Grid> factory;

	// vertex coordinates v[0], v[1]
	Dune::FieldVector<double,dim> v;
	v[0]=-1; v[1]=-1; factory.insertVertex(v);
	v[0]=1; v[1]=-1; factory.insertVertex(v);
	v[0]=1; v[1]=1; factory.insertVertex(v);
	v[0]=-1; v[1]=1; factory.insertVertex(v);
	v[0]=0; v[1]=0; factory.insertVertex(v);
	// triangle defined by 3 vertex indices
	std::vector<unsigned int> vid(3);
	Dune::GeometryType gt(Dune::GeometryType::simplex,2);
	vid[0]=0; vid[1]=1; vid[2]=4; factory.insertElement(gt,vid);
	vid[0]=1; vid[1]=2; vid[2]=4; factory.insertElement(gt,vid);
	vid[0]=2; vid[1]=3; vid[2]=4; factory.insertElement(gt,vid);
	vid[0]=3; vid[1]=0; vid[2]=4; factory.insertElement(gt,vid);
	std::auto_ptr<Grid> grid( factory.createGrid() ) ;

	// the coarse grid will be refined three times
	// some information on the refined mesh
	std::cout << "Grid: " << grid->size(0) << " triangles, " << std::endl;
	std::cout << "      " << grid->size(1) << " edges, " << std::endl;
	std::cout << "      " << grid->size(2) << " points" << std::endl;
	// a gridmanager is constructed
	// as connector between geometric and algebraic information

        SquareToCircle def;

        std::auto_ptr<GGrid> ggrid(new GGrid(*grid,def));

	ggrid->globalRefine(6);

	GridManager<GGrid> gridManager(ggrid);

//StartSnippet2
	// construction of finite element space for the scalar solution T
	typedef GGrid::LeafGridView GridView;
	typedef FEFunctionSpace<ContinuousLagrangeMapper<double,GridView> > H1Space;
	H1Space temperatureSpace(gridManager,gridManager.grid().leafView(),
							 1);
	typedef boost::fusion::vector<H1Space const*> Spaces;
	Spaces spaces(&temperatureSpace);
	// VariableDescription<int spaceId, int components, int Id>
	// spaceId: number of associated FEFunctionSpace
	// components: number of components in this variable
	// Id: number of this variable
	typedef boost::fusion::vector<VariableDescription<0,1,0> >
			VariableDescriptions;
	std::string varNames[1] = { "T" };
	typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
	VariableSet variableSet(spaces,varNames);
//StopSnippet2

//StartSnippet3
	typedef HeatFunctional<double,VariableSet> Functional;
	Functional F;
	typedef VariationalFunctionalAssembler<LinearizationAt<Functional> > GOP;
	typedef GOP::TestVariableRepresentation<>::type Rhs;
	GOP gop(gridManager.signals,spaces);
	VariableSet::VariableSet x(variableSet);
//StopSnippet3

//StartSnippet4

	typedef GOP::AnsatzVariableRepresentation<>::type Sol;
	Sol solution(GOP::AnsatzVariableRepresentation<>::init(gop));
	solution = 0;
	gop.assemble(linearization(F,x));

	Rhs rhs(GOP::TestVariableRepresentation<>::rhs(gop));
	AssembledGalerkinOperator<GOP,0,1,0,1> A(gop, false);
	AssembledGalerkinOperator<GOP,0,1,0,1>::matrix_type tri(A.getmat());
//	A.getmat().print();

//StartSnippet5
    boost::timer directTimer;
    directInverseOperator(A,DirectType::UMFPACK,MatrixProperties::GENERAL).applyscaleadd(-1.0,rhs,solution);
    std::cout << "direct solve: " << directTimer.elapsed() << "s\n";
    x.data = solution.data;
//StopSnippet5

//StartSnippet6
	// output of solution in VTK format for visualization,
	// the data are written as ascii stream into file temperature.vtu,
	// possible is also binary
 	IoOptions options;
 	options.outputType = IoOptions::ascii;

    typedef GGrid::LeafGridView LeafGridView;
    LeafGridView leafView = gridManager.grid().leafView();
    writeVTKFile(leafView,variableSet,x,"temperature",options);

  }


#include <algorithm>
#include <cmath>
#include <vector>

#include <Eigen/Dense>

#include <isce3/antenna/EdgeMethodCostFunc.h>
#include <isce3/except/Error.h>
#include <isce3/math/RootFind1dNewton.h>
#include <isce3/math/polyfunc.h>

namespace isce3 { namespace antenna {

std::tuple<double, double, bool, int> rollAngleOffsetFromEdge(
        const poly1d_t& polyfit_echo, const poly1d_t& polyfit_ant,
        const isce3::core::Linspace<double>& look_ang,
        std::optional<poly1d_t> polyfit_weight)
{
    // check the input arguments
    if (polyfit_echo.order != 3 || polyfit_ant.order != 3)
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Requires 3rd-order poly-fit object for both "
                "Echo and Antenna!");
    constexpr double a_tol {1e-5};
    if (std::abs(polyfit_echo.mean - polyfit_ant.mean) > a_tol ||
            std::abs(polyfit_echo.norm - polyfit_ant.norm) > a_tol)
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Requires same (mean, std) for Echo and Antenna Poly1d obj!");

    if (!(polyfit_echo.norm > 0.0))
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Requires positive std of Echo and Antenna Poly1d obj!");

    if (polyfit_weight) {
        if (polyfit_weight->order < 0)
            throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                    "The order of polyfit for weights must be "
                    "at least 0 (constant weights)!");
        if (!(polyfit_weight->norm > 0.0))
            throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                    "Requires positive std of weight Poly1d obj!");
    }

    // create a copy polyfit objects "echo" and "ant" with zero mean and unit
    // std
    auto pf_echo_cp = polyfit_echo;
    pf_echo_cp.mean = 0.0;
    pf_echo_cp.norm = 1.0;
    auto pf_ant_cp = polyfit_ant;
    pf_ant_cp.mean = 0.0;
    pf_ant_cp.norm = 1.0;

    // declare and initialize a look angle vector
    Eigen::ArrayXd lka_vec(look_ang.size());
    for (int idx = 0; idx < look_ang.size(); ++idx)
        lka_vec(idx) = look_ang[idx];

    // create a weighting vector from look vector and weighting Poly1d
    Eigen::ArrayXd wgt_vec;
    if (polyfit_weight) {
        Eigen::Map<Eigen::ArrayXd> wgt_coef(
                polyfit_weight->coeffs.data(), polyfit_weight->coeffs.size());
        wgt_vec = isce3::math::polyval(
                wgt_coef, lka_vec, polyfit_weight->mean, polyfit_weight->norm);
        // normalize power in dB
        wgt_vec -= wgt_vec.maxCoeff();
        // convert from dB to linear power scale
        wgt_vec = Eigen::pow(10, 0.1 * wgt_vec);
    }
    // centralized and scaled the look vector based on mean/std of the echo
    // Poly1d to be used for both antenna and echo in the cost function.
    lka_vec -= polyfit_echo.mean;
    const auto std_inv = 1.0 / polyfit_echo.norm;
    lka_vec *= std_inv;

    // form some derivatives used in the cost function
    auto pf_echo_der = pf_echo_cp.derivative();
    auto pf_ant_der = pf_ant_cp.derivative();
    auto pf_ant_der2 = pf_ant_der.derivative();
    // create a memmap of the coeff for the first and second derivatives
    Eigen::Map<Eigen::ArrayXd> coef_ant_der(
            pf_ant_der.coeffs.data(), pf_ant_der.coeffs.size());
    Eigen::Map<Eigen::ArrayXd> coef_ant_der2(
            pf_ant_der2.coeffs.data(), pf_ant_der2.coeffs.size());
    Eigen::Map<Eigen::ArrayXd> coef_echo_der(
            pf_echo_der.coeffs.data(), pf_echo_der.coeffs.size());
    // form some arrays over scaled look angles for diff of first derivatives
    // and for second derivative
    auto ant_echo_der_dif_vec =
            isce3::math::polyval(coef_ant_der - coef_echo_der, lka_vec);
    auto ant_der2_vec = isce3::math::polyval(coef_ant_der2, lka_vec);

    // build cost function in the form of Poly1d object (3th order polynimal!)
    auto cf_pf = isce3::core::Poly1d(3, 0.0, 1.0);
    // fill up the coeff for the derivative of the WMSE cost function:
    // cost(ofs) = pf_wgt*(pf_echo_der(el) - pf_ant_der(el + ofs))**2
    // See section 1.1 of the cited reference.
    if (polyfit_weight) {
        auto tmp1 = wgt_vec * ant_echo_der_dif_vec;
        auto tmp2 = wgt_vec * ant_der2_vec;
        cf_pf.coeffs[0] = (tmp1 * ant_der2_vec).sum();
        cf_pf.coeffs[1] = (tmp2 * ant_der2_vec).sum() +
                          6 * pf_ant_cp.coeffs[3] * tmp1.sum();
        cf_pf.coeffs[2] = 9 * pf_ant_cp.coeffs[3] * tmp2.sum();
        cf_pf.coeffs[3] =
                18 * pf_ant_cp.coeffs[3] * pf_ant_cp.coeffs[3] * wgt_vec.sum();
    } else // no weighting
    {
        cf_pf.coeffs[0] = (ant_echo_der_dif_vec * ant_der2_vec).sum();
        cf_pf.coeffs[1] = ant_der2_vec.square().sum() +
                          6 * pf_ant_cp.coeffs[3] * ant_echo_der_dif_vec.sum();
        cf_pf.coeffs[2] = 9 * pf_ant_cp.coeffs[3] * ant_der2_vec.sum();
        cf_pf.coeffs[3] = 18 * pf_ant_cp.coeffs[3] * pf_ant_cp.coeffs[3] *
                          look_ang.size();
    }
    // form Root finding object
    auto rf_obj =
            isce3::math::RootFind1dNewton(1e-4, 20, look_ang.spacing() / 10.);
    // solve for the root/roll offset via Newton
    auto [roll, f_val, flag, n_iter] = rf_obj.root(cf_pf);
    // scale back the roll angle by std of original poly1d object
    roll *= polyfit_echo.norm;

    return {roll, f_val, flag, n_iter};
}

std::tuple<double, double, bool, int> rollAngleOffsetFromEdge(
        const poly1d_t& polyfit_echo, const poly1d_t& polyfit_ant,
        double look_ang_near, double look_ang_far, double look_ang_prec,
        std::optional<poly1d_t> polyfit_weight)
{
    if (!(look_ang_near > 0.0 && look_ang_far > 0.0 && look_ang_prec > 0.0))
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "All look angles values must be positive numbers!");
    if (look_ang_near >= (look_ang_far - look_ang_prec))
        throw isce3::except::InvalidArgument(ISCE_SRCINFO(),
                "Near-range look angle shall be smaller than "
                "far one by at least one prec!");

    const auto ang_size = static_cast<int>(
            std::round((look_ang_far - look_ang_near) / look_ang_prec) + 1);
    auto look_ang = isce3::core::Linspace<double>::from_interval(
            look_ang_near, look_ang_far, ang_size);

    return rollAngleOffsetFromEdge(
            polyfit_echo, polyfit_ant, look_ang, polyfit_weight);
}

}} // namespace isce3::antenna


//============================================================================
// Name        : test2.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
#include <Eigen/Dense>
//#include <unsupported/Eigen/AutoDiff>
#include <unsupported/Eigen/AdolcForward>
//#include <unsupported/Eigen/AdolcSupport>

//#include <chrono>
//#include <iomanip>
#include <adolc.h>
#include <adolc/adouble.h>
#include <vector>
#include <time.h>
#include <math.h>
#include <fstream>
#include <thread>
#include <future>
#include <algorithm>
#include <iterator>
#include "cost.h"
#include "dynamics.h"
#include "iLQR.h"
#include "utils.h"
#include<unistd.h>
#include "DataStreamClient.h"

int main()
{

//
//	Eigen::VectorXd X0(4,1),inputs(2,1);
//	X0<<0,0,0,0;
//	inputs<<0,0;
//
//	constexpr size_t state_size=4;
//	constexpr size_t input_size=2;
	constexpr size_t horizon=5;
	constexpr int total_steps=300;
	srand((unsigned int) time(0));
////	int horizon=1000;
	unsigned int tag1(1),tag2(2),tag3(3),tag4(4),tag5(5),tag6(6)
	,tag7(7),tag8(8),tag9(9);

//	double time_step=0.2;
	double time_step=0.1;

//

//	/* This part is unicycle simulation*/
//
//	cost<4,2> running_cost_uni(Unicycle_Cost::running_cost,tag1);
//	cost<4,2> terminal_cost_uni(Unicycle_Cost::terminal_cost,tag2);
//	dynamics<4,2> dynamics_uni(Unicycle_Dynamics::dynamics,tag3,time_step);
//
//	iLQR<4,2,horizon> uni_solver(running_cost_uni,terminal_cost_uni,dynamics_uni);
//	Eigen::Matrix<double,2,1> u_uni=Eigen::Matrix<double,2,1>::Zero();
//	Eigen::Matrix<double,4,1> x0_uni=Eigen::Matrix<double,4,1>::Zero();
//	x0_uni<<-5,-5,0,0;
//////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//	Eigen::Matrix<double,4,1> x_goal_uni;
//
//
////
//	iLQR<4,2,horizon>::input_trajectory u_init_uni;
//	std::fill(std::begin(u_init_uni), std::end(u_init_uni), u_uni);
//	iLQR<4,2,horizon>::state_input_trajectory soln_uni;
//	iLQR<4,2,total_steps>::state_input_trajectory soln_uni_rhc;
//
//
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	uni_solver.set_MPC(u_init_uni);
//	int execution_steps=1;
//	soln_uni_rhc.first[0]=x0_uni;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//
//	x_goal_uni<<5,5,0,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
////		X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_uni=uni_solver.run_MPC(soln_uni_rhc.first[i], x_goal_uni, 100, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
////		std::cout<<soln_uni.first[39]<<std::endl;
//		soln_uni_rhc.first[i+1]=soln_uni.first[1];
//		soln_uni_rhc.second[i]=soln_uni.second[0];
//	}
//	t_end = clock();
//////
//////
////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<4,2,total_steps>(state_path,input_path, soln_uni_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);
//
//
//	/* End of unicycle simulation*/

/* This part is 2-unicycle simulation*/

//	cost<8,4> running_cost_uni2(Unicycle_Cost::running_cost2,tag1);
//	cost<8,4> terminal_cost_uni2(Unicycle_Cost::terminal_cost2,tag2);
//	dynamics<8,4> dynamics_uni2(Unicycle_Dynamics::dynamics_2,tag3,time_step);
//
//	iLQR<8,4,horizon> uni2_solver(running_cost_uni2,terminal_cost_uni2,dynamics_uni2);
//	Eigen::Matrix<double,4,1> u_uni2=Eigen::Matrix<double,4,1>::Random()*0.1;
//	Eigen::Matrix<double,8,1> x0_uni2=Eigen::Matrix<double,8,1>::Zero();
//	x0_uni2<<0,0.1,0,0, 10,0,M_PI,0;
//
//	Eigen::Matrix<double,8,1> x_goal_uni2;
//
//
////
//	iLQR<8,4,horizon>::input_trajectory u_init_uni2;
//	for(auto &item : u_init_uni2)
//		item=Eigen::Matrix<double,4,1>::Random()*0.1;
//	std::fill(std::begin(u_init_uni2), std::end(u_init_uni2), u_uni2);
//	iLQR<8,4,horizon>::state_input_trajectory soln_uni2;
//	iLQR<8,4,total_steps>::state_input_trajectory soln_uni2_rhc;
//
//
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	uni2_solver.set_MPC(u_init_uni2);
//	int execution_steps=1;
//	soln_uni2_rhc.first[0]=x0_uni2;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//
//	x_goal_uni2<<10,0,0,0, 0,0,M_PI,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
////		X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_uni2=uni2_solver.run_MPC(soln_uni2_rhc.first[i], x_goal_uni2, 5, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
////		std::cout<<soln_uni.first[39]<<std::endl;
//		soln_uni2_rhc.first[i+1]=soln_uni2.first[1];
//		soln_uni2_rhc.second[i]=soln_uni2.second[0];
//	}
//	t_end = clock();
////
////
//////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<8,4,total_steps>(state_path,input_path, soln_uni2_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);

/* End of 2-unicycle simulation*/

/* This part is single drone simulation*/

//	cost<12,4> running_cost_dr(Drone_Cost::running_cost,tag1);
//	cost<12,4> terminal_cost_dr(Drone_Cost::terminal_cost,tag2);
//	dynamics<12,4> dynamics_dr(Drone_Dynamics::dynamics,tag3,time_step);
//
//	iLQR<12,4,horizon> drone_solver(running_cost_dr,terminal_cost_dr,dynamics_dr);
//	Eigen::Matrix<double,4,1> u_drone=Eigen::Matrix<double,4,1>::Ones();
//	Eigen::Matrix<double,12,1> X0_drone=Eigen::Matrix<double,12,1>::Zero();
//////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//	Eigen::Matrix<double,12,1> X_goal_drone;
//
//	u_drone=u_drone*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
////
//	iLQR<12,4,horizon>::input_trajectory u_init_drone;
//	std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
//	iLQR<12,4,horizon>::state_input_trajectory soln_drone;
//	iLQR<12,4,total_steps>::state_input_trajectory soln_drone_rhc;
//
//
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	drone_solver.set_MPC(u_init_drone);
//	int execution_steps=1;
//	soln_drone_rhc.first[0]=X0_drone;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//	X_goal_drone<<1,1,1.5, 0,0,0, 0,0,0, 0,0,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
////		X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_drone=drone_solver.run_MPC(soln_drone_rhc.first[i], X_goal_drone, 100, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		std::cout<<soln_drone.first[39]<<std::endl;
//		soln_drone_rhc.first[i+1]=soln_drone.first[1];
//		soln_drone_rhc.second[i]=soln_drone.second[0];
//	}
//	t_end = clock();
//////
//////
////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<12,4,total_steps>(state_path,input_path, soln_drone_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);

/* End of single drone simulation*/

/* This part is 2-drone simulation*/

//	cost<12*2,4*2> running_cost_dr2(Drone_Cost::running_cost2,tag4);
//	cost<12*2,4*2> terminal_cost_dr2(Drone_Cost::terminal_cost2,tag5);
//	dynamics<12*2,4*2> dynamics_dr2(Drone_Dynamics::dynamics_2,tag6,time_step);
//	iLQR<12*2,4*2,horizon>::input_trajectory u_init_drone2;
//
//	iLQR<12*2,4*2,horizon> drone2_solver(running_cost_dr2,terminal_cost_dr2,dynamics_dr2);
//	Eigen::Matrix<double,4*2,1> u_drone2=Eigen::Matrix<double,4*2,1>::Ones();
//	Eigen::Matrix<double,12*2,1> X0_drone2=Eigen::Matrix<double,12*2,1>::Zero();
////	X0_drone2<<-2.5,-2.5,-2.5, 0,0,0, 0,0,0, 0,0,0,
////			2.5,-2.5,-2.5, 0,0,0, 0,0,0, 0,0,0;
//	X0_drone2<<-1,0,2, 0,0,0, 0,0,0, 0,0,0,
//			1,0,2, 0,0,0, 0,0,0, 0,0,0;
//	Eigen::Matrix<double,12*2,1> X_goal_drone2;
//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//	clock_t t_start, t_end;
//
////
//	double seconds;
//	drone2_solver.set_MPC(u_init_drone2);
//	int execution_steps=1;
//	soln_drone2_rhc.first[0]=X0_drone2;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
////	X_goal_drone2<<2.5,2.5,2.5, 0,0,0, 0,0,0, 0,0,0,
////			-2.5,2.5,2.5, 0,0,0, 0,0,0, 0,0,0;
//	X_goal_drone2<<1,0,2, 0,0,0, 0,0,0, 0,0,0,
//			-1,0,2, 0,0,0, 0,0,0, 0,0,0;
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
//
////		if (i>total_steps/3)
////			X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
//////
//		soln_drone2=drone2_solver.run_MPC(soln_drone2_rhc.first[i], X_goal_drone2, 20, execution_steps);
////
////////		unsigned int microsecond = 1000000;
////////		usleep(2 * microsecond);
////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		soln_drone2_rhc.first[i+1]=soln_drone2.first[1];
//		soln_drone2_rhc.second[i]=soln_drone2.second[0];
//	}
//	t_end = clock();
//////
//////
////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<12*2,4*2,total_steps>(state_path,input_path, soln_drone2_rhc);
////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);
//
/* End of 2-drone simulation*/

//
//

//
////	diff = difftime( stop, start );
////	std::cout << "Time: " << diff << " seconds";
////	seconds = (double)(t_end - t_start) / CLOCKS_PER_SEC;
////	printf("Time: %f s\n", seconds);
//		drone_solver.set_MPC(u_init_drone);
////		t_start = clock();
//		std::string state_write_path="/Users/talhakavuncu/Desktop/cflib_stream.txt";
//		std::string state_read_path="/Users/talhakavuncu/Desktop/stream.txt";
//		std::ifstream state_read_file(state_read_path);
//		std::ofstream state_write_file(state_write_path);
//		Eigen::IOFormat CommaInitFmt(Eigen::FullPrecision, Eigen::DontAlignCols, ",", ", ", "", "", "", "");
//		double dt,x,y,z,roll,pitch,yaw;
//		state_read_file>>dt>>x>>y >> z>>roll >> pitch>>yaw;
//		X0_drone<<x,y,z,roll,pitch,yaw,x/dt,y/dt,z/dt,roll/dt,pitch/dt,yaw/dt;
//		X_goal_drone<<0,0,1, 0,0,0, 0,0,0, 0,0,0;
//		while(true)
//		{
////			X_goal_drone<<10,0,0, 0,0,0, 0,0,0, 0,0,0;
////			if (i>total_steps/3)
////				X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
////			std::cout<<"iteration "<<i<<std::endl;
//	//
//
//			soln_drone=drone_solver.run_MPC(X0_drone, X_goal_drone, 50, 1);
//			state_write_file.open(state_write_path,std::ios::out | std::ios::trunc);
//			state_write_file<<soln_drone.first[1].transpose().format(CommaInitFmt);
////			i.transpose().format(CommaInitFmt)
//			state_write_file.close();
//			state_read_file>>dt>>x>>y >> z>>roll >> pitch>>yaw;
//			X0_drone<<x,y,z,roll,pitch,yaw,x/dt,y/dt,z/dt,roll/dt,pitch/dt,yaw/dt;
//	////		unsigned int microsecond = 1000000;
//	////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
////			soln_drone_rhc.first[i+1]=soln_drone.first[1];
//
////			soln_drone_rhc.second[i]=soln_drone.second[0];
//		}
////		t_end = clock();

/* This part is integrator simulation */

//
//cost<3,3> integrator_running_cost(Single_Integrator_Cost::running_cost,tag7);
//cost<3,3> integrator_terminal_cost(Single_Integrator_Cost::terminal_cost,tag8);
//dynamics<3,3> integrator_dynamics(Single_Integrator_3D::dynamics,tag9,time_step);
//iLQR<3,3,horizon>::input_trajectory u_init_integrator;
//iLQR<3,3,horizon> integrator_solver(integrator_running_cost,integrator_terminal_cost,integrator_dynamics);
//Eigen::Matrix<double,3,1> u_integrator=Eigen::Matrix<double,3,1>::Zero();
//Eigen::Matrix<double,3,1> x0_integrator=Eigen::Matrix<double,3,1>::Zero();
//Eigen::Matrix<double,3,1> x_goal_integrator;
//iLQR<3,3,horizon>::state_input_trajectory soln_integrator;
//iLQR<3,3,total_steps>::state_input_trajectory soln_integrator_rhc;
//
////	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//	std::fill(std::begin(u_init_integrator), std::end(u_init_integrator), u_integrator);
////	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
////	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//	clock_t t_start, t_end;
////
//////
//	x0_integrator<<0,0,0.5;
////	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
//	double seconds;
//	integrator_solver.set_MPC(u_init_integrator);
//	int execution_steps=1;
//	soln_integrator_rhc.first[0]=x0_integrator;
//	std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//	std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//	t_start = clock();
//	for(int i=0;i<total_steps;++i)
//	{
//		x_goal_integrator<<2,2,2;
////		if (i>total_steps/3)
////			x_goal_integrator<<0,0,0;
//		std::cout<<"iteration "<<i<<std::endl;
////////
////		auto term1=soln_integrator_rhc.first[i]+0.2*Eigen::Matrix<double,3,1>::Random();
////		auto term2=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term3=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term4=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term5=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term6=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term7=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term8=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term9=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term10=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
////		auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//		soln_integrator=integrator_solver.run_MPC(soln_integrator_rhc.first[i],
//				x_goal_integrator, 100, execution_steps);
//////
//////////		unsigned int microsecond = 1000000;
//////////		usleep(2 * microsecond);
//////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		soln_integrator_rhc.first[i+1]=soln_integrator.first[1];
//		soln_integrator_rhc.second[i]=soln_integrator.second[0];
//	}
//	t_end = clock();
//////
////////
//////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//	write_file<3,3,total_steps>(state_path,input_path, soln_integrator_rhc);
//////
//	std::cout<<"finished"<<std::endl;
//	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//	printf("Time: %f s\n", seconds);

/*single integrator simulation ends */

/*Single integrator 2 agent simulation */

//	cost<6,6> integrator2_running_cost(Single_Integrator_Cost::running_cost2,tag7);
//	cost<6,6> integrator2_terminal_cost(Single_Integrator_Cost::terminal_cost2,tag8);
//	dynamics<6,6> integrator2_dynamics(Single_Integrator_3D::dynamics2,tag9,time_step);
//	iLQR<6,6,horizon>::input_trajectory u_init_integrator2;
//	iLQR<6,6,horizon> integrator2_solver(integrator2_running_cost,integrator2_terminal_cost,integrator2_dynamics);
//	Eigen::Matrix<double,6,1> u_integrator2=Eigen::Matrix<double,6,1>::Zero();
//	Eigen::Matrix<double,6,1> x0_integrator2=Eigen::Matrix<double,6,1>::Zero();
//	x0_integrator2<<-1,0,2.001, 1,0,2;
////	std::cout<<x0_integrator2<<std::endl;
//	Eigen::Matrix<double,6,1> x_goal_integrator2;
//	x_goal_integrator2<<1,0,2, -1,0,2;
//	iLQR<6,6,horizon>::state_input_trajectory soln_integrator2;
//	iLQR<6,6,total_steps>::state_input_trajectory soln_integrator2_rhc;
//
//	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//	//	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//		clock_t t_start, t_end;
//	//
//	////
//		double seconds;
//		integrator2_solver.set_MPC(u_init_integrator2);
//		int execution_steps=1;
//		soln_integrator2_rhc.first[0]=x0_integrator2;
//		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//		t_start = clock();
//		for(int i=0;i<total_steps;++i)
//		{
////			x_goal_integrator2<<2,2,2;
//	//		if (i>total_steps/3)
//	//			x_goal_integrator<<0,0,0;
//			std::cout<<"iteration "<<i<<std::endl;
//	//////
//			soln_integrator2=integrator2_solver.run_MPC(soln_integrator2_rhc.first[i],
//					x_goal_integrator2, 5, execution_steps);
//	////
//	////////		unsigned int microsecond = 1000000;
//	////////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//			soln_integrator2_rhc.first[i+1]=soln_integrator2.first[1];
//			soln_integrator2_rhc.second[i]=soln_integrator2.second[0];
//		}
//		t_end = clock();
//	////
//	//////
//	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		write_file<6,6,total_steps>(state_path,input_path, soln_integrator2_rhc);
//	////
//		std::cout<<"finished"<<std::endl;
//		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//		printf("Time: %f s\n", seconds);

/*Single integrator 2 agent simulation ends*/

/*Double Integrator Simulation*/

//
//
//	cost<6,3> integrator_running_cost(Double_Integrator_Cost::running_cost,tag7);
//	cost<6,3> integrator_terminal_cost(Double_Integrator_Cost::terminal_cost,tag8);
//	dynamics<6,3> integrator_dynamics(Double_Integrator_3D::dynamics,tag9,time_step);
//	iLQR<6,3,horizon>::input_trajectory u_init_integrator;
//	iLQR<6,3,horizon> integrator_solver(integrator_running_cost,integrator_terminal_cost,integrator_dynamics);
//	Eigen::Matrix<double,3,1> u_integrator=Eigen::Matrix<double,3,1>::Zero();
//	Eigen::Matrix<double,6,1> x0_integrator=Eigen::Matrix<double,6,1>::Zero();
//	Eigen::Matrix<double,6,1> x_goal_integrator;
//	iLQR<6,3,horizon>::state_input_trajectory soln_integrator;
//	iLQR<6,3,total_steps>::state_input_trajectory soln_integrator_rhc;
//
//	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//		std::fill(std::begin(u_init_integrator), std::end(u_init_integrator), u_integrator);
//	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//		clock_t t_start, t_end;
//	//
//	////
//		x0_integrator<<0,0,0.5, 0,0,0;
//	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
//		double seconds;
//		integrator_solver.set_MPC(u_init_integrator);
//		int execution_steps=1;
//		soln_integrator_rhc.first[0]=x0_integrator;
//		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//		t_start = clock();
//		for(int i=0;i<total_steps;++i)
//		{
//			x_goal_integrator<<2,2,2, 0,0,0;
//	//		if (i>total_steps/3)
//	//			x_goal_integrator<<0,0,0;
//			std::cout<<"iteration "<<i<<std::endl;
//	//////
//	//		auto term1=soln_integrator_rhc.first[i]+0.2*Eigen::Matrix<double,3,1>::Random();
//	//		auto term2=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term3=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term4=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term5=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term6=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term7=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term8=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term9=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term10=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//			soln_integrator=integrator_solver.run_MPC(soln_integrator_rhc.first[i],
//					x_goal_integrator, 5, execution_steps);
//	////
//	////////		unsigned int microsecond = 1000000;
//	////////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//			soln_integrator_rhc.first[i+1]=soln_integrator.first[1];
//			soln_integrator_rhc.second[i]=soln_integrator.second[0];
//		}
//		t_end = clock();
//	////
//	//////
//	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		write_file<6,3,total_steps>(state_path,input_path, soln_integrator_rhc);
//	////
//		std::cout<<"finished"<<std::endl;
//		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//		printf("Time: %f s\n", seconds);

/*Double Integrator Simulation Ends*/

/*First Order Drone Simulation */

	cost<6,6> drone_running_cost(Drone_First_Order_Cost::running_cost,tag7);
	cost<6,6> drone_terminal_cost(Drone_First_Order_Cost::terminal_cost,tag8);
	dynamics<6,6> drone_dynamics(Drone_First_Order_Dynamics::dynamics,tag9,time_step);
	iLQR<6,6,horizon>::input_trajectory u_init_drone;
	iLQR<6,6,horizon> drone_solver(drone_running_cost,drone_terminal_cost,drone_dynamics);
	Eigen::Matrix<double,6,1> u_drone=Eigen::Matrix<double,6,1>::Zero();
	Eigen::Matrix<double,6,1> x0_drone=Eigen::Matrix<double,6,1>::Zero();
	Eigen::Matrix<double,6,1> x_goal_drone;
	iLQR<6,6,horizon>::state_input_trajectory soln_drone;
	iLQR<6,6,total_steps>::state_input_trajectory soln_drone_rhc;

	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
		std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;

		clock_t t_start, t_end;
	//
	////
		x0_drone<<0,0,0.5, 0,0,0;
	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
		double seconds;
		drone_solver.set_MPC(u_init_drone);
		int execution_steps=1;
		soln_drone_rhc.first[0]=x0_drone;
		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
		t_start = clock();
		for(int i=0;i<total_steps;++i)
		{
			x_goal_drone<<2,2,2, 0,0,0;
	//		if (i>total_steps/3)
	//			x_goal_integrator<<0,0,0;
			std::cout<<"iteration "<<i<<std::endl;
			double scale=0.01;
			auto term1=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term2=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term3=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term4=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term5=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term6=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term7=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term8=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term9=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term10=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
//			auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
			soln_drone=drone_solver.run_MPC(term1,
					x_goal_drone, 5, execution_steps);
	////
	////////		unsigned int microsecond = 1000000;
	////////		usleep(2 * microsecond);
	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
			soln_drone_rhc.first[i+1]=soln_drone.first[1];
			soln_drone_rhc.second[i]=soln_drone.second[0];
		}
		t_end = clock();
	////
	//////
	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
		write_file<6,6,total_steps>(state_path,input_path, soln_drone_rhc);
	////
		std::cout<<"finished"<<std::endl;
		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
		printf("Time: %f s\n", seconds);

/*First Order Drone Simulation Ends*/

/*First Order 2 Drone Simulation*/

//	cost<12,12> drone2_running_cost(Drone_First_Order_Cost::running_cost2,tag7);
//	cost<12,12> drone2_terminal_cost(Drone_First_Order_Cost::terminal_cost2,tag8);
//	dynamics<12,12> drone2_dynamics(Drone_First_Order_Dynamics::dynamics_2,tag9,time_step);
//	iLQR<12,12,horizon>::input_trajectory u_init_drone2;
//	iLQR<12,12,horizon> drone2_solver(drone2_running_cost,drone2_terminal_cost,drone2_dynamics);
//	Eigen::Matrix<double,12,1> u_drone2=Eigen::Matrix<double,12,1>::Zero();
//	Eigen::Matrix<double,12,1> x0_drone2=Eigen::Matrix<double,12,1>::Zero();
//	Eigen::Matrix<double,12,1> x_goal_drone2;
//	iLQR<12,12,horizon>::state_input_trajectory soln_drone2;
//	iLQR<12,12,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
//		std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
//	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
//		clock_t t_start, t_end;
//	//
//	////
////		x0_drone2<<0,0,0.5, 0,0,1;
//		x0_drone2<<-3,0,2.1, 0,0,0, 3,0,2, 0,0,0;
//		x_goal_drone2<<3,0,2, 0,0,0, -3,0,2, 0,0,0;
//
//	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
//		double seconds;
//		drone2_solver.set_MPC(u_init_drone2);
//		int execution_steps=1;
//		soln_drone2_rhc.first[0]=x0_drone2;
//		std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//		std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//		t_start = clock();
//		for(int i=0;i<total_steps;++i)
//		{
////			x_goal_drone2<<2,2,2, 0,0,0;
//	//		if (i>total_steps/3)
//	//			x_goal_integrator<<0,0,0;
//			std::cout<<"iteration "<<i<<std::endl;
//	//////
//	//		auto term1=soln_integrator_rhc.first[i]+0.2*Eigen::Matrix<double,3,1>::Random();
//	//		auto term2=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term3=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term4=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term5=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term6=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term7=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term8=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term9=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term10=soln_integrator_rhc.first[i]+0.02*Eigen::Matrix<double,3,1>::Random();
//	//		auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//			soln_drone2=drone2_solver.run_MPC(soln_drone2_rhc.first[i],
//					x_goal_drone2, 5, execution_steps);
//	////
//	////////		unsigned int microsecond = 1000000;
//	////////		usleep(2 * microsecond);
//	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//			soln_drone2_rhc.first[i+1]=soln_drone2.first[1];
//			soln_drone2_rhc.second[i]=soln_drone2.second[0];
//		}
//		t_end = clock();
//	////
//	//////
//	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
//	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//		write_file<12,12,total_steps>(state_path,input_path, soln_drone2_rhc);
//	////
//		std::cout<<"finished"<<std::endl;
//		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//		printf("Time: %f s\n", seconds);

/*First Order 2 Drone Simulation Ends*/
};


#pragma once

#include <scomplex/simplicial_complex.hpp>
#include <scomplex/types.hpp>

#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>
#include <cmath>
#include <exception>
#include <tuple>
#include <vector>

#include <memory>
#include <fstream>

namespace gsimp {

class non_zero_chain : public std::exception {};

class bounding_chain {
    std::shared_ptr<simplicial_complex> s_comp;
    std::vector<std::unique_ptr<matrix_t>> boundary_matrices;
    void populate_matrices();

   public:
    bounding_chain(simplicial_complex& sc);
    bounding_chain(std::shared_ptr<simplicial_complex> sc);
    bounding_chain(std::vector<point_t>& points, std::vector<cell_t>& tris);
    ~bounding_chain();
    chain_t get_bounding_chain(chain_t&);
};

//---------------------------------------
// implementation

bounding_chain::~bounding_chain() {}

bounding_chain::bounding_chain(std::shared_ptr<simplicial_complex> sc) {
    s_comp = sc;
    populate_matrices();
}

bounding_chain::bounding_chain(simplicial_complex& sc) {
    s_comp = std::make_shared<simplicial_complex>(sc);
    populate_matrices();
}

bounding_chain::bounding_chain(std::vector<point_t>& points,
                               std::vector<cell_t>& tris) {
    s_comp = std::make_shared<simplicial_complex>(points,tris);
    populate_matrices();
}

void bounding_chain::populate_matrices() {
    for (int d = 0; d < s_comp->dimension(); ++d) {
        auto level_matrix = s_comp->get_boundary_matrix(d);
        boundary_matrices.push_back(
            std::unique_ptr<matrix_t>(new matrix_t(level_matrix)));
    }
}

// round vectors for comparison
vector_t round_vec(vector_t vec) {
    vector_t rounded_vec(vec.rows());
    for (int i = 0; i < vec.rows(); ++i) {
        int coef = round(vec.coeffRef(i));
        if (coef != 0) rounded_vec.coeffRef(i) = coef;
    }
    return rounded_vec;
}

bool equals(vector_t vec1, vector_t vec2) {
    for (int i = 0; i < vec1.rows(); ++i) {
        if (vec1.coeffRef(i) != vec2.coeffRef(i)) {
            std::cout << i << ": " << vec1.coeffRef(i) << " "
                      << vec2.coeffRef(i) << '\n';
            return false;
        }
    }
    return true;
}

chain_t bounding_chain::get_bounding_chain(chain_t& chain) {
    int chain_d;
    vector_t chain_v;
    std::tie<int, vector_t>(chain_d, chain_v) = chain;

    if (chain_d >= s_comp->dimension()) throw non_zero_chain();

    Eigen::LeastSquaresConjugateGradient<matrix_t> lscg;
    lscg.compute(*(boundary_matrices.at(chain_d)));

    vector_t bound_chain(round_vec(lscg.solve(chain_v)));
    vector_t result(round_vec(*(boundary_matrices.at(chain_d)) * bound_chain));

    if (equals(result, chain_v))
        return chain_t(chain_d + 1, bound_chain);
    else
        throw non_zero_chain();
}

};


#include <boost/config.hpp>
#include <iostream>
#include <fstream> //file output
#include <cfloat>
#include <omp.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graph_traits.hpp>

void printUsageAndExit()
{
  printf("%s", "Usage:./rmatg x y\n");
  printf("%s", "x is the size of the graph, x>32 (Boost generator hang if x<32)\n");
  printf("%s", "y is the source of sssp\n");
  exit(0);
}

int main(int argc, char *argv[])
{
  // read size
  if (argc < 3) printUsageAndExit();
  int size = atoi (argv[1]);
  if (size<32) printUsageAndExit();
  int source_sssp =atoi (argv[2]);
  assert (size > 1 && size < INT_MAX);
  assert (source_sssp >= 0 && source_sssp < size);
  const unsigned num_edges = 15 * size;

  // Some boost types
  typedef boost::no_property VertexProperty;
  typedef boost::property<boost::edge_weight_t, float> EdgeProperty;
  typedef boost::adjacency_list<boost::mapS, boost::vecS, boost::directedS, VertexProperty, EdgeProperty> Graph;
  typedef boost::unique_rmat_iterator<boost::minstd_rand, Graph> RMATGen;
  typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  boost::minstd_rand gen;
  boost::graph_traits<Graph>::edge_iterator edge, edge_end;

  /************************
   * Random weights
   ************************/
  // !!! WARNING !!!
  // watch the stack
  float* weight = new float[num_edges];
  int count = 0;
  for( int i = 0; i < num_edges;  ++i)
    weight[i] = (rand()%10)+(rand()%100)*(1.2e-2f);

  /************************
   * RMAT Gen
   ************************/
  Graph g(RMATGen(gen, size, num_edges, 0.57, 0.19, 0.19, 0.05,true),RMATGen(),weight, size);
  std::cout << "Generator : done. Edges = "<<boost::num_edges(g)<<std::endl;
  assert (num_edges == boost::num_edges(g));
  // debug print after gen
  //for( boost::tie(edge, edge_end) = boost::edges(g); edge != edge_end; ++edge)
  //  std::cout << boost::source(*edge, g) << ' ' << boost::target(*edge, g)<< ' '<<  boost::get(boost::get(boost::edge_weight, g),*edge) << '\n';

  /************************
   * Dijkstra
   ************************/
  std::vector<vertex_descriptor> p(num_vertices(g));
  std::vector<float> d(num_vertices(g));
  vertex_descriptor s = vertex(source_sssp, g); //define soruce node

  double start = omp_get_wtime();
  dijkstra_shortest_paths(g, s,
                          predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
                          distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))));

  double stop = omp_get_wtime();
  std::cout << "Time = " << stop-start << "s"<< std::endl;

  /************************
   * Print
   ************************/
  /*
  boost::graph_traits<Graph>::vertex_iterator vi, vend;
  std::cout << "SOURCE = "<< source_sssp << std::endl;
  for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi)
  {
    if (d[*vi] != FLT_MAX)
    {
      std::cout << "d(" << *vi << ") = " << d[*vi] << ", ";
      std::cout << "parent = " << p[*vi] << std::endl;
    }
    else
      std::cout << "d(" << *vi << ") = INF"<< std::endl;
  }
  */
  return 0;

}


/*
 * Open Source Movement Analysis Library
 * Copyright (C) 2016, Moveck Solution Inc., all rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer.
 *     * 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.
 *     * Neither the name(s) of the copyright holders nor the names
 *       of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written
 *       permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "openma/body/unitquaternionposeestimator.h"

#include "openma/body/landmarksregistrar.h"
#include "openma/body/landmarkstranslator.h"
#include "openma/body/model.h"
#include "openma/body/point.h"
#include "openma/body/referenceframe.h"
#include "openma/body/segment.h"
#include "openma/body/skeletonhelper.h"
#include "openma/body/utils.h"
#include "openma/base/trial.h"
#include "openma/base/logger.h"
#include "openma/math.h"

#include <Eigen/Eigenvalues> // Eigen::SelfAdjointEigenSolver

#include <iostream>

// -------------------------------------------------------------------------- //
//                                 PUBLIC API                                 //
// -------------------------------------------------------------------------- //

OPENMA_INSTANCE_STATIC_TYPEID(ma::body::UnitQuaternionPoseEstimator);

namespace ma
{
namespace body
{
  /**
   * @class UnitQuaternionPoseEstimator openma/body/unitquaternionposeestimator.h
   * @brief Least square pose estimator based on unit quaternions
   *
   * This estimator is based on the paper of Horn [1] that use unit quaternions to find the rigid body transformation betwen two group of markers.
   *
   * @par References
   *  1. Horn B.K.P., Closed-form solution of absolute orientation using unit quaternions, Journal of the Optical Society of America, 1987, 4
   *
   * @ingroup openma_body
   */

  /**
   * Constructor
   */
  UnitQuaternionPoseEstimator::UnitQuaternionPoseEstimator(const std::string& name, Node* parent)
  : PoseEstimator(name,parent)
  {};

  /**
   * Destructor (default)
   */
  UnitQuaternionPoseEstimator::~UnitQuaternionPoseEstimator() _OPENMA_NOEXCEPT = default;

  /**
   * Internaly call the method UnitQuaternion::reconstruct().
   */
  bool UnitQuaternionPoseEstimator::run(Model* output, SkeletonHelper* helper, Trial* trial)
  {
    // 0. Check
    if (output == nullptr)
    {
      error("UnitQuaternionPoseEstimator - Null output passed. Pose estimator aborted.");
      return false;
    }
    if (helper == nullptr)
    {
      error("UnitQuaternionPoseEstimator - Null helper passed. Pose estimator aborted.");
      return false;
    }
    if (trial == nullptr)
    {
      error("UnitQuaternionPoseEstimator - Null trial passed. Pose estimator aborted.");
      return false;
    }
    // 1. Look for the child node MarkerClusterRegistration
    auto mcr = helper->findChild("MarkerClusterRegistration",{},false);
    if (mcr == nullptr)
    {
      error("UnitQuaternionPoseEstimator - No marker cluster registration found. Pose estimator aborted.");
      return false;
    }
    const auto& lt = helper->findChild<LandmarksTranslator*>({},{},false);
    const auto& segments = output->segments()->findChildren<Segment*>({},{},false);
    double startTime = 0.0, sampleRate = 0.0;
    bool ok = false;
    for (auto segment : segments)
    {
      const auto& lr = segment->findChild<LandmarksRegistrar*>({},{},false);
      if (lr == nullptr)
        continue;
      // Look for the markers in the trial and the marker cluster registration
      auto globalMarkers = extract_landmark_positions(nullptr, lr->retrieveLandmarks(lt, trial->timeSequences()), &sampleRate, &startTime, &ok);
      if (!ok)
      {
        error("UnitQuaternionPoseEstimator - The sampling information is not consistent between required landmarks (sampling rates or start times are not the same). Calibration aborted.");
        return false;
      }
      std::vector<std::pair<Eigen::Map<Eigen::Matrix<double,3,1>>,const math::Map<math::Position>&>> mappedMarkers;
      mappedMarkers.reserve(globalMarkers.size());
      auto it = globalMarkers.begin();
      while (it != globalMarkers.end())
      {
        Point* localMarker = nullptr;
        if (!it->second.isValid() || ((localMarker = mcr->findChild<Point*>(segment->name()+"."+it->first,{},false)) == nullptr))
          it = globalMarkers.erase(it);
        else
        {
          mappedMarkers.push_back({localMarker->data(),it->second});
          ++it;
        }
      }
      assert(globalMarkers.size() == mappedMarkers.size());
      if (mappedMarkers.size() < 3)
      {
        error("Less than 3 valid markers was found for the segment '%s'. Impossible to compute the TCS. Pose estimator aborted.", segment->name().c_str());
        return false;
      }
      // Reconstruct for each sample
      unsigned numSamples = std::numeric_limits<unsigned>::max();
      for (const auto& marker : globalMarkers)
      {
        numSamples = std::min<unsigned>(numSamples, marker.second.rows());
        if (marker.second.rows() != numSamples)
        {
          error("The number of samples for the markers used by the cluster '%s.Cluster' is not the same. Impossible to compute the TCS. Pose estimator aborted.", segment->name().c_str());
          return false;
        }
      }
      ma::math::Pose tcs(numSamples);
      Eigen::Matrix<double,3,3> M;
      Eigen::Matrix<double,4,4> N;
      Eigen::Matrix<double,3,Eigen::Dynamic> ps1, ps2;
      for (unsigned i = 0 ; i < numSamples ; ++i)
      {
        int inc = 0;
        ps1.setZero(3,static_cast<int>(globalMarkers.size()));
        ps2.setZero(3,static_cast<int>(globalMarkers.size()));
        for (const auto& m : mappedMarkers)
        {
          if (m.second.residuals().coeff(i) >= 0.0)
          {
            ps1.col(inc) = m.first; // Local
            ps2.col(inc) = m.second.values().row(i); // Global
            ++inc;
          }
        }
        if (inc < 3) // Not enough landmark to create the least square fitting.
        {
          tcs.residuals().coeffRef(i) = -1.0;
          continue;
        }
        ps1.resize(Eigen::NoChange,inc);
        ps2.resize(Eigen::NoChange,inc);
        // Express the point sets regarding to their respective center
        Eigen::Matrix<double,3,1> p1 = (ps1.rowwise().sum() / static_cast<double>(inc));
        ps1 -= p1.replicate(1,inc);
        // ps1.row(0).array() -= p1.x();
        // ps1.row(1).array() -= p1.y();
        // ps1.row(2).array() -= p1.z();
        Eigen::Matrix<double,3,1> p2 = ps2.rowwise().sum() / static_cast<double>(inc);
        ps2 -= p2.replicate(1,inc);
        // ps2.row(0).array() -= p2.x();
        // ps2.row(1).array() -= p2.y();
        // ps2.row(2).array() -= p2.z();
        // Build the matrice N
        M.setZero();
        for (int j = 0 ; j < inc ; ++j)
          M += ps1.col(j) * ps2.col(j).transpose();
        N.setZero();
        N.coeffRef(0,1) = N.coeffRef(1,0) = M.coeff(1,2) - M.coeff(2,1);
        N.coeffRef(0,2) = N.coeffRef(2,0) = M.coeff(2,0) - M.coeff(0,2);
        N.coeffRef(0,3) = N.coeffRef(3,0) = M.coeff(0,1) - M.coeff(1,0);
        N.coeffRef(1,2) = N.coeffRef(2,1) = M.coeff(0,1) + M.coeff(1,0);
        N.coeffRef(1,3) = N.coeffRef(3,1) = M.coeff(2,0) + M.coeff(0,2);
        N.coeffRef(2,3) = N.coeffRef(3,2) = M.coeff(1,2) + M.coeff(2,1);
        N.coeffRef(0,0) =  M.coeff(0,0) + M.coeff(1,1) + M.coeff(2,2);
        N.coeffRef(1,1) =  M.coeff(0,0) - M.coeff(1,1) - M.coeff(2,2);
        N.coeffRef(2,2) = -M.coeff(0,0) + M.coeff(1,1) - M.coeff(2,2);
        N.coeffRef(3,3) = -M.coeff(0,0) - M.coeff(1,1) + M.coeff(2,2);
        // Extract the eigen vector associated with the most positive eigen value and compute the rotation matrix (the extracted eigen vector is a quaternion)
        Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double,4,4> > eig(N);
        int idx; eig.eigenvalues().maxCoeff(&idx);
        // Q2R need to be done in 2 steps as the eigen vector is formatted as WXYZ and the internal storage in Eigen is XYZW
        Eigen::Matrix<double,4,1> q = eig.eigenvectors().col(idx);
        Eigen::Matrix<double,3,3> R = Eigen::Quaternion<double>(q.coeff(0),q.coeff(1),q.coeff(2),q.coeff(3)).toRotationMatrix();
        // Set the pose
        auto row = tcs.values().row(i);
        row.segment<3>(0) = R.col(0);    // u
        row.segment<3>(3) = R.col(1);    // v
        row.segment<3>(6) = R.col(2);    // w
        row.segment<3>(9) = p2 - R * p1; // o
        tcs.residuals().coeffRef(i) = 0.0;
      }
      // Reconstruction of the SCS
      // Look for Reference frame in the node MarkerClusterRegistration
      auto relframe = mcr->findChild<ReferenceFrame*>(segment->name() + ".SCS", {}, false);
      if (relframe != nullptr)
      {
        relframe->addParent(segment);
        math::to_timesequence(transform_relative_frame(relframe, segment, tcs), segment->name() + ".SCS", sampleRate, startTime, TimeSequence::Pose, "", segment);
      }
      math::to_timesequence(tcs, segment->name() + ".TCS", sampleRate, startTime, TimeSequence::Pose, "", segment);
    }
    return true;
  }
};
};


#ifndef STAN_MATH_PRIM_SCAL_FUN_LGAMMA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LGAMMA_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace stan {
namespace math {

/**
 * Return the natural logarithm of the gamma function applied to
 * the specified argument.
 *
   \f[
   \mbox{lgamma}(x) =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \ln\Gamma(x) & \mbox{if } x\not\in \{\dots, -3, -2, -1, 0\}\\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{lgamma}(x)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x\in \{\dots, -3, -2, -1, 0\}\\
     \Psi(x) & \mbox{if } x\not\in \{\dots, -3, -2, -1, 0\}\\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN}
   \end{cases}
\f]
*
* @param x argument
* @return natural logarithm of the gamma function applied to
* argument
*/
inline double lgamma(double x) {
  return boost::math::lgamma(x, boost_policy_t());
}

/**
 * Return the natural logarithm of the gamma function applied
 * to the specified argument.
 *
 * @param x argument
 * @return natural logarithm of the gamma function applied to
 * argument
 */
inline double lgamma(int x) { return boost::math::lgamma(x, boost_policy_t()); }

}  // namespace math
}  // namespace stan
#endif


#include <NTL/ZZ_p.h>
#include <NTL/xdouble.h>
#include <sstream>
#include <vector>
#include <tuple>
#include <complex>
#include <parallel/algorithm>
#include <fftw3.h>
#include "bleichenbacher.h"

using namespace std;
using namespace NTL;

xdouble average, stdDev_accum;

struct {

	bool operator()(tuple<ZZ_p, ZZ_p> a,
		tuple<ZZ_p, ZZ_p> b) const
	{
		ZZ a_ZZ, b_ZZ;

		a_ZZ = rep(get<1>(a));
		b_ZZ = rep(get<1>(b));

		return a_ZZ < b_ZZ;
	}

} compareHCtuple;

template<class To, class From>
To NTLtoOther(From *x)
{
	stringstream ss;
	ss << *x;
	return atof(ss.str().c_str());
}

double Z_exp(ZZ_p h);
vector<tuple<int, double>> internal_maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int Z_index_divisor,
	int l);
void internal_hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int numBits,
	ZZ knownBits,
	bool specialH);

void hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int numBits,
	ZZ knownBits)
{
	internal_hcFromRs(rsmTuples,
		hcPairs,
		numBits,
		knownBits,
		true);
}

void hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs)
{
	ZZ zero;
	zero = 0;
	internal_hcFromRs(rsmTuples,
		hcPairs,
		0,
		zero,
		false);
}

void internal_hcFromRs(vector<tuple<ZZ_p, ZZ_p, ZZ_p>> *rsmTuples,
	vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int numBits,
	ZZ knownBits,
	bool specialH)
{
	for(vector<tuple<ZZ_p, ZZ_p, ZZ_p>>::iterator it = rsmTuples->begin();
		it != rsmTuples->end(); ++it)
	{
		ZZ_p h, c, r, s, m, sInverse;

		r = get<0>(*it);
		s = get<1>(*it);
		m = get<2>(*it);

		if(s == 0) continue;

		power(sInverse, s, -1);

		c = r * sInverse;

		if(specialH)
		{
			ZZ modulus, keyBits;

			/* Place known key bits in MSBs of modulus */
			keyBits = knownBits;
			keyBits >>= (NumBits(keyBits) - numBits);
			keyBits <<= (NumBits(r.modulus()) - numBits);

			h = m * sInverse + c * to_ZZ_p(keyBits);

		} else {
			h = m * sInverse;
		}

		hcPairs->push_back(make_tuple(h, c));
	}
}

void sortAndDiff(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int l,
	int t)
{
	ZZ comp;
	ZZ_p zero;
	int S;

	S = hcPairs->size();
	omp_set_nested(1);
	omp_set_num_threads(NUM_CPUs * THREADS_PER_CPU);

	for(int i = 0; i < t; i++)
	{
		ZZ_p hFirst, cFirst;

		__gnu_parallel::sort(hcPairs->begin(), hcPairs->end(),
			compareHCtuple);

		for(int j = 0; j <= S-t; j++)
		{
			ZZ_p new_h, new_c;

			sub(new_h, get<0>((*hcPairs)[j+1]),
				get<0>((*hcPairs)[j]));
			sub(new_c, get<1>((*hcPairs)[j+1]),
				get<1>((*hcPairs)[j]));

			hcPairs->at(j) = make_tuple(new_h, new_c);
		}
	}

	/* Remove (h,c) pairs with c < 2^l */

	comp = 2;
	zero = 0;
	power(comp, comp, l);

	for(int i = 0; i < S; i++)
	{
		if(rep(get<1>((*hcPairs)[i])) >= comp)
			hcPairs->at(i) = make_tuple(zero, zero);
	}

	__gnu_parallel::sort(hcPairs->begin(), hcPairs->end(),
			compareHCtuple);

	if(rep(get<1>((*hcPairs)[S-1])) == rep(zero))
	{
		hcPairs->clear();
		return;
	}

	for(int i = 0; i < S; i++)
	{
		if(rep(get<1>((*hcPairs)[i])) != rep(zero))
		{
			hcPairs->erase(hcPairs->begin(),
				(hcPairs->begin()) + i);
			break;
		}
	}
}

vector<tuple<int, double>> maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int l)
{
	return internal_maxM(hcPairs, 1, l);
}

vector<tuple<int, double>> maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int bits,
	int l)
{
	return internal_maxM(hcPairs, bits, l);
}

vector<tuple<int, double>> internal_maxM(vector<tuple<ZZ_p, ZZ_p>> *hcPairs,
	int Z_index_divisor,
	int l)
{
	int size;
	fftw_complex *in, *out;
    	fftw_plan p;
	vector<tuple<int, double>> indexValuePairs;

	size = pow(2,l)/Z_index_divisor;

	in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * size);
    	out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * size);
	p = fftw_plan_dft_1d(size, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);

	/* Compute and store Z values using Euler's Formula */
	for(vector<tuple<ZZ_p, ZZ_p>>::iterator it = hcPairs->begin();
		it != hcPairs->end(); ++it)
	{
		int c;
		double z_exp;
		ZZ zz_c;

		zz_c = rep(get<1>(*it));
		c = NTLtoOther<int, ZZ>(&zz_c);
		z_exp = Z_exp(get<0>(*it));

		in[c/Z_index_divisor][0] += cos(z_exp); // Re
		in[c/Z_index_divisor][1] += sin(z_exp); // Im
	}

	/* Do FFT */
	fftw_execute(p);

	/* Get max |Z_m| */
	average = 0;
	for(int i = 0; i < size; i++)
	{
		double magnitude;

		magnitude = abs(complex<double>((out[i])[0], (out[i])[1]));
		average += magnitude;

		if(indexValuePairs.size() < 10)
		{
			indexValuePairs.push_back(make_tuple(i, magnitude));
		} else {
			for(int j = 0; j < 10; j++)
			{
				if(magnitude > get<1>(indexValuePairs[j]))
				{
					indexValuePairs.at(j) =
						make_tuple(i, magnitude);
					break;
				}
			}
		}
	}
	average /= size;

	/* Multiply the 1/L back in */
	average /= hcPairs->size();
	for(int i = 0; i < 10; i++)
	{
		int index;
		double B;

		index = get<0>(indexValuePairs[i]);
		B = get<1>(indexValuePairs[i]) / hcPairs->size();

		indexValuePairs.at(i) = make_tuple(index, B);
	}

	/* Compute square of standard deviation */
	stdDev_accum = 0;
	for(int i = 0; i < size; i++)
	{
		xdouble term;
		term = abs(complex<double>((out[i])[0], (out[i])[1]))
			/ hcPairs->size();
		term -= average;
		power(term, term, 2);
		stdDev_accum += term;
	}
	stdDev_accum /= size;

	fftw_destroy_plan(p);
	fftw_free(in);
	fftw_free(out);

	return indexValuePairs;
}

double avgBias()
{
	double ret;
	conv(ret, average);
	return ret;
}

double stdDevBias()
{
	double ret;
	conv(ret, stdDev_accum);
	ret = sqrt(ret);
	return ret;
}

double Z_exp(ZZ_p h)
{
	ZZ n;
	xdouble h_doub, n_doub, x, pi2;

	pi2 = 2 * M_PI;

	n = h.modulus();
	n_doub = to_xdouble(n);
	h_doub = to_xdouble(rep(h));
	x = pi2 * h_doub / n_doub;

	return NTLtoOther<double, xdouble>(&x);
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_FALLING_FACTORIAL_HPP
#define STAN_MATH_PRIM_SCAL_FUN_FALLING_FACTORIAL_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the falling factorial function evaluated
 * at the inputs.
 * Will throw for NaN x and for negative n
 *
 * @tparam T Type of x argument.
 * @param x Argument.
 * @param n Argument
 * @return Result of falling factorial function.
 * @throw std::domain_error if x is NaN
 * @throw std::domain_error if n is negative
 *
   \f[
   \mbox{falling\_factorial}(x, n) =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0\\
     (x)_n & \mbox{if } x > 0 \textrm{ and } -\infty \leq n \leq \infty \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } n = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{falling\_factorial}(x, n)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0\\
     \frac{\partial\, (x)_n}{\partial x} & \mbox{if } x > 0 \textrm{ and }
 -\infty \leq n \leq \infty \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or
 } n = \textrm{NaN} \end{cases} \f]

   \f[
   \frac{\partial\, \mbox{falling\_factorial}(x, n)}{\partial n} =
   \begin{cases}
     \textrm{error} & \mbox{if } x \leq 0\\
     \frac{\partial\, (x)_n}{\partial n} & \mbox{if } x > 0 \textrm{ and }
 -\infty \leq n \leq \infty \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or
 } n = \textrm{NaN} \end{cases} \f]

   \f[
   (x)_n=\frac{\Gamma(x+1)}{\Gamma(x-n+1)}
   \f]

   \f[
   \frac{\partial \, (x)_n}{\partial x} = (x)_n\Psi(x+1)
   \f]

   \f[
   \frac{\partial \, (x)_n}{\partial n} = -(x)_n\Psi(n+1)
   \f]
 *
 */
template <typename T>
inline return_type_t<T> falling_factorial(const T& x, int n) {
  static const char* function = "falling_factorial";
  check_not_nan(function, "first argument", x);
  check_nonnegative(function, "second argument", n);
  return boost::math::falling_factorial(x, n, boost_policy_t());
}

}  // namespace math
}  // namespace stan

#endif