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

#ifndef BOOST_MATH_SPECIAL_FUNCTIONS_IGAMMA_INVERSE_HPP
#define BOOST_MATH_SPECIAL_FUNCTIONS_IGAMMA_INVERSE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/tr1/tuple.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T>
T find_inverse_s(T p, T q)
{
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   // See equation 32.
   //
   BOOST_MATH_STD_USING
   T t;
   if(p < 0.5)
   {
      t = sqrt(-2 * log(p));
   }
   else
   {
      t = sqrt(-2 * log(q));
   }
   static const double a[4] = { 3.31125922108741, 11.6616720288968, 4.28342155967104, 0.213623493715853 };
   static const double b[5] = { 1, 6.61053765625462, 6.40691597760039, 1.27364489782223, 0.3611708101884203e-1 };
   T s = t - tools::evaluate_polynomial(a, t) / tools::evaluate_polynomial(b, t);
   if(p < 0.5)
      s = -s;
   return s;
}

template <class T>
T didonato_SN(T a, T x, unsigned N, T tolerance = 0)
{
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   // See equation 34.
   //
   T sum = 1;
   if(N >= 1)
   {
      T partial = x / (a + 1);
      sum += partial;
      for(unsigned i = 2; i <= N; ++i)
      {
         partial *= x / (a + i);
         sum += partial;
         if(partial < tolerance)
            break;
      }
   }
   return sum;
}

template <class T, class Policy>
inline T didonato_FN(T p, T a, T x, unsigned N, T tolerance, const Policy& pol)
{
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   // See equation 34.
   //
   BOOST_MATH_STD_USING
   T u = log(p) + boost::math::lgamma(a + 1, pol);
   return exp((u + x - log(didonato_SN(a, x, N, tolerance))) / a);
}

template <class T, class Policy>
T find_inverse_gamma(T a, T p, T q, const Policy& pol, bool* p_has_10_digits)
{
   //
   // In order to understand what's going on here, you will
   // need to refer to:
   //
   // Computation of the Incomplete Gamma Function Ratios and their Inverse
   // ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR.
   // ACM Transactions on Mathematical Software, Vol. 12, No. 4,
   // December 1986, Pages 377-393.
   //
   BOOST_MATH_STD_USING

   T result;
   *p_has_10_digits = false;

   if(a == 1)
   {
      result = -log(q);
      BOOST_MATH_INSTRUMENT_VARIABLE(result);
   }
   else if(a < 1)
   {
      T g = boost::math::tgamma(a, pol);
      T b = q * g;
      BOOST_MATH_INSTRUMENT_VARIABLE(g);
      BOOST_MATH_INSTRUMENT_VARIABLE(b);
      if((b > 0.6) || ((b >= 0.45) && (a >= 0.3)))
      {
         // DiDonato & Morris Eq 21:
         //
         // There is a slight variation from DiDonato and Morris here:
         // the first form given here is unstable when p is close to 1,
         // making it impossible to compute the inverse of Q(a,x) for small
         // q.  Fortunately the second form works perfectly well in this case.
         //
         T u;
         if((b * q > 1e-8) && (q > 1e-5))
         {
            u = pow(p * g * a, 1 / a);
            BOOST_MATH_INSTRUMENT_VARIABLE(u);
         }
         else
         {
            u = exp((-q / a) - constants::euler<T>());
            BOOST_MATH_INSTRUMENT_VARIABLE(u);
         }
         result = u / (1 - (u / (a + 1)));
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if((a < 0.3) && (b >= 0.35))
      {
         // DiDonato & Morris Eq 22:
         T t = exp(-constants::euler<T>() - b);
         T u = t * exp(t);
         result = t * exp(u);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if((b > 0.15) || (a >= 0.3))
      {
         // DiDonato & Morris Eq 23:
         T y = -log(b);
         T u = y - (1 - a) * log(y);
         result = y - (1 - a) * log(u) - log(1 + (1 - a) / (1 + u));
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if (b > 0.1)
      {
         // DiDonato & Morris Eq 24:
         T y = -log(b);
         T u = y - (1 - a) * log(y);
         result = y - (1 - a) * log(u) - log((u * u + 2 * (3 - a) * u + (2 - a) * (3 - a)) / (u * u + (5 - a) * u + 2));
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else
      {
         // DiDonato & Morris Eq 25:
         T y = -log(b);
         T c1 = (a - 1) * log(y);
         T c1_2 = c1 * c1;
         T c1_3 = c1_2 * c1;
         T c1_4 = c1_2 * c1_2;
         T a_2 = a * a;
         T a_3 = a_2 * a;

         T c2 = (a - 1) * (1 + c1);
         T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2);
         T c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6);
         T c5 = (a - 1) * (-(c1_4 / 4)
                           + (11 * a - 17) * c1_3 / 6
                           + (-3 * a_2 + 13 * a -13) * c1_2
                           + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2
                           + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12);

         T y_2 = y * y;
         T y_3 = y_2 * y;
         T y_4 = y_2 * y_2;
         result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4);
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
         if(b < 1e-28f)
            *p_has_10_digits = true;
      }
   }
   else
   {
      // DiDonato and Morris Eq 31:
      T s = find_inverse_s(p, q);

      BOOST_MATH_INSTRUMENT_VARIABLE(s);

      T s_2 = s * s;
      T s_3 = s_2 * s;
      T s_4 = s_2 * s_2;
      T s_5 = s_4 * s;
      T ra = sqrt(a);

      BOOST_MATH_INSTRUMENT_VARIABLE(ra);

      T w = a + s * ra + (s * s -1) / 3;
      w += (s_3 - 7 * s) / (36 * ra);
      w -= (3 * s_4 + 7 * s_2 - 16) / (810 * a);
      w += (9 * s_5 + 256 * s_3 - 433 * s) / (38880 * a * ra);

      BOOST_MATH_INSTRUMENT_VARIABLE(w);

      if((a >= 500) && (fabs(1 - w / a) < 1e-6))
      {
         result = w;
         *p_has_10_digits = true;
         BOOST_MATH_INSTRUMENT_VARIABLE(result);
      }
      else if (p > 0.5)
      {
         if(w < 3 * a)
         {
            result = w;
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            T D = (std::max)(T(2), T(a * (a - 1)));
            T lg = boost::math::lgamma(a, pol);
            T lb = log(q) + lg;
            if(lb < -D * 2.3)
            {
               // DiDonato and Morris Eq 25:
               T y = -lb;
               T c1 = (a - 1) * log(y);
               T c1_2 = c1 * c1;
               T c1_3 = c1_2 * c1;
               T c1_4 = c1_2 * c1_2;
               T a_2 = a * a;
               T a_3 = a_2 * a;

               T c2 = (a - 1) * (1 + c1);
               T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2);
               T c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6);
               T c5 = (a - 1) * (-(c1_4 / 4)
                                 + (11 * a - 17) * c1_3 / 6
                                 + (-3 * a_2 + 13 * a -13) * c1_2
                                 + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2
                                 + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12);

               T y_2 = y * y;
               T y_3 = y_2 * y;
               T y_4 = y_2 * y_2;
               result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4);
               BOOST_MATH_INSTRUMENT_VARIABLE(result);
            }
            else
            {
               // DiDonato and Morris Eq 33:
               T u = -lb + (a - 1) * log(w) - log(1 + (1 - a) / (1 + w));
               result = -lb + (a - 1) * log(u) - log(1 + (1 - a) / (1 + u));
               BOOST_MATH_INSTRUMENT_VARIABLE(result);
            }
         }
      }
      else
      {
         T z = w;
         T ap1 = a + 1;
         if(w < 0.15f * ap1)
         {
            // DiDonato and Morris Eq 35:
            T v = log(p) + boost::math::lgamma(ap1, pol);
            T s = 1;
            z = exp((v + w) / a);
            s = boost::math::log1p(z / ap1 * (1 + z / (a + 2)));
            z = exp((v + z - s) / a);
            z = exp((v + z - s) / a);
            s = boost::math::log1p(z / ap1 * (1 + z / (a + 2) * (1 + z / (a + 3))));
            z = exp((v + z - s) / a);
            BOOST_MATH_INSTRUMENT_VARIABLE(z);
         }

         if((z <= 0.01 * ap1) || (z > 0.7 * ap1))
         {
            result = z;
            if(z <= 0.002 * ap1)
               *p_has_10_digits = true;
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
         else
         {
            // DiDonato and Morris Eq 36:
            T ls = log(didonato_SN(a, z, 100, T(1e-4)));
            T v = log(p) + boost::math::lgamma(ap1, pol);
            z = exp((v + z - ls) / a);
            result = z * (1 - (a * log(z) - z - v + ls) / (a - z));

            BOOST_MATH_INSTRUMENT_VARIABLE(result);
         }
      }
   }
   return result;
}

template <class T, class Policy>
struct gamma_p_inverse_func
{
   gamma_p_inverse_func(T a_, T p_, bool inv) : a(a_), p(p_), invert(inv)
   {
      //
      // If p is too near 1 then P(x) - p suffers from cancellation
      // errors causing our root-finding algorithms to "thrash", better
      // to invert in this case and calculate Q(x) - (1-p) instead.
      //
      // Of course if p is *very* close to 1, then the answer we get will
      // be inaccurate anyway (because there's not enough information in p)
      // but at least we will converge on the (inaccurate) answer quickly.
      //
      if(p > 0.9)
      {
         p = 1 - p;
         invert = !invert;
      }
   }

