//  Copyright John Maddock 2006, 2007.
//  Copyright Paul A. Bristow 2006, 2007.
//  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_STATS_TRIANGULAR_HPP
#define BOOST_STATS_TRIANGULAR_HPP

// http://mathworld.wolfram.com/TriangularDistribution.html
// http://en.wikipedia.org/wiki/Triangular_distribution

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/constants/constants.hpp>

#include <utility>

namespace boost{ namespace math
{
  namespace detail
  {
    template <class RealType, class Policy>
    inline bool check_triangular_lower(
      const char* function,
      RealType lower,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(lower))
      { // Any finite value is OK.
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "Lower parameter is %1%, but must be finite!", lower, pol);
        return false;
      }
    } // bool check_triangular_lower(

    template <class RealType, class Policy>
    inline bool check_triangular_mode(
      const char* function,
      RealType mode,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(mode))
      { // any finite value is OK.
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "Mode parameter is %1%, but must be finite!", mode, pol);
        return false;
      }
    } // bool check_triangular_mode(

    template <class RealType, class Policy>
    inline bool check_triangular_upper(
      const char* function,
      RealType upper,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(upper))
      { // any finite value is OK.
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "Upper parameter is %1%, but must be finite!", upper, pol);
        return false;
      }
    } // bool check_triangular_upper(

    template <class RealType, class Policy>
    inline bool check_triangular_x(
      const char* function,
      RealType const& x,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(x))
      { // Any finite value is OK
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "x parameter is %1%, but must be finite!", x, pol);
        return false;
      }
    } // bool check_triangular_x

    template <class RealType, class Policy>
    inline bool check_triangular(
      const char* function,
      RealType lower,
      RealType mode,
      RealType upper,
      RealType* result, const Policy& pol)
    {
      if ((check_triangular_lower(function, lower, result, pol) == false)
        || (check_triangular_mode(function, mode, result, pol) == false)
        || (check_triangular_upper(function, upper, result, pol) == false))
      { // Some parameter not finite.
        return false;
      }
      else if (lower >= upper) // lower == upper NOT useful.
      { // lower >= upper.
        *result = policies::raise_domain_error<RealType>(
          function,
          "lower parameter is %1%, but must be less than upper!", lower, pol);
        return false;
      }
      else
      { // Check lower <= mode <= upper.
        if (mode < lower)
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "mode parameter is %1%, but must be >= than lower!", lower, pol);
          return false;
        }
        if (mode > upper)
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "mode parameter is %1%, but must be <= than upper!", upper, pol);
          return false;
        }
        return true; // All OK.
      }
    } // bool check_triangular
  } // namespace detail

  template <class RealType = double, class Policy = policies::policy<> >
  class triangular_distribution
  {
  public:
    typedef RealType value_type;
    typedef Policy policy_type;

    triangular_distribution(RealType lower = -1, RealType mode = 0, RealType upper = 1)
      : m_lower(lower), m_mode(mode), m_upper(upper) // Constructor.
    { // Evans says 'standard triangular' is lower 0, mode 1/2, upper 1,
      // has median sqrt(c/2) for c <=1/2 and 1 - sqrt(1-c)/2 for c >= 1/2
      // But this -1, 0, 1 is more useful in most applications to approximate normal distribution,
      // where the central value is the most likely and deviations either side equally likely.
      RealType result;
      detail::check_triangular("boost::math::triangular_distribution<%1%>::triangular_distribution",lower, mode, upper, &result, Policy());
    }
    // Accessor functions.
    RealType lower()const
    {
      return m_lower;
    }
    RealType mode()const
    {
      return m_mode;
    }
    RealType upper()const
    {
      return m_upper;
    }
  private:
    // Data members:
    RealType m_lower;  // distribution lower aka a
    RealType m_mode;  // distribution mode aka c
    RealType m_upper;  // distribution upper aka b
  }; // class triangular_distribution

  typedef triangular_distribution<double> triangular;

  template <class RealType, class Policy>
  inline const std::pair<RealType, RealType> range(const triangular_distribution<RealType, Policy>& /* dist */)
  { // Range of permissible values for random variable x.
    using boost::math::tools::max_value;
    return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>());
  }

  template <class RealType, class Policy>
  inline const std::pair<RealType, RealType> support(const triangular_distribution<RealType, Policy>& dist)
  { // Range of supported values for random variable x.
    // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
    return std::pair<RealType, RealType>(dist.lower(), dist.upper());
  }

  template <class RealType, class Policy>
  RealType pdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)
  {
    static const char* function = "boost::math::pdf(const triangular_distribution<%1%>&, %1%)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_triangular_x(function, x, &result, Policy()))
    {
      return result;
    }
    if((x < lower) || (x > upper))
    {
      return 0;
    }
    if (x == lower)
    { // (mode - lower) == 0 which would lead to divide by zero!
      return (mode == lower) ? 2 / (upper - lower) : RealType(0);
    }
    else if (x == upper)
    {
      return (mode == upper) ? 2 / (upper - lower) : RealType(0);
    }
    else if (x <= mode)
    {
      return 2 * (x - lower) / ((upper - lower) * (mode - lower));
    }
    else
    {  // (x > mode)
      return 2 * (upper - x) / ((upper - lower) * (upper - mode));
    }
  } // RealType pdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)

  template <class RealType, class Policy>
  inline RealType cdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)
  {
    static const char* function = "boost::math::cdf(const triangular_distribution<%1%>&, %1%)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_triangular_x(function, x, &result, Policy()))
    {
      return result;
    }
    if((x <= lower))
    {
      return 0;
    }
    if (x >= upper)
    {
      return 1;
    }
    // else lower < x < upper
    if (x <= mode)
    {
      return ((x - lower) * (x - lower)) / ((upper - lower) * (mode - lower));
    }
    else
    {
      return 1 - (upper - x) *  (upper - x) / ((upper - lower) * (upper - mode));
    }
  } // RealType cdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)

  template <class RealType, class Policy>
  RealType quantile(const triangular_distribution<RealType, Policy>& dist, const RealType& p)
  {
    BOOST_MATH_STD_USING  // for ADL of std functions (sqrt).
    static const char* function = "boost::math::quantile(const triangular_distribution<%1%>&, %1%)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks
    if(false == detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_probability(function, p, &result, Policy()))
    {
      return result;
    }
    if(p == 0)
    {
      return lower;
    }
    if(p == 1)
    {
      return upper;
    }
    RealType p0 = (mode - lower) / (upper - lower);
    RealType q = 1 - p;
    if (p < p0)
    {
      result = sqrt((upper - lower) * (mode - lower) * p) + lower;
    }
    else if (p == p0)
    {
      result = mode;
    }
    else // p > p0
    {
      result = upper - sqrt((upper - lower) * (upper - mode) * q);
    }
    return result;

  } // RealType quantile(const triangular_distribution<RealType, Policy>& dist, const RealType& q)

  template <class RealType, class Policy>
  RealType cdf(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)
  {
    static const char* function = "boost::math::cdf(const triangular_distribution<%1%>&, %1%)";
    RealType lower = c.dist.lower();
    RealType mode = c.dist.mode();
    RealType upper = c.dist.upper();
    RealType x = c.param;
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_triangular_x(function, x, &result, Policy()))
    {
      return result;
    }
    if (x <= lower)
    {
      return 1;
    }
    if (x >= upper)
    {
      return 0;
    }
    if (x <= mode)
    {
      return 1 - ((x - lower) * (x - lower)) / ((upper - lower) * (mode - lower));
    }
    else
    {
      return (upper - x) *  (upper - x) / ((upper - lower) * (upper - mode));
    }
  } // RealType cdf(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)

  template <class RealType, class Policy>
  RealType quantile(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)
  {
    BOOST_MATH_STD_USING  // Aid ADL for sqrt.
    static const char* function = "boost::math::quantile(const triangular_distribution<%1%>&, %1%)";
    RealType l = c.dist.lower();
    RealType m = c.dist.mode();
    RealType u = c.dist.upper();
    RealType q = c.param; // probability 0 to 1.
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, l, m, u, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_probability(function, q, &result, Policy()))
    {
      return result;
    }
    if(q == 0)
    {
      return u;
    }
    if(q == 1)
    {
      return l;
    }
    RealType lower = c.dist.lower();
    RealType mode = c.dist.mode();
    RealType upper = c.dist.upper();

    RealType p = 1 - q;
    RealType p0 = (mode - lower) / (upper - lower);
    if(p < p0)
    {
      RealType s = (upper - lower) * (mode - lower);
      s *= p;
      result = sqrt((upper - lower) * (mode - lower) * p) + lower;
    }
    else if (p == p0)
    {
      result = mode;
    }
    else // p > p0
    {
      result = upper - sqrt((upper - lower) * (upper - mode) * q);
    }
    return result;
  } // RealType quantile(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)

  template <class RealType, class Policy>
  inline RealType mean(const triangular_distribution<RealType, Policy>& dist)
  {
    static const char* function = "boost::math::mean(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0;  // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return (lower + upper + mode) / 3;
  } // RealType mean(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType variance(const triangular_distribution<RealType, Policy>& dist)
  {
    static const char* function = "boost::math::mean(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return (lower * lower + upper * upper + mode * mode - lower * upper - lower * mode - upper * mode) / 18;
  } // RealType variance(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType mode(const triangular_distribution<RealType, Policy>& dist)
  {
    static const char* function = "boost::math::mode(const triangular_distribution<%1%>&)";
    RealType mode = dist.mode();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular_mode(function, mode, &result, Policy()))
    { // This should never happen!
      return result;
    }
    return mode;
  } // RealType mode

  template <class RealType, class Policy>
  inline RealType median(const triangular_distribution<RealType, Policy>& dist)
  {
    BOOST_MATH_STD_USING // ADL of std functions.
    static const char* function = "boost::math::median(const triangular_distribution<%1%>&)";
    RealType mode = dist.mode();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular_mode(function, mode, &result, Policy()))
    { // This should never happen!
      return result;
    }
    RealType lower = dist.lower();
    RealType upper = dist.upper();
    if (mode < (upper - lower) / 2)
    {
      return lower + sqrt((upper - lower) * (mode - lower)) / constants::root_two<RealType>();
    }
    else
    {
      return upper - sqrt((upper - lower) * (upper - mode)) / constants::root_two<RealType>();
    }
  } // RealType mode

  template <class RealType, class Policy>
  inline RealType skewness(const triangular_distribution<RealType, Policy>& dist)
  {
    BOOST_MATH_STD_USING  // for ADL of std functions
    using namespace boost::math::constants; // for root_two
    static const char* function = "boost::math::skewness(const triangular_distribution<%1%>&)";

    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == boost::math::detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return root_two<RealType>() * (lower + upper - 2 * mode) * (2 * lower - upper - mode) * (lower - 2 * upper + mode) /
      (5 * pow((lower * lower + upper + upper + mode * mode - lower * upper - lower * mode - upper * mode), RealType(3)/RealType(2)));
  } // RealType skewness(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType kurtosis(const triangular_distribution<RealType, Policy>& dist)
  { // These checks may be belt and braces as should have been checked on construction?
    static const char* function = "boost::math::kurtosis(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType upper = dist.upper();
    RealType mode = dist.mode();
    RealType result = 0;  // of checks.
    if(false == detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return static_cast<RealType>(12)/5; //  12/5 = 2.4;
  } // RealType kurtosis_excess(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType kurtosis_excess(const triangular_distribution<RealType, Policy>& dist)
  { // These checks may be belt and braces as should have been checked on construction?
    static const char* function = "boost::math::kurtosis_excess(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType upper = dist.upper();
    RealType mode = dist.mode();
    RealType result = 0;  // of checks.
    if(false == detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return static_cast<RealType>(-3)/5; // - 3/5 = -0.6
    // Assuming mathworld really means kurtosis excess?  Wikipedia now corrected to match this.
  }

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_TRIANGULAR_HPP


#include <functional>

#include <Qt3DIncludes.h>
#include <GaussIncludes.h>
#include <FEMIncludes.h>

//Any extra things I need such as constraints
#include <ConstraintFixedPoint.h>
#include <TimeStepperEigenFitSMW.h>
#include <EigenFit.h>
#include <fstream>

using namespace Gauss;
using namespace FEM;
using namespace ParticleSystem; //For Force Spring

/* Tetrahedral finite elements */

//typedef physical entities I need

//typedef scene
typedef PhysicalSystemFEM<double, NeohookeanHFixedTet> FEMLinearTets;

typedef World<double, std::tuple<FEMLinearTets *>,
std::tuple<ForceSpringFEMParticle<double> *, ForceParticlesGravity<double> *>,
std::tuple<ConstraintFixedPoint<double> *> > MyWorld;

//typedef World<double, std::tuple<FEMLinearTets *,PhysicalSystemParticleSingle<double> *>,
//                      std::tuple<ForceSpringFEMParticle<double> *>,
//                      std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
//typedef TimeStepperEigenFitSMW<double, AssemblerEigenSparseMatrix<double>, AssemblerEigenVector<double>> MyTimeStepper;
typedef TimeStepperEigenFitSMW<double, AssemblerParallel<double, AssemblerEigenSparseMatrix<double>>, AssemblerParallel<double, AssemblerEigenVector<double>> > MyTimeStepper;

typedef Scene<MyWorld, MyTimeStepper> MyScene;

//typedef TimeStepperEigenFitSI<double, AssemblerParallel<double, AssemblerEigenSparseMatrix<double> >,
//AssemblerParallel<double, AssemblerEigenVector<double> > > MyTimeStepper;

//typedef Scene<MyWorld, MyTimeStepper> MyScene;

// used for preStepCallback. should be delete
std::vector<ConstraintFixedPoint<double> *> movingConstraints;
Eigen::VectorXi movingVerts;
Eigen::MatrixXd V;
Eigen::MatrixXi F;
char **arg_list;
unsigned int istep;

void preStepCallback(MyWorld &world) {
//    // This is an example callback
//    if (atoi(arg_list[5]) == 2)
//    {
//        // This is an example callback
//
//        //script some motion
////
//        if (istep < 50) {
//
//            for(unsigned int jj=0; jj<movingConstraints.size(); ++jj) {
//                if(movingConstraints[jj]->getImpl().getFixedPoint()[0] > -3) {
//                    Eigen::Vector3d v = V.row(movingVerts[jj]);
//                    Eigen::Vector3d new_p = v + Eigen::Vector3d(0.0,1.0/10,0.0);
//                    movingConstraints[jj]->getImpl().setFixedPoint(new_p);
//                }
//            }
//        }
//    }
}

int main(int argc, char **argv) {
    //    arg list
    //    1: full path to coarse mesh
    //    2: full path to fine mesh
    //    3: youngs modulus (SI unit)
    //    4: constraint threshold (for defualt constraint profile)
    //    5: constraint profile switch
    //    6: name of the initial deformation profile
    //    7. number of time steps
//    8. flag for using hausdorff distance
//    9. number of modes to modifies
    std::cout<<"Test Neohookean FEM EigenFit\n";

    //Setup Physics
    MyWorld world;

    arg_list = argv;

    Eigen::MatrixXd Vf;
    Eigen::MatrixXi Ff;

    //    define the file separator base on system
    const char kPathSeparator =
#ifdef _WIN32
    '\\';
#else
    '/';
#endif

    if (argc > 1) {
        // must supply all 9 parameters

        std::string cmeshname = argv[1];
        std::string fmeshname = argv[2];

        readTetgen(V, F, dataDir()+cmeshname+".node", dataDir()+cmeshname+".ele");
        readTetgen(Vf, Ff, dataDir()+fmeshname+".node", dataDir()+fmeshname+".ele");

        std::string::size_type found = cmeshname.find_last_of(kPathSeparator);
        //    acutal name for the mesh, no path
        std::string cmeshnameActual = cmeshname.substr(found+1);

        //    acutal name for the mesh, no path
        std::string fmeshnameActual = fmeshname.substr(found+1);

        //    parameters
        double youngs = atof(argv[3]);
        double poisson = 0.45;
        int constraint_dir = 0; // constraint direction. 0 for x, 1 for y, 2 for z
        double constraint_tol = atof(argv[4]);
        //
        // send the constraint switch in as well, or the fine embedded mesh. ugly
        // the flag indicate whether to recalculated or not
        // need to pass the material and constraint parameters to embedding too. need to do it again below. ugly
        // also use the last two args to determine how many modes to fix. have to put it here now. ugly
        EigenFit *test = new EigenFit(V,F,Vf,Ff,true,youngs,poisson,constraint_dir,constraint_tol, atoi(argv[5]),atoi(argv[8]),atoi(argv[9]));

        world.addSystem(test);

        // projection matrix for constraints
        Eigen::SparseMatrix<double> P;
        if (atoi(argv[5]) == 0) {
            // constraint switch

            //            zero gravity
            Eigen::Vector3x<double> g;
            g(0) = 0;
            g(1) = 0;
            g(2) = 0;

            for(unsigned int iel=0; iel<test->getImpl().getF().rows(); ++iel) {

                test->getImpl().getElement(iel)->setGravity(g);

            }

            world.finalize(); //After this all we're ready to go (clean up the interface a bit later)

            //            set the projection matrix to identity because there is no constraint to project
//            Eigen::SparseMatrix<double> P;
            P.resize(V.rows()*3,V.rows()*3);
            P.setIdentity();
//            std::cout<<P.rows();
            //            no constraints
        }
        else if(atoi(argv[5]) == 1)
        {
            //    default constraint
            fixDisplacementMin(world, test,constraint_dir,constraint_tol);
            world.finalize(); //After this all we're ready to go (clean up the interface a bit later)

            // construct the projection matrix for stepper
            Eigen::VectorXi indices = minVertices(test, constraint_dir,constraint_tol);
            P = fixedPointProjectionMatrix(indices, *test,world);

        }
        else if (atoi(argv[5]) == 2)
        {

            movingVerts = minVertices(test, constraint_dir, constraint_tol);//indices for moving parts
//
            for(unsigned int ii=0; ii<movingVerts.rows(); ++ii) {
                movingConstraints.push_back(new ConstraintFixedPoint<double>(&test->getQ()[movingVerts[ii]], Eigen::Vector3d(0,0,0)));
                world.addConstraint(movingConstraints[ii]);
            }
            fixDisplacementMin(world, test,constraint_dir,constraint_tol);

            world.finalize(); //After this all we're ready to go (clean up the interface a bit later)

            P = fixedPointProjectionMatrix(movingVerts, *test,world);

        }

        // set material
        for(unsigned int iel=0; iel<test->getImpl().getF().rows(); ++iel) {

            test->getImpl().getElement(iel)->setParameters(youngs, poisson);

        }

        auto q = mapStateEigen(world);
        auto fine_q = mapStateEigen(test->getFineWorld());
        //    default to zero deformation
        q.setZero();

        if (strcmp(argv[6],"0")==0) {

            q.setZero();
        }
        else
        {

            std::string qfileName(argv[6]);
            Eigen::VectorXd  tempv;
            loadMarketVector(tempv,qfileName);

            q = tempv;

        }

        MyTimeStepper stepper(0.01,P);

        //         the number of steps to take

        unsigned int file_ind = 0;
        std::string name = "pos";
        std::string fformat = ".obj";
        std::string filename = name + std::to_string(file_ind) + fformat;
        std::string qname = cmeshnameActual + "ExampleQ";
        std::string qfformat = ".mtx";
        std::string qfilename = qname + std::to_string(file_ind) + qfformat;
        std::string qname2 = fmeshnameActual + "ExampleQ";
        std::string qfilename2 = qname2 + std::to_string(file_ind) + qfformat;

        struct stat buf;
        unsigned int idx;

        for(istep=0; istep<atoi(argv[7]) ; ++istep) {
            stepper.step(world);

            // acts like the "callback" block
            if (atoi(arg_list[5]) == 2)
            {
                //script some motion
                //

                for(unsigned int jj=0; jj<movingConstraints.size(); ++jj) {

                    auto v_q = mapDOFEigen(movingConstraints[jj]->getDOF(0), world.getState());
//
//                    if ((istep%150) < 50) {
//                        Eigen::Vector3d new_q = (istep%150)*Eigen::Vector3d(0.0,-1.0/100,0.0);
//                        v_q = new_q;
//                    }
//                    else if ((istep%150) < 100)
//                    {}
//                    else
//                    {
//                        Eigen::Vector3d new_q =  (150-(istep%150))*Eigen::Vector3d(0.0,-1.0/100,0.0);
//                        v_q = new_q;
//                    }
                    Eigen::Vector3d new_q = (istep)*Eigen::Vector3d(0.0,-1.0/100,0.0);
                    v_q = new_q;

                }
            }

            //output data here
            std::ofstream ofile;
            ofile.open("KE.txt", std::ios::app); //app is append which means it will put the text at the end
            ofile << std::get<0>(world.getSystemList().getStorage())[0]->getImpl().getKineticEnergy(world.getState()) << std::endl;
            ofile.close();

            while (stat(filename.c_str(), &buf) != -1)
            {
                file_ind++;
                filename = name + std::to_string(file_ind) + fformat;
                qfilename = qname + std::to_string(file_ind) + qfformat;
                qfilename2 = qname2 + std::to_string(file_ind) + qfformat;

            }

            idx = 0;
            // getGeometry().first is V
            Eigen::MatrixXd V_disp = std::get<0>(world.getSystemList().getStorage())[0]->getGeometry().first;

            for(unsigned int vertexId=0;  vertexId < std::get<0>(world.getSystemList().getStorage())[0]->getGeometry().first.rows(); ++vertexId) {

                // because getFinePosition is in EigenFit, not another physical system Impl, so don't need getImpl()
                V_disp(vertexId,0) += q(idx);
                idx++;
                V_disp(vertexId,1) += q(idx);
                idx++;
                V_disp(vertexId,2) += q(idx);
                idx++;
            }
            igl::writeOBJ(filename,V_disp,std::get<0>(world.getSystemList().getStorage())[0]->getGeometry().second);
            // coarse mesh data
            q = mapStateEigen(world);
            saveMarketVector(q, qfilename);
            // fine mesh data from embedded mesh in eigenfit
            fine_q = mapStateEigen(test->getFineWorld());
            saveMarketVector(fine_q, qfilename2);
        }
    }
    else
    {
        // using all default paramters for eigenfit

        //    default example meshes
        std::string cmeshname = "/meshesTetgen/arma/arma_6";
        std::string fmeshname = "/meshesTetgen/arma/arma_1";

        readTetgen(V, F, dataDir()+cmeshname+".node", dataDir()+cmeshname+".ele");
        readTetgen(Vf, Ff, dataDir()+fmeshname+".node", dataDir()+fmeshname+".ele");

        std::string::size_type found = cmeshname.find_last_of(kPathSeparator);
        //    acutal name for the mesh, no path
        std::string cmeshnameActual = cmeshname.substr(found+1);

        //    default parameters
        double youngs = 5e5;
        double poisson = 0.45;
        int constraint_dir = 0; // constraint direction. 0 for x, 1 for y, 2 for z
        double constraint_tol = 2e-1;

        // no constraint switch so just create the eigenfit obj with constraint switch set to 1
        // the flag indicate whether to recalculated or not
        // need to pass the material and constraint parameters to embedding too. need to do it again below. ugly
        // also use the last two args to determine how many modes to fix. default not using hausdorff distance, and use 10 modes. have to put it here now.  ugly
        EigenFit *test = new EigenFit(V,F,Vf,Ff,true,youngs,poisson,constraint_dir,constraint_tol, 1,false,10);

        // set material
        for(unsigned int iel=0; iel<test->getImpl().getF().rows(); ++iel) {

            test->getImpl().getElement(iel)->setParameters(youngs, poisson);

        }

        world.addSystem(test);

        world.finalize(); //After this all we're ready to go (clean up the interface a bit later)
        // IMPORTANT, need to finalized before fix boundary

        //    default constraint
        fixDisplacementMin(world, test,constraint_dir,constraint_tol);

        // construct the projection matrix for stepper
        Eigen::VectorXi indices = minVertices(test, constraint_dir,constraint_tol);
        Eigen::SparseMatrix<double> P = fixedPointProjectionMatrix(indices, *test,world);

        auto q = mapStateEigen(world);

        //    default to zero deformation
        q.setZero();

        MyTimeStepper stepper(0.01,P);

        //Display
        QGuiApplication app(argc, argv);

        MyScene *scene = new MyScene(&world, &stepper, preStepCallback);
        GAUSSVIEW(scene);

        return app.exec();

    }

}


#include "HelmertTransformation.h"
#include "Exception.h"
#include <boost/math/constants/constants.hpp>
#include <cstdio>
#include <sstream>
#include <stdexcept>

#ifdef _MSC_VER
#define __PRETTY_FUNCTION__ BOOST_CURRENT_FUNCTION
#endif

Fmi::HelmertTransformation::HelmertTransformation() : m(1), ex(0), ey(0), ez(0), tx(0), ty(0), tz(0)
{
}

boost::array<double, 3> Fmi::HelmertTransformation::operator()(
    const boost::array<double, 3>& x) const
{
  try
  {
    boost::array<double, 3> y = {m * (x[0] + (-ez) * x[1] + (ey)*x[2]) + tx,
                                 m * ((ez)*x[0] + x[1] + (-ex) * x[2]) + ty,
                                 m * ((-ey) * x[0] + (ex)*x[1] + x[2]) + tz};
    return y;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void Fmi::HelmertTransformation::set_fmi_sphere_to_reference_ellipsoid_conv(
    double r,
    double lat,
    double lon,
    const ReferenceEllipsoid& ref,
    enum FmiSphereConvScalingType scaling_type)
{
  try
  {
    ex = 0;
    ey = 0;
    ez = 0;
    if (scaling_type == FMI_SPHERE_NO_SCALING)
    {
      m = 1;
      const boost::array<double, 3> x0 = ref.to_geocentric(lat, lon, -r);
      tx = x0[0];
      ty = x0[1];
      tz = x0[2];
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_EAST_WEST_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      m = a / (r * v);
      tx = 0;
      ty = 0;
      tz = -a * e * e * sin(lat) / v;
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_SOUTH_NORTH_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      const double sf = sin(lat);
      const double cf = cos(lat);
      m = (a * (1 - e * e)) / (r * v * v * v);
      tx = a * e * e * cos(lon) * cf * cf * cf / (v * v * v);
      ty = a * e * e * sin(lon) * cf * cf * cf / (v * v * v);
      tz = -a * e * e * (1 - e * e) * sf * sf * sf / (v * v * v);
    }
    else
    {
      std::ostringstream msg;
      msg << __PRETTY_FUNCTION__ << ": wrong scaling type " << scaling_type;
      throw Fmi::Exception(BCP, msg.str());
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void Fmi::HelmertTransformation::set_reference_ellipsoid_to_fmi_sphere_conv(
    double r,
    double lat,
    double lon,
    const ReferenceEllipsoid& ref,
    enum FmiSphereConvScalingType scaling_type)
{
  try
  {
    ex = 0;
    ey = 0;
    ez = 0;
    if (scaling_type == FMI_SPHERE_NO_SCALING)
    {
      m = 1;
      const boost::array<double, 3> x0 = ref.to_geocentric(lat, lon, -r);
      tx = -x0[0];
      ty = -x0[1];
      tz = -x0[2];
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_EAST_WEST_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      m = r * v / a;
      tx = 0;
      ty = 0;
      tz = r * e * e * sin(lat);
    }
    else if (scaling_type == FMI_SPHERE_PRESERVE_SOUTH_NORTH_SCALE)
    {
      const double a = ref.get_semimajor_axis();
      const double e = ref.get_eccentricity();
      const double v = sqrt(1 - e * e * sin(lat) * sin(lat));
      const double sf = sin(lat);
      const double cf = cos(lat);
      m = r * v * v * v / (a * (1 - e * e));
      tx = -r * e * e * cos(lon) * cf * cf * cf / (1 - e * e);
      ty = -r * e * e * sin(lon) * cf * cf * cf / (1 - e * e);
      tz = r * e * e * sf * sf * sf;
    }
    else
    {
      std::ostringstream msg;
      msg << __PRETTY_FUNCTION__ << ": wrong scaling type " << scaling_type;
      throw Fmi::Exception(BCP, msg.str());
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

std::string Fmi::get_fmi_sphere_towgs84_proj4_string(
    double r,
    double lat,
    double lon,
    enum Fmi::HelmertTransformation::FmiSphereConvScalingType scaling_type)
{
  try
  {
    const double AS = 180.0 * 3600.0 / boost::math::constants::pi<double>();
    Fmi::HelmertTransformation conv;
    conv.set_fmi_sphere_to_reference_ellipsoid_conv(
        r, lat, lon, Fmi::ReferenceEllipsoid::wgs84, scaling_type);
    char buffer[512];
#ifndef _MSC_VER
    snprintf(buffer,
             sizeof(buffer),
#else
    _snprintf(buffer,
              sizeof(buffer),
#endif
             "+towgs84=%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.0f ",
             conv.tx,
             conv.ty,
             conv.tz,
             AS * conv.ex,
             AS * conv.ey,
             AS * conv.ez,
             1e6 * (conv.m - 1));
    buffer[sizeof(buffer) - 1] = 0;
    return buffer;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}


#include "TROOT.h"
#include "TStyle.h"
#include "TFile.h"
#include "TH1.h"
#include "TGraphAsymmErrors.h"
#include "TCanvas.h"
#include <cmath>
#include <fstream>
#include <iostream>

#include <exception>
#include <iterator>
#include <string>
#include <vector>

#if (defined (STANDALONE) or defined (__CINT__) )
    #include "ClopperPearsonBinomialInterval.h"
#else
    #include "PhysicsTools/RooStatsCms/interface/ClopperPearsonBinomialInterval.h"
#endif

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

// A helper function to simplify the main part.
template<class T>
ostream& operator<<(ostream& os, const vector<T>& v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

int main(int ac, char *av[]) {
  gROOT->SetBatch(kTRUE);
  gROOT->SetStyle("Plain");

  try{
    string file;
    string num;
    string den;
    string ext;
    string pname;
    unsigned int rebin=1;
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("input-file,i", po::value<string >(&file), "input file")
      ("num,n", po::value<string > (&num), "numHisto")
      ("den,d", po::value<string > (&den), "denHisto")
      ("plotname,o", po::value<string > (&pname), "plot name")
      ("rebin,r", po::value<unsigned int > (&rebin)->default_value(1), "rebin")
      ("plot-format,p", po::value<string>(&ext)->default_value("gif"),
       "output plot format");

    po::positional_options_description p;
    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
	      options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
    }

   	TFile * root_file = new TFile(file.c_str(),"read");
	//TFile * root_file = new TFile("MuTrigger_133874_133828_768ub.root","read");
	//string d_string = *itd;
	string  dirDen =   den;
	string  dirNum =   num;

	TH1D * denh = (TH1D*) root_file->Get( dirDen.c_str()  );
	TH1D * numh = (TH1D*) root_file->Get( dirNum.c_str() );

         int bins = denh->GetXaxis()->GetNbins();
        bins = bins/rebin;
	const double xMax = denh->GetXaxis()->GetXmax();
  	const double xMin = denh->GetXaxis()->GetXmin();
	//std::cout << "xMax " << xMax << endl;
	//std::cout << "xMin " << xMax << endl;
	//std::cout << "bins " << bins << endl;
	double * x = new double[bins];
	double *eff = new double[bins];
	double * exl= new double[bins];
	double *exh = new double[bins];
	double *  eefflCP= new double[bins];
	double * eeffhCP = new double[bins];

	ClopperPearsonBinomialInterval cp;
	//  alpha = 1 - CL
	const double alpha = (1-0.682);
	cp.init(alpha);
	TH1D histo("histo", "Efficiency", bins, xMin, xMax);

	for(int i = 1; i <= bins; i++) {
          int j = i-1;
	  x[j] = ((double(i-0.5)) * (xMax - xMin) / (bins )) + xMin;
	    int n0 = denh->GetBinContent(i);
	  //	  std::cout << " n0 " << n0 << endl;
	   int n1 = numh->GetBinContent(i);
	  // std::cout << " n1 " << n1 << endl;
	  if ( n0!=0) {
	    eff[j] = double(n1)/double(n0);
	    histo.SetBinContent(i,eff[j]);
	    exl[j] = exh[j] = 0;
	    cp.calculate(n1, n0);
	    eefflCP[j] = eff[j] - cp.lower();
	    eeffhCP[j] = cp.upper() - eff[j];
	  } else {
	    eff[j]=0;
	    histo.SetBinContent(i,eff[j]);
	    exl[j] = exh[j] = 0;
	    //cp.calculate(n1, n0);
	    eefflCP[i] = 0;
	    eeffhCP[i] = 0;

	  }
	  //std::cout<< "x[j] " <<x[j]<<std::endl;
	  //std::cout<< "n0 " <<n0<<std::endl;
	  //std::cout<< "n1 " <<n1<<std::endl;
	  //histo.SetBinContent(i+1,eff[i]);
	      //exl[i] = exh[i] = 0;
	      //cp.calculate(n1, n0);
	      //eefflCP[i] = eff[i] - cp.lower();
	      //eeffhCP[i] = cp.upper() - eff[i];
	}
	TGraphAsymmErrors graphCP(bins, x, eff, exl, exh, eefflCP, eeffhCP);
	graphCP.SetTitle("trigger (HLT_Mu9 path) efficiency");
	graphCP.SetMarkerColor(kRed);
	graphCP.SetMarkerStyle(21);
	graphCP.SetLineWidth(1);
	graphCP.SetLineColor(kRed);
        string cname = pname;
	TCanvas * c = new TCanvas(cname.c_str());
	gStyle->SetOptStat(0);
	histo.SetTitle("MC trigger (HLT_Mu9 path) efficiency");
	histo.Draw();
	histo.SetLineColor(kWhite);
        histo.SetMinimum(0.0)
;	histo.GetXaxis()->SetTitle("p_{T} (GeV/c)");
	histo.GetYaxis()->SetTitle("efficiency");
        graphCP.Draw("P");
    	string plot= "HLTMu_9" +  pname + "." +  ext ;
	c->SaveAs(plot.c_str());
        string outfile= "Effhisto" +   pname + ".root";
	TFile * output_file = TFile::Open(outfile.c_str(), "recreate");
        string outdir =  pname;
	TDirectory * dir = output_file->mkdir(outdir.c_str());
	dir->cd();
        c->Write();
        histo.Write();
        graphCP.Write();
	output_file->Close();
        delete c;
      }

  catch(std::exception& e) {
    cerr << "error: " << e.what() << "\n";
    return 1;
  }
  catch(...) {
    cerr << "Exception of unknown type!\n";
  }

  return 0;
}


/*-------------------------------------------------------------------

                  Copyright (c) 2006
         Nicola Beume <nicola.beume@tu-dortmund.de>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 2 of the License, or
(at your option) any later version.

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
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

---------------------------------------------------------------------

This program calculates the dominated hypervolume or S-metric of a
set of d-dimensional points (d>=3). Please refer to the following
publication for a description of the algorithm:

Nicola Beume and Guenter Rudolph.
Faster S-Metric Calculation by Considering Dominated Hypervolume
as Klee's Measure Problem.
In: B. Kovalerchuk (ed.): Proceedings of the Second IASTED
Conference on Computational Intelligence (CI 2006), pp. 231-236.
ACTA Press: Anaheim, 2006.

Extended version published as:
Technical Report of the Collaborative Research Centre 531
'Computational Intelligence', CI-216/06, ISSN 1433-3325.
University of Dortmund, July 2006.

-------------------------------------------------------------------*/

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cmath>
#include <bitset> // for calculation of trellis
#include <vector>
#include <Eigen/Dense>
using namespace std;

/* function invoked by main */
void stream(double regLow[], double regUp[], const vector<double*>& cubs, int lev, double cov);
bool cmp(double* a, double* b);
/* function invoked by stream */
/*
inline bool cmp(double* a, double* b);
inline bool covers(const double* cub, const double regLow[]);
inline bool partCovers(const double* cub, const double regUp[]);
inline int containsBoundary(const double* cub, const double regLow[], const int split);
inline double getMeasure(const double regLow[], const double regUp[]);
inline int isPile(const double* cub, const double regLow[], const double regUp[]);
inline double computeTrellis(const double regLow[], const double regUp[], const double trellis[]);
inline double getMedian(vector<double>& bounds);
*/

/* global variables */
static int dataNumber;
static int dimension;
static double dSqrtDataNumber;
static double volume;

static double calculate(int dim, int dataNum, const Eigen::MatrixXd &xd, const Eigen::MatrixXd &xr) {
    /*
     * arguments are:
     * dimension of input data points
     * number of input data points
     * file name of input data
     * file name of reference point
     */

    int i,j;
    dimension = dim;
    dataNumber = dataNum;
    if (dimension < 3) {
        fprintf(stderr, "invalid argument\n");
        exit(1);
    }

    static vector<double*> pointsInitial(dataNumber);

    for (int i = 0; i < dataNumber; i++) {
        pointsInitial[i] = new double[dimension];
        for (int j = 0; j < dimension; j++) {
            pointsInitial[i][j] = xd(i, j);
        }
    }

    // read in reference point
    static double* refPoint = new double[dimension];
    for (int i = 0; i < dimension; i++) {
        refPoint[i] = xr(i);
    }

    // initialize volume
    volume = 0.0;
    // sqrt of dataNumber
    dSqrtDataNumber = sqrt((double)dataNumber);

    // initialize region
    double* regionLow = new double[dimension-1];
    double* regionUp = new double[dimension-1];
    for (j=0; j<dimension-1; j++)  {
        // determine minimal j coordinate
        double min = 10000000.0;
        for (i=0; i<dataNumber; i++) {
            if (pointsInitial[i][j] < min) {
                min = pointsInitial[i][j];
            }
        }
        regionLow[j] = min;
        regionUp[j] = refPoint[j];
    }

    // sort pointList according to d-th dimension
    sort(pointsInitial.begin(), pointsInitial.end(), cmp);

    // call stream initially
    stream(regionLow, regionUp, pointsInitial, 0, refPoint[dimension-1]);
    // return hypervolume
    return volume;
}

inline bool cmp(double* a, double* b) {
    return (a[dimension-1] < b[dimension-1]);
}

inline bool covers(const double* cub, const double regLow[]) {
    static int i;
    for (i=0; i<dimension-1; i++) {
        if (cub[i] > regLow[i]) {
            return false;
        }
    }
    return true;
}

inline bool partCovers(const double* cub, const double regUp[]) {
    static int i;
    for (i=0; i<dimension-1; i++) {
        if (cub[i] >= regUp[i]) {
            return false;
        }
    }
    return true;
}

inline int containsBoundary(const double* cub, const double regLow[], const int split) {
    // condition only checked for split>0
    if (regLow[split] >= cub[split]){
        // boundary in dimension split not contained in region, thus
        // boundary is no candidate for the splitting line
        return -1;
    }
    else {
        static int j;
        for (j=0; j<split; j++) { // check boundaries
            if (regLow[j] < cub[j]) {
                // boundary contained in region
                return 1;
            }
        }
    }
    // no boundary contained in region
    return 0;
}

inline double getMeasure(const double regLow[], const double regUp[]) {
    static double vol;
    static int i;
    vol = 1.0;
    for (i=0; i<dimension-1; i++) {
        vol *= (regUp[i] - regLow[i]);
    }
    return vol;
}

inline int isPile(const double* cub, const double regLow[], const double regUp[]) {
    static int pile;
    static int k;

    pile = dimension;
    // check all dimensions of the node
    for (k=0; k<dimension-1; k++) {
        // k-boundary of the node's region contained in the cuboid?
        if (cub[k] > regLow[k]) {
            if (pile != dimension) {
                // second dimension occured that is not completely covered
                // ==> cuboid is no pile
                return -1;
            }
            pile = k;
        }
    }
    // if pile == this.dimension then
    // cuboid completely covers region
    // case is not possible since covering cuboids have been removed before

    // region in only one dimenison not completly covered
    // ==> cuboid is a pile
    return pile;
}

inline double computeTrellis(const double regLow[], const double regUp[], const double trellis[]) {

    static int i,j;
    static double vol;
    static int numberSummands;
    static double summand;
    static bitset<16> bitvector;

    vol= 0.0;
    summand = 0.0;
    numberSummands = 0;

    // calculate number of summands
    static bitset<16> nSummands;
    for (i=0; i<dimension-1; i++) {
        nSummands[i] = 1;
    }
    numberSummands = nSummands.to_ulong();

    static double* valueTrellis = new double[dimension-1];
    static double* valueRegion = new double[dimension-1];
    for (i=0; i<dimension-1; i++) {
        valueTrellis[i] = trellis[i] - regUp[i];
    }
    for (i=0; i<dimension-1; i++) {
        valueRegion[i] = regUp[i] - regLow[i];
    }

    static double* dTemp = new double[numberSummands/2 + 1];

    // sum
    for (i=1; i<=numberSummands/2; i++) {

        // set bitvector length to fixed value 16
        // TODO Warning: dimension-1 <= 16 is assumed
        bitvector = (long)i;

        // construct summand
        // 0: take factor from region
        // 1: take factor from cuboid
        summand = 1.0;
        for (j=0; j<dimension-2; j++) {
            if (bitvector[j]) {
                summand *= valueTrellis[j];
            }
            else {
                summand *= valueRegion[j];
            }
        }
        summand *= valueRegion[dimension-2];

        // determine sign of summand
        vol -= summand;
        dTemp[i] =- summand;

        // add summand to sum
        // sign = (int) pow((double)-1, (double)counterOnes+1);
        // vol += (sign * summand);
    }

    bitvector = (long)i;
    summand = 1.0;
    for (j=0; j<dimension-1; j++) {
        if (bitvector[j]) {
            summand *= valueTrellis[j];
        }
        else {
            summand *= valueRegion[j];
        }
    }
    vol -= summand;

    for (i=1; i<=numberSummands/2; i++) {
        summand = dTemp[i];
        summand *= regUp[dimension-2] - trellis[dimension-2];
        summand /= valueRegion[dimension-2];
        vol -= summand;
    }

    //delete[] valueTrellis;
    //delete[] valueRegion;
    return vol;
}

// return median of the list of boundaries considered as a set
// TODO linear implementation
inline double getMedian(vector<double>& bounds) {
    // do not filter duplicates
    static unsigned int i;
    if (bounds.size()==1) {
        return bounds[0];
    }
    else if (bounds.size()==2) {
        return bounds[1];
    }
    vector<double>::iterator median;
    median = bounds.begin();
    for(i=0;i<=bounds.size()/2;i++){
        median++;
    }
    partial_sort(bounds.begin(),median,bounds.end());
    return bounds[bounds.size()/2];
}

// recursive calculation of hypervolume
inline void stream(double regionLow[], double regionUp[], const vector<double*>& points, int split, double cover) {

    //--- init --------------------------------------------------------------//

    static double coverOld;
    coverOld = cover;
    unsigned int coverIndex = 0;
    static int c;

    //--- cover -------------------------------------------------------------//

    // identify first covering cuboid
    double dMeasure = getMeasure(regionLow, regionUp);
    while (cover == coverOld && coverIndex < points.size()) {
        if ( covers(points[coverIndex], regionLow) ) {
            // new cover value
            cover = points[coverIndex][dimension-1];
            volume += dMeasure * (coverOld - cover);
        }
        else coverIndex++;
    }

    /* coverIndex shall be the index of the first point in points which
     * is ignored in the remaining process
     *
     * It may occur that that some points in front of coverIndex have the same
     * d-th coordinate as the point at coverIndex. This points must be discarded
     * and therefore the following for-loop checks for this points and reduces
     * coverIndex if necessary.
     */
    for (c=coverIndex; c>0; c--) {
        if (points[c-1][dimension-1] == cover) {
            coverIndex--;
        }
    }

    // abort if points is empty
    if (coverIndex == 0) {
        return;
    }
    // Note: in the remainder points is only considered to index coverIndex

    //--- allPiles  ---------------------------------------------------------//

    bool allPiles = true;
    unsigned int i;

    static int* piles = new int[coverIndex];
    for (i = 0; i<coverIndex; i++) {
        piles[i] = isPile(points[i], regionLow, regionUp);
        if (piles[i] == -1) {
            allPiles = false;
            //delete[] piles;
            break;
        }
    }

    /*
     * trellis[i] contains the values of the minimal i-coordinate of
     * the i-piles.
     * If there is no i-pile the default value is the upper bpund of the region.
     * The 1-dimensional KMP of the i-piles is: reg[1][i] - trellis[i]
     *
     */

    if (allPiles) { // sweep

        // initialize trellis with region's upper bound
        static double* trellis = new double[dimension-1];
        for (c=0; c<dimension-1; c++) {
            trellis[c] = regionUp[c];
        }

        double current = 0.0;
        double next = 0.0;
        i = 0;
        do { // while(next != coverNew)
            current = points[i][dimension-1];
            do { // while(next == current)
                if (points[i][piles[i]] < trellis[piles[i]]) {
                    trellis[piles[i]] = points[i][piles[i]];
                }
                i++; // index of next point
                if (i < coverIndex) {
                    next = points[i][dimension-1];
                }
                else {
                    next = cover;
                }

            } while(next == current);
            volume += computeTrellis(regionLow, regionUp, trellis) * (next - current);
        } while(next != cover);
    }

        //--- split -------------------------------------------------------------//
        // inner node of partition tree
    else{
        double bound = -1.0;
        vector<double> boundaries;
        vector<double> noBoundaries;

        do {
            for (i=0; i<coverIndex; i++) {
                int contained = containsBoundary(points[i], regionLow, split);
                if (contained == 1) {
                    boundaries.push_back(points[i][split]);
                } else if (contained == 0) {
                    noBoundaries.push_back(points[i][split]);
                }
            }

            if (boundaries.size() >  0) {
                bound = getMedian(boundaries);
                //bound = getRandom(boundaries);
            }
            else if (noBoundaries.size() >  dSqrtDataNumber) {
                bound = getMedian(noBoundaries);
                //bound = getRandom(noBoundaries);
            }
            else {
                split++;
            }
        } while (bound == -1.0);

        double dLast;
        vector<double*> pointsChild;
        pointsChild.reserve(coverIndex);

        // left child
        // reduce maxPoint
        dLast = regionUp[split];
        regionUp[split] = bound;
        for (i=0; i<coverIndex; i++) {
            if (partCovers(points[i], regionUp)) {
                pointsChild.push_back(points[i]);
            }
        }
        if (!pointsChild.empty()) {
            stream(regionLow, regionUp, pointsChild, split, cover);
        }

        // right child
        // increase minPoint
        pointsChild.clear();
        regionUp[split] = dLast;
        dLast = regionLow[split];
        regionLow[split] = bound;
        for (i=0; i<coverIndex; i++) {
            if (partCovers(points[i], regionUp)) {
                pointsChild.push_back(points[i]);
            }
        }
        if (!pointsChild.empty()) {
            stream(regionLow, regionUp, pointsChild, split, cover);
        }
        regionLow[split] = dLast;

    }// end inner node

} // end stream

// ----------------
// Python interface
// ----------------

namespace py = pybind11;

PYBIND11_MODULE(hypervolume, m)
{
    m.doc() = "pybind11 hv plugin";

    m.def("calculate", &calculate);
}


/**
 * @file dbscan.hpp
 * @author Ryan Curtin
 *
 * An implementation of the DBSCAN clustering method, which is flexible enough
 * to support other algorithms for finding nearest neighbors.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_DBSCAN_DBSCAN_HPP
#define MLPACK_METHODS_DBSCAN_DBSCAN_HPP

#include <mlpack/core.hpp>
#include <mlpack/methods/range_search/range_search.hpp>
#include "random_point_selection.hpp"
#include <boost/dynamic_bitset.hpp>

namespace mlpack {
namespace dbscan {

/**
 * DBSCAN (Density-Based Spatial Clustering of Applications with Noise) is a
 * clustering technique described in the following paper:
 *
 * @code
 * @inproceedings{ester1996density,
 *   title={A density-based algorithm for discovering clusters in large spatial
 *       databases with noise.},
 *   author={Ester, M. and Kriegel, H.-P. and Sander, J. and Xu, X.},
 *   booktitle={Proceedings of the Second International Conference on Knowledge
 *       Discovery and Data Mining (KDD '96)},
 *   pages={226--231},
 *   year={1996}
 * }
 * @endcode
 *
 * The DBSCAN algorithm iteratively clusters points using range searches with a
 * specified radius parameter.  This implementation allows configuration of the
 * range search technique used and the point selection strategy by means of
 * template parameters.
 *
 * @tparam RangeSearchType Class to use for range searching.
 * @tparam PointSelectionPolicy Strategy for selecting next point to cluster
 *      with.
 */
template<typename RangeSearchType = range::RangeSearch<>,
         typename PointSelectionPolicy = RandomPointSelection>
class DBSCAN
{
 public:
  /**
   * Construct the DBSCAN object with the given parameters.
   *
   * @param epsilon Size of range query.
   * @param minPoints Minimum number of points for each cluster.
   * @param rangeSearch Optional instantiated RangeSearch object.
   * @param pointSelector OptionL instantiated PointSelectionPolicy object.
   */
  DBSCAN(const double epsilon,
         const size_t minPoints,
         RangeSearchType rangeSearch = RangeSearchType(),
         PointSelectionPolicy pointSelector = PointSelectionPolicy());

  /**
   * Performs DBSCAN clustering on the data, returning number of clusters
   * and also the centroid of each cluster.
   *
   * @tparam MatType Type of matrix (arma::mat or arma::sp_mat).
   * @param data Dataset to cluster.
   * @param centroids Matrix in which centroids are stored.
   */
  template<typename MatType>
  size_t Cluster(const MatType& data,
                 arma::mat& centroids);

  /**
   * Performs DBSCAN clustering on the data, returning number of clusters
   * and also the list of cluster assignments.
   *
   * @tparam MatType Type of matrix (arma::mat or arma::sp_mat).
   * @param data Dataset to cluster.
   * @param assignments Vector to store cluster assignments.
   */
  template<typename MatType>
  size_t Cluster(const MatType& data,
                 arma::Row<size_t>& assignments);

  /**
   * Performs DBSCAN clustering on the data, returning number of clusters,
   * the centroid of each cluster and also the list of cluster assignments.
   * If assignments[i] == assignments.n_elem - 1, then the point is considered
   * "noise".
   *
   * @tparam MatType Type of matrix (arma::mat or arma::sp_mat).
   * @param data Dataset to cluster.
   * @param assignments Vector to store cluster assignments.
   * @param centroids Matrix in which centroids are stored.
   */
  template<typename MatType>
  size_t Cluster(const MatType& data,
                 arma::Row<size_t>& assignments,
                 arma::mat& centroids);

 private:
  //! Maximum distance between two points to be part of same cluster.
  double epsilon;

  //! Minimum number of points to be in the epsilon-neighborhood (including
  //! itself) for the point to be a core-point.
  size_t minPoints;

  //! Instantiated range search policy.
  RangeSearchType rangeSearch;

  //! Instantiated point selection policy.
  PointSelectionPolicy pointSelector;

  /**
   * This function processes the point at index. It  marks the point as visited,
   * checks if the given point is core or non-core.  If it is a core point, it
   * expands the cluster, otherwise it returns.
   *
   * @tparam MatType Type of matrix (arma::mat or arma::sp_mat).
   * @param data Dataset to cluster.
   * @param unvisited Remembers if a point has been visited.
   * @param index Index of point to be visited now.
   * @param assignments Vector to store cluster assignments.
   * @param currentCluster Index of cluster which will be  assigned to points in
   *     current cluster.
   * @param neighbors Matrix containing list of neighbors for each point which
   *     fall in its epsilon-neighborhood.
   * @param distances Matrix containing list of distances for each point which
   *     fall in its epsilon-neighborhood.
   * @param topLevel If true, then current point is the first point in the
   *     current cluster, helps in detecting noise.
   */
  template<typename MatType>
  size_t ProcessPoint(const MatType& data,
                      boost::dynamic_bitset<>& unvisited,
                      const size_t index,
                      arma::Row<size_t>& assignments,
                      const size_t currentCluster,
                      const std::vector<std::vector<size_t>>& neighbors,
                      const std::vector<std::vector<double>>& distances,
                      const bool topLevel = true);
};

} // namespace dbscan
} // namespace mlpack

// Include implementation.
#include "dbscan_impl.hpp"

#endif


#include <Columns/ColumnArray.h>
#include <Columns/IColumn.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypesNumber.h>
#include <DataTypes/IDataType.h>
#include <DataTypes/getLeastSupertype.h>
#include <Functions/FunctionFactory.h>
#include <Functions/FunctionHelpers.h>

#include <Eigen/Core>

namespace DB
{
namespace ErrorCodes
{
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
    extern const int LOGICAL_ERROR;
}

template <const int N>
struct LpNorm
{
    static inline String name = "L" + std::to_string(N);
    template <typename T>
    static void compute(const std::vector<Eigen::VectorX<T>> & vec, PaddedPODArray<T> & array)
    {
        array.reserve(vec.size());
        for (const auto & v : vec)
        {
            array.push_back(v.template lpNorm<N>());
        }
    }
};

struct LinfNorm : LpNorm<Eigen::Infinity>
{
    static inline String name = "Linf";
};

template <class Kernel>
class FunctionArrayNorm : public IFunction
{
public:
    static inline auto name = "array" + Kernel::name + "Norm";
    String getName() const override { return name; }
    static FunctionPtr create(ContextPtr) { return std::make_shared<FunctionArrayNorm<Kernel>>(); }
    size_t getNumberOfArguments() const override { return 1; }
    bool isSuitableForShortCircuitArgumentsExecution(const DataTypesWithConstInfo & /*arguments*/) const override { return true; }
    bool useDefaultImplementationForConstants() const override { return true; }

    DataTypePtr getReturnTypeImpl(const ColumnsWithTypeAndName & arguments) const override
    {
        DataTypes types;
        for (const auto & argument : arguments)
        {
            const auto * array_type = checkAndGetDataType<DataTypeArray>(argument.type.get());
            if (!array_type)
                throw Exception(ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT, "Argument of function {} must be array.", getName());

            types.push_back(array_type->getNestedType());
        }
        const auto & common_type = getLeastSupertype(types);
        switch (common_type->getTypeId())
        {
            case TypeIndex::UInt8:
            case TypeIndex::UInt16:
            case TypeIndex::UInt32:
            case TypeIndex::Int8:
            case TypeIndex::Int16:
            case TypeIndex::Int32:
            case TypeIndex::Float32:
                return std::make_shared<DataTypeFloat32>();
            case TypeIndex::UInt64:
            case TypeIndex::Int64:
            case TypeIndex::Float64:
                return std::make_shared<DataTypeFloat64>();
            default:
                throw Exception(
                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                    "Arguments of function {} has nested type {}. "
                    "Support: UInt8, UInt16, UInt32, UInt64, Int8, Int16, Int32, Int64, Float32, Float64.",
                    getName(), common_type->getName());
        }
    }

    ColumnPtr
    executeImpl(const ColumnsWithTypeAndName & arguments, const DataTypePtr & result_type, size_t /*input_rows_count*/) const override
    {
        DataTypePtr type = typeid_cast<const DataTypeArray *>(arguments[0].type.get())->getNestedType();
        ColumnPtr column = arguments[0].column->convertToFullColumnIfConst();
        const auto * arr = assert_cast<const ColumnArray *>(column.get());

        auto result = result_type->createColumn();
        switch (result_type->getTypeId())
        {
            case TypeIndex::Float32:
                executeWithType<Float32>(*arr, type, result);
                break;
            case TypeIndex::Float64:
                executeWithType<Float64>(*arr, type, result);
                break;
            default:
                throw Exception(ErrorCodes::LOGICAL_ERROR, "Unexpected result type.");
        }
        return result;
    }

private:
    template <typename MatrixType>
    void executeWithType(const ColumnArray & array, const DataTypePtr & type, MutableColumnPtr & column) const
    {
        std::vector<Eigen::VectorX<MatrixType>> vec;
        columnToVectors(array, type, vec);
        auto & data = assert_cast<ColumnVector<MatrixType> &>(*column).getData();
        Kernel::compute(vec, data);
    }

    template <typename MatrixType>
    void columnToVectors(const ColumnArray & array, const DataTypePtr & nested_type, std::vector<Eigen::VectorX<MatrixType>> & vec) const
    {
        switch (nested_type->getTypeId())
        {
            case TypeIndex::UInt8:
                fillVectors<MatrixType, UInt8>(vec, array);
                break;
            case TypeIndex::UInt16:
                fillVectors<MatrixType, UInt16>(vec, array);
                break;
            case TypeIndex::UInt32:
                fillVectors<MatrixType, UInt32>(vec, array);
                break;
            case TypeIndex::UInt64:
                fillVectors<MatrixType, UInt64>(vec, array);
                break;
            case TypeIndex::Int8:
                fillVectors<MatrixType, Int8>(vec, array);
                break;
            case TypeIndex::Int16:
                fillVectors<MatrixType, Int16>(vec, array);
                break;
            case TypeIndex::Int32:
                fillVectors<MatrixType, Int32>(vec, array);
                break;
            case TypeIndex::Int64:
                fillVectors<MatrixType, Int64>(vec, array);
                break;
            case TypeIndex::Float32:
                fillVectors<MatrixType, Float32>(vec, array);
                break;
            case TypeIndex::Float64:
                fillVectors<MatrixType, Float64>(vec, array);
                break;
            default:
                throw Exception(
                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                    "Arguments of function {} has nested type {}. "
                    "Support: UInt8, UInt16, UInt32, UInt64, Int8, Int16, Int32, Int64, Float32, Float64.",
                    getName(), nested_type->getName());
        }
    }

    template <typename MatrixType, typename DataType>
    requires std::is_same_v<MatrixType, DataType>
    void fillVectors(std::vector<Eigen::VectorX<MatrixType>> & vec, const ColumnArray & array) const
    {
        const auto & data = typeid_cast<const ColumnVector<DataType> &>(array.getData()).getData();
        const auto & offsets = array.getOffsets();
        vec.reserve(offsets.size());
        ColumnArray::Offset prev = 0;
        for (auto off : offsets)
        {
            vec.emplace_back(Eigen::Map<const Eigen::VectorX<MatrixType>>(data.data() + prev, off - prev));
            prev = off;
        }
    }

    template <typename MatrixType, typename DataType>
    void fillVectors(std::vector<Eigen::VectorX<MatrixType>> & vec, const ColumnArray & array) const
    {
        const auto & data = typeid_cast<const ColumnVector<DataType> &>(array.getData()).getData();
        const auto & offsets = array.getOffsets();
        vec.reserve(offsets.size());

        ColumnArray::Offset prev = 0;
        for (auto off : offsets)
        {
            Eigen::VectorX<MatrixType> mat(off - prev);
            for (ColumnArray::Offset row = 0; row + prev < off; ++row)
            {
                mat[row] = static_cast<MatrixType>(data[prev + row]);
            }
            prev = off;
            vec.emplace_back(mat);
        }
    }
};

void registerFunctionArrayNorm(FunctionFactory & factory)
{
    factory.registerFunction<FunctionArrayNorm<LpNorm<1>>>();
    factory.registerFunction<FunctionArrayNorm<LpNorm<2>>>();
    factory.registerFunction<FunctionArrayNorm<LinfNorm>>();
}

}


//
// Created by Beck on 7/3/18.
// Mahony filter for attitude estimation
// http://www.olliw.eu/2013/imu-data-fusing/#mahonycode
//

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

ros::Publisher pose_pub;
string imu_topic, publisher_topic;

Quaterniond q = Quaterniond::Identity();  // state
double sampleFreq = 400.0;  // sample frequency in Hz
double Kp = 0.5;   // proportional gain
double Ki = 0.1;   // integral gain
Vector3d integral = {0.0, 0.0, 0.0};

void pub_fused_pose(const sensor_msgs::Imu &msg)
{
    sensor_msgs::Imu imu_with_pose;
    imu_with_pose.header = msg.header;
    imu_with_pose.orientation.w = q.w();
    imu_with_pose.orientation.x = q.x();
    imu_with_pose.orientation.y = q.y();
    imu_with_pose.orientation.z = q.z();
    imu_with_pose.angular_velocity = msg.angular_velocity;
    imu_with_pose.angular_velocity_covariance = msg.angular_velocity_covariance;
    imu_with_pose.linear_acceleration = msg.linear_acceleration;
    imu_with_pose.linear_acceleration_covariance = msg.linear_acceleration_covariance;

    pose_pub.publish(imu_with_pose);
}

void imu_callback(const sensor_msgs::Imu::ConstPtr &imu)
{
    Vector3d halfd, halfe;

    Vector3d acc(imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z);
    acc.normalize();
    Vector3d omg(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z);
    // omg.normalize();
    double dt = 1.0 / sampleFreq;

    if (!((acc[0] == 0.0) && (acc[1] == 0.0) && (acc[2] == 0.0))) {
        // Estimate the gravity vector d from quaternion q
        // d = Im(q^-1 e_z q)
        halfd[0] = q.x() * q.z() - q.w() * q.y();
        halfd[1] = q.w() * q.x() + q.y() * q.z();
        halfd[2] = q.w() * q.w() - 0.5f + q.z() * q.z();

        // calculate error vector e = a x d
        halfe = acc.cross(halfd);

        if (Ki > 0.0) {
            // In = In-1 + Ki * dt * e
            integral += 2.0 * Ki * dt * halfe;
            omg += integral;
        }
        else {
            integral.setZero();
        }

        // Apply proportional feedback w' = w + Kp * e + In
        omg += 2.0 * Kp * halfe;
    }

    // Integrate rate of change using dq = 0.5 * q x w'
    omg *= 0.5 * dt;

    q.w() += (-q.x() * omg[0] - q.y() * omg[1] - q.z() * omg[2]);
    q.x() += (q.w() * omg[0] + q.y() * omg[2] - q.z() * omg[1]);
    q.y() += (q.w() * omg[1] - q.x() * omg[2] + q.z() * omg[0]);
    q.z() += (q.w() * omg[2] + q.x() * omg[1] - q.y() * omg[0]);

    q.normalize();
    pub_fused_pose(*imu);
}

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

    n.param("imu_raw", imu_topic, string("/dji_sdk/imu")); // 400Hz
    n.param("publisher_topic", publisher_topic, string("/attitude_estimator/imu"));

    ros::Subscriber s3 = n.subscribe(imu_topic, 400, imu_callback);
    pose_pub = n.advertise<sensor_msgs::Imu>(publisher_topic, 100);

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


//Backend for the instanton finder in the AdS/Flat spacetime case.
#define DLL_EXPORT
#define INSTANTON_SOLVER_DLL_API //Comment this out if we wish to import these
//from a DLL instead.
#include "project_specific.h"

#include "multi_precision_definitions.h"
#include <boost/array.hpp>//For boost arrays (needed for ode_types)
#include "Flat.h"

//------------------------------------------------------------------------------
//Main function to export:
template< class value_type , class time_type , class solution_type,
          class solution_type_action >
void odeSolveFlatFixedBackground
    (value_type false_vacuum , value_type true_vacuum , value_type barrier ,
     potential< value_type >& V , time_type chimax , int odeSolverToUse,
     value_type RelTol, value_type AbsTol, value_type stepError, value_type xi,
     value_type lowerBound , value_type upperBound ,
     solution_grid< time_type , solution_type_action , value_type >& solOut,
     value_type& DSout,value_type precision,std::ostream& outStream,
     bool useSimpleBC,time_type epsilon_step)
{
//INPUTS:
    /*prhs[0] = a in GeV
     *prhs[1] = b (should be -1 < b < 1 to have a barrier)
     *prhs[2] = g (dimensionless coupling strength)
     *prhs[3] = chimax ("infinity" - ie, maximum time we allow)
     *prhs[4] = odeSolver, ie, which ode integration routine to use.
     *  odeSolver = 0 -> (Default) Cash-Karp method (Runge-Kutta)
     *  odeSolver = 1 -> Cash-Karp method (Runge-Kutta)
     *  odeSolver = 2 -> Dormand-Prince algorithm (Runge-Kutta)
     *  odeSolver = 3 -> Fehlberg algorithm (Runge-Kutta)
     *  odeSolver = 4 -> Bulirsch-Stoer algorithm (multi-step method)
     *prhs[5] = RelTol, optional (defaults to 1e-15)
     *prhs[6] = AbsTol, optional (defaults to 1e-15)
     *prhs[7] - stepError - optional. Fraction of Taylor series validity bound
     *          to use for first step. Defaults to 1e-15
     *prhs[8] - xi, non-minimal coupling. (optional - default to 0).
        NB - this won't actually affect the fixed background case in flat
            space!!!
     *prhs[9] - Lower bound on range to search (optional, defaults to barrier)
     *prhs[10] - Upper bound on range to search (optional, defaults to
                                                    true-vacuum)
     *prhs[11] - solOut - solution grid to store final result in.
     *prhs[12] - DSout - multi in which to store computed decay exponent.
     prhs[13] - precision - difference desired between upper and lower bounds
                on initial value of solution.
     */

    //Ok. we proceed by bisecting until we have a solution which overshoots
    //beyond
    //chimax, which stands in for infinity.

    //Overshoots occur when the solution crosses zero before chimax.
    //Undershoots when
    //its derivative crosses zero before then.

    //Instantiate the events function, state-type, dynamic BCs etc...
    //typedef boost::array< value_type , 2 > FlatSolType;//solution without
    //action, for speed.
    //typedef boost::array< value_type , 5 > AdSFlatSolTypeAction;
    typedef solution_type FlatSolType;
    typedef solution_type_action FlatSolTypeAction;
    typedef odeFlat< FlatSolType , time_type , value_type > odeRHSFlat;
    typedef eventsFlat< FlatSolType , time_type , value_type > evFlat;
    typedef dynamicBCsFlatFixed_taylor< value_type , FlatSolType , time_type >
            dynBCFlat;
    typedef odeFlatAction< FlatSolTypeAction , time_type , value_type >
            odeRHSFlatAction;
    typedef eventsFlat< FlatSolTypeAction , time_type , value_type >
            evFlatAction;
    typedef dynamicBCsFlatFixed_taylor< value_type , FlatSolTypeAction ,
                                        time_type > dynBCFlatAction;

    //Instantiation for overshoot/undershoot:
	value_type h = value_type(1.0);//h = phi_scale/M_p so h = 1 is Planck units.
	value_type V0 = value_type(0.0);//V at phi = 0.
    odeRHSFlat odeToSolve(V,xi, h, V0); //ode to solve (gravitational instanton
    //equation)
    //Events function - choose upper and lower bounds 5% outside false and true
    // vacuum, to leave us some leeway for numerical
    //fluctuations near to the nominal boundaries.
    evFlat events(false_vacuum,true_vacuum,false_vacuum,barrier,true_vacuum,
                  outStream); //events function.
        //Checks for events labelled IE:
        //IE = 1 -> y crosses zero (terminal event, => overshoot)
        //IE = 2 -> y' crosses zero (terminal event, => undershoot)
        //IE = 3 -> a crosses zero (non-terminal event, for reference only)
        //IE = 4 -> y crosses lowerBound (usually 0, but could be different -
                                          //terminal event)
        //IE = 5 -> y crosses upperBound (terminal event, indicates bad
                                          //initial condition or perhaps a->0)
    dynBCFlat bcAdjuster(V,stepError,1);//Performs analytic first step using a
                                        // taylor series approximation.
        //stepError - determines size of initial step.
        //version (last argument) - specifies which version of the ode we are
            //using.
            //version = 0 -> normal ode, no action.
            //version = 1 -> ode with action
            //version = 2 -> linearised ode.
    //bcFlat bcAdjusterSimple(V,xi,h,V0);

    //Search loop:
    int nMax = 200;
    int counter = 0;
    //Current bounds on location of bounce:
    value_type lower = lowerBound;//Barrier
    value_type upper = upperBound;//True vacuum
    //outStream << "Lower initial = " << lower << std::endl;
    //outStream << "Upper initial = " << upper << std::endl;
    value_type guess;
    multi diff = abs(upper - lower);
    while(diff > precision)
    {
        //Bisect to get initial guess:
        guess = (lower + upper)/multi(2.0);

        //Check for when the solution passes through its FWHM:
        events.value_to_check = (guess + false_vacuum)/value_type(2.0);

        //Formulate initial conditions vector:
        FlatSolType y0 = {{ guess , value_type(0.0) }};
        //Step away to avoid co-ordinate singularity at chi = 0:
        FlatSolType y01step;
        time_type epsilon;
        /*
        if(!useSimpleBC)
        {
            epsilon = bcAdjuster(y0,y01step,0);//Copies solution after step of
            //epsilon into y01step. Last argument tells us whether
        }
        else
        {
            epsilon = epsilon_step;
            y01step = bcAdjusterSimple(epsilon,y0);
        }
        */
        epsilon = bcAdjuster(y0,y01step);//Copies solution after step of
        //epsilon into y01step. Last argument tells us whether
            //we step forwards (0) or backwards (1) in time.
        //Set up time range:
        time_type tspanArray[2] = { epsilon , chimax };
        std::vector< time_type > tspan;
        tspan.assign(tspanArray,tspanArray + 2);
        //Solve the ode:
        solution_grid< time_type , FlatSolType , value_type > solTemp;
        time_type initStepMax = 0.01/time_type(chimax);
        int nSuccess = ode_solve<time_type,solution_type,value_type>
        (odeToSolve,y01step,tspan,events,odeSolverToUse,solTemp,RelTol,AbsTol,
         initStepMax);
        if(nSuccess != 1)
        {
            outStream << "nSuccess = " << nSuccess << " when solving for "
                      << "AdS-Flat instantons." << std::endl;
            throw "Integration failure.";
        }
        //Now check whether we found an overshoot or an undershoot:
        bool overshoots = false;
        bool undershoots = false;
        for(int j = 0; j < int(solTemp.IE.size());j++)
        {
            switch(solTemp.IE[j])
            {
            case 1:
                //overshoot.
                //outStream << "Overshoot." << std::endl;
                //reportToCaller("Overshoot.\n");
                overshoots = true;
                upper = guess;
                break;
            case 2:
                //undershoot, IF it occurs on the opposite side of the barrier
                //to the true vacuum (theoretically impossible to occur on the
                                      //other
                //side, so must be a numerical artefact if it does).
                if((true_vacuum - barrier)*(solTemp.YE[j][0] - barrier)
                   < value_type(0.0))
                {
                    //outStream << "Undershoot." << std::endl;
                    //reportToCaller("Undershoot.\n");
                    undershoots = true;
                    lower = guess;
                }
                break;
            case 3:
                //FWHM. No need to do anything.
                break;
            case 4:
                //Crossed lower bound. Indicates a bad initial condition.
                outStream << "Upper bound crossed during ode integration. "
                          << "Possibly a bad initial condition (wrong side of "
                          << "true vacuum) or solution undershot and event "
                          << "detection failed to detect this." << std::endl;
                //reportToCaller("Upper bound crossed during ode integration.
                //Possibly a bad initial condition (wrong side of true vacuum)
                //or solution undershot and event detection failed to detect
                //this.\n");
                throw "Unexpected ode-event encountered during integration.";
                break;
            case 5:
                //Crossed upper bound. Indicates a bad initial condition.
                outStream << "Lower bound crossed during ode integration. "
                          << "Possibly a bad initial condition (wrong side of "
                          << "true vacuum) or solution overshot and event"
                          << " detection failed to detect this." << std::endl;
                //reportToCaller("Upper bound crossed during ode integration.
                                 //Possibly a bad initial condition
                                 //(wrong side of true vacuum) or solution
                                 //undershot and event detection failed to
                                 //detect this.\n");
                throw "Unexpected ode-event encountered during integration.";
            default:
                outStream << "Unrecognised ode-event." << std::endl;
                //reportToCaller("Unrecognised ode-event.\n");
                throw "Unexpected ode-event encountered during integration.";
            }
            if(overshoots || undershoots)
            {
                break;
            }
        }
        counter++;
        diff = abs(lower - upper);

        if(overshoots)
        {
            //outStream << "Overshoot.\n" << std::endl;
        }
        else if(undershoots)
        {
            //outStream << "Undershoot.\n" << std::endl;
        }

        //Dump all available data if we get neither an overshoot nor an
        //undershoot, as this is
        //theoretically impossible so we want to understand what went wrong:
        else if(!(overshoots||undershoots))
        {
            outStream << "Neither undershoot nor overshoot detected. "
                      << "(Perhaps integration range should be extended?) "
                      << "Relevant data:\n";
            outStream << "no. of events = " << int(solTemp.IE.size())
                      << std::endl;
            for(int i = 0; i < int(solTemp.IE.size());i++)
            {
                outStream << "IE[" << i << "] = " << solTemp.IE[i] << std::endl;
                outStream << "YE[" << i << "] = " << std::endl;
                for(int j = 0;j < 2;j++)
                {
                    outStream << solTemp.YE[i][j] << std::endl;
                }
                outStream << "TE[" << i << "] = " << solTemp.TE[i] << std::endl;
            }
            outStream << "y0 = \n";
            for(int i = 0;i < 2;i++)
            {
                outStream << y01step[i] << std::endl;
            }
            outStream << "yend = \n";
            for(int i = 0;i < 2;i++)
            {
                outStream << solTemp.Y[int(solTemp.Y.size() - 1)][i]
                          << std::endl;
            }
            outStream << "Tend = " << solTemp.T[int(solTemp.T.size()) - 1]
                      << std::endl;
            //Now exit, throwing an error:
            throw "Error - Neither overshoot nor undershoot detected.\n";
        }

    }
    //We found the bounce. Are we within precision?
    if(diff > precision)
    {
        outStream << "Loop exited before finding bounce." << std::endl;
        //reportToCaller("Loop exited before finding bounce.\n");
    }

    //Compute action:
    odeRHSFlatAction odeToSolveAction(V,xi,value_type(1.0),value_type(0.0));
    evFlatAction eventsAction(false_vacuum,true_vacuum,false_vacuum,barrier,
                              true_vacuum , outStream);
    dynBCFlatAction bcAdjusterAction(V,stepError,2);

    //Check for when the solution passes through its FWHM:
    eventsAction.value_to_check = (guess + false_vacuum)/value_type(2.0);
    //Disable termination so that we can see the full extent of the solution:
    //eventsAction.terminate_on = false;
    //eventsAction.dynamicSwitch[0] = false;
    //eventsAction.dynamicSwitch[1] = false;

    FlatSolTypeAction y0Action = {{ guess , value_type(0.0) ,
                                    value_type(0.0) }};
    FlatSolTypeAction y0Action1step;
    time_type epsAction;
    /*
    if(!useSimpleBC)
    {
        epsAction = bcAdjusterAction(y0Action,y0Action1step,0);
        //Copies solution after step of epsilon into y01step.
        //Last argument tells us whether
    }
    else
    {
        epsAction = epsilon_step;
        y0Action1step = bcAdjusterSimple(epsAction,y0Action);
    }
    */
    epsAction = bcAdjusterAction(y0Action,y0Action1step);
    time_type tspanArray[2] = { epsAction , chimax };
    std::vector< time_type > tspan;
    tspan.assign(tspanArray,tspanArray + 2);
    time_type initStepMax = 0.01/time_type(chimax);
    int nSuccess = ode_solve<time_type,solution_type_action,value_type>
    (odeToSolveAction,y0Action1step,tspan,eventsAction,odeSolverToUse,solOut,
     RelTol,AbsTol,initStepMax);
    if(nSuccess != 1)
    {
        outStream << "nSuccess = " << nSuccess << " when solving for AdS-Flat "
                  << "instantons." << std::endl;
        throw "Integration failure.";
    }
    DSout = solOut.Y[int(solOut.T.size()) - 1][2];
    //Done.
}
//------------------------------------------------------------------------------
//Export an ode solver:
//Main function to export:
template< class value_type , class time_type , class solution_type,
          class solution_type_action >
void odeSolveFlatFixedBackgroundSingle
    (value_type y0,value_type false_vacuum , value_type true_vacuum ,
     value_type barrier , potential< value_type >& V , time_type chimax ,
     int odeSolverToUse, value_type RelTol, value_type AbsTol,
     value_type stepError, value_type xi, value_type lowerBound ,
     value_type upperBound , solution_grid< time_type , solution_type_action ,
     value_type >& solOut,value_type& DSout,value_type precision,
     std::ostream& outStream)
{
//INPUTS:
    /*prhs[0] = a in GeV
     *prhs[1] = b (should be -1 < b < 1 to have a barrier)
     *prhs[2] = g (dimensionless coupling strength)
     *prhs[3] = chimax ("infinity" - ie, maximum time we allow)
     *prhs[4] = odeSolver, ie, which ode integration routine to use.
     *  odeSolver = 0 -> (Default) Cash-Karp method (Runge-Kutta)
     *  odeSolver = 1 -> Cash-Karp method (Runge-Kutta)
     *  odeSolver = 2 -> Dormand-Prince algorithm (Runge-Kutta)
     *  odeSolver = 3 -> Fehlberg algorithm (Runge-Kutta)
     *  odeSolver = 4 -> Bulirsch-Stoer algorithm (multi-step method)
     *prhs[5] = RelTol, optional (defaults to 1e-15)
     *prhs[6] = AbsTol, optional (defaults to 1e-15)
     *prhs[7] - stepError - optional. Fraction of Taylor series validity bound
     *          to use for first step. Defaults to 1e-15
     *prhs[8] - xi, non-minimal coupling. (optional - default to 0).
        NB - this won't actually affect the fixed background case in
        flat space!!!
     *prhs[9] - Lower bound on range to search (optional, defaults to barrier)
     *prhs[10] - Upper bound on range to search (optional, defaults to
                                                 true-vacuum)
     *prhs[11] - solOut - solution grid to store final result in.
     *prhs[12] - DSout - multi in which to store computed decay exponent.
     prhs[13] - precision - difference desired between upper and lower bounds
        on initial value of solution.
     */

    //Ok. we proceed by bisecting until we have a solution which overshoots
    //beyond
    //chimax, which stands in for infinity.

    //Overshoots occur when the solution crosses zero before chimax.
    // Undershoots when
    //its derivative crosses zero before then.

    //Instantiate the events function, state-type, dynamic BCs etc...
    //typedef boost::array< value_type , 2 > FlatSolType;//solution without
    //action, for speed.
    //typedef boost::array< value_type , 5 > AdSFlatSolTypeAction;
    typedef solution_type FlatSolType;
    typedef solution_type_action FlatSolTypeAction;
    typedef odeFlat< FlatSolType , time_type , value_type > odeRHSFlat;
    typedef eventsFlat< FlatSolType , time_type , value_type > evFlat;
    typedef dynamicBCsFlatFixed_taylor< value_type , FlatSolType , time_type >
            dynBCFlat;
    typedef odeFlatAction< FlatSolTypeAction , time_type , value_type >
            odeRHSFlatAction;
    typedef eventsFlat< FlatSolTypeAction , time_type , value_type >
            evFlatAction;
    typedef dynamicBCsFlatFixed_taylor< value_type , FlatSolTypeAction ,
                                         time_type > dynBCFlatAction;

    //Instantiation for overshoot/undershoot:
    outStream << "Setup ode..." << std::endl;
	value_type h = value_type(1.0);//h = phi_scale/M_p so h = 1 is Planck units.
	value_type V0 = value_type(0.0);//V at phi = 0.
    odeRHSFlat odeToSolve(V,xi, h, V0); //ode to solve (gravitational
                                                        //instanton equation)

    //Compute action:
    odeRHSFlatAction odeToSolveAction(V,xi,value_type(1.0),value_type(0.0));
    evFlatAction eventsAction(false_vacuum*value_type(1.05),true_vacuum*
                              value_type(1.05),false_vacuum,barrier,
                              true_vacuum , outStream);
    dynBCFlatAction bcAdjusterAction(V,stepError,2);

    outStream << "Setup initial conditions..." << std::endl;
    FlatSolTypeAction y0Action = {{ y0 , value_type(0.0) , value_type(0.0) }};
    outStream << "V'(y0) = " << V.d(y0) << std::endl;
    FlatSolTypeAction y0Action1step;
    time_type epsAction = bcAdjusterAction(y0Action,y0Action1step);
    outStream << "y0Action1step = \n" << y0Action1step[0] << "\n"
              << y0Action1step[1] << "\n" << y0Action1step[2] << std::endl;
    outStream << "Setup timespan..." << std::endl;
    time_type tspanArray[2] = { epsAction , chimax };
    std::vector< time_type > tspan;
    tspan.assign(tspanArray,tspanArray + 2);
    outStream << "Integrating..." << std::endl;
    time_type initStepMax = 0.01/time_type(chimax);
    int nSuccess = ode_solve<time_type,solution_type_action,value_type>
    (odeToSolveAction,y0Action1step,tspan,eventsAction,odeSolverToUse,solOut,
     RelTol,AbsTol,initStepMax);
    if(nSuccess != 1)
    {
        outStream << "nSuccess = " << nSuccess << " when solving for AdS-Flat "
                  << "instantons." << std::endl;
        throw "Integration failure.";
    }
    outStream << "Compute action..." << std::endl;
    DSout = solOut.Y[int(solOut.T.size()) - 1][2];
    //Done.
    outStream << "Done." << std::endl;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//Explicit instantiation and export of this function:
template DLL_EXPORT void odeSolveFlatFixedBackground
    < multi , multi , boost::array< multi , 2 > , boost::array< multi , 3 > >
    (multi , multi , multi , potential< multi >& , multi , int, multi, multi,
     multi, multi, multi , multi ,
     solution_grid< multi , boost::array< multi , 3 > , multi >& ,
     multi&,multi,std::ostream&,bool,multi);
//------------------------------------------------------------------------------
template DLL_EXPORT void odeSolveFlatFixedBackgroundSingle
    < multi , multi , boost::array< multi , 2 > , boost::array< multi , 3 > >
    (multi,multi , multi , multi , potential< multi >& , multi , int,
     multi, multi, multi, multi,multi , multi ,
     solution_grid< multi , boost::array< multi , 3 > , multi >& ,
     multi&,multi,std::ostream&);
//------------------------------------------------------------------------------