   std::tr1::tuple<T, T, T> operator()(const T& x)const
   {
      BOOST_FPU_EXCEPTION_GUARD
      //
      // Calculate P(x) - p and the first two derivates, or if the invert
      // flag is set, then Q(x) - q and it's derivatives.
      //
      typedef typename policies::evaluation<T, Policy>::type value_type;
      typedef typename lanczos::lanczos<T, Policy>::type evaluation_type;
      typedef typename policies::normalise<
         Policy,
         policies::promote_float<false>,
         policies::promote_double<false>,
         policies::discrete_quantile<>,
         policies::assert_undefined<> >::type forwarding_policy;

      BOOST_MATH_STD_USING  // For ADL of std functions.

      T f, f1;
      value_type ft;
      f = static_cast<T>(boost::math::detail::gamma_incomplete_imp(
               static_cast<value_type>(a),
               static_cast<value_type>(x),
               true, invert,
               forwarding_policy(), &ft));
      f1 = static_cast<T>(ft);
      T f2;
      T div = (a - x - 1) / x;
      f2 = f1;
      if((fabs(div) > 1) && (tools::max_value<T>() / fabs(div) < f2))
      {
         // overflow:
         f2 = -tools::max_value<T>() / 2;
      }
      else
      {
         f2 *= div;
      }

      if(invert)
      {
         f1 = -f1;
         f2 = -f2;
      }

      return std::tr1::make_tuple(f - p, f1, f2);
   }
private:
   T a, p;
   bool invert;
};

template <class T, class Policy>
T gamma_p_inv_imp(T a, T p, const Policy& pol)
{
   BOOST_MATH_STD_USING  // ADL of std functions.

   static const char* function = "boost::math::gamma_p_inv<%1%>(%1%, %1%)";

   BOOST_MATH_INSTRUMENT_VARIABLE(a);
   BOOST_MATH_INSTRUMENT_VARIABLE(p);

   if(a <= 0)
      policies::raise_domain_error<T>(function, "Argument a in the incomplete gamma function inverse must be >= 0 (got a=%1%).", a, pol);
   if((p < 0) || (p > 1))
      policies::raise_domain_error<T>(function, "Probabilty must be in the range [0,1] in the incomplete gamma function inverse (got p=%1%).", p, pol);
   if(p == 1)
      return tools::max_value<T>();
   if(p == 0)
      return 0;
   bool has_10_digits;
   T guess = detail::find_inverse_gamma<T>(a, p, 1 - p, pol, &has_10_digits);
   if((policies::digits<T, Policy>() <= 36) && has_10_digits)
      return guess;
   T lower = tools::min_value<T>();
   if(guess <= lower)
      guess = tools::min_value<T>();
   BOOST_MATH_INSTRUMENT_VARIABLE(guess);
   //
   // Work out how many digits to converge to, normally this is
   // 2/3 of the digits in T, but if the first derivative is very
   // large convergence is slow, so we'll bump it up to full
   // precision to prevent premature termination of the root-finding routine.
   //
   unsigned digits = policies::digits<T, Policy>();
   if(digits < 30)
   {
      digits *= 2;
      digits /= 3;
   }
   else
   {
      digits /= 2;
      digits -= 1;
   }
   if((a < 0.125) && (fabs(gamma_p_derivative(a, guess, pol)) > 1 / sqrt(tools::epsilon<T>())))
      digits = policies::digits<T, Policy>() - 2;
   //
   // Go ahead and iterate:
   //
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   guess = tools::halley_iterate(
      detail::gamma_p_inverse_func<T, Policy>(a, p, false),
      guess,
      lower,
      tools::max_value<T>(),
      digits,
      max_iter);
   policies::check_root_iterations(function, max_iter, pol);
   BOOST_MATH_INSTRUMENT_VARIABLE(guess);
   if(guess == lower)
      guess = policies::raise_underflow_error<T>(function, "Expected result known to be non-zero, but is smaller than the smallest available number.", pol);
   return guess;
}

template <class T, class Policy>
T gamma_q_inv_imp(T a, T q, const Policy& pol)
{
   BOOST_MATH_STD_USING  // ADL of std functions.

   static const char* function = "boost::math::gamma_q_inv<%1%>(%1%, %1%)";

   if(a <= 0)
      policies::raise_domain_error<T>(function, "Argument a in the incomplete gamma function inverse must be >= 0 (got a=%1%).", a, pol);
   if((q < 0) || (q > 1))
      policies::raise_domain_error<T>(function, "Probabilty must be in the range [0,1] in the incomplete gamma function inverse (got q=%1%).", q, pol);
   if(q == 0)
      return tools::max_value<T>();
   if(q == 1)
      return 0;
   bool has_10_digits;
   T guess = detail::find_inverse_gamma<T>(a, 1 - q, q, pol, &has_10_digits);
   if((policies::digits<T, Policy>() <= 36) && has_10_digits)
      return guess;
   T lower = tools::min_value<T>();
   if(guess <= lower)
      guess = tools::min_value<T>();
   //
   // Work out how many digits to converge to, normally this is
   // 2/3 of the digits in T, but if the first derivative is very
   // large convergence is slow, so we'll bump it up to full
   // precision to prevent premature termination of the root-finding routine.
   //
   unsigned digits = policies::digits<T, Policy>();
   if(digits < 30)
   {
      digits *= 2;
      digits /= 3;
   }
   else
   {
      digits /= 2;
      digits -= 1;
   }
   if((a < 0.125) && (fabs(gamma_p_derivative(a, guess, pol)) > 1 / sqrt(tools::epsilon<T>())))
      digits = policies::digits<T, Policy>();
   //
   // Go ahead and iterate:
   //
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   guess = tools::halley_iterate(
      detail::gamma_p_inverse_func<T, Policy>(a, q, true),
      guess,
      lower,
      tools::max_value<T>(),
      digits,
      max_iter);
   policies::check_root_iterations(function, max_iter, pol);
   if(guess == lower)
      guess = policies::raise_underflow_error<T>(function, "Expected result known to be non-zero, but is smaller than the smallest available number.", pol);
   return guess;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   gamma_p_inv(T1 a, T2 p, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::gamma_p_inv_imp(
      static_cast<result_type>(a),
      static_cast<result_type>(p), pol);
}

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   gamma_q_inv(T1 a, T2 p, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::gamma_q_inv_imp(
      static_cast<result_type>(a),
      static_cast<result_type>(p), pol);
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   gamma_p_inv(T1 a, T2 p)
{
   return gamma_p_inv(a, p, policies::policy<>());
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   gamma_q_inv(T1 a, T2 p)
{
   return gamma_q_inv(a, p, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_FUNCTIONS_IGAMMA_INVERSE_HPP


/*
 * dynamics_structure.cpp
 *
 *  Created on: Jan 20, 2021
 *      Author: talhakavuncu
 */

#include <iostream>
#include <unsupported/Eigen/AdolcForward>
#include <adolc/adolc.h>
#include <Eigen/Dense>
#include "dynamics_structure.h"
namespace Unicycle_Dynamics
{
	unicycle_state_tensor dynamics(const unicycle_state_tensor & x,const unicycle_input_tensor & u)
	{
		unicycle_state_tensor x_dot;

	//	std::cout<<"no issues"<<std::endl;
	//	std::cout<<x.rows()<<std::endl;
		adouble px,py,theta,v,omega,a;
	//	px=x[0];
	//	py=x[1];
	//
	//	theta=x[2];
	//
	//	v=x[3];
	//
	//	omega=u[0];
	//	a=u[1];
	//
	//	x_dot[0]=v*cos(theta);
	//	x_dot[1]=v*sin(theta);
	//	x_dot[2]=omega;
	//	x_dot[3]=a;

	//	px=x[0];
	//	py=x[1];
	//
	//	theta=x[2];
	//
	//	v=x[3];
	//
	//	omega=u[0];
	//	a=u[1];

		x_dot[0]=x[3]*cos(x[2]);
		x_dot[1]=x[3]*sin(x[2]);
		x_dot[2]=u[0];
		x_dot[3]=u[1];

		return x_dot;

	};
	unicycle2_state_tensor dynamics_2(const unicycle2_state_tensor & x,const unicycle2_input_tensor & u)
	{
		unicycle2_state_tensor x_dot;

		for(int i=0;i<2;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));

		return x_dot;
	};
	unicycle4_state_tensor dynamics_4(const unicycle4_state_tensor & x,const unicycle4_input_tensor & u)
	{
		unicycle4_state_tensor x_dot;

		for(int i=0;i<4;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));

		return x_dot;
	};

	unicycle3_state_tensor dynamics_3(const unicycle3_state_tensor & x,const unicycle3_input_tensor & u)
	{
		unicycle3_state_tensor x_dot;
		for(int i=0;i<3;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));
//	//	std::cout<<"no issues"<<std::endl;
//	//	std::cout<<x.rows()<<std::endl;
//		adouble px1,py1,theta1,v1,omega1,a1;
//		adouble px2,py2,theta2,v2,omega2,a2;
//		adouble px3,py3,theta3,v3,omega3,a3;
//		px1=x[0];
//		py1=x[1];
//		theta1=x[2];
//		v1=x[3];
//		omega1=u[0];
//		a1=u[1];
//
//		px2=x[4];
//		py2=x[5];
//		theta2=x[6];
//		v2=x[7];
//		omega2=u[2];
//		a2=u[3];
//
//
//		px3=x[8];
//		py3=x[9];
//		theta3=x[10];
//		v3=x[11];
//		omega3=u[4];
//		a3=u[5];
//
//		x_dot[0]=v1*cos(theta1);
//		x_dot[1]=v1*sin(theta1);
//		x_dot[2]=omega1;
//		x_dot[3]=a1;
//
//		x_dot[4]=v2*cos(theta2);
//		x_dot[5]=v2*sin(theta2);
//		x_dot[6]=omega2;
//		x_dot[7]=a2;
//
//		x_dot[8]=v3*cos(theta3);
//		x_dot[9]=v3*sin(theta3);
//		x_dot[10]=omega3;
//		x_dot[11]=a3;

		return x_dot;

	};

	unicycle6_state_tensor dynamics_6(const unicycle6_state_tensor & x,const unicycle6_input_tensor & u)
	{
		unicycle6_state_tensor x_dot;
		for(int i=0;i<6;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));

		return x_dot;
	}
}

namespace Drone_Dynamics{

drone_state_tensor dynamics(const drone_state_tensor & X,const drone_input_tensor & u)
{
	drone_state_tensor X_dot;
	adouble x,y,z,phi,theta,psi,x_d,y_d,z_d,phi_d,theta_d,psi_d,w1,w2,w3,w4,thrust;
	x=X[0];
	y=X[1];
	z=X[2];
	phi=X[3];
	theta=X[4];
	psi=X[5];
	x_d=X[6];
	y_d=X[7];
	z_d=X[8];
	phi_d=X[9];
	theta_d=X[10];
	psi_d=X[11];

	w1=u[0];
	w2=u[1];
	w3=u[2];
	w4=u[3];

	Eigen::Matrix<adouble,3,3> Rsb,mat,mat_inv,I,I_inv;

	Rsb<<cos(theta)*cos(psi), cos(theta)*sin(psi), -sin(theta),
		-cos(phi)*sin(psi)+sin(theta)*cos(psi)*sin(phi), cos(psi)*cos(phi)+sin(theta)*sin(psi)*sin(phi), sin(phi)*cos(theta),
		sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi), -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi), cos(theta)*cos(phi);

	mat<<1,0,-sin(theta),
		 0,cos(phi),sin(phi)*cos(theta),
		 0,-sin(phi),cos(theta)*cos(phi);

	mat_inv<<1,sin(phi)*tan(theta),cos(phi)*tan(theta),
			0,cos(phi),-sin(phi),
			0,sin(phi)/cos(theta),cos(phi)/cos(theta);

	I<<I_xx,0,0,
		0,I_yy,0,
		0,0,I_zz;

	I_inv<<1/I_xx,0,0,
		0,1/I_yy,0,
		0,0,1/I_zz;

	thrust=C_T*(w1*w1 + w2*w2 + w3*w3 + w4*w4);

	Eigen::Matrix<adouble,3,1> angle_dots,Omega_in,v_dots,gravity_term,thrust_term,pos_dots,Omega_b,L_b,Moments,angle_ddots,temp;
	Eigen::Matrix<adouble,3,3> skew1,skew2;
	pos_dots<<x_d,y_d,z_d;
	gravity_term<<0,0,g;
	thrust_term<<0,0,thrust/mass;
	angle_dots<<phi_d,theta_d,psi_d;
	Omega_in=Rsb*(mat*angle_dots);
//	temp=Omega_in.cross(pos_dots);
	skew1<<0,-Omega_in[2],Omega_in[1],
			Omega_in[2],0,-Omega_in[0],
			-Omega_in[1],Omega_in[0],0;
	v_dots=Rsb*thrust_term-gravity_term-skew1*pos_dots;//Omega_in.cross(pos_dots);

	X_dot[0]=x_d;
	X_dot[1]=y_d;
	X_dot[2]=z_d;
	X_dot[3]=phi_d;
	X_dot[4]=theta_d;
	X_dot[5]=psi_d;
	X_dot[6]=v_dots[0];
	X_dot[7]=v_dots[1];
	X_dot[8]=v_dots[2];

	Omega_b=mat*angle_dots;

	L_b=I*Omega_b;

	Moments<<d*C_T/sqrt(2)*(-w1*w1-w2*w2+w3*w3+w4*w4),
			d*C_T/sqrt(2)*(-w1*w1+w2*w2+w3*w3-w4*w4),
			C_D*(-w1*w1+w2*w2-w3*w3+w4*w4);

	skew2<<0,-Omega_b[2],Omega_b[1],
			Omega_b[2],0,-Omega_b[0],
			-Omega_b[1],Omega_b[0],0;

	angle_ddots=mat_inv*(I_inv*(Moments-skew2*L_b));//mat_inv*(I_inv*(Moments-Omega_b.cross(L_b))); //Euler's eqn.

	X_dot[9]=angle_ddots[0];
	X_dot[10]=angle_ddots[1];
	X_dot[11]=angle_ddots[2];

	return X_dot;

};

drone2_state_tensor dynamics_2(const drone2_state_tensor & X,const drone2_input_tensor & u)
{
	drone2_state_tensor X_dot;
	drone_state_tensor X_dot1,X_dot2;
	X_dot1=dynamics(X.segment(0, 12),u.segment(0,4));
	X_dot2=dynamics(X.segment(12, 12),u.segment(4,4));
	X_dot<<X_dot1,X_dot2;
	return X_dot;

//	drone2_state_tensor X_dot;
//	adouble x1,y1,z1,phi1,theta1,psi1,x_d1,y_d1,z_d1,phi_d1,theta_d1,psi_d1,w11,w21,w31,w41,thrust1;
//	adouble x2,y2,z2,phi2,theta2,psi2,x_d2,y_d2,z_d2,phi_d2,theta_d2,psi_d2,w12,w22,w32,w42,thrust2;
//	x1=X[0];
//	y1=X[1];
//	z1=X[2];
//	phi1=X[3];
//	theta1=X[4];
//	psi1=X[5];
//	x_d1=X[6];
//	y_d1=X[7];
//	z_d1=X[8];
//	phi_d1=X[9];
//	theta_d1=X[10];
//	psi_d1=X[11];
//
//	w11=u[0];
//	w21=u[1];
//	w31=u[2];
//	w41=u[3];
//
//	x2=X[12];
//	y2=X[13];
//	z2=X[14];
//	phi2=X[15];
//	theta2=X[16];
//	psi2=X[17];
//	x_d2=X[18];
//	y_d2=X[19];
//	z_d2=X[20];
//	phi_d2=X[21];
//	theta_d2=X[22];
//	psi_d2=X[23];
//
//	w12=u[4];
//	w22=u[5];
//	w32=u[6];
//	w42=u[7];
//
//	Eigen::Matrix<adouble,3,3> Rsb1,mat1,mat_inv1,I,I_inv,
//	Rsb2,mat2,mat_inv2;
//
//	Rsb1<<cos(theta1)*cos(psi1), cos(theta1)*sin(psi1), -sin(theta1),
//		-cos(phi1)*sin(psi1)+sin(theta1)*cos(psi1)*sin(phi1), cos(psi1)*cos(phi1)+sin(theta1)*sin(psi1)*sin(phi1), sin(phi1)*cos(theta1),
//		sin(phi1)*sin(psi1)+cos(phi1)*sin(theta1)*cos(psi1), -sin(phi1)*cos(psi1)+cos(phi1)*sin(theta1)*sin(psi1), cos(theta1)*cos(phi1);
//
//	mat1<<1,0,-sin(theta1),
//		 0,cos(phi1),sin(phi1)*cos(theta1),
//		 0,-sin(phi1),cos(theta1)*cos(phi1);
//
//	mat_inv1<<1,sin(phi1)*tan(theta1),cos(phi1)*tan(theta1),
//			0,cos(phi1),-sin(phi1),
//			0,sin(phi1)/cos(theta1),cos(phi1)/cos(theta1);
//
//
//
//	Rsb2<<cos(theta2)*cos(psi2), cos(theta2)*sin(psi2), -sin(theta2),
//		-cos(phi2)*sin(psi2)+sin(theta2)*cos(psi2)*sin(phi2), cos(psi2)*cos(phi2)+sin(theta2)*sin(psi2)*sin(phi2), sin(phi2)*cos(theta2),
//		sin(phi2)*sin(psi2)+cos(phi2)*sin(theta2)*cos(psi2), -sin(phi2)*cos(psi2)+cos(phi2)*sin(theta2)*sin(psi2), cos(theta2)*cos(phi2);
//
//	mat2<<1,0,-sin(theta2),
//		 0,cos(phi2),sin(phi2)*cos(theta2),
//		 0,-sin(phi2),cos(theta2)*cos(phi2);
//
//	mat_inv2<<1,sin(phi2)*tan(theta2),cos(phi2)*tan(theta2),
//			0,cos(phi2),-sin(phi2),
//			0,sin(phi2)/cos(theta2),cos(phi2)/cos(theta2);
//
//
//	I<<I_xx,0,0,
//		0,I_yy,0,
//		0,0,I_zz;
//
//	I_inv<<1/I_xx,0,0,
//		0,1/I_yy,0,
//		0,0,1/I_zz;
//
//
//
//
//
//	thrust1=C_T*(w11*w11 + w21*w21 + w31*w31 + w41*w41);
//	thrust2=C_T*(w12*w12 + w22*w22 + w32*w32 + w42*w42);
//
//	Eigen::Matrix<adouble,3,1> angle_dots1,Omega_in1,v_dots1,gravity_term1,thrust_term1,pos_dots1,Omega_b1,L_b1,Moments1,angle_ddots1,temp1;
//	Eigen::Matrix<adouble,3,1> angle_dots2,Omega_in2,v_dots2,gravity_term2,thrust_term2,pos_dots2,Omega_b2,L_b2,Moments2,angle_ddots2,temp2;
//	Eigen::Matrix<adouble,3,3> skew11,skew21,skew12,skew22;
//	pos_dots1<<x_d1,y_d1,z_d1;
//	gravity_term1<<0,0,g;
//	thrust_term1<<0,0,thrust1/mass;
//	angle_dots1<<phi_d1,theta_d1,psi_d1;
//	Omega_in1=Rsb1*(mat1*angle_dots1);
////	temp=Omega_in.cross(pos_dots);
//	skew11<<0,-Omega_in1[2],Omega_in1[1],
//			Omega_in1[2],0,-Omega_in1[0],
//			-Omega_in1[1],Omega_in1[0],0;
//	v_dots1=Rsb1*thrust_term1-gravity_term1-skew11*pos_dots1;//Omega_in.cross(pos_dots);
//
//	pos_dots2<<x_d2,y_d2,z_d2;
//	gravity_term2<<0,0,g;
//	thrust_term2<<0,0,thrust2/mass;
//	angle_dots2<<phi_d2,theta_d2,psi_d2;
//	Omega_in2=Rsb2*(mat2*angle_dots2);
////	temp=Omega_in.cross(pos_dots);
//	skew12<<0,-Omega_in2[2],Omega_in2[1],
//			Omega_in2[2],0,-Omega_in2[0],
//			-Omega_in2[1],Omega_in2[0],0;
//	v_dots2=Rsb2*thrust_term2-gravity_term2-skew12*pos_dots2;//Omega_in.cross(pos_dots);
//
//
//
//
//
//	X_dot[0]=x_d1;
//	X_dot[1]=y_d1;
//	X_dot[2]=z_d1;
//	X_dot[3]=phi_d1;
//	X_dot[4]=theta_d1;
//	X_dot[5]=psi_d1;
//	X_dot[6]=v_dots1[0];
//	X_dot[7]=v_dots1[1];
//	X_dot[8]=v_dots1[2];
//
//	X_dot[12]=x_d2;
//	X_dot[13]=y_d2;
//	X_dot[14]=z_d2;
//	X_dot[15]=phi_d2;
//	X_dot[16]=theta_d2;
//	X_dot[17]=psi_d2;
//	X_dot[18]=v_dots2[0];
//	X_dot[19]=v_dots2[1];
//	X_dot[20]=v_dots2[2];
//
//	Omega_b1=mat1*angle_dots1;
//	Omega_b2=mat2*angle_dots2;
//
//	L_b1=I*Omega_b1;
//	L_b2=I*Omega_b2;
//
//	Moments1<<d*C_T/sqrt(2)*(-w11*w11-w21*w21+w31*w31+w41*w41),
//			d*C_T/sqrt(2)*(-w11*w11+w21*w21+w31*w31-w41*w41),
//			C_D*(-w11*w11+w21*w21-w31*w31+w41*w41);
//
//	Moments2<<d*C_T/sqrt(2)*(-w12*w12-w22*w22+w32*w32+w42*w42),
//			d*C_T/sqrt(2)*(-w12*w12+w22*w22+w32*w32-w42*w42),
//			C_D*(-w12*w12+w22*w22-w32*w32+w42*w42);
//
//	skew21<<0,-Omega_b1[2],Omega_b1[1],
//			Omega_b1[2],0,-Omega_b1[0],
//			-Omega_b1[1],Omega_b1[0],0;
//
//	skew22<<0,-Omega_b2[2],Omega_b2[1],
//			Omega_b2[2],0,-Omega_b2[0],
//			-Omega_b2[1],Omega_b2[0],0;
//
//
//	angle_ddots1=mat_inv1*(I_inv*(Moments1-skew21*L_b1));//mat_inv*(I_inv*(Moments-Omega_b.cross(L_b))); //Euler's eqn.
//
//
//	angle_ddots2=mat_inv2*(I_inv*(Moments2-skew22*L_b2));
//
//
//	X_dot[9]=angle_ddots1[0];
//	X_dot[10]=angle_ddots1[1];
//	X_dot[11]=angle_ddots1[2];
//
//	X_dot[21]=angle_ddots2[0];
//	X_dot[22]=angle_ddots2[1];
//	X_dot[23]=angle_ddots2[2];
//
//	return X_dot;

}

}

namespace Single_Integrator_3D
{
	state_tensor dynamics(const state_tensor & x, const input_tensor & u)
	{
		return u;
	};

	state_tensor2 dynamics2(const state_tensor2 & x, const input_tensor2 & u)
	{
		return u;
	}
}
namespace Double_Integrator_3D
{
	state_tensor dynamics(const state_tensor & x, const input_tensor & u)
	{
		state_tensor x_dot;
		x_dot[0]=x[3];
		x_dot[1]=x[4];
		x_dot[2]=x[5];
		x_dot[3]=u[0]/mb;
		x_dot[4]=u[1]/mb;
		x_dot[5]=u[2]/mb;

		return x_dot;
	};
//
//	state_tensor2 dynamics2(const state_tensor2 & x, const input_tensor2 & u)
//	{
//		return u;
//	}
}
namespace Drone_First_Order_Dynamics
{
	typedef Eigen::Matrix<adouble,6,1> drone_state_tensor;
	typedef Eigen::Matrix<adouble,6,1> drone_input_tensor;

	typedef Eigen::Matrix<adouble,6*2,1> drone2_state_tensor;
	typedef Eigen::Matrix<adouble,6*2,1> drone2_input_tensor;

//	const double C_T=3.1582*1e-10;
//	const double C_D=7.9379*1e-12;
//	const double g=9.80665;
//	const double d=39.73*1e-3;
//	const double I_xx=1.395*1e-5;
//	const double I_yy=1.436*1e-5;
//	const double I_zz=2.173*1e-5;
//	const double mass=0.033;
	drone_state_tensor dynamics(const drone_state_tensor & X,const drone_input_tensor & input)
	{
		adouble x,y,z,phi,theta,psi,u,v,w,p,q,r,x_d,y_d,z_d,phi_d,theta_d,psi_d;
		drone_state_tensor x_dot;
		u=input[0];
		v=input[1];
		w=input[2];
		p=input[3];
		q=input[4];
		r=input[5];
		Eigen::Matrix<adouble,3,3> Rsb,mat,mat_inv;
		Eigen::Matrix<adouble,3,1> v_b,w_b,v_i,euler_d;

		v_b<<u,v,w;
		w_b<<p,q,r;

		Rsb<<cos(theta)*cos(psi), cos(theta)*sin(psi), -sin(theta),
				-cos(phi)*sin(psi)+sin(theta)*cos(psi)*sin(phi), cos(psi)*cos(phi)+sin(theta)*sin(psi)*sin(phi), sin(phi)*cos(theta),
				sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi), -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi), cos(theta)*cos(phi);

//		mat<<1,0,-sin(theta),
//			 0,cos(phi),sin(phi)*cos(theta),
//			 0,-sin(phi),cos(theta)*cos(phi);

		mat_inv<<1,sin(phi)*tan(theta),cos(phi)*tan(theta),
				0,cos(phi),-sin(phi),
				0,sin(phi)/cos(theta),cos(phi)/cos(theta);

		v_i=Rsb*v_b;
		euler_d=mat_inv*w_b;

		x_d=v_i[0];
		y_d=v_i[1];
		z_d=v_i[2];

		phi_d=euler_d[0];
		theta_d=euler_d[1];
		psi_d=euler_d[2];

//		x_d=w*(sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta))-
//				v*(cos(phi)*sin(psi)-cos(psi)*sin(phi)*sin(theta))+
//				u*(cos(psi)*cos(theta));
//
//		y_d=v*(cos(phi)*cos(psi)+sin(phi)*sin(psi)*sin(theta))-
//				w*(cos(psi)*sin(phi)-cos(phi)*sin(psi)*sin(theta))+
//				u*(cos(theta)*sin(psi));
//		z_d=w*(cos(phi)*cos(theta))-u*(sin(theta))+v*(cos(theta)*sin(phi));
//
//		phi_d=p+r*(cos(phi)*tan(theta))+q*(sin(phi)*tan(theta));
//		theta_d=q*cos(phi)-r*sin(phi);
//		psi_d=r*cos(phi)/cos(theta)+q*sin(phi)/cos(theta);
//
		x_dot<<x_d,y_d,z_d,phi_d,theta_d,psi_d;

		return x_dot;
	}

	drone2_state_tensor dynamics_2(const drone2_state_tensor & x,const drone2_input_tensor & u)
	{
		drone2_state_tensor X_dot;
		drone_state_tensor X_dot1,X_dot2;
		X_dot1=dynamics(x.segment(0, 6),u.segment(0,6));
		X_dot2=dynamics(x.segment(6, 6),u.segment(6,6));
		X_dot<<X_dot1,X_dot2;
		return X_dot;
	}

}


/**
 *           c++11-only implementation of the L-BFGS-B algorithm
 *
 * Copyright (c) 2014 Patrick Wieschollek
 *               https://github.com/PatWie/LBFGSB
 * All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:

 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef LBFGSB_H_
#define LBFGSB_H_

#include "meta.h"

#include <list>
#include <stdio.h>
#include <iostream>
#include <functional>
#include <vector>
#include <armadillo>

/* coded from scratch !!!
 * based on the paper
 * A LIMITED MEMORY ALGORITHM FOR BOUND CONSTRAINED OPTIMIZATION
 * (Byrd, Lu, Nocedal, Zhu)
 */

class LBFGSB {

  // contains options for optimization process
  Options Options_;

  // oracles for function value and gradient
  FunctionOracleType FunctionObjectiveOracle_;
  GradientOracleType FunctionGradientOracle_;

  arma::mat W, M;
  arma::vec lb, ub;
  double theta;
  int DIM;

  std::list<arma::vec> xHistory;

public:

  arma::vec XOpt;

  LBFGSB(const arma::vec & l, const arma::vec & u) :
      lb(l), ub(u), theta(1.0), DIM(l.n_rows) {
    lb = l;
    ub = u;
    theta = 1.0;
    DIM = l.n_rows;
    W = arma::zeros(DIM, 0);
    M = arma::zeros(0, 0);
  }

  LBFGSB(Options & Options, const arma::vec & l, const arma::vec & u) {
    Options_ = Options;
    lb = l;
    ub = u;
    theta = 1.0;
    DIM = l.n_rows;
    W = arma::zeros(DIM, 0);
    M = arma::zeros(0, 0);

  }

  /// <summary>
  /// find cauchy point in x
  /// </summary>
  /// <parameter name="x">start in x</parameter>
  void
  GetGeneralizedCauchyPoint(arma::vec & x, arma::vec & g, arma::vec & x_cauchy,
                            arma::vec & c) {
    const int DIM = x.n_rows;
    // PAGE 8
    // Algorithm CP: Computation of the generalized Cauchy point
    // Given x,l,u,g, and B = \theta I-WMW

    // {all t_i} = { (idx,value), ... }
    // TODO: use "std::set" ?
    std::vector<std::pair<int, double> > SetOfT;
    // the feasible set is implicitly given by "SetOfT - {t_i==0}"
    arma::vec d = arma::zeros(DIM);

    // n operations
    for (int j = 0; j < DIM; j++) {
      if (g(j) == 0) {
        SetOfT.push_back(std::make_pair(j, INF));
      } else {
        double tmp = 0;
        if (g(j) < 0) {
          tmp = (x(j) - ub(j)) / g(j);
        } else {
          tmp = (x(j) - lb(j)) / g(j);
        }
        d(j) = -g(j);
        SetOfT.push_back(std::make_pair(j, tmp));
      }

    }Debug(d.transpose());

    // paper: using heapsort
    // sortedindices [1,0,2] means the minimal element is on the 1th entry
    std::vector<int> SortedIndices = sort_indexes(SetOfT);

    x_cauchy = x;
    // Initialize
    // p := 	W^T*p
    arma::vec p = (W.t() * d);            // (2mn operations)
    // c := 	0
    c = arma::zeros(M.n_rows);
    // f' := 	g^T*d = -d^Td
    double f_prime = arma::dot(-d, d);              // (n operations)
    // f'' :=	\theta*d^T*d-d^T*W*M*W^T*d = -\theta*f' - p^T*M*p
    double f_doubleprime = (double) (-1.0 * theta) * f_prime -
                           arma::dot(p, M * p);// (O(m^2) operations)
    // \delta t_min :=	-f'/f''
    double dt_min = -f_prime / f_doubleprime;
    // t_old := 	0
    double t_old = 0;
    // b := 	argmin {t_i , t_i >0}
    int i = 0;
    for (int j = 0; j < DIM; j++) {
      i = j;
      if (SetOfT[SortedIndices[j]].second != 0)
        break;
    }
    int b = SortedIndices[i];
    // see below
    // t        			:= 	min{t_i : i in F}
    double t = SetOfT[b].second;
    // \delta t 			:= 	t - 0
    double dt = t - t_old;

    // examination of subsequent segments
    while ((dt_min >= dt) && (i < DIM)) {
      if (d(b) > 0)
        x_cauchy(b) = ub(b);
      else if (d(b) < 0)
        x_cauchy(b) = lb(b);

      // z_b = x_p^{cp} - x_b
      double zb = x_cauchy(b) - x(b);
      // c   :=  c +\delta t*p
      c += dt * p;
      // cache
      arma::rowvec wbt = W.row(b);

      f_prime += dt * f_doubleprime + g(b) * g(b)
                 + theta * g(b) * zb
                 - g(b) * arma::dot(wbt.t(), (M * c));
      f_doubleprime += -1.0 * theta * g(b) * g(b);
      f_doubleprime += 2.0 * g(b) * arma::dot(wbt.t(), M * p);
      // TODO Check M * wbt.t() is correct
      f_doubleprime += -g(b) * g(b) * arma::dot(wbt.t(), M * wbt.t());
      p += g(b) * wbt.t();
      d(b) = 0;
      dt_min = -f_prime / f_doubleprime;
      t_old = t;
      ++i;
      if (i < DIM) {
        b = SortedIndices[i];
        t = SetOfT[b].second;
        dt = t - t_old;
      }

    }

    dt_min = max(dt_min, 0);
    t_old += dt_min;

    Debug(SortedIndices[0] << " " << SortedIndices[1]);

    for (int ii = i; ii < x_cauchy.n_rows; ii++) {
      x_cauchy(SortedIndices[ii]) = x(SortedIndices[ii])
                                    + t_old * d(SortedIndices[ii]);
    }Debug(x_cauchy.transpose());

    c += dt_min * p;
    Debug(c.transpose());

  }

  /// <summary>
  /// find valid alpha for (8.5)
  /// </summary>
  /// <parameter name="x_cp">cauchy point</parameter>
  /// <parameter name="du">unconstrained solution of subspace minimization</parameter>
  /// <parameter name="FreeVariables">flag (1 if is free variable and 0 if is not free variable)</parameter>
  double FindAlpha(arma::vec & x_cp, arma::vec & du,
                   std::vector<int> & FreeVariables) {
    /* this returns
     * a* = max {a : a <= 1 and  l_i-xc_i <= a*d_i <= u_i-xc_i}
     */
    double alphastar = 1;
    const unsigned int n = FreeVariables.size();
    for (unsigned int i = 0; i < n; i++) {
      if (du(i) > 0) {
        alphastar = min(alphastar,
                        (ub(FreeVariables[i]) - x_cp(FreeVariables[i]))
                        / du(i));
      } else {
        alphastar = min(alphastar,
                        (lb(FreeVariables[i]) - x_cp(FreeVariables[i]))
                        / du(i));
      }
    }
    return alphastar;
  }

  /// <summary>
  /// using linesearch to determine step width
  /// </summary>
  /// <parameter name="x">start in x</parameter>
  /// <parameter name="dx">direction</parameter>
  /// <parameter name="f">current value of objective (will be changed)</parameter>
  /// <parameter name="g">current gradient of objective (will be changed)</parameter>
  /// <parameter name="t">step width (will be changed)</parameter>
  void LineSearch(arma::vec & x, arma::vec dx, double & f, arma::vec & g,
                  double & t) {

    const double alpha = 0.2;
    const double beta = 0.8;

    const double f_in = f;
    const arma::vec g_in = g;
    const double Cache = alpha * arma::dot(g_in, dx);

    t = 1.0;
    f = FunctionObjectiveOracle_(x + t * dx);
    while (f > f_in + t * Cache) {
      t *= beta;
      f = FunctionObjectiveOracle_(x + t * dx);
    }
    FunctionGradientOracle_(x + t * dx, g);
    x += t * dx;

  }

  /// <summary>
  /// direct primal approach
  /// </summary>
  /// <parameter name="x">start in x</parameter>
  void SubspaceMinimization(arma::vec & x_cauchy, arma::vec & x, arma::vec & c,
                            arma::vec & g,
                            arma::vec & SubspaceMin) {

    // cached value: ThetaInverse=1/theta;
    double theta_inverse = 1 / theta;

    // size of "t"
    std::vector<int> FreeVariablesIndex;
    Debug(x_cauchy.transpose());

    //std::cout << "free vars " << FreeVariables.rows() << std::endl;
    for (int i = 0; i < x_cauchy.n_rows; i++) {
      Debug(x_cauchy(i) << " " << ub(i) << " " << lb(i));
      if ((x_cauchy(i) != ub(i)) && (x_cauchy(i) != lb(i))) {
        FreeVariablesIndex.push_back(i);
      }
    }
    const int FreeVarCount = FreeVariablesIndex.size();

    arma::mat WZ = arma::zeros(W.n_cols, FreeVarCount);

    for (int i = 0; i < FreeVarCount; i++)
      WZ.col(i) = W.row(FreeVariablesIndex[i]).t();

    Debug(WZ);

    // r=(g+theta*(x_cauchy-x)-W*(M*c));
    Debug(g);Debug(x_cauchy);Debug(x);
    arma::vec rr = (g + theta * (x_cauchy - x) - W * (M * c));
    // r=r(FreeVariables);
    arma::vec r = arma::zeros(FreeVarCount);
    for (int i = 0; i < FreeVarCount; i++)
      r.row(i) = rr.row(FreeVariablesIndex[i]);

    Debug(r.transpose());

    // STEP 2: "v = w^T*Z*r" and STEP 3: "v = M*v"
    arma::vec v = M * (WZ * r);
    // STEP 4: N = 1/theta*W^T*Z*(W^T*Z)^T
    arma::mat N = theta_inverse * WZ * WZ.t();
    // N = I - MN
    N = arma::eye(N.n_rows, N.n_rows) - M * N;
    // STEP: 5
    // v = N^{-1}*v
    v = arma::inv(N) * v;
    // STEP: 6
    // HERE IS A MISTAKE IN THE ORIGINAL PAPER!
    arma::vec du = -theta_inverse * r
                   - theta_inverse * theta_inverse * WZ.t() * v;
    Debug(du.transpose());
    // STEP: 7
    double alpha_star = FindAlpha(x_cauchy, du, FreeVariablesIndex);

    // STEP: 8
    arma::vec dStar = alpha_star * du;

    SubspaceMin = x_cauchy;
    for (int i = 0; i < FreeVarCount; i++) {
      SubspaceMin(FreeVariablesIndex[i]) = SubspaceMin(
          FreeVariablesIndex[i]) + dStar(i);
    }
  }

  void Solve(arma::vec & x0, const FunctionOracleType & FunctionValue,
             const GradientOracleType & FunctionGradient) {
    FunctionObjectiveOracle_ = FunctionValue;
    FunctionGradientOracle_ = FunctionGradient;

    Assert(x0.n_rows == lb.n_rows, "lower bound size incorrect");
    Assert(x0.n_rows == ub.n_rows, "upper bound size incorrect");

    Assert(arma::all(x0 >= lb),
           "seed is not feasible (violates lower bound)");
    Assert(arma::all(x0 <= ub),
           "seed is not feasible (violates upper bound)");

    const int DIM = x0.n_rows;

    xHistory.push_back(x0);

    arma::mat yHistory = arma::zeros(DIM, 0);
    arma::mat sHistory = arma::zeros(DIM, 0);

    arma::vec x = x0, g;
    int k = 0;

    double f = FunctionObjectiveOracle_(x);
    FunctionGradientOracle_(x, g);

    theta = 1.0;

    W = arma::zeros(DIM, 0);
    M = arma::zeros(0, 0);

    auto noConvergence =
        [&](arma::vec & x, arma::vec & g) -> bool {
            arma::vec clamped = x - g;
            const auto too_big = arma::find(clamped > lb);
            clamped(too_big) = lb(too_big);
            const auto too_small = arma::find(clamped < ub);
            clamped(too_small) = ub(too_small);
            return (arma::norm(clamped - x, "inf") >= Options_.tol);
        };

    while (noConvergence(x, g) && (k < Options_.maxIter)) {
      Debug("iteration " << k)
      double f_old = f;
      arma::vec x_old = x;
      arma::vec g_old = g;

      // STEP 2: compute the cauchy point by algorithm CP
      arma::vec CauchyPoint = arma::zeros(DIM);
      arma::vec c = arma::zeros(DIM);
      GetGeneralizedCauchyPoint(x, g, CauchyPoint, c);
      // STEP 3: compute a search direction d_k by the primal method
      arma::vec SubspaceMin;
      SubspaceMinimization(CauchyPoint, x, c, g, SubspaceMin);

      arma::mat H;
      double Length = 0;

      // STEP 4: perform linesearch and STEP 5: compute gradient
      LineSearch(x, SubspaceMin - x, f, g, Length);

      xHistory.push_back(x);

      // prepare for next iteration
      arma::vec newY = g - g_old;
      arma::vec newS = x - x_old;

      // STEP 6:
      double test = arma::dot(newS, newY);
      test = (test < 0) ? -1.0 * test : test;

      if (test > EPS * arma::norm(newY, 2)) {
        if (k < Options_.m) {
          yHistory.resize(DIM, k + 1);
          sHistory.resize(DIM, k + 1);
        } else {

          yHistory.head_cols(Options_.m - 1) = yHistory.tail_cols(
              Options_.m - 1);
          sHistory.head_cols(Options_.m - 1) = sHistory.tail_cols(
              Options_.m - 1);
        }
        yHistory.tail_cols(1) = newY;
        sHistory.tail_cols(1) = newS;

        // STEP 7:
        theta = arma::dot(newY, newY)
                / arma::dot(newY, newS);

        W = arma::zeros(yHistory.n_rows,
                        yHistory.n_cols + sHistory.n_cols);

        W(0, 0, arma::size(yHistory)) = yHistory;

        W(0, yHistory.n_cols, arma::size(sHistory)) = theta * sHistory;

        arma::mat A = sHistory.t() * yHistory;
        arma::mat L = arma::trimatl(A);
        arma::mat MM(A.n_rows + L.n_rows, A.n_rows + L.n_cols);
        arma::mat D = -arma::diagmat(A);
        MM(0, 0, arma::size(D)) = D;
        MM(0, D.n_cols, arma::size(L.t())) = L.t();
        MM(D.n_rows, 0, arma::size(L)) = L;
        arma::mat bottom_right = (sHistory.t() * sHistory)
                                 * theta;
        MM(D.n_rows, D.n_cols, arma::size(bottom_right)) = bottom_right;

        M = arma::inv(MM);
      }

      arma::vec ttt = arma::zeros(1);
      ttt(0) = f_old - f;
      Debug("--> " << arma::norm(ttt));
      if (arma::norm(ttt) < Options_.tol) {
        // successive function values too similar
        break;
      }
      k++;

    }

    XOpt = x;
    x0 = x;

  }
};

#endif /* LBFGSB_H_ */


#include <transwarp.h>
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <parallel/numeric>

namespace daq
{
    constexpr double f = 15.0;       // Hz
    constexpr double rate = 95000.0; // Hz

    int ricker_filter_size(double f_, double rate_, double width = 5.0) noexcept
    {
        constexpr double ricker_coefficient = 2.2508;
        return static_cast<int>(lround (width / ricker_coefficient / f_ * rate_ + 0.5));
    }

    // TODO: Improve make_odd(), align_by_ten(), ricker_filter_size() functions to do a simmetrical filter without sly tricks
    int align_by_ten(int sz) noexcept
    {
        return sz / 10 * 10 + 10;
    }

    int make_odd(int sz) noexcept
    {
        return (sz % 2) ? sz : (sz + 1);
    }

    constexpr size_t ricker_filter_max_size = 20000;
    using ricker_filter_data_type = std::array<double, ricker_filter_max_size + 1>;

    struct ricker_filter {
        ricker_filter_data_type& data;
        size_t& sz;
    };

    void create_filter(int dots, ricker_filter & rfd) noexcept
    {
        assert (rfd.data.size() > dots);

        auto const tricky_dots = (dots - 1) / 10;
        double const c = 2 / ((sqrt (3)) * pow (boost::math::constants::pi<double>(), 0.25));
        double const one_per_mh_dots = 1.0 / tricky_dots;
        double t = -5.0;

        for (int i = 0; i < dots; ++i)
        {
            const double pow_t_2 = pow (t, 2);
            rfd.data[i] = c * exp(-pow_t_2 / 2) * (1 - pow_t_2);
            t += one_per_mh_dots;
        }

        rfd.sz = dots;
    }

    using sample_type = int32_t;
    using signal_sequence_type = std::vector<sample_type>;
    using transform_result_type = double;

    using sample_quantity_type = int;
    using prepare_type = std::tuple<sample_quantity_type, const signal_sequence_type &>;
    using output_type = std::shared_ptr<std::vector<transform_result_type>>;

    output_type partial_task(prepare_type input, uint8_t part_index, uint8_t part_number, ricker_filter const & rfd)
    {
        auto const sample_quantity = std::get<0>(input);
        auto const & signal_sequence = std::get<1>(input);

        auto const default_iteration_number = sample_quantity / part_number;
        auto const begin_iter = std::begin(signal_sequence) + ((part_index - 1) * default_iteration_number);
        auto const end_iter = (part_index != part_number) ? (std::begin(signal_sequence) + ((part_index) * default_iteration_number + rfd.sz)) : std::end(signal_sequence);

        sample_type signal_subsequence [std::distance(begin_iter, end_iter)];
        ricker_filter_data_type::value_type filter [rfd.sz];

        std::copy (begin_iter, end_iter, signal_subsequence);
        std::copy (std::begin(rfd.data), std::begin(rfd.data) + rfd.sz, filter);

        auto const total_iterations = (part_index != part_number) ? (sample_quantity / part_number) : (sample_quantity / part_number + (sample_quantity % part_number));
        auto const data = std::make_shared<std::vector<transform_result_type>>(total_iterations);

        auto curr = std::begin(*data);
        auto const end = curr + total_iterations;

        auto pure_s_iter = signal_subsequence;

        while (curr != end)
        {
            *curr++ = static_cast<transform_result_type>(std::inner_product(filter, filter + rfd.sz, pure_s_iter++, 0.0) / rfd.sz);
        }

        return data;
    }

    namespace tw = transwarp;
    constexpr int hardware_threads = 8; /*std::thread::hardware_concurrency();*/
    tw::parallel & get_executor()
    {
        static tw::parallel executor(hardware_threads);
        return executor;
    }

    static std::shared_ptr<tw::task<output_type>> make_task_by_index (uint8_t index, signal_sequence_type const & ss, size_t from, size_t to, ricker_filter const & fd)
    {
        return tw::make_task(tw::consume, [&] (prepare_type input, uint8_t index)
                             {
                                 return partial_task(input, index, hardware_threads, fd);
                             },
                             tw::make_value_task (std::make_tuple(to - from, ss)), tw::make_value_task(index)
        );
    }

    static std::shared_ptr<tw::task<void>> gather_task(signal_sequence_type const & ss, size_t from, size_t to,
                                                       transform_result_type * const result, ricker_filter const & filter_data) {

        std::vector<std::shared_ptr<tw::task<output_type>>> vec (hardware_threads);
        uint8_t task_idx = 1;
        for (auto & elem : vec)
        {
            elem = make_task_by_index(static_cast<uint8_t>(task_idx++), ss, from, to, filter_data);
        }

        // capture by value
        return tw::make_task(tw::consume, [=](std::vector<output_type> const & parents) {
            size_t copy_pos = 0;
            for (auto & elem : parents)
            {
                std::copy (elem->begin(), elem->begin()+elem->size(), &result[copy_pos]);
                copy_pos += elem->size();
            }
        }, vec);

    }

    auto inner_product_lambda = [](auto&&... args){return std::inner_product(decltype(args)(args)...);};
    auto parallel_inner_product_lambda = [](auto&&... args){return __gnu_parallel::inner_product(decltype(args)(args)...);};

    template <typename L>
    inline void seq_transform(signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data, L const & lambda)
    {
        auto const iterations = static_cast<size_t>(rate);
        int curr_iteration = 0;
        while (curr_iteration < iterations)
        {
            result[curr_iteration] = static_cast<transform_result_type> (
                    lambda(std::begin(filter_data.data), std::begin(filter_data.data) + filter_data.sz, std::begin(ss) + curr_iteration, 0.0) / filter_data.sz
            );
            ++curr_iteration;
        }
    }

    void transform_uni(signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data)
    {
        seq_transform (ss, result, filter_data, inner_product_lambda);
    }

    void transform_omp (signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data)
    {
        seq_transform (ss, result, filter_data, parallel_inner_product_lambda);
    }

    void transform_tw(signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data)
    {
        auto final_task = gather_task(ss, 0, static_cast<size_t>(rate), result, filter_data);
        final_task->schedule_all(get_executor());
        final_task->get();
    }
}

template <typename F, typename ...T>
void measure_it(char const* f_name, F const & f, T &&... arguments)
{
    auto const time_point1 = std::chrono::high_resolution_clock::now();
    f (std::forward<T>(arguments)...);
    auto const time_point2 = std::chrono::high_resolution_clock::now();
    std::cout << f_name << std::chrono::duration_cast<std::chrono::milliseconds>(time_point2-time_point1).count() << std::endl;
}

std::array<daq::transform_result_type, static_cast<size_t>(daq::rate)> transform_uni_result{};
std::array<daq::transform_result_type, static_cast<size_t>(daq::rate)> transform_omp_result{};
std::array<daq::transform_result_type, static_cast<size_t>(daq::rate)> transform_tw_result{};

#ifndef MAIN_IS_ABSENT
int main()
{
    using namespace daq;

    ricker_filter_data_type filter_data {};
    size_t filter_size {};
    ricker_filter filter {filter_data, filter_size};

    create_filter (make_odd(align_by_ten(ricker_filter_size(f, rate))), filter);

    signal_sequence_type ss (static_cast<size_t>(rate) + filter.sz);
    std::iota(std::begin(ss), std::end(ss), -rate/2);

    measure_it("transform_uni: ", transform_uni, ss, std::addressof(transform_uni_result[0]), filter);
    measure_it("transform_omp: ", transform_omp, ss, std::addressof(transform_omp_result[0]), filter);
    measure_it("transform_tw:  ", transform_tw, ss, std::addressof(transform_tw_result[0]), filter);
    return 0;
}
#endif


#include "house.hpp"
#include "ukf.hpp"
#include "dyn.hpp"
#include "filter_aux.hpp"
#include "pearsonator.hpp"
#include "timer.hpp"
#include "eigen_csv.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <ctime>
#include <functional>
#include <iostream>
#include <random>
#include <vector>

using namespace Eigen;
using namespace std;

void run(bool gauss) {

    Matrix3d I;

    I << 500+400, 0, 0,
         0, 500+300, 0,
         0, 0, 400+300;

    DynamicModel::stf g = [&I] (double t, const Vector3d& w, const Vector3d& Td)
        -> VectorXd {
            return I.inverse() * (Td - w.cross(I * w));
        };

    DynamicModel f(g, 3, 1E-9, 1E-9);

    UKF::meas_model h = [] (double t, const Vector3d& w)
        -> VectorXd {
            return w.head(1);
        };

    HOUSE::meas_model hh = [] (double t, const VectorXd& w, const VectorXd& v)
        -> VectorXd {
            return w.head(1) + v;
        };

    double stdx0, stdw, stdn, dt;

    stdx0 = 0.01;
    stdw  = 0.001;
    stdn  = 0.001;

    dt = 0.1;

    Matrix3d Pxx0, Pww;
    MatrixXd Pnn(1,1);

    Pxx0 = Matrix3d::Identity() * stdx0 * stdx0;
    Pww  = Matrix3d::Identity() * stdw  * stdw;
    Pnn(0,0) = stdn * stdn;

    Vector3d wm0;
    wm0 << 0, 0, 0;

    Vector3d w0;

    int trials = 100;
    int steps = 6001;

    VectorXd t;
    t.setLinSpaced(steps, 0, (steps-1)*dt);

    vector<vector<VectorXd>> xtru, xest_ukf, xest_cut4, xest_cut6, xest_cut8,
        xest_house;

    double skew0, skeww, skewn, kurt0, kurtw, kurtn;

    if (gauss) {

        skew0 = 0;
        kurt0 = 3;

        skeww = 0;
        kurtw = 3;

        skewn = 0;
        kurtn = 3;

    } else {

        skew0 = -1;
        kurt0 = 30;

        skeww = -1;
        kurtw = 30;

        skewn = -1;
        kurtn = 30;

    }

    Pearsonator::TypeIV gen0_p(0, stdx0, skew0, kurt0), genw_p(0, stdw, skeww, kurtw),
        genn_p(0, stdn, skewn, kurtn);

    normal_distribution<double> gen0_g(0, stdx0), genw_g(0, stdw), genn_g(0, stdn);

    mt19937_64 mt(0);

    typedef function<double(void)> noisemaker;

    noisemaker gen0 = [&] () -> double {return gauss ? gen0_g(mt) : gen0_p(mt);};
    noisemaker genw = [&] () -> double {return gauss ? genw_g(mt) : genw_p(mt);};
    noisemaker genn = [&] () -> double {return gauss ? genn_g(mt) : genn_p(mt);};

    UKF::cut_dir = "../CUT/";

    UKF ukf (f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::JU,   1);
    UKF cut4(f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::CUT4, 1);
    UKF cut6(f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::CUT6, 1);
    UKF cut8(f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::CUT8, 1);

    HOUSE::Dist distx0(Pxx0), distw(Pww), distn(Pnn);

    distx0.mean = wm0;

    distx0.skew.setConstant(skew0);
    distx0.kurt.setConstant(kurt0);

    distw.skew.setConstant(skeww);
    distw.kurt.setConstant(kurtw);

    distn.skew.setConstant(skewn);
    distn.kurt.setConstant(kurtn);

    HOUSE house(f, hh, 1, 0, distx0, distw, distn, 0);

    MatrixXd run_times(trials,5);
    Timer timer;

    for (int j = 0; j < trials; j++) {

        ukf. reset(0, wm0, Pxx0);
        cut4.reset(0, wm0, Pxx0);
        cut6.reset(0, wm0, Pxx0);
        cut8.reset(0, wm0, Pxx0);
        house.reset(0, distx0);

        cout << "Running Trial " << j+1 << endl;

        MatrixXd T(3, steps), v(1, steps);
        for (int k = 0; k < steps; k++) {
            v(k) = genn();
            T(0,k) = genw();
            T(1,k) = genw();
            T(2,k) = genw();
        }

        w0 = wm0;
        w0(0) += gen0();
        w0(1) += gen0();
        w0(2) += gen0();

        vector<VectorXd> xtruk;
        VectorXd w = w0;
        for (int k = 0; k < steps; k++) {
            xtruk.push_back(w);
            if (k < steps-1)
                w = f(t(k), t(k+1), xtruk.back(), T.col(k));
        }
        xtru.push_back(xtruk);

        MatrixXd Z(1, steps);
        for (int k = 0; k < steps; k++)
            Z.col(k) = h(t(k), xtruk[k]) + v.col(k);

        cout << "   HOUSE" << endl;
        timer.tick();
        house.run(t, Z);
        run_times(j, 0) = timer.tock();
        vector<VectorXd> xest_house_trial;
        for (int k = 0; k < steps; k++)
            xest_house_trial.push_back(house.distx[k].mean);
        xest_house.push_back(xest_house_trial);

        cout << "   UKF" << endl;
        timer.tick();
        ukf.run(t, Z);
        run_times(j, 1) = timer.tock();
        xest_ukf.push_back(ukf.xest);

        cout << "   CUT4" << endl;
        timer.tick();
        cut4.run(t, Z);
        run_times(j, 2) = timer.tock();
        xest_cut4.push_back(cut4.xest);

        cout << "   CUT6" << endl;
        timer.tick();
        cut6.run(t, Z);
        run_times(j, 3) = timer.tock();
        xest_cut6.push_back(cut6.xest);

        cout << "   CUT8" << endl;
        timer.tick();
        cut8.run(t, Z);
        run_times(j, 4) = timer.tock();
        xest_cut8.push_back(cut8.xest);

    }

    string dist = gauss ? "gauss" : "pearson";

    save_rmse(t, xtru, xest_house, "out/house_rmse_" + dist + ".csv");
    save_rmse(t, xtru, xest_ukf,   "out/ukf_rmse_"   + dist + ".csv");
    save_rmse(t, xtru, xest_cut4,  "out/cut4_rmse_"  + dist + ".csv");
    save_rmse(t, xtru, xest_cut6,  "out/cut6_rmse_"  + dist + ".csv");
    save_rmse(t, xtru, xest_cut8,  "out/cut8_rmse_"  + dist + ".csv");

    save_abs_err_lump(t, xtru, xest_house, "out/house_err_" + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_ukf,   "out/ukf_err_"   + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_cut4,  "out/cut4_err_"  + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_cut6,  "out/cut6_err_"  + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_cut8,  "out/cut8_err_"  + dist + ".csv");

    vector<string> filters;
    filters.push_back("house");
    filters.push_back("ukf");
    filters.push_back("cut4");
    filters.push_back("cut6");
    filters.push_back("cut8");

    string time_file = "out/run_time_";
    time_file += dist;
    time_file += ".csv";

    EigenCSV::write(run_times, filters, time_file);

}

int main() {

    cout << "--- Gaussian Distributions ---" << endl;
    run(true);

    cout << "--- Pearson Type IV Distributions ---" << endl;
    run(false);

    return 0;

}


#include "FiniteElementTetrahedronMesh.h"

#include <Eigen/Dense>

#include <map>

template<class T>
struct TetrahedralMesh : public FiniteElementTetrahedronMesh<T>
{
	using Base = FiniteElementTetrahedronMesh<T>;

	// from FiniteElementTetrahedonMesh
	using Base::m_meshElements;
	using Base::m_particleX;
	using Base::initializeUSD;
	using Base::initializeTopology;
	using Base::initializeParticles;
	using Vector3 = typename Base::Vector3;

	std::array<int, 3> m_cellSize; // dimensions in grid cells
	int m_radius; // radius of sphere in grid cells
	T m_gridDX;

	std::vector<std::array<int, 3>> m_activeCells; // Marks the "active" cells in the lattice
	std::map<std::array<int, 3>, int> m_activeNodes; // Maps the "active" nodes to their particle index

	std::vector<Vector3> m_particleUndeformedX;
	std::vector<int> m_leftSquish;
	std::vector<int> m_rightSquish;

	TetrahedralMesh()
		:Base(1.e2, .2, 5., .05)
	{
	}

	void initialize()
	{
		initializeUSD("tetrahedral.usda");

		// Activate cells within a sphere of radius m_radius (in cells)

		for (int cell_i = 0; cell_i < m_cellSize[0]; cell_i++)
			for (int cell_j = 0; cell_j < m_cellSize[1]; cell_j++)
				for (int cell_k = 0; cell_k < m_cellSize[1]; cell_k++) {

					int r = (cell_i - m_cellSize[0] / 2) * (cell_i - m_cellSize[0] / 2) +
						(cell_j - m_cellSize[1] / 2) * (cell_j - m_cellSize[1] / 2) +
						(cell_k - m_cellSize[2] / 2) * (cell_k - m_cellSize[2] / 2);

					if (r <= m_radius * m_radius)
						m_activeCells.push_back(std::array<int, 3>{cell_i, cell_j, cell_k});

				}

		std::cout << "Created a model including " << m_activeCells.size() << " lattice cells" << std::endl;

		// Create (uniquely numbered) particles at the node corners of active cells

		for (const auto& cell : m_activeCells) {
			std::array<int, 3> node;
			for (node[0] = cell[0]; node[0] <= cell[0] + 1; node[0]++)
				for (node[1] = cell[1]; node[1] <= cell[1] + 1; node[1]++)
					for (node[2] = cell[2]; node[2] <= cell[2] + 1; node[2]++) {
						auto search = m_activeNodes.find(node);
						if (search == m_activeNodes.end()) { // Particle not yet created at this lattice node location -> make one
							m_activeNodes.insert({ node, m_particleX.size() });
							m_particleX.emplace_back(m_gridDX * T(node[0]), m_gridDX * T(node[1]), m_gridDX * T(node[2]));
							m_particleUndeformedX.emplace_back(m_gridDX * T(node[0]), m_gridDX * T(node[1]), m_gridDX * T(node[2]));
						}
					}
		}
		std::cout << "Model contains " << m_particleX.size() << " particles" << std::endl;

		// Make tetrahedra out of all active cells (6 tetrahedra per cell)

		for (const auto& cell : m_activeCells) {
			int vertexIndices[2][2][2];
			for (int i = 0; i <= 1; i++)
				for (int j = 0; j <= 1; j++)
					for (int k = 0; k <= 1; k++) {
						std::array<int, 3> node{ cell[0] + i, cell[1] + j, cell[2] + k };
						auto search = m_activeNodes.find(node);
						if (search != m_activeNodes.end())
							vertexIndices[i][j][k] = search->second;
						else
							throw std::logic_error("particle at cell vertex not found");
					}

			m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][0][0], vertexIndices[1][1][0], vertexIndices[1][1][1]});
			m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][0][0], vertexIndices[1][1][1], vertexIndices[1][0][1]});
			m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][0][1], vertexIndices[1][1][1], vertexIndices[0][0][1]});
			m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][1][1], vertexIndices[0][1][1], vertexIndices[0][0][1]});
			m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][1][1], vertexIndices[0][1][0], vertexIndices[0][1][1]});
			m_meshElements.push_back(std::array<int, 4>{ vertexIndices[0][0][0], vertexIndices[1][1][0], vertexIndices[0][1][0], vertexIndices[1][1][1]});
		}

		// Perform the USD-specific initialization of topology & particles
		// (this will also create a boundary *surface* to visualuze

		initializeTopology();
		initializeParticles();
		initializeUndeformedConfiguration();

		// Check particle indexing in mesh

		for (const auto& element : m_meshElements)
			for (const auto vertex : element)
				if (vertex < 0 || vertex >= m_particleX.size())
					throw std::logic_error("mismatch between mesh vertex and particle array");

		for (int e = 0; e < m_particleX.size(); e++)
		{
			float leftThreshold = 1.3 - m_gridDX * (m_radius - 1);
			float rightThreshold = 1.25 + m_gridDX * (m_radius - 1);
			// Left side
			if (m_particleUndeformedX[e][0] < leftThreshold) {
				std::cout << "Particles...  " << m_particleUndeformedX[e][0] << std::endl;
				m_leftSquish.push_back(e);
			}
			// Right side
			if (m_particleUndeformedX[e][0] > rightThreshold) {
				std::cout << "XXXParticles...  " << m_particleUndeformedX[e][0] << std::endl;
				m_rightSquish.push_back(e);
			}
		}
	}

	void clearConstrainedParticles(std::vector<Vector3>& x) override
	{
		for (const auto v : m_leftSquish)
			x[v] = Vector3::Zero();
		for (const auto v : m_rightSquish)
			x[v] = Vector3::Zero();
	}

	void setBoundaryConditions() override
	{
		std::cout << "Setting boundary conditions" << std::endl;
		T effectiveTime = std::min<T>(m_stepEndTime, 1.5);

		for (const auto v : m_leftSquish) {
			m_particleX[v] = m_particleUndeformedX[v] + effectiveTime * Vector3(-.5, .5, 0);
		}
		for (const auto v : m_rightSquish) {
			m_particleX[v] = m_particleUndeformedX[v] + effectiveTime * Vector3(.5, -.5, 0);
		}

		std::cout << "Boundary conditions set" << std::endl;
	}
};

int main(int argc, char *argv[])
{
	TetrahedralMesh<float> simulationMesh;
	simulationMesh.m_cellSize = { 50, 50, 50 };
	simulationMesh.m_radius = 20;
	simulationMesh.m_gridDX = 0.05;
	simulationMesh.m_nFrames = 75;
	simulationMesh.m_subSteps = 1;
	simulationMesh.m_frameDt = 0.02;

	// Initialize the simulation example
	simulationMesh.initialize();

	// Output the initial shape of the mesh
	simulationMesh.writeFrame(0);

	for (int frame = 1; frame <= simulationMesh.m_nFrames; frame++) {
		simulationMesh.simulateFrame(frame);
		simulationMesh.writeFrame(frame);
	}

	// Write the entire timeline to USD
	simulationMesh.writeUSD();

	return 0;
